简介本资源是一套面向机器人控制初学者与高校自动化/机电专业学生的MATLAB实践代码包聚焦二关节机械臂的PD阻抗控制原理实现与工程验证。通过完整动力学建模含前向运动学、逆动力学、PD控制器设计、虚拟阻抗环境构建及闭环仿真帮助学习者深入理解机器人控制中误差反馈、刚度-阻尼调节、力/位置混合控制等核心概念。压缩包共12个文件449KB含4个核心MATLAB函数.m实现系统建模、参考轨迹生成、控制器计算与结果可视化1个Simulink模型.mdl支持交互仿真5张关键仿真图.jpg直观展示末端轨迹、位置跟踪、控制力矩及力响应效果另配PDF与DOCX双版本程序详解文档逐行注释关键公式、变量物理意义与调试要点。目前已有103人学习下载结构清晰、注释详尽、即开即跑是掌握机器人经典控制方法不可多得的入门级实操范例。 做机械臂控制绕不开柔顺这个话题而PD阻抗控制又是柔顺控制里门槛最低、最容易落地验证的一类方案。这篇文章我把一套二关节机械臂PD阻抗控制的MATLAB仿真源码完整梳理出来代码里的注释写得很细从模型矩阵到控制律每一行都讲清楚适合正在做机器人控制课设、研究生入门或者工作中需要快速验证控制算法的朋友参考。内容上我会先把模型公式和控制原理拆开讲明白再给完整可运行源码最后聊聊我在调试过程中踩过的几个坑。先说清楚这套仿真解决了什么问题给定一条关节空间期望轨迹让二关节机械臂在PD阻抗控制律下跟踪这条轨迹同时在仿真中人为加入外部力矩扰动观察系统对外力的柔顺响应。换句话说它既能展示常规的位置跟踪能力也能展示阻抗控制“遇到外力不硬顶、撤掉外力后恢复”的核心特性。项目只需要MATLAB环境不需要Simulink全部用脚本和函数实现改起来非常方便。1. 项目整体思路与核心原理拆解1.1 这个项目到底做了什么整个项目围绕一个平面二关节机械臂展开。机械臂有两个旋转关节一个在肩部一个在肘部运动范围限制在平面内。我选择二关节而不是单关节是因为二关节已经能体现绝大多数机械臂动力学特征惯性矩阵随构型变化、存在科氏力和离心力、重力项与关节角度强相关。如果只拿单关节做很多动力学耦合问题根本暴露不出来这样的源码参考价值会大打折扣。仿真流程是这样组织的先定义机械臂的物理参数和质量分布再定义期望轨迹函数然后把控制律和动力学方程一起写进状态方程函数交给MATLAB的ode45做数值积分最后把关节角度、速度、误差和控制力矩画出来。整个过程没有引入复杂的实时环境也不需要特殊的工具箱纯手写函数非常便于理解内部机制。控制方案上我采用的是“计算力矩PD阻抗”结构。所谓计算力矩是指控制器内部用机械臂模型前馈掉惯量、科氏力和重力所谓PD阻抗是指在关节空间对跟踪误差施加比例和微分增益形成一个等效的弹簧阻尼系统。两者合在一起之后闭环误差动力学是一个标准的二阶系统用户可以通过调节Kp和Kd来直接设定机械臂对外表现出的“刚度”和“阻尼”。1.2 为什么选择PD阻抗控制机械臂在工业场景里传统上以位置控制为主要求的是“指哪打哪”控制器非常硬。但一旦机械臂需要与人协作、与环境接触、或者在未知空间里探索纯位置控制就出问题了位置误差稍有偏差就会产生很大的接触力轻则损坏工件重则伤人。于是柔顺控制出现其中阻抗控制是影响最深的一类思路。阻抗控制的核心思想很直白就是不让机械臂末端硬邦邦地跟踪位置而是给机械臂设定一个目标动态特性。这个动态特性用质量-弹簧-阻尼系统来描述当外部有力作用时机械臂会像被弹簧拉着一样偏离期望位置而不是硬顶回去外部力撤销后机械臂又会像阻尼系统回稳一样逐渐回到期望位置。这样的交互方式对人和环境都友好很多。PD控制恰好就是阻抗控制在关节空间的一种直接实现方式。比例项对应弹簧项微分项对应阻尼项。如果控制器里再补偿掉重力等模型项理想情况下关节角误差方程就是M_de_ddot Kde_dot Kp*e 0这就是一个标准的二阶阻抗关系。所以“PD阻抗控制”这个叫法没有毛病它不是两个东西拼接而是同一个控制律从两个角度去理解一个角度是经典PID位置控制另一个角度是阻抗行为塑造。1.3 控制方案的整体框架整套方案的信号流可以概括为期望轨迹生成器输出q_des、qd_des、qdd_des送入阻抗控制器控制器结合机械臂当前状态q和qd计算出参考加速度a再乘上惯性矩阵并加上C*dq和G得到控制力矩tau这个tau作用到机械臂动力学方程上再加上外部扰动tau_ext得到真实的关节加速度qddqdd积分两次得到新的状态进入下一轮循环。这里最关键的一点是参考加速度a的构造方式。a取的是期望加速度qdd_des加上误差修正项修正项包括比例误差和微分误差并且由期望惯量矩阵Md来缩放。写成公式就是a qdd_des Md^{-1}(Kdedot Kpe)。控制力矩则取tau Ma Cdq G。这样设计的好处是在模型完全精确的前提下机械臂真实加速度就等于参考加速度闭环误差方程变成e_ddot Md^{-1}Kdedot Md^{-1}Kpe 0阻抗的动态特性完全由参数Md、Kp、Kd决定和机械臂本身参数无关。这就叫阻抗成型。2. 二关节机械臂动力学模型推导与关键矩阵意义2.1 平面2R机械臂的M、C、G矩阵二关节机械臂的动力学方程是标准形式M(q)*qdd C(q,dq)*dq G(q) tau。M是惯量矩阵C是科氏力和离心力矩阵G是重力项tau是关节驱动力矩。写代码前必须先把这三个矩阵的解析表达式拿准否则控制器和仿真模型不一致算法再漂亮也算不出来。惯量矩阵M是一个2x2对称矩阵它的元素依赖于当前关节角度尤其是q2。M11是第一个关节轴上的总等效惯量M12和M21是两关节之间的耦合惯量M22是第二个关节的惯量。这个矩阵不是常数机械臂构型变化时惯性感知会明显不同这也是为什么不能用简单线性系统去套机械臂的原因。科氏力矩阵C的写法比较讲究。物理上它代表运动时产生的速度和速度耦合项比如关节2转动时会对关节1产生一个交互力矩。C矩阵的表达式并不唯一但是必须保证M_dot - 2C是斜对称矩阵这个结构性质这个性质在很多控制律稳定性证明里都会用到。我代码里采用的是经典配置h -m2l1r2sin(q2)C [hdq2, h*(dq1dq2); -h*dq1, 0]这个写法是教科书版本稳定性分析最友好。重力项G的物理意义是各关节在重力场中维持当前姿态所需的力矩。G(1)包含连杆1自身重力矩加上连杆2对关节1的重力矩G(2)是连杆2对关节2的重力矩都依赖cos(q1)和cos(q1q2)。正因为重力项是角度强函数仿真里有重力前馈和没重力前馈的结果会差很多这一点后面我会专门讲。2.2 two_link_matrices函数实现细节模型矩阵我单独写在two_link_matrices.m文件里这样做的好处是main.m、robot_dynamics.m都能调用避免重复复制公式也方便以后换成三关节或者SCARA机械臂时只改这一个文件。代码里我加了一个小优化r1和r2表示质心到关节轴线的距离I1和I2是绕质心的转动惯量。为了让模型更直观我直接让质心在连杆中点转动惯量按细杆绕质心的公式m*l^2/12计算。实际机械臂的质心和惯量一般从CAD模型或辨识实验获取换成真实机械臂时只需要把这三个参数替换掉即可控制律一行都不用改。需要提一下的是我在M矩阵里写了完整的表达式M11 I1 I2 m1r1^2 m2(l1^2 r2^2 2l1r2cos(q2))。这个2l1r2cos(q2)项就是两关节耦合的根源。如果拿掉它就变成了两个独立单摆模型那仿真就失去意义了。实际调试时我还会特意输出一下M矩阵在各个构型下的数值确认它始终正定对称这也是判断模型写没写错的一个有效手段。2.3 为什么控制器里必须有重力前馈很多新手做PD控制仿真时直接在关节上施加tau Kpe Kdedot觉得效果还行。这是因为仿真初始位置和期望位置离得近或者期望轨迹变化缓慢重力被PD的积分或者较大的比例增益硬抗住了。但这种做法有隐患最直接的表现是存在稳态误差甚至机械臂在低速大负载场景下会直接坠落。我在这套代码里把C*dq和G都放进控制力矩里实现的是前馈补偿。就算只有PD项加重力前馈轨迹跟踪的稳态误差也是零因为重力项在控制律里被精确抵消了。你可能觉得“既然模型精确才能抵消那模型不准怎么办”这个问题真实存在工业上会再叠加上自适应、扰动观测器或者学习项来处理。但作为学习阻抗控制的起点先把模型已知的理想情况吃透再往非理想情况扩路径更稳。3. PD阻抗控制器设计3.1 控制律的深入拆解控制器核心就在robot_dynamics.m里的几行代码。第一步取期望轨迹和实际状态的误差e q_des - qedot qd_des - qd。第二步构造参考加速度a qdd_des Md \ (Kdedot Kpe)。这里用左除而不是求逆数值稳定性更好。第三步算控制力矩tau Ma Cqd G。猛一看这就是计算力矩控制但关键在Md参数上。如果我让Md等于单位阵那么a qdd_des Kdedot Kpe这就是教科书版PD前馈。如果我让Md取其他值a里对误差修正的等效增益就变成Md^{-1}*Kp而不是Kp本身。所以期望惯量其实在调节“同样位置误差下关节能产生多大修正加速度”它改变了整个闭环系统的质量感。真实阻抗控制里期望惯量一般会取得比真实惯量小一些让机械臂在被推动时感觉更轻。我在这套参数里把Md设成了diag([2 2])单位是kg*m^2Kp是diag([160 120])Kd是diag([25.3 21.9])。你可以算一下两个关节通道的等效阻尼比大概在0.7左右既有一定的快速性又不会明显超调。这个参数组合不是随手填的是我调了几轮之后觉得跟踪误差和扰动恢复速度都比较均衡详细推导放在3.2节。3.2 期望惯量、刚度、阻尼参数如何整定阻抗控制的参数整定核心是搞明白每个参数影响什么。Kp决定接触刚度Kp越大机械臂对外力越“硬”位置恢复能力越强Kd决定接触阻尼Kd越大能量耗散越快振荡衰减越快但Kd过大会让控制反应迟钝Md则决定机械臂表现出的等效惯量Md越小机械臂在外力下越容易被推动柔顺感越强。参数整定有个基础方法先把Md固定比如都取1然后按二阶系统设计Kp和Kd。期望闭环自然频率w_n sqrt(Kp/Md)阻尼比zeta Kd/(2sqrt(MdKp))。如果你希望系统像临界阻尼一样不振荡就取Kd 2sqrt(MdKp)。拿第1关节举例Kp取160Md取2则w_n sqrt(80)约8.94 rad/s响应速度还算可以如果取Kd 2sqrt(2160)约35.8就是临界阻尼但实际我取25.3阻尼比约0.707响应更快一点会带一点轻微超调这在轨迹跟踪里通常可以接受。实际调整顺序我建议是先把Md定下来再用w_n确定Kp最后用阻尼比确定Kd。仿真里观察误差曲线如果振荡明显就增大Kd如果恢复太慢就增大Kp如果对外力太硬就调小Kp或者增大Md。这个顺序比盲目乱调高效得多。3.3 在代码中加入外部扰动来验证阻抗特性如果只做轨迹跟踪仿真PD阻抗控制和普通PD控制看起来没区别完全体现不出阻抗的柔顺特点。所以我特意在仿真到第4秒到第6秒之间给关节2施加了一个大小为3N*m的外部力矩扰动。你想象一下相当于有人在第4秒开始推了机械臂一下持续2秒后松开。在阻抗控制下机械臂的响应应该是扰动出现瞬间关节2会偏离期望轨迹偏离量近似等于扰动除以刚度参数而不是被强行拉回原位扰动持续期间机械臂维持在这个偏离状态附近表现出“让开”而不是“硬顶”扰动撤掉后机械臂在阻尼作用下逐步回到期望轨迹。这个动态过程就是阻抗行为最直观的验证。代码里实现很简单就是在动力学方程右侧加上tau_extif语句判断时间窗口就行。4. 完整源码与运行结果4.1 main.m完整源码下面是主程序完整代码注释我已经写得比较细直接复制到MATLAB里就能跑。%% main.m —— 二关节机械臂PD阻抗控制仿真入口 % 功能实现关节空间PD阻抗控制输出轨迹跟踪曲线和力矩曲线 % 运行直接运行本文件依赖 robot_dynamics.m 和 two_link_matrices.m % 作者xxx % 时间2025 clear; clc; close all; %% 1. 机械臂模型参数平面2R机械臂 params.l1 1.0; % 连杆1长度 [m] params.l2 1.0; % 连杆2长度 [m] params.m1 2.0; % 连杆1质量 [kg] params.m2 1.5; % 连杆2质量 [kg] params.r1 params.l1 / 2; % 连杆1质心位置 [m] params.r2 params.l2 / 2; % 连杆2质心位置 [m] params.I1 params.m1 * params.l1^2 / 12; % 连杆1绕质心惯量 [kg*m^2] params.I2 params.m2 * params.l2^2 / 12; % 连杆2绕质心惯量 [kg*m^2] params.g 9.81; % 重力加速度 [m/s^2] %% 2. PD阻抗控制器参数 Md diag([2 2]); % 期望惯量矩阵kg*m^2 Kp diag([160 120]); % 期望刚度矩阵N*m/rad Kd diag([25.3 21.9]); % 期望阻尼矩阵N*m*s/rad %% 3. 期望轨迹关节空间 q_des_fun (t) [0.8*sin(0.8*t); 1.2*cos(0.6*t)]; qd_des_fun (t) [0.64*cos(0.8*t); -0.72*sin(0.6*t)]; qdd_des_fun (t) [-0.512*sin(0.8*t); -0.432*cos(0.6*t)]; %% 4. 初始状态与仿真时长 x0 [q_des_fun(0); qd_des_fun(0)]; % [q1; q2; qd1; qd2] tspan [0 10]; % 仿真时间 [s] %% 5. 调用ode45求解 odeopt odeset(RelTol, 1e-6, AbsTol, 1e-8); [t, X] ode45((t,x) robot_dynamics(t, x, params, q_des_fun, qd_des_fun, qdd_des_fun, Md, Kp, Kd), ... tspan, x0, odeopt); %% 6. 整理仿真结果 q X(:, 1:2); % 关节角度 qd X(:, 3:4); % 关节速度 % 计算期望轨迹用于画图 q_des zeros(length(t), 2); qd_des zeros(length(t), 2); for k 1:length(t) q_des(k, :) q_des_fun(t(k)); qd_des(k, :) qd_des_fun(t(k)); end % 重新计算控制力矩用于画图 tau_log zeros(length(t), 2); for k 1:length(t) q_k q(k, :); qd_k qd(k, :); [M_k, C_k, G_k] two_link_matrices(q_k, qd_k, params); e_k q_des(k, :) - q_k; edot_k qd_des(k, :) - qd_k; a_k qdd_des_fun(t(k)) Md \ (Kd*edot_k Kp*e_k); tau_log(k, :) (M_k*a_k C_k*qd_k G_k); end %% 7. 绘图 figure(Color, w, Position, [100 100 1100 750]); subplot(3,2,1); plot(t, q(:,1), b-, LineWidth, 1.5); hold on; plot(t, q_des(:,1), r--, LineWidth, 1.2); xlabel(t/s); ylabel(q1/rad); title(关节1角度跟踪); legend(实际, 期望, Location, best); grid on; subplot(3,2,2); plot(t, q(:,2), b-, LineWidth, 1.5); hold on; plot(t, q_des(:,2), r--, LineWidth, 1.2); xlabel(t/s); ylabel(q2/rad); title(关节2角度跟踪); legend(实际, 期望, Location, best); grid on; subplot(3,2,3); plot(t, q(:,1)-q_des(:,1), LineWidth, 1.5); xlabel(t/s); ylabel(误差/rad); title(关节1跟踪误差); grid on; subplot(3,2,4); plot(t, q(:,2)-q_des(:,2), LineWidth, 1.5); xlabel(t/s); ylabel(误差/rad); title(关节2跟踪误差); grid on; subplot(3,2,5); plot(t, tau_log(:,1), LineWidth, 1.5); xlabel(t/s); ylabel(tau1/N*m); title(关节1控制力矩); grid on; subplot(3,2,6); plot(t, tau_log(:,2), LineWidth, 1.5); xlabel(t/s); ylabel(tau2/N*m); title(关节2控制力矩); grid on; sgtitle(二关节机械臂PD阻抗控制仿真结果);4.2 robot_dynamics.m完整源码这是ode45的核心函数机械臂真实动力学、控制律、外部扰动全部集成在这里。%% robot_dynamics.m —— 二关节机械臂状态方程与控制律 % 输入: % t : 当前时间 % x : 状态向量 [q1; q2; qd1; qd2] % params: 机械臂参数结构体 % q_des_fun, qd_des_fun, qdd_des_fun: 期望轨迹函数句柄 % Md, Kp, Kd: 阻抗参数矩阵 % 输出: % dx : 状态导数 [qd1; qd2; qdd1; qdd2] function dx robot_dynamics(t, x, params, q_des_fun, qd_des_fun, qdd_des_fun, Md, Kp, Kd) % 拆解状态向量 q x(1:2); % 当前关节角 qd x(3:4); % 当前关节角速度 % 1. 计算机械臂模型矩阵 M, C, G [M, C, G] two_link_matrices(q, qd, params); % 2. 计算期望轨迹在当前时刻的值 q_des q_des_fun(t); qd_des qd_des_fun(t); qdd_des qdd_des_fun(t); % 3. PD阻抗控制律 e q_des - q; % 位置误差 edot qd_des - qd; % 速度误差 a qdd_des Md \ (Kd*edot Kp*e); % 参考加速度 tau M*a C*qd G; % 控制力矩计算力矩形式 % 4. 外部扰动用于验证阻抗柔顺性 tau_ext [0; 0]; if t 4 t 6 tau_ext(2) 3.0; % 4~6s 在关节2施加 3N*m 力矩扰动 end % 5. 机械臂真实动力学 % M*qdd C*qd G tau tau_ext % 因此 qdd M \ (tau - C*qd - G tau_ext) % 由于控制器里用了 tau M*a C*qd G % 代入后 qdd a M \ tau_ext说明外部扰动会直接反映到加速度上 qdd M \ (tau - C*qd - G tau_ext); % 返回状态导数 dx [qd; qdd]; end4.3 two_link_matrices.m完整源码%% two_link_matrices.m —— 二关节机械臂模型矩阵计算 % 输入: % q : 关节角 [q1; q2] % dq : 关节角速度 [dq1; dq2] % params: 机械臂参数结构体 % 输出: % M : 惯性矩阵 2x2 % C : 科氏力/离心力矩阵 2x2 % G : 重力向量 2x1 function [M, C, G] two_link_matrices(q, dq, params) % 从状态和参数中取出所需变量 q1 q(1); q2 q(2); dq1 dq(1); dq2 dq(2); l1 params.l1; l2 params.l2; m1 params.m1; m2 params.m2; r1 params.r1; r2 params.r2; I1 params.I1; I2 params.I2; g params.g; % 惯性矩阵 M(q) % 注意 M12 M21矩阵对称且正定 M11 I1 I2 m1*r1^2 m2*(l1^2 r2^2 2*l1*r2*cos(q2)); M12 I2 m2*(r2^2 l1*r2*cos(q2)); M21 M12; M22 I2 m2*r2^2; M [M11 M12; M21 M22]; % 科氏力/离心力矩阵 C(q, dq) % 这里采用经典形式满足 M_dot - 2C 斜对称性质 h -m2*l1*r2*sin(q2); C [h*dq2, h*(dq1dq2); -h*dq1, 0]; % 重力向量 G(q) G [(m1*r1 m2*l1)*g*cos(q1) m2*r2*g*cos(q1q2); m2*r2*g*cos(q1q2)]; end4.4 仿真结果怎么解读运行完代码后你会看到六张图。前两张是关节角跟踪红虚线是期望轨迹蓝实线是实际轨迹。正常情况下两条线几乎重合但在第4秒到第6秒之间第二张图蓝色线会有一个明显的小“鼓包”这就是外部扰动导致关节2偏离期望轨迹然后再逐渐回到红虚线上这个“鼓包再回落”的过程就是阻抗柔顺性的直接体现。第三张和第四张是跟踪误差图最能说明问题。仿真前4秒误差基本维持在很小的量级说明跟踪性能是好的。第4秒开始关节2误差曲线上出现一个尖峰这个尖峰的高度由扰动大小和系统刚度共同决定扰动是3N*m关节2通道等效刚度Kp(2,2)120所以稳态偏离量大概在0.025弧度左右也就是约1.4度这和仿真曲线吻合。扰动结束后误差曲线以阻尼振荡的方式快速回到零。第五张和第六张是控制力矩图你会看到力矩在第4秒到第6秒之间也出现变化。这是因为控制器在“主动抵抗”扰动同时还要维持跟踪所以力矩会有一个调整过程。这个力矩波动幅度是评估执行器需求的重要指标如果你换到真实机械臂上这个数值一定要控制在驱动器允许范围以内。4.5 改哪些地方可以快速迁移到自己的项目很多读者拿源码过来不是为了跑着玩而是想改一改用到自己的课题或者项目里。我列几个最常见的改动点。换期望轨迹最方便只需要改main.m里的三个匿名函数。比如你想让机械臂跟踪阶跃信号就把q_des_fun改成(t)[pi/6; pi/4]注意初始状态x0也要改成对应的角度不然一开始误差过大轨迹会有一段剧烈调整想跟踪五次多项式轨迹就写一个函数返回位置、速度、加速度三个值。换扰动方式也很方便想模拟碰撞接触就把robot_dynamics.m里的tau_ext改成和位置相关的力模型比如弹簧接触力。想模拟持续负载就在时间窗口外加一个常值。需要特别说明的是如果扰动过大或者刚度过高仿真时间步长可能要减小否则数值积分容易出现高频振荡。你可以在odeset里把RelTol再调小一个数量级试试。想扩展成三关节或者SCARA结构核心工作是把two_link_matrices.m扩展成3x3的M矩阵和对应的C、G向量。控制律代码不用换但参数整定要重新做因为三关节的耦合会比二关节复杂很多。5. 调参、踩坑与扩展建议5.1 常见问题与排查思路仿真代码能跑通之后很多人会去改参数改着改着就出问题了。我这里把实际调试中常见的几个问题整理成了一张表方便对照排查。现象可能原因解决办法仿真发散角度跑飞初始状态与期望轨迹初始点差距过大令x0等于期望轨迹在t0时的值跟踪误差一直不为零控制器中没有重力前馈G检查robot_dynamics.m里tau是否包含G项关节1跟踪正常关节2振荡Kd(2,2)偏小增大Kd(2,2)观察误差曲线衰减情况扰动后恢复太慢Kp偏小或者Kd偏大增大Kp或减小Kd但注意Kp过大可能放大噪声ode45运行速度很慢系统刚度太大数值刚性强降低Kp/Md或者减小RelTol并尝试ode15s控制力矩尖峰过大期望加速度变化太剧烈平滑期望轨迹比如用梯形速度规划排查这类问题有一个通用思路先关掉外部扰动把纯轨迹跟踪调好再打开扰动验证柔顺性。如果纯跟踪都不好说明模型或控制器基础有问题不要急着调阻抗参数。5.2 调Kp/Kd的实操顺序我调试这套代码时不是一次性把参数调到位的而是分了三步。第一步把Md固定为diag([1 1])Kp设为diag([100 100])Kd设为diag([20 20])先跑通观察基本跟踪效果。第二步根据误差曲线的振荡情况调Kd。如果误差曲线像弹簧一样来回弹说明阻尼不够我会先把Kd加倍看趋势。第三步根据稳态误差和恢复速度调Kp。如果轨迹跟踪总是慢半拍误差曲线在快速段偏大就本文还有配套的精品资源点击获取