改進AOA結合貝塞爾曲線平滑的機器人路徑規劃

張亞林，李曉松

(1.廣州應用科技學院 計算機學院，廣東 廣州 511370；2.廣州大學 計算機科學與網絡工程學院，廣東 廣州 511370)

0 引 言

路徑規劃是機器人導航[1]的關鍵部分，其本質是根據預設條件，在起點到終點間搜索一條安全避讓障礙物的最短路徑[2]。目前，機器人路徑規劃方法可分為兩種，一種是如人工勢場[3，4]、A*[5]、Voronoi圖[6]、可視圖等傳統方法。另一種是智能優化算法。智能優化算法擁有傳統方法無可比擬的啟發搜索機制，能夠模擬自然界種群的覓食和進化行為。隨著路徑規劃場景規模、類型和障礙物復雜度增加，傳統方法已出現性能不足，智能算法在路徑規劃上逐漸體現優勢。目前，蟻群算法[7]、遺傳算法[8]、粒子群算法[9]、蜂群算法[10]、蛙跳算法[11]等都已在路徑規劃有所應用。但應用智能算法解決路徑規劃問題，搜索精度高、收斂快的算法仍是學者們面臨的新問題。

阿基米德優化算法AOA是受阿基米德原理啟發提出的一種元啟發隨機優化算法[12]。由于算法較新，目前搜索到的文獻已應用在參數尋優[13]、風電系統功率優化[14]和機械設計[15]等領域。然而，AOA與同類智能算法一樣，在求解復雜多維問題時依然易出現搜索精度差、局部最優及尋優性能不穩定的不足。研究人員也在該算法基礎上做了改進工作[15]。然而，分析現有工作不難發現，AOA仍有不足：首先，種群初始化隨機，導致種群多樣性較差，迭代早期個體搜索盲目；其次，局部開發階段僅以最優個體引領，全局搜索階段則以隨機個體指引，牽引方式盲目，容易導致算法陷入局部最優；最后，平衡全局搜索與局部開發的密度降低因子沒有自調節功能，在不同迭代時期無法賦予個體差異化尋優能力，易丟失適應度較優的個體。

為此，本文將設計一種改進阿基米德優化算法SIAOA，利用改進阿基米德算法對路徑規劃進行迭代求解，引入貝塞爾曲線平滑策略對生成路徑作平滑處理。在不同規模和復雜性的柵格地圖中對算法的有效性進行驗證。

1 阿基米德優化算法AOA

AOA算法中，轉移因子TF用于控制個體碰撞與平衡態切換，即控制全局搜索和局部開發，定義為

(1)

式中：Tmax為最大迭代次數。

算法首先初始化個體密度den、體積vol及加速度acc。計算最優個體xbest、最優密度denbest、最優體積volbest和最優加速度accbest，并對密度、體積和加速度更新，具體為

(2)

式中：rand為(0，1)間隨機值，deni(t) 和deni(t+1) 指迭代t、t+1時個體i的密度，voli(t) 和voli(t+1) 指迭代t、t+1時個體i的體積，i=1，2，…，N，N為種群規模。

若TF≤0.5，AOA進入全局搜索。個體加速度為

(3)

式中：denmr、volmr和accmr分別指隨機選擇碰撞個體mr的密度、體積和加速度。

若TF>0.5，AOA進入局部開發。個體加速度更新為

(4)

將加速度進行標準化處理為

(5)

式中：accmin、accmax為當前種群中個體的最小加速度和最大加速度，u、l為常量。

全局搜索階段碰撞個體位置更新為

xi(t+1)=xi(t)+
c1·rand·acci-norm(t+1)·d·(xrand(t)-xi(t))

(6)

式中：c1為常量，xi(t+1)、xi(t) 指迭代t+1、t時個體i的位置，xrand(t) 為迭代t時隨機選擇個體位置，d為密度降低因子，更新為

(7)

局部開發階段個體位置更新為

xi(t+1)=xbest(t)+
F·c2·rand·acci-norm(t+1)·d·(T·xbest(t)-xi(t))

(8)

式中：xbest(t) 為迭代t時最優個體，c2為常量，T=c3×TF，c3為常量。F為控制個體運動方向的標識因子，定義為

(9)

式中：p=2×rand-c4，c4為常量。

2 分段慣性權重自適應改進AOA：SIAOA

2.1 混沌Circle種群初始化

AOA隨機生成初始種群，這會導致個體分布缺乏均勻性，算法易得到局部最優。為此，改進算法采用Circle混沌映射改進初始種群生成方式。混沌系統具有隨機性和遍歷性，對種群的均勻分布更有利。目前，常用混沌映射有Logistic、Tent和Circle。圖1展示了3種混沌系統的序列值分布，一根柱狀圖的取值為0.05，即分別在[0，0.05]、[0.05，0.1]…進行取值頻次統計。可以看出，Logistic在兩個邊界區域的取值概率明顯更高，呈切比雪夫型分布，這種不均勻分布對AOA的尋優精度有不利影響。Tent的分布更加均勻，但容易陷入不動點，且周期不穩定，也存在均勻度較差的問題。Circle映射擁有與Tent一致的分布均勻性，且更加穩定。

圖1 不同混沌映射混沌值取值頻次分布

改進算法利用混沌Circle映射實現種群初始化，公式

yk+1=mod(yk+0.2-(0.5/2π)sin(2π×yk)，1)

(10)

式中：yk、yk+1為k、k+1次迭代的Circle混沌值。

生成Circle混沌值后，將混沌值與個體位置進行逆映射，映射規則為xi，j=lbj+yi，j×(ubj-lbj)，xi，j為個體i在維度j的位置，yi，j為混沌值，[lbj，ubj] 為搜索邊界，j=1，2，…，d，d為位置維度。

在二維空間內布置30個種群個體為例展示不同方法生成的個體分布情況。圖2是兩種方法生成的種群個體分布。隨機初始化容易發生位置重疊，Circle混沌初始化的均勻性更好，能夠更好地對解空間進行遍歷。

圖2 兩種種群初始化分布

2.2 自適應密度降低因子更新

根據式(7)，隨著算法迭代，密度降低因子d會從1呈線性遞減至0。結合AOA的全局搜索和局部開發式(6)、式(8)，d過小，會導致種群個體原有優質信息丟失，降低種群個體多樣性；d過大，又會影響個體向全局最優的漸進關系，降低收斂精度。為此，改進算法設計自適應密度降低因子更新，引入個體進化成功率對因子d更新，使個體能夠依據進化進程對自身位置自適應調整。

令Si(t+1) 為個體i在t+1次迭代的成功率，則

(11)

式中：pbi(t+1)、pbi(t) 指個體i在t+1、t次迭代的歷史最優解，f(pbi(t+1))、f(pbi(t)) 指個體歷史最優解適應度。將種群進化成功率SP定義為個體進化成功數與種群規模之比，即

(12)

算法迭代早期，個體分布廣泛，成功進化較多，表明全局最優對種群進化引領有效。迭代后期，種群逐漸聚集，進化成功率隨之變低，此時應保證更多優質個體信息促進局部開采精度。結合種群進化成功率SP，對個體進化狀態自適應更新，均衡算法全局尋優，定義密度降低因子為

(13)

式中：γ、λ分別指種群進化成功率系數和適應度歸一化系數，0<γ、λ<1，且γ+λ=1，Fi(t) 為適應度歸一化值，定義為

(14)

式中：κ為衰減因子，ζ控制分母不為0，fi(t) 為迭代t時個體i的適應度，fmin(t)、fmax(t) 為種群當前最優適應度和最差適應度。根據式(13)，d將根據個體適應度動態自適應調整，若適應度較優，處于最優解鄰域，接近全局最優，d將維持原有性質，提高收斂性；若適應度較差，離全局最優仍較遠，d將接近最大值，激勵個體保持多樣性，實現廣泛搜索。

2.3 分段慣性權重位置更新

根據局部開發式(8)可知，種群更新將由最優個體引導，若最優個體為局部最優解，種群將出現早熟收斂。為此，改進算法引入分段慣性權重機制，在迭代前/中期，利用雙曲正切函數對權重值w(t) 更新，使算法中前期慣性權重呈非線性遞減；在算法迭代后期，利用正弦函數上下波動對權重值w(t) 更新，利用跳躍式位置變異降低算法陷入局部最優的概率。分段慣性權重計算公式為

(15)

式中：wstart、wend為慣性權重初值和終值，α、β2為調節因子，用于控制正切函數和正弦函數的曲線平滑性，β1、β3為常量。圖3為400次迭代w(t) 的變化曲線，相關參數均是多次實驗的最優檢驗值。可見，迭代前期，w(t) 較大，這可保證算法在更廣泛空間內進行全局搜索，增加找到全局最優解的概率；迭代中期，w(t) 逐漸減小，利于算法在最優解鄰域進行精細開發，提高搜索精度；而迭代后期，正弦函數不規則波動使w(t) 變化出現不確定性，這可以增強局部區域內開發的多元性，使算法具備跳離局部極值的能力。

圖3 分段慣性權重

結合分段慣性權重，改進算法全局搜索為

xi(t+1)=w(t)·xi(t)+
c1·rand·acci-norm(t+1)·d·(xrand(t)-xi(t))

(16)

改進算法局部開發為

xi(t+1)=w(t)·xbest(t)+
F·c2·rand·acci-norm(t+1)·d·(T·xbest(t)-xi(t))

(17)

2.4 SIAOA設計

步驟1 設置種群規模N、空間維度dim、個體密度den、體積vol、加速度acc、最大迭代次數Tmax、常量u、l、c1、c2、c3、c4、慣性權重初值wstart和終值wend、α、β1、β2、β3、θ；

步驟2 利用混沌Circle映射初始化種群，初始迭代t=0；

步驟3 計算個體適應度，確定最優的xbest、denbest、volbest和accbest；

步驟4 利用式(1)更新TF，利用式(2)更新den和vol，利用式(13)更新d，利用式(15)更新w(t)；

步驟5 若TF≤0.5，進入全局搜索。利用式(3)更新個體加速度，根據式(5)對加速度acc作標準化處理，根據式(16)更新個體位置；

步驟6 若TF<0.5，進入局部開發。根據式(17)更新個體位置；

步驟7 更新迭代次數t=t+1；

步驟8 判斷算法是否迭代Tmax次，若是，輸出全局最優個體；否則，返回步驟3執行。

3 結合SIAOA與貝塞爾曲線平滑的路徑規劃

3.1 路徑規劃建模

利用柵格地圖建立機器人路徑規劃模型。柵格地圖由二進制0和1取值的柵格矩陣構成。若柵格單元取值0，表示白色柵格，機器人可通過；若取值1，表示黑色柵格(障礙物)，機器人無法通過，需繞行。柵格地圖從下向上、從左向右依次編號1、2、…。如10×10柵格地圖，共有100個柵格單元，柵格序號與坐標對應，對應關系可表示為式(18)，且坐標值為柵格中心點坐標

(18)

式中：(xi，yi) 為柵格i中心坐標，Nx、Ny為地圖行列數，a為柵格單元邊長，且柵格單元為相同正方形，mod為模運算符，ceil()為向上取整。

柵格地圖實際應用時，若柵格單元內障礙物面積小于柵格單元，為避免機器人與障礙物碰撞，需對障礙物膨化處理，填充整個柵格。令機器人處于某柵格，如圖4(a)所示，周圍不存在任一障礙物柵格，可行進路徑共8條，即圖4(a)的1～8，剔除前一步原路返回路徑，實際路徑共7條。若令柵格邊長為1，在保證機器人不與障礙物碰撞前提下，機器人移動一次的距離范圍為[1，21/2]。圖4(b)是有效避障可行進路徑。圖4(c)和圖4(d)均為無效路徑，圖4(c)直接與障礙物發生碰撞，圖4(d)則是非行進路徑。

圖4 路徑說明

3.2 路徑規劃評估

最優路徑由適應度函數評估，適應度函數綜合考慮兩個指標：一是路徑最短，二是確保路徑盡可能平滑，路徑拐點盡量少。

(1)最短路徑f1。令兩個臨近柵格單元的坐標為Pi(xi，yi)、Pi+1(xi+1，yi+1)， 機器人從Pi移動至Pi+1的距離為

(19)

則機器人路徑規劃總長度為

(20)

其中，dim為經過柵格總數。則最短路徑函數f1為

minf1=1/L

(21)

(2)路徑平滑度f2。機器人行進僅向臨近柵格單元移動。如圖5所示，若機器人從柵格A移動至O，接下來有OB、OC、OD、OE、OF、OG、OH共7個方向。由于對稱性，行進路徑與原路徑AO之間夾角θ可能為45°、90°、135°和180°，分別對應OB、OC、OD和OE。夾角θ越大(180°)，表明路徑越平滑，拐點越少。根據原路徑與下一條路徑夾角θ的不同，設置不同懲罰值β，具體為

圖5 路徑平滑度

(22)

則路徑平滑度函數f2為

(23)

結合路徑最短和路徑平滑度因素，適應度函數定義為

fit=a×f1+b×f2

(24)

其中，a、b指路徑長度和平滑度權重，a+b=1，0

路徑尋優過程中，個體在搜索空間內的位置代表一條候選路徑。算法通過啟發式搜索策略，從若干候選路徑集中搜索一條適應度最優的路徑，還需滿足兩項約束：①機器人移動限定在固定大小柵格矩陣中，且移動路徑和路徑間節點不能碰撞或穿越障礙物柵格；②移動路徑不能迂回，即若機器人從柵格單元Pi(xi，yi) 移動至Pi+1(xi+1，yi+1)， 則xi>xi+1和yi>yi+1或xi

3.3 個體編碼

利用SIAOA搜索機器人最優路徑，即通過適應度函數尋優并以迭代方式更新個體代表的路徑規劃候選解。若個體編碼維度為dim，則代表路徑規劃中經過dim個柵格，每個維度上元素為柵格序號(xj)，代表柵格j的位置，則個體i可表示為Xi={xi，S，…xi，j，…，xi，T}， 代表一條路徑規劃候選解，xi，S和xi，T代表機器人起點和終點。dim對應路徑規劃優化維度，個體位置的每一個維度元素對應下一個移動柵格位置。

3.4 貝塞爾曲線路徑平滑

結合柵格地圖的機器人移動是以柵格中心點為導航點移動，這種方式路徑轉折較多，機器人耗能較大。為了優化路徑，引入貝塞爾曲線平滑對路徑拐點處理。

n階貝塞爾曲線函數式為

(25)

式中：u為獨立變量，P(u) 為貝塞爾曲線運動控制點，P(i) 為位置點，P(0)、P(1) 為曲線起點和終點。位置點P(i) 可構成特征多邊形，Bi，n(u) 為n次伯恩斯坦多項式，滿足

(26)

若n=1，貝塞爾曲線為一階曲線，即直線，位置點僅有P(0) 和P(1) 兩個點；若n=2，貝塞爾曲線為二階曲線，即拋物線，位置點有P(0)、P(1)和P(2)； 若n≥3，曲線為高階貝塞爾曲線，位置點有n+1個。貝塞爾曲線導數形式為

(27)

圖6是貝塞爾曲線平滑路徑示意圖。

圖6 貝塞爾曲線平滑

3.5 路徑規劃算法

利用SIAOA求解機器人路徑規劃問題，即以適應度函數(24)最小為目標，迭代搜索行進柵格單元。每個搜索空間內的個體視為一條候選路徑規劃方案，個體位置維度對應經過柵格單元數，個體各維度的元素即為柵格單元坐標值，整個種群代表路徑規劃候選解集。SIAOA將圍繞候選解集迭代尋優，以目標函數(24)最小作為評估個體位置質量的適應度函數，通過SIAOA對個體位置迭代更新，得到最終路徑規劃最優解。算法具體步驟如下：

SIAOA求解機器人路徑規劃：

(1)設置SIAOA參數：種群規模N、搜索維度dim、最大迭代數Tmax和搜索范圍[lb，ub]；

(2)建立柵格地圖，設置機器人起點S和終點T，利用混沌映射規則生成N條初始可行路徑X1，X2，…，XN；

(3)對種群個體編碼，表示為一條路徑Xi={xi，1，xi，2，…，xi，dim}，i∈[1，N]，xi，j對應一個柵格位置，dim為路徑節點數；

(4)按式(24)計算種群個體適應度，并對個體降序排列；

(5)確定當前種群最優解，并標記為下一輪的搜索目標Xbest；

(6)whilet≤Tmaxthen

(7)執行SIAOA的迭代搜索過程；

(8)End while

(9)返回最優個體，解碼為路徑規劃解，并以貝塞爾曲線進行路徑平滑處理。

以上算法步驟中，種群規模不能設置過大，一般設置30個種群個體即可，否則算法迭代時間會過長。搜索維度則對應于機器人行進路徑中柵格中心的坐標。對于路徑搜索問題，算法最大迭代次數可在實驗運行中動態設置，一般小規模地圖中的路徑規劃，200次內迭代可以找到最優解。個體搜索范圍即對應路徑坐標的取值范圍。算法第(9)步中，最優個體即代表機器人的最優路徑，由機器人所經過的柵格中心點坐標構成，連接中心點坐標即可生成路徑規劃解。而貝塞爾曲線是對生成路徑中的轉彎點進行平滑性處理，以減小機器人實際行進路徑長度。

4 仿真實驗

4.1 實驗配置

在Matlab R2019a平臺構建仿真實驗，先以數組定義柵格地圖，并以元素為1的柵格定義障礙物柵格。然后，對SIAOA算法進行初始化操作，迭代執行SIAOA算法生成柵格地圖中路徑搜索最優解。設置種群規模N=30，迭代次數Tmax=200，u=0.9，l=0.1，c1=2，c2=6，c3=1，c4=2，權重初值wstart=0.8，終值wend=0.4，α=0.75，β1=0.23，β2=0.18，β3=1.6，θ=0.5，適應度函數權重因子a=b=0.5。引入標準AOA算法[12]、蟻群算法ACO[7]和協同改進阿基米德優化算法CIAOA[15]進行對比分析。

構造10×10和30×30柵格地圖進行實驗，10×10柵格地圖可模擬障礙物稀疏分布的小型室內場景，30×30柵格地圖可模擬障礙物隨機且密集分布的大型室內場景，柵格單元大小為1。為了避免偶然性，在相同參數配置下獨立運行算法10次。將機器人起/終點設置在柵格地圖左上角和右下角柵格，即10×10中起/終點為柵格91和10，坐標分別為(0.5，9.5)和(9.5，0.5)，30×30中起/終點為柵格871和30，坐標分別為(0.5，29.5)和(29.5，0.5)。

4.2 實驗分析

(1)10×10柵格地圖。圖7是10×10柵格地圖的路徑規劃結果。該地圖有22個障礙物，結構相對簡單，算法均能找到有效路徑。結合表1統計結果，SIAOA路徑規劃結果最優。由于柵格規模較小，ACO、CIAOA雖然規劃路徑不一樣，但最優路徑長度相同，SIAOA在這兩種算法基礎上路徑長度減小了4.0%。路徑拐點上，SIAOA拐點最少，僅有3個，且路徑長度也最短。ACO和CIAOA在最優解上均有5個拐點，而AOA雖然拐點數更少為4個，但其路徑最長，不是最優解。在路徑長度和拐點數上標準差更小表明改進算法的搜索穩定性更好，其波動幅度更小。此外，在迭代次數均值上，4種算法基本可在約30次迭代內找到最優解，而SIAOA可以在約22次迭代后即收斂在最優解上，是算法中最少的。在尋優運行時間上，4種算法差別不是很明顯，主要是柵格規模較小，路徑搜索相對容易。圖8是算法迭代曲線圖。從最后的收斂精度看，SIAOA是所有算法中最優的。

表1 10×10柵格地圖

圖7 10×10柵格地圖

圖8 10×10柵格地圖迭代曲線

(2)30×30柵格地圖。如圖9是30×30柵格地圖中算法的路徑規劃結果，表2是相關統計結果。在路徑長度最優值上，SIAOA為27.46，分別比CIAOA、ACO和AOA降低了15.79%、19.23%和27.38%。在路徑長度均值上，分別降低了17.08%、25.85%和34.18%。此外，在該地圖中，SIAOA的拐點數量優勢更為明顯，平均要比對比算法少約一半路徑拐點。傳統算法搜索方式盲目性較大，個體搜索性能沒有得到有效優化，導致路徑搜索時遠離目標方向行進，甚至出現路徑回環交叉，規劃路徑肯定不是最優。SIAOA在綜合考慮初始種群質量、全局搜索與局部開發均衡和分段權重賦予的跳離局部最優能力能夠有效提升算法搜索精度，得到拐點更少、與目標位置方向更一致的柵格。從路徑長度、拐點數量及迭代效率看，隨著柵格地圖規模增加，SIAOA的性能優勢顯現更加明顯，說明在模型復雜性增加的環境下，算法保持著強健的搜索穩定性。圖10也顯示SIAOA收斂更快。

圖9 30×30柵格地圖路徑規劃結果

圖10 30×30柵格地圖迭代曲線

(3)貝塞爾曲線路徑平滑。對SIAOA的最優路徑做貝塞爾曲線平滑處理，結果如圖11所示。經過平滑處理后，SIAOA路徑長度分別減小3.61%和10.34%，說明引入貝塞爾曲線平滑能有效對路徑進行二次優化，平滑路徑長度優于原始折線路徑長度，且在地圖規模增加后，曲線平滑對路徑長度降低比例逐步提高，這是由于路徑規劃規模增加后，原算法路徑拐點會增加，而對更多拐點平滑處理勢必降低路徑長度。相同平滑處理也可擴展至對比算法生成的路徑上。

圖11 兩個柵格地圖的最優路徑平滑前后對比

5 實證案例研究

為了驗證SIAOA的實用性，以SpiderPi(樹莓派)六足機器人展開實證研究。在實驗室建立一個3m×3m的正方形區域地圖。將SpiderPi樹莓派六足機器人放置于起始位置，在區域內設置若干障礙物，并設置機器人的行進終點位置。實際場地拍攝如圖12所示。將該實驗場景處理為邊長15 cm，大小為20×20的柵格地圖。設置機器人起點坐標為(0.5，19.5)，目標點坐標為(19.5，0.5)，分別處于左上角柵格和右下角柵格。在六足機器人開發板中燒制程序實現蟻群算法ACO、AOA和SIAOA算法，使機器人自動搜索最優移動路徑。

圖12 實驗場景

兩種算法的路徑規劃結果如圖13所示。可以看出，SIAOA在路徑長度、轉彎次數方面都要優于AOA和ACO，驗證SIAOA在解決實際問題的機器人路徑規劃中具有實用性。

圖13 實證場景規劃結果

6 結束語

為了解決傳統算法求解機器人路徑規劃無法找到最短路徑、收斂慢且拐點多的不足，提出結合改進阿基米德優化算法與貝塞爾曲線平滑的機器人路徑規劃算法。首先引入混沌Circle映射、自適應密度降低因子調節及分段慣性權重對傳統阿基米德優化算法進行改進；再構建移動機器人路徑規劃模型，結合路徑長度和路徑平滑性構造適應度函數，并利用改進算法和貝塞爾曲線平滑對路徑規劃迭代求解。結果表明，改進算法能夠更快地生成更短更平滑的機器人路徑，算法在搜索路徑性能上是具備優勢的。