绘制clothoid曲线

绘制clothoid曲线

  • clothoid介绍
  • 绘制clothoid曲线
    • 积分近似
    • 以直代曲
  • 注意事项

clothoid介绍

  clothoid曲线是一种曲率半径与长度成线性关系的曲线,由于其曲率平滑过渡,在路径规划中有所应用。在直角坐标系中,clothoid曲线的微分方程如式(1)所示。
d k = a d l (1) dk=adl \tag1 dk=adl(1)

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

绘制clothoid曲线

积分近似

  通过对式(1)进行处理得到clothoid曲线在直角坐标系的参数方程(具体推导过程略),如式(2)所示。
x = 1 a ∫ 0 τ c o s ( t 2 ) d t y = 1 a ∫ 0 τ s i n ( t 2 ) d t (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 \tag2 \end{array} x=a10τcos(t2)dty=a10τsin(t2)dt(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()

绘制clothoid曲线_第1张图片

以直代曲

  使用python求解虽然可以获得非常高的求解精度,但计算速度慢,便考虑使用更简便的方式求解,这里用“以直代曲”的方式绘制clothoid曲线。迭代公式如式(3)所示。
r n = 2 / ( 2 k n − 1 + a d l ) k n = k n − 1 + a d l θ n = θ n − 1 + d l / r n x n = x n − 1 + d l c o s ( θ n ) y n = y n − 1 + d l s i n ( θ n ) (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) \tag3 \end{array} rn=2/(2kn1+adl)kn=kn1+adlθn=θn1+dl/rnxn=xn1+dlcos(θn)yn=yn1+dlsin(θn)(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()

绘制clothoid曲线_第2张图片

注意事项

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

你可能感兴趣的:(其他)