Simulink二关节机械臂计算力矩控制仿真模型详解

简介：面向机器人控制与仿真学习者，这份二关节机械臂计算力矩控制Simulink程序包以二连杆动力学模型为基础，演示如何通过逆动力学求解关节驱动力矩并完成末端轨迹跟踪。程序包含两类控制策略：常规跟踪控制与正弦轨迹跟踪控制，配合Simulink模型与m脚本覆盖参数初始化、惯性矩阵计算、控制器设计和结果绘图等完整流程，可帮助理解计算力矩控制的实现细节与建模方法。压缩包共11个文件，其中2个slx模型直接搭建控制回路，9个m文件负责动力学运算与绘图，整体仅31KB，结构简洁便于按模块研读。目前已有2356人浏览学习，适合自动化、机器人方向的本科生或工程师用以上手仿真实验、调节参数，并观察不同轨迹下的跟踪响应，加深对机器人控制理论的理解。 做机械臂控制仿真，最绕不开的一类程序就是用Simulink搭二关节机械臂的计算力矩控制（CTC）模型。别小看这个看起来“老掉牙”的控制策略，它能把机器人动力学中的非线性项在前馈里直接抵消掉，让系统变成一个线性二阶对象，后续想加轨迹规划、力控制、自适应扩展都非常方便。我在早期做多关节机械臂控制器验证时，就是靠这套二关节计算力矩程序把整个仿真流程跑通的，省掉了很多从零搭模型的麻烦。

这个程序包里包含完整的二关节机械臂动力学模型、计算力矩控制器和轨迹跟踪仿真，适合正在学机器人控制理论、准备做课程设计，或者想快速验证自己控制算法的人。它的价值不在于“多高级”，而在于把计算力矩控制从公式变成可运行的模型，你改了增益、换了轨迹，马上就能看到跟踪误差和力矩变化，比对着课本推导省力得多。

1. 项目概述与核心原理

计算力矩控制本质上是“反动力学控制”。机械臂的动力学方程是

M(q)q̈ + C(q, q̇)q̇ + G(q) = τ

左边的 M、C、G 分别代表惯性矩阵、科氏力/离心力矩阵和重力项，右边 τ 是关节力矩。这个方程组是非线性、强耦合的，直接用PID控制很难在所有位置和速度下都表现好。计算力矩控制的做法很直接：把控制律设计成

τ = M(q)(q̈_d + Kd·ė + Kp·e) + C(q, q̇)q̇ + G(q)

其中 q_d 是期望轨迹，e = q_d − q 是跟踪误差。把 τ 代入原方程后，M(q)、C、G 全部被精确对消，剩下的就是

ë + Kd·ė + Kp·e = 0

也就是说，原系统被“线性化”成了一个解耦的二阶线性系统，你只需要设计 Kp 和 Kd，就能决定误差收敛的动态特性。这就是这个控制策略最核心、也最好用的地方。

1.1 为什么二关节模型最合适

二关节机械臂是验证计算力矩控制的“最小完整系统”。它虽然有耦合项、重力项，但自由度少、动力学表达式不难推导，适合在Simulink里用MATLAB Function块直接写。对比单关节模型，二关节把“耦合”这个关键难点体现出来了；对比六关节机器人，二关节又不至于被庞大的符号推导淹没。

在这个程序包里，动力学模型采用的是传统拉格朗日法。你可以看到 M(q) 是一个 2×2 对称矩阵，C(q, q̇) 矩阵里的元素包含 sin(q2)、cos(q2)，这些项在驱动器和控制器之间的相互作用中扮演了关键角色。前馈里的 G(q) 若被忽略，仿真里常用到的重力补偿就没了，力矩输出会出现明显低频偏差。

1.2 适用场景：从课程设计到算法验证

这个程序最典型的用途有几种：

• 把计算力矩控制和普通PID放一起对比，观察跟踪误差差别；
• 当作“基线控制器”，在上面叠加滑模控制、自适应控制等复杂策略；
• 分析不同控制器增益对系统稳定性和控制力矩的影响；
• 改成三关节模型，验证动力学耦合对控制性能的影响。

我做实验时还发现一个细节：同样是计算力矩控制，用基于模型的前馈和不用前馈，对轨迹跟踪误差的影响差异很大。程序里默认是把 C、G 都做前馈的，如果你调试时发现跟踪误差很大，可以先检查是否动了前馈通道。

2. Simulink程序整体设计与模块拆解

拿到这个 .7z 压缩包，解压后先别急着运行。建议先看一遍模型的层级结构，搞清楚每个模块的用途，后面改参数的时候心里才有底。

2.1 模型层级结构

顶层模型通常是一个ctc_two_link.slx（或类似名称），里面主要分三块：

1. 轨迹生成模块：产生期望关节位置 q_d、速度 q̇_d、加速度 q̈_d；
2. 控制器模块：实现 τ = M(q)(q̈_d + Kd·ė + Kp·e) + C(q, q̇)q̇ + G(q)；
3. 机械臂模型模块：求解真实动力学方程，输出 q、q̇ 给控制器作为反馈。

控制器和机械臂模型是两个对称的动力学计算。很多时候，控制律里需要 M、C、G，而机械臂模型本身就是 M、C、G 组成的微分方程，能不能把这两处动力学算对，直接决定了仿真结果是否合理。这也是为什么程序里会放着重复的计算函数——一个用于前馈，一个用于被控对象。

2.2 核心模块说明

轨迹生成部分一般给的是五次多项式规划或者正弦轨迹。我特别推荐用正弦轨迹做初测，因为它的加速度连续，不会在起始瞬间给力矩带来突变，方便观察稳态跟踪情况。程序里如果默认是阶跃或斜坡，你在调试时最好改成平滑轨迹，否则一上来力矩就冲击很大，容易误判控制效果。

被控对象模块是整个仿真里面最容易写错的地方。标准写法是用积分器把 q̈ 积分成 q̇，再积分成 q。如果你的模型里还有状态空间的表达，注意状态变量排列顺序必须和控制器的反馈对齐。常见做法是状态向量 x = [q1; q2; q1_dot; q2_dot]，积分器初始条件对应机械臂初始关节角。

注意：积分器初始状态一定要设成与期望轨迹的初始值一致，比如两个关节初始角是 [0; 0]，期望轨迹也从 0 出发，否则仿真一开始就会出现非常大的误差尖峰，会让你的控制器瞬间输出饱和力矩。

3. 核心细节解析与实操要点

程序光能跑起来还不够，你得懂几个关键参数是怎么来的，改错了会出什么现象，这些问题在教材里经常不会细讲，但仿真时一定会遇到。

3.1 M、C、G 的计算实现

在 Simulink 里实现动力学项，通常有几种方式：

• MATLAB Function 块：写一个 m 函数，输入 q、q̇，输出 M、C、G；
• S-Function：适合更复杂的模型，但维护麻烦；
• 积分器加常数乘法器：适合非常简单的演示，但二关节模型耦合项多，基本不适合手工搭。

这个程序里用的应该是 MATLAB Function 块，这也是我最推荐的方式。它比 S-Function 直观，比纯 Simulink 模块搭出来的结构要简洁得多。你需要重点检查 M 矩阵对角线和非对角线元素、C 矩阵每一项的系数符号。比如二关节模型中，C(q, q̇) 的非对角元素常写成 -m2 * l1 * lc2 * sin(q2) * q2_dot，这里面的符号一旦写反，前馈就变成了“正反馈”，仿真直接发散。

动力学参数一般包括：连杆长度 l1、l2，质心距离 lc1、lc2，质量 m1、m2，转动惯量 I1、I2。程序中这些参数通常放在初始化脚本里，比如init_params.m。我建议对每个参数加注释，格式统一，后续做参数辨识或蒙特卡洛分析时会更方便。

3.2 控制器增益 Kp、Kd 怎么选

计算力矩控制把系统变成了 ë + Kd·ė + Kp·e = 0，所以增益设计就是典型的二阶系统极点配置。常用方法是设每个关节的闭环自然频率为 ωn，阻尼比 ζ≈1，则

Kp = ωn²，Kd = 2ζωn

程序中默认值一般是 Kp = diag([100, 100])、Kd = diag([20, 20])，对应 ωn = 10 rad/s、ζ = 1。如果你要加快响应速度，把 Kp 调大，但 Kd 要跟着调，否则系统会震荡。一个经验是从 ωn = 5 开始，看跟踪误差曲线是否单调衰减，再逐步提高到 20 左右。

提示：计算力矩控制的 Kp、Kd 矩阵通常取对角阵，但理论上如果你想让关节间有更好的协调性，也可以取非对角增益。实际调试时非对角增益很难直观调整，建议先用对角阵把基线跑好，再考虑扩展。

3.3 采样时间与求解器设置

很多人跑仿真时只用默认的变步长求解器，这通常会出问题。计算力矩控制里前馈项包含 q̈_d，而 Simulink 在变步长下对输入信号求导时容易产生抖动。我的习惯是：

• 使用定步长求解器，比如 ode4（四阶龙格库塔）；
• 步长设为 1e-3 s 或更小；
• 如果模型还有离散控制器，需要把控制周期和仿真步长对齐。

步长过大时会看到误差曲线呈锯齿状，看起来就像控制器有延迟；步长过小时仿真速度又慢。二关节模型不复杂，取 1e-3 s 是个平衡点。

4. 实操过程与核心环节复现

下面按实际运行顺序，把从解压到看结果的关键步骤过一遍。我以 Windows 环境、MATLAB R2019b 以上版本为例，其他版本差距不大。

4.1 解压与目录准备

压缩包是 .7z 格式，用 7-Zip 就能解压，别直接用 Windows 自带的解压工具硬解，遇到中文路径可能乱码。解压到一个纯英文路径下，比如D:\Robot_CTC\sim。这一步看着很基础，但我在实际使用中遇到过好几次因为中文路径导致 MATLAB Function 块里某些文件读取失败的坑。

解压后的目录一般包括：

• ctc_two_link.slx：主仿真模型；
• init_params.m：初始化参数脚本；
• plot_results.m：绘制结果脚本；
• dynamics_ctrl.m/dynamics_plant.m：控制器和被控对象的动力学函数；
• README.txt：说明文件。

先打开init_params.m，在 MATLAB 命令行运行，把工作区变量都加载好。再用open_system('ctc_two_link')打开模型，或者直接双击 .slx 文件。

4.2 仿真参数设置

打开模型后，按 Ctrl+E 检查求解器设置。我建议修改为：

• Solver：ode4；
• Fixed-step size：0.001；
• Stop Time：10。

仿真时间太长没必要，10 秒足够看 2~3 个完整轨迹周期。如果看到前 2 秒误差已经收敛，后面基本就是稳态跟踪了。

然后运行init_params.m，确保工作区出现 M1、M2、L1、L2、LC1、LC2、I1、I2 等变量。如果模型打开时报错“未定义函数或变量”，基本就是没先运行初始化脚本。

4.3 运行与结果分析

点击运行按钮。仿真完成后运行plot_results.m，会看到两组关键曲线：

• 两个关节的期望轨迹与实际轨迹对比；
• 两个关节的跟踪误差曲线。

如果程序正常工作，期望与实际轨迹曲线应该几乎重合，误差量级在 1e-3 到 1e-5 之间，具体取决于增益大小。力矩曲线可能会在起始瞬间有一个尖峰，那是轨迹起步时加速度突变造成的，不属于模型发散。

我一般会把误差曲线单独放大看，确认误差在 0.1 秒内收敛，且没有持续振荡。如果误差一直上下波动，先怀疑增益太小；如果误差一开始很大但后续逐渐趋近于零，说明前馈有效，只是初始误差收敛需要时间。

5. 常见问题与排查技巧

我在这类模型上踩过不少坑，也帮别人排查过各种奇怪的仿真现象。下面整理成一张速查表，按问题出现的概率排序。

5.1 典型问题汇总

5.2 调参时的两个细节

第一个细节是增益不要两个关节设成完全相同的值。两个连杆长度和质量不同，同样的闭环频率在实际效果上会有差异。最好先单独调第一关节，再单独调第二关节，最后再合到一起看耦合影响。

第二个细节是轨迹生成里的加速度前馈 q̈_d 一定要连接到控制器，而不是只有位置和速度反馈。我第一次自己搭模型时漏掉了这一路，结果跟踪误差始终偏大，试了很久才意识到是前馈加速度缺失。程序里如果默认连接了，也建议你主动断开试试，感受一下前馈到底有多大作用。

5.3 判断模型动力学是否写错的绝招

如果你怀疑动力学写错了，但不知道具体错在哪里，有个很简单的验证方法：给控制器设零增益，也就是 Kp = Kd = 0，此时控制律变成 τ = M(q)q̈_d + C(q, q̇)q̇ + G(q)，如果动力学完全正确，机械臂的实际加速度应该严格等于 q̈_d。仿真里如果实际轨迹能够跟随期望轨迹，说明 M、C、G 全部正确；如果不跟随，那动力学肯定有错误。这个方法我在调任何新机械臂模型时都会用一遍，能省大量排查时间。

6. 扩展思路与个人操作体会

计算力矩控制程序跑通之后，我不会建议你停在“运行成功”这一步，因为它能扩展的方向太多了。

最直接的扩展是换成滑模控制。在计算力矩控制的基础上加一个鲁棒项，比如 τ = M(q)(q̈_d + Kd·ė + Kp·e + Δ) + C(q, q̇)q̇ + G(q)，其中 Δ 是滑模项，用来抑制模型误差和外部扰动。此时二关节模型前面的动力学函数可以完全复用，只需要在控制律里加一路修正项，对比计算力矩控制就能明显看出鲁棒性的差异。

另一个扩展方向是在轨迹规划层面。程序里默认轨迹是确定好的，但你可以把轨迹改成实时生成的梯形速度曲线或者 S 型曲线，观察不同加速段对控制力矩的影响。这个对后续做工业机械臂运动规划很有帮助。

我在实际使用中发现，Simulink 里的计算力矩控制程序最容易被忽视的是模型复现的准确性。你看教材时，动力学公式推导再怎么复杂，最后都是一个表达式；但落到 Simulation 里，符号、矩阵维度、初始条件都会产生难以预料的坑。所以我的建议是：从头到尾自己动手搭一个最基本的二关节计算力矩模型，哪怕只是照着程序的结构重写一遍，也比直接跑通别人的程序收获大多了。

这个程序和普通 PID 程序放在一起对比时，你会真正理解“非线性补偿”带来的差异。PID 在单点附近调好了，换一个姿态可能就变差了；计算力矩控制则能在整个工作空间保持一致的动态响应。想进一步深入的朋友，可以把 M、C、G 的计算过程里的参数写成可辨识形式，在 Simulink 里做参数估计，这又是另一个有趣的方向了。

最后再分享一个小技巧：模型里所有自定义变量最好都在初始化脚本里统一命名，不要直接在 MATLAB Function 块里写数字。我吃过一次亏，某个版本里把连杆质量直接写死在函数里，后来调参只能翻函数代码，效率特别低。程序包本身已经把基本框架给你了，你只需要在参数初始化和控制器结构上按自己的需求调整，就能快速得到可靠的结果。

本文还有配套的精品资源，点击获取