CN113910247B - 考虑关节间隙演化的工业机器人末端轨迹控制方法及系统 - Google Patents

Abstract

本发明提供一种考虑关节间隙演化的工业机器人末端轨迹控制方法，属于机器人控制技术领域，工业机器人末端轨迹控制方法包括：采集工业机器人在实际运行过程中机械臂的实际末端轨迹；计算实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列；根据误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的近似熵；根据近似熵以及预先确定的“间隙大小‑近似熵值‑PID参数”对照表，确定对应的PID参数；通过PID控制器根据PID参数，控制工业机器人电机的输出力矩，进而调整工业机器人机械臂末端的运行轨迹，提高了机器人末端轨迹自动控制的精确性，并节省了大量的停机检查时间。

Description

考虑关节间隙演化的工业机器人末端轨迹控制方法及系统

技术领域

本发明涉及机器人控制技术领域，特别是涉及一种考虑关节间隙演化的工业机器人末端轨迹控制方法及系统。

背景技术

工业机器人在实际生产生活中占有至关重要的地位，相较人类，机器人对极端环境的适应能力更强，功能作用更丰富且专业。然而由于工艺问题等因素的影响，机器人关节会产生不断增大的间隙，将严重地影响系统的动态性能以及可靠性。因此关节间隙的问题将是未来生产高性能、高可靠性、长寿命机械系统制造生产的关键前提。

针对含间隙机械臂的运动学及动力学分析问题，众多学者已开展了卓有成效的研究。Earles和Wu将运动副间隙表示为一个无质量连杆，采用了改进的拉格朗日方法研究了含运动副间隙的刚体系统的动力学特性。Wilson研究了滑动铰间隙对曲柄滑块机构动力学特性的影响。Innocenti对含间隙转动副的空间机构，采用虚功原理对运动间隙进行灵敏度分析。Ting将含间隙运动副用间隙连杆表示，研究了由于运动副间隙造成的含间隙平面单环机构的最大定位误差。Parenti-Castelli在含间隙机构运动学建模的基础上，采用虚功原理进行了机构间隙影响分析，并提出了改进的间隙影响分析方法。Flores研究了含间隙旋转铰的描述方法和计算方法，所建立的模型是基于几何描述的接触条件和连续接触碰撞力模型。并针对含间隙的平面四连杆机构进行了详细的分析。Hayasaka等针对含间隙铰接桁架结构，研究了铰接间隙对系统的影响。Moon等研究了铰接空间桁架结构振动特性，并通过实验发现铰接空间桁架结构存在混沌振动现象；Folkman等人针对铰接空间机构，考虑了重力和间隙影响，详细地分析了铰接空间结构的阻尼特性。García针对典型多体系统中的光滑铰间隙，提出了一种分析光滑铰间隙的方法。

然而以上方法均只考虑了固定间隙对系统的动力学以及运动学等特性的影响，尚未考虑实际工况中当间隙不断演化变大时的情形下间隙的影响以及相应的控制问题。

发明内容

本发明的目的是提供一种考虑关节间隙演化的工业机器人末端轨迹控制方法及系统，可提高机器人末端轨迹自动控制的精确性，并节省了大量的停机检查时间。

为实现上述目的，本发明提供了如下方案：

一种考虑关节间隙演化的工业机器人末端轨迹控制方法，所述考虑关节间隙演化的工业机器人末端轨迹控制方法包括：

采集工业机器人在实际运行过程中机械臂的实际末端轨迹；

计算所述实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列；

根据所述误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的近似熵；

根据所述近似熵以及预先确定的“间隙大小-近似熵值-PID参数”对照表，确定对应的PID参数；

通过PID控制器根据所述PID参数，控制所述工业机器人电机的输出力矩，进而调整所述工业机器人机械臂末端的运行轨迹。

可选地，所述工业机器人的机械臂包括第一机械臂和第二机械臂，所述第一机械臂的一端与电机连接，所述第一机械臂的另一端与所述第二机械臂连接；

所述计算所述实际末端轨迹与预期末端轨迹之间的误差序列，具体包括：

以所述电机所在位置为原点建立极坐标系；

获取所述预期末端轨迹在所述极坐标系中的极径，得到第一极径集，以及所述实际末端轨迹在所述极坐标系中的极径，得到第二极径集；

根据所述第一极径集和所述第二极径集，确定误差序列。

可选地，所述第一极径集中的极径与所述第二极径集中的极径一一对应；

根据以下公式，确定误差序列：

E＝[r₁₀-r₀₀,r₁₁-r₀₁,…,r_1N-r_0N]；

其中，E为误差序列，r_0N为第一极径集中的极径，r_1N为第二极径集中与r_0N对应的极径。

可选地，所述“间隙大小-近似熵值-PID参数”对照表的确定方法具体包括：

调整所述工业机器人机械臂的间隙大小；

针对每一间隙，在所述机械臂运行时，采集机械臂的实验间隙末端轨迹；

基于近似熵算法，确定所述实验间隙末端轨迹与所述预期末端轨迹的实验近似熵；

根据所述实验近似熵，调整工业机器人的PID控制器的PID参数，直至所述实验间隙末端轨迹与所述预期末端轨迹之间的误差小于误差阈值时，确定间隙大小、所述实验近似熵以及所述PID参数的对应关系；

根据间隙大小从0增大到最大间隙阈值时，间隙大小、实验近似熵以及PID参数的对应关系，确定“间隙大小-近似熵值-PID参数”对照表。

可选地，所述考虑关节间隙演化的工业机器人末端轨迹控制方法还包括：

基于机械动力学原理，根据所述预期末端轨迹、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，建立无间隙时工业机器人动力学模型。

可选地，所述基于机械动力学原理，根据所述预期末端轨迹、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，建立无间隙时工业机器人动力学模型，具体包括：

获取所述预期末端轨迹中的末端坐标；

根据所述末端坐标、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，确定系统动能和系统势能；

根据所述系统动能和系统势能，采用拉格朗日方程，确定工业机器人动力学模型。

可选地，根据以下公式，确定末端坐标：

其中，(x_c,y_c)为末端坐标，l₁为第一机械臂杆长，l₂为第二机械臂杆长，θ₁为第一机械臂转角，θ₂为第二机械臂转角。

可选地，根据以下公式，确定系统动能：

其中，E_K为系统动能，m₁为第一机械臂质量，m₂为第二机械臂质量；

根据以下公式，确定系统势能：

E_P＝m₁gl₁cosθ₁+m₂g(l₁cosθ₁+l₂cos(θ₁+θ₂))；

其中，E_P为系统势能，g为重力加速度，m₁为第一机械臂质量，m₂为第二机械臂质量。

为实现上述目的，本发明还提供了如下方案：

一种考虑关节间隙演化的工业机器人末端轨迹控制系统，所述考虑关节间隙演化的工业机器人末端轨迹控制系统包括：

末端轨迹采集单元，用于采集工业机器人在实际运行过程中机械臂的实际末端轨迹；

误差确定单元，与所述末端轨迹采集单元连接，用于计算所述实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列；

近似熵确定单元，与所述误差确定单元连接，用于根据所述误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的近似熵；

PID参数确定单元，与所述近似熵确定单元连接，用于根据所述近似熵以及预先确定的“间隙大小-近似熵值-PID参数”对照表，确定对应的PID参数；

控制单元，与所述PID参数确定单元连接，用于通过PID控制器根据所述PID参数，控制所述工业机器人电机的输出力矩，进而调整所述工业机器人机械臂末端的运行轨迹。

可选地，所述工业机器人的机械臂包括第一机械臂和第二机械臂，所述第一机械臂的一端与电机连接，所述第一机械臂的另一端与所述第二机械臂连接；

所述误差确定单元包括：

极坐标系建立模块，用于以所述电机所在位置为原点建立极坐标系；

极径确定模块，分别与所述极坐标系建立模块及所述末端轨迹采集单元连接，用于获取所述预期末端轨迹在所述极坐标系中的极径，得到第一极径集，以及所述实际末端轨迹在所述极坐标系中的极径，得到第二极径集；

误差序列确定模块，与所述极径确定模块连接，用于根据所述第一极径集和所述第二极径集，确定误差序列。

根据本发明提供的具体实施例，本发明公开了以下技术效果：通过计算实际末端轨迹与预期末端轨迹的误差序列，并基于近似熵算法，确定实际末端轨迹与预期末端轨迹的近似熵，进而直接通过“间隙大小-近似熵值-PID参数”对照表确定PID参数，调节机械臂末端的运行轨迹。可针对实际工况中机械臂关节间隙不断演化的机械系统，通过提前确定的“间隙大小-近似熵值-PID参数”对照表，使得机器人在实际工作中可自动调节相应的PID参数，使机器人的运行轨迹保持在允许范围内，从而避免实际工作中由于间隙增大频繁对控制器参数进行调节，节省了大量的停机检查时间，并且用实际计算出的偏差近似熵针对性的反映出了关节中较难测出的间隙大小，提高了机器人末端轨迹自动控制的精确性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明考虑关节间隙演化的工业机器人末端轨迹控制方法的流程图；

图2为含间隙二自由度机械臂结构图；

图3为由预定轨迹计算出的理想状况下的两机械臂转角θ₁(t)，θ₂(t)变化图；

图4为间隙为0时的实际末端轨迹与预期末端轨迹比较图；

图5为间隙为0.05时的实际末端轨迹与预期末端轨迹比较图；

图6为间隙为0.15时的实际末端轨迹与预期末端轨迹比较图；

图7为间隙为0.05时经过PID控制后的末端轨迹与预期末端轨迹比较图；

图8为间隙为0.15时经过PID控制后的末端轨迹与预期末端轨迹比较图；

图9为末端轨迹误差近似熵与间隙大小变化图；

图10为工业机器人末端轨迹控制系统的模块结构示意图。

符号说明：

末端轨迹采集单元-1，误差确定单元-2，近似熵确定单元-3，PID参数确定单元-4，控制单元-5。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种考虑关节间隙演化的工业机器人末端轨迹控制方法及系统，通过计算实际末端轨迹与预期末端轨迹的误差序列，并基于近似熵算法，确定实际末端轨迹与预期末端轨迹的近似熵，进而直接通过“间隙大小-近似熵值-PID参数”对照表确定PID参数，调节机械臂末端的运行轨迹。可针对实际工况中机械臂关节间隙不断演化的机械系统，通过提前确定的“间隙大小-近似熵值-PID参数”对照表，使得机器人在实际工作中可自动调节相应的PID参数，使机器人的运行轨迹保持在允许范围内，从而避免实际工作中由于间隙增大频繁对控制器参数进行调节，节省了大量的停机检查时间，并且用实际计算出的偏差近似熵针对性的反映出了关节中较难测出的间隙大小，提高了机器人末端轨迹自动控制的精确性。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本发明考虑关节间隙演化的工业机器人末端轨迹控制方法包括：

S1：采集工业机器人在实际运行过程中机械臂的实际末端轨迹。在本实施例中，通过激光跟踪仪测量机械臂的实际末端轨迹。本发明首先在Solid workss平台上根据给定的机械臂杆长，机械臂结构以及关节间隙大小等参数建立机械臂的三维模型，采用激光跟踪仪实时跟踪机械臂末端末端位置。

S2：计算所述实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列。

S3：根据所述误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的近似熵。

S4：根据所述近似熵以及预先确定的“间隙大小-近似熵值-PID参数”对照表，确定对应的PID参数。

S5：通过PID控制器根据所述PID参数，控制所述工业机器人电机的输出力矩，进而调整所述工业机器人机械臂末端的运行轨迹。具体地，所述工业机器人电机包括第一电机和第二电机。

具体地，所述工业机器人的机械臂包括第一机械臂和第二机械臂，所述第一机械臂的一端与电机连接，所述第一机械臂的另一端与所述第二机械臂连接。另外，所述第二机械臂也对应连接一个电机。

进一步地，S2具体包括：

S21：以所述电机所在位置为原点建立极坐标系。

S22：获取所述预期末端轨迹在所述极坐标系中的极径，得到第一极径集，以及所述实际末端轨迹在所述极坐标系中的极径，得到第二极径集。

S23：根据所述第一极径集和所述第二极径集，确定误差序列。

所述第一极径集中的极径与所述第二极径集中的极径一一对应；

在本实施例中，根据以下公式，确定误差序列：

E＝[r₁₀-r₀₀,r₁₁-r₀₁,…,r_1N-r_0N]；

其中，E为误差序列，r_0N为第一极径集中的极径，r_1N为第二极径集中与r_0N对应的极径。

具体地，所述“间隙大小-近似熵值-PID参数”对照表的确定方法具体包括：

调整所述工业机器人机械臂的间隙大小。

针对每一间隙，在所述机械臂运行时，采集机械臂的实验间隙末端轨迹。

基于近似熵算法，确定所述实验间隙末端轨迹与所述预期末端轨迹的实验近似熵。

根据所述实验近似熵，调整工业机器人的PID控制器的PID参数，直至所述实验间隙末端轨迹与所述预期末端轨迹之间的误差小于误差阈值时，确定间隙大小、所述实验近似熵以及所述PID参数的对应关系。

根据间隙大小从0增大到最大间隙阈值时，间隙大小、实验近似熵以及PID参数的对应关系，确定“间隙大小-近似熵值-PID参数”对照表。

进一步地，具体地，PID控制器包括比例控制器k_p、积分控制器k_i和微分控制器k_d。

比例控制器的目的是成比例的放大与缩小偏差，但会产生一定的余差。

积分控制器的目的是消除自控系统产生的余差，只要偏差不为0，积分控制器就一直起作用。

微分控制器的目的是为了克服控制系统的滞后，与偏差变化的速度有关，变化越快，微分控制越显著，一般用于温度控制系统，本发明中kd并不显著，设置为0。

为了提高末端轨迹的控制效率，本发明考虑关节间隙演化的工业机器人末端轨迹控制方法还包括：

基于机械动力学原理，根据所述预期末端轨迹、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，建立无间隙时工业机器人动力学模型。

具体地，获取所述预期末端轨迹中的末端坐标。在本实施例中，两机械臂杆长分别为l₁和l₂，两机械臂与铅垂方向的夹角分别为θ₁和θ₂，两机械臂之间的间隙大小为σ，间隙与铅垂方向的夹角为(θ₁+θ₂)/2，根据以下公式，确定末端坐标：

其中，(x_c,y_c)为末端坐标，l₁为第一机械臂杆长，l₂为第二机械臂杆长，θ₁为第一机械臂转角，θ₂为第二机械臂转角。

根据所述末端坐标、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，确定系统动能和系统势能。具体地，根据以下公式，确定系统动能：

其中，E_K为系统动能，m₁为第一机械臂质量，m₂为第二机械臂质量；

根据以下公式，确定系统势能：

E_P＝m₁gl₁cosθ₁+m₂g(l₁cosθ₁+l₂cos(θ₁+θ₂))；

其中，E_P为系统势能，g为重力加速度，m₁为第一机械臂质量，m₂为第二机械臂质量。

根据所述系统动能和系统势能，采用拉格朗日方程，确定工业机器人动力学模型。

进一步地，所述根据所述系统动能和系统势能，采用拉格朗日方程，确定工业机器人动力学模型具体包括：

根据所述系统动能和系统势能，可得拉格朗日方程中各项偏导数：

将各项偏导数代入拉格朗日方程中，并经整理后化简可得如下方程组：

其中：

D₁₁₁＝D₂₂₂＝0，

D₁₂₂＝-m₂l₁l₂sinθ₂，

D₂₁₁＝m₂l₁l₂sinθ₂，

D₁₁₂＝D₁₂₁＝-m₂l₁l₂sinθ₂，

D₂₁₂＝D₂₂₁＝0，

D₁＝-(m_i+m₂)gl₁sinθ₁-m₂gl₂sin(θ₁+θ₂)，

D₂＝-m₂gl₂sin(θ₁+θ₂)。

如此，当第一机械臂杆长l₁、第二机械臂杆长l₂、第一机械臂质量m₁、第二机械臂质量m₂给定之后，由预期末端轨迹L₀(t)＝(x_c(t)，y_c(t))可以求出两机械臂的转角θ₁(t)，θ₂(t)，然后经由上述的方程组即可求出两个机械臂对应电机的输出力矩M₁(t)，M₂(t)。对预期末端轨迹进行离散化，将预期末端轨迹分成N等份，得到含有N个元素的序列，其中每个元素为预期末端轨迹上一个点的坐标，t为序列的编号，t＝1时x_c(t)，y_c(t)为第一个点的末端坐标，θ₁(t)，θ₂(t)则是根据雅可比矩阵算出来的第一个点对应的两机械臂转角，M₁(t)，M₂(t)为第一个点对应的两电机的输出力矩。

在实际工况中，考虑初始间隙为0，不设置控制器参数，当轨迹误差超出允许范围时，测量此时的轨迹误差近似熵，在“间隙大小-近似熵值-PID参数”对照表中找出对应的间隙大小以及控制器参数对系统进行控制，使得轨迹误差回到允许范围内。而随着间隙的增大，当轨迹误差再次超出允许范围时，将PID控制器参数清零，测量此时无控制的轨迹误差近似熵，在“间隙大小-近似熵值-PID参数”对照表中找出此时对应的间隙大小以及PID控制器参数，调节PID参数使得轨迹误差回到允许范围内，如此循环往复，直到机器人的机械系统彻底不可控。

为了更好的理解本发明的方案，下面结合具体实施例进一步进行说明。

针对无间隙、间隙等级1和间隙等级2三种工况下由试验台测量得到工业机器人的末端轨迹L₁(t)，L₂(t)，L₃(t)，具体地，在Solid works平台中，针对特定输入θ₁(t)和θ₂(t)，分别由激光跟踪器记录下无间隙、间隙等级1和间隙等级2三种工况下的末端轨迹L₁(t)，L₂(t)，L₃(t)。

基于近似熵复杂度分析法得到L₁(t)，L₂(t)，L₃(t)与预期末端轨迹L₀(t)的误差近似熵Apen₁，Apen₂，Apen₃。具体步骤为：

(1)计算误差E₁，E₂，E₃：

分别对预期末端轨迹L₀(t)与L₁(t)，L₂(t)，L₃(t)进行采样，以电机位置为坐标原点建立极坐标系，则L₀(t)，L₁(t)，L₂(t)，L₃(t)均可以表示成L₀(r_L0，θ_L0)，L₀(r_L1，θ_L1)，L₀(r_L2，θ_L2)和L₀(r_L3，θ_L3)，L₀(t)起点与终点坐标分别为(r_q，θ_q)，(r_z，θ_z)，r_L0为预期末端轨迹在极坐标系中的极径集，θ_L0为预期末端轨迹在极坐标系中的极角集，同理可知r_L1、r_L2、r_L3、θ_L1、θ_L2和θ_L3的含义。

对弧度θ进行N等分采样：

相应的r组成一个序列

其中r₀₀＝r_q，r_0N＝r_z,同时当弧度θ选取θ_L0中的值时，对应的L₁(t)也存在极径序列

考虑L₁(t)与L₀(t)的误差序列E₁＝[r₁₀-r₀₀，r₁₁-r₀₁，…，r_1N-r_0N]＝[e₁₀，e₁₁，…，e_1N]，同理也可以得到E₂与E₃。

(2)计算误差近似熵Apen₁，Apen₂，Apen₃。

(2.1)定义参数a＝2，b＝0.2std，其中a为整数，表示比较向量的长度，b为实数，表示“相似度”的度量值，std是原序列的标准差。

(2.2)针对长度为N+1的误差序列E₁＝[e₁₀,e₁₁,...e_1N,]，重构m维向量X(1)，X(2)，...，X(N-M+1)，X(N-m+2)，其中X(i)＝[e_1(i-1),e_1(i),...,e_1(i+m-2)]。

(2.3)对于1≤i≤N-M+2，统计满足以下条件的向量个数：

其中

e_1(i)为X(i)中的元素，d表示向量X(i)与X(j)的距离，由对应元素的最大差值决定，j的取值范围与i一样，也包含j＝i。

(2.4)定义

则近似熵Apen1＝Φ^m(b)-Φ^m+1(b)。同理可以算出Apen2，Apen3。

分别针对无间隙、间隙等级1和间隙等级2三种工况下设计三个不同参数的PID控制器，参数分别为P₁，I₁，D₁；P₂，I₂，D₂；P₃，I₃，D₃，使得相应工况下误差在工程控制允许范围之内。本实施例中，PID参数是通过做实验仿真试验出来的，通过调试记录下使得末端轨迹最接近预期末端轨迹时对应的PID参数，与此时的间隙大小σ和相应的Apen对应起来建立表格。

具体的参数设置参考仿真情况而定，根据三种不同的工况可以得出三组不同的控制器参数P₁，I₁，D₁；P₂，I₂，D₂；P₃，I₃，D₃。

重复上述步骤，针对间隙σ＝0到不可控点σ＝σ_max，σ_max为已经基本无法通过pid控制补偿其间隙存在用以维持系统重复定位精度的临界点，由于间隙的增大，末端轨迹的时间复杂度将越来越大，考虑间隙从σ＝0到σ_max，按照等比例的分成n段，节点为σ₀，σ₁，...，σ_n，其中σ₀＝0，σ_n＝σ_max，计算出对应的近似熵：Apen₀，Apen₁，...，，Apen_n，同时调整好对应的PID控制器参数[kp₀,ki₀,kd₀]，[kp₁,ki₁,kd₁]，...，[kp₂₀,ki₂₀,kd_n]，建立“Apen-σ-[kp,ki,kd]”对照表。

在实际工况中，当由于间隙变大导致偏差超出允许范围，近似熵也随之超出限定值，可以直接根据此时测得的偏差近似熵对应的调整PID控制器参数至下一个区间，重新保证系统在误差允许范围内的重复定位精度。

以下以具体的实例，选择两个自由度机械臂作为计算对象，具体的参数为：第一机械臂杆长l₁＝2m，第二机械臂杆长l₂＝1m，间隙大小σ分别为0m，0.05m和0.15m，重力加速度g＝9.8m/s²，预期末端轨迹为以原点为中心x轴方向长a＝3m，y轴方向宽b＝4m的矩形，且转速ω＝π/50rad/s。

工业机器人末端轨迹控制方法，具体包括：

步骤一：在Solid works平台上根据给定的杆长，机械臂结构以及关节间隙大小等参数建立机械臂的三维模型，采用激光跟踪仪实时跟踪机械臂末端末端位置。

步骤二：机械臂结构如图2所示，基于机械动力学原理得到无间隙工业机器人动力学模型，并得到预期末端轨迹L₀。具体步骤如下：

考虑预期末端轨迹为以原点为中心，转速ω＝π/50rad/s，得出预期末端轨迹L₀(t)的坐标为：

由

经由matlab计算可以得出θ₁(t)和θ₂(t)，如图3所示。

步骤三：在Solidworks平台中，针对特定输入θ₁(t)和θ₂(t)，分别由激光跟踪器记录下无间隙、间隙等级1和间隙等级2三种工况下的末端轨迹L₁(t)，L₂(t)，L₃(t)，如图4-图6所示。

步骤四：基于近似熵复杂度分析法借助matlab计算出L₁(t)，L₂(t)，L₃(t)与L₀(t)的误差近似熵Apen₁，Apen₂，Apen₃：

Apen_1x＝Apen_1y＝0；

Apen_2x＝0.0426，Apen_2y＝0.0499；

Apen_3x＝0.0575，Apen_1y＝0.0674。

步骤五：易知无间隙时轨迹复杂度为0，无需控制，故而控制器参数选择kp₂＝0，ki₂＝0，kd₂＝0，分别针对间隙等级1和间隙等级2的不同工况设计两个不同参数的PID控制器，参数分别为P₂I₂D₂，P₃I₃D₃，使得相应工况下误差在工程控制允许范围之内，控制后轨迹如图7和图8所示，得到

kp₂＝0.9899；ki₂＝0，kd₂＝0；

kp₃＝0.9697；ki₃＝0，kd₃＝0。

步骤六：重复前面工作，针对间隙σ＝0增大到σ＝0.2m，考虑间隙从σ＝0到σ＝0.2m，按照等比例的分成20段，节点为σ₀，σ₁，...，σ₂₀，其中σ₀＝0，σ₂₀＝0.2m，测出除0外节点处所对应的近似熵：Apen₁，Apen₂，...，Apen₂₀，如图9所示,对应的控制器参数分别为[kp₀,ki₀,kd₀]，[kp₁,ki₁,kd₁]，...，[kp₂₀,ki₂₀,kd₂₀]建立“Apen-σ-[kp,ki,kd]”对照表。

本发明可针对实际工况中关节间隙不断演化的机械系统，根据机器人参数提前计算出“间隙大小-近似熵值-PID参数”对照表，使得机器人在实际工作中随着间隙增大可以自动地调节相应的PID参数，令间隙在可控范围内时，系统精度均保持在允许范围内，从而避免实际工作中由于间隙增大从而需要频繁的对PID控制器参数进行调节，节省了大量的重复性工作，并且用实际计算出的偏差近似熵针对性的反映出了关节中较难测出的间隙大小，使得控制效果更加精确，具有适用范围广，应用方便，且控制效果出众的优点。

如图10所示，本发明考虑关节间隙演化的工业机器人末端轨迹控制系统包括：末端轨迹采集单元1、误差确定单元2、近似熵确定单元3、PID参数确定单元4以及控制单元5。

其中，所述末端轨迹采集单元1用于采集工业机器人在实际运行过程中机械臂的实际末端轨迹。

所述误差确定单元2与所述末端轨迹采集单元1连接，所述误差确定单元2用于计算所述实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列。

所述近似熵确定单元3与所述误差确定单元2连接，所述近似熵确定单元3用于根据所述误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的近似熵。

所述PID参数确定单元4与所述近似熵确定单元3连接，所述PID参数确定单元4用于根据所述近似熵以及预先确定的“间隙大小-近似熵值-PID参数”对照表，确定对应的PID参数。

所述控制单元5与所述PID参数确定单元4连接，所述控制单元5用于通过PID控制器根据所述PID参数，控制所述工业机器人电机的输出力矩，进而调整所述工业机器人机械臂末端的运行轨迹。

所述工业机器人的机械臂包括第一机械臂和第二机械臂，所述第一机械臂的一端与电机连接，所述第一机械臂的另一端与所述第二机械臂连接。

具体地，所述误差确定单元2包括：极坐标系建立模块、极径确定模块以及误差序列确定模块。

其中，所述极坐标系建立模块用于以所述电机所在位置为原点建立极坐标系。

所述极径确定模块分别与所述极坐标系建立模块及所述末端轨迹采集单元1连接，所述极径确定模块用于获取所述预期末端轨迹在所述极坐标系中的极径，得到第一极径集，以及所述实际末端轨迹在所述极坐标系中的极径，得到第二极径集。

所述误差序列确定模块与所述极径确定模块连接，所述误差序列确定模块用于根据所述第一极径集和所述第二极径集，确定误差序列。

相对于现有技术，本发明考虑关节间隙演化的工业机器人末端轨迹控制系统与上述考虑关节间隙演化的工业机器人末端轨迹控制方法的有益效果相同，在此不再赘述。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，所述考虑关节间隙演化的工业机器人末端轨迹控制方法包括：

采集工业机器人在实际运行过程中机械臂的实际末端轨迹；

计算所述实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列；

根据所述误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的误差近似熵，具体包括：

针对长度为N+1的误差序列E₁＝[e₁₀,e₁₁,...e_1N,]，重构m维向量X(1)，X(2)，...，X(N-M+1)，X(N-m+2)，其中X(i)＝[e_1(i-1),e_1(i),...,e_1(i+m-2)]；

对于1≤i≤N-m+2，统计满足以下条件的向量个数：

其中，

e_1(i)为X(i)中的元素，d表示向量X(i)与X(j)的距离，由对应元素的最大差值决定，j的取值范围与i一样，也包含j＝i，a为整数，表示比较向量的长度，b为实数，表示“相似度”的度量值；

采用公式Apen1＝Φ^m(b)-Φ^m+1(b)确定误差近似熵，其中，Apen1为实际末端轨迹与预期末端轨迹的误差近似熵，

根据所述误差近似熵以及预先确定的“间隙大小-近似熵值-PID参数”对照表，确定对应的PID参数；

通过PID控制器根据所述PID参数，控制所述工业机器人电机的输出力矩，进而调整所述工业机器人机械臂末端的运行轨迹。

2.根据权利要求1所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，所述工业机器人的机械臂包括第一机械臂和第二机械臂，所述第一机械臂的一端与电机连接，所述第一机械臂的另一端与所述第二机械臂连接；

所述计算所述实际末端轨迹与预期末端轨迹之间的误差序列，具体包括：

以所述电机所在位置为原点建立极坐标系；

获取所述预期末端轨迹在所述极坐标系中的极径，得到第一极径集，以及所述实际末端轨迹在所述极坐标系中的极径，得到第二极径集；

根据所述第一极径集和所述第二极径集，确定误差序列。

3.根据权利要求2所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，所述第一极径集中的极径与所述第二极径集中的极径一一对应；

根据以下公式，确定误差序列：

E＝[r₁₀-r₀₀,r₁₁-r₀₁,...,r_1N-r_0N]；

其中，E为误差序列，r_0N为第一极径集中的极径，r_1N为第二极径集中与r_0N对应的极径。

4.根据权利要求1所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，所述“间隙大小-近似熵值-PID参数”对照表的确定方法具体包括：

调整所述工业机器人机械臂的间隙大小；

针对每一间隙，在所述机械臂运行时，采集机械臂的实验间隙末端轨迹；

基于近似熵算法，确定所述实验间隙末端轨迹与所述预期末端轨迹的实验误差近似熵；

根据所述实验误差近似熵，调整工业机器人的PID控制器的PID参数，直至所述实验间隙末端轨迹与所述预期末端轨迹之间的误差小于误差阈值时，确定间隙大小、所述实验误差近似熵以及所述PID参数的对应关系；

根据间隙大小从0增大到最大间隙阈值时，间隙大小、实验误差近似熵以及PID参数的对应关系，确定“间隙大小-近似熵值-PID参数”对照表。

5.根据权利要求2所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，所述考虑关节间隙演化的工业机器人末端轨迹控制方法还包括：

基于机械动力学原理，根据所述预期末端轨迹、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，建立无间隙时工业机器人动力学模型。

6.根据权利要求1所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，所述基于机械动力学原理，根据所述预期末端轨迹、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，建立无间隙时工业机器人动力学模型，具体包括：

获取所述预期末端轨迹中的末端坐标；

根据所述末端坐标、第一机械臂质量、第二机械臂质量、第一机械臂杆长、第二机械臂杆长、第一机械臂转角以及第二机械臂转角，确定系统动能和系统势能；

根据所述系统动能和系统势能，采用拉格朗日方程，确定工业机器人动力学模型。

7.根据权利要求6所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，根据以下公式，确定末端坐标：

其中，(x_c,y_c)为末端坐标，l₁为第一机械臂杆长，l₂为第二机械臂杆长，θ₁为第一机械臂转角，θ₂为第二机械臂转角。

8.根据权利要求6所述的考虑关节间隙演化的工业机器人末端轨迹控制方法，其特征在于，根据以下公式，确定系统动能：

其中，E_K为系统动能，m₁为第一机械臂质量，m₂为第二机械臂质量；

根据以下公式，确定系统势能：

E_P＝m₁gl₁cosθ₁+m₂g(l₁cosθ₁+l₂cos(θ₁+θ₂))；

其中，E_P为系统势能，g为重力加速度，m₁为第一机械臂质量，m₂为第二机械臂质量，l₁为第一机械臂杆长，l₂为第二机械臂杆长，θ₁为第一机械臂转角，θ₂为第二机械臂转角。

9.一种考虑关节间隙演化的工业机器人末端轨迹控制系统，其特征在于，所述考虑关节间隙演化的工业机器人末端轨迹控制系统包括：

末端轨迹采集单元，用于采集工业机器人在实际运行过程中机械臂的实际末端轨迹；

误差确定单元，与所述末端轨迹采集单元连接，用于计算所述实际末端轨迹与无间隙时的预期末端轨迹之间的误差序列；

近似熵确定单元，与所述误差确定单元连接，用于根据所述误差序列，基于近似熵算法，确定所述实际末端轨迹与所述预期末端轨迹的误差近似熵，具体包括：

针对长度为N+1的误差序列E₁＝[e₁₀,e₁₁,...e_1N,]，重构m维向量X(1)，X(2)，...，X(N-M+1)，X(N-m+2)，其中X(i)＝[e_1(i-1),e_1(i),...,e_1(i+m-2)]；

对于1≤i≤N-m+2，统计满足以下条件的向量个数：

其中，

e_1(i)为X(i)中的元素，d表示向量X(i)与X(j)的距离，由对应元素的最大差值决定，j的取值范围与i一样，也包含j＝i，a为整数，表示比较向量的长度，b为实数，表示“相似度”的度量值；

采用公式Apen1＝Φ^m(b)-Φ^m+1(b)确定误差近似熵，其中，Apen1为实际末端轨迹与预期末端轨迹的误差近似熵，

PID参数确定单元，与所述近似熵确定单元连接，用于根据所述误差近似熵以及预先确定的“间隙大小-近似熵值-PID参数”对照表，确定对应的PID参数；

控制单元，与所述PID参数确定单元连接，用于通过PID控制器根据所述PID参数，控制所述工业机器人电机的输出力矩，进而调整所述工业机器人机械臂末端的运行轨迹。

10.根据权利要求9所述的考虑关节间隙演化的工业机器人末端轨迹控制系统，其特征在于，所述工业机器人的机械臂包括第一机械臂和第二机械臂，所述第一机械臂的一端与电机连接，所述第一机械臂的另一端与所述第二机械臂连接；

所述误差确定单元包括：

极坐标系建立模块，用于以所述电机所在位置为原点建立极坐标系；

极径确定模块，分别与所述极坐标系建立模块及所述末端轨迹采集单元连接，用于获取所述预期末端轨迹在所述极坐标系中的极径，得到第一极径集，以及所述实际末端轨迹在所述极坐标系中的极径，得到第二极径集；

误差序列确定模块，与所述极径确定模块连接，用于根据所述第一极径集和所述第二极径集，确定误差序列。

Images