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

2018-03-13 09:12 次阅读

这篇文章主要介绍加速度计和陀螺仪的数学模型和基本算法,以及如何融合这两者,侧重算法、思想的讨论.

介绍

本指南旨在向兴趣者介绍惯性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))^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).

其中,当 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现在,可以再次进行下一轮循环了。

热门推荐

原文标题:考验数学水平的时候到了!加速度计和陀螺仪指南

文章出处:【微信号:weixin21ic,微信公众号:21ic电子网】欢迎添加关注!文章转载请注明出处。

收藏 人收藏
分享:

评论

相关推荐

手机中的各种传感器你知道吗?

智能手机技术的发展速度快得令人难以想象,这其中就包含传感器技术。手机当中的传感器有能力让我们的生活方....

的头像 传感器技术 发表于 06-19 16:01 300次 阅读
手机中的各种传感器你知道吗?

MEMS初创公司SMG宣布完成700万美元融资

SMG是一家专注于创新技术开发和应用的私人控股公司,主要面向改善石油和天然气的监测、评估和生产,并延....

发表于 06-17 16:59 53次 阅读
MEMS初创公司SMG宣布完成700万美元融资

KVH发布基于光纤陀螺顶级IMU,可提供±10g或±25g加速度计的产品

1775 IMU(惯性测量单元)是KVH性能最高的光纤陀螺惯性测量单元,理想地适用于各种恶劣环境,外....

发表于 06-13 09:24 109次 阅读
KVH发布基于光纤陀螺顶级IMU,可提供±10g或±25g加速度计的产品

高性能MEMS加速度计低成本解决方案

导航和AHRS系统、机器健康状况检测的振动监控、基础设施的结构健康状况监控和平台稳定、井下定向钻探的....

的头像 电子设计 发表于 06-12 09:40 768次 阅读
高性能MEMS加速度计低成本解决方案

加速度传感器让人们避免电梯中的不适感

电梯是现代楼宇建筑中常见的一种运载设施,能够快速方便地将搭载者带到指定楼层,省时又省力。随着电梯在生....

发表于 06-07 11:00 47次 阅读
加速度传感器让人们避免电梯中的不适感

将ADXL355等低噪声加速度计与一款受欢迎的低功耗加速度计相比较

当传感器未在使用时,如果应用对传感器周期供电以节省电能,噪声与功耗的关系会大不相同。这种差异来源于建....

的头像 电机控制设计加油站 发表于 06-06 10:33 366次 阅读
将ADXL355等低噪声加速度计与一款受欢迎的低功耗加速度计相比较

看3D鼠标如何实现3D

我们大家知道传统的鼠标只能做到上下左右移动的功能,而当我们在玩游戏的时候例如大家不陌生的CS游戏,因....

发表于 06-06 10:20 115次 阅读
看3D鼠标如何实现3D

面向条件监测的MEMS加速度计产品演示

这款最新类型的MEMS加速度计具有低噪声、高工作频率性能,针对条件监测可实现全新的产品设计。MEMS....

的头像 EE techvideo 发表于 06-05 09:51 236次 观看
面向条件监测的MEMS加速度计产品演示

基于MPU6050加速度传感器的跌倒检测与报警系统设计

本文主要介绍了基于MPU6050加速度传感器的跌倒检测与报警系统设计。

发表于 06-05 08:00 23次 阅读
基于MPU6050加速度传感器的跌倒检测与报警系统设计

高要求应用中的精密惯性检测

ADI高性能MEMS IMU和陀螺仪能为高要求应用提供精确的方向跟踪,同时操作复杂的运动配置并可在苛....

的头像 EE techvideo 发表于 06-03 02:47 246次 观看
高要求应用中的精密惯性检测

根据手机的传感器数据,用于预测用户的运动模式

本例的数据来源于UCI(即UC Irvine,加州大学欧文分校)。数据由年龄在19-48岁之间的30....

发表于 06-02 09:57 117次 阅读
根据手机的传感器数据,用于预测用户的运动模式

机器人选择陀螺仪时喜欢使用的指标是漂移,这对机器人意味着什么?

然而,因为没有任何陀螺仪是十全十美的,所以这个方向会随时间而漂移。还记得我们如何获得实际方向的角度吗....

的头像 电机控制设计加油站 发表于 06-01 09:58 833次 阅读
机器人选择陀螺仪时喜欢使用的指标是漂移,这对机器人意味着什么?

如何正确的选择压电式加速度传感器 6大方面可以参考

压电加速度传感器因其频响宽、动态范围大、可靠性高、使用方便,受到广泛应用。在一般通用振动测量时,用户....

发表于 05-14 07:35 333次 阅读
如何正确的选择压电式加速度传感器 6大方面可以参考

走直线很难吗?陀螺仪表示,少了它机器人连直线都走不了

发表于 05-11 11:52 287次 阅读
走直线很难吗?陀螺仪表示,少了它机器人连直线都走不了

惯性导航的工作原理及惯性导航系统分类

惯性导航系统根据陀螺仪的不同,可分为机电(包含液浮、气浮、静电、 挠性等种类)陀螺仪、光学(包含激光....

的头像 传感器技术 发表于 05-10 17:09 1251次 阅读
惯性导航的工作原理及惯性导航系统分类

MEMS陀螺仪的简要介绍(性能参数和使用)

MEMS传感器市场浪潮可以从最早的汽车电子到近些年来的消费电子,和即将来到的物联网时代。如今单一的传....

发表于 05-08 16:25 538次 阅读
MEMS陀螺仪的简要介绍(性能参数和使用)

MEMS加速度计是什么 加速度计工作原理

加速度计是一种惯性传感器,能够测量物体的加速力。加速力就是当物体在加速过程中作用在物体上的力,就比如....

发表于 05-08 12:42 543次 阅读
MEMS加速度计是什么 加速度计工作原理

通过不同加速度传感器的原理、制作工艺及应用展开全面了解

MEMS压阻式加速度传感器的敏感元件由弹性梁、质量块、固定框组成。压阻式加速度传感器实质上是一个力传....

的头像 传感器技术 发表于 05-03 16:55 772次 阅读
通过不同加速度传感器的原理、制作工艺及应用展开全面了解

陀螺仪航向的漂移误差可能会随时间推移而累积吗?

测量角速率的MEMS陀螺仪有多个误差贡献因素,偏置不稳定性是其中之一。然而,与提供增强性能的分立器件....

的头像 玩转单片机 发表于 05-03 11:19 1008次 阅读
陀螺仪航向的漂移误差可能会随时间推移而累积吗?

智腾微电子自主研发SiA200系列MEMS加速度计

SiA200系列是青岛智腾微电子自主研发的MEMS加速度计,SiA200系列是一种的电容体的MEMS....

的头像 MEMS 发表于 05-02 17:54 1671次 阅读
智腾微电子自主研发SiA200系列MEMS加速度计

惯性定位如何解决陀螺仪漂移和磁场干扰问题?

针对在导航系统姿态解算中,陀螺仪和电子罗盘在解算姿态时分别存在积分误差和磁场干扰的问题,提出了利用K....

发表于 05-02 08:27 546次 阅读
惯性定位如何解决陀螺仪漂移和磁场干扰问题?

状态监控中的MEMS加速度计的介绍和应用范围

如今的MEMS设备可包括大量集成、单电压信号调理电子器件,提供具有极低功耗的模拟和数字接口来帮助实现....

的头像 MEMS 发表于 04-30 11:57 376次 阅读
状态监控中的MEMS加速度计的介绍和应用范围

加速度传感器的技术要求和正确选型

传感器的灵敏度应根据被测振动量(加速度值)大小而定,但由于压电加速度传感器是测量振动的加速度值,而在....

的头像 传感器技术 发表于 04-28 10:42 847次 阅读
加速度传感器的技术要求和正确选型

加速度传感器种类和应用

发表于 04-28 10:04 415次 阅读
加速度传感器种类和应用

加速度传感器测量电路中的问题

发表于 04-26 20:38 390次 阅读
加速度传感器测量电路中的问题

MMA7455角速度模块简介

MMA7455数字三轴加速度传感器模块核心为飞思卡尔公司的MMA7455L数字三轴加速度传感器,该模....

的头像 畅学单片机 发表于 04-26 16:40 833次 阅读
MMA7455角速度模块简介

三分钟了解惯性导航系统、加速度计、陀螺仪的工作原理

惯性导航系统 INS( Inertia Navigation System ,以下简称惯导 ) 惯导....

发表于 04-26 09:06 1494次 阅读
三分钟了解惯性导航系统、加速度计、陀螺仪的工作原理

一种四阶多位量化微机械加速度计系统设计

该加速度计接口电路原型芯片采用0.35 μm标准CMOS工艺进行流片验证,如图4所示。芯片尺寸约为....

的头像 电子技术应用ChinaAET 发表于 04-20 08:56 502次 阅读
一种四阶多位量化微机械加速度计系统设计

Ameya360无人机视觉、高级传感与处理解决方案

Ameya360 无人机视觉、高级传感与处理解决方案主要有陀螺仪(飞行姿态感知),加速计,地磁感应,....

的头像 电子发烧友网工程师 发表于 04-18 09:50 1282次 阅读
Ameya360无人机视觉、高级传感与处理解决方案

可穿戴设备ALEX 让你时刻保持正确的坐姿

智能设备的普及让我们大多数人都变成了低头族,设备越来越简单化我们不用再局限在桌子上使用智能设备,因而....

的头像 39度创意研究所 发表于 04-17 10:41 618次 阅读
可穿戴设备ALEX 让你时刻保持正确的坐姿

基于PCI总线的卫星用陀螺仪测试系统设计与实现

卫星用陀螺仪是一种惯性测量装置,能够输出卫星三轴相对于惯性空间的角速率,经积分后可得到连续的姿态输出....

发表于 04-03 11:18 43次 阅读
基于PCI总线的卫星用陀螺仪测试系统设计与实现

有浅入深的介绍BLE与MEMS中的开发板设计

当BLE遇到MEMS,就如同天使有了翅膀。本期从有浅入深的介绍BLE与MEMS的那些事,就从ST的S....

的头像 MEMS技术 发表于 03-31 08:44 1983次 阅读
有浅入深的介绍BLE与MEMS中的开发板设计

计步器是计步工作原理解析

计步器到底是怎么工作的呢?是怎么知道我们每天走了多少步的呢? 记步神器:雷布斯小米手环2 中国电子科....

发表于 03-26 10:37 774次 阅读
计步器是计步工作原理解析

基于MEMS传感器的行人航位推算(PDR)解决方案

前言定位是感知应用的一个重要属性。在室内环境中,如果位置信息可用并非常可靠,有更多的应用场景可以实现....

的头像 传感器技术 发表于 03-16 09:02 3181次 阅读
基于MEMS传感器的行人航位推算(PDR)解决方案

如何用加速度计提高倾角测量精度(含详细计算过程)

倾角测量的典型应用 本文旨在探讨如何用组合器件一类的加速度计提高倾角测量的精度。在乘用车上,电动驻车....

的头像 模拟对话 发表于 03-15 17:16 4630次 阅读
如何用加速度计提高倾角测量精度(含详细计算过程)

几个步骤更简单设计低功耗9轴运动跟踪功能(含程序例程)

越来越多的可穿戴设备和物联网设计将检测方向和跟踪运动的能力视为重要要求。尽管已经有各种各样的可用运动....

发表于 03-15 09:48 730次 阅读
几个步骤更简单设计低功耗9轴运动跟踪功能(含程序例程)

双框架硅微型角振动陀螺仪再平衡控制方案研究

双框架式角振动陀螺仪是利用振动质量块在被基座(仪表壳体)带动旋转时的哥氏效应来测量角速度。双框架硅微....

发表于 03-10 11:15 106次 阅读
双框架硅微型角振动陀螺仪再平衡控制方案研究

ADXL203加速度计现在哪里有卖的呢?

发表于 03-09 21:37 299次 阅读
ADXL203加速度计现在哪里有卖的呢?

了解IMU带宽及其在系统稳定性中的应用

无人飞行器安装的监控设备、海上微波接收机、车辆安装的红外成像系统传感器以及其他仪器系统都需要具有稳定....

的头像 Excelpoint世健 发表于 03-06 08:26 4935次 阅读
了解IMU带宽及其在系统稳定性中的应用

基本平台稳定系统中MEMS IMU频率响应分析

无人飞行器安装的监控设备、海上微波接收机、车辆安装的红外成像系统传感器以及其他仪器系统都需要具有稳定....

的头像 亚德诺半导体 发表于 03-01 09:11 2706次 阅读
基本平台稳定系统中MEMS IMU频率响应分析

评估加速计和陀螺仪,全方位了解噪音和漂移误差

加速计和陀螺仪是用于从无人机、手机、汽车、飞机和移动物联网设备中获取加速度和旋转信息的传感器。但加速....

的头像 Duke 发表于 02-27 11:41 1944次 阅读
评估加速计和陀螺仪,全方位了解噪音和漂移误差

MEMS陀螺仪可否取代光纤陀螺仪技术

光纤陀螺仪(FOG)以前曾经是环形激光陀螺仪(RLG)等其他技术的低成本替代品,现在该技术面临着新的....

发表于 02-24 17:24 407次 阅读
MEMS陀螺仪可否取代光纤陀螺仪技术

基于ADIS16255 MEMS芯片陀螺仪的应用研究

陀螺仪是能够精确地确定运动物体方位的仪器,它在现代航空,航海,航天等国防工业以及汽车、转台、工业生产....

发表于 02-24 09:54 176次 阅读
基于ADIS16255 MEMS芯片陀螺仪的应用研究

基于加速度传感器阵列定位的无线高亮LED灯光控制系统

1.1赛题来源及科学意义 随着社会的不断进步与科学技术的不断发展,人们对我们赖以生存的地球投入了越来....

发表于 02-20 22:08 163次 阅读
基于加速度传感器阵列定位的无线高亮LED灯光控制系统

加速度传感器存在安全隐患 丰田计划2月10日起召回18万辆汽车

据报道,近日丰田决定自2018年2月10日起紧急召回18万辆汽车,全新汉兰达汽车共计73084辆;雷....

发表于 02-13 11:14 1765次 阅读
加速度传感器存在安全隐患 丰田计划2月10日起召回18万辆汽车

基于Fuzzy-PID的陀螺仪温度控制系统设计

陀螺仪是舰船上的重要组成部件,其性能的稳定对于舰船的控制至关重要。将Fuzzy-PID算法应用于陀螺....

发表于 02-10 13:32 279次 阅读
基于Fuzzy-PID的陀螺仪温度控制系统设计

加速度传感器芯片有哪些_常见MEMS加速度传感器芯片盘点

加速度传感器是一种能够测量加速度的传感器。通常由质量块、阻尼器、弹性元件、敏感元件和适调电路等部分组....

的头像 陈翠 发表于 02-05 11:47 3817次 阅读
加速度传感器芯片有哪些_常见MEMS加速度传感器芯片盘点

加速度传感器是什么_常用加速度传感器有哪几种分类

加速度传感器是一种能够测量加速度的传感器。通常由质量块、阻尼器、弹性元件、敏感元件和适调电路等部分组....

发表于 02-05 11:28 1505次 阅读
加速度传感器是什么_常用加速度传感器有哪几种分类

选择了合适的传感器,才能更好地解决机器人的移动

总而言之,机器人走直线或者像上图一样扭腰,看起来像是一个很简单的事情,但是影响因素有很多,每一步的实....

的头像 亚德诺半导体 发表于 02-03 12:26 1447次 阅读
选择了合适的传感器,才能更好地解决机器人的移动

日立新型MEMS加速度计比商用MEMS加速度计灵敏度跟高,且功耗降低50%

日立通过在MEMS质量块上制作贯通孔来降低空气阻力,并且贯通孔的直径在上层和下层之间是变化的。上层为....

的头像 MEMS 发表于 01-29 16:04 1035次 阅读
日立新型MEMS加速度计比商用MEMS加速度计灵敏度跟高,且功耗降低50%

为什么arduino uno只能每秒读传感器80次?

发表于 11-27 03:16 868次 阅读
为什么arduino uno只能每秒读传感器80次?

Matlab 能读 Arduino监视窗口的数据吗?

发表于 11-20 08:03 633次 阅读
Matlab 能读 Arduino监视窗口的数据吗?

急!!Matlab 能读 Arduino监视窗口的数据吗?

发表于 11-20 08:02 686次 阅读
急!!Matlab 能读 Arduino监视窗口的数据吗?

请教关于VISA串口读取数据时,出现严重延时的问题

发表于 11-17 04:22 950次 阅读
请教关于VISA串口读取数据时,出现严重延时的问题

无线加速度传感器

发表于 10-25 21:09 256次 阅读
无线加速度传感器

加速计

发表于 09-18 10:42 421次 阅读
加速计