SAMPLING-BASED MOTION PLANNING · 6-DOF

融合 EST 启发式与 RRT-Connect 的双向树路径规划器

Bidirectional Expansive-Space Trees with Greedy Dual-Tree Connection

一个深度集成 MoveIt 2 的自定义 6-DOF 机械臂规划器。算法维护起点树与目标树，通过节点度数加权优先探索低扩展区域，结合局部高斯—镜像采样与 RRT-Connect 式贪婪连接，在汽车内部等高度受限构型空间中搜索无碰路径。

MOVEIT 2 / UR5E / CONFIGURATION SPACE

核心算法实现: PlanningContext 插件 · 双树搜索 · 启发式采样 · 连续碰撞检测 · 路径回溯

TECH STACK: ROS 2 Humble · MoveIt 2 · C++ · Pluginlib · PlanningScene · UR5e / Robotiq

代码状态

本页内容已根据上传代码核对。该课程源码快照依赖外部 ASBR 描述包，并含一处需清理的语法残留，因此当前不直接公开分发。

规划问题

在汽车驾驶舱等窄通道环境中，机械臂可行构型只占高维关节空间的一小部分。均匀采样容易反复落入碰撞区域，而单向树也可能长时间停留在局部开阔空间，因此需要同时改善探索偏置和树间连接效率。

设计目标

将自定义 PlannerManager 与 PlanningContext 注册到 MoveIt 2 规划流水线。
在 6 维关节构型空间中维护两棵树，并从起点与目标端交替扩展。
以节点度数构造采样权重，降低对已充分扩展区域的重复探索。
对每条局部边执行离散连续碰撞检测，并使用另一棵树贪婪逼近新节点。

项目结果

80%+ 成功率与 50 次以上测试来自项目演示材料；上传代码不包含批量测试脚本或原始日志。

6 DOF

UR5e 关节构型空间

交替扩展搜索树

12,000

源码最大迭代次数

80%+*

演示材料报告成功率

规划器通过 Pluginlib 导出 PlannerManager，并为 MoveIt 请求创建自定义 PlanningContext。规划场景负责机械臂与汽车模型的状态碰撞查询；成功连接后，父节点链被回溯并转换为 RobotTrajectory。

REQUEST

RViz2 / move_group

起点、目标关节状态与 PlanningScene

PLUGIN

ASBRPlannerManager

注册算法并注入规划请求与场景

ASBRContext

运行 Bi-EST 搜索、连接与路径回溯

OUTPUT

RobotTrajectory

将关节路径插值为 MoveIt 轨迹

算法不是标准 EST 或标准 RRT-Connect 的直接复刻，而是围绕窄通道搜索组合了四种机制。所有距离和步长均定义在关节空间。

度数加权采样中心

W(v_i)=\dfrac{1}{\mathrm{degree}(v_i)+1}

以轮盘赌方式选择局部采样中心。子节点越少的节点权重越高，使后续高斯样本偏向尚未充分展开的树区域；实际扩展点仍由最近邻查询确定。

Gaussian Mirror 采样

q_{\mathrm{cand}}=\mathrm{clamp}\!\big(q_{\mathrm{seed}}+\mathcal{N}(0,0.28^{2})\big)

围绕种子生成局部高斯样本；若样本碰撞，则检查关于种子的镜像点。每次最多尝试 160 轮。

随机步长局部扩展

\mathrm{STEP}\sim\mathcal{U}(0.18,\,0.35)\,\mathrm{rad}

在当前树中寻找欧氏最近节点，并沿目标样本方向走一个随机关节空间步长，避免固定步长造成单一探索尺度。

贪婪双树连接

\Delta q_{\mathrm{connect}}=0.25\,\mathrm{rad}

另一棵树从最近节点开始连续向新节点推进，直到抵达目标或局部边发生碰撞；两树连接后回溯父节点并拼接路径。

源码参数

0.12

朝另一棵树采样概率

0.28 rad

高斯采样标准差

0.18–0.35

主树随机扩展步长

0.25 rad

连接树贪婪步长

0.04 rad

单关节碰撞检查分辨率

160

局部采样最大尝试

初始化双树

分别以 qstart 和 qgoal 为根节点建立 V₁、V₂，并记录父节点与度数。

选择与采样

按 1/(degree+1) 加权选择种子；12% 概率直接偏向另一棵树，否则执行 Gaussian Mirror 局部采样。

局部扩展

寻找当前树中距离样本最近的节点，随机步进并沿插值边完成碰撞检测。

触发 Connect

另一棵树以 0.25 rad 步长持续逼近 qnew，每一段均检查碰撞。

连接或交换

若抵达则回溯并拼接两条父链；否则交换主树，在下一轮从另一端扩展。

连续碰撞检测

对局部边 qnear → qnew，算法取所有关节变化量中的最大值，并按 0.04 rad 分辨率决定插值段数。每个插值状态交由 MoveIt PlanningScene 执行机械臂—环境碰撞检测。

N_{\mathrm{steps}}=\left\lceil\dfrac{\max_{i}\lvert q_{\mathrm{new}}^{i}-q_{\mathrm{near}}^{i}\rvert}{0.04}\right\rceil

演示材料展示了墙面窄通道和汽车驾驶舱内的多组起终点姿态规划。下方附有规划器运行测试视频；批量成功率仍等待测试脚本与日志复核。

SCENE 01
汽车内饰高度约束环境
机械臂在车门、座椅和中控附近搜索关节路径，多个连杆同时受到障碍几何限制。

SCENE 03
MoveIt 2 规划界面
通过 RViz2 MotionPlanning 面板设置随机有效目标并查看规划轨迹。