王佳　胡旭晓　吴跃成

摘  要：为了在现有基础上提高机械臂轨迹跟踪控制的精度，采用自适应模糊反演控制算法对六自由度机械臂UR5进行轨迹跟踪控制算法研究。用拉格朗日法对UR5机械臂进行动力学建模以避免系统的不确定因素影响。由于UR5机械臂满足Pieper准则，故对其进行模型简化，减少运算量，并用拉格朗日法对前两个自由度进行动力学方程推导，该算法在角位移和角速度跟踪方面都表现出了很好的性能。

关键词：UR5;模型简化;拉格朗日法;轨迹跟踪;自适应模糊反演控制

中图分类号：TP241.2     文献标识码：A

Research on Trajectory Tracking Control Algorithm based on UR5 Manipulator

WANG Jia， HU Xuxiao， WU Yuecheng

Abstract： In order to improve the accuracy of manipulator trajectory tracking control on the existing basis， this paper proposes to study the trajectory tracking control algorithm of 6-DOF （degree of freedom） manipulator UR5 by using the adaptive fuzzy inversion control algorithm. The dynamic modeling of UR5 manipulator is established by Lagrange method to avoid the influence of uncertain factors of the system. Since the UR5 manipulator meets the Pieper criterion， its model is simplified to reduce the amount of computation， and the dynamic equations of the first two degrees of freedom are deduced by Lagrange method. The algorithm shows good performance in angular displacement and angular velocity tracking.

Keywords： UR5; model simplification; Lagrange method; tracking; adaptive fuzzy inversion control

1   引言（Introduction）

随着工业技术的发展，机械臂逐渐成为工业领域自动化水平最高的科技产品之一[1]。在处于确定环境下的生产中，它可以代替人进行繁重的劳动，以实现生产的机械化和自动化并提高产品的质量和生产效率，在有害环境下作业还能够保障人身安全。但当环境发生变化或变得复杂而无法精确测量机械臂的各个参数，以及难以建立精确的动力学模型时，这些不确定因素均会限制机械臂的运动控制，尤其是在机械臂自主进行接触式操作如切割、电焊等方面将会面临非常大的挑战[2]。PID或PD控制[3]是目前工业机器人应用最为广泛的一种控制算法，虽易于操作但难以保证机械臂的动态性能，当启动时的输出力矩过大时容易造成机械臂损坏。本文选用不需要精确模型的控制策略进行跟踪控制研究，避免了因模型建立不够精确而降低控制精度的问题。

2  UR5机械臂动力学模型的建立（Dynamic model establishment of UR5 manipulator）

2.1   UR5机械臂简介

UR5机械臂[4]是由Universal Robots公司开发的灵活、轻型的六关节工业机器人，由六个关节和挤压铝管组成，所有关节均为转动关节。关节1—6分别命名为：机座、肩部、肘部、手腕1、手腕2和手腕3。其中机座可以用来确定机器臂的安裝位置。UR5机械臂质量较小（仅18.4 kg），可以方便地在生产场地移动，而且不需要烦琐的安装与设置就可以迅速地融入生产线中，与员工交互合作，并可处理最大5 kg的有效负载。UR5机械臂编程可通过教学编程模式实现，用户扶住机械臂，手动引导机械臂按所需的路径及移动模式运行一次，它就能自动记住移动路径和模式。UR5机械臂可通过一套独特的、友好的图形用户界面进行操作，在触摸屏幕上有多种功能供用户选择。任何重复性的生产过程都能够使用它并从中受益，这使得UR5机械臂的应用十分广泛。

UR5机械臂前三个关节主要控制末端的位置，后三个关节主要控制末端的姿态[5]。通过各个关节之间的协调运动，它可以在其工作范围内任意移动。UR5机械臂实物图如图1所示。

2.2   模型简化

由于通用工业机械臂满足Pieper准则[6]，即后三个关节轴相交于一点，因此，前三个关节主要完成机械臂末端位置运动，且动力学参数值（转动惯量、质量等）较大，非线性效应比较明显;后三个关节主要完成机械臂末端位姿调整，动力学参数值较小。在误差允许范围内，考虑将六自由度机械臂模型简化为前三个自由度机械臂模型，并将后三个关节看作动力效应的外部干扰，对其进行动力学分析。实验表明，当只考虑机械臂前三个关节时其动力学特性与考虑全部六个关节相比不失一般性，且误差较小。这不仅减少了计算量，对于参数辨识来讲，减少了待辨识的参数，也使获得相对精确较高的动力学模型变得更加容易。简化模型如图2所示。

2.3   拉格朗日法建立动力学方程

拉格朗日力学[7]与牛顿力学完全等效，其优势在于只需要写出拉格朗日量，就可由简单求导得到完整的动力学微分方程而无须进行受力分析;方程的形式不随广义坐标的选取而改变;无须解方程即可得到一些守恒量。

利用牛顿力学分析动力学时，在分析的过程中可以知道所有连杆生成运动的方向以及大小，但分析太过复杂。UR5机械臂动力学模型是一个非常复杂的非线性模型，若使用牛顿力学，出错的机会就会大大提高。而拉格朗日力學是基于能量项对系统变量及时间的微分方法，是以能量的角度来看待系统的。

欧拉-拉格朗日运动方程为：

（1）

其中，L是拉格朗日函数，K是系统动能，P是系统势能。

系统的拉格朗日方程为：

（2）

其中，为机械臂关节i的力矩，和为关节位置和速度。

最终写成如下公式：

（3）

其中，M为机械臂的惯性矩阵，是与角加速度相关的项;C为科氏力与离心力项矩阵，是与角速度有关的项;G为重力项，不受角速度和角加速度的影响，仅与机械臂所处位置有关;为外部输入力矩，q为机械臂的角位移。

（4）

（5）

其中，A为离心力系数，B为科氏力系数。

（6）

（7）

（8）

由于机械臂的动力学模型还受到外界干扰的影响，令外界干扰为d，动力学模型可建立为：

（9）

假设系统参数是未知但有界的，且系统具有以下特性：

（1）惯性矩阵是正定对称矩阵，且有界，即存在，;

（2）惯性矩阵、向心力和科氏力矩存在以下关系：

（10）

3  自适应模糊反演控制算法（Adaptive-fuzzy-backstepping control algorithm）

3.1   反演控制算法

反演控制算法是一种非线性系统设计的方法[8]，它是将下一级的变量当作虚拟输入作用在该一级系统上，同时根据上一级系统建立Lyapunov函数，实现系统稳定，直到最后一个系统实现控制，得到输入量。反演控制算法保证在任何情况下存在Lyapunov函数，使得该系统稳定，同时利用该函数找到一个参考输入，最终实现对输入的推导和设计。

机械臂动态方程可写为：

（11）

令，则动态方程可改写为：

（12）

定义误差：

（13）

（14）

式中，为虚拟控制量，通过选取，使得趋近0;为控制轨迹，为期望轨迹，为控制角速度。

取：

（15）

对于第一个子系统，Lyapunov函数为：

（16）

（17）

若为0，则第一个子系统稳定。

（18）

控制律设计为：

（19）

对于第二个子系统，Lyapunov函数为：

（20）

（21）

当时，第二个子系统稳定。

反演控制[9]算法具有如下优势：

（1）其设计的控制器以及Lyapunov函数的过程都非常清晰明了，具有系统化、结构化的特点。

（2）对于高阶的非线性系统也可以取得非常好的控制效果，消除了经典无源性设计中相对阶是1的限制。

（3）不确定性要满足匹配条件的要求，控制不确定非线性系统时有着明显的优越性。

3.2   自适应模糊控制

模糊控制就是将模糊数学的基本思想和理论应用到控制的领域。模糊控制[10]的一般流程包括：

（1）确定模糊控制器的结构：确定控制器的输入、输出。

（2）定义输入、输出的模糊集。

（3）定义输入、输出的隶属度函数。

（4）建立模糊控制规则。

（5）建立模糊控制集。

（6）进行模糊推理。

传统的基于模型的控制方法非常依赖于精确的动态模型。但是对于UR5六自由度非线性的复杂系统来说，是很难实现精确控制的。而模糊控制不依赖于被控对象精确的数学模型，适合动态特性不易获取的控制对象。同时，模糊控制是以人类的经验积累为出发点的，且这种控制的鲁棒性好，可以用于逼近参数不确定的各种线性和非线性系统的模型。

式（21）中，当时，，此时：

（22）

将式（19）代入式（22）中得：

（23）

式（23）中，包含机器人的建模信息。由于机械臂的精确动力学模型是很难建立的，因此我们采用自适应模糊控制，这种控制方式不需要精确的模型信息。假设是用来逼近的模糊系统。这里我们采用单值模糊化、乘积推理机和重心平均反模糊化。

假设模糊系统由N 条模糊规则构成，第 条模糊规则表达形式为：

： IF  is  and ... is ，then  is 。

（=1，2，…，N）

其中，为（j=1，2，…，n）的隶属函数。

则模糊系统的输出为：

（24）

其中，，，。

对于的模糊逼近，分别采用逼近（1）、（2）的形式，相应的模糊系统设计为：

（25）

（26）

（27）

其中，，。

定义最优逼近常量，对于给定的任意小的常量（>0），如下不等式成立：

设计自适应控制律为：

（28）

4   实验结果及分析（Experiment analysis）

对UR5机械臂进行模型简化后，并对前两个自由度进行轨迹跟踪控制，采用的控制算法为自适应模糊控制算法。

UR5机械臂动力学参数各个连杆质心位置及质量如表1所示。

表1 UR5机械臂动力学参数

Tab.1 Dynamic parameters of UR5 manipulator

连杆编号 xi/m yi/m zi/m m/kg Li/m

1 0 -1.93e-3 -26.51e-3 3.7 0.892

2 212.5e-3 0 113.36e-3 8.393 0.425

本文以UR5机械臂为研究对象，对UR5机械臂进行动力学轨迹规划控制。设期望轨迹分别为、

。

系统的初始状态为。外界扰动。

建立二自由度机械臂自适应模糊反演控制系统Simulink仿真模型，如图3所示，其中control为控制器子程序，D1plant为运算实际轨迹子程序。

关节1、关节2控制轨迹与期望轨迹的跟踪情况如图4和图5所示。

关节1、关节2控制角速度与期望角速度的跟踪情况如图6和图7所示。

通过对机械臂的前两个关节进行仿真实验，由实验可知，两个关节的角位移跟踪精度高，说明机械臂能按照预定轨迹进行运动;两个关节的角速度跟踪效果好，说明机械臂能按照一定的速度进行运动，不会发生突变产生冲击。

5 结论（Conclusion）

本文通过对UR5机械臂进行模型简化并分析前两个自由度机械臂动力学模型，设计自适应模糊反演控制器并对其进行误差分析，利用MATLAB的Simulink仿真模块实现机械臂各关节末端的轨迹跟踪。仿真结果表明，位置环与速度环跟踪的效果都很好。

参考文献（References）

[1] 孫开林.六自由度工业机器人运动与控制技术的研究[D].无锡：江南大学，2012.

[2] 张果，赵艳花，陈文清.二自由度机械臂的鲁棒轨迹跟踪控制及仿真[J].洛阳理工学院学报（自然科学版），2017，27（03）：62-64，83.

[3] 李岩，丁浩，孙中波，等.基于一种加速PD型迭代学习控制算法的机械臂轨迹跟踪[J].科学技术与工程，2019，19（26）：265-269.

[4] 蔡肖肖.UR5型机器人的运动学分析与标定实验研究[D].杭州：浙江理工大学，2017.

[5] 王海，付邦晨，薛彬，等.六自由度柔性关节机械臂的动力学分析[J].中国机械工程，2016，27（08）：1096-1101.

[6] 晁智强，王飞，李华莹，等.六自由度工业机械臂动力学模型简化分析[J].火力与指挥控制，2018，43（02）：112-115.

[7] 宋花.六自由度机械臂轨迹跟踪控制[D].包头：内蒙古科技大学，2020.

[8] 马宇豪.六自由度机械臂避障轨迹规划及控制算法研究[D].北京：中国科学院大学（中国科学院西安光学精密机械研究所），2019.

[9] PIKASO P， MUKHERJEE V， HINSERMU A， et al. Generalized adaptive backstepping sliding mode control for synchronizing chaotic systems with uncertainties and disturbance[J]. Mathematics and Computers in Simulation， 2021， 190（01）：793-807.

[10] CHEN Z， WANG D， DAO G H， et al. Environmental impact of the effluents discharging from full-scale wastewater treatment plants evaluated by a hybrid fuzzy approach[J]. Science of the Total Environment， 2021， 50（08）：790-798.

作者简介：

王  佳（1998-），女，硕士生.研究领域：六自由度机械臂轨迹跟踪控制.

胡旭晓（1965-），男，博士，教授.研究领域：图像处理，机器人轨迹跟踪控制.

吴跃成（1966-），男，博士，副教授.研究领域：人机交互.