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

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

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

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

INS/ODO組合導(dǎo)航保留了 INS本身不需要任何外部信息的特性，同時(shí)大幅減緩了INS誤差累積的速度，因此受到了廣大研究工作者的關(guān)注并得到了較大程度的發(fā)展。這方面研究主要包括航位推算方法、基于航位推算過程誤差模型的組合導(dǎo)航以及考慮部分里程計(jì)誤差的慣導(dǎo)/里程計(jì)組合導(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)時(shí)，垂直于旋轉(zhuǎn)軸方向的陀螺和加速度計(jì)零偏引起的導(dǎo)航誤差可以得到有效抑制，從而使慣性器件誤差在短時(shí)間內(nèi)的均值盡量接近零，以此來減小系統(tǒng)誤差累計(jì)，提高導(dǎo)航精度[1-7]。但是，對(duì)轉(zhuǎn)臺(tái)的依賴也限制了旋轉(zhuǎn)調(diào)制方案在低成本車載導(dǎo)航中的應(yīng)用。

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

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

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

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

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

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

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

圖1給出了車輪安裝IMU導(dǎo)航方案的詳細(xì)數(shù)據(jù)處理流程：IMU輸出的測量值用于慣導(dǎo)機(jī)械編排解算獲得車輪的位置、速度和姿態(tài)；同時(shí)，垂直于車輪平面軸向（如圖2中y軸）的陀螺輸出值用于計(jì)算車輪的里程速度，平行于車輪平面兩軸（如圖2中x和z軸）的比力輸出值用于計(jì)算水平角，然后使用擴(kuò)展卡爾曼濾波進(jìn)行數(shù)據(jù)融合，得到位置、速度和姿態(tài)的最優(yōu)估計(jì)。另一方面，做周期旋轉(zhuǎn)運(yùn)動(dòng)的 MEMS-IMU具有旋轉(zhuǎn)調(diào)制效應(yīng)，可以有效地抵消陀螺零偏對(duì)慣導(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)航是一種相對(duì)定位的導(dǎo)航方式，系統(tǒng)狀態(tài)初始化是導(dǎo)航定位精度的基本條件，包括位置、速度、姿態(tài)[9]。在實(shí)際應(yīng)用中，由于本方案中MEMS慣性傳感器精度的限制不具備完全自主的對(duì)準(zhǔn)能力，位置需要由 GNSS、全站儀或其他高精度手段給定。初始對(duì)準(zhǔn)階段一般為靜態(tài)，初速一般設(shè)置為零，可由在靜止條件下的加速度計(jì)輸出計(jì)算橫滾角、俯仰角，航向角通過GNSS的位置結(jié)果進(jìn)行反正切計(jì)算確定。本方案可以作為INS/GNSS組合導(dǎo)航系統(tǒng)在GNSS受外界影響不可用情況下的一種補(bǔ)充方案。

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

本文中導(dǎo)航坐標(biāo)系選取北-東-地的水平坐標(biāo)系（n系），即原點(diǎ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)的誤差模型可由一個(gè)連續(xù)線性隨機(jī)系統(tǒng)方程(2)表示[10]：

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

其中，

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

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

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

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

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

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

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

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

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

式中，

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

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

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

2.3 卡爾曼濾波的更新

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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