ros机器人路径远离障碍物的方法
A星或dijkstra规划的路径会贴着障碍物,如果膨胀半径设置过小机器人在跟踪路径运动时会碰到障碍物,特别是在转弯的时候。
这里提供一种路径优化的方法让路径与膨胀层保持一定距离。
步骤:
1、遍历所有的路径点,记录下路径点周围一定范围length(可设置)的障碍物。
2、若所有的障碍物都在路径的同一测则找到距离该路径点A最近的障碍物点B,设最近的距离为d。
3、将路径点A平移,平移的方向为A指向B的方向,平移的距离为length – d,即让A与B的保持距离为length。
效果图如下:
红色的路径为未优化的全局路径,绿色的为优化后的局部路径
代码如下:
v
1