简介机械臂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 I2M12 m2·(lc2² L1·lc2·cos q2) I2M21 M12M22 m2·lc2² I2重力向量g1 (m1·lc1 m2·L1)·g0·cos q1 m2·lc2·g0·cos(q1q2)g2 m2·lc2·g0·cos(q1q2)2.2.2 Coriolis矩阵C(q, qd)的紧凑写法C矩阵用Christoffel符号推导后可以整理成只含一个中间变量h的紧凑形式h -m2·L1·lc2·sin(q2)然后C [h·qd2, h·(qd1qd2); -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*(qd1qd2); -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(q1q2); g(2) m2*lc2*g0*cos(q1q2); 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质量m14.0kg连杆2质量m22.0kg连杆1长度L10.5m连杆2长度L20.4m连杆1质心距lc10.25m连杆2质心距lc20.2m连杆1惯量I10.1kg·m²连杆2惯量I20.05kg·m²关节力矩饱和tau_max80 / 40N·m重力加速度g09.81m/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² · M0Kd 2·ζ·ωn · M0M0取什么值取零位构型下质量矩阵的对角元。按2.2的参数计算M11(0) 1.38 kg·m²M22 0.13 kg·m²。按阻尼比ζ 0.86、无超调范围取值得到下表ωn (rad/s)Kp1Kd1Kp2Kd2预期效果8881981.8慢但稳适合熟悉流程1219929192.7常规演示推荐1635338333.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%力矩曲线没有高频抖动。如果超调偏大先加大KdKp保持不变如果稳态存在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_exttau 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(q1q2); J(1,2) -L2*sin(q1q2); J(2,1) L1*cos(q1) L2*cos(q1q2); J(2,2) L2*cos(q1q2); endF_cmd作用在末端笛卡尔空间里通过J转成关节力矩。如果机械臂接近奇异位形J的条件数会很大同样大小的F_cmd会产生极大的关节力矩仿真里表现为某个关节瞬间打到限幅。遇到这种情况先检查轨迹是否穿过了奇异位形而不是盲目调低B_d。4.3 接触力模型的选取阻抗控制必须有环境力反馈。仿真里最常见的做法是弹簧-阻尼接触模型末端穿透虚拟墙的深度记为deltaF_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(q1q2); L1*sin(q1) L2*sin(q1q2)]; end仿真场景设计末端x方向朝墙运动wall_x设为0.55末端初始x位置约为0.57所以会先前进一段再碰墙y方向保持恒定。阻抗控制会让末端轻轻接触墙并停住接触力收敛到稳定值换成纯PD控制会一直向墙压接触力持续增长直到力矩饱和。这个对比是理解阻抗控制价值的最直接实验。参数说明Md、Bd、Kd的单位分别是kg、N·s/m、N/m与关节空间的PD增益是两组完全独立的参数。建议初值Md2、Bd80、Kd200开始跑确认接触不发散后再按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_d200、B_d80与B_d200两组参数分别跑对比接触力曲线的振荡次数B_d从80加到200接触力会从明显的两三拍振荡变成单调收敛这就是虚阻尼吸收冲击的直观证据。保持K_d不变只调B_d可以独立观察阻尼项的作用这是阻抗参数整定中最快见效的一步。本文还有配套的精品资源点击获取