CN118143968A - 一种工业机器人动力学参数复合辨识方法 - Google Patents

Abstract

本发明公开了一种工业机器人动力学参数复合辨识方法，其包括：建立工业机器人动力学模型并进行线性化处理；采用五阶傅里叶级数作为工业机器人的激励轨迹；控制工业机器人按设计的激励轨迹运动，并对关节力矩和关节角度信息进行采样，得到观测力矩和观测矩阵；基于工业机器人动力学方程与观测数据，使用最小二乘法辨识得到工业机器人的动力学参数；通过BP神经网络对摩擦力矩进行补偿。本发明将激励轨迹设计为五阶傅里叶级数，有效降低了噪声等干扰因素对观测数据集的影响，极大提高了观测数据集精度；通过训练BP神经网络得到的摩擦模型对摩擦力矩进行补偿，有效减小了预测摩擦力矩的偏差，提高了工业机器人动力学参数辨识准确性。

Description

一种工业机器人动力学参数复合辨识方法

技术领域

本发明涉及工业机器人技术领域，特别涉及一种工业机器人动力学参数的辨识方法。

背景技术

工业机器人在多个行业得到了广泛应用，极大提高了生产力，随着科技的发展，人们对工业机器人的性能要求更高，要求其实现功能更复杂，传统的PID控制已经无法满足对工业机器人进行高精度控制的需求。因此，基于工业机器人机理模型来实现高精度控制的方法将成为主流。基于模型的工业机器人控制器以机器人动力学模型作为先验值，但是工业机器人是一种多变量、强耦合的非线性系统，通过数学计算获得动力学模型很困难。

目前，常用辨识方法先通过牛顿-欧拉法进行动力学建模，创建机器人的动力学模型，然后通过参数重组得到可辨识的最小参数集，并设计最优激励轨迹得到一组有效输入输出数据用于机械臂的动力参数辨识，通过合理的参数辨识方法对工业机器人进行参数辨识，获得工业机器人的准确机理模型，最后通过BP神经网络对摩擦力矩进行有效补偿，提高输出力矩的预测精度，从而为后续对工业机器人高精度控制研究的开展提供有力保障。

目前，国内外已有大量关于工业机器人动力学参数辨识的研究，提出了多种有效的工业机器人参数辨识方法。

祖丽楠等人在申请号为202111654616.7,名称为“一种基于遗传算法优化的神经网络机械臂动力学建模方法”的专利申请中提出了一种基于神经网络的机械臂网络模型建模方法，该方法通过一种基于遗传算法优化的神经网络建立了机械臂网络模型，所建网络模型机准确反映了机械臂的关节运动变量与输出力矩间的关系。但该方法不考虑辨识对象内部结构，而是将整个系统看作一个“黑箱”，无法得到所辨识系统内部具体动力学参数，因此依靠该方法无法得到工业机器人准确的机理模型来实现对工业机器人的高精度控制。

在题为“工业机器人动力学参数辨识及软件系统开发”的文章中，作者屈艺丹通过牛顿-欧拉法对工业机器人进行动力学建模，选择有穷傅里叶级数做为最优激励轨迹的表达式，通过最小化退化矩阵的条件数来获得最优激励轨迹中的参数，同时把各关节位置、速度和加速度的物理约束与激励轨迹结合起来，使获得的最优激励轨迹在物理意义上是可行的，通过在激励轨迹中指定频率范围来避免激励机器人的柔性特性，最后利用最小二乘法实现了机械臂动力学参数辨识。该方法能够在观测数据集正确的情况下，实现工业机器人的最小动力学参数集辨识，但由于最小二乘法自身的特点，以及机器人摩擦力模型的复杂与非线性，摩擦力的输出力矩存在不可避免的动态误差，当观测数据集中存在异常数据时，辨识参数精度会大大降低，导致所建立动力学模型不准确。

在题为“基于免疫算法和神经网络的机械臂动力学参数辨识与验证”的文章中，作者泮求亮针对传统激励轨迹设计以傅里叶级数形式展开，会导致机械臂启停过程中产生抖动，导致跟随轨迹精度降低的问题，通过免疫克隆算法优化，得到一条首尾封闭，抗噪声能力强的较优激励轨迹，并通过神经网络来辨识动力学参数，相较于传统最小二乘辨识方法，该方法有效提高了工业机器人动力学参数辨识的准确性和可靠性。但该方法在进行工业机器人参数辨识时采用的神经网络过于简单，仅设置一层隐藏层，导致所用网络性能较差，可能出现训练过程收敛慢甚至不收敛的问题。

发明内容

有鉴于此，本发明的目的是提供一种工业机器人动力学参数复合辨识方法，以解决工业机器人关键动力学参数辨识问题，帮助准确建立工业机器人机理模型，为后续开展工业机器人高精度控制打下良好基础。

本发明工业机器人动力学参数复合辨识方法包括：

步骤一：通过牛顿-欧拉法建立工业机器人动力学模型，再将工业机器人动力学模型线性化，得到工业机器人动力学方程的线性分离形式：

其中：τ表示工业机器人各个关节的输出力矩，为工业机器人各个关节当前角度q、角速度角加速度组成的系数矩阵，p为可辨识的参数集；

其中：下标j表示工业机器人的自由度，p是由p₁到p_j组成的列向量； 为工业机器人的连杆i的惯性张量集合，m_j、r_jx、r_jy、r_jz为连杆i的质量与质心分量，I_aj为关节i的电机等效转动惯量，f_cj、f_vj为库伦摩擦力系数和粘滞摩擦力系数；

步骤二：采用五阶傅里叶级数作为工业机器人的激励轨迹：

其中：q₀表示激励轨迹的补偿量，为常数；N＝5表示傅里叶级数的阶次；a_i,b_i分别表示傅里叶级数中正弦项、余弦项的幅值；ω_f表示傅里叶级数的频率；k为周期采样数，T_s为采样周期；

步骤三：控制工业机器人按步骤二设计的激励轨迹运动，并对工业机器人在不同时刻的关节力矩和关节角度信息进行采样，得到：

Γ_n＝W_n·p (4)

其中表示观测力矩，观测矩阵中为第n个采样点的系数矩阵；

对上述n阶非齐次方程组Γ_n＝W_n·p，考虑观测矩阵和输出力矩的扰动δW_n和δΓ_n，得到添加扰动后的方程：

(Γ_n+δΓ_n)＝(W_n+δW_n)·(p+δp) (5)

其中δp为最终辨识结果中的误差；

步骤四：基于步骤一建立的工业机器人动力学方程与步骤三产生的观测数据，使用最小二乘法辨识得到工业机器人的动力学参数：

p＝(Y^TY)^-1Y^TT (6)；

其中：观测矩阵观测响应下标m表示数据点的个数；

步骤五：通过BP神经网络对摩擦力矩进行补偿，包括：

对步骤四辨识后得到的结果，计算实际力矩与由最小二乘法辨识得到的结果预测的输出力矩之差，以此来估计摩擦力的大小：

其中：W为各关节输入激励轨迹得到的观测矩阵，为最小参数集合中不包含摩擦力参数的常规参数集合，实际力矩的表达式如下Γ：

其中：为最小参数集合中的摩擦力参数集合，ε为误差变量；

以作为BP神经网络的输入，以作为BP神经网络训练数据的期望输出，对BP神经网络进行训练；其中为连杆i的第k个采样点的加速度，为连杆i的第k个采样点使用初步辨识得到的摩擦力参数，通过库伦-粘滞摩擦力模型求得的预测输出摩擦力，sign()表示符号函数；

训练结束后，将给定验证轨迹的速度符号函数使用辨识结果求得的摩擦力作为训练好的BP神经网络的输入，得到验证输出Γ_f；使用该验证输出代替摩擦力矩与预测力矩相加来得到最终的预测力矩

进一步，在步骤三控制工业机器人按步骤二设计的激励轨迹运动中，对工业机器人设置如下约束条件：

条件一：

条件二：

条件三：

τ_i,min≤τ_i≤τ_i,max (11)

条件四：

其中：q_i,max，q_i,min分别表示工业机器人连杆i的关节角度上限与角度下限；表示连杆i的关节角速度上限；表示连杆i的关节角加速度上限；q_i,init表示连杆i的初始关节角度；T为傅里叶级数周期；τ_i,max，τ_i,min为连杆i允许的驱动力矩最大最小值；{s(q(t))}表示工业机器人t时刻的末端运动位置；S表示工业机器人的运动空间。

本发明的有益效果：

1、本发明将激励轨迹设计为五阶傅里叶级数，该方法能为工业机器人提供合适的激励轨迹参数。利用傅里叶级数周期性性质，通过多周期采样再平均的方法，有效降低了噪声等干扰因素对观测数据集的影响，极大提高了观测数据集精度。

2、相较于传统最小二乘回归参数辨识法受异常数据干扰大，稳定性差；一般线性摩擦力模型辨识对摩擦力矩表述的不准确问题和非线性摩擦力模型辨识时带来的不确定性误差等问题，本发明通过训练BP神经网络得到的摩擦模型对摩擦参数进行补充，该方法有效减小了预测摩擦力矩的偏差，提高了工业机器人动力学参数辨识准确性，为后续开展工业机器人高精度控制研究打下了良好基础。

3、本发明针对传统激励轨迹设计中未考虑工业机器人激励过程中发生抖动，而缺少必要约束条件，导致激励过程中抖动严重的现象。在约束条件中增加了初始与周期末机器人关节速度与加速度限制，有效减少了激励过程中机器人的抖动。

附图说明

图1为工业机器人的部分连杆连接示意图；

图2为工业机器人连杆定义示意图；

图3为和的定义的示意图；

图4为常见单变量系统图；

图5为BP神经网络结构示意图；

图6为工业机器人动力学参数复合辨识流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步描述。

本实施例中工业机器人动力学参数复合辨识方法包括以下步骤：

步骤一：通过牛顿-欧拉法建立工业机器人动力学模型：

以六关节轴工业机器人为例，图1为六关节轴工业机器人的部分连杆连接图，在各连杆的关节点建立相应坐标系，通过DH建模法得到各相邻坐标系间的转换矩阵，从而实现工业机器人系统中各连杆间的运动学关联。

由牛顿第三定律可知，工业机器人连杆i质心所受合力f_ic为：

f_ic＝m_ia_ic (1)

其中：m为连杆质量；a_ic为连杆i质心处线加速度。

由欧拉方程可知，工业机器人连杆i质心所受合力矩^cn_ic为：

其中：^cI_i为工业机器人连杆i以质心为原点所建立坐标系{c}中的惯性张量；ω_i为工业机器人连杆i相对于基坐标系的角速度；为工业机器人连杆i相对于基坐标系的角加速度。

通过牛顿欧拉法进行正向递推，得到工业机器人第i个连杆相对于坐标系i的角速度ⁱω_i，角加速度线加速度ⁱa_i与i-1杆间的迭代关系。

角速度ⁱω_i迭代关系：

角加速度迭代关系：

线加速度ⁱa_i迭代关系：

其中：表示i-1坐标系与i坐标系间的旋转矩阵，说明了两坐标系之间的转化关系；表示工业机器人连杆i的关节角速度；表示到坐标系Z轴方向的投影；表示工业机器人连杆i的关节角加速度；^i-1P_i表示从坐标系i-1原点到坐标系i原点的向量。

通过牛顿欧拉法进行反向递推，可知工业机器人连杆i关节处合力矩ⁱn_i与连杆i+1处合力矩ⁱ⁺¹n_i+1间迭代关系：

将所得合力矩ⁱn_i投影到坐标系i中的Z轴方向，可得工业机器人连杆i的驱动力矩τ_i：

联立式(1)-(7)，建立机器人的动力学方程：

其中：τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T为机器人关节驱动力矩矢量，q＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]为关节角度矢量，为关节角速度矢量，为关节角加速度矢量。

M(q)为工业机器人的惯性矩阵，具体表示为如下：

其中⁰A_k为系k到系0的变换矩阵，J_i为连杆i的质量分布常矩阵。

为机器人的科氏力及离心力矩阵，具体表现为：

其中中各分量均为广义速度的二次型，其中含的项为离心力，含的项为哥氏力，C_i＝[r_x r_y r_z]^T为连杆i的质心。

G(q)为重力项，其中g_i为重力加速度矢量在连杆i坐标系中的表达式。

式(8)所述动力学模型是未考虑摩擦力作用的理想模型，采用库伦——粘滞摩擦模型，加入摩擦力矩项τ_f，具体表现为：

其中：f_c为库伦摩擦系数矩阵；f_v为粘性摩擦系数矩阵。

由此建立完整工业机器人动力学模型：

上式可知驱动力矩τ的四项分量均与工业机器人的连杆角度q，角速度角加速度有关，且均包含工业机器人系统所有惯性参数，因此可将工业机器人动力学模型线性化。将工业机器人动力学模型线性化的过程如下：

如图2所示的工业机器人连杆，在牛顿欧拉法的前向递归中，主要计算连杆角度、角加速度、质心加速度等，进而计算连杆的惯性力和力矩，具体过程如下：

其中为坐标系的原点加速度；为从坐标系O_i到坐标系O_i+1的旋转矩阵；ω_i和为坐标系O_i下连杆的角速度和角加速度；和为连杆i+1的角速度和角加速度标量；p_i为从坐标系O_i-1原点到坐标系O_i原点的向量在坐标系O_i-1下的表达式；z₀为常矢量[0 0 1]^T；m_i+1为第i+1个连杆的质量；r_i+1为第i+1个连杆的质心位置在坐标系O_i下的表达式，为常向量；F_i+1和N_i+1为运动产生的力和力矩，I_ai+1为第关节i+1的电机等效转动惯量。

在后向递归中，主要计算关节力、力矩，具体过程如下：

n_i和f_i+1为连杆i-1对连杆i传递的力矩和力，为矢量；而τ_i为节轴的驱动力矩，为标量。将式中的力和力矩的参考坐标系从i+1转移到i，矢量力矩变为：

对等式右边的递归量进行展开，得到：

和的具体表达式为：

结合惯性张量集合的定义以及叉乘算子：

l(I)＝[I_xx I_xy I_xz I_yy I_yz I_zz]^T (21)

有：

式中，即连杆i+1相对于连杆坐标系的转动惯量。

由式(23)和(24)可以看出，关于模型参数m_i+1r_i+1、m_i+1成线性关系，关于模型参数m_i+1r_i+1成线性关系，因此可以推出关节驱动力矩τ_i关于这些模型参数也成线性关系。

令因此将其带入式可得到机器人动力学方程的线性分离形式

其中：τ表示工业机器人各个关节的输出力矩，为工业机器人各个关节当前角度q、角速度角加速度组成的系数矩阵，p为可辨识的参数集。

考虑关节摩擦力时，将摩擦力参数f_c和f_v并入最小参数集合中，得到

其中：下标j表示工业机器人的自由度，p是由p₁到p_j组成的列向量； 为工业机器人的连杆i的惯性张量集合，m_j、r_jx、r_jy、r_jz为连杆i的质量与质心分量，I_aj为关节i的电机等效转动惯量，f_cj、f_vj为库伦摩擦力系数和粘滞摩擦力系数。

步骤二：采用五阶傅里叶级数作为工业机器人的激励轨迹：

其中：q₀表示激励轨迹的补偿量，为常数；N＝5表示傅里叶级数的阶次；a_i,b_i分别表示傅里叶级数中正弦项、余弦项的幅值；ω_f表示傅里叶级数的频率；k为周期采样数，T_s为采样周期。

只有设计出合适的激励轨迹，才能得到正确的输入得到输出力矩数据，从而提高参数辨识精度。本步骤将激励轨迹设计为五阶傅里叶级数，能为工业机器人提供合适的激励轨迹参数。利用傅里叶级数周期性性质，通过多周期采样再平均的方法，能有效降低了噪声等干扰因素对观测数据集的影响，提高观测数据集精度。

步骤三：控制工业机器人按步骤二设计的激励轨迹运动，并对工业机器人在不同时刻的关节力矩和关节角度信息进行采样，得到：

Γ_n＝W_n·p (28)

其中表示观测力矩，观测矩阵中为第n个采样点的系数矩阵；

对上述n阶非齐次方程组Γ_n＝W_n·p，考虑观测矩阵和输出力矩的扰动δW_n和δΓ_n，得到添加扰动后的方程：

(Γ_n+δΓ_n)＝(W_n+δW_n)·(p+δp) (29)

其中δp为最终辨识结果中的误差。

下面分别分析δW_n和δΓ_n对辨识误差的影响。

1)δW_n＝0时，有(Γ_n+δΓ_n)＝W_n·(p+δp)，由Γ_n＝W_n·p，得到

δΓ_n＝W_n·δp (30)

对上述两式分别求范数

联立得到定义观测矩阵条件数Cond(W_n)＝||W_n ^-1||||W_n||，有

可以看出，观测矩阵的条件数越小，动力学参数的相对误差受到输出力矩的扰动的影响越小。

2)当δΓ_n＝0时，有Γ_n＝(W_n+δW_n)·(p+δp)，由Γ_n＝W_n·p，得到

δp＝-W_n ^-1·δW_n·(p+δp) (31)

定义观测矩阵条件数Cond(W_n)＝||W_n ^-1||||W_n||，有

观测矩阵的条件数越小，动力学参数的相对误差受到观测矩阵的扰动的影响越小。

由式(32)可知，辨识参数偏差大小取决于观测矩阵W_n条件数，为保证辨识参数准确率，需在设计激励轨迹时合理设置傅里叶级数参数θ₀，a_i，b_i使Cond(W_n)在符合约束条件的情况下尽可能小。

由于工业机器人需要符合一定约束条件，才能保证系统正常运行，因此作为对本步骤的进一步改进，在控制工业机器人按设计的激励轨迹运动时对工业机器人设置如下约束条件：

条件一：

条件二：

条件三：

τ_i,min≤τ_i≤τ_i,max (35)

条件四：

其中：q_i,max，q_i,min分别表示工业机器人连杆i的关节角度上限与角度下限；表示连杆i的关节角速度上限；表示连杆i的关节角加速度上限；q_i,init表示连杆i的初始关节角度；T为傅里叶级数周期；τ_i,max，τ_i,min为连杆i允许的驱动力矩最大最小值；{s(q(t))}表示工业机器人t时刻的末端运动位置；S表示工业机器人的运动空间。

对激励轨迹的求解可以表述为：

步骤四：基于步骤一建立的工业机器人动力学方程与步骤三产生的观测数据，使用最小二乘法辨识得到工业机器人的动力学参数：

p＝(Y^TY)^-1Y^TT (38)；

其中：观测矩阵观测响应下标m表示数据点的个数。

最小二乘法辨识的原理介绍如下：

最小二乘法是一种常用的线性回归方法，它通常用于数学拟合以及模型参数估计等领域。它的推导过程可以通过计算残差平方和的最小值来进行。假定常见单变量系统如图4所示，图4中n(k)是随机噪声，需要辨识的系统模型如下：

A(z^-1)z(k)＝B(z^-1)u(k)+n(k)

模型的差分方程形式为：

z(k)+a₁z(k-1)+...+a_nz(k-n)＝b₁z(k-1)+...+b_mz(k-m)+n(k)

定义：h^T(k)＝[-z(k-1),...,-z(k-n),u(k-1),...,u(k-m)]，θ^T＝[a₁,...,a_n,b₁,...,b_m]；

则z(k)＝h^T(k)·θ+n(k)，Z(L)＝[z(1) … z(L)]^T，N(L)＝[n(1) … n(L)]^T，

数学模型的矩阵表达式为Z_L＝H_Lθ+N_L，则参数θ的最小二乘估计量为

步骤五：通过BP神经网络对摩擦力矩进行补偿，包括：

对步骤四辨识后得到的结果，计算实际力矩与由最小二乘法辨识得到的结果预测的输出力矩之差，以此来估计摩擦力的大小：

其中：W为各关节输入激励轨迹得到的观测矩阵，为最小参数集合中不包含摩擦力参数的常规参数集合，实际力矩的表达式如下Γ：

其中：为最小参数集合中的摩擦力参数集合，ε为误差变量；实际力矩在具体实施中由传感器测量得到。

以作为BP神经网络的输入，以作为BP神经网络训练数据的期望输出，其中为连杆i的第k个采样点的加速度，为连杆i的第k个采样点使用初步辨识得到的摩擦力参数，通过库伦-粘滞摩擦力模型(如公式12所示)求得的预测输出摩擦力，sign()表示符号函数；对BP神经网络进行训练。对于给定训练集设定学习率为η的BP神经网络，训练过程包括：

②初始化BP神经网络中所有连接权w_hj、w_qj和阈值θ_j、γ_h；

②进入迭代训练：1)根据当前参数计算当前样本的输出；2)根据式(41)计算输出层神经元的梯度项g_j；3)根据式(42)计算隐层神经元的梯度项e_h；4)根据式(43)更新连接权w_hj、w_qj与阈值θ_j、γ_h。

③训练完成后输出连接权和阈值确定的多层前馈BP神经网络。

训练结束后，将给定验证轨迹的速度符号函数使用辨识结果求得的摩擦力作为训练好的BP神经网络的输入，得到验证输出Γ_f；使用该验证输出代替摩擦力矩与预测力矩相加来得到最终的预测力矩

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种工业机器人动力学参数复合辨识方法，其特征在于：包括：

步骤一：通过牛顿-欧拉法建立工业机器人动力学模型，再将工业机器人动力学模型线性化，得到工业机器人动力学方程的线性分离形式：

其中：τ表示工业机器人各个关节的输出力矩，为工业机器人各个关节当前角度q、角速度角加速度组成的系数矩阵，p为可辨识的参数集；

其中：下标j表示工业机器人的自由度，p是由p₁到p_j组成的列向量； 为工业机器人的连杆i的惯性张量集合，m_j、r_jx、r_jy、r_jz为连杆i的质量与质心分量，I_aj为关节i的电机等效转动惯量，f_cj、f_vj为库伦摩擦力系数和粘滞摩擦力系数；

步骤二：采用五阶傅里叶级数作为工业机器人的激励轨迹：

其中：q₀表示激励轨迹的补偿量，为常数；N＝5表示傅里叶级数的阶次；a_i,b_i分别表示傅里叶级数中正弦项、余弦项的幅值；ω_f表示傅里叶级数的频率；k为周期采样数，T_s为采样周期；

步骤三：控制工业机器人按步骤二设计的激励轨迹运动，并对工业机器人在不同时刻的关节力矩和关节角度信息进行采样，得到：

Γ_n＝W_n·p (4)

其中表示观测力矩，观测矩阵中为第n个采样点的系数矩阵；

对上述n阶非齐次方程组Γ_n＝W_n·p，考虑观测矩阵和输出力矩的扰动δW_n和δΓ_n，得到添加扰动后的方程：

(Γ_n+δΓ_n)＝(W_n+δW_n)·(p+δp) (5)其中δp为最终辨识结果中的误差；

步骤四：基于步骤一建立的工业机器人动力学方程与步骤三产生的观测数据，使用最小二乘法辨识得到工业机器人的动力学参数：

p＝(Y^TY)^-1Y^TT (6)

其中：观测矩阵观测响应下标m表示数据点的个数；

步骤五：通过BP神经网络对摩擦力矩进行补偿，包括：

对步骤四辨识后得到的结果，计算实际力矩与由最小二乘法辨识得到的结果预测的输出力矩之差，以此来估计摩擦力的大小：

其中：W为各关节输入激励轨迹得到的观测矩阵，为最小参数集合中不包含摩擦力参数的常规参数集合，实际力矩的表达式如下Γ：

其中：为最小参数集合中的摩擦力参数集合，ε为误差变量；

以作为BP神经网络的输入，以作为BP神经网络训练数据的期望输出，对BP神经网络进行训练；其中为连杆i的第k个采样点的加速度，为连杆i的第k个采样点使用初步辨识得到的摩擦力参数，通过库伦-粘滞摩擦力模型求得的预测输出摩擦力，sign()表示符号函数；

训练结束后，将给定验证轨迹的速度符号函数使用辨识结果求得的摩擦力作为训练好的BP神经网络的输入，得到验证输出Γ_f；使用该验证输出代替摩擦力矩与预测力矩相加来得到最终的预测力矩

2.根据权利要求1所述的工业机器人动力学参数复合辨识方法，其特征在于：在步骤三控制工业机器人按步骤二设计的激励轨迹运动中，对工业机器人设置如下约束条件：

条件一：

条件二：

条件三：

τ_i,min≤τ_i≤τ_i,max (11)

条件四：

其中：q_i,max，q_i,min分别表示工业机器人连杆i的关节角度上限与角度下限；表示连杆i的关节角速度上限；表示连杆i的关节角加速度上限；q_i,init表示连杆i的初始关节角度；T为傅里叶级数周期；τ_i,max，τ_i,min为连杆i允许的驱动力矩最大最小值；{s(q(t))}表示工业机器人t时刻的末端运动位置；S表示工业机器人的运动空间。