有人問我關於四元數姿態解算演算法的分析，每次都解釋好久，今日空閒，特發一帖，供大家參考。本分析將結合程式，分析姿態解算思路，由於能力有限，難免有錯誤之處請諒解，同時希望能夠拋磚引玉，得到大神指點。感謝圓點博士提供資料參考。

為尊重原創，轉載請註明原出處，謝謝。

http://blog.csdn.net/u013608300/article/details/52459515

四軸之互補濾波與四元數演算法最詳細分析

原創：冰三點水

關鍵字：姿態解算 互補濾波 四元數

下面進入正題。

第一部分 感測器說明

         通常我們開源小四軸使用mpu6050姿態感測器，可以輸出三軸加速度和三軸角速度資料。角速度資料就是x、y、z三個軸的旋轉角速度。而加速度是沿三個軸的加速度，這個加速度計測量的是比力。這裡對於加速度計有個重點，我們想象這樣一個模型，在太空中有一個盒子，盒子裡連著三軸彈簧，力感應球位於彈簧連線點，在外太空失重時，三條彈簧伸縮量為0，x = y = z= 0；

當在重力場內，盒子靜置與地面時，力感應球收到重力的吸引孤兒有一個軸的彈簧有伸縮變化，或者說當盒子（加速度計）靜置時會有一個輸出，大小等於重力，這就是我們說的比力。（感謝第七實驗室的圖片）

在互補濾波演算法中，我們認為四軸是平滑運動，也就是說我們忽略了加速度計測量的和運動帶來的影響，而認為加速度計測量的就是重力（比力）不包括和運動，這是互補濾波是侷限性所在，補償方法請自行分析。（感謝投筆從容提供思路）

         實際上，我們單單通過三軸角速度就可以得到四元數，進而將四元數轉換成尤拉角，也就是姿態角，問題是陀螺儀會有積分漂移，得到的角度會有誤差，這時候我們就引入加速度計對陀螺儀資料進行校正，以得到更準確的角度。不過這時yaw，也會存在誤差，在誤差不能忽略的使用情況下，我們就需要用磁力計了。

第二部分 四元數

    二維平面內的旋轉可以用複數表示（即a+bi），三維空間內的旋轉需要用四元數（即a+bi+cj+dk）來表示，而不是我們猜測的類似於三元數（a+bi+cj）表示。注意四元數表示的是一個旋轉。相比於複數，四元數也有類似與i的虛部，所以它其實是超複數。四元數的轉化成三角形式可以看出它描述了剛體的定點運動（見秦永元 慣性導航 四元數相關章節），當只關心v系相對於R系的角位置時，可以認為是b繫到R系經過一次等效旋轉而成的。四元數Q包含了等效旋轉的全部資訊。注意，描述剛體旋轉的四元數是規範化四元數，也就是說四元數的範數（模）等於1！！！！！！這就是為什麼引入誤差向量後，四元數要重新歸一化。

    我們常聽說四元數微分方程，這個方程的輸入是角速度資料，我們只要輸入角速度資料，就可以更新四元數資料。

上圖為程式實現的對四元數微分方程的解。這個方程是怎麼來的呢？由於機體的運動，四元數Q也是變化的，換句話說，q0 q1 q2q3是時間的函式。我們把四元數寫成三角形式，

即

這裡有角度量 ，然後對這個式子進行求導，然後進行一下相應的轉換，這裡提供一個四路，細節就不講了。上面的程式是解這個方程的常用方法。

第三部分 四元數到尤拉角

         四元數到尤拉角有公式，對於一個固定的尤拉角旋轉順序有相應的公式。程式上實現起來比較容易

程式中的57.3是個轉換系數，是弧度和角度的轉換系數。

這個公式具體請參考秦永元老師的慣性導航。

第四部分 互補濾波

         到目前為止，我們已經可以通過四元數微分方程得到四元數，在通過四元數到尤拉角的轉換得到四軸的姿態，但是為了得到更加準確的姿態，我們引入了加速度計。

在IMU程式中我們剛開始看到的往往是將加速度原始資料歸一化。

歸一化後的ax、ay、az的平方和為1，各自方向不變，也就是合向量的方向不變，模等於1，這是個重點。

建立小四軸座標系

這個很簡單，所謂的建立小四軸座標系不過是把世界座標系下的重力向量（0 0 1）轉換到小四軸座標系下而已，換句話說，小四軸座標系相對於世界座標系是運動的，而重力向量在世界座標系是｛0 0 1｝，那麼它在小四軸座標系下是多少呢？這就通過這個旋轉矩陣*Rb來求得。圖就不畫了，有想要的@我。

R為小四軸座標系，b為世界座標系，是方向餘弦矩陣

這個方向餘弦矩陣是怎麼來的呢？

請參考慣性導航或下面連結（請諸位一定閱讀相關內容，起碼下面的連線解釋了旋轉矩陣的由來）

互補濾波這個部分到目前為止還沒進入正題。（樓主已經寫的好煩啊。。。。。。）

目前的進度是建立了小四軸座標系。

接下來是重點！

將剛才轉移到小四軸座標系下的重力向量和機體加速度計測量的重量向量叉乘！

得到一個誤差向量。這個誤差向量是有物理意義的。叉乘等於|A|*|B|*sin(角度),轉移到小四軸座標系的重力向量模為1，加速度計測量的重力向量經過歸一化也為1，那麼叉乘的結果只與角度有關，並且角度越大，誤差向量的莫越大！歸一化的目的在這裡體現。向量的叉乘還包括方向資訊，還是一個向量，有人認為，在固定的取樣計算時間內的按一定方向轉過這些誤差向量，也可以看作成角速度（來自黑暗騎士）這讀者自己揣摩吧。總之，我們就是通過這個誤差向量來修正陀螺儀資料的。

通過PI運算來融合陀螺儀和加速度資料。

如果Kp = Ki = 0就是完全相信陀螺儀資料。

將融合後的角速度輸入到四元數微分方程 就得到四元數，進而得到尤拉角。但是這組四元數又歸一化，因為引入誤差向量使四元數失去規範性，範數（模）不等於1了，所以這裡要歸一化。

至於為什麼四元數又歸一化處理，為因為引入誤差向量後四元數失去了規範化，請參考慣性導航。

最後在說兩句

得到的原始角速度資料需要乘一個係數轉換成實際的角速度（弧度每秒），而加速度不需要，因為加速度歸一化後與大小無關了。

分析完畢。

筆者能力有限，希望能夠給各位帶來啟發，而不是誤人子弟，同時歡迎大神指正其中的錯誤。

為尊重原創，轉載請註明原出處，謝謝。

冰三點水

於天津工業