(完整版)一级直线倒立摆matlab程序

非线性作业
一 一级直线倒立摆
如图1所示
系统里的各参数变量
M ：小车系统的等效质量（1.096kg ）；
1m ：摆杆的质量（0.109kg ）；
2m ：摆杆的半长（0.25m ）；
J ：摆杆系统的转动惯量（0.0034kg*m ）；
g ：重力加速度（9.8N/Kg ）；
r ：小车的水平位置（m ）；
θ：摆角大小（以竖直向上为0起始位置，逆时针方向为正方向）； h F ：小车对摆杆水平方向作用力（N ）（向左为正方向），h F ’是其反作用力； v F ：小车对摆杆竖直方向作用力（N ）（向上为正方向），v F ’是其反作用力； U ：电动机经传动机构给小车的力，可理解为控制作用u’（向左为正方向）； p x ：摆杆重心的水平位置（m ）；p y ：摆杆重心的竖直位置（m ）。

1．1一级倒立摆的数学建模
定义系统的状态为[r,r, θ, θ]
经推导整理后可以达到倒立摆系统的牛顿力学模型：
θθθsin cos )(2mgl l r m ml I =-+ （1）
u ml r m M ml -⋅=+-⋅2sin )(cos θ
θθθ （2） 因为摆杆一般在工作在竖直向上的小领域内θ=0，可以在小范围近似处理：
0,0sin ,1cos 2==≈θθθ ，则数学模型可以整理成：
θθmgl l r m ml I =-+ )(2 （3） u r m M ml =++- )(θ （4） 系统的状态空间模型为
⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡θθ r r =⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎣⎡+++++0)()
(0010000)(0000102222Mml m M I m M mgl Mml m M I gl m ⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡θθ r r +⎥⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡+++++222)(0)(0Mml m M I ml Mml m M I ml I u （5） u r r r y ⎥⎦⎤⎢⎣⎡+⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎣⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡=0000101000θθθ （6） 代人实际系统的参数后状态方程为：
⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣
⎡θθ r r =⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡08285.2700100006293.0000010⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡θθ r r +u ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡3566.208832.00 （7） u r r r y ⎥⎦⎤⎢⎣⎡+⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣
⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡=0000101000θθθ （8） 1.2滑模变结构在一级倒立摆系统的应用
主要包括切换函数的设计、控制率的设计和系统消除抖振的抑制。

基于线性二次型最优化理论的切换函数设计，定义系统的优化积分指标是：
Qxdt x J T ⎰∞
=0 Q>0， 本文采用指数趋近律：)sgn(S kS S ε--= ，其中k 和ε为正数。

将其代人S=Cx=0中，可以得到：
)sgn(S kS CBu CAx x C S
ε--=+== （9） 控制率为：))sgn(()(1S kS CAx CB u ε++-=- （10） ε的选取主要是为了抑制系统的摩擦力和近似线性化所带来的误差和参数摄动等因素，从而使得系统具有良好的鲁棒性。

文中k=25, ε=0.8。

取变换矩阵T 。

其中T=⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎣⎡+-10000100)(01000012ml ml I ， 去Q*11=diag([300 50 350])，Q22=10
关于Riccati 方程01112122121111=+-+*-**Q P A Q PA P A PA T T
的解有MATLAB 的lqr 函数可以解出
⎢⎢⎢⎣⎡-=6098.3599281.2147165.692P 8204.1491894.1129281.214-⎥⎥⎥⎦
⎤--4007.3548204.1496098.359 [0000.1000-=C 2717.69- 6295.210 ]9610.35
[r C x S ⋅=0)( r θ ]
T θ 二 程序
%主程序：直线一级倒立摆
clear all
close all
global C M0 F
ts=0.02; %采样时间
T=30; %仿真时间
TimeSet=[0:ts:T];
para=[];
%options 为解微分函数中的调整参数,reltol 和abstol 分别是设置相对误差和绝对误差
options=odeset('RelTol',1e-3,'AbsTol',[1e-3 1e-3 1e-3 1e-3]);
%options=[];
x0=[0.5,0.3,0,0]; %初始值
[t,xout]=ode45('daolibai2eq',TimeSet,x0,options,para);%固定格式，子程序调用 %返回值
x1=xout(:,1);
x2=xout(:,2);
x3=xout(:,3);
x4=xout(:,4);
s=C(1)*x1+C(2)*x2+C(3)*x3+C(4)*x4; %切换函数或切换面
%选择控制器
if F==1
% for k=1:1:T/ts+1
M0=40;
%u(k)=-M0*sign(s(k));
u=-M0*sign(s);
%end
else if F==2
beta=30;
delta=0;
for k=1:1:T/ts+1
u(k)=-beta*(abs(x1(k))+abs(x2(k))+abs(x3(k))+abs(x4(k))+delta)*sign(s(k));
end
end
%绘图
figure(1);
plot(t,x1,'r');
xlabel('time(s)');ylabel('Cart Position');
figure(2);
plot(t,x2,'r');
xlabel('time(s)');ylabel('Pendulum Angle');
figure(3);
plot(t,s,'r');
xlabel('time(s)');ylabel('s');
figure(4);
plot(t,u,'r');
xlabel('time(s)');
ylabel('u');
end
%子程序
function dx=DxnamicModel(t,x,flag,para)%自定义动态函数
global C M0 F
%倒立摆经计算后模型
M=1.096;m=0.109;b=0.1;l=0.25;I=0.0034;T=0.005;g=0.98;
k22=-(I+m*l^2)*b/(I*(M+m)+M*m*l^2);
k23=m^2*g*l^2/(I*(M+m)+M*m*l^2);
k42=-m*l*b/(I*(M+m)+M*m*l^2);
k43=m*g*l*(M+m)/(I*(M+m)+M*m*l^2);
b12=(I+m*l^2)/(I*(M+m)+M*m*l^2);
b14=m*l/(I*(M+m)+M*m*l^2);
A=[0,1,0,0;0,k22,k23,0;0,0,0,1;0,k42,k43,0];
b=[0;b12;0;b14];
%Ackermann's formula
n1=-1;n2=-2;n3=-3;
C=[0,0,0,1]*inv([b,A*b,A^2*b,A^3*b])*(A-n1*eye(4))*(A-n2*eye(4))*(A-n3*eye(4));
s=C*x;
F=2;
if F==1
M0=40;
u=-M0*sign(s);
elseif F==2
beta=30;
delta=0;
u=-beta*(abs(x(1))+abs(x(2))+abs(x(3))+abs(x(4))+delta)*sign(s);
end
%state equation
dx=zeros(4,1);
f0=0.5;
ft=f0*sin(3*t);
dx=A*x+b*(u+ft);
三 仿真结果
time(s)C a r t P o s i t i o n
图2
time(s)P e n d u l u m A n g l e 图3
time(s)s
图4
time(s)
图5。