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

本发明专利技术公开了一种基于动态窗口和冗余节点过滤的路径优化方法，其包括利用已有的栅格地图数据集，确定运动的起始位置和目标位置的坐标信息，在栅格地图中标注目标节点和障碍物节点；利用A

【技术实现步骤摘要】
一种基于动态窗口和冗余节点过滤的路径优化方法


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

技术介绍

[0002]机器人技术作为一个综合了多个学科的技术，是多年研究的热门。其中，路径规划作为移动机器人技术研发的关键技术，其目的在于参照某一个参数的指标(如代价最低、选择路径最短、运算时间最短等)，在任务区域选择出一条可以从起点运动到终点的最优或次优的避障路径。路径规划按照机器人对工作区域信息的理解层次，可以分为给予完整区域信息理解的全局路径规划和基于部分区域信息理解的局部路径规划，两者各有优缺。现在实际工作中大多情况下都需要融合兼用全局和局部路径规划，前者旨在寻找全局优化路径，后者旨在实时避障。
[0003]A
*
算法作为最流行的全局路径查找选择方式之一，具有高度灵活性，且可以广泛应用于不同的环境之中。A
*
算法的成功秘诀在于它结合了Dijkstra算法使用的信息(对接近起点的位置有利)和贪婪最佳优先搜索使用的信息(对接近目的地的位置有利)。其通过计算路由节点成本，选择成本最低的节点，通过依次扩展具有最小启发函数的节点来寻找最优路径。虽然传统的A*算法能得到一条长度相对比较优秀的路径，但是该路径存在的问题也很明显：有冗余节点、转折点较多，这些对机器人实际的运动都是不利的存在。
[0004]动态窗口法因为其符合移动机器人的运动特征，具有灵活性强等优点，成为了动态环境下局部避障的主要算法。动态窗口法的基本思想是结合移动机器人的运动特性，在路径规划过程中，实时预测时间t内移动机器人的速度空间(v
t
，w
t
)和状态空间(x
t
，y
t
，yaw
t
，v
t
，w
t
)，模拟移动机器人在预测时间内的运动轨迹，再根据评价函数确定最优的运动轨迹，从而到达目标位置完成路径规划。该方法属于一个局部的路径规划，机器人如果只在这个算法下运动，虽然可以保证一个良好的运动姿态，但是要达到目标点需要遍历太多节点，效率很低。

技术实现思路

[0005]针对现有技术存在的上述问题，本专利技术的要解决的技术问题是：如何尽可能的优化路线，使机器人的路线更平滑和可靠。
[0006]为解决上述第一个技术问题，本专利技术采用如下技术方案：一种基于动态窗口和冗余节点过滤的路径优化方法，包括以下步骤：
[0007]S100：利用已有的栅格地图数据集，确定运动的起始位置和目标位置的坐标信息，在栅格地图中标注目标节点和障碍物节点；
[0008]S200：利用A
*
算法规划出一条全局路径；
[0009]S300：利用A
*
算法规划处的全局路径进行全局优化，过滤掉冗余节点；
[0010]S400：结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的
全局路径。
[0011]作为改进，所述S200中利用A
*
算法规划出一条全局路径，其包括以下步骤：
[0012]S210：构建并且初始化两个空表OpenList和CloseList，将起始位置设置为当前节点并存入OpenList列表；如果当前节点不是目标点，那么调用扩充节点函数选出与当前节点相邻且可扩展的所有节点，并且将扩展的所有节点信息都存入OpenList列表；
[0013]S220：调用插入节点函数，遍历可以扩展的所有节点，计算出这些节点对应的代价函数F，代价函数表示为：
[0014]F(n)＝G(n)+H(n)
[0015]其中，n表示当前节点；F(n)是当前节点n的代价函数；G(n)是移动机器人从初始节点到节点n的实际代价值；H(n)是从当前节点n到达目标点的代价值，选取这两点之间的欧式距离代表H(n)，H(n)函数表示为：
[0016][0017]其中，(x
g
,y
g
)代表目标节点在栅格地图中的坐标，(x
n
，y
n
)代表当前节点在栅格地图中的坐标；
[0018]S230：选出代价函数F(n)最小值所对应的节点，将其从OpenList列表中删除，存入CloseList列表之中，同时将该节点设为当前节点连接至上一节点，重复S220，直至当前节点为目标点，导出该全局路径。
[0019]作为改进，所述S300利用A
*
算法规划处的全局路径进行全局优化，过滤掉冗余节点，包括以下步骤：
[0020]S310：将S200得到的CloseList列表中的路径节点集记为P{P
i
,1≤j≤n}，其中P1为路径的起点，P
n
为路径的终点，创建初始值只有P1和P
n
的关键点集合U{P1,P
n
}用于存放路径优化之后的关键节点；
[0021]S320：对于节点集P{P
i
,1≤j≤n}，令m＝2,3,4
…
n，由P1开始依次连接P2,P3,
…
,P
m
，判断直线P1P
m
之间是否存在阻碍物节点，如果存在，则节点P
m
‑1为路径的必经节点；如果直线P1P
m
没有穿过障碍物节点，则判定P
m
‑1为冗余节点；
[0022]将所有必经节点都添加至集合U中，关键点选取完成之后，集合U{P1，P
m
‑1，...，P
m+k
，P
n
}(2≤m≤n，1≤k≤n
‑
m)包含了所有关键节点，假设U中的节点数目为r，即U{P1，P2，...，P
r
}，(1≤r≤n)；
[0023]S330：依次连接集合U中的所有节点，完成对路径的全局优化。
[0024]作为改进，所述S400所述的结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全局路径的具体步骤如下：
[0025]S410：针对集合U，从起点P1开始，依次将除开终点P
r
之外的所有节点作为局部路径规划的起点，记作{S1，S2，...，S
r
‑1}；同时将集合U中的第二个节点P2开始，依次将{P2，P3，...，P
r
}记作局路径规划的终点{D1，D2，...，D
r
‑1}，则可以将全局路径分为S1D1，S2D2，...，S
r
‑1D
r
‑1共r
‑
1段，记S1在栅格地图中的坐标值为D1的坐标值为以此类推；
[0026]S420：初始化移动机器人的状态参数集L(l)，l(x，y,yaw,v,w)记录机器人运动在规划路径中的状态参数本文档来自技高网...

【技术保护点】

【技术特征摘要】
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}，其中P1为路径的起点，P
n
为路径的终点，创建初始值只有P1和P
n
的关键点集合U{P1，P
n
}用于存放路径优化之后的关键节点；S320：对于节点集P{P
i
，1≤j≤n}，令m＝2，3，4
…
n，由P1开始依次连接P2，P3，
…
，P
m
，判断直线P1P
m
之间是否存在阻碍物节点，如果存在，则节点P
m
‑1为路径的必经节点；如果直线P1P
m
没有穿过障碍物节点，则判定P
m
‑1为冗余节点；将所有必经节点都添加至集合U中，关键点选取完成之后，集合U{P1，P
m
‑1，...，P
m+k
，P
n
}(2≤m≤n，1≤k≤n
‑
m)包含了所有关键节点，假设U中的节点数目为r，即U{P1，P2，...，P
r
}，(1≤r≤n)；S330：依次连接集合U中的所有节点，完成对路径的全局优化。4.如权利要求3所述的基于动态窗口和冗余节点过滤的路径优化方法，其特征在于，所述S400所述的结合动态窗口法对S300优化后的全局路径进行分段局部优化，得到最终的全
局路径的具体步骤如下：S410：针对集合U，从起点P1开始，依次将除开终点P
...

【专利技术属性】
技术研发人员：宋永端，张聪毅，谭力珲，赖俊峰，王赛宇，张艳锴，
申请(专利权)人：星际重庆智能装备技术研究院有限公司迪比重庆智能科技研究院有限公司，
类型：发明
国别省市：