CN115981328A - 一种基于动态运动基元的轨迹规划与调整方法及系统 - Google Patents

Abstract

本发明属于机器人轨迹规划技术领域，公开了一种基于动态运动基元的轨迹规划与调整方法及系统，方法包括以下步骤：步骤1，获取原始路径点中包括位置和姿态信息的经验轨迹；步骤2，从经验轨迹中提取独立的位置轨迹和姿态轨迹，对独立的位置轨迹和姿态轨迹进行速度规划预处理，获得加速度层面连续平滑的位置示范轨迹和姿态示范轨迹；步骤3，构建分别面向位置和姿态的动态运动基元模型，优化位置示范轨迹和姿态示范轨迹，得到新的位置轨迹和姿态轨迹；本发明提供了一种规划效率高且难度低的基于动态运动基元的轨迹规划与调整方法。

Description

一种基于动态运动基元的轨迹规划与调整方法及系统

技术领域

本发明属于机器人轨迹规划技术领域，具体涉及一种基于动态运动基元的轨迹规划与调整方法及系统。

背景技术

随着智能制造技术的发展，机器人被广泛应用于汽车、飞机、家具等工业产品的制造中，代替工人进行例如喷漆、打磨、涂胶以及焊接等工序。机器人的工作轨迹好坏直接影响了产品加工的精度和效率。

目前对机器人目标轨迹的规划与编程大多依然停留在原始的人工示教手段。机器人的目标轨迹依赖工人根据工艺与经验针对每种产品进行独立示教与编程。然而随着产品的种类愈发多样与频繁的更新换代，生产加工对机器人轨迹规划的要求也越来越高。针对每种产品进行独立的目标轨迹规划耗时且影响生产效率，一旦产品更新又必须抛弃原来的轨迹重新规划，加之机器人应用人才的短缺，无疑给中小型企业的机器人应用推广增加了难度。

发明内容

本发明的目的在于提供一种规划效率高且难度低的基于动态运动基元的轨迹规划与调整方法及系统。

为实现上述目的，本发明采用如下技术方案：

一种基于动态运动基元的轨迹规划与调整方法，包括以下步骤：

步骤1，获取原始路径点中包括位置和姿态信息的经验轨迹；本发明的方法以过往的经验轨迹为学习样本，在经验轨迹的基础上进行复现与形状调整来满足新的工作情况，获得目标轨迹，因此首先需要获取经验轨迹的原始路径点，包括路径点的位置与姿态信息，作为方法的输入数据。

步骤2，对原始路径点进行位姿解耦，从经验轨迹中提取独立的位置轨迹和姿态轨迹，其中独立的位置轨迹以笛卡尔坐标系下路径点在x、y、z方向上的位置信息来表示；独立的姿态轨迹以四元数的表达形式表示路径点的姿态信息。对独立的位置轨迹和姿态轨迹进行速度规划预处理，获得加速度层面连续平滑的位置示范轨迹和姿态示范轨迹；由于作为复现学习示范样本的原始轨迹路径点可能来自人工示教或其他离线编程方法，提取出独立的位置轨迹和姿态轨迹可能无法达到连续平滑，进而会影响动态运动基元学习复现过程中的拟合质量，为使轨迹规划获得质量与精度满足应用的位姿轨迹(目标轨迹)，首先对获取的独立的位置轨迹和姿态轨迹进行速度规划预处理，从而获得加速度层面连续平滑的示范轨迹。

步骤3，构建分别面向位置和姿态的动态运动基元模型，优化位置示范轨迹和姿态示范轨迹，得到新的位置轨迹和姿态轨迹。

进一步的，在步骤2中，获得位置示范轨迹的方法为：针对独立的位置轨迹，以目标轨迹的期望插值点数、轨迹运行时的最大速度、最大加速度、最大急动度以及最大痉挛度为约束参数，使用15段S形曲线速度规划算法计算时间最优的平滑的位置轨迹，获得位置示范轨迹。位置示范轨迹在第n个点的表达式为：

[t_n，x_n，y_n，z_n，v_xn，v_yn，v_zn，a_xn，a_yn，a_zn]

其中，t_n表示当前位置轨迹点记录的时间，[x_n，y_n，z_n]表示当前点在三个方向上位置信息，[v_xn，v_yn，v_zn]表示当前点在三个方向上的速度信息，[a_xn，a_yn，a_zn]表示当前点在三个方向上的加速度信息。

进一步的，在步骤2中，获得姿态示范轨迹的方法为：首先计算位置示范轨迹的速度归一化值，然后以期望插值点数和位置示范轨迹的速度归一化值为约束参数，对独立的姿态轨迹进行球面线性插值规划，获得姿态示范轨迹。姿态示范轨迹在第n个点的表达式为：

[t_n，q_0n，q_1n，q_2n，q_3n，ω_xn，ω_yn，ω_zn，α_xn，α_yn，α_zn]

其中，t_n表示当前姿态轨迹点记录的时间，[q_0n，q_1n，q_2n，q_3n]表示当前点在四元数表达式下的姿态信息，[ω_xn，ω_yn，ω_zn]表示当前点在三个方向上的角速度信息，[α_xn，α_yn，α_zn]表示当前点在三个方向上的角加速度信息。

进一步的，在步骤3中，首先构建动态基元耦合项，记为C_t；然后构建面向位置的动态运动基元模型的表达式：

式中，τ为时间常量；g为轨迹目标终点；s为轨迹运动点的三维位移量；v为轨迹点速度；a为轨迹点加速度；k为模型的阻尼系数；f为一非线性强迫项。本发明通过对位置轨迹进行复现以及形状调整来重复利用相似工况的经验轨迹并满足新的工作需求。为此，需要对动态运动基元添加耦合项，根据输入参数构建动态运动基元耦合项，从而在动态运动基元复现位置轨迹的过程中施加扰动力，调整复现后的位置轨迹的形状。

进一步的，在步骤3中，构建动态基元耦合项的方法如下：

首先构建高斯径向基函数中sigmoid系统：

然后构建基于高斯径向基函数及其中sigmoid系统的初始控制参数的虚拟扰动力F_c的表达式：

式中，a_n为一组基函数的高斯分布偏度系数；b_n为高斯分布峰度系数；c_n为高斯分布缩放系数；r_n为对应每组sigmoid系统的斜率系数；d_n为对应每组sigmoid系统的中心偏移系数；μ表示所选轨迹区间内位置归一化值的平均值；σ则表示所选轨迹区间内位置归一化值的标准差；

然后构建偏转耦合项

的表达式：

式中，

表示一个基于虚拟交互力与速度方向夹角的动态偏转项，能够对动态运动基元的运动点产生偏转的扰动力，以达到轨迹特征形变的效果。R表示一个偏转矩阵；v表示运动点的速度；γ为缩放变量；θ表示为虚拟扰动力矢量与速度方向的夹角；β为一常系数；

最后构建动态基元耦合项：

进一步的，在步骤3中，优化位置示范轨迹并得到新的位置轨迹的方法为：首先，将位置示范轨迹的τ、s、v和a，约束条件，以及目标轨迹的g输入到面向位置的动态运动基元模型中；然后面向位置的动态运动基元模型提取位置示范轨迹的运动特征，然后使用非线性强迫项对运动特征进行拟合，实现对位置示范轨迹的运动特征的复现，得到复现后的位置轨迹，然后使用C_t给面向位置的动态运动基元模型提供扰动，将复现后的位置轨迹向目标轨迹的形状进行调整，得到调整后的位置轨迹，然后对高斯径向基函数控制参数以及调整后的位置轨迹进行循环优化，改变虚拟扰动力在轨迹区间内的分布，直到获得满足目标轨迹形状的新的位置轨迹，输出新的位置轨迹。

进一步的，对调整后的位置轨迹进行循环优化的方法为：根据输入的约束条件计算轨迹调整的控制点位置，包括位置轨迹开始调整的起点位置、终点位置、目标波峰位置和目标偏离距离；将调整后位置轨迹的实际波峰位置与目标波峰位置对比，若实际波峰位置早于目标波峰位置出现，则增大对应高斯分布的偏度系数a_n或峰度系数b_n，若实际波峰位置晚于目标波峰位置出现，则减小对应高斯分布的偏度系数a_n或峰度系数b_n，每次高斯分布的偏度系数a_n或峰度系数b_n增大或减小后，得到新的s、v、a，循环迭代，直到实际波峰位置与目标波峰位置重合后，确定波峰位置；然后将调整后位置轨迹的实际偏离距离和目标偏离距离进行比对，当实际偏离距离小于目标偏离距离时，增大对应高斯分布的缩放系数c_n，当实际偏离距离大于目标偏离距离时，则减小对应高斯分布的缩放系数c_n，每次高斯分布的缩放系数c_n增大或减小后，得到新的s、v、a；循环迭代，直到实际偏移距离等于目标偏移距离，得到新的位置轨迹。

进一步的，在步骤3中，面向姿态的动态运动基元模型表达式为：

式中，τ为时间常量，q_g为四元数表示的轨迹目标姿态，q为轨迹运动点的四元数姿态量，对应的，ω为轨迹点角速度，α为轨迹点的角加速度，k为模型的阻尼系数，f为一非线性强迫项。

进一步的，获得新的姿态轨迹的方法为：将姿态示范轨迹的τ、q、ω、α和目标轨迹的姿态终点q_g输入到面向姿态的动态运动基元模型中，面向姿态的动态运动基元模型提取姿态示范轨迹的运动特征，然后使用非线性强迫项对运动特征进行拟合，对姿态示范轨迹的运动特征复现，得到复现后的姿态轨迹，然后根据输入目标轨迹的q_g作为姿态轨迹终点，对复现后的姿态轨迹进行泛化学习，获得改变终点但运动特征不变的姿态轨迹，泛化后的姿态轨迹为新的姿态轨迹，输出新的姿态轨迹。

一种基于动态运动基元的轨迹规划与调整系统，包括储存模块，储存模块连接有数据处理模块，数据处理模块连接有位置轨迹规划模块和姿态轨迹规划模块；储存模块用于储存原始路径点中包括位置和姿态信息的经验轨迹；数据处理模块包括解耦单元、位置轨迹处理单元和姿态轨迹处理单元；解耦单元用于从经验轨迹中提取独立的位置轨迹和姿态轨迹；位置轨迹处理单元用于对独立的位置轨迹进行速度规划预处理，获得加速度层面连续平滑的位置示范轨迹；姿态轨迹处理单元用于对独立的姿态轨迹进行速度规划预处理，获得加速度层面连续平滑的姿态示范轨迹；位置轨迹规划模块包括位置特征提取单元、位置特征拟合单元、扰动单元和循环优化单元；位置特征提取单元用于提取位置示范轨迹的运动特征，位置特征拟合单元用于使用非线性强迫项对运动特征进行拟合，实现对位置示范轨迹的运动特征的复现，得到复现后的位置轨迹；扰动单元用于使用构建的动态基元耦合项提供扰动，将复现后的位置轨迹向目标轨迹的形状进行调整，得到调整后的位置轨迹；循环优化单元包括波峰优化器和偏离距离优化器，波峰优化器用于将调整后的位置轨迹的实际波峰位置与目标波峰位置对比，循环调整位置轨迹，使实际波峰位置与目标波峰位置重合，确定波峰位置；偏离距离优化器在波峰位置确定后用于将调整后位置轨迹的实际偏离距离和目标偏离距离进行比对，循环调整位置轨迹，使实际偏离距离和目标偏离距离相等，得到新的位置轨迹；姿态轨迹规划模块包括姿态特征提取单元、姿态特征拟合单元和泛化学习单元；姿态特征提取单元用于提取姿态示范轨迹的运动特征；姿态特征拟合单元用于使用非线性强迫项对运动特征进行拟合，对姿态示范轨迹的运动特征复现；泛化学习单元用于对复现后的姿态轨迹进行泛化学习，获得新的姿态轨迹。

与现有技术相比，本发明的有益效果如下：

1.本发明使用动态运动基元算法进行离线轨迹规划，能够对以往规划过的经验轨迹进行复现，并根据新的工作需求改变轨迹形状与终点；能够有效解决相似但细节不同的产品加工轨迹需要重复依赖人工示教的问题；大大提升经验轨迹的利用率，提高轨迹规划编程的效率。

2.本发明的方法针对轨迹的多维度位置信息与姿态信息均具有良好的适应性。通过位姿解耦的速度规划预处理，能够使学习复现后的轨迹具有优秀的连续性与平滑性，满足应用中对轨迹的质量要求。

3.本发明使用的动态运动基元算法在对轨迹进行复现、形状调整以及泛化学习时能够保留原示范轨迹的运动特征，仅针对新的工作需求对局部或轨迹终点进行调整，确保生成的轨迹在实际应用时既能保留先验知识，同时对新的工作目标具有良好的适应性。

综上，本发明对于相似结构产品的加工轨迹规划能够通过对已有的经验轨迹进行复现与形状调整来获取，与人工示教需要对每个轨迹点进行确认和编程的方法相比，本发明可以通过直接生成轨迹省去这些步骤，能够大大减少重复示教编程的工作量，简化了机器人轨迹规划难度，能够吸收经验，提高过往成功轨迹利用率的柔性离线编程，解决了机器人轨迹规划重复依赖人工示教的问题。

附图说明

图1为本发明实施例1的流程图；

图2为本发明实施例1的轨迹速度规划预处理的流程图；

图3为本发明实施例1的位置示范轨迹效果图；

图4为本发明实施例1的姿态示范轨迹效果图；

图5为本发明实施例1的复现后的位置轨迹效果图；

图6为本发明实施例1的位置轨迹特征复现与调整效果图；

图7为本发明实施例1的复现后的姿态轨迹效果图；

图8为本发明实施例1的姿态轨迹特征复现与调整效果图。

具体实施方式

实施例1

一种基于动态运动基元的轨迹规划与调整方法，如图1所示，包括以下步骤：

步骤1，获取原始路径点中包括位置和姿态信息的经验轨迹作为方法的输入数据。本实施例获取的是一条通过人工示教的机器人打磨工作轨迹作为经验轨迹，经验轨迹中包括位置路径点信息以及使用矩阵表示的姿态路径点信息。经验轨迹组成如表1所示。

表1.经验轨迹的原始路径点位置、姿态信息

步骤2，如图2所示，首先对经验轨迹上的原始路径点进行位姿解耦处理，从中分别提取出独立的位置与姿态信息，得到独立的位置轨迹和姿态轨迹。其中，独立的位置轨迹以笛卡尔坐标系下路径点在x、y、z方向上的位置信息来表示；独立的姿态轨迹以四元数的表达形式表示路径点的姿态信息，包括机器人末端对x轴、y轴以及z轴的旋转角度。

然后以目标轨迹的期望插值点数、轨迹运行时的最大速度、最大加速度、最大急动度、最大痉挛度为约束参数，使用15段S形曲线速度规划对独立的位置轨迹进行速度规划预处理，获得位置示范轨迹如图3所示；并在此基础上计算位置示范轨迹的速度归一化值。约束参数如表2所示，位置示范轨迹如表3所示。

表2. 15段S形曲线速度规划约束参数

	插值点数	最大速度	最大加速度	最大急动度	最大痉挛度
						参数值	50	60	120	200	600

表3.速度规划预处理后的位置示范轨迹

路径点顺序	<![CDATA[t,x,y,z,v<sub>x</sub>,v<sub>y</sub>,v<sub>x</sub>,a<sub>x</sub>,a<sub>y</sub>,a]]>
		...	...
150	[5.31,-1040.21,-844.68,-352.83,60.00,0,-0.67,0,0,-0.0000051]
		151	[5.35,-1038.07,-844.68,-352.85,60.00,0,-0.67,0,0,-0.0000015]
152	[5.38,-1035.93,-844.68,-352.88,60.00,0,-0.67,0,0,-0.0000004]
		...	...

然后使用相同的插值点数以及示范轨迹的速度归一化值，对独立的姿态轨迹进行球面线性插值，输出姿态示范轨迹。经过规划后输出的姿态示范轨迹如表4所示。姿态示范轨迹效果图如图4所示。

表4.速度规划预处理后的姿态示范轨迹

步骤3，获取位置示范轨迹的轨迹区间信息以及轨迹形状参数，生成动态运动基元耦合项。首先构建高斯径向基函数中sigmoid系统：

然后构建基于高斯径向基函数及其中sigmoid系统的初始控制参数的虚拟扰动力Fc的表达式：

式中，a_n为一组基函数的高斯分布偏度系数；b_n为高斯分布峰度系数；c_n为高斯分布缩放系数；r_n为对应每组sigmoid系统的斜率系数；d_n为对应每组sigmoid系统的中心偏移系数；μ表示所选轨迹区间内位置归一化值的平均值；σ则表示所选轨迹区间内位置归一化值的标准差。

以位置示范轨迹的轨迹区间信息、特征调整的目标方向、偏离距离等参数为约束条件，从控制参数参考表中选取控制每组高斯径向基函数的初始参数，使用控制变量法得到的控制参数参考表如表5所示，本实施例以位置示范轨迹为对象，对z方向上的轨迹特征进行形状调整，约束条件如表6所示。根据输入的约束条件生成的两个高斯径向基函数及其中sigmoid系统s_n的初始控制参数如表7所示。

表5.控制参数参考表

表6.耦合项输入约束条件

	轨迹区间[起点序列，终点序列]	目标方向	偏离距离
				参数	[106,236]	z方向	50

表7.高斯径向基函数及sigmoid系统初始控制参数

	<![CDATA[F<sub>1</sub>]]>	<![CDATA[F<sub>2</sub>]]>
			<![CDATA[a<sub>n</sub>]]>	0.8	1.9
<![CDATA[b<sub>n</sub>]]>	0.6	0.4
			<![CDATA[c<sub>n</sub>]]>	3.5	-2
<![CDATA[r<sub>n</sub>]]>	-20	-50
			<![CDATA[d<sub>n</sub>]]>	0.8	0.3

然后构建偏转耦合项

的表达式：

式中，R表示一个偏转矩阵；v表示运动点的速度；γ为缩放变量；θ表示为虚拟扰动力矢量与速度方向的夹角；β为一常系数；在本实施例中，偏转耦合项中缩放系数及常系数的值分别为γ＝40，β＝5/Π。

最后构建动态基元耦合项：

步骤4，构建面向位置与姿态的动态运动基元算法。

构建面向位置的动态运动基元模型，其表达式为：

式中，τ为时间常量，对应轨迹的运动时间；g为轨迹目标终点；s为轨迹运动点的三维位移量；v为轨迹点速度；a为轨迹点加速度；k＝25为模型的阻尼系数；f为一非线性强迫项。首次输入的的s、v、a为位置示范轨迹的s、v、a。

首先，将位置示范轨迹的τ、s、v和a，约束条件，以及目标轨迹的g输入到面向位置的动态运动基元模型中；然后面向位置的动态运动基元模型以位置示范轨迹作为学习样本，提取位置示范轨迹的运动特征，然后使用非线性强迫项对运动特征进行拟合，实现对位置示范轨迹的运动特征的复现，得到复现后的位置轨迹如图5，图5中黑色为位置示范轨迹，红色为复现后的位置轨迹。然后使用C_t给面向位置的动态运动基元模型提供扰动，将复现后的位置轨迹向目标轨迹的形状进行调整，得到调整后的位置轨迹。

根据输入的约束条件，对调整后的位置轨迹进行比对评价，根据比对结果对虚拟扰动力中每一组高斯径向基函数控制参数进行循环优化，改变虚拟扰动力在轨迹区间内的分布，直到获得的位置轨迹满足目标轨迹的形状，输出循环优化后的位置轨迹，具体方法如下：

根据输入的位置示范轨迹的区间信息、特征调整的目标方向和偏离距离计算轨迹调整的控制点位置，包括：位置轨迹开始调整的起点位置、终点位置以及偏离最大距离的波峰位置，将位置轨迹偏离最大距离的波峰位置记为目标波峰位置。将调整后的位置轨迹的实际偏离最大距离的波峰位置记为实际波峰位置，将实际波峰位置与目标波峰位置进行比对，若实际波峰位置早于目标波峰位置出现，则增大对应高斯分布的偏度系数a_n或峰度系数b_n，若实际波峰位置晚于目标波峰位置出现，则减小对应高斯分布的偏度系数a_n或峰度系数b_n，使实际波峰位置向目标波峰位置逼近；在本实施例中，对所使用的两组高斯分布偏度系数a₁与a₂的最小优化量为0.0125，每次高斯分布的偏度系数a_n或峰度系数b_n增大或减小后，得到新的s、v、a，循环迭代，直到实际波峰位置与目标波峰位置重合后，确定波峰位置。确定波峰位置后，将调整后的位置轨迹的实际偏离距离与目标偏离距离进行比对(目标偏离距离是自定义的输入量)，当实际偏离距离小于目标偏离距离时，根据目标偏离距离与实际偏离距离的比例关系增大对应高斯分布的缩放系数c_n(c_n乘以目标偏离距离与实际偏离距离的比例关系)，当实际偏离距离大于目标偏离距离时，则减小对应高斯分布的缩放系数c_n，使位置轨迹调整区域的偏离距离在目标方向上逼近目标偏离距离。每次高斯分布的缩放系数c_n增大或减小后，得到新的s、v、a；循环迭代，直到实际偏移距离等于目标偏移距离。此时位置轨迹形状调整区域在目标方向的波峰位置以及偏离距离满足输入的约束条件。循环优化后的参数如表8所示，获得的经过复现与特征调整的新的位置轨迹效果图如图6，图6为在z方向上进行特征形状调整后的位置轨迹效果，其中黑色为位置示范轨迹，红色复现和调整后的新的位置轨迹。最终复现和调整后的新的位置轨迹中的点位如表9所示。

表8.循环优化后高斯径向基函数及sigmoid系统控制参数

表9.复现与特征调整后的位置轨迹

路径点顺序	<![CDATA[t,x,y,z,v<sub>x</sub>,v<sub>y</sub>,v<sub>x</sub>,a<sub>x</sub>,a<sub>y</sub>,a<sub>z</sub>]]]>
		...	...
150	[5.31,-1040.41,-844.68,-316.11,60.00,0,26.27,0,0,-12.87]
		151	[5.35,-1038.27,-844.68,-315.19,60.00,0,25.48,0,0,-12.78]
152	[5.38,-1036.12,-844.68,-314.31,60.00,0,24.62,0,0,-12.54]
		...	...

构建面向姿态的动态运动基元模型，其表达式为：

式中，τ为时间常量，对应姿态示范轨迹的运动时间；q_g为四元数表示的轨迹目标姿态；q为轨迹运动点的四元数姿态量；对应的，ω为轨迹点角速度；α为轨迹点的角加速度；k＝25为模型的阻尼系数，f为一非线性强迫项。将姿态示范轨迹的τ、q、ω、α和目标轨迹的姿态终点q_g输入到面向姿态的动态运动基元模型中，面向姿态的动态运动基元模型首先以姿态示范轨迹作为学习样本，提取姿态示范轨迹的运动特征，然后使用非线性强迫项对运动特征进行拟合，实现对姿态示范轨迹的运动特征的复现，复现效果如图7，图7中黑色为姿态示范轨迹，红色为复现后的姿态轨迹。然后，根据目标轨迹工作需要，以新的目标轨迹的q_g作为轨迹终点，使动态运动基元模型对姿态示范轨迹进行泛化学习，获得改变终点但运动特征不变的姿态轨迹，也即获得新的q、ω、α，以适应新的工况。在本实施例中，输入的目标轨迹的终点姿态q_g为[0.5,0.5,0.5,0.5]，获得的经过复现与泛化学习后的姿态轨迹参考附图8中的轨迹效果，其中黑色为姿态示范轨迹，红色为复现后的姿态轨迹。轨迹中的点位如表10所示。

表10.复现与泛化学习后的姿态轨迹

本实施例首先获取经验轨迹的原始路径点，然后对经验轨迹进行位姿解耦与速度规划预处理，再根据轨迹目标构建位置动态运动基元耦合项，最后通过面向位置与姿态的动态运动基元模型对轨迹进行复现、特征调整与泛化学习。本实施例通过使用动态运动基元算法进行离线轨迹规划，对先验轨迹进行复现，并根据新的工作需求改变轨迹形状与终点，解决了相似但细节不同的产品加工轨迹需要重复依赖人工示教的问题，大大提升先验轨迹的利用率，提高轨迹规划编程的效率。本实施例通过位姿解耦的速度规划预处理，使学习复现后的轨迹具有优秀的连续性与平滑性，且对轨迹的多维度位置信息与姿态信息均具有良好的适应性，能够满足应用中对轨迹的质量要求。本实施例使用的动态运动基元算法在对轨迹进行复现、形状调整以及泛化学习时保留了原示范轨迹的运动特征，仅针对新的工作需求对局部或轨迹终点进行调整。生成的轨迹在实际应用时既能保留先验知识，同时对新的工作目标具有良好的适应性。

实施例2

一种基于动态运动基元的轨迹规划与调整系统，包括储存模块，储存模块连接有数据处理模块，数据处理模块连接有位置轨迹规划模块和姿态轨迹规划模块。储存模块用于储存原始路径点中包括位置和姿态信息的经验轨迹。

数据处理模块包括解耦单元、位置轨迹处理单元和姿态轨迹处理单元；解耦单元用于从经验轨迹中提取独立的位置轨迹和姿态轨迹；位置轨迹处理单元用于对独立的位置轨迹进行速度规划预处理，获得加速度层面连续平滑的位置示范轨迹；姿态轨迹处理单元用于对独立的姿态轨迹进行速度规划预处理，获得加速度层面连续平滑的姿态示范轨迹。

位置轨迹规划模块包括位置特征提取单元、位置特征拟合单元、扰动单元和循环优化单元；位置特征提取单元用于提取位置示范轨迹的运动特征，位置特征拟合单元用于使用非线性强迫项对运动特征进行拟合，实现对位置示范轨迹的运动特征的复现，得到复现后的位置轨迹；扰动单元用于使用构建的动态基元耦合项提供扰动，将复现后的位置轨迹向目标轨迹的形状进行调整，得到调整后的位置轨迹。

循环优化单元包括波峰优化器和偏离距离优化器，波峰优化器用于将调整后的位置轨迹的实际波峰位置与目标波峰位置对比，循环调整位置轨迹，使实际波峰位置与目标波峰位置重合，确定波峰位置；偏离距离优化器在波峰位置确定后用于将调整后位置轨迹的实际偏离距离和目标偏离距离进行比对，循环调整位置轨迹，使实际偏离距离和目标偏离距离相等，得到新的位置轨迹。

姿态轨迹规划模块包括姿态特征提取单元、姿态特征拟合单元和泛化学习单元；姿态特征提取单元用于提取姿态示范轨迹的运动特征；姿态特征拟合单元用于使用非线性强迫项对运动特征进行拟合，对姿态示范轨迹的运动特征复现；泛化学习单元用于对复现后的姿态轨迹进行泛化学习，获得新的姿态轨迹。

Claims (10)

1.一种基于动态运动基元的轨迹规划与调整方法，其特征在于，包括以下步骤：

步骤1，获取原始路径点中包括位置和姿态信息的经验轨迹；

步骤2，从经验轨迹中提取独立的位置轨迹和姿态轨迹，对独立的位置轨迹和姿态轨迹进行速度规划预处理，获得加速度层面连续平滑的位置示范轨迹和姿态示范轨迹；

步骤3，构建分别面向位置和姿态的动态运动基元模型，优化位置示范轨迹和姿态示范轨迹，得到新的位置轨迹和姿态轨迹。

2.如权利要求1所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，在步骤2中，获得位置示范轨迹的方法为：针对独立的位置轨迹，以目标轨迹的期望插值点数、轨迹运行时的最大速度、最大加速度、最大急动度以及最大痉挛度为约束参数，使用15段S形曲线速度规划算法计算时间最优的平滑的位置轨迹，获得位置示范轨迹。

3.如权利要求1或2所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，在步骤2中，获得姿态示范轨迹的方法为：首先计算位置示范轨迹的速度归一化值，然后以期望插值点数和位置示范轨迹的速度归一化值为约束参数，对独立的姿态轨迹进行球面线性插值规划，获得姿态示范轨迹。

4.如权利要求3所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，在步骤3中，首先构建动态基元耦合项，记为C_t；然后构建面向位置的动态运动基元模型的表达式：

式中，τ为时间常量；g为轨迹目标终点；s为轨迹运动点的三维位移量；v为轨迹点速度；a为轨迹点加速度；k为模型的阻尼系数；f为一非线性强迫项。

5.如权利要求4所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，在步骤3中，构建动态基元耦合项的方法如下：

首先构建高斯径向基函数中sigmoid系统：

然后构建基于高斯径向基函数及其中sigmoid系统的初始控制参数的虚拟扰动力F_c的表达式：

式中，a_n为一组基函数的高斯分布偏度系数；b_n为高斯分布峰度系数；c_n为高斯分布缩放系数；r_n为对应每组sigmoid系统的斜率系数；d_n为对应每组sigmoid系统的中心偏移系数；μ表示所选轨迹区间内位置归一化值的平均值；σ则表示所选轨迹区间内位置归一化值的标准差；

然后构建偏转耦合项

的表达式：

式中，R表示一个偏转矩阵；v表示运动点的速度；γ为缩放变量；θ表示为虚拟扰动力矢量与速度方向的夹角；β为一常系数；

最后构建动态基元耦合项：

6.如权利要求5所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，在步骤3中，优化位置示范轨迹并得到新的位置轨迹的方法为：首先，将位置示范轨迹的τ、s、v和a，约束条件，以及目标轨迹的终点g输入到面向位置的动态运动基元模型中；然后面向位置的动态运动基元模型提取位置示范轨迹的运动特征，使用非线性强迫项对运动特征进行拟合，实现对位置示范轨迹的运动特征的复现，得到复现后的位置轨迹，然后使用C_t给模型提供扰动，将复现后的位置轨迹向目标轨迹的形状进行调整，得到调整后的位置轨迹，然后对调整后的位置轨迹进行循环优化，输出新的位置轨迹。

7.如权利要求6所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，对调整后的位置轨迹进行循环优化的方法为：根据输入的约束条件计算轨迹调整的控制点位置，包括位置轨迹开始调整的起点位置、终点位置、目标波峰位置和目标偏离距离；将调整后位置轨迹的实际波峰位置与目标波峰位置对比，若实际波峰位置早于目标波峰位置出现，则增大对应高斯分布的偏度系数a_n或峰度系数b_n，若实际波峰位置晚于目标波峰位置出现，则减小对应高斯分布的偏度系数a_n或峰度系数b_n，每次高斯分布的偏度系数a_n或峰度系数b_n增大或减小后，得到新的s、v、a，循环迭代，直到实际波峰位置与目标波峰位置重合后，确定波峰位置；然后将调整后位置轨迹的实际偏离距离和目标偏离距离进行比对，当实际偏离距离小于目标偏离距离时，增大对应高斯分布的缩放系数c_n，当实际偏离距离大于目标偏离距离时，则减小对应高斯分布的缩放系数c_n，每次高斯分布的缩放系数c_n增大或减小后，得到新的s、v、a；循环迭代，直到实际偏移距离等于目标偏移距离，得到新的位置轨迹。

8.如权利要求3所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，在步骤3中，面向姿态的动态运动基元模型表达式为：

式中，τ为时间常量，q_g为四元数表示的轨迹目标姿态，q为轨迹运动点的四元数姿态量，对应的，ω为轨迹点角速度，α为轨迹点的角加速度，k为模型的阻尼系数，f为一非线性强迫项。

9.如权利要求8所述的基于动态运动基元的轨迹规划与调整方法，其特征在于，获得新的姿态轨迹的方法为：将姿态示范轨迹的τ、q、ω、α和目标轨迹的姿态终点q_g输入到面向姿态的动态运动基元模型中，面向姿态的动态运动基元模型首先提取姿态示范轨迹的运动特征，使用非线性强迫项对运动特征进行拟合，对姿态示范轨迹的运动特征复现，然后根据输入目标轨迹的q_g作为姿态轨迹终点，对复现后的姿态轨迹进行泛化学习，获得新的姿态轨迹。

10.一种基于动态运动基元的轨迹规划与调整系统，其特征在于，包括储存模块，所述储存模块连接有数据处理模块，所述数据处理模块连接有位置轨迹规划模块和姿态轨迹规划模块；所述储存模块用于储存原始路径点中包括位置和姿态信息的经验轨迹；所述数据处理模块包括解耦单元、位置轨迹处理单元和姿态轨迹处理单元；所述解耦单元用于从经验轨迹中提取独立的位置轨迹和姿态轨迹；所述位置轨迹处理单元用于对独立的位置轨迹进行速度规划预处理，获得加速度层面连续平滑的位置示范轨迹；所述姿态轨迹处理单元用于对独立的姿态轨迹进行速度规划预处理，获得加速度层面连续平滑的姿态示范轨迹；所述位置轨迹规划模块包括位置特征提取单元、位置特征拟合单元、扰动单元和循环优化单元；所述位置特征提取单元用于提取位置示范轨迹的运动特征，所述位置特征拟合单元用于使用非线性强迫项对运动特征进行拟合，实现对位置示范轨迹的运动特征的复现，得到复现后的位置轨迹；所述扰动单元用于使用构建的动态基元耦合项提供扰动，将复现后的位置轨迹向目标轨迹的形状进行调整，得到调整后的位置轨迹；所述循环优化单元包括波峰优化器和偏离距离优化器，所述波峰优化器用于将调整后的位置轨迹的实际波峰位置与目标波峰位置对比，循环调整位置轨迹，使实际波峰位置与目标波峰位置重合，确定波峰位置；所述偏离距离优化器在波峰位置确定后用于将调整后位置轨迹的实际偏离距离和目标偏离距离进行比对，循环调整位置轨迹，使实际偏离距离和目标偏离距离相等，得到新的位置轨迹；所述姿态轨迹规划模块包括姿态特征提取单元、姿态特征拟合单元和泛化学习单元；所述姿态特征提取单元用于提取姿态示范轨迹的运动特征；所述姿态特征拟合单元用于使用非线性强迫项对运动特征进行拟合，对姿态示范轨迹的运动特征复现；所述泛化学习单元用于对复现后的姿态轨迹进行泛化学习，获得新的姿态轨迹。

Images