无人驾驶路径规划算法——ProbabilisticRoadMapPlanner中谈了ProbabilisticRoadMap的算法原理,这次讨论算法Python实现。
图构建阶段(construction)点采样,采样过程中剔除在障碍物区域中的点。
采用KDTree实现近邻查找
生成RoadMap,obkdtree是障碍物的kdtree。算法先寻找采样点附近的K近邻点,将与障碍物碰撞的变剔除掉。
障碍物碰撞的算法
查询(query)阶段查询是一次图搜索的过程。
PRM的整体实现
领取专属 10元无门槛券
私享最新 技术干货