姿態(tài)解算簡介

1、姿態(tài)解算簡介作者：nieyong本文需要講清楚在無人機(jī)飛行器算法中，什么是姿態(tài)，怎么表示姿態(tài)，如何得到姿態(tài)。什么是姿態(tài)姿態(tài)就是指飛行器的俯仰/橫滾/航向情況。在咱們地球上，就是指飛行器在地球坐標(biāo)系中的俯仰/橫滾/航向情況。飛行器需要實(shí)時(shí)知道當(dāng)前自己的姿態(tài)，才能夠根據(jù)需要操控其接下來的動(dòng)作，例如保持平穩(wěn)，例如實(shí)現(xiàn)翻滾。下面是學(xué)術(shù)型的嚴(yán)密論述。數(shù)學(xué)模型姿態(tài)是用來描述一個(gè)剛體的固連坐標(biāo)系和參考坐標(biāo)系之間的角位置關(guān)系，有一些數(shù)學(xué)表示方法。很常見的就是歐拉角，四元數(shù)，矩陣，軸角。地球坐標(biāo)系又叫做地理坐標(biāo)系，是固定不變的。正北，正東，正向上構(gòu)成了這個(gè)坐標(biāo)系的X，Y，Z軸，我們用坐標(biāo)系R表示。四軸飛行器上固

2、定著一個(gè)坐標(biāo)系，我們一般稱之為機(jī)體坐標(biāo)系，用坐標(biāo)系r表示。那么我們就可以用歐拉角，四元數(shù)等來描述r和R的角位置關(guān)系。這就是四軸飛行器姿態(tài)解算的數(shù)學(xué)模型和基礎(chǔ)。姿態(tài)表示方式姿態(tài)有多種數(shù)學(xué)表示方式，常見的是四元數(shù)，歐拉角，矩陣和軸角。他們各自有其自身的優(yōu)點(diǎn)，在不同的領(lǐng)域使用不同的表示方式。在四軸飛行器中使用到了四元數(shù)和歐拉角。Crazepony開源四軸飛行器也是一樣的。四元數(shù)四元數(shù)是由愛爾蘭數(shù)學(xué)家威廉盧云哈密頓在1843年發(fā)現(xiàn)的數(shù)學(xué)概念。從明確地角度而言，四元數(shù)是復(fù)數(shù)的不可交換延伸。如把四元數(shù)的集合考慮成多維實(shí)數(shù)空間的話，四元數(shù)就代表著一個(gè)四維空間，相對(duì)于復(fù)數(shù)為二維空間。四元數(shù)大量用于電腦繪圖（及

3、相關(guān)的圖像分析）上表示三維物件的旋轉(zhuǎn)及方位。四元數(shù)亦見于控制論、信號(hào)處理、姿態(tài)控制、物理和軌道力學(xué)，都是用來表示旋轉(zhuǎn)和方位。相對(duì)于另幾種旋轉(zhuǎn)表示法（矩陣，歐拉角，軸角），四元數(shù)具有某些方面的優(yōu)勢(shì)，如速度更快、提供平滑插值、有效避免萬向鎖問題、存儲(chǔ)空間較小等等。以上部分摘自維基百科-四元數(shù)。歐拉角萊昂哈德歐拉用歐拉角來描述剛體在三維歐幾里得空間的取向。對(duì)于在三維空間里的一個(gè)參考系，任何坐標(biāo)系的取向，都可以用三個(gè)歐拉角來表現(xiàn)。參考系又稱為實(shí)驗(yàn)室參考系，是靜止不動(dòng)的。而坐標(biāo)系則固定于剛體，隨著剛體的旋轉(zhuǎn)而旋轉(zhuǎn)。以上部分摘自維基百科-歐拉角。下面我們通過圖例來看看歐拉角是如何產(chǎn)生的，并且分別對(duì)應(yīng)哪個(gè)角

4、度。姿態(tài)解算姿態(tài)解算需要解決的是無人機(jī)飛行器在地球坐標(biāo)系中姿態(tài)。姿態(tài)解算的英文是attitude algorithm，也叫做姿態(tài)分析，姿態(tài)估計(jì)，姿態(tài)融合。姿態(tài)解算是指根據(jù)IMU數(shù)據(jù)（陀螺儀、加速度計(jì)、羅盤等）求解出飛行器的空中姿態(tài)，所以也叫做IMU數(shù)據(jù)融合（IMU Data Fusing）。角位置關(guān)系測(cè)量如上所說，地球坐標(biāo)系R是固定的。四軸飛行器上固定一個(gè)坐標(biāo)系r，這個(gè)坐標(biāo)系r在坐標(biāo)系R中運(yùn)動(dòng)。那么如何知道坐標(biāo)系r和坐標(biāo)系R的角位置關(guān)系呢，也就是怎么知道飛行器相對(duì)于地球這個(gè)固定坐標(biāo)系R轉(zhuǎn)動(dòng)了一下航向，或者側(cè)翻了一下機(jī)身，或者掉頭下栽。這就是傳感器需要測(cè)量的數(shù)據(jù)，傳感器包括陀螺儀，加速度計(jì)，磁力

5、計(jì)。通過獲得這些測(cè)量數(shù)據(jù)，得到坐標(biāo)系r和坐標(biāo)系R的角位置關(guān)系。慣性測(cè)量模塊：IMU（Inertial Measurement Unit），提供飛行器在空間姿態(tài)的傳感器原始數(shù)據(jù)，一般由陀螺儀傳感器/加速度傳感器/電子羅盤提供飛行器9DOF數(shù)據(jù)。飛行器根據(jù)陀螺儀的三軸角速度對(duì)時(shí)間積分得到的俯仰/橫滾/航向角，這是快速解算?？焖俳馑愕玫降淖藨B(tài)是存在誤差的，而且誤差會(huì)累加，如果再結(jié)合三軸地磁和三軸加速度數(shù)據(jù)進(jìn)行校正，得到準(zhǔn)確的姿態(tài)，這就是深度解算。當(dāng)然，快速解算的姿態(tài)一般是不能夠用于控制飛行器的，因?yàn)檎`差太大。我們一般說的姿態(tài)解算就是深度解算。四元數(shù)和歐拉角在姿態(tài)解算中如何使用姿態(tài)解算的核心在于旋轉(zhuǎn)，

6、一般旋轉(zhuǎn)有4種表示方式：矩陣表示、歐拉角表示、軸角表示和四元數(shù)表示。矩陣表示適合變換向量，歐拉角最直觀，軸角表示則適合幾何推導(dǎo)，而在組合旋轉(zhuǎn)方面，四元數(shù)表示最佳。因?yàn)樽藨B(tài)解算需要頻繁組合旋轉(zhuǎn)和用旋轉(zhuǎn)變換向量，所以采用四元數(shù)保存飛行器的姿態(tài)。在Crazepony中，使用四元數(shù)來保存飛行器的姿態(tài)（也就是在地球坐標(biāo)系中的俯仰/橫滾/航向情況）。在需要控制的時(shí)候，會(huì)將四元數(shù)轉(zhuǎn)化為歐拉角，然后輸入到姿態(tài)控制算法中。姿態(tài)控制算法的輸入?yún)?shù)必須要是歐拉角。下面就是Crazepony開源四軸飛行器上，姿態(tài)解算到姿態(tài)控制的整個(gè)流程。AD值是指MPU6050的陀螺儀和加速度值，3個(gè)維度的陀螺儀值和3個(gè)維度的加速度值，每個(gè)值為16位精度。AD值通過姿態(tài)解算算法得到飛行器當(dāng)前的姿態(tài)（姿態(tài)使用四元數(shù)表示），然后將四元數(shù)轉(zhuǎn)化為歐拉角，用于姿態(tài)控制算法（PID控制）中。姿態(tài)解算算法如何根據(jù)IMU提供的數(shù)據(jù)（三軸陀螺儀，三軸加速度計(jì)，電子羅盤數(shù)據(jù)），得到準(zhǔn)確的姿態(tài)呢？飛行器中的姿態(tài)一般