LQR与MPC：机器人最优控制的核心算法对比

1. 最优控制概述：从理论到机器人实践

在机器人控制领域，我们常常面临一个核心挑战：如何在保证系统稳定的同时，实现某种意义上的"最优"性能。这种最优性可能体现在能耗最低、响应最快、轨迹跟踪最精确，或是这些指标的综合平衡上。作为一名从事机器人控制系统开发多年的工程师，我发现最优控制理论为解决这类问题提供了强有力的数学工具。

最优控制的核心思想可以类比为驾驶汽车时的决策过程。当你从A点驶向B点时，会下意识地选择一条既省油又省时的路线，同时避免急加速和急刹车——这本质上就是在优化一个综合了时间、油耗和舒适度的性能指标。在机器人控制中，我们需要将这个直觉过程数学化。

线性二次型调节器(LQR)和模型预测控制(MPC)是目前工业界最常用的两种最优控制方法。LQR诞生于20世纪60年代，是控制理论中的经典成果；MPC则起源于过程控制领域，近年来随着计算能力的提升，在机器人控制中获得了广泛应用。这两种方法分别代表了最优控制的不同哲学：

• LQR采用"一次性全局优化"的思路，通过求解Riccati方程得到一个固定的最优反馈控制律
• MPC则采用"滚动时域优化"策略，在每个控制周期重新求解有限时域的最优控制问题

在实际机器人项目中，我经常需要根据系统特性和硬件条件在这两种方法间做出选择。例如，在计算资源有限的嵌入式平台上，LQR通常是更稳妥的选择；而在处理复杂约束（如机械臂的关节限位）时，MPC则展现出明显优势。

2. 线性二次型调节器(LQR)深度解析

2.1 LQR的数学基础与物理意义

LQR控制器的设计始于对系统动态的线性描述。考虑一个n维状态向量x(t)和m维控制输入u(t)的线性时不变系统：

code复制ẋ(t) = Ax(t) + Bu(t)

其中A是系统矩阵，B是输入矩阵。LQR的目标是找到一个控制律u(t)=-Kx(t)，使得以下二次型性能指标最小化：

code复制J = ∫[x(t)ᵀQx(t) + u(t)ᵀRu(t)]dt

这个性能指标由两部分组成：

• 状态惩罚项x(t)ᵀQx(t)：衡量系统状态偏离期望值的程度
• 控制惩罚项u(t)ᵀRu(t)：反映控制能量消耗

Q和R矩阵的选择是LQR设计中最需要工程经验的部分。根据我的实践，以下原则通常有效：

1. Q矩阵的对角元素决定了各状态变量的相对重要性。例如，在倒立摆控制中，角度误差通常比位置误差更重要
2. R矩阵限制了控制输入的幅度。过小的R会导致控制量过大，可能引发执行器饱和；过大的R则会使系统响应迟缓
3. 经验法则是从Q=diag([1,...,1])和R=ρI开始，通过调整ρ来平衡响应速度和控制量大小

2.2 Riccati方程的求解与实践技巧

LQR的核心数学工具是代数Riccati方程：

code复制AᵀP + PA - PBR⁻¹BᵀP + Q = 0

求解这个方程得到对称正定矩阵P后，最优反馈增益矩阵K可由下式计算：

code复制K = R⁻¹BᵀP

在实际工程中，我们通常使用现成的求解工具（如MATLAB的lqr函数或Python的control库）来避免手动求解的复杂性。不过，理解其数学本质对调试控制器至关重要。

注意：当系统不可控或Q/R选择不当时，Riccati方程可能无解。在工程实践中，我通常会先验证系统的可控性矩阵是否满秩。

2.3 LQR的优缺点与典型应用场景

LQR的优势包括：

• 理论完备，保证闭环系统稳定（只要(A,B)可控且(Q,A)可观测）
• 计算效率高，适合嵌入式平台实现
• 对模型误差有一定的鲁棒性

但LQR也存在明显局限：

• 仅适用于线性系统（实际中常在工作点附近线性化）
• 难以显式处理状态和输入约束
• 全局优化特性可能导致局部性能不理想

在机器人领域，LQR常用于：

• 无人机姿态控制
• 轮式机器人平衡控制
• 简单轨迹跟踪问题

3. 模型预测控制(MPC)原理与实现

3.1 MPC的基本原理与算法框架

与LQR不同，MPC采用"在线优化"的策略，其核心思想可以概括为：

1. 在每个采样时刻，基于当前状态和系统模型，预测未来有限时域内的系统行为
2. 求解一个带约束的优化问题，得到最优控制序列
3. 仅实施控制序列的第一个元素，到下一时刻重新优化

MPC的数学表述通常为：

code复制min_u Σ[x(k)ᵀQx(k) + u(k)ᵀRu(k)] + x(N)ᵀPx(N)
s.t. x(k+1) = f(x(k),u(k))
     x(k) ∈ X, u(k) ∈ U

其中N是预测时域，P是终端代价矩阵。这个优化问题在每个控制周期都要重新求解，因此MPC的计算负担显著高于LQR。

3.2 MPC的关键设计要素

3.2.1 预测模型选择

MPC的性能很大程度上取决于预测模型的准确性。在机器人控制中，常用的模型类型包括：

• 线性模型：计算简单，适用于工作点附近
• 非线性模型（如机械臂的动力学方程）：精度高但计算复杂
• 数据驱动模型：当机理模型难以建立时使用

3.2.2 约束处理

MPC最强大的能力在于可以显式处理各种约束：

• 输入约束（执行器饱和限制）
• 状态约束（安全区域限制）
• 输出约束（工作空间限制）

在实现中，这些约束通常转化为优化问题的线性或非线性不等式约束。

3.2.3 时域参数选择

预测时域N和控制时域M的选择需要权衡：

• 较大的N可以提高性能但增加计算量
• 通常选择M ≤ N，后N-M步可用终端控制器

3.3 MPC的实时实现技巧

在资源受限的嵌入式平台上实现MPC是一大挑战。根据我的项目经验，以下技巧很实用：

1. 热启动：用上一周期的解作为当前优化的初始猜测
2. 代码生成：使用CVXGEN或ACADO等工具生成定制化的高效求解代码
3. 降阶模型：在预测时使用简化模型，提高实时性
4. 事件触发：当状态变化不大时跳过重新优化

4. LQR与MPC的对比与工程选择

4.1 方法特性对比

4.2 工程选型建议

在选择控制策略时，我通常会考虑以下因素：

1. 系统动态特性：

   • 强非线性系统优先考虑MPC
   • 近似线性系统可考虑LQR

2. 约束条件：

   • 有严格约束时选择MPC
   • 无约束或软约束可用LQR

3. 硬件资源：

   • 计算资源有限时选择LQR
   • 有足够计算能力时考虑MPC

4. 实时性要求：

   • 极高采样频率（>1kHz）倾向LQR
   • 中等采样频率可尝试MPC

4.3 混合策略与进阶技巧

在一些复杂项目中，我经常结合使用LQR和MPC：

1. 分层控制：

   • 上层MPC处理长期规划和约束
   • 下层LQR实现快速稳定

2. 终端代价设计：
   在MPC中使用LQR的代价函数作为终端代价，保证稳定性

3. 线性参数变化(LPV)：
   在工作点附近线性化，设计LQR增益调度

5. 实战案例与常见问题排查

5.1 两轮平衡机器人控制实例

以我参与开发的两轮平衡机器人为例，说明两种方法的实际应用：

LQR实现：

1. 在直立位置线性化动力学方程
2. 选择Q=diag([10,1,1,1])（强调角度误差）
3. 选择R=0.1I（允许适度控制量）
4. 求解Riccati方程得到反馈增益
5. 在STM32上实现，采样频率1kHz

MPC实现：

1. 建立非线性动力学模型
2. 预测时域N=10，控制时域M=5
3. 添加电机转矩约束
4. 使用ACADO生成嵌入式代码
5. 在NXPCortex-M7上运行，采样频率100Hz

实测发现，在较小倾角时两者性能相当，但在大角度恢复或受外力冲击时，MPC表现更优。

5.2 常见问题与解决方案

问题1：LQR系统响应过慢

• 可能原因：Q矩阵惩罚不足或R矩阵惩罚过重
• 解决方案：增大Q的对角元素或减小R的值
• 调试技巧：先设R=ρI，通过调整ρ改变响应速度

问题2：MPC优化求解失败

• 可能原因：初始猜测不合理或约束冲突
• 解决方案：检查约束可行性，使用热启动
• 调试技巧：可视化预测轨迹，找出冲突点

问题3：实际性能与仿真差异大

• 可能原因：模型误差或未建模动态
• 解决方案：增强模型精度或添加鲁棒项
• 调试技巧：记录实际与预测状态对比

问题4：MPC计算时间超出采样周期

• 可能原因：预测时域过长或求解器效率低
• 解决方案：缩短时域或使用更高效求解器
• 调试技巧：分析计算时间分布，优化热点

在实际项目中，我通常会建立一个系统的调试流程：

1. 先验证开环模型准确性
2. 在仿真中测试控制器性能
3. 进行硬件在环(HIL)测试
4. 最后进行实物测试
5. 根据实测结果迭代调整参数

这种循序渐进的方法可以显著减少调试时间和风险。