基于Rodrigues參數和量測非線性的SINS/GPS動基座對準

梅春波，秦永元，付強文，嚴恭敏

（西北工業大學 自動化學院，西安 710129）

基于Rodrigues參數和量測非線性的SINS/GPS動基座對準

梅春波，秦永元，付強文，嚴恭敏

（西北工業大學 自動化學院，西安 710129）

對 GPS輔助下，捷聯慣導系統動基座對準問題進行了研究。利用姿態陣分解，將動態對準問題轉換為一個常值姿態的估計，采用Rodrigues參數進行姿態描述，建立了系統方程線性，量測方程具有二階非線性的對準模型，進而推導了基于二階非線性量測完整泰勒級數展開的濾波算法。同時，對Rodrigues參數描述姿態時的奇異點問題進行了詳細地討論，設計了能自動判別并規避奇異點的濾波對準方案。以車輛典型機動軌跡為對象進行了蒙特卡洛仿真，結果表明，所設計算法能夠在5 s內實現奇異點的判別及處理，且當GPS速度誤差為0.1 m/s，位置誤差為3 m，更新率為1 s時，慣性級捷聯慣導系統在120 s時間內，可以達到水平姿態誤差角均方根小于10″，方位誤差角均方根小于4′的對準精度。

動基座對準；Rodrigues參數；非線性量測；完整泰勒級數；奇異點

動基座對準能夠有效提高捷聯慣導系統載體平臺的機動性能，具有很高的軍事應用價值。動基座對準的研究內容主要包括兩個方面，一是建立大失準角條件下的非線性誤差模型[1-3]，是設計相應的非線性濾波估計算法[2-9]。

由不同的姿態描述方式可以得到不同的誤差模型，如四元數非線性誤差模型[1]、歐拉角非線性誤差模型[2]，及僅方位失準角為大角度時，基于方位角正余弦函數的非線性誤差模型[4]等。其中，四元數非線性誤差模型無奇異點，使用最為廣泛，但在設計濾波算法時需要考慮其模值約束的影響。歐拉角姿態描述法存在奇異點，因此不適用于任意姿態對準，且基于歐拉角的非線性誤差模型中含有狀態量的正余弦函數，使得誤差模型非線性增大。傳統的基于修正Rodrigues參數的誤差模型雖可避免奇異點，但是模型非線性度同樣很大。此外，傳統方法建立動基座對準非線性誤差模型時，均以動態載體系和動態導航系之間的實時姿態為估計對象，系統方程及量測方程均建立在動態的導航坐標系下。以速度為觀測量時，以上誤差模型只有系統方程為非線性，量測方程則是線性的。

在非線性狀態估計領域，使用最早最廣泛的是標準EKF算法，但該算法需要求導計算雅克比矩陣，且其高階截斷誤差僅為二階。此后，一類基于sigma點的非線性狀態估計方法被廣泛研究，如UKF濾波[2,4]、強跟蹤濾波[5]、粒子濾波[6]、高斯-厄米特濾波[7]，以及容積卡爾曼濾波(CKF)等。這類算法，用確定性或隨機采樣策略逼近非線性函數的概率分布，避免了對非線性模型進行求導。通過優化采樣策略，一方面可以減小計算量，另一方面則可提高非線性函數概率分布的近似精度。相對而言，基于sigma點的非線性濾波算法在計算量上與 EKF濾波相當，但是精度優于EKF濾波。

本文設計的動基座對準算法，利用姿態陣分解，將動態姿態的估計問題轉化為一個常值姿態的估計，借助經典Rodrigues參數和姿態陣之間的凱萊變換[8]，建立了在慣性坐標系內描述的系統方程線性、量測方程具有二階非線性的弱非線性誤差模型。對于這種非線性系統的估計，采用標準線性卡爾曼濾波算法完成時間更新。對于量測更新，借鑒二階EKF濾波方程的構造思路，推導得到了一種在實現形式上與線性卡爾曼濾波完全一致，而在計算量上也基本相當的任意失準角下的線性濾波對準方案。在計算量上遠小于導航系內基于sigma點的各類非線性濾波對準算法。對于經典 Rodrigues參數描述姿態存在奇異點的問題，進行了詳細分析并給出了解決辦法。

1 慣性系動基座對準方案

式中：nt系為實時導航坐標系，即載體時變位置東北天地理坐標系；in為導航慣性系，與動基座對準開始時刻的n系重合；b為載體坐標系；ib為載體慣性系，與對準開始時刻的b系重合。

利用牛頓第二定律和哥氏定理，經過簡易推導可得到慣性系比力方程如下：

式中： vin(t)為t時刻載體對地速度在慣性系in內的投影； gin

(t)為t時刻載體所在位置重力加速度在in系內投影； fin(t)為t時刻理想比力值。對式(2)兩端分別進行積分，并記

式中，

k-1k導航系內對地速度vnt為線性函數，即

式中： t∈[tk-1, tk]； T = tk- tk-1為 GPS量測更新周期；。

將式(9)(10)代入式(7)(8)中，整理可得：

利用捷聯慣導姿態、速度二子樣更新算法，可以實現對式(4)中 V(t)的求解。進一步，考慮陀螺儀隨k機常值漂移 εb和加速度計隨機常值零偏▽b的影響，推導可得：

式中：φib為 C姿態誤差角；δV(t)為加速度計慣k性系比力積分誤差。

由式(2)～(4)和式(13)可得，求解常值姿態陣 Cib的

in觀測方程為

進一步，用經典 Rodrigues參數法來等價描述姿態陣 C，記對應Rodrigues參數為l，則二者滿足凱萊變換關系式[11]，即

將式(17)代入式(16)，整理可得：

綜上，慣性系動基座對準可選取如下15維狀態：

由上述推導，可得系統方程及量測方程分別為

利用式(21)(22)設計濾波算法，可以實現對l的估計，進而得到姿態陣 C，通過式(1)即可實現動基座對準。

2 二階非線性量測濾波估計算法

式(21)描述的系統方程為線性，式(22)描述的量測方程為非線性，但僅是狀態量的二階非線性函數，可以用有限階Taylor級數展開描述，即

式中：Xk0為Taylor級數展開點；=Xk-Xk0；ei是第i個分量為1，其余元素為0的3維單位向量；Hk為非線性函數h的雅克比陣；Di為非線性函數h的二階偏導數陣；Tr為矩陣求跡函數，且有

注意到，對于二階非線性函數，其二階偏導數陣為常值矩陣，故由式(21)(22)(25)可知，Di為15維常值對稱陣，其中非零元素僅有：

2.1 濾波時間更新算法

式(21)系統方程為線性，可采用標準卡爾曼濾波算法完成時間更新，得到狀態量和估計誤差方差陣的一步預測，即/k-1和Pk/k-1。用狀態一步預測結果k/k-1代替式(23)中Xk0，從而建立起當前觀測量與狀態一步預測關系式，進而設計量測更新算法對一步預測結果?k/k-1進行校正，得到當前時刻的狀態最優估計值。下面推導基于式(23)二階泰勒級數量測方程的濾波量測更新算法。

2.2 濾波量測更新算法

由于量測量僅為二階非線性函數，因此可以采用二階EKF量測更新方法來完成濾波量測更新。下面給出簡要給出基于量測方程二階泰勒級數展開的量測更新推導過程，假設k時刻狀態估計結果為

式中，Lk為引入的補償項，與最佳增益Kk一樣均為待定值。Lk和Kk的確定原則分別為使?k為無偏估計和使k的均方誤差陣Pk的跡最小。

定義狀態估計誤差：

由式(23)(24)(27)(28)整理可得：

式中，E[·]表示對括號內變量求期望。將式(30)代入式(29)中，整理可得：

式中，

式中，

式(32)中，A為3維列向量，從而 Λk為3階方陣。利用式(32)，經過推導可得 Λk第i行第j列元素為

式(33)中協方差陣更新方式與標準卡爾曼濾波形式一致，從而式(27)中最佳增益陣Kk可類比得到：

考察量測更新方程式(27)(33)(36)不難發現，基于二階泰勒級數的量測更新算法與標準卡爾曼濾波在形式上完全一致，僅增加了對Lk、 Λk的計算。而由式(30)(35)可知，Lk、 Λk的求解非常簡單。考慮到Di為常值稀疏矩陣，可將Lk、Λk描述為僅與 Pk/k-1相關的形式，如此可進一步減小在線計算量。

2.3 姿態估計結果反饋

由于所估計的姿態陣為常值陣，可以方便得到基于姿態估計結果閉環反饋形式的濾波算法。通過反饋，可以使得剩余待估計姿態陣最終趨于單位陣。下面省略推導過程，僅給出姿態估計結果反饋實現算法。

假定k時刻濾波估計結果為

利用Ck更新直至k時刻總的反饋姿態陣

由Ck、 C可以實現k時刻姿態估計結果的反饋，反饋后狀態量記為kf，方差陣記為，則有：

式中，

在k+1時刻進行時間更新時，將式(21)系統方程系數矩陣中參數和修正為、 C：

在k+1時刻進行量測更新時，依據式(19)，將式(22)中 dtk、stk的修正為

然后，采用所設計的時間更新和量測更新算法完成k+1時刻濾波計算，依次遞推。k時刻姿態陣 C的估計結果即是 C。

3 經典Rodrigues參數奇異點及其處理

經典 Rodrigues參數是最少參數姿態描述方法之一，存在奇異點，一種等價表示方法為

式中，u為兩坐標系之間等效旋轉矢量的方向向量，φ為等效旋轉矢量轉過的角度。

顯然，當φ取值為±π時，l值為無窮大，也即是經典Rodrigues參數法進行姿態描述時的奇異點。

一種直觀的規避思路是，當φ取值為±π時，對ib系進行某種虛擬轉動得到ib1，使得ib1系相對于in系的Rodrigues參數遠離奇異點，然后采用前述濾波對準算法完成 C對應Rodrigues參數的估計，進而借助已知的虛擬轉動，計算 Cibin。

實現上述規避策略需要具備三個條件：第一，要設計一個函數，對φ取值是否位于奇異點附近給出明確判斷；第二，要設計一種虛擬轉動，保證轉動后姿態陣C對應的Rodrigues參數遠離奇異點；第三，對濾波算法做相應改進，保證對奇異點的規避策略不影響對準快速性和對準精度。

3.1 奇異點判別函數設計

當φ取值為±π時，ib系至in系姿態矩陣Cibin可描述為

此時，任意矢量R在兩坐標系內投影滿足

在式(46)兩端分別加上Rib，并利用單位向量恒等關系式 I + （u × ）2=uuT，則有

也即是，此時任意矢量在兩坐標系內的投影之和均共線，且與等效旋轉矢量平行。

因此，對當前濾波變量l是否接近奇異點的判斷可等價于判斷式(19)中不同時刻的 stk是否接近共線。為了充分利用可用信息，縮短得出有效判斷所需時間，設定如下判別函數：

若用于計算gk的所有 sti均共線，即當前對變量l的估計位于奇異點時，gk理論值為零。

3.2 虛擬轉動設計

虛擬轉動的設計可依據如下基本定理[11]：若ib系至in系等效旋轉矢量轉角大于π/2，將ib系繞著其某個坐標軸轉動π角得到ib1系，一定可使ib1系至in系等效旋轉矢量轉角小于π/2。

據此，記ib系繞其x軸轉動π角得到ibx系，ib系繞其y軸轉動π角得到iby系，ib系繞其z軸轉動π角得到ibz系，則有：

依據定理可知，無論載體動基座對準開始時刻真實姿態為何值，姿態陣所對應的四個等效旋轉矢量中，至少有一個等效旋轉矢量的轉角小于π/2，遠離濾波奇異點。

3.3 考慮奇異點規避的濾波算法實現

依據上面設計的判別函數和虛擬轉動，可得到考慮奇異點規避的濾波算法實現：

① 給定奇異點判別時間tk，在tk時刻以前，利用第1小節和第2小節推導的對準濾波算法，同時處理四個獨立的濾波過程，分別對進行估計。其中，有兩點需要注意：對進行估計時，式(21)(22)中相關參變量需要進行相應的坐標變換；所有濾波器狀態量初值均設為零，考慮式(27)及虛擬轉動分析結論，可將l對應的方差陣初值設定 P00=1，P11= 1， P22=1。

② 在tk時刻，分別計算姿態陣所對應的判別函數值gk、gkx、gky、gkz，僅選擇最大判別函數值所對應的濾波過程完成后續濾波對準。

4 仿真分析

以車載SINS/GPS動基座對準為例，仿真軌跡為：由靜止加速至10 m/s，加速度為0.5 m/s2；然后勻速行駛50 s；之后轉彎90°，角速率為15 (°)/s；之后勻速直線行駛。對準總時間取為120 s。

慣性器件精度：陀螺零偏為0.01 (°)/h，角隨機游走系數為0.001 (°)/h；加速度計零偏為5×10-5g，加速度計白噪聲方差強度為3×10-6g·s ，不考慮其他誤差項。

GPS精度：速度測量噪聲為0.1 m/s，水平位置測量噪聲為3 m，高度位置測量噪聲為5 m，GPS數據周期為1 s。

考慮車載 SINS測量坐標系與車體坐標系安裝誤差為小角度，故水平姿態角通常為小角，奇異點只出現在方位角接近180°時。此外，由3.3節的分析易知，采用奇異點規避策略后，最終對準算法所估計的姿態對應的等效旋轉矢量轉角一定不大于π/2。綜上，仿真中，驗證兩種姿態初值條件下的對準效果：姿態 1—俯仰角、滾轉角、方位角分別為5°、3°、80°；姿態2—俯仰角、滾轉角、方位角分別為5°、3°、180°。對上述兩種姿態初值條件下的初始對準算法各進行100次的蒙特卡洛仿真，仿真結果如圖1～圖6所示。

圖1、圖4描述的是兩種初始姿態條件下，奇異點規避策略所使用判別函數的值隨時間變化曲線，該函數值為無量綱量。從圖1、圖4中可以得出兩點結論：第一，不同等效旋轉矢量對應的判別函數在較短時間內即具有很高的區分度，這樣奇異點判別時間tk即可設計的很小，以仿真為例，tk最短可以選擇為2 s；第二，判別函數的值與對準過程中載體機動相關。仿真程序中tk設計為5 s。

姿態1時對準姿態誤差收斂曲線如圖2、圖3所示，其中圖2為整個對準區間收斂曲線，圖3為70 s至100 s的收斂曲線。姿態2時對準姿態誤差收斂曲線如圖5、圖6所示，圖5對應整個對準區間收斂曲線，圖6為70 s到100 s收斂曲線。

與gk、gkx、gky、gkz相關的等效旋轉矢量轉角，在姿態1初值下，分別為80.05°、174.24°、179.08°、100.26°；在姿態2初值條件下，分別為179.87°、177°、175°、5.83°。由圖1、圖4易知：在姿態1初值條件下，等效旋轉矢量轉角為80.05°的濾波過程在判別完成后被保留；在姿態2初值條件下，等效旋轉矢量轉角為5.83°的濾波過程在判別完成后被保留。

兩種姿態初值條件下，濾波對準結果如圖 2、圖5所示。在車輛加速和轉彎的機動條件下，對準算法能夠快速收斂。在120 s對準結束時刻：姿態1初值條件下，兩水平姿態誤差角和方位姿態誤差角均方根分別為9.6″、10″、3.97′；姿態2初值條件下，兩水平姿態誤差角和方位姿態誤差角均方根分別為9.9″、9.4″、3. 81′。

圖1 姿態1時判別函數隨時間變化曲線Fig.1 Criterions with respect to time at Ori.1

圖2 姿態1時姿態誤差角均方根曲線Fig.2 Root mean square of attitude errors at Ori.1

圖3 姿態1時姿態誤差角均方根曲線Fig.3 Root mean square of attitude errors at Ori.1

圖4 姿態2時判別函數隨時間變化曲線Fig.4 Criterions with respect to time at Ori.2

圖5 姿態2時姿態誤差角均方根曲線Fig.5 Root mean square of attitude errors at Ori.2

圖6 姿態2時姿態誤差角均方根曲線Fig.6 Root mean square of attitude errors at Ori.2

5 結 論

對于捷聯慣導系統動基座對準問題，本文給出了一種基于羅德里格參數和二階非線性量測的濾波對準算法，并對羅德里格參數奇異點問題進行了詳細討論，設計了處理方案。仿真分析結果表明，該算法在車輛啟動段典型機動條件下，能夠快速收斂，對準精度高，以與線性卡爾曼濾波相當的計算量及計算復雜度實現了任意失準角非線性動基座對準。

（References）：

[1] Dai H D, Zhou S L, Chen M, et al. Quaternion based nonlinear error model for rapid transfer alignment[J]. Journal of Astronautics, 2010, 31(10): 2328-2334

[2] 夏家和, 秦永元, 游金川. 搖擺狀態下基于非線性誤差模型的慣導對準研究[J]. 宇航學報, 2010, 31(2): 410-415. Xia Jia-he, Qin Yong-yuan, You Jin-chuan. Nonlinear error model based alignment method for SINS on swing base[J]. Journal of Astronautics, 2010, 31(2): 410-415.

[3] 謝波, 江一夫, 嚴恭敏, 等. 捷聯慣導基于地球系的動基座間接精對準算法[J]. 中國慣性技術學報, 2014, 22(5): 593-596. Xie Bo, Jiang Yi-fu, Yan Gong-min, et al. Indirect fine-alignment algorithm of in-motion SINS based on ECEF-frame[J]. Journal of Chinese Inertial Technology, 2014, 22(5): 593-596.

[4] 郭澤, 繆玲娟. 基于KF/UKF組合濾波的SINS大方位失準角初始對準[J]. 宇航學報, 2014, 35(2): 163-170. Guo Zhe, Miao Ling-juan. KF/UKF based SINS initial alignment under large azimuth misalignment angle[J]. Journal of Astronautics, 2014, 35(2): 163-170.

[5] 郭澤, 繆玲娟, 趙洪松. 一種改進的強跟蹤 UKF算法及其在大方位失準角初始對準中的應用[J]. 航空學報, 2014, 35(1): 203-214. Guo Zhe, Miao Ling-juan, Zhao Hong-song. An improved strong tracking UKF algorithm and its application in SINS initial alignment under large azimuth misalignment angles[J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(1): 203-214.

[6] 郭子偉, 繆玲娟, 趙洪松, 等. 一種改進的類高斯和粒子濾波在大失準角傳遞對準中的應用[J]. 航空學報, 2013, 34(1): 164-172. Guo Zi-wei, Miao Ling-juan, Zhao Hong-song. Application of an improved Gaussian-like sum particle filter to large misalignment transfer alignment[J]. Acta Aeronautica et Astronautica Sinica, 2013, 34(1): 164-172.

[7] 冉昌艷, 程向紅, 王海鵬. 基于 Gauss-Hermite 求積分卡爾曼濾波的SINS非線性初始對準方法[J]. 東南大學學報(自然科學版), 2014, 44(2): 265-271. Ran Chang-yan, Cheng Xiang-hong, Wang Hai-peng. SINS nonlinear initial alignment using Gauss-Hermite quadrature Kalman filter[J]. Journal of Southeast University (Natural Science Edition), 2014, 44(2): 265-271.

[8] Daniele M, Markley F L, Puneet S. Optimal linear attitude estimator[J]. Journal of Guidance, Control, and Dynamics, 2007, 30(6): 1619-1627.

[9] 梅春波, 秦永元, 楊鵬翔. 基于羅德里格參數的線性最優估計自對準[J]. 中國慣性技術學報, 2015, 23(3): 298-302. Mei Chun-bo, Qin Yong-yuan, Yang Peng-xiang. Linear optimized self-alignment for SINS using Rodrigues parameters[J]. Journal of Chinese Inertial Technology, 2015, 23(3): 298-302.

[10] Wu Y X, Pan X F. Velocity/position integration formula, Part I: Application to in-flight coarse alignment[J]. IEEE Transaction on Aerospace and Electronic Systems, 2013, 49(2): 1006-1023.

[11] Peter M G S. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci[J]. IEEE Transactions on Instrumentation and Measurement, 2011, 60(6): 1930-1941.

[12] Li W L, Tang K H, Lu L Q, etc. Optimization-based INS in-motion alignment approach for underwater vehicles[J]. International Journal for Light and Electron Optics, 2013, 124(20): 4581-4585.

[13] Kang T Z, Fang J C, Wang W. Quaternion optimization based in-flight alignment approach for airborne POS[J]. IEEE Transactions on Instrumentation and Measurement, 2012, 61(11): 2916-2923.

In-motion alignment for SINS/GPS based on Rodrigues parameter and nonlinear measurement

MEI Chun-bo, QIN Yong-yuan, FU Qiang-wen, YAN Gong-min
(School of Automation, Northwestern Polytechnical University, Xi’an 710129, China)

The alignment of strapdown inertial navigation system/GPS on moving base is studied. Based on the direct cosine matrix decomposition and Rodrigues parameter description, an alignment model with linear system equation and second-order nonlinear measurement equation is established. Furthermore, an optimal estimation algorithm using exact Taylor series expansions of second-order nonlinear measurement is deduced. Meanwhile, the singular problem of Rodrigues parameter description is discussed and a singular point avoidance schema is proposed. Finally, Monte Carlo simulation for the representative trajectory of the land vehicle is performed, and the results indicate that the singular point can be recognized and avoided within 5 s. In addition, the root mean square errors of level attitudes are less than 10″ and the root mean square error of yaw is less than 4′ after 120 s alignment for inertial-grade IMU, in case the errors of velocity and position are within 0.1 m/s and 3 m.

in-motion alignment; Rodrigues parameter; nonlinear measurement; exact Taylor series expansion; singular point

U666.12

：A

2015-08-28；

：2015-11-27

國家自然科學基金（61273333）

梅春波（1985—），男，博士研究生，主要研究方向為慣性導航、組合導航。E-mail: meichunbo@126.com

聯 系 人：秦永元（1946—），男，博士生導師。E-mail: qinyongyuan@nwpu.edu.cn

1005-6734(2015)06-0739-07

10.13695/j.cnki.12-1222/o3.2015.06.008