浅谈 vins 、mavros 和 px4 之间的坐标变换

一、前言

在自主无人机系统中，VINS-Fusion、Mavros、PX4 之间的参考坐标系是不同的，而这些坐标系之间的关系又很容易让人产生困惑，本文旨在理清不同模块之间的坐标变换关系，这有利于不同模块之间数据的转换，最后也会给出一些坐标变换方式。

二、 里程计坐标变换关系

本节主要介绍不同的里程计算法，如 VINS-Fusion、msckf_vio、open_vins 等里程计算法与世界坐标系的关系以及其内部不同元素之间的坐标变换关系！

2.1 world 坐标系

每一个里程计一般都会有一个 world 坐标系，然后再估计 body 坐标系在 world 坐标系中的位姿（也就是 body 坐标系 到 world 坐标系的变换关系）。

这里的 world 坐标系指的是一个全局参考系，也就是一个坐标系要确定其世界位置和姿态，共同参考的一个坐标系（参考的点不同，全局位姿就不同，但是全局参考系只能有一个！！！），如下图所示

其中黑色的三轴表示 world 坐标系，而两个蓝色的三轴分别表示 body 1 坐标系和 body 2 坐标系，绿色的箭头指的是 world 坐标系分别到 body 1 坐标系 和 body 2 （transform frame）。

由此可知 body 1 坐标系和 body 2 坐标系 的位姿都是相对 world 坐标系而言的，其中 body 1 坐标系和body 2 坐标系可以是 imu 位姿 或者 相机位姿 或者 雷达位姿，它们的位姿都是相对 world 坐标系而言的！

那么有一个很重要的问题：world 坐标系是怎么确定的？

一般来说，world 坐标系往往会根据 某个 body 坐标系在某个固定时刻的位姿 来确定！

这个问题在后续分析不同的里程计算法时会详细阐释。

2.1 VINS-Fusion 坐标变换

接下来我们详细介绍 VINS-Fusion 算法中的坐标变换。

在 VINS-Fusion 中，主要有坐标系： world 坐标系、body 坐标系、camera 坐标系，其中 body 坐标系就是 IMU 的坐标系（位姿），camera 坐标系是相机（如果是双目则是左目相机的坐标系），而 world 坐标系 是在初始化完成后通过 关键帧选取和重力对齐 两个方式结合产生的。

（1）选取初始滑窗中第一帧的 body 坐标系位置 作为 world 坐标系的位置

（2）将 world 坐标系与重力方向对齐

接下来将重力方向作为 世界坐标系 z 轴（所谓的重力对齐）

需要注意的是， IMU 测的实际上是重力的平衡力（也就是支持力），而 VINS-Fuison 中 g 是正的，因此直接把 IMU 测得的 支持力 和 正的 g 对齐就可以，此时他们都是朝向 z 轴正方向！！！

此外，由于仅仅通过重力对齐，得到的 world 坐标系并不是唯一的，此时 world 坐标系绕 z 轴旋转所产生的坐标系 都可以保证 重力对齐，因此为了保证唯一性，VINS-Fusion 将 world 坐标系的 x 轴方向与 初始滑窗中第一帧 IMU 的 x 轴进行了对齐

从而获得一个新的坐标系，这两个坐标系之间会有一个旋转关系，这个旋转关系可以通过一次重力对齐旋转和 x 轴对齐旋转获得！

接下来我通过图例进一步阐释，这里我做了两个测试，第一个测试将 相机正方，如下所示

运行 在这种情况下启动 realsense d435i 和 VINS-Fusion，启动 rviz 可视化并订阅 TF 坐标变换

其中 红色表示 x 轴，绿色表示 y 轴，蓝色表示 z 轴。由于 VINS-Fuison 中进行了重力对齐，因此此时世界坐标系和真实世界坐标系的重力方向都是一致的，竖直向下。

接下来我们作轻微的向上移动相机

我们发现世界坐标系 红色的 x 轴与相机 红色的 x 轴是对齐的，但是 y 轴和 z 轴并不是对齐的，事实上，通过分析 d435i 相机的 /camera/imu 加速度数据可知，d435i 相机的坐标应该如下所示

再结合前面 rviz 中的 world 坐标系和 body 坐标系的对比可知，world 的坐标系的确立方式：

在VINS-Fusion 初始化完成之后，以相机的初始位置作为 world 坐标系的位置，以重力方向作为 z 轴，相机 x 方向 在水平面上的投影作为 world 坐标系的 x 轴，最后以 xz 平面的法线方向作为 y 轴方向！！！

最后举一个例子验证一下，我们将相机的的摆放位置进行简单的旋转

接下来运行 VINS-Fusion，并启动 rviz 可得

此时我们发现， world 坐标系红色的 x 轴实际上就是相机 body 坐标系红色的 x 轴在 水平面上的投影，而 相机坐标系 蓝色的 z 轴 此时正好与 world 坐标系的 y 轴重合！

综上所述，对于一个正常放置的相机而言，如果假设 VINS-Fusion 开始运行时，相机的朝向为正东方，则 world 坐标系 的 x 轴方向是朝向正南的，y 轴方向是朝正东的，z 轴方向是朝天的，也就是说 VINS-Fusion 的参考的坐标系是 南东天 （SEU）坐标系！！！

三、Mavros 坐标变换关系

3.1 mavros 坐标系介绍

mavros 的坐标变换关系主要分为有三种：东北天坐标系（ENU），北东地坐标系（NED），前左上坐标系（FLU），但是网上很多资料都没有说清楚它们到底是什么意思，本文通过实际实验对 mavros 的坐标变换关系进行分析，旨在缕清楚 mavros 中的坐标变换关系！

本文以 D435i 相机和 Pixhawk 6X 飞控 为工具，尝试缕清 mavros 中的坐标系 与 VINS-Fusion 里程计中的坐标系之间的关系。

由于 mavros 中常用的 坐标系 有 global 坐标系，local 坐标系，body 坐标系，其中 global 坐标系指的是经纬度坐标，常在使用 GPS 时才会产生（应用较少），因此主要关注 local 坐标系和 body 坐标系。

首先我们启动 mavros 节点，终端输入

roslaunch mavros px4.launch

此时我们查看 对应于 机体在 local 坐标系下的位姿的 /mavros/local_posion/pose 话题，终端输入

rostopic echo /mavros/local_position/pose

发现此时话题没有输出，

这说明此时 mavros 并没有自己的局部坐标信息，这意味着目前飞控并没有产生局部坐标系，所以网上所谓的 “局部坐标系以指的是机体上电时的位置作为原点” 的言论是不正确的，因为此时还没有产生局部坐标系！（事实上，这是里程计的效果，飞控并没有生成里程计信息的能力，只有 IMU 信息）

3.2 VINS-Fusion 到 Mavros 的坐标变换

此时我们启动 d435i 相机，并运行 VINS-Fusion 里程计，打开 rviz 查看

此时假设机体朝向正东，则世界坐标系 红色的 x 轴表示正南，绿色的 y 轴表示正东，蓝色的 z 轴表示天上！

注意！！！这里的假设机体朝向正东的意思是 将机体当前所朝的方向定义为东方（并不是真的知道东方在哪边）！！！

接下来我们将 VINS-Fusion 的里程计输出 发布给 mavros 的 /mavros/vision_pose/pose 话题，但是这里会涉及到一个问题：VINS-Fusion 的 world 坐标系 与 mavros 中的 map 坐标系（发布给 /mavros/vision_pose/pose 话题的位姿就是其在 map 坐标系中的位姿，也就是 local 坐标系）并不是对齐的，因此不能直接地将 VINS-Fusion 中 里程计的位姿直接发布给机体，而是需要做一定的坐标变换！

正如上节提到的，VINS-Fusion 的 world 坐标系 是 初始化滑窗第一帧 决定的，在机体和相机正常摆放的情况下，就是 world 坐标系是 南东天坐标系（SEU），此时机体朝东，也就是 y 轴正向！

而 mavros 的 map 坐标系原点是 mavros 收到的里程计信息时 飞控所在的位置，并且其 map 坐标系的 x 轴朝东，y 轴朝北，z 轴朝天，也就是所谓的东北天坐标系（ENU），此时机体方向朝东，也就是 x 轴正向！

其中 world 是 VINS-Fusion 中的参考系，而 map 坐标系是 mavros 中的参考系。

因此，要将 VINS-Fusion 的 机体（body 坐标系）在 world 坐标系下的位姿 转化为 机体 在 map 坐标系下的位姿，必须进行相应的坐标变换！

接下来的坐标变换我们基于一个假设：VINS-Fusion 中的 D435i 相机的 IMU 位置（body 坐标系） 与 Pixhawk 6X 飞控的 IMU 位置（base_link 坐标系）是重合的（但是角度不是重合的）！

为了保证 body 坐标系 与 base_link 坐标系的一致性，我们必须将 body 坐标系在 world 坐标系中的姿态 转化为 base_link 坐标系在 map 坐标系中的姿态！！！

需要做的是先将 body 坐标系绕 x 轴转 90 度，再绕旋转后的 body 坐标系的 z 轴旋转 90 度，此时旋转后的坐标系就是 base_link 坐标系在 world 坐标系中的姿态！

接下来要将这个旋转后的坐标系变换到 map 坐标系，需要做的是将此坐标系绕其本身的 z 轴旋转 -90 度（注意是负的90度），此时得到的 坐标系就是 base_link 坐标系在 map 坐标系中的姿态！

我们需要做的就是计算出其中的旋转量，然后利用这个旋转量将 body 坐标系在 world 坐标系中的位置和姿态 转化为 base_link 在 map 坐标系中的位置和姿态！！！

其中 第三幅 和 第四幅 图表示的是同一个意思。

最后将 base_link 在 map 坐标系中的位置和姿态发布给 /mavros/vision_pose/pose 话题即可！

注意，在将视觉定位数据发布给 mavros 之前，需要在 QGroundControl 中设置参数。

打开 QGroundControl，搜索 EKF2_HGT_MODE，将其对应的值修改为 Vision

然后搜索 EKF2_AID_MASK，将其值修改为 24

接下来我们在 rviz 中 订阅 world 坐标系和 body 坐标系的位姿关系，可以看到

在将 body 坐标系进行旋转得到 base_link 坐标系，并发布 base_link 坐标系 与 world 坐标系，的相对位姿关系

接下来我们在 tf 中发布 world 和 map 之间的坐标变换关系

此时 map 坐标系 与 world 坐标系的原点是重合的，图中最大的坐标系为 world 坐标系，被 world 坐标系 挡住的是 map 坐标系，并且此时在图中，map 坐标系 与 base_link 坐标系的位姿是一样的（map 坐标系 与 world 坐标系的相对位姿是固定的，不会随 base_link 坐标系的改变而改变）

最后我们看一下，base_link 坐标系在 map 坐标系中的位姿关系

在将 base_link 在 map 坐标系中的位置和姿态发布给 /mavros/vision_pose/pose 话题之后，我们可以查看 /mavros/local_position/pose 话题

其中 红色的大箭头指的 是当前 机体的朝向，也就是 飞控 IMU 坐标系的 x 轴，此时保证了 飞控 IMU 的坐标轴 与 base_link 坐标系（也就是 发布给 /mavros/vision_pose/pose 的位姿）保持了一致，这样才能保证飞机飞行时 飞控 IMU 位姿 与 视觉定位提供的飞机位姿 的一致性，从而保证飞机飞行的稳定性！！！

事实上，由于 飞控内部会对 /mavros/vision_pose/pose 提供的视觉定位位姿信息 和 飞控 IMU 提供的信息进行融合，最终生成 /mavros/local_position/pose 中的位姿信息，如果视觉信息和 IMU 的基本坐标系都不对应，那么融合生成的 /mavros/local_position/pose 就会很奇怪的随意旋转，导致一系列错误的发生！

3.3 其他产品到 mavros 的坐标变换关系

需要注意的是，本节的内容主要针对 Realsense D435i 相机作为 VINS-Fusion 的视觉相机时， VINS-Fusion 中 body 坐标系 到 base_link 坐标系的变换关系，如果使用的 是其他相机，虽然思路是一样的，但是具体的细节有所不同。

3.3.1 Realsense T265 相机

如 Realsense T265 相机，虽然思路是一样的，但是具体的细节有所不同，其原因在于 D435i 相机 与 T265 相机的 IMU 坐标系 有所不同

我们发现 T265 相机的 y 轴是朝上的，z 轴是朝后的，而 D435i 相机的 y 轴是朝下的，z 轴是朝前的，因此在将 T265 相机的位姿发布给 mavros 时需要做的坐标变换 与 D435i 的位姿发布给 mavros 时需要做的坐标变换是不同的，但是基本原理是一样的！！！

3.3.2 飞控提供 IMU 信息

如果使用 飞控 提供 VINS-Fusion 的 IMU 信息，并且飞控的朝向与相机的朝向是一致的，此时 VINS-Fusion 和 mavros 使用的都是 飞控的 IMU 信息，这保证了 body 坐标系 与 base_link 坐标系 的一致性。

并且在这种情况下 world 坐标系 map 与坐标系是重合的，因为这种情况下初始时刻 world 坐标系与 body 坐标系重合，并且相机朝向 x 轴，map 坐标系与 base_link 坐标系重合，相机头朝向 x 轴。

这就意味着可以直接将 VINS-Fusion 的 body 坐标系的位姿信息发布给 mavros，此时不需要做任何的坐标变换！！！