STM32自主定位的小車導航系統設計

王玥，張海

(北京航空航天大學 自動化科學與電氣工程學院，北京 100191)

?

王玥，張海

(北京航空航天大學 自動化科學與電氣工程學院，北京 100191)

摘要:設計了基于STM32F103VET6的自主定位四輪差速小車。為單軸陀螺儀ADIS16265、集成慣性測量單元MPU-9150及超聲波測距模塊設計了合理的數據采集與處理方法，完成小車短時間內高精度的航向角計算及長時間穩定的磁航向角計算，并對程序開發中遇到的堆棧溢出問題提出了解決方案。最后提出了各傳感器信息的有效融合方法，采用航位推算的方法完成小車自主定位，實現了小車長時間沿預定軌跡自主穩定行駛的功能。

關鍵詞:STM32；ADIS16265；MPU-9150；磁航向角；堆棧溢出

引言

本文采用慣性/碼盤/磁羅盤/超聲波測距組合的方式，在測量數據融合的基礎上實現小車的自主導航定位，并通過電機調速控制實現了車輛的自主定位。

1方案選擇

小車的導航與控制系統由導航/控制解算模塊、慣性/距離/測速傳感器、電機調速控制電路、WiFi無線通信模塊等構成，系統組成及主控芯片接口分配如圖1所示。

圖1　系統組成及主控芯片接口分配圖

車輛采用4輪差速控制，每個減速電機上裝有光電碼盤測量電機的實際轉速。

主控芯片在采集到傳感器數據后進行小車導航定位信息的解算。已知初始航向、位置信息，利用陀螺儀測量的航向角速度信息積分得到短時間內精確的小車航向角信息，加上光電碼盤測得的車輪轉速信息，通過航位推算的方法獲得小車定位信息。利用慣性/磁集成傳感器測量小車姿態,進而獲得磁航向，抑制慣性航向解算的誤差累積。在本文實驗環境下，還可以利用超聲波測距信息對小車位置及航向信息進行修正。

在獲得了小車的導航定位信息之后，可通過軌跡跟蹤控制規律計算電機的控制參數，實現差速控制，進而完成小車對預定軌跡的跟蹤。此外，系統還加入了無線通信模塊,實現了與PC機的無線通信，方便關鍵數據的實時上傳與分析。

2硬件設計

2.1微控制器

主控芯片選用STM32F103VET6[1]，內核為ARM 32位的Cortex-M3 CPU，最大工作頻率為72 MHz，指令執行速度達到1.25 DMIPS/MHz，片上有512 KB的Flash及64 KB的SRAM，芯片含有80個GPIO接口，提供I2C、SPI和USART接口，滿足系統與不同傳感器的通信功能及對執行單元的控制功能。

2.2慣性測量模塊設計

小車的導航與定位信息主要通過慣性測量模塊獲得。由于小車的使用環境可基本視為二維平面，即小車行走過程中的俯仰角與橫滾角可近似為零，因此為小車安裝測量軸垂直運動平面的單軸陀螺儀進行航向角速度的測量。通過單軸陀螺儀的測量值可以在短時間內解算得到精確的小車航向角信息，但由于測量誤差的累積，長時間后求得的航向角會發散，因此又為小車安裝了由三軸陀螺儀、三軸加速度計及三軸磁羅盤組成的集成慣性測量單元，利用磁航向角測量無誤差累積的特點，融合修正小車的航向角信息，使系統可以在長時間內有效地運作。

2.2.1ADIS16265接口及數據處理方法

考慮到成本及測量精度等因素，本文中的單軸陀螺儀選用ADI公司生產的ADIS16265[2]，陀螺儀工作電壓為4.75～5.25 V，提供±80/±160/±320 °/s三種量程供用戶選擇，自帶溫度補償功能，校準溫度范圍為-40~+85 ℃，可選測量帶寬為50或330 Hz，可配置陀螺儀內部采樣頻率，輸出數據可通過自帶的Bartlett窗口FIR濾波器進行處理，濾波階數可以設置，陀螺儀通過SPI通信協議輸出14位數字量角速度測量數據。

ADIS16265的接口電路略——編者注。

本文設置陀螺儀量程為±80 °/s，測量帶寬為50 Hz，陀螺儀自帶的FIR濾波器濾波階數為32階，可將數字濾波器的帶寬降至2.6 Hz。考慮到小車導航信息解算的頻率要求，經過測量與比較不同采樣頻率下陀螺儀零偏的平均值與方差，確定陀螺儀內部采樣頻率為128 Hz。主控芯片通過SPI (CPOL=1，CPHA=1，MSB first,16位) 向陀螺儀寫命令，對其進行初始化，寫入的第一個字節為寄存器地址，第二個字節為配置字，寄存器配置操作如表1所列。

陀螺儀初始化過程的具體代碼略——編者注。

ADIS16265陀螺儀在靜止情況下，128 Hz的1 h測量情況如圖2所示。多次實驗表明，在上電工作5 min后陀螺儀零偏趨于穩定，且每隔5 min需要重新標定，使用時

表1　ADIS16265寄存器配置

圖2　單軸陀螺儀零偏變化曲線

應特別注意。

為了提高計算精度，實際測試表明ADIS16265的輸出仍需要進行濾波處理，本文采用3連續點中位值濾波，然后對4連續點中位值進行平均的方式，獲得了5 min航向角漂移2°的效果。

2.2.2MPU-9150磁航向測量與校正

本文中的集成慣性測量單元選用InvenSense公司生產的MPU-9150[3]，它整合了含三軸陀螺儀與三軸加速度計的MPU-6050和三軸電子羅盤AK8975。MPU-9150采用溫度補償技術和內部濾波器，通過I2C總線接口連接微處理器，工作電壓范圍為2.375~3.465 V。MPU-9150的配置及接口電路請借鑒參考文獻[4]。

使用MPU-9150的三軸加速度計獲得元件傾角，在該傾角的基礎上利用三軸磁羅盤的磁通量測量結果計算磁航向[5]，由于傳感器受小車自身的鐵磁干擾[6]，航向計算值會偏離真值。實驗室中在無外磁場干擾、已知航向情況下，以10°為間隔對磁羅盤解算結果進行標定，獲得各方向的磁羅盤誤差，由圖3可知，各方向原始計算誤差小于2°，在實際使用中進行補償即可獲得高精度磁航向。

圖3　靜態情況下磁航向角解算精度

2.3其他硬件的設計

小車的電機選用帶334線碼盤的減速電機，型號是JGA25-371；超聲波測距模塊選擇了國產的US-100超聲波測距模塊；此外小車還安裝了有人公司生產的USR-WIFI232-T串口轉無線模塊。系統的電源供應選擇12 V鋰電池，經過電源電路變壓為3.3 V及5 V，分別為電機、主控芯片及不同的傳感器供電。

參考文獻電源電路、超聲波測距模塊切換電路、光電碼盤接口電路及電機驅動電路設計請借鑒[4]。

3軟件設計

為了精確地控制小車的運動，需要準確地解算小車導航參數，因此需要合理分配各任務的處理周期，設計各傳感器數據采集與處理的方法。

3.1處理邏輯設計

系統要完成傳感器數據采集、導航參數解算、對電機的控制等多種任務，根據任務需求為它們分配處理頻率。導航解算相關的傳感器數據采集及對電機的控制輸出采用較高的處理頻率，保證了解算與控制的精確；用于避障并輔助導航解算的超聲波測距的采樣頻率相對較低；航位推算及軌跡跟蹤參數解算的頻率適中，在確保系統正常運行的情況下降低程序計算量。

根據系統的要求，設置不同的任務周期如表2所列。

表2　系統不同任務的周期分配

為了精確控制各任務的處理時機，并設置處理周期，本文將STM32F103VET6的基本定時器6設置為10 ms的溢出中斷來完成。在中斷服務函數中使用任務周期計數器判斷是否到達各個任務的處理時間點，并進行任務處理標志位的設置。在主程序中循環檢測各個任務處理標志位情況，若標志位被置位，則執行相應的任務處理程序。具體流程如圖4所示。

圖4　系統任務處理流程圖

除了使用基本定時器6的溢出中斷，還使用了PD6雙邊沿跳變觸發的外部中斷，用于測量超聲波模塊測距的渡越時間，需要對這兩個中斷設置不同的優先級。由于對超聲波模塊渡越時間的測量過程不能被其他中斷打斷，因此將PD6引腳對應的EXTI9_5_IRQn中斷向量的優先級設置為搶占優先級最高，響應優先級最高。將TIM6的溢出中斷對應的TIM6_IRQn中斷向量的優先級設置為搶占優先級次高。

3.2慣性測量模塊數據的存儲與處理

為了提高導航解算精度，使用TIM6的10 ms定時中斷進行ADIS16265及MPU-9150的數據采集，以每條記錄的方式循環存儲，主程序查詢數據更新并進行解算。本文在程序中設置數據存儲指針與數據處理指針對此過程進行管理。

具體過程如下：在中斷服務函數中置標志位，采集慣性測量模塊數據存入緩沖區內，并使用數據存儲指針記錄存儲緩沖區空間的使用情況，每次存儲后數據存儲指針加1。在主程序中檢測標志位，取出存儲緩沖區內的數據進行處理，將數據處理指針與存儲指針進行比較，每次處理后數據處理指針加1，持續處理數據直至數據處理指針與存儲指針相等，從而保證了歷史數據能得到及時處理。

3.3超聲波模塊的數據采集與處理

與陀螺儀及磁羅盤的數據采集原理不同，本文從超聲波模塊處采集到的是測距過程中從測量聲波發射到接收的渡越時間。為了在測量渡越時間的同時不影響主程序的運行，將渡越時間的測量放在中斷中進行，具體做法是主控芯片的PD6引腳檢測超聲波ECHO引腳的電平跳變，在中斷中控制定時器的開關以實現計時。獲得的計時時間即為超聲波模塊測距的渡越時間TH，通過以下公式計算超聲波模塊測得的距障礙物的距離：

代碼略——編者注。

3.4程序穩定性設計

因存在MPU-9150、ADIS16265、超聲波測距等多個高頻率、多時間間隔、不同級別的中斷測量，大量的中斷嵌套導致了微控制器死機現象[7]。在中斷處理機制分析基礎上，采用了全局變量存儲采集數據的方式，減少了動態存儲空間分配，無需調節默認堆?？臻g大小即可實現程序的穩定運行。

4功能實現

4.1小車的慣性定位解算

已知小車初始位置及航向信息，使用單軸陀螺儀與光電碼盤測量值，通過航位推算可求出車輛行駛的軌跡。在直線行駛測試中，慣性定位解算得到的法向偏差與實際測量值相差較大。經過分析，誤差最主要的來源是積分求解的航向角隨時間發散。換言之，僅使用小車系統內部的測量信息進行慣性定位解算得到的導航參數在長時間使用中精度無法保證，因此應對外部環境的測量數據進行信息融合修正。

4.2多傳感器信息融合

本文中小車實驗的場地在樓內的回字形走廊中(尺寸為28 m×18 m)，走廊的墻壁基本可以視為光滑連續且平行。因此考慮使用超聲波模塊測量的小車距左右墻的距離信息與MPU-9150解算的磁航向角進行信息融合，提高小車長時間導航解算的精度。

在進行信息融合前需要對傳感器數據的可用性進行判斷，對于超聲波模塊測距信息，需要判斷墻壁是否連續且平行。墻壁的連續性可通過相鄰兩次超聲波測量得到的小車距左右墻距離變化是否有突變進行判別；而墻壁的平行性可通過當航向角變化較小時，超聲波測距結果的變化情況進行判別。對于磁航向角，其可用性可通過一段較短時間內，慣性求解的航向角變化量與磁航向角變化量二者之間的偏差是否超過設定閾值進行判斷。

當判斷墻壁連續且平行時，認為超聲波測距信息可用。連續存儲多組左右超聲波模塊測距結果，結合小車沿設定軌跡切線方向的前進距離信息，求解出在這段時間內小車的航向角近似值，及小車的法向偏移值，以修正慣性定位解算結果。當超聲波測距信息無效時，可使用磁航向角信息融合修正小車航向角信息。

最后，為了保證通過陀螺儀采集的數據在系統運行長時間后仍真實可信，可在系統運行一段時間后，使小車停車重新采集零偏。本文中設定當小車每跑完一圈路徑，即系統運行3 min左右后，重新采集10 s內單軸陀螺儀的數據，取平均值更新零偏。

實際跑車結果略——編者注。

可以看出，只使用碼盤及陀螺儀測量的內部信息慣性解算得到的軌跡隨著時間推移有發散的趨勢，而加入了超聲波測距模塊及磁羅盤測量的外部信息輔助修正導航參數之后，小車的行駛能夠很好地跟蹤設定的軌跡。最終小車可以沿預定軌跡自主穩定行駛40 min以上，即沿設定軌跡行駛10圈以上。

結語

本文設計了基于STM32F103VET6微控制器的小車導航與控制系統，詳細介紹了ADIS16265、MPU-9150、超聲波測距傳感器數據的有效采集與處理方法，對程序穩定性設計提出了建議，最后介紹了利用多傳感器信息融合方法選擇有效的傳感器數據實現小車的軌跡跟蹤控制的方法，達到了理想的效果。

[1] ST Microelectronics.STM32F103VE Product Specifications [EB/OL].[2015-05].http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00191185.pdf.

[2] Analog Devices.ADIS16260/ADIS16265 Data Sheet [EB/OL].[2015-05].http://www.analog.com/media/en/technical-documentation/data-sheets/ADIS16260_16265.pdf.

[3] InvenSense Inc.MPU-9150 Product Specification Revision 4.3 [EB/OL].[2015-05].http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A.pdf.

[4] 鄭潤芳,張海.STM32的小車自主定位與控制系統設計[J].單片機與嵌入式系統應用,2013(9):46-49.

[5] Caruso M J.Applications of magnetic sensors for low cost compass systems[C]//Position Location and Navigation Symposium,IEEE,2000:177-184.

[6] 蔣賢志.數字電子羅盤誤差分析及校正技術研究[J].現代雷達,2005(6):39-41,44.

[7] Joseph Yiu.ARM Cortex-M3權威指南[M].宋巖,譯.北京:北京航空航天大學出版社,2009.

Wang Yue,Zhang Hai

(School of Automation Science and Electrical Engineering,Beihang University,Beijing 100191,China)

Abstract：A self-positioning four-wheel differential speed drive vehicle based on STM32F103VET6 is designed.In the paper,the reasonable method of data acquisition and processing for single-axis gyroscope ADIS16265,integrated inertial measurement unit MPU-9150 and ultrasonic distance measuring module is designed,which is used to complete the calculation of the heading angle with high precision in short-time and the calculation of the magnetic heading which is stable in long-time.The solution for the stack overflow problem encountered in program development is proposed.Finally,the fusion method of multi-sensor information is given,and the dead-reckoning method is used to accomplish self-positioning for the vehicle.The vehicle implements the function of driving autonomously along predetermined trajectory with high stability in long-time.

Key words:STM32;ADIS16265;MPU-9150;magnetic heading;stack overflow

收稿日期：(責任編輯：薛士然2015-06-15)

中圖分類號:TP242

文獻標識碼:A