基于無(wú)線網(wǎng)絡(luò)的室內(nèi)移動(dòng)機(jī)器人定位方法研究

1、作者簡(jiǎn)介：楊佳麗，1986.2，女，陜西，碩士研究生，車輛導(dǎo)航與移動(dòng)機(jī)器人智能控制，聯(lián)系電話E-mail: kkarrie 1基于無(wú)線網(wǎng)絡(luò)的室內(nèi)移動(dòng)機(jī)器人定位方法研究楊佳麗 史恩秀西安理工大學(xué)機(jī)械與精密儀器工程學(xué)院 陜西 西安 710048摘 要 本文基于超聲波測(cè)距原理，研究開(kāi)發(fā)了一種以nRF24L01無(wú)線通信芯片為核心的移動(dòng)機(jī)器人（Mobile Robot, MR ）室內(nèi)無(wú)線網(wǎng)絡(luò)定位系統(tǒng)。定位系統(tǒng)由定位傳感器、一臺(tái)計(jì)算機(jī)、一個(gè)主控制器和多個(gè)無(wú)線網(wǎng)絡(luò)節(jié)點(diǎn)組成，主控制器和無(wú)線網(wǎng)絡(luò)節(jié)點(diǎn)都使用STC12C5616AD 單片機(jī)作為核心控制芯片，主控制器通過(guò)串口將數(shù)據(jù)發(fā)送到上位

2、機(jī)，上位機(jī)用V isual C+編寫的程序進(jìn)行數(shù)據(jù)采集和處理，實(shí)現(xiàn)了上位機(jī)與室內(nèi)定位系統(tǒng)及移動(dòng)機(jī)器人之間的無(wú)線通信。實(shí)驗(yàn)結(jié)果證明：該系統(tǒng)成本低廉、結(jié)構(gòu)簡(jiǎn)單, 能實(shí)現(xiàn)可靠的無(wú)線數(shù)據(jù)傳輸。 關(guān)鍵詞 STC12C5616AD ，nRF24L01，超聲波，定位系統(tǒng)，無(wú)線數(shù)據(jù)傳輸 中圖分類號(hào) TP242.2 文獻(xiàn)標(biāo)識(shí)碼: AStudy on the Position Method for Indoor Mobile RobotBased-in Wireless Network TechnologyY ang Jiali Shi EnxiuSchool of Mechanical and Instrume

3、ntal， XiAn University of Technology， Xian 710048Abstract : A new wireless network for indoor positioning system was developed based on nRF24L01. The system which used ultrasonic positioning method，was composed by a computer, a main controller and many wireless net work nodesThe main controller and t

4、he wireless network node are implemented by STC12C5616AD as a core control chip microcontroller. The main controller send the data to the host computer through the Serial portsThe data acquisition and processing program is written in VC+. To achieve the host computer for wireless communication betwe

5、en indoor positioning system and mobile robot. Experimental results show that this control method is low cost, simple structure and can deliver reliable wireless data transmission.Key Words: nRF24L01, STC12C5616AD, ultrasonic, indoor positioning system robot, wireless data transmission引言隨著移動(dòng)機(jī)器人MR (M

6、obile Robot 應(yīng)用范圍的擴(kuò)大，對(duì)其的導(dǎo)航與定位技術(shù)成為研究的重點(diǎn)，這也是MR 實(shí)現(xiàn)智能化和完全自主移動(dòng)的關(guān)鍵所在1。對(duì)導(dǎo)航與定位技術(shù)的研究，其目的在于使MR 在無(wú)人干預(yù)的條件下使其有沿規(guī)劃的路徑安全移動(dòng)到目標(biāo)位置以完成指定的任務(wù)。在MR 的定位導(dǎo)航系統(tǒng)中，定位是實(shí)現(xiàn)導(dǎo)航的前提與基礎(chǔ)。目前，移動(dòng)目標(biāo)定位技術(shù)主要有獨(dú)立定位、衛(wèi)星定位和無(wú)線電定位等。獨(dú)立定位技術(shù)不需要使用通信設(shè)備，具有完全自主性，受外界影響小，但誤差會(huì)累積，即定位精度隨定位過(guò)程的進(jìn)行不斷下降1。衛(wèi)星定位可為全球范圍內(nèi)的用戶提供連續(xù)準(zhǔn)確的位置、速度和時(shí)間信息，但小距離定位精度差，在室內(nèi)無(wú)法可靠地接收定位信號(hào)，不適用于室內(nèi)移動(dòng)

7、機(jī)器人IMR(Indoor Mobile Robot 。無(wú)線電定位是依據(jù)電磁波的恒定傳播速率和路徑的可預(yù)測(cè)性來(lái)實(shí)現(xiàn)的2,3。本文采用此定位原理對(duì)移動(dòng)機(jī)器人進(jìn)行定位，通過(guò)紅外光和超聲波可實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的絕對(duì)定位，原本定位系統(tǒng)的MR 是在固定的室內(nèi)短距離傳輸，不會(huì)影響定位精度4。解決了現(xiàn)有室內(nèi)移動(dòng)機(jī)器人導(dǎo)航與定位技術(shù)存在的性價(jià)比低、路徑設(shè)置不靈活的問(wèn)題。1 定位系統(tǒng)設(shè)計(jì)本文以用于車間的IMR 為研究對(duì)象，針對(duì)其定位問(wèn)題進(jìn)行研究，設(shè)計(jì)了一種基于無(wú)線通信的IMR 室內(nèi)定位系統(tǒng)。定位系統(tǒng)包括車載控制子系統(tǒng)和定位子系統(tǒng)(如圖1所示 。定位子系統(tǒng)中的主控制器與車載控制子系統(tǒng)中的車載控制器間通過(guò)無(wú)線通信模塊

8、進(jìn)行通信。定位子系統(tǒng)由上位機(jī)、主控制器和位于機(jī)器人工作空間上方陣列式分布的定位單元模塊組成，其中，定位單元模塊的分布如圖2所示。平面P 是一位于IMR 工作空間上方平行于MR 工作平面且距離一定的平面，定位單元模塊在平面P 內(nèi)以等間距a 成方陣式分布。定位單元模塊由從控制器、紅外光接收器和超聲波接收器和無(wú)線通訊模塊組成。 車載控制子系統(tǒng)由車載控制器和分別安裝在MR 前后端的發(fā)射單元模塊M1和M2組成，其中的發(fā)射單元模塊由從控制器、超聲波發(fā)送器和紅外光發(fā)送器組成。其中車載控制器的作用是控制發(fā)射單元模塊中的從控制器以一定的頻率發(fā)送紅外光和超聲波，根據(jù)無(wú)線通訊模塊接收的來(lái)自定位系統(tǒng)的信息控制MR 運(yùn)

9、動(dòng)。2 定位原理車載控制器分時(shí)控制位于MR 前后端的發(fā)射單元模塊M 1和M 2中的發(fā)射單元模塊以一定的頻率同時(shí)發(fā)送超聲波和紅外光。定位子系統(tǒng)的定位單元模塊中的從控制器通過(guò)檢測(cè)紅外光接收器(接收來(lái)自MR 的紅外光 的狀態(tài)啟動(dòng)計(jì)時(shí)器，根據(jù)超聲波接收器(接收來(lái)自MR 的超聲波 停止計(jì)時(shí)器，根據(jù)計(jì)時(shí)器的數(shù)據(jù)計(jì)算該節(jié)點(diǎn)M i (i=1,2 距離A 、B 、C 三點(diǎn)的距離S n ，并將此距離信息通過(guò)無(wú)線通訊模塊發(fā)送給主控制器。主控制器通過(guò)串口將所接收到的距離信息傳給上位機(jī)，上位機(jī)完成該發(fā)射模塊在MR 工作平面上的位置 (x i, yi (i =1,2，依據(jù)兩發(fā)射模塊的位置信息可得MR 在其工作空間的位置(

10、x , y 和姿態(tài)角(簡(jiǎn)稱位姿，記為P = x y T 。結(jié)合MR 的目標(biāo)點(diǎn)位姿 P t = xt y t t T 計(jì)算出MR 左、右輪在下一控制周期T 內(nèi)的運(yùn)動(dòng)量（如速度或位移量）。上位機(jī)通過(guò)串口將此運(yùn)動(dòng)量發(fā)送給主控制器，再通過(guò)無(wú)線模塊將此發(fā)送給車載控制子系統(tǒng)以控制MR 。2.1紅外超聲波測(cè)距原理為獲得發(fā)射單元模塊M i (i=1,2點(diǎn)位置信息的關(guān)鍵是測(cè)出其到A 、B 、C 三點(diǎn)距離M i 點(diǎn)的距離S i,n（n =A,B,C）。本研究采用超聲波測(cè)距法。測(cè)距原理如下： MR 上的發(fā)射單元模塊M i (i=1,2在單片機(jī)的控制下由紅外光和超聲波發(fā)射器同時(shí)發(fā)射紅外光和超聲波。由于光速比聲速快，位

11、于P 平面上的某節(jié)點(diǎn)處的從控制器先接收到紅外光，后接收到超聲波信號(hào)。A i,A圖1 系統(tǒng)總體設(shè)計(jì)框圖 圖2 IMR 定位系統(tǒng)及定位原理圖 Fig.1 System design diagram Fig.2 The Principle of Position System for IMR由于光速遠(yuǎn)大于聲速，所以光通過(guò)距離S i,n 的時(shí)間t 光遠(yuǎn)小于超聲波通過(guò)其的時(shí)間t 聲，由于S i,n 很?。▽?duì)室內(nèi)移動(dòng)機(jī)器人，在S i,n 10m 內(nèi)），可認(rèn)為t 光0，即可認(rèn)為位于節(jié)點(diǎn)A 、B 、C 處的紅外光接收器同時(shí)接收到紅外光信號(hào)，相應(yīng)地，計(jì)時(shí)器的值即可為超聲波從MR 到該節(jié)點(diǎn)的傳播時(shí)間t 聲6，據(jù)此

12、可得S i,n ：, , i n i n S t V 聲 (12.2 MR 定位原理為確定MR 在其工作平面上的位姿P ，以通過(guò)車載發(fā)射單元且平行于MR 工作平面的一平面為坐標(biāo)平面建立坐標(biāo)系XOY，定位單元模塊所在平面P 平行于坐標(biāo)平面XOY ，其間距離為H 。設(shè)根據(jù)在P 平面中距離Mi 最近且相鄰三點(diǎn)A 、B 、C 處定位模塊的距離信息S i,n 作為定位用信息。設(shè)三點(diǎn)在XOY 內(nèi)的投影分別為A、B、C，點(diǎn)A、B、C分別到Mi 點(diǎn)間的距離為L(zhǎng) i,n （如圖2所示）：222, , i n i n L S H =- (2設(shè)投影點(diǎn)A、B、C在平面XOY 上的位置為 (x n , y n ，以(x

13、 n , y n 為圓心，L i,n 為半徑畫圓(如圖3所示 。理論上，多圓必交于一點(diǎn)，該點(diǎn)即為對(duì)應(yīng)的發(fā)射單元M i 。設(shè)M i 點(diǎn)的坐標(biāo)為(x m , y m ，根據(jù)下式可得M i 點(diǎn)的坐標(biāo)(x m,i , y m,i ：222, 222, 222, ( ( ( ( ( ( M i A M i A i A M i B M i B i B M iC M i C i C x x y y L x x y y L x x y y L -+-=-+-=-+-= (3由此可知，要確定M 點(diǎn)的位置，只能測(cè)得A 、B 、C 三點(diǎn)距離發(fā)射點(diǎn)M 的距離S i,n 即可5。若能得到MR 前后兩發(fā)射點(diǎn)M 1和M 2

14、在平面XOY 上的點(diǎn)坐標(biāo)，通過(guò)(3式可得MR 的實(shí)際位姿P （如圖4所示）：M 1M 2M 1M 2M 2M 122arctan x x x y y y y y x x +=+=-=- (4 上位機(jī)根據(jù)MR 的位姿P 和其目標(biāo)點(diǎn)位姿P t 計(jì)算出MR 下一控制周期的量；并通過(guò)無(wú)線發(fā)射模塊傳輸給機(jī)器人的車載控制器，車載控制器據(jù)此控制左、右輪運(yùn)動(dòng)以控制MR 向目標(biāo)點(diǎn)運(yùn)動(dòng)。3 系統(tǒng)軟硬件設(shè)計(jì)3.1 系統(tǒng)硬件設(shè)計(jì)本系統(tǒng)均采用宏晶科技有限公司研發(fā)生產(chǎn)的STC12C5616AD 單片機(jī)作為各控制器的核心。STC12C5616AD 是一種高速、低功耗、超強(qiáng)抗干擾的新一代8051單片機(jī)。指令代碼完全兼容傳統(tǒng)的

15、8051，但速度提高了8-12倍，內(nèi)部集成有專用的復(fù)位電路；可采用多種編程方式，也可通過(guò)串口直接下載用戶程序。其含有一個(gè)通用全雙工異步串行口和一個(gè)SPI 串行外設(shè)接口，工作頻率范圍可達(dá)35MHZ ；兩個(gè)專用的16位定時(shí)器；一個(gè)具有4路PWM 輸出的可編程記數(shù)器陣列PCA ；8路高速10位的A/D轉(zhuǎn)換器。主要應(yīng)用于強(qiáng)干擾場(chǎng)合7 。3.1.1無(wú)線通訊模塊設(shè)計(jì)無(wú)線通信模塊是主控制器、網(wǎng)絡(luò)節(jié)點(diǎn)和MR 間的紐帶。本系統(tǒng)的無(wú)線通信模塊采用nRF24L01芯片，其特性有工作頻段在2.4-2.5GHz 間，有125個(gè)頻道；傳輸速率為1Mbit/s或2Mbit/s，SPI 速率最高可達(dá)8Mbit/s；GFSK

16、調(diào)制；具有自應(yīng)答和自重發(fā)功能，并自動(dòng)生成報(bào)頭和CRC 校驗(yàn)碼；接收工作電流只有12.3mA ，并配有多種低功率工作模式，節(jié)能 8。nRF24L01的外圍電路比較簡(jiǎn)單，其CE 、CSN 和SCK 等管腳輸出到無(wú)線通信模塊接口，通過(guò)SPI 與單片機(jī)的相應(yīng)管腳相連。發(fā)送數(shù)據(jù)時(shí)，只要將配置寄存器信息、所需發(fā)送數(shù)據(jù)和接收地址通知nRF24L01，它便會(huì)自動(dòng)完成數(shù)據(jù)打包和發(fā)送操作；接收數(shù)據(jù)時(shí)，nRF24L01會(huì)自動(dòng)進(jìn)行數(shù)據(jù)檢測(cè)、地址配置、解包和CRC 校驗(yàn)，最后將數(shù)據(jù)傳到單片機(jī)進(jìn)行處理。nRF24L01能很好的滿足MR 定位系統(tǒng)的設(shè)計(jì)要求。3.1.2 各定位節(jié)點(diǎn)的從控制器和主控制器的設(shè)計(jì)系統(tǒng)定位節(jié)點(diǎn)的從控

17、制器的結(jié)構(gòu)原理如圖5所示。其集超聲波收發(fā)于一身，并帶有測(cè)溫模塊可校正測(cè)距誤差，并可對(duì)數(shù)據(jù)進(jìn)行預(yù)處理和實(shí)時(shí)顯示，同時(shí)無(wú)線通訊模塊將超聲波傳感器采集到的時(shí)間信號(hào)進(jìn)行編址，并附加數(shù)據(jù)，打包發(fā)送給接收主控制器。數(shù)量可根據(jù)實(shí)際需要進(jìn)行配置。主控制器通過(guò)無(wú)線模塊接收所有從控制器傳送來(lái)的數(shù)據(jù)通過(guò)RS232傳送到上位機(jī)，同時(shí)通過(guò)無(wú)線模塊把上位機(jī)傳來(lái)的數(shù)據(jù)傳送給MR （如圖6所示）。 圖5從控制器硬件框圖 圖6主控制器硬件框圖Fig.5 Schematic Diagram for Slave Controller Fig.6 Schematic Diagram for Master Controller3.1.

18、3 紅外超聲波發(fā)射控制器設(shè)計(jì) 位于MR 前后端的發(fā)射模塊M 1和M 2其結(jié)構(gòu)原理如圖7所示。車載控制器分時(shí)控制M 1和M 2的同時(shí)發(fā)射紅外光和超聲波。本文采用的超聲波發(fā)射和接收頭的中心頻率為40kHz ，一體化紅外接收頭HX1838的中心頻率為38kHz 。為了使單片機(jī)產(chǎn)生精準(zhǔn)的脈沖信號(hào)兼顧紅外超聲波發(fā)射器電路的簡(jiǎn)單和可靠性，本文經(jīng)過(guò)實(shí)驗(yàn)中信號(hào)調(diào)試，采用39kHz 頻率，該方法簡(jiǎn)單可行。3.2 系統(tǒng)軟件設(shè)計(jì)3.2.1 紅外超聲波發(fā)射軟件設(shè)計(jì)紅外超聲波發(fā)射程序流程圖如圖8所示。該發(fā)射程序使用STC12C5616AD 單片機(jī)的PCA 模塊，設(shè)置其為PWM 模式，由于該型號(hào)單片機(jī)的PWM 為8位，則

19、：PCA PW M =256時(shí)鐘輸入源頻率的頻率（8） 將PWM 的占空比分別設(shè)置為50%和20%。占空比為50%的方波用于激勵(lì)超聲波發(fā)射頭發(fā)射超聲波；占空比為20%的矩形波用于激勵(lì)紅外發(fā)射管產(chǎn)生紅外光脈沖信號(hào)。3.2.2各控制器軟件設(shè)計(jì)定位節(jié)點(diǎn)的從控制器程序流程如圖9所示. 紅外超聲波傳感器上電后首先進(jìn)行初始化，然后開(kāi)始采集時(shí)間數(shù)據(jù)，單片機(jī)將采集的數(shù)據(jù)計(jì)算成實(shí)際距離并在數(shù)碼管中顯示出來(lái)，送入nRF24L01無(wú)線模塊控制其發(fā)送出去。主控制器程序流程如圖10所示，初始化后，主控制器的無(wú)線模塊負(fù)責(zé)數(shù)據(jù)的接收；將各節(jié)點(diǎn)發(fā)來(lái)的數(shù)據(jù)通過(guò)串口總線上傳到上位機(jī)。另一方面，主控制器的無(wú)線模塊負(fù)責(zé)數(shù)據(jù)的發(fā)送，將

20、上位機(jī)傳送來(lái)的數(shù)據(jù)通過(guò)無(wú)線模塊發(fā)送給MR 。車載控制器負(fù)責(zé)人機(jī)交互、發(fā)出控制信號(hào)，通過(guò)無(wú)線通訊模塊接收來(lái)自主控制器的數(shù)據(jù)，再通過(guò)串口把左右輪速度和方向信號(hào)發(fā)送給下位機(jī)并完成對(duì)左右驅(qū)動(dòng)輪的運(yùn)動(dòng)控制，程序流程如圖11所示。 圖9 從控制器程序流程圖 圖10 主控制器程序流程圖 圖11 車載控制器程序流程圖Fig.9 The Chart for Sub-Controller Fig.10 The Chart for Main-Controller Fig.11 The Chart for MR-Controller3.2.3上位機(jī)軟件設(shè)計(jì)上位機(jī)要實(shí)現(xiàn)的功能有：（1）傳感器數(shù)據(jù)的采集與處理；（2）定位

21、系統(tǒng)與車載系統(tǒng)間的數(shù)據(jù)傳輸；（3）MR 的位姿計(jì)算；（4）MR 的運(yùn)動(dòng)控制。為此，選用Visual C+6.0編寫。該軟件提供了良好的可視化編程環(huán)境。4 定位實(shí)驗(yàn)針對(duì)所設(shè)計(jì)的定位系統(tǒng)設(shè)計(jì)了定位實(shí)驗(yàn)(如圖12所示 ，通過(guò)實(shí)驗(yàn)驗(yàn)證定位方法的可行性和可靠性。實(shí)驗(yàn)中的超聲波發(fā)射器和接收器采用T/R40-16防水型超聲波傳感器，中心頻率40kHz ，發(fā)射角45°，測(cè)量范圍200mm 11000mm ，滿足企業(yè)高廠房?jī)?nèi)移動(dòng)作業(yè)機(jī)器人導(dǎo)航的要求。構(gòu)造的超聲波接收網(wǎng)絡(luò)與超聲波發(fā)射器的垂直距離H=3835mm，超聲波接收器的分布間隔d=1000mm。兩個(gè)超聲波發(fā)射器分別位于不同位置，選4個(gè)接收裝置，分

22、別選用其中三個(gè)重復(fù)測(cè)量3次，實(shí)驗(yàn)結(jié)果如表1所示。根據(jù)實(shí)驗(yàn)，可得定位誤差。由實(shí)驗(yàn)知，超聲波發(fā)射器的計(jì)算位置和實(shí)際位置相差很小，最大誤差m =41.5mm，可滿足一般移動(dòng)機(jī)器人在車間內(nèi)作業(yè)的定位要求，從而證明了所設(shè)計(jì)的定位方法是可行的，也是可靠的。圖8 紅外超聲波發(fā)射程序 Fig.8 The Chart for Sending Model2011 年西安理工大學(xué)研究生學(xué)術(shù)活動(dòng)月 (a 發(fā)送模塊 (b定位系統(tǒng) 圖12 定位實(shí)驗(yàn)裝置圖 Fig.12 The Picture for Positioning IMR 實(shí)際位置 Ti(xi,yi T1 (478,782 表1 實(shí)驗(yàn)數(shù)據(jù)和結(jié)果分析(單位：mm

23、Table 1 The Result of Experiment (Unit: mm 接收位置 Ri(xi,yi 測(cè)試距離 1 測(cè)試距離 2 測(cè)試距離 3 計(jì)算位置 Ti(xi,yi R1(0,0 R2(0,1000 R3(1000,1000 R1(0,0 R2(1000,0 R3(1000,1000 R1(0,0 R2(0,1000 R3(1000,0 3956 3949 3955 3954 3956 3956 3954 3949 3955 3887 3886 3886 3892 3887 3884 3888 3892 3892 3880 3873 3878 3961 3962 3962 3

24、878 3876 3877 3880 3878 3879 3961 3962 3962 3879 3879 3876 3887 3888 3888 3888 3887 3887 3962 3965 3968 3956 3957 3956 3956 3954 3954 3968 3965 3963 （473,798） （442,797） （461,801） （472,786） （476,794） （476,794） （468,798） （437,786） （449,805） （202,527） （191,531） （198,527） （229,520） （206,532） （194,532） （186,535） （213,551） （220,562） 誤差 16.6 39.3 25.8 7.3 12.5 12.5 18.4 41.5 37.7 26.1 32.3 28.6 30.3 20.5 29.0 34.8 5.4 14.5 T2 (218,548 R1(0,0 R2(0,1000 R3(1000,1000 R1(0,0 R2(1000