基于蟻群算法的多移動機(jī)器人避障路徑規(guī)劃方法

丁 艷，畢 楊

（1.漢中職業(yè)技術(shù)學(xué)院 機(jī)電工程系，漢中723000；2.西安航空學(xué)院 電子工程學(xué)院，西安710077）

隨著工業(yè)機(jī)器人的發(fā)展，機(jī)器人的種類越來越廣泛，應(yīng)用范圍也越來越廣[1]。在機(jī)器人的智能控制設(shè)計中，相關(guān)的多移動機(jī)器人避障和路徑規(guī)劃方法研究受到人們的極大關(guān)注[2]。

傳統(tǒng)對多移動機(jī)器人避障路徑規(guī)劃方法主要有粒子群尋優(yōu)法、誤差反饋校正法、末端位置參數(shù)調(diào)節(jié)法等[3-4]。文獻(xiàn)[5]提出未知環(huán)境下機(jī)器人避障及動態(tài)目標(biāo)追蹤。在引力公式中加入速度差和加速度差，提高了機(jī)器人動態(tài)目標(biāo)追蹤的靈活性和對環(huán)境的適應(yīng)能力，但其耗時較長。文獻(xiàn)[6]提出基于混合策略的移動機(jī)器人避障算法探究，運(yùn)用矢量場直方圖法結(jié)合神經(jīng)網(wǎng)絡(luò)算法實現(xiàn)基礎(chǔ)避障，但該方法需要大量樣本訓(xùn)練集，自適應(yīng)能力不強(qiáng)。

故在此提出基于蟻群算法的多移動機(jī)器人避障路徑規(guī)劃方法。該方法結(jié)合小擾動解析方法進(jìn)行多移動機(jī)器人避障網(wǎng)格區(qū)域化匹配，建立尋優(yōu)約束參數(shù)， 通過自適應(yīng)蒙特卡洛定位法對路徑定位，采用蟻群算法尋優(yōu)控制。通過仿真測試，驗證其在提高多移動機(jī)器人避障路徑規(guī)劃能力方面的優(yōu)越性能。

1 多移動機(jī)器人避障規(guī)劃運(yùn)動學(xué)模型及參數(shù)尋優(yōu)

1.1 多移動機(jī)器人避障規(guī)劃的運(yùn)動學(xué)模型

為實行多移動機(jī)器人避障規(guī)劃的運(yùn)動學(xué)規(guī)劃設(shè)計， 構(gòu)建了多移動機(jī)器人避障規(guī)劃控制流程，如圖1 所示。

圖1 多移動機(jī)器人避障規(guī)劃控制流程Fig.1 Control flow chart of obstacle avoidance planning for multiple mobile robots

在圖1 控制流程的基礎(chǔ)上，結(jié)合小擾動解析方法，進(jìn)行多移動機(jī)器人路徑規(guī)劃的運(yùn)動學(xué)模型構(gòu)建[5]。首先，進(jìn)行多移動機(jī)器人避障規(guī)劃的姿態(tài)信息融合，結(jié)合誤差反饋跟蹤調(diào)節(jié)的方法，得到多移動機(jī)器人避障路徑規(guī)劃的傳遞函數(shù)為

其中

式中：w（k）為多移動機(jī)器人繞體坐標(biāo)系各軸的慣性協(xié)方差矩陣；若w（k）∈L2（0，∞），A，B，C，D 為多移動機(jī)器人質(zhì)心分布矩陣參數(shù)；n 為控制約束的自變量維數(shù)，為正整數(shù)；ΔA1，ΔB1為多移動機(jī)器人避障穩(wěn)態(tài)誤差增益。

引入多移動機(jī)器人的誤差擾動項，結(jié)合姿態(tài)陀螺儀、加速度計對多移動機(jī)器人路徑和姿態(tài)采樣結(jié)果，構(gòu)建多移動機(jī)器人避障規(guī)劃的控制器。其表達(dá)為

以步態(tài)運(yùn)動力學(xué)和阻尼參量為自鎮(zhèn)定性調(diào)節(jié)參數(shù)，進(jìn)行線性規(guī)劃設(shè)計和自適應(yīng)處理，得到參數(shù)融合和信息處理函數(shù)[6]。其表達(dá)為

式中：i 為多移動機(jī)器人的模糊度參數(shù)；u（x）為多移動機(jī)器人路徑空間的測量矩陣；z（x）為多移動機(jī)器人路徑分布空間的末端位姿陣列矩陣；n 為適當(dāng)矩陣維數(shù)。

結(jié)合參數(shù)融合和信息處理結(jié)果，構(gòu)建多移動機(jī)器人的運(yùn)動學(xué)模型，其表達(dá)為

式中：m 為多移動機(jī)器人路徑規(guī)劃的約束狀態(tài)參數(shù)變量。

通過上述分析，得到多移動機(jī)器人的運(yùn)動學(xué)模型結(jié)果，采用地圖尋優(yōu)方法進(jìn)行多移動機(jī)器人避障過程中的網(wǎng)格區(qū)域化匹配，結(jié)合SLAM（simultaneous localization and mapping）算法，進(jìn)行多移動機(jī)器人避障參數(shù)尋優(yōu)[7]。

1.2 多移動機(jī)器人避障參數(shù)尋優(yōu)

通過構(gòu)建多移動機(jī)器人的運(yùn)動學(xué)模型，進(jìn)行機(jī)器人避障過程中的網(wǎng)格區(qū)域化匹配，得到多移動機(jī)器人的步態(tài)參數(shù)跟蹤模型[8]。其表達(dá)為

式中：（A+B）為運(yùn)動學(xué)方程的自變量分布矩陣；F，G分別為多個蟻群濾波尋優(yōu)參數(shù)和系數(shù)；t 為時間。

基于運(yùn)動學(xué)特征分析方法進(jìn)行多移動機(jī)器人避障過程中的路徑分布特征變量分析，建立多移動機(jī)器人避障路徑分布的尋優(yōu)約束參數(shù)， 采用擴(kuò)展SLAM 算法進(jìn)行多移動機(jī)器人質(zhì)心加速控制[9]，結(jié)合參數(shù)融合，得到避障路徑分布的尋優(yōu)約束參數(shù)模型。其表達(dá)為

式中：Ф1，Ф2為多種機(jī)器人的位置標(biāo)定。

基于運(yùn)動學(xué)特征分析方法進(jìn)行多移動機(jī)器人避障過程中的路徑分布特征變量分析，通過局部定位，構(gòu)造多移動機(jī)器人步進(jìn)跟蹤穩(wěn)態(tài)參數(shù)學(xué)習(xí)模型。其表達(dá)為

式中：ea為多移動機(jī)器人路徑規(guī)劃的狀態(tài)特征分布量化參數(shù)，且為正整數(shù)；q 為跟蹤修正系數(shù)。

分析可知，多移動機(jī)器人的路徑規(guī)劃參數(shù)模型分為確定和不確定性兩部分，采用時延估計進(jìn)行誤差修正[10]，得到采樣特征分布項L2。在多移動機(jī)器人質(zhì)心規(guī)劃空間內(nèi)，引入多移動機(jī)器人路徑的反演規(guī)劃方法，得到多移動機(jī)器人路徑的穩(wěn)態(tài)控制參數(shù)變量為

式中：P，R 為多移動機(jī)器人路徑分布的實對稱矩陣；θi為位姿系數(shù)；α 為反演規(guī)劃參數(shù)。

根據(jù)多移動機(jī)器人測量參數(shù)，結(jié)合系統(tǒng)的空間增益梯度， 得到多移動機(jī)器人避障參數(shù)尋優(yōu)模型。其描述為

式中：lj（k）為多移動機(jī)器人路徑規(guī)劃的目標(biāo)狀態(tài)；li（k）為多移動機(jī)器人路徑規(guī)劃的參數(shù)信息融合矩陣；i，j 為機(jī)器人的位姿信息；Γ（k）為多移動機(jī)器人路徑尋優(yōu)參數(shù)。

通過分析，實現(xiàn)了多移動機(jī)器人避障參數(shù)尋優(yōu)，以多移動機(jī)器人的質(zhì)心參數(shù)為約束對象，得到多移動機(jī)器人路徑空間規(guī)劃模型， 結(jié)合避障參數(shù)尋優(yōu)，進(jìn)行避障路徑規(guī)劃[11]。

2 多移動機(jī)器人避障路徑優(yōu)化

2.1 多移動機(jī)器人避障路徑定位優(yōu)化

通過局部定位和全局跟蹤識別方法進(jìn)行多移動機(jī)器人避障路徑規(guī)劃的障礙物定位優(yōu)化設(shè)計。采用快速擴(kuò)展隨機(jī)樹RRT（rapidly-exploring random tree）運(yùn)動規(guī)劃模型，結(jié)合自適應(yīng)蒙特卡洛定位的方法[13]，進(jìn)行多移動機(jī)器人避障路徑過程中障礙物定位優(yōu)化。其描述為

其中

式中：θ 為多移動機(jī)器人避障目標(biāo)位形矢量傾角；α 為機(jī)器人避障路徑空間分布縱軸（x 軸）與水平面（Oxy 平面）之間的夾角；f（x），f（t）分別為機(jī)器人避障路徑分布的橫向、縱向偏移量；Mz為空間坐標(biāo)系Oxy 上的投影角；z 軸垂直于多移動機(jī)器人避障分布狀態(tài)空間坐標(biāo)系Oxy；m 為移動機(jī)器人的質(zhì)量。

綜上分析，通過自適應(yīng)蒙特卡洛定位的方法，進(jìn)行多移動機(jī)器人避障路徑規(guī)劃過程中的障礙位置準(zhǔn)確定位[14]。

2.2 多移動機(jī)器人避障蟻群尋優(yōu)算法

在實現(xiàn)障礙物精準(zhǔn)定位的基礎(chǔ)上，采用蟻群尋優(yōu)算法進(jìn)行多移動機(jī)器人路徑空間規(guī)劃，假設(shè)蟻群的個數(shù)為N，得到多移動機(jī)器人避障路徑規(guī)劃的網(wǎng)格數(shù)為m，每個螞蟻的信息熵濃度指標(biāo)函數(shù)為

式中：eq為第q 個蟻群尋優(yōu)算子的最佳路徑空間匹配樣本集；ekq為蟻群尋優(yōu)的模板參數(shù)。

通過多移動機(jī)器人避障控制，其中最佳路徑空間匹配樣本集的第k 個步態(tài)跟蹤融合狀態(tài)分量為τk，由于時延參數(shù)是確定的，滿足τk=τx+τy+τz，其中τx，τy，τz分別為步態(tài)在x，y，z 軸上的分量。采用反演控制，得到多移動機(jī)器人避障的蟻群尋優(yōu)修正向量為

根據(jù)非線性參考模板匹配方法，利用蟻群尋優(yōu)控制的參量量化分布雅可比矩陣，得到蟻群尋優(yōu)的自適應(yīng)迭代步長分布算子為

式中：?為機(jī)器人的阻尼系數(shù)。在多移動機(jī)器人避障姿態(tài)校正過程中，受到小擾動力矩因素的影響[12]，得到多移動機(jī)器人避障路徑的蟻群尋優(yōu)路徑為

式中：α 為機(jī)器人避障路徑夾角。根據(jù)多移動機(jī)器人步進(jìn)跟蹤的穩(wěn)態(tài)擾動與加速度測量矩陣的關(guān)聯(lián)性，得到多移動機(jī)器人避障蟻群尋優(yōu)算法方程式。即

其中

式中：x（0）為多移動機(jī)器人避障的初始狀態(tài)均值；P0為其方差。當(dāng)多移動機(jī)器人在避障過程中處于非加速運(yùn)動狀態(tài)時，通過實現(xiàn)多移動機(jī)器人避障的蟻群尋優(yōu)算法，實現(xiàn)路徑規(guī)劃優(yōu)化設(shè)計[15]。

3 仿真測試分析

為驗證所提方法的有效性，進(jìn)行了仿真試驗。設(shè)定機(jī)器人路徑規(guī)劃的動態(tài)補(bǔ)償系數(shù)為0.34，對機(jī)器人路徑參數(shù)采樣的傳感器節(jié)點數(shù)設(shè)定為200，姿態(tài)參數(shù)的測量誤差分別設(shè)定為0.67 mm，0.89 mm 和1.26 mm。多移動機(jī)器人路徑分布的初始位置狀態(tài)為

飽和非線性狀態(tài)參數(shù)控制特征量為λ1=1.21，λ2=1.34，c1=2.11，c2=2.25；大幅度的振動強(qiáng)度為Q（k）=0.45。

多移動機(jī)器人避障路徑規(guī)劃的蟻群尋優(yōu)參數(shù)設(shè)定如下：

連桿的動能Kk=0.0508 N·m；

勢能Kp=0.5732 N·m；

閉環(huán)系統(tǒng)的平衡漸變參數(shù)為

穩(wěn)態(tài)增益控制矩陣為

控制矢量參數(shù)Jp=0.804（1±0.5）kg·m2。

根據(jù)上述仿真環(huán)境和參數(shù)設(shè)定，進(jìn)行多移動機(jī)器人避障路徑規(guī)劃仿真，得到多移動機(jī)器人避障路徑規(guī)劃的尋優(yōu)參數(shù)，尋優(yōu)效果如圖2 所示。

圖2 多移動機(jī)器人避障路徑規(guī)劃參數(shù)尋優(yōu)效果Fig.2 Optimization effect of obstacle avoidance path planning parameters for multiple mobile robots

由圖可見，隨著采樣次數(shù)多增加，參數(shù)尋優(yōu)效果逐漸上升，且一直保持在0.7 以上。這主要是因為所提方法對多移動機(jī)器人的路徑分布特征變量進(jìn)行分析，結(jié)合系統(tǒng)的空間增益梯度，得到多移動機(jī)器人避障參數(shù)尋優(yōu)模型，提升了參數(shù)尋優(yōu)效果。

根據(jù)圖2 的參數(shù)解析結(jié)果，進(jìn)行了避障路徑規(guī)劃，得到優(yōu)化的規(guī)劃路徑如圖3 所示。

圖3 多移動機(jī)器人避障路徑規(guī)劃結(jié)果Fig.3 Obstacle avoidance path planning results of multiple mobile robots

分析圖3 可見，本文方法能夠有效地實現(xiàn)多移動機(jī)器人避障路徑規(guī)劃， 路徑分布閾值穩(wěn)定在3～5之間，變化幅度較小，相鄰單位時間內(nèi)最大路徑分布閾值差僅為1.2，路徑尋優(yōu)能力較好。這主要是因為在實現(xiàn)參數(shù)尋優(yōu)的基礎(chǔ)上，采用了蟻群尋優(yōu)算法進(jìn)行多移動機(jī)器人路徑空間規(guī)劃，實現(xiàn)了避障路徑最優(yōu)化。

為測試路徑規(guī)劃的誤差，分別采用文獻(xiàn)[4]和文獻(xiàn)[5]所提方法進(jìn)行對比，得到的結(jié)果如圖4 所示。

圖4 多移動機(jī)器人避障定位誤差對比Fig.4 Error comparison of obstacle avoidance and positioning for multiple mobile robots

分析圖4 可見，本文方法進(jìn)行多移動機(jī)器人避障路徑規(guī)劃的誤差較低， 且隨著迭代次數(shù)的增加，定位誤差率基本穩(wěn)定。對比其他方法，本文方法的機(jī)器人路徑規(guī)劃定位精度較高，能夠有效提高多移動機(jī)器人避障能力。這主要是因為所提方法采用局部定位和全局跟蹤識別方法對障礙物進(jìn)行定位，提高了定位精度，降低誤差。

4 結(jié)語

本文提出基于蟻群算法的多移動機(jī)器人避障路徑規(guī)劃方法。結(jié)合小擾動解析方法，構(gòu)建多移動機(jī)器人避障規(guī)劃運(yùn)動學(xué)模型；采用擴(kuò)展濾波方法進(jìn)行多移動機(jī)器人的步態(tài)參數(shù)跟蹤；通過自適應(yīng)蒙特卡洛定位的方法，進(jìn)行多移動機(jī)器人避障路徑障礙物精準(zhǔn)定位；采用蟻群尋優(yōu)算法得到蟻群尋優(yōu)控制的參量量化分布矩陣，進(jìn)行多移動機(jī)器人路徑空間規(guī)劃。分析得知，本文方法進(jìn)行多移動機(jī)器人避障的路徑規(guī)劃能力較好，避障定位誤差較低。對于多移動機(jī)器人避障路徑規(guī)劃具有一定參考價值。