一種基于領航跟隨的移動機器人編隊控制方法
【技術領域】
[0001] 本發明設及移動機器人運動控制領域，尤其設及一種基于領航跟隨的移動機器人 編隊運動控制方法。
【背景技術】
[0002] 隨著機器人技術的發展，移動機器人在應用范圍和功能方面都得到了較大的拓展 和提高，并且廣泛應用于國防、工業、服務業等行業。多移動機器人編隊可W有效彌補單個 移動機器人能力的不足，使單個機器人在編隊行為的群體中擁有了原來所沒有的顯著優 勢，通過群體間機器人的協調合作可W完成單個機器人無法或難W完成的工作。因此，對于 移動機器人編隊控制系統及控制方法的研究是十分必要的。
[0003] 經過對現有文獻的檢索，中國專利申請號為:CN201010618568.1，名稱為:基于Ad-化C網絡和leader-follower算法的多機器人編隊方法，該發明提出了一種多機器人編隊方 法，建立leader運動學模型，根據人工勢場法確定follower運動軌跡，并在leader和 follower之間引入Ad-Hoc網絡，建立信息反饋來保證follower對leader的跟蹤過程無丟 失。但該發明在領航跟隨者中引入Ad-Hoc網絡的同時沒有考慮網絡通信異常的情況，易出 現由于網絡通信異常造成的跟隨者運動偏離現象。中國專利申請號為:201310281499.3,名 稱為:一種基于行為的領航者一跟隨者多智能體隊形控制方法，該發明公開了一種基于行 為的領航者一跟隨者多智能體隊形控制方法，該方法通過增加候補領航者在一定程度上彌 補領航者在發生障礙后導致隊形失效的缺陷。但該發明沒有考慮在領航跟隨編隊中跟隨者 對領航者信息獲取失敗的問題，容易導致跟隨者丟失的情況。文獻名稱為:一種多智能體領 航跟隨編隊新型控制器的設計，在該算法中通過引入基于鄰居的局部控制率W及基于鄰居 的狀態估計規則設計了一種新型的控制器，在該控制器中通過簡單設定領航者和跟隨者之 間的相對坐標即可方便的實現任意形狀編隊，但該方法將領航跟隨者組成的編隊形成一個 固定或可切換的連接拓撲，缺乏一定的編隊靈活性且沒有考慮到通信拓撲受限情況下的領 航跟隨編隊問題。
[0004] 通過檢索發現，現有技術中關于移動機器人領航跟隨編隊運動控制的研究很多， 雖然都能夠實現對機器人的編隊控制，但都沒有考慮到在領航跟隨編隊中通信受限情況下 的跟隨者運動問題，編隊穩定性差，缺乏有效的編隊保障。因此，本發明提出一種基于領航 跟隨的移動機器人編隊控制方法。
【發明內容】
[0005] 本發明目的在于提供一種針對領航跟隨編隊運動中的通信異常、跟隨者運動情況 W及整體編隊隊形保持有明顯優化的基于領航跟隨的移動機器人編隊控制方法。
[0006] 為實現上述目的，采用了 W下技術方案:本發明所述控制方法包括全局定位系統、 無線通信系統、算法處理系統、調速系統W及標定不同顏色的移動機器人;全局定位系統利 用攝像頭對不同顏色的移動機器人進行顏色識別，采集移動機器人的位姿信息;無線通信 系統將采集到的位姿信息發送給各機器人;算法處理系統根據接收到的各移動機器人位姿 信息做出運動控制及協調編隊算法處理，并且不斷地與調速系統進行信息交互，調速系統 采集處理各移動機器人的速度信息，對實際控制輸出不斷做出反饋，實時矯正速度偏差，最 終完成各移動機器人編隊運動的控制。
[0007] 所述控制方法的具體步驟如下：
[0008] 步驟1，在工作平臺中，利用全局定位系統獲取編隊中各機器人相對于平臺全局坐 標系的位置坐標信息；
[0009] 步驟2,根據各機器人的位置坐標信息構建領航機器人與跟隨機器人之間的相對 位置角度關系式，定義編隊期望的領航機器人與跟隨機器人的距離角度關系，得到誤差動 力學方程，進而推導出跟隨機器人的運動控制率；
[0010] 步驟3,在編隊運動過程中，領航機器人向目標點自主運動并通過無線通信系統不 斷向跟隨機器人發送信息，跟隨機器人接收領航機器人信息并計算自己的位置，記錄跟隨 機器人每一時刻的位置信息作為訓練數據，建立基于非線性最小二乘法的跟隨機器人軌跡 預測模型；
[0011] 步驟4,利用粒子群算法解決軌跡預測模型參數選取問題，將軌跡預測模型的一組 參數作為軌跡預測模型的一組解，初始化粒子群，計算每個粒子的適應度函數，根據粒子的 適應度函數值更新粒子的個體極值及全局極值；
[0012] 步驟5,利用迭代公式更新每個粒子的速度和位置；
[0013] 步驟6,不斷利用下一時刻的數據更新軌跡預測模型，并向后預測軌跡信息，定義 一個通信數據異常范圍，若接收到的數據在正常范圍之內，則根據接收到的領航機器人數 據計算跟隨機器人位置信息并向其運動;若數據異常時，則啟用預測點代替異常數據點作 為下一時刻的跟隨機器人軌跡點；
[0014] 步驟7,利用全局定位系統不斷更新編隊中各機器人的位置姿態信息，直到領航跟 隨編隊到達目標點。
[0015] 進一步地，步驟6中，所述的通信數據異常范圍，是利用全局定位系統和無線通信 系統采集每帖圖像，并且計算發送機器人位置信息的時間間隔及機器人的平均運動速度來 確定通信數據是否為異常數據。
[0016] 進一步地，步驟6中，所述的數據更新策略為，下一時刻的跟隨機器人軌跡點值是 根據當前時刻的跟隨機器人軌跡點值與數據超出正常范圍的最大闊值關系來確定的，通過 分別判斷當前時刻和下一時刻的跟隨機器人軌跡點值在沿平臺X、Y軸方向的距離是否滿足 通信數據異常范圍來做出更新決定。
[0017] 進一步地，所述的全局定位系統由上位機和一個高清CCD攝像頭組成，將高清CCD 攝像頭置于工作平臺的頂部，對平臺中每個移動機器人標定不同的顏色，利用顏色識別算 法獲取機器人在平臺中的位置和姿態信息，再將信息傳回上位機進行處理，上位機處理后 再發送給各移動機器人，從而實現移動機器人的全局定位。
[0018] 進一步地，所述的無線通信系統包括上位機、移動機器人及無線路由器，其中移動 機器人上安裝有無線收發模塊，上位機和無線路由通過網線連接，上位機、移動機器人和無 線路由器組成基于局域網的通信系統，通過建立上位機與各機器人之間W及各機器人之間 的通信協議，實現上位機與機器人、機器人與機器人之間的無線通信。
[0019]進一步地，所述算法處理系統，由DSP數字信號處理器組成，安裝在機器人內部，在 該DSP數字信號處理器中植入前面所述的基于領航跟隨的移動機器人編隊運動控制算法， 接收來自無線通信系統的數據，對機器人的自主運動及編隊協調控制作出處理；
[0020] 進一步地，所述調速系統包括測速碼盤模塊、Arduino模塊及PID調速模塊；
[0021] 其中，所述的測速碼盤模塊由光柵盤和光電檢測裝置組成，安裝在機器人的左右 輪電機旁，通過計算每秒光電編碼器輸出脈沖的個數反映當前左右輪電機的轉數，并結合 車輪轉動一周的周長，換算得到移動機器人電機左右輪的實際轉速；
[0022] 所述的Arduino模塊為Arduino Mega 2560單片機，安裝在機器人內部，用于擴展 機器人的數字和模擬輸入輸出口，進而采集接收測速碼盤輸出脈沖個數，對數據進行分析 處理后，通過I2C總線發送給算法處理系統；
[0023] 所述的PID調速模塊是基于BP神經網絡的PID控制器，是在經典PID控制器的基礎 上，將Arduino模塊傳回的實測速度與算法處理系統中的理論速度做誤差比較，利用BP神經 網絡優化PID控制器參數，從而實現機器人在不同運動狀態下控制器參數的自適應調節，實 時矯正速度偏差。
[0024] 與現有技術相比，本發明具有如下優點：建立跟隨機器人的軌跡預測模型，采用改 進的粒子群算法優化最小二乘預測模型，彌補了因短暫通信異常而導致的編隊隊