LatticePlanning_基于matlab的实现

最新推荐文章于 2024-05-04 16:30:15 发布

真诚的灰灰

最新推荐文章于 2024-05-04 16:30:15 发布

阅读量2k

点赞数 13

分类专栏：规划控制 apollo

本文链接：https://blog.csdn.net/jch924583667/article/details/119755799

版权

规划控制同时被 2 个专栏收录

18 篇文章 12 订阅

订阅专栏

apollo

6 篇文章 0 订阅

订阅专栏

思路参考，需要预先看这个文章内容后再看本篇：社群分享内容 | Lattice Planner规划算法

路径的表示方法

纵向位置s是关于时间t的多项式函数。
$s(t)=a_0 + a_1t +a_2t^2 + a_3t^3+ a_4t^4+ a_5t^5$
其各阶导数为：
$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^{(2)}(t)= a_s(t)=0 + 0 +a_2*2 + a_3*6t+ a_4*12t^2+ a_5*20t^3$
横向位置l是关于纵向位置s的多项式函数。
$l(s)=b_0 + b_1s +b_2s^2 + b_3s^3+ b_4s^4+ b_5s^5$
其各阶导数为：
$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^{(2)}(s)= a_l(s)=0 + 0 +b_2*2 + b_3*6s+ b_4*12s^2+ b_5*20s^3$
若已知纵向初始状态 $(s 0, s^{'} 0, s^{''} 0)$ 和末状态 $(s 1, s^{'} 1, s^{''} 1)$ ,初始状态t0=0,末状态t1=t则
$a_0 + 0+0 +0+0+0 =s0$
$0 + a_1 +0 + 0+ 0+ 0=s'0$
$0 + 0 +2a_2+ 0+ 0+ 0=s''0$
$a_0 + a_1t +a_2t^2 + a_3t^3+ a_4t^4+ a_5t^5 =s1$
$0 + a_1 +a_2*2t + a_3*3t^2+ a_4*4t^3+ a_5*5t^4 = s'1$
$0 + 0 +a_2*2 + a_3*6t+ a_4*12t^2+ a_5*20t^3 = s''1$
写成矩阵形式 $A x = b$ ：
$\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]$
则线性方程组的解为 $x=A^{-1}b$ ,
在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$ ，则通过下面的程序进行求解和绘制各采样点的到达路径。

%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$ 赋值不为0时，就会出现非常大的偏离和调整后才能达到末状态，不太清楚为什么，可以通过什么办法把这个偏离给压制住？

l0_dot = -0.1;
l0_dot_dot = -0.2;

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

采样纵向 $s^{'} 1$ 和调整时间t1（巡航车速控制）

设纵向速度限制 $v_u$ ,
采样末状态速度： $s'_1=[0,0.25v_u,0.5v_u,0.75v_u,v_u]$
采样调整时间：t1=[2,4,6,8]
初始状态 $s 0 = [0, s^{'} 0, s^{''} 0]$
末状态 $s 1 = [n o t s e t, s^{'} 1, 0]$
由于只有五个条件，则用四次多项式计算即可：
$s(t)=a_0 + a_1t +a_2t^2 + a_3t^3+ a_4t^4$
其各阶导数为：
$s^{(1)}(t)= v_s(t)=0 + a_1 +a_2*2t + a_3*3t^2+ a_4*4t^3$
$s^{(2)}(t)= a_s(t)=0 + 0 +a_2*2 + a_3*6t+ a_4*12t^2$
写成矩阵形式 $A x = b$ ：
$\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]$
通过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)$
在这里插入图片描述

跟车、超车规划

对障碍车辆进入的时间段进行采样，例如障碍物进入本车行驶路径的时间段为4-8s内，则采样时间为4s,5s,6s,7s,8s.
对每个采样时间点采样超车和跟车的纵向距离s，采样点分布为超过障碍车10m处，超过障碍车5m处，在障碍车后5m处，在障碍车后10m处进行采样。
起始点 $s0=0，s'0=ego_initV,s''0=ego_initAccel$
采样的终止点 $s1=s_sample，s'0=obstacle_v,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

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

轨迹cost的计算方法

真诚的灰灰

关注

13
点赞
踩
42

收藏

觉得还不错? 一键收藏
2
评论
LatticePlanning_基于matlab的实现

思路参考：社群分享内容 | Lattice Planner规划算法路径的表示方法纵向位置s是关于时间t的多项式函数。s(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5s(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)=vs(t)=0+a1+a2∗2t+a3∗3t2+a4∗4t3+a5∗5t4s^{(1)}(t)= v_s(t)=0 +
复制链接

扫一扫