4、数学工具库：NumPy、SciPy、Matplotlib在轨迹规划中的基础应用

做轨迹规划，说白了就是跟数学公式和矩阵打交道。你想想看，一条平滑的机器人运动轨迹，背后全是向量、矩阵、插值、优化这些东西。我刚开始入行那会儿，觉得手写这些数学运算挺酷的，后来发现纯属浪费时间——NumPy、SciPy、Matplotlib这三个库，基本就是轨迹规划领域的「三件套」。

这一章，我就带你看看这三个库在轨迹规划里到底怎么用。不扯虚的，直接上干货。

4.1 NumPy：轨迹数据的「骨架」

NumPy是Python科学计算的基石。轨迹规划里，你处理的所有数据——位置点、速度、加速度、时间序列——本质上都是数组或矩阵。

我个人习惯用NumPy的ndarray来存储轨迹点。比如一个5秒的轨迹，控制周期10ms，那就是500个点。每个点如果是6轴关节角度，数组形状就是(500, 6)。

import numpy as np

# 生成一条简单的直线轨迹
start_point = np.array([0.0, 0.0, 0.0])
end_point = np.array([1.0, 1.0, 1.0])
num_points = 100

# 线性插值
t = np.linspace(0, 1, num_points)
trajectory = start_point + t[:, np.newaxis] * (end_point - start_point)

print(trajectory.shape)  # (100, 3)

这里有个小技巧：np.newaxis用来扩展维度，让广播机制生效。我在项目中遇到过新手直接用t * (end_point - start_point)，结果报维度错误——其实就是没理解广播的规则。

4.2 SciPy：插值与优化的「瑞士军刀」

NumPy给了你数据结构，SciPy给了你算法。轨迹规划里最常用的两个模块：scipy.interpolate和scipy.optimize。

4.2.1 插值：让轨迹变平滑

实际项目中，你往往只有几个关键路点。比如抓取点、放置点、中间避障点。怎么让机器人平滑地经过这些点？插值。

我推荐用CubicSpline（三次样条插值）。它保证位置和速度连续，轨迹足够平滑。

from scipy.interpolate import CubicSpline
import numpy as np

# 假设有5个关键路点（时间，位置）
time_points = np.array([0.0, 1.0, 2.5, 4.0, 5.0])
pos_points = np.array([0.0, 0.5, 1.2, 0.8, 0.0])

# 构建三次样条
cs = CubicSpline(time_points, pos_points, bc_type='clamped')

# 生成密集轨迹
t_dense = np.linspace(0, 5, 500)
trajectory_smooth = cs(t_dense)

# 顺便算速度和加速度
velocity = cs.derivative()(t_dense)
acceleration = cs.derivative(2)(t_dense)

4.2.2 优化：找最优轨迹

有时候轨迹不是「插出来」的，而是「算出来」的。比如时间最优轨迹——在满足速度、加速度约束的前提下，让运动时间最短。

SciPy的minimize函数可以搞定这类问题。我一般用SLSQP方法，它支持约束条件。

from scipy.optimize import minimize

# 一个简化版的时间最优轨迹问题
def objective(t_total):
    # 目标：最小化总时间
    return t_total

def constraint_velocity(t):
    # 约束：速度不能超过上限
    # 这里简化处理，实际需要根据轨迹计算
    return 1.0 - np.abs(compute_max_velocity(t))

# 初始猜测
t0 = 5.0
# 约束：速度上限1.0 m/s
cons = {'type': 'ineq', 'fun': constraint_velocity}
# 边界：总时间在1到10秒之间
bounds = [(1.0, 10.0)]

result = minimize(objective, t0, method='SLSQP', bounds=bounds, constraints=cons)
print(f"最优时间: {result.x[0]:.2f} 秒")

4.3 Matplotlib：把轨迹「画」出来

做轨迹规划，看不见轨迹等于盲人摸象。Matplotlib就是你的眼睛。

我调试轨迹时，必看三张图：位置-时间图、速度-时间图、加速度-时间图。三张图并排，一眼就能看出轨迹是否平滑、有没有冲击。

import matplotlib.pyplot as plt

# 假设已有轨迹数据
t = np.linspace(0, 5, 500)
pos = np.sin(t)  # 示例数据
vel = np.cos(t)
acc = -np.sin(t)

fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)

axes[0].plot(t, pos, 'b-', linewidth=2)
axes[0].set_ylabel('位置 (rad)')
axes[0].grid(True, alpha=0.3)

axes[1].plot(t, vel, 'g-', linewidth=2)
axes[1].set_ylabel('速度 (rad/s)')
axes[1].grid(True, alpha=0.3)

axes[2].plot(t, acc, 'r-', linewidth=2)
axes[2].set_ylabel('加速度 (rad/s²)')
axes[2].set_xlabel('时间 (s)')
axes[2].grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

4.4 知识体系总览

下面这张图，是我梳理的这三个库在轨迹规划中的分工和协作关系。你可以把它当作一个「地图」，以后做轨迹规划时按图索骥就行。

4.5 三个库的协作流程

在实际项目中，这三个库是串在一起用的。我总结了一个标准流程：

1. 用NumPy 定义路点、时间序列、约束边界
2. 用SciPy 做插值生成密集轨迹，或者优化求解最优参数
3. 用Matplotlib 把结果画出来，检查平滑度、约束是否满足
4. 发现问题，回到第1步调整参数，循环迭代

嗯，这一章的内容就到这里。这三个库是轨迹规划的「基本功」，后面所有章节都会用到它们。你先把这些基础操作练熟，后面做多项式轨迹、贝塞尔曲线、时间最优规划的时候，才能游刃有余。