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

有效

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

1.一種考慮車頭指向的兩階段多抽樣的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；

步驟2.4：從候選臨時采樣點集合CTP中，剔除掉碰撞障礙物的候選臨時采樣點，剩余的n個候選臨時采樣點的集合為S；

步驟2.5：從集合S中隨機選擇二號臨時采樣點TP₂；

步驟2.6：以隨機樹T中離一號臨時采樣點TP₁最近的樹節點P_near為起點，向P_near和二號臨時采樣點TP₂的連線方向延伸步長L，此線段的終點為此兩階段所確定的隨機采樣點P_rand；

步驟2.7：隨機樹T向隨機采樣點P_rand進行預擴展；

步驟3：無人車從隨機樹T中離一號臨時采樣點TP₁最近的樹節點P_near運行到隨機采樣點P_rand的過程中，判斷無人車與障礙物是否無碰撞、無人車與障礙物是否滿足安全距離SD、無人車轉向角α是否滿足α90°；若三個條件判斷結果都為真，則執行步驟4；否則，執行步驟8；

無人車的轉向角α具體為：以隨機樹T中離一號臨時采樣點TP₁最近的樹節點P_near為原點向其前驅節點P_pioneer方向引出的射線與以樹節點P_near為原點向隨機采樣點P_rand方向引出的射線，兩條射線的夾角即為無人車轉向角α；若樹節點P_near為起點P_init，則以樹節點P_near為原點向終點P_goal方向引出的射線與以樹節點P_near為原點向隨機采樣點P_rand方向引出的射線，兩條射線的夾角即為無人車轉向角α；

步驟4：隨機樹T向隨機采樣點P_rand進行擴展，將樹節點與對應的邊添加到隨機樹T中，同時采樣輪次k+1；

步驟5：判斷終點P_goal是否包含在隨機樹T中；若終點P_goal包含在隨機樹T中，則執行步驟6；否則，執行步驟8；

步驟6：對步驟5輸出的路徑做平滑處理；

步驟7：判斷平滑后的路徑是否滿足安全距離SD；若平滑后的路徑滿足安全距離SD，則跳執行步驟9；否則，執行步驟8；

步驟8：判斷當前采樣輪次k是否大于采樣輪次上限M；若kM，則判斷無符合要求的路徑；否則，返回步驟2；

步驟9：輸出滿足要求的平滑路徑，完成路徑規劃。