CN113341991A - 一种基于动态窗口和冗余节点过滤的路径优化方法 - Google Patents

Abstract

本发明公开了一种基于动态窗口和冗余节点过滤的路径优化方法，其包括利用已有的栅格地图数据集，确定运动的起始位置和目标位置的坐标信息，在栅格地图中标注目标节点和障碍物节点；利用A^*算法规划出一条全局路径；利用A^*算法规划处的全局路径进行全局优化，过滤掉冗余节点；结合动态窗口法对优化后的全局路径进行分段局部优化，得到最终的全局路径。本发明使用组合算法减少了移动机器人的单次运动时长，提高了运动路径曲线的平滑性；同时改善了机器人在静态行驶路线过程中的问题，在规划路径段内可以实现灵活的避障功能。

Description

一种基于动态窗口和冗余节点过滤的路径优化方法

技术领域

本发明涉及移动机器人技术中的运动规划领域，更具体地涉及一种可用于移动机器人的路径规划及优化方法。

背景技术

机器人技术作为一个综合了多个学科的技术，是多年研究的热门。其中，路径规划作为移动机器人技术研发的关键技术，其目的在于参照某一个参数的指标(如代价最低、选择路径最短、运算时间最短等)，在任务区域选择出一条可以从起点运动到终点的最优或次优的避障路径。路径规划按照机器人对工作区域信息的理解层次，可以分为给予完整区域信息理解的全局路径规划和基于部分区域信息理解的局部路径规划，两者各有优缺。现在实际工作中大多情况下都需要融合兼用全局和局部路径规划，前者旨在寻找全局优化路径，后者旨在实时避障。

A^*算法作为最流行的全局路径查找选择方式之一，具有高度灵活性，且可以广泛应用于不同的环境之中。A^*算法的成功秘诀在于它结合了Dijkstra算法使用的信息(对接近起点的位置有利)和贪婪最佳优先搜索使用的信息(对接近目的地的位置有利)。其通过计算路由节点成本，选择成本最低的节点，通过依次扩展具有最小启发函数的节点来寻找最优路径。虽然传统的A*算法能得到一条长度相对比较优秀的路径，但是该路径存在的问题也很明显：有冗余节点、转折点较多，这些对机器人实际的运动都是不利的存在。

动态窗口法因为其符合移动机器人的运动特征，具有灵活性强等优点，成为了动态环境下局部避障的主要算法。动态窗口法的基本思想是结合移动机器人的运动特性，在路径规划过程中，实时预测时间t内移动机器人的速度空间(v_t，w_t)和状态空间(x_t，y_t，yaw_t，v_t，w_t)，模拟移动机器人在预测时间内的运动轨迹，再根据评价函数确定最优的运动轨迹，从而到达目标位置完成路径规划。该方法属于一个局部的路径规划，机器人如果只在这个算法下运动，虽然可以保证一个良好的运动姿态，但是要达到目标点需要遍历太多节点，效率很低。

发明内容

针对现有技术存在的上述问题，本发明的要解决的技术问题是：如何尽可能的优化路线，使机器人的路线更平滑和可靠。

为解决上述第一个技术问题，本发明采用如下技术方案：一种基于动态窗口和冗余节点过滤的路径优化方法，包括以下步骤：

S100：利用已有的栅格地图数据集，确定运动的起始位置和目标位置的坐标信息，在栅格地图中标注目标节点和障碍物节点；

S200：利用A^*算法规划出一条全局路径；

S300：利用A^*算法规划处的全局路径进行全局优化，过滤掉冗余节点；

S400：结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全局路径。

作为改进，所述S200中利用A^*算法规划出一条全局路径，其包括以下步骤：

S210：构建并且初始化两个空表OpenList和CloseList，将起始位置设置为当前节点并存入OpenList列表；如果当前节点不是目标点，那么调用扩充节点函数选出与当前节点相邻且可扩展的所有节点，并且将扩展的所有节点信息都存入OpenList列表；

S220：调用插入节点函数，遍历可以扩展的所有节点，计算出这些节点对应的代价函数F，代价函数表示为：

F(n)＝G(n)+H(n)

其中，n表示当前节点；F(n)是当前节点n的代价函数；G(n)是移动机器人从初始节点到节点n的实际代价值；H(n)是从当前节点n到达目标点的代价值，选取这两点之间的欧式距离代表H(n)，H(n)函数表示为：

其中，(x_g,y_g)代表目标节点在栅格地图中的坐标，(x_n，y_n)代表当前节点在栅格地图中的坐标；

S230：选出代价函数F(n)最小值所对应的节点，将其从OpenList列表中删除，存入CloseList列表之中，同时将该节点设为当前节点连接至上一节点，重复S220，直至当前节点为目标点，导出该全局路径。

作为改进，所述S300利用A^*算法规划处的全局路径进行全局优化，过滤掉冗余节点，包括以下步骤：

S310：将S200得到的CloseList列表中的路径节点集记为P{P_i,1≤j≤n}，其中P₁为路径的起点，P_n为路径的终点，创建初始值只有P₁和P_n的关键点集合U{P₁,P_n}用于存放路径优化之后的关键节点；

S320：对于节点集P{P_i,1≤j≤n}，令m＝2,3,4…n，由P₁开始依次连接P₂,P₃,…,P_m，判断直线P₁P_m之间是否存在阻碍物节点，如果存在，则节点P_m-1为路径的必经节点；如果直线P₁P_m没有穿过障碍物节点，则判定P_m-1为冗余节点；

将所有必经节点都添加至集合U中，关键点选取完成之后，集合U{P₁，P_m-1，...，P_m+k，P_n}(2≤m≤n，1≤k≤n-m)包含了所有关键节点，假设U中的节点数目为r，即U{P₁，P₂，...，P_r}，(1≤r≤n)；

S330：依次连接集合U中的所有节点，完成对路径的全局优化。

作为改进，所述S400所述的结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全局路径的具体步骤如下：

S410：针对集合U，从起点P₁开始，依次将除开终点P_r之外的所有节点作为局部路径规划的起点，记作{S₁，S₂，...，S_r-1}；同时将集合U中的第二个节点P₂开始，依次将{P₂，P₃，...，P_r}记作局路径规划的终点{D₁，D₂，...，D_r-1}，则可以将全局路径分为S₁D₁，S₂D₂，...，S_r-1D_r-1共r-1段，记S₁在栅格地图中的坐标值为

D₁的坐标值为

以此类推；

S420：初始化移动机器人的状态参数集L(l)，l(x，y,yaw,v,w)记录机器人运动在规划路径中的状态参数，包括位置、航向角、线速度、角速度，设定机器人的初始线速度为v(m/s)，初始角速度为w(rad/s)和初始导航角yaw(rad)；

S430：计算S_iD_i的倾斜角度，计算公式如下：

其中，

分别是D_i的横纵坐标，

分别是S_i的横纵坐标；

将S_iD_i段路径的倾斜角度转换为弧度值作为移动机器人的初始航向角，斜率角度转化为弧度值的公式为：

yaw_i＝α×180°÷π

得到在S_iD_i段路径中机器人的所有状态参数l_i(x_i,y_i,yaw_i,v_i,w_i),1≤i≤r，其中，x_i,y_i是节点S_i的横纵坐标，v_i是S_iD_i段路径中读取的上一段路径的终点处的线速度，w_i是S_iD_i段路径中读取的上一段路径的终点处的角速度；

S440：按照S430中计算S_i+1D_i+1段路径的倾斜角度α_i+1，同时将机器人上一个状态参数l_i中的yaw_i转换为角度值β，弧度值转化为角度值的公式如下：

β＝yaw_i×π÷180°

比较α_i+1和β，如果|α_i+1-β|＜60°，则机器人保持最新的状态l_i+1(x_i+1,y_i+1,yaw_i+1,v_i+1,w_i+1)，如果|α_i+1-β|≥60°，为避免移动机器人因为绕路转弯造成了冗余路段和耗费不必要运动时间，则令机器人导航角

S450：重复步骤S440，直至移动机器人到达第r-1路段的终点D_r-1，得到记录移动机器人所有的状态参数集L(l₁,l₂,...,l_i,l_i+1,...,l_r)，完成局部路径的优化。

相对于现有技术，本发明至少具有如下优点：

1.本发明一种基于动态窗口和冗余节点过滤的路径优化方法，通过将A^*算法和动态窗口融合，并且对全局规划和局部规划进行优化处理，一方面在很大程度上缓解了两种算法各种的局限性，传统的A*算法能得到一条长度相对比较优秀的路径，但是该路径有冗余节点、转折点较多；而动态窗口法属于一个局部的路径规划，机器人如果只在这个算法下运动，虽然可以保证一个良好的运动姿态，但是要达到目标点需要遍历太多节点，效率很低。本专利首先利用在一个地图中已经规划好的一个全局路径的基础上，先进行去除冗余节点的操作，之后在此优化后路径的基础上，利用动态窗口法进行有针对性(针对折点)的局部优化，达到取长补短的目的，最后能够取得一个长度比较优秀，同时符合机器人实际运动的一个更加平滑的路径。另一方面使得规划出的路径更加平滑更加安全，使得移动机器人的运动更有效率。首先，通过A^*算法规划出一条全局路径，再利用一种过滤冗余节点保留关键节点的算法剔除全局路径中的没必要节点，保证路径的全局最优。之后，根据关键节点，结合动态窗口法的思想对每段局部进行局部优化，提高路径的安全性和平滑性。

2.本发明具有较高的准确性、安全性和运动的效率性，根据不同场景的路径规划也具备优秀的适应能力，能够很好支持移动机器人在特定环境下的巡逻任务。

3.本发明所述方法将全局路径规划A^*算法和局部路径规划动态窗口算法组合，并且分别进行优化处理，从而为移动机器人规划出路径长度最短、路径平滑度高、安全性高的可靠路径。

附图说明

图1是本发明路径规划及优化方法的流程示意图。

图2是A^*算法及全局优化的程序流程示意图。

图3为基于动态窗口法思想的局部优化的流程示意图。

具体实施方式

下面结合附图附本发明的实施方式进行详细描述。

参见图1-3，一种基于动态窗口和冗余节点过滤的路径优化方法，包括以下步骤：

S100：利用已有的栅格地图数据集，确定运动的起始位置和目标位置的坐标信息，在栅格地图中标注目标节点和障碍物节点；其中地图数据集可通过人工测量、遥感测量、摄影测量、卫星数据等途径得到，也可通过操作移动机器人在未知环境重复采集地图信息得到。

S200：利用A^*算法规划出一条全局路径，包括如下步骤：

S210：构建并且初始化两个空表OpenList和CloseList，将起始位置设置为当前节点并存入OpenList列表；如果当前节点不是目标点，那么调用扩充节点函数选出与当前节点相邻且可扩展的所有节点【去掉障碍物节点后的所有节点】，并且将扩展的所有节点信息都存入OpenList列表；

S220：调用插入节点函数，遍历可以扩展的所有节点，计算出这些节点对应的代价函数F，代价函数表示为：

F(n)＝G(n)+H(n)

其中，n表示当前节点；F(n)是当前节点n的代价函数；G(n)是移动机器人从初始节点到节点n的实际代价值；H(n)是从当前节点n到达目标点的代价值，选取这两点之间的欧式距离代表H(n)，H(n)函数表示为：

其中，(x_g,y_g)代表目标节点在栅格地图中的坐标，(x_n,y_n)代表当前节点在栅格地图中的坐标；

S230：选出代价函数F(n)最小值所对应的节点，将其从OpenList列表中删除，存入CloseList列表之中，同时将该节点设为当前节点连接至上一节点，重复S220，直至当前节点为目标点，导出该全局路径。

S300：利用A^*算法规划处的全局路径进行全局优化，过滤掉冗余节点，包括以下步骤：

S310：将S200得到的CloseList列表中的路径节点集记为P{P_i,1≤j≤n}，其中P₁为路径的起点，P_n为路径的终点，创建初始值只有P₁和P_n的关键点集合U{P₁,P_n}用于存放路径优化之后的关键节点；

S320：对于节点集P{P_i,1≤j≤n}，令m＝2,3,4…n，由P₁开始依次连接P₂,P₃,…,P_m，判断直线P₁P_m之间是否存在阻碍物节点，如果存在，则节点P_m-1为路径的必经节点；如果直线P₁P_m没有穿过障碍物节点，则判定P_m-1为冗余节点；

将所有必经节点都添加至集合U中，关键点选取完成之后，集合U{P₁,P_m-1,...,P_m+k,P_n}(2≤m≤n,1≤k≤n-m)包含了所有关键节点，假设U中的节点数目为r，即U{P₁,P₂,...,P_r}，(1≤r≤n)；

S330：依次连接集合U中的所有节点，完成对路径的全局优化，包括如下步骤：具体步骤如下：

S410：针对集合U，从起点P₁开始，依次将除开终点P_r之外的所有节点作为局部路径规划的起点，记作{S₁,S₂,...,S_r-1}；同时将集合U中的第二个节点P₂开始，依次将{P₂,P₃,...,P_r}记作局路径规划的终点{D₁,D₂,...,D_r-1}，则可以将全局路径分为S₁D₁,S₂D₂,...,S_r-1D_r-1共r-1段，记S₁在栅格地图中的坐标值为

D₁的坐标值为

以此类推；

S420：初始化移动机器人的状态参数集L(l)，l(x,y,yaw,v,w)记录机器人运动在规划路径中的状态参数，包括位置、航向角、线速度、角速度，设定机器人的初始线速度为v(m/s)，初始角速度为w(rad/s)和初始导航角yaw(rad)；

S430：计算S_iD_i的倾斜角度，计算公式如下：

其中，

分别是D_i的横纵坐标，

分别是S_i的横纵坐标；

将S_iD_i段路径的倾斜角度转换为弧度值作为移动机器人的初始航向角，斜率角度转化为弧度值的公式为：

yaw_i＝α×180°÷π

得到在S_iD_i段路径中机器人的所有状态参数l_i(x_i,y_i,yaw_i，v_i，w_i)，1≤i≤r，其中，x_i,y_i是节点S_i的横纵坐标，v_i是S_iD_i段路径中读取的上一段路径的终点处的线速度，w_i是S_iD_i段路径中读取的上一段路径的终点处的角速度；

S440：按照S430中计算S_i+1D_i+1段路径的倾斜角度α_i+1，同时将机器人上一个状态参数l_i中的yaw_i转换为角度值β，弧度值转化为角度值的公式如下：

β＝yaw_i×π÷180°

比较α_i+1和β，如果|α_i+1-β|＜60°，则机器人保持最新的状态l_i+1(x_i+1，y_i+1，yaw_i+1，v_i+1，w_i+1)，如果|α_i+1-β|≥60°，为避免移动机器人因为绕路转弯造成了冗余路段和耗费不必要运动时间，则令机器人导航角

S450：重复步骤S440，直至移动机器人到达第r-1路段的终点D_r-1，得到记录移动机器人所有的状态参数集L(l₁，l₂，...，l_i，l_i+1，...，l_r)，完成局部路径的优化。

S400：结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全局路径。

动态窗口法简称DWA，DWA的核心是通过不同的角速度和线速度来预测机器人的未来轨迹，本发明是利用DWA方法在行进过程中的实况进行优化，简单的可以理解为：A^*算法规划出一条全局路径，该全局路径就是机器人的运动路径，过滤掉冗余节点就相当于不让机器人多走冤枉路。当机器人行走在优化后的全局路径上时，利用DWA方法，避开了该路径上的“坑坑洼洼”就好比开车时避让路上石头，坑洼，弯道很宽，我们从内圈转弯。

实验对比：

针对本发明专利提及的可用于移动机器人的路径规划及优化方法，为凸显该发明的创新点，将该技术与传统A^*算法、Dijkstra算法、RRT算法进行对比仿真实验得到下表数据。在仿真实验中，构建了三个尺寸一致、障碍物随机生成的栅格地图；水平方向35个方格，竖直方向33个方格，分辨率10cm*10cm；设定起点坐标为(0，0)，终点坐标为(31，28)。

不同算法之间的长度、计算时间与拐点数对比如下表所示。

由上表我们可以得知，在三个仿真实验中，4种路径规划的算法都能够规划出路径，但是在路径长度、计算时间、拐点数等方面处在一定的差异。

从表中内容我们可以看出，RRT算法的计算时间最短，但是其拐点数过多，造成这个结果的原因在于该算法的路径是由一个一个随机树节点组成，可能包含棱角，不够光滑，并非理想的平滑曲线，无利于机器人的实际运动。Dijkstra算法、A^*算法、本算法都获得了相比较RRT算法更优的全局路径，但是相比较于Dijkstra算法和A^*算法，本算法的优势在于路径长度更短，其拐点较少，路径的平滑性更好，适合移动机器人的实际运动，便于机器人执行路径轨迹达到目标点。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (4)

1.一种基于动态窗口和冗余节点过滤的路径优化方法，其特征在于，包括以下步骤：

S100：利用已有的栅格地图数据集，确定运动的起始位置和目标位置的坐标信息，在栅格地图中标注目标节点和障碍物节点；

S200：利用A^*算法规划出一条全局路径；

S300：利用A^*算法规划处的全局路径进行全局优化，过滤掉冗余节点；

S400：结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全局路径。

2.如权利要求1所述的基于动态窗口和冗余节点过滤的路径优化方法，其特征在于，所述S200中利用A^*算法规划出一条全局路径，其包括以下步骤：

S210：构建并且初始化两个空表OpenList和CloseList，将起始位置设置为当前节点并存入OpenList列表；如果当前节点不是目标点，那么调用扩充节点函数选出与当前节点相邻且可扩展的所有节点，并且将扩展的所有节点信息都存入OpenList列表；

S220：调用插入节点函数，遍历可以扩展的所有节点，计算出这些节点对应的代价函数F，代价函数表示为：

F(n)＝G(n)+H(n)

其中，n表示当前节点；F(n)是当前节点n的代价函数；G(n)是移动机器人从初始节点到节点n的实际代价值；H(n)是从当前节点n到达目标点的代价值，选取这两点之间的欧式距离代表H(n)，H(n)函数表示为：

其中，(x_g，y_g)代表目标节点在栅格地图中的坐标，(x_n，y_n)代表当前节点在栅格地图中的坐标；

S230：选出代价函数F(n)最小值所对应的节点，将其从OpenList列表中删除，存入CloseList列表之中，同时将该节点设为当前节点连接至上一节点，重复S220，直至当前节点为目标点，导出该全局路径。

3.如权利要求2所述的基于动态窗口和冗余节点过滤的路径优化方法，其特征在于，所述S300利用A^*算法规划处的全局路径进行全局优化，过滤掉冗余节点，包括以下步骤：

S310：将S200得到的CloseList列表中的路径节点集记为P{P_i，1≤j≤n}，其中P₁为路径的起点，P_n为路径的终点，创建初始值只有P₁和P_n的关键点集合U{P₁，P_n}用于存放路径优化之后的关键节点；

S320：对于节点集P{P_i，1≤j≤n}，令m＝2，3，4…n，由P₁开始依次连接P₂，P₃，…，P_m，判断直线P₁P_m之间是否存在阻碍物节点，如果存在，则节点P_m-1为路径的必经节点；如果直线P₁P_m没有穿过障碍物节点，则判定P_m-1为冗余节点；

将所有必经节点都添加至集合U中，关键点选取完成之后，集合U{P₁，P_m-1，...，P_m+k，P_n}(2≤m≤n，1≤k≤n-m)包含了所有关键节点，假设U中的节点数目为r，即U{P₁，P₂，...，P_r}，(1≤r≤n)；

S330：依次连接集合U中的所有节点，完成对路径的全局优化。

4.如权利要求3所述的基于动态窗口和冗余节点过滤的路径优化方法，其特征在于，所述S400所述的结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全局路径的具体步骤如下：

S410：针对集合U，从起点P₁开始，依次将除开终点P_r之外的所有节点作为局部路径规划的起点，记作{S₁，S₂，...，S_r-1}；同时将集合U中的第二个节点P₂开始，依次将{P₂，P₃，...，P_r}记作局路径规划的终点{D₁，D₂，...，D_r-1}，则可以将全局路径分为S₁D₁，S₂D₂，...，S_r-1D_r-1共r-1段，记S₁在栅格地图中的坐标值为

D₁的坐标值为

以此类推；

S420：初始化移动机器人的状态参数集L(l)，l(x，y，yaw，v，w)记录机器人运动在规划路径中的状态参数，包括位置、航向角、线速度、角速度，设定机器人的初始线速度为v(m/s)，初始角速度为w(rad/s)和初始导航角yaw(rad)；

S430：计算S_iD_i的倾斜角度，计算公式如下：

其中，

分别是D_i的横纵坐标，

分别是S_i的横纵坐标；

将S_iD_i段路径的倾斜角度转换为弧度值作为移动机器人的初始航向角，斜率角度转化为弧度值的公式为：

yaw_i＝α×180°÷π

得到在S_iD_i段路径中机器人的所有状态参数l_i(x_i，y_i，yaw_i，v_i，w_i)，1≤i≤r，其中，x_i，y_i是节点S_i的横纵坐标，v_i是S_iD_i段路径中读取的上一段路径的终点处的线速度，w_i是S_iD_i段路径中读取的上一段路径的终点处的角速度；

S440：按照S430中计算S_i+1D_i+1段路径的倾斜角度α_i+1，同时将机器人上一个状态参数l_i中的yaw_i转换为角度值β，弧度值转化为角度值的公式如下：

β＝yaw_i×π÷180°

比较α_i+1和β，如果|α_i+1-β|＜60°，则机器人保持最新的状态l_i+1(x_i+1，y_i+1，yaw_i+1，v_i+1，w_i+1)，如果|α_i+1-β|≥60°，为避免移动机器人因为绕路转弯造成了冗余路段和耗费不必要运动时间，则令机器人导航角

S450：重复步骤S440，直至移动机器人到达第r-1路段的终点D_r-1，得到记录移动机器人所有的状态参数集L(l₁，l₂，...，l_i，l_i+1，...，l_r)，完成局部路径的优化。

Images