王 艷，金 晶，黃 晨

(哈爾濱工業(yè)大學(xué)控制科學(xué)與工程系，黑龍江哈爾濱 150001)

多機(jī)器人編隊(duì)控制的研究是多機(jī)器人協(xié)調(diào)問(wèn)題研究的基礎(chǔ)，具有典型性和通用性。本文對(duì)常用的多機(jī)器人編隊(duì)算法進(jìn)行了研究并加以改進(jìn)，設(shè)計(jì)出一種融合了多種機(jī)器人編隊(duì)算法優(yōu)點(diǎn)的新算法。我們?cè)诶碚摲抡骝?yàn)證的基礎(chǔ)上，通過(guò)自主搭建的群體智能移動(dòng)機(jī)器人系統(tǒng)進(jìn)行了實(shí)物仿真驗(yàn)證，展現(xiàn)出新編隊(duì)算法的通用性和有效性。

1 編隊(duì)算法概述

常用的多機(jī)器人編隊(duì)算法有領(lǐng)航跟隨法、人工勢(shì)場(chǎng)法和基于行為法等。

1.1 領(lǐng)航跟隨法

領(lǐng)航跟隨法的基本思想是:在多機(jī)器人組成的群體中，某個(gè)機(jī)器人被指定為領(lǐng)航者，其余作為它的跟隨者。跟隨者以一定的距離間隔跟蹤領(lǐng)航者的位置和方向，該方法可進(jìn)行擴(kuò)展，即不僅可以指定一個(gè)領(lǐng)航者，也可以指定多個(gè)，但群體隊(duì)形的領(lǐng)航者只有一個(gè)。根據(jù)領(lǐng)航者與跟隨者之間的相對(duì)位置關(guān)系，就可以形成不同的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)［1］。

1.2 基于行為法

該法首先為機(jī)器人規(guī)定一些期望的基本行為，一般情況下，機(jī)器人的行為包括躲避障礙、避免碰撞、駛向目標(biāo)和保持隊(duì)形等，當(dāng)機(jī)器人的傳感器接受到外界刺激時(shí)，根據(jù)傳感器的輸入信息做出反應(yīng)，并輸出反應(yīng)量作為該行為的期望反應(yīng)(例如方向和運(yùn)動(dòng)速度)。行為選擇模塊通過(guò)一定的機(jī)制來(lái)綜合各行為的輸出，并將綜合結(jié)果作為機(jī)器人對(duì)環(huán)境刺激的反應(yīng)而輸出［2］。3)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法主要是通過(guò)設(shè)計(jì)人工勢(shì)場(chǎng)和勢(shì)場(chǎng)函數(shù)來(lái)表示環(huán)境以及隊(duì)形中各機(jī)器人之間的約束關(guān)系，并以此為基礎(chǔ)進(jìn)行分析和控制。它的基本思想是機(jī)器人在一個(gè)虛擬的力場(chǎng)中運(yùn)動(dòng)，障礙物被斥力勢(shì)場(chǎng)所包圍，其產(chǎn)生的排斥力隨機(jī)器人與障礙物距離的減少而增大;目標(biāo)點(diǎn)被引力勢(shì)場(chǎng)所包圍，其產(chǎn)生的吸引力隨機(jī)器人與目標(biāo)的接近而減少;在合力的作用下機(jī)器人沿最小化勢(shì)能的方向運(yùn)動(dòng)［3］。

2 編隊(duì)算法的改進(jìn)與仿真研究

2.1 單體機(jī)器人算法研究

1)改進(jìn)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法的優(yōu)點(diǎn)是計(jì)算簡(jiǎn)單，便于實(shí)現(xiàn)實(shí)時(shí)控制，但是勢(shì)場(chǎng)函數(shù)的設(shè)計(jì)比較困難，而且存在局部極值點(diǎn)的問(wèn)題。如圖1(a)所示，如果選擇的勢(shì)場(chǎng)函數(shù)不好，就會(huì)陷入局部最小的情況，從而不能正確的駛向目標(biāo)，形成死點(diǎn)。為了防止機(jī)器人在駛向目標(biāo)的時(shí)候陷入局部最小，在此對(duì)勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn)［4］。主要是在斥力勢(shì)場(chǎng)函數(shù)中引入了機(jī)器人與目標(biāo)之間的相對(duì)距離。

引力場(chǎng)函數(shù)為

其中，K為位置增益，(X－Xgoat)為當(dāng)前位置與目標(biāo)位置的相對(duì)位置，相應(yīng)的吸引力為目標(biāo)勢(shì)場(chǎng)的負(fù)梯度為

斥力勢(shì)場(chǎng)為

其中，l是機(jī)器人與障礙物間的距離，障礙物影響范圍在距離l0之內(nèi)，g是位置增益系數(shù)。斥力可以寫(xiě)成以下形式:

其中，

矢量Frel的方向從障礙物指向機(jī)器人，矢量Fre2的方向從機(jī)器人指向目標(biāo)點(diǎn)。

機(jī)器人所受到的合力為

經(jīng)過(guò)我們改進(jìn)后的人工勢(shì)場(chǎng)法的仿真結(jié)果如圖1(b)所示。根據(jù)仿真可見(jiàn)，改進(jìn)算法可以消除局部最小點(diǎn)，在理論上得到了很好的避障效果。

圖1 人工勢(shì)場(chǎng)法仿真效果

2)跟隨領(lǐng)航者法

由對(duì)攝像頭所采集圖片進(jìn)行實(shí)時(shí)分析所得到的數(shù)據(jù)信息，根據(jù)跟隨領(lǐng)航者法實(shí)現(xiàn)機(jī)器人的搜索目標(biāo)與動(dòng)態(tài)追蹤功能。

我們首先需要對(duì)機(jī)器人進(jìn)行隨動(dòng)系統(tǒng)建模［5］。由實(shí)際參數(shù)計(jì)算可以知道，電機(jī)感應(yīng)電磁系數(shù)Ke為0.0215(V/rad/s)，機(jī)電時(shí)間常數(shù) τm為 0.029s，電磁時(shí)間常數(shù)τe很小，因此可得電機(jī)模型。經(jīng)計(jì)算簡(jiǎn)化得

我們定義輸入為電壓，輸出為機(jī)器人的位置信息，因此機(jī)器人的運(yùn)動(dòng)的最終模型為

接下來(lái)通過(guò)Simulink設(shè)計(jì)PID控制器來(lái)觀察該模型的跟蹤情況，圖2是系統(tǒng)控制圖。

圖2 系統(tǒng)仿真框圖

當(dāng)輸入為階躍信號(hào)時(shí)，系統(tǒng)的跟蹤效果和跟蹤誤差如圖3所示。跟蹤頻率為1Hz的正弦信號(hào)的跟蹤效果和跟蹤誤差如圖4所示。可見(jiàn)，其穩(wěn)態(tài)誤差小于3%。

圖3 階躍信號(hào)跟蹤和誤差仿真效果

圖4 正弦信號(hào)跟蹤效果和誤差仿真效果

當(dāng)輸入為單位階躍信號(hào)，干擾幅值為10時(shí)的系統(tǒng)階躍響應(yīng)和響應(yīng)誤差如圖5所示。由圖可見(jiàn)系統(tǒng)在加入干擾后響應(yīng)依然穩(wěn)定，穩(wěn)態(tài)誤差在3%以?xún)?nèi)。

圖5 加擾動(dòng)后階躍響應(yīng)效果和誤差

當(dāng)輸入幅值為1的正弦信號(hào)，干擾幅值為10時(shí)的系統(tǒng)正弦響應(yīng)和跟蹤誤差如圖6所示。圖中顯示當(dāng)輸入為正弦信號(hào)時(shí)，系統(tǒng)仍具有較好的抗干擾能力?？梢?jiàn)干擾對(duì)其影響很小，可不予考慮。

圖6 加擾動(dòng)后正弦響應(yīng)效果和誤差

通過(guò)以上仿真可以知道，該系統(tǒng)的穩(wěn)態(tài)誤差小于3%，并且有很好的抑制干擾的能力，當(dāng)加入階躍為10的干擾后響應(yīng)依然穩(wěn)定，穩(wěn)態(tài)誤差在3%以?xún)?nèi)。所以可以得出通過(guò)PID控制可以使機(jī)器人的視覺(jué)動(dòng)態(tài)追蹤達(dá)到很好的跟蹤效果。

2.2 群體機(jī)器人算法研究

基于單體移動(dòng)機(jī)器人的避障和跟蹤兩個(gè)基本功能，本實(shí)驗(yàn)平臺(tái)群體編隊(duì)功能采用跟隨領(lǐng)航者法、基于行為法和人工勢(shì)場(chǎng)法三種方法綜合而成的融合算法實(shí)現(xiàn)。

基于人工勢(shì)場(chǎng)法實(shí)現(xiàn)單體移動(dòng)機(jī)器人的實(shí)時(shí)避碰行為。基于跟隨領(lǐng)航者法設(shè)定一個(gè)領(lǐng)航機(jī)器人，若干個(gè)跟隨機(jī)器人，實(shí)現(xiàn)跟蹤目標(biāo)行為。領(lǐng)航機(jī)器人的行為主要就是躲避障礙和駛向目標(biāo)，而跟隨機(jī)器人的行為則包括躲避障礙、保持隊(duì)形和駛向目標(biāo)。

根據(jù)基于行為法實(shí)現(xiàn)融合算法，在每一個(gè)時(shí)間間隔都分別計(jì)算基本行為模式下機(jī)器人下一步的運(yùn)動(dòng)路徑，再加上一定的權(quán)值，求出合力，從而得到機(jī)器人下一步的運(yùn)動(dòng)路徑［6］。根據(jù)基于行為法所形成的控制量與基于領(lǐng)航的協(xié)調(diào)策略相結(jié)合，形成控制器，控制小車(chē)的運(yùn)動(dòng)。該融合算法結(jié)構(gòu)圖如圖7所示。

圖7 多機(jī)器人編隊(duì)結(jié)構(gòu)模型

從實(shí)現(xiàn)來(lái)講，我們?cè)诜抡嬷性O(shè)定一個(gè)機(jī)器人為領(lǐng)航機(jī)器人，其余兩個(gè)機(jī)器人為跟蹤機(jī)器人，領(lǐng)航機(jī)器人的主要運(yùn)動(dòng)軌跡就是避障并且駛向目標(biāo)，而跟隨機(jī)器人則按照一定的編隊(duì)方式(三角形，線(xiàn)形)跟隨領(lǐng)航機(jī)器人，其中還得避開(kāi)障礙物，其效果如圖8所示。

3 編隊(duì)算法實(shí)物驗(yàn)證

3.1 實(shí)驗(yàn)平臺(tái)

硬件平臺(tái)采用自主開(kāi)發(fā)設(shè)計(jì)的ARM嵌入式核心板和外圍電路控制板構(gòu)成。車(chē)體平臺(tái)選用了1:10四驅(qū)電動(dòng)模型遙控車(chē)VH－32作為車(chē)架，將電機(jī)更換為速度稍慢的545SA－2588B型號(hào)電機(jī)。此外擴(kuò)展了Futaba S3003舵機(jī)、URM37V3.2超聲傳感器和ZC301攝像頭。

圖8 移動(dòng)機(jī)器人三角編隊(duì)仿真圖

控制主板以三星公司基于ARM920T核的S3C2440作為CPU核心處理器，圍繞S3C2440設(shè)計(jì)了JTAG及復(fù)位模塊、Nandflash模塊和Norflash模塊和SDRAM模塊。外圍電路設(shè)計(jì)為四部分:串口模塊、USB接口模塊、網(wǎng)卡驅(qū)動(dòng)模塊和電源模塊。

軟件平臺(tái)由LINUX實(shí)時(shí)操作系統(tǒng)、多線(xiàn)程編程技術(shù)、驅(qū)動(dòng)和算法程序構(gòu)成。軟件的設(shè)計(jì)主要是驅(qū)動(dòng)程序和應(yīng)用程序的編寫(xiě)。

驅(qū)動(dòng)程序有:PWM電機(jī)驅(qū)動(dòng)程序、舵機(jī)控制驅(qū)動(dòng)程序、超聲測(cè)距傳感器數(shù)據(jù)采集驅(qū)動(dòng)程序和USB及攝像頭驅(qū)動(dòng)程序。主要是通過(guò)Linux下的字符設(shè)備驅(qū)動(dòng)的編寫(xiě)，完成子函數(shù)并填充file_operations的各個(gè)域，使得應(yīng)用程序可以對(duì)硬件設(shè)備進(jìn)行操作。

應(yīng)用程序設(shè)計(jì)基本可以分為三個(gè)部分:傳感器數(shù)據(jù)采集模塊、圖像分析處理模塊和運(yùn)動(dòng)控制模塊。采用Linux下多線(xiàn)程編程技術(shù)，創(chuàng)建了三個(gè)線(xiàn)程來(lái)運(yùn)行三個(gè)模塊，其中負(fù)責(zé)機(jī)器人的運(yùn)動(dòng)控制模塊的作為主線(xiàn)程。

3.2 實(shí)物驗(yàn)證效果

1)駛向目標(biāo)與實(shí)時(shí)避碰

單體機(jī)器人通過(guò)人工勢(shì)場(chǎng)法實(shí)現(xiàn)了機(jī)器人駛向目標(biāo)和實(shí)時(shí)避碰功能，功能實(shí)現(xiàn)框圖如圖9所示。單體機(jī)器人通過(guò)人工勢(shì)場(chǎng)法實(shí)現(xiàn)躲避障礙物功能實(shí)物效果如圖10所示。

圖9 機(jī)器人避障功能的實(shí)現(xiàn)

2)搜索目標(biāo)與動(dòng)態(tài)追蹤

通過(guò)跟隨領(lǐng)航者法對(duì)攝像頭所采集圖片信息進(jìn)行實(shí)時(shí)分析，實(shí)現(xiàn)機(jī)器人的搜索目標(biāo)與動(dòng)態(tài)追蹤功能，實(shí)現(xiàn)框圖如圖11所示。

群體移動(dòng)機(jī)器人通過(guò)基于行為法將改進(jìn)人工勢(shì)場(chǎng)法和領(lǐng)航跟隨法融合實(shí)現(xiàn)隊(duì)形保持功能效果如圖12所示。機(jī)器人還能在障礙物隨機(jī)分布的環(huán)境下完成多機(jī)器人編隊(duì)，在實(shí)驗(yàn)中，我們采用隨機(jī)行走的人員作為多機(jī)器人編隊(duì)所遭遇障礙物。

圖10 躲避障礙物功能實(shí)物圖

圖11 跟蹤功能實(shí)現(xiàn)框圖

圖12 編隊(duì)實(shí)物效果圖

4 結(jié)語(yǔ)

本文對(duì)人工勢(shì)場(chǎng)法進(jìn)行了有效地改進(jìn)，提出了一種基于領(lǐng)航跟隨法、人工勢(shì)場(chǎng)法和基于行為法的新融合算法。該算法不僅有效地實(shí)現(xiàn)了單體機(jī)器人實(shí)時(shí)避障和動(dòng)態(tài)跟蹤的功能，還適用于全局環(huán)境未知，障礙物隨機(jī)分布條件下的多機(jī)器人編隊(duì)，效果良好，本文對(duì)于多機(jī)器人編隊(duì)的研究工作具有重要的參考借鑒價(jià)值。

［1］ Jaydev P.Desai，Jim Ostrowski，Vijay Kumar.Controlling formations of multiple mobile robots［C］，IEEE International Conference on Robotics and Automation，1998:2864～2869

［2］ BALCH T，ARKIN R C.BehaviorP-based formation control for multi－ robot teams［J］.IEEE Transactions on Robotics and Automation，1998，14(6):926-939

［3］ 賈秋玲，閆建國(guó)，王新民.基于勢(shì)函數(shù)的多機(jī)器人系統(tǒng)的編隊(duì)控制［J］.沈陽(yáng):機(jī)器人，2006:111-114

［4］ 吳鏡開(kāi)，黃遠(yuǎn)燦，王世興.基于勢(shì)場(chǎng)法的移動(dòng)機(jī)器人避障路徑規(guī)劃［J］.北京:微計(jì)算機(jī)信息，2007，23(2):228～230

［5］ 宗光華.機(jī)器人的創(chuàng)意設(shè)計(jì)與實(shí)踐［M］.北京:北京航空航天大學(xué)出版社，2004:1～3

［6］ CAO Z Q，ZHANG B，WANG S.Cooperative hunting of multiple mobile robots in an unknown environment［J］.Acta Automatica Sinica，2003，29(4):536～543