1. 项目背景与核心概念
在机械控制领域,连杆系统是最基础也是最经典的研究对象之一。单连杆系统可以简单理解为机械臂的最简化模型——就像人的手臂从肩膀到肘部这一段;而二连杆系统则相当于包含了上臂和前臂的完整手臂模型。计算力矩法(Computed Torque Control)则是解决这类系统控制问题的经典方法,它本质上是一种基于模型的前馈控制策略。
我第一次接触这个课题是在研究生阶段的机器人控制课程上。当时为了完成课程设计,在Simulink里搭建第一个单连杆模型就花了整整三天时间。最让人头疼的不是理论推导,而是如何把教科书上的公式真正转化为可运行的仿真模型。这种"理论到实践"的鸿沟,正是本仿真项目要解决的核心问题。
2. 系统动力学建模
2.1 单连杆系统建模
单连杆的动力学方程可以表示为:
τ = M(q)q̈ + C(q,q̇)q̇ + G(q)
其中:
- τ 是关节扭矩(控制输入)
- q, q̇, q̈ 分别是关节角度、角速度、角加速度
- M(q) 是惯性矩阵(标量)
- C(q,q̇) 是科里奥利力项
- G(q) 是重力项
对于长度为l,质量为m的单连杆:
M = ml²/3 (假设质量均匀分布)
C = 0 (单连杆无科氏力)
G = mgl/2 * cos(q)
注意:这里的1/3系数来自于对细长杆件惯量的计算,如果使用点质量模型则应为ml²
2.2 二连杆系统建模
二连杆系统的动力学更为复杂,其矩阵形式为:
τ = M(q)q̈ + C(q,q̇)q̇ + G(q)
此时:
- q = [q1; q2] (两个关节角度)
- M是2×2正定矩阵
- C包含速度耦合项
- G是2×1向量
具体表达式较为复杂,通常通过拉格朗日方程推导。以第一个连杆质量m1、长度l1,第二个连杆m2、长度l2为例:
M11 = (m1/3 + m2)l1² + m2l2²/3 + m2l1l2cos(q2)
M12 = m2l2²/3 + m2l1l2cos(q2)/2
M21 = M12
M22 = m2l2²/3
C矩阵中的非线性项:
C111 = 0, C121 = -m2l1l2sin(q2)q̇2/2
C211 = m2l1l2sin(q2)q̇1/2, C221 = m2l1l2sin(q2)(q̇1+q̇2)/2
重力项:
G1 = (m1/2 + m2)gl1cos(q1) + m2gl2cos(q1+q2)/2
G2 = m2gl2cos(q1+q2)/2
3. 计算力矩控制原理
3.1 控制律设计
计算力矩法的核心思想是通过动力学模型的逆运算来抵消系统非线性。控制律分为两部分:
τ = M(q)[q̈d + Kv(q̇d - q̇) + Kp(qd - q)] + C(q,q̇)q̇ + G(q)
其中:
- qd, q̇d, q̈d 是期望轨迹
- Kv, Kp 是微分和比例增益矩阵
- 前两项构成PD反馈控制
- 后两项用于动态补偿
3.2 实现步骤
- 获取当前关节位置q和速度q̇
- 计算跟踪误差e = qd - q
- 计算控制加速度:q̈c = q̈d + Kv(q̇d - q̇) + Kp(qd - q)
- 计算所需扭矩:τ = M(q)q̈c + C(q,q̇)q̇ + G(q)
- 将τ作为输入施加到系统
关键点:这种方法将非线性系统转化为误差空间的线性系统,使得(ë + Kvė + Kpe = 0)
4. Simulink建模实现
4.1 单连杆模型搭建
-
创建新模型,添加以下模块:
- "Interpreted MATLAB Function"用于动力学计算
- "Integrator"模块用于位置和速度计算
- "Gain"模块设置PD参数
- "Scope"用于显示结果
-
MATLAB函数代码示例:
matlab复制function tau = single_link_dynamics(q, qd, qd_dot, qd_ddot, Kp, Kv)
% 参数设置
m = 1; l = 1; g = 9.81;
% 误差计算
e = qd - q;
e_dot = qd_dot - q(2);
% 控制加速度
qc_ddot = qd_ddot + Kv*e_dot + Kp*e;
% 动力学计算
M = m*l^2/3;
G = m*g*l*cos(q(1))/2;
tau = M*qc_ddot + G;
end
- 连接模块时注意:
- 位置信号需要双重积分获得
- 确保反馈信号的符号正确
- 采样时间建议设为0.001s
4.2 二连杆模型搭建
二连杆模型更为复杂,建议采用S-Function实现:
- 创建Level-2 MATLAB S-Function
- 在Outputs方法中添加动力学计算:
matlab复制function sys = mdlOutputs(~,~,x,u,~,m1,m2,l1,l2,g,Kp,Kv)
q = x(1:2); qd = u(1:2);
qd_dot = u(3:4); qd_ddot = u(5:6);
% 误差计算
e = qd - q;
e_dot = qd_dot - x(3:4);
% 控制加速度
qc_ddot = qd_ddot + Kv*e_dot + Kp*e;
% 动力学计算
M11 = (m1/3+m2)*l1^2 + m2*l2^2/3 + m2*l1*l2*cos(q(2));
M12 = m2*l2^2/3 + m2*l1*l2*cos(q(2))/2;
M = [M11 M12; M12 m2*l2^2/3];
C1 = -m2*l1*l2*sin(q(2))*x(4)/2;
C2 = m2*l1*l2*sin(q(2))*x(3)/2;
C = [C1*x(4); C2*(x(3)+x(4))];
G1 = (m1/2+m2)*g*l1*cos(q(1)) + m2*g*l2*cos(q(1)+q(2))/2;
G2 = m2*g*l2*cos(q(1)+q(2))/2;
G = [G1; G2];
sys = [x(3:4); M\(-C-G); M*qc_ddot + C + G];
end
- 模型搭建技巧:
- 使用Bus Creator整合多个信号
- 为每个关节添加独立的Scope监控
- 使用MATLAB Function生成期望轨迹
5. 参数调试与优化
5.1 增益选择原则
-
临界阻尼条件:
选择Kp和Kv使系统处于临界阻尼状态:
Kv = 2*sqrt(Kp) -
初始值建议:
- 单连杆:Kp=100, Kv=20
- 二连杆:Kp=diag([100,100]), Kv=diag([20,20])
-
调整方法:
- 先增大Kp直到出现轻微振荡
- 然后增大Kv消除振荡
- 最后微调两个参数
5.2 常见问题排查
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 系统发散 | 增益过大 | 逐步降低Kp和Kv |
| 响应迟缓 | 增益过小 | 适当增大增益 |
| 稳态误差 | 重力补偿不足 | 检查G(q)计算 |
| 关节不同步 | 耦合项忽略 | 确认C(q,q̇)正确性 |
| 高频振荡 | 采样时间过大 | 减小仿真步长 |
6. 仿真结果分析
6.1 单连杆性能验证
测试正弦轨迹跟踪:qd = sin(t)
典型结果指标:
- 稳态误差:<0.01rad
- 响应时间:<0.5s
- 超调量:<5%
调试心得:重力补偿项的准确性对垂直平面运动至关重要。曾因漏掉cos(q)导致45度位置有持续误差。
6.2 二连杆耦合分析
测试场景:第一关节保持不动,第二关节运动
关键观察点:
- 非运动关节的振动幅度
- 能量传递现象
- 耦合项的影响程度
典型问题:当第二个连杆快速运动时,会引起第一个连杆的明显振动,这需要通过调整交叉增益来抑制。
7. 高级应用扩展
7.1 鲁棒性改进
基本计算力矩法对模型误差敏感,可添加:
-
滑模变结构控制:
τ = τCTC + K*sign(s)
其中s是滑模面,K是鲁棒增益 -
自适应控制:
在线估计参数不确定性
M̂ = M + ΔM, 更新ΔM
7.2 实时实现考虑
-
计算简化:
- 预先计算三角函数
- 使用查表法替代实时计算
-
离散化处理:
- 欧拉法:q̈ = (q̇[k] - q̇[k-1])/T
- 保持计算频率>1kHz
-
代码优化:
c复制// 示例:二连杆动力学计算的C实现
void compute_torque(float q[2], float dq[2], float torque[2]) {
float c2 = cos(q[1]), s2 = sin(q[1]);
float M11 = (m1/3+m2)*l1*l1 + m2*l2*l2/3 + m2*l1*l2*c2;
float M12 = m2*l2*l2/3 + m2*l1*l2*c2/2;
float detM = M11*(m2*l2*l2/3) - M12*M12;
// 逆矩阵计算
float invM11 = (m2*l2*l2/3)/detM;
float invM12 = -M12/detM;
float invM22 = M11/detM;
// 科氏力计算
float C1 = -m2*l1*l2*s2*dq[1]*dq[1]/2;
float C2 = m2*l1*l2*s2*dq[0]*dq[0]/2;
// 重力项
float G1 = (m1/2+m2)*g*l1*cos(q[0]) + m2*g*l2*cos(q[0]+q[1])/2;
float G2 = m2*g*l2*cos(q[0]+q[1])/2;
// 控制律实现
float qc_ddot[2];
qc_ddot[0] = qd_ddot[0] + Kv[0]*(qd_dot[0]-dq[0]) + Kp[0]*(qd[0]-q[0]);
qc_ddot[1] = qd_ddot[1] + Kv[1]*(qd_dot[1]-dq[1]) + Kp[1]*(qd[1]-q[1]);
torque[0] = M11*qc_ddot[0] + M12*qc_ddot[1] + C1 + G1;
torque[1] = M12*qc_ddot[0] + (m2*l2*l2/3)*qc_ddot[1] + C2 + G2;
}
8. 工程实践建议
-
模型验证步骤:
- 先测试开环响应(去掉控制器)
- 检查重力项:保持位置时τ=G(q)
- 测试惯性:空载加速验证M(q)
-
实际部署注意事项:
- 添加扭矩饱和限制
- 实现安全自检功能
- 添加紧急停止逻辑
-
性能提升技巧:
- 使用RTAI或Xenomai实现硬实时
- 采用FPGA加速矩阵运算
- 使用EtherCAT等高带宽通信
在实验室测试阶段,我们曾因为忽略扭矩饱和导致电机过热损坏。后来在控制律中添加了以下限幅逻辑后问题解决:
matlab复制tau_max = 10; % Nm
tau = min(max(tau, -tau_max), tau_max);
