基于勢(shì)函數(shù)的多機(jī)器人編隊(duì)協(xié)調(diào)控制模型仿真

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

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

1 引言

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

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

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

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

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

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

(1)

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

L=y2|ko-I|-μ

(2)

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

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

(3)

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

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

(4)

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

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

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

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

(5)

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

(6)

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

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

(7)

式中,A3表示信號(hào)特征值;cosq1表示信號(hào)幅值。

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

3.2 Backstepping控制器

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

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

(8)

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

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

γ=ω(xref×yref)

(9)

式中,ω表示編隊(duì)統(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)動(dòng)軌跡偏離目標(biāo)運(yùn)動(dòng)軌跡?？刂撇呗云谕轿坏臄?shù)學(xué)表達(dá)式如下

(11)

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

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

E′=v11

(12)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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