張海波，嚴(yán)小珊，畢齊林，唐惠玲

(1.廣州航海學(xué)院土木與工程管理學(xué)院，廣東廣州510725；2.廣東工業(yè)大學(xué)物理與光電工程學(xué)院，廣東廣州 510006)

0 前言

傳統(tǒng)的輪式、腿式、履帶式等移動(dòng)機(jī)器人難以在狹小空間移動(dòng)，全位置式移動(dòng)機(jī)器人(簡(jiǎn)稱(chēng)MW-WH-OIR)有助于改善這種情況，因此具有較廣泛的應(yīng)用前景及重要的研究意義。

在“十三五”、“工業(yè)4.0計(jì)劃”等政策的推動(dòng)下，我國(guó)巡檢業(yè)加快了向規(guī)?；?、自動(dòng)化、智能化方向轉(zhuǎn)型的步伐，工業(yè)移動(dòng)機(jī)器人也迎來(lái)了快速發(fā)展的戰(zhàn)略機(jī)遇期[1-2]。目前，國(guó)內(nèi)的巡檢業(yè)(如變電站巡檢，軌道交通巡檢，城市應(yīng)急安防巡檢業(yè))還存在大量人工巡檢、多固定點(diǎn)監(jiān)控的方法。此類(lèi)工作量大、枯燥，有時(shí)候還存在一定危險(xiǎn)，很多時(shí)候靠人眼并不能及時(shí)發(fā)現(xiàn)缺陷，效率低、周期長(zhǎng)，存在極大的盲區(qū)，易造成潛在的安全隱患。此外，多固定點(diǎn)監(jiān)視范圍有限、成本高、實(shí)用性差，不能滿(mǎn)足現(xiàn)代需求。

MW-WH-OIR能夠在任意方向移動(dòng)，靈活性好，搭載攝像頭便可以大大擴(kuò)展檢測(cè)范圍，可以廣泛應(yīng)用于智能巡檢領(lǐng)域。國(guó)內(nèi)智能巡檢市場(chǎng)對(duì)工業(yè)移動(dòng)機(jī)器人的需求正在不斷增加，然而在移動(dòng)機(jī)器人的相關(guān)技術(shù)研究中，導(dǎo)航技術(shù)是最核心的技術(shù)，路徑規(guī)劃則是導(dǎo)航技術(shù)的一項(xiàng)重要內(nèi)容。

工業(yè)移動(dòng)機(jī)器人成為當(dāng)代工業(yè)重要利器的同時(shí)，常常要考慮其運(yùn)動(dòng)規(guī)劃問(wèn)題，高效的路徑規(guī)劃有利于提高工作效率。如何在復(fù)雜的動(dòng)態(tài)障礙物環(huán)境下，規(guī)劃一條實(shí)時(shí)的從起始點(diǎn)到目標(biāo)點(diǎn)無(wú)碰撞的可行路徑是MW-WH-OIR技術(shù)研發(fā)的關(guān)鍵。

MW-WH-OIR的路徑規(guī)劃是指在具有障礙物的復(fù)雜環(huán)境下，按照規(guī)劃路徑的某個(gè)測(cè)度(如時(shí)間最短、路徑最優(yōu)、能量消耗最低等)，在任務(wù)區(qū)域內(nèi)尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)與障礙物無(wú)碰撞的可行路徑[3]。20世紀(jì)70年代至今，國(guó)內(nèi)外學(xué)者對(duì)此進(jìn)行了大量的研究，同時(shí)還提出了許多適用于不同應(yīng)用場(chǎng)景下的路徑規(guī)劃算法并不斷進(jìn)行改進(jìn)、優(yōu)化。2007年趙曉等人[4]利用A*算法實(shí)現(xiàn)了移動(dòng)機(jī)器人的無(wú)碰撞路徑規(guī)劃。王淼弛[5]針對(duì)A*算法存在的次優(yōu)路徑問(wèn)題，改進(jìn)了A*算法的啟發(fā)搜索策略，優(yōu)化了路徑長(zhǎng)度，同時(shí)提高了路徑平滑度；王奇志[6]針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法出現(xiàn)的零勢(shì)能域現(xiàn)象，采用限定范圍的方法改進(jìn)了人工勢(shì)場(chǎng)法，實(shí)現(xiàn)了多障礙物情況下機(jī)器人快速、實(shí)時(shí)、避障的路徑規(guī)劃。2010年SHI等[7]為解決傳統(tǒng)人工勢(shì)場(chǎng)法用于移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃時(shí)，出現(xiàn)目標(biāo)點(diǎn)距離障礙物太近而無(wú)法到達(dá)目標(biāo)點(diǎn)以及存在局部極小點(diǎn)的陷阱問(wèn)題，提出了縮小障礙物影響范圍、障礙物影響范圍分層的改進(jìn)方法，使得算法適用于移動(dòng)機(jī)器人在各種復(fù)雜環(huán)境下的路徑規(guī)劃。李寶霞[8]針對(duì)模糊系統(tǒng)的隸屬度函數(shù)確定過(guò)程繁瑣問(wèn)題，將遺傳算法結(jié)合到模糊系統(tǒng)中，對(duì)模糊算法進(jìn)行改進(jìn)，生成了模糊遺傳算法。

以上常用的路徑規(guī)劃算法在簡(jiǎn)單的環(huán)境下能快速地找到最優(yōu)路徑，但其沒(méi)有考慮非完整性約束問(wèn)題，也不適合解決高維空間多自由度機(jī)器人復(fù)雜約束下的運(yùn)動(dòng)規(guī)劃問(wèn)題，可能導(dǎo)致實(shí)際路徑不可行，不能滿(mǎn)足移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃的實(shí)時(shí)性。

為此，基于隨機(jī)采樣的運(yùn)動(dòng)規(guī)劃算法，YANG等提出了一種基于采樣的快速隨機(jī)擴(kuò)展樹(shù)方法(Rapidly-exploring Random Trees，RRT)算法[9]。由于RRT算法的快速搜索隨機(jī)性，增量式搜索方式能夠把非完整性約束或非完整性動(dòng)力學(xué)約束考慮到運(yùn)動(dòng)規(guī)劃中，因此非常適合解決高維空間多自由度機(jī)器人在復(fù)雜環(huán)境和變化場(chǎng)景中的運(yùn)動(dòng)規(guī)劃問(wèn)題。樊曉平和李雙艷[10]針對(duì)復(fù)雜的動(dòng)態(tài)環(huán)境，將RRT算法與滾動(dòng)規(guī)劃結(jié)合，同時(shí)在RRT算法中引入選擇性參數(shù)BIAS，實(shí)現(xiàn)了向目標(biāo)點(diǎn)快速收斂的實(shí)時(shí)在線(xiàn)路徑規(guī)劃。喬慧芬等[11]為實(shí)現(xiàn)動(dòng)態(tài)不確定環(huán)境下實(shí)時(shí)路徑規(guī)劃，結(jié)合人工勢(shì)場(chǎng)法下規(guī)劃的初始路徑和RRT算法指導(dǎo)下的局部路徑，有效實(shí)現(xiàn)了移動(dòng)機(jī)器人的無(wú)碰撞實(shí)時(shí)路徑規(guī)劃。

本文作者基于MW-WH-OIR的工作環(huán)境，提出一種改進(jìn)的快速擴(kuò)展隨機(jī)樹(shù)算法，使MW-WH-OIR能夠在一個(gè)復(fù)雜的動(dòng)態(tài)環(huán)境下進(jìn)行在線(xiàn)實(shí)時(shí)路徑規(guī)劃。

實(shí)際情況下，MW-WH-OIR更多時(shí)候需要對(duì)動(dòng)態(tài)環(huán)境變化實(shí)時(shí)做出動(dòng)作，并不斷尋找最優(yōu)可行路徑。ZHUANG等[12]基于極坐標(biāo)空間，實(shí)現(xiàn)了以機(jī)器人期望運(yùn)動(dòng)方向角為指標(biāo)的動(dòng)態(tài)不確定環(huán)境下移動(dòng)機(jī)器人的在線(xiàn)實(shí)時(shí)路徑規(guī)劃。曾碧和楊宜民[13]利用模糊描述對(duì)環(huán)境進(jìn)行建模,將改進(jìn)的蟻群算法與機(jī)器人滾動(dòng)在線(xiàn)路徑規(guī)化方法相結(jié)合，實(shí)現(xiàn)了動(dòng)態(tài)環(huán)境下在線(xiàn)實(shí)時(shí)規(guī)劃問(wèn)題。以上算法都未考慮移動(dòng)機(jī)構(gòu)的非完整約束條件?？焖偎阉麟S機(jī)樹(shù)可在環(huán)境中的從起始點(diǎn)開(kāi)始快速隨機(jī)擴(kuò)展搜索樹(shù)，直到尋找到目標(biāo)點(diǎn)，其結(jié)構(gòu)簡(jiǎn)單、搜索能力強(qiáng)且考慮了執(zhí)行機(jī)構(gòu)的非完整約束條件，還易于與其他算法結(jié)合生成一個(gè)新的可行算法。在國(guó)內(nèi)，RRT算法應(yīng)用于已知的環(huán)境下的研究已經(jīng)很成熟，但應(yīng)用于動(dòng)態(tài)環(huán)境下的實(shí)時(shí)路徑規(guī)劃的研究還較少。

在實(shí)際工作中，MW-WH-OIR多處于一個(gè)復(fù)雜的動(dòng)態(tài)環(huán)境，傳統(tǒng)的RRT算法及大多數(shù)改進(jìn)后的RRT算法生成的路徑往往不能被采用。因此，如何使MW-WH-OIR在一個(gè)復(fù)雜的動(dòng)態(tài)環(huán)境下進(jìn)行高效的在線(xiàn)實(shí)時(shí)避障及路徑規(guī)劃是文中需要解決的問(wèn)題。

1 傳統(tǒng)RRT算法

1.1 傳統(tǒng)RRT算法基本流程

傳統(tǒng)RRT算法相較其他路徑規(guī)劃算法更加具備快速搜索的能力，能夠很好地適應(yīng)動(dòng)態(tài)變化的環(huán)境，也因其快速擴(kuò)展的隨機(jī)性，使得該算法具有很強(qiáng)的避障功能。圖1所示為傳統(tǒng)RRT算法的流程。

圖1 傳統(tǒng)RRT算法基本流程

1.2 傳統(tǒng)RRT算法的基本原理

傳統(tǒng)RRT算法不斷地將樹(shù)枝從起始節(jié)點(diǎn)Xinit(根節(jié)點(diǎn))隨機(jī)擴(kuò)展到目標(biāo)節(jié)點(diǎn)Xgoal，其過(guò)程中合理避開(kāi)障礙物，遍歷生成隨機(jī)樹(shù)T，最后選取一條最優(yōu)的可行路徑[16]。傳統(tǒng)的RRT算法：

Algorithm1：傳統(tǒng)RRT

1：TU←Xinit；

2：while(n←1 to Nmax)do

3：Xrand← Sample_node(Xfree)；

4：Xnear←Nearclose(Xrand，TU)；

6：Xnew←Steer(Xnear，Xrand)；

7：if Check_path(Xnearest，Xnew)= = true then；

8：Add_node(Xnew)；

9：end if

10：end while

11：return TU；

傳統(tǒng)RRT算法的基本步驟如下：

步驟1：初始化隨機(jī)樹(shù)T后將給定起始點(diǎn)Xinit添加到隨機(jī)樹(shù)T作為根節(jié)點(diǎn)；

步驟2：在Wfree空間中隨機(jī)獲取一個(gè)生長(zhǎng)樹(shù)節(jié)點(diǎn)Xrand；

步驟3：用Nearclose()函數(shù)計(jì)算樹(shù)T中與Xrand距離最近的擴(kuò)展節(jié)點(diǎn)Xnear；

步驟4：用Steer()函數(shù)在Xrand與Xnear方向上生長(zhǎng)新節(jié)點(diǎn)Xnew；

步驟5：判斷Xnew生長(zhǎng)區(qū)域是否存在障礙物，若是，則跳轉(zhuǎn)進(jìn)行步驟3；若否，則在隨機(jī)樹(shù)T上添加一個(gè)新節(jié)點(diǎn)Xnewi(i∈N，N為自然數(shù))

步驟6：判斷是否滿(mǎn)足Xrandi=Xgoal或者Xnewi∈Xgoal的條件，若不滿(mǎn)足條件，則跳轉(zhuǎn)進(jìn)行步驟4；若滿(mǎn)足條件，則從目標(biāo)節(jié)點(diǎn)Xgoal開(kāi)始依次找到父親節(jié)點(diǎn)直到起始點(diǎn)Xinit為止，之后返回一條可行的規(guī)劃路徑，結(jié)束程序。

2 基于改進(jìn)RRT算法的MW-WH-OIR實(shí)時(shí)路徑規(guī)劃方法理論分析

2.1 定義滾動(dòng)窗口

MW-WH-OIR在進(jìn)行管道、軌道、安防巡檢作業(yè)時(shí)，常處于一個(gè)復(fù)雜動(dòng)態(tài)障礙物環(huán)境下，為此首要考慮路徑規(guī)劃的實(shí)時(shí)性。全局路徑規(guī)劃方法運(yùn)算時(shí)間太長(zhǎng)，不適合動(dòng)態(tài)復(fù)雜環(huán)境下的路徑規(guī)劃，為此文中采用局部路徑規(guī)劃方法。為滿(mǎn)足路徑規(guī)劃的實(shí)時(shí)性，將傳感器檢測(cè)范圍視為一個(gè)可進(jìn)行自適應(yīng)調(diào)節(jié)大小的窗口(文中視檢測(cè)范圍是一個(gè)半圓)，如圖2所示，當(dāng)H2大于檢測(cè)范圍時(shí)，對(duì)整個(gè)路徑進(jìn)行多階段的規(guī)劃。

圖2 滾動(dòng)窗口示意

2.2 無(wú)障礙物時(shí)的MW-WH-OIR實(shí)時(shí)路徑規(guī)劃

通過(guò)GPS導(dǎo)航系統(tǒng)確定起始點(diǎn)Xinit與目標(biāo)點(diǎn)Xgoal位置，若起始節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間，不存在障礙物時(shí)，起始節(jié)點(diǎn)Xinit與目標(biāo)節(jié)點(diǎn)Xgoal之間的連線(xiàn)距離就是MW-WH-OIR所要行走的最短可行路徑，如圖3所示。

圖3 無(wú)障礙物處理模型

2.3 有障礙物時(shí)的MW-WH-OIR實(shí)時(shí)路徑規(guī)劃

動(dòng)態(tài)障礙物的位置會(huì)隨著時(shí)間發(fā)生變化，故建模是以時(shí)間為軸建立方程。障礙物任意時(shí)刻的位置狀態(tài)用(x，y，t)表示。

2.3.1 面向MW-WH-OIR的勻速障礙物預(yù)測(cè)模型

如圖4所示，假設(shè)障礙物以速度v(t)=b勻速移動(dòng)，在t時(shí)刻于點(diǎn)A位置處(xi，yi，t)被滑動(dòng)窗口檢測(cè)到，而下一時(shí)刻t+Δt時(shí)，處于點(diǎn)B(Δt取值極小)，當(dāng)Δt取極小值時(shí)，AB之間的連線(xiàn)可視為一條直線(xiàn)，直線(xiàn)AB與X軸之間的夾角為θ，則對(duì)于障礙物從t→t+Δt過(guò)程中，它的位置坐標(biāo)變化情況如下：

圖4 勻速障礙物預(yù)測(cè)模型

(1)

通過(guò)公式(1)可以快速計(jì)算出任意時(shí)刻障礙物的移動(dòng)位置。

2.3.2 面向MW-WH-OIR的變速障礙物預(yù)測(cè)模型

MW-WH-OIR在二維平面工作，由于傳感器可以實(shí)時(shí)獲取局部動(dòng)態(tài)障礙物的位置信息，文中通過(guò)傳感器在前期獲取的動(dòng)態(tài)障礙物位置信息，結(jié)合自回歸模型預(yù)測(cè)下一時(shí)刻動(dòng)態(tài)障礙物的運(yùn)動(dòng)位置，進(jìn)行局部規(guī)劃，避開(kāi)障礙物。

如圖5所示，假設(shè)傳感器的滑動(dòng)窗口檢測(cè)到的動(dòng)態(tài)障礙物為obsq(q∈N)，并記錄了該障礙物上一時(shí)刻和當(dāng)前時(shí)刻的位置信息，記為sq(t-1)、sq(t)(t∈N)，由此建立預(yù)測(cè)下一時(shí)刻位置sq(t+1)的自回歸模型：

圖5 存在變速的障礙物預(yù)測(cè)模型

sq(t)=λ1sq(t-1)+e(t)

(2)

相應(yīng)的推導(dǎo)n階自回歸模型：

(3)

其中：e(t)為位置預(yù)測(cè)誤差；λ為位置的回歸系數(shù)。

同理對(duì)動(dòng)態(tài)障礙物的加速度aq進(jìn)行n階自回歸建模：

(4)

其中：p(t)為加速度預(yù)測(cè)誤差；β為加速度的回歸系數(shù)。

速度與位置的關(guān)系式：

vq(t)=sq(t)-sq(t-1)

(5)

加速度與速度關(guān)系式：

aq(t)=vq(t)-vq(t-1)

(6)

結(jié)合上式，易得加速度與位置的關(guān)系式：

aq(t)=sq(t)-2sq(t-1)+sq(t-2)

(7)

加速度回歸系數(shù)β通過(guò)最小二乘法擬合，由公式(4)可引入加速度的殘差平方和函數(shù)：

(8)

(9)

通過(guò)對(duì)E(β)進(jìn)行微分求最值，可以得到：

(10)

當(dāng)加速度的回歸系數(shù)βq與時(shí)間相關(guān)時(shí)，在殘差平方和函數(shù)取值最小的函數(shù)中引入一個(gè)指數(shù)權(quán)重因子μ(0<μ<1),μ越趨近于1，表明加速度變化得越快。

(11)

求上式的解：

(12)

當(dāng)能擬合估算回歸系數(shù)βq，便可以計(jì)算下一時(shí)刻動(dòng)態(tài)障礙物的預(yù)測(cè)位置，即：

(13)

3 面向MW-WH-OIR的改進(jìn)RRT算法

3.1 MW-WH-OIR運(yùn)動(dòng)學(xué)模型

Mecanum Wheel能實(shí)現(xiàn)全方位移動(dòng)，1973年由瑞士人LION發(fā)明[14]。它有3個(gè)自由度，即繞輥?zhàn)虞S轉(zhuǎn)動(dòng)、繞輪子軸轉(zhuǎn)動(dòng)、繞輥?zhàn)优c地面的接觸點(diǎn)轉(zhuǎn)動(dòng)。MW-WH-OIR的移動(dòng)機(jī)構(gòu)主要采用4個(gè)Mecanum Wheel，可以實(shí)現(xiàn)任意方向的自由移動(dòng)，其轉(zhuǎn)彎半徑近似為零，非常適合代替作業(yè)人員在工作空間狹小或?qū)C(jī)器人的機(jī)動(dòng)性要求較高的場(chǎng)合，如大型管道內(nèi)側(cè)巡檢、變電站巡檢、城市應(yīng)急安防巡檢等，因而在工業(yè)上應(yīng)用越來(lái)越廣泛[15]。本文作者針對(duì)MW-WH-OIR的路徑規(guī)劃分析時(shí)，可以不考慮其轉(zhuǎn)彎半徑限制的問(wèn)題。以下對(duì)MW-WH-OIR運(yùn)動(dòng)學(xué)模型進(jìn)行分析。

如圖6所示，以O(shè)為原點(diǎn)建立坐標(biāo)系，Y軸為MW-WH-OIR的正前方，X軸為MW-WH-OIR正右方。vn(n=1，2，3，4)為4個(gè)Mecanum Wheel上對(duì)應(yīng)與地面接觸的轉(zhuǎn)動(dòng)輥?zhàn)永@軸的自身線(xiàn)速度；r為Mecanum Wheel的半徑，則：vn=r×ωn(n=1，2，3，4)；ωn(n=1，2，3，4)為對(duì)應(yīng)4個(gè)Mecanum Wheel的自身角速度；vx、vy分別為MW-WH-OIR在X、Y方向的移動(dòng)速度；ω為繞中心點(diǎn)O垂直軸轉(zhuǎn)動(dòng)的角速度；α為Mecanum Wheel輪轂軸線(xiàn)和輥?zhàn)虞S線(xiàn)的夾角；2L2、2L1為機(jī)器人車(chē)身的長(zhǎng)和寬。

圖6 MW-WH-OIR運(yùn)動(dòng)學(xué)分析示意

若vx、vy已知，求4個(gè)輪子的線(xiàn)速度vn，則根據(jù)MW-WH-OIR運(yùn)動(dòng)學(xué)分析得計(jì)算公式如下：

v1=vy-vxtanα-(L2tanα+L1)ω

(14)

v2=vy+vxtanα+(L2tanα+L1)ω

(15)

v3=vy+vxtanα-(L2tanα+L1)ω

(16)

v4=vy-vxtanα+(L2tanα+L1)ω

(17)

反之，若4個(gè)輪子的線(xiàn)速度vn已知，即可求解MW-WH-OIR在X、Y方向上的移動(dòng)速度(vx，vy)及相應(yīng)的角速度ω，公式如下：

(18)

vy=0.25(v1+v2+v3+v4)

(19)

(20)

結(jié)合公式(14)—(17)，可分析MW-WH-OIR在各個(gè)方向上的移動(dòng)情況，4種典型MW-WH-OIR的移動(dòng)情況和4個(gè)Mecanum Wheel轉(zhuǎn)向關(guān)系如圖7所示。

圖7 4種典型MW-WH-OIR移動(dòng)情況

3.2 改進(jìn)RRT算法

改進(jìn)的RRT算法針對(duì)上述討論的傳統(tǒng)RRT算法存在的不足之處進(jìn)行改進(jìn)和創(chuàng)新，同時(shí)將其應(yīng)用于更加復(fù)雜的動(dòng)態(tài)環(huán)境下，擴(kuò)展了算法的應(yīng)用范圍，其中考慮了MW-WH-OIR車(chē)身約束，從而更加符合MW-WH-OIR的實(shí)際工作情況。以下為改進(jìn)RRT算法的偽代碼：

Algorithm2：改進(jìn)RRT

1：構(gòu)建：仿真環(huán)境地圖；

2：定義環(huán)境信息：機(jī)器人運(yùn)動(dòng)狀態(tài)點(diǎn)Xinit、目標(biāo)點(diǎn)Xgoal、子目標(biāo)點(diǎn)Zgoal、步長(zhǎng)Stepsize、安全閾值范圍Q；

3：檢測(cè)：判斷起始點(diǎn)、目標(biāo)點(diǎn)是否在仿真地圖的障礙物上，若是，則退出，否則繼續(xù)；

4：TU←Xinit；

5：while N>Nmaxdo；

6：Update(Zgoal)

7：obsn←Sample_obs;(n=1,2)；

8： Filter(Xnearest，Xnew)

9：Index_node = Index_tree(Xrand)；

10：while Index_node> 1 do

11：Update_node(Xrand)；

12：Select(Xnew，Xnear)

13：end while；

14：end while；

15：return T；

Function：Update(Zgoal，Xgoal)

1：while Xgoal==Zgoaldo

2：if Checkpath1(Zgoal)

3：Zgoal← Get(Zgoal)；

4：else if (a1Q))

5：Zgoal← Get(A1)；

6：else

7：Zgoal←Get(A2)

8：end if

9：if Xgoal?S

10：Xgoal=Zgoal

11：end if

12：end while

Function：Filter(Xnearest，Xnew)

1：Xrand← Sample_node；

2：Xnearest← Nearclose(Xrand，TU)；

3：Index_node = Index_tree(Xrand)；

4：for Xnearest∈Xneardo

5：if Xnear≠ ?；

6： Total C = Cost(Xnearest)+Cost(Xnew，)；

7：end if；

8：if Total C1==min(total C)；

9：Xrand←Xnearest；

10：end if；

11：end for；

Function：Select(Xnew，Xnear)

1：if Checkpath2(Xnear，Xnew)&distance(Xnew>Q)；

2：Xnew← New_node(Xnewest，Xrand)；

3：else;

4：delete(Xnew)；

5：end if;

為彌補(bǔ)傳統(tǒng)RRT算法的不足，本文作者提出的Algorithm2在Algorithm1的基礎(chǔ)上添加了3個(gè)函數(shù)，下面對(duì)Algorithm2中的3個(gè)重要函數(shù)進(jìn)行簡(jiǎn)單的介紹。

3.2.1 建立最小安全距離Q模型

由于傳統(tǒng)RRT算法中并沒(méi)有考慮移動(dòng)機(jī)器人車(chē)身尺寸D的約束，故在規(guī)劃路徑過(guò)程中存在生長(zhǎng)節(jié)點(diǎn)靠近障礙物的現(xiàn)象。當(dāng)生長(zhǎng)節(jié)點(diǎn)已經(jīng)與障礙物粘在一起，MW-WH-OIR沿著該路徑行走時(shí)必定會(huì)跟障礙物發(fā)生碰撞，損壞車(chē)身。為避免MW-WH-OIR與障礙物發(fā)生碰撞，設(shè)置最小安全距離Q(Algorithm2：第22～26行，其中Q>D)，如圖8所示，障礙物邊緣擴(kuò)展的黃色區(qū)域即是安全閾值范圍Q，令生長(zhǎng)節(jié)點(diǎn)禁止擴(kuò)展到安全閾值范圍內(nèi)，這樣便使得MW-WH-OIR在移動(dòng)過(guò)程中與障礙物始終保持一定的安全距離。以下為安全閾值Q算法：

圖8 設(shè)置安全閾值

Algorithm3：Select(Xnew，Xnear)

1：if Checkpath(Xnear，Xnew)&distance(Xnew>Q)；

2：Xnew← New_node(Xnewest，Xrand)；

3：else;

4：delete(Xnew)；

5：end if;

在檢測(cè)生長(zhǎng)節(jié)點(diǎn)Xnew是否與障礙物碰撞的同時(shí)判斷與障礙物的距離是否大于Q值，若是，則添加新的生長(zhǎng)節(jié)點(diǎn)，否則刪除Xnew，重新尋找符合條件的Xnew。

3.2.2 子目標(biāo)點(diǎn)的選取及更新算法

圖9 子目標(biāo)節(jié)點(diǎn)的選取及更新

Algorithm4：Update(Zgoal，Xgoal)

1：while Xgoal==Zgoaldo

2：if Checkpath1(Zgoal)

3：Zgoal← Get(Zgoal)；

4：elseif (a1Q))

5：Zgoal← Get(A1)；

6：else

7：Zgoal←Get(A2)

8：end if

9：if Xgoal?S

10：Xgoal=Zgoal

11：end if

11：end while

3.2.3 過(guò)濾無(wú)用節(jié)點(diǎn)

在基本RRT樹(shù)枝生長(zhǎng)過(guò)程中，會(huì)生成許多不必要的樹(shù)枝，需要大量的迭代次數(shù)才能找到目標(biāo)點(diǎn)，而且生成的路徑曲折冗長(zhǎng)、不平滑，耗費(fèi)大量的搜索時(shí)間，搜索效率極低[17]。

為解決以上問(wèn)題，本文作者在傳統(tǒng)RRT算法的基礎(chǔ)上添加了一個(gè)過(guò)濾不必要節(jié)點(diǎn)的過(guò)程(Algorithm5：第5～9行)，僅保留一個(gè)最優(yōu)生長(zhǎng)結(jié)點(diǎn)，如圖10所示。品紅色的生長(zhǎng)節(jié)點(diǎn)明顯偏離Zgoal，不是最優(yōu)生長(zhǎng)節(jié)點(diǎn)故將它過(guò)濾。這樣就降低了迭代次數(shù)，大大提高搜索效率，減少冗余節(jié)點(diǎn)，路徑的平滑度也有所提升。

圖10 過(guò)濾無(wú)用節(jié)點(diǎn)

Algorithm5：Filter(Xnearest，Xnew)：

1：Xrand← Sample_node；

2：Xnearest← Nearclose(Xrand，TU)；

3：Index_node = Index_tree(Xrand)；

4：for Xnearest∈Xneardo

5：if Xnear≠ ?；

6： Total C = Cost(Xnearest)+Cost(Xnew，)；

7：end if；

8：if Total C1==min(total C)；

9：Xrand←Xnearest；

10：end if；

11：end for；

當(dāng)Xnear不為空集時(shí)，尋找Xnear集合中與Xnew形成最小長(zhǎng)度代價(jià)的Xnearest，最后賦值給Xrand進(jìn)行路徑規(guī)劃。

4 仿真實(shí)驗(yàn)

通過(guò)建立上述模型和RRT實(shí)時(shí)路徑規(guī)劃算法，在MATLAB2018a下進(jìn)行了仿真實(shí)驗(yàn)。實(shí)驗(yàn)用計(jì)算機(jī)配置如下：處理器為Intel-Core-i5-7200，Xeon(R)，主頻為3.10 GHz，內(nèi)存為16 GB，操作系統(tǒng)為64位Windows10。實(shí)驗(yàn)設(shè)置地圖大小為500×500(無(wú)量綱)，如圖11所示。此次仿真實(shí)驗(yàn)中設(shè)置傳統(tǒng)RRT算法在靜態(tài)障礙物環(huán)境下的生長(zhǎng)步長(zhǎng)為25，隨機(jī)生長(zhǎng)節(jié)點(diǎn)概率閾值為20；起始點(diǎn)Xinit=(5,499)，子目標(biāo)點(diǎn)Zgoal=(150,300)，目標(biāo)點(diǎn)Xgoal=(499,5)。

圖11 傳統(tǒng)RRT算法路徑規(guī)劃仿真實(shí)驗(yàn)

利用改進(jìn)的RRT算法將MW-WH-OIR的工作環(huán)境建模定位于500×500的地圖區(qū)域，如圖12中左上角為機(jī)器人第一次局部檢測(cè)范圍內(nèi)路徑規(guī)劃的全過(guò)程，圓形的障礙物為可視范圍內(nèi)的隨機(jī)動(dòng)態(tài)障礙物，其余為靜態(tài)障礙物，其中動(dòng)態(tài)障礙物的起始點(diǎn)是任意設(shè)定的。

圖12 基于改進(jìn)RRT算法的實(shí)時(shí)路徑規(guī)劃仿真實(shí)驗(yàn)

4.1 傳統(tǒng)RRT算法MW-WH-OIR路徑規(guī)劃仿真

傳統(tǒng)RRT算法在500×500的復(fù)雜環(huán)境下(環(huán)境中的障礙物都為靜態(tài))的規(guī)劃路徑如圖11所示，節(jié)點(diǎn)表示MW-WH-OIR經(jīng)過(guò)的路徑。由圖11可以明顯看出：隨機(jī)樹(shù)的生長(zhǎng)區(qū)域幾乎覆蓋了整個(gè)工作環(huán)境，整個(gè)過(guò)程需要經(jīng)過(guò)大量的迭代才能找到目標(biāo)點(diǎn)Xgoal，極大地降低了搜索效率。圖11(d)中的最終規(guī)劃路徑存在大量的冗余點(diǎn)，路徑較為曲折，甚至在一些空曠區(qū)域內(nèi)也存在許多彎曲的路徑，偏離最優(yōu)解。此外，還存在生長(zhǎng)節(jié)點(diǎn)貼近障礙物的現(xiàn)象，如圖11(d)紅色標(biāo)注)，在實(shí)際駕駛過(guò)程很容易與障礙物發(fā)生碰撞。

4.2 改進(jìn)RRT算法MW-WH-OIR實(shí)時(shí)路徑規(guī)劃仿真

對(duì)比圖11、圖12的仿真結(jié)果可知:相較于傳統(tǒng)RRT算法，改進(jìn)的RRT算法規(guī)劃路徑并沒(méi)有出現(xiàn)大量的冗余點(diǎn)、曲折點(diǎn)，樹(shù)枝生長(zhǎng)過(guò)程中能夠順利地避開(kāi)障礙物，始終與障礙物保持一定的安全距離，最終朝向子目標(biāo)點(diǎn)生長(zhǎng)。此外，由圖12可知，本文作者提出的算法解決了路徑曲折、樹(shù)枝生長(zhǎng)無(wú)方向性、搜索效率低的問(wèn)題，進(jìn)而大幅度提高了規(guī)劃路徑平滑度，同時(shí)避免了MW-WH-OIR與障礙物發(fā)生碰撞的現(xiàn)象，滿(mǎn)足移動(dòng)機(jī)器人的實(shí)際移動(dòng)需求。

4.3 實(shí)驗(yàn)結(jié)果對(duì)比分析

為了更好地驗(yàn)證算法性能，分別對(duì)3種算法在文中設(shè)定的障礙物環(huán)境下進(jìn)行500次仿真實(shí)驗(yàn)，得到每種算法規(guī)劃所需的最小、最大以及平均時(shí)間的實(shí)驗(yàn)數(shù)據(jù)，如表1所示。

表1 復(fù)雜障礙物環(huán)境下算法路徑規(guī)劃性能數(shù)據(jù)

從表1可看出：不管是在簡(jiǎn)單靜態(tài)障礙物威脅還是復(fù)雜動(dòng)態(tài)障礙物威脅環(huán)境下，文中算法的規(guī)劃時(shí)間均是最少的，與傳統(tǒng)RRT算法相比較，它將搜索效率提高了43%。

5 結(jié)論

針對(duì)傳統(tǒng)RRT算法應(yīng)用于MW-WH-OIR生成的路徑存在路徑曲折、搜索效率低、樹(shù)枝生長(zhǎng)缺乏目標(biāo)引導(dǎo)性甚至路徑不可行的問(wèn)題，首先，添加了刪除無(wú)用節(jié)點(diǎn)策略，減少冗余點(diǎn)，提高探索效率；其次，提出了子目標(biāo)點(diǎn)的選取原則，在能夠保證順利避障的前提下，使得生長(zhǎng)樹(shù)優(yōu)先朝向目標(biāo)點(diǎn)生長(zhǎng)；最后，設(shè)置合適的安全閾值范圍，使移動(dòng)機(jī)器人與障礙物能夠保持一定的安全距離。最終提高了樹(shù)枝生長(zhǎng)效率，生成的路徑較為平滑，能夠更好地應(yīng)用于MW-WH-OIR的實(shí)際工作中。