碰撞錐檢測改進的多智能體避障算法

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

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

1 引言

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

通常，復雜網絡可能包含大量節點，通過在網絡內的所有節點上添加控制器來同步復雜網絡既不實際也不經濟，解決該問題的有效方法是僅在一小部分網絡節點上添加控制器，這種方法稱為牽制控制。由于牽制控制的優越性，文獻[3-4]就多智能體牽制控制進行了深入的研究。

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

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

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

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

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

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

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

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

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

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

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

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

圖1 β-智能體示意圖

3 自適應快速避障控制算法

3.1 問題提出與算法設計

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

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

3.2 避障系數

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

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

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

其中，j=1,2。

圖3 單個障礙物環境中智能體的碰撞錐

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

避障系數gi(t)為：

3.3 算法設計和一致性證明

結合文獻[6]和避障系數gi(t)，定義ui(t)如下：

其中，(t)實現多智能體聚合、分離和速度對齊三個規則，(t)用于避障，(t)接收虛擬領導者信息，數學定義為：

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

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

圖4 避障控制算法流程圖

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

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

其中：

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

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

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

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

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

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

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

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

4 實驗結果與分析

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

4.1 圓形障礙物的避障實驗

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

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

圖5 一組多智能體自適應避障實驗

圖6 多智能體速度差異

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

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

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

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

4.2 墻形障礙物的避障實驗

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

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

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

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

5 結束語

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