简介质量块导纳控制Simulink程序面向机器人柔顺控制与阻抗/导纳控制方向的研究者提供基于MATLAB/Simulink的完整单质量块导纳控制模型。程序将期望输入、被控对象动力学、位置控制等环节封装为独立的M脚本与Simulink模型结合Ott等人的统一阻抗与导纳控制理论可帮助读者理解导纳控制在力控中的应用与参数调节方法。压缩包共5个文件包含四个MATLAB脚本与一个Simulink模型文件整体27KB体积小巧便于下载后直接运行调试。目前已有3979人学习浏览内容覆盖从质量块模型搭建到控制器实现的完整链路适合控制相关专业学生、科研人员及工程师作为入门参考通过运行示例可观察导纳控制下的位置响应差异并可自行修改质量、刚度等参数加深对频域、时域特性的理解。1. 质量块导纳控制Simulink程序为什么做力控的第一课都从它开始如果你调过机械臂的力控大概率遇到过这类现象末端用力压工件位置环用力顶着接触力不受控地往上涨换成力控模式后机器人要么抖得厉害要么轻轻一碰就退出去很远。质量块导纳控制Simulink程序就是用来把这类问题关进笼子里的最小可复现方案。它把一个带惯性负载的被控对象抽象成质量块把力传感器读到的接触力通过导纳模型换算成位置修正量再交给底层位置环执行。这个方案不追求花哨的阻抗切换而是先把力控的链路打通传感器、导纳模型、位置内环、被控对象四段信号链在Simulink里一目了然。适合谁刚接触柔顺控制、想在仿真里验证导纳参数对接触力影响的学生以及要在样机上做力控预研但不想先碰硬件的工程师。一个反直觉的结论先放在前面导纳控制做得好不好力传感器精度只是次要因素真正卡脖子的是内环位置环的刚度和采样率。这也是为什么很多人在仿真里参数调得好好的一下硬件就翻车——这个坑本文后面专门讲。2. 导纳控制与质量块模型先搞懂它在Simulink里是怎么被拆成几块的2.1 导纳控制的方向力进来位置出去做柔顺控制先要分清阻抗控制和导纳控制的方向。阻抗控制是位置进来、力出去控制器根据位置偏差计算期望接触力适合没有力传感器、靠关节力矩估计接触的场景导纳控制正好相反力传感器测到外界接触力控制器把它换算成位置修正量再让位置环去跟踪。质量块导纳控制用的就是后一种外力F通过目标导纳模型产生位置偏移x系统对外表现成质量-弹簧-阻尼的柔顺特性。导纳控制的核心方程是一个二阶微分方程[ M_d (\ddot{x}_d - \ddot{x}_0) B_d (\dot{x}_d - \dot{x}0) K_d (x_d - x_0) F{ext} ]M_d是目标惯量B_d目标阻尼K_d目标刚度F_ext是传感器测得的外部接触力。x_d是修正后的期望位置x_0是未修正的原始期望位置。这个方程的意义在于外力越大位置偏移越大但偏移的快慢和回弹特性由M_d、B_d、K_d三个参数共同塑造。质量块导纳控制里的质量块既指被控对象抽象成一个纯惯性负载也指M_d这个目标惯量参数——两者都不是一回事但经常被混着说。2.2 质量块被控对象一个纯惯性负载的Simulink实现质量块在仿真里代表什么最简单的理解是一个刚体在光滑平面上滑动不受重力影响只受控制力F和外部扰动力F_ext的作用动力学方程为[ m \ddot{x} F F_{ext} ]m是负载质量x是位移。在Simulink里搭建这个模型不需要复杂工具箱两个积分器串联就能搞定第一个积分器把加速度积分成速度第二个积分器把速度积分成位移。加速度由F F_ext/ m 算出来。提示质量块被控对象是导纳控制验证时最干净的负载模型但不代表真实系统。真实系统还有摩擦力、柔性、延迟质量块模型的目的只是先让导纳控制器本身的行为可观测、可复现。实际搭建中我一般把被控对象单独放一个子系统输入是控制力输出是位置和速度方便后续替换成更复杂的负载模型。初始条件设置为零m的取值参考机器人末端等效质量一般在0.5到5公斤之间。2.3 连续域和离散域的差异为什么仿真能跑、硬件就抖导纳控制方程在连续域里写起来漂亮但落到Simulink里仿真可以是连续解算器硬件部署时一定是离散解算器。这里有一个关键差异连续域里M_d、B_d、K_d的取值范围不受采样时间约束但离散化之后等效阻尼和惯量会随采样周期变化系统的稳定域也跟着缩水。最常见的离散化做法是把二阶导纳方程拆成两个一阶方程用前向欧拉法更新[ v_d[k1] v_d[k] \frac{F_{ext}[k] - B_d v_d[k] - K_d x_d[k]}{M_d} \cdot T_s ][ x_d[k1] x_d[k] v_d[k1] \cdot T_s ]T_s是控制周期。前向欧拉法实现简单但在M_d偏小、T_s偏大时容易引入数值振荡。更稳妥的做法是使用双线性变换Tustin把连续传递函数离散化成差分方程稳定性更好代价是推导稍麻烦。对于初学者我建议先按前向欧拉搭通跑通后再评估是否需要换更高级的离散化方法。3. Simulink建模逐步实现从零复现质量块导纳控制的最小闭环3.1 顶层架构四段信号链怎么连搭模型之前先想清楚信号流向。整个闭环分为四段力输入、导纳控制器、位置内环、质量块被控对象。力输入用一个常数或阶跃信号代替传感器读数导纳控制器根据外力算期望位置位置内环用PD控制器跟踪期望位置输出控制力给质量块质量块的位置和速度反馈回导纳控制器和内环。顶层建模步骤新建Simulink模型设置求解器为固定步长步长取0.001秒1 kHz。拖入一个Constant模块值设为0代表外力输入方便后面切换成阶跃或正弦信号。拖入一个MATLAB Function模块编写导纳控制算法。拖入一个Subsystem命名为PositionInnerLoop内部放PD控制器。拖入两个Integrator模块串联作为质量块被控对象输入是力输出是位置和速度。用Mux把位置和速度打包成状态向量反馈给导纳控制器。其中MATLAB Function模块是导纳控制的大脑输入是外力F_ext和当前状态x、v输出是期望位置x_d和期望速度v_d。位置内环接收x_d结合实际位置x计算控制力。3.2 导纳控制器的S函数实现可以直接抄的代码导纳控制器实现方式有两种MATLAB Function模块和手写Level-2 S函数。前者适合仿真验证后者适合代码生成和硬件部署。下面给一段MATLAB Function的实现逻辑完整可以直接复制到模块里。function [x_d, v_d] admittance_controller(F_ext, x_cur, v_cur, x_ref, params) % 导纳控制器外力 - 位置修正 - 期望轨迹 % 输入: % F_ext : 外部接触力 (N) % x_cur : 当前实际位置 (m) % v_cur : 当前实际速度 (m/s) % x_ref : 未修正的原始期望位置 (m) % params: 参数结构体 % 输出: % x_d : 修正后的期望位置 (m) % v_d : 修正后的期望速度 (m/s) % 参数解包 M_d params.M_d; % 目标惯量 (kg) B_d params.B_d; % 目标阻尼 (N*s/m) K_d params.K_d; % 目标刚度 (N/m) Ts params.Ts; % 控制周期 (s) persistent x_err v_err if isempty(x_err) x_err 0; v_err 0; end % 误差状态期望位置偏移量 acc_err (F_ext - B_d * v_err - K_d * x_err) / M_d; v_err v_err acc_err * Ts; x_err x_err v_err * Ts; % 修正后的期望轨迹 原始参考 导纳偏移 x_d x_ref x_err; v_d (x_d - x_prev) / Ts; % 近似速度实际用状态变量更稳 % 保存上一次位置用于速度近似也可以直接用v_err x_prev x_d;代码逻辑分三步第一步从外力算出加速度修正量基于当前导纳误差状态第二步用欧拉积分更新速度误差和位置误差第三步把位置误差叠加到原始参考轨迹上。注意v_d这里用差分近似只是为了兼容某些位置型内环如果你用的是速度型内环直接输出v_err加原始速度参考更合适。参数结构体在MATLAB工作区里定义params.M_d 1.0; % 目标惯量越大越重对外力响应越迟钝 params.B_d 20.0; % 目标阻尼决定能量耗散速度太小会振 params.K_d 100.0; % 目标刚度越大越硬外力下形变越小 params.Ts 0.001; % 控制周期 1kHz3.3 位置内环的参数匹配PD增益怎么和导纳参数搭配导纳控制器输出的是期望位置真正出力的是内环位置控制器。内环常用PD控制[ F K_p (x_d - x) K_d (v_d - v) ]但这里有一个原则性问题内环的刚度必须远大于导纳模型的等效刚度否则内环跟踪不上导纳给出的期望轨迹接触力就会出现滞后和振荡。K_p的初始值建议取导纳模型K_d的20到50倍K_d取内环临界阻尼即2sqrt(K_pm)。注意很多人把内环PD增益和导纳参数混在一起调。记住分工导纳参数定义期望的柔顺特性内环参数只负责能不能跟上指令。两者相差不到一个数量级时系统会变得极其难调。4. 参数调优与稳定性判断让程序不只是在仿真里能跑4.1 三个目标参数的物理意义和初始设定M_d、B_d、K_d三个参数不是随便给的它们的物理意义直接对应实际力控手感。K_d决定稳态柔顺量用恒力F推稳态位置偏差是F/K_d。K_d设小轻轻一碰就退开适合打磨、抛光K_d设大接触刚度高适合精密装配。初始设定建议先定K_d再反推B_d和M_d。B_d决定动态响应临界阻尼时B_d 2sqrt(K_dM_d)。B_d小于临界值系统会振荡表现为接触力上下波动B_d过大系统变得迟钝外力撤销后位置恢复慢。初始设定直接取临界阻尼值跑一次阶跃力响应看是否超调再微调。M_d决定高频响应的惯性感M_d大对外力的响应慢但高频扰动不容易穿透到位置环M_d小响应快但容易激励机械共振。质量块仿真里M_d的初始值取实际负载质量的1到2倍比较合理。推荐的参数调整顺序先调K_d得到稳态偏置再调B_d消除振荡最后调M_d优化响应快慢。顺序反了三个参数互相拉扯很难收敛。4.2 采样时间选择1kHz是底线不是推荐值导纳控制对采样时间的敏感度远超普通位置控制。原因是导纳回路里存在数值积分采样时间直接影响等效阻尼。一个经验公式Ts需要满足Ts 2B_dM_d / (B_d^2 - 4M_dK_d) 这样的稳定性条件实际工程中很少有人去解这个不等式直接按内环位置环采样时间的5到10倍来选导纳控制周期硬件上位置环跑1kHz导纳环跑100到200Hz这是常见做法。但需要注意如果导纳环和位置环用同一个固定步长跑步长就不能选太大。Simulink里用1e-3秒步长配合M_d设1、B_d设20前向欧拉法的导纳控制器基本稳定步长放宽到5e-3秒同样的参数可能出现小幅振荡。仿真里遇到高频抖动先看步长再调参数。4.3 稳定性验证方法阶跃响应和能量监视参数调完怎么确认系统真的稳定两种验证手段基本够用阶跃力响应和能量监视。阶跃力响应的做法是在外力输入端加一个Step模块幅值设为10N仿真时长2秒观察位置修正量x_err的响应曲线。理想的临界阻尼响应是位置快速到达稳态值F/K_d附近过程中不超调或只轻微超调然后保持不变。如果出现等幅振荡说明B_d太小如果响应慢吞吞说明M_d太大或B_d太大。能量监视则是把外力做的功和控制力做的功分别积分画在同一个坐标轴里。系统稳定时两条曲线差值趋于常数不会持续增长如果差值随时间线性增加说明有能量注入系统正在发展振荡。Simulink里用Integrator模块对Fdx/dt和F_controldx/dt做积分输出到Scope即可。5. 质量块导纳控制Simulink避坑现象、原因、解决5.1 导纳输出发散x_err直接飞到天上现象仿真运行几百毫秒后x_err的值线性增长不收敛。有些人在模拟恒力接触时遇到这个问题误以为导纳参数设错了。原因看x_err的更新方程它积分的是F_ext经过阻尼和刚度衰减后的加速度。如果F_ext一直存在且B_d和K_d的乘积远小于外力这个积分器会持续累积x_err必然线性增长。这不是控制器算错而是恒力下位置无界的物理事实——导纳模型本身是柔顺的持续外力必然持续位移。解决真实接触场景中外力不会无限存在接触过程是有界的。仿真时改用有限时长的外力脉冲比如幅值10N、持续0.5秒或者给x_err设置上下限饱和模块模拟机械限位。必要时检查是否把F_ext和期望力搞混了——导纳控制输入的应该是外部接触力不是期望接触力。5.2 位置内环和导纳环采样率不匹配高频抖动现象仿真曲线看起来正常但频谱上在某个频率有尖峰SPT工具看到明显的周期性振荡周期等于导纳环的更新周期。原因导纳环输出的x_d是一个阶梯状信号位置内环以更高频率跟踪阶梯目标时会产生高频激励。尤其是K_d设得大、位置环K_p再放大的场景阶梯跳变被内环放大形成抖动。解决在导纳控制器输出端加一阶低通滤波器截止频率设为导纳环更新频率的1/2到1/3。也可以用速率限制模块Rate Limiter限制x_d的变化率防止阶梯跳变。实际仿真中加低通更简单加在x_d后面时间常数取导纳周期的2到3倍。5.3 质量块模型不收敛积分器输出NaN现象仿真在某个时刻突然报错积分器输出NaN模型停止运行。原因一种情况是外力输入端接错了信号类型比如把Simulink中的无穷大或空值传进来另一种是代数环问题质量块的位置直接反馈到导纳控制器而导纳控制器又直接输出位置给内环形成没有延时的代数环。代数环在固定步长解算器下面有时候算不收敛。解决在反馈回路里插入一个单位延迟Unit Delay模块人为打破代数环。仿真层面的做法是在导纳控制器的输出端加一个Memory模块虽然会引入一个步长的延迟但稳定性显著改善。更彻底的办法是把被控对象的状态位置和速度用积分器内部的输出端口引出不要通过信号线做全反馈。5.4 参数不变改变仿真步长后结果天差地别现象同样的参数步长设0.5ms和1ms时位置响应幅值差20%以上。原因前向欧拉法的离散化误差随步长线性增长M_d设置越小误差影响越大。另外一个隐藏因素是Simulink的固定步长求解器如果选择ode1欧拉法本身精度低选择ode4四阶龙格库塔时虽然被控对象积分精度高但导纳控制器内部仍然是MATLAB Function里写死的欧拉法两者组合出的离散动力学和步长强相关。解决仿真阶段用ode4固定步长1ms导纳控制器内部也改用双线性变换离散化或者把导纳控制器也拆成积分器模块完成这样Simulink求解器统一用更高精度的方法处理。硬件部署时再变回固定步长1kHz的离散代码行为才能和仿真接近。5.5 换了复杂负载模型后原导纳参数全部失效现象把质量块替换成带弹簧阻尼的二阶负载模型后原来调好的M_d、B_d、K_d参数下系统出现明显振荡。有人误以为是导纳控制器写错了。原因导纳控制是基于位置内环理想跟踪的假设设计的。质量块模型下内环很容易做到理想跟踪换成柔性更大的负载模型后位置内环跟踪性能下降导纳控制器接收到的实际位置和期望位置之间不再是近似相等的关系外环的相位裕度被消耗振荡随之而来。解决换负载模型后先把内环PD增益提高保证内环跟踪带宽至少是导纳频率的5倍以上再调导纳参数。如果内环已经到极限仍然振荡需要降低K_d即降低外环对位置内环的期望刚度要求。6. 进阶验证与C代码部署从仿真到样机移植的四个细节仿真跑通只是第一步。把质量块导纳控制程序部署到实际控制器时有几个细节决定成败。第一个细节是信号类型。Simulink里一切正常但代码生成后力传感器的数据类型可能是int16需要先转换成double再做运算。很多人忽略这一步导致导纳控制器输出跳动。建议在模型入口处加Data Type Conversion模块统一转换成double避免隐式转换带来的精度损失。第二个细节是饱和处理。仿真里x_err可能跑到很大但真实系统的执行器有行程限制。代码生成前在导纳控制器输出端加Saturation模块上下限根据机械限位设定。这种保护逻辑在仿真里不起眼在硬件上能救一台设备。第三个细节是力传感器偏置。仿真里的F_ext 是干净的真实传感器的零点偏置和噪声会让导纳控制器在无接触时也产生位置漂移。部署时要在导纳控制器之前做力信号的低通滤波和零漂补偿。注意滤波器的截止频率要远大于导纳带宽但远小于传感器噪声频率一般取20到50Hz。第四个细节是验证方法。硬件上验证导纳参数是否和仿真一致最直接的办法是做自由空间下的力阶跃响应用手或砝码给末端一个瞬态力记录位置响应曲线和仿真曲线对比。如果上升时间一致稳态偏置一致说明模型和实物已经对齐如果偏置一致但上升时间明显变慢多半是低通滤波器引入了额外延迟需要重新匹配截止频率。最后说一个我自己的习惯每次调参只改一个量记录仿真曲线导出成图片标注参数值和日期存进项目文件夹。导纳控制参数之间的耦合很重靠记忆调参三天后就会忘掉哪个组合才是最优解。做力控没有捷径一个参数一个参数地试比什么都快。希望这篇文章能帮你在质量块导纳控制这条路上少走几个弯路。本文还有配套的精品资源点击获取