四旋翼飞行器姿态控制(四轴飞行器姿态解算)

大家好，又见面了，我是你们的朋友全栈君。

1、惯性测量单元IMU(InertialMeasurement Unit)

姿态航向参考系统AHRS(Attitude and Heading Reference System)

地磁角速度重力MARG(Magnetic, Angular Rate, and Gravity)

微机电系统MEMS(Micro Electrical Mechanical Systems)

自由度维数DOF(Dimension Of Freedom)

无人驾驶飞行器UAV(Unmanned Aerial Vehicle)

扩展卡尔曼滤波EKF(Extended Kalman Filter)

无损卡尔曼滤波UKF(Unscented Kalman Filter)

惯性导航系统INS(Inertial Navigation System)

全球导航卫星系统GNSS(Global Navigation Satellite System)

天文导航系统CNS(Celestial Navigation System)

可垂直起降VTOL(Vertical Take-off and Landing)

2、  常见的导航系统：惯性导航、天文导航、卫星导航、路标导航、无线电导航、推算导航、组合导航。

3、  有两个基本坐标系：“地理”坐标系和“载体”坐标系。”地理”坐标系指的就是地球上的“东北天（ENU）”坐标系，而“载体”坐标系值的就是四轴自己的坐标系。

4、  在“地理”坐标系中，重力的值始终是（0，0，1g），地磁的值始终是（0，1，x）。这些值就是由放置在四轴上的传感器测量出来的。

5、  “地理”坐标系和“载体”坐标系是两个不同的坐标系，需要转化。转化的方法就是坐标系的转换，目前有三种方式：四元数（q0123）、欧拉角（yaw（Z轴）/ pitch（Y轴）/roll（X轴）属于其中一种旋转顺序Z-Y-Xà航空次序欧拉角）、方向余弦矩阵（9个系数）。

6、  所谓的姿态，就是公式+系数。比如：欧拉角公式和欧拉角的系数（翻滚、倾仰、偏航）

7、  姿态的数据来源有5个：重力、地磁、陀螺仪、加速度计、电子罗盘。其中前两个来自“地理”坐标系，后三个来自“载体”坐标系。

8、  导航的基本原则就是保证两个基本坐标系的正确转化，没有误差。只有实现了这个原则，载体才可以在自己的坐标系中完成一系列动作而被转换到地理坐标系中看起来是正确的。为了达到这个目标，需要对两个坐标系进行实时的标定和修正。因为坐标系有三个轴，偏航yaw修正由电子罗盘（基于载体）、地磁（基于地理）对比修正误差补偿得到。倾仰pitch和翻滚roll上的修正由加速度计（基于载体）、重力（基于地理）对比修正误差得到。在完成了基本原则的基础之后，即保证两个坐标系的正确转化后，利用基于载体上的陀螺仪进行积分运算，得到基于载体坐标系的姿态数据，经过一系列PID控制，给出控制量，完成基于载体坐标系上的稳定控制后，反应到地理坐标系上的稳定控制，从而达到我们观察到的定高、偏航、翻滚、倾仰等动作。

对于上述论述可以看出，导航姿态从理论上讲只用陀螺仪是可以完成任务的。但是由于陀螺仪在积分过程中会产生误差累计，外加上白噪声、温度偏差等会造成导航姿态的解算随着时间的流逝而逐渐增加。所以就需要用加速度计在水平面对重力进行比对和补偿，用来修正陀螺仪的垂直误差。但是对于竖直轴上的旋转，加速度计是无能为力的，此时用的是电子罗盘。他也可以测量出水平面内的地磁方向用来修正陀螺仪的水平误差。通过这两个器件的修正补偿，使得陀螺仪更加稳定、可靠的工作。

9、  加速度计在地球上测量的是重力加速度，如果载体沿着z轴旋转，加速度计是无法感知他的运动的；类似的，电子罗盘测量的是地球上的磁场方向，如果载体沿着y轴旋转，电子罗盘同样也是无法感知他的运动的。综上所述，加速度计和电子罗盘只能得到2维的角度关系，通过某种方式的融合，可以得到正确的三维姿态信息。

10、   在这里要弄清楚一个问题，前面第8条所说的关于地理坐标系和载体坐标系之间的互相转化。这样就有两种转换方向：一是把B系（载体）转换到N系（地理）；二是把N系转到B系。当我们在实际控制当中，我们关心的显然是载体坐标系相对于地理坐标系之间的变化，所以我们通常使用的旋转矩阵是把N系转到B系的矩阵（两者的关系是转置关系）。比如本次在利用加速度计计算姿态误差时，可以利用上一次的四元数姿态在N系中的三个轴的垂直分量转换到B系中垂直分量来算误差。 式中的右边为N系到B系的旋转矩阵的第三列元素（恰好是重力g在B系中的值）

11、   在单位时间内的位移被定义为速度，速度有线速度和角速度之分，分别对应两种传感器测量这两种不同的速度：线速度传感器（加速度计）、角速度传感器（陀螺仪）。所以，陀螺仪是用来测量角速度的，用于坐标系的旋转，也就是导航姿态了。加速度计只能测量线速度，最典型的例子就是重力加速度，如果加上水平坐标系上的加速度，形成合力F产生a。考虑一个导弹，他的飞行速度由加速度计来测量而飞行过程中的转体姿态由陀螺仪来测量。

12、   当我们把加速度计拿在手上随意转动时，我们看的是重力加速度在三个轴上的分量值，无法直观的观察到三个轴上的加速度分别是多少。为了实现这样一个目的(可以看到每个轴上的真实加速度)，我们需要一个旋转矩阵，这个矩阵的作用就是把放置在载体坐标系上的加速度计值转换到参考坐标系中，在参考坐标系中，三个轴上的值始终都是（0，0，1）。所以当我们把加速度计以任意角度固定在空间中时，无论加速度计的三个轴的值是多少，当经过旋转矩阵变换后，在参考坐标戏中输出的值始终都是（0，0，1）–>这表明在参考坐标系中，物体在x和y轴上是没有加速度的，只有在z轴上存在重力加速度。但是这里又存在一个问题，既然z轴的输出是1，就是说存在加速度，物体应该运动起来才对。但是这里物体并没有运动。为什么输出是1呢？这涉及到加速度计的设计问题：加速度计测量加速度是通过比力来测量，而不是通过加速度。通过想象一个盒子中的小球就就可以明白。加速度计只有在自由落体时，其输出为0。

13、   便于记忆的一个例子就是如何从青山到黄家湖。对于一个人来讲，要从青山到黄家湖，必须满足两个要求：1、你必须有张武汉地图，并且知道黄家湖的位置和青山的位置。2、你必须带有方向导航系统，实时更新你目前的朝向。对应到飞行导航上面，黄家湖的位置对应“地理”坐标系，青山的位置对应“载体”坐标系。你的目的就是让这个两个坐标系被正确转化和标定。这部分工作交给加速度计和电子罗盘处理。至于你具体是走过去，骑自行车去，乘公交去还是做出租车过去，对应在飞行导航上面的话，利用的陀螺仪通过积分作用确定自己的动态姿态。

14、           单轴融合的最简单的例子：在这里，K= 控制周期/传感器采样周期。

15、   在复数域里面，二维坐标通过对复数的加减乘除运算可以快速方便地表达出来，尤其是旋转。现在考虑三维空间的复数向量的拉伸和旋转，或者更高维度。那么就需要一个复数域坐标系，容易想到的形式就是h=a+bi+cj，事实证明在二维复数域里面简单添加一元j是无法构成三维复数空间的，实际上需要四个参数才能够构建三维复数空间（两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例），即h=a+bi+cj+dk。这就是四元数的基本表达形式（其中i²=j²=k²=-1）。即用四个变量来表达三维空间的位置坐标，这就是复数域和实数域的不同。但是，这样定义是有前提条件的，即牺牲了乘法的交换律。例如两个四元数hp≠ph。如此一来，就出现了Q8乘法矩阵表。

16、   对四元数更进一步分析，发现四元数可以写成一个实数加上一个三维向量的和，即h=d+u（其中d为实数，u为三维向量）。令p=w+v，则

而

其中，实数乘法和内积具有乘法交换律，但是三维向量的外积不同，有 u x v = -v x u。所以，hp-ph就是两个向量外积的两倍。如果两个向量部分外积为0，那么乘法运算就可交换了。

17、   对于四元数的乘法pq，就是在四维空间F上一个线性变换，因此必有两个互相垂直的二维不变子空间，分别是（1，0，0，0）和u张成的二维平面（这个平面在四维空间中，我们无法看到全貌，只能看到与我们相交的一条直线，即u）和由u1和u2组成的二维平面（u1和u2是在u的三维空间中找到的三者两两垂直的符合右手定则的一组基，这个平面我们是可以看到的）。所以四元数的乘法的几何意义就是在这两个二维不变子空间中做伸缩旋转的线性变换。角度。伸缩因子为||p||（从（1，0，0，0）到u旋转，从u1到u2旋转）。如果p乘在右边，第一次旋转与上述方向相同，但是第二次旋转方向则与上述相反。本条所述内容全部发生在四维空间中，记住，四元数无法表示四维空间中的所有拉伸旋转，因为他要求两个不变子空间上的旋转角相同。但是他完全可以表示三维空间中的所有拉伸旋转。如果要讨论三维空间，那么四元数是完全可以胜任的。

18、   在三维空间用应用四元数乘法做线性变换时，会存在两次旋转，一次从（1，0，0，0）到u的旋转，第二次从u1到u2的旋转。前者旋转发生在四维空间，我们看不到，只看到u这一条交线。但是第二次旋转发生在三维空间，我们是可以看到的。

19、   我们来看一下在三维空间中是如何旋转的。给定一个三维向量p(0,x,y,z)，这是用四元数来表示的。然后做四维空间中的线性变换RPQ（Q为R的共轭向量，并且R为单位四元数，即N(R)=1），得到的答案就是(0,x’,y’,z’)。其中R=(cos(theta/2),alpha*sin(theta/2),beta*sin(theta/2),gama*sin(theta/2))，且alpha²+ beta²+gama²=1。这表示：在三维空间中将P向量绕着(alpha,beta, gama)轴逆时针旋转theta角度，长度不变。之所以为什么是theta/2，是因为在四维空间中实际上只转了theta角度。

20、   关于高维空间的知识。低维度事物无法感知在高维度发生的事情和动作。比如我们将一条纸袋旋转对折后首尾相连后在纸带的一面沿着直线一直画线，在二面平面上我们一直以为我们走的是直线，但是在三维上我们却是在走圆，只不过首尾相接，二维无法感知，这是在三维上干的事情。并且低维度的实物只能观察到高维度的实物在低纬度上的投影图像，比如扑克牌人看到的人体模型就是用一张纸纵切我们的人体，比如我们在现实生活中看到的人的外貌其实是四维空间在三维空间上的投影而已。二维空间上看到的直线有可能在三维空间上是一个圆，所以三维空间上看到的直线有可能在思维空间上是一个圆。所以我们在用四元数表达三维空间的时候，看到的实际上是四维空间中的一个切线，我们看到的直线有可能在四维空间是一个圆。

21、   球极投影对于从低维到高维的理解是比较好的一种方式。比如地球的地图球极投影。

22、   将一个数乘以-1，相当于找到对应与原点的镜像相反数，再乘以-1后又回到了原来的位置。这样的一个-1x-1的过程，相当于把数字转了360度。也就是说-1就意味着将数字旋转180度。现在定义一个数，只需要旋转90度，即出现 。在这里特别注意一下，我们在横坐标上操作的是只具有一维长度的实数，这样定义会出现一个不在横坐标上的数，这样需要扩展维度，如此一来，定义i为旋转90度，对应画出垂直于横坐标的纵坐标，就出现了复平面。既然是二维的平面，就需要两个数来表示坐标，正如我们的实数平面中的x和y坐标。但是复数不同，复数只需要一个复数就可以表达一个平面位置的拉伸和旋转。

23、   四元数p=[w,u]（其中w为标量，u为矢量）。描述的是一个旋转轴和一个旋转角度。如果用一个向量乘以一个四元数p，表示的是该向量在这个旋转轴旋转一个特定角度。

24、   用于表示旋转的方法有很多：Axis/angle、欧拉角、方向余弦矩阵、四元数。相比于其他几种表示方法，四元数具有不存在欧拉角存在的gimbal lock 问题、只需要4个系数而非方向余弦矩阵的9个系数、两个四元数更容易插值、两个四元数相乘表示旋转等优点。

方向余弦矩阵系数太多，难以插值。

欧拉角虽然表达简单，但是存在Gimbal lock问题（即可能失去一个自由度）

Axis/angle的问题如同欧拉角。

25、   用四元数直接表示旋转是很困难的，所以我们可以采用欧拉角来表示，但是在进行空间旋转的计算和插值时，需要对欧拉角和四元数进行转化，因为直接计算欧拉角会遇到Gimballock问题，而用四维空间中的四元数进行计算没有此类问题，并且插值简单（因为在思维空间中进行插值，就是在三维球形空间中的最短路径问题，个人理解，可能有误）。这就是优缺点的互补：采用欧拉角来表示当前载体的姿态，而在具体计算时将其转化为四元数。

26、 该融合方案是将加速度计和地磁计的值经过QUEST算法融合后计算出四元数abcd，然后和陀螺仪的输出（角度速率）经过卡尔曼滤波后给出物体的估计四元数q。其中QUREST算法可以换成高斯算法（需要大量矩阵运算，可能需要DSP）或者梯度下降算法（折衷算法）。

27、  

28、   这样图从理论上给出了融合的具体依据。图中的中间竖线表示高斯算法，左下角关于四元数的微分方程很重要，该方程将四元数和角度变化率联系起来构成常系数齐次线性微分方程。两个相加融合后积分后再归一化，即可得到物体的姿态四元数表达式。再经过欧拉角的变换即可转换为我们熟知的Roll，Pitch，Yaw。

29、   从一个坐标系到另一个坐标系的转换前面谈到有多种转换方法：欧拉角法、方向余弦矩阵法、四元数法等。其中欧拉角法的核心思想是：一个坐标系可以用另一个参考坐标系的三次空间旋转来表达。旋转坐标系的方法又有两种：一种是依次旋转三个不同的坐标轴；另一种是相邻两次旋转不同的坐标轴。第一种旋转方法称之为Tait–Bryan angles（可选顺序有x-y-z, y-z-x, z-x-y,x-z-y, z-y-x, y-x-z）；第二种旋转方法称之为Euler angles（可选顺序有z-x-z, x-y-x, y-z-y,z-y-z, x-z-x, y-x-y）。另外还有两个概念，外在旋转（extrinsic rotations）和内在旋转（intrinsic rotations）。我们固定不动的参考坐标系为xyz，需要被旋转的坐标系为abc。初始状态两个坐标值完全重合，现在的目标是旋转坐标abc到达指定位置。所谓的外在旋转指的是三次旋转中每次旋转的旋转轴都是固定参考系中的xyz轴中的一个轴。例如：Tait–Bryan angles的xyz顺序，那么在旋转abc的时候，每次旋转把abc坐标系围绕固定参考系xyz中的某个轴旋转；而内在旋转指的是在旋转abc的时候，每次旋转围绕的的轴是上一次abc旋转后的某个轴。打个比方，就好比数学中的数列问题，题目一般给出的是n项和n-1项的关系表达式，n项的值是根据前一项推导出来的，建立在前一次的值之上，而通项公式则是可以直接通过n的表达式计算任意第n项的值，比如计算第10项的值直接通过n的表达式就可以计算出来，而不需要通过计算第9项、第8项…直到第一项后再反推。外在旋转好比通项公式，每次旋转都是通过固定的参考系xyz旋转而来，与旋转过程中的abc状态无关。而内在旋转则需要根据上次旋转后转轴，在这个转轴的基础上再旋转，所以旋转轴是变动的，好比数列中的n项和n-1项的递推关系。关于内在旋转和外在旋转的关系，如果将其中一种旋转的第一次旋转和第三次旋转互换位置，那么他们就是等价的。上图为内在旋转。（联想数列公式的n项和n-1项关系）

A rotation represented by Euler angles (α, β, γ) =(−60°, 30°, 45°), using z-x’-z″intrinsic rotations

上图为外在旋转。（联想数列公式的通项公式）

The same rotation represented by (γ, β, α) = (45°, 30°, −60°), usingz-x-z extrinsic rotations

可以看到最终的坐标系姿态相同。

最后关于Tait–Bryan angles，由于是在三个参考坐标系xyz上的旋转，所以刚好可以利用这个性质用来导航，就形成了roll、pitch、yaw等概念。但是这是一种外在旋转，我们画图经常利用的是内在旋转（因为便于记忆，好画），所以就需要利用内在旋转和外在旋转的关系：互换第一次旋转和第三次旋转的位置。刚才已经说明。并且在一些参考文献（James, D.,Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors）中会涉及到一些欧拉角转换的图解实例，如果出现Tait–Bryan angle，比如顺序为1-2-3，但是在图解时使用的是内在旋转，此时真正的旋转顺序是3-2-1，即替换1和3的位置。特别注意！！！

30、