多傳感器融合算法
多傳感器數(shù)據(jù)融合算法---9軸慣性傳感器 - hmmwjs的博客 - CSDN博客
加速度計(jì)模型
每個(gè)ADC模塊都有一個(gè)參考電壓驯杜，假設(shè)它是3.3V。要將一個(gè)10位的ADC值轉(zhuǎn)成電壓值济欢，使用下列公式：? ?VoltsRx = AdcRx * VREF / 1023（小注：8位ADC的最大值是255 = 2 ^ 8 -1，12位ADC最大值是4095 = 2 ^ 12 -1）。每個(gè)加速度計(jì)都有一個(gè)零加速度的電壓值稱(chēng)為VzeroG挡鞍，你可以在它的說(shuō)明書(shū)中找到，這個(gè)電壓值對(duì)應(yīng)于加速度為0g预烙，利用公式DeltaVoltsRx = VoltsRx - VzeroG 可以得到相對(duì)0g電壓點(diǎn)的偏移量墨微。
在得到了加速度計(jì)的電壓值后，利用加速度計(jì)的靈敏度Sensitivity可將電壓值轉(zhuǎn)化成為單位為g（9.8m/s^2）使用公式為RX = DeltaVoltsRx /Sensitivit可以獲得每個(gè)方向的加速度值扁掸。
綜合上述公式翘县，即可得到怎么讀取一個(gè)ADC值并將其轉(zhuǎn)換為單位為g的矢量力的分量。
Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity?
Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity谴分。
利用反三角函數(shù)計(jì)算向量R在每個(gè)軸上的角度：Axr = arccos(Rx/R)锈麸，Ayr = arccos(Ry/R)，Azr = arccos(Rz/R)牺蹄。
陀螺儀模型
陀螺儀測(cè)試向量R與各軸之間的角度的變化率
陀螺儀輸出值即AdcGyroXZ（由ADC模塊得到）轉(zhuǎn)換中的ADC部分（假設(shè)使用10位ADC模塊忘伞，如果是8位ADC，用1023代替255沙兰，如果是12為ADC用4095代替1023）氓奈，將其單位轉(zhuǎn)換成為度/秒，公式為RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity鼎天。其中VzeroRate 是零變化率電壓舀奶，換句話說(shuō)它是陀螺儀不受任何轉(zhuǎn)動(dòng)影響時(shí)的輸出值，通逞荡耄可以由說(shuō)明書(shū)得到伪节，需要校準(zhǔn)，而Sensitivity為陀螺儀的靈敏度绩鸣，單位為mV/deg/s,它的意思就是如果旋轉(zhuǎn)速度增加1°/s,陀螺儀的輸出就會(huì)增加多少mV怀大。
例子：假設(shè)我們的ADC模塊返回以下值：AdcGyroXZ = 571，AdcGyroXZ = 323
用上面的公式呀闻，在代入Acc Gyro板的參數(shù)化借，可得：
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
即此設(shè)備繞Y軸（也可以說(shuō)在XZ平面內(nèi)）以306°/s速度和繞X軸（或者說(shuō)YZ平面內(nèi)）以-94°/s的速度旋轉(zhuǎn)。
加速度計(jì)和陀螺儀融合算法
1.統(tǒng)一坐標(biāo)軸
一般將加速度計(jì)作為參考坐標(biāo)
2. 確定陀螺儀的輸出對(duì)應(yīng)到RateAxz捡多，RateAyz的值
3.根據(jù)陀螺儀和加速度計(jì)的位置決定是否要反轉(zhuǎn)輸出值
4.算法處理
由于加速度計(jì)和陀螺儀會(huì)受到許多因素的干擾蓖康，所以在使用時(shí)要利用一定的濾波算法來(lái)處理噪聲铐炫。由第一部分計(jì)算得到的數(shù)據(jù)容易受到噪聲的干擾，稱(chēng)之為Racc – 是由加速度計(jì)測(cè)量到得慣性力矢量蒜焊，它可分解為下面的分量RxAcc倒信，RyAcc，RzAcc泳梆。
這組數(shù)據(jù)是只來(lái)自于加速度計(jì)ADC的值鳖悠，我們把這組數(shù)據(jù)叫做“vector”，并使用下面的符號(hào)：Racc = [RxAcc,RyAcc,RzAcc]优妙。我們引進(jìn)一個(gè)新的變量Rest = [RxEst,RyEst,RzEst]來(lái)作為算法計(jì)算的值乘综，它是經(jīng)過(guò)陀螺儀數(shù)據(jù)的修正和基于上一次估算的值。
開(kāi)始時(shí)規(guī)定加速度計(jì)輸出為準(zhǔn)確值套硼，Rest[0] = Racc[0]卡辰，每隔時(shí)間T就得到新的值即為Racc（1），Racc（2）和新的估算值Rest（1）邪意，Rest（2）等九妈，假設(shè)我們?cè)诘趎步。我們有兩列已知的值可以用：Rest(n-1) – 前一個(gè)估算值抄罕，Rest(0) = Racc(0)允蚣，Racc(n) – 當(dāng)前加速度計(jì)測(cè)量值，在計(jì)算Rest（n）前呆贿，我們先引進(jìn)一個(gè)新的值Rgyro嚷兔，它可由陀螺儀和前一個(gè)估算值得到，同樣它是個(gè)矢量并由3個(gè)分量組成（Rgyro = [RxGyro,RyGyro,RzGyro]）做入。
首先觀察陀螺儀模型中下面的關(guān)系冒晰，根據(jù)由Rz和Rxz組成的直角三角形我們能推出：tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)，atan2函數(shù)和atan類(lèi)似竟块，但atan返回值范圍是（-PI/2,PI/2）壶运，atan2返回值范圍是（-PI，PI）浪秘，并且有兩個(gè)參數(shù)蒋情。它能將Rx，Rz值轉(zhuǎn)換成360°（-PI耸携，PI）內(nèi)的角度棵癣。知道了RxEst（n-1）和RzEst（n-1）我們發(fā)現(xiàn)：Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) )
陀螺儀測(cè)量的是Axz角度變化率，因此夺衍，我們可以按如下方法估算新的角度Axz（n）：Axz(n) = Axz(n-1) + RateAxz(n) * T狈谊，RateAxz可由陀螺儀ADC讀取得到。
通過(guò)使用平均轉(zhuǎn)速可由得到一個(gè)更準(zhǔn)確的公式：
RateAxzAvg =（RateAxz（N）+ RateAxz（N-1））/ 2
就得到了這兩個(gè)值
Axz(n) = Axz(n-1) + RateAxzAvg * T
Ayz(n) = Ayz(n-1) + RateAyz(n) * T。
現(xiàn)在推導(dǎo)RxGyro/RyGyro河劝，將RGyro標(biāo)準(zhǔn)化為| Rgyro | = sqrt（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2）壁榕，經(jīng)過(guò)公式帶入和簡(jiǎn)化后可得到
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 )
RzGyro ?= ?Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2)? 其中，當(dāng) RzGyro>=0時(shí)赎瞎，Sign(RzGyro) = 1 , 當(dāng) RzGyro<0時(shí)牌里，Sign(RzGyro) = -1 。Rz可以用作計(jì)算Axz和Ayz傾角的參考
然后采用Racc和RGyro的加權(quán)平均值來(lái)更新Rest（n）即Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)煎娇，令w2=w1=wGyro二庵，最后可得：
Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)? 其中wGyro表示我們對(duì)加速度計(jì)和陀螺儀的相信程度，這個(gè)值在5到20之間取會(huì)比較好缓呛。
得到了Rest（n）后就可以算出向量的投影算出最新的估算值了：
RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)
最后得到最新的標(biāo)準(zhǔn)化矢量?
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。
