RT3000慣性GPS組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)車輛運(yùn)動高精度測量

（江蘇省交通技師學(xué)院，江蘇 鎮(zhèn)江 212006）

RT3000慣性GPS組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)車輛運(yùn)動高精度測量

劉 斌

（江蘇省交通技師學(xué)院，江蘇 鎮(zhèn)江 212006）

本文以RT3000慣性GPS組合導(dǎo)航系統(tǒng)為研究對象，進(jìn)行了組合導(dǎo)航的技術(shù)介紹，介紹了SINS（捷聯(lián)慣性）/GPS組合導(dǎo)航系統(tǒng)工作原理，采用四元數(shù)法進(jìn)行姿態(tài)描述，通過捷聯(lián)慣性導(dǎo)航計(jì)算導(dǎo)航參數(shù)，利用卡爾曼濾波進(jìn)行修正。通過實(shí)驗(yàn)發(fā)現(xiàn)，在這些技術(shù)的支持下，SINS/GPS組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)地面車輛的精確導(dǎo)航。

RT3000；SINSGPS組合導(dǎo)航；四元數(shù)；卡爾曼濾波

1 慣性/GPS組合導(dǎo)航技術(shù)

為了獲得移動載體的實(shí)時位置和姿態(tài)信息，已經(jīng)提出和采用了多種導(dǎo)航方式。其中，以慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）和全球衛(wèi)星導(dǎo)航系統(tǒng)（以Global Positioning System，GPS為典型代表）應(yīng)用最為廣泛。INS不僅能夠提供載體位置速度參數(shù)，還能提供載體的三維姿態(tài)參數(shù)，是完全自主的導(dǎo)航方式,在航空、航天、航海和陸地等幾乎所有領(lǐng)域中都得到了廣泛應(yīng)用。

隨著慣性技術(shù)與衛(wèi)星導(dǎo)航定位技術(shù)的發(fā)展，由GPS/INS不同程度組合而成的定位定姿傳感器已成為移動測圖系統(tǒng)中確定載體軌跡和平臺姿態(tài)的重要工具，其中GPS多用于定位而INS則用于測姿。隨著城市建設(shè)的飛速發(fā)展和人民生活水平的日益提高，車輛在人們的生活中起著越來越重要的作用，其發(fā)展速度也越來越快。因此，如何有效的對其指揮和管理己成為交通運(yùn)輸和安全管理等部門面臨的一個重要問題。由慣性/GPS組成的車載導(dǎo)航系統(tǒng)有著廣泛的應(yīng)用前景。本文重在介紹RT3000慣性GPS組合導(dǎo)航系統(tǒng)（以下簡稱RT3000）以及內(nèi)部先進(jìn)技術(shù)。

2 RT300簡介

RT3000由Oxford Technical Solutions公司研發(fā)，目的是實(shí)時地對車輛，飛機(jī)和船只等的運(yùn)動做高精度的測量。

為了獲得高精度的測量，RT使用了為戰(zhàn)斗機(jī)導(dǎo)航系統(tǒng)開發(fā)的數(shù)學(xué)算法。一個由三個加速度計(jì)和三個陀螺儀（角速度傳感器）組成的慣性傳感器組塊用來計(jì)算所有的輸出。當(dāng)從高級的運(yùn)動GPS獲得的測量數(shù)據(jù)刷新由慣性傳感器導(dǎo)航的位移和速度時，一個WGS-84型捆綁導(dǎo)航算法用來補(bǔ)償?shù)孛娴膹澢D(zhuǎn)和科里奧利加速。采用捷聯(lián)慣性（SINS）導(dǎo)航方式。

圖1 RT300

圖2 RT300內(nèi)部組成

這個獨(dú)創(chuàng)性的方案使RT3000比只使用GPS的系統(tǒng)具有了一些獨(dú)特的優(yōu)勢：（1）RT3000有一個很高（100Hz）的刷新率和很寬的帶寬；（2）輸出可以被訪問的反應(yīng)時間很低，只有3.9ms；（3）所有的輸出可以持續(xù)被訪問，即使GPS中斷期間，例如，車輛在橋下行駛時；（4）RT3000可 以辨識GPS位置的跳動并忽略它；（5）GPS進(jìn)行的位置和速度測量中的高頻噪聲可以被消除；（6）RT3000可以進(jìn)行許多GPS不能作的測量，例如，加速度，角速率，航向，俯仰，側(cè)傾等。

圖3 RT3000工作原理圖

圖4 捷聯(lián)力學(xué)編排計(jì)算流程

和常規(guī)的慣性導(dǎo)航系統(tǒng)不同，RT3000用GPS校正所有的測量值。GPS測量位置，速度和（雙天線）航向，但是，通過這些測量，RT3000可以使其它量，如俯仰，側(cè)傾非常精確。當(dāng)GPS起作用時RT3000進(jìn)行測量時就沒有漂移。

標(biāo)準(zhǔn)的RT3000系統(tǒng)可以實(shí)時處理數(shù)據(jù)。實(shí)時結(jié)果可以通過RS232串口輸出，通過10/1000Base-T以太網(wǎng)通過一個UDP無線電廣播，和一個選項(xiàng)，CAN總線。輸出帶有時間的印記并參考GPS時間，1PPS時間同步器可以給不同系統(tǒng)提供很精確的時間同步，慣性測量和GPS時鐘同步。內(nèi)部數(shù)據(jù)日志使數(shù)據(jù)可以在任務(wù)后再處理。數(shù)據(jù)可以在盒子里收集，用“ftp”下載，在計(jì)算機(jī)上處理，也可以通過用戶軟件查看。

3 RT3000的SINSGPS組合導(dǎo)航系統(tǒng)工作原理

單純的GPS技術(shù)是無法滿足汽車導(dǎo)航需要的，因此，必須采用其它技術(shù)與GPS進(jìn)行組合，應(yīng)用信息融合技術(shù)對導(dǎo)航信息進(jìn)行處理，獲得具有高精度和高可靠性的導(dǎo)航信息。而低成本IMU的陀螺儀精度不足以感受地球自轉(zhuǎn)角速率，無法獲得航向信息，只能引入外部航向信息，利用GPS所測速度進(jìn)行匹配對準(zhǔn)，或引入其它方位測量方法，如磁航向或雙天線GPS定向系統(tǒng)等來獲得初始航向信息。

SINS（捷聯(lián)慣性系統(tǒng)）也就是將慣性敏感元件（陀螺儀和加速度計(jì)）直接“捆綁”在運(yùn)載體的機(jī)體上，從而完成制導(dǎo)和導(dǎo)航任務(wù)的系統(tǒng)。在捷聯(lián)式慣導(dǎo)中，慣性平臺的功能由計(jì)算機(jī)完成，故有時也稱作“數(shù)學(xué)平臺”，它的姿態(tài)數(shù)據(jù)是通過計(jì)算得到的。

圖5 RT3000的捷聯(lián)導(dǎo)航系統(tǒng)

IMU測得機(jī)體相對于慣性空間的角速度和加速度，再通過捷聯(lián)式慣導(dǎo)（SINS）力學(xué)編排算法，實(shí)時計(jì)算導(dǎo)航信息，然后利用SINS算法得到導(dǎo)航參數(shù)。卡爾曼濾波對誤差進(jìn)行估計(jì)出并校正系統(tǒng)，DSP（數(shù)字信號處理器）包含高精度的校正矩陣，可以進(jìn)行數(shù)字抗鋸齒濾波器和圓錐積分/劃船效應(yīng)補(bǔ)償算法，保證了測量精度精確到0.01°。其工作原理如圖3。

4.1 四元數(shù)法的姿態(tài)描述

姿態(tài)描述的四種方法:歐拉角、歐拉軸旋轉(zhuǎn)參數(shù)、方向余弦和四元數(shù)法。RT使用四元數(shù)微分方程，消除了歐拉方程的奇異性，修正姿態(tài)組合信息的數(shù)值漂移。計(jì)算效率也遠(yuǎn)遠(yuǎn)優(yōu)于其他三種方法。姿態(tài)描述最常用的方法是歐拉角。非慣性坐標(biāo)系相對于慣性坐標(biāo)系可以用三個歐拉角表示：ψ、θ和φ,分別表示方位角、俯仰角和傾斜角。將慣性坐標(biāo)系按繞相應(yīng)的軸依次旋轉(zhuǎn)ψ、θ和φ,可使慣性坐標(biāo)系與非慣性坐標(biāo)系重合。,但是歐拉角表示兩個坐標(biāo)系之間的關(guān)系,當(dāng)俯仰角為±90°時,描述姿態(tài)角與角速度關(guān)系的運(yùn)動學(xué)方程的系數(shù)矩陣出現(xiàn)奇異。避免出現(xiàn)這種奇異情況發(fā)生的有效辦法就是在座標(biāo)變換中用四元數(shù)代替歐拉角。

4 RT3000內(nèi)部技術(shù)

這四個參數(shù)就是著名的Euler-Rodrigues對稱參數(shù),又稱四元數(shù)。推導(dǎo)得出任一向量v在體軸系中的分量與在地軸系中的分量關(guān)系見公式2：

該方程避免了歐拉方程的奇異性。四元數(shù)的另外一個優(yōu)點(diǎn)是便于采用卡爾曼濾波實(shí)現(xiàn)四元數(shù)最優(yōu)估計(jì),也正是基于這個原因TR3000采用四元數(shù)進(jìn)行姿態(tài)控制。

4.2 SINS（捷聯(lián)慣性系統(tǒng)）的計(jì)算

捷聯(lián)式慣性導(dǎo)航系統(tǒng)在開始導(dǎo)航之前，必須進(jìn)行初始對準(zhǔn)，也就是確定導(dǎo)航計(jì)算的初始條件。捷聯(lián)式系統(tǒng)數(shù)學(xué)平臺的水平基準(zhǔn)是計(jì)算機(jī)根據(jù)加速度計(jì)所測量到的重力加速度水平分量用數(shù)學(xué)計(jì)算方法確定。在完成水平基準(zhǔn)的確定以后，根據(jù)陀螺儀跟隨地球轉(zhuǎn)動所測量的信息，利用與平臺式慣導(dǎo)系統(tǒng)計(jì)算羅經(jīng)法相同的關(guān)系確定出數(shù)學(xué)平臺所處的方位，也就完成了捷聯(lián)式慣導(dǎo)系統(tǒng)的初始對準(zhǔn)。

RT3000直接獲得載體在地心直角坐標(biāo)系（如WGS84）中的三維直角坐標(biāo)（X,Y,Z），直接用于修正INS，適合與GPS數(shù)據(jù)進(jìn)行組合處理，且省去了反復(fù)的坐標(biāo)變換，程序更簡潔和模塊化。

SINS力學(xué)編排即利用陀螺儀所得旋轉(zhuǎn)角速度計(jì)算出b系至e系坐標(biāo)變換矩陣；利用該矩陣將所測比力變換至e系并兩次積分得速度增量。計(jì)算分兩個環(huán)路：1）角速度積分環(huán)路：將測得的b系對i系的旋轉(zhuǎn)角速度，減去e系對i系的地球自轉(zhuǎn)角速度，得所需的b系對e系的旋轉(zhuǎn)角速度；利用該角速度按四元數(shù)積分法修正b系至e系坐標(biāo)變換矩陣。2）加速度積分環(huán)路：用上述所得坐標(biāo)變換矩陣，將比力觀測值轉(zhuǎn)換至e系，并從中補(bǔ)償?shù)糁亓铀俣群涂评飱W里加速度；對轉(zhuǎn)換并補(bǔ)償后所得比力值進(jìn)行兩次積分可得位置增量,由此可提取出載體的姿態(tài)速度與位置信息。如圖4所示

圖6 輸出校正與反饋校正示意圖

圖7 傾斜測試路面?zhèn)葍A和俯仰角度

對于測量出有誤差角速度，RT3000先進(jìn)行偏差修正，再進(jìn)行比例因子的修正。為避免地球每分鐘0.25°的旋轉(zhuǎn)需要減去地球自轉(zhuǎn)速率。受地球自傳速度的影響，物體運(yùn)動的水平速度需按正比例進(jìn)行修正。通過四元數(shù)法角速度和卡爾曼濾波將整合產(chǎn)生方位角、俯仰角和傾斜角度。（采用RT可以工作在任何角度下，不會產(chǎn)生任何奇異。）地球的自轉(zhuǎn)給坐標(biāo)系方向上加速度，通過偏差修正再通過旋轉(zhuǎn)矩陣，（利用卡爾曼濾波）加速度的誤差修正，使得重力場消失并且消除科氏加速度的影響，從而整合加速度產(chǎn)生速度值，進(jìn)而得到位移值。RT3000的捷聯(lián)導(dǎo)航系統(tǒng)見圖5。

4.3 卡爾曼濾波的修正

在組合導(dǎo)航系統(tǒng)，利用卡爾曼濾波進(jìn)行導(dǎo)航參數(shù)估計(jì)，可較好實(shí)現(xiàn)測量數(shù)據(jù)的混合。根據(jù)濾波器狀態(tài)選取的不同，估計(jì)方法分直接法和間接法兩種。直接法直接以各種導(dǎo)航參數(shù)X為主要狀態(tài)，濾波器估值的主要部分就是導(dǎo)航參數(shù)估值間接法以組合導(dǎo)航系統(tǒng)中某一種導(dǎo)航系統(tǒng)（經(jīng)常采用慣導(dǎo)系統(tǒng)）輸出的導(dǎo)航參數(shù)X1的誤差ΔX為濾波器主要狀態(tài)，濾波器估值的主要部分是導(dǎo)航參數(shù)誤差估值用去校正X1。

利用直接法進(jìn)行估計(jì)時，慣導(dǎo)的系統(tǒng)方程和量測方程是非線性的。利用間接法進(jìn)行估計(jì)時，系統(tǒng)方程中的主要部分是導(dǎo)航參數(shù)誤差方程，由于這種方法的誤差很小，通常可忽略二階小量，所以間接法的系統(tǒng)方程和量測方程一般都是線性的，故而組合導(dǎo)航系統(tǒng)的綜合濾波器常采用間接法。利用狀態(tài)估值 X?

Δ 對系統(tǒng)進(jìn)行校正也有兩種方法，輸出校正和反饋校正的原理框圖如圖6所示。

5 RT3000傾斜路面的實(shí)車測試

車輛在有一定坡度或傾斜的測試路面測量得到的側(cè)傾角、俯仰角、車身加速度和滑動角有些反常。對俯仰角和側(cè)傾角的影響最明顯。這些反常的測試值，必須加以補(bǔ)償修正，以滿足正常行駛需要。

傾斜測試路面上坡部分俯仰角是正的，下坡部分是負(fù)的。同樣的問題出現(xiàn)在側(cè)傾角上，車輛在坡頂處為負(fù)的，在坡底處為正的。如圖7所示。

利用RT3000中產(chǎn)生新的“俯仰角平面”和“側(cè)傾角平面”通過CAN總線（CANbus）技術(shù)得以輸出，補(bǔ)償了測試路面面的傾斜角。Configuration Software（組態(tài)軟件）用來測量路面的傾斜角度，并將數(shù)據(jù)記錄到測試系統(tǒng)中，從而也可對路面不平度進(jìn)行統(tǒng)計(jì)測量。

補(bǔ)償路面傾角后，RT3000輸出的相對新的“俯仰角平面”和“側(cè)傾角平面”的側(cè)傾角和俯仰角，而不是相對于重力的。但是水平方向的側(cè)傾角和俯仰角的真值仍然正常輸出，不受影響。圖8給出了車輛在循環(huán)加速測試下，對其側(cè)傾角修正的效果圖。俯仰角也是同樣變化。可見通過RT3000的修正，提升了測量的精度。更好了滿足精確導(dǎo)航的需要。

結(jié)語

本文進(jìn)行了組合導(dǎo)航的技術(shù)介紹，針對RT3000分析了其內(nèi)部先進(jìn)的技術(shù)：采用四元數(shù)法進(jìn)行姿態(tài)描述，通過捷聯(lián)慣性導(dǎo)航計(jì)算導(dǎo)航參數(shù)，利用卡爾曼濾波進(jìn)行修正。在這些先進(jìn)技術(shù)的支持下，確保了RT3000測量的精確性，以更好的滿足導(dǎo)航的需要。并且通過傾斜路面的實(shí)車測試，發(fā)現(xiàn)RT3000能補(bǔ)償了道路傾斜角度，提高了測量的精度。

[1]RT3000產(chǎn)品手冊[M]．Oxford Technical Solutions， 2006．

[2]John Miles．Roll and Pitch on inclines[J]．Vehicle Dynamics International，2008．

[3]張海燕．慣性/GPS/GLONASS組合導(dǎo)航系統(tǒng)在地面車輛上的應(yīng)用[J]．2001年飛行器慣性器件學(xué)術(shù)交流會論文集，2011．

[4]丁文娟，李歲勞，熊偉．捷聯(lián)慣導(dǎo)系統(tǒng)/里程計(jì)自主式車載組合導(dǎo)航系統(tǒng)研究[J]．計(jì)測技術(shù)，2006．

[5]鐘樂，胡延霖，陳永明，王翌臣．一種基于DSP的SINS/GPS組合導(dǎo)航系統(tǒng)實(shí)現(xiàn)[J]．艦船電子工程，2011（02）．

圖8 側(cè)傾角的修正比較

V249 < class="emphasis_bold"> 文獻(xiàn)標(biāo)識碼：A

A