無(wú)人機(jī)四元數(shù)技術(shù)

許景偉，余麗云

（1.中國(guó)人民解放軍92853部隊(duì)，遼寧 興城 125106；2.工業(yè)和信息化部電子第五研究所華東分所，江蘇 蘇州 215011）

0 引言

無(wú)人機(jī)技術(shù)在現(xiàn)代生活中的應(yīng)用越來(lái)越廣泛。越來(lái)越多的人加入了無(wú)人機(jī)使用者或設(shè)計(jì)者行列。但目前大多數(shù)無(wú)人機(jī)愛(ài)好者僅僅停留在使用者層次，并且很多無(wú)人機(jī)愛(ài)好者由于缺乏數(shù)學(xué)基礎(chǔ)而無(wú)法進(jìn)階到初步設(shè)計(jì)者。因此，對(duì)無(wú)人機(jī)技術(shù)有更多、更深入的了解，不管是對(duì)無(wú)人機(jī)愛(ài)好者更好地操作無(wú)人機(jī)，還是有志于進(jìn)階無(wú)人機(jī)設(shè)計(jì)者進(jìn)行學(xué)習(xí)開(kāi)發(fā)無(wú)人機(jī)技術(shù)，都有著極大的幫助。本文著重討論無(wú)人機(jī)軟件技術(shù)中的一個(gè)基礎(chǔ)算法——四元數(shù)法。

1 四元數(shù)定義

無(wú)人機(jī)在空中的姿態(tài)問(wèn)題是所有的無(wú)人機(jī)軟件技術(shù)中首要解決的問(wèn)題。目前常用的無(wú)人機(jī)姿態(tài)定義的方法有歐拉角、旋轉(zhuǎn)矩陣和四元數(shù)等[1]。一般無(wú)人機(jī)姿態(tài)定義方法會(huì)采用四元數(shù)法。因?yàn)樗脑獢?shù)法相對(duì)于另外兩種算法有著數(shù)據(jù)量少、姿態(tài)解算快的優(yōu)點(diǎn)。四元數(shù)概念最早由愛(ài)爾蘭數(shù)學(xué)家發(fā)明，可以把四元數(shù)法看作是描述無(wú)人機(jī)三維姿態(tài)的一組向量。

四元數(shù)的定義為q=s+xi+yj+z k。i，j，k是單位虛數(shù)。復(fù)數(shù)p=a+bi可以表述二維平面向量，四元數(shù)可以簡(jiǎn)單地認(rèn)為是一個(gè)三維空間的多維復(fù)數(shù)表示形式，類(lèi)似復(fù)數(shù)表示，可定義為

2 四元數(shù)在三維空間的旋轉(zhuǎn)

無(wú)人機(jī)機(jī)體坐標(biāo)系如圖1a所示，為B坐標(biāo)系；地球坐標(biāo)系如圖1a所示，為A坐標(biāo)系。B坐標(biāo)系和A坐標(biāo)系在初始的狀態(tài)下完全平行或重合，I//X，J//Y，K//Z。為了表示機(jī)體坐標(biāo)系，采用四元數(shù)的定義機(jī)體姿態(tài)為：

其中，cosα、cosβ、cosγ為旋轉(zhuǎn)軸在I、J、K方向的分量，表示機(jī)體的姿態(tài)旋轉(zhuǎn)方向[2-3]。

如圖1b所示，若機(jī)體從初始狀態(tài)開(kāi)始繞I軸旋轉(zhuǎn)90°，那么機(jī)體的四元數(shù)表示變化為：

機(jī)體旋轉(zhuǎn)定義遵循右手定則（右手大拇指指向旋轉(zhuǎn)軸方向，余下四指自然彎曲方向?yàn)檎D(zhuǎn)方向）。cosα、cosβ、cosγ在本次旋轉(zhuǎn)中表示為[1，0，0]。

如圖1c所示，若機(jī)體繼續(xù)繞I軸旋轉(zhuǎn)90°，那么機(jī)體的四元數(shù)表示變化為：

此時(shí)機(jī)體是在前面一次旋轉(zhuǎn)的基礎(chǔ)上繼續(xù)旋轉(zhuǎn)的，不是從初始化位置開(kāi)始旋轉(zhuǎn)的，得到：

如圖1d所示，若機(jī)體繼續(xù)繞I軸旋轉(zhuǎn)90°，可以得：

圖1 坐標(biāo)系

根據(jù)四元數(shù)的旋轉(zhuǎn)差值計(jì)算公式，可以得到3次旋轉(zhuǎn)后的四元數(shù)公式為：

若是3次旋轉(zhuǎn)后，繼續(xù)按原來(lái)的規(guī)律旋轉(zhuǎn)，那么機(jī)體正好旋轉(zhuǎn)一周，可得：

經(jīng)計(jì)算，這和實(shí)際情況相符[4-6]。

3 坐標(biāo)系變換

無(wú)人機(jī)姿態(tài)定義主要是要考慮如何描述無(wú)人機(jī)在地球坐標(biāo)系中的相對(duì)位置。一般可以把無(wú)人機(jī)自身姿態(tài)定義成一個(gè)機(jī)體坐標(biāo)系，如B坐標(biāo)系；把地球坐標(biāo)系定義成導(dǎo)航坐標(biāo)系，如A坐標(biāo)系。因此無(wú)人機(jī)姿態(tài)定義問(wèn)題就轉(zhuǎn)變?yōu)橐粋€(gè)數(shù)學(xué)問(wèn)題，即如何在A坐標(biāo)系中描述B坐標(biāo)系；或者說(shuō)如何在B坐標(biāo)系中描述A坐標(biāo)系[7]。

顯然，通過(guò)數(shù)學(xué)常識(shí)，A和B的關(guān)系完全可以利用某個(gè)運(yùn)算方式表述，運(yùn)算方式假定為C，那么可以獲得A=BC。由此，進(jìn)一步可以明確，無(wú)人機(jī)姿態(tài)定義就是為了定義這個(gè)C。四元數(shù)能完美地表述這個(gè)C。

假定機(jī)體姿態(tài)定義為VB，映射到A坐標(biāo)系的姿態(tài)為VA，根據(jù)四元數(shù)運(yùn)算原則，VA=q·VB·q-1。即為：

4 四元數(shù)姿態(tài)解算

對(duì)機(jī)體姿態(tài)進(jìn)行四元數(shù)解算，要結(jié)合傳感器參數(shù)采樣更新四元數(shù)參數(shù)，并采用龍格庫(kù)塔法更新四元數(shù)[4]?？梢垣@得四元數(shù)解算更新姿態(tài)結(jié)果。

式（10）中：gx、gy、gz——PI迭代計(jì)算的誤差修正參數(shù)。

5 結(jié)束語(yǔ)

無(wú)人機(jī)姿態(tài)解算是無(wú)人機(jī)飛控技術(shù)實(shí)現(xiàn)的基礎(chǔ)，而四元數(shù)法無(wú)疑是無(wú)人機(jī)姿態(tài)解算的較優(yōu)方法，對(duì)無(wú)人機(jī)設(shè)計(jì)者來(lái)說(shuō)需要理解、掌握[8]。即使是無(wú)人機(jī)使用者，理解四元數(shù)法也有助于更好地操控?zé)o人機(jī)、了解無(wú)人機(jī)技術(shù)。