0 引言

    隨著科學(xué)技術(shù)突飛猛進(jìn)的發(fā)展，人們開(kāi)始解放雙手，更加注重設(shè)備的智能化和信息化。行李箱雖有輪子輔助可以替人們省力，但也已經(jīng)不能滿足人們對(duì)智能化生活的更高需求，而智能跟隨車(chē)使箱體具有自動(dòng)跟隨的功能，可以極大地減輕人們的負(fù)擔(dān)。目前雖已出現(xiàn)一些智能跟隨車(chē)的控制方法^[1-4]和定位方法^[5-9]，但由于控制系統(tǒng)和定位精度的局限，基本都面臨跟隨效果不理想、功能單一等問(wèn)題。

    本設(shè)計(jì)提供了一種智能跟隨裝置，具有更好的控制方法及定位算法，使智能跟隨車(chē)可以更加安全有效地跟隨主人，不會(huì)出現(xiàn)丟失目標(biāo)或者路徑紊亂的情況。本設(shè)計(jì)提出了一種定位算法，在智能跟隨車(chē)上方的兩角分別固定一個(gè)基站，人手持移動(dòng)標(biāo)簽在兩基站的前方，先由到達(dá)時(shí)間算法(Time Of Arrival，TOA)^[10]得出標(biāo)簽到兩個(gè)基站的距離，數(shù)據(jù)經(jīng)過(guò)卡爾曼濾波處理，再利用三角函數(shù)進(jìn)行相關(guān)計(jì)算，以兩個(gè)基站的中點(diǎn)為基準(zhǔn)點(diǎn)，兩個(gè)基站所在直線為基準(zhǔn)線，得到手持標(biāo)簽到基準(zhǔn)點(diǎn)的距離以及偏離基準(zhǔn)線的角度，再將所得的距離和角度測(cè)量值與實(shí)際值相比較并分析誤差，預(yù)期結(jié)果為：距離誤差不超過(guò)10 cm，角度誤差不超過(guò)10°。

1 系統(tǒng)總體設(shè)計(jì)

    實(shí)驗(yàn)初期進(jìn)行了一系列的方案選擇，有單藍(lán)牙方案、單紅外方案、單433 MHz無(wú)線方案和單超聲波方案，均被驗(yàn)證效果不理想。最終采用了無(wú)線和藍(lán)牙結(jié)合的方案。

    系統(tǒng)總體設(shè)計(jì)及工作原理為：由手環(huán)發(fā)射信號(hào)，射頻模塊接收此信號(hào)，用低通濾波算法濾除噪聲和不可靠信號(hào),完成數(shù)據(jù)預(yù)處理工作。圖1所示為智能跟隨車(chē)初步定位示意圖，預(yù)處理信號(hào)通過(guò)TOA算法可以確定手環(huán)到兩射頻模塊的距離，兩射頻模塊之間的距離固定，利用海倫公式和三角形面積相等原理，即得到手環(huán)與兩基站所在直線的垂直距離h，并由高度補(bǔ)償傳感器得到手環(huán)與射頻模塊的垂直誤差X，由勾股定理即可算出手環(huán)與智能跟隨車(chē)水平方向的直線距離L。至此完成對(duì)智能跟隨車(chē)的初步定位工作，整個(gè)過(guò)程中用低通濾波算法和卡爾曼濾波算法對(duì)數(shù)據(jù)融合，以盡可能減小誤差。

    初步定位信息結(jié)合九軸平衡傳感模塊和深度傳感器不斷反饋數(shù)據(jù)誤差，經(jīng)離散化比例積分微分(Proportion Integration Differentiation，PID)算法^[11]處理間隔為50 ms的數(shù)據(jù)，使智能跟隨車(chē)始終處于穩(wěn)定狀態(tài)，完成跟隨車(chē)的精確定位。精確定位的信息結(jié)合避障模塊、重力傳感器的反饋信息在控制單元STM32模糊PID算法的控制下，對(duì)手環(huán)實(shí)際位置和智能跟隨車(chē)的實(shí)際路況信息進(jìn)行融合、濾波以及規(guī)則分析，完成最優(yōu)路徑規(guī)劃，此處采用的路徑規(guī)劃算法是最優(yōu)解求解過(guò)程的粒子群優(yōu)化算法(Particle Swarm Optimization，PSO)^[12]。

    各模塊信息最后也都要進(jìn)行低通濾波和卡爾曼濾波處理，防止數(shù)據(jù)丟失，保證各模塊的數(shù)據(jù)信息能夠同步交給控制模塊處理。控制模塊根據(jù)各模塊反饋的數(shù)據(jù)信息來(lái)合理分配兩電機(jī)的脈沖寬度調(diào)制值(Pulse Width Modulation，PWM)，以不斷地實(shí)時(shí)控制電機(jī)的行進(jìn)方向和速度，最終實(shí)現(xiàn)跟隨車(chē)安全高效的跟隨功能。

2 硬件設(shè)計(jì)

    智能跟隨車(chē)的目標(biāo)物為具有信號(hào)收發(fā)功能的手環(huán)。智能跟隨車(chē)具有控制單元、避障模塊、兩個(gè)射頻模塊、高度補(bǔ)償傳感器、重力傳感器、深度傳感器、平衡模塊和驅(qū)動(dòng)模塊。車(chē)底盤(pán)的四角共設(shè)有4個(gè)避障模塊，前后兩端共設(shè)有2個(gè)深度傳感器，底盤(pán)的前端設(shè)有2個(gè)射頻模塊和1個(gè)高度傳感器，底盤(pán)的后端設(shè)有1個(gè)平衡模塊。其他部件也都設(shè)置在底盤(pán)上。硬件總體結(jié)構(gòu)如圖2所示。

2.1 射頻模塊

    兩個(gè)射頻模塊采用的是UWB Mini 3s模塊，以STM32F103T8U6為主控芯片，外圍電路包括DWM1000模塊、電源模塊、LED指示模塊和復(fù)位電路等。該模塊基站標(biāo)簽為一體，通過(guò)撥碼開(kāi)關(guān)切換基站和標(biāo)簽功能。其作用是接收手環(huán)發(fā)來(lái)的信息，并傳送給控制模塊。

2.2 高度補(bǔ)償傳感器

    高度補(bǔ)償傳感器用于確定手環(huán)與智能跟隨車(chē)之間的垂直距離，將所收集到的信息反饋到控制模塊。其作用是優(yōu)化箱子的位置信息，使定位更加準(zhǔn)確。

2.3 重力傳感器

    重力傳感器用來(lái)測(cè)量智能跟隨車(chē)所裝行李的重量。當(dāng)智能跟隨車(chē)內(nèi)裝載有不同重量的行李時(shí)，相應(yīng)調(diào)節(jié)電機(jī)轉(zhuǎn)速，保證跟隨車(chē)在裝不同重量的行李下都可以跟上手環(huán)持有者的速度。

3 軟件設(shè)計(jì)

3.1 主程序設(shè)計(jì)

    智能跟隨車(chē)上電后，首先進(jìn)行系統(tǒng)的初始化，包括時(shí)鐘配置、定時(shí)器初始化以及調(diào)試程序時(shí)所需串口的初始化。接著進(jìn)行DWM1000模塊初始化。初始化工作完成后，判斷基站是否收到了標(biāo)簽發(fā)送的中斷信號(hào)，若是，則基站將信息傳給單片機(jī)計(jì)算出所需的距離和角度數(shù)據(jù)，經(jīng)由PID控制算法調(diào)節(jié)電機(jī)驅(qū)動(dòng)模塊的PWM值，從而控制電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向；否則返回繼續(xù)進(jìn)行該判斷。本設(shè)計(jì)中智能跟隨車(chē)的軟件設(shè)計(jì)流程如圖3所示。

3.2 定位原理與算法描述

    本節(jié)算法設(shè)計(jì)的目的為計(jì)算出手持標(biāo)簽到兩個(gè)基站中點(diǎn)的距離，以及偏離兩基站所在直線的角度，并通過(guò)串口輸出在顯示屏上，便于觀測(cè)和記錄。具體算法描述如下：圖4所示為實(shí)驗(yàn)定位算法原理圖，圖中點(diǎn)A為手持標(biāo)簽，點(diǎn)B和點(diǎn)C為智能跟隨車(chē)上的兩個(gè)固定基站，且兩基站之間距離固定為33 cm，點(diǎn)D為兩個(gè)基站的中點(diǎn)，線段邊上的高。手持標(biāo)簽A到兩個(gè)基站B和C的距離通過(guò)TOA方法得到。

    海倫公式為：

3.3 卡爾曼濾波程序設(shè)計(jì)

    卡爾曼濾波是一種利用線性系統(tǒng)狀態(tài)方程；通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。由于觀測(cè)數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，因此也可看作是濾波過(guò)程。首先引入一個(gè)線性隨機(jī)微分方程：

4 測(cè)試數(shù)據(jù)及誤差分析

4.1 實(shí)驗(yàn)設(shè)計(jì)

    為了測(cè)試本設(shè)計(jì)中智能跟隨車(chē)的定位精度，設(shè)計(jì)了如下實(shí)驗(yàn)，實(shí)驗(yàn)示意圖如圖5所示。圖中右側(cè)智能跟隨車(chē)為俯視圖，其中A和B為智能跟隨車(chē)上方兩腳固定的兩個(gè)基站，C為可移動(dòng)的手持標(biāo)簽，D為A、B兩個(gè)基站的中點(diǎn)。

    在本實(shí)驗(yàn)設(shè)計(jì)中，共選取了9個(gè)點(diǎn)進(jìn)行測(cè)試，在圖5中分別編號(hào)為1～9。其中點(diǎn)1～3的實(shí)際距離均為50 cm，實(shí)際角度分別為45°、60°和90°；點(diǎn)4～6的實(shí)際距離均為100 cm，實(shí)際角度分別為45°、60°和90°；點(diǎn)7～9的實(shí)際距離均為150 cm，實(shí)際角度分別為45°、60°和90°。

    利用3.2中所介紹的算法，可以求解出圖中的距離以及∠CDB的角度，并通過(guò)串口輸出顯示出來(lái)。在實(shí)驗(yàn)中對(duì)每個(gè)點(diǎn)均選取100組連續(xù)的測(cè)試數(shù)據(jù)，并對(duì)所有數(shù)據(jù)取幾何平均值作為每個(gè)點(diǎn)的測(cè)量值，以避免均值受極端值的較大影響。最后，將測(cè)量值與實(shí)際值進(jìn)行對(duì)比，分析誤差。

4.2 測(cè)量數(shù)據(jù)

    經(jīng)過(guò)實(shí)驗(yàn)，測(cè)出實(shí)驗(yàn)數(shù)據(jù)并在極坐標(biāo)中畫(huà)出每個(gè)點(diǎn)對(duì)應(yīng)的距離和角度，得到如圖6所示的測(cè)量數(shù)據(jù)點(diǎn)圖。

    圖6中點(diǎn)1～3為實(shí)際距離50 cm，實(shí)際角度分別是45°、60°和90°對(duì)應(yīng)點(diǎn)的測(cè)量值；點(diǎn)4～6為實(shí)際距離100 cm，實(shí)際角度分別是45°、60°和90°對(duì)應(yīng)點(diǎn)的測(cè)量值；點(diǎn)7～9為實(shí)際距離150 cm，實(shí)際角度分別為45°、60°和90°對(duì)應(yīng)點(diǎn)的測(cè)量值。

    將每個(gè)點(diǎn)測(cè)量值的距離和角度分別與實(shí)際值的距離和角度作差，得到各個(gè)點(diǎn)對(duì)應(yīng)的距離誤差和角度誤差數(shù)據(jù)，并繪制得到如圖7所示的距離誤差數(shù)據(jù)圖和如圖8所示的角度誤差數(shù)據(jù)圖。

4.3 誤差分析

    由圖7和圖8可得，本實(shí)驗(yàn)中距離的誤差在-2 cm～+9 cm之間，角度的誤差在-8°～+10°之間，滿足設(shè)計(jì)和定位的需求。

    從實(shí)驗(yàn)的過(guò)程來(lái)看，實(shí)驗(yàn)數(shù)據(jù)誤差的來(lái)源可能有：

    (1)手持標(biāo)簽不穩(wěn)定。由于實(shí)驗(yàn)過(guò)程中可移動(dòng)標(biāo)簽由人手持，在實(shí)驗(yàn)中可能會(huì)有輕微的不穩(wěn)定，會(huì)導(dǎo)致數(shù)據(jù)有偏差。

    (2)實(shí)際距離和角度標(biāo)定的誤差。兩個(gè)基站的距離和九個(gè)測(cè)試點(diǎn)在實(shí)際操作中要用卷尺測(cè)距進(jìn)行定點(diǎn)，在此過(guò)程中可能會(huì)有測(cè)量誤差存在。

    (3)手持標(biāo)簽和基站所在平面不水平。當(dāng)手持標(biāo)簽與基站所在平面不水平時(shí)，標(biāo)簽到基站的距離會(huì)比實(shí)際值偏大，導(dǎo)致距離測(cè)量值偏大，角度測(cè)量值也會(huì)偏大。

    (4)數(shù)據(jù)位數(shù)的丟失。由于在軟件部分程序的計(jì)算中，數(shù)據(jù)的小數(shù)部分在運(yùn)算中會(huì)被截?cái)?，并不能得到精確的數(shù)據(jù)，也會(huì)導(dǎo)致數(shù)據(jù)的誤差。

5 結(jié)束語(yǔ)

    本文設(shè)計(jì)了一種可以有效融合各個(gè)功能模塊，集定位、避障、平衡、跟隨、網(wǎng)絡(luò)于一體的安全、可靠、高效的智能跟隨裝置、控制方法及定位方法，通過(guò)實(shí)驗(yàn)驗(yàn)證，本設(shè)計(jì)中智能跟隨車(chē)定位的距離誤差不超過(guò)9 cm，角度誤差不超過(guò)10°，滿足設(shè)計(jì)需求。在后續(xù)的實(shí)驗(yàn)設(shè)計(jì)中，會(huì)繼續(xù)優(yōu)化軟件部分的設(shè)計(jì)，使實(shí)驗(yàn)誤差進(jìn)一步縮小。本設(shè)計(jì)系統(tǒng)相比于現(xiàn)有技術(shù)的控制和定位方法，可以使智能跟隨車(chē)更加安全有效地跟隨主人，不會(huì)出現(xiàn)丟失目標(biāo)或者路徑紊亂的情況，在市場(chǎng)上也有廣泛的應(yīng)用價(jià)值，對(duì)國(guó)內(nèi)智能跟隨領(lǐng)域的成長(zhǎng)與進(jìn)步也具有重要的意義。

參考文獻(xiàn)

[1] 丁柏文，金琦淳，任俊，等.基于IAP15F2K61S2單片機(jī)的紅外與無(wú)線搬運(yùn)小車(chē)[J].儀表技術(shù)與傳感器，2017(10)：110-114.

[2] 王志鎧.基于UWB無(wú)線定位技術(shù)的AGV室內(nèi)導(dǎo)航與控制技術(shù)研究[D].成都：電子科技大學(xué)，2018.

[3] 趙昊寧，聶憲波，關(guān)立強(qiáng)，等.自動(dòng)跟隨運(yùn)輸系統(tǒng)設(shè)計(jì)[J].自動(dòng)化應(yīng)用，2015(12)：69-71.

[4] 劉磊，孫曉菲，張煜.基于STM32的可遙控智能跟隨小車(chē)設(shè)計(jì)[J].電子測(cè)量技術(shù)，2015(6)：31-33.

[5] 王飛.基于UWB的室內(nèi)移動(dòng)機(jī)器人導(dǎo)航定位技術(shù)的研究[D].哈爾濱：哈爾濱工程大學(xué)，2017.

[6] 賀晶晶，姜平，馮曉榮.基于UWB的無(wú)人運(yùn)輸車(chē)的導(dǎo)航定位算法研究[J].電子測(cè)量與儀器學(xué)報(bào)，2016，30(11)：1743-1749.

[7] 蔡磊，周亭亭，郭云鵬，等.基于超聲波定位的智能跟隨小車(chē)[J].電子測(cè)量技術(shù)，2013，36(11)：76-79.

[8] 盧靖宇，余文濤，趙新，等.基于超寬帶的移動(dòng)機(jī)器人室內(nèi)定位系統(tǒng)設(shè)計(jì)[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的中心車(chē)輛定位系統(tǒng)的算法研究[J].通信技術(shù)，2001(5)：23-25.

[11] 薛輝，王叢嶺.基于單片機(jī)采用PID算法的電機(jī)運(yùn)行控制系統(tǒng)設(shè)計(jì)[J].機(jī)床電器，2006，33(1)：51-52.

[12] 李炳宇，蕭蘊(yùn)詩(shī)，汪鐳.PSO算法在工程優(yōu)化問(wèn)題中的應(yīng)用[J].計(jì)算機(jī)工程與應(yīng)用，2004，40(18)：74-76.

作者信息:

胡海兵1，2，張文達(dá)1，2，鄭希鵬1，2，曾貴苓3

（1.合肥工業(yè)大學(xué) 光電技術(shù)研究院 特種顯示技術(shù)國(guó)家工程實(shí)驗(yàn)室，安徽 合肥230009；

2.合肥工業(yè)大學(xué) 光電技術(shù)研究院 現(xiàn)代顯示技術(shù)省部共建國(guó)家重點(diǎn)實(shí)驗(yàn)室，安徽 合肥230009；

3.蕪湖職業(yè)技術(shù)學(xué)院 電氣工程學(xué)院，安徽 蕪湖241006）