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

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

0 引 言

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

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

1 姿態(tài)四元素

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

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

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

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

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

稱為四元數(shù)的球面表示法。四元數(shù)球面表示法的物理意義如圖2所示，其中α表示轉(zhuǎn)角，珒ξ表示轉(zhuǎn)軸。

2 插值策略

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

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

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

3 線性插值

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

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

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

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

4 球矢插值

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

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

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

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

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

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

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

5.1 參數(shù)模型

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

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

在球矢插值中，當(dāng)式（5）中的t遍歷0到1時，所得結(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）可得，對應(yīng)時刻ti的插值參數(shù)

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

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

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

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

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

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

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

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

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

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

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

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

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

表1 兩種插值方法誤差均方差比對

7 結(jié)束語

本文所提出的模型參數(shù)球矢插值不涉及歸一化，可以根據(jù)應(yīng)用需求對參數(shù)建模，來適應(yīng)多種情況下的應(yīng)用，所得插值結(jié)果的誤差在可接受范圍內(nèi)，同時大大減小相鄰四元數(shù)夾角的誤差。今后的工作可以為夾角最小二乘擬合過程加入適當(dāng)?shù)臋?quán)值，以提高精度，同時可以考慮將旋轉(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）.［張顯庫，王鳳英，楊鹽生，等.艦載電視跟蹤儀魯棒自穩(wěn)定控制 ［J］.測試技術(shù)學(xué)報，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ù)測跟蹤技術(shù)研究 ［J］.光子學(xué)報，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］.兵工學(xué)報，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）.［齊鑫，彭勤素，趙長山，等.一種新的基于姿態(tài)四元數(shù)匹配的傳遞對準(zhǔn)方法 ［J］.宇航學(xué)報，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）.［劉愛東，黃彬，盧中武.基于四元數(shù)的坐標(biāo)旋轉(zhuǎn)與插值方法研究 ［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)問題 ［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)動姿態(tài)仿真研究 ［J］.中國造船，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)動插值 ［J］.系統(tǒng)仿真學(xué)報，2012，24 （3）：624－627.］