我在csdn上看到做ADAS轨迹设计中的matlab代码中用了两段回旋线和一段圆弧来绘制一小段类似弯道的曲线,目前我在理解代码过程中,有一些参数不是很理解,遇到一些问题,我很想了解一下这个代码的思想,请教一下。
在代码中轨迹参数设计的时候,通过圆的半径、切向角和圆心角就能直接画出这三段曲线,请问这三个参数是怎么确定的呢,它们在图中的几何意义是什么呢?特别是其中的A是表达什么意思呢?在for循环中是怎么设置循环边界的呢?这个代码是不是先确定的圆弧的参数,然后再来绘制回旋线的呢?
%% 输入轨迹参数
R_circle = 9;%中间圆弧段曲率半径(m)
Beta_max_deg = 20.62;%最大切向角(deg)
k = 1/R_circle;
Circle_angle_deg = 90 - 2*Beta_max_deg;%圆弧段圆心角(deg)
%Velocity = 10;km/h
%% 第1段
A = 1/(2*R_circle*R_circle*Beta_max_deg*pi/180);%计算Clothoid曲线曲率相对弧长的变化率
Lmax = 2*R_circle*Beta_max_deg*pi/180;%由最大切向角决定的最大弧长
step_X1 = 0.001;%弧长步长(m)
Len1 = fix(Lmax/step_X1)+1;%靠向0取整
X1 = zeros(1,Len1);%Clothoid曲线1x坐标集合
Y1 = zeros(1,Len1);%Clothoid曲线1y坐标集合
for L1 = 0:1:(Len1-1)
x1 = 0;
y1 = 0;
for n=0:1:50
x1 = x1 + ((-1)^n*A^(2*n)*(L1*step_X1)^(4*n+1))/(factorial(2*n)*(4*n+1)*(2^(2*n)));%factorial阶乘
y1 = y1 + ((-1)^n*A^(2*n+1)*(L1*step_X1)^(4*n+3))/(factorial(2*n+1)*(4*n+3)*(2^(2*n+1)));
end
X1(L1+1) = x1;
Y1(L1+1) = y1;
end
%% 第2段
X2_center = X1(end) - R_circle*sin(Beta_max_deg*pi/180);%第二段圆心X坐标
Y2_center = Y1(end) + R_circle*cos(Beta_max_deg*pi/180);%第二段圆心Y坐标
step_X2 = 0.001;%第二段步长(deg)
arcLength = R_circle * Circle_angle_deg * pi/180;%圆弧长度
Len2 = fix(Circle_angle_deg/step_X2)+1;
X2 = zeros(1,Len2);%圆弧曲线x坐标集合
Y2 = zeros(1,Len2);%圆弧曲线y坐标集合
for L2 = 0:1:(Len2-1)
X2(L2+1) = X2_center + R_circle*sin((Beta_max_deg+step_X2*L2)*pi/180);
Y2(L2+1) = Y2_center - R_circle*cos((Beta_max_deg+step_X2*L2)*pi/180);
end
%% 第3段
step_X3 = 0.001;%弧长步长(m)
Len3 = Len1;
X3 = zeros(1,Len3);%Clothoid曲线3x坐标集合
Y3 = zeros(1,Len3);%Clothoid曲线3y坐标集合
for L3 = 0:1:(Len3-1)
x3 = 0;
y3 = 0;
for n=0:1:50
x3 = x3 + ((-1)^n*A^(2*n)*((Len3-L3-1)*step_X3)^(4*n+1))/(factorial(2*n)*(4*n+1)*(2^(2*n)));
y3 = y3 + ((-1)^n*A^(2*n+1)*((Len3-L3-1)*step_X3)^(4*n+3))/(factorial(2*n+1)*(4*n+3)*(2^(2*n+1)));
end
X3(L3+1) = X2(end) + Y1(end) -y3;
Y3(L3+1) = Y2(end) + X1(end) -x3;
end
plot(X1,Y1,'o',X2,Y2,'x',X3,Y3,'*')
%plot(X1,Y1)
grid on