往前看, 别回头啊. — 漫长的季节 王响
🏰代码及环境配置：请参考 环境配置和代码运行!
本节提供了贝塞尔曲线的代码测试
python3 tests/curves/cubic_spiral_test.py
2.4.c.1 3次螺旋线代码代码实现
我们定义了CubicSpiral , 实现了和
class CubicSpiral:
def __init__(self, a0, a1, a2, a3, start_x=0, start_y=0, start_theta=0) -> None:
self.start_x = start_x
self.start_y = start_y
self.start_theta = start_theta
self.a = [a0, a1, a2, a3]
def calc_curvature(self, s):
return self.a[0] + self.a[1] * s + self.a[2] * s**2 + self.a[3] * s**3
def calc_theta(self, s):
return normallization(
self.start_theta
+ (self.a[3] * s**4 / 4)
+ (self.a[2] * s**3 / 3)
+ (self.a[1] * s**2 / 2)
+ (self.a[0] * s)
)
我们基于上一节的辛普森规则, 实现了相应的x, y的计算
def calc_coarse_x(self, s):
h = s / 8
sum_cos = (
cos(self.calc_theta(0))
+ 4 * cos(self.calc_theta(h))
+ 2 * cos(self.calc_theta(2 * h))
+ 4 * cos(self.calc_theta(3 * h))
+ 2 * cos(self.calc_theta(4 * h))
+ 4 * cos(self.calc_theta(5 * h))
+ 2 * cos(self.calc_theta(6 * h))
+ 4 * cos(self.calc_theta(7 * h))
+ cos(self.calc_theta(s))
)
return self.start_x + (s / 24) * sum_cos
def calc_coarse_y(self, s):
h = s / 8
sum_sin = (
sin(self.calc_theta(0))
+ 4 * sin(self.calc_theta(h))
+ 2 * sin(self.calc_theta(2 * h))
+ 4 * sin(self.calc_theta(3 * h))
+ 2 * sin(self.calc_theta(4 * h))
+ 4 * sin(self.calc_theta(5 * h))
+ 2 * sin(self.calc_theta(6 * h))
+ 4 * sin(self.calc_theta(7 * h))
+ sin(self.calc_theta(s))
)
return self.start_y + (s / 24) * sum_sin
为了进行对比, 我们调用了python中的求积分方法:scipy.integrate.quad 作为真值.
def calc_x(self, s):
def cos_theta(t):
theta = self.calc_theta(t)
return cos(theta)
result, error = quad(cos_theta, 0, s)
return self.start_x + result
def calc_y(self, s):
def sin_theta(t):
theta = self.calc_theta(t)
return sin(theta)
result, error = quad(sin_theta, 0, s)
return self.start_y + result
2.4.c.1 3次螺旋线代码代码测试
在cubic_spiral_test中, 我们随机生成了螺旋线的参数 , 之后使用:直接计算积分和辛普森两种方法计算x, y进行对比.
a0 = 0
a1 = random.uniform(-0.1, 0.1)
a2 = random.uniform(-0.01, 0.01)
a3 = random.uniform(-0.001, 0.001)
cubic_spiral = CubicSpiral(a0, a1, a2, a3)
效果如上图所示, 很明显可以看到:
当整体曲率不大时, 辛普森方法精度很高 当曲率较大时, 辛普森方法累计误差会越来越大
不过对于运动规划算法来说, 车辆允许的曲率本身就比较小(大部分情况不超过0.2). 所以辛普森方法在运动规划算法的应用中, 精度是足够的.
🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn
🐮GitHub代码仓：https://github.com/Hello-Xiao-Bai/Planning-XiaoBai!
🔥课程答疑,面试辅导:https://shop380995420.taobao.com
🌠代码配合官网教程食用更佳！
🚀知乎，微信，知识星球全平台同号！
推荐阅读:
如何入门运动规划算法? 50篇教程教你手把手推导公式! 实现代码!
端到端解读: Imitation Is Not Enough - 在运动规划中克服模仿学习的局限性
