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