加速度传感器算法：三步教你结合加速度传感器和陀螺仪算法设计

这篇文章主要介绍加速度计和陀螺仪的数学模型和基本算法，以及如何融合这两者，侧重算法、思想的讨论.
介绍
本指南旨在向兴趣者介绍惯性MEMS（微机电系统）传感器，特别是加速度计和陀螺仪以及其他整合IMU（惯性测量单元）设备。

IMU单元例子：上图中MCU顶端的ACC Gyro 6DOF，名为USBThumb，支持USB/串口通信
在这篇文章中我将概括这么几个基本并且重要的话题：
- 加速度计（accelerometer）检测什么
- 陀螺仪（gyroscope，也称作 gyro）检测什么
- 如何将传感器ADC读取的数据转换为物理单位（加速度传感器的单位是g，陀螺仪的单位是 度/秒）
- 如何结合加速度传感器和陀螺仪的数据以得到设备和地平面之间的倾角的准确信息
在整篇文章中我尽量将数学运算降低到最少。如果你知道什么是正弦、余弦、正切函数，那无论你的项目使用哪种平台你应该都会明白和运用这篇文章中的思想，这些平台如Arduino、Propeller、Basic Stamp、Ateml芯片、PIC芯片等等。总有些人认为使用IMU单元需要复杂的数学运算（复杂的FIR或IIR滤波，如卡尔曼滤波，Parks-McClellan滤波等）。你如果研究这些会得到很棒且很复杂的结果。我解释事情的方式，只需要基本的数学。我非常坚信简单的原则。我认为一个简单的系统更容易操作和监控，另外许多嵌入式设备并不具备能力和资源去实现需要进行矩阵运算的复杂算法。
我会用我设计的一个新IMU模块——Acc_Gyro Accelerometer + Gyro IMU作为例子。在下面的例子中我们会使用这个设备的参数。用这个模块作为介绍非常合适，因为它由3个设备组成：
- LIS331AL (datasheet) – 3轴 2G 模拟加速度计
- LPR550AL (datasheet) – 双轴（俯仰、翻滚） 500°/s 加速度传感器
- LY550ALH (datasheet) –单轴（偏航）陀螺仪 最后这个设备在这篇介绍中不使用，不过他在?DCM Matrix implementation中有重要作用
它们一起组成了一个6自由度的惯性测量单元。这是个花哨的名字！然而，在花哨的名字后面是个非常有用的设备组合，接下来我们会详细介绍之。
第一部分 加速度计
要了解这个模块我们先从加速度计开始。当我们在想象一个加速度计的时候我们可以把它想作一个圆球在一个方盒子中。你可能会把它想作一个饼干或者甜圈，但我就把它当做一个球好了：

请注意加速度计检测到得力的方向与它本身加速度的方向是相反的。这种力量通常被称为惯性力或假想力。在这个模型中你你应该学到加速度计是通过间接测量力对一个墙面的作用来测量加速度的，在实际应用中，可能通过弹簧等装置来测量力。这个力可以是加速度引起的，但在下面的例子中，我们会发现它不一定是加速度引起的。
如果我们把模型放在地球上，球会落在Z-墙面上并对其施加一个1g的力，见下图：

在这种情况下盒子没有移动但我们任然读取到Z轴有-1g的值。球在墙壁上施加的压力是由引力造成的。在理论上，它可以是不同类型的力量 - 例如，你可以想象我们的球是铁质的，将一个磁铁放在盒子旁边那球就会撞上另一面墙。引用这个例子只是为了说明加速度计的本质是检测力而非加速度。只是加速度所引起的惯性力正好能被加速度计的检测装置所捕获。
虽然这个模型并非一个MEMS传感器的真实构造，但它用来解决与加速度计相关的问题相当有效。实际上有些类似传感器中有金属小球，它们称作倾角开关，但是它们的功能更弱，只能检测设备是否在一定程度内倾斜，却不能得到倾斜的程度。
到目前为止，我们已经分析了单轴的加速度计输出，这是使用单轴加速度计所能得到的。三轴加速度计的真正价值在于它们能够检测全部三个轴的惯性力。让我们回到盒子模型，并将盒子向右旋转45度。现在球会与两个面接触：Z-和X-，见下图：

0.71g这个值是不是任意的，它们实际上是1/2的平方根的近似值。我们介绍加速度计的下一个模型时这一点会更清楚。
在上一个模型中我们引入了重力并旋转了盒子。在最后的两个例子中我们分析了盒子在两种情况下的输出值，力矢量保持不变。虽然这有助于理解加速度计是怎么和外部力相互作用的，但如果我们将坐标系换为加速度的三个轴并想象矢量力在周围旋转，这会更方便计算。

请看看在上面的模型，我保留了轴的颜色，以便你的思维能更好的从上一个模型转到新的模型中。想象新模型中每个轴都分别垂直于原模型中各自的墙面。矢量R是加速度计所检测的矢量（它可能是重力或上面例子中惯性力的合成）。RX，RY，RZ是矢量R在X，Y，Z上的投影。请注意下列关系：
，R ^ 2=RX ^ 2 + RY ^ 2 + RZ ^ 2（公式1）
此公式等价于三维空间勾股定理。
还记得我刚才说的1/2的平方根0.71不是个随机值吧。如果你把它们代回上式，回顾一下重力加速度是1g，那我们就能验证：
1 ^ 2=（SQRT（1/2））^ 2 + 0 ^ 2 +（SQRT（1/2））^ 2在公式1中简单的取代： R=1, Rx=-SQRT(1/2), Ry=0 , Rz=-SQRT(1/2)
经过一大段的理论序言后，我们和实际的加速度计很靠近了。RX，RY，RZ值是实际中加速度计输出的线性相关值，你可以用它们进行各种计算。
在我们运用它之前我们先讨论一点获取加速度计数据的方法。大多数加速度计可归为两类：数字和模拟。数字加速度计可通过I2C，SPI或USART方式获取信息，而模拟加速度计的输出是一个在预定范围内的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。我将不会详细介绍ADC是怎么工作的，部分原因是这是个很广的话题，另一个原因是不同平台的ADC都会有差别。有些MCU具有内置ADC模块，而有些则需要外部电路进行ADC转换。不管使用什么类型的ADC模块，你都会得到一个在一定范围内的数值。例如一个10位ADC模块的输出值范围在0 .. 1023间，请注意，1023=2 ^ 10 -1。一个12位ADC模块的输出值范围在0 .. 4095内，注意，4095=2 ^ 12-1。
我们继续，先考虑下一个简单的例子，假设我们从10位ADC模块得到了以下的三个轴的数据：
AdcRx=586AdcRy=630
AdcRz=561
每个ADC模块都有一个参考电压，假设在我们的例子中，它是3.3V。要将一个10位的ADC值转成电压值，我们使用下列公式：
VoltsRx=AdcRx * VREF / 1023
小注：8位ADC的最大值是255=2 ^ 8 -1，12位ADC最大值是4095=2 ^ 12 -1。
将3个轴的值代入上式，得到：
VoltsRx=586 * 3.3 / 1023=～1.89V（结果取两位小数）VoltsRy=630 * 3.3 / 1023=～2.03V
VoltsRz=561 * 3.3 / 1023=～1.81V
每个加速度计都有一个零加速度的电压值，你可以在它的说明书中找到，这个电压值对应于加速度为0g。通过计算相对0g电压的偏移量我们可以得到一个有符号的电压值。比方说，0g电压值 VzeroG=1.65V，通过下面的方式可以得到相对0g电压的偏移量:
DeltaVoltsRx=1.89V - 1.65V=0.24VDeltaVoltsRy=2.03V - 1.65V=0.38V
DeltaVoltsRz=1.81V - 1.65V=0.16V
现在我们得到了加速度计的电压值，但它的单位还不是g（9.8m/s^2），最后的转换，我们还需要引入加速度计的灵敏度（Sensitivity），单位通常是 mV/g。比方说，加速度计的灵敏度 Sensitivity=478.5mV / g=0.4785V /g。灵敏度值可以在加速度计说明书中找到。要获得最后的单位为g的加速度，我们使用下列公式计算：
RX=DeltaVoltsRx /SensitivityRX=0.24V / 0.4785V / G=～0.5gRY=0.38V / 0.4785V / G=～0.79g
RZ=0.16V / 0.4785V / G=～0.33g
当然，我们可以把所有的步骤全部放在一个式子里，但我想通过介绍每一个步骤以便让你了解怎么读取一个ADC值并将其转换为单位为g的矢量力的分量。
Rx=(AdcRx * Vref / 1023 – VzeroG) / Sensitivity(公式2)Ry=(AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz=(AdcRz * Vref / 1023 – VzeroG) / Sensitivity
现在我们得到了惯性力矢量的三个分量，如果设备除了重力外不受任何外力影响，那我们就可以认为这个方向就是重力矢量的方向。如果你想计算设备相对于地面的倾角，可以计算这个矢量和Z轴之间的夹角。如果你对每个轴的倾角都感兴趣，你可以把这个结果分为两个分量：X轴、Y轴倾角，这可以通过计算重力矢量和X、Y轴的夹角得到。计算这些角度比你想象的简单，现在我们已经算出了Rx，Ry，Rz的值，让我们回到我们的上一个加速度模型，再加一些标注上去：

我们感兴趣的角度是向量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()函数（cos()的反函数）我们可以计算出所需的角度：
Axr=arccos(Rx/R)
Ayr=arccos(Ry/R)
Azr=arccos(Rz/R)
我们花了大段的篇幅来解释加速度计模型，最后所要的只是以上这几个公式。根据你的应用场合，你可能会用到我们推导出来的几个过渡公式。我们接下来要介绍陀螺仪模块，并向大家介绍怎么融合加速度计和陀螺仪的数据以得到更精确的倾角值。
但在此之前，我们再介绍几个很常用的公式：cosX=cos(Axr)=Rx / RcosY=cos(Ayr)=Ry / RcosZ=cos(Azr)=Rz / R
这三个公式通常称作方向余弦，它主要表达了单位向量（长度为1的向量）和R向量具有相同的方向。你可以很容易地验证：
SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）=1
这是个很好的性质，因为它避免了我们一直检测R向量的模（长度）。通常如果我们只是对惯性力的方向感兴趣，那标准化模长以简化其他计算是个明智的选择。
第二部分陀螺仪
对于陀螺仪我们将不会像加速度计一样介绍它的等价盒子模型，而是直接跳到加速度计的第二个模型，通过这个模型我们会向大家介绍陀螺仪是怎么工作的。

陀螺仪的每个通道检测一个轴的旋转。例如，一个2轴陀螺仪检测绕X和Y轴的旋转。为了用数字来表达这些旋转，我们先引进一些符号。首先我们定义：
Rxz – 惯性力矢量R在XZ平面上的投影Ryz – 惯性力矢量R在YZ平面的上投影在由Rxz和Rz组成的直角三角形中，运用勾股定理可得：Rxz^2=Rx^2 + Rz^2 ，同样：Ryz^2=Ry^2 + Rz^2同时注意：R^2=Rxz^2 + Ry^2 ，这个公式可以公式1和上面的公式推导出来，也可由R和Ryz所组成的直角三角形推导出来R ^ 2=Ryz ^ 2 + RX ^ 2
在这篇文章中我们不会用到这些公式，但知道模型中的那些数值间的关系有助于理解。
相反，我们按如下方法定义Z轴和Rxz、Ryz向量所成的夹角：AXZ - Rxz（矢量R在XZ平面的投影）和Z轴所成的夹角AYZ - Ryz（矢量R在YZ平面的投影）和Z轴所成夹角
现在我们离陀螺仪要测量的东西又近了一步。陀螺仪测量上面定义的角度的变化率。换句话说，它会输出一个与上面这些角度变化率线性相关的值。为了解释这一点，我们先假设在t0时刻，我们已测得绕Y轴旋转的角度（也就是Axz），定义为Axz0，之后在t1时刻我们再次测量这个角度，得到Axz1。
角度变化率按下面方法计算：
RateAxz=(Axz1 – Axz0) / (t1 – t0).
如果用度来表示角度，秒来表示时间，那这个值的单位就是 度/秒。这就是陀螺仪检测的东西。
在实际运用中，陀螺仪一般都不会直接给你一个单位为度/秒的值（除非它是个特殊的数字陀螺仪）。就像加速度计一样，你会得到一个ADC值并且要用类似公式2的式子将其转换成单位为 度/秒的值。让我们来介绍陀螺仪输出值转换中的ADC部分（假设使用10位ADC模块，如果是8位ADC，用1023代替255，如果是12为ADC用4095代替1023）。
RateAxz=(AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity公式3RateAyz=(AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity
AdcGyroXZ，AdcGyroYZ - 这两个值由ADC读取，它们分别代表矢量R的投影在XZ和YZ平面内里的转角，也可等价的说，旋转可分解为单独绕Y和X轴的运动。
Vref – ADC的参考电压，上例中我们使用3.3V
VzeroRate – 是零变化率电压，换句话说它是陀螺仪不受任何转动影响时的输出值，对Acc Gyro板来说，可以认为是1.23V（此值通常可以在说明书中找到——但千万别相信这个值，因为大多数的陀螺仪在焊接后会有一定的偏差，所以可以使用电压计测量每个通道的输出值，通常这个值在焊接后就不会改变，如果有跳动，在设备使用前写一个校准程序对其进行测量，用户应当在设备启动的时候保持设备静止以进行校准）。
Sensitivity –陀螺仪的灵敏度，单位mV/(deg/s)，通常写作mV/deg/s,它的意思就是如果旋转速度增加1°/s,陀螺仪的输出就会增加多少mV。Acc_Gyro板的灵敏度值是2mV/deg/s或0.002V/deg/s让我们举个例子，假设我们的ADC模块返回以下值：AdcGyroXZ=571AdcGyroXZ=323用上面的公式，在代入Acc Gyro板的参数，可得：RateAxz=(571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s)=~ 306 deg/sRateAyz=(323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s)=~ -94 deg/s换句话说设备绕Y轴（也可以说在XZ平面内）以306°/s速度和绕X轴（或者说YZ平面内）以-94°/s的速度旋转。请注意，负号表示该设备朝着反方向旋转。按照惯例，一个方向的旋转是正值。一份好的陀螺仪说明书会告诉你哪个方向是正的，否则你就要自己测试出哪个旋转方向会使得输出脚电压增加。最好使用示波器进行测试，因为一旦你停止了旋转，电压就会掉回零速率水平。如果你使用的是万用表，你得保持一定的旋转速度几秒钟并同时比较电压值和零速率电压值。如果值大于零速率电压值那说明这个旋转方向是正向。
第三部分 将它们综合起来。融合加速度计和陀螺仪的数据
如果你在阅读这篇文章你可能已经有了或准备购买一个IMU设备，或者你准备用独立的加速度计和陀螺仪搭建一个。
在使用整合了加速度计和陀螺仪的IMU设备时，首先要做的就是统一它们的坐标系。最简单的办法就是将加速度计作为参考坐标系。大多数的加速度计技术说明书都会指出对应于物理芯片或设备的XZY轴方向。例如，下面就是Acc Gyro板的说明书中给出的XYZ轴方向：

接下来的步骤是：
- 确定陀螺仪的输出对应到上述讨论的RateAxz，RateAyz值。
- 根据陀螺仪和加速度计的位置决定是否要反转输出值
不要设想陀螺仪陀的输出有XY，它会适应加速度计坐标系里的任何轴，尽管这个输出是IMU模块的一部分。最好的办法就是测试。
接下来的示例用来确定哪个陀螺仪的输出对应RateAxz。
- 首先将设备保持水平。加速度计的XY轴输出会是零加速度电压（Acc Gyro板的值是1.65V）
- 接下来将设备绕Y轴旋转，换句话说就是将设备在XZ平面内旋转，所以X、Z的加速度输出值会变化而Y轴保持不变。
-当以匀速旋转设备的时候，注意陀螺仪的哪个通道输出值变化了，其他输出应该保持不变。
- 在陀螺仪绕Y轴旋转（在XZ平面内旋转）的时候输出值变化的就是AdcGyroXZ，用于计算RateAxz
-最后一步，确认旋转的方向是否和我们的模型对应，因为陀螺仪和加速度的位置关系，有时候你可能要把RateAxz值反向
-重复上面的测试，将设备绕Y轴旋转，这次查看加速度计的X轴输出（也就是AdcRx）。如果AdcRx增大（从水平位置开始旋转的第一个90°），那AdcGyroXZ应当减小。这是因为我们观察的是重力矢量，当设备朝一个方向旋转时矢量会朝相反的方向旋转（相对坐标系运动）。所以，如果你不想反转RateAxz，你可以在公式3中引入正负号来解决这个问题：
RateAxz=InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity ,其中InvertAxz=1 或-1
同样的方法可以用来测试RateAyz，将设备绕X轴旋转，你就能测出陀螺仪的哪个输出对应于RateAyz，以及它是否需要反转。一旦你确定了InvertAyz，你就能可以用下面的公式来计算RateAyz：
RateAyz=InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity
如果对Acc Gyro板进行这些测试，你会得到下面的这些结果：
- RateAxz的输出管脚是GX4，InvertAxz=1
- RateAyz输出管脚是GY4，InvertAyz=1
从现在开始我们认为你已经设置好了IMU模块并能计算出正确的Axr，Ayr，Azr值（在第一部分加速度计中定义）以及RateAyz，RateAyz（在第二部分陀螺仪中）。下一步，我们分析这些值之间的关系并得到更准确的设备和地平面之间的倾角。
你可能会问自己一个问题，如果加速度计已经告诉我们Axr，Ayr，Azr的倾角，为什么还要费事去得到陀螺仪的数据？答案很简单：加速度计的数据不是100%准确的。有几个原因，还记加速度计测量的是惯性力，这个力可以由重力引起（理想情况只受重力影响），当也可能由设备的加速度（运动）引起。因此，就算加速度计处于一个相对比较平稳的状态，它对一般的震动和机械噪声很敏感。这就是为什么大部分的IMU系统都需要陀螺仪来使加速度计的输出更平滑。但是怎么办到这点呢？陀螺仪不受噪声影响吗？
陀螺仪也会有噪声，但由于它检测的是旋转，因此对线性机械运动没那么敏感，不过陀螺仪有另外一种问题，比如漂移（当选择停止的时候电压不会回到零速率电压）。然而，通过计算加速度计和陀螺仪的平均值我们能得到一个相对更准确的当前设备的倾角值，这比单独使用加速度计更好。
接下来的步骤我会介绍一种算法，算法受卡尔曼滤波中的一些思想启发，但是它更简单并且更容易在嵌入式设备中实现。在此之前，让我们先看看我们需要算法计算什么值。所要算的就是重力矢量R=[Rx,Ry,Rz]，它可由其他值推导出来，如Axr，Ayr，Azr或者cosX，cosY，cosZ，由这些值我们能得到设备相对地平面的倾角值，这些关系我们在第一部分已经讨论过。有人可能会说-根据第一部分的公式2我们不是已经得到Rx，Ry，Rz的值了吗？是的，但是记住，这些值只是由加速度计数据推导出来的，如果你直接将它们用于你的程序你会得到难以忍受的噪声。为了避免进一步的混乱，我们重新定义加速度计的测量值：
Racc – 是由加速度计测量到得惯性力矢量，它可分解为下面的分量（在XYZ轴上的投影）：
RxAcc=(AdcRx * Vref / 1023 – VzeroG) / SensitivityRyAcc=(AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc=(AdcRz * Vref / 1023 – VzeroG) / Sensitivity
现在我们得到了一组只来自于加速度计ADC的值。我们把这组数据叫做“vector”，并使用下面的符号：
Racc=[RxAcc,RyAcc,RzAcc]
因为这些Racc的分量可由加速度计数据得到，我们可以把它当做算法的输入。
请注意Racc测量的是重力，如果你得到的矢量长度约等于1g那么你就是正确的：
|Racc|=SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),
但是请确定把矢量转换成下面的矢量非常重要：
Racc(normalized)=[RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|].
这可以确保标准化Racc始终是1。
接来下我们引进一个新的向量：
Rest=[RxEst,RyEst,RzEst]
这就是算法的输出值，它经过陀螺仪数据的修正和基于上一次估算的值。
这是算法所做的事：
-加速度计告诉我们：“你现在的位置是Racc”
我们回答：“谢谢，但让我确认一下”
-然后根据陀螺仪的数据和上一次的Rest值修正这个值并输出新的估算值Rest。
-我们认为Rest是当前设备姿态的“最佳值”。
让我们看看它是怎么实现的。
数列的开始，我们先认为加速度值正确并赋值：
Rest(0)=Racc(0)
Rest和Racc是向量，所以上面的式子可以用3个简单的式子代替，注意别重复了：
RxEst（0）=RxAcc（0）
RyEst（0）=RyAcc（0）
RzEst（0）=RzAcc（0）
接下来我们在每个等时间间隔T秒做一次测量，得到新的测量值，并定义为Racc（1），Racc（2），Racc（3）等等。同时，在每个时间间隔我们也计算出新的估算值Rest（1）,Rest（2），Rest（3），等等。
假设我们在第n步。我们有两列已知的值可以用：
Rest(n-1) – 前一个估算值，Rest(0)=Racc(0)
Racc(n) – 当前加速度计测量值
在计算Rest（n）前，我们先引进一个新的值，它可由陀螺仪和前一个估算值得到。
叫做Rgyro，同样它是个矢量并由3个分量组成：
Rgyro=[RxGyro,RyGyro,RzGyro]
我们分别计算这个矢量的分量，从RxGyro开始。

首先观察陀螺仪模型中下面的关系，根据由Rz和Rxz组成的直角三角形我们能推出：
tan(Axz)=Rx/Rz=> Axz=atan2(Rx,Rz)
你可能从未用过atan2这个函数，它和atan类似，但atan返回值范围是（-PI/2,PI/2），atan2返回值范围是（-PI，PI），并且他有两个参数。它能将Rx，Rz值转换成360°（-PI，PI）内的角度。
所以，知道了RxEst（n-1）和RzEst（n-1）我们发现：
Axz(n-1)=atan2( RxEst(n-1) , RzEst(n-1) ).
记住，陀螺仪测量的是Axz角度变化率，因此，我们可以按如下方法估算新的角度Axz（n）：
Axz(n)=Axz(n-1) + RateAxz(n) * T
请记住，RateAxz可由陀螺仪ADC读取得到。通过使用平均转速可由得到一个更准确的公式：
RateAxzAvg=（RateAxz（N）+ RateAxz（N-1））/ 2
Axz(n)=Axz(n-1) + RateAxzAvg * T
同理可得：
Ayz(n)=Ayz(n-1) + RateAyz(n) * T
好了，现在我们有了Axz（n），Ayz（n）。现在我们如何推导出RxGyro/RyGyro?根据公式1我们可以把Rgyro长度写成下式：
| Rgyro |=SQRT（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2）
同时，因为我们已经将Racc标准化，我们可以认为它的长度是1并且旋转后保持不变，所以写成下面的方式相对比较安全：
| Rgyro |=1
我们暂时采用更短的符号进行下面的计算：
x=RxGyro , y=RyGyro, z=RzGyro
根据上面的关系可得：
x=x / 1=x / SQRT(x^2+y^2+z^2)分子分母同除以SQRT（X ^ 2 + Z ^ 2）x=( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )注意x / SQRT(x^2 + z^2)=sin(Axz), 所以:x=sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )将SQRT内部分式的分子分母同乘以z^2x=sin(Axz) / SQRT (1 + y^2* z ^2 / (z^2 * (x^2 + z^2)) )注意 z / SQRT(x^2 + z^2)=cos(Axz)， y / z=tan(Ayz), 所以最后可得:
x=sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )
替换成原来的符号可得：
RxGyro=sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )
同理可得：
RyGyro=sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )
提示：这个公式还可以更进一步简化。分式两边同除以sin（axz（你））可得：
RxGyro=1/ SQRT (1/ sin(Axz(n))^2+ cos(Axz(n))^2 / sin(Axz(n))^2* tan(Ayz(n))^2 )RxGyro=1/ SQRT (1/ sin(Axz(n))^2+ cot(Axz(n))^2* sin(Ayz(n))^2/ cos(Ayz(n))^2 )现在加减 cos(Axz(n))^2/sin(Axz(n))^2=cot(Axz(n))^2RxGyro=1/ SQRT (1/ sin(Axz(n))^2-cos(Axz(n))^2/sin(Axz(n))^2 + cot(Axz(n))^2* sin(Ayz(n))^2/ cos(Ayz(n))^2+ cot(Axz(n))^2 )
综合条件1、2和3、4可得：
RxGyro=1/ SQRT (1+ cot(Axz(n))^2 * sec(Ayz(n))^2 ), 其中cot(x)=1 / tan(x)， sec(x)=1 / cos(x)
这个公式只用了2个三角函数并且计算量更低。如果你有Mathematica程序，通过使用 FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2 * Tan[B]^2)]你可以验证这个公式。
现在我们发现：
RzGyro=Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).
其中，当 RzGyro>=0时，Sign(RzGyro)=1 , 当 RzGyro<0时，Sign(RzGyro)=-1 。 一个简单的估算方法：Sign(RzGyro)=Sign(RzEst(n-1))在实际应用中，当心RzEst（n-1）趋近于0。这时候你可以跳过整个陀螺仪阶段并赋值：Rgyro=Rest（n-1）。Rz可以用作计算Axz和Ayz倾角的参考，当它趋近于0时，它可能会溢出并引发不好的后果。这时你会得到很大的浮点数据，并且tan()/atan()函数得到的结果会缺乏精度。 现在我们回顾一下已经得到的结果，我们在算法中的第n步，并计算出了下面的值： Racc – 加速度计读取的当前值Rgyro –根据Rest(-1)和当前陀螺仪读取值所得我们根据哪个值来更新Rest（n）呢？你可能已经猜到，两者都采用。我们会用一个加权平均值，得：Rest(n)=(Racc * w1 + Rgyro * w2 ) / (w1 + w2)分子分母同除以w1，公式可简化成：Rest(n)=(Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)令w2=w1=wGyro，可得：Rest(n)=(Racc + Rgyro * wGyro ) / (1 + wGyro)在上面的公式中，wGyro表示我们对加速度计和陀螺仪的相信程度。这个值可以通过测试确定，根据经验值5-20之间会得到一个很好的结果。此算法和卡尔曼滤波最主要的差别是它的权重是相对固定的，而卡尔曼滤波中的权重会随着加速度计读取的噪声而改变。卡尔曼滤波注重给你一个“最好”的理论结果，而此算法给你的是实际项目中“够用”的结果。你可以实现一个算法，它能根据测量的噪声而改变wGyro值，但对大部分应用来说固定的权重也能工作的很好。现在得到最新的估算值还差一步：RxEst(n)=(RxAcc + RxGyro * wGyro ) / (1 + wGyro)RyEst(n)=(RyAcc + RyGyro * wGyro ) / (1 + wGyro)RzEst(n)=(RzAcc + RzGyro * wGyro ) / (1 + wGyro)现在，再次标准化矢量：R=SQRT(RxEst(n) ^2 + RyEst(n)^2 +RzEst(n)^2 )RxEst(n)=RxEst(n)/RRyEst(n)=RyEst(n)/RRzEst(n)=RzEst(n)/R现在，可以再次进行下一轮循环了。                  

加速度传感器算法：利用三轴加速度传感器的计步测算法

　　现如今，很多现代人都非常注重自己的日常锻炼，计步作为一种有效记录监控锻炼的监控手段，被广泛应用在移动终端的应用中。
　　目前，大部分的计步都是通过GPS信号来测算运动距离，再反推行走步数实现的。这种方法很是有效，但在室内或没有GPS信号的设备上无法工作。同时，GPS精度对结果的干扰也比较大。
　　为避免上述问题的出现，我们可以考虑一种新的测步方法，即：通过设备上的加速度传感器来计算步数，在不支持GPS的设备上也可正常工作。还可以与GPS互相配合测步，这样可令使用场景变得多样。
　1.先要摸清模型的特征
　　目前，大部分设备都提供了可以检测各个方向的加速度传感器。以iOS设备为例，我们利用了其三轴加速度传感器（x，y，z轴代表方向如图）的特性来分析。分别用以检测人步行中三个方向的加速度变化。
iOS设备的三轴加速度传感器示意图
　　用户在水平步行运动中，垂直和前进两个加速度会呈现周期性变化，如图所示。在步行收脚的动作中，由于重心向上单只脚触地，垂直方向加速度是呈正向增加的趋势，之后继续向前，重心下移两脚触底，加速度相反。水平加速度在收脚时减小，在迈步时增加。
　　反映到图表中，可以看到，在步行运动中，垂直和前进产生的加速度与时间大致为一个正弦曲线，而且在某点有一个峰值。其中，垂直方向的加速度变化最大，通过对轨迹的峰值进行监测计算和加速度阀值决策，即可实时计算用户运动的步数，还可依此进一步估算用户步行距离。
2.计步的合理算法
　　因为用户在运动中可能用手平持设备，或者将设备置于口袋中。所以，设备的放置方向不定。为此，通过计算三个加速度的矢量长度，我们可以获得一条步行运动的正弦曲线轨迹。
　　第二步是峰值检测，我们记录了上次矢量长度和运动方向，通过矢量长度的变化，可以判断目前加速度的方向，并和上一次保存的加速度方向进行比较。如果是相反的，即是刚过峰值状态，则进入计步逻辑进行计步，否则舍弃。通过对峰值的次数累加，可得到用户步行的步伐。
　　最后，就是去干扰。手持设备会有一些低幅度和快速的抽动状态，或是我们俗称的手抖，或者某个恶作剧用户想通过短时快速反复摇动设备来模拟人走路，这些干扰数据如果不剔除，会影响记步的准确值，对于这种干扰，我们可以通过给检测加上阀值和步频判断来过滤。
　　人体最快的跑步频率为5HZ，也就是说相邻两步的时间间隔的至少大于0.2秒，如图所示，我们设置了timespan在记步过程中我们过滤了高频噪声，即步频过快的情况。同时我们通过和上次加速度大小进行比较，设置设立一定的阀值Threshold来判断运动是否属于有效，有效运动才可进行记步。
3.关于计步器的扩展
　　以上是一个依靠加速度测算的计步器实现原理，已知步行和跑步的步伐经验值，那么稍微改进下即可变成一个测距测速计。
　　通过三轴加速度传感器，我们可以知道用户的运动状态。除了计步，还可以通过加速器的变化曲线判断用户摔倒状态，做成一个老人和儿童摔倒检测自动报警器。

加速度传感器算法：基于三轴加速度传感器计步算法

本文介绍可穿戴设备加速度传感器－Lis3dh的特性原理和应用场景。意法半导体研发的Lis3dh广泛应用在智能手环、智能计步鞋等智能穿戴产品中。
Lis3dh有两种工作方式，一种是其内置了多种算法来处理常见的应用场景（如静止检测、运动检测、屏幕翻转、失重、位置识别、单击和双击等等），用户只需简单配置算法对应的寄存器即可开始检测，一旦检测到目标事件，Lis3dh的外围引脚INT1会产生中断。另一种是支持用户通过SPI/I2C来读取底层加速度数据，并自行通过软件算法来做进一步复杂的处理，如计步等等。
本文以Lis3dh为讲解案例，但工作原理和应用场景对其他加速度传感器同样适用。更多嵌入式和物联网原创技术分享敬请关注微信公众号：嵌入式企鹅圈。
一、加速度传感器工作原理
加速度传感器自然是对自身器件的加速度进行检测。其自身的物理实现方式咱们就不去展开了，可以想象芯片内部有一个真空区域，感应器件即处于该区域，其通过惯性力作用引起电压变化，并通过内部的ADC给出量化数值。
Lis3dh是三轴加速度传感器，因此其能检测X、Y、Z的加速度数据，如下图：
在静止的状态下，传感器一定会在一个方向重力的作用，因此有一个轴的数据是1g（即9.8米/秒的二次）。在实际的应用中，我们并不使用跟9.8相关的计算方法，而是以1g作为标准加速度单位，或者使用1/1000g，即mg。既然是ADC转换，那么肯定会有量程和精度的概念。在量程方面，Lis3dh支持（+-）2g/4g/8g/16g四种。一般作为计步应用来说，2g是足够的，除去重力加速度1g，还能检测出1g的加速度。至于精度，那就跟其使用的寄存器位数有关了。Lis3dh使用高低两个8位（共16位）寄存器来存取一个轴的当前读数。由于有正反两个方向的加速度，所以16位数是有符号整型，实际数值是15位。以（+-）2g量程来算，精度为2g/2^15= 2000mg/ =0.061mg。
当以上图所示的静止状态，z轴正方向会检测出1g，X、Y轴为0.如果调转位置（如手机屏幕翻转），那总会有一个轴会检测出1g，其他轴为0，在实际的测值中，可能并不是0，而是有细微数值。
在运动过程中，x，y，z轴都会发生变化。计步运动也有其固有的数值规律，因为迈步过程也有抬脚和放脚的规律过程，如下图。“脚蹬离地是一步的开始，此时由于地面的反作用力，垂直方向加速度开始增大，当脚达到最高位置时，垂直方向加速度达到最大；然后脚向下运动，垂直加速度开始减小，直到脚着地，垂直加速度减到最小值。接着下一步迈步。前向加速度由脚与地面的摩擦力产生，双脚触地时增大，一脚离地时减小。”[此处引用韩文正等人发表的《基于加速度传感器LIS3DH的计步器设计》]。
二、理解加速度传感器的一个坐标系误区
意法半导体针对LIS3DH发布两个文档，官方规格书和应用设计指导。单独提出这点是为因为本人之前在使用LIS3DH时可能是太久没有运用过立体几何思维，导致在X,Y,Z坐标系上混淆概念，对位置识别迟迟没能理解，现在指出这个误区。
下图的X,Y,Z除了代表我们所认识的三维坐标系外，还有一个重要的认知，那就是X,Y,Z轴对应的寄存器分别按照芯片图示（以芯片的圆点来确定）的方向来测加速度值，而不管芯片的位置如何，即X,Y,Z轴对应的三个寄存器总是以这样工作的：Z轴寄存器测芯片垂直方向的数据、Y轴测芯片左右方的数据、X轴测芯片前后的数据（前后左右的定义可能不够形象，大家能理解就好）。例如，图示静止状态下，X轴寄存器测芯片前后方向的加速度；如果芯片如右边图示静止时，X轴寄存器测的是坐标系的Z轴方向加速度。
三、LIS3DH内置硬件算法工作原理
由于计步等场景是需要先读取底层X,Y,Z轴数据再进行处理的，所以我们这里不去探讨这个算法。这里主要阐述如何利用LIS3DH内置的硬件算法来检测常用的场景。
LIS3DH的内置硬件算法主要由2个参数和1个模式选择来确定。2个参数分别是阈值和持续时间。例如，在静止的时候我们要去检测芯片的运动（wakeup）时，我们可以设定一个运动对应的阈值，并且要求芯片检测数据在超过这个阈值时要持续一定的时间才可以认为芯片是运动的。内置算法基本都是基于阈值和持续时间来进行检测的。
LIS3DH一共有两套能够同时工作的硬件算法电路，一种是专门针对单击、双击这种场景，如鼠标应用，另一种是针对其他所有场景的，如静止运动检测、运动方向识别、位置识别等等。这里我们主要讲述后者，其有四种工作模式：
第一种：OR或电路，即X,Y,Z任一轴数据超过阈值即可完成检测。
第二种：AND与电路，即X,Y,Z所有轴的数据均超过阈值才能完成检测。当然，其也允许只检测任意两个轴或者一个轴，不检测的轴的阈值检测可以认为是永远为真。
以上两种电路的阈值比较图示如下，阈值比较是绝对值比较，没有方向之分。不管在正方向还是负方向，只要绝对值超过阈值，那么XH(YH、ZH)为1，此时相应的XL(YL、ZL)为0；否则XL(YL、ZL)为1，相应的XH(YH、ZH)为0。XH(YH、ZH)、XL(YL、ZL)可以认为是检测条件是否满足的pending指示位。
第三种和第四种是一个物体六个方向的检测，movement检测芯片的运动方向变化，即从一种方向变化到另一种方向；而position检测芯片稳定为一种确定的方向（如稳定为平放朝上、平放朝下、竖立前后左右）等等。
其阈值比较电路如下，该阈值比较使用正负数真实数据比较。正方向超过阈值，则XH(YH、ZH)为1，否则为0；负方向超过阈值，XL(YL、ZL)为1，否则为0。XH(YH、ZH)、XL(YL、ZL)代表了六个方向。由于静止稳定状态时，只有一个方向有重力加速度，因此可以据此知道当时芯片的位置姿势。
四、加速度传感器应用
如果能够理解第三部分的工作原理，那么也能够很好理解以下的应用。
1.?静止时进行运动检测
使用OR电路工作方式，设置一个较小的运动阈值，只检测X,Y轴数据是否超过该阈值（Z轴这时有1g，咱不管这个轴了）即可。只要X,Y任一轴数据超过阈值一定时间即认为设备处于wakeup状态了。
2.?失重检测
失重时Z轴的加速度和重力加速度抵消，在短时间内会为0，而且X,Y轴没有变化，因此在短时间内三者都为0。这里使用AND电路工作方式，设置一个较小的运动阈值，当三个方向的数据都小于阈值一定时间时即认为是失重。
3.?位置姿势识别
例如手机翻转等应用场景就是利用这个特性。这里在第三部分讲解工作原理时已经讲得很清楚了。?
有了以上理解，以后在使用LIS3DH时直接找寄存器填数值就可以完成功能啦。
如转载请务必全文转载，保留嵌入式企鹅圈的微信公众号，否则即视为侵权。
? ? 群猴报喜，祝愿大家在猴年事事如意！嵌入式企鹅圈坚持百分百地原创高质量技术研发文章。更多原创技术分享敬请关注微信公众号：嵌入式企鹅圈

加速度传感器算法：求助如何对加速度传感器采集的信号进行滤波？

小白，现在有一套加速度传感器设备用于测量机械加工振动，采集卡保存了加工过程中的加速度(g)、速度(mm/s)和位移（mm），供应商那边说这套设备没有滤波功能，让我自己滤波。搜论文、相关书籍看了好几天，滤波方法很多，但是没看懂要处理加速度还是速度，还是位移？也不知道我如何处理这些数据，请大佬指点。
就我现有的这些数据，我应该如何做才能让这些数据可以使用？
先不管你的物理量是什么，这里只是对你的数据进行滤波，Python里面有很多滤波方法，下面举几个例子。你先导出自己的数据，可参考如下方式进行滤波操作。
import numpy as np
import matplotlib.pyplot as plt
from obspy import read
from scipy.signal import savgol_filter as sgolay
from scipy import signal
tr=read()[0]
# 原始信号 s1, 及对应的时间t
s1=tr.data; t=np.arange(len(s1)) * tr.stats.delta
# 相同权重卷积平均
s2=np.convolve(s1, np.ones(50)/50, mode='same')
# Savitsky-Golay 平滑滤波
s3=sgolay(s1, 91, 3)
# 低通滤波
fs=1 / (t[1]-t[0]) # 采样率 (赫兹)
fc=2 # 截止频率 （赫兹）
b, a=signal.butter(4, 2.0*fc/fs, 'lowpass')
s4=signal.filtfilt(b, a, s1)
# 高通滤波
fs=1 / (t[1]-t[0]) # 采样率 (赫兹)
fc=1 # 截止频率 （赫兹）
b, a=signal.butter(4, 2.0*fc/fs, 'highpass')
s5=signal.filtfilt(b, a, s1)
# 带通滤波
fs=1 / (t[1]-t[0]) # 采样率 (赫兹)
f1=.1; f2=2 # 通带的两个截止频率 （赫兹）
b, a=signal.butter(4,[2.0*f1/fs, 2.0*f2/fs], 'bandpass')
s6=signal.filtfilt(b, a, s1)
# # 带阻滤波
fs=1 / (t[1]-t[0]) # 采样率 (赫兹)
f1=.5; f2=5 # 阻带的两个截止频率
b, a=signal.butter(4,[2.0*f1/fs, 2.0*f2/fs], 'bandstop')
s7=signal.filtfilt(b, a, s1)
# 画图对比
l=6
plt.figure(figsize=(15, 20))
plt.subplot(l, 1, 1)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s2, label='Convolved&filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 2)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s3, label='Savitzky-Golay&filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 3)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s4, label='Lowpass filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 4)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s5, label='Highpass filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 5)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s6, label='Bandpass filtered')
plt.legend(fontsize=15)
plt.subplot(l, 1, 6)
plt.plot(t, s1, label='Original signal')
plt.plot(t, s7, label='Bandstop filtered')
plt.legend(fontsize=15)
plt.show()
下面展示这些滤波方法的结果：