[發明專利]變概率雙向快速搜索隨機樹改進路徑規劃算法

有效

• 申請號: 201810561112.2
• 申請日: 2018-06-01
• 公開（公告）號: CN108762270B
• 公開（公告）日: 2021-04-27
• 發明（設計）人: 宋燕;胡澮冕;何壯壯;陳晗
• 申請（專利權）人: 上海理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京東方盛凡知識產權代理事務所(普通合伙) 11562
• 代理人: 王穎

本發明涉及一種變概率雙向快速搜索隨機樹改進路徑規劃算法，首先，在導入狀態空間圖時，需要根據車輛體積的設定進行狀態空間預處理，將狀態空間邊緣進行擴展保護，防止節點過于接近狀態空間邊緣，導致碰撞。其次，采用了一種基于節點環境的變概率目標選取策略加快收斂速度，最后，對生成后的路徑進行去彎取直，即以第一個節點為起點與后續節點的連線之間是否有障礙物判斷，刪除多余節點，進行優化路徑，減少小車行駛過程中的拐彎次數和總路徑長度。在原始雙向RRT算法上實現變概率優化算法，利用目標指向的方式提高搜索速度，降低計算量；同時降低路徑長度和節點個數，并確保了可通行性。

技術領域

本發明涉及一種智能車路徑規劃技術，特別涉及一種變概率雙向快速搜索隨機樹改進路徑規劃算法。

背景技術

智能車路徑規劃是指智能車在位姿空間中找到一條初始位姿點到目標位姿點的連續無碰撞路徑，同時這條路徑還要滿足環境約束以及智能車本身運動特點。

針對這種情況，人們提出了RRT(快速搜索隨機樹)算法，該算法核心是隨機搜索方式，能夠在高維非凸空間實現路徑搜索，由于其采用隨機采樣的規劃方式，不需要預處理，而且搜索速度快，在高維空間速度優勢尤其明顯，近年來在機器人運動規劃領域得到了廣泛的應用和研究。但是其本身也存在一些缺陷：(1)采樣為隨機采樣，大量無效采樣導致算法收斂速度慢；(2) 度量函數(最近鄰算法)在存在復雜約束的情況下可能會很難獲得有效解； (3)算法的隨機性會導致生成的路徑不平滑，直接使用路徑時會出現大量轉折與無效曲線路徑。針對基本RRT算法的上述不足，國內外學者也對該算法進行了不斷地改進，以適應不同的應用環境。Kuffner和LaValle提出了 Bi-RRT(雙向RRT)，從初始狀態和目標狀態并行生成兩棵樹，能夠快速實現收斂；隨后他們又提出了RRT-connect算法，該算法在節點擴展時采用指向性拓展，其指向性拓展的概率也需要在實際應用中根據情況選取。對于RRT算法的隨機性所造成的路徑不平滑問題，Fraichard和Scheuer提出了用回旋曲線來做平滑處理，但是回旋曲線求解計算量很大；Lau等人采用了5次貝塞爾曲線，但是沒有考慮路徑曲率的連續性和機器人的自身特點，計算量也較大。

發明內容

本發明是針對快速搜索隨機樹算法解決機器人運動規劃存在的問題，提出了一種變概率雙向快速搜索隨機樹改進路徑規劃算法，保證搜索速度下確保通行性。

本發明的技術方案為：一種變概率雙向快速搜索隨機樹改進路徑規劃算法，具體包括如下步驟：

1)根據運行車輛體積的設定進行狀態空間預處理：即擴大障礙物區域，預留出安全距離；

2)對變概率雙向RRT算法進行路徑生成：雙向RRT算法生成路徑L，在迭代生長路徑生成過程中，加入變概率算法，增加了周邊環境的約束條件，每生成一個節點后，采用函數detect(q)檢測以此節點為行駛車輛中心，行駛車輛所需面積圓內是否存在遮擋，即面積圓是否與障礙物區域有交集，如有交集，根據交集由均勻概率分布隨機獲得一個概率值P，概率值P與設定的目標偏向概率值 Pset進行對比，Pset小于10％，P大于Pset，則放棄此節點，重新采用隨機函數生成節點；當P小于Pset時，則認可此節點，放入對應集合；

3)優化路徑：

3.1)初始化：將已生成路徑L節點path賦予Q，Q＝(q₀，q₁，q₂…q_n)，并且將 q₀賦予q_temp，i＝1；

3.2)通過checkPath()函數依次判斷q_temp與其后續節點q_i∈Q連接線段之間是否存在障礙物，i＜n，判斷執行3.3)；當i＞＝n時，進入3.4)；