一种融合全局及局部算法的路径规划方法及装置与流程

1.本发明涉及一种融合全局及局部算法的路径规划方法及装置。


背景技术：

2.对于机器人运动轨迹的规划，在全局路径规划技术方面已经取得了不错成果，但是全局规划仅适用于活动区域信息己知的情况，当环境比较复杂时，如存在各种不规则障碍物时，全局路径规划可能无法适用，而且现有技术中的全局路径规划还存在冗余节点过多、拐点过多、路径过长的缺陷。而局部路径规划虽然可以在环境未知的情况下使用，但其在复杂环境中计算量过大，消耗时间过长。现有技术中也有将全局路径规划和局部路径规划融合的方案，但是仍然存在拐点过多、路径不平滑、冗余节点过多、路径过长、最终路径与全局路径相差较大、速度与安全性无法同时满足的缺陷。


技术实现要素：

3.本发明提出一种融合全局及局部算法的路径规划方法及装置，路径平滑且无冗余节点，提升效率，且在完成动态避障的同时还有方向性地快速逼近终点，兼具速度与安全性。
4.本发明通过以下技术方案实现：
5.一种融合全局及局部算法的路径规划方法，包括如下步骤：
6.步骤s1：初始化栅格地图，设置障碍物位置、起始节点s和目标节点g；
7.步骤s2：通过基于16邻域搜索的a*算法进行全局路径规划，得到路径点集合closelist＝[c1,c2,
…cn
]，其中，c1＝s，cn＝g；
[0008]
步骤s3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[k1,k2,
…
,km]，其中，k1＝s，km＝g；
[0009]
步骤s4：判断关键点集合keylist中，ki与k
i+2
的连线是否与障碍物相交，若是，则提取k
i+1
作为优化路径点，否则，将k
i+1
从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[l1,l2,
…
,lz]，其中，l1＝s，lz＝g，1≤i≤m-2；
[0010]
步骤s5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用dwa算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。
[0011]
进一步的，所述步骤s5中，将优化路径集合lastlist中的路径点作为局部路径规
划的子目标，具体为：
[0012]
依次将优化路径集合lastlist中的路径点作为子目标g
min
，但当机器人与子目标g
min
＝li的距离小于第一阈值时，设置g
min
＝l
i+1
，当子目标为g
min
＝li且l
i+j
出现在最优的动作轨迹中时，设置g
min
＝l
i+j
，其中，第一阈值与机器人底盘半径相关，j＞0。
[0013]
进一步的，所述步骤s5中，其中，γ
max
为最短时间通过障碍物密集区域的对应值，γ
min
为通过障碍物密集区域最安全的对应值，d为机器人到最近障碍物的距离，v
max
为预设的最大线速度，va为预设的线加速度。
[0014]
进一步的，所述步骤s2具体包括如下步骤：
[0015]
步骤s21：创建空集openlist和closelist，将起始节点s存储到closelist中；
[0016]
步骤s22：寻找起始节点s周围16个方向上可以到达的24个节点，将它们放入openlist中，并设置它们的父节点为s，其中，以节点s为原点，24个节点的坐标为：[[1，0]，[0，1]，[-1，0]，[0，-1]，[-1，-1]，[-1，1]，[1，-1]，[1，1]，[2，0]，[0，2]，[-2，0]，[0，-2]，[-2，-2]，[-2，2]，[2，-2]，[2，2]，[1，3]，[3，1]，[3，-1]，[1，-3]，[-1，3]，[-3，1]，[-3，-1]，[-1，-3]]；
[0017]
步骤s23：将节点s从从openlist放入closelist中，再分别计算步骤s22中各节点的f值，并将f值最小的节点a从openlist放入closelist中，再将节点a周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为a，并以节点a为当前节点，分别计算以这些节点为目标节点的f值，其中，f＝g+h，f值表示从起始节点到目标节点的代价消耗，g值表示从起始节点到当前节点的实际消耗，h值表示从当前节点到目标节点的估计代价消耗；
[0018]
步骤s24：按照步骤s23继续从openlist中找出f值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点g，逆序遍历父节点即可得到路径点集合closelist＝[c1,c2,
…cn
]。
[0019]
进一步的，所述步骤s23中，若节点a可以到达的节点b本就已经在openlist中，则计算从起始节点s经节点a到达节点b的g值，将该g值与从起始节点s直接到达节点b的g值进行比较，若前者更低，则将节点b的父节点修改为a，并重新计算节点b的f值，否则，则不作改变。
[0020]
进一步的，所述步骤s3中，设ci(xi,yi)、c
i+1
(x
i+1
,y
i+1
)和c
i+2
(x
i+2
,y
i+2
)为路径点集合closelist中相邻的三点，若(x
i+1-xi)(y
i+2-y
i+1
)＝(y
i+1-yi)(x
i+2-x
i+1
)成立，判定ci(xi,yi)、c
i+1
(x
i+1
,y
i+1
)和c
i+2
(x
i+2
,y
i+2
)共线，否则，判定ci(xi,yi)、c
i+1
(x
i+1
,y
i+1
)和c
i+2
(x
i+2
,y
i+2
)不共线，其中，1≤i≤n-2。
[0021]
进一步的，所述步骤s5具体包括如下步骤：
[0022]
步骤s51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径以及dwa算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间；
[0023]
步骤s52：按照采样周期在由步骤s51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用dwa算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹；
[0024]
步骤s53：利用评估函数g(v,w)给各运动轨迹进行打分，选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度；
[0025]
步骤s54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤s52，重新进行采样。
[0026]
根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤s5中，在利用评估函数g(v,w)进行打分前，分别对heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)进行归一化。
[0027]
进一步的，所述h值即为当前节点到目标节点的欧式距离。
[0028]
本发明还通过以下技术方案实现：
[0029]
一种融合全局及局部算法的路径规划装置，包括：
[0030]
初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点s和目标节点g；
[0031]
全局规划模块：用于通过基于5
×
5搜索邻域的a*算法进行全局路径规划，得到路径点集合closelist＝[c1,c2,
…cn
]，其中，c1＝s，cn＝g；
[0032]
关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[k1,k2,
…
,km]，其中，k1＝s，km＝g；
[0033]
冗余点删除模块：用于判断关键点集合keylist中，ki与k
i+2
的连线是否与障碍物相交，若是，则提取k
i+1
作为优化路径点，否则，将k
i+1
从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[l1,l2,
…
,lz]，其中，l1＝s，lz＝g，1≤i≤m-2；
[0034]
轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用dwa算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。
[0035]
本发明具有如下有益效果：
[0036]
1、本发明先基于16邻域搜索的a*算法进行全局路径规划以得到路径点集合closelist，然后根据closelist中相邻三点是否共线进行关键点提取以得到关键点集合keylist，再将关键点集合keylist中的冗余点删除，得到优化后的全局路径集合lastlist，最后将优化后的全局路径集合lastlist作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用dwa算法进行轨迹预测，并利用评估函数选择最优的运动轨迹控制机器人运动，如此将全局路径规划和局部路径规划融合，得到在完成动态避障的同时还有方向性地快速逼近终点的最优路径，兼具速度与安全性，当出现未知障碍物时，机器人在行驶过程中不会与故障物发生碰撞，且始终沿着全局最优路径的目标方向行
驶，提升了路径规划的效率和实时性；在全局路径规划部分，增大搜索范围，避免出现拐点过多的现象，且提取关键点，删除冗余点，精简路径且使路径更为平滑；评价函数中的加权系数γd与机器人到最近障碍物的距离相关，保证了路径与动态障碍物之间的距离，且使机器人能够根据环境的变化自动调整该加权系数，从而得到机器人的最佳运动速度，以获得更安全合理的路径。
附图说明
[0037]
下面结合附图对本发明做进一步详细说明。
[0038]
图1为本发明流程图。
[0039]
图2-1为现有技术中8邻域搜索方式示意图。
[0040]
图2-2为本发明中16邻域搜索方式示意图。
[0041]
图3为本发明中全局路径规划的路径仿真图。
[0042]
图4为本发明中全局与局部融合路径规划的路径仿真图。
[0043]
其中，1、已知障碍物；2、未知障碍物；3、动态障碍物；31、动态障碍物运动轨迹；4、优化路径集合lastlist中的路径点。
具体实施方式
[0044]
如图1所示，融合全局及局部算法的路径规划方法，包括如下步骤：
[0045]
步骤s1：初始化栅格地图，设置障碍物位置、起始节点s和目标节点g；
[0046]
步骤s2：通过基于16邻域搜索的a*算法进行全局路径规划，得到路径点集合closelist＝[c1,c2,
…cn
]，其中，c1＝s，cn＝g；
[0047]
具体包括如下步骤：
[0048]
步骤s21：创建空集openlist和closelist，openlist是存放等待检查的节点的列表，closelist是存放不再需要检查的节点的列表，将起始节点s存储到closelist中；
[0049]
步骤s22：寻找起始节点s周围16个方向上可以到达的24个节点，将它们放入openlist中，并设置它们的父节点为s，其中，16邻域搜索方式如图2-2所示，以节点s为原点，24个节点的坐标为：[[1，0]，[0，1]，[-1，0]，[0，-1]，[-1，-1]，[-1，1]，[1，-1]，[1，1]，[2，0]，[0，2]，[-2，0]，[0，-2]，[-2，-2]，[-2，2]，[2，-2]，[2，2]，[1，3]，[3，1]，[3，-1]，[1，-3]，[-1，3]，[-3，1]，[-3，-1]，[-1，-3]]；
[0050]
与如图2-1所示的8邻域搜索方式不同，16邻域搜索方式能够去除同方向的多余冗余子节点，提高搜索效率；
[0051]
步骤s23：将节点s从从openlist放入closelist中，再分别计算步骤s22中各节点的f值，并将f值最小的节点a从openlist放入closelist中，再将节点a周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为a，并以节点a为当前节点，分别计算以这些节点为目标节点的f值，其中，f＝g+h，f值表示从起始节点到目标节点的代价消耗，g值表示从起始节点到当前节点的实际消耗，h值表示从当前节点到目标节点的估计代价消耗，选取当前节点与目标节点之间的欧式距离作为h值；
[0052]
若节点a可以到达的节点b本就已经在openlist中，则计算从起始节点s经节点a到
达节点b的g值，将该g值与从起始节点s直接到达节点b的g值进行比较，若前者更低，则将节点b的父节点修改为a，并重新计算节点b的f值，否则，则不作改变；
[0053]
步骤s24：按照步骤s23继续从openlist中找出f值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点g，逆序遍历父节点即可得到路径点集合closelist＝[c1,c2,
…cn
]，若openlist中没有了数据，则说明没有合适路径，将重新设置起始点和目标点；
[0054]
步骤s3：创建空集keylist作为关键点集合，将c1添加至keylist中，判断路径点集合closelist中相邻的三点ci(xi,yi)、c
i+1
(x
i+1
,y
i+1
)和c
i+2
(x
i+2
,y
i+2
)是否共线，若是不共线，即(x
i+1-xi)(y
i+2-y
i+1
)＝(y
i+1-yi)(x
i+2-x
i+1
)成立，则将相邻三点的中间点c
i+1
(x
i+1
,y
i+1
)作为关键点进行提取，若是共线，即(x
i+1-xi)(y
i+2-y
i+1
)＝(y
i+1-yi)(x
i+2-x
i+1
)不成立，则将相邻三点的中间点c
i+1
(x
i+1
,y
i+1
)作为冗余点删除，再令i＝i+1，再次进行共线判断，最终形成关键点集合keylist＝[k1,k2,
…
,km]，其中，k1＝s，km＝g，1≤i≤n-2；
[0055]
步骤s4：创建空集lastlist作为优化路径集合，将k1添加至lastlist中，判断关键点集合keylist中，ki与k
i+2
的连线是否与障碍物相交，若是，则提取k
i+1
作为优化路径点添加至lastlist内，否则，将k
i+1
从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[l1,l2,
…
,lz]，规划出的路径如图3所示，其中，l1＝s，lz＝g，1≤i≤m-2；
[0056]
步骤s5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用dwa算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，如图4所示即为规划出的路径，该采样速度包括线速度和角速度；α、β、γd和分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度；
[0057]
具体包括如下步骤：
[0058]
步骤s51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径r以及dwa算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间以及评估函数的加权系数α、β和
[0059]
步骤s52：按照采样周期在由步骤s51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用dwa算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹，这个过程为现有技术；
[0060]
步骤s53：分别通过各运动轨迹，计算机器人与最近障碍物的距离(x
t
,y
t
)为机器人当前坐标，(x0,y0)为障碍物坐标，计算动态范围阈值v
max
为预设的最大线速度，va为预设的最大线加速度，并根据公式计算加权系数γd，γ
max
为最短时间通过障碍物密
集区域的对应值(“最短时间”的具体时长由实际情况中障碍物密集程度而定)，γ
min
为通过障碍物密集区域最安全的对应值；
[0061]
则评估函数的各个加权系数均已确定，可利用评估函数进行评价打分，以选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度，其中，
[0062]
其中，子函数用于衡量机器人对子目标的方向性，θg为机器人指向目标节点与水平线的夹角，θ
t
为机器人行进方向与水平线的夹角，两者差值越小，说明与目标节点的方位差越小；
[0063]
子函数表示运动轨迹中距离障碍物的最小距离,d表示计算机器人与最近障碍物的距离，为保证机器人移动安全性，在机器人底盘半径r的基础上增加0.2r的安全距离，当机器人到障碍物的距离大于机器人底盘半径r时一般不会发生碰撞，此时子函数值为0，即不发生碰撞时不考虑障碍物距离评价；当机器人到障碍物的距离小于或等于r时，机器人发生碰撞的可能性较大，此时值随着距离的减小而增大，即距离越近发生碰撞的影响越大；
[0064]
子函数用于评价机器人当前速度v大小，速度v越大，则评价得分越高；
[0065]
子函数用于衡量向前预估时间内所预测的点(xm,ym)与子目标(xa,ya)的距离；
[0066]
但在评价之前，要对上述4个子函数进行归一化处理：
[0067]
其中，i表示所预测的第i条预测轨迹，n表示共有n条预测轨迹；
[0068]
步骤s54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次
进入步骤s52，重新进行采样。
[0069]
如图3所示为全局路径规划的路径仿真图，图4为全局与局部融合的路径仿真图，当仅有已知障碍物1，而未出现未知障碍物2时，机器人根据优化路径集合lastlist中的路径点4所形成的路径行驶，但可看出，如此无法避开未知障碍物2，且会和动态障碍物的运动轨迹31相交，而图4中，当出现未知障碍物2和动态障碍物3时，机器人能够自动避开障碍物，不会与障碍物发生碰撞，且还是会沿着优化路径集合lastlist中的路径点所形成的路径方向行驶。
[0070]
对应的，一种融合全局及局部算法的路径规划装置，包括：
[0071]
初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点s和目标节点g；
[0072]
全局规划模块：用于通过基于5
×
5搜索邻域的a*算法进行全局路径规划，得到路径点集合closelist＝[c1,c2,
…cn
]，其中，c1＝s，cn＝g；
[0073]
关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[k1,k2,
…
,km]，其中，k1＝s，km＝g；
[0074]
冗余点删除模块：用于判断关键点集合keylist中，ki与k
i+2
的连线是否与障碍物相交，若是，则提取k
i+1
作为优化路径点，否则，将k
i+1
从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[l1,l2,
…
,lz]，其中，l1＝s，lz＝g，1≤i≤m-2；
[0075]
轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用dwa算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。
[0076]
以上所述，仅为本发明的较佳实施例而已，故不能以此限定本发明实施的范围，即依本发明申请专利范围及说明书内容所作的等效变化与修饰，皆应仍属本发明专利涵盖的范围内。