[深入淺出多旋翼飛控開發(fā)][姿態(tài)篇][一][初識姿態(tài)估計]

作者：王偉韻
QQ : 352707983
Github

一.什么是姿態(tài)

在學(xué)習(xí)姿態(tài)估計之前，我們先來了解一下，什么是“姿態(tài)”。

想象一架飛機(jī)準(zhǔn)備起飛弊知，于是它在機(jī)場跑道上進(jìn)行一段加速助跑香追，達(dá)到一定速度后盲赊，機(jī)頭抬升15度，騰空而起惊窖，離開了地面刽宪。然而由于起飛時的方向和目標(biāo)方向相差甚大，于是飛機(jī)調(diào)轉(zhuǎn)機(jī)頭界酒，最終往北偏東30度的方向飛去圣拄。

這個過程中，根據(jù)日常生活經(jīng)驗毁欣，我們?nèi)绱苏f明飛機(jī)在起飛過程中的發(fā)生的變化：“抬升15度”庇谆，“北偏東30度”。也就是說凭疮，通常我們會使用“角度”族铆，來表示一個物體的姿態(tài)。那這個角度大小是如何得來的哭尝？其中，“15度”指飛機(jī)機(jī)身與水平地面的夾角剖煌，“北偏東30度”則指飛行方向與正北方向的夾角材鹦。也就是說，日常生活中我們描述物體的姿態(tài)耕姊，是以腳下的大地作為參考的桶唐。通常人類在太空中會失去方向感，是因為地球上的重力加速度的存在讓我們始終能夠以大地作為參考物茉兰，而失重環(huán)境下失去了參考物尤泽，我們便無法感知自身以及周圍物體的姿態(tài)了。

為了方便描述物體之間的方位關(guān)系，我們定義了坐標(biāo)系坯约。在上面的例子中熊咽，我們可以定義成兩個坐標(biāo)系，如圖1：

• 大地坐標(biāo)系O-xyz
• 機(jī)體坐標(biāo)系P-vuw

圖1 坐標(biāo)系定義

于是闹丐，我們將姿態(tài)定義為：

姿態(tài)横殴，即表示O-xyz與P-vuw兩個坐標(biāo)系之間的關(guān)系。

通常卿拴，坐標(biāo)系會有多個衫仑，但必須選擇一個坐標(biāo)系作為參考系，大部分時候我們會選取大地坐標(biāo)系作為參考系堕花，比較符合人類的直覺與習(xí)慣文狱。

了解了姿態(tài)的定義后，我們又面臨著一個問題缘挽，如何使用數(shù)學(xué)方式來描述姿態(tài)瞄崇，以便下一步的計算？

二.歐拉角

歐拉角是最直觀的一種姿態(tài)描述方式到踏，其定義為：

一個坐標(biāo)系到另一個坐標(biāo)系的變換杠袱，可以通過繞不同坐標(biāo)軸的3次連續(xù)轉(zhuǎn)動來實現(xiàn)。這三次的轉(zhuǎn)動角度統(tǒng)一稱之為歐拉角

但有一點值得注意的是窝稿，歐拉角并不直接等同于我們常說的姿態(tài)角楣富。

歐拉角由三次繞軸旋轉(zhuǎn)組成，而這三次轉(zhuǎn)動順序任意伴榔，且轉(zhuǎn)動軸可以是參考系的纹蝴，也可以是機(jī)體系的，因此共有24種轉(zhuǎn)動方式踪少。

而通常飛控上所使用的姿態(tài)角塘安，指的是航空領(lǐng)域主要應(yīng)用的航空次序歐拉角，也叫卡爾丹角（Tait-Bryan angles）援奢，定義歐拉角的轉(zhuǎn)動順序為Z-Y-X兼犯。其中繞Z軸轉(zhuǎn)動為偏航角（Yaw），繞Y軸轉(zhuǎn)動為俯仰角（Pitch）集漾，繞X軸轉(zhuǎn)動為橫滾角（Roll）切黔，其具體意義如下：

• 偏航角（Yaw）
  機(jī)體系x軸投影到水平面與參考系x軸的夾角，順時針旋轉(zhuǎn)為正具篇。

• 俯仰角（Pitch）
  機(jī)體系x軸與水平面的夾角纬霞，抬頭為正。

• 橫滾角（Roll）
  機(jī)體坐標(biāo)系z軸與通過機(jī)體系x軸的鉛垂面間的夾角驱显，機(jī)體右旋為正诗芜。

圖2表示了姿態(tài)歐拉角的轉(zhuǎn)動方向

圖2 Z-Y-X次序歐拉角

上面提到了瞳抓，歐拉角意義上并不直接等于姿態(tài)角，它只是描述了三次繞軸轉(zhuǎn)動伏恐。而我們使用的姿態(tài)角：偏航角孩哑、橫滾角與俯仰角實際上是機(jī)體坐標(biāo)系與參考坐標(biāo)系（大地坐標(biāo)系）之間關(guān)系的表述。大致清楚姿態(tài)角的概念后脐湾，接下來便了解一下臭笆，如何使用傳感器測量姿態(tài)角。

三.姿態(tài)的測量

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

假設(shè)飛控處于靜止?fàn)顟B(tài)，此時加速度計僅受到重力的作用。當(dāng)飛控與水平面完全平行時奔穿，此時加速度計的輸出理論值為（g為重力加速度的大小）茵乱。而當(dāng)飛控以一定角度傾斜于水平面時，其輸出值可能為孟岛，且其模值為1g瓶竭。實際上，靜止?fàn)顟B(tài)下飛控傾斜時渠羞，加速度計所測量到的數(shù)據(jù)斤贰，便是重力加速度在機(jī)體坐標(biāo)系下的投影。

于是根據(jù)加速度計的測量值與三角函數(shù)關(guān)系次询，我們便能計算出飛控的姿態(tài)角荧恍。不過由于偏航角定義為機(jī)體系x軸投影到水平面與參考系x軸的夾角，而重力加速度正好完全正交于水平面屯吊，因此加速度計測量值中并沒有包含偏航角信息送巡。

加速度與姿態(tài)角之間的三角函數(shù)關(guān)系

姿態(tài)角與重力加速度投影之間的三角函數(shù)關(guān)系，這里就不具體解釋了盒卸，直接給出結(jié)論骗爆，在本文的下一節(jié)中我們再從數(shù)學(xué)的角度來推導(dǎo)出姿態(tài)角公式。

設(shè)當(dāng)前有歸一化的加速度向量蔽介，飛機(jī)橫滾角為摘投，俯仰角為，有如下關(guān)系：

在天穹飛控的vector3.c文件中定義了AccVectorToRollPitchAngle函數(shù)來實現(xiàn)該轉(zhuǎn)換：

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

2.磁力計測量偏航角

上面我們利用加速度計測得飛機(jī)的橫滾與俯仰姿態(tài)角虹蓄，但還有一個偏航角是未知的犀呼。偏航角有許多方式可以獲得，但是最簡單和最低成本的方式武花，還是通過磁力計進(jìn)行測量。

假設(shè)當(dāng)前有磁力計測量得到的磁場強(qiáng)度向量杈帐，然后計算得到其在大地坐標(biāo)系下的投影（具體的轉(zhuǎn)換方式請閱下一節(jié)）体箕。

設(shè)飛機(jī)的偏航角為专钉，有：

該公式的推導(dǎo)請參閱下節(jié)。同樣累铅，在天穹飛控中定義了MagVectorToYawAngle函數(shù)來實現(xiàn)該轉(zhuǎn)換：

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

3.關(guān)于陀螺儀

至此跃须，我們已經(jīng)了解完畢如何使用加速度計和磁力計來測量飛機(jī)的姿態(tài)角。但你可能發(fā)現(xiàn)娃兽，飛控中最重要的一個傳感器：陀螺儀菇民，還沒有登場。既然使用上述兩個傳感器就已經(jīng)能得到飛機(jī)的姿態(tài)投储，那還需要陀螺儀做什么呢第练？

實際上，使用加速度計和磁力計計算姿態(tài)的前提是玛荞，飛機(jī)處于一個理想的靜止?fàn)顟B(tài)下娇掏。然而現(xiàn)實中這是不可能的，那么當(dāng)飛機(jī)處于飛行狀態(tài)時勋眯，加速度計測量得到的數(shù)據(jù)婴梧，便不僅僅是重力加速度了，可能還包含了飛行過程中所產(chǎn)生的運動加速度客蹋，以及一些有害加速度塞蹭，通常由于機(jī)身震動，還會引入額外的震動噪聲（也屬于線性加速度）讶坯。此時直接使用加速度數(shù)據(jù)計算姿態(tài)番电，最終得到的是帶有了巨大誤差以及噪聲的值，對于飛控來說是完全不可用的闽巩。同樣钧舌，磁力計的實際測量噪聲一般也比較大，而且在許多環(huán)境下還會存在磁場干擾涎跨，從而導(dǎo)致直接使用磁力計計算出來的偏航角存在大量誤差而不可用洼冻。

而陀螺儀是一種可以測量旋轉(zhuǎn)角速度的傳感器，對角速度值進(jìn)行離散化數(shù)值積分隅很，便得到某段時間內(nèi)的姿態(tài)變化量撞牢，這就是我們使用陀螺儀來計算飛機(jī)姿態(tài)的方式。但需要注意叔营，使用陀螺儀只能計算出飛機(jī)姿態(tài)的相對變化量屋彪，而無法直接測量絕對姿態(tài)值，因此使用陀螺儀來更新飛機(jī)姿態(tài)時绒尊，必須先得知初始姿態(tài)值畜挥。

相比加速度計和磁力計，陀螺儀幾乎不受外界環(huán)境干擾婴谱，且對線性加速度（震動）不敏感蟹但，這些特性使得陀螺儀成為了飛行器中最核心的傳感器躯泰，甚至在某些極端情況下（比如火箭的起飛加速階段），只有陀螺儀可用华糖，這時陀螺儀的精度便至關(guān)重要了麦向。實際上，盡管陀螺儀擁有如此優(yōu)秀的特性客叉，卻也不是完美的诵竭，自身會存在各種誤差，而在角速度積分階段兼搏，陀螺儀誤差會被累積卵慰，從而使得姿態(tài)逐漸偏離真實值。

經(jīng)驗告訴我們向族，使用單一傳感器來計算獲取飛機(jī)的姿態(tài)是不現(xiàn)實的呵燕，所以實際應(yīng)用中，需要同時使用多種傳感器來計算姿態(tài)件相。上面我們已經(jīng)了解了使用加速度計和磁力計來計算姿態(tài)角的方法再扭，下面便接著學(xué)習(xí)如何使用陀螺儀來更新姿態(tài)。在這之前夜矗，必須要先介紹姿態(tài)的另一種描述方式：方向余弦矩陣泛范。

四.方向余弦矩陣

1.方向余弦矩陣的定義

第二節(jié)中我們定義姿態(tài)歐拉角的轉(zhuǎn)動順序為Z-Y-X。

將姿態(tài)角的單次轉(zhuǎn)動轉(zhuǎn)化為矩陣的形式紊撕，有：

• 繞Z軸轉(zhuǎn)動（偏航角）的旋轉(zhuǎn)矩陣

• 繞Y軸轉(zhuǎn)動（俯仰角）的旋轉(zhuǎn)矩陣

• 繞X軸轉(zhuǎn)動（橫滾角）的旋轉(zhuǎn)矩陣
  

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

旋轉(zhuǎn)矩陣对扶，也被稱之為方向余弦矩陣（Direction Cosine Matrix区赵，簡稱DCM），同樣可用于表示兩個坐標(biāo)系之間的關(guān)系浪南。這里的方向余弦矩陣笼才，實際上描述了從參考系到機(jī)體系的轉(zhuǎn)動。

由于轉(zhuǎn)動順序以及轉(zhuǎn)動軸的不一致络凿，DCM的最終表示形式會有很多種骡送，所以在網(wǎng)上看到的DCM公式各不一致，實際使用中要根據(jù)坐標(biāo)系的定義及歐拉角的旋轉(zhuǎn)次序來計算DCM絮记。

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

當(dāng)旋轉(zhuǎn)角足夠小時摔踱，根據(jù)三角函數(shù)的小角度近似關(guān)系，有：

并忽略小角度連乘怨愤，可以得到方向余弦矩陣的簡化形式

當(dāng)然這樣會損失一定的計算精度派敷，且只能在角度變化量較小時使用。過去由于單片機(jī)資源有限，運行速度較慢篮愉，因此在許多8位機(jī)上會選擇使用該簡化版的DCM般眉，以節(jié)省大量運算時間（三角函數(shù)運算比較費時）。而現(xiàn)在飛控主控的運行速度已經(jīng)足夠快潜支，這種用法已經(jīng)很少見了。

歐拉角轉(zhuǎn)換為方向余弦矩陣

在方向余弦矩陣的定義中可以得到使用歐拉角轉(zhuǎn)化為方向余弦矩陣的計算公式柿汛。在天穹飛控中冗酿，定義了EulerAngleToDCM函數(shù)：

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;
}

該函數(shù)將歐拉角轉(zhuǎn)換為了方向余弦矩陣，其角度輸入值可以是某段時間內(nèi)的由陀螺儀測量到的角度變化量络断，也可以是當(dāng)前飛機(jī)的姿態(tài)角裁替。

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

從參考系到機(jī)體系

已知參考系下的加速度向量，當(dāng)前飛機(jī)姿態(tài)角分別為

定義表示從參考系旋轉(zhuǎn)到機(jī)體系的方向余弦矩陣為貌笨，那么我們便能計算得到參考系加速度向量在機(jī)體系下的投影：

其中

同樣在天穹飛控的vector3.c文件中弱判，定義了VectorRotateToBodyFrame函數(shù)，用于轉(zhuǎn)換向量至機(jī)體坐標(biāo)系：

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

    //歐拉角轉(zhuǎn)為方向余弦矩陣
    EulerAngleToDCM(deltaAngle, dcMat);

    //方向余弦矩陣乘以向量锥惋，得到旋轉(zhuǎn)后的新向量
    return Matrix3MulVector3(dcMat, vector);
}

從機(jī)體系到參考系

已知參考系的加速度向量在機(jī)體下的投影（在飛控中即為加速度傳感器的測量值）昌腰，當(dāng)前飛機(jī)姿態(tài)角為，定義表示從機(jī)體系旋轉(zhuǎn)到參考系的方向余弦矩陣為膀跌，因此可以計算得到原參考系下的加速度向量：

可以看出遭商，等于從參考系旋轉(zhuǎn)到機(jī)體系的方向余弦矩陣的轉(zhuǎn)置，即：

在vector3.c中捅伤，定義了函數(shù)VectorRotateToEarthFrame劫流，用于轉(zhuǎn)換向量至參考系（大地坐標(biāo)系）：

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

    //歐拉角轉(zhuǎn)為方向余弦矩陣
    EulerAngleToDCM_T(deltaAngle, dcMat);

    //方向余弦矩陣乘以向量，得到旋轉(zhuǎn)后的新向量
    return Matrix3MulVector3(dcMat, vector);
}

注意函數(shù)中使用的是歐拉角構(gòu)建的方向余弦矩陣的轉(zhuǎn)置丛忆。

3.利用方向余弦矩陣推導(dǎo)姿態(tài)角公式

橫滾與俯仰角

設(shè)靜止?fàn)顟B(tài)下加速度計測量到的加速度向量為祠汇，并對其歸一化，有熄诡，為參考系下的歸一化重力加速度向量可很，有，由第二小節(jié)中的方向余弦矩陣旋轉(zhuǎn)關(guān)系粮彤，即：

可得：

從而得到以下關(guān)系：

其中根穷，z（偏航角）為無關(guān)變量（繞z軸的任意旋轉(zhuǎn)都不會改變重力加速度向量），因此导坟，對上式化簡可得：

即：屿良，，于是我們得到了橫滾與俯仰角的計算公式：

偏航角

推導(dǎo)方式類似惫周，不同的是參考系下的歸一化地磁場向量為尘惧，利用同樣的轉(zhuǎn)換關(guān)系，可以得到：

即：递递，從而得出偏航角的計算公式為：

到這里便結(jié)束了喷橙？并沒有啥么。
與使用加速度向量計算橫滾俯仰角不一樣的是，上面計算所使用的贰逾，并非磁力計的原始測量數(shù)據(jù)悬荣。原因是在姿態(tài)角的旋轉(zhuǎn)順序定義Z-Y-X中，偏航角是最先發(fā)生旋轉(zhuǎn)的疙剑，此時地磁場向量仍與水平面平行氯迂，即該向量實則為機(jī)體系磁場向量在水平面上的投影，設(shè)為言缤，設(shè)機(jī)體系下的地磁場向量為（磁力計的測量值）嚼蚀，有如下關(guān)系：

其中 為由當(dāng)前姿態(tài)角（z=0）構(gòu)成的方向余弦矩陣。這一步實則為地磁場向量的坐標(biāo)系轉(zhuǎn)換管挟，在網(wǎng)上的許多相關(guān)資料中被描述為“傾斜補(bǔ)償”轿曙。

五.姿態(tài)更新

上一節(jié)中我們了解了方向余弦矩陣的基本定義及其使用方法，本節(jié)中將重點講解僻孝，如何通過方向余弦矩陣的方式导帝，使用陀螺儀數(shù)據(jù)進(jìn)行姿態(tài)更新。

1.數(shù)值積分

學(xué)習(xí)過高中物理的童鞋穿铆，對積分的概念肯定比較熟悉舟扎。已知一個速度與時間的相關(guān)函數(shù)，在某時間段內(nèi)對其積分悴务，有睹限，s即為t0到t1時刻的位移。在該式子中讯檐，我們需要求得的定積分公式羡疗，然后代入和，求出s别洪。

然而在現(xiàn)實中叨恨，絕大部分函數(shù)是無法得出其定積分公式的，所以通常我們使用數(shù)值逼近的方法來計算給定函數(shù)的定積分值挖垛。

一階矩形積分

設(shè)有一角度函數(shù)痒钝，其微分方程為：

即為角速度對時間的相關(guān)函數(shù)。

從微積分學(xué)中得知微分方程的定義可以被近似為：

重新整理形式痢毒，可得：

這便是一階積分的公式送矩，其幾何意義如下圖∧奶妫可以看出栋荸，一階積分實際上等同于將原函數(shù)分解成無數(shù)個寬度為固定時間間隔，高度為的矩形，然后計算這些矩形的面積并求和晌块，從而得到了原函數(shù)的積分近似值爱沟。選取的時間間隔越小，積分精度越高匆背。

一階矩形積分的幾何意義

二階梯形積分

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

其幾何意義如下圖。

二階梯形積分的幾何意義

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

積分方式的選擇

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

顯然，對于特定方程以及同等積分步長兄墅，使用高階數(shù)的龍格庫塔法可以得到更高的積分精度踢星。然而在天穹飛控中，我們選擇使用一階積分隙咸，這是因為使用較高的積分頻率（積分步長很秀逶谩）如1000Hz時，其積分效果實測基本完全一致五督。原因是此時傳感器自身的誤差已經(jīng)遠(yuǎn)遠(yuǎn)大于不同積分算法所帶來的誤差藏否。

2.使用陀螺儀更新姿態(tài)

設(shè)有陀螺儀在某時刻的角速度采樣值，采樣時間間隔為充包，使用一階積分算法副签，可以得到間隔內(nèi)的角度變化量為：

于是可以得到一個描述物體在間隔內(nèi)的轉(zhuǎn)動的方向余弦矩陣：

有上節(jié)中方向余弦矩陣的使用方式得知，我們可以使用對一個三維向量或者描述當(dāng)前姿態(tài)的方向余弦矩陣進(jìn)行更新基矮，從而實現(xiàn)了使用陀螺儀測量得到的角速度數(shù)據(jù)對姿態(tài)進(jìn)行更新這一目的淆储。

例如：

設(shè) 為t時刻重力加速度向量在機(jī)體坐標(biāo)系下的投影，在間隔內(nèi)發(fā)送了一次轉(zhuǎn)動家浇，得到了下一時刻的重力加速度向量投影 本砰，即：

該式即為姿態(tài)的迭代公式，使用每一個新時刻地角速度數(shù)據(jù)進(jìn)行迭代計算钢悲，便能不斷地獲取到新的姿態(tài)值点额。不過這里的只包含了俯仰與橫滾角信息，同理如果我們需要得到偏航角莺琳，只需定義一個向量為地磁場向量投影还棱，然后使用對其進(jìn)行迭代更新即可。

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

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;                   //姿態(tài)角
    
    //一階積分計算角度變化量惭等，單位為弧度
    //gyro為角速度數(shù)據(jù)诱贿，單位為°/s，deltaT為該函數(shù)的運行時間間隔，單位為s
    deltaAngle.x = Radians(gyro.x * deltaT); 
    deltaAngle.y = Radians(gyro.y * deltaT); 
    deltaAngle.z = Radians(gyro.z * deltaT); 
    
    //角度變化量轉(zhuǎn)換為方向余弦矩陣
    EulerAngleToDCM(deltaAngle, dcMat);
    
    //更新姿態(tài)向量 vector = DCM * vector
    vectorG = Matrix3MulVector3(dcMat, vectorG);
    vectorM = Matrix3MulVector3(dcMat, vectorM);
    
    //計算俯仰與橫滾角珠十，并由弧度制轉(zhuǎn)為角度制
    AccVectorToRollPitchAngle(&angle, vectorG);   
    angle.x = Degrees(angle.x);
    angle.y = Degrees(angle.y);    

    //轉(zhuǎn)換地磁場向量投影到參考系的水平面
    Vector3f_t vectorM_Ef;
    BodyFrameToEarthFrame(angle, vectorM, &vectorM_Ef);
    
    //計算偏航角料扰，并由弧度制轉(zhuǎn)為角度制
    MagVectorToYawAngle(&angle, vectorM_Ef);
    angle.z = Degrees(angle.z);    
    
    //將偏航角限制為0-360°
    angle.z = WrapDegree360(angle.z);    
}

3.姿態(tài)的初始對準(zhǔn)

前面提過，使用陀螺儀更新姿態(tài)焙蹭，有一個前提是需要先獲得姿態(tài)的初始值晒杈。在上面的代碼實例中，我們直接在變量初始化中給了一個初始值孔厉，假定初始狀態(tài)為機(jī)體系與參考系完全貼合拯钻。但實際應(yīng)用中，在飛控剛上電時撰豺，我們必須對姿態(tài)的狀態(tài)量進(jìn)行初始化粪般。最簡單直接的方法便是多次讀取加速度計與磁力計的值，然后求平均值污桦，把最終得到的數(shù)據(jù)作為姿態(tài)的初始值亩歹。

在天穹飛控中定義了AttitudeInitAlignment函數(shù)用于姿態(tài)初始化。

4.傳感器校準(zhǔn)

在飛控中運行第二小節(jié)中給出的姿態(tài)更新示例凡橱，你會發(fā)現(xiàn)小作，盡管計算出來的姿態(tài)值能夠很好的反映飛控的真實姿態(tài)變化，但依然存在著一個很大的問題：當(dāng)飛控靜止不動時稼钩，姿態(tài)值仍然在不斷變化顾稀，隨著時間的增加，姿態(tài)值與飛控的真實姿態(tài)之間的誤差會越來越大坝撑。

這是因為陀螺儀傳感器本身存在著誤差静秆。飛控靜止時，理論上陀螺儀的輸出為0巡李，即角速度為0诡宗，姿態(tài)不變。但實際上即使在靜止?fàn)顟B(tài)击儡，陀螺儀依然會有輸出塔沃，這稱之為陀螺儀的零偏誤差，姿態(tài)更新過程中阳谍，零偏誤差也會不斷被累積蛀柴，最終誤差越來越大，使得姿態(tài)值逐漸偏離真實值矫夯。

為了解決這個問題鸽疾，需要對傳感器進(jìn)行校準(zhǔn)，減小甚至消除其誤差训貌。對于陀螺儀來說制肮，校準(zhǔn)零偏誤差的最簡單方式冒窍，便是將飛控靜止放置，在一定時間內(nèi)連續(xù)讀取其輸出豺鼻，然后取平均值综液，得到的數(shù)據(jù)即為零偏誤差。在后面的姿態(tài)更新中儒飒，角速度值減去該零偏誤差即可谬莹。

實際上陀螺儀的誤差組成并非如此簡單，要取得較好的校準(zhǔn)效果需要花費很多功夫桩了，且要有額外設(shè)備的支持附帽。另外其余傳感器如加速度計和磁力計，也均需要校準(zhǔn)井誉，其具體校準(zhǔn)實現(xiàn)頗為復(fù)雜喳钟，這里不便展開共郭，在本教程的【姿態(tài)篇】的第四篇中尉咕，將進(jìn)行詳細(xì)講述单芜。有興趣的童鞋可前往查看：姿態(tài)篇：四.非線性最小二乘與飛控傳感器校準(zhǔn)绢淀。

經(jīng)過校準(zhǔn)后，陀螺儀誤差將極大減小拌倍，但是并非代表完全不存在誤差泡孩。事實上，陀螺儀的零偏誤差并不會固定不變闷板，而是會隨著環(huán)境、溫度蛔垢、時間等因素的變化而變化击孩，這就帶來了很多麻煩。另外通常多旋翼飛控上使用的均為低成本MEMS陀螺儀鹏漆，其隨機(jī)噪聲誤差也非常大巩梢。這就表示创泄，陀螺儀沒有誤差的理想情況在現(xiàn)實中是不可能存在的。因此單獨使用陀螺儀來更新姿態(tài)括蝠，其誤差會逐漸累積鞠抑，無法長時間保證姿態(tài)值的準(zhǔn)確。

下一節(jié)中忌警，我們將介紹如何使用多傳感器融合的方式搁拙，計算出盡可能準(zhǔn)確的姿態(tài)值。

六.姿態(tài)估計與互補(bǔ)濾波

估計法绵，其意為對事物做出大致的推斷箕速。某一天下班后路過一個水果攤，突然想買點龍眼解解饞朋譬，可是囊中羞澀盐茎，支付寶上的余額大約只能買2到3斤。于是找老板要了個袋子徙赢，挑了許多龍眼字柠，大概用手感覺了一下，應(yīng)該有2斤狡赐。這時候我對龍眼重量的猜測窑业，便是估計值。然后給老板放到秤上一秤枕屉，2.3斤常柄，這是真實值。而2.3 - 2 = 0.3搀庶，則是這個過程中的估計誤差拐纱。

而對于飛控而言的姿態(tài)估計铜异，便是計算出一個最接近真實值的姿態(tài)值哥倔，也就是讓估計誤差最小。姿態(tài)的測量與計算揍庄，離不開傳感器咆蒿，但是各種傳感器均存在不同的優(yōu)缺點，其中：

• 陀螺儀可用于姿態(tài)的連續(xù)更新蚂子，優(yōu)點是精度較高且?guī)缀醪皇芡饨缫蛩馗蓴_沃测，缺點是自身存在誤差，在積分階段會因為誤差不斷累積從而使得計算結(jié)果不準(zhǔn)確食茎。

• 加速度計和磁力計可直接用于姿態(tài)的測量蒂破，優(yōu)點是絕對準(zhǔn)確，不存在累積誤差别渔，但缺點也很明顯附迷，容易受到干擾惧互。另外加速度計容易受到震動影響，并且對姿態(tài)的測量結(jié)果僅在靜止或者物體勻速運動時才準(zhǔn)確喇伯。

單一使用任何傳感器喊儡，都無法得到有效準(zhǔn)確的姿態(tài)信息。因此稻据，我們需要同時使用多個傳感器艾猜，結(jié)合各自優(yōu)缺點，將其數(shù)據(jù)進(jìn)行融合捻悯，從而得到最準(zhǔn)確的估計匆赃。

其中，多傳感器信息融合的最有效的一種方式為加權(quán)融合秋度。而通常用于融合的多個傳感器之間存在性質(zhì)互補(bǔ)的情況炸庞，所以我們也稱之為互補(bǔ)濾波。盡管名字中帶有“濾波”兩字荚斯，卻完全不同于傳統(tǒng)意義上的對某信號特定頻域上進(jìn)行濾除操作的濾波埠居，實則上它是一種最優(yōu)估計方法。

定義如下式子：

這就是一個簡單的使用互補(bǔ)濾波更新姿態(tài)的示例事期。其中和分別為滥壕，時刻的姿態(tài)角估計值，為時刻的角速度兽泣，為時刻來至于加速度計或磁力計的姿態(tài)測量值绎橘。

其中的，便是最核心的權(quán)重因子了唠倦。它的大小決定了時刻的估計中称鳞，我們相信哪個傳感器的數(shù)據(jù)多一點。比如稠鼻，則有冈止，意味著在每一次估計中，角速度所更新的姿態(tài)值要遠(yuǎn)比來自加速度（或磁力計）的姿態(tài)測量值準(zhǔn)確候齿，但同時還是使用了姿態(tài)測量值對姿態(tài)進(jìn)行修正熙暴，以消除角速度積分帶來的累積誤差。這樣慌盯，我們便得到了一個相對而言更準(zhǔn)確的姿態(tài)估計：對外界環(huán)境因素的干擾不敏感周霉，也不會因為角速度積分累積誤差而逐漸偏離真實值。

對上式進(jìn)行分解和變換亚皂，可以得到：

即先使用角速度數(shù)據(jù)更新姿態(tài)俱箱，然后使用姿態(tài)觀測值對姿態(tài)估計值進(jìn)行修正，修正速度取決于的大小灭必。

在上節(jié)使用角速度更新姿態(tài)的例子中狞谱，有：

定義t時刻的加速度觀測為巍膘，可以得到：

其中是一個對角線元素為權(quán)重因子的3x3矩陣，即：

下面貼上在上一節(jié)的角速度更新姿態(tài)代碼基礎(chǔ)上加入互補(bǔ)濾波的程序?qū)嵗?/p>

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;                   //姿態(tài)角
    
    static float K_G[9] = {0.0003, 0, 0, 0, 0.0003, 0, 0, 0, 0.0003};  //重力加速度向量的互補(bǔ)濾波權(quán)重矩陣
    static float K_M[9] = {0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.001};     //地磁場向量的互補(bǔ)濾波權(quán)重矩陣
    
    //一階積分計算角度變化量芋簿，單位為弧度
    //gyro為角速度數(shù)據(jù)峡懈，單位為°/s，deltaT為該函數(shù)的運行時間間隔与斤，單位為s
    deltaAngle.x = Radians(gyro.x * deltaT); 
    deltaAngle.y = Radians(gyro.y * deltaT); 
    deltaAngle.z = Radians(gyro.z * deltaT); 
    
    //角度變化量轉(zhuǎn)換為方向余弦矩陣
    EulerAngleToDCM(deltaAngle, dcMat);
    
    //更新姿態(tài)向量 vector = DCM * vector
    vectorG = Matrix3MulVector3(dcMat, vectorG);
    vectorM = Matrix3MulVector3(dcMat, vectorM);
    
    //使用互補(bǔ)濾波對姿態(tài)估計量進(jìn)行修正
    //acc為加速度計測量值肪康，mag為磁力計測量值
    vectorG = Vector3f_Add(vectorG, Matrix3MulVector3(K_G, Vector3f_Sub(acc, vectorG)));
    vectorM = Vector3f_Add(vectorM, Matrix3MulVector3(K_M, Vector3f_Sub(mag, vectorM)));
    
    //計算俯仰與橫滾角，并由弧度制轉(zhuǎn)為角度制
    AccVectorToRollPitchAngle(&angle, vectorG);   
    angle.x = Degrees(angle.x);
    angle.y = Degrees(angle.y);    

    //轉(zhuǎn)換地磁場向量投影到參考系的水平面
    Vector3f_t vectorM_Ef;
    BodyFrameToEarthFrame(angle, vectorM, &vectorM_Ef);
    
    //計算偏航角撩穿，并由弧度制轉(zhuǎn)為角度制
    MagVectorToYawAngle(&angle, vectorM_Ef);
    angle.z = Degrees(angle.z);    
    
    //將偏航角限制為0-360°
    angle.z = WrapDegree360(angle.z);    
}

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

七.代碼與工程實例

本節(jié)中將給出一個具體的工程實例，可以直接在天穹飛控的硬件上進(jìn)行運行和調(diào)試食寡，幫助初學(xué)者對本篇內(nèi)容的學(xué)習(xí)和理解雾狈。

后續(xù)再更新。

八.總結(jié)

本篇文章介紹了姿態(tài)的基本定義和表示方式抵皱，并講解了多種傳感器計算姿態(tài)的方法善榛，以及使用多傳感器融合估計姿態(tài)的一種基本算法。結(jié)合有大量代碼實例呻畸，可以幫助初學(xué)者入門移盆，如果依然有不明白的地方，可以上論壇或者Q群提問伤为，也可以同時結(jié)合Github上的天穹飛控的代碼進(jìn)行學(xué)習(xí)咒循。

這里是傳送門：

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

作為本教程姿態(tài)篇的開篇及入門篇，僅僅是介紹了一些基本概念及算法绞愚，然而實際環(huán)境頗為復(fù)雜叙甸，需要考慮的因素非常多，要提高姿態(tài)估計的精度位衩，還有很長的路要走裆蒸。最后用于多傳感器融合的互補(bǔ)濾波算法中，其權(quán)重因子通常依靠調(diào)試和經(jīng)驗給出蚂四。而下一節(jié)我們將會學(xué)習(xí)卡爾曼濾波光戈，一種從理論角度計算融合權(quán)重的最優(yōu)估計方法哪痰，順便介紹另一種常見的姿態(tài)表示方式：四元數(shù)遂赠。