李佳佳，李 智，管四海

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

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

李佳佳，李 智，管四海

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

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

室內定位；路徑規(guī)劃；極坐標

0 引言

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

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

1 系統(tǒng)整體設計

系統(tǒng)整體結構設計如圖1，流程如下：

圖1 基于路徑規(guī)劃的室內定位系統(tǒng)結構框圖

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

圖2 電機驅動模塊電路圖

2 硬件設計

2.1 主控芯片

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

2.2 HMC5883L模塊

霍尼韋爾HMC5883L是一種高集成模塊，選其為偏角測量模塊，是因為其具有高達1°～2°的測量精度；同時采用I2C數字接口通信，高效、穩(wěn)定，程序設計簡單。需要特別注意的是：

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

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

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

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

(1)

經過此運算后，將電子羅盤測得的數據范圍轉化為0°～360°。即在0°時，x軸所指向的方位為南。

2.3 電機驅動設計

電機驅動模塊電路設計如圖2，驅動芯片采用L293，一個芯片可以驅動兩個直流電機，簡化了系統(tǒng)電路的設計，同時，在引腳電平低于一定水平時，芯片停止工作，對電源起到了保護作用。電機調速方式為PWM方式，即在周期固定的前提下，通過控制輸出脈寬的不同來進行速度的調節(jié)。

2.4 測速模塊

測速模塊電路設計如圖3。其中，光電對管選用ITR9608，在系統(tǒng)設計時，將碼盤固定在電機上，并置于光電對管的凹槽中，車輪帶動碼盤轉動，將PWM的信號傳輸至主控芯片，從而計算車輪的轉速，并通過LCD1602實時顯示。

設車輪速度為v，直徑為d，光電碼盤的齒數是n，即車輪轉動一周的計數次數為2n，單位時間高低電平的變化次數為t，則車輪速度：

(2)

圖3 測速模塊設計

2.5 超聲波避障模塊

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

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

(3)

3 軟件設計

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

圖4 軟件設計流程圖

(1)先對系統(tǒng)的外設資源初始化，然后判斷當jiaodu≠預設角時，執(zhí)行右轉函數Turn_Right()，直到jiaodu=預設角，執(zhí)行停止函數Stop()，并使得標志位set=1。

jiaodu=read_HMC5883L();

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

//預設角度為0

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

{ Turn_Right();

jiaodu=read_HMC5883L();

}

Stop();

set=1;

Delayms(2000);

}

(2)當set=1時，表明已找到指定方向，執(zhí)行直行函數Go()，并延遲適當時間，該時間由目標距離和車速決定。

if(set==1)

{ set=0;

just_car();

//前進中避障

Stop();

}

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

(4)在直行過程中，用TIM2定時器產生1 ms時基信號，每隔10 ms，執(zhí)行一次距離檢測函數Get_distance()和障礙處理函數Handle_bar()。并定義一個num變量用來計數，每過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)當Get_distance()返回值小于10 cm時，則認為前方存在障礙，執(zhí)行障礙處理函數Handle_bar()。

void Handle_bar()

{ if(juli_cm < 10)

{ while(1)

{

障礙處理

……

}

}

else

Go();

break;

}

(6)當距離檢測函數Get_distance()返回值大于10 cm時，則認為障礙已經撤走，此時程序通過num中保存的值可以計算出剩余要走的路程。

(7)返回(3)。

4 實際測試

為了測試該系統(tǒng)能準確定位且能有效避障，設計實驗如下：

圖5 目標點分布

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

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

圖6 實際軌跡與理論軌跡對比

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

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

[3] 劉洺辛, 孫建利. 基于能效的WLAN室內定位系統(tǒng)模型設計與實現[J]. 儀器儀表學報, 2014, 35(5):1169-1178.

[4] 宋婷.室內移動機器人路徑規(guī)劃與定位技術研究[D].西安：西安電子科技大學，2011.

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

[6] 劉詩斌，馮小毅，李宏.基于橢圓假設的電子羅盤誤差補償方法[J].傳感器技術，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

國家自然科學基金(61673310)

TN96

A

10.19358/j.issn.1674- 7720.2017.07.025

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

2016-11-10)

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

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

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