基于状态空间方程的倒立摆小车的控制方法设计2

系统参数：

(M)       小车质量                       0.5 kg

(m)       倒立摆质量                     0.2 kg

(b)       小车的阻尼系数                0.1 N/m/sec

(l)       倒立摆长度                    0.3 m

(I)       倒立摆转动惯量               0.006 kg.m^2

(F)       施加在小车上的力

(x)       小车的位置坐标

(theta)   倒立摆与垂直方向的夹角（从垂直向下开始计）

动力学建模：

 考虑水平方向的力矩平衡：

以上两式合并得到：

考虑垂直施加在倒立摆上的力及质心力矩平衡方程：

消去  P N ，得到如下公式

简化： 

简化后模型：

传递函数模型：

设：

PID 控制：

或者如下表示：

M = 0.5; m = 0.2; b = 0.1; I = 0.006; g = 9.8; l = 0.3; q = (M+m)*(I+m*l^2)-(m*l)^2; s = tf('s'); P_pend = (m*l*s/q)/(s^3 + (b*(I + m*l^2))*s^2/q - ((M + m)*m*g*l)*s/q - b*m*g*l/q);

Kp = 1; Ki = 1; Kd = 1; C = pid(Kp,Ki,Kd); T = feedback(P_pend,C);

t=0:0.01:10; impulse(T,t) title('Response of Pendulum Position to an Impulse Disturbance under PID Control: Kp = 1, Ki = 1, Kd = 1');

Kp = 100; Ki = 1; Kd = 1; C = pid(Kp,Ki,Kd); T = feedback(P_pend,C); t=0:0.01:10; impulse(T,t) axis([0, 2.5, -0.2, 0.2]); title('Response of Pendulum Position to an Impulse Disturbance under PID Control: Kp = 100, Ki = 1, Kd = 1');

Kp = 100; Ki = 1; Kd = 20; C = pid(Kp,Ki,Kd); T = feedback(P_pend,C); t=0:0.01:10; impulse(T,t) axis([0, 2.5, -0.2, 0.2]); title('Response of Pendulum Position to an Impulse Disturbance under PID Control: Kp = 100, Ki = 1, Kd = 20');

                         这时，小车的位置?

传递函数：  

P_cart = (((I+m*l^2)/q)*s^2 - (m*g*l/q))/(s^4 + (b*(I + m*l^2))*s^3/q - ((M + m)*m*g*l)*s^2/q - b*m*g*l*s/q); T2 = feedback(1,P_pend*C)*P_cart; t = 0:0.01:5; impulse(T2, t); title('Response of Cart Position to an Impulse Disturbance under PID Control: Kp = 100, Ki = 1, Kd = 20');

基于状态空间方程的倒立摆小车的控制方法设计

▪开环根

▪(LQR)

▪增加补偿

▪基于观测器的控制

1 状态空间方程

设计目标：

1．倒立摆倒立状态

2．小车向右移动0.2米 

首先假设系统状态全部测量获得：

求开环的根，即A的特征值

M = 0.5;

m = 0.2;

b = 0.1;

I = 0.006;

g = 9.8;

l = 0.3;

p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices

A = [0      1              0           0;

     0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;

     0      0              0           1;

     0 -(m*l*b)/p       m*g*l*(M+m)/p  0];

B = [     0;

     (I+m*l^2)/p;

          0;

        m*l/p];

C = [1 0 0 0;

     0 0 1 0];

D = [0;

     0];

states = {'x' 'x_dot' 'phi' 'phi_dot'};

inputs = {'u'};

outputs = {'x'; 'phi'};

sys_ss = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);

poles = eig(A)

poles =

         0

   -5.6041

   -0.1428

    5.5651

线性二次型整定 (LQR)

1．检查可控性

co = ctrb(sys_ss);

controllability = rank(co)

controllability = 4

设 R=1

Q = C'*C

Q =

     1     0     0     0

     0     0     0     0

     0     0     1     0

     0     0     0     0

Q = C'*C;

R = I;

K = lqr(A,B,Q,R)

Ac = [(A-B*K)];

Bc = [B];

Cc = [C];

Dc = [D];

states = {'x' 'x_dot' 'phi' 'phi_dot'};

inputs = {'r'};

outputs = {'x'; 'phi'};

sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;

r =0.2*ones(size(t));

[y,t,x]=lsim(sys_cl,r,t);

[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');

set(get(AX(1),'Ylabel'),'String','cart position (m)')

set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')

title('Step Response with LQR Control')

K =

   -1.0000   -1.6567   18.6854    3.4594

Q = C'*C;

Q(1,1) = 5000;

Q(3,3) = 100

R = 1;

K = lqr(A,B,Q,R)

Ac = [(A-B*K)];

Bc = [B];

Cc = [C];

Dc = [D];

states = {'x' 'x_dot' 'phi' 'phi_dot'};

inputs = {'r'};

outputs = {'x'; 'phi'};

sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;

r =0.2*ones(size(t));

[y,t,x]=lsim(sys_cl,r,t);

[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');

set(get(AX(1),'Ylabel'),'String','cart position (m)')

set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')

title('Step Response with LQR Control')

Q =

        5000           0           0           0

           0           0           0           0

           0           0         100           0

           0           0           0           0

K =

  -70.7107  -37.8345  105.5298   20.9238

增加补偿系数：

Cn = [1 0 0 0];

sys_ss = ss(A,B,Cn,0);

Nbar = rscale(sys_ss,K)

Nbar =

  -70.7107

sys_cl = ss(Ac,Bc*Nbar,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;

r =0.2*ones(size(t));

[y,t,x]=lsim(sys_cl,r,t);

[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');

set(get(AX(1),'Ylabel'),'String','cart position (m)')

set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')

title('Step Response with Precompensation and LQR Control')

基于能观器的控制

检查能观性：

ob = obsv(sys_ss);

observability = rank(ob)

observability =

     4

能观器的根要比控制器的根快5到10倍的收敛

控制器的根：

poles = eig(Ac)

poles =

  -8.4910 + 7.9283i

  -8.4910 - 7.9283i

  -4.7592 + 0.8309i

  -4.7592 - 0.8309i

置根法设置 能观器的根：

P = [-40 -41 -42 -43];

L = place(A',C',P)'

L =

   1.0e+03 *

    0.0826   -0.0010

    1.6992   -0.0402

   -0.0014    0.0832

   -0.0762    1.7604

Ace = [(A-B*K) (B*K);

       zeros(size(A)) (A-L*C)];

Bce = [B*Nbar;

       zeros(size(B))];

Cce = [Cc zeros(size(Cc))];

Dce = [0;0];

states = {'x' 'x_dot' 'phi' 'phi_dot' 'e1' 'e2' 'e3' 'e4'};

inputs = {'r'};

outputs = {'x'; 'phi'};

sys_est_cl = ss(Ace,Bce,Cce,Dce,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;

r = 0.2*ones(size(t));

[y,t,x]=lsim(sys_est_cl,r,t);

[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');

set(get(AX(1),'Ylabel'),'String','cart position (m)')

set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')

title('Step Response with Observer-Based State-Feedback Control')

Function [Nbar]=rscale(a,b,c,d,k)

         % Given the single-input linear system:

         %       .

         %       x = Ax + Bu

         %       y = Cx + Du

         % and the feedback matrix K,

         %

         % the function rscale(sys,K) or rscale(A,B,C,D,K)

         % finds the scale factor N which will

         % eliminate the steady-state error to a step reference

         % for a continuous-time, single-input system

         % with full-state feedback using the schematic below:

         %

         %                         /---------\

         %      R         +     u  | .       |

% ---> N --->() ---->| X=Ax+Bu |--> y=Cx ---> y

         %                -|       \\---------/

         %                 |             |

% |<---- K <----|

         %

         %8/21/96 Yanjie Sun of the University of Michigan

         %        under the supervision of Prof. D. Tilbury

         %6/12/98 John Yook, Dawn Tilbury revised

         error(nargchk(2,5,nargin));

         % --- Determine which syntax is being used ---

         nargin1 = nargin;

         if (nargin1==2),    % System form

                 [A,B,C,D] = ssdata(a);

                 K=b;

         elseif (nargin1==5), % A,B,C,D matrices

                 A=a; B=b; C=c; D=d; K=k;

         else error('Input must be of the form (sys,K) or (A,B,C,D,K)')

         end;

         % compute Nbar

         s = size(A,1);

         Z = [zeros([1,s]) 1];

         N = inv([A,B;C,D])*Z';

         Nx = N(1:s);

         Nu = N(1+s);

         Nbar=Nu + K*Nx;