LatticePlanning_基于matlab的实现

思路参考,需要预先看这个文章内容后再看本篇:社群分享内容 | 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+a22t+a33t2+a44t3+a55t4
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+a22+a36t+a412t2+a520t3
横向位置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+b22s+b33s2+b44s3+b55s4
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+b22+b36s+b412s2+b520s3
若已知纵向初始状态 ( s 0 , s ′ 0 , s ′ ′ 0 ) (s0,s'0,s''0) (s0,s0,s0)末状态 ( s 1 , s ′ 1 , s ′ ′ 1 ) (s1,s'1,s''1) (s1,s1,s1),初始状态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=s0
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=s0
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+a22t+a33t2+a44t3+a55t4=s1
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+a22+a36t+a412t2+a520t3=s1
写成矩阵形式 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] 100100010t10002t22t2000t33t26t000t44t312t2000t55t420t3a0a1a2a3a4a5=s0s0s0s1s1s1
则线性方程组的解为 x = A − 1 b x=A^{-1}b x=A1b,
在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 l0=0,l0=0,l1=0,l1=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 l0l0赋值不为0时,就会出现非常大的偏离和调整后才能达到末状态,不太清楚为什么,可以通过什么办法把这个偏离给压制住?

l0_dot = -0.1;
l0_dot_dot = -0.2;

其结果为:。。。。采样的s越近,横向偏离的越小,难道是通过增加控制点来控制横向偏离???
在这里插入图片描述

采样纵向 s ′ 1 s'1 s1和调整时间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,s0,s0]
末状态 s 1 = [ n o t s e t , s ′ 1 , 0 ] s1=[notset,s'1,0] s1=[notset,s1,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+a22t+a33t2+a44t3
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+a22+a36t+a412t2
写成矩阵形式 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] 10000010100022t20003t26t0004t312t2a0a1a2a3a4=s0s0s0s1s1
通过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=0s0=egoinitV,s0=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=ssamples0=obstaclev,s0=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

得到的结果如下图所示:但是碰撞检测没有做,碰撞检测在后面进行讨论。
在这里插入图片描述

轨迹cost的计算方法

  • 13
    点赞
  • 42
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值