绘制clothoid曲线

clothoid介绍

  clothoid曲线是一种曲率半径与长度成线性关系的曲线，由于其曲率平滑过渡，在路径规划中有所应用。在直角坐标系中，clothoid曲线的微分方程如式（1）所示。
$$dk=adl\text{\hspace{1em}(1)}$$

  式（1）中，$k$表示曲率，$l$表示曲线长度，$a$表示系数，可以明显地看到clothoid曲线的曲率与曲线长度成线性关系。

绘制clothoid曲线

积分近似

  通过对式（1）进行处理得到clothoid曲线在直角坐标系的参数方程（具体推导过程略），如式（2）所示。
$$\begin{array}{l}x=\frac{1}{a}{\int }_0^{\tau }cos(t^2)dt\\ y=\frac{1}{a}{\int }_0^{\tau }sin(t^2)dt\end{array}\text{\hspace{1em}(2)}$$

  方程（2）虽然形式简单，但是一个超越方程，当前没有精确解。通过使用python定积分模块近似求解方程（2），并绘制图片如下。计算时间有点长，耐心等待即可。

# -*- coding: utf-8 -*-
import numpy
import matplotlib.pyplot as plt
from sympy import *
import math

fig = plt.figure()
ax1 = fig.add_subplot(111)
#ax1.set_title('Fig')
plt.xlabel('x(m)')
plt.ylabel('y(m)')

if __name__ == '__main__':
    pi = 3.14159265
    a = 0.5
    x_list = [0]
    y_list = [0]
    t = symbols('t')
    for i in range(500):
        theta = (i + 1) * 2 * 3.14159265 / 360
        x = a * integrate(cos(t * t), (t, 0, theta))
        y = a * integrate(sin(t * t), (t, 0, theta))
        x_list.append(x)
        y_list.append(y)
        print(i, x, y)
    plt.plot(x_list, y_list, '.')
    plt.show()

以直代曲

  使用python求解虽然可以获得非常高的求解精度，但计算速度慢，便考虑使用更简便的方式求解，这里用“以直代曲”的方式绘制clothoid曲线。迭代公式如式（3）所示。
$$\begin{array}{l}{r}_n=2/(2k_{n-1}+adl)\\ k_n=k_{n-1}+adl\\ {\theta }_n={\theta }_{n-1}+dl/r_n\\ x_n=x_{n-1}+dlcos({\theta }_n)\\ y_n=y_{n-1}+dlsin({\theta }_n)\end{array}\text{\hspace{1em}(3)}$$

# -*- coding: utf-8 -*-
import numpy
import matplotlib.pyplot as plt
from sympy import *
import math

fig = plt.figure()
ax1 = fig.add_subplot(111)
#ax1.set_title('Fig')
plt.xlabel('x(m)')
plt.ylabel('y(m)')

if __name__ == '__main__':
    dl = 0.01
    a = 2
    k0 = 0
    x = 0
    y = 0
    theta= 0
    x_list = [0]
    y_list = [0]
    for i in range(500):
        r = 2 / (2 * k0 + a * dl)
        k0 += a * dl
        theta += dl / r
        x += dl * cos(theta)
        y += dl * sin(theta)
        x_list.append(x)
        y_list.append(y)
    dl = 0.01
    a = 1
    k0 = 0
    x = 0
    y = 0
    theta= 0
    x_list2 = [0]
    y_list2 = [0]
    for i in range(500):
        r = 2 / (2 * k0 + a * dl)
        k0 += a * dl
        theta += dl / r
        x += dl * cos(theta)
        y += dl * sin(theta)
        x_list2.append(x)
        y_list2.append(y)
    plt.plot(x_list, y_list, '-')
    plt.plot(x_list2, y_list2, '-')
    plt.show()

注意事项

1. 精度要求高，相对求解时间长；
2. 可以使用数值积分的方式对式（2）进行求解，如高斯——勒让德求积公式精度高，求解速度快；
3. “以直代曲”虽然计算速度快，最终会有不收敛的问题，可以调整步长进行缓解；