原生导航路径存在的问题

在决策规划的过程中，路径计算是以参考线为自然坐标系，将欧式空间中的点转换到自然坐标系下再进行路径规划，但是在实际情况下，导航模块给出的全局导航路径可能存在以下问题：

路径过长；
路径不平滑。

导航路径能否直接作为参考线呢?

导航路径一般来说很长，且可能不光滑，若直接将导航路径作为参考线，第一个是不利于坐标转换，因为坐标转换计算过程中涉及到导数、斜率和曲率的计算，且坐标转换过程需要遍历参考线点找匹配点，参考线过长意味着离散点数量较多，这样影响计算效率，第二就是车辆或者障碍物在参考线上的投影可能不唯一。

解决方案

提取当前车辆附近局部导航路径作为参考线

由于在某个规划周期内我们并不需要对未来较远的时间内进行路径规划，所以我们只要考虑当前车辆附近的参考线点，先找到自车点在参考线上的匹配点，然后以该点为基准，向后取30个点，向前取150个点（向前指的是行车方向），共计181个连续参考线点组成的参考线作为当前规划周期所使用的参考线。（注意：若向前不足30个，则向前多取点位，若向后不足则向前多取点位，保证总数181个不变）。

对局部参考线做平滑

这里使用FEM平滑方法(参考百度阿波罗)：

平滑后的平滑效果度量

经过1的处理后，参考线离散点的规模固定为181个，我们利用二次规划对这部分的参考线进行平滑处理，得到平滑后的参考线点位。那用什么来衡量平滑呢，简单通俗的来说，线型越接近直线就越平滑，先简单考虑三个点，设平滑后的三个点分别为 $p_0$ 、 $p_1$ 、 $p_2$ ，以 $|\overrightarrow{p_1p_3}|^2$ 来表是平滑后线型的平滑程度， $|\overrightarrow{p_1p_3}|^2$ 越小，越平滑。

那么对于由N个点构成的参考线，其被平滑后的平滑度量可以写为下图中的矩阵表达形式（N个点，每相邻三个点计算一次，一共计算N-2次）：

平滑后的紧凑性度量

如果只考虑平滑性这一个指标，那么就都平滑成直线了，所以我们还对平滑后的点有其他要求，第一个是相邻两点之间的距离不能差的太多，即 $|\overrightarrow{p_1p_0}|^2$ + $|\overrightarrow{p_1p_2}|^2$ 要尽可能的小，那就要求 $|\overrightarrow{p_1p_0}|$ 和 $|\overrightarrow{p_1p_2}|$ 尽可能的接近。

同样的，那么对于由N个点构成的参考线，其被平滑后的紧凑性度量可以写为下图中的矩阵表达形式（N个点，每相邻两个点计算一次，一共计算N-1次）：

平滑前后的相似性度量

还需要考虑平滑后和平滑前的点的形状不能相差太多，即相似度我们也需要考虑，设平滑前的点的坐标为 $p_{0r}$ 、 $p_{1r}$ 、 $p_{2r}$ ,使用平滑前后点距离平方和来表征相似度同样的，那么对于由N个点构成的参考线，其被平滑前后的相似性度量可以写为下图中的矩阵表达形式（N个点，一共计算N次）：

最后，我们就可以建立平滑优化的目标函数 $cost$ ，由平滑性度量、紧凑性度量和相似性度量组成，我们希望求得一个 $\overrightarrow{x}$ 使得这个cost最小，这是一个典型的二次规划问题，决策变量为平滑后的点的坐标向量 $\overrightarrow{x} = [x_0,y_0,x_1,y_1,...,x_{N-1},y_{N-1}]$ ，我们对 $\overrightarrow{x}$ 的可行域做了一定的限制， $\overrightarrow{x_r}$ 为平滑前的坐标向量，为已知值， $\overrightarrow{x_r} = [x_{0r},y_{0r},x_{1r},y_{1r},...]$ .

使用拼接

使用拼接算法充分利用上周期的平滑结果，减少本周期平滑计算量。考虑前一个周期对181个参考点进行了平滑处理，由于规划周期为百毫秒级别，车辆在规划周期向前行进的距离是比较有限的，导致当前周期所关联的181个参考线点与上个周期的参考线点应该是有重合的，我们仅仅对新多出的点位进行平滑，再与上周期的平滑结果进行拼接即可，这样可以大大降低二次规划的问题规模，加快运行速度。

如下图，相较于上周期，本周期的参考线点多出了绿色点的部分，少掉了黑色点的部分，中间蓝色点是与上周期重合的点，平滑操作已经执行过了，并且存储在内存中，我们只需要对多出来的三个绿色点位进行平滑操作即可，但是为了参考线能够平稳过渡，我们还需要取出上周期末端的三个点位一起进行平滑操作，即绿色三个点 + $p_1$ 、 $p_2$ 、 $p_3$ 三个点，再将平滑后的结果和上周期的平滑结果进行拼接即可，这样我们的平滑优化中的矩阵规模能够大大降低，可以加快程序运行的速度。

具体实现

从导航路径点中提取待投影点的匹配索引以及投影信息

为了提取自车点附近的181个参考线点，首先需要确定自车点在当前导航路径下的匹配点，因此需要开发一个匹配点计算通用模块，可以处理多个待匹配点，并且输出投影信息。

从上周期的匹配点开始搜索

初步的想法是，输入一系列的待匹配点(车辆位置点或者障碍物点)，对于每一个待匹配点，从参考线点的第一个点开始遍历，直到找到待匹配点的匹配点，然后按照规划决策篇：1.2计算投影点信息的方法计算其投影点的信息并且保存在数组中。

这样看来好像没有问题，但是效率却不高，有必要在每一轮的规划周期中都从参考线的起始点开始遍历嘛？我如果能记录车辆位置点或者障碍物点在上一个规划周期中匹配的索引，本周期，我其实从上一周期匹配的索引开始遍历就好了，向前遍历还是向后遍历呢？因此我们还需要考虑车辆的行进方向，如果是沿着参考线索引顺序方向，则正向遍历，如果是沿着参考线索引逆序方向，则反向遍历。

再继续考虑，我只要记录上周期待匹配点对应的参考线匹配索引就行了嘛？如果本周期和上周期的参考线不一致呢？所以我们要记录的是上周期的参考线以及待配点对应的匹配索引，如果发现本周期和上周期的参考线不是同一参考线，我们就不能使用上一周期的记录信息了，而是重新开始从参考线的起点开始遍历。

由于我们是一次可以处理多个待投影点，因此待投影点的x、y坐标序列使用数组x_set和y_set来表示，x_set(i)与y_set(i)表示本轮次第i个待投影点的x与y坐标,pre_x_set(i)、pre_y_set(i)上一轮规划周期第i个待投影点的xy坐标，pre_match_index_set(i)表示上一轮第i个待匹配点在参考线上的匹配索引。思考一个问题：本周期的第i个待投影点和上周期的第i个待投影点一定是同一个物体嘛？一般来说，我们的待投影点由自车点和障碍物点组成，我们可以保证将自车点坐标信息放在数组的第一个位置，然后再存放障碍物信息，但是障碍物信息是由视觉模块检测的，它并不一定总能保证相邻周期的同一个障碍物在数组中的位置一致，尤其是上周期某障碍物被识别到了，这周期障碍物没有被识别到。如下图：

在第n个规划周期，车辆识别到了两个障碍物，一动一静，按照图示的索引顺序存储在数组里，进行投影计算后，自车点的匹配点索引为k，障碍1的匹配索引为k+3，障碍2的匹配索引为k，紧接着在下一个规划周期，动态障碍物1由于速度较快，脱离了自车的识别范围，此周期只检测到1个障碍物，自车点从上一周期的匹配索引点k开始遍历，可以顺利找到本周期的k+1索引，但是数组索引为1的障碍物不能沿用上周期的匹配索引k+3，从k+3开始遍历参考线点寻找匹配点，因为它俩虽然在数组中的索引一致但是不是代表同一个障碍物，所以我们在使用上周期的匹配结果时需要判断当前的待投影点和上一周期的带投影点是不是同一个点，通常可以通过计算距离，当距离小于某个阈值我们才认为是同一个障碍物点。

判断待投影点的实际行进方向

通过记录上一周期匹配结果，结合本周期待投影点的位矢，通过向量点积可以判断待投影点的行进方向，如下图：

综上所属，此模块我们要考虑的细节问题有：

通过沿用上一周期规划结果来加速匹配
沿用上周期的结果，需要我们考虑待投影点的实际行进方向；
沿用上周期的结果，需要我们考虑本规划周期和上一个规划周期的参考线是否是同一参考线；
沿用上周期的结果，需要我们考虑本规划周期和上一个规划周期的待投影点是否是同一个物体；

具体实现代码(matlab)：

输入：

全局参考线x坐标：path_x

全局参考线y坐标：path_y

全局参考线heading：path_heading

全局参考线斜率：path_kappa

待投影点的x坐标序列(最多处理128个点)：x_set

待投影点的y坐标序列(最多处理128个点)：y_set

输出：

待投影点的匹配点索引：match_index_set

待投影点的投影点x坐标：prj_x_set

待投影点的投影点y坐标：prj_y_set

待投影点的投影点斜率： prj_kappa_set

待投影点的投影点航向角：prj_heading_set

function [match_index_set, prj_x_set, prj_y_set, prj_kappa_set, prj_heading_set] = fcn(x_set, y_set,path_x, path_y, path_heading, path_kappa)

% 初始化匹配结果数组
n = 128;  % 最多一次处理128个待投影点
prj_x_set = ones(n, 1) * nan;
prj_y_set = ones(n, 1)* nan;
prj_kappa_set = ones(n, 1)* nan;
prj_heading_set = ones(n, 1)* nan;
match_index_set = ones(n, 1)* nan;
max_search_times = 40;

% 这周期和上周期的参考线可能不一样
persistent is_first_run;
persistent pre_match_index_set;
persistent pre_frenet_path_x;
persistent pre_frenet_path_y;
persistent pre_frenet_path_kappa;
persistent pre_frenet_path_heading;
persistent pre_x_set;
persistent pre_y_set;


% 判断是否是第一次运行
if isempty(is_first_run)
    % 是第一次运行
    pre_match_index_set = ones(n,1)*nan;
    pre_frenet_path_x = path_x; % 保存这个周期的参考线x坐标数据
    pre_frenet_path_y = path_y; % 保存这个周期的参考线y坐标数据
    pre_frenet_path_kappa = path_kappa; % 保存这个周期的参考线斜率数据
    pre_frenet_path_heading = path_heading; % 保存这个周期的参考线夹角数据
    is_first_run = 0;
    
    for i = 1:length(x_set)
        % 如果遇到空元素, 说明以及遍历完了所有需要计算的点
        if isnan(x_set(i))
            break;
        end
        % 开始计算第i个待投影点的匹配点和投影信息
        min_dis = 1e15;
        increase_count = 0;
        start_index = 1;
        for j = start_index:length(path_x)
            dis = ((x_set(i) - path_x(j))^2 + (y_set(i) - path_y(j))^2)^0.5;
            if dis < min_dis
                match_index_set(i) = j; % 本轮次的匹配点
                increase_count = 0;
                min_dis = dis;
            else
                increase_count = increase_count +1;
            end

             if increase_count > max_search_times
                 break;
             end
        end
         
         %%% 计算投影点信息 %%%
         match_point_index = match_index_set(i);
         host_vec = [x_set(i);y_set(i)]; % 车辆点的位矢
         match_point_vec = [path_x(match_point_index);path_y(match_point_index)]; % 匹配点的位矢
         match_point_kappa = path_kappa(match_point_index);% 匹配点的曲率
         match_point_heading = path_heading(match_point_index);% 匹配点的航向角
         match_tau = [cos(match_point_heading);sin(match_point_heading)];% 匹配点的方向向量
         % 投影点的位矢
         d_dot_tau = (host_vec - match_point_vec)'* match_tau;
         prj_vec = match_point_vec + d_dot_tau * match_tau;
         prj_x_set(i) = prj_vec(1);
         prj_y_set(i) = prj_vec(2);

         % 投影点的航向角和曲率
         prj_kappa_set(i) = match_point_kappa;
         prj_heading_set(i) = path_heading(match_point_index) + match_point_kappa * d_dot_tau;
         %%% 计算投影点信息 %%%
    end
     
     % 保存这个周期的信息
     pre_match_index_set = match_index_set;
     pre_x_set = x_set;
     pre_y_set = y_set;
    
else
    % 如果不是第一次运行
     % 判断参考线是不是和上周期的参考线是否一致
    
     if (path_x(1) == pre_frenet_path_x(1)) && (path_y(1) == pre_frenet_path_y(1))
         same_ref = 1; % 同一条参考线, 则可以继续沿用上周期的结果
     else
         same_ref = 0; % 不是同一条参考线, 不可以继续沿用上周期的结果
     end
     
    for i = 1:length(x_set)
        base_increase = 0; % 如果沿用了上周期的结果, 试探性搜索的次数可以减少base_increase次数
        
        % 如果遇到空元素, 说明以及遍历完了所有需要计算的点
        if isnan(x_set(i))
            break;
        end
        
        min_dis = 1e15;
        increase_count = 0;
        
        % 判断参考线是不是和上周期的参考线是否一致
        if same_ref == 0
            start_index = 1; % 不是同一条参考线, 不可以继续沿用上周期的结果
        else
            % 看看这个点的上一个周期有没有结果
            if isnan(pre_match_index_set(i))  % 没有结果的话, 直接从1开始搜索
                start_index = 1;
            else
                % 判断这周期的第i个物体和上周期的第i个物体是不是同一个物体
                square_dis = (x_set(i) - pre_x_set(i))^2 + (y_set(i) - pre_y_set(i))^2;
                if square_dis > 25
                    start_index = 1;
                else
                    start_index = pre_match_index_set(i);
                    base_increase = 20; % 如果继续沿用了上周期的结果, 可以减少搜索20次
                end
            end
        end

        %%% 判断行驶方向 %%%
        car_vec = [x_set(i); y_set(i)]; % 当前周期车辆的位置
        pre_match_point_vec = [pre_frenet_path_x(pre_match_index_set(i));pre_frenet_path_y(pre_match_index_set(i))]; % 上次的匹配点坐标
        pre_match_point_heading = pre_frenet_path_heading(pre_match_index_set(i));
        pre_match_point_tau = [cos(pre_match_point_heading);sin(pre_match_point_heading)];
        dot_flag = (car_vec - pre_match_point_vec)'*pre_match_point_tau;
        %%% 判断行驶方向 %%%

       if dot_flag > 0.001
           % 正方向行车
           %%% 搜索 %%%
           for j = start_index:length(path_x)
                dis = ((x_set(i) - path_x(j))^2 + (y_set(i) - path_y(j))^2)^0.5;
                if dis < min_dis
                    match_index_set(i) = j;
                    increase_count = 0;
                    min_dis = dis;
                else
                    increase_count = increase_count +1;
                end
                if increase_count > max_search_times - base_increase
                    break;
                end
           end
           %%% 搜索 %%%
           
       elseif dot_flag < -0.001
           % 反方向行车
           %%% 搜索 %%%
           for j = start_index:-1:1
                dis = ((x_set(i) - path_x(j))^2 + (y_set(i) - path_y(j))^2)^0.5;
                if dis < min_dis
                    match_index_set(i) = j;
                    increase_count = 0;
                    min_dis = dis;
                else
                    increase_count = increase_count +1;
                end
                if increase_count > max_search_times - base_increase
                    break;
                end
           end
           %%% 搜索 %%%
       else
           % 几乎没动
           match_index_set(i) = pre_match_index_set(i);
           
       end
       %%% 计算投影点信息 %%%
       match_point_index = match_index_set(i);
       host_vec = [x_set(i);y_set(i)]; % 车辆点的位矢
       match_point_vec = [path_x(match_point_index);path_y(match_point_index)]; % 匹配点的位矢
       match_point_kappa = path_kappa(match_point_index);% 匹配点的曲率
       match_point_heading = path_heading(match_point_index);% 匹配点的航向角
       match_tau = [cos(match_point_heading);sin(match_point_heading)];% 匹配点的方向向量
       % 投影点的位矢
       d_dot_tau = (host_vec - match_point_vec)'* match_tau;
       prj_vec = match_point_vec + d_dot_tau * match_tau;
       prj_x_set(i) = prj_vec(1);
       prj_y_set(i) = prj_vec(2);
       
       % 投影点的航向角和曲率
       prj_kappa_set(i) = match_point_kappa;
       prj_heading_set(i) = path_heading(match_point_index) + match_point_kappa * d_dot_tau;
       %%% 计算投影点信息 %%%
       
    end
    
    pre_frenet_path_x = path_x; % 保存这个周期的参考线数据
    pre_frenet_path_y = path_y; % 保存这个周期的参考线数据
    pre_frenet_path_kappa = path_kappa;
    pre_frenet_path_heading = path_heading;
    pre_match_index_set = match_index_set;
    pre_x_set = x_set;
    pre_y_set = y_set;
end

抽取局部参考线点

以自车点的匹配点为基准，在全局参考线（全局导航路径）上向前取150个点，向后取30个点，作为当前规划周期的参考线点（未平滑）

实现代码如下：

输入：

参考线x坐标序列：global_path_x

参考线y坐标序列：global_path_y

自车点的匹配点索引：host_match_point_index

输出：

局部参考线点x坐标：referenceline_x_init

局部参考线点y坐标：referenceline_y_init

function [referenceline_x_init,referenceline_y_init] = fcn(global_path_x,global_path_y,host_match_point_index)
% 在host_match_point_index往前取150个点，往后取30个点，一共181个点
% 如果后面不够的话，比如后面只有5个点，那就后面取5个，前面取175个
% 如果前面点不够的话，同理往后面补，保证一共181个点

%索引初始化
start_index = -1;
end_index = -1;
host_index = host_match_point_index;
if host_index - 30 < 1 %判断后面的点是否足够
    start_index = 1;
    end_index = 181;
elseif host_index + 150 > length(global_path_x) %判断前面的点是否足够
        end_index = length(global_path_x);
        start_index = end_index - 180;
else
        start_index = host_index -30;
        end_index = host_index + 150;
end

referenceline_x_init = global_path_x(start_index:start_index + 180);
referenceline_y_init = global_path_y(start_index:start_index + 180);

平滑

平滑问题是一个二次规划问题，具体的数学推导我们已经推导过了，下面是实现代码(这里写的比较简单，没有写拼接的逻辑，有时间再添加)

输入：

局部参考线点x坐标:referenceline_x_init

局部参考线点y坐标:referenceline_y_init

x方向下界偏移、x方向上界偏移:x_lb,x_ub

y方向下界偏移、y方向上界偏移：y_lb,y_ub

相似项权重、紧凑项权重、光滑项权重：w_ref,w_len,w_smooth

输出：

光滑后参考线点x坐标：referenceline_x

光滑后参考线点y坐标：referenceline_y

function [reference_x, reference_y] = smooth(reference_x_init, reference_y_init,x_lb,x_ub,y_lb,y_ub,w_ref,w_len,w_smooth)
% reference_x_init, reference_y_init,x_lb,x_ub,y_lb,y_ub都是列向量

persistent pre_x_init;
persistent pre_y_init;
persistent is_first_run;
persistent pre_reference_x_smooth;
persistent pre_reference_y_smooth;

% 求解点坐标平滑的二次规划问题
% reference_x_init, reference_y_init是列向量
coder.extrinsic("quadprog");
% 获取 坐标点的数目
loc_num = 181;
    
if isempty(is_first_run)
    is_first_run = 0; % 第一次执行
    
    % 初始化矩阵
    reference_x = zeros(loc_num, 1);
    reference_y = zeros(loc_num, 1);
    a_smooth_mat = zeros(2 * loc_num, 2 * loc_num - 4);
    a_len_mat = zeros(2 * loc_num, 2 * loc_num - 2);
    a_ref_mat = diag(ones(2 * loc_num,1));
    f = zeros(2 * loc_num, 1);
    lb = zeros(2 * loc_num, 1);
    ub = zeros(2 * loc_num, 1);

    % 1.生成H矩阵的平滑部分
    % 先是生成A_smooth
    for j = 1: 2*loc_num - 4

        a_smooth_mat(j, j) = 1;
        a_smooth_mat(j + 2, j) = -2;
        a_smooth_mat(j + 4, j) = 1;

    end

    % 生成A_len
    for j = 1: 2*loc_num - 2
        a_len_mat(j, j) = -1;
        a_len_mat(j + 2, j) = 1;
    end

    h_mat = w_smooth * (a_smooth_mat * a_smooth_mat.') + w_len * (a_len_mat * a_len_mat.') + w_ref * (a_ref_mat * a_ref_mat.');
    disp('start caculating smooth !');
    
    % 生成f_t
    f(1:2:2*loc_num - 1) = reference_x_init;
    f(2:2:2*loc_num) = reference_y_init;
    f_t = -2 * w_ref * f;

    % 生成lb, ub
    lb(1:2:2*loc_num - 1) = reference_x_init + x_lb;
    lb(2:2:2*loc_num) = reference_y_init + y_lb;
    ub(1:2:2*loc_num - 1) = reference_x_init + x_ub;
    ub(2:2:2*loc_num) = reference_y_init + y_ub;
    
    h_mat = 2 * h_mat;
    X = quadprog(h_mat,f_t,[], [],[], [],lb, ub, f);
    % disp('lb:');
    % disp(lb);
    % disp('ub:');
    % disp(ub);
    % disp('H:');
    % disp(h_mat);
    % disp('f:');
    % disp(f_t);
    % disp('x0:');
    % disp(f);
    disp('reference_x_size');
    disp(size(reference_x));
    disp('X_size');
    disp(size(X));
    
    for i=1:loc_num
        reference_x(i)=X(2*i-1);
        reference_y(i)=X(2*i);
    end
 
    disp('reference_x');
    disp(reference_x);
    % 保存此次的结果
    pre_x_init = reference_x_init;
    pre_y_init= reference_y_init;
    pre_reference_x_smooth = reference_x;
    pre_reference_y_smooth = reference_y;
    disp('end caculating smooth !');
else
    % 不是第一次执行
    
    % 如果待优化的参考线点和上个周期完全相同, 则直接使用上次的结果
    if reference_x_init(1) == pre_x_init(1) && reference_y_init(1) == pre_y_init(1)
        reference_x = pre_reference_x_smooth;
        reference_y = pre_reference_y_smooth;
    else
        % 再进行二次规划, 这里可以使用轨迹拼接, 先暂时不这么写
        % 初始化矩阵
        reference_x = zeros(loc_num, 1);
        reference_y = zeros(loc_num, 1);
        a_smooth_mat = zeros(2 * loc_num, 2 * loc_num - 4);
        a_len_mat = zeros(2 * loc_num, 2 * loc_num - 2);
        a_ref_mat = diag(ones(1, 2 * loc_num));
        f = zeros(2 * loc_num, 1);
        lb = zeros(2 * loc_num, 1);
        ub = zeros(2 * loc_num, 1);

        % 1.生成H矩阵的平滑部分
        % 先是生成A_smooth
        for j = 1: 2*loc_num - 4

            a_smooth_mat(j, j) = 1;
            a_smooth_mat(j + 2, j) = -2;
            a_smooth_mat(j + 4, j) = 1;

        end

        % 生成A_len
        for j = 1: 2*loc_num - 2
            a_len_mat(j, j) = -1;
            a_len_mat(j + 2, j) = 1;
        end

        h_mat = w_smooth * (a_smooth_mat * a_smooth_mat.') + w_len * (a_len_mat * a_len_mat.') + w_ref * (a_ref_mat * a_ref_mat.');
        %disp(h_mat);
        % 生成f_t
        f(1:2:2*loc_num - 1) = reference_x_init;
        f(2:2:2*loc_num) = reference_y_init;
        f_t = -2 * w_ref * f;

        % 生成lb, ub
        lb(1:2:2*loc_num - 1) = reference_x_init + x_lb;
        lb(2:2:2*loc_num) = reference_y_init + y_lb;

        ub(1:2:2*loc_num - 1) = reference_x_init + x_ub;
        ub(2:2:2*loc_num) = reference_y_init + y_ub;
        h_mat = 2 * h_mat;
        X = quadprog(h_mat,f_t,[], [],[], [],lb, ub, f);
        % disp('lb:');
        % disp(lb);
        % disp('ub:');
        % disp(ub);
        % disp('H:');
        % disp(h_mat);
        % disp('f:');
        % disp(f_t);
        % disp('x0:');
        % disp(f);
        for i=1:loc_num
            reference_x(i)=X(2*i-1);
            reference_y(i)=X(2*i);
        end
        
        % 保存此次的结果
        pre_x_init = reference_x_init;
        pre_y_init= reference_y_init;
        pre_reference_x_smooth = reference_x;
        pre_reference_y_smooth = reference_y;
    end
end

经过自车匹配点搜索、局部参考线提取、局部参考线平滑，我们就可以得到用于坐标转换计算的参考线，自车点和障碍物点到参考线的投影以及坐标转换都要基于该参考线。

完整simulink工程和代码见：

规划决策篇：2.1参考线模块——由导航路径生成参考线

原生导航路径存在的问题

解决方案

具体实现

从导航路径点中提取待投影点的匹配索引以及投影信息

从上周期的匹配点开始搜索

判断待投影点的实际行进方向

抽取局部参考线点

平滑