本文主要是介绍车辆运动模型中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
欢迎大家讨论、交流!
这篇关于车辆运动模型中LQR代码实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!