錢華明 孫 龍 黃 蔚

（哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001）

劉 璇

（黑龍江科技學(xué)院 電氣與信息工程學(xué)院，哈爾濱 150029）

慣 性／天 文 組 合 導(dǎo) 航 系 統(tǒng) （SINS／CNS，Strap-down Inertial Navigation Systems／Celestial Naviga-tion Systems）是一種利用天體測量信息和慣性測量信息獲取高精度導(dǎo)航參數(shù)的導(dǎo)航系統(tǒng)，具有自主性強、測姿精度高的特點，而且還可以通過天文量測信息有效地修正陀螺漂移，因此被廣泛地應(yīng)用到高空無人機、彈道導(dǎo)彈、衛(wèi)星和深空探測等高性能導(dǎo)航載體.

對于SINS／CNS組合導(dǎo)航系統(tǒng)，傳統(tǒng)方法是將旋轉(zhuǎn)和平移拆分開來，利用方向余弦陣或四元數(shù)描述轉(zhuǎn)動，利用向量描述平移.然而實際載體坐標(biāo)系之間的轉(zhuǎn)換是二者兼具且一般同時發(fā)生的，與質(zhì)點只存在平移而沒有旋轉(zhuǎn)的運動不同，因此旋轉(zhuǎn)和平移是不能完全解耦分離開來的，特別是高動態(tài)的載體.因此迫切需要一種能將旋轉(zhuǎn)和平移統(tǒng)一起來、簡單又高效的計算方法.

文獻(xiàn)［1］提出的對偶四元數(shù)能夠?qū)⑿D(zhuǎn)和平移統(tǒng)一考慮，以最簡潔的形式表示一般的剛體運動.文獻(xiàn)［2］將對偶四元數(shù)算法應(yīng)用到捷聯(lián)慣導(dǎo)系統(tǒng)中，并詳細(xì)分析了算法誤差.文獻(xiàn)［3］從理論上證明了在高動態(tài)環(huán)境中，對偶四元數(shù)算法比傳統(tǒng)算法精度高.同時對偶四元數(shù)在機械、機器人視覺等領(lǐng)域也得到了廣泛地應(yīng)用.文獻(xiàn)［4］將對偶四元數(shù)應(yīng)用到衛(wèi)星姿態(tài)確定中；文獻(xiàn)［5］將對偶四元數(shù)應(yīng)用到航天器的相對導(dǎo)航與控制，但是上述應(yīng)用都是針對單一系統(tǒng)，若將其應(yīng)用于SINS／CNS組合導(dǎo)航系統(tǒng)能夠充分體現(xiàn)子系統(tǒng)的優(yōu)勢，各子系統(tǒng)相互彌補對方的缺點，將會進(jìn)一步提高導(dǎo)航的精度.

針對SINS／CNS組合導(dǎo)航系統(tǒng)所應(yīng)用的載體大部分都是高動態(tài)載體這一特點，將對偶四元數(shù)應(yīng)用于基于修正陀螺漂移的SINS／CNS組合導(dǎo)航系統(tǒng)中，并通過仿真對所提出方案進(jìn)行實驗驗證.

1 對偶四元數(shù)及螺旋矢量

對偶四元數(shù)繼承了所有四元數(shù)特性，定義為

其中，q和q′分別為對偶四元數(shù)的標(biāo)量和對偶部分；ε2＝0，但ε≠0；＊＝－1為共軛對偶四元數(shù).由泰勒展開定理，可得

根據(jù)Chasles定理［5］：三維空間的任何有限位移都可以由一個螺旋運動獲得.由此引入螺旋矢量來表示螺旋運動為

對偶四元數(shù)可以由螺旋運動的參數(shù)表示為

其中，為坐標(biāo)系繞其旋轉(zhuǎn)和平移的三維空間向量（螺旋軸）為旋轉(zhuǎn)和平移的對偶角.

其中，n為單位向量，其方向代表平移的方向；d為螺距；φ為旋轉(zhuǎn)角度.圖1描述了原始坐標(biāo)系O1經(jīng)過螺旋運動變換到O2的過程.旋轉(zhuǎn)是繞具有向量n且過點m的直線旋轉(zhuǎn)θ角（θ＝φ）；O1m垂直于n，其方向矢量為p；坐標(biāo)系原點之間的連線為矢量t；旋轉(zhuǎn)和平移的順序是可以顛倒的，因此對偶四元數(shù)又有如下表示［6］：

其中，?為四元數(shù)乘子；q為轉(zhuǎn)動四元數(shù)；tO1 和tO2為平移向量，上標(biāo)O1，O2為t在O1，O2系上的投影.結(jié)合式（1）和式（6）可以看出，對偶四元數(shù)標(biāo)量部分是轉(zhuǎn)動四元數(shù)，對偶部分是轉(zhuǎn)動四元數(shù)和平移向量的函數(shù)，因此對偶四元數(shù)能將旋轉(zhuǎn)和平移統(tǒng)一考慮.

圖1 旋轉(zhuǎn)和平移幾何圖示

2 系統(tǒng)的對偶四元數(shù)誤差模型

2.1 坐標(biāo)系定義［3］

T為推力速度坐標(biāo)系，與體坐標(biāo)系平行，從地心到坐標(biāo)原點的向量等于推力速度.

G為引力速度坐標(biāo)系，與地球坐標(biāo)系平行，從地心到坐標(biāo)原點的向量等于引力速度.

E為地球坐標(biāo)系，原點在地心，一個軸與地軸重合，其他兩個軸在赤道平面.

I為慣性坐標(biāo)系，t＝0時刻與地球坐標(biāo)系重合.

U為位置坐標(biāo)系，與地球坐標(biāo)系平行，從地心到坐標(biāo)原點的向量等于載體的位置向量.

B為載體坐標(biāo)系，捷聯(lián)慣性器件的坐標(biāo)系.

N為地理坐標(biāo)系，導(dǎo)航中常用的東北天坐標(biāo)系.

2.2 狀態(tài)誤差模型

對偶四元數(shù)的加性誤差定義為

其中，為計算或者測量得到的對偶四元數(shù)；δq＾為 ADQE（Additive Dual Quaterrion Error）.

對偶四元數(shù)的微分方程為

由上式可得到其誤差的微分方程為

將式（7）代入式（9）得

忽略攝動誤差的乘積，得

同理，G系與I系之間的誤差方程為

U系的旋量表示為

其誤差表示為

由式（16）得

將其實部、虛部分開為

將陀螺和加速度計的常值誤差和隨機誤差分離，將常值誤差作為狀態(tài)量，隨機誤差作為系統(tǒng)噪聲，并連同式（14）、式（22）、式（23）組合在一起依據(jù)如下誤差狀態(tài)向量的定義：

當(dāng)四元數(shù)寫成q＝［s，vT］T，v＝［v1v2v3］T時，四元數(shù)乘法可以表示成如下形式［3］：

矩陣q＋和q-定義為

其中，I3為3×3單位陣；［v×］為反對矩陣：

因此，式（24）中的F，G可表示為

上述的0矩陣均為4×4零矩陣.

2.3 量測方程

星敏感器是天文導(dǎo)航中重要的測量儀器，利用拍攝到的星圖確定其光軸在慣性空間中的瞬時指向，從而確定載體的姿態(tài).由星敏感器輸出參數(shù)可得到B系相對于I系的四元數(shù)qX.將qX－qIB，可得量測方程為

其中，H＝［I4×404×20］；v為星敏感器的測量噪聲.

2.4 導(dǎo)航參數(shù)誤差量計算

建立對偶四元數(shù)線性導(dǎo)航模型之后，用卡爾曼濾波進(jìn)行在線估計.得到狀態(tài)量后就可以計算速度、位置、姿態(tài)誤差.載體相對N系的速度為

將式（18）代入式（27）得到速度誤差為

位置誤差由rU＝rE＝2q＊IU?q′IU得

由于δqIU＝0則載體相對N系的位置誤差為

B系相對于N系的姿態(tài)矩陣計算如下：

載體姿態(tài)角為

則載體姿態(tài)角誤差為

以δC12為例，由四元數(shù)與方向余弦的關(guān)系得

其中，δq（2）由狀態(tài)變量δqIT得到，q（2）由qIT得到，將得到的計算值代入式（32）即可得姿態(tài)誤差角.同理式（30由qIG計算得到，即

3 對偶四元數(shù)更新算法

1）使用螺旋矢量的雙子樣優(yōu)化算法［8］計算螺旋矢量Φ＾.

其中，Φce和ΔVre分別為傳統(tǒng)捷聯(lián)算法中的圓錐誤差補償項和劃船誤差補償項.

2）用得到的螺旋矢量代入式（37）計算q（ΔT）和q′（ΔT）.由式（4）得

結(jié)合式（2）將式（36）所有對偶四元數(shù)實數(shù)部分和對偶部分拆開得

3）將得到的q（ΔT）和q′（ΔT）代入式（39）計算tm時刻的對偶四元數(shù).

設(shè)tm時刻的對偶四元數(shù)可表示為

將式（38）按照定義拆分成標(biāo)量部分和對偶部分為

4 仿真與分析

仿真初始條件：初始位置東經(jīng)120°，北緯36°；陀螺常值漂移和隨機漂移分別為0.1（°）／h和0.05（°）／h；加速度計常值和隨機偏置分別為100μg和50μg；星敏感器精度為10″（1σ）.分高、低動態(tài)兩種環(huán)境對比傳統(tǒng)SINS／CNS和對偶四元數(shù)組合導(dǎo)航系統(tǒng)（兩種動態(tài)環(huán)境是驗證算法的正確性而假設(shè)的，不代表載體的真實運動情況，表1是載體運行過程中爬升前后的參數(shù)對比）.

表1 載體爬升前后導(dǎo)航參數(shù)

限于篇幅本文仿真和圖表數(shù)據(jù)均以東向為例，低動態(tài)環(huán)境下的仿真曲線如圖2所示.

圖2 低動態(tài)環(huán)境仿真曲線

高動態(tài)環(huán)境仿真曲線如圖3所示.

圖3 高動態(tài)環(huán)境仿真曲線

由仿真曲線和表2看出，基于修正陀螺漂移的SINS／CNS組合導(dǎo)航系統(tǒng)，使用卡爾曼濾波進(jìn)行狀態(tài)估計后，可以校正由陀螺漂移引起的姿態(tài)誤差，速度、位置誤差也起到了明顯的抑制作用.但由于對加速度計偏置估計不準(zhǔn)確，無法有效修正由加速度計引起的導(dǎo)航誤差，所以仍然不能阻止位置和速度誤差的發(fā)散.在低動態(tài)條件下，對偶四元數(shù)法精度與傳統(tǒng)方法基本相同.因為在低動態(tài)條件下，載體的平動和角運動在濾波周期內(nèi)比較小，積分運算得到的都是小量，不可交換性誤差等都很小，旋轉(zhuǎn)和平移可以近似的拆分開來.

表2 導(dǎo)航誤差仿真結(jié)果

但是當(dāng)載體在高動態(tài)條件下時，對偶四元數(shù)法的優(yōu)勢就很明顯.相比于低動態(tài)條件下的曲線，雖然各導(dǎo)航誤差都有所增加，但是變化都不大.反觀傳統(tǒng)導(dǎo)航方式，由于此時積分運算得到的不是誤差小量，因此旋轉(zhuǎn)和平移就不能完全解耦.姿態(tài)角誤差、陀螺漂移誤差增加明顯，這就導(dǎo)致了速度誤差和位置誤差的快速發(fā)散.說明了在高動態(tài)條件下使用螺旋算法和對偶四元數(shù)算法的優(yōu)越性和有效性.

5 結(jié) 論

在高動態(tài)條件下，基于加性對偶四元數(shù)的SINS／CNS組合導(dǎo)航算法運用螺旋矢量更新對偶四元數(shù)，將姿態(tài)和速度同時更新，統(tǒng)一考慮了旋轉(zhuǎn)和平移，相比傳統(tǒng)算法導(dǎo)航精度更高.SINS／CNS組合導(dǎo)航系統(tǒng)經(jīng)常被應(yīng)用于導(dǎo)彈、高空無人機、深空探測器等高動態(tài)載體，有著廣闊的發(fā)展前景.

（References）

［1］Clifford W.Preliminary sketch of bi-quaternions ［J］.Proceedings of the London Mathematical Society，1873，4（4）：381－395

［2］Wu Yuanxin，Hu Xiaoping，Hu Dewen，et al.Strapdown inertial navigation system algorithms based on dual quaternions［J］.IEEE Transacions on Aerospace and Electronic Systems，2005，41（1）：110－132

［3］武元新.對偶四元數(shù)導(dǎo)航算法與非線性高斯濾波研究［D］.長沙：國防科技大學(xué)機電工程與自動化學(xué)院，2005

Wu Yuanxin.Resarch on dual quaternion navigation algorithm and nonlinear gaussion filtering［D］.Changsha：Collge of Mechatronics and Automation，National University of Defense Technology，2005（in Chinese）

［4］左俊青，王惠南，劉海穎，等.單目視覺下基于對偶四元數(shù)的衛(wèi)星姿態(tài)的確定［J］.中國慣性技術(shù)學(xué)報，2008，16（5）：577－581

Zuo Junqing，Wang Huinan，Liu Haiying，et al.Satellite attitude estimation based on dual quaternion from monocular camera［J］.Journal of Chinese Inertial Technology，2008，16（5）：577－581（in Chinese）

［5］崔本杰.基于對偶四元數(shù)的航天器相對導(dǎo)航方法研究［D］.哈爾濱：哈爾濱工業(yè)大學(xué)衛(wèi)星技術(shù)研究所，2009

Cui Benjie.Study on relative navigation method for spacecraft based on dual quaternion［D］.Haerbin：Research Center of Satellite Technology，Harbin Institute of Technology，2009（in Chinese）

［6］Wang Jianying，Liang Haizhao，Sun Zhaowei，et al.Relative motion coupled control based on dual quaternion ［J］.Aerospace Science and Technology，2011，12（13）：1－12

［7］吳高龍，劉華偉，郝順義，等.對偶四元數(shù)導(dǎo)航算法的改進(jìn)及仿真研究［J］.系統(tǒng)工程與電子技術(shù)，2011，33（4）：862－868

Wu Gaolong，Liu Huawei，Hao Shunyi，et al.Improvement of dual quaternion navigation algorithm and relevant simulation［J］.Systems Engineering and Electronics，2011，33（4）：862－868（in Chinese）

［8］陳亮.基于對偶四元數(shù)的捷聯(lián)慣性導(dǎo)航算法［D］.哈爾濱：哈爾濱工業(yè)大學(xué)控制科學(xué)與工程學(xué)院，2011

Chen Liang.SINS algorithms based on dual quaternions［D］.Haerbin：School of Control Science and Engineering，Harbin Institute of Technology，2011（in Chinese）