【系列专题】MAVROS中的坐标转换

　　最近在做视觉识别的项目，视觉开发的工作在NVIDIA的JETSONTX2板卡上完成，也可以用ODROID，这个视情况而定。

　　而视觉识别后的结果要传输给飞控，因为在TX2上面跑的是ROS系统，所以TX2与PIXHAWK数据之间的交互使用到了MAVROS，飞控在OFFBOARD模式下通过MAVLINK的接口接收MAVROS上的期望。

　　这些期望可以是期望位置、期望速度和期望姿态，而同时TX2也会从MAVROS上获取需要的飞机状态信息，一般包括飞机的控制模式、解锁状态、姿态、速度、位置信息等。

　　TX2获取的主要信息都来自MAVROS的/mavros/local_position/pose这个话题，但所有的位置和姿态信息都要根据坐标系来定义。本来以为它们都是使用的NED和Aircraft系，结果在使用它们运算的时候出现了很多错误，通过echo此topic的值，很容易就发现在位置上使用的是EDU坐标系。

　　但是姿态由于是四元数的表示方法，很难明确使用的是哪两个坐标系之间的转换关系。

　　因此，只有到MAVROS的源码中寻找了。

　　先到github上下载mavros的源码：

　　gitclone https://github.com/mavlink/mavros.git

　　然后在plugins文件夹下找到local_position.cpp文件：

 

　　advertise函数返回一个 Publisher对象。通过调用对象的publish( )函数，我们可以在这个topic上发布 message。

　　这就是MAVROS发布消息的过程。

　　接下来我们看到的是位置信息是东北天(EDU)坐标系下的，而姿态信息也是EDU坐标系向Baselink坐标系的旋转关系。

　　这些代码在imu.cpp文件中可以找到：

　　姿态信息来源代码：

　　ned_aircraft_orientation是来自飞控计算得到的NED-Aircraft的四元数姿态坐标，

　　enu_baselink_orientation是将坐标系经过两次转换变成了ENU-Baselink坐标系的四元数姿态坐标。

　　Aircraft是这样的(X在参考平面内沿机头方向向前，Y轴垂直机身参考平面向右，Z轴在参考平面内垂直XOY平面向下)：

　　Baselink坐标系是这样的(X在参考平面内沿机头方向向前，Y轴垂直机身参考平面向左，Z轴在参考平面内垂直XOY平面向上)：

　　而上述这些坐标转换都使用了Eigen库中的旋转公式来完成的：

　　上述的公式其实就是著名的罗德里格斯旋转公式，其源码是这样的，因为是绕轴逆时针旋转，所以其绕坐标轴旋转得到的结果和我们以前推导的旋转公式刚好是转置的关系：

 

　　罗德里格斯旋转公式是这样的：

　　关于Eigen库中欧拉角、四元数、旋转矩阵的转换关系，是在ROS开发中经常用到的线性代数运算库。

　　我们后面有时间可以再来写一写，其实最重要的还是要理清楚他们的旋转方向，在什么坐标系下面进行的旋转以及旋转的顺序是怎么样的。

　　本文转载于【无人机系统技术】，作者文龙，北航博士。提供服务：免费分享无人机技术知识，开发经验，实际问题解析，解读开源飞控px4和ardupilot的算法和代码，关注公众号第一时间免费看心得知识。