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

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

1 引言

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

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

傳統(tǒng)針對(duì)線性系統(tǒng)的Kalman 濾波[7]無法完成此類耦合情況下的標(biāo)定工作,EKF 濾波[8]多運(yùn)用于狀態(tài)量相互耦合的場(chǎng)景,但耦合的狀態(tài)量相互影響而導(dǎo)致估計(jì)精度下降。自標(biāo)定法得到的安裝誤差精度受自身慣組精確度影響,且在線條件下無法通過高精度的加速度與角速度信息進(jìn)行標(biāo)定,依賴三正交慣組進(jìn)行標(biāo)定時(shí)需要考慮非正交誤差。因子圖[9]作為一種參數(shù)優(yōu)化方法,相比于濾波法模型可調(diào)節(jié),收斂速度取決于優(yōu)化算法以及計(jì)算機(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)角誤差,將角速度積分會(huì)擴(kuò)大誤差,影響標(biāo)定結(jié)果;Lu等[13]與Cho 等[14]以三正交慣組為基礎(chǔ),對(duì)斜置軸角度進(jìn)行估計(jì),但未考慮三正交慣組是否存在非正交誤差的情況;梁晴[15]將安裝誤差用高斯馬爾可夫過程表示,并給出不同主軸下的斜置軸安裝誤差模型;Lu 等[16]利用星敏感器作為外部導(dǎo)航信息,通過12 位置試驗(yàn)方式,采用Kalman 濾波對(duì)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ù)誤差與安裝誤差的耦合模型,使用因子圖方法,選擇窗口與閾值,有效解耦并測(cè)算出標(biāo)度因數(shù)誤差與安裝誤差角,并驗(yàn)證算法的有效性。

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

2.1 安裝誤差模型

2.1.1 坐標(biāo)系定義

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

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

3)傳感器理論坐標(biāo)系(ts系):坐標(biāo)原點(diǎn)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)系三軸測(cè)得的角速度,ωsi,i=1,2,…n表示第i個(gè)斜置陀螺儀測(cè)量的角速度。兩者的轉(zhuǎn)換關(guān)系可用安裝矩陣H表示,如式(2)所示:

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

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

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

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

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

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

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

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

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

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

2.2.1 器件誤差模型

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

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

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

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

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

2.2.3 因子圖模型

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

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

當(dāng)給定測(cè)量值Z時(shí),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)合概率密度表示為一系列因式乘積的形式,乘積中的每一項(xiàng)都可以表示成一個(gè)因子,將這些因子表示成圖的形式即為因子圖。

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

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

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

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

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

實(shí)際中與所求狀態(tài)相關(guān)的代價(jià)函數(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í)刻的解算數(shù)據(jù)。

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

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

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

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

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

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

3 仿真驗(yàn)證

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

3.1 仿真條件

設(shè)定載體的運(yùn)動(dòng)方式如圖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,加速度計(jì)白噪聲大小設(shè)置為10× 10-6g/h,其中g(shù)為重力加速度,h為隨機(jī)游走系數(shù)(由白噪聲產(chǎn)生的隨機(jī)時(shí)間累積的陀螺輸出誤差系數(shù)),單位為h;IMU 頻率為100 Hz,GPS 頻率為10 Hz。

3.2 仿真分析

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

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

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

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

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

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

表3 陀螺儀在線標(biāo)定結(jié)果與誤差統(tǒng)計(jì)表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òng)窗口與反饋的方式,利用外部導(dǎo)航信息提供的速度、位置數(shù)據(jù),建立基于因子圖的標(biāo)定方法。

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

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