想让LQR控制器精准跟踪轨迹？别急着调参，先搞懂‘增广系统’这个核心概念
LQR轨迹跟踪中的增广系统：从理论到实践的深度解析
当你在MATLAB中实现一个简单的LQR轨迹跟踪控制器时，是否遇到过系统"摆烂"的情况？明明期望状态清晰可见，控制器却始终与之保持一段微妙的距离，仿佛在无声抗议。这种现象背后，往往隐藏着LQR设计中一个关键但常被忽视的概念——增广系统建模。
1. 为什么你的LQR控制器在轨迹跟踪时"摆烂"？
许多工程师在初次实现LQR轨迹跟踪时会遇到一个典型问题：系统状态最终稳定在期望值附近，但始终存在无法消除的静态误差。这不是代码错误，而是基础LQR原理导致的必然结果。
标准LQR设计有一个隐含特性——它总是将系统状态推向原点。当我们简单地将期望状态设为非零值时，相当于要求系统稳定在一个非平衡点上。这就像试图让一个弹簧质量系统在没有外力作用下静止在拉伸位置，物理上就不可能实现。
典型问题表现：
- 位置误差始终存在，无法完全收敛到期望轨迹
- 控制输入逐渐趋近于零，系统进入"节能模式"
- 代价函数显示已达到"最优"，但实际效果不理想
% 典型的问题现象示例 figure; subplot(2,1,1); plot(t, x_actual, 'b', t, x_desired, 'r--'); title('位置跟踪：存在静态误差'); subplot(2,1,2); plot(t, u); title('控制输入：最终趋于零');2. 增广系统：LQR轨迹跟踪的核心思想
增广系统(Augmented System)是解决LQR跟踪问题的关键思路。它的核心在于将状态误差而非原始状态作为新的系统状态，从而将跟踪问题重新转化为调节问题。
2.1 从误差状态到增广状态
传统思路中，我们可能尝试直接定义误差e=x-xd，但这会遇到一个根本问题：误差动力学不完全由控制输入决定，因为期望状态xd可能有自己的动态特性。
增广系统的精妙之处在于同时考虑实际系统与期望系统的动态：
实际系统: x(k+1) = A x(k) + B u(k) 期望系统: xd(k+1) = Ad xd(k)将两者组合，得到增广状态空间：
Xa = [x; xd] % 增广状态向量对应的增广系统动态：
Xa(k+1) = [A 0; 0 Ad] Xa(k) + [B; 0] u(k)2.2 误差的重新表达
在增广系统框架下，误差可以表示为：
e = C_a * Xa = [I -I] * [x; xd] = x - xd这种表达的关键优势在于：
- 误差动态完全由增广状态和控制输入决定
- 将跟踪问题转化为增广状态的调节问题
- 保持了LQR原有的数学框架和求解方法
3. 增广LQR的完整设计与实现
3.1 代价函数的改造
传统LQR代价函数：
J = Σ (x'Qx + u'Ru)增广系统下的新代价函数：
J = Σ (e'Qe + u'Ru) = Σ (Xa' [I -I]' Q [I -I] Xa + u'Ru)这相当于定义了新的状态权重矩阵：
Q_a = [I -I]' Q [I -I] = [Q -Q; -Q Q]3.2 控制律的求解
增广系统的LQR求解流程：
定义增广系统矩阵：
A_a = [A, zeros(n); zeros(n), Ad]; B_a = [B; zeros(size(B))]; Q_a = [Q, -Q; -Q, Q];求解代数Riccati方程：
[K_a, S_a] = dlqr(A_a, B_a, Q_a, R);分解增益矩阵：
K_x = K_a(1:n); K_d = K_a(n+1:end);实现控制律：
u = -K_x*x + K_d*xd;
3.3 MATLAB实现示例
% 系统参数 A = [1.1 0.2; -0.1 0.9]; B = [0.1; 0.2]; Q = diag([10, 1]); R = 0.1; % 期望系统(假设为恒定值) Ad = eye(2); % 构建增广系统 n = size(A,1); A_a = [A, zeros(n); zeros(n), Ad]; B_a = [B; zeros(size(B))]; Q_a = [Q, -Q; -Q, Q]; % 求解LQR [K_a, ~, ~] = dlqr(A_a, B_a, Q_a, R); % 分解控制增益 K_x = K_a(1:n); K_d = K_a(n+1:end); % 仿真 N = 100; x = zeros(n, N); xd = [1; 0]; % 期望状态 x(:,1) = [3; 0]; % 初始状态 for k = 1:N-1 u = -K_x*x(:,k) + K_d*xd; x(:,k+1) = A*x(:,k) + B*u; end % 绘制结果 figure; plot(1:N, x(1,:), 'b', [1 N], [xd(1) xd(1)], 'r--'); title('增广LQR跟踪性能'); xlabel('时间步'); ylabel('位置'); legend('实际','期望');4. 调参艺术：如何避免系统"摆烂"或"过激"
即使理解了增广系统原理，参数选择仍然至关重要。不恰当的权重会导致两种极端：
- 系统"摆烂"：R过大，控制代价太高，系统选择接受静态误差
- 系统"过激"：Q过大，控制过于激进，可能导致振荡或饱和
4.1 权重选择原则
| 参数 | 影响 | 过大后果 | 过小后果 |
|---|---|---|---|
| Q(1,1) | 位置误差权重 | 响应快速但可能振荡 | 收敛慢，静态误差大 |
| Q(2,2) | 速度误差权重 | 阻尼过大，响应迟缓 | 超调明显 |
| R | 控制输入权重 | 系统"节能"，误差大 | 控制量过大，可能饱和 |
4.2 调试实战技巧
从对角线Q开始：先关注主要状态(如位置)，再调整次要状态(如速度)
Q = diag([10, 1]); % 典型初始选择R的黄金法则：从估计最大允许控制量倒推
R = 1/(u_max^2); % u_max为最大允许控制量观察代价函数分解：
J_state = x'*Q*x; J_control = u'*R*u;渐进调整法：
- 先设R=0，观察无约束时的最优响应
- 逐步增加R，直到响应速度可接受
- 微调Q的非对角线元素改善动态特性
4.3 处理时变期望轨迹
增广系统同样适用于跟踪时变轨迹，关键在于正确建模期望动态Ad：
恒定值跟踪：
Ad = eye(n);斜坡信号跟踪：
Ad = [1 T; 0 1]; % T为采样时间正弦轨迹跟踪：
omega = 2*pi*f; % f为信号频率 Ad = [cos(omega*T) sin(omega*T); -sin(omega*T) cos(omega*T)];
对于复杂轨迹，可考虑将轨迹生成器离散化作为Ad，或采用预测控制等更高级方法。
5. 超越基础：增广LQR的高级应用
5.1 积分动作增强
为彻底消除静态误差，可在增广系统中引入误差积分：
增广状态：Xa = [x; xd; ∫e dt]对应的系统矩阵：
A_a = [A zeros(n) zeros(n); zeros(n) Ad zeros(n); C -Cd zeros(n)]; B_a = [B; zeros(n,1); zeros(size(C,1),1)];5.2 输出反馈扩展
当不能直接测量所有状态时，可结合观测器设计：
- 设计状态观测器估计x
- 基于估计状态构建增广系统
- 分离原理保证稳定性
% 观测器设计 L = place(A', C', obs_poles)'; % 增广控制律 u = -K_x*x_hat + K_d*xd;5.3 鲁棒性增强技巧
权重调度：根据误差大小动态调整Q
Q_scaled = Q * (1 + k*norm(e));约束处理：使用参考 governor 避免饱和
xd_modified = xd * (u_max/abs(u_calc));抗积分饱和：在误差大时暂停积分
if norm(e) > threshold integral_term = 0; end
在实际工程中，我经常发现初学者过度关注代码实现而忽视概念理解。曾有一个机器人定位项目，团队花费两周调试参数却收效甚微，直到重新审视增广系统设计，才发现期望速度建模不正确。调整Ad矩阵后，性能立即提升了60%。这印证了控制领域的一句老话：好的算法设计抵得过无数小时的参数调试。