Home » PythonRobotics机器人算法库 » PythonRobotics机器人算法库-三次样条规划

PythonRobotics机器人算法库-三次样条规划

说明:

步骤:

样条曲线连续性

  • 样条曲线的平滑度取决于使用哪种样条模型。

  • 样条曲线的平滑度表示为C0 C1, 等等。

  • 该表示表示曲线的连续性。例如,对于二维空间中的样条曲线:

  • C0位置是否连续

  • C1切向量是连续的

  • C2曲率向量是连续的

  • 如下图所示:

请输入图片描述

  • 三次样条可以生成一条曲线,C0,C1,C2

一维三次样条

  • 三次样条插值是在给定多个数据点的情况下,在多个数据点之间进行平滑插值的方法,如下图所示。

请输入图片描述

  • 它在数据点之间的每个间隔之间进行分隔。

  • 每个区间部分由每个三次多项式近似。

  • 三次样条使用三次多项式方程进行插值:

请输入图片描述

约束一:终端约束

请输入图片描述

约束2:点连续约束

请输入图片描述

约束3:切向量连续约束

请输入图片描述

约束4:曲率连续约束

请输入图片描述

约束 5:终端曲率约束

请输入图片描述

请输入图片描述

请输入图片描述

请输入图片描述

  • 例子,插入一维数据点
import numpy as np
import matplotlib.pyplot as plt
x = np.arange(5)
y = [1.7, -6, 5, 6.5, 0.0]
sp = CubicSpline1D(x, y)
xi = np.linspace(0.0, 5.0)
yi = [sp.calc_position(x) for x in xi]
plt.plot(x, y, "xb", label="Data points")
plt.plot(xi, yi , "r", label="Cubic spline interpolation")
plt.grid(True)
plt.legend()
plt.show()

请输入图片描述

请输入图片描述

  • 二维三次样条

请输入图片描述

  • 例如,可以对二维数据点进行插值
import matplotlib.pyplot as plt
x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
ds = 0.1  # [m] distance of each interpolated points
sp = CubicSpline2D(x, y)
s = np.arange(0, sp.s[-1], ds)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
    ix, iy = sp.calc_position(i_s)
    rx.append(ix)
    ry.append(iy)
    ryaw.append(sp.calc_yaw(i_s))
    rk.append(sp.calc_curvature(i_s))
plt.subplots(1)
plt.plot(x, y, "xb", label="Data points")
plt.plot(rx, ry, "-r", label="Cubic spline path")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.show()

请输入图片描述

plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")

请输入图片描述

plt.subplots(1)
plt.plot(s, rk, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")

请输入图片描述

请输入图片描述

  • 进入目录PythonRobotics/PathPlanning/CubicSpline

  • 执行文件

python3 cubic_spline_planner.py

python3 spline_continuity.py
  • 结果如下

请输入图片描述

请输入图片描述

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: none