% Pick the closest node from existing list to branch out from
ndist = [];
for j = 1:1:length(nodes)
n = nodes(j);
tmp = dist(n.coord, q_rand);
ndist = [ndist tmp];
end
val, idx\] = min(ndist);
q_near = nodes(idx);
q_new.coord = steer(q_rand, q_near.coord, val, EPS);
if noCollision(q_rand, q_near.coord, obstacle)
line(\[q_near.coord(1), q_new.coord(1)\], \[q_near.coord(2), q_new.coord(2)\], 'Color', 'k', 'LineWidth', 2);
drawnow
hold on
q_new.cost = dist(q_new.coord, q_near.coord) + q_near.cost;
% Within a radius of r, find all existing nodes
q_nearest = \[\];
r = 60;
neighbor_count = 1;
for j = 1:1:length(nodes)
if noCollision(nodes(j).coord, q_new.coord, obstacle) \&\& dist(nodes(j).coord, q_new.coord) \<= r
q_nearest(neighbor_count).coord = nodes(j).coord;
q_nearest(neighbor_count).cost = nodes(j).cost;
neighbor_count = neighbor_count+1;
end
end
% Initialize cost to currently known value
q_min = q_near;
C_min = q_new.cost;
% Iterate through all nearest neighbors to find alternate lower
% cost paths
for k = 1:1:length(q_nearest)
if noCollision(q_nearest(k).coord, q_new.coord, obstacle) \&\& q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord) \< C_min
q_min = q_nearest(k);
C_min = q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord);
line(\[q_min.coord(1), q_new.coord(1)\], \[q_min.coord(2), q_new.coord(2)\], 'Color', 'g');
hold on
end
end
% Update parent to least cost-from node
for j = 1:1:length(nodes)
if nodes(j).coord == q_min.coord
q_new.parent = j;
end
end
% Append to nodes
nodes = \[nodes q_new\];
end
end
D = \[\];
for j = 1:1:length(nodes)
tmpdist = dist(nodes(j).coord, q_goal.coord);
D = \[D tmpdist\];
end
## ****🎉3**** ****参考文献****
> 文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。
\[1\] LaValle, S. M., 'Rapidly-Exploring Random Trees: A New Tool for Path Planning', TR 98-11, Computer Science Department, Iowa State University, Oct. 1998.
\[2\] Karaman, Sertac, and Emilio Frazzoli. "Incremental sampling-based algorithms for optimal motion planning." Robotics Science and Systems VI 104 (2010).
\[3\]莫栋成,刘国栋.改进的快速探索随机树双足机器人路径规划算法\[J\].计算机应用, 2013, 33(01):199-201.DOI:10.3724/SP.J.1087.2013.00199.
## [🌈](https://mp.weixin.qq.com/mp/appmsgalbum?__biz=Mzk0MDMzNzYwOA==&action=getalbum&album_id=2591810113208958977#wechat_redirect "🌈")****4 Matlab代码实现****
