多旋翼无人机

        无人机的状态估计包括姿态解算和速度位置估计，为了进行状态估计，我们首先需要了解状态估计所需要用到的4个坐标系。这4个坐标系为：传感器坐标系（sensor）、机体坐标系（body）、本地坐标第（local）、全局坐标系（global）。

        我们来讨论坐标系变换的相关内容。假设在3维直角坐标系x^0、y^0、z^0中坐标系以z^0为转轴按逆时针旋转θ之后得到新的坐标系坐标系x^1、y^1、z^1，那么在原坐标系下的向量p在源坐标系下的坐标和新坐标系下的坐标如图所示：

        于是有：

        其中，

        其中，R_0^1为坐标系0变换到坐标系1的变换矩阵，也就是说，坐标系变化后，向量p在原坐标系下的坐标左乘变换矩阵得到其在变换后的坐标系下的坐标。变换矩阵是以z轴为转轴的，我们再给出分别以x轴和y轴为转轴的变换矩阵：

        设无人机的3个姿态角分别为：
                ∅：滚转角（Roll）；
                θ：俯仰角（Pitch）；
                φ：航向角（Yaw）。
        于是，当无人机产生滚转角、俯仰角和航向角之后，其变换矩阵为：

        注意，3个单坐标轴旋转的变换矩阵相乘的顺序不同，所得到变换矩阵的结果也是不同的。因此，在实际应用用通常采用四元数来进行坐标变换。由姿态角到四元数的转换公式如下：

        其中，

        四元数的变换矩阵为：

        接下来我们通过一个例子来简单说明变换矩阵的作用：设在无人机机体坐标系中得到的当前机体的速度v^b为：

        当前姿态角为： ∅、 θ、 φ，将姿态角转为四元数并得到其变换矩阵中的R_q，于是无人机在本地坐标系下的速度v^l为：

        坐标变换是无人机姿态解算以及速度和位置估计的重要工具，在数据融合时需要将各个传感器数据以及GPS所采集到的数据从自个的坐标系通过变换矩阵转换到本地坐标系当中，然后参与各项计算。