如何用Matlab仿真一阶倒立摆模型

Posted 2023-05-12

%主程序：

clear all
close all
global A B C D

g=9.8;%重力加速度
M=1.0;%小车质量
m=0.1;%杆的质量
L=0.5;%杆长度的一半
Fc=0.0005;%小车相对导轨的摩擦系数
Fp=0.000002;%杆相对于小车的摩擦系数

I=1/12*m*L^2;
l=1/2*L;
t1=m*(M+m)*g*l/[(M+m)*I+M*m*l^2];
t2=-m^2*g*l^2/[(m+M)*I+M*m*l^2];
t3=-m*l/[(M+m)*I+M*m*l^2];
t4=(I+m*l^2)/[(m+M)*I+M*m*l^2];

A=[0 1 0 0;
t1 0 0 0;
0 0 0 1;
t2 0 0 0];

B=[0;t3;0;t4];

C=[1 0 0 0;
0 0 1 0];

D=[0;0];

Q=[100 0 0 0;
0 10 0 0;
0 0 1 0;
0 0 0 1];

R=[0.1];
K=lqr(A,B,Q,R);

e1_1=0;
e2_1=0;
e3_1=0;
e3_1=0;

u_1=0;
xk=[-10/57.3,0,0.20,0];

ts=0.02;

for k=1:1:1000
time(k)=k*ts;
Tspan=[0 ts];

para=u_1;
[t,x]=ode45('daolibai_PID_Controller_SubFunction',Tspan,xk,[],para);
xk=x(length(x),:);

r1(k)=0.0;
r2(k)=0.0;
r3(k)=0.0;
r4(k)=0.0;

x1(k)=xk(1);
x2(k)=xk(2);
x3(k)=xk(3);
x4(k)=xk(4);

e1(k)=r1(k)-x1(k);
e2(k)=r2(k)-x2(k);
e3(k)=r3(k)-x3(k);
e4(k)=r4(k)-x4(k);

S=1;
if S==1
u(k)=K(1)*e1(k)+K(2)*e2(k)+K(3)*e3(k)+K(4)*e4(k);
elseif S==2
de1(k)=e1(k)-e1_1;
u1(k)=-50*e1(k)-10*de1(k);
de2(k)=e2(k)-e2_1;
u2(k)=-10*e2(k)-10*de2(k);
de3(k)=e3(k)-e3_1;
u3(k)=-10*e3(k)-10*de3(k);
de4(k)=e4(k)-e4_1;
u4(k)=-10*e4(k)-10*de4(k);
u(k)=u1(k)+u2(k)+u3(k)+u4(k);
end

if u(k)>=10
u(k)=10;
elseif u(k)<=-10
u(k)=-10;
end
e1_1=e1(k);
e2_1=e2(k);
e3_1=e3(k);
e4_1=e4(k);
u_1=u(k);
end

figure(1);
subplot(411);
plot(time,r1,'k',time,x1,'k');
xlabel('time(s)');ylabel('Angle');
subplot(412);
plot(time,r2,'k',time,x2,'k');
xlabel('time(s)');ylabel('Angle rate');
subplot(413);
plot(time,r3,'k',time,x3,'k');
xlabel('time(s)');ylabel('Car Position');
subplot(414);
plot(time,r4,'k',time,x4,'k');
xlabel('time(s)');ylabel('Car rate');

figure(5);
plot(time,u,'k');
xlabel('time(s)');ylabel('Force');

%子程序
function dx=dym(t,x,flag,para)
global A B C D
u=para;
dx=zeros(4,1);
dx=A*x+B*u;
%刚学，望多多交流，多多批评指正 参考技术A 请参考MATLAB的help中的Demo，，里面有一个例子，详细的分析了建模的过程以及使用MATLAB实现的过程。

倒立摆控制基于UKF无迹卡尔曼滤波的倒立摆控制simulink仿真

1.软件版本

matlab2013b

2.系统概述

整个系统的理论来自如下的论文：

其中的设计结构为：

2.1 UKF

其仿真结果如下所示：

反馈控制信号：

倒立摆四个参数信号：

2.2 PF

其仿真结果如下所示：

反馈控制信号：

倒立摆四个参数信号：

2.3 UPF

其仿真结果如下所示：

反馈控制信号：

倒立摆四个参数信号：

A08-13