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。
最后優(yōu)化的曲線為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 :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;
%%擬合曲線系數(shù)
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軸方向投影距離很小時的處理。文章來源：http://www.zghlxwxcb.cn/news/detail-689933.html
2030901
鞠春宇文章來源地址http://www.zghlxwxcb.cn/news/detail-689933.html
到了這里，關于自動駕駛——【規(guī)劃】記憶泊車特殊學習路徑擬合的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
