考慮桿臂誤差的組合導(dǎo)航分離協(xié)方差交叉算法

張大龍，余 剛，李致遠，韓剛濤

(1.鄭州大學(xué) 信息工程學(xué)院，河南 鄭州 450001；2.工業(yè)和信息化部產(chǎn)業(yè)發(fā)展促進中心，北京 100036)

0 引言

慣性導(dǎo)航(以下簡稱慣導(dǎo))系統(tǒng)(inertial navigation system，INS)具有導(dǎo)航數(shù)據(jù)更新率高、隱蔽性較好和不依賴外界設(shè)備的特點，能夠在密閉的環(huán)境中進行自主導(dǎo)航且提供比較全面的運動數(shù)據(jù)[1]。但是，由于慣性元件漂移導(dǎo)致的導(dǎo)航定位誤差會隨時間積累，慣性導(dǎo)航系統(tǒng)難以完成較長時域范圍內(nèi)的自主導(dǎo)航，需要借助外部信息進行輔助校正。北斗衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system，GNSS)具有全天候、導(dǎo)航精度高、各測量點誤差相對獨立等特點。但是，在室內(nèi)或密閉環(huán)境等衛(wèi)星信號受到遮擋的環(huán)境下，北斗系統(tǒng)無法正常使用。同時，相比于慣性導(dǎo)航系統(tǒng)，衛(wèi)星導(dǎo)航更新速率較慢，且無法提供載體的姿態(tài)信息。上述兩者在性能上優(yōu)勢互補，其組合導(dǎo)航系統(tǒng)可以具有高于單一導(dǎo)航系統(tǒng)的精度和可靠性[2]，因此北斗/慣導(dǎo)組合系統(tǒng)是一種被廣泛應(yīng)用的組合導(dǎo)航系統(tǒng)。

傳統(tǒng)組合導(dǎo)航系統(tǒng)信息融合方法通常采用卡爾曼濾波，卡爾曼濾波是在隨機線性系統(tǒng)模型建立準(zhǔn)確情況下的最小方差無偏估計[3]?？柭鼮V波進行濾波的前提是保證不同估計源之間相互獨立或者已經(jīng)知道融合數(shù)據(jù)源間數(shù)據(jù)相關(guān)的統(tǒng)計特性，以確保在特定的條件下可以達到較高的濾波精度和可靠性。由此可知，卡爾曼濾波算法在進行信息融合時并未考慮估計源間未知的相關(guān)性，這使得卡爾曼濾波在多源數(shù)據(jù)融合時不能保證融合一致性，即存在濾波過度收斂問題，導(dǎo)致理論精度與實際精度不相符。為了解決該問題，Julier等[4]提出了分離協(xié)方差交叉濾波(split covariance intersection filter, split CIF)算法，用于解決更廣泛的信息融合時存在未知相關(guān)性而不能保證一致性的問題。Li等[5]從理論上證明了split CIF在融合時間和空間相關(guān)數(shù)據(jù)時具有一致性融合估計，并且推導(dǎo)了在部分可觀測情況下的濾波算法。Li等[6]通過仿真實驗證明了split CIF進行相關(guān)信息融合時能夠保證融合一致性。Fang等[7]為了處理不同估計源之間的相關(guān)性和異常的量測值，將split CIF應(yīng)用到多車輛協(xié)同SLAM環(huán)境中，實驗結(jié)果表明了split CIF能夠保證信息融合后的濾波一致性且對于異常的量測數(shù)據(jù)具有較好的精度和魯棒性。Chen等[8]將該濾波方法應(yīng)用到無通信車輛編組的前車航跡和速度估計，仿真結(jié)果證明了該濾波方法能夠提高前車軌跡和速度的估計精度,同時保證了融合估計的一致性。本文提出的北斗/慣導(dǎo)組合導(dǎo)航系統(tǒng)是對同一個目標(biāo)的衛(wèi)星信號進行接收處理，采用2種定位方式進行測量，并且2種測量信息在信息融合時需要轉(zhuǎn)換到同一個坐標(biāo)系中。所以，2個測量結(jié)果之間存在一定的相關(guān)性，并非完全獨立。同時，衛(wèi)星觀測量偏移誤差以及觀測量之間也存在相關(guān)性[9-10]?？紤]到上述相關(guān)性問題，本文使用split CIF進行組合導(dǎo)航信息融合來保證濾波后的一致性融合估計。

通常高精度的北斗定位系統(tǒng)采用RTK定位方式，定位精度在厘米量級，然而在車載組合定位環(huán)境中，桿臂誤差通常可以達到米量級。如果桿臂誤差大于衛(wèi)星測量誤差，那么觀測信息會被嚴(yán)重削弱，最終影響組合導(dǎo)航系統(tǒng)濾波估計精度[11]。因此，高精度的北斗/慣導(dǎo)組合導(dǎo)航中考慮桿臂誤差并對其補償是十分必要的。

針對上述情況，本文提出一種考慮桿臂誤差的北斗/慣導(dǎo)組合導(dǎo)航split CIF信息融合算法。該方法建立分離形式的組合導(dǎo)航系統(tǒng)狀態(tài)方程，在此基礎(chǔ)上，通過在靜止?fàn)顟B(tài)下標(biāo)定桿臂長度并對桿臂誤差進行補償來減小導(dǎo)航定位誤差，最后，將桿臂長度作為狀態(tài)變量并實時對桿臂進行估計，實現(xiàn)對桿臂的校驗。

1 組合導(dǎo)航系統(tǒng)原理

1.1 組合導(dǎo)航耦合方式

由于松耦合結(jié)構(gòu)簡單，適用于對實時性要求較高的系統(tǒng)[12]?？紤]到車輛高動態(tài)、實時性的要求，本文車載組合導(dǎo)航系統(tǒng)采用松耦合的耦合方式，將慣導(dǎo)和北斗的位置和速度的差分作為濾波的量測向量輸入。同時，考慮桿臂誤差的影響,對慣導(dǎo)更新信息進行桿臂誤差補償，組合導(dǎo)航系統(tǒng)原理如圖1所示。

圖1 組合導(dǎo)航系統(tǒng)原理圖

本文組合系統(tǒng)采用間接法濾波，將慣性導(dǎo)航的導(dǎo)航參數(shù)誤差、慣性測量器件誤差和桿臂組合作為系統(tǒng)狀態(tài)向量[13]，通過濾波輸出的狀態(tài)誤差值對慣導(dǎo)輸出進行部分校正，最終得到精確的導(dǎo)航結(jié)果。

1.2 桿臂誤差原理

在慣導(dǎo)/北斗組合導(dǎo)航系統(tǒng)的實際應(yīng)用中，慣導(dǎo)模組(inertial measurement unit，IMU)和衛(wèi)星信號接收機天線的安裝一般不在同一位置，在安裝位置上存在距離誤差δl，該距離誤差稱之為桿臂，幾何結(jié)構(gòu)如圖2所示。

圖2 慣導(dǎo)模組與衛(wèi)星接收天線之間的桿臂

假設(shè)IMU和衛(wèi)星接收機天線到地心的矢量分別為lI和lG，兩者之間的相對矢量為δl，圖2中3個矢量之間的關(guān)系如下：

lG=lI+δl。

(1)

將所有矢量轉(zhuǎn)換到地球坐標(biāo)系(e系)下。假設(shè)桿臂δl在載體坐標(biāo)系(b系)下為常值矢量，且2個導(dǎo)航坐標(biāo)系是相互平行的，將式(1)兩邊求導(dǎo)并根據(jù)哥氏定理可得

(2)

(3)

北斗衛(wèi)星接收機與慣導(dǎo)之間的桿臂速度誤差為

(4)

(5)

(6)

北斗信號接收機與慣導(dǎo)模組之間的桿臂位置誤差為

(7)

式中:PINS=[LINS,λINS,hINS];PGNSS=[LGNSS,λGNSS,hGNSS]為衛(wèi)星接收機測量的位置；Mpv為系數(shù)矩陣。Mpv為

(8)

式中：RMh=RM+h；RNh=RN+h。其中RM、RN分別為地球子午圈主曲率半徑和卯酉圈主曲率半徑。

由于安裝位置不同而導(dǎo)致IMU和衛(wèi)星系統(tǒng)定位的信息不匹配，在車輛靜止?fàn)顟B(tài)下對桿臂長度進行標(biāo)定，根據(jù)式(5)～(7)對慣導(dǎo)更新后的位置和速度信息進行實時桿臂誤差補償，保證融合濾波器輸入量測信息的準(zhǔn)確性。

2 split CIF算法

不同于適用獨立數(shù)據(jù)源融合的卡爾曼濾波，本文所采用的split CIF考慮多源數(shù)據(jù)之間的相關(guān)性，以克服卡爾曼濾波在融合相關(guān)數(shù)據(jù)時產(chǎn)生不一致的估計結(jié)果。該算法將估計狀態(tài)的獨立性信息和相關(guān)性信息分別考慮[14]，狀態(tài)和協(xié)方差矩陣形式上可以表示為{X,Pc+Pd}。其中，X為狀態(tài)估計值；Pc為誤差協(xié)方差矩陣的獨立部分；Pd為誤差協(xié)方差矩陣的相關(guān)部分;總協(xié)方差矩陣P=Pc+Pd。split CIF算法可以分為時間狀態(tài)更新和量測更新2個部分，濾波算法[5]具體如下。

時間狀態(tài)更新：

(9)

式中：下標(biāo)c表示相關(guān)矩陣的獨立部分；k為離散時刻；Xk-1為(k-1)時刻的狀態(tài)值；Xk，k-1為k時刻的狀態(tài)預(yù)測；Pk、Pck表示k時刻的誤差協(xié)方差矩陣及其獨立部分；uk表示狀態(tài)傳感器量測；Rk和Rck分別為狀態(tài)運動模型誤差及其獨立部分；GX、Gu分別為?(·)對Xk-1和uk求偏導(dǎo)的雅可比矩陣。

量測更新：

(10)

式中:下標(biāo)d表示相關(guān)矩陣的相關(guān)部分；Xk為k時刻的狀態(tài)值；Pk、Pck、Pdk分別為k時刻的誤差協(xié)方差矩陣及其獨立部分和相關(guān)部分；Δc和Δd為量測噪聲方差陣獨立部分和相關(guān)部分，為計算方便，噪聲方差陣通常選擇對角陣；H為量測矩陣；K為系統(tǒng)最佳增益，與μ的取值有關(guān)；μ為[0，1]中的任意值，并且通過最小化包含μ的新協(xié)方差矩陣的行列式確定[15]。

3 考慮桿臂誤差的組合導(dǎo)航模型

3.1 組合導(dǎo)航系統(tǒng)狀態(tài)方程

取東北天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系(n系)，慣性坐標(biāo)系為i系。慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程和姿態(tài)誤差角方程[16]如下。

慣導(dǎo)姿態(tài)誤差角方程：

(11)

慣導(dǎo)速度誤差方程：

(12)

慣導(dǎo)位置誤差方程：

(13)

式中：[δL,δλ,δh]為慣導(dǎo)位置誤差分量；P=[L,λ,h]、vn=[vE,vN,vU]分別為慣導(dǎo)位置和速度。

陀螺的漂移和加速度計的零偏通?？梢杂秒S機常值來表示：

(14)

IMU和北斗信號接收機之間的桿臂在載體坐標(biāo)系下通常認為是常值矢量，桿臂狀態(tài)模型為

(15)

(16)

根據(jù)式(11)～(15)以及所選取的狀態(tài)向量，在量測時間為t時，組合系統(tǒng)狀態(tài)方程可以表示為

(17)

(18)

(19)

3.2 組合導(dǎo)航系統(tǒng)量測方程

組合導(dǎo)航系統(tǒng)的量測向量由北斗和慣導(dǎo)定位輸出的速度和位置的差分構(gòu)成。在慣導(dǎo)測量信息中對桿臂誤差進行補償，則量測向量表示為

(20)

量測方程表示為

Z(t)=H(t)X(t)+V(t)。

(21)

式中:Z(t)為量測向量；V(t)為北斗衛(wèi)星量測噪聲，V(t)=[Vv;Vp]，Vv為衛(wèi)星測量的速度白噪聲，Vp為衛(wèi)星測量的位置白噪聲；H(t)為量測矩陣。

(22)

(23)

4 仿真分析

本文以慣導(dǎo)和北斗組合導(dǎo)航系統(tǒng)為研究對象進行仿真驗證分析，模擬車載運動軌跡，并根據(jù)設(shè)定的傳感器誤差數(shù)據(jù)生成最終的慣性傳感器動態(tài)輸出數(shù)據(jù)[18]。北斗衛(wèi)星導(dǎo)航的測量數(shù)據(jù)由理想慣性導(dǎo)航數(shù)據(jù)加入隨機測量誤差生成。車載運動的過程具體包括靜止、加速、減速、勻速、爬坡、下坡、左右轉(zhuǎn)彎等車載動作，仿真時間為600 s，其運動軌跡如圖3所示。

圖3 車載運動軌跡仿真

對桿臂進行實時校驗來驗證本文算法對桿臂估計的有效性。對桿臂長度進行實時濾波估計，并和設(shè)置的真實值進行對比，桿臂長度各個方向的估計結(jié)果如圖4所示。

圖4 桿臂長度真實值與估計值對比

由圖4可以看出，隨著時間推移桿臂估計值最終收斂到真實值附近。X軸和Y軸方向上的桿臂估計值在300 s時都可以較好地收斂到相應(yīng)的真實值附近，而桿臂Z軸分量在450 s左右時開始收斂到真實值附近，收斂之后的各個桿臂分量的估計值如表1所示。

表1 基于split CIF的桿臂長度估計

由表1可以看出，基于split CIF的組合導(dǎo)航桿臂估計值和相應(yīng)的桿臂分量真實值比較接近，X、Y、Z軸方向上的誤差分別為1.8%、3.2%、2.7%，從而表明基于split CIF的組合系統(tǒng)能夠準(zhǔn)確地估計出桿臂長度值。

圖5為桿臂長度估計誤差協(xié)方差平方根曲線。由圖5可以看出，由于建模時考慮了估計源之間未知相關(guān)性及量測誤差偏移的影響，使得 split CIF的誤差協(xié)方差平方根值在200 s時增大，從而避免產(chǎn)生濾波過度自信和不一致的估計結(jié)果，導(dǎo)致估計結(jié)果與理論不符。結(jié)合圖4和圖5的結(jié)果可以看出，考慮桿臂誤差補償?shù)?split CIF信息融合算法能夠?qū)U臂長度進行準(zhǔn)確的估計。

圖5 桿臂誤差協(xié)方差平方根值

圖6、7分別為桿臂誤差補償前后組合系統(tǒng)東向速度誤差對比曲線圖。從圖6、7中可以看出，split CIF補償前東向速度誤差最大值為0.090 m/s，補償后東向速度誤差最大值為0.041 m/s。表2為桿臂誤差補償前后東向速度誤差，其中均方差能夠較好的表示東向速度誤差的估計精度。由表2可知，補償后速度精度比卡爾曼濾波提高了45.8%，比EKF提高了55.1%。在桿臂誤差補償前，考慮了相關(guān)性的split CIF的誤差精度比卡爾曼提高了69.0%，且split CIF相較于補償前提高了45.8%。可以看出，卡爾曼濾波和擴展卡爾曼濾波在進行桿臂誤差補償前后東向速度誤差均大于相應(yīng)的split CIF東向速度誤差，證實了考慮估計源之間相關(guān)性的split CIF比獨立假設(shè)的濾波方法誤差小，且保證了濾波融合后的一致性。

圖6 split CIF和KF補償前東向速度誤差比較

圖7 split CIF、EKF和KF補償后東向速度誤差比較

表2 桿臂誤差補償前后東向速度誤差

圖8、9為桿臂誤差補償前后緯度誤差對比曲線圖。表3為桿臂誤差補償前后緯度誤差，其中均方差反映了緯度誤差估計的精確度。由圖8、9和表3可以看出，split CIF補償前緯度誤差最大值為0.500 m，補償后為0.495 m。在補償后split CIF緯度定位精度相比于卡爾曼提高了34.0%，相比于EKF精度提高了10.0%。split CIF補償后比補償前誤差精度提高了44.2%。進行桿臂誤差補償?shù)膕plit CIF緯度誤差小于未進行桿臂誤差補償?shù)恼`差，從而證實了桿臂誤差能夠嚴(yán)重影響導(dǎo)航結(jié)果。且補償后的split CIF比卡爾曼濾波、擴展卡爾曼濾波的誤差精度高，也證實了考慮桿臂誤差補償?shù)膕plit CIF信息融合算法的有效性。

圖8 split CIF和KF補償前緯度誤差比較

圖9 split CIF、EKF和KF補償后緯度誤差比較

表3 桿臂誤差補償前后緯度誤差

5 結(jié)論

本文采用了一種適用于處理信息融合時估計源之間存在相關(guān)性問題的split CIF算法，并把該算法應(yīng)用于高精度北斗/慣導(dǎo)組合導(dǎo)航系統(tǒng)信息融合中。該方法通過分析組合導(dǎo)航系統(tǒng)的桿臂誤差的原理，建立了包含桿臂長度的split CIF算法系統(tǒng)狀態(tài)方程，并在量測方程中進行桿臂誤差補償。仿真結(jié)果表明，包含桿臂誤差補償?shù)膕plit CIF算法對桿臂估計有較好的準(zhǔn)確性，且相較于未進行桿臂誤差補償?shù)膕plit CIF算法和補償后的卡爾曼濾波算法、擴展卡爾曼濾波算法，在濾波不過度收斂的情況下提高了導(dǎo)航定位精度。在下一步研究中將進一步分析該方法和其他濾波方法的定位誤差和算法復(fù)雜度比較，并考慮兩導(dǎo)航坐標(biāo)系存在安裝偏角的情形。