基于Cortex-M4的多傳感器組合導航系統設計*

楊 歡， 熊 劍， 郭 杭， 衷衛聲

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

基于Cortex-M4的多傳感器組合導航系統設計*

楊 歡1， 熊 劍2， 郭 杭3， 衷衛聲2

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

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

移動機器人； 微慣性測量單元(MIMU)； 全球定位系統(GPS)； 電子羅盤； 組合導航

0 引 言

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

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

1 導航方法與信息融合

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

在信息融合處理過程建立了組合導航系統數學模型的狀態方程，這只與慣性儀表誤差模型有關，因此，在GPS信號有無的情況下該狀態模型是不變的。

1)其中組合導航系統數學模型狀態方程為

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

(1)

式中X為系統狀態矢量

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

(2)

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

2)組合導航系統數學模型量測方程

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

(3)

式中 PIMU,VIMU,φIMU為捷聯慣性導航系統輸出的位置、速度和姿態信息；PGPS,VGPS為GPS信息輸出的位置、速度信息，φEC為電子羅盤解算的姿態信息

(4)

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

在ARM平臺中，必須將連續系統進行離散化方可進行信息融合處理[8]，結合組合導航量測方程和觀測方程對系統進行離散化可得

(5)

2 系統總體方案設計

2.1 硬件平臺設計

系統結構原理如圖1所示。

圖1 系統結構原理圖

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

1)MEMS 慣性器件：系統采用的慣性器件是ADI公司的微慣性測量單元ADIS16365，其具有成本低、功耗低、體積小等優點。ADIS16365器件是完整的慣性系統，內置一個三軸陀螺儀和一個三軸加速度計。它結合了業界領先的iMEMS技術與優化動態性能信號調理功能，在平臺控制、機器人技術和導航領域有著廣泛的應用。

2)電子羅盤芯片:系統選用了霍尼韋爾公司的HMC5983，其內置高分辨的HMC118X系列的磁阻傳感器，包含了 Honeywell 公司的先進的工藝，具有自動消磁帶、偏置帶和一個12位ADC，數據更新率達220 Hz，并且帶溫度補償，通過I2C或者SPI進行通信。

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

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

2.2 導航系統軟件設計

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

在數據融合處理過程中，因載體與ADIS16365直接固聯，故當載體發生不同的振動時，這將引起在計算姿態和速度時產生圓錐誤差和劃槳誤差。該組合導航系統采用了圓錐誤差和劃槳誤差補償的方法進行捷聯解算得到載體的姿態和速度信息。針對航向角容易發散的問題，利用電子羅盤數據進行了誤差補償。軟件整體流程圖如圖2所示。

圖2 組合導航系統算法框圖

3 實驗結果分析

3.1 靜態性能測試

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

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

3.2 動態航向實驗

移動機器人導航系統通過靜態初始對準，在室外進行實驗。在實驗中移動機器人行駛速度設置為0.86 m/s，繞行測試時間1 500 s，測試期間所有采集到的數據存儲于SD卡中。實驗結束后，將采集到的數據通過Matlab軟件進行作圖，實驗中系統姿態誤差和位置誤差的誤差曲線如圖3,圖4所示。

表1 靜態性能測試

均值最大值最小值標準差經度/(°)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 姿態誤差曲線

圖4 位置誤差曲線圖

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

4 結束語

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

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

[2] 田增山,李 鎮,王海涌,等.一種彈道導彈捷聯慣導/地磁組合導航方法[J].中國慣性技術學報,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].傳感器與微系統,2013,32(2):102-104.

[5] 劉峰麗.MIMU/GPS/磁力計單兵系統組合導航技術研究[D].哈爾濱：哈爾濱工程大學,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

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

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

TN 96

A

1000—9787(2017)02—0088—03

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

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