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

阿里云国内75折回扣微信号：monov8

阿里云国际，腾讯云国际，低至75折。AWS 93折免费开户实名账号代冲值优惠多多微信号：monov8 飞机：@monov6

3、纵向轨迹规划求解

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

由于是定速巡航那么此时沿着参考线方向的位置配置则无需考虑仅需要配置纵向速度 $\dot{s_1}$ 和纵向加速度 $\ddot{s_1}$ 。同时由于减少了一个配置量轨迹多项式可以降为一个4次多项式来考虑。由 t_0 时刻的配置 $S_0=[s(t_0),\dot{s(t_0)},\ddot{s(t_0)}]^T$ 和 t_1 时刻目标配置 $S_1=[s(t_1),\dot{s(t_1)},\ddot{s(t_1)}]^T$ 4次多项式可以写为

$\\s(t)=c_{s0}+c_{s1}t+c_{s2}t^2+c_{s3}t^3+c_{s4}t^4 \\\dot{s(t)}=c_{s1}+2c_{s2}t+3c_{s3}t^2+4c_{s4}t^3\\\ddot{s(t)}=2c_{s2}+6c_{s3}t+12c_{s4}t^2$

则有

$\begin{bmatrix} \dot{s(t)} \\ \ddot{s(t)} \end{bmatrix}=\begin{bmatrix} 1 & 2t\\ 0 & 2 \end{bmatrix}\begin{bmatrix} c_{s1} \\ c_{s2} \end{bmatrix}+\begin{bmatrix} 3t^2& 4t^3\\ 6t& 12t^2 \end{bmatrix}\begin{bmatrix} c_{s3} \\ c_{s4} \end{bmatrix}$

同样令 t_0=0 $t_1=\tau$ 则可求得

$c_{s0}=s(0), c_{s1}=\dot{s(0)}, c_{s2}=\frac{1}{2}\ddot{s(0)}$

代入上式可以解得 $c_{s3}, c_{s4}, c_{s5}$

$\\\begin{bmatrix} c_{s3} \\ c_{s4} \end{bmatrix}=\begin{bmatrix} 3\tau^2 &4\tau^3 \\ 6\tau& 12\tau^2 \end{bmatrix}^{-1} (\begin{bmatrix} \dot{s(\tau)} \\ \ddot{s(\tau)} \end{bmatrix}-\begin{bmatrix} 1 & 2\tau\\ 0 & 2 \end{bmatrix}\begin{bmatrix} \dot{s(0)} \\\ddot{s(0)} \end{bmatrix})$

通过设置采样间隔 $\Delta T$ 和速度变化间隔 $\Delta \dot{s(t_1)}$ 也可以生成一系列的备选曲线

4、最优轨迹选择

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

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

C_d=w_JJ_t(d(t))+w_dd^2+w_tT

其中 $J_t(d(t))=\int_{t_0}^{t_1}\dddot{d(\tau)d}\tau$ 即jerk在采样时间内的变化幅度用来评价舒适性； w_dd^2 表示目标状态偏离中心线的指标； w_tT 为车辆行驶效率评价指标。

纵向轨迹评价函数如下

$C_s=w_JJ_t(d(t))+w_s(\dot{s_1}-\dot{s_{set}})^2+w_tT$

与横向评价函数唯一不同的是 $w_s(\dot{s_1}-\dot{s_{set}})^2$ 代表目标配置速度与设定期望速度的差距损失。

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

$C=K_{lat}C_d+K_{lon}C_s$

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

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动态轨迹规划算法

这一部分是该算法的核心函数即完成了纵向、横向轨迹的规划过程备选轨迹的评价过程及碰撞检测过程。最后将当前时刻最优轨迹输出给pathpath属于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