模板网站劣势,网站栏目按扭,免费流量网站推广,客户管理系统管理本帖翻译自
IMU#xff08;加速度计和陀螺仪设备#xff09;在嵌入式应用中使用的指南。这篇文章主要介绍加速度计和陀螺仪的数学模型和基本算法#xff0c;以及如何融合这两者#xff0c;侧重算法、思想的讨论介绍本指南旨在向兴趣者介绍惯性MEMS#xff08;微机电系统加速度计和陀螺仪设备在嵌入式应用中使用的指南。这篇文章主要介绍加速度计和陀螺仪的数学模型和基本算法以及如何融合这两者侧重算法、思想的讨论介绍本指南旨在向兴趣者介绍惯性MEMS微机电系统传感器特别是加速度计和陀螺仪以及其他整合IMU
惯性测量单元
设备。2012-8-22 16:38 上传 下载附件 (16.48 KB) 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自由度的惯性测量单元。这是个花哨的名字然而在花哨的名字后面是个非常有用的设备组合接下来我们会详细介绍之。第一部分 加速度计要了解这个模块我们先从加速度计开始。当我们在想象一个加速度计的时候我们可以把它想作一个圆球在一个方盒子中。你可能会把它想作一个饼干或者甜圈但我就把它当做一个球好了2012-8-22 16:38 上传 下载附件 (15.85 KB) 我们假定这个盒子不在重力场中或者其他任何会影响球的位置的场中球处于盒子的正中央。你可以想象盒子在外太空中远离任何天体如果很难想象那就当做盒子在航天飞机中一切东西都处于无重力状态。在上面的图中你可以看到我们给每个轴分配了一对墙我们移除了Y以此来观察里面的情况。设想每面墙都能感测压力。如果我们突然把盒子向左移动加速度为1g9.8m/s^2那么球会撞上X-墙。然后我们检测球撞击墙面产生的压力X轴输出值为-1g。2012-8-22 16:38 上传 下载附件 (19.23 KB) 请注意加速度计检测到得力的方向与它本身加速度的方向是相反的。这种力量通常被称为
惯性力或假想力。在这个模型中你你应该学到加速度计是通过间接测量力对一个墙面的作用来测量加速度的在实际应用中可能通过弹簧等装置来测量力。这个力可以是加速度引起的但在下面的例子中我们会发现它不一定是加速度引起的。如果我们把模型放在地球上球会落在Z-墙面上并对其施加一个1g的力见下图2012-8-22 16:38 上传 下载附件 (19.54 KB) 在这种情况下盒子没有移动但我们任然读取到Z轴有-1g的值。球在墙壁上施加的压力是由引力造成的。在理论上它可以是不同类型的力量 - 例如你可以想象我们的球是铁质的将一个磁铁放在盒子旁边那球就会撞上另一面墙。引用这个例子只是为了说明加速度计的本质是检测力而非加速度。只是加速度所引起的惯性力正好能被加速度计的检测装置所捕获。虽然这个模型并非一个MEMS传感器的真实构造但它用来解决与加速度计相关的问题相当有效。实际上有些类似传感器中有金属小球它们称作倾角开关但是它们的功能更弱只能检测设备是否在一定程度内倾斜却不能得到倾斜的程度。到目前为止我们已经分析了单轴的加速度计输出这是使用单轴加速度计所能得到的。三轴加速度计的真正价值在于它们能够检测全部三个轴的惯性力。让我们回到盒子模型并将盒子向右旋转45度。现在球会与两个面接触Z-和X-见下图2012-8-22 16:38 上传 下载附件 (34.03 KB) 0.71g这个值是不是任意的它们实际上是1/2的平方根的近似值。我们介绍加速度计的下一个模型时这一点会更清楚。在上一个模型中我们引入了重力并旋转了盒子。在最后的两个例子中我们分析了盒子在两种情况下的输出值力矢量保持不变。虽然这有助于理解加速度计是怎么和外部力相互作用的但如果我们将坐标系换为加速度的三个轴并想象矢量力在周围旋转这会更方便计算。2012-8-22 16:38 上传 下载附件 (7.19 KB) 请看看在上面的模型我保留了轴的颜色以便你的思维能更好的从上一个模型转到新的模型中。想象新模型中每个轴都分别垂直于原模型中各自的墙面。矢量R是加速度计所检测的矢量它可能是重力或上面例子中惯性力的合成。RXRYRZ是矢量R在XYZ上的投影。请注意下列关系R ^ 2 RX ^ 2 RY ^ 2 RZ ^ 2
公式1此公式等价于
三维空间勾股定理
。还记得我刚才说的1/2的平方根0.71不是个随机值吧。如果你把它们代回上式回顾一下重力加速度是1g那我们就能验证1 ^ 2 SQRT1/2^ 2 0 ^ 2 SQRT1/2^ 2在
公式1
中简单的取代 R1, Rx -SQRT(1/2), Ry 0 , Rz -SQRT(1/2)经过一大段的理论序言后我们和实际的加速度计很靠近了。RXRYRZ值是实际中加速度计输出的线性相关值你可以用它们进行各种计算。在我们运用它之前我们先讨论一点获取加速度计数据的方法。大多数加速度计可归为两类数字和模拟。数字加速度计可通过I2CSPI或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 630AdcRz 561每个ADC模块都有一个参考电压假设在我们的例子中它是3.3V。要将一个10位的ADC值转成电压值我们使用下列公式VoltsRx AdcRx * VREF / 1023小注8位ADC的最大值是255 2 ^ 8 -112位ADC最大值是4095 2 ^ 12 -1。将3个轴的值代入上式得到VoltsRx 586 * 3.3 / 1023 1.89V结果取两位小数VoltsRy 630 * 3.3 / 1023 2.03VVoltsRz 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.38VDeltaVoltsRz 1.81V - 1.65V 0.16V现在我们得到了加速度计的电压值但它的单位还不是g9.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.79gRZ 0.16V / 0.4785V / G 0.33g当然我们可以把所有的步骤全部放在一个式子里但我想通过介绍每一个步骤以便让你了解怎么读取一个ADC值并将其转换为单位为g的矢量力的分量。Rx (AdcRx * Vref / 1023 – VzeroG) / Sensitivity(公式2)Ry (AdcRy * Vref / 1023 – VzeroG) / SensitivityRz (AdcRz * Vref / 1023 – VzeroG) / Sensitivity现在我们得到了惯性力矢量的三个分量如果设备除了重力外不受任何外力影响那我们就可以认为这个方向就是重力矢量的方向。如果你想计算设备相对于地面的倾角可以计算这个矢量和Z轴之间的夹角。如果你对每个轴的倾角都感兴趣你可以把这个结果分为两个分量X轴、Y轴倾角这可以通过计算重力矢量和X、Y轴的夹角得到。计算这些角度比你想象的简单现在我们已经算出了RxRyRz的值让我们回到我们的上一个加速度模型再加一些标注上去2012-8-22 16:38 上传 下载附件 (9.9 KB) 我们感兴趣的角度是向量R和XYZ轴之间的夹角那就令这些角度为AxrAyrAzr。观察由R和Rx组成的直角三角形cos(Axr) Rx / R , 类似的cos(Ayr) Ry / Rcos(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向量具有相同的方向。你可以很容易地验证SQRTcosX ^ 2 COSY ^ 2 cosZ ^ 2 1这是个很好的性质因为它避免了我们一直检测R向量的模长度。通常如果我们只是对惯性力的方向感兴趣那标准化模长以简化其他计算是个明智的选择。第二部分陀螺仪对于陀螺仪我们将不会像加速度计一样介绍它的等价盒子模型而是直接跳到加速度计的第二个模型通过这个模型我们会向大家介绍陀螺仪是怎么工作的。2012-8-22 16:38 上传 下载附件 (12.55 KB) 陀螺仪的每个通道检测一个轴的旋转。例如一个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) / SensitivityAdcGyroXZAdcGyroYZ - 这两个值由ADC读取它们分别代表矢量R的投影在XZ和YZ平面内里的转角也可等价的说旋转可分解为单独绕Y和X轴的运动。Vref – ADC的参考电压上例中我们使用3.3VVzeroRate – 是零变化率电压换句话说它是陀螺仪不受任何转动影响时的输出值对
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设备或者你准备用独立的加速度计和陀螺仪搭建一个。注具体的代码实现和算法测试请阅读这篇文章http://starlino.com/imu_kalman_arduino.html在使用整合了加速度计和陀螺仪的IMU设备时首先要做的就是统一它们的坐标系。最简单的办法就是将加速度计作为参考坐标系。大多数的加速度计技术说明书都会指出对应于物理芯片或设备的XZY轴方向。例如下面就是Acc Gyro板的说明书中给出的XYZ轴方向2012-8-22 16:39 上传 下载附件 (35.18 KB) 接下来的步骤是- 确定陀螺仪的输出对应到上述讨论的RateAxzRateAyz值。- 根据陀螺仪和加速度计的位置决定是否要反转输出值不要设想陀螺仪陀的输出有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你就能可以用下面的公式来计算RateAyzRateAyz InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity如果对Acc Gyro板进行这些测试你会得到下面的这些结果- RateAxz的输出管脚是GX4InvertAxz 1- RateAyz输出管脚是GY4InvertAyz 1从现在开始我们认为你已经设置好了IMU模块并能计算出正确的AxrAyrAzr值在第一部分加速度计中定义以及RateAyzRateAyz在第二部分陀螺仪中。下一步我们分析这些值之间的关系并得到更准确的设备和地平面之间的倾角。你可能会问自己一个问题如果加速度计已经告诉我们AxrAyrAzr的倾角为什么还要费事去得到陀螺仪的数据答案很简单加速度计的数据不是100%准确的。有几个原因还记加速度计测量的是惯性力这个力可以由重力引起理想情况只受重力影响当也可能由设备的加速度运动引起。因此就算加速度计处于一个相对比较平稳的状态它对一般的震动和机械噪声很敏感。这就是为什么大部分的IMU系统都需要陀螺仪来使加速度计的输出更平滑。但是怎么办到这点呢陀螺仪不受噪声影响吗陀螺仪也会有噪声但由于它检测的是旋转因此对线性机械运动没那么敏感不过陀螺仪有另外一种问题比如漂移当选择停止的时候电压不会回到零速率电压。然而通过计算加速度计和陀螺仪的平均值我们能得到一个相对更准确的当前设备的倾角值这比单独使用加速度计更好。接下来的步骤我会介绍一种算法算法受卡尔曼滤波中的一些思想启发但是它更简单并且更容易在嵌入式设备中实现。在此之前让我们先看看我们需要算法计算什么值。所要算的就是重力矢量R[Rx,Ry,Rz]它可由其他值推导出来如AxrAyrAzr或者cosXcosYcosZ由这些值我们能得到设备相对地平面的倾角值这些关系我们在第一部分已经讨论过。有人可能会说-根据第一部分的
公式2
我们不是已经得到RxRyRz的值了吗是的但是记住这些值只是由加速度计数据推导出来的如果你直接将它们用于你的程序你会得到难以忍受的噪声。为了避免进一步的混乱我们重新定义加速度计的测量值Racc – 是由加速度计测量到得惯性力矢量它可分解为下面的分量在XYZ轴上的投影RxAcc (AdcRx * Vref / 1023 – VzeroG) / SensitivityRyAcc (AdcRy * Vref / 1023 – VzeroG) / SensitivityRzAcc (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个简单的式子代替注意别重复了RxEst0 RxAcc0RyEst0 RyAcc0RzEst0 RzAcc0接下来我们在每个等时间间隔T秒做一次测量得到新的测量值并定义为Racc1Racc2Racc3等等。同时在每个时间间隔我们也计算出新的估算值Rest1,Rest2Rest3等等。假设我们在第n步。我们有两列已知的值可以用Rest(n-1) – 前一个估算值Rest(0) Racc(0)Racc(n) – 当前加速度计测量值在计算Restn前我们先引进一个新的值它可由陀螺仪和前一个估算值得到。叫做Rgyro同样它是个矢量并由3个分量组成Rgyro [RxGyro,RyGyro,RzGyro]我们分别计算这个矢量的分量从RxGyro开始。2012-8-22 16:39 上传 下载附件 (12.55 KB) 首先观察陀螺仪模型中下面的关系根据由Rz和Rxz组成的直角三角形我们能推出tan(Axz) Rx/Rz Axz atan2(Rx,Rz)你可能从未用过atan2这个函数它和atan类似但atan返回值范围是-PI/2,PI/2atan2返回值范围是-PIPI并且他有两个参数。它能将RxRz值转换成360°-PIPI内的角度。更多信息请阅读
atan2
.所以知道了RxEstn-1和RzEstn-1我们发现Axz(n-1) atan2( RxEst(n-1) , RzEst(n-1) ).记住陀螺仪测量的是Axz角度变化率因此我们可以按如下方法估算新的角度AxznAxz(n) Axz(n-1) RateAxz(n) * T请记住RateAxz可由陀螺仪ADC读取得到。通过使用平均转速可由得到一个更准确的公式RateAxzAvg RateAxzN RateAxzN-1/ 2Axz(n) Axz(n-1) RateAxzAvg * T同理可得Ayz(n) Ayz(n-1) RateAyz(n) * T好了现在我们有了AxznAyzn。现在我们如何推导出RxGyro/RyGyro?根据
公式1
我们可以把Rgyro长度写成下式| Rgyro | SQRTRxGyro ^ 2 RyGyro ^ 2 RzGyro ^ 2同时因为我们已经将Racc标准化我们可以认为它的长度是1并且旋转后保持不变所以写成下面的方式相对比较安全| Rgyro | 1我们暂时采用更短的符号进行下面的计算x RxGyro , yRyGyro, zRzGyro根据上面的关系可得x x / 1 x / SQRT(x^2y^2z^2)分子分母同除以SQRTX ^ 2 Z ^ 2x ( 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 )提示这个公式还可以更进一步简化。分式两边同除以sinaxz你可得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))^2 RxGyro 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).其中当 RzGyro0时Sign(RzGyro) 1 , 当 RzGyro0时Sign(RzGyro) -1 。一个简单的估算方法Sign(RzGyro) Sign(RzEst(n-1))在实际应用中当心RzEstn-1趋近于0。这时候你可以跳过整个陀螺仪阶段并赋值RgyroRestn-1。Rz可以用作计算Axz和Ayz倾角的参考当它趋近于0时它可能会溢出并引发不好的后果。这时你会得到很大的浮点数据并且tan()/atan()函数得到的结果会缺乏精度。现在我们回顾一下已经得到的结果我们在算法中的第n步并计算出了下面的值Racc – 加速度计读取的当前值Rgyro –根据Rest(-1)和当前陀螺仪读取值所得我们根据哪个值来更新Restn呢你可能已经猜到两者都采用。我们会用一个加权平均值得Rest(n) (Racc * w1 Rgyro * w2 ) / (w1 w2)分子分母同除以w1公式可简化成Rest(n) (Racc * w1/w1 Rgyro * w2/w1 ) / (w1/w1 w2/w1)令w2w1wGyro可得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现在可以再次进行下一轮循环了。
注关于此算法的具体实现和测试请阅读这篇文章 http://starlino.com/imu_kalman_arduino.html 加速度计和陀螺仪IMU融合的其他资源 http://www.mikroquad.com/pub/Res ... ryFilter/filter.pdf http://stackoverflow.com/questio ... -accelerometer-data http://www.dimensionengineering.com/accelerometers.htm
