[發明專利]一種考慮車頭指向的兩階段多抽樣的RRT路徑規劃方法

有效

• 申請號: 202011059189.3
• 申請日: 2020-09-30
• 公開（公告）號: CN112197783B
• 公開（公告）日: 2022-08-02
• 發明（設計）人: 呂宏武;陸東浩;李宜杭;王慧強;郭方方;馮光升;李冰洋
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/34
• 分類號: G01C21/34
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明屬于無人車路徑規劃技術領域，具體涉及一種考慮車頭指向的兩階段多抽樣的RRT路徑規劃方法。本發明以傳統RRT路徑規劃方法作為主體流程，采用兩階段多抽樣的隨機采樣點采集方法，并且考慮安全距離和路徑平滑，尤其是考慮了車頭的指向，使得本方法更加適合于無人車尋路。本發明用了兩階段多抽樣的方法確定隨機采樣點，縮短了規劃路徑的總長度。本發明在路徑規劃過程中考慮了車頭指向并對規劃出的路徑做了平滑處理，使輸出的規劃路徑結果更加適合于無人車尋路并且可以縮短規劃路徑的總長度，得到更加接近實際的行駛路徑。

技術領域

本發明屬于無人車路徑規劃技術領域，具體涉及一種考慮車頭指向的兩階段多抽樣的RRT路徑規劃方法。

背景技術

目前，在以大數據和云計算等新技術推動的新一輪技術革命的背景下，無人駕駛技術正在迅猛發展。無人車的路徑規劃是指已知車輛的起點、終點以及環境中的障礙物分布，規劃出一條從起點至終點的與障礙物不相碰撞的路徑，路徑規劃是無人車技術中的關鍵技術之一。

為了滿足無人車路徑規劃的特點，相關研究者也進行了大量的工作。Dijkstra算法發明很早，用于最短路徑的搜索，但搜索過程中過多無關節點的擴展會極大降低算法效率。A*是一種啟發式最優搜索算法，其框架與Dijkstra基本一致，但增加了對當前節點到目標節點最低代價的估計函數，從而使靠近目標節點的節點獲得優先擴展機會，但當搜索柵格較多時，需處理的節點數目過多，算法效率不高。基于仿生優化算法(諸如蜂群、蟻群等)的路徑規劃，通常需要對大量參數進行訓練，而這些參數與場景緊密耦合，移植性不強。RRT(Rapid-exploration Random Tree，快速擴展隨機樹)的基本思想是以產生隨機點的方式通過一個固定步長向目標點搜索前進，有效躲避障礙物，避免路徑陷入局部極小值，收斂速度快。但是隨著采樣點數目的增加，樹最終會稠密地充滿整個狀態空間自由區域，算法效率很低，尤其是未考慮車頭的指向。對于車輛而言，通常會選擇小角度轉向而不會采用倒車往反方向運動，因此傳統RRT算法往往規劃出的路線與實際運動規律不符。

發明內容

本發明的目的在于提供解決已知車輛的起點、終點以及環境中的障礙物分布，在考慮車頭指向的條件下，規劃出一條從起點至終點的與障礙物不相碰撞的平滑路徑的問題的一種考慮車頭指向的兩階段多抽樣的RRT路徑規劃方法。

本發明的目的通過如下技術方案來實現：包括以下步驟：

步驟1：初始化地圖空間，輸入無人車的起點P_init、終點P_goal以及環境中的障礙物分布信息；初始化隨機樹T、步長L、采樣輪次k＝0及采樣輪次上限M；設定安全距離SD；其中，以起點P_init作為隨機樹T的根節點；步長L為無人車每一步運動的長度；采樣輪次k初始值為0，每確定一個隨機采樣點P_rand，采樣輪次k的值都加1，當kM時，采樣終止；

步驟2：確定隨機采樣點P_rand進行RRT樹節點預擴展；

步驟2.1：設定在地圖空間中任意位置隨機取點作為一號臨時采樣點TP₁的概率為p₁₁；設定在地圖空間中自起點至終點的線段上隨機取點作為一號臨時采樣點TP₁的概率為p₁₂；執行第一階段抽樣，確定一號臨時采樣點TP₁；

步驟2.2：查找隨機樹T中離一號臨時采樣點TP₁最近的樹節點P_near作為本次樹節點擴展的原點；

步驟2.3：執行第二階段抽樣；以一號臨時采樣點TP₁為原點，以隨機樹T中離一號臨時采樣點TP₁最近的樹節點P_near和一號臨時采樣點TP₁的連線為橫軸，設定常數a，順時針每隔a°確定一個候選臨時采樣點CTP_i，共確定360/a個候選臨時采樣點，構成集合CTP；