自动驾驶小白说 🏎️5.5 基于PiecewiseJerk的路径优化方法
type
status
slug
date
summary
tags
category
password
icon
知我者，谓我心忧. 不知我者，谓我何求。— 佚名 黍离
🏰代码及环境配置：请参考0.2 环境配置和代码运行 | 动手学运动规划!
PiecewiseJerkOptimizer是Apollo中planning模块生成Path/Speed曲线的优化方法. 基于Frenet坐标系, 生成平滑, 安全的目标曲线. 本节将讲解基于PiecewiseJerk的路径优化方法.
5.5.1 问题定义
SL坐标系下, 给定路径总长, 我们将路径均匀的离散成个点(上图中的蓝点), 其中. 我们定义个优化变量, 每个点有3个优化变量:
- :第个点的横向坐标,
- :第个点的横向偏差的1阶导数, 代表横向的速度
- :第个点的横向偏差的2阶导数, 代表横向的加速度
5.5.2 目标函数
目标函数定义如下:
- :代表第i个点的引导点的横向坐标
- :代表偏离引导线的惩罚
- :代表横向速度和加速度的惩罚
- :横向的3阶导数(jerk), 只计算粗糙值:二阶导数插值得到.
- :常数, 不同cost的权重
5.5.3 约束条件
5.5.3.1 连接约束
我们决策变量只有, 那么如何保证点与点之间的连接是平滑的呢?
可以利用简单的积分来约束 和 的关系:
本质上, 上面的积分就是泰勒展开式的过程:
泰勒展开式是一种重要的函数近似方法, 精度取决于泰勒展开的最高阶数. 如果我们用泰勒展开式近似点和点之间的曲线, 舍弃掉泰勒展开式的误差项, 可以得到3阶泰勒展开:
5.5.3.2 边界约束
边界约束较为简单, 主要是
5.5.4 完整问题
最终, 我们得到了完整的PiecewiseJerkPath优化问题, 显然这是一个QP优化问题.
参考链接:
Loading...
