車輪型倒立振子ロボッ​トのライントレース機​能の実装について。

%%CMG台車型倒立振子inverted pendulum chassis 最適レギュレータシステム

%%x=[θ dθ φ dφ y dy]

x0=[pi/90 0 0 0 0 0];%初期値

omega=187.44;%[rad/s]

g=9.81;%重力加速度[m/(s^2)]

mw=0.10112; mg=0.09925; mb=0.45207; mc=0.300; %円板，ジンバル，本体，台車，[kg]

M=mw+mg+mb+mc;%[g]

M1=mw+mg+mb;%[g]

L=0.650;%全体の重心[m]

m=mg+mw;%[g]

lg=0.01; lw=0.158; lb=(M1*L-(mg*(lw-lg)+mw*lw))/mb;%重心

I1=0.00872;%[kg*m^2]Iθ θ方向の全体慣性モーメント

I2=0.00032;%Iφ φ方向のジンバル+円板慣性モーメント

Iw=0.0006857;%円板の慣性モーメント

C1=0.001756;%θ方向の粘性抵抗係数[Nms/rad]

C2=0.00054952;%φ方向の粘性抵抗係数[Nms/rad]

C3=0.001;%ｙ方向の抵抗係数[N/(m/s)]

%%

J=(M*I1-(M1*L)^2);

A=[0 1 0 0 0 0;

M*M1*g*L/J -M*C1/J 0 -M*Iw*omega/J 0 -M1*L*C3/J;

0 0 0 1 0 0;

0 Iw*omega/I2 -m*g*lg/I2 -C2/I2 0 0;

0 0 0 0 0 1;

(M1*L)^2*g/J -M1*L*C1/J 0 -M1*L*Iw*omega/J 0 -I1*C3/J];

B=[0 0;%%ジンバル，台車入力

0 M1*L/J;

0 0;

1/I2 0;

0 0;

0 I1/J];

B2=[0 0;%%台車入力のみ

0 M1*L/(M*I1-(M1*L)^2);

0 0;

0 0;

0 0;

0 I1/(M*I1-(M1*L)^2)];

B3=[0 0;%%ジンバル入力のみ

0 0;

0 0;

1/I2 0;

0 0;

0 0];

C=[180/pi 0 0 0 0 0;

0 0 180/pi 0 0 0;

0 0 0 0 100 0];

Pe=eig(A)

%%

U=ctrb(A,B);%可制御性行列

RankU=rank(U)

%detU=det(U)

V=obsv(A,C);%可観測性行列

RankV=rank(V)

%detV=det(V)

Q = [2000 0 0 0 0 0;

0 10 0 0 0 0;

0 0 100 0 0 0;

0 0 0 10 0 0;

0 0 0 0 10 0;

0 0 0 0 0 10];

R = [1 0;

0 1];

F=-lqr(A,B,Q,R)%最適レギュレータの状態フィードバック行列を求める

p0=eig(A+B*F)%最適レギュレータの極を確認する