自动驾驶之轨迹规划5——Apollo规划中的离散点曲线平滑数学原理

1. 前言

本文详细讲解Apollo的曲线平滑的数学原理，原文在《开发者说丨离散点曲线平滑原理》，参考Apollo代码在此，以下为整理内容。

2. 离散点曲线平滑的数学原理

如下图所示，P0P_0P0，P1P_1P1，P2P_2P2，P3P_3P3，…，PnP_nPn，一共n+1n+1n+1个离散点组成原始参考线。本节介绍一种方法通过对原始参考线上离散点的有限偏移对原始参考线进行平滑。

2.1. 优化变量

优化变量：离散点坐标(xi,yi)(x_i,y_i)(xi,yi)

2.2. 优化目标

优化目标：平滑度、长度、相对原始点偏移量

2.3. 目标函数

目标函数：cost=cost1+cost2+cost3cost=cost_1+cost_2+cost_3cost=cost1+cost2+cost3。其中cost1cost_1cost1为平滑度代价，cost2cost_2cost2为长度代价，cost3cost_3cost3为相对原始点偏移量代价。

cost1cost_1cost1物理意义：如下图中红色向量P1P3⃗\vec {P_1P_3}P1P3的模，它可以理解为：向量P1P0⃗\vec {P_1P_0}P1P0和向量P1P2⃗\vec {P_1P_2}P1P2相加结果（P1P3⃗\vec {P_1P_3}P1P3）的模平方。如果这三个点在一条直线上，那么这个值最小；三个点组成的两个线段的夹角θ\thetaθ越大，即曲线越趋于平直，也因此越平滑。

关于平滑度的表示除了上面提到的cost1cost_1cost1形式之外，还存在其他形式去表示，如下图所示，连续3点P0P_0P0，P1P_1P1，P2P_2P2，其中向量P0P1⃗=(x1−x0,y1−y0)\vec {P_0P_1}=(x_1-x_0,y_1-y_0)P0P1=(x1−x0,y1−y0)和P1P2⃗=(x2−x1,y2−y1)\vec {P_1P_2}=(x_2-x_1,y_2-y_1)P1P2=(x2−x1,y2−y1)之间的夹角θ\thetaθ。

cosθcos\thetacosθ值越大，θ\thetaθ越小，P0P_0P0，P1P_1P1，P2P_2P2越接近直线，曲线越平滑。

因此，cost1cost_1cost1可以表示为：

这两种代价形式也是开源Apollo代码中discretized_points_smoothing中cos_theta_ipopt_interface、fem_pos_deviation_ipopt_interface、fem_pos_deviation_osqp_interface、fem_pos_deviation_sqp_osqp_interface方法中对应用到的基本原理。

cost2cost_2cost2物理意义：代表离散点之间距离平方之和。

cost3cost_3cost3物理意义：代表优化后的离散点相对原始参考点偏移距离平方之和。

2.4. 约束

约束条件包含三个：位置约束，曲率约束，松弛变量。这里我们希望原始参考点的偏移量不应该太大，因此偏移量构成了第一个约束，如图中红色框所示把散点偏移量（X方向Y方向）给限制在一定的范围内。

位置约束：

曲率约束：
除此之外，我们在路径规划的时候通常需要考虑转弯半径的约束，也就是路径需要满足一定的曲率约束。关于曲率的计算我们的主要思想在这里采取是三点构成一个圆的思想去求解半径，具体原理如下图所示：

如图所示，假设P0P_0P0，P1P_1P1，P2P_2P2三点处在同一个圆上，当θ\thetaθ较小时，向量P1P0⃗\vec {P_1P_0}P1P0和向量P1P2⃗\vec {P_1P_2}P1P2的模近似等于弧长，因此有：

根据O−P0−P1O-P_0-P_1O−P0−P1等腰三角形几何关系有：

由于∣P1P0⃗∣=∣P1P2⃗∣|\vec {P_1P_0}|=|\vec {P_1P_2}|∣P1P0∣=∣P1P2∣，所以CCC是的P1P3P_1P_3P1P3的中点，由此得：

根据C−P0−P1C-P_0-P_1C−P0−P1直角三角形几何关系有：

把P1CP_1CP1C代入P1P3⃗\vec {P_1P_3}P1P3得：

把θ2\theta_2θ2代入得：

把θ1\theta_1θ1代入得：

我们希望路径曲率约束满足车辆运动学最小转弯半径的约束，也就是满足P1P3⃗≤Rmin\vec {P_1P_3}≤R_{min}P1P3≤Rmin即满足：

根据上式其中n-2个点中间点满足：

其中：△s△ s△s是散点之间平均长度，curcstrcur_cstrcurcstr是最大去率约束。
为了保证有解以及求解更快的收敛这里引入了大于0的松弛变量stackistack_istacki。因此：

2.5. 总结

目标函数

约束条件

附录_曲率约束的线性化

至此，作为一个优化问题，模型基本建立，通过观察我们可以知道目标函数为二次方程，但是很遗憾约束条件里面关于曲率约束是非线性的。此问题除了直接使用IPOPT求解之外，经过思考我们可以通过线性化方法把曲率约束转化成线性约束之后就可以把问题转化为二次优化问题使用OSQP加速求解。

假设：

泰勒展开：

线性化只保留一阶项：

在F(x)F(x)F(x)原始参考点xrefx_{ref}xref处的值为：

在F(x)F(x)F(x)原始参考点xrefx_{ref}xref处的导数为：
从曲率约束方程我们可以看到包含6个变量：xi−1x_{i-1}xi−1，xix_{i}xi，xi+1x_{i+1}xi+1，yi−1y_{i-1}yi−1，yiy_{i}yi，yi+1y_{i+1}yi+1，对方程求导得：

其中包含6个变量：xi−1x_{i-1}xi−1，xix_{i}xi，xi+1x_{i+1}xi+1，yi−1y_{i-1}yi−1，yiy_{i}yi，yi+1y_{i+1}yi+1：

因此：

带入曲率约束方程得：

曲率线性化约束化简：