超聲波定位智能小車自動(dòng)跟隨算法

夏曉楓 楊佳麗 寧維陽 吳博 高鋒

摘? 要：智能小車中的自動(dòng)跟隨算法應(yīng)用廣泛，該文研究了一種基于IAP15F2K61S2單片機(jī)的超聲波定位自動(dòng)跟隨、自動(dòng)避障智能小車。小車系統(tǒng)通過車體正前方等距分布并由舵機(jī)控制的3個(gè)超聲波模塊，實(shí)時(shí)測出了與目標(biāo)物的3個(gè)距離，并通過文中設(shè)計(jì)的幾何算法——距離差值比較法、平面坐標(biāo)法，求出被跟隨目標(biāo)的坐標(biāo)位置信息，再以坐標(biāo)情況執(zhí)行不同的跟隨動(dòng)作。

關(guān)鍵詞：自動(dòng)跟隨? 智能小車? 單片機(jī)? 超聲波測距? 超聲波定位

中圖分類號(hào)：TP274? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼：A? ? ? ? ? ? ? ? ? ? ? ? ? 文章編號(hào)：1672-3791（2019）03（a）-0025-03

相比汽車，智能小車是一種輪式機(jī)器人，有安全、環(huán)保、應(yīng)用廣等優(yōu)越性，可以解決人們許多問題，如無人車間工作、教育機(jī)器人等。該文研究一種智能小車，運(yùn)用多種傳感器，在能避障的同時(shí)跟隨目標(biāo)行駛，設(shè)備供電后無需人為干預(yù)，解放雙手，方便人們生活。

1? 總體方案設(shè)計(jì)

小車總體設(shè)計(jì)：（1）系統(tǒng)由IAP15F2K61S2單片機(jī)、電源、電機(jī)驅(qū)動(dòng)、超聲波測距、紅外對管避障、蜂鳴器報(bào)警等硬件組成;（2）Arduino uno和舵機(jī)掃描電路;（3）兩控制芯片通信及它們各自軟件編程調(diào)試。

具體工作原理：超聲波模塊在預(yù)設(shè)距離內(nèi)檢測前方是否有目標(biāo)，如果有則自主跟隨。同時(shí)紅外對管檢測前方障礙物，當(dāng)檢測到有障礙物且小于預(yù)設(shè)值時(shí)把信息反饋給單片機(jī)，單片機(jī)控制驅(qū)動(dòng)電路調(diào)整車輪轉(zhuǎn)動(dòng)繞過障礙物繼續(xù)跟隨。跟隨過程中舵機(jī)不動(dòng)，停止時(shí)舵機(jī)轉(zhuǎn)動(dòng)直到超聲波模塊掃描到目標(biāo)物為止。小車能短程加減速后再穩(wěn)速行駛，蜂鳴器報(bào)警功能僅用以在小車脫離跟隨范圍后提醒。所用模塊較少且便宜，制作成本低。不僅四驅(qū)結(jié)構(gòu)平穩(wěn)，而且模塊布局對整體外觀結(jié)構(gòu)影響小，為后續(xù)設(shè)計(jì)提供方便性和靈活性。

2? 系統(tǒng)硬件設(shè)計(jì)

系統(tǒng)硬件由MCU搭載多個(gè)硬件模塊并配合其他電路構(gòu)成，系統(tǒng)硬件框圖如圖1。

2.1 主控芯片

IAP15F2K61S2是STC生產(chǎn)的單時(shí)鐘新一代8051單片機(jī)，工作電壓5V，不需晶振且內(nèi)部資源豐富。用匯編、C等語言編程。Arduino由基于單片機(jī)的開源硬件平臺(tái)和專門的開發(fā)環(huán)境組成，可用來開發(fā)交互產(chǎn)品。小車采用的uno微處理器是ATmega328，輸入電壓7～12V，用Arduino語言、scratch積木式編程皆可編程。

2.2 硬件模塊

該系統(tǒng)采用硬件模塊包括直流電源（給整個(gè)系統(tǒng)供電）、直流電機(jī)驅(qū)動(dòng)模塊（驅(qū)動(dòng)直流電機(jī)）、超聲波模塊（實(shí)時(shí)測量與目標(biāo)物距離）、紅外避障模塊（跟隨過程中避開障礙物）、舵機(jī)（承載超聲波模塊上下掃描來擴(kuò)大超聲波覆蓋面積）、蜂鳴器（當(dāng)小車脫離正前方0.5～2m的跟隨范圍后報(bào)警）。

3? 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件為設(shè)計(jì)的關(guān)鍵，小車程序分以下幾部分：單片機(jī)主程序，測距子程序，比較距離差值子程序，平面幾何定位子程序，避障子程序，調(diào)速子程序;uno舵機(jī)掃描程序。單片機(jī)為主控核心，其主程序流程圖如圖1。

4? 核心技術(shù)

4.1 超聲波測距

系統(tǒng)采用超聲波渡越時(shí)間檢測法。距離計(jì)算公式：。其中：d為被測物與測物的距離，s為來回聲波路程，v為聲速，t為聲波來回的時(shí)間。由于聲波在空氣中傳播速度與溫度有關(guān)，故傳播速度查表可得近似公式v=332+0.607Tm/s，距離公式：S=（332+0.607T）t/2其中T為空氣溫度，t為聲波傳播時(shí)間。

4.2 定位方法

我們把3個(gè)超聲波模塊分別測一次距當(dāng)作一個(gè)完整測距周期，一個(gè)測距周期總時(shí)長約0.3s，各測距周期互不影響，一個(gè)測距周期內(nèi)所得3個(gè)距離值作為一組數(shù)據(jù)使用（見圖2）。

4.2.1 距離差值比較法

在一個(gè)測距周期內(nèi)，利用小車正前方3個(gè)等距離分布超聲波模塊測得的與目標(biāo)物的3個(gè)距離值，與跟隨范圍0.5～2m做對比，依據(jù)3個(gè)距離值在區(qū)域（0.5m及以內(nèi)、2m及以外、0.5～2m之間）里的分布情況，做出相應(yīng)運(yùn)動(dòng)方向和狀態(tài)的判斷。具體如下。

4、5、11、12狀態(tài)下，目標(biāo)物在跟隨范圍外，小車停止并蜂鳴報(bào)警。

1、8、10、13狀態(tài)下，已有兩個(gè)距離值在范圍外，小車瀕臨跟丟，此時(shí)亦停下報(bào)警。

2、7、9、14狀態(tài)下，仍有兩個(gè)距離值在范圍內(nèi)，小車須趕緊調(diào)整狀態(tài)以防跟丟。此時(shí)根據(jù)還在范圍內(nèi)的兩個(gè)距離值進(jìn)行差值比較估計(jì)偏轉(zhuǎn)角度，超過預(yù)定值則轉(zhuǎn)彎，否則繼續(xù)直行。

3、6狀態(tài)下，3個(gè)距離值都在范圍內(nèi)，小車正常跟隨。此時(shí)先根據(jù)最大的距離值判斷轉(zhuǎn)彎方向，再依據(jù)較小的兩個(gè)距離值進(jìn)行差值比較估計(jì)偏轉(zhuǎn)角度，超過預(yù)定值則朝此方向轉(zhuǎn)彎，否則繼續(xù)直行。

小車偏離目標(biāo)物程度的估定：將3個(gè)距離值兩兩做差，3個(gè)差值均小于預(yù)設(shè)值就認(rèn)為小車微偏需調(diào)整方向糾正，否則認(rèn)為偏離在非可調(diào)狀態(tài)，保持原態(tài)。如上，單片機(jī)控制小車正確調(diào)整運(yùn)動(dòng)狀態(tài)跟隨目標(biāo)物。

4.2.2 平面坐標(biāo)法

正常跟隨過程中小車運(yùn)行是一個(gè)不間斷的過程，即小車一直行駛著，故對于同一目標(biāo)物來說，即使其在原地不動(dòng)，它相對于小車的坐標(biāo)位置也會(huì)稍微改變。目標(biāo)物的速度較低，在0.3s的周期內(nèi)其真實(shí)位置不會(huì)有太大改變，故可認(rèn)為一次測距周期內(nèi)目標(biāo)物的真實(shí)位置不變。系統(tǒng)采用3個(gè)超聲波模塊測距，實(shí)則是在3個(gè)不同的時(shí)刻分別測了1個(gè)距離值，但因小車速度較慢，測距周期短，所以3個(gè)距離值相近。此方法將目標(biāo)物理想化（距離較遠(yuǎn)時(shí)）視作一個(gè)點(diǎn)（X，Y），如圖建立平面直角坐標(biāo)系。由3個(gè)距離值理論上得到如下3個(gè)方程。

但實(shí)際上由3個(gè)距離值得到3個(gè)不同的但又相接近的坐標(biāo)值（X1，Y1）、（X2，Y2）、（X3，Y3）。將其中兩兩理想化視為橫縱坐標(biāo)值相等，即令上述3個(gè)方程兩兩相等可得到三組不同的解，分別作為（X1，Y1）、（X2，Y2）、（X3，Y3）。最后分別取X1、X2、X3平均值作為X，取Y1、Y2、Y3平均值作為Y，可得到目標(biāo)物相對于小車此時(shí)的位置（X，Y）的近似值。

3組解及坐標(biāo)計(jì)算公式如下。

單片機(jī)再依據(jù)得出的坐標(biāo)，合理地做出讓小車調(diào)整運(yùn)轉(zhuǎn)狀態(tài)的命令，控制其跟隨目標(biāo)物。

4.3 紅外避障

小車運(yùn)動(dòng)過程中，紅外發(fā)射管發(fā)射一定頻率的紅外線，當(dāng)檢測方向遇到障礙物時(shí)紅外線反射回來被接收管接收，經(jīng)模塊內(nèi)部比較器電路處理，信號(hào)輸出口會(huì)輸出低電平數(shù)字信號(hào)，單片機(jī)讀取該口電平信號(hào)，可判斷前方是否存在障礙物。

4.4 舵機(jī)掃描

車停時(shí)舵機(jī)承載固定有3個(gè)超聲波模塊的云臺(tái)上下掃描。直到掃描到目標(biāo)物后舵機(jī)停止轉(zhuǎn)動(dòng)并鎖定角度，保持此角度小車開始運(yùn)動(dòng)跟隨目標(biāo)物，以此擴(kuò)大超聲波覆蓋面積。舵機(jī)由uno單獨(dú)控制，單片機(jī)的一個(gè)引腳做輸出與uno的一個(gè)引腳做輸入通過導(dǎo)線連接，稱信號(hào)線。上電后uno讀取線上其引腳的輸入電平，若為低則舵機(jī)來回轉(zhuǎn)動(dòng)，若為高則舵機(jī)不動(dòng)。至于信號(hào)線的電平，由單片機(jī)據(jù)小車運(yùn)動(dòng)狀態(tài)來對應(yīng)置高或拉低，車動(dòng)置高，車停拉低。

4.5 電機(jī)調(diào)速

對所用直流電機(jī)轉(zhuǎn)速的調(diào)控，用單片機(jī)軟件模擬PWM波方法實(shí)現(xiàn)。PWM波有數(shù)字信號(hào)輸出特點(diǎn)可有效降噪。為保證小車平穩(wěn)運(yùn)行，采用可調(diào)占空比的PWM波，可實(shí)現(xiàn)小車加速、減速、壓制速度行駛，某些特殊場合能夠變速，同時(shí)也可保證小車跟隨的可靠性。

5? 系統(tǒng)測試與誤差分析

5.1 系統(tǒng)測試

雖12V電源供電，但小車速度被調(diào)控得很好，距目標(biāo)物距離不適需加減速時(shí)，可明顯表現(xiàn)出加減速過程，其他時(shí)刻穩(wěn)定速度跟隨目標(biāo)物行駛。小車整體平穩(wěn)，可在0.5～2m范圍內(nèi)實(shí)現(xiàn)180°的、自動(dòng)繞過障礙物的自主跟隨行駛。在脫離該跟隨范圍后能自動(dòng)蜂鳴報(bào)警，舵機(jī)能在小車不動(dòng)時(shí)轉(zhuǎn)動(dòng)，在小車運(yùn)動(dòng)時(shí)停止。小車的整體測試效果與預(yù)期相差不大，系統(tǒng)測試正常。

5.2 誤差分析及減少措施

經(jīng)分析，給小車帶來跟隨效果誤差的因素主要有以下這些：傳播過程中超聲波信號(hào)衰減，可在同一位置多添置幾個(gè)等價(jià)超聲波模塊來改善，擴(kuò)大發(fā)射接收面，提高發(fā)射增益和接收靈敏度。聲波接收到與被單片機(jī)檢測到的時(shí)差、計(jì)時(shí)開始與觸發(fā)超聲波發(fā)射的時(shí)差，可優(yōu)化軟件代碼來做部分彌補(bǔ)。系統(tǒng)硬件電路連線存在一些弊端與瑕疵，可印制電路板然后焊接減少接線，優(yōu)化硬件線路來調(diào)整優(yōu)化。

6? 結(jié)語

總體來說項(xiàng)目設(shè)計(jì)很成功，在老師指導(dǎo)和團(tuán)隊(duì)努力下，順利實(shí)現(xiàn)超聲波定位跟隨、紅外避障、舵機(jī)掃描、蜂鳴器報(bào)警、小車調(diào)速功能，達(dá)到預(yù)期效果。小車后續(xù)設(shè)計(jì)可以為系統(tǒng)添加無線模塊NRF24L01，將超聲波發(fā)射頭接收頭分開，小車配置接收頭，持拿發(fā)射頭，可令小車與目標(biāo)物一對一匹配，實(shí)現(xiàn)特定跟隨。

參考文獻(xiàn)

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

[2] 張文霞，張凱，喬良，等.基于單片機(jī)的智能跟隨小車的設(shè)計(jì)與實(shí)現(xiàn)[J].科技與企業(yè)，2015（12）：251.

[3] 陳呂洲.Arduino程序設(shè)計(jì)基礎(chǔ)[M].2版.北京：北京航空航天大學(xué)出版社，2015.