「单摆」单摆控制教程

单摆

1. 单摆模型

$\ddot{\theta} = -asin(\theta)-b\dot{\theta}+cT$

为使得单摆在 $\theta = \delta$ 处保持平衡，力矩必须有一个稳态分量 $T_{ss}$ 满足

$0 = -asin(\delta)+cT_{ss}$

选择状态变量为 $x_1 = \theta-\delta, x_2 = \dot{\theta}$ , 控制变量为 $u = T- T_{ss}$ ,则状态方程变为

$\dot{x}_1=x_2$

$\dot{x}_2=-a[sin(x_1+\delta)-sin(\delta)]-bx_2+cu$

2. 线性化控制

将系统在原点线性化得：

$A= \begin{bmatrix} 0 & 1 \\ -acos(x_1+\delta) & -b \end{bmatrix} =\begin{bmatrix} 0 & 1 \\ -acos(\delta) & -b \end{bmatrix} B= \begin{bmatrix} 0 \\ c \end{bmatrix}$

取 K = [k_1,k_2] ，容易验证当

$k_1>-\frac{acos{\delta}}{c},k_2>\frac{b}{c}$

时，A-BK是赫尔维茨矩阵，力矩为

$T=\frac{asin(\delta)}{c}-Kx=\frac{asin(\delta)}{c}-k_1(\theta-\delta)-k_2\dot{\theta}$

3. Matlab仿真代码

%=========== 单摆控制-状态反馈 ===========%
clear all;clc;close all;
%% 参数设置
g = 9.8; % 重力加速度
l = 1;   % 摆长
k = 0.5; % 摩擦系数
m = 1;   % 摆球质量
a = g/l; b = k/m; c = 1/(m*l^2);
theta(1) = 0;
delta = -pi/5;
dtheta(1) = 0.2;
Interval = 0.05;
t = 0:interval:40;
k1 = -a*cos(delta)+5;
k2 = -b/c + 5;
A = [0 1; -a*cos(delta) -b]; B = [0;c];
K = [k1 k2];
eig(A - B*K)
%% 状态变化
for i = 2:1:length(t)
   T = a*sin(delta)/c - k1*(theta(i-1)-delta) - k2*dtheta(i-1);
%   T =  0;
  ddtheta = -a*sin(theta(i-1)) - b*dtheta(i-1) +c*T;
  dtheta(i) = dtheta(i-1) + ddtheta*interval;
  theta(i) = theta(i-1) + dtheta(i)*interval;
end
figure 
plot(t,theta,'r')

figure
%绘制横梁

colordef black
plot([-0.2;0.2],[0;0],'-y','LineWidth',20);
x0=l*sin(theta(1));% 初始 x 坐标
y0=-l*cos(theta(1));% 初始 y 坐标
axis([-0.75,0.75,-1.25,1.25]);
axis off
%创建摆锤
%擦除模式为 xor
% head=line(x0,y0,'color','r','linestyle','.',...
% 'erasemode','xor','markersize',40);
hold on
%创建摆杆
body=line([0;x0],[-0.05;y0],'color','g','linestyle','-','erasemode','xor','LineWidth',2);
head = [];
for i = 2:1:length(t)
    x=l*sin(theta(i));
    y=-l*cos(theta(i));
%     set(head,'xdata',x,'ydata',y);% 改变擦除对象的坐标数据
    set(body,'xdata',[0;x],'ydata',[-0.05;y]);
    delete(head);
    head = plot(x,y,'m.','MarkerSize',40);
    
    drawnow;% 刷新屏幕
    pause(0.1)
    
    F = getframe(gcf);
    I = frame2im(F);
    [I,map] = rgb2ind(I,256);
    if (i == 2)
       imwrite(I,map,'single.gif','gif','Loopcount',inf,'Delaytime',0.2);
    else
       imwrite(I,map,'single.gif','gif','WriteMode','APPend','DelayTime',0.2); 
    end
    
end

4. 控制效果

5. 积分控制

积分控制中，不用寻找计算为保持平衡位置所需要的稳态力矩。此时的反馈控制率为：

$u= -k_1(\theta-\delta)-k2\dot{\theta}-k_3\sigma$

$\dot{\sigma}=\theta-\delta$

加入积分控制后，即不需要再寻找平衡力矩就可以实现稳态控制

6. 仿真结果

%=========== 单摆控制-线性化状态反馈 ===========%
clear all;clc;close all;
%% 参数设置
g = 9.8; % 重力加速度
l = 1;   % 摆长
k = 0.5; % 摩擦系数
m = 1;   % 摆球质量
a = g/l; b = k/m; c = 1/(m*l^2);
theta(1) = -pi/2;
delta = pi/4;
dtheta(1) = 0.2;
interval = 0.05;
t = 0:interval:40;
k1 = -a*cos(delta)+5;
k2 = -b/c + 5;
A = [0 1; -a*cos(delta) -b]; B = [0;c];
K = [k1 k2];
k3 = 3;
eig(A - B*K)
alpha(1) = 0;
%% 状态变化
for i = 2:1:length(t)
%    T = a*sin(delta)/c - k1*(theta(i-1)-delta) - k2*dtheta(i-1);
%   T =  0;
  dalpha = theta(i-1) - delta;
  alpha(i) =  alpha(i-1) + dalpha*interval;
  T = - k1*(theta(i-1)-delta) - k2*dtheta(i-1) -k3*alpha(i);
  ddtheta = -a*sin(theta(i-1)) - b*dtheta(i-1) +c*T;
  dtheta(i) = dtheta(i-1) + ddtheta*interval;
  theta(i) = theta(i-1) + dtheta(i)*interval;
end
figure 
plot(t,theta,'y','LineWidth',2)

figure
%绘制横梁
colordef black
plot([-0.2;0.2],[0;0],'-y','LineWidth',20);
x0=l*sin(theta(1));% 初始 x 坐标
y0=-l*cos(theta(1));% 初始 y 坐标
axis([-1,1,-1.25,1.25]);
axis off

%创建摆锤
%擦除模式为 xor
% head=line(x0,y0,'color','r','linestyle','.',...
% 'erasemode','xor','markersize',40);
hold on
%创建摆杆
body=line([0;x0],[-0.05;y0],'color','g','linestyle','-','erasemode','xor','LineWidth',2);
head = [];
for i = 2:1:length(t)
    x=l*sin(theta(i));
    y=-l*cos(theta(i));
%     set(head,'xdata',x,'ydata',y);% 改变擦除对象的坐标数据
    set(body,'xdata',[0;x],'ydata',[-0.05;y]);
    delete(head);
    head = plot(x,y,'m.','MarkerSize',40);
    
    drawnow;% 刷新屏幕
    pause(0.1)
    
%     F = getframe(gcf);
%     I = frame2im(F);
%     [I,map] = rgb2ind(I,256);
%     if (i == 2)
%        imwrite(I,map,'single.gif','gif','Loopcount',inf,'Delaytime',0.2);
%     else
%        imwrite(I,map,'single.gif','gif','WriteMode','append','DelayTime',0.2); 
%     end
    
end

文章最后发布于: 2019-06-28 09:40:10

单摆控制教程

单摆

1. 单摆模型

2. 线性化控制

3. Matlab仿真代码

4. 控制效果

5. 积分控制

6. 仿真结果

相关阅读

栏目导航

推荐阅读

热门阅读