Apollo 计算几何算法

Apollo 计算几何算法

1. 介绍

Planning 模块中, 路径和速度曲线都被抽象成 Polyline, 障碍物被抽象成 Polygon. 在碰撞检测、投影计算距离、平滑曲线等方面都大量运用到了几何算法. 在本文中, 将介绍 Apollo 所用到的计算几何相关的基础库, 包括LineSegment2d Box2dAABox2d Polygon2d, 样条曲线, 螺旋线等等.

附赠自动驾驶最全的学习资料和量产经验以及100T的资源分享：链接

2. 几何算法

2.1. 线段

线段的相关类, 文件路径: modules/common/math/line_segment2d.h

LineSegment2d: 大部分成员函数比较简单, 可自行查阅相关代码.

1. 成员变量: 两点确定一条直线. start_表示线段的起点, end_表示线段的终点.

2. double DistanceTo(const Vec2d &point, Vec2d *const nearest_pt) const;给定一个点, 计算到线段的最短距离, 同时返回最近的点(过给定点的垂线与原线段的交点, 或者线段的端点).

2.2. 包围盒

在二维平面上, Box 特指矩形包围盒. 在 Planning 模块, 经常将自车和障碍物抽象成一个矩形框, 从而简化计算.

Box2d: 普通矩形包围盒. 文件路径: modules/common/math/box2d.h

AABox2d: axes-aligned bounding boxes, 其矩形边都是沿着 XY 坐标轴的. 文件路径: modules/common/math/aabox2d.h

2.3. 多边形

3. 数值分析

3.1. 样条曲线

在数值分析中, 样条(Spline)是一种特殊的函数, 由多项式分段定义.[1] 在一些情况下, 比如直角弯或者 U 形掉头时, 使用一个多项式无法精确描绘轨迹的形状, 所以需要进行分段连接. 多项式的连接通过连接点一阶导二阶导相等来保证光滑性.

相关文件路径: modules/planning/math/smoothing_spline/

相关类的UML关系如图 3.1 所示:

图 3.1 Spline UML

以Spline1d为例:

1. 

3.2. 螺旋线

3.2.1. 螺旋线的构造

3.2.3. 代码解析

相关文件路径: modules/planning/math/curve1d/

相关类的UML关系如图 3.2 所示:

图 3.2 Spiral UML

QuinticSpiralPath:

1. 成员函数: coef_deriv_, 用于求偏导.

2. 构造函数: 利用公式 3.3 计算coef_deriv_

QuinticSpiralPathWithDerivation:

1.