章华燕1. 机器人控制算法实验概述
在机器人控制领域，LQR（线性二次型调节器）作为一种最优控制方法，相比传统的PID控制具有更优的性能和理论保证。但实现一个完整的LQR控制实验需要跨越多个技术环节，包括数学模型建立、控制算法实现、物理仿真验证等。本文将详细介绍我在机器人研究中实现LQR控制的全过程，分享从理论到实践的完整经验。
1.1 LQR控制的核心优势
LQR控制之所以在机器人研究中备受青睐，主要基于以下几个特点：
- 最优性保证：通过最小化二次型代价函数，LQR能够提供系统状态和控制输入之间的最优平衡
- 多变量处理能力：天然适合多输入多输出(MIMO)系统，而PID需要为每个通道单独设计
- 稳定性分析：基于Lyapunov理论，可以严格证明闭环系统的稳定性
- 抗干扰性能：通过合理设计权重矩阵，可以获得良好的鲁棒性
1.2 实验环境搭建要点
完整的LQR控制实验需要以下软件环境支持：
- MATLAB/Simulink：算法开发和仿真平台
- Simscape Multibody：物理建模和仿真环境
- Robotics System Toolbox（可选）：机器人专用工具包
- Symbolic Math Toolbox（可选）：用于推导运动方程
提示：建议使用MATLAB R2020b或更新版本，以确保所有工具箱功能的完整性和兼容性。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. LQR控制实现全流程
2.1 系统建模与线性化
实现LQR控制的第一步是建立准确的系统数学模型。对于机器人系统，通常采用拉格朗日力学方法推导非线性运动方程：
matlab复制syms q1 q2 dq1 dq2 ddq1 ddq2 tau1 tau2 real
% 定义系统参数（以二连杆机械臂为例）
m1 = 1; m2 = 1; l1 = 1; l2 = 1; g = 9.81;
% 动能和势能计算
T = 0.5*m1*(l1*dq1)^2 + 0.5*m2*((l1*dq1)^2 + (l2*dq2)^2 + 2*l1*l2*dq1*dq2*cos(q2));
V = m1*g*l1*(1-cos(q1)) + m2*g*(l1*(1-cos(q1)) + l2*(1-cos(q1+q2)));
% 拉格朗日方程推导
L = T - V;
eq1 = simplify(diff(diff(L,dq1),t) - diff(L,q1) - tau1);
eq2 = simplify(diff(diff(L,dq2),t) - diff(L,q2) - tau2);
得到非线性方程后，需要在平衡点附近进行线性化：
matlab复制% 在平衡点(q1=0,q2=0)附近线性化
A = jacobian([dq1; dq2; solve(eq1,ddq1); solve(eq2,ddq2)], [q1;q2;dq1;dq2]);
B = jacobian([dq1; dq2; solve(eq1,ddq1); solve(eq2,ddq2)], [tau1;tau2]);
A = subs(A, {q1,q2,dq1,dq2,tau1,tau2}, {0,0,0,0,0,0});
B = subs(B, {q1,q2,dq1,dq2,tau1,tau2}, {0,0,0,0,0,0});
2.2 黎卡提方程求解与增益计算
获得线性化模型后，可以设计LQR控制器。核心是求解代数黎卡提方程：
matlab复制% 权重矩阵设计
Q = diag([10, 10, 1, 1]); % 状态权重
R = eye(2); % 控制输入权重
% 连续时间黎卡提方程求解
[K, P, E] = lqr(A, B, Q, R);
% 离散时间系统求解（可选）
sys = ss(A, B, eye(4), zeros(4,2));
sysd = c2d(sys, 0.01); % 离散化采样时间0.01s
[Kd, Pd, Ed] = dlqr(sysd.A, sysd.B, Q, R);
注意：权重矩阵Q和R的选择对控制性能影响很大。通常从单位矩阵开始，然后根据响应特性调整，原则是：
- 增大Q对角线元素会加快状态收敛但可能增大控制量
- 增大R对角线元素会减小控制量但可能减慢响应
2.3 Simulink模型搭建技巧
在Simulink中实现LQR控制时，推荐采用以下结构：
- 状态观测模块：从物理模型获取状态变量
- 控制增益模块：实现u = -Kx计算
- 前馈补偿模块（可选）：用于轨迹跟踪
- 饱和限制模块：防止执行器饱和
关键实现代码：
matlab复制function u = lqr_control(x, x_ref)
% LQR控制计算函数
persistent K;
if isempty(K)
K = [1.23, 0.45, 0.12, 0.05; % 示例增益矩阵
0.45, 1.67, 0.05, 0.15];
end
u = -K * (x - x_ref);
end
3. Simscape Multibody物理仿真
3.1 物理模型搭建要点
在Simscape Multibody中搭建机器人模型时，需要注意：
- 关节类型选择：旋转关节vs平移关节
- 参考坐标系对齐：确保各部件坐标系正确连接
- 质量属性设置：准确设置质量、惯量等参数
- 传感器配置：位置、速度等测量信号输出
3.2 模型与控制算法对接
将Simulink控制算法与Simscape模型对接的关键步骤：
- 在Simscape模型中添加相应的输入端口（控制量）和输出端口（状态量）
- 使用Simulink-PS Converter和PS-Simulink Converter模块进行信号类型转换
- 设置合适的采样时间（通常控制算法比物理仿真步长大）
- 配置正确的求解器参数（推荐使用ode15s或ode23t）
3.3 仿真参数配置建议
| 参数 | 推荐值 | 说明 |
|---|---|---|
| Solver | ode15s | 适合刚性系统 |
| Max step size | auto | 自动确定最大步长 |
| Relative tolerance | 1e-4 | 平衡精度和速度 |
| Absolute tolerance | 1e-6 | 对小信号更精确 |
4. 调试技巧与常见问题
4.1 LQR实现中的典型问题
-
系统不可控：
- 检查可控性矩阵：
rank(ctrb(A,B))应等于系统阶数 - 可能原因：模型线性化点选择不当，或物理连接错误
- 检查可控性矩阵：
-
求解黎卡提方程失败：
- 确保(A,B)可控，(A,Q^(1/2))可观
- 尝试调整权重矩阵Q和R
-
仿真发散：
- 检查物理单位是否一致（如角度用弧度还是度）
- 验证控制量是否超出执行器限幅
4.2 性能优化技巧
-
权重矩阵调整策略：
- 先设R为单位矩阵，调整Q使状态收敛
- 然后增大R对角线元素直到控制量合理
-
实时性优化：
- 将K矩阵计算移到初始化阶段
- 使用Lookup Table近似非线性系统
-
抗饱和处理：
matlab复制function u = safe_control(x, K, u_lim) u = -K * x; for i = 1:length(u) if u(i) > u_lim(i) u(i) = u_lim(i); elseif u(i) < -u_lim(i) u(i) = -u_lim(i); end end end
5. 扩展应用与进阶方向
5.1 不同机器人平台的适配
-
机械臂控制：
- 需要考虑关节摩擦、柔性等非线性因素
- 可以结合计算力矩控制提高性能
-
无人机控制：
- 通常分为姿态环和位置环级联控制
- 需要处理强耦合和欠驱动特性
-
无人车控制：
- 重点在于路径跟踪和避障
- 可以结合MPC实现更优性能
5.2 从仿真到实物的关键考虑
- 执行器动力学：仿真中理想的执行器与实际电机/液压系统差异
- 状态估计：实际系统中状态可能无法直接测量
- 延迟补偿：通信和控制计算引入的延迟
- 鲁棒性增强：考虑模型不确定性和外部扰动
在实际项目中，我发现在仿真中表现良好的LQR控制器，转移到实物平台时通常需要以下调整：
- 增加低通滤波消除测量噪声
- 加入积分环节消除稳态误差
- 适当降低增益防止执行器饱和
- 添加前馈补偿提高跟踪性能