基于動態(tài)勢場法的最優(yōu)一致性避障算法研究

孫景亮，劉春生，史浩明

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

0 引言

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

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

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

1 圖論

多智能體之間的通訊拓?fù)浣Y(jié)構(gòu)可以用一個圖G=(V，E)來表示。其中:V表示圖G的有限非空頂點(diǎn)集合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，則稱該圖為有向圖，否則為無向圖。

本文假設(shè)智能體之間的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)為無向圖，假設(shè)存在5個智能體，其網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)如圖1所示。

圖1 智能體通信拓?fù)浣Y(jié)構(gòu)Fig.1 Communication topology of agents

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

2 問題描述

用一組單積分器動態(tài)系統(tǒng)來描述多智能體系統(tǒng)，其狀態(tài)空間表達(dá)式為:

其中:

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

定義誤差動態(tài)方程為:

式中:Xf∈Rm表示飛行器終端狀態(tài);Uf為終端控制輸入。由圖論拉普拉斯矩陣性質(zhì)可得0nm×1，即終端狀態(tài)Xf為常數(shù)，終端控制輸入U(xiǎn)f為零。

3 避障函數(shù)

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

其中:

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

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

圖2 動態(tài)勢能場避障示意圖Fig.2 Obstacle avoidance in dynamic potential field

假設(shè)旋轉(zhuǎn)勢能場向量可以表示為:

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

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

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

其中:

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

4 控制器設(shè)計(jì)

最優(yōu)一致性避障問題可表述為以下最小化性能指標(biāo):

假設(shè)最優(yōu)代價函數(shù)

則Hamilton函數(shù)可表示為:

矩陣P滿足Riccati方程:

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

所以最優(yōu)控制

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

由式(16)可知，總可以找到一個足夠小的權(quán)重矩陣M和適當(dāng)?shù)臋?quán)重系數(shù)λ，使得避障勢能函數(shù)

5 穩(wěn)定性分析

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

6 仿真驗(yàn)證

多智能體系統(tǒng)拓?fù)浣Y(jié)構(gòu)如圖1所示。該拓?fù)浣Y(jié)構(gòu)是無向連通圖，則拉普拉斯矩陣可以定義為:

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

智能體在無障礙物情況下飛行，飛行軌跡和控制輸入如圖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

由飛行軌跡圖和控制輸入可以得出，飛行器最終匯聚于某一點(diǎn)，設(shè)計(jì)的最優(yōu)控制律使系統(tǒng)達(dá)到一致性要求，控制輸入逐漸減少為零。

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

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

從仿真圖可以驗(yàn)證動態(tài)勢能場避障函數(shù)的有效性，能夠使飛行器順利躲避障礙物，達(dá)到系統(tǒng)整體的一致性要求。

假設(shè)系統(tǒng)飛行軌跡上存在3個障礙物，位置坐標(biāo)分別為(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

仿真結(jié)果表明，在存在多個障礙物的復(fù)雜環(huán)境中，飛行器依然能夠有效躲避障礙物，實(shí)現(xiàn)一致性要求，達(dá)到期望的目標(biāo)。

7 結(jié)束語

本文對多智能體飛行器的最優(yōu)一致性避障問題進(jìn)行了研究，仿真結(jié)果表明所設(shè)計(jì)的算法能夠有效躲避障礙物，實(shí)現(xiàn)一致性。

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

［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］.長沙:中南大學(xué)，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.