基于車輪安裝慣性測量單元的車載組合導(dǎo)航

陳映秋，曠 儉，牛小驥，李 由，高柯夫，劉經(jīng)南

（武漢大學(xué) 衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢 430079）

慣性導(dǎo)航系統(tǒng)（INS）具有實時性強、抗干擾、不受氣象條件限制等優(yōu)點。但由于慣性器件存在自身誤差，會造成系統(tǒng)定位誤差會隨時間不斷累積，導(dǎo)致其有長時間導(dǎo)航精度低的缺點。目前主流的做法是將誤差不隨時間積累的衛(wèi)星導(dǎo)航定位系統(tǒng)（GNSS）作為定位精度控制手段，與INS進行組合導(dǎo)航定位，使用卡爾曼濾波對系統(tǒng)狀態(tài)量進行最優(yōu)估計，充分利用多系統(tǒng)相互之間的互補特性，既充分發(fā)揮慣導(dǎo)的自主推算能力又可以控制導(dǎo)航誤差不隨時間累積[1-2]。

現(xiàn)有的車載組合導(dǎo)航系統(tǒng)中，雖然主流的 INS/GNSS組合方式可以在很大程度上解決了誤差累積的問題，但是GNSS受環(huán)境因素的影響較大，在復(fù)雜環(huán)境區(qū)域（比如城市峽谷），定位精度嚴(yán)重下降甚至在極端區(qū)域（比如隧道）不可用。因此，車載導(dǎo)航中多采用INS/里程計（ODO）組合導(dǎo)航作為補充，在GNSS定位受到環(huán)境影響時的短時間內(nèi)仍然能夠提供高精度的定位，構(gòu)建一個更穩(wěn)健、更高精度的車載組合導(dǎo)航系統(tǒng)[3]。

INS/ODO組合導(dǎo)航保留了 INS本身不需要任何外部信息的特性，同時大幅減緩了INS誤差累積的速度，因此受到了廣大研究工作者的關(guān)注并得到了較大程度的發(fā)展。這方面研究主要包括航位推算方法、基于航位推算過程誤差模型的組合導(dǎo)航以及考慮部分里程計誤差的慣導(dǎo)/里程計組合導(dǎo)航。然而，使用INS/ODO需要增加額外的里程觀測傳感器，增加了系統(tǒng)構(gòu)建成本和系統(tǒng)算法復(fù)雜性，因此一定程度上限制了該方法的應(yīng)用推廣。

旋轉(zhuǎn)調(diào)制技術(shù)是指當(dāng)慣導(dǎo)的慣性傳感器組合（ISA）做周期性的旋轉(zhuǎn)時，垂直于旋轉(zhuǎn)軸方向的陀螺和加速度計零偏引起的導(dǎo)航誤差可以得到有效抑制，從而使慣性器件誤差在短時間內(nèi)的均值盡量接近零，以此來減小系統(tǒng)誤差累計，提高導(dǎo)航精度[1-7]。但是，對轉(zhuǎn)臺的依賴也限制了旋轉(zhuǎn)調(diào)制方案在低成本車載導(dǎo)航中的應(yīng)用。

受旋轉(zhuǎn)調(diào)制技術(shù)的啟發(fā)，本文提出了一套基于低成本 MEMS-IMU的車輪安裝導(dǎo)航方案。具體而言，將慣性測量單元（IMU）安裝在車輛車輪中心，使IMU跟隨車輪一起進行周期性旋轉(zhuǎn)運動。相比傳統(tǒng) INS/ODO方案，本文方案利用了傳統(tǒng)的INS機械編排算法，通過垂直于旋轉(zhuǎn)軸的陀螺角速度輸出結(jié)合車輪半徑實時地獲得精準(zhǔn)的車體前向速度，取代對物理里程計傳感器的依賴，從軟件層面達(dá)到 INS/ODO組合導(dǎo)航定位的性能，從而節(jié)約成本，因此尤其適用低成本車載導(dǎo)航應(yīng)用。此外，本方案還利用了車輪的周期性旋轉(zhuǎn)特性，有效地降低陀螺零偏引起的位置誤差。相對于傳統(tǒng)安裝方式的INS/ODO組合導(dǎo)航，具有成本更低、精度更高以及結(jié)構(gòu)更簡單的優(yōu)勢。

此外，本文方案還有如下創(chuàng)新點：

1）一方面發(fā)揮了傳統(tǒng)慣性導(dǎo)航自主提供姿態(tài)、速度和位置的優(yōu)勢，另一方面利用IMU測量值結(jié)合輪式運動，挖掘出速度量測，提高自主導(dǎo)航性能。

2）綜合多種運動信息構(gòu)造卡爾曼濾波量測方程。推導(dǎo)了車輪安裝IMU的兩種測速模型，并利用前向速度，加上非完整性約束，構(gòu)造三維卡爾曼濾波速度修正。此外，針對水平姿態(tài)角發(fā)散的情況，利用加速度計量測修正水平姿態(tài)角。

本文首先對車輪中心安裝 IMU的機械結(jié)構(gòu)以及算法流程進行了闡述說明，然后詳細(xì)描述了組合導(dǎo)航卡爾曼濾波算法的設(shè)計和觀測信息的使用，通過真實的輪式機器人載體測試對本方案的定位精度進行了較全面評測和比較，最后給出結(jié)論。

1 車輪安裝IMU導(dǎo)航方案

本方案中利用 IMU最基本的角速度測量能力獲得實時準(zhǔn)確的里程速度，從而實現(xiàn)了無里程計傳感器條件下的 INS/ODO組合導(dǎo)航。其基本設(shè)計思路為：將慣性測量單元安裝在運動車輛的車輪中心并隨車輪做同軸旋轉(zhuǎn)運動，充分利用慣性導(dǎo)航能夠真實反映載體運動軌跡的特性以及絕大多數(shù)情況下車輪只會向前或向后運動的特點，達(dá)到更低成本更高精度的目的。

圖1給出了車輪安裝IMU導(dǎo)航方案的詳細(xì)數(shù)據(jù)處理流程：IMU輸出的測量值用于慣導(dǎo)機械編排解算獲得車輪的位置、速度和姿態(tài)；同時，垂直于車輪平面軸向（如圖2中y軸）的陀螺輸出值用于計算車輪的里程速度，平行于車輪平面兩軸（如圖2中x和z軸）的比力輸出值用于計算水平角，然后使用擴展卡爾曼濾波進行數(shù)據(jù)融合，得到位置、速度和姿態(tài)的最優(yōu)估計。另一方面，做周期旋轉(zhuǎn)運動的 MEMS-IMU具有旋轉(zhuǎn)調(diào)制效應(yīng)，可以有效地抵消陀螺零偏對慣導(dǎo)系統(tǒng)誤差的影響，提高數(shù)據(jù)質(zhì)量[8]。下節(jié)將詳細(xì)介紹上述方案中的組合導(dǎo)航卡爾曼濾波算法。

圖1 數(shù)據(jù)處理流程Fig.1 Algorithm execution process

圖2 坐標(biāo)系示意圖Fig.2 Axes definitions for wheel-fixed frame(b) and chassis-fixed frame(v)

2 組合導(dǎo)航數(shù)據(jù)融合算法

慣性導(dǎo)航是一種相對定位的導(dǎo)航方式，系統(tǒng)狀態(tài)初始化是導(dǎo)航定位精度的基本條件，包括位置、速度、姿態(tài)[9]。在實際應(yīng)用中，由于本方案中MEMS慣性傳感器精度的限制不具備完全自主的對準(zhǔn)能力，位置需要由 GNSS、全站儀或其他高精度手段給定。初始對準(zhǔn)階段一般為靜態(tài)，初速一般設(shè)置為零，可由在靜止條件下的加速度計輸出計算橫滾角、俯仰角，航向角通過GNSS的位置結(jié)果進行反正切計算確定。本方案可以作為INS/GNSS組合導(dǎo)航系統(tǒng)在GNSS受外界影響不可用情況下的一種補充方案。

2.1 卡爾曼濾波狀態(tài)誤差模型

本文中導(dǎo)航坐標(biāo)系選取北-東-地的水平坐標(biāo)系（n系），即原點為當(dāng)前位置，x軸指向正北，y軸指向正東，z軸垂直x、y軸構(gòu)成的平面垂直向下為正。其中慣性導(dǎo)航系統(tǒng)狀態(tài)有三維位置誤差 δrn、三維速度誤差 δvn、三維姿態(tài)誤差ψ、三軸陀螺零偏誤差bg、三軸加表零偏誤差ba，共15維，狀態(tài)量如式(1)所示：

慣性導(dǎo)航系統(tǒng)的誤差模型可由一個連續(xù)線性隨機系統(tǒng)方程(2)表示[10]：

式中，X(t)為系統(tǒng)誤差狀態(tài)向量，F(xiàn)(t)是系統(tǒng)矩陣，G(t)是連續(xù)時間的系統(tǒng)噪聲驅(qū)動矩陣，w(t)是系統(tǒng)噪聲。對于低成本的MEMS慣性器件，精細(xì)的誤差改正并不能帶來明顯的導(dǎo)航精度提升，因此離散化之后的慣導(dǎo)誤差模型可簡化為：

其中，

式中，I3和 03分別表示三階單位矩陣和零矩陣，w是系統(tǒng)噪聲向量，Φ為狀態(tài)轉(zhuǎn)移矩陣，Γ表示系統(tǒng)噪聲系數(shù)矩陣，下標(biāo)k是時刻標(biāo)記，Cn表示由b系（即IMUb傳感器坐標(biāo)系，如圖2所示）轉(zhuǎn)換到n系的方向余弦矩陣，Δt表示IMU觀測值相鄰歷元的時間間隔，τbg、τbf分別表示陀螺零偏和加速度計零偏建模為一階高斯馬爾科夫過程對應(yīng)的相關(guān)時間，fn表示比力向量在n系下的投影。

在得到上述系統(tǒng)狀態(tài)模型后，仍需要相應(yīng)的觀測模型利用修正信息改善系統(tǒng)狀態(tài)的估計精度。線性化后的觀測誤差模型可用式(6)表示：

式中，δZk為觀測向量誤差，Hk是觀測矩陣，vk為觀測噪聲。觀測模型的具體設(shè)計將在2.2節(jié)中具體介紹。

2.2 卡爾曼濾波觀測誤差模型

本文的整體方案為慣性器件與里程計組合導(dǎo)航，特別之處在于慣性元件信息與里程信息均來源于同一器件，即安裝在車輪中心的慣性導(dǎo)航模塊。一方面，IMU的輸出用于機械編排解算獲得位置、速度、姿態(tài)；另一方面，IMU的測量中心與車輪軸心完全重合且進行同軸周期旋轉(zhuǎn)（如圖2所示），根據(jù)陀螺與車輪平面垂直軸的輸出，結(jié)合車輪半徑可得到前向速度信息。

此外，本方案進一步加入非完整性約束，與前向速度共同構(gòu)成三維速度約束。非完整性約束是指載體在正常運動過程中，一般不會發(fā)生橫向側(cè)滑和垂向跳躍的現(xiàn)象，此時認(rèn)為載體在橫向和垂向的速度為零。

車輛在行進過程中通常符合此特性，因此除了沿車輛前進方向上的速度為vwheel，橫向以及垂向速度為零[9]，即：

式中，里程速度由陀螺輸出計算得到，ωy為陀螺y軸角速率輸出，rw為車輪半徑。

慣性導(dǎo)航推算的速度vn與車輛速度vv的關(guān)IMUwheel系如式(9)所示[9]：

式中，

式中，（Cn）表示矩陣Cn的第3行第1列對應(yīng)的數(shù)值。b31b需要說明一點的是，本文只針對車輛在平坦道路的行駛環(huán)境進行測試，后續(xù)的討論都將基于此假設(shè)展開。而對于更復(fù)雜的環(huán)境，需要對車輛進行橫滾、俯仰角估計才能很好地計算安裝角方向余弦矩陣，此處對該情況不做更多地描述。

另外，由里程計得到的速度觀測模型如下：ev是速度的觀測噪聲。

綜上，速度的誤差觀測方程可由式(13)表示：

2.3 卡爾曼濾波的更新

卡爾曼濾波分為預(yù)測和更新兩個步驟，每次完成預(yù)測后，都需要對系統(tǒng)誤差狀態(tài)量進行更新：

式中，(δZk-HkδXk,k-1)為新息，代表新的量測信息與系統(tǒng)模型預(yù)測量的差異，增益矩陣K決定新息在更新過程中所占的比例權(quán)重。相應(yīng)的誤差協(xié)方差矩陣如下：

式中，P是狀態(tài)量δX的協(xié)方差矩陣，Q是系統(tǒng)噪聲方差陣。估計狀態(tài)誤差向量的協(xié)方差矩陣P可由式(16)(17)得到：

其中，R為觀測向量的協(xié)方差矩陣。

本文中所采用的卡爾曼濾波為擴展卡爾曼濾波（EKF），EKF估計的誤差是當(dāng)前時刻系統(tǒng)狀態(tài)量的估計值，即此種濾波器的工作形式是帶有誤差反饋的閉環(huán)修正。因此系統(tǒng)誤差狀態(tài)量在每次觀測更新后，都將用于修正導(dǎo)航狀態(tài)和參數(shù)，并將誤差狀態(tài)向量置零。

3 實驗結(jié)果及數(shù)據(jù)分析

3.1 實驗設(shè)備及環(huán)境

車載實驗使用輪式機器人（PioneerP3-DX-SH）作為運動載體，IMU選用InvenSense公司的九軸運動傳感器模塊MPU9150，其主要性能參數(shù)見表1。

實驗所用慣導(dǎo)模塊為同一型號的IMU，具體安裝情況如圖3所示，固定在車輪中心為IMU1、固定在車身為 IMU2。在采集數(shù)據(jù)前，所有模塊的零偏和比例因子都進行了實驗室標(biāo)定和補償。

在試驗場采集得到兩個IMU的數(shù)據(jù)后，進行以下三種方案模式的數(shù)據(jù)處理和效果對比：

方案a：在IMU2數(shù)據(jù)做慣導(dǎo)解算的基礎(chǔ)上，進行非完整性約束；

方案b：在IMU2數(shù)據(jù)做慣導(dǎo)解算的基礎(chǔ)上，進行里程計速度和非完整性約束修正；

方案c：在IMU1數(shù)據(jù)做慣導(dǎo)解算的基礎(chǔ)上，進行里程計速度和非完整性約束修正。

同時，利用INS/GNSS松組合解算并進行雙向平滑后的結(jié)果作為參考真值。其中，GNSS接收機為天寶R9，數(shù)據(jù)處理方案為實時動態(tài)差分（RTK）后處理解算（精度可達(dá)厘米級），IMU數(shù)據(jù)為車身上的MPU9150提供，IMU和GNSS采樣率分別為250 Hz和1 Hz。

表1 MPU9150性能參數(shù)Tab.1 Performance parameters of MPU9150

圖3 模塊安裝環(huán)境及RTK實際參考軌跡Fig.3 IMU installation and the reference trajectory in Google Earth from GPS-RTK

本方案的理想工作情況是車輛置于完全水平的場地，而將實驗小車置于圖3所示的室外環(huán)境中，是因為可以利用GNSS定位結(jié)果作為絕對參考值來驗證此方案的可行性，因此在后續(xù)分析中，僅考察該方案相比于傳統(tǒng)方案在水平面（北向、東向）上的精度提升。實驗過程中，控制小車圍繞所在場地運動，動態(tài)實驗持續(xù)約11 min，整個實驗過程中實時采集IMU原始數(shù)據(jù)，對數(shù)據(jù)進行后處理解算和分析。

圖3中右下角顯示為小車運動的實際參考路徑，在后續(xù)圖中則會顯示三種方案相對于 RTK參考結(jié)果的北向與東向位置誤差圖。表2則是三種方案的位置誤差統(tǒng)計表。

圖4中，由于行人對GNSS衛(wèi)星信號的遮擋，解算所得的速度值會發(fā)生跳變，將GNSS速度粗差剔除后作為參考真值，考察使用輪上IMU陀螺角速度換算的速度的品質(zhì)，其精度較為平穩(wěn)，均方根誤差約為0.075 m/s。

圖4 由陀螺輸出計算所得的前向速度Fig.4 Forward speed calculated from gyro output

由圖5中各方案的軌跡可以看出：方案a只對車身上的IMU2模塊進行數(shù)據(jù)處理，沒有穩(wěn)定的里程速度約束，位置誤差發(fā)散較快；方案b采用車身IMU2與里程信息做組合，即傳統(tǒng)車載組合導(dǎo)航方案，也得到了比較好的結(jié)果，但是需要額外的設(shè)備提供里程信息，成本相比單個IMU模塊增加；本文方案處理結(jié)果在漂移與發(fā)散方面均有明顯提升。

圖5 各方案軌跡圖Fig.5 Estimated trajectories of different methods

圖6示出了三種方案相對于RTK參考結(jié)果的北向與東向位置誤差圖，由此可得：方案a由于沒有融合里程計信息作為穩(wěn)定的速度約束，誤差發(fā)散較快；方案b相對于方案a來說，增加了里程計信息，北向位置誤差的均方根誤差（RMS）由6.50 m降低到3.64 m，東向位置誤差的RMS由5.59 m變到3.15 m，垂向位置誤差的均方根誤差由1.70 m變到0.13 m；而本方案（方案c）相對于方案b，在北向上精度又提高了34.45%，東向上提高了 18.05%。因此，本車輪 IMU方案在北向和東向上的位置精度相較于其他兩種方案均有明顯提升。

圖6 各方案誤差圖Fig.6 Estimated trajectories of different methods

表2 位置誤差統(tǒng)計表Tab.2 Statistical table of position errors

實驗結(jié)果表明，本文方案顯著提高了安裝在機器人車輪上的 MEMS-IMU的導(dǎo)航定位精度，實驗小車在平均速度約為0.8 m/s的條件下運動11 min，平面位置RMS降低到3.52 m，相比于傳統(tǒng)安裝MEMS-IMU的導(dǎo)航方案降低了58.53%，相比于傳統(tǒng)MEMS-IMU和里程計組合導(dǎo)航方案降低了29.67%。

高程方面，傳統(tǒng)的 INS/ODO車載組合導(dǎo)航方案（方案b）精度最高，這是因為試驗場地存在一定的坡度變化；而本方案的理想情況是在車輛在平整的水平面上行駛，因此在垂直方向的精度受到了一定影響。

4 結(jié) 論

本文采用了將MEMS-IMU模塊安裝在車輛車輪中心隨車輪做周期旋轉(zhuǎn)運動的方法。一方面，該方案類似于對MEMS慣導(dǎo)模塊實時地進行旋轉(zhuǎn)調(diào)制，能夠有效地減弱了陀螺零偏對慣導(dǎo)定位誤差的影響，在水平定位精度上取得了顯著改善；另一方面，本方案由單個模塊同時提供慣導(dǎo)原始數(shù)據(jù)和里程計信息（即車輪運動速度），在提高定位精度的前提下降低了設(shè)備成本和復(fù)雜度。

本方案僅使用一個低成本MEMS-IMU，通過軟件算法取代了里程計傳感器，從而提供對MEMS慣導(dǎo)誤差的約束，達(dá)到并超越了“慣導(dǎo)+里程計”組合導(dǎo)航的效果，是一種適合于低成本車輛和輪式機器人導(dǎo)航定位的實用方案。

本文算法還存在一定的局限性，在平坦地面精度比較高，上下坡或者通用的路面算法局限性更加明顯，因此在后續(xù)研究中會針對通用路面對算法進行改進。