碰撞錐檢測改進(jìn)的多智能體避障算法

許元云，何 明，劉錦濤，周 波，楊 鋮

中國人民解放軍陸軍工程大學(xué) 指揮控制工程學(xué)院，南京 210007

1 引言

動物集群經(jīng)常表現(xiàn)出有趣的集體行為[1]：它們在有限時間內(nèi)建立有序的結(jié)構(gòu)而不會發(fā)生碰撞，也可以對環(huán)境變化迅速做出反應(yīng)，例如突然出現(xiàn)捕食者或障礙物。這些生物行為非常復(fù)雜，相關(guān)研究人員通過引入多智能體，并用抽象的數(shù)學(xué)形式模擬這些動物行為。多智能體不光用于解釋這些行為，同時由于其擴(kuò)展性強、適應(yīng)性強、復(fù)雜性低等優(yōu)點，廣泛應(yīng)用于航天航空控制、交通控制和能源控制等領(lǐng)域[2]。

通常，復(fù)雜網(wǎng)絡(luò)可能包含大量節(jié)點，通過在網(wǎng)絡(luò)內(nèi)的所有節(jié)點上添加控制器來同步復(fù)雜網(wǎng)絡(luò)既不實際也不經(jīng)濟(jì)，解決該問題的有效方法是僅在一小部分網(wǎng)絡(luò)節(jié)點上添加控制器，這種方法稱為牽制控制。由于牽制控制的優(yōu)越性，文獻(xiàn)[3-4]就多智能體牽制控制進(jìn)行了深入的研究。

避障是多智能體能夠適應(yīng)復(fù)雜環(huán)境并順利完成任務(wù)的必要條件之一。目前多智能體避障算法包括人工勢力場法[5-6]、模糊控制法[7]、幾何法[8]和強化學(xué)習(xí)[9]等，人工勢場法在障礙物和智能體之間建立排斥力場來避免智能體與障礙物的碰撞，這種方法簡單靈活，通常比其他避障方法更高效[10]，最早由Khatib[5]引入到機器人避障中。Olfati-Saber 算法[6]中的β-智能體是一個典型的基于人工勢力場的多智能體避障算法，這個算法有兩個部分構(gòu)成，距離項部分是構(gòu)建排斥力場來避免智能體與障礙物之間的碰撞，速度項部分是引導(dǎo)智能體沿著障礙物切線方向運動。Sakai等人[11]不區(qū)分障礙物和智能體，將感知的對象全都視為障礙物，使用β-智能體完成了蜂擁控制和避障，劉嬋等人[12]介紹了Olfati-Saber 算法，并在多障礙物環(huán)境中進(jìn)行了實驗仿真。同其他人工勢力場法一樣，β-智能體也存在局部最小值問題和非最佳路徑問題，后續(xù)的改進(jìn)主要集中在這兩點。在避免陷入最小極值點方面，Li等人[13]提出了一種新穎的障礙物幾何表示規(guī)則以填充障礙物的非凸區(qū)域，高晶英[14]構(gòu)建了兩個β-智能體，其中一個與障礙物平面切線方向垂直，避免陷入最小值，Wu等人[15]將使用流體力場替代了智能體與障礙物之間的人工勢力場。為解決非最佳路徑問題，Hu等人[16]結(jié)合空間幾何知識，確定圓錐體的最短路徑為β-智能體軌跡從而實現(xiàn)圓錐體障礙物的快速避障，Wang 等人[17]結(jié)合幾何法的碰撞錐檢測方法[18-19]，根據(jù)α-智能體、β-智能體和γ-智能體三者的速度和位置關(guān)系確定單個智能體的速度大小和方向，使得智能體能夠快速通過障礙物，但注意到此方法未涉及智能體之間的避免碰撞問題，Zhao等人[20]通過智能體與其鄰接節(jié)點交互障礙物信息，將局部避障算法改進(jìn)為全局避障算法，有效地避免了局部最小值問題，同時更好地實現(xiàn)了多智能體的一致性，但算法復(fù)雜度有所增加。

本文基于Olfati-Saber 算法提出了一種多智能體自適應(yīng)避障控制算法，算法基于角度比較的碰撞錐檢測方法引出智能體的避障系數(shù)，并結(jié)合牽制控制輸入完成多智能體的快速避障。證明了在本算法作用下所有智能體最終會避開障礙，避免碰撞并最終達(dá)到一致。仿真實驗表明在圓形障礙物和墻形障礙物環(huán)境中基于本算法多智能體避障所用時間較少，驗證了本算法的有效性。

2 多智能體建模和障礙物表示

考慮m個智能體工作在n維歐幾里德空間上，它們的動力學(xué)方程可以寫為：

其中，qi,pi,ui∈?n表示多智能體i的位置向量、速度向量和加速度向量。

t時刻多智能體構(gòu)成的無向圖G(t)由節(jié)點和邊組成，其中節(jié)點集合表示為V={1 ,2,…,m} ，邊的集合表示為設(shè)r是α-智能體的感知半徑，期望距離為dα，則多智能體的鄰域為：

參照文獻(xiàn)[6]，定義β-智能體構(gòu)成的集合為V′={1 ′,2′,…,m′} ，則β-智能體鄰域為：

其中，r′是α-智能體感知β-智能體的距離，期望距離為dβ，這里假設(shè)r=r′。

圓形障礙物的β-智能體如圖1（a）所示，其數(shù)學(xué)定義為：

其中，yk,Rk分別是障礙物的圓心位置和半徑；μ=。

墻形障礙物的β-智能體如圖1（b）所示，其數(shù)學(xué)定義為：

其中，yk是一個固定點，這里假設(shè)為墻形障礙物線段的中心，ak是墻形障礙物的單位法向量。

圖1 β-智能體示意圖

3 自適應(yīng)快速避障控制算法

3.1 問題提出與算法設(shè)計

一個基于Olfati-Saber算法的避障實例如圖2所示，可以看出當(dāng)整個網(wǎng)絡(luò)經(jīng)過半圓后時在β-智能體的速度項作用下仍然繞著圓運動，這增加了智能體繞過障礙物的時間，降低了多智能體的避障效率。為避免這類現(xiàn)象的發(fā)生，提出一種自適應(yīng)避障系數(shù)gi(t),gi(t)在智能體遇到障礙物以后調(diào)節(jié)智能體是否受到β-智能體速度項的影響，再結(jié)合牽制控制輸入完成多智能體的避障。

圖2 一個多智能體避障實例

3.2 避障系數(shù)

避障系數(shù)gi(t)的取值根據(jù)碰撞錐檢測方法來確定，本文通過角度比較來完成碰撞錐檢測。當(dāng)智能體的速度和碰撞錐相交時，首要任務(wù)是避開障礙物，需要β-智能體速度項的引導(dǎo)；當(dāng)智能體運動到速度方向和碰撞錐不相交時，需要考慮多智能體的一致性問題，而β-智能體速度項與智能體的平均速度、虛擬領(lǐng)導(dǎo)者速度都無關(guān)，此時可以通過調(diào)節(jié)gi(t)排除β-智能體速度項的影響。

圓形障礙物環(huán)境中的碰撞錐如圖3（a）紅色區(qū)域所示，由對稱性θ角可表示為：

當(dāng)智能體感知到墻形障礙物時，由于墻形障礙物邊緣的存在，墻形障礙物環(huán)境中的碰撞錐是可能是三角和長方形、兩個三角形或者兩個長方體的組合，這取決于智能體i能否感知到障礙物邊緣。如圖3（b）所示，設(shè)點O1和O2是墻型障礙物的兩個端點，則θ角可表示為：

其中，j=1,2。

圖3 單個障礙物環(huán)境中智能體的碰撞錐

智能體速度pi和智能體指向圓心的向量的夾角φ滿足：

避障系數(shù)gi(t)為：

3.3 算法設(shè)計和一致性證明

結(jié)合文獻(xiàn)[6]和避障系數(shù)gi(t)，定義ui(t)如下：

其中，(t)實現(xiàn)多智能體聚合、分離和速度對齊三個規(guī)則，(t)用于避障，(t)接收虛擬領(lǐng)導(dǎo)者信息，數(shù)學(xué)定義為：

其中，qγ(t),pγ(t)∈?n代表虛擬領(lǐng)導(dǎo)者的位置向量和速度向量，這里虛擬領(lǐng)導(dǎo)者為常速虛擬領(lǐng)導(dǎo)者；gi(t)只作用于(t)的速度項，(t)的位置項用于保持障礙物與智能體的距離；hi(t)值的選取采用度中心性選擇，具體選擇方法同文獻(xiàn)[4]；勢能方程ψα、ψβ和速度交互矩陣{aij}、{ai,k} 的定義同文獻(xiàn)[6]。

整個避障控制算法流程如圖4 所示，其中Δt是時間步長，一般較小，T是算法終止時間，一般根據(jù)終止條件來確定合適的值。

圖4 避障控制算法流程圖

定理1多智能體運動方程如公式（1）所示，控制輸入如公式（10）所示，初始能量Q0是一個有限值，則在圓形障礙物和墻形障礙物中所有智能體最終會避開障礙，避免碰撞并趨于一致。

證明多智能體的總能量由總勢能和總動能構(gòu)成，設(shè)q?i(t)=qi(t)-qγ(t),p?i(t)=pi(t)-pγ,q?ij(t)=qi(t)-qj(t),根據(jù)公式（1）、（10），總能量Q(t)為：

其中：

Q(t)對時間t求導(dǎo)得到：

代入公式（10）～（13），化簡得到：

初始能量Q0是一個有限值，由公式（14）、（15）可知，均是有限量，由此可以推得也是有限量。根據(jù)LaSalle 定理，多智能體最終會趨于的一致性狀態(tài)。

由公式（17）可得Q˙(t)=0 的充分必要條件為：

即對于所有的智能體都有：

由于圓形障礙物的β-智能體速度一直變化，墻形障礙物β-智能體方向與γ-智能體方向不同（如果相同，則智能體不會碰到障礙物），Q˙(t)=0 成立的充分必要條件是gi(t)≡0，即所有的智能體都不在障礙物環(huán)境的危險區(qū)域中。

由pj(t)=pi(t)，則qj(t)-qi(t)為一個恒定的值，由公式（20）可知qi,k(t)-qi(t)也為一個恒定的值，可以得到pi,k(t)=pi(t)，從上文可知，β-智能體沿著障礙物表面移動，速度不會始終與虛擬領(lǐng)導(dǎo)者的速度保持一致，故所有智能體不會感知到障礙物，即最終避開了障礙物。

初始能量Q0是一個有限值，由文獻(xiàn)[3]，多智能體之間不會發(fā)生碰撞。

4 實驗結(jié)果與分析

本實驗研究多智能體在常速虛擬領(lǐng)導(dǎo)者的作用下避開圓形障礙物和墻型障礙物。

4.1 圓形障礙物的避障實驗

多智能體由區(qū)域區(qū)間為[0 ,50]×[0 , 50]、速度區(qū)間為[-1,1]×[-1,1]隨機生成的10個智能體構(gòu)成，通信距離r=r′=6，理想距離dα=dβ=5。

一組多智能體如圖5（a）所示，其中藍(lán)點和紅色箭頭分別表示智能體位置和速度，黑圈表示該節(jié)點為牽制節(jié)點，紅色五角星表示虛擬領(lǐng)導(dǎo)者，位置向量為(2 5,25 )，速度向量為(0.5,0.5 )，圓形障礙物的半徑為20，圓心位于(1 00,100 )，采樣時間 Δt=0.1 s。圖5（b）為多智能體在[1 0 0 s,350 s]時間段內(nèi)的運動軌跡，可以看出多智能體最終成功避開圓形障礙物，并且在繞過了半圓后沒有像圖1一樣繞著圓周運動，而是朝著虛擬領(lǐng)導(dǎo)者的方向運動。

圖5 一組多智能體自適應(yīng)避障實驗

圖6 多智能體速度差異

圖7是避障過程中智能體與障礙物的距離差異，其中未遇到障礙物時設(shè)置距離為6。從圖7 可以看出，多智能體在避障過程中只有5個智能體感知到障礙物，遇到障礙物時這些智能體在輸入控制的作用下避免了與障礙物的碰撞，同時其他智能體通過智能體之間的相互作用繞開了障礙物。

圖7 多智能體與障礙物的距離差異

隨機選取20組智能體開展避障實驗。設(shè)多智能體避障所用時間為多智能體遇到障礙物到所有智能體離開障礙物的時間間隔。圖8 是Olfati-Saber 算法和本文所提算法避障所用時間的比較，可以看出在圓形障礙物中本文所提算法避障所用時間較少，驗證了算法的有效性。

圖8 圓形障礙物中兩種算法避障所用時間的比較

4.2 墻形障礙物的避障實驗

多智能體和虛擬領(lǐng)導(dǎo)者的設(shè)置同3.1 節(jié)，墻型障礙物的中點為(1 00,100 )，長度為20，法向量為(3,2)（未與虛擬領(lǐng)導(dǎo)者的速度平行是避免多智能體陷入最小值）。圖9 為一組多智能體在時間段[1 0 0 s,235 s]的軌跡，可以看出圖9（a）采用Olfati-Saber算法部分智能體在235 s未通過障礙物，而圖9（b）采用本文所提算法所有智能體都通過了障礙物。

圖9 墻形障礙物中兩種算法下多智能體的運動軌跡

隨機選取20組多智能體開展避障實驗。從圖10可以在看出在墻形障礙物中本文所提算法避障所用時間較少，驗證了算法的有效性。

圖10 圓形障礙物中兩種算法避障所用時間的比較

5 結(jié)束語

本文提出了一種多智能體避障控制算法，本算法由避障系數(shù)和牽制控制輸入構(gòu)成，能夠使多智能體避開障礙，避免碰撞并最終達(dá)到一致。通過在圓形和墻形障礙物中的多智能體避障實驗驗證了該算法能夠快速避障。下一步將主要就非光滑障礙物、多個障礙物和運動障礙物的避障問題以及避障算法的局部最小值問題進(jìn)行深入研究。