基于純角度觀測信息的leader-followers機器人編隊控制方法

?

基于純角度觀測信息的leader-followers機器人編隊控制方法

韓青，孫樹棟，智睿瑞

(西北工業大學機電學院現代設計與集成制造技術教育部重點實驗室，陜西西安710072)

摘要:提出一種純角度觀測信息的leader-followers多機器人編隊控制方法．多個跟隨機器人(followers)僅觀測其領航機器人(leader)角度信息;基于非線性系統可觀測性的理論研究，這種純角度觀測信息能夠滿足leader-followers的可觀測性要求;利用無跡卡爾曼濾波算法對leader-followers機器人系統的狀態進行估計，根據狀態估計結果設計了輸入-輸出狀態反饋控制規律控制跟隨機器人運動，以達到理想的編隊效果。仿真驗證了該方法的可行性。

關鍵詞:算法;角速度;控制;實驗;反饋控制;卡爾曼濾波;數學模型; MATLAB;矩陣代數;非線性系統;機器人;軌跡;速度;編隊控制; leader-followers編隊控制及可觀測性;移動機器人;無跡卡爾曼濾波;無跡卡爾曼濾波算法與輸入-輸出反饋控制規律

多移動機器人系統具有廣泛的應用前景，是近年來機器人研究熱點，一些研究成果已經在工業實際中得到廣泛應用。已從傳統的工業領域擴展到醫療服務、教育娛樂、勘探勘測、生物工程、柔性制造、救災救援等新領域，并快速發展［1］。其中，多機器人編隊控制是多機器人研究的一個難點，尤其是在觀測信息非常有限的情況下，多機器人的編隊控制具有很強的非線性系統特性。

目前，多機器人編隊控制算法主要包括虛擬結構(virtual structure)法［2］、基于行為(behaviorbased)的方法［3］、領航-跟隨(leader-follower)法［4-6］、人工勢場法［7］等。文獻［4］提出的編隊控制方法能實現避碰和跟蹤參考軌跡。文獻［5］提出了適用于2個機器人距離-方位-方向控制方法和適用于3個機器人的距離-距離-方向控制方法，解決編隊控制問題。文獻［6］提出了一種不確定環境下多機器人的動態編隊控制方法。文獻［4-6］都是至少需要距離-角度信息或者更多信息。鑒于此，基于純角度信息［8-12］的機器人編隊研究成為了一個比較新的研究熱點。文獻［8-10］提出了3個或者4個機器人組成的編隊分布控制規律，分布控制規律使編隊全局穩定。文獻［11］研究了機器人的隊形控制，并提出了3個機器人編隊控制規律。文獻［12］基于輸出擴展雅克比矩陣的秩，研究了非線性系統的可觀測性。

本文提出了一種通過跟隨機器人(followers)觀測領航機器人(leader)方位信息進行機器人相對定位，并通過反饋用于跟隨機器人的實時運動調節，以進行編隊控制的方法，該方法對機器人數量和編隊形狀沒有特殊要求，且能使編隊保持穩定。

1　leader-followers編隊控制及其可觀測性

1. 1leader-follower機器人運動模型

設(x，y)是機器人在世界坐標系中的位置坐標，θ是機器人在世界坐標系中的方向角。ρ1是跟隨機器人質心到leader機器人質心的距離。φ1是follower機器人y軸到leader機器人質心的視角，leader(R1)的坐標位置向量是［x1y1θ1］，follower(R2)的坐標位置向量是［x2y2θ2］，leader和follower的控制輸入分別是線速度和角速度，即［ν1ω1］和［ν2ω2］。leader-follower的方向角差值是α1=θ1－θ2，leader-follower坐標關系如圖1。

leader-follower機器人編隊運動系統模型如(1)式所示。

輸出向量y =[ y1y2]T=[φ1α1]T。

對(1)式機器人運動模型擴展，可以得到n個followers的情況。

狀態向量:

輸入向量:

輸出向量:

圖1　leader-follower坐標關系

1. 2李導數及leader-followers非線性系統可觀測性

n個followers的情況，leader-followers機器人編隊運動系統模型如(3)式。

根據文獻［13］，以秩評價標準來確定非線性系統的局部弱可觀測性。

李導數計算:

任何函數的零階李導數是其本身:

hk(s)關于fνi一階李導數定義為:

hk(s)關于fνi二階李導數(s)定義為:

基于李導數的定義，由各階李導數梯度構成的行向量觀測矩陣定義為(8)式:

式中，表示梯度操作，·表示向量內積，i，k = 1，…，n，j = 2，…，n + 1; p∈N。

引理1對于非線性系統Sn，若由行向量構成的可觀測矩陣M滿秩(leader-followers系統，rank(M) = 3n，n是followers機器人數量)，則非線性系統是局部弱可觀測的。

由(2)式，計算各階李導數和觀測矩陣M。

零階李導數:

零階李導數梯度:

一階李導數:

一階李導數梯度:

由零階李導數梯度和一階李導數梯度構成的行向量矩陣M。

定理1當leader和follower 2個機器人滿足:

1)ν1＞0，νj＞0，j = 2，…，n + 1。

2)φj≠0，j = 1，…，n，即測量方位的follower機器人不沿著follower和leader 2個機器人連線做直線運動。

3) leader-follower機器人不能做平行直線運動。

(23)式中矩陣M的秩是3。

證明:在滿足3個前提條件的情況下，對(23)式中M經過一系列的初等行變換，

由變換后的結果可以看出，變換后的矩陣有3個線性獨立行，因此rank(M) = 3。

由文獻［12］命題3知，各階李導數梯度等于相同階數h(s)對時間導數的梯度。

定理2當leader和follower 2個機器人滿足:

1)ν1＞0，νj＞0，j = 2，…，n + 1。

2)φj= k1，αj= k2，j = 1，…，n，k1，k2是常數，即leader-follower機器人同時做平行直線運動。

(23)式中矩陣M的秩是2。

證明當φj= k1，αj= k2，k1，k2是常數，φj和αj對時間的導數為零，上標(0，…，n)是函數h1(s)對時間的導數階數。

(23)式中的M由零階李導數梯度至一階李導數梯度構成，當然M也可以由更高階李導數的梯度構成，但是推導發現，二階李導數梯度及更高階李導數梯度跟零階李導數梯度及一階李導數梯度成線性比例關系，所以，由零階李導數梯度至更高階李導數梯度構成M的秩不會改變，即定理1中rank(M)仍是3，定理2中rank(M)還是2。

當follower和leader機器人沿著曲線軌跡運動時，根據定理1，rank(M) = 3，即M滿秩，由引理1知，系統Sn是局部弱可觀測的。當follower和leader機器人沿著直線軌跡運動，根據定理2，rank(M) = 2，即M不滿秩，由引理1知，系統Sn不具有局部弱可觀測性。

系統是局部弱可觀測的，系統輸出能夠傳遞足夠豐富的信息，觀測器對系統狀態產生比較準確的估計，改善編隊控制;系統不具有局部弱可觀測性，系統輸出不能夠傳遞足夠豐富的信息，觀測器對系統狀態產生不準確估計，影響編隊控制。

2　UKF濾波算法和輸入-輸出反饋控制規律

為了實現反饋運動控制，狀態估計是必要的，本文選擇UKF濾波算法。通過UKF濾波算法和輸入-輸出狀態反饋控制規律的有機結合，獲取多機器人方位信息進行運動控制，以達到理想的編隊效果。

2. 1UKF濾波算法

系統輸入向量為U，輸出向量為y，UKF濾波算法用來估計狀態S的角度信息，即y = h(s) =[φjαj]T，j = 1，…，n，通過UKF濾波算法，獲得了比較準確的角度值。(27)式和(28)式分別是帶有噪聲的狀態方程和觀測方程。

D是輸出轉換矩陣，O和N是零均值、協方差分別是PL和PN白高斯噪聲，且s(0)、O和N假定為不相關。對(27)式采用前向歐拉方法進行離散化處理，采樣時間為Tc，得到式(29)。

式中Γ(s(k)，u(k) ) = TcF(s) U + s(k)，k∈N。

UKF基于UT變換過程，該算法包括初始化、時間更新和測量更新幾個階段，詳細過程見文獻［14］。

2. 2輸入-輸出狀態反饋控制

以R1和R2機器人為對象設計控制規律，設計的控制規律同樣適用于其他跟隨機器人。

對(1)式的狀態方程進行變形，得到與其等價的(30)式，并對α1=θ1－θ2兩邊求導，得到(31)式。

式中: sr［ρ1φ1］T，M2×2和N2×2分別是F的右上角和左上角的子矩陣。

借鑒文獻［15］I/O標準線性化技術和文獻［16］的方法，提出了應用于機器人編隊控制的輸入-輸出狀態反饋控制規律。

C是輔助控制變量，控制增益k1，k2＞0。上標“ide”指理想值。(32)式代入(30)式并結合(31)式，得到簡化的閉環運動學方程(34)式。

該輸入-輸出反饋控制規律可以穩定編隊。

3　實驗驗證

本文通過3個pioneer-3at機器人(1個leader、2 個followers)組成leader-followers編隊來驗證上述控制方法的有效性。pioneer-3at機器人具有激光雷達、全景相機、聲吶、紅外等傳感器，很容易獲取角度信息。為了使仿真更為真實，利用功能強大的Webots 7搭建3D仿真平臺，能夠精確模擬真實環境，也可以將仿真數據導入MATLAB中進行研究分析。實驗仿真場景如圖2所示，follower機器人是R2、R3，leader機器人是R1。

leader-followers機器人編隊經過分段直線-曲線組合軌跡1、正弦形軌跡2及螺旋形軌跡3。仿真實驗效果見圖3～圖5。

仿真中，采用基于純角度信息的編隊控制方法，followers根據leader的運動情況進行實時自我調整;采用UKF濾波算法對leader-followers機器人系統狀態進行估計;采用輸入-輸出狀態反饋閉環控制規律來穩定編隊。

圖2　移動機器人仿真場景

3. 1初始條件

機器人編隊采用直線-曲線組合軌跡1時，leader線速度和角速度:

機器人編隊采用正弦軌跡2時，leader線速度和角速度:

機器人編隊采用螺旋軌跡3時，leader線速度和角速度: ω1(t)是時間的復雜函數。

leader和follower初始位置及方向角:

UKF和反饋控制相關參數:

s(0) =

3. 2仿真分析

對于軌跡1:

從圖3a)可見，R1、R2、R3機器人沿直線-曲線組合軌跡運動時保持了比較理想的編隊。

從圖3b)可見，機器人編隊沿曲線軌跡1運動時，觀測角估計誤差幾乎是零，即使在運動軌跡發生變化時，最大誤差是0. 000 518 6。

從圖3c)可見，機器人編隊在曲線軌跡1運動時，方向角估計誤差也很小，即使在運動軌跡發生變化時，最大誤差是－0. 003 94。

從圖3d)可見，機器人沿軌跡1運動時，一直沿著直線或曲線運動時，速度變化不大，但當運動軌跡發生變化時，速度的方向和大小變化比較明顯;沿著直線軌跡運行時，跟隨機器人的速度接近領航機器人的初始速度。

圖3　仿真結果

對于軌跡2:

從圖4a)可見，R1、R2、R3機器人沿軌跡2運動時保持了比較理想的編隊。

從圖4b)可見，機器人編隊沿曲線軌跡2運動時，觀測角估計誤差幾乎是零，即使在運動軌跡發生變化時，最大誤差是－0. 000 6。

從圖4c)可見，機器人編隊沿曲線軌跡2運動時，方向角估計誤差最大是－0. 002 862。

從圖4d)可見，機器人沿軌跡2運動時，R2的速度變化范圍是0. 904 6～1. 402; R3的速度變化范圍是0. 848 2～1. 392。

對于軌跡3:

從圖5a)可見，R1、R2、R3機器人在軌跡3運動時保持了比較理想的編隊。

從圖5b)可見，機器人編隊沿軌跡3運動時，觀測角估計誤差幾乎是零。

從圖5c)可見，機器人編隊沿軌跡3運動時，方向角估計誤差收斂速度比較快，在t= 1 s時刻幾乎收斂到零。

從圖5d)可見，機器人沿軌跡3運動時，跟隨機器人速度呈現增大趨勢。

圖4　仿真結果

圖5　仿真結果

4　結論

本文提出了一種跟隨機器人(followers)觀測領航機器人(leader)純角度信息的leader-followers編隊控制方法。基于非線性系統的可觀測性理論，分析了機器人編隊的可觀測性;應用UKF算法對leader-followers系統的狀態進行估計;設計了輸入-輸出狀態反饋控制規律穩定編隊。仿真結果表明，多個機器人能快速形成編隊，并以較小的誤差做復雜軌跡運動，保持了比較理想的隊形。后續研究將包括實現多機器人隊形變換及避障方法。

參考文獻:

［1］譚民，王碩．機器人技術研究進展［J］．自動化學報，2013，39(7) : 963-972 Tan M，Wang S．Research progress on robotics［J］．Acta Automatica Sinica，2013，39(7) : 963-972 (in Chinese)

［2］Ren W，Beard R W．Formation Feedback Control for Multiple Spacecraft Via virtual Structures［J］．IEEE Proceedings Control Theory Application，2004，151(3) : 357-368

［3］Long M，Gage A，Murphy R，et al．Application of The Distributed Field Robot Architecture to a Simulated Demining Task［C］∥Proceedings IEEE International Conference on Robotics and Automation，Barcelona，2005: 3193-3200

［4］Mastellone S，Stipanovic D M，Graunke C R，et al．Formation Control and Collision Avoidance for Multi-Agent-Nonholonomic Systems: Theory and Experiments［J］．The International Journal of Robotics Research，2008，27(1) : 107-126

［5］Chen J，Sun D，Yang J，et al．Leader-Follower Formation Control of Multiple Non-Holonomic Mobile Robots Incorporating a Receding-Horizon Scheme［J］．The International Journal of Robotics Research，2010，29(6) : 727-747

［6］楊麗，曹志強，譚民．不確定環境下多機器人的動態編隊控制［J］．機器人，2010，32(2) : 283-288 Yang L，Cao Z Q，Tan M．Dynamic Formation Control for Multiple Robots in Uncertain Environments［J］．Robot，2010，32(2) : 283-288 (in Chinese)

［7］Kwon J W，Chwa D．Hierarchical Formation Control Based on a Vector Field Method for Wheeled Mobile Robots［J］．IEEE Trans on Robotics，2012，28(6) : 1335-1345

［8］Basiri M，Bishop A N，Jensfelt P．Distributed Control of Triangular Formations with Angle Only Constraints［J］．Systems＆Control Letters，2010，59(2) : 147-154

［9］Bishop A N．A Very Relaxed Control Law for Bearing-Only Triangular Formation Control［C］∥Proceedings of the 18th IFAC World Congress，Milano，Italy，2011: 5991-5998

［10］Bishop A N．Distributed Bearing-Only Quadrilateral Formation Control［C］∥Proceedings of the 18th IFAC WorldCongress，Milano，Italy，2011: 4507-4512

［11］Eren T．Formation Shape Control Based on Bearing Rigidity［J］．International Journal of Control，2012，85(9) : 1361-1379

［12］Mariottini G L，Morbidi F，Prattichizzo D，et al．Vision-Based Localization for Leader-Follower Formation Control［J］．IEEE Trans on Robotics，2009，25(6) : 1431-1438

［13］Hermann R，Krener A J．Nonlinear Controllability and Observability［J］．IEEE Transactions on Automatic Control，1977，22 (5) : 728-740

［14］Wan E A，Van Der Merwe R．The Unscented Kalmanfilter for Nonlinear Estimation［C］∥Adaptive Systems for Signal Processing，Communications，and Control Symposium，2000: 153-158

［15］Slotine J J E，Li W．Applied Nonlinear Control［M］．Englewood Cliffs，NJ: Prentice-Hall，1991

［16］Das A K，Fierro R，Kumar V，et al．A Vision-Based Formation Control Framework［J］．IEEE Trans on Robotics and Automation，2002，18(5) : 813-825

Bearing-only Leader-Followers Multi-Robot Formation Control

Han Qing，Sun Shudong，Zhi Ruirui

(Contemporary Design and Integrated Manufacturing Technology Key Laboratory of Ministry of Education， School of Mechanical Engineering，Northwestern Polytechnical University，Xi'an 710072，China)

Abstract:A bearing-only formation control method for leader-followers multi-robots is proposed．In the study，follower-robots can only observe the bearing information of the leader-robot．Based on the observability of nonlinear system，studies show that the bearing-only observation meets the leader-followers observability condition for general nonlinear system．The unscented Kalman filter(UKF) is employed to estimate the state of leader-followers’robots．The results are used for followers’movement control via the input-output feedback control law，so that the desired formation of the robots are maintained．Simulation results are presented to demonstrate the feasibility of the approach．

Key words:algorithms，angular velocity，control，experiments，feedback control，Kalman filters，mathematical models，MATLAB，matrix algebra，nonlinear systems，robots，trajectories，velocity; formation control，leader-followers formation control and observability，mobile robot，UKF(Unscented Kalman filter)，UKF filter algorithm and input-output feedback control law

作者簡介:韓青(1973—)，西北工業大學講師、博士研究生，主要從事智能機器人、多機器人協調控制與機電一體化與機電控制技術的研究。

收稿日期:2014-10-08基金項目:國家自然科學基金(51475383、51075337)資助

文章編號:1000-2758(2015) 02-0244-07

文獻標志碼:A

中圖分類號:TP24