厦门网站制作系统,网站建设 静态类,青岛制作企业网站,包装设计效果图1.Back ground 如上图#xff0c;SLAM学习路线Start到End路径#xff0c;其中曲线SDAB为D档位学习路径#xff0c;曲线BC为R学习路径#xff0c;曲线AE为前进档D档学习路径。 为了使其使用记忆泊车时#xff0c;其驾驶员体验感好#xff0c;需去除R档倒车部分轨迹#x…1.Back ground 如上图SLAM学习路线Start到End路径其中曲线SDAB为D档位学习路径曲线BC为R学习路径曲线AE为前进档D档学习路径。 为了使其使用记忆泊车时其驾驶员体验感好需去除R档倒车部分轨迹并拟合一条可用的曲线
2.Algorithm Introduction D点作为起点DXD,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 byJuchunyu
%Date2023-09-01 17:00:42%设计轨迹xy
% y 2 (10x0)
% y -1.2/50 *x^2 - 4.4/10 *x (10x5)
% y 1.6 (20x5)
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 鞠春宇
