本發(fā)明自動控制技術(shù)領(lǐng)域，涉及一種移動平臺自動避障系統(tǒng)，尤其大中型移動平臺自動避障系統(tǒng)。

背景技術(shù)：

隨著時代的發(fā)展和科學(xué)的進(jìn)步，具有人機(jī)交互接口的移動平臺智能化水平不斷提高，而自主避障功能則是移動平臺適應(yīng)未知復(fù)雜環(huán)境的智能化重要體現(xiàn)之一，如何以簡易高效的算法實現(xiàn)移動平臺自主避障具有重要應(yīng)用前景。

超聲波具有較好的方向性、較強(qiáng)的適應(yīng)性以及穿透能力強(qiáng)等特點，因此往往被加載在移動平臺上實現(xiàn)避障功能。目前應(yīng)用于移動平臺的超聲波傳感器的波束角一般為小角度，通常不大于30°，而市場上常用的超聲波傳感器波束角一般為15°左右，這使得傳統(tǒng)超聲波陣列的檢測盲區(qū)往往較大，降低移動平臺避障的安全性和智能性；或移動平臺體積較大不得不加載多個超聲波傳感器實現(xiàn)避障功能，增加控制復(fù)雜性且需投入更多資源。如何加載盡量少資源且盡可能減小檢測盲區(qū)實現(xiàn)大中型移動平臺安全成功的自主避障具有重要應(yīng)用價值。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的是提供一種應(yīng)用于移動平臺的超聲波陣列避障系統(tǒng)，加載盡量少的超聲波傳感器且盡可能減小檢測盲區(qū)，提高較大移動平臺自主避障的安全性、智能性及可靠性。本發(fā)明特別適應(yīng)于移動平臺寬度(x)30cm≦x≦50cm的大中型移動平臺的超聲波陣列避障系統(tǒng)為宜，同樣也適用于移動平臺寬度≦30cm的小型移動平臺。

本發(fā)明是通過以下技術(shù)方案實現(xiàn)。

本發(fā)明所述的一種應(yīng)用于大中型移動平臺的超聲波陣列避障系統(tǒng)，硬件部分包括stm32處理器，電源模塊，溫度補(bǔ)償模塊，電機(jī)驅(qū)動模塊，測速模塊，通信模塊以及超聲波測距模塊，其中超聲波測距模塊由六個超聲波傳感器組成，每個傳感器均由超聲波發(fā)射電路及超聲波接收電路構(gòu)成。

本發(fā)明所述的超聲波傳感器設(shè)置位置如圖2所示，首先超聲波傳感器一和超聲波傳感器二分別用來檢測移動平臺左前方和左方區(qū)域，超聲波傳感器一擺放在超聲波傳感器二上面，共同擺放在移動平臺后端的最左側(cè)；超聲波傳感器三和超聲波傳感器四用來檢測移動平臺正前方區(qū)域，分別擺放在移動平臺后端(1/4)x和(3/4)x處；超聲波傳感器五和超聲波傳感器六分別用來檢測移動平臺左方和左前方區(qū)域，超聲波傳感器六擺放在超聲波傳感器五上面，共同擺放在移動平臺后端的最右側(cè)。然后以水平為基準(zhǔn)，超聲波傳感器一逆時針旋轉(zhuǎn)3α/2，超聲波傳感器二逆時針旋轉(zhuǎn)α/2，超聲波傳感器三順時針旋轉(zhuǎn)β，超聲波傳感器四逆時針旋轉(zhuǎn)δ，超聲波傳感器五順時針旋轉(zhuǎn)α/2，超聲波傳感器六順時針旋轉(zhuǎn)3α/2。

其中x為移動平臺寬度；角度α為超聲波傳感器的波束角(10°＜α＜30°)；角度β＝∠f-(90°-α/2)；角度δ＝∠f-(90°-α/2)；根據(jù)附圖3，角度f計算如下：

令ea＝fd

即ac*tan∠e＝cd*tan∠f

ac*tan∠(180°-α-∠f)＝cd*tan∠f

其中∠e為三角形eac中邊ea所對應(yīng)的內(nèi)角，∠f為三角形fdc中邊f(xié)d所對應(yīng)的內(nèi)角，a、b、c、d四個點分別為超聲波傳感器一，超聲波傳感器三，超聲波傳感器四，超聲波傳感器六擺放位置的中點，e點為超聲波傳感器三的波束角的左邊沿與移動平臺左側(cè)前向延長線的交點，f點為超聲波傳感器四的波束角的右邊沿與移動平臺右側(cè)前向延長線的交點，角度f大小要滿足ea＝fd。故公式中ac，cd，α均為已知，可由條件等式解出∠f。

結(jié)合附圖3可知，此種超聲波陣列擺放方式優(yōu)點：

(1)超聲波傳感器三和超聲波傳感器四檢測完全覆蓋移動平臺正前方區(qū)域，保障其安全有效直行運(yùn)動。

(2)超聲波傳感器二和超聲波傳感器五檢測完全覆蓋移動平臺左右側(cè)區(qū)域，保障其安全有效轉(zhuǎn)彎運(yùn)動。

(3)超聲波傳感器一和超聲波傳感器六檢測移動平臺左前方和右前方區(qū)域，預(yù)判其轉(zhuǎn)彎后障礙物情況，完全避免左右搖擺避障的影響并在很大程度上減少二次避障的可能。

(4)超聲波傳感器擺放在移動平臺后端有助于減小因波束角產(chǎn)生的檢測盲區(qū)。

本發(fā)明所述的一種應(yīng)用于移動平臺的超聲波陣列避障系統(tǒng)，軟件部分采用二分法和模糊控制相結(jié)合的避障算法。即首先根據(jù)超聲波傳感器三和超聲波傳感器四測量到的障礙物的距離，采用二分法判斷移動平臺前方障礙物是偏左還是偏右，再繼續(xù)根據(jù)其他超聲波傳感器測量到的障礙物的距離，用模糊控制的方法確定移動平臺轉(zhuǎn)動的角度。如附圖3所示，其中二分法即通過比較超聲波傳感器三和超聲波傳感器四到3.1、3.2區(qū)域內(nèi)障礙物距離的大小來判斷出移動平臺前方障礙物是偏左還是偏右；模糊控制即把超聲波傳感器一、超聲波傳感器二、超聲波傳感器五、超聲波傳感器六檢測到障礙物的距離做如下模糊處理：超聲波傳感器一和超聲波傳感器六檢測距離分為(近，遠(yuǎn)，很遠(yuǎn))，超聲波傳感器二、超聲波傳感器五檢測距離分為(近，遠(yuǎn))，設(shè)j為檢測距離，則近是指0＜j＜n，遠(yuǎn)是指n＜j＜l，很遠(yuǎn)是指j＞l，其中n＝x/sin(2α)厘米，l＝x/sin(α)厘米，x為平臺寬度，α為超聲波傳感器波束角，實際距離劃分也應(yīng)根據(jù)實際情況而定。具體軟件算法如附圖4所示：

其中距離i(i＝1，2，3，4，5，6)為超聲波傳感器j(j為一、二、三、四、五、六)檢測到障礙物的距離。

首先，檢測距離3和距離4，若兩者均大于l厘米，則該障礙物不在考慮范圍內(nèi)，移動平臺繼續(xù)保持前行。否則，移動平臺需要對該障礙物進(jìn)行避障處理，具體避障方法如下：

若距離3大于距離4則進(jìn)入避障狀態(tài)1，否則進(jìn)入避障狀態(tài)2。

避障狀態(tài)1：

情況一：檢測距離1和距離2，若距離1且距離2均大于n厘米，進(jìn)一步檢測距離1，若距離1大于l厘米則移動平臺左轉(zhuǎn)2α；否則檢測距離5和距離6，若距離5大于n厘米且距離6大于l厘米則移動平臺右轉(zhuǎn)2α，否則移動平臺左轉(zhuǎn)并同時檢測距離1和距離2，若距離1且距離2均大于n厘米則移動平臺左轉(zhuǎn)至4α，否則移動平臺停止運(yùn)動，返回避障狀態(tài)重新檢測距離3和距離4。

情況二：檢測距離1和距離2，若距離1或距離2小于n厘米，則檢測距離5和距離6，若距離5或距離6小于n厘米則移動平臺停止運(yùn)動；否則進(jìn)一步檢測距離6，若距離6大于l厘米則移動平臺右轉(zhuǎn)2α，否則移動平臺右轉(zhuǎn)并同時檢測距離5和距離6，若距離5且距離6均大于n厘米則移動平臺右轉(zhuǎn)至4α，否則移動平臺停止運(yùn)動。返回避障狀態(tài)重新檢測距離3和距離4。

避障狀態(tài)2：

情況一：檢測距離5和距離6，若距離5且距離6均大于n厘米，進(jìn)一步檢測距離6，若距離6大于l厘米則移動平臺右轉(zhuǎn)2α；否則檢測距離1和距離2，若距離2大于n厘米且距離1大于l厘米則移動平臺左轉(zhuǎn)2α，否則移動平臺右轉(zhuǎn)并同時檢測距離5和距離6，若距離5且距離6均大于n厘米則移動平臺右轉(zhuǎn)至4α，否則移動平臺停止運(yùn)動，返回避障狀態(tài)重新檢測距離3和距離4。

情況二：檢測距離5和距離6，若距離5或距離6小于n厘米，則檢測距離1和距離2，若距離1或距離2小于n厘米則移動平臺停止運(yùn)動；否則進(jìn)一步檢測距離1，若距離1大于l厘米則移動平臺左轉(zhuǎn)2α，否則移動平臺左轉(zhuǎn)并同時檢測距離1和距離2，若距離1且距離2均大于n厘米則移動平臺左轉(zhuǎn)至4α，否則移動平臺停止運(yùn)動。返回避障狀態(tài)重新檢測距離3和距離4。

以上避障方法中的距離n厘米和l厘米實際應(yīng)用時可以根據(jù)移動機(jī)器人車體大小以及運(yùn)行速度進(jìn)行適當(dāng)調(diào)整。

結(jié)合附圖3和附圖4可知，此種避障方式的優(yōu)點：

(1)能夠全覆蓋檢測出移動平臺運(yùn)行前方的障礙物情況，并判斷出具體位置是左還是偏右，便于更智能的優(yōu)先選擇向左或右轉(zhuǎn)彎進(jìn)行避障。如若3.1、3.2區(qū)內(nèi)有障礙物，本專利算法通過比較超聲波傳感器三和超聲波傳感器四到障礙物距離的大小來判斷出移動平臺前方障礙物具體位置是偏左還是偏右。傳統(tǒng)避障方式或是不能實現(xiàn)全覆蓋檢測，或是只能初步判斷出障礙物在前方，不能夠進(jìn)一步確定障礙物具體位置。

(2)根據(jù)移動平臺前方障礙物偏左或偏右優(yōu)先選擇相反方向進(jìn)行避障，提高避障智能性。如若3.2區(qū)內(nèi)有障礙物，移動平臺優(yōu)先左轉(zhuǎn)避障；如若3.2區(qū)和3.8區(qū)有障礙物，移動平臺選擇右轉(zhuǎn)避障；如若3.2區(qū)，3.8區(qū)和3.12區(qū)(或3.11區(qū))有障礙物，移動平臺選擇左轉(zhuǎn)大角度避障。傳統(tǒng)避障方式只能實現(xiàn)左轉(zhuǎn)或右轉(zhuǎn)避障，不能根據(jù)移動平臺周邊障礙物情況來選擇最佳避障路徑。

(3)確定較佳的轉(zhuǎn)彎避障角度，更為精確地實現(xiàn)移動平臺的轉(zhuǎn)彎避障。如若3.2區(qū)有障礙物，移動平臺優(yōu)先左轉(zhuǎn)小角度避障；如若3.2區(qū)3.8區(qū)和3.12區(qū)(或3.11區(qū))有障礙物，移動平臺選擇左轉(zhuǎn)大角度避障。傳統(tǒng)避障方式或是實現(xiàn)粗略轉(zhuǎn)彎避障或是只設(shè)定固定的轉(zhuǎn)彎角度，不能根據(jù)障礙物情況來選擇較佳避障角度。

(4)轉(zhuǎn)彎過程中同時全面覆蓋檢測移動平臺左右兩側(cè)障礙物情況，完全避免了移動平臺轉(zhuǎn)彎過程中碰撞情況的發(fā)生，提高運(yùn)行安全性。如左轉(zhuǎn)大角度避障的同時超聲波傳感器一和超聲波傳感器二檢測到障礙物存在，則移動平臺停止運(yùn)行。傳統(tǒng)避障方式或是轉(zhuǎn)彎過程沒有同時開啟檢測或是檢測范圍盲區(qū)較大，無法完全保障轉(zhuǎn)彎過程的安全性。

(5)預(yù)先判斷移動平臺左前方和右前方障礙物情況，有效減小移動平臺二次避障的可能，避免了移動平臺出現(xiàn)左右交替轉(zhuǎn)彎的的情況。如若3.2區(qū)和3.8區(qū)均有障礙物時，移動平臺將右轉(zhuǎn)避障，若沒有安置超聲波傳感器一檢測3.8區(qū)，移動平臺將先左轉(zhuǎn)，然后檢測到前方偏左有障礙物繼而再右轉(zhuǎn)回來，如此往復(fù)交替。

附圖說明

圖1為本發(fā)明硬件電路系統(tǒng)框圖。其中包括stm32處理器，電源模塊，溫度補(bǔ)償模塊，電機(jī)驅(qū)動模塊，測速模塊，通信模塊以及超聲波測距模塊。

圖2為本發(fā)明超聲波陣列擺放圖。其中移動平臺總寬為x厘米，1為超聲波傳感器一，2為超聲波傳感器二，3為超聲波傳感器三，4為超聲波傳感器四，5為超聲波傳感器五，6為超聲波傳感器六，超聲波傳感器一和超聲波傳感器六分別擺放在超聲波傳感器二和超聲波傳感器五上面。

圖3為本發(fā)明移動平臺超聲波檢測原理圖。其中：3.1為正前方偏左避障檢測區(qū)，3.2為正前方偏右避障檢測區(qū)，3.3為超聲波傳感器三單獨(dú)檢測區(qū)，3.4為超聲波傳感器四單獨(dú)檢測區(qū)，3.5和3.6為可能盲區(qū)，3.7為左側(cè)避障檢測區(qū)，3.8為左前方避障檢測區(qū)，3.9為左轉(zhuǎn)后二次避障檢測區(qū)，3.10和3.14為可能盲區(qū)，3.11為右側(cè)避障檢測區(qū)，3.12為右前方避障檢測區(qū)，3.13為右轉(zhuǎn)后二次避障檢測區(qū)。1為超聲波傳感器一逆時針旋轉(zhuǎn)3α/2，2為超聲波傳感器二逆時針旋轉(zhuǎn)α/2，3為超聲波傳感器三順時針旋轉(zhuǎn)β＝∠f-(90°-α/2)，4為超聲波傳感器四逆時針旋轉(zhuǎn)δ＝∠f-(90°-α/2)。a點為超聲波傳感器一和超聲波傳感器二的擺放中點，b點為超聲波傳感器三的擺放中點，c點為超聲波傳感器四的擺放中點，d點為超聲波傳感器五和超聲波傳感器六的擺放中點，e點為超聲波傳感器三的波束角的左邊沿與移動平臺左側(cè)前向延長線的交點，f點為超聲波傳感器四的波束角的右邊沿與移動平臺右側(cè)前向延長線的交點。(可能盲區(qū)在實際應(yīng)用過程中真正存在障礙物而檢測不到的概率很小。其中可能盲區(qū)3.5、3.6在直線運(yùn)行過程中不會存在，可能盲區(qū)3.10、3.14只是在轉(zhuǎn)彎時刻存在，若干時刻后也不會存在。)

圖4為本發(fā)明超聲波避障策略流程圖。其中距離i(i＝1，2，3，4，5，6)為超聲波傳感器j(j為一、二、三、四、五、六)檢測到障礙物的距離，n為超聲波傳感器檢測的近距離值為n＝x/sin(2α)厘米，l為超聲波傳感器檢測的遠(yuǎn)距離值為l＝x/sin(α)厘米，公式中x為車體寬度，α為超聲波傳感器的波束角，10°＜α＜30°。

圖5為本發(fā)明實施例超聲波陣列擺放圖。其中移動平臺為寬30cm，1為超聲波傳感器一逆時針旋轉(zhuǎn)22.5°，2為超聲波傳感器二逆時針旋轉(zhuǎn)7.5°，3為超聲波傳感器三順時針旋轉(zhuǎn)3.7°，4為超聲波傳感器四逆時針旋轉(zhuǎn)3.7°，5為超聲波傳感器五順時針旋轉(zhuǎn)7.5°，6為超聲波傳感器六順時針旋轉(zhuǎn)22.5°，超聲波傳感器一和超聲波傳感器六分別擺放在超聲波傳感器二和超聲波傳感器五上面。

圖6為本發(fā)明實施例移動平臺前方超聲波檢測原理圖。其中移動平臺為寬30cm，6.1為正前方偏左避障檢測區(qū)，6.2為正前方偏右避障檢測區(qū)，6.3和6.6為可能盲區(qū)，6.4為超聲波傳感器三單獨(dú)檢測區(qū)，6.5為超聲波傳感器四單獨(dú)檢測區(qū)，6.7為車體部分，3為超聲波傳感器三順時針旋轉(zhuǎn)3.7°，4為超聲波傳感器四逆時針旋轉(zhuǎn)3.7°。(可能盲區(qū)6.3、6.6實際在移動平臺前行過程中并不存在。)

圖7為本發(fā)明實施例移動平臺左右兩側(cè)及左前方和右前方超聲波檢測原理圖。其中7.1為左前方避障檢測區(qū)，7.2為左轉(zhuǎn)后二次避障檢測區(qū)，7.3為左側(cè)避障檢測區(qū)，7.4和7.6為可能盲區(qū)，7.5為右側(cè)避障檢測區(qū)，7.7為右轉(zhuǎn)后二次避障檢測區(qū)，7.8為右前方避障檢測區(qū)，1為超聲波傳感器一逆時針旋轉(zhuǎn)22.5°，2為超聲波傳感器二逆時針旋轉(zhuǎn)7.5°，4為超聲波傳感器四逆時針旋轉(zhuǎn)3.7°。(可能盲區(qū)7.4、7.6只是在轉(zhuǎn)彎時刻存在，若干時刻后也不會存在。)

圖8為本發(fā)明超聲波避障策略流程圖。其中距離i(i＝1，2，3，4，5，6)為超聲波傳感器j(j為一、二、三、四、五、六)檢測到障礙物的距離。

圖9為本發(fā)明超聲波測距程序流程圖。

具體實施方式

本發(fā)明將結(jié)合附圖，通過以下實施例作進(jìn)一步說明。

實施例以目前市場上最常用型號為hc-sr04超聲波傳感器為例，加載在寬30cm的移動平臺實現(xiàn)其自主避障，實驗知該超聲波陣列具有較好的避障效果。

本發(fā)明實施例的超聲波陣列擺放如附圖5。

本發(fā)明所述實施例的移動平臺前方超聲波檢測原理如附圖6，首先由對稱原理及檢測需要確定超聲波傳感器三和超聲波傳感器四分別擺放在7.5cm和22.5cm位置，現(xiàn)以超聲波傳感器四為例進(jìn)行角度擺放計算分析：

令ea＝fd

即ac*tan∠e＝cd*tan∠f

22.5*tan∠(180°-15°-∠f)＝7.5*tan∠f

∠f＝86.207°≈86.2°

∠e＝78.8°

再由直角三角形eab，直角三角形eac，直角三角形fbd，直角三角形fcd計算分析出避障距離為115.1cm，考慮到安全性及誤差等影響設(shè)定避障距離為116cm。

經(jīng)計算分析可知，此種擺放方式只需加載兩個超聲波傳感器即可全面覆蓋移動平臺運(yùn)動方向上的檢測區(qū)域。首先只要超聲波傳感器三和超聲波傳感器四在此距離內(nèi)沒有檢測到障礙物則移動平臺繼續(xù)保持前行；其次本發(fā)明研究了一種二分法障礙物定位算法，通過比較超聲波傳感器三檢測和超聲波傳感器四檢測的距離，判斷出障礙物大致方位，選擇最佳避障方向，提高智能化和安全性；再次超聲波傳感器放置在移動平臺后端，既不影響避障區(qū)檢測又減小超聲波傳感器的檢測盲區(qū)；由附圖6可知，在移動平臺直行情況下，避障區(qū)6.1和6.2的存在消除了可能盲區(qū)6.3和6.6對移動平臺直行的影響；最后避障區(qū)6.4和避障區(qū)6.5減小移動平臺的檢測盲區(qū)(假若障礙物在避障區(qū)6.4，超聲波傳感器四因檢測不到則視為2m)，提高運(yùn)行安全性。

本發(fā)明所述實施例的移動平臺左右兩側(cè)及左前方和右前方超聲波檢測原理如附圖7，首先由對稱原理及檢測需要確定超聲波傳感器一、超聲波傳感器二和超聲波傳感器五、超聲波傳感器六分別擺放在0cm和30cm位置，現(xiàn)以超聲波傳感器一和超聲波傳感器二為例進(jìn)行角度擺放計算分析：

為檢測左前方障礙物的同時盡可能多的檢測出左方障礙物情況，本發(fā)明使超聲波傳感器二波束角的右邊沿與移動平臺左邊沿相吻合，即超聲波傳感器二逆時針旋轉(zhuǎn)7.5°，再使超聲波傳感器一波束角的右邊沿與超聲波傳感器二波束角的左邊沿相吻合，即超聲波傳感器一逆時針旋轉(zhuǎn)22.5°。再由直角三角形deb，直角三角形abc計算分析出避障距離分別為116cm和60cm。

經(jīng)計算可知，此種擺放方式只需加載一個超聲波傳感器即可全面覆蓋移動平臺左方檢測區(qū)域，再加載一個超聲波傳感器使檢測移動平臺左前方的同時增大移動平臺左方檢測區(qū)域，提高避障安全性。首先為了使移動平臺安全左轉(zhuǎn)30°，考慮到移動平臺旋轉(zhuǎn)過程最大距離為50cm和偏移誤差等因素，設(shè)定超聲波傳感器一和超聲波傳感器二近的避障距離為60cm；其次為了減小移動平臺旋轉(zhuǎn)后二次避障的可能，經(jīng)計算設(shè)定超聲波傳感器一遠(yuǎn)的避障距離為116cm，既合理覆蓋左轉(zhuǎn)30°后的檢測區(qū)域，又有效避免移動平臺出現(xiàn)左右搖擺情況，如先左轉(zhuǎn)30°后又右轉(zhuǎn)30°做無用功；最后超聲波傳感器放置在移動平臺后端，既不影響避障區(qū)檢測又減小超聲波傳感器的檢測盲區(qū)，如附圖7可知，在粗略避障情況下，可能盲區(qū)7.4的最寬距離小于4.8cm相較移動平臺30cm寬影響微乎其微；移動平臺可通過左轉(zhuǎn)二次避障區(qū)7.2減小檢測盲區(qū)對運(yùn)行的影響。

本發(fā)明所述實施例的寬30cm的移動平臺在超聲波傳感器波束角不同時主要參數(shù)下表。

由計算可知，當(dāng)移動平臺為寬30cm時，波束角為10°到25°的超聲波傳感器均有良好效果。

本發(fā)明考慮到超聲波傳感器具有一定的發(fā)射角和檢測盲區(qū)，首先通過超聲波陣列計算和避障算法推導(dǎo)，加載盡量少的超聲波傳感器合理覆蓋移動平臺運(yùn)動方向上的檢測區(qū)域，以更少的資源實現(xiàn)較大體積移動平臺的自主避障控制；其次超聲波位置擺放和避障算法可以減小甚至避免檢測盲區(qū)對成功避障的干擾，使避障策略更具智能化，提高移動平臺運(yùn)行安全性；最后經(jīng)計算可知，本發(fā)明也適用寬度小于50cm的移動平臺，具有較好的拓展性和實用價值。