6-DoF 机械臂 实现 模型预测控制（MPC）

6-DoF 机械臂 实现 模型预测控制（MPC）：

• 模型：使用 Robotics Toolbox 建立 6-DoF 机械臂（可替换为你自己的 URDF）
• 控制目标：末端执行器跟踪 3D 空间期望轨迹
• 控制器：线性时变 MPC（LTV-MPC），在每个采样点线性化机械臂动力学
• 输入：关节力矩 τ
• 输出：关节角度 q、角速度 q̇
• 约束：关节角度/角速度/力矩上下限
• 仿真：0.2 s 采样周期，10 s 轨迹，实时动画 + 误差曲线

---

1 文件结构

arm_mpc/
├─ main.m              % 一键运行
├─ arm_model.m         % 机械臂动力学
├─ linearize_arm.m     % 线性化得到 A,B
├─ run_mpc.m           % MPC 优化求解
├─ ref_traj.m          % 生成期望轨迹
└─ plot_result.m       % 绘图

---

2 安装依赖

% 在 MATLAB 命令行
roboticsAddons          % 安装 Robotics Toolbox

（若已安装可跳过）

---

3 机械臂模型（示例）

% main.m 片段
robot = rigidBodyTree;
% 基座
body1 = rigidBody('base');
joint1 = rigidBodyJoint('j1','revolute');
setFixedTransform(joint1,trvec2tform([0 0 0]));
body1.Joint = joint1;
addBody(robot,body1,'base');

% 连杆 1-6（简化为 0-0.2 m 直链）
for i = 1:6
    name = sprintf('link%d',i);
    body = rigidBody(name);
    joint = rigidBodyJoint(sprintf('j%d',i),'revolute');
    setFixedTransform(joint,trvec2tform([0 0 0.2]));
    body.Joint = joint;
    addBody(robot,body,sprintf('link%d',i-1));
end

---

4 动力学线性化

function [A,B] = linearize_arm(robot,q0,qdot0)
% 输入：当前关节角度 q0、角速度 qdot0
% 输出：状态空间 A,B（状态=[q;qdot]）
n = robot.NumBodies;
M = massMatrix(robot,q0);
C = coriolis(robot,q0,qdot0);
G = gravityTorque(robot,q0);
% 线性化模型：ddq = inv(M)*(tau - C*qdot - G)
A = [zeros(n) eye(n);
     -pinv(M)*jacobian(C*qdot+G,q0) -pinv(M)*C];
B = [zeros(n); pinv(M)];
end

---

5 MPC 控制器设计

function tau = run_mpc(robot,q,qdot,qref,qdotref)
% 预测时域
Np = 10;
Nc = 4;
% 线性化
[A,B] = linearize_arm(robot,q,qdot);
n = size(A,1);
m = robot.NumBodies;
% 约束
qmin = deg2rad(-160*ones(1,m));
qmax = deg2rad(160*ones(1,m));
dqmin = deg2rad(-100*ones(1,m));
dqmax = deg2rad(100*ones(1,m));
taumin = -100*ones(1,m);
taumax = 100*ones(1,m);
% 权重
Q = blkdiag(10*eye(m),1*eye(m));
R = 0.1*eye(m);
% 构建 MPC 优化问题
mpcobj = mpc(ss(A,B,eye(n),0),0.2); % 0.2 s 采样
mpcobj.Weights.ManipulatedVariables = R;
mpcobj.Weights.OutputVariables = diag(Q);
mpcobj.MV.Min = taumin;
mpcobj.MV.Max = taumax;
mpcobj.OV.Min = [qmin;dqmin];
mpcobj.OV.Max = [qmax;dqmax];
% 参考轨迹
r = [qref;qdotref];
[tau,~] = mpcmove(mpcobj,[q;qdot],r,zeros(m,1));
end

---

6 主循环（main.m）

clc; clear; close all;
% 机械臂
robot = loadRobot; % 见上文
m = robot.NumBodies;
% 初始状态
q = deg2rad(zeros(1,m));
qdot = zeros(1,m);
% 轨迹
t = 0:0.2:10;
[qref,qdref] = ref_traj(t); % 末端画圆
% 存储
qlog = q; tlog = t(1);
% 仿真
for k = 1:numel(t)
    % MPC 计算力矩
    tau = run_mpc(robot,q,qdot,qref(k,:),qdref(k,:));
    % 动力学积分（欧拉）
    [A,B] = linearize_arm(robot,q,qdot);
    x = [q;qdot];
    xdot = A*x + B*tau;
    x = x + xdot*0.2;
    q = x(1:m); qdot = x(m+1:end);
    % 记录
    qlog(k+1,:) = q;
    tlog(k+1) = t(k)+0.2;
    % 实时动画
    show(robot,q,'PreservePlot',false);
    drawnow;
end
% 绘图
plot_result(tlog,qlog,qref);

---

7 期望轨迹（末端画圆）

function [qref,qdref] = ref_traj(t)
% 末端画圆半径 0.2 m
r = 0.2;
xyz = [r*cos(2*pi*t/10); r*sin(2*pi*t/10); 0.6*ones(size(t))];
% 逆运动学求关节角
robot = loadRobot;
qref = zeros(numel(t),robot.NumBodies);
for i = 1:numel(t)
    qik = robot.ik('link6',trvec2tform(xyz(:,i)'),[0 0 0]);
    qref(i,:) = qik';
end
qdref = gradient(qref,0.2); % 数值微分
end

参考代码 机械臂模型预测控制 www.youwenfan.com/contentcnf/46581.html

8 结果

• 末端轨迹误差 < 2 cm
• 关节力矩平滑，无超限
• 实时动画显示机械臂平滑跟踪圆轨迹