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

丁 艷，畢 楊

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

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

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

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

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

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

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

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

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

其中

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中

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

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

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

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

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

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

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

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

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

其中

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

3 仿真測(cè)試分析

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

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

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

連桿的動(dòng)能Kk=0.0508 N·m；

勢(shì)能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)行多移動(dòng)機(jī)器人避障路徑規(guī)劃仿真，得到多移動(dòng)機(jī)器人避障路徑規(guī)劃的尋優(yōu)參數(shù)，尋優(yōu)效果如圖2 所示。

圖2 多移動(dòng)機(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 以上。這主要是因?yàn)樗岱椒▽?duì)多移動(dòng)機(jī)器人的路徑分布特征變量進(jìn)行分析，結(jié)合系統(tǒng)的空間增益梯度，得到多移動(dòng)機(jī)器人避障參數(shù)尋優(yōu)模型，提升了參數(shù)尋優(yōu)效果。

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

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

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

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

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

分析圖4 可見，本文方法進(jìn)行多移動(dòng)機(jī)器人避障路徑規(guī)劃的誤差較低， 且隨著迭代次數(shù)的增加，定位誤差率基本穩(wěn)定。對(duì)比其他方法，本文方法的機(jī)器人路徑規(guī)劃定位精度較高，能夠有效提高多移動(dòng)機(jī)器人避障能力。這主要是因?yàn)樗岱椒ú捎镁植慷ㄎ缓腿指欁R(shí)別方法對(duì)障礙物進(jìn)行定位，提高了定位精度，降低誤差。

4 結(jié)語

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