楊凌耀，張愛華，張 潔，宋季強(qiáng)

上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院，上海201620

移動(dòng)機(jī)器人依托高效智能的數(shù)字化管理和規(guī)劃可以方便地完成復(fù)雜環(huán)境的搬運(yùn)、爆發(fā)式增長的物流快件分揀等任務(wù)。在智能制造場景中，移動(dòng)機(jī)器人的運(yùn)動(dòng)路徑上通常會(huì)有各種形狀的障礙物存在，移動(dòng)機(jī)器人想要到達(dá)目標(biāo)點(diǎn)必須躲避環(huán)境中的障礙物，這就要求機(jī)器人在向目標(biāo)點(diǎn)運(yùn)動(dòng)的同時(shí)即時(shí)地產(chǎn)生避障行為。隨著智能制造業(yè)的發(fā)展，現(xiàn)代工業(yè)對(duì)移動(dòng)機(jī)器人的智能化水平提出了更高的要求，尤其是避障路徑規(guī)劃問題，已成為近年來國內(nèi)外研究的熱點(diǎn)[1]。

路徑規(guī)劃通常可以劃分為環(huán)境建模和路徑搜索兩部分。合理的環(huán)境建模方法可以極大提高路徑搜索效率和避障精準(zhǔn)度[2]。常用的環(huán)境建模方法有柵格法、可視圖法、維諾圖法等。其中柵格法因其強(qiáng)大的處理不規(guī)則障礙物的能力成為了當(dāng)前較為常用的建模方法，主要思想是將環(huán)境進(jìn)行離散分解，用一定大小柵格來表示基本單元[3]。文獻(xiàn)[4]在設(shè)計(jì)路徑規(guī)劃算法時(shí)就先利用柵格對(duì)機(jī)器人工作環(huán)境進(jìn)行分區(qū)預(yù)處理，從而提前量化環(huán)境的復(fù)雜度，提高了算法搜索效率、精度和靈活性。

路徑搜索已有多種算法，包括人工勢(shì)場法[5]、蟻群算法[6]、神經(jīng)網(wǎng)絡(luò)避障[7]、遺傳算法[8]等，其中人工勢(shì)場法是較為基礎(chǔ)的算法。人工勢(shì)場法最初是由Khatib在1986年提出的[9]，人工勢(shì)場法的基本思想是根據(jù)“趨利避害”的原則，移動(dòng)機(jī)器人在一個(gè)虛擬的力場中運(yùn)動(dòng)，目標(biāo)點(diǎn)被引力和斥力勢(shì)場包圍，產(chǎn)生的合力使機(jī)器人朝向目標(biāo)點(diǎn)運(yùn)動(dòng)，其優(yōu)點(diǎn)是系統(tǒng)的路徑生成與控制直接與環(huán)境實(shí)現(xiàn)了閉環(huán)，從而大大加強(qiáng)了系統(tǒng)的適應(yīng)性與避障性能。文獻(xiàn)[10]針對(duì)傳統(tǒng)人工勢(shì)場在復(fù)雜的障礙物下難以規(guī)劃出完整路徑問題，提出了一種改進(jìn)的人工勢(shì)場，改進(jìn)了斥力生成和計(jì)算以及添加了識(shí)別周圍環(huán)境的機(jī)制，有效地解決了傳統(tǒng)算法容易出現(xiàn)路徑規(guī)劃中斷的情況。文獻(xiàn)[11]將勢(shì)場法與蟻群算法相結(jié)合，解決了原有算法收斂速度過快、易出現(xiàn)局部最優(yōu)解、路徑不平滑等問題。但是上述的人工勢(shì)場法是針對(duì)靜態(tài)環(huán)境提出來的，在實(shí)際情況下機(jī)器人往往需要實(shí)時(shí)避開動(dòng)態(tài)障礙物或?qū)崟r(shí)跟蹤動(dòng)態(tài)目標(biāo)點(diǎn)。文獻(xiàn)[12]在傳統(tǒng)的人工勢(shì)場法加入障礙物的速度和加速度矢量修正勢(shì)場角度方向，相當(dāng)于把原來的靜態(tài)勢(shì)場變?yōu)閯?dòng)態(tài)勢(shì)場，滿足了某些特定條件下的動(dòng)態(tài)避障需求。文獻(xiàn)[13]在文獻(xiàn)[12]的基礎(chǔ)上將機(jī)器人自身的速度和加速度也考慮進(jìn)來，根據(jù)當(dāng)前的機(jī)器人、障礙物、目標(biāo)物的位置對(duì)下一步的動(dòng)作做出預(yù)判，避免由于不必要的增加修正力而使得機(jī)器人的規(guī)避路徑無謂的增加。文獻(xiàn)[14]提出將速度障礙思想和人工勢(shì)場理論相結(jié)合的船舶避碰決策方法，保證船舶在障礙物、來船和可航水域邊界組成的復(fù)雜動(dòng)態(tài)環(huán)境下的避障性能。應(yīng)用人工勢(shì)場法規(guī)劃出來的路徑是比較平滑和安全的，但是存在障礙物附近不可達(dá)和局部極小點(diǎn)等問題[15]，并且在勢(shì)場對(duì)機(jī)器人的合力計(jì)算較為繁瑣，計(jì)算效率較低。文獻(xiàn)[16]提出了一種新的排斥勢(shì)函數(shù)，當(dāng)檢測(cè)到局部極小值時(shí)，通過激活虛擬逃逸力來脫離局部極小點(diǎn)。文獻(xiàn)[17]在人工勢(shì)場算法的基礎(chǔ)上，設(shè)計(jì)新的勢(shì)能函數(shù)以減少運(yùn)動(dòng)過程中計(jì)算勢(shì)場對(duì)智能體作用力合力的繁瑣步驟，提高了算法效率。

受到前人的研究工作啟發(fā)，提出一種基于柵格地圖的移動(dòng)機(jī)器人速度勢(shì)實(shí)時(shí)避障路徑規(guī)劃方法，將工作環(huán)境進(jìn)行二值化的柵格劃分，再利用柵格搜索的方法計(jì)算移動(dòng)機(jī)器人和障礙物的實(shí)時(shí)距離，通過設(shè)計(jì)速度勢(shì)函數(shù)產(chǎn)生與實(shí)時(shí)距離相關(guān)的速度增量用來驅(qū)動(dòng)移動(dòng)機(jī)器人運(yùn)動(dòng)，既省去了傳統(tǒng)勢(shì)場法繁瑣的合力計(jì)算過程，又達(dá)到實(shí)時(shí)路徑規(guī)劃的目的。同時(shí)在零勢(shì)點(diǎn)處設(shè)置最小速度增量，避免局部極小，并在目標(biāo)點(diǎn)附近設(shè)立距離閾值，避免機(jī)器人運(yùn)動(dòng)到目標(biāo)點(diǎn)附近可能出現(xiàn)的速度增量趨于無窮的情況。最后，仿真模擬移動(dòng)機(jī)器人在復(fù)雜多變的障礙物環(huán)境下的實(shí)時(shí)避障規(guī)劃，驗(yàn)證算法的有效性。

1 問題描述

在移動(dòng)機(jī)器人的現(xiàn)實(shí)場景中存在不同形狀的障礙物，假設(shè)通過校準(zhǔn)攝像機(jī)攝取工作環(huán)境的圖像，使用多個(gè)攝影機(jī)捕捉整個(gè)工作區(qū)的不同部分，并通過融合以創(chuàng)建整體地圖，通過對(duì)障礙物形狀進(jìn)行簡化，得到工作場景地圖的示意圖如圖1所示。圖1中不同形狀黑色物塊表示障礙物，它們隨機(jī)地分布在機(jī)器人工作的環(huán)境中，移動(dòng)機(jī)器人需要在此環(huán)境中完成兩點(diǎn)之間的路徑規(guī)劃，假設(shè)機(jī)器人的中心點(diǎn)為f，機(jī)器人運(yùn)動(dòng)的目標(biāo)點(diǎn)為g，機(jī)器人的速度為v；在700 dm×700 dm的工作地圖中運(yùn)動(dòng)，移動(dòng)機(jī)器人需要根據(jù)自身的當(dāng)前位置即時(shí)地躲避隨機(jī)分布在工作場景中的障礙物，最終到達(dá)目標(biāo)點(diǎn)完成工作任務(wù)。因?yàn)闄C(jī)器人駛出工作區(qū)域會(huì)產(chǎn)生無法預(yù)測(cè)的結(jié)果，故工作區(qū)域的邊界也視為障礙物處理。

圖1 移動(dòng)機(jī)器人工作場景示意圖Fig.1 Sketch map of mobile robot working scene

選取XOY為大地坐標(biāo)系，設(shè)定移動(dòng)機(jī)器人t時(shí)刻位姿為f(t)=[x(t)y(t)θ(t)]T，其中[x(t)y(t)]T∈R2表示中心點(diǎn)f在XOY中的實(shí)時(shí)位置，θ(t)表示機(jī)器人前進(jìn)方向與x軸之間的實(shí)時(shí)夾角，t時(shí)刻到t+1時(shí)刻的時(shí)間微分為dt。

假設(shè)移動(dòng)機(jī)器人驅(qū)動(dòng)輪與地面不滑移，其運(yùn)動(dòng)學(xué)方程可表示如下：

2 柵格地圖與柵格搜索

為充分表達(dá)整個(gè)環(huán)境的分布情況，以柵格法構(gòu)建地圖，以機(jī)器人和障礙物的實(shí)體長度尺寸作柵格的基本單元，根據(jù)障礙物的實(shí)際位置來確定柵格的狀態(tài)，有障礙物的柵格被賦值0，無障礙物的柵格被賦值1，如圖2所示。柵格越密能夠表示的工作環(huán)境越詳細(xì)，對(duì)障礙物外形特點(diǎn)描繪的就越逼真，路徑規(guī)劃精準(zhǔn)度就越高。從機(jī)器人f出發(fā)向四周不同的方向進(jìn)行柵格搜索，如搜索到1，表示是自由區(qū)域，繼續(xù)沿著該方向搜索；如搜索到0，表示到達(dá)障礙物邊緣，搜索停止，計(jì)算柵格數(shù)，得到該搜索方向上機(jī)器人距離障礙物的距離。

圖2 柵格地圖構(gòu)建與二值化Fig.2 Construction and binarization of raster map

3 速度勢(shì)避障算法

考慮到距離障礙物越近的區(qū)域機(jī)器人越難到達(dá)，距離目標(biāo)點(diǎn)越近的區(qū)域機(jī)器人越容易到達(dá)，障礙物對(duì)機(jī)器人有排斥作用，可產(chǎn)生負(fù)的速度增量aw(t)，目標(biāo)點(diǎn)對(duì)障礙物有吸引作用，可產(chǎn)生正的速度增量av(t)，如圖3所示。進(jìn)而合成移動(dòng)機(jī)器人的實(shí)時(shí)速度增量函數(shù)a?(t)：

圖3 中箭頭指向表示勢(shì)向量的方向，由公式（2）可以看到，此處通過速度勢(shì)分布定義速度增量函數(shù)，這樣既可達(dá)到速度驅(qū)動(dòng)的目的，又能實(shí)現(xiàn)避障行為。

圖3 速度勢(shì)場分布示意圖Fig.3 Schematic diagram of velocity potential field distribution

進(jìn)一步，為體現(xiàn)目標(biāo)對(duì)移動(dòng)機(jī)器人的吸引作用，機(jī)器人越靠近目標(biāo)點(diǎn)，目標(biāo)點(diǎn)對(duì)機(jī)器人的吸引越大，越遠(yuǎn)離目標(biāo)點(diǎn)，目標(biāo)點(diǎn)對(duì)機(jī)器人的吸引越小，定義目標(biāo)點(diǎn)對(duì)機(jī)器人的速度增量矩陣a?v(t)：

為防止距離目標(biāo)點(diǎn)過遠(yuǎn)導(dǎo)致吸引勢(shì)過小，引入最小吸引勢(shì)速度增量矩陣，方向與相同；為避免機(jī)器人接近目標(biāo)點(diǎn)時(shí)，速度增量趨于無窮，引入目標(biāo)點(diǎn)附近的距離閾值dgm(t)，即當(dāng)dg(t)＜dgm(t)時(shí)，dg(t)=dgm(t)。dgm(t)的取值與上一時(shí)刻吸引勢(shì)速度增量的單位時(shí)間增幅有關(guān)，當(dāng)單位時(shí)間增幅急劇增大就表示機(jī)器人進(jìn)入目標(biāo)點(diǎn)附近的小鄰域內(nèi)。

式中，ε為距離閾值增益，‖‖2為向量的二范數(shù)，ρ為距離閾值半徑，從而得到新目標(biāo)點(diǎn)對(duì)機(jī)器人吸引的速度增量矩陣為：

接著，根據(jù)障礙物對(duì)機(jī)器人有排斥作用的思想，機(jī)器人越靠近障礙物，障礙物對(duì)機(jī)器人的排斥越大，越遠(yuǎn)離障礙物，障礙物對(duì)機(jī)器人的排斥越小，定義障礙物對(duì)機(jī)器人排斥的速度增量矩陣：

式中，j為柵格搜索的方向的個(gè)數(shù)，β為排斥勢(shì)的程度參數(shù)，ηj為各排斥勢(shì)的影響比例因子，與障礙物的形狀和大小有關(guān)，awx(t)和awy(t)分別為aw(t)在x方向和y方向上的速度增量，θbj(t)為第j個(gè)方向的測(cè)量角度，dbj(t)為第j個(gè)方向的測(cè)量距離。dm為障礙物外圍的最小安全距離，σ(t)∝v(t)表示機(jī)器人對(duì)障礙物的敏感度，他和機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)速度大小成正比，當(dāng)最小安全距離dm一定，機(jī)器人的運(yùn)動(dòng)速度增大，機(jī)器人對(duì)障礙物的敏感度會(huì)變高，障礙物外圍的實(shí)時(shí)安全距離σ(t)dm會(huì)隨之增大，最大程度避免機(jī)器人和障礙物發(fā)生碰撞。

此外，為防止速度增量累加產(chǎn)生零勢(shì)點(diǎn)，使機(jī)器人陷入局部極小點(diǎn)，引入最小合成速度增量a?(t)min，增量方向與零勢(shì)點(diǎn)處相同，當(dāng)機(jī)器人陷入局部極小點(diǎn)時(shí)，最小合成速度增量會(huì)幫助機(jī)器人離開零勢(shì)點(diǎn)區(qū)域，得到新的速度增量矩陣：

δ為最小實(shí)時(shí)速度增量的二范數(shù)，用于界定零勢(shì)點(diǎn)范圍。

4 基于柵格法的實(shí)時(shí)避障路徑規(guī)劃

4.1 實(shí)時(shí)軌跡生成

通過對(duì)式（1）中的運(yùn)動(dòng)學(xué)模型得出的速度矩陣施加實(shí)時(shí)速度增量a(t)，即可得到t+1時(shí)刻機(jī)器人的速度矩陣為：

通過速度矩陣，可以得到移動(dòng)機(jī)器人穿越障礙物的實(shí)時(shí)軌跡曲線：

機(jī)器人下一時(shí)刻的方位角為：

4.2 實(shí)時(shí)路徑規(guī)劃流程

柵格搜索方法和速度勢(shì)增量函數(shù)建立之后，根據(jù)機(jī)器人的實(shí)時(shí)位置的調(diào)整在工作環(huán)境中的位姿，從而實(shí)時(shí)規(guī)劃一條可靠的通往目標(biāo)點(diǎn)的路徑，實(shí)時(shí)路徑規(guī)劃流程如圖4所示，路徑規(guī)劃步驟如下：

步驟1初始化，設(shè)定計(jì)算機(jī)和機(jī)器人初始參數(shù)。

步驟2對(duì)工作環(huán)境攝取并柵格化，再對(duì)柵格做二值化處理。

步驟3判斷機(jī)器人是否到達(dá)目標(biāo)，如到達(dá)路徑規(guī)劃結(jié)束，如未到達(dá)，機(jī)器人開始路徑規(guī)劃。

步驟4機(jī)器人向四周不同方向進(jìn)行柵格搜索，測(cè)得在不同方向上機(jī)器人與障礙物的實(shí)時(shí)距離dbj(t)，并根據(jù)自身坐標(biāo)位置計(jì)算與目標(biāo)點(diǎn)的距離dg(t)。

步驟5判斷dg(t)是否達(dá)到距離閾值dm(t)，如達(dá)到，dg(t)=dgm(t)。

步驟6根據(jù)dg(t)、dbj(t)計(jì)算得出吸引勢(shì)和排斥勢(shì)速度增量

步驟8 av(t)和aw(t)累加合成機(jī)器人在t時(shí)刻速度增量

步驟10 a(t)對(duì)機(jī)器人作速度驅(qū)動(dòng)，得到機(jī)器人下一時(shí)刻的目標(biāo)點(diǎn)。

5 仿真與分析

為驗(yàn)證避障方法的有效性，本章針對(duì)不同障礙物環(huán)境以及不同起始點(diǎn)和目標(biāo)點(diǎn)的避障規(guī)劃仿真。通過柵格搜索測(cè)量機(jī)器人周圍所有角度的障礙物與機(jī)器人的距離來計(jì)算合成的速度增量a(t)。在仿真中，挑選了5個(gè)特定角度的距離來計(jì)算排斥勢(shì)，它們是向前、左側(cè)、右側(cè)、向前左對(duì)角線和向前右對(duì)角線。

仿真環(huán)境根據(jù)坐標(biāo)環(huán)境而定，在700 dm×700 dm的工作區(qū)域內(nèi)，坐標(biāo)系中的每個(gè)整數(shù)坐標(biāo)對(duì)應(yīng)柵格地圖中的一個(gè)網(wǎng)格單元，每個(gè)單元根據(jù)障礙物的位置被賦予二值化信息，有障礙物的區(qū)域設(shè)置為0，無障礙物的區(qū)域設(shè)置為1。設(shè)定吸引勢(shì)的影響比例因子λ為400 000和各排斥勢(shì)的影響比例因子ηj為300 000，排斥勢(shì)的影響程度為2和吸引勢(shì)的影響程度為3。設(shè)定最小吸引勢(shì)速度增量，最小速度增量機(jī)器人初始轉(zhuǎn)角為，機(jī)器人初始速度為0。

設(shè)定不同的起點(diǎn)和終點(diǎn)，檢驗(yàn)避障算法對(duì)于地圖上任意可通過點(diǎn)的有效性，并得到路徑在坐標(biāo)系中的曲線方程，仿真如下：

（1）起點(diǎn)[20，20]，終點(diǎn)[650，20]的情況

（2）起點(diǎn)[20，20]，終點(diǎn)[20，650]的情況

（3）起點(diǎn)[20，20]，終點(diǎn)[650，650]的情況

圖5 起點(diǎn)[20，20]，終點(diǎn)[650，20]時(shí)避障路徑及曲線特征圖。圖6為起點(diǎn)[20，20]，終點(diǎn)[20，650]時(shí)避障路徑及曲線特征圖。圖7為起點(diǎn)[20，20]，終點(diǎn)[650，650]時(shí)避障路徑及曲線特征圖。在圖5（a）、圖6（a）和圖7（a）仿真圖中，路徑中的小方塊表示機(jī)器人的實(shí)際形狀，黑色的物塊表示障礙物的形狀和位置。機(jī)器人從自身的中心點(diǎn)出發(fā)向指定的各個(gè)角度進(jìn)行柵格搜索，求得各個(gè)角度上機(jī)器人和障礙物之間的距離，再通過增量函數(shù)求得實(shí)時(shí)的速度增量，得出機(jī)器人在下一時(shí)刻期望位置。不同時(shí)刻的位置被記錄在地圖中，形成了機(jī)器人的避障路徑。從仿真結(jié)果可以看出，盡管障礙物的形狀較為復(fù)雜多樣，但在對(duì)地圖中任意位置的目標(biāo)點(diǎn)的避障運(yùn)動(dòng)過程中，機(jī)器人總是實(shí)時(shí)規(guī)劃出了安全路徑。通過設(shè)置不同的終點(diǎn)以引導(dǎo)機(jī)器人穿越不同形狀的障礙物，來驗(yàn)證處理不同類型實(shí)時(shí)路徑規(guī)劃問題的能力。

圖5 起點(diǎn)[20，20]，終點(diǎn)[650，20]時(shí)避障路徑及曲線特征Fig.5 Obstacle avoidance path and curve characteristics at starting point[20，20]and ending point[650，20]

圖6 起點(diǎn)[20，20]，終點(diǎn)[20，650]時(shí)避障路徑及曲線特征Fig.6 Obstacle avoidance path and curve characteristics at starting point[20，20]and ending point[20，650]

圖7 起點(diǎn)[20，20]，終點(diǎn)[650，650]時(shí)避障路徑及曲線特征Fig.7 Obstacle avoidance path and curve characteristics at starting point[20，20]and ending point[650，650]

圖5 （b）、圖6（b）和圖7（b）為不同終點(diǎn)情況下機(jī)器人在避障過程中機(jī)器人中心在笛卡爾坐標(biāo)系中的曲線特征，可以看出實(shí)時(shí)路徑規(guī)劃出的曲線均較為平滑，這非常有利于后續(xù)軌跡跟蹤控制器的設(shè)計(jì)。圖5（c）、圖6（c）和圖7（c）為三種情況下機(jī)器人的期望速度增量曲線變化圖，可以看出，避障開始階段機(jī)器人因?yàn)殡x目標(biāo)點(diǎn)過遠(yuǎn)，排斥勢(shì)的影響遠(yuǎn)大于吸引勢(shì)，故合成速度增量數(shù)值非常大。隨著機(jī)器人向目標(biāo)點(diǎn)前進(jìn)，吸引勢(shì)的影響逐漸增大，合成速度增量數(shù)值在小范圍內(nèi)波動(dòng)。但是合成速度增量始終未趨于零，說明機(jī)器人在運(yùn)動(dòng)過程中未陷入局部極小陷阱。結(jié)果表明所提出的充分考慮了避障過程中機(jī)器人和障礙物外形特點(diǎn)，具有更好的實(shí)時(shí)性和容錯(cuò)性。

6 結(jié)論

針對(duì)未知且復(fù)雜多變的障礙物環(huán)境，提出了一種基于柵格地圖的速度勢(shì)實(shí)時(shí)避障路徑規(guī)劃方法，該方法利用柵格劃分對(duì)移動(dòng)機(jī)器人工作環(huán)境進(jìn)行二值化處理，再通過柵格搜索計(jì)算出不同方向上機(jī)器人與障礙物之間的距離。進(jìn)而，基于障礙物對(duì)移動(dòng)機(jī)器人有排斥作用以及目標(biāo)點(diǎn)對(duì)機(jī)器人有吸引作用的思想，并將障礙物的形狀和最小安全距離等因素考慮進(jìn)來，在機(jī)器人運(yùn)動(dòng)學(xué)的基礎(chǔ)上，引入速度勢(shì)產(chǎn)生與實(shí)時(shí)距離相關(guān)的速度增量對(duì)移動(dòng)機(jī)器人進(jìn)行實(shí)時(shí)的速度驅(qū)動(dòng)，減少勢(shì)場計(jì)算時(shí)的工作量和提高實(shí)時(shí)避障的效率。最后通過仿真軟件進(jìn)行驗(yàn)證，結(jié)果表明：

（1）所提的算法在包含隨機(jī)分布的形狀多樣的障礙物的工作環(huán)境中，總是能實(shí)時(shí)規(guī)劃出一條平滑、安全的無碰撞路徑。

（2）所提的方法在針對(duì)工作地圖中安全區(qū)域的任意點(diǎn)都普遍具有可達(dá)性。

（3）移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中，實(shí)時(shí)的速度增量大小僅在小范圍波動(dòng)但是沒有趨于零的時(shí)刻，說明該避障算法對(duì)機(jī)器人的運(yùn)動(dòng)性能要求較低且不存在局部極小的問題。

但是，在柵格環(huán)境下需要根據(jù)障礙物的密度來的決定柵格的密度大小，而柵格密度越小，障礙物的輪廓描述的越詳細(xì)，避障精確度越高，但計(jì)算的代價(jià)也會(huì)越大，因此如何找到障礙物的密度來決定柵格密度的關(guān)系是下一步研究方向。