應(yīng)急場景通信機(jī)器人布放路徑規(guī)劃

孫 弋，雷錦航

(西安科技大學(xué) 通信與信息工程學(xué)院， 西安 710600)

0 引言

通信基站布放在應(yīng)急場景下的機(jī)器人通信系統(tǒng)搭建過程中具有十分重要的作用[1]。采用具有通信能力的通信機(jī)器人進(jìn)行通信站點布放就是在一定環(huán)境下尋找一條連接起始點到目標(biāo)點的可行路徑的過程[2]。該過程即路徑規(guī)劃，是機(jī)器人技術(shù)的重要研究方向[3]。

路徑規(guī)劃可表示為：在已知范圍內(nèi)，機(jī)器人自動搜尋一條連接起始點到目標(biāo)點的最優(yōu)可通行路徑[4]。目前主要路徑規(guī)劃算法有人工勢場法[5]、A*算法[6]、Dijkstra 算法[7]、可視圖法[8]和快速擴(kuò)展隨機(jī)樹(RRT)算法等。其中，RRT算法因更適合移動機(jī)器人自主路徑規(guī)劃而被廣泛應(yīng)用及改進(jìn)[9]。RRT算法優(yōu)點在于無需對所需場景進(jìn)行建模，僅通過隨機(jī)樹擴(kuò)展就可以找到一條通往目標(biāo)點的可行路徑，但正是因為隨機(jī)樹擴(kuò)展的隨機(jī)性導(dǎo)致最終產(chǎn)生的路徑隨機(jī)性過強(qiáng)、冗余節(jié)點過多且尋路運(yùn)算過程耗時，增加了計算機(jī)內(nèi)存負(fù)擔(dān)[10]。

為彌補(bǔ)經(jīng)典RRT算法的不足，研究學(xué)者提出了以目標(biāo)點為導(dǎo)向[11]的采樣方法，以大于p的概率將目標(biāo)點采樣為隨機(jī)點，提高了RRT算法的目標(biāo)導(dǎo)向性，但是依舊存在路徑節(jié)點數(shù)過多和冗余支路的問題。劉恩海等[12]引入目標(biāo)點吸引力思想，改變了RRT擴(kuò)展無方向性的問題，但算法尋路效率依然較低。Wan等[13]將RRT算法與人工勢場法結(jié)合，減小了RRT算法的采樣個數(shù)，但其算法搜索時間加長。楊洪濤[14]在RRT中引入至目標(biāo)點的初始方向向量，加強(qiáng)了隨機(jī)樹擴(kuò)展的方向性問題，但其方向性過強(qiáng)，導(dǎo)致RRT擴(kuò)展極易陷入隨機(jī)樹擴(kuò)展失敗的局面。閆明亮等[15]提出了雙采樣點的RRT改進(jìn)方法，也在一定程度上加強(qiáng)了RRT擴(kuò)展的導(dǎo)向性，但其在路徑長度上并未有明顯改善。趙惠等[16]結(jié)合目標(biāo)偏置和雙采樣點原則對RRT進(jìn)行總體改進(jìn)，其在路經(jīng)長度、尋路時間上均有所改進(jìn)，但易陷入RRT節(jié)點擴(kuò)展不易的局面。

針對以上RRT算法中的不足，引入目標(biāo)偏置采樣策略，減少RRT算法的采樣盲目性；引入剪枝采樣方法，通過計算新節(jié)點位置對下一采樣位置進(jìn)行約束，減少冗余支路的產(chǎn)生；引入目標(biāo)點引力、隨機(jī)點引力及權(quán)重距離系數(shù)，實時調(diào)整新節(jié)點生成方向和步長，提高路徑生成效率；結(jié)合三次B樣條對路徑進(jìn)行平緩，增強(qiáng)通信機(jī)器人行駛的穩(wěn)定性。

1 問題模型建立

近年來，各地受災(zāi)情況時有發(fā)生，導(dǎo)致環(huán)境原有形態(tài)遭到破壞且通信網(wǎng)絡(luò)癱瘓。針對這種情況，采用機(jī)器人代替人類進(jìn)入受災(zāi)現(xiàn)場實現(xiàn)臨時通信網(wǎng)的建立。現(xiàn)有的機(jī)器人一般都具有導(dǎo)航定位、地形探測、無線通信等眾多功能[17]。因此，采用多個具備無線通信和傳感器等設(shè)備的通信機(jī)器人來充當(dāng)應(yīng)急場景下的臨時通信基站。

臨時通信網(wǎng)組建過程中，需要將具備通信功能的通信機(jī)器人布放到合適的基站位置。布放過程即找到一條連接基站通信機(jī)器人和基站站點的無障礙路徑。由于最終路徑僅在非障礙區(qū)出現(xiàn)，將三維空間的應(yīng)急場景簡化為平面的俯視二維圖。圖1為應(yīng)急場景下的通信基站布放俯視圖，黑色部分代表障礙物區(qū)域，白色空白區(qū)域代表非障礙區(qū)域，左上角為中心通信機(jī)器人位置和基站通信機(jī)器人起始位置，即為中心站點，右下角為基站通信機(jī)器人布放位置，即為基站站點。通信機(jī)器人通信半徑均為R，弧線L1為中心通信機(jī)器人可通信的通信邊界位置，弧線L2為基站通信機(jī)器人布放成功后可通信的通信邊界位置，各通信機(jī)器人構(gòu)成臨時通信網(wǎng)絡(luò)。針對基站通信機(jī)器人到達(dá)指定基站站點的路徑規(guī)劃過程進(jìn)行研究。

圖1 通信基站布放示意圖

2 基站通信機(jī)器人布放路徑規(guī)劃

2.1 基于RRT的機(jī)器人路徑規(guī)劃

將中心站點、基站站點質(zhì)點化，中心基站置為坐標(biāo)原點，規(guī)定以中心站點為RRT算法的根節(jié)點qinit，基站站點為RRT算法的目標(biāo)點qgoal，初始化隨機(jī)樹，并在應(yīng)急場景內(nèi)以隨機(jī)采樣的方式向非障礙區(qū)生成一隨機(jī)點qrand。遍歷樹上現(xiàn)有節(jié)點，找到距隨機(jī)點qrand最近的樹節(jié)點qnear，以樹節(jié)點qnear為起始點，向隨機(jī)點qrand方向生長λ，得到新節(jié)點qnew。判斷新節(jié)點qnew與樹節(jié)點qnear之間是否存在障礙物，若存在障礙物，則此次生長作廢，重新進(jìn)行隨機(jī)采樣過程，否則將新節(jié)點qnew加入樹中，成為樹節(jié)點。判斷新加入的樹節(jié)點是否進(jìn)入目標(biāo)點qgoal的閾值Thre范圍內(nèi)，若進(jìn)入，停止樹的擴(kuò)展，并從目標(biāo)點qgoal回溯，找到一條連通目標(biāo)點qgoal和根節(jié)點qinit的可通行路徑；否則，重新進(jìn)入隨機(jī)采樣過程，直到找到最終路徑。RRT通信機(jī)器人布放路徑規(guī)劃過程如圖2所示。

圖2 基于RRT通信機(jī)器人布放路徑規(guī)劃過程示意圖

2.2 存在問題

依據(jù)基站通信機(jī)器人布放路徑規(guī)劃過程，基站通信機(jī)器人初始布放路徑如圖3所示，黑色加粗線條為基站通信機(jī)器人最終布放路徑，藍(lán)色線條代表廢棄路徑，藍(lán)色小點代表廢棄路徑節(jié)點。可以看出，運(yùn)用傳統(tǒng)RRT進(jìn)行基站布放存在一些不足之處。

圖3 基站通信機(jī)器人初始布放路徑示意圖

1) RRT算法進(jìn)行尋路是隨機(jī)的，在相同場景中多次進(jìn)行路徑規(guī)劃，得出的路徑結(jié)果并不相同，甚至與最優(yōu)路徑相差甚遠(yuǎn)。

2) 尋路過程中隨機(jī)點位置選取在整個范圍內(nèi)進(jìn)行，導(dǎo)致最終尋路成功后產(chǎn)生多余的冗余路徑節(jié)點被儲存，產(chǎn)生冗余支路，且容易出現(xiàn)規(guī)劃路徑時間過長的情況。

3) RRT算法最終生成路徑由各個路徑節(jié)點連接而成，并不是一條適合機(jī)器人行走的光滑曲線。

3 機(jī)器人代價評估函數(shù)

為便于對比改進(jìn)算法和未改進(jìn)算法，對模型中的機(jī)器人消耗代價做如下定義：

1) RRT算法規(guī)劃出的路徑由目標(biāo)點回溯得到，最終路徑長度為路徑上兩相鄰節(jié)點間距離之和。機(jī)器人速度恒定，單位距離內(nèi)機(jī)器人能量消耗一定，因此規(guī)定行駛這段距離所產(chǎn)生的的能耗代價由起始點到目標(biāo)點的歐幾里得距離確定。

(1)

式中：qk表示路徑節(jié)點的位置；qk-1表示路徑前一節(jié)點的位置；dis(qk,qk-1)表示機(jī)器人從qk-1行駛到qk的能量消耗。

2) RRT算法尋路過程中會產(chǎn)生尋路節(jié)點和路徑節(jié)點，其中尋路節(jié)點為不可用節(jié)點，路徑節(jié)點為可用節(jié)點，兩類節(jié)點皆占用機(jī)器人內(nèi)存，規(guī)定路徑節(jié)點與尋路節(jié)點比例為機(jī)器人內(nèi)存利用率。

(2)

式中：r(t)表示尋路時間內(nèi)產(chǎn)生的路徑節(jié)點數(shù)；f(t)表示尋路時間內(nèi)產(chǎn)生的尋路節(jié)點數(shù)。

3) RRT算法尋路時間為尋路開始與尋路完畢的時間差，規(guī)定該時間差為機(jī)器人尋路時間代價。

T(t)=T(qgoal)-T(qinit)

(3)

式中：T(qgoal)代表尋路結(jié)束時間；T(qinit)代表尋路開始時間。

4) 路徑轉(zhuǎn)折度依據(jù)路徑節(jié)點之間的夾角來判斷路徑是否平滑，規(guī)定路徑轉(zhuǎn)折度為機(jī)器人行駛過程中的穩(wěn)定代價。

(4)

式中：A(qk-1,qk)表示路徑qk-1qk與路徑qkqk+1的夾角。

4 改進(jìn)基站通信機(jī)器人布放路徑規(guī)劃

針對傳統(tǒng)RRT算法規(guī)劃路徑存在的問題，分別采用目標(biāo)概率采樣、剪枝采樣和引力場思想來對RRT算法進(jìn)行改進(jìn)，提升整體規(guī)劃效率。其擴(kuò)展流程如下：

Step1定義中心通信機(jī)器人位置為起始點qinit，以該位置為起點進(jìn)行隨機(jī)樹擴(kuò)展，基站通信機(jī)器人布放位置為目標(biāo)點qgoal，目標(biāo)閾值thre，目標(biāo)偏置采樣概率pa，剪枝采樣半徑new_dis，擴(kuò)展步長λ，最大迭代次數(shù)iter；

Step2以偏向概率pa進(jìn)行采樣點qrand的選擇；

Step3計算dis(qrand,qgoal)，并與剪枝采樣半徑進(jìn)行比較，若小于該半徑，則進(jìn)入Step 4，否則進(jìn)入Step 2；

Step4尋找距qrand最近的樹節(jié)點qnear。

Step5引入目標(biāo)點引力、隨機(jī)點引力、權(quán)重距離系數(shù)共同引導(dǎo)qnew生成；

Step6檢測qnear與qnew之間是否存在障礙物，若存在，則廢除此次擴(kuò)展，重新進(jìn)入Step 2；否則，將qnew添加到樹上；

Step7判斷qnew是否進(jìn)入目標(biāo)閾值thre范圍內(nèi)，若進(jìn)入，進(jìn)入Step 8；否則，進(jìn)入Step 2；

Step8對生成的路徑節(jié)點進(jìn)行采集，利用3次B樣條對路徑進(jìn)行平滑，得到最終路徑。

圖4為改進(jìn)的基站通信機(jī)器人布放流程框圖。

圖4 改進(jìn)的基站通信機(jī)器人布放流程框圖

4.1 目標(biāo)偏置采樣

基本RRT算法采樣在整個空間X內(nèi)進(jìn)行，算法采樣過程的隨機(jī)性強(qiáng)，致使隨機(jī)樹擴(kuò)展存在一定盲目性，導(dǎo)致最終獲取的路徑冗余節(jié)點過多，且算法迭代時間過長。為提高采樣的方向性，結(jié)合偏置概率采樣的思想，預(yù)設(shè)采樣概率pa，通過函數(shù)均勻生成概率p，p∈(0,1)；以大于pa的概率將目標(biāo)點qgoal作為采樣點，其余采樣點依然在采樣空間X內(nèi)隨機(jī)選取。如式(5)所示，目標(biāo)偏置采樣式為：

(5)

式中X(q)為采樣空間X內(nèi)的隨機(jī)采樣點。

4.2 剪枝采樣

以一定偏置概率進(jìn)行采樣后，除開最終需要的可通行路徑外，存在一系列的分叉路徑，導(dǎo)致尋路成功后依然存在冗余采樣點形成的冗余路徑，消耗機(jī)器人內(nèi)存空間。為避免這些多余路徑的形成，對導(dǎo)致多余路徑形成的隨機(jī)采樣點采樣位置進(jìn)行約束，其約束思想為：每迭代產(chǎn)生1個新節(jié)點qnew，即對新節(jié)點qnew位置進(jìn)行判斷，并計算新節(jié)點qnew與目標(biāo)點qgoal之間的直線距離，以目標(biāo)點qgoal為圓心，該距離為半徑，對下次迭代的采樣點位置進(jìn)行約束。若下一采樣點在約束范圍內(nèi)采樣，則采樣成功；否則，重新進(jìn)行采樣過程，直到采樣點出現(xiàn)在約束范圍內(nèi)。規(guī)定初始采樣半徑由起始點和目標(biāo)點的位置決定，之后根據(jù)每次新節(jié)點位置更新采樣范圍。圖5所示為剪枝采樣示意圖。

圖5 剪枝采樣示意圖

采樣部分偽代碼如下：

T.init

new_dis=sqrt(qinit,qgoal)；

fori=count

隨機(jī)生成一個采樣概率p；

if 0

進(jìn)行隨機(jī)采樣，得到預(yù)備采樣點qrand；

else

qrand=qgoal；

end

計算rand_dis=sqrt(qrand,qgoal)；

ifrand_dis

采樣節(jié)點生成qrand；

else

continue；

end

end

依據(jù)生成新節(jié)點qnew；

重置new_dis=sqrt(qnew,qgoal)，進(jìn)行下一次迭代

4.3 變步長擴(kuò)展

基本RRT算法在新節(jié)點擴(kuò)展中采用恒定步長進(jìn)行擴(kuò)展，其擴(kuò)展方向均由采樣點決定，致使尋路方向隨機(jī)且尋路時間過長。為進(jìn)一步減小規(guī)劃時間和優(yōu)化節(jié)點擴(kuò)展方向，結(jié)合引力場思想和權(quán)重距離系數(shù)，在生成新節(jié)點時，引入隨機(jī)點引力和目標(biāo)點引力兩力合一來引導(dǎo)新節(jié)點生長，并依據(jù)權(quán)重距離系數(shù)對兩者比例進(jìn)行自適應(yīng)調(diào)控，使得隨機(jī)樹在遠(yuǎn)離障礙物時獲得偏向目標(biāo)點生長的引力，加快隨機(jī)樹在非障礙區(qū)域的蔓延過程，減小尋路時間。在靠近障礙物時，獲得偏向隨機(jī)點生長的引力，避免隨機(jī)樹因遇到連續(xù)障礙物而停止擴(kuò)展。

具體生成過程：在生成隨機(jī)點qrand后，尋找到距該點最近的樹節(jié)點qnear，借用引力場思想對該節(jié)點施加2個力，分別為隨機(jī)點引力r(q)和目標(biāo)點引力g(q)，并利用權(quán)重距離系數(shù)，根據(jù)樹節(jié)點qnear位置實時分配目標(biāo)點引力大小和隨機(jī)點引力大小；在障礙物附近時，由隨機(jī)點引力為主要導(dǎo)向，反之，以目標(biāo)點為主要導(dǎo)向，引導(dǎo)樹節(jié)點向不同方向擴(kuò)展，而其實時的合力大小也作為新節(jié)點的生長步長生成新節(jié)點qnew；檢測新節(jié)點qnew樹節(jié)點qnear之間是否存在障礙物，若存在，廢除此次擴(kuò)展，若不存在，將其加入隨機(jī)樹中。新節(jié)點擴(kuò)展表達(dá)式為：

qnew=qnear+p*r(q)*nr(q)+

(1-p)*g(q)*ng(q)

(6)

(7)

(8)

(9)

式(6)—(9)中:p為權(quán)重距離系數(shù)；r(q)代表隨機(jī)點引力系數(shù)；g(q)代表為目標(biāo)點引力系數(shù)；nr(q)代表隨機(jī)點引力方向；ng(q)代表目標(biāo)點引力方向；dis(qrand,qnear)代表隨機(jī)點qrand與最近樹節(jié)點qnear的直線距離；dis(qgoal,qnear)代表目標(biāo)點qgoal與擴(kuò)展節(jié)點qnear的直線距離；dis代表障礙物影響最近距離；qnear_ob表示最近樹節(jié)點qnear與最近障礙物邊界點的距離。

新節(jié)點擴(kuò)展方向如圖6所示。

圖6 新節(jié)點擴(kuò)展方向示意圖

4.4 路徑平滑

RRT算法得到的可行路徑由樹節(jié)點組成，為一系列離散位置點，而并非一條平滑曲線。機(jī)器人按此路徑行駛時，突發(fā)轉(zhuǎn)折情況時有發(fā)生，并不符合機(jī)器人運(yùn)動學(xué)規(guī)律，減慢機(jī)器人行駛速度，因此在此基礎(chǔ)上，對路線進(jìn)行平緩處理，B樣條曲線能夠?qū)﹄x散路徑節(jié)點進(jìn)行平緩而不影響整體形態(tài)，被廣泛應(yīng)用于路徑運(yùn)動學(xué)處理[18]。考慮到該函數(shù)的這些特點，采用三次B樣條對改進(jìn)RRT生成的路徑節(jié)點進(jìn)行平緩處理。

三次B樣條曲線數(shù)學(xué)表達(dá)式為：

(10)

三次B樣條基函數(shù)表達(dá)式為：

(11)

5 仿真實驗

在 Matlab R2018a 實施，實驗環(huán)境配置：Window10，Intel(R) Core(TM) i7-7500U CPU @ 2.70GHz，RAM 8.00GB。

仿真地圖為800 mm×800 mm，節(jié)點擴(kuò)展步長為35 mm，目標(biāo)點閾值為20 mm，障礙物影響距離為12 mm。基站通信機(jī)器人起始點坐標(biāo)為(0,0)，基站站點坐標(biāo)為(750,750)。最終目標(biāo)是得到從基站通信機(jī)器人位置到基站站點的機(jī)器人布放路徑。圖7分別為3種算法的仿真示意圖和改進(jìn)算法路徑平滑示意圖。設(shè)置黑色方塊代表障礙物，藍(lán)線條為搜索形成的隨機(jī)樹，黑色加粗線條為規(guī)劃成功的路徑。由于算法采樣隨機(jī)進(jìn)行，每次產(chǎn)生的結(jié)果不同，故分別對RRT算法、文獻(xiàn)[11]改進(jìn)算法、本文改進(jìn)算法在通信基站布放俯視圖上各進(jìn)行500次仿真實驗，取各項結(jié)果的平均值。對不同算法的內(nèi)存占用情況、能量消耗情況和尋路時間代價進(jìn)行分析。

圖7 機(jī)器人布放路徑示意圖

圖7(a)為基于傳統(tǒng)RRT算法進(jìn)行機(jī)器人布放所得路徑，可以看出，路徑轉(zhuǎn)折點較多，冗余節(jié)點和冗余支路雜亂，進(jìn)行機(jī)器人布放效果不是很理想；圖7(b)為文獻(xiàn)[11]改進(jìn)算法進(jìn)行機(jī)器人布放所得路徑，相比于傳統(tǒng)RRT算法，其冗余路徑節(jié)點和冗余支路明顯減少，路徑質(zhì)量明顯提高；圖7(c)為本文改進(jìn)RRT算法進(jìn)行機(jī)器人布放所得路徑，改進(jìn)算法在路徑質(zhì)量、冗余節(jié)點、冗余支路上相比于前2種具有明顯改進(jìn)；圖7(d)為最終的平滑布放路徑圖，可以看出，紅色線條相比于黑色線條更加平緩，且路徑轉(zhuǎn)折點更少。

3種算法在500次路徑規(guī)劃情況下對基站通信機(jī)器人的能量消耗、內(nèi)存占用情況、尋路時間代價如表1所示。

表1 3種算法機(jī)器人代價情況

改進(jìn)RRT算法在進(jìn)行基站布放時能量消耗比傳統(tǒng)RRT算法減少了14.2%，相比于文獻(xiàn)[11]改進(jìn)算法減少了7.0%；在機(jī)器人內(nèi)存利用率上，改進(jìn)算法比基本RRT算法提高了72.2%，比文獻(xiàn)[11]改進(jìn)算法提高了47.4%；在尋路時間代價上，改進(jìn)算法相比基本RRT算法降低了77.6%，比文獻(xiàn)[11]改進(jìn)算法降低了31.5%。

6 結(jié)論

針對應(yīng)急場景的基站通信機(jī)器人布放路徑規(guī)劃問題，提出了一種改進(jìn) RRT的基站通信機(jī)器人布放路徑規(guī)劃算法。利用目標(biāo)偏置采樣、剪枝采樣和節(jié)點引力，提高基站通信機(jī)器人尋路效率，令機(jī)器人快速找到一條無碰撞的可行路徑進(jìn)行站點通信機(jī)器人布放，及時搭建應(yīng)急場景下的內(nèi)部局域網(wǎng)。改進(jìn)算法在內(nèi)存占用率、能量消耗和尋路代價方面均有明顯改進(jìn)，減少了機(jī)器人能耗；通過平滑后的路徑轉(zhuǎn)折點明顯減少，使機(jī)器人行駛更加平穩(wěn)。但本文中僅對單站點布放做了路徑規(guī)劃，而實際應(yīng)急場景區(qū)域范圍不定，站點布放問題不單為單通信機(jī)器人路徑規(guī)劃問題，因此后續(xù)研究將考慮多通信機(jī)器人布放的路徑規(guī)劃問題。