Page 193 - 《软件学报》2025年第7期

3114 软件学报 2025 年第 36 卷第 7 期

具体表现. 记 nd 0 为根节点, 满足 nd 0 .nd = None; 若节点 nd 满足 nd.ND = ∅, 即该节点的后继为空, 则称该节点为
s
p
叶子节点.

200
终止点
180
160
生成的 140
随机树
120
通过随机树回
100 溯生成路径
80
60 威胁
40
20 起始点
0
0 20 40 60 80 100 120 140 160 180 200
图 2 RRT 路径规划算法示意图

B
E
基于上述定义, 现已知环境 E 包含任务空间 M 和威胁集合 Obs, 规划任务为 task 包含起点 p 和终点 p , 则随
机树的具体构建方法如下.
Step 1. 初始化. 在 M 空间内初始化一棵只有根节点的树 Tree = {nd 0 }, 并将根节点的位置设为任务的起始点,
c
B
即令 nd 0 .p = task.p .
Step 2. 随机采样. 假设当前已经生成的随机树为 Tree, 在空间 M 中随机生成一个临时的坐标点 p tmp , 并在
Tree 中搜索与 p tmp 最近且后继节点未满的节点 nd , 即:
′

c
tmp
′
nd = argmin dis(nd.p , p ) (3)
nd∈Tree∧|nd.ND s |<2
其中, dis 为距离函数.
Step 3. 更新树. 求 nd 到 ′ p tmp 的向量 np:

tmp
c
tmp
c
′
′
np = [p .x−nd .p .x, p .y−nd .p .y] (4)
′
Step 4. 尝试以 nd 为父节点沿着 np 方向扩展一个新节点 nd new , 该节点在 nd 到 ′ p tmp 的连线上, 并且 nd new 到
′ ag.r. 具体来说, 令:
nd 的距离等于智能体的步长

( )
np.x np.y
(δx,δy) = ag.r · , ·(δx,δy) (5)
∥np∥ ∥np∥
则有:

′
′
′
nd new = ⟨nd ,(nd .x+δx,nd .y+δy),∅⟩ (6)
公式 (6) 将新的叶子节点 nd new 的前驱节点设为 nd , 即令 nd new .nd = nd , 并将其后继节点集设为空.
p
′
′
Step 5. 检查新节点 nd new 的有效性, 即其是否与威胁障碍物发生碰撞, 如公式 (7) 所示.

c
∃obs ∈ E.Obs, nd new .p ∈ obs (7)
若公式 (7) 满足则说明新的节点会引发碰撞, 那么我们删除该节点, 并返回 Step 2 继续采样; 反之, 则说明新
的节点是安全的, 那么我们将该节点加入当前的树中, 即令:

Tree = Tree∪{nd new } (8)
′ nd new , 即令:
同时, 向其父节点 nd 的后继节点集中加入

S
s
′
′
nd .ND = nd .ND ∪{nd new } (9)
′
其中, nd 表示节点 nd 的子节点集合.
G
E
Step 6. 停止条件. 判断新节点 nd new 与规划任务的目标点 task.p 的距离, 若该距离大于阈值 θ , 则返回 Step 2,
G
E
继续随机树的构建; 反之, 若 nd new 与 task.p 的距离小于阈值 θ , 说明我们已经找到到达终点的路径, 则将 nd new 记