机械运动系统设计与实践报告.docx

1、机械运动系统设计与实践报告机械运动系统设计与实践报告 学 院： 工程学院 专 业： 机械设计及理论 姓 名： 学 号： 指导教师： 2012 年 3 月环形倒立摆Adams与Mat lab联合仿真分析一、实践目的1. 熟悉和理解环形倒立摆的组成部件及运动原理。2. 通过ADAMS软件对环形倒立摆进行建模，熟练该款软件。3. 通过运用ADAMS导出模块与mat lab进行对接处理，熟悉mat lab软件的PID运行控制以及掌握simulink的相关设置。二、实验原理环形倒立摆是通过电机驱动，带动连杆，同时连杆来带动摆杆运动。中间需要测量的物理参数主要有：连杆的角度、连杆的角速度、摆杆的角度、摆杆

2、的角速度。通过两个光电编码器进行反馈。从反馈过程中来调节电机的转速及转向，进而达到摆杆的倒立并保持相对平衡。下图1即为实验室固高环形倒立摆系统组成框架：图1 环形倒立摆组成框架机房中matlab6.5中控制倒立摆的原理图二老师给的程序% this is a new simulation program for new arm-type inverted pendulum % by lgr input on the state of 0 of arm and that of 0 of pendulum% ! CAUTION ,changed base of direction of arm% S

3、uc.clear allclose all%-Ma=0.21;Mp=0.062;la=0.133;lp=0.068;La=0.25;Lp=0.19;Ja=5E-3;Jp=5.65E-4;Ca=5.52E-3;Cp=1.69E-4;kt=0.0419;g=9.8;%-continuous state equation-ce= 1 0 0 0 ; 0 1 0 0 ; 0 0 Mp*La2+Ja Mp*La*lp ; 0 0 Mp*La*lp Jp ;ca= 0 0 1 0 ; 0 0 0 1 ; -(Mp*la+Mp*La)*g 0 -(Ca+Cp) Cp ; 0 Mp*g*lp Cp -Cp ;

4、cb=0;0;kt;0;CA=inv(ce)*ca;CB=inv(ce)*cb;C=1 0 0 0 ; 0 1 0 0;%-Q=diag(100,10,1,1);R=1;f,s,e=lqr(CA,CB,Q,R);%-dt=0.005;%sampling timeth_a=0.1 %accdd(Initial angle of arm ,0.1);% ! caution,clock di%rection th_p=pi;% =accdd(Initial angle of pendulum ,pi);% ! caution,clock dir%ection +d_th_a =0.0;%d_th_p

5、 =0.0;%initial values of statesdd_th_a =0.0;%dd_th_p =0.0;%u =0.0;z =0.0;energy =0.0;i =1;%- input parameter-alpha =30.0 ; %alpha =accdd(Design Parameter alpha,30.0);energy_d =1.5*Mp*g*lp; %accdd(Desired Energy,1.5*Mp*g*lp);zeta =1 % accdd(Damping Factor,1.0);wc =1 % accdd(Damping Factor,1.0);flag =

6、0;%- starting loop -for i =1:6.0/dt th_a_0 =th_a;%th_p_0 =th_p;%d_th_a_0 =d_th_a;%d_th_p_0 =d_th_p;%dd_th_a_1 =dd_th_a;%dd_th_p_0 =dd_th_p;% if cos (th_p)=cos(0.2); flag=1;endif flag=0 %- swimping up - dd_th_a = z-(2*zeta*wc*d_th_a+wc2*th_a); ydd_temp = -Mp*La*lp*cos(th_a-th_p)*dd_th_a+Mp*La*lp*d_th

7、_a2*sin(th_a-th_p); dd_th_p =(ydd_temp+Mp*g*lp*sin(th_p)+Cp*d_th_a-Cp*d_th_p)/Jp; d_th_a =d_th_a_0+dd_th_a*dt; d_th_p =d_th_p_0+dd_th_p*dt; th_a =th_a_0+d_th_a_0*dt+dd_th_a*dt2*0.5; th_p =th_p_0+d_th_p_0*dt+dd_th_p*dt2*0.5;else eq_1_1 =-Mp*La*lp*d_th_p2*sin(th_a-th_p)-(Mp*la+Mp*La)*g*sin(th_a); eq_1

8、_2 =-(Ca+Cp)*d_th_a+Cp*d_th_p+kt*u; eq_1_left =eq_1_1+eq_1_2; eq_2_1 =Mp*La*lp*d_th_a2*sin(th_a-th_p)+Mp*lp*g*sin(th_p); eq_2_2 =Cp*d_th_a-Cp*d_th_p; eq_2_left =eq_2_1+eq_2_2;%- calculation of inverse matrix - M11 =Mp*La2+Ja; M12 =Mp*La*lp*cos(th_a-th_p); M21=Mp*La*lp*cos(th_a-th_p); M22=Jp; M=M11 M

9、12;M21 M22; MI=inv(M);%-calculation of parameters- dd_th_a=MI(1,1)*eq_1_left+MI(1,2)*eq_2_left; dd_th_p=MI(2,1)*eq_1_left+MI(2,2)*eq_2_left; d_th_a= d_th_a_0+ dd_th_a*dt; d_th_p= d_th_p_0+ dd_th_p*dt; th_a= th_a_0+ d_th_a_0*dt+ dd_th_a*dt2*0.5; th_p= th_p_0+ d_th_p_0*dt+ dd_th_p*dt2*0.5;endenergy=0.

10、5*Jp*d_th_p2+Mp*g*lp*cos(th_p);%energy of pendulumpotential=Mp*g* lp*cos(th_p);kinetic=0.5*Jp*d_th_p2;%-if flag=0 error=energy-energy_d; u1=(Mp*La2+Ja)*dd_th_a+Mp*La*lp* cos(th_a-th_p)*dd_th_p+(Ca+Cp)*d_th_a; u2=Mp*La*lp*d_th_p2*sin(th_a-th_p)+(Ma*la+Mp*La)*g*sin(th_a)-Cp*d_th_p; u=(u1+u2)/kt; z=alp

11、ha*d_th_p*cos(th_p)*error;else u=-f*th_a;th_p;d_th_a;d_th_p;end%-Limit input-if u=4.995 u=4.995;else if u=-4.995 u=-4.995; endend%-save data-accel_th_a(i)=dd_th_a;a的加速度accel_th_p(i)=dd_th_p;p的加速度veloc_th_a(i)=dd_th_a;a的速度veloc_th_p(i)=dd_th_p;p的速度angle_th_a(i)=th_a;a的角度angle_th_p(i)=th_p;p的角度Energy1

12、(i)=energy;能量方程Energy2(i)=kinetic;动力方程Energy3(i)=potential;input(i)=u;t(i)=i*dt; end %-end loop-subplot(221);plot(t,angle_th_a);xlabel(timesec);ylabel(Angle of Arm rad);gridsubplot(222);plot(t,angle_th_p);xlabel(timesec);ylabel(Angle of Pendulum rad);gridsubplot(223);plot(t,input);xlabel(timesec);yl

13、abel(Input);gridsubplot(224);plot(t,Energy1,t,Energy2,:,t,Energy3,-);xlabel(timesec);ylabel(Energy of Pendulum);程序中的一些句子% this is a new simulation program for new arm-type inverted pendulum这是一个新臂型倒立摆的新的仿真程序% by lgr input on the state of 0 of arm and that of 0 of pendulum由 lgr 输入的手臂 0 和 0 摆的状态% ! CAUTION ,changed base of direction of arm注意，改变基