ros机器人路径远离障碍物的方法

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