基于加性對偶四元數的慣性／天文組合導航算法

錢華明 孫 龍 黃 蔚

（哈爾濱工程大學 自動化學院，哈爾濱 150001）

劉 璇

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

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

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

文獻［1］提出的對偶四元數能夠將旋轉和平移統一考慮，以最簡潔的形式表示一般的剛體運動.文獻［2］將對偶四元數算法應用到捷聯慣導系統中，并詳細分析了算法誤差.文獻［3］從理論上證明了在高動態環境中，對偶四元數算法比傳統算法精度高.同時對偶四元數在機械、機器人視覺等領域也得到了廣泛地應用.文獻［4］將對偶四元數應用到衛星姿態確定中；文獻［5］將對偶四元數應用到航天器的相對導航與控制，但是上述應用都是針對單一系統，若將其應用于SINS／CNS組合導航系統能夠充分體現子系統的優勢，各子系統相互彌補對方的缺點，將會進一步提高導航的精度.

針對SINS／CNS組合導航系統所應用的載體大部分都是高動態載體這一特點，將對偶四元數應用于基于修正陀螺漂移的SINS／CNS組合導航系統中，并通過仿真對所提出方案進行實驗驗證.

1 對偶四元數及螺旋矢量

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

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

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

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

其中，為坐標系繞其旋轉和平移的三維空間向量（螺旋軸）為旋轉和平移的對偶角.

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

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

圖1 旋轉和平移幾何圖示

2 系統的對偶四元數誤差模型

2.1 坐標系定義［3］

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

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

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

I為慣性坐標系，t＝0時刻與地球坐標系重合.

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

B為載體坐標系，捷聯慣性器件的坐標系.

N為地理坐標系，導航中常用的東北天坐標系.

2.2 狀態誤差模型

對偶四元數的加性誤差定義為

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

對偶四元數的微分方程為

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

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

忽略攝動誤差的乘積，得

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

U系的旋量表示為

其誤差表示為

由式（16）得

將其實部、虛部分開為

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

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

矩陣q＋和q-定義為

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

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

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

2.3 量測方程

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

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

2.4 導航參數誤差量計算

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

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

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

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

B系相對于N系的姿態矩陣計算如下：

載體姿態角為

則載體姿態角誤差為

以δC12為例，由四元數與方向余弦的關系得

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

3 對偶四元數更新算法

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

其中，Φce和ΔVre分別為傳統捷聯算法中的圓錐誤差補償項和劃船誤差補償項.

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

結合式（2）將式（36）所有對偶四元數實數部分和對偶部分拆開得

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

設tm時刻的對偶四元數可表示為

將式（38）按照定義拆分成標量部分和對偶部分為

4 仿真與分析

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

表1 載體爬升前后導航參數

限于篇幅本文仿真和圖表數據均以東向為例，低動態環境下的仿真曲線如圖2所示.

圖2 低動態環境仿真曲線

高動態環境仿真曲線如圖3所示.

圖3 高動態環境仿真曲線

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

表2 導航誤差仿真結果

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

5 結 論

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

（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］武元新.對偶四元數導航算法與非線性高斯濾波研究［D］.長沙：國防科技大學機電工程與自動化學院，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］左俊青，王惠南，劉海穎，等.單目視覺下基于對偶四元數的衛星姿態的確定［J］.中國慣性技術學報，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］崔本杰.基于對偶四元數的航天器相對導航方法研究［D］.哈爾濱：哈爾濱工業大學衛星技術研究所，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］吳高龍，劉華偉，郝順義，等.對偶四元數導航算法的改進及仿真研究［J］.系統工程與電子技術，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］陳亮.基于對偶四元數的捷聯慣性導航算法［D］.哈爾濱：哈爾濱工業大學控制科學與工程學院，2011

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