简介：机械臂PD控制与阻抗控制MATLAB仿真源码包，面向控制工程、机器人学及机械设计领域的研究者、工程师与高年级学生，用于机械臂控制算法的建模、设计与性能验证。压缩包内共11个文件，包括MATLAB脚本（.m）、Simulink仿真模型（.mdl及.r2011a兼容版本）以及末端轨迹、控制力矩、位置跟踪、力控制等多张仿真结果图片（.jpg），打包后仅146KB，下载与部署方便。源码涵盖机械臂动力学模型、PD控制器设计、阻抗控制策略实现与仿真参数配置，无需真实硬件即可模拟机械臂在不同控制策略下的运动轨迹、力矩响应与交互力表现，适用于课堂教学演示、科研预研和算法初步验证。通过修改比例增益、微分增益及阻抗目标刚度、阻尼等参数，可系统对比PD控制与阻抗控制的动态性能，结合可视化曲线直观分析末端轨迹、位置跟踪误差和接触力变化，帮助深入理解机器人柔顺控制机理，并为后续机构设计与控制优化提供参考。目前已有399人学习，是快速上手机械臂控制仿真的实用参考资料。
1. 机械臂PD控制与阻抗控制，先分清这两层再谈仿真
机械臂仿真里最常见的混淆，是把PD控制和阻抗控制当成两条互不相干的路线。实际做装配、打磨、插孔这类任务时，PD控制负责把关节角按规划轨迹跟住，阻抗控制负责在末端碰到环境时，把位置偏差和接触力之间的动态关系调成用户指定的弹簧-阻尼系统。下面这套方案在Matlab里只需要一个二连杆模型、一个ode45积分器、一组可调的阻抗参数就能跑通，不依赖Simulink也能复现。适合正在搭机械臂轨迹跟踪仿真、或准备从位置控制向力控过渡的工程师查看；看完这套仿真，你会理解PD增益怎么定初值、阻抗刚度为什么要低于接触刚度、以及仿真发散时先查哪三个变量。
2. 机械臂动力学模型与Matlab仿真主循环
2.1 为什么要从完整动力学模型开始
机械臂PD控制只是外层控制律。仿真里真正被积分的是正动力学方程：给定关节力矩，解算出关节加速度，再积分出速度和位置。这个闭环路径和真实机器人一致——控制律输出力矩，力矩进动力学方程，动力学方程输出运动状态，状态再反馈回控制律。如果图省事，直接给每个关节角套一个二阶低通来模拟响应，等于把重力、科氏力和惯性耦合全部丢掉，整出来的PD增益到真机上完全不能直接用。
二连杆模型的自由度刚好覆盖平面内主要耦合项：惯性矩阵随构型变化、重力矩随角度变化、科氏力与两个关节速度的乘积相关。M矩阵、C矩阵、g向量都有解析表达式，便于逐项验证；多连杆机械臂也只是把这三项从解析式换成数值计算，外层控制律和仿真循环结构完全不用变。
2.2 二连杆动力学方程的M、C、g解析式与Matlab代码
2.2.1 质量矩阵M(q)与重力向量g(q)
对平面二连杆（连杆1长度为L1，质心距关节1为lc1；连杆2长度为L2，质心距关节2为lc2），质量矩阵按标准形式写为：
M11 = m1·lc1² + m2·(L1² + lc2² + 2·L1·lc2·cos q2) + I1 + I2
M12 = m2·(lc2² + L1·lc2·cos q2) + I2
M21 = M12
M22 = m2·lc2² + I2
重力向量：
g1 = (m1·lc1 + m2·L1)·g0·cos q1 + m2·lc2·g0·cos(q1+q2)
g2 = m2·lc2·g0·cos(q1+q2)
2.2.2 Coriolis矩阵C(q, qd)的紧凑写法
C矩阵用Christoffel符号推导后，可以整理成只含一个中间变量h的紧凑形式，h = -m2·L1·lc2·sin(q2)，然后：
C = [h·qd2, h·(qd1+qd2); -h·qd1, 0]
对应代码如下，放在函数文件里方便复用：
function M = mass_matrix(q) % 二连杆质量矩阵 % 参数直接写在函数内，单文件可运行 m1 = 4.0; m2 = 2.0; % 连杆质量 kg L1 = 0.5; L2 = 0.4; % 连杆长度 m lc1 = 0.25; lc2 = 0.2; % 质心位置 m I1 = 0.1; I2 = 0.05; % 转动惯量 kg*m^2 q1 = q(1); q2 = q(2); M = zeros(2, 2); M(1,1) = m1*lc1^2 + m2*(L1^2 + lc2^2 + 2*L1*lc2*cos(q2)) + I1 + I2; M(1,2) = m2*(lc2^2 + L1*lc2*cos(q2)) + I2; M(2,1) = M(1,2); M(2,2) = m2*lc2^2 + I2; endfunction C = coriolis_matrix(q, qd) % 科氏力矩阵，注意返回的是C本身，使用时乘以qd m2 = 2.0; L1 = 0.5; lc2 = 0.2; q2 = q(2); qd1 = qd(1); qd2 = qd(2); h = -m2 * L1 * lc2 * sin(q2); C = [h*qd2, h*(qd1+qd2); -h*qd1, 0]; endfunction g = gravity_vector(q) % 重力项，g0作为局部变量避免与函数名冲突 m1 = 4.0; m2 = 2.0; L1 = 0.5; lc1 = 0.25; lc2 = 0.2; g0 = 9.81; q1 = q(1); q2 = q(2); g = zeros(2, 1); g(1) = (m1*lc1 + m2*L1) * g0 * cos(q1) + m2*lc2*g0*cos(q1+q2); g(2) = m2*lc2*g0*cos(q1+q2); end代码的逻辑说明：质量矩阵把两个连杆的平动动能和转动动能折算到关节坐标上，M(1,1)里出现cos(q2)就是在表达第二个连杆对第一个关节惯性的耦合贡献；科氏力矩阵乘以关节速度向量后得到的是与速度平方相关的虚拟力，在低速轨迹里影响不大，但在接触瞬间速度突变时不能省略；重力向量是两个连杆重力对各自关节轴的力矩投影。
参数说明：这套参数对应的是一台小负载桌面机械臂的量级，末端的最大静载约2kg。如果你要仿真UR5e这类六轴臂，直接把m、L、I换成对应连杆参数即可，控制律和仿真循环不用改动。
2.3 ode45正动力学积分主循环
控制律的函数句柄形式可以很方便替换：PD控制传pd_gravity_ctrl，阻抗控制传impedance_ctrl。运行仿真的脚本结构如下：
% 主脚本 main_pd_sim.m t_span = [0 5]; % 仿真时长 5 秒 x0 = [0.1; -0.2; 0; 0]; % 初始关节角和角速度 opt = odeset('RelTol', 1e-6, 'AbsTol', 1e-8); [t, x] = ode45(@(t, x) robot_dynamics(t, x, ... pd_gravity_ctrl(t, x, qd_des_fun, Kp, Kd)), t_span, x0, opt);function xd = robot_dynamics(t, x, tau) % 正动力学：由力矩算角加速度 q = x(1:2); qd = x(3:4); M = mass_matrix(q); C = coriolis_matrix(q, qd); g = gravity_vector(q); qdd = M \ (tau - C*qd - g); xd = [qd; qdd]; end代码逻辑说明：robot_dynamics把状态向量拆成位置和速度两块，M矩阵求逆得到加速度，再拼装成状态导数返回给ode45。用M \ 而不是inv(M) *，是因为反斜杠在Matlab里对2x2矩阵走的是LU分解路径，数值更稳，速度差异在这个规模下可以忽略。
参数说明：t_span起点要留到控制律内部用的插值范围之外，避免轨迹函数在端点外取到未定义值；AbsTol给到1e-8是因为阻抗仿真中接触力突变时，位置量级小但力变化大，容差太粗会丢掉接触瞬间的细节；如果发现仿真步数过多导致速度慢，可以先把AbsTol放宽到1e-6跑通逻辑，再做精调。
2.4 仿真参数表与驱动器限幅
用一组固定的物理参数，便于对比结果和复现：
| 参数 | 符号 | 数值 | 单位 |
|---|---|---|---|
| 连杆1质量 | m1 | 4.0 | kg |
| 连杆2质量 | m2 | 2.0 | kg |
| 连杆1长度 | L1 | 0.5 | m |
| 连杆2长度 | L2 | 0.4 | m |
| 连杆1质心距 | lc1 | 0.25 | m |
| 连杆2质心距 | lc2 | 0.2 | m |
| 连杆1惯量 | I1 | 0.1 | kg·m² |
| 连杆2惯量 | I2 | 0.05 | kg·m² |
| 关节力矩饱和 | tau_max | 80 / 40 | N·m |
| 重力加速度 | g0 | 9.81 | m/s² |
力矩饱和是真实驱动器都有的特性，控制律输出后要加一句限幅：tau = max(min(tau, tau_max), -tau_max);。没有限幅时PD增益调大还能勉强运行；加上限幅后，增益过高会出现两个关节交替饱和的极限环，这个现象在Simulink里同样存在，根因是积分饱和，排查方法一致。
3. PD控制在关节空间的实现与增益整定
3.1 带重力补偿的PD控制律
标准PD控制律在关节空间的写法是：
tau = Kp·(q_des - q) + Kd·(qd_des - qd) + g(q)
为什么一定要加重力补偿？因为PD本身是线性反馈，而重力是与构型相关的非线性项。不加补偿时，重力对稳态误差的贡献和Kp成反比，Kp不够大关节就会垂下去一段距离；Kp加大又容易引入振荡。加入g(q)后，控制对象在平衡点附近近似为线性二阶系统，增益设计与真机调试经验可以直接平移。
Matlab实现如下：
function tau = pd_gravity_ctrl(t, x, qd_des_fun, Kp, Kd) % qd_des_fun: 函数句柄，输入t返回[q_des; qd_des; qdd_des] % Kp, Kd: 2x1向量，用.*实现逐元素相乘 q = x(1:2); qd = x(3:4); [q_des, qd_des, ~] = qd_des_fun(t); tau = Kp .* (q_des - q) + Kd .* (qd_des - qd) + gravity_vector(q); % tau = max(min(tau, tau_max), -tau_max); % 驱动器限幅 end逻辑说明：期望轨迹用函数句柄而不是固定常量，是为了在控制律内部每个积分步都能取到当前时刻的位置、速度、加速度参考。第三个返回值qdd_des在PD控制里用不到，但后面做前馈控制或阻抗控制时需要加速度参考，接口上先预留，避免后面改函数签名。
参数说明：Kp和Kd用2x1向量而不是2x2矩阵，意思是两个关节的增益完全解耦，调参时互不影响。实际调试中关节1承受两个连杆的重力和惯性，Kp通常比关节2大一个数量级；Kd主要用来抑制超调，但Kd过大会放大速度测量噪声，后面3.3会看到具体现象。
3.2 用极点配置确定Kp、Kd初值
在重力补偿后，单关节闭环特征方程近似为二阶系统：
M0·s² + Kd·s + Kp = 0
写成标准形式 M0·(s² + 2·ζ·ωn·s + ωn²)，对比系数得到：
Kp = ωn² · M0
Kd = 2·ζ·ωn · M0
M0取什么值？取零位构型下质量矩阵的对角元。按2.2的参数计算，M11(0) = 1.38 kg·m²，M22 = 0.13 kg·m²。按阻尼比ζ = 0.86、无超调范围取值，得到下表：
| ωn (rad/s) | Kp1 | Kd1 | Kp2 | Kd2 | 预期效果 |
|---|---|---|---|---|---|
| 8 | 88 | 19 | 8 | 1.8 | 慢但稳，适合熟悉流程 |
| 12 | 199 | 29 | 19 | 2.7 | 常规演示推荐 |
| 16 | 353 | 38 | 33 | 3.6 | 响应快，噪声敏感 |
注意这张表给出的是初值。真机上惯量辨识不准，且未建模的摩擦和柔性会吃掉相位裕度，所以ωn要从低往高加，而不是直接上16。如果目标轨迹速度较高，还要检查力矩曲线是否撞到饱和限幅，撞了就得降ωn或改轨迹，不能硬调增益。
3.3 轨迹跟踪验收：五次多项式与误差曲线
常见做法是给一个从0到0.5 rad的五次多项式轨迹，跑5秒。五次多项式的好处是位置、速度、加速度都连续，起步和停止没有冲击，不会把跟踪误差和轨迹本身的不光滑混在一起。
function [q_des, qd_des, qdd_des] = traj_step(t) % 五次多项式：起点0，终点0.5，总时间2秒 tf = 2.0; q0 = 0; qf = 0.5; tau_s = min(t / tf, 1); q_des = q0 + (qf - q0) * (10*tau_s^3 - 15*tau_s^4 + 6*tau_s^5); qd_des = (qf - q0) / tf * (30*tau_s^2 - 60*tau_s^3 + 30*tau_s^4); qdd_des = (qf - q0) / tf^2 * (60*tau_s - 180*tau_s^2 + 120*tau_s^3); end验收标准：跟踪误差收敛到±0.005 rad以内，超调量小于5%，力矩曲线没有高频抖动。如果超调偏大，先加大Kd，Kp保持不变；如果稳态存在0.01 rad量级的固定偏差，优先检查是否忘了加g(q)补偿，这是PD仿真里最常见的错误，比增益整定问题出现频率高得多。
4. 笛卡尔阻抗控制、接触模型与参数匹配
4.1 阻抗控制的目标方程和目标解释
阻抗控制的控制目标不是跟踪位置，而是把末端所受外力F_ext与位置偏差的关系塑造成一个二阶系统：
M_d·(xdd - xdd_des) + B_d·(xd - xd_des) + K_d·(x - x_des) = -F_ext
这个方程的含义是：当末端碰到障碍物时，外力增大，位置偏差相应增大或运动减速，而不是硬顶。M_d是虚拟质量，决定动态过渡的速度；B_d是虚拟阻尼，吸收接触瞬间的冲击能量；K_d是虚拟刚度，决定稳定接触时的位置偏差大小。注意等式右侧的负号：外力方向指向机器人时，期望位置要往反方向让开，而不是继续向前压。
这套思路和PD控制不冲突。内层PD保证关节能跟上笛卡尔空间的期望加速度，外层阻抗决定这个加速度怎么算。很多真实工业机械臂的力控模式就是这种内外环结构，内环频率1kHz，外环200Hz，仿真里合并成一个循环即可。
4.2 从目标方程到控制律
把目标方程改写成加速度形式，再映射到关节力矩：
F_cmd = M_d·xdd_des + B_d·(xd_des - xd) + K_d·(x_des - x) - F_ext
tau = J(q)' · F_cmd + g(q)
需要机械臂雅可比矩阵，二连杆雅可比实现如下：
function J = jacobian2(q) % 平面二连杆末端速度雅可比 L1 = 0.5; L2 = 0.4; q1 = q(1); q2 = q(2); J = zeros(2, 2); J(1,1) = -L1*sin(q1) - L2*sin(q1+q2); J(1,2) = -L2*sin(q1+q2); J(2,1) = L1*cos(q1) + L2*cos(q1+q2); J(2,2) = L2*cos(q1+q2); endF_cmd作用在末端笛卡尔空间里，通过J'转成关节力矩。如果机械臂接近奇异位形，J的条件数会很大，同样大小的F_cmd会产生极大的关节力矩，仿真里表现为某个关节瞬间打到限幅。遇到这种情况，先检查轨迹是否穿过了奇异位形，而不是盲目调低B_d。
4.3 接触力模型的选取
阻抗控制必须有环境力反馈。仿真里最常见的做法是弹簧-阻尼接触模型，末端穿透虚拟墙的深度记为delta：
F_ext = K_c · max(delta, 0) + B_c · delta_dot
当末端不接触墙时，delta为负，接触力为0，不会出现“吸住”墙面的假象；接触后，K_c提供弹性恢复力，B_c提供接触阻尼，避免接触瞬间力值突变形成数值刚性。
function [F_ext, delta] = contact_force(x_tip, xd_tip, wall_x) % x_tip: 末端x坐标 % xd_tip: 末端x速度 delta = x_tip - wall_x; % 穿透深度，大于0表示进入墙体 K_c = 2000; B_c = 50; % 接触刚度和接触阻尼 if delta > 0 F_ext = K_c * delta + B_c * xd_tip; else F_ext = 0; delta = 0; end end参数说明：K_c取2000是一个经验值，比阻抗刚度K_d高一个数量级，目的是让接触环境本身偏硬，阻抗参数能在接触力曲线上体现出来。如果K_c太低，比如取200，那么大部分位置偏差会被环境本身的柔性吃掉，K_d怎么调都看不出来。B_c取50用于抑制接触瞬间的振荡，太小会看到力曲线接触点出现尖峰。
4.4 阻抗控制完整仿真循环代码
function tau = impedance_ctrl(t, x, param) % 笛卡尔空间阻抗控制，param为结构体参数 q = x(1:2); qd = x(3:4); x_tip = forward_kinematics(q); % 正运动学 J = jacobian2(q); xd_tip = J * qd; [F_ext, ~] = contact_force(x_tip(1), xd_tip(1), param.wall_x); % 阻抗控制律 F_cmd = param.Md * param.xdd_des ... + param.Bd * (param.xd_des - xd_tip) ... + param.Kd * (param.x_des - x_tip) - [F_ext; 0]; tau = J' * F_cmd + gravity_vector(q); end正运动学函数：
function x_tip = forward_kinematics(q) L1 = 0.5; L2 = 0.4; q1 = q(1); q2 = q(2); x_tip = [L1*cos(q1) + L2*cos(q1+q2); L1*sin(q1) + L2*sin(q1+q2)]; end仿真场景设计：末端x方向朝墙运动，wall_x设为0.55（末端初始x位置约为0.57，所以会先前进一段再碰墙），y方向保持恒定。阻抗控制会让末端轻轻接触墙并停住，接触力收敛到稳定值；换成纯PD控制会一直向墙压，接触力持续增长直到力矩饱和。这个对比是理解阻抗控制价值的最直接实验。
参数说明：Md、Bd、Kd的单位分别是kg、N·s/m、N/m，与关节空间的PD增益是两组完全独立的参数。建议初值Md=2、Bd=80、Kd=200开始跑，确认接触不发散后再按5.1的顺序调整。
提示：K_d必须比接触刚度K_c低一个数量级左右。两者接近时，阻抗回路和接触弹簧形成刚性串联，接触力曲线会出现高频振荡，此时调低K_d比调大B_d更有效。
5. 阻抗控制与PD控制的参数协同排错
5.1 两组参数的分工与整定顺序
PD增益管轨迹跟踪能力，阻抗参数管与环境交互时的顺从性。整定顺序有讲究：先在无接触场景下把PD调稳，再打开阻抗接触。顺序反过来，接触瞬间发散的根因会同时落在两组参数上，很难定位。实际操作是：把wall_x设到末端永远碰不到的位置，按第3章的验收标准调完PD；然后打开接触，先设大虚阻尼B_d、中等M_d、小K_d，确认接触力不发散；最后逐步提高K_d，观察末端位置误差与接触力的比值是否接近1/K_d。
5.2 仿真发散时的三个排查点
按顺序检查，不要跳跃。
第一，确认限幅有没有加。无限幅时加大Kp会立刻发散；有限幅后发散往往表现为极限环，即两个关节交替触及力矩上限，此时应该降Kp而不是降Kd，因为Kp增大导致饱和后系统等价于降低了阻尼。
第二，检查微分通道是否用了差分。qd直接从ode45状态里取是干净的，但如果你对F_ext做数值差分求导来获取接触力变化率，步长噪声会被放大，表现为接触力曲线上的毛刺。正确做法是像contact_force里那样直接使用xd_tip解析速度。
第三，检查模型参数是否一致。forward_kinematics、jacobian2、contact_force三处都用到L1、L2，如果某一处写成0.45，位置反馈和接触力之间会出现固定比例偏差，仿真发散的形态与控制器参数无关，表现为怎么调都不稳定。
5.3 用末端力曲线验证阻抗参数匹配
跑完仿真后，把末端位置x_tip、接触力F_ext和期望位置x_des画在同一张图上。三个特征值得关注：接触前轨迹完全贴合期望，误差在0.005 m内；接触瞬间位置曲线出现平滑圆角，不应有尖刺，接触力单调上升到稳定值；稳态时位置误差和接触力的比值约等于1/K_d，这个比值偏差超过20%说明B_d或M_d偏小，过渡过程衰减不充分。
用这条曲线判断参数比看关节角误差更直观，因为笛卡尔阻抗本来就不是以位置跟踪为目标。取K_d=200、B_d=80与B_d=200两组参数分别跑，对比接触力曲线的振荡次数：B_d从80加到200，接触力会从明显的两三拍振荡变成单调收敛，这就是虚阻尼吸收冲击的直观证据。保持K_d不变只调B_d，可以独立观察阻尼项的作用，这是阻抗参数整定中最快见效的一步。
本文还有配套的精品资源，点击获取
