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

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

RT3000慣性GPS組合導(dǎo)航系統(tǒng)實現(xià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)航計算導(dǎo)航參數(shù)，利用卡爾曼濾波進(jìn)行修正。通過實驗發(fā)現(xiàn)，在這些技術(shù)的支持下，SINS/GPS組合導(dǎo)航系統(tǒng)實現(xiàn)地面車輛的精確導(dǎo)航。

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

1 慣性/GPS組合導(dǎo)航技術(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ā)展速度也越來越快。因此，如何有效的對其指揮和管理己成為交通運輸和安全管理等部門面臨的一個重要問題。由慣性/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ā)，目的是實時地對車輛，飛機和船只等的運動做高精度的測量。

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

圖1 RT300

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

這個獨創(chuàng)性的方案使RT3000比只使用GPS的系統(tǒng)具有了一些獨特的優(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é)編排計算流程

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

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

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

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

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

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

姿態(tài)描述的四種方法:歐拉角、歐拉軸旋轉(zhuǎn)參數(shù)、方向余弦和四元數(shù)法。RT使用四元數(shù)微分方程，消除了歐拉方程的奇異性，修正姿態(tài)組合信息的數(shù)值漂移。計算效率也遠(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)系的運動學(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)點是便于采用卡爾曼濾波實現(xiàn)四元數(shù)最優(yōu)估計,也正是基于這個原因TR3000采用四元數(shù)進(jìn)行姿態(tài)控制。

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

捷聯(lián)式慣性導(dǎo)航系統(tǒng)在開始導(dǎo)航之前，必須進(jìn)行初始對準(zhǔn)，也就是確定導(dǎo)航計算的初始條件。捷聯(lián)式系統(tǒng)數(shù)學(xué)平臺的水平基準(zhǔn)是計算機根據(jù)加速度計所測量到的重力加速度水平分量用數(shù)學(xué)計算方法確定。在完成水平基準(zhǔn)的確定以后，根據(jù)陀螺儀跟隨地球轉(zhuǎn)動所測量的信息，利用與平臺式慣導(dǎo)系統(tǒng)計算羅經(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)角速度計算出b系至e系坐標(biāo)變換矩陣；利用該矩陣將所測比力變換至e系并兩次積分得速度增量。計算分兩個環(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系，并從中補償?shù)糁亓铀俣群涂评飱W里加速度；對轉(zhuǎn)換并補償后所得比力值進(jìn)行兩次積分可得位置增量,由此可提取出載體的姿態(tài)速度與位置信息。如圖4所示

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

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

對于測量出有誤差角速度，RT3000先進(jìn)行偏差修正，再進(jìn)行比例因子的修正。為避免地球每分鐘0.25°的旋轉(zhuǎn)需要減去地球自轉(zhuǎ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ù)估計，可較好實現(xiàn)測量數(shù)據(jù)的混合。根據(jù)濾波器狀態(tài)選取的不同，估計方法分直接法和間接法兩種。直接法直接以各種導(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)行估計時，慣導(dǎo)的系統(tǒng)方程和量測方程是非線性的。利用間接法進(jìn)行估計時，系統(tǒng)方程中的主要部分是導(dǎo)航參數(shù)誤差方程，由于這種方法的誤差很小，通常可忽略二階小量，所以間接法的系統(tǒng)方程和量測方程一般都是線性的，故而組合導(dǎo)航系統(tǒng)的綜合濾波器常采用間接法。利用狀態(tài)估值 X?

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

5 RT3000傾斜路面的實車測試

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

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

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

補償路面傾角后，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)航計算導(dǎo)航參數(shù)，利用卡爾曼濾波進(jìn)行修正。在這些先進(jìn)技術(shù)的支持下，確保了RT3000測量的精確性，以更好的滿足導(dǎo)航的需要。并且通過傾斜路面的實車測試，發(fā)現(xiàn)RT3000能補償了道路傾斜角度，提高了測量的精度。

[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)/里程計自主式車載組合導(dǎo)航系統(tǒng)研究[J]．計測技術(shù)，2006．

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

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

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

A