張 軍,李林森,解乃軍,溫秀平

(1. 南京工程學(xué)院,江蘇 南京 210016;2. 上海交通大學(xué)電子信息與電氣工程學(xué)院,上海 閔行 200240)

1 引言

相較于工作能力有限的單機(jī)器人,多機(jī)器人編隊無論是在工作能力方面,還是在工作效率方面,均表現(xiàn)出一定的優(yōu)勢,因此,多機(jī)器人編隊已成為當(dāng)下社會的熱議話題,被廣泛應(yīng)用于醫(yī)療衛(wèi)生、機(jī)械制造、航空航天、災(zāi)難救援、軍事演練等多個領(lǐng)域[1,2]。然而,多機(jī)器人編隊的應(yīng)用存在兩面性,即多機(jī)器人編隊在給人類生活帶來便利的同時,其編隊協(xié)調(diào)能力不足也會直接導(dǎo)致機(jī)器人隊形散亂、步調(diào)不統(tǒng)一,甚至產(chǎn)生嚴(yán)重的障礙物碰撞問題。為了改善該問題,國內(nèi)外相關(guān)人員對多機(jī)器人編隊協(xié)調(diào)控制方法展開研究。

彭滔[3]等人通過分析多機(jī)器人編隊運(yùn)動狀態(tài)獲取多機(jī)器人編隊運(yùn)動學(xué)特性,并利用徑向基函數(shù)將多機(jī)器人編隊運(yùn)動學(xué)特性映射至神經(jīng)網(wǎng)絡(luò)中。經(jīng)過映射的神經(jīng)網(wǎng)絡(luò)根據(jù)李雅普諾夫穩(wěn)定性理論收斂網(wǎng)絡(luò)邊界,直至網(wǎng)絡(luò)內(nèi)“距離-角度”呈現(xiàn)自適應(yīng)閉環(huán)狀態(tài),此時的神經(jīng)網(wǎng)絡(luò)已脫離未知信息描述階段,成為描述多機(jī)器人編隊打滑和不打滑狀態(tài)的控制性網(wǎng)絡(luò),將多機(jī)器人編隊相關(guān)數(shù)據(jù)輸入該網(wǎng)絡(luò)中,即可實(shí)現(xiàn)多機(jī)器人編隊協(xié)調(diào)控制。師五喜[4]等人通過虛擬領(lǐng)航-跟隨裝置,讀取多機(jī)器人編隊在全局坐標(biāo)系下的運(yùn)動學(xué)模型,通過將模型與編隊控制器結(jié)合,實(shí)現(xiàn)多機(jī)器人編隊協(xié)調(diào)控制。吳軍成[5]等人通過類鳥群模擬方法將多機(jī)器人編隊置于全局坐標(biāo)系中,并利用控制器協(xié)調(diào)變換編隊坐標(biāo)信息,實(shí)現(xiàn)多機(jī)器人編隊協(xié)調(diào)控制。

當(dāng)隊形發(fā)生切換時,上述方法均存在協(xié)調(diào)控制性能較差的問題,為此,本文提出一種基于勢函數(shù)的多機(jī)器人編隊協(xié)調(diào)控制方法。

2 多機(jī)器人編隊數(shù)學(xué)模型

多機(jī)器人編隊數(shù)學(xué)模型是將單機(jī)器人視為二維平面上運(yùn)動的質(zhì)點(diǎn),并從速度向量、位置向量和間距向量三個方面描述多機(jī)器人編隊運(yùn)動的虛擬模型。想要協(xié)調(diào)控制多機(jī)器人編隊,首先需要建立闡述多機(jī)器人編隊運(yùn)動的多機(jī)器人編隊數(shù)學(xué)模型[6,7]。

一致性協(xié)議[8]是Reynolds教授于1986年參考自然生態(tài)環(huán)境中群居鳥類行為特點(diǎn)提出的一種模態(tài)理論。該理論認(rèn)為多個個體組成群體后,群體會在附加增益的作用下自發(fā)產(chǎn)生分離、匹配、聚合等編隊效應(yīng)。分離編隊效應(yīng)是指個體通過斥力場函數(shù)與鄰近個體保持不接觸、不碰撞的位置關(guān)系。匹配編隊效應(yīng)是指個體通過SLAM(simultaneous localization and mapping)算法[9]維持與其它個體同等運(yùn)動速度,以保證群體運(yùn)動的一致性。聚合編隊效應(yīng)是指個體根據(jù)線性引力論[10]與鄰近個體保持相對距離,使群體朝向同一目標(biāo)共同運(yùn)動,且不會出現(xiàn)個體離隊或掉隊的問題。根據(jù)上述理論,給出斥力場函數(shù)的表達(dá)式如下

(1)

SLAM算法的表達(dá)式如下

L=y2|ko-I|-μ

(2)

式中,y2表示個體速度耗散力;ko表示匹配編隊控制條件;I表示群體狀態(tài)同步程度;μ表示附加增益值。

線性引力論的表達(dá)式如下

(3)

式中,ψ表示引力均值;r表示聚合編隊控制條件;mi表示個體間的數(shù)據(jù)交換量;B表示負(fù)反饋比例調(diào)節(jié)因子。

將單機(jī)器人視為一致性協(xié)議中的個體,通過組織多個單機(jī)器人形成編隊,實(shí)現(xiàn)多機(jī)器人編隊數(shù)學(xué)模型的構(gòu)建。多機(jī)器人編隊數(shù)學(xué)模型的表達(dá)式如下

(4)

式中,Os表示多機(jī)器人編隊運(yùn)動學(xué)特征;?z表示機(jī)器人間的排斥力;xo表示機(jī)器人間的吸引力;xn表示多機(jī)器人編隊期望速度分量;σ2表示多機(jī)器人編隊幾何中心;b表示多機(jī)器人編隊最大步長。

3 多機(jī)器人編隊協(xié)調(diào)控制

3.1 領(lǐng)導(dǎo)者-跟隨者狀態(tài)

勢函數(shù)又稱二重勢函數(shù)法[11],是Khatib教授在法國攻讀博士學(xué)位期間提出的多機(jī)器人編隊數(shù)學(xué)模型狀態(tài)重構(gòu)方法。該函數(shù)自1980年提出以來,先后被美國Stanford大學(xué)、印度國際會議列為最具影響力的機(jī)器人科技研究成果。勢函數(shù)的核心思路是通過在多機(jī)器人編隊數(shù)學(xué)模型中擇選出一個領(lǐng)導(dǎo)型機(jī)器人,并從運(yùn)動初始時刻開始要求該機(jī)器人攜帶六維力傳感器[12]和輔助平衡輪,以達(dá)到維持編隊速度分量和隊形統(tǒng)一的目的。除領(lǐng)導(dǎo)型機(jī)器人外的其余機(jī)器人統(tǒng)稱為跟隨型機(jī)器人,這種機(jī)器人不存在數(shù)量限制,即編隊容量的大小并不影響整體編隊的協(xié)調(diào)控制效果。勢函數(shù)擇選領(lǐng)導(dǎo)型機(jī)器人并不是隨機(jī)操作,而是在徑向滑動和側(cè)向滑動兩種復(fù)雜環(huán)境下,根據(jù)模型內(nèi)機(jī)器人雙輪驅(qū)動連線與障礙物位置產(chǎn)生的偏離角度判斷機(jī)器人是否符合擇選條件。只有當(dāng)偏離角度小于30度時,才能確定該機(jī)器人能夠在任意工況環(huán)境下作出合理運(yùn)動,且不存在與障礙物對峙的死鎖[13]問題。偏離角度的計算公式如下

(5)

領(lǐng)導(dǎo)型機(jī)器人的成功擇選對多機(jī)器人編隊數(shù)學(xué)模型協(xié)調(diào)控制起到基礎(chǔ)性作用。在編隊尚未區(qū)分機(jī)器人屬性時,各機(jī)器人間并不存在直接或間接的物理聯(lián)系,而勢函數(shù)的加入使模型重新審視各機(jī)器人之間的關(guān)系。領(lǐng)導(dǎo)型機(jī)器人作為編隊的領(lǐng)導(dǎo)者,不僅可以通過六維力傳感器向任意機(jī)器人施加作用信號或接收任意機(jī)器人的反饋信號,還可以通過輔助平衡輪在一定距離和角度內(nèi)緩解編隊變形趨勢。跟隨型機(jī)器人作為編隊的跟隨者,主要將領(lǐng)導(dǎo)型機(jī)器人視為軸線所在直線上持續(xù)運(yùn)動的跟蹤對象,并在跟蹤對象的指導(dǎo)下完成編隊需要執(zhí)行的目標(biāo)任務(wù)。六維力傳感器發(fā)送信號的表達(dá)式如下

(6)

式中,Δs′表示信號波形;ti表示信號恒收斂系數(shù);tj表示六維力傳感器演化時間。

六維力傳感器接收信號的表達(dá)式如下

(7)

式中,A3表示信號特征值;cosq1表示信號幅值。

輔助平衡輪要想緩解編隊變形趨勢,需要從距離和打滑量兩方面入手,一方面,縮減兩驅(qū)動輪轉(zhuǎn)速不等的機(jī)器人距離,另一方面,沿編隊軸線重合方向降低機(jī)器人打滑量。在勢函數(shù)的協(xié)助下,多機(jī)器人編隊數(shù)學(xué)模型由初始散亂狀態(tài)轉(zhuǎn)化為有序的領(lǐng)導(dǎo)者-跟隨者狀態(tài),極大程度提升了多機(jī)器人編隊協(xié)調(diào)控制的操作空間。

3.2 Backstepping控制器

想要從根本上提升多機(jī)器人編隊隊形穩(wěn)定性和靈活變換性,實(shí)現(xiàn)多機(jī)器人編隊協(xié)調(diào)控制,需要將處于領(lǐng)導(dǎo)者-跟隨者狀態(tài)的多機(jī)器人編隊數(shù)學(xué)模型與Backstepping控制器[14]結(jié)合,根據(jù)控制器輸出的控制策略,針對性調(diào)節(jié)多機(jī)器人編隊隊形參數(shù)。

Backstepping控制器是由上層SDK通信接口[15]和下層領(lǐng)導(dǎo)者-跟隨者控制中心構(gòu)成的雙層機(jī)器人約束系統(tǒng)?？刂破魃蠈油ㄟ^讀取多機(jī)器人編隊完整移動空間,將空間內(nèi)障礙物坐標(biāo)通過無線通信的方式記錄在matlab/simulink仿真平臺。障礙物坐標(biāo)作為機(jī)器人虛擬位姿變換的定點(diǎn)坐標(biāo),只有在多機(jī)器人編隊遵循指定隊形參數(shù),并跟蹤領(lǐng)導(dǎo)型機(jī)器人主軌跡的前提下,才能以多機(jī)器人編隊理想拓?fù)浣Y(jié)構(gòu)跨越定點(diǎn)坐標(biāo)。考慮到多機(jī)器人編隊數(shù)學(xué)模型既適用于純滾動復(fù)雜環(huán)境,又適用于純滑動復(fù)雜環(huán)境,因此,控制器上層在完成障礙物坐標(biāo)記錄工作后,會根據(jù)matlab/simulink仿真平臺記錄結(jié)果制定多機(jī)器人編隊控制策略。編隊理想拓?fù)浣Y(jié)構(gòu)的數(shù)學(xué)表達(dá)式如下

(8)

式中,ux表示編隊質(zhì)心加速度;λij表示多機(jī)器人間的安全距離;zx表示編隊對齊規(guī)則;φij表示編隊分離規(guī)則。

控制器下層是Backstepping控制器的執(zhí)行層,其根據(jù)上層控制策略,從領(lǐng)導(dǎo)型機(jī)器人和跟隨型機(jī)器人兩方面入手,開始對多機(jī)器人編隊協(xié)調(diào)控制,針對領(lǐng)導(dǎo)型機(jī)器人的協(xié)調(diào)控制主要集中在領(lǐng)航速度和領(lǐng)航方位上。根據(jù)控制策略的期望速度調(diào)整領(lǐng)航速度,避免領(lǐng)導(dǎo)型機(jī)器人脫離編隊匹配位置,從而導(dǎo)致編隊在短時間內(nèi)發(fā)生速度控制量時變。控制策略期望速度的數(shù)學(xué)表達(dá)式如下

γ=ω(xref×yref)

(9)

式中,ω表示編隊統(tǒng)一速度;xref表示速度分布控制律;yref表示速度協(xié)調(diào)策略。

領(lǐng)航速度的調(diào)節(jié)公式如下

H=rirjcos(rθ)

(10)

式中,ri表示領(lǐng)航線速度閾值;rj表示領(lǐng)航角速度閾值;rθ表示領(lǐng)航速度調(diào)節(jié)常量。

根據(jù)控制策略的期望方位調(diào)整領(lǐng)航方位,避免領(lǐng)導(dǎo)型機(jī)器人的運(yùn)動軌跡偏離目標(biāo)運(yùn)動軌跡?？刂撇呗云谕轿坏臄?shù)學(xué)表達(dá)式如下

(11)

式中,ΔE表示方位分布控制律;Pp′表示方位協(xié)調(diào)策略。

領(lǐng)航方位的調(diào)節(jié)公式如下

E′=v11

(12)

式中,v1表示下步方位信息;1表示多機(jī)器人編隊角度裕量。

針對跟隨型機(jī)器人的協(xié)調(diào)控制主要集中在跟隨關(guān)系的全局一致性上,根據(jù)控制策略的期望一致性加強(qiáng)跟隨關(guān)系的全局一致性,使多機(jī)器人編隊無論在隊形變換方面,還是在運(yùn)動速度方面,均能夠高度協(xié)調(diào)。

綜上所述,根據(jù)協(xié)調(diào)控制器輸出的控制策略,即可實(shí)現(xiàn)多機(jī)器人編隊的協(xié)調(diào)控制。

4 實(shí)驗(yàn)與分析

為了驗(yàn)證基于勢函數(shù)的多機(jī)器人編隊協(xié)調(diào)控制方法仿真的整體有效性,需要對其測試。

4.1 實(shí)驗(yàn)條件設(shè)置

實(shí)驗(yàn)中所用機(jī)器人型號為IRB6700-150/3.2,軸數(shù)為6軸,有效載荷為150kg,重復(fù)定位精度為±0.1mm。選取5個該型號的機(jī)器人作為實(shí)驗(yàn)對象,進(jìn)行編隊協(xié)調(diào)控制。采用上文提到的matlab/simulink仿真平臺對機(jī)器人的運(yùn)動軌跡進(jìn)行采集,獲取多機(jī)器人運(yùn)動的隊形變化情況,圖1為仿真界面輸出結(jié)果。

圖1 仿真實(shí)驗(yàn)結(jié)果輸出圖

在上述實(shí)驗(yàn)環(huán)境下,將文獻(xiàn)[3]方法、文獻(xiàn)[4]方法、文獻(xiàn)[5]方法和所提方法進(jìn)行對比,得出相關(guān)結(jié)論。

4.2 控制效果驗(yàn)證

隨機(jī)選擇一組包含5個機(jī)器人的多機(jī)器人編隊作為驗(yàn)證算法隊形保持能力的試驗(yàn)對象。將多機(jī)器人編隊置于直角坐標(biāo)系中,其初始位置及初始隊形如下圖2所示。

圖2 多機(jī)器人編隊初始位置及初始隊形

分別采用所提方法、文獻(xiàn)[3]方法和文獻(xiàn)[4]方法協(xié)調(diào)控制多機(jī)器人編隊,使其從初始位置移動一段距離。通過觀察不同方法控制下,編隊運(yùn)動軌跡和隊形保持情況,判斷不同方法對多機(jī)器人編隊的協(xié)調(diào)控制性能。不同方法控制下,編隊運(yùn)動軌跡和隊形保持情況如下圖3所示。

圖3 不同方法控制下編隊運(yùn)動軌跡和隊形保持情況

如上圖3可見,采用所提方法協(xié)調(diào)控制多機(jī)器人編隊時,其編隊運(yùn)動軌跡較短,且編隊內(nèi)各機(jī)器人步調(diào)統(tǒng)一,隊形保持情況較好。而采用文獻(xiàn)[3]方法和文獻(xiàn)[4]方法協(xié)調(diào)控制多機(jī)器人編隊時,二者編隊運(yùn)動軌跡散亂,且編隊內(nèi)各機(jī)器人步調(diào)不統(tǒng)一,隊形保持情況較差。經(jīng)上述對比,可知所提方法對多機(jī)器人編隊的協(xié)調(diào)控制能力明顯優(yōu)于傳統(tǒng)方法。因?yàn)樗岱椒▽⒍鄼C(jī)器人編隊數(shù)學(xué)模型與Backstepping控制器結(jié)合,根據(jù)控制器輸出的控制策略實(shí)現(xiàn)多機(jī)器人編隊協(xié)調(diào)控制,這樣獲取的控制結(jié)果可信度更高、效果更好。

為了進(jìn)一步驗(yàn)證所提方法的實(shí)用性,現(xiàn)規(guī)定多機(jī)器人編隊每間隔1min切換一次指定隊形(指定隊形包括直線形和三角形)。分別采用所提方法、文獻(xiàn)[4]方法和文獻(xiàn)[5]方法協(xié)調(diào)控制多機(jī)器人編隊,通過觀察不同方法控制下編隊隊形切換及保持情況,判斷不同方法對多機(jī)器人編隊的協(xié)調(diào)控制能力。不同方法控制下編隊隊形切換及保持情況如下圖4所示。

圖4 不同方法控制下編隊隊形切換及保持情況

如上圖4可見,在隊形保持方面,所提方法、文獻(xiàn)[4]方法和文獻(xiàn)[5]方法的控制效果均較好,而在隊形切換控制中,所提方法的隊形切換及保持效果更好,沒有出現(xiàn)。采用文獻(xiàn)[4]方法和文獻(xiàn)[5]方法協(xié)調(diào)控制多機(jī)器人編隊,二者隊形切換及保持效果不佳。經(jīng)上述對比,進(jìn)一步驗(yàn)證了所提方法的實(shí)用性。

5 結(jié)束語

多機(jī)器人編隊作為融合多項跨專業(yè)學(xué)科的先進(jìn)技術(shù),因其優(yōu)越的命令執(zhí)行能力,被廣泛應(yīng)用于巡邏、搜救、偵察等領(lǐng)域。為了提高多機(jī)器人編隊控制效果,提出基于勢函數(shù)的多機(jī)器人編隊協(xié)調(diào)控制方法。仿真實(shí)驗(yàn)結(jié)果表明,該方法不僅能夠?qū)Χ鄼C(jī)器人編隊進(jìn)行有效保持,而且具有良好的隊形切換控制效果。如何在保證多機(jī)器人協(xié)調(diào)控制性能的同時,對多機(jī)器人協(xié)調(diào)控制過程實(shí)時監(jiān)控,是研究人員下一步工作的重點(diǎn)。