楊凌耀，張愛華，張 潔，宋季強

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

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

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

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

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

1 問題描述

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

圖1 移動機器人工作場景示意圖Fig.1 Sketch map of mobile robot working scene

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

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

2 柵格地圖與柵格搜索

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

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

3 速度勢避障算法

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

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

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

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

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

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

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

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

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

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

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

4.1 實時軌跡生成

通過對式（1）中的運動學(xué)模型得出的速度矩陣施加實時速度增量a(t)，即可得到t+1時刻機器人的速度矩陣為：

通過速度矩陣，可以得到移動機器人穿越障礙物的實時軌跡曲線：

機器人下一時刻的方位角為：

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

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

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

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

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

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

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

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

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

步驟10 a(t)對機器人作速度驅(qū)動，得到機器人下一時刻的目標(biāo)點。

5 仿真與分析

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

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

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

（1）起點[20，20]，終點[650，20]的情況

（2）起點[20，20]，終點[20，650]的情況

（3）起點[20，20]，終點[650，650]的情況

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

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

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

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

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

6 結(jié)論

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

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

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

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

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