简介：面向机器人控制学习者与工程技术人员的二关节机械臂计算力矩控制Simulink仿真程序，基于逆动力学模型实时求解关节驱动力矩，实现末端轨迹的高精度跟踪控制。压缩包一共包含11个文件，其中有2个slx仿真模型（分别对应Tracking Control和Tracking Control Sin(x)两种不同的参考轨迹场景）和9个m脚本，这些脚本用于设定机械臂物理参数、计算惯性矩阵、初始化控制器参数以及绘制仿真结果曲线，整个压缩包仅31KB，下载后可在MATLAB环境中直接打开运行。目前已有2356人学习下载，非常适合正在学习机械臂动力学、非线性控制或Simulink建模的本科生、研究生以及相关工程技术人员。通过亲自动手运行与修改参数，可以清晰观察二关节机械臂在不同输入信号下的动态响应，比较普通跟踪控制与正弦轨迹跟踪控制的效果差异，进而深入理解计算力矩控制的核心原理、逆动力学求解过程以及基于模块化建模的仿真调试方法。 说实话，看到“二关节机械臂计算力矩控制simulink程序.7z”这个文件名的时候，我第一反应是：终于有人把这套东西整理出来了。二关节机械臂是机器人控制领域最经典的被控对象，计算力矩控制又是教科书里绕不开的核心算法，但难点从来不在公式本身，而在怎么把它在Simulink里真正跑起来。我见过太多人在这一步卡住——模型建不对、矩阵算错、控制器发散，折腾几天也不知道问题出在哪个模块。这篇文章就结合这套程序，把动力学建模、控制器设计、Simulink实现细节和调试踩坑的经验一次讲清楚，适合正在做机器人控制课程设计、毕设，或者刚接触机械臂控制仿真的同学直接参考。
1. 二关节机械臂与计算力矩控制：这套程序到底做了什么
1.1 为什么二关节机械臂是控制算法验证的“标准考场”
二关节机械臂结构上只有两个旋转关节、两根连杆，但它的动力学特性已经足够复杂：两个关节之间存在强耦合——关节1转动会引起关节2的动力学变化，关节2运动产生的反作用力又会反馈到关节1上。再加上重力项、科氏力项和离心力项，这一整套非线性关系让它成为验证控制算法非常好的“试验田”。
很多刚接触机器人控制的同学会问：既然要学控制，直接用电机模型不更简单吗？问题在于，单关节系统本质上是线性的，用经典PID就能获得不错的控制效果，体现不出非线性控制方法的价值。而二关节机械臂保留了完整的非线性动力学特征，同时状态量又只有两个，数学推导和仿真验证都很方便。可以说，把二关节机械臂吃透了，后面再去处理六轴机械臂、移动机械臂，思路是完全相通的，只是矩阵规模和计算量变大而已。
1.2 计算力矩控制在控制链条里的定位
计算力矩控制（Computed Torque Control）在机器人控制体系里属于“基于模型的控制方法”，核心思想非常直观：既然机械臂的动力学模型已知，那就先把模型里的非线性项算出来并补偿掉，让系统从控制器的视角看变成一个线性系统，然后再用线性控制理论来设计跟踪控制器。
打个比方，这就像开车时你知道路况是上坡还是下坡，于是预先调整油门，而不是等车速掉了再补油。传统PID看到速度偏差才反应，计算力矩控制则在偏差出现之前就已经把已知的干扰消除了。这套程序里用的正是这个思路，结合外环PD反馈来保证跟踪精度和抗扰能力。
2. 二关节机械臂动力学建模：从拉格朗日方程到Simulink模块
2.1 拉格朗日方程推导的核心思想
二关节机械臂的动力学模型一般通过拉格朗日方程建立，形式是：
M(q)q̈ + C(q, q̇)q̇ + G(q) = τ
其中M(q)是2×2惯性矩阵，C(q, q̇)是科氏力和离心力矩阵，G(q)是重力项，τ是关节力矩。这里M矩阵的元素与角度相关，C矩阵的元素同时与角度和角速度相关，重力项只与角度相关。这套程序里，模型采用的是两连杆平面机械臂，重力作用在平面内，因此重力项会参与运算。
拉格朗日方程推导的步骤比较机械：先写出每个连杆的动能和势能，得到拉格朗日量，然后对广义坐标求导。真正花时间的是化简整理，因为展开后会得到一长串三角函数项。用符号计算工具先算一遍，再手写核对关键项，最后用数值仿真验证，这样效率最高。
2.2 M、C、G矩阵的具体计算
在Simulink里实现动力学模型，核心工作就是算出三个矩阵。惯性矩阵M(q)的表达式为：
M = [m11, m12; m21, m22]
其中m11 = m1lc1² + m2(l1² + lc2² + 2l1lc2cos(q2)) + I1 + I2，m12 = m2(lc2² + l1lc2cos(q2)) + I2，m22 = m2*lc2² + I2。
科氏力矩阵C(q, q̇)需要与q̇乘在一起，展开后得到：
C(q, q̇)q̇ = [-m2l1lc2sin(q2)(2q̇1q̇2 + q̇2²); m2l1lc2*sin(q2)*q̇1²]
重力项G(q)则为：
G = [(m1lc1 + m2l1)gcos(q1) + m2lc2gcos(q1+q2); m2lc2gcos(q1+q2)]
这里我把连杆假设为质量均匀分布的杆件，质心在连杆中点，所以lc1 = l1/2、lc2 = l2/2。如果需要更精确的模型，可以把质心位置和转动惯量改成测量值，控制结构本身不受影响。
2.3 在Simulink里搭建动力学模型的几种思路
Simulink里实现动力学模型有三种常见做法。第一种是把M、C、G矩阵用Fcn模块或MATLAB Function模块直接写成表达式，输入是角度、角速度，输出是加速度，再加两个积分器得到角速度和角度，这是最容易理解和排查的做法。第二种是用S函数实现，运行效率高，但调试时需要编译，对新手不够友好。第三种是Simscape Multibody物理建模，拖拽连杆和关节即可，不需要推导方程，但模型运行速度偏慢，而且做控制算法验证时，Simscape的物理引擎反而会掩盖一些控制器本身的问题。
这套程序选用的是第一种方式，用MATLAB Function模块封装动力学计算，结构清晰，每个环节都能单独加Scope查看信号，非常适合教学和二次开发。
3. 计算力矩控制器设计与参数整定
3.1 外环PD加内环非线性补偿的控制律
计算力矩控制器的控制律为标准形式：
τ = M(q)(q̈d + Kdė + Kpe) + C(q, q̇)q̇ + G(q)
其中e = qd - q是跟踪误差，qd、q̇d、q̈d是期望轨迹的位置、速度和加速度。这个控制律从结构上分两层：M(q)乘以一个“辅助控制量”构成内环，把被控对象线性化为一个解耦的双积分器系统；外环的PD反馈则保证闭环误差动态按照期望的阻尼比和自然频率收敛。
选择Kp和Kd的时候，最直接的方法是让误差方程具有期望的特征方程形式。二阶系统误差方程为：
ë + Kdė + Kpe = 0
如果希望系统自然频率为ωn、阻尼比为ζ，那么Kp = ωn²，Kd = 2ζωn。比如取ζ = 1（临界阻尼）、ωn = 10 rad/s，就有Kp = 100、Kd = 20，此时跟踪误差快速收敛又没有超调。实际仿真中可以先以这组参数起步，再根据波形微调。
3.2 轨迹规划的配合：为什么三次多项式不够用
计算力矩控制要求期望轨迹的加速度必须连续且已知，这直接决定了轨迹规划的方法。很多同学用三次多项式规划关节轨迹，角度轨迹和速度轨迹连续，加速度却是阶跃变化的，控制器跟踪时会在每个路径点处出现力矩突变，波形上表现为明显的抖动。
这套程序里采用五次多项式插值，从初始角度到目标角度，在起点和终点同时约束位置、速度、加速度均为零或给定值，这样加速度曲线是光滑连续的，控制器输出的力矩也更平滑。在Simulink中实现时，可以直接用Clock模块作为时间输入，在MATLAB Function里实时计算期望位置、速度和加速度，也可以提前离线算好数据，用From Workspace模块导入。前者更经典，参数修改也方便。
3.3 关于M矩阵求逆的数值陷阱
计算力矩控制律里，M(q)被当成一个“补偿增益”使用，但控制律本身包含M(q)与辅助控制量的乘积，并不需要显式求逆。这其实是计算力矩控制的一个优点：相比逆动力学控制（Inverse Dynamics Control），它避免了对M矩阵求逆带来的数值问题。
不过要注意的是，如果参考程序里写成了先求逆再计算的形式，比如τ = inv(M)*(...)再折腾一圈，M矩阵在机械臂接近奇异位形时可能变成一个病态矩阵，数值上会出现很大的异常值。标准计算力矩控制不需要inv(M)，这里提醒一下，看到类似写法时多看一步，验证一下是否真的有必要。
4. Simulink程序结构与建模细节
4.1 压缩包内的文件构成与整体架构
7z压缩包解压之后，一般会看到样式接近的主模型文件（如computed_torque_control.slx）、初始化脚本（如init_params.m）和说明文档。先运行初始化脚本给工作区填变量，再打开模型跑仿真，这是这套程序的标准流程。
主模型内部结构通常是自上而下三个大模块：轨迹生成模块、计算力矩控制器模块、二关节机械臂动力学模块。控制器的输出直接连接到机械臂动力学模块的力矩输入，机械臂输出的角度和角速度则反馈到控制器，形成闭环。
这种分层结构的好处是，你可以单独把控制器禁用，把输入换成阶跃力矩，验证动力学模型本身是否正确；也可以单独把控制器拿出来接上一个简化模型，验证算法逻辑；排查问题时能够按环节隔离。
4.2 初始化脚本里的关键参数
初始化脚本是关键，它定义了所有会用到的变量。典型参数包括：连杆质量m1、m2（比如5kg、3kg），连杆长度l1、l2（比如1m、0.8m），质心位置lc1、lc2，转动惯量I1、I2，重力加速度g，控制增益Kp、Kd。注意，如果计算M矩阵时用了简化假设，比如把连杆当作匀质杆，那么I和lc之间一定是对应的，不能一边用匀质杆的I = ml²/12，一边又用lc = 0.3l这种不相匹配的数据，否则动力学模型与真实物理对象不一致，控制器设计就会出问题。
建议把机械臂物理参数和控制器参数分开设置，比如两个结构体robot_params和ctrl_params，或者用脚本来区分配置。这样在做参数扫描时，可以只改其中一个结构体，不会互相干扰。
4.3 从Simulink模型到C代码：一点工业级思考
很多搜索过相关话题的同学会顺带关心Simulink模型的C代码生成，这套机械臂控制程序在仿真验证完成后，确实具备走Embedded Coder生成C代码的条件。前提是模型里不能有Scope、Clock这类非代码生成模块，连续积分器可能需要替换为离散形式，配置好求解器步长和目标硬件即可。把仿真模型改造成可用于产品代码的模型，是一套独立的方法论，这里先提个醒：控制算法仿真和工程化落地之间存在一道明确的转换过程，这也是企业里控制工程师和算法工程师经常需要协作的原因。
5. 常见问题与调试记录
5.1 仿真一开始就发散，问题出在哪
仿真发散是百分之百会遇到的问题，最常见的原因是符号错误，特别是在手写C矩阵和重力项时，多一个负号或少一个负号都会导致严重发散。排查时把控制器暂时断开，给动力学模型单独加一个固定的常值力矩（比如零力矩），看看两个关节是不是按照重力自然摆动，如果没有重力作用就保持匀速，或者按预期规律运动，说明模型基本正确。
另一个常见原因是积分器初值问题。机械臂的初始角度必须和期望轨迹的初始位置一致，如果期望轨迹从零开始，机械臂初始角度却是90度，瞬间误差巨大，控制器会输出一个超大的力矩，仿真直接爆掉。解决办法是把机械臂初始状态设置为与期望轨迹起点一致，或者给轨迹加一段平滑爬升的起始段。
5.2 跟踪误差收敛不了，该查哪个模块
误差在初始时刻就存在但始终不收敛，首先检查Kp、Kd是否太小，导致系统带宽不足。其次检查轨迹模块输出的期望加速度是否正确，如果辅助控制量里丢掉了q̈d项，系统会存在稳态误差。
误差波形出现明显周期性波动，大概率是轨迹加速度不连续引起的，换五次多项式轨迹或者提高插值阶数即可。还有一个比较隐蔽的问题是解算器步长设置过大，固定步长可以取得较小比如1e-4，如果使用变步长求解器，需要把最大步长限制在合理范围。
5.3 7z包解压和模型打开的那些小事
首先，7z是一种高压缩率的压缩格式，用7-Zip、Bandizip、WinRAR都能解压，注意压缩包的完整路径最好不要包含中文或空格，防止个别版本工具出现解压异常。其次，.slx模型文件的版本兼容性值得注意，新版Matlab保存的模型在旧版上打不开，如果你的Matlab版本较低，需要请对方另存为兼容版本。打开模型后如果提示找不到数据字典或依赖文件，确认工作目录是否切换到了模型所在文件夹，运行初始化脚本是必要条件，很多报错都源于没有先加载变量。
5.4 基于实际经验给初学者的调试顺序
我调试这类仿真程序时，习惯使用分层验证的思路进行。第一步验证轨迹生成模块，单独运行轨迹模块，确认位置连续、速度连续、加速度平滑；第二步验证动力学模型，给一个已知力矩，观察关节运动是否与物理直觉一致；第三步闭环，先给很小的Kp、Kd，确认系统稳定，再逐步增大增益观察响应速度；第四步加入重力补偿，再逐步调整外环PD参数。
这个顺序看起来很慢，但实际是最快的。直接一把梭把控制器和模型连起来跑，一旦发散，根本不知道问题出在哪一环。分步验证一段时间后，你就能形成自己的手感，一看波形就知道是哪类问题。
6. 从这套程序还能往哪个方向扩展
6.1 加入摩擦项和外部扰动
很多教材里的动力学模型只包含惯性、科氏力和重力项，真实机械臂还有库仑摩擦、粘滞摩擦和外部扰动。在动力学模型中加入摩擦项B*q̇，再在控制器的补偿项里忽略它，就能测试计算力矩控制对外部扰动的抑制能力。或者主动在仿真中给关节加一个固定大小的扰动力矩，看看跟踪误差的变化，这是验证控制系统鲁棒性的最基本手段。
6.2 从计算力矩控制切换到滑模控制
许多搜索相关关键词的同学同时也在找四旋翼滑模控制、机械臂滑模控制的内容。计算力矩控制虽然效果不错，但对模型精确度要求较高。如果你想对比鲁棒控制方法，可以在这套Simulink程序的基础上，把控制律从计算力矩控制切换为滑模控制：保留完整的动力学模型和轨迹规划模块，只修改控制器内部的控制律，用滑模趋近率取代PD反馈。这样一来，动力学模型和轨迹生成部分完全复用，对比实验的工作量集中在控制器模块上，非常方便。
6.3 联合仿真方向：控制算法与虚拟样机
Carsim和Simulink联合仿真、Amesim与Simulink联合仿真、四旋翼滑模控制Simulink仿真等方向，本质上都是同一个思路——被控对象的物理模型用专业软件建立，控制算法放在Simulink里实现，通过接口模块实现数据交换。这套二关节机械臂程序可以视为一个微缩版本的控制算法验证平台，理解它的架构后，迁移到其他联合仿真场景只需替换被控对象模型和接口配置。先弄懂单一模型的信号流，再学习联合仿真接口，会顺畅很多。
一些写在最后的实际体会
我最初拿到这套程序时，也犯过错误，第一件事就去翻控制律代码，觉得核心全在控制器。后来发现，真正决定仿真成败的，反而是看似简单的动力学模型是否正确、轨迹是否平滑、参数是否匹配。计算力矩控制这个算法本身已经足够成熟，难的是你怎么把论文里的公式落地成一个稳定运行的仿真程序。
另一个体会是，Simulink调试要善用Scope和数据记录。不要只盯着最终跟踪误差波形，把每个关键点的信号都拉出来看，特别是控制力矩、辅助控制量、各项补偿项，它们能告诉你系统内部到底发生了什么。力矩波形毛刺多，多半是轨迹加速度不平滑；辅助控制量震荡，多半是反馈通道出现代数环或延迟。看波形找问题，比盯着代码猜要高效得多。
如果你手里正卡在某一步仿真发散的阶段，先静下心按本文第5章的排查顺序走一遍，大概率问题很快就能定位。这套计算力矩控制程序只是一个起点，后续无论是做滑模控制对比、加入摩擦项分析，还是过渡到工业机械臂的工程化控制，建模和调试的思路都是通用的。
本文还有配套的精品资源，点击获取
