车辆运动模型中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

欢迎大家讨论、交流！