QD-RRT：基于Q距离函数和快速扩展随机树的机械臂运动规划方法

吴昊、丁烨*、朱向阳
上海交通大学机械与动力工程学院
发表于《中国科学: 技术科学》2022年第52卷第5期

研究背景与目的

运动规划是机器人领域的核心问题之一，其本质是为机器人寻找一条从起始点到终止点的无碰撞路径。传统运动规划算法如路径图法、势场法和胞分解法在高维空间（如多连杆机械臂或超冗余机器人）中存在计算量大或易陷入局部最小值等问题。基于采样的运动规划算法（如RRT）因其实现简单、概率完备性等优势被广泛应用，但在复杂障碍环境或需要通过狭长通道时，传统RRT算法因随机采样的盲目性导致收敛速度显著降低。

本研究旨在改进RRT算法，通过引入目标偏置策略和基于Q距离函数（Q-distance, QD）的避障方法，减少路径扩展的随机性，提升算法在复杂环境下的效率和路径质量。

研究方法与流程

1. 算法改进设计

目标偏置策略：在随机树生长过程中，以概率p选择目标点作为采样点（而非完全随机采样），引导随机树向目标方向扩展。
Q距离函数避障：
- 碰撞检测：使用GJK算法判断机械臂连杆与环境障碍物的相交状态。
- Q距离计算：将两凸集（机械臂连杆与障碍物）的距离求解转化为线性规划问题：
- 若不相交，计算最小间隔距离（式2）；若相交，计算最大嵌入距离（式3）。
- 梯度修正：利用Q距离函数的可微性（式4-6），计算碰撞点关于关节角度的梯度，通过迭代修正（式9）调整路径节点位形，使其避开障碍物。

QD-RRT算法流程：
1. 初始化随机树，以起始点位形为根节点。
2. 以概率p选择目标点或随机点作为采样点xrand。
3. 从随机树中找到最近节点xnearest，沿xrand方向扩展新节点xnew。
4. 若路径无碰撞，将xnew加入随机树；若碰撞且xrand为目标点，则：
- 计算碰撞位形的Q距离梯度，生成修正后的新节点。
- 重复修正最多t次，若仍不可行则废弃该节点。
5. 重复迭代直至到达目标点阈值范围内。

2. 仿真实验设计

实验平台：
- 硬件：Intel Core i5-9400F CPU, 16GB内存
- 软件：MATLAB 2018a, CoppeliaSim EDU 4.1.0

对比算法：
- 基础对比：RRT、RRT*（渐近最优改进版）、P-RRT（仅含目标偏置的RRT）。
- 改进算法：QD-RRT、QD-RRT*（结合Q距离与RRT*的Rewire步骤）。

实验对象与场景：
1. 平面三连杆机构：设置4个障碍物，对比五种算法的路径长度与耗时（20次重复实验）。
2. 复杂障碍验证：增加第5个障碍物，测试RRT、P-RRT、QD-RRT的成功率与性能。
3. 超冗余机器人：6自由度平面机器人，验证算法在高维空间的适用性。
4. UR10机械臂：三维环境下的运动规划，使用长方体包络简化碰撞检测。

研究结果

1. 平面三连杆仿真

• 收敛速度：QD-RRT比RRT平均耗时减少63.43%（3.1674 s vs. 8.6615 s），QD-RRT*比RRT*减少85.58%（6.8324 s vs. 48.2470 s）。
  
• 路径质量：QD-RRT路径长度比RRT缩短37.54%（5.6179 vs. 8.9957），QD-RRT*比RRT*缩短35.35%（4.8453 vs. 7.4950）。
  
• 复杂障碍测试：QD-RRT在20次实验中均成功规划路径（平均耗时3.3597 s），而P-RRT仅成功4次（平均耗时6.3165 s）。
  

2. 超冗余机器人

QD-RRT在15/20次实验中找到路径（平均耗时10.1428 s），RRT则全部失败。

3. UR10机械臂

• 成功率：QD-RRT全部成功，P-RRT在6/20次实验中超时。
  
• 性能：QD-RRT平均耗时比P-RRT减少48.89%（15.8870 s vs. 31.0851 s），路径长度缩短8.77%。
  

结论与价值

科学意义

1. 理论创新：将Q距离函数的可微性应用于运动规划，提出梯度修正策略，为高维空间避障提供了新思路。
   
2. 算法通用性：QD模块可独立集成至RRT*等改进算法，拓展了应用场景。
   

应用价值

1. 工业机器人：在离线编程环境中提升机械臂避障效率，尤其适用于狭窄通道或复杂障碍场景。
   
2. 未来方向：结合机器视觉实现动态环境实时规划，如无人机或服务机器人。
   

研究亮点

1. 高效避障：Q距离函数通过线性规划快速计算碰撞梯度，显著减少迭代次数。
   
2. 双策略协同：目标偏置与梯度修正结合，既减少盲目搜索，又避免局部最小值。
   
3. 跨平台验证：从二维连杆到六自由度UR10，验证算法的普适性。
   

其他贡献

• 开源实现：算法基于MATLAB和CoppeliaSim，便于复现与二次开发。
  
• 实物验证：UR10实验证实仿真结果的可移植性，为工业应用提供参考。
  

（注：术语对照：RRT—快速扩展随机树；Q-distance—Q距离函数；Rewire—重布线；GJK算法—Gilbert-Johnson-Keerthi碰撞检测算法）