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

--可编辑修改---可编辑修改-实验二自由度机器人的位置控制一、实验目的.运用Matlab语言、Simulink及Robot工具箱，搭建二自由度机器人的几何模型、动力学模型，构建控制器的模型，通过调整控制器参数，对二自由度机器人的位姿进行控制，并达到较好控制效果。二、工具软件.Matlab软件.Simulink动态仿真环境robot工具箱模型可以和实际中一样，有自己的质量、质心、长度以及转动惯量等，但需要注意的是它所描述的模型是理想的模型，即质量均匀。这个工具箱还支持Simulink的功能，因此，可以根据需要建立流程图，这样就可以使仿真比较明了。把robot工具箱拷贝到MATLAB/toolbox文件夹后，打开matalb软件，点击file--setpath，在打开的对话框中选addwithsubfolders,选中添加MATLAB/toolbox/robot,保存。这是在matlab命令窗口键入roblocks就会弹出robot工具箱中的模块（如下图）。--可编辑修改--可编辑修改--可编辑修改-RoboticsToolboxforMatlab(release7}DyiMillies Gi Tbijedoiy KineihAtics三、实验原理在本次仿真实验中，主要任务是实现对二自由度机器人的控制，那么首先就要创建二自由度机器人对象，yofXoXo二自由度机器人4标配置仿真参数如下表1：表1二连杆参数配置

意义符号值单位杆1长10.45m杆2长120.55m杆1重心lc10.091m杆2重心1c20.105m杆1重量m123.90kg杆2重量m24.44kg杆1惯量I11.27kg.m2杆2惯量20.24kg.m2重力加速度G9.8m/sec21.运动学模型 构建二连杆的运动学模型，搭建twolink模型在MATLAB命令窗口下用函数drivebot(WJB)即可观察到该二连杆的动态位姿图。%文件名命名为自己名字的首字母 _twolink%构造连杆一L{1}=link([00.45 00 0],'standard');L{1}.m=23.9;L{1}.r=[0.091 00];L{1}」二[00 0000];L{1}.Jm=0;L{1}.G=1;%构造连杆二L{2}=link([0 0.55000]；standard');L{2}.m=4.44;L{2}.r=[0.10500];L{2}.l=[00 0000];L{2}.Jm=0;L{2}.G=1;%(机器人的名字请用自己名字的首字母如)WJB=robot(L);WJB.name='WJB_twolink'; %设定一连杆名字qz=[00];qr=[0pi/2];2.二连杆动力学部分实现机器人内部动力学构建，根据拉格朗日法建立机器人动力学模型(见书上P55)即下式：q.二1..M(.—G(q)—C(q,q)*q)仍然用matlab下M函数来实现：%文件名命名为自己名字的首字母_dl%二连杆动力学部分functionqdd=WJB_dl(u)%自己名字的首字母q=u(1:2);qd=u(3:4);tau=u(5:6);g=9.8;m仁23.9;m2=4.44;11=0.45;12=0.55;lc1=0.091;lc2=0.105;11=1.27;12=0.24;M11=m1*lc1a2+m2*(l1a2+lc2a2+2*l1*lc2*cos(q(2)))+I1+I2;M12=m2*(lc2a2+l1*lc2*cos(q(2)))+l2;M21=m2*(lc2A2+l1*lc2*cos(q(2)))+l2;M22=m2*lc2A2+I2;M=[M11M12;M21M22];C11=-(m2*l1*lc2*sin(q(2)))*qd(2);C12=-m2*l1*lc2*sin(q(2))*(qd(1)+qd(2));--可编辑修改---可编辑修改-C21=m2*l1*lc2*sin(q(2))*qd(1);C22=0;C=[C11C12;C21C22];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)MATLABFunction二自由度机器人的结构模型MATLABFunction二自由度机器人的结构模型4H―MATLABFen最后，还需将机器人动力学和几何学联系在一起。通过机器人学工具箱中的robot模块实现。3•控制器设计(任选一二)(1)简单PD控制率，结构图如下，此种方法没有加任何补偿，存在较大稳态误差，但是控制算法非常简单。

5ct-ptConGanlGj.nTe'rr.iO-aivil5ct-ptConGanlGj.nTe'rr.i（2）PD加重力补偿带有重力补偿的P