二自由度机器人的位置控制

1、实验 二自由度机器人的位置控制一、实验目的1. 运用Matlab语言、Simulink及Robot工具箱，搭建二自由度机器人的几何模型、动力学模型，2. 构建控制器的模型，通过调整控制器参数，对二自由度机器人的位姿进行控制，并达到较好控制效果。二、工具软件1.Matlab软件2.Simulink动态仿真环境3.robot工具箱模型可以和实际中一样，有自己的质量、质心、长度以及转动惯量等，但需要注意的是它所描述的模型是理想的模型，即质量均匀。这个工具箱还支持Simulink的功能，因此，可以根据需要建立流程图，这样就可以使仿真比较明了。把robot 工具箱拷贝到MATLAB/toolbox文件夹

2、后，打开matalb软件，点击file-set path，在打开的对话框中选add with subfolders，选中添加MATLAB/toolbox/robot，保存。这是在matlab命令窗口键入roblocks就会弹出robot 工具箱中的模块（如下图）。三、实验原理在本次仿真实验中，主要任务是实现对二自由度机器人的控制，那么首先就要创建二自由度机器人对象，二自由度机器人坐标配置仿真参数如下表1： 表1 二连杆参数配置意义符号值单位杆1长l10.45m杆2长l20.55m杆1重心lc10.091m杆2重心lc20.105m杆1重量m123.90kg杆2重量m24.44kg杆1惯量I11

3、.27kg. m2杆2惯量I20.24kg. m2重力加速度G9.8m /sec21.运动学模型 构建二连杆的运动学模型，搭建twolink模型在MATLAB命令窗口下用函数drivebot(WJB)即可观察到该二连杆的动态位姿图。%文件名命名为自己名字的首字母_twolink%构造连杆一L1=link(0 0.45 0 0 0,'standard') ;L1.m=23.9 ;L1.r=0.091 0 0 ;L1.I=0 0 0 0 0 0 ;L1.Jm=0 ;L1.G=1 ;%构造连杆二L2=link(0 0.55 0 0 0,'standard') ;L2.

4、m=4.44 ;L2.r=0.105 0 0 ;L2.I=0 0 0 0 0 0 ;L2.Jm=0 ;L2.G=1 ;%（机器人的名字请用自己名字的首字母如）WJB=robot(L) ;WJB.name='WJB_twolink' ; %设定二连杆名字qz=0 0 ;qr=0 pi/2 ;2.二连杆动力学部分实现机器人内部动力学构建，根据拉格朗日法建立机器人动力学模型（见书上P55）即下式 ： 仍然用matlab下M函数来实现：%文件名命名为自己名字的首字母_dl%二连杆动力学部分function qdd=WJB_dl(u) %自己名字的首字母q=u(1:2); qd=u(3:

5、4); tau=u(5:6);g=9.8;m1=23.9 ; m2=4.44 ;l1=0.45 ; l2=0.55 ;lc1=0.091 ;lc2=0.105 ;I1=1.27 ; I2=0.24 ;M11=m1*lc12+m2*(l12+lc22+2*l1*lc2*cos(q(2)+I1+I2 ;M12=m2*(lc22+l1*lc2*cos(q(2)+I2 ;M21=m2*(lc22+l1*lc2*cos(q(2)+I2 ;M22=m2*lc22+I2 ;M=M11 M12 ;M21 M22 ;C11=-(m2*l1*lc2*sin(q(2)*qd(2) ;C12=-m2*l1*lc2*s

6、in(q(2)*(qd(1)+qd(2) ;C21=m2*l1*lc2*sin(q(2)*qd(1);C22=0 ;C=C11 C12 ;C21 C22 ;G1=(m1*lc1+m2*l1)*g*sin(q(1)+m2*lc2*g*sin(q(1)+q(2) ;G2=m2*lc2*g*sin(q(1)+q(2) ;G=G1 ;G2 ;qdd=inv(M)*(tau-G-C*qd)最后，还需将机器人动力学和几何学联系在一起。通过机器人学工具箱中的robot模块实现。3控制器设计（任选一二）（1）简单PD控制率，结构图如下，此种方法没有加任何补偿，存在较大稳态误差，但是控制算法非常简单。（2）PD

7、加重力补偿 带有重力补偿的PD控制可设计成 t=Kp(q期望值-q)-Kd*qd+G(q)重力项 3. PD加前馈补偿控制加了一个逆动力学模块 t=Kp(q期望值-q)+Kd*(q期望值一阶导-q一阶导)+M（q）*q二阶导+C*q一阶导+G（q）四、实验步骤1运动学模型 在matlab菜单file下新建一个M-file，将机器人运动学模型添加进去（注意更改自己的机器人命名,自己名字的首字母缩写_twolink），并将此M-file命名后保存在work文件夹下，备用。2在matlab命令窗口调用函数drivebot（机器人名字自己名字首字母的缩写，不加twolink），出现机器人的动态位姿图，

8、调节q1、q2可直观的看出二自由度机器人的位姿在改变。3动力学模型 在matlab菜单file下再新建一个M-file，将机器人动力学学模型添加进去，并将此M-file命名后（自己名字首字母_mdl）保存在work文件夹下，备用。4，将机器人运动学模型和动力学模型联系起来在matlab命令窗口输入命令roblocks调出robot工具箱，再输入simulink调出SImulink动态仿真环境。5、在Matlab菜单file下新建一个model，将robot工具箱中的robot模块拖拽到model文件里，双击编辑机器人属性，将robot object改为机器人的名字（自己名字首字母的缩写）（即运

9、动学构建的机器人对象）。再选中robot模块，右键菜单找到look under mask，点开，可以找到机器人内部动力学模型，将其中的S-Function替换成Simulink下面的MatlabFunction，双击此MatlabFunction弹出对话框，将其中的函数改为动力学模型文件名。6.添加控制器 根据控制器设计的方案，在Simulink下找出构成控制系统的其他模块，其中综合点及matrix gain 在math operations里；示波器scope和终止端terminator在输出池sinks里；常量constant在输入模块sources里； 将各个模块拖拽到model文件里，可以通过鼠标拖住连线。7、动态仿真 双击综合点，将其属性改成有一个减号，形成负反馈；常量constant给定你期望位姿（注意是二自由度机器人，需输入2*！的矩阵），初步给定KP、KD参数（2*2的矩阵）。在model文件菜单栏下面，点击一个箭头（start simulation）或者在菜单栏点击Simulation ，在下拉菜单中选择start simulation，即可开始仿真，此时双击打开scope即可得到响应曲