四旋翼飞行器的姿态解算小知识点.docx

1、 惯性测量单元 IMU(InertialMeasurement Unit)姿态航向参考系统 AHRS(Attitude and Headi ng Refere nee 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 Lan di ng)2、 常见的导航系统：惯性导航、天文导航、卫星导航、路标导航、无线电导航、推算导 航、组合导航。

3、 有两个基本坐标系：“地理”坐标系和“载体”坐标系地理”坐标系指的就是地球上的“东 北天(ENU) ”坐标系，而“载体”坐标系值的就是四轴自己的坐标系4、 在“地理”坐标系中，重力的值始终是(0, 0, 1g)，地磁的值始终是(0, 1, x)这 些值就是由放置在四轴上的传感器测量出来的5、 “地理”坐标系和“载体”坐标系是两个不同的坐标系，需要转化转化的方法就是坐标系 的转换，目前有三种方式：四元数(q0123)、欧拉角(yaw (Z轴)/ pitch (Y轴)/roll (X 轴)属于其中一种旋转顺序Z-Y-Xa航空次序欧拉角)、方向余弦矩阵(9个系数)6、 所谓的姿态，就是公式+系数比如：欧拉角公式和欧拉角的系数(翻滚、倾仰、偏航)7、 姿态的数据来源有5个：重力、地磁、陀螺仪、加速度计、电子罗盘其中前两个来 自“地理”坐标系，后三个来自“载体”坐标系 导航的基本原则就是保证两个基本坐标系的正确转化，没有误差只有实现了这个原则,载体才可以在自己的坐标系中完成一系列动作而被转换到地理坐标系中看起来是正确的 为了达到这个目标，需要对两个坐标系进行实时的标定和修正因为坐标系有三个轴，偏航 yaw修正由电子罗盘(基于载体)、地磁(基于地理)对比修正误差补偿得到。

倾仰pitch 和翻滚roll上的修正由加速度计(基于载体)、重力(基于地理)对比修正误差得到在完 成了基本原则的基础之后，即保证两个坐标系的正确转化后，利用基于载体上的陀螺仪进行 积分运算，得到基于载体坐标系的姿态数据，经过一系列PID控制，给出控制量，完成基 于载体坐标系上的稳定控制后，反应到地理坐标系上的稳定控制，从而达到我们观察到的定 高、偏航、翻滚、倾仰等动作对于上述论述可以看出，导航姿态从理论上讲只用陀螺仪是可以完成任务的但是由于陀螺 仪在积分过程中会产生误差累计，外加上白噪声、温度偏差等会造成导航姿态的解算随着时 间的流逝而逐渐增加所以就需要用加速度计在水平面对重力进行比对和补偿，用来修正陀 螺仪的垂直误差但是对于竖直轴上的旋转，加速度计是无能为力的，此时用的是电子罗盘 他也可以测量出水平面内的地磁方向用来修正陀螺仪的水平误差通过这两个器件的修正补 偿，使得陀螺仪更加稳定、可靠的工作9、 加速度计在地球上测量的是重力加速度，如果载体沿着z轴旋转，加速度计是无法感 知他的运动的；类似的，电子罗盘测量的是地球上的磁场方向，如果载体沿着y轴旋转，电 子罗盘同样也是无法感知他的运动的。

综上所述，加速度计和电子罗盘只能得到2维的角 度关系，通过某种方式的融合，可以得到正确的三维姿态信息10、 在这里要弄清楚一个问题，前面第8条所说的关于地理坐标系和载体坐标系之间的 互相转化这样就有两种转换方向：一是把B系（载体）转换到N系（地理）；二是把N 系转到B系当我们在实际控制当中，我们关心的显然是载体坐标系相对于地理坐标系之 间的变化，所以我们通常使用的旋转矩阵是把N系转到B系的矩阵（两者的关系是转置关 系）比如本次在利用加速度计计算姿态误差时，可以利用上一次的四元数姿态在N系中 的三个轴的垂直分量转换到B系中垂直分量来算误差vx 二 2*（qVq3 - q0^q2）;vy 二 2*（q0*ql + q2p引；vz 二 qO^qO - qVql - q2*q2 + q3*q3；式中的右边为N系到B系的 旋转矩阵的第三列元素（恰好是重力g在B系中的值）11、 在单位时间内的位移被定义为速度，速度有线速度和角速度之分，分别对应两种传 感器测量这两种不同的速度：线速度传感器（加速度计）、角速度传感器（陀螺仪）所以， 陀螺仪是用来测量角速度的，用于坐标系的旋转，也就是导航姿态了加速度计只能测量线 速度，最典型的例子就是重力加速度，如果加上水平坐标系上的加速度，形成合力F产生a。

考虑一个导弹，他的飞行速度由加速度计来测量而飞行过程中的转体姿态由陀螺仪来测量12、 当我们把加速度计拿在手上随意转动时，我们看的是重力加速度在三个轴上的分量 值，无法直观的观察到三个轴上的加速度分别是多少为了实现这样一个目的（可以看到每 个轴上的真实加速度），我们需要一个旋转矩阵，这个矩阵的作用就是把放置在载体坐标系 上的加速度计值转换到参考坐标系中，在参考坐标系中，三个轴上的值始终都是（0, 0, 1） 所以当我们把加速度计以任意角度固定在空间中时，无论加速度计的三个轴的值是多少，当 经过旋转矩阵变换后，在参考坐标戏中输出的值始终都是（0, 0, 1）--＞这表明在参考坐标 系中，物体在X和y轴上是没有加速度的，只有在z轴上存在重力加速度但是这里又存在 一个问题，既然z轴的输出是1,就是说存在加速度，物体应该运动起来才对但是这里物 体并没有运动为什么输出是1呢？这涉及到加速度计的设计问题：加速度计测量加速度 是通过比力来测量，而不是通过加速度通过想象一个盒子中的小球就就可以明白加速度 计只有在自由落体时，其输出为013、便于记忆的一个例子就是如何从青山到黄家湖对于一个人来讲,要从青山到黄家湖， 必须满足两个要求：1、你必须有张武汉地图，并且知道黄家湖的位置和青山的位置。

2、 你必须带有方向导航系统，实时更新你目前的朝向对应到飞行导航上面，黄家湖的位置对 应“地理”坐标系，青山的位置对应“载体”坐标系你的目的就是让这个两个坐标系被正确转 化和标定这部分工作交给加速度计和电子罗盘处理至于你具体是走过去，骑自行车去, 乘公交去还是做出租车过去，对应在飞行导航上面的话，利用的陀螺仪通过积分作用确定自 己的动态姿态在这里，K=控制周期/传感器采样周期15、 在复数域里面，二维坐标通过对复数的加减乘除运算可以快速方便地表达出来，尤 其是旋转现在考虑三维空间的复数向量的拉伸和旋转，或者更高维度那么就需要一个复 数域坐标系，容易想到的形式就是h=a+bi+cj,事实证明在二维复数域里面简单添加一元j 是无法构成三维复数空间的，实际上需要四个参数才能够构建三维复数空间（两个变量决定 轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例），即h=a+bi+cj+dk这就是 四元数的基本表达形式（其中i2=j2=k2=-1）即用四个变量来表达三维空间的位置坐标，这 就是复数域和实数域的不同但是，这样定义是有前提条件的，即牺牲了乘法的交换律例 如两个四元数hpMph如此一来，就出现了 Q8乘法矩阵表。

16、 对四元数更进一步分析，发现四元数可以写成一个实数加上一个三维向量的和，即 h=d+u （其中d为实数，u为三维向量）令p=w+v,则lip — [d + 11）（训—— dw—< U. v > +dv + wu + 口 X v Jph — (w — v)(d + ii) = wd—< u > -|-dv + wu + 灯 X tip其中，实数乘法和内积具有乘法交换律，但是三维向量的外积不同，有u x v = -vx u所 以，hp-ph就是两个向量外积的两倍如果两个向量部分外积为0,那么乘法运算就可交换 了17、 对于四元数的乘法pq,就是在四维空间F上一个线性变换，因此必有两个互相垂直 的二维不变子空间，分别是(1, 0, 0，0)和u张成的二维平面(这个平面在四维空间中， 我们无法看到全貌，只能看到与我们相交的一条直线，即u)和由u1和u2组成的二维平面(u1和u2是在u的三维空间中找到的三者两两垂直的符合右手定则的一组基这个平面我 们是可以看到的)所以四元数的乘法的几何意义就是在这两个二维不变子空间中做伸缩旋转的线性变换角度II训'伸缩因子为IIPII (从(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'其中)R=(cos(theta/2),alpha*sin(theta/2),beta*sin(theta/2),gama*sin(theta/2)),且 alpha2+ beta2+gama2=1这表示 在三维空间中将P向量绕着(alpha,beta, gama)轴逆时针旋转theta 角度，长度不变。

之所以为什么是theta/2，是因为在四维空间中实际上只转了 theta角度20、 关于高维空间的知识低维度事物无法感知在高维度发生的事情和动作比如我们 将一条纸袋旋转对折后首尾相连后在纸带的一面沿着直线一直画线，在二面平面上我们一直 以为我们走的是直线，但是在三维上我们却是在走圆，只不过首尾相接，二维无法感知，这 是在三维上干的事情并且低维度的实物只能观察到高维度的实物在低纬度上的投影图像， 比如扑克牌人看到的人体模型就是用一张纸纵切我们的人体，比如我们在现实生活中看到的 人的外貌其实是四维空间在三维空间上的投影而已二维空间上看到的直线有可能在三维空 间上是一个圆，所以三维空间上看到的直线有可能在思维空间上是一个圆所以我们在用四 元数表达三维空间的时候，看到的实际上是四维空间中的一个切线，我们看到的直线有可能 在四维空间是一个圆21、 球极投影对于从低维到高维的理解是比较好的一种方式比如地球的地图球极投影22、 将一个数乘以-1，相当于找到对应与原点的镜像相反数，再乘以-1后又回到了原来 的位置这样的一个1x-1的过程，相当于把数字转了 360度也就是说-1就意味着将数字 旋转180度。

现在定义一个数，只需要旋转90度，即出现在这里特别注意一下，我们 在横坐标上操作的是只具有一维长度的实数，这样定义会出现一个不在横坐标上的数，这样 需要扩展维度，如此一来，定义i为旋转90度。