[Python|Clothoid]Clothoid曲线（回旋曲线）与直角坐标求解的python实现

前言

Clothoid曲线是一种曲率随着弧长线性变化的曲线。在直线段与其它曲线轨迹过渡的阶段采用这类曲线可以起到平滑曲率的作用。在车辆行驶轨迹设计中，如果使用这类曲线，可以减少曲率突变对转向控制的要求，降低控制难度，改善控制效果。这篇文章主要用于工程应用，而非理论推导。

直角坐标系表达

Clothoid曲线的数学表达式如下，其中a为常数，l为曲线长度， k为曲率；

如果将曲线上k=0的点作为原定建立直角坐标系（简称为“标准坐标系”），a>0时候的图像为：

进一步，求解微分方程可以得到clothoid曲线上所有点的坐标：

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

其中，角度alpha为标准坐标系clothoid曲线上点的航向角。曲率原始定义为曲线上某点切线方向随弧长的变化率

结合上面Clothoid曲线曲率与弧长的关系，很容易计算出标准坐标系中任意一点的航向角。知道航线角以后，就知道非标准坐标系xoy和标准坐标系XOY的坐标转换矩阵：

则两个坐标系坐标的转换关系为：

Xo和Yo为非标准坐标系xoy原点o在XOY坐标系中的坐标，则反过来，XOY中的红色曲线段的坐标映射到xoy坐标系中为：

按照上述思路，要想得到一条非标准坐标系中的cothoid曲线，只需要给定一个初始点曲率，一个终止点曲率和该曲线的长度即可。首先，按照标准坐标系计算处该段曲线上所有点的坐标，然后在变换得到非标准坐标系下的坐标。

但在实际应用中，发现上述思路还存在一定的约束，即给定初始曲率和终止曲率后，曲线的长度并不是给定任意值就可以的：
1、首先， 上面计算标准坐标系下曲线的点坐标采用的是无穷计算展开进行计算，在实际的计算机上面是不可能实现的，因此就必须近似求解，也就是对上面的迭代次数n选取，n太大计算太慢，太小精度不够
2、观察这个无穷技术，其中最高阶次项为弧长l的4n+3次方，对于python来说，最大的浮点数阶次为300左右，因此有：

3、另外，将k=al带入级数当中的y坐标得到：

进一步化简可得：

可以发现，对于计算机计算而言，当n给定以后，要想让上述的级数在有限的迭代次数内收敛较好的一个必要不充分条件（能收敛一定要小于1，但小于1不一定收敛）：

进一步，将上述结论强化，拆分分别得到两个约束条件：

则：


这里k0和kt分别为截取的曲线起点和终点的曲率。注意，上面是通过坐标y推出的约束条件, 用x坐标推导得到：：


因为2n阶乘的(1/2n)次方为递增函数，最终弧长的约束为：


所以，截取的曲线的允许的最大长度为：

python代码实现

计算非标准坐标系下的坐标点

def clothoid(curveStart, curveEnd, curveLength, distance):#curveLength为截取曲线的长度，distance为相对于截取段起始点s0处的距离，给定一个distance就返回该distance处的XY坐标
    A = (curveEnd - curveStart) / curveLength
    s0 = curveStart / A
    X0, Y0 = clothoidcooridnate(A, s0)
    heading0 = A / 2 * s0 ** 2
    transferMatrix = np.linalg.inv(transfercoordinate(heading0))
    X, Y = clothoidcooridnate(A, distance + s0)
    local = np.dot(transferMatrix, np.array([[X], [Y]]) - np.array([[X0], [Y0]]))
    x = local[0, 0]
    y = local[1, 0]
    heading = A / 2 * distance ** 2 + curveStart * distance
    return x, y, heading, math.cos(heading), math.sin(heading)

计算标准坐标系下的坐标点

def clothoidcooridnate(A: float, distance: float):
    x = 0.0
    y = 0.0
    for n in range(30):
        x += math.pow(-1, n) * math.pow(A, 2 * n) * math.pow(distance, 4 * n + 1) / \
             (math.factorial(2 * n) * (4 * n + 1) * math.pow(2, 2 * n))
        y += math.pow(-1, n) * math.pow(A, 2 * n + 1) * math.pow(distance, 4 * n + 3) / \
             (math.factorial(2 * n + 1) * (4 * n + 3) * math.pow(2, 2 * n + 1))
    return x, y

坐标转换矩阵

def transfercoordinate(heading: float = 0.0) -> np.ndarray:
    return np.array([[math.cos(heading), -math.sin(heading)],
                     [math.sin(heading), math.cos(heading)]])

主程序

if __name__=="__main__":
    curStart = 0.4#截取曲线起点曲率
    curEnd = 0.5##截取曲线终点曲率
    n = 30#设置的迭代步长30
    l2 = 4*n + 1
    l3= math.pow(10, 300/(4*n+1))
    lk0 = min(2* math.pow(math.factorial(2*n), 1 / (2*n))/abs(curStart + 1e-5), l2, l3)
    lkt = min(2*math.pow(math.factorial(2*n), 1 / (2*n))/abs(curEnd + 1e-5), l2, l3)
    '''允许设置的曲线长度'''
    allowsCurveLength = abs(lk0+lkt) if curEnd*curStart<=0 else abs(lkt-lk0)
    print("The allowed curve length is {}".format(allowsCurveLength))
    '''实际设置的曲线长度'''
    curveLength = 15
    print("The given curve length is {}".format(curveLength))

    x = []
    y = []
    heading = []
    distance = np.linspace(0, curveLength, 1000)
    for i in distance:
        X, Y, Heading, W, D = clothoid(curStart, curEnd, curveLength, i)
        x.append(X)
        y.append(Y)
        heading.append(Heading)
    plt.plot(x, y)
    plt.show()

可以看到，当设置长度小于允许的最大长度时候，曲线是可以正常表达的。反正，当设置长度大于允许长的时候：

curveLength = 25

但是，设置曲线长度为20时：

curveLength = 20

虽然设置的长度小于允许的长度，但曲线还是发散了。这是因为，我们的推导结果是必要不充分条件，所以并不是小于允许值的所有长度都可以，这里在实际应用的时候应该注意，可以设置一个安全边界来避免。

将初始点曲率设置为-0.4：

curStart = -0.4
curveLength = 160

同样，将长度设置为220，曲线发散

    curveLength = 220

总结

本文介绍了一种通过计算机在直角坐标系中绘制任意曲率的Clothoid曲线的方法。通过给定期望曲线的初始曲率、终止曲率、迭代步长、计算机能表示的最大浮点数位数，可以计算得到一个允许的曲线长度；当给定的期望曲线的长度大于该允许长度时候，该曲线将会发散，无法生成；当给定长度小于允许的曲线长度的时候，此时不一定能生成合理的曲线，但这个允许长度给出了一个参考，可以采用二分法来尝试。

追加：在后续使用中发现，l2，l3在n取值30的时候，通常都有l2和l3都小于lk，导致曲线允许的长度为0。这时候，可以去掉l2和l3，直接以lk作为判断曲线长度的条件。

l2 = 4*n + 1
l3= math.pow(10, 300/(4*n+1))
lk0 = min(2* math.pow(math.factorial(2*n), 1 / (2*n))/abs(curStart + 1e-5), l2, l3)
lkt = min(2*math.pow(math.factorial(2*n), 1 / (2*n))/abs(curEnd + 1e-5), l2, l3)

应用

Clothoid曲线在ADAS测试方面的应用以及Matlab源代码