轨迹规划 | 图解最优控制LQR算法(附ROS C++/Python/Matlab仿真)

轨迹规划与LQR算法

在机器人控制和自动驾驶等领域，轨迹规划是一个至关重要的任务。它主要涉及在已知环境中，规划出一条最佳路径，以使得移动体能够安全、高效地到达目标位置。最优控制理论中的线性二次调节器（LQR，Linear Quadratic Regulator）是一种常用的方法，用于解决线性系统的最优控制问题。

LQR算法概述

LQR的基本思想是通过最小化一个二次成本函数来达到最优控制目的。该成本函数通常包含状态变量和控制输入的加权平方和。具体形式如下：

[ J = \int_0^T (x^T Q x + u^T R u) dt ]

其中，( J )是总成本，( x )是状态向量，( u )是控制输入，( Q )和( R )是权重矩阵，决定了状态误差和控制能量的相对重要性。

LQR的步骤主要包括以下几点：

1. 构建状态空间模型：将系统动态描述为线性状态方程。
2. 定义成本函数：设定合适的权重矩阵。
3. 求解Riccati方程：使用代数Riccati方程求解出增益矩阵。
4. 实施控制策略：根据控制律进行实时控制。

ROS中的LQR实现

下面是一个使用Python和ROS实现LQR的简单示例。假设我们有一个简单的线性动态系统：

[ \dot{x} = Ax + Bu ]

示例代码

import numpy as np
import rospy
from geometry_msgs.msg import Pose2D

# 定义状态空间矩阵
A = np.array([[0, 1],
              [0, 0]])
B = np.array([[0],
              [1]])
Q = np.diag([1, 0])  # 状态权重
R = np.array([[1]])   # 控制权重

# 计算LQR控制增益
def lqr(A, B, Q, R):
    from scipy.linalg import solve_continuous_are
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.inv(R) @ B.T @ P
    return K

# 初始化ROS节点
rospy.init_node('lqr_ctrl_node')

# 主循环
def main():
    K = lqr(A, B, Q, R)
    x = np.array([[0], [0]])  # 初始状态
    dt = 0.1  # 控制周期
    rate = rospy.Rate(10)  # 10Hz

    while not rospy.is_shutdown():
        # 控制输入
        u = -K @ x
        # 更新状态
        x_dot = A @ x + B @ u
        x += x_dot * dt  # Euler积分法

        # 发布当前状态
        pose_msg = Pose2D()
        pose_msg.x = x[0, 0]
        pose_msg.y = x[1, 0]
        rospy.loginfo("Current state: x: %f, v: %f" % (pose_msg.x, pose_msg.y))

        rate.sleep()

if __name__ == '__main__':
    main()

MATLAB中的LQR实现

在MATLAB中实现LQR也非常简单，以下是一个完整的代码示例：

A = [0 1; 0 0];
B = [0; 1];
Q = [1 0; 0 0];  % 状态权重
R = 1;           % 控制权重

% 计算LQR控制增益
[K, S, e] = lqr(A, B, Q, R);

% 初始化状态
x = [0; 0];  % 初始状态
dt = 0.1;    % 控制周期
T = 10;      % 总时间

for t = 0:dt:T
    u = -K*x;  % 控制输入
    x_dot = A*x + B*u;  % 状态更新
    x = x + x_dot*dt;  % 使用Euler法进行积分

    disp(['Current state: x=' num2str(x(1)) ', v=' num2str(x(2))]);
end

总结

LQR算法是一种有效的轨迹规划方法，能够为线性系统提供实时控制方案。通过适当选择权重矩阵，我们可以在状态误差和控制能量之间找到平衡，从而实现最优控制。在ROS和MATLAB中实现LQR都是比较直接的，开发者可以根据具体需求进行调整与优化。