ghc_lqr_p.m

三自由度直升飞机最优控制的三轴控制和位置控制原程序

g = 9.8;  %重力加速度 单位：m/s2
pi = 3.14; %圆周率

mh = 1.8; %直升机本体质量 单位：kg
la = 0.88; %直升机本体到旋转轴的距离 单位：m
mw = 3.433; %平衡块的质量 单位：kg
lb = 0.35; %平衡快到支点的距离 单位：m

mf = 0.552; %直升机单个推进器的质量 单位：kg
mb = 0.552; %直升机单个推进器的质量 单位：kg
lh = 0.17; %直升机推进器到翻转轴的距离 单位：m

Jp = mf*lh^2 + mb*lh^2 %直升机系统翻转轴的转动惯量 单位：m*kg2

Tg = mh*g*la - mw*g*lb %系统有效的重力矩 单位：N*m
Je = mh*la^2 + mw*lb^2 %直升机系统旋转轴的转动惯量 单位：m*kg2

Kp = Tg/la %直升机的等效重力 单位：N
Jt = mh*la^2 + mw*lb^2 %直升机系统旋转轴的转动惯量 单位：m*kg2

%Kf = 1.0; %直升机电机的力常数
Kf = 12; %直升机电机的力常数

A = [ 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 0;
      0    0     0 0 0 0 0 0;
      0 Kp*la/Jt 0 0 0 0 0 0;
      1    0     0 0 0 0 0 0;
      0    0     0 0 0 1 0 0];
B = [    0         0; 
         0         0;
         0         0;
      Kf*la/Je  Kf*la/Je;
      Kf*lh/Jp  -Kf*lh/Jp;
         0         0;
         0         0;
         0         0];
R = diag([1 1]);
%Q = diag([.1 1 .1 1 1 1 .5]);
Q = diag([2 .2 .2 .02 .02 2 .02 .01]);
K = lqr(A,B,Q,R)