基于因子圖的冗余慣組斜置軸在線標(biāo)定方法研究

胡任祎, 王秀春, 史麗楠, 賀彥峰, 陳 平

(北京航天自動控制研究所, 北京 100854)

1 引言

慣性測量單元(Inertial Measurement Unit ,IMU)具有全自主、適應(yīng)性強(qiáng)的特點,在導(dǎo)航領(lǐng)域中得到普遍應(yīng)用[1]。由于受到生產(chǎn)工藝水平、維護(hù)成本等因素限制,僅靠單一慣性元件提升導(dǎo)航系統(tǒng)的可靠性面臨投入大、周期長、見效慢等問題。而冗余型慣性測量單元(Redundant Inertial Measurement Unit, RIMU)較之于普通IMU 元件,通過冗余安裝多個慣性儀表,具有更高的可靠性和精度,可有效提升單一慣性元件的系統(tǒng)可靠性[2]。國內(nèi)外現(xiàn)役運載火箭大多采用冗余技術(shù)提高慣性系統(tǒng)的可靠性,通過設(shè)置冗余慣性元件,采用冗余管理算法對測量信息進(jìn)行重構(gòu),提升運載火箭慣性導(dǎo)航系統(tǒng)的可靠性[3]。

RIMU 系統(tǒng)采用3 個以上的非正交配置,與正交型IMU 不同的是,非正交配置的安裝誤差無法使用正交慣組地面標(biāo)定方法[4-5](12 位置標(biāo)定法)進(jìn)行標(biāo)定,且地面條件下無法完全模擬真實環(huán)境。同時,慣組的器件誤差主要包含標(biāo)度因數(shù)誤差、安裝誤差、零偏與白噪聲[6],而標(biāo)度因數(shù)誤差與安裝誤差存在耦合。

傳統(tǒng)針對線性系統(tǒng)的Kalman 濾波[7]無法完成此類耦合情況下的標(biāo)定工作,EKF 濾波[8]多運用于狀態(tài)量相互耦合的場景,但耦合的狀態(tài)量相互影響而導(dǎo)致估計精度下降。自標(biāo)定法得到的安裝誤差精度受自身慣組精確度影響,且在線條件下無法通過高精度的加速度與角速度信息進(jìn)行標(biāo)定,依賴三正交慣組進(jìn)行標(biāo)定時需要考慮非正交誤差。因子圖[9]作為一種參數(shù)優(yōu)化方法,相比于濾波法模型可調(diào)節(jié),收斂速度取決于優(yōu)化算法以及計算機(jī)的算力[10]。Cheng 等[11]構(gòu)建了線性系統(tǒng)誤差模型,采用自標(biāo)定形式,利用Kalman 濾波方法進(jìn)行標(biāo)定。自標(biāo)定精度受到自身慣組精確度影響,導(dǎo)致安裝誤差角標(biāo)定結(jié)果較差;Cheng等[12]也提出離線條件下的斜置軸標(biāo)定,提升了標(biāo)定精度,但離線條件下只能得到高精度加速度與角速度數(shù)據(jù),而陀螺儀安裝誤差表現(xiàn)為姿態(tài)角誤差,將角速度積分會擴(kuò)大誤差,影響標(biāo)定結(jié)果;Lu等[13]與Cho 等[14]以三正交慣組為基礎(chǔ),對斜置軸角度進(jìn)行估計,但未考慮三正交慣組是否存在非正交誤差的情況;梁晴[15]將安裝誤差用高斯馬爾可夫過程表示,并給出不同主軸下的斜置軸安裝誤差模型;Lu 等[16]利用星敏感器作為外部導(dǎo)航信息,通過12 位置試驗方式,采用Kalman 濾波對IMU 的標(biāo)度因數(shù)誤差、安裝誤差、零偏以及星敏感器安裝誤差進(jìn)行標(biāo)定。

本文提出一種在線條件下冗余慣組斜置軸的標(biāo)度因數(shù)誤差與安裝誤差的因子圖標(biāo)定方法。新的安裝誤差模型不依賴三正交慣組,能夠標(biāo)定冗余慣組斜置軸的標(biāo)度因數(shù)誤差及安裝誤差。采用GPS 作為外部導(dǎo)航信息,構(gòu)建冗余慣組斜置軸的標(biāo)度因數(shù)誤差與安裝誤差的耦合模型,使用因子圖方法,選擇窗口與閾值,有效解耦并測算出標(biāo)度因數(shù)誤差與安裝誤差角,并驗證算法的有效性。

2 數(shù)學(xué)模型

2.1 安裝誤差模型

2.1.1 坐標(biāo)系定義

1)載體坐標(biāo)系(b系):坐標(biāo)系原點o與載體重心重合,xb為載體俯仰軸,指向載體右側(cè)為正;yb為載體滾轉(zhuǎn)軸,指向載體前側(cè)為正;zb為載體偏航軸,指向載體上側(cè)為正。

2)傳感器真實坐標(biāo)系(s系):坐標(biāo)原點o位于傳感器中心,zs為傳感器敏感軸,此軸表示傳感器測量值的方向,xs表示傳感器旋轉(zhuǎn)軸,其與ozb以及ozts共面,ys與xs,zs兩軸互相正交,其滿足右手定則。

3)傳感器理論坐標(biāo)系(ts系):坐標(biāo)原點o位于傳感器中心,與s系區(qū)別在于,ts系不存在安裝誤差,是理論模型。

三坐標(biāo)系的示意圖如圖1 所示。

圖1 三坐標(biāo)系示意圖Fig.1 Diagram of three coordinate systems

2.1.2 冗余慣組安裝誤差模型

斜置型慣組如圖2 所示,斜置型慣組在體坐標(biāo)系中的位置可用α,β表示,其中α為傳感器敏感軸到xboyb平面與xb軸的夾角,β為傳感器敏感軸到zb軸的夾角。

圖2 斜置型慣組示意圖Fig.2 Diagram of redundant configuration

因此,以斜置陀螺儀為例,RIMU 到體坐標(biāo)系的轉(zhuǎn)換關(guān)系可用式(1)表示:

其中,ωxb,ωyb,ωzb表示體坐標(biāo)系三軸測得的角速度,ωsi,i=1,2,…n表示第i個斜置陀螺儀測量的角速度。兩者的轉(zhuǎn)換關(guān)系可用安裝矩陣H表示,如式(2)所示:

其中,ωs為冗余慣組角速度測量值,ωb為體坐標(biāo)系三軸角速度。在保證基本導(dǎo)航功能的條件下,H為列滿秩矩陣,因此其廣義逆矩陣存在,如式(4)所示:

安裝誤差是由安裝時的不完美造成慣性器件敏感軸與理論位置不一致所導(dǎo)致。由于安裝誤差的存在,傳感器的真實坐標(biāo)系與理論坐標(biāo)系不重合,因此可通過坐標(biāo)系旋轉(zhuǎn)的方法對準(zhǔn)兩坐標(biāo)系,旋轉(zhuǎn)的角度可作為衡量安裝誤差的具體參數(shù)。

根據(jù)圖1 所示,先以ys軸為旋轉(zhuǎn)軸,轉(zhuǎn)動θy角度,令平面ysozs與ytsozts重合,再以xs為旋轉(zhuǎn)軸,轉(zhuǎn)動θx角度,令兩坐標(biāo)系重合。因此可通過計算θx與θy數(shù)值,進(jìn)而標(biāo)定安裝誤差。由于θx與θy為小量,因此可近似得到式(5):

在傳感器坐標(biāo)系中,陀螺儀的測量值可表示為式(6):

結(jié)合式(6),建立安裝誤差模型如式(7)所示:

其中,ωzs為傳感器理論坐標(biāo)系z軸測量值,ωxs為傳感器理論坐標(biāo)系x軸測量值,ωys為傳感器理論坐標(biāo)系y軸測量值。

已知載體坐標(biāo)系與傳感器坐標(biāo)系的轉(zhuǎn)換關(guān)系如式(8)所示:

式中,ω-s為斜置慣組在傳感器真實坐標(biāo)系下的測量值,將其投影到載體坐標(biāo)系下,如式(10)所示:

式中,C1,C2,C3分別為xs軸、ys軸和zs軸到b系三軸的投影矩陣。

2.2 數(shù)學(xué)模型

2.2.1 器件誤差模型

考慮標(biāo)度因數(shù)誤差與安裝誤差,結(jié)合式(10)安裝誤差的建模形式,存在式(11):

式中,fb為加速度計測量的比力,vn為導(dǎo)航坐標(biāo)系下的載體速度,本文導(dǎo)航坐標(biāo)系采用東北天坐標(biāo)系表示法。

分別對捷聯(lián)慣導(dǎo)位置(緯度、經(jīng)度、高度)微分方程式求偏差,可得位置誤差方程,如式(16)所示:

由于標(biāo)度因數(shù)誤差與安裝誤差存在耦合關(guān)系,因此需對狀態(tài)方程求Jacobian 矩陣。由式(14)～(16)設(shè)計狀態(tài)方程,如式(17)所示:

F具體形式見式(18)～(28),其中×為向量的反對稱矩陣。

2.2.3 因子圖模型

因子圖方法主要應(yīng)用于數(shù)據(jù)融合、參數(shù)優(yōu)化,并將其轉(zhuǎn)化為狀態(tài)估計問題,根據(jù)觀測到的信息對當(dāng)前狀態(tài)進(jìn)行推斷。由于測量具有不確定性,雖然無法準(zhǔn)確地反應(yīng)所求變量的真實狀態(tài),但利用測量可推斷出真實狀態(tài)的概率。因此,狀態(tài)估計可用條件概率密度表示p(X|Z) ,其中,X為待估計的狀態(tài),Z為傳感器的測量值。當(dāng)條件概率值達(dá)到最大時,X-即為X的估計值。對狀態(tài)X的估計,可采用最大后驗估計的方法,如式(29)所示:

對式(29)應(yīng)用貝葉斯公式,可將后驗概率密度轉(zhuǎn)化為狀態(tài)變量的先驗概率密度p(X) 和該狀態(tài)下測量值的概率密度p(Z|X) 的乘積,并通過觀測值的概率密度歸一化,得式(30):

當(dāng)給定測量值Z時,p(Z) 為已知量,式(30)可進(jìn)一步轉(zhuǎn)化為式(31):

l(X;Z) ∝p(Z|X) 為關(guān)于狀態(tài)X的似然函數(shù)。因此式(31)可展開為式(32):

式(31)將聯(lián)合概率密度表示為一系列因式乘積的形式,乘積中的每一項都可以表示成一個因子,將這些因子表示成圖的形式即為因子圖。

因子圖是一種基于馬爾科夫鏈的圖模型,在形式上可記為G= (F,X) , 變量節(jié)點記為xj∈X,表示待估計的狀態(tài)。因子節(jié)點記為fi∈F,表示變量的后驗概率密度。

因子節(jié)點只與其測量模型中出現(xiàn)的狀態(tài)變量節(jié)點連接。當(dāng)一個新的傳感器測量加入因子圖中,只需建立該傳感器測量值的因子模型并與相關(guān)的變量節(jié)點建立連接。

如圖3 所示,變量節(jié)點為帶估計的器件誤差,由高頻率的IMU 因子相連接,每個變量節(jié)點均有外部數(shù)據(jù)因子進(jìn)行約束。

圖3 器件誤差標(biāo)定的因子圖Fig.3 Factor graph system of calibration

因子圖G的數(shù)學(xué)模型f(X) 是構(gòu)成因子圖的因子的乘積,如式(33)所示:

實際中與所求狀態(tài)相關(guān)的代價函數(shù)通常為非線性,因此常通過非線性優(yōu)化方法求解。

2.2.4 誤差因子

根據(jù)外部導(dǎo)航信息提供的速度、位置數(shù)據(jù)與慣組自身解算所得數(shù)據(jù)構(gòu)建殘差,以建立外部數(shù)據(jù)因子模型,如式(37)所示:

其中zi為衛(wèi)星導(dǎo)航數(shù)據(jù),hGPS(Xi,ui) 為帶有器件誤差的慣導(dǎo)在當(dāng)前時刻的解算數(shù)據(jù)。

根據(jù)建立的狀態(tài)方程,狀態(tài)量自身對下一時刻存在一步估計,將其作為慣性因子節(jié)點,如式(38)所示:

因此在線標(biāo)定問題可轉(zhuǎn)化為優(yōu)化問題,如式(40)所示:

因子圖的在線估計流程如圖4 所示,首先構(gòu)造因子圖模型,變量節(jié)點設(shè)置為包含有姿態(tài)、速度、位置誤差及器件誤差的狀態(tài)量,根據(jù)外部數(shù)據(jù)因子與慣性因子構(gòu)建代價函數(shù),利用L-M 算法對待估計變量進(jìn)行優(yōu)化補(bǔ)償。

圖4 因子圖在線估計流程Fig.4 Procedure of calibration w ith factor graph method

與濾波法不用,因子圖法無法做到每幀更新,原因為單幀的殘差信息不足以優(yōu)化帶有3 個器件誤差參數(shù)的非線性方程,因此需要構(gòu)建滑動窗口。窗口長度包含的數(shù)據(jù)量能夠?qū)崿F(xiàn)對器件誤差參數(shù)的單次優(yōu)化,由于待標(biāo)定的參數(shù)共有6 個,因此窗口長度所包含的衛(wèi)星導(dǎo)航數(shù)據(jù)不得少于6 組。同時設(shè)置反饋回路,當(dāng)前窗口完成一次優(yōu)化后,將單次器件誤差優(yōu)化結(jié)果補(bǔ)償至原始慣導(dǎo)解算數(shù)據(jù)中,補(bǔ)償標(biāo)度因數(shù)誤差與安裝誤差,下一次優(yōu)化將針對被補(bǔ)償后的慣導(dǎo)解算數(shù)據(jù),做到逐次優(yōu)化,直至結(jié)果收斂。算法具體流程圖如圖5 所示。

圖5 因子圖方法反饋流程示意圖Fig.5 Procedure of feedback w ith factor graph method

3 仿真驗證

本文采用MATLAB 2019b 軟件仿真,利用因子圖方法,針對冗余慣組斜置軸的器件誤差進(jìn)行標(biāo)定。

3.1 仿真條件

設(shè)定載體的運動方式如圖6 所示,ωx,ωy,ωz為三軸角速度,ax,ay,az為三軸加速度。

圖6 角速度與加速度示意圖Fig.6 Diagram of angular velocity and acceleration

考慮標(biāo)度因數(shù)誤差、安裝誤差與白噪聲,設(shè)置慣組斜置角分別為α= 45°,β= 30° ;標(biāo)度因數(shù)誤差S=5×10-4;安裝誤差設(shè)置為30″;陀螺白噪聲大小設(shè)置為0.001°/h,加速度計白噪聲大小設(shè)置為10× 10-6g/h,其中g(shù)為重力加速度,h為隨機(jī)游走系數(shù)(由白噪聲產(chǎn)生的隨機(jī)時間累積的陀螺輸出誤差系數(shù)),單位為h;IMU 頻率為100 Hz,GPS 頻率為10 Hz。

3.2 仿真分析

為測試驗證因子圖方法在不同標(biāo)度因數(shù)與安裝誤差條件下的標(biāo)定性能,設(shè)置多組不同的誤差系數(shù),表1 為待標(biāo)定的誤差表。

表1 在線標(biāo)定情況下誤差系數(shù)表Table 1 Error index under on-line calibration condition

測量出GPS 與IMU 的速度、位置差值,分別采用EKF 和因子圖算法進(jìn)行仿真分析并比對。

表2 為2 種算法的標(biāo)定精度對比。仿真結(jié)果表明,因子圖算法能夠在給定的仿真條件下標(biāo)定出標(biāo)度因數(shù)誤差與安裝誤差,誤差控制在10%以內(nèi),具有良好的標(biāo)定效果。

表2 標(biāo)定結(jié)果對比表Table 2 Comparison of calibration results %

根據(jù)建立的模型與條件進(jìn)行仿真,陀螺儀與加速度計的標(biāo)定誤差如表3 所示,圖7 為仿真結(jié)果示意圖。仿真結(jié)果表明,針對不同的誤差系數(shù),因子圖方法均能良好標(biāo)定,誤差在12%以內(nèi)。說明其具有穩(wěn)定的標(biāo)定性能。

表3 陀螺儀在線標(biāo)定結(jié)果與誤差統(tǒng)計表Table 3 Calibration results and errors of gyro under on-line condition %

圖7 標(biāo)定結(jié)果示意圖Fig.7 Diagram of calibration results

4 結(jié)論

本文建立不依賴正交系統(tǒng)的斜置型慣組安裝誤差模型,通過設(shè)定滑動窗口與反饋的方式,利用外部導(dǎo)航信息提供的速度、位置數(shù)據(jù),建立基于因子圖的標(biāo)定方法。

1) 由于慣組標(biāo)定存在天地不一致問題,即慣組在地面的標(biāo)定結(jié)果與飛行過程中的標(biāo)定結(jié)果存在偏差,且地面靜態(tài)實驗無法準(zhǔn)確激勵出器件誤差參數(shù),提出在線標(biāo)定的方法,在飛行過程中利用GPS 作為外部導(dǎo)航信息進(jìn)行標(biāo)定以解決天地不一致的問題。

2) 針對EKF 濾波本身無法完全解耦的問題,本文提出了基于因子圖的標(biāo)定方法,設(shè)置滑動窗口與反饋,選擇合適的窗口與反饋,對原始數(shù)據(jù)進(jìn)行補(bǔ)償,排除了零偏對安裝誤差標(biāo)定的影響,并降低了由于標(biāo)度因數(shù)誤差與安裝誤差的耦合關(guān)系而造成的影響,對標(biāo)度因數(shù)誤差與安裝誤差進(jìn)行逐次逼近估計,提高兩者的標(biāo)定精度。仿真結(jié)果表明,在不同誤差系數(shù)下的標(biāo)定誤差均控制在12%以內(nèi)。