matlab仿真二自由度机械臂动态仿真

机电系统的动力与运动的计算机仿真基于二自由度两连杆平面机器人系统仿真马国锋梁应海周凯（武汉理工大学机电工程学院机械工程及门动化系）摘要：平面两连杆机器人（机械臂）是-种简单的两自山度的机械装冒，其具有一定的复杂动力特 性，对其的简单研究能够对机电系统和机器人冇更好的学习了解和认识。利用matlab仿真的快捷，简 洁，以及可视化操作可以使其研究更方便，以及利用PID调节，使系统具有更好的时间响应性能。关键词：matlab仿真 PID控制调节平面机器人伺服直流电动机Abstract: The Planar two-link robot （Robot Arm） is a simple mechanical device of two degrees of freedom, it has complex dynamic characteristics. We can gain better learning and understanding for Mechanical and Electrical systems and Robots only through studying 让 simply! Using the superior performance of MATLAB zwe can make the research more convenient,besides ,we also can make the system have better performance in Time Response through the PID correction.0、引言随着科学技术的发展，利用计算机对控制系统进行仿真和分析，是研究控制系统的重要方 法。对控制系统进行仿真，首先应该建立系统模型，然后根据系统模型进行仿真,并充分的利用 计算机作为工具进行数值求解。Mat lab是目前应用最为广泛的仿真语言之一。该软件具有以 下特点：数值计算功能强大：编程环简单：数据可视化功能强：丰富的程序工具箱；可扩展 性能强等。Simulink是MATLAB下用于建立系统框图和仿真的环境。Simulink环境仿真的优 点是：框图搭建方便、仿真参数可以随时修改、可实现完全可视化编程。并且可以再仿真过 程中进行系统的相关调节，利用PID校正或相位滞后校正使系统具有更优的性能。本文就从系统仿真和调节以及运动过程的可视化进行研究说明1、二自由度两连杆平面机器人系统模型介绍下图为一个两连杆平面机器人的三维示意图以及简单的平而示意图这个例子在机器人学文献中经常能够遇到，它为平面机器人最为简单的形式，由两根连杆和 两个由电机(伺服直流电机)驱动的两个转动狡所组成，其具有一定的复杂动力特性，本文 就将对这个系统的伺服电机进行仿真与调节，并且导出该机构的动力方程，其动力学仿真就 要求再有两个输入参数下的运动问题。2直流伺服电机系统仿真2.1直流电机的物理模型由于直流电动机具有良好的启动性能 和调速性能，而机械臂由于调速要求高，正反 转和启制动频繁，所以仍选用直流伺服电机來 驱动。右图为直流伺服电机线路示意图。图中, Ra、La分别为电枢绕组的电阻和电感，ia为电 枢电流，RfLf分别为励磁电路的电阻和电感,if为励磁电流，ef为磁场励磁电HG &为加到电枢上的电床，而eb为电枢中的反电动势；9。为电动机的轴的角位移，Tm为电动机产生的转矩，Tn为负载；J、B分别为电动机和负载折 算到电动机轴上的等效转动惯量和粘性阻尼系数。2. 2直流电机的数学模型由控制输入电床6旳开始，系统的因果方程式为:电枢电压方程：R 2匚+叩亠(0 = ex (t) - q (t)电动机转矩方程:mW)转矩平衡方程：+cttdt电动机的反电动势正比于速度：q(t) = %2%(t)dt其中反电动势常数根据本系统，查找相关资料以及计算，设置其中电机的参数如卜：J=0. 016kg m B=0. lN/m s1; K=KbM(t=0. 04N m/A;R=Ra=lQ ; L=La=O. 01H;现利用两种方法来进行模型的建立:(1)拉普拉斯变化和simulink相结合进行建立 对上面四个方程进行变换如下：(Las + Rj Ia(s) = E1(s)-Eb(s TmCXiqiaG)(Js2 + Bs) )o(s)二R(s)A(s)Eb(s) = Kbs0(s)则建立系统方框图如下：利用simulink进行建模当负载转矩为0时，由上式消去中间变量，可以得到电枢控制伺服电机以e(t)为输入量、 久(t)为输出量的传递函数，即：G丽色血=咚 =冷 Ei(s) s(-s+&)(Js+B)+KTKbs(s?+2纽+血)若得到输出为角速度的传递函数时，由角速度为角度的微分，即需在上述传递函数中乘 以S即可。将设置的参数带入方程屮既可以得到系统传递函数方程式:当输出为角度时:250当输出为角度时:250当输出为角速度时:S (sA2 +106.3 s+ 635)250sA2 + 106.3 s + 635(2)、可以利用空间状态方程来进行系统的建模 由系统的数学模型可以得到下面的矩阵关系式:1B7K0KTRLe(i)0 = (0 1 0) 0Imatlab编程,并利用sys=ss(A,B,C,D)指令进行编程,程序如下:当输出为角度时:250J=0.016;B=0. 1;K=O. 04;R=1.L=O. 01;A=0, 1?0;0? -B/J, K/J;0, -K/-R/L; 氐血 0,1/L5 ;mcu；D=0;sys=ss(Aj Bj CjD)SYSl=tf (sys)输出为角速度模型程序得到传递函数如下：Transfer function:250J=0. 016;B=0. l;K=0. 04;R=.;L=0. 01;A二 Q 1, U; 0, -B/L K/J ; 0, -K/L, -R/L;B= D 0 J/L;0=11. 0,0;D=0;sys=ss (Aj C, D)SYS2=tf (sys)输出为角度模型程序Transfer function:250当输出为角度时:250当输出为角度时:250sA2 + 106.3 s + 635s3 + 106. 32 + 035 s3直流伺服电机系统PID控制调节校正3.1 PID简介PID控制是将设定値r(t)与输出反馈值C(t)的偏差e(t)=r(t)-C(t),按比例、积分、微分 运算后，并通过线性组合构成控制量u(t),对控制对象进行控制，如图所示，所以简称为P(比 例)、1(积分)、D(微分)控制器。de(t)ITPID控制器的结构方程u(t) = KpC(t) + KJ e(t)dt + Kd式中，e是偏差量，即输出量与设定值之间的差值：u是控制量，作用于被控制对象并 引起输出量的变化;Kp是比例增益系数，其控制效果是减少响应曲线的上升时间及静态误差， 但不能消除豫态误差：Ki是积分部分的增益系数，控制效果是消除稳态误差：Kd是微分部分 的增益系数，其控制效果是增强系统的稳定性，减少过渡时间过程，降低超调量。在应用过 程屮，可能会用PI、PD或PID控制，本系统只利用PI控制器进行校正优化。3. 2 simulink动态仿真以及系统校正本文所述电机系统要求在电圧输入端输入单位阶跃电床(1V)后，直流伺服电机的转 轴应能输出稳泄的转角，且应同时满足下列要求：系统调整时间tso.is,最大超调量Mp ki=253, kd=O发现在随着kd的增加其稳定时间在减小，超调最也在减小，故仍需要增加kd比例 增益系数值Kp=30, ki=253, kd=OKp=40t ki=253, kd=O通过观察，当大于40时，随着kp增加，稳定时间在减少，但超调量在增加，故需要将 kp值设置在30到40之间，通过以上分析可知，随着kp值的增加，其稳定时间在减小，但超 调量先减小，后增加故对30和40之间进行试验，通过实验和调节，得到kp最佳值为33,这 是的响应曲线为：00.050.10.15Time (sec)系统的上升时间为0.02s,超调量为14.4%,稳定时间为0.0703,满足系统时域要求。V分析其频率特性，观察其频域响应,其bode图如下:Q Figure 1File Edit Vievk Insert Tools Desktop 空ndow HelpD旨口俸fe釵QOS）眶 0 口口(Eop) OOELCfipepnuuBes101 102 103Froquoncy (rad/eoc)通过分析知其幅值裕度为无穷大，相位穿越频率为无穷大。相位裕度为S4.5,幅值穿越 频率为68.3rad/s,系统是稳定。对系统不再进行微分校正，通过PD校正，可知系统的时域和频域响应性能都比较好4、对利用matlab对机械臂进行动力学分析(1)模型的简介建立的模型如下：为方便对其进行分析，做一下假设：连杆的密度是均匀的， rl=O. 8, rcl=O. 4, r2=0. 6, rc2=0. 3,连杆质星ml二4kg, m2=2. 8kg,并计算得到两连杆各自质心的 转动惯量分别为：11=0. 16kg -m2 , 12=0.063 kg m2 ,其所承受重量为mpl=2kgo(2)运动学方程的建立其中以第一较链点为坐标原点，则其手部点的矢量方程为：Rpi=Ri+R2需要注意的是下面所给的角度中，都是相对于前一连杆的方位，而不是相对于整体坐标 系x轴的，这种习惯在机器人学中很普遍，其源于安装在及其人手臂的传感器所测得的是连杆 的相对转角，而不是绝对转角。对应的标量方程为：xpi = r cos(?i + r2cos(0i -b 62)yPi = ri sinft +%)对上式进行求导得:工讯=rtwjsin r2（ttj）+?）sin（!?i + 仇）yPi = 门oncost + 厂2（仙Taj：cos（ +%）转化为矩阵如下：并有G? = cos（久十念）,Sn =sin（仿 +）dpi r 厂】$ r?S2 尸？Si2 -yPi J L 门 G + r2 C12 r2 Ci2 .通过查找相关资料可以得到以下方程：Xpi 4-（H Si + 厂2S12）tfi + FSjai = （尸 1C + C2+ eG2i+（H Sj + rc2 S12 ）ai +rkSi2G =（厂iC+nG2o/? + eG?応+2&（ui 妙 Cj4,y + 1G rclC2 ）ai rC3（Z2 =一 （口 Si +&Si2）tt 4 Ta+223蚀 Sj（3人动力学方程第一个连杆的受力分析和相关方程F 01 “Fiji+ F?1,J =Few + Em rnxg = mxacX,yM M2 F21/1S1 +F2i./G mgrci Ci = IiQi 第二个连杆的受力分析及运动方程：Fg Fzj.z =F3川一 Eg 一叫g = m mplg综上所分析，共有六个运动学方程，有八个动力学方程，联立这十四个方程 （4）建立约束矩阵联立这14个方程如下：小2殆Z200001000000o 1i000001000000 a2汕0100000000000cU001000000000lrrl5l *112Gia001000000000%22000100000000心y0000000101000A/iU000-眄0000010100A000000000貞口000000加、A00000-101000000处200000T01hx0h00000000V2vn。:1C|?“-巧2lly000000mpt0（JD0e10Slx.0000000叶00000132y 一 qq +勺qjto?十Ge? + 2勺叫c】21-（rjSj + r2sxl十2曲七234弭门-乜G斫-詁研TSG + G2G2冏 + rc2Cl2i + 2力3|5久 J亠+力殆）十一22囲十2:22250叫T| 一乃一叫jG0叫20-咻根据方程建立simulink框图，并进行编程仍真:其中建立的simulink模型如卜:编写的仿真程序为： 文件名robot. mfunct ion out=robot(u)%u(l)=wl%u(2)=sl%u(3)二 w2%u(4)=s2%u(5) =torl%u(6) =tor2 g=9. 8067;rl=0. 8;rcl=0. 4;r2=0. 6; rc2=0. 3; ml=4;m2=2. 8;11=0. 16; 12=0. 063;mpl=2;sl=sin(u(2);sl2=sin(u(2)+u(4); cl=cos(u(2);cl2=cos(u(2)+u(4); a=zeros(14, 14);b=zeros (14, 1);a(l, I)=rl*sl+r2*sl2; a(l, 2)=r2*sl2; a(l, 7)=1;a(2, l)=-rl*cl-r2*cl2; a(2, 2)=-r2*cl2; a(2, 8)=1;a(3, l)=rcl*sl; a(3, 3)=1;a(4, l)=-rcl*cl; a(4, 4)=1;a(5, I)=rl*sl+rc2*sl2; a(5, 2)=rc2*sl2; a(5, 5) =1a(6, l)=-rl*cl-rc2*cl2; a(6, 2)=-rc2*cl2; a(6, 6) =1;a(7, 3)二一ml;a(7, 9) =1;a(7, 11) =1;a(8, 4) =-ml; a(8, 10)=1; a(8, 12)=1;a(9, l)=Il;a(9, U)=rl*sl;a(9, 12)=-rl*cl;a(10, 5)=-m2;a(10, ll)=-l;a(10, 13)=1;a(ll, 6)=-m2;a(U, 12)=-l;a(ll, 14)=1;a(12, 2) =12;a(12, 11) =rc2*sl2;a(12, 12)=-rc2*cl2.a(12, 13) = (r2-rc2)*sl2;a(12, 14)=-(r2-rc2)*d2;a(13, 7) =mpl; a(13, 13) =1;a(14, 8)=mpl;a(14, 14)=1;%b(1) =-(r 1 *c 1+r2*c 12)*u(l)*2+r2*c 12*u(3)*2+2*r2*u(l)*u(3)*cl2); b(2) =-(rl*sl+r2*sl2)*u(l) 2+r2*sl2*u(3) 2+2*r2*u(l)*u(3)*sl2); b(3)=-rcl*cl*u(l) 2;b(4)=-rcl*sl*u(l) 2;b(5)=-(rl*cl+rc2*cl2)*u(l) 2+rc2*cl2*u(3) 2+2*rc2*u(1)*u(3)*c12);b(6)=-(rl*sl+rc2*sl2)*u(l) 2+rc2*sl2*u(3)2+2*rc2*u(l)*u(3)*sl2); b(8)二 ml*g;b(9)=u(5)u(6)-ml*g*rcl*cl;b仃1)=m2*g;b(12)=u(6);b(14)=-mpl*g;%out=inv(a)*bMATLAB Fen两数的设置如F：0 Bloek Parameters! MATLAB Fen仿真结果当系统输入转矩都为0.且处于下图所示位宣时（8 1二tt/2, 6 2=0）,在重力作用及轴 承等縻擦粘滞阻尼而产生的数度损失系数为2这时利用matlab进行系统仿真，输出四个数据，分别为连杆一的角度，连杆二的角度。 端点P的X方向，Y方向点的变化坐标这时示波器显示如卜：Q $copl1 o T寻23MOQ e A在这样的条件下，端点p的坐标轨迹的求法：利用to workspace模块对仿真程序的数据进行保存。保存数组名称为simout,然后让输岀 的P点X方向坐标为横坐标，丫方向坐标为从坐标画图，并修饰，程序如下：plot(simout(1:60,3),simout(1:60,4),b);hold on;plot(simout(60:115,3),simout(60:115,4),r);hold on;plot(simout(115:171,3),simout(115:171,4),/);hold on;plot(simout(171:201,3),simout(171:201,4),g);grid on;box off;title(卩点运动轨迹)；xlabel(卩点横坐标);ylabel(卩点纵坐标)；legendf第一次顺时针运动第一次逆时针运动第二次顺时针运动T第二次逆时针运 动)；得到如下图： . - h 第一次颇时针运动 第一次逆时针运动 第二次妙时针运动 第二次逆时苕运动Vn 1r1P点运动轨进2 4 6 8-O-O4-O.:+-.12槪坐标当系统输入转矩T1=T2=0,且处于卜图所示位置时(8 1=0, 0 2=0)驱动及巫力作用及轴承等摩擦粘滞阻尼而产生的数度损失系数为2利用to workspace模块对仿真程序的数据进行保存。保存数组名称为simout,然后让输出的P 点X方向坐标为横坐标，丫方向坐标为从坐标画图，并修饰得到如下图：Q5005P点構坐标0 2 40 06 80 0归斛S取d第一次时针运动 第一次谨时针运动 箱二次颤时针运动 第二次逆时竹运动P点运动91进当系统输入转矩T1=5. T2=0,且处于位置时(6 1=0, 0 2=0),在驱动及更力作用及轴承等 摩擦粘滞阻尼而产生的数度损失系数为2利用to workspace模块对仿真程序的数据进行保存。保存数组名称为simout,然后让输出的P 点X方向坐标为横坐标，丫方向坐标为从坐标画图，并修饰，得到如下图：0204-06-08薦一次談时针运动第一次逆时针运动第二次議时针运动第次 ifritiain-05(6 1=0? 0 2=0)，在驱动及重力作用当系统输入转矩T1=5. T2=0,且处于下图所示位置时 及轴承等摩擦粘滞阻尼而产生的数度损失系数为20P点椅生标0.50P点椅生标0.51.5P点运动轨迹0.5005-1怎部衮旺d0P点椅生标0.50P点椅生标0.5由此，可以得到此次仿真是完全成功的.0P点椅生标0.5