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

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

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

1 引言

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

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

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

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

2 數學模型

2.1 安裝誤差模型

2.1.1 坐標系定義

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

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

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

三坐標系的示意圖如圖1 所示。

圖1 三坐標系示意圖Fig.1 Diagram of three coordinate systems

2.1.2 冗余慣組安裝誤差模型

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

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

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

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

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

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

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

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

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

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

已知載體坐標系與傳感器坐標系的轉換關系如式(8)所示:

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

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

2.2 數學模型

2.2.1 器件誤差模型

考慮標度因數誤差與安裝誤差,結合式(10)安裝誤差的建模形式,存在式(11):

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

分別對捷聯慣導位置(緯度、經度、高度)微分方程式求偏差,可得位置誤差方程,如式(16)所示:

由于標度因數誤差與安裝誤差存在耦合關系,因此需對狀態方程求Jacobian 矩陣。由式(14)～(16)設計狀態方程,如式(17)所示:

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

2.2.3 因子圖模型

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

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

當給定測量值Z時,p(Z) 為已知量,式(30)可進一步轉化為式(31):

l(X;Z) ∝p(Z|X) 為關于狀態X的似然函數。因此式(31)可展開為式(32):

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

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

因子節點只與其測量模型中出現的狀態變量節點連接。當一個新的傳感器測量加入因子圖中,只需建立該傳感器測量值的因子模型并與相關的變量節點建立連接。

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

圖3 器件誤差標定的因子圖Fig.3 Factor graph system of calibration

因子圖G的數學模型f(X) 是構成因子圖的因子的乘積,如式(33)所示:

實際中與所求狀態相關的代價函數通常為非線性,因此常通過非線性優化方法求解。

2.2.4 誤差因子

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

其中zi為衛星導航數據,hGPS(Xi,ui) 為帶有器件誤差的慣導在當前時刻的解算數據。

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

因此在線標定問題可轉化為優化問題,如式(40)所示:

因子圖的在線估計流程如圖4 所示,首先構造因子圖模型,變量節點設置為包含有姿態、速度、位置誤差及器件誤差的狀態量,根據外部數據因子與慣性因子構建代價函數,利用L-M 算法對待估計變量進行優化補償。

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

與濾波法不用,因子圖法無法做到每幀更新,原因為單幀的殘差信息不足以優化帶有3 個器件誤差參數的非線性方程,因此需要構建滑動窗口。窗口長度包含的數據量能夠實現對器件誤差參數的單次優化,由于待標定的參數共有6 個,因此窗口長度所包含的衛星導航數據不得少于6 組。同時設置反饋回路,當前窗口完成一次優化后,將單次器件誤差優化結果補償至原始慣導解算數據中,補償標度因數誤差與安裝誤差,下一次優化將針對被補償后的慣導解算數據,做到逐次優化,直至結果收斂。算法具體流程圖如圖5 所示。

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

3 仿真驗證

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

3.1 仿真條件

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

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

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

3.2 仿真分析

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

表1 在線標定情況下誤差系數表Table 1 Error index under on-line calibration condition

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

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

表2 標定結果對比表Table 2 Comparison of calibration results %

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

表3 陀螺儀在線標定結果與誤差統計表Table 3 Calibration results and errors of gyro under on-line condition %

圖7 標定結果示意圖Fig.7 Diagram of calibration results

4 結論

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

1) 由于慣組標定存在天地不一致問題,即慣組在地面的標定結果與飛行過程中的標定結果存在偏差,且地面靜態實驗無法準確激勵出器件誤差參數,提出在線標定的方法,在飛行過程中利用GPS 作為外部導航信息進行標定以解決天地不一致的問題。

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