基于改進四元數阻尼誤差模型的SINS初始對準算法

趙仁杰, 李開龍, 胡柏青, 田佳玉

(海軍工程大學電氣工程學院, 湖北 武漢 430033)

0 引 言

初始對準是捷聯慣性導航系統(strapdown inertial navigation system, SINS)的一項關鍵性技術,主要為SINS提供準確的初始姿態等信息,其中,對準的精度和速度是影響SINS后續工作的兩項重要指標[1]。

經過不斷研究,SINS初始對準方法逐漸豐富和完善。目前,這些方法主要可以歸納為以下兩類:第一類是兩階段對準,先通過粗對準獲取粗略的姿態信息,再進行精對準。其中,傳統粗對準方法為解析法[2-3],即利用慣性器件敏感重力加速度和地球自轉角速度,通過矢量定姿求解姿態,一般僅適用于靜止或微幅擾動條件下,并對器件精度有一定要求。為克服解析法的局限性,文獻[4]提出了一種基于坐標系分解的慣性系對準方法,能夠有效隔離角運動,實現晃動基座粗對準;在此基礎上,Wu等[5]又將對準問題轉化為姿態確定問題(Wahba問題[6]),提出了優化對準(optimization-based alignment, OBA)方法,提高了信息利用率,具有較強的魯棒性,后續得到了廣泛關注和研究[7-10];精對準方法主要有羅經法、參數辨識法和卡爾曼濾波法。第二類是大失準角非線性對準[11-12],研究主要集中在非線性誤差模型和非線性濾波算法兩方面,其中,非線性誤差模型的研究重點在于姿態[13-15];非線性濾波算法主要有拓展卡爾曼濾波、無跡卡爾曼濾波、容積卡爾曼濾波、粒子濾波等,其中,無跡卡爾曼濾波(unscented Kalman filter, UKF)憑借較高精度以及適中的計算量,在解決非線性問題中被廣泛應用。

非線性對準可以簡化對準過程,縮短對準時間,但實際對準效果往往卻達不到最優,對此許多學者對濾波算法進行了改進,有效提高了濾波性能[16-19];文獻[20]提出了一種基于坐標系一致性的矢量幾何誤差構建思想,在此基礎上,王茂松等[21-22]指出傳統速度誤差只考慮了向量的大小差異,而忽視了方向差異,重新對其進行了定義,提出一種狀態變換卡爾曼濾波方法,有效提高了組合導航的航向估計精度和穩定性。如果真實導航系未知,實際SINS解算應在計算坐標系下進行,上述文獻將速度誤差矢量統一在真實導航系下的做法仍有待討論。Chang等[23]將速度誤差統一定義在計算坐標系下,推導了改進歐拉角非線性對準模型,并通過試驗驗證了模型的有效性。

基于矢量誤差坐標系一致性思想,本文對傳統四元數非線性誤差模型中的姿態誤差模型和速度誤差模型進行改進,將誤差矢量統一投影在計算導航系下,同時引入外界阻尼信息,提出一種改進四元數阻尼誤差模型對準算法,并應用于系泊狀態下的SINS初始對準,通過仿真和車載試驗對比了該改進算法與傳統四元數阻尼誤差模型對準算法以及文獻[23]中歐拉角阻尼誤差模型對準算法的對準效果,驗證了本文所提算法的有效性。

1 四元數阻尼SINS誤差模型

關于SINS的大失準角非線性誤差模型已有完整推導[13-14],考慮形式比較復雜,本節引入全球定位系統(global positioning system,GPS)速度和位置阻尼信息進行模型簡化,推導了四元數阻尼非線性誤差模型。

為方便建模,將由GPS阻尼信息獲取的參量統一用下標g表示,忽略小量誤差,認為帶下標g的計算參量值等效為真實值。此外,定義坐標系如下:記地心慣性坐標系為i系,地球坐標系為e系,選擇“右前上”載體坐標系為b系,“東北天”地理坐標系為導航坐標系,記為n系,計算導航坐標系為n′系。

1.1 SINS狀態微分方程

給出由四元數表示的SINS姿態、速度和位置微分方程,具體如下:

(1)

(2)

(3)

(4)

(5)

式中:I3×3為3維單位矩陣;RP為當地曲率矩陣,表達式為

(6)

式中:RM和RN分別表示子午圈與卯酉圈主曲率半徑。

1.2 傳統阻尼誤差方程

引入GPS阻尼速度和位置,實際阻尼狀態微分方程可表示為

(7)

(8)

(9)

(10)

(11)

(12)

(13)

式中:q*代表四元數共軛。

對式(13)左右兩邊同時微分可得

(14)

將式(1)和式(7)代入式(14),參照文獻[14]推導得到傳統的阻尼姿態誤差方程,具體如下:

(15)

傳統的速度誤差定義為

δvn′=vn′-vn

(16)

對式(16)兩邊同時微分,推導可得傳統的阻尼速度誤差方程,具體如下:

(17)

式中:δfb表示加速度計測量誤差。

直接給出傳統的阻尼位置誤差方程如下:

(18)

1.3 改進阻尼誤差方程

基于實際SINS解算在n′系下進行這一認識,通過觀察第1.2節傳統阻尼非線性誤差方程的推導過程,不難發現有以下兩處不夠嚴格:

(2)式(16)速度誤差定義并未考慮速度矢量的方向性,只是標量意義上的減法,存在坐標系不一致問題。

針對上述問題,本節將誤差統一至n′系,重新推導了改進阻尼非線性誤差方程,其中,改進阻尼姿態誤差方程如下所示:

(19)

考慮矢量方向性,重新定義速度誤差[23]為

(20)

對式(20)左右兩邊同時微分可得:

(21)

將式(2)和式(8)代入式(21),推導可得改進阻尼速度誤差方程,具體如下:

(22)

改進阻尼位置誤差方程如下所示:

(23)

2 系泊條件下的非線性對準算法

本文研究系泊狀態下的SINS初始對準,在系泊狀態下,載體位置相對固定,可以通過GPS準確獲取,線速度近似為零,是有效的零速約束條件。SINS誤差模型通常包括姿態、速度、位置和器件誤差,由于對準的目的是在短時間內快速獲取準確的姿態,考慮到器件誤差估計不僅受器件精度的影響,還和載體運動狀態有關,且靜態下器件誤差的可觀性不強,因此,本節僅將姿態誤差和速度誤差列為狀態,推導系泊條件下的非線性對準模型,同時結合UKF算法給出具體的對準步驟。

2.1 非線性對準模型

(24)

式中:

(25)

(26)

以速度誤差dvn′=vn′作為量測,相應量測模型如下:

y=Hx+v=[03×3,I3×3]x+v

(27)

式中:v表示速度噪聲。

式(24)和式(27)共同構成了非線性對準模型。

2.2 非線性對準算法

本文選取UKF算法用于非線性對準,為避免四元數在UKF中存在的歸一化約束以及方差匹配問題,采用四元數無跡估計器(unscented quaternion estimator, USQUE)框架[24]。

下文給出對準算法流程。

步驟 1初始化

定義四元數誤差δq,參照下式將其轉換為誤差修正羅德里格斯參數δσ,設置局部狀態為x=[δσT,xeT]T,其中,xe表示非姿態誤差狀態。

σ=ρ/(1+q0)

(28)

(29)

式中:n為狀態向量維數;α為調節因子,一般取1e-4≤α≤1;κ、β均為比例因子,通常取κ=0,β≥0;

步驟 2導航解算

(30)

(31)

步驟3濾波

(1)時間更新

產生狀態sigma點:

(32)

(33)

(34)

(35)

(36)

(37)

(38)

(2)量測更新

由于量測模型為線性,量測更新如下:

(39)

(40)

Pk=(In×n-KkHk)Pk/k-1

(41)

(3)姿態更新

(42)

3 試驗驗證

3.1 仿真試驗

為驗證本文所提改進四元數阻尼誤差模型對準算法在系泊條件下對準的有效性,將改進算法與傳統四元數阻尼誤差模型對準算法以及文獻[23]中改進和傳統歐拉角阻尼誤差模型對準算法進行對比,為簡便表述,將上述4種算法分別記為Quaternion-I、Quaternion-T、Euler-I和Euler-T。

觀察圖1可以看出,相比其他3種算法,本文提出的Quaternion-I算法在對準速度和精度上均具有優勢;此外,相同姿態表示下,改進模型對準算法的姿態估計效果整體上優于傳統模型;在改進算法中,基于四元數模型的Quaternion-I算法對準效果要優于基于歐拉角模型的Euler-I算法。

圖1 200次蒙特卡羅仿真姿態角估計誤差

為更加直觀的對比4種算法對準效果,將200次蒙特卡羅仿真試驗姿態估計誤差取絕對值后求平均,得到姿態平均估計誤差,如圖2所示。

圖2 200次蒙特卡羅仿真姿態角平均估計誤差

觀察圖2,在靜基座大失準角仿真條件下,4種對準算法均能將姿態失準角收斂至小范圍,由于水平姿態角的可觀性強于航向角,其估計效果也明顯優于航向角。在航向估計上,本文提出的Quaternion-I算法收斂速度最快,其次為Euler-I算法,而Quaternion-T算法和Euler-T算法則相對較慢,雖然也可以將航向失準角拉回至小角度,但對準結束時刻仍未穩定收斂。為便于比較,統計對準結束時刻的姿態角平均誤差作為算法的對準精度,如表1所示。

表1 4種算法姿態對準精度

由表1可知,Quaternion-I算法的俯仰角估計精度為0.399′,橫滾角精度為0.375′,航向角精度為0.097°,整體估計精度最優。

大失準角對準條件下,非線性濾波采用反饋校正方式,將濾波器輸出結果反饋至導航結算過程,每一步都會對狀態誤差進行修正,進而使得誤差能夠快速有效收斂;同一姿態表示下,改進模型修正了由坐標系投影不一致引起的模型誤差,相比傳統模型具有更好的對準效果;此外,對于改進模型對準算法,由于大失準角下歐拉角表示的姿態誤差矩陣會因轉動次序不同而不同,模型并不嚴格,相比之下,四元數是作為整體被使用,不存在轉動次序問題且無奇異,故四元數模型對準算法的姿態對準效果要優于歐拉角模型。仿真結果表明,本文提出的Quaternion-I算法不僅在對準速度和精度上具有優勢,同時還具有較好的穩定性。

3.2 車載試驗

利用慣性級激光捷聯慣組靜態數據進行算法驗證。捷聯慣組器件精度如下:陀螺隨機漂移穩定性為0.01°/h,加速度計零偏穩定性為50 μg;數據采集位置為北緯34.246°。試驗將捷聯慣組安裝在載車上,靜止采集約半小時數據,實驗過程存在開關車門、上下車走動等干擾活動。

本文選取600 s數據用于算法驗證,在姿態基準上加入大失準角構造初始姿態,為充分驗證算法有效性,設置5組不同的大失準角,具體如下:Ⅰ[-10° 10° 20°]、Ⅱ[-20° 20° 40°]、Ⅲ[-30° 30° 60°]、Ⅳ[-40° 40° 80°]、Ⅳ[-50° 50° 100°],針對不同失準角條件,執行4種算法,對比姿態對準結果,限于篇幅僅給出第Ⅲ組條件下的姿態誤差對比,如圖3所示。

統計5組失準角下4種算法的姿態估計誤差,如表2所示。分析圖3和表2可得,在不同條件下,4種算法的水平失準角均能夠很快收斂,且精度較高,但在航向估計上,600 s對準時間內,隨著初始失準角的增大,Euler-T算法和Quaternion-T算法的對準誤差有增加趨勢,而Euler-I算法和 Quaternion-I算法對準精度較好且相對穩定,反映出改進算法的收斂速度較傳統算法要快,其中Quaternion-I算法對準效果最優;此外,失準角范圍設置較廣,Quaternion-I算法均能夠快速完成對準,表明該算法具有較好的穩定性,車載試驗與仿真試驗結果基本一致。

圖3 第Ⅲ組失準角下姿態估計誤差

表2 5組失準角下4種算法姿態估計誤差

4 結 論

本文針對傳統SINS四元數非線性誤差模型存在的坐標系不一致問題進行了研究,提出了一種改進四元數阻尼誤差模型對準算法,并應用于系泊狀態下的SINS初始對準。通過仿真和車載試驗驗證了算法的有效性,不同大失準角條件下,改進算法在對準速度、精度以及穩定性方面都具有較好的優勢,具有一定的工程應用價值。