基于多策略融合优化改进RRT路径规划算法

魏绍亮; 王利锋; 吕冲; 贾威风

doi:10.12404/j.issn.1671-1815.2508444

科学技术与工程 ›› 2026, Vol. 26 ›› Issue (29) :12688-12697 DOI: 10.12404/j.issn.1671-1815.2508444

自动化技术、计算机技术

基于多策略融合优化改进RRT路径规划算法

作者信息 +

Improved RRT Path Planning Algorithm Based on Multi-strategy Fusion Optimization

Author information +

文章历史 +

摘要

针对传统快速扩展随机树(rapidly-exploring random tree,RRT)算法在机械臂路径规划中存在收敛效率低、路径冗余度高及适应性差等问题,提出一种多策略融合的改进RRT算法,以提升机械臂在复杂环境下运动的路径规划性能。创建六自由度机械臂模型,通过目标导向与局部障碍感知相结合的高斯混合采样机制,动态调节采样点的生成概率,增强算法在狭窄通道和障碍密集区域中的搜索能力,引入k维树(k-dimensional tree,KD-Tree)结构优化最近邻搜索效率,设计自适应步长机制提升局部通过性,并结合路径剪枝策略减少冗余节点,提升路径平滑性。最终通过在关节空间中应用五次多项式插值方法来完成轨迹的平滑生成。通过MATLAB仿真与ROS(robot operating system)实验验证结果表明,改进RRT算法相对于传统RRT算法的平均路径长度缩短了13.2%~18.3%,收敛时间减少了77.9%~93.8%。在机械臂实体实验中,该算法同样实现了平稳避障与连续轨迹规划,验证了其在真实场景中的有效性和适应性。所提出的改进型RRT算法有效提高了机械臂在路径规划中的执行速率与环境适应能力,为智能制造领域机械臂自主作业提供了切实可行的技术路径。

Abstract

To address the issues of low convergence efficiency, high path redundancy, and poor adaptability in traditional RRT algorithms for robotic arm path planning, an improved rapidly-exploring random tree(RRT) algorithm with multi-strategy fusion was proposed to enhance the path planning performance of robotic arms in complex environments. A six-degree-of-freedom robotic arm model was created. By combining goal-oriented and local obstacle-aware Gaussian mixture sampling mechanisms, the generation probability of sampling points was dynamically adjusted to enhance the algorithm's search ability in narrow channels and obstacle-dense areas. The k-dimensional tree(KD-Tree) structure was introduced to optimize the nearest neighbor search efficiency, an adaptive step size mechanism was designed to improve local passability, and a path pruning strategy was combined to reduce redundant nodes and enhance path smoothness. The trajectory was ultimately smoothed and generated by applying quintic polynomial interpolation in the joint space. The results verified by MATLAB simulation and robot operating system (ROS) experiments show that the average path length of the improved RRT algorithm is shortened by 13.2% to 18.3% compared with the traditional RRT algorithm, and the convergence time is reduced by 77.9% to 93.8%. In the physical experiment of the robotic arm, this algorithm also achieved smooth obstacle avoidance and continuous trajectory planning, verifying its effectiveness and adaptability in real scenarios. The improved RRT algorithm proposed effectively enhances the execution rate and environmental adaptability of robotic arms in path planning, providing a practical and feasible technical approach for the autonomous operation of robotic arms in the field of intelligent manufacturing.

Graphical abstract

关键词

机械臂 / 路径规划 / 改进RRT算法 / 混合采样 / 轨迹规划

Key words

mechanical arm / path planning / improved RRT algorithm / hybrid sampling / trajectory planning

引用本文

引用格式 ▾

魏绍亮, 王利锋, 吕冲, 贾威风. 基于多策略融合优化改进RRT路径规划算法[J]. 科学技术与工程, 2026, 26(29): 12688-12697 DOI:10.12404/j.issn.1671-1815.2508444

WEI Shao-liang, WANG Li-feng, LÜ Chong, JIA Wei-feng. Improved RRT Path Planning Algorithm Based on Multi-strategy Fusion Optimization[J]. Science Technology and Engineering, 2026, 26(29): 12688-12697 DOI:10.12404/j.issn.1671-1815.2508444

登录浏览全文

4963

注册一个新账户忘记密码

近年来,智能制造技术作为工业4.0的核心驱动力^[1],对工业机器人提出了更高要求:一方面需处理复杂任务,另一方面需提升运动控制的灵敏性与精确性。为满足这些需求,机械臂路径规划技术的研究成为关键。当前,中国智能制造加速转型,机器人自主规划能力直接决定产线效率。尤其在航空航天、汽车制造等高端领域,机械臂常在狭窄多障的复杂工位执行高精度作业,对规划算法的实时性、适应性与平滑性提出严峻挑战。

然而,在当前主流方法中,图搜索法(如A*^[2]和Dijkstra^[3]算法)虽能保证路径搜索的完备性和最优性,却难以适应高速运动场景;人工势场法^[4-5]凭借其计算简便、易于控制集成的特点获得应用,但存在高维空间控制能力不足的固有缺陷;相比之下,基于概率采样的快速扩展随机树(rapidly-exploring random tree,RRT)算法^[6-7]与概率路线图(probabilistic roadmap,PRM)算法^[8]因其在多维复杂环境中的适应性而更具优势,其中RRT算法由于更符合机械臂运动学特性,在实际工程中应用最为广泛。

尽管RRT算法在多维空间路径规划中优势显著,但其固有的路径冗余度高、收敛效率低^[9-10]等缺陷限制了其在复杂作业场景中的应用。为此,众多研究者致力于该算法的优化与改进。李文君等^[11]在改进RRT*算法中引入了目标偏置机制,从而有效降低了随机树扩展过程中的随机性与盲目性;康博涵等^[12]设计了一种基于RRT框架的自适应步长路径规划方法。该方法通过建立步长与路径规划性能指标之间的关联模型,借助遗传算法优化环境复杂度下的步长选择,从而提高了节点利用效率。然而,由于遗传算法固有的计算复杂性和较长耗时,该方法的整体运行效率仍受到一定制约;符强等^[13]采用双向目标RRT融合Dijkstra算法提升了路径的目标性与平滑度,但该方法在高维或动态环境中计算负担较大,实时性受限;Huang等^[14]提出的BMGA-RRT Connect算法,通过自适应步长与层级碰撞检测提升了采摘路径规划效率,但在复杂多变的真实果园环境中,算法的普适性与鲁棒性仍需进一步提高;Zhao等^[15]提出的GD-RRT*算法通过高斯混合采样与目标偏置策略的动态调整,有效降低了路径成本并显著提升了采摘机械臂的规划效率与避障成功率;Yuan等^[16]提出一种结合目标偏置与自适应步长的改进RRT算法,提升了移动机器人路径规划的效率和路径质量。但该方法在高度复杂环境中,其自适应参数与重力系数的调节策略仍显不足,导致规划稳定性存在局限。

基于以上研究现状,现提出一种多策略融合优化的改进型RRT算法。该算法通过目标导向与局部障碍感知相结合的高斯混合采样机制,动态调节采样点的生成概率,增强算法在狭窄通道和障碍密集区域中的搜索能力;引入KD-Tree结构进行高效最近邻搜索,减少冗余计算;结合自适应步长机制,根据局部环境动态调整扩展距离,提升在狭窄空间中的通过性;同时采用路径剪枝策略,有效消除冗余节点,优化路径平滑性。最终,基于仿真实验与物理平台实测的双重验证实验验证所提方法在路径规划任务中的工程可行性与收敛有效性。

1 机械臂运动学参数及碰撞模型

1.1 机械臂DH参数

采用自研六轴机械臂为研究对象,根据其结构建立如图1所示坐标系,机械臂相关D-H参数如表1所示。

机械臂相邻关节连杆坐标系x_n-₁y_n-₁z_n-₁与坐标系x_ny_nz_n之间的坐标系变换可以通过式(1)变换矩阵通式来表示。

(1)${}_{\mathit{　}\mathit{i}}^{\mathit{i}-1}$T=$\left[\begin{array}{llll}\mathrm{c}{\mathit{\theta }}_{\mathit{i}}& -\mathrm{s}{\mathit{\theta }}_{\mathit{i}}& 0& {\mathit{a}}_{\mathit{i}-1}\\ \mathrm{s}{\mathit{\theta }}_{\mathit{i}}\mathrm{c}{\mathit{\alpha }}_{\mathit{i}-1}& \mathrm{c}{\mathit{\theta }}_{\mathit{i}}\mathrm{c}{\mathit{\alpha }}_{\mathit{i}-1}& -\mathrm{s}{\mathit{\alpha }}_{\mathit{i}-1}& -{\mathit{d}}_{\mathit{i}}\mathrm{s}{\mathit{\alpha }}_{\mathit{i}-1}\\ \mathrm{s}{\mathit{\theta }}_{\mathit{i}}\mathrm{s}{\mathit{\alpha }}_{\mathit{i}-1}& \mathrm{c}{\mathit{\theta }}_{\mathit{i}}\mathrm{s}{\mathit{\alpha }}_{\mathit{i}-1}& \mathrm{c}{\mathit{\alpha }}_{\mathit{i}-1}& {\mathit{d}}_{\mathit{i}}\mathrm{c}{\mathit{\alpha }}_{\mathit{i}-1}\\ 0& 0& 0& 1\end{array}\right]$

式(1)中:c表示cos运算;s表示sin运算。

由式(1)可知,相对于基坐标系的机械臂末端坐标系齐次变换矩阵为

(2)${}_{6}^{0}$T${=}_{1}^{0}{\mathit{T}}_{2}^{1}{\mathit{T}}_{3}^{2}{\mathit{T}}_{4}^{3}{\mathit{T}}_{5}^{4}{\mathit{T}}_{6}^{5}$T=$\left[\begin{array}{llll}{\mathit{n}}_{\mathit{x}}& {\mathit{o}}_{\mathit{x}}& {\mathit{a}}_{\mathit{x}}& {\mathit{P}}_{\mathit{x}}\\ {\mathit{n}}_{\mathit{y}}& {\mathit{o}}_{\mathit{y}}& {\mathit{a}}_{\mathit{y}}& {\mathit{P}}_{\mathit{y}}\\ {\mathit{n}}_{\mathit{z}}& {\mathit{o}}_{\mathit{z}}& {\mathit{a}}_{\mathit{z}}& {\mathit{P}}_{\mathit{z}}\\ 0& 0& 0& 1\end{array}\right]$

式(2)中:

(3)$\left\{\begin{array}{l}\begin{array}{l}{\mathit{n}}_{\mathit{x}}={\mathrm{c}}_{6}\left[\right({\mathrm{c}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}-{\mathrm{s}}_{1}{\mathrm{s}}_{4}){\mathrm{c}}_{5}-{\mathrm{c}}_{1}{\mathrm{s}}_{23}{\mathrm{s}}_{5}]-\\ \mathit{　}\mathit{　}{\mathrm{s}}_{6}({\mathrm{c}}_{1}{\mathrm{s}}_{23}{\mathrm{s}}_{4}+{\mathrm{s}}_{1}{\mathrm{c}}_{4})\\ {\mathit{n}}_{\mathit{y}}={\mathrm{c}}_{6}\left[\right({\mathrm{s}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}+{\mathrm{c}}_{1}{\mathrm{s}}_{4}){\mathrm{c}}_{5}-{\mathrm{s}}_{1}{\mathrm{s}}_{23}{\mathrm{s}}_{5}]-\\ \mathit{　}\mathit{　}{\mathrm{s}}_{6}({\mathrm{s}}_{1}{\mathrm{c}}_{23}{\mathrm{s}}_{4}-{\mathrm{c}}_{1}{\mathrm{c}}_{4})\\ {\mathit{n}}_{\mathit{z}}=-{\mathrm{c}}_{6}({\mathrm{s}}_{23}{\mathrm{c}}_{4}{\mathrm{c}}_{5}+{\mathrm{c}}_{23}{\mathrm{s}}_{5})+{\mathrm{s}}_{23}{\mathrm{s}}_{4}{\mathrm{s}}_{6}\\ {\mathit{o}}_{\mathit{x}}=-{\mathrm{s}}_{6}\left[\right({\mathrm{c}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}-{\mathrm{s}}_{1}{\mathrm{s}}_{4}){\mathrm{c}}_{5}-{\mathrm{c}}_{1}{\mathrm{s}}_{23}{\mathrm{s}}_{5}]-\\ \mathit{　}\mathit{　}{\mathrm{c}}_{6}({\mathrm{c}}_{1}{\mathrm{c}}_{23}{\mathrm{s}}_{4}+{\mathrm{s}}_{1}{\mathrm{c}}_{4})\\ {\mathit{o}}_{\mathit{y}}=-{\mathrm{s}}_{6}\left[\right({\mathrm{s}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}+{\mathrm{c}}_{1}{\mathrm{s}}_{4}){\mathrm{c}}_{5}-{\mathrm{s}}_{1}{\mathrm{s}}_{23}{\mathrm{s}}_{5}]-\\ \mathit{　}\mathit{　}{\mathrm{s}}_{6}({\mathrm{s}}_{1}{\mathrm{c}}_{23}{\mathrm{s}}_{4}-{\mathrm{c}}_{1}{\mathrm{c}}_{4})\\ {\mathit{o}}_{\mathit{z}}={\mathrm{s}}_{6}({\mathrm{s}}_{23}{\mathrm{c}}_{4}{\mathrm{c}}_{5}+{\mathrm{c}}_{23}{\mathrm{s}}_{5})+{\mathrm{s}}_{23}{\mathrm{s}}_{4}{\mathrm{s}}_{6}\\ {\mathit{a}}_{\mathit{x}}={\mathrm{s}}_{5}({\mathrm{c}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}-{\mathrm{s}}_{1}{\mathrm{s}}_{4})+{\mathrm{c}}_{1}{\mathrm{s}}_{23}{\mathrm{c}}_{5}\\ {\mathit{a}}_{\mathit{y}}={\mathrm{s}}_{5}({\mathrm{s}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}+{\mathrm{c}}_{1}{\mathrm{s}}_{4})+{\mathrm{s}}_{1}{\mathrm{s}}_{23}{\mathrm{c}}_{5}\\ {\mathit{a}}_{\mathit{z}}=-{\mathrm{s}}_{23}{\mathrm{c}}_{4}{\mathrm{s}}_{5}+{\mathrm{c}}_{23}{\mathrm{c}}_{5}\\ {\mathit{P}}_{\mathit{x}}={\mathit{d}}_{6}\left[\right({\mathrm{c}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}-{\mathrm{s}}_{1}{\mathrm{s}}_{4}){\mathrm{s}}_{5}+{\mathrm{c}}_{1}{\mathrm{s}}_{23}{\mathrm{c}}_{5}]+\\ \mathit{　}\mathit{　}{\mathit{d}}_{4}{\mathrm{c}}_{1}{\mathrm{s}}_{23}+{\mathit{a}}_{2}{\mathrm{c}}_{1}{\mathrm{c}}_{2}\\ {\mathit{P}}_{\mathit{y}}={\mathit{d}}_{6}\left[\right({\mathrm{s}}_{1}{\mathrm{c}}_{23}{\mathrm{c}}_{4}+{\mathrm{c}}_{1}{\mathrm{s}}_{4}){\mathrm{s}}_{5}+{\mathrm{s}}_{1}{\mathrm{s}}_{23}{\mathrm{c}}_{5}]+\\ \mathit{　}\mathit{　}{\mathit{d}}_{4}{\mathrm{s}}_{1}{\mathrm{s}}_{23}+{\mathit{a}}_{2}{\mathrm{s}}_{1}{\mathrm{c}}_{2}\\ {\mathit{P}}_{\mathit{z}}=-{\mathit{d}}_{6}({\mathrm{s}}_{23}{\mathrm{c}}_{4}{\mathrm{s}}_{5}-{\mathrm{s}}_{23}{\mathrm{c}}_{5})+{\mathit{d}}_{4}{\mathrm{c}}_{23}-{\mathit{a}}_{2}{\mathrm{s}}_{2}+{\mathit{d}}_{1}\\ {\mathrm{c}}_{1}=\mathrm{c}\mathrm{o}\mathrm{s}{\mathit{\theta }}_{1},{\mathrm{s}}_{1}=\mathrm{s}\mathrm{i}\mathrm{n}{\mathit{\theta }}_{1}\end{array}\\ {\mathrm{c}}_{23}=\mathrm{c}\mathrm{o}\mathrm{s}({\mathit{\theta }}_{1}+{\mathit{\theta }}_{2}),{\mathrm{s}}_{23}=\mathrm{s}\mathrm{i}\mathrm{n}({\mathit{\theta }}_{1}+{\mathit{\theta }}_{2})\end{array}\right.$

其他运算形式同理。

1.2 碰撞检测原理

为防止机械臂在运动过程中与作业空间内的障碍物产生干涉,需对其各连杆模型及环境障碍物进行碰撞检测处理。由于机械臂和障碍物的形状不规则,进行检测时会耗费大量的时间,故采用几何包络法^[17]对机械臂进行模型简化。采用圆柱体对机械臂连杆进行几何建模,并将不规则障碍物以球体与长方体进行包络近似。基于上述建模方式,机械臂与环境障碍物的碰撞检测可转化为圆柱体与球体、圆柱体与长方体之间的几何干涉分析。其中,球形障碍物与机械臂连杆的碰撞检测简化模型如图2所示。

基于齐次变换矩阵,可将球形障碍物的球心坐标由世界坐标系依次转换至各连杆的关节坐标系中。可将碰撞问题转化为空间点到线段的最小距离判定,只需计算P到AB的垂距d,即可快速完成碰撞检测。

无论P在何处,都有

(4)l_AC=$\frac{{\mathit{l}}_{\mathrm{A}\mathrm{P}}{\mathit{l}}_{\mathrm{A}\mathrm{B}}}{{\left|{\mathit{l}}_{\mathrm{A}\mathrm{B}}\right|}^{2}}$l_AB

式(4)中:l_AB为A到B的位移向量;l_AC为A到C的位移向量;l_AP为A到P的位移向量。

令

(5)c=$\frac{{\mathit{l}}_{\mathrm{A}\mathrm{P}}{\mathit{l}}_{\mathrm{A}\mathrm{B}}}{{\left|{\mathit{l}}_{\mathrm{A}\mathrm{B}}\right|}^{2}}$

根据向量方向性,可将点P在线段AB上的投影位置参数化为标量c:当投影落在A点外侧时,c<0;落在B点外侧时,c>1;若投影严格位于线段内部,则0<c<1。

点到该线段的最短距离相应分为3种情形,即

(6)l=$\left\{\begin{array}{ll}\left|{\mathit{l}}_{\mathrm{A}\mathrm{P}}\right|,& \mathit{c}<0\\ \left|{\mathit{l}}_{\mathrm{C}\mathrm{P}}\right|,& 0\le \mathit{c}\le 1\\ \left|{\mathit{l}}_{\mathrm{B}\mathrm{P}}\right|,& \mathit{c}>1\end{array}\right.$

式(6)中:l_CP为C到P的位移向量;l_BP为B到P的位移向量。

通过比较两包围体半径之和与线段l长度,即可判定碰撞是否发生。

对于长方体障碍物,采用如图3所示的轴对齐包围盒(AABB)方法进行包络建模。设该包围盒在各坐标轴方向的最小坐标为(x_min,y_min,z_min),其沿坐标轴方向的长度、宽度与高度分别为l、w、h,机械臂连杆的半径为r。为进行碰撞检测,将长方体的长、宽、高各方向分别扩展2r,形成放大后的包围盒。机械臂连杆被简化为一条轴线,通过判断该轴线是否与扩展后的包围盒相交,来判定是否发生碰撞。

为进行碰撞检测,首先将长方体障碍物在各轴正向延伸其对应尺寸,并沿每个方向外扩半径r,得到扩展后的最大坐标为

(7)$\left\{\begin{array}{l}{\mathit{x}}_{\mathrm{m}\mathrm{a}\mathrm{x}}={\mathit{x}}_{\mathrm{m}\mathrm{i}\mathrm{n}}+\mathit{l}+\mathit{r}\\ {\mathit{y}}_{\mathrm{m}\mathrm{a}\mathrm{x}}={\mathit{y}}_{\mathrm{m}\mathrm{i}\mathrm{n}}+\mathit{w}+\mathit{r}\\ {\mathit{z}}_{\mathrm{m}\mathrm{a}\mathrm{x}}={\mathit{z}}_{\mathrm{m}\mathrm{i}\mathrm{n}}+\mathit{h}+\mathit{r}\end{array}\right.$

碰撞判定是将机械臂连杆中心线离散化为若干空间点,若所有点均不满足以下条件。

(8)$\left\{\begin{array}{l}{\mathit{x}}_{\mathrm{m}\mathrm{i}\mathrm{n}}-\mathit{r}<\mathit{x}<{\mathit{x}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ {\mathit{y}}_{\mathrm{m}\mathrm{i}\mathrm{n}}-\mathit{r}<\mathit{y}<{\mathit{y}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ {\mathit{z}}_{\mathrm{m}\mathrm{i}\mathrm{n}}-\mathit{r}<\mathit{z}<{\mathit{z}}_{\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.$

则判定机械臂与障碍物未发生碰撞;反之,若存在任一点满足上述条件,则判定发生碰撞。

2 改进RRT路径规划算法

2.1 RRT算法原理

如图4所示,RRT算法以初始位置为根节点构建随机扩展树。该算法通过在构型空间中随机采样,并选取当前树中与采样点欧氏距离最近且满足无碰撞连接条件的节点,将二者连接后把该采样点作为新节点加入扩展树。此过程迭代执行,直至扩展树覆盖目标区域邻近空间^[18]。RRT算法的详细执行流程如下。

步骤1 以起始位置作为随机树的根节点x_init并进行初始化。

步骤2 在状态空间中随机采样一个目标点x_rand。

步骤3 在当前随机树的所有节点中搜索与x_rand欧氏距离最近的节点x_near。

步骤4 沿x_near至x_rand方向扩展一个固定步长ε_q,生成新节点x_new。

步骤5 检测x_new是否与环境中障碍物发生碰撞,若未发生碰撞,则将x_new加入随机树,否则,放弃该节点;重复执行步骤2~步骤5,直至扩展至目标位置x_goal附近。

2.2 混合采样策略

在传统的RRT方法中,采样点通常是随机生成的,这样可以较全面地反映工作空间的总体信息,但在狭窄通道或障碍物稠密区域,随机采样方法生成的有效节点数量显著降低,难以对通道几何特征进行充分表征,导致路径搜索效率下降,并引入大量无效扩展节点。为降低RRT算法在路径探索中的随机性并加速其收敛进程,在传统随机采样机制基础上,引入了一种基于碰撞节点的高斯采样混合策略。

当快速扩展随机树中节点x_new扩展失败时,可判定其父节点x_near附近存在障碍物。此时,以x_near为中心,基于协方差矩阵Σ构建多元高斯分布N(x_near,Σ),并通过该分布生成若干候选采样点。该方法能有效提升障碍物边界及狭窄通道区域的采样密度,从而改善路径探索效率。多元高斯概率密度可表示为

(9)f_μ_,_Σ(X)=$\frac{\mathrm{e}\mathrm{x}\mathrm{p}\left[-\frac{1}{2}{(\mathit{X}-\mathit{\mu })}^{\mathrm{{\rm T}}}{\mathit{\Sigma }}^{-1}(\mathit{X}-\mathit{\mu })\right]}{{\left(2\mathrm{\pi }\right)}^{\frac{\mathit{n}}{2}}{\left|\mathit{\Sigma }\right|}^{\frac{1}{2}}}\mathit{ }$

式(9)中:X为一个n维向量;μ为一个n维向量,表示多元高斯分布的均值向量;Σ为一个n×n的协方差矩阵,用来表示各维度之间的协方差关系;$\left|\mathit{\Sigma }\right|$为协方差矩阵的行列式。

基于式(9)定义多元高斯分布函数为

(10)φ(X,μ,Σ)=∫f_μ_,_Σ(X)dX

为量化采样点是否值得继续尝试采样,定义距离-目标折中权重函数为

(11)w=α$\left(1-\frac{{\mathit{d}}_{\mathrm{o}\mathrm{b}\mathrm{s}}}{{\mathit{d}}_{\mathrm{m}\mathrm{a}\mathrm{x}}}\right)$+(1-α)$\left(1-\frac{{\mathit{d}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}}{{\mathit{D}}_{\mathrm{m}\mathrm{a}\mathrm{x}}}\right)$

式(11)中:d_obs为采样点X到最近障碍的距离;d_goal为X到目标点的距离;d_max、D_max为场景归一化系数;α∈[0,1],可随迭代次数线性下降。

结合式(9)~式(11),定义基于距离-目标折中加权的概率分布函数为

(12)g(X,x_near,Σ)=wφ(X,μ,Σ)

基于式(12)进行阈值判断,算法仅在近期发生碰撞失效的局部区域内激活高斯混合采样:若权重评估表明该分布无法产出兼具安全性与目标趋向性的合格候选,则即刻终止高斯流程并回退至构型空间均匀随机采样;反之,则保留权重最优且距目标最近的候选点加入随机树。

2.3 KD-Tree近邻搜索优化

在RRT算法中,随着树中节点数量的不断增加,若每次最近邻搜索都对所有节点逐一计算距离,将导致效率显著下降,计算时间和内存开销急剧上升。KD-Tree能够根据节点在不同维度上的坐标值进行划分,快速排除大量不可能的候选节点,仅在可能的范围内搜索,从而显著提高最近邻查询的效率。RRT算法中,寻找距离随机采样点x_rand最近的树中节点v可表示为

(13)x_near(V,x_rand)=argminKDTree(v,x_rand)

式(13)中:V为当前树中的所有节点组合;x_rand为在自由空间中随机采样得到的点;v为当前树中的某一个节点。

2.4 自适应步长控制策略

传统RRT算法中,在每次迭代时,以恒定的步长沿着采样点的方向进行扩展,然而,这种方式在不同环境条件下存在一定的局限性。在开阔环境中,固定步长可能导致探索效率降低,而在复杂或狭窄环境中,过大的步长容易导致采样点越过可行解,增加无效扩展的概率。因此,引入一种自适应步长策略,如图5所示,设置一个步长取值区间[s_min,s_max],步长具体计算过程如下。

在随机采样过程中,以最近节点x_near为中心,R为半径,计算该范围内障碍像素占比为

(14)ρ=$\frac{\mathit{N}\_\mathrm{o}\mathrm{b}\mathrm{s}}{\mathit{N}\_\mathrm{t}\mathrm{a}\mathrm{l}}$,$\begin{array}{ll}& \end{array}$ρ∈[0,1)

式(14)中:ρ为统计范围内障碍物像素占总像素的比值;N_obs为统计范围内障碍物像素点个数;N_tal为统计范围内总像素点个数。

设计自适应步长调整公式为

(15)S=s_min+(1-ρ)(s_max-s_min)

式(15)中:S为调整后的步长;s_min为最小步长;s_max为最大步长。

2.5 贪心策略剪枝处理

在三维空间中,RRT算法生成的路径由一系列离散节点构成,呈连续折线状。该路径通常存在曲折程度较高、平滑性较差的问题,包含大量冗余节点,不利于机械臂的平稳运行。设RRT算法生成的路径节点序列为向量x=[x₁,x₂,…,x_n],对节点x_a和x_b之间路径进行碰撞检测a<b,其中a,b∈[1,2,…,n],若不发生碰撞,则删除x_a和x_b之间的节点,从x₁开始遍历所有节点序列,得到简化后的路径,如图6绿色线条所示。

综合上述各模块的优化策略,为更清晰地展示算法流程,现将改进后的RRT算法流程总结如图7所示,其核心伪代码如下。

3 机械臂轨迹规划

改进后的算法存在“急转弯”的问题,这势必会导致机械臂在运动过程中产生振动和冲击,影响机械臂的使用寿命和安全性,因此应对其进行轨迹规划。轨迹规划通常可分为关节空间规划与笛卡尔空间规划两种实现方式。笛卡尔空间方法难以完全规避运动学奇异点问题,而关节空间中常用的三次多项式轨迹规划则存在加速度突变现象,影响系统运动的平稳性。为此,选用五次多项式插值方法对机械臂进行轨迹规划。

所用五次多项式插值的表达式为

(16)θ_i(t)=a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

对式(16)求导可得各关节角速度和角加速度方程为

(17)$\left\{\begin{array}{l}{\stackrel{·}{\mathit{\theta }}}_{\mathit{i}}\left(\mathit{t}\right)={\mathit{a}}_{1}+2{\mathit{a}}_{2}\mathit{t}+3{\mathit{a}}_{3}{\mathit{t}}^{2}+4{\mathit{a}}_{4}{\mathit{t}}^{3}+5{\mathit{a}}_{5}{\mathit{t}}^{4}\\ {\stackrel{··}{\mathit{\theta }}}_{\mathit{i}}\left(\mathit{t}\right)=2{\mathit{a}}_{2}+6{\mathit{a}}_{3}\mathit{t}+12{\mathit{a}}_{4}{\mathit{t}}^{2}+20{\mathit{a}}_{5}{\mathit{t}}^{3}\end{array}\right.$

五次多项式插值算法一共存在6个未知数,对于五次多项式,条件限定需要6个已知的条件求解a₀、a₁、a₂、a₃、a₄、a₅的表达式,给定t_n和t_n+₁时刻的角度θ_n和θ_n+1以及两点之间的速度v_n和v_n+₁以及两点之间的加速度a_n和a_n+₁,将该参数添加到式(17)中求解可得

(18)$\left\{\begin{array}{l}{\mathit{a}}_{0}={\mathit{\theta }}_{\mathit{n}}\\ {\mathit{a}}_{1}={\mathit{v}}_{\mathit{n}}\\ {\mathit{a}}_{2}=\frac{1}{2}{\mathit{a}}_{0}\\ {\mathit{a}}_{3}=\frac{20{\mathit{\theta }}_{\mathit{n}+1}-20{\mathit{\theta }}_{\mathit{n}}-(8{\mathit{v}}_{\mathit{n}+1}+12{\mathit{v}}_{\mathit{n}}){\mathit{t}}_{\mathit{n}+1}-(3{\mathit{a}}_{\mathit{n}}-{\mathit{a}}_{\mathit{n}+1}){\mathit{t}}_{\mathit{n}+1}^{2}}{2{\mathit{t}}_{\mathit{n}+1}^{3}}\\ {\mathit{a}}_{4}=\frac{30{\mathit{\theta }}_{\mathit{n}}-30{\mathit{\theta }}_{\mathit{n}+1}+(14{\mathit{v}}_{\mathit{n}+1}+16{\mathit{v}}_{\mathit{n}}){\mathit{t}}_{\mathit{n}+1}+(3{\mathit{a}}_{\mathit{n}}-2{\mathit{a}}_{\mathit{n}+1}){\mathit{t}}_{\mathit{n}+1}^{2}}{2{\mathit{t}}_{\mathit{n}+1}^{4}}\\ {\mathit{a}}_{5}=\frac{12{\mathit{\theta }}_{\mathit{n}+1}-12{\mathit{\theta }}_{\mathit{n}}-(6{\mathit{v}}_{\mathit{n}}+6{\mathit{v}}_{\mathit{n}+1}){\mathit{t}}_{\mathit{n}+1}-({\mathit{a}}_{0}-{\mathit{a}}_{\mathit{n}+1}){\mathit{t}}_{\mathit{n}+1}^{2}}{2{\mathit{t}}_{\mathit{n}+1}^{5}}\end{array}\right.$

五次多项式曲线的位置、速度和加速度曲线如图8所示,曲线展示了机械臂关节角度随时间变化的平滑过渡,有助于减少运动过程中的振动和冲击。

4 仿真与实验

为评估改进后RRT算法的性能表现,基于MATLAB仿真平台开展对比实验,在不同场景下将该算法与经典RRT、RRT-Connect及RRT*算法进行定量分析与数据比较,并将该算法部署到ROS(robot operating system)环境下控制实体机械臂进行避障实验。

4.1 二维场景对比实验

设置500×500像素的二维栅格地图作为测试场景1。起点坐标为[20,20],目标点坐标为[480,480],其中,传统RRT、RRT-Connect与RRT*算法均采用固定步长,设置为10像素;而改进RRT算法采用自适应步长策略,其最大步长设为15像素,最小步长设为5像素。分别使用RRT、RRT-Connect、RRT*算法和改进的RRT算法搜索无碰撞路径,规划结果如图9所示。通过对各算法执行100组独立重复实验,所得性能测试数据汇总如表2所示。

由图9可看出,改进后的RRT算法减少了采样点的随机性和搜索时间;缩短了路径的长度;减少了路径中“急转弯”次数;从而显著提升了路径规划的整体效率。对比表2中的数据可得,改进RRT算法在平均路径长度上表现最佳,显著优于RRT和RRT-Connect;在平均采样次数和运行时间上,改进RRT算法显著低于RRT算法和RRT*算法,显示出更高的效率,虽然平均运行时间与RRT-Connect相近,但综合考虑路径优化和采样效率方面展现出了明显优势,特别是与原始RRT算法的对比:在路径长度方面,相较于原始RRT算法减少了13.2%;在运行时间方面,更是实现了93.8%的降低。这些结果充分证明了改进RRT算法在效率和性能方面的显著提升。

4.2 狭窄通道仿真对比实验

相比于之前场景,场景2在起点和终点之间设置了狭窄通道,目的是检验改进算法在受限空间中的路径规划能力。分别使用RRT、RRT-Connect、RRT*算法和改进的RRT算法搜索无碰撞路径,规划结果如图10所示。通过对各算法执行100组独立重复实验,所得性能测试数据汇总如表3所示。

由图10可看出,改进后的RRT算法在狭窄环境中展现出了优异的搜索能力,并且显示出良好的环境适应性。对比表3测试结果可得,改进RRT算法在路径长度、采样次数和运行时间方面均实现了优化。特别是在与原始RRT算法的对比中,改进RRT算法的性能提升尤为显著:在路径长度方面,相较于原始RRT算法减少了18.3%;在运行时间方面,更是实现了81.4%的降低。这些结果充分证明了改进RRT算法在效率和性能方面的显著提升。

4.3 大规模及高密度障碍对比实验

为进一步验证改进算法在大规模及高密度障碍环境下的计算性能与可扩展性,构建了规模为1 000×1 000像素的场景3作为测试环境。该场景在起点(50,50)与目标点(950,950)之间设置了多组密集障碍区域与连续狭窄通道,旨在模拟真实工业场景中机械臂所需面对的复杂结构化环境。同样分别采用RRT、RRT-Connect、RRT*算法及改进RRT算法进行无碰撞路径搜索,其规划结果如图11所示。通过对各算法执行100组独立重复实验,所得性能对比数据汇总如表4所示。

由图11可看出,改进后的RRT算法在大规模及高密度障碍环境中仍然展现出了优异的搜索能力,并且显示出良好的环境适应性。对比表4测试结果可得,改进RRT算法在路径长度、采样次数和运行时间方面均实现了优化。特别是在与原始RRT算法的对比中,改进RRT算法的性能提升尤为显著:在路径长度方面,相较于原始RRT算法减少了14%;在运行时间方面,更是实现了77.9%的降低。这些结果充分证明了改进RRT算法在效率和性能方面的显著提升。

通过上述3组仿真结果可以看出,RRT算法的随机性较大,导致路径冗长和时间较长,存在较多的转折点;RRT-Connect在RRT算法的基础上采用双向扩展的方式进行路径搜索,故其路径搜索时间较短;RRT*算法在RRT的基础上增加了路径点优化策略,在每次扩展新节点时,会尝试连接到附近所有可能的父节点,并选择代价最小的路径作为最终路径,因此生成的路径更加平滑且接近最优路径。改进后的RRT算法结合了多种策略的优势:引入混合采样策略,使随机树以更高概率向目标方向进行扩展;在进行最近点计算时,采用KD-TREE快速排除大量不可能的候选节点;采用自适应步长的搜索方式使其在开阔环境中以较大步长提升搜索速度,在复杂或狭窄环境中,减小步长提高有效扩展概率;采用路径优化策略减少了路径长度和转折点个数,减少了机械臂在运动过程中的时间和冲击,从而显著提升了路径规划的整体效率。

4.4 机械臂实体实验

在Linux平台上搭建了机械臂仿真环境,基于ROS中的MoveIt框架完成机械臂运动规划与避障控制,并将本文算法在该平台上进行集成与验证。机械臂实验平台如图12所示。

以抓取绿色正方体为目标,并将其放置到盒子中,在机械臂抓取与放置的最佳路径中加入两个圆锥桶作为障碍物,然后将障碍物发布到环境中,通过深度相机识别绿色正方体和盒子位置,控制机械臂夹取绿色正方体放置到盒子中,其避障过程如图13所示。整个过程中,机械臂未与障碍物发生碰撞且运行平稳。

通过在终端中执行rqt_plot命令,并订阅机械臂6个关节角位置话题,可以得到6个关节角随时间变化的曲线,如图14所示。通过该曲线可以看出,关节角随时间变化平稳连续,验证了运动规划结果的可行性。

5 结论

针对六自由度机械臂在复杂环境下的路径规划问题,提出了一种基于多策略融合优化的改进RRT算法。主要工作如下。

(1)提出了多策略融合的路径规划算法。算法引入了目标导向与局部障碍感知相结合的高斯混合采样机制,动态调节采样概率,增强在狭窄通道和障碍密集区域的搜索能力;同时,集成KD-Tree结构优化最近邻搜索效率,并设计自适应步长机制,根据局部环境复杂度动态调整扩展步长,提升了算法的整体收敛速度和复杂场景通过性。最后,结合路径剪枝策略与五次多项式插值,在减少冗余节点的同时,实现了机械臂关节空间的平滑轨迹生成。

(2)设计了仿真与实物对比实验。在多种二维复杂场景下进行了算法性能测试,并在ROS环境中基于实体六自由度机械臂完成了实物避障与抓取实验。结果表明,本文算法在路径质量和规划效率上均显著优于传统RRT算法:平均路径长度缩短了13.2%~18.3%,收敛时间减少了77.9%~93.8%。实物实验验证了算法在真实场景中生成平滑、无碰撞轨迹的有效性与适应性。

改进RRT算法通过融合多种优化策略,显著提升了机械臂在复杂环境下的路径规划性能,有效提升了中国工业机器人在复杂场景下的自主作业能力,为智能制造领域的技术升级提供了切实可行的解决方案。后续研究将围绕动态环境下的实时重规划与多机协同控制等方向展开,以进一步提升系统的实用性与鲁棒性。

参考文献

原文顺序 | 出版日期 | 本文引用

[1]	Ji Y, Zhou C, Lin R, et al. Path planning strategy for a space relocatable robotic manipulator based on improved GBNN algorithm[J]. Robotics and Autonomous Systems, 2025, 187: 104939.

[2]	张伟民, 徐森生, 张月. 基于改进A*算法的室内巡检机器人路径规划研究[J]. 机械工程学报, 2024, 60(20): 315-326.

[3]	Zhang Weimin, Xu Sensheng, Zhang Yue. Research on path planning of indoor inspection robot based on improved A* algorithm[J]. Journal of Mechanical Engineering, 2024, 60(20): 315-326.

[4]	Kheildar F, Samouei P, Mansoorizadeh M. Reinforcement learning-dijkstra-genetic algorithm for debris removal problem under different scenarios: an earthquake case study[J]. Engineering Applications of Artificial Intelligence, 2025, 151: 110693.

[5]	袁朝春, 翁烁丰, 何友国, 等. 基于改进人工势场法的路径规划决策一体化算法研究[J]. 农业机械学报, 2019, 50(9): 394-403.

[6]	Yuan Chaochun, Weng Shuofeng, He Youguo, et al. Integration algorithm of path planning and decision-making based on improved artificial potential field[J]. Transactions of the Chinese Society for Agricultural Machinery, 2019, 50(9): 394-403.

[7]	Zhang Z, Qin J, Tan S, et al. Lane change trajectory planning based on improved model predictive control with artificial potential field for autonomous vehicles in medium-high speed scenarios[J]. Engineering Applications of Artificial Intelligence, 2025, 150: 110601.

[8]	龚立雄, 陈佳霖, 黄霄, 等. 基于改进Informed-RRT*算法的舰载机甲板平面路径规划[J]. 科学技术与工程, 2024, 24(17): 7429-7437.

[9]	Gong Lixiong, Chen Jialin, Huang Xiao, et al. Aircraft deck flat path planning based on improved Informed-RRT* algorithm[J]. Science Technology and Engineering, 2024, 24(17): 7429-7437.

[10]	王锁男, 张磊, 刘少胡. 油气压裂机组无人机安全巡检路径规划[J]. 科学技术与工程, 2024, 24(25): 10832-10840.

[11]	Wang Suonan, Zhang Lei, Liu Shaohu. Safety inspection path planning for unmanned aerial vehicle in oil and gas fracturing unit[J]. Science Technology and Engineering, 2024, 24(25): 10832-10840.

[12]	蔡健荣, 王锋, 吕强, 等. 基于SBL-PRM算法的柑橘采摘机器人实时路径规划[J]. 农业工程学报, 2009, 25(6): 158-162.

[13]	Cai Jianrong, Wang Feng, Lü Qiang, et al. Real-time path planning for citrus picking robot based on SBL-PRM[J]. Transactions of the Chinese Society of Agricultural Engineering, 2009, 25(6): 158-162.

[14]	许万, 杨晔, 余磊涛, 等. 一种基于改进RRT*的全局路径规划算法[J]. 控制与决策, 2022, 37(4): 829-838.

[15]	Xu Wan, Yang Ye, Yu Leitao, et al. A global path planning algorithm based on improved RRT*[J]. Control and Decision, 2022, 37(4): 829-838.

[16]	谭仲万, 袁宇浩. 基于复杂环境的RRT-Connect路径规划算法改进[J]. 科学技术与工程, 2025, 25(19): 8127-8134.

[17]	Tan Zhongwan, Yuan Yuhao. Improvement of RRT-connect path planning algorithm based on complex environments[J]. Science Technology and Engineering, 2025, 25(19): 8127-8134.

[18]	李文君, 李忠伟, 罗偲. 基于RRT*算法的移动机器人安全光滑路径生成[J]. 电子测量技术, 2024, 47(2): 51-60.

[19]	Li Wenjun, Li Zhongwei, Luo Cai. Safe and smooth path generation of mobile robot based on RRT* algorithm[J]. Electronic Measurement Technology, 2024, 47(2): 51-60.

[20]	康博涵, 黄静雯. 基于环境复杂度的移动机器人变步长RRT路径规划算法与仿真研究[J]. 北京化工大学学报(自然科学版), 2023, 50(4): 87-93.

[21]	Kang Bohan, Huang Jingwen. Variable step size rapidly-exploring random tree (RRT) path planning algorithms and simulation of a mobile robot based on environment complexity[J]. Journal of Beijing University of Chemical Technology(Natural Science Edition), 2023, 50(4): 87-93.

[22]	符强, 蓝星辉, 任风华, 等. 一种双向目标RRT路径规划算法研究[J]. 计算机仿真, 2023, 40(1): 447-454, 461.

[23]	Fu Qiang, Lan Xinghui, Ren Fenghua, et al. Research on route planning algorithm of bidirectional target RRT and Dijkstra[J]. Computer Simulation, 2023, 40(1): 447-454, 461.

[24]	Huang A, Zhou M, Liu M, et al. Adaptive path planning for robotic winter jujube harvesting using an improved RRT-Connect algorithm[J]. Agriculture, 2025, 16(1): 47.

[25]	Zhao X, Wang K, Zhang P, et al. An improved RRT* path planning algorithm combining Gaussian distributed sampling and depth strategy for robotic arm of fruit-picking robot[J]. Computers and Electronics in Agriculture, 2026, 240: 111244.

[26]	Yuan L, Sun G, Ma L, et al. Obstacle avoidance path planning by improved RRT algorithm for mobile robot[J]. Journal of Physics: Conference Series, 2025, 3077(1): 012005.

[27]	赵志刚, 杨迦辉, 赵祥堂, 等. 基于改进RRT算法与碰撞检测的多机悬吊系统避障规划[J/OL]. 北京航空航天大学学报[2026-07-31]. DOI:10.13700/j.bh.1001-5965.2025.0464.

[28]	Zhao Zhigang, Yang Jiahui, Zhao Xiangtang, et al. Obstacle avoidance planning for multi-crane suspended system based on improved RRT algorithm and collision detection[J/OL]. Journal of Beijing University of Aeronautics and Astronautics[2026-07-31]. DOI:10.13700/j.bh.1001-5965.2025.0464.

[29]	陈丹, 谭钦, 徐哲壮. 基于采样点优化RRT算法的机械臂路径规划[J]. 控制与决策, 2024, 39(8): 2597-2604.

[30]	Chen Dan, Tan Qin, Xu Zhezhuang. Robotic arm path planning based on sampling point optimization RRT algorithm[J]. Control and Decision, 2024, 39(8): 2597-2604.

基金资助

国家资助博士后研究人员计划(GZC20250925)

中国博士后科学基金(2025M771354)

河南省自然科学基金(252300423402)

河南省科技攻关计划项目(252102220084)

河南省高等学校重点科研项目计划(25A460002)

河南省高校基本科研业务费专项资金(NSFRF2502055)

河南理工大学博士基金(B2024-42)

PDF (11075KB)

访问

被引

详细

导航

Received	Published	Revised
2025-12-08	2026-10-18	2026-03-16
Online Date
2026-10-30

摘要