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

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) > 1warning('步%d: 不可达位置，r=%.3f', k, r);qd(:,k) = qd(:,max(1,k-1));continue;endq2 = 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)-1e = qd(:,i) - q_pid(:,i);e_int = e_int + e*Ts;if i > 1prev_error = qd(:,i-1) - q_pid(:,i-1);e_deriv = (e - prev_error)/Ts;elsee_deriv = [0; 0];endtau = 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_minq_pid(2,i+1) = q2_min; dq_pid(2,i+1) = 0;elseif q_pid(2,i+1) > q2_maxq_pid(2,i+1) = q2_max; dq_pid(2,i+1) = 0;endq_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)-1ref = zeros(nlobj.PredictionHorizon, 2);for k = 1:nlobj.PredictionHorizonfuture_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-6ddq = zeros(2,1);elseddq = M \ (tau_mpc(:,i) - C*dq_mpc(:,i) - G);enddq_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);endq2_min = 0.2; q2_max = pi - 0.2;if q_mpc(2,i+1) < q2_minq_mpc(2,i+1) = q2_min; dq_mpc(2,i+1) = 0;elseif q_mpc(2,i+1) > q2_maxq_mpc(2,i+1) = q2_max; dq_mpc(2,i+1) = 0;endq_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. 初始化与参数设置

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. 直线轨迹规划

%% 直线轨迹参数
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. 逆运动学计算

%% 逆运动学计算
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) > 1warning('步%d: 不可达位置，r=%.3f', k, r);qd(:,k) = qd(:,max(1,k-1));continue;endq2 = acos(D);q1 = atan2(y, x) - atan2(l2*sin(q2), l1 + l2*cos(q2));qd(:,k) = wrapToPi([q1; q2]);
end

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

4. PID控制

%% 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)-1e = qd(:,i) - q_pid(:,i);e_int = e_int + e*Ts;if i > 1prev_error = qd(:,i-1) - q_pid(:,i-1);e_deriv = (e - prev_error)/Ts;elsee_deriv = [0; 0];endtau = 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_minq_pid(2,i+1) = q2_min; dq_pid(2,i+1) = 0;elseif q_pid(2,i+1) > q2_maxq_pid(2,i+1) = q2_max; dq_pid(2,i+1) = 0;endq_pid(:,i+1) = wrapToPi(q_pid(:,i+1));
end

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

5. 非线性MPC控制

%% 非线性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)-1ref = zeros(nlobj.PredictionHorizon, 2);for k = 1:nlobj.PredictionHorizonfuture_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-6ddq = zeros(2,1);elseddq = M \ (tau_mpc(:,i) - C*dq_mpc(:,i) - G);enddq_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);endq2_min = 0.2; q2_max = pi - 0.2;if q_mpc(2,i+1) < q2_minq_mpc(2,i+1) = q2_min; dq_mpc(2,i+1) = 0;elseif q_mpc(2,i+1) > q2_maxq_mpc(2,i+1) = q2_max; dq_mpc(2,i+1) = 0;endq_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. 性能指标计算

%% 性能指标计算
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. 可视化

%% 原有可视化（角度和轨迹）
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. 辅助函数

%% 动力学模型
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控制在机械臂轨迹跟踪任务中的性能，为实际应用中选择合适的控制方法提供了参考。