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

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

劉 斌

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

本文以RT3000慣性GPS組合導(dǎo)航系統(tǒng)為研究對(duì)象，進(jìn)行了組合導(dǎo)航的技術(shù)介紹，介紹了SINS（捷聯(lián)慣性）/GPS組合導(dǎo)航系統(tǒng)工作原理，采用四元數(shù)法進(jìn)行姿態(tài)描述，通過(guò)捷聯(lián)慣性導(dǎo)航計(jì)算導(dǎo)航參數(shù)，利用卡爾曼濾波進(jìn)行修正。通過(guò)實(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ù)

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

2 RT300簡(jiǎn)介

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

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

圖1 RT300

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

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

圖3 RT3000工作原理圖

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

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

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

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

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

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

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

IMU測(cè)得機(jī)體相對(duì)于慣性空間的角速度和加速度，再通過(guò)捷聯(lián)式慣導(dǎo)（SINS）力學(xué)編排算法，實(shí)時(shí)計(jì)算導(dǎo)航信息，然后利用SINS算法得到導(dǎo)航參數(shù)。卡爾曼濾波對(duì)誤差進(jìn)行估計(jì)出并校正系統(tǒng)，DSP（數(shù)字信號(hào)處理器）包含高精度的校正矩陣，可以進(jìn)行數(shù)字抗鋸齒濾波器和圓錐積分/劃船效應(yīng)補(bǔ)償算法，保證了測(cè)量精度精確到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)系相對(duì)于慣性坐標(biāo)系可以用三個(gè)歐拉角表示：ψ、θ和φ,分別表示方位角、俯仰角和傾斜角。將慣性坐標(biāo)系按繞相應(yīng)的軸依次旋轉(zhuǎn)ψ、θ和φ,可使慣性坐標(biāo)系與非慣性坐標(biāo)系重合。,但是歐拉角表示兩個(gè)坐標(biāo)系之間的關(guān)系,當(dāng)俯仰角為±90°時(shí),描述姿態(tài)角與角速度關(guān)系的運(yùn)動(dòng)學(xué)方程的系數(shù)矩陣出現(xiàn)奇異。避免出現(xiàn)這種奇異情況發(fā)生的有效辦法就是在座標(biāo)變換中用四元數(shù)代替歐拉角。

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

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

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

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

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

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

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

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

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

對(duì)于測(cè)量出有誤差角速度，RT3000先進(jìn)行偏差修正，再進(jìn)行比例因子的修正。為避免地球每分鐘0.25°的旋轉(zhuǎn)需要減去地球自轉(zhuǎn)速率。受地球自傳速度的影響，物體運(yùn)動(dòng)的水平速度需按正比例進(jìn)行修正。通過(guò)四元數(shù)法角速度和卡爾曼濾波將整合產(chǎn)生方位角、俯仰角和傾斜角度。（采用RT可以工作在任何角度下，不會(huì)產(chǎn)生任何奇異。）地球的自轉(zhuǎn)給坐標(biāo)系方向上加速度，通過(guò)偏差修正再通過(guò)旋轉(zhuǎn)矩陣，（利用卡爾曼濾波）加速度的誤差修正，使得重力場(chǎng)消失并且消除科氏加速度的影響，從而整合加速度產(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)測(cè)量數(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ì)時(shí)，慣導(dǎo)的系統(tǒng)方程和量測(cè)方程是非線性的。利用間接法進(jìn)行估計(jì)時(shí)，系統(tǒng)方程中的主要部分是導(dǎo)航參數(shù)誤差方程，由于這種方法的誤差很小，通?？珊雎远A小量，所以間接法的系統(tǒng)方程和量測(cè)方程一般都是線性的，故而組合導(dǎo)航系統(tǒng)的綜合濾波器常采用間接法。利用狀態(tài)估值 X?

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

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

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

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

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

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

結(jié)語(yǔ)

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

[1]RT3000產(chǎn)品手冊(cè)[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ù)交流會(huì)論文集，2011．

[4]丁文娟，李歲勞，熊偉．捷聯(lián)慣導(dǎo)系統(tǒng)/里程計(jì)自主式車載組合導(dǎo)航系統(tǒng)研究[J]．計(jì)測(cè)技術(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)識(shí)碼：A

A