STM32自主定位的小車導(dǎo)航系統(tǒng)設(shè)計(jì)

王玥，張海

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

?

王玥，張海

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

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

關(guān)鍵詞:STM32；ADIS16265；MPU-9150；磁航向角；堆棧溢出

引言

本文采用慣性/碼盤/磁羅盤/超聲波測(cè)距組合的方式，在測(cè)量數(shù)據(jù)融合的基礎(chǔ)上實(shí)現(xiàn)小車的自主導(dǎo)航定位，并通過(guò)電機(jī)調(diào)速控制實(shí)現(xiàn)了車輛的自主定位。

1方案選擇

小車的導(dǎo)航與控制系統(tǒng)由導(dǎo)航/控制解算模塊、慣性/距離/測(cè)速傳感器、電機(jī)調(diào)速控制電路、WiFi無(wú)線通信模塊等構(gòu)成，系統(tǒng)組成及主控芯片接口分配如圖1所示。

圖1　系統(tǒng)組成及主控芯片接口分配圖

車輛采用4輪差速控制，每個(gè)減速電機(jī)上裝有光電碼盤測(cè)量電機(jī)的實(shí)際轉(zhuǎn)速。

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

在獲得了小車的導(dǎo)航定位信息之后，可通過(guò)軌跡跟蹤控制規(guī)律計(jì)算電機(jī)的控制參數(shù)，實(shí)現(xiàn)差速控制，進(jìn)而完成小車對(duì)預(yù)定軌跡的跟蹤。此外，系統(tǒng)還加入了無(wú)線通信模塊,實(shí)現(xiàn)了與PC機(jī)的無(wú)線通信，方便關(guān)鍵數(shù)據(jù)的實(shí)時(shí)上傳與分析。

2硬件設(shè)計(jì)

2.1微控制器

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

2.2慣性測(cè)量模塊設(shè)計(jì)

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

2.2.1ADIS16265接口及數(shù)據(jù)處理方法

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

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

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

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

ADIS16265陀螺儀在靜止情況下，128 Hz的1 h測(cè)量情況如圖2所示。多次實(shí)驗(yàn)表明，在上電工作5 min后陀螺儀零偏趨于穩(wěn)定，且每隔5 min需要重新標(biāo)定，使用時(shí)

表1　ADIS16265寄存器配置

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

應(yīng)特別注意。

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

2.2.2MPU-9150磁航向測(cè)量與校正

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

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

圖3　靜態(tài)情況下磁航向角解算精度

2.3其他硬件的設(shè)計(jì)

小車的電機(jī)選用帶334線碼盤的減速電機(jī)，型號(hào)是JGA25-371；超聲波測(cè)距模塊選擇了國(guó)產(chǎn)的US-100超聲波測(cè)距模塊；此外小車還安裝了有人公司生產(chǎn)的USR-WIFI232-T串口轉(zhuǎn)無(wú)線模塊。系統(tǒng)的電源供應(yīng)選擇12 V鋰電池，經(jīng)過(guò)電源電路變壓為3.3 V及5 V，分別為電機(jī)、主控芯片及不同的傳感器供電。

參考文獻(xiàn)電源電路、超聲波測(cè)距模塊切換電路、光電碼盤接口電路及電機(jī)驅(qū)動(dòng)電路設(shè)計(jì)請(qǐng)借鑒[4]。

3軟件設(shè)計(jì)

為了精確地控制小車的運(yùn)動(dòng)，需要準(zhǔn)確地解算小車導(dǎo)航參數(shù)，因此需要合理分配各任務(wù)的處理周期，設(shè)計(jì)各傳感器數(shù)據(jù)采集與處理的方法。

3.1處理邏輯設(shè)計(jì)

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

根據(jù)系統(tǒng)的要求，設(shè)置不同的任務(wù)周期如表2所列。

表2　系統(tǒng)不同任務(wù)的周期分配

為了精確控制各任務(wù)的處理時(shí)機(jī)，并設(shè)置處理周期，本文將STM32F103VET6的基本定時(shí)器6設(shè)置為10 ms的溢出中斷來(lái)完成。在中斷服務(wù)函數(shù)中使用任務(wù)周期計(jì)數(shù)器判斷是否到達(dá)各個(gè)任務(wù)的處理時(shí)間點(diǎn)，并進(jìn)行任務(wù)處理標(biāo)志位的設(shè)置。在主程序中循環(huán)檢測(cè)各個(gè)任務(wù)處理標(biāo)志位情況，若標(biāo)志位被置位，則執(zhí)行相應(yīng)的任務(wù)處理程序。具體流程如圖4所示。

圖4　系統(tǒng)任務(wù)處理流程圖

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

3.2慣性測(cè)量模塊數(shù)據(jù)的存儲(chǔ)與處理

為了提高導(dǎo)航解算精度，使用TIM6的10 ms定時(shí)中斷進(jìn)行ADIS16265及MPU-9150的數(shù)據(jù)采集，以每條記錄的方式循環(huán)存儲(chǔ)，主程序查詢數(shù)據(jù)更新并進(jìn)行解算。本文在程序中設(shè)置數(shù)據(jù)存儲(chǔ)指針與數(shù)據(jù)處理指針對(duì)此過(guò)程進(jìn)行管理。

具體過(guò)程如下：在中斷服務(wù)函數(shù)中置標(biāo)志位，采集慣性測(cè)量模塊數(shù)據(jù)存入緩沖區(qū)內(nèi)，并使用數(shù)據(jù)存儲(chǔ)指針記錄存儲(chǔ)緩沖區(qū)空間的使用情況，每次存儲(chǔ)后數(shù)據(jù)存儲(chǔ)指針加1。在主程序中檢測(cè)標(biāo)志位，取出存儲(chǔ)緩沖區(qū)內(nèi)的數(shù)據(jù)進(jìn)行處理，將數(shù)據(jù)處理指針與存儲(chǔ)指針進(jìn)行比較，每次處理后數(shù)據(jù)處理指針加1，持續(xù)處理數(shù)據(jù)直至數(shù)據(jù)處理指針與存儲(chǔ)指針相等，從而保證了歷史數(shù)據(jù)能得到及時(shí)處理。

3.3超聲波模塊的數(shù)據(jù)采集與處理

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

代碼略——編者注。

3.4程序穩(wěn)定性設(shè)計(jì)

因存在MPU-9150、ADIS16265、超聲波測(cè)距等多個(gè)高頻率、多時(shí)間間隔、不同級(jí)別的中斷測(cè)量，大量的中斷嵌套導(dǎo)致了微控制器死機(jī)現(xiàn)象[7]。在中斷處理機(jī)制分析基礎(chǔ)上，采用了全局變量存儲(chǔ)采集數(shù)據(jù)的方式，減少了動(dòng)態(tài)存儲(chǔ)空間分配，無(wú)需調(diào)節(jié)默認(rèn)堆?？臻g大小即可實(shí)現(xiàn)程序的穩(wěn)定運(yùn)行。

4功能實(shí)現(xiàn)

4.1小車的慣性定位解算

已知小車初始位置及航向信息，使用單軸陀螺儀與光電碼盤測(cè)量值，通過(guò)航位推算可求出車輛行駛的軌跡。在直線行駛測(cè)試中，慣性定位解算得到的法向偏差與實(shí)際測(cè)量值相差較大。經(jīng)過(guò)分析，誤差最主要的來(lái)源是積分求解的航向角隨時(shí)間發(fā)散。換言之，僅使用小車系統(tǒng)內(nèi)部的測(cè)量信息進(jìn)行慣性定位解算得到的導(dǎo)航參數(shù)在長(zhǎng)時(shí)間使用中精度無(wú)法保證，因此應(yīng)對(duì)外部環(huán)境的測(cè)量數(shù)據(jù)進(jìn)行信息融合修正。

4.2多傳感器信息融合

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

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

當(dāng)判斷墻壁連續(xù)且平行時(shí)，認(rèn)為超聲波測(cè)距信息可用。連續(xù)存儲(chǔ)多組左右超聲波模塊測(cè)距結(jié)果，結(jié)合小車沿設(shè)定軌跡切線方向的前進(jìn)距離信息，求解出在這段時(shí)間內(nèi)小車的航向角近似值，及小車的法向偏移值，以修正慣性定位解算結(jié)果。當(dāng)超聲波測(cè)距信息無(wú)效時(shí)，可使用磁航向角信息融合修正小車航向角信息。

最后，為了保證通過(guò)陀螺儀采集的數(shù)據(jù)在系統(tǒng)運(yùn)行長(zhǎng)時(shí)間后仍真實(shí)可信，可在系統(tǒng)運(yùn)行一段時(shí)間后，使小車停車重新采集零偏。本文中設(shè)定當(dāng)小車每跑完一圈路徑，即系統(tǒng)運(yùn)行3 min左右后，重新采集10 s內(nèi)單軸陀螺儀的數(shù)據(jù)，取平均值更新零偏。

實(shí)際跑車結(jié)果略——編者注。

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

結(jié)語(yǔ)

本文設(shè)計(jì)了基于STM32F103VET6微控制器的小車導(dǎo)航與控制系統(tǒng)，詳細(xì)介紹了ADIS16265、MPU-9150、超聲波測(cè)距傳感器數(shù)據(jù)的有效采集與處理方法，對(duì)程序穩(wěn)定性設(shè)計(jì)提出了建議，最后介紹了利用多傳感器信息融合方法選擇有效的傳感器數(shù)據(jù)實(shí)現(xiàn)小車的軌跡跟蹤控制的方法，達(dá)到了理想的效果。

[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] 鄭潤(rùn)芳,張海.STM32的小車自主定位與控制系統(tǒng)設(shè)計(jì)[J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用,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] 蔣賢志.數(shù)字電子羅盤誤差分析及校正技術(shù)研究[J].現(xiàn)代雷達(dá),2005(6):39-41,44.

[7] Joseph Yiu.ARM Cortex-M3權(quán)威指南[M].宋巖,譯.北京:北京航空航天大學(xué)出版社,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

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

中圖分類號(hào):TP242

文獻(xiàn)標(biāo)識(shí)碼:A