MATLAB仿真一级倒立摆程序

一级倒立摆系统数学模型

系统参数与变量

符号 含义 典型值/单位
\(M\) 小车质量 1.0 kg
\(m\) 摆杆质量 0.1 kg
\(l\) 摆杆长度 0.5 m
\(I\) 摆杆转动惯量 ( \(\frac{1}{12}ml^2\) )
\(b\) 小车摩擦系数 0.1 N·s/m
\(g\) 重力加速度 9.81 m/s²
$x $ 小车位置 m
\(\theta\) 摆杆角度（垂直向上为0） rad
$F $ 施加给小车的力 N

非线性动力学方程

通过拉格朗日法推导：

\(\begin{aligned} &(M+m)\ddot{x} + b\dot{x} + ml\ddot{\theta}\cos\theta - ml\dot{\theta}^2\sin\theta = F \\ &(I+ml^2)\ddot{\theta} + mgl\sin\theta + ml\ddot{x}\cos\theta = 0 \end{aligned}\)

线性化模型（在平衡点 (\theta \approx 0)）

\(\begin{aligned} &\ddot{x} = \frac{-(I+ml^2)b}{p}\dot{x} + \frac{m^2gl^2}{p}\theta + \frac{I+ml^2}{p}F \\ &\ddot{\theta} = \frac{-mlb}{p}\dot{x} + \frac{mgl(M+m)}{p}\theta + \frac{ml}{p}F \end{aligned}\)

其中 \(p = (I+ml^2)(M+m) - m^2l^2\)

完整MATLAB仿真程序

1. 主仿真脚本

%% 一级倒立摆系统MATLAB仿真主程序
clear; clc; close all;

%% 1. 系统参数设置
params.M = 1.0;      % 小车质量 (kg)
params.m = 0.1;      % 摆杆质量 (kg)
params.l = 0.5;      % 摆杆长度 (m)
params.g = 9.81;     % 重力加速度 (m/s^2)
params.b = 0.1;      % 小车摩擦系数 (N·s/m)
params.I = params.m * params.l^2 / 12;  % 摆杆转动惯量

fprintf('========== 一级倒立摆仿真参数 ==========\n');
fprintf('小车质量 M = %.2f kg\n', params.M);
fprintf('摆杆质量 m = %.2f kg\n', params.m);
fprintf('摆杆长度 l = %.2f m\n', params.l);
fprintf('摩擦系数 b = %.2f N·s/m\n', params.b);
fprintf('=======================================\n\n');

%% 2. 线性化状态空间模型
[A, B, C, D] = linearize_inverted_pendulum(params);

% 显示系统特性
fprintf('系统特征值:\n');
eig_val = eig(A);
disp(eig_val);

% 检查能控性
Co = ctrb(A, B);
rank_Co = rank(Co);
if rank_Co == size(A, 1)
    fprintf('系统完全能控 (能控性秩 = %d)\n', rank_Co);
else
    fprintf('系统不完全能控 (能控性秩 = %d)\n', rank_Co);
end

%% 3. 控制器设计
% 状态向量: x = [小车位置; 小车速度; 摆杆角度; 摆杆角速度]
%            = [x; dx/dt; theta; dtheta/dt]

% 3.1 LQR控制器设计
Q = diag([10, 1, 100, 1]);  % 状态权重矩阵
R = 0.1;                    % 控制输入权重

[K_lqr, S, E] = lqr(A, B, Q, R);
fprintf('\nLQR控制器增益 K = [');
fprintf('%.2f ', K_lqr);
fprintf(']\n');

% 3.2 PID控制器参数（用于对比）
Kp_theta = 50;   % 角度比例增益
Ki_theta = 5;    % 角度积分增益
Kd_theta = 10;   % 角度微分增益

Kp_x = 2;        % 位置比例增益
Ki_x = 0.1;      % 位置积分增益
Kd_x = 5;        % 位置微分增益

%% 4. 非线性系统仿真
% 4.1 初始条件
x0 = [0; 0; deg2rad(10); 0];  % 初始角度10度

% 4.2 仿真时间
tspan = [0 10];  % 仿真10秒

% 4.3 选择控制器类型
controller_type = 'LQR';  % 可选 'LQR' 或 'PID'

% 4.4 运行仿真
fprintf('\n开始非线性仿真...\n');
options = odeset('RelTol', 1e-6, 'AbsTol', 1e-9);
[t_nl, x_nl] = ode45(@(t,x) inverted_pendulum_dynamics(t, x, params, K_lqr, controller_type), tspan, x0, options);

% 提取状态变量
x_pos = x_nl(:, 1);      % 小车位置
x_vel = x_nl(:, 2);      % 小车速度
theta = rad2deg(x_nl(:, 3));  % 摆杆角度(度)
theta_vel = rad2deg(x_nl(:, 4)); % 摆杆角速度(度/秒)

%% 5. 线性系统仿真（用于对比）
fprintf('开始线性仿真...\n');
% 闭环系统矩阵
A_cl = A - B * K_lqr;
sys_cl = ss(A_cl, zeros(4,1), eye(4), zeros(4,1));
[Y_lin, T_lin, X_lin] = initial(sys_cl, x0, tspan(2));

%% 6. 可视化结果
figure('Position', [100, 100, 1200, 800]);

% 6.1 状态响应对比
subplot(3, 2, 1);
plot(t_nl, x_pos, 'b-', 'LineWidth', 2);
hold on;
plot(T_lin, X_lin(:,1), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('时间 (s)');
ylabel('小车位置 (m)');
title('小车位置响应');
legend('非线性模型', '线性模型');
xlim([0 tspan(2)]);

subplot(3, 2, 2);
plot(t_nl, theta, 'b-', 'LineWidth', 2);
hold on;
plot(T_lin, rad2deg(X_lin(:,3)), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('时间 (s)');
ylabel('摆杆角度 (°)');
title('摆杆角度响应');
legend('非线性模型', '线性模型');
xlim([0 tspan(2)]);
ylim([-15 15]);

% 6.2 相平面图
subplot(3, 2, 3);
plot(theta, theta_vel, 'b-', 'LineWidth', 1.5);
hold on;
plot(rad2deg(X_lin(:,3)), rad2deg(X_lin(:,4)), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('角度 θ (°)');
ylabel('角速度 dθ/dt (°/s)');
title('相平面图 (角度 vs 角速度)');
legend('非线性模型', '线性模型');

% 6.3 控制力
subplot(3, 2, 4);
% 计算控制力
F_control = zeros(length(t_nl), 1);
for i = 1:length(t_nl)
    if strcmp(controller_type, 'LQR')
        F_control(i) = -K_lqr * x_nl(i, :)';
    else
        % PID控制力计算
        F_control(i) = calculate_pid_control(x_nl(i, :)', t_nl(i), Kp_theta, Ki_theta, Kd_theta, Kp_x, Ki_x, Kd_x);
    end
end
plot(t_nl, F_control, 'b-', 'LineWidth', 2);
grid on;
xlabel('时间 (s)');
ylabel('控制力 F (N)');
title('控制力变化');
xlim([0 tspan(2)]);

% 6.4 能量分析
subplot(3, 2, 5);
% 计算系统总能量
E_total = zeros(length(t_nl), 1);
for i = 1:length(t_nl)
    E_total(i) = calculate_total_energy(x_nl(i, :)', params);
end
plot(t_nl, E_total, 'b-', 'LineWidth', 2);
grid on;
xlabel('时间 (s)');
ylabel('总能量 (J)');
title('系统总能量变化');
xlim([0 tspan(2)]);

% 6.5 动画演示按钮
subplot(3, 2, 6);
axis off;
text(0.1, 0.7, '仿真控制选项:', 'FontSize', 12, 'FontWeight', 'bold');
uicontrol('Style', 'pushbutton', 'String', '播放动画', ...
    'Position', [50 100 100 30], ...
    'Callback', @(src,event) animate_pendulum(t_nl, x_nl, params));

%% 7. 性能指标计算
fprintf('\n========== 性能指标 ==========\n');
% 调节时间 (2%误差带)
theta_final = theta(end);
theta_settled = find(abs(theta - theta_final) > 0.02*abs(theta(1)-theta_final), 1, 'last');
if ~isempty(theta_settled)
    settling_time = t_nl(theta_settled);
    fprintf('调节时间 (2%%): %.3f s\n', settling_time);
else
    fprintf('调节时间 (2%%): < %.3f s\n', t_nl(1));
end

% 超调量
[theta_peak, idx_peak] = max(abs(theta));
overshoot = (theta_peak - abs(theta_final)) / abs(theta(1)) * 100;
fprintf('角度超调量: %.2f%%\n', overshoot);

% 控制力统计
fprintf('最大控制力: %.3f N\n', max(abs(F_control)));
fprintf('平均控制力: %.3f N\n', mean(abs(F_control)));

%% 8. 鲁棒性分析
fprintf('\n========== 鲁棒性分析 ==========\n');
% 参数变化分析
param_variations = 0.8:0.1:1.2;  % 参数变化范围
stability_margin = zeros(length(param_variations), 1);

for i = 1:length(param_variations)
    params_test = params;
    params_test.m = params.m * param_variations(i);  % 改变摆杆质量
    [A_test, B_test] = linearize_inverted_pendulum(params_test);
    A_cl_test = A_test - B_test * K_lqr;
    eig_test = max(real(eig(A_cl_test)));
    stability_margin(i) = -eig_test;  % 稳定裕度
end

figure('Name', '参数鲁棒性分析');
plot(param_variations, stability_margin, 'bo-', 'LineWidth', 2);
grid on;
xlabel('参数变化系数');
ylabel('稳定裕度');
title('控制器对摆杆质量变化的鲁棒性');

2. 核心函数定义

%% 线性化模型函数
function [A, B, C, D] = linearize_inverted_pendulum(params)
    % 一级倒立摆线性化状态空间模型
    % 状态: x = [位置; 速度; 角度; 角速度]
    
    M = params.M;
    m = params.m;
    l = params.l;
    g = params.g;
    b = params.b;
    I = params.I;
    
    % 计算中间变量
    p = (I + m*l^2)*(M + m) - (m*l)^2;
    
    % 状态空间矩阵 A
    A = zeros(4, 4);
    A(1, 2) = 1;
    A(2, 1) = 0;
    A(2, 2) = -(I + m*l^2)*b / p;
    A(2, 3) = (m^2 * g * l^2) / p;
    
    A(3, 4) = 1;
    A(4, 1) = 0;
    A(4, 2) = -(m*l*b) / p;
    A(4, 3) = (m*g*l*(M + m)) / p;
    
    % 输入矩阵 B
    B = zeros(4, 1);
    B(2, 1) = (I + m*l^2) / p;
    B(4, 1) = (m*l) / p;
    
    % 输出矩阵 C (全状态输出)
    C = eye(4);
    
    % 直接传输矩阵 D
    D = zeros(4, 1);
end

%% 非线性动力学方程
function dxdt = inverted_pendulum_dynamics(t, x, params, K, controller_type)
    % 一级倒立摆非线性动力学方程
    % x: 状态向量 [位置; 速度; 角度; 角速度]
    
    % 提取参数
    M = params.M;
    m = params.m;
    l = params.l;
    g = params.g;
    b = params.b;
    I = params.I;
    
    % 提取状态变量
    x_pos = x(1);       % 小车位置
    x_dot = x(2);       % 小车速度
    theta = x(3);       % 摆杆角度
    theta_dot = x(4);   % 摆杆角速度
    
    % 根据控制器类型计算控制力
    if strcmp(controller_type, 'LQR')
        % LQR控制
        F = -K * x;
    else
        % PID控制
        F = calculate_pid_control(x, t, 50, 5, 10, 2, 0.1, 5);
    end
    
    % 非线性动力学方程
    denom = (M + m)*(I + m*l^2) - (m*l*cos(theta))^2;
    
    dxdt = zeros(4, 1);
    
    % 小车加速度
    dxdt(1) = x_dot;
    dxdt(2) = (F - b*x_dot + m*l*theta_dot^2*sin(theta) ...
              - m*g*l*sin(theta)*cos(theta)/(l)) / denom * (I + m*l^2);
    
    % 摆杆角加速度
    dxdt(3) = theta_dot;
    dxdt(4) = (m*l*cos(theta)*(F - b*x_dot) ...
              + (M + m)*m*g*l*sin(theta) ...
              - (m*l)^2*theta_dot^2*sin(theta)*cos(theta)) / (denom*l);
end

%% PID控制器函数
function F = calculate_pid_control(x, t, Kp_theta, Ki_theta, Kd_theta, Kp_x, Ki_x, Kd_x)
    % PID控制器计算
    persistent integral_theta integral_x prev_theta_error prev_x_error prev_t
    
    % 初始化持久变量
    if isempty(integral_theta) || t == 0
        integral_theta = 0;
        integral_x = 0;
        prev_theta_error = 0;
        prev_x_error = 0;
        prev_t = 0;
    end
    
    % 期望值
    theta_desired = 0;      % 希望摆杆垂直向上
    x_desired = 0;          % 希望小车在原点
    
    % 当前状态
    x_pos = x(1);
    theta = x(3);
    
    % 计算误差
    theta_error = theta_desired - theta;
    x_error = x_desired - x_pos;
    
    % 积分项更新
    dt = t - prev_t;
    if dt > 0
        integral_theta = integral_theta + theta_error * dt;
        integral_x = integral_x + x_error * dt;
    end
    
    % 微分项
    if dt > 0
        dtheta_error = (theta_error - prev_theta_error) / dt;
        dx_error = (x_error - prev_x_error) / dt;
    else
        dtheta_error = 0;
        dx_error = 0;
    end
    
    % PID控制力
    F_theta = Kp_theta * theta_error + Ki_theta * integral_theta + Kd_theta * dtheta_error;
    F_x = Kp_x * x_error + Ki_x * integral_x + Kd_x * dx_error;
    
    % 总控制力
    F = F_theta + F_x;
    
    % 控制力限幅
    F_max = 20;  % 最大控制力
    if F > F_max
        F = F_max;
    elseif F < -F_max
        F = -F_max;
    end
    
    % 更新持久变量
    prev_theta_error = theta_error;
    prev_x_error = x_error;
    prev_t = t;
end

%% 能量计算函数
function E = calculate_total_energy(x, params)
    % 计算系统总能量
    m = params.m;
    M = params.M;
    l = params.l;
    g = params.g;
    
    x_pos = x(1);
    x_dot = x(2);
    theta = x(3);
    theta_dot = x(4);
    
    % 动能
    T = 0.5 * M * x_dot^2 + ...
        0.5 * m * (x_dot^2 + l^2 * theta_dot^2 + 2 * x_dot * l * theta_dot * cos(theta));
    
    % 势能
    V = m * g * l * (1 - cos(theta));
    
    % 总能量
    E = T + V;
end

%% 动画演示函数
function animate_pendulum(t, x, params)
    % 创建动画窗口
    fig_anim = figure('Name', '倒立摆动画演示', 'Position', [200, 200, 800, 600]);
    
    % 设置动画参数
    l = params.l;
    cart_width = 0.2;
    cart_height = 0.1;
    
    % 动画循环
    for i = 1:10:length(t)
        clf;
        
        % 小车位置和摆杆角度
        x_pos = x(i, 1);
        theta = x(i, 3);
        
        % 计算摆杆端点坐标
        pendulum_x = x_pos + l * sin(theta);
        pendulum_y = l * cos(theta);
        
        % 绘制轨道
        plot([-2, 2], [0, 0], 'k-', 'LineWidth', 2);
        hold on;
        
        % 绘制小车
        rectangle('Position', [x_pos - cart_width/2, -cart_height/2, cart_width, cart_height], ...
                 'FaceColor', [0.2, 0.6, 0.8], 'EdgeColor', 'k', 'LineWidth', 2);
        
        % 绘制摆杆
        plot([x_pos, pendulum_x], [0, pendulum_y], 'r-', 'LineWidth', 3);
        
        % 绘制摆锤
        plot(pendulum_x, pendulum_y, 'ko', 'MarkerSize', 15, 'MarkerFaceColor', 'r');
        
        % 设置图形属性
        axis equal;
        xlim([-2, 2]);
        ylim([-0.5, 1.5]);
        grid on;
        title(sprintf('一级倒立摆动画 (t = %.2f s)', t(i)));
        xlabel('位置 (m)');
        ylabel('高度 (m)');
        
        % 显示状态信息
        text(-1.8, 1.3, sprintf('小车位置: %.3f m', x_pos));
        text(-1.8, 1.2, sprintf('摆杆角度: %.1f°', rad2deg(theta)));
        
        drawnow;
        
        % 控制动画速度
        pause(0.01);
    end
end

3. Simulink模型（可选）

%% 创建Simulink模型（如果在MATLAB中运行）
function create_simulink_model()
    % 创建新的Simulink模型
    model_name = 'InvertedPendulum_Simulink';
    new_system(model_name);
    open_system(model_name);
    
    % 添加模块
    % 1. 状态空间模块
    add_block('simulink/Continuous/State-Space', [model_name '/Pendulum Model']);
    
    % 2. LQR控制器
    add_block('simulink/Math Operations/Gain', [model_name '/LQR Gain']);
    
    % 3. 示波器
    add_block('simulink/Sinks/Scope', [model_name '/Scope']);
    
    % 4. 输入源
    add_block('simulink/Sources/Step', [model_name '/Step Input']);
    
    % 连接模块
    add_line(model_name, 'Step Input/1', 'LQR Gain/1');
    add_line(model_name, 'LQR Gain/1', 'Pendulum Model/1');
    add_line(model_name, 'Pendulum Model/1', 'Scope/1');
    
    % 保存模型
    save_system(model_name);
end

仿真结果分析要点

控制器性能对比

控制器类型 优点 缺点 适用场景
LQR 全局最优，理论保证稳定 需要精确模型，参数整定复杂 精确模型已知的系统
PID 简单易实现，鲁棒性好 多变量耦合处理困难 初步调试和教学演示
滑模控制 强鲁棒性，抗干扰 存在抖振现象 存在参数不确定性和干扰

参数整定建议

  1. LQR权重矩阵：

    • 加大Q(3,3)（角度权重）→ 摆杆更快稳定
    • 加大R值 → 减小控制力，但响应变慢
    • 典型初值：\(Q = diag([1, 0.1, 10, 0.1]), R = 0.1\)

  2. PID参数：

    • 先调角度环(\(Kp_theta\))，再调位置环(\(Kp_x\))
    • 积分项防止稳态误差，但可能引起振荡
    • 微分项抑制超调，但对噪声敏感

参考代码 一级倒立摆MATLAB仿真程序 www.3dddown.com/cnb/97769.html

扩展功能建议

  1. 鲁棒控制：添加参数不确定性分析
  2. 自适应控制：在线调整控制器参数
  3. 神经网络控制：使用深度学习补偿非线性
  4. 硬件在环：连接实际倒立摆硬件
  5. 能量控制：基于能量整形的控制器设计
posted @ 2026-01-27 16:47  alloutlove  阅读(2)  评论(0)    收藏  举报