基于增强型动态运动基元与导向力场的动态路径点轨迹规划方法

《Advanced Intelligent Systems》：Via-Points Task Execution Based on Enhanced Dynamic Movement Primitives and Steering Force Fields

本文提出了一种结合增强型动态运动基元（DMP）与复合导向力场的机器人轨迹规划新框架。该方法通过引入导向吸引力与回归吸引力，有效解决了传统DMP在动态路径点（via-points）穿越和轨迹形状调控方面的局限性。研究证实，该方法能在不改变起止点位置的前提下，实现机械臂在复杂受限空间中对动态路径点的灵活穿越，同时保持演示轨迹的原有特征，显著提升了系统在动态环境中的适应性与泛化能力。数值仿真与Franka机械臂实验均验证了该方法的有效性与稳定性。

　　

¹ 引言

在机器人运动规划领域，学习示教（LfD）是一种通过模仿人类操作者技能使机器人执行特定任务的有效技术。该方法在降低编程成本的同时，提升了机器人在多样化复杂作业场景中的适应性与可靠性，其应用范围涵盖工业制造至航空航天等多个领域。例如，在工业制造中的拆装任务涉及结构复杂的物体，机械臂的工作空间受到高度限制。根据柔性制造的要求，操作需具备泛化能力并允许轨迹形状的调控。然而，在穿越路径点以满足特定任务需求时，常面临挑战。这些路径点可能在执行过程中动态变化，且环境中存在空间约束与潜在障碍。然而，当前主流的LfD技术，包括概率方法与基于动态系统的方法，在应对路径点穿越与轨迹形状调整方面仍存在不足。

概率方法如概率运动基元（ProMP）与高斯混合模型-高斯混合回归（GMM-GMR）通过分析大量人类示教数据提取技能特征并估计先验概率，为机器人学习提供基础。这类方法的主要优势在于能够处理不确定性与变异性，使机器人能够在不同环境与条件下复制与适应学习到的行为。然而，当示教数据有限时，其有效性主要局限于接近演示轨迹的区域。若路径点超出高斯模型的方差边界，再现的轨迹可能被过度放大，常常超出机器人的操作空间而变得不可行。此外，在多自由度系统中，轨迹建模需要高维高斯分布，这会显著增加计算复杂度。

基于动态系统的方法，如动态运动基元（DMP），采用非线性系统模型来逼近轨迹，使机器人能够通过最少的参数调整来复制与泛化演示运动。这类方法在解决复杂轨迹生成问题时具有实时性与计算效率，因此吸引了大量研究关注。然而，其泛化能力仍然有限。在实际场景中，面对环境变化或任务需求改变，DMP往往难以适应新条件，常常需要重新示教或参数调整。因此，如何在不增加学习过程的前提下提升DMP对动态环境的适应性至关重要。

目前，基于DMP的路径点穿越方法引入随时间变化的吸引子，使其平滑地从一个路径点过渡至下一个直至最终目标，或通过复杂的参数变化进行调整。然而，这些方法无法保证路径点被准确穿越，且时变吸引子引起的轨迹畸变仍不可预测。改进基于DMP的轨迹形状调整方法，确保在无需额外学习阶段或参数修改的前提下准确穿越动态路径点，将为导航任务带来显著益处。基于DMP的轨迹形状调整方法大致可分为两类耦合项：一类采用传统势场方法，将排斥势场作为耦合项集成至DMP中以实现避障，但该方法仅当机器人进入势场区域时有效，可能导致轨迹突变并存在陷入局部极值的风险；另一类采用转向角方法，引入类人避障功能作为耦合项以调整轨迹形状并避开点障碍，然而该方法在某些情况下会引发轨迹抖动。这两类耦合项均存在明显局限：轨迹根据障碍物位置被动修改，无法有效引导路径点穿越。迄今为止，尚未见报道能够可靠实现路径点穿越的耦合项方法。

本文提出一种新方法，通过将导向力场约束引入耦合项来改进DMP。该方法引入导向吸引力与回归吸引力，以促进路径点穿越并调整轨迹形状，显著提升了在动态环境中的泛化能力与适应性。具体而言，该方法实现了以下关键目标：1）在规划过程中通过路径点处的吸引力调整轨迹形状，实现平滑穿越指定路径点；2）实时规划动态调整导向力，确保高效穿越动态路径点；3）引入回归吸引力以优化轨迹形状，提升对演示意图的保持；4）通过消除抖动与避免陷入局部极值，保证轨迹平滑。

² 原始DMP及其局限性

DMP是一种基于“吸引点”概念的模型，采用具有自稳定性的二阶弹簧阻尼系统，旨在将末端执行器吸引至目标位置，并利用强迫函数引导机器人沿期望轨迹运动。对于三维系统，其状态转移可表达为公式（1）。其中，τ为时间尺度因子，系统持续时间可通过τ调整；向量x、?、?（d = 3）分别表示系统的位置、速度与加速度；x_init、g ∈ R^d为起始与目标位置；对角矩阵K、D为系统的弹性与阻尼项，满足临界阻尼关系D_i = 2√K_i；强迫项f(s) = [f₁(s), f₂(s), …, f_d(s)]通过基函数表示，每个分量f_p(s), p = 1,2,…,d具有公式（2）所示形式。其中，ψ_i(s)为径向基函数，其权重w_i^(p)可通过局部加权回归等优化技术选择，以使强迫函数匹配期望轨迹。强迫项f依赖于相位变量，后者由规范系统对时间t进行重参数化定义，其瞬态行为如公式（3）所示，其中λ为预定义正常数。径向基函数ψ_i(s)定义如公式（4），其中高斯函数宽度h_i与中心c_i分别按公式（5）与（6）计算。

在学习阶段，记录演示轨迹x_demo、?_demo与?_demo，由公式（1）计算目标强迫项f_target，进而通过线性回归计算权重w_i^(p)。在执行阶段，设置起止点位置x_init、g，利用已计算权重通过公式（2）计算强迫项f，求解公式（1）所示动态系统即可得到与学习轨迹形状相似、从x_init收敛至g的轨迹。传统DMP通过调整τ实现对轨迹的拉伸或压缩，但该方法无法设置路径点或调整轨迹形状。

注：DMP可有效应用于高维场景。对于d维轨迹，每个维度对应独立的DMP框架，因各维度运动相互独立，而所有维度共享同一规范系统。本研究主要解决操作空间中机械臂的轨迹规划问题，考虑d = 3（三维笛卡尔空间）。通过集成基于四元数的姿态插值算法，并结合逆运动学实现完整运动规划，同时满足位置与姿态约束。所提方法同样适用于关节空间。

³ 力场约束下的轨迹规划

当空间机械臂执行任务时，由于工作空间受限，直接示教常不切实际，通常采用地面示教方式，使机械臂基于获取与增强的技能在太空中自主执行任务。考虑到空间环境的复杂性，本文通过引入导向力场约束增强DMP框架，提出一种轨迹规划方法。改进后的方法在原始DMP基础上引入力场耦合项，克服了原始DMP在泛化能力方面的局限。控制方程定义如公式（7），其中附加项f_couple表示力场耦合项。

通过在设计耦合项时引入吸引力，增强型DMP获得了在三维工作空间中穿越动态路径点并有效调整轨迹形状的能力。

^3.1 静态路径点

当操作空间中存在静止路径点P时，基于B样条曲线的轨迹规划难以适应初始演示轨迹。因此，将其归类为“吸引点”，产生导向吸引力以引导轨迹穿越该指定点。吸引力f_a1的方向垂直于当前速度v并指向路径点P。该吸引力的大小在方向确定后对引导轨迹穿越指定路径点起关键作用。在接近路径点P的过程中，关键因素包括机械臂末端执行器位置X与路径点P之间的距离d，以及当前速度向量v与向量XP之间的夹角θ。因此，将这些因素纳入决定导向吸引力大小的评价指标中。导向吸引力的 formulation 如公式（8）所示，其中v为末端执行器当前速度，d = |X ? P|表示位置X与点P之间的距离，θ = arccos[(v · XP) / (|v| |XP|)]表示当前速度方向v与向量XP之间的夹角。函数f₁(d)定义为关于d的函数，f₂(θ)定义为关于θ的函数。

f₁(d)是与距离d相关的函数。为确保吸引力对原始轨迹产生平衡影响，从而最小化自由空间损失，目标是在d较大或较小时保持f₁(d)为较小值，且在d = d₀附近显著增大以引导轨迹穿越点P。参数d₀由点P到原始轨迹的最小距离d_min确定。经过点P后，f₁(d)设为零。函数f₁(d)定义如公式（9），其中μ₁为正常数。f₂(θ)是关于θ定义的函数，θ ∈ [0, π)。当轨迹穿越路径点时，对于较大的θ，需要较大的转向角，因此期望f₂(θ)增大；反之，对于较小的θ，转向角要求降低，故f₂(θ)应保持较小。函数f₂(θ)定义如公式（10），其中μ₂为正常数。

基于以上讨论，导向吸引力可表达为公式（11），其中k₁为常数，R为旋转π/2或?π/2的旋转矩阵。

将公式（11）代入公式（7），其中f_couple = f_a1，所得规划轨迹如图4a所示。图中黑线表示原始轨迹，绿线表示穿越路径点的轨迹。可见，在吸引力f_a1影响下，泛化轨迹成功穿越路径点P，且与原始演示轨迹偏差最小。对于单一路径点情况，泛化轨迹可在无需参数调整的情况下，以不同距离穿越P。当多个路径点位于轨迹同侧或对侧时，吸引力为各路径点所产生力的合力，从而使泛化轨迹灵活穿越所有指定路径点。

高质量的规划轨迹不仅应穿越路径点，还应尽可能接近原始轨迹。该特性可提高与原始轨迹的相似度，同时最小化自由空间损失。为此，提出一种基于DMP原理的动态逼近方法。在规范系统的每个时间步更新耦合项，并引入回归吸引力f_a2以平衡路径点P产生的吸引力。在此动态逼近过程中，当前速度v与原始轨迹速度v_d之间的夹角φ，以及当前位置X与原始轨迹X₀之间的欧氏距离δ，被纳入影响f_a2项的因素。力f_a2定义如公式（12），其中f₃(δ) = exp(?μ₃δ)，f₄(φ) = 1 ? exp(?μ₄φ)。

引入吸引力f_a2后所得的规划轨迹如图4b所示，其中虚线表示加入f_a2后的轨迹。可见，自由空间损失减小，且规划轨迹与原始轨迹的相似度提高。

^3.2 动态路径点

上述轨迹规划方法针对静态环境设计。然而，在动态环境中，路径点P以不同速度移动，需进一步修改以确保有效完成任务。路径点的动态运动在空间机器人应用中常见，尤其当目标呈现漂浮行为时。

在动态规划过程中，需考虑两个关键因素：（i）点P的速度向量v_p；（ii）公式（13）中的距离d、δ以及夹角θ、φ，所有这些均实时更新。在此阶段，系统的耦合项可表述为公式（13），其中v_rel = v ? v_p表示末端执行器与P之间的相对速度。

动态规划仿真结果如图5所示。左侧路径点的初始位置与速度分别为[?0.3, 0.2] m与[0.2, 0] m T^?1，右侧路径点则为[0.3, 0.2] m与[0, ?0.1] m T^?1。结果表明，所提运动规划方法不仅克服了在动态环境中穿越路径点P的挑战，而且生成的轨迹与预期教学目的一致。

为进一步逼近真实操作场景，研究了包含多个路径点的复杂轨迹，如图6所示，其中绿色标记表示静态路径点。演示轨迹呈现明显的曲率变化，四个路径点沿路径分布在不同位置。动态过程中，对这些路径点施加不同的速度约束，其最终位置由红色标记指示。结果证实，所提方法可同时满足所有路径点的位置与速度约束，生成平滑连续的轨迹，并保持较低的自由空间损失水平。

^3.3 三维泛化

上述方法主要针对二维空间开发。为提升其适用性，将算法扩展至三维空间。力耦合项中的距离与角度d、θ、δ、φ可轻松从二维推广至三维。主要挑战在于确定吸引力的方向，特别是推导R的表达式。吸引力分析如图7a所示。令f位于点P与速度向量v张成的平面内，其方向定义为绕旋转轴n = (v × XP) / |v × XP|旋转π/2或?π/2朝向点P。相应地，R的表达式由公式（14）给出，其中θ = π/2或?π/2，n_x、n_y、n_z分别表示向量n在x、y、z方向的分量。

三维动态路径点规划的轨迹如图7b所示。原始轨迹定义为x = sin t、y = cos t、z = 0.2 t，其中t ∈ (?π/2, π/2)。点P₁位于[?0.8, 0.5, ?0.1] m，作为静态路径点；点P₂位于[0, 0.8, 0.1] m，被赋予速度v = [0.2, ?0.1, 0.1] m T^?1。图7c进一步展示了沿复杂轨迹穿越多个静态与动态路径点的过程。结果表明，所提运动规划方法可灵活实现三维空间中静态与动态路径点的穿越。

^3.4 整体算法结构

基于前述分析，提出一种集成DMP与导向吸引力的机器人轨迹规划方法。f_a1表示吸引点产生的导向吸引力，f_a2表示演示轨迹产生的回归吸引力，f_t表示各吸引点的合力导向力。为更清晰理解整体算法，已 formulated 伪代码（见算法1），其中x_g、?_g、?_g分别表示泛化轨迹的位置、速度与加速度；x_d与?_d表示演示轨迹的位置与速度；ε表示小常数，δt表示时间步长。

⁴ 稳定性分析与方案比较

^4.1 稳定性分析

本节分析DMP与导向吸引力方案之间耦合项的稳定性。证明该耦合项满足Lyapunov稳定性准则，从而保证所有初始状态向目标收敛。当t → ∞时，相位变量s趋于0。因此，简化后的DMP方程可表达为公式（15）。

点[x, ?] = [g, 0]表示方程的停滞点。通过构造Lyapunov函数V并证明V > 0、dV/dt < 0，保证所有状态收敛至该停滞点。具体地，采用阻尼弹簧质量系统的能量函数V = (1/2)?^T? + (1/2)(x ? g)^TK(x ? g)，V可表达为公式（16）。

对V求导得表达式（17）。

由于R是执行π/2旋转的矩阵，故v^TR v = 0。且当? ≠ 0时，dV/dt = ??^TD? < 0，这证实了DMP从任意初始状态收敛至平衡点[g, 0]。

^4.2 方案比较

当前流行的LfD技术包括概率方法与基于动态系统的方法。为验证本文所提方法的有效性，在相同条件下将其与主流方法进行比较，如图8所示。图中红色虚线表示刚性边界（如墙壁或围栏），灰色方块表示操作过程中遇到的障碍物（如结构部件或凸起）。由于演示轨迹通常经过避障、能效等约束优化，在泛化过程中保持演示路径尤为重要。图8中，黑线表示ProMP方法得到的均值轨迹，作为其他方法的演示轨迹。绿色区域表示一倍标准差范围内区域，橙色曲线表示ProMP在穿越路径点时的泛化轨迹。可见，该方法导致可用自由空间大量损失，尤其在低方差区域（如图8b中x = 0.5附近）。此外，即使路径点位置的微小调整也会导致泛化轨迹的显著偏离，易超出关节限位。路径点放大图进一步表明，概率方法难以准确穿越路径点。蓝线对应ELM方法，青线表示DMP++方法，二者均引起相当大的自由空间损失并导致与障碍物碰撞。相比之下，本文提出的SFF-DMP与LI-DMP方法在确保轨迹准确再现的同时减少了自由空间损失。然而，线性插值方法存在速度曲线不平滑、对动态路径点适应性差等问题。图8中，以均值轨迹作为演示输入，红线描绘了所提方法产生的泛化轨迹。与其他方法相比，所提方法实现了最小的自由空间损失。

⁵ Franka机械臂实验

实验采用7自由度Franka Emika Panda机器人手臂进行，如图9所示，同时配置含路径点的桌面装置。路径点的运动由三个伺服电机控制以实现三个自由度，机械臂在Ubuntu 20.04下的机器人操作系统（ROS）框架中运行。

本实验采用的专家演示定义为：x_demo(t) = [sin(t), cos(t), 0.2t]，t ∈ [?π/2, π/2]。以Franka机械臂底座中心为坐标系原点。如图10所示，实验过程快照描绘了Franka末端执行器的轨迹。每行图对应一次实验运行，包含两个快照。初始与目标位置分别为x_init = [0.5, 0.0, 0.3] m与g = [0.0, 0.5, 0.5] m。为评估所提方法的有效性，测试了其在穿越两个静态路径点（位于[0.4, 0.2, 0.45] m与[0.18, 0.4, 0.45] m）时的性能。考虑到穿越路径点的要求，并为实验演示与设备安全，路径点固定有柔性杆。穿越路径点时，杆件变形，从而验证成功穿越同时防止设备损坏。图10所示实验证实了算法在静态环境中的有效性。为清晰起见，期望轨迹与执行过程合并于同一图示中。生成的轨迹与使用Franka机器人得到的经验结果一致。