楊菊花 張琳婧 陳光武 程鑒皓 李 鵬

①(蘭州交通大學(xué)自動(dòng)控制研究所 蘭州 730070)

②(甘肅省高原交通信息工程及控制重點(diǎn)實(shí)驗(yàn)室 蘭州 730070)

③(蘭州交通大學(xué)交通運(yùn)輸學(xué)院 蘭州 730070)

1 引言

隨著微機(jī)電系統(tǒng)(Micro Electro Mechanical Systems, MEMS)的發(fā)展，基于MEMS的慣性測(cè)量單元(Inertial Measurement Unit, IMU)因?yàn)榫哂械统杀?、自主性?qiáng)的優(yōu)勢(shì)而在智能交通領(lǐng)域中被廣泛應(yīng)用。由于MEMS-IMU逐次啟動(dòng)的零偏范圍變化較大[1]，在使用前對(duì)系統(tǒng)進(jìn)行對(duì)準(zhǔn)是必要的環(huán)節(jié)。靜態(tài)對(duì)準(zhǔn)對(duì)環(huán)境要求比較高，行進(jìn)間對(duì)準(zhǔn)可有效提高載體的機(jī)動(dòng)能力，但需要外部設(shè)備提供輔助信息。外部輔助有諸如磁強(qiáng)計(jì)、單天線、多天線等設(shè)備，或者機(jī)動(dòng)輔助等運(yùn)行方式，都有其適用環(huán)境與限制。全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)與IMU的組合算法已經(jīng)相對(duì)成熟[2–5]，但存在兩點(diǎn)不足，其一低精度SINS誤差累積較大，其二運(yùn)載體姿態(tài)的觀測(cè)能力不強(qiáng)，特別是航向角的可觀性較弱?；诖藘牲c(diǎn)，本文對(duì)方位誤差進(jìn)行較全面的分析，結(jié)合卡爾曼濾波，設(shè)計(jì)了一種兩段連續(xù)式對(duì)準(zhǔn)方法，使用雙天線設(shè)備，輔助低精度IMU完成導(dǎo)航前一刻的自主對(duì)準(zhǔn)，抑制了航向的發(fā)散，增加了系統(tǒng)的可靠性。

針對(duì)車載，由于其橫滾旋轉(zhuǎn)幅度較小，所以先不考慮撓曲變形。在GNSS測(cè)向系統(tǒng)正常工作的前提下，單幅天線無法測(cè)得運(yùn)載體靜態(tài)姿態(tài)，雙幅天線無論運(yùn)載體靜態(tài)或行進(jìn)狀態(tài)都可以同時(shí)測(cè)得運(yùn)載體航向角與俯仰角，3幅天線則可同時(shí)測(cè)得運(yùn)載體3個(gè)姿態(tài)角。理論上，天線數(shù)量越多，布設(shè)越均勻，解算精度則越高，但基于成本和精度需要的實(shí)際考量，本文的GNSS基線測(cè)向系統(tǒng)為雙天線布局。由于雙天線相較單天線可靜態(tài)定向，較里程計(jì)測(cè)速精度高，較磁強(qiáng)計(jì)受干擾范圍較少，所以利用雙天線設(shè)備輔助IMU完成對(duì)準(zhǔn)。針對(duì)GNSS測(cè)向系統(tǒng)，文獻(xiàn)[6]通過GNSS測(cè)向獲得航向角，實(shí)現(xiàn)與低精度SINS初始對(duì)準(zhǔn)的互輔解算。文獻(xiàn)[7]探討了兩種多天線定姿方法，文獻(xiàn)[8]設(shè)計(jì)了一種短基線旋轉(zhuǎn)定向方法。針對(duì)自主對(duì)準(zhǔn)，文獻(xiàn)[9]研究了一種雙天線與低成本IMU組合的姿態(tài)確定方法，文獻(xiàn)[10]研究了飛行載體的對(duì)準(zhǔn)，文獻(xiàn)[11]使用GNSS天線、MEMS陀螺與磁力計(jì)組合實(shí)現(xiàn)姿態(tài)確定。目前文獻(xiàn)研究較多地集中在靜態(tài)或者行進(jìn)間對(duì)準(zhǔn)的單個(gè)環(huán)節(jié)，本文使用雙天線布局，針對(duì)低精度IMU/GNSS的車載組合導(dǎo)航系統(tǒng)，在文獻(xiàn)[7]的基礎(chǔ)上，推導(dǎo)了基于雙天線基線矢量的最小二乘定姿模型，其次給出了一種包含姿態(tài)量測(cè)的卡爾曼濾波器，最后實(shí)現(xiàn)了車載的兩段連續(xù)對(duì)準(zhǔn)，并在此基礎(chǔ)上進(jìn)行了車載的靜態(tài)對(duì)準(zhǔn)測(cè)試與行進(jìn)間的組合導(dǎo)航測(cè)試。

2 方位誤差分析

本文采用當(dāng)?shù)氐乩碜鴺?biāo)系為參考坐標(biāo)系，坐標(biāo)系轉(zhuǎn)換如文獻(xiàn)[12]。將初始對(duì)準(zhǔn)定義為運(yùn)載體坐標(biāo)系向?qū)Ш接?jì)算機(jī)坐標(biāo)系旋轉(zhuǎn)的過程[1]。初始方位誤差存在的原因是計(jì)算空間與真實(shí)空間不一致，即計(jì)算所用的導(dǎo)航坐標(biāo)系與真實(shí)導(dǎo)航坐標(biāo)系存在偏差量φ，稱其為失準(zhǔn)角誤差。

本文將初始對(duì)準(zhǔn)問題轉(zhuǎn)換為最優(yōu)初始姿態(tài)的求取，使用雙天線設(shè)備與最小二乘算法，補(bǔ)償姿態(tài)誤差量得到最優(yōu)初始姿態(tài)。在得到估計(jì)失準(zhǔn)角偏差量后，進(jìn)行方位角的補(bǔ)償，如式(1)，其中 φb為計(jì)算得到的方位角， φ為真實(shí)方位角， ?表示誤差量，失準(zhǔn)角微分方程如式(2)

其中， L為當(dāng)?shù)氐乩砭暥龋琜 ωx,ωy,ωz]為陀螺隨機(jī)游走，ωie為地球自轉(zhuǎn)角速度矢量，[ εx,εy,εz]為陀螺常值漂移，[ φx,φy,φz]為3個(gè)失準(zhǔn)角。將方位角的誤差分為失準(zhǔn)角誤差與姿態(tài)測(cè)量誤差兩部分，如式(3)

其中，[ φ,θ,γ]分別為航向角、俯仰角、橫滾角。

3 方位對(duì)準(zhǔn)

3.1 靜態(tài)對(duì)準(zhǔn)

利用雙天線的觀測(cè)數(shù)據(jù)，即基線矢量在當(dāng)?shù)氐乩碜鴺?biāo)系中的分量，可得運(yùn)載體航向角與俯仰角，如式(4)

其中，A, B表示兩個(gè)天線，天線安裝位置使得其基線方向(主天線的相位中心指向從天線的相位中心)與載體坐標(biāo)系y 軸需要保持一致。對(duì)式(4)進(jìn)行微分并忽略坐標(biāo)分量之間的相關(guān)性，得到航向角誤差與俯仰角誤差如式(5)和式(6)

其中， SAB為 基線長(zhǎng)度，σ 表示誤差估計(jì)值，可由GNSS定位模型得到。靜態(tài)對(duì)準(zhǔn)時(shí)，低精度IMU陀螺儀無法敏感地測(cè)得地球自轉(zhuǎn)角速率，使用加速度計(jì)實(shí)現(xiàn)運(yùn)載體的水平方位校準(zhǔn)，如式(7)

其中，f 為加速度計(jì)在載體坐標(biāo)系b系下的水平分量輸出，利用主、從天線之間形成的基線，經(jīng)過旋轉(zhuǎn)矩陣轉(zhuǎn)換可得到

作為教師，我們希望能夠?qū)⑸鷦?dòng)的課堂帶給學(xué)生，讓課堂不再是傳統(tǒng)意義上的課堂，而是將快樂帶入課堂，孩子們能夠在這樣的課堂上對(duì)語(yǔ)文產(chǎn)生極大的興趣，并且進(jìn)而能夠主動(dòng)的學(xué)習(xí)語(yǔ)文知識(shí)。而要想做到這樣的教育，就需要通過教師需要在精心的備課設(shè)計(jì)下，實(shí)施情景教學(xué)。

其中， bi,Si分別為在b 系和n 系下的第i 條基線矢量，建立誤差方程模型如式(9)—式(11)

其中， A 為系數(shù)矩陣；待估狀態(tài)量X =[δy,δp,δr]T為姿態(tài)誤差；L為第i條基線矢量的觀測(cè)值，即在載體系下的基線矢量差值；(0)為由式(4)、式(7)的姿態(tài)初值轉(zhuǎn)換而來的旋轉(zhuǎn)矩陣；最小二乘遞推公式如式(12)和式(13)

其中，P為權(quán)矩陣，Qs與Qb分 別為S 和b 的協(xié)方差陣。

3.2 行進(jìn)間對(duì)準(zhǔn)

針對(duì)航向誤差難以抑制的問題，本文在載體系下推導(dǎo)基于卡爾曼濾波的車載行進(jìn)間濾波模型，首先將GNSS雙天線安裝至基線方向平行于載體系的Y軸，即可利用GNSS測(cè)向結(jié)果輔助IMU完成組合導(dǎo)航。理論上，一個(gè)運(yùn)載體由IMU與雙天線測(cè)得的航向角是相同的，但由于IMU漂移較大，導(dǎo)致IMU計(jì)算得到的航向角與雙天線測(cè)得的航向角不一致。

3.2.1 狀態(tài)方程

對(duì)于低精度IMU，(1)忽略由于位置變化引起的重力加速度誤差，(2)忽略地球自轉(zhuǎn)引起的導(dǎo)航系旋轉(zhuǎn)誤差，得到簡(jiǎn)化后誤差微分方程，如式(14)

其中， f 為測(cè)量加速度計(jì)輸出比力值，? 為加速度計(jì)3常值零偏。基于式(15)與式(16)，選取失準(zhǔn)角誤差、3個(gè)速度誤差、3個(gè)陀螺零偏與3個(gè)加速度計(jì)零偏作為系統(tǒng)狀態(tài)量，如式(17)

建立行進(jìn)間對(duì)準(zhǔn)狀態(tài)空間模型如式(18)

3.2.2 量測(cè)方程

低精度MEMS陀螺儀信號(hào)在積分過程中計(jì)算出的航向角與真實(shí)的航向角會(huì)產(chǎn)生不可忽略的誤差。所以在IMU解算速度與雙天線給出的速度之差作為系統(tǒng)觀測(cè)量的基礎(chǔ)上，針對(duì)航向角可觀性比較差的問題，引入雙天線基線航向角，擴(kuò)充以IMU解算航向角與雙天線基線航向角之差的1維量測(cè)。由于GNSS輸出頻率為2 Hz，通過與IMU結(jié)合，其一可解決可觀性差的問題，其二可以在較高動(dòng)態(tài)下輸出較精確的航向值。由式(3)，取航向誤差關(guān)系式，得到觀測(cè)噪聲矩陣[14]，如式(22)和式(23)，得系統(tǒng)量測(cè)方程如式(24)

圖1 對(duì)準(zhǔn)流程框圖

上文所討論的一種雙天線輔助低精度IMU的方位對(duì)準(zhǔn)方法包含靜態(tài)的初始對(duì)準(zhǔn)與行進(jìn)間導(dǎo)航前一刻的自主對(duì)準(zhǔn)。圖1為雙天線GNSS輔助低精度IMU進(jìn)行兩段連續(xù)式對(duì)準(zhǔn)的流程框圖，其中，采用閾值法判別航向差值可用與否，滿足則進(jìn)行時(shí)間與量測(cè)更新，否則只進(jìn)行時(shí)間更新。在兩段連續(xù)對(duì)準(zhǔn)中針對(duì)方位角參數(shù)進(jìn)行補(bǔ)償，靜態(tài)狀態(tài)下補(bǔ)償初始方位角誤差，行進(jìn)間對(duì)準(zhǔn)抑制航向發(fā)散并補(bǔ)償失準(zhǔn)角誤差。兩段連續(xù)對(duì)準(zhǔn)的一次離散處理算法如圖2所示，圖中實(shí)線為靜態(tài)對(duì)準(zhǔn)的算法處理路線，虛線為行進(jìn)間對(duì)準(zhǔn)的算法處理路線。首先使得運(yùn)載體保持一段時(shí)間的靜止，利用雙天線設(shè)備和最小二乘濾波估計(jì)得到較為精確的初始姿態(tài)角；其次當(dāng)載體處于行進(jìn)中時(shí)，利用航向差值擴(kuò)充卡爾曼濾波器的觀測(cè)量；最后進(jìn)行狀態(tài)量的反饋，完成兩段連續(xù)對(duì)準(zhǔn)。綜上，本文利用雙天線輔助低精度IMU對(duì)準(zhǔn)，減小了初始對(duì)準(zhǔn)的偏差，抑制了行進(jìn)間對(duì)準(zhǔn)的航向角發(fā)散。

4 試驗(yàn)

4.1 仿真試驗(yàn)

在本節(jié)的仿真試驗(yàn)中，首先設(shè)置仿真條件，運(yùn)載體的總對(duì)準(zhǔn)仿真時(shí)間為3000 s，初始方位誤差為0°, 5°, 10°，結(jié)果顯示航向角在100 s之內(nèi)可收斂。表1列出了方位角在不同慣性器件誤差與初始方位誤差情況下的收斂值，可得到，陀螺儀的精度直接影響方位角收斂值。其次，設(shè)置MEMS陀螺東向常值漂移0.007°/s，加速度計(jì)常值零偏為200 mg的低精度IMU器件誤差，以0°, 5°與10°初始方位誤差仿真航向角作圖3，圖4顯示了航向角的誤差圖?？傻?，初始方位誤差在5°之內(nèi)其航向的振蕩范圍有明顯變化，在5°之上，其振蕩范圍變化不甚明顯，航向誤差基本保持在±5°之內(nèi)?？傻媒Y(jié)論：低精度IMU的初始對(duì)準(zhǔn)，初始方位誤差應(yīng)盡可能地小，以便使得航向的振蕩范圍越小，能更快地開始收斂，更有利于信息融合時(shí)狀態(tài)量的準(zhǔn)確估計(jì)。

圖2 對(duì)準(zhǔn)算法框圖

4.2 車載試驗(yàn)

4.2.1 靜態(tài)對(duì)準(zhǔn)

在真實(shí)的物理環(huán)境下，不僅存在慣性器件誤差，而且噪聲與振動(dòng)的影響會(huì)更大，航向的誤差與收斂時(shí)間都會(huì)不確定性地增加。雙天線輔助低精度IMU的組合導(dǎo)航系統(tǒng)主要由2個(gè)GPS天線、1塊GPS的OEM板卡、1個(gè)IMU模塊、采集單元、解算單元、電源模塊組成。其中IMU精度為陀螺儀漂移0.005?/s，加速度計(jì)零偏200 μg，采樣率100 Hz，采用高精度OEM7系列NovAtel接收機(jī)數(shù)據(jù)作為真值，采樣率20 Hz，進(jìn)行試驗(yàn)比對(duì)。設(shè)定基線長(zhǎng)度，將雙天線設(shè)備固定在車載設(shè)備上，系統(tǒng)上電，在GPS雙天線都能收到6顆以上的衛(wèi)星信號(hào)時(shí)，開始采集數(shù)據(jù)。首先進(jìn)行3 min的解析粗對(duì)準(zhǔn)，驗(yàn)證改進(jìn)方法的兩個(gè)效果，其一為方位精度，其二為方位波動(dòng)范圍，前者用真值的平均值作參考，后者用與真值比對(duì)的標(biāo)準(zhǔn)偏差做參考。

將基線劃分為兩個(gè)范圍，并間隔0.2 m設(shè)置7組基線值，短基線范圍為0.3～1 m，長(zhǎng)度依次為0.3 m, 0.5 m, 0.7 m, 0.9 m。中長(zhǎng)基線范圍1～2 m，長(zhǎng)度依次為1.1 m, 1.3 m, 1.5 m。作表2進(jìn)行不同基線之間的航向、俯仰的誤差與標(biāo)準(zhǔn)偏差的對(duì)比，可得，0.7 m的基線與1.5 m的基線解算效果經(jīng)度較高。短基線的航向誤差在0.2 m/0.7°左右，航向標(biāo)差逐漸減小，即基線長(zhǎng)度越長(zhǎng)，航向角越穩(wěn)定。中長(zhǎng)基線的航向誤差在0.2 m/0.9°左右，航向標(biāo)差無明顯變化趨勢(shì)，基本保持穩(wěn)定。

將高精度OEM7系列NovAtel接收機(jī)的靜態(tài)對(duì)準(zhǔn)航向均值10.3712°與俯仰均值–1.3304°視作真值，圖5較清楚的顯示了0.7 m基線與1.5 m基線與真值航向的對(duì)比，將航向誤差控制在了0.5°附近，圖6為0.7 m基線與1.5 m基線下航向的標(biāo)準(zhǔn)偏差?？傻玫絻蓚€(gè)結(jié)論：(1)基線越長(zhǎng)，測(cè)得的航向角越穩(wěn)定；(2)證明了上文所述靜態(tài)對(duì)準(zhǔn)方法可以控制航向誤差優(yōu)于0.7°，滿足精度需求。

表1 仿真方位誤差

圖3 靜態(tài)仿真航向角

圖4 靜態(tài)仿真航向誤差

表2 不同基線的航向、俯仰誤差與標(biāo)差對(duì)比

4.2.2 行進(jìn)間對(duì)準(zhǔn)

在上述靜態(tài)對(duì)準(zhǔn)的基礎(chǔ)上進(jìn)行行進(jìn)間的組合導(dǎo)航測(cè)試，IMU精度為陀螺儀漂移0.005°/s，加速度計(jì)零偏200 mg。使用實(shí)測(cè)車載動(dòng)態(tài)數(shù)據(jù)，進(jìn)行半物理仿真實(shí)驗(yàn)，結(jié)果如圖7，圖8所示。如圖7中，傳統(tǒng)組合導(dǎo)航的航向在70 s以后開始收斂，即行進(jìn)間前1 min左右的航向數(shù)據(jù)誤差過大，改進(jìn)行進(jìn)間對(duì)準(zhǔn)在經(jīng)過靜態(tài)對(duì)準(zhǔn)后航向一開始即為收斂，在運(yùn)載體有大幅度轉(zhuǎn)彎的時(shí)候能較好地跟蹤航向的變化，如圖8中，航向修正后相應(yīng)的速度也會(huì)被修正，圖9和圖10為北向速度與東向速度的誤差對(duì)比，其中：方案1為加入航向量測(cè)；方案2為未加入航向量測(cè)。

圖5 0.7 m與1.5 m基線與真值航向的對(duì)比

圖6 0.7 m與1.5 m基線的航向標(biāo)差

圖7 動(dòng)態(tài)半仿真航向角

4.3 試驗(yàn)分析

圖8 動(dòng)態(tài)半仿真航向誤差

圖9 北向速度誤差

圖10 東向速度誤差

從算法復(fù)雜度進(jìn)行分析，上文方法精度更高，且未增加更多的計(jì)算量，滿足使用線性濾波算法進(jìn)行數(shù)據(jù)融合的條件。從算法精度進(jìn)行分析，可控制航向靜態(tài)對(duì)準(zhǔn)誤差優(yōu)于0.7°，由表3，改進(jìn)方法可將行進(jìn)間航向誤差降低70%，東向速度誤差降低40%，北向速度誤差降低10%。從方法的使用便利程度上進(jìn)行分析，不需要增加除過GNSS與IMU設(shè)備之外的傳感器，不需要通過機(jī)動(dòng)輔助、設(shè)備旋轉(zhuǎn)等步驟完成對(duì)準(zhǔn)。

5 結(jié)論

針對(duì)低精度IMU/GNSS松組合導(dǎo)航系統(tǒng)中初始方位難以精確得到和行進(jìn)間航向容易發(fā)散的問題，提出了一種兩段連續(xù)式對(duì)準(zhǔn)方法。采用雙天線設(shè)備輔助低精度IMU，將初始對(duì)準(zhǔn)的轉(zhuǎn)化為初始最優(yōu)姿態(tài)求取的問題，基于基線矢量推導(dǎo)了最小二乘算法的姿態(tài)測(cè)量模型，擴(kuò)展1維量測(cè)建立卡爾曼濾波模型，并提供了理論依據(jù)與具體實(shí)現(xiàn)步驟。試驗(yàn)表明，基于雙天線基線矢量測(cè)姿的方法可以將初始對(duì)準(zhǔn)誤差控制在0.7°以內(nèi)。同時(shí)，增加航向差值量測(cè)的濾波器提高了組合導(dǎo)航精度，能夠解決低精度IMU/GNSS組合導(dǎo)航系統(tǒng)中航向易發(fā)散、難以準(zhǔn)確跟蹤的問題，保證系統(tǒng)的可靠性。