★ FEATURED ARTICLE 简介二关节机械臂计算力矩控制Simulink程序包面向机器人控制初学者、自动化专业学生及相关工程人员提供一套基于Simulink的二连杆机械臂计算力矩控制仿真示例帮助理解机械臂动力学建模与逆动力学驱动的控制思路。压缩包共11个文件包含2个Simulink模型slx和9个MATLAB脚本m模型分别对应常规跟踪与正弦跟踪两种轨迹任务脚本用于初始化关节参数、计算惯性矩阵/哥氏力/重力项、绘制仿真曲线及对比分析。该程序包已有2360人学习下载。运行后可直观观察机械臂在计算力矩控制下的关节响应理解力矩控制器与积分器设置对稳定性和精度的影响通过修改参考轨迹或控制器参数还能自主验证不同策略下的跟踪性能为后续接触滑模控制、自适应控制等进阶方法提供可扩展的仿真平台。1. 二关节机械臂计算力矩控制不用调PID也能让双关节不打架我之前调过二关节机械臂的PID单关节单独动作没问题两个关节同时动就开始震荡——M矩阵里的耦合项和科里奥利力把单关节的调试经验全部推翻。这个基于Simulink的计算力矩控制程序包用逆动力学把非线性耦合项全部补偿掉让被控对象退化成两个独立的线性二阶系统外环只需要一个简单的PD就能同时跟踪两条关节轨迹。对做机器人控制课程设计、毕业设计或者刚入门机器人控制仿真的人来说这套程序的价值在于你把动力学模型写对了控制律部分可以直接照着程序包的思路套不需要在PID参数上反复做实验。整个仿真环境不依赖真实硬件一台装了MATLAB的电脑就能跑完位置跟踪、速度观测和力矩输出全流程。2. 技术栈与整体架构Simulink模块划分与二关节动力学建模2.1 为什么选Simulink从手写ODE45到可视化建模仿真这个程序包没有选择用纯MATLAB脚本加ODE45求解微分方程而是把整个系统搭在Simulink模型里这不是没有原因的。计算力矩控制的核心工作量不在PD参数而在逆动力学模型的准确性——你需要把M矩阵、C矩阵、G向量都按符号形式推进控制器里再实时求逆、乘矩阵、做补偿。在Simulink里做这件事MATLAB Function模块可以直接写矩阵运算Scope模块可以实时看每个关节的角度误差和力矩波形改一个参数点一下运行就能对比调试效率比纯脚本高得多。另一个重要原因是模块化。Simulink模型天然把轨迹生成、控制器、被控对象、观测显示拆成不同的模块块每个模块可以用子系统封装起来。我做仿真排查的时候可以直接用信号线点亮的方式看中间信号传到哪个模块断掉了不用像脚本那样满屏disp打印。对做课程设计的人来说这种分层结构更容易在答辩时讲清楚控制架构也让后续二次开发时替换某个模块的工作量降到最低。2.2 程序包目录结构与顶层模型拿到压缩包解压后典型的文件组织方式是下面这张表控制器相关代码和模型文件是分离的这样既能在Simulink里使用也能单独在MATLAB脚本里做离线测试。文件作用init_params.m初始化机械臂参数和控制器增益运行前必跑two_link_arm_computed_torque.slx顶层Simulink模型包含轨迹生成、控制器、动力学三大部分trajectory_planner.m五次多项式轨迹规划输出每个时刻的期望位置、速度、加速度computed_torque_controller.m计算力矩控制器核心代码供MATLAB Function调用two_link_dynamics.m二关节机械臂逆动力学函数返回M、C、G矩阵two_link_arm_sfun.cS-Function版本被控对象模型C语言实现仿真速度更快plot_results.m把Scope数据导出成Figure并绘制对比曲线顶层Simulink模型从信号流方向看分四段期望轨迹信号源、计算力矩控制器、二关节动力学被控对象、示波器显示组。控制器部分接收期望轨迹和反馈状态两个输入经过逆动力学计算输出力矩到被控对象被控对象内部用S-Function求解当前状态下的加速度再积分得到角度和角速度。整个模型的信号线只有六根架构清楚适合逐步看信号流向也适合教学场景当示例拆解。2.3 二关节机械臂动力学方程建立计算力矩控制全部建立在二关节机械臂的动力学方程基础上这里直接给出这个程序包使用的标准形式。M(q) * q̈ C(q, q̇) * q̇ G(q) τ其中q是二关节角度向量M(q)是2×2质量矩阵C(q,q̇)乘q̇得到科里奥利力和离心力项G(q)是重力项τ是关节力矩。质量矩阵在这个程序包里取的是简化二连杆模型M11 m1*l1² m2*(l1² l2² 2*l1*l2*cos(q2)) I1 I2 M12 m2*(l2² l1*l2*cos(q2)) I2 M22 m2*l2² I2这里m1、m2是连杆质量l1、l2是连杆长度I1、I2是绕关节轴的转动惯量。注意M12和M22不依赖q1而依赖q2所以当第二关节转动时第一关节感受到的耦合惯性会发生明显变化——这就是单关节PID调好之后双关节又震荡的根本原因。参数表如下参数默认值说明m1, m21.0 kg连杆质量l1, l21.0 m连杆长度I1, I20.1 kg·m²连杆转动惯量g9.81 m/s²重力加速度动力学模型的准确性决定计算力矩控制的补偿效果。模型参数和真实系统不一致时补偿后会残留模型误差项这时外环PD还能兜底但跟踪精度会下降。这个程序包里默认参数是理想化的匀质连杆模型二次开发时如果要模拟真实机械臂需要把它替换成实测参数。3. 核心模块拆解计算力矩控制从轨迹规划到力矩输出的完整链路3.1 轨迹规划器与五次多项式插值轨迹规划的输入是起点角度、终点角度和运动总时间输出是每个时刻的期望角度、期望角速度和期望角加速度。程序包用的是五次多项式插值它的特点是起点和终点的速度、加速度都为零整条轨迹平滑不会有阶跃冲击。function [q_d, dq_d, ddq_d] trajectory_planner(t, q0, qf, T) % 五次多项式轨迹规划 % t: 当前仿真时刻 % q0: 起点关节角度 [2x1] % qf: 终点关节角度 [2x1] % T: 运动总时间 % 归一化时间 0~1 s t / T; % 五次多项式6s^5 - 15s^4 10s^3 % 该多项式满足 s0 时值为0s1 时值为1 % 且一阶、二阶导数在端点均为零 s5 6*s^5 - 15*s^4 10*s^3; ds5 (30*s^4 - 60*s^3 30*s^2) / T; dds5 (120*s^3 - 180*s^2 60*s) / (T^2); q_d q0 (qf - q0) * s5; dq_d (qf - q0) * ds5; ddq_d (qf - q0) * dds5; end代码里s是归一化时间这样轨迹形状和运动总时间解耦——你改了T轨迹的形变逻辑不变只是快慢不同。返回值q_d用来和当前角度做差得到位置误差dq_d和当前角速度做差得到速度误差ddq_d则在控制律中作为前馈加速度项直接参与力矩计算。这个设计要点了计算力矩控制要求期望轨迹二阶可导所以不能用阶跃信号当期望位置输入否则控制律里会出现冲击项。3.2 控制器内核逆动力学补偿与外环PD计算力矩控制的核心代码只有十几行但设计思路是整个控制律的精髓。控制器的输入是期望轨迹和当前关节状态输出是驱动力矩。控制律写成τ M(q) * (q̈_d Kp * e Kd * ė) C(q, q̇) * q̇ G(q)把τ代入动力学方程M q̈ C q̇ G τ左边和右边相同的C和G直接抵消剩下M q̈ M(q̈_d Kp e Kd ė)。因为M可逆两边同时左乘M的逆得到误差动力学方程ë Kd * ė Kp * e 0这就是计算力矩控制名字的由来——力矩是算出来的不是调出来的。只要M、C、G模型准确系统就变成一个线性齐次方程Kp和Kd可以按线性系统理论任意配置极点。function tau computed_torque_controller(q, dq, q_d, dq_d, ddq_d, Kp, Kd) % 计算力矩控制器 % q, dq: 当前关节角度和角速度 [2x1] % q_d, dq_d: 期望角度和角速度 [2x1] % ddq_d: 期望角加速度 [2x1] % Kp, Kd: PD增益矩阵 [2x2] % 位置误差和速度误差 e q_d - q; de dq_d - dq; % 基于当前状态计算逆动力学 [M, C, G] two_link_dynamics(q, dq); % 外环PD 内环动力学补偿 tau M * (ddq_d Kp * e Kd * de) C G; end代码里的two_link_dynamics函数负责算出M矩阵、C矩阵和G向量这三项是当前状态的非线性函数每步仿真都要重新计算。如果你把C和G这两项注释掉就退化成PD控制双关节耦合会让跟踪误差不再解耦。参数Kp和Kd的选择有一个常用套路设Kp diag(wn², wn²)Kd diag(2wn, 2wn)能得到临界阻尼响应wn设为5到10之间对应系统带宽约0.8到1.6赫兹跟踪阶跃响应没有超调也不会有明显滞后。3.3 被控对象二关节动力学S-Function的实现被控对象在Simulink里用S-Function实现文件是two_link_arm_sfun.c。S-Function的意义在于用C语言写动力学求解避免MATLAB解释执行在循环求解时的性能损耗同时它在Simulink里能以微分方程形式接入数值求解器可以统一管理积分步长。static void mdlDerivatives(SimStruct *S) { real_t *x ssGetRealArrayData(S, 0); // 状态量: q1, q2, dq1, dq2 real_t *u ssGetInputArrayRealSignal(S, 0); // 输入: 力矩 tau1, tau2 real_t *dx ssGetDerivatives(S); real_t q1 x[0], q2 x[1]; real_t dq1 x[2], dq2 x[3]; real_t tau1 u[0], tau2 u[1]; // 质量矩阵元素和2.3节公式一致 real_t M11 m1*l1*l1 m2*(l1*l1 l2*l2 2*l1*l2*cos(q2)) I1 I2; real_t M12 m2*(l2*l2 l1*l2*cos(q2)) I2; real_t M22 m2*l2*l2 I2; // 科里奥利力与离心力项 real_t h1 -m2*l1*l2*sin(q2)*dq2*dq2 - 2*m2*l1*l2*sin(q2)*dq1*dq2; real_t h2 m2*l1*l2*sin(q2)*dq1*dq1; // 重力项 real_t G1 (m1 m2)*g*l1*cos(q1) m2*g*l2*cos(q1 q2); real_t G2 m2*g*l2*cos(q1 q2); // 加速度M(q) \ (tau - C - G) real_t det M11*M22 - M12*M12; real_t rhs1 tau1 - h1 - G1; real_t rhs2 tau2 - h2 - G2; dx[0] dq1; dx[1] dq2; dx[2] (M22*rhs1 - M12*rhs2) / det; dx[3] (M11*rhs2 - M12*rhs1) / det; }这段代码的一个关键细节是用解析行列式求逆而不是直接调用矩阵求逆函数。二关节系统M矩阵只有2×2行列式det M11*M22 - M12²可以解析表达速度和精度都优于通用数值求逆也避免了Simulink在矩阵求逆时可能产生的代数环。mdlDerivatives函数返回的是四个状态的导数Simulink求解器会负责积分更新q和q̇整个循环在仿真步长内稳定推进。4. 仿真运行与参数调整从初始化脚本到Scope波形判读4.1 初始化脚本与控制器参数选取程序包运行的第一步是执行init_params.m所有机械臂物理参数和控制器增益都在这里集中配置。脚本会把这些变量加载到MATLAB工作区Simulink模型里的MATLAB Function模块直接引用这些工作区变量。%% 机械臂物理参数 m1 1.0; m2 1.0; % 连杆质量 kg l1 1.0; l2 1.0; % 连杆长度 m I1 0.1; I2 0.1; % 连杆转动惯量 kg*m^2 g 9.81; % 重力加速度 m/s^2 %% 控制器参数 wn 5; % 自然频率 rad/s Kp wn^2 * eye(2); % 位置增益矩阵 Kd 2 * wn * eye(2); % 速度增益矩阵 %% 轨迹参数 q0 [0; 0]; % 起始关节角度 rad qf [pi/2; pi/3]; % 目标关节角度 rad T 2.0; % 运动时间 sKp和Kd的选取在这里没有被神秘化。wn取5对应位置增益Kp25速度增益Kd10这样误差动力学是一个阻尼比等于1的二阶系统理论上2秒内完成轨迹跟踪且没有超调。如果实在拿不准从wn3开始慢慢加观察实际轨迹是否跟随期望轨迹。4.2 运行步骤启动仿真前建议按顺序走一遍。第一步在MATLAB命令行窗口切换到程序包目录并执行init_params.m工作区出现所有变量。第二步打开two_link_arm_computed_torque.slx检查求解器设置典型配置是步长上限0.01秒的ode45变步长算法。第三步确认两个MATLAB Function模块的输入端口顺序——轨迹规划器输入时间为当前仿真时间控制器输入为期望状态和反馈状态。第四步点击Run按钮运行仿真时长设置为4到6秒确保2秒的轨迹运动结束后还有2秒的稳定观察期。一个容易被忽略的细节S-Function首次用到时需要编译。如果程序包附带的是C文件而不是已编译的mex文件MATLAB会在第一次运行仿真时自动调用编译器此时要求机器上已经配置好MATLAB支持的编译器。具体配置方法在避坑章节的5.5节单独说明。4.3 Scope波形判读与收敛性验证模型里放置了三组示波器关节角度跟踪波形、角度误差波形和关节力矩波形。正常仿真结束后角度跟踪波形里期望轨迹和实际轨迹两条曲线几乎重合只在轨迹开始和结束的瞬间有极小的动态偏差误差波形显示两个关节的误差从启动时刻开始快速收敛2秒后衰减到10的负三次方量级以下力矩波形在启动瞬间有一个比较明显的冲击峰值随后平缓跟随两个关节的力矩值在±5牛米范围内波动。力矩波形是判断控制质量的核心指标。如果力矩值在仿真中途突然跳到几十甚至上百通常是数值积分出了问题而不是控制律写错了。遇到这种情况先减小求解器步长上限到0.001秒再跑一次看峰值是否回落。如果回落说明是求解精度问题如果依旧爆炸则回到避坑章节里检查模型本身。5. 踩坑记录模型跑不通时的五个高频问题5.1 代数环警告和仿真卡死现象模型运行时报代数环警告或者仿真时间推进极其缓慢几分钟都跑不到1秒。原因控制器里M矩阵求逆直接用了MATLAB Function的inv函数而这个函数的输入又依赖当前时刻的状态量Simulink在同一个步长内无法确定信号传递顺序形成代数环。解决程序包在控制器和被控对象之间用S-Function形式解析传递或者在被控对象内部用解析行列式求逆避免了Simulink对逆矩阵的代数依赖。你自己改代码时记住不要在MATLAB Function里直接调inv函数而是把求逆逻辑放进动力学函数内部。5.2 启动瞬间力矩巨大导致发散现象仿真一开始力矩输出就冲到几百牛米关节角度直接发散。原因期望轨迹从q00出发而机械臂初始状态可能不是0初始误差e在第一个仿真步长就很大。更极端的情况是初始状态位于q20附近——此时M矩阵接近奇异行列式det趋近零逆动力学求出的补偿力矩被放大。解决把初始状态设置为和轨迹起点一致通常在积分器模块的Initial condition里填入q0的数值同时给力矩输出加Saturation限幅模块设置±50牛米的合理范围避免数值异常时力矩无界增长。5.3 盲目加大Kp导致高频抖动现象加大Kp之后跟踪误差确实变小了但力矩波形出现高频抖动仿真步长被自动压缩到极小速度变慢。原因Kp过大会让闭环系统带宽逼近甚至超过仿真采样频率同时忽略了执行器响应速度的约束。计算力矩控制虽然能把系统线性化但线性化后的系统仍然是物理系统带宽不可能无限提升。解决不要单独加大KpKp和Kd要同步调整并保持阻尼比在1附近。如果一定要提高跟踪精度先确认动力学模型足够准确再考虑增加前馈项而不是单纯提高增益。5.4 轨迹结束后的残余振动现象轨迹跟踪到2秒终点后角度在目标值附近出现小幅振荡过很久才停稳。原因由于仿真数值积分误差和模型参数误差的存在闭环系统并非完全解耦残留误差会在机械臂柔性模态或数值噪声激励下产生振铃。解决检查轨迹规划器在t大于T时的输出——常见做法是让t超过T后保持终点值不变另外把期望速度设为终点时刻以后恒为零程序包里的trajectory_planner已经处理了这种情况自己改代码时注意边界条件。5.5 S-Function编译失败现象运行模型时报错说找不到two_link_arm_sfun或无法加载mex文件。原因64位MATLAB需要匹配的编译器配置MATLAB自带的LCC编译器有时在特定版本上不支持某些C语法或者系统没有安装编译器工具链。解决在命令行执行mex -setup按提示选择已安装的编译器如果机器上没有编译器程序包里的two_link_dynamics.m也能在MATLAB Function模块里完成同样的动力学计算缺点是仿真速度会慢一些但功能完全等价。6. 二次开发把单条轨迹换成圆形轨迹验证力矩前馈的正确性程序包默认的验证场景是两点之间的五次多项式运动但在实际研究中往往需要连续轨迹跟踪比如画圆或走正弦曲线。改造思路其实很直接——把trajectory_planner的返回值换成连续轨迹公式就行。以圆形轨迹为例假设期望的末端位置在操作空间画一个半径为r的圆利用二连杆正运动学反解出对应的关节角度q_d(t)再用链式法则求dq_d和ddq_d。你会立刻发现计算力矩控制的优势只要逆动力学模块给出的M、C、G足够准确连续轨迹跟踪的误差依然控制在很小的范围内而不需要针对每个轨迹点重新调PID。另外想验证计算力矩控制对模型误差的鲁棒性可以故意在two_link_dynamics.m里把连杆质量加10%控制器用带误差的模型做补偿被控对象用真实模型。你会发现跟踪误差不再归零而是有一个稳态值。这时候你可以在控制律里引入积分项或滑模项做补偿这也是从计算力矩控制向自适应控制、滑模控制过渡的标准改法。我建议逐步走三步第一步只改轨迹规划器验证前馈力矩与期望加速度的对齐关系第二步在控制器输出端串联一个饱和模块模拟真实执行器限制第三步修改逆动力学函数加入连杆质量参数误差观察鲁棒性。每改一步都用Scope里的误差曲线做对比记录最大误差和稳态误差。我一开始做这类二次开发的时候也犯过直接在原模型上乱改参数的毛病结果模型越改越乱最后只能重新解压。后来我养成了一个习惯每次改造前先把原模型另存一份带版本号的副本然后启动一次基线仿真把Scope数据导出成mat文件作为后续对比的基准。从那以后我每次做控制律迭代都强制走一遍这个流程改完一处跑一次基线对比误差曲线往哪个方向偏一目了然。希望这个习惯也能帮你在二次开发时少走弯路。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
