PX4 与 Fast-Planner 实机自主避障联调：从 VIO 接入到航线重规划踩坑记录

很多模友在 Gazebo 仿真里跑 Fast-Planner 感觉顺风顺水，B-spline 轨迹丝滑得不行，但一搬到实机挂上 PX4，切 Offboard 的瞬间要么原地抽搐，要么直接往树上怼。

这篇记录整理一下 Fast-Planner（主要指经典的 kinodynamic 方案及衍生版本）与 PX4 实机联调的踩坑路径。重点放在离线数据校验、坐标系对齐、MAVROS 通信控制闭环，以及怎么在不摔桨的前提下完成从离线到实飞的过渡。

1. 硬件架构与通信拓扑

实飞测试平台是一台 380 轴距的四旋翼，配置如下：

• 飞控：Holybro Pixhawk 6C（固件 PX4 v1.14.0）
• 机载电脑：Jetson Orin Nano（8GB），跑 Ubuntu 20.04 + ROS Noetic
• 里程计来源：前端 D435i + T265（或者单目跑 VINS-Fusion），通过 MAVROS 注入飞控作为外部视觉里程计（EV）
• 深度传感器：RealSense D435i（下倾 10 度安装，消除前下方盲区）
• 飞控与机载通信：串口 /dev/ttyTHS1 跑 TELEM2，波特率 921600，MAVLink 消息流速率锁死在 50 Hz 以上

整个闭环流程是： D435i 点云 + VIO 里程计 $\to$ Fast-Planner $\to$ 轨迹生成与优化 $\to$ 轨迹跟踪控制器 $\to$ MAVROS (/mavros/setpoint_raw/local) $\to$ PX4 位置/速度环。

2. 坐标系对齐：炸机高发区

实机部署里有 80% 的侧翻和撞击是因为坐标系弄混。

ROS 体系里惯用 ENU（东北天），PX4 底层控制与 MAVLink 则是 NED（北东地），而相机的光学坐标系是前右下或者右下前。

2.1 MAVROS 的自动转换

MAVROS 会在内部处理 ENU 与 NED 的转换，但前提是你发给 MAVROS 的 topic 必须严格遵守 ROS REP-103 标准（X 向前，Y 向左，Z 向上）。

在 Fast-Planner 的配置文件里，必须确认全局坐标系（通常叫 world 或 map）与局部里程计（odom）的对齐关系。如果使用外部定位（比如 VINS），发布给 Fast-Planner 的里程计坐标系务必保证：

• 航向角 0 度对应 X 轴正方向。
• 逆时针旋转偏航角 Yaw 为正。

2.2 相机外参（Extrinsic）

检查 camera_link 到 base_link 的静态 TF 变换。很多新手直接用 Fast-Planner 默认的 launch 文件，默认相机是水平安装的。如果你像我一样把 RealSense 下倾了 10 度（约 0.174 rad），必须在 TF tree 里把这个俯仰角（Pitch）写进去，否则建出来的栅格地图全部是斜的，无人机平飞时会把前方的地面误判为障碍物，导致航向直接抬高飘天。

<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_camera"
      args="0.12 0.0 -0.05 0.0 0.174 0.0 base_link camera_link" />

3. 控制接口与 PX4 参数匹配

Fast-Planner 优化出来的是高阶连续曲线（包含位置、速度、加速度、加加速度 Jerk）。不要直接把位置点一股脑发给 /mavros/setpoint_position/local，这种单纯的位置阶跃控制会让 PX4 的 PID 超调严重，机身剧烈抖动。

3.1 下发三阶前馈控制指令

推荐使用带有加速度前馈的控制节点，通过 mavros_msgs/PositionTarget 下发给 /mavros/setpoint_raw/local。

#include <mavros_msgs/PositionTarget.h>

mavros_msgs::PositionTarget pos_cmd;
pos_cmd.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
// 屏蔽 Yaw_rate，保留位置、速度、加速度前馈及 Yaw 角度
pos_cmd.type_mask = mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

// Fast-Planner 输出的期望位置 (ENU)
pos_cmd.position.x = cmd_p.x();
pos_cmd.position.y = cmd_p.y();
pos_cmd.position.z = cmd_p.z();

// 速度前馈
pos_cmd.velocity.x = cmd_v.x();
pos_cmd.velocity.y = cmd_v.y();
pos_cmd.velocity.z = cmd_v.z();

// 加速度前馈（极其关键，大幅减小高速机动时的动态滞后）
pos_cmd.acceleration_or_force.x = cmd_a.x();
pos_cmd.acceleration_or_force.y = cmd_a.y();
pos_cmd.acceleration_or_force.z = cmd_a.z();

pos_cmd.yaw = cmd_yaw;

cmd_pub.publish(pos_cmd);

3.2 PX4 端动力学限幅协同

Fast-Planner 内部有自己的动力学边界约束：

• max_vel: 比如设为 2.0 m/s
• max_acc: 比如设为 2.5 m/s²

在 PX4 参数中，必须保证飞控的自身限制略大于规划器的上限，否则飞控自身限幅会破坏规划出的轨迹形态，导致轨迹解算与飞控执行对抗：

• MPC_XY_VEL_MAX $\ge 2.5$ m/s（略大于 Planner 的 2.0）
• MPC_ACC_HOR_MAX $\ge 3.0$ m/s²
• MPC_JERK_MAX 适当调大，避免加速度响应延迟

4. 离线验证与地面联调流程

在挂桨起飞前，按以下三步做离线验证，能省下 90% 的碳板和射桨成本。

步骤一：录包回放与静态栅格检验

1. 手持无人机在室内环境走一圈，录制包含 RealSense 深度图、点云以及 VIO 里程计的 rosbag。
2. 离线回放 rosbag，启动 Fast-Planner 的建图与规划节点。
3. 打开 RViz，在地图上随机点 3D Nav Goal，观察规划出的 B-spline 曲线是否在障碍物边缘留出足够的膨胀半径（Safe Distance 建议实机至少设为 0.35 m）。
4. 观察规划耗时：用 rostopic hz 检查 /planning/bspline 的耗时，确保在 Jetson 上重规划耗时稳定在 15 ms 以内。

步骤二：卸桨台架联调（台架或手持）

1. 拆掉所有桨叶。
2. 机载电脑启动所有节点，遥控器切入 Offboard 模式，解锁。
3. 此时电机怠速旋转。在 RViz 里给一个目标点，手持飞机沿着规划出的轨迹路线行走。
4. 监听 /mavros/local_position/pose 和 planner 发布的期望轨迹，检查误差曲线。如果手持飞机跟不上规划点，观察电机会不会突然暴走；如果电机转速异常拉满，检查加速度前馈正负号是否写反。

步骤三：实机原地起飞切 Offboard

1. 先在 Position 模式下定点悬停。
2. 启动规划节点，使其首个目标点就是当前悬停点，确保切入瞬间轨迹期望速度为 0。
3. 切入 Offboard。此时飞机应稳稳保持在原地。
4. 遥控器设置好 Kill Switch（急停开关）和返回 Position 模式的切换开关，手始终放在切模式拨杆上。

5. 炸机复盘与坑点记录

案例 1：透光玻璃与黑色吸光材质导致的“看不见”

• 现象：在园区测试时，飞机直线冲向深色金属栅栏，规划器毫无重规划动作，直接碰撞。
• 原因分析：D435i 是双目结构光，针对深黑色吸光物体以及高反光表面，深度图会出现大面积空洞（NaN 或零值）。Fast-Planner 默认如果接收不到点云，会在局部地图中将其视为空闲（Free）空间。
• 对策：在滤波预处理节点中，对深度图的无效点做就近填补，或者把点云投影到 ESDF 地图的膨胀半径增大至 0.4 m；同时加装单线激光雷达辅助融合避障。

案例 2：MAVROS 心跳掉帧触发 Failsafe 悬停

• 现象：飞机飞着飞着突然刹车悬停，切回 Position 模式再切 Offboard 又能动。
• 原因分析：飞控参数 COM_OF_LOSS_T 设成了 0.5 s。当 Fast-Planner 在复杂环境中由于障碍物突发引发高负荷计算，机载 CPU 短暂卡顿，发向 /mavros/setpoint_raw/local 的频率从 50 Hz 跌到了 1 Hz 以下，超过 500 ms 飞控自动退出 Offboard。
• 对策：单独起一个控制线程（100 Hz），无论 Planner 是否在重规划，该线程均基于当前最新的 B-spline 样条曲线做插值下发，将规划计算与控制下发解耦。

案例 3：电机饱和导致掉高与姿态失控

• 现象：进行连续直角弯重规划避障时，无人机出现严重掉高，机身震颤后接地。
• 原因分析：Fast-Planner 给出了极高的偏航角加速度和垂直速度变化，电调响应达到极限，分配给升力的油门被姿态环挤占，造成推力不足。
• 对策：在 Fast-Planner 中严格限制垂直方向的动力学边界：将 max_vel_z 压到 1.0 m/s，max_acc_z 压到 1.5 m/s²，实飞以稳定安全为先，切忌拿仿真的极限速度直接在实机上跑。

自主避障的实机部署是算法工程和飞控硬件底层的结合，先把坐标系转换、时间戳同步、丢包兜底和动力学限幅做扎实，把大部分逻辑问题在地面不装桨阶段消化掉，到了外场才能少修机臂。