基于路徑規(guī)劃的室內(nèi)小車定位系統(tǒng)*

李佳佳，李 智，管四海

(西安電子科技大學(xué) 機(jī)電工程學(xué)院，陜西 西安 710071)

基于路徑規(guī)劃的室內(nèi)小車定位系統(tǒng)*

李佳佳，李 智，管四海

(西安電子科技大學(xué) 機(jī)電工程學(xué)院，陜西 西安 710071)

針對(duì)現(xiàn)有室內(nèi)定位技術(shù)模型復(fù)雜、成本高等問(wèn)題，設(shè)計(jì)了一種基于路徑規(guī)劃的室內(nèi)小車定位系統(tǒng)。該系統(tǒng)的主控芯片選用STM32F103ZET6，偏角測(cè)量模塊基于HMC5883L，障礙處理模塊選用HC-SR04；另外，采用極坐標(biāo)的表示方法，給定室內(nèi)初始位置和目標(biāo)點(diǎn)，測(cè)出各目標(biāo)點(diǎn)相對(duì)于初始位置的偏角和距離，依據(jù)相對(duì)偏角和相對(duì)距離規(guī)劃小車的行走路徑。實(shí)際環(huán)境測(cè)試表明，該系統(tǒng)不僅能使小車準(zhǔn)確到達(dá)目標(biāo)點(diǎn)，而且還能有效避障。

室內(nèi)定位；路徑規(guī)劃；極坐標(biāo)

0 引言

GPS定位系統(tǒng)由于其快速、高效、全球覆蓋等特點(diǎn)，被廣泛地應(yīng)用于汽車導(dǎo)航、工程測(cè)量、飛機(jī)導(dǎo)航等領(lǐng)域。而在室內(nèi)，由于建筑物的遮擋，GPS的信號(hào)會(huì)減弱，并且精度滿足不了室內(nèi)定位的要求?，F(xiàn)有的室內(nèi)定位技術(shù)主要有RFID、視頻、ZibBee[1]、藍(lán)牙[2]、WLAN[3]等，但由于室內(nèi)物體間距離較短，信號(hào)存在反射、衍射和繞射等非直線傳播的情況，再加之現(xiàn)有設(shè)備分辨率不足，使得室內(nèi)定位誤差較大[2]。

在現(xiàn)有的室內(nèi)路徑規(guī)劃定位系統(tǒng)中，大多使用柵格構(gòu)圖法和路標(biāo)法[4]進(jìn)行室內(nèi)定位，其中柵格構(gòu)圖法是在機(jī)器人行走過(guò)程中，實(shí)時(shí)構(gòu)建地圖并規(guī)劃路徑，但在定位過(guò)程中，需要搜索的空間范圍很大，若沒(méi)有很好的簡(jiǎn)化算法，實(shí)時(shí)性很難保證。路標(biāo)法的定位中，若有一個(gè)路標(biāo)出現(xiàn)故障，則系統(tǒng)的整體穩(wěn)定性就會(huì)受到很大的影響。本文采用二維極坐標(biāo)的方式確定位置，根據(jù)各目標(biāo)點(diǎn)的位置，事先為智能小車規(guī)劃好行走路線[5]，避免了復(fù)雜算法的優(yōu)化和參照物的使用，還能有效避障。本系統(tǒng)具有設(shè)計(jì)簡(jiǎn)單、易操作、成本低的特點(diǎn)，可以應(yīng)用于餐廳服務(wù)機(jī)器人的設(shè)計(jì)，以機(jī)器人代替人工送餐，給人們的生活帶來(lái)便利。

1 系統(tǒng)整體設(shè)計(jì)

系統(tǒng)整體結(jié)構(gòu)設(shè)計(jì)如圖1，流程如下：

圖1 基于路徑規(guī)劃的室內(nèi)定位系統(tǒng)結(jié)構(gòu)框圖

首先需要事先測(cè)量小車的起點(diǎn)與各目標(biāo)點(diǎn)的距離ρ1,ρ2…ρn和角度θ1,θ2…θn，并將坐標(biāo)表示為(ρ1,θ1),(ρ2,θ2)…(ρn,θn)；然后利用碼盤和光電對(duì)管測(cè)出固定占空比下的車輪轉(zhuǎn)速，并通過(guò)LCD1602實(shí)時(shí)顯示，從而通過(guò)控制時(shí)間來(lái)達(dá)到控制小車前進(jìn)指定距離的目的。利用HMC5883L電子羅盤測(cè)量角度，將電子羅盤的測(cè)量數(shù)據(jù)通過(guò)I2C總線傳輸?shù)街骺匦酒琒TM32F103ZET6，主控芯片將傳感器采集的數(shù)據(jù)進(jìn)行分析處理，進(jìn)而控制電機(jī)轉(zhuǎn)動(dòng)使車前進(jìn)。在此期間，通過(guò)舵機(jī)帶動(dòng)HC-SR04超聲波模塊不斷地轉(zhuǎn)動(dòng)測(cè)距，當(dāng)測(cè)得距離小于某一設(shè)定值時(shí)，控制小車停止，并且繼續(xù)不斷檢測(cè)距離。當(dāng)撤去障礙物，再按事先規(guī)劃的路徑繼續(xù)前進(jìn)，到達(dá)目標(biāo)點(diǎn)。

圖2 電機(jī)驅(qū)動(dòng)模塊電路圖

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

2.1 主控芯片

該系統(tǒng)的主控芯片選用STM32F103ZET6，此芯片作為32位ARM微控制器，有著比16位、8位處理器更強(qiáng)的數(shù)據(jù)處理能力、更豐富的外設(shè)接口。在該系統(tǒng)設(shè)計(jì)中，由于STM32系列微控制器的I2C接口穩(wěn)定性較差，因此采用IO口模擬I2C總線的方式實(shí)現(xiàn)穩(wěn)定的信息傳輸。選用PB6和PB7模擬I2C總線的SCL和SDA，實(shí)現(xiàn)了與HMC5883L模塊的穩(wěn)定通信。

2.2 HMC5883L模塊

霍尼韋爾HMC5883L是一種高集成模塊，選其為偏角測(cè)量模塊，是因?yàn)槠渚哂懈哌_(dá)1°～2°的測(cè)量精度；同時(shí)采用I2C數(shù)字接口通信，高效、穩(wěn)定，程序設(shè)計(jì)簡(jiǎn)單。需要特別注意的是：

(1)為防止干擾，使用時(shí)應(yīng)避免該模塊直接與金屬物體接觸；

(2)在電磁干擾較大的室內(nèi)環(huán)境，要對(duì)該模塊進(jìn)行校正[6]，以確保測(cè)量的準(zhǔn)確；

(3)由于其測(cè)量靈敏，在設(shè)置轉(zhuǎn)角時(shí)，可以用一個(gè)小范圍的值代替特定值。

該系統(tǒng)在設(shè)計(jì)時(shí)，向小車后方加裝木板，將HMC5883L模塊固定于木板上，以降低干擾，模塊上的x和y軸即為在水平面上建立的二維坐標(biāo)系。下面給出電子羅盤返回角度θ的計(jì)算，其中x和y表示讀出HMC5883L寄存器中的值。

(1)

經(jīng)過(guò)此運(yùn)算后，將電子羅盤測(cè)得的數(shù)據(jù)范圍轉(zhuǎn)化為0°～360°。即在0°時(shí)，x軸所指向的方位為南。

2.3 電機(jī)驅(qū)動(dòng)設(shè)計(jì)

電機(jī)驅(qū)動(dòng)模塊電路設(shè)計(jì)如圖2，驅(qū)動(dòng)芯片采用L293，一個(gè)芯片可以驅(qū)動(dòng)兩個(gè)直流電機(jī)，簡(jiǎn)化了系統(tǒng)電路的設(shè)計(jì)，同時(shí)，在引腳電平低于一定水平時(shí)，芯片停止工作，對(duì)電源起到了保護(hù)作用。電機(jī)調(diào)速方式為PWM方式，即在周期固定的前提下，通過(guò)控制輸出脈寬的不同來(lái)進(jìn)行速度的調(diào)節(jié)。

2.4 測(cè)速模塊

測(cè)速模塊電路設(shè)計(jì)如圖3。其中，光電對(duì)管選用ITR9608，在系統(tǒng)設(shè)計(jì)時(shí)，將碼盤固定在電機(jī)上，并置于光電對(duì)管的凹槽中，車輪帶動(dòng)碼盤轉(zhuǎn)動(dòng)，將PWM的信號(hào)傳輸至主控芯片，從而計(jì)算車輪的轉(zhuǎn)速，并通過(guò)LCD1602實(shí)時(shí)顯示。

設(shè)車輪速度為v，直徑為d，光電碼盤的齒數(shù)是n，即車輪轉(zhuǎn)動(dòng)一周的計(jì)數(shù)次數(shù)為2n，單位時(shí)間高低電平的變化次數(shù)為t，則車輪速度：

(2)

圖3 測(cè)速模塊設(shè)計(jì)

2.5 超聲波避障模塊

該系統(tǒng)將超聲波和舵機(jī)通過(guò)云臺(tái)結(jié)合起來(lái)，放在車體的最前端，通過(guò)轉(zhuǎn)動(dòng)舵機(jī)獲取小車各個(gè)方向的距離值，進(jìn)行障礙處理。在對(duì)舵機(jī)的控制中，用不同寬度的脈沖信號(hào)，對(duì)應(yīng)舵機(jī)轉(zhuǎn)動(dòng)的不同角度。選用的HC-SR04超聲波模塊測(cè)量范圍可達(dá)2 cm～400 cm，滿足該系統(tǒng)對(duì)避障精度的要求，并通過(guò)高電平的持續(xù)時(shí)間計(jì)算障礙物的距離，即：

距離=(高電平時(shí)間×聲速)/2

(3)

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

軟件設(shè)計(jì)流程如圖4所示，其主要部分包括系統(tǒng)初始化、采集角度數(shù)據(jù)、角度的判斷、避障等。系統(tǒng)選用Keil uVision5開(kāi)發(fā)工具，選用C語(yǔ)言編程。其中，為了使智能小車在行走的同時(shí)檢測(cè)障礙，設(shè)計(jì)了10 ms計(jì)數(shù)法，同時(shí)定義num變量，保存測(cè)距函數(shù)執(zhí)行的次數(shù)，從而達(dá)到記錄距離的目的。在避障函數(shù)中，控制舵機(jī)左轉(zhuǎn)、居中、右轉(zhuǎn)，不斷地循環(huán)，達(dá)到多方位檢測(cè)距離的目的。具體程序設(shè)計(jì)如下：

圖4 軟件設(shè)計(jì)流程圖

(1)先對(duì)系統(tǒng)的外設(shè)資源初始化，然后判斷當(dāng)jiaodu≠預(yù)設(shè)角時(shí)，執(zhí)行右轉(zhuǎn)函數(shù)Turn_Right()，直到j(luò)iaodu=預(yù)設(shè)角，執(zhí)行停止函數(shù)Stop()，并使得標(biāo)志位set=1。

jiaodu=read_HMC5883L();

if(jiaodu>=5&&jiaodu<=355)

//預(yù)設(shè)角度為0

{ while(jiaodu>=5&&jiaodu<=355 )

{ Turn_Right();

jiaodu=read_HMC5883L();

}

Stop();

set=1;

Delayms(2000);

}

(2)當(dāng)set=1時(shí)，表明已找到指定方向，執(zhí)行直行函數(shù)Go()，并延遲適當(dāng)時(shí)間，該時(shí)間由目標(biāo)距離和車速?zèng)Q定。

if(set==1)

{ set=0;

just_car();

//前進(jìn)中避障

Stop();

}

(3)接著獲取角度值，判斷是否為下一目標(biāo)點(diǎn)所設(shè)角度，不斷循環(huán)，直到最后一個(gè)目標(biāo)點(diǎn)停止。

(4)在直行過(guò)程中，用TIM2定時(shí)器產(chǎn)生1 ms時(shí)基信號(hào)，每隔10 ms，執(zhí)行一次距離檢測(cè)函數(shù)Get_distance()和障礙處理函數(shù)Handle_bar()。并定義一個(gè)num變量用來(lái)計(jì)數(shù)，每過(guò)10 ms，num++。

void just_car(void)

{ int num=0;

while(num<300)

//延遲3 s

{ if(count_1ms>10)

{ count_1ms=0;

Get_distance();

Handle_bar();

num++;

}

}

}

(5)當(dāng)Get_distance()返回值小于10 cm時(shí)，則認(rèn)為前方存在障礙，執(zhí)行障礙處理函數(shù)Handle_bar()。

void Handle_bar()

{ if(juli_cm < 10)

{ while(1)

{

障礙處理

……

}

}

else

Go();

break;

}

(6)當(dāng)距離檢測(cè)函數(shù)Get_distance()返回值大于10 cm時(shí)，則認(rèn)為障礙已經(jīng)撤走，此時(shí)程序通過(guò)num中保存的值可以計(jì)算出剩余要走的路程。

(7)返回(3)。

4 實(shí)際測(cè)試

為了測(cè)試該系統(tǒng)能準(zhǔn)確定位且能有效避障，設(shè)計(jì)實(shí)驗(yàn)如下：

圖5 目標(biāo)點(diǎn)分布

在室內(nèi)環(huán)境，給定初始位置和各目標(biāo)點(diǎn)，并假定預(yù)設(shè)角為0°，即正南方向，對(duì)小車進(jìn)行直行和轉(zhuǎn)彎定位測(cè)試。為了更易觀察實(shí)驗(yàn)效果，設(shè)定直行距離為50 cm，轉(zhuǎn)角為90°，避障距離為10 cm。設(shè)置預(yù)設(shè)角時(shí)，由于電子羅盤的靈敏度較高，因此用一定角度范圍代替具體的角度值，且在此實(shí)驗(yàn)中角度范圍為10°。測(cè)試中的實(shí)際目標(biāo)點(diǎn)分布如圖5示：O表示起點(diǎn)，A、B、C分別為目標(biāo)點(diǎn)。

首先，將小車置于O點(diǎn)尋找預(yù)設(shè)角；然后前進(jìn)50 cm，右轉(zhuǎn)90°，到達(dá)點(diǎn)A；接著前進(jìn)50 cm，再右轉(zhuǎn)90°，到達(dá)點(diǎn)B；最后前進(jìn)50 cm停止于點(diǎn)C。在小車前進(jìn)過(guò)程中，將手擋在小車的前方作為障礙物，當(dāng)手與小車距離小于10 cm時(shí)，小車停止前進(jìn)，同時(shí)舵機(jī)帶動(dòng)超聲波模塊左右轉(zhuǎn)動(dòng)測(cè)距，若障礙一直存在，則會(huì)循環(huán)檢測(cè)距離，直到將障礙物撤去，此后小車?yán)^續(xù)前進(jìn)剩余距離。實(shí)驗(yàn)中測(cè)量其運(yùn)行時(shí)距離與角度的對(duì)應(yīng)關(guān)系，并將實(shí)際測(cè)得的軌跡與理想軌跡進(jìn)行仿真，結(jié)果如圖6。從圖6中可知，基于路徑規(guī)劃的室內(nèi)小車定位系統(tǒng)，能夠在有效誤差范圍內(nèi)到達(dá)各目標(biāo)點(diǎn)，完成室內(nèi)定位任務(wù)。

圖6 實(shí)際軌跡與理論軌跡對(duì)比

[1] 何海平，郭杭，方爽.基于模糊聚類的ZigBee室內(nèi)定位系統(tǒng)設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2016,42(5)：71-77.

[2] 陳國(guó)平，馬耀輝，張百珂.基于指紋技術(shù)的藍(lán)牙室內(nèi)定位系統(tǒng)[J]. 電子技術(shù)應(yīng)用，2013,39(3)：104-107.

[3] 劉洺辛, 孫建利. 基于能效的WLAN室內(nèi)定位系統(tǒng)模型設(shè)計(jì)與實(shí)現(xiàn)[J]. 儀器儀表學(xué)報(bào), 2014, 35(5):1169-1178.

[4] 宋婷.室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃與定位技術(shù)研究[D].西安：西安電子科技大學(xué)，2011.

[5] JARVIS R. Distance transform based path planning for robot navigation[M]. Singapore：World Scientific, 1993.

[6] 劉詩(shī)斌，馮小毅，李宏.基于橢圓假設(shè)的電子羅盤誤差補(bǔ)償方法[J].傳感器技術(shù)，2002,21(10)：28-33.

Indoor vehicle positioning system based on path planning

Li Jiajia，Li Zhi，Guan Sihai

(School of Electromechanical Engineering, Xidian University, Xi′an 710071, China)

In view of the existing model of indoor positioning technology is complex, high cost, this paper designs an indoor car positioning system based on path planning. The main control chip of the system is STM32F103ZET6, angle measurement is based on the HMC5883L module, obstacle processing module adopts HC-SR04. In addition, the location is expressed by polar coordinates, and indoor initial position and target points are given. Measuring the angle and distance of the target points relative to the initial position, based on the relative angle and relative distance to plan the path of the car. The actual environmental measurement shows that this system can not only make the car reach the target points accurately, but also avoid the obstacles effectively.

indoor positioning; path planning; polar coordinates

國(guó)家自然科學(xué)基金(61673310)

TN96

A

10.19358/j.issn.1674- 7720.2017.07.025

李佳佳，李智，管四海.基于路徑規(guī)劃的室內(nèi)小車定位系統(tǒng)[J].微型機(jī)與應(yīng)用，2017,36(7)：84-87.

2016-11-10)

李佳佳(1992-)，男，碩士研究生，主要研究方向：嵌入式系統(tǒng)設(shè)計(jì)與應(yīng)用。

李智(1961-)，通信作者，男，博士，教授，主要研究方向：復(fù)雜網(wǎng)絡(luò)，博弈演化，自適應(yīng)濾波算法。E-mail：jiali_xd@163.com。

管四海(1990-)男，博士研究生，主要研究方向：自適應(yīng)信號(hào)處理。