姿態解算系列一：經驗型卡爾曼數據融合

目的：我們需要得到機器人運動的姿態信息，三個軸的角度以及角速度。

本文大綱：

1、傳感器相關模型 

2、座標變換   

3、經驗型卡爾曼數據融合

4、姿態解算流程 

5、DMP 

6、數據融合效果 

本文出處：http://blog.csdn.net/haishaoli/article/details/52965457

本文內容：

姿態解算總框圖（歐拉角方式）：

一、數據模型、 噪聲模型及曲線

1、歐拉角定義：

2、數據模型：

其中,φ表示的是測量的角度，b表示的是陀螺儀的飄移誤差(爲常值偏差)，ωgyro表示的是陀螺儀的角速度輸出值。ωg爲陀螺儀測量噪聲，φacce爲加速度計經過處理後得到的角度值，ωa爲加速度計測量噪聲。

噪聲模型近似爲高斯分佈，如下圖：

A、加速度計噪聲圖

B、陀螺儀噪聲圖

二、 座標變換（歐拉角方式）：把陀螺儀角速度變換到加速度計的地理座標系上

1、陀螺儀數據的使用：

A的逆：其中A矩陣爲姿態矩陣。

下面內容來自《慣性導航》：主要是把三軸角速度從機體座標系換算到地理座標系上。

使用上述公式，在只使用陀螺儀的條件下，可以求得地理座標系下的三軸角速度信息。

角速度進行積分，就能得到相應的角度信息。

2、加速度計數據的使用：

使用上述公式，在只使用加速度計的條件下，可以求得俯仰角和橫滾角信息。

三、 卡爾曼數據融合

A、 融合效果：本文最後附圖。

B、一行代碼：

a = (1 - d%) * b + d% * c;

a 是 融合輸出 ，b 是預測量，c 是觀察量

這是關鍵的融合代碼。

C、提問：

上述代碼的含義是什麼？

加權平均？互補濾波？低通高通濾波器？

同樣的式子，不同的使用過程，有不同的意義。

如果係數是一個固定的值，則有可能是加權平均或者高通低通濾波器。

我講的是動態的係數。獲取數據在變，方差（或協方差）在變，係數就在變。

D、理解：

1、什麼情況使用 ？    高斯分佈。

單模、零均和、高斯噪聲 （高斯白噪聲）。之前說明了傳感器的噪聲屬於白噪聲。

2、卡爾曼爲什麼是最佳估計 ？下面使用最小二乘來理解。

推導方式一：

q1 是預測量，q2是觀察量，q是估計值，w是權值。下文來自《自主移動機器人導論》。

其中q1和q2的係數之和爲1。

利用這種推導方式可說明卡爾曼是最佳估計，但是並不能推出方差的表達式，並且，權值Wi還是我們自己取的。

推導方式二：

運用貝葉斯定理推導：推出融合表達式和方差表達式

3、卡爾曼的核心。 方差，或者說權重。其實就是卡爾曼增益。

方差公式，跟電阻類似，結果比原來兩者都小。因此就算是預測量的累積誤差較大，也能提高系統的精度，誤差通過迭代而減少。這說明了該方法的有效性。

4、獲得卡爾曼增益的方式對比：

 自動通過統計和計算數據方差 VS 人爲經驗估計方差  的對比  ：

 1、自動需要高頻數據，人爲經驗不用。

 2、自動具有通用性，人爲特定性。

 3、自動計算量大，人爲的計算量較少。

從5.35可推出：q = (d%) * q1 + (1-d%) * q2;

5、應用：我們的姿態融合，定位融合都是人爲給出卡爾曼增益係數的，叫做“經驗型卡爾曼濾波”。

     經驗之談：

A、參數一般是給的很小，原因跟傳感器（數據）的誤差模型相關；

        參數小的原因：分析卡爾曼增益的式子，預測量的方差較小，觀測量的方差較大，不是同一個數量級，因此增益係數一般給的很小；見定位融合圖，可看出方差情況。

        B、“經驗型卡爾曼濾波”的核心思想是變化卡爾曼增益係數；根據經驗我們能知道哪些條件下傳感器數據的方差大，哪些條件下方差小，從而改變增益係數。

C、融合線程的頻率越高越好。頻率高，一方面收斂快，另外一方面獲得的預測量誤差小。

四、 姿態解算流程

以下是以數據流的方式去理解。
1、傳感器原始數據輸入。
2、傳感器標定，方法很多，比如六面法。把標定後的數據換算成標準單位。
3、確定座標軸和數據符號，符號用右手定則。

4、歐拉角變換或者四元數變換，變換是爲了使陀螺儀角速度的座標系與加速度計的座標重合。

5、數據融合，減小方差，降低噪聲。
6、輸出融合後的姿態信息。

五、 DMP

這是MPU6X00芯片的自帶功能。不開源。

性能還不錯，甚至更好。計算有一部分是利用IMU自身的資源，不過依賴芯片產商。

六、數據融合實驗效果：

A、 姿態融合效果：藍色的線是融合輸出。平滑並且響應快。

B、定位融合效果：綠色的線是融合輸出，藍色的線是觀測量，紅色是預測量。