【机器人】轨迹规划之 spline 规划

在轨迹规划中，使用 spline （通常是指通过样条曲线进行轨迹规划）可以实现平滑、连续的路径。以下是使用样条（如B样条、三次样条插值）的具体方法和步骤，结合一个简单的例子说明：

示例场景：

假设需要规划一辆机器人从点 A(0,0)A(0, 0) 到点 B(10,10)B(10, 10) 的平滑轨迹，并通过中间点 C(5,15)C(5, 15)。

步骤 1: 明确输入数据

给定点的坐标：轨迹的关键点（控制点）。
时间参数（可选）：在一些情况下，需要为每个点分配一个时间戳。

步骤 2: 选择样条类型

常见样条类型包括：

线性样条插值：点之间是直线，简单但不光滑。
三次样条插值（Cubic Spline Interpolation）：点之间是平滑曲线，具有连续的一阶和二阶导数。
B样条（B-Spline）：更灵活，可调整曲线的平滑程度。
其他高阶样条：适用于更复杂的需求。

在本例中，使用三次样条插值（Cubic Spline）。

步骤 3: 编写代码实现（Python 示例）

使用 `scipy` 库进行三次样条插值：

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline

# 控制点
points = np.array([[0, 0], [5, 15], [10, 10]])
x = points[:, 0]  # 提取 x 坐标
y = points[:, 1]  # 提取 y 坐标

# 生成三次样条插值
cs = CubicSpline(x, y)

# 生成平滑曲线上的点
x_new = np.linspace(x[0], x[-1], 100)  # 在 [0, 10] 间取 100 个点
y_new = cs(x_new)

# 可视化
plt.plot(x, y, 'o', label='控制点')  # 控制点
plt.plot(x_new, y_new, label='三次样条曲线')  # 插值曲线
plt.legend()
plt.xlabel("X")
plt.ylabel("Y")
plt.title("轨迹规划（三次样条插值）")
plt.grid()
plt.show()

步骤 4: 验证轨迹的平滑性

通过计算插值曲线的一阶和二阶导数来验证其连续性：

# 一阶导数
y_new_derivative = cs(x_new, 1)

# 二阶导数
y_new_second_derivative = cs(x_new, 2)

# 绘制导数曲线
plt.figure(figsize=(12, 6))
plt.plot(x_new, y_new_derivative, label='一阶导数')
plt.plot(x_new, y_new_second_derivative, label='二阶导数')
plt.legend()
plt.title("一阶与二阶导数")
plt.grid()
plt.show()