赵海滨， 于清文， 刘 冲， 陆志国， 颜世玉

(东北大学 机械工程与自动化学院， 沈阳 110819)

0 引 言

机器人是一门跨专业，高度综合的新兴学科[1]，无论在基础理论方面还是在实践应用方面发展速度都非常快。越来越多的高等学校面向高年级本科生和低年级研究生开设机器人学等机器人领域的相关课程，并对课程的改革进行了研究[2-4]。机器人课程的教学包括理论教学和实验教学[5-7]。由于资金和设备等原因，各个高校往往以理论教学为主，对实验教学不够重视。在机器人课程的理论教学中存在大量的公式推导和微分方程等，对于学生比较抽象、复杂和难以理解。因此，采用Matlab/Simulink软件进行机器人的仿真和控制实验非常必要。

在机器人的教材中，机器人动力学部分的公式推导均以两自由度机械臂为例，本文也以两自由度机械臂为研究对象。根据机械臂的动力学方程，采用Matlab/Simulink软件建立机械臂仿真模型，采用五阶多项式进行轨迹规划，并采用逆动力学进行机械臂的控制。通过机械臂的运动学方程获取关节的位置坐标，并动态的显示机械臂的运动过程。最后，对机械臂控制系统进行动态仿真，并对仿真结果进行了分析。学生可以修改参数，然后进行动态仿真。通过仿真实验，向学生完整的展示机械臂的控制过程，能够加深学生对机器人动力学、运动学和控制等理论的理解，增强学生的编程能力和学习兴趣，有助于机器人课程的理论和实验教学。

1 机械臂

本文以两自由度机械臂为研究对象，通过拉格朗日方程建立动力学方程[1]为

(1)

(2)

在动力学方程中，机械臂的参数[8-9]如下：

c12=2c11

c21=0

g1=(m1+m2)gr1cos(q2)+m2gr2cos(q1+q2)

g2=m2gr2cos(q1+q2)

式中：m1和m2分别为两个连杆的质量；r1和r2分别为两个连杆的长度；J1和J2分别为两个连杆的转动惯量；g为重力加速度。

2 控制系统

本文以两自由度机械臂为研究对象，采用Matlab/Simulink软件建立综合性仿真实验平台。根据动力学方程建立机械臂的模型，然后采用轨迹规划和逆动力学控制进行机械臂的控制，最后通过运动学方程将机械臂在关节空间的坐标转换为工作空间下的位置坐标，并将机械臂的运动通过动画的形式进行显示。机械臂控制仿真系统如图1所示。

机械臂的控制方法有PD控制、带重力补偿的PD控制、逆动力学控制、鲁棒控制和自适应控制等。本文采用常用的逆动力学控制方法。机械臂的轨迹规划采用点到点运动方法，常用的有3阶多项式轨迹规划、5阶多项式轨迹规划、带有抛物线的直线段方法和开关(bang-bang)方法等。本文采用5阶多项式轨迹规划方法。

图1 机械臂控制系统原理图

2.1 轨迹规划

轨迹规划采用点到点运动， 即规划一条从初始位置q(t0)到最终位置q(tf)的轨迹。本文采用5阶多项式轨迹规划方法[10]。在t0时刻的角度、角速度和角加速度分别满足：

(3)

在tf时刻的角度、角速度和角加速度分别满足：

(4)

采用如下的5阶多项式轨迹，

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(5)

根据起始位置和终点位置的角度、角速度和角加速度，可以得到下面的方程组：

(6)

通过求解该方程组可以得到参数ai(i=0,1,…,5)。然后，可以得到期望角度、角速度和角加速度分别为：

(7)

2.2 逆动力学控制

根据机械臂的动力学方程(1)，选择控制输入为

(8)

通过比较式(1)和式(8)可以得到

(9)

稳定的控制律y可以选择为

(10)

期望轨迹为qd(t)，为了跟踪该轨迹，选择r为

(11)

将式(11)代入到式(10)，然后再代入到式(9)，可以得到二阶微分方程

(12)

2.3 运动学方程

正运动学是根据机械臂各个关节的角度值，来确定工作空间下的位置坐标。对于具有两个自由度的机械臂，q1和q2分别为两个关节的角度，r1和r2分别为两个关节的长度，则第1个关节末端的位置坐标x1,y1为

(13)

第2个关节末端的位置坐标x2,y2为

(14)

3 仿真实验

Matlab功能强大、使用简单方便，并且对问题的描述和求解符合人们的思维习惯和数学表达习惯，已经广泛应用于动态系统仿真[11]。Simulink是Matlab软件最重要的组件之一，能够进行系统建模、仿真和综合分析等[12]。Simulink和外界硬件的接口还可以建立半实物仿真和实时控制实验[13]。在Simulink系统中用户自定义函数库中的Matlab Function模块，可以采用Matlab语言非常方便灵活的建立复杂系统[14]。Matlab/Simulink软件非常适合进行机器人的控制和仿真实验[15-16]，因此本文采用Matlab/Simulink软件建立仿真实验系统。

两自由度机械臂的参数选择为：m1=0.5 kg，m2=1.5 kg，r1=1 m，r2=0.8 m，J1=5 kg·m，J2=5 kg·m，g=9.81 m/s2。在逆动力学控制中参数Kp=diag(9,9)，Kd=diag(6,6)。

采用采用Matlab/Simulink软件建立机械臂控制系统仿真实验平台，如图2所示。系统采用变步长的ode45算法，最大仿真步长为1 ms，仿真时间为4 s。在图2中主要采用了Simulink中用户自定义功能模块库中的Matlab Function模块和积分模块等。通过常量模块Constant设置机械臂的初始角度和角速度。通过To Workspace模块将运行结果保存在工作空间中。 机械臂两个关节的期望角度分别采用5阶多项式进行轨迹规划。关节1和关节2的初始角度q1和q2分别为π/10和π/8，目标位置的角度分别为3π/4和π/2。关节1和关节2的初始角速度和目标位置的角速度均为0，初始角加速度和目标位置的角加速度也均为0。

通过轨迹规划得到的期望角度、角速度和角加速度，以及机械臂的角度和角速度，采用式(8)进行逆动力学控制。在图2中，逆动力学控制模块内的代码为

function tau = fcn(dq,q,qd,dqd,ddqd)

q1=q(1);q2=q(2);dq1=dq(1);dq2=dq(2);

m1=0.5;m2=1.5;r1=1;r2=0.8;g=9.81;J1=5;

J2=5;

d11=(m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*

cos(q2)+J1;

d12=m2*r2^2 +m2*r1*r2*cos(q2);

d21=d12;d22=m2*r2^2 +J2;

D=[d11,d12;d21,d22];

c11=m2*r1*r2*sin(q2);

图2 机械臂控制系统仿真实验平台

C=[-c11*dq1,-2*c11*dq1; 0,c11*dq2];

g1=(m1+m2)*g*r1*cos(q2)+m2*g*r2*cos(q1+q2);

g2=m2*g*r2*cos(q1+q2);

G=[g1;g2];

w=3;e=q-qd;de=dq-dqd;

Kp=diag([w^2,w^2]);Kd=diag([2*w, 2*w]);

y=ddqd-Kd*de-Kp*e;

tau=D*y+C*dq+G;

机械臂控制系统运行后，机械臂的角度如图3所示。关节1的角度q1从初始位置π/10运动到目标位置3π/4，关节2的角度q2从初始位置π/8运动到目标位置π/2。

图3 机械臂的角度

机械臂的角速度如图4所示。在图4中，关节1的角速度初始值为0，然后速度逐渐增大，在第2 s达到最大值为0.957 rad/s，最后逐渐减小到0。关节2的角速度初始值为0，然后逐渐增大，并在第2 s达到最大值0.552 rad/s，最后逐渐减小到0。

图4 机械臂的角速度

机械臂的驱动力矩如图5所示。在图5中，关节1的驱动力矩在0.605 s达到最大值33.83 Nm，在0.306 s达到最小值-15.36 Nm。关节2的驱动力矩在0.563 s达到最大值12.04 Nm，在0.275 s达到最小值-14.20 Nm。

通过机械臂的运动学方程，可以得到两个关节的位置坐标。在关节空间中，通过逆动力学控制将机械臂从初始位置移动到目标位置。通过运动学方程，可以得到机械臂第一个关节初始位置的坐标为cos(π/10),sin(π/10)=0.951,0.309。机械臂第2个关节初始位置的横坐标为cos(π/10)+0.8cos(π/10+π/8)=1.56

图5 机械臂的驱动力矩

纵坐标为

sin(π/10)+0.8sin(π/10+π/8)=0.829

在工作空间中机械臂的运动轨迹如图6所示。在图6中，每间隔0.2 s绘制机械臂的位置，动态的显示机械臂的运动过程。

图6 机械臂的运动轨迹

4 结 语

通过Matlab/Simulink软件建立了两自由度机械臂控制系统仿真实验平台，能够进行机械臂的点到点控制，并动态的显示机械臂的运动过程。通过仿真实验，学生可以对机械臂的整个控制系统有一个完整的了解，还可以自己编写控制算法。通过机械臂控制系统的仿真实验，有助于学生对机器人的基本理论的理解，能够增强学生的实际编程能力，激发学生的学习兴趣，有助于机器人课程的理论和实验教学。