基于单片机的云台姿态测量系统设计（二）

本文接上文基于单片机的云台姿态测量系统设计（一），主要对选用的姿态传感器进行介绍和简单的原理分析，涉及到MEMS加速度计和陀螺仪的原理、卡尔曼滤波的原理等。
基于单片机的云台姿态测量系统设计（一）
基于单片机的云台姿态测量系统设计（三）
基于单片机的云台姿态测量系统设计（四）

3.4传感器模块

本次设计需要测量三轴的加速度和角速度，选用的六自由度的姿态传感器MPU6050刚好可以满足需求。MPU6050之所以能够输出三轴的加速度和角度，是因为其内部集成了三轴的加速度计和陀螺仪。在静止情况下，加速度计可以根据重力加速度在各轴上的分量的三角函数关系得到三轴的角度值并输出；陀螺仪则可以根据转动时角度的变化率记录其角速度，通过积分得到角度值并输出。陀螺仪和加速度计也是本次设计中的重要元件，下面笔者将从建立模型入手，较为详细的向大家分别介绍加速度计和陀螺仪的原理。

3.4.1加速度计原理

加速度计正如其名，其功能是检测加速度。为了方便理解加速度计的原理，在此建立一个盒子模型。
假设一个正方体的小盒处于失重状态中，小盒中有一个小球静止于小盒的正中间，且不触碰小盒的六个面，在此状况下，六个面所受的压力都为零，如图。

小球在小盒中心 如果保持失重状态，突然打破静止状态，突然给小盒整体（不给小球）施加一个加速度为1G的力，小盒受力后将向左运动，与此同时小球将会打在小盒侧面上，使得X轴上输出-1G的数值，如图。

小盒向左移动 如此一来，上文已经建立了X轴的加速度计模型，并且对X轴的输出做出了数学分析。但是这还不够，笔者还要把单轴加速度计的模型稍作改动，期望得到一个多轴的输出，具体操作如下：假设笔者将小盒旋转四十五度并且不再处于失重环境下，小球会由于重力的作用向下掉落并最终接触Z-和X-两个面，如图。

小盒旋转45° 这样就近似获得了一个多轴的加速度计，其中X轴和Z轴受到的0.71G并不是一个任意的值，实际上是SQRT(1/2)的近似值，当球只受重力时根据受力分析，X轴和Z轴的值各近似为0.71G。 下面我们将引入向量模型，如图

向量模型 在向量模型中，小盒的六个面由垂直于六个面的三个轴来代替,加速度计测量的矢量由向量R表示，RX、RY、RZ分别表示向量R在X、Y、Z三轴上的分量。三者的关系式为：  将R=1代入也能验证上文所说的0.71G并不是任意的值，而是近似值。 经过小盒模型和向量模型的建立，上述分析的原理已经十分接近现实中的加速度计了，但是通过上述分析得到的输出与实际想要的输出单位不一致，还需要进行一次单位换算。加速度计的灵敏度通常以LSB /G为单位。我们可以用公式（3.2）进行单位换算，最终得到单位为G的输出：  其中,k为灵敏度，也就是说当X轴的计数为ADCRX时，在2g的量程下，对应的加速度值就是(ADCRX/16384)G。 让我们回到向量模型中，并引入相关的角度，如图。

引入角度的向量模型 我们引入向量R和X、Y、Z轴之间的角度，并将它们定义为 Axr、Ayr、Azr 。 可以看到由R和 RX组成的直角三角形：

我们发现当使用 反余弦：

让我们暂时抛开理论模型将传感器水平放置，正常情况下只有Z轴受力且仅受到重力，在2g的量程下传感器将在Z轴输出1G，模拟量和数字量转换后输出结果为16384 。

假设在理想情况下，将传感器设置为2g量程并水平放置，传感器由于仅受重力影响将会在三轴分别感受到001，Z轴输出16384。但是在真实情况下很可能并不是这样，由于芯片在制作加工过程中存在差异，现实情况下由于误差输出结果并不相同，因此需要对结果进行补偿，补偿的方法用数学公式表示为：

其中，ADCx为X轴的传感器输出，Gx为X轴的真实的加速度值，Offset为X轴的加速度为0G时传感器的输出，K为标度因数，标度因数可通过多次测量求得。
模型已经建立清楚，实际上加速度计是由MEMS传感器和信号处理芯片组成的。电学原理图和力学结构示意图如图。加速度计由上电容板、可移动的中电容板和下电容板组成，其中中电容板受到一定大小的力可以上下移动。当加速度达到一定数值时，中电容板会上下移动使得其与上下极板的距离相应的增大或减小如式（3.10）和（3.11），Vm的电容大小也相应的增大减小。

加速度计电学模型  加速度计力学模型 输出电压与电容电压变化部分的关系：

其中为输出电压，为输入电压，为上电容板电容，为下电容板电容。
电容的大小和位移变化的关系：

其中ε为极板间介质的介电常数，d为极板间距离，x为中电容板的位移。联立可得

根据胡克定律和牛顿第二定律得：

其中k为弹性系数，m为质量块的质量，a为加速度。
则加速度与电压的关系是：

通过对电压如上处理就可以获得加速度的数字输出。

3.4.2陀螺仪原理

陀螺仪是测量角速度的传感器，MPU6050带有三个陀螺仪，并将同时检测X、Y、Z三轴的旋转。为了方便理解上述介绍，对向量模型进行补充，如图。

在图3.10中，我们定义：
rxz是R向量在XZ平面上的投影；
ryz - 是R向量在XY平面上的投影；
rxz和rz所形成的直角三角形，利用勾股定理，我们得到：

角速度的物理意义实际上是角度的变化率，为了方便理解，我们把在t0时刻角Axz（围绕Y轴的旋转角）定义为Axz0；相对应的在t1时刻为Axz1。角度变化率的计算公式如下：

如果单位是度，并以秒为时间单位，那么将以deg/s表示。
我们通过读取陀螺仪接口输出的模拟量与数字量转换后的值不是以我们想要的度每秒为单位的，所以需要对结果进行单位转换。
单位转换公式如下：

k为灵敏度，假设陀螺仪的量程为±1000º/s，x轴输出的ADC值为200,在±1000º/s下的灵敏度为32.8LSB/(º/s)。根据式（3.17）可得：即陀螺仪检测的目标正在以大约6度每秒的速度绕X轴旋转。陀螺仪输出的ADC值并只有正数，如果输出为负数，就表示检测目标绕着正方向的反方向旋转。
陀螺仪的工作原理基于科里奥利效应所产生的科里奥利力，它的结构类似于加速度计，都是上中下三个电容板，并且中电容板可动。科里奥利力可以把角速度转换成位移，并带动中电容板使得上下电容发生变化，感测部分通过测量上下电容的变化量通过公式来确定科里奥利力产生的位移。位移大小与角速率大小成正比，以此可以获得角速度和角加速度量。

3.4.3MPU6050的缺陷分析

（1）数据校准
MPU6050输出的数据不仅具有零点漂移而且还带有噪声，需要将数据校准后再滤波才能得到较为准确的输出。
（2）数据偏移处理
传感器制作完成后都有一定的误差，为了得到较为精确的输出，我们需要将误差进行校准。本次设计中我们需要校准的器件由陀螺仪和加速度计，其中陀螺仪的误差相对好校准一些，我们可以通过多次测量求平均值的方法获得陀螺仪的误差；而加速度计的误差校准就不像陀螺仪那样方便，需要单独校准。
（3）Z轴累计误差
本次设计选用的MPU6050没有拓展磁力计，无法通过磁场数据校正Z轴读数，Z轴输出的结果是通过数学方法积分得到的结果，计算出X、Y轴的姿态角可以使用通过常规方法，因此Z轴的读数存在长时间积分带来的累积误差，无法通过滤波消除，所以Z轴输出的姿态角只在短时间内可靠。
前两个问题皆可通过阅读厂商说明书，在上位机程序上进行校正；后一个问题在需要高精度的姿态数据的情况下，可以通过选用有磁力计的9轴MPU6050来解决。