[发明专利]一种基于动态窗口和冗余节点过滤的路径优化方法

权利要求书

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

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中的所有节点，完成对路径的全局优化；

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)，完成局部路径的优化。