二阶双连杆机械臂的非线性反馈控制的MATLAB仿真程序

原创于 2026-06-26 17:37:58 发布 · 154 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#matlab #开发语言

二阶双连杆机械臂的非线性反馈控制（计算力矩控制）的MATLAB仿真程序：

%% 二阶双连杆机械臂非线性反馈控制（计算力矩控制）
clear; clc; close all;

%% 机械臂参数
m1 = 1.0;   % 连杆1质量 (kg)
m2 = 1.0;   % 连杆2质量 (kg)
l1 = 1.0;   % 连杆1长度 (m)
l2 = 1.0;   % 连杆2长度 (m)
lc1 = 0.5;  % 连杆1质心位置 (m)
lc2 = 0.5;  % 连杆2质心位置 (m)
I1 = 0.12;  % 连杆1转动惯量 (kg·m²)
I2 = 0.12;  % 连杆2转动惯量 (kg·m²)
g = 9.81;   % 重力加速度 (m/s²)

% 初始状态
q0 = [pi/6; pi/4];      % 初始关节角度 (rad)
qd0 = [0; 0];           % 初始关节角速度 (rad/s)

%% 控制器参数
Kp = diag([50, 50]);    % 位置增益矩阵
Kd = diag([20, 20]);    % 速度增益矩阵

%% 时间设置
tspan = [0 10];         % 仿真时间 (s)
dt = 0.01;              % 时间步长
t = tspan(1):dt:tspan(2);

%% 期望轨迹（示例：正弦轨迹）
qd_des = @(t) [pi/3*sin(0.5*t); pi/4*cos(0.5*t)];          % 期望角度
qd_des_dot = @(t) [pi/6*0.5*cos(0.5*t); -pi/4*0.5*sin(0.5*t)]; % 期望角速度
qd_des_ddot = @(t) [-pi/12*0.25*sin(0.5*t); -pi/8*0.25*cos(0.5*t)]; % 期望角加速度

%% 初始化存储数组
n = length(t);
q_history = zeros(2, n);
qd_history = zeros(2, n);
tau_history = zeros(2, n);
q_error_history = zeros(2, n);
qd_error_history = zeros(2, n);

%% 初始状态
q = q0;
qd = qd0;

%% 主循环
for i = 1:n
    % 当前时间
    ti = t(i);
    
    % 期望轨迹及其导数
    q_des = qd_des(ti);
    q_des_dot = qd_des_dot(ti);
    q_des_ddot = qd_des_ddot(ti);
    
    % 误差计算
    q_error = q_des - q;
    qd_error = q_des_dot - qd;
    
    % 计算动力学项
    [M, C, G] = manipulator_dynamics(q, qd, m1, m2, l1, l2, lc1, lc2, I1, I2, g);
    
    % 非线性反馈控制律（计算力矩控制）
    v = q_des_ddot + Kd*qd_error + Kp*q_error;  % 辅助控制输入
    tau = M*v + C*qd + G;                       % 控制力矩
    
    % 存储数据
    q_history(:, i) = q;
    qd_history(:, i) = qd;
    tau_history(:, i) = tau;
    q_error_history(:, i) = q_error;
    qd_error_history(:, i) = qd_error;
    
    % 数值积分（欧拉法）
    if i < n
        % 计算加速度
        qdd = M \ (tau - C*qd - G);
        
        % 更新状态
        qd = qd + qdd * dt;
        q = q + qd * dt;
    end
end

%% 绘图
figure('Color', 'white', 'Position', [100, 100, 1200, 800])

% 1. 关节角度跟踪
subplot(3, 2, 1)
plot(t, q_history(1, :), 'b-', 'LineWidth', 2)
hold on
plot(t, qd_des(t), 'r--', 'LineWidth', 1.5)
ylabel('Joint 1 Angle (rad)')
legend('Actual', 'Desired')
title('Joint 1 Angle Tracking')
grid on

subplot(3, 2, 2)
plot(t, q_history(2, :), 'b-', 'LineWidth', 2)
hold on
plot(t, qd_des(t), 'r--', 'LineWidth', 1.5)
ylabel('Joint 2 Angle (rad)')
legend('Actual', 'Desired')
title('Joint 2 Angle Tracking')
grid on

% 2. 关节角速度跟踪
subplot(3, 2, 3)
plot(t, qd_history(1, :), 'b-', 'LineWidth', 2)
hold on
plot(t, qd_des_dot(t), 'r--', 'LineWidth', 1.5)
ylabel('Joint 1 Velocity (rad/s)')
legend('Actual', 'Desired')
title('Joint 1 Velocity Tracking')
grid on

subplot(3, 2, 4)
plot(t, qd_history(2, :), 'b-', 'LineWidth', 2)
hold on
plot(t, qd_des_dot(t), 'r--', 'LineWidth', 1.5)
ylabel('Joint 2 Velocity (rad/s)')
legend('Actual', 'Desired')
title('Joint 2 Velocity Tracking')
grid on

% 3. 控制输入
subplot(3, 2, 5)
plot(t, tau_history(1, :), 'b-', 'LineWidth', 2)
ylabel('Torque 1 (Nm)')
xlabel('Time (s)')
title('Control Input - Joint 1')
grid on

subplot(3, 2, 6)
plot(t, tau_history(2, :), 'b-', 'LineWidth', 2)
ylabel('Torque 2 (Nm)')
xlabel('Time (s)')
title('Control Input - Joint 2')
grid on

sgtitle('Nonlinear Feedback Control of 2-DOF Manipulator', 'FontSize', 14, 'FontWeight', 'bold')

%% 相位图
figure('Color', 'white', 'Position', [200, 200, 800, 400])
subplot(1, 2, 1)
plot(q_history(1, :), qd_history(1, :), 'b-', 'LineWidth', 1.5)
xlabel('Angle (rad)')
ylabel('Angular Velocity (rad/s)')
title('Phase Portrait - Joint 1')
grid on

subplot(1, 2, 2)
plot(q_history(2, :), qd_history(2, :), 'b-', 'LineWidth', 1.5)
xlabel('Angle (rad)')
ylabel('Angular Velocity (rad/s)')
title('Phase Portrait - Joint 2')
grid on

%% 误差分析
figure('Color', 'white', 'Position', [300, 300, 800, 400])
subplot(1, 2, 1)
plot(t, q_error_history(1, :), 'r-', 'LineWidth', 1.5)
hold on
plot(t, q_error_history(2, :), 'b-', 'LineWidth', 1.5)
xlabel('Time (s)')
ylabel('Position Error (rad)')
legend('Joint 1', 'Joint 2')
title('Position Tracking Error')
grid on

subplot(1, 2, 2)
plot(t, qd_error_history(1, :), 'r-', 'LineWidth', 1.5)
hold on
plot(t, qd_error_history(2, :), 'b-', 'LineWidth', 1.5)
xlabel('Time (s)')
ylabel('Velocity Error (rad/s)')
legend('Joint 1', 'Joint 2')
title('Velocity Tracking Error')
grid on

%% 机械臂动力学函数
function [M, C, G] = manipulator_dynamics(q, qd, m1, m2, l1, l2, lc1, lc2, I1, I2, g)
    % 提取关节角度
    q1 = q(1);
    q2 = q(2);
    q1d = qd(1);
    q2d = qd(2);
    
    % 惯性矩阵 M(q)
    M11 = I1 + I2 + m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*cos(q2));
    M12 = I2 + m2*(lc2^2 + l1*lc2*cos(q2));
    M21 = M12;
    M22 = I2 + m2*lc2^2;
    M = [M11, M12; M21, M22];
    
    % 科里奥利力和离心力矩阵 C(q, qd)
    h = -m2*l1*lc2*sin(q2);
    C11 = h*q2d;
    C12 = h*(q1d + q2d);
    C21 = -h*q1d;
    C22 = 0;
    C = [C11, C12; C21, C22];
    
    % 重力向量 G(q)
    G1 = (m1*lc1 + m2*l1)*g*cos(q1) + m2*lc2*g*cos(q1 + q2);
    G2 = m2*lc2*g*cos(q1 + q2);
    G = [G1; G2];
end

程序说明：

1. 控制原理

采用计算力矩控制（Computed Torque Control），这是非线性反馈控制的典型实现
控制律：τ = M(q)v + C(q,q̇)q̇ + G(q)
其中 v = q̈_d + K_d(q̇_d - q̇) + K_p(q_d - q)

2. 主要特点

精确线性化：通过前馈补偿非线性项，将系统转化为双重积分器
PD控制结构：包含位置和速度反馈
全状态反馈：需要测量关节位置和速度

3. 参数调整建议

% 增大Kp提高响应速度但可能引起超调
Kp = diag([50, 50]);    

% 增大Kd提高阻尼，减少振荡
Kd = diag([20, 20]);    

% 临界阻尼条件：Kd ≈ 2*sqrt(Kp)

4. 扩展功能

添加扰动观测器：

% 在控制律中添加扰动估计
L = diag([10, 10]);  % 观测器增益
dq_hat = zeros(2,1); % 扰动估计
dq_hat_dot = -L*dq_hat + L*(qd - qd_history(:,i-1)/dt);
tau_dist = M*dq_hat; % 扰动补偿
tau = M*v + C*qd + G + tau_dist;

改用ODE求解器（更精确）：

% 定义状态方程
dynamics = @(t, x) manipulator_ode(t, x, tau_func, params);
[t_ode, x_ode] = ode45(dynamics, tspan, [q0; qd0]);

function dx = manipulator_ode(t, x, tau_func, params)
    q = x(1:2);
    qd = x(3:4);
    [M, C, G] = manipulator_dynamics(q, qd, params{:});
    tau = tau_func(t, q, qd);
    qdd = M \ (tau - C*qd - G);
    dx = [qd; qdd];
end