基于ARM的移動(dòng)機(jī)器人組合導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)*

曹 超，馬 磊，趙 舵，李 應(yīng)

(西南交通大學(xué)電氣學(xué)院，四川成都610031)

0 引言

對(duì)自主性、可靠性、精度等要求較高的室外自主移動(dòng)機(jī)器人運(yùn)用場(chǎng)合，常使用多傳感器組合導(dǎo)航系統(tǒng)。慣性導(dǎo)航系統(tǒng)(INS)是一種能夠提供全面的導(dǎo)航信息，完全自主的導(dǎo)航系統(tǒng)，但其誤差隨時(shí)間不斷累計(jì)。全球衛(wèi)星定位系統(tǒng)(GPS)定位范圍廣、精度高，誤差不隨時(shí)間累計(jì)，其不足之處在于自主性和可靠性差，信號(hào)易受外界遮擋和干擾，接收機(jī)數(shù)據(jù)更新率低。INS/GPS組合導(dǎo)航系統(tǒng)利用INS和GPS互補(bǔ)的特點(diǎn)，繼承了2個(gè)獨(dú)立系統(tǒng)的優(yōu)點(diǎn)，被認(rèn)為是導(dǎo)航領(lǐng)域最理想的組合方式，成為當(dāng)前研究熱點(diǎn)。隨著組合水平的提高，INS與GPS之間相互輔助，傳遞使用信息的加強(qiáng)，組合系統(tǒng)的總體性能遠(yuǎn)遠(yuǎn)高于獨(dú)立系統(tǒng)［1，2］。

1 硬件系統(tǒng)

為了實(shí)現(xiàn)室外移動(dòng)機(jī)器的導(dǎo)航定位需求，需確定其精確的運(yùn)動(dòng)狀態(tài)信息，通常包括三軸加速度信息、速度信息、位置信息、姿態(tài)信息。一般的，GPS可以提供載體的運(yùn)行速度和位置(經(jīng)緯度和高度)信息，慣性測(cè)量單元(IMU)可以得到三軸角速度和加速度信息，通過(guò)積分后可得到姿態(tài)與速度、位置信息。室外移動(dòng)機(jī)器人組合導(dǎo)航系統(tǒng)，除了考慮導(dǎo)航精度外，還考慮導(dǎo)航系統(tǒng)成本和體積重量，故INS一般采用成本低、體積小、質(zhì)量輕的微電子機(jī)械系統(tǒng)—慣性測(cè)量單元(mirco-electro-mechanical systems-inertial measurement unit，MEMS-IMU)構(gòu)成的捷聯(lián)慣性導(dǎo)航系統(tǒng)(strap-down inertial navigation system，SINS)。而GPS采用體積小、精度高的OEM板卡。根據(jù)以上原則選取硬件。

1)IMU ADIS16365:ADI公司MEMSIMU ADIS16365是一種由三軸陀螺儀和三軸加速度計(jì)組成的六自由度慣性感應(yīng)系統(tǒng)，它能夠準(zhǔn)確地測(cè)量運(yùn)動(dòng)物體繞三軸的角速度和加速度，在醫(yī)療器械，平臺(tái)控制，機(jī)器人技術(shù)和導(dǎo)航領(lǐng)域有著廣泛的應(yīng)用。該IMU內(nèi)部采樣率高達(dá)819.2 Hz，陀螺儀量程有 ±300°/s，±150°/s，±75°/s3個(gè)量程;加速度計(jì)分辨率為3.33 m gn。

2)磁力計(jì)GPSHMC5883L:HMC5833L是一種體積小、集成度高的三軸磁傳感器，用于測(cè)量地球磁場(chǎng)分量。三軸磁阻傳感器和ASIC被封裝在3.0 mm×3.0 mm×0.9 mm LCC表面裝配中，采用I2C接口，具有12 bit ADC數(shù)字輸出功能。

3)GPSCrescent SX—2A:GPS 采用 Hemisphere公司2006年推出的一種GPS OEM模塊SX—2A。該模塊是一塊單頻12通道接收機(jī)，單點(diǎn)定位精度小于2.5 m，差分模式定位精度小于0.5 m。模塊接上電源電線即可工作，數(shù)據(jù)通過(guò)RS—232接口進(jìn)行通信。

4)處理器STM32F407VGT:處理器 STM32F407VGT是ST公司于2011年9月出的一種低功耗、高性能工業(yè)級(jí)最新處理器。內(nèi)核使用ARM Cortex M—4內(nèi)核，核心處理速度可達(dá)168 MHz，210DMIPS。與 M—3系列相比，M—4增加了單周期DSP指令和浮點(diǎn)運(yùn)算單元(FPU)功能，能夠滿足捷聯(lián)慣性解算與組合導(dǎo)航中的大量浮點(diǎn)運(yùn)算需求。

組合導(dǎo)航系統(tǒng)的硬件設(shè)計(jì)按照功能可以劃分7個(gè)模塊:微慣性測(cè)量模塊、GPS接收機(jī)模塊、數(shù)據(jù)采集模塊、數(shù)據(jù)存儲(chǔ)模塊、數(shù)據(jù)顯示模塊、導(dǎo)航計(jì)算機(jī)模塊和電源模塊。系統(tǒng)的總體結(jié)構(gòu)如圖1所示。

圖1 組合導(dǎo)航系統(tǒng)總體結(jié)構(gòu)圖Fig 1 Overall structure diagram of integrated navigation system

由于采用低精度MEMS—IMU，無(wú)法感知地球自轉(zhuǎn)角速度，不能利用IMU自身進(jìn)行初始姿態(tài)對(duì)準(zhǔn)，所以，加入三軸磁力計(jì)HMC5883L，結(jié)合三軸重力感應(yīng)計(jì)，確定初始姿態(tài)和航向角信息。

2 INS/GPS組合導(dǎo)航

2.1 Kalman濾波

本文采用位置速度組合模式［3］，線性Kalman濾波，其原理框圖如圖2所示。

圖2 基于位置速度組合Kalman濾波框圖Fig 2 Block diagram of Kalman filtering based on location and velocity combination

濾波器以INS誤差方程作為狀態(tài)方程，GPS和INS輸出東北天坐標(biāo)系位置、速度差值作為量測(cè)值，經(jīng)Kalman濾波估計(jì)INS誤差，然后對(duì)INS輸出參數(shù)進(jìn)行校正。這種組合方式簡(jiǎn)單，有一定的冗余度，工程上易于實(shí)現(xiàn)，是目前運(yùn)用最多的組合方式。IMU數(shù)據(jù)經(jīng)過(guò)預(yù)處理(包含剔除野值、數(shù)字濾波、誤差補(bǔ)償?shù)阮A(yù)處理)后［4］，捷聯(lián)解算得出最佳導(dǎo)航參數(shù)。

選取東北天坐標(biāo)系，以慣導(dǎo)系統(tǒng)平臺(tái)角誤差、速度誤差、位置誤差、陀螺和加速度計(jì)誤差作為狀態(tài)變量，忽略天向速度和位置分量，選取狀態(tài)方程如下［5］

其中，δφE，δφN，δφU為平臺(tái)角誤差，δVE，δVN為速度誤差，δλ，δL 為位置誤差，εrx，εry，εrz為陀螺一階馬爾可夫噪聲，εbx，εby，εbz為陀螺隨機(jī)漂移，Δ x，Δy，Δz為加速度計(jì)隨機(jī)漂移。

將INS輸出位置速度信息與GPS輸出相減得到組合導(dǎo)航系統(tǒng)觀測(cè)方程如下

系統(tǒng)噪聲和測(cè)量噪聲為

其中，W(t)方差矩陣 Q(t)，V(t)方差矩陣 R(t)。W(t)，V(t)符合高斯統(tǒng)計(jì)特性，通過(guò)實(shí)驗(yàn)統(tǒng)計(jì)分析得出。系統(tǒng)白噪聲和測(cè)量白噪聲方差矩陣為

其中，σrx，σry，σrz為陀螺一階馬爾可夫驅(qū)動(dòng)白噪聲方差;σgx，σgy，σgz為陀螺隨機(jī)白噪聲方差;Trx，Try，Trz陀螺一階馬爾可夫相關(guān)時(shí)間;σax，σay，σaz為加速度計(jì)隨機(jī)白噪聲方差，Tay，Tay，Taz為加速度計(jì)一階馬爾可夫過(guò)程相關(guān)時(shí)間;σλ，σL為 GPS 位置誤差白噪聲方差;σve，σvn為 GPS 速度誤差白噪聲方差。將以上建立的隨機(jī)線性系統(tǒng)式(1)、式(3)的狀態(tài)方程離散化后，按照Kalman濾波器的計(jì)算步驟遞歸更新［6］。

主程序開(kāi)始后初始化配置，進(jìn)行初始對(duì)準(zhǔn)，確定初始姿態(tài)、速度、位置等信息。GPS數(shù)據(jù)中斷優(yōu)先級(jí)高于IMU數(shù)據(jù)，GPS信號(hào)有效時(shí)輸出數(shù)據(jù)與相鄰IMU輸出導(dǎo)航參數(shù)進(jìn)行Kalman數(shù)據(jù)融合，濾波器輸出校正INS輸出導(dǎo)航參數(shù);GPS信號(hào)無(wú)效時(shí)，INS單獨(dú)導(dǎo)航。

2.2 仿真測(cè)試

為了驗(yàn)證上述方案，采集靜止?fàn)顟B(tài)下500 s數(shù)據(jù)在Matlab上進(jìn)行分析，IMU采樣率為81.92 Hz，GPS采樣率為1 Hz，組合周期為1 s。初值設(shè)定如下:經(jīng)度103°59'15.9″;緯度30°40'6.6″;東向速度0 m/s;北向速度0 m/s;俯仰角2°;橫滾角0°;航向角156.2°，誤差初值假設(shè)均為 0。實(shí)驗(yàn)結(jié)果的各導(dǎo)航參數(shù)估計(jì)誤差曲線如圖3、圖4和圖5所示。

圖3 位置誤差曲線比較Fig 3 Comparison of position error curve

圖4 速度誤差曲線比較Fig 4 Comparison of velocity error curve

圖5 組合導(dǎo)航姿態(tài)誤差曲線Fig 5 Attitude error curve of integrated navigation

由圖3可知，INS單獨(dú)導(dǎo)航時(shí)，短時(shí)精度較高，但誤差由于時(shí)間累計(jì)發(fā)散，幾分鐘內(nèi)位置誤差甚至達(dá)到幾百米至幾千米，不適合于長(zhǎng)時(shí)間定位，而INS/GPS組合導(dǎo)航時(shí)，位置誤差得到校正呈收斂狀，定位精度得到了明顯提高;由圖4可看出:組合導(dǎo)航時(shí)，速度誤差曲線收斂，優(yōu)于INS單獨(dú)導(dǎo)航。由圖5所示，組合導(dǎo)航姿態(tài)角誤差曲線，在大約40 s時(shí)穩(wěn)定。

3 實(shí)驗(yàn)測(cè)試

該組合導(dǎo)航系統(tǒng)安裝于旅行者II號(hào)移動(dòng)機(jī)器人，繞某教學(xué)樓進(jìn)行實(shí)驗(yàn)。小車速度設(shè)置為1.32 m/s，沿道路中線運(yùn)動(dòng)，繞教學(xué)樓一周。由GPS提供初始位置與標(biāo)稱軌跡，評(píng)估組合導(dǎo)航系統(tǒng)性能，根據(jù)靜止時(shí)ADIS16365加速度計(jì)數(shù)據(jù)與HMC5883L數(shù)據(jù)解出載體初始姿態(tài)與航向角，經(jīng)組合導(dǎo)航后解算軌跡如圖6所示。

圖6 GPS和INS/GPS輸出軌跡對(duì)比Fig 6 Comparison of output trajectory INS/GPS and GPS

4 結(jié)論

該嵌入式車載組合導(dǎo)航系統(tǒng)以嵌入式ARM為硬件平臺(tái)，采用Kalman濾波組合導(dǎo)航信息融合算法，提高了系統(tǒng)導(dǎo)航的可靠性和精度。嵌入式導(dǎo)航系統(tǒng)采用GPS輔助INS，能夠提供連續(xù)全面的導(dǎo)航參數(shù)，在短期GPS信號(hào)失鎖情況下，純慣性導(dǎo)航仍然在一段時(shí)間內(nèi)(約15s)保持導(dǎo)航精度，使該系統(tǒng)具有一定的抗干擾能力。實(shí)際實(shí)驗(yàn)測(cè)試表明:該系統(tǒng)能夠較好地跟蹤姿態(tài)速度位置等狀態(tài)信息，滿足對(duì)室外移動(dòng)機(jī)器人實(shí)時(shí)定位功能，達(dá)到了設(shè)計(jì)要求。

［1］秦永元.慣性導(dǎo)航［M］.北京:科學(xué)出版社，2006:12-36.

［2］李寶林，李勇建，劉 勇，等.INS/GPS多傳感器車載組合導(dǎo)航系統(tǒng)算法研究［J］.壓電與聲光，2012(1):37-41，80.

［3］尚 捷，顧啟泰.MIMS/GPS組合導(dǎo)航系統(tǒng)中卡爾曼濾波器設(shè)計(jì)與實(shí)驗(yàn)研究［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2005(2):45-48，53.

［4］趙偉臣，付夢(mèng)印，張啟鴻，等.微機(jī)械IMU數(shù)據(jù)建模與濾波方法研究［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2005(6):13-17.

［5］林敏敏，房建成，高國(guó)江.GPS/SINS組合導(dǎo)航系統(tǒng)混合校正卡爾曼濾波方法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2003(3):30-34.

［6］卞鴻巍，李 安，覃方君.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用［M］.北京:國(guó)防工業(yè)出版社，2010:70-73.