Clothoid回旋曲线在APA路径优化中的工程应用实例及其C++源码分析与下载

回旋曲线的定义

回旋曲线是缓和曲线的一种，而缓和曲线的线型多种多样，如回旋线、三次抛物线、七次四项式型、半波正弦型、一波正弦型、双纽线、多心复曲线……，公路中推荐使用的是回旋曲线，因为回旋曲线的曲率变化率是定值，故在遇到直线驶入圆弧或者圆弧驶入直线的场景下，方向盘转角匀速变化，符合人类驾驶习惯且行驶较为顺滑。
回旋曲线也叫羊角螺线（clothoid），本质特点是：曲率与长度成线性关系。

                                                              dk/ds = c(常数)
                                                  其中，k是曲率，s是回旋曲线的长度，c是曲率的变化率。

回旋曲线的坐标计算公式（重点，代码实现的公式来源）

最核心的公式的前提假设：

注意这里的前提假设就是原点位于曲率为0的地方，即x0=0, y0=0, s0=0, k0=0，并且这是一个无穷级数。但是，在平时的应用中，不可能每次都从曲率为0的地方开始，比如要连接两段圆弧曲线的时候，曲率就是从一个非零曲率到另一个非零曲率（中间曲率不一定过渡到0）。这时候，上面这个表达式就有限制。然而，如果直角坐标系原点不建立在曲率为零（简称为非标准坐标系）的地方，直接去推导直角坐标公式对于非数学专业的人来说估计很难，涉及复变函数积分。

满足上述前提假设的前提下，回旋曲线上任意一点坐标以及切线角度的计算公式如下：

其中， =，即回旋曲线上任一点(xk,yk)处切线的角度。

工程应用中的处理方式

在实际工程应用中，进行2处的简化处理：
1，级数和累加次数N一般取50,100次，根据精度需要进行取值近似。
2，将Sk进行离散化处理
通过上述简化，即可求得回旋曲线的全部轨迹点。

实例：已知两点坐标，曲率以及角度，求对应的回旋曲线。（泊车场景，用于平滑直线和圆弧，曲线的曲率连续）

一，根据约束条件求解回旋曲线

起始和结束两端点的约束为：
1，坐标：起点为(x1,y1) = (0,0)，终点为(x2,y2)=(10,5)，
2，曲率：起点的曲率为0，终点的曲率为R = 4.0，
3，方向角度：起点为0（0度方向），终点为pi/2（90度方向）
根据上述约束条件2和3，结合上述任一点的切线角度公式（11），可确定唯一的回旋曲线长度S（S = 2Rpi/2 ）。回旋曲线的S一旦确定，根据起始和终点的曲率，即可算出C，则回旋曲线被唯一确定，如下图所示

二，产生的问题及工程解决办法：

产生的问题：
由上述约束条件2和3可唯一地确定回旋曲线，但如上图所示，回旋曲线的终点坐标不是(x2,y2)=(10,5)，而是（9.7,5.2），即满足约束条件2和3，且起点坐标一定时，无法再约束终点的坐标，终点的坐标根据上述回旋曲线的坐标公式（9）、（10）计算得到。但泊车场景下必须约束终点坐标，即终点坐标是固定唯一确定下来的，故直接用上述的回旋曲线无法满足要求，必须进行近似优化才能在工程中使用。
解决办法：（重点）
将上述求解出的回旋曲线的坐标横纵向进行缩放，保证起点和终点的坐标是预期设定的起点和终点坐标，即起点为(x1,y1) = (0,0)，终点为(x2,y2)=(10,5)。

源码下载链接：https://download.csdn.net/download/ChenGuiGan/81749994?spm=1001.2014.3001.5503