APM姿態(tài)旋轉(zhuǎn)理論基礎(chǔ)閱讀源碼的過程中發(fā)現(xiàn)對(duì)于一些基礎(chǔ)理論的掌握還是不夠深，因此本篇先把一些理論知識(shí)部分匯總一下,。本博文會(huì)持續(xù)更新修改,，所有引用均會(huì)表明出處。如有問題,，歡迎指正,。 以下內(nèi)容均為個(gè)人認(rèn)為需要掌握之處，可能不會(huì)涵蓋一些太基礎(chǔ)的理論,，但是會(huì)提供一些參考的鏈接供大家學(xué)習(xí)（超長(zhǎng)篇警告）,。 一、坐標(biāo)系無人機(jī)領(lǐng)域中最重要的就是慣性坐標(biāo)系和機(jī)體坐標(biāo)系。 1.1 NED坐標(biāo)系因?yàn)樵诂F(xiàn)實(shí)中通常需要在平面控制無人機(jī),，因此NED坐標(biāo)系通常作為無人機(jī)領(lǐng)域的導(dǎo)航坐標(biāo)系使用,。 NED坐標(biāo)系如下圖綠色坐標(biāo)系，即北東地坐標(biāo)系,，三個(gè)方向始終保持不變,。
1.2 機(jī)體坐標(biāo)系機(jī)體坐標(biāo)系始終與無人機(jī)本體固連，同時(shí)也表明了無人機(jī)當(dāng)前的姿態(tài),。其原點(diǎn)位于無人機(jī)的重心,，X軸朝向機(jī)頭，Y軸垂直于X軸指向機(jī)身右方,，Z軸按照右手法則正交于X和Y軸（這邊先關(guān)注軸即可,，旋轉(zhuǎn)和角度表示后面需要再統(tǒng)一一下標(biāo)準(zhǔn)）。 通常無人機(jī)的姿態(tài)一般由慣性測(cè)量單元IMU來獲取,，其中陀螺儀用于獲取角速度,，加速度計(jì)顧名思義獲取3軸的加速度，如果帶有磁力計(jì),，則可以獲取到磁場(chǎng)方向,。 后面過程N(yùn)ED坐標(biāo)系會(huì)簡(jiǎn)稱為n系，機(jī)體坐標(biāo)系會(huì)簡(jiǎn)稱為b系,。 舉例來說,，在n系下我們獲取到的是無人機(jī)的“東南西北”的絕對(duì)位置信息，而在b系下,，我們會(huì)要求無人機(jī)進(jìn)行“前后左右”的增量式運(yùn)動(dòng),，如何將我們給機(jī)體的期望運(yùn)動(dòng)與導(dǎo)航給我們反饋的n系下的位置、姿態(tài)等絕對(duì)信息聯(lián)系起來,，就是我們后面要做的事情,。
參考資料：
二,、歐拉角借一張已經(jīng)被用爛了的圖簡(jiǎn)單來說一下歐拉角的一些基本概念：
在無人機(jī)領(lǐng)域中，我們通常使用的歐拉角旋轉(zhuǎn)方式表示為（根據(jù)國內(nèi)標(biāo)準(zhǔn)統(tǒng)一符號(hào),，記住此處的角度符號(hào),，會(huì)在旋轉(zhuǎn)矩陣中用到）：
姿態(tài)變化率與機(jī)體角速度的關(guān)系以下內(nèi)容來自全權(quán)老師的《多旋翼飛行器設(shè)計(jì)與控制》,。
其中,，向量 ω ω ω 表示的是機(jī)體的旋轉(zhuǎn)角速度， W W W表示旋轉(zhuǎn)過程,， Θ Θ Θ表示RPY角,，則對(duì)應(yīng)的微分表示的即是姿態(tài)變化率（原諒我在編輯器里面打不出 Θ Θ Θ上的一點(diǎn)）。
三,、旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣表示的是坐標(biāo)系的旋轉(zhuǎn) 3.1 基本公式推導(dǎo)就不推導(dǎo)了,，直接給出結(jié)論，常用的繞X,、Y,、Z軸方向的旋轉(zhuǎn)矩陣如下（注意推導(dǎo)時(shí)根據(jù)旋轉(zhuǎn)方向會(huì)有正負(fù)號(hào)的差異）。我們通常用Φ,，θ,，ψ表示繞X、Y,、Z軸的旋轉(zhuǎn)角度,。
從n系向b系旋轉(zhuǎn)的旋轉(zhuǎn)矩陣
R
n
b
R{^b_n}
Rnb?（順序Z-Y-X） 那么b系向n系旋轉(zhuǎn)的旋轉(zhuǎn)矩陣正好為
R
b
n
=
R
n
b
T
=
R
n
b
?
1
R{^n_b}= R{^b_n}^T= R{^b_n}^{-1}
Rbn?=Rnb?T=Rnb??1 3.2 矩陣作差來源：[飛控]姿態(tài)誤差(二)-旋轉(zhuǎn)矩陣做差 通常歐拉角下姿態(tài)的誤差能夠通過期望角和當(dāng)前角相減得到,，但是對(duì)于由旋轉(zhuǎn)矩陣描述的姿態(tài)來說,，無法通過矩陣相減得到姿態(tài)誤差。 通常,，假設(shè)我們有
R
1
2
R{^2_1}
R12?描述從坐標(biāo)系1到2的旋轉(zhuǎn)，而
R
2
3
R{^3_2}
R23?描述從坐標(biāo)系2到3的旋轉(zhuǎn),，則有如下性質(zhì) 又由于旋轉(zhuǎn)矩陣為正交矩陣，因此有 由此可見旋轉(zhuǎn)矩陣中通過左乘一個(gè)旋轉(zhuǎn)矩陣的逆表示姿態(tài)作差,。 3.3 旋轉(zhuǎn)矩陣與變換矩陣的區(qū)別旋轉(zhuǎn)矩陣通常是以R表示3×3矩陣,，而變換矩陣則更多的是用T表示的在齊次坐標(biāo)系下的4×4矩陣。
一些論文里面會(huì)以群的形式描述三維空間內(nèi)的旋轉(zhuǎn)矩陣和變換矩陣,，通常前者以SO(3)表示,，后者用SE(3)表示，有興趣的同學(xué)可以自行翻閱相關(guān)群論,，此處知道如上表示即可,。
四,、DCM上面的旋轉(zhuǎn)矩陣是從歐拉角進(jìn)行推導(dǎo)得出的，除此之外,，我們還可以直接利用坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系進(jìn)行推導(dǎo),，此時(shí)獲得的矩陣成為方向余弦矩陣DCM，實(shí)際上從某種意義上來說兩者是等價(jià)的,，因?yàn)樽詈螳@得的姿態(tài)是一樣的,。 推薦閱讀：DCM Tutorial – An Introduction to Orientation Kinematics
定義全局坐標(biāo)系為OXYZ,，本體坐標(biāo)系為Oxyz,，兩者原點(diǎn)相同。全局坐標(biāo)系中定義各軸（X,、Y,、Z）的單位向量分別為 I 、 J ,、 K I,、J、K I,、J,、K，同理本體坐標(biāo)系中各軸對(duì)應(yīng)的單位向量為 i ,、 j ,、 k i,、j、k i,、j,、k。 全局坐標(biāo)系下的單位向量
I
,、
J
,、
K
I、J,、K
I,、J、K表示為 由此獲得向量
I
,、
J
、
K
I,、J,、K
I、J,、K在本體坐標(biāo)系中的投影（注意
I
.
i
I.i
I.i和
i
.
I
i.I
i.I等價(jià)） 可以發(fā)現(xiàn) D C M G DCM^G DCMG和 D C M B DCM^B DCMB是各為彼此的轉(zhuǎn)置,，并且DCM本身為一個(gè)正交矩陣。
五,、軸角法5.1 基本概念由于APM中主要采用的就是軸角和四元數(shù)來計(jì)算姿態(tài)誤差 先來看看什么是旋轉(zhuǎn)向量,，旋轉(zhuǎn)向量定義為：任意旋轉(zhuǎn)均可用一個(gè)旋轉(zhuǎn)軸和一個(gè)旋轉(zhuǎn)角來表示，由此使用一個(gè)向量,，其方向與旋轉(zhuǎn)軸保持一致,，向量長(zhǎng)度等于旋轉(zhuǎn)角（摘自《視覺SLAM十四講》）。 而軸角法（Axis-Angle）實(shí)際上就是旋轉(zhuǎn)向量,，它使用一個(gè)轉(zhuǎn)軸（單位向量）和一個(gè)旋轉(zhuǎn)角來描述旋轉(zhuǎn)過程,。 如下圖所示，以z為轉(zhuǎn)軸,，α為轉(zhuǎn)角構(gòu)建軸角,，將坐標(biāo)系xyz旋轉(zhuǎn)到X’Y’Z’。 5.2 與旋轉(zhuǎn)矩陣的相互轉(zhuǎn)換只給出結(jié)論,。 從軸角轉(zhuǎn)換到旋轉(zhuǎn)矩陣,，根據(jù)羅德里格斯公式有： 可以倒推獲得旋轉(zhuǎn)矩陣向軸角的轉(zhuǎn)換： 轉(zhuǎn)軸n,，在旋轉(zhuǎn)軸上的向量旋轉(zhuǎn)前后不發(fā)生變化
參考資料：
六,、四元數(shù)首先明確四元數(shù)描述的是三維空間的旋轉(zhuǎn)過程，而不是一個(gè)點(diǎn),。 基礎(chǔ)理論和數(shù)學(xué)計(jì)算方式這邊將不再介紹,，后面均表示為國內(nèi)統(tǒng)一標(biāo)準(zhǔn)。 下圖來自《多旋翼飛行器設(shè)計(jì)與控制》
q ? = q 0 + q 1 i ? + q 2 j ? + q 3 k ? \vec{q} = q_0+q_1\vec{i}+q_2\vec{j}+q_3\vec{k} q ?=q0?+q1?i +q2?j ?+q3?k 在APM中則是以 q 1 ,， q 2 ,， q 3 ,， q 4 q_1，q_2,，q_3,，q_4 q1?，q2?,，q3?,，q4?進(jìn)行表示。 一些基本運(yùn)算可以看這篇博文：旋轉(zhuǎn)表達(dá)之四元數(shù) 注意：?jiǎn)挝凰脑獢?shù)的逆等于它的共軛 在統(tǒng)一使用國內(nèi)標(biāo)準(zhǔn)描述四元數(shù)時(shí),，四元數(shù)與旋轉(zhuǎn)矩陣也可以像旋轉(zhuǎn)矩陣一樣上下標(biāo)約去 6.1 四元數(shù)表示旋轉(zhuǎn)6.1.1 與軸角的相互轉(zhuǎn)換前提：軸角已知 四元數(shù)本身也是存儲(chǔ)了一個(gè)旋轉(zhuǎn)軸和一個(gè)旋轉(zhuǎn)角度,。如果已知一個(gè)由軸角n和θ指定的旋轉(zhuǎn)。其中 n ? = x i ? + y j ? + z k ? \vec{n}=x\vec{i}+y\vec{j}+z\vec{k} n =xi +yj ?+zk 并且向量n為單位向量 ∣ ∣ n ? ∣ ∣ = 1 ||\vec{n}||=1 ∣∣n ∣∣=1 則用四元數(shù)描述這個(gè)旋轉(zhuǎn)過程為 q = c o s θ 2 + ( x + y + z ) s i n θ 2 q=cos\frac{θ}{2}+(x+y+z)sin\frac{θ}{2} q=cos2θ?+(x+y+z)sin2θ? 表示為 q = [ q 0 , q 1 , q 2 , q 3 ] = [ c o s θ 2 , x s i n θ 2 , y s i n θ 2 , z s i n θ 2 ] = [ c o s θ 2 , n s i n θ 2 ] q=[q_0 ,q_1,q_2,q_3]=[cos\frac{θ}{2},xsin\frac{θ}{2},ysin\frac{θ}{2},zsin\frac{θ}{2}]=[cos\frac{θ}{2},nsin\frac{θ}{2}] q=[q0?,q1?,q2?,q3?]=[cos2θ?,xsin2θ?,ysin2θ?,zsin2θ?]=[cos2θ?,nsin2θ?] 那么反過來也可以求得軸角 四元數(shù)恢復(fù)軸角形式 雖然上面給出了公式,，然而一種更直觀的計(jì)算方法如下,，前提是已知四元數(shù)表達(dá)
6.1.2 四元數(shù)表示旋轉(zhuǎn)向量旋轉(zhuǎn) 假設(shè)
q
q
q是一個(gè)用四元數(shù)表示的旋轉(zhuǎn)過程，
v
1
∈
R
3
v_1\in R^3
v1?∈R3表示一個(gè)向量,，那么在
q
q
q的作用下,，向量
v
1
v_1
v1?旋轉(zhuǎn)為向量
v
1
′
v_1'
v1′?可表示為 =q× ×q^{-1}
(0v1′??)=q×(0v1??)×q?1
注意以上過程為0標(biāo)量運(yùn)算，具體的運(yùn)算過程以及非0標(biāo)量四元數(shù)運(yùn)算詳見下面這篇博客（內(nèi)容會(huì)在APM姿態(tài)誤差計(jì)算中用到） 坐標(biāo)系旋轉(zhuǎn) 坐標(biāo)系旋轉(zhuǎn)則剛好相反,，想象在一個(gè)坐標(biāo)系下將向量逆時(shí)針旋轉(zhuǎn)45°,，這個(gè)過程是不是等價(jià)于將坐標(biāo)系順時(shí)針旋轉(zhuǎn)45°呢？ 因此公式表示為： =q^{-1}× ×q
(0v1′??)=q?1×(0v1??)×q
6.2 四元數(shù)與旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣 R b e R^e_b Rbe?是從機(jī)體坐標(biāo)系b到地球固連坐標(biāo)系e的轉(zhuǎn)換 摘自《多旋翼飛行器設(shè)計(jì)與控制》 摘自《視覺SLAM十四講》
6.3 四元數(shù)與歐拉角歐拉角轉(zhuǎn)換為四元數(shù),。 回憶一下我們通常用φ，θ,，ψ表示繞X,、Y,、Z軸的旋轉(zhuǎn)角度
參考資料：
總結(jié)
|
|
|
