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