A Real-Time Motion Planner with Trajectory Optimization forAutonomous Vehicles
参考资料来源:《A Real-Time Motion Planner with Trajectory Optimization forAutonomous Vehicles》一文
注:本文章在于《Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform》一文的解读,大家在学习过程中请关注以下几个问题:
- 轨迹如何产生
- cost函数设计过程
- iterative trajectory optimization algorithm的优势
1 基础知识
1.1 多项式螺旋线
多项式螺旋线如上图所示。它代表了一类曲率可以用弧长(对应轨迹的s方向)的多项式函数来表示的曲线簇。
使用三阶或者五阶多项式螺旋线,其曲率
κ
\kappa
κ和轨迹弧长
s
s
s的关系
κ
(
s
)
\kappa(s)
κ(s)如下。
三阶:
κ
(
s
)
=
κ
0
+
κ
1
s
+
κ
2
s
2
+
κ
3
s
3
\kappa(s)=\kappa_{0}+\kappa_{1}s+\kappa_{2}s^2+\kappa_{3}s^3
κ(s)=κ0+κ1s+κ2s2+κ3s3
五阶:
κ
(
s
)
=
κ
0
+
κ
1
s
+
κ
2
s
2
+
κ
3
s
3
+
κ
4
s
4
+
κ
5
s
5
\kappa(s)=\kappa_{0}+\kappa_{1}s+\kappa_{2}s^2+\kappa_{3}s^3+\kappa_{4}s^4+\kappa_{5}s^5
κ(s)=κ0+κ1s+κ2s2+κ3s3+κ4s4+κ5s5
基于这种使用三阶(五阶)螺旋线连接的轨迹,其参数可以通过梯度下降的方法快速有效地进行搜索
2 轨迹产生
该文章依旧采用自然坐标系下解耦规划+多项式螺旋线分别规划path和speed,从而产生多条候选轨迹,后续在采用cost函数进行选择。
2.1 path规划
采用如上图的path规划手段。蓝色车代表当前车的状态,灰色车代表采样点,红色轨迹代表四次多项式螺旋线,绿色代表三次多项式螺旋线。连接方式:1.当前车与endpoints进行连接(红色);2.不同endpoints间进行连接(绿色)。
path规划中,采样+多项式连接属于老套路,在该文章中比较新颖的是
- 计算四次多项式螺旋线是耗时的,所以只有一部分通过四次多项式螺旋线连接,其余使用三次多项式螺旋线连接。
- 三次多项式螺旋线虽然计算快,单存在如下的缺点,该方法的在每个周期内的曲率变化是连续的,但在cycles间的junction point不连续
- 为了解决cycles间的junction point不连续问题,引入一个新的约束:即当前车辆姿态点处曲率的一阶导数。那么此时就应当是四次多项式了
注:关于这个不同cycles之间的拼接问题,这个先mark住,回头学习完Apollo代码回来再补充,这个论文结论可以先记住。
2.2 speed规划
采用如上图speed规划手段,绿色点代表采样点,蓝色点代表当前状态点,红色为无效曲线(超出了加速度限制),灰色代表有效曲线,关于这个speed规划有如下几个点需要关注
- 一般的speed规划都是s-t规划,而这个是speed-s规划(这里使用的是四次多项式连接,纵坐标是速度,横坐标是弧长)。其目的是为了与path规划相对应(横坐标都是弧长)
3 cost函数设计
设计如上图的cost函数,各个指标如下,该文章将其分为了静态和动态指标
该文章其余指标大家应该理解都没问题,比较新颖的是有关障碍物的指标
c
o
b
s
s
c_{obs}^s
cobss和
c
o
b
s
d
c_{obs}^d
cobsd,下面详细说明一下障碍物指标的计算过程
使用多个circles代表无人车的结构,这样与传统的包络盒相比,这样的碰撞检测精度更高,因此采用如下公式计算
c
o
b
s
s
c_{obs}^s
cobss和
c
o
b
s
d
c_{obs}^d
cobsd中的
s
i
s_{i}
si和
d
i
d_{i}
di
当无人车和障碍物距离小于某一阈值时,直接给障碍物cost赋值无穷大,否则使用(7)和(8)公式进行计算, g j g_{j} gj是障碍物和覆盖汽车的第 j t h j^th jth, λ o b s s \lambda_{obs}^s λobss和 λ o b s d \lambda_{obs}^d λobsd表示常数。
4 iterative trajectory optimization algorithm
该文章的planning框架如上,path和speed解耦规划,随后合并轨迹并使用cost函数选择最佳的轨迹(博客2,3所介绍的过程)。紧接着需要对生成的轨迹优化,下面主要说一下轨迹优化的东西。
- 该文章使用的是速度和路径分别优化并非同时优化,虽然同时优化解的质量更高,但是计算效率很低(维数变大,耗时很大)。而分别优化,虽然解次优,但是计算效率很高,同时得到的解也是可以接受的。(EM planner也是采用如上的策略)
- Compared with the planner without optimization, this framework can reduce the planning time by 52% and improve the trajectory quality.(摘要结论,时间计算大幅度减少)