简介面向机器人控制与自动化专业学习者的MATLAB/Simulink仿真资料围绕三自由度工业机器人混合位置与力控制展开建模和验证适合本科生课程设计、期末大作业及毕业设计参考。资源共45个文件整个压缩包仅1.74MB以24个MATLAB脚本仿真源码为核心配合Simulink模型、STL三维模型文件、效果展示图片、说明文档及数据库文件覆盖从机器人运动学/动力学模型搭建到GUI交互控制的完整流程。目前已有289人学习使用。内容采用参数化编程并附有清晰注释可方便修改机器人连杆尺寸与控制器参数附赠案例数据可直接运行便于快速复现混合位置/力控制效果理解力控与位置控制之间的切换逻辑。对于需要完成机器人仿真实验或算法对比的读者是一份轻量且结构完整的学习素材。1. 接触任务里3 自由度机械臂为什么必须补上力控制这一环打磨、去毛刺、装配这类机器人任务难点从来不是把末端从 A 点移到 B 点而是末端必须贴在某个表面上既不能脱离也不能顶穿。纯位置控制在碰到约束面时位置误差会转化成接触力误差一个毫米力可能放大到一个无法接受的量级。3 自由度平面机械臂虽然结构简单但恰好是验证混合位置和力控制最便宜的试验台关节少、雅可比好写、Simulink 模型跑得快而控制思路和六自由度工业机器人完全一致。这篇文章会从任务空间的正交分解讲起给出平面 3R 机械臂在 Simulink 中做混合位置/力控制的完整搭建路径包括 Simscape Multibody 本体建模、接触力计算、控制器实现和参数整定最后聊几个从仿真往实机迁移时一定会踩到的坑。2. 混合位置和力控制的分解逻辑与平面3自由度任务空间雅可比2.1 位置子空间与力子空间为什么必须正交混合位置和力控制的核心不是同时反馈位置误差和力误差而是先把机械臂末端的任务空间拆成两个互补子空间一组自由度用来控制位置另一组自由度用来控制接触力。两个子空间必须互相正交否则位置误差会向力环泄漏力误差也会向位置环泄漏最终谁都收敛不了。以平面 3R 机械臂接触水平面为例。末端在二维平面里有三个自由度x 方向沿接触面切线可以控制位置y 方向垂直指向接触面法线应该控制接触力末端姿态 theta 与法线没有直接接触可以继续做位置控制。于是任务向量可以写成 r [x, y, theta]对应的选择矩阵为S diag([1, 0, 1])I - S diag([0, 1, 0])S 的作用是让位置控制器只管 x 和 theta(I-S) 让力控制器只管 y。这里最容易犯的错误是直接在关节空间里做类似的选择比如把第一个关节作为位置关节、第二个关节作为力关节这是不对的。接触任务发生在末端直角坐标系里关节空间的选择矩阵无法和前向运动学解耦必须先在任务空间里完成控制分量计算再用雅可比转置映射回关节力矩。2.2 用 MATLAB Function 写平面3R机械臂的雅可比矩阵平面 3R 机械臂的末端位置和姿态由三个关节角 q1、q2、q3 决定设三段臂长为 l1、l2、l3正运动学为x l1·cos(q1) l2·cos(q1q2) l3·cos(q1q2q3)y l1·sin(q1) l2·sin(q1q2) l3·sin(q1q2q3)theta q1 q2 q3对状态向量求偏导可以得到 3x3 的几何雅可比矩阵。这个矩阵在混合控制器里会被转置后使用因为它能把任务空间力映射成关节力矩。建议直接在 MATLAB Function 模块里实现正运动学和雅可比输入 q输出 r、dr 和 J状态干净后续调试方便function [r, J] planar3R_kinematics(q) % 平面3R机械臂正运动学与几何雅可比 % 输入 q: [q1;q2;q3] 单位弧度 % 输出 r: [x;y;theta] 末端位形 % 输出 J: 3x3 雅可比矩阵 l1 0.4; % 连杆1长度单位m l2 0.35; % 连杆2长度 l3 0.2; % 连杆3长度 % 角度和与三角中间量 q12 q(1) q(2); q123 q(1) q(2) q(3); c1 cos(q(1)); s1 sin(q(1)); c12 cos(q12); s12 sin(q12); c123 cos(q123); s123 sin(q123); % 正运动学 r [l1*c1 l2*c12 l3*c123; l1*s1 l2*s12 l3*s123; q123]; % 雅可比矩阵 J [-l1*s1 - l2*s12 - l3*s123, -l2*s12 - l3*s123, -l3*s123; l1*c1 l2*c12 l3*c123, l2*c12 l3*c123, l3*c123; 1, 1, 1]; end代码里的 J 第一行是末端 x 速度对各关节角速度的偏导第二行是 y 速度第三行是姿态角速度。这个矩阵后续在控制器里以 J 的形式参与计算转置后维度是 3x3能够把任务空间控制力向量映射为三个关节力矩。保持计算雅可比和正运动学在同一个函数里可以避免 Simulink 信号线上同时存在两组不一致的坐标。2.3 选择矩阵 S 的取值与接触面坐标系的关系上面给出的 Sdiag([1,0,1]) 只在接触面是水平面时成立。如果工件表面是斜面任务坐标系需要先绕世界坐标系旋转一个角度 alpha选择矩阵也应该变成旋转后的版本。常见做法是先在接触面局部坐标系里定义选择矩阵 S_local再通过旋转矩阵 R 转换到世界坐标系得到 S_world R * S_local * R力控制方向同样由 I-S_world 决定。在 Simulink 仿真里我一般把 S 和 I-S 作为常量矩阵填入控制器函数的参数区而不是在函数内部硬编码。这样后续做优化或者切换接触面方向时只需要改模型工作区的变量不用重新打开编辑页面。3. 在 Simulink 中用 Simscape Multibody 搭建 3 自由度机器人接触仿真3.1 Simulink模型信号流与模块清单混合位置/力控制仿真模型从宏观看分四层指令源、混合控制器、被控机械臂、接触力反馈。指令源输出期望位置 x_des、theta_des 和期望接触力 F_des控制器根据反馈计算关节力矩力矩驱动 Simscape Multibody 里的机械臂模型机械臂的关节传感器把 q 和 dq 送回控制器同时正运动学模块根据 q 算出末端位形和雅可比。接触力不直接来自 Simscape 接触库而是用一个独立模块计算末端穿透水平面的深度和法向速度映射为接触力再通过空间力执行器施加到末端。这种做法的好处是接触刚度可控也方便单独观测穿透量。整个 Simulink 模型的信号线不需要经过微分环节因为雅可比已经有了速度映射能减少微分噪声。3.2 用 Simscape Multibody 搭 3 自由度平面机械臂新建 Simulink 模型从 Simscape Multibody 库拖入 World Frame 和 Mechanism Configuration按平面机器人配置把重力方向设为零或者保持重力但让所有连杆在水平面内运动。每个连杆用 Solid 模块定义几何和质量用 Rigid Transform 定义关节坐标系相对连杆质心的位置用 Revolute Joint 连接相邻连杆。Joint Actuator 模块接在每个 Revolute Joint 的输入端用来接收混合控制器输出的关节力矩。Joint Sensor 接在输出端采集关节角度和角速度输出信号经过 Simulink-PS Converter 转换后分别送给运动学模块和控制器。三根连杆的具体尺寸、质量和惯量可以参考下面的初始参数表实际项目以三维模型为准。连杆长度(m)质量(kg)绕 Z 轴惯量(kg·m²)关节阻尼(N·m·s/rad)连杆10.402.00.0270.1连杆20.351.50.0150.1连杆30.200.80.0030.1关节阻尼如果设得过大力环看起来非常稳定但位置响应会变慢如果设为零仿真在接近接触时又容易高频振荡。先按 0.1 起步控制参数整定结束之后再折回来降低。3.3 接触力计算把穿透量转换为末端外力混合位置/力控制需要实时反馈末端接触力。这里用一个简化模型认为接触面是刚性平面 y0机械臂末端点低于这个平面即为穿透法向接触力与穿透深度和穿透速度成正比。接触力函数写在 MATLAB Function 模块里输入末端位形 r 和速度 dr输出 F_tip单位是 Nfunction F_tip contact_force(r, dr) % 接触力模型水平面 y0 上方为自由运动下方为穿透 % 接触刚度 Kc 和接触阻尼 Dc 在工作区定义 y r(2); % 末端在世界坐标系中的y坐标 vy dr(2); % y方向速度向下为正 Kc 5000; % 接触刚度 N/m Dc 20; % 接触阻尼 N·s/m if y 0 % 穿透量 penetration -y F_tip [0; -Kc*y - Dc*vy; 0]; else F_tip [0; 0; 0]; end end注意 F_tip 的方向是接触面对末端的反作用力所以计算出来为正表示末端受到向上的推力。如果接触面不是 y0 而是任意平面需要先做坐标变换把末端坐标转换到接触面局部坐标系计算法向穿透量之后再把力旋转回世界坐标系。3.4 固定步长求解器配置与仿真启动混合力控制模型中同时存在连续动力学和接触切换变步长求解器在接触瞬间容易自动缩短步长导致仿真时间被拖长。建议在模型配置面板里把求解器设置为 ode4 固定步长步长取 1e-3 秒仿真时长 5 到 10 秒。如果接触力出现明显的高频尖峰第一步先把步长降到 1e-4看是否与积分步长有关再回头调接触阻尼。机械臂初始位形要避开接触面。比如让末端起点在 y0.02 米以上位置指令落在接触面以下这样机械臂会先做位置运动接触后再切换到力控模式。如果在初始时刻就已经穿透接触面力环一开始就要对抗一个很大的接触力控制器容易发散。4. 混合位置/力控制器实现与 4 个必调参数4.1 控制器的 Simulink 实现混合位置/力控制函数控制器核心是一个 MATLAB Function 模块输入信号包括关节状态 q、dq末端位形 r、速度 dr接触力 F_tip以及力误差积分项 integral_F输出关节力矩 tau。位置子空间只包含 x 和 theta力子空间只包含 y控制律写成任务空间力合成再通过雅可比转置映射回关节空间function tau hybrid_pos_force(q, dq, r, dr, F_tip, integral_F) % 混合位置/力控制器 % 位置子空间x与theta力子空间y % 控制参数和期望值都来自工作区常量 % 选择矩阵位置控制Sdiag([1,0,1])力控制I-Sdiag([0,1,0]) S diag([1 0 1]); I_S eye(3) - S; % 末端位形速度和期望值 x_des 0.6; % 期望x位置m theta_des 0.3; % 期望末端姿态rad F_des 10; % 期望接触力N % 位置误差和速度误差向量y分量会被S忽略 e_pos [x_des - r(1); 0; theta_des - r(3)]; e_vel [0 - dr(1); 0; 0 - dr(3)]; % 位置子空间PD控制Kp_pos和Kd_pos为对角矩阵 Kp_pos diag([800 0 600]); % N/m 和 Nm/rad Kd_pos diag([80 0 60]); % N·s/m 和 Nm·s/rad F_pos S * (Kp_pos * e_pos Kd_pos * e_vel); % 力子空间PI控制Kp_force无量纲Ki_force单位1/s F_err F_des - F_tip(2); F_force I_S * [0; F_des 0.5 * F_err 20 * integral_F; 0]; % 任务空间力合成 F_task F_pos F_force; % 雅可比转置映射到关节力矩 T planar3R_kinematics(q); tau T * F_task; end控制函数里没有包含关节重力补偿原因是机器人默认在水平面内运动重力与运动平面垂直。如果机械臂做竖直平面运动必须把重力矩加到 tau 上否则力环稳态会产生一个由机器人自重引起的偏置误差。4.2 4 个必调参数与初始值表这套控制器真正需要调整的参数其实不超过两组位置环的 PD 增益力环的 PI 增益再加上接触模型的刚度和阻尼。位置环增益决定机械臂能否快速到达接触点力环增益决定接触建立后的力跟踪质量。初始值可以按下面的表设置接触刚度只影响接触力计算模块不属于控制器但对收敛速度影响很大。参数符号含义初始值Kp_posx方向位置刚度800 N/mKd_posx方向位置阻尼80 N·s/mKp_pos_theta姿态角刚度600 Nm/radKd_pos_theta姿态角阻尼60 Nm·s/radKp_force力环比例增益0.5Ki_force力环积分增益20 1/sKc接触刚度5000 N/mDc接触阻尼20 N·s/m力环的比例增益看起来不大是因为它作用在力误差上误差 10N 乘以 0.5 只有 5N 的前馈修正量再加上 10N 前馈总力需求为 15N这对接触过程比较温和。如果一开始就把 Kp_force 调到 2 以上接触瞬间力响应会像敲击一样末端弹跳明显。4.3 用 Scope 和 MATLAB 脚本观察位置、力响应仿真跑完后除了看 Scope我通常会把响应数据导出到工作区用脚本同时画位置误差和接触力波形。这样可以精确计算超调量和稳定时间而不是靠眼睛判断曲线平不平% 假设Simulink中勾选了单输出格式tout和yout在工作区 figure; subplot(2,1,1); plot(tout, yout(:,1), LineWidth, 1.2); hold on; plot(tout, yout(:,2), LineWidth, 1.2); legend(期望位置,实际位置); ylabel(x 位置(m)); title(位置子空间响应); subplot(2,1,2); plot(tout, yout(:,3), LineWidth, 1.2); hold on; plot(tout, yout(:,4), LineWidth, 1.2); legend(期望力,实际接触力); ylabel(F_y (N)); xlabel(时间(s));要确保你从 Scope 或输出端口记录的变量顺序与绘图变量一致否则位置和力画反了会完全误导整定。建议在模型里用 Outport 复用一个 Bus把 q、r、F_tip 显式打出来比记住列号持久得多。4.4 仿真发散的常见原因代数环、积分饱和、接触刚度整个闭环最容易出现的是高频振荡而不是完全不收敛。高频振荡多数来自接触刚度和力环增益一起拉高了系统自然频率可以把 Kc 从 5000 降到 2000同时把 Kp_force 下调 20%通常能把振荡压住。位置环的代数环很少出现因为雅可比和正运动学都在同一个 MATLAB Function 里运动学函数没有隐式依赖当前时刻的 tau控制器反馈回路里有积分器作为连续状态。力环的积分器如果直接在 Simulink 里用连续积分模块接触瞬间力误差跳得很大积分项在几毫秒内就会饱和导致力超调后长时间回不到期望值。解决办法是在积分器里设置输出限幅建议限在 -50 到 50或者在进入积分器之前对 F_err 做饱和处理。这个限幅值不是控制器的期望输出限幅而是积分器本身的抗饱和保护两者要分开看。5. 从仿真到实机接触刚度、平滑过渡和量化验证技巧5.1 接触刚度、阻尼与力环带宽的匹配Simulink 里用一个线性弹簧模型模拟接触面时接触刚度 Kc 决定了接触状态下的等效闭环刚度。Kc 越大末端法向方向的位置细微变化就会产生很大的力波动力环比例增益必须相应减小。经验上可以先用启动机器人本体的自然频率做参考如果把 Kc 换算成末端集中质量 m 下的弹簧频率 sqrt(Kc/m)力环的期望响应带宽不要超过这个频率的 1/5。否则控制器是在一个已经被接触刚度抬高了很多倍的系统上继续加增益结果必然是振荡。5.2 用平滑选择矩阵过渡到纯力控制初始位置在接触面以上时整个末端应该先做纯位置控制等接触建立之后再把法线方向切换到力子空间。直接在某一帧把 S 从单位矩阵切换到 diag([1,0,1])相当于给系统注入一个阶跃干扰接触力会出现明显尖峰。常见做法是用一个与接触力单调相关的系数 alpha 来连续过渡alpha 从 0 平滑变为 1S diag([alpha, 1-alpha, alpha])。alpha 越接近 1位置控制越强接触力建立后 alpha 退下来力控接管。这个过渡过程只要持续 0.2 秒左右力波形比硬切换平滑得多。5.3 量化验证位置 RMSE 与力超调混合位置/力控制做好之后评价标准不是曲线好不好看而是三个数字位置子空间的稳态误差、力环超调量、力稳态误差。在 MATLAB 里可以用脚本取接触段的数据计算% 提取仿真结果 tout, r, Ftip contact_start find(tout 2, 1); % 接触后取稳定段 pos_rmse sqrt(mean((x_ref(contact_start:end) - x_act(contact_start:end)).^2)); force_ov max(Ftip(contact_start:end)) / F_des - 1; force_ss_err abs(mean(Ftip(end-500:end)) - F_des);这三个指标可以直接作为混合控制是否收敛的准入门槛位置 RMSE 在毫米量级力超调小于 20%力稳态误差在 0.5N 以内对于打磨类任务已经可以用。后续如果再调接触刚度或机械臂参数回归验证也必须重新跑这三个数才能知道改动是变好还是变坏。本文还有配套的精品资源点击获取