一种融合全局及局部算法的路径规划方法及装置制造方法及图纸

本发明专利技术提供一种融合全局及局部算法的路径规划方法及装置，方法包括如下步骤：步骤S1：设置障碍物位置、起始节点S和目标节点G；步骤S2：进行全局路径规划，得到路径点集合closelist；步骤S3：判断路径点集合closelist中相邻的三点是否共线，最终形成关键点集合keylist；步骤S4：判断关键点集合keylist中，K

A path planning method and device integrating global and local algorithms

【技术实现步骤摘要】
一种融合全局及局部算法的路径规划方法及装置


[0001]本专利技术涉及一种融合全局及局部算法的路径规划方法及装置。

技术介绍

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

技术实现思路

[0003]本专利技术提出一种融合全局及局部算法的路径规划方法及装置，路径平滑且无冗余节点，提升效率，且在完成动态避障的同时还有方向性地快速逼近终点，兼具速度与安全性。
[0004]本专利技术通过以下技术方案实现：
[0005]一种融合全局及局部算法的路径规划方法，包括如下步骤：
[0006]步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；
[0007]步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,
…
C
n
]，其中，C1＝S，C
n
＝G；
[0008]步骤S3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K1,K2,
…
,K
m
]，其中，K1＝S，K
m
＝G；
[0009]步骤S4：判断关键点集合keylist中，K
i
与K
i+2
的连线是否与障碍物相交，若是，则提取K
i+1
作为优化路径点，否则，将K
i+1
从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,
…
,L
z
]，其中，L1＝S，L
z
＝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
＝L
i
的距离小于第一阈值时，设置G
min
＝L
i+1
，当子目标为G
min
＝L
i
且L
i+j
出现在最优的动作轨迹中时，设置G
min
＝L
i+j
，其中，第一阈值与机器人底盘半径相关，j＞0。
[0013]进一步的，所述步骤S5中，其中，γ
max
为最短时间通过障碍物密集区域的对应值，γ
min
为通过障碍物密集区域最安全的对应值，D为机器人到最近障碍物的距离，v
max
为预设的最大线速度，v
a
为预设的线加速度。
[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,
…
C
n
]。
[0019]进一步的，所述步骤S23中，若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变。
[0020]进一步的，所述步骤S3中，设C
i
(x
i
,y
i
)、C
i+1
(x
i+本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种融合全局及局部算法的路径规划方法，其特征在于：包括如下步骤：步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,
…
C
n
]，其中，C1＝S，C
n
＝G；步骤S3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K1,K2,
…
,K
m
]，其中，K1＝S，K
m
＝G；步骤S4：判断关键点集合keylist中，K
i
与K
i+2
的连线是否与障碍物相交，若是，则提取K
i+1
作为优化路径点，否则，将K
i+1
从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,
…
,L
z
]，其中，L1＝S，L
z
＝G，1≤i≤m
‑
2；步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γ
d
和分别为加权系数，γ
d
与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。2.根据权利要求1所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5中，将优化路径集合lastlist中的路径点作为局部路径规划的子目标，具体为：依次将优化路径集合lastlist中的路径点作为子目标G
min
，但当机器人与子目标G
min
＝L
i
的距离小于第一阈值时，设置G
min
＝L
i+1
，当子目标为G
min
＝L
i
且L
i+j
出现在最优的动作轨迹中时，设置G
min
＝L
i+j
，其中，第一阈值与机器人底盘半径相关，j＞0。3.根据权利要求1所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5中，其中，γ
max
为最短时间通过障碍物密集区域的对应值，γ
min
为通过障碍物密集区域最安全的对应值，D为机器人到最近障碍物的距离，v
max
为预设的最大线速度，v
a
为预设的线加速度。4.根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S2具体包括如下步骤：步骤S21：创建空集openlist和closelist，将起始节点S存储到closelist中；步骤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]]；步骤S23：将节点S从从openlist放入closelist中，再分别计算步骤S22中各节点的F
值，并将F值最小的节点A从openlist放入closelist中，再将节点A周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为A，并以节点A为当前节点，分别计算以这些节点为目标节点的F值，其中，F＝G+H，F值表示从起始节点到目标节点的代价消耗，G值表示从起始节点到当前节点的实际消耗，H值表示从当前节点到目标节点的估计代价消耗；步骤S24：按照步骤S23继续从openlist中找出F值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点G，逆序遍历父节点即可得到路径点集合closelist＝[C1,C2,
…
C
...

【专利技术属性】
技术研发人员：郭杰龙，俞辉，陈霞，魏宪，张剑锋，邵东恒，李杰，兰海，
申请(专利权)人：泉州装备制造研究所，
类型：发明
国别省市：