轉載自:https://blog.csdn.net/wxc971231/article/details/97449026
一、姿態解算原理相關
1、簡介
- 對於四旋翼無人機來說,有兩個相關座標系,即地理座標系OnXnYnZnOnXnYnZnOnXnYnZn和機體座標系ObXbYbZbObXbYbZbObXbYbZb。
- 姿態角的檢測對於四旋翼控制是至關重要的。比如在做飛行器姿態控制的時候,通常使用串級pid的方法,需要得到飛行器姿態角(即歐拉角)來做角度位置環反饋。再如利用裝置在機腹垂直向下拍攝的攝像頭,進行機體視覺定位時,也需要靠姿態角進行視覺反饋補償。
- 要解算姿態角,就要研究從地理座標系到機體座標系的轉換過程。這個轉換過程不是唯一的,比如可以先繞X軸旋轉θθθ,再繞Y軸旋轉γγγ,最後繞Z軸旋轉ψψψ,這樣得到的一組姿態角稱爲卡爾丹角;也可以按
ZYX
的順序旋轉,這樣得到的一組姿態角稱爲歐拉角(這是最常用的,pitch
,roll
,yaw
)。不管按照什麼順序,得到的角度都可以稱作廣義歐拉角。實際理論分析時,旋轉順序不是很重要,這個順序會影響四元數與歐拉角的關係,但是都可以進行解算。
2、座標變換和旋轉矩陣
下面從最基本的座標變換入手開始講解,請看如下座標系旋轉:OAXAYAZAOAXAYAZAOAXAYAZA繞OX旋轉θθθ到OBXBYBZBOBXBYBZBOBXBYBZB
對於原系OAXAYAZAOAXAYAZAOAXAYAZA中的一個向量[rxA,ryA,rzA][rxA,ryA,rzA][rxA,ryA,rzA],轉換到新系OBXBYBZBOBXBYBZBOBXBYBZB中的向量[rxB,ryB,rzB][rxB,ryB,rzB][rxB,ryB,rzB],有:
rxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzArxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzArxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzA
可以用旋轉矩陣表示
[rxBryBrzB]=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)][rxAryArzA]⎣⎡rxBryBrzB⎦⎤=⎣⎡100amp;0amp;cos(θ)amp;−sin(θ)amp;0amp;sin(θ)amp;cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤⎣⎡rxBryBrzB⎦⎤=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤
-
中間的就是 繞x軸旋轉θθθ 的旋轉矩陣,記作
Cx(θ)=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)]Cx(θ)=⎣⎡100amp;0amp;cos(θ)amp;−sin(θ)amp;0amp;sin(θ)amp;cos(θ)⎦⎤Cx(θ)=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤
-
同樣的,還有 繞y軸旋轉γγγ 的旋轉矩陣,記作
Cy(γ)=[cos(γ)0−sin(γ)010sin(γ)0cos(γ)]Cy(γ)=⎣⎡cos(γ)0sin(γ)amp;0amp;1amp;0amp;−sin(γ)amp;0amp;cos(γ)⎦⎤Cy(γ)=⎣⎡cos(γ)0sin(γ)010−sin(γ)0cos(γ)⎦⎤
-
同樣的,還有 繞z軸旋轉ψψψ 的旋轉矩陣,記作
Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001]Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0amp;−sin(ψ)amp;cos(ψ)amp;0amp;0amp;0amp;1⎦⎤Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎦⎤
-
假如現在我們按ZXY
的順序,從地理座標系OnXnYnZnOnXnYnZnOnXnYnZn旋轉到機體座標系ObXbYbZbObXbYbZbObXbYbZb,整體旋轉矩陣爲
Cnb=Cy(γ)Cx(θ)Cz(ψ)=[cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)−sin(ψ)cos(θ)sin(ψ)cos(θ)cos(ψ)cos(θ)sin(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)cos(γ)cos(θ)]Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)amp;−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)amp;cos(ψ)cos(θ)amp;−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)amp;−sin(ψ)cos(θ)amp;sin(θ)amp;cos(γ)cos(θ)⎦⎤Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)cos(ψ)cos(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)−sin(ψ)cos(θ)sin(θ)cos(γ)cos(θ)⎦⎤
-
注意一點,如果從n繫到b系的旋轉矩陣爲CnbCnbCnb,則從b繫到n系的旋轉矩陣爲CnbTCnbTCnbT,顯然有Cnb∗CnbT=ECnb∗CnbT=ECnb∗CnbT=E,所以旋轉矩陣是正交矩陣
-
這個結論不能直接使用,一方面這個直接是不可解的,另一方面大量三角函數運算會導致大量資源佔用。
3、四元數
- 四元數是一種超複數,由四個元構成:Q(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3k
- 關於四元數更詳細的介紹可以參考四元數
- 利用四元數我們可以提出另一種描述空間矩陣的方法,具體可以參考秦永元的《慣性導航》和【教程】四旋翼飛行器姿態解算算法入門學習-Rick Grimes
(1)四元數和歐拉角的關係
- 注意這三個式子中,θθθ是繞Y軸轉角,ϕϕϕ是繞X軸轉角,ψψψ是繞Z軸轉角,和前文有所不同
- 按
ZYX
的順序從地理座標系OeXeYeZeOeXeYeZeOeXeYeZe旋轉到機體座標系ObXbYbZbObXbYbZbObXbYbZb,整體旋轉矩陣爲
- 對應的四元數表示法爲:
- 解算出的歐拉角爲
(2)四元數的求解
- 飛行器姿態的改變,可以對應到旋轉矩陣的改變,進一步對應到四元數的改變。實時姿態計算,實際上也就是實時更新四元數。
- 我們可以構建四元數關於時間的微分方程,來研究四元數關於時間的變化規律,求解該微分方程,即可解出四元數
1、建立微分方程
- 從四元數的三角表示法入手推到微分方程,具體推到過程可以參考秦永元的《慣性導航》
Q=cos(θ)2+λsin(θ)2Q=2cos(θ)+2λsin(θ)Q=2cos(θ)+2λsin(θ)dQdt=1/2[0−ωx−ωy−ωxωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]⋅[q0q1q2q3]dtdQ=1/2⎣⎢⎢⎡0ωxωyωzamp;−ωxamp;0amp;−ωzamp;ωyamp;−ωyamp;ωzamp;0amp;−ωxamp;−ωxamp;−ωyamp;ωxamp;0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤dtdQ=1/2⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωx−ωyωx0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤記爲dQdt=Φ⋅Q記爲dtdQ=Φ⋅Q記爲dtdQ=Φ⋅Q
2、一階龍格庫塔法求解微分方程
一階龍格庫塔法是求解微分方程常用的工程方法,原理是把微分轉化爲微元增量,利用遞推迭代的方法求解
-
設有微分方程dydx=f(x,y)dxdy=f(x,y)dxdy=f(x,y)
求解y的迭代公式爲y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))
-
對應到四元數微分方程:
Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)
整理得
[q0q1q2q3]t+Δt=[q0q1q2q3]t+12⋅Δt⋅[−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0]⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤
這裏ωωω就是三軸角速度,可以用陀螺儀直接測得
4、旋轉矩陣中的一列
- 旋轉矩陣中的一列有特殊意義,它代表原座標系中一個方向的單位向量(X/Y/Z),在新座標系中對應的向量值
- 用方向向量(如X方向[1,0,0]T[1,0,0]T[1,0,0]T)左乘旋轉矩陣可以輕易看出,這裏不寫了
二、匿名四軸姿態解算分析
按Ano_Imu.c
文件順序進行分析
1、座標系定義
- 匿名四軸姿態解算部分主要寫在
Ano_Imu.c
中
- 匿名四軸程序中一共有三個座標系:
- 地理座標系,標記爲w(官方定義如下)
- 機體座標系,標記爲a(官方定義如下)
- 航向座標系,標記爲h(專門解釋一下,四旋翼飛行時,姿態角pitch和roll一般很小,在任意時刻,如果我們粗略地認爲飛行器始終與底面平行(把飛機擺正,使pitch=roll=0),此時的機體座標系即爲航向座標系)
2、座標系轉換
(1)匿名旋轉座標系
- 匿名的座標系變換,是按是按
ZYX
順序從地理座標系轉到機體座標系的,這個旋轉矩陣是前面提過的 (注意這三個式子中,θθθ是繞Y軸轉角,ϕϕϕ是繞X軸轉角,ψψψ是繞Z軸轉角)
Cwa=[cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)]Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)amp;sin(ψ)cos(θ)amp;sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)amp;cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)amp;−sin(θ)amp;sin(ϕ)cos(θ)amp;cos(ϕ)cos(θ)⎦⎤Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)⎦⎤
- 程序中用
二維數組att_matrix
表示從機體座標系轉到地理座標系的旋轉矩陣,是上面那個的轉置
attmatrix=Caw=(Cwa)T=[cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)]att_matrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)amp;sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)amp;sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)amp;sin(ϕ)cos(θ)amp;cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)amp;cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)amp;cos(ϕ)cos(θ)⎦⎤attmatrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)⎦⎤
- 注意這裏繞Y軸轉角θθθ (pitch),繞X軸轉角ϕϕϕ (roll)都是很小的角
(2)相關代碼分析
#