一种适用于小型无人机的加速度计与陀螺仪的矫正方法

本人研究飞控也有些年月了，一直想写点什么东西，但是一直没有时间，今天，就试着写写自己在飞控的研发过程中所弄明白的一些问题，如果后续效果好，我会一一把飞控研发中的重点和难点写出来。这样有两个好处，一是方便广大初学者和网友的查看，二是也方便自己的回忆与查看，俗话说好记性不如烂笔头！好，废话不多说，开始进入正题。

一、陀螺仪和加速度计

陀螺仪为惯性测量单元的元件之一，也是整个无人机系统的重要元件。陀螺仪具有定轴性（不改变方向）和进动性的功能，陀螺仪主要用来测量运载体的角运动中的角速度量。对于不同的飞机系统，有时也将测量出的角速度用于控制或者反馈中，增加阻尼。角速度主要是在惯性下测量得到的，所以需要服从牛顿定理。通过对角速度的积分，便可得到运动的角度，从而解算出运载体动态运动的实时姿态。在大型飞机上的陀螺仪指的是转子陀螺仪，随着科学技术中的激光技术的发展，又有光学陀螺仪的产生，这类陀螺仪不在符合经典物理定律，而是服从量子力学的理论，所以含义更加广泛。本文所选取的陀螺仪是得益于科学技术的发展的另一种产物，微机械电子陀螺仪，它的大小可以做到很小，比一个指甲盖的大小还小，现在广泛运用于智能手机，无人机，运动手表，这种微型陀螺仪也为本文的无人机设计提供了方便。

加速度计，顾名思义，就是测量加速度计的一种传感器，能够制作出加速度计的传感器很多，最为简单的一种就是通过电阻来进行加速度计的制作，当然，我们现在的重点不是在加速度计的设计和研究上，具体怎么实现的，可以去参考一些传感器的书了解，本文所讲述的加速度计主要是MEMS（微机械电子器件）器件，大小往往就是小拇指大小。由于是MEMS器件，所以通常都会有噪声，输出也不准。

加速度计也为惯性测量单元之一，加速度计既可以测量物体的运动加速度，同时也可以用来矫正陀螺仪的误差。选择加速度计，主要是选择加速度计的精度和性能，两者会影响无人机的控制性能和导航精度。加速度是牛顿力学中的物理量，当然符合经典的牛顿定理，因此也可适用牛顿坐标系。

加速度计最为简单的模型就是弹簧模型，现在所使用的加速度计也是根据这个原理设计的，但是由于科学技术的发展，加速度可以做得十分小巧，还可以和陀螺仪合并，做在一块芯片内。在实际使用中，需要注意的是，加速度不仅包含载体运动的加速度，也包括地球的重力加速度，所以也称为比力计，在进行导航设计时要注意这一点。

本文使用的由InvenSense公司生产的MPU6050芯片便是一个六轴芯片，包含一个三轴陀螺仪和一个三轴的加速度，大大减少了封装面积，该芯片通过IIC输出加速度和角速度信号，该信号是已经通过16位ADC转换的数字信号。该芯片输出的陀螺仪的量程为：±250, ±500, ±1000, ±2000°/sec (dps) ，根据不同的使用场合可以使用不同的测量范围，当然精度也不相同，陀螺仪的精度可以由下面的公式计算：

其中precision表示测量的精度，range表示测量的范围，不难看出量程越低，精度越高。

该芯片输出的加速度的量程为±2g, ±4g, ±8g, ±16g，当然可以根据使用要求进行编程测量，同理，该芯片的测量精度可以通过上公式解出。

该芯片支持高速IIC通信，通信速率达到400Khz，芯片的封装采用很小的4x4mm 的QFN封装，引脚为24脚，工作温度为-40-85度。电源供电电压宽广，范围为2.375V-3.46V，在不使用内部滤波的情况下，输出速度可以达到8Khz，在使用内部滤波的情况下加速度和陀螺仪也可以输出1Khz的输出。该芯片也属于低功耗芯片陀螺仪工作消耗3.6mA，加速度计消耗电流500μA。

二、传感器校准

姿态解算和惯性导航的重要传感器为加速度计、陀螺仪和磁强计，这三种传感器中，陀螺仪主要是用于姿态解算，加速度计和磁强计也会在姿态解算中使用，但是在惯性导航中，主要是靠加速度计和磁强计。

由于采用的是MEMS传感器，所以会有温度漂移，比例缩放和安装误差。这些造成了传感器的读数不准确，无法使用。为了能够使传感器的读数正常，可以用于解算和控制中，需要对其进行校正处理。

本文先叙述加速度计校准，其它传感器校准和加速度计校准大同小异。

加速度计校正的方法较多，可以使用高精度实验台进行校正，但是实验台设备昂贵，需要摆放在实验室进行使用，并不是最优方案。一般也可以采用上电校准，但是上电直接校准显得不够精确，有一定误差。本文采取最小二乘法进行加速度计的校准，使用时只需要采集六个面的数据即可，有足够高的精度，能够基本满足校准的要求。

首先给出校准模型

式中 、 、 为校准后的值， 为校准比例缩放， 为加速度计三轴测量值，bx、by、bz 为加速度计零偏校正值。

而

公式中g 是重力加速度。

现在要求校准比例缩放和零偏校正值，只需将式（4-24）和式（4-25）联立，采用高斯牛顿法便可求出参数。由于无论怎么放置飞行控制板，重力的大小始终为常数，可以采用六个面的数据对其进行最小二乘拟合，近似逼近参数。

其中，。

现在只需要求令S最小的六个参数，编写程序，采集飞行控制板上六面数据，利用高斯牛顿方法，求解出六个参数，本文所得的六个参数如下表所示：

 

                                             表    实际校正的参数

kx	1.00171232
ky	1.00240743
kz	0.98347652
bx	0.6521215748
by	-0.0468268767
bz	-1.0974468

 

根据上表参数，代入原公式中，可得校准后加速度值，将原加速度值和校准后加速度值比较，可明显发现加速度计的测量更为准确。

 

图4  校正后加速度值（左）与原值（右）比较

 

左图为校正后的值，右图为校正前的加速度值，其中最上面一条线为Z轴测量值，表示地球的重力加速度，从图中可以清晰看到，未经校正的加速度值离重力加速度有一定偏差，大约为9.4 ，而校正后的重力加速度则为9.8  。在X轴和Y轴的测量值，也可以看到X轴和Y轴不经过校正的值和经过校正的值有很大出入，未经校正的值偏离大约有0.5 ，校正后的值几乎收敛于0，可以说完全达到了精度。

同理，利用该种方法也可校正磁强计，方法和加速度计同样，因为磁场和重力一样，都为矢量。校正陀螺仪时，只采用零飘校正即可，思想为上电多次采集读数，取得平均零飘量，每次采集数据时，减去该零飘量即可。

陀螺仪的校准就颇为简单，采用和磁强计相同的校准手段，且在上电时自动执行程序，不需要人为转动，因此读到的校正数据为零偏校正数据。

三、具体实现方法

在实际应用中，我们仅知道大概是基于什么原理实现的，很显然是不够的，不能让我们写出可以实现的源码，也不能让我们读懂他人的源码，所以，我们需要一个深入的理解，既要知其然，也要知其所以然。