運(yùn)動(dòng)平臺(tái)跟蹤系統(tǒng)姿態(tài)四元數(shù)模型參數(shù)插值法

呂 舒，張涯輝，包啟亮，陳洪斌

（1.中國(guó)科學(xué)院光電技術(shù)研究所，四川 成都610209；2.中國(guó)科學(xué)院大學(xué)，北京100049）

0 引 言

運(yùn)動(dòng)平臺(tái)下，平臺(tái)的平動(dòng)或轉(zhuǎn)動(dòng)均會(huì)給跟蹤系統(tǒng)造成干擾［1］。普遍的做法便是測(cè)量平臺(tái)姿態(tài)，對(duì)跟蹤系統(tǒng)進(jìn)行補(bǔ)償［2］，以此來(lái)隔離擾動(dòng)。

相比較傳統(tǒng)的歐拉角法，四元數(shù)法在描述姿態(tài)的應(yīng)用方面有計(jì)算量小、無(wú)奇異性以及可以直接作為捷聯(lián)系統(tǒng)的控制量等優(yōu)點(diǎn)［3－6］，現(xiàn)已經(jīng)廣泛應(yīng)用于姿態(tài)控制與空間變換領(lǐng)域，成為描述姿態(tài)的有效工具。

工程應(yīng)用中，為了匹配控制系統(tǒng)的頻率，需要對(duì)描述平臺(tái)姿態(tài)的四元數(shù)序列進(jìn)行插值。主要方法有四元數(shù)線性插值、四元數(shù)角插值和四元數(shù)球矢插值［7］。四元數(shù)線性插值的結(jié)果序列夾角不規(guī)律［8］，會(huì)給控制系統(tǒng)引入震蕩；而四元數(shù)角插值的結(jié)果序列夾角雖然均等，但是插值區(qū)間內(nèi)轉(zhuǎn)軸固定，相鄰插值區(qū)間旋轉(zhuǎn)軸有較大跳變，應(yīng)用受限；球矢插值使用最新的數(shù)據(jù)節(jié)點(diǎn)，實(shí)時(shí)性強(qiáng)，而且充分考慮旋轉(zhuǎn)的球面性，插值的結(jié)果序列不僅夾角均等，轉(zhuǎn)軸也漸變。球矢插值一般用于平穩(wěn)旋轉(zhuǎn)的姿態(tài)描述，因此在平臺(tái)運(yùn)轉(zhuǎn)平穩(wěn)時(shí)描述姿態(tài)較為準(zhǔn)確。但若平臺(tái)的機(jī)動(dòng)性較強(qiáng)，球矢插值由于其平穩(wěn)的特點(diǎn)，會(huì)產(chǎn)生失真，從而影響光電跟蹤系統(tǒng)的整體精度。針對(duì)這一應(yīng)用，本文提出一種四元數(shù)球矢插值的改進(jìn)方法稱(chēng)為模型參數(shù)球矢插值法，其相鄰插值結(jié)果四元數(shù)之間夾角誤差大大減??；并通過(guò)仿真實(shí)驗(yàn)對(duì)比分析給出相關(guān)結(jié)論。

1 姿態(tài)四元素

運(yùn)動(dòng)平臺(tái)下的光電跟蹤系統(tǒng)不僅需要同地基光電跟蹤系統(tǒng)一樣對(duì)目標(biāo)進(jìn)行快速穩(wěn)定跟蹤，還要克服運(yùn)動(dòng)平臺(tái)帶來(lái)的干擾?？朔\(yùn)動(dòng)平臺(tái)干擾的一個(gè)有效方法即測(cè)量運(yùn)動(dòng)平臺(tái)的姿態(tài)，加入控制系統(tǒng)進(jìn)行補(bǔ)償。

而在實(shí)際光電跟蹤系統(tǒng)中，傳感器采樣率較低，有些傳感器甚至?xí)в醒舆t，為了配合控制系統(tǒng)平穩(wěn)控制，需要對(duì)表示姿態(tài)的四元數(shù)進(jìn)行插值和外推如圖1所示，其中點(diǎn)為基點(diǎn)，叉為插值點(diǎn)。

圖1 插值外推點(diǎn)

姿態(tài)四元數(shù)序列的物理意義在于表示平臺(tái)的旋轉(zhuǎn)。設(shè)有姿態(tài)四元數(shù)：q＝q0＋q1i＋q2j＋q3k，表示向量由r到r′的旋轉(zhuǎn)。將其規(guī)范化

則四元數(shù)q可以寫(xiě)為

稱(chēng)為四元數(shù)的球面表示法。四元數(shù)球面表示法的物理意義如圖2所示，其中α表示轉(zhuǎn)角，珒?chǔ)伪硎巨D(zhuǎn)軸。

2 插值策略

旋轉(zhuǎn)由轉(zhuǎn)軸和轉(zhuǎn)角組成。因此對(duì)姿態(tài)四元數(shù)序列的插值策略也主要考慮這兩個(gè)方面。最直觀的策略為均等策略，即插值結(jié)果將插值基均分。此策略在運(yùn)動(dòng)平臺(tái)較為平穩(wěn)運(yùn)行時(shí)有較好的插值性能，而且操作簡(jiǎn)單。但是如果運(yùn)動(dòng)平臺(tái)機(jī)動(dòng)性較強(qiáng)，均等策略此時(shí)便會(huì)稍顯劣勢(shì)。

圖2 定點(diǎn)旋轉(zhuǎn)的四元數(shù)表示

針對(duì)機(jī)動(dòng)性較強(qiáng)的運(yùn)動(dòng)平臺(tái)的姿態(tài)描述，應(yīng)該采用更加能體現(xiàn)機(jī)動(dòng)性的策略?？筛鶕?jù)已知轉(zhuǎn)角趨勢(shì)，預(yù)測(cè)插值點(diǎn)的轉(zhuǎn)角，再依據(jù)預(yù)測(cè)趨勢(shì)對(duì)姿態(tài)四元數(shù)進(jìn)行插值。預(yù)測(cè)夾角趨勢(shì)策略的好處在于利用量測(cè)信息，可以及時(shí)體現(xiàn)運(yùn)動(dòng)平臺(tái)的姿態(tài)變化，而此策略的劣勢(shì)在于未考慮旋轉(zhuǎn)軸的變化。然而旋轉(zhuǎn)軸的變化規(guī)律描述復(fù)雜，不適合加入插值策略中。

3 線性插值

四元數(shù)線性插值，是將傳統(tǒng)的插值推廣到四元數(shù)領(lǐng)域的應(yīng)用，設(shè)已知一對(duì)四元數(shù)qa、qb，四元數(shù)的線性插值公式

式中：t——插值參數(shù)。

值得注意的是，由式（3）得到的四元數(shù)q 是非規(guī)范化的，無(wú)法直接應(yīng)用于姿態(tài)描述。因此插值后須對(duì)其進(jìn)行規(guī)范化處理［7］。于是，式（3）完整形式為

式（4）中含有求平方根運(yùn)算，增加了計(jì)算負(fù)擔(dān)和程序中控制的復(fù)雜性［8，9］。此外，線性插值法并未考慮到四元數(shù)所代表的物理意義。當(dāng)以時(shí)間點(diǎn)為參數(shù)t等步長(zhǎng)帶入時(shí)，得到的四元數(shù)序列其相鄰點(diǎn)之間的變化并不相同［8］，其變化規(guī)律與插值端點(diǎn)相關(guān)，四元數(shù)這一工具的優(yōu)勢(shì)并未充分發(fā)揮。

4 球矢插值

四元數(shù)在姿態(tài)控制的應(yīng)用中表示的是一種旋轉(zhuǎn)，而旋轉(zhuǎn)具有球面性?？紤]到這一點(diǎn)，球面矢量插值的方法被引入。對(duì)四元數(shù)序列進(jìn)行插值。球矢插值公式

式中：ω——四元數(shù)qa、qb的夾角，其計(jì)算公式為

式中：qa·qb——向量點(diǎn)乘。

球面矢量插值法，充分考慮了四元數(shù)代表旋轉(zhuǎn)的物理意義，因此當(dāng)插值參數(shù)t等步長(zhǎng)變化時(shí)，插值結(jié)果的夾角也是均等的。如圖3所示，在每個(gè)插值區(qū)間內(nèi)插值結(jié)果的夾角是均等的，表現(xiàn)為圖中階梯型曲線中呈現(xiàn)出水平線段，而插值區(qū)間之間呈現(xiàn)突跳。

圖3 球矢插值結(jié)果序列夾角

5 模型參數(shù)球矢插值

在球矢插值中，參數(shù)t是作為時(shí)間等步長(zhǎng)帶入，所得插值結(jié)果的夾角也均等，如圖3所示。此種方法應(yīng)用于虛擬現(xiàn)實(shí)和計(jì)算機(jī)視覺(jué)等領(lǐng)域時(shí)，對(duì)平滑已知起點(diǎn)與終點(diǎn)的運(yùn)動(dòng)起到了良好的效果［10］。然而將四元數(shù)用于動(dòng)平臺(tái)的實(shí)時(shí)姿態(tài)描述時(shí)，往往并不希望其均勻變化，而是期待其根據(jù)平臺(tái)姿態(tài)真實(shí)的變化規(guī)律而變化。由于球矢插值利用最新兩個(gè)基點(diǎn)進(jìn)行插值，能較真實(shí)地反應(yīng)姿態(tài)變化；而且球矢插值利用了四元數(shù)所代表的旋轉(zhuǎn)的物理意義，所以仍可以利用球矢插值的形式，但是要通過(guò)對(duì)其中的步長(zhǎng)參數(shù)t建立模型，控制插值結(jié)果盡可能接近姿態(tài)變化規(guī)律。

5.1 參數(shù)模型

在球矢插值中結(jié)果四元數(shù)與初始四元數(shù)的夾角

式中：ω——起始四元數(shù)與終點(diǎn)四元數(shù)之間的夾角。

在球矢插值中，當(dāng)式（5）中的t遍歷0到1時(shí)，所得結(jié)果四元數(shù)從起始四元數(shù)到終點(diǎn)四元數(shù)之間變化。因此，四元數(shù)的夾角代表兩四元數(shù)之間的抽象距離。

圖4 四元數(shù)夾角序列

式中：n＝f／F。

由于插值頻率較大，在插值周期內(nèi)，四元數(shù)變化較小，因此近似可得

定義狀態(tài)四元數(shù)序列珗qm與初始狀態(tài)的距離

結(jié)合式（7）、式（10）和式（11）可得，對(duì)應(yīng)時(shí)刻ti的插值參數(shù)

5.2 模型參數(shù)球矢插值法迭代流程

（1）計(jì)算測(cè)量姿態(tài)四元數(shù)夾角

（2）計(jì)算測(cè)量姿態(tài)四元數(shù)與初始狀態(tài)的距離

（4）計(jì)算插值姿態(tài)四元數(shù)夾角

（5）由式（13）計(jì)算插值參數(shù)t

（6）將參數(shù)t帶入式（5）求得插值結(jié)果。采樣Ti＋1時(shí)刻的姿態(tài)四元數(shù)（i＋1），返回（1）。

6 仿真結(jié)果及分析

仿真如下進(jìn)行，讓單位向量 ［1 0 0］ 按某種規(guī)律旋轉(zhuǎn)，模擬運(yùn)動(dòng)平臺(tái)的姿態(tài)變化。考察按照插值結(jié)果四元數(shù)旋轉(zhuǎn)后的向量與目標(biāo)向量（后稱(chēng)goal）的誤差來(lái)比較兩種插值方法的優(yōu)劣。仿真中，F(xiàn)＝10 Hz，f＝50 Hz。給出測(cè)量的姿態(tài)角序列（偏航角（yaw），俯仰角（pitch）以及橫滾角（roll））如圖5所示。

圖5 姿態(tài)角序列

分別使用球矢插值和模型參數(shù)插值的算法對(duì)姿態(tài)四元數(shù)序列進(jìn)行插值。單位向量按照兩組插值結(jié)果旋轉(zhuǎn)，對(duì)比結(jié)果。

從圖6中，整體看來(lái)球矢插值的結(jié)果序列夾角為階梯型，并不能準(zhǔn)確反映夾角的變化規(guī)律，相反，本文中提出的模型參數(shù)球矢插值法所得結(jié)果的夾角與目標(biāo)基本一致。

圖6 球矢插值與模型參數(shù)插值結(jié)果夾角對(duì)比

將兩種方法的旋轉(zhuǎn)向量目標(biāo)序列對(duì)比，表1分別列出了旋轉(zhuǎn)向量位置（position）、速度（velocity）以及結(jié)果夾角（angel）的誤差均方差。可以看出，本文提出的模型參數(shù)法（mod－arg）減小了球矢插值方法的誤差，特別是夾角（angel）誤差顯著減小。

表1 兩種插值方法誤差均方差比對(duì)

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

本文所提出的模型參數(shù)球矢插值不涉及歸一化，可以根據(jù)應(yīng)用需求對(duì)參數(shù)建模，來(lái)適應(yīng)多種情況下的應(yīng)用，所得插值結(jié)果的誤差在可接受范圍內(nèi)，同時(shí)大大減小相鄰四元數(shù)夾角的誤差。今后的工作可以為夾角最小二乘擬合過(guò)程加入適當(dāng)?shù)臋?quán)值，以提高精度，同時(shí)可以考慮將旋轉(zhuǎn)軸的變化規(guī)律加入插值策略中，以進(jìn)一步降低插值誤差和擴(kuò)大模型參數(shù)法的適用范圍。

［1］ZHANG Xianku，WANG Fengying，YANG Yansheng，et al.Robust self－stabilization control of the shipborne TV tracker［J］.Journal of Test and Measurement Technology，2006，20（8）：213－222 （in Chinese）.［張顯庫(kù)，王鳳英，楊鹽生，等.艦載電視跟蹤儀魯棒自穩(wěn)定控制 ［J］.測(cè)試技術(shù)學(xué)報(bào)，2006，20 （8）：213－222.］

［2］ZHANG Haiyang，SU Xiuqin，HAO Wei，et al.Study on forecast tracking technique of photoelectric transit on ship ［J］.ACTA Photonica Sinica，2008，37 （9）：1878－1882 （in Chinese）.［張海洋，蘇秀琴，郝偉，等.艦載光電經(jīng)緯儀預(yù)測(cè)跟蹤技術(shù)研究 ［J］.光子學(xué)報(bào)，2008，37 （9）：1878－1882.］

［3］QIAO Xiangwei，ZHOU Weidong，JI Yuren.Study on aerial vehicle attitude estimation based on quaternion particle filter algorithm ［J］.ACTA ArmamentarII，2012，33 （9）：1070－1075 （in Chinese）. ［喬相偉，周衛(wèi)東，吉宇人.基于四元數(shù)粒子濾波的飛行器姿態(tài)估計(jì)算法研究 ［J］.兵工學(xué)報(bào)，2012，33 （9）：1070－1075.］

［4］XIA Xiwang，DU Han，LIU Hanbing.Discussion on the transformation between attitude quaternion and Euler angles in large－angle maneuver scenario ［J］.Missiles and Space Vehicles，2012，41 （5）：47－53 （in Chinese）.［夏喜旺，杜涵，劉漢兵.關(guān)于大角度范圍內(nèi)四元數(shù)與歐拉角轉(zhuǎn)換的思考 ［J］.導(dǎo)彈與航天運(yùn)載技術(shù)，2012，41 （5）：47－53.］

［5］ZHANG Ronghui，JIA Hongguang，CHEN Tao，et al.Attitude solution for strapdown inertial navigation system based on quaternion algorithm ［J］.Optics and Precision Engineering，2008，16 （10）：1963－1970 （in Chinese）. ［張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算［J］.光學(xué)精密工程，2008，16 （10）：1963－1970.］

［6］QI Xin，PENG Qinsu，ZHAO Changshan，et al.A new method based on attitude quaternion matching for transfer alignment［J］.Journal of Astronautics，2011，32 （1）：81－87 （in Chinese）.［齊鑫，彭勤素，趙長(zhǎng)山，等.一種新的基于姿態(tài)四元數(shù)匹配的傳遞對(duì)準(zhǔn)方法 ［J］.宇航學(xué)報(bào)，2011，32 （1）：81－87.］

［7］Ahn Jinsu，Chung Wonjee，Jung Changdoo.Realization of orientation interpolation of 6－axis articulated robot using quaternion ［J］.J Cent South Univ，2012，19 （12）：3407－3414.

［8］LIU Aidong，HUANG Bin，LU Zhongwu.Research on method of coordinate rotation and interpo－lation based on quaterni－on ［J］.Computer and Mod－ernization，2012 （2）：44－47 （in Chinese）.［劉愛(ài)東，黃彬，盧中武.基于四元數(shù)的坐標(biāo)旋轉(zhuǎn)與插值方法研究 ［J］.計(jì)算機(jī)與現(xiàn)代化，2012 （2）：44－47.］

［9］MA Yanhong，HU Jun.On attitude quaternion ［J］.Aerospace Control and Application，2008，34 （3）：55－60 （in Chinese）.［馬艷紅，胡軍.姿態(tài)四元數(shù)相關(guān)問(wèn)題 ［J］.空間控制技術(shù)與應(yīng)用，2008，34 （3）：55－60.］

［10］XIE Rong，LU Haiyan.Simulation research of ship motion gestures based on quaternion ［J］.Shipbuilding of China，2011，52 （4）：147－152 （in Chinese）.［謝榮，魯海燕.基于四元數(shù)的船舶運(yùn)動(dòng)姿態(tài)仿真研究 ［J］.中國(guó)造船，2011，52（4）：147－152.］

［11］XIAO Wei，LIANG Jiuzhen，CHEN Weiqi.3Dobject rotation and motion interpolation based on quaternion ［J］.Journal of System Simulation，2012，24 （3）：624－627 （in Chinese）.［肖偉，梁久禎，陳瑋琪.基于四元數(shù)的3D 物體旋轉(zhuǎn)及運(yùn)動(dòng)插值 ［J］.系統(tǒng)仿真學(xué)報(bào)，2012，24 （3）：624－627.］