李佳佳，李智，管四海

　?。ㄎ靼搽娮涌萍即髮W(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)，而且還能有效避障。

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

　　中圖分類號(hào)：TN96文獻(xiàn)標(biāo)識(shí)碼：ADOI： 10.19358/j.issn.1674-7720.2017.07.025

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

0引言

　　*基金項(xiàng)目：國(guó)家自然科學(xué)基金（61673310）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ò)程中，需要搜索的空間范圍很大，若沒有很好的簡(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硬件設(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.2HMC5883L模塊

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

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

　?。?）在電磁干擾較大的室內(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寄存器中的值。

　　經(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.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開發(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ì)如下：　

　?。?）先對(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);

　　}

　?。?）當(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();

　　}

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

　?。?）在直行過(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++;

　　}

　　}

　　}

　?。?）當(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ì)算出剩余要走的路程。

　?。?）返回（3）。

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

　　為了測(cè)試該系統(tǒng)能準(zhǔn)確定位且能有效避障，設(shè)計(jì)實(shí)驗(yà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ù)。

參考文獻(xiàn)

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

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

　?。?］ 劉洺辛, 孫建利. 基于能效的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.

　?。?］ JARVIS R. Distance transform based path planning for robot navigation［M］. Singapore：World Scientific, 1993.

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