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

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

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

0 引言

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

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

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

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

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

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

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

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

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

SINS誤差模型為:

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

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

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

2.2 OD輸出模型

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

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

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

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

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

狀態(tài)方程為:

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

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

改寫(xiě)為矩陣形式,為:

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

3 導(dǎo)航系統(tǒng)實(shí)驗(yàn)

3.1 實(shí)驗(yàn)驗(yàn)證

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

在實(shí)驗(yàn)前,靜止5min,進(jìn)行慣導(dǎo)自對(duì)準(zhǔn),所使用的設(shè)備如圖5所示。隨后,載體車(chē)輛沿圖6所示的軌跡行駛,行駛時(shí)間超過(guò)60min。在實(shí)驗(yàn)過(guò)程中,記錄各傳感器的原始采樣數(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)計(jì)Table 1 Results statistics

3.2 實(shí)驗(yàn)結(jié)果分析

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

4 結(jié)論

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

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

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

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