航天科工【转】【翻译】加速度计和陀螺仪指南

源地址:http://www.geek-workshop.com/thread-1695-1-1.html

本帖翻译由IMU(加速度计和陀螺仪设备)在嵌入式应用中动用的指南。
即首文章要介绍加速度计和陀螺仪的数学模型和主导算法,以及哪融合这片者,侧重算法、思想的座谈

介绍
遵循指南旨在向兴趣者介绍惯性MEMS(微机电系统)传感器,特别是加快度计和陀螺仪和其它整合IMU(惯性测量单元)设备。
航天科工 1
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自由度的惯性测量单元。这是只花哨的名!然而,在鲜艳的讳背后是个坏管用的装备做,接下去我们会详细介绍的。
    第一片段 加速度计
    倘了解是模块我们先从加快度计开始。当我们以设想一个加速度计的时我们得以把它想发一个圆球在一个方盒子中。你可能会见管它们想发一个饼干或甜圈,但自己就算把她当作一个圆球好了:
    航天科工 2
    咱们若这个盒子不在地心引力场中要其它任何会影响球的职务的集被,球处于盒子的刚刚中央。你可想像盒子在他太空中,远离任何天体,如果大麻烦想象,那便作为
    盒子在航天飞机中,一切事物都处于无重力状态。在面的觊觎备受公可看来咱们深受每个轴分配了一对墙(我们移除了Y+以这个来考察中的景)。设想每面墙都能够
    感测压力。如果我们忽然把盒子向左移动(加速度为1g=9.8m/s^2),那么球会撞上X-墙。然后我们检测球撞击墙面产生的压力,X轴输出值为
    -1g。
    航天科工 3
    告留意加速度计检测及能的取向和她自身加速度之自由化是倒转的。这种能力便被称为放纵性力或假想力
    。在是模型中君乃应该学到加速度计是经过间接测量力对一个墙面的意图来测量加速度之,在实际上采用被,可能通过弹簧等设置来测量力。这个力量可是加速度引起的,但于脚的例证中,我们见面发觉其不自然是加速度引起的。
    倘我们将模型在地球上,球会沾于Z-墙面上并针对性该施加一个1g的能力,见下图:
    航天科工 4

    在这种状态下盒子没挪动但我们任然读取到Z轴有-1g的值。球在墙及栽的下压力是由于引力造成的。在理论及,它可是殊种类的力

    诸如,你可想象我们的球体是铁质的,将一个磁铁放在盒子旁边那球就会碰到上任何一样对墙壁。引用这例子只是为说明加速度计的真相是检测力而不加速度。只是加快
    度所引起的惯性力正好能吃加速度计的检测装置所捕获。
    尽管如此是模型并非一个MEMS传感器的真构造,但它用来化解与加速度计相关的问题相当有效。实际上有些看似传感器中生出金属小球,它们叫倾角开关,但是她的效益更弱,只能检测设备是否在得水准内倾斜,却休克获倾斜的水平。
    交目前为止,我们早就分析了单轴的加快度计输出,这是采取单轴加速度计所能够获取的。三轴加速度计的真的价值在它们能够检测全叔单轴的惯性力。让咱们回来盒子模型,并以盒子向右侧旋转45过。现在球会与个别个当接触:Z-和X-,见下图:
    航天科工 5
    0.71g是价值是勿是擅自的,它们其实是1/2的平方根的靠近似值。我们介绍加速度计的生一个模时马上同一接触见面再次亮。
    当达标一个型中我们引入了重力并转了盒子。在末之有数只例中我们分析了盒子在少数栽状况下之输出值,力矢量保持无更换。虽然当时促进了解加速度计是怎么与外部力相互作用的,但倘若我们拿坐标系换呢加速度之老三个轴并设想矢量力在四周转悠,这会另行便利计算。
    航天科工 6
    告看当方的型,我保留了轴的水彩,以便你的思辨能重好的从达成一个模子转至新的型中。想象新模型中每个轴都分别垂直于原来模中分头的墙面。矢量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 = 586
    AdcRy = 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.24V
    DeltaVoltsRy = 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 /Sensitivity
    RX = 0.24V / 0.4785V / G =~0.5g
    RY = 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的价值,让咱返回我们的齐一个加速度型,再加有标号上去:
    航天科工 7
    咱俩感谢兴趣的角度是往量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 / R
    cosY = cos(Ayr) = Ry / R
    cosZ = cos(Azr) = Rz / R
    立刻三独公式通常称为方向余弦
    ,它主要表达了单位向量(长度也1之向量)和R向量具有同等的势头。你可以很轻地说明:
    SQRT(cosX ^ 2 + COSY ^ 2 + cosZ ^ 2)= 1
    眼看是独好好的习性,因为它们避免了俺们直接检测R向量的模型(长度)。通常如果我们只是对惯性力的取向感兴趣,那极模长以简化其他计量是单神的选料。
    次片陀螺仪
    对此陀螺仪我们将未会见如加速度计一样介绍她的对等盒子模型,而是一直跨越到加快度计的亚个模型,通过这模型我们见面往大家介绍陀螺仪是怎么工作之。
    航天科工 8
    陀螺仪的每个通道检测一个幅的旋转。例如,一个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
    公式3
    RateAyz = (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 = 571
    AdcGyroXZ = 323
    故者的公式,在代入Acc
    Gyro死的参数,可得:
    RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s
    RateAyz = (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轴方向:
航天科工 9
连下去的手续是:

  • 规定陀螺仪的出口对应到上述议论的RateAxz,RateAyz值。
  • 冲陀螺仪和增速度计的职决定是否如反转输出值
    不用设想陀螺仪陀的出口有XY,它会适应加速度计坐标系里的任何轴,尽管是输出是IMU模块的均等部分。最好的道尽管是测试。
    搭下去的言传身教用来确定谁陀螺仪的出口对应RateAxz。
  • 首先将装备保障水平。加速度计的XY轴输出会是零加速度电压([Acc

    Gyro](http://gadgetgangster.com/find-a-project/56?projectnum=213)板的值是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) / Sensitivity
    RyAcc = (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开始。
    航天科工 10
    首先观察陀螺仪模型中下面的关联,根据由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)内之角度。更多信息要阅读
    atan2.
    之所以,知道了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^2
    x = 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)/R
RyEst(n) = RyEst(n)/R
RzEst(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

发表评论

电子邮件地址不会被公开。 必填项已用*标注