MATLAB编写的机械臂控制仿真程序，它主要实现了对一个二连杆机械臂的运动控制仿真，比较了PID控制和非线性模型预测控制两种方法在机械臂轨迹跟踪任务中的性能

pk_xz1234562025-04-12 16:11
matlab 复制代码 
clc; clear; close all;

%% 机械臂参数
l1 = 0.5; l2 = 0.4; Ts = 0.02; sim_time = 60; t = 0:Ts:sim_time;

%% 物理参数
m1 = 1.0; m2 = 0.8; g = 9.81;

%% 直线轨迹参数
start_point = [0.3; 0.1]; end_point = [0.7; 0.3];
progress = t/sim_time;
xd = start_point(1) + (end_point(1) - start_point(1)) * progress;
yd = start_point(2) + (end_point(2) - start_point(2)) * progress;

%% 逆运动学计算
qd = zeros(2, length(t));
for k = 1:length(t)
    x = xd(k); y = yd(k);
    r = sqrt(x^2 + y^2);
    D = (r^2 - l1^2 - l2^2)/(2*l1*l2);
    if abs(D) > 1
        warning('步%d: 不可达位置，r=%.3f', k, r);
        qd(:,k) = qd(:,max(1,k-1));
        continue;
    end
    q2 = acos(D);
    q1 = atan2(y, x) - atan2(l2*sin(q2), l1 + l2*cos(q2));
    qd(:,k) = wrapToPi([q1; q2]);
end

%% PID参数
Kp = [150; 150]; Ki = [20; 20]; Kd = [25; 25]; tau_max = [30; 30];

%% PID控制
q_pid = qd(:,1); dq_pid = zeros(2, length(t)); 
e_int = zeros(2,1); tau_pid = zeros(2, length(t));
for i = 1:length(t)-1
    e = qd(:,i) - q_pid(:,i);
    e_int = e_int + e*Ts;
    if i > 1
        prev_error = qd(:,i-1) - q_pid(:,i-1);
        e_deriv = (e - prev_error)/Ts;
    else
        e_deriv = [0; 0];
    end
    tau = Kp.*e + Ki.*e_int + Kd.*e_deriv;
    tau_pid(:,i) = min(max(tau, -tau_max), tau_max);
    [M, C, G] = dynamics_model(q_pid(:,i), dq_pid(:,i), m1, m2, l1, l2, g);
    ddq = M \ (tau_pid(:,i) - C*dq_pid(:,i) - G);
    dq_pid(:,i+1) = dq_pid(:,i) + ddq*Ts;
    q_pid(:,i+1) = q_pid(:,i) + dq_pid(:,i+1)*Ts;
    q2_min = 0.2; q2_max = pi - 0.2;
    if q_pid(2,i+1) < q2_min
        q_pid(2,i+1) = q2_min; dq_pid(2,i+1) = 0;
    elseif q_pid(2,i+1) > q2_max
        q_pid(2,i+1) = q2_max; dq_pid(2,i+1) = 0;
    end
    q_pid(:,i+1) = wrapToPi(q_pid(:,i+1));
end

%% 非线性MPC控制
nlobj = nlmpc(4, 2, 2);
nlobj.Ts = Ts;
nlobj.PredictionHorizon = 10;
nlobj.ControlHorizon = 3;

nlobj.Model.StateFcn = @(x, u) stateFcn(x, u, m1, m2, l1, l2, g);
nlobj.Model.OutputFcn = @(x, u) x(1:2);

% 权重和约束（优化能耗）
nlobj.Weights.OutputVariables = [500 500];
nlobj.Weights.ManipulatedVariables = [2 2];
nlobj.Weights.ManipulatedVariablesRate = [0.5 0.5];
nlobj.MV(1).Min = -30; nlobj.MV(1).Max = 30;
nlobj.MV(2).Min = -30; nlobj.MV(2).Max = 30;

% 初始化状态
q_mpc = qd(:,1); dq_mpc = zeros(2, length(t));
tau_mpc = zeros(2, length(t));
x_mpc = [q_mpc; zeros(2,1)];
u_mpc = [0; 0];

% MPC主循环
tic;
for i = 1:length(t)-1
    ref = zeros(nlobj.PredictionHorizon, 2);
    for k = 1:nlobj.PredictionHorizon
        future_idx = min(i + k - 1, length(t));
        ref(k, :) = qd(:, future_idx)';
    end
    
    [tau_mpc(:,i), ~] = nlmpcmove(nlobj, x_mpc, u_mpc, ref);
    u_mpc = tau_mpc(:,i);
    
    [M, C, G] = dynamics_model(q_mpc(:,i), dq_mpc(:,i), m1, m2, l1, l2, g);
    if any(isnan(M(:))) || any(isinf(M(:))) || det(M) < 1e-6
        ddq = zeros(2,1);
    else
        ddq = M \ (tau_mpc(:,i) - C*dq_mpc(:,i) - G);
    end
    
    dq_mpc(:,i+1) = dq_mpc(:,i) + ddq*Ts;
    q_mpc(:,i+1) = q_mpc(:,i) + dq_mpc(:,i+1)*Ts;
    
    if any(~isfinite(q_mpc(:,i+1))) || any(~isfinite(dq_mpc(:,i+1)))
        q_mpc(:,i+1) = q_mpc(:,i);
        dq_mpc(:,i+1) = dq_mpc(:,i);
    end
    
    q2_min = 0.2; q2_max = pi - 0.2;
    if q_mpc(2,i+1) < q2_min
        q_mpc(2,i+1) = q2_min; dq_mpc(2,i+1) = 0;
    elseif q_mpc(2,i+1) > q2_max
        q_mpc(2,i+1) = q2_max; dq_mpc(2,i+1) = 0;
    end
    q_mpc(:,i+1) = wrapToPi(q_mpc(:,i+1));
    x_mpc = [q_mpc(:,i+1); dq_mpc(:,i+1)];
end
mpc_time = toc;
fprintf('MPC运行时间: %.2f 秒\n', mpc_time);

%% 性能指标计算
error_pid = sqrt(sum((qd - q_pid).^2, 1));
error_mpc = sqrt(sum((qd - q_mpc).^2, 1));
mean_error_pid = mean(error_pid);
mean_error_mpc = mean(error_mpc);
error_improvement = (mean_error_pid - mean_error_mpc) / mean_error_pid * 100;

threshold = 0.05;
settling_time_pid = find(error_pid < threshold, 1) * Ts;
if isempty(settling_time_pid), settling_time_pid = sim_time; end
settling_time_mpc = find(error_mpc < threshold, 1) * Ts;
if isempty(settling_time_mpc), settling_time_mpc = sim_time; end
speed_improvement = (settling_time_pid - settling_time_mpc) / settling_time_pid * 100;

energy_pid = sum(sum(tau_pid.^2)) * Ts;
energy_mpc = sum(sum(tau_mpc.^2)) * Ts;
energy_reduction = (energy_pid - energy_mpc) / energy_pid * 100;

fprintf('平均跟踪误差：PID = %.4f rad, MPC = %.4f rad, 提升 %.2f%%\n', ...
    mean_error_pid, mean_error_mpc, error_improvement);
fprintf('响应时间：PID = %.2f s, MPC = %.2f s, 提升 %.2f%%\n', ...
    settling_time_pid, settling_time_mpc, speed_improvement);
fprintf('能耗：PID = %.2f J, MPC = %.2f J, 降低 %.2f%%\n', ...
    energy_pid, energy_mpc, energy_reduction);

%% 原有可视化（角度和轨迹）
figure('Name','PID vs MPC控制结果');
subplot(3,1,1);
plot(t, qd(1,:), 'k--', t, q_pid(1,:), 'b', t, q_mpc(1,:), 'r');
title('关节1角度跟踪'); legend('期望', 'PID', 'MPC');
xlabel('时间 (s)'); ylabel('角度 (rad)');

subplot(3,1,2);
plot(t, qd(2,:), 'k--', t, q_pid(2,:), 'b', t, q_mpc(2,:), 'r');
title('关节2角度跟踪'); legend('期望', 'PID', 'MPC');
xlabel('时间 (s)'); ylabel('角度 (rad)');

x_act_pid = l1*cos(q_pid(1,:)) + l2*cos(q_pid(1,:)+q_pid(2,:));
y_act_pid = l1*sin(q_pid(1,:)) + l2*sin(q_pid(1,:)+q_pid(2,:));
x_act_mpc = l1*cos(q_mpc(1,:)) + l2*cos(q_mpc(1,:)+q_mpc(2,:));
y_act_mpc = l1*sin(q_mpc(1,:)) + l2*sin(q_mpc(1,:)+q_mpc(2,:));

subplot(3,1,3);
plot(xd, yd, 'k--', x_act_pid, y_act_pid, 'b', x_act_mpc, y_act_mpc, 'r');
axis equal; title('末端轨迹跟踪'); legend('期望', 'PID', 'MPC');
xlabel('X位置 (m)'); ylabel('Y位置 (m)');

%% 添加机械臂运动动画
figure('Name', '机械臂运动动画');
set(gcf, 'Position', [100, 100, 800, 400]);

% 计算关节位置
x1_pid = l1 * cos(q_pid(1,:));
y1_pid = l1 * sin(q_pid(1,:));
x2_pid = x1_pid + l2 * cos(q_pid(1,:) + q_pid(2,:));
y2_pid = y1_pid + l2 * sin(q_pid(1,:) + q_pid(2,:));

x1_mpc = l1 * cos(q_mpc(1,:));
y1_mpc = l1 * sin(q_mpc(1,:));
x2_mpc = x1_mpc + l2 * cos(q_mpc(1,:) + q_mpc(2,:));
y2_mpc = y1_mpc + l2 * sin(q_mpc(1,:) + q_mpc(2,:));

% 动画循环
for i = 1:10:length(t)  % 每10步绘制一次，加速动画
    clf;  % 清除当前图形
    
    % PID控制的机械臂（蓝色）
    subplot(1,2,1);
    plot([0, x1_pid(i)], [0, y1_pid(i)], 'b-', 'LineWidth', 2); hold on;
    plot([x1_pid(i), x2_pid(i)], [y1_pid(i), y2_pid(i)], 'b-', 'LineWidth', 2);
    plot(xd, yd, 'k--', 'LineWidth', 1);  % 期望轨迹
    plot(x2_pid(i), y2_pid(i), 'bo', 'MarkerSize', 5);  % 末端点
    axis equal; axis([-1 1 -1 1]);
    title('PID控制机械臂运动');
    xlabel('X位置 (m)'); ylabel('Y位置 (m)');
    grid on; hold off;
    
    % MPC控制的机械臂（红色）
    subplot(1,2,2);
    plot([0, x1_mpc(i)], [0, y1_mpc(i)], 'r-', 'LineWidth', 2); hold on;
    plot([x1_mpc(i), x2_mpc(i)], [y1_mpc(i), y2_mpc(i)], 'r-', 'LineWidth', 2);
    plot(xd, yd, 'k--', 'LineWidth', 1);  % 期望轨迹
    plot(x2_mpc(i), y2_mpc(i), 'ro', 'MarkerSize', 5);  % 末端点
    axis equal; axis([-1 1 -1 1]);
    title('MPC控制机械臂运动');
    xlabel('X位置 (m)'); ylabel('Y位置 (m)');
    grid on; hold off;
    
    drawnow;  % 刷新图形
    pause(0.01);  % 控制动画速度
end

%% 动力学模型
function [M, C, G] = dynamics_model(q, dq, m1, m2, l1, l2, g)
    M = [(m1 + m2)*l1^2 + m2*l2^2 + 2*m2*l1*l2*cos(q(2)), m2*l2^2 + m2*l1*l2*cos(q(2)); ...
         m2*l2^2 + m2*l1*l2*cos(q(2)), m2*l2^2];
    C = [-m2*l1*l2*sin(q(2))*dq(2), -m2*l1*l2*sin(q(2))*(dq(1) + dq(2)); ...
         m2*l1*l2*sin(q(2))*dq(1), 0];
    G = [(m1 + m2)*g*l1*cos(q(1)) + m2*g*l2*cos(q(1)+q(2)); m2*g*l2*cos(q(1)+q(2))];
end

%% 状态函数
function dx = stateFcn(x, u, m1, m2, l1, l2, g)
    q = x(1:2); dq = x(3:4);
    [M, C, G] = dynamics_model(q, dq, m1, m2, l1, l2, g);
    ddq = M \ (u - C*dq - G);
    dx = [dq; ddq];
end

%% 角度标准化
function angles = wrapToPi(angles)
    angles = mod(angles + pi, 2*pi) - pi;
end

上面的代码是一个用MATLAB编写的机械臂控制仿真程序，它主要实现了对一个二连杆机械臂的运动控制仿真，比较了PID控制和非线性模型预测控制（Nonlinear Model Predictive Control, NMPC）两种方法在机械臂轨迹跟踪任务中的性能。以下是对代码各部分功能的详细解释：

1. 初始化与参数设置

matlab 复制代码 
clc; clear; close all;

%% 机械臂参数
l1 = 0.5; l2 = 0.4; Ts = 0.02; sim_time = 60; t = 0:Ts:sim_time;

%% 物理参数
m1 = 1.0; m2 = 0.8; g = 9.81;

这部分代码进行了初始化操作，清除命令窗口、工作区变量并关闭所有图形窗口。同时设置了机械臂的连杆长度、采样时间、仿真时间等参数，以及机械臂连杆的质量和重力加速度。

2. 直线轨迹规划

matlab 复制代码 
%% 直线轨迹参数
start_point = [0.3; 0.1]; end_point = [0.7; 0.3];
progress = t/sim_time;
xd = start_point(1) + (end_point(1) - start_point(1)) * progress;
yd = start_point(2) + (end_point(2) - start_point(2)) * progress;

此部分代码规划了机械臂末端执行器的直线运动轨迹，根据起始点和终点的坐标，结合仿真时间计算出每个时刻的期望位置。

3. 逆运动学计算

matlab 复制代码 
%% 逆运动学计算
qd = zeros(2, length(t));
for k = 1:length(t)
    x = xd(k); y = yd(k);
    r = sqrt(x^2 + y^2);
    D = (r^2 - l1^2 - l2^2)/(2*l1*l2);
    if abs(D) > 1
        warning('步%d: 不可达位置，r=%.3f', k, r);
        qd(:,k) = qd(:,max(1,k-1));
        continue;
    end
    q2 = acos(D);
    q1 = atan2(y, x) - atan2(l2*sin(q2), l1 + l2*cos(q2));
    qd(:,k) = wrapToPi([q1; q2]);
end

通过逆运动学计算，将末端执行器的期望位置转换为机械臂各关节的期望角度。如果计算过程中出现不可达位置，会发出警告并使用上一时刻的关节角度。

4. PID控制

matlab 复制代码 
%% PID参数
Kp = [150; 150]; Ki = [20; 20]; Kd = [25; 25]; tau_max = [30; 30];

%% PID控制
q_pid = qd(:,1); dq_pid = zeros(2, length(t)); 
e_int = zeros(2,1); tau_pid = zeros(2, length(t));
for i = 1:length(t)-1
    e = qd(:,i) - q_pid(:,i);
    e_int = e_int + e*Ts;
    if i > 1
        prev_error = qd(:,i-1) - q_pid(:,i-1);
        e_deriv = (e - prev_error)/Ts;
    else
        e_deriv = [0; 0];
    end
    tau = Kp.*e + Ki.*e_int + Kd.*e_deriv;
    tau_pid(:,i) = min(max(tau, -tau_max), tau_max);
    [M, C, G] = dynamics_model(q_pid(:,i), dq_pid(:,i), m1, m2, l1, l2, g);
    ddq = M \ (tau_pid(:,i) - C*dq_pid(:,i) - G);
    dq_pid(:,i+1) = dq_pid(:,i) + ddq*Ts;
    q_pid(:,i+1) = q_pid(:,i) + dq_pid(:,i+1)*Ts;
    q2_min = 0.2; q2_max = pi - 0.2;
    if q_pid(2,i+1) < q2_min
        q_pid(2,i+1) = q2_min; dq_pid(2,i+1) = 0;
    elseif q_pid(2,i+1) > q2_max
        q_pid(2,i+1) = q2_max; dq_pid(2,i+1) = 0;
    end
    q_pid(:,i+1) = wrapToPi(q_pid(:,i+1));
end

这部分代码实现了PID控制器，根据关节角度的误差计算控制力矩，并通过机械臂的动力学模型更新关节角度和角速度。同时对关节角度进行了限制，避免超出合理范围。

5. 非线性MPC控制

matlab 复制代码 
%% 非线性MPC控制
nlobj = nlmpc(4, 2, 2);
nlobj.Ts = Ts;
nlobj.PredictionHorizon = 10;
nlobj.ControlHorizon = 3;

nlobj.Model.StateFcn = @(x, u) stateFcn(x, u, m1, m2, l1, l2, g);
nlobj.Model.OutputFcn = @(x, u) x(1:2);

% 权重和约束（优化能耗）
nlobj.Weights.OutputVariables = [500 500];
nlobj.Weights.ManipulatedVariables = [2 2];
nlobj.Weights.ManipulatedVariablesRate = [0.5 0.5];
nlobj.MV(1).Min = -30; nlobj.MV(1).Max = 30;
nlobj.MV(2).Min = -30; nlobj.MV(2).Max = 30;

% 初始化状态
q_mpc = qd(:,1); dq_mpc = zeros(2, length(t));
tau_mpc = zeros(2, length(t));
x_mpc = [q_mpc; zeros(2,1)];
u_mpc = [0; 0];

% MPC主循环
tic;
for i = 1:length(t)-1
    ref = zeros(nlobj.PredictionHorizon, 2);
    for k = 1:nlobj.PredictionHorizon
        future_idx = min(i + k - 1, length(t));
        ref(k, :) = qd(:, future_idx)';
    end
    
    [tau_mpc(:,i), ~] = nlmpcmove(nlobj, x_mpc, u_mpc, ref);
    u_mpc = tau_mpc(:,i);
    
    [M, C, G] = dynamics_model(q_mpc(:,i), dq_mpc(:,i), m1, m2, l1, l2, g);
    if any(isnan(M(:))) || any(isinf(M(:))) || det(M) < 1e-6
        ddq = zeros(2,1);
    else
        ddq = M \ (tau_mpc(:,i) - C*dq_mpc(:,i) - G);
    end
    
    dq_mpc(:,i+1) = dq_mpc(:,i) + ddq*Ts;
    q_mpc(:,i+1) = q_mpc(:,i) + dq_mpc(:,i+1)*Ts;
    
    if any(~isfinite(q_mpc(:,i+1))) || any(~isfinite(dq_mpc(:,i+1)))
        q_mpc(:,i+1) = q_mpc(:,i);
        dq_mpc(:,i+1) = dq_mpc(:,i);
    end
    
    q2_min = 0.2; q2_max = pi - 0.2;
    if q_mpc(2,i+1) < q2_min
        q_mpc(2,i+1) = q2_min; dq_mpc(2,i+1) = 0;
    elseif q_mpc(2,i+1) > q2_max
        q_mpc(2,i+1) = q2_max; dq_mpc(2,i+1) = 0;
    end
    q_mpc(:,i+1) = wrapToPi(q_mpc(:,i+1));
    x_mpc = [q_mpc(:,i+1); dq_mpc(:,i+1)];
end
mpc_time = toc;
fprintf('MPC运行时间: %.2f 秒\n', mpc_time);

此部分代码实现了非线性模型预测控制（NMPC），通过预测未来一段时间内的系统状态，优化控制输入以最小化目标函数。同时记录了MPC控制器的运行时间。

6. 性能指标计算

matlab 复制代码 
%% 性能指标计算
error_pid = sqrt(sum((qd - q_pid).^2, 1));
error_mpc = sqrt(sum((qd - q_mpc).^2, 1));
mean_error_pid = mean(error_pid);
mean_error_mpc = mean(error_mpc);
error_improvement = (mean_error_pid - mean_error_mpc) / mean_error_pid * 100;

threshold = 0.05;
settling_time_pid = find(error_pid < threshold, 1) * Ts;
if isempty(settling_time_pid), settling_time_pid = sim_time; end
settling_time_mpc = find(error_mpc < threshold, 1) * Ts;
if isempty(settling_time_mpc), settling_time_mpc = sim_time; end
speed_improvement = (settling_time_pid - settling_time_mpc) / settling_time_pid * 100;

energy_pid = sum(sum(tau_pid.^2)) * Ts;
energy_mpc = sum(sum(tau_mpc.^2)) * Ts;
energy_reduction = (energy_pid - energy_mpc) / energy_pid * 100;

fprintf('平均跟踪误差：PID = %.4f rad, MPC = %.4f rad, 提升 %.2f%%\n', ...
    mean_error_pid, mean_error_mpc, error_improvement);
fprintf('响应时间：PID = %.2f s, MPC = %.2f s, 提升 %.2f%%\n', ...
    settling_time_pid, settling_time_mpc, speed_improvement);
fprintf('能耗：PID = %.2f J, MPC = %.2f J, 降低 %.2f%%\n', ...
    energy_pid, energy_mpc, energy_reduction);

计算了PID控制和NMPC控制的平均跟踪误差、响应时间和能耗，并比较了两种控制方法的性能提升。

7. 可视化

matlab 复制代码 
%% 原有可视化（角度和轨迹）
figure('Name','PID vs MPC控制结果');
subplot(3,1,1);
plot(t, qd(1,:), 'k--', t, q_pid(1,:), 'b', t, q_mpc(1,:), 'r');
title('关节1角度跟踪'); legend('期望', 'PID', 'MPC');
xlabel('时间 (s)'); ylabel('角度 (rad)');

subplot(3,1,2);
plot(t, qd(2,:), 'k--', t, q_pid(2,:), 'b', t, q_mpc(2,:), 'r');
title('关节2角度跟踪'); legend('期望', 'PID', 'MPC');
xlabel('时间 (s)'); ylabel('角度 (rad)');

x_act_pid = l1*cos(q_pid(1,:)) + l2*cos(q_pid(1,:)+q_pid(2,:));
y_act_pid = l1*sin(q_pid(1,:)) + l2*sin(q_pid(1,:)+q_pid(2,:));
x_act_mpc = l1*cos(q_mpc(1,:)) + l2*cos(q_mpc(1,:)+q_mpc(2,:));
y_act_mpc = l1*sin(q_mpc(1,:)) + l2*sin(q_mpc(1,:)+q_mpc(2,:));

subplot(3,1,3);
plot(xd, yd, 'k--', x_act_pid, y_act_pid, 'b', x_act_mpc, y_act_mpc, 'r');
axis equal; title('末端轨迹跟踪'); legend('期望', 'PID', 'MPC');
xlabel('X位置 (m)'); ylabel('Y位置 (m)');

%% 添加机械臂运动动画
figure('Name', '机械臂运动动画');
set(gcf, 'Position', [100, 100, 800, 400]);

% 计算关节位置
x1_pid = l1 * cos(q_pid(1,:));
y1_pid = l1 * sin(q_pid(1,:));
x2_pid = x1_pid + l2 * cos(q_pid(1,:) + q_pid(2,:));
y2_pid = y1_pid + l2 * sin(q_pid(1,:) + q_pid(2,:));

x1_mpc = l1 * cos(q_mpc(1,:));
y1_mpc = l1 * sin(q_mpc(1,:));
x2_mpc = x1_mpc + l2 * cos(q_mpc(1,:) + q_mpc(2,:));
y2_mpc = y1_mpc + l2 * sin(q_mpc(1,:) + q_mpc(2,:));

% 动画循环
for i = 1:10:length(t)  % 每10步绘制一次，加速动画
    clf;  % 清除当前图形
    
    % PID控制的机械臂（蓝色）
    subplot(1,2,1);
    plot([0, x1_pid(i)], [0, y1_pid(i)], 'b-', 'LineWidth', 2); hold on;
    plot([x1_pid(i), x2_pid(i)], [y1_pid(i), y2_pid(i)], 'b-', 'LineWidth', 2);
    plot(xd, yd, 'k--', 'LineWidth', 1);  % 期望轨迹
    plot(x2_pid(i), y2_pid(i), 'bo', 'MarkerSize', 5);  % 末端点
    axis equal; axis([-1 1 -1 1]);
    title('PID控制机械臂运动');
    xlabel('X位置 (m)'); ylabel('Y位置 (m)');
    grid on; hold off;
    
    % MPC控制的机械臂（红色）
    subplot(1,2,2);
    plot([0, x1_mpc(i)], [0, y1_mpc(i)], 'r-', 'LineWidth', 2); hold on;
    plot([x1_mpc(i), x2_mpc(i)], [y1_mpc(i), y2_mpc(i)], 'r-', 'LineWidth', 2);
    plot(xd, yd, 'k--', 'LineWidth', 1);  % 期望轨迹
    plot(x2_mpc(i), y2_mpc(i), 'ro', 'MarkerSize', 5);  % 末端点
    axis equal; axis([-1 1 -1 1]);
    title('MPC控制机械臂运动');
    xlabel('X位置 (m)'); ylabel('Y位置 (m)');
    grid on; hold off;
    
    drawnow;  % 刷新图形
    pause(0.01);  % 控制动画速度
end

通过绘制图形和动画，直观地展示了两种控制方法下机械臂各关节角度的跟踪情况、末端执行器的轨迹跟踪情况以及机械臂的运动过程。

8. 辅助函数

matlab 复制代码 
%% 动力学模型
function [M, C, G] = dynamics_model(q, dq, m1, m2, l1, l2, g)
    M = [(m1 + m2)*l1^2 + m2*l2^2 + 2*m2*l1*l2*cos(q(2)), m2*l2^2 + m2*l1*l2*cos(q(2)); ...
         m2*l2^2 + m2*l1*l2*cos(q(2)), m2*l2^2];
    C = [-m2*l1*l2*sin(q(2))*dq(2), -m2*l1*l2*sin(q(2))*(dq(1) + dq(2)); ...
         m2*l1*l2*sin(q(2))*dq(1), 0];
    G = [(m1 + m2)*g*l1*cos(q(1)) + m2*g*l2*cos(q(1)+q(2)); m2*g*l2*cos(q(1)+q(2))];
end

%% 状态函数
function dx = stateFcn(x, u, m1, m2, l1, l2, g)
    q = x(1:2); dq = x(3:4);
    [M, C, G] = dynamics_model(q, dq, m1, m2, l1, l2, g);
    ddq = M \ (u - C*dq - G);
    dx = [dq; ddq];
end

%% 角度标准化
function angles = wrapToPi(angles)
    angles = mod(angles + pi, 2*pi) - pi;
end

定义了机械臂的动力学模型、状态函数和角度标准化函数，用于计算机械臂的动力学参数、更新系统状态和将角度限制在[-π, π]范围内。

综上所述，这段代码通过仿真比较了PID控制和NMPC控制在机械臂轨迹跟踪任务中的性能，为实际应用中选择合适的控制方法提供了参考。

相关推荐
晴天毕设工作室6 分钟前
计算机毕业设计指南
java·开发语言·python·计算机网络·课程设计
jhtwn9 分钟前
Java NIO之Buffer
java·开发语言
沐墨专攻技术10 分钟前
顺序表专题（C语言）
c语言·开发语言·数据结构·顺序表
爱学习的大牛12314 分钟前
flutter 获取通话记录和通讯录
开发语言·flutter
有梦想的鱼1 小时前
创建虚拟环境无法加载到pycharm当conda环境，只能为python环境
开发语言·python
请叫我欧皇i1 小时前
vue2使用ezuikit-js播放萤石视频
开发语言·javascript·ecmascript
IT专家-大狗2 小时前
Google Chrome Canary版官方下载及安装教程【适用于开发者与进阶用户】
开发语言·javascript·chrome·ecmascript
martian6653 小时前
100道C#高频经典面试题带解析答案——全面C#知识点总结
开发语言·c#
电子连接器CAE与高频分析3 小时前
matlab求和∑函数方程编程？
开发语言·算法·matlab
热门推荐
01我决定放弃搞 Java 了02RAG 实践- Ollama+RagFlow 部署本地知识库03从零安装 LLaMA-Factory 微调 Qwen 大模型成功及所有的坑04Vue3 + TypeScript 的 Hooks 实用示例05DeepSeek RAGFlow构建本地知识库系统06如何在WPS和Word/Excel中直接使用DeepSeek功能07【WRF理论第十三期】详细介绍 Registry 的作用、结构和内容08汽车上的各种质量：整备质量、总质量、装载质量、簧上质量、簧下质量09DeepSeek各版本说明与优缺点分析10快刷每日面经😋