
基于MATLAB的全局路径规划算法:RRT、RRT Star、RRT_Connect及其改进方法
最近在折腾机器人路径规划的时候,被各种RRT变种算法搞得又爱又恨。这玩意儿就像玩迷宫游戏,
既要绕开障碍物又要找最短路线。今天咱们就扒一扒这个让无数工程师秃头的RRT家族,手把手撸几个Mat
lab代码片段看看门道。
先来段最原始的RRT算法核心代码热热身:
```matlab
function new_node = extend_rrt(tree, goal, map)
rand_point = [randi(map.width), randi(map.height)];
nearest_node = find_nearest(tree, rand_point);
new_point = steer(nearest_node.position, rand_point, step_size);
if collision_free(nearest_node.position, new_point, map)
new_node.position = new_point;
new_node.parent = nearest_node;
tree.add(new_node);
if norm(new_point - goal) < goal_radius
path = extract_path(new_node);
return
end
end
end
```
这段代码里藏着RRT的精髓——随机撒点+最近邻连接。steer函数控制着每次扩展的步长,就像盲人
摸象一样在空间里试探。不过原始RRT生成的路径跟喝醉似的歪歪扭扭,这时候就该RRT*登场了。
看这段改进的重新布线逻辑:
```matlab
% RRT*特有的重选父节点环节
near_nodes = find_near_nodes(tree, new_point, radius);
min_cost = nearest_node.cost + norm(new_point - nearest_node.position);
for node = near_nodes