黃鳳榮，邢路然，陳英姝，王 震，朱雨晨

（河北工業(yè)大學 機械工程學院，天津 300401）

初始對準是慣性導航系統(tǒng)進行導航的前提，初始對準的精度將直接影響系統(tǒng)的導航精度。水下潛器(Autonomous Underwater Vehicle,AUV)在被母船投放之前，受限于實際安裝使用環(huán)境和艦船新舊電氣設(shè)備鏈接的困難，傳遞對準很難實施，這要求AUV 必須具備在航行過程中實現(xiàn)自主初始對準的功能；也就是在裝訂大概的速度信息和初始航向角，甚至在沒有裝訂初始航行角條件下盡快完成動基座初始對準。在這種條件下，慣性導航系統(tǒng)完成自對準的主要難點是：首先，由于外界行駛環(huán)境和自身運動影響，載體的速度不可能與裝訂的速度完全一致，因而速度是不準確的，這將導致初始對準常用的卡爾曼濾波收斂精度降低，甚至發(fā)散；特別是，慣性導航系統(tǒng)在運動過程中系統(tǒng)的誤差模型是非線性的，系統(tǒng)需要在大失準角的情況下，實現(xiàn)初始對準。

近年來非線性系統(tǒng)狀態(tài)估計在慣性導航領(lǐng)域的組合導航、初始對準等方面受到廣泛關(guān)注，包括擴展卡爾曼濾波（Extended Kalman Filter,EKF）、無跡卡爾曼濾波（Unscented Kalman Filter,UKF）、粒子濾波（Particle Filter,PF）等[1-6]。其中，傳統(tǒng)的EKF 有兩個主要缺陷，首先EKF 對系統(tǒng)狀態(tài)初值的準確性要求很高，如果狀態(tài)初值設(shè)定的與實際情況差距較大，濾波器很難收斂；其次EKF 還會導致不一致性問題，即每當新的觀測到來時，EKF 會通過舊狀態(tài)的線性化來計算當前狀態(tài)的協(xié)方差矩陣，但是當前狀態(tài)協(xié)方差矩陣的實際值并不與其保持一致。UKF 相較于EKF 來說，實用性更強，但傳統(tǒng)的UKF 和EKF 都需要已知系統(tǒng)噪聲和量測噪聲的先驗統(tǒng)計，在實際應用中，由于各種環(huán)境限制以及算法本身存在的問題，必然會導致濾波精度下降，甚至發(fā)散[7,8]。

針對水下潛器慣性導航系統(tǒng)動基座初始對準的難點，本文通過分析水下潛器慣導系統(tǒng)誤差特性，建立捷聯(lián)慣性導航系統(tǒng)（Strapdown Inertial Navigation System,SINS）模型，設(shè)計系統(tǒng)在大失準角下的不變擴展卡爾曼濾波（Invariant Extended Kalman Filter,IEKF）初始對準算法，從而克服外界參考信息不準確以及存在擾動的情況。IEKF 基于李代數(shù)將群變換應用于狀態(tài)變量，改變誤差的定義方式，實現(xiàn)系統(tǒng)矩陣與狀態(tài)估計值的獨立，擴展了保證狀態(tài)和協(xié)方差收斂的狀態(tài)空間[9,10]，解決了EKF 存在的問題。跑車試驗證明，該方法在大方位失準角的情況下，航向角在30 min內(nèi)快速收斂到1 mil 以內(nèi)，證明了該方法的有效性和可行性。

1 不變擴展卡爾曼濾波

假設(shè)有如下形式的非線性離散系統(tǒng)：

1.1 擴展卡爾曼濾波

傳統(tǒng)卡爾曼濾波是在標準條件下推導而來的，是一種無偏的線性最小方差估計算法，同時對系統(tǒng)要求嚴格，僅適用于系統(tǒng)模型準確、噪聲統(tǒng)計特性已知的線性系統(tǒng)。但在實際應用過程中由于外部運動的不規(guī)律和慣性元件的隨機誤差，在動態(tài)運動中很難得到系統(tǒng)精確的數(shù)學模型和噪聲統(tǒng)計特性[11]，系統(tǒng)本質(zhì)上是非線性的，這將導致濾波器精度降低甚至濾波發(fā)散的現(xiàn)象。擴展卡爾曼濾波通過進行泰勒級數(shù)展開，略去高階項，將非線性系統(tǒng)近似為線性系統(tǒng)，再作線性卡爾曼濾波估計，解決了標準卡爾曼濾波僅適用于線性系統(tǒng)的問題。

EKF 遞推公式為：

狀態(tài)預測值：

相關(guān)協(xié)方差：

濾波增益：

狀態(tài)更新：

協(xié)方差更新：

其中，I 為n 階單位陣。

1.2 不變擴展卡爾曼濾波

為解決上述問題，IEKF 基于李代數(shù)，將群變換應用于系統(tǒng)狀態(tài)變量，所以IEKF 的狀態(tài)量是一個矩陣，并且構(gòu)成群。為了實現(xiàn)系統(tǒng)矩陣與狀態(tài)估計值的獨立，解決傳統(tǒng)EKF 存在的問題，將誤差也定義在群上，即下面左不變誤差和右不變誤差兩種形式：

1)左不變誤差

2)右不變誤差

選擇左不變誤差還是右不變誤差的依據(jù)是能否實現(xiàn)系統(tǒng)矩陣與狀態(tài)估計值的獨立。本文使用右不變誤差建立SINS 模型。在此誤差定義下，IEKF 的系統(tǒng)矩陣為常值，與當前狀態(tài)估計值以及輸入均無關(guān)，因此解決了上述EKF 存在的正反饋與不一致性問題。

2 水下無人潛器IEKF 濾波模型

為了解決水下無人潛器在沒有準確外參考信息條件下的慣導系統(tǒng)動基座自對準問題，本文提出基于不變擴展卡爾曼濾波的動基座自對準方法。首先選取SINS 的方向余弦矩陣速度位置，陀螺和加速度計的隨機游走偏差作為狀態(tài)變量，建立系統(tǒng)狀態(tài)方程；其次選取SINS 解算速度與用解算航向角分解的外參考速度之差作為量測信息構(gòu)建量測方程；同時構(gòu)建IEKF 濾波模型，設(shè)計濾波算法，實現(xiàn)了水下無人潛器的無源動基座自對準。

2.1 慣性測量單元模型

2.2 系統(tǒng)模型

載體的動力學模型如下：

當考慮IMU 的隨機游走偏差時，IEKF 系統(tǒng)模型的狀態(tài)量分為可被描述成群元素的量，和不能被描述為群元素的量即：

增廣右不變誤差定義為：

其中，

其中令：

要想得到系統(tǒng)狀態(tài)方程，首先需要對增廣右不變誤差式(27)對時間進行微分，即：

綜上，系統(tǒng)狀態(tài)方程為：

其中，

F為系統(tǒng)矩陣，G是噪聲輸入矩陣。

從式(42)(43)可以看出，在不考慮陀螺和加速度計偏差的情況下，系統(tǒng)矩陣為，與當前狀態(tài)估計值和輸入均無關(guān)，是一個常值。正是因為這一特點，IEKF解決了傳統(tǒng)EKF 存在的正反饋和不一致性問題。

而在考慮IMU 的偏差時，系統(tǒng)矩陣F不再是一個常量，每個時刻的系統(tǒng)矩陣與當前狀態(tài)估計值有關(guān)。但是這種聯(lián)系只通過慣性元件偏差或噪聲等小量引入，對于這樣的系統(tǒng)方程[12]，仍然能夠保證一致性，同樣在魯棒性和收斂性上優(yōu)于傳統(tǒng)的EKF。

2.3 觀測模型

初始對準過程中航向信息的可觀測度較弱，特別是在運動過程中外界提供速度信息不穩(wěn)定情況下。在采用速度誤差作為觀測量時，考慮航向信息的融合，在計算慣導系統(tǒng)北向和東向速度時，用導航解算的航向角對外參考速度進行分解。

圖1 合速度分解示意圖Fig.1 Resolution of resultant velocity

設(shè)合速度為V，航向角為，則系統(tǒng)的速度分解公式可以表達為：

量測方程：

其中，σ為觀測噪聲。

2.4 算法流程

IEKF 算法的流程與傳統(tǒng)EKF 相同，只是狀態(tài)更新公式不同，即：

設(shè)線性化的狀態(tài)誤差為，

可得狀態(tài)更新公式，

其中

如式(51)所示，IEKF 中能表示為群元素的部分采用指數(shù)更新，不能表示為群元素的部分采用普通的線性更新。

3 試驗驗證

為了驗證基于IEKF 水下無人潛器無源動基座自對準算法的可行性與工程實用性，進行了跑車試驗。試驗設(shè)備采用光纖陀螺捷聯(lián)慣性導航系統(tǒng)，其中光纖陀螺零偏穩(wěn)定性為0.01 °/h，加速度計零偏穩(wěn)定性為50 μg。同時將一套高精度慣導系統(tǒng)作為基準設(shè)備，以驗證IEKF 方法的性能。試驗中同時同步錄取基準設(shè)備以及試驗設(shè)備的信息。

在試驗過程中，待試驗車基本在勻速運動狀態(tài)穩(wěn)定后，將光纖捷聯(lián)慣導系統(tǒng)通電，根據(jù)實際車速人工裝訂一個固定的速度。本次試驗裝訂速度為6.5 kn，保持車輛勻速運動，半小時后完成初始對準，同時同步錄取基準設(shè)備的導航信息進行對比。

在載體勻速運動過程中，由于開車人員或道路條件的外界因素影響較大，載體的速度很難保證精確地按照設(shè)定的速度（6.5 kn）勻速運動。在這種情況下首先將IEKF 估計出來的速度與GPS 速度進行對比，如圖2所示。由圖2 可以看出，設(shè)計的IEKF 濾波器能在速度波動的時候有效地跟蹤實際速度，保證了對準精度。

圖2 IEKF 輸出速度與GPS 速度Fig.2 Velocity of IEKF and GPS velocity

航向精度是初始對準方法是否可行的風向標，基于IEKF 的潛器動基座無源自對準的試驗結(jié)果如表1所示。

表1 對準結(jié)果及誤差Tab.1 Alignment result and errors

從表1 的試驗結(jié)果可以看出，基于IEKF 的無源動基座自對準方法的最大航向誤差為0.12 °，最小為0.01 °，達到了慣性元件所對應的航向精度水平，方法的可行性與有效性得到了驗證。

最后，將本文所設(shè)計的IEKF 濾波算法估計的航向角與傳統(tǒng)EKF 濾波算法估計得到的航向角信息進行了試驗對比。試驗條件選取一個相對平緩沒有紅綠燈的路徑，保證車速不會出現(xiàn)太大的突變，分別在朝向東和北的路面上進行跑車試驗。發(fā)動車輛進入勻速運動狀態(tài)后再進行設(shè)備通電，錄取原始脈沖數(shù)，30 min后停車，同時記錄此時參考基準設(shè)備的姿態(tài)角備用。兩種方法仿真結(jié)果對比如圖3所示。

由圖3 可見，IEKF 能夠快速收斂，在短時間內(nèi)就收斂到小角度，而傳統(tǒng)的EKF 剛開始方位失準角超調(diào)很大，并且最終沒有達到收斂狀態(tài)。這是因為傳統(tǒng)EKF的系統(tǒng)矩陣依賴當前狀態(tài)的估計值，當狀態(tài)估計值與真值差距較大時，會直接導致依賴狀態(tài)估計值的系統(tǒng)矩陣也有較大偏差，使用這樣的系統(tǒng)矩陣繼續(xù)傳遞誤差，使誤差又進一步放大，整個系統(tǒng)形成正反饋，最終導致濾波器發(fā)散。

圖3 EKF 和IEKF 算法的航向?qū)是€Fig.3 Head curve using EKF and IEKF

4 結(jié)論

半實物仿真和跑車試驗驗證了所設(shè)計的IEKF 算法可以實現(xiàn)AUV 的無源動基座對準。試驗結(jié)果表明：提出的方法在大失準角情況下依然能夠保證較好的對準精度，使用陀螺偏差為0.01 °/h 的光纖陀螺，航向角在30 min 內(nèi)收斂到1 mil 以內(nèi)，達到了慣性元件所對應的航向精度水平，驗證了該方法的有效性。與EKF方法相比，IEKF 算法不論是在精度還是在收斂性上，都優(yōu)于EKF，但是在實現(xiàn)和計算負載方面與EKF 相似。在未來的工作中，我們希望不僅限于水下潛器的運動條件，也為其他載體的對準方法提供設(shè)計參考，以及研究是否可以將李代數(shù)與群變換應用于其他濾波算法，形成新的濾波器，如不變無跡卡爾曼濾波等。