自动驾驶 规划综述

Motion Planning

What is motion planning?

规划的本质是:argmin_{x}f(x)

- 搜索问题

- “好”的规划就是一个目标函数:f(x),求最优解

Motion Planning的三个领域

  • Robotic Fields
    • 生成轨迹寻找目标
    • RRT, A*, D* ,D* Lite
  • Control Theory
    • 动态系统理论实现目标状态
    • MPC模型预测控制, LQR线性二次型
  • AI:状态到action的映射
    • 强化学习,模仿学习等

http://planning.cs.uiuc.edu/http://planning.cs.uiuc.edu/par1.pdf

如何解决一个Motion Planning问题?

我们需要找到一个简单的突破口,首先是一个path finding 问题,也就是路径选择问题。在这个问题中,我们不关心速度,不关心机器人如何运动,我们只关心路径的生成。

什么样的Path才是最好的path?

PathFinding.jshttps://qiao.github.io/PathFinding.js/visual/

无人驾驶中的规划和A* search有什么区别?

  • 无人驾驶场景下是部分感知
  • 有动态障碍物
  • 复杂环境:交通约束、碰瓷
  • 而A* 是一个global algorithm

  • Partial observed situation
    • 贪心算法
      increamental search:目前状态求解到最优
  • D* 
    • 部分环境信息的一种搜索算法,经典的apollo登月小车上就采用了这种算法。
    • D* Lite
    • 使用D*算法进行路径搜索不一定能够搜索到全局最优解,但是经过统计学的分析,是能够逼近全局最优解的。
      Stentz Anthony, "Optimal and Efficient Path Planning for Partially-Known Environments", 1994

至此,我们已经有了如下几个方法:

  • 构造目标函数并且结合了平滑性和目标cost
  • 使用通用的search方法来最小化Cost从而找到一个最优解
  • 通过Partial observed information 来做局部planning

路径规划还需要什么?

  • 处理动态障碍物,动态环境
  • 遵守交通规则,公共安全
  • 实时计算
    • 计算时间100ms-150ms
    • 人的一般反应时间300ms-500ms
    • 酒驾 1000ms
    • 有限时间内找到最优解
      这就是为什么很多大公司使用C++而不用python的原因,因为C++能够对代码进行更多的优化。

motion planning定义

  • Safely
  • Smootly
  • Achieve to destination (能够到达目的地)
  • 输出三维的轨迹 X,Y,Time :3D trajectory optimization problem
  • 无人车硬件系统
    • 定位设备
    • 感知设备
  • 无人车软件信息
    • 动态信息
    • 静态信息
      - HD map(用来保证实时性,如果没有高精地图,仅仅通过感知的信息进行计算,那么很难保证实时性,因为这样会造成很大的计算量)
  • 如何设置出一个合理的轨迹?
    • 路径Path
    • 速度Speed
  • 经典参考书籍
    • Steve Lavelle, Motion Planning Algorithm
    • Principles of Robot Motion:Theory, Algorithm, and Implementations
  • 经典文献(一篇综述):
    • A review of Motion Planning Techniques for Automated Vehicles在这里插入图片描述
      • A. Graph search based planners(基于图搜索的规划方法)
        这种方法把状态空间表达成网格或者lattice的形式,然后在这些状态里面找到一个可达的path。这类方法主要有A* D* Dijkstra algorithm 算法。值得一提的还有state lattice算法,虽然这个图看起来和Apollo里面的lattice不一样,但是这个是爸爸,在这篇文章[1]里面提出了时空lattice,这个也就是后来Apollo算法里面用的
        在这里插入图片描述
      • B. Sampling based planners(基于采样的规划方法)
        这个主要介绍了RRT算法
      • C. Interpolating Curve Planners(插值曲线规划)
        介绍了几种曲线生成的方法,主要有羊角螺旋线(Clothoid Curves)多项式曲线(Polynomial Curves) 贝塞尔曲线(Be ́zier Curves)。并且分别介绍了这几类样条曲线在路径规划的优化过程中作用。

        clothoid curves 曲线的曲率是线性变化的,又因为车辆运行轨迹的曲率和方向盘基本上成正比,也就是说这种线型出来的结果方向盘会非常顺滑。
        贝塞尔曲线 计算简单 速度快。
        多项式拟合也是一个比较好的方法。
        在这里插入图片描述

      • D. Numerical Optimization(数值优化)在这里插入图片描述 

基本的Planning方法

  • 经典的基于环境建模的方法
    • RRT
    • Lattice
  • 现代无人车Planning方法
    • Darpa
    • Lattice in Frenet Frame
    • Spiral polynomial
  • 质点模型
  • 刚体问题
    • BycicleModel
    • XY Heading
    • Collision
  • Planning 限制条件
    • 避免碰撞
    • 边界阈值
  • 连续空间问题
    • 离散化,无人车路径规划问题是求解一个连续空间问题,那么常用的一个方法就是连续空间离散化。
    • 网格化

传统的机器人基础

  • PRM(Probabilistic RoadMAP Planning)
    • 非常常用的方法
    • 连续空间离散化
      • 随即撒点
      • obstacles上的点删除
    • 连接可行点,形成可行空间
    • A* 
  • RRT(Incremental version of PRM)
    • 基于采样,使用增量搜索的方式来进行
    • 找到附近可行点的最优点
      • F(x)最小,cost最小
      • 走过程中也不能有障碍,使cost最小
    • 走的过程中还可能碰到障碍物
    • 所以撒点搜索不能太远,一步一步地移动
    • 形成的路径是一个折现,后面还需要平滑曲线
  • Lattice
    •  改进了RRT的折现问题
    • 直接给出path的平滑曲线
    • 思路:网格化,每个采样个中都是用曲线连接
    • 指数级别的搜索算法(NP-Hard)
  • DP(动态规划

    • 减少搜索空间
    • 复用已有结果
  • Lattice DP
    • 平滑度够吗?
      • 曲率连续
      • 曲率导数不一定连续
  • QP(二次规划
    • 凸优化问题最优化求解
    • 公式表达
    • 性质:在凸优化中的凸空间问题,用QP中有最优解
    • QP如何找到平滑曲线?
      • 一阶导数最优
      • 二阶导数最优
      • 三阶导数最优
    • 其他平滑曲线的方法:贝塞尔曲线,样条插值
  • 刚体模型

    • 前轮转向和heading的关系
      • 车轮是沿着切线方向行驶
      • 前后轮是同一个旋转中心
      • 左右轮的结构相同
    • Bbicycle Model
      • 曲率公式 

        \frac{1}{R}=kappa=\frac{tan(\omega )}{L}

无人车Planning

定义:

  • A点到B点 ,构建一个车辆运动轨迹
  • 输入:HDMap,Localization 和Prediction
  • 输出:可行驶轨迹,有一系列点组成
  • 两个层面:导航层面; 运动轨迹层面

Routing

  • routing的目标是导航一条A到B的全局路径,一条cost最小的路径。
  • 输入:地图(路网信息,交通信息等)、当前位置、目的地(乘客决定)
  • 输出:可行驶道路的连线
  • 搜索:地图数据转化成图网络
    • 节点表示道路
    • 边表示路口

什么情况下cost高?

  • 权重规则:例如左转的权重相较于直行的权重更高,所以Node1到Node4的边权重大,Node1到Node3权重小。
  • 拥堵情况:比如说Node1到Node3的道路很拥堵,那么它的cost就高;Node4的道路更堵,那么Node1到Node4的cost更高。

A*经典算法

  • 最经典的路径查找算法
    • 在rounting中目前A*算法的应用还是非常广泛的。
    • 公式:F(n) = G(n) + H(n)
    • F(n)表示道路的routing的总cost
    • G(n)表示起始点到候选点的cost
    • H(n)表示候选点通过启发函数得到的目标点cost
  • 真实地图中的应用

    •  左转Cost最大

Planning

Motion Planning

  • 导航信息相当于给了粗略的路径信息,而Planning相当于一个高精度,低级别的search。trajectory planning
  • 轨迹点:XY、Time、Velocity

规划的约束条件

  • Collision 碰撞(躲避任何障碍物)
  • Comfortable 舒适 (路径点必须平滑,速度也要平滑)
  • 运动学约束(高速转弯,掉头曲率角度)
  • Illegal合法 (交通法规,人类约定俗成的规则)

Cost Function

  • 真实情况下有多条可行轨迹

  • Cost由许多条件组成 

    • 道路偏离中心线距离

    • 碰撞物靠的太近

    • 速度太大,超速

    • Curvature太大,方向盘太急

  • 针对不同的场景,我们可能有多个不同得cost
    • 高速场景
    • 停车场
    • 不同车辆

Frenet坐标系(车辆坐标系)

  • 一般情况下我们会用笛卡尔坐标系(世界坐标系),但是表征的东西并不全面。
    • xy坐标无法知道我们车开了多远
    • 车有没有偏离车道中心线
    • 不知道道路在哪
  • 更好的坐标系:Frenet
    • 在道路形式方面,我们采用Frenet坐标系,能够更好地表征偏离道路中心线的距离。
    • 【注】Frenet坐标系和Track坐标系的区别
      • L方向不同
      • Track是基于Road级别
      • Frenet是基于Lane级别
    • S表示走了多远
    • L表示距离车道中心线多远

Path 和 Speed解耦

  • Path Planning
    • 生成可行轨迹
    • Cost(选最低的)
      • path平滑性
      • 碰撞安全性
      • 距离道路中心线的偏离性
  • Speed Planning
    • 每个path上选择一系列速度
    • 生成速度轨迹
  • Path和Speed解耦能够让我们将motion planning问题转化为多个凸优化问题。

Path Planning(Littice DP)

  • 步骤

    • 先生成道路网格(GridMap)
    • 每个网格单元中随机采样(撒点)
    • 每个网格选一个点
    • 组成多条候选Path
  • Cost Function
    需要cost最低的path,也就是最优path,cost的设计往往是planning的重点

    • 中心线距离 l*a0
    • 障碍物距离 d*a1
    • 速度变化率 acc * a2
    • 曲率 kappa * a3
    • F(x) = l*a0 + d*a1 + acc * a2 + kappa * a3 + a4
      【思考】为什么线性加权可以在一定程度上解决所有问题呢?

Speed Planning

  • ST图
    •  S-T图表示在path上的速度规划,S表示Path上的纵向距离,T表示运动时间。
    • 斜率越大,表示速度越快。

  • 如何规划ST轨迹
    • 连续空间离散化(grid map)

      • 单元格内速度不变

    • 把障碍物投影进来

      • 将挡住我们Path轨迹的部分画进ST图中

      • 因此必须要有良好的轨迹预测

      • 例如下图中,to-t1时刻障碍物会在我们的Path轨迹中挡住s0,s1部分,(如何理解黄色部分? 相当于t0-t1时刻,s0-s1这块区域是不能有车通行的)

    • 速度曲线不能碰到这个区域
    • 有多个车的情况
    • 凸优化来求解得到最优的速度曲线
      • search
      • 限制条件:速度限制、距离限制(安全距离)、车辆动力学限制(车的加速度、刹车性能)
  • 如何优化
    • 对不平滑的速度线优化
      • QP优化速度、加速度、加速度导数
    • 我们的这个方法很大程度依赖于连续空间离散化
    • 网格、单元格方法
    • 由于折线并不平滑,我们需要将不平滑的折线优化成平滑的线性曲线。

轨迹规划

  • 实例:遇到一辆速度很慢的车我们如何超车

  • 生成很多轨迹(撒点采样)

  • 用cost function对其进行评估,选择cost最小的一条

  • 生成一个ST图来表述速度规划

  • 生成多条速度曲线

  • 使用优化工具对多条速度采样进行最优化求解

    • 让整个路线和速度曲线变得平滑

Lattice Planning(网格规划)

  • SL轨迹和ST轨迹
  • Lattice将两个图合并处理,同事进行Path和Speed的采样
  • 示例:如果我们遇到一个cut in场景
    • 先对整个候选轨迹进行采样
    • 设计一个合理的Cost
    • 选择一个Cost最小的轨迹
    • 条件检查和碰撞检查
    • 检查失败则返回继续找Cost次优候选项
    • 成功的输出结果
  • Littice因为其采样计算量大,为了优化其采样效果,需要先进行场景化以简化计算量

 

 


课程:B站【全】无人驾驶系列知识入门到提高

  • 4
    点赞
  • 18
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
自动驾驶感知算法综述是关于自动驾驶系统中用于感知环境的算法的一份综合调查报告。其中涉及的算法包括图像处理、道路检测、目标检测和3D物体检测等。引用中的《2.3D Object Detection from Images for Autonomous Driving: A Survey》是一份关于从图像中进行三维物体检测的综述,主要探讨了目前用于自动驾驶的图像处理算法和相关技术。引用的《3.End-to-End Deep Learning of Lane Detection and Path Prediction for Real-Time Autonomous Driving》是一篇关于利用深度学习进行车道检测和路径预测的文章,该方法可以实时地进行自动驾驶车辆的车道线检测和路径规划。引用的《3.A Survey of Robust LiDAR-based 3D Object Detection Methods for autonomous driving》则是对基于激光雷达的三维物体检测方法进行的一项调查研究,该研究主要关注激光雷达在自动驾驶中的应用,并介绍了一些具有鲁棒性的方法用于检测和识别环境中的三维物体。综合这三个引用,我们可以获得关于自动驾驶感知算法的全面综述。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* *2* *3* [自动驾驶感知/定位/部署全栈算法综述汇总](https://blog.csdn.net/CV_Autobot/article/details/126113336)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_1"}}] [.reference_item style="max-width: 100%"] [ .reference_list ]

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值