基于IEKF的水下無人潛器無源動基座自對準方法

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

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

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

近年來非線性系統(tǒng)狀態(tài)估計在慣性導航領域的組合導航、初始對準等方面受到廣泛關注，包括擴展卡爾曼濾波（Extended Kalman Filter,EKF）、無跡卡爾曼濾波（Unscented Kalman Filter,UKF）、粒子濾波（Particle Filter,PF）等[1-6]。其中，傳統(tǒng)的EKF 有兩個主要缺陷，首先EKF 對系統(tǒng)狀態(tài)初值的準確性要求很高，如果狀態(tài)初值設定的與實際情況差距較大，濾波器很難收斂；其次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）模型，設計系統(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內快速收斂到1 mil 以內，證明了該方法的有效性和可行性。

1 不變擴展卡爾曼濾波

假設有如下形式的非線性離散系統(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)本質上是非線性的，這將導致濾波器精度降低甚至濾波發(fā)散的現(xiàn)象。擴展卡爾曼濾波通過進行泰勒級數(shù)展開，略去高階項，將非線性系統(tǒng)近似為線性系統(tǒng)，再作線性卡爾曼濾波估計，解決了標準卡爾曼濾波僅適用于線性系統(tǒng)的問題。

EKF 遞推公式為：

狀態(tài)預測值：

相關協(xié)方差：

濾波增益：

狀態(tài)更新：

協(xié)方差更新：

其中，I 為n 階單位陣。

1.2 不變擴展卡爾曼濾波

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

1)左不變誤差

2)右不變誤差

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

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

為了解決水下無人潛器在沒有準確外參考信息條件下的慣導系統(tǒng)動基座自對準問題，本文提出基于不變擴展卡爾曼濾波的動基座自對準方法。首先選取SINS 的方向余弦矩陣速度位置，陀螺和加速度計的隨機游走偏差作為狀態(tài)變量，建立系統(tǒng)狀態(tài)方程；其次選取SINS 解算速度與用解算航向角分解的外參考速度之差作為量測信息構建量測方程；同時構建IEKF 濾波模型，設計濾波算法，實現(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)估計值和輸入均無關，是一個常值。正是因為這一特點，IEKF解決了傳統(tǒng)EKF 存在的正反饋和不一致性問題。

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

2.3 觀測模型

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

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

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

量測方程：

其中，σ為觀測噪聲。

2.4 算法流程

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

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

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

其中

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

3 試驗驗證

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

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

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

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

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

表1 對準結果及誤差Tab.1 Alignment result and errors

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

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

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

圖3 EKF 和IEKF 算法的航向對準曲線Fig.3 Head curve using EKF and IEKF

4 結論

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