前言

通过提取参考线后，下一步我们可以开始做路径规划了。在规划之前要想清楚一个问题，每个规划周期的起点是哪个点？是当前时刻车辆的实际位置？还是去上一周期规划的点位中寻找当前时刻的点位作为规划起点？

如何选定规划起点

设想我们将当前 $T$ 时刻的车辆实际位置作为规划起点，开始进行规划，假设上一周期规划的轨迹线是下图红色线，本周期规划出了绿色的路线，但是在规划算法执行的 $dt$ 时间内，车辆还是会沿着上周期规划的路线继续行驶，等到算法执行完，本周期规划的路线中 $[T,T+dt]$ 时间区间内的轨迹都丢掉了，这样看来规划的轨迹是不连续的。所以我们规划的起点应该是从 $T+dt$ 的时刻车辆所在的位置开始规划。

而且还需要依据控制的误差来判断需不需要拼接轨迹。为什么需要拼接轨迹呢？因为在实际的工程中， $dt$ 取的是一个定值，一般是100ms，如果算法30ms就执行完了，但是我们的轨迹却是从 $T+100ms$ 开始的，那么时间区间 $[T+30ms ,T+100ms]$ 的轨迹从哪去获得呢？对，需要从上周期去获得，所以我们需要将本周期的轨迹与上周期的轨迹进行拼接，我们依据 $T+dt$ 时间戳去上周期轨迹中搜索时间在 $T+dt$ 之前的最近的 $N$ 个点。

首次规划

第一次规划，没有上一周期轨迹，从 $T+dt(T=0)$ 时刻开始规划:

x_{T+dt}=0\\ y_{T+dt}=0\\ v_{x,T+dt}=0\\ v_{y,T+dt}=0\\ a_{T+dt}=0\\ a_{T+dt}=0\\ \theta_{T+dt}=0\\ k_{T+dt}=0\\ t = dt\\

非首次规划

若不是第一次执行规划，那么我们可以拿到上一周期规划的点位信息，首先我们需要去上一周期的规划点位信息中搜索本时刻 $T$ 车辆本应该在的位置，将其与车辆此时刻的实际位置进行比较，若误差较大，说明控制没有跟上，需要做replan，若误差较小，说明控制跟上了，不需要replan。

控制跟上了规划的点位

控制跟上了规划的轨迹，我们认为在下一个 $dt$ 时间内，控制也能够跟着规划的轨迹走，我们只需要在上一个周期的规划结果中搜索时间戳为 $T+dt$ 的车辆点位信息作为车辆的规划起点即可，同时我们还需要将 $T+dt$ 前的 $N$ 个点位保留到内存，与本周期的规划轨迹进行拼接， $N$ 一般取20。

控制没有跟上规划的点位

如果控制没有跟上规划的轨迹，那么我们将使用车辆运动学公式推算 $dt$ 时间后车辆所在的位置，将其作为本规划周期的规划起点，不进行本周期规划轨迹和上周期规划轨迹的拼接，相当于做了一次replan。设当前时刻 $T$ 的车辆的位置为 $x,y$ ，速度为 $v_x,v_y$ ，速度为 $a_x,a_y$ ， $\theta$ 表示车辆的航向角， $k$ 表示斜率。

x_{T+dt}=x+v_x * dt + 0.5 * a_x * {dt}^2\\ y_{T+dt}=y+v_y * dt + 0.5 * a_y * {dt}^2\\ v_{x,T+dt}=v_x + a_x * dt\\ v_{y,T+dt}=v_y + a_y * dt\\ a_{T+dt}=a_x\\ a_{T+dt}=a_y\\ \theta_{T+dt}=atan2(vy,vx)\\ k_{T+dt}=0\\ t = T +dt

实现

function [plan_start_x, plan_start_y, plan_start_heading, plan_start_kappa, plan_start_vx, plan_start_vy,...
    plan_start_ax, plan_start_ay, plan_start_time, ...
    stitch_x, stitch_y, stitch_heading, stitch_kappa, stitch_accel, stitch_velocity, stitch_time] = ...
    fcn(pre_trajectory_x, pre_trajectory_y, pre_trajectory_heading,pre_trajectory_kappa, ...
    pre_trajectory_velocity, pre_trajectory_accel, pre_trajectory_time, current_time, host_x,host_y, host_heading_xy, host_vx, host_vy, host_ax, host_ay)
% pre_trajectory_x, pre_trajectory_y: 上周期的规划轨迹序列x,y坐标
% pre_trajectory_heading,pre_trajectory_kappa:上周期的规划轨迹序列的航向角和曲率
% pre_trajectory_velocity, pre_trajectory_accel, pre_trajectory_time:上一周期的轨迹点速度(合加速度大小)、加速度(切向加速度大小)、时间序列
% current_time: 当前时间
% host_x,host_y, host_heading_xy, host_vx, host_vy, host_ax, host_ay:
% 当前时间的的车辆位置x\y坐标,航向角，速度，加速度

dt = 0.1; % 以当前时刻往后100ms的位置作为规划起点

% 初始化上周期用于拼接的轨迹序列
stitch_x = zeros(20, 1);
stitch_y = zeros(20, 1);
stitch_heading = zeros(20, 1);
stitch_kappa = zeros(20, 1);
stitch_accel = zeros(20, 1);
stitch_velocity = zeros(20, 1);
stitch_time = zeros(20, 1) * -1;

persistent is_first;

if isnan(is_first)
    is_first = 0;
    % 如果是首次运行, 规划起点就为当前车辆的状态
    plan_start_x = host_x;
    plan_start_y = host_y;
    plan_start_heading = host_heading_xy;
    plan_start_kappa = 0;
    plan_start_vx = 0;
    plan_start_vy = 0;
    plan_start_ax = 0;
    plan_start_ay = 0;
    plan_start_time = current_time + dt;
else
    % 如果不是首次运行
    host_vx_wcs, host_vy_wcs = vcs_to_wcs(host_vx,host_vy, host_heading_xy); % 自车坐标系转化为世界坐标系
    host_ax_wcs, host_ay_wcs = vcs_to_wcs(host_ax,host_ay, host_heading_xy);% 自车坐标系转化为世界坐标系
    pre_plan_index = point_in_pre_plan(pre_trajectory_time, current_time,1); % 在上个周期中找到本时刻时间车辆应该在位置
    pre_plan_x_desire = pre_trajectory_x(pre_plan_index);
    pre_plan_y_desire = pre_trajectory_y(pre_plan_index);
    pre_plan_heading_desire = pre_trajectory_heading(pre_plan_index);
    in_control = if_in_control(pre_plan_x_desire, pre_plan_y_desire, pre_plan_heading_desire, host_x, host_y);
    if in_control == 1
        % 控制跟上了, 就去上周期寻找current_time + dt的信息,
        % 将其作为规划起点,这一步骤会有轨迹拼接,取出上一周期current_time + dt的点位往前的20个点
        dt_point_index = point_in_pre_plan(pre_trajectory_time, current_time+dt,pre_plan_index);
        plan_start_x = pre_trajectory_x(dt_point_index);
        plan_start_y = pre_trajectory_y(dt_point_index);
        plan_start_heading = pre_trajectory_heading(dt_point_index);
        plan_start_kappa = pre_trajectory_kappa(dt_point_index);
        plan_start_vx = pre_trajectory_velocity(dt_point_index)*cos(plan_start_heading);
        plan_start_vy = pre_trajectory_velocity(dt_point_index)*sin(plan_start_heading);
        pre_tor_a = pre_trajectory_accel(dt_point_index); % 切向加速度大小
        pre_nor_a = pre_trajectory_kappa(dt_point_index) * pre_trajectory_velocity(dt_point_index)^2; % 法向加速度大小
        plan_start_ax = pre_tor_a * cos(plan_start_heading) - pre_nor_a * sin(plan_start_heading);
        plan_start_ay = pre_tor_a * sin(plan_start_heading) + pre_nor_a * cos(plan_start_heading);
        plan_start_time = current_time + dt;
        % 开始轨迹拼接
        dt_point_index = dt_point_index - 1;
        if dt_point_index >=20
            % 有大于20个点
            stitch_x = pre_trajectory_x(dt_point_index-19:dt_point_index);
            stitch_y = pre_trajectory_y(dt_point_index-19:dt_point_index);
            stitch_heading = pre_trajectory_heading(dt_point_index-19:dt_point_index);
            stitch_kappa = pre_trajectory_kappa(dt_point_index-19:dt_point_index);
            stitch_accel =pre_trajectory_accel(dt_point_index-19:dt_point_index);
            stitch_velocity =pre_trajectory_velocity(dt_point_index-19:dt_point_index);
            stitch_time = pre_trajectory_time(dt_point_index-19:dt_point_index);
        else
            stitch_x(1:dt_point_index) = pre_trajectory_x(1:dt_point_index);
            stitch_y(1:dt_point_index) = pre_trajectory_y(1:dt_point_index);
            stitch_heading(1:dt_point_index) = pre_trajectory_heading(1:dt_point_index);
            stitch_kappa(1:dt_point_index) = pre_trajectory_kappa(1:dt_point_index);
            stitch_accel(1:dt_point_index) =pre_trajectory_accel(1:dt_point_index);
            stitch_velocity(1:dt_point_index) =pre_trajectory_velocity(1:dt_point_index);
            stitch_time(1:dt_point_index) = pre_trajectory_time(1:dt_point_index);
        end
    else
        % 控制没跟上了, 从当前位置利用车辆动力学外推current_time + dt的点位信息
        plan_start_x = host_x + host_vx * dt + 0.5 * host_ax * dt^2;
        plan_start_y = host_y + host_vy * dt + 0.5 * host_ay * dt^2;
        plan_start_vx = host_vx + host_ax * dt;
        plan_start_vy = host_vy + host_ay * dt;
        plan_start_heading = atan2(plan_start_vy, plan_start_vx);
        plan_start_kappa = 0;
        plan_start_ax = host_ax ;
        plan_start_ay = host_ay;
        plan_start_time = current_time + dt;
    end
end

function[val_x_wcs, val_y_wcs] = vcs_to_wcs(val_x_vcs,val_y_vcs, theta)
val_x_wcs = val_x_vcs * sin(theta) + val_y_vcs * cos(theta);
val_y_wcs = val_y_vcs * sin(theta) - val_x_vcs * cos(theta);

function[pre_index] = point_in_pre_plan(pre_time_seq, search_time,start_index)
pre_index = 1;
for i = start_index:length(pre_time_seq) - 1
    if search_time >= pre_time_seq(i) && search_time < pre_time_seq(i)
        pre_index = i;
        break
    end
end


function[in_control] = if_in_control(x_desire, y_desire, heading_desire, x_cur, y_cur)
desire_vec = [x_desire; y_desire];% 按照规划本应该在的位置矢量
cur_vec = [x_cur; y_cur];% 实际的位置矢量
err_vec = desire_vec - cur_vec; % 位矢误差向量
tor_vec = [cos(heading_desire), sin(heading_desire)]; % 利用上周期该点位航向角投影
nor_vec = [-sin(heading_desire), cos(heading_desire)]; % 利用上周期该点位航向角投影
err_x = abs(err_vec'*tor);% x方向误差
err_y = abs(err_vec'*nor);% y方向误差
if (err_x > 1.5) || ( err_y > 0.5 )
    in_control = 0;
else
    in_control = 1;
end

规划决策篇：2.2 规划起点确定

前言

如何选定规划起点

首次规划

非首次规划

控制跟上了规划的点位

控制没有跟上规划的点位

实现