Lattice Planner从入坑到入土（一）

旧势力底层学习者

已于 2023-06-14 01:22:44 修改

阅读量860

点赞数 1

分类专栏：路径规划与算法文章标签： linux c++ 自动驾驶

于 2023-05-23 17:54:30 首次发布

本文链接：https://blog.csdn.net/qq_46274948/article/details/130747932

版权

路径规划与算法专栏收录该内容

1 篇文章 1 订阅

订阅专栏

最近在使用DWA和TEB，发现对障碍物的规划如何同其它局部规划器进行切换控制时会出现局部死区的情况（各种原因导致，最典型的是出现无规划的状况）。

然而Lattice Planner简单粗暴直接的解决了一系列问题，也省去了各规划区切换的问题。

一、Frenet坐标系

经典论文：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame

基于Frenet坐标系的自动驾驶轨迹规划与优化算法 - 道客巴巴

我们常用的是笛卡尔坐标系（后续称 cartesian 坐标系），其中最常用的就是典型的直角坐标系，还有东北天坐标系，即ENU坐标系。在对车辆坐标进行描述时，在 cartesian 坐标系中，我们使用的是车辆的位置、状态信息，用(x,y,θ,vx,vy,vθ)来表示：

那么在路径规划中，我们通过全局规划器（A*/Dijkstra等）规划出了一条全局路径，也就是图中的曲线，后续我们称为 reference line，其方向以箭头为正向。

假设现在车辆坐标为 (x,y) ，朝向为箭头所指方向，记为 θ ，从reference line 找一个最近点，记为点 (x2,y2)，其切线方向记为 $\Theta _{2}$ ，那么车辆的暂运行的误差就始终需要通过这两个在 cartesian 坐标系下的量来进行计算：

那么 Frenet 坐标系提出就是为了符合车辆的运动状态，即纵向状态与横向状态，采用四次/五次多项式来描述车辆当前的运行轨迹，后续表示为 Trajectory 。

Frenet 坐标系我借用一位大神画的一个动态图来表示（大神的文章我没找到，当时存了个图，未表明出处，后续专门补一下，非常抱歉），这里要记住，Frenet坐标系是随车辆在 cartesian 坐标下的位置进行改变的，理解为随动：

这个动图中的坐标系就是笛卡尔坐标系，而随鼠标活动的就是 Frenet 坐标系， Frenet坐标系中我们将沿 reference line 切线的蓝色作为 s 轴，红色的为横向 d 轴，正常的路线的采样我们使用论文中的这张图更为准确：

Frenet 坐标系的好处有两点：

1---在此坐标系下我们可以清楚的知道当前偏离车道中心线的距离以及车辆沿车道中心线形势距离。

2---cartesian 坐标系下的运动规划是 t 时间内它所规划的横向与纵向运动，是一个耦合系统，转为 Frennet 坐标系下将其分解为横向---t 与纵向---t 两个一维规划问题，并且可以各自对 t 求导，更容易表达车辆状态。

二、Frenet坐标系与笛卡尔坐标系的状态转换

我们在cartesian坐标系下的运动状态描述为 $\left [\vec{x}, \Theta ,v_{x},a_{x}, k_{x}\right ]$ :

$\vec{x}$ --------cartesian坐标系下的位置，这里用向量来表示，也可以表示为位置（x,y）；

$\Theta$ --------朝向角；

$v_{x}$ -------车辆速度；

$a_{x}$ -------车辆加速度；

$k_{x}$ -------实际轨迹曲率；

我们说车辆在Frenet坐标系下的运动状态描述为 $\left [ s,s{}',s{}'',d,d{}',d{}'',\dot{d},\ddot{d}\right ]$ ：

$s$ -----------纵向位移；

$s{}'$ ----------纵向速度（纵向位移对时间 $t$ ）；

$s{}''$ ---------纵向加速度；

同理 $d,d{}',d{}''$ 分别对应横向位移，速度，加速度；

比较特殊的是 $\dot{d}$ 与 $\ddot{d}$ ，是横向位移对弧长的一阶导数与二阶导数。

（代码中由于符号 $d$ 代表了导数，所以上方在Frenet坐标系运动描述中的 $d$ 全部置换为 $l$ ,并且与下方的 “l” 一一对应）

2.1 Frenet坐标系的point match

上述具体的推理过程这里并不展示，主要说一个重点，即求车辆当前状态下在 reference line上所对应的 Frenet 坐标系下的运动状态，首先主要是投影点的状态计算。reference line 是车辆运动参考线，也可以说是当前车道中心线，在Apollo中对其进行了离散化处理，这里因为直接使用了全局规划器，就不涉及离散化处理，即微观化（简单理解放大）都为点：