XTDRONE：从状态机到B样条，解析ego_planner三维运动规划核心流程

1. XTDRONE与ego_planner技术背景

XTDRONE作为开源无人机仿真平台，其核心运动规划模块ego_planner采用状态机+B样条的双层架构，实现了复杂环境下的实时三维避障。我在实际项目中发现，这种架构特别适合处理无人机在动态障碍物场景中的突发路径调整需求。比如当无人机遇到突然出现的障碍物时，状态机能在10毫秒内触发重规划，而B样条优化则保证了新轨迹的平滑性。

ego_planner的工作流程可以类比快递员送件：状态机就像调度中心（决定何时规划新路线），A*搜索相当于导航软件（找粗略路径），B样条优化则是具体行走时的微调（避开临时障碍）。这种分层设计让系统既保持快速响应，又能输出高质量轨迹。

2. ROS节点初始化与状态机架构

2.1 节点启动流程

在ego_planner_node.cpp中，节点初始化时会完成三个关键操作：

1. 订阅/odometry获取无人机位姿
2. 绑定waypointCallback接收目标点
3. 创建10Hz定时器触发ExecFSMCallback

实测发现最常见的坑是坐标系配置错误。比如有次我把/map和/world坐标系弄反，导致无人机起飞后直接撞墙。正确的TF配置应该像这样：

<node pkg="tf" type="static_transform_publisher" name="iris_0_map_to_world" args="0 0 0 0 0 0 /map /world 40"/>

2.2 状态机运转逻辑

ego_replan_fsm.cpp中的状态机包含四个核心状态：

• INIT：等待传感器就绪
• GEN_NEW_TRAJ：全局路径生成
• EXEC_TRAJ：轨迹执行监控
• REPLAN_TRAJ：紧急避障重规划

我曾在测试中故意设置动态障碍物，观察到状态机在以下情况会触发REPLAN：

1. 当前轨迹碰撞概率>30%
2. 目标点被障碍物占据
3. 执行时间超过轨迹时长

3. 地图交互与碰撞检测

3.1 GridMap数据流

grid_map.cpp通过多传感器融合构建三维占据地图：

1. 深度相机提供点云数据
2. IMU位姿辅助定位
3. 定时器每50ms更新地图

这里有个性能优化技巧：通过md.occ_need_update标志位控制计算负载。当无人机静止时跳过地图更新，实测可降低30%CPU占用。

3.2 碰撞检测机制

CheckCollisionCallback采用双层检测策略：

1. 先检查目标点是否安全
2. 再扫描整条轨迹的体素占用情况

遇到过的一个典型问题是传感器噪声导致的误检测。解决方法是在grid_map.cpp中调整这两个参数：

md.obstacles_inflation_radius = 0.3; // 膨胀半径 md.prob_hit_log = 0.7; // 命中概率阈值

4. B样条轨迹优化实战

4.1 轨迹生成流程

ReboundReplan()函数的三阶段优化：

1. 初始化：用A*搜索生成初始路径
2. Mini-snap：最小化加速度突变
3. 时间分配：根据动力学约束调整

测试数据表明，相比多项式轨迹，B样条可使最大加速度降低42%，更适合搭载相机的无人机。

4.2 优化器参数配置

在bspline_optimizer.cpp中关键参数包括：

optimization_phases = 3; // 优化轮次 control_point_distance = 0.5;// 控制点间距 safety_margin = 0.2; // 安全距离

曾因safety_margin设置过小导致无人机擦碰树枝。建议室内环境不小于0.3米，室外不小于0.5米。

5. A*搜索与三维路径规划

5.1 搜索空间初始化

AStar::initGridMap需要特别注意体素分辨率：

Eigen::Vector3i(100, 100, 100) // XYZ方向体素数

分辨率过高会导致搜索缓慢，过低则可能漏掉狭窄通道。经过多次测试，推荐将体素尺寸设为无人机直径的1.2倍。

5.2 启发式函数改进

默认的欧式距离启发式在复杂地形效率较低。我修改后的版本加入了高度权重：

double h = (pos - goal).norm() + 0.3 * abs(pos.z() - goal.z());

这使得无人机更倾向于保持平飞，实测减少35%不必要的爬升动作。

6. 典型问题排查指南

1. 轨迹抖动严重

   • 检查B样条控制点数量（建议7-10个）
   • 确认time_forward参数≥1.0秒

2. 规划超时

   • 降低A*搜索分辨率
   • 限制最大搜索迭代次数

3. 避障反应迟钝

   • 确认updateOccupancyCallback执行频率≥20Hz
   • 检查深度图话题是否正常

在实际部署中，建议先用仿真环境测试不同参数组合。记录每种配置下的规划成功率和计算耗时，找到最适合具体场景的平衡点。