基于IEKF的水下無(wú)人潛器無(wú)源動(dòng)基座自對(duì)準(zhǔn)方法

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

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

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

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

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

1 不變擴(kuò)展卡爾曼濾波

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

1.1 擴(kuò)展卡爾曼濾波

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

EKF 遞推公式為：

狀態(tài)預(yù)測(cè)值：

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

濾波增益：

狀態(tài)更新：

協(xié)方差更新：

其中，I 為n 階單位陣。

1.2 不變擴(kuò)展卡爾曼濾波

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

1)左不變誤差

2)右不變誤差

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

2 水下無(wú)人潛器IEKF 濾波模型

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

2.1 慣性測(cè)量單元模型

2.2 系統(tǒng)模型

載體的動(dòng)力學(xué)模型如下：

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

增廣右不變誤差定義為：

其中，

其中令：

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

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

其中，

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

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

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

2.3 觀測(cè)模型

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

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

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

量測(cè)方程：

其中，σ為觀測(cè)噪聲。

2.4 算法流程

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

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

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

其中

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

3 試驗(yàn)驗(yàn)證

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

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

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

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

航向精度是初始對(duì)準(zhǔn)方法是否可行的風(fēng)向標(biāo)，基于IEKF 的潛器動(dòng)基座無(wú)源自對(duì)準(zhǔn)的試驗(yàn)結(jié)果如表1所示。

表1 對(duì)準(zhǔn)結(jié)果及誤差Tab.1 Alignment result and errors

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

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

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

圖3 EKF 和IEKF 算法的航向?qū)?zhǔn)曲線Fig.3 Head curve using EKF and IEKF

4 結(jié)論

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