吃透LQR：线性二次调节器原理、仿真与ROS实现

学习机器人控制，很多人是从 PID 开始的。构造一条反馈回路，调好三个系数，系统就能在多数条件下稳定下来。但等真正接触到机械臂、无人机、倒立摆这类系统时，你会发现 PID 的能力边界越来越明显：无法在多变量之间做权衡，很难回答“我的控制作用力是不是最优”这个问题。于是线性二次调节器（LQR）成了绕不开的一道坎。在阿尔伯塔大学 2022 年的机器人控制相关内容中，LQR 被当作最优控制模块的核心内容来讲，原因也在于此：它是从“让系统稳定”迈向“让系统在某种意义下最优”的起手式。

本文以“吃透 LQR”为目标，从数学原理、仿真实现、ROS 集成、调参方法到排错思路，完整梳理一遍。无论你是在校学生准备机器人控制面试，还是工程师想在项目里落地 LQR，这篇文章都能给你一条清晰的路线。

1. 背景与核心概念

1.1 LQR 是什么：最优控制的入门钥匙

LQR 全称 Linear Quadratic Regulator，中文一般叫“线性二次型调节器”。拆开来看：

• Linear：系统模型是线性的，即状态空间形式ẋ = Ax + Bu。
• Quadratic：性能指标是二次型的，即被积函数是状态和输入的二次数。
• Regulator：目标是“调节”，让系统状态从初始偏差回归到平衡点。

它解决的问题可以这样通俗理解：系统现在偏离了平衡点，你需要给它施加控制力把它拉回来，同时希望整个过程“代价最小”。代价由两部分组成——状态偏差的代价和控制能量的代价。LQR 做的事情，就是找一个状态反馈控制律u = -Kx，使得这条代价函数最小。

在机器人领域，LQR 的应用场景非常典型：倒立摆起摆后的平衡、机械臂关节运动到目标点后的稳定、无人机悬停位置修正、移动机器人轨迹跟踪误差的收敛等。这些系统在平衡点附近往往可以线性化，LQR 刚好是一个计算量小、稳定性有保证、物理意义清晰的控制器。

1.2 为什么机器人控制离不开 LQR

机器人动力学本质上是一个高度非线性、多变量强耦合的系统。直接用非线性控制理论设计控制器，数学推导复杂，工程实现成本高。而 LQR 提供了另一条路径：

1. 在平衡点附近对动力学方程做线性化，得到状态空间模型。
2. 基于线性模型，用黎卡提方程求解最优反馈增益。
3. 将增益部署到控制器中。

这条路径成本低、效果好，因此几乎所有现代机器人控制课程都会花大量篇幅讲 LQR。它同时涉及了机器人动力学建模、线性代数、最优化和控制理论的交叉内容，是“从理论到工程”衔接最紧密的算法之一。

另外，LQR 并不只是一个孤立的控制器。它是 LQG（线性二次高斯）控制的基础，也是模型预测控制（MPC）在无限时域、无约束条件下的退化形式。把 LQR 吃透，相当于给你后续学习更高级控制算法铺好了第一级台阶。

1.3 LQR 与 PID、极点配置的差异

极点配置回答的是“我想让系统多快收敛”，LQR 回答的是“在收敛和控制代价之间怎样分配最合理”。因此 LQR 被称为最优控制，本质上是把工程直觉转化为一个可求解的数学问题。

2. LQR 数学原理拆解

2.1 状态空间模型与动力学线性化

几乎所有 LQR 设计都从状态空间模型开始。对于一个连续时间系统：

ẋ = Ax + Bu y = Cx + Du

其中：

• x是状态向量，比如倒立摆的摆角 θ 和角速度 θ̇。
• u是控制输入向量，比如关节力矩。
• A是系统矩阵，描述系统自身的动态。
• B是输入矩阵，描述控制输入如何影响状态变化率。

以最简单的一维倒立摆为例。摆杆围绕关节旋转，忽略摩擦，动力学方程为：

m l² θ̈ = m g l sinθ + τ

在 θ = 0 附近做线性化，令sinθ ≈ θ，得到：

θ̈ = (g / l) θ + (1 / (m l²)) τ

取状态x = [θ, θ̇]ᵀ，即可写成：

ẋ = A x + B u

其中：

A = [[0, 1], [g/l, 0]] B = [[0], [1/(m l²)]]

线性化这一步很关键。它不是“近似凑合”，而是控制理论中标准做法：只要控制器能够在平衡点附近让状态偏差足够小，线性模型就能准确反映系统行为。

2.2 二次型性能指标：Q 和 R 的含义

LQR 的“最优”不是凭空定义的，而是通过最小化下面这个性能指标得到的：

J = ∫₀^∞ ( xᵀ Q x + uᵀ R u ) dt

• Q是状态权重矩阵，对称半正定。它的大小表示你对“状态偏差”的重视程度。
• R是控制权重矩阵，对称正定。它的大小表示你对“控制能量”的重视程度。

如果Q相对R更大，控制器会偏向于快速把状态压回零，代价是控制量更大。如果R相对更大，控制量会更保守，状态恢复会慢一些。

这里有一个容易混淆的点：Q和R不是“越大越好”或“越小越好”，它们之间的相对比值才决定控制行为。工程上也常把Q、R视为可调参数，类似于 PID 中的三个系数。

2.3 最优反馈增益的推导思路

对于无限时域的 LQR 问题，最优控制律是一个线性状态反馈：

u = -K x

其中增益矩阵K由代数黎卡提方程（Algebraic Riccati Equation, ARE）给出：

P A + Aᵀ P - P B R⁻¹ Bᵀ P + Q = 0

先求解 P，再计算：

K = R⁻¹ Bᵀ P

这个结论背后是哈密尔顿-雅可比-贝尔曼（HJB）方程在二次型指标、线性系统下的解析解。理解到这个程度，基本就掌握了 LQR 的全部数学核心。

实际求解黎卡提方程并不需要手算。MATLAB 中有lqr函数，Python 的 SciPy 中有solve_continuous_are，都可以直接求解。

2.4 从连续到离散

数字控制器和仿真环境都是离散运行的，因此通常需要对连续系统做离散化。常见的做法是零阶保持（ZOH）：假设每个采样周期内控制输入保持不变。

离散系统模型写作：

x[k+1] = A_d x[k] + B_d u[k]

对应的离散黎卡提方程为：

P_d = A_dᵀ P_d A_d - A_dᵀ P_d B_d (R + B_dᵀ P_d B_d)⁻¹ B_dᵀ P_d A_d + Q

离散增益为：

K_d = (R + B_dᵀ P_d B_d)⁻¹ B_dᵀ P_d A_d

一个常见误区是：先用连续系统求 K，然后把 K 直接用在离散循环里。如果采样频率足够高，这种做法勉强可用；但当采样周期较大时，必须重新计算离散增益，否则可能出现闭环不稳定。

3. 环境准备与工具选型

3.1 用 Python 验证 LQR 的最小环境

本文所有仿真示例基于 Python 3.x，使用以下库：

• NumPy：矩阵运算。
• SciPy：求解黎卡提方程、连续系统离散化。
• Matplotlib：绘制状态曲线和控制曲线。

安装命令：

pip install numpy scipy matplotlib

版本没有太多约束，只要是较新的稳定版本即可。如果你用 Anaconda，这些库通常已经预装。

3.2 使用 MATLAB/Octave 或 ROS 仿真

如果你更熟悉 MATLAB，可以直接使用内置的lqr函数，计算一步到位。MATLAB 的 Control System Toolbox 还提供了完整的线性系统分析工具。

如果目标是机器人项目落地，推荐再加入 ROS 环境。LQR 的计算本身很简单，在 ROS 节点里本质上就是“订阅状态估计话题 → 计算 u = -Kx → 发布控制指令话题”的一个循环。本文第 5 节会给出 ROS 节点框架。

3.3 示例项目结构

为了让后续代码尽量贴近工程习惯，这里按如下结构组织：

lqr_demo/ ├── lqr_continuous.py # 连续系统 LQR 仿真 ├── lqr_discrete.py # 离散系统 LQR 仿真 ├── lqr_ros_node.py # ROS 节点核心框架 └── params.py # 模型参数与权重配置

在实际项目中，建议把参数和控制器计算拆开，方便后续替换模型和调整权重。

4. Python 完整实战：一维倒立摆 LQR 控制

4.1 建模与参数定义

我们用一个一维倒立摆作为被控对象。摆杆通过关节电机驱动，目标是在 θ = 0（竖直向上）位置保持平衡。

物理参数如下：

• 摆杆质量m = 0.2 kg
• 摆杆长度l = 0.5 m
• 重力加速度g = 9.81 m/s²

根据前面推导：

import numpy as np g = 9.81 l = 0.5 m = 0.2 A = np.array([ [0.0, 1.0], [g / l, 0.0] ]) B = np.array([ [0.0], [1.0 / (m * l * l)] ]) print("A =\n", A) print("B =\n", B)

输出：

A = [[ 0. 1. ] [19.62 0. ]] B = [[ 0.] [20.]]

这个模型告诉我们：系统矩阵 A 中存在不稳定极点sqrt(g/l)，如果不加控制，摆杆会迅速倒下。

4.2 求解黎卡提方程

先选择权重矩阵：

Q = np.diag([10.0, 1.0]) # 状态权重：角度 10，角速度 1 R = np.array([[0.1]]) # 控制权重：力矩代价 0.1

然后求解黎卡提方程：

from scipy.linalg import solve_continuous_are P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ (B.T @ P) print("Riccati 解 P =\n", P) print("LQR 增益 K =\n", K)

运行后会得到一个 1×2 的增益矩阵，例如：

Riccati 解 P = [[15.90463534 1.61920547] [ 1.61920547 0.38725318]] LQR 增益 K = [[19.97697126 7.74506355]]

这里的K就是我们要部署到控制器中的反馈增益。控制律为：

u = -K x = -19.98 * θ - 7.75 * θ̇

可以看到，LQR 输出本质上也是一个 P 控制器加 D 控制器，但系数是整个系统联合优化出来的。

4.3 连续系统仿真

使用scipy.integrate.solve_ivp对闭环系统做仿真。初始状态设为θ = 0.2 rad，角速度为 0。

from scipy.integrate import solve_ivp import matplotlib.pyplot as plt def closed_loop_dynamics(t, x): x = x.reshape(-1, 1) u = -K @ x xdot = A @ x + B @ u return xdot.flatten() x0 = [0.2, 0.0] t_span = (0.0, 5.0) sol = solve_ivp( closed_loop_dynamics, t_span, x0, method="RK45", max_step=0.01 ) # 绘图 plt.figure(figsize=(10, 4)) plt.subplot(1, 2, 1) plt.plot(sol.t, sol.y[0], label="θ") plt.plot(sol.t, sol.y[1], label="θ̇") plt.xlabel("t (s)") plt.ylabel("state") plt.legend() plt.grid(True) plt.title("State Response") # 计算控制量 u_log = [] for t, x in zip(sol.t, sol.y.T): x_col = x.reshape(-1, 1) u_log.append(-K @ x_col) u_log = np.array(u_log) plt.subplot(1, 2, 2) plt.plot(sol.t, u_log, label="u") plt.xlabel("t (s)") plt.ylabel("control torque (N·m)") plt.legend() plt.grid(True) plt.title("Control Input") plt.tight_layout() plt.show()

结果应该看到：摆角从 0.2 rad 指数收敛到 0，角速度先增大后收敛到 0，控制力矩也随状态收敛而趋近于 0。

4.4 离散系统仿真

实际代码运行在数字控制器中，所以离散仿真更贴近真实。先把连续系统离散化：

from scipy.signal import cont2discrete dt = 0.01 system_cont = (A, B, np.eye(2), np.zeros((2, 1))) Ad, Bd, Cd, Dd, _ = cont2discrete(system_cont, dt, method="zoh") print("Ad =\n", Ad) print("Bd =\n", Bd)

然后求解离散黎卡提方程：

from scipy.linalg import solve_discrete_are P_d = solve_discrete_are(Ad, Bd, Q, R) K_d = np.linalg.inv(R + Bd.T @ P_d @ Bd) @ (Bd.T @ P_d @ Ad) print("离散 LQR 增益 K_d =\n", K_d)

离散仿真循环：

N = int(10 / dt) x = np.array([[0.2], [0.0]]) x_log = [] u_log = [] for i in range(N): u = -K_d @ x x = Ad @ x + Bd @ u x_log.append(x.flatten()) u_log.append(u.flatten()) x_log = np.array(x_log) u_log = np.array(u_log) time = np.arange(N) * dt plt.figure(figsize=(10, 4)) plt.subplot(1, 2, 1) plt.plot(time, x_log[:, 0], label="θ") plt.plot(time, x_log[:, 1], label="θ̇") plt.xlabel("t (s)") plt.ylabel("state") plt.legend() plt.grid(True) plt.title("Discrete State Response") plt.subplot(1, 2, 2) plt.plot(time, u_log[:, 0], label="u") plt.xlabel("t (s)") plt.ylabel("control torque (N·m)") plt.legend() plt.grid(True) plt.title("Discrete Control Input") plt.tight_layout() plt.show()

4.5 结果说明

连续和离散仿真的结果应当基本一致，唯一的区别是离散仿真存在采样间隔带来的微小误差。只要采样周期相对系统动态足够小，ZOH 离散模型就能很好地近似连续系统。

这个小实验完整验证了 LQR 的工作流程：

1. 建模并线性化，得到 A、B 矩阵。
2. 选取 Q、R 权重矩阵。
3. 求解黎卡提方程得到 P 与 K。
4. 用 u = -Kx 构成闭环。
5. 仿真验证状态收敛和控制量合理。

5. 在 ROS 中集成 LQR 控制器

5.1 ROS 控制循环的基本思路

很多初学者会问“如何用 ROS 控制机器人”，其实 ROS 本身不负责控制算法，它只负责通信。LQR 在 ROS 中的落地方式非常直接：

• 订阅机器人的状态估计话题，获取当前状态 x。
• 在回调函数中计算控制量 u = -Kx。
• 将控制量发布到执行器接口话题。

关键在于：控制周期是否固定。ROS 中回调函数的触发时间不严格均匀，因此工程上通常用定时器或单独的 controller 线程来保证固定控制频率。控制频率低于模型要求时，系统可能不稳定。

5.2 节点代码框架

下面是一个 ROS 节点的核心框架，使用 Python 编写。实际运行需要先创建 ROS 功能包，这里只给出主逻辑。

#!/usr/bin/env python3 import rospy import numpy as np from std_msgs.msg import Float64MultiArray class LQRController: def __init__(self, K, rate=100.0): self.K = K self.rate = rate # 订阅状态估计话题 self.state_sub = rospy.Subscriber( "/state_estimate", Float64MultiArray, self.state_callback ) # 发布控制指令话题 self.control_pub = rospy.Publisher( "/control_input", Float64MultiArray, queue_size=1 ) self.latest_state = None rospy.loginfo("LQR controller initialized") def state_callback(self, msg): # 将 ROS 消息转为状态向量 self.latest_state = np.array(msg.data).reshape(-1, 1) def control_loop(self): rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): if self.latest_state is not None: x = self.latest_state u = -self.K @ x out = Float64MultiArray() out.data = [float(u[0, 0])] self.control_pub.publish(out) rate.sleep() if __name__ == "__main__": rospy.init_node("lqr_controller") # 这里的 K 应从模型计算得到，可读取自参数服务器或 yaml 配置 K_example = np.array([[19.98, 7.75]]) controller = LQRController(K_example, rate=100.0) try: controller.control_loop() except rospy.ROSInterruptException: pass

这里有几个工程要点：

• K矩阵不应该硬编码在节点代码里，建议放在参数服务器或 yaml 配置文件中，方便不同工况切换。
• 状态话题的数据顺序要与建模时一致，否则反馈符号会错乱。
• 控制频率 100 Hz 是常见选择，但要根据系统动态和模型确定。

5.3 与状态估计、执行器交互的注意事项

LQR 是状态反馈控制器，它默认“状态已知”。真实机器人上，状态来自编码器、IMU、视觉里程计等传感器的融合结果，因此实际项目中几乎不会只用 LQR，而是 LQR + 状态估计器一起工作。

常见做法是：

• 关节位置和速度由编码器直接测量。
• 姿态、角速度由 IMU 滤波得到。
• 如果状态中有无法直接测量的分量，引入卡尔曼滤波器或状态观测器。

另外，控制指令发布到执行器后，执行器本身可能也有底层 PID 环。此时 LQR 的输出往往作为“力矩指令”或“加速度指令”传给底层，而不是直接控制电机电压。需要根据你的硬件接口调整话题类型和单位。

6. Q 和 R 矩阵的调参方法论

6.1 Bryson 法则：第一个可用的初值

很多人第一次调 LQR 都会问：Q 和 R 到底怎么选？一个非常实用的经验法则是 Bryson 法则：

Q_ii = 1 / (最大允许状态偏差)² R_jj = 1 / (最大允许控制量)²

也就是说，你先问自己一个问题：这个状态量我允许它偏离多少？这个控制量我允许它有多大？然后取平方倒数作为权重。

举例来说，倒立摆允许摆角偏差 0.1 rad，角速度偏差 0.5 rad/s，最大控制力矩 0.2 N·m：

Q = diag([1/0.1², 1/0.5²]) = diag([100, 4]) R = diag([1/0.2²]) = [25]

这个初值通常不会太好，但会给你一个合理的起点。之后再根据仿真结果做局部调整。

6.2 调参流程

建议按下面流程调参，不要直接改权重然后看效果，那样容易迷失方向。

1. 固定 R，从小到大调整 Q。观察状态收敛速度。
2. 如果收敛太慢，增大 Q 中对应状态项的对角元素。
3. 如果控制量过大或出现饱和，增大 R。
4. 反复对比状态曲线和控制曲线，找到一个折中点。
5. 将最终参数写入配置文件，记录版本和效果。

关键是每次只调一个参数，并且记录前后对比。LQR 调参不是一次性的，模型变化、负载变化都会要求重新调参。

6.3 常见调参陷阱

在实际项目中，调参工作永远是被低估的。建议一开始就建立仿真环境，把参数调整和验证流程固化下来。

7. 常见问题与排查思路

7.1 常见问题速查表

7.2 排查流程建议

当你发现 LQR 系统表现不正常时，不要直接去调参数，可以按下面的顺序排查：

1. 开环验证模型：去掉控制器，让系统自由演化，观察是否符合物理直觉。
2. 单独检查增益：手动计算 K 是否过大或过小，画出 Bode 图或看极点位置。
3. 简化仿真：先在线性模型上跑，确认控制器逻辑正确，再加入非线性、噪声和执行器饱和。
4. 逐步加复杂度：从仿真到硬件，每个环节都保留回退点。

这套流程基本能覆盖大多数 LQR 落地问题。

8. 工程最佳实践与扩展方向

8.1 LQR + 卡尔曼滤波器 = LQG

LQR 假设状态全部可测。真实系统中，不少状态是估计出来的，直接使用带噪声的状态会导致控制效果下降。标准的工程组合是：

• LQR 负责“控制”。
• 卡尔曼滤波器负责“状态估计”。

两者合起来称为 LQG（Linear Quadratic Gaussian）。LQG 在机器人中应用非常广泛，值得作为 LQR 之后的下一站学习。

8.2 控制约束与抗饱和

LQR 本身不处理输入约束。如果控制量超出执行器限制，直接截断会破坏 LQR 的最优性，甚至导致极限环震荡。

工程中常用方法：

• 在仿真中加入饱和环节，通过调大 R 避免控制量频繁触碰边界。
• 引入 anti-windup 机制，防止积分类控制器饱和。
• 如果约束主要矛盾，可以直接切换到模型预测控制（MPC），将约束显式建模。

8.3 从仿真到实车的关键检查项

仿真到实车之间，最容易出问题的不是算法，而是接口。

• 确认状态话题单位是 rad 还是 degree。
• 确认控制指令是力矩还是 PWM 占空比。
• 确认传感器方向与模型方向一致。
• 先从很小的增益开始，手动验证控制方向。
• 加上控制量限幅和急停逻辑。

这些检查项每一条都可能在实车测试中避免一次事故。

8.4 LQR 的下一步：轨迹跟踪、无人机与 MPC

本文讨论的是调节器（Regulator），目标是让状态回归零点。实际机器人更多时候需要跟踪一条参考轨迹，这就涉及到轨迹跟踪 LQR，又叫 LQR 跟踪器（LQR tracker）。

另外，最近几年无人机控制、机械臂运动规划与 LQR 的结合也非常多。对于多旋翼无人机，内环姿态 controllers 常用 LQR；对于机械臂末端轨迹跟踪，LQR 可以配合运动规划使用。当你真正理解了 LQR，再去看这些应用会轻松很多。

9. 总结与下一步学习建议

到这里，LQR 的核心内容已经完整走了一遍。你需要确认自己掌握了以下几点：

• 能用动力学方程写出状态空间矩阵 A、B。
• 能解释 Q、R 的物理含义。
• 能使用 SciPy 或 MATLAB 求解黎卡提方程。
• 能写连续和离散闭环仿真。
• 能在 ROS 节点中实现 u = -Kx 控制循环。
• 能根据仿真结果反推参数调整方向。

LQR 的重点不是背公式，而是要把“建模 → 线性化 → 求解 → 仿真 → 调参 → 部署”这条链路跑通。建议你先用一维倒立摆把全流程复现一遍，再尝试加入传感器噪声、控制饱和和非线性项，逐步逼近真实机器人环境。

如果本文对你有帮助，可以收藏备用。下一步学习建议沿着这条路线走：先学卡尔曼滤波器，然后学 LQG，最后进入模型预测控制（MPC）或更现代的非线性控制方法。机器人控制这条路很长，但 LQR 是值得花时间打牢的一块地基。