一種基于領(lǐng)航跟隨的移動機(jī)器人編隊控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及移動機(jī)器人運動控制領(lǐng)域，尤其設(shè)及一種基于領(lǐng)航跟隨的移動機(jī)器人 編隊運動控制方法。
【背景技術(shù)】
[0002] 隨著機(jī)器人技術(shù)的發(fā)展，移動機(jī)器人在應(yīng)用范圍和功能方面都得到了較大的拓展 和提高，并且廣泛應(yīng)用于國防、工業(yè)、服務(wù)業(yè)等行業(yè)。多移動機(jī)器人編隊可W有效彌補(bǔ)單個 移動機(jī)器人能力的不足，使單個機(jī)器人在編隊行為的群體中擁有了原來所沒有的顯著優(yōu) 勢，通過群體間機(jī)器人的協(xié)調(diào)合作可W完成單個機(jī)器人無法或難W完成的工作。因此，對于 移動機(jī)器人編隊控制系統(tǒng)及控制方法的研究是十分必要的。
[0003] 經(jīng)過對現(xiàn)有文獻(xiàn)的檢索，中國專利申請?zhí)枮?CN201010618568.1，名稱為:基于Ad-化C網(wǎng)絡(luò)和leader-follower算法的多機(jī)器人編隊方法，該發(fā)明提出了一種多機(jī)器人編隊方 法，建立leader運動學(xué)模型，根據(jù)人工勢場法確定follower運動軌跡，并在leader和 follower之間引入Ad-Hoc網(wǎng)絡(luò)，建立信息反饋來保證follower對leader的跟蹤過程無丟 失。但該發(fā)明在領(lǐng)航跟隨者中引入Ad-Hoc網(wǎng)絡(luò)的同時沒有考慮網(wǎng)絡(luò)通信異常的情況，易出 現(xiàn)由于網(wǎng)絡(luò)通信異常造成的跟隨者運動偏離現(xiàn)象。中國專利申請?zhí)枮?201310281499.3,名 稱為:一種基于行為的領(lǐng)航者一跟隨者多智能體隊形控制方法，該發(fā)明公開了一種基于行 為的領(lǐng)航者一跟隨者多智能體隊形控制方法，該方法通過增加候補(bǔ)領(lǐng)航者在一定程度上彌 補(bǔ)領(lǐng)航者在發(fā)生障礙后導(dǎo)致隊形失效的缺陷。但該發(fā)明沒有考慮在領(lǐng)航跟隨編隊中跟隨者 對領(lǐng)航者信息獲取失敗的問題，容易導(dǎo)致跟隨者丟失的情況。文獻(xiàn)名稱為:一種多智能體領(lǐng) 航跟隨編隊新型控制器的設(shè)計，在該算法中通過引入基于鄰居的局部控制率W及基于鄰居 的狀態(tài)估計規(guī)則設(shè)計了一種新型的控制器，在該控制器中通過簡單設(shè)定領(lǐng)航者和跟隨者之 間的相對坐標(biāo)即可方便的實現(xiàn)任意形狀編隊，但該方法將領(lǐng)航跟隨者組成的編隊形成一個 固定或可切換的連接拓?fù)?，缺乏一定的編隊靈活性且沒有考慮到通信拓?fù)涫芟耷闆r下的領(lǐng) 航跟隨編隊問題。
[0004] 通過檢索發(fā)現(xiàn)，現(xiàn)有技術(shù)中關(guān)于移動機(jī)器人領(lǐng)航跟隨編隊運動控制的研究很多， 雖然都能夠?qū)崿F(xiàn)對機(jī)器人的編隊控制，但都沒有考慮到在領(lǐng)航跟隨編隊中通信受限情況下 的跟隨者運動問題，編隊穩(wěn)定性差，缺乏有效的編隊保障。因此，本發(fā)明提出一種基于領(lǐng)航 跟隨的移動機(jī)器人編隊控制方法。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明目的在于提供一種針對領(lǐng)航跟隨編隊運動中的通信異常、跟隨者運動情況 W及整體編隊隊形保持有明顯優(yōu)化的基于領(lǐng)航跟隨的移動機(jī)器人編隊控制方法。
[0006] 為實現(xiàn)上述目的，采用了 W下技術(shù)方案:本發(fā)明所述控制方法包括全局定位系統(tǒng)、 無線通信系統(tǒng)、算法處理系統(tǒng)、調(diào)速系統(tǒng)W及標(biāo)定不同顏色的移動機(jī)器人;全局定位系統(tǒng)利 用攝像頭對不同顏色的移動機(jī)器人進(jìn)行顏色識別，采集移動機(jī)器人的位姿信息;無線通信 系統(tǒng)將采集到的位姿信息發(fā)送給各機(jī)器人;算法處理系統(tǒng)根據(jù)接收到的各移動機(jī)器人位姿 信息做出運動控制及協(xié)調(diào)編隊算法處理，并且不斷地與調(diào)速系統(tǒng)進(jìn)行信息交互，調(diào)速系統(tǒng) 采集處理各移動機(jī)器人的速度信息，對實際控制輸出不斷做出反饋，實時矯正速度偏差，最 終完成各移動機(jī)器人編隊運動的控制。
[0007] 所述控制方法的具體步驟如下：
[0008] 步驟1，在工作平臺中，利用全局定位系統(tǒng)獲取編隊中各機(jī)器人相對于平臺全局坐 標(biāo)系的位置坐標(biāo)信息；
[0009] 步驟2,根據(jù)各機(jī)器人的位置坐標(biāo)信息構(gòu)建領(lǐng)航機(jī)器人與跟隨機(jī)器人之間的相對 位置角度關(guān)系式，定義編隊期望的領(lǐng)航機(jī)器人與跟隨機(jī)器人的距離角度關(guān)系，得到誤差動 力學(xué)方程，進(jìn)而推導(dǎo)出跟隨機(jī)器人的運動控制率；
[0010] 步驟3,在編隊運動過程中，領(lǐng)航機(jī)器人向目標(biāo)點自主運動并通過無線通信系統(tǒng)不 斷向跟隨機(jī)器人發(fā)送信息，跟隨機(jī)器人接收領(lǐng)航機(jī)器人信息并計算自己的位置，記錄跟隨 機(jī)器人每一時刻的位置信息作為訓(xùn)練數(shù)據(jù)，建立基于非線性最小二乘法的跟隨機(jī)器人軌跡 預(yù)測模型；
[0011] 步驟4,利用粒子群算法解決軌跡預(yù)測模型參數(shù)選取問題，將軌跡預(yù)測模型的一組 參數(shù)作為軌跡預(yù)測模型的一組解，初始化粒子群，計算每個粒子的適應(yīng)度函數(shù)，根據(jù)粒子的 適應(yīng)度函數(shù)值更新粒子的個體極值及全局極值；
[0012] 步驟5,利用迭代公式更新每個粒子的速度和位置；
[0013] 步驟6,不斷利用下一時刻的數(shù)據(jù)更新軌跡預(yù)測模型，并向后預(yù)測軌跡信息，定義 一個通信數(shù)據(jù)異常范圍，若接收到的數(shù)據(jù)在正常范圍之內(nèi)，則根據(jù)接收到的領(lǐng)航機(jī)器人數(shù) 據(jù)計算跟隨機(jī)器人位置信息并向其運動;若數(shù)據(jù)異常時，則啟用預(yù)測點代替異常數(shù)據(jù)點作 為下一時刻的跟隨機(jī)器人軌跡點；
[0014] 步驟7,利用全局定位系統(tǒng)不斷更新編隊中各機(jī)器人的位置姿態(tài)信息，直到領(lǐng)航跟 隨編隊到達(dá)目標(biāo)點。
[0015] 進(jìn)一步地，步驟6中，所述的通信數(shù)據(jù)異常范圍，是利用全局定位系統(tǒng)和無線通信 系統(tǒng)采集每帖圖像，并且計算發(fā)送機(jī)器人位置信息的時間間隔及機(jī)器人的平均運動速度來 確定通信數(shù)據(jù)是否為異常數(shù)據(jù)。
[0016] 進(jìn)一步地，步驟6中，所述的數(shù)據(jù)更新策略為，下一時刻的跟隨機(jī)器人軌跡點值是 根據(jù)當(dāng)前時刻的跟隨機(jī)器人軌跡點值與數(shù)據(jù)超出正常范圍的最大闊值關(guān)系來確定的，通過 分別判斷當(dāng)前時刻和下一時刻的跟隨機(jī)器人軌跡點值在沿平臺X、Y軸方向的距離是否滿足 通信數(shù)據(jù)異常范圍來做出更新決定。
[0017] 進(jìn)一步地，所述的全局定位系統(tǒng)由上位機(jī)和一個高清CCD攝像頭組成，將高清CCD 攝像頭置于工作平臺的頂部，對平臺中每個移動機(jī)器人標(biāo)定不同的顏色，利用顏色識別算 法獲取機(jī)器人在平臺中的位置和姿態(tài)信息，再將信息傳回上位機(jī)進(jìn)行處理，上位機(jī)處理后 再發(fā)送給各移動機(jī)器人，從而實現(xiàn)移動機(jī)器人的全局定位。
[0018] 進(jìn)一步地，所述的無線通信系統(tǒng)包括上位機(jī)、移動機(jī)器人及無線路由器，其中移動 機(jī)器人上安裝有無線收發(fā)模塊，上位機(jī)和無線路由通過網(wǎng)線連接，上位機(jī)、移動機(jī)器人和無 線路由器組成基于局域網(wǎng)的通信系統(tǒng)，通過建立上位機(jī)與各機(jī)器人之間W及各機(jī)器人之間 的通信協(xié)議，實現(xiàn)上位機(jī)與機(jī)器人、機(jī)器人與機(jī)器人之間的無線通信。
[0019]進(jìn)一步地，所述算法處理系統(tǒng)，由DSP數(shù)字信號處理器組成，安裝在機(jī)器人內(nèi)部，在 該DSP數(shù)字信號處理器中植入前面所述的基于領(lǐng)航跟隨的移動機(jī)器人編隊運動控制算法， 接收來自無線通信系統(tǒng)的數(shù)據(jù)，對機(jī)器人的自主運動及編隊協(xié)調(diào)控制作出處理；
[0020] 進(jìn)一步地，所述調(diào)速系統(tǒng)包括測速碼盤模塊、Arduino模塊及PID調(diào)速模塊；
[0021] 其中，所述的測速碼盤模塊由光柵盤和光電檢測裝置組成，安裝在機(jī)器人的左右 輪電機(jī)旁，通過計算每秒光電編碼器輸出脈沖的個數(shù)反映當(dāng)前左右輪電機(jī)的轉(zhuǎn)數(shù)，并結(jié)合 車輪轉(zhuǎn)動一周的周長，換算得到移動機(jī)器人電機(jī)左右輪的實際轉(zhuǎn)速；
[0022] 所述的Arduino模塊為Arduino Mega 2560單片機(jī)，安裝在機(jī)器人內(nèi)部，用于擴(kuò)展 機(jī)器人的數(shù)字和模擬輸入輸出口，進(jìn)而采集接收測速碼盤輸出脈沖個數(shù)，對數(shù)據(jù)進(jìn)行分析 處理后，通過I2C總線發(fā)送給算法處理系統(tǒng)；
[0023] 所述的PID調(diào)速模塊是基于BP神經(jīng)網(wǎng)絡(luò)的PID控制器，是在經(jīng)典PID控制器的基礎(chǔ) 上，將Arduino模塊傳回的實測速度與算法處理系統(tǒng)中的理論速度做誤差比較，利用BP神經(jīng) 網(wǎng)絡(luò)優(yōu)化PID控制器參數(shù)，從而實現(xiàn)機(jī)器人在不同運動狀態(tài)下控制器參數(shù)的自適應(yīng)調(diào)節(jié)，實 時矯正速度偏差。
[0024] 與現(xiàn)有技術(shù)相比，本發(fā)明具有如下優(yōu)點：建立跟隨機(jī)器人的軌跡預(yù)測模型，采用改 進(jìn)的粒子群算法優(yōu)化最小二乘預(yù)測模型，彌補(bǔ)了因短暫通信異常而導(dǎo)致的編隊隊