一种基于粒子群和改进动态窗口法的避障路径规划方法

1.本发明属于路径规划技术领域，具体涉及一种基于粒子群和改进动态窗口法的避障路径规划方法。


背景技术：

2.根据agv工作环境的不同，可以将agv的路径规划问题分为全局路径规划和局部路径规划两类。全局路径规划指在已知全部外界环境信息的情况下进行预先规划，常使用粒子群优化算法求解，但在agv运行环境信息有误或噪声过大时，其生成路径的鲁棒性较差。局部路径规划指在已知部分外界环境信息的情况下进行的实时规划，对环境误差和噪声具有较高的鲁棒性，常使用动态窗口法求解，但其在每一次迭代时都选择当前情况下的局部最优路径，导致最终生成的路径往往无法达到全局最优的目标。
3.因此，针对单一算法存在的缺陷，将粒子群优化算法与动态窗口算法融合，通过算法间的优劣互补，既考虑agv行驶过程中的避障问题，又保证路径规划的完备性，规划出同时满足agv运动约束和实际应用需求的路径。动态窗口算法生成路径还存在路径冗余和无法有效避障等问题，无法保障agv在智能仓储环境中的安全运行并提高其搬运包裹的效率。


技术实现要素：

4.有鉴于此，本发明的目的在于提供一种避障路径规划算法，利用粒子群优化算法生成全局路径，并提出关键节点选取策略删除冗余节点，为全局路径保留关键节点；然后在动态窗口法的基础上改进评价函数，改善动态窗口法生成路径冗余或目标不可达问题；最后以关键节点作为改进动态窗口法算法的子目标节点，引导agv沿着全局路径方向进行实时路径规划，最终实现agv在同时有静态障碍物和动态障碍物的复杂智能仓储环境中能够生成安全的最优或次优路径。
5.为达到上述目的，本发明提供如下技术方案：
6.一种基于粒子群和改进动态窗口法的避障路径规划方法，包括以下步骤：
7.s1：使用粒子群优化算法进行路径规划获得全局路径；
8.s2：利用关键点选取策略对路径进行优化，得到全局路径生成的关键节点；
9.s3：将关键节点作为改进动态窗口法的局部目标点；
10.s4：根据环境需要设定agv的初始化速度空间(v,ω)、航向角θ0；
11.s5：利用改进动态窗口法对agv进行速度采样，获得各速度组合的模拟移动轨迹；
12.s6：依据评价函数选出最优的模拟移动轨迹，以该速度组合控制agv向目标点移动；
13.s7：继承agv本段局部路径规划完成后的位置和运行速度等信息，重复步骤s3-s4直到agv到达最终目标点。
14.进一步，步骤s1中，为粒子群优化算法设计适应度函数如式(5)所示：
[0015][0016]
式(1)中，f
l
用于计算agv从起始点到目标点之间的路径长度，(xi,yi)和(x
i+1
,y
i+1
)分别是路径点pi和p
i+1
的位置坐标；
[0017][0018][0019][0020]
式(2)-(4)中，(ox j,oy j)是第j个障碍物圆心的坐标，v
ij
是路径中第i个节点和第j个障碍物的惩罚项，fv用于计算生成路径的风险程度；
[0021][0022]
式(5)中，(x0,y0)和(xn,yn)分别是起始点p0和目标点pn的坐标。
[0023]
进一步，步骤s2中，使用关键节点选取策略保留关键节点，具体包括：
[0024]
从粒子群优化算法生成的全局路径sol中的第二个节点p1开始，如果当前节点与前面一个节点之间的距离小于某一阈值时，则认为以上两个节点间距离过近，当前节点为冗余点，删除该节点，更新路径；依次遍历到倒数第2个节点p
n-1
，删除其中的所有冗余点，得到新的路径{p0,p1,
…
,pm},m≤n。
[0025]
进一步，所述改进动态窗口法具有以下评价函数：
[0026][0027]
式(6)中，heading(v,ω)是方向角评价子函数，评价agv和目标方向的一致性；dist(v,ω)是距离评价子函数，评价agv与障碍物之间的距离；velocity(v,ω)是速度评价子函数，评价agv的运动性能；α、β和γ是评价子函数的权重因子；σ是归一化因子；obs
min
是agv在当前位置时，预测轨迹末端与障碍物间最小距离的平均值，其取值范围为obs
min
∈(0,2r2)；distance是起始点到目标点的距离；r2是静态障碍物膨胀化处理后的半径；η是系数。
[0028]
进一步，所述改进动态窗口法中，计算heading(v,ω)评价函数值的参考位置是模拟轨迹的末端，将agv从当前位置向前行驶1s后的位置作为heading(v,ω)子评价函数值计算的参考位置。
[0029]
本发明的有益效果在于：采用粒子群优化算法作为全局路径规划算法，生成静态
环境下最优路径，使用关键节点选取策略删除冗余节点，保留关键节点；在动态窗口法的基础上改进评价函数，改善使用动态窗口法生成路径冗余和目标不可达的问题；最后以关键节点作为改进动态窗口法的子目标节点，引导agv全局路径方向进行实时路径规划，实现平稳避障的同时生成最优或次优路径。
[0030]
本发明的其他优点、目标和特征将在随后的说明书中进行阐述，并且在某种程度上对本领域技术人员而言是显而易见的，或者本领域技术人员可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
[0031]
为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：
[0032]
图1为agv向前行驶不同时间后的角度示意图；
[0033]
图2为基于粒子群和改进动态窗口法的避障路径规划方法流程图；
[0034]
图3为agv运行环境示意图；
[0035]
图4中(a)为全局路径图，(b)为关键路径节点图；
[0036]
图5为生成路径示意图。
具体实施方式
[0037]
基于全局路径规划的粒子群优化算法生成全局路径由节点集合sol＝{p0,p1,
…
,pn}表示，并使用关键节点选取策略保留关键节点。
[0038]
为粒子群优化算法设计适应度函数如式(5)所示。
[0039][0040]
式(1)中，f
l
用于计算agv从起始点到目标点之间的路径长度，(xi,yi)和(x
i+1
,y
i+1
)分别是路径点pi和p
i+1
的位置坐标。
[0041][0042][0043][0044]
式(2)-(4)中，(ox j,oy j)是第j个障碍物圆心的坐标，v
ij
是路径中第i个节点和第j个障碍物的惩罚项，fv用于计算生成路径的风险程度。
[0045][0046]
式(5)中，(x0,y0)和(xn,yn)分别是起始点p0和目标点pn的坐标。
[0047]
使用关键节点选取策略保留关键节点，具体为从sol中的第二个节点p1开始，如果当前节点与前面一个节点之间的距离小于某一阈值时，则认为以上两个节点间距离过近，当前节点为冗余点，删除该节点，更新路径；依次遍历到倒数第2个节点p
n-1
，删除其中的所有冗余点，得到新的路径{p0,p1,
…
,pm},m≤n。
[0048]
基于局部路径规划的改进动态窗口法，对动态窗口法做出以下两个改进。
[0049]
(1)改进动态窗口法的评价子函数权重，提出新的评价函数公式如式(6)所示。
[0050][0051]
式(6)中，heading(v,ω)是方向角评价子函数，评价agv和目标方向的一致性；dist(v,ω)是距离评价子函数，评价agv与障碍物之间的距离；velocity(v,ω)是速度评价子函数，评价agv的运动性能；α、β和γ是评价子函数的权重因子；σ是归一化因子；obs
min
是agv在当前位置时，预测轨迹末端与障碍物间最小距离的平均值，其取值范围为obs
min
∈(0,2r2)；distance是起始点到目标点的距离；r2是静态障碍物膨胀化处理后的半径；η是系数。
[0052]
(2)在动态窗口法中，计算heading(v,ω)子评价函数值的参考位置是模拟轨迹的末端，常选取agv从当前位置持续行驶3s后的位置，但实际上0.1s后agv的速度就会发生改变，即模拟轨迹中只有前方一小段是agv实际将行驶到的位置，如图1中θ1为当前agv行驶方向和目标方向之间的夹角，θ2和θ3分别为预测agv向前行驶1s和3s后的agv行驶方向和目标方向之间的夹角，显然agv向前行驶3s后的预测位置和agv当前位置相对于目标点所在方向具有较大差距，而1s后的预测位置相对于目标点所在方向差距更小一些。
[0053]
为了避免参考位置偏离原位置过多而失去参考价值，选取agv向前行驶不同时间后的位置作为heading(v,ω)子评价函数值计算的参考位置，进行了大量避障路径规划仿真实验，发现将agv向前行驶1s后位置作为heading(v,ω)子评价函数值计算的参考位置时算法运行时间和生成路径长度均较优。
[0054]
因此，将方向角评价子函数值计算的参考位置由原本常用的agv向前行驶3s后的位置改为agv向前行驶1s后的位置。
[0055]
基于粒子群优化算法和改进动态窗口法的避障路径规划算法的具体算法流程如图2所示，下面结合具体的实施例对本发明的方法进行阐释：建立agv在智能仓储中的运行环境为10m
×
10m的方形场地，设置12个货架和1个打包台作为环境中的静态障碍物由大圆形表示，3个执行其他任务的agv作为环境中的动态障碍物由小圆形表示。其中4个货架是由2个货架组成的组合货架，打包台大小同8个货架组合相同，并对其分别进行膨胀化处理，本实施例中r2为agv半径为0.15m，如图3所示。
[0056]
(1)使用粒子群优化算法进行路径规划获得全局路径。设置粒子群优化算法参数ω
min
＝0.4，ω
max
＝0.9，c1＝c2＝1.5，粒子种群大小为50，最大迭代次数为100，agv起始点位置为(1,1)，目标点位置为(9,9)，迭代计算得到全局路径如图4(a)所示。
[0057]
(2)利用关键点选取策略对路径进行优化，即从全局路径中的第二个初始节点p1开始，如果当前节点与前面一个节点之间的距离小于2m时，则认为以上两个节点间距离过近，当前节点为冗余点，删除该节点，更新路径；依次遍历到倒数第2个初始节点p
n-1
，删除其中的所有冗余点，得到关键路径节点{p0,p1,
…
,p6}如图4(b)；
[0058]
(3)设定agv的初始化速度，其中线速度为0m/s，初始角速度为0rad/s，最大线速度为1m/s、最大角速度为20/180*pi rad/s、最大线加速度为0.2m/s2、最大角加速度为50/180*pi rad/s2、航向角θ0为其中p
ix
和分别为pi的横坐标和纵坐标，p
iy
和分别为p
i-1
的横坐标和纵坐标。
[0059]
(4)将全局路径中选取出的关键节点作为改进动态窗口法的局部目标点，对agv进行速度采样，获得各速度组合的模拟移动轨迹，具体步骤为：
[0060]
以图4中(b)中{p0,p1}段为例，p0点坐标为(1,1)，p1点坐标为(2.439,2.446)，航向角θ0为0.785rad，在agv的速度范围中，线速度和角速度分别以0.01m/s和为采样间隔进行速度采样。
[0061]
(5)设置α＝0.1，β＝0.3，γ＝0.2，η＝60，依据式(6)所示的评价函数计算每对速度组合的得分，选出评价函数值最大的速度组合对应的轨迹作为最优的模拟移动轨迹，以该速度组合控制agv向目标点移动，具体步骤为：
[0062]
以图4中(b)中{p0,p1}段为例，第一次进行速度采样后得到速度组合(0,-0.087)，其对应的heading(v,ω)子评价函数值为达到模拟轨迹末端时的朝向和目标点之间的角度差即174.348，dist(v,ω)子评价函数值为agv在当前模拟轨迹上与最近的障碍物之间的距离即0.639(如果该条轨迹上没有障碍物，设置为)，velocity(v,ω)子评价函数值为其当前线速度值即0。同样方式得到其他速度组合的各子评价函数值后为防止量纲不同对结果产生影响，对各数值做正则化处理，使用处理后的数据和式(6)所示的评价函数计算各速度组合的得分，选出其中最大值以该速度组合控制agv向目标点移动，本实施例中为0.066，对应的速度组合为(0.02，0)。
[0063]
(6)继承agv本段局部路径规划完成后的位置和运行速度等信息，重复步骤(3)-(4)直到agv到达最终目标点，如图5所示。
[0064]
最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。

技术特征：
1.一种基于粒子群和改进动态窗口法的避障路径规划方法，其特征在于：包括以下步骤：s1：使用粒子群优化算法进行路径规划获得全局路径；s2：利用关键点选取策略对路径进行优化，得到全局路径生成的关键节点；s3：将关键节点作为改进动态窗口法的局部目标点；s4：根据环境需要设定agv的初始化速度空间(v,ω)、航向角θ0；s5：利用改进动态窗口法对agv进行速度采样，获得各速度组合的模拟移动轨迹；s6：依据评价函数选出最优的模拟移动轨迹，以该速度组合控制agv向目标点移动；s7：继承agv本段局部路径规划完成后的位置和运行速度等信息，重复步骤s3-s4直到agv到达最终目标点。2.根据权利要求1所述的基于粒子群和改进动态窗口法的避障路径规划方法，其特征在于：步骤s1中，为粒子群优化算法设计适应度函数如式(5)所示：式(1)中，f
l
用于计算agv从起始点到目标点之间的路径长度，(x
i
,y
i
)和(x
i+1
,y
i+1
)分别是路径点p
i
和p
i+1
的位置坐标；的位置坐标；的位置坐标；式(2)-(4)中，(ox j,oy j)是第j个障碍物圆心的坐标，v
ij
是路径中第i个节点和第j个障碍物的惩罚项，fv用于计算生成路径的风险程度；式(5)中，(x0,y0)和(x
n
,y
n
)分别是起始点p0和目标点p
n
的坐标。3.根据权利要求1所述的基于粒子群和改进动态窗口法的避障路径规划方法，其特征在于：步骤s2中，使用关键节点选取策略保留关键节点，具体包括：从粒子群优化算法生成的全局路径sol中的第二个节点p1开始，如果当前节点与前面一个节点之间的距离小于某一阈值时，则认为以上两个节点间距离过近，当前节点为冗余点，删除该节点，更新路径；依次遍历到倒数第2个节点p
n-1
，删除其中的所有冗余点，得到新的路径{p0,p1,
…
,p
m
},m≤n。4.根据权利要求1所述的基于粒子群和改进动态窗口法的避障路径规划方法，其特征在于：所述改进动态窗口法具有以下评价函数：
式(6)中，heading(v,ω)是方向角评价子函数，评价agv和目标方向的一致性；dist(v,ω)是距离评价子函数，评价agv与障碍物之间的距离；velocity(v,ω)是速度评价子函数，评价agv的运动性能；α、β和γ是评价子函数的权重因子；σ是归一化因子；obs
min
是agv在当前位置时，预测轨迹末端与障碍物间最小距离的平均值，其取值范围为obs
min
∈(0,2r2)；distance是起始点到目标点的距离；r2是静态障碍物膨胀化处理后的半径；η是系数。5.根据权利要求1所述的基于粒子群和改进动态窗口法的避障路径规划方法，其特征在于：所述改进动态窗口法中，计算heading(v,ω)评价函数值的参考位置是模拟轨迹的末端，将agv从当前位置向前行驶1s后的位置作为heading(v,ω)子评价函数值计算的参考位置。

技术总结
本发明公开了一种基于粒子群和改进动态窗口法的避障路径规划方法，属于路径规划技术领域，包括以下步骤：S1：使用粒子群优化算法进行路径规划获得全局路径；S2：利用关键点选取策略对路径进行优化，得到全局路径生成的关键节点；S3：将关键节点作为改进动态窗口法的局部目标点；S4：根据环境需要设定AGV的初始化速度空间(v,ω)、航向角θ0；S5：利用改进动态窗口法对AGV进行速度采样，获得各速度组合的模拟移动轨迹；S6：依据评价函数选出最优的模拟移动轨迹，以该速度组合控制AGV向目标点移动；S7：继承AGV本段局部路径规划完成后的位置和运行速度等信息，重复步骤S3-S4直到AGV到达最终目标点。终目标点。终目标点。


技术研发人员：周雪艳 赵铖 俞晓燕 祝曼
受保护的技术使用者：西安邮电大学
技术研发日：2023.06.16
技术公布日：2023/8/16