自动驾驶——【规划】记忆泊车特殊学习路径拟合

1.Back ground

在这里插入图片描述
如上图,SLAM学习路线Start到End路径,其中曲线SDAB为D档位学习路径,曲线BC为R学习路径,曲线AE为前进档D档学习路径。
为了使其使用记忆泊车时,其驾驶员体验感好,需去除R档倒车部分轨迹,并拟合一条可用的曲线

2.Algorithm Introduction

在这里插入图片描述
D点作为起点,D(XD,YD,theta_D),C点作为终点(XC,YC,theta_C),使用y = a0 + a1 * x + a2 * x^2 + a3 * x^3拟合曲线DC,有:
YC = a0 + a1 * XC+ a2 * XC ^2 + a3 * XC^3
YD = a0 + a1 * XD + a2 * XD ^2 + a3 * XD ^3
tan(theta_C) = a1 + 2 * a2 * XC + 3 * a3 * XC^2
tan(theta_D) = a1 + 2 * a2 * XD + 3 * a3 * XD^2
即可求解a0 a1 a2 a3,进而得出曲线DC。
最后优化的曲线为SDCE。

3.Coding using MATLAB

%Function:记忆泊车学习路径拟合
%Create by:Juchunyu
%Date:2023-09-01 17:00:42%设计轨迹x,y
% y = 2 (10>=x>=0)
% y = -1.2/50 *x^2 - 4.4/10 *x   (10>=x>=5)
% y = 1.6 (20>=x>=5)
slam_x     = [];
slam_y     = [];
slam_theta = [];
GearInfo   = [];%D:4 R:2
D  = 4;
R  = 2;
%Generate trajpoint
for i = 0 : 0.2 :10slam_x   = [slam_x i];slam_y   = [slam_y 2];GearInfo = [GearInfo D];slam_theta = [slam_theta 0];
end
for i =10:-0.2:5slam_x   = [slam_x i];y_temp   = -1.2*i*i/50 + 4.4 * i/10;slam_y   = [slam_y y_temp];GearInfo = [GearInfo R];slam_theta_temp = -2.4*i/50 - 4.4/10;slam_theta = [slam_theta slam_theta_temp];
endfor i = 5:0.2:20slam_x   = [slam_x i];slam_y   = [slam_y 1.6];GearInfo = [GearInfo D];slam_theta = [slam_theta 0];
endfigure(1)
plot(slam_x,slam_y);
title('SLAM学习曲线')
hold on 
%%处理算法%检测倒车 只检测一次倒车
Index_start = 0;
Index_end   = 0;
Index_startArr = [];
Index_endArr   = [];[m_ size_] = size(slam_x);while i < size_Index_start = 0;Index_end   = 0;finish_Flag = 0;if(GearInfo(1,i) == R)Index_start = i;j = Index_start;while j < size_if GearInfo(1,j) == DIndex_end   = j;finish_Flag = 1;break;endj = j + 1;  endif(finish_Flag == 1)Index_startArr = [Index_startArr Index_start];Index_endArr   = [Index_endArr Index_end];endi = j;endi = i + 1;
endPointCIndx = Index_endArr(1,1);
PointBIndx = Index_startArr(1,1); 
PointAIndx = 0;
%处理算法
% find near Point
min_ = 1000000;
for i = 1:1:Index_startArr(1,1)dist = ((slam_x(1,PointCIndx) - slam_x(1,i))^2 + (slam_y(1,PointCIndx) - slam_y(1,i))^2)^(0.5);if(dist < min_)min_       =  dist;PointAIndx = i;end
end%计算DAdistDA = ((slam_x(1,PointAIndx) - slam_x(1,1))^2 + (slam_y(1,PointAIndx) - slam_y(1,1))^2)^(0.5);%往前推算1m
PointDIndx = PointAIndx;
if(distDA > 1.0)for i = PointAIndx:-1:1dist_  = ((slam_x(1,PointAIndx) - slam_x(1,i))^2 + (slam_y(1,PointAIndx) - slam_y(1,i))^2)^(0.5);if(dist_ > 1.0)PointDIndx = i;break; endend
end%处理D点到C点曲线平滑
PointDx = slam_x(1,PointDIndx);
PointDy = slam_y(1,PointDIndx);PointCx = slam_x(1,PointCIndx);
PointCy = slam_y(1,PointCIndx);
%A*X = BA(1,1) = 1;
A(1,2) = PointCx;
A(1,3) = PointCx * PointCx;
A(1,4) = PointCx * PointCx * PointCx;A(2,1) = 1;
A(2,2) = PointDx;
A(2,3) = PointDx * PointDx;
A(2,4) = PointDx * PointDx * PointDx;A(3,1) = 0;
A(3,2) = 1;
A(3,3) = 2 * PointCx;
A(3,4) = 3 * PointCx * PointCx;A(4,1) = 0;
A(4,2) = 1;
A(4,3) = 2 * PointDx;
A(4,4) = 3 * PointDx * PointDx;B(1,1) = PointCy;
B(2,1) = PointDy;
B(3,1) = tan(slam_theta(1,PointCIndx));
B(4,1) = tan(slam_theta(1,PointDIndx));X = A^-1 * B;%%拟合曲线系数
a0 = X(1,1);
a1 = X(2,1);
a2 = X(3,1);
a3 = X(4,1);%重组轨迹曲线
slam_Xfinal = [];
slam_Yfinal = [];
slam_thetaFinal = [];
for i = 1:1:PointDIndxslam_Xfinal = [slam_Xfinal slam_x(1,i)];slam_Yfinal = [slam_Yfinal slam_y(1,i)];slam_thetaFinal = [slam_thetaFinal slam_theta(1,i)];
end%拟合曲线DC
for x = PointDx:0.2:PointCxslam_Xfinal = [slam_Xfinal x];y_temp      = a0 + a1 * x + a2 * x^2 + a3 * x^3;theta_temp  = a1 + 2 * a2 * x + 3 * a3 *x^2;slam_Yfinal = [slam_Yfinal y_temp];slam_thetaFinal = [slam_thetaFinal theta_temp]; 
end%组合后部分曲线
for i = PointCIndx:1:size_slam_Xfinal = [slam_Xfinal slam_x(1,i)];slam_Yfinal = [slam_Yfinal slam_y(1,i)];slam_thetaFinal = [slam_thetaFinal slam_theta(1,i)];
endhold on figure(2)
plot(slam_Xfinal,slam_Yfinal,'r');
title('处理后的SLAM学习曲线')

在这里插入图片描述
在这里插入图片描述

4.Exist Problems

但是存在问题,
(1) AC距离很小的时候的处理
(2) 学习路线中多次倒车的处理
(3) DC在X轴方向投影距离很小时的处理。

2030901
鞠春宇

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.rhkb.cn/news/116319.html

如若内容造成侵权/违法违规/事实不符,请联系长河编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

2023最新 Electron.js 桌面应用开发教程(基础篇)更新中

Electron是什么&#xff1f; Electron是一个使用 JavaScript、HTML 和 CSS 构建桌面应用程序的框架。 嵌入 Chromium 和 Node.js 到 二进制的 Electron 允许您保持一个 JavaScript 代码代码库并创建 在Windows上运行的跨平台应用 macOS和Linux Electron Fiddle 运行实例 Ele…

在CAD中批量测量线段的长度

我们经常会有批量测量线段长度的需求&#xff0c;但是线段太多了&#xff0c;一个个加属实浪费时间。网上对这方面也只是个别晦涩的解决方法&#xff0c;大部分无法实用&#xff0c;本文介绍了笔者认为一种最轻松的测量办法。 下图即我们需要测量的线段组&#xff0c;正常测肯定…

python后端,一个账户,多设备登录管理

一个账号&#xff0c;多台设备同时登陆的问题&#xff0c;设计以及实现 参考这篇文章&#xff1a; https://www.alibabacloud.com/help/zh/tair/use-cases/manage-multi-device-logon-from-a-single-user-by-using-tairhash1.0 设计思路 利用的是Redis&#xff0c;主设备的保…

CAD图纸加密软件——公司核心文件数据防泄密「天锐绿盾」

PC访问地址&#xff1a; isite.baidu.com/site/wjz012xr/2eae091d-1b97-4276-90bc-6757c5dfedee 数据安全保护系统 数据安全保护系统以全面数据文件安全策略、加解密技术与强制访问控制有机结合为设计思想&#xff0c;对信息媒介上的各种数据资产&#xff0c;实施不同安全等级…

02_nodejs开发环境安装

02 【nodejs开发环境安装】 1.版本介绍 在命令窗口中输入 node -v 可以查看版本0.x 完全不技术 ES64.x 部分支持 ES6 特性5.x 部分支持ES6特性&#xff08;比4.x多些&#xff09;&#xff0c;属于过渡产品&#xff0c;现在来说应该没有什么理由去用这个了6.x 支持98%的 ES6 特…

jvm 新生代的区域划分

虚拟机将内存分为一块较大的 Eden 空间和两块较小的 Survivor 空间&#xff0c;每次分配内存只使用 Eden 和其中一块 Survivor。发生垃圾收集时&#xff0c;将 Eden 和 Survivor 中仍然存活的对象一次性复制到另外一块 Survivor 空间上&#xff0c;然后直接清理掉 Eden 和已用过…

耐蚀点蚀镀铜工艺

引言 随着5G技术的推出&#xff0c;导致电子电路和IC基板在设计中要求更高的密度。由于5G应用的性质&#xff0c;这些设计中的高可靠性和出色的电气性能也越来越重要。为了满足5G应用和其他下一代设备平台的需求&#xff0c;逐渐建立了使用改良半加成加工(mSAP)制造电路板的制…

安达发|生产计划排程APS系统整体结构引领企业智能化生产

随着科技的不断发展&#xff0c;企业对于生产计划排程的需求也在不断提高。在过去的生产计划管理中&#xff0c;企业往往需要根据销售订单或销售报表来分配生产加工任务&#xff0c;这种方式容易导致生产加工不均衡&#xff0c;影响企业的竞争力。为了解决这一问题&#xff0c;…

对ioc的简单理解

最近闲着无聊&#xff0c;又把ioc梳理了一遍&#xff0c;一边看一边满脑子是王宝强的“啥啥啥&#xff0c;这又是个啥”的表情包。 一会注入、一会依赖、一会又自动装配的……哎……还好有了点头绪。 ioc的概念 1、ioc是什么&#xff1f;有什么用&#xff1f; 老生常谈&…

设计模式备忘录+命令模式实现Word撤销恢复操作

文章目录 前言思路代码实现uml类图总结 前言 最近学习设计模式行为型的模式&#xff0c;学到了备忘录模式提到这个模式可以记录一个对象的状态属性值&#xff0c;用于下次复用&#xff0c;于是便想到了我们在Windows系统上使用的撤销操作&#xff0c;于是便想着使用这个模式进…

【Linux】fork函数的基础知识

文章目录 前言一、fork的返回值二、常见问题 1.为什么fork要给子进程返回0&#xff0c;给父进程返回子进程pid&#xff1f;2.一个函数返回两次值怎么理解&#xff1f; 3.一个变量怎么会有不同的内容&#xff1f; 4.fork函数干了什么&#xff1f; 前言 fork初识&#xff1a; …

【分布式搜索引擎elasticsearch】

文章目录 1.elasticsearch基础索引和映射索引库操作索引库操作总结 文档操作文档操作总结 RestAPIRestClient操作文档 1.elasticsearch基础 什么是elasticsearch&#xff1f; 一个开源的分布式搜索引擎&#xff0c;可以用来实现搜索、日志统计、分析、系统监控等功能 什么是…

电脑莫名其妙重启 为设备 ROOT\DISPLAY\0000 加载驱动程序 \Driver\WUDFRd 失败

卸载向日葵即可解决&#xff01;&#xff01;&#xff01;&#xff01;&#xff01;下面是报错日志&#xff0c;估计是远程连接导致的问题

10. selenium API (二)

目录 1. 多层框架/窗口定位 2. 下拉框处理 2.1 前端界面 2.2 代码 3. 针对 alert 弹窗进行操作 3.1 前端界面 3.2 代码 4. 文件提交 4.1 前端界面 4.2 代码 5. 显示等待 6. 操作浏览器滚动条 7. 截图 8. 浏览器关闭 9. 窗口切换 在上篇文章中&#xff0c;我们学…

【仿写spring之ioc篇】二、bean生命周期中的创建以及属性赋值

扫描类 这个类就不多说了&#xff0c;基本所有框架都要有这一步&#xff0c;这里主要关注我们目前要实现的方法&#xff0c;其他的具体方法可以查看源码 isComponent方法 /*** 扫描所有带有Component注解的java类&#xff0c;放入到BeanRegistry** return boolean*/public bo…

vcs仿真教程(查看断言)

VCS是在linux下面用来进行仿真看波形的工具&#xff0c;类似于windows下面的modelsim以及questasim等工具&#xff0c;以及quartus、vivado仿真的操作。 1.vcs的基本指令 vcs的常见指令后缀 sim常见指令 2.使用vcs的实例 &#xff08;1&#xff09;新建文件夹&#xff1a; …

C++STL——deque容器详解

纵有疾风起&#xff0c;人生不言弃。本文篇幅较长&#xff0c;如有错误请不吝赐教&#xff0c;感谢支持。 &#x1f4ac;文章目录 一.deque容器的基本概念二.deque容器常用操作①deque构造函数②deque元素操作③deque赋值操作④deque交换操作⑤deque大小操作⑥deque插入和删除…

vue3中axios的使用方法

在Vue 3中使用axios发送HTTP请求的方法与Vue 2中基本相同。首先&#xff0c;需要安装axios库&#xff1a; npm install axios然后&#xff0c;在Vue组件中引入axios&#xff1a; import axios from axios;接下来&#xff0c;可以在Vue组件的方法中使用axios发送HTTP请求。例如…

Linux 系统运维工具之 OpenLMI

一、前要 OpenLMI&#xff08;全称 Open Linux Management Infrastructure&#xff09;即开放式的 Linux 管理基础架构。OpenLMI 是一个开源项目&#xff0c;用于管理 Linux 系统管理的通用基础架构。它建立在现有工具基础上&#xff0c;充当抽象层&#xff0c;以便向系统管理…

英码深元“三位一体”AI场景化解决方案,助力多地化工园区快速实现智慧化转型!

我国是世界公认的化工大国&#xff0c;同时也是崛起中的化工强国。近年来多起重大爆炸事故暴露出我国化工园区安全问题突出&#xff0c;特别是在安全风险管控数字化转型、智能化升级方面存在明显短板和不足&#xff0c;尤其突出的痛点&#xff1a;化工园区的日常管理方式较为粗…