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

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

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 :10
    slam_x   = [slam_x i];
    slam_y   = [slam_y 2];
    GearInfo = [GearInfo D];
    slam_theta = [slam_theta 0];
end
for i =10:-0.2:5
    slam_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];
end
    
for i = 5:0.2:20
    slam_x   = [slam_x i];
    slam_y   = [slam_y 1.6];
    GearInfo = [GearInfo D];
    slam_theta = [slam_theta 0];
end

figure(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) == D
                Index_end   = j;
                finish_Flag = 1;
                break;
            end
            j = j + 1;  
        end
        if(finish_Flag == 1)
            Index_startArr = [Index_startArr Index_start];
            Index_endArr   = [Index_endArr Index_end];
        end
        i = j;
    end
    i = i + 1;
end


PointCIndx = 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

%计算DA

distDA = ((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:1
        dist_  = ((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; 
        end
   end
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 = B

A(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:PointDIndx
    slam_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:PointCx
    slam_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)];
end

hold on 

figure(2)
plot(slam_Xfinal,slam_Yfinal,'r');
title('处理后的SLAM学习曲线')


4.Exist Problems

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

2030901
鞠春宇

相关推荐
云上艺旅15 小时前
K8S学习之基础七十四:部署在线书店bookinfo
学习·云原生·容器·kubernetes
你觉得20516 小时前
哈尔滨工业大学DeepSeek公开课:探索大模型原理、技术与应用从GPT到DeepSeek|附视频与讲义下载方法
大数据·人工智能·python·gpt·学习·机器学习·aigc
gaoshengdainzi17 小时前
镜片防雾性能测试仪在自动驾驶与无人机领域的创新应用
人工智能·自动驾驶·无人机·镜片防雾性能测试仪
A旧城以西17 小时前
数据结构(JAVA)单向,双向链表
java·开发语言·数据结构·学习·链表·intellij-idea·idea
无所谓จุ๊บ17 小时前
VTK知识学习(50)- 交互与Widget(一)
学习·vtk
FAREWELL0007517 小时前
C#核心学习(七)面向对象--封装(6)C#中的拓展方法与运算符重载: 让代码更“聪明”的魔法
学习·c#·面向对象·运算符重载·oop·拓展方法
吴梓穆17 小时前
UE5学习笔记 FPS游戏制作38 继承标准UI
笔记·学习·ue5
向上的车轮17 小时前
NOA是什么?国内自动驾驶技术的现状是怎么样的?
人工智能·机器学习·自动驾驶
Three~stone18 小时前
MySQL学习集--DDL
数据库·sql·学习
齐尹秦18 小时前
HTML 音频(Audio)学习笔记
学习