控制工程入门：LQR——基于显式优先级的状态反馈

Contents — find the section you need

线性二次调节 (LQR) 为状态偏差和执行器的使用赋予成本，然后推导出使该成本最小化的状态反馈律（适用于线性模型）。与直接响应当前误差、历史误差及其变化率的 PID 调节不同，LQR 可以将位置、速度、角度、角速率、电流和耦合轴作为一个状态进行处理。它尤其适用于倒立摆、飞行器姿态、平衡机器人或机械臂的局部稳定控制。

LQR 并非万能的最优开关。如果所需状态无法测量，则需要观测器；如果系统超出局部线性模型的范围，增益效果将大打折扣；如果输入或状态约束至关重要，则模型预测控制 (MPC) 通常是更佳的选择。有关 ROS 2 执行边界，请参阅 ROS 2 入门。关于估计假设，请参阅传感器融合入门。

实际结论

基于 $\dot{x}=Ax+Bu$ 和权重 $Q,R$ ，无限时域线性二次型回归 (LQR) 生成 $u=-Kx$ 。 $Q$ 表示状态误差的惩罚强度； $R$ 表示指令代价。
增益 $K$ 来自 Riccati 方程及其解 $P$ 和 $K=R^{-1}B^TP$ 。它考虑了模型耦合，而不是手动调整每个状态的增益。
$(A,B)$ 的稳定性以及 $(A,C)$ 的可检测性/可观测性是前提条件。拥有传感器并不等同于能够重建所需的状态。
LQR 本身并不强制执行器限制、碰撞间隙或紧急停止。这些属于增益之外的限制和安全功能。

状态空间和信号流

状态 $x$ 是最小的变量集合，它与输入和扰动一起决定未来的行为。小车需要位置和速度；旋转体需要角度和角速度；机电驱动器可能需要电流。线性时不变模型为：

\dot{x}=Ax+Bu+Ew,\qquad y=Cx+Du+v

其中 $u$ 为指令， $w$ 为未建模的扰动， $y$ 为测量值， $v$ 为传感器噪声。 $A$ 描述状态演化， $B$ 为输入路径， $C$ 为传感器所揭示的信息。

在图中，求和节点形成 $e=\hat{x}-x_r$ （估计状态减去参考值），LQR 输出 $u=-Ke$ 。对于零参考值，则简化为通常的 $u=-K\hat{x}$ 。

图表：黄昏线圈，概念性而非测量值。实际实现还包含校准、时间戳、传输延迟和执行器内环。

围绕平衡点 $(x_e,u_e)$ ，定义偏差 $\tilde{x}=x-x_e$ 和 $\tilde{u}=u-u_e$ ，然后基于 $\dot{\tilde{x}}=A\tilde{x}+B\tilde{u}$ 进行设计。这是一个局部语句。能够稳定接近零角度的直立摆的增益，并不一定能作为摆锤大幅下落或执行器饱和系统的恢复控制器。

Riccati 方程和成本

连续无限时域 LQR 最小化

J=\int_0^\infty\left(x^TQx+u^TRu\right)dt

其中 $Q\succeq0$ 和 $R\succ0$ 。较大的 $Q$ 会使状态误差成本更高；较大的 $R$ 会使计算成本更高。不要将米、弧度和米相加。每秒，单位为安培，不进行缩放。一个有用的起点是基于允许值 $x_{max}$ 和 $u_{max}$ 的对角权重，例如 $1/x_{max}^2$ 和 $1/u_{max}^2$ 。

连续代数 Riccati 方程为：

A^TP+PA-PBR^{-1}B^TP+Q=0

其稳定解 $P$ 给出：

K=R^{-1}B^TP,\qquad u=-Kx

这给出了控制器使用的反馈增益 $K$ 。

采样系统需要一个离散模型和一个离散 Riccati 方程。连续有限时域问题使用微分 Riccati 方程；离散有限时域问题使用差分方程；离散无限时域问题使用 DARE。连续模型矩阵为 $A,B$ ，离散模型为： $x_{k+1}=A_dx_k+B_du_k$ 。将连续增益当作离散控制器来应用，将角度与弧度混用，或忽略延迟，都可能导致原本正确的计算变成不稳定的设备。

可控性和可观测性：增益前的条件检查

可控性矩阵为：

\mathcal{C}=[B\ AB\ A^2B\ \cdots\ A^{n-1}B]

对于一个 $n$ 状态系统，满秩条件为 $\operatorname{rank}\mathcal{C}=n$ 。

满秩意味着每个模式都是可控的；LQR要求 $(A,B)$ 可稳定，并且 $(Q^{1/2},A)$ 可检测，才能得到标准的稳定解。这与传感器对 $(C,A)$ 的可观测性不同。可观测性矩阵为：

\mathcal{O}=\begin{bmatrix}C\\CA\\\vdots\\CA^{n-1}\end{bmatrix}

满秩意味着状态可以从输出历史中重建。参见[卡尔曼滤波器]对于通常与 LQR 配对的估计器，请参阅入门教程（/zh/blog/posts/control-kalman-filter.html）。

数值示例：稳定双积分器

对于具有 $x=[p\ v]^T$ 和加速度指令 $u$ 的归一化双积分器，

A=\begin{bmatrix}0&1\\0&0\end{bmatrix},\quad B=\begin{bmatrix}0\\1\end{bmatrix}

使用 $Q=\operatorname{diag}(q_p,q_v)$ 可得到 $u=-k_pp-k_vv$ 。它类似于 PD 算法，但两个增益均由模型和 $Q,R$ 共同选择。如果 $q_p$ 过大而 $q_v$ 过小，则会导致急加速和急制动；如果 $R$ 过小，则表示执行器几乎可以忽略不计。最终增益为 $K=[k_p\ k_v]$ ，其中 $k_p,k_v$ 也同时选择。物理边界 $|u|\le u_{max}$ 和 $|\Delta u|\le r_{max}$ 仍然是必需的。如果频繁出现削波，参考整形、重新设计或约束 MPC 比将削波后的结果称为无约束 LQR 更为准确。

对于具有 $Q=I$ 和 $R=1$ 的连续双积分器，同样的推导过程可以得到 $K=[1\ \sqrt{3}]\simeq[1\ 1.732]$ 。这是一个归一化示例，并非可以直接复制到硬件中的增益：请使用实际系统的采样模型、质量、执行器限制和延迟重新计算。

PID、LQR 和 MPC 的选择

方法	强拟合	输入	约束处理	主要注意事项
PID	快速单回路调节	误差/历史/速率	外部限幅器	积分饱和和噪声
LQR	局部线性多状态稳定	估计状态	本身不显式	模型范围和饱和度
MPC	带限制的多变量预测	状态、模型、参考	显式优化约束	截止时间和可行性

嵌套设计很常见：PID 电流/速度回路、LQR 局部姿态或位置稳定以及用于轨迹或约束的 MPC。对于跟踪而非固定调节，请使用时变参考、前馈、增益调度或 LTV-LQR。

机器人实现和安全性

编码器、IMU、摄像头和激光雷达以不同的速率和延迟发布数据。不要将按到达顺序排列的消息直接作为状态传递。传递时间戳对齐的估计值、置信度/协方差和有效性状态。ROS 2 Primer中描述的 ROS 2 生命周期和硬件接口是设计边界，而不仅仅是软件组织结构。

代表性的IMU/INS

图片：Xsens MTi-G (Kallap85, CC BY-SA 4.0), Wikimedia Commons。这是一张代表性的传感器图像，并非LQR系统的实测数据或产品推荐。

监控估计器协方差增长、传感器超时、编码器和IMU之间的不一致性、持续饱和以及超时情况。如果状态不再可信，则停止发出 $-K\hat{x}$ 指令。根据惯性、重力和人员接近程度，选择系统特定的转换方式——减速至零、扭矩禁用、机械制动或紧急停止。LQR稳定性声明的前提是模型正确、状态正确且输入未饱和；独立的保护措施必须涵盖该证明之外的假设。

实施清单

确定平衡点、单位、符号、坐标系和离散周期。
测量线性化点附近的模型残差并定义工作范围。
通过数值方法检查可控性和可观测性/可检测性。
将 $Q,R$ 从允许的状态和指令进行归一化；记录每次变化。
监测饱和度、速率限制、估计新鲜度以及 LQR 范围之外的独立停止情况。
在扩展工作范围之前，测试低输出下的延迟、摩擦、有效载荷变化和传感器损耗。

参考资料

检查你的理解

更大的输入惩罚会促使什么？

它会促使控制力保持稳定。检查响应权衡，并适当地使用不同的单位缩放状态变量。