从VINS到PX4：视觉惯性里程计与飞控坐标系的融合实践

1. 视觉惯性里程计与飞控的基础概念当你第一次尝试把VINS-Fusion这样的视觉惯性里程计和PX4飞控对接时可能会被各种坐标系搞得晕头转向。我刚开始做这个项目时整整花了三天时间才搞清楚为什么飞机一启动就开始原地打转。其实核心问题就在于视觉里程计和飞控使用的坐标系根本不是一家人。视觉惯性里程计VIO就像无人机的眼睛内耳它通过相机和IMU的数据估算出机体在空间中的位置和姿态。而PX4飞控则是无人机的小脑负责控制机体稳定飞行。两者都需要知道机体当前的状态但如果对方向的理解不一致就会像两个语言不通的人配合跳舞——必然踩脚。VINS-Fusion这类算法通常会建立一个世界坐标系(world frame)这个坐标系的特点是以算法初始化时刻的机体位置为原点Z轴与重力方向对齐指向地心X轴通常与初始化时机体的某个轴向对齐而PX4飞控通过MAVROS通信时默认期望的是ENU东-北-天坐标系X轴指向正东Y轴指向正北Z轴垂直向上2. VINS-Fusion的坐标系详解2.1 世界坐标系的建立过程让我们用实际案例来理解VINS-Fusion的坐标系。假设你使用Realsense D435i相机正常放置在桌面上镜头朝东初始化阶段算法会选取前几帧图像建立初始地图重力对齐将IMU测量的重力方向(实际是支持力)与Z轴正方向对齐轴向确定X轴取初始化时机体X轴在水平面的投影我做过一个实验将相机朝东放置启动VINS后发现它的世界坐标系实际上是X轴指向南红色Y轴指向东绿色Z轴指向上蓝色这被称为SEU南-东-天坐标系与PX4期望的ENU坐标系相差了90度偏航旋转。如果不做转换直接发送数据飞机会认为东方是南方导致控制混乱。2.2 机体坐标系的特殊性不同设备的坐标系定义也各不相同。以常见的传感器为例设备型号X轴方向Y轴方向Z轴方向Realsense D435i右下前Realsense T265右上后Pixhawk飞控前左上这个差异意味着即使用同一个IMU设备不同厂商对前方的定义可能完全不同。我在项目中就遇到过T265和D435i混用时出现的坐标系混乱问题。3. MAVROS与PX4的坐标系关系3.1 PX4飞控的坐标系期望PX4飞控通过MAVROS接收外部定位信息时默认期望的是FLU前-左-上坐标系。但这里有个关键细节这个FLU坐标系是相对于ENU世界坐标系的。实际操作中MAVROS会维护几个重要坐标系map坐标系ENU方向的固定世界坐标系odom坐标系里程计坐标系可能随时间漂移base_link坐标系机体固定坐标系当你通过/mavros/vision_pose/pose发送位姿时MAVROS期望这个位姿是base_link在map坐标系中的表达。但VINS输出的却是body在SEU坐标系中的位姿这就是需要转换的根本原因。3.2 坐标变换的数学实现坐标变换的核心是找到旋转矩阵R和平移向量t使得P_map R * P_world t对于SEU到ENU的转换旋转矩阵可以分解为首先绕Z轴旋转-90度顺时针然后绕X轴旋转180度用ROS的tf2库实现如下#include tf2/LinearMath/Quaternion.h #include tf2_geometry_msgs/tf2_geometry_msgs.h // SEU到ENU的旋转 tf2::Quaternion q; q.setRPY(M_PI, 0, -M_PI/2); // 先绕X转180度再绕Z转-90度 geometry_msgs::PoseStamped enu_pose; tf2::doTransform(vins_pose, enu_pose, transform);我在实际项目中发现单纯旋转还不够还需要考虑传感器安装位置的偏移。比如相机安装在飞机重心上方10cm处就需要额外添加Z轴负方向的偏移。4. 完整实现与验证4.1 系统配置步骤硬件连接确认确保IMU数据来源一致建议全部使用飞控IMU测量相机与飞控的物理安装偏移量PX4参数设置param set EKF2_AID_MASK 24 # 启用视觉定位 param set EKF2_HGT_MODE 3 # 视觉高度作为主高度源坐标变换节点实现 创建一个ROS节点订阅VINS的位姿话题进行坐标转换后发布到/mavros/vision_pose/pose。关键代码段import tf2_ros from geometry_msgs.msg import PoseStamped def vins_callback(msg): # 创建变换 transform geometry_msgs.msg.TransformStamped() transform.transform.rotation.x 0.7071 # 90度绕Z轴 transform.transform.rotation.w 0.7071 # 应用变换 transformed_pose tf2_geometry_msgs.do_transform_pose(msg, transform) vision_pub.publish(transformed_pose)4.2 验证方法我总结了一套有效的验证流程静态验证保持飞机静止检查/mavros/local_position/pose的朝向在QGC中观察飞机模型方向是否与实际一致动态验证缓慢移动飞机观察rviz中的轨迹特别注意偏航角是否正确响应飞行测试先进行系留飞行测试检查位置保持模式下是否出现位置漂移一个常见的问题是初始偏航角不对。我遇到过飞机总是反向飞行的情况最后发现是因为旋转矩阵的顺序搞反了。这时候可以用tf_echo工具实时查看坐标系关系rosrun tf tf_echo world map5. 不同设备的适配经验5.1 Realsense T265的特殊处理T265的坐标系定义比较特殊它的Y轴朝上、Z轴朝后。这意味着需要额外的变换先将T265的坐标系旋转到与D435i一致再应用标准的SEU到ENU变换具体变换矩阵为# T265到D435i的旋转 transform.transform.rotation.w 0.5 transform.transform.rotation.x -0.5 transform.transform.rotation.y 0.5 transform.transform.rotation.z -0.55.2 多传感器融合情况当同时使用视觉里程计和GPS时要特别注意GPS提供的是地理坐标系LLA需要转换为局部ENU坐标系确保视觉里程计的ENU与GPS的ENU对齐我建议在开阔场地先进行GPS定位然后以此为基准对齐视觉里程计。可以通过以下命令检查一致性rostopic echo /mavros/global_position/local6. 常见问题与调试技巧在项目实践中我积累了一些宝贵的调试经验坐标系翻转问题现象飞机朝相反方向飞行解决检查旋转矩阵顺序特别是绕Z轴的旋转方向高度漂移问题现象飞机无法保持固定高度解决校准IMU加速度计检查VINS的重力对齐延迟导致的振荡现象飞机在位置保持模式下振荡解决优化VINS计算速度减小发布延迟一个特别有用的调试技巧是在rviz中同时显示VINS的原始轨迹通常发布在/vins_estimator/odometry转换后的轨迹发布到/mavros/vision_pose/posePX4的实际估计/mavros/local_position/odom这样可以直接观察转换是否正确。我在调试时发现有时候问题不是出在旋转上而是出在坐标系原点的选择上。特别是在VINS重新初始化时世界坐标系可能会跳变这时候需要特殊的处理逻辑。