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

最优控制LQR（Linear Quadratic Regulator）是一种常用的算法，用于在动态系统中找到使得系统输出信号的期望平方最小的控制输入。以下是一个简化的ROS中使用LQR进行轨迹规划的示例代码：

// ROS C++ 示例
#include <ros/ros.h>
#include <lqrrt_ros/LQR.h>
 
int main(int argc, char **argv) {
    ros::init(argc, argv, "lqr_example");
    ros::NodeHandle nh;
 
    // 初始化LQR对象
    lqrrt_ros::LQR lqr;
 
    // 设置系统矩阵和状态权重
    lqr.setA(Eigen::MatrixXd::Identity(2, 2)); // 状态转移矩阵
    lqr.setB(Eigen::MatrixXd::Identity(2, 1)); // 状态-控制矩阵
    lqr.setQ(Eigen::MatrixXd::Identity(2, 2)); // 状态权重矩阵
    lqr.setR(Eigen::MatrixXd::Identity(1, 1)); // 控制权重矩阵
 
    // 设置初始状态和终止条件
    lqr.setState(Eigen::Vector2d(0.0, 0.0)); // 初始状态
    lqr.setTermCond(100); // 迭代次数终止条件
 
    // 计算控制输入
    lqr.compute();
 
    // 打印结果
    ROS_INFO("LQR gains: K = %f, L = %f", lqr.getK(), lqr.getL());
 
    return 0;
}

# Python 示例
import numpy as np
from lqrrt import LQR
 
# 初始化LQR对象
lqr = LQR()
 
# 设置系统矩阵和状态权重
lqr.setA(np.eye(2))  # 状态转移矩阵
lqr.setB(np.eye(2))  # 状态-控制矩阵
lqr.setQ(np.eye(2))  # 状态权重矩阵
lqr.setR(np.eye(1))  # 控制权重矩阵
 
# 设置初始状态和终止条件
lqr.setState(np.array([0.0, 0.0]))  # 初始状态
lqr.setTermCond(100)  # 迭代次数终止条件
 
# 计算控制输入
lqr.compute()
 
# 打印结果
print(f"LQR gains: K = {lqr.getK()}, L = {lqr.getL()}")

% MATLAB 示例
A = eye(2); % 状态转移矩阵
B = eye(2); % 状态-控制矩阵
Q = eye(2); % 状态权重矩阵
R = eye(1); % 控制权重矩阵
 
% 初始化LQR对象
lqr = lqr(A, B, Q, R);
 
% 设置初始状态和终止条件
x0 = [0; 0]; % 初始状态
lqr.setTermCond('MaxIters', 100); % 迭代次数终止条件
 
% 计算控制输入
[K, L] = lqr(lqr, x0);
 
% 打印结果
fprintf('LQR gains: K = %f, L = %f\n', K, L);

以上代码仅展示了如何初始化LQR对象、设置系统矩阵和权重、计