多傳感器融合算法

多傳感器數據融合算法---9軸慣性傳感器 - hmmwjs的博客 - CSDN博客

加速度計模型

每個ADC模塊都有一個參考電壓，假設它是3.3V。要將一個10位的ADC值轉成電壓值，使用下列公式：? ?VoltsRx = AdcRx * VREF / 1023（小注：8位ADC的最大值是255 = 2 ^ 8 -1，12位ADC最大值是4095 = 2 ^ 12 -1）。每個加速度計都有一個零加速度的電壓值稱為VzeroG，你可以在它的說明書中找到，這個電壓值對應于加速度為0g，利用公式DeltaVoltsRx = VoltsRx - VzeroG 可以得到相對0g電壓點的偏移量。

在得到了加速度計的電壓值后，利用加速度計的靈敏度Sensitivity可將電壓值轉化成為單位為g（9.8m/s^2）使用公式為RX = DeltaVoltsRx /Sensitivit可以獲得每個方向的加速度值。

綜合上述公式，即可得到怎么讀取一個ADC值并將其轉換為單位為g的矢量力的分量。

Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity?

Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity

Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity。

利用反三角函數計算向量R在每個軸上的角度：Axr = arccos(Rx/R)，Ayr = arccos(Ry/R)，Azr = arccos(Rz/R)。

陀螺儀模型

陀螺儀測試向量R與各軸之間的角度的變化率

陀螺儀輸出值即AdcGyroXZ（由ADC模塊得到）轉換中的ADC部分（假設使用10位ADC模塊，如果是8位ADC，用1023代替255，如果是12為ADC用4095代替1023），將其單位轉換成為度/秒，公式為RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity。其中VzeroRate 是零變化率電壓，換句話說它是陀螺儀不受任何轉動影響時的輸出值，通常可以由說明書得到，需要校準，而Sensitivity為陀螺儀的靈敏度，單位為mV/deg/s,它的意思就是如果旋轉速度增加1°/s,陀螺儀的輸出就會增加多少mV。

例子：假設我們的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的速度旋轉。

加速度計和陀螺儀融合算法

1.統一坐標軸

一般將加速度計作為參考坐標

2. 確定陀螺儀的輸出對應到RateAxz，RateAyz的值

3.根據陀螺儀和加速度計的位置決定是否要反轉輸出值

4.算法處理

由于加速度計和陀螺儀會受到許多因素的干擾，所以在使用時要利用一定的濾波算法來處理噪聲。由第一部分計算得到的數據容易受到噪聲的干擾，稱之為Racc – 是由加速度計測量到得慣性力矢量，它可分解為下面的分量RxAcc，RyAcc，RzAcc。

這組數據是只來自于加速度計ADC的值，我們把這組數據叫做“vector”，并使用下面的符號：Racc = [RxAcc,RyAcc,RzAcc]。我們引進一個新的變量Rest = [RxEst,RyEst,RzEst]來作為算法計算的值，它是經過陀螺儀數據的修正和基于上一次估算的值。

開始時規定加速度計輸出為準確值，Rest[0] = Racc[0]，每隔時間T就得到新的值即為Racc（1），Racc（2）和新的估算值Rest（1），Rest（2）等，假設我們在第n步。我們有兩列已知的值可以用：Rest(n-1) – 前一個估算值，Rest(0) = Racc(0)，Racc(n) – 當前加速度計測量值，在計算Rest（n）前，我們先引進一個新的值Rgyro，它可由陀螺儀和前一個估算值得到，同樣它是個矢量并由3個分量組成（Rgyro = [RxGyro,RyGyro,RzGyro]）。

首先觀察陀螺儀模型中下面的關系，根據由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。

現在推導RxGyro/RyGyro，將RGyro標準化為| Rgyro | = sqrt（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 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 )

RzGyro ?= ?Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2)? 其中，當 RzGyro>=0時，Sign(RzGyro) = 1 , 當 RzGyro<0時，Sign(RzGyro) = -1 。Rz可以用作計算Axz和Ayz傾角的參考

然后采用Racc和RGyro的加權平均值來更新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表示我們對加速度計和陀螺儀的相信程度，這個值在5到20之間取會比較好。

得到了Rest（n）后就可以算出向量的投影算出最新的估算值了：

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。