三级倒立摆实验仿真

本实验包含，ｍａｔｌａｂ中ｓｉｍｕｌｉｎｋ的三级倒立摆的仿真研究，以及ＬＱＲ寻优的函数，希望对大家有所帮助，谢谢

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Googol Linear 2 stage Inverted Pendulum LQR Control  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
format long;
k12=159.504;k13=-38.4356;k14=0.784646;k19=6.64904;
k22=-127.9;k23=57.9951;k24=-17.189;k29=-0.15831;
k32=2.57086;k33=-16.9245;k34=39.3485;k39=0.00318211;

A=[0  0  0  0 1  0  0  0;
   0  0  0  0 0  1  0  0;
   0  0  0  0 0  0  1  0; 
   0  0  0  0 0  0  0  1;  
   0  0  0  0 0  0  0  0;
   0 k12 k13 k14 0 0 0 0;
   0 k22 k23 k24 0 0 0 0;
   0 k32 k33 k34 0 0 0 0]

B=[ 0  0  0  0  1  k19  k29  k39]‘;

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

 Q11=300;Q22=400;Q33=500;Q44=600;
%Q11=300;Q22=500;Q33=700;Q44=900;
%Q11=1;Q22=1;Q33=1;

Q=[Q11  0   0   0   0   0   0   0; 
    0  Q22  0   0   0   0   0   0;
    0  0   Q33  0   0   0   0   0;
    0   0   0  Q44  0   0   0   0;
    0   0   0   0   0   0   0   0;
    0   0   0   0   0   0   0   0;
    0   0   0   0   0   0   0   0;
    0   0   0   0   0   0   0   0];
R=1;
K=lqr（ABQR）
Ka=[1 K];   %因为输出总是把第一位省略，所以加上一位
Ac=[（A-B*K）];
Bc=[B];
Cc=[C];
Dc=[D];

T=0:0.005:5;
U=0*ones（size（T））;
Cn=[1 0 0 0 0 0 0 0];

s=size（A1）;
Z=[zeros（[1s]） 1];

N=inv（[AB;Cn0]）*Z‘;
Nx=N（1:s）;
Nu=N（1+s）;
Nbar=Nu+K*Nx;

Bcn=[Nbar*B];
x0=[0 0 0.05 0 0 0 0 0];
[YX]=lsim（AcBcnCcDcUTx0）;
xpos=Y（:1）;
xangle=Y（:2）;
xangle2=Y（:3）;
xangle3=Y（:4）;

% plot（Txpos‘:‘）
% hold on;
% plot（Txangle‘--‘）
% hold on;
% plot（Txangle2‘*‘）
% hold on;
% plot（Txangle3‘-‘）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       34278  2013-03-15 14:36  三级倒立摆实验\L3DIPLQR1.mdl
     文件        1623  2013-03-15 14:03  三级倒立摆实验\test1.asv
     文件        1669  2013-03-15 14:29  三级倒立摆实验\test1.m
     目录           0  2013-03-15 14:36  三级倒立摆实验\