四旋翼与四元数学习笔记

为了实现四旋翼无人机的编队控制，重点学习了四旋翼加速度转换为姿态角和推力指令的具体手段，详细学习了四元数、旋转矩阵、欧拉角在四旋翼上的应用，学习了四旋翼的几何控制法、姿态角控制器设计，学习了mavros、c++中四元数有关内容。

1、学习了四元数在四旋翼无人机上的应用

常见的加速度指令并不是转换为姿态角来控制四旋翼，而是转化为四元数。在mavros的控制程序中，也没有使用欧拉角，而是采用了四元数。这是因为欧拉角存在多种表达方式难以统一定义，而且存在奇异点（俯仰角为90度时），无人机进行大机动时欧拉角容易在最大或最小值间跳变。四元数虽然不存在奇点且变量只有四个，但是也存在同一旋转有两种表示的问题（q和-q表示同一个旋转），这不利于全局稳定控制器的设计。主要学习参考了苏黎世联邦理工学院的四旋翼非线性控制报告《Nonlinear Quadrocopter Attitude Control》。该报告十分精辟，讲解通俗易懂且全面，是入门四旋翼控制的十分好的参考资料。

还有四元数有关的很好的讲解资料：

http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/

2、学习了四元数控制四旋翼的具体实现代码和算法

主要参考了阿木的prometheus项目和论文《Geometric Tracking Control of a Quadrotor UAV on SE(3)》。由于四旋翼有四个输入量（三个姿态角和一个推力），但是只需要控制x、y、z三个方向的加速度，因此航向角psi可以选择为定值或者不控制。另外，查找很多资料和代码后，得出结论：mavros中采用的欧拉角RPY是ZYX顺序（从world坐标系到body系经过z、y、x连续旋转，转轴在动系；等效于从world系到body系经过x、y、z连续旋转，转轴在静系），四元数是由ENU系旋转到body系的旋转四元数。mavros会进行自动的转换。主要参考mavros的github源码。c++中的Eigen库可以进行四元数的转换。其中Eigen:quaternion(Matrix)是把R(IB)转换到q。R(IB)的三个列向量表示body系系三轴在world系中的坐标，q表示world系z轴旋转到body系的z轴。这两种定义是R和q的常用坐标系旋转定义。

所谓四元数q和方位余弦R都可以表示坐标系的旋转。但是要注意，这里有两种“旋转”：

1. 坐标系不动，矢量a转动到矢量b。即有两个不同的矢量。
2. 矢量r不动，坐标系从I系（静系）转动到B系（动系）。然后讨论矢量r在I系和B系中的坐标表示。矢量只有一个，只是在两个坐标系中的坐标表示不同。

对于方位余弦矩阵R，主要用于第二种“旋转”，即同一向量在不同坐标系之间的坐标转换。方位余弦矩阵的三个列向量是动系的三轴在静系中的坐标，三个行向量是静系的三轴在动系中的坐标。欧拉角一般描述的是静系到动系的旋转，可以根据欧拉角写出方位余弦矩阵，注意欧拉角的正负（分清是要写动系还是静系的三轴坐标）。

对于四元数q，主要描述的是第一种“旋转”，用在坐标系的旋转时，就是指静系的z轴绕旋转轴q旋转到了动系的z轴处（坐标表示都是在静系里，因此旋转轴q的坐标也应该是在静系里）。

当然，用四元数q也可以表示第二种“旋转”，即矢量在不同坐标系中的坐标转换。此时，旋转轴q表示的是动系相对于静系的旋转轴，且旋转轴q的坐标表示在动系里。（虽然旋转轴q的坐标在动系里和在静系里是一样的，因为动系是相对于这个轴旋转的，但是连续旋转时就必须弄清楚旋转轴q的坐标是在动系里表示还是在静系里表示了）。具体可以看我写的《四元数乘法计算》中的四元数的用途部分。

一般来说，同一个四元数q既可以用在静系z轴到动系z轴的矢量旋转中，也可以用在矢量在动系中的坐标到静系中坐标的转换中。可见，在坐标系的旋转中，“矢量旋转”和“坐标转换”这两种旋转是“互逆”的。在表示动系的位姿时，我们倾向于用前者表示，然而在实际使用中，后者“坐标转换”是真正的需求。

以上，I表示静系，B表示动系。下标表示是哪个系的z轴，上标表示该矢量的坐标在哪个系里表示。

对于连续旋转的情况：

四元数的左乘表示z轴的连续旋转，但旋转轴一直都是在静系里，因此RPY角下，四元数q从左到右应该是qx，qy，qz（绕静系旋转是XYZ顺序）。当然，如果理解成绕动系中的转轴连续旋转，即ZYX顺序的话，那么连续旋转应该表示为四元数的右乘。具体可以参考上面提到的http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/

方位余弦矩阵R就简单很多，反正左乘R表示同一矢量从静系到动系的坐标转换，那么绕动系中轴的连续旋转自然就对应多次左乘。

为什么四元数q和方位余弦矩阵R表示绕动系中轴旋转时乘法顺序不同呢，前者是右乘，后者是左乘。因为我们用四元数表示的是坐标轴z的旋转，即“第一种旋转”，而用R表示的是矢量的坐标转换，即“第二种旋转”。从这里也可以看出这两种旋转是“互逆”的。

RPY角的连续旋转对应四元数和方位余弦矩阵情况如下：

详细推导情况建议参考上面提到的苏黎世联邦理工学院的四旋翼非线性控制报告《Nonlinear Quadrocopter Attitude Control》

在mavros的setpoint_raw/attitude中，静系world系是ENU系，动系body系是FLU系，那么输入的四元数自然是ENU系的z轴转动到FLU系对应的四元数，对应到旋转矩阵是从ENU系z轴转动到FLU系对应的方位余弦矩阵（即列向量由body系的坐标轴在ENU系中的坐标构成）。但是px4只接收NED系向FRD系旋转的四元数，因此mavros的setpoint_raw.cpp中有对应转换的部分。具体转换过程是遵循“四元数右乘表示绕动系中的旋转轴连续旋转”的思路。

3、四旋翼的高度环设计

我们一般假设四旋翼的高度环单独控制保持高度恒定，然后单独设计xy水平方向的控制器。然而在实际的四旋翼控制中并不是这样的。推力的大小不仅取决于高度误差，也和xy方向的加速度指令有关，在设计真实控制器的时候一般假设总推力方向垂直于机身平面，且平行于加速度方向（含重力加速度）。而且控制真实四旋翼的时候，并不能直接控制推力大小，而是只能控制“油门”（可以理解为电调输出到电机的pwm值），油门大小是归一化的，只能取(0,1)之间的数值。油门-推力关系是二次多项式，且系数和真实飞机电压、电调、电机有关，每架飞机的这些系数都不一样，如何获取这些系数是有待解决的很重要的问题。可以参考论文《Design, Trajectory Generation and Control of Quadrotor Research Platform》。

4、学习了欠驱动系统控制的有关知识

尤其是“微分平坦性”（Differential Flatness）的概念。这部分主要参考论文《Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories》、《Minimum snap trajectory generation and control for quadrotors》，以及MIT的课程：

《Underactuated Robotics：Algorithms for Walking, Running, Swimming, Flying, and Manipulation》。

微分平坦性的概念对于四旋翼、前轮转向车这些欠驱动系统的轨迹设计十分有用，选定某些“平坦”的输出就能确定所有输入量和状态量。