MATLAB实现倒立摆演示动画

发表于 分类于 Waline： 本文字数： 535 阅读时长 ≈ 2 分钟

前几篇文章以一阶倒立摆为例介绍了状态反馈控制器和状态观测器的设计与仿真，系统状态变化通过曲线展现。本文用MATLAB实现倒立摆的动画演示，相比图像更加直观，其效果如下图所示。

jpg name

摆杆用直线表示，小车用圆圈表示，虽然简陋但足以展现效果。实现起来也特别简单，根据小车位移和摆杆角度计算摆杆两端的坐标，用直线连接起来，一帧一帧地显示就可以了。基于倒立摆的状态反馈控制仿真一文中的代码，加上动画展示以及GIF动图制作功能，如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
%% 倒立摆模型
M = 0.5; m = 0.2; b = 0.1; I = 0.006; g = 9.8; l = 0.3;
den = I*(M+m)+M*m*l^2;
A = [0,        1,                 0,        0;
     0, -(I+m*l^2)*b/den, -(m^2*g*l^2)/den, 0;
     0,        0,                 0,        1;
     0,   (m*l*b)/den,     m*g*l*(M+m)/den, 0];
B = [      0;
     (I+m*l^2)/den;
           0;
       -m*l/den];
C = [1, 0, 0, 0;
     0, 0, 1, 0];
D = [0;
     0];
sys_open = ss(A, B, C, D);

%% 能控性能观性检查，极点配置
co = ctrb(sys_open);
controllability = rank(co);
poles = [-5; -10; -5+10j; -5-10j];
K = place(A, B, poles);

%% 状态反馈闭环控制，绘制状态变化曲线
Ac = A-B*K; Bc = B;
Cc = C; Dc = D;
sys_closed = ss(Ac, Bc, Cc, Dc);
x0 = [-0.2; 0; pi/4; 0];
t = 0:0.01:1.5;
r = zeros(size(t));
y = lsim(sys_closed, r, t, x0);
figure(1); plot(t, y(:, 1:2), 'LineWidth', 1.5); grid on
legend('Cart position (m)', 'Pendulum angle (rad)');
title('Step Response with State-Feedback Control')

%% 倒立摆动画演示
im = cell(length(t), 1);
h = figure(2);
for k = 1:length(t)
    x = y(k, 1); theta = y(k, 2);
    bot_x = x; bot_y = 0;                           % 杆底坐标
    top_x = x + l*sin(theta); top_y = l*cos(theta); % 杆顶坐标
    plot(bot_x, bot_y, 'o', 'LineWidth', 1.5); hold on;
    plot([bot_x, top_x], [bot_y, top_y], 'LineWidth', 2); hold off;
    axis equal; axis([-0.5, 0.5, -0.10, 0.35]); grid on;
    title('Pendulum Animation')
    drawnow;
    frame = getframe(h);      % 记录该帧
    im{k} = frame2im(frame);
    pause(0.01);
end

%% GIF图片制作
filename = 'pendulum_animation.gif';
for idx = 1:length(t)
    [A, map] = rgb2ind(im{idx}, 256);
    if idx == 1
        imwrite(A, map, filename, 'gif', 'LoopCount', Inf, 'DelayTime', 1.00);
    else
        imwrite(A, map, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.01);
    end
end