LQR 最优控制算法

什么是 LQR？

LQR（Linear Quadratic Regulator，线性二次调节器）是现代控制理论中最重要的算法之一。它通过求解最优化问题来计算最优的反馈增益，使系统在满足物理约束的情况下达到最优性能。

LQR 的基本概念

LQR 的目标是通过设计控制输入，使得系统的状态接近期望值，同时最小化控制能量消耗。这个问题可以形式化为：

最小化目标函数：

其中：

是系统状态向量
是控制输入向量
是状态权重矩阵（对角矩阵）
是控制输入权重矩阵（对角矩阵）
更大的值意味着更强调状态精度
更大的值意味着更关心控制能量消耗

LQR 与 VMC 的区别

特性	VMC	LQR
理论基础	虚拟弹簧-阻尼模型	最优控制理论
计算复杂度	低	中等
适用范围	腿部轨迹跟踪	整体身体控制
实时性	优秀	良好
鲁棒性	基于模型	基于最优性

线性系统与状态空间表示

连续时间线性系统

LQR 适用于线性系统，其模型为：

其中：

：状态向量
：控制输入向量
：系统矩阵
：输入矩阵

离散时间线性系统

对于数字控制系统，使用离散模型：

其中和是离散化后的矩阵。

LeBot 的状态空间模型

对于 LeBot 的身体运动控制，状态向量可以定义为：

其中：

：身体在平面上的位置
：身体方向角
：身体速度
：角速度

控制输入为：

其中是合力，是合力矩。

LQR 算法原理

最优反馈控制律

LQR 的最优控制律是线性反馈形式：

其中是反馈增益矩阵。

求解 LQR 问题

反馈增益矩阵通过求解代数Riccati方程得到：

连续时间情况：

离散时间情况：

其中是对称正定矩阵，反馈增益为：

连续时间：

离散时间：

Python 实现

使用 scipy 求解 LQR

python

import numpy as np
from scipy.linalg import solve_continuous_are, solve_discrete_are

class LQRController:
    """LQR 控制器"""
    
    def __init__(self, A, B, Q, R, dt=None):
        """
        初始化 LQR 控制器
        
        参数：
            A: 系统矩阵
            B: 输入矩阵
            Q: 状态权重矩阵
            R: 输入权重矩阵
            dt: 采样时间（如果为 None，则使用连续时间）
        """
        self.A = A
        self.B = B
        self.Q = Q
        self.R = R
        self.dt = dt
        
        # 求解 LQR 问题
        if dt is None:
            # 连续时间 LQR
            self.P = solve_continuous_are(A, B, Q, R)
            self.K = np.linalg.inv(R) @ B.T @ self.P
        else:
            # 离散时间 LQR
            self.P = solve_discrete_are(A, B, Q, R)
            self.K = np.linalg.inv(R + B.T @ self.P @ B) @ B.T @ self.P @ A
    
    def compute_control(self, x, x_ref=None):
        """
        计算控制输入
        
        参数：
            x: 当前状态
            x_ref: 参考状态（默认为零）
        
        返回：
            控制输入 u
        """
        if x_ref is None:
            x_ref = np.zeros_like(x)
        
        # 计算误差
        error = x - x_ref
        
        # 计算控制输入
        u = -self.K @ error
        
        return u
    
    def get_gains(self):
        """获取反馈增益矩阵"""
        return self.K.copy()
    
    def get_riccati_matrix(self):
        """获取 Riccati 矩阵 P"""
        return self.P.copy()


# LeBot 身体控制示例
def design_lebot_lqr():
    """为 LeBot 设计 LQR 控制器"""
    
    # 系统参数
    m = 5.0  # 机器人质量 (kg)
    I = 0.1  # 转动惯量 (kg·m^2)
    
    # 连续时间状态空间模型
    # 状态: [x, y, theta, vx, vy, omega]^T
    # 输入: [ax, ay, alpha]^T (加速度和角加速度)
    
    A = np.array([
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 0, 1, 0],
        [0, 0, 0, 0, 0, 1],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0]
    ], dtype=float)
    
    B = np.array([
        [0, 0, 0],
        [0, 0, 0],
        [0, 0, 0],
        [1/m, 0, 0],
        [0, 1/m, 0],
        [0, 0, 1/I]
    ], dtype=float)
    
    # 权重矩阵
    # 强调位置和角度的精度，对速度要求适中
    Q = np.diag([100, 100, 50, 10, 10, 5])
    
    # 输入权重 - 平衡控制能量消耗
    R = np.diag([1, 1, 1])
    
    # 创建 LQR 控制器
    lqr = LQRController(A, B, Q, R)
    
    return lqr


# 使用示例
if __name__ == "__main__":
    lqr = design_lebot_lqr()
    K = lqr.get_gains()
    print("反馈增益矩阵 K:")
    print(K)
    
    # 测试控制
    x = np.array([0.5, 0.3, 0.1, 0.2, 0.1, 0.05])  # 当前状态
    u = lqr.compute_control(x)
    print(f"\n当前状态: {x}")
    print(f"控制输入: {u}")

跟踪参考轨迹的 LQR

python

class TrackingLQRController:
    """用于跟踪参考轨迹的 LQR 控制器"""
    
    def __init__(self, A, B, Q, R, dt=None):
        """初始化跟踪 LQR 控制器"""
        self.lqr = LQRController(A, B, Q, R, dt)
        self.reference_trajectory = []
    
    def set_reference_trajectory(self, trajectory):
        """
        设置参考轨迹
        
        参数：
            trajectory: 参考轨迹列表 [x_ref, y_ref, theta_ref, ...]
        """
        self.reference_trajectory = trajectory
    
    def compute_tracking_control(self, x, k):
        """
        计算跟踪控制输入
        
        参数：
            x: 当前状态
            k: 当前时步
        
        返回：
            控制输入 u
        """
        if k < len(self.reference_trajectory):
            x_ref = self.reference_trajectory[k]
        else:
            x_ref = self.reference_trajectory[-1]
        
        # 计算控制输入
        u = self.lqr.compute_control(x, x_ref)
        
        return u

LQR 参数调整

权重矩阵 Q 的选择

矩阵的对角元素表示对每个状态变量的重视程度：

python

# 示例 1：严格控制位置，忽视速度
Q1 = np.diag([1000, 1000, 100, 1, 1, 1])

# 示例 2：平衡位置和速度控制
Q2 = np.diag([100, 100, 50, 10, 10, 5])

# 示例 3：关注快速响应
Q3 = np.diag([50, 50, 25, 50, 50, 25])

权重矩阵 R 的选择

矩阵的对角元素表示对控制能量消耗的关注程度：

python

# 示例 1：能量消耗关键（如电池供电系统）
R1 = np.diag([100, 100, 100])

# 示例 2：平衡能量和性能
R2 = np.diag([1, 1, 1])

# 示例 3：强调快速控制
R3 = np.diag([0.1, 0.1, 0.1])

离散时间 LQR 实现

对于实际的数字控制系统：

python

class DiscreteLQRController:
    """离散时间 LQR 控制器"""
    
    def __init__(self, A_continuous, B_continuous, Q, R, dt):
        """
        初始化离散时间 LQR
        
        参数：
            A_continuous: 连续系统矩阵
            B_continuous: 连续输入矩阵
            Q: 状态权重
            R: 输入权重
            dt: 采样时间
        """
        # 离散化
        self.A_d = np.eye(A_continuous.shape[0]) + A_continuous * dt
        self.B_d = B_continuous * dt
        
        # 求解离散 LQR
        self.P = solve_discrete_are(self.A_d, self.B_d, Q, R)
        self.K = np.linalg.inv(R + self.B_d.T @ self.P @ self.B_d) @ self.B_d.T @ self.P @ self.A_d
    
    def compute_control(self, x):
        """计算控制输入"""
        return -self.K @ x
    
    def simulate(self, x0, T, disturbance=None):
        """
        模拟闭环系统
        
        参数：
            x0: 初始状态
            T: 模拟时间步数
            disturbance: 外部扰动
        
        返回：
            状态和控制输入的历史
        """
        x = x0.copy()
        x_history = [x.copy()]
        u_history = []
        
        for t in range(T):
            # 计算控制输入
            u = self.compute_control(x)
            u_history.append(u.copy())
            
            # 应用控制并更新状态
            x = self.A_d @ x + self.B_d @ u
            
            # 添加扰动（如果有）
            if disturbance is not None:
                x += disturbance(t)
            
            x_history.append(x.copy())
        
        return np.array(x_history), np.array(u_history)


# 性能评估
def evaluate_controller_performance(x_history, u_history, Q, R):
    """
    评估控制器性能
    
    参数：
        x_history: 状态历史
        u_history: 控制输入历史
        Q: 状态权重
        R: 输入权重
    
    返回：
        总成本
    """
    J = 0
    for x, u in zip(x_history[:-1], u_history):
        J += (x @ Q @ x) + (u @ R @ u)
    return J

LeBot 实际应用

身体稳定控制

python

def lebot_body_stabilization():
    """LeBot 身体稳定控制"""
    
    # 系统参数
    m = 5.0
    I = 0.1
    dt = 0.01  # 10 ms 采样时间
    
    # 连续系统
    A = np.array([
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 0, 1, 0],
        [0, 0, 0, 0, 0, 1],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0]
    ], dtype=float)
    
    B = np.array([
        [0, 0, 0],
        [0, 0, 0],
        [0, 0, 0],
        [1/m, 0, 0],
        [0, 1/m, 0],
        [0, 0, 1/I]
    ], dtype=float)
    
    # 权重矩阵
    Q = np.diag([100, 100, 50, 10, 10, 5])
    R = np.diag([1, 1, 1])
    
    # 创建离散 LQR 控制器
    lqr = DiscreteLQRController(A, B, Q, R, dt)
    
    # 初始状态（稍微偏离平衡）
    x0 = np.array([0.1, 0.05, 0.02, 0.0, 0.0, 0.0])
    
    # 模拟
    x_history, u_history = lqr.simulate(x0, T=500)
    
    # 评估性能
    J = evaluate_controller_performance(x_history, u_history, Q, R)
    
    print(f"总成本: {J:.2f}")
    print(f"最终状态: {x_history[-1]}")
    
    return lqr, x_history, u_history

最佳实践

权重选择：从相等权重开始，根据需要调整
系统辨识：准确识别系统矩阵 A 和 B
离散化：使用合适的方法离散化连续系统
性能验证：在仿真中验证性能后再实施
鲁棒性分析：考虑模型不确定性和扰动

总结

LQR 是一种强大的最优控制算法：

基于坚实的控制理论基础
计算相对简单，易于实现
提供最优的控制性能
适合整体身体运动控制

在 LeBot 中，LQR 可用于：

身体稳定控制
轨迹跟踪
扰动拒绝
多任务协调

参考资源

现代控制理论教科书
MATLAB Control System Toolbox 文档
"Linear Quadratic Control" - IEEE Control Systems Magazine

LQR 最优控制算法 ​

什么是 LQR？ ​

LQR 的基本概念 ​

LQR 与 VMC 的区别 ​

线性系统与状态空间表示 ​

连续时间线性系统 ​

离散时间线性系统 ​

LeBot 的状态空间模型 ​

LQR 算法原理 ​

最优反馈控制律 ​

求解 LQR 问题 ​

Python 实现 ​

使用 scipy 求解 LQR ​

跟踪参考轨迹的 LQR ​

LQR 参数调整 ​

权重矩阵 Q 的选择 ​

权重矩阵 R 的选择 ​

离散时间 LQR 实现 ​

LeBot 实际应用 ​

身体稳定控制 ​

最佳实践 ​

总结 ​

参考资源 ​

LQR 最优控制算法

什么是 LQR？

LQR 的基本概念

LQR 与 VMC 的区别

线性系统与状态空间表示

连续时间线性系统

离散时间线性系统

LeBot 的状态空间模型

LQR 算法原理

最优反馈控制律

求解 LQR 问题

Python 实现

使用 scipy 求解 LQR

跟踪参考轨迹的 LQR

LQR 参数调整

权重矩阵 Q 的选择

权重矩阵 R 的选择

离散时间 LQR 实现

LeBot 实际应用

身体稳定控制

最佳实践

总结

参考资源