精华内容
下载资源
问答
  • 局部路径规划
    千次阅读
    2018-12-24 14:40:15

    更多相关内容
  • 机器人局部路径规划算法——VFH系列论文。主要根据传感器的观测数据,更新占用栅格地图,然后计算下一步的运动方向。
  • 为了实现未知复杂环境下机器人的局部路径规划,提出了一种新的局部路径规划方法,使机器人自主探测周边障碍物情况。通过滚动窗口计算局部目标等途径进行路径规划,从而实现机器人无碰撞到达全局目标点。该方法可以使...
  • 人工势场法是一种常用的具有算法简单和便于实时控制的局部路径规划方法,但存在容易产生局部极小值的问题。基于模糊逻辑的局部路径规划法具有环境适应性强等优点,它在连续论域内采用模糊路径规划时,计算量比较大。...
  • 文章针对近年来的无人驾驶汽车路径规划算法进行总结和归纳。首先对目前主流的环境建模方法进行阐述;其次对路径规划算法进行介绍,通过分析其优缺点,指出融合轨迹规划算法具有最好的适用性;最后总结当前研究挑战并提出...
  • 利用遗传算法( GA)实现 AUV(自主水下机器人)对于运动目标的自主避碰,根据前视声纳信息探测到 的障碍物距离信息,AUV的运动信息以及航迹规划信息。算法采用了二进制编码规则,并把避碰、航迹跟踪等 约束条件作为适应度...
  • 前言:对于无人驾驶路径规划系列的第二篇RRT算法的改进部分,由于有些内容属于个人想到的创新点,有想法投一篇小论文所以暂时没有公开,等后续完成后我会再公开介绍。今天第三篇内容开启一个新的算法介绍:Frenet...

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

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

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

    一、轨迹规划方法简介

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

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

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

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

    二、Frenet坐标系转换

    1、Frenet坐标系概念

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

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

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

    全局坐标系下,t时刻车辆的状态可以描述为:[\vec{x}, \theta_{x}, \kappa _{x}, v_{x}, a_{x}],各参数含义如下:

    \vec{x} -Q(x, y) / Q (s, d),车辆当前位置;

    \theta_{x}- 方位角,速度方向与x正向夹角;

    \kappa _{x}- 曲率;v_{x}- 速度; a_{x}- 加速度。

    \vec{r}(t)- Q投影到参考线上的点,P在全局坐标系下的位置向量。

    Frenet坐标系下,车辆的状态量可以描述为:[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d}, \frac{\ddot{d}}{​{d}'}, {d}''],各参数含义如下:

    s- 沿参考线纵向位移; \dot{s}- 沿参考线纵向速度; \ddot{s}- 沿参考线纵向加速度;

    d- 参考线法线方向(横向)位移; \dot{d}- 横向速度; \ddot{d}- 横向加速度;

    {​{d}'}- 横向位移对应的弧长的一阶导数;{​{d}''}- 横向位移对应的弧长的二阶导数。

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

    \\v_x =\frac{\dot{s}(1-\kappa_rd)}{cos\Delta \theta} \\a_x=\dot{v_x}=\ddot{s}\frac{(1-\kappa_rd)}{cos\Delta \theta}+\frac{\dot{s}^2}{cos\Delta\theta}[(1-\kappa_rd)tan\Delta\theta\Delta\dot{\theta}-({\kappa_r}'d+\kappa_r{d}')] \\x = x_r - dsin\Delta\theta_r \\y = y_r + dcos\Delta\theta_r \\\theta_x = arctan\frac{​{d}'}{1-\kappa_rd}+\theta_r \\\kappa_x =\frac{\theta_x^k-\theta_x^{k-1}}{\sqrt{(x_k-x_{k-1})^2}+(y_k-y_{k-1})^2}

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

    三、动态轨迹规划过程

    1、基于jerk实时轨迹规划

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

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

    \dot{\overrightarrow{u(t)}}=\begin{bmatrix} 0 &1 &0 \\ 0 &0 &1\\ 0& 0 & 0 \end{bmatrix}\overrightarrow{u(t)}+\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}\dddot{f(t)}

    其中\overrightarrow{u(t)}=[f(t), \dot{f(t)}, \ddot{f(t)}]f(t)为横向/纵向运动,d(t)/s(t)\dddot{f(t)}为jerk。

    Takahashi已证明对于上述系统由t_0时刻初始配置S_0=[f(t_0), \dot{f(t_0)}, \ddot{f(t_0)}]^Tt_1时刻目标配置S_1=[f(t_1), \dot{f(t_1)}, \ddot{f(t_1)}]^T,基于jerk的优化轨迹都在一个五次多项式中,且必存在最小化J_{f(t)}的解:

    J_{f(t)}=\int_{t_0}^{t_1}g(\dddot{f(t)})dt+h(\overrightarrow{u(t)},t)_{t1}

    其中g(\dddot{f(t)})=\frac{1}{2}\dddot{f(t)},可以用于评价舒适性,h(\overrightarrow{u(t)},t)_{t1}是目标配置函数,用以评价轨迹。

    2、横向轨迹规划求解

    横向规划主要承担的是避障、换道等任务,已知在t_0时刻的配置D_0=[d(t_0),\dot{d(t_0)},\ddot{d(t_0)}]^Tt_1时刻目标配置D_1=[d(t_1),\dot{d(t_1)}, \ddot{d(t_1)}]^T,则五次多项式可以写为:

    \\d(t)=c_{d0}+c_{d1}t+c_{d2}t^2+c_{d3}t^3+c_{d4}t^4+c_{d5}t^5 \\\dot{d(t)}=c_{d1}+2c_{d2}t+3c_{d3}t^2+4c_{d4}t^3+5c_{d5}t^4 \\\ddot{d(t)}=2c_{d2}+6c_{d3}t+12c_{d4}t^2+20c_{d5}t^3

    则有:

    \begin{bmatrix} d(t) \\\dot{d(t)} \\\ddot{d(t)} \end{bmatrix}=\begin{bmatrix} 1 & t^2 & t^3\\ 0& 1 & 2t\\ 0& 0 & 2 \end{bmatrix}\begin{bmatrix} c_{d0} \\c_{d1} \\c_{d2} \end{bmatrix}+\begin{bmatrix} t^3 & t^4 & t^5\\ 3t^2& 4t^3 & 5t^4\\ 6t& 12t^2 & 20t^3 \end{bmatrix}\begin{bmatrix} c_{d3} \\c_{d4} \\c_{d5} \end{bmatrix}

    为简化运算,令t_0=0t_1=\tau,则可求得:

    c_{d0}=d(0), c_{d1}=\dot{d(0)}, c_{d2}=\frac{1}{2}\ddot{d(0)}

    代入上式,可以解得c_{d3}, c_{d4}, c_{d5}

    \begin{bmatrix} c_{d3} \\c_{d4} \\c_{d5} \end{bmatrix}=\begin{bmatrix} \tau^3 & \tau^4 &\tau^5 \\ 3\tau^2& 4\tau^3 & 5\tau^4\\ 6\tau& 12\tau^2 & 20\tau^3 \end{bmatrix}^{-1} \bigl(\begin{smallmatrix} (\begin{bmatrix} d(\tau) \\ \dot{d(\tau)} \\ \ddot{d(\tau)} \end{bmatrix}-\begin{bmatrix} 1 & \tau^2 & \tau^3\\ 0& 1 & 2\tau\\ 0& 0 & 2 \end{bmatrix}\begin{bmatrix} c_{d0} \\ c_{d1} \\ c_{d2} \end{bmatrix} \end{smallmatrix}\bigr)=\begin{bmatrix} \tau^3 & \tau^4 &\tau^5 \\ 3\tau^2& 4\tau^3 & 5\tau^4\\ 6\tau& 12\tau^2 & 20\tau^3 \end{bmatrix}^{-1} \bigl(\begin{smallmatrix} (\begin{bmatrix} d(\tau) \\ \dot{d(\tau)} \\ \ddot{d(\tau)} \end{bmatrix}-\begin{bmatrix} 1 & \tau^2 & \tau^3\\ 0& 1 & 2\tau\\ 0& 0 & 2 \end{bmatrix}\begin{bmatrix} d(0) \\ \dot{d(0)} \\ \frac{1}{2}\ddot{d(0)} \end{bmatrix} \end{smallmatrix}\bigr)

    由此可以求得d(t)。值得注意的是,通常来说希望车辆沿着参考线行驶,横向速度和横向加速度均为0,那么t_1时刻的目标配置可以写为D_1=[d(t_1),0, 0]^T

    通过设置采样间隔\Delta T和横向距离间隔\Delta d,可以生成一系列的备选曲线:

    3、纵向轨迹规划求解

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

    由于是定速巡航,那么此时沿着参考线方向的位置配置则无需考虑,仅需要配置纵向速度\dot{s_1}和纵向加速度\ddot{s_1}。同时,由于减少了一个配置量,轨迹多项式可以降为一个4次多项式来考虑。由t_0时刻的配置S_0=[s(t_0),\dot{s(t_0)},\ddot{s(t_0)}]^Tt_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=0t_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动态轨迹规划算法

    这一部分是该算法的核心函数,即完成了纵向、横向轨迹的规划过程,备选轨迹的评价过程及碰撞检测过程。最后将当前时刻最优轨迹输出给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挑战赛,首次接触到了机器人导航领域,...​ 机器人导航的路径规划问题主要分为全局路径规划和局部路径规划,这两者是根据对环境信息获取程度划分的。 全局规划通常需

    简介

    ​ 最近,作者参加了关于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。

    t i m e + e l a s t i c b a n d = t i m e d e l a t i c s b a n d time + elastic band = timed elatics band time+elasticband=timedelaticsband

    其目标函数的定义:

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

    优点

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

    缺点

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

    模型预测控制(MPC)

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

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

    问题模型

    m i n C F ( x ( t f ) ) + f t = t 0 t f C R ( x , u ) d t x ˙ = f ( x , u ) g ( x , u ) < 0 h ( x , u ) = 0 minC_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\\ minCF(x(tf))+ft=t0tfCR(x,u)dtx˙=f(x,u)g(x,u)<0h(x,u)=0

    参数空间

    m i n C F ( x ( t f ) ) + f t = t 0 t f C R ( x , u ) d t minC_F(x(t_f))+f_{t=t_0}^{t_f}C_R(x, u)dt\\ minCF(x(tf))+ft=t0tfCR(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学习笔记-局部路径规划算法对比

    后续

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

    展开全文
  • dwa轨迹规划,局部路径规划

    千次阅读 2021-05-09 16:14:41
    # 更新机器人状态的函数 u ; u:卡尔曼滤波中表示控制向量 def updateState(state,u,dt): """ :param state [x,y,里程计走的角度,当前的速度,当前的角速度]: :param u 控制向量 [速度,角速度]: ...
    展开全文
  • 基于改进人工势场法的路径规划,解决了局部极小值问题和目标不可达问题
  • 针对RoboCup足球机器人比赛对抗性强和实时性要求高以及障碍物运动状态时刻变化的特点,而传统路径规划方法的机器人路径规划方法难以满足比赛需要的情况,提出了一种基于模糊神经网络的局部路径规划方法,通过定义...
  • 路径规划】基于matlab DWA算法机器人局部避障路径规划【含Matlab源码 890期】.zip
  • VFH避障/局部路径规划算法

    千次阅读 2021-08-31 16:43:04
    VFH避障/局部路径规划算法 本文章是我看《The Vector Field Histogram-Fast Obstacle Avoidance for Mobile Robots》论文时候的笔记,感兴趣的欢迎去看原论文。 VFH中有一些概念是从其他算法中借鉴过来的,并且VFH...
  • VFH*避障/局部路径规划算法

    千次阅读 2021-09-11 12:37:35
    VFH*避障/局部路径规划算法1、VFH+存在的问题——dead-end2、VFH*算法2.1 VFH*算法概述2.1.1 VFH*的参数2.2.2 表示2.2.3 算法步骤2.2 投影位置和方向2.3 代价函数2.3.1 kek_eke​项2.3.2 平滑项2.3.3 折扣因子λ2.4 ...
  • 全局路径规划与局部路径规划有什么区别?

    万次阅读 多人点赞 2019-04-28 10:50:52
    移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划路径规划是移动机器人导航最基本的环节,指的是机器人在有障碍物的工作环境中,如何找到一条从起点到...
  • ROS常用局部路径规划算法比较

    千次阅读 多人点赞 2021-06-07 20:07:39
    本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA、TEB、MPC等算法在使用过程中的各自的优缺点。以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的地方,还希望在评论区多多讨论。 1. 动态...
  • 提出了基于粒子群径向基函数网络的矿井救援机器人局部路径规划研究。利用算法模拟矿井复杂环境对救援机器人进行训练,调整权值,从而得到最优解,同时利用确定性局部规划算法来优化粒子群算法,使其对局部的处理更加合理...
  • 针对当前水下机器人多以前视声呐作为环境感知设备的特点,设计了一种基于声学图像处理的水下机器人 局部路径规划算法 。给出了基于声学图像处理的局部路径规划基本步骤 。以前视图像声呐得到的声学图像为基础,进行...
  • 路径规划算法学习笔记(三)——局部路径规划(Dubins Curve)路径规划算法学习笔记(三)——局部路径规划(Dubins Curve)路径规划算法学习笔记(三)——局部路径规划(Dubins Curve)实际问题Dubins曲线基础问题关键CSC型CCC...
  • 局部路径规划的人工势场法 源代码 经调试后发现可以避障且能到达目标点-Local path planning of artificial potential field source can be found by debugging and can reach the target point of obstacle ...
  • 针对传统人工势场法在智能车辆局部路径规划中未充分考虑车辆动力学和运动学约束的不足,提出一种基于动态虚拟障碍物的局部路径规划方法.首先根据环境、车辆运行状态和道路交通规则分析车辆行驶安全性并获得虚拟车道线...
  • 基于QL-anfis算法的移动机器人局部路径规划研究
  • 局部路径规划:五次多项式

    千次阅读 2022-02-19 21:18:59
    重点注意:进行曲线插值时,分2步: 1,确定所用的曲线类型,即确定曲线的...横纵向解耦分别用多项式曲线进行分别规划,即横纵向分别对应着2个不同的多项式曲线,然后进行合并即可得到横纵向联合控制的曲线。 ...
  • 终于做完了机器人movebase路径规划算法的修改工作,现在把工作时写的笔记分享出来,希望可以起到搞清楚算法步骤的作用(建议配合代码一起食用) ----------------------------------------------------------------...
  • 为解决复杂环境下机器人路径规划问题,提出了基于人工免疫网络(artmcial immune network,AIN)的移动机器人局部路径规划算法。建立了AIN与机器人局部路径规划问题的映射关系,给出了算法流程,最后对提出的方法...
  • 为此,提出一种基于Kinect的移动机器人实时局部路径规划方法。利用Kinect产生的RGB图像和3D图像实时获取移动机器人的周边动态环境信息,通过模糊逻辑和信息融合技术完成机器人的实时路径规划,从而实现机器人的目标...
  • 基于Autoware分析op_local_planner局部路径规划模块 今天来讲讲Autoware中的局部路径规划模块,实际道路场景中有这么一种情况,路边停车起步,那么这个时候如果你的车辆不在道路上,那么怎么回到道路上呢? 不用担心...
  • 局部路径规划之贝塞尔曲线

    千次阅读 2021-05-04 11:25:57
    而五阶贝塞尔曲线的曲率变化相对平缓,根据车辆运动学模型可知,曲率的突变会引起方向盘转角的突变、平缓变化的曲率对应的方向盘转角变化也相对平稳,所以,在车辆变道的路径规划中,更推荐使用五阶贝塞尔曲线。...
  • 双目避障 单目避障是否可行? 全局路径规划有RRT、A*、D*。 局部路径规划有DWA、TEB。 控制有 MPC。

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 245,531
精华内容 98,212
关键字:

局部路径规划

友情链接: 小车(开发中).rar