文獻標識碼： A
DOI：10.16157/j.issn.0258-7998.183260
中文引用格式： 胡海兵，張文達，鄭希鵬，等. 基于UWB的智能跟隨車導(dǎo)航定位算法研究[J].電子技術(shù)應(yīng)用，2019，45(3)：80-83，87.
英文引用格式： Hu Haibing，Zhang Wenda，Zheng Xipeng，et al. Research on navigation and positioning algorithm for intelligent following vehicle based on UWB[J]. Application of Electronic Technique，2019，45(3)：80-83，87.
0 引言
隨著科學(xué)技術(shù)突飛猛進的發(fā)展，人們開始解放雙手，更加注重設(shè)備的智能化和信息化。行李箱雖有輪子輔助可以替人們省力，但也已經(jīng)不能滿足人們對智能化生活的更高需求，而智能跟隨車使箱體具有自動跟隨的功能，可以極大地減輕人們的負擔。目前雖已出現(xiàn)一些智能跟隨車的控制方法[1-4]和定位方法[5-9]，但由于控制系統(tǒng)和定位精度的局限，基本都面臨跟隨效果不理想、功能單一等問題。
本設(shè)計提供了一種智能跟隨裝置，具有更好的控制方法及定位算法，使智能跟隨車可以更加安全有效地跟隨主人，不會出現(xiàn)丟失目標或者路徑紊亂的情況。本設(shè)計提出了一種定位算法，在智能跟隨車上方的兩角分別固定一個基站，人手持移動標簽在兩基站的前方，先由到達時間算法(Time Of Arrival，TOA)[10]得出標簽到兩個基站的距離，數(shù)據(jù)經(jīng)過卡爾曼濾波處理，再利用三角函數(shù)進行相關(guān)計算，以兩個基站的中點為基準點，兩個基站所在直線為基準線，得到手持標簽到基準點的距離以及偏離基準線的角度，再將所得的距離和角度測量值與實際值相比較并分析誤差，預(yù)期結(jié)果為：距離誤差不超過10 cm，角度誤差不超過10°。
1 系統(tǒng)總體設(shè)計
實驗初期進行了一系列的方案選擇，有單藍牙方案、單紅外方案、單433 MHz無線方案和單超聲波方案，均被驗證效果不理想。最終采用了無線和藍牙結(jié)合的方案。
系統(tǒng)總體設(shè)計及工作原理為：由手環(huán)發(fā)射信號，射頻模塊接收此信號，用低通濾波算法濾除噪聲和不可靠信號,完成數(shù)據(jù)預(yù)處理工作。圖1所示為智能跟隨車初步定位示意圖，預(yù)處理信號通過TOA算法可以確定手環(huán)到兩射頻模塊的距離，兩射頻模塊之間的距離固定，利用海倫公式和三角形面積相等原理，即得到手環(huán)與兩基站所在直線的垂直距離h，并由高度補償傳感器得到手環(huán)與射頻模塊的垂直誤差X，由勾股定理即可算出手環(huán)與智能跟隨車水平方向的直線距離L。至此完成對智能跟隨車的初步定位工作，整個過程中用低通濾波算法和卡爾曼濾波算法對數(shù)據(jù)融合，以盡可能減小誤差。
初步定位信息結(jié)合九軸平衡傳感模塊和深度傳感器不斷反饋數(shù)據(jù)誤差，經(jīng)離散化比例積分微分(Proportion Integration Differentiation，PID)算法[11]處理間隔為50 ms的數(shù)據(jù)，使智能跟隨車始終處于穩(wěn)定狀態(tài)，完成跟隨車的精確定位。精確定位的信息結(jié)合避障模塊、重力傳感器的反饋信息在控制單元STM32模糊PID算法的控制下，對手環(huán)實際位置和智能跟隨車的實際路況信息進行融合、濾波以及規(guī)則分析，完成最優(yōu)路徑規(guī)劃，此處采用的路徑規(guī)劃算法是最優(yōu)解求解過程的粒子群優(yōu)化算法(Particle Swarm Optimization，PSO)[12]。
各模塊信息最后也都要進行低通濾波和卡爾曼濾波處理，防止數(shù)據(jù)丟失，保證各模塊的數(shù)據(jù)信息能夠同步交給控制模塊處理?？刂颇K根據(jù)各模塊反饋的數(shù)據(jù)信息來合理分配兩電機的脈沖寬度調(diào)制值(Pulse Width Modulation，PWM)，以不斷地實時控制電機的行進方向和速度，最終實現(xiàn)跟隨車安全高效的跟隨功能。
2 硬件設(shè)計
智能跟隨車的目標物為具有信號收發(fā)功能的手環(huán)。智能跟隨車具有控制單元、避障模塊、兩個射頻模塊、高度補償傳感器、重力傳感器、深度傳感器、平衡模塊和驅(qū)動模塊。車底盤的四角共設(shè)有4個避障模塊，前后兩端共設(shè)有2個深度傳感器，底盤的前端設(shè)有2個射頻模塊和1個高度傳感器，底盤的后端設(shè)有1個平衡模塊。其他部件也都設(shè)置在底盤上。硬件總體結(jié)構(gòu)如圖2所示。
2.1 射頻模塊
兩個射頻模塊采用的是UWB Mini 3s模塊，以STM32F103T8U6為主控芯片，外圍電路包括DWM1000模塊、電源模塊、LED指示模塊和復(fù)位電路等。該模塊基站標簽為一體，通過撥碼開關(guān)切換基站和標簽功能。其作用是接收手環(huán)發(fā)來的信息，并傳送給控制模塊。
2.2 高度補償傳感器
高度補償傳感器用于確定手環(huán)與智能跟隨車之間的垂直距離，將所收集到的信息反饋到控制模塊。其作用是優(yōu)化箱子的位置信息，使定位更加準確。
2.3 重力傳感器
重力傳感器用來測量智能跟隨車所裝行李的重量。當智能跟隨車內(nèi)裝載有不同重量的行李時，相應(yīng)調(diào)節(jié)電機轉(zhuǎn)速，保證跟隨車在裝不同重量的行李下都可以跟上手環(huán)持有者的速度。
3 軟件設(shè)計
3.1 主程序設(shè)計
智能跟隨車上電后，首先進行系統(tǒng)的初始化，包括時鐘配置、定時器初始化以及調(diào)試程序時所需串口的初始化。接著進行DWM1000模塊初始化。初始化工作完成后，判斷基站是否收到了標簽發(fā)送的中斷信號，若是，則基站將信息傳給單片機計算出所需的距離和角度數(shù)據(jù)，經(jīng)由PID控制算法調(diào)節(jié)電機驅(qū)動模塊的PWM值，從而控制電機的轉(zhuǎn)速和轉(zhuǎn)向；否則返回繼續(xù)進行該判斷。本設(shè)計中智能跟隨車的軟件設(shè)計流程如圖3所示。
3.2 定位原理與算法描述
本節(jié)算法設(shè)計的目的為計算出手持標簽到兩個基站中點的距離，以及偏離兩基站所在直線的角度，并通過串口輸出在顯示屏上，便于觀測和記錄。具體算法描述如下：圖4所示為實驗定位算法原理圖，圖中點A為手持標簽，點B和點C為智能跟隨車上的兩個固定基站，且兩基站之間距離固定為33 cm，點D為兩個基站的中點，線段
邊上的高。手持標簽A到兩個基站B和C的距離通過TOA方法得到。
海倫公式為：
3.3 卡爾曼濾波程序設(shè)計
卡爾曼濾波是一種利用線性系統(tǒng)狀態(tài)方程；通過系統(tǒng)輸入輸出觀測數(shù)據(jù)對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，因此也可看作是濾波過程。首先引入一個線性隨機微分方程：
4 測試數(shù)據(jù)及誤差分析
4.1 實驗設(shè)計
為了測試本設(shè)計中智能跟隨車的定位精度，設(shè)計了如下實驗，實驗示意圖如圖5所示。圖中右側(cè)智能跟隨車為俯視圖，其中A和B為智能跟隨車上方兩腳固定的兩個基站，C為可移動的手持標簽，D為A、B兩個基站的中點。
在本實驗設(shè)計中，共選取了9個點進行測試，在圖5中分別編號為1～9。其中點1～3的實際距離均為50 cm，實際角度分別為45°、60°和90°；點4～6的實際距離均為100 cm，實際角度分別為45°、60°和90°；點7～9的實際距離均為150 cm，實際角度分別為45°、60°和90°。
利用3.2中所介紹的算法，可以求解出圖中
的距離以及∠CDB的角度，并通過串口輸出顯示出來。在實驗中對每個點均選取100組連續(xù)的測試數(shù)據(jù)，并對所有數(shù)據(jù)取幾何平均值作為每個點的測量值，以避免均值受極端值的較大影響。最后，將測量值與實際值進行對比，分析誤差。
4.2 測量數(shù)據(jù)
經(jīng)過實驗，測出實驗數(shù)據(jù)并在極坐標中畫出每個點對應(yīng)的距離和角度，得到如圖6所示的測量數(shù)據(jù)點圖。
圖6中點1～3為實際距離50 cm，實際角度分別是45°、60°和90°對應(yīng)點的測量值；點4～6為實際距離100 cm，實際角度分別是45°、60°和90°對應(yīng)點的測量值；點7～9為實際距離150 cm，實際角度分別為45°、60°和90°對應(yīng)點的測量值。
將每個點測量值的距離和角度分別與實際值的距離和角度作差，得到各個點對應(yīng)的距離誤差和角度誤差數(shù)據(jù)，并繪制得到如圖7所示的距離誤差數(shù)據(jù)圖和如圖8所示的角度誤差數(shù)據(jù)圖。
4.3 誤差分析
由圖7和圖8可得，本實驗中距離的誤差在-2 cm～+9 cm之間，角度的誤差在-8°～+10°之間，滿足設(shè)計和定位的需求。
從實驗的過程來看，實驗數(shù)據(jù)誤差的來源可能有：
(1)手持標簽不穩(wěn)定。由于實驗過程中可移動標簽由人手持，在實驗中可能會有輕微的不穩(wěn)定，會導(dǎo)致數(shù)據(jù)有偏差。
(2)實際距離和角度標定的誤差。兩個基站的距離和九個測試點在實際操作中要用卷尺測距進行定點，在此過程中可能會有測量誤差存在。
(3)手持標簽和基站所在平面不水平。當手持標簽與基站所在平面不水平時，標簽到基站的距離會比實際值偏大，導(dǎo)致距離測量值偏大，角度測量值也會偏大。
(4)數(shù)據(jù)位數(shù)的丟失。由于在軟件部分程序的計算中，數(shù)據(jù)的小數(shù)部分在運算中會被截斷，并不能得到精確的數(shù)據(jù)，也會導(dǎo)致數(shù)據(jù)的誤差。
5 結(jié)束語
本文設(shè)計了一種可以有效融合各個功能模塊，集定位、避障、平衡、跟隨、網(wǎng)絡(luò)于一體的安全、可靠、高效的智能跟隨裝置、控制方法及定位方法，通過實驗驗證，本設(shè)計中智能跟隨車定位的距離誤差不超過9 cm，角度誤差不超過10°，滿足設(shè)計需求。在后續(xù)的實驗設(shè)計中，會繼續(xù)優(yōu)化軟件部分的設(shè)計，使實驗誤差進一步縮小。本設(shè)計系統(tǒng)相比于現(xiàn)有技術(shù)的控制和定位方法，可以使智能跟隨車更加安全有效地跟隨主人，不會出現(xiàn)丟失目標或者路徑紊亂的情況，在市場上也有廣泛的應(yīng)用價值，對國內(nèi)智能跟隨領(lǐng)域的成長與進步也具有重要的意義。
參考文獻
[1] 丁柏文，金琦淳，任俊，等.基于IAP15F2K61S2單片機的紅外與無線搬運小車[J].儀表技術(shù)與傳感器，2017(10)：110-114.
[2] 王志鎧.基于UWB無線定位技術(shù)的AGV室內(nèi)導(dǎo)航與控制技術(shù)研究[D].成都：電子科技大學(xué)，2018.
[3] 趙昊寧，聶憲波，關(guān)立強，等.自動跟隨運輸系統(tǒng)設(shè)計[J].自動化應(yīng)用，2015(12)：69-71.
[4] 劉磊，孫曉菲，張煜.基于STM32的可遙控智能跟隨小車設(shè)計[J].電子測量技術(shù)，2015(6)：31-33.
[5] 王飛.基于UWB的室內(nèi)移動機器人導(dǎo)航定位技術(shù)的研究[D].哈爾濱：哈爾濱工程大學(xué)，2017.
[6] 賀晶晶，姜平，馮曉榮.基于UWB的無人運輸車的導(dǎo)航定位算法研究[J].電子測量與儀器學(xué)報，2016，30(11)：1743-1749.
[7] 蔡磊，周亭亭，郭云鵬，等.基于超聲波定位的智能跟隨小車[J].電子測量技術(shù)，2013，36(11)：76-79.
[8] 盧靖宇，余文濤，趙新，等.基于超寬帶的移動機器人室內(nèi)定位系統(tǒng)設(shè)計[J].電子技術(shù)應(yīng)用，2017，43(5)：25-28.
[9] IBRAHEEM M.Gyroscope-enhanced dead reckoning localization system for an intelligent Walker[C].2010 International Conference on Information Networking and Automation(ICINA).IEEE，2010：67-72.
[10] 李俊杰，王世練，路軍，等.基于TOA的中心車輛定位系統(tǒng)的算法研究[J].通信技術(shù)，2001(5)：23-25.
[11] 薛輝，王叢嶺.基于單片機采用PID算法的電機運行控制系統(tǒng)設(shè)計[J].機床電器，2006，33(1)：51-52.
[12] 李炳宇，蕭蘊詩，汪鐳.PSO算法在工程優(yōu)化問題中的應(yīng)用[J].計算機工程與應(yīng)用，2004，40(18)：74-76.
作者信息:
胡海兵1，2，張文達1，2，鄭希鵬1，2，曾貴苓3
（1.合肥工業(yè)大學(xué) 光電技術(shù)研究院 特種顯示技術(shù)國家工程實驗室，安徽 合肥230009；
2.合肥工業(yè)大學(xué) 光電技術(shù)研究院 現(xiàn)代顯示技術(shù)省部共建國家重點實驗室，安徽 合肥230009；
3.蕪湖職業(yè)技術(shù)學(xué)院 電氣工程學(xué)院，安徽 蕪湖241006）