2.参考线平滑算法解析及实现

星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

中间这一块是planning的职责

下面来梳理一些apollo参考线的重要的数据结构:

1.ReferenceLine数据结构

其存储了参考线的所有的信息:

SpeedLimit表示起点、重点的位置，以及该路径上的速度限制

而SpeedLimit又是一个数组，存放在speed_limitb_变量里，表示某一段路径有多段的速度限制

ReferencePoint也是一个数组，表示参考线中具体的离散点

Map_path表示地图中的参考线，其存放的参考点和参考线是一致的，为一一对应的关系。

接下来学习ReferencePoint

最上面的父类Vec2d，包含了参考点的二维坐标，MapPathPoint继承自Vec2d，MapPathPoint增加了两个重要的数据结构，一个点的朝向，另一个是地图上的点对应道路上的点(对应Map_path)，因为LaneWayPoint是一个数组，地图上的车道有可能有重合的地方，所以地图上的一个点可能同属于多个车道，所以就会有多个LaneWayPoint。ReferencePoint由MapPathPoint继承得来，其描述了参考线当中的点，除了其父类中的信息之外，又增加了曲线的曲率和曲率的导数。

以下是处理参考线的流程：

在planning模块主要有两个任务(两个线程)，一个为参考线提供的线程，另一个是规划线程，其根据参考线提供线程输出的参考线进行相应的路径规划。提供的线程在直行的情况下只会生成一条参考线，如果有换道的情况就会有多条参考线以供换道的选择。对于参考线的提供者(ReferenceLineProvider),其会根据车辆的位置及PNC的地图来生成参考线，其对参考线的处理主要分成了两部分:一部分是参考线的生成，另一部分是参考线的平滑。

参考线的平滑主要支持三种方式：离散点的平滑，螺旋线的平滑，样条曲线的平滑。apollo中主要采用的是离散点的平滑。

下面讲解参考线的生成：

其主要在CreateReferenceLine这个函数中进行，首先其根据UpdateVehicleState(获取车辆状态)，然后获取Routing结果，根据routing的结果的路线里去生成RouteSegments(路径片段)，然后在对参考线进行平滑和分割，如果没有新的routing进来就可以用上一个周期处理的参考线(延用，以减少计算量)。

接下来介绍参考线平滑算法：

参考线平滑算法一共有三种：

一种是离散点的平滑，另一种是螺旋线的平滑，最后是样条曲线的平滑，apollo中主要使用离散点的平滑，其包含两种平滑的方法：costheta平滑算法、FemPos平滑算法。

可以通过配置参数选择要采用的平滑算法：

参考线的平滑函数在ReferenceProvider这个类当中(SmoothReferenceLine)

可以看到，该函数的输入是raw_reference_line（猜测使用了引用应该就是输入），输出是reference_line(猜测使用了指针就是输出)，raw_reference_line经过了设置中间点和平滑，最终输出了平滑后的结果smoothed_reference_line。

现在来讲解如何设置中间点：

第一步:根据原始的参考线来选取中间点

这一步就是根据referenceline的S值在纵向的投影进行一个均匀的采样，采样的间隔大概是0.25米，采样完毕后就能得到一系列的AnchorPoint，每个AnchorPoint包含了一个path_point和一个横纵向的裕度(采样点周围可以平移的空间)。强约束表示必须必须严格遵守横纵向的裕度，所以在平滑过程中只有首尾两个点是强约束，中间的点都不是强约束。

参考线平滑比较重要的默认配置主要为:纵向边界(2.0)，横向边界(最大值0.5，最小值0.1)

参考线平滑中主体的函数(smooth函数)，是通过原始参考线上的离散点有限的平移对原始的参考线进行平滑。

可以看到这个类当中还有两个不同的平滑算法，分别是刚才提到的costheta和FemPos平滑算法，这两种通过在配置文件中的配置去选择。

现在来看FemPos平滑算法：

从其输入的参数来看，主要是AnchorPoint中的x，y和横纵向的裕度，输出是平滑后的point。

首先对横向和纵向的裕度进行一定的收缩,收缩后对二次化的问题进行求解，现在来看求解函数：

可以看到其有三种求解方式，分别是利用不同的求解器实现了求解的方法，如果考虑了曲率的约束，该优化问题就是非线性的，就可以用Ipopt非线性求解器进行求解，也可以将曲率约束进行线性化之后用Osqp进行求解。

如果不考虑曲率约束的话，就可以直接用Osqp进行求解该二次优化的问题。

目前apollo中默认的是不考虑曲率约束，这样求解会快一些。

下面是默认的平滑的求解过程，其默认的是基于离散点的平滑，把参考线平滑构造成了一个二次优化的问题，并且使用Osqp求解器进行求解

二次优化问题，主要的优化变量就是Anchor Point的离散点，优化目标是平滑度、参考线的长度、相对原始路线的偏移量，优化函数cost有三部分组成:平滑度、长度、两个点偏离误差。Cost length通过计算两点的长度来的草，偏移误差通过各个点偏移量的求和得到。

对于cost smooth，其有两种计算方式：FemPos和CosTheta

FemPosSmooth的计算方式：

假设参考线上有相邻的三个点，这三个点组成两个向量(蓝色箭头)，其结果相当于这两个向量和向量的模的平方。若P0，P1，P2在同一条线上，P1，Ps就会比较小，得到的值也就比较小，相应的平滑度的代价也就比较小。

如果这三个点组成的两个向量的夹角越小，那么和向量的值就越大，平滑度的代价也就越大。所以最终得到的值和平滑度是相关的。

CosTheta的计算方式:

可以理解为相邻两个向量夹角的cos值，这个值越大表示夹角越小，P0，P1，P2也就越接近直线，此时参考线也就越平滑，所以平滑度的代价可以用costheta的负值来表示，这种计算方式需要采用非线性优化器。

接下来讲解二次优化问题的约束条件：

我们希望平滑度的参考线距离原始路线的偏移量不能太大，虽然在代价函数中已经有了偏移量的惩罚项，但是惩罚项只是一个软约束，不能保证偏离的具体数值能够确定的限制在这个范围内，所以这里添加了关于偏移量的硬约束，偏移的范围就是设置的横纵向裕度构成的box(黄色方块)范围内，box构成的约束方程就是对x和y分别进行限制。

第二个是曲率的约束，曲率的约束在apollo中是通过近似的假设，找到相邻的三个点，其三个点的外接圆，外接圆之间的离散点是均匀采样的，两两之间的距离是相等的(第一个假设)，第二个假设，相邻线段之间的夹角很小，第三个假设弧长与弦长接近。经过这三个假设进行推导，换算出来我们对曲率的约束，根据曲率的方程，得到如下的约束方程：

以上就是参考线的平滑算法计算原理。