clothoid介绍
clothoid曲线是一种曲率半径与长度成线性关系的曲线,由于其曲率平滑过渡,在路径规划中有所应用。在直角坐标系中,clothoid曲线的微分方程如式(1)所示。
dk=adl (1)
式(1)中,k表示曲率,l表示曲线长度,a表示系数,可以明显地看到clothoid曲线的曲率与曲线长度成线性关系。
绘制clothoid曲线
积分近似
通过对式(1)进行处理得到clothoid曲线在直角坐标系的参数方程(具体推导过程略),如式(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)所示。
# -*- 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()
注意事项
- 精度要求高,相对求解时间长;
- 可以使用数值积分的方式对式(2)进行求解,如高斯——勒让德求积公式精度高,求解速度快;
- “以直代曲”虽然计算速度快,最终会有不收敛的问题,可以调整步长进行缓解;
评论(0)
您还未登录,请登录后发表或查看评论