环形倒立摆Adams与Matlab联合仿真实例

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

学 院： 专 业： 姓 名： 学 号： 指导教师：

2012 年 3 月

环形倒立摆Adams与Matlab联合仿真分析

一、实践目的

1. 熟悉和理解环形倒立摆的组成部件及运动原理。

2. 通过ADAMS软件对环形倒立摆进行建模，熟练该款软件。

3. 通过运用ADAMS导出模块与matlab进行对接处理，熟悉matlab软件的PID运行控制

以及掌握simulink的相关设置。

二、实验原理

环形倒立摆是通过电机驱动，带动连杆，同时连杆来带动摆杆运动。中间需要测量的物理参数主要有：连杆的角度、连杆的角速度、摆杆的角度、摆杆的角速度。通过两个光电编码器进行反馈。从反馈过程中来调节电机的转速及转向，进而达到摆杆的倒立并保持相对平衡。下图1即为实验室固高环形倒立摆系统组成框架：

图1 环形倒立摆组成框架

实验室固高模型使用程序如下： （1）老师给的程序：

% 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 % Suc. clear all close 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*La^2+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 ]; 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 time

th_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 =0.0;%initial values of states dd_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('Cut Off Frequency',1.0); flag =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; end

if flag==0 %----------------------- swimping up ------------------ dd_th_a = z-(2*zeta*wc*d_th_a+wc^2*th_a); ydd_temp =

-Mp*La*lp*cos(th_a-th_p)*dd_th_a+Mp*La*lp*d_th_a^2*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*dt^2*0.5; th_p =th_p_0+d_th_p_0*dt+dd_th_p*dt^2*0.5; else eq_1_1

=-Mp*La*lp*d_th_p^2*sin(th_a-th_p)-(Mp*la+Mp*La)*g*sin(th_a); eq_1_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_a^2*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*La^2+Ja;

M12 =Mp*La*lp*cos(th_a-th_p); M21=Mp*La*lp*cos(th_a-th_p); M22=Jp;

M=[M11 M12;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*dt^2*0.5; th_p= th_p_0+ d_th_p_0*dt+ dd_th_p*dt^2*0.5;

energy=0.5*Jp*d_th_p^2+Mp*g*lp*cos(th_p);%energy of pendulum potential=Mp*g* lp*cos(th_p); kinetic=0.5*Jp*d_th_p^2;

%------------------------------------------------------------------------ if flag==0

error=energy-energy_d;

u1=(Mp*La^2+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_p^2*sin(th_a-th_p)+(Ma*la+Mp*La)*g*sin(th_a)-Cp*d_th_p;

u=(u1+u2)/kt;

z=alpha*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; end end

%----------------------------save data---------------------------- accel_th_a(i)=dd_th_a; accel_th_p(i)=dd_th_p; veloc_th_a(i)=dd_th_a; veloc_th_p(i)=dd_th_p; angle_th_a(i)=th_a; angle_th_p(i)=th_p; Energy1(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('time[sec]');

ylabel('Angle of Arm [rad]');

subplot(222);plot(t,angle_th_p); xlabel('time[sec]');

ylabel('Angle of Pendulum [rad]'); grid

subplot(223); plot(t,input); xlabel('time[sec]'); ylabel('Input'); grid

subplot(224);

plot(t,Energy1,t,Energy2,':',t,Energy3,'--'); xlabel('time[sec]');

ylabel('Energy of Pendulum');

控制模型的如图2所示：

图2 控制模型

控制曲线如图3所示：

0.2

0-0.2-0.4

Angle of Pendulum [rad]

0.4

Angle of Arm [rad]

64

20-2

24time[sec]

60

24time[sec]

6

5

Energy of Pendulum

0.150.1

0.050-0.05

Input

-5

24time[sec]

60

24time[sec]

6

图3 控制曲线

三、 ADAMS建模及参数设置处理 1. 建模

此处为了更为清晰的建模，将电机驱动以及连杆、摆杆的模型简化。通过Adams建立的模型效果整体图如图4所示：

图4 倒立摆模型

2. 模型参数化设置

（1）首 …… 此处隐藏：6019字，全部文档内容请下载后查看。喜欢就下载吧 ……