起伏地形環(huán)境下多機(jī)器人動(dòng)態(tài)路徑規(guī)劃算法

張瑞雷，丑永新，丁 衛(wèi)

（常熟理工學(xué)院 電氣與自動(dòng)化工程學(xué)院，江蘇 常熟 215500）

1 引言

多機(jī)器人路徑規(guī)劃［1］在未知環(huán)境探索、災(zāi)后地形重構(gòu)等特定任務(wù)中發(fā)揮著越來(lái)越大的作用，在交通運(yùn)輸、軍事、航空航天、工業(yè)生產(chǎn)等領(lǐng)域有著廣闊的應(yīng)用前景. 例如在軍事和國(guó)家安全領(lǐng)域可以用于偵查搜尋、排雷反恐、區(qū)域巡邏以及物資運(yùn)輸?shù)热蝿?wù)；在倉(cāng)儲(chǔ)管理中可用于包裹分揀、分類擺放；農(nóng)業(yè)上可用于農(nóng)作物區(qū)域協(xié)作播種、灌溉、農(nóng)藥噴灑和收割等. 多機(jī)器人系統(tǒng)路徑規(guī)劃研究?jī)?nèi)容［2］主要包括在環(huán)境中行駛路徑的規(guī)劃、隊(duì)形形成和保持以及隊(duì)形切換等.

目前，傳統(tǒng)機(jī)器人路徑規(guī)劃方法主要有模版匹配圖、勢(shì)場(chǎng)法、地圖構(gòu)建法等，智能路徑規(guī)劃算法有粒子群、蟻群等［3-8］，但主要側(cè)重于單機(jī)器人系統(tǒng)的路徑規(guī)劃. 對(duì)于多機(jī)器人系統(tǒng)還必須考慮編隊(duì)中機(jī)器人之間的沖突和協(xié)作等. 研究隊(duì)形形成和保持的主要方法有領(lǐng)航跟隨法、虛擬結(jié)構(gòu)法、基于行為法等［9］. 相比較而言，由于領(lǐng)航跟隨法具有數(shù)學(xué)描述簡(jiǎn)單、易于分析等優(yōu)點(diǎn)，應(yīng)用更為廣泛. 然而在許多應(yīng)用場(chǎng)合由于環(huán)境約束和任務(wù)需要等［10］原因，需要多機(jī)器人系統(tǒng)在前行過(guò)程中改變隊(duì)形形狀. 隊(duì)形在環(huán)境和任務(wù)中的調(diào)整在現(xiàn)有文獻(xiàn)中研究較少，還沒有統(tǒng)一的模式和方法來(lái)解決這個(gè)問(wèn)題. 研究隊(duì)形切換的主要方法有固定隊(duì)形切換和隊(duì)形自適應(yīng)選擇等，但都要求標(biāo)準(zhǔn)的環(huán)境模型.

如需將編隊(duì)控制應(yīng)用于具體的任務(wù)中，就必須設(shè)計(jì)合適的協(xié)作體系結(jié)構(gòu)，把路徑規(guī)劃、編隊(duì)保持和編隊(duì)切換等功能有機(jī)統(tǒng)一起來(lái). 本文先簡(jiǎn)要介紹編隊(duì)?wèi)?yīng)用的場(chǎng)合，然后設(shè)計(jì)多機(jī)器人系統(tǒng)的全局和局部路徑規(guī)劃，并在此基礎(chǔ)上詳細(xì)描述此動(dòng)態(tài)算法，最后通過(guò)多機(jī)器人在起伏地形環(huán)境下編隊(duì)運(yùn)輸行駛的兩個(gè)具體任務(wù)，驗(yàn)證所提方法的有效性.

2 問(wèn)題描述

2.1 機(jī)器人模型

車式移動(dòng)機(jī)器人對(duì)起伏地形的適應(yīng)能力要強(qiáng)于兩輪差動(dòng)的移動(dòng)機(jī)器人. 本文采用四輪車式移動(dòng)機(jī)器人進(jìn)行路徑規(guī)劃仿真研究. 具體運(yùn)動(dòng)學(xué)模型如圖1所示，其對(duì)應(yīng)公式為式（1）.

其中后輪速度方向與機(jī)器人運(yùn)動(dòng)方向一致，前輪繞垂軸轉(zhuǎn)動(dòng)，l表示前輪與后輪間的軸間距，機(jī)器人的位姿向量為p=[x,y,θ,φ]T. 其中，(x,y)T為后輪輪軸中心C的坐標(biāo)，θ為車身方向與x軸的夾角，φ為前輪轉(zhuǎn)向角，v為后輪前進(jìn)速度，ω為前輪轉(zhuǎn)向角速度，(v,ω)T為機(jī)器人的控制輸入.

圖1 車式移動(dòng)機(jī)器人模型

2.2 地形環(huán)境

本文主要研究地面移動(dòng)機(jī)器人，而地面移動(dòng)機(jī)器人通常在地形表面運(yùn)動(dòng). 在地理學(xué)上，通常采用等高圖來(lái)描述起伏地形. 由等高圖可以轉(zhuǎn)換得到含有高度信息的地形圖，如圖2所示. 然后將地形導(dǎo)入到PhysX物理引擎中繪制并生成三維起伏地形環(huán)境.

起伏地形通常用于仿真野外環(huán)境，如山地、土坡以及隕石坑等，用于測(cè)試機(jī)器人在起伏地形環(huán)境下如何克服地形對(duì)編隊(duì)控制以及路徑規(guī)劃帶來(lái)的影響. 平緩起伏地形主要用來(lái)測(cè)試地形崎嶇起伏對(duì)機(jī)器人運(yùn)動(dòng)性能以及編隊(duì)精度的影響. 陡峭起伏地形主要用來(lái)測(cè)試編隊(duì)對(duì)環(huán)境的適應(yīng)能力，如不可通過(guò)區(qū)域隊(duì)形切換以及路徑規(guī)劃的算法有效性. 平緩起伏地形設(shè)計(jì)如圖3所示，高度單位為m.

地形可設(shè)置多種參數(shù)，其中包括還原系數(shù)（指物體碰撞后速度和碰撞前速度的比值）、靜摩擦力系數(shù)、滑動(dòng)摩擦力系數(shù)等，用于近似模擬實(shí)際實(shí)驗(yàn)中不同材質(zhì)的地形特性，如干燥木板的還原系數(shù)0.2～0.4，靜摩擦力系數(shù)為0.4～0.6，滑動(dòng)摩擦力系數(shù)為0.2～0.5.

圖2 起伏地形模型

圖3 平緩起伏地形

2.3 雙向RRT路徑規(guī)劃算法

雙向RRT規(guī)劃算法如圖4所示. 受到經(jīng)典雙向搜索理論的啟發(fā)，可以通過(guò)兩個(gè)RRTs增長(zhǎng)（一個(gè)來(lái)自xinit，另一個(gè)來(lái)自xgoal）改善系統(tǒng)性能. 當(dāng)這兩個(gè)RRTs相遇時(shí)，一條可行的路徑就規(guī)劃出來(lái)了. 在復(fù)雜二維環(huán)境下RRT與雙向RRT路徑規(guī)劃曲線如圖5所示. 起伏地形環(huán)境下RRT算法將結(jié)合仿真進(jìn)行說(shuō)明.

圖4 雙向RRT算法

3 路徑規(guī)劃方法

3.1 全局路徑規(guī)劃

為了更好適應(yīng)起伏地形，機(jī)器人選擇四輪車式移動(dòng)機(jī)器人.同時(shí)，在地形設(shè)計(jì)時(shí)需要考慮起伏地形的典型特征，需要具備凸起和凹下兩種陡峭地形. 設(shè)計(jì)的兩種典型測(cè)試地形的等高線圖和高度圖如圖6所示，高度標(biāo)尺單位為m. 地形1狹窄通道寬于地形2.地形1用于測(cè)試固定隊(duì)形編隊(duì)運(yùn)輸行駛，地形2用于測(cè)試切換隊(duì)形編隊(duì)運(yùn)輸行駛.

圖5 地形2的RRT路徑規(guī)劃流程

圖6 地形設(shè)計(jì)

在完成地形設(shè)計(jì)后，需要依據(jù)地形和編隊(duì)運(yùn)輸行駛?cè)蝿?wù)設(shè)置的起點(diǎn)和終點(diǎn)規(guī)劃行駛參考路徑. 全局路徑由任務(wù)規(guī)劃模塊生成. 以地形2為例，在起伏地形環(huán)境下的RRT路徑規(guī)劃過(guò)程如圖6所示.

在起伏地形環(huán)境下，經(jīng)過(guò)平滑和優(yōu)化后的參考路徑如圖7所示. 在仿真初始化時(shí)，由任務(wù)規(guī)劃模塊將路徑傳輸給領(lǐng)航機(jī)器人作為行駛參考.

3.2 局部路徑規(guī)劃

無(wú)障礙空曠地形如圖8所示. （a）圖為視覺信息，（b）圖為經(jīng)過(guò)處理的深度信息，顏色越深表示障礙物或地面離機(jī)器人越近. 在這種特征地形下，領(lǐng)航機(jī)器人不改變?cè)新窂椒较颍凑找?guī)劃路線行駛，跟隨機(jī)器人保持原有隊(duì)形行駛.

單側(cè)不可通過(guò)：凹下和凸起地形分別如圖9和圖10所示. 機(jī)器人可以從另一側(cè)通過(guò)，領(lǐng)航機(jī)器人左轉(zhuǎn)或右轉(zhuǎn)一定角度避開突然凸起和下凹地形，跟隨機(jī)器人保持隊(duì)形并避開危險(xiǎn)地形. 由于RRT路徑規(guī)劃器路徑有可能貼近這些地形邊緣，為了避免意外，需要依據(jù)地形調(diào)整行駛路徑.

中間可通過(guò)：兩側(cè)凸起和凹下地形分別如圖11和圖12所示. 領(lǐng)航機(jī)器人沿著中心通過(guò)該地形，跟隨機(jī)器人切換為柔性隊(duì)形，跟隨領(lǐng)航機(jī)器人，不保持原有隊(duì)形，防止跟隨機(jī)器人跌落和碰撞等.

上下坡：如圖13和圖14所示，深度信息相似，結(jié)合姿態(tài)傳感器綜合判斷地形，適當(dāng)減速以減少行駛過(guò)程的顛簸，激活地形行駛策略以減少打滑、卡住等故障.

不可通過(guò)地形：正常的路徑規(guī)劃一般不會(huì)遇到這種情況. 但是為了防止環(huán)境中存在的不確定性等，對(duì)凸起和凹下不可通過(guò)的地形（如圖15和圖16所示），領(lǐng)航機(jī)器人必須停止行駛、倒退、右轉(zhuǎn)或者左轉(zhuǎn)繞行，但在全局路徑規(guī)劃中應(yīng)該通過(guò)參數(shù)優(yōu)化、路徑優(yōu)化等，盡可能避免出現(xiàn)這種情況.

圖7 參考行駛路徑

圖8 空曠地形

圖9 單側(cè)凹下地形

圖10 單側(cè)凸起地形

4 仿真結(jié)果與分析

4.1 固定隊(duì)形多機(jī)器人編隊(duì)運(yùn)輸任務(wù)

4個(gè)車式移動(dòng)機(jī)器人初始位姿分別為：

期望隊(duì)形參數(shù)：

編隊(duì)中機(jī)器人由初始位置先完成菱形編隊(duì)，然后保持隊(duì)形沿著預(yù)定參考軌跡行駛到目標(biāo)位置. 環(huán)境中地形由圖7（a）導(dǎo)入MRDS生成，可見環(huán)境地形起伏明顯，其中高坡和峽谷地形是機(jī)器人前進(jìn)過(guò)程中需要盡可能避免的. 沿著參考軌跡在較為平緩的地形上行駛，由于仿真采用物理引擎，多機(jī)器人編隊(duì)行駛仿真過(guò)程中存在隨機(jī)的環(huán)境噪聲、CPU占用率、分布式線程響應(yīng)等因素影響，以及仿真最大速度設(shè)置等不同，每次完成時(shí)間不同. 在同一最大速度指標(biāo)下，誤差小于3 s. 只要編隊(duì)中有一個(gè)機(jī)器人卡死或翻車就判定任務(wù)失敗. 在編隊(duì)運(yùn)輸過(guò)程中還會(huì)出現(xiàn)運(yùn)輸車輛碰撞陡坡和滑落深坑等問(wèn)題，但概率很小.

各機(jī)器人的軌跡如圖17所示，其中a，b，c，d 4個(gè)點(diǎn)的編隊(duì)仿真場(chǎng)景如圖18所示. 雖然參考路徑選擇平緩地形優(yōu)先進(jìn)行路徑規(guī)劃，但是由于行駛地面崎嶇不平，機(jī)器人行駛?cè)匀槐容^顛簸，地形顛簸的程度可以由領(lǐng)航機(jī)器人的姿態(tài)曲線（圖19）看出. 其中α為俯仰角，β為傾斜角，機(jī)器人姿態(tài)角度大部分小于10°，可知全局路徑規(guī)劃使機(jī)器人編隊(duì)基本在平緩起伏的路面上行駛. 跟隨機(jī)器人的位置誤差如圖20所示，可以看出編隊(duì)以較小誤差沿著預(yù)定軌跡從起點(diǎn)安全行駛到終點(diǎn).

如圖18所示，在編隊(duì)兩側(cè)的機(jī)器人由于行駛路面狹窄容易發(fā)生卡住、翻車等問(wèn)題，需要通過(guò)靈活隊(duì)形切換策略更好解決這類問(wèn)題.

4.2 調(diào)整隊(duì)形多機(jī)器人編隊(duì)運(yùn)輸任務(wù)

圖11 兩側(cè)凹下地形

圖12 兩側(cè)凸起地形

圖13 上坡地形

圖14 下坡地形

圖15 陡峭凸起地形

圖16 陡峭凹下地形

圖17 編隊(duì)各機(jī)器人實(shí)際軌跡圖

圖18 行駛過(guò)程場(chǎng)景

機(jī)器人初始位姿分別為：

期望隊(duì)形參數(shù)：

編隊(duì)中機(jī)器人由初始位置先完成矩形編隊(duì)，然后保持隊(duì)形沿著預(yù)定參考軌跡行駛到目標(biāo)位置. 環(huán)境中地形由圖7（b）導(dǎo)入MRDS生成，可見環(huán)境中地形起伏明顯，行駛過(guò)程遇到狹窄地形，需要切換為柔性隊(duì)形. 只要編隊(duì)中有一個(gè)機(jī)器人卡死或翻車就判定任務(wù)失敗. 任務(wù)失敗如圖21所示，機(jī)器人R4在通過(guò)狹窄通道進(jìn)行隊(duì)形調(diào)整過(guò)程中，由于坡度較陡峭，出現(xiàn)翻車故障，但是其他機(jī)器人仍然能夠保持編隊(duì)行駛. 由于地形環(huán)境限制，機(jī)器人隊(duì)形進(jìn)行了8次隊(duì)形調(diào)整. 每次隊(duì)形調(diào)整，由于機(jī)器人當(dāng)前位置遠(yuǎn)離配置位置，都會(huì)有較大的初始誤差，但都很快收斂. 編隊(duì)切換過(guò)程如圖22所示.

圖19 領(lǐng)航機(jī)器人姿態(tài)

圖20 跟隨機(jī)器人位置誤差

圖21 編隊(duì)切換失敗

圖22 編隊(duì)切換過(guò)程

圖23 編隊(duì)路徑軌跡圖

圖24 領(lǐng)航機(jī)器人姿態(tài)

圖25 跟隨機(jī)器人位置誤差

由于地形起伏并且機(jī)器人行駛速度較快，對(duì)隊(duì)形切換速度要求較高，不能采用平緩隊(duì)形切換策略. 跟隨機(jī)器人通過(guò)參考全局路徑信息，結(jié)合地形對(duì)隊(duì)形進(jìn)行動(dòng)態(tài)調(diào)整. 由于起伏地形行駛過(guò)程較為顛簸，機(jī)器人配置的深度視覺傳感器會(huì)受到干擾，所以每次隊(duì)形切換時(shí)間會(huì)有差別.

各機(jī)器人的軌跡如圖23所示. 雖然參考路徑選擇平緩地形優(yōu)先進(jìn)行路徑規(guī)劃，但是由于行駛地面崎嶇不平，地形顛簸起伏的程度可以由領(lǐng)航機(jī)器人的姿態(tài)曲線（圖24）看出. 跟隨機(jī)器人的位置誤差如圖25所示，可以看出編隊(duì)以較小誤差沿著預(yù)定軌跡能夠自主調(diào)整隊(duì)形從起點(diǎn)安全行駛到終點(diǎn).

5 總結(jié)與展望

本文研究了多機(jī)器人在起伏地形環(huán)境下的路徑規(guī)劃問(wèn)題. 首先， 依據(jù)坡度對(duì)地形特征進(jìn)行處理得到可規(guī)劃路徑的區(qū)域，并應(yīng)用雙向RRT算法進(jìn)行全局路徑規(guī)劃. 對(duì)于可規(guī)劃區(qū)域邊緣易出現(xiàn)事故等問(wèn)題，利用深度視覺傳感器，將局部行為策略和全局路徑規(guī)劃相結(jié)合，并通過(guò)兩個(gè)典型的起伏地形下多機(jī)器人路徑規(guī)劃仿真，驗(yàn)證了所提方法的有效性. 但是本文僅考慮環(huán)境中地形起伏，沒有考慮有亂石、樹木等障礙物情況，需要進(jìn)一步研究. 同時(shí)也需要提高仿真中路徑規(guī)劃的成功率及切換隊(duì)形的效率等.