[發(fā)明專利]一種基于動態(tài)步長的目標引力雙向RRT路徑規(guī)劃方法

在審

• 申請?zhí)枺?/td>: 201810767113.2
• 申請日: 2018-07-13
• 公開（公告）號: CN108981704A
• 公開（公告）日: 2018-12-11
• 發(fā)明（設計）人: 胡兵;向鳳紅;毛劍琳;郭寧
• 申請（專利權）人: 昆明理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于動態(tài)步長的目標引力雙向RRT路徑規(guī)劃方法，其特征在于：所述方法步驟如下：

Step1、設置智能機器人所處的狀態(tài)空間C，靜態(tài)障礙物隨機分布，大小不一；

Step2、確定智能機器人在狀態(tài)空間C中的初始點Xinit和目標點Xgoal位置；其中，狀態(tài)空間C指代工作環(huán)境，狀態(tài)空間C由自由空間Xfree和障礙空間Xobs構成，障礙空間Xobs由靜態(tài)障礙物構成，自由空間Xfree表示除障礙空間Xobs之外的空間；初始點Xinit和目標點Xgoal位于自由空間Xfree；

Step3、同時以初始點Xinit和目標點Xgoal為起點分別擴展RRT隨機樹Ti和Tj，搜索過程中，根據(jù)基于動態(tài)步長的目標引力雙向RRT算法新節(jié)點Xnew的位置確定公式來搜索新節(jié)點Xnew，每搜索新節(jié)點Xnew一次，將其加入到對應的隨機樹中，直到兩顆隨機樹新節(jié)點Xnew在狀態(tài)空間C中相遇，選出兩顆隨機樹新節(jié)點Xnew連通后生成的最優(yōu)路徑并保存。

2.根據(jù)權利要求1所述的基于動態(tài)步長的目標引力雙向RRT路徑規(guī)劃方法，其特征在于：所述根據(jù)基于動態(tài)步長的目標引力雙向RRT算法新節(jié)點Xnew的位置確定公式來搜索新節(jié)點Xnew，具體為：

其中，ρ為朝著隨機點Xrand方向生長的步長，k為引力場系數(shù)，kρ為朝著目標點Xgoal方向生長的步長，||Xrand－Xnear||代表歐拉范數(shù)定義的Xrand與Xnear之間的距離，||Xgoal－Xnear||代表歐拉范數(shù)定義的Xgoal與Xnear之間的距離，Xrand為每次擴展RRT隨機樹時在自由區(qū)域Xfree中隨機選取的點，Xnear為每次擴展RRT隨機樹時在當前RRT隨機樹中與Xrand距離最近的點，Xnew每次新增加的擴展點。

3.根據(jù)權利要求2所述的基于動態(tài)步長的目標引力雙向RRT路徑規(guī)劃方法，其特征在于：當RRT隨機樹在運動范圍內遇到障礙物時取ρ大于等于kρ，根據(jù)擴展新節(jié)點Xnew的公式在ρ大于等于kρ時生成新節(jié)點Xnew的方法，RRT隨機樹將具有雙向RRT算法的隨機性，偏向隨機點Xrand方向生長，不會碰到障礙物；當RRT隨機樹運動范圍內沒有遇到障礙物時取ρ小于kρ，根據(jù)擴展新節(jié)點Xnew的公式在ρ小于kρ時生成新節(jié)點Xnew的方法，RRT隨機樹偏向終點方向生長。