多機器人路徑規劃及編隊控制策略

屈軍鎖,馬玉環,蔡 星,李鵬飛

(西安郵電大學 通信與信息工程學院,陜西 西安 710121)

工業化進程的加快使得機器人技術的發展愈發被重視。機器人技術[1]是對未來新興產業發展具有重要意義的高技術之一,現已廣泛應用在工業、醫療和科學探索等眾多領域。隨著需求的多樣化和復雜化,單機器人在許多場景下已無法滿足需求,而多機器人技術開拓了機器人領域的新方向,彌補了單機器人的缺陷。多機器人系統可以通過機器人之間相互合作共同完成復雜的任務,比單機器人系統更靈活、穩定性表現更好[2]。目前,多機器人的編隊控制問題已成為機器人領域的重要研究方向。多機器人編隊控制問題源于自然界生物的群體行為,如鳥群的集體遷徙行為、魚群編隊防御行為和蟻群集體協作覓食行為等。在機器人領域,多機器人編隊是指多個移動式機器人在運動過程中,按照一定的距離和運動狀態的約束,使得多機器人形成期望的幾何形狀[3],如工業中利用倉儲機器人協同搬運貨物、軍事中利用多無人機協同作業等[4]。

多機器人系統的控制結構主要包含集中式、分散式和分布式等3類,其中分布式結構包含了集中式與分散式結構的優點,應用較為廣泛。多機器人編隊的控制方法主要有領航者-跟隨者編隊控制策略[5]、基于行為的編隊控制策略[6],以及基于虛擬結構的編隊控制策略[7]。領航者-跟隨者編隊控制策略具有結構簡潔易于實現的優點,但編隊穩定性受到領航者狀態的影響較大[8]。文獻[9]在領航者-跟隨者策略的基礎上提出了基于拍賣算法的編隊控制策略,提高了編隊的效率,但是該策略對于系統資源消耗較大。文獻[10]引入了虛擬領航者,使得編隊中跟隨者相對于領航者的跟蹤誤差更小?；谛袨榈木庩牽刂撇呗赃m用于機器人數量較多的系統,但編隊隊形的穩定性和實時性方面存在缺陷?；谔摂M結構的編隊控制策略[11]編隊精度較高,但在實際中的通信與計算成本較大,編隊實時性較差。文獻[12]針對多機器人的編隊運動問題,提出一種鏈式編隊控制方法,該方法通過主從協同的方式實現了多機器人的編隊行進,但編隊穩定性較差。文獻[13]針對未知環境下的多機器人編隊隊形控制問題,基于類鳥群模擬放射方法設計了不同規則的控制器,能夠實現機器人編隊任意隊形的變換和保持,但是該方法計算復雜度較高。

近年來,一致性理論在編隊控制領域的應用逐漸受到重視。Ren等[14]將一致性控制律應用于多智能體系統的編隊控制問題中,并驗證了基于領航-跟隨者策略等幾種傳統編隊策略均可利用一致性理論實現?；诜柪绽咕仃嚨亩鄼C器人仿射編隊控制方法[15]分析了多機器人系統能夠實現仿射變換的條件,為多機器人的編隊控制提供了一種新的方法。文獻[16]研究了對應于無向圖應力矩陣的仿射編隊控制方法,給出了機器人編隊能夠實現仿射定位的充要條件,基于仿射變換設計了不同情況下的編隊控制器,并證明了所提出的控制律的穩定性和全局收斂性。然而,各種策略均存在著各自的局限性和不足,限制了其在復雜場景下的廣泛應用。因此,擬對多機器人路徑規劃以及編隊運動控制問題進行研究,給出一種多機器人路徑規劃及編隊控制策略。首先,基于IPOPT(Interior Point Optimizer)路徑優化方法,改善A*算法規劃的路徑不平滑問題,得到軌跡平滑的路徑。然后,根據有向圖結構給出領航-跟隨者型仿射編隊控制方法,并基于一致性理論設計一階和二階多機器人系統的編隊運動控制協議,實現機器人編隊隊形的控制。

1 系統設計

將多機器人編隊視為一個尺度可伸縮的整體,參照單機器人的路徑規劃方法,由編隊中的領航者負責規劃編隊整體的參考路徑,根據編隊中機器人之間的位置關系對獲得的路徑偏移,得到全部機器人的路徑。選擇A*算法[17]作為多機器人編隊的路徑規劃算法,基于IPOPT對A*算法規劃的全局路徑進行優化,得到軌跡平滑的路徑?；谟邢驁D結構,給出領航-跟隨者型仿射編隊控制方法,實現編隊的放縮、旋轉等變換。系統流程如圖1所示。

圖1 系統流程

1.1 A*算法優化

1.1.1 A*算法

A*算法[17]是一種靜態路網中求解最短路徑搜索算法,通過增加啟發式函數確定最優路徑,是目前應用較為廣泛的路徑查找算法。A*算法的公式描述為

F(n)=g(n)+h(n)

(1)

其中,

式中：F(n)為節點n的估價函數;g(n)為結點t的實際代價;h(n)為從結點n到目標節點p的最佳路徑的估計代價;(xn,yn)為節點n的坐標;(xt,yt)為節點t的坐標;(xp,yp)為節點p的坐標。

通過仿真軟件對A*算法進行路徑規劃仿真,A*算法路徑規劃過程如圖2所示,規劃結果如圖3所示。

圖2 A*算法路徑規劃

圖3 A*算法路徑規劃結果

觀察圖2和圖3仿真結果發現,A*算法能夠從起點位置搜索出到達目標位置的無碰撞路徑,但是路徑的轉折角度過大,路徑不夠平滑。

1.1.2 基于IPOPT的A*算法軌跡優化

為了使得A*算法規劃的路徑更加平滑,基于IPOPT對A*算法規劃的全局路徑進行優化,將線性搜索過濾原始對偶內點算法用于求解非線性方程組。內點算法是將非線性規劃的不等式約束轉化為障礙并將其添加到目標函數中,將原始的非線性規劃轉化為僅等式約束或無約束問題。簡化后,非線性優化問題便轉換為障礙因素類型的子問題。

非線性優化的問題模型表示為

(2)

s.t.gL≤g(x)≤gU
xL≤x≤xU

式中：x∈Rn為優化變量,下限xL∈(R∪{-∞})n,上限xU∈(R∪{+∞})n;函數f：Rn→R為目標函數;函數g：Rn→Rm為一般非線性約束,其上限gL∈(R∪{-∞})m,下限gU∈(R∪{+∞})m。將式(2)中的約束g(x)和變量x提取為如下形式

(3)

將式(3)中所有的變量下限為0,那么原非線性規劃式(2)可以轉換為一個新的非線性規劃問題

s.t.c(x)=0

x≥0

(4)

式中：函數f(x)為目標函數;函數c(x)為約束函數,其約束條件為ci(x)=gi(x)-si=0,(i=1,2,3,…n);x≥0為添加新的約束,使得所有變量的下限等于0。在這個新的非線性規劃問題中,使用內點法并引入對數障礙項,構建得到新的目標函數

s.t.c(x)=0

(5)

式(5)中,對數障礙函數φμ(x)替代了式(4)中c(x),μ為障礙項參數且μ>0。φμ(x)將x的分量xi添加到目標函數中,從而保證了xi≥0,在優化過程中,任意xi保持非負?？紤]到xi趨于0時,φμ(x)的值將區域無窮大,因此式(5)的最優解將出現在x的定義域內,且滿足xi≥0。

原非線性優化問題的求解可以轉換為φμ(x)的最小值求解問題,求解步驟如下。

步驟1確定合適的障礙參數μ,為變量x賦初始值。

步驟2把μ的值減少,求解下一個障礙問題,起始點為前一個障礙問題的解。

步驟3不斷重復步驟2,直到得到式(4)的解,或者滿足一階最優條件的解。

求得式(5)的一階的最優條件為

(6)

式中：x,z≥0;e為全1向量;y∈Rn為拉格朗日乘子;z∈Rn為邊界約束的拉格朗日乘子;X=Diag(x),Z=Diag(z)。

(7)

式中：I為單位矩陣;Wi為式(3)中gi(x)的拉格朗日函數的海森矩陣;Ai為等式約束的雅克比矩陣的轉置;gi為目標函數的梯度。

在zi方向上更新時滿足

其中,τ=min{0.99,μ}。

(8)

設約束條件為g、狀態變量為x,那么使用IPOPT求解非線性優化問題的主要步驟描述如下。

步驟1設置狀態變量x和約束條件g的數量。

步驟2設置狀態變量x的初始值。

步驟3設置狀態變量x和約束條件g的上下界。

步驟4構建約束條件g。

步驟5構建問題的目標函數。

步驟6設置IPOPT求解參數。

步驟7迭代求解,得到優化結果。

優化后的路徑貼近原始路徑,且路徑彎折區域軌跡平滑連續,使得機器人在該路徑上的運動連續,降低了機器人的運動代價?；贗POPT的A*軌跡優化過程如圖4所示。

圖4 基于IPOPT的A*軌跡優化過程

1.2 仿射編隊設計

仿射變換是指一個向量空間通過線形變換與平移變換組合,轉換為另一個向量空間的變換過程。圖形的仿射變換如圖5所示。

圖5 圖形仿射變換

二維平面中的一個多機器人編隊,如果編隊變換前后的隊形仿射等價,那么編隊隊形變換過程可以被仿射變換描述,編隊可以基于仿射變換生成任意與之仿射等價的隊形。

設d維空間d中由N個機器人組成多機器人系統,前Nl個機器人為領航者,跟隨者數量為Nf=N-Nl,則系統中機器人的位置集合可表示為

(9)

式中：pl(l=1,2…Nl)為系統中領航者的位置集合;pf(f=Nl+1,Nl+2,…N)為跟隨者的位置集合。

系統中機器人之間通信拓撲用有向圖G=(V,E)表示,其中：V=(Vl,Vf)為節點集;Vl表示領航者集合;Vf表示跟隨者集合;E?V×V為邊集,若圖中存在有向邊(i,j)∈E,那么節點j定義為節點i的入鄰居,節點i為j的出鄰居。系統編隊表示為(G,p),有向圖G的節點與系統中的機器人依次對應。

標稱編隊描述了編隊的期望隊形,定義系統的標稱編隊表示為(G,r),r為標稱編隊的隊形矩陣,表示為

(10)

式中：rl為領航者的標稱隊形;rf為跟隨者標稱編隊。

定義標稱隊形r的仿射映射為

(11)

式中：(A,b)記作仿射變換;IN與1N分別為N維單位矩陣和全1向量;?為克羅內克積。

定義位置集合p的配置矩陣P(p)∈d×N,定義P(p)的增廣矩陣為

(12)

定義時變目標編隊為

p*(t)=[Id?A(t)]r+1N?b(t)

(13)

式中：Id為d維單位矩陣;A(t)表示仿射變換中的線性變換,而b(t)則仿射變換中的平移變換;t為時間參數。

文獻[16]給出了無向圖結構下的標稱編隊能夠實現仿射定位的條件,那么根據無向圖結構下的標稱編隊仿射定位的條件可以推導出有向圖結構下標稱編隊(G,r)能夠實現仿射定位的條件如下。

1) 系統中跟隨者的位置集合pf總是能夠被領航者的位置pl確定。

2) 對于有向圖G的符號拉普拉斯矩陣LΩ及機器人位置集合p,滿足

(LΩ?Id)p=0

(14)

其中,

式中,kij表示連接節點i與節點j的邊的權重。

(15)

標稱編隊隊形及其拓撲結構如圖6所示,圖6中標號1,2,3的圓形表示領航者機器人,標號4,5,6,7的圓形表示跟隨機器人。

圖6 標稱編隊隊形

基于以上多機器人能夠實現仿射定位的條件,設計一階多機器人系統的運動控制協議,一階多機器人系統中第i個機器人的動力學方程為

(16)

式中,ui(t)表示第i個機器人的控制輸入。

設編隊中領航者的速度為有界的時變值,設計編隊中跟隨機器人i的運動控制協議為

(17)

(18)

由此,根據文獻[18]給出的一致性判定條件,式(17)所示的控制協議能使得系統中跟隨機器人的位置跟隨誤差全局收斂于領航者。

設計二階系統的運動控制協議,系統中機器人的二階動力學方程為

(19)

式中,vi與ui分別表示系統中機器人的速度與控制輸入,i=1,2,…N。對于二階系統,控制輸入為加速度。受文獻[18]中關于二階多機器人系統運動控制協議設計思路的啟發,設編隊中領航者的加速度為有界的時變值,則設計編隊中第i個跟隨機器人的運動控制協議為

(20)

根據文獻[18]給出的一致性判定條件,式(20)所示的控制協議能使得系統中跟隨機器人相對于領航者的跟隨誤差全局收斂。

2 驗證結果分析

2.1 路徑優化仿真驗證

首先利用A*算法規劃出全局路徑,基于IPOPT對獲得的路徑進行優化。由大小為22×22的柵格單元組成的封閉地圖,地圖中的障礙物隨機生成。第一次仿真的起始點坐標設置為(3,18),目標點坐標(18,2),結果如圖7所示。第二次仿真的起始點坐標設置為(3,18),目標點坐標設為(19,7),結果如圖8所示。圖中黑色柵格代表障礙物,連續曲線代表規劃的路徑。圖7(a)與圖8(a)分別表示第一次和第二次仿真的原始路徑,圖7(b)與圖8(b)分別表示第一次和第二次仿真改進后的路徑。

圖7 路徑優化第一次仿真結果

圖8 路徑優化第二次仿真結果

相比于圖7(a)和圖8(a)原始算法的路徑,圖7(b)和圖8(b)中改進后的路徑在靠近障礙物的區域沒有出現角轉彎,軌跡連續性比較好,且貼近原始路徑?？梢钥闯?基于IPOPT的軌跡平滑方案能夠有效平滑A*算法規劃路徑中的轉折區域。

2.2 編隊運動規劃及仿真

創建二維平面中大小為55 m×55 m的封閉有障礙地圖,確定圖6編隊中前3個機器人作為領航者,領航者的初始位置為

跟隨者的初始位置為

領航者的目標位置為

編隊整體的路徑規劃步驟如下。

步驟1獲取當前環境地圖信息。

步驟2確定編隊期望隊形,根據期望隊形確定3個領航者之間的相對位置關系。

步驟3由編隊中第一個領航者利用 A*算法在當前環境中規劃出平滑的全局路徑,并基于IPOPT優化當前路徑,將該路徑發布給其余領航者。

步驟4編隊中其余領航者根據與第一個領航者的相對位置關系以及環境中的障礙物分布信息,對步驟3中得到的路徑進行坐標偏移,分別得到其他領航者的路徑。

步驟5判斷偏移后的路徑軌跡是否與障礙物發生碰撞,規劃編隊的避障策略。

當編隊中的領航者獲得全局路徑后,根據編隊隊形幾何結構,規劃領航者在當前路徑下的速度與加速度等控制變量,進而進行運動規劃。編隊在當前環境下的運動軌跡如圖9所示,圖中紅色圓形為編隊的領航者,藍色圓形為編隊中的跟隨者,綠色虛線為第一個領航者的路徑,黑色區域為障礙物分布。

圖9 編隊運動軌跡

編隊中各機器人在X軸與Y軸上位移、速度和加速度曲線分別如圖10與圖11所示,可以看出,系統中的領航者和跟隨者機器人在X軸與Y軸上位移、速度和加速度等均平緩變化,未出現突變現象。

圖10 X軸上位移、速度及加速度與時間的關系

圖11 Y軸上位移、速度及加速度與時間的關系

3 結語

研究了多機器人編隊的路徑規劃以及運動控制問題?；贗POPT的路徑優化方法改善了A*算法規劃的路徑轉彎不平滑的問題。對于多機器人的編隊控制問題,給出了基于有向圖結構的領航-跟隨型仿射編隊控制方法,并設計了一階和二階系統的運動控制協議。實驗結果表明,基于IPOPT的路徑優化方法能夠有效平滑A*算法規劃的路徑,且編隊在沿著規劃的路徑運動時能夠保持編隊隊形。