GPS與INS組合的便攜式軌道類游樂(lè)設(shè)施軌跡研究

黃輝瓊， 項(xiàng)輝宇， 孫九增

(北京工商大學(xué) 人工智能學(xué)院， 北京 100048)

0 引 言

過(guò)山車等軌道游樂(lè)設(shè)施一般具有軌道蜿蜒曲折、行駛姿態(tài)多變的特征，而且由于受起落高度、載客人數(shù)等因素影響，其運(yùn)行軌跡和乘客姿態(tài)存在一定差異，增加了設(shè)計(jì)評(píng)價(jià)和安全評(píng)估的難度。目前，關(guān)于軌道類游樂(lè)設(shè)施軌跡檢測(cè)方面的研究成果較少，通過(guò)實(shí)測(cè)獲取其運(yùn)行過(guò)程空間位置和姿態(tài)信息的技術(shù)手段不足[1]。因此開(kāi)發(fā)一種便攜式適應(yīng)現(xiàn)場(chǎng)測(cè)試評(píng)估需求的軌道類游樂(lè)設(shè)施姿態(tài)與軌跡測(cè)量裝置十分必要。

安卓智能手機(jī)內(nèi)部搭載多種傳感器，雖然其中的慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)具有抗干擾性強(qiáng)、可靠性高、自主性好等特點(diǎn)，但往往會(huì)存在累積誤差，單獨(dú)使用難以提供高精度的位姿和速度信息；而將其與手機(jī)自帶的GPS(Global Positioning System)導(dǎo)航組合使用，由于GPS誤差不在時(shí)間上累積，通過(guò)卡爾曼濾波器實(shí)現(xiàn)INS與GPS組合，能夠有效克服慣導(dǎo)系統(tǒng)自身不足，同時(shí)矯正慣導(dǎo)系統(tǒng)累積誤差，提高系統(tǒng)準(zhǔn)確度，獲取速度、位置的最優(yōu)估計(jì)[2～3]。

目前，INS與GPS組合使用有多種方式。采用卡爾曼濾波作為組合算法，通過(guò)狀態(tài)方程及量測(cè)方程，以前一個(gè)數(shù)據(jù)參量，推測(cè)當(dāng)前應(yīng)得數(shù)據(jù)，通過(guò)以當(dāng)前所測(cè)數(shù)據(jù)參量為校正量進(jìn)行修正，從而獲得數(shù)據(jù)參量得最佳估算[4]。為收集試驗(yàn)所需的INS、GPS數(shù)據(jù)，開(kāi)發(fā)Android客戶端采集相關(guān)數(shù)據(jù)，在Matlab軟件環(huán)境下搭建算法平臺(tái)，對(duì)數(shù)據(jù)進(jìn)行處理，在試驗(yàn)場(chǎng)地采集數(shù)據(jù)，將處理完成的數(shù)據(jù)導(dǎo)入U(xiǎn)G，編寫擬合程序二次開(kāi)發(fā)，擬合游樂(lè)設(shè)施運(yùn)行軌跡，實(shí)現(xiàn)可視化，驗(yàn)證方法的可用性及有效性。

1 慣性導(dǎo)航系統(tǒng)

1.1 慣導(dǎo)系統(tǒng)原理

采用捷聯(lián)慣導(dǎo)系統(tǒng)，系統(tǒng)原理如圖1所示。慣性元件安裝在運(yùn)載體上，通過(guò)導(dǎo)航計(jì)算機(jī)進(jìn)行運(yùn)算，經(jīng)過(guò)求解加速度計(jì)的線運(yùn)動(dòng)數(shù)據(jù)、陀螺儀的角運(yùn)動(dòng)數(shù)據(jù)，計(jì)算姿態(tài)轉(zhuǎn)換矩陣；進(jìn)行坐標(biāo)轉(zhuǎn)換，載體坐標(biāo)系—oxbybzb固定于運(yùn)載體上，原點(diǎn)位于運(yùn)載體重心上，xb軸、yb軸、zb軸分別指向載體前進(jìn)正右方、載體前進(jìn)正前方、載體前進(jìn)正上方，載體運(yùn)動(dòng)狀態(tài)需要通過(guò)導(dǎo)航坐標(biāo)系—oxnynzn進(jìn)行衡量。捷聯(lián)慣導(dǎo)系統(tǒng)通常選擇地理坐標(biāo)系(東北天坐標(biāo)系)作為導(dǎo)航坐標(biāo)系—oxtytzt，原點(diǎn)與載體重心重合，用于求解運(yùn)載體各個(gè)方向的加速度數(shù)值[5]。

圖1 捷聯(lián)式慣性導(dǎo)航系統(tǒng)原理

1.2 慣性導(dǎo)航基本方程

用四元數(shù)表示姿態(tài)矩陣(方向余弦矩陣)，如式(1)、式(2)所示：

(1)

(2)

參照文獻(xiàn)[6]，分別通過(guò)式(3)～式(5)用四元數(shù)值反求載體的歐拉角。

θ=sin-1(T32)

(3)

(4)

(5)

式中:θ表示俯仰角,γ為橫滾角,φ為航向角的值。

將比力fb從載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系fn的形式如式(6)所示：

(6)

速度可通過(guò)加速度一次積分求解，速度V的即時(shí)修正如式(7)所示：

(7)

姿態(tài)速率(載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的角速度)可通過(guò)式(8)計(jì)算得到：

(8)

載體位置的計(jì)算主要包括載體的經(jīng)緯度和高度的求解，具體實(shí)現(xiàn)過(guò)程如式(9)～式(11)所示：

(9)

(10)

(11)

載體坐標(biāo)系和地理坐標(biāo)系之間歐拉角的對(duì)應(yīng)關(guān)系如式(12)所示：

(12)

2 GPS/INS組合導(dǎo)航系統(tǒng)

2.1 全球定位系統(tǒng)

全球定位系統(tǒng)(GPS)是利用3顆衛(wèi)星發(fā)射時(shí)間到接收機(jī)(用戶)之間的時(shí)差及軌道位置進(jìn)行導(dǎo)航[7]。構(gòu)建導(dǎo)航位置的3個(gè)方程，分別求解觀測(cè)點(diǎn)的x、y、z坐標(biāo)，即三角定位法。為保證測(cè)量的準(zhǔn)確性，不受接收機(jī)與衛(wèi)星時(shí)鐘偏差影響，需要第四顆衛(wèi)星，組成4個(gè)偽距方程，分別求解x、y、z坐標(biāo)以及時(shí)鐘偏差，從而達(dá)到導(dǎo)航定位的目的。雖然GPS導(dǎo)航信息輸出頻率較低，但是輸出頻率穩(wěn)定，誤差不隨時(shí)間積累[8]。

2.2 組合導(dǎo)航系統(tǒng)原理

如圖2所示，采用松組合模式進(jìn)行INS/GPS組合測(cè)量。通過(guò)線性卡爾曼濾波作為融合算法，提取最小方差估計(jì)值，對(duì)INS進(jìn)行速度、位置校正。當(dāng)GPS輸出速度、位置信息時(shí)，以GPS與INS速度、位置差值作為量測(cè)方程，利用INS誤差方程構(gòu)造組合系統(tǒng)狀態(tài)方程[9]。

圖2 INS/GPS松耦合結(jié)構(gòu)圖

2.3 組合系統(tǒng)狀態(tài)方程與量測(cè)方程

慣性導(dǎo)航系統(tǒng)的誤差包括INS位置誤差、INS速度誤差、INS姿態(tài)誤差，通過(guò)誤差組合，可得系統(tǒng)誤差狀態(tài)方程，如式(13)、式(14)所示，其狀態(tài)量為18維。

(13)

X(t)=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εcx,εcy,εcz,εrx,εry,εrz,…,?rx,?ry,?rz]T

(14)

式中：k表示第k時(shí)刻，φE、φN、φU分別表示數(shù)據(jù)測(cè)量后的姿態(tài)角誤差，δvE、δvN、δh表示方向速度誤差,δL、δλ、δvU分別表示經(jīng)度、維度和高度誤差，εcx、εcy、εcz表示陀螺儀產(chǎn)生的隨機(jī)誤差,εrx、εry、εrz表示陀螺儀一階馬爾代夫過(guò)程，?rx、?ry、?rz分別表示加速度計(jì)的零偏,W(t)表示系統(tǒng)陀螺儀、加速度隨機(jī)白噪聲。噪聲系數(shù)矩陣G(t)如式(15)，INS系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣如式(16)所示：

(15)

(16)

取INS、GPS速度、位置信息做差，作為量測(cè)方程。位置量測(cè)信息分別如式(17)、式(18)所示：

(17)

(18)

式中:(L,λ,h)表示位置信息，反映載體真實(shí)位置，(LI,λI,hI)表示基于慣性導(dǎo)航系統(tǒng)的經(jīng)度、緯度和高度信息，(δL,δλ,δh)表示捷聯(lián)慣導(dǎo)系統(tǒng)的經(jīng)度、緯度和高度誤差。(LG,λG,hG)下標(biāo)帶有G的表示GPS的位置信息，主要是GPS得到的經(jīng)緯度和高度信息，(δLG,δλG,δhG)則是表示GPS的誤差，是GPS定位系統(tǒng)得到的經(jīng)緯度和高度誤差，(NE,NE,NU)同樣也表示位置信息，是GPS在東北天方向的位置誤差[10]。

慣性導(dǎo)航系統(tǒng)的速度信息和GPS全球定位系統(tǒng)的速度信息如式(19)、式(20)所示：

(19)

(20)

式中：(δvE,δvN,δvU)和上述位置信息相對(duì)應(yīng)，表示載體的真正速度，(vEI,vNI,vUI)表示捷聯(lián)慣性導(dǎo)航系統(tǒng)的速度信息(東北天向速度信息)，(δvE,δvn,δvU)表示捷聯(lián)慣導(dǎo)的東北天向速度誤差信息，(vEG,vNG,vUG)表示GPS的速度信息，(ME,MN,MU)是GPS在東北天方向的速度誤差。

位置、速度量測(cè)矢量如式(21)、式(22)所示：

(21)

式中：

(22)

GPS全球定位系統(tǒng)的量測(cè)白噪聲矢量如式(23)所示：

N(t)=[NNNENhMEMNMU]T

(23)

對(duì)應(yīng)的量測(cè)白噪聲矢量，方差矩陣如式(24)所示：

(24)

2.4 Matlab軟件仿真結(jié)果分析

在試驗(yàn)場(chǎng)地進(jìn)行直線引動(dòng)加水平回環(huán)運(yùn)動(dòng)，整合實(shí)驗(yàn)數(shù)據(jù)。通過(guò)建立的組合導(dǎo)航狀態(tài)和量測(cè)方程，使用Matlab軟件構(gòu)建算法環(huán)境，仿真所得速度、位置誤差曲線如圖3、圖4所示。

圖3 組合導(dǎo)航前、后速度誤差

圖3中虛線表示組合導(dǎo)航前速度誤差，實(shí)線表示組合導(dǎo)航后速度誤差。由圖3、圖4分析可知，未進(jìn)行組合導(dǎo)航前，速度誤差隨時(shí)間積累，最終導(dǎo)致癱瘓；而通過(guò)組合導(dǎo)航后，誤差得到有效控制，速度誤差的范圍在-0.5～0.5 m/s之間，最大誤差在-1～1 m/s之間，準(zhǔn)確性明顯提高；位置誤差的波動(dòng)也穩(wěn)定在同一范圍內(nèi)，誤差較小，準(zhǔn)確度較高，組合導(dǎo)航具有一定的校正效果，使誤差降低、精度提高，證明組合導(dǎo)航的合理性。

圖4 組合導(dǎo)航位置誤差

3 游樂(lè)設(shè)施軌跡測(cè)量及可視化

3.1 軌跡測(cè)量流程設(shè)計(jì)

為確保測(cè)量準(zhǔn)確，盡可能消除重力分量，需要進(jìn)行初始對(duì)準(zhǔn)與捷聯(lián)矩陣即時(shí)修正；采用GPS全球定位系統(tǒng)校正，彌補(bǔ)慣性導(dǎo)航因長(zhǎng)時(shí)間積分運(yùn)算而造成的誤差累積。具體軌跡測(cè)量步驟如下：

(1)初始對(duì)準(zhǔn)，保證導(dǎo)航坐標(biāo)系與參考坐標(biāo)系重合，手機(jī)水平固定，重力分量在z軸方向。

(2)捷聯(lián)矩陣T的初始值為0，在運(yùn)動(dòng)過(guò)程中，對(duì)捷聯(lián)矩陣即時(shí)修正。

(3)加速度數(shù)據(jù)利用捷聯(lián)矩陣進(jìn)行坐標(biāo)轉(zhuǎn)換。通過(guò)四元數(shù)關(guān)系矩陣將加速度分量從載體坐標(biāo)系轉(zhuǎn)換到地理坐標(biāo)系；將一個(gè)g的重力分量通過(guò)四元數(shù)的姿態(tài)矩陣轉(zhuǎn)換到平臺(tái)坐標(biāo)系下，再用轉(zhuǎn)換后的加速度分量減去轉(zhuǎn)換后的重力分量。由于不同地方的經(jīng)度和緯度對(duì)重力加速度g有一定的影響，g0是赤道表面的加速度(g0=9.780 326 771 4 m/s2)，可通過(guò)式(25)計(jì)算地球引力加速度g：

g=g0×(1+0.005 217×sin2L)-

3.086×10-6h

(25)

(4)加速度數(shù)據(jù)經(jīng)過(guò)姿態(tài)矩陣變換后，進(jìn)行二次積分運(yùn)算。在已知物體初始速度和位置的前提下，計(jì)算載體軌跡。

(5)用GPS全球定位系統(tǒng)輔助測(cè)量方法，對(duì)軌跡測(cè)量進(jìn)行校正。

通過(guò)融合GPS，采用Great-Circle距離公式方法，來(lái)對(duì)軌跡進(jìn)行校正。計(jì)算公式如式(26)所示：

(26)

式中:R為地球半徑，Aj、Aw分別表示A點(diǎn)的經(jīng)度、緯度,Bj、Bw分別表示B點(diǎn)的經(jīng)度、緯度[11]。

3.2 軌道游樂(lè)設(shè)施實(shí)驗(yàn)測(cè)試

軌道游樂(lè)設(shè)施的現(xiàn)場(chǎng)取樣如圖5所示。將手機(jī)固定在過(guò)山車座椅上，使用由Android Studio軟件開(kāi)發(fā)的應(yīng)用程序進(jìn)行手機(jī)加速度傳感器、陀螺儀傳感器、GPS傳感器數(shù)據(jù)采集。應(yīng)用程序客戶端架構(gòu)組成與應(yīng)用程序前端界面如圖6、圖7所示。

圖5 軌道游樂(lè)設(shè)施數(shù)據(jù)采集現(xiàn)場(chǎng)

采集到的數(shù)據(jù)包括三軸加速度、三軸角速度、GPS數(shù)據(jù)，以及采集點(diǎn)的對(duì)應(yīng)時(shí)間，數(shù)據(jù)被保存至后臺(tái)文件中。測(cè)量完畢后，在手機(jī)的后臺(tái)已經(jīng)保存了3個(gè)文件，在后端程序的設(shè)計(jì)中，已將文件保存格式設(shè)置為.txt格式，保存的部分?jǐn)?shù)據(jù)信息如圖8所示。

圖6 數(shù)據(jù)采集程序客戶端架構(gòu)組成圖

圖7 應(yīng)用程序前端界面

圖8 部分?jǐn)?shù)據(jù)信息示意圖

將數(shù)據(jù)導(dǎo)入，在Matlab軟件中進(jìn)行整合、求解四元數(shù)姿態(tài)、運(yùn)動(dòng)軌跡解算。載體姿態(tài)測(cè)得值經(jīng)過(guò)卡爾曼濾波處理后，數(shù)值信息如圖9～圖11所示：

圖9 橫滾角濾波前后示意圖

圖10 俯仰角濾波前后示意圖

圖11 偏航角濾波前后示意圖

在前500采樣點(diǎn)階段，手機(jī)處于初始對(duì)準(zhǔn)靜止階段，數(shù)值變化不明顯；初始對(duì)準(zhǔn)之后開(kāi)始運(yùn)動(dòng)，在采樣點(diǎn)500～1 000之間時(shí)，出現(xiàn)最大峰值，在接近1 000點(diǎn)左右時(shí)，數(shù)據(jù)較密集，采樣點(diǎn)在1 000～2 000之間時(shí)大概呈現(xiàn)均勻變化。圖12中，偏航角在前500個(gè)采樣中大降幅，降幅后趨于穩(wěn)定，原因是試驗(yàn)開(kāi)始時(shí)，初始化后手機(jī)放置位置向右偏移，安裝過(guò)程中手機(jī)位置回偏，后趨于位置正常狀態(tài)，500～1 000采樣點(diǎn)間，有一段大幅波動(dòng)，后趨于平穩(wěn)，手機(jī)狀況符合數(shù)據(jù)反饋情況，符合載體姿態(tài)變化情況。通過(guò)對(duì)比濾波前后數(shù)據(jù)，濾波后的角度更加平滑，有效的剔除毛刺，降低誤差，提高精度。

將最終組合導(dǎo)航后的位置坐標(biāo)導(dǎo)入U(xiǎn)G NX界面中，擬合運(yùn)動(dòng)軌跡曲線 。運(yùn)動(dòng)軌跡圖如圖12、圖13所示。

圖12 擬合后軌跡圖像

圖13 回環(huán)運(yùn)動(dòng)放大部分

可以看出，整體軌跡曲線光滑流暢。軌道擬合的回環(huán)運(yùn)動(dòng)放大部分(圖13)與實(shí)際游樂(lè)設(shè)施中的軌道相比，回環(huán)半徑偏小。在高難度的回環(huán)運(yùn)動(dòng)中，由于軌道游樂(lè)設(shè)施進(jìn)入回環(huán)前的沖擊震動(dòng)干擾了手機(jī)內(nèi)傳感器的采集，使得解算軌跡產(chǎn)生誤差，總體來(lái)說(shuō)擬合程度良好，基本上還原了軌道游樂(lè)設(shè)施的運(yùn)動(dòng)軌跡，能夠真實(shí)反映運(yùn)動(dòng)軌跡。

為更直觀檢驗(yàn)三維軌跡擬合，選擇一段三維軌跡進(jìn)行測(cè)量擬合，擬合結(jié)果如圖14所示。

圖14 三維擬合軌跡

由此可見(jiàn)，軌道游樂(lè)設(shè)施下滑階段的運(yùn)動(dòng)軌跡相應(yīng)的保持一定的弧度，對(duì)應(yīng)的角度與實(shí)際軌道地面夾角基本一致，能夠正確直觀反映軌道游樂(lè)設(shè)施的運(yùn)動(dòng)軌跡，證明了方法的有效性。

4 結(jié)束語(yǔ)

針對(duì)目前軌道式游樂(lè)設(shè)施物理樣機(jī)運(yùn)動(dòng)狀態(tài)研究分析的不足，開(kāi)發(fā)安卓程序采集手機(jī)內(nèi)置慣性元件及GPS的測(cè)量數(shù)據(jù)，通過(guò)Matlab軟件對(duì)數(shù)據(jù)進(jìn)行處理，設(shè)計(jì)姿態(tài)求解算法以及軌跡測(cè)量算法，進(jìn)行卡爾曼濾波、姿態(tài)求解與組合導(dǎo)航，將處理的軌跡數(shù)據(jù)通過(guò)UG軟件二次開(kāi)發(fā)實(shí)現(xiàn)運(yùn)行可視化。

經(jīng)過(guò)試驗(yàn)證明，該方法可用于軌道式游樂(lè)設(shè)施的加速度、速度、姿態(tài)、軌跡測(cè)量，彌補(bǔ)軌道類游樂(lè)設(shè)施物理樣機(jī)運(yùn)動(dòng)數(shù)據(jù)采集、測(cè)量的不足。通過(guò)手機(jī)測(cè)量，提高了數(shù)據(jù)采集的便攜性，為虛擬樣機(jī)運(yùn)動(dòng)分析與物理樣機(jī)運(yùn)動(dòng)分析之間的聯(lián)系與對(duì)比提供橋梁，提高了軌道游樂(lè)設(shè)施的安全性、可靠性。