楊 歡， 熊 劍， 郭 杭， 衷衛(wèi)聲

(1.南昌大學(xué) 機(jī)電工程學(xué)院，江西 南昌 330031；2.南昌大學(xué) 信息工程學(xué)院 測控教研室，江西 南昌 330031；3.南昌大學(xué) 空間科學(xué)與技術(shù)研究院，江西 南昌 330031)

基于Cortex-M4的多傳感器組合導(dǎo)航系統(tǒng)設(shè)計*

楊 歡1， 熊 劍2， 郭 杭3， 衷衛(wèi)聲2

(1.南昌大學(xué) 機(jī)電工程學(xué)院，江西 南昌 330031；2.南昌大學(xué) 信息工程學(xué)院 測控教研室，江西 南昌 330031；3.南昌大學(xué) 空間科學(xué)與技術(shù)研究院，江西 南昌 330031)

針對移動機(jī)器人導(dǎo)航定位需求，設(shè)計了一種低成本、便攜式的多傳感器組合導(dǎo)航系統(tǒng)。采用ARM Cortex—M4內(nèi)核的STM32F407處理器為電路核心，利用微慣性測量單元(MIMU) 、全球衛(wèi)星定位系統(tǒng)(GPS)以及電子羅盤作為導(dǎo)航數(shù)據(jù)源，進(jìn)行了相關(guān)的硬件設(shè)計；采用移動導(dǎo)航平臺進(jìn)行實驗驗證，通過實測數(shù)據(jù)進(jìn)行分析，結(jié)果表明：該組合導(dǎo)航系統(tǒng)可以有效提高導(dǎo)航精度，使其導(dǎo)航定位經(jīng)緯度誤差穩(wěn)定在1.0 m左右，航向角誤差穩(wěn)定在0.8°以內(nèi)，對移動機(jī)器人導(dǎo)航有一定的參考價值。

移動機(jī)器人； 微慣性測量單元(MIMU)； 全球定位系統(tǒng)(GPS)； 電子羅盤； 組合導(dǎo)航

0 引 言

為了實現(xiàn)移動機(jī)器人能夠自主、穩(wěn)定并且滿足一定的導(dǎo)航精度要求來完成某些特定的任務(wù)，在工程應(yīng)用中常采用多傳感器組合導(dǎo)航系統(tǒng)。全球定位系統(tǒng)(GPS)的導(dǎo)航誤差不隨時間積累，受氣候天氣影響較小，其不足之處在于自主性和可靠性差，信號易受到障礙物影響而大大衰減，接收機(jī)的數(shù)據(jù)更新率低[1]。捷聯(lián)式慣性導(dǎo)航系統(tǒng)(SINS)具有抗干擾性強(qiáng)、自主性高、導(dǎo)航信息更新率快等優(yōu)點，但微慣性器件的精度不高，并且其誤差會隨著時間積累，不適用長時間的工作[2]。隨著微電子集成技術(shù)以及加工工藝、材料技術(shù)的不斷發(fā)展，三維電子羅盤克服了平面電子羅盤在使用中的嚴(yán)格限制，在三維電子羅盤內(nèi)部加入傾角傳感器，當(dāng)電子羅盤發(fā)生傾斜時可以對羅盤進(jìn)行傾斜補(bǔ)償。因此，即使羅盤發(fā)生傾斜，航向數(shù)據(jù)依然準(zhǔn)確無誤[3]。

目前，利用GPS和SINS組合設(shè)計的導(dǎo)航系統(tǒng)使得移動機(jī)器人能夠出色地在室外空曠地區(qū)完成作業(yè)[4]。隨著城市的建筑物越建越高，基礎(chǔ)建設(shè)的復(fù)雜多樣化，移動機(jī)器人在這樣的環(huán)境下完成特定任務(wù)將十分困難[5]。為解決移動機(jī)器人在復(fù)雜環(huán)境下導(dǎo)航航向精度問題，本文設(shè)計了多傳感器移動機(jī)器人導(dǎo)航系統(tǒng)，利用得到的航向信息、速度信息、位置信息，并對微慣性測量單元(MIMU)和電子羅盤進(jìn)行了溫度補(bǔ)償，結(jié)合設(shè)計的組合導(dǎo)航系統(tǒng)信息融合算法，對多源信息進(jìn)行了合理的、有效的組合配置，總體上提高了移動機(jī)器人導(dǎo)航系統(tǒng)的穩(wěn)定性，能滿足其一定的航向精度要求。

1 導(dǎo)航方法與信息融合

由MEMS型的MIMU、GPS接收機(jī)以及電子羅盤設(shè)計構(gòu)成組合導(dǎo)航系統(tǒng)，通過數(shù)據(jù)融合解算，系統(tǒng)采用東北天地理坐標(biāo)系。在該系統(tǒng)GPS接收機(jī)信號良好的情況下，GPS可以實時獲得載體的位置信息、速度信息以及時間信息[6]；而SINS利用陀螺儀和加速度計獲得載體的角度信息和加速度信息，通過四元數(shù)法解算出載體的姿態(tài)信息；由電子羅盤傳感器測得數(shù)據(jù)通過解算得到航向角信息，與SINS輸出的航向角做差作為航向角誤差的觀測量。

在信息融合處理過程建立了組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型的狀態(tài)方程，這只與慣性儀表誤差模型有關(guān)，因此，在GPS信號有無的情況下該狀態(tài)模型是不變的。

1)其中組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型狀態(tài)方程為

X(t)=F(t)X(t)+G(t)W(t)

(1)

式中X為系統(tǒng)狀態(tài)矢量

X=[φE,φN,φU,δVE,δVN,δVU",δL,δλ,δh,εbx,εby,εbz,

(2)

F(t)和G(t)分別為系統(tǒng)方程狀態(tài)系數(shù)矩陣和誤差系數(shù)矩陣，這里不作詳述。

2)組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型量測方程

利用GPS進(jìn)行速度、位置的松組合工作方式，其量測方程為

(3)

式中 PIMU,VIMU,φIMU為捷聯(lián)慣性導(dǎo)航系統(tǒng)輸出的位置、速度和姿態(tài)信息；PGPS,VGPS為GPS信息輸出的位置、速度信息，φEC為電子羅盤解算的姿態(tài)信息

(4)

式中 L為緯度，RM,RN分別為卯酉圈和子午圈曲率半徑。觀測噪聲V(t)為隨機(jī)白噪聲。

在ARM平臺中，必須將連續(xù)系統(tǒng)進(jìn)行離散化方可進(jìn)行信息融合處理[8]，結(jié)合組合導(dǎo)航量測方程和觀測方程對系統(tǒng)進(jìn)行離散化可得

(5)

2 系統(tǒng)總體方案設(shè)計

2.1 硬件平臺設(shè)計

系統(tǒng)結(jié)構(gòu)原理如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)原理圖

系統(tǒng)以微慣性測量單元為核心傳感器，GPS和電子羅盤加以輔助確保移動機(jī)器人能獲得較好的導(dǎo)航和定位精度。

1)MEMS 慣性器件：系統(tǒng)采用的慣性器件是ADI公司的微慣性測量單元ADIS16365，其具有成本低、功耗低、體積小等優(yōu)點。ADIS16365器件是完整的慣性系統(tǒng)，內(nèi)置一個三軸陀螺儀和一個三軸加速度計。它結(jié)合了業(yè)界領(lǐng)先的iMEMS技術(shù)與優(yōu)化動態(tài)性能信號調(diào)理功能，在平臺控制、機(jī)器人技術(shù)和導(dǎo)航領(lǐng)域有著廣泛的應(yīng)用。

2)電子羅盤芯片:系統(tǒng)選用了霍尼韋爾公司的HMC5983，其內(nèi)置高分辨的HMC118X系列的磁阻傳感器，包含了 Honeywell 公司的先進(jìn)的工藝，具有自動消磁帶、偏置帶和一個12位ADC，數(shù)據(jù)更新率達(dá)220 Hz，并且?guī)囟妊a(bǔ)償，通過I2C或者SPI進(jìn)行通信。

3)GPS模塊采用的u-blox LEA—5T:該接收機(jī)模塊能夠提供偽距、載波相位等原始數(shù)據(jù)，擁有強(qiáng)大的在線配置功能，同時也可以提供位置、速度和時間等結(jié)果。它的位置測量精度為：2.5 m(CEP)，數(shù)據(jù)通過RS—232通信。

4)電路采用了ARM Cortex—M4為內(nèi)核的STM32F407處理器:這是由ST(意法半導(dǎo)體)公司推出的低功耗、高性能的微處理器，其內(nèi)核主頻高達(dá)168 MHz，具有單周期數(shù)字信號處理(DSP)指令和浮點運(yùn)算單元(FPU)功能，可以較好地完成捷聯(lián)式慣性導(dǎo)航數(shù)據(jù)解算和數(shù)據(jù)融合處理時大量浮點運(yùn)算的需求。

2.2 導(dǎo)航系統(tǒng)軟件設(shè)計

多傳感器組合導(dǎo)航軟件的設(shè)計采用了模塊化的設(shè)計思路，主要包括數(shù)據(jù)接收模塊、導(dǎo)航解算、卡爾曼濾波和數(shù)據(jù)保存和顯示模塊。采用C語言進(jìn)行編程，導(dǎo)航解算時需要進(jìn)行大量的矩陣運(yùn)算，為能保證組合導(dǎo)航系統(tǒng)的實時性要求，結(jié)合微處理器的性能優(yōu)勢，本系統(tǒng)對傳感器數(shù)據(jù)采集的過程全部使用狀態(tài)機(jī)+硬件直接內(nèi)存存取(direct memory access，DMA)的方式實現(xiàn)，能在傳輸大量數(shù)據(jù)時幾乎不占用CPU資源。由于無須CPU干預(yù)，數(shù)據(jù)可以通過DMA快速地移動，節(jié)省了CPU的資源來做其他操作。

在數(shù)據(jù)融合處理過程中，因載體與ADIS16365直接固聯(lián)，故當(dāng)載體發(fā)生不同的振動時，這將引起在計算姿態(tài)和速度時產(chǎn)生圓錐誤差和劃槳誤差。該組合導(dǎo)航系統(tǒng)采用了圓錐誤差和劃槳誤差補(bǔ)償?shù)姆椒ㄟM(jìn)行捷聯(lián)解算得到載體的姿態(tài)和速度信息。針對航向角容易發(fā)散的問題，利用電子羅盤數(shù)據(jù)進(jìn)行了誤差補(bǔ)償。軟件整體流程圖如圖2所示。

圖2 組合導(dǎo)航系統(tǒng)算法框圖

3 實驗結(jié)果分析

3.1 靜態(tài)性能測試

將組合導(dǎo)航計算機(jī)固定于水平放置的移動導(dǎo)航平臺上，測定了移動導(dǎo)航平臺的定點位置、速度、姿態(tài)信息。將GPS天線引出到空曠無遮擋物處，進(jìn)行了5次的靜態(tài)實驗，每組實驗進(jìn)行時間5 min。存儲采集得到的數(shù)據(jù)，將姿態(tài)信息、位置信息的均值、最值整理如表1所示。

從表1中可以看出，在靜態(tài)時測得的各項導(dǎo)航數(shù)據(jù)都能穩(wěn)定的輸出，系統(tǒng)可解算出位置精度約為1.2 m，解算的速度精度值約為0.05 m/s，在這里對高度數(shù)據(jù)不予分析。系統(tǒng)的橫滾、俯仰角均值都在0.6°左右，航向角數(shù)據(jù)也比較穩(wěn)定輸出。

3.2 動態(tài)航向?qū)嶒?/p>

移動機(jī)器人導(dǎo)航系統(tǒng)通過靜態(tài)初始對準(zhǔn)，在室外進(jìn)行實驗。在實驗中移動機(jī)器人行駛速度設(shè)置為0.86 m/s，繞行測試時間1 500 s，測試期間所有采集到的數(shù)據(jù)存儲于SD卡中。實驗結(jié)束后，將采集到的數(shù)據(jù)通過Matlab軟件進(jìn)行作圖，實驗中系統(tǒng)姿態(tài)誤差和位置誤差的誤差曲線如圖3,圖4所示。

表1 靜態(tài)性能測試

均值最大值最小值標(biāo)準(zhǔn)差經(jīng)度/(°)115．794718115．7947610115．79466341．22m緯度/(°)28．66417128．664193328．66413280．53m東速/(m/s)0．01830．0312-0．06120．0210北速/(m/s)-0．02020．0521-0．06020．0243橫滾角/(°)-0．4521-0．0185-0．61260．1151俯仰角/(°)-0．39280．2061-0．67380．0962航向角/(°)88．936289．692387．96420．7130

圖3 姿態(tài)誤差曲線

圖4 位置誤差曲線圖

從圖3可以看出，利用SINS/GPS/電子羅盤結(jié)合起來構(gòu)成組合導(dǎo)航系統(tǒng)，移動機(jī)器人的航向角誤差有非常顯著的減小，其俯仰角和橫滾角誤差范圍為1.0°以內(nèi)，航向角精度控制在0.8°左右；移動機(jī)器人的位置誤差也得到了明顯的收斂。實驗結(jié)果表明：對移動機(jī)器人平臺而言，采用該組合導(dǎo)航系統(tǒng)能提供一種高精度航向角的方法，能有效抑制系統(tǒng)航向角誤差，進(jìn)而提高組合導(dǎo)航系統(tǒng)的濾波精度。

4 結(jié)束語

為滿足移動機(jī)器人自主導(dǎo)航的任務(wù)需求，基于ARM-Cortex-M4為內(nèi)核的32位微處理器設(shè)計一種低成本、便攜式的多傳感器組合導(dǎo)航計算機(jī)，能有效提高移動機(jī)器人自主導(dǎo)航精度。闡述了多傳感器組合導(dǎo)航系統(tǒng)的設(shè)計方法，融入了基于多傳感器信息融合的卡爾曼濾波算法。在多傳感器數(shù)據(jù)采集過程中采用了硬件DMA方式，旨在為移動機(jī)器人導(dǎo)航平臺提供一種有效實時的導(dǎo)航信息。系統(tǒng)硬件電路設(shè)計體積小、功耗低，具有使用靈活便捷、性能穩(wěn)定的優(yōu)點。所建立的多源信息融合下的組合模型具有一定的工程應(yīng)用價值。

[1] 包 超,郭美鳳,周 斌,等.MIMU/GPS組合導(dǎo)航系統(tǒng)小型化設(shè)計[J].傳感器與微系統(tǒng),2014,33(1):116-119.

[2] 田增山,李 鎮(zhèn),王海涌,等.一種彈道導(dǎo)彈捷聯(lián)慣導(dǎo)/地磁組合導(dǎo)航方法[J].中國慣性技術(shù)學(xué)報,2015,23(5):636-641.

[3] Khoder W,Jida B.A quaternion scaled unscented Kalman estimator for inertial navigation states determination using INS/GPS/magnetometer fusion[J].Journal of Sensor Technology,2014,4(2):101.

[4] 曹 超,馬 磊,趙 舵,等.基于ARM的移動機(jī)器人組合導(dǎo)航系統(tǒng)設(shè)計與實現(xiàn)[J].傳感器與微系統(tǒng),2013,32(2):102-104.

[5] 劉峰麗.MIMU/GPS/磁力計單兵系統(tǒng)組合導(dǎo)航技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué),2013.

[6] El-Shafie A,Najah A,Karim O A.Amplified wavelet-ANFIS-based model for GPS/INS integration to enhance vehicular navigation system[J].Neural Computing and Applications,2014,24(7-8):1905-1916.

Design of multi-sensor integrated navigation system based on Cortex-M4*

YANG Huan1, XIONG Jian2, GUO Hang3, ZHONG Wei-sheng2

(1.College of Mechanical and Electrical Engineering,Nanchang University,Nanchang 330031,China； 2.Department of Measuring & Control Engineering,School of Informaition Engineering,Nanchang University,Nanchang 330031,China；3.Institute of Space Science and Technology,Nanchang University,Nanchang 330031,China)

In view of demand for navigation and positioning of mobile robot,a low cost and portable multi-sensor integrated navigation system is designed.The design uses the processor named STM32F407,whose kernel is ARM Cortex-M4,as the core of the hardware circuit.The system uses micro inertial measurement unit(MIMU),global positioning system(GPS)and electronic compass as navigation data source,and hardware design is carried out.Experimental verification is performed with mobile navigation platform, and experimental data is analyzed,the results show that the integrated navigation system can effectively improve the navigation precision,and manages to stable the navigation and positioning error of latitude and longitude at about 1.0 m,also heading angle error is stable within 0.8°,it has certain reference value for mobile robot navigation.

mobile robot; micro inertial measurement unit(MIMU); global positioning system(GPS); electronic compass; integrated navigation

2016—03—18

國家自然科學(xué)基金資助項目(41374039,41164001)

10.13873/J.1000—9787(2017)02—0088—03

TN 96

A

1000—9787(2017)02—0088—03

楊 歡(1991-)，男，碩士研究生，從事多傳感器組合導(dǎo)航以及數(shù)據(jù)融合處理方面的研究。

熊 劍(1977-)，男，通訊作者，博士，講師，從事多源信息融合、非線性濾波方面研究工作，E—mail：xiongjian@ncu.edu.cn。