基于动态势场引导RRT*算法的机械臂路径规划

Robotic Arm Path Planning Based on Dynamic Potential Field-Guided RRT* Algorithm

摘要

针对机械臂路径规划目标点位于障碍物内部所导致的规划空间非凸、非连通问题，提出一种动态势场引导RRT*算法（DPFG-RRT*）。首先，通过构建虚拟可进入区域，将不可达的内部目标点转化为算法可引导的过渡性区域，破解路径拓扑不连通；其次，设计两阶段动态切换势场函数，在“入口寻找”阶段引导机械臂安全接近目标障碍物入口区域，在“内部精瞄”阶段驱动其精确抵达内部目标点；最后，结合动态权重调节、自适应偏置采样、安全节点扩展与连续碰撞检测等策略，进一步提升规划效率与路径质量。仿真结果表明，在包含动、静态障碍物的三维规划空间中，面向障碍物内部目标点这一特殊规划场景，DPFG-RRT*算法在规划成功率、收敛速度、路径平滑性等方面均具有优良性能。

Abstract

To address the non-convex and disconnected planning space caused by target points located inside obstacles in robotic arm path planning, a dynamic potential field-guided RRT* (DPFG-RRT*) algorithm is proposed. Firstly, a virtually accessible region is constructed to convert the unreachable internal target point into a transition area that can be effectively guided by the algorithm, thereby resolving the topological disconnection issue. Secondly, a two-stage dynamic switching potential field function is designed. In the entrance-seeking stage, the robotic arm is guided to safely approach the entrance of the target obstacle, and in the internal fine-positioning stage, it is driven to precisely reach the internal target point. Finally, strategies such as dynamic weight adjustment, adaptive bias sampling, safe node expansion, and continuous collision detection are integrated to enhance planning efficiency and path quality. Simulation results in three-dimensional planning environments with both dynamic and static obstacles demonstrate that the proposed DPFG-RRT* algorithm achieves superior performance in terms of planning success rate, convergence speed, and path smoothness for the specific scenario of internal obstacle targets.

Graphical abstract

关键词

机械臂 / 路径规划 / 障碍物 / 动态势场

Key words

robotic arm / path planning / obstacle / dynamic potential field

机械臂是信息自动化领域高精度作业的核心执行单元，其在三维复杂环境中的路径规划性能，直接影响作业效率与任务可靠性。在内部检修、腔内探测、狭小空间装配等典型任务场景中，目标点往往位于障碍物内部空间，而非自由空间中的可达位置，导致规划空间呈现非凸、非连通特性。传统方法在此类场景下面临目标偏置采样失效、搜索缺乏方向性、规划空间拓扑不连通等困境，严重制约了机械臂在复杂环境的作业能力。现有路径规划算法普遍默认目标点位于自由空间，难以直接适用于目标点位于障碍物内部的任务场景。因此，开展面向障碍物内部目标点的机械臂路径规划研究，具有重要的意义。

针对机械臂路径规划问题，现有研究如下。1）基于图搜索算法。文献[1]提出的双分辨率栅格地图改进A*算法，通过环境栅格化实现路径搜索，但在高维构型空间中需预先离散化环境，难以应对动态障碍物且计算复杂度较高。2）基于随机采样算法。文献[2]通过动态约束采样提升规划效率，文献[3]采用椭球子集引导加速收敛，此类算法在高维空间中具有良好的适应性，但当目标点不可直达时，存在盲目搜索、入口发现困难等问题，难以有效引导树结构向目标障碍物内部生长。3）基于智能优化算法。文献[4-5]分别采用改进蚁群算法和改进蜣螂算法，在特定场景下取得良好效果，但此类算法求解量较大且易陷入局部最优，难以满足强实时性要求。4）基于人工势场算法。文献[6-7]通过引入斥力补偿机制与避障角优化增强避障能力，但均默认目标点位于自由空间，无法直接处理目标点位于障碍物内部的情形。上述研究均假设目标点位于自由可达空间，当目标点位于障碍物内部时，规划空间在拓扑中呈现非连通状态，直接向目标点采样的策略必然失败，目标偏置采样失效，且算法在发现入口前缺乏明确的方向性，导致规划效率显著下降甚至无法求解。

针对上述问题，提出一种动态势场引导RRT*算法（Dynamic Potential Field Guided RRT*, DPFG-RRT*）。1）提出虚拟可进入区域的概念，将不可达的内部目标点转化为算法可引导的过渡性区域，有效破解路径拓扑不连通问题；2）设计两阶段动态切换势场函数，在“入口寻找”阶段引导机械臂安全接近目标障碍物入口区域，在“内部精瞄”阶段驱动其在受限空间内精确抵达目标点；3）结合自适应偏置采样、安全节点扩展与连续碰撞检测等策略，进一步提升规划效率与路径质量。仿真结果表明，在包含动、静态障碍物的三维规划空间中，DPFG-RRT*算法在规划成功率、收敛速度、路径平滑性等方面均具有优良特性。

1 数学模型与问题的提出

当目标点位于复杂三维工作空间障碍物内部时，路径规划需同时考虑几何约束、运动学约束和动态优化需求。

1.1 机械臂数学模型

采用六自由度（Six Degrees of Freedom, 6-DoF）串联机械臂进行路径规划分析，其关节构型空间可定义为C\subseteq {\mathbb{R}}^{\mathrm{6}}。其中：单个关节构型表示为\mathit{q}={\left[q_{\mathrm{1}},q_{\mathrm{2}},\cdots ,q_{\mathrm{6}}\right]}^{\mathrm{T}}\in C；各关节满足物理限位约束q_i^{\mathrm{m}\mathrm{i}\mathrm{n}}\le q_i\le q_i^{\mathrm{m}\mathrm{a}\mathrm{x}},i=\mathrm{1,2},\dots ,\mathrm{6}。

机械臂正向运动学可表示为

f\left(\mathit{q}\right)=\left[\begin{array}{cc}\mathit{R}\left(\mathit{q}\right)& \mathit{p}\left(\mathit{q}\right)\\ {\mathrm{0}}^{\mathrm{T}}& \mathrm{1}\end{array}\right]（1）

式中：\mathit{R}\left(\mathit{q}\right)\in \mathrm{S}\mathrm{O}\left(\mathrm{3}\right)表示末端执行器的姿态旋转矩阵，SO(·)表示特殊正交群；\mathit{p}\left(\mathit{q}\right)={\left[p_x,p_y,p_z\right]}^{\mathrm{T}}\in {\mathbb{R}}^{\mathrm{3}}表示末端执行器在基坐标系下的位置矢量。

设定末端执行器位置函数{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)可表示为：

\left\{\begin{array}{l}{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)=\mathit{p}\left(\mathit{q}\right);\\ {\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}:C\to {\mathbb{R}}^{\mathrm{3}}.\end{array}\right.（2）

1.2 障碍物数学模型

工作空间W\subseteq {\mathbb{R}}^{\mathrm{3}}中包含两类刚性障碍物：外部障碍物和包含目标点的目标障碍物。

外部障碍物设定为多个不可进入的刚性物体的合集：

\left\{\begin{array}{l}{O}_{\mathrm{e}\mathrm{x}\mathrm{t}}={\displaystyle \underset{j=\mathrm{1}}{\overset{M}{\cup }}}{O}_j;\\ O_j\subset W.\end{array}\right.（3）

式中，O_j均为闭集，其边界\partial O_j表示障碍物表面。

目标障碍物O_i表示内部空腔包含目标点的特殊障碍物，可表示为

O_i=O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}\bigcup O_{\mathrm{w}\mathrm{a}\mathrm{l}\mathrm{l}}（4）

式中：O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}表示障碍物实体部分；O_{\mathrm{w}\mathrm{a}\mathrm{l}\mathrm{l}}表示障碍物内壁，构成内部空腔边界，如图1所示。目标点\mathit{B}位于障碍物内部，满足\mathit{B}\in \mathrm{I}\mathrm{n}\mathrm{t}\left(O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}\right)且B\notin O_{\mathrm{w}\mathrm{a}\mathrm{l}\mathrm{l}}，其中\mathrm{I}\mathrm{n}\mathrm{t}\left(·\right)表示集合内部。

1.3 问题的提出

传统算法通过在构型空间中均匀随机采样渐进探索连通自由空间。然而，当目标点\mathit{B}位于障碍物内部时，目标点本身不属于自由空间C_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}，导致如下失效现象。

1）目标偏置采样失效：直接向目标点\mathit{B}采样的节点必然与障碍物外壁碰撞，成为无效采样。

2）搜索缺乏方向性：在发现入口区域前，其随机探索本质上是盲目的，探索效率极低。

3）规划空间拓扑不连通：由于目标点\mathit{B}不属于自由空间，从起点到目标点的路径在工作空间拓扑上不连通，传统规划器难以处理。

因此，为解决“目标点位于障碍物内部”这一特殊的路径规划问题，设定虚拟可进入区域为路径规划中间目标，将原本不连通的规划问题，分解为“入口寻找”和“内部精瞄”两个阶段，提出DPFG-RRT*算法。

2 DPFG-RRT*算法设计

2.1 总体框架

DPFG-RRT*算法核心思想为通过构建虚拟可进入区域，将不可达的内部目标点转化为算法可引导的过渡性区域，将原本不连通的规划问题分解为“入口寻找”与“内部精瞄”两个阶段。在此基础上，设计两阶段动态切换势场函数，分别引导机械臂安全接近入口区域并在内部精确抵达目标点。同时，结合自适应偏置采样、安全节点扩展与连续碰撞检测等策略，提升规划效率与路径质量。

2.2 两阶段动态势场设计

2.2.1 设定入口平面与入口区域

为实现机械臂从外部工作空间进入目标障碍物内部的可行路径引导，需对“入口”进行精确定义，因此设定入口平面与入口区域。

入口平面P表示目标障碍物O_i外表面上的一个连通开集，是障碍物内部与外部自由空间之间的几何界面，可表示为：

\left\{\begin{array}{l}P=\left\{\mathit{p}\in W\left|\left(\mathit{p}-{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{r}\mathrm{y}}\right)\right.\cdot {\mathit{N}}_P=\mathrm{0}\right\};\\ P\subset \left(\partial O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}\bigcap {\overline{W}}_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}\right);\\ P\bigcap O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}=\mathrm{\varnothing }.\end{array}\right.（5）

式中：{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{r}\mathrm{y}}表示入口平面上的参考点；{\mathit{N}}_P表示入口平面P的单位法向量（指向障碍物内部）；\partial O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}表示目标障碍物实体部分O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}的边界；{\overline{W}}_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}表示自由空间W_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}的闭包。

入口区域E表示以入口平面P为中心面向内外侧延展形成的三维空间区域，表示机械臂穿越入口的物理通道，可表示为

（6）

式中：\mathit{x}表示自由空间中的点且不与任何障碍物重叠；\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\left(·\right)表示欧氏距离函数；{\delta }_E>\mathrm{0}表示入口区域厚度阈值；W_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}=W\setminus \left(O_{\mathrm{e}\mathrm{x}\mathrm{t}}\bigcup O_{\mathrm{b}\mathrm{o}\mathrm{d}\mathrm{y}}\right)表示自由空间。

入口区域分为内侧区域E_{\mathrm{i}\mathrm{n}\mathrm{t}}与外侧区域E_{\mathrm{o}\mathrm{u}\mathrm{t}}：

\left\{\begin{array}{l}{E}_{\mathrm{o}\mathrm{u}\mathrm{t}}=\left\{\mathit{p}\in E\left|\mathrm{0}\le \left(\mathit{p}(\mathit{q})-{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{r}\mathrm{y}}\right)\right.\cdot {\mathit{N}}_E\le {\delta }_E\right\};\\ E_{\mathrm{i}\mathrm{n}\mathrm{t}}=\left\{\mathit{p}\in E\left|-{\delta }_E\le \left(\mathit{p}(\mathit{q})-{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{r}\mathrm{y}}\right)\right.\cdot {\mathit{N}}_E\le \mathrm{0}\right\}.\end{array}\right.（7）

入口平面与入口区域结构示意图如图2所示。

由图2可知，E_{\mathrm{o}\mathrm{u}\mathrm{t}}位于入口平面外部，吸引机械臂进入障碍物内部；E_{\mathrm{i}\mathrm{n}\mathrm{t}}位于入口平面内部，防止机械臂退出障碍物内部区域。

2.2.2 设定虚拟可进入区域

如式（6）所示设定入口区域，但当末端执行器接近入口区域，算法难以稳定判断是否从“入口寻找”切换至“内部精瞄”。

在入口区域边界上，外部势场与内部势场的结构和参数差异巨大，如果以入口区域为切换边界，当末端执行器在入口区域附近小幅振荡时，会导致势场模式频繁切换，虚拟力不具备稳定性。

因此，虚拟可进入区域V_B可表示为：

\left\{\begin{array}{l}{V}_B=\left\{\mathit{p}\in {\mathbb{R}}^{\mathrm{3}}\left|‖\mathit{p}-\mathit{B}‖\le R_{\mathrm{v}}\right.\right\};\\ R_{\mathrm{v}}\ge \underset{\mathit{p}\in E}{\mathrm{m}\mathrm{a}\mathrm{x}}‖\mathit{p}-\mathit{B}‖.\end{array}\right.（8）

式中，R_{\mathrm{v}}表示设计参数，确保从入口区域的任何点出发，只需直线运动小于R_{\mathrm{v}}距离即可进入V_B。而面对障碍物形态复杂、入口区域不为规则平面时，可采用入口区域离散采样的最大距离估计法求解。

虚拟可进入区域V_B如图3所示。由图3可知，V_B设计为以目标点\mathit{B}为中心、半径为R_{\mathrm{v}}的超球体，将不可达的精确点转化为可达区域，提供稳定的势场引导基准和清晰的模式切换边界，防止势场模式在边界附近频繁切换，是连接全局探索与内部收敛的关键枢纽，保障了规划效率和成功率。

2.2.3 设定总势场模式函数

经典势场法在复杂环境中易陷入局部最小值，因此引入动态势场引导策略，并依据机械臂末端执行器位置{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)与目标点\mathit{B}的距离，结合虚拟可进入区域，设定总势场模式函数：

\mathrm{m}\mathrm{o}\mathrm{d}\mathrm{e}\left(\mathit{q}\right)=\left\{\begin{array}{l}\mathrm{m}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{1},‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-\mathit{B}‖>R_{\mathrm{v}}+{\delta }_{\mathrm{h}};\\ \mathrm{m}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{2},‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-\mathit{B}‖\le R_{\mathrm{v}}-{\delta }_{\mathrm{h}}.\end{array}\right.（9）

式中：{\delta }_{\mathrm{h}}为滞后阈值，防止模式在边界附近频繁切换，基于敏感性分析，设定{\delta }_{\mathrm{h}}=\mathrm{0.1}{R}_{\mathrm{v}}；参数R_{\mathrm{v}}表示算法从“入口寻找”切换到“内部精瞄”的决策边界。R_{\mathrm{v}}取值略大于从入口区域到\mathit{B}点的最大距离，以确保一旦末端进入该区域，就必然已经穿过入口并处于障碍物内部。

总势场模式函数示意图如图4所示。由图4可知，设计两阶段动态切换势场，依据末端与虚拟区域的包含关系，动态切换“入口寻找”与“内部精瞄”两种阶段：一是引导机械臂找寻并正对入口区域；二是在受限空间内精确抵达目标点。

2.2.4 设定外部总势场函数

外部总势场结构如图5所示。由图5可知，当末端执行器在V_B外部时，外部总势场U的核心任务为引导机械臂安全、高效地接近入口区域。设定外部总势场由虚拟区域引力场F_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}、障碍物斥力场F_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{e}\mathrm{x}\mathrm{t}}与入口导向势场F_{\mathrm{e}\mathrm{n}\mathrm{d}}^E叠加构建。

1）虚拟区域引力场。虚拟区域引力场并非直接吸引至目标点\mathit{B}，而是吸引至V_B的边界，势能函数设计为

U_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}\left(\mathit{q}\right)=\frac{\mathrm{1}}{\mathrm{2}}{\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}\cdot {\left[\mathrm{m}\mathrm{a}\mathrm{x}\left(\mathrm{0},d\left({\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right),\partial V_B\right)\right)\right]}^{\mathrm{2}}（10）

式中：{\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}>\mathrm{0}表示引力增益系数；d\left({\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right),\partial V_B\right)表示末端执行器到虚拟区域V_B边界的有符号距离（在区域外部时为正）。

对应的引力势能为：

\begin{array}{l}{F}_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}=-\nabla U_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}=\\ \left\{\begin{array}{l}{\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_{\mathrm{B}}}\cdot d\left({\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right),\partial V_B\right)\cdot \frac{{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-\mathit{B}}{‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-\mathit{B}‖},d_{\mathrm{r}\mathrm{e}\mathrm{g}}>\mathrm{0};\\ \mathrm{0},d_{\mathrm{r}\mathrm{e}\mathrm{g}}\le \mathrm{0}.\end{array}\right.\end{array}（11）

式（11）表明，当末端执行器在区域外时，引力大小与距区域边界的距离成正比，方向指向区域中心\mathit{B}；当末端执行器进入区域后，此项引力归0。

2）障碍物斥力场。为避免末端执行器与障碍物外壁碰撞，并引导规划路径朝向入口区域，采用高斯核函数与Sigmoid函数构建模型：

\left\{\begin{array}{l}\begin{array}{l}{U}_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{e}\mathrm{x}\mathrm{t}}\left(\mathit{q}\right)={\displaystyle \sum _{j=\mathrm{1}}^M}{\alpha }_j\cdot \mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{d_j^{\mathrm{2}}\left(\mathit{q}\right)}{\mathrm{2}{\sigma }^{\mathrm{2}}}\right)\cdot \\ {\left(\mathrm{1}+\mathrm{e}\mathrm{x}\mathrm{p}\left(-\beta \left(d_j^{}\left(\mathit{q}\right)-d_{safe}^{}\right)\right)\right)}^{-\mathrm{1}};\end{array}\\ F_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{e}\mathrm{x}\mathrm{t}}=-\nabla U_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{e}\mathrm{x}\mathrm{t}}\left(\mathit{q}\right).\end{array}\right.（12）

式中：{\alpha }_j表示斥力强度系数；\mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{d_j^{\mathrm{2}}\left(\mathit{q}\right)}{\mathrm{2}{\sigma }^{\mathrm{2}}}\right)表示高斯函数；d_j\left(\mathit{q}\right)表示机械臂模型与障碍物O_j之间的最近距离；\sigma表示高斯函数的标准差，控制斥力的衰减速率；{\left(\mathrm{1}+\mathrm{e}\mathrm{x}\mathrm{p}\left(-\beta \left(d_j^{}\left(\mathit{q}\right)-d_{safe}^{}\right)\right)\right)}^{-\mathrm{1}}表示Sigmoid函数，当d_j\gg d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}时，其值趋近于1，当d_j\ll d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}时，趋近于0；\beta控制过渡锐度。

由式（12）可知，Sigmoid函数确保斥力平滑下降为0，避免在边界处斥力不连续的问题；高斯函数确保斥力随距离增加而平滑衰减。

障碍物斥力场F_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{e}\mathrm{x}\mathrm{t}}确保势场函数的可微性和力变化的连续性，有利于数值计算和路径平滑，实现斥力的平滑衰减和可控作用范围。

3）入口导向势场。入口导向势场是路径规划的关键，不仅引导末端执行器达到入口，还需约束末端执行器姿态，校准运动方向至与入口区域法线方向一致，可表示为：

\left\{\begin{array}{l}\begin{array}{l}{U}_{\mathrm{e}\mathrm{n}\mathrm{d}}^E\left(\mathit{q}\right)={\eta }_{\mathrm{e}\mathrm{n}\mathrm{d}}\cdot \left(\mathrm{1}-\mathrm{c}\mathrm{o}\mathrm{s}\theta \left(\mathit{q}\right)\right)\cdot \\ \mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{{‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-{\mathit{C}}_E‖}^{\mathrm{2}}}{\mathrm{2}{\sigma }_E^{\mathrm{2}}}\right);\end{array}\\ F_{\mathrm{e}\mathrm{n}\mathrm{d}}^E=-\nabla U_{\mathrm{e}\mathrm{n}\mathrm{d}}^E\left(\mathit{q}\right).\end{array}\right.（13）

式中：\theta \left(\mathit{q}\right)=\mathrm{\angle }\left({\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-{\mathit{C}}_E,{\mathit{N}}_E\right)表示末端执行器指向入口区域中心{\mathit{C}}_E的向量与入口法向量{\mathit{N}}_E的夹角；余弦项\left(\mathrm{1}-\mathrm{c}\mathrm{o}\mathrm{s}\theta \left(\mathit{q}\right)\right)在方向对准入口区域时取最小值0，偏离时增大；\mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{{‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-{\mathit{C}}_E‖}^{\mathrm{2}}}{\mathrm{2}{\sigma }_E^{\mathrm{2}}}\right)高斯项确保此导向势场只在入口中心{\mathit{C}}_E附近显著;{\sigma }_E控制作用范围。

式（13）有效引导机械臂以“正面朝向”的姿态接近入口区域，提升路径规划成功率。

2.2.5 设定内部总势场函数

内部总势场结构如图6所示。由图6可知，当末端执行器进入V_B后，内部总势场转变为在障碍物内部受限空间内精确、安全地抵达目标点。

设定内部总势场由目标点引力场F_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^B、障碍物内壁斥力场F_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{i}\mathrm{n}\mathrm{t}}与入口区域斥力场F_{\mathrm{r}\mathrm{e}\mathrm{p}}^E叠加构建。

1）目标点引力场。采用标准的二次型势能，提供强而稳定的线性引力，确保末端快速收敛至目标点，可表示为

U_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^B\left(\mathit{q}\right)=\frac{\mathrm{1}}{\mathrm{2}}{\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}}^B\cdot {‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-\mathit{B}‖}^{\mathrm{2}}（14）

对应的目标点引力场为

F_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^B=-\nabla U_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^B\left(\mathit{q}\right)=-{\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^B\cdot \left[{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)-\mathit{B}\right]（15）

式中，{\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^B\gg {\eta }_{\mathrm{a}\mathrm{t}\mathrm{t}\mathrm{r}}^{V_B}，确保在狭小空间内的快速精确定位。

2）障碍物内壁斥力场。由于障碍物内部空间狭窄，需要更精细、更强度的斥力防止碰撞，障碍物内壁斥力场可表示为：

\left\{\begin{array}{l}{U}_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{i}\mathrm{n}\mathrm{t}}\left(\mathit{q}\right)=\left\{\begin{array}{l}\frac{\mathrm{1}}{\mathrm{2}}{\eta }_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{i}\mathrm{n}\mathrm{t}}{\left[\frac{\mathrm{1}}{d_{\mathrm{i}\mathrm{n}\mathrm{t}}\left(\mathit{q}\right)}-\frac{\mathrm{1}}{d_{\mathrm{i}\mathrm{n}\mathrm{t}}^{\mathrm{0}}}\right]}^{\mathrm{2}},d_{\mathrm{i}\mathrm{n}\mathrm{t}}\left(\mathit{q}\right)\le d_{\mathrm{i}\mathrm{n}\mathrm{t}}^{\mathrm{0}};\\ \mathrm{0},d_{\mathrm{i}\mathrm{n}\mathrm{t}}\left(\mathit{q}\right)>d_{\mathrm{i}\mathrm{n}\mathrm{t}}^{\mathrm{0}}.\end{array}\right.\\ F_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{i}\mathrm{n}\mathrm{t}}=-\nabla U_{\mathrm{r}\mathrm{e}\mathrm{p}}^{\mathrm{i}\mathrm{n}\mathrm{t}}\left(\mathit{q}\right)\end{array}\right.（16）

式中，d_{\mathrm{i}\mathrm{n}\mathrm{t}}\left(\mathit{q}\right)表示末端执行器到障碍物内壁的最近距离，当距离趋于零时，障碍物内壁斥力场势能趋近于无穷大，形成有效的硬性约束。

3）入口区域斥力场。为防止末端执行器在未达目标点前退出内部空间，构建入口区域斥力场：

\left\{\begin{array}{l}{U}_{\mathrm{r}\mathrm{e}\mathrm{p}}^E\left(\mathit{q}\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\frac{d_E^{\mathrm{2}}\left[{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left(\mathit{q}\right)\right]}{\mathrm{2}{\sigma }_E^{\mathrm{2}}}\right);\\ F_{\mathrm{r}\mathrm{e}\mathrm{p}}^E=-\nabla U_{\mathrm{r}\mathrm{e}\mathrm{p}}^E\left(\mathit{q}\right).\end{array}\right.（17）

2.3 基于势场引导的随机树生长策略

在动态势场构建基础上，采用自适应偏置采样、安全节点扩展与连续碰撞检测，确保新生成的路径段在考虑机械臂多连杆结构的前提下满足安全无碰撞约束。

2.3.1 自适应偏置采样策略

路径规划时直接根据势场力采样极易陷入局部最优，因此采用总势场产生的虚拟力构建自适应偏置采样策略。

将势场虚拟力归一化为单位引导向量，可表示为

\left\{\begin{array}{l}{g}_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}\mathrm{e}\mathrm{d}}\left(\mathit{q}\right)=\frac{F_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}\left(\mathit{q}\right)}{‖F_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}\left(\mathit{q}\right)‖+\epsilon };\\ F_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}=-\nabla U_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}.\end{array}\right.（18）

式中：\epsilon表示一个极小量，防止虚拟力归零时数值不稳定，确保算法的鲁棒性。

标准RRT*算法从随机采样点{\mathit{q}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}向最近邻节点{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}扩展，DPFG-RRT*算法则构造偏置采样点{\mathit{q}}_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}\mathrm{e}\mathrm{d}}，可表示为

{\mathit{q}}_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}\mathrm{e}\mathrm{d}}={\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}+\lambda \cdot \mathrm{N}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}\left(\alpha \left(k\right)\cdot g_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}\mathrm{e}\mathrm{d}}\left({\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)+\right.

\left.\left(\mathrm{1}-\alpha \left(k\right)\right)\cdot \left({\mathit{q}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)\right)（19）

式中：\lambda表示固定的扩展步长；\alpha \left(k\right)表示自适应权重系数；k表示迭代次数；\mathrm{N}\mathrm{o}\mathrm{r}\mathrm{m}\mathrm{a}\mathrm{l}\mathrm{i}\mathrm{z}\mathrm{e}(·)函数将括号内的向量转换为单位方向向量。

\alpha \left(k\right)={\alpha }_{\mathrm{m}\mathrm{i}\mathrm{n}}+\left({\alpha }_{\mathrm{m}\mathrm{a}\mathrm{x}}-{\alpha }_{\mathrm{m}\mathrm{i}\mathrm{n}}\right)\frac{k}{K_{\mathrm{m}\mathrm{a}\mathrm{x}}}（20）

由式（20）可知，算法初期\alpha较小，以广泛随机探索为主，避免陷入局部最优，保障算法完备性；算法后期\alpha较大，强化势场引导，加速算法收敛。

2.3.2 安全节点扩展与连续碰撞检测

由于机械臂包含多个连杆，路径规划时{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}向{\mathit{q}}_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}\mathrm{e}\mathrm{d}}方向产生新节点{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{w}}后，仅检查末端执行器无法满足实际需求。因此，进行连续碰撞检测，确保从{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}到{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{w}}的整个运动路径中，所有连杆与障碍物均无干涉。

若规划路径段满足连续碰撞检测，则{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{w}}加入随机树，并计算该路径段的成本c\left({\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}},{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{w}}\right)，可表示为

c\left({\mathit{q}}_a,{\mathit{q}}_b\right)=w_l\cdot ‖{\mathit{q}}_b-{\mathit{q}}_a‖+w_s\cdot ‖{\mathit{q}}_b-\mathrm{2}{\mathit{q}}_m+{\mathit{q}}_a‖（21）

式中：w_l表示路径长度权重；w_s表示平滑性权重；{\mathit{q}}_m表示路径中点附近的某个插值点，避免路径规划中机械臂加速度剧烈变化。

2.4 路径规划与优化

完成随机树生长后，进一步对规划路径进行约束判定与优化处理，确保规划结果符合实际要求，实现从路径生成到路径优化的完整闭环。

2.4.1 设定规划路径是否通过入口区域

1）规划路径约束条件。路径规划在关节空间表示为连续映射^[8]：

\left\{\begin{array}{l}\tau :\left[\mathrm{0,1}\right]\to C;\\ \tau \left(s\right)=\mathit{q}\left(s\right).\end{array}\right.（22）

对应的末端轨迹为

\gamma \left(s\right)={\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left[\tau \left(s\right)\right]（23）

式中，s\in \left[\mathrm{0,1}\right]。

对于六自由度机械臂，需考虑所有连杆的碰撞检测，设定机械臂连杆集合为

L=\left\{\left.l_{\mathrm{1}},l_{\mathrm{2}},\cdots ,l_{\mathrm{6}},l_{\mathrm{e}\mathrm{e}}\right\}\right.（24）

式中：l_k为第k个连杆的几何包络体；l_{\mathrm{e}\mathrm{e}}为末端执行器。

对于路径上的任意点s\in \left[\mathrm{0,1}\right]和任意连杆l\in L，存在连续无碰撞条件：

M\left(\tau \left(s\right)\bigcap \left(O_{\mathrm{e}\mathrm{x}\mathrm{t}}\bigcup O_{\mathrm{w}\mathrm{a}\mathrm{l}\mathrm{l}}\right)\right)=\mathrm{\varnothing }（25）

式中，M(·)表示在该时刻机械臂所有刚体占据的空间区域。

设定安全距离函数d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(\mathit{q}\right)：

\left\{\begin{array}{l}{d}_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(\mathit{q}\right)=\underset{l\in L,O\in \left(O_{ext}\bigcup O_{wall}\right)}{\mathrm{m}\mathrm{i}\mathrm{n}}\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\left[l\left(\mathit{q}\right),O\right];\\ d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(\mathit{q}\right)\ge {\delta }_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}.\end{array}\right.（26）

式中，{\delta }_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}>\mathrm{0}表示预设的最小安全距离。

2）规划路径通过入口区域判定约束。设定入口区域通过判定函数：

\chi \left(\mathit{q}\right)=\Pi \left({\mathit{p}}_{end}\left(\mathit{q}\right)\cdot {\mathit{N}}_E>h_E\right)（27）

式中：h_E表示入口平面的位置阈值；\Pi \left(·\right)表示指示函数。

路径规划结果必须满足入口穿越的单调性约束，即存在唯一的s^{\mathrm{*}}\in \left(\mathrm{0,1}\right)使得：

\chi \left[\tau \left(s\right)\right]=\left\{\begin{array}{l}\mathrm{0},s\in \left.\left[\mathrm{0},s^{\mathrm{*}}\right.\right);\\ \mathrm{1},s\in \left.\left(s^{\mathrm{*}},\mathrm{1}\right.\right].\end{array}\right.（28）

式（28）表明，在机械臂路径规划中，末端执行器有且仅有一次穿越入口的行为。

3）规划路径精确到达目标点约束。末端执行器必须精确到达目标点\mathit{B}：

‖{\mathit{p}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\left[\tau \left(\mathrm{1}\right)\right]-\mathit{B}‖\le \varsigma（29）

式中，\varsigma表示到达精度容差，在刚性环境中通常要求小于1 mm。

2.4.2 设定规划路径自适应调节综合代价函数

1）综合性能指标代价函数。综合性能指标代价函数可表示为

J\left(\tau \right)={\lambda }_{\mathrm{1}}\cdot J_{\mathrm{l}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h}}\left(\tau \right)+{\lambda }_{\mathrm{2}}\cdot J_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}\left(\tau \right)+{\lambda }_{\mathrm{3}}\cdot J_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(\tau \right)（30）

各分项代价设定如下。路径长度代价：

J_{\mathrm{l}\mathrm{e}\mathrm{n}\mathrm{g}\mathrm{t}\mathrm{h}}\left(\tau \right)={\int }_{\mathrm{0}}^{\mathrm{1}}‖\dot{\tau }\left(s\right)‖w\mathrm{d}s（31）

式中：‖·‖w为加权的欧几里得范数；权重矩阵\mathit{W}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(w_{\mathrm{1}},\cdots ,w_{\mathrm{6}}\right)反映各关节运动代价差异。

平滑性代价：

J_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}\left(\tau \right)={{\int }_{\mathrm{0}}^{\mathrm{1}}‖\ddot{\tau }\left(s\right)‖}^{\mathrm{2}}\mathrm{d}s（32）

安全性代价：

J_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(\tau \right)={\int }_{\mathrm{0}}^{\mathrm{1}}\mathrm{e}\mathrm{x}\mathrm{p}\left[-\frac{d_{\mathrm{m}\mathrm{i}\mathrm{n}}\left(\tau \right)}{\sigma }\right]\mathrm{d}s（33）

式中，\sigma >\mathrm{0}表示敏感度参数。

2）引入自适应调节策略。针对不同场景需求，采用多源信息融合的自适应调节策略，实现权重系数随路径规划动态调节。

设定任务进度变量\Phi \left(s\right)，表示当前路径点在从起点到目标的整体进度中的位置，可表示为：

\left\{\begin{array}{l}\Phi \left(s\right)=\frac{d_{\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\mathrm{e}\mathrm{d}}\left(\mathit{p}\left(s\right)\right)+d_{\mathrm{m}\mathrm{a}\mathrm{x}}}{\mathrm{2}{d}_{\mathrm{m}\mathrm{a}\mathrm{x}}};\\ d_{\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\mathrm{e}\mathrm{d}}\left(\mathit{p}\right)=\left(\mathit{p}-{\mathit{p}}_E\right)\cdot {\mathit{N}}_E;\\ d_{\mathrm{m}\mathrm{a}\mathrm{x}}=\mathrm{m}\mathrm{a}\mathrm{x}\left(\left|d_{\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\mathrm{e}\mathrm{d}}\left({\mathit{p}}_{\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t}}\right)\right|,\left|d_{\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\mathrm{e}\mathrm{d}}\left({\mathit{p}}_{\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}}\right)\right|\right).\end{array}\right.（34）

式中：d_{\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\mathrm{e}\mathrm{d}}\left(\mathit{p}\left(s\right)\right)表示末端位置相对于入口平面P的符号距离;d_{\mathrm{m}\mathrm{a}\mathrm{x}}表示最大符号距离。

设定环境信息变量\phi \left(s\right)，表示当前路径点处的环境特征，如障碍物分布、拥挤程度等，可表示为：

\left\{\begin{array}{l}\phi \left(s\right)=\left[-\frac{d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\left(\mathit{q}\right)}{d_{\mathrm{r}\mathrm{e}\mathrm{f}}}\right];\\ d_{\mathrm{r}\mathrm{e}\mathrm{f}}=\mathrm{C}\mathrm{l}\mathrm{i}\mathrm{p}\left(\eta \cdot {\overline{d}}_{\mathrm{o}\mathrm{b}\mathrm{s}},d_{\mathrm{r}\mathrm{e}\mathrm{f}}^{\mathrm{m}\mathrm{i}\mathrm{n}},d_{\mathrm{r}\mathrm{e}\mathrm{f}}^{\mathrm{m}\mathrm{a}\mathrm{x}}\right);\\ {\overline{d}}_{\mathrm{o}\mathrm{b}\mathrm{s}}=\frac{\mathrm{1}}{N}\cdot {\displaystyle \sum _{i=\mathrm{1}}^N}\underset{O\in \left(O_{\mathrm{e}\mathrm{x}\mathrm{t}}\bigcup O_{\mathrm{w}\mathrm{a}\mathrm{l}\mathrm{l}}\right)}{\mathrm{m}\mathrm{i}\mathrm{n}}‖x_i-{\Pi }_O{x}_i‖.\end{array}\right.（35）

式中：\eta表示尺度调节系数；{\overline{d}}_{\mathrm{o}\mathrm{b}\mathrm{s}}表示障碍物间平均自由距离的估计值；d_{\mathrm{r}\mathrm{e}\mathrm{f}}^{\mathrm{m}\mathrm{i}\mathrm{n}}表示参考距离下限；d_{\mathrm{r}\mathrm{e}\mathrm{f}}^{\mathrm{m}\mathrm{a}\mathrm{x}}表示参考距离上限；\mathrm{C}\mathrm{l}\mathrm{i}\mathrm{p}(·)表示限幅函数。

设定路径状态变量S\left(s\right)，反映当前路径的局部特性，如曲率、平滑度、安全性等，可表示为

S\left(s\right)=\mathrm{m}\mathrm{i}\mathrm{n}\left[\mathrm{1},\frac{‖\ddot{\tau }\left(s\right)‖}{\ddot{\tau }\left(s\right)}\right]（36）

该变量对应\tau \left(s\right)中的路径平滑度，表明当前路径片段的运动复杂度，数值越大表明路径越不平滑。

自适应调节策略为：

\left\{\begin{array}{l}{\lambda }_{\mathrm{1}}=\frac{\mathrm{1}}{\mathrm{3}}\left(\mathrm{1}-\Phi \left(s\right)\right)\cdot \left(\mathrm{1}-\phi \left(s\right)\right);\\ {\lambda }_{\mathrm{2}}=\frac{\mathrm{1}}{\mathrm{3}}\left(\mathrm{1}-\Phi \left(s\right)\right)\cdot \left(\mathrm{1}-S\left(s\right)\right);\\ {\lambda }_{\mathrm{3}}=\mathrm{1}-{\lambda }_{\mathrm{1}}-{\lambda }_{\mathrm{2}}.\end{array}\right.（37）

自适应调节策略综合代价函数在保证安全的前提下，实现路径长度与平滑性的合理平衡，适应不同场景下的规划需求。

2.4.3 渐进最优性分析与收敛性保障

概率完备性。式（19）中，随机项\left({\mathit{q}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}-{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{a}\mathrm{r}}\right)始终以\left(\mathrm{1}-\alpha \left(k\right)\right)>\mathrm{0}的概率存在。由于{\mathit{q}}_{\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}}在整个自由空间C_{\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}}内均匀采样，因此DPFG-RRT*算法能够探测到路径规划空间的任意角落，保证了算法的概率完备性。

渐进最优性。DPFG-RRT*算法保留重连接过程，如式（38）所示：

r_{\mathrm{r}\mathrm{e}\mathrm{w}\mathrm{i}\mathrm{r}\mathrm{e}}\left(n\right)=\gamma {\left(\frac{\mathrm{l}\mathrm{n}n}{n}\right)}^{\frac{\mathrm{1}}{\mathrm{2}}}（38）

式中，\gamma表示几何常数。当新节点{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{w}}加入后，DPFG-RRT*算法检查其一定半径r_{\mathrm{r}\mathrm{e}\mathrm{w}\mathrm{i}\mathrm{r}\mathrm{e}}内的邻近节点，尝试通过{\mathit{q}}_{\mathrm{n}\mathrm{e}\mathrm{w}}为这些节点提供成本更低的父节点路径，从而持续优化树结构。

动态势场引导策略仅影响新节点的产生位置，而不影响已存在节点间的连接优化，有效保障改进算法的渐进最优性。

2.4.4 路径规划\mathrm{B}样条拟合

采用3次\mathrm{B}样条函数，对规划路径点\left\{q_{\mathrm{0}},q_{\mathrm{1}},\cdots ,\right.

\left.q_m\right\}进行拟合，可表示为^[9-11]

Q\left(s\right)=\mathrm{m}\mathrm{i}\mathrm{n}{\displaystyle \sum _{i=\mathrm{0}}^m}{‖Q\left(s_i\right)-q_i‖}^{\mathrm{2}}+\lambda {\int }_{\mathrm{0}}^{\mathrm{1}}{‖\frac{{\mathrm{d}}^{\mathrm{2}}Q}{\mathrm{d}{s}^{\mathrm{2}}}‖}^{\mathrm{2}}\mathrm{d}s（39）

式中，\lambda表示平滑项权重。

综上所述，障碍物内部目标点的路径规划问题如图7所示。由图7可知，DPFG-RRT*算法通过引入虚拟可进入区域，有效克服目标点位于障碍物内部所导致的路径拓扑不连通问题；设计两阶段动态势场函数，有效引导机械臂安全、精准地规划至目标点；结合动态权重调节、自适应偏置采样以及连续碰撞检测等策略，保持渐进最优性的同时，显著提升搜索导向性。

3 仿真分析

3.1 函数测试

为全面验证DPFG-RRT*算法的寻优性能，选取单峰Generalized Rosenbrock、多峰Ackley、固定维多峰Kowalik等标准测试函数，与常用算法进行对比，收敛曲线结果如图8所示^[12-15]。其中，图例Ⅰ、Ⅱ、Ⅲ、Ⅳ和Ⅴ分别表示DPFG-RRT*、RRT*-Connect、Informed-RRT*、RRT*-Smart和RRT*算法。由图8可知，在单峰、多峰及固定维多峰3类测试函数中，DPFG-RRT*算法均表现出更快的收敛速度、更优的函数值及更高的求解精度，其寻优性能显著优于对比算法。

为进一步量化分析，设定每个测试函数独立运行50次，统计最优值、最差值、平均值、标准差及平均运行时间，结果如表1所示。由表1可知，DPFG-RRT*算法展现出全面稳定的优化性能。单峰函数下，解的质量与稳定性突出，可应对复杂搜索空间；多峰函数下，能快速一致收敛，求解效率优于对比算法；固定维多峰函数下，函数求解精度高。

3.2 路径规划

为验证DPFG-RRT*算法在实际任务中的有效性，在MATLAB环境中构建六自由度机械臂模型，如图9所示。

在其工作空间内设置静态（球体、立方体、圆柱体）和动态（沿X、Y轴运动及圆周运动的球体）障碍物，如图10所示。

分别采用DPFG-RRT*、RRT*-Connect、Informed-RRT*、RRT*-Smart和RRT*算法进行路径规划仿真，结果如图11和表2所示，其中RRT*算法路径规划失败，在表2中使用“—”表示。DPFG-RRT*算法下机械臂能够有效避开三维空间内静态、动态障碍物，构建光滑平顺的规划路径，具备较好的泛化性。

为更好地验证DPFG-RRT*算法在三维空间中的机械臂路径规划性能，对上述5种算法进行50次重复仿真，仿真结果如表3所示^[16-17]。

由表3可知，DPFG-RRT*算法在各项性能指标上均表现出显著优势。1）运行效率显著提升。DPFG-RRT*算法平均运行时间最短，较RRT*-Connect、Informed-RRT*、RRT*-Smart和RRT*算法分别降低19.6%、46.8%、28.2%和55.4%，表明动态势场引导机制有效减少了无效采样与冗余扩展，提高了搜索的导向性与收敛速度。2）路径质量明显改善。DPFG-RRT*算法平均路径节点数最少，且平均路径长度最短，表明动态势场在入口寻找与内部精瞄阶段的有效协同，克服末端执行器在入口区域边界处的振荡与冗余探索，规划路径生成更简洁、更接近最优的路径。3）路径规划稳定性强。DPFG-RRT*算法的规划成功率最高，表明虚拟可进入区域与滞后切换策略有效克服势场在入口区域的频繁切换问题，增强了算法在复杂环境中的鲁棒性。

综上所述，DPFG-RRT*算法在机械臂路径规划任务中，从规划效率、路径质量与运行稳定性等3个方面分析，均表现出优良的规划性能。

4 结束语

针对目标点位于障碍物内部，路径规划连通性缺失、搜索方向性弱、规划效率低等问题，提出了一种动态势场引导RRT*算法。研究表明，DPFG-RRT*算法通过引入虚拟可进入区域将不可达目标点转化为可引导的过渡区域，并设计两阶段动态势场分别实现“入口寻找”与“内部精瞄”，结合动态权重调节、自适应偏置采样、安全节点扩展与连续碰撞检测等策略，显著提升了路径规划的成功率、效率与路径质量。仿真实验验证了算法在静态与动态障碍物共存的三维环境中具有更优的收敛速度、路径平滑性及运行稳定性。

DPFG-RRT*算法有效拓展路径规划算法在非凸、非连通场景下的适用性，为克服“障碍物内部目标点”这类具有几何与拓扑约束的规划问题提供了有效参考。后续，将围绕高度动态、多障碍物密集环境中的实时性问题进行深入研究。

参考文献

[1] 苏毅，吴文欢，李鼎鑫.基于双分辨率栅格地图的机器人路径规划研究［J］.仪器仪表学报，2025，46（3）：86-100.

[2] 陈忠，阴艳超，易斌，等.动态约束采样RRT*-Connect算法的移动机器人路径规划［J］.机械科学与技术，2025，44（12）：2078-2089.

[3] 潘博阳.基于混合策略的苦鱼优化算法［J］.信息工程大学学报，2025，26（5）：548-553.

[4] 赵子捷，张学鑫，樊伟，等.面向大型筒件装配界面的机器人扫描测量路径规划［J］.计算机集成制造系统，2025，31（5）：1555-1568.

[5] 禤逸曈，李立君，陈海飞.基于改进蜣螂算法的机械臂路径规划［J］.机械传动，2025，49（2）：70-78.

[6] 赵佳豪，钱德猛，汤传业.基于改进人工势场法的AGV路径规划算法研究［J］.黑龙江工业学院学报（综合版），2025，25（6）：121-128.

[7] 万俊，孙薇，葛敏，等.基于含避障角人工势场法的机器人路径规划［J］.农业机械学报，2024，55（1）：409-418.

[8] 马奔驰，章翔峰，姜宏，等.BIA-RRT：自适应障碍物机械臂路径规划［J］.现代电子技术，2025，48（9）：130-136.

[9] 王萍，潘树国，蔚保国，等.基于RSS_GN RRT算法的狭长空间路径规划［J］.电子测量与仪器学报，2024，38（1）：72-85.

[10] 赵紫彤，秦宁宁，孙虎，等.曲折障碍场景下无人机的动态路径规划［J］.信息与控制，2025，54（6）：812-826.

[11] 王蕾，齐铎，林晋福.面向突防作战的无人机集群融合路径规划算法［J］.信息工程大学学报，2025，26（5）：624-630.

[12] 郭利进，李强.基于改进RRT*算法的移动机器人路径规划［J］.智能系统学报，2024，19（5）：1209-1217.

[13] 张晨，袁宇浩.基于改进的RRT∗-Connect机械臂路径规划［J］.计算机仿真，2024，41（11）：458-464.

[14] 鲁宇明，周羽逵，郭鑫，等.改进Informed RRT*算法移动机器人路径规划［J］.计算机工程与应用，2025，61（8）：283-293.

[15] 赖荣燊，窦磊，巫志勇，等.改进RRT*-Smart算法的移动机器人全局路径规划研究［J］.计算机工程与应用，2025，61（13）：378-388.

[16] 霍韩淋，邹湘军，陈燕，等.基于视觉机器人障碍点云映射避障规划及仿真［J］.系统仿真学报，2024，36（9）：2149-2158.

[17] 章一鸣，姚文广，陈海进.动态环境下自主机器人的双机制切向避障［J］.浙江大学学报（工学版），2024，58（4）：779-789.

基金资助

湖北省自然科学基金(2021CFB135)