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

2024-05-12 00:36

本文主要是介绍车辆运动模型中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代码实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/981104

相关文章

java之Objects.nonNull用法代码解读

《java之Objects.nonNull用法代码解读》:本文主要介绍java之Objects.nonNull用法代码,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐... 目录Java之Objects.nonwww.chinasem.cnNull用法代码Objects.nonN

Python如何使用__slots__实现节省内存和性能优化

《Python如何使用__slots__实现节省内存和性能优化》你有想过,一个小小的__slots__能让你的Python类内存消耗直接减半吗,没错,今天咱们要聊的就是这个让人眼前一亮的技巧,感兴趣的... 目录背景:内存吃得满满的类__slots__:你的内存管理小助手举个大概的例子:看看效果如何?1.

Python+PyQt5实现多屏幕协同播放功能

《Python+PyQt5实现多屏幕协同播放功能》在现代会议展示、数字广告、展览展示等场景中,多屏幕协同播放已成为刚需,下面我们就来看看如何利用Python和PyQt5开发一套功能强大的跨屏播控系统吧... 目录一、项目概述:突破传统播放限制二、核心技术解析2.1 多屏管理机制2.2 播放引擎设计2.3 专

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很

Java的IO模型、Netty原理解析

《Java的IO模型、Netty原理解析》Java的I/O是以流的方式进行数据输入输出的,Java的类库涉及很多领域的IO内容:标准的输入输出,文件的操作、网络上的数据传输流、字符串流、对象流等,这篇... 目录1.什么是IO2.同步与异步、阻塞与非阻塞3.三种IO模型BIO(blocking I/O)NI

idea中创建新类时自动添加注释的实现

《idea中创建新类时自动添加注释的实现》在每次使用idea创建一个新类时,过了一段时间发现看不懂这个类是用来干嘛的,为了解决这个问题,我们可以设置在创建一个新类时自动添加注释,帮助我们理解这个类的用... 目录前言:详细操作:步骤一:点击上方的 文件(File),点击&nbmyHIgsp;设置(Setti

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.

MySQL大表数据的分区与分库分表的实现

《MySQL大表数据的分区与分库分表的实现》数据库的分区和分库分表是两种常用的技术方案,本文主要介绍了MySQL大表数据的分区与分库分表的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有... 目录1. mysql大表数据的分区1.1 什么是分区?1.2 分区的类型1.3 分区的优点1.4 分

一文详解如何从零构建Spring Boot Starter并实现整合

《一文详解如何从零构建SpringBootStarter并实现整合》SpringBoot是一个开源的Java基础框架,用于创建独立、生产级的基于Spring框架的应用程序,:本文主要介绍如何从... 目录一、Spring Boot Starter的核心价值二、Starter项目创建全流程2.1 项目初始化(

Mysql删除几亿条数据表中的部分数据的方法实现

《Mysql删除几亿条数据表中的部分数据的方法实现》在MySQL中删除一个大表中的数据时,需要特别注意操作的性能和对系统的影响,本文主要介绍了Mysql删除几亿条数据表中的部分数据的方法实现,具有一定... 目录1、需求2、方案1. 使用 DELETE 语句分批删除2. 使用 INPLACE ALTER T