本文主要记录在 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 -->
  <node pkg="tf" type="static_transform_publisher" name="tf_baseLink_cameraPose"
        args="0 0 0 0 0 0 base_link camera_pose_frame 1000"/>

  <node pkg="tf" type="static_transform_publisher" name="tf_odom_cameraOdom"
        args="0 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 原始里程计的数据变化规律：

• 机头往前推 $\to$ Y 轴增加
• 机头往右推 $\to$ X 轴增加

结论：可以显著看出，当前安装姿态下 T265 的坐标系与 PX4 期望的 ROS 官方坐标系相差了 90 度（即绕 Z 轴旋转了 90 度）。

2.3 修改 TF 变换参数

基于上述结论，我们需要将 Launch 文件中的两个 TF 参数改为如下配置：
(参数顺序为：x y z yaw pitch roll)

  <!-- 1. 修正物理安装的平移与旋转 -->
  <node pkg="tf" type="static_transform_publisher" name="tf_baseLink_cameraPose"
        args="0 0 -0.1 -1.5708 0 0 base_link camera_pose_frame 1000"/>

  <!-- 2. 修正全局里程计坐标系映射 -->
  <node pkg="tf" type="static_transform_publisher" name="tf_odom_cameraOdom"
        args="0 0 0 -1.5708 0 0 odom camera_odom_frame 1000"/>

深入原理解析：

1. 为什么改第一个 TF (tf_baseLink_cameraPose)？
   • 平移补偿：因为我的 T265 相对于飞控（重心）往下移了 10cm，所以 z 设置为 -0.1。
   • 旋转补偿：由于 T265 的水平坐标系与飞控差了 90 度，所以 yaw 设置为 -1.5708（即 $-\frac{\pi }{2}$）。这表示 camera_pose_frame 相对于 base_link 绕 Z 轴顺时针旋转了 90°。
2. 为什么还要改第二个 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，强烈建议：

1. 将算法输出的里程计数据发送给 /mavros/vision_pose/pose 话题。
2. 在你自己的控制节点中，不要直接使用 SLAM 的输出，而是订阅飞控返回的 /mavros/local_position/odom 进行闭环控制。
3. 原因：PX4 内部的 EKF2 极其强大，经过 MAVROS 融合后的位置和速度数据，其平滑度和低噪声特性显著优于单纯的 VINS 或 Lidar Odometry 输出。(同样，QGC 参数中无需勾选 3D Velocity)。

💡 建议 2：突破 MAVROS Odom 的频率限制

如果你嫌弃 MAVROS 默认返回的里程计频率太低（通常为 30Hz），阻碍了高频控制，可以通过发送 MAVLink 指令强行提频至约 100Hz。
只需在终端运行以下命令即可：

rosrun mavros mavcmd long 511 32 5000 0 0 0 0 0

(注：指令中的 511 代表 MAV_CMD_SET_MESSAGE_INTERVAL，32 代表消息 ID，5000 代表间隔时间 5000us，即 200Hz 目标速率，实际受限于总线带宽通常稳定在 100Hz 左右)。

---

5. 最终融合效果展示

最后，贴上一张通过上述配置后抓取的 EKF 融合对比图。

可以看出，观测和输出两条曲线严丝合缝，几乎没有出现反向跌落和剧烈超调，飞机的定点悬停效果稳如磐石。

---

作者后记： 多传感器融合往往是“失之毫厘，谬以千里”，坐标系与协方差的任何一点差错都会在空中被无限放大。希望这份记录能帮到正在啃 PX4 视觉定位的同学们少走弯路！

---