无人驾驶路径规划（三）局部路径规划-Frenet坐标系下的动态轨迹规划

前言：对于无人驾驶路径规划系列的第二篇RRT算法的改进部分，由于有些内容属于个人想到的创新点，有想法投一篇小论文所以暂时没有公开，等后续完成后我会再公开介绍。今天第三篇内容开启一个新的算法介绍：Frenet坐标系下的动态规划。我花了将近半个月的时间来了解、研究算法原理，理解网上python开源的代码，最后根据个人理解在matlab上进行了复现。如果还没有看过我前面文章的读者，可以点击下方的传送门：

无人驾驶路径规划（一）全局路径规划 - RRT算法原理及实现

同样，如果文中有错误或侵权的地方还请各位读者指出，我会及时作出修改，笔者在这先行谢过。

一、轨迹规划方法简介

在第一篇文章中我对无人驾驶路径规划技术做了个简单的介绍，即可以分为全局路径规划和局部路径规划两部分。今天介绍的Frenet坐标系下动态轨迹规划就属于局部路径规划的内容。在这我先简要介绍一下轨迹规划的方法。参考论文：基于激光雷达的环境识别及局部路径规划技术研究。

无人车的局部路径规划不仅需要考虑到道路交通中的静态障碍物的避障问题，还需要对环境中行人、车辆等动态障碍物未来轨迹做出的预判，对未来时间段内的碰撞可能性进行分析，保证车辆安全、舒适的行驶，因而在求解过程中需要考虑时间维度 t。当路径点增加了时间这一维度，实质上转变为了轨迹点，因而在自动驾驶技术领域局部路径规划也可以称为轨迹规划。车辆在三维空间内的轨迹规划，一般有三种常用的策略：

• 一将三维空间离散化，通过启发式搜索算法进行求解。
• 二是将轨迹分解为路径规划和速度规划两个过程。该方法的思想在于通过路径规划完成静态环境下路径求解，然后使用速度规划方法解决运动障碍物的避撞问题。
• 三是将轨迹分解为两个方向关于时间的优化求解问题。最早由于 Werling 提出将三维的轨迹规划分解为横向的运动规划和纵向的运动规划问题，这里的横向是指车辆相对于道路中心的法线方向，纵向指中心线的切线方向。

那么Frenet坐标系下动态轨迹规划实际上就属于第三种，将规划问题分解为纵向和横向来分别求解。

二、Frenet坐标系转换

1、Frenet坐标系概念

在轨迹规划过程中，使用笛卡尔直角坐标系并不能很好的描述车辆当前位置与当前所在车道的关系，因此需要引入Frenet坐标系的概念。在Frenet坐标系中，使用道路中心线作为参考线，将车辆的轨迹点投影到参考线上得到参考点，令沿参考线方向为纵轴s，垂直于参考线方向为横轴d。Frenet坐标系可以很容易确定车辆偏离车道中心线的距离及沿着车道线行驶的距离，可以忽略道路曲率的影响，相比于笛卡尔坐标系下的描述更为简洁和直观。 Frenet坐标系和笛卡尔坐标系关系如下：

2、Frenet-笛卡尔坐标系转换

假设车辆行驶至t时刻实际轨迹和参考轨迹如下：

全局坐标系下，t时刻车辆的状态可以描述为：，各参数含义如下：

，车辆当前位置；

 方位角，速度方向与x正向夹角；

 曲率； 速度；  加速度。

 Q投影到参考线上的点，P在全局坐标系下的位置向量。

Frenet坐标系下，车辆的状态量可以描述为：，各参数含义如下：

 沿参考线纵向位移；  沿参考线纵向速度；  沿参考线纵向加速度；

 参考线法线方向（横向）位移；  横向速度；  横向加速度；

 横向位移对应的弧长的一阶导数； 横向位移对应的弧长的二阶导数。

笛卡尔坐标系与Frenet坐标系状态量转化可以表示为如下关系：

由于具体推导过程比较繁琐和冗长，我在这不进行展开介绍，有兴趣的读者可以参考这篇论文的内容：基于Frenet坐标系的自动驾驶轨迹规划与优化算法。

三、动态轨迹规划过程

1、基于jerk实时轨迹规划

轨迹规划过程实际上是一个优化问题。通常来说，乘坐舒适性和安全性是轨迹规划的目标，过大的加速度变化会使得乘客感到不适。因此轨迹规划过程中可以将加速度变化率，也就是“加加速度”-jerk作为优化的目标，来保证乘坐的舒适性。

根据文献中的介绍，经过Frenet分解后可以构建一个一维积分系统：

其中，为横向/纵向运动，，为jerk。

Takahashi已证明对于上述系统由时刻初始配置和时刻目标配置，基于jerk的优化轨迹都在一个五次多项式中，且必存在最小化的解：

其中，可以用于评价舒适性，是目标配置函数，用以评价轨迹。

2、横向轨迹规划求解

横向规划主要承担的是避障、换道等任务，已知在时刻的配置和时刻目标配置，则五次多项式可以写为：

则有：

为简化运算，令，，则可求得：

代入上式，可以解得：

由此可以求得。值得注意的是，通常来说希望车辆沿着参考线行驶，横向速度和横向加速度均为0，那么时刻的目标配置可以写为。

通过设置采样间隔和横向距离间隔，可以生成一系列的备选曲线：

3、纵向轨迹规划求解

对于纵向轨迹规划，需要考虑到车辆的实际行驶场景需求，例如跟车、定速巡航、停车、汇流等等。不同的行驶情景有不同的期望配置。在这我仅针对最简单的定速巡航情景进行介绍。

由于是定速巡航，那么此时沿着参考线方向的位置配置则无需考虑，仅需要配置纵向速度和纵向加速度。同时，由于减少了一个配置量，轨迹多项式可以降为一个4次多项式来考虑。由时刻的配置和时刻目标配置，4次多项式可以写为：

则有：

同样，令，，则可求得：

代入上式，可以解得：

通过设置采样间隔和速度变化间隔，也可以生成一系列的备选曲线：

4、最优轨迹选择

对于横向和纵向轨迹规划，都生成了一系列的备选轨迹，通过设定评价函数来确定每一时刻的最优路径。

根据参考论文，横向轨迹评价函数如下：

其中：，即jerk在采样时间内的变化幅度，用来评价舒适性；表示目标状态偏离中心线的指标；为车辆行驶效率评价指标。

纵向轨迹评价函数如下：

与横向评价函数唯一不同的是代表目标配置速度与设定期望速度的差距损失。

最后将二者进行合并，加入权重系数，即可得到最优路径评价函数：

通常轨迹评价函数越小，代表该轨迹的代价越小，优先度越高。

5、碰撞检测

Atsushi Sakai在Python代码中设置的碰撞检测方法是：

假设车辆在备选轨迹一个采样时刻内沿参考线方向行驶纵向距离s，若结束后车辆与障碍物的距离小于设定的阈值，则该轨迹不满足碰撞检测需求，进行剔除。对所有备选轨迹进行该项操作。

我在matlab移植代码的时候，没有对这部分进行修改。

四、Python-Matlab代码移植

我在网上找到的算法的Python代码是日本无人驾驶工程师Atsushi Sakai编写的，并已经在github上开源。我在文末也给出了源码的链接，有需要的读者可以自行获取。

为了加深对算法的理解，同时保持之前所有算法验证平台的一致性，我将Python源码移植到了matlab2019。在这我不对Python源码进行解释，仅对我移植后的matlab代码进行一个介绍。

1、初始参数定义（宏定义）

在程序最开始先给出需要用到的参数的定义及数值，例如最大采样横向距离、横向距离采样间隔、最大采样时间、采样时间间隔、最大加速度/速度等等，具体如下：

SIM_LOOP = 500;

MAX_SPEED = 50.0 / 3.6;  % 最大速度 [m/s]
MAX_ACCEL = 2.0;  % 最大加速度 [m/ss]
MAX_CURVATURE = 1.0;  % 最大曲率 [1/m]
MAX_ROAD_WIDTH = 7.0;  % 最大采样横向距离 [m]
D_ROAD_W = 1.0;  % 横向距离采样间隔 [m]
DT = 0.2;  % 采样时间间隔 [s]
MAX_T = 5.0;  % 最大采样时间 [s]
MIN_T = 4.0;  % 最小采样时间 [s]
TARGET_SPEED = 30.0 / 3.6;  % 期望速度 [m/s]
D_T_S = 5.0 / 3.6;  % 纵向速度采样间隔 [m/s]
N_S_SAMPLE = 1;
ROBOT_RADIUS = 2;  % 碰撞检测阈值 [m]
robot_width = 1;   % 机器人宽 [m]
robot_length = 2;  % 机器人长 [m]

% 评价函数权重
K_J = 0.1;
K_T = 0.1;
K_D = 1.0;
K_LAT = 1.0;
K_LON = 1.0;

%% 参数范围确定
c_speed = 10.0 / 3.6;   % 当前速度
c_d = 2.0;              % 当前横向位移
c_d_d = 0.0;            % 当前横向速度
c_d_dd = 0.0;           % 当前横向加速度
s0 = 0.0;               % 当前沿车道线位移
area = 20.0;

2、障碍物点及参考路径生成

原文中是给定了几个参考路径点，采用三次样条的方法生成参考路径。

%% 参考路径点
nodes = [0, 0;
        10, -6;
        20.5 5;
        35, 6.5;
        70.5, 0;
        100, 5];

%% 设置障碍物坐标点
ob = [20, 10
      30, 9;
      30, 6;
      35, 9;
      50, 3;
      75, 0];
%% 生成期望路径
csp = Cubic_spline(nodes);

Cubic_spline是我自定义的三次样条生成函数。

3、定义FrenetPath结构体

定义FrenetPath结构体，存储每一个备选路径的横向、纵向规划结果，及笛卡尔坐标系下的状态量：

%%  Frenet轨迹结构体
FrenetPath.t = [];
FrenetPath.d = [];
FrenetPath.d_d = [];
FrenetPath.d_dd = [];
FrenetPath.d_ddd = [];
FrenetPath.s = [];
FrenetPath.s_d = [];
FrenetPath.s_dd = [];
FrenetPath.s_ddd = [];
FrenetPath.cd = 0.0;
FrenetPath.cv = 0.0;
FrenetPath.cf = 0.0;

FrenetPath.x = [];
FrenetPath.y = [];
FrenetPath.yaw = [];
FrenetPath.ds = [];
FrenetPath.c = [];

4、Frenet动态轨迹规划算法

这一部分是该算法的核心函数，即完成了纵向、横向轨迹的规划过程，备选轨迹的评价过程及碰撞检测过程。最后将当前时刻最优轨迹输出给path，path属于FrenetPath结构体类型。

path =  calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ...
                               s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ...
                               FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ...
                               K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ...
                               MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);

calc_frenet_path_fixed_velocity函数具体内容如下：

function fplist = calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ...
                               s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ...
                               FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ...
                               K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ...
                               MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);
    min_cost = [inf,inf];
    cost = [];
    flag = 1;
    for di=-MAX_ROAD_WIDTH:D_ROAD_W:MAX_ROAD_WIDTH
        for Ti = MIN_T:DT:MAX_T
            fp = FrenetPath;
            lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0, 0, Ti);
            
            for t=0:DT:Ti-DT
                fp.t(1,end+1) = t;
            end
            
            for t=fp.t(1,1):DT:fp.t(1,end)
                fp.d(1,end+1)     = lat_qp.a0   + lat_qp.a1*t    + lat_qp.a2*t^2    + lat_qp.a3*t^3   + lat_qp.a4*t^4  + lat_qp.a5*t^5;
                fp.d_d(1,end+1)   = lat_qp.a1   + 2*lat_qp.a2*t  + 3*lat_qp.a3*t^2  + 4*lat_qp.a4*t^3 + 5*lat_qp.a5*t^4;
                fp.d_dd(1,end+1)  = 2*lat_qp.a2 + 6*lat_qp.a3*t  + 12*lat_qp.a4*t^2 + 20*lat_qp.a5*t^3;
                fp.d_ddd(1,end+1) = 6*lat_qp.a3 + 24*lat_qp.a4*t + 60*lat_qp.a5*t^2;
            end
            
            for tv = TARGET_SPEED - D_T_S * N_S_SAMPLE : D_T_S: TARGET_SPEED + D_T_S * N_S_SAMPLE
                tfp = fp;
                lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti);
                
                for t=fp.t(1,1):DT:fp.t(1,end)
                    tfp.s(1,end+1)     = lon_qp.b0   + lon_qp.b1*t   + lon_qp.b2*t^2   + lon_qp.b3*t^3  + lon_qp.b4*t^4;
                    tfp.s_d(1,end+1)   = lon_qp.b1   + 2*lon_qp.b2*t + 3*lon_qp.b3*t^2 + 4*lon_qp.b4*t^3;
                    tfp.s_dd(1,end+1)  = 2*lon_qp.b2 + 6*lon_qp.b3*t + 12*lon_qp.b4*t^2 ;
                    tfp.s_ddd(1,end+1) = 6*lon_qp.b3 + 24*lon_qp.b4*t;
                end
                Jp = sum(tfp.d_ddd .^2);
                Js = sum(tfp.s_ddd .^2);
                
                ds = (TARGET_SPEED - tfp.s_d(1,end))^ 2;
                tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d(1,end)^2;
                tfp.cv = K_J * Js + K_T * Ti + K_D * ds;
                tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv;
                
                tfp = calc_global_paths(tfp, csp);
                flag = check_paths(tfp, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);  

                if (flag == 1)
                    cost(end+1,:) = [tfp.cf, di];
                    if min_cost(end,1) >= tfp.cf
                        min_cost(end+1,:) = [tfp.cf, di];
                        fplist = tfp;
                    end
                else
                    flag = 1;
                end
                
            end
        end
    end
end

其中QuinticPolynomial和QuarticPolynomial函数就是轨迹规划的具体求解公式：

function lat_qp = QuinticPolynomial(xs, vxs, axs, xe, vxe, axe, time)
    lat_qp.a0 = xs;
    lat_qp.a1 = vxs;
    lat_qp.a2 = axs/2;
    A = [time^3,   time^4,     time^5;
         3*time^2, 4*time^3,   5*time^4;
         6*time,   12*time^2,  20*time^3];
    B = [xe - lat_qp.a0 - lat_qp.a1*time - lat_qp.a2*time^2;
         vxe - lat_qp.a1 - 2*lat_qp.a2*time;
         axe - 2*lat_qp.a2];
    temp = A^(-1)*B;
    lat_qp.a3 = temp(1,1);
    lat_qp.a4 = temp(2,1);
    lat_qp.a5 = temp(3,1);   
end

function lon_qp = QuarticPolynomial(xs, vxs, axs, vxe, axe, time)
    lon_qp.b0 = xs;
    lon_qp.b1 = vxs;
    lon_qp.b2 = axs / 2.0;
    A = [3*time^2, 4*time^3
         6*time,   12*time^2];
    B = [vxe - lon_qp.b1 - 2*lon_qp.b2*time;
         axe - 2*lon_qp.b2];
    temp = A^(-1)*B;
    lon_qp.b3 = temp(1,1);
    lon_qp.b4 = temp(2,1);
end

calc_global_paths用于求解笛卡尔坐标系下的状态量：

function fplist = calc_global_paths(fplist, csp)
    for i = 1:1:size(fplist.s,2)
        [idx] = findPoint(fplist.s(1,i), csp);
        i_x = csp(idx,1);
        i_y = csp(idx,2);
        i_yaw = (csp(idx+1,2) - csp(idx,2))/(csp(idx+1,1) - csp(idx,1));
        di = fplist.d(1,i);
        fplist.x(1,end+1) = i_x - di * sin(i_yaw);
        fplist.y(1,end+1) = i_y + di * cos(i_yaw);
    end
    
    for i = 1:1:size(fplist.x,2)-1
        dx = fplist.x(1,i+1) - fplist.x(1,i);
        dy = fplist.y(1,i+1) - fplist.y(1,i);
        fplist.yaw(1,end+1) = atan(dy/dx);
        fplist.ds(1,end+1) = sqrt(dx^1 + dy^2);
    end
    
    fplist.yaw(1,end+1) = fplist.yaw(1,end);
    fplist.ds(1,end+1) = fplist.ds(1,end);
    
    for i=1:1:size(fplist.yaw,2)-1
        fplist.c(1,end+1) = (fplist.yaw(1,i+1) - fplist.yaw(1,i+1))/fplist.ds(1,i);
    end
    fplist.c(1,end+1) = fplist.c(1,end);
end

check_paths用于碰撞检测，在这里还加入了最大纵向加速度、最大纵向速度和最大曲率约束。我设置了一个标志位flag，只有当通过碰撞检测，即标志位为1的时候，才会进行后续的判断，否则该轨迹就被舍弃：

function flag = check_paths(fplist, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS)
    flag = 1;
    for i=1:1:size(fplist.s_d ,2)
        if (fplist.s_d(1,i) > MAX_SPEED)
            flag = 0;
            break
        end
        
        if (fplist.s_dd(1,i) > MAX_ACCEL)
            flag = 0;
            break
        end
        
        if (fplist.c(1,i) > MAX_CURVATURE)
            flag = 0;
            break
        end
    end
    
    if (flag == 1)
        for i=1:1:size(fplist.x,2)
            for j=1:1:size(ob)
                d = sqrt((ob(j,1)-fplist.x(1,i))^2 + (ob(j,2)-fplist.y(1,i))^2);
                if(d <= ROBOT_RADIUS)
                    flag = 0;
                    break
                end
            end
            if (flag == 0)
                break
            end
            if (flag == 0)
                break
            end
        end
    end
end

若当前轨迹通过碰撞检测即标志位为1，则与截至目前最小代价的轨迹进行比较，如果新的轨迹的代价小于当前的最小代价，则把当前轨迹选作临时最优轨迹，并把其代价值赋给最小代价；若标志位为0，则将其复位为1，并不进行最小代价的判断过程：

if (flag == 1)
    cost(end+1,:) = [tfp.cf, di];
    if min_cost(end,1) >= tfp.cf
        min_cost(end+1,:) = [tfp.cf, di];
        fplist = tfp;
    end
else
    flag = 1;
end

5、主循环部分

当循环次数小于最大循环次数时，重复进行轨迹动态规划的过程，如果车辆到达了参考轨迹的最后一个点，则提前跳出循环过程。每次循环都对车辆当前的位置和最优轨迹进行显示：

%% 循环过程
for i=1:1:SIM_LOOP
    path =  calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, s0,MAX_ROAD_WIDTH, ...
                               D_ROAD_W, MIN_T, MAX_T, DT, FrenetPath, TARGET_SPEED,...
                               D_T_S, N_S_SAMPLE,K_D, K_J, K_LAT, K_LON, K_T, ob,...
                               MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);

    s0 = path.s(1,2);
    c_d = path.d(1,2);
    c_d_d = path.d_d(1,2);
    c_d_dd = path.d_dd(1,2);
    c_speed = path.s_d(1,2);
    

    plot(csp(:,1), csp(:,2),'-.b'); hold on
    plot(csp(:,1), csp(:,2)+8,'-k');
    plot(csp(:,1), csp(:,2)-8,'-k');
    plot(ob(:,1), ob(:,2),'*g');
    plot_robot(robot_length, robot_width, path.yaw(1,1) , path.x(1,1), path.y(1,1));
    plot(path.x(1,1), path.y(1,1),'vc','MarkerFaceColor','c','MarkerSize',6);
    plot(path.x(1,:), path.y(1,:),'-r','LineWidth',2);
    plot(path.x(1,:), path.y(1,:),'ro','MarkerFaceColor','r','MarkerSize',4);    
    set(gca,'XLim',[path.x(1,1) - area, path.x(1,1) + area]);
    set(gca,'YLim',[path.y(1,1) - area, path.y(1,1) + area]);
    grid on
    title('Frenet');
    xlabel('横坐标 x'); 
    ylabel('纵坐标 y');
    pause(0.01);
    hold off
    
    if sqrt((path.x(1,1) - nodes(end,1))^2+ (path.y(1,1) - nodes(end,2))^2) <= 1.5
        disp("Goal");
        break
    end
end

6、算法运行效果

Python源码运行效果如下：

matlab2019代码移植运行效果如下：

五、结语

由仿真结果可以看出，车辆在定速巡航的模式下可以按照期望的速度沿着参考线行驶，当遇到障碍物的时候由于实时规划采样的缘故，即使最优轨迹会与障碍物相交，车辆也可以做出判断从而选择其他备选轨迹，从而避开障碍物，保证行驶的安全性。

对于其他的车辆行驶情景，均可以根据需求进行目标配置和评价函数的修改，在这我不在做更多的说明。

PS： Frenet坐标系下动态轨迹规划技术是在2010年的ICRA（机器人领域顶会）上首次提出并得到广泛的关注，会议原文链接如下：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frames

Takahashi关于jerk优化过程五次多项式推导原文链接如下：

Local Path Planning And Motion Control For Agv In Positioning

Atsushi Sakai的Python源码地址如下：

https://github.com/AtsushiSakai/

最后，如有需要我matlab原码的朋友可以在评论区留下邮箱，我看到后会及时回复。

彩蛋：这篇文章的公式实在是太多了，手都要敲麻了TAT。