作者/李繼陽(yáng)，上海交通大學(xué)電子信息與電氣工程學(xué)院

?

姿態(tài)顯示系統(tǒng)的算法研究

作者/李繼陽(yáng)，上海交通大學(xué)電子信息與電氣工程學(xué)院

文章摘要：隨著高性能高分辨率的微機(jī)電系統(tǒng)傳感器的發(fā)展，越來(lái)越多的系統(tǒng)中采用了低能耗，高精度，高可靠性的微機(jī)電傳感器。其中基于微機(jī)電傳感器的姿態(tài)顯示系統(tǒng)已經(jīng)初露頭角并部分程度上投入應(yīng)用。更高效，更精確的的解算算法對(duì)姿態(tài)顯示系統(tǒng)十分重要，現(xiàn)對(duì)姿態(tài)解算的算法進(jìn)行分析。

關(guān)鍵詞：姿態(tài) 算法

姿態(tài)的解算方法一直是人們研究的方向，現(xiàn)代的高科技產(chǎn)品需要更加精確，更加穩(wěn)定，更加迅速而占用更少的計(jì)算資源的姿態(tài)解算方法。但是綜合各種解算方法來(lái)看，某一方面性能的突出必然是以犧牲其他方面的性能為代價(jià)來(lái)實(shí)現(xiàn)的，所以在工程應(yīng)用中，要考慮實(shí)際的需要，選擇最適合的解算方法而并非一味追求其中的某項(xiàng)指標(biāo)?，F(xiàn)對(duì)幾種工程上常用的姿態(tài)算法進(jìn)行分析。

圖1.1

如圖1.1所示，其中XYZ坐標(biāo)為地理坐標(biāo)系，X1Y1Z1為機(jī)體坐標(biāo)系，初始條件機(jī)體坐標(biāo)系與地理坐標(biāo)系重合，圖示中以繞Y軸旋轉(zhuǎn)α角度為例，描述了地理系與機(jī)體系之間的轉(zhuǎn)換。當(dāng)Y軸發(fā)生α°的正向旋轉(zhuǎn)時(shí)，X軸，Z軸相應(yīng)的轉(zhuǎn)過(guò)α個(gè)角度，從而使機(jī)體系與地理系不重合成為X1Y1Z1。當(dāng)發(fā)生繞X軸或者Z軸的旋轉(zhuǎn)時(shí)，同樣可以有類似的表示。這說(shuō)明，機(jī)體坐標(biāo)系與地理坐標(biāo)系之間可以通過(guò)轉(zhuǎn)換矩陣來(lái)實(shí)現(xiàn)兩種坐標(biāo)系之間的轉(zhuǎn)換。而當(dāng)物體發(fā)生復(fù)雜的姿態(tài)變化時(shí)，就可以轉(zhuǎn)化成一系列不同的姿態(tài)轉(zhuǎn)換矩陣的按次序相乘。每一次姿態(tài)的變化都可以看做物體依次繞三軸旋轉(zhuǎn)了一個(gè)特定的角度，因?yàn)樵诶@其中一軸轉(zhuǎn)動(dòng)時(shí)，另外兩軸都分別轉(zhuǎn)過(guò)了一個(gè)角度，再次發(fā)生繞其他兩軸的轉(zhuǎn)動(dòng)時(shí)，兩軸位置已經(jīng)發(fā)生了變化，所以旋轉(zhuǎn)次序不同，結(jié)果也不同。姿態(tài)解算算法法的實(shí)質(zhì)就是努力求出最準(zhǔn)確的姿態(tài)轉(zhuǎn)換矩陣。

1.四元數(shù)法

四元數(shù)法是工程上有著廣泛應(yīng)用的一種姿態(tài)解算方法，最早由愛(ài)爾蘭數(shù)學(xué)家哈密頓于1843年發(fā)明，四元數(shù)法引入了新定義的虛數(shù)單位，應(yīng)用范圍十分廣泛。在姿態(tài)解算算法中，由于四元數(shù)使得各變量之間之間有了方向性，形成姿態(tài)矩陣從而反應(yīng)姿態(tài)的變化。通過(guò)四元數(shù)法可以計(jì)算出較為準(zhǔn)確的姿態(tài)信息，其最大的好處是不占用大量的計(jì)算資源，這無(wú)疑降低了對(duì)微處理器的性能要求，從而使工程設(shè)計(jì)更易實(shí)現(xiàn)，同時(shí)也能節(jié)約成本，這使得四元數(shù)法在工程上得到了廣泛的應(yīng)用。

四元數(shù)的定義：

其中q是一個(gè)四元數(shù)，a b c d為實(shí)數(shù)，i j k為虛數(shù)單位，各敘述單位之間的關(guān)系如下:

由四元數(shù)的定義可以看出，i，j，k是一種帶有指向性的虛數(shù)單位，可以用來(lái)描述坐標(biāo)軸的旋轉(zhuǎn)，并且這種旋轉(zhuǎn)是帶有方向性的矢量，所以四元數(shù)之間的乘法并不滿足交換律，必須按次相乘，這正好與姿態(tài)矩陣要按次相乘相對(duì)應(yīng)。因此可以由四元數(shù)來(lái)表示物體姿態(tài)的變化。其中i旋轉(zhuǎn)代表X軸與Y軸相交平面中X軸正向向Y軸正向的旋轉(zhuǎn)，j旋轉(zhuǎn)代表Z軸與X軸相交平面中Z軸正向向X軸正向的旋轉(zhuǎn)，k旋轉(zhuǎn)代表Y軸與Z軸相交平面中Y軸正向向Z軸正向的旋轉(zhuǎn)，即圖1.1中α的正方向。-i，-j，-k分別代表i、j、k旋轉(zhuǎn)的反向旋轉(zhuǎn)。

2.歐拉角法

歐拉角法可以直觀的反映出姿態(tài)的變化，易于人們的理解與開(kāi)發(fā)，對(duì)于要進(jìn)行姿態(tài)顯示的物體的俯仰角（pitch），橫滾角（roll），偏航角（yaw）正好構(gòu)成了一組歐拉角，這組歐拉角用來(lái)實(shí)時(shí)的反映物體的姿態(tài)信息。

歐拉角法微分關(guān)系簡(jiǎn)單，概念直觀，容易理解，但是解算過(guò)程中含有三角運(yùn)算，計(jì)算量大，對(duì)處理器要求高，其中的反正切運(yùn)算會(huì)使俯仰角在接近90°時(shí)出現(xiàn)計(jì)算數(shù)據(jù)不準(zhǔn)確的現(xiàn)象，所以歐拉角法只適用于水平姿態(tài)變化不大的情況[3]，不能進(jìn)行全姿態(tài)的姿態(tài)解算。

3.旋轉(zhuǎn)矢量法

旋轉(zhuǎn)矢量法可以可以采用多子樣算法以實(shí)現(xiàn)對(duì)誤差進(jìn)行有效的補(bǔ)償，其算法關(guān)系 式比較簡(jiǎn)單，并且在相同的子樣算法中可以通過(guò)對(duì)系數(shù)的糾正實(shí)現(xiàn)最小的算法漂移。旋轉(zhuǎn)矢量法與四元數(shù)法都是通過(guò)四元數(shù)的計(jì)算來(lái)求得姿態(tài)更新矩陣進(jìn)而反應(yīng)姿態(tài)的，但兩種算法的計(jì)算方式和側(cè)重點(diǎn)不同，所以應(yīng)用范圍上也有差異。旋轉(zhuǎn)矢量法通常應(yīng)用于姿態(tài)角變化較為劇烈的物體姿態(tài)系統(tǒng)中。

4.方向余弦法

方向余弦法直接求解姿態(tài)矩陣的微分方程，可全姿態(tài)工作，不會(huì)出現(xiàn)死鎖和方程退化的問(wèn)題，但是方向余弦法對(duì)計(jì)算資源要求較高，給實(shí)時(shí)計(jì)算帶來(lái)困難，不能保證姿態(tài)更新矩陣的實(shí)時(shí)性[4]。

5.三角函數(shù)法

三角函數(shù)法是目前最常用的一種綜合性的姿態(tài)解算方法，其實(shí)質(zhì)就是歐拉角法簡(jiǎn)化版，其忽略了地球運(yùn)動(dòng)對(duì)慣性坐標(biāo)系造成的影響，所以三角函數(shù)法并不存在對(duì)慣性坐標(biāo)系的補(bǔ)償，通過(guò)集成的三角函數(shù)關(guān)系式直接解算相關(guān)姿態(tài)信息，姿態(tài)更新實(shí)時(shí)性好，應(yīng)用于姿態(tài)角運(yùn)動(dòng)十分劇烈但位移不明顯的物體姿態(tài)角的解算[5]。三角函數(shù)法部分綜合了方向余弦法和四元數(shù)法，對(duì)計(jì)算資源的消耗十分嚴(yán)重。但是隨著計(jì)算機(jī)處理器主頻的增加，在計(jì)算資源足夠的情況下，采用三角函數(shù)法無(wú)疑是最好的選擇。

現(xiàn)代應(yīng)用的姿態(tài)解算算法不止單純的某一種，一般會(huì)根據(jù)其性能要求在特定的時(shí)期選擇最優(yōu)的解算方法。在運(yùn)載火箭，運(yùn)輸機(jī)上一般會(huì)考慮對(duì)地球坐標(biāo)系進(jìn)行校正以修正其對(duì)慣性系的誤差，而在小型無(wú)人機(jī)上由于位移不是很大則更多地考慮姿態(tài)對(duì)飛行器的飛行狀態(tài)的影響，而究竟采取何種解算方法則要取決于具體的工程應(yīng)用，在計(jì)算資源足夠的情況下一般會(huì)綜合各種方法進(jìn)行姿態(tài)的解算以滿足在某一時(shí)段對(duì)性能參數(shù)的要求。

【參考文獻(xiàn)】

＊[1]秦永元.慣性導(dǎo)航［M］.北京: 科學(xué)出版社，2009.

＊[2]Ken Shoemake.Quaternions

＊[3]朱啟舉，秦永元，梅春波，薛媛元 基于MEMS 慣性器件的水平姿態(tài)解算方案設(shè)計(jì)，彈箭與制導(dǎo)學(xué)報(bào)，2013.10.

＊[4]胡佳興，魏延輝，劉鑫，李光春.基于MEMS 的捷聯(lián)姿態(tài)系統(tǒng)的設(shè)計(jì)與試驗(yàn) 創(chuàng)意與實(shí)踐 2015.7

＊[5]郭訪社，于云峰，劉書(shū)盼.捷聯(lián)慣性導(dǎo)航系統(tǒng)姿態(tài)算法研究[J].航天控制，2010，28（1）.