姿態篇：一.初識姿態估計

[深入淺出多旋翼飛控開發][姿態篇][一][初識姿態估計]

作者：BlueSky
QQ : 352707983
Github

一.什麼是姿態

在學習姿態估計之前，我們先來了解一下，什麼是“姿態”。

想象一架飛機準備起飛，於是它在機場跑道上進行一段加速助跑，達到一定速度後，機頭擡升15度，騰空而起，離開了地面。然而由於起飛時的方向和目標方向相差甚大，於是飛機調轉機頭，最終往北偏東30度的方向飛去。

這個過程中，根據日常生活經驗，我們如此說明飛機在起飛過程中的發生的變化：“擡升15度”，“北偏東30度”。也就是說，通常我們會使用“角度”，來表示一個物體的姿態。那這個角度大小是如何得來的？其中，“15度”指飛機機身與水平地面的夾角，“北偏東30度”則指飛行方向與正北方向的夾角。也就是說，日常生活中我們描述物體的姿態，是以腳下的大地作爲參考的。通常人類在太空中會失去方向感，是因爲地球上的重力加速度的存在讓我們始終能夠以大地作爲參考物，而失重環境下失去了參考物，我們便無法感知自身以及周圍物體的姿態了。

爲了方便描述物體之間的方位關係，我們定義了座標系。在上面的例子中，我們可以定義成兩個座標系，如圖1：

• 大地座標系O-xyz
• 機體座標系P-vuw

座標系定義

於是，我們將姿態定義爲：

姿態，即表示O-xyz與P-vuw兩個座標系之間的關係。

通常，座標系會有多個，但必須選擇一個座標系作爲參考系，大部分時候我們會選取大地座標系作爲參考系，比較符合人類的直覺與習慣。

瞭解了姿態的定義後，我們又面臨着一個問題，如何使用數學方式來描述姿態，以便下一步的計算？

二.歐拉角

歐拉角是最直觀的一種姿態描述方式，其定義爲：

一個座標系到另一個座標系的變換，可以通過繞不同座標軸的3次連續轉動來實現。這三次的轉動角度統一稱之爲歐拉角

但有一點值得注意的是，歐拉角並不直接等同於我們常說的姿態角。

歐拉角由三次繞軸旋轉組成，而這三次轉動順序任意，且轉動軸可以是參考系的，也可以是機體系的，因此共有24種轉動方式。

而通常飛控上所使用的姿態角，指的是航空領域主要應用的航空次序歐拉角，也叫卡爾丹角（Tait-Bryan angles），定義歐拉角的轉動順序爲Z-Y-X。其中繞Z軸轉動爲偏航角（Yaw），繞Y軸轉動爲俯仰角（Pitch），繞X軸轉動爲橫滾角（Roll），其具體意義如下：

• 偏航角（Yaw）
  機體系x軸投影到水平面與參考系x軸的夾角，順時針旋轉爲正。

• 俯仰角（Pitch）
  機體系x軸與水平面的夾角，擡頭爲正。

• 橫滾角（Roll）
  機體座標系z軸與通過機體系x軸的鉛垂面間的夾角，機體右旋爲正。

圖2表示了姿態歐拉角的轉動方向

Z-Y-X次序歐拉角

上面提到了，歐拉角意義上並不直接等於姿態角，它只是描述了三次繞軸轉動。而我們使用的姿態角：偏航角、橫滾角與俯仰角實際上是機體座標系與參考座標系（大地座標系）之間關係的表述。大致清楚姿態角的概念後，接下來便了解一下，如何使用傳感器測量姿態角。

三.姿態的測量

1.加速度計測量俯仰與橫滾角

假設飛控處於靜止狀態，此時加速度計僅受到重力的作用。當飛控與水平面完全平行時，此時加速度計的輸出理論值爲[0,0,g]$[0,0,g]$ （g爲重力加速度的大小）。而當飛控以一定角度傾斜於水平面時，其輸出值可能爲[0.37g,−0.5g,0.78g]$[0.37g,-0.5g,0.78g]$ ，且其模值爲1g。實際上，靜止狀態下飛控傾斜時，加速度計所測量到的數據，便是重力加速度在機體座標系下的投影。

於是根據加速度計的測量值與三角函數關係，我們便能計算出飛控的姿態角。不過由於偏航角定義爲機體系x軸投影到水平面與參考系x軸的夾角，而重力加速度正好完全正交於水平面，因此加速度計測量值中並沒有包含偏航角信息。

加速度與姿態角之間的三角函數關係

姿態角與重力加速度投影之間的三角函數關係，這裏就不具體解釋了，直接給出結論，在本文的下一節中我們再從數學的角度來推導出姿態角公式。

設當前有歸一化的加速度向量a=[ax,ay,az]$a=[a_x,a_y,a_z]$ ，飛機橫滾角爲anglex$angl{e}_x$ ，俯仰角爲angley$angl{e}_y$ ，有如下關係：

{anglex=−arcsin(ay)angley=arctan2(ax,az)$$\{\begin{array}{l}angl{e}_x=-arcsin(a_y)\\ angl{e}_y=arctan2(a_x,a_z)\end{array}$$

在天穹飛控的vector3.c文件中定義了AccVectorToRollPitchAngle函數來實現該轉換：

void AccVectorToRollPitchAngle(Vector3f_t* angle, Vector3f_t vector)
{
    angle->x = -asinf(vector.y);            //橫滾角
    angle->y = atan2f(vector.x, vector.z);  //俯仰角
}

2.磁力計測量偏航角

上面我們利用加速度計測得飛機的橫滾與俯仰姿態角，但還有一個偏航角是未知的。偏航角有許多方式可以獲得，但是最簡單和最低成本的方式，還是通過磁力計進行測量。

假設當前有磁力計測量得到的磁場強度向量m=[mx,my,mz]$m=[m_x,m_y,m_z]$ ，然後計算得到其在大地座標系下的投影mn=[mnx,mny,mnz]$m_n=[m_{nx},m_{ny},m_{nz}]$ （具體的轉換方式請閱下一節）。

設飛機的偏航角爲anglez$angl{e}_z$ ，有：

anglez=−arctan2(mny,mnx)$$angl{e}_z=-arctan2(m_{ny},m_{nx})$$

該公式的推導請參閱下節。同樣，在天穹飛控中定義了MagVectorToYawAngle函數來實現該轉換：

void MagVectorToYawAngle(Vector3f_t* angle, Vector3f_t vector)
{
    angle->z = -atan2f(vector.y, vector.x);     //偏航角
}

3.關於陀螺儀

至此，我們已經瞭解完畢如何使用加速度計和磁力計來測量飛機的姿態角。但你可能發現，飛控中最重要的一個傳感器：陀螺儀，還沒有登場。既然使用上述兩個傳感器就已經能得到飛機的姿態，那還需要陀螺儀做什麼呢？

實際上，使用加速度計和磁力計計算姿態的前提是，飛機處於一個理想的靜止狀態下。然而現實中這是不可能的，那麼當飛機處於飛行狀態時，加速度計測量得到的數據，便不僅僅是重力加速度了，可能還包含了飛行過程中所產生的運動加速度，以及一些有害加速度，通常由於機身震動，還會引入額外的震動噪聲（也屬於線性加速度）。此時直接使用加速度數據計算姿態，最終得到的是帶有了巨大誤差以及噪聲的值，對於飛控來說是完全不可用的。同樣，磁力計的實際測量噪聲一般也比較大，而且在許多環境下還會存在磁場干擾，從而導致直接使用磁力計計算出來的偏航角存在大量誤差而不可用。

而陀螺儀是一種可以測量旋轉角速度的傳感器，對角速度值進行離散化數值積分，便得到某段時間內的姿態變化量，這就是我們使用陀螺儀來計算飛機姿態的方式。但需要注意，使用陀螺儀只能計算出飛機姿態的相對變化量，而無法直接測量絕對姿態值，因此使用陀螺儀來更新飛機姿態時，必須先得知初始姿態值。

相比加速度計和磁力計，陀螺儀幾乎不受外界環境干擾，且對線性加速度（震動）不敏感，這些特性使得陀螺儀成爲了飛行器中最核心的傳感器，甚至在某些極端情況下（比如火箭的起飛加速階段），只有陀螺儀可用，這時陀螺儀的精度便至關重要了。實際上，儘管陀螺儀擁有如此優秀的特性，卻也不是完美的，自身會存在各種誤差，而在角速度積分階段，陀螺儀誤差會被累積，從而使得姿態逐漸偏離真實值。

經驗告訴我們，使用單一傳感器來計算獲取飛機的姿態是不現實的，所以實際應用中，需要同時使用多種傳感器來計算姿態。上面我們已經瞭解了使用加速度計和磁力計來計算姿態角的方法，下面便接着學習如何使用陀螺儀來更新姿態。在這之前，必須要先介紹姿態的另一種描述方式：方向餘弦矩陣。

四.方向餘弦矩陣

1.方向餘弦矩陣的定義

第二節中我們定義姿態歐拉角的轉動順序爲Z-Y-X。

根據天穹飛控中座標系和旋轉方向的定義，將姿態角的單次轉動轉化爲矩陣的形式，有：

• 繞Z軸轉動（偏航角z$z$ ）的旋轉矩陣

Cz=⎡⎣⎢cosz−sinz0sinzcosz0001⎤⎦⎥$$C_z=\left[\begin{array}{cccc}cosz& sinz& 0& \\ -sinz& cosz& 0& \\ 0& 0& 1& \end{array}\right]$$

• 繞Y軸轉動（俯仰角y$y$ ）的旋轉矩陣

Cy=⎡⎣⎢cosy0−siny010siny0cosy⎤⎦⎥$$C_y=\left[\begin{array}{cccc}cosy& 0& siny& \\ 0& 1& 0& \\ -siny& 0& cosy& \end{array}\right]$$

• 繞X軸轉動（橫滾角x$x$ ）的旋轉矩陣
  
  Cx=⎡⎣⎢1000cosxsinx0−sinxcosx⎤⎦⎥$$C_x=\left[\begin{array}{cccc}1& 0& 0& \\ 0& cosx& -sinx& \\ 0& sinx& cosx& \end{array}\right]$$

將上面的3個旋轉矩陣以Z-Y-X的轉動順序進行連乘，我們便得到了一個可以表示此次歐拉轉動的旋轉矩陣C$C$ ，計算過程如下：

C=Cz∗Cy∗Cx$$C=C_z\ast C_y\ast C_x$$

=⎡⎣⎢cosz−sinz0sinzcosz0001⎤⎦⎥⎡⎣⎢cosy0−siny010siny0cosy⎤⎦⎥⎡⎣⎢1000cosxsinx0−sinxcosx⎤⎦⎥$$=\left[\begin{array}{cccc}cosz& sinz& 0& \\ -sinz& cosz& 0& \\ 0& 0& 1& \end{array}\right]\left[\begin{array}{cccc}cosy& 0& siny& \\ 0& 1& 0& \\ -siny& 0& cosy& \end{array}\right]\left[\begin{array}{cccc}1& 0& 0& \\ 0& cosx& -sinx& \\ 0& sinx& cosx& \end{array}\right]$$

=⎡⎣⎢coszcosy−sinzcosy−sinysinzcosz0coszsiny−sinzsinycosy⎤⎦⎥⎡⎣⎢1000cosxsinx0−sinxcosx⎤⎦⎥$$=\left[\begin{array}{cccc}coszcosy& sinz& coszsiny& \\ -sinzcosy& cosz& -sinzsiny& \\ -siny& 0& cosy& \end{array}\right]\left[\begin{array}{cccc}1& 0& 0& \\ 0& cosx& -sinx& \\ 0& sinx& cosx& \end{array}\right]$$

=⎡⎣⎢cosycosz−sinzcosy−sinysinzcosx+sinxsinycoszcosxcosz−sinxsinysinzsinxcosy−sinxsinz+sinycosxcosz−sinxcosz−sinysinzcosxcosxcosy⎤⎦⎥$$=\left[\begin{array}{cccc}cosycosz& sinzcosx+sinxsinycosz& -sinxsinz+sinycosxcosz& \\ -sinzcosy& cosxcosz-sinxsinysinz& -sinxcosz-sinysinzcosx& \\ -siny& sinxcosy& cosxcosy& \end{array}\right]$$

旋轉矩陣C$C$ ，也被稱之爲方向餘弦矩陣（Direction Cosine Matrix，簡稱DCM），同樣可用於表示兩個座標系之間的關係。這裏的方向餘弦矩陣C$C$ ，實際上描述了從參考系到機體系的轉動。

由於轉動順序以及轉動軸的不一致，DCM的最終表示形式會有很多種，所以在網上看到的DCM公式各不一致，實際使用中要根據座標系的定義及歐拉角的旋轉次序來計算DCM。

利用小角度近似簡化方向餘弦矩陣

當旋轉角足夠小時，根據三角函數的小角度近似關係，有：

{sinθ≈θcosθ≈1$$\{\begin{array}{l}sin\theta \approx \theta \\ cos\theta \approx 1\end{array}$$

並忽略小角度連乘，可以得到方向餘弦矩陣的簡化形式

C=⎡⎣⎢1−z−yz1xy−x1⎤⎦⎥$$C=\left[\begin{array}{cccc}1& z& y& \\ -z& 1& -x& \\ -y& x& 1& \end{array}\right]$$

當然這樣會損失一定的計算精度，且只能在角度變化量較小時使用。過去由於單片機資源有限，運行速度較慢，因此在許多8位機上會選擇使用該簡化版的DCM，以節省大量運算時間（三角函數運算比較費時）。而現在飛控主控的運行速度已經足夠快，這種用法已經很少見了。

歐拉角轉換爲方向餘弦矩陣

在方向餘弦矩陣的定義中可以得到使用歐拉角轉化爲方向餘弦矩陣的計算公式。在天穹飛控中，定義了EulerAngleToDCM函數：

void EulerAngleToDCM(Vector3f_t angle, float* dcM)
{
    Vector3f_t cos, sin;

    cos.x = cosf(angle.x);
    cos.y = cosf(angle.y);
    cos.z = cosf(angle.z);   
    sin.x = sinf(angle.x);
    sin.y = sinf(angle.y);
    sin.z = sinf(angle.z);    

    dcM[0] = cos.y * cos.z; 
    dcM[1] = sin.z * cos.x + sin.x * sin.y * cos.z; 
    dcM[2] = -sin.x * sin.z + sin.y * cos.x * cos.z; 
    dcM[3] = -sin.z * cos.y;
    dcM[4] = cos.x * cos.z - sin.x * sin.y * sin.z;
    dcM[5] = -sin.x * cos.z - sin.y * sin.z * cos.x; 
    dcM[6] = -sin.y;
    dcM[7] = sin.x * cos.y;
    dcM[8] = cos.x * cos.y;
}

該函數將歐拉角轉換爲了方向餘弦矩陣，其角度輸入值可以是某段時間內的由陀螺儀測量到的角度變化量，也可以是當前飛機的姿態角。

2.方向餘弦矩陣的使用舉例

從參考系到機體系

已知參考系下的加速度向量An=[0,0,g]T$A_n=[0,0,g{]}^T$ ，當前飛機姿態角分別爲

⎧⎩⎨x（橫滾角）y（俯仰角）z（偏航角）$$\{\begin{array}{l}x（橫滾角）\\ y（俯仰角）\\ z（偏航角）\end{array}$$

定義表示從參考系旋轉到機體系的方向餘弦矩陣爲Cbn$C_n^b$ ，那麼我們便能計算得到參考系加速度向量在機體系下的投影Ab$A_b$ ：

Ab=Cbn∗An$$A_b=C_n^b\ast A_n$$

其中

Cbn=⎡⎣⎢cosycosz−sinzcosy−sinysinzcosx+sinxsinycoszcosxcosz−sinxsinysinzsinxcosy−sinxsinz+sinycosxcosz−sinxcosz−sinysinzcosxcosxcosy⎤⎦⎥$$C_n^b=\left[\begin{array}{cccc}cosycosz& sinzcosx+sinxsinycosz& -sinxsinz+sinycosxcosz& \\ -sinzcosy& cosxcosz-sinxsinysinz& -sinxcosz-sinysinzcosx& \\ -siny& sinxcosy& cosxcosy& \end{array}\right]$$

同樣在天穹飛控的vector3.c文件中，定義了VectorRotateToBodyFrame函數，用於轉換向量至機體座標系：

Vector3f_t VectorRotateToBodyFrame(Vector3f_t vector, Vector3f_t deltaAngle)
{
    float dcMat[9];

    //歐拉角轉爲方向餘弦矩陣
    EulerAngleToDCM(deltaAngle, dcMat);

    //方向餘弦矩陣乘以向量，得到旋轉後的新向量
    return Matrix3MulVector3(dcMat, vector);
}

從機體系到參考系

已知參考系的加速度向量在機體下的投影Ab=[0.2g,0.3g,0.9g]T$A_b=[0.2g,0.3g,0.9g{]}^T$ （在飛控中即爲加速度傳感器的測量值），當前飛機姿態角爲x,y,z$x,y,z$ ，定義表示從機體系旋轉到參考系的方向餘弦矩陣爲Cnb$C_b^n$ ，因此可以計算得到原參考系下的加速度向量An$A_n$ ：

An=Cnb∗Ab$$A_n=C_b^n\ast A_b$$

可以看出，Cnb$C_b^n$ 等於從參考系旋轉到機體系的方向餘弦矩陣Cbn$C_n^b$ 的轉置，即：

Cnb=(Cbn)T$$C_b^n=(C_n^b{)}^T$$

=⎡⎣⎢cosycoszsinzcosx+sinxsinycosz−sinxsinz+sinycosxcosz−sinzcosycosxcosz−sinxsinysinz−sinxcosz−sinysinzcosx−sinysinxcosycosxcosy⎤⎦⎥$$=\left[\begin{array}{cccc}cosycosz& -sinzcosy& -siny& \\ sinzcosx+sinxsinycosz& cosxcosz-sinxsinysinz& sinxcosy& \\ -sinxsinz+sinycosxcosz& -sinxcosz-sinysinzcosx& cosxcosy& \end{array}\right]$$

在vector3.c中，定義了函數VectorRotateToEarthFrame，用於轉換向量至參考系（大地座標系）：

Vector3f_t VectorRotateToEarthFrame(Vector3f_t vector, Vector3f_t deltaAngle)
{
    float dcMat[9];

    //歐拉角轉爲方向餘弦矩陣
    EulerAngleToDCM_T(deltaAngle, dcMat);

    //方向餘弦矩陣乘以向量，得到旋轉後的新向量
    return Matrix3MulVector3(dcMat, vector);
}

注意函數中使用的是歐拉角構建的方向餘弦矩陣的轉置。

3.利用方向餘弦矩陣推導姿態角公式

橫滾與俯仰角

設靜止狀態下加速度計測量到的加速度向量爲Ab$A_b$ ，並對其歸一化，有Ab=[ax,ay,az]T$A_b=[a_x,a_y,a_z{]}^T$ ，An$A_n$ 爲參考系下的歸一化重力加速度向量，有An=[0,0,1]T$A_n=[0,0,1{]}^T$ ，由第二小節中的方向餘弦矩陣旋轉關係，即：

Ab=Cbn∗An$$A_b=C_n^b\ast A_n$$

可得：

⎡⎣⎢axayaz⎤⎦⎥=⎡⎣⎢cosycosz−sinzcosy−sinysinzcosx+sinxsinycoszcosxcosz−sinxsinysinzsinxcosy−sinxsinz+sinycosxcosz−sinxcosz−sinysinzcosxcosxcosy⎤⎦⎥⎡⎣⎢001⎤⎦⎥$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{cccc}cosycosz& sinzcosx+sinxsinycosz& -sinxsinz+sinycosxcosz& \\ -sinzcosy& cosxcosz-sinxsinysinz& -sinxcosz-sinysinzcosx& \\ -siny& sinxcosy& cosxcosy& \end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

從而得到以下關係：

⎧⎩⎨ax=−sinxsinz+sinycosxcoszay=−sinxcosz−sinysinzcosxaz=cosxcosy$$\{\begin{array}{l}{a}_x=-sinxsinz+sinycosxcosz\\ a_y=-sinxcosz-sinysinzcosx\\ a_z=cosxcosy\end{array}$$

其中，z（偏航角）爲無關變量（繞z軸的任意旋轉都不會改變重力加速度向量），因此z=0$z=0$ ，對上式化簡可得：

⎧⎩⎨ax=sinycosxay=−sinxaz=cosxcosy$$\{\begin{array}{l}{a}_x=sinycosx\\ a_y=-sinx\\ a_z=cosxcosy\end{array}$$

即：ax/az=tany$a_x/a_z=tany$ ，ay=−sinx$a_y=-sinx$ ，於是我們得到了橫滾與俯仰角的計算公式：

{anglex=−arcsin(ay)angley=arctan2(ax,az)$$\{\begin{array}{l}angl{e}_x=-arcsin(a_y)\\ angl{e}_y=arctan2(a_x,a_z)\end{array}$$

偏航角

推導方式類似，不同的是參考系下的歸一化地磁場向量爲Mn=[1,0,0]T$M_n=[1,0,0{]}^T$ ，利用同樣的轉換關係，可以得到：

⎧⎩⎨mnx=cosycoszmny=−sinzcosymnz=−siny$$\{\begin{array}{l}{m}_{nx}=cosycosz\\ m_{ny}=-sinzcosy\\ m_{nz}=-siny\end{array}$$

即：my/mx=−tanz$m_y/m_x=-tanz$ ，從而得出偏航角的計算公式爲：

anglez=−arctan2(my,mx)$$angl{e}_z=-arctan2(m_y,m_x)$$

到這裏便結束了？並沒有。
與使用加速度向量計算橫滾俯仰角不一樣的是，上面計算所使用的mnx,mny,mnz$m_{nx},m_{ny},m_{nz}$ ，並非磁力計的原始測量數據。原因是在姿態角的旋轉順序定義Z-Y-X中，偏航角是最先發生旋轉的，此時地磁場向量仍與水平面平行，即該向量實則爲機體系磁場向量在水平面上的投影，設爲mn$m_n$ ，設機體系下的地磁場向量爲m$m$ （磁力計的測量值），有如下關係：

mn=Cnb∗m=(Cbn)T∗m$$m_n=C_b^n\ast m=(C_n^b{)}^T\ast m$$

其中Cbn$C_n^b$ 爲由當前姿態角x,y,z$x,y,z$ （z=0）構成的方向餘弦矩陣。這一步實則爲地磁場向量的座標系轉換，在網上的許多相關資料中被描述爲“傾斜補償”。

五.姿態更新

上一節中我們瞭解了方向餘弦矩陣的基本定義及其使用方法，本節中將重點講解，如何通過方向餘弦矩陣的方式，使用陀螺儀數據進行姿態更新。

1.數值積分

學習過高中物理的童鞋，對積分的概念肯定比較熟悉。已知一個速度v$v$ 與時間t$t$ 的相關函數f(v,t)$f(v,t)$ ，在某時間段內對其積分，有s=∫t1t0f(v,t)$s={\int }_{t0}^{t1}f(v,t)$ ，s即爲t0到t1時刻的位移。在該式子中，我們需要求得f(v,t)$f(v,t)$ 的定積分公式，然後代入t0$t_0$ 和t1$t_1$ ，求出s。

然而在現實中，絕大部分函數是無法得出其定積分公式的，所以通常我們使用數值逼近的方法來計算給定函數的定積分值。

一階矩形積分

設有一角度函數θ$\theta$ ，其微分方程爲：

θ′=f(ω,t)$${\theta }^{\mathrm{\prime }}=f(\omega ,t)$$

f(ω,t)$f(\omega ,t)$ 即爲角速度ω$\omega$ 對時間t$t$ 的相關函數。

從微積分學中得知微分方程的定義可以被近似爲：

θ′=f(ω,t)=θ(t+h)−θ(t)h=θk−θk−1h（當h足夠小時）$${\theta }^{\mathrm{\prime }}=f(\omega ,t)=\frac{\theta (t+h)-\theta (t)}{h}=\frac{{\theta }_k-{\theta }_{k-1}}{h}（當h足夠小時）$$

重新整理形式，可得：

θk=θk−1+h∗f(ω,t)$${\theta }_k={\theta }_{k-1}+h\ast f(\omega ,t)$$

這便是一階積分的公式，其幾何意義如下圖。可以看出，一階積分實際上等同於將原函數分解成無數個寬度爲固定時間間隔h$h$ ，高度爲f(ω,t)$f(\omega ,t)$ 的矩形，然後計算這些矩形的面積並求和，從而得到了原函數的積分近似值。選取的時間間隔h$h$ 越小，積分精度越高。

一階矩形積分的幾何意義

二階梯形積分

對於上述例子，可以得到另一種微分方程的近似形式：

θk=θk−1+0.5∗h∗(f(ω,t)+f(ω,t+h))$${\theta }_k={\theta }_{k-1}+0.5\ast h\ast (f(\omega ,t)+f(\omega ,t+h))$$

其幾何意義如下圖。

二階梯形積分的幾何意義

明顯可以看出這種對梯形求面積的二階積分方式的精度要高於一階矩形積分。

積分方式的選擇

上面我們瞭解到了兩種數值積分方式：一階矩形積分和二階梯形積分。實際上它們還有另一種叫法：一階龍格庫塔積分和二階龍格庫塔積分，往上還有更高階數的龍格庫塔積分算法。

顯然，對於特定方程以及同等積分步長h$h$ ，使用高階數的龍格庫塔法可以得到更高的積分精度。然而在天穹飛控中，我們選擇使用一階積分，這是因爲使用較高的積分頻率（積分步長h$h$ 很小）如1000Hz時，其積分效果實測基本完全一致。原因是此時傳感器自身的誤差已經遠遠大於不同積分算法所帶來的誤差。

2.使用陀螺儀更新姿態

設有陀螺儀在某時刻的角速度採樣值ω=(ωx,ωy,ωz)$\omega =({\omega }_x,{\omega }_y,{\omega }_z)$ ，採樣時間間隔爲Δt$\mathrm{\Delta }t$ ，使用一階積分算法，可以得到Δt$\mathrm{\Delta }t$ 間隔內的角度變化量爲：

⎧⎩⎨Δx=ωx∗ΔtΔy=ωy∗ΔtΔz=ωz∗Δt$$\{\begin{array}{l}\mathrm{\Delta }x={\omega }_x\ast \mathrm{\Delta }t\\ \mathrm{\Delta }y={\omega }_y\ast \mathrm{\Delta }t\\ \mathrm{\Delta }z={\omega }_z\ast \mathrm{\Delta }t\end{array}$$

於是可以得到一個描述物體在Δt$\mathrm{\Delta }t$ 間隔內的轉動的方向餘弦矩陣CΔt$C_{\mathrm{\Delta }t}$ ：

CΔt=⎡⎣⎢cosΔycosΔz−sinΔzcosΔy−sinΔysinΔzcosΔx+sinΔxsinΔycosΔzcosΔxcosΔz−sinΔxsinΔysinΔzsinΔxcosΔy−sinΔxsinΔz+sinΔycosΔxcosΔz−sinΔxcosΔz−sinΔysinΔzcosΔxcosΔxcosΔy⎤⎦⎥$$C_{\mathrm{\Delta }t}=\left[\begin{array}{cccc}cos\mathrm{\Delta }ycos\mathrm{\Delta }z& sin\mathrm{\Delta }zcos\mathrm{\Delta }x+sin\mathrm{\Delta }xsin\mathrm{\Delta }ycos\mathrm{\Delta }z& -sin\mathrm{\Delta }xsin\mathrm{\Delta }z+sin\mathrm{\Delta }ycos\mathrm{\Delta }xcos\mathrm{\Delta }z& \\ -sin\mathrm{\Delta }zcos\mathrm{\Delta }y& cos\mathrm{\Delta }xcos\mathrm{\Delta }z-sin\mathrm{\Delta }xsin\mathrm{\Delta }ysin\mathrm{\Delta }z& -sin\mathrm{\Delta }xcos\mathrm{\Delta }z-sin\mathrm{\Delta }ysin\mathrm{\Delta }zcos\mathrm{\Delta }x& \\ -sin\mathrm{\Delta }y& sin\mathrm{\Delta }xcos\mathrm{\Delta }y& cos\mathrm{\Delta }xcos\mathrm{\Delta }y& \end{array}\right]$$

有上節中方向餘弦矩陣的使用方式得知，我們可以使用CΔt$C_{\mathrm{\Delta }t}$ 對一個三維向量或者描述當前姿態的方向餘弦矩陣進行更新，從而實現了使用陀螺儀測量得到的角速度數據對姿態進行更新這一目的。

例如：

設At=[ax,ay,az]T$A_t=[a_x,a_y,a_z{]}^T$ 爲t時刻重力加速度向量在機體座標系下的投影，在Δt$\mathrm{\Delta }t$ 間隔內發送了一次轉動，得到了下一時刻的重力加速度向量投影At+1$A_{t+1}$ ，即：

At+1=CΔt∗At$$A_{t+1}=C_{\mathrm{\Delta }t}\ast A_t$$

該式即爲姿態的迭代公式，使用每一個新時刻地角速度數據進行迭代計算，便能不斷地獲取到新的姿態值。不過這裏的At$A_t$ 只包含了俯仰與橫滾角信息，同理如果我們需要得到偏航角，只需定義一個向量爲地磁場向量投影，然後使用CΔt$C_{\mathrm{\Delta }t}$ 對其進行迭代更新即可。

下面給出一個使用陀螺儀更新姿態的代碼示例：

void AttitudeUpdate(Vector3f_t gyro, float deltaT)
{
    Vector3f_t deltaAngle;   
    static Vector3f_t vectorG = {0, 0, 1};     //定義重力加速度向量投影並初始化
    static Vector3f_t vectorM = {1, 0, 0};     //定義地磁場向量投影並初始化  
    float  dcMat[9];                           //方向餘弦矩陣
    static Vector3f_t angle;                   //姿態角

    //一階積分計算角度變化量，單位爲弧度
    //gyro爲角速度數據，單位爲°/s，deltaT爲該函數的運行時間間隔，單位爲s
    deltaAngle.x = Radians(gyro.x * deltaT); 
    deltaAngle.y = Radians(gyro.y * deltaT); 
    deltaAngle.z = Radians(gyro.z * deltaT); 

    //角度變化量轉換爲方向餘弦矩陣
    EulerAngleToDCM(deltaAngle, dcMat);

    //更新姿態向量 vector = DCM * vector
    vectorG = Matrix3MulVector3(dcMat, vectorG);
    vectorM = Matrix3MulVector3(dcMat, vectorM);

    //計算俯仰與橫滾角，並由弧度制轉爲角度制
    AccVectorToRollPitchAngle(&angle, vectorG);   
    angle.x = Degrees(angle.x);
    angle.y = Degrees(angle.y);    

    //轉換地磁場向量投影到參考系的水平面
    Vector3f_t vectorM_Ef;
    BodyFrameToEarthFrame(angle, vectorM, &vectorM_Ef);

    //計算偏航角，並由弧度制轉爲角度制
    MagVectorToYawAngle(&angle, vectorM_Ef);
    angle.z = Degrees(angle.z);    

    //將偏航角限制爲0-360°
    angle.z = WrapDegree360(angle.z);    
}

3.姿態的初始對準

前面提過，使用陀螺儀更新姿態，有一個前提是需要先獲得姿態的初始值。在上面的代碼實例中，我們直接在變量初始化中給了一個初始值，假定初始狀態爲機體系與參考系完全貼合。但實際應用中，在飛控剛上電時，我們必須對姿態的狀態量進行初始化。最簡單直接的方法便是多次讀取加速度計與磁力計的值，然後求平均值，把最終得到的數據作爲姿態的初始值。

在天穹飛控中定義了AttitudeInitAlignment函數用於姿態初始化。

4.傳感器校準

在飛控中運行第二小節中給出的姿態更新示例，你會發現，儘管計算出來的姿態值能夠很好的反映飛控的真實姿態變化，但依然存在着一個很大的問題：當飛控靜止不動時，姿態值仍然在不斷變化，隨着時間的增加，姿態值與飛控的真實姿態之間的誤差會越來越大。

這是因爲陀螺儀傳感器本身存在着誤差。飛控靜止時，理論上陀螺儀的輸出爲0，即角速度爲0，姿態不變。但實際上即使在靜止狀態，陀螺儀依然會有輸出，這稱之爲陀螺儀的零偏誤差，姿態更新過程中，零偏誤差也會不斷被累積，最終誤差越來越大，使得姿態值逐漸偏離真實值。

爲了解決這個問題，需要對傳感器進行校準，減小甚至消除其誤差。對於陀螺儀來說，校準零偏誤差的最簡單方式，便是將飛控靜止放置，在一定時間內連續讀取其輸出，然後取平均值，得到的數據即爲零偏誤差。在後面的姿態更新中，角速度值減去該零偏誤差即可。

實際上陀螺儀的誤差組成並非如此簡單，要取得較好的校準效果需要花費很多功夫，且要有額外設備的支持。另外其餘傳感器如加速度計和磁力計，也均需要校準，其具體校準實現頗爲複雜，這裏不便展開，在本教程的【姿態篇】的第四篇中，將進行詳細講述。有興趣的童鞋可前往查看：姿態篇：四.非線性最小二乘與飛控傳感器校準。

經過校準後，陀螺儀誤差將極大減小，但是並非代表完全不存在誤差。事實上，陀螺儀的零偏誤差並不會固定不變，而是會隨着環境、溫度、時間等因素的變化而變化，這就帶來了很多麻煩。另外通常多旋翼飛控上使用的均爲低成本MEMS陀螺儀，其隨機噪聲誤差也非常大。這就表示，陀螺儀沒有誤差的理想情況在現實中是不可能存在的。因此單獨使用陀螺儀來更新姿態，其誤差會逐漸累積，無法長時間保證姿態值的準確。

下一節中，我們將介紹如何使用多傳感器融合的方式，計算出儘可能準確的姿態值。

六.姿態估計與互補濾波

估計，其意爲對事物做出大致的推斷。某一天下班後路過一個水果攤，突然想買點龍眼解解饞，可是囊中羞澀，支付寶上的餘額大約只能買2到3斤。於是找老闆要了個袋子，挑了許多龍眼，大概用手感覺了一下，應該有2斤。這時候我對龍眼重量的猜測，便是估計值。然後給老闆放到秤上一秤，2.3斤，這是真實值。而2.3 - 2 = 0.3，則是這個過程中的估計誤差。

而對於飛控而言的姿態估計，便是計算出一個最接近真實值的姿態值，也就是讓估計誤差最小。姿態的測量與計算，離不開傳感器，但是各種傳感器均存在不同的優缺點，其中：

• 陀螺儀可用於姿態的連續更新，優點是精度較高且幾乎不受外界因素干擾，缺點是自身存在誤差，在積分階段會因爲誤差不斷累積從而使得計算結果不準確。

• 加速度計和磁力計可直接用於姿態的測量，優點是絕對準確，不存在累積誤差，但缺點也很明顯，容易受到干擾。另外加速度計容易受到震動影響，並且對姿態的測量結果僅在靜止或者物體勻速運動時才準確。

單一使用任何傳感器，都無法得到有效準確的姿態信息。因此，我們需要同時使用多個傳感器，結合各自優缺點，將其數據進行融合，從而得到最準確的估計。

其中，多傳感器信息融合的最有效的一種方式爲加權融合。而通常用於融合的多個傳感器之間存在性質互補的情況，所以我們也稱之爲互補濾波。儘管名字中帶有“濾波”兩字，卻完全不同於傳統意義上的對某信號特定頻域上進行濾除操作的濾波，實則上它是一種最優估計方法。

定義如下式子：

anglet+1=(1−k)∗(anglet+ωt∗Δt)+k∗anglemeasure$$angl{e}_{t+1}=(1-k)\ast (angl{e}_t+{\omega }_t\ast \mathrm{\Delta }t)+k\ast angl{e}_{measure}$$

這就是一個簡單的使用互補濾波更新姿態的示例。其中anglet$angl{e}_t$ 和anglet+1$angl{e}_{t+1}$ 分別爲t$t$ ，t+1$t+1$ 時刻的姿態角估計值，ωt${\omega }_t$ 爲t$t$ 時刻的角速度，anglemeasure$angl{e}_{measure}$ 爲t$t$ 時刻來至於加速度計或磁力計的姿態測量值。

其中的k$k$ ，便是最核心的權重因子了。它的大小決定了t$t$ 時刻的估計中，我們相信哪個傳感器的數據多一點。比如k=0.001$k=0.001$ ，則有1−k=0.999$1-k=0.999$ ，意味着在每一次估計中，角速度所更新的姿態值要遠比來自加速度（或磁力計）的姿態測量值準確，但同時還是使用了姿態測量值對姿態進行修正，以消除角速度積分帶來的累積誤差。這樣，我們便得到了一個相對而言更準確的姿態估計：對外界環境因素的干擾不敏感，也不會因爲角速度積分累積誤差而逐漸偏離真實值。

對上式進行分解和變換，可以得到：

{anglet=anglet+ωt∗Δtanglet+1=anglet+k∗(anglemeasure−anglet)$$\{\begin{array}{l}angl{e}_t=angl{e}_t+{\omega }_t\ast \mathrm{\Delta }t\\ angl{e}_{t+1}=angl{e}_t+k\ast (angl{e}_{measure}-angl{e}_t)\end{array}$$

即先使用角速度數據更新姿態，然後使用姿態觀測值對姿態估計值進行修正，修正速度取決於k$k$ 的大小。

在上節使用角速度更新姿態的例子中，有：

At+1=CΔt∗At$$A_{t+1}=C_{\mathrm{\Delta }t}\ast A_t$$

定義t時刻的加速度觀測爲at$a_t$ ，可以得到：

{A−t=CΔt∗AtAt+1=A−t+K∗(at−A−t)$$\{\begin{array}{l}{A}_t^{-}=C_{\mathrm{\Delta }t}\ast A_t\\ A_{t+1}=A_t^{-}+K\ast (a_t-A_t^{-})\end{array}$$

其中K$K$ 是一個對角線元素爲權重因子的3x3矩陣，即：

K=⎡⎣⎢k1000k2000k3⎤⎦⎥$$K=\left[\begin{array}{cccc}{k}_1& 0& 0& \\ 0& k_2& 0& \\ 0& 0& k_3& \end{array}\right]$$

下面貼上在上一節的角速度更新姿態代碼基礎上加入互補濾波的程序實例：

void AttitudeUpdateByComplementaryFilter(Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag, float deltaT)
{
    Vector3f_t deltaAngle;   
    static Vector3f_t vectorG = {0, 0, 1};     //定義重力加速度向量投影並初始化
    static Vector3f_t vectorM = {1, 0, 0};     //定義地磁場向量投影並初始化  
    float  dcMat[9];                           //方向餘弦矩陣
    static Vector3f_t angle;                   //姿態角

    static float K_G[9] = {0.0003, 0, 0, 0, 0.0003, 0, 0, 0, 0.0003};  //重力加速度向量的互補濾波權重矩陣
    static float K_M[9] = {0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.001};     //地磁場向量的互補濾波權重矩陣

    //一階積分計算角度變化量，單位爲弧度
    //gyro爲角速度數據，單位爲°/s，deltaT爲該函數的運行時間間隔，單位爲s
    deltaAngle.x = Radians(gyro.x * deltaT); 
    deltaAngle.y = Radians(gyro.y * deltaT); 
    deltaAngle.z = Radians(gyro.z * deltaT); 

    //角度變化量轉換爲方向餘弦矩陣
    EulerAngleToDCM(deltaAngle, dcMat);

    //更新姿態向量 vector = DCM * vector
    vectorG = Matrix3MulVector3(dcMat, vectorG);
    vectorM = Matrix3MulVector3(dcMat, vectorM);

    //使用互補濾波對姿態估計量進行修正
    //acc爲加速度計測量值，mag爲磁力計測量值
    vectorG = Vector3f_Add(vectorG, Matrix3MulVector3(K_G, Vector3f_Sub(acc, vectorG)));
    vectorM = Vector3f_Add(vectorM, Matrix3MulVector3(K_M, Vector3f_Sub(mag, vectorM)));

    //計算俯仰與橫滾角，並由弧度制轉爲角度制
    AccVectorToRollPitchAngle(&angle, vectorG);   
    angle.x = Degrees(angle.x);
    angle.y = Degrees(angle.y);    

    //轉換地磁場向量投影到參考系的水平面
    Vector3f_t vectorM_Ef;
    BodyFrameToEarthFrame(angle, vectorM, &vectorM_Ef);

    //計算偏航角，並由弧度制轉爲角度制
    MagVectorToYawAngle(&angle, vectorM_Ef);
    angle.z = Degrees(angle.z);    

    //將偏航角限制爲0-360°
    angle.z = WrapDegree360(angle.z);    
}

其中Vector3f_Add和Matrix3MulVector3爲天穹飛控中定義的各類向量及矩陣運算函數，詳情請參閱Math文件夾下的代碼：天穹飛控

七.代碼與工程實例

本節中將給出一個具體的工程實例，可以直接在天穹飛控的硬件上進行運行和調試，幫助初學者對本篇內容的學習和理解。

後續再更新。

八.總結

本篇文章介紹了姿態的基本定義和表示方式，並講解了多種傳感器計算姿態的方法，以及使用多傳感器融合估計姿態的一種基本算法。結合有大量代碼實例，可以幫助初學者入門，如果依然有不明白的地方，可以上論壇或者Q羣提問，也可以同時結合Github上的天穹飛控的代碼進行學習。

這裏是傳送門：

天穹飛控
飛控交流論壇
飛控交流Q羣：472648354

作爲本教程姿態篇的開篇及入門篇，僅僅是介紹了一些基本概念及算法，然而實際環境頗爲複雜，需要考慮的因素非常多，要提高姿態估計的精度，還有很長的路要走。最後用於多傳感器融合的互補濾波算法中，其權重因子通常依靠調試和經驗給出。而下一節我們將會學習卡爾曼濾波，一種從概率角度計算融合權重的最優估計方法，順便介紹另一種常見的姿態表示方式：四元數。