采样 Xrand → 找 Xnear → 朝它生成 Xnew → 线段碰撞检测 → 成功则“加入树与边” → 若 Xnew 距目标 < t 则输出路径

1、初始化：把起点作为根节点加入树；设定边界（采样范围）、步长 S、到达阈值 t、最大迭代次数 N、障碍物表示与碰撞检测函数。

2、循环扩展

• 1） 随机采样 q_rand，是临时点，只用来决定树往哪儿伸一步，它本身不进树
• 2） 最近邻搜索：在树中找离 q_rand 最近的 q_near（即q_near是“已长出的树”的节点）
• 3） 生成新点：从 q_near 朝 q_rand 方向走固定步长 S 得 q_new
• 4） 检查线段 q_near→q_new 是否碰障碍；碰了就丢弃，不碰就把 q_new 加进树，并连一条蓝线
• 5）更新“当前最好节点”：如果 q_new 比历史上任何节点都更接近目标，就把“最好节点”改成 q_new，并把“起点→q_new”的父指针链画成红线
• 6）终止判定： 如果 q_new 距目标小于阈值 t（认为“够近了”），结束并回溯父指针得到路径，红线就是最终路径；否则继续

3、输出路径：从末节点沿父指针回溯到起点得到折线；可再做“捷径平滑”（随机挑两点直连、B样条等）

• 红线：从起点到“当前最有希望的那个树节点”的父指针路径。这个“最有希望”在多数演示里指的是“树里离目标最近的节点”。每出现一个更靠近目标的新节点，红线就会改为通向它的那条链。
• 浅蓝线：整棵“搜索树”的所有边。它显示了算法迄今为止尝试过、并且无碰撞的延伸。

当出现了一个距离目标更近的节点，红线就改为通向这个新节点的父链——这会让红线向目标大方向推进

发现蓝色树另一支存在离目标更近的节点，轨迹重新规划：

树的覆盖范围很大了（蓝线）

一旦某次 q_new 落在距离目标 < t 的圆内，红线就定格成最终路径