思路参考,需要预先看这个文章内容后再看本篇:社群分享内容 | Lattice Planner规划算法
路径的表示方法
纵向位置s是关于时间t的多项式函数。
s
(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
+
a
5
t
5
s(t)=a_0 + a_1t +a_2t^2 + a_3t^3+ a_4t^4+ a_5t^5
s(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5
其各阶导数为:
s
(
1
)
(
t
)
=
v
s
(
t
)
=
0
+
a
1
+
a
2
∗
2
t
+
a
3
∗
3
t
2
+
a
4
∗
4
t
3
+
a
5
∗
5
t
4
s^{(1)}(t)= v_s(t)=0 + a_1 +a_2*2t + a_3*3t^2+ a_4*4t^3+ a_5*5t^4
s(1)(t)=vs(t)=0+a1+a2∗2t+a3∗3t2+a4∗4t3+a5∗5t4
s
(
2
)
(
t
)
=
a
s
(
t
)
=
0
+
0
+
a
2
∗
2
+
a
3
∗
6
t
+
a
4
∗
12
t
2
+
a
5
∗
20
t
3
s^{(2)}(t)= a_s(t)=0 + 0 +a_2*2 + a_3*6t+ a_4*12t^2+ a_5*20t^3
s(2)(t)=as(t)=0+0+a2∗2+a3∗6t+a4∗12t2+a5∗20t3
横向位置l是关于纵向位置s的多项式函数。
l
(
s
)
=
b
0
+
b
1
s
+
b
2
s
2
+
b
3
s
3
+
b
4
s
4
+
b
5
s
5
l(s)=b_0 + b_1s +b_2s^2 + b_3s^3+ b_4s^4+ b_5s^5
l(s)=b0+b1s+b2s2+b3s3+b4s4+b5s5
其各阶导数为:
l
(
1
)
(
s
)
=
v
l
(
s
)
=
0
+
b
1
+
b
2
∗
2
s
+
b
3
∗
3
s
2
+
b
4
∗
4
s
3
+
b
5
∗
5
s
4
l^{(1)}(s)= v_l(s)=0 + b_1 +b_2*2s + b_3*3s^2+ b_4*4s^3+ b_5*5s^4
l(1)(s)=vl(s)=0+b1+b2∗2s+b3∗3s2+b4∗4s3+b5∗5s4
l
(
2
)
(
s
)
=
a
l
(
s
)
=
0
+
0
+
b
2
∗
2
+
b
3
∗
6
s
+
b
4
∗
12
s
2
+
b
5
∗
20
s
3
l^{(2)}(s)= a_l(s)=0 + 0 +b_2*2 + b_3*6s+ b_4*12s^2+ b_5*20s^3
l(2)(s)=al(s)=0+0+b2∗2+b3∗6s+b4∗12s2+b5∗20s3
若已知纵向初始状态
(
s
0
,
s
′
0
,
s
′
′
0
)
(s0,s'0,s''0)
(s0,s′0,s′′0)
和末状态
(
s
1
,
s
′
1
,
s
′
′
1
)
(s1,s'1,s''1)
(s1,s′1,s′′1)
,初始状态t0=0,末状态t1=t则
a
0
+
0
+
0
+
0
+
0
+
0
=
s
0
a_0 + 0+0 +0+0+0 =s0
a0+0+0+0+0+0=s0
0
+
a
1
+
0
+
0
+
0
+
0
=
s
′
0
0 + a_1 +0 + 0+ 0+ 0=s'0
0+a1+0+0+0+0=s′0
0
+
0
+
2
a
2
+
0
+
0
+
0
=
s
′
′
0
0 + 0 +2a_2+ 0+ 0+ 0=s''0
0+0+2a2+0+0+0=s′′0
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
+
a
5
t
5
=
s
1
a_0 + a_1t +a_2t^2 + a_3t^3+ a_4t^4+ a_5t^5 =s1
a0+a1t+a2t2+a3t3+a4t4+a5t5=s1
0
+
a
1
+
a
2
∗
2
t
+
a
3
∗
3
t
2
+
a
4
∗
4
t
3
+
a
5
∗
5
t
4
=
s
′
1
0 + a_1 +a_2*2t + a_3*3t^2+ a_4*4t^3+ a_5*5t^4 = s'1
0+a1+a2∗2t+a3∗3t2+a4∗4t3+a5∗5t4=s′1
0
+
0
+
a
2
∗
2
+
a
3
∗
6
t
+
a
4
∗
12
t
2
+
a
5
∗
20
t
3
=
s
′
′
1
0 + 0 +a_2*2 + a_3*6t+ a_4*12t^2+ a_5*20t^3 = s''1
0+0+a2∗2+a3∗6t+a4∗12t2+a5∗20t3=s′′1
写成矩阵形式
A
x
=
b
Ax=b
Ax=b
:
[
1
0
0
0
0
0
0
1
0
0
0
0
0
0
2
0
0
0
1
t
t
2
t
3
t
4
t
5
0
1
2
t
3
t
2
4
t
3
5
t
4
0
0
2
6
t
12
t
2
20
t
3
]
[
a
0
a
1
a
2
a
3
a
4
a
5
]
=
[
s
0
s
′
0
s
′
′
0
s
1
s
′
1
s
′
′
1
]
\left[ \begin{matrix} 1 &0 &0 &0 &0 &0 \\ 0 &1 &0 &0 &0 &0 \\ 0 &0 &2 &0 &0 &0 \\ 1 &t &t^2 &t^3 &t^4 &t^5 \\ 0 &1 &2t &3t^2 &4t^3 &5t^4 \\ 0 &0 &2 &6t &12t^2 &20t^3 \\ \end{matrix} \right] \left[ \begin{matrix} a_0\\ a_1\\ a_2\\ a_3\\ a_4\\ a_5\\ \end{matrix} \right]= \left[ \begin{matrix} s0\\ s'0\\ s''0\\ s1\\ s'1\\ s''1\\ \end{matrix} \right]
⎣⎢⎢⎢⎢⎢⎢⎡100100010t10002t22t2000t33t26t000t44t312t2000t55t420t3⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡a0a1a2a3a4a5⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡s0s′0s′′0s1s′1s′′1⎦⎥⎥⎥⎥⎥⎥⎤
则线性方程组的解为
x
=
A
−
1
b
x=A^{-1}b
x=A−1b,
在matlab中进行求解的方法:
x=inv(A)*b
//sloveLinearEquation.m
clc,clear;
s0=[0,10,2];
s1=[20,4,0];
t=5;
A=[1 ,0 ,0 ,0 ,0 ,0;
0 ,1 ,0 ,0 ,0 ,0;
0 ,0 ,2 ,0 ,0 ,0;
1 ,t ,t^2 ,t^3 ,t^4 ,t^5;
0 ,1 ,2*t ,3*t^2 ,4*t^3 ,5*t^4;
0 ,0 ,2 ,6*t ,12*t^2 ,20*t^3];
b=[s0(1),s0(2),s0(3),s1(1),s1(2),s1(3)]';
x = inv(A)*b;
a0=x(1);
a1=x(2);
a2=x(3);
a3=x(4);
a4=x(5);
a5=x(6);
plot_t = 0:0.1:t;
plot_y = a0+a1*plot_t+a2*plot_t.^2+a3*plot_t.^3+a4*plot_t.^4+a5*plot_t.^5;
plot_y_dot = a1+a2*2*plot_t+a3*3*plot_t.^2+a4*4*plot_t.^3+a5*5*plot_t.^4;
plot_y_dot_dot = a2*2+a3*6*plot_t+a4*12*plot_t.^2+a5*20*plot_t.^3;
subplot(1,3,1)
plot(plot_t,plot_y);
legend('s(t)');
subplot(1,3,2)
plot(plot_t,plot_y_dot);
legend("s'(t)");
subplot(1,3,3)
plot(plot_t,plot_y_dot_dot);
legend("s''(t)");
计算结果:
采样纵向距离和横向距离得到多条路径
采样纵向距离d0 = 20、40、60、80,横向距离d1 = -0.5,0.0.5。
若起始点的
l
′
0
=
0
,
l
′
′
0
=
0
,
l
′
1
=
0
,
l
′
′
1
=
0
l'0=0,l''0=0,l'1=0,l''1=0
l′0=0,l′′0=0,l′1=0,l′′1=0,则通过下面的程序进行求解和绘制各采样点的到达路径。
%sampleS_L.m
clc,clear;
s1_sample = [20,40,60,80];
l0_dot = 0;
l0_dot_dot = 0;
l0=[0,l0_dot,l0_dot_dot];
l1_sample = [-0.5,0,0.5];
for i=1:length(s1_sample)
for j=1:length(l1_sample)
s1=s1_sample(i);
l1=[l1_sample(j),0,0];
[plot_t,plot_y] = slovefunction(s1,l0,l1);
plot(plot_t,plot_y);
hold on;
grid on;
end
end
function [plot_t,plot_y] = slovefunction(dt,s0,s1)
t=dt;
A=[1 ,0 ,0 ,0 ,0 ,0;
0 ,1 ,0 ,0 ,0 ,0;
0 ,0 ,2 ,0 ,0 ,0;
1 ,t ,t^2 ,t^3 ,t^4 ,t^5;
0 ,1 ,2*t ,3*t^2 ,4*t^3 ,5*t^4;
0 ,0 ,2 ,6*t ,12*t^2 ,20*t^3];
b=[s0(1),s0(2),s0(3),s1(1),s1(2),s1(3)]';
x = inv(A)*b;
a0=x(1);
a1=x(2);
a2=x(3);
a3=x(4);
a4=x(5);
a5=x(6);
plot_t = 0:0.1:t;
plot_y = a0+a1*plot_t+a2*plot_t.^2+a3*plot_t.^3+a4*plot_t.^4+a5*plot_t.^5;
end
结果如下:横轴是采样的纵向距离s,纵轴是采样的横向距离l。
但是当把初始位置的
l
′
0
和
l
′
′
0
l'0和l''0
l′0和l′′0赋值不为0时,就会出现非常大的偏离和调整后才能达到末状态,不太清楚为什么,可以通过什么办法把这个偏离给压制住?
l0_dot = -0.1;
l0_dot_dot = -0.2;
其结果为:。。。。采样的s越近,横向偏离的越小,难道是通过增加控制点来控制横向偏离???
采样纵向 s ′ 1 s'1 s′1和调整时间t1(巡航车速控制)
设纵向速度限制
v
u
v_u
vu,
采样末状态速度:
s
1
′
=
[
0
,
0.25
v
u
,
0.5
v
u
,
0.75
v
u
,
v
u
]
s'_1=[0,0.25v_u,0.5v_u,0.75v_u,v_u]
s1′=[0,0.25vu,0.5vu,0.75vu,vu]
采样调整时间:t1=[2,4,6,8]
初始状态
s
0
=
[
0
,
s
′
0
,
s
′
′
0
]
s0=[0,s'0,s''0]
s0=[0,s′0,s′′0]
末状态
s
1
=
[
n
o
t
s
e
t
,
s
′
1
,
0
]
s1=[notset,s'1,0]
s1=[notset,s′1,0]
由于只有五个条件,则用四次多项式计算即可:
s
(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
s(t)=a_0 + a_1t +a_2t^2 + a_3t^3+ a_4t^4
s(t)=a0+a1t+a2t2+a3t3+a4t4
其各阶导数为:
s
(
1
)
(
t
)
=
v
s
(
t
)
=
0
+
a
1
+
a
2
∗
2
t
+
a
3
∗
3
t
2
+
a
4
∗
4
t
3
s^{(1)}(t)= v_s(t)=0 + a_1 +a_2*2t + a_3*3t^2+ a_4*4t^3
s(1)(t)=vs(t)=0+a1+a2∗2t+a3∗3t2+a4∗4t3
s
(
2
)
(
t
)
=
a
s
(
t
)
=
0
+
0
+
a
2
∗
2
+
a
3
∗
6
t
+
a
4
∗
12
t
2
s^{(2)}(t)= a_s(t)=0 + 0 +a_2*2 + a_3*6t+ a_4*12t^2
s(2)(t)=as(t)=0+0+a2∗2+a3∗6t+a4∗12t2
写成矩阵形式
A
x
=
b
Ax=b
Ax=b
:
[
1
0
0
0
0
0
1
0
0
0
0
0
2
0
0
0
1
2
t
3
t
2
4
t
3
0
0
2
6
t
12
t
2
]
[
a
0
a
1
a
2
a
3
a
4
]
=
[
s
0
s
′
0
s
′
′
0
s
′
1
s
′
′
1
]
\left[ \begin{matrix} 1 &0 &0 &0 &0 \\ 0 &1 &0 &0 &0 \\ 0 &0 &2 &0 &0 \\ 0 &1 &2t &3t^2 &4t^3 \\ 0 &0 &2 &6t &12t^2 \\ \end{matrix} \right] \left[ \begin{matrix} a_0\\ a_1\\ a_2\\ a_3\\ a_4\\ \end{matrix} \right]= \left[ \begin{matrix} s0\\ s'0\\ s''0\\ s'1\\ s''1\\ \end{matrix} \right]
⎣⎢⎢⎢⎢⎡10000010100022t20003t26t0004t312t2⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡a0a1a2a3a4⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡s0s′0s′′0s′1s′′1⎦⎥⎥⎥⎥⎤
通过matlab进行采样计算:
%curise_sampleV_t.m
clc,clear;
v_u = 20; % m/s
v_u_sample = [0,0.25*v_u,0.5*v_u,0.75*v_u,v_u];
s0_dot = 10;
s0_dot_dot = 2;
s0=[0,s0_dot,s0_dot_dot];
t_sample = [2,4,6,8]; % uint:s
for i=1:length(v_u_sample)
for j=1:length(t_sample)
t_s=t_sample(j);
s1=[0,v_u_sample(i),0];
[plot_t,plot_y,plot_y_dot,plot_y_dot_dot] = slovefunction(t_s,s0,s1);
subplot(1,3,1)
plot(plot_t,plot_y);
hold on;
grid on;
subplot(1,3,2)
plot(plot_t,plot_y_dot);
hold on;
grid on;
subplot(1,3,3)
plot(plot_t,plot_y_dot_dot);
hold on;
grid on;
end
end
function [plot_t,plot_y,plot_y_dot,plot_y_dot_dot] = slovefunction(dt,s0,s1)
t=dt;
A=[1 ,0 ,0 ,0 ,0;
0 ,1 ,0 ,0 ,0;
0 ,0 ,2 ,0 ,0;
0 ,1 ,2*t ,3*t^2 ,4*t^3;
0 ,0 ,2 ,6*t ,12*t^2];
b=[s0(1),s0(2),s0(3),s1(2),s1(3)]';
x = inv(A)*b;
a0=x(1);
a1=x(2);
a2=x(3);
a3=x(4);
a4=x(5);
plot_t = 0:0.1:t;
plot_y = a0+a1*plot_t+a2*plot_t.^2+a3*plot_t.^3+a4*plot_t.^4;
plot_y_dot = a1+a2*2*plot_t+a3*3*plot_t.^2+a4*4*plot_t.^3;
plot_y_dot_dot = a2*2+a3*6*plot_t+a4*12*plot_t.^2;
end
结果如下图:从左到右分别为
s
(
t
)
,
s
′
(
t
)
,
s
′
′
(
t
)
s(t),s'(t),s''(t)
s(t),s′(t),s′′(t)
跟车、超车规划
对障碍车辆进入的时间段进行采样,例如障碍物进入本车行驶路径的时间段为4-8s内,则采样时间为4s,5s,6s,7s,8s.
对每个采样时间点采样超车和跟车的纵向距离s,采样点分布为超过障碍车10m处,超过障碍车5m处,在障碍车后5m处,在障碍车后10m处进行采样。
起始点
s
0
=
0
,
s
′
0
=
e
g
o
i
n
i
t
V
,
s
′
′
0
=
e
g
o
i
n
i
t
A
c
c
e
l
s0=0,s'0=ego_initV,s''0=ego_initAccel
s0=0,s′0=egoinitV,s′′0=egoinitAccel
采样的终止点
s
1
=
s
s
a
m
p
l
e
,
s
′
0
=
o
b
s
t
a
c
l
e
v
,
s
′
′
0
=
0
s1=s_sample,s'0=obstacle_v,s''0=0
s1=ssample,s′0=obstaclev,s′′0=0
采用五次多项式进行路径计算:
通过matlab程序进行计算:
%overtake_follow_sample.m
clc,clear;
ego_initV = 10; %ego vehicle speed,units:m/s
ego_initAccel = 0; % ego vehicle acceleration,units:m/s2
% step1: Create obstacles and draw obstacles
obstacle_in_t = 4; % unit:s
obstacle_out_t = 8; % unit:s
obstacle_v = 5; %unit:m/s
obstacle_intercept = 20; %unit:m
obstacle_length = 5; %unit:m
half_obstacle_length = obstacle_length/2;
x = obstacle_in_t:0.1:obstacle_out_t;
y = obstacle_v .* x + obstacle_intercept;
fill_x = [obstacle_in_t,obstacle_in_t,obstacle_out_t,obstacle_out_t];
fill_y_temp = obstacle_v .* fill_x + obstacle_intercept;
fill_y_append = [-half_obstacle_length,half_obstacle_length,half_obstacle_length,-half_obstacle_length];
fill_y = fill_y_temp + fill_y_append;
fill(fill_x,fill_y,'b');
hold on;
plot(x,y,'r');
hold on;
axis([0,obstacle_out_t + 1,0,fill_y(3)+20])
% step2: sample t and the distance in front of and behind the obstacle
for sample_t = obstacle_in_t:1:obstacle_out_t
obstacle_y = obstacle_v * sample_t + obstacle_intercept;
sample_s = [obstacle_y+half_obstacle_length+10,obstacle_y+half_obstacle_length+5,obstacle_y-half_obstacle_length-5,obstacle_y-half_obstacle_length-10];
for i=1:length(sample_s)
s0=[0,ego_initV,ego_initAccel];
s1=[sample_s(i),obstacle_v,0];
[plot_t,plot_y] = slovefunction(sample_t,s0,s1);
plot(plot_t,plot_y);
hold on;
grid on;
end
end
function [plot_t,plot_y] = slovefunction(dt,s0,s1)
t=dt;
A=[1 ,0 ,0 ,0 ,0 ,0;
0 ,1 ,0 ,0 ,0 ,0;
0 ,0 ,2 ,0 ,0 ,0;
1 ,t ,t^2 ,t^3 ,t^4 ,t^5;
0 ,1 ,2*t ,3*t^2 ,4*t^3 ,5*t^4;
0 ,0 ,2 ,6*t ,12*t^2 ,20*t^3];
b=[s0(1),s0(2),s0(3),s1(1),s1(2),s1(3)]';
x = inv(A)*b;
a0=x(1);
a1=x(2);
a2=x(3);
a3=x(4);
a4=x(5);
a5=x(6);
plot_t = 0:0.1:t;
plot_y = a0+a1*plot_t+a2*plot_t.^2+a3*plot_t.^3+a4*plot_t.^4+a5*plot_t.^5;
end
得到的结果如下图所示:但是碰撞检测没有做,碰撞检测在后面进行讨论。