介绍加速度计和陀螺仪的数学模型和基本算法讲解人：李伟1.概述2.加速度计3.陀螺仪4.加速度计和陀螺仪两者融合1.概述本文向大家介绍惯性MEMS（微机电系统）传感器，特别是加速度计和陀螺仪以及其他整合IMU（惯性测量单元）设备。加速度计检测什么？陀螺仪检测什么？如何将传感器ADC读取的数据转换为物理单位？如何结合加速度传感器和陀螺仪的数据以得到设备和地平面之间的倾角的准确信息？本文的数学运算尽量减少到最少，大家只需知道一些数学三角函数的基本知识，就能理解本文的中心思想。总有一些人认为IMU需要复杂的数学运算（复杂的FIR或IIR滤波，如卡尔曼滤波，Parks-McClellan滤波等），其实一些很简单的数学也能解决问题，越简单越可靠，另外一些嵌入式设备并不具备能力去解决复杂的矩阵运算。本文利用AccGyro+gyroimu作为例子。加速度计上面fig1.1是一个加速度计的数学模型，相当于一个六面体的盒子，图示的小球可以想象成模型处于无重力状态，（为了观察方便，暂时移出Y+，）这时，突然把盒子向左移动，可以看到球会撞上X-墙。然后我们检测球撞击墙面产生的压力，X轴输出值为-1g。请注意加速度计检测到的力的方向和它本身的加速度的方向是反的，虽然这个模型并非一个MEMS传感器的真实构造，但它用来解决与加速度计相关的问题相当有效。Fig1.1Fig1.2到目前为止，我们已经分析了单轴的加速度计输出，这是使用单轴加速度计所能得到的，三轴加速度计的真正价值在于它们能够检测全部三个轴的惯性力。让我们回到盒子模型，并将盒子向右旋转45度。现在球会与两个面接触：Z-和X-，见下图：Fig1.3在上一个模型中我们引入了重力并旋转了盒子。在最后的两个例子中我们分析了盒子在两种情况下的输出值，力矢量保持不变。虽然这有助于理解加速度计是怎么和外部力相互作用的，但如果我们将坐标系换为加速度的三个轴并想象矢量力在周围旋转，这会更方便计算。见下图：Fig1.4矢量R是加速度计所检测的矢量（它可能是重力或上面例子中惯性力的合成）。RX，RY，RZ是矢量R在X，Y，Z上的投影。请注意下列关系：R2=RX2+RY2+RZ2（公式1）此公式等价于三维空间的勾股定理。经过一大段的理论序言后，我们和实际的加速度计很靠近了。RX，RY，RZ值是实际中加速度计输出的线性相关值，你可以用它们进行各种计算。在我们运用它之前我们先讨论一点获取加速度计数据的方法。1.数字加速度计：通过I2C，SPI或USART方式获取信息2.模拟加速度计：输出是一个在预定范围内的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。有些MCU具有内置ADC模块，而有些则需要外部电路进行ADC转换。例如一个10位ADC模块的输出值范围在0.1023间，请注意，1023=210-1。接下来，通过具体例子来阐述ADC转换。假设我们从10位ADC模块得到了以下的三个轴的数据，为了简单起见，只写了x轴的，AdcRx=586，每个ADC模块都有一个参考电压，假设在我们的例子中，它是3.3V。要将一个10位的ADC值转成电压值，我们使用下列公式：VoltsRx=AdcRx*VREF/1023所以，可得VoltsRx=586*3.3/1023=1.89V（结果取两位小数）每个加速度计都有一个零加速度的电压值，这个电压值对应于加速度为0g，通过计算相对0g电压的偏移量我们可以得到一个有符号的电压值。比如说，0g电压值VzeroG=1.65V。DeltaVoltsRx=1.89V-1.65V=0.24V我们只得到了加速度的电压值，还需转化为加速度，这时候就要知道灵敏度的值，取为Sensitivity=478.5mV/g=0.4785V/g。所以RX=DeltaVoltsRx/SensitivityRX=0.24V/0.4785V/G=0.5g最后，我们结合以上四个公式（总要把它整合一下吧）Rx=(AdcRx*Vref/1023VzeroG)/Sensitivity现在，我们得到三个矢量，如果设备不受影响，那么我们就可以认为这个方向就是重力矢量的方向。如果你想计算设备相对于地面的倾角，可以计算这个矢量和Z轴之间的夹角。现在我们已经算出了Rx，Ry，Rz的值，让我们回到我们的上一个加速度模型，再加一些标注上去：R和X，Y，Z轴之间的夹角，可表示为Axr，Ayr，Azr。观察由R和Rx组成的直角。cos(Axr)=Rx/R,类似的：cos(Ayr)=Ry/Rcos(Azr)=Rz/R从公式1我们可以推导出R=SQRT(Rx2+Ry2+Rz2)通过arccos()函数（cos()的反函数）我们可以计算出所需的角度：Axr=arccos(Rx/R)Ayr=arccos(Ry/R)Azr=arccos(Rz/R)陀螺仪部分2轴陀螺仪检测绕X和Y轴的旋转。为了用数字来表达这些旋转，我们先引进一些符号。首先我们定义：Rxz惯性力矢量R在XZ平面上的投影Ryz惯性力矢量R在YZ平面的上投影在由Rxz和Rz组成的直角三角形中，运用勾股定理可得：Rxz2=Rx2+Rz2，同样：Ryz2=Ry2+Rz2同时注意：R2=Rxz2+Ry2，这个公式可以公式1和上面的公式推导出来，也可由R和Ryz所组成的直角三角形推导出来R2=Ryz2+RX2在这篇文章中我们不会用到这些公式，但知道模型中的那些数值间的关系有助于理解。相反，我们按如下方法定义Z轴和Rxz、Ryz向量所成的夹角：AXZ-Rxz（矢量R在XZ平面的投影）和Z轴所成的夹角AYZ-Ryz（矢量R在YZ平面的投影）和Z轴所成夹角陀螺仪测量上面定义的角度的变化率。换句话说，它会输出一个与上面这些角度变化率线性相关的值。角度变化率按下面方法计算：RateAxz=(Axz1Axz0)/(t1t0).和加速度计部分一样，陀螺仪也可以整合RateAxz=(AdcGyroXZ*Vref/1023VzeroRate)/Sensitivity第三部分 融合加速度计和陀螺仪的数据在使用整合了加速度计和陀螺仪的IMU设备时，首先要做的就是统一它们的坐标系。接下来的步骤我会介绍一种算法，算法受卡尔曼滤波中的一些思想启发，但是它更简单并且更容易在嵌入式设备中实现。这个我就自己讲了。Thank you for listening !