一、 “diff_planner”算法的核心流程这个项目先通过grid_map和raycast把传感器输入转成可查询的局部地图再由DynAStar在地图上搜索出一条粗路径随后交给poly_traj_optimizer生成满足动态约束的连续轨迹最后由traj_server把轨迹转换成无人机实际可执行的控制指令并通过diff_replan_fsm持续管理规划状态和重规划切换。1、这个项目到底是怎么启动起来的如果想理解整个系统第一步不是去看某个公式而是先看 ROS 节点是怎么组织的。这个项目里最值得先看的是plan_manage这个包重点文件有plan_manage/src/diff_planner_node.cpp --- 算法入口节点plan_manage/src/diff_replan_fsm.cpp --- 状态机负责规划重规划plan_manage/src/planner_manager.cpp --- 总控代码串连地图生成、路径搜索与轨迹优化2、地图是怎么来的先把环境变成规划器能看懂的形式这一部分主要在plan_env里重点文件是plan_env/include/plan_env/grid_map.hplan_env/src/grid_map.cppplan_env/src/raycast.cpp这部分的作用是把传感器输入变成栅格地图让后面的搜索器和优化器知道哪里能飞、哪里不能飞。首先最值得关注的是raycast这条线。它不是简单地把点云里的点映射到地图上而是沿着激光束从传感器位置一路遍历到障碍点把中间经过的格子标成自由空间最后把终点标成占用。这样做的好处是地图更新会更符合真实传感器的观测逻辑不会只留下一个个零散的障碍点。而且后续的规划器不是直接看点云而是看经过射线更新后的体素地图。另外grid_map.h里那些坐标转换和栅格索引相关函数也很关键因为后面所有碰撞判断最后都要回到“这个世界坐标点到底落在哪个体素格子里”这个问题上。3、A* 是怎么工作的先找一条能走的粗路径地图有了之后下一步就是路径搜索。这个项目里对应的是path_searching/include/path_searching/dyn_a_star.hpath_searching/src/dyn_a_star.cpp在DynAStar这一部分主要做的是搜索出一条粗路径再把路径送到轨迹优化器里做连续化处理。A* 只负责保证可达性优化器负责保证平滑性和可执行性。所以这一部分的核心目标不是“最优”而是“先找到一条能过去的路”。它的大致流程一般是输入起点和终点在栅格地图里扩展邻居用启发式函数估计到目标的代价找出一条可行路径把这条路径交给后面的优化模块。4、最核心的一步轨迹是怎么从“粗路径”变成“可执行轨迹”的这个项目最核心的部分其实就在轨迹优化模块traj_opt/include/optimizer/poly_traj_optimizer.htraj_opt/src/poly_traj_optimizer.cpp这个过程大体可以理解成1先把离散路径变成可优化对象A* 给出来的是一串点但无人机不能直接“跳点飞”。所以优化器会把这些点转成多项式轨迹的控制点或者断点后面再做优化。2再建立目标函数优化器不是随便调一条线而是会同时考虑几个目标轨迹要平滑轨迹不能撞障碍轨迹要满足速度、加速度等约束有时还要尽量贴近搜索出来的引导路径。这也是为什么poly_traj_optimizer.cpp里碰撞检查、可行性判断、重启策略这些内容都很重要。它不是优化一次就完而是会在优化过程中不断检查当前轨迹是不是还能继续用。3再做数值优化使用 L-BFGS 优化器通过迭代求解把轨迹慢慢推到一个更合理的位置上。5、这个项目为什么不是“算出一条轨迹就结束了”这一部分的重点文件还是这两个plan_manage/src/diff_replan_fsm.cppplan_manage/src/planner_manager.cppplanner_manager.cpp负责把前面的几个模块串起来从当前位姿和目标点开始调用路径搜索模块把搜索结果送去轨迹优化器再把结果整理成可发布的轨迹。diff_replan_fsm.cpp状态机决定系统当前处于什么阶段是在规划还是在执行还是需要重规划如果规划失败了要不要继续尝试6、轨迹最后是怎么发给无人机的将轨迹变成无人机能执行的控制指令是在plan_manage/src/traj_server.cpp它做的事情是订阅规划好的轨迹按当前时间从轨迹中取样生成位置、速度、加速度、yaw 等控制量再发布成控制器能接收的PositionCommand。所以整个链路就变成地图更新 → A* 搜索 → 多项式轨迹优化 → traj_server 采样并下发 → 无人机执行这条链路串起来之后你会发现这个项目不是停留在“算法演示”层面而是真的把规划结果接到了飞行控制流程里。二、Diff-Planner算法相比于Ego-Planner算法的提升点解析1、修复局部规划时 A* 终点在障碍物里尝试沿 A* 起始方向推出障碍物时的bug在traj_opt/src/poly_traj_optimizer.cpp的roughlyCheckConstraintPoints()和distinctiveTrajs()更新后算法会先找先找碰撞段两端的最近自由点然后再把端点“往外推”所以不是一次性改掉整个轨迹而是把障碍段两侧的约束点往自由空间里重新安放。2、修复优化过程中频繁打印局部目标点在障碍物里规划器卡死的bug这条 bug 的修复核心不是“让碰撞目标继续硬算”而是与第一条修复串起来先尽量把局部目标改到自由空间如果行不通就换初始化方式随着次数增加随机扰动幅度会表达避免每次陷入一个局部最优最后再不行就别死循环直接进入安全停机。3、修复在状态机中使用planNextWaypoint()导致状态机卡死的bug在plan_manage/src/diff_replan_fsm.cpp中先尝试调用planner_manager_-planGlobalTrajWaypoints(...)如果不是第一次进行规划、也没有触发紧急停止就进入一个等待循环等到状态机真的切到了EXEC_TRAJ再把状态切到REPLAN_TRAJ。本质上就是把“同步等待状态变更”改成了“带超时的协作式等待”。4、修复遇到大障碍物后无人机在大障碍物面前反复徘徊卡死的bug这一部分作者没有做一个复杂的“大障碍物分类器”而是通过运动行为判断分为两种主要情况情况 A接近最终目标时记录一条从起点到最终目标的基准线看当前位姿在这条线上的投影前进了多少如果一段时间内投影长度几乎不变就认为在原地打转如果卡住太久就停止规划无人机进入悬停情况 B还没到最终目标local_target_pt_有没有长时间不变如果局部目标很久没推进也判定 stuck还有一个额外保护如果飞得离规划方向太远也会触发 emergency stop。它解决的是这种场景前方有个大障碍物轨迹规划一直绕来绕去位置在变但实际上没有朝目标推进。作者通过“进度检测”把它识别成卡死而不是让它一直绕圈。5、新增优化失败次数过多的处理当前的diff-planner在在DiffReplanFSM里会在每一次局部路径规划失败之后进行计数成功之后就会把当前规划的失败计数清空一旦连续规划失败次数超过10就会强制无人机进入悬停状态。关于每次规划失败后的流程与前面两次介绍的修复一致第一次失败调整初始值只是轻微扰动连续失败越多随机初值偏移越来越大。短期来看给优化器更多不同起点减少局部最优卡死长期来看如果真的没办法就别耗着安全退出。6、新增激光雷达建图raycast版本使激光雷达建图更加稳定以前不稳定的原因如果只把点云的“终点”打进栅格图容易有几个问题1、中间的自由空间没被正确清空2、同一帧里大量点可能重复更新3、稀疏点云会让障碍边界抖动raycast 版本现在的操作是对每个点从雷达/相机位置到这个点做一次体素遍历遍历到的中间格子记为 free终点格子记为 occupied最后统一按 log-odds 更新占用概率raycast 版本明确地做了“射线穿越”free 空间和 occupied 空间被分开处理不容易被单个点的噪声带偏地图更新更符合激光雷达/深度图的物理观测过程。总结上述6条修复让轨迹更容易从障碍物附近恢复—— A* 终点外推、局部目标修正、随机初值增强。让状态机不再无限重试——planNextWaypoint()超时、连续失败计数、stuck 检测、emergency stop。让地图更可信—— raycast 体素更新减少点云建图抖动。7、traj_server 节点新增yaw角控制接口用户可根据需要在规划过程中控制无人机yaw角traj_server订阅一个专门的/planning/yaw话题收到外部 yaw 后在 0.5 秒内优先使用该 yaw 作为当前目标角否则默认根据未来轨迹方向自动生成 yaw再通过限速/限加速度做平滑处理最后随 position/velocity/acc/jerk 一起发布到/position_cmd三、在Ubuntu20.04系统上的部署测试1、 XTDrone仿真环境一键化安装1 一键化脚本下载链接 脚本将会一键安装ros noetic、px4 1.13固件、Gazebo 11、以及XTDrone仿真环境。下载链接里面的zip文件 解压到Linux里面推荐放到下载文件夹或者桌面。如果是全新的环境运行setup.sh。./setup.sh如果已经有ros环境可运行setup_without_ros.sh来跳过ros安装。运行之后要输入密码按照提示输入即可。./setup_without_ros.sh2测试XTDrone是否安装成功打开新的终端运行indoor1.launch。如果弹出的gazebo界面有一个房子房子的门附近有一架四旋翼无人机那么恭喜你仿真环境应该是安装成功了cd ~/PX4_Firmware roslaunch px4 indoor1.launch2、下载Diff-Planner源码并构建工作空间git clone https://github.com/DifferentialRobotics/Diff-Planner.git3、下载安装Sophuscd ~/Diff-Planner/src/utils git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout a621ff mkdir build cd build cmake .. make sudo make install4、下载VikitVikit包含摄像机模型、一些我们需要的数学和插值函数。cd ~/Diff-Planner/src/utils git clone https://github.com/xuankuzcr/rpg_vikit.git5、LIvox-SDK2cd ~/Diff-Planner/src/utils git clone https://github.com/Livox-SDK/Livox-SDK2.git cd Livox-SDK2 mkdir build cd build cmake .. make -j sudo make install6、Mid360_imu_simcd ~/Diff-Planner/src/HIGH_FAST_LIO git clone https://github.com/qiurongcan/Mid360_imu_sim.git7、livox_ros_driver2cd ~/Diff-Planner/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd livox_ros_driver2 ./build.sh ROS18、编译Diff-Plannercd ~/Diff-Planner catkin_make四、launch文件调整1、 打开 PX4_Firmware/launch 文件夹找到 single_vehicle.launch文件更改文件中的 .world 文件为你自己的虚拟世界2、更改 HIGH_FAST_LIO/FAST_LIO/config/mid360_sim.yaml文件更改其中的“lid_topic”和imu_topic作为参考我这边更改如下lid_topic: /livox/lidar2 imu_topic: /iris_0/mavros/imu/data3、更改HIGH_FAST_LIO/Mid360_imu_sim/script/pointcloud2livox.py文件中的点云转换话题名称def main(): global pub # 初始化 ROS 节点 rospy.init_node(pre_mmw, anonymousTrue) # 订阅 PointCloud 话题 sub_mmw_cloud rospy.Subscriber(/scan, PointCloud, mmw_handler) # pub_laser_cloud rospy.Publisher(livox/lidar, PointCloud2, queue_size2000) pub rospy.Publisher(/livox/lidar2, CustomMsg, queue_size10) # 保持节点运行 rospy.spin()注上述话题的具体名称都需要根据自己环境中的实际话题名称来做相关的对应可以使用 “ rostopic list ”查看当前所有已经启动的节点话题使用 rqt_graph 可以查看当前项目的ros话题链接情况对于odom 或者最终输出的 “~planning/trajectory” 路径话题不能准确发送到planning/pos_cmd控制话题的情况给定目标点无人机不能正常跟随飞行也需要进行相关的话题修改。五、仿真测试1、打开Gazebo仿真环境新开一个终端roslaunch px4 single_vehicle.launch2、启动Diff-Planner算法在Diff-Planner工作空间所在地新开五个终端窗口并设置环境变量source devel/setup.bash然后在前四个窗口中依次输入下面的命令# bash1 cd Diff-Planner/src/HIGH_FAST_LIO/Mid360_imu_sim/script python3 pointcloud2livox.py # bash2 roslaunch fast_lio mapping_mid360_sim.launch # bash3 roslaunch diff_planner run_sim_single.launch # bash4 roslaunch diff_planner exp_rviz.launch第五个窗口首先启动run_ctrl_gazebo.launch文件起飞节点启动节点后无人机螺旋桨开始转动在无人机螺旋桨转动的时候需要通过QGC手动切换到offboard模式。这时候无人机就能够正常起飞并悬停等待任务点下发。注如果在启动起飞节点文件之后使用QGC无法切换到offboard模式并且在QGC中出现报错信息“Failsafe enabled: No manual control stick input”这时候就需要你打开QGC的设置搜索“COM_RCL_EXCEPT”然后勾上 Offboard。此时再启动起飞节点使用QGC就能够正常切换到offboard模式了。QGC下载地址roslaunch px4ctrl single_ctrl_in_gazebo.launch