使用APF进行局部路径规划

使用APF进行局部路径规划

局部路径规划

局部路径规划（局部避障）常见的算法有人工势场、直方图法和动态窗口法。

人工势场法APF是局部路径规划的一种比较常用的方法。这种方法假设机器人在一种虚拟力场下运动。人工势场包括引力场合斥力场，其中目标点对物体产生引力，引导物体朝向其运动。障碍物对物体产生斥力，避免物体与之发生碰撞。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。这里的关键是如何构建引力场和斥力场。

APF存在的问题

• 当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物
• 当目标点附近有障碍物时，斥力将非常大，引力相对较小，物体很难到达目标点
• 在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡

可以通过对引力场和斥力函数，以及添加随机扰动等方式缓解上述问题。

APF参考阅读: http://kovan.ceng.metu.edu.tr/~kadir/academia/courses/grad/cs548/hmws/hw2/report/apf.pdf

APF算法程序介绍

与算法相关的程序有

• 主节点：planner_node.cpp
• 局部规划算法管理函数：local_planning.cpp
• 人工势场算法：apf.cpp
• 具体请参见代码注释！

Gazebo仿真环境运行

使用激光雷达作为传感器

• 运行launch文件（请查看launch文件参数说明，并进行调整）

   roslaunch prometheus_gazebo sitl_apf_3Dlidar.launch 
  

• 在打开的rviz窗口中勾选局部规划及Ground_Truth显示(绿色为真值,红色为激光雷达局部点云)

• 输入1选择APF算法，无人机将自动起飞

• 在rviz中通过3D Nav Goal按钮指定目标点，点选该按钮后，同时按住鼠标左右键在rviz窗口中选择一点向上拉
  

• 也可以通过终端发布目标点

   rostopic pub /prometheus/planning/goal ...
  

运行截图

使用RGBD相机作为传感器

• 运行launch文件（请查看launch文件参数说明，并进行调整）

   roslaunch prometheus_gazebo sitl_apf_rgbd.launch 
  

• 其他同上

说明: 并不推荐此方案,因为RGBD相机视场较差,避障效果较差,如需使用此方案,需对参数进行调试