基于動態勢場法的最優一致性避障算法研究

孫景亮，劉春生，史浩明

(南京航空航天大學 自動化學院，江蘇 南京210016)

0 引言

避障問題是研究多智能體飛行器協同控制不可忽視的關鍵問題，這將直接影響飛行器能否順利完成既定任務以及適應復雜環境的能力。近年來，針對飛行器協同一致性以及避障的研究不勝枚舉，其主要應用工具就是圖論。它將多飛行器系統看作網絡拓撲［1］，這對多智能體系統的研究具有指導意義。同時，考慮到續航能力、經濟性、實時性等重要指標的要求，最優一致性算法得到廣泛發展［2-3］。針對復雜環境下的最優避障，其方法主要是兩類:一是分別設計最優一致性控制器和避障控制器［4］，該方法應用廣泛且簡單實用，但并不能保證系統的全局最優，即飛行器的避障過程不是最優化的;二是將避障函數直接設計在優化性能指標函數中［5］，從而保證了飛行器的全局最優。這兩種方法均未過多考慮傳統人工勢能場的缺點，即飛行器易陷入局部極值。文獻［6］采用選擇速度避障的方法，將飛行器的速度信息用于避障，有效降低了陷入局部極值的可能性，但該方法只針對特定的三種模式下的避障，因而不具有普遍性。

傳統人工勢能場避障函數是利用對飛行器的排斥力迫使其遠離障礙物，但當飛行器處于勢能場向量的相反方向時，勢能場的排斥力很容易抵消飛行器的速度，從而使飛行器陷入局部極值或瞬間失速的狀態，這對于飛行器而言，無疑是致命弱點。

針對以上分析，本文提出基于動態勢場法的最優一致性避障算法，使勢能場向量根據飛行器距離障礙物的位置信息和速度信息而變化，這樣勢能場向量和飛行器速度向量的合力，迫使飛行器平滑繞過障礙物，既解決了飛行器陷入局部極值的問題，又能夠避免接近障礙物時引起的振蕩，同時可以保證系統的全局最優。

1 圖論

多智能體之間的通訊拓撲結構可以用一個圖G=(V，E)來表示。其中:V表示圖G的有限非空頂點集合V={1，2，…，n};E表示圖的有限邊集合，E={(i，j)|i，j∈V，i≠j}，(i，j)表示智能體 i和 j之間的通訊，i=1，2，…，n。在圖 G 中，若(i，j)∈E≠(j，i)∈E，則稱該圖為有向圖，否則為無向圖。

本文假設智能體之間的網絡拓撲結構為無向圖，假設存在5個智能體，其網絡拓撲結構如圖1所示。

圖1 智能體通信拓撲結構Fig.1 Communication topology of agents

圖G的拉普拉斯矩陣L=D(G)－A(G)。其中:度矩陣D(G)=diag(d(i))，d(i)為圖G中與頂點 i相關聯邊的數目;鄰接矩陣 A(G)=［aij］，aij為連接頂點i與頂點j的邊數。拉普拉斯矩陣L存在如下性質:0是矩陣L的一個特征值，向量1是特征值0所對應的特征向量［7］。

2 問題描述

用一組單積分器動態系統來描述多智能體系統，其狀態空間表達式為:

其中:

式中:xi∈ Rm，ui∈ Rm(i=1，2，…，n)分別表示智能體i的位置和控制輸入;?表示矩陣的Kronecker積;0n表示n維零矩陣;Im，In分別表示m維和n維單位矩陣。

定義誤差動態方程為:

式中:Xf∈Rm表示飛行器終端狀態;Uf為終端控制輸入。由圖論拉普拉斯矩陣性質可得0nm×1，即終端狀態Xf為常數，終端控制輸入Uf為零。

3 避障函數

假設障礙物可表述為矩形(x0±v1，y0±v2)，其中(x0，y0)為矩形中心，v1，v2∈R+。則圍繞該矩形障礙物所形成的勢能場向量可表述為:

其中:

引理［8］:在橢圓簇上任意一點(x，y)沿著橢圓軌跡順時針旋轉的軌跡方程為:

如圖2所示，飛行器與障礙物中心連線和水平方向的夾角為χi=atan 2(y0－yi，x0－xi)，函數atan 2計算兩點形成的斜率角度，取值范圍atan 2(y，x)∈［－ π，π］。

圖2 動態勢能場避障示意圖Fig.2 Obstacle avoidance in dynamic potential field

假設旋轉勢能場向量可以表示為:

則由旋轉軌跡方程式(4)得:

式中:(vxc，vyc)，(vxcc，vycc)分別表示勢能場向量順時針旋轉向量和逆時針旋轉向量。

為保證勢能場向量隨著飛行器與障礙物距離的減小而增大，對勢能場向量進行如下改造:

其中:

因此障礙物周圍勢能場向量可以表示為:

4 控制器設計

最優一致性避障問題可表述為以下最小化性能指標:

假設最優代價函數

則Hamilton函數可表示為:

矩陣P滿足Riccati方程:

將矩陣 A，B，R，Q 帶入式(13)，解得:

所以最優控制

令 ?H/?U=0，可得:

由式(16)可知，總可以找到一個足夠小的權重矩陣M和適當的權重系數λ，使得避障勢能函數

5 穩定性分析

由文獻［9］關于非二次型性能指標的最優控制問題證明可以得出:非線性系統Hamilton-Jacobi-Bellman(HJB)方程的穩態解是一個李雅普諾夫函數，這樣不僅保證了系統的穩定性，而且滿足最優條件。因此，選取李雅普諾夫函數證明系統的穩定性:

6 仿真驗證

多智能體系統拓撲結構如圖1所示。該拓撲結構是無向連通圖，則拉普拉斯矩陣可以定義為:

假設5個智能體初始位置分別為(5，－6)，(8，8)，(－6，－3)，(－2，－6)和(－2，4)，權值矩陣 M=0.85In，λ=5.24。本文分別對智能體群在無障礙物、存在單個障礙物和多個障礙物的情況下進行仿真驗證。

智能體在無障礙物情況下飛行，飛行軌跡和控制輸入如圖3～圖5所示。

圖3 無障礙物一致性飛行軌跡Fig.3 Consensus trajectories of five agents without obstacles

圖4 x軸控制輸入Fig.4 Control input on x-axis

圖5 y軸控制輸入Fig.5 Control input on y-axis

由飛行軌跡圖和控制輸入可以得出，飛行器最終匯聚于某一點，設計的最優控制律使系統達到一致性要求，控制輸入逐漸減少為零。

假設飛行軌跡上存在一個障礙物，位置坐標為(4±1，4±0.5)，軌跡如圖6所示。

圖6 單個障礙物避障軌跡Fig.6 Trajectories of agents with a single obstacle

從仿真圖可以驗證動態勢能場避障函數的有效性，能夠使飛行器順利躲避障礙物，達到系統整體的一致性要求。

假設系統飛行軌跡上存在3個障礙物，位置坐標分別為(3±1，3±0.5)，(－3±1，－2±0.5)，(6±0.5，6±1)，飛行軌跡如圖7所示。

圖7 多個障礙物避障軌跡Fig.7 Trajectories of agents with multiple obstacles

仿真結果表明，在存在多個障礙物的復雜環境中，飛行器依然能夠有效躲避障礙物，實現一致性要求，達到期望的目標。

7 結束語

本文對多智能體飛行器的最優一致性避障問題進行了研究，仿真結果表明所設計的算法能夠有效躲避障礙物，實現一致性。

本課題待解決的問題還有:多個距離較近障礙物的情況，這種情況下動態勢能場會相互影響;飛行器存在通信時滯問題，這必將降低系統的一致性，同時影響飛行器的避障過程。

［1］ Olfati-Saber R，Murray R M.Consensus problems in networks of agents with switching topology and time-delays［J］.Automatic Control，IEEE Transactions on，2004，49(9):1520-1533.

［2］ Semsar-Kazerooni E，Khorasani K.An LMI approach to optimal consensus seeking in multi-agent systems［C］//American Control Conference，ACC'09.St.louis，Mo，USA:IEEE，2009:4519-4524.

［3］ Cao Y，Ren W.Optimal linear-consensus algorithms:an LQR perspective［J］.Systems，Man，and Cybernetics，Part B:Cybernetics，IEEE Transactions on，2010，40(3):819-830.

［5］ Wang J，Xin M.Optimal consensus algorithm integrated with obstacle avoidance［J］.International Journal of Systems Science，2013，44(1):166-177.

［6］ Jenie Y I，Van Kampen E J，de Visser CC，et al.Selective velocity obstacle method for cooperative autonomous collision avoidance system for UAVs［C］//Guidance，Navigation，and Control(GNC)Conference.Boston，MA:AIAA，2013:4627-4646.

［7］ 潘歡.二階多智能體一致性算法研究［D］.長沙:中南大學，2012.

［8］ Sabir D，Wu Q.Cooperative tracking control and obstacles avoidance for single-integrator dynamics［C］//Control Conference(CCC)，2013 32nd Chinese.IEEE，2013:7162-7167.

［9］ Bernstein D S.Non-quadratic cost and nonlinear feedback control［J］.International Journal of Robust and Nonlinear Control，1993，3(3):211-229.