車載高精度里程計輔助捷聯(lián)慣性組合導(dǎo)航系統(tǒng)設(shè)計與實驗

高福隆， 石 然， 劉 洋， 幸 偉， 張銘濤

(1.上海航天控制技術(shù)研究所，上海201109；2.上海慣性工程技術(shù)研究中心，上海201109)

0 引言

近年來,捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)在軍事與民用領(lǐng)域中的應(yīng)用越來越廣泛,但其所面臨的應(yīng)用環(huán)境也越來越復(fù)雜,對其的精度要求也越來越高[1-2]。傳統(tǒng)的高精度捷聯(lián)導(dǎo)航技術(shù)成本高,并且在復(fù)雜的工作環(huán)境下(如煤礦)難以實現(xiàn)(對采煤車)精確的導(dǎo)航定位[3]。任何單一的導(dǎo)航系統(tǒng)或簡單組合的導(dǎo)航系統(tǒng)都難以在提供高精度導(dǎo)航信息的同時具備高度的可靠性、自主性和抗干擾性[4-5]。本文提出了利用里程計(OD)輔助捷聯(lián)慣性導(dǎo)航的組合導(dǎo)航技術(shù)。SINS技術(shù)和OD技術(shù)各有優(yōu)缺點,因此可以將二者有機結(jié)合起來,以提高導(dǎo)航系統(tǒng)的整體性能[6-7]。同時,為了保證組合導(dǎo)航系統(tǒng)的實時性,本文還將FPGA、DSP進行了組合使用,這樣可以克服FPGA在處理數(shù)據(jù)時數(shù)度不足與DSP在單獨工作時時序性不強的缺點,使二者相互取長補短[8-9]。

1 組合導(dǎo)航系統(tǒng)的硬件設(shè)計

組合捷聯(lián)慣導(dǎo)系統(tǒng)的硬件設(shè)計結(jié)構(gòu)示意圖如圖1所示。硬件平臺按其功能劃分為5個模塊,分別為IMU接收模塊、硬件濾波 AD采集模塊、FPGA和DSP解算模塊、電源模塊和RS422發(fā)送數(shù)據(jù)模塊。

為保障系統(tǒng)的實時性,組合導(dǎo)航系統(tǒng)解算模塊的核心器件DSP選用了TI公司推出的高速浮點芯片TMS320C6713。同時,為滿足多個數(shù)據(jù)接口的數(shù)據(jù)傳輸需要,FPGA采用了Actel公司的現(xiàn)場可編程門陣列A3P1000-144FBGA。平臺通過模塊化的VHDL語言設(shè)計,實現(xiàn)了軟件通用化,方便了后續(xù)的設(shè)計[10]。

DSP芯片負責(zé)接收FPGA打包完成的數(shù)據(jù),完成復(fù)雜的數(shù)學(xué)運算,包括誤差補償、初始對準(zhǔn)和導(dǎo)航運算等。FPGA芯片負責(zé)完成所有外圍接口(包括陀螺、加速度計等)的高速采樣及收集I/O接口采集的指令信息,將采集數(shù)據(jù)送至DSP芯片進行處理[11]。在設(shè)計上,FPGA的內(nèi)部RAM將作為FPGA與DSP通信的中轉(zhuǎn)站,以提高系統(tǒng)的實時性和芯片的利用率[12]。

導(dǎo)航系統(tǒng)與上位機之間以Rs422串口形式進行數(shù)據(jù)交互,完成串行通信(包括初始化參數(shù)的裝載、導(dǎo)航功能的選擇和導(dǎo)航功能的輸出),數(shù)據(jù)流程圖如圖2所示。

2 SINS/OD組合濾波器設(shè)計

2.1 SINS誤差狀態(tài)方程

位置誤差、速度誤差、姿態(tài)誤差角、加速度計零偏和陀螺漂移構(gòu)成了SINS誤差狀態(tài)向量,如式(1)所示。

SINS誤差模型為:

式中,α、β、γ為姿態(tài)誤差角,A＝[AxAyAz]T表示加速度計輸出。

SINS的誤差狀態(tài)方程表達式為:

式中,WI為系統(tǒng)噪聲,其分配矩陣為GI。

2.2 OD輸出模型

將里程計測量坐標(biāo)系定義為m系,其原點為被測車輪與地面的接觸點OP,坐標(biāo)系OP－xmymzm沿車體右前上構(gòu)成右手系,僅考慮標(biāo)度因數(shù)誤差δKD和測量噪聲,里程計輸出模型為[13]:

進一步考慮光纖慣組與里程計之間的安裝桿臂誤差Lb,以及兩者之間的安裝誤差角α＝[αθαγαψ]T。根據(jù)車輛運動學(xué)約束,即側(cè)向(x)和法向(z)速度為0,則橫滾安裝角αγ不對速度vb產(chǎn)生影響,故里程計的輸出速度為:

2.3 SINS/OD組合導(dǎo)航模型

光纖慣導(dǎo)和里程計都是車載自主導(dǎo)航的首選,由于里程計的測速誤差沒有隨時間累積的趨勢,將其與光纖慣導(dǎo)組合進行航位推算,可以減緩慣導(dǎo)誤差的累積趨勢。組合導(dǎo)航模型如圖3所示。

考慮里程計輸出模型及SINS誤差狀態(tài),則SINS/OD組合導(dǎo)航的狀態(tài)向量如下[15]:

狀態(tài)方程為:

狀態(tài)轉(zhuǎn)移矩陣為:

構(gòu)造量測方程如下[14]:

改寫為矩陣形式,為:

SINS/OD車載組合導(dǎo)航采用Kalman濾波,濾波計算流程如圖4所示。

3 導(dǎo)航系統(tǒng)實驗

3.1 實驗驗證

實驗樣機由高精度光纖捷聯(lián)慣組、里程計、差分北斗衛(wèi)星定位系統(tǒng)(定位精度＜40mm)、數(shù)據(jù)采集電路和工控機組成。差分北斗衛(wèi)星定位系統(tǒng)用于對慣組跑車定位精度進行精確驗證比對。行駛路線和速度的設(shè)定以采煤機等特殊車種為參考依據(jù)。

在實驗前,靜止5min,進行慣導(dǎo)自對準(zhǔn),所使用的設(shè)備如圖5所示。隨后,載體車輛沿圖6所示的軌跡行駛,行駛時間超過60min。在實驗過程中,記錄各傳感器的原始采樣數(shù)據(jù),供后續(xù)處理和數(shù)據(jù)分析。

以差分北斗衛(wèi)星定位系統(tǒng)為基準(zhǔn),給出組合導(dǎo)航系統(tǒng)定位信息與北斗衛(wèi)星定位信息在經(jīng)度及緯度方向上的偏差情況,如圖7和圖8所示,具體數(shù)據(jù)如表1所示。

表1 結(jié)果統(tǒng)計Table 1 Results statistics

3.2 實驗結(jié)果分析

從跑車實驗結(jié)果可以看出,導(dǎo)航系統(tǒng)在速度緩慢、行駛軌跡單一、無GNSS信號且需要長時間導(dǎo)航的條件下,實現(xiàn)了總里程為323m、偏差為1.7m(即相對誤差為0.526%)、東向/北向偏差分別為1.68m/0.3m的高精度導(dǎo)航。誤差產(chǎn)生的主要原因為里程計坐標(biāo)與載體坐標(biāo)不完全一致,以及里程計自身的累計誤差(相對前者為極小量)。后續(xù),將加入里程計的坐標(biāo)標(biāo)定方法,來修正里程計與慣導(dǎo)的安裝偏角,預(yù)計修正后的仿真位移誤差可控制在±0.1%以內(nèi),與慣導(dǎo)系統(tǒng)進行組合導(dǎo)航,完全可以滿足精確定位、定向的要求。考慮到GNSS和里程計的使用環(huán)境要求,可將兩種算法組合使用,實時切換,在接收不到GNSS信號的環(huán)境中,依然可確保組合導(dǎo)航系統(tǒng)的精度和可靠性。

4 結(jié)論

本文設(shè)計了適用于特殊工作條件下的捷聯(lián)慣導(dǎo)組合導(dǎo)航系統(tǒng),通過由里程計輔助的組合導(dǎo)航算法的設(shè)計仿真及實驗對比分析,得出以下結(jié)論:

1)慣導(dǎo)系統(tǒng)采用FPGA作為外圍數(shù)據(jù)接口、采用DSP進行數(shù)據(jù)處理的通信模式,依靠FPGA自身的RAM作為中轉(zhuǎn)數(shù)據(jù)的存儲站,具有采集速度快、運算能力強、響應(yīng)指令迅速的特點。可實時給出慣導(dǎo)位置、姿態(tài)信息,保證了慣導(dǎo)系統(tǒng)的實時性及可靠性。

2)采用由里程計輔助SINS的組合導(dǎo)航算法使慣導(dǎo)系統(tǒng)實現(xiàn)了自主導(dǎo)航,系統(tǒng)在獲取了初始位置信息后能夠不依賴GNSS信號而完成長時間(＞1h)、長距離(300m)的高精度導(dǎo)航,滿足了精確定位、定向的使用要求。

3)基于OD輔助設(shè)計的組合導(dǎo)航系統(tǒng)在跑車的初始狀態(tài),特別是在GNSS信號易受到外界影響的條件下,具有更高的精度和穩(wěn)定性。