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

相关文章

pandas中位数填充空值的实现示例

《pandas中位数填充空值的实现示例》中位数填充是一种简单而有效的方法,用于填充数据集中缺失的值,本文就来介绍一下pandas中位数填充空值的实现,具有一定的参考价值,感兴趣的可以了解一下... 目录什么是中位数填充?为什么选择中位数填充?示例数据结果分析完整代码总结在数据分析和机器学习过程中,处理缺失数

Golang HashMap实现原理解析

《GolangHashMap实现原理解析》HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持高效的插入、查找和删除操作,:本文主要介绍GolangH... 目录HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持

Pandas使用AdaBoost进行分类的实现

《Pandas使用AdaBoost进行分类的实现》Pandas和AdaBoost分类算法,可以高效地进行数据预处理和分类任务,本文主要介绍了Pandas使用AdaBoost进行分类的实现,具有一定的参... 目录什么是 AdaBoost?使用 AdaBoost 的步骤安装必要的库步骤一:数据准备步骤二:模型

使用Pandas进行均值填充的实现

《使用Pandas进行均值填充的实现》缺失数据(NaN值)是一个常见的问题,我们可以通过多种方法来处理缺失数据,其中一种常用的方法是均值填充,本文主要介绍了使用Pandas进行均值填充的实现,感兴趣的... 目录什么是均值填充?为什么选择均值填充?均值填充的步骤实际代码示例总结在数据分析和处理过程中,缺失数

Java对象转换的实现方式汇总

《Java对象转换的实现方式汇总》:本文主要介绍Java对象转换的多种实现方式,本文通过实例代码给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录Java对象转换的多种实现方式1. 手动映射(Manual Mapping)2. Builder模式3. 工具类辅助映

Go语言开发实现查询IP信息的MCP服务器

《Go语言开发实现查询IP信息的MCP服务器》随着MCP的快速普及和广泛应用,MCP服务器也层出不穷,本文将详细介绍如何在Go语言中使用go-mcp库来开发一个查询IP信息的MCP... 目录前言mcp-ip-geo 服务器目录结构说明查询 IP 信息功能实现工具实现工具管理查询单个 IP 信息工具的实现服

利用Python调试串口的示例代码

《利用Python调试串口的示例代码》在嵌入式开发、物联网设备调试过程中,串口通信是最基础的调试手段本文将带你用Python+ttkbootstrap打造一款高颜值、多功能的串口调试助手,需要的可以了... 目录概述:为什么需要专业的串口调试工具项目架构设计1.1 技术栈选型1.2 关键类说明1.3 线程模

SpringBoot基于配置实现短信服务策略的动态切换

《SpringBoot基于配置实现短信服务策略的动态切换》这篇文章主要为大家详细介绍了SpringBoot在接入多个短信服务商(如阿里云、腾讯云、华为云)后,如何根据配置或环境切换使用不同的服务商,需... 目录目标功能示例配置(application.yml)配置类绑定短信发送策略接口示例:阿里云 & 腾

Python Transformers库(NLP处理库)案例代码讲解

《PythonTransformers库(NLP处理库)案例代码讲解》本文介绍transformers库的全面讲解,包含基础知识、高级用法、案例代码及学习路径,内容经过组织,适合不同阶段的学习者,对... 目录一、基础知识1. Transformers 库简介2. 安装与环境配置3. 快速上手示例二、核心模

python实现svg图片转换为png和gif

《python实现svg图片转换为png和gif》这篇文章主要为大家详细介绍了python如何实现将svg图片格式转换为png和gif,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录python实现svg图片转换为png和gifpython实现图片格式之间的相互转换延展:基于Py