思路参考，需要预先看这个文章内容后再看本篇：社群分享内容 | 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​+a1​t+a2​t2+a3​t3+a4​t4+a5​t5
其各阶导数为：
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​+b1​s+b2​s2+b3​s3+b4​s4+b5​s5
其各阶导数为：
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​+a1​t+a2​t2+a3​t3+a4​t4+a5​t5=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] ⎣⎢⎢⎢⎢⎢⎢⎡​100100​010t10​002t22t2​000t33t26t​000t44t312t2​000t55t420t3​⎦⎥⎥⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎡​a0​a1​a2​a3​a4​a5​​⎦⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎡​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​+a1​t+a2​t2+a3​t3+a4​t4
其各阶导数为：
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] ⎣⎢⎢⎢⎢⎡​10000​01010​0022t2​0003t26t​0004t312t2​⎦⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎡​a0​a1​a2​a3​a4​​⎦⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎡​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=egoi​nitV,s′′0=egoi​nitAccel
采样的终止点 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=ss​ample，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

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

轨迹cost的计算方法

Logo
魔乐社区

魔乐社区（Modelers.cn) 是一个中立、公益的人工智能社区，提供人工智能工具、模型、数据的托管、展示与应用协同服务，为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作，由全产业链共同建设、共同运营、共同享有，推动国产AI生态繁荣发展。

更多推荐

cover

【计算机视觉】Pixel逐像素分类&Mask掩码分类理解摘要

avatar 魔乐社区

计算机视觉（opencv）实战三十二——CascadeClassifier 人脸微笑检测（摄像头）

本文从原理到实现，详细介绍了基于 OpenCV Haar 分类器的人脸与微笑检测：讲解了 Haar 特征和级联检测原理。对代码逐行拆解并解释参数含义。画出完整流程图，帮助理解执行过程。给出了常见问题和优化建议，甚至扩展到深度学习方法。这种方法简单、轻量、实时性好，非常适合入门和小型应用项目。但如果需要更高准确率和更强鲁棒性，建议使用深度学习检测器替代 Haar 分类器。

avatar 魔乐社区
cover

小杰机器学习（seven）——贝叶斯分类

avatar 魔乐社区