CN114779772A - 一种融合全局及局部算法的路径规划方法及装置 - Google Patents

Abstract

本发明提供一种融合全局及局部算法的路径规划方法及装置，方法包括如下步骤：步骤S1：设置障碍物位置、起始节点S和目标节点G；步骤S2：进行全局路径规划，得到路径点集合closelist；步骤S3：判断路径点集合closelist中相邻的三点是否共线，最终形成关键点集合keylist；步骤S4：判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，最终形成优化路径集合lastlist；步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，进行轨迹预测得到多个运动轨迹，利用评估函数选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度。本发明的路径平滑且无冗余节点，提升效率，且在完成动态避障的同时还有方向性地快速逼近终点，兼具速度与安全性。

Description

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

技术领域

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

背景技术

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

发明内容

本发明提出一种融合全局及局部算法的路径规划方法及装置，路径平滑且无冗余节点，提升效率，且在完成动态避障的同时还有方向性地快速逼近终点，兼具速度与安全性。

本发明通过以下技术方案实现：

一种融合全局及局部算法的路径规划方法，包括如下步骤：

步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；

步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C₁,C₂,…C_n]，其中，C₁＝S，C_n＝G；

步骤S3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K₁,K₂,…,K_m]，其中，K₁＝S，K_m＝G；

步骤S4：判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，若是，则提取K_i+1作为优化路径点，否则，将K_i+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L₁,L₂,…,L_z]，其中，L₁＝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分别为线速度和角速度。

进一步的，所述步骤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。

进一步的，所述步骤S5中，

其中，γ_max为最短时间通过障碍物密集区域的对应值，γ_min为通过障碍物密集区域最安全的对应值，D为机器人到最近障碍物的距离，

v_max为预设的最大线速度，v_a为预设的线加速度。

进一步的，所述步骤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＝[C₁,C₂,…C_n]。

进一步的，所述步骤S23中，若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变。

进一步的，所述步骤S3中，设C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)为路径点集合closelist中相邻的三点，若(x_i+1-x_i)(y_i+2-y_i+1)＝(y_i+1-y_i)(x_i+2-x_i+1)成立，判定C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)共线，否则，判定C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)不共线，其中，1≤i≤n-2。

进一步的，所述步骤S5具体包括如下步骤：

步骤S51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径以及DWA算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间；

步骤S52：按照采样周期在由步骤S51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用DWA算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹；

步骤S53：利用评估函数G(v,w)给各运动轨迹进行打分，选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度；

步骤S54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤S52，重新进行采样。

根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5中，在利用评估函数G(v,w)进行打分前，分别对heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)进行归一化。

进一步的，所述H值即为当前节点到目标节点的欧式距离。

本发明还通过以下技术方案实现：

一种融合全局及局部算法的路径规划装置，包括：

初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；

全局规划模块：用于通过基于5×5搜索邻域的A*算法进行全局路径规划，得到路径点集合closelist＝[C₁,C₂,…C_n]，其中，C₁＝S，C_n＝G；

关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K₁,K₂,…,K_m]，其中，K₁＝S，K_m＝G；

冗余点删除模块：用于判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，若是，则提取K_i+1作为优化路径点，否则，将K_i+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L₁,L₂,…,L_z]，其中，L₁＝S，L_z＝G，1≤i≤m-2；

轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数

选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γ_d和

分别为加权系数，γ_d与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。

本发明具有如下有益效果：

1、本发明先基于16邻域搜索的A*算法进行全局路径规划以得到路径点集合closelist，然后根据closelist中相邻三点是否共线进行关键点提取以得到关键点集合keylist，再将关键点集合keylist中的冗余点删除，得到优化后的全局路径集合lastlist，最后将优化后的全局路径集合lastlist作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，并利用评估函数选择最优的运动轨迹控制机器人运动，如此将全局路径规划和局部路径规划融合，得到在完成动态避障的同时还有方向性地快速逼近终点的最优路径，兼具速度与安全性，当出现未知障碍物时，机器人在行驶过程中不会与故障物发生碰撞，且始终沿着全局最优路径的目标方向行驶，提升了路径规划的效率和实时性；在全局路径规划部分，增大搜索范围，避免出现拐点过多的现象，且提取关键点，删除冗余点，精简路径且使路径更为平滑；评价函数中的加权系数γ_d与机器人到最近障碍物的距离相关，保证了路径与动态障碍物之间的距离，且使机器人能够根据环境的变化自动调整该加权系数，从而得到机器人的最佳运动速度，以获得更安全合理的路径。

附图说明

下面结合附图对本发明做进一步详细说明。

图1为本发明流程图。

图2-1为现有技术中8邻域搜索方式示意图。

图2-2为本发明中16邻域搜索方式示意图。

图3为本发明中全局路径规划的路径仿真图。

图4为本发明中全局与局部融合路径规划的路径仿真图。

其中，1、已知障碍物；2、未知障碍物；3、动态障碍物；31、动态障碍物运动轨迹；4、优化路径集合lastlist中的路径点。

具体实施方式

如图1所示，融合全局及局部算法的路径规划方法，包括如下步骤：

步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；

步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C₁,C₂,…C_n]，其中，C₁＝S，C_n＝G；

具体包括如下步骤：

步骤S21：创建空集openlist和closelist，openlist是存放等待检查的节点的列表，closelist是存放不再需要检查的节点的列表，将起始节点S存储到closelist中；

步骤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]]；

与如图2-1所示的8邻域搜索方式不同，16邻域搜索方式能够去除同方向的多余冗余子节点，提高搜索效率；

步骤S23：将节点S从从openlist放入closelist中，再分别计算步骤S22中各节点的F值，并将F值最小的节点A从openlist放入closelist中，再将节点A周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为A，并以节点A为当前节点，分别计算以这些节点为目标节点的F值，其中，F＝G+H，F值表示从起始节点到目标节点的代价消耗，G值表示从起始节点到当前节点的实际消耗，H值表示从当前节点到目标节点的估计代价消耗，选取当前节点与目标节点之间的欧式距离作为H值；

若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变；

步骤S24：按照步骤S23继续从openlist中找出F值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点G，逆序遍历父节点即可得到路径点集合closelist＝[C₁,C₂,…C_n]，若openlist中没有了数据，则说明没有合适路径，将重新设置起始点和目标点；

步骤S3：创建空集keylist作为关键点集合，将C₁添加至keylist中，判断路径点集合closelist中相邻的三点C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)是否共线，若是不共线，即(x_i+1-x_i)(y_i+2-y_i+1)＝(y_i+1-y_i)(x_i+2-x_i+1)成立，则将相邻三点的中间点C_i+1(x_i+1,y_i+1)作为关键点进行提取，若是共线，即(x_i+1-x_i)(y_i+2-y_i+1)＝(y_i+1-y_i)(x_i+2-x_i+1)不成立，则将相邻三点的中间点C_i+1(x_i+1,y_i+1)作为冗余点删除，再令i＝i+1，再次进行共线判断，最终形成关键点集合keylist＝[K₁,K₂,…,K_m]，其中，K₁＝S，K_m＝G，1≤i≤n-2；

步骤S4：创建空集lastlist作为优化路径集合，将K₁添加至lastlist中，判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，若是，则提取K_i+1作为优化路径点添加至lastlist内，否则，将K_i+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L₁,L₂,…,L_z]，规划出的路径如图3所示，其中，L₁＝S，L_z＝G，1≤i≤m-2；

步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数

选择最优的运动轨迹控制机器人运动，如图4所示即为规划出的路径，该采样速度包括线速度和角速度；α、β、γ_d和

分别为加权系数，γ_d与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度；

具体包括如下步骤：

步骤S51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径R以及DWA算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间以及评估函数的加权系数α、β和

步骤S52：按照采样周期在由步骤S51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用DWA算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹，这个过程为现有技术；

步骤S53：分别通过各运动轨迹，计算机器人与最近障碍物的距离

(x_t,y_t)为机器人当前坐标，(x₀,y₀)为障碍物坐标，计算动态范围阈值

v_max为预设的最大线速度，v_a为预设的最大线加速度，并根据公式

计算加权系数γ_d，γ_max为最短时间通过障碍物密集区域的对应值(“最短时间”的具体时长由实际情况中障碍物密集程度而定)，γ_min为通过障碍物密集区域最安全的对应值；

则评估函数

的各个加权系数均已确定，可利用评估函数进行评价打分，以选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度，其中，

其中，子函数

用于衡量机器人对子目标的方向性，θ_g为机器人指向目标节点与水平线的夹角，θ_t为机器人行进方向与水平线的夹角，两者差值越小，说明与目标节点的方位差越小；

子函数

表示运动轨迹中距离障碍物的最小距离,D表示计算机器人与最近障碍物的距离，为保证机器人移动安全性，在机器人底盘半径R的基础上增加0.2R的安全距离，当机器人到障碍物的距离大于机器人底盘半径R时一般不会发生碰撞，此时子函数值为0，即不发生碰撞时不考虑障碍物距离评价；当机器人到障碍物的距离小于或等于R时，机器人发生碰撞的可能性较大，此时值随着距离的减小而增大，即距离越近发生碰撞的影响越大；

子函数

用于评价机器人当前速度v大小，速度v越大，则评价得分越高；

子函数

用于衡量向前预估时间内所预测的点(x_m,y_m)与子目标(x_a,y_a)的距离；

但在评价之前，要对上述4个子函数进行归一化处理：

其中，i表示所预测的第i条预测轨迹，n表示共有n条预测轨迹；

步骤S54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤S52，重新进行采样。

如图3所示为全局路径规划的路径仿真图，图4为全局与局部融合的路径仿真图，当仅有已知障碍物1，而未出现未知障碍物2时，机器人根据优化路径集合lastlist中的路径点4所形成的路径行驶，但可看出，如此无法避开未知障碍物2，且会和动态障碍物的运动轨迹31相交，而图4中，当出现未知障碍物2和动态障碍物3时，机器人能够自动避开障碍物，不会与障碍物发生碰撞，且还是会沿着优化路径集合lastlist中的路径点所形成的路径方向行驶。

对应的，一种融合全局及局部算法的路径规划装置，包括：

初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；

全局规划模块：用于通过基于5×5搜索邻域的A*算法进行全局路径规划，得到路径点集合closelist＝[C₁,C₂,…C_n]，其中，C₁＝S，C_n＝G；

关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K₁,K₂,…,K_m]，其中，K₁＝S，K_m＝G；

冗余点删除模块：用于判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，若是，则提取K_i+1作为优化路径点，否则，将K_i+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L₁,L₂,…,L_z]，其中，L₁＝S，L_z＝G，1≤i≤m-2；

轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数

选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γ_d和

分别为加权系数，γ_d与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。

以上所述，仅为本发明的较佳实施例而已，故不能以此限定本发明实施的范围，即依本发明申请专利范围及说明书内容所作的等效变化与修饰，皆应仍属本发明专利涵盖的范围内。

Claims (10)

1.一种融合全局及局部算法的路径规划方法，其特征在于：包括如下步骤：

步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；

步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C₁,C₂,…C_n]，其中，C₁＝S，C_n＝G；

步骤S3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K₁,K₂,…,K_m]，其中，K₁＝S，K_m＝G；

步骤S4：判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，若是，则提取K_i+1作为优化路径点，否则，将K_i+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L₁,L₂,…,L_z]，其中，L₁＝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＝[C₁,C₂,…C_n]。

5.根据权利要求4所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S23中，若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变。

6.根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S3中，设C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)为路径点集合closelist中相邻的三点，若(x_i+1-x_i)(y_i+2-y_i+1)＝(y_i+1-y_i)(x_i+2-x_i+1)成立，判定C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)共线，否则，判定C_i(x_i,y_i)、C_i+1(x_i+1,y_i+1)和C_i+2(x_i+2,y_i+2)不共线，其中，1≤i≤n-2。

7.根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5具体包括如下步骤：

步骤S51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径以及DWA算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间；

步骤S52：按照采样周期在由步骤S51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用DWA算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹；

步骤S53：利用评估函数G(v,w)给各运动轨迹进行打分，选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度；

步骤S54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤S52，重新进行采样。

8.根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5中，在利用评估函数G(v,w)进行打分前，分别对heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)进行归一化。

9.根据权利要求4所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述H值即为当前节点到目标节点的欧式距离。

10.一种融合全局及局部算法的路径规划装置，其特征在于：包括：

初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；

全局规划模块：用于通过基于5×5搜索邻域的A*算法进行全局路径规划，得到路径点集合closelist＝[C₁,C₂,…C_n]，其中，C₁＝S，C_n＝G；

关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K₁,K₂,…,K_m]，其中，K₁＝S，K_m＝G；

冗余点删除模块：用于判断关键点集合keylist中，K_i与K_i+2的连线是否与障碍物相交，若是，则提取K_i+1作为优化路径点，否则，将K_i+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L₁,L₂,…,L_z]，其中，L₁＝S，L_z＝G，1≤i≤m-2；

轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数

选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γ_d和

分别为加权系数，γ_d与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。

Images