?
基于純角度觀測(cè)信息的leader-followers機(jī)器人編隊(duì)控制方法
韓青,孫樹(shù)棟,智睿瑞
(西北工業(yè)大學(xué)機(jī)電學(xué)院現(xiàn)代設(shè)計(jì)與集成制造技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,陜西西安710072)
摘要:提出一種純角度觀測(cè)信息的leader-followers多機(jī)器人編隊(duì)控制方法.多個(gè)跟隨機(jī)器人(followers)僅觀測(cè)其領(lǐng)航機(jī)器人(leader)角度信息;基于非線性系統(tǒng)可觀測(cè)性的理論研究,這種純角度觀測(cè)信息能夠滿足leader-followers的可觀測(cè)性要求;利用無(wú)跡卡爾曼濾波算法對(duì)leader-followers機(jī)器人系統(tǒng)的狀態(tài)進(jìn)行估計(jì),根據(jù)狀態(tài)估計(jì)結(jié)果設(shè)計(jì)了輸入-輸出狀態(tài)反饋控制規(guī)律控制跟隨機(jī)器人運(yùn)動(dòng),以達(dá)到理想的編隊(duì)效果。仿真驗(yàn)證了該方法的可行性。
關(guān)鍵詞:算法;角速度;控制;實(shí)驗(yàn);反饋控制;卡爾曼濾波;數(shù)學(xué)模型; MATLAB;矩陣代數(shù);非線性系統(tǒng);機(jī)器人;軌跡;速度;編隊(duì)控制; leader-followers編隊(duì)控制及可觀測(cè)性;移動(dòng)機(jī)器人;無(wú)跡卡爾曼濾波;無(wú)跡卡爾曼濾波算法與輸入-輸出反饋控制規(guī)律
多移動(dòng)機(jī)器人系統(tǒng)具有廣泛的應(yīng)用前景,是近年來(lái)機(jī)器人研究熱點(diǎn),一些研究成果已經(jīng)在工業(yè)實(shí)際中得到廣泛應(yīng)用。已從傳統(tǒng)的工業(yè)領(lǐng)域擴(kuò)展到醫(yī)療服務(wù)、教育娛樂(lè)、勘探勘測(cè)、生物工程、柔性制造、救災(zāi)救援等新領(lǐng)域,并快速發(fā)展[1]。其中,多機(jī)器人編隊(duì)控制是多機(jī)器人研究的一個(gè)難點(diǎn),尤其是在觀測(cè)信息非常有限的情況下,多機(jī)器人的編隊(duì)控制具有很強(qiáng)的非線性系統(tǒng)特性。
目前,多機(jī)器人編隊(duì)控制算法主要包括虛擬結(jié)構(gòu)(virtual structure)法[2]、基于行為(behaviorbased)的方法[3]、領(lǐng)航-跟隨(leader-follower)法[4-6]、人工勢(shì)場(chǎng)法[7]等。文獻(xiàn)[4]提出的編隊(duì)控制方法能實(shí)現(xiàn)避碰和跟蹤參考軌跡。文獻(xiàn)[5]提出了適用于2個(gè)機(jī)器人距離-方位-方向控制方法和適用于3個(gè)機(jī)器人的距離-距離-方向控制方法,解決編隊(duì)控制問(wèn)題。文獻(xiàn)[6]提出了一種不確定環(huán)境下多機(jī)器人的動(dòng)態(tài)編隊(duì)控制方法。文獻(xiàn)[4-6]都是至少需要距離-角度信息或者更多信息。鑒于此,基于純角度信息[8-12]的機(jī)器人編隊(duì)研究成為了一個(gè)比較新的研究熱點(diǎn)。文獻(xiàn)[8-10]提出了3個(gè)或者4個(gè)機(jī)器人組成的編隊(duì)分布控制規(guī)律,分布控制規(guī)律使編隊(duì)全局穩(wěn)定。文獻(xiàn)[11]研究了機(jī)器人的隊(duì)形控制,并提出了3個(gè)機(jī)器人編隊(duì)控制規(guī)律。文獻(xiàn)[12]基于輸出擴(kuò)展雅克比矩陣的秩,研究了非線性系統(tǒng)的可觀測(cè)性。
本文提出了一種通過(guò)跟隨機(jī)器人(followers)觀測(cè)領(lǐng)航機(jī)器人(leader)方位信息進(jìn)行機(jī)器人相對(duì)定位,并通過(guò)反饋用于跟隨機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)調(diào)節(jié),以進(jìn)行編隊(duì)控制的方法,該方法對(duì)機(jī)器人數(shù)量和編隊(duì)形狀沒(méi)有特殊要求,且能使編隊(duì)保持穩(wěn)定。
1. 1leader-follower機(jī)器人運(yùn)動(dòng)模型
設(shè)(x,y)是機(jī)器人在世界坐標(biāo)系中的位置坐標(biāo),θ是機(jī)器人在世界坐標(biāo)系中的方向角。ρ1是跟隨機(jī)器人質(zhì)心到leader機(jī)器人質(zhì)心的距離。φ1是follower機(jī)器人y軸到leader機(jī)器人質(zhì)心的視角,leader(R1)的坐標(biāo)位置向量是[x1y1θ1],follower(R2)的坐標(biāo)位置向量是[x2y2θ2],leader和follower的控制輸入分別是線速度和角速度,即[ν1ω1]和[ν2ω2]。leader-follower的方向角差值是α1=θ1-θ2,leader-follower坐標(biāo)關(guān)系如圖1。
leader-follower機(jī)器人編隊(duì)運(yùn)動(dòng)系統(tǒng)模型如(1)式所示。
輸出向量y =[ y1y2]T=[φ1α1]T。
對(duì)(1)式機(jī)器人運(yùn)動(dòng)模型擴(kuò)展,可以得到n個(gè)followers的情況。
狀態(tài)向量:
輸入向量:
輸出向量:
圖1 leader-follower坐標(biāo)關(guān)系
1. 2李導(dǎo)數(shù)及l(fā)eader-followers非線性系統(tǒng)可觀測(cè)性
n個(gè)followers的情況,leader-followers機(jī)器人編隊(duì)運(yùn)動(dòng)系統(tǒng)模型如(3)式。
根據(jù)文獻(xiàn)[13],以秩評(píng)價(jià)標(biāo)準(zhǔn)來(lái)確定非線性系統(tǒng)的局部弱可觀測(cè)性。
李導(dǎo)數(shù)計(jì)算:
任何函數(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)成的行向量觀測(cè)矩陣定義為(8)式:
式中,表示梯度操作,·表示向量?jī)?nèi)積,i,k = 1,…,n,j = 2,…,n + 1; p∈N。
引理1對(duì)于非線性系統(tǒng)Sn,若由行向量構(gòu)成的可觀測(cè)矩陣M滿秩(leader-followers系統(tǒng),rank(M) = 3n,n是followers機(jī)器人數(shù)量),則非線性系統(tǒng)是局部弱可觀測(cè)的。
由(2)式,計(jì)算各階李導(dǎo)數(shù)和觀測(cè)矩陣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個(gè)機(jī)器人滿足:
1)ν1>0,νj>0,j = 2,…,n + 1。
2)φj≠0,j = 1,…,n,即測(cè)量方位的follower機(jī)器人不沿著follower和leader 2個(gè)機(jī)器人連線做直線運(yùn)動(dòng)。
3) leader-follower機(jī)器人不能做平行直線運(yùn)動(dòng)。
(23)式中矩陣M的秩是3。
證明:在滿足3個(gè)前提條件的情況下,對(duì)(23)式中M經(jīng)過(guò)一系列的初等行變換,
由變換后的結(jié)果可以看出,變換后的矩陣有3個(gè)線性獨(dú)立行,因此rank(M) = 3。
由文獻(xiàn)[12]命題3知,各階李導(dǎo)數(shù)梯度等于相同階數(shù)h(s)對(duì)時(shí)間導(dǎo)數(shù)的梯度。
定理2當(dāng)leader和follower 2個(gè)機(jī)器人滿足:
1)ν1>0,νj>0,j = 2,…,n + 1。
2)φj= k1,αj= k2,j = 1,…,n,k1,k2是常數(shù),即leader-follower機(jī)器人同時(shí)做平行直線運(yùn)動(dòng)。
(23)式中矩陣M的秩是2。
證明當(dāng)φj= k1,αj= k2,k1,k2是常數(shù),φj和αj對(duì)時(shí)間的導(dǎo)數(shù)為零,上標(biāo)(0,…,n)是函數(shù)h1(s)對(duì)時(shí)間的導(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的秩不會(huì)改變,即定理1中rank(M)仍是3,定理2中rank(M)還是2。
當(dāng)follower和leader機(jī)器人沿著曲線軌跡運(yùn)動(dòng)時(shí),根據(jù)定理1,rank(M) = 3,即M滿秩,由引理1知,系統(tǒng)Sn是局部弱可觀測(cè)的。當(dāng)follower和leader機(jī)器人沿著直線軌跡運(yùn)動(dòng),根據(jù)定理2,rank(M) = 2,即M不滿秩,由引理1知,系統(tǒng)Sn不具有局部弱可觀測(cè)性。
系統(tǒng)是局部弱可觀測(cè)的,系統(tǒng)輸出能夠傳遞足夠豐富的信息,觀測(cè)器對(duì)系統(tǒng)狀態(tài)產(chǎn)生比較準(zhǔn)確的估計(jì),改善編隊(duì)控制;系統(tǒng)不具有局部弱可觀測(cè)性,系統(tǒng)輸出不能夠傳遞足夠豐富的信息,觀測(cè)器對(duì)系統(tǒng)狀態(tài)產(chǎn)生不準(zhǔn)確估計(jì),影響編隊(duì)控制。
為了實(shí)現(xiàn)反饋運(yùn)動(dòng)控制,狀態(tài)估計(jì)是必要的,本文選擇UKF濾波算法。通過(guò)UKF濾波算法和輸入-輸出狀態(tài)反饋控制規(guī)律的有機(jī)結(jié)合,獲取多機(jī)器人方位信息進(jìn)行運(yùn)動(dòng)控制,以達(dá)到理想的編隊(duì)效果。
2. 1UKF濾波算法
系統(tǒng)輸入向量為U,輸出向量為y,UKF濾波算法用來(lái)估計(jì)狀態(tài)S的角度信息,即y = h(s) =[φjαj]T,j = 1,…,n,通過(guò)UKF濾波算法,獲得了比較準(zhǔn)確的角度值。(27)式和(28)式分別是帶有噪聲的狀態(tài)方程和觀測(cè)方程。
D是輸出轉(zhuǎn)換矩陣,O和N是零均值、協(xié)方差分別是PL和PN白高斯噪聲,且s(0)、O和N假定為不相關(guān)。對(duì)(27)式采用前向歐拉方法進(jìn)行離散化處理,采樣時(shí)間為Tc,得到式(29)。
式中Γ(s(k),u(k) ) = TcF(s) U + s(k),k∈N。
UKF基于UT變換過(guò)程,該算法包括初始化、時(shí)間更新和測(cè)量更新幾個(gè)階段,詳細(xì)過(guò)程見(jiàn)文獻(xiàn)[14]。
2. 2輸入-輸出狀態(tài)反饋控制
以R1和R2機(jī)器人為對(duì)象設(shè)計(jì)控制規(guī)律,設(shè)計(jì)的控制規(guī)律同樣適用于其他跟隨機(jī)器人。
對(duì)(1)式的狀態(tài)方程進(jìn)行變形,得到與其等價(jià)的(30)式,并對(duì)α1=θ1-θ2兩邊求導(dǎo),得到(31)式。
式中: sr[ρ1φ1]T,M2×2和N2×2分別是F的右上角和左上角的子矩陣。
借鑒文獻(xiàn)[15]I/O標(biāo)準(zhǔn)線性化技術(shù)和文獻(xiàn)[16]的方法,提出了應(yīng)用于機(jī)器人編隊(duì)控制的輸入-輸出狀態(tài)反饋控制規(guī)律。
C是輔助控制變量,控制增益k1,k2>0。上標(biāo)“ide”指理想值。(32)式代入(30)式并結(jié)合(31)式,得到簡(jiǎn)化的閉環(huán)運(yùn)動(dòng)學(xué)方程(34)式。
該輸入-輸出反饋控制規(guī)律可以穩(wěn)定編隊(duì)。
本文通過(guò)3個(gè)pioneer-3at機(jī)器人(1個(gè)leader、2 個(gè)followers)組成leader-followers編隊(duì)來(lái)驗(yàn)證上述控制方法的有效性。pioneer-3at機(jī)器人具有激光雷達(dá)、全景相機(jī)、聲吶、紅外等傳感器,很容易獲取角度信息。為了使仿真更為真實(shí),利用功能強(qiáng)大的Webots 7搭建3D仿真平臺(tái),能夠精確模擬真實(shí)環(huán)境,也可以將仿真數(shù)據(jù)導(dǎo)入MATLAB中進(jìn)行研究分析。實(shí)驗(yàn)仿真場(chǎng)景如圖2所示,follower機(jī)器人是R2、R3,leader機(jī)器人是R1。
leader-followers機(jī)器人編隊(duì)經(jīng)過(guò)分段直線-曲線組合軌跡1、正弦形軌跡2及螺旋形軌跡3。仿真實(shí)驗(yàn)效果見(jiàn)圖3~圖5。
仿真中,采用基于純角度信息的編隊(duì)控制方法,followers根據(jù)leader的運(yùn)動(dòng)情況進(jìn)行實(shí)時(shí)自我調(diào)整;采用UKF濾波算法對(duì)leader-followers機(jī)器人系統(tǒng)狀態(tài)進(jìn)行估計(jì);采用輸入-輸出狀態(tài)反饋閉環(huán)控制規(guī)律來(lái)穩(wěn)定編隊(duì)。
圖2 移動(dòng)機(jī)器人仿真場(chǎng)景
3. 1初始條件
機(jī)器人編隊(duì)采用直線-曲線組合軌跡1時(shí),leader線速度和角速度:
機(jī)器人編隊(duì)采用正弦軌跡2時(shí),leader線速度和角速度:
機(jī)器人編隊(duì)采用螺旋軌跡3時(shí),leader線速度和角速度: ω1(t)是時(shí)間的復(fù)雜函數(shù)。
leader和follower初始位置及方向角:
UKF和反饋控制相關(guān)參數(shù):
s(0) =
3. 2仿真分析
對(duì)于軌跡1:
從圖3a)可見(jiàn),R1、R2、R3機(jī)器人沿直線-曲線組合軌跡運(yùn)動(dòng)時(shí)保持了比較理想的編隊(duì)。
從圖3b)可見(jiàn),機(jī)器人編隊(duì)沿曲線軌跡1運(yùn)動(dòng)時(shí),觀測(cè)角估計(jì)誤差幾乎是零,即使在運(yùn)動(dòng)軌跡發(fā)生變化時(shí),最大誤差是0. 000 518 6。
從圖3c)可見(jiàn),機(jī)器人編隊(duì)在曲線軌跡1運(yùn)動(dòng)時(shí),方向角估計(jì)誤差也很小,即使在運(yùn)動(dòng)軌跡發(fā)生變化時(shí),最大誤差是-0. 003 94。
從圖3d)可見(jiàn),機(jī)器人沿軌跡1運(yùn)動(dòng)時(shí),一直沿著直線或曲線運(yùn)動(dòng)時(shí),速度變化不大,但當(dāng)運(yùn)動(dòng)軌跡發(fā)生變化時(shí),速度的方向和大小變化比較明顯;沿著直線軌跡運(yùn)行時(shí),跟隨機(jī)器人的速度接近領(lǐng)航機(jī)器人的初始速度。
圖3 仿真結(jié)果
對(duì)于軌跡2:
從圖4a)可見(jiàn),R1、R2、R3機(jī)器人沿軌跡2運(yùn)動(dòng)時(shí)保持了比較理想的編隊(duì)。
從圖4b)可見(jiàn),機(jī)器人編隊(duì)沿曲線軌跡2運(yùn)動(dòng)時(shí),觀測(cè)角估計(jì)誤差幾乎是零,即使在運(yùn)動(dòng)軌跡發(fā)生變化時(shí),最大誤差是-0. 000 6。
從圖4c)可見(jiàn),機(jī)器人編隊(duì)沿曲線軌跡2運(yùn)動(dòng)時(shí),方向角估計(jì)誤差最大是-0. 002 862。
從圖4d)可見(jiàn),機(jī)器人沿軌跡2運(yùn)動(dòng)時(shí),R2的速度變化范圍是0. 904 6~1. 402; R3的速度變化范圍是0. 848 2~1. 392。
對(duì)于軌跡3:
從圖5a)可見(jiàn),R1、R2、R3機(jī)器人在軌跡3運(yùn)動(dòng)時(shí)保持了比較理想的編隊(duì)。
從圖5b)可見(jiàn),機(jī)器人編隊(duì)沿軌跡3運(yùn)動(dòng)時(shí),觀測(cè)角估計(jì)誤差幾乎是零。
從圖5c)可見(jiàn),機(jī)器人編隊(duì)沿軌跡3運(yùn)動(dòng)時(shí),方向角估計(jì)誤差收斂速度比較快,在t= 1 s時(shí)刻幾乎收斂到零。
從圖5d)可見(jiàn),機(jī)器人沿軌跡3運(yùn)動(dòng)時(shí),跟隨機(jī)器人速度呈現(xiàn)增大趨勢(shì)。
圖4 仿真結(jié)果
圖5 仿真結(jié)果
本文提出了一種跟隨機(jī)器人(followers)觀測(cè)領(lǐng)航機(jī)器人(leader)純角度信息的leader-followers編隊(duì)控制方法?;诜蔷€性系統(tǒng)的可觀測(cè)性理論,分析了機(jī)器人編隊(duì)的可觀測(cè)性;應(yīng)用UKF算法對(duì)leader-followers系統(tǒng)的狀態(tài)進(jìn)行估計(jì);設(shè)計(jì)了輸入-輸出狀態(tài)反饋控制規(guī)律穩(wěn)定編隊(duì)。仿真結(jié)果表明,多個(gè)機(jī)器人能快速形成編隊(duì),并以較小的誤差做復(fù)雜軌跡運(yùn)動(dòng),保持了比較理想的隊(duì)形。后續(xù)研究將包括實(shí)現(xiàn)多機(jī)器人隊(duì)形變換及避障方法。
參考文獻(xiàn):
[1]譚民,王碩.機(jī)器人技術(shù)研究進(jìn)展[J].自動(dòng)化學(xué)報(bào),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]楊麗,曹志強(qiáng),譚民.不確定環(huán)境下多機(jī)器人的動(dòng)態(tài)編隊(duì)控制[J].機(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
作者簡(jiǎn)介:韓青(1973—),西北工業(yè)大學(xué)講師、博士研究生,主要從事智能機(jī)器人、多機(jī)器人協(xié)調(diào)控制與機(jī)電一體化與機(jī)電控制技術(shù)的研究。
收稿日期:2014-10-08基金項(xiàng)目:國(guó)家自然科學(xué)基金(51475383、51075337)資助
文章編號(hào):1000-2758(2015) 02-0244-07
文獻(xiàn)標(biāo)志碼:A
中圖分類號(hào):TP24