车辆运动模型中LQR代码实现

一、前言

最近看到关于架构和算法两者关系的一个描述，我觉得非常认同，分享给大家。

1、好架构起到两个作用：合理的分解功能、合理的适配算法；
2、好的架构是好的功能的必要条件，不是充分条件，一味追求架构的完美是不可取的，并且不存在完美的架构，只存在合适的架构；
3、好的架构可以一定层度上提升算法的能力，但功能开发不能将重心全放在框架上，应该追求框架+算法的适配，从而实现 1+1 > 2 。

**我的理解是这样的：**架构的设计一定要结合功能、硬件、人力、成本、时间等多种因素，在此基础上尽量去满足算法的低耦合、高内聚。

二、LQR跟踪效果视频

说明：LQR跟踪比较依赖路径的平滑度（路径曲率的变化）

LQR路径跟踪

三、车辆运动学模型的详细推导

上一篇文章中（LQR原理及其在路径跟踪的应用，http://t.csdnimg.cn/4YDos），有人私信问关于车辆运动学模型的推导过程，这里我进行更详细的推导
在这里插入图片描述

到这里就应该没什么问题了吧？再通过向前欧拉法离散化，套公式即可求解；

四、差速模型的详细推导

其一：为了举一反三，所有对差速模型也进行了推导；
其二：即使控制对象是车辆模型，也可以先使用差速模型先进行计算得到v,w，然后根据车辆的模型二次解算得到速度和转向角。
在这里插入图片描述

五、具体代码实现

注意事项：
1、使用lqr跟踪一段轨迹，轨迹中的v,w不知道的情况下可以给0，但那么lqr中的调节矩阵R就应该尽量的小;
2、轨迹中的yaw尽量保证突变较小（没有前置轨迹平滑的基础可以简单用均值滤波），否则lqr无法收敛，跟踪效果很差；
3、以差速模型计算得到v,w ，可以再车辆的模型二次解算得到速度和转向角。

/**
 * lqr_controler.hpp
 * @brief  lqr控制器 构建的是一个差速机器人模型
 * @author MCE
 * @date 2024-5-9
 */
#ifndef LQR_CONTROLER
#define LQR_CONTROLER
#include <math.h>
#include <Eigen/Dense>
#include <iostream>
#include "utils.hpp"
namespace lqr_control {
using namespace std;
using namespace Eigen;
using namespace lqr_control;

class LQRControler {
public:
    LQRControler(){};
    /**
     * @brief lqr 初始化
     * @return void
     */
    void init();

    /**
     * @brief lqr 控制器 输出 v, w
     * @param robot_state 机器人状态
     * @param front_point 预瞄点
     * @param v 以引用的方式输出v
     * @param w 以引用的方式输出w
     * @return void
     */
    void lqrControl(const state& robot_state, const state& front_point, float& v, float& w);

private:
    MatrixXf A;
    MatrixXf B;
    MatrixXf Q;
    MatrixXf R;

    // P矩阵最多迭代次数
    int n;
    // 离散化控制周期
    float dt;
    // P容许误差
    float eps;

    /**
     * @brief 黎卡迪计算方程
     * @return MatrixXf P矩阵
     */
    MatrixXf calRicatti();

    /**
     * @brief 计算AB 矩阵
     * @return void
     */
    void calAB(const state& front_point);
};
} // namespace lqr_control
#endif

/**
 * lqr_controler.cpp
 * @brief  lqr控制器
 * @author MCE
 * @date 2024-5-9
 */
#include "../include/lqr_controler.hpp"

namespace lqr_control {

void LQRControler::init() {
    A.resize(3, 3);
    A.setZero();
    B.resize(3, 2);
    B.setZero();

    Q.resize(3, 3);
    Q << 1.0, 0.0, 0.0,
         0.0, 1.0, 0.0, 
         0.0, 0.0, 1.0; 
    R.resize(2, 2);
    R << 0.001, 0.0, 
         0.0, 0.001; 

    n = 100;
    dt = 0.02;
    eps = 1.0e-4;
}

void LQRControler::calAB(const state& front_point) {
    A << 1.0, 0.0, -front_point.linear_vel * dt * sin(front_point.yaw), 0.0, 1.0, front_point.linear_vel * dt * cos(front_point.yaw), 0.0, 0.0, 1.0;
    B << dt * cos(front_point.yaw), 0.0, dt * sin(front_point.yaw), 0.0, 0.0, dt;
}

// 离散时间Riccati方程求解函数
MatrixXf LQRControler::calRicatti() {
    MatrixXf P = Q;
    for (int i = 0; i < n; ++i) {
        MatrixXf P_next = A.transpose() * P * A - A.transpose() * P * B * (R + B.transpose() * P * B).inverse() * B.transpose() * P * A + Q;
        if ((P_next - P).norm() < eps) {
            P = P_next;
            break;
        }
        P = P_next;
    }
    return P;
}

void LQRControler::lqrControl(const state& robot_state, const state& front_point, float& v, float& w) {
    MatrixXf X(3, 1);
    X << robot_state.x - front_point.x, robot_state.y - front_point.y, angleNormalize(robot_state.yaw - front_point.yaw);
    calAB(front_point);
    MatrixXf P = calRicatti();
    MatrixXf K = -(R + B.transpose() * P * B).inverse() * B.transpose() * P * A;
    MatrixXf U = K * X;
    v = U(0, 0);
    w = U(1, 0);
}
} // namespace lqr_control