融柵格法和人工勢場法的機器人三維路徑規劃

葛笑寒

（1.河南省高校節能照明工程技術研究中心；2.三門峽職業技術學院，河南三門峽472000）

0 引言

隨著技術的進步和經濟的發展，機器人技術更加智能化、多元化。工業生產中對其穩定性和可靠性要求越來越高。這就對其導航系統有了更嚴格的要求。在局部路徑規劃方面，人工勢場法的核心是把機器人在環境的運行軌跡抽象成一種虛擬受力運動[1]。柵格法又稱為單元分解建模法，其本質是把機器人的運行空間細分為很多個二維信息的位置，將機器人工作空間劃分為具有多個二值信息的區域，也即柵格[2]。文獻[3]則以改進人工勢場法為主，柵格法為輔的方案來實施避障，使得機器人能夠盡快地脫離局部極小并成功地繞過障礙物到達目標點。但建模環境為二維，具有局限性。文獻[4]對移動機器人三維空間路徑規劃問題,提出了一種基于改進蟻群算法的路徑規劃算法。結果證明了算法的低耗時和實用性。但計算量大，易陷入局部最優解。本文結合柵格和人工勢場法，使機器人沿著位勢減小的方向，避開障礙物，最終達到目標點。同時利用柵格三維模型把運行軌跡拓展至立體空間。

1 柵格法三維空間建模

將機器人運動的區域成從真實的環境中抽象出三維空間模型，并細分多個三維區域，且每個區域都用三維信息表示，最后通過優化算法實現路徑搜索[5]。如圖1所示，將左下角頂點作為三維坐標的原點建立三維坐標系統。x軸和y軸分別為縱向橫向增加方向，z軸為高度增加方向，這樣就組成了三維空間。該區域即為三維路徑規劃空間，然后把坐標等分，就形成柵格三維空間。在此空間中把目標點、移動機器人和障礙物都看作是一個質點，用x軸、y軸和z軸三個方向表示它在空間的位置。每個質點不能超越分割后的小立體柵格的邊界。在該空間中，用黑色圓點代表障礙物，則黑點存在的空間位置表示障礙物所在位置；黑色方格代表目標點位置已經被目標點占據。假設三維柵格地圖尺度和坐標不變，各個立體柵格也是靜止不變，則要求目標點和障礙物在空間中運行時不能相互碰撞，也即它們任意時刻不能同時占用同一個柵格。它們的速度和運行方向隨機生成，檢測到邊緣柵格時按相反方向運行。

工作流程如圖2所示,首先確定起始點、目標點和障礙物，其次搜索機器人的相鄰柵格并進一步選出自由柵格。計算自由柵格的中心點與目標柵格中心點的距離，然后推算機器人下一步的行進軌跡，判定機器人質點是否已經到達目標柵格，如果沒有則返回繼續搜索柵格，如果到達則任務完成。

三維機器人路徑規劃的數學模型與二維環境下的比較相似，從起點出發，在三維空間以路徑最短為目標，規避障礙物到達終點。三維路徑規劃即找到一條可行的最短區域L。要求機器人行走的每一步只能朝著一個方向移動一步，可以是前后、左右或是上下。當遇到障礙柵格時，須避免碰撞。在機器人移動過程中，需移動到目標點后，方可停歇。移動包括橫向、縱向及前后的位置移動，希望機器人以盡量少的總位移達到既定目標。任意時刻，障礙物和目標點的運動方向有6種，如表1所示。

圖1 三維空間模型

圖2 柵格法流程圖

表1 機器人運動方向及坐標

2 柵格勢場的建立

人工勢場法的本質就是把移動機器人的工作環境虛擬成引力和斥力的合成勢場，移動機器人在該勢場中受到障礙物的斥力和目標點的引力的雙重作用，移動機器人在兩種力的作用下最終達到目標點[6]。

假設移動機器人的任意時刻的坐標為X=(x，y，z)，對應目標點的坐標為Xg=(xg，yg,zg)，則引力勢場的函數表達式公式為

公式中X與Xg指的是機器人當前位置和目標值在在三維空間中的位置，k為比例位置增益，ρ2(X,Xg)是機器人坐標到目標的距離。

引力勢場函數的負梯度，則定義引力公式為

斥力場常見函數

斥力函數實用公式為

當移動機器人接近目標時，如果目標點周圍存在障礙物，則斥力場遠高于引力場，移動機器人將不能運動到目標。可引入調和函數來解決[7]，調和函數是可微的，故可以保障機器人運動的軌跡的正確性。在計算出空間每個位置位勢值后，機器人從起始點出發，每一步都應沿著最小位勢值方向行進，一直到達目標點。如果環境突然變化，則應重新規劃路徑。

使用人工勢場法時，三維環境中的每個坐標都有一個位勢值，代表移動機器人前進一步與障礙物碰撞概率的大小。取目標點坐標的位勢為0，障礙物的位勢值為1。其他位置的位勢與時間有關。任意時間，空間某位置的位勢值是該坐標在下一時刻所能夠到達的所有空間坐標位置的位勢的均值。它的含義是移動機器人從空間某位置坐標隨機運動一步與障礙物發生碰撞的概率。值越小，代表碰撞的概率越低。

P(x,y,z,t)表示t時刻位置P(x,y,z)的位勢值。S(x,y,z)指(t+1)時刻，機器人達到所有位置的可能性。在三維空間環境中，機器人每個步長可以有7種位置：向上(N)，向下(S)，向左(W)，向右（E），向前(F)，向后(B)以和停止(T)。p(x,y,z,t+1)表示機器人從P(x,y,z)出發，在(t+1)時刻，所有位置的位勢值之和。

根據上式，知道(t+1)時刻的位勢后，才能推算t時刻空間某個位置的位勢時，即必須逆向計算。為了計算方便，需確定一個時間步長。步長L的推算流程如圖3所示。建立一個空節點隊列，把起始位置入列。取三維空間模型構建對角線，按照步長推算流程，找到所有對角線中按照此流程確定的的最大值作為系統的步長，然后計算三維空間中任意位置任意時間的位勢。則移動機器人從起始點出發，沿位勢減少的方向前進，最后避開障礙物抵達目標。

圖3 確定步長流程

3 仿真及結果分析

使用MATLAB7.0建立一個邊長9的立體三維柵格，并把地圖9等分，這樣就形成了729個小立體柵格地圖。把移動機器人、目標點和障礙物看成質點，使用x,y,z代表質點在空間中的位置。系統的工作流程如圖4所示，利用對角線原則及推算流程確定最大時間步長，然后計算空間所有位置任意時刻的位勢。則移動機器人從始點開始，沿位勢值降低的方向行走，最后到達目標。

圖4 機器人行進流程

圖5 三維環境下機器人的運行軌跡

在三維空間環境中機器人起始點坐標(2,1,1),目標點(8,9,9)，運動方向在七個方向中隨機產生，系統中設30個障礙物，方向和速度隨機。據對角線確定步長L=16，在該空間中，用黑色圓點代表障礙物，黑方格代表目標位置點。行走方向由速度矢量決定，行進至空間邊界時，速度大小恒定，反方向運動。大黑圓圈代表移動機器人任意時刻的行走位置，把這些點用平滑導線連接就是機器人的行進路線。機器人在仿真條件下的運行軌跡如圖5所示，從圖5可以看出，機器人從起始位置開始，沿著位勢降低的方向行走，此方向發生碰撞的概率最低最小，最后移動機器人到達目標點。

4 結論

通過柵格法建立了機器人的三維空間模型，對人工勢場算法做了分析。在空間中確定了柵格的信息，并且根據對角線原則確定了達到目標點的時間步長。然后計算三維空間位置每個時刻的位勢值。機器人從原點出發沿著位勢降低的方向最終到達目標點。最后在MATLAB環境下，建立三維空間模型。仿真結果表明該方法在三維空間能找到最優的路徑，且避開障礙物到達目標點。