[發明專利]基于三角形內心引導RRT算法的路徑規劃方法

有效

• 申請號: 201910964030.7
• 申請日: 2019-10-11
• 公開（公告）號: CN110705803B
• 公開（公告）日: 2022-06-21
• 發明（設計）人: 張衛波;肖繼亮;陳泉泉
• 申請（專利權）人: 福州大學
• 主分類號: G06Q10/04
• 分類號: G06Q10/04
• 代理公司: 福州元創專利商標代理有限公司 35100
• 代理人: 陳明鑫;蔡學俊

1.一種基于三角形內心引導RRT算法的路徑規劃方法，提供規劃路徑的起始點qstart、目標點qgoal、障礙物位置和規劃地圖，其特征在于，包括以下步驟：

步驟1：定義步長eps和最大迭代次數n，以qstart為隨機樹T的根節點，并將其作為下一次擴展隨機樹的父節點，以qgoal為隨機樹T的目標點，三角形內心采樣點數Ndc初始值為0，隨機點采樣數Rdc初始值為0，初始化隨機樹；

步驟2：通過隨機函數生成隨機點qrand的坐標值；

步驟3：遍歷隨機樹T中的每個節點，通過計算隨機樹T中節點與隨機點qrand之間的距離，找出離隨機點qrand距離最小的節點設為qnear；

步驟4：在Ndc數值小于100的情況下，以隨機點qrand、離隨機點qrand距離最小的隨機點qnear及目標節點qgoal三點的坐標值作為三角形三個頂點的坐標值，計算三個頂點組成的三角形的內心坐標節點qinh重新賦值給隨機點qrand；

步驟5：從節點qnear向隨機點qrand方向延伸步長eps，得到新節點qnew；

步驟6：判斷新節點qnew與節點qnear之間是否存在障礙物，若不存在，則將生成的新節點qnew加入的隨機樹T中，并判斷新節點qnew的坐標值是否在目標點qgoal坐標值步長eps范圍內，若在目標點qgoal坐標值步長eps范圍內，表明已經找到路徑，跳出循環，否則進行下一步；若存在障礙物，則舍棄新節點qnew；

步驟7：更新三角形內心采樣點數Ndc及隨機點采樣數Rdc；

步驟8：重復步驟2-7，直到到達目標點的eps范圍內或達到最大迭代次數；

步驟9：將位于目標點qgoal的步長eps范圍內的節點作為目標節點的父節點，從目標點qgoal開始，根據父節點回溯到起始點qstart，得到最終路徑；

所述步驟4中的三角形內心坐標計算公式如下：

上式中的x1表示隨機點qrand的x坐標值，x2表示節點qnear的x坐標，x3表示目標點qgoal的x坐標值，y1表示隨機點qrand的y坐標值，y2表示節點qnear的y坐標，y3表示目標點qgoal的y坐標值；

所述步驟5中新節點qnew坐標的計算過程為：

計算節點qnear與隨機點qrand之間的距離為val；若val小于eps，則新節點qnew的坐標即為隨機點qrand的坐標，否則新節點qnew的坐標由以下公式計算得出：

上式中的qnear.x表示節點qnear的x坐標、qrand.x表示隨機點qrand的x坐標值、qenar.y表示節點qnear的y坐標、qrand.y表示隨機點qrand的y坐標值,dist表示節點qnear和隨機點qrand的距離。

2.根據權利要求1所述的基于三角形內心引導RRT算法的路徑規劃方法，其特征在于：所述步驟6中，判斷新節點qnew的坐標值是否在目標點qgoal坐標值步長eps范圍內，是通過判斷新節點qnew是否在以qgoal為圓心，以eps為半徑的圓域內。

3.根據權利要求1所述的基于三角形內心引導RRT算法的路徑規劃方法，其特征在于：所述步驟7中的更新三角形內心采樣點數Ndc及隨機點采樣數Rdc具體操作為：若Ndc數值小于100且新節點qnew到目標點的距離小于隨機樹T中除新節點qnew以外的任一點到目標點的距離qmin，則將新節點qnew到目標點的距離賦值給qmin，Ndc的值歸零；若Ndc數值小于100且新節點qnew到目標點的距離不小于隨機樹T中除新節點qnew以外的任一點到目標點的距離qmin，Ndc數值加一；若Ndc數值小于100，Rdc數值歸零；若Ndc數值不小于100，Rdc數值加一；若Rdc數值大于50，Ndc數值歸零。