復雜山地環(huán)境下的機器人路徑規(guī)劃

胡平志，楊小柳，李澤滔

(貴州大學電氣工程學院，貴州 貴陽550025)

1 引言

現(xiàn)實中，機器人所處的環(huán)境通常是三維的，因此機器人的三維路徑規(guī)劃更具有普遍性和現(xiàn)實性。然而由于三維空間數(shù)據(jù)量大，導致其路徑規(guī)劃比較復雜。近年來，關于路徑規(guī)劃算法有：人工勢場法[1]、模擬退火算法[2]，蟻群算法等。除使用單一方法進行路徑識別外，文獻[3]提出了一種動態(tài)搜索算子，提高算法收斂的效果；文獻[4]提出蟻群算法和遺傳算法相結合的方法，增加了算法搜索效率；文獻[5]引入一些策略，解決死鎖問題，增加解的多樣性，提高算法的搜索能力;文獻[6]通過引入障礙物排斥權重增加新的啟發(fā)因子修改轉移概率，提高螞蟻避障能力。

不同于空間機器人和水下機器人，地面機器人移動受到地形曲面約束，即機器人被約束在一個曲面上運動，同時又會出現(xiàn)高度上的變化，既要對路徑進行規(guī)劃，同時又要考慮重力約束，雖然文獻[7]提出了結合人工勢場算法的改進蟻群算法，做出了一種曲面地形下的最短路徑，但是算法并未考慮機器人是否有足夠牽引力上坡的問題。

該算法引入了一種新的轉移概率函數(shù)，能夠在完成路徑規(guī)劃的同時，兼顧機器人爬坡的問題，由于引進新的轉移概率函數(shù)，改進算法不再只局限于規(guī)劃最短路徑，也可以規(guī)劃坡度符合機器人爬坡能力的路徑。同時，在實際應用中，相較于常用的柵格地形圖[6，7]，等高線地形圖數(shù)據(jù)更容易獲取，該算法采用等高線對丘陵山地地形進行分層，使得路徑規(guī)劃分層進行。

2 傳統(tǒng)蟻群算法概述

2.1 基本蟻群算法

模擬螞蟻覓食尋跡的過程，單只螞蟻攜帶信息素總量不變，螞蟻走過的路徑越短，單位路程的信息素量越多，經(jīng)過多次迭代，較短的路徑上累積的信息素濃度逐漸增高，選擇該路徑的螞蟻個數(shù)越多。最終，整個蟻群會在正反饋的作用下集中到最佳的路徑上，此時應的便是待優(yōu)化問題的最優(yōu)解。蟻群算法中，第t次迭代下，第k只螞蟻從i點到j點的轉移概率如式(1)所示

(1)

其中：dij為i點到j點的直線距離，τij(t)為t時刻i點到j點的信息素濃度；ρ為信息素耗散系數(shù)；Δτij為i點到j點的信息素濃度總和；Q為單只螞蟻所帶信息素總量；Lk為第k只螞蟻單次迭代所走過的路程。

2.2 三維空間的路徑規(guī)劃

三維空間路徑優(yōu)化也需要先建立一個抽象的三維空間地形圖，方法如下：

1)利用柵格將三維空間劃分為若干小塊。

2)填充部分小塊，作為障礙物，這些區(qū)域不可通行。

不同于二維平面，三維空間中節(jié)點的數(shù)量遠大于二維平面，若不加以處理集合allowk將包含大量元素，將導致以下情況

(2)

使得Pij趨近于0，導致程序運算時間冗長，所以為了避免這一情況，要縮小allowk的范圍，常用的方法有：

1)將三維空間進行分層，使得路徑規(guī)劃過程由起點開始向終點逐層遞進。

2)對每一層在劃分可達域，即第n層向第n+1層轉移時，只選取第n+1層中某一個區(qū)域，這樣就能進一步縮小allowk的范圍。

3 山地環(huán)境的路徑規(guī)劃

3.1 數(shù)字山地地形建模

貴州山地丘陵地占92.5%，為了在這樣的特殊地理環(huán)境下大力發(fā)展包含光伏等新型能源，遂將光伏陣列建在如圖1所示的地形下。

圖1 山地地形下的光伏陣列

該光伏陣列所處位置除了山頂一條主干道路外，沒有其它可供車輛通行的道路，并且該地形起伏程度較小，不存在斷崖式地形，且山地上植被低矮，沒有高大障礙物，所以非常適合開發(fā)機器人進行巡檢以減少人力成本。本文基于這一類地形，探究復雜地形下機器人路徑規(guī)劃問題。

為便于仿真計算，該算法利用式(9)所示函數(shù)，模擬生成一個擁有三個山峰，以及三個低洼的山地地形。

-10(x/5-x3-y5)e(-x2-y2)

(3)

圖2 數(shù)字模擬山地地形

其中藍色五角星代表障礙物。本次尋跡仿真，以水平面為起始位置，以最高頂峰為終點，對于水平面以下部分忽略不計。利用等高線，將山地地形分層；水平線及以上部分高度為7.055共23層，隨機在每層插入零點(0，0，0)作為障礙物，如圖2所示。在路徑優(yōu)化時，會規(guī)避這些障礙點。

3.2 初始位置

曲面內(nèi)蟻群算法可以針對從一個海拔高度到另一個海拔高度上某一點的尋跡，該算法以山腳作為初始位置，以山頂作為終點。第一輪尋路時隨機生成初始點，之后根據(jù)信息素濃度隨機選取初始位置，第一層第s個初始點位搜尋概率如式(4)所示

(4)

由于路程越長信息素濃度越低，所以隨著路程變長初始點位在被選取時的概率就會越小。故經(jīng)過多次迭代，上山起始點，會趨向于靠近目標點。

3.3 等高線分層

三維曲面類似于空間地形，在研究山地地形尋跡的時候，必須考慮山地地形存在高度變化。由于山地地形的特點，該算法利用等高線將地形分層，每層層高為Δz，使得路徑優(yōu)化可以分層進行，為了減少不必要的運算，加快計算速度，就需要減小allowk的范圍，該算法不采用柵格，而是根據(jù)山地地形特點，利用等高線將山地進行分層，使得每次轉移必須從第n層轉移到n+1層，不能跨層轉移，集合allowk的范圍由任意未經(jīng)過點，縮小至第n+1層任一點。等高線示意圖如圖3所示

圖3 等高線示意圖

其中Δx、Δy、Δz分別表示等高線鄰近兩條等高線沿x、y、z軸方向上的增量。在Δz恒定不變的情況下，dij越長坡度角θ會越小，即對機器人爬坡能力要求也越低。

3.4 曲面上兩點間的距離

在曲面上兩點的最短距離被稱為“測地線”，盡管數(shù)學上可以結合曲面方程求取“測地線”，但是山地地形不存在確定的曲面方程，考慮到當|Δx|?ε，|Δy|?ε，|Δz|?ε時有式(5)關系(其中ε為某個非零的極小值)

(5)

所以該算法采用短距離近似曲面上兩點間的距離。同時利用dij

3.5 坡度函數(shù)

該算法將山地地形抽象為一個三維曲面，不同于二維平面，由于在登山時，機器人上坡能力，如果坡太陡，機器人牽引力不足，則會出現(xiàn)打滑現(xiàn)象。該算法引入一個與坡度有關的函數(shù)，如式(6)所示

(6)

該函數(shù)描述了，機器人每移動(Δx2+Δy2)0.5的水平距離所上升Δz的高度，函數(shù)值越大說明坡度越陡，對機器人爬坡能力要求越高。因為山地地形并不是確定函數(shù)關系，所以采用近似量Δx、Δy、Δz來表示，而不適用變分進行計算。同時引入與坡度相關的啟發(fā)函數(shù)[gij(t)]γ=f(Δx，Δy，Δz)-γ。其中γ為坡度函數(shù)因子。

3.6 適應度函數(shù)及約束條件

最佳個體適應度是體現(xiàn)優(yōu)化度的重要指標。該算法以路徑總長度為優(yōu)化目標，每只螞蟻完成搜索，都會記錄下該個體所走過的路徑長度，并用評價函數(shù)記錄總長度，得到最短路徑，同時也會通過引入的坡度函數(shù)，使得該最短路徑上所以坡度滿足約束條件。評價函數(shù)以及約束條件如式(7)所示

(7)

其中文獻[16]中機器人最大爬坡角度為θmax=25.6°，tan(θmax)≈0.479，文獻[17]，機器人翻越障礙物時俯仰角為68°，tan(θmax)≈2.475，文獻[18]中機器人最大爬坡角度為θmax=20°，tan(θmax)≈0.364，由此，即使以最大值作為參考，機器人最大爬坡坡度θmax<68°，tan(θmax)<2.475。

3.7 改進后的節(jié)點轉移概率函數(shù)

改進后，三維曲面上的節(jié)點轉移概率函數(shù)為

(8)

圖4 改進蟻群算法流程圖

3.8 坡度函數(shù)驗證

為驗證γ變化可以改變坡度大小，通過仿真得到結果如表1所示

表1 γ變化與坡度變化

當γ值由1增加到3.5時tan(θ)方差、均值以及峰值逐漸下降，由此表明引入的坡度函數(shù)可以有效地改變路徑的坡度，從而使得四足機器人可以適應被規(guī)劃路徑得坡度。當γ=3.5時tan(θ)的值均低于2.475，爬坡角度θ均小于68°，在機器人爬坡能力允許范圍內(nèi)，所以在規(guī)劃最優(yōu)路徑時取γ=3.5。

在上升高度相同的前提下，γ越大，所規(guī)劃路徑坡度越小路程越長，反之γ越小，坡度越大，路程也會越短。為確定γ變化時對路徑長度的影響，進行仿真，得到結果如圖4所示。

圖5 γ值對收斂效果的影響

同樣150只螞蟻，20次迭代，隨著γ值逐漸增大，路徑長度逐漸增大，同時由圖4結合表2可以看出，隨著坡度因子γ減小坡度不斷減小，但是算法收斂速度下降，當γ>0時會嚴重影響算法收斂性。

3.9 改進信息素函數(shù)

(9)

為驗證改進后的算法可以使得算法加速收斂，以路程平均值作為y軸，以迭代次數(shù)作為x軸，令γ=3.5，分別測試偏差因子ν=2、4、6時對于算法收斂速度的影響，經(jīng)過20次迭代，收斂情況對比如圖6所示。

圖6 ν值對最優(yōu)路徑的影響

由圖6可知，該改進算法可以有效提升算法收斂效果，當ν=6時能得到更好的收斂效果，所以確定參數(shù)取值參數(shù)ν=6。

4 算法仿真及結果分析

4.1 算法仿真

以表2作為算法初始條件進行仿真。

表2 算法初始條件

在文獻[5，16]中，蟻群算法種群大小通常不超過50，但是在做仿真時發(fā)現(xiàn)，種群數(shù)量太少難以達到理想效果，因此設置如表初始參數(shù)，以N只螞蟻平均路程為y軸，以迭代次數(shù)為x軸做了多次實驗得到如下折線圖

圖7 種群大小對收斂結果的影響

如圖7所示，當種群數(shù)量N=150時收斂效果較好，故該算法中所算法方法均設置種群數(shù)量為150。

4.3 路徑優(yōu)化方案

機器人由山底到達山頂，路徑越短，坡度越陡，運輸相同負載，攀爬所需的牽引力越大。反之路徑越長，坡度越緩，運輸相同負載攀爬所需牽引力越小。坡度平緩和路徑最短是相互矛盾的。以下將路程最短路徑簡稱為最短路徑，將最適坡度路徑簡稱為最優(yōu)路徑。下文將著重對比二者的路徑長度以及坡度陡峭程度。

4.3.1 最短路徑

本節(jié)基于原始蟻群算法規(guī)劃出最短路徑，迭代條件如表2最短路徑參數(shù)為初始條件，其中取ν=6，γ=-3如圖8所示。

圖8 最短路徑仿真效果

其中圖8(a)為最短路徑三維圖形，中圖8(b)為最短路徑的俯視圖。如圖所示，本次規(guī)劃路徑直接由水平面上某一點，直接到達山頂，多次仿真結果如表3所示。

表3 最短路徑仿真結果

最小路程收斂于7.7附近，但是這一路徑坡度極大，坡度最大值達到8.216，坡度角達到83°，遠超過機器人爬坡能力極限。所以盡管路徑最短但是不符合實際情況。

4.3.2 機器人爬坡能力允許范圍內(nèi)的最優(yōu)路徑

圖9 最優(yōu)路徑仿真效果

其中圖9(a)為最優(yōu)路徑三維圖形，圖9(b)為最優(yōu)路徑的俯視圖。如圖所示，路徑不再是直接行進，而是在保證坡度不超過機器人爬坡能力的前提下，盤山前行。

表4 最優(yōu)路徑路程仿真結果

對比可明顯看出，盡管路徑曲折，路程較最短路徑更長，但是始終保證tan(θ)<2.475，即上坡角度θ<68°，符合機器人爬坡能力。

圖10 最短路徑與最優(yōu)路徑坡度對比

如圖10所示，改進后算法所規(guī)劃最優(yōu)路徑的坡度相較于最小路徑的坡度更小了，該算法確保了所規(guī)劃路徑符合四足機器人爬坡能力。

5 結論

本文提出了一種加入了帶有坡度函數(shù)的蟻群算法。由于引入了坡度函數(shù)，使得該算法在搜尋最優(yōu)路徑時，能充分考慮到機器人有限的爬坡能力問題。

針對于引入坡度函數(shù)后的蟻群算法存在收斂速度慢以及難以收斂到最優(yōu)路徑的問題，本文進一步改進了信息素濃度計算方法，使得該算法快速收斂，更好地適應復雜地形。通過仿真及結果分析表明，相較于原始的蟻群算法，該算法能更快的搜索到更優(yōu)的解。該算法為三維曲面上的路徑規(guī)劃提供了一種快速有效地解決問題的方法。

這一改進算法所得到的路徑，充分地考慮了機器人在復雜地形下的爬坡能力，相較于以往的路徑規(guī)劃算法更具有實用價值。