www.gusucode.com > 二级倒立摆的建模及仿真simulink源码程序 > code/倒立摆/参数求解.txt

    >> m0=1.328;
>> m1=0.22;
>> m2=0.187;
>> j1=0.004963;
>> j2=0.004824;
>> l1=0.304;
>> l2=0.226;
>> ll1=0.49;
>> f0=22.915;
>> f1=0.00705;
>> f2=0.00264;g0=11.887;
>> m=[m0+m1+m2,m2*ll1+m1*l1,m2*l2;m2*l1+m1*l1,j1+m1*l1^2+m2*ll1^2,m2*ll1*l2;m2*l2,m1*ll1*l2,j2+m2*l2^2]

m =

    1.7350    0.1585    0.0423
    0.1237    0.0702    0.0207
    0.0423    0.0244    0.0144

>> f=[-22.915,0,0;0,-0.00969,0.00264;0,0.00264,-0.00264]

f =

  -22.9150         0         0
         0   -0.0097    0.0026
         0    0.0026   -0.0026

>> n=[0 0 0;0 (m2*ll1+m1*l1)*10 0;0 0 m2*l2*10]

n =

         0         0         0
         0    1.5851         0
         0         0    0.4226

>> t=[1 0 0;0 1 0;0 -1 1]

t =

     1     0     0
     0     1     0
     0    -1     1
>> a21=t*inv(m)*n*inv(t)

a21 =

         0   -2.5147    0.1819
         0   32.3198  -17.6722
         0  -50.3025   76.4870

>> a22=t*inv(m)*f*inv(t)

a22 =

  -15.7473    0.0120   -0.0056
   28.1980   -0.2223    0.1937
  -29.6917    0.5639   -0.6890

>> g=[g0;0;0]

g =

   11.8870
         0
         0
>> b2=t*inv(m)*g

b2 =

    8.1688
  -14.6275
   15.4024
>> a=[zeros(3) eye(3);a21 a22]

a =

         0         0         0    1.0000         0         0
         0         0         0         0    1.0000         0
         0         0         0         0         0    1.0000
         0   -2.5147    0.1819  -15.7473    0.0120   -0.0056
         0   32.3198  -17.6722   28.1980   -0.2223    0.1937
         0  -50.3025   76.4870  -29.6917    0.5639   -0.6890
>> b=[0;0;0;b2]

b =

         0
         0
         0
    8.1688
  -14.6275
   15.4024
>> c=[eye(3) zeros(3)]

c =

     1     0     0     0     0     0
     0     1     0     0     0     0
     0     0     1     0     0     0
>> q=diag([1.6 1.6 1.6 1.6 1.6 1.6])

q =

    1.6    0     0     0     0     0
     0   1.6     0     0     0     0
     0     0   1.6     0     0     0
     0     0     0    1.6    0     0
     0     0     0     0    1.6    0
     0     0     0     0     0    1.6

>> r=0.8

r =

     0.8

>> k=lqr(a,b,q,r)

   k =

    1.4142    6.8843   80.8552    1.6163    9.4257   10.8052