【自动驾驶规划】Lattice Planner

原创

已于 2022-11-24 11:32:01 修改 · 6.4k 阅读

于 2021-09-01 22:29:24 首次发布

LatticePlanner是一种局部轨迹规划器，生成无碰撞、满足车辆动力学约束的平滑轨迹。它基于Frenet坐标系进行采样和规划，通过多项式拟合生成轨迹，并通过轨迹评价函数选择最优轨迹。该算法包括感知信息处理、参考线与定位信息融合、轨迹采样、轨迹优化和控制器输入轨迹生成等步骤，确保车辆行驶的安全性和舒适性。

低功耗蓝牙项目，需要一块懂省电的板

思澈 SF32LB52 芯片，BLE 协议栈深度优化，上手即开发

点击查看

Lattice Planner简介

LatticePlanner算法属于一种局部轨迹规划器，输出轨迹将直接输入到控制器，由控制器完成对局部轨迹的跟踪控制。因此，Lattice Planner输出的轨迹是一条光滑无碰撞满足车辆运动学约束和速度约束的平稳安全的局部轨迹。Lattice Planner的输入端主要由三部分组成，感知及障碍物信息、参考线信息及定位信息。局部规划模块的输出是带有速度信息的一系列轨迹点组成的轨迹，其保证了车辆控制器在车辆跟踪控制过程中的平稳性和安全性。

preview

局部规划模块的输出是带有速度信息的一系列轨迹点组成的轨迹，其保证了车辆控制器在车辆跟踪控制过程中的平稳性和安全性。

Lattice规划算法实现过程

Lattice规划算法是一种基于采样的运动规划算法，通过将车辆坐标系转换到参考线坐标系，也就是frenet坐标系下，然后在frenet坐标系下分别对frenet的d轴和s轴进行规划，形成frenet坐标系下的规划轨迹，然后将frenet坐标系下的轨迹合成到世界坐标系下还原为世界坐标系下的轨迹。算法实现过程大概可以分为以下几步：

将车辆当前位姿信息转换到frenet坐标系下，获得车辆在frenet坐标系的初始状态；根据当前速度计算前瞻距离，获得前瞻点，获得车辆在前瞻点位置frenet坐标系下的目标状态。
对轨迹状态进行采样，分别是轨迹运行时间t，目标速度v，及到参考线的横向位移d，通过这三个规划参数可以获得采样状态。
构建横向位移和纵向位移的多项式规划函数s(t)，d(s)，获得横向位移和纵向位移的规划函数后，进行时间插值就可以获得参考线frenet坐标系下的轨迹点，最后将轨迹点从frenet坐标系转换到cartesian坐标系，就可以获得物理世界采样轨迹，由于横向和纵向都是通过高次多项式插值获得，以此cartesian坐标系下的轨迹也是光滑的。
采样轨迹的碰撞检测、曲率约束及最优轨迹打分。采样轨迹是一系列满足速度约束的光滑轨迹，但其还需要满足无碰撞和车辆运动学曲率约束的强制约束，及远离障碍物和靠近参考线等组成的代价约束。采样轨迹的打分就是为了获得一条最优的满足约束条件的无碰撞光滑轨迹。该轨迹也是lattice输出到controller用于车辆跟随的轨迹。