基于歐拉角的李群捷聯(lián)慣導(dǎo)誤差模型分析與比較研究

朱天高, 劉 勇, 李開龍,*, 趙仁杰

(1. 海軍工程大學(xué)電氣工程學(xué)院, 湖北 武漢 430033; 2. 海軍工程大學(xué)艦船與海洋學(xué)院, 湖北 武漢 430033; 3. 海裝駐北京地區(qū)軍事代表局, 北京 100071)

0 引 言

以捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system, SINS)為核心,其他定位系統(tǒng)(全球定位系統(tǒng)(global position system, GPS)、多普勒計程儀(Doppler velocity log, DVL)等,為輔助信息源,實時修正構(gòu)成捷聯(lián)慣性基組合導(dǎo)航系統(tǒng)[1-6]。對于捷聯(lián)慣性基組合導(dǎo)航系統(tǒng),組合導(dǎo)航模型的構(gòu)建是最為核心的部分之一[7-10]。組合導(dǎo)航模型主要分為基本模型和誤差模型,一般采用誤差模型進行組合導(dǎo)航。誤差模型的選取對于組合導(dǎo)航性能的影響極大,因此如何構(gòu)建合適的誤差模型至關(guān)重要[11-16]。

近年來,李群這一數(shù)學(xué)模型被引入慣導(dǎo)誤差模型的構(gòu)建,引起眾多學(xué)者的關(guān)注[17-18]。在傳統(tǒng)誤差定義中,姿態(tài)誤差定義在特殊正交群(special orthogonal group,SO(3)),而其他狀態(tài)誤差均定義于歐式空間中[19-20]。若將姿態(tài)、速度狀態(tài)量引入一個群中,可構(gòu)成特殊歐式群(special euclidean group,SE(3)),從而構(gòu)建新的誤差模型[21-22]。在構(gòu)建誤差模型時,根據(jù)誤差定義的不同,可分為左乘和右乘,對于觀測量也存在左不變與右不變之分[23-24]。因此,如何針對不同的觀測量選取相對應(yīng)的誤差模型,是實現(xiàn)高精度組合導(dǎo)航的關(guān)鍵所在。

文獻[25]提出載體系四元數(shù)誤差模型,并證明該誤差模型更適用于SINS/GPS組合導(dǎo)航。文獻[26-27]提出基于歐拉角的李群左誤差模型和右誤差模型,證明將左誤差模型應(yīng)用于SINS/GPS組合導(dǎo)航的效果更佳。文獻[28]提出地球系下的李群左誤差模型和右誤差模型,進行SINS/GPS與SINS/OD(odometer)兩種組合導(dǎo)航實驗,對誤差模型的選取提出了有效的方法。

以上對于誤差模型的應(yīng)用選擇均取得諸多成果,但針對基于歐拉角的兩種李群誤差模型的選取和適用差異性等的研究還十分缺乏。基于此,本文以SINS/GPS和SINS/DVL兩種典型組合導(dǎo)航應(yīng)用為研究對象,通過車載實驗與船載實驗探究李群左、右誤差模型的適用性能差異,為不同應(yīng)用場景的誤差模型選取提供了有益的思考。

1 李群模型

定義坐標系如下:記地心慣性坐標系為i系,地球坐標系為e系,選擇“右-前-上“載體坐標系為b系,計算載體坐標系為b′系,“東-北-天”地理坐標系為導(dǎo)航坐標系,記為n系,計算導(dǎo)航坐標系為n′系。

1.1 李群與李代數(shù)

李群是指具有連續(xù)(光滑)性質(zhì)的群,每個李群對應(yīng)一個李代數(shù)[29-31]。特殊正交群和特殊歐式群由矩陣乘法分別與旋轉(zhuǎn)矩陣集合和變換矩陣集合組合,主要用來描述剛體運動,分別用SO(3)和SE(3)表示:

SO(3)={R∈SO(3) |RRT=I3,det(R)=1}

(1)

(2)

式中:R為旋轉(zhuǎn)矩陣;t為平移向量。SE(3)的逆矩陣為

(3)

李代數(shù)位于向量空間,是李群對應(yīng)一種結(jié)構(gòu)。SO(3)和SE(3)對應(yīng)的李代數(shù)分別為

SO(3)={θ∈R3,(θ×)∈R3×3}

(4)

(5)

式中:(·×)表示反對稱矩陣;θ為三維的旋轉(zhuǎn)向量;ξ為三維的平移向量。

李群與李代數(shù)之間的轉(zhuǎn)換可通過映射實現(xiàn),SE(3)和SE(3)的映射關(guān)系如下:

(6)

式中:exp表示矩陣映射,J表示SO(3)的雅可比矩陣,表示如下:

(7)

式中:a表示單位向量。

1.2 李群捷聯(lián)慣導(dǎo)更新模型

導(dǎo)航坐標系下SO(3)空間中捷聯(lián)慣導(dǎo)更新方程為

(8)

(9)

(10)

(11)

(12)

對式(12)求逆,可得其逆矩陣為

(13)

2 李群狀態(tài)誤差模型

2.1 李群右誤差模型

李群右誤差定義為

(14)

設(shè)φr為右誤差模型的失準角誤差,當φr取極小值時,根據(jù)李群和李代數(shù)之間關(guān)系可得[15]

(15)

(16)

分別對式(15)、式(16)求導(dǎo),可得右誤差模型的姿態(tài)和速度誤差微分方程為

(17)

(18)

右誤差位置誤差微分方程為

(19)

式中:

對于慣性傳感器而言,如果只考慮常值漂移與隨機游走噪聲,則有

(20)

(21)

定義右誤差模型狀態(tài)誤差為

(22)

由式(17)～式(21)可得基于歐拉角的李群右誤差模型在導(dǎo)航系下的狀態(tài)誤差方程為

(23)

式中:Fr為系統(tǒng)矩陣;Gr為噪聲轉(zhuǎn)移矩陣;ηb為過程噪聲向量,它們的表達式分別如下:

(24)

(25)

2.2 李群左誤差模型

李群左誤差定義為

(26)

(27)

(28)

分別對式(27)、式(28)求導(dǎo),可得左誤差模型的姿態(tài)和速度誤差微分方程為

(29)

(30)

左誤差模型位置誤差微分方程為

(31)

定義左誤差模型狀態(tài)誤差方程為

(32)

式中:Fl和Gl的表達式分別為

(33)

(34)

3 實驗驗證

3.1 李群誤差模型分析與比較

在實際應(yīng)用中,誤差模型的選擇可以根據(jù)觀測值的“不變”類型來確定,“不變”觀測值有以下兩種定義[12-13]:

“左不變”觀測:y=χb

(35)

“右不變”觀測:y=χ-1b

(36)

式中:b是一個常值向量。若觀測量滿足式(35)“左不變”觀測條件,則在組合導(dǎo)航模型中應(yīng)用狀態(tài)左誤差模型會更有利,反之,如果觀測量滿足式(36)“右不變“觀測條件,則在組合導(dǎo)航模型中應(yīng)用狀態(tài)右誤差模型會更有利。

(37)

式中:bv和bp均為常值向量,速度和位置觀測滿足左不變觀測定義。因此,GPS提供的觀測信息屬于左不變觀測。

對于SINS/DVL組合導(dǎo)航系統(tǒng),DVL可以提供載體系下的速度vb的觀測信息。若選用速度觀測y=vb,可以寫成以下形式:

(38)

此外,組合導(dǎo)航關(guān)注的重點在于姿態(tài),也可以從姿態(tài)誤差定義角度來進行討論。左、右誤差模型姿態(tài)誤差定義分別為

(39)

(40)

綜上分析,SINS/GPS和SINS/DVL分別對于狀態(tài)左誤差模型和狀態(tài)右誤差模型效果最佳。下面通過跑車和跑船實驗,利用實測數(shù)據(jù)比較兩種狀態(tài)誤差模型的組合導(dǎo)航效果,驗證實驗結(jié)果是否與本節(jié)理論分析一致。

3.2 SINS/GPS組合導(dǎo)航實驗

本節(jié)開展實驗驗證,主要對基于歐拉角的李群狀態(tài)左誤差模型、李群狀態(tài)右誤差模型以及傳統(tǒng)誤差模型進行對比分析。為方便后續(xù)描述,將這3種模型分別記為LSE-Euler model、RSE-Euler model和SO-Euler model,狀態(tài)量的具體表示分別如下:

(41)

(42)

(43)

對于SINS/GPS組合導(dǎo)航系統(tǒng),上文已給出姿態(tài)、速度和位置誤差微分方程。量測量選擇速度和位置,則3種模型的量測矩陣分別為

(44)

(45)

(46)

根據(jù)狀態(tài)誤差模型不同,反饋校正也有所差別,表達式分別如下:

(47)

(48)

(49)

下面進行車載實驗,對誤差模型進行對比分析。微電子機械系統(tǒng)(micro-electro-mechanical system,MEMS)/GPS車載實驗平臺主要包括MEMS(STIM300)、GPS接收機以及高精度的作為參考系統(tǒng)的光纖慣導(dǎo),實驗一共持續(xù)3 000 s。MEMS的性能指標如表1所示,GPS接收機的速度精度小于0.1 m/s,位置精度小于10 m,采樣頻率為1 Hz。組合導(dǎo)航系統(tǒng)采用GPS速度和位置作為觀測量,實驗過程全程都能夠較好地接收GPS信號[32]。

表1 MEMS性能指標 Table 1 Specifications of MEMS

首先設(shè)置初始失準角為[1°1°3°]和[3°3°9°]進行組合導(dǎo)航實驗,由于實驗過程采用速度和位置作為觀測量,因此重點比較姿態(tài)量的估計結(jié)果。圖1和圖2是姿態(tài)估計誤差結(jié)果。由圖可知,3種模型的姿態(tài)失準角均可收斂至較小范圍,但在不同失準角條件下,3種模型的航向角估計誤差有一定變化,縱傾角和橫滾角估計誤差仍幾乎重合。因此,在小失準角條件下,3種誤差模型均適用。

圖1 [1° 1° 3°]姿態(tài)估計誤差Fig.1 Estimation error of attitude on [1° 1° 3°]

圖2 [3° 3° 9°]姿態(tài)估計誤差Fig.2 Estimation error of attitude on [3° 3° 9°]

接下來設(shè)置初始失準角為[5° 5° 15°]、[10° 10° 30°]、[20° 20° 60°]以及[30° 30° 90°],進行組合導(dǎo)航實驗。由上述實驗可知,3種誤差模型的縱傾角和橫滾角的估計差別不大,故而重點關(guān)注航向角姿態(tài)估計。圖3～圖6為航向角估計誤差。觀察圖可知,相比于SO-Euler模型,RSE-Euler模型和LSE-Euler模型在大失準角條件下有較大優(yōu)勢,可快速收斂到較小誤差。尤其是從圖5和圖6可知,SO-Euler模型的航向角估計誤差最小也是在5°以上,已無法進行組合導(dǎo)航,而RSE-Euler模型和LSE-Euler模型航向角估計誤差仍可收斂到1°以下,且誤差曲線波動較小,可進行高精度穩(wěn)定的組合導(dǎo)航。相比于RSE-Euler模型,LSE-Euler模型的姿態(tài)估計效果整體更優(yōu),精度更高,收斂速度更快,誤差曲線波動更小。實驗結(jié)果表明,基于李群的誤差模型的姿態(tài)估計效果優(yōu)于常規(guī)的線性誤差模型,LSE-Euler模型整體優(yōu)于RSE-Euler模型,此實驗結(jié)果與上文SINS/GPS組合導(dǎo)航中左誤差模型要優(yōu)于右誤差模型的理論分析保持一致。

圖3 [5° 5° 15°]航向角估計誤差Fig.3 Estimation error of yaw on [5° 5° 15°]

圖4 [10° 10° 30°]航向角估計誤差Fig.4 Estimation error of yaw on [10° 10° 30°]

圖5 [20° 20° 60°]航向角估計誤差Fig.5 Estimation error of yaw on [20° 20° 60°]

圖6 [30° 30° 90°]航向角估計誤差Fig.6 Estimation error of yaw on [30° 30° 90°]

3.3 SINS/DVL組合導(dǎo)航實驗

對于SINS/DVL組合導(dǎo)航系統(tǒng),選取速度作為量測量,由于需考慮DVL的刻度系數(shù)因子,則LSE-Euler、RSE-Euler、SO-Euler 3種誤差模型的狀態(tài)量分別具體表示如下:

(50)

(51)

(52)

則3種模型的量測模型分別為

(53)

下面通過船載實驗對李群左、右誤差模型進行對比分析。實驗數(shù)據(jù)是從一套安裝有慣性測量單元(inertial measurement unit,IMU)和DVL的船載實驗系統(tǒng)中采集得到的。表2和表3分別為IMU和DVL的主要性能指標。實驗船上同時安裝了一個單天線的GPS接收機,用于接收GPS數(shù)據(jù),輸出速度和位置信息,其數(shù)據(jù)更新率為1 Hz。利用IMU輸出數(shù)據(jù)和GPS輸出數(shù)據(jù)進行組合導(dǎo)航,生成參考姿態(tài)、速度和位置信息,分別作為實驗中的姿態(tài)、速度和位置參考基準。船載試驗在長江中進行,實驗系統(tǒng)開機后15 min內(nèi)保持系泊狀態(tài),然后試驗船開始航行,航行時間大約為6 h。記錄整個運動過程中的IMU和DVL的原始數(shù)據(jù)以及GPS輸出的速度和位置數(shù)據(jù)。本文選取其中10 000 s船載實測數(shù)據(jù)用于組合導(dǎo)航仿真實驗驗證。

表2 IMU性能指標 Table 2 Specifications of IMU

表3 DVL性能指標 Table 3 Specifications of DVL

本實驗首先在初始失準角條件下進行組合導(dǎo)航實驗,3種誤差模型的差別較小,限于篇幅原因在此不予以展示。下面主要介紹初始失準角為[5° 5° 15°]時進行的組合導(dǎo)航實驗。姿態(tài)估計誤差如圖7～圖9所示,3種誤差模型的姿態(tài)誤差估計相差較小,最終均可收斂至較小且保持穩(wěn)定。

圖7 縱傾角估計誤差Fig.7 Estimation error of pitch angle

圖8 橫滾角估計誤差Fig.8 Estimation error of roll angle

圖9 航向角估計誤差Fig.9 Estimation error of yaw angle

對于SINS/DVL組合導(dǎo)航而言,DVL只有速度輸出,因此速度和位置的估計精度是非常重要的導(dǎo)航參數(shù)指標。圖10和圖11為東向速度誤差和北向速度誤差,對于東向速度誤差,LSE-Euler模型的誤差最大,RSE-Euler模型和SO-Euler模型的誤差較小。對于北向速度誤差,3種模型相差不大,基本保持一致,由此可以看出RSE-Euler模型和SO-Euler模型要優(yōu)于LSE-Euler模型。圖12和圖13分別為緯度誤差和經(jīng)度誤差,圖14為軌跡對比圖。由圖可知,RSE-Euler模型緯度誤差最小,SO-Euler模型緯度誤差次之,LSE-Euler模型緯度誤差最大。而對于經(jīng)度誤差,SO-Euler模型則要優(yōu)于LSE-Euler模型和RSE-Euler模型。通過圖14中的軌跡對比可發(fā)現(xiàn),SO-Euler模型和RSE-Euler模型明顯優(yōu)于LSE-Euler模型,RSE-Euler模型則要略微優(yōu)于SO-Euler模型。因此,由實驗結(jié)果可以得出,RSE-Euler模型最佳,SO-Euler模型稍次之,LSE-Euler模型最差。故而對于SINS/DVL組合導(dǎo)航系統(tǒng),RSE-Euler模型為最佳組合導(dǎo)航方式,與上文右誤差模型最適合SINS/DVL組合導(dǎo)航系統(tǒng)的理論分析取得了相互印證。

圖10 東向速度誤差Fig.10 Estimation error of east velocity

圖11 北向速度誤差Fig.11 Estimation error of north velocity

圖12 緯度誤差Fig.12 Estimation error of latitude

圖13 經(jīng)度誤差Fig.13 Estimation error of longitude

4 結(jié) 論

本文對基于歐拉角的兩種李群誤差模型進行了比較研究,提出了針對SINS/GPS和SINS/DVL兩種典型的組合導(dǎo)航應(yīng)用的誤差模型選擇方案,并對該方案進行了理論分析。通過車載實驗和船載實驗驗證了本文提出的誤差模型選擇方案的有效性。實驗結(jié)果表明,李群左誤差模型和右誤差模型分別更適用于SINS/GPS和SINS/DVL組合導(dǎo)航系統(tǒng),分別具備更好的定位效果。本文所提方案在實際組合導(dǎo)航系統(tǒng)的誤差模型選擇上具有較大的優(yōu)勢,具有良好的工程應(yīng)用價值,同時也為其他組合導(dǎo)航模型的構(gòu)建拓寬了思路。