CN113787525A - 一种基于关节性能限制的机械臂运动时间优化方法 - Google Patents

Abstract

本发明属于机械臂控制技术领域，具体公开一种基于关节性能限制的机械臂运动时间优化方法，包括以下步骤：（1）构造映射；（2）对机械臂关节电机进行力矩限制；（3）对轨迹点位进行加速度求解，获取加速度限制值；（4）将机械臂关节电机的速度限制映射为沿轨迹运动的末端执行器速度限制，获取基于关节电机速度限制对应的(

)平面曲线；（5）对各类特征轨迹点位的加速度限制值进行前后数值积分处理，获取基于关节电机力矩限制对应的(

)平面曲线；（6）曲线结合，得到轨迹长度‑速度限制曲线；（7）完成机械臂运动时间优化。本发明的算法简单，基于机械臂的关节电机性能，有效提高机械臂运动的速度，实现对运动时间的优化。

Description

一种基于关节性能限制的机械臂运动时间优化方法

技术领域

本发明涉及一种机械臂控制技术领域，具体涉及一种基于关节性能限制的机械臂运动时间优化方法。

背景技术

机械臂的应用极大的提高了生产效率，随着智能制造政策的落实，机器人的应用范围也越来越广泛。在设计机械臂时，关节电机的性能往往是依赖于工程师的工作经验，对固定的运动轨迹进行仿真计算从而确定关节电机的选型。对于机械臂用户来说，所规划的运动轨迹是复杂且多变的，机械臂在沿着用户设定的轨迹运行时，其末端执行器往往以较低的速度沿轨迹运动，这虽然保证了机械臂能够完成所规划的轨迹，但是却没有发挥关节电机性能的最大效率。现有技术中，为提高机械臂运动轨迹的速度，优化运动时间，大多采用神经网络等智能算法，但这需要大量的数据集进行训练，不仅消耗时间而且同样需要工程经验进行调参。

发明内容

本发明目的在于克服现有技术的不足，提供一种基于关节性能限制的机械臂运动时间优化方法，该方法的算法简单，基于机械臂的关节电机性能，有效地提高机械臂运动时的速度，从而实现对运动时间的优化。

本发明的目的通过以下技术方案实现：

一种基于关节性能限制的机械臂运动时间优化方法，包括以下步骤：

（1）机械臂的各个关节电机之间存在耦合关系，可通过Lagrange方程对此进行描述，其中Lagrange方程为：

式中，M(θ)为6×6的关节空间惯量矩阵，

为6×6的科氏力和向心力的耦合矩阵，G(θ)为6×1的重力矢量矩阵，θ为6×1的机械臂关节空间的旋转角度矩阵，τ为6×1的机械臂的关节力矩矩阵；

为对θ求导，即机械臂关节的速度矩阵；

为对θ求两次导，即机械臂关节的加速度矩阵；

利用Christoffel符号将上述方程可以改写为以下动力学运动方程：

式中，

为机械臂关节的速度矩阵的转置矩阵，

为6×6×6的三维张量；

通过构造映射的方式，将Lagrange方程映射到

平面，从而得出：

式中，s为沿轨迹运动的轨迹长度，

为机械臂的末端执行器沿轨迹运动的速度，

为机械臂的末端执行器沿轨迹运动的加速度，θ _s 为关节角度对轨迹长度的微分矩阵，θ _ss 为关节角度对轨迹长度的二阶微分矩阵；

（2）对机械臂关节电机的力矩进行限制，具体为：

式中，i为机械臂的第i个关节，τ _i 为机械臂的第i个关节电机某时刻的力矩，τ _i 为τ矩阵中的其中一个数值；τ _i ^min为第i个关节电机的最小力矩，τ _i ^max为第i个关节电机的最大力矩；

根据上述方程公式（1）、（2）、（3）和（4），得到在

平面上基于机械臂关节电机力矩限制的约束不等式：

其中，

式中，

为通过轨迹长度映射出机械臂关节的角度值，具体为一个映射关系函数；

（3）对轨迹点位进行加速度求解；先获取在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围，并在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围的交集中取最大值作为该轨迹点位的加速度限制值，记为sdd；其中，对约束不等式（5）中的符号进行变换：

从而将约束不等式转化为：

其中，根据运算规则，其中的a(s)、b(s)和c(s)均为12×1的矩阵；

根据约束不等式（6），轨迹点位上的机械臂每个关节能够提供的末端执行器加速度上限值和末端执行器加速度下限值的获取如下：

当a _i (s)＞0时，机械臂关节能提供的末端执行器加速度上限值为：

当a _i (s)＜0时，机械臂关节能提供的末端执行器加速度下限值为：

当a _i (s)=0时，选取该轨迹点位前后相邻的两个轨迹点位中的较小的末端执行器加速度限制值作为该轨迹点位能够提供的末端执行器加速度限制值；

在公式（7）和公式（8）中，i为机械臂上的第i个关节；a _i (s)为12×1矩阵a(s)中与第i个关节所对应的数值，b _i (s)为12×1矩阵b(s)中与第i个关节所对应的数值，c _i (s)为12×1矩阵c(s)中与第i个关节所对应的数值；

（4）将机械臂关节电机的速度限制映射为沿轨迹运动的末端执行器速度限制，具体地：

式中，t为时间，

为机械臂关节角度的变化量，dt为积分时间间隔，ds为轨迹长度的变化量，

为机械臂末端执行器的速度限制，

为机械臂关节电机的速度限制，上述公式（9）的后部分具体为由机械臂关节电机的速度限制转换为机械臂末端执行器的速度限制；

通过上述公式（9）的映射，得出沿指定运动轨迹的所有离散点上基于关节电机速度限制对应的

平面曲线；

（5）对各类特征轨迹点位的加速度限制值进行前后数值积分处理，获取基于关节电机力矩限制对应的

平面曲线；其中各类特征轨迹点位的加速度限制值通过步骤（3）获取，所述各类特征轨迹点位包括起始点、终止点以及间断点；具体地，积分公式如下：

式中，

为轨迹点位向前的轨迹长度值，

为轨迹点位向前的速度值，

为当前轨迹点位处的轨迹长度值，

为当前轨迹点位处的机械臂末端执行器的速度值，

为轨迹点位向后的轨迹长度值，

为轨迹点位向后的速度值，dt为积分时间间隔，sdd为步骤（3）中求得的该轨迹点位的加速度限制值；轨迹点位的前向为轨迹长度从1往0的方向，轨迹点位的后向为轨迹长度从0往1的方向，由于机械臂的运动轨迹已经确定，因此采用归一方法可得出轨迹长度s的范围为0-1；

（6）将步骤（4）中获得的基于关节电机速度限制对应的

平面曲线与步骤（5）中获得的基于关节电机力矩限制对应的

平面曲线进行结合；将基于关节电机力矩限制对应的

平面曲线中高于基于关节电机速度限制对应的

平面曲线的部分舍去，同时在同一点位上取较小的速度作为曲线速度值，从而得到同时满足关节电机速度限制和关节电机力矩限制且连续的轨迹长度-速度限制曲线；

（7）将步骤（6）中所得到的轨迹长度-速度限制曲线作为依据条件，机械臂末端执行器进行指定运动轨迹的运行，完成机械臂运动时间的优化。

本发明的一个优选方案，在步骤（5）中，对间断点进行前后数值积分时，逐步提高间断点处的速度积分起始值的选取，以获取该间断点在满足轨迹长度条件下最高的加速度值进行积分；在逐步提高间断点处的速度积分起始值的选取时，根据预设的固定增长比例，不断逼近间断点处的最小值，且通过设定阈值终止速度积分起始值的选取。

本发明的一个优选方案，采用多次B样条曲线对轨迹长度-速度限制曲线的速度规划进行平滑处理，从而获得平滑的轨迹长度-速度限制曲线。

优选地，将u记为轨迹长度-速度限制曲线上离散点到轨迹起始点的距离与轨迹长度的比值，并令

为单调不减的数组序列，n为所述数组序列U中的元素数量，可自定义选取；其中的u ₀，u ₁，…u _n 表示轨迹长度-速度限制曲线上不同离散点的u值，即下述公式（14）中u的具体数值；同时，具体采用三次B样条曲线对轨迹长度-速度限制曲线进行平滑处理，因此三次B样条曲线方程可以表达：

式中，p(u)为映射在轨迹长度-速度限制曲线的速度值；

为k次方的B样条基函数，其中k=3；j为该基函数在数组序列U上的位置，C _j 为控制点；

进一步地，p(u)值可通过u值得出，因此上述公式（14）中的控制点C _j 为唯一变量；此时，将数组序列中的所有u值代入到公式（14）以及标准的德布尔-考克斯递推公式，即可求得控制点C _j 序列，从而得出平滑的轨迹长度-速度限制曲线。

优选地，对平滑处理后的轨迹长度-速度限制曲线的速度进行验证；在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点，并将每个待验证的离散点的平滑速度值V′与该待验证的离散点在平滑处理前的轨迹长度-速度限制曲线对应的速度值V进行比较，且根据比较结果进行曲线调整；

当V′≤V时，不做处理；

当V′＞V时，则将所有控制点C _j 的纵向坐标下降指定比例，再次比较该离散点的V′值与V值，直至选取的所有待验证的离散点均满足V′≤V。

优选地，完成平滑处理后的轨迹长度-速度限制曲线的平滑速度验证后，对平滑处理后的轨迹长度-速度限制曲线的平滑加速度进行验证；在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点，对同一个待验证的离散点分别计算平滑加速度值a以及平滑加速度限制值sdd′，并对同一个待验证的离散点的平滑加速度值a与平滑加速度限制值sdd′进行比较和调整处理，其中的平滑加速度限制值sdd′通过步骤（3）即可求得，其中的平滑加速度值a通过以下公式求得：

式中，

为待验证的离散点的平滑速度值，

为相邻点的平滑速度值，dm为待验证的离散点与相邻点之间的距离；

若a ≤ sdd′，不做处理；

若a > sdd′，则将所有控制点C _j 的纵向坐标下降指定比例，重新计算和比较该离散点的a值与sdd′值，直至选取的所有待验证的离散点均满足a ≤ sdd′。

本发明与现有技术相比具有以下有益效果：

1、在机械臂的运动轨迹已经确定的前提下，本发明通过对机械臂的运动速度进行限制，通过轨迹长度-速度限制曲线，实现对机械臂进行运动轨迹时的指导，充分发挥关节电机的性能，让机械臂快速高效地完成轨迹运动，缩短运动时间，从而达到机械臂运动时间优化的目的。

2、本发明从机械臂的关节电机的力矩限制和速度限制两方面条件因素求解出机械臂预定轨迹的时间优化规划，使得机械臂的运动时间优化准确度高，且能充分发挥关节电机的性能，让机械臂以尽可能大的速度沿预定轨迹进行运动。

3、本发明采用数值算法的方式实现对机械臂的运动时间优化，具有计算实时效果，且计算量小，无需大量的数据训练或繁杂的计算过程，具有实时性和良好的可靠性，能适应任何预定运动轨迹。

附图说明

图1为本发明的基于关节性能限制的机械臂运动时间优化方法的流程框图。

图2为基于关节电机速度限制对应的

平面曲线（连续的）示意图。

图3为基于关节电机速度限制对应的

平面曲线（间断的）示意图。

图4为基于关节电机速度限制对应的

平面曲线（间断的细实线L2）和基于关节电机力矩限制对应的

平面曲线（虚线L3）示意图。

图5为轨迹长度-速度限制曲线（粗实线L4）示意图。

图6为平滑处理后的轨迹长度-速度限制曲线（双点划线L5）示意图。

具体实施方式

下面结合实施例和附图对本发明作进一步描述，但本发明的实施方式不仅限于此。

参见图1-图6，本实施例公开一种基于关节性能限制的机械臂运动时间优化方法，包括以下步骤：

（1）机械臂的各个关节电机之间存在耦合关系，可通过Lagrange方程对此进行描述，其中Lagrange方程为：

式中，M(θ)为6×6的关节空间惯量矩阵，

为6×6的科氏力和向心力的耦合矩阵，G(θ)为6×1的重力矢量矩阵，θ为6×1的机械臂关节空间的旋转角度矩阵，τ为6×1的机械臂的关节力矩矩阵；

为对θ求导，即机械臂关节的速度矩阵；

为对θ求两次导，即机械臂关节的加速度矩阵；

利用Christoffel符号将上述方程可以改写为以下动力学运动方程：

式中，

为机械臂关节的速度矩阵的转置矩阵，

为6×6×6的三维张量。

机械臂运动轨迹的时间最优问题实际上就是求解机械臂在满足动力学方程的条件下速度的最大值问题，把此问题转化到

平面上将能更加直观的感受所求解的目标。而Lagrange方程是在关节空间对机械臂的运动状态进行描述，因此通过构造映射的方式，将Lagrange方程映射到

平面，从而得出：

式中，s为沿轨迹运动的轨迹长度，

为机械臂的末端执行器沿轨迹运动的速度，

为机械臂的末端执行器沿轨迹运动的加速度，θ _s 为关节角度对轨迹长度的微分矩阵，θ _ss 为关节角度对轨迹长度的二阶微分矩阵。

（2）对机械臂关节电机的力矩进行限制，具体为：

式中，i为机械臂的第i个关节，τ _i 为机械臂的第i个关节电机某时刻的力矩，τ _i 为τ矩阵中的其中一个数值；τ _i ^min为第i个关节电机的最小力矩，τ _i ^max为第i个关节电机的最大力矩；

根据上述方程公式（1）、（2）、（3）和（4），得到在

平面上基于机械臂关节电机力矩限制的约束不等式：

其中，

式中，

为通过轨迹长度映射出机械臂关节的角度值，具体为一个映射关系函数。

（3）对轨迹点位进行加速度求解；先获取在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围，并在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围的交集中取最大值作为该轨迹点位的加速度限制值，记为sdd；其中，对约束不等式（5）中的符号进行变换：

从而将约束不等式转化为：

其中，根据运算规则，其中的a(s)、b(s)和c(s)均为12×1的矩阵；

根据约束不等式（6），轨迹点位上的机械臂每个关节能够提供的末端执行器加速度上限值和末端执行器加速度下限值的通过以下方程公式获取。由于本实施例中采用的机械臂为六轴机械臂，而经过前面的计算后，对于每个机械臂关节，会存在小于不等式（公式（7））和大于不等式（公式（8））两个结果，因此总的来说，每个轨迹点位将会包括12个不等式。

当a _i (s)＞0时，机械臂关节能提供的末端执行器加速度上限值为：

当a _i (s)＜0时，机械臂关节能提供的末端执行器加速度下限值为：

当a _i (s)=0时，选取该轨迹点位前后相邻的两个轨迹点位中的较小的末端执行器加速度限制值作为该轨迹点位能够提供的末端执行器加速度限制值。具体地，例如此时该机械臂关节i所对应的s为0.1，取多条积分虚线中s=0.1附近能提供的末端执行器加速度最小值作为该机械臂关节能提供的末端执行器加速度上限值（这里可理解为：如果ai(s)=0，那么这个轨迹点位的计算先放着不管，当计算完其他所有不为零的离散点之后，先通过积分形成多条虚线，再从多条虚线中取得这个轨迹点位的相邻点sdd最小值作为该轨迹点位的sdd，从而补全该轨迹点位的虚线）。

在上述公式（7）和公式（8）中，i为机械臂上的第i个关节；a _i (s)为12×1矩阵a(s)中与第i个关节所对应的数值，b _i (s)为12×1矩阵b(s)中与第i个关节所对应的数值，c _i (s)为12×1矩阵c(s)中与第i个关节所对应的数值。

（4）将机械臂关节电机的速度限制映射为沿轨迹运动的末端执行器速度限制，目的是将机械臂的关节电机速度层面的限制，通过变换转化到

平面上的限制；具体地：

式中，t为时间，

为机械臂关节角度的变化量，dt为积分时间间隔，ds为轨迹长度的变化量，

为机械臂末端执行器的速度限制，

为机械臂关节电机的速度限制，上述公式（9）的后部分具体为由机械臂关节电机的速度限制转换为机械臂末端执行器的速度限制；

通过上述公式（9）的映射，得出沿指定运动轨迹的所有离散点上基于关节电机速度限制对应的

平面曲线。如图2的细实线L1所示，该平面曲线为连续的曲线，但更一般的情况是如图3的细实线L2所示，该平面曲线存在不连续点P1、P2，甚至更多的不连续点。

（5）对各类特征轨迹点位的加速度限制值进行前后数值积分处理，获取基于关节电机力矩限制对应的

平面曲线，同样包含了多条不连续的曲线，如图4的虚线L3所示，而图4中的细实线L2为基于关节电机速度限制对应的

平面曲线。其中各类特征轨迹点位的加速度限制值通过步骤（3）获取，所述各类特征轨迹点位包括起始点、终止点以及间断点；具体地，积分公式如下：

式中，

为轨迹点位向前的轨迹长度值，

为轨迹点位向前的速度值，

为当前轨迹点位处的轨迹长度值，

为当前轨迹点位处的机械臂末端执行器的速度值，

为轨迹点位向后的轨迹长度值，

为轨迹点位向后的速度值，dt为积分时间间隔，sdd为步骤（3）中求得的该轨迹点位的加速度限制值；轨迹点位的前向为轨迹长度从1往0的方向，轨迹点位的后向为轨迹长度从0往1的方向，由于机械臂的运动轨迹已经确定，因此采用归一方法可得出轨迹长度s的范围为0-1。

另外，在对间断点进行前后数值积分时，逐步提高间断点处的速度积分起始值的选取，以获取该间断点在满足轨迹长度条件下最高的加速度值进行积分；在逐步提高间断点处的速度积分起始值的选取时，根据预设的固定增长比例，不断逼近间断点处的最小值，且通过设定阈值终止速度积分起始值的选取。具体地，可在对间断点进行前后数值积分时，先选取不连续点处的较小值的三分之一作为速度积分起始值，经过数值积分后，若满足轨迹长度0-1的范围，则继续提高速度积分起始值的选取，将其选取为不连续点处的较小值的三分之二，并重新数值积分，直至在满足轨迹长度0-1的范围前提下，逐渐逼近不连续点处的较小值；速度积分起始值的选取，可通过设置与不连续点处的较小值的误差阈值进行终止。这样，有利于让间断点在基于关节电机力矩限制对应的(s,s ̇)平面上尽可能地高，从而使得在后续处理中该点的速度限制值越高，以提高机械臂运动时的速度，运动时间得到进一步的优化。

（6）将步骤（4）中获得的基于关节电机速度限制对应的

平面曲线（如图3和图4的细实线L2所示）与步骤（5）中获得的基于关节电机力矩限制对应的

平面曲线（如图4的虚线L3所示）进行结合。在结合时，将基于关节电机力矩限制对应的

平面曲线中高于基于关节电机速度限制对应的

平面曲线的部分舍去，同时在同一点位上取较小的速度作为曲线速度值，从而得到同时满足关节电机速度限制和关节电机力矩限制且连续的轨迹长度-速度限制曲线，如图5中的粗实线L4所示，而图5中的细实线L2为基于关节电机速度限制对应的

平面曲线，虚线为基于关节电机力矩限制对应的

平面曲线中舍去的部分。

（7）采用多次B样条曲线对轨迹长度-速度限制曲线的速度规划进行平滑处理，从而获得平滑的轨迹长度-速度限制曲线，如图6中的双点划线L5所示。

首先，将u记为轨迹长度-速度限制曲线上离散点到轨迹起始点的距离与轨迹长度的比值，并令

为单调不减的数组序列，n为所述数组序列U中的元素数量，可自定义选取；其中的u ₀，u ₁，…u _n 表示轨迹长度-速度限制曲线上不同离散点的u值，即下述公式（14）中u的具体数值；同时，具体采用三次B样条曲线对轨迹长度-速度限制曲线进行平滑处理，因此三次B样条曲线方程可以表达：

式中，p(u)为映射在轨迹长度-速度限制曲线的速度值；

为k次方的B样条基函数，其中k=3；j为该基函数在数组序列U上的位置，C _j 为控制点；

在上述处理过程中，p(u)值可通过u值得出，因此上述公式（14）中的控制点C _j 为唯一变量；此时，将数组序列中的所有u值代入到公式（14）以及标准的德布尔-考克斯递推公式，即可求得控制点C _j 序列，从而得出平滑的轨迹长度-速度限制曲线。

其中，本实施例中的标准的德布尔-考克斯递推公式如下：

式中，j为该基函数在数组序列U上的位置，k=3；

同时，由于三次B样条曲线的特性，需要额外增加两个控制点，本实施例中具体增加的两个控制点的方程组分别为轨迹起始点、终止点处的速度为0的两个方程。

例如，以轨迹长度为100cm作为参考，则可选取50个离散点的比例来确定控制点C _j ，由于三次B样条曲线的特性，则另外增加两个控制点，因此一共有52个控制点。上述50个离散点在轨迹长度100cm上均匀选取，即u分别为1/50、2/50、3/50、…、50/50，同时根据这些u值即可相应地获得p(u)值；这样，就得出50条关于获取控制点的方程，即50条上述方程公式（14），随后经过解方程即可得出50个控制点。在得出控制点后，根据B样条曲线的特性，即可获得平滑处理的轨迹长度-速度限制曲线。

（8）对平滑处理后的轨迹长度-速度限制曲线的速度进行验证。在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点，并将每个待验证的离散点的平滑速度值V′与该待验证的离散点在平滑处理前的轨迹长度-速度限制曲线对应的速度值V进行比较，且根据比较结果进行曲线调整；

当V′≤V时，不做处理；

当V′＞V时，则将所有控制点C _j 的纵向坐标下降指定比例，再次比较该离散点的V′值与V值，直至选取的所有待验证的离散点均满足V′≤V。例如，对于轨迹长度为100cm的参考值，可等距选取100个离散点，并依次计算该100个离散点在平滑后的曲线上的平滑速度值，且与平滑后的曲线上的速度值进行比较，根据比较结果进行平滑后的曲线的调整。调整时，可先对所有控制点C _j 的纵向坐标下降10%，再次比较计算后，若仍不满足条件，则继续对所有控制点C _j 的纵向坐标再下降10%，直至所有离散点满足条件位置。

（9）对平滑处理后的轨迹长度-速度限制曲线的平滑加速度进行验证，从而获得经过平滑和验证调整的轨迹长度-速度限制曲线。在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点，对同一个待验证的离散点分别计算平滑加速度值a以及平滑加速度限制值sdd′，并对同一个待验证的离散点的平滑加速度值a与平滑加速度限制值sdd′进行比较和调整处理，其中的平滑加速度限制值sdd′通过步骤（3）即可求得，其中的平滑加速度值a通过以下公式求得：

式中，

为待验证的离散点的平滑速度值，

为相邻点的平滑速度值，dm为待验证的离散点与相邻点之间的距离；其中的相邻点可在待验证的离散点的左侧或右侧选取，此处目的是将待验证的离散点的平滑加速度值a求出即可，对相邻点的选取不作限定；

若a ≤ sdd′，不做处理；

若a > sdd′，则将所有控制点C _j 的纵向坐标下降指定比例，重新计算和比较该离散点的a值与sdd′值，直至选取的所有待验证的离散点均满足a ≤ sdd′。

（10）将步骤（9）的经过平滑和验证调整的轨迹长度-速度限制曲线作为依据条件，机械臂末端执行器进行指定运动轨迹的运行，完成机械臂运动时间的优化。

本实施例中，在得出轨迹长度-速度限制曲线后，由于曲线并非是平滑的，因此进一步采用B样条曲线作平滑处理，使得机械臂进行指定运动轨迹时的速度更加平滑，从而有利于提高机械臂运动时的稳定性。与此同时，对平滑后的曲线进行验证，以确保平滑后的曲线仍然在机械臂关节电机的速度和力矩限制的范围内，从而提高优化后的曲线可靠性。当然，对于本实施例的轨迹长度-速度限制曲线，具体的B样条曲线平滑处理可参见现有技术，也可采用其他方式进行平滑处理。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所做的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

1.一种基于关节性能限制的机械臂运动时间优化方法，其特征在于，包括以下步骤：

（1）机械臂的各个关节电机之间存在耦合关系，可通过Lagrange方程对此进行描述，其中Lagrange方程为：

式中，M(θ)为6×6的关节空间惯量矩阵，

为6×6的科氏力和向心力的耦合矩阵，G (θ)为6×1的重力矢量矩阵，θ为6×1的机械臂关节空间的旋转角度矩阵，τ为6×1的机械臂的关节力矩矩阵；

为对θ求导，即机械臂关节的速度矩阵；

为对θ求两次导，即机械臂关节的加速度矩阵；

利用Christoffel符号将上述方程可以改写为以下动力学运动方程：

式中，

为机械臂关节的速度矩阵的转置矩阵，

为6×6×6的三维张量；

通过构造映射的方式，将Lagrange方程映射到

平面，从而得出：

式中，s为沿轨迹运动的轨迹长度，

为机械臂的末端执行器沿轨迹运动的速度，

为机械臂的末端执行器沿轨迹运动的加速度，θ _s 为关节角度对轨迹长度的微分矩阵，θ _ss 为关节角度对轨迹长度的二阶微分矩阵；

（2）对机械臂关节电机的力矩进行限制，具体为：

式中，i为机械臂的第i个关节，τ _i 为机械臂的第i个关节电机某时刻的力矩，τ _i 为τ矩阵中的其中一个数值；τ _i ^min 为第i个关节电机的最小力矩，τ _i ^max 为第i个关节电机的最大力矩；

根据上述方程公式（1）、（2）、（3）和（4），得到在

平面上基于机械臂关节电机力矩限制的约束不等式：

其中，

式中，

为通过轨迹长度映射出机械臂关节的角度值，具体为一个映射关系函数；

（3）对轨迹点位进行加速度求解；先获取在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围，并在该轨迹点位上的每个机械臂关节能够提供的末端执行器加速度范围的交集中取最大值作为该轨迹点位的加速度限制值，记为sdd；其中，对约束不等式（5）中的符号进行变换：

从而将约束不等式转化为：

其中，根据运算规则，其中的a(s)、b(s)和c(s)均为12×1的矩阵；

根据约束不等式（6），轨迹点位上的机械臂每个关节能够提供的末端执行器加速度上限值和末端执行器加速度下限值的获取如下：

当a _i (s)＞0时，机械臂关节能提供的末端执行器加速度上限值为：

当a _i (s)＜0时，机械臂关节能提供的末端执行器加速度下限值为：

当a _i (s)=0时，选取该轨迹点位前后相邻的两个轨迹点位中的较小的末端执行器加速度限制值作为该轨迹点位能够提供的末端执行器加速度限制值；

在公式（7）和公式（8）中，i为机械臂上的第i个关节；a _i (s)为12×1矩阵a(s)中与第i个关节所对应的数值，b _i (s)为12×1矩阵b(s)中与第i个关节所对应的数值，c _i (s)为12×1矩阵c(s)中与第i个关节所对应的数值；

（4）将机械臂关节电机的速度限制映射为沿轨迹运动的末端执行器速度限制，具体地：

式中，t为时间，

为机械臂关节角度的变化量，dt为积分时间间隔，ds为轨迹长度的变化量，

为机械臂末端执行器的速度限制，

为机械臂关节电机的速度限制，上述公式（9）的后部分具体为由机械臂关节电机的速度限制转换为机械臂末端执行器的速度限制；

通过上述公式（9）的映射，得出沿指定运动轨迹的所有离散点上基于关节电机速度限制对应的

平面曲线；

（5）对各类特征轨迹点位的加速度限制值进行前后数值积分处理，获取基于关节电机力矩限制对应的

平面曲线；其中各类特征轨迹点位的加速度限制值通过步骤（3）获取，所述各类特征轨迹点位包括起始点、终止点以及间断点；具体地，积分公式如下：

式中，

为轨迹点位向前的轨迹长度值，

为轨迹点位向前的速度值，

为当前轨迹点位处的轨迹长度值，

当前轨迹点位处的机械臂末端执行器的速度值，

为轨迹点位向后的轨迹长度值，

为轨迹点位向后的速度值，dt为积分时间间隔，sdd为步骤（3）中求得的该轨迹点位的加速度限制值；轨迹点位的前向为轨迹长度从1往0的方向，轨迹点位的后向为轨迹长度从0往1的方向，由于机械臂的运动轨迹已经确定，因此采用归一方法可得出轨迹长度s的范围为0-1；

（6）将步骤（4）中获得的基于关节电机速度限制对应的

平面曲线与步骤（5）中获得的基于关节电机力矩限制对应的

平面曲线进行结合；将基于关节电机力矩限制对应的

平面曲线中高于基于关节电机速度限制对应的

平面曲线的部分舍去，同时在同一点位上取较小的速度作为曲线速度值，从而得到同时满足关节电机速度限制和关节电机力矩限制且连续的轨迹长度-速度限制曲线；

（7）将步骤（6）中所得到的轨迹长度-速度限制曲线作为依据条件，机械臂末端执行器进行指定运动轨迹的运行，完成机械臂运动时间的优化。

2.根据权利要求1所述的基于关节性能限制的机械臂运动时间优化方法，其特征在于，在步骤（5）中，对间断点进行前后数值积分时，逐步提高间断点处的速度积分起始值的选取，以获取该间断点在满足轨迹长度条件下最高的加速度值进行积分；在逐步提高间断点处的速度积分起始值的选取时，根据预设的固定增长比例，不断逼近间断点处的最小值，且通过设定阈值终止速度积分起始值的选取。

3.根据权利要求1或2所述的基于关节性能限制的机械臂运动时间优化方法，其特征在于，采用多次B样条曲线对轨迹长度-速度限制曲线的速度规划进行平滑处理，从而获得平滑的轨迹长度-速度限制曲线。

4.根据权利要求3所述的基于关节性能限制的机械臂运动时间优化方法，其特征在于，将u记为轨迹长度-速度限制曲线上离散点到轨迹起始点的距离与轨迹长度的比值，并令

为单调不减的数组序列，n为所述数组序列U中的元素数量，可自定义选取；其中的u ₀，u ₁，… u _n 表示轨迹长度-速度限制曲线上不同离散点的u值，即下述公式（14）中u的具体数值；同时，具体采用三次B样条曲线对轨迹长度-速度限制曲线进行平滑处理，因此三次B样条曲线方程可以表达：

式中，p(u)为映射在轨迹长度-速度限制曲线的速度值；

为k次方的B样条基函数，其中k=3；j为该基函数在数组序列U上的位置，C _j 为控制点；

进一步地，p(u)值可通过u值得出，因此上述公式（14）中的控制点C _j 为唯一变量；此时，将数组序列中的所有u值代入到公式（14）以及标准的德布尔-考克斯递推公式，即可求得控制点C _j 序列，从而得出平滑的轨迹长度-速度限制曲线。

5.根据权利要求4所述的基于关节性能限制的机械臂运动时间优化方法，其特征在于，对平滑处理后的轨迹长度-速度限制曲线的速度进行验证；在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点，并将每个待验证的离散点的平滑速度值V′与该待验证的离散点在平滑处理前的轨迹长度-速度限制曲线对应的速度值V进行比较，且根据比较结果进行曲线调整；

当V′≤V时，不做处理；

当V′＞V时，则将所有控制点C _j 的纵向坐标下降指定比例，再次比较该离散点的V′值与V值，直至选取的所有待验证的离散点均满足V′≤V。

6.根据权利要求5所述的基于关节性能限制的机械臂运动时间优化方法，其特征在于，完成平滑处理后的轨迹长度-速度限制曲线的平滑速度值验证后，对平滑处理后的轨迹长度-速度限制曲线的平滑加速度值进行验证；在平滑处理后的轨迹长度-速度限制曲线上选取若干个待验证的离散点，对同一个待验证的离散点分别计算平滑加速度值a以及平滑加速度限制值sdd′，并对同一个待验证的离散点的平滑加速度值a与平滑加速度限制值sdd′进行比较和调整处理，其中的平滑加速度限制值sdd′通过步骤（3）即可求得，其中的平滑加速度值a通过以下公式求得：

式中，

为待验证的离散点的平滑速度值，

为相邻点的平滑速度值， dm为待验证的离散点与相邻点之间的距离；

若a ≤ sdd′，不做处理；

若a > sdd′，则将所有控制点C _j 的纵向坐标下降指定比例，重新计算和比较该离散点的a值与sdd′值，直至选取的所有待验证的离散点均满足a ≤ sdd′。

Images