技術(shù)領(lǐng)域

本發(fā)明屬于傳感探測領(lǐng)域，特別涉及一種基于激光測距數(shù)據(jù)來構(gòu)建極坐標(biāo)柱狀圖以解決勢場法容易陷入局部最小值并頻繁震顫的問題，克服增強(qiáng)式矢量場法在閾函數(shù)構(gòu)造中未考慮機(jī)器人實時速度的缺點的檢修機(jī)器人避碰規(guī)劃方法。

背景技術(shù)：

面對日益嚴(yán)峻的能源形勢，當(dāng)今世界各國政府都開始積極發(fā)展核電。眾所周知，核電區(qū)別與火電的最大特點是核反應(yīng)會產(chǎn)生電離輻射和放射性廢物。為避免發(fā)生事故導(dǎo)致放射性物質(zhì)進(jìn)入環(huán)境，定期檢修就顯得至關(guān)重要。然而最關(guān)系到核安全的反應(yīng)堆堆芯因電離輻射強(qiáng)度過高被隔離，人員無法進(jìn)入，于是核工業(yè)機(jī)器人應(yīng)運而生。核工業(yè)機(jī)器人最早于20世紀(jì)50年代開始使用，后逐漸發(fā)展成為遙控機(jī)器人。運用核工業(yè)機(jī)器人可以減少操作者在各種檢查和維護(hù)處理中所受的核輻射劑量，并可以提高監(jiān)控能力，避免替換發(fā)電設(shè)備，降低運營成本。

適用于核工業(yè)的機(jī)器人主要有以下幾個特征：具備較強(qiáng)的可移動性，能夠穿越和繞過障礙物；裝備各種攝像機(jī)和傳感器，有很好的偵察能力；可以快速到達(dá)操作地點，具備良好的機(jī)動性；具有雙向控制器，可以快速將狀態(tài)反饋給操作者；具有很好的力反饋，能夠保護(hù)操作對象。隨著機(jī)器人技術(shù)的發(fā)展，自主機(jī)器人將逐漸代替人工操作。

怎樣使檢修機(jī)器人通過攜帶傳感器自主完成無碰作業(yè)并安全抵達(dá)指定區(qū)域的問題是典型的避碰規(guī)劃問題。考慮到核電站事故情況下安全殼內(nèi)充滿高溫高壓蒸汽能見度極差，光學(xué)傳感器及超聲波傳感器無法發(fā)揮作用使用，因而選擇抗干擾能力較強(qiáng)的激光測距儀。本發(fā)明基于激光測距技術(shù)，對動態(tài)避障算法進(jìn)行研究，以求實現(xiàn)實時高效安全可靠的檢修機(jī)器人動態(tài)避碰。

目前，國內(nèi)外已有多種方法用于實現(xiàn)機(jī)器人動態(tài)避碰，其中人工勢場法和矢量場法在近些年受到了越來越廣泛的關(guān)注。可是除了算法自身的缺陷（如人工勢場法無法穿越緊密分布的障礙物群，矢量場法計算量大且計算過程復(fù)雜導(dǎo)致算法實時性差），他們也沒有考慮機(jī)器人的實時速度，或其環(huán)境建模方式只適用于裝有低精度傳感器（例如超聲傳感器）的機(jī)器人進(jìn)行避碰規(guī)劃。因此，這些規(guī)劃方法難以滿足檢修機(jī)器人的工程實際應(yīng)用需求。

為了解決以上常用避碰方法存在的問題，并進(jìn)一步滿足檢修機(jī)器人在現(xiàn)場作業(yè)中的避障需求，給出極坐標(biāo)柱狀圖法，該方法明顯減少了計算，更適合用于激光測距儀避障。同時，通過在閾函數(shù)構(gòu)造中考慮機(jī)器人速度和加速度，從而保證了檢修機(jī)器人能夠以較快的速度安全移動。

文獻(xiàn)《VPH+:AnEnhancedVectorPolarHistogramMethodforMobileRobotObstacleAvoidance》提到了相近的技術(shù)。但該文獻(xiàn)認(rèn)為障礙通常由一系列的障礙點構(gòu)成，彼此距離很近的障礙點視為一個障礙集。根據(jù)每個障礙集之間的幾何關(guān)系，將障礙集歸類劃分為凹集或凸集，凹集意味著該集合內(nèi)的障礙點離機(jī)器人較近，因此凹集所在位置對應(yīng)的方向不作為機(jī)器人行進(jìn)的可選方向；相反凸集內(nèi)的障礙點離機(jī)器人相對較遠(yuǎn)，其所在位置對應(yīng)的方向作為機(jī)器人行進(jìn)的可選方向。該方法在處理時雖然提高了篩選機(jī)器人可行進(jìn)方向的效率，但是事實上凹集內(nèi)仍然可能存在機(jī)器人行進(jìn)過程中不會發(fā)生碰撞的障礙點，而根據(jù)該文獻(xiàn)方法，這部分障礙點所在位置對應(yīng)的方向也被劃入機(jī)器人不可行進(jìn)的范圍，因此凹集的定義并非嚴(yán)密；而本發(fā)明在處理時，將機(jī)器人到每個方向上障礙點的距離分別與定義的安全距離（保證機(jī)器人在以確定速度行進(jìn)時不會與障礙發(fā)生碰撞）依次進(jìn)行比較，這樣就不會遺漏機(jī)器人可行進(jìn)方向的候選解。其二，該文獻(xiàn)在定義代價函數(shù)時，選取障礙方向與目標(biāo)方向的夾角作為機(jī)器人趨向目標(biāo)的限制引導(dǎo)項，從分析看選取該夾角所起的限制作用并不明顯；而本發(fā)明在定義代價函數(shù)時，直接選取機(jī)器人當(dāng)前行進(jìn)方向與目標(biāo)方向的夾角作為機(jī)器人趨向目標(biāo)的限制引導(dǎo)項，該夾角變量的意義更加明確，作用更為突出。其三，該文獻(xiàn)在選取最佳行進(jìn)方向時，在行進(jìn)角度范圍內(nèi)進(jìn)行代價函數(shù)值的計算和比較，因而計算量較大；而本發(fā)明通過二維柱狀圖的直觀分析，首先選取候選行進(jìn)方向，然后在候選行進(jìn)方向中進(jìn)行代價函數(shù)值的計算和比較，所以計算量明顯較少。

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

本發(fā)明的目的是提供一種計算量更小、反應(yīng)速度更快、實用性更高的基于激光測距數(shù)據(jù)來構(gòu)建極坐標(biāo)柱狀圖的檢修機(jī)器人避碰規(guī)劃方法。

本發(fā)明的目的是這樣實現(xiàn)的：

（1）建立極坐標(biāo)柱狀圖：

激光測距儀探測障礙物與檢修機(jī)器人的距離，并將測得的距離分布信息傳給微型計算機(jī)，用極坐標(biāo)下的柱狀圖表示，x軸表示激光束的序號，y軸表示該探測方向上的障礙物距離；

（2）通過閾函數(shù)簡化極坐標(biāo)柱狀圖：

建立機(jī)器人運動方程，

t=V(t)a,]]>

S(t)=V(t)t-12at2=V2(t)2a,]]>

其中：t為時間，V(t)為t時刻的實時速度，a為加速度，S(t)為t時間走過的距離。

對于探測方向i，

V(i,t)=(Vt→·di→)di=Vt·cos(θ(t)-i);|θ(t)-i|≤900;|θ(t)-i|≥90,]]>

S(i,t)=V2(i,t)2a=Vt2·cos2(θ(t)-i)2a;|θ(t)-i|≤900;|θ(t)-i|≥90,]]>

其中，V(i,t)為檢修機(jī)器人t時刻在方向i上的速度，Vt為檢修機(jī)器人的實時速度，di為檢修機(jī)器人與i方向上障礙物間的距離，θ(t)為檢修機(jī)器人隨時間改變的前進(jìn)方向，S(i,t)為檢修機(jī)器人t時間在方向i上走過的距離，a為檢修機(jī)器人的運動加速度。

建立閾函數(shù)：

W(i,t)=S(i,t)+R+D=Vt2·cos2(θ(t)-i)2a+R+D;|θ(t)-i|≤90R+D;|θ(t)-i|≥90,]]>

其中：Vt為檢修機(jī)器人的實時速度，θ(t)為檢修機(jī)器人隨時間改變的前進(jìn)方向，a為檢修機(jī)器人的運動加速度，R為檢修機(jī)器人自身尺寸的等效半徑，D為從檢修機(jī)器人到障礙物之間的安全距離；

將極坐標(biāo)柱狀圖轉(zhuǎn)化為二元柱狀圖：

H(i)=1d(i)≥W(i,t)0d(i)<W(i,t)]]>

其中：1表示機(jī)器人可以通行，0表示機(jī)器人不可以通行；

（3）根據(jù)評價函數(shù)選擇檢修機(jī)器人前進(jìn)方向：

建立可通行區(qū)集合，

U＝{i|H(i)＝1}，

定義評價函數(shù)C，

C(i)＝K1|i-Θ|+K2|i-θ(t)|i∈U，

其中，K1，K2為常數(shù)，Θ為檢修機(jī)器人的目標(biāo)方向，θ(t)為檢修機(jī)器人隨時間改變的前進(jìn)方向，

選取最佳行進(jìn)方向：

C(Ω)＝minC(i),i∈U

其中：Ω為最優(yōu)前進(jìn)方向。minC(i)為可通行區(qū)中各方向的評價函數(shù)的最小值。

將極坐標(biāo)柱狀圖轉(zhuǎn)化為二元柱狀圖，是把極坐標(biāo)下柱狀圖中的距離分布和閾值函數(shù)公式、數(shù)據(jù)的比較轉(zhuǎn)化為能直觀表示機(jī)器人是否可以通行的二元柱狀圖，二元柱狀圖的橫坐標(biāo)為0～180°的掃描角，縱坐標(biāo)的取值表示在0～180°范圍內(nèi)的每一個掃描角度上，機(jī)器人是否可以通行。

本發(fā)明的有益效果在于：

本發(fā)明不僅解決了傳統(tǒng)避碰方法固有的缺陷，而且無需建立復(fù)雜的物理模型便可直接處理傳感器原始數(shù)據(jù)，更有利于機(jī)器人實現(xiàn)在線的避碰規(guī)劃，具有計算量小、反應(yīng)速度快的優(yōu)點；同時該方法在閾函數(shù)構(gòu)造中充分考慮了機(jī)器人實時速度，更符合檢修機(jī)器人的工程實際應(yīng)用需求。使用更能直觀表示機(jī)器人是否可以通行的二元柱狀圖，縮小了機(jī)器人下一工作周期選擇行進(jìn)方向的范圍，減小了相應(yīng)的計算時間與能耗。

附圖說明

圖1為測距儀探測信息圖示；

圖2為機(jī)器人與障礙物相對位置示意圖；

圖3為柱狀圖簡化示意圖；

圖4為試驗臺架系統(tǒng)；

圖5為激光測距傳感器；

圖6為LMS200設(shè)備構(gòu)成圖；

圖7為避碰算法流程圖；

圖8為小尺寸多障礙物避碰規(guī)劃實驗示意圖；

圖9為落入陷阱示意圖；

圖10為脫離陷阱示意圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明做進(jìn)一步描述。

極坐標(biāo)柱狀圖算法（以下簡稱極柱圖法）的輸入值為傳感器實時探測的障礙物距離分布并通過三個數(shù)據(jù)處理步驟來計算機(jī)器人行進(jìn)方向：

將用于存放障礙物距離的數(shù)組中的數(shù)據(jù)轉(zhuǎn)變?yōu)闃O坐標(biāo)下的柱狀圖，

建立閾函數(shù)，并通過該閾函數(shù)將極坐標(biāo)柱狀圖轉(zhuǎn)化為二元柱狀圖，

通過評價函數(shù)計算，從二元柱狀圖上選取最佳行進(jìn)角，

①建立極坐標(biāo)柱狀圖

機(jī)器人在行進(jìn)過程中依靠激光測距儀探測前進(jìn)方向上的障礙。在每個探測周期中，測距儀發(fā)出一束激光并接收由障礙物反射的光束，根據(jù)其測距方位給出距離值，隨著激光發(fā)射角的逐漸改變，經(jīng)過若干個探測周期傳感器完成一個工作周期。在每個工作周期末傳感器將距離分布信息傳給微型計算機(jī)。接著將距離分布信息用極坐標(biāo)下的柱狀圖表示出來，如圖1所示。其中x軸表示激光束的序號（同時也表征其發(fā)射角，通常設(shè)定傳感器以1°為間隔單位，每個工作周期探測181次）；y軸表示該探測方向上的障礙物距離。

②基于閾函數(shù)的極坐標(biāo)柱狀圖簡化

充分考慮到檢修機(jī)器人的工程實際應(yīng)用需求，建立考慮機(jī)器人具有加速度的運動方程，在已知實時速度和加速度的情況下，就可以獲得機(jī)器人制動必須預(yù)留的時間和距離。方程如下：

t=V(t)a,]]>

S(t)=V(t)t-12at2=V2(t)2a,]]>

其中：t為時間，

V(t)為t時刻的實時速度，

a為加速度，

S(t)為t時間走過的距離。

如圖2所示，對于任一方向i（請注意圖2中i1，i2方向上迫近程度的差異），就可以將以上方程組改寫為：

V(i,t)=(Vt→·di→)di=Vt·cos(θ(t)-i);|θ(t)-i|≤900;|θ(t)-i|≥90,]]>

S(i,t)=V2(i,t)2a=Vt2·cos2(θ(t)-i)2a;|θ(t)-i|≤900;|θ(t)-i|≥90,]]>

其中：V(i,t)為檢修機(jī)器人t時刻在方向i上的速度，

Vt為檢修機(jī)器人的實時速度，

di為檢修機(jī)器人與i方向上障礙物間的距離，

θ(t)為檢修機(jī)器人隨時間改變的前進(jìn)方向，

S(i,t)為檢修機(jī)器人t時間在方向i上走過的距離，

a為檢修機(jī)器人的運動加速度。

同時考慮檢修機(jī)器人自身的尺寸（將其描述為半徑為R的圓），以及安全距離（機(jī)器人與障礙物之間無碰的最小距離D），由此得到最終隨時間變化的閾函數(shù)，如下式：

W(i,t)=S(i,t)+R+D=Vt2·cos2(θ(t)-i)2a+R+D;|θ(t)-i|≤90R+D;|θ(t)-i|≥90]]>

其中：Vt為檢修機(jī)器人的實時速度，

θ(t)為檢修機(jī)器人隨時間改變的前進(jìn)方向，

a為檢修機(jī)器人的運動加速度，

R為檢修機(jī)器人自身尺寸的等效半徑，

D為從檢修機(jī)器人到障礙物之間的安全距離。

在以上閾函數(shù)的表達(dá)式中，Vt和θ(t)可以實時獲取，a和R為恒定值（為了簡化問題，假設(shè)其為恒定值），D是唯一的待定系數(shù)，可以根據(jù)具體環(huán)境適當(dāng)改變D的值。

另外，由于本設(shè)計中使用的試驗平臺由步進(jìn)電機(jī)驅(qū)動，針對其行動特性需要將通用閾函數(shù)做以下修改。在前述情況中檢修機(jī)器人有提供加速度為-a的制動裝置，制動過程遵循牛頓運動定律，而在試驗平臺中，當(dāng)微型計算機(jī)停止發(fā)送驅(qū)動脈沖時步進(jìn)電機(jī)便立即停止運轉(zhuǎn)，在此之前一個周期內(nèi)，移動時機(jī)器人在θ(t)方向上走過固定長度S，用這個長度代入閾函數(shù)表達(dá)式中得：

W(i,t)=S(i)+R+D=S·cos+(θ(t)-i)R+D;|θ(t)-i|≤90R+D;|θ(t)-i|≥90]]>

通過比較極坐標(biāo)下柱狀圖中的距離分布和閾函數(shù)，可以得到一個二元柱狀圖，如圖3所示。

其中，根據(jù)以下規(guī)則創(chuàng)建二元柱狀圖：

H(i)=1d(i)≥W(i,t)0d(i)<W(i,t)]]>

其中：1表示機(jī)器人可以通行，0表示機(jī)器人不可以通行。

根據(jù)以上閾函數(shù)的定義，就可以將極坐標(biāo)下的柱狀圖轉(zhuǎn)化為二元柱狀圖，如圖3所示，根據(jù)這種規(guī)則創(chuàng)建的二元柱狀圖中，谷值表示不可通行區(qū)，峰值表示可通行區(qū)。

把極坐標(biāo)柱狀圖進(jìn)行簡化是把極坐標(biāo)下柱狀圖中的距離分布和閾值函數(shù)等公式、數(shù)據(jù)的比較轉(zhuǎn)化為能直觀表示機(jī)器人是否可以通行的二元柱狀圖。二元柱狀圖的橫坐標(biāo)代表0～180°的掃描角，縱坐標(biāo)的取值表示在0～180°范圍內(nèi)的每一個掃描角度上，機(jī)器人是否可以通行。當(dāng)縱坐標(biāo)取值為1時，代表機(jī)器人在該掃描角度上可以通行；否則當(dāng)縱坐標(biāo)取值為0時，則代表機(jī)器人在該掃描角度上不能通行。二元柱狀圖的生成，本質(zhì)是為機(jī)器人選擇最優(yōu)行進(jìn)方向提供可行進(jìn)方向的范圍，即縮小了機(jī)器人下一工作周期選擇行進(jìn)方向的范圍，減小了相應(yīng)的計算時間與能耗。

③基于評價函數(shù)選擇檢修機(jī)器人前進(jìn)方向

如圖2所示，峰值說明機(jī)器人在該方向可以自由通行，這時問題已經(jīng)被簡化為在已知的可通行區(qū)內(nèi)選取一個最佳方向。

把所有機(jī)器人可以通行的方向歸為可通行區(qū)集合U：

U＝{i|H(i)＝1}

接下來定義一個適當(dāng)?shù)脑u價函數(shù)C，利用這個函數(shù)帶入U中全部i值，即可計算出最優(yōu)前進(jìn)方向Ω，其表達(dá)式如下：

C(i)＝K1|i-Θ|+K2|i-θ(t)|i∈U

其中：K1，K2為常數(shù)，

Θ為檢修機(jī)器人的目標(biāo)方向，

θ(t)為檢修機(jī)器人隨時間改變的前進(jìn)方向。

該表達(dá)式的第一部分K1|i-Θ|代表評價了各個計算方向與目標(biāo)方向的貼近程度，用于引導(dǎo)機(jī)器人趨向目標(biāo)；第二部分K2|i-θ(t)|則評價了各個計算方向與當(dāng)前機(jī)器人前進(jìn)方向的貼近程度，避免機(jī)器人轉(zhuǎn)角過大。

在極柱圖法中，適當(dāng)選取K1，K2這兩個待定系數(shù)顯得尤為重要。為了優(yōu)先考慮引導(dǎo)機(jī)器人趨向目標(biāo)，K1必須大于K2。具體選值要適合特定的工作環(huán)境。

根據(jù)以上評價函數(shù)的定義，最佳行進(jìn)方向必須盡量同時接近目標(biāo)方向和前一工作周期的前進(jìn)方向，也就是將這一方向i帶入評價函數(shù)表達(dá)式取得盡可能小的值。因此最終選取的行進(jìn)方向為Ω的表達(dá)式為：

C(Ω)＝minC(i),i∈U

其中：Ω為最優(yōu)前進(jìn)方向，

minC(i)為可通行區(qū)中各方向的評價函數(shù)的最小值。

④速度控制

如果檢修機(jī)器人的速度可調(diào)，且其最大速度為Vmax。機(jī)器人的行進(jìn)模式應(yīng)該滿足：自由行進(jìn)時保持最大速度，而在探測到障礙物并實施避碰時使用一個較小的速度V。該速度控制表達(dá)式為：

V=(1-Dmax-d(Ω)Dmax-Dmin)Vmax;]]>Dmin＝R+D

其中：

Ω由公式C(Ω)＝minC(i),i∈U計算得到，

Dmax為激光測距儀的最大探測距離，

Dmin為機(jī)器人的最小安全距離。

（一）試驗系統(tǒng)硬件組成

1）試驗臺架系統(tǒng)

本次使用的試驗臺架由臺架主體框架、主控計算機(jī)、HIT6502四軸步進(jìn)電機(jī)控制卡和步進(jìn)電機(jī)組成，如圖4所示。

控制主體為工業(yè)控制計算機(jī)，由其實現(xiàn)任務(wù)設(shè)置、運動算法實現(xiàn)和命令接收、發(fā)送等功能。步進(jìn)電機(jī)運動控制由Hit6502型四軸步進(jìn)電機(jī)運動控制卡實現(xiàn)，運動控制卡通過ISA接口與主控計算機(jī)連接。本試驗臺架包含4塊運動控制卡，共可實現(xiàn)對11個自由度的獨立控制（平臺六個自由度和空間障礙五個自由度）。本設(shè)計中移動平臺攜帶激光測距儀需實現(xiàn)XOY平面（水平面）中兩個平動自由度以及圍繞Z軸（鉛垂向）旋轉(zhuǎn)的轉(zhuǎn)動自由度。系統(tǒng)的各個自由度的極限位置處都設(shè)置有限位傳感器（共11個），傳感器也同運動控制卡連接，當(dāng)平臺運動到某自由度的極限位姿處，傳感器向運動控制卡發(fā)出報警信號，運動控制卡立即向電機(jī)驅(qū)動器發(fā)出制動信號。

2）激光測距傳感器

本設(shè)計采用的是德國SICK公司生產(chǎn)的LMS200-30106型激光測距傳感器系統(tǒng)，如圖5所示。該型非接觸式激光測量系統(tǒng)，可以像激光雷達(dá)那樣，以二維方式探測其周圍環(huán)境。由于其屬于主動掃描系統(tǒng)，不需要外部被動器件如反射鏡或位置標(biāo)記。SICK擁有較高的分辨率，廣泛應(yīng)用于二維距離檢測，尤為重要的是它可以識別反射率僅為10%的障礙如玻璃等光滑表面，或者在煙霧環(huán)境中工作，因而被選擇作為核電站檢修機(jī)器人的主要傳感器。

根據(jù)其通訊編程手冊，LMS200-30106型激光測距儀使用標(biāo)準(zhǔn)RS232串口與微機(jī)通訊，并嚴(yán)格遵照標(biāo)準(zhǔn)串口通訊規(guī)則。

SICKLMS200-30106型激光測距傳感器系統(tǒng)的的技術(shù)指標(biāo)：

掃描范圍：80m（最大值）10m（10%反射率）

掃描角度：最大180°（可調(diào)）

角度分辨率：0.25°/0.5°/1°（可調(diào)）

響應(yīng)時間：53ms/26ms/13ms

分辨率/系統(tǒng)誤差：10mm/typ±15mm

數(shù)據(jù)接口：RS232/RS422

開關(guān)量輸出：typ.24VDC

激光防護(hù)等級：1（人眼安全）

運行環(huán)境溫度：0—±50℃

封裝等級：IP65

尺寸（W×H×D）:155×210×156mm3

LMS200由設(shè)備外殼、激光發(fā)射器和工作指示燈組成。機(jī)身兩路接線端分別連接微型計算機(jī)的RS-232串行接口和24V穩(wěn)壓電源。設(shè)備連接如圖6：

LMS200機(jī)身內(nèi)部有驅(qū)動激光發(fā)射器構(gòu)件旋轉(zhuǎn)的伺服電機(jī)，因而可在機(jī)身本體固定的情況下實現(xiàn)0到180度范圍內(nèi)的障礙物探測。在每一個探測周期末，LMS200將掃描的障礙物距離信息通過RS-232串行端口發(fā)送到主控微機(jī)，經(jīng)過程序拆包后可將環(huán)境信息在可視化界面以障礙物輪廓形式表示出來。

（二）編程環(huán)境簡介

本設(shè)計采用的編程環(huán)境是由Microsoft公司推出的VisualC++6.0，它從誕生以來一直是Windows環(huán)境下最主要的應(yīng)用開發(fā)系統(tǒng)之一。VisualC++不僅是C++語言的集成開發(fā)環(huán)境，而且與Win32緊密相連，利用它可以完成從底層軟件直到上層直接面向用戶的軟件等各種應(yīng)用程序的開發(fā)。

MFC(MicrosoftFoundationClasses)是用來編寫Windows應(yīng)用程序的C++類集，其中封裝了大部分常用的的WindowsAPI函數(shù)和Windows控件。使用MFC類庫和VisualC++提供的高度可視應(yīng)用程序開發(fā)工具，可以使程序開發(fā)變得更簡單，極大地縮短了開發(fā)周期。本設(shè)計的編程部分主要依靠MFC庫類實現(xiàn)。

（三）基于極坐標(biāo)柱狀圖法的避碰算法實現(xiàn)步驟

該算法的步驟流程圖如圖7所示：

(1)每個工作周期開始時要讀取位姿信息，包括檢修機(jī)器人的當(dāng)前位置、目標(biāo)點的位置，以及機(jī)器人幾何尺寸的描述。

(2)接著判斷機(jī)器人是否到達(dá)目標(biāo)位置，如果是，則結(jié)束避碰導(dǎo)航，否則判斷是否需要避碰。

(3)判斷是否需要避碰的條件是當(dāng)前行進(jìn)方向是否有影響機(jī)器人行進(jìn)的障礙。分析fDistance[181]數(shù)組（用于存放障礙物距離），若機(jī)器人通過激光測距儀探測得知在前方安全距離內(nèi)出現(xiàn)障礙，則進(jìn)入避碰分支，否則在計算當(dāng)前位置與目標(biāo)位置的連線方向角后，命令機(jī)器人沿該方向徑直前進(jìn)。隨后儲存位姿信息，結(jié)束該工作周期。

(4)當(dāng)程序進(jìn)入避碰分支后，根據(jù)極坐標(biāo)柱狀圖算法的基本思路，由機(jī)器人幾何尺寸建立閾函數(shù)。

(5)將閾函數(shù)與障礙物距離數(shù)組fDistance[181]比較，確定可通行區(qū)。

(6)建立評價函數(shù)處理可通行區(qū)信息，選擇最佳前進(jìn)方向角。

(7)最后命令機(jī)器人沿該方向徑直前進(jìn)，儲存位姿信息并結(jié)束該工作周期。

（四）檢修機(jī)器人動態(tài)避碰模擬仿真試驗

①小尺寸多障礙物避碰規(guī)劃實驗

本設(shè)計假定的檢修機(jī)器人工作環(huán)境為核電站工業(yè)現(xiàn)場，其中可能包含有大量密集分布的，僅僅數(shù)倍于機(jī)器人尺寸的障礙物群。本實驗的目的是驗證在小尺寸多障礙物的情況下，極坐標(biāo)向量圖法的實際避碰效果。

本實驗中，障礙物為如圖8所示的密集分布的半徑為30像素的圓形障礙，而機(jī)器人平臺設(shè)置為默認(rèn)模式（半徑為5像素的圓）。

機(jī)器人順利通過該類型障礙物群，且路徑較為平滑。

分析如圖8所示的避碰規(guī)劃結(jié)果：在圖中點1位置機(jī)器人首次探測到在其安全距離內(nèi)有障礙物，且障該物相對目標(biāo)連線方向偏右，因此通過評價函數(shù)在可通行區(qū)中選擇出最佳路徑為左偏某一角度，該動作符合極坐標(biāo)柱狀圖法的基本思想。在點2位置附近前向角波動較頻繁，究其原因是由于此處障礙物分布較密集，多個障礙物對機(jī)器人的影響相互疊加造成。以下用圓心坐標(biāo)表示圓。在圓（253,124）和圓（185,210）之間時，由于圓（320,203）的影響，機(jī)器人選擇向右偏轉(zhuǎn)；然而立刻受到圓（205,297）、圓（320,203）和圓（298,317）的綜合影響，機(jī)器人在數(shù)個周期后決定重新轉(zhuǎn)向左側(cè)；又行進(jìn)了數(shù)個周期，在圓（320,203）的影響下再次右轉(zhuǎn)；這里要注意的是，由于圓（298,317）和圓（358,334）極為貼近，在避碰運算過程中它們被定義為連續(xù)的不可通行區(qū)，因而在這個“連續(xù)障礙”的作用下，點2到點3之間的路徑較光滑；最后到達(dá)點3附近后，由于脫離全部障礙，機(jī)器人徑直向目標(biāo)點移動。

以上過程證明了極坐標(biāo)柱狀圖法對于小尺寸的障礙物群有著較好的避碰效果。但是在障礙物較密集，多個障礙共同作用的情況下，路徑會出現(xiàn)明顯波動。

②陷阱環(huán)境避碰規(guī)劃實驗

檢修機(jī)器人的工作環(huán)境中可能存在各種陷阱區(qū)域。這里說的陷阱，是指由于算法缺陷導(dǎo)致機(jī)器人進(jìn)入該區(qū)域后無法脫離，如圖9所示。

這是因為某些算法中機(jī)器人不具備“記憶”能力，在發(fā)現(xiàn)最優(yōu)方向存在障礙，退后并走回一段距離后，仍然轉(zhuǎn)回錯誤路徑。因此在本設(shè)計算法中為機(jī)器人加入了“記憶”功能，即保存全部運動周期的機(jī)器人位姿數(shù)據(jù)及環(huán)境信息。

本實驗設(shè)置如圖10中所示的連續(xù)U形陷阱障礙，目的是驗證加入“記憶”功能避碰算法是否對陷阱障礙具有規(guī)避能力。通過仿真程序生成如圖所示機(jī)器人路徑，機(jī)器人順利從陷阱中脫離，證明了算法的可行性。