CN114800521A - 一种带约束的三自由度机械臂固定路径点运动控制系统 - Google Patents

Abstract

本发明涉及一种带约束的三自由度机械臂固定路径点运动控制系统，利用欧拉‑拉格朗日法建立所述机械臂动力学模型，给出其状态空间的动力学方程表达形式，利用所述机械臂的连杆间的约束关系构造一种轨迹跟踪控制器，对所述机械臂动力学模型的运动规划进行计算。本发明具备快速响应能力；能够获得准确的位置偏差；控制方法具有较强的鲁棒性和自适应性；调速范围和力矩输出范围足够大。

Description

一种带约束的三自由度机械臂固定路径点运动控制系统

技术领域

本发明涉及工业机器人技术领域，具体涉及一种带约束的三自由度机械臂固定路径点运动控制系统。

背景技术

机械臂在生产和生活中用途广泛，作为机电一体化设备，能够高效完成各种复杂和危险的作业，提高生产效率，在工业、航天、医疗等领域中得到广泛的应用。机械臂运动控制本质上是用于机械臂关节动作控制的多轴位置随动系统，其被控量(输出量)是负载机械空间位置的角位移，当位置给定量(输入量)做特定任务变化时，系统要能使输出量快速而准确地跟踪给定量变化。因此，机械臂运动控制系统包括轨迹规划和轨迹跟踪两个部分，其任务是不仅要对机械臂的行走轨迹进行规划，还需要对其每个轴的伺服驱动机构的角位移、角速度以及角加速度进行闭环控制，以实现机械臂整体的位姿联合控制。机械臂轨迹规划是根据任务要求计算轨迹并研究运动中机械臂关节的位移、角速度和角加速度的生成方法。这些轨迹既可以在关节空间生成，也可以在笛卡尔空间生成。在关节空间中进行轨迹规划，是用关节变量来描述机械臂的运动，实时性好，但很难确定连杆和末端执行器的姿态；在笛卡尔空间进行轨迹规划，是将路径约束映射成关节空间坐标，然后确定满足参数约束的关节轨迹。相比较而言，在关节空间中进行轨迹规划计算量小，实时性高，而且能够避免奇异点现象。

现有技术中，一部分从模型的准确性以及模型参数的不确定性出发，研究其对控制精度的影响，例如“基于多层神经网络的模型不确定性机械臂运动控制方法”、“一种多自由度机械臂柔性控制方法和系统”；一部分只对轨迹设计的是否合理进行了研究，如“一种机械臂多点运动轨迹规划方法”等，但在轨迹规划过程中，对于满足具体任务需求的轨迹函数及其约束条件的讨论还需要进一步细化；还有一部分专利专门研究轨迹跟踪控制器的设计，如“一种机械臂分散化神经鲁棒控制的轨迹跟踪算法”，这些专利没有考虑轨迹合理性的因素，忽略了机械臂惯性力的影响，降低了其适用性，同时在跟踪精度和系统稳定性方面也有待进一步提高。

发明内容

为了克服现有技术的不足，本发明设计一套合理的一种带约束的三自由度机械臂固定路径点运动控制系统，所述模型具有以下四个基本特征：具备快速响应能力；能够获得准确的位置偏差；控制方法具有较强的鲁棒性和自适应性；调速范围和力矩输出范围足够大。本发明技术方案为：一种带约束的三自由度机械臂固定路径点运动控制系统，利用欧拉-拉格朗日法建立所述机械臂动力学模型，给出其状态空间的动力学方程表达形式，利用所述机械臂的连杆间的约束关系构造一种轨迹跟踪控制器，对所述机械臂动力学模型的运动规划进行计算，所述三自由度机械臂固定路径点运动控制系统的机械臂动力学模型的动力学方程为：

其中：q＝[q₁,q₂,q₃]^T是系统的角度向量；τ＝[τ₁,0,τ₃]^T是控制力矩向量；M(q)∈R^{3 ×3}是惯性矩阵，具有正定性和对称性；

是哥氏力和离心力的结合向量；

则所述机械臂动力学模型的状态空间形式为：

其中：

f(x)＝[x₄,x₅,x₆,f₁,f₂,f₃]

g(x)＝[g_1(x),g_2(x),g_3(x)]^T

x为状态向量变量，f(x)为状态目标函数，g(x)为关于x的非线性约束关联函数，g_1(x)为3阶零矩阵，g_ij为关于g(x)为的分量函数；令

则有：

所述M(q)和

的具体分别为如下形式：

所述轨迹跟踪控制器的构造方法为：

构造滑模面为公式：

对滑模面求导可得公式：

其中：x_1d＝q_1d x_2d＝q_2d x_3d＝q_3d

β₁，β₂，β₃为常数。

设计滑模趋近律为公式：

其中

k_i＞1，η_i＞0，ζ_i＞0，双幂次趋近律以|S_i|＝1为界，将系统分为两个阶段：

第一阶段：当|S_i|＜1时系统功能主要取决于前一项

此时

越大，趋近速度越快，但带来的抖振也相应增大，η_i越小系统速度和抖振同时减小。

第二阶段：当|S_i|＞1时，系统功能主要取决于第二项

k_i越大，趋近速度越快的同时抖振也增大；ζ_i越小，抖振和速度同时变小。因此，采用这种方法需要根据实验选择合适的参数。同时，引入边界层使系统状态控制在一定范围内，并用饱和函数来定义，选取的饱和函数为公式：

其中，为保证系统稳定性，令1＜γ＜2。此时系统整体趋近律为公式：

由上述公式可得各关节的控制律τ＝[τ₁,τ₂,τ₃]^Τ。

给出如下设定条件：

|v_i,j|≤1.57rad/s

|a_i,j|≤2.09rad/s²

关节1、2、3的起始点和三个固定点的关节向量分别为：

q₁＝[0.50,1.54,0.32,-0.55]rad，

q₂＝[-1.20,-1.51,-1.02,0.2798]rad，

q₃＝[1.70,0.04,-2.20,-1.09]rad。

本发明有益效果：

1)本发明所述模型具备快速响应能力；

2)本发明所述模型能够获得准确的位置偏差；

3)本发明所述模型控制方法具有较强的鲁棒性和自适应性；

4)本发明所述模型调速范围和力矩输出范围足够大。

附图说明：

图1为本发明在系统方案的结构示意图；

图2为本发明的三关节角度轨迹图；

图3为本发明的粒子群算法在轨迹规划方法中的流程图；

图4为本发明三关节角速度轨迹图；

图5为本发明三关节加速度轨迹图；

图6为本发明三关节力矩曲线图；

图7为本发明三关节轨迹跟踪误差曲线图。

具体实施方式：

参见图1至图7所示，本发明涉及一种带约束的三自由度机械臂固定路径点运动控制系统，利用欧拉-拉格朗日法建立所述机械臂动力学模型，给出其状态空间表达形式，基于粒子群算法对所述机械臂动力学模型的运动规划进行计算，所述机械臂动力学模型的动力学方程为：

其中：q＝[q₁,q₂,q₃]^T是系统的角度向量；τ＝[τ₁,0,τ₃]^T是控制力矩向量；M(q)∈R^{3 ×3}是惯性矩阵，具有正定性和对称性；

是哥氏力和离心力的结合向量。

则系统模型的状态空间形式为：

其中：

f(x)＝[x₄,x₅,x₆,f₁,f₂,f₃]

g(x)＝[g_1(x),g_2(x),g_3(x)]^T

x为状态向量变量，f(x)为状态目标函数，g(x)为关于x的非线性约束关联函数，g_1(x)为3阶零矩阵，g_ij为关于g(x)为的分量函数；令

则有：

优选的，所述M(q)和

的具体分别为如下形式：

所述粒子群优化算法是一种基于群体的随机优化算法，所述群体中的每个个体称为粒子，所述群体的规模为N，每个粒子在D维空间中的位置向量为X_i＝(x_i1,x_i2,x_i3,x_i4,...,x_id,...,x_iD)；速度向量为V_i＝(v_i1,v_i2,v_i3,v_i4,...,v_id,...,v_iD)；个体的最优位置为P_i＝(p_i1,p_i2,p_i3,p_i4,...,p_id,...,p_iD)；所述群体的最佳位置表示为P_g＝(p_g1,p_g2,p_g3,p_g4,...,p_gd,...,p_gD)，个体最优位置的更新模型为：

所述群体的最佳位置是所有个体的最佳位置，其速度和位置的更新模型如下式所示：

其中，ω为惯性权重，决定了算法收敛速度的强弱。ω越大收敛速度越强，反之越弱。实验证明ω在0.8-1.2之间粒子群算法有更快的收敛速度。c₁和c₂为学习因子，也称加速常数；r₁和r₂为[0,1]范围内的均匀随机数。

本发明根据机械臂系统连杆间的约束关系设计轨迹跟踪控制器。

构造滑模面为公式：

对滑模面求导可得公式：

其中：x_1d＝q_1d x_2d＝q_2d x_3d＝q_3d

β₁，β₂，β₃为常数。

设计滑模趋近律为公式：

其中

k_i＞1，η_i＞0，ζ_i＞0，双幂次趋近律以|S_i|＝1为界，将系统分为两个阶段：

第一阶段：当|S_i|＜1时系统功能主要取决于前一项

此时

越大，趋近速度越快，但带来的抖振也相应增大，η_i越小系统速度和抖振同时减小。

第二阶段：当|S_i|＞1时，系统功能主要取决于第二项

k_i越大，趋近速度越快的同时抖振也增大；ζ_i越小，抖振和速度同时变小。因此，采用这种方法需要根据实验选择合适的参数。同时，引入边界层使系统状态控制在一定范围内，并用饱和函数来定义，选取的饱和函数为公式：

其中，为保证系统稳定性，令1＜γ＜2。此时系统整体趋近律为公式：

由上述公式可得各关节的控制律τ＝[τ₁,τ₂,τ₃]^Τ。

给出如下设定条件：

|v_i,j|≤1.57rad/s

|a_i,j|≤2.09rad/s²

关节1、2、3的起始点和三个固定点的关节向量分别为：

q₁＝[0.50,1.54,0.32,-0.55]rad，

q₂＝[-1.20,-1.51,-1.02,0.2798]rad，

q₃＝[1.70,0.04,-2.20,-1.09]rad；

粒子群算法的迭代次数为200。

据此得到三个关节的角度、角速度、角加速度轨迹如图3-图5所示。本专利采用的所述基于粒子群优化的三-五-三组合多项式插值法结构框图如图2所示。

本发明利用三次多项式函数为每两固定点进行轨迹规划。

首先在笛卡尔空间中确定机械臂的起始点和三个固定点的空间位置，再通过机械臂逆运动学模型得到对应的关节位置。由于轨迹中有一个起始点和三个路径点，故有三段轨迹插值。设置如下参数：i＝1,2,3表示关节数；j＝1,2,3表示插值轨迹段数；θ_i,j表示第i个关节的插值角度向量。

建立第i个关节角度关于时间的3-5-3多项式函数，如公式(3)所示：

其中，l_i,1(t)表示第i个关节角度的第一段三次多项式插值轨迹；l_i,2(t)为第i个关节角度的第二段五次多项式插值轨迹；l_i,3(t)表示第i个关节角度的第三段三次多项式插值轨迹；系数a_i为多项式对应的相关系数。

令多式的系数a_i为向量a＝[a_i,13,a_i,12,a_i,11,a_i,10,a_i,25,...,a_i,20,a_i,33,...,a_i,30]^Τ,设多项式函数的时间矩阵为T，则对应的矩阵解为T^14×14a^14×1＝θ^14×1，其中T和θ公式(4)和公式(5)所示。当由于第二段轨迹规划时关节的起始速度和加速度与第一段轨迹末端关节的速度与加速度一致，所以在第二段五次多项式插值轨迹规划前需要做完第一段轨迹。在时间矩阵T中，每一行都与列向量a的元素相对应：一至三行表示第一段三次多项式插值，需消除初始时刻所对应的第二段五次多项式所对应初始时刻的轨迹，四行至九行同理。

θ＝[0,0,0,0,0,0,x₃,0,0,x₀,0,0,x₂,x₁]^Τ

其中，x₃对应T的第七行和a的第七元素，表示第三段三次多项式插值轨迹规划的位移；x₀表示时间为0时第一段三次多项式插值规划的初始位移；x₂表示时间为0时第三段三次多项式插值规划的初始位移；x₁表示时间为0时第二段五次多项式插值规划的初始位移。

本发明设计一种多约束条件下基于时间最优的联合轨迹规划方法，对固定路径点间的轨迹进行时间最优规划。具体方案如下：

对于多固定路径点轨迹规划任务，时间最优指标应为多段时间之和最小，则适应度函数应定义如公式(6)：

f(t)＝(t₁+t₂+t₃)_min

由于机械臂系统构造、材料结构和电机选型等多种原因，机械臂会存在关节速度、加速度的极限。如果在轨迹规划中没有将这类限制因素充分考虑在内，得到的轨迹通常既会产生速度、加速度短时内急剧变化的现象，又不符合机械臂实际工作情况。这种不稳定的轨迹在实际执行过程中不仅会对周围环境产生安全隐患，而且会对机械臂的系统构造产生不可逆的影响甚至损坏，同时对轨迹跟踪控制的要求也更高，控制难度也更大。为了保证任务的安全性和准确性，同时符合机械臂系统的实际要求，因此在轨迹规划任务中引入速度和加速度的双约束条件。设置速度约束：

其中v_i,j表示第i个关节的第j段轨迹的速度，V_i,j为第i个关节的第j段轨迹的速度最大限制。设置加速度约束：

其中a_i,j表示第i个关节的第j段轨迹的加速度，aa_i,j为第i个关节的第j段轨迹的加速度最大限制。

多约束下的时间最优粒子群规划的步骤为：

(1)确定粒子的维度，确定粒子群的初始种群数量，初始化粒子的位置和速度，设定最优适应度值；

(2)设定适应度函数，并初步筛选具有更优适应度值的粒子，计算多项式的系数；

(3)由已得多项式系数计算关节速度、加速度轨迹，并检验是否超出所设定的最大限制值，即满足式(7)和式(8)；

(4)遍历所有粒子，计算出每个粒子的适应度值，并检验每次的适应度值是否相比更新的最优适应度值更小，同时检验由该粒子计算得到的多项式系数所对应的关节速度、加速度轨迹是否存在超过限制的值。若不存在，则更新最优粒子，同时更新最优适应度值；若存在，跳过该粒子，对下一粒子进行检测，直至检测完所有粒子；

(5)每个粒子与更新的最优粒子的适应度值相比较，并得到当前整体最优粒子，再与群体所经历的全局最优粒子比较，若出现适应度值更小的情况，则更新全局最优粒子和最优适应度值；

(6)一直迭代至终止次数，停止更新，并根据全局最优粒子得到对应的多项式系数，计算相对应的关节角度、角速度、角加速度轨迹。

从如图3-图5所示的实验结果可以看到：每个关节的角速度和角加速度轨迹均未超出最大绝对值，关节轨迹满足所有约束条件。关节角速度轨迹和关节角加速度轨迹在第二段轨迹中更接近所限制最大值，表明关节整体第二段轨迹在加速收敛。另外，由于三次多项式插值法和五次多项式插值法需要设置时间，若时间设置不当，会导致关节速度和加速度不稳定，使得系统稳定性下降，任务完成效率变低甚至无法完成。而本专利设计的基于粒子群优化的轨迹规划方法在保证时间最短的前提下，同时能够保证各关节角度、角速度、角加速度轨迹的平滑性和连续性，提高任务完成的效率和准确性。

针对图3给出的期望轨迹，专利采用改进滑膜控制器对其进行跟踪控制，得出的控制律如图6所示，其轨迹跟踪误差曲线如图7所示。

由图6可以看出采用双幂次饱和趋近律的滑模控制得出的力矩比较平稳，并且无明显抖振，能够使电机输出保持在额定功率内，保证了机械臂执行任务的精度。

由图7可以看出三个关节的轨迹跟踪误差均较小，最大幅值为0.015rad，并在一定时间内收敛到0附近，具有较高的轨迹跟踪精度。

Claims (2)

1.一种带约束的三自由度机械臂固定路径点运动控制系统，利用欧拉-拉格朗日法建立所述机械臂动力学模型，给出其状态空间的动力学方程表达形式，利用所述机械臂的连杆间的约束关系构造一种轨迹跟踪控制器，对所述机械臂动力学模型的运动规划进行计算，所述三自由度机械臂固定路径点运动控制系统的机械臂动力学模型的动力学方程为：

其中：q＝[q₁,q₂,q₃]^T是所述运动控制系统的角度向量；τ＝[τ₁,0,τ₃]^T是控制力矩向量；M(q)∈R^3×3是惯性矩阵，具有正定性和对称性；

是哥氏力和离心力的结合向量；

所述机械臂动力学模型的状态空间形式为：

其中：

f(x)＝[x₄,x₅,x₆,f₁,f₂,f₃]

g(x)＝[g_1(x),g_2(x),g_3(x)]^T

x为状态向量变量，f(x)为状态目标函数，g(x)为关于x的非线性约束关联函数，g_1(x)为3阶零矩阵，g_ij为关于g(x)为的分量函数；令

则有：

所述M(q)和

的具体分别为如下形式：

其特征是：

所述轨迹跟踪控制器的构造方法为：

构造滑模面为公式：

对滑模面求导可得公式：

其中：

β₁，β₂，β₃为常数；

设计滑模趋近律为公式：

其中

η_i＞0，ζ_i＞0，双幂次趋近律以|S_i|＝1为界，将所述运动控制系统分为两个阶段：

第一阶段：当|S_i|＜1时所述运动控制系统功能主要取决于前一项

此时

越大，趋近速度越快，但带来的抖振也相应增大，η_i越小所述运动控制系统速度和抖振同时减小；

第二阶段：当|S_i|＞1时，所述运动控制系统功能主要取决于第二项

k_i越大，趋近速度越快的同时抖振也增大；ζ_i越小，抖振和速度同时变小；因此，采用这种方法需要根据实验选择合适的参数；同时，引入边界层使系统状态控制在一定范围内，并用饱和函数来定义，选取的饱和函数为公式：

其中，为保证所述运动控制系统稳定性，令1＜γ＜2；此时所述运动控制系统整体趋近律为公式：

由上述公式可得各关节的控制律τ＝[τ₁,τ₂,τ₃]^Τ；

给出如下设定条件：

|v_i,j|≤1.57rad/s

|a_i,j|≤2.09rad/s²。

2.根据权利要求1所述的带约束的三自由度机械臂固定路径点运动控制系统，其特征是：

关节1、2、3的起始点和三个固定点的关节向量分别为：

q₁＝[0.50,1.54,0.32,-0.55]rad，

q₂＝[-1.20,-1.51,-1.02,0.2798]rad，

q₃＝[1.70,0.04,-2.20,-1.09]rad。

Images