多機(jī)器人編隊(duì)控制模型仿真實(shí)證研究301821引言 721892多機(jī)器人編隊(duì)方法理論研究 8166132.1領(lǐng)航者跟隨法（Leader-follower法） 10184672.2虛擬結(jié)構(gòu)法 11240742.3圖論法 1392462.4基于行為法 15231643避障方法理論研究 17321943.1人工勢場法 17246383.2BUG算法 20137153.3向量直方圖法 20230783.4搜索類算法 21281353.5其他算法 2783593.5.1神經(jīng)網(wǎng)絡(luò)方法 27100003.5.2模糊邏輯方法 28158174編隊(duì)控制模型理論分析 30107334.1移動(dòng)機(jī)器人動(dòng)力學(xué)模型 3025674.2移動(dòng)機(jī)器人運(yùn)動(dòng)控制 3071304.3移動(dòng)機(jī)器人正多邊形編隊(duì) 31125194.4機(jī)器人群體的形成 32121284.5避障 32254795仿真實(shí)驗(yàn)與結(jié)論分析 38161415.1領(lǐng)航者跟隨法編隊(duì) 38287975.2人工勢場的MATLAB仿真實(shí)現(xiàn) 39195445.3人工勢場法單機(jī)多障礙避障 42296205.4多機(jī)協(xié)同避障 43272775.5A*算法MATLAB仿真 4697716總結(jié)和展望 4876856.1論文主要結(jié)論 48217036.2展望 4814910參考文獻(xiàn) 5032404附錄A人工勢場仿真MATLAB主程序 5323948附錄B多機(jī)協(xié)同避障MATLAB仿真程序 5524105附錄CA*算法MATLAB仿真代碼 59引言有界區(qū)域內(nèi)多機(jī)器人協(xié)同避障策略研究包含了兩大關(guān)鍵性問題。其一是多機(jī)器人的編隊(duì)問題，其二是路徑規(guī)劃避障問題。多機(jī)器人的編隊(duì)協(xié)同控制問題，是研究多移動(dòng)機(jī)器人領(lǐng)域首要解決的。其內(nèi)涵有三，第一，選取多機(jī)器人編隊(duì)方法，解決多機(jī)器人的編隊(duì)隊(duì)形；其二，多機(jī)器人群體形成設(shè)計(jì)好的隊(duì)形，要考慮在不涉及障礙的情況下保持運(yùn)動(dòng)穩(wěn)定；最后，研究機(jī)器人的避障行為，即應(yīng)考慮到在運(yùn)動(dòng)過程中與外界障礙物或編隊(duì)中臨近機(jī)器人間保持安全裕度。機(jī)器人避障是移動(dòng)機(jī)器人技術(shù)中一項(xiàng)最基礎(chǔ)也是極關(guān)鍵的功能，避障REF_Ref15709\r\h[1]是指機(jī)器人在行走過程中，通過傳感器獲取搜索范圍內(nèi)影響機(jī)器人下一步移動(dòng)的障礙物位置信息，在受到障礙影響前行的情況下，根據(jù)避障算法更改既有前行路徑，從而消除障礙物對(duì)機(jī)器人向目標(biāo)點(diǎn)前進(jìn)的負(fù)影響，最終到達(dá)目標(biāo)點(diǎn)。機(jī)器人避障技術(shù)的核心包括了傳感器的選擇和避障算法以及策略的選取。根據(jù)實(shí)際需要，所選取的不同的傳感器有著不同特色以及原理，而采用的不同的算法所需要的執(zhí)行時(shí)間也不同。本課題著重點(diǎn)在于算法策略上的分析，不深入探討傳感器的相關(guān)方面。故而更多的是對(duì)多機(jī)器人編隊(duì)方法及所采取的避障策略進(jìn)行闡述。避障是機(jī)器人行為決策的一個(gè)重要考量因素，有多種算法，策略設(shè)計(jì)針對(duì)機(jī)器人避障這一課題，如經(jīng)典的人工勢場法，易于程序?qū)崿F(xiàn)的BUG算法，對(duì)傳感器等硬件設(shè)施要求較高的向量直方圖法，更加符合人的思維的模糊邏輯算法、神經(jīng)網(wǎng)絡(luò)方法等。其中人工勢場法，以其具有計(jì)算復(fù)雜度低、實(shí)時(shí)性好、避障路徑平滑等優(yōu)點(diǎn)一直得到人們青睞，在一般障礙環(huán)境情況條件下得到廣泛的研究和應(yīng)用。有界區(qū)域內(nèi)，在非極端環(huán)境條件下人工勢場法有著先天的優(yōu)勢，本文正是基于人工勢場法進(jìn)行了避障的實(shí)驗(yàn)仿真，并給出了一種適合在極端障礙條件下避障的A*算法。彌補(bǔ)了人工勢場所存在的諸如存在局部極小值等問題的不足之處。通過理論分析和MATLAB仿真實(shí)驗(yàn)結(jié)果印證了所提多機(jī)器人協(xié)同避障策略的有效性。多機(jī)器人編隊(duì)方法理論研究編隊(duì)行為是自然界常見的生物集群現(xiàn)象REF_Ref15784\r\h[2]。群居生物在運(yùn)動(dòng)過程中保持一定的隊(duì)形，可以大大提高每個(gè)個(gè)體存活率和尋找食物的效率，降低單體的體能消耗，提高其活動(dòng)范圍（如圖2-1所示）。圖2-1自然界編隊(duì)集群行為受自然界動(dòng)物的編隊(duì)和集群行為諸多優(yōu)點(diǎn)如任務(wù)執(zhí)行效率高、運(yùn)動(dòng)過程穩(wěn)定、降低能量消耗等的啟發(fā)，在人類生活中也越來越多地應(yīng)用了集群編隊(duì)（如圖2-2所示）。軍事領(lǐng)域上，多種不同機(jī)型的飛機(jī)混合編隊(duì)可以通過任務(wù)分配細(xì)化，專精尖的執(zhí)行既定作戰(zhàn)任務(wù)，可以在軍事護(hù)航、協(xié)同一體化作戰(zhàn)、空中加油等復(fù)雜任務(wù)中發(fā)揮不可替代的作用；由各類軍艦、潛艇組成的航母編隊(duì)可以大幅提高航母編隊(duì)的作戰(zhàn)能力和戰(zhàn)場存活能力，發(fā)揮出大規(guī)模集群作戰(zhàn)的戰(zhàn)略威懾力；在人們?nèi)粘？梢姷慕煌ㄟ\(yùn)輸領(lǐng)域，車輛自覺地形成的編隊(duì)可以大幅提高交通通行率以及道路利用率。不僅如此，高速公路上車輛編隊(duì)的形成可以有效降低風(fēng)阻從而減少能源消耗。在多機(jī)器人研究領(lǐng)域，專業(yè)技術(shù)人員也越來越多地將多智能機(jī)器人群體編隊(duì)技術(shù)應(yīng)用到礦物資源勘探、事故災(zāi)害救援等領(lǐng)域。圖2-2國慶70周年飛機(jī)編隊(duì)，航母編隊(duì)人們研究多移動(dòng)機(jī)器人編隊(duì)系統(tǒng)的初衷也是由于單個(gè)機(jī)器人上所能搭載的傳感器數(shù)量有限且探測范圍較小，導(dǎo)致每位“隊(duì)員”數(shù)據(jù)獲取量有限，執(zhí)行任務(wù)能力存在有限性、局限性。通過多機(jī)器人的編隊(duì)，機(jī)器人之間可相互交流信息，一方面大大減少每個(gè)“隊(duì)員”數(shù)據(jù)的獲取量，另一方面可以增加群體對(duì)外界數(shù)據(jù)的采集，從而提高機(jī)器人群體的整體探測精度與任務(wù)執(zhí)行能力，提高群體反應(yīng)速度。多移動(dòng)機(jī)器人系統(tǒng)由多個(gè)能力有限的機(jī)器人組成，提高了工作效率與任務(wù)執(zhí)行效率REF_Ref15918\r\h[3]。與單機(jī)器人相比，多機(jī)器人系統(tǒng)具體有如下優(yōu)勢：（1）環(huán)境適應(yīng)力強(qiáng)：多機(jī)器人系統(tǒng)在靈活性和對(duì)任務(wù)執(zhí)行過程中的變通性得到了大幅提升。由于多機(jī)器人系統(tǒng)擁有環(huán)境的部分或全局信息，因此能夠維護(hù)、加強(qiáng)和調(diào)整它們的定位模式，以便在有限的時(shí)間內(nèi)完成它們的任務(wù)，適應(yīng)性強(qiáng)。不同編隊(duì)隊(duì)形可以適應(yīng)不同場景，在群體執(zhí)行力和空間分布性上顯著優(yōu)于單個(gè)機(jī)器人。針對(duì)不同的復(fù)雜地形設(shè)計(jì)不同的編隊(duì)策略成為編隊(duì)的重要優(yōu)勢之一。（2）任務(wù)承受力強(qiáng)：“人多力量大”，一個(gè)大任務(wù)被分配到每個(gè)成員機(jī)器人上，每個(gè)機(jī)器人執(zhí)行大任務(wù)下的子任務(wù)，這就提高了任務(wù)的承載力REF_Ref15957\r\h[4]。每個(gè)機(jī)器人在完成自己部分的工作任務(wù)的同時(shí)，需配合其他機(jī)器人對(duì)總?cè)蝿?wù)負(fù)責(zé)，有效提高任務(wù)完成的效率，更好地解決復(fù)雜度較高的任務(wù)，保證任務(wù)順利完成。（3）魯棒性高REF_Ref15999\r\h[5]：在多機(jī)器人系統(tǒng)中，任務(wù)進(jìn)行了過程上的劃分，階段性的分配執(zhí)行，由各位子成員共同參與完成，并不由一個(gè)機(jī)器人“說了算”。這樣做的好處是，即使群體中一個(gè)成員出現(xiàn)問題，可以有其它機(jī)器人接管，交由其他機(jī)器人完成相應(yīng)任務(wù)，不導(dǎo)致整個(gè)任務(wù)的全盤失敗。且一旦障礙環(huán)境發(fā)生改變，多機(jī)器人避障系統(tǒng)可以更好地調(diào)整適應(yīng)，具有更強(qiáng)的環(huán)境適應(yīng)能力和更高的系統(tǒng)魯棒性。面對(duì)復(fù)雜的任務(wù)和多變的情況，顯然僅僅依靠單個(gè)機(jī)器人的能力是不夠的，需要多機(jī)器人協(xié)同完成，這就不得不考慮到協(xié)同編隊(duì)以及避障的問題。多機(jī)器人的協(xié)同編隊(duì)控制問題，包含了從形成編隊(duì)隊(duì)形，隊(duì)形的保持，遇障礙隊(duì)形變換這三大關(guān)鍵性問題REF_Ref16039\r\h[6]。多機(jī)器人編隊(duì)方法有領(lǐng)航者跟隨法、虛擬結(jié)構(gòu)法、圖論法以及基于行為法等REF_Ref14308\r\h[7]。領(lǐng)航者跟隨法（Leader-follower法）領(lǐng)航者跟隨法（Leader-follower法）編隊(duì)控制方法是多移動(dòng)機(jī)器人系統(tǒng)中最早得以利用的一種編隊(duì)實(shí)現(xiàn)方法。領(lǐng)航者跟隨法的基本思想是在機(jī)器人群體中選定一個(gè)作為多機(jī)器人編隊(duì)的“領(lǐng)航者”REF_Ref17743\r\h[8]，并以此機(jī)器人為基，建立極坐標(biāo)系，編隊(duì)中其他機(jī)器人設(shè)定為跟隨者，可以建立不同的拓?fù)?。跟隨者機(jī)器人根據(jù)編隊(duì)隊(duì)形的不同以一定的相對(duì)距離l和相對(duì)角度ψ跟隨“領(lǐng)航者”運(yùn)動(dòng)，從而保持穩(wěn)定的隊(duì)形。其中領(lǐng)航者既可以是真實(shí)的個(gè)體成員機(jī)器人，也可以是虛擬的領(lǐng)航機(jī)REF_Ref19157\r\h[9]。在編隊(duì)形成及保持過程中，領(lǐng)航者機(jī)器人的作用是控制整個(gè)機(jī)器人編隊(duì)的前行（如圖2-3所示）。在實(shí)際編隊(duì)行進(jìn)中，需要做到跟隨者機(jī)器人與領(lǐng)航者機(jī)器人之間的實(shí)際距離LLF趨于根據(jù)隊(duì)形要求設(shè)定好的理論間距L，兩者的相對(duì)角度ΨLF趨于理論值Ψ。即實(shí)際距離和實(shí)際方位角要漸進(jìn)穩(wěn)定于期望值。圖2-3領(lǐng)航者跟隨法編隊(duì)模型領(lǐng)航者跟隨法最早于1991年由Wang在文獻(xiàn)REF_Ref20845\r\h[10]提出。在此基礎(chǔ)上Kumar教授及其團(tuán)隊(duì)（GRASP）經(jīng)過理論探索和實(shí)驗(yàn)探索，在其論述REF_Ref27400\r\h[11]中提出了基于領(lǐng)航者跟隨法的l—ψ和l—l構(gòu)型，通過仿真實(shí)驗(yàn)和實(shí)地試驗(yàn)得出的結(jié)論也驗(yàn)證了其所提出的編隊(duì)構(gòu)型和控制方法的有效性。HanzhenXiaoREF_Ref27184\r\h[12]等人采用基于神經(jīng)動(dòng)力學(xué)優(yōu)化的非線性模型預(yù)測控制算法實(shí)現(xiàn)了領(lǐng)航者跟隨法機(jī)器人編隊(duì)，同時(shí)進(jìn)行了編隊(duì)避障的研究。ParkBongSeokREF_Ref28141\r\h[13]等人針對(duì)不確定的非完整移動(dòng)機(jī)器人的主從編隊(duì)跟蹤問題，提出了一種基于連通性保持的避障方法，推導(dǎo)了機(jī)器人之間在避障時(shí)保持領(lǐng)航者和跟隨者之間連通性所需的相對(duì)角度，以制定一種新的保持連通性的避障策略。Jadbabaie等證明了如果每一個(gè)跟隨者都與領(lǐng)航者保持跟隨狀態(tài)，那么隨著時(shí)間的推移，跟隨者機(jī)器人的狀態(tài)將趨于領(lǐng)航者的狀態(tài)REF_Ref28164\r\h[14]。國內(nèi)，南開大學(xué)王中林REF_Ref3651\r\h[15]等人研究了基于離散時(shí)間模型的多智能體領(lǐng)航跟隨編隊(duì)控制算法，并分別通過Matlab仿真和在Amigobot機(jī)器人平臺(tái)上做實(shí)驗(yàn)，驗(yàn)證了該算法的正確性及可行性。領(lǐng)航者跟隨法編隊(duì)控制的優(yōu)點(diǎn)是所需的信息量相對(duì)較少，表現(xiàn)在當(dāng)領(lǐng)航者的運(yùn)動(dòng)狀態(tài)確定后，其余跟隨者就能通過跟隨領(lǐng)航者來實(shí)現(xiàn)編隊(duì)控制，而且控制策略較為靈活。其缺點(diǎn)是，一旦領(lǐng)航者出現(xiàn)故障，那么整個(gè)多機(jī)器人系統(tǒng)就會(huì)無法工作，而且領(lǐng)航者機(jī)器人難以根據(jù)跟隨者機(jī)器人的運(yùn)行狀態(tài)做出相應(yīng)的運(yùn)行狀態(tài)上的調(diào)整，這樣多機(jī)器人系統(tǒng)的魯棒性不強(qiáng)。除此之外，若跟隨者出現(xiàn)網(wǎng)絡(luò)延遲等傳輸延遲問題時(shí)也會(huì)導(dǎo)致跟蹤不及時(shí)，進(jìn)而影響到整個(gè)多機(jī)器人編隊(duì)的控制效果REF_Ref3929\r\h[16]。領(lǐng)航者跟隨法是最常用、最成熟的策略。在有限資源的情況下，該方法有著先天的優(yōu)勢。虛擬結(jié)構(gòu)法早在1997年，M.AnthonyLewis

就給出了虛擬結(jié)構(gòu)的定義REF_Ref4223\r\h[17]，即虛擬結(jié)構(gòu)(VirtualStructure)是元件的集合，例如機(jī)器人，它們彼此之間以及與參考系之間保持(半)剛性幾何關(guān)系。舉例來講，推箱子問題是利用虛擬結(jié)構(gòu)法解決多機(jī)器人協(xié)同編隊(duì)的一個(gè)經(jīng)典演繹。如圖2-4所示，當(dāng)機(jī)器人推動(dòng)一個(gè)盒子在地面做運(yùn)動(dòng)的時(shí)候，要想得到穩(wěn)定的推動(dòng)效果，就要要求多機(jī)器人與盒子保持嚴(yán)格固定的幾何關(guān)系。不嚴(yán)格遵守幾何約束會(huì)導(dǎo)致荷載不均勻，甚至當(dāng)機(jī)器人沿著盒子滑動(dòng)或失去與盒子的聯(lián)系時(shí)會(huì)發(fā)生推動(dòng)失敗。解決這個(gè)問題的一種方法是，將多機(jī)器人群體中每個(gè)機(jī)器人想象成是嵌入盒子結(jié)構(gòu)中的粒子。實(shí)際結(jié)構(gòu)中通常機(jī)械施加的約束是由控制策略施加的。多個(gè)機(jī)器人可以利用與箱子的運(yùn)動(dòng)并行性、一致性增強(qiáng)它們的隊(duì)形保持能力。由于每個(gè)機(jī)器人的不論如何運(yùn)動(dòng)，但其與盒子的位置始終保持相對(duì)固定，就好像它們嵌入了這個(gè)箱子一樣。稱之為虛擬結(jié)構(gòu)。圖2-4多個(gè)移動(dòng)機(jī)器人推動(dòng)盒子虛擬結(jié)構(gòu)模型中根據(jù)多機(jī)器人編隊(duì)為每個(gè)機(jī)器人設(shè)定了相應(yīng)的虛擬個(gè)體，設(shè)置了一種虛擬框架結(jié)構(gòu)，虛擬個(gè)體的位姿即為實(shí)際機(jī)器人的期望位姿。多機(jī)器人編隊(duì)在整體運(yùn)行時(shí)，雖然每個(gè)機(jī)器人的絕對(duì)坐標(biāo)發(fā)生了變化，但機(jī)器人間的相對(duì)坐標(biāo)卻保持不變。把每個(gè)機(jī)器人看作剛體結(jié)構(gòu)中的一個(gè)固定點(diǎn)，所以只需要知道一個(gè)機(jī)器人的運(yùn)動(dòng)，然后通過機(jī)器人之間的幾何關(guān)系，就可以推導(dǎo)出編隊(duì)中其他機(jī)器人的運(yùn)動(dòng)狀態(tài)，從而達(dá)到控制編隊(duì)隊(duì)形的效果。在應(yīng)用虛擬結(jié)構(gòu)法編隊(duì)思想時(shí)。首先，依據(jù)任務(wù)所需，提前編排好多機(jī)器人隊(duì)形，采用相應(yīng)的編隊(duì)隊(duì)形結(jié)構(gòu)。接著，通過形成的整個(gè)隊(duì)形得出每個(gè)機(jī)器人的相對(duì)位置，繼而得出出每個(gè)機(jī)器人應(yīng)該完成的相應(yīng)運(yùn)動(dòng)。最后，為每個(gè)機(jī)器人設(shè)計(jì)合適群體編隊(duì)行為的運(yùn)動(dòng)軌跡。如圖2-5所示，三輛移動(dòng)機(jī)器人依據(jù)三角形虛擬結(jié)構(gòu)，形成三角形編隊(duì)。圖2-5基于虛擬結(jié)構(gòu)的編隊(duì)南京航空航天大學(xué)的何真等人提出了一種基于虛擬結(jié)構(gòu)的分布式編隊(duì)控制方法用于編隊(duì)運(yùn)動(dòng)控制REF_Ref6166\r\h[18]。以虛擬結(jié)構(gòu)為框架，在編隊(duì)控制算法中加入隊(duì)形反饋，使得虛擬結(jié)構(gòu)法設(shè)計(jì)的編隊(duì)控制器既能控制編隊(duì)機(jī)動(dòng)運(yùn)動(dòng)速度，又能在編隊(duì)運(yùn)行過程中較好地維持設(shè)定的隊(duì)形。實(shí)驗(yàn)證明了該編隊(duì)控制方法的收斂性。針對(duì)無人機(jī)編隊(duì)控制問題，天津大學(xué)的李正平等人提出了一種基于虛擬結(jié)構(gòu)法的非線性魯棒控制算法REF_Ref6365\r\h[19]并基于Lyapunov穩(wěn)定性分析方法證明了閉環(huán)系統(tǒng)的穩(wěn)定性。虛擬結(jié)構(gòu)法的優(yōu)點(diǎn)是降低了任務(wù)分配的難度和工作量，其控制規(guī)律的設(shè)計(jì)通常需要考慮反饋環(huán)節(jié)來消除編隊(duì)誤差，并且由于其剛性結(jié)構(gòu)，有良好的隊(duì)形反饋，編隊(duì)隊(duì)形形成起來較為輕松，且編隊(duì)能夠較為迅速的達(dá)到穩(wěn)定。另外易于設(shè)計(jì)每個(gè)機(jī)器人的運(yùn)動(dòng)軌跡，且隊(duì)形運(yùn)動(dòng)軌跡具有較高的精度。但剛性的結(jié)構(gòu)也使他缺乏靈活性，一旦需要變化隊(duì)形就必須重新設(shè)計(jì)虛擬剛體編隊(duì)以及各個(gè)機(jī)器人的控制規(guī)律。此外若機(jī)器人數(shù)量較多時(shí)，不僅設(shè)計(jì)控制規(guī)律會(huì)很繁瑣，而且系統(tǒng)穩(wěn)定性較難保證，魯棒性和適應(yīng)性也不佳。對(duì)機(jī)器人間的相對(duì)位置要求苛刻，實(shí)用性往往不夠，穩(wěn)定性不高，且對(duì)通信和機(jī)載計(jì)算能力要求高。圖論法圖論法是以圖作為研究對(duì)象的方法。圖論法的提出由來已久，早在18世紀(jì)，大數(shù)學(xué)家歐拉為了解決著名的哥尼斯堡七橋問題（如圖2-6所示），探究是否可以不重復(fù)的走過點(diǎn)之間的連線而完整地走過所有路徑。從而創(chuàng)立了圖論這一研究方法。圖論法由此而來，歐拉也成了圖論法的始祖。時(shí)至今日，無論是在數(shù)學(xué)、天文學(xué)、化學(xué)、物理學(xué)等許多重要學(xué)科，圖論法都為問題的解決搭建了基礎(chǔ)性的框架。圖2-6哥尼斯堡七橋問題簡化圖應(yīng)用到機(jī)器人領(lǐng)域，機(jī)器人搜索區(qū)域內(nèi)的圖由地圖上的節(jié)點(diǎn)及節(jié)點(diǎn)與節(jié)點(diǎn)之間的連線兩種元素組成。用圖的節(jié)點(diǎn)來指示機(jī)器人的動(dòng)力學(xué)特性，用連接節(jié)點(diǎn)的邊線來示意機(jī)器人間的通信拓?fù)潢P(guān)系。如圖2-7所示，我們可以清楚從圖中看出節(jié)點(diǎn)間的相互關(guān)聯(lián)以及他們的信息流向。可將圖中元素的關(guān)系用函數(shù)抽象地描述為G=(V，E，Φ)，其中V表示搜索圖的頂點(diǎn)，E是圖的頂點(diǎn)的連線，Φ表示V，E間的某種函數(shù)關(guān)系。圖論法在用來分析解決多機(jī)器人的編隊(duì)的問題時(shí)，將多機(jī)器人形成的編隊(duì)看作一幅圖。在用圖論法研究多機(jī)器人編隊(duì)問題時(shí)只需注意兩機(jī)器人間是否被一連線所連，而機(jī)器人形成的頂點(diǎn)的位置和連接的方式則無關(guān)緊要。圖2-7三種機(jī)器人編隊(duì)拓?fù)浣Y(jié)構(gòu)運(yùn)用圖論法解決多機(jī)器人編隊(duì)問題時(shí)，首要做得事就是將多機(jī)器人編隊(duì)系統(tǒng)轉(zhuǎn)化成對(duì)應(yīng)的圖，接著運(yùn)用圖論法加以分析。圖論法首先是對(duì)機(jī)器人編隊(duì)進(jìn)行圖形化建模，通過使用相關(guān)的控制理論和圖論知識(shí)從而得到編隊(duì)的控制規(guī)律。在實(shí)際應(yīng)用圖論法研究多機(jī)器人編隊(duì)問題時(shí)，其主要對(duì)多機(jī)器人一致性REF_Ref9604\r\h[20]問題的研究大有裨益，從而分析出多機(jī)器人編隊(duì)中每個(gè)機(jī)器人的位置狀態(tài)能否隨著相關(guān)算法的指引最終形成限定區(qū)域內(nèi)的一致。運(yùn)用圖論法研究多機(jī)器人編隊(duì)的優(yōu)點(diǎn)是相關(guān)理論豐厚且成熟?？梢詤⒖紨?shù)學(xué)上關(guān)于圖論的知識(shí)，尤其是在理論上分析涉及到多機(jī)器人編隊(duì)隊(duì)形上有變化的情況時(shí)，圖論法顯得簡單得多。另外，在多機(jī)器人隊(duì)形隨意的時(shí)候比較有優(yōu)勢，使得編隊(duì)的隊(duì)形轉(zhuǎn)換更為容易，包括機(jī)器人個(gè)數(shù)增減以及相互之間關(guān)系的變化REF_Ref8363\r\h[21]。缺點(diǎn)是很難物理實(shí)現(xiàn)，更多的是運(yùn)用到理論分析和仿真實(shí)驗(yàn)中?；谛袨榉ㄋ^基于行為法，即進(jìn)行編隊(duì)控制時(shí)，首先對(duì)機(jī)器人所有可能出現(xiàn)的，提前可以分析出的行為加以預(yù)測，比如機(jī)器人編隊(duì)整體的運(yùn)行方向、各個(gè)機(jī)器人的運(yùn)動(dòng)姿態(tài)、避障采取的動(dòng)作等等REF_Ref10812\r\h[22]。在對(duì)機(jī)器人的行為進(jìn)行預(yù)先考慮后，所有的預(yù)期行為都有自己的任務(wù)和目標(biāo)，針對(duì)機(jī)器人的每一種具體行為進(jìn)行控制器設(shè)計(jì)，則多機(jī)器人系統(tǒng)最終的控制協(xié)議是由這些子行為的控制協(xié)議融合而成，針對(duì)行為設(shè)計(jì)的控制器可以有效的使得最終的多機(jī)器人群體產(chǎn)生所需的整體行為。子行為的融合方法，根據(jù)外界障礙的不同及多機(jī)器人的編隊(duì)任務(wù)的不同而不同。為了實(shí)現(xiàn)子行為間執(zhí)行的優(yōu)先排序，可以對(duì)各個(gè)子行為賦以相應(yīng)的權(quán)重，從而有效實(shí)現(xiàn)對(duì)多機(jī)器人各個(gè)子系統(tǒng)的控制，分行為設(shè)計(jì)相關(guān)策略?；谛袨榉ǖ暮诵氖菍?duì)機(jī)器人在環(huán)境中所有可能出現(xiàn)的各種基本行為加以預(yù)測，并對(duì)這些行為進(jìn)行協(xié)調(diào)統(tǒng)籌，如圖2-8所示。圖2-8基于行為法示意圖Balch和ARkin首先提出了基于行為的控制方法REF_Ref11348\r\h[23]。在此基礎(chǔ)上，哈爾濱工程大學(xué)雷艷敏等人基于行為法進(jìn)行了多機(jī)器人編隊(duì)控制的仿真研究REF_Ref11537\r\h[24]，在基礎(chǔ)的駛向目標(biāo)行為，構(gòu)型維持行為，避障行為上又增設(shè)計(jì)了兩種基本行為，并對(duì)各行為配以權(quán)重，優(yōu)先級(jí)，通過仿真驗(yàn)證了該方法的可行性和有效性?；谛袨榉w現(xiàn)了機(jī)器人的智能性。符合人們的決策思維，分步解決大任務(wù)。基于行為法的優(yōu)勢是分布性好，在設(shè)計(jì)不同子行為控制器時(shí)顯得有條理得多。由于分不同的行為設(shè)計(jì)不同的控制器，這樣方便實(shí)時(shí)地在多機(jī)器人編隊(duì)中加入新成員。隨之而來的，勢必會(huì)帶來繁瑣的設(shè)計(jì)過程，很難對(duì)系統(tǒng)進(jìn)行數(shù)學(xué)角度的穩(wěn)定性分析。避障方法理論研究在機(jī)器人向目標(biāo)點(diǎn)前行過程中，通過傳感器感知障礙物的存在，利用避障算法進(jìn)行處理，指導(dǎo)機(jī)器人的下一步行為，指引其路線的選擇，實(shí)時(shí)更新路線軌跡，繞過障礙物REF_Ref14966\r\h[25]。隨著計(jì)算機(jī)運(yùn)算能力的不斷加強(qiáng)，傳感器精度的升級(jí)以及自動(dòng)化程度的提升，對(duì)避障算法提出了更高的要求，越來越多的需要適應(yīng)性更強(qiáng)的、統(tǒng)一化程度更高的避障算法，以期能解決更為復(fù)雜障礙環(huán)境條件，不同類型機(jī)器人避障的客觀需要。然而至今沒有任何一種避障策略算法能夠解決在多障礙，多地形條件，多維度環(huán)境中機(jī)器人都能進(jìn)行有效地避障這一問題，突破解決統(tǒng)一化程度更高，適應(yīng)性更強(qiáng)的避障算法的問題勢必會(huì)帶來機(jī)器人領(lǐng)域的一場大的革新。人工勢場法人工勢場(Artificialpotentialfields,APF）是由KhatibREF_Ref15156\r\h[26]在上世紀(jì)九十年代年提出的。將一個(gè)物體被放置在一個(gè)力場中，這個(gè)力場是由一個(gè)被定義為目標(biāo)的點(diǎn)的引力和被建模為不可通過的障礙物表面所產(chǎn)生的的斥力所形成的，那么這個(gè)物體就有可能在不與障礙物碰撞的情況下穿過存在障礙的實(shí)驗(yàn)環(huán)境并到達(dá)目標(biāo)點(diǎn)。APF技術(shù)基于目標(biāo)的吸引力和移動(dòng)機(jī)器人運(yùn)行檢測識(shí)別區(qū)域內(nèi)障礙物的排斥力的矢量和。一般的設(shè)計(jì)是，機(jī)器人在當(dāng)前位置所受目標(biāo)的吸引力隨著距目標(biāo)點(diǎn)位置距離的增加而增加，所受到的排斥力同障礙物表面與機(jī)器人之間的距離成反比。對(duì)于避障區(qū)域內(nèi)每個(gè)點(diǎn)，都可以計(jì)算出在該點(diǎn)機(jī)器人所受到的合力，該合力將指引機(jī)器人下一步行為（如圖3-1所示）。人工勢場法將地圖中的障礙物產(chǎn)生的斥力場表現(xiàn)為電磁學(xué)上的一個(gè)高峰，而目標(biāo)點(diǎn)產(chǎn)生的引力表現(xiàn)為低谷。在機(jī)器人要進(jìn)行避障的區(qū)域內(nèi)，勢場會(huì)對(duì)機(jī)器人每個(gè)位置產(chǎn)生指引下一步運(yùn)行的合力，指引著機(jī)器人向目標(biāo)點(diǎn)前行。圖3-1APF原理示意圖如圖3-2所示，三幅圖中，展示了人工勢場的原理。左圖是障礙環(huán)境，中間兩個(gè)黑塊是障礙物。機(jī)器人要從圖中無障礙地從起始點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)。在通過人工勢場法形成勢場后，建立了中圖所示的模型，最后那幅圖展示了形成勢場后得到的等勢位圖，圖中的每條連續(xù)的曲線就表示了同勢力值大小的位置，可以看到示意機(jī)器人運(yùn)行軌跡的從起點(diǎn)到終點(diǎn)的連線不會(huì)跨過障礙，機(jī)起始點(diǎn)出發(fā)，一路沿著勢場下降的方向到達(dá)最終的目標(biāo)點(diǎn)REF_Ref17017\r\h[27]。圖3-2勢場示意圖想象將一個(gè)小球從碗邊放入碗中，小球會(huì)自然地向碗的底部滑落。這就好比一個(gè)勢場。機(jī)器人正是沿著“碗底”這個(gè)勢場所指向的那個(gè)方向一直行走。更進(jìn)一步的是，如果碗的壁不是光滑的，有凸起的。那么小球會(huì)在下滑的過程中繞開凸起滑向碗底。類似的，機(jī)器人在人工市場中也會(huì)將障礙物看成是一個(gè)個(gè)的凸起，從而繞過這個(gè)比較高的障礙物向著目標(biāo)行進(jìn)。從一方面來看，在數(shù)學(xué)表達(dá)式上人工勢場法顯現(xiàn)了出簡潔、優(yōu)美的亮眼之處，得到了很多人的青睞。且在常規(guī)障礙環(huán)境中，適應(yīng)性良好，得到了極大的應(yīng)用。人工勢場法也存在著需要解決的問題與瓶頸。人工勢場法存在局部最優(yōu)點(diǎn)問題：多數(shù)情況下，在應(yīng)用避障算法的時(shí)候并不會(huì)將目標(biāo)點(diǎn)置于障礙物極近的位置。這樣當(dāng)機(jī)器人運(yùn)行到目標(biāo)點(diǎn)位置附近時(shí)，障礙物的斥力會(huì)變小，不能與目標(biāo)點(diǎn)產(chǎn)生的引力抗衡，機(jī)器人受到目標(biāo)點(diǎn)吸引而向目標(biāo)點(diǎn)進(jìn)發(fā)直至到到終點(diǎn)。但在一個(gè)極端情況下，將目標(biāo)點(diǎn)附近放置一個(gè)障礙物，這樣當(dāng)機(jī)器人接近目標(biāo)時(shí)，機(jī)器人在受到目標(biāo)位置引力的同時(shí)還會(huì)受到相近位置的障礙物產(chǎn)生的斥力。會(huì)造成斥力與引力“較勁”的情況，合力變不會(huì)指向目標(biāo)點(diǎn)，機(jī)器人并不會(huì)繼續(xù)向著目標(biāo)位置進(jìn)發(fā)，合勢場導(dǎo)向產(chǎn)生了錯(cuò)誤，機(jī)器人最終也就無法到達(dá)目標(biāo)點(diǎn)，反而會(huì)被障礙物排斥的更遠(yuǎn)。解決方法上，可以調(diào)整合勢場函數(shù)，通過建立統(tǒng)一的勢能函數(shù)來解決這一問題。如在合勢場函數(shù)中加入零極值點(diǎn)，這樣就可以無論終點(diǎn)位置是否存在障礙物，都會(huì)保證目標(biāo)點(diǎn)的是勢場零點(diǎn)，是“碗”的最低部。但為了機(jī)器人實(shí)際運(yùn)行算法的時(shí)候進(jìn)行有效率的避障，這種方法勢必會(huì)增大運(yùn)算量。因而，這種改進(jìn)方法只適用障礙物是規(guī)則的，且數(shù)量上不宜過多。否則算法的計(jì)算量將很大，有時(shí)甚至是無法計(jì)算的。處理函數(shù)中的局部極小值問題的另一種思路是，一旦檢測到機(jī)器人沒有到達(dá)目標(biāo)位置（即卡住在局部極小值）就給機(jī)器人施加隨機(jī)力，稱為隨機(jī)漫步。例如，如果機(jī)器人被想象成一個(gè)流體或氣體粒子，機(jī)器人將能夠通過機(jī)器人和目標(biāo)位置之間的障礙物。但是，這種方法在遇到狹窄的通道時(shí)就不會(huì)成功，會(huì)導(dǎo)致與障礙發(fā)生碰撞。人工勢場法還存在一個(gè)致命缺點(diǎn)——零勢能點(diǎn)。在某些特殊情況下，障礙物形成的斥力與目標(biāo)位置的引力恰抵消，這會(huì)使得使移動(dòng)機(jī)器人陷入局部極小點(diǎn)，機(jī)器人利用合力無法走出極小點(diǎn)，達(dá)不到路徑規(guī)劃的目的。隨著障礙物的數(shù)量增加，產(chǎn)生零勢能點(diǎn)的可能性也會(huì)隨之增加REF_Ref18101\r\h[28]。因此，解決零勢能點(diǎn)的問題在復(fù)雜障礙環(huán)境條件下顯得尤為重要。另外在局部雜亂的環(huán)境中，機(jī)器人若遇到不規(guī)則障礙物，例如鋸齒狀障礙物前，由于機(jī)器人可以在無障礙的路徑中獲得更高的速度，可能存在歐氏距離意義上更長但運(yùn)行時(shí)間更短的路徑，合勢場會(huì)形成“渦流”現(xiàn)象，機(jī)器人會(huì)在障礙物前震蕩，形成徘徊抖動(dòng)現(xiàn)象。震蕩運(yùn)動(dòng)問題的一種解決方案是將障礙物分組在一起，設(shè)置虛擬障礙物，這些障礙物可能包含狹窄的通道，并且障礙物之間距離足夠近。在一些特殊避障場景下，例如迷宮，人工勢場法還會(huì)存在陷阱區(qū)域的問題。通過對(duì)人工勢場法的優(yōu)劣分析，在3.4節(jié)提出A*算法徹底解決這種弊端，并在5.5節(jié)通過仿真實(shí)驗(yàn)加以驗(yàn)證。BUG算法Bug算法是最符合機(jī)器思維的避障算法。其基本思想是，機(jī)器人向著目標(biāo)點(diǎn)直線前行，直到在檢測到障礙后，原有的直行行不通了，下一步就是圍著檢測到的障礙物輪廓保持安全距離裕量繞行，從而“繞開”它。為了簡化運(yùn)算步驟，優(yōu)化軌跡曲線，Bug算法經(jīng)過發(fā)展有著不同的改善版本。最初版的Bug1避障思路是這樣的。機(jī)器人在向著目標(biāo)點(diǎn)直線行進(jìn)過程中遇到障礙物時(shí)，它首先完全地圍繞物體運(yùn)動(dòng)，并在從距目標(biāo)點(diǎn)距離最短的點(diǎn)與障礙物分離（如圖3-3所示）。雖然Bug1算法的效率很低，但通過這樣做卻可以保證機(jī)器人到達(dá)目標(biāo)，這也為后續(xù)的版本地改進(jìn)提供了基礎(chǔ)。優(yōu)化后的Bug2算法中，機(jī)器人在遇到障礙時(shí)雖然也會(huì)跟蹤圍繞著障礙物邊沿運(yùn)行，但它與Bug1算法最大的不同是，其并不會(huì)完全圍繞物體一整周，一旦機(jī)器人發(fā)現(xiàn)可以直線移動(dòng)至目標(biāo)時(shí)，就立即與障礙分離，這樣比起B(yǎng)ug1算法在總行走距離上少了不少（如圖3-3所示）。分析可知，路線的選取長度較長是Bug算法得到應(yīng)用的一個(gè)重要的限制因素。機(jī)器人采用該算法避障會(huì)走許多“彎路”，運(yùn)行效率低下。更為關(guān)鍵的是，Bug算法只能用于二維場景，三維環(huán)境下并不適用。當(dāng)遇到特殊形狀的障礙物時(shí)，這種算法往往會(huì)得到錯(cuò)誤的路。圖3-3BUG1算法（左）與改進(jìn)后BUG2算法（右）對(duì)比示意圖向量直方圖法向量直方圖法將機(jī)器人當(dāng)前位置周圍環(huán)境對(duì)機(jī)器人運(yùn)行產(chǎn)生的影響用概率加以描述，并用柱狀圖刻畫影響。向量直方圖法是典型的即走即更新地圖的實(shí)時(shí)性避障算法。向量直方圖法基本過程有兩步，首先刻畫機(jī)器人周圍的環(huán)境，并采用直方圖柵格化轉(zhuǎn)換為極坐標(biāo)向量的形式描述，第二階段依據(jù)通過概率，選擇對(duì)機(jī)器人運(yùn)行影響概率最小的方向前進(jìn)。向量直方圖法難點(diǎn)在于基于機(jī)器人當(dāng)前位置做到生成一個(gè)實(shí)時(shí)的直方圖。該圖會(huì)根據(jù)機(jī)器人所搭載的傳感器所傳來的數(shù)據(jù)來進(jìn)行實(shí)時(shí)的更新。這就對(duì)傳感器硬件和機(jī)器人的運(yùn)算力提出了更高的要求。VFH算法生成的的極坐標(biāo)直方圖如圖3-4所示：圖3-4VFH示意圖REF_Ref22631\r\h[29]如圖3-4所示，橫軸為機(jī)器人當(dāng)前位置探測到360°方向上的角度范圍，縱坐標(biāo)為機(jī)器人在各方向上通過周邊環(huán)境的概率值。實(shí)際過程中機(jī)器人會(huì)每隔一個(gè)時(shí)間采樣點(diǎn)，生成一幅柱狀概率圖，根據(jù)通過概率選擇通過概率最高的去行進(jìn)。搜索類算法談及避障，機(jī)器人首先要做的事不是如何去“躲避”，而是向哪走。因?yàn)檎系K物的未知性，機(jī)器人“視野”有界性，避障只應(yīng)是機(jī)器人在向目標(biāo)點(diǎn)移動(dòng)過程中遇到障礙后采取的行動(dòng)。無論是人工勢場法，BUG算法亦或是向量直方圖法均是在機(jī)器人前進(jìn)過程中，先感知障礙物，通過相應(yīng)算法計(jì)算出下一步的位置從而規(guī)劃路徑。另一種思路是，將機(jī)器人從當(dāng)前位置到目標(biāo)位置所有路徑加以預(yù)先探索，從中選取一條運(yùn)行。在小范圍內(nèi)，這種思路具有可行性。一個(gè)直接的想法是，從機(jī)器人當(dāng)前位置與目標(biāo)點(diǎn)建立直線L，機(jī)器人沿著這條直線行走，遇到障礙后再采取避障的策略，如順時(shí)針或逆時(shí)針繞障礙物行走直到重回直線L上，再繼續(xù)向目標(biāo)點(diǎn)行進(jìn)。如此往復(fù)執(zhí)行，勢必會(huì)到達(dá)“終點(diǎn)”。在不考慮路徑規(guī)劃的情況下這是一種簡單的且切實(shí)可行的策略。這個(gè)策略明顯存在一個(gè)缺陷，就是“繞”，會(huì)顯著增加機(jī)器人路線長度。為了解決這一路徑優(yōu)化問題，1968年Hart教授等專家提出了一種用于路徑搜索的啟發(fā)式搜索算法——A*算法REF_Ref24466\r\h[30]。與人工勢場法最大的不同是，A*解決了障礙“圈套”的問題。要想逃離，躲避障礙圈套的第一步就是要識(shí)別處于路線上所有可能對(duì)機(jī)器人“設(shè)套”的障礙。這就需要機(jī)器人在向目標(biāo)點(diǎn)前行之前遍歷搜索這些障礙物。這其中的方法有很多。為了實(shí)現(xiàn)所謂的搜索，就需要量化地圖，這就需要將路徑量化，即柵格化（如圖3-5所示）。將地圖劃分為一個(gè)個(gè)大小一致的柵格，之后就是搜索了。尋路的工作核心就是去遍歷設(shè)定地圖區(qū)域內(nèi)機(jī)器人下一步或下幾步將要走的或者是可能要去走的地圖，識(shí)別出其中存在的影響向目標(biāo)行進(jìn)的障礙并加以刻畫，緊接著對(duì)可通過的部分進(jìn)行線路規(guī)劃，從而指導(dǎo)機(jī)器人的行進(jìn)。圖3-5柵格地圖示意圖地圖柵格化之后形成一個(gè)個(gè)形狀、大小相同的小室。這些小室的形狀為了方便下一步機(jī)器人行進(jìn)線路的統(tǒng)一化，最好是對(duì)稱圖形。比如可以把要搜尋的區(qū)域劃分成了正方形的格子。這樣一來，還可以方便的把機(jī)器人要進(jìn)行搜索的每個(gè)柵格標(biāo)記成0，1數(shù)組。數(shù)組的每一項(xiàng)代表一個(gè)格子，標(biāo)記為0的柵格被認(rèn)為是下一步機(jī)器人可以走的，可以用來規(guī)劃行進(jìn)路線的，而標(biāo)記為1的柵格就是障礙物所在的柵格，在規(guī)劃機(jī)器人線路的時(shí)候加以過濾掉。再下一步的工作就是遍歷地圖，通過不同的算法決定走那些代表著0的格子最合理，最有效。這樣就實(shí)現(xiàn)了優(yōu)化機(jī)器人路徑的過程從而完成路徑規(guī)劃的目的。方便起見，把每個(gè)劃分好的方格的中心點(diǎn)定義為“節(jié)點(diǎn)”（node），這樣每個(gè)單元格都是一個(gè)節(jié)點(diǎn)。邊緣連接相鄰的單元格。障礙物可以被設(shè)置為“空”，這樣機(jī)器人就無法與障礙相遇，復(fù)雜情況下，可以對(duì)不同路況的區(qū)域格子賦以不同的權(quán)重?？梢哉f用于機(jī)器人搜索的地圖是由節(jié)點(diǎn)斷續(xù)組成的?？梢灶A(yù)見，柵格化程度越高，即密化搜索地圖程度也越高，規(guī)劃出的機(jī)器人行進(jìn)路線也就越平滑。當(dāng)然的，生成的數(shù)組量也越大，會(huì)帶來運(yùn)算量和響應(yīng)時(shí)間上的同步提升。因而，根據(jù)不同的障礙環(huán)境，避障要求指標(biāo)的不同，選取不同的密化度尤為重要。從整體響應(yīng)時(shí)間來看，并非一味地的追求高平滑度的行進(jìn)路線就是最優(yōu)的。通過以上分析，研究搜索區(qū)域內(nèi)的路徑規(guī)劃，障礙環(huán)境中機(jī)器人的避障問題其實(shí)就是就是研究遍歷圖的方法。把從圖中的一個(gè)節(jié)點(diǎn)出發(fā)訪問圖中其他所有的節(jié)點(diǎn)的過程稱為遍歷，為了高效，只要是某一節(jié)點(diǎn)被訪問過，當(dāng)然的，它就不必被再次訪問了。所以，遍歷過程需要兩個(gè)“記錄”，或者說是記錄表。一個(gè)用來記錄未被考察的小室，不妨把它稱作OPEN表；一個(gè)用來記錄已經(jīng)考察過的小室，稱為CLOSED表。要明確一點(diǎn)的是，遍歷的過程機(jī)器人事先并不知道終點(diǎn)的位置以及障礙物的位置，他只在自己的搜索框地圖內(nèi)遍歷。這也是搜索遍歷的意義所在。而搜索的方法有多種。從深度與廣度兩種不同優(yōu)先遍歷節(jié)點(diǎn)的方式來分，遍歷搜索圖有深度優(yōu)先遍歷（DepthFirstSearch，DFS）與廣度優(yōu)先遍歷（BreadthFirstSearch，BFS）兩種方式。第一種是“打破砂鍋問到底”的尋訪節(jié)點(diǎn)的方式。機(jī)器人從當(dāng)前位置開始，縱向或橫向選擇一條支路，將其看為是一條沒有岔路的單行路，盡可能不斷地向前推進(jìn)搜索，直至遍歷到了圖的邊界或探尋到障礙節(jié)點(diǎn)就往回退。回退時(shí)，只要再次發(fā)現(xiàn)一條可以繼續(xù)縱深推進(jìn)搜索的下級(jí)支路，則繼續(xù)向前。依次往復(fù)，直至發(fā)現(xiàn)目標(biāo)點(diǎn)或遍歷盡地圖。像這樣以縱深推進(jìn)為優(yōu)先搜索方向，不斷先縱深再回退尋找其他支路的遍歷方式，稱為深度優(yōu)先遍歷（DFS）（如圖3-6所示）。DFS使用“后進(jìn)先出”原則。圖3-6DFS示意圖與之相對(duì)應(yīng)的，還有另一種尋訪方式：在遍歷順序上，將寬度或者說是廣度看成是優(yōu)先搜索的過程就是廣度優(yōu)先搜索算法。具體來講，以機(jī)器人當(dāng)前位置為核心點(diǎn)，首先把它周圍的的幾個(gè)節(jié)點(diǎn)探索完成，接著在以已經(jīng)形成的搜索過的區(qū)域?yàn)楹诵膮^(qū)域，將其相接的更外圍的一圈節(jié)點(diǎn)加以搜索，然后再去搜索距離機(jī)器人起始點(diǎn)起點(diǎn)更遠(yuǎn)一些（間隔兩層）的節(jié)點(diǎn)……這樣不斷擴(kuò)大搜索區(qū)域，直至所有的節(jié)點(diǎn)被遍歷。這種由機(jī)器人起始點(diǎn)為核心節(jié)點(diǎn)，不斷地向外圍擴(kuò)散的遍歷方式就是廣度優(yōu)先搜索算法（BFS）。為了有序的遍歷到每一圈的節(jié)點(diǎn)，需要在執(zhí)行算法的過程中，記錄當(dāng)前被尋訪的節(jié)點(diǎn)的上一個(gè)節(jié)點(diǎn)的位置，稱之為根節(jié)點(diǎn)。這樣做的好處是，一旦遍歷結(jié)束，搜索到目標(biāo)點(diǎn)后，便可以從目標(biāo)點(diǎn)開始，溯源根節(jié)點(diǎn)逆序找到起點(diǎn)，由此就構(gòu)成了一條路徑（如圖3-7所示）。BFS使用“先進(jìn)先出”原則。圖3-7BFS示意圖在對(duì)節(jié)點(diǎn)數(shù)據(jù)的采集，存儲(chǔ)以及運(yùn)算上，深度優(yōu)先算法與廣度優(yōu)先算法有著顯著的區(qū)別（如圖3-8所示）。廣度優(yōu)先算法新節(jié)點(diǎn)的信息會(huì)頂?shù)糇畛豕?jié)點(diǎn)的信息，所以BFS選取狀態(tài)采用了隊(duì)列的形式，先進(jìn)先出。廣度優(yōu)先算法由于需要不斷記錄上一根節(jié)點(diǎn)的位置信息，數(shù)據(jù)需要加以堆積，采用了類似堆棧的方式。當(dāng)前遍歷的節(jié)點(diǎn)信息在下一遍歷過程中會(huì)立即被提出。圖3-8DFS（左圖）與BFS（右圖）數(shù)據(jù)查詢方式對(duì)比更進(jìn)一步的情況是，如果一些障礙物并不影響機(jī)器人通過，這樣并沒必要刻意去躲避障礙。也就是說，可以將圖形中相鄰節(jié)點(diǎn)之間的移動(dòng)代價(jià)加以賦值。鑒于此，上世紀(jì)六十年代，計(jì)算機(jī)科學(xué)家EdsgerW.Dijkstra設(shè)計(jì)出Dijkstra算法REF_Ref28159\r\h[31]。Dijkstra算法考慮到了當(dāng)前節(jié)點(diǎn)與起始節(jié)點(diǎn)的移動(dòng)代價(jià)。為了實(shí)現(xiàn)易于統(tǒng)計(jì)節(jié)點(diǎn)之間機(jī)器人移動(dòng)的代價(jià)和，采用設(shè)置優(yōu)先隊(duì)列表的方式加以記錄。優(yōu)先隊(duì)列表根據(jù)每個(gè)節(jié)點(diǎn)與機(jī)器人起始節(jié)點(diǎn)的預(yù)估值來進(jìn)行排序。在機(jī)器人避障前加以遍歷地圖，對(duì)遍歷過的每一個(gè)節(jié)點(diǎn)均需計(jì)算其從當(dāng)前位置到起始位置所移動(dòng)的距離代價(jià)，即對(duì)每個(gè)可通過的節(jié)點(diǎn)賦以權(quán)值，最終選取權(quán)重小的節(jié)點(diǎn)規(guī)劃機(jī)器人路徑。一步步反復(fù)進(jìn)行，以保證每步的移動(dòng)代價(jià)是最小的?？梢钥闯鲈贒ijkstra算法中，并沒有考慮到下一節(jié)點(diǎn)的選取問題，或者說在搜索方向上存在一定的盲目性。針對(duì)這個(gè)痛點(diǎn)，我們不妨再增加一個(gè)考慮因素，及給定一個(gè)搜索方向，使得每次選取下一個(gè)搜索節(jié)點(diǎn)都具有向目標(biāo)點(diǎn)進(jìn)發(fā)的方向性。這就是貪婪最佳優(yōu)先搜索算法。貪婪最佳優(yōu)先搜索算法在優(yōu)先隊(duì)列表里通過每個(gè)節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的預(yù)估值來進(jìn)行排序。在搜索時(shí)間上不難分析出，貪婪最佳優(yōu)先搜索算法遍歷時(shí)間要顯著短于廣度優(yōu)先算法，效率上得以大幅提升。盡管它的路徑不是最優(yōu)和最短的。這也正是貪婪最佳優(yōu)先搜索算法的優(yōu)勢之處，基于目標(biāo)方向去搜索，摒棄了全圖全域式地完全搜索。在有了搜索時(shí)間上最快的遍歷方式——貪婪最佳優(yōu)先算法和搜索路徑上最短的遍歷方式——Dijkstra算法的基礎(chǔ)上，就可以綜合兩者優(yōu)勢得到又快，又短的遍歷節(jié)點(diǎn)的算法——A*算法。既考慮當(dāng)前代價(jià)，即為每個(gè)尋訪節(jié)點(diǎn)設(shè)置權(quán)值，不停的計(jì)算每個(gè)節(jié)點(diǎn)到起始節(jié)點(diǎn)的距離，以獲得最短路線，同時(shí)也照顧到搜索方向上的目的性，持續(xù)計(jì)算每個(gè)節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)點(diǎn)的距離，從而引導(dǎo)機(jī)器人的搜索隊(duì)列不斷地向目標(biāo)位置逼近，從而搜索更少的節(jié)點(diǎn)，以保證路徑規(guī)劃的最優(yōu)。經(jīng)過以上分析，不難得出A*算法依據(jù)的基本原理公式：（3-1）F(n)表示從機(jī)器人當(dāng)前所在節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)總共需要經(jīng)過的預(yù)估距離。G(n)是機(jī)器人當(dāng)前所在節(jié)點(diǎn)到節(jié)點(diǎn)n的最短距離，H(n)是節(jié)點(diǎn)n到終點(diǎn)的預(yù)估距離。G(n)的計(jì)算只需在搜索隊(duì)列中選取權(quán)值最小的即可得出。而對(duì)于H(n)由于是目標(biāo)節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)的距離，這個(gè)值機(jī)器人并不知道，需要加以估計(jì)，預(yù)測。這里要強(qiáng)調(diào)的是，在計(jì)算H(n)時(shí)，要選取曼哈頓距離來度量其大小。因?yàn)槿绻唵蔚倪x取歐氏距離又可能會(huì)途徑障礙，這與機(jī)器人實(shí)際運(yùn)行軌跡不符，會(huì)顯著增加預(yù)估值F(n)。圖3-9不同距離度量方式如圖3-9所示，圖中兩節(jié)點(diǎn)之間通過線段直接連接的綠線是兩節(jié)點(diǎn)之間的歐氏距離。紅線、黃線或藍(lán)線這三條并不直接通過兩節(jié)點(diǎn)之間做直線連接的線表征了曼哈頓距離，用這種方法計(jì)量預(yù)估距離可以避免遍歷地圖是預(yù)估的機(jī)器人行經(jīng)路線“橫跨”障礙物。如圖3-10所示為廣度優(yōu)先搜索算法，貪婪最佳優(yōu)先算法與A*算法的遍歷方式對(duì)比圖。圖3-10三種搜索方式對(duì)比其他算法神經(jīng)網(wǎng)絡(luò)方法神經(jīng)網(wǎng)絡(luò)（NeuralNetworks，NN），是一種模仿生物神經(jīng)元結(jié)構(gòu)和功能的數(shù)學(xué)模型，定義為一種模仿人類行為的能力REF_Ref30426\r\h[32]。模仿了人對(duì)問題的思維過程以及遇到問題解決方法。人工神經(jīng)網(wǎng)絡(luò)方法避障是統(tǒng)計(jì)學(xué)在智能領(lǐng)域的實(shí)際應(yīng)用，它的基本要求是確保系統(tǒng)能夠像人一樣執(zhí)行給定的任務(wù)，依據(jù)輸入與輸出間復(fù)雜的關(guān)系進(jìn)行訓(xùn)練建模。從工程的角度來看，它能夠提供并行計(jì)算的替代品。這一領(lǐng)域的研究是與許多相關(guān)科學(xué)領(lǐng)域結(jié)合而發(fā)展的。神經(jīng)網(wǎng)絡(luò)的基本架構(gòu)如圖3-11所示。圖3-SEQ表\*ARABIC11神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)從圖3-11，可以了解神經(jīng)網(wǎng)絡(luò)的基本操作路線。網(wǎng)絡(luò)層數(shù)因系統(tǒng)而異，一般來說，對(duì)于三層結(jié)構(gòu)，第1層是輸入層，第2層作為隱藏層，在第2層實(shí)現(xiàn)成員函數(shù)，而最后一層是輸出層REF_Ref31614\r\h[33]。應(yīng)用到機(jī)器人領(lǐng)域，神經(jīng)網(wǎng)絡(luò)方法會(huì)把機(jī)器人從起始點(diǎn)到目標(biāo)點(diǎn)所有行經(jīng)路線加以建模，通過對(duì)上一時(shí)刻機(jī)器人的位置，運(yùn)動(dòng)姿態(tài)以及周圍的障礙信息的建模，得出機(jī)器人應(yīng)該進(jìn)行的下一步包括速度，方向在內(nèi)的的運(yùn)動(dòng)姿態(tài)。不斷前行的過程就是不斷建模輸出的過程。這對(duì)機(jī)器人的運(yùn)算能力提出較高的要求。但在硬件設(shè)施不斷升級(jí)換代的今天，這已不成限制因素。神經(jīng)網(wǎng)絡(luò)方法適用領(lǐng)域更廣的避障場景是針對(duì)動(dòng)態(tài)環(huán)境的障礙。動(dòng)態(tài)障礙環(huán)境中，可以建立基于動(dòng)態(tài)神經(jīng)網(wǎng)絡(luò)的避障算法，動(dòng)態(tài)神經(jīng)網(wǎng)絡(luò)在機(jī)器人所處障礙環(huán)境復(fù)雜度高時(shí)優(yōu)勢會(huì)更為顯著且可以調(diào)整算法的結(jié)構(gòu)層級(jí)，實(shí)時(shí)地改變機(jī)器人當(dāng)前所處障礙環(huán)境狀態(tài)與其下一步進(jìn)行避障動(dòng)作之間的映射關(guān)系，這種方法較其他算法可以顯著減輕機(jī)器人的運(yùn)算壓力REF_Ref1413\r\h[34]。模糊邏輯方法模糊邏輯模仿了人類的模糊性和推理能力。人類在日常行走過程中遇到障礙時(shí)，會(huì)這樣避障：看到前方不遠(yuǎn)有障礙，可以提前向右走一點(diǎn)。機(jī)器人也可以仿照著這樣做，比如可以為機(jī)器人設(shè)定這樣一條避障的模糊邏輯：在感知范圍內(nèi)傳感器傳回?cái)?shù)據(jù)檢測到前方偏右側(cè)有障礙物，機(jī)器人執(zhí)行稍微向左轉(zhuǎn)前行的運(yùn)動(dòng)；類似的，若偏右前方且較近處檢測到障礙物存在，機(jī)器人執(zhí)行減速命令并向左轉(zhuǎn)更多角度等等一系列操作。其實(shí)，就像我們?nèi)艘粯?，避障過程中機(jī)器人并不一定需要精確避障信息。模糊邏輯方法核心是模糊控制器，需要將工程師能預(yù)想到的所有符合人類思維的避障邏輯輸入到控制器中，以此控制機(jī)器人的避障過程REF_Ref1478\r\h[35]。模糊控制器一般來說由三個(gè)模塊組成。第一個(gè)模塊是模糊輸入接口模塊，它的基本功能是將精確的障礙信息進(jìn)行模糊處理，即把精確的物理模型數(shù)字轉(zhuǎn)換成語言變量值。這也是衡量模糊處理的關(guān)鍵指標(biāo)。語言變量值的區(qū)間加以劃分，得到不同的區(qū)間值間距，間距值越小，模糊化程度越精確。第二個(gè)模塊是模糊推理機(jī)構(gòu)，它的主要功能仿照人類處理這一障礙信息應(yīng)作出的下一步動(dòng)作，輸出到下一模塊——模糊輸出接口。模糊輸出接口的功能是把輸出模糊量轉(zhuǎn)化為精確數(shù)字量，這一過程恰與輸入接口相反。模糊邏輯控制系統(tǒng)的一個(gè)有趣的特點(diǎn)是，用“if-then”的方式表示輸入輸出關(guān)系，并建立知識(shí)庫，可以很容易地處理不確定的情況。模糊規(guī)則的輸入輸出關(guān)系如圖3-12所示。圖3-12模糊規(guī)則塊在路徑規(guī)劃中，模糊邏輯方法的應(yīng)用主要體現(xiàn)針對(duì)機(jī)器人行為的處理方式上，模糊邏輯系統(tǒng)具有處理未知條件的能力。機(jī)器人的最終操作由上層控制器基于機(jī)器人基本行為進(jìn)行平衡后作出綜合反饋動(dòng)作。模糊邏輯控制非常適合低成本的移動(dòng)機(jī)器人，不需要非常復(fù)雜的導(dǎo)航，因?yàn)槟：壿嬍窃S多形式的輸入邏輯值的組合。在對(duì)資源的利用率上，模糊邏輯是較高的一種方式，有效的信息可以達(dá)到更優(yōu)的避障行為結(jié)果。此外，模糊邏輯控制器可以與其他基于人工智能的控制器相結(jié)合，如神經(jīng)網(wǎng)絡(luò)，可以產(chǎn)生更好的控制器。編隊(duì)控制模型理論分析本章從理論上設(shè)計(jì)了一種基于虛擬結(jié)構(gòu)和行為結(jié)構(gòu)的移動(dòng)機(jī)器人編隊(duì)控制方法。為了實(shí)現(xiàn)這一目標(biāo)，每個(gè)機(jī)器人都被一個(gè)電荷建模，并定義一個(gè)軌跡使機(jī)器人向一個(gè)圓移動(dòng)。由于相同電荷之間的排斥力使得每個(gè)機(jī)器人在隊(duì)列中找到自己設(shè)定的的位置而不會(huì)相撞，以實(shí)現(xiàn)機(jī)器人的正多邊形隊(duì)列。在群隊(duì)形設(shè)計(jì)中，設(shè)定一個(gè)實(shí)時(shí)跟隨多機(jī)器人編隊(duì)進(jìn)行移動(dòng)的虛擬機(jī)器人并定位在圓心處，保證了群隊(duì)形的保持。當(dāng)機(jī)器人接近障礙物時(shí)，基于旋轉(zhuǎn)勢場得到的力使機(jī)器人既能避免碰撞，又能定位于局部最小位置。移動(dòng)機(jī)器人動(dòng)力學(xué)模型考慮設(shè)計(jì)一個(gè)在二維環(huán)境中包含N個(gè)相似移動(dòng)機(jī)器人的多機(jī)器人系統(tǒng)。第個(gè)機(jī)器人的動(dòng)力學(xué)方程如下：（4-1）其中，為機(jī)器人的位置，為質(zhì)量，是阻尼效應(yīng)，為PD控制器的系數(shù)。該P(yáng)D控制器用于控制機(jī)器人的瞬態(tài)響應(yīng)和B的影響。是控制輸入，強(qiáng)制機(jī)器人向目標(biāo)移動(dòng)。在下一節(jié)中，提出了一種面向虛擬結(jié)構(gòu)的圓移動(dòng)機(jī)器人的方法。移動(dòng)機(jī)器人運(yùn)動(dòng)控制在本節(jié)中，式（4-1）中的被定義為強(qiáng)制機(jī)器人向半徑為和圓心的圓移動(dòng)。引理1：(式4-2)提供了一個(gè)圓心為，半徑為的圓的軌跡：（4-2）其中。引理I介紹了期望的軌跡，這使機(jī)器人向一個(gè)圓移動(dòng)，當(dāng)它到達(dá)圓時(shí)會(huì)停止。證明：用極坐標(biāo)以及代入式（4-2）中，可以得到和，因此可以得到：（4-3）這意味著所有的軌跡，不管是圓內(nèi)還是圓外，都會(huì)收斂于圓。因?yàn)?，狀態(tài)在到達(dá)圓時(shí)停止運(yùn)動(dòng)。因此，這證明了引理。引理1為我們找到式（4-1）中定義的機(jī)器人的正確方向提供了指導(dǎo)。也就是說，考慮引理1的結(jié)果，式（4-1）中輸入的力定義如下:（4-4）其中為正增益。因此，時(shí)，式（4-4）中定義的力使機(jī)器人向圓運(yùn)動(dòng)。移動(dòng)機(jī)器人正多邊形編隊(duì)我們的目標(biāo)是將機(jī)器人排列成正多邊形的編隊(duì)。為了實(shí)現(xiàn)這一目標(biāo)，每個(gè)機(jī)器人都由一個(gè)電荷來建模。如果所有的電荷都是正電荷或負(fù)電荷，它們就互相排斥。在式（4-4）中定義的力使電荷在一個(gè)半徑為和中心的圓上，不讓它們走遠(yuǎn)。換句話說，當(dāng)使電荷朝向圓的力大于半徑方向上的排斥力的合力時(shí)，電荷就能保持在圓上。現(xiàn)在，如果電荷相等，當(dāng)它們之間的距離相等時(shí)，它們會(huì)到達(dá)期望的位置。由于每個(gè)機(jī)器人兩側(cè)的排斥力相等，且與圓相切的排斥力之和為零，可以得到機(jī)器人在圓上的正多邊形形態(tài)。兩個(gè)電荷之間的力定義如下:（4-5）其中是第個(gè)和第個(gè)電荷之間的力，是一個(gè)正的常數(shù)系數(shù)，和是電荷，是它們之間的距離。因此，由于其他個(gè)電荷對(duì)第個(gè)電荷施加的輸入力如下：（式4-6）（式4-6）中的力在x和y軸上可以分解為:（式4-7）其中，，。因此，對(duì)基于虛擬結(jié)構(gòu)的第k個(gè)機(jī)器人正多邊形編隊(duì)輸入的力可以表述為:（4-8）機(jī)器人群體的形成在上一節(jié)中，機(jī)器人圍繞半徑為和中心的圓以正多邊形形式排列。對(duì)于機(jī)器人群結(jié)構(gòu)，將虛擬機(jī)器人置于圓心處，而將虛擬機(jī)器人置于圓心處。因此將式（4-8）中的替換為，即虛擬機(jī)器人的坐標(biāo)。虛擬機(jī)器人的動(dòng)力學(xué)可以表示為:（4-9）這里是一個(gè)虛擬的外力。需要注意的是，機(jī)器人的初始位置應(yīng)該足夠接近虛擬機(jī)器人，或者虛擬機(jī)器人的移動(dòng)應(yīng)該足夠慢，以便機(jī)器人可以跟隨它REF_Ref2758\r\h[36]。避障本節(jié)提出一種基于旋轉(zhuǎn)勢場的移動(dòng)機(jī)器人避障新技術(shù)。在不失一般性的前提下，將障礙物視為方向和軸向的矩形，其中心和尺寸由機(jī)器人傳感器識(shí)別。所提出的方法的主要優(yōu)點(diǎn)為避開障礙物的斥力不是徑向的。勢場矢量圍繞障礙物旋轉(zhuǎn)，勢場矢量的方向取決于機(jī)器人接近障礙物的運(yùn)動(dòng)方向。例如，若機(jī)器人接近障礙物時(shí)，障礙物在其左邊，則圍繞障礙物的旋轉(zhuǎn)勢場力是逆時(shí)針方向的。當(dāng)機(jī)器人以相反的方向接近障礙物時(shí)，勢場的方向可以是順時(shí)針方向。因此，它們并不是在接近機(jī)器人的相反方向上，對(duì)機(jī)器人的合力不會(huì)使其粘在局部的最小位置。可以定義一個(gè)旋轉(zhuǎn)勢場向量使其在障礙物周圍的橢圓上。橢圓方程與障礙物的位置和尺寸成正比。這個(gè)橢圓從障礙物的頂點(diǎn)經(jīng)過的面積最小，勢場向量平行于這個(gè)橢圓。下面的引理給出了上述橢圓的方程：引理2：一個(gè)面積最小的橢圓包住一個(gè)頂點(diǎn)為的矩形，其方程為:（4-10）其中是矩形的中心，?+。證明：考慮一個(gè)帶方程的橢圓：（式4-11）橢圓的面積與成正比。因此，為了使橢圓的面積最小，和應(yīng)該是最大的。這可以通過以下條件得到：（式4-12）將（式4-11）中矩形頂點(diǎn)的坐標(biāo)代入，可以得到：（式4-13）將（式4-13）代入（式4-12），由于，可以得到，。因此，將（式4-11）中的和代入，得到（式4-10），由此證明了引理。因此，在頂點(diǎn)為的平面上，障礙物周圍面積最小的橢圓方程為，其中，。勢場向量平行于該橢圓。這些向量在橢圓上旋轉(zhuǎn)，可以用以下方程表示：（4-14）其中。引理3提供了所需的狀態(tài)空間方程，其軌跡圍繞式（4-14）中引入的橢圓旋轉(zhuǎn)。引理3：在下列狀態(tài)空間方程中，任意初始位置的狀態(tài)軌跡在橢圓上順時(shí)針旋轉(zhuǎn)，即。（4-15）證明：定義極坐標(biāo)，并代入式（4-15）中，可以得到，。因此，軌道以固定的半徑順時(shí)針旋轉(zhuǎn)。需要注意的是，向量的方向可以通過反求和的符號(hào)來反轉(zhuǎn)。為了避免移動(dòng)機(jī)器人定位于局部極小值位置，勢場的方向根據(jù)移動(dòng)機(jī)器人的運(yùn)動(dòng)方向來確定，即。當(dāng)接近勢場向量的有效區(qū)域時(shí)，方向應(yīng)與移動(dòng)機(jī)器人與障礙物重心的連線與橫軸(用表示)的夾角相比較。因此，基于上述原因，當(dāng)機(jī)器人以接近障礙物時(shí)，順時(shí)針方向移動(dòng)是合適的。在其他情況下，，向量的方向應(yīng)該是逆時(shí)針。移動(dòng)機(jī)器人與障礙物重心的連線與水平軸線的夾角為：（4-16）函數(shù)在到之間變化。因此，和可以比較如下:（4-17）其中為除法后的模。換句話說，式（4-16）使得圖4-1中的角度大于。圖4-1使用（式4-17）時(shí)與的比較由于函數(shù)在到之間變化，且大于，故障礙物周圍的轉(zhuǎn)動(dòng)矢量可表示為:（4-18）以下方程表示了兩種不同情況下的勢場向量的方向：（4-19）式中和分別表示順時(shí)針方向和逆時(shí)針方向的力向量?？梢酝ㄟ^以下方式獲?。海?-20）和（4-21）勢場向量式（4-20）和式（4-21）越靠近障礙物，勢場向量越小。然而，當(dāng)移動(dòng)機(jī)器人接近障礙物時(shí)，需要增加勢場。為了對(duì)式（4-19）進(jìn)行適當(dāng)?shù)男薷?，首先將其?guī)范化如下：（4-22）其中為（式4-18）的標(biāo)準(zhǔn)化形式，然后乘以，其中與移動(dòng)機(jī)器人相切的橢圓有關(guān)，移動(dòng)機(jī)器人定義如下:（4-23）另一方面，勢場應(yīng)該在距離障礙物一定的距離內(nèi)活動(dòng)，以擊退機(jī)器人。歸一化向量應(yīng)該足夠大，能夠改變移動(dòng)機(jī)器人的方向：因此，這些矢量的大小取決于所需的輸入力的大小，而沒有避障。以下方程表示考慮上述問題后的避障輸入力：當(dāng)：（4-24）否則，（4-25）其中為機(jī)器人期望的輸入力，為的函數(shù)，定義為一個(gè)橢圓，其中勢場向量在橢圓內(nèi)活動(dòng)。在式（4-24）的最后一項(xiàng)，有助于保持勢場矢量的內(nèi)聚力對(duì)移動(dòng)機(jī)器人的影響。越小(即距離障礙物越近)，勢場對(duì)輸入力的影響越大。仿真實(shí)驗(yàn)與結(jié)論分析本文利用MATLAB實(shí)現(xiàn)了基于領(lǐng)航者跟隨法編隊(duì)，人工勢場法避障的多機(jī)器人協(xié)同避障仿真。針對(duì)特殊情況下人工勢場法的不足性，利用A*搜索算法加以解決，從人工勢場的形成，到利用領(lǐng)航者跟隨法形成多機(jī)器人的編隊(duì)，再到利用人工勢場法的單個(gè)機(jī)器人的避障仿真。通過多組實(shí)驗(yàn)驗(yàn)證編隊(duì)協(xié)同避障策略的有效性。領(lǐng)航者跟隨法編隊(duì)針對(duì)多機(jī)器人利用領(lǐng)航者跟隨法進(jìn)行編隊(duì)，通過MATLAB進(jìn)行了多種隊(duì)形的仿真。運(yùn)用領(lǐng)航者跟隨方法，給定領(lǐng)航者坐標(biāo)（x,y）、航向角θ、速度v，跟隨者機(jī)器人即可據(jù)此作出自己的運(yùn)動(dòng)調(diào)整，向著每個(gè)跟隨者機(jī)器人在設(shè)定的編隊(duì)隊(duì)形中的位置運(yùn)動(dòng)。最終形成既定編隊(duì)隊(duì)形，如圖5-1所示。圖5-1三機(jī)編隊(duì)仿真示意圖圖5-2是在6機(jī)情況下，利用領(lǐng)航者跟隨法得出多機(jī)的編隊(duì)圖。圖5-SEQ圖_1.\*ARABIC2六機(jī)矩形編隊(duì)仿真示意圖人工勢場的MATLAB仿真實(shí)現(xiàn)引力場常用的引力函數(shù)：（5-1）這里的是尺度因子。表示機(jī)器人當(dāng)前位置與目標(biāo)點(diǎn)的直線距離。引力是引力場對(duì)距離的導(dǎo)數(shù)：（5-2）如圖5-3所示，形象地將引力場比作一個(gè)對(duì)機(jī)器人有吸引的凹槽，出發(fā)點(diǎn)位于高點(diǎn)，目標(biāo)點(diǎn)位于凹槽的底端。機(jī)器人自然會(huì)向目標(biāo)點(diǎn)運(yùn)動(dòng)。圖5-3引力場模型斥力場公式：(5-3)式5-3是傳統(tǒng)的斥力場公式。公式中是斥力尺度因子。根據(jù)不同的障礙環(huán)境、避障效果選擇合適的。為當(dāng)前機(jī)器人與障礙物之間的距離。為每個(gè)障礙物的影響半徑。這說明在影響半徑之外，障礙物對(duì)機(jī)器人并無斥力作用。斥力就是斥力場的梯度，形象地將障礙物產(chǎn)生的斥力場比作海拔不一的山丘，這樣機(jī)器人在運(yùn)動(dòng)過程中便不會(huì)與障礙物相撞(如圖5-4所示)。圖5-4斥力場模型（5-4）最終形成的合勢場為引力場與斥力場的疊加，機(jī)器人在每個(gè)位置受到的合力也是在該位置受到的引力與斥力的的疊加。如圖5-5所示是單個(gè)障礙物時(shí)單機(jī)器人運(yùn)行軌跡。圖5-5運(yùn)行軌跡示意圖圖5-6為機(jī)器人運(yùn)行過程中引力，斥力及合力示意圖。圖5-6運(yùn)行受力情況示意圖人工勢場法單機(jī)多障礙避障在形成人工勢場后仿真實(shí)現(xiàn)單個(gè)機(jī)器人在多障礙環(huán)境下無碰撞走向終點(diǎn)。經(jīng)過不斷調(diào)參，測試。設(shè)定每個(gè)障礙物的影響半徑為20，終點(diǎn)對(duì)機(jī)器人的吸引增益參數(shù)為5，障礙物斥力增益參數(shù)為12000，得到了良好的仿真避障效果。如圖5-7所示。圖5-7單機(jī)避障示意圖多機(jī)協(xié)同避障利用領(lǐng)航者跟隨法的編隊(duì)思路，人工勢場法的避障策略，仿真實(shí)現(xiàn)了多障礙環(huán)境下，多機(jī)器人協(xié)同避障。在一般障礙環(huán)境條件下，得到了符合預(yù)期的通過編隊(duì)隊(duì)形變換實(shí)行避障的仿真效果。第一種情況，障礙物將編隊(duì)隊(duì)形暫時(shí)分割，多機(jī)器人編隊(duì)遇到障礙實(shí)行“分流”避障（如圖5-8所示）。表5-8編隊(duì)通過隊(duì)形擴(kuò)散實(shí)現(xiàn)避障第二種情況，編隊(duì)中某幾個(gè)隊(duì)員機(jī)器人受到障礙影響，領(lǐng)航者機(jī)器人指引通過隊(duì)形收縮以繞過障礙物駛向目標(biāo)點(diǎn)（如圖5-9所示）。圖5-9編隊(duì)“收縮”以實(shí)現(xiàn)避障第三種情況，領(lǐng)航者機(jī)器人航行過程未遇到障礙阻擋情況，那么跟隨者機(jī)器人通過主動(dòng)向領(lǐng)航者機(jī)器人靠近從而完成避障（如圖5-10所示）。圖5-10跟隨者機(jī)器人向領(lǐng)航者靠近實(shí)現(xiàn)避障第四種情況，通過隊(duì)形的整體收縮從而實(shí)現(xiàn)編隊(duì)穿過障礙物，達(dá)到避障的效果（如圖5-11所示）。圖5-11編隊(duì)隊(duì)形收縮實(shí)現(xiàn)避障當(dāng)在通過障礙物形成的狹小區(qū)域后，多機(jī)器人編隊(duì)迅速恢復(fù)原有隊(duì)形向目標(biāo)點(diǎn)進(jìn)發(fā)（如圖5-12所示）。圖5-12通過障礙后恢復(fù)原有編隊(duì)A*算法MATLAB仿真如圖5-13所示，當(dāng)障礙物的擺放恰取在目標(biāo)點(diǎn)正對(duì)方向上時(shí)，機(jī)器人編隊(duì)受到的目標(biāo)點(diǎn)的引力與障礙物所形成的斥力合力抵消為零，機(jī)器人陷入零勢能點(diǎn)，從而無法走出“陷阱”，避障失效。圖5-13人工勢場下機(jī)器人碰壁示意圖圖5-14A*示意仿真圖針對(duì)人工勢場法零勢能點(diǎn)，局部極小點(diǎn)的問題，利用A*算法解決這個(gè)問題。如圖5-14所示，圖中黃色代表機(jī)器人起始位置，黑色為障礙部分，綠色部分為探索部分，紅色為最終路徑。在機(jī)器人從起點(diǎn)出發(fā)前，利用A*算法會(huì)遍歷從機(jī)器人當(dāng)前位置到終點(diǎn)的地圖，形成識(shí)別障礙陷阱，形成機(jī)器人可過通路——綠色區(qū)域，接著機(jī)器人選取最優(yōu)路徑駛向終點(diǎn)。解決了人工勢場法存在的障礙陷阱問題?？偨Y(jié)和展望論文主要結(jié)論論文課題的選定經(jīng)過深思熟慮。在選課題前查找了大量的相關(guān)文獻(xiàn)，從編隊(duì)控制，避障方法，路徑規(guī)劃再到協(xié)同控制，不斷地了解到多機(jī)器人協(xié)同避障的發(fā)展階段。協(xié)同避障策略的研究也是在不斷地學(xué)習(xí)前人的成果中進(jìn)行的。期間參考了避障算法與編隊(duì)策略的論文，技術(shù)文檔，以及大量網(wǎng)上論壇發(fā)布的資料。理論上總結(jié)分析了多機(jī)器人編隊(duì)及協(xié)同避障各類算法及策略。重點(diǎn)介紹了領(lǐng)航者跟隨法的編隊(duì)策略與人工勢場法的避障方法，針對(duì)人工勢場法的缺陷采用A*算法解決了其存在局部極小值點(diǎn)的問題。完成了任務(wù)書的要求。運(yùn)用領(lǐng)航者跟隨法利用MATLAB仿真實(shí)現(xiàn)了機(jī)器人編隊(duì)的問題。實(shí)現(xiàn)多機(jī)器人從初始的隨機(jī)分布狀態(tài)到形成設(shè)定的隊(duì)形編隊(duì)并在無障礙情況下實(shí)現(xiàn)了同步運(yùn)動(dòng)。從單個(gè)機(jī)器人利用人工勢場法進(jìn)行避障的研究開始，逐步在MATLAB上仿真實(shí)現(xiàn)了從人工勢場的形成，多個(gè)機(jī)器人在不同初始位置，不同障礙環(huán)境條件下的協(xié)同避障。針對(duì)人工勢場法存在局部極小值問題，采用A*算法仿真實(shí)現(xiàn)了特殊情況下機(jī)器人避障的問題。展望由于個(gè)人知識(shí)能力上的不足，時(shí)間上的有限性，論文還存在一些不足之處，以下方面需要進(jìn)一步研究：本文所指的多機(jī)器人是指陸地機(jī)器人，僅在二維環(huán)境中航行。不包含具有空中、水下等三維空間運(yùn)動(dòng)姿態(tài)的航行器，而三維的規(guī)劃勢必會(huì)更加的復(fù)雜。本文的方法還需要進(jìn)一步的研究才能適用三維環(huán)境下的避障。論文只利用MATLAB仿真驗(yàn)證了協(xié)同避障理論的有效性，并未實(shí)地實(shí)驗(yàn)加以檢驗(yàn)，理論分析與仿真同實(shí)際實(shí)驗(yàn)有著聯(lián)系但也有不同。由于本次任務(wù)側(cè)重理論上的策略分析和規(guī)劃算法研究，為進(jìn)行實(shí)際的實(shí)驗(yàn)過程驗(yàn)證，但深知理論是為實(shí)際應(yīng)用服務(wù)的。實(shí)際應(yīng)用時(shí)要考慮到數(shù)據(jù)的獲取，傳感器的選取的問題，包括考慮到移動(dòng)機(jī)器人本身的運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型。在真正應(yīng)用設(shè)計(jì)好的策略的過程中，為了保證可靠性，需要采取多種傳感器的結(jié)合，對(duì)不同傳感器采集到的數(shù)據(jù)進(jìn)行一個(gè)交叉驗(yàn)證，通過數(shù)據(jù)的對(duì)比分析以及信息的融合，保證機(jī)器人能夠穩(wěn)定性、可靠性足夠。在理論上與實(shí)際應(yīng)用時(shí)會(huì)存在差距。比如一個(gè)若機(jī)器人物理結(jié)構(gòu)是四輪式的汽車結(jié)構(gòu)，那么它就無法實(shí)現(xiàn)仿真中得出的算法指令“原地旋轉(zhuǎn)”。由于涉及到多機(jī)器人的之間的協(xié)同聯(lián)系，勢必就要考慮到傳輸上的延時(shí)，以及通信帶寬的問題。在整個(gè)算法的架構(gòu)設(shè)計(jì)優(yōu)先級(jí)上，要考慮到為了避讓，是否做出了何種“犧牲”，代價(jià)是否是值得的。在機(jī)器人的執(zhí)行力方面，設(shè)計(jì)的避障策略以及算法應(yīng)當(dāng)簡潔高效以便實(shí)現(xiàn)起來速度足夠快，響應(yīng)時(shí)間足夠短，滿足實(shí)際需要和指標(biāo)要求。參考文獻(xiàn)周雄才．基于Realsense的深度信息的障礙檢測與避障方法（碩士學(xué)位論文）[D]．李紅波，指導(dǎo)．重慶：重慶郵電大學(xué)，2019．芮可人．多移動(dòng)機(jī)器人系統(tǒng)編隊(duì)協(xié)調(diào)與控制研究（碩士學(xué)位論文）[D]．謝能剛，指導(dǎo)．馬鞍山：安徽工業(yè)大學(xué)，2019．曲士鄭．群機(jī)器人編隊(duì)及協(xié)同避障方法研究（碩士學(xué)位論文）[D]．陳春林，指導(dǎo)．南京：南京大學(xué)，2015．夏清松．復(fù)雜環(huán)境下多移動(dòng)機(jī)器人協(xié)同路徑規(guī)劃（碩士學(xué)位論文）[D]．唐秋華，指導(dǎo)．武漢：武漢科技大學(xué)，2019．黃小起．多移動(dòng)機(jī)器人編隊(duì)控制的研究（碩士學(xué)位論文）[D]．楊光紅，指導(dǎo)．沈陽：東北大學(xué)，2009．趙海波．多智能體運(yùn)動(dòng)編隊(duì)控制關(guān)鍵問題的研究（碩士學(xué)位論文）[D]．任彥，指導(dǎo)．包頭：內(nèi)蒙古科技大學(xué)，2019．王祥科，李迅，鄭志強(qiáng)．多智能體系統(tǒng)編隊(duì)控制相關(guān)問題研究[J]．控制與決策，2013，28(11):1601-1613．劉克波．基于HLA/RTI的多機(jī)器人仿真系統(tǒng)的通信研究（碩士學(xué)位論文）[D]．趙春霞，指導(dǎo)．南京：南京理工大學(xué)，2004．賈瑞明．基于領(lǐng)導(dǎo)者-跟隨者策略的多輪式移動(dòng)機(jī)器人編隊(duì)控制方法（碩士學(xué)位論文）[D]．岳明，指導(dǎo)．大連：大連理工大學(xué)，2018．WangPKC.Navigationstrategiesformultipleautonomousmobilerobotsmovinginformation[J].JournalofFieldRobotics,1991,8(2):177:195.DasAK,FierroR,KumarV,etal.Avision-basedformationcontrolframework[J].IEEETransactionsonRobotics&Automation,2002,18(5):813-825.XiaoH,LiZ,ChenCLP.Formationcontrolofleader-followermobilerobots’systemsusingmodelpredictivecontrolbasedonneural-dynamicoptimization[J].IEEETransactionsonIndustrialElectronics,2016,63(9):5752-5762.ParkBongSeok,YooSungJin.Connectivity-maintainingobstacleavoidanceapproachforleader-followerformationtrackingofuncertainmultiplenonholonomicmobilerobots[J].ExpertSystemswithApplications,2021.JadbabaieA,LinJ.Coordinationofgroupsofmobileautonomousagentsusingnearestneighborrules[J].IEEETransactionsonAutomaticControl,2003,48(6):988-1001.王中林，劉忠信，陳增強(qiáng)，孫青林，吳垠，馬爭光．一種多智能體領(lǐng)航跟隨編隊(duì)新型控制器的設(shè)計(jì)[J]．智能系統(tǒng)學(xué)報(bào)，2014(3):298-306．楊立煒，付麗霞，李萍．多智能體系統(tǒng)編隊(duì)控制發(fā)展綜述[J]．電子測量技術(shù)，2020,43(24):18-27．LewisMA,TanKH.Highprecisionformationcontrolofmobilerobotsusingvirtualstructures[J].AutonomousRobots,1997,4(4).何真，陸宇平，劉燕斌．基于虛擬結(jié)構(gòu)的分布式編隊(duì)控制方法[J]．應(yīng)用科學(xué)學(xué)報(bào)，2007(4):387-391．李正平，鮮斌．基于虛擬結(jié)構(gòu)法的分布式多無人機(jī)魯棒編隊(duì)控制[J]．控制理論與應(yīng)用，2020,37(11):2423-2431．李浩然．基于狀態(tài)估計(jì)算法的多機(jī)器人編隊(duì)控制（碩士學(xué)位論文）[D]．伍清河，指導(dǎo)．北京：北京理工大學(xué)，2016．蔣紅杰．多機(jī)器人編隊(duì)問題研究（碩士學(xué)位論文）[D]．楊益群，指導(dǎo)．上海：上海應(yīng)用技術(shù)大學(xué)，2017．ZhangZ,ZhangR,LiuX.Multi-robotformationcontrolbasedonbehavior[C].InternationalConferenceonComputerScienceandSoftwareEngineering.IEEE,2008:1045-1048.BalchT,ArkinRC.Behaviorbasedformationcontrolformulti-robotteams.[J].IEEETransactionsonRoboticsandAutomation,1998,14(6):926-939.雷艷敏，馮志彬，宋繼紅．基于行為的多機(jī)器人編隊(duì)控制的仿真研究[J]．長春大學(xué)學(xué)報(bào)，2008(08):40-44．盛宗生，胡永才．基于BUG理論的一種改進(jìn)算法[J]．河南師范大學(xué)學(xué)報(bào)（自然科學(xué)版），2007，03:172-174．OussamaK.Real-timeobstacleavoidanceformanipulatorsandmobilerobots[J].TheInternationalJournalofRoboticsResearch,1986,5(1).沈猛．輪式移動(dòng)機(jī)器人導(dǎo)航控制與路徑規(guī)劃研究（博士學(xué)位論文）[D]．徐德民，指導(dǎo)．西安：西北工業(yè)大學(xué)，2006．黃強(qiáng)．基于人工勢場和虛擬領(lǐng)航者的多智能體協(xié)同控制研究（碩士學(xué)位論文）[D]．張澤旭，指導(dǎo)．哈爾濱：哈爾濱工業(yè)大學(xué)，2013．鄭新娟．多機(jī)器人路徑規(guī)劃與協(xié)同編隊(duì)研究（碩士學(xué)位論文）[D]．鐘毅，指導(dǎo)．武漢：武漢理工大學(xué)，2014．陳素瓊．基于A~*算法的地圖游戲?qū)窖芯浚ùT士學(xué)位論文）[D]．馬燕，指導(dǎo)．重慶：重慶師范大學(xué)，2016．王志義．無人機(jī)作戰(zhàn)任務(wù)規(guī)劃研究（碩士學(xué)位論文）[D]．郭順生，指導(dǎo)．武漢：武漢理工大學(xué)，2017．謝浩．基于BP神經(jīng)網(wǎng)絡(luò)及其優(yōu)化算法的汽車車速預(yù)測（碩士學(xué)位論文）[D]．舒紅，指導(dǎo)．重慶：重慶大學(xué)，2014．黃偉．基于機(jī)器學(xué)習(xí)的AIOps技術(shù)研究（碩士學(xué)位論文）[D]．秦雅娟，指導(dǎo)．北京：北京交通大學(xué)，2019．鄒良開．三維空間中基于安全通信的多無人機(jī)的路徑規(guī)劃（碩士學(xué)位論文）[D]．吳杰宏，指導(dǎo)．沈陽：沈陽航空航天大學(xué)，2018．林政．無人船避障規(guī)劃算法研究（碩士學(xué)位論文）[D]．呂霞付，指導(dǎo)．重慶：重慶郵電大學(xué)，2020．張玉麗．基于虛擬力的多機(jī)器人化學(xué)源定位研究（博士學(xué)位論文）[D]．馬小平，指導(dǎo)．徐州：中國礦業(yè)大學(xué)，2015．人工勢場仿真MATLAB主程序%main%初始化粒子的參數(shù)closeall;clearall;clc;Xo=[1010];%起點(diǎn)位置k=1;%計(jì)算引力需要的增益系數(shù)m=1;%計(jì)算斥力的增益系數(shù)n=1;%障礙個(gè)數(shù)longth=0.5;%步長J=200;%循環(huán)迭代次數(shù)K=0;%初始化m_Target=[6070];%[m_Obs,m_ObsR]=Obs_Generate([510],[2080],[1080],n);Xj=Xo;%j=1循環(huán)初始，將車的起始坐標(biāo)賦給Xj%隨即產(chǎn)生障礙m_Obs=[5050];m_ObsR=[10];Po=min(m_ObsR)/2;%***************初始化結(jié)束，開始主體循環(huán)******************forj=1:J%循環(huán)開始Current(j,1)=Xj(1);%Goal是保存車走過的每個(gè)點(diǎn)的坐標(biāo)。剛開始先將起點(diǎn)放進(jìn)該向量。Current(j,2)=Xj(2);%調(diào)用計(jì)算角度模塊[angle_att,angle_rep]=compute_angle(Xj,m_Target,m_Obs,n);%調(diào)用計(jì)算引力模塊[Fatt,Uatt(j)]=compute_Attract(Xj,m_Target,k,angle_att);%調(diào)用計(jì)算斥力模塊[Frep,Fatt_add,Urep(j)]=compute_repulsion(Xj,m_Target,m_Obs,m_ObsR,m,angle_att,angle_rep,n,Po);%計(jì)算合力和方向[Position_angle(j)]=compute_Ftotal(Fatt,Frep,Fatt_add,n);%計(jì)算車的下一步位置Xnext(1)=Xj(1)+longth*cos(Position_angle(j));Xnext(2)=Xj(2)+longth*sin(Position_angle(j));%保存車的每一個(gè)位置在向量中Xj=Xnext;%Draw(Xj,m_Obs,m_Target,n);%判斷if(Is_Reach(Xj,m_Target,longth)==1)K=j;%記錄迭代到多少次，到達(dá)目標(biāo)。break;%記錄此時(shí)的j值end%如果不符合if的條件，重新返回循環(huán)，繼續(xù)執(zhí)行。end%大循環(huán)結(jié)束%****************************以下是圖形顯示部分****************************%畫勢場分析圖Draw(Uatt,Urep);%畫模型效果圖Draw_Model(Xo,Current,m_Obs,m_ObsR,m_Target,n);%畫總體勢場分布圖Draw_Potential(Xo,m_Obs,m,m_ObsR,Po,m_Target,k,n,[0100],[0100]);clear;%斥力計(jì)算function[Frep,Fatt,Urep]=compute_repulsion(X,m_Target,m_Obs,m_ObsR,m,angle_att,angle_rep,n,Po)%輸入?yún)?shù)為當(dāng)前坐標(biāo)，Xsum是目標(biāo)和障礙的坐標(biāo)向量，增益常數(shù),障礙，目標(biāo)方向的角度Rat=(X(1)-m_Target(1))^2+(X(2)-m_Target(2))^2;%路徑點(diǎn)和目標(biāo)的距離平方rat=sqrt(Rat);%路徑點(diǎn)和目標(biāo)的距離urep=0;fori=1:nRrei(i)=(X(1)-m_Obs(i,1))^2+(X(2)-m_Obs(i,2))^2;%路徑點(diǎn)和障礙的距離平方rre(i)=sqrt(Rrei(i))-m_ObsR(i);%路徑點(diǎn)和障礙的距離保存在數(shù)組rrei中ifrre(i)>Po%如果每個(gè)障礙和路徑的距離大于障礙影響距離，斥力令為0Yrerx(i)=0;Yrery(i)=0;Yatax(i)=0;Yatay(i)=0;elseYrer(i)=m*(1.0/rre(i)-1.0/Po)*(1.0/rre(i)^2)*Rat;%分解的Fre1向量Yata(i)=m*((1.0/rre(i)-1.0/Po)^2)*rat;%分解的Fre2向量Yrerx(i)=Yrer(i)*cos(angle_rep(i));%angle_re(i)=Y(i+1)Yrery(i)=Yrer(i)*sin(angle_rep(i));Yatax(i)=Yata(i)*cos(angle_att);%angle_at=Y(1)Yatay(i)=Yata(i)*sin(angle_att);urep=urep+1/2.0*m*(1.0/rre(i)-1.0/Po)^2*Rat;%計(jì)算斥力勢場end%判斷距離是否在障礙影響范圍內(nèi)endFrep(1)=sum(Yrerx);%疊加斥力的分量Frep(2)=sum(Yrery);Fatt(1)=sum(Yatax);Fa