PX4 + T265 视觉定位全方位调试记录与 TF 坐标系避坑指南

本文主要记录在 PX4 飞控体系下使用 Intel RealSense T265 进行视觉定位VIO的调试过程。本项目采用 PX4 官方团队Auterion 公司当年专门为 T265 编写的桥接包px4_realsense_bridge。虽然官方已经删库但有幸保留了当年的官方例程在此感谢WaterQun开源地址如下GitHub 仓库https://github.com/WaterQun/VIO详细的编译与安装方法已在仓库的README中阐述本文将重点讲解T265 的物理摆放与 TF 坐标系变换以及QGC 的核心参数配置。1. 启动节点与初始状态当你完成环境配置后只需要执行以下命令roslaunch px4_realsense_bridge bridge_mavros.launch该 Launch 文件会同时启动mavros、T265 驱动节点以及桥接包因此无需再额外开终端启动其他节点非常便捷。在默认的bridge.launch文件中关于 TF 静态坐标变换的配置初始如下全为 0!-- Launch static transform publishers --nodepkgtftypestatic_transform_publishernametf_baseLink_cameraPoseargs0 0 0 0 0 0 base_link camera_pose_frame 1000/nodepkgtftypestatic_transform_publishernametf_odom_cameraOdomargs0 0 0 0 0 0 odom camera_odom_frame 1000/这里包含两个 TF 的发布我们需要根据 T265 实际的物理安装方式来对它们进行修改。2. 核心痛点坐标系对齐与 TF 参数推导2.1 搞懂 PX4 与 T265 的底层坐标系差异PX4 坐标系经过测试PX4 在没有任何外部观测的情况下发布的里程计话题/mavros/local_position/odom严格遵循 ROS 官方的ENU右手坐标系东北天。即机头往前 X 增大往左 Y 增大往上 Z 增大。T265 坐标系无论你怎么摆放 T265由于其内部有 IMU 重力对齐机制只要往上抬Z 轴数值必然增加。2.2 实际安装测试在我的实际无人机机架上T265 的放置模式为相机镜头朝下看USB 接口与机尾同向。在此物理放置模式下推动飞机进行测试得出 T265 原始里程计的数据变化规律机头往前推→ \rightarrow→Y 轴增加机头往右推→ \rightarrow→X 轴增加结论可以显著看出当前安装姿态下 T265 的坐标系与 PX4 期望的 ROS 官方坐标系相差了 90 度即绕 Z 轴旋转了 90 度。2.3 修改 TF 变换参数基于上述结论我们需要将 Launch 文件中的两个 TF 参数改为如下配置(参数顺序为x y z yaw pitch roll)!-- 1. 修正物理安装的平移与旋转 --nodepkgtftypestatic_transform_publishernametf_baseLink_cameraPoseargs0 0 -0.1 -1.5708 0 0 base_link camera_pose_frame 1000/!-- 2. 修正全局里程计坐标系映射 --nodepkgtftypestatic_transform_publishernametf_odom_cameraOdomargs0 0 0 -1.5708 0 0 odom camera_odom_frame 1000/深入原理解析为什么改第一个 TF (tf_baseLink_cameraPose)平移补偿因为我的 T265 相对于飞控重心往下移了 10cm所以z设置为-0.1。旋转补偿由于 T265 的水平坐标系与飞控差了 90 度所以yaw设置为-1.5708即− π 2 -\frac{\pi}{2}−2π​。这表示camera_pose_frame相对于base_link绕 Z 轴顺时针旋转了 90°。为什么还要改第二个 TF (tf_odom_cameraOdom)当我们改完第一个 TF 后PX4 返回的里程计/mavros/local_position/odom的坐标系会被带偏变成跟 T265 一样往前 Y 增加往右 X 增加。为了让我们在做上层控制时依然能使用标准的 ROS FLU 坐标系机头往前 X 增大往左 Y 增大我们需要用第二个 TF 把它“拧”回来。设置yaw为-1.5708表示camera_odom_frame相对于全局odom原点绕 Z 轴顺时针旋转 90°。注MAVROS 发出的 odom 话题中Twist速度默认是在机体坐标系Body Frame下的。3. QGC 地面站核心参数设置机载电脑端的参数配置完成后必须在 QGC 地面站中修改 PX4 的 EKF2 参数否则飞控无法正确吸收视觉数据。✅EKF2_EV_CTRL(外部视觉融合开关)必须勾选水平位置 (Horizontal position)、垂直位置 (Vertical position)、偏航角 (Yaw)。 避坑警告切记不要勾选 3D Velocity由于 T265 在当前设定下的速度估计有问题勾选速度融合极易导致报错甚至会导致 PX4 彻底拒绝接收 Odom 数据。✅EKF2_HGT_REF(高度参考源)室内比赛/纯视觉飞行务必改为Vision以 T265 作为唯一高度基准。室外或室内外无缝切换建议酌情改为Barometric气压计以保证切换瞬间高度的绝对连续性具体效果需结合实际场景测试。✅EKF2_EV_DELAY(视觉延迟补偿)一般保持默认即可稳定飞行。如果追求极致的抑制超调效果精益求精可以尝试改为0.01或0.02即补偿 10ms ~ 20ms 的通信与计算延迟但务必经过大量实际推拉测试否则不建议盲目修改。4. 进阶建议与技巧分享 建议 1其他 SLAM 算法的完美融合方案如果你在跑VINS-Fusion或者FAST-LIO2强烈建议将算法输出的里程计数据发送给/mavros/vision_pose/pose话题。在你自己的控制节点中不要直接使用 SLAM 的输出而是订阅飞控返回的/mavros/local_position/odom进行闭环控制。原因PX4 内部的 EKF2 极其强大经过 MAVROS 融合后的位置和速度数据其平滑度和低噪声特性显著优于单纯的 VINS 或 Lidar Odometry 输出。(同样QGC 参数中无需勾选 3D Velocity)。 建议 2突破 MAVROS Odom 的频率限制如果你嫌弃 MAVROS 默认返回的里程计频率太低通常为 30Hz阻碍了高频控制可以通过发送 MAVLink 指令强行提频至约100Hz。只需在终端运行以下命令即可rosrun mavros mavcmd long51132500000000(注指令中的 511 代表MAV_CMD_SET_MESSAGE_INTERVAL32 代表消息 ID5000 代表间隔时间 5000us即 200Hz 目标速率实际受限于总线带宽通常稳定在 100Hz 左右)。5. 最终融合效果展示最后贴上一张通过上述配置后抓取的 EKF 融合对比图。可以看出观测和输出两条曲线严丝合缝几乎没有出现反向跌落和剧烈超调飞机的定点悬停效果稳如磐石。作者后记多传感器融合往往是“失之毫厘谬以千里”坐标系与协方差的任何一点差错都会在空中被无限放大。希望这份记录能帮到正在啃 PX4 视觉定位的同学们少走弯路