2Dslam无人机坐标系

室内2D激光雷达SLAM中涉及的无人机坐标系转换问题

前面几篇博客再配置好Cartographer之后进行了一次失败的室内试飞，但是失败了，当时把问题归结于飞控内环没有调好。但是后来又测了几组静态的，并且上网查阅了一个mavros和px4底层的源码，发现了坐标系转换的过程中存在遗漏之处。简单说就是雷达slam得到的定位值怎么和localNED坐标系建立起联系。

坐标系问题的引出

在QGC中将位置信息源选为vision_position模式，也就是靠外部传感器的SLAM定位结果对无人机进行导航。查阅了资料之后，我们发现QGC有一个参数是localNED的x，y，z坐标值，突然意识到之前没有测量飞机在移动过程中，这三个参数变化是不是符合正常趋势，正常趋势直白定义可以是无论飞机初始化的Heading方向如何，飞机沿着正北飞行时，仅x增大、沿着正东飞行时，仅y增大，带有角度移动时同理。
我们按照这个思路进行了一下测试，开始将飞机heading指向正北，在雷达slam给出的结果中，飞机初始位置为原点origin，前方Forward为x轴正方向，左侧Left为y轴正方向，上方Up为z轴正方向（由定高雷达得到）。其实这个坐标系是ROS里面的一种标准机体系(FLU)。而px4中localNED坐标系是正北方North为x轴正方向，正东East为y轴正方向，正下方Down为z轴正方向。
当飞机向前方移动时，x_Vision和x_NED同向正常变化，而向右移动时，y_Vision负方向平稳增大，y_NED的值在最初时刻几乎不动，然后过了一小段时间收敛到和y_Vision同方向，也就是负值。
问题很明显向右移动过程中明明y_NED应该为正值，而实际上接收到的是一个负值。联想卡尔曼滤波（EKF）的预测和更新过程，我认为这里面IMU和磁力计会对飞机位置进行一个预测，也就是飞机在上述情况下移动时，飞控本能的认为y_NED应该是一个平缓增大的正值，姑且称为预测值。然而得到的定位信息（量测值）为平缓增大的负值，也就是预测和量测得到的值完全相反，但是在参数设置过程中，我们给量测值一个很高的信任度，几番较量之后，飞控还是相信了量测值。不过这么信任下去就凉了，所以切成了定高模式，取消了位置外环控制权。之后的几组实验都发生了类似情况。

坐标系问题的解决思路

不过回想起来确实整个过程中都没有考虑过坐标系对齐的问题，上ROS的Wiki上 Coordinate frames for mobile platforms.找了找资料，对ROS的坐标关系进行梳理。其他博客有这部分的中文翻译，我粘出部分原图，顺带一点自己的想法。

map和odom都是world-fixed frame，以slam无人机举例，我的理解是map和odom在完全理想的情况下其实是重合的，而在实际情况中，初始时刻可以认为是重合的。无人机在移动过程中，slam会给出一个里程计来估计自己相对原点的位置，这个值短时间内准确且连续，但是长时间会有飘逸误差，不能作为长时间的local参考；map是长时间准确但不连续，可以作为长时间的local参考，我认为slam经过后端优化（回环检测）之后的全局信息就是一个map系，无非是算法精度会产生影响，但是每个slam算法得到的map系中的信息是可以用来作为导航参考的。

Earth和Map坐标系差初始化位置飞机和大地系的偏角（比如与正北正东方向的夹角）。

文档后文对odom坐标系进行了详细的叙述。

TF

在ROS的wiki上对TF有很详细的介绍配合小乌龟的例子也很好懂，简单说TF就像一棵树，将整个系统的pose，frame等主要信息串联起来，并且能实时发布和收听。一个无人机系统的定位、导航以及目标检测与识别涉及到多个坐标系之间的转换，因此TF是一个很好用的工具。

坐标系的选择

坐标系的选择和使用的定位信息有关，目前PX4常用的有GPS、光流、SLAM等方法获得自身位置信息，使用GPS的一般都会选择NED_local作为基准参考系，不过在一些SLAM的定位方法中，使用机体本身的参考系更加方便，如ROS里面的FLU，直观理解就是在设置目标飞行点的时候可以按照我看到哪就飞到哪的想法去设置。如果在后续要将多种定位信息融合起来，比较推荐的方法还是统一转换到GPS的NED坐标系下。待测试完成后更新细节。