[0001] 本发明涉及机械臂轨迹规划技术领域，尤其是涉及基于加权势函数和DMP的双臂机器人避障方法及系统。

[0002] 随着工业自动化和智能制造的快速发展，机器人技术尤其是双臂机器人在生产线上的应用变得日益重要。双臂机器人通过模拟人类的双手操作，能够完成更复杂、更精细的任务。然而，随着应用场景的不断复杂化，如自动化装配、精密加工和复杂物料搬运等，双臂机器人面临的挑战也随之增加。在这些场景中，虽然通常对双臂机器人进行的是在无障碍物情况下的示教演示，但实际操作环境往往充斥着移动的作业人员、其他机械设备，以及在执行任务过程中产生的临时障碍物。因此，双臂机器人在执行任务时不仅要保持示教轨迹的运动特征，还必须具备灵活避开障碍物的能力以保证操作的安全性和高效性。

[0003] 在众多避障技术中，结合动态势函数的人工势场法因其简洁高效而被广泛采用。该方法通过构造一个虚拟的势场，使得机器人能够在势场的引导下绕开障碍物。然而，传统的势函数法存在局部最小值问题，可能导致机器人陷入障碍物周围无法脱离。同时，现有的一些单独的避障算法如超二次势函数法等容易生成不够平滑和不紧凑的路径，这对于需要执行精细操作的双臂机器人来说是不可接受的，因为它可能导致操作过程中的精度降低、对机器人自身和周围环境造成损伤以及机器人工作空间的浪费。此外，考虑到双臂机器人操作的动态性，两个机械臂在运动过程中也可能发生碰撞，单独考虑每个臂对周围障碍物的避障是不完整的。因此，提出一种基于DMP（Dynamic Movement Primitives，动态运动基元）和动态势函数的双臂机器人避障方法，在考虑机械臂的动态特性的前提下实现双臂机器人系统对周围障碍物的避障以及双臂之间的避免碰撞。

[0015] 为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

[0016] 参见图1，基于加权势函数和DMP的双臂机器人避障方法，包括如下步骤：S1、获取双臂机器人的机械臂在无障碍物环境下的示教轨迹，建立机械臂的初始DMP模型，将机械臂在无障碍物环境下的示教轨迹输入初始DMP模型进行学习并获取权重系数，采用权重系数更新初始DMP模型，得到参数化的DMP模型。

[0017] 具体地，示教轨迹包括无障碍物环境下左机械臂的位置和速度、无障碍物环境下右机械臂的位置和速度，建立机械臂的初始DMP（Dynamic Movement Primitives，动态运动基元）模型，DMP模型是以弹簧阻尼系统为基础模型，利用非线性力项来学习和近似示教轨迹，以获取权重系数。

[0018] 进一步地，机械臂的初始DMP模型可表示为：（1）
式中, 是时间缩放因子，用于控制运动持续时间，和 分别表示机械臂当前时刻的速度和位置， 和 均为对角矩阵，分别对应DMP模型的刚度矩阵和阻尼矩阵， 和 分别表示机械臂的目标位置和初始位置， 表示一阶导数。

[0019] 为了确保DMP模型最终收敛到目标 ，假设 和 之间的关系为：，是一个相位变量，随着时间从0到 ，将会从1减至0。具体可用下面的正则
系统方程来表示:
 （2）
表示强迫项，由N个非线性径向基函数线性组合而成,用于调节DMP模型以重
现从示教轨迹中学到的技能， 可以表示为：
（3）
其中， （4）
式中， 为第 个非线性径向基函数， 和 分别表示第 个非线性径向基函
数 的中心和高度， ， ， 为非线性径向基函数的总数， 是
权重，通过从示教轨迹中学习获得。

[0020] 将示教轨迹的位置与速度参数输入到DMP模型中，通过局部加权回归确定权重 ，过程如下:（5）
式中，
（5）‑1
（5）‑2
（5）‑3
（5）‑4
式中， 为系数矩阵， 为示教轨迹, 为示教轨迹的时间点，是遗忘因子，
为系数矩阵 中第 个列向量的转置， 表示第 个预测误差协方差逆矩阵，用于更新权重， 为初始值， ，为单位矩阵, 示教轨迹的轨迹持续时间。

[0021] 得到权重 之后，可以利用公式（3）计算强迫项 ，同时选择性的设置机械臂的起始位置与目标位置，通过求解DMP模型就能得到与示教轨迹运动风格相似的轨迹。

[0022] S2、根据机械臂与最近障碍物的距离设置自适应变化加权系数，根据自适应变化加权系数、超二次势函数与高斯势函数构建加权组合等势函数。

[0023] 具体地，基于超二次势函数构建超二次势函数的等势函数：（6）
式中， 为超二次势函数的等势函数， ， 和
分别是确定x,y和z轴尺度的实值函数， 是超二次势函数的阶数，均为常数。

[0024] 超二次势函数以其形状适应性在避障策略中得到了应用，通过调整超二次势函数等势函数的参数，超二次势能场可以更好地匹配不同形状的障碍物，为机械臂提供更精确的避障指引。但是，超二次势函数的复杂性和潜在的尖锐边缘可能会导致避障路径出现不必要的急转弯，从而影响运动的平滑性和连贯性。

[0025] 基于高斯势函数构建高斯势函数的等势函数：（7）
式中， 为高斯势函数的等势函数， 是高斯分布的幅度， 是高斯分布的
均值向量，代表分布的中心， 是协方差矩阵，用于控制高斯分布的形状和方向，是一个常数，用于确定高斯势函数等势面的高度。

[0026] 高斯势函数因其数学特性和平滑性被广泛应用于避障算法中，高斯势能场以障碍物为中心，向外辐射衰减，能够引导机械臂平滑绕过障碍。然而，高斯函数的快速衰减特性和对称性在处理形状复杂或分布不均的障碍物时显示出局限性，特别是在障碍物形状与高斯势能场不匹配的情况下，可能导致避障路径不够紧凑或效率低下。

[0027] 为了提高DMP模型避障效率和适应性，还需要设置加权系数，如果将加权系数设置为一个固定值，可能会导致一些突变和不紧凑的避障行为，同时针对不同障碍物所需要设置的加权系数的具体值也不确定，因此本实施例中根据机械臂与最近障碍物之间的距离设置自适应变化加权系数 ，可用公式表示为：（8）
式中， 为自适应变化加权系数， ， 是机械臂与最近障碍物中心之
间的距离， 是定义权重变化中心点的距离阈值，通过设置 的值来适应不同形状大小的障碍物同时允许控制自适应变化加权系数 的变化更集中于特定的输入值，是控制权重变化陡峭程度的控制系数。

[0028] 将超二次势函数的等势函数与高斯函数的等势函数进行加权组合，得到加权组合等势函数 ：（9）
式中， 为加权组合等势函数，为自适应变化加权系数， 超二次势函
数的等势函数， 为高斯势函数的等势函数。

[0029] 上述超二次势函数的等式函数与高斯势函数的等式函数组合避障策略旨在利用高斯势函数的等势函数的平滑性和超二次势函数的等势函数的形状适应性，通过动态调整二者的加权系数来适应不同的避障需求。尤其是在距障碍物不同距离时，通过自适应调整加权系数，既可以保证在远离障碍物时路径的平滑性，也可以在靠近障碍物时提供精确的避障指引。

[0030] S3、根据加权组合等势函数、机械臂当前位置和速度设置动态势函数，计算动态势函数的负梯度，将动态势函数的负梯度作为第一耦合项加入参数化的DMP模型，由此得到机械臂的可避障DMP模型，可避障DMP模型包括左机械臂的可避障DMP模型和右机械臂的可避障DMP模型。

[0031] 具体地，需要给DMP模型赋予避障功能，人工势场法是机械臂避障领域常用的一种方法，该方法通常不考虑机械臂速度或动力学属性，往往会导致非平滑的机械臂避障轨迹，因此在本方案中，根据加权组合等势函数、机械臂当前位置和速度构造动态势函数，动态势函数具体可表示如下：（10）
其中，
（10）‑1
式中， 表示动态势函数， 和 是恒定增益， 是等势函数，
为等势函数 的梯度， 为机械臂的速度 与机械臂位置相对最近障碍物的
朝向之间的夹角， 表示等势函数的梯度与机械臂的速度 之间的标准标量积，表示范数。

[0032] 将上述动态势函数（10）的负梯度作为第一耦合项，即：（11）
式中， 为第一耦合项。

[0033] 将上述第一耦合项（11）添加到参数化的DMP模型（参数化的模型和初始DMP模型的表达式相同，只是参数值不同）中，得到可避障DMP模型，使机械臂具有避障功能。

[0034] 进一步地，机械臂的可避障DMP模型具体可表示为：（12）
需要说明的是，上式可避障DMP模型包括左机械臂的可避障DMP模型和右机械臂的可避障DMP模型，具体是将公式（12）中的相关参数替换成左机械臂的对应参数，即可对应左机械臂的可避障DMP模型，将公式（12）中的相关参数替换成右机械臂的对应参数，即可对应得到右机械臂的可避障DMP模型，其中：
左机械臂的可避障DMP模型可表示为：
（12）‑1
式中， 和 分别表示左机械臂的速度和位置， 和 分别表示左机械臂可避障
的DMP模型的刚度矩阵和阻尼矩阵， 和 分别表示左机械臂的目标位置和初始位置，表示相位变量， 表示一阶导数， 表示左机械臂可避障的DMP模型的强迫项。

[0035] 右机械臂的可避障DMP模型可表示为：（12）‑2
式中， 和 分别表示右机械臂当前时刻的速度和位置, 和 分别为右机械
臂的可避障DMP模型的刚度矩阵和阻尼矩阵, 和 分别表示右机械臂的目标位置和初始位置，表示相位变量， 表示一阶导数， 为右机械臂的可避障DMP模型的强迫项。

[0036] 经过上述设置，右机械臂和右机械臂均可通过各自对应的可避障DMP模型模仿与泛化示教轨迹并具有对环境中的障碍物避障的功能。另外，将左右机械臂的相位变量设为相同，相当于两个机械臂具有相同的正则化时间。

[0037] S4、构建基于范德波尔振荡器的第二耦合项，以右机械臂的位置和速度为参考将第二耦合项添加到左机械臂的可避障DMP模型，得到左机械臂的可自主避障DMP模型，右机械臂的可避障DMP模型和左机械臂的可自主避障DMP模型共同组成双臂机器人的自主避障模型。

[0038] 具体地，由于双臂机器人在作业过程中除了每个机械臂要对周围障碍物进行避障之外，还可能出现两个机械臂之间发生碰撞的情况，为了避免在作业过程中出现两个机械臂之间发生碰撞，本实施例中首先引入范德波尔振荡器，其中，范德波尔振荡器的表达式为：（13）
然后建立基于范德波尔振荡器的第二耦合项，第二耦合项用于防止两个机械臂之间相互碰撞，第二耦合项具体可表示为：
（14）
其中， 表示第二耦合项，是用于控制振荡幅度的振荡系数。

[0039] 在上述公式（14）中，当 时就是普通的简谐振动方程；当 时就是阻尼振动，振幅逐渐衰减为0；当 时就出现自激振动,振幅逐渐增加直到达到一个稳定的值。

[0040] 针对公式（14）的第二耦合项，将 与 分别设置为右机械臂的位置与速度，并取振荡系数 ，然后将其添加到左机械臂的可避障DMP模型中，得到左机械臂的可自主避障DMP模型。通过这样的操作，当右机械臂和左机械臂之间距离过近或可能发生碰撞时，右机械臂的自激振动效果会通过双臂机器人自主避障耦合项传递给左机械臂，从而使得左机械臂偏离右机械臂的运动轨迹，避免两个机械臂之间发生碰撞。

[0041] 进一步地，S4中左机械臂可自主避障的DMP模型具体可表示为：（15）
其中，
；

[0042] 式中， 表示以右机械臂的位置和速度为参考的第二耦合项。

[0043] 左机械臂的可自主避障DMP模型具有在模仿与泛化示教轨迹的基础上对环境中的障碍物避障以及对右机械臂防止相互碰撞的功能。

[0044] 得到右机械臂的可避障DMP模型（对应公式（12）‑2）和左机械臂的可自主避障DMP模型（对应公式（15））后，将两者共同组成双臂机器人的自主避障模型。

[0045] S5、设置每个机械臂在真实场景下的起点和终点，将每个机械臂在真实场景下的起点和终点以及障碍物信息输入到双臂机器人的自主避障模型处理,输出双臂机器人可避障又能防止两个机械臂相互碰撞的运动轨迹。

[0046] 在一个实施例中，一种双臂机器人避障系统，采用上述基于加权势函数和DMP的双臂机器人避障方法进行避障，该系统包括双臂机器人以及设置在双臂机器人本体上且依次连接的图像采集单元和自主避障模型，其中：图像采集单元用于获取周围障碍物位置信息，并将障碍物位置信息发送至自主避障模型；
自主避障模型接收每个机械臂预设的初始位置和目标位置以及障碍物位置信息并处理，得到双臂机器人可避障又能防止两个机械臂相互碰撞的运动轨迹。

[0047] 具体地，自主避障模型接收每个机械臂预设的初始位置和目标位置以及障碍物位置信息后，对于每一个时间步，先使用双臂机器人的自主避障模型（右机械臂的可避障DMP模型（对应公式（12）‑2）和左机械臂的可自主避障DMP模型（对应公式（15）））计算每个机械臂在当前时间步的加速度，使用加速度更新速度，使用速度更新每个机械臂在当前时间步的位置，由此得到双臂机器人可避障又能防止两个机械臂相互碰撞的运动轨迹。

[0048] 关于一种双臂机器人避障系统的具体限定可以参见上文中对于基于加权势函数和DMP的双臂机器人避障方法的限定，在此不再赘述。

[0049] 进一步地，通过实验对本发明提出的基于加权势函数和DMP的双臂机器人避障方法进行验证。

[0050] 参见图2，图2为本发明一实施例中在二维空间示教轨迹为直线时的机械臂避障效果图，其中，图2（a）为在二维空间示教轨迹为直线时的机械臂总体避障效果图，图2（b）为在二维空间中示教轨迹为直线时在x轴和y轴的机械臂避障效果图；在图2（a）中，示教轨迹（对应图2（a）中无障碍物示教轨迹）为直线，设置与示教轨迹相同的起点与终点，并添加障碍物来测试单个机械臂的轨迹模仿与避障功能，另外，为了进行对比，除了采用本发明提出的自适应变化加权系数（对应图2（a）中的自适应权重）以外，还手动设置了两组自适应变化加权系数，分别对应图2（a）中的 和 ：当时，超二次势函数的等势函数占主导地位，对障碍物形状具有良好的适应性，但存在一些突变的避障轨迹，这对于机械臂的运动是不友好的；当 时，高斯势函数的等势函数占主导地位，避障轨迹具有很好的平滑性，但是存在与示教轨迹运动重合度低的问题，降低了DMP的性能。参见图2（b），图2（b）中分别对x轴和y轴的变化和与示教轨迹的差异进行详细展示。从图2可以看出，本发明提出方法能够结合超二次势函数的等势函数的形状适应性和高斯势函数的等势函数的平滑性，能够对障碍物形状进行良好适应性和平滑性的同时避免产生突变的、重合度低的轨迹。

[0051] 图3为本发明一实施例中在二维空间示教轨迹为曲线且障碍物较远时的机械臂避障效果图，其中，图3（a）为在二维空间示教轨迹为曲线且障碍物较远时的机械臂总体避障效果图，图3（b）为在二维空间示教轨迹为曲线且障碍物较远时在x轴和y轴的机械臂避障效果图。

[0052] 在图3（a）中，示教轨迹（对应图3（a）中无障碍物示教轨迹）为曲线且障碍物较远，同样地，设置与示教轨迹具有相同的起点与终点，并添加障碍物来测试单个机械臂的轨迹模仿与避障功能，同样地，为了进行对比，还手动设置了两组自适应变化加权系数，分别为和 。结果显示，当障碍物较远不影响机械臂的正常运动时，机械臂对示教轨迹具有良好的模仿效果。

[0053] 图4为本发明一实施例中在二维空间示教轨迹为曲线且障碍物较近时的机械臂避障效果图，其中，图4（a）为在二维空间示教轨迹为曲线且障碍物较近时的机械臂总体避障效果图，图4（b）为在二维空间示教轨迹为曲线且障碍物较近时在x轴和y轴的机械臂避障效果图。

[0054] 在图4（a）中，示教轨迹（对应图4（a）中无障碍物示教轨迹）为曲线且障碍物较近，同样地，设置与示教轨迹具有相同的起点与终点，并添加障碍物来测试单个机械臂的轨迹模仿与避障功能，另外，为了进行对比，还手动设置了两组自适应变化加权系数，分别为和 。结果表明本发明提出的方法对于曲线的示教轨迹也具有良好的模仿与避障效果。

[0055] 图5为本发明一实施例中在三维空间双臂机器人的双臂防止相互碰撞效果图，其中图5（a）为在三维空间双臂机器人的左臂目标点位于右臂运动轨迹上的某一点时双臂防止相互碰撞效果图，图5（b）为在三维空间双臂机器人的左臂目标点与右臂目标点相同时双臂防止相互碰撞效果图。

[0056] 在图5中，为了避免杂乱，未显示无障碍物环境下的示教轨迹，图5中的线条分别为右臂DMP模仿轨迹、左臂DMP模仿轨迹以及防止相互碰撞的左臂DMP模仿轨迹。其中图5（b）左臂目标点与右臂目标点相同是因为左臂和右臂的运动方向相反。结果显示，未添加双臂之间防止相互碰撞耦合项（也就是第二耦合项）的左臂DMP模仿轨迹会直接与右臂DMP模仿轨迹相重叠，也就是会产生左右臂之间的碰撞。添加了双臂之间防止相互碰撞耦合项（也就是第二耦合项）的左臂可自主避障DMP模仿轨迹会在右臂位置与速度的影响下在原位置处产生一个偏移从而避免双臂之间的碰撞。

[0057] 上述基于加权势函数和DMP的双臂机器人避障方法及系统，首先获取双臂机器人的机械臂在无障碍物环境下的示教轨迹，建立机械臂的初始DMP模型，将示教轨迹输入初始DMP模型进行学习，获取权重系数并参数化初始DMP模型，然后构建加权组合等势函数并在此基础上设置动态势函数，将动态势函数的负梯度作为第一耦合项加入参数化的DMP模型，得到机械臂的可避障DMP模型，具体包括左机械臂的可避障DMP模型和右机械臂的可避障DMP模型，接着构建基于范德波尔振荡器的第二耦合项，以右机械臂的位置和速度为参考，将第二耦合项添加到左机械臂的可避障DMP模型，得到左机械臂的可自主避障DMP模型，右机械臂的可避障DMP模型和左机械臂的可自主避障DMP模型共同组成双臂机器人的自主避障模型，最后将自主避障模型应用于真实场景中，将每个机械臂在真实场景下的起点和终点以及障碍物信息输入到双臂机器人的自主避障模型处理,输出双臂机器人可避障又能防止两个机械臂相互碰撞的运动轨迹，该方法得到的两个机械臂的运动轨迹能够避免每个机械臂与周围障碍物的碰撞以及两个机械臂相互之间的碰撞。

[0058] 以上对本发明所提供的基于加权势函数和DMP的双臂机器人避障方法及系统进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。