https://blog.csdn.net/super_mice/article/details/45619945
這篇博客應該在一年前,做四軸畢設的時候寫的.結果一直拖到現在...
有問題請回複評論，然後郵箱提醒我回復，[email protected] 私信不回。。。
一年前的時候關於四元數和姿態解算的中文資料還不多,至少我是找了很久纔有那麼點真正有價值的資料.
今年又很多學生做四軸相關的畢業論文,所以我希望這篇文章能夠整合一些現有的資料並且加上一些自己的理解,方便後來的同學少走些彎路.由於時間久遠,很多細節會記不清,還請多包涵.其實簡單的飛控已經非常成熟而便宜了,各位技術大神們可以把你們的精力放在更有價值的問題上.比如飛行器的避障,導航,多機協作等.不要爲了學技術而學技術,什麼事情都想自己DIY,別人做過的事情就沒有必要自己過一遍,你一個人做的東西也不大可能比很多牛人一起維護的開源項目來得優秀.可以做的事情太多,我們把精力集中起來做些更需要人去做的工作是比較高效和有價值的,至於基礎的東西儘量藉助於別人肩膀嗎，比如一些開源項目就好.
另外一篇關於四軸雙閉環PID(串級PID)的帖子請移步:四軸PID調試
言歸正傳,當初困擾我的主要是這麼幾個問題:
[1]四元數是什麼鬼?她和歐拉角又是什麼曖昧關係?
[2]陀螺儀和加速度計的讀數和四元數怎麼建立關係?
[3]歐拉角的"gimal lock"?她和歐拉角不合適表示姿態有什麼關係?
[4]姿態解算的方法:互補濾波,卡爾曼濾波,梯度下降,DCM都是啥?
[5]如何真正通過代碼實現姿態結算?
下面逐個講講
[1]四元數是什麼鬼?她和歐拉角又是什麼曖昧關係?
四元數,百度搜一下一大片,可是我就是不理解到底和姿態有什麼關係?
可以把四元數理解爲旋轉軸+旋轉角的描述方法的優化版.用一個3維向量表示轉軸(暫時理解爲四元數的x,y,z,表示的向量),一個角度表示繞此轉軸的旋轉角度(暫時理解爲四元數的w).
下面舉例子說明下，有點長,配了些圖片方便理解,跟着文章一步步來,就能理解
假設你一個人在野外，餓了幾天飢腸轆轆。突然有隻野雞飛出來，一頭撞在了你身邊的大樹上，掛了。於是你感謝上蒼的恩賜。。。有點扯遠了。之後你生起了火要烤了那隻野雞。可是怎麼烤呢？你找了根木棒，削尖了想插進野雞裏，首先爲了轉起來順暢棒子要穿過雞的重心。那麼問題來了。這個棒要從哪個角度插進去呢？，爲了方便描述，我們把雞按照活着時候的樣子擺好，並且雞頭對着正東方向。我們把重心當做座標原點，重心到雞頭方向當做x正方向（正東），重心到左翅膀方向爲y正方向（正北），重心到雞背方向爲z正方向。這個樣子就是我們的初始姿態（和世界座標系重合）。如下圖所示：綠色箭頭爲y軸正方向，紅色箭頭爲x軸正方向，藍色爲z軸正方向。（之前寫的帖子有錯誤，現已改正，詳見9樓的回覆）大的座標爲世界座標系。綠色指向正北，紅色指向正東，藍色指向天空。小的座標爲野雞的姿態，紅色是雞頭，綠色是左翅膀方向，藍色是雞背方向。本文只需要關注兩個座標系之間的旋轉變化即可，請無視兩個座標系之間的距離變化。
不過一般我們都喜歡從菊花方向插入，那這個方向單位化後就是（-1,0,0）。然後我們把棒子和雞放上烤架，準備轉起來烤雞。那麼問題又來了，往哪個方向轉呢？如果從雞屁股的方向看，你按逆時針方向把雞轉動了90度。現在雞背朝北，雞頭仍舊朝東。如下圖所示：大的座標（世界座標）不變。小座標系和上圖相比繞着紅軸的負方向旋轉了90度。
剛纔我們對雞的這個轉動可以用四元數來表示，
w=cos（90/2) ,
x=sin(90/2)*-1,
y=sin(90/2)*0,
z=sin(90/2)*0;
w=0.707, x=-0.707, y=0. z=0(如果是順時針轉90度那麼w=cos（-90/2)=0.707，x=-0.707，y=0,z=0),旋轉角度的正負由右手定則確定。
現在對四元數表示旋轉大概明白了吧？當然轉軸可以沿任何方向，不一定沿着座標軸。旋轉角度爲theta，（ax,ay,az）代表單位化的轉軸方向。那麼
w = cos(theta/2)
x = ax * sin(theta/2)
y = ay * sin(theta/2)
z = az * sin(theta/2)
這個就是表示旋轉的四元數。
其實四元數的定義是q=w+xi+yj+zk
其中w是實數，x,y,z是虛數，其中:
i*i=-1
j*j=-1
k*k=-1
當然，只有單位化的四元數纔可以用來描述旋轉。下面看下如何單位化
||q|| = Norm(q)=sqrt(w2 + x2 + y2 + z2)
Normlize(q)=q/Norm(q)=q / sqrt(w2 + x2 + y2 + z2)
現在我們再次回到烤野雞的問題。經過剛纔的那次旋轉後，你突然想給野雞換個姿勢烤了。你不想按之前的轉軸方向轉了。於是你把雞的位置和姿態固定不動（雞頭朝東，雞背朝北），把棒子拔出來，換了個方向從雞背上插進去，烤架按照棒子的方向架好。現在我們繞着新的轉軸轉動從雞背方向看過去沿順時針轉動90度。
現在雞的姿態應該是雞頭朝上，雞背朝北。剛纔這次的旋轉用四元數表示（注意，這次旋轉的基準座標系是上次旋轉完後的座標系，不是和世界座標系重合的初始姿態座標系）。所以我們的轉軸方向是（0,0,1），角度是 -90 度。所以
w=cos(-90/2)=0.707,
x=sin(-90/2)*0=0 ,
y=sin(-90/2)*0=0,
z=sin(-90/2)*1=-0.707
問題又又來了，你想知道野雞相對於初始位置的姿態。這時候只要把剛纔得到的兩個四元數相乘就可以把兩次旋轉合併爲一次旋轉，得到一個相對於初始位置的四元數（四元數方便插值就是這個意思），四元數的乘法公式如下，其中q1是後來旋轉的四元數，q2是上次旋轉的四元數，q3是兩次旋轉合併後得到的四元數。
q3=q1 * q2
w3=(w1*w2 - x1*x2 - y1*y2 - z1*z2)
x3=(w1*x2 + x1*w2 + y1*z2 - z1*y2)
y3=(w1*y2 - x1*z2 + y1*w2 + z1*x2)
z3=(w1*z2 + x1*y2 - y1*x2 + z1*w2)
計算後可得 q3爲
w3=0.5
x3=-0.5
y3=-0.5
z3=-0.5
可以看做是沿着相對於繞着初始姿態座標中的( -1/sqrt(3) , -1/sqrt(3) , -1/sqrt(3) )（sqrt是開根號的意思）這個方向的轉軸，做了從轉軸正方向看逆時針轉動120度的旋轉。啓動你的空間想象能力，你應該可想象到野雞從初始姿態經過這樣的一次旋轉就變成了現在的姿態。其實這和繞着( 1/sqrt(3) , 1/sqrt(3) , 1/sqrt(3) )方向的轉軸，從轉軸正方向看順時針轉動120度是一樣的效果。
這就是四元數表示姿態最有利的地方，可以將多次的旋轉通過簡單的運算變爲一次旋轉。