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

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电子网】欢迎添加关注!文章转载请注明出处。

收藏 人收藏
分享:

评论

相关推荐

AD7694电路及串口传输

AD的精英们,我有问题需要解决,麻烦帮我看看 ! 电路目的:将陀螺仪的输出电压进行AD转换发送给ARM 陀螺仪:ADXRS...

发表于 12-11 10:35 8次 阅读
AD7694电路及串口传输

LSM9DS0陀螺仪和磁力计有哪些?

LSM9DS0数据表似乎只提到了加速度计的准确性!?!? 那么陀螺仪和磁力计有哪些?请解释!! 以上来自于谷歌翻译...

发表于 12-05 16:16 19次 阅读
LSM9DS0陀螺仪和磁力计有哪些?

LSM9DS0陀螺仪和磁力计有哪些?

LSM9DS0数据表似乎只提到了加速度计的准确性!?!? 那么陀螺仪和磁力计有哪些?请解释!! 以上来自于谷歌翻译...

发表于 12-05 16:16 19次 阅读
LSM9DS0陀螺仪和磁力计有哪些?

手持陀螺仪传感器原理

  陀螺仪因其独特的性质,被广泛应用在导航系统(航空航海)、运动分析与控制、平台的稳定性检测与控制、滚动检测、转速控制、...

发表于 12-05 15:25 60次 阅读
手持陀螺仪传感器原理

LSM9DS1和LSM6DS3加速度计分辨率只有14位

你好 我在确认LSM9DS1中的加速度计和LSM6DS3的分辨率是否实际为16位时遇到了一些麻烦? 两个传感器的数据表...

发表于 12-04 16:20 32次 阅读
LSM9DS1和LSM6DS3加速度计分辨率只有14位

加速度传感器在高铁中的作用

         加速度传感器负责对动车组的失稳、车厢舒适度和走行部件健康状态进行检测,广泛应用于轨...

发表于 12-03 16:33 103次 阅读
加速度传感器在高铁中的作用

LIS3DH加速度计自我测试失败

我们有一个LIS3DH加速度计,最终处于闩锁状态。输出数据为-205 / -1149 / -4759(X / Y / Z,mg)。当进行自测试时,2个...

发表于 12-03 09:46 12次 阅读
LIS3DH加速度计自我测试失败

LIS3DH加速度计在低功耗模式下的灵敏度

你好, 我打算在低功耗模式下操作LIS3DH加速度计(根据数据表中的表26,输出数据速率为5kHz)。 在数据表的表3中给出...

发表于 11-30 14:31 17次 阅读
LIS3DH加速度计在低功耗模式下的灵敏度

LIS3DH峰值峰值低

我使用LIS3DH测量机器的振动,我知道mems加速度计通常不用于此应用,但是,我设置了高分辨率模式,这给了我150Hz的带宽。我...

发表于 11-30 14:28 17次 阅读
LIS3DH峰值峰值低

下载不了,但是编译没问题

最近在做陀螺仪,可能数据类型的问题,总是出现这种错误,希望大家可以帮帮忙解决一下,谢谢!!!!...

发表于 11-30 09:58 123次 阅读
下载不了,但是编译没问题

加速度传感器在高铁中的作用

“要保证动车组高速平稳行驶,首先要由加速度传感器对来自X(轴)、Y(径)和Z(垂)向的应力进行实时监....

发表于 11-28 15:39 280次 阅读
加速度传感器在高铁中的作用

陀螺仪怎么用

陀螺的校准比较简单,一般上电后,自己执行即可,然后保存这个零偏,另每次上电得到的零偏都不同,所以需要....

的头像 发烧友学院 发表于 11-24 10:11 385次 阅读
陀螺仪怎么用

陀螺仪是什么

陀螺仪是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置。利用其....

的头像 发烧友学院 发表于 11-24 10:03 1262次 阅读
陀螺仪是什么

采用PSoC芯片CY29666-24PVXI进行光纤陀螺脉冲输出的采集设计

陀螺仪是一种测量运动物体相对惯性空间旋转的装置。绕一个支点高速转动的刚体称为陀螺(top)。通常所说....

的头像 电子设计 发表于 11-21 07:51 1155次 阅读
采用PSoC芯片CY29666-24PVXI进行光纤陀螺脉冲输出的采集设计

意法半导体推出超低功耗和高分辨率可互换的IIS2DLPC 3轴MEMS加速度计

2018年11月12日 - 意法半导体推出IIS2DLPC 3轴MEMS加速度计,可以在超低功耗和高....

的头像 人间烟火123 发表于 11-13 12:36 2432次 阅读
意法半导体推出超低功耗和高分辨率可互换的IIS2DLPC 3轴MEMS加速度计

加速度传感器的原理与应用解析

有一种特殊的材料,叫压电陶瓷材料,这种材料制备成的加速度传感器可根据作用在上面的力的大小产生不同的形....

发表于 11-12 10:06 274次 阅读
加速度传感器的原理与应用解析

美国科学家研制出全球最小的光学陀螺仪

美国科学家研制出了迄今全球最小的光学陀螺仪,其个头比一粒米还小,仅为目前最尖端光学陀螺仪大小的1/5....

的头像 MEMS 发表于 11-07 09:03 523次 阅读
美国科学家研制出全球最小的光学陀螺仪

平衡环陀螺仪是什么

平衡环(早期的陀螺仪)。平衡环,或称平衡陀,是现代陀螺仪的基础。在欧洲又称为卡丹环, 以意大利数学家....

的头像 发烧友学院 发表于 11-05 16:32 563次 阅读
平衡环陀螺仪是什么

旋转机械利用NI/PXI平台进行工作模态测试

工作状态模态分析(Operational Modal Analysis)是从来自于工作运行状态的结构....

的头像 电子设计 发表于 11-04 10:28 291次 阅读
旋转机械利用NI/PXI平台进行工作模态测试

传感器在消防中有着怎样的作用

在消防人员定位设备中,目前多采用加速度传感器、陀螺仪、磁传感器和气压传感器等感应元器件,以惯性运动测....

发表于 11-02 14:58 197次 阅读
传感器在消防中有着怎样的作用

L3G4200D三轴数字输出陀螺仪的详细数据手册免费下载

L3G4200D是一种低功耗三轴陀螺仪,提供三种不同的可供选择的全量程(±250/±500/±200....

发表于 10-26 15:34 89次 阅读
L3G4200D三轴数字输出陀螺仪的详细数据手册免费下载

运动传感器的详细介绍包括:加速度计,PIR,振动传感器等

了解各种运动型传感器,包括加速度计,倾斜传感器,振动传感器,PIR(被动红外)传感器和旋转传感器。

发表于 10-25 17:38 86次 阅读
运动传感器的详细介绍包括:加速度计,PIR,振动传感器等

带你看看高精度陀螺仪有哪些

陀螺仪的另一种叫法又称角速度传感器,从定义上来看陀螺仪是测量载体角运动或者角速度的传感器从应用的角度....

发表于 10-23 17:34 687次 阅读
带你看看高精度陀螺仪有哪些

分析三轴陀螺仪的工作原理及应用

三轴陀螺仪也叫作微机械陀螺仪,而微机械陀螺仪也会被称作MEMS陀螺仪。它的特点在于能够同时进行六个方....

发表于 10-23 11:16 348次 阅读
分析三轴陀螺仪的工作原理及应用

解答陀螺仪在手机中有什么作用/手机里都有陀螺仪吗

现如今,智能手机不断发展,智能手机已经成为了人们日常生活中很普通的消费品。新款的手机不断推出,人们更....

发表于 10-23 10:10 469次 阅读
解答陀螺仪在手机中有什么作用/手机里都有陀螺仪吗

解答手机陀螺仪坏了怎么办/如何检测陀螺仪是否坏了

陀螺仪其实通常被叫做角速度传感器,是通过测量物理量的偏转与倾斜的转动角速度来进行判定的。通过高速回转....

发表于 10-23 09:43 2030次 阅读
解答手机陀螺仪坏了怎么办/如何检测陀螺仪是否坏了

解答vr陀螺仪是什么

随着VR技术的愈渐成熟,目前各大城市的商业广场里、步行街当中都能看到VR体验店,都愿意进去体验一把V....

发表于 10-23 09:24 582次 阅读
解答vr陀螺仪是什么

利用射线光学仿真揭秘飞行器陀螺仪的工作原理

在本文中,我们将利用射线光学仿真,观察简单萨格纳克干涉器中的萨格纳克效应。

的头像 COMSOL 发表于 10-18 15:40 597次 阅读
利用射线光学仿真揭秘飞行器陀螺仪的工作原理

智能手机上的七大传感器的用途及原理解析

智能手机指南针功能是利用地磁场与手机内置地磁传感器,来实现地理方向定位的,指南针指针方向有变化,说明....

发表于 10-16 16:29 522次 阅读
智能手机上的七大传感器的用途及原理解析

传感器是物联网世界的主要感官部分

传感器是物联网世界的眼睛、耳朵、鼻子和触觉。可用作温度计、陀螺仪、加速度计、压力传感器、光传感器、磁....

发表于 10-12 15:18 306次 阅读
传感器是物联网世界的主要感官部分

传感器的研发过程将呈现两阶段的趋势

“MEMS是目前世界制造业的热点,MEMS以其微型化的优势,在加速度计、陀螺仪、光学MEMS、图像传....

发表于 10-12 15:00 435次 阅读
传感器的研发过程将呈现两阶段的趋势

关于加速传感器原理的介绍及应用领域

加速传感器顾名思义就是一种能够测量出产品加速度的一种设备。生活中我们一般常用的主要是两种加速度计,分....

发表于 10-09 16:24 238次 阅读
关于加速传感器原理的介绍及应用领域

加速度计在温度变化及振动条件下是否仍然可以实现这样的测量精度?

如表3所示,振动校正误差(VRE)是加速度计暴露于宽带振动时引入的失调误差。当加速度计暴露于振动环境....

的头像 MEMS技术 发表于 09-30 17:10 611次 阅读
加速度计在温度变化及振动条件下是否仍然可以实现这样的测量精度?

如何提高无人机飞行感知模块测量精度的方法

无人机的飞行感知技术主要用作两个用途,其一是提供给飞行控制系统,由于飞行控制系统的主要功能是控制飞机....

的头像 玩转单片机 发表于 09-20 10:01 2332次 阅读
如何提高无人机飞行感知模块测量精度的方法

陀螺仪制作方法及使用方法

很好玩,欢迎仿制,结构方向都是我自己设计的。

的头像 39度创意研究所 发表于 09-13 11:15 1147次 阅读
陀螺仪制作方法及使用方法

GyroSphere MEMS随钻陀螺服务介绍

“GyroSphere服务不需要额外的钻机时间,只需通过启用陀螺仪测量,因此大大提高了运行效率。”S....

的头像 MEMS 发表于 09-11 09:30 1124次 阅读
GyroSphere MEMS随钻陀螺服务介绍

MEMS传感器的出货量预计将在2018年增长11%

另外,2018年,MEMS执行器预计将带来额外的59亿美元营收。这些MEMS执行器使用其微机电换能器....

的头像 MEMS 发表于 09-11 09:01 2789次 阅读
MEMS传感器的出货量预计将在2018年增长11%

测量角速率的MEMS陀螺仪有多个误差贡献因素

引起陀螺仪漂移的主要原因是两个因素的结合:一个是慢速变化的接近DC的变量,称为偏置不稳定性;另一个是....

的头像 电子工程专辑 发表于 09-07 14:20 1047次 阅读
测量角速率的MEMS陀螺仪有多个误差贡献因素

手环加速度传感器原理及选型技巧

电容式加速度传感器是基于电容原理的极距变化型的电容传感器。电容式加速度传感器/电容式加速度计是对比较....

发表于 09-03 15:03 280次 阅读
手环加速度传感器原理及选型技巧

加速度传感器测量信号失真的原因和解决办法

这有两方面的原因:加速度传感器低频响应的截至频率不够低;与传感器配套使用的恒流电压源或电荷放大器的截....

发表于 08-24 17:37 263次 阅读
加速度传感器测量信号失真的原因和解决办法

汽车加速度传感器常见故障

空气流量传感器常见故障之一:发动机怠速不稳定,局部会出现负荷冒黑烟状况,并且会有换挡熄火情况出现;故....

发表于 08-24 17:16 744次 阅读
汽车加速度传感器常见故障

加速度传感器测速原理及电路图

加速度传感器自然是对自身器件的加速度进行检测。其自身的物理实现方式咱们就不去展开了,可以想象芯片内部....

发表于 08-24 16:36 1577次 阅读
加速度传感器测速原理及电路图

横向加速度传感器原理

横向加速度传感器的测量原理基于带电容探测器的偏移/延伸和质量系统,电压通过ESP控制单元提供,转弯过....

发表于 08-24 16:08 650次 阅读
横向加速度传感器原理

振动加速度传感器原理图与应用

振动加速度传感器主要是用于测量轴承的振动,个别情况下也会用于测量转轴振动,它主要是安装在各种旋转机....

发表于 08-24 15:46 664次 阅读
振动加速度传感器原理图与应用

光纤加速度传感器原理及应用

直射式光纤传感器,原理就是利用光纤作为光信号的传输通道,直射式出射光纤发出光,接收光纤接收光信号,接....

发表于 08-24 15:12 572次 阅读
光纤加速度传感器原理及应用

重力加速度传感器原理及应用

重力传感器简称GV-sensor,输出重力数据。 在地球上,重力数值为9.8,单位是m/s^2。....

发表于 08-24 14:26 519次 阅读
重力加速度传感器原理及应用

纵向加速度传感器工作原理介绍

车身纵向加速度传感器的作用是检测车辆起步加速或制动减速时车辆发生“抬头”或“点头”的现象,以控制前、....

发表于 08-24 11:56 542次 阅读
纵向加速度传感器工作原理介绍

差动变压器式加速度传感器原理分析

差动变压器加速度传感器基本原理是当压力信号作用与传感器时,压力传感器将压力信号转换成电信号,经差分放....

发表于 08-24 11:25 516次 阅读
差动变压器式加速度传感器原理分析

什么是弱加速度传感器 作用是什么

它可 以将测出车的抖动信号传给ECU,并经三次运算确定是发动机某 缸熄火引起发动机抖动还是地面不平引....

发表于 08-24 10:22 348次 阅读
什么是弱加速度传感器 作用是什么

MEMS加速度计和陀螺仪原理分析

从上面的公式中,我们可以看到科里奥利加速度的大小是正比于物体旋转的角速度。如果我们在切向安装一个加速....

的头像 电子工程专辑 发表于 08-14 09:03 1394次 阅读
MEMS加速度计和陀螺仪原理分析

以压力传感器和加速度传感器为核心的防撞预警系统为大桥安全运行保驾护航

据介绍,桥上装的工控机可控制激光光束机、摄像头、压力传感器、加速度传感器、LED显示屏以及警示灯,来....

发表于 08-10 17:41 665次 阅读
以压力传感器和加速度传感器为核心的防撞预警系统为大桥安全运行保驾护航

因加速度传感器和压力传感器存在隐患,部分埃尔法被召回

上述召回范围内部分车辆的安全气囊控制系统中,由于加速度传感器或压力传感器内IC芯片的生产条件不恰当,....

发表于 08-10 17:32 190次 阅读
因加速度传感器和压力传感器存在隐患,部分埃尔法被召回

九轴传感器是什么?九轴传感器在VR中有什么应用?

九轴传感器已经被应用于多哚H1等高端手机VR产品中,区别于市面上几十元的Carboard类玩具版VR....

发表于 08-07 16:24 792次 阅读
九轴传感器是什么?九轴传感器在VR中有什么应用?

如何将3D加速计和陀螺仪组合使用

加速计和陀螺仪是获取无人机、蜂窝电话、汽车、接收机和移动物联网设备中加速和旋转信息的传感器选择。但加....

的头像 电子设计 发表于 08-07 09:49 1942次 阅读
如何将3D加速计和陀螺仪组合使用

基于加速度传感器计算物体垂直平面上的倾斜角度

由于加速度传感在静止放置时受到重力作用,因此会有1g的重力加速度。利用这个性质,通过测量重力加速度在....

的头像 摄像头观察 发表于 07-29 10:00 1046次 阅读
基于加速度传感器计算物体垂直平面上的倾斜角度