楊 超， 袁曉宇

(1.中國(guó)電子科技集團(tuán)公司 第十研究所，成都 610207；2.浙江科技學(xué)院，杭州 310023)

路徑規(guī)劃算法的研究是無人車導(dǎo)航系統(tǒng)中的關(guān)鍵技術(shù)，但應(yīng)用在地下狹窄空間較多的環(huán)境中仍是困難的.根據(jù)普遍情況下的導(dǎo)航任務(wù)需求，研究者通?？紤]算法中的性能指標(biāo).近幾年，國(guó)內(nèi)外學(xué)者已經(jīng)提出許多運(yùn)動(dòng)規(guī)劃算法，例如人工勢(shì)場(chǎng)法[1]、遺傳算法[2-3]、蟻群算法[4-5]、RRT算法及其變體[6-8].文獻(xiàn)[9]中針對(duì)傳統(tǒng)RRT算法隨機(jī)性強(qiáng)、生長(zhǎng)步長(zhǎng)固定等問題，結(jié)合了引力場(chǎng)和動(dòng)態(tài)步長(zhǎng)兩種策略，收斂速度和時(shí)間.文獻(xiàn)[10]提出RRT森林算法解決了搜索區(qū)域受限、耗時(shí)較長(zhǎng)等問題.文獻(xiàn)[11]通過對(duì)狀態(tài)空間隨機(jī)采樣，檢測(cè)碰撞點(diǎn)，解決了三維狀態(tài)的路徑規(guī)劃問題.對(duì)于連接策略問題上，Lin等[12]提出了通過簡(jiǎn)化節(jié)點(diǎn)連接策略來生成滿足操作環(huán)境約束的可行路徑.在隨機(jī)性高，收斂速度慢，飛行軌跡彎曲的問題上，F(xiàn)an等人[13]引入了ACO(蟻群優(yōu)化)使規(guī)劃路徑漸近最優(yōu).RRT*算法作為RRT算法的一種改進(jìn)型，在不斷的研究與拓展中Qureshi A H等對(duì)此類算法做出總結(jié)：只要時(shí)間足夠，RRT*算法會(huì)收斂到最優(yōu)解，但效率大大減少[14].由Karaman等提出的Informed RRT*算法是RRT*的改進(jìn)，旨在縮短搜索時(shí)間[15].雖然針對(duì)傳統(tǒng)路徑規(guī)劃問題進(jìn)行了諸多研究，但仍存在很難通過狹窄通道或入口的問題.這是必須穿越低采樣區(qū)域完成的任務(wù)，這就提高了無人車導(dǎo)航系統(tǒng)對(duì)路徑規(guī)劃的要求.

文中提出了一種針對(duì)狹窄空間探索的改進(jìn)IA-RRT算法，該算法可以產(chǎn)生更有效的導(dǎo)航路徑.它引入了一種距離優(yōu)先的啟發(fā)式采樣策略，通過距離計(jì)算，優(yōu)點(diǎn)選點(diǎn)等任務(wù)進(jìn)行階段規(guī)劃，以及采用了一種適應(yīng)環(huán)境的步長(zhǎng)選擇策略，通過環(huán)境中障礙物的占比實(shí)現(xiàn)合理分配更適合環(huán)境的步長(zhǎng).

1 改進(jìn)RRT算法

1.1 改進(jìn)思想

RRT算法是一種基于隨機(jī)采樣和迭代的路徑規(guī)劃算法.它通過逐步生長(zhǎng)一棵樹，從初始點(diǎn)開始，不斷隨機(jī)采樣并連接新采樣點(diǎn)，構(gòu)建出一個(gè)樹狀結(jié)構(gòu)，其中包含了可行路徑.在構(gòu)建樹的過程中，通過判斷采樣點(diǎn)是否接近目標(biāo)點(diǎn)來逐步擴(kuò)展樹的結(jié)構(gòu)，直至找到一條有效的路徑，如圖1所示.但由于從節(jié)點(diǎn)向外搜索的方式具有一定的隨機(jī)性和多樣性，所以每次生成的路徑都是不同的，這會(huì)導(dǎo)致規(guī)劃所需的時(shí)間增加，降低工作效率.

圖1 RRT采樣示意圖

RRT及其變體已經(jīng)被廣泛地應(yīng)用于解決許多領(lǐng)域的路徑規(guī)劃問題，都具有類似樹的結(jié)構(gòu)，并且在尋路中逐步增加這些樹的節(jié)點(diǎn).其采樣方式是空間內(nèi)隨機(jī)，環(huán)境中有大量狹窄通道時(shí)，采樣點(diǎn)落在寬開放區(qū)域的可能性大大高于狹窄通道，可能會(huì)變得低效，在很多情況下，樹的擴(kuò)展會(huì)更快填充整個(gè)空間，使每次路徑規(guī)劃完成時(shí)間很長(zhǎng).一種改進(jìn)的IA-RRT算法被提出，該方法限制采樣區(qū)域和方向，在生成圓上采樣，如圖2所示.

圖2 改進(jìn)算法采樣示意圖

采樣計(jì)算方式如下

(1)

式中：Xnew為每次尋找到的可以加入樹中的節(jié)點(diǎn)；σ為以Xnew為圓心建立坐標(biāo)系的x軸的正方向；τ為負(fù)方向；L為步長(zhǎng)；i為圓上正方向或負(fù)方向生成節(jié)點(diǎn)的個(gè)數(shù).

(2)

式中：Δx1和Δx2分別為正方向和負(fù)方向上的距離.

選擇節(jié)點(diǎn)的縱坐標(biāo)可以表示為

(3)

由(1)、(2)、(3)式可以得到圓上的全部節(jié)點(diǎn)集合.

Xall={(xσ，y1)，(xτ，y2)}.

(4)

把Xall保存到列表A中，每次采樣的第一個(gè)節(jié)點(diǎn)Xall1，是由啟發(fā)函數(shù)計(jì)算列表A中所有節(jié)點(diǎn)，得出距離目標(biāo)點(diǎn)代價(jià)值最小的節(jié)點(diǎn).假設(shè)有n個(gè)節(jié)點(diǎn)，要計(jì)算的區(qū)間為

[(X1，Xgoal)，(X2，Xgoal)…(Xn，Xgoal)].

(5)

當(dāng)此啟發(fā)式采樣點(diǎn)Xrand與障礙物的連線之間存在障礙物時(shí)，在列表A中刪除這個(gè)Xrand，并在Xall中隨機(jī)選擇下一個(gè)點(diǎn)為Xrand，再次判斷碰撞.每次得到的可行的Xrand重新命名為Xnew存入列表B中.這樣選擇在一定程度上減少了重復(fù)采樣，既體現(xiàn)了使用啟發(fā)式搜索的有效性，又保留了原始RRT的隨機(jī)性.

(6)

式中：x，y分別表示橫向距離和縱向距離.

由(6)可得新節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的間距和角度：

(7)

(8)

式中：d表示新節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的間距；θ表示新節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)間的角度.

1.2 步長(zhǎng)選擇

為了提高導(dǎo)航的成功率和安全性，選擇適合環(huán)境的步長(zhǎng)來進(jìn)行路徑規(guī)劃，改進(jìn)算法提出了一種適應(yīng)全局的步長(zhǎng)選擇策略，在開始導(dǎo)航之前，先計(jì)算該環(huán)境的障礙物占比，障礙物占比較多，說明環(huán)境復(fù)雜，狹窄通道較多，選擇較小的步長(zhǎng)對(duì)于通過該環(huán)境更有效，步長(zhǎng)選擇方法為

(9)

式中：c表示障礙物占比；m為先驗(yàn)地圖中根據(jù)地圖大小隨機(jī)生成的點(diǎn)的個(gè)數(shù)；k為計(jì)數(shù)器計(jì)算的隨機(jī)點(diǎn)與障礙物碰撞次數(shù).

當(dāng)c大于或等于閾值p時(shí)，步長(zhǎng)選擇為2；當(dāng)c小于閾值p時(shí)，步長(zhǎng)選擇為3[16-17].即

(10)

1.3 距離優(yōu)先的啟發(fā)式采樣策略

改進(jìn)RRT算法的啟發(fā)式搜索是在空間中對(duì)每一個(gè)搜索位置進(jìn)行評(píng)估，得到最好的位置，再?gòu)倪@個(gè)位置重復(fù)搜索步驟一直到目標(biāo)為止，傳統(tǒng)RRT啟發(fā)式搜索的采樣方法存在隨機(jī)性和發(fā)散性，為了提高采樣效率和縮短通過狹窄通道的時(shí)間，改進(jìn)算法提出一種距離優(yōu)先采樣的啟發(fā)式搜索方法.

(11)

式中：Xnow為當(dāng)前節(jié)點(diǎn)；Xrand為隨機(jī)選擇節(jié)點(diǎn).

計(jì)算列表中的隨機(jī)節(jié)點(diǎn)和目標(biāo)點(diǎn)的距離，以固定的步長(zhǎng)L向Xnow、Xnew方向進(jìn)行生長(zhǎng)，優(yōu)先選擇使總代價(jià)值最小的點(diǎn)，生成新節(jié)點(diǎn)，此方法的代價(jià)函數(shù)為

f(s)=g(s)+h(s)，

(12)

式中：f(s)為路徑的總代價(jià)值；g(s)為已存在列表B中的當(dāng)前節(jié)點(diǎn)Xnew到選擇點(diǎn)Xrand的代價(jià)值，由步長(zhǎng)L加上優(yōu)先級(jí)?決定.計(jì)算方法為

(13)

式中：?為在以當(dāng)前點(diǎn)為原點(diǎn)所形成的二維坐標(biāo)系中象限優(yōu)先級(jí).目標(biāo)點(diǎn)所在的象限α優(yōu)先級(jí)記為1，對(duì)稱象限β記為3，其它兩個(gè)象限γ記為2，優(yōu)先選擇1象限里的目標(biāo)點(diǎn)，其次選擇2，最后選擇3.

g(s)=L+?，?[1，2，3].

(14)

h(s)為選擇點(diǎn)Xrand到目標(biāo)點(diǎn)Xgoal的代價(jià)值，計(jì)算方法為

(15)

(16)

2 仿真實(shí)驗(yàn)設(shè)計(jì)及測(cè)試

2.1 仿真實(shí)驗(yàn)設(shè)計(jì)

為了驗(yàn)證本算法的有效性和保證環(huán)境變量唯一性，分別設(shè)置3個(gè)障礙物占比不同，但大小相同的50米×30米的環(huán)境場(chǎng)景，該環(huán)境(2，2)為起始位置，(40，20)為目標(biāo)位置，設(shè)置啟發(fā)式概率為0.05，設(shè)置最大節(jié)點(diǎn)數(shù)為2 000，最大迭代次數(shù)為2 000.通過仿真實(shí)驗(yàn)進(jìn)行驗(yàn)證并分別與RRT、RRT*、Informed RRT*算法在多環(huán)境中的路徑規(guī)劃結(jié)果進(jìn)行對(duì)比.

如圖3所示，在只考慮規(guī)劃路徑問題上，無人車會(huì)將自身看成一個(gè)居于中心位置的質(zhì)點(diǎn)，但在實(shí)際工作空間中，無人車和障礙物邊緣通常是不規(guī)則形狀，為保證安全性和科學(xué)性，在配置空間中將障礙物邊緣設(shè)置膨脹層擴(kuò)大障礙物實(shí)體尺寸，將無人車身統(tǒng)一看成正圓，膨脹層厚度設(shè)置為車身半徑.對(duì)于一些較簡(jiǎn)單的不規(guī)則物體，設(shè)置適當(dāng)?shù)呐蛎泴涌赡苣軌蛴行У乇苊馀鲎?然而，對(duì)于非常復(fù)雜或特殊形狀的不規(guī)則物體，需要結(jié)合激光雷達(dá)或深度攝像頭等傳感器來實(shí)時(shí)感知環(huán)境，可以提供更精細(xì)的避障信息，從而使路徑規(guī)劃更加準(zhǔn)確和可靠.

圖3 轉(zhuǎn)化配置空間示意圖

2.2 仿真實(shí)驗(yàn)結(jié)果

環(huán)境A中障礙物占比為16.74%；環(huán)境B障礙物占比為22.45%，環(huán)境C障礙物占比為52.15%，各進(jìn)行100組重復(fù)實(shí)驗(yàn)，4種算法運(yùn)行效果如圖4所示.結(jié)果見表1.

表1 環(huán)境A的實(shí)驗(yàn)結(jié)果

圖4 環(huán)境A仿真實(shí)驗(yàn)

結(jié)合圖4和表1，在狹窄通道較少的環(huán)境中，改進(jìn)的RRT算法的平均搜索時(shí)間比傳統(tǒng)RRT減少約53%，搜索覆蓋率減少約28%.

環(huán)境B中，結(jié)合圖5和表2，在障礙物較分散的環(huán)境中下，改進(jìn)的RRT算法的平均時(shí)間較傳統(tǒng)RRT減少約58%，并且改進(jìn)算法只需要搜索較少的自由區(qū)域，搜索覆蓋率減少約33%.

表2 環(huán)境B的實(shí)驗(yàn)結(jié)果

圖5 環(huán)境B仿真實(shí)驗(yàn)

環(huán)境C中，結(jié)合圖6和表3，在狹窄通道較多的環(huán)境中，改進(jìn)的RRT算法具有較強(qiáng)的適應(yīng)能力，尋路時(shí)間減少了約62%，搜索覆蓋率減少約25.6%，體現(xiàn)出了啟發(fā)式搜索的有效性.

表3 環(huán)境C的實(shí)驗(yàn)結(jié)果

圖6 環(huán)境C仿真實(shí)驗(yàn)

3 改進(jìn)IA-RRT算法在無人車上的測(cè)試

改進(jìn)IA-RRT算法需要有控制軌跡和速度以及建圖和定位能力，綜合考慮成本和實(shí)現(xiàn)算法的可能性，文中設(shè)計(jì)的是Nvidia公司的jetson nano做計(jì)算板，Jetson Nano采用四核64位ARM CPU和128核集成NVIDIA GPU，動(dòng)力系統(tǒng)是有刷編碼電機(jī)通過電機(jī)的脈沖返回值可以測(cè)出無人車的速度，從而實(shí)現(xiàn)無人車的速度閉環(huán)控制.感知模塊是建圖與路徑規(guī)劃的重要數(shù)據(jù)來源，主傳感器裝備了激光雷達(dá)和雙目相機(jī)，其中，激光雷達(dá)用來避障和構(gòu)建室內(nèi)地圖，雙目相機(jī)識(shí)別障礙物.實(shí)驗(yàn)裝置如圖7所示.

圖7 實(shí)驗(yàn)裝置

經(jīng)過一系列仿真實(shí)驗(yàn)已經(jīng)基本驗(yàn)證算法的有效性和對(duì)于環(huán)境變化下的自適應(yīng)性.如圖8所示，設(shè)計(jì)了真實(shí)環(huán)境下的動(dòng)態(tài)實(shí)驗(yàn).在該實(shí)驗(yàn)中泡沫塊設(shè)置為未知障礙物.真實(shí)實(shí)驗(yàn)中，在微型無人車執(zhí)行任務(wù)遇到未知障礙物時(shí)，無人車能夠靈活地避開障礙物，優(yōu)化路徑，高效地完成任務(wù)，傳統(tǒng)算法進(jìn)行路徑規(guī)劃將導(dǎo)致整體路徑冗余、運(yùn)行時(shí)間過長(zhǎng)、死鎖，用改進(jìn)方法后，無人車加強(qiáng)了對(duì)環(huán)境的理解能力，優(yōu)化了環(huán)境帶來的路徑規(guī)劃問題.

圖8 真實(shí)環(huán)境實(shí)驗(yàn)

如圖9所示，設(shè)計(jì)一個(gè)回環(huán)路徑指令，展示了無人車從起點(diǎn)A到終點(diǎn)B的回環(huán)路徑.

圖9 實(shí)際環(huán)境中運(yùn)行的點(diǎn)云圖

圖中曲線軌跡為無人車軌跡，點(diǎn)云圖形為無人車識(shí)別出的障礙物.實(shí)驗(yàn)過程中，無人車能快速、準(zhǔn)確的計(jì)算路徑和通過障礙物，該實(shí)驗(yàn)展現(xiàn)了改進(jìn)IA-RRT算法對(duì)真實(shí)環(huán)境的適應(yīng)性和準(zhǔn)確性.

4 結(jié) 論

文中提出了一種改進(jìn)的IA-RRT算法，改進(jìn)算法的啟發(fā)式搜索是在空間中對(duì)每一個(gè)搜索位置進(jìn)行評(píng)估，得到最好的位置，再?gòu)倪@個(gè)位置重復(fù)搜索步驟一直到目標(biāo)為止，提高RRT算法在復(fù)雜環(huán)境中的路徑規(guī)劃效率.數(shù)值仿真實(shí)驗(yàn)結(jié)果表明：改進(jìn)的IA-RRT算法具有較強(qiáng)的適應(yīng)能力，體現(xiàn)出了啟發(fā)式搜索的有效性，并在微型無人車導(dǎo)航系統(tǒng)的路徑規(guī)劃測(cè)試中，能夠?qū)崟r(shí)進(jìn)行路徑規(guī)劃，展現(xiàn)了改進(jìn)的IA-RRT算法對(duì)真實(shí)環(huán)境的適應(yīng)性和準(zhǔn)確性.