前言：对于无人驾驶路径规划系列的第二篇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。

简介

​ 最近，作者参加了关于RMUS 高校 SimReal挑战赛，首次接触到了机器人导航领域，这里记录一下这段时间的收货。sim2real的全称是simulation to reality，是强化学习的一个分支，同时也属于transfer learning的一种。主要解决的问题是机器人领域中，直接让机器人或者机械臂在仿真中对于物理环境存在误差，如何将仿真上取得的成果应用到实际中的问题。

​ 机器人导航的路径规划问题主要分为全局路径规划和局部路径规划，这两者是根据对环境信息获取程度划分的。

• 全局规划通常需要在已知环境中进行，属于一种事前规划，可以找到最优解，一旦环境发生变化，未及时更新地图时，该方法就不能达到预期效果。
• 局部路径规划通常用在未知或部分已知的环境中，系统根据传感器实时获取到环境障碍物的信息，并做出相应规划，这对系统的实时计算处理能力有着较高的要求。由于缺乏全局环境信息，结果很可能不是最优的。

​ 全局路径规划和局部路径规划并没有本质的区别，很多适用于全局路径规划的方法经过改进也可以用于局部路径规划，而适用于局部路径规划的方法同样经过改进后也可适用于全局路径规划。两者协同工作，机器人可更好的规划从起始点到终点的行走路径。

​ 本文主要针对局部路径规划作介绍。

局部路径规划

​ 机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略。常用的局部路径规划算法有动态窗口法(DWA)、时间弹性带(TEB)和模型预测控制(MPC)。

动态窗口法(DWA)

​ 动态窗口法在一定程度上采用了粒子滤波的思想，在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。

基本思想

1. 在机器人的控制空间中离散采样 (dx,dy,dtheta)
2. 对于每个采样速度，从机器人的当前状态执行前向模拟，以预测如果采样速度应用于某个（短）时间段会发生什么。
3. 使用包含以下特征的度量来评估前向模拟产生的每个轨迹：与障碍物的距离、与目标的距离、与全局路径的距离和速度，排除非法轨迹（与障碍物相撞的轨迹）。
4. 选择得分最高的轨迹并将相关的速度发送到移动基地。
5. 重复执行上述步骤。

优点

• 计算简单
• 适用于差分和全向车模

缺点

• 前瞻性不足
• 动态效果差
• 不适用于阿克曼模型车模

时间弹性带(TEB)

​ 时间弹性带(TEB)简而言之，就是连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time。

$$time+elasticband=timedelaticsband$$

其目标函数的定义：

​ 虽然待优化的橡皮筋有不少状态点与时间段，目标函数也好像很多。但是，每个目标函数只与橡皮筋中的某几个状态有关，而非整条橡皮筋。将它描述成图，然后用图优化。

优点

• 前瞻性好
• 适用于各种车模

缺点

• 计算复杂
• 速度和角速度波动大，控制不稳定

模型预测控制(MPC)

​ MPC其实是一种基于对受控对象进行预测的控制方法。MPC最大的特点在于，相对于LQR控制而言，MPC可以考虑空间状态变量的各种约束，而LQR，PID等控制只能够考虑输入输出变量的各种约束。

​ MPC的作用机理可以表述为：在每一个采样时刻，根据当前的测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素用于被控对象；在下一个采样时刻，用新的测量值作为此时预测系统未来动态的初试条件，刷新优化问题求解。应用于机器人的典型的模型预测控制方法：

问题模型

$$\begin{gathered} min{C}_F(x(t_f))+f_{t=t_0}^{t_f}{C}_R(x,u)dt \\ \dot{x}=f(x,u) \\ g(x,u)<0 \\ h(x,u)=0 \end{gathered}$$

参数空间

$$min{C}_F(x(t_f))+f_{t=t_0}^{t_f}{C}_R(x,u)dt$$

控制

传统MPC控制框图为

1. 设置优化问题
2. 使用测量模块告诉我们当前的initial state
3. 求解优化问题得到参数，这些参数构成系统的最优输入 u*
4. 使用 u*驱动系统，由于系统受到干扰无法保证求解得到的 u*我们想要的，仅此旨在很小一段时间中使用，然后利用观测的状态重新求解问题，转回步骤(2)

局部规划器的应用

​ 这里以mpc_local_planner为例，首先，在本地终端中键入

sudo apt install ros-melodic-mpc-local-planner

​ 由于apt安装有可能导致我们运行失败，上面的指令只是用来安装依赖。同时，克隆远程仓库mpc_local_planner到ros的工作空间中，执行catkin_make编译。对应ros navigation中的move_base.launch，修改其中的局部路径规划器类型

<?xml version="1.0"?>
<launch>

  <!-- 运行move_base节点  -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    
    <!--move_base参数配置http://wiki.ros.org/move_base -->
    <param name="base_global_planner" value="global_planner/GlobalPlanner" /><!-- 选择全局规划器类型 -->
    <rosparam file="$(find mpc_navigation)/config/base_global_planner_params.yaml" command="load" />
    
    <rosparam file="$(find mpc_navigation)/config/mpc_local_planner_params_minimum_time.yaml" command="load" />
    <param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />

    <rosparam file="$(find mpc_navigation)/config/move_base_params.yaml" command="load" /><!-- 其它参数 -->

    <param name="controller_frequency" value="1" /><!-- 选择全局规划器类型 -->

    <!-- 全局代价地图和局部代价地图参数配置http://wiki.ros.org/costmap_2d -->
    <rosparam file="$(find mpc_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mpc_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mpc_navigation)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mpc_navigation)/config/global_costmap_params.yaml" command="load" />
    
  </node>

</launch>

​ 对应mpc_local_planner的参数文件可以在克隆仓库中的mpc_local_planner_examples中获得，将cfg/diff_drive中的参数yaml文件复制到对应的config文件夹中即可，base_ local_planner参数设置为mpc_local_planner中定义的类型MpcLocalPlannerROS。

​ mpc_local_planner提供了两个参数文件，一个是以最短时间为目的，另一个是二次规划模型，可以根据需求自行选择。

​ 其他的局部规划器同样是修改base_local_planner的类型和提供相应的功能包以及yaml参数文件实现。

参考

ros-dwa_local_planner

ros-teb_local_planner

ros-mpc_local_planner

ROS学习笔记-局部路径规划算法对比

后续

 喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！