简介：机械臂PD控制与阻抗控制的MATLAB仿真源码，适合机器人控制、自动化及机电领域的研究者与工程师，用于在无硬件环境下完成机械臂控制算法的建模、验证与参数整定。资源共11个文件，以.m脚本和Simulink模型（.mdl）为核心，包含机械臂动力学模型、PD控制器设计、阻抗控制策略实现及仿真设置，并附带末端轨迹、控制力矩、位置跟踪、力控制等结果图片，便于直观分析控制效果；压缩包仅146KB，轻量易用。目前已有399人学习下载。通过调整比例微分增益与阻抗参数，可对比不同控制策略下的响应特性，为实际机械臂系统开发、课题研究或课程教学提供可复现的仿真实验基础。
1. 为什么说PD控制只是阻抗控制的一个特例
调二连杆机械臂时，最常遇到两难：比例增益调大，轨迹跟得紧，但末端撞到障碍物就像敲铁块；调小，又软得没有刚度可言。很多人在Simulink里同时写了PD和阻抗两套控制器，却始终说不清两者有多大差别。
其实这两个控制器并不对立。任务空间阻抗控制的目标刚度K_d和阻尼B_d给定后，在无外力位置跟踪场景下，它会直接退化成带阻尼的广义PD控制，差别只在坐标系与雅可比矩阵的投影方式。把两者放在同一套仿真里对照调试，能省下大量比对时间。
这份matlab源码包正是这个思路：以平面二连杆机械臂为对象，L_system.m负责动力学求解，L_xd.m生成期望轨迹，L_sim.mdl是顶层Simulink模型，还附带末端轨迹、控制力矩、位置跟踪、力控制四张仿真结果图。下文按“动力学建模 → PD实现 → 阻抗控制 → 读图验证”的顺序拆解这套源码。
2. 从L_system.m开始：二连杆动力学建模与Simulink文件结构解析
2.1 动力学方程与L_system.m的标准写法
整套仿真的起点是L_system.m。平面二连杆机械臂在关节空间中的动力学方程为：
M(q) q̈ + C(q, q̇) q̇ + G(q) = τ + τ_ext
其中M(q)是2×2惯性矩阵，C(q, q̇)是科里奥利与离心力矩矩阵，G(q)是重力项。平面机械臂在水平面内运动时G(q)可以直接置零，仿真包里L_system.m走的正是这个简化路径。控制器给出τ，L_system.m算出q̈，积分两次得到关节角与角速度，再反馈回控制器参与下一拍计算。
常见做法是把动力学写成独立函数，输入当前状态和力矩，输出关节加速度：
function ddq = L_system(q, dq, tau) % L_system.m 二连杆平面机械臂动力学 % 输入: q 关节角度 [q1; q2] % dq 关节角速度 [dq1; dq2] % tau 控制力矩 [tau1; tau2] % 输出: ddq 关节加速度，为 2x1 向量 q1 = q(1); q2 = q(2); dq1 = dq(1); dq2 = dq(2); m1 = 1; m2 = 1; % 连杆质量，单位 kg l1 = 1; l2 = 1; % 连杆长度，单位 m a1 = (m1 + m2) * l1^2 + m2 * l2^2; a2 = m2 * l1 * l2; a3 = m2 * l2^2; M = [a1 + 2*a2*cos(q2), a3 + a2*cos(q2); a3 + a2*cos(q2), a3]; h = a2 * sin(q2); C = [-h*dq2, -h*(dq1+dq2); h*dq1, 0]; G = [0; 0]; ddq = M \ (tau - C*dq - G); end这段代码等于把拉格朗日动力学直接数值化。a1、a2、a3只是把质量、长度合并成惯性矩阵系数的简记写法，真实参数取值如下表，源码包里基本沿用同一组量纲：
| 符号 | 含义 | 取值 | 单位 |
|---|---|---|---|
| m1 | 连杆1质量 | 1 | kg |
| m2 | 连杆2质量 | 1 | kg |
| l1 | 连杆1长度 | 1 | m |
| l2 | 连杆2长度 | 1 | m |
调试时如果发现力矩输出很大但末端移动缓慢，优先检查这一层：质量或长度差一个数量级，控制增益就必须跟着差两个数量级。另外，M\（tau - C*dq）用的是矩阵左除，写成分式形式的M^(-1) * ...在数值上也可以，但左除对2×2矩阵更稳定，也更容易扩展到六自由度。
2.2 Simulink顶层模型与源码文件的分工
L_sim.mdl是整套仿真的顶层文件。对比压缩包内文件名，可以梳理出如下数据流：L_input.m在仿真开始前把期望轨迹、增益矩阵写入工作区；L_xd.m在仿真中按t生成期望关节角；L_system.m接收控制力矩并积分得到机械臂状态；L_plot.m在仿真结束后读取工作区变量并画图。
| 文件 | 作用 | 上下游位置 |
|---|---|---|
| L_system.m | 二连杆动力学，输出关节加速度 | 接收tdau，向积分器供状态 |
| L_xd.m | 生成期望关节轨迹 | 接收t，输出期望角度xd |
| L_input.m | 初始化Kp、Kd及轨迹相关参数 | 仿真前写入MATLAB工作区 |
| L_sim.mdl | 顶层Simulink模型 | 串联控制器、动力学、积分器 |
| L_plot.m | 读取仿真结果并绘图 | 仿真结束后运行 |
在Simulink中，L_system.m通常被封装进MATLAB Function模块或S-Function模块。我一般建议调试阶段用MATLAB Function模块：双击就能改参数，断点也能直接打到.m文件里。S-Function的好处是编译速度更快，适合最后跑批量仿真时使用。
2.3 r2011a版本模型文件与新版Simulink的兼容处理
压缩包里另外还有一份L_sim.mdl.r2011a，这是用MATLAB R2011a保存的旧版模型文件。新版Simulink打开时通常会弹出版本升级提示，操作时注意三点：
- 打开前先复制一份原文件，升级过程不可逆。
- 升级后检查求解器配置，固定步长仿真建议将步长设在0.001 s到0.01 s之间，默认变步长在阻抗接触场景下可能失真。
- 核对模型里是否有已删除模块或失效函数名，老版本遗留的某些Simulink模块在新版中已被替换。
遇到“Cannot load model”这类报错，本质是mdl内部schema版本号与当前Simulink不匹配，处理方式只有一个：用安装的新版MATLAB重新打开并执行升级保存。若你的版本低于R2011a，则直接无法打开，这不是源码问题，是版本向下不兼容。
3. 在Simulink里落地PD控制：增益设计、期望轨迹与发散排查
3.1 关节空间PD控制律与重力补偿的关系
PD控制器的标准形式可以写成：
τ = Kp · e + Kd · ė + G(q)
其中e = qd − q是关节角误差，ė = dqd − dq是角速度误差。由于L_system.m中G(q) = 0，这里稳态误差理论上为零。如果在重力环境下使用同样的控制器，比例环节无法完全消除稳态误差，必须额外叠加重力前馈，这是仿真与真机调试差别最大的地方。
在L_sim.mdl内部，PD控制可以用两种方式搭建。老模型里常见的是两个Gain模块分别乘Kp和Kd，再通过Add模块合成力矩；新版Simulink可以直接用PID Controller模块，把积分系数设为零，微分系数设为Kd。需要注意微分通道的输入源：如果直接对误差取微分，噪声会被放大；规范做法是把期望角速度dqd和实际角速度dq作差，再从Gain模块输出，这样能减少一次数值微分。
增益正负也要盯住。力矩τ = Kp · (qd − q)中，Kp取正数，若模型里接反成了Kp · (q − qd)，仿真会立刻发散，表现为关节角快速飞向极限位置，此时先检查符号，再去调增益大小。
3.2 期望轨迹生成：L_xd.m与五次多项式
L_xd.m的核心任务是生成平滑期望轨迹。直接给阶跃位置信号会让PD控制器在起始时刻输出极大力矩，仿真图上也会出现一个不自然的力矩尖峰，这正是需要用连续轨迹的原因。
常用的做法是五次多项式插值：
function xd = L_xd(t, q0, qf, tf) % L_xd.m 生成五次多项式期望关节轨迹 % q0: 起始关节角, qf: 终止关节角 % tf: 运动总时间, 默认 2 秒 if nargin < 4 tf = 2; end tau = min(t / tf, 1); s = 10*tau^3 - 15*tau^4 + 6*tau^5; xd = q0 + (qf - q0) * s; end这里位置、速度、加速度在t = 0和t = tf时刻都满足边界条件，因此不会在切换瞬间给PD控制器带来冲击。Simulink中只需要一个Clock模块驱动L_xd.m，输出与机械臂状态作差后进入控制器。
仿真结束后对比xd.jpg和位置跟踪.jpg，如果两条曲线几乎重合，说明轨迹生成环节没有问题。如果位置跟踪始终落后期望轨迹一个固定时间，多半是期望轨迹给的加速度前馈不足，PD控制本身是靠误差产生力矩的，任何跟踪滞后都会留下稳态误差或延迟，只能靠调大Kp来压缩。
3.3 PD增益设定表与仿真发散排查
Kp和Kd的数值必须和动力学模型匹配。源码包中质量取1 kg、杆长取1 m，量纲相对规整，按下面表格起步基本不会炸：
| 增益组合 | Kp | Kd | 适用场景 |
|---|---|---|---|
| 柔顺 | 50~100 | 10~20 | 阻抗控制对比试验 |
| 标准 | 200~400 | 30~60 | 位置跟踪仿真 |
| 高刚性 | 600~1000 | 80~150 | 快速轨迹跟踪 |
增益并非越大越好。Kp过大会让系统闭环极点远离实轴，仿真曲线出现等幅振荡；Kd过大会引入高频噪声，体现在控制力矩.jpg上就是毛刺。调整顺序建议是先固定Kp为200，逐步增加Kd直到力矩曲线平滑；再把Kp调大，每步增幅不超过50%，观察位置跟踪曲线是否出现超调。
注意：仿真发散不总是增益问题。固定步长设得太大，比如0.05 s，二连杆动力学会出现离散化不稳定；积分器用的是ode45变步长，遇到强非线性接触又会产生事件检测失败。排查顺序依次是：步长、增益符号、Kp/Kd数量级、模型代数环。
代数环是Simulink里最隐蔽的问题。当力矩τ同时出现在控制器输出和动力学输入两端，且没有单位延迟模块缓存一拍，Simulink会直接报代数环错误或拉慢仿真速度。处理办法是让动力学输出端的信号经过一个Memory或Unit Delay模块，断开直接回路。
4. 任务空间阻抗控制的推导、封装与力跟踪调试
4.1 阻抗模型数学表达与参数物理含义
阻抗控制不再直接指定机械臂要走到哪个点，而是指定一种“受力就变形、撤力就恢复”的动态关系。最常见的二阶阻抗表达式写成：
M_d · (ẍ − ẍ_d) + B_d · (ẋ − ẋ_d) + K_d · (x − x_d) = F_ext
其中x是末端笛卡尔位置，M_d、B_d、K_d分别代表期望惯性、期望阻尼和期望刚度。这三个物理量对应机械臂对外界力F_ext的动态响应特性。刚度K_d决定接触时的静态压缩量，阻尼B_d决定接触瞬间的振荡衰减速度，惯性M_d决定响应快慢。
当M_d取零且F_ext = 0时，上式退化为：
B_d · (ẋ − ẋ_d) + K_d · (x − x_d) = 0
这本质上就是任务空间中的一个PD控制，K_d对应位置误差增益，B_d对应速度误差增益。这也是开头说“PD是阻抗特例”的直接原因。真实系统里M_d不能取零，通常取一个估计的末端等效惯性再适当放大，避免加速度指令过大。
4.2 从关节PD切换到任务空间阻抗控制
关节PD控制在关节空间处理误差，阻抗控制则需要先把末端笛卡尔误差换算成关节力矩。二者之间通过雅可比矩阵J(q)连接：
τ = J(q)ᵀ · (B_d · (ẋ_d − ẋ) + K_d · (x_d − x))
这里x_d和x是末端期望位置与实际位置，J(q)ᵀ把笛卡尔力映射成关节力矩。平面二连杆机械臂的雅可比矩阵可以按下面的方式写在一个函数里：
function J = jacobian2(q) % jacobian2.m 平面二连杆机械臂的几何雅可比矩阵 q1 = q(1); q2 = q(2); l1 = 1; l2 = 1; J = [-l1*sin(q1) - l2*sin(q1+q2), -l2*sin(q1+q2); l1*cos(q1) + l2*cos(q1+q2), l2*cos(q1+q2)]; end在Simulink中实现时，末端位置x需要通过运动学正解模块计算，雅可比矩阵J也由q实时更新。整个回路包括：运动学正解 → x_d − x误差计算 → K_d、B_d增益 → Jᵀ变换 → 关节力矩τ。与PD控制器最大的区别是，传统PD的Kp/Kd直接作用在关节角上，而阻抗控制的K_d/B_d作用在笛卡尔空间，物理意义更直观：想让机械臂横向柔顺，就减小x方向的K_d。
4.3 接触力跟踪与力控制.jpg的复现思路
力控制.jpg对应的是带力反馈的阻抗控制。机械臂末端接触环境后，力传感器读回F_ext，期望接触力F_des与F_ext的误差被折算成末端位置修正量，再进入阻抗回环。常用实现是在阻抗控制外叠加一个力修正项：
function tau = impedance_force_control(q, dq, x, dx, x_d, dx_d, F_ext, F_des) % 阻抗 + 力跟踪复合控制 K_d = diag([200, 200]); % 任务空间刚度 B_d = diag([40, 40]); % 任务空间阻尼 Kf = 0.02; % 力误差修正增益 J = jacobian2(q); Fx = K_d * (x_d - x) + B_d * (dx_d - dx) + Kf * (F_des - F_ext); tau = J.' * Fx; end几点需要注意。Kf的量纲是位置/力，取值很小，通常在0.001到0.01之间，偏大容易让机械臂在接触面上颤振。力跟踪阶段要降低任务空间刚度K_d，否则位置内环会抗拒力修正，表现为接触力始终达不到期望值。仿真里模拟接触力时，常见做法是用弹簧模型F_ext = k_env · (x − x_wall)计算环境反力，k_env取500到1000 N/m，墙的位置设置在期望轨迹的终点附近。
力控制.jpg如果开始振荡，先降低B_d对应的速度增益，再减小Kf，最后才考虑减小k_env。顺序反过来的话，很容易误判成环境太硬，实际上是控制器自身阻尼不足。
5. 五张仿真结果图的读法：从xd.jpg到控制力矩.jpg的验证顺序
5.1 xd.jpg与位置跟踪.jpg先看，判断轨迹跟踪误差
xd.jpg画的是期望关节角的规划曲线，位置跟踪.jpg画的是实际关节角的响应曲线。把两张图叠在一起看，重点找三个问题：起始段误差是否收敛、中间段有没有持续滞后、结束段是否出现振荡。如果实际曲线在起点和终点都贴着期望曲线，只有中间短暂偏移，说明PD增益处于合理区间。若曲线像波浪一样上下抖动，说明Kd偏小。
5.2 末端轨迹.jpg与控制力矩.jpg接着看
末端轨迹.jpg反映的是笛卡尔空间的实际运动路径。二连杆机械臂的关节轨迹即使看起来平滑，末端轨迹也可能出现弧线偏移，这是运动学耦合带来的，不是控制问题。控制力矩.jpg则能看到力矩是否饱和：如果力矩曲线频繁打到上下限，说明模型里的轨迹规划超出了执行器能力，需要把目标轨迹的tf调大而不是继续调增益。
5.3 用L_plot.m的扫参思路定位最佳增益
L_plot.m不只用来出图，稍加改动就能变成批量扫参脚本。典型操作是把Kp和Kd定义成数组，用循环反复调用sim命令：
% 批量扫Kp，固定Kd Kp_list = [100, 200, 400, 600]; Kd = 40; err_rms = zeros(length(Kp_list), 1); for i = 1:length(Kp_list) Kp = Kp_list(i); sim('L_sim.mdl'); err = yout.signals(1).values(:, 1) - yout.signals(1).values(:, 2); err_rms(i) = sqrt(mean(err.^2)); end plot(Kp_list, err_rms);这个脚本先把Kd固定在40，再逐个试Kp。选择RMS误差曲线进入平台区的点作为最佳增益，一般平台区起点在200到400之间。注意sim命令会直接刷新工作区变量，脚本里最好在sim前加clear命令清理旧数据。
最后一轮调参时，按“先B后K”的顺序来。先把K_d固定在200，B_d从20逐步加到80，观察末端轨迹的过冲量；之后每增大10%的K_d，就复查一次接触力曲线，直到K_d提高到轨迹误差不再明显下降为止。这样调出来的阻抗参数，通常同时满足位置跟踪精度和接触柔顺两个指标。
本文还有配套的精品资源，点击获取
