简介面向机器人控制与动力学建模学习者的二关节机械臂计算力矩控制Simulink程序包以典型的二自由度旋转关节机械臂为对象演示基于逆动力学算法的计算力矩控制策略帮助理解非线性耦合系统的建模与实时控制难点。资源共11个文件包含2个Simulink模型slx和9个MATLAB脚本m分别对应Tracking Control与Tracking Control Sin(x)两种跟踪控制方案可运行仿真并观察关节角度、速度与力矩响应也可修改参考轨迹和控制器参数对比性能差异。压缩包体积仅31KB结构紧凑适合机器人控制初学者快速上手。已有2360人学习下载说明该资源在机器人控制学习群体中具有较高参考价值。通过该程序可系统掌握机械臂动力学模型构建、惯性矩阵与驱动力矩计算、控制器参数调整、Simulink积分器设置等关键技能为深入理解计算力矩控制及先进机器人控制方法打下基础。1. 二关节机械臂计算力矩控制与Simulink实现换掉纯PID后的实际效果二关节机械臂的计算力矩控制Simulink程序解决的是一个很具体的问题当关节之间存在动力学耦合时独立PD/PID控制很容易顾此失彼。我在某高校实验室调一台桌面级二关节臂时空载状态下独立PD的跟踪误差在1°以内末端挂上负载后关节2的误差直接翻倍怎么加Kp都压不回来。换用这套计算力矩控制程序后相同PD参数在空载和满载两种条件下末端误差曲线几乎重叠。这是因为程序把拉格朗日动力学模型做成了前馈补偿把非线性系统在反馈环里修正为线性二阶系统。这套Simulink工程从参数初始化、五次多项式轨迹规划到控制律S函数全部手写不依赖机器人工具箱适合正在学机械臂控制和机器人控制的开发者直接跑通也适合做二次开发。2. 控制原理与架构设计从拉格朗日方程到前馈加反馈的完整链路2.1 计算力矩控制的核心思想把非线性“算清楚”而不是“压下去”先说一个独立PD控制里常见的现象你给关节1加了增益关节2一动关节1还是跟着偏再加增益系统开始振荡。这不是增益不够而是两个关节之间存在科里奥利力和向心力耦合这种耦合与当前角度、角速度强相关PD环只能把耦合当作一种持续变化的扰动来“硬扛”。n自由度机械臂的动力学方程可以写成标准形式M(q) * q_ddot C(q, q_dot) * q_dot G(q) tau其中M(q)是惯性矩阵C(q, q_dot)q_dot是科里奥利力和向心力项G(q)是重力项。这三者都是关于状态的非线性函数。如果动力学模型精确已知就可以设计控制律tau M(q) * u C(q, q_dot) * q_dot G(q)代入动力学方程后M(q)q_dot和C(q, q_dot)q_dot两项被完全抵消剩下的关系是q_ddot u。一个原本耦合的非线性系统被变成了一组解耦的双积分器。外环再取u qd_ddot Kd * (qd_dot - q_dot) Kp * (qd - q)闭环误差方程就变成e_ddot Kd * e_dot Kp * e 0。这意味着两个关节的控制问题从“调两个互相打架的PID”变成了“配置两个二阶系统的极点”。Kp和Kd只要取正定对角矩阵两个关节就完全解耦这也是计算力矩控制也叫逆动力学控制的原因——把逆动力学算成前馈而不是靠反馈去压。2.2 拉格朗日模型二关节的M、C、G具体长什么样这套程序里的被控对象是标准二连杆平面机械臂每个连杆用质量m、长度l、质心距关节距离lc、绕质心转动惯量I四个参数描述。拉格朗日方程推导出的惯性矩阵各元素如下M11 I1 I2 m1 * lc1^2 m2 * (l1^2 lc2^2 2 * l1 * lc2 * cos(q2)) M12 I2 m2 * (lc2^2 l1 * lc2 * cos(q2)) M22 I2 m2 * lc2^2可以看到M12里含有cos(q2)这就是两个关节之间的惯性耦合项。当q2 0时耦合最强当q2接近π时M矩阵接近奇异仿真时不要从这类位形起步。科里奥利和向心力项合并后写成C(q, q_dot) * q_dot [ -m2 * l1 * lc2 * sin(q2) * (q_dot2^2 2 * q_dot1 * q_dot2); m2 * l1 * lc2 * sin(q2) * q_dot1^2 ]重力向量G1 (m1 * lc1 m2 * l1) * g * cos(q1) m2 * lc2 * g * cos(q1 q2) G2 m2 * lc2 * g * cos(q1 q2)程序里所有动力学计算都是这几个表达式在MATLAB函数中的直接翻译。参数表如下全部使用国际单位制参数含义单位m1 / m2连杆1/2质量kgl1 / l2连杆1/2长度mlc1 / lc2连杆质心到关节距离mI1 / I2连杆绕质心转动惯量kg·m²g重力加速度m/s²2.3 控制回路整体结构轨迹规划、动力学补偿与反馈三层整个模型在信号流上分三层。上层是轨迹规划把用户给的关节角起点q0和终点qf变成随时间变化的期望轨迹qd、期望速度qd_dot、期望加速度qd_ddot。程序里用五次多项式因为它有6个系数恰好满足位置、速度、加速度起止共6个边界条件加速度不会阶跃。三次多项式只有4个系数加速度在起止时刻会跳变仿真中会看到力矩突变物理上对应机械冲击。中间层是计算力矩控制律接收机械臂反馈的角度、角速度以及期望轨迹三路信号按照上面推导的公式计算出控制力矩tau。最下层是二连杆被控对象tau进去经过正动力学计算得到q_ddot再积分两次得到q和q_dot反馈回中间层。整个闭环就是“期望轨迹生成 - 前馈加反馈控制律 - 被控对象正动力学 - 状态反馈”。和独立PD相比核心差异是负载变化。独立PD要靠误差来克服负载力矩误差变大是必然的计算力矩控制里负载变化体现在M矩阵和G向量中只要模型参数更新前馈就自动把负载部分补偿掉PD环只需要处理很小残差。3. Simulink工程结构拆解压缩包里每个文件在模型里干什么3.1 解压后的文件清单与运行顺序压缩包解压后是一组互相配合的MATLAB/Simulink文件不是我常见的“一个slx走天下”那种工程。文件职责如下表文件名职责是否需要改init_params.m机器人动力学参数、控制增益、轨迹参数统一初始化必改gen_traj.m五次多项式轨迹生成函数被模型内模块调用按需ctc_sfun.m计算力矩控制律的Level-2 M文件S函数不建议改plant_sfun.m被控对象正动力学S函数按需ctc_controller.slx顶层仿真模型包含S函数、积分链、示波器必看run_demo.m一键初始化并运行仿真可选draw_result.m从工作区读取仿真结果绘制误差曲线可选运行顺序很重要先执行init_params.m让m1、m2、l1、l2、Kp、Kd、q0、qf这些变量进入base workspace再打开ctc_controller.slx运行仿真。如果跳过第一步直接运行模型S函数里evalin(base)读变量时就会报错。run_demo.m把这一步做成了自动化脚本方便你验证整套流程% 一键运行初始化参数 - 设置停止时间 - 运行模型 - 绘图 init_params; % 将参数写入base工作区 open(ctc_controller.slx); set_param(ctc_controller, StopTime, num2str(tf)); sim(ctc_controller.slx); % 执行仿真 draw_result; % 绘制跟踪误差曲线这里set_param直接改模型停止时间为tf对应的数值避免手动去Configuration Parameters里来回填。sim命令是在脚本中运行Simulink模型的标准方式仿真结束后结果变量自动回到工作区。3.2 顶层模型里的信号流从Clock到Scope的七个关键模块打开ctc_controller.slx模型的信号流向从左侧的Clock开始。Clock产生连续时间信号进入一个MATLAB Function块块内调用gen_traj.m输出三路2×1向量信号分别是期望角度、期望角速度、期望角加速度。这三路信号进入控制律S函数的第3、4、5号输入端口。被控对象plant_sfun输出的实际q和q_dot则反馈到S函数的第1、2号端口。S函数输出2×1的控制力矩接到plant_sfun的输入形成闭环。模型的右侧接有Mux和Scope同时用To Workspace把角度误差和控制力矩导出到工作区方便脚本画图。S函数采用5个输入端口、1个输出端口的布局每个端口都是2维列向量。我第一次看这个设计以为会拆成6个标量口实际上用向量端口的好处是后面想扩展到三关节只需要把Dimensions从2改成3轨迹生成函数和控制律内部矩阵维度一起改就行不用在模型里加一堆连线和Mux扩展成本低很多。3.3 被控对象子系统的搭建两个积分器组成二阶链被控对象不是直接用传递函数而是用正动力学计算。给定tau先算q_ddot M^-1 * (tau - C * q_dot - G)。Simulink里典型的搭法是两个级联积分器第一个积分器输入q_ddot输出q_dot第二个积分器输入q_dot输出q。在plant_sfun.m里Outputs方法负责计算状态导数核心逻辑如下% 正动力学由控制力矩和当前状态求角加速度 function Outputs(block) q block.InputPort(1).Data; % 当前角度 2x1 qd block.InputPort(2).Data; % 当前角速度 2x1 tau block.InputPort(3).Data; % 控制力矩 2x1 M compute_M(q, block.UserData); C compute_C(q, qd, block.UserData); G compute_G(q, block.UserData); qdd M \ (tau - C * qd - G); % 左除代替求逆数值更稳 block.OutputPort(1).Data qdd; % 接第一个积分器 endM矩阵的求解用了左除运算符“\”而不是inv(M)乘过去。2×2矩阵直接求逆问题不大但这是好习惯——矩阵维度变大或者遇到接近奇异的位形时左除的数值稳定性更好。3.4 仿真求解器配置变步长是连续S函数的最佳搭档模型里S函数的采样时间设置为[0 0]代表连续时间所以求解器不要选固定步长。建议使用ode45变步长求解器最大步长限制在0.01秒。原因是控制律里有强非线性项sin(q2)和cos(q1q2)在关节高速摆动时信号变化剧烈固定大步长容易漏掉尖峰导致误差计算不准确甚至发散。相对误差保持1e-3默认值即可不需要收太紧否则仿真速度会明显变慢。仿真停止时间直接取自tf变量默认2秒。想要看更长的轨迹可以改init_params.m里的tf和qf再重新执行脚本。一个容易被忽略的点是两个积分器的初始条件位置积分器初始值要设成q0速度积分器初始值设成[0;0]且q0必须与轨迹生成函数里的起点一致。不一致的话仿真第一拍误差就是跳变的控制力矩会出现一个很大的尖峰。4. 核心实现逐段拆解参数脚本、轨迹生成器与控制律S函数4.1 init_params.m所有参数集中管理整个工程里最值得养成习惯的是参数集中管理。init_params.m里把动力学参数、控制增益、轨迹参数全部写在一个脚本里模型和S函数都从base workspace读而不是在模型里硬编码数字。%% 机械臂动力学参数单位全部为国际单位制 m1 2.0; % 连杆1质量 kg m2 1.2; % 连杆2质量 kg l1 0.6; % 连杆1长度 m l2 0.5; % 连杆2长度 m lc1 0.3; % 连杆1质心到关节1距离 m lc2 0.25; % 连杆2质心到关节2距离 m I1 0.12; % 连杆1绕质心转动惯量 kg*m^2 I2 0.08; % 连杆2绕质心转动惯量 kg*m^2 g 9.81; % 重力加速度 m/s^2 %% 计算力矩控制的外环PD增益按临界阻尼整定 wn 10; % 期望闭环自然频率 rad/s Kp wn^2 * eye(2); % 50 * I Kd 2 * wn * eye(2); % 20 * I %% 五次多项式轨迹参数 q0 [0; 0]; % 初始关节角 rad qf [pi/2; pi/3]; % 目标关节角 rad tf 2.0; % 过渡时间 s增益参数不是拍脑袋给的。把闭环误差方程e_ddot Kd * e_dot Kp * e 0看成一个二阶系统Kp wn²Kd 2 * wnwn取10意味着闭环带宽约10 rad/s。这个值对低速桌面机械臂是稳妥的想要更快的跟踪响应再往上加。注意所有力臂参数都用国际单位制质量用kg长度用m转动惯量用kg·m²。如果读者手头有实际机械臂的数据手册只要把这几个常量替换掉仿真结果就有参考意义。4.2 gen_traj.m五次多项式轨迹生成轨迹生成函数是模型上方最左侧MATLAB Function块调用的核心函数。输入当前时间t、起点q0、终点qf和总时间tf输出该时刻的期望位置、速度、加速度。function [q, qd, qdd] gen_traj(t, q0, qf, tf) % 五次多项式轨迹起止速度、加速度均为0 if t 0 tau 0; elseif t tf tau 1; else tau t / tf; end % 五次多项式标准系数 10tau^3 - 15tau^4 6tau^5 q q0 (qf - q0) * (10*tau^3 - 15*tau^4 6*tau^5); % 速度注意除以tf得到对真实时间的导数 qd (qf - q0) * (30*tau^2 - 60*tau^3 30*tau^4) / tf; % 加速度连续且起止时刻为0 qdd (qf - q0) * (60*tau - 180*tau^2 120*tau^3) / tf^2; end这里的边界条件是位置、速度、加速度在t0和ttf都满足约束6个系数正好解6个方程。代码里的tau是归一化时间在0到1之间速度表达式里除以tf是把对tau的导数转换成对真实时间的导数。函数开头做了t 0和t tf的饱和处理原因是Simulink仿真的求解器有时会在停止时间之后多走一步或者因为变步长自适应把采样点略过边界没有饱和保护就会算出轨迹范围之外的数值控制力矩跟着出问题。4.3 ctc_sfun.m计算力矩控制律的S函数实现控制律本身是一个Level-2 M文件S函数文件名为ctc_sfun.m。它的setup方法里配置了5个输入端口和1个输出端口每个端口都是2维。Start方法里从base workspace读取一次参数缓存到UserData避免Outputs里反复evalin影响仿真速度。function ctc_sfun(block) setup(block); function setup(block) block.NumInputPorts 5; block.NumOutputPorts 1; block.SetPreCompInpPortInfoToDynamic; block.SetPreCompOutPortInfoToDynamic; % 输入端口顺序q, q_dot, qd, qd_dot, qd_ddot for i 1:5 block.InputPort(i).Dimensions 2; block.InputPort(i).DirectFeedthrough true; end block.OutputPort(1).Dimensions 2; block.SampleTimes [0 0]; % 连续时间 block.RegBlockMethod(Start, Start); block.RegBlockMethod(Outputs, Outputs); function Start(block) % 仿真开始时读取一次参数缓存进UserData block.UserData.m1 evalin(base, m1); block.UserData.m2 evalin(base, m2); block.UserData.l1 evalin(base, l1); block.UserData.lc1 evalin(base, lc1); block.UserData.lc2 evalin(base, lc2); block.UserData.I1 evalin(base, I1); block.UserData.I2 evalin(base, I2); block.UserData.g evalin(base, g); block.UserData.Kp evalin(base, Kp); block.UserData.Kd evalin(base, Kd); function Outputs(block) q block.InputPort(1).Data; % 实际角度 qd block.InputPort(2).Data; % 实际角速度 qd_des block.InputPort(3).Data; % 期望角度 qd_dot_des block.InputPort(4).Data; % 期望角速度 qdd_des block.InputPort(5).Data; % 期望角加速度 ud block.UserData; M compute_M(q, ud); C compute_C(q, qd, ud); G compute_G(q, ud); e qd_des - q; % 位置误差 ed qd_dot_des - qd; % 速度误差 u qdd_des ud.Kd * ed ud.Kp * e; % 外环线性控制律 tau M * u C * qd G; % 前馈 线性化补偿 block.OutputPort(1).Data tau; endDirectFeedthrough必须全部设为true因为Outputs方法直接由当前输入计算出控制量涉及q、q_dot、全部期望轨迹不存在纯积分状态延迟。如果改成falseSimulink会报代数环错误或者直接警告。compute_M、compute_C、compute_G是这个文件下半部分的局部函数里面从ud结构体取参数翻译2.2节中的表达式。这里不在每个局部函数里再evalin(base)而是用传递UserData的方式仿真性能会好很多也方便以后把参数改成结构体统一管理。4.4 初始状态与模型参数的一致性约定这套程序里有一个容易翻车的约定init_params.m里的q0同时被两处消费一处是轨迹生成器的起点另一处是被控对象积分器的初始条件。通常的做法是在init_params.m最后追加两行q_init q0; % 积分器位置初始条件 qd_init [0; 0]; % 积分器速度初始条件然后在Simulink积分器块的Initial Condition里填q_init和qd_init。这样做的好处是改轨迹时只需要动init_params.m一处不会出现轨迹起点是0.5但机械臂初始角度是0这种隐蔽错误。我在第一次跑这个模型时就是在两个地方手写了不同的初始角度结果示波器里看跟踪曲线好像没问题看工作区数据才发现初始误差跳变很大控制力矩一开始就有一个尖峰。5. 常见问题排查与参数调优四条实战避坑记录5.1 现象仿真一开始就发散控制量直接打到1e10这是计算力矩控制新手最常遇到的情况。表现是Scope里tau曲线第一拍就到10的8次方量级随后机械臂角度绕了好几圈数值直接变成NaN。原因通常是三类。第一类是M矩阵接近奇异当q2接近π时M12里的cos(q2)趋近-1矩阵行列式很小左除M \ (tau - C*qd - G)的结果会被放大。第二类是初始状态和轨迹起点不一致导致第一拍误差就是0.5 rad量级Kp * e直接产生巨大力矩。第三类是控制律里使用的质量参数和plant里不一致前馈方向错了系统被推离而不是拉向期望轨迹。排查办法是先做“开环前馈测试”把Kp和Kd临时设成零矩阵只保留前馈项。如果模型参数一致前馈应该让机械臂大致沿着期望轨迹走误差不消失但系统不发散。如果这样还发散问题一定在模型参数或初始状态而不是增益。确认前馈正确后再恢复增益。5.2 现象跟踪误差稳定在0.02 rad不消失加大Kp也没有效果这种误差曲线看起来已经跟随上了但有一条固定偏移PD增益加到很大也只是从0.02变成0.015再加大就开始振荡。原因是计算力矩控制的本质缺陷它完全依赖模型。当控制律里的m2和实际被控对象的m2不一致时前馈补偿不完整线性化不彻底闭环就不再是标准的二阶系统而是带有参数失配扰动的系统。很多人在init_params.m里把m2改成1.2但plant_sfun里还写死着1.5两边数字不一致误差自然降不下来。解决方法是把两端参数统一成同一个脚本来源。我在工程里始终保留一个原则控制律和对象模型只使用同一组init_params.m里的变量任何参数只在一处定义。如果刻意想测试鲁棒性就在脚本里加一个delta参数比如把控制律的m2设为m2 * (1 0.05)单独看误差恶化程度而不是无意中制造模型失配。5.3 现象速度反馈毛刺大Kd一加大整个系统开始抖示波器里q_dot曲线有明显的锯齿状毛刺控制力矩跟着出现高频抖动Kd加大后抖动越来越强最后变成啸叫式振荡。原因是速度信号来源。很多版本的程序里为了省事直接用Derivative块对q做数值微分或者S函数内部用差分近似速度。量化噪声和离散舍入误差都被Kd放大Kd越大噪声放大越凶这就是“速度反馈毛刺被增益放大”的典型表现。解决方法是不要在反馈回路里用纯数值微分。我通常会在速度反馈支路串一个一阶低通滤波传递函数写成1 / (0.02 * s 1)。滤波器会引入相位滞后速度增益Kd可以适当加大一档补偿滞后。想要更干净的方案可以改成速度观测器或者卡尔曼滤波但那个工程量对二关节仿真来说不太值当。5.4 现象换一组轨迹参数后仿真结果突然变得不正常把qf从pi/2改成pi或者把tf从2秒改成0.5秒跟踪误差变差甚至后半段开始振荡。同一个控制器为什么换个任务就拉胯。原因是轨迹越远、时间越短关节速度和加速度越大需要的力矩峰值也越大。固定增益的PD外环在这种条件下提供的修正能力不够而系统在高加速度下激励出的非线性耦合更强之前调好的增益就显出局限了。另一个隐蔽原因是当tf过大时误差项里速度误差在加减速段几乎为零位置误差被压得很小但模型静差还是会通过重力项G暴露出来。解决方法是按二阶系统极点来重新整定不要沿用旧参数。给定期望的自然频率wn和阻尼比zetaKp直接等于wn²Kd等于2 * zeta * wn。我一般做个批量扫描wn从5到20每5取一个值跑一遍仿真记录RMS误差用脚本而不是肉眼挑参数result []; for wn 5:5:20 Kp wn^2 * eye(2); Kd 2 * wn * eye(2); sim(ctc_controller.slx); result [result; wn, rms(err1), rms(err2)]; end disp(result);这样跑完就能看到wn对两个关节误差的影响趋势。注意每个wn跑完后控制律S函数里的UserData是在Start阶段缓存的必须重新仿真才能读到新的Kp、Kd直接在命令行改工作区变量而不重新运行sim是无效的。提示排查这类问题时先确认base workspace里有没有m1这个变量再确认S函数里Start方法的evalin能取到值。我遇到过好几次改了init_params.m但没重新执行导致模型里用的还是旧参数的情况。6. 二次开发改造负载补偿、摩擦项与离散化三个方向6.1 三个值得动手改的方向第一末端负载补偿。把m2从固定值改成m2_base m_load在init_params.m里增加一行m_load 0.5这样就可以模拟机械臂抓取不同重量物体的场景。这个改动只需要改脚本和compute_M、compute_G里的参数引用其他部分不用动。负载变化正是计算力矩控制相对独立PD的优势展示场景适合做对比实验。第二加入摩擦项。实际关节摩擦存在库仑摩擦和粘滞摩擦力矩可以写成tau_f fv * q_dot fc * sign(q_dot)。注意必须在控制律和plant两个方向同时加入这一项否则控制律里补偿了摩擦但plant里没有又变成模型失配。在compute函数里增加输入参数UserData里增加fv和fc两个字段即可。第三离散化。如果目标是在真实控制器上运行必须把连续S函数改成离散版本。把SampleTimes改成[1e-3 0]在Update方法里做状态记录在Outputs方法里用上一拍的控制律计算torque。离散化后控制频率就是1kHz和实际嵌入式控制周期一致增益也要重新按离散二阶系统整定。6.2 如何验证改造效果我的习惯是保留一份原版模型的仿真结果作为基线。改之前先跑一遍原版把两个关节的误差信号用To Workspace存下来改完之后用同一组增益和轨迹再跑一遍计算RMS误差对比。如果加了负载补偿空载和满载各跑一组重点看满载误差相对空载是否明显增加增加幅度大说明前馈没有正确更新。如果加了摩擦项重点观察低速段低速时库仑摩擦是主导项误差曲线在换向点附近是否有明显变差的“死区”特征。6.3 一个参数化技巧把每次仿真要变的参数都收进init_params.m用结构体统一管理会比散落变量更稳。我曾经为了省事用set_param批量改增益结果有一次改错了S函数块的参数名字仿真在示波器里看起来挺漂亮一看工作区数据才发现增益根本没用上。从那以后我每次跑仿真前都先单步执行一遍init_params.m把变量列表对着模型里用到的名字核对一遍再点运行。这个习惯帮我省掉了大半的翻车时间也逼着我把参数源统一成一个脚本。希望帮到你。本文还有配套的精品资源点击获取