無人駕駛路徑規划算法——Probabilistic RoadMap Planner(1)

中談了Probabilistic RoadMap的算法原理,這次討論算法Python實現。

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

圖構建階段(construction)

點採樣,採樣過程中剔除在障礙物區域中的點。

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

採用KDTree實現近鄰查找

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

生成RoadMap,obkdtree是障礙物的kdtree。算法先尋找採樣點附近的K近鄰點,將與障礙物碰撞的變剔除掉。

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

障礙物碰撞的算法

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

查詢(query)階段

查詢是一次圖搜索的過程。

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)

PRM的整體實現

無人駕駛路徑規劃算法——Probabilistic RoadMap Planner(1)


分享到:


相關文章: