名词解释：
惯性测量单元 IMU (InertialMeasurementUnit)
姿态航向参考系统 AHRS (Attitudeand Heading Reference System)
地磁角速度重力 MARG (Magnetic,Angular Rate, and Gravity)
微机电系统 MEMS (MicroElectrical Mechanical Systems)
自由度维数 DOF (Dimension OfFreedom)
无人驾驶飞行器 UAV (UnmannedAerial Vehicle)
互补滤波算法 ECF （explicit complement filter）
扩展卡尔曼滤波 EKF (Extended Kalman Filter)
无损卡尔曼滤波 UKF (Unscented Kalman Filter)
梯度下降算法 GD （gradient descent）
惯性导航系统 INS (InertialNavigation System)
全球导航卫星系统 GNSS (GlobalNavigation Satellite System)
天文导航系统 CNS (CelestialNavigation System)
可垂直起降 VTOL (VerticalTake-off and Landing)
坐标系介绍：
有两个基本坐标系：“地理”坐标系（Earth Frame）和“载体”坐标系(Body Frame)。”地理”坐标系指的就是地球上的“东北天（ENU）”坐标系，而“载体”坐标系值的就是四轴自己的坐标系。当我们在实际控制当中，我们关心的显然是载体坐标系相对于地理坐标系之间的变化，所以我们通常使用的旋转矩阵是把“地理”坐标系转到“载体”坐标系的矩阵，两者之间的转化的方法就是坐标系的转换，目前有三种方式：四元数、欧拉角、方向余弦矩阵。其中使用四元数运算比较快，但是它没有实际的物理含义，纯数学推导。
姿态数据：
姿态的数据来源有5个：重力、地磁、陀螺仪、加速度计、电子罗盘。其中前两个来自“地理”坐标系，后三个来自“载体”坐标系。在“地理”坐标系中，重力的值始终是（0，0，1g），地磁的值始终是（0，1，x），这里y指向正北方。这些值就是由放置在四轴上的传感器测量出来的。在单位时间内的位移被定义为速度，速度有线速度和角速度之分，分别对应两种传感器测量这两种不同的速度：线速度传感器（加速度计）、角速度传感器（陀螺仪）。
传感器介绍：
（1）陀螺仪：
陀螺仪，是一种用来感测与维持方向的装置，基於角动量不灭的理论设计出来的。陀螺仪主要是由一个位於轴心可以旋转的轮子构成。 陀螺仪一旦开始旋转，由於轮子的角动量，陀螺仪有抗拒方向改变的趋向。万万想不到的是鸡头就是一个非常完美的陀螺仪，无论你怎样动鸡的身体，它总是能hold住自己的头，太强大了!
以MPU6050为例，MPU6050芯片的座标系是这样定义的：令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为X轴，竖直向上的为Y轴，指向自己的为Z轴。见下图：
绕X、Y和Z三个座标轴旋转的角速度分量GYR_X、GYR_Y和GYR_Z均为16位有符号整数。从原点向旋转轴方向看去，取正值时为顺时针旋转，取负值时为逆时针旋转。三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X为例，若倍率设定为250度/秒，则意味着GYR取正最大值32768时，当前角速度为顺时针250度/秒；若设定为500度/秒，取32768时表示当前角速度为顺时针500度/秒。显然，倍率越低精度越好，倍率越高表示的范围越大。我们用f表示倍率，f=0为250度/秒，f=3为2000度/秒。
以GYR_X为例，若当前设定的角速度倍率为1000度/秒，那么将GRY_X读数换算为角速度（顺时针）的公式为：g_x=1000 x GYR_X / 32768。
（2）加速度计：
当我们在想象一个加速度计的时候我们可以把它想作一个圆球在一个方盒子中。假定这个盒子不在重力场中或者其他任何会影响球的位置的场中，球处于盒子的正中央。你可以想象盒子在外太空中，远离任何天体，如果很难想象，那就当做盒子在航天飞机中，一切东西都处于无重力状态。在下面的图中你可以看到我们给每个轴分配了一对墙（我们移除了Y+以此来观察里面的情况）。设想每面墙都能感测压力。
如果我们突然把盒子向左移动（加速度为1g=9.8m/s^2），那么球会撞上X-墙。然后我们检测球撞击墙面产生的压力，X轴输出值为-1g。
请注意加速度计检测到得力的方向与它本身加速度的方向是相反的。这种力量通常被称为惯性力或假想力 。在这个模型中你你应该学到加速度计是通过间接测量力对一个墙面的作用来加速度的，在实际应用中，可能通过弹簧等装置来测量力。这个力可以是加速度引起的，但在下面的例子中，我们会发现它不一定是加速度引起的。如果我们把模型放在地球上，球会落在Z-墙面上并对其施加一个1g的力，见下图：
新模型中每个轴都分别垂直于原模型中各自的墙面。矢量R是加速度计所检测的矢量（它可能是重力或上面例子中惯性力的合成）。Rx，Ry，Rx是矢量R在X，Y，Z上的投影。请注意下列关系：
R^2 = Rx^2 + Ry^2 + Rx^2 （三维空间勾股定理）
我们感兴趣的角度是向量R和X，Y，Z轴之间的夹角，那就令这些角度为Axr，Ayr，Azr。观察由R和Rx组成的直角三角形：
cos(Axr) = Rx / R , cos(Ayr) = Ry / R， cos(Azr) = Rz / R
从公式1我们可以推导出 R = Sqrt( Rx^2 + Ry^2 + Rz^2)，通过arccos()函数我们可以计算出所需的角度：
Axr = arccos(Rx/R)，Ayr = arccos(Ry/R)，Azr = arccos(Rz/R)
注：加速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知这种水平旋转。
（3）地磁计：
如图所示，地球的磁场象一个条形磁体一样由磁南极指向磁北极。在磁极点处磁场和当地的水平面垂直，在赤道磁场和当地的水平面平行，所以在北半球磁场方向倾斜指向地面。用来衡量磁感应强度大小的单位是Tesla或者Gauss（1Tesla=10000Gauss）。随着地理位置的不同，通常地磁场的强度是0.4-0.6 Gauss。需要注意的是，磁北极和地理上的北极并不重合，通常他们之间有11度左右的夹角。
地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面平行的分量mx、my和一个与当地水平面垂直的分量mz，这里的mx指向正北方。如果保持电子罗盘和当地的水平面平行，那么罗盘中磁力计的三个轴就和这三个分量对应起来，如图所示：
实际上对水平方向的两个分量来说，他们的矢量和总是指向磁北的。罗盘中的偏航角（Yaw）就是当前方向和磁北的夹角。由于罗盘保持水平，只需要用磁力计水平方向两轴（通常为X轴和Y轴）的检测数据就可以用式1计算出航向角。当罗盘水平旋转的时候，航向角在0?- 360?之间变化。同加速度计一样，我们也可以根据地磁计输出的三个分量，求出地磁T与三个轴的夹角。
注：同加速度计类似的，若是沿着磁场方向的轴旋转，测量值不会改变，地磁计无法感知这种旋转。
（4）GPS：
空间星座部分的各颗GPS卫星向地面发射信号，然后用户设备通过接收、测量各颗可见卫星信号，并从信号中获取卫星的运行轨道信息，进而确定用户接收机自身的空间位置。空间星座部分与用户设备的这种联系是单向的，信号、信息只从空间星座部分向用户设备部分传递。卫星信号从20200Km的高空被播发后，大约需要70ms的时间就到达地球表面。卫星信号中包含着信号发射时间的精确信息，这是用户设备用来测量其本身到卫星距离的一个必要条件。
空间星座部分为GPS提供了定位基础，并且可以支持无数个GPS用户。然而，它们不会替用户自己定位，用户只有通过GPS用户设备才能实现定位。用户设备可以简单地理解为GPS接收机，它主要由接收机硬件、数据处理软件、微处理器和终端设备组成。用户设备的主要任务是跟踪可见GPS卫星，对接收到的卫星无线电信号经过数据处理后获得定位所需的测量值和导航信息，最后完成对用户的定位运算和可能的导航任务。
通过天线接收所有可见GPS卫星的信号后，接收机对这些信号进行数据处理而精确地测量出各个卫星信号的发射时间，接着将其自备时钟所显示的信号接收时间与测量所得的信号发射时间相减后再乘以光速，由此得到接收机与卫星之间的距离。同时，接收机还从卫星信号中解译处卫星的运行轨道参数，并以此准确地计算出卫星的空间位置。如果卫星1、2、3的空间位置在某一直角坐标系中的坐标为：
其中，未知数(x,y,z)正是我们想要的用户接收机位置。从方程组中解出三个未知数(x,y,z)，便得到了用户自身的三维坐标。因为接收机时钟通常与卫星时钟不同步，所以接收机需要有4颗卫星的测量值，然后4个方程一并求解出x,y,z和接收机钟差(t)这4个未知数，这便是GPS定位、定时的基本原理。
GPS数据遵循NMEA-0183协议，该数据标准是由NMEA（National Marine Electronics Association，美国国家海事电子协会）于1983年制定的。统一标准格式NMEA-0183输出采用ASCII 码，其串行通信的参数为：波特率＝4800bps，数据位＝8bit，开始位=1bit，停止位＝1bit，无奇偶校验。数据传输以“语句”的方式进行，每个语句均以“$”开头，然后是两个字母的“识别符”和三个字母的“语句名”，接着就是以逗号分割的数据体，语句末尾为校验和，整条语句以回车换行符结束。
NMEA-0183的数据信息有十几种，这些信息的作用分别是：$GPGGA：输出GPS的定位信息；$GPGLL：输出大地坐标信息；$GPZDA：输出UTC时间信息；$GPGSV：输出可见的卫星信息；$GPGST：输出定位标准差信息；$GPGSA：输出卫星DOP值信息；$GPALM：输出卫星星历信息；$GPRMC：输出GPS推荐的最短数据信息等。常用的协议类型解释如下：
1）Global Positioning System Fix Data（GGA）GPS 定位信息
$GPGGA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,M,<10>,M,<11>,<12>*hh
<1> UTC 时间，hhmmss（时分秒）格式
<2> 纬度ddmm.mmmm（度分）格式（前面的0 也将被传输）
<3> 纬度半球N（北半球）或S（南半球）
<4> 经度dddmm.mmmm（度分）格式（前面的0 也将被传输）
<5> 经度半球E（东经）或W（西经）
<6> GPS 状态：0=未定位，1=非差分定位，2=差分定位，6=正在估算
<7> 正在使用解算位置的卫星数量（00~12）（前面的0 也将被传输）
<8> HDOP 水平精度因子（0.5~99.9）
<9> 海拔高度（-9999.9~99999.9）
<10> 地球椭球面相对大地水准面的高度
<11> 差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空
<12> 差分站ID 号0000~1023（前面的0 也将被传输，如果不是差分定位将为空）
2）GPS DOP and Active Satellites（GSA）当前卫星信息
$GPGSA,<1>,<2>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<4>,<5>,<6>*hh
<1> 模式，M=手动，A=自动
<2> 定位类型，1=没有定位，2=2D 定位，3=3D 定位
<3> PRN 码（伪随机噪声码），正在用于解算位置的卫星号（01~32，前面的0 也将被传输）。
<4> PDOP 位置精度因子（0.5~99.9）
<5> HDOP 水平精度因子（0.5~99.9）
<6> VDOP 垂直精度因子（0.5~99.9）
3）GPS Satellites in View（GSV）可见卫星信息
$GPGSV,<1>,<2>,<3>,<4>,<5>,<6>,<7>,…<4>,<5>,<6>,<7>*hh
<1> GSV 语句的总数
<2> 本句GSV 的编号
<3> 可见卫星的总数（00~12，前面的0 也将被传输）
<4> PRN 码（伪随机噪声码）（01~32，前面的0 也将被传输）
<5> 卫星仰角（00~90 度，前面的0 也将被传输）
<6> 卫星方位角（000~359 度，前面的0 也将被传输）
<7> 信噪比（00~99dB，没有跟踪到卫星时为空，前面的0 也将被传输）
注：<4>,<5>,<6>,<7>信息将按照每颗卫星进行循环显示，每条GSV 语句最多可以显示4 颗卫星的信息。其他卫星信息将在下一序列的NMEA0183 语句中输出。
4）Recommended Minimum Specific GPS/TRANSIT Data（RMC）推荐定位信息
$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*hh
<1> UTC 时间，hhmmss（时分秒）格式
<2> 定位状态，A=有效定位，V=无效定位
<3> 纬度ddmm.mmmm（度分）格式（前面的0 也将被传输）
<4> 纬度半球N（北半球）或S（南半球）
<5> 经度dddmm.mmmm（度分）格式（前面的0 也将被传输）
<6> 经度半球E（东经）或W（西经）
<7> 地面速率（000.0~999.9 节，前面的0 也将被传输）
<8> 地面航向（000.0~359.9 度，以真北为参考基准，前面的0 也将被传输）
<9> UTC 日期，ddmmyy（日月年）格式
<10> 磁偏角（000.0~180.0 度，前面的0 也将被传输）
<11> 磁偏角方向，E（东）或W（西）
<12> 模式指示（仅NMEA0183 3.00 版本输出，A=自主定位，D=差分，E=估算，N=数据无效）
5）Track Made Good and Ground Speed（VTG）地面速度信息
$GPVTG,<1>,T,<2>,M,<3>,N,<4>,K,<5>*hh
<1> 以真北为参考基准的地面航向（000~359 度，前面的0 也将被传输）
<2> 以磁北为参考基准的地面航向（000~359 度，前面的0 也将被传输）
<3> 地面速率（000.0~999.9 节，前面的0 也将被传输）
<4> 地面速率（0000.0~1851.8 公里/小时，前面的0 也将被传输）
<5> 模式指示（仅NMEA0183 3.00 版本输出，A=自主定位，D=差分，E=估算，N=数据无效）
6）Geographic Position（GLL）定位地理信息
$GPGLL,<1>,<2>,<3>,<4>,<5>,<6>,<7>*hh
<1> 纬度ddmm.mmmm（度分）格式（前面的0 也将被传输）
<2> 纬度半球N（北半球）或S（南半球）
<3> 经度dddmm.mmmm（度分）格式（前面的0 也将被传输）
<4> 经度半球E（东经）或W（西经）
<5> UTC 时间，hhmmss（时分秒）格式
<6> 定位状态，A=有效定位，V=无效定位
<7> 模式指示（仅NMEA0183 3.00 版本输出，A=自主定位，D=差分，E=估算，N=数
据无效）
数据融合：
（1）为什么还需要加速度计和地磁计呢？
对于上述论述可以看出，导航姿态从理论上讲只用陀螺仪是可以完成任务的，对陀螺仪输出的三个角速度进行积分就能得到飞行器的姿态数据。那为什么还需要加速度计和地磁计呢？主要是由于陀螺仪在积分过程中会产生误差累计，加上白噪声、温度偏差等会造成导航姿态的解算随着时间的流逝而逐渐增加。 所以就需要用加速度计在水平面对重力进行比对和补偿，用来修正陀螺仪的误差。但是对于竖直轴上的旋转，加速度计是无能为力的，此时用的是电子罗盘。也可以测量出水平面内的地磁方向用来修正陀螺仪的水平误差。通过这两个器件的修正补偿，使得陀螺仪更加稳定、可靠的工作。
（2）为什么还需要陀螺仪呢？
加速度计在地球上测量的是重力加速度，如果载体沿着Z轴旋转，加速度计是无法感知他的运动的；类似的，电子罗盘测量的是地球上的磁场方向，如果载体沿着Y轴旋转，电子罗盘同样也是无法感知他的运动的。综上所述，加速度计和电子罗盘只能得到2维的角度关系，通过某种方式的融合，可以得到正确的三维姿态信息。到目前为止，你可能会问自己，如果加速度计和磁力计在任何时间点能给出DCM矩阵，为什么我们需要陀螺仪？ 陀螺仪实际上是一个比加速度计和磁力计更精确的设备，如果直接从加速度计和磁力计估计读数，其在形式上包含大量的噪声，包括外部（非重力）惯性力（即加速度）或不是由地球磁场引起的磁力。
（3）三个传感器融合
综合考虑，加速度计和磁传感器都是极易受外部干扰的传感器，都只能得到2维的角度关系，但是测量值随时间的变化相对较小，结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。
现在有了三个传感器，都能在一定程度上测量角度关系，但是究竟相信谁？根据刚才的分析，应该是在短时间内更加相信陀螺仪，隔三差五的问问加速度计和磁传感器，角度飘了多少了？有一点必须非常明确，陀螺仪才是主角，加速度计和磁传感器仅仅是跑龙套的。其实加速度计无法对航向角进行修正，修正航向角需要磁力计。 数据融合原理参考这一节。
（4）导航
导航的基本原则就是保证两个基本坐标系的正确转化，没有误差。只有实现了这个原则，载体才可以在自己的坐标系中完成一系列动作而被转换到地理坐标系中看起来是正确的。为了达到这个目标，需要对两个坐标系进行实时的标定和修正。因为坐标系有三个轴，偏航yaw的修正由电子罗盘（基于载体）、地磁（基于地理）对比修正误差补偿得到。俯仰pitch和横滚roll上的修正由加速度计（基于载体）、重力（基于地理）对比修正误差得到。在完成了基本原则的基础之后，即保证两个坐标系的正确转化后，利用基于载体上的陀螺仪进行积分运算，得到基于载体坐标系的姿态数据，经过一系列PID控制，给出控制量，完成基于载体坐标系上的稳定控制后，反应到地理坐标系上的稳定控制，从而达到我们观察到的定高、偏航、翻滚、倾仰等动作。
AHRS和IMU的差异：
AHRS（Attitudeand Heading Reference System）为姿态航向参考系统，它由加速度计、磁场计、陀螺仪构成，AHRS的真正参考来自于地球的重力场和地球的磁场，它的静态精度取决于对磁场的测量精度和对重力的测量精度，而陀螺仪决定了他的动态性能。在这种前提下，说明AHRS离开了地球这种有重力和磁场环境的时候是没法正常工作的。而且特别注意，磁场和重力场越正交，航姿测量效果越好；也就是说如果磁场和重力场平行了，比如在地磁南北极。这里的磁场是向下的，即和重量场方向相同了。这个时候航线交是没法测出的，这是航姿系统的缺陷所在；在高纬度的地方航线角误差会越来越大。
IMU（Inertial measurement unit）学名惯性测量单元，大学的理论力学告诉我们，所有的运动都可以分解为一个直线运动和一个旋转运动，故这个惯性测量单元就是测量这两种运动，直线运动通过加速度计可以测量，旋转运动则通过陀螺仪。假设IMU的陀螺仪和加速度计的测量是没有任何误差的，那么通过陀螺仪则可以精确的测量物体的姿态。通过加速度计可以二次积分得出位移，实现完整的6DOF（自由度维数），也就是说你带着一台这种理论型的IMU在宇宙任何位置运动。我们都可以知道它当前的姿态和相对位移，这将不局限于任何场。
从上面的描述何以看出。实际上AHRS比IMU还多一个磁场传感器，而为什么AHRS的级别却低于IMU而需要依赖于重力场和磁场呢？这是由传感器器件架构所决定的。AHRS的传感器通常是成本低廉的mems传感器。这种传感器的陀螺仪和加速度计的噪声相对来说很大。以平面陀螺仪为例：用ADI的陀螺仪进行积分一分钟会漂移2度左右，这种前提下如果没有磁场和重力场来修正三轴陀螺仪的话。那么基本上3分钟以后物体的实际姿态和测量输出姿态就完全变样了，所以在这种低价陀螺仪和加速度计的架构下必须运用场向量来进行修正，而IMU实际上也是这样的。因为我们知道没有绝对精确的传感器，只有相对精确的传感器，IMU的陀螺仪用的是光纤陀螺或者机械陀螺，这种陀螺的成本很高。精度相对mems陀螺也很高，精度高不代表准确，IMU的姿态精度参数通常是一小时飘多少度。
而用加速度计积分做位置的话。AHRS是不现实的（1分钟就能飘出几十米，而且是成二次方的速度递增）。AHRS通常要结合GPS和气压计做位置，IMU积分做位置的是一天多少海里。这样的一个参数数量级。也许在海上还能用的到，这就是AHRS和IMU在我的理解里的一个差异。
京公网安备 11010802033920号
