CN112847351B - 一种基于指数趋近率的并联机器人轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及机器人智能控制技术领域，特别是涉及一种基于指数趋近率的并联机器人轨迹跟踪控制方法，包括以下步骤：S1、建立并联机器人系统的坐标系，计算并联机器人系统的动能和势能；S2、将步骤S1的计算结果引入拉格朗日方程，建立并联机器人系统的动力学模型；S3、以功率放大器、电液伺服阀、液压缸及其负载为广义控制对象，建立并联机器人系统的液压伺服系统模型；S4、采用滑膜控制中的指数趋近率控制算法作为动力学模型和液压伺服系统模型的轨迹跟踪控制算法，对并联机器人进行轨迹跟踪控制。本发明解决了现有并联机器人控制精度不高，控制不方便的问题。

Description

一种基于指数趋近率的并联机器人轨迹跟踪控制方法

技术领域

本发明涉及机器人智能控制技术领域，特别是涉及一种基于指数趋近率的并联机器人轨迹跟踪控制方法。

背景技术

随着机器人结构学的发展工业机器人的种类越来广泛，从机器人机构学大范围来划分，可分为串联式机器人、并联式机器人以及串联并联混合式的混联机器人三大类。并联机器人是指动平台和定平台，通过至少两个独立的运动链相连接，机构具有两个或者两个以上的自由度，以并联方式驱动的一种闭环的机器人。并联机器人和传统工业用串联机器人在应用上构成互补关系，它是一个封闭的运动链，和串联机器人相比较，一般由上下运动平台和两条或者两条以上运动支链构成，运动平台和运动支链之间构成一个或多个闭环机构，通过改变各个支链的运动状态，使整个机构具有多个可以操作的自由度。

目前，并联机器人在航天、海底作业、计算机辅助医疗设备、生物工程以及制造业等方面有着广泛而重要的应用。而并联机器人的核心是机器人的控制系统，并联机器人的先进程度和功能强弱通常都直接与其控制系统的性能有关，如何保证并联机器人准确到达空间某一状态并具有良好的跟踪姿态，是并联机器人实现点到点(PTP)运动和连续轨迹(CP)运动时经常遇到的问题，这就需要对并联机器人末端进行轨迹控制即位姿控制，但并联机器人系统具有复杂、非线性和混合性的特点。组织越复杂，特性越突出，控制越不方便，使得控制精度不高，无法满足实际生产应用的要求。因此需要探索一种有效的轨迹跟踪与控制算法。

发明内容

本发明的目的在于提供一种基于指数趋近率的并联机器人轨迹跟踪控制方法，解决现有并联机器人控制精度不高，控制不方便的问题。

为了达到上述目的，提供了一种基于指数趋近率的并联机器人轨迹跟踪控制方法，包括以下步骤：

S1、建立并联机器人系统的广义坐标系作为拉格朗日函数的参数，基于拉格朗日函数的参数计算并联机器人系统的动能和势能并得到拉格朗日量；

所述并联机器人系统包括连杆、动平台和定平台，以及用于设置在连杆端部且分别与动平台和定平台连接的旋转接头；并联机器人系统的广义坐标系中广义坐标表示为：

q^T＝[q₁,q₂,q₃…q_i]^T，

q_i表示各旋转接头的位移或旋转角度，其速度或角速度用

表示，T表示齐次变换矩阵，q^T表示势能广义坐标，

表示动能广义坐标；

所述并联机器人系统通过广义坐标系计算其动能和势能的计算公式如下：

P_总＝P_定+P_动+P_杆

其中，P_总为并联机器人系统的势能，P_定为定平台的势能，P_动为动平台的势能，P_杆为若干连杆的势能，K为是定平台动能和若干连杆的动能，v_p和ω_p分别是并联机器人系统的动平台的质心速度矢量和质心角速度矢量；m是并联机器人动平台的质量；m_i是杆的质量；v是杆的线速度矢量，I是并联机器人系统的位姿变量参数；

并联机器人系统的位姿变量参数I计算公式如下：

其中，l_i是并联机器人系统的动平台的位置，J表示精密并联机器人系统的雅可比矩阵；

S2、将步骤S1计算的拉格朗日量引入拉格朗日方程，建立并联机器人系统的动力学模型；

所述拉格朗日方程的计算公式如下：

所述拉格朗日量L的计算公式如下：

L＝K-P_总

其中，t是时间，τ_i表示作用在并联机器人系统第i旋转接头作用于连杆上的主动力或驱动力；K表示定平台动能和若干连杆的动能，P_总为并联机器人系统的势能；

S3、以并联机器人系统中的一组功率放大器、电液伺服阀、液压缸及其负载为广义控制对象，建立并联机器人系统的液压伺服系统模型；

S4、采用投影法进行精密并联机器人系统进行轨迹规划，获取期望轨迹，并将期望轨迹输入到并联机器人系统的液压伺服系统模型；

S5、采用滑膜控制中的指数趋近率控制算法作为动力学模型和液压伺服系统模型的轨迹跟踪控制算法，对并联机器人进行轨迹跟踪控制；

所述步骤S5中，指数趋近率控制算法的公式如下：

其中，ε是运动点接近开关面的速率，K表示常数；

用于跟踪并联机器人的轨迹的公式如下：

其中，

和

是并联机器人系统中外界干扰的上界和下界，ΔH是建模误差；M是并联机器人系统定平台和若干连杆的等效惯性质量；u是输入电压,C_i表示模型参数，e_i表示系统误差。

原理及优点：

滑模控制是一种变结构控制，即非线性控制，即控制的不连续性。这种控制方法允许系统按照规定的滑动模式移动。可不受目标参数和扰动的影响，其优点有：易于实现，无需系统识别，响应快；良好的鲁棒性和较小的参数变化等。本方案基于滑模控制的指数趋近率控制算法，能够让实际轨迹跟踪结果与期望轨迹基本一致，并将误差控制在一定范围内，以解决现有并联机器人控制精度不高，控制不方便的问题。

进一步，并联机器人系统的位姿变量参数I计算公式如下：

其中，l_i是并联机器人系统的动平台的位置，J表示精密并联机器人系统的雅可比矩阵。

进一步，旋转矩阵R的计算公式为：

其中，R是旋转矩阵，τ_i表示作用在并联机器人系统第i旋转接头作用于连杆上的主动力或驱动力，(x_d，z_d，y_d)是原点为O₁的坐标系O₁-X₁Y₁Z₁中的坐标，γ为O₁-X₁Y₁Z₁绕X旋转角度，β为O₁-X₁Y₁Z₁绕Y旋转角度，α为O₁-X₁Y₁Z₁绕Z旋转角度，cα＝cosα，cβ＝cosβ，cγ＝cosγ，sα＝sinα，sβ＝sinβ，sγ＝sinγ。

进一步，所述并联机器人系统的动力学模型的表达式如下：

τ＝I+K+L

其中，τ是动力学模型。

附图说明

图1为本发明实施例一种基于指数趋近率的并联机器人轨迹跟踪控制方法的逻辑框图；

图2为并联机器人投影法轨迹规划范例示意图；

图3为并联机器人系统中关节1的轨迹跟踪曲线；

图4为并联机器人系统中关节2的轨迹跟踪曲线；

图5为并联机器人系统中关节1的轨迹跟踪误差曲线；

图6为并联机器人系统中关节2的轨迹跟踪误差曲线。

具体实施方式

下面通过具体实施方式进一步详细说明：

实施例

一种基于指数趋近率的并联机器人轨迹跟踪控制方法，基本如附图1所示，包括以下步骤：

S1、建立并联机器人系统的广义坐标系作为拉格朗日函数的参数，基于拉格朗日函数的参数计算并联机器人系统的动能和势能并得到拉格朗日量；

所述并联机器人系统包括连杆、动平台和定平台，以及用于设置在连杆端部且分别与动平台和定平台连接的旋转接头；本实施例中，由连杆、动平台和定平台组合而成的并联机器人系统具有六组相同的液压驱动伺服系统，因此n＝6,每组液压驱动伺服系包括功率放大器、电液伺服阀、液压缸及其负载，并联机器人系统的广义坐标系中广义坐标表示为：

q^T＝[q₁,q₂,q₃…q_i]^T，

q_i表示各旋转接头的位移或旋转角度，其速度或角速度用

表示，T表示齐次变换矩阵，q^T表示势能广义坐标，

表示动能广义坐标。

所述并联机器人系统通过广义坐标系计算其动能和势能的计算公式如下：

P_总＝P_定+P_动+P_杆

其中，P_总为并联机器人系统的势能，P_定为定平台的势能，P_动为动平台的势能，P_杆为若干连杆的势能，K为是定平台动能和若干连杆的动能，v_p和ω_p分别是并联机器人系统的动平台的质心速度矢量和质心角速度矢量；m是并联机器人动平台的质量；m_i是杆的质量；v是杆的线速度矢量，I是并联机器人系统的位姿变量参数。

所述拉格朗日量L的计算公式如下：

L＝K-P_总

其中，K表示定平台动能和若干连杆的动能，P_总为并联机器人系统的势能。

并联机器人系统动态坐标系的初始方位角O₁-X₁Y₁Z₁与静态坐标系O-XYZ一致。首先O₁-X₁Y₁Z₁是X到γ的转向角度，接着Y是β到的转向角度，最后Z是α到的转向角度。三次旋转是相对于静止坐标系的，即并联机器人的姿态不变。根据从右到左的原则，可以得到相应的旋转矩阵。旋转矩阵R的计算公式为：

其中，R是旋转矩阵，τ_i表示作用在并联机器人系统第i旋转接头作用于连杆上的主动力或驱动力，(x_d，z_d，y_d)是原点为O₁的坐标系O₁-X₁Y₁Z₁中的坐标，γ为O₁-X₁Y₁Z₁绕X旋转角度，β为O₁-X₁Y₁Z₁绕Y旋转角度，α为O₁-X₁Y₁Z₁绕Z旋转角度，cα＝cosα，cβ＝cosβ，cγ＝cosγ，sα＝sinα，sβ＝sinβ，sγ＝sinγ。

S2、将步骤S1计算的拉格朗日量引入拉格朗日方程，建立并联机器人系统的动力学模型；

拉格朗日方程的计算公式如下：

其中，t是时间，τ_i表示作用在并联机器人系统第i旋转接头作用于连杆上的主动力或驱动力；

并联机器人系统的动力学模型的表达式如下：

τ＝I+K+L

其中，τ是动力学模型。

并联机器人系统的位姿变量参数I计算公式如下：

其中，l_i是并联机器人系统的动平台的位置，J是。

假设静态坐标系O-XYZ中每个旋转接头的坐标表示为(x_pi,y_pi,z_pi),i∈6,然后从同一坐标系中求出上下平台相应旋转接头之间的距离,他们的距离是P_iB_i。

将并联机器人系统中各旋转接头的坐标代入上式可得

l_i＝l_i(x_p,y_p,z_p,α,β,γ),i∈6

S3、以并联机器人系统中的一组功率放大器、电液伺服阀、液压缸及其负载为广义控制对象，建立并联机器人系统的液压伺服系统模型；

每组液压驱动伺服系统表示并联机器人系统的液压伺服系统模型，因此只需要一个子系统就可以推导出另外五个子系统。从理论上讲，六杆的速度还应包括空间旋转的角速度，但实际中杆的角速度相对较小，不适用于并联机器人的工作空间。一般来说，它可以近似为零。此外，当取适当的参考值时，势能P_总可以近似为零。通过以上公式分析可以看出，并联机器人的实际驱动过程是液压缸的油压，液压缸驱动活塞杆的位移，导致平台的姿态和位置发生变化。

S4、采用投影法进行精密并联机器人系统进行轨迹规划，获取期望轨迹，并将期望轨迹输入到并联机器人系统的液压伺服系统模型；

S5、采用滑膜控制中的指数趋近率控制算法作为动力学模型和液压伺服系统模型的轨迹跟踪控制算法，对并联机器人进行轨迹跟踪控制。

所述指数趋近率控制算法的公式如下：

其中，ε是运动点接近开关面的速率；

用于跟踪并联机器人的轨迹的公式如下：

其中，

和

是并联机器人系统中外界干扰的上界和下界，ΔH是建模误差；M是系统可运动部分的等效惯性质量；u是输入电压,C_i表示模型参数，e_i表示系统误差。

实验需要机器人的两个关节来跟踪正弦曲线。由于等速趋近率控制算法不需要太多的在线计算，除控制算法的采样周期为1ms外，其他算法的采样周期为2ms,每个实验的时间是6s。首先将所有的控制算法写入实时控制系统，并在实验前完成轨迹规划脱机，然后加载到精密并联机器人的实时控制系统中。为了防止驱动饱和，输出电压范围设置为(-6V,6V)。各环节的跟踪误差值和控制输入由实时控制系统保存后发送到工业控制计算机，由主控制系统输出到文本文件，再由Matlab软件进行处理。

为了验证本方案的可行性，本方案与不同算法进行了比较测试，采用不同的算法进行了机器人的轨迹跟踪和控制实验。从实验结果来看，比较了各种算法在轨迹跟踪中的优缺点。这些算法包括:并联机器人动力学模型的牛顿一欧拉方法(算法1)、三自由度空间柔性并联机器人动力学研究中的算法(算法2)、基于凯恩方法的机器人动力学建模与仿真中的算法(算法3)和本方案提出的指数逼近速率控制算法。用四种不同的算法对精密并联机器人的轨迹跟踪和控制进行了测试，测试结果如表1所示。

表1不同算法的实验数据比较

表1的实验结果表明，指数逼近速率控制算法与其他算法相比，能够取得较好的控制效果，即精密并联机器人系统关节1的跟踪误差小于0.01rad，关节2的跟踪误差小于0.005rad。响应时间小于0.2s。

验证实验，使用指数逼近速率控制算法，跟踪机器人的终端轨迹作为直径为200毫米的圆，如图2所示。首先，它必须跟踪精密并联机器人的关节。采用投影法进行精密并联机器人系统关节的轨迹规划。将期望轨迹输入到精密并联机器人系统的关节中。如图5、图6所示，分别给出了关节1和关节2的轨迹跟踪误差。

从图3、图4的并联机器人系统两个关节轨迹的跟踪结果可以看出，机器人关节1和关节2的实际轨迹跟踪结果与期望轨迹基本一致，误差控制在一定范围内。这是由于机器人的液压驱动变结构控制系统以及适应性强的外部干扰。主要原因有两个：一是机器人液压驱动控制系统本身具有滤波效果，因此输入扰动对输出位置没有影响，二是滑模变结构控制法对扰动的适应性，因此扰动对轨迹位置的影响很小。

从实验的角度探讨了工业机器人控制系统的实现。从机械结构、电气控制和控制系统等方面，完成了实验平台的建立，并用拉格朗日函数建立了实验机器人的动力学方程。然后对机器人的轨迹跟踪和控制进行了研究，并对实验数据进行了处理。试验结果表明，指数逼近速率控制算法的跟踪误差和响应速度明显优于其他算法。从实际应用的角度来看，该算法是可行的，具有一定的优势。

以上所述的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此过多描述，所属领域普通技术人员知晓申请日或者优先权日之前发明所属技术领域所有的普通技术知识，能够获知该领域中所有的现有技术，并且具有应用该日期之前常规实验手段的能力，所属领域普通技术人员可以在本申请给出的启示下，结合自身能力完善并实施本方案，一些典型的公知结构或者公知方法不应当成为所属领域普通技术人员实施本申请的障碍。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本申请要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。

Claims (4)

1.一种基于指数趋近率的并联机器人轨迹跟踪控制方法，其特征在于：包括以下步骤：

S1、建立并联机器人系统的广义坐标系作为拉格朗日函数的参数，基于拉格朗日函数的参数计算并联机器人系统的动能和势能并得到拉格朗日量；

所述并联机器人系统包括连杆、动平台和定平台，以及用于设置在连杆端部且分别与动平台和定平台连接的旋转接头；并联机器人系统的广义坐标系中广义坐标表示为：

q_i表示各旋转接头的位移或旋转角度，其速度或角速度用

表示，T表示齐次变换矩阵，q^T表示势能广义坐标，

表示动能广义坐标；

所述并联机器人系统通过广义坐标系计算其动能和势能的计算公式如下：

P_总＝P_定+P_动+P_杆

其中，P_总为并联机器人系统的势能，P_定为定平台的势能，P_动为动平台的势能，P_杆为若干连杆的势能，K为是定平台动能和若干连杆的动能，v_p和ω_p分别是并联机器人系统的动平台的质心速度矢量和质心角速度矢量；m是并联机器人动平台的质量；m_i是杆的质量；v是杆的线速度矢量，I是并联机器人系统的位姿变量参数；

S2、将步骤S1计算的拉格朗日量引入拉格朗日方程，建立并联机器人系统的动力学模型；

所述拉格朗日方程的计算公式如下：

所述拉格朗日量L的计算公式如下：

L＝K-P_总

其中，t是时间，τ_i表示作用在并联机器人系统第i旋转接头作用于连杆上的主动力或驱动力；K表示定平台动能和若干连杆的动能，P_总为并联机器人系统的势能；

S3、以并联机器人系统中的一组功率放大器、电液伺服阀、液压缸及其负载为广义控制对象，建立并联机器人系统的液压伺服系统模型；

S4、采用投影法进行精密并联机器人系统进行轨迹规划，获取期望轨迹，并将期望轨迹输入到并联机器人系统的液压伺服系统模型；

S5、采用滑膜控制中的指数趋近率控制算法作为动力学模型和液压伺服系统模型的轨迹跟踪控制算法，对并联机器人进行轨迹跟踪控制；

所述步骤S5中，指数趋近率控制算法的公式如下：

其中，ε是运动点接近开关面的速率，K表示常数；

用于跟踪并联机器人的轨迹的公式如下：

其中，

和

是并联机器人系统中外界干扰的上界和下界，ΔH是建模误差；M是并联机器人系统定平台和若干连杆的等效惯性质量；u是输入电压,C_i表示模型参数，e_i表示系统误差。

2.根据权利要求1所述的一种基于指数趋近率的并联机器人轨迹跟踪控制方法，其特征在于：并联机器人系统的位姿变量参数I计算公式如下：

其中，l_i是并联机器人系统的动平台的位置，J表示精密并联机器人系统的雅可比矩阵。

3.根据权利要求2所述的一种基于指数趋近率的并联机器人轨迹跟踪控制方法，其特征在于：旋转矩阵R的计算公式为：

其中，R是旋转矩阵，τ_i表示作用在并联机器人系统第i旋转接头作用于连杆上的主动力或驱动力，(x_d，z_d，y_d)是原点为O₁的坐标系O₁-X₁Y₁Z₁中的坐标，γ为O₁-X₁Y₁Z₁绕X旋转角度，β为O₁-X₁Y₁Z₁绕Y旋转角度，α为O₁-X₁Y₁Z₁绕Z旋转角度，cα＝cosα，cβ＝cosβ，cγ＝cosγ，sα＝sinα，sβ＝sinβ，sγ＝sinγ。

4.根据权利要求3所述的一种基于指数趋近率的并联机器人轨迹跟踪控制方法，其特征在于：所述并联机器人系统的动力学模型的表达式如下：

τ＝I+K+L

其中，τ是动力学模型。

Images