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

?

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

韓青，孫樹棟，智睿瑞

(西北工業(yè)大學(xué)機電學(xué)院現(xiàn)代設(shè)計與集成制造技術(shù)教育部重點實驗室，陜西西安710072)

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

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

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

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

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

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

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

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

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

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

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

狀態(tài)向量:

輸入向量:

輸出向量:

圖1　leader-follower坐標關(guān)系

1. 2李導(dǎo)數(shù)及l(fā)eader-followers非線性系統(tǒng)可觀測性

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

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

李導(dǎo)數(shù)計算:

任何函數(shù)的零階李導(dǎo)數(shù)是其本身:

hk(s)關(guān)于fνi一階李導(dǎo)數(shù)定義為:

hk(s)關(guān)于fνi二階李導(dǎo)數(shù)(s)定義為:

基于李導(dǎo)數(shù)的定義，由各階李導(dǎo)數(shù)梯度構(gòu)成的行向量觀測矩陣定義為(8)式:

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

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

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

零階李導(dǎo)數(shù):

零階李導(dǎo)數(shù)梯度:

一階李導(dǎo)數(shù):

一階李導(dǎo)數(shù)梯度:

由零階李導(dǎo)數(shù)梯度和一階李導(dǎo)數(shù)梯度構(gòu)成的行向量矩陣M。

定理1當(dāng)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經(jīng)過一系列的初等行變換，

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

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

定理2當(dāng)leader和follower 2個機器人滿足:

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

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

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

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

(23)式中的M由零階李導(dǎo)數(shù)梯度至一階李導(dǎo)數(shù)梯度構(gòu)成，當(dāng)然M也可以由更高階李導(dǎo)數(shù)的梯度構(gòu)成，但是推導(dǎo)發(fā)現(xiàn)，二階李導(dǎo)數(shù)梯度及更高階李導(dǎo)數(shù)梯度跟零階李導(dǎo)數(shù)梯度及一階李導(dǎo)數(shù)梯度成線性比例關(guān)系，所以，由零階李導(dǎo)數(shù)梯度至更高階李導(dǎo)數(shù)梯度構(gòu)成M的秩不會改變，即定理1中rank(M)仍是3，定理2中rank(M)還是2。

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

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

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

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

2. 1UKF濾波算法

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

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

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

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

2. 2輸入-輸出狀態(tài)反饋控制

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

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

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

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

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

該輸入-輸出反饋控制規(guī)律可以穩(wěn)定編隊。

3　實驗驗證

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

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

仿真中，采用基于純角度信息的編隊控制方法，followers根據(jù)leader的運動情況進行實時自我調(diào)整;采用UKF濾波算法對leader-followers機器人系統(tǒng)狀態(tài)進行估計;采用輸入-輸出狀態(tài)反饋閉環(huán)控制規(guī)律來穩(wěn)定編隊。

圖2　移動機器人仿真場景

3. 1初始條件

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

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

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

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

UKF和反饋控制相關(guān)參數(shù):

s(0) =

3. 2仿真分析

對于軌跡1:

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

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

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

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

圖3　仿真結(jié)果

對于軌跡2:

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

從圖4b)可見，機器人編隊沿曲線軌跡2運動時，觀測角估計誤差幾乎是零，即使在運動軌跡發(fā)生變化時，最大誤差是－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運動時，跟隨機器人速度呈現(xiàn)增大趨勢。

圖4　仿真結(jié)果

圖5　仿真結(jié)果

4　結(jié)論

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

參考文獻:

［1］譚民，王碩．機器人技術(shù)研究進展［J］．自動化學(xué)報，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］楊麗，曹志強，譚民．不確定環(huán)境下多機器人的動態(tài)編隊控制［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，F(xiàn)ierro 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—)，西北工業(yè)大學(xué)講師、博士研究生，主要從事智能機器人、多機器人協(xié)調(diào)控制與機電一體化與機電控制技術(shù)的研究。

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

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

文獻標志碼:A

中圖分類號:TP24