3.2.c RRT代码解析 | 自动驾驶小白说

在tests/sampling_based_planning/rrt.py 中给定start point和goal point，在上述构建的地图环境中进行测试，代码如下：

get_random_node : 获取随机点

get_nearest_node_index ：在已有的node list中找到最近点

steer ：按照一定的步长往前计算得到新节点

check_if_outside_play_area：判断新节点是否出了地图

check_collision ：判断是否节点和边有碰撞

calc_dist_to_goal ：判断是否到终点了，即算法结束

generate_final_course ：使用回溯的方式得到最终的路径

测试结果如下，可以看到RRT算法可以找到一条从起点到终点的路径（但不是最优的）。

RRT Star的算法实现和RRT的算法实现大体相同，为了保证代码的独立性，将其定义在tests/sampling_based_planning/rrt_star.py 中，并在上述构建的地图环境中进行测试，代码如下：

除了与RRT算法中相同的函数，此处不再赘述，其他的函数定义如下：

new_node.cost ：每个节点定义了cost，此处用的是欧式距离

choose_parent ：给新节点重新寻找父节点

rewite ：随机树的重布线

search_best_goal_node ：在当前的迭代情况下，找寻最优的目标节点

测试结果如下，可以看到RRT Star算法得到的路径相比RRT算法得到的路径更优。