地下狹窄空間探索無人車的改進路徑優化算法研究

楊 超， 袁曉宇

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

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

文中提出了一種針對狹窄空間探索的改進IA-RRT算法，該算法可以產生更有效的導航路徑.它引入了一種距離優先的啟發式采樣策略，通過距離計算，優點選點等任務進行階段規劃，以及采用了一種適應環境的步長選擇策略，通過環境中障礙物的占比實現合理分配更適合環境的步長.

1 改進RRT算法

1.1 改進思想

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

圖1 RRT采樣示意圖

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

圖2 改進算法采樣示意圖

采樣計算方式如下

(1)

式中：Xnew為每次尋找到的可以加入樹中的節點；σ為以Xnew為圓心建立坐標系的x軸的正方向；τ為負方向；L為步長；i為圓上正方向或負方向生成節點的個數.

(2)

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

選擇節點的縱坐標可以表示為

(3)

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

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

(4)

把Xall保存到列表A中，每次采樣的第一個節點Xall1，是由啟發函數計算列表A中所有節點，得出距離目標點代價值最小的節點.假設有n個節點，要計算的區間為

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

(5)

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

(6)

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

由(6)可得新節點到目標節點的間距和角度：

(7)

(8)

式中：d表示新節點到目標節點的間距；θ表示新節點與目標節點間的角度.

1.2 步長選擇

為了提高導航的成功率和安全性，選擇適合環境的步長來進行路徑規劃，改進算法提出了一種適應全局的步長選擇策略，在開始導航之前，先計算該環境的障礙物占比，障礙物占比較多，說明環境復雜，狹窄通道較多，選擇較小的步長對于通過該環境更有效，步長選擇方法為

(9)

式中：c表示障礙物占比；m為先驗地圖中根據地圖大小隨機生成的點的個數；k為計數器計算的隨機點與障礙物碰撞次數.

當c大于或等于閾值p時，步長選擇為2；當c小于閾值p時，步長選擇為3[16-17].即

(10)

1.3 距離優先的啟發式采樣策略

改進RRT算法的啟發式搜索是在空間中對每一個搜索位置進行評估，得到最好的位置，再從這個位置重復搜索步驟一直到目標為止，傳統RRT啟發式搜索的采樣方法存在隨機性和發散性，為了提高采樣效率和縮短通過狹窄通道的時間，改進算法提出一種距離優先采樣的啟發式搜索方法.

(11)

式中：Xnow為當前節點；Xrand為隨機選擇節點.

計算列表中的隨機節點和目標點的距離，以固定的步長L向Xnow、Xnew方向進行生長，優先選擇使總代價值最小的點，生成新節點，此方法的代價函數為

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

(12)

式中：f(s)為路徑的總代價值；g(s)為已存在列表B中的當前節點Xnew到選擇點Xrand的代價值，由步長L加上優先級?決定.計算方法為

(13)

式中：?為在以當前點為原點所形成的二維坐標系中象限優先級.目標點所在的象限α優先級記為1，對稱象限β記為3，其它兩個象限γ記為2，優先選擇1象限里的目標點，其次選擇2，最后選擇3.

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

(14)

h(s)為選擇點Xrand到目標點Xgoal的代價值，計算方法為

(15)

(16)

2 仿真實驗設計及測試

2.1 仿真實驗設計

為了驗證本算法的有效性和保證環境變量唯一性，分別設置3個障礙物占比不同，但大小相同的50米×30米的環境場景，該環境(2，2)為起始位置，(40，20)為目標位置，設置啟發式概率為0.05，設置最大節點數為2 000，最大迭代次數為2 000.通過仿真實驗進行驗證并分別與RRT、RRT*、Informed RRT*算法在多環境中的路徑規劃結果進行對比.

如圖3所示，在只考慮規劃路徑問題上，無人車會將自身看成一個居于中心位置的質點，但在實際工作空間中，無人車和障礙物邊緣通常是不規則形狀，為保證安全性和科學性，在配置空間中將障礙物邊緣設置膨脹層擴大障礙物實體尺寸，將無人車身統一看成正圓，膨脹層厚度設置為車身半徑.對于一些較簡單的不規則物體，設置適當的膨脹層可能能夠有效地避免碰撞.然而，對于非常復雜或特殊形狀的不規則物體，需要結合激光雷達或深度攝像頭等傳感器來實時感知環境，可以提供更精細的避障信息，從而使路徑規劃更加準確和可靠.

圖3 轉化配置空間示意圖

2.2 仿真實驗結果

環境A中障礙物占比為16.74%；環境B障礙物占比為22.45%，環境C障礙物占比為52.15%，各進行100組重復實驗，4種算法運行效果如圖4所示.結果見表1.

表1 環境A的實驗結果

圖4 環境A仿真實驗

結合圖4和表1，在狹窄通道較少的環境中，改進的RRT算法的平均搜索時間比傳統RRT減少約53%，搜索覆蓋率減少約28%.

環境B中，結合圖5和表2，在障礙物較分散的環境中下，改進的RRT算法的平均時間較傳統RRT減少約58%，并且改進算法只需要搜索較少的自由區域，搜索覆蓋率減少約33%.

表2 環境B的實驗結果

圖5 環境B仿真實驗

環境C中，結合圖6和表3，在狹窄通道較多的環境中，改進的RRT算法具有較強的適應能力，尋路時間減少了約62%，搜索覆蓋率減少約25.6%，體現出了啟發式搜索的有效性.

表3 環境C的實驗結果

圖6 環境C仿真實驗

3 改進IA-RRT算法在無人車上的測試

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

圖7 實驗裝置

經過一系列仿真實驗已經基本驗證算法的有效性和對于環境變化下的自適應性.如圖8所示，設計了真實環境下的動態實驗.在該實驗中泡沫塊設置為未知障礙物.真實實驗中，在微型無人車執行任務遇到未知障礙物時，無人車能夠靈活地避開障礙物，優化路徑，高效地完成任務，傳統算法進行路徑規劃將導致整體路徑冗余、運行時間過長、死鎖，用改進方法后，無人車加強了對環境的理解能力，優化了環境帶來的路徑規劃問題.

圖8 真實環境實驗

如圖9所示，設計一個回環路徑指令，展示了無人車從起點A到終點B的回環路徑.

圖9 實際環境中運行的點云圖

圖中曲線軌跡為無人車軌跡，點云圖形為無人車識別出的障礙物.實驗過程中，無人車能快速、準確的計算路徑和通過障礙物，該實驗展現了改進IA-RRT算法對真實環境的適應性和準確性.

4 結 論

文中提出了一種改進的IA-RRT算法，改進算法的啟發式搜索是在空間中對每一個搜索位置進行評估，得到最好的位置，再從這個位置重復搜索步驟一直到目標為止，提高RRT算法在復雜環境中的路徑規劃效率.數值仿真實驗結果表明：改進的IA-RRT算法具有較強的適應能力，體現出了啟發式搜索的有效性，并在微型無人車導航系統的路徑規劃測試中，能夠實時進行路徑規劃，展現了改進的IA-RRT算法對真實環境的適應性和準確性.