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

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 :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学习曲线')

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181

4.Exist Problems

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

2030901
鞠春宇