4. 轨迹表示方法：路径点序列、多项式曲线、样条曲线

聊到轨迹跟踪，第一个绕不开的问题就是：轨迹长什么样？

说白了，我们得先知道怎么把一条“路”用数学的方式描述出来，机器人才能理解。我见过不少新手，一上来就调PID参数，结果轨迹本身就有问题——那调参再努力也是白费。

今天我就把三种最常见的轨迹表示方法掰开揉碎讲清楚。这三种方法，基本覆盖了从简单到复杂的全部场景。

4.1 路径点序列：最朴素的方式

路径点序列，就是给机器人一串坐标点。比如：

waypoints = [(0, 0), (1, 2), (3, 3), (5, 1), (7, 4)]

机器人拿到这些点，挨个走过去就行。简单粗暴。

优点很明显：

• 实现起来几乎零门槛
• 数据存储量小
• 人类容易理解

缺点也致命：

• 点与点之间是直线连接，拐角处会有突变
• 机器人走到每个点必须停下来，否则会冲出轨迹
• 无法保证速度、加速度的连续性

我在项目中遇到过一件事：有个学生用路径点序列让AGV小车送货，结果每次转弯时货物都甩出去。为什么？因为点之间是直线，转弯时速度方向突变，加速度无穷大。嗯，物理上不可能实现。

4.2 多项式曲线：让轨迹变平滑

路径点序列太“硬”了，我们需要让它变“软”。多项式曲线就是干这个的。

最简单的，用三次多项式：

q(t) = a₀ + a₁t + a₂t² + a₃t³

四个系数，刚好可以约束起点和终点的位置、速度。这样轨迹就是一条平滑的曲线，不再有突变。

常用的多项式阶次：

我个人习惯用五次多项式。为什么？三次多项式只能保证速度连续，但加速度可能跳变。五次多项式连加速度都连续了，机器人跑起来才真正“丝滑”。

4.3 样条曲线：多段拼接的艺术

当轨迹需要经过多个点，又要保证平滑性，样条曲线就登场了。

样条曲线的核心思想是：分段拟合，整体连续。每一段用低阶多项式，段与段之间保证连续性。

最常见的两种：

• 三次样条： 每段三次多项式，保证位置、速度、加速度连续
• B样条： 更灵活，可以局部调整而不影响全局

我举个例子。假设有5个路径点，用三次样条会生成4段曲线。每段都是三次多项式，但在连接点处，位置、速度、加速度都匹配得天衣无缝。

# 三次样条插值示例（简化版）
from scipy.interpolate import CubicSpline
import numpy as np

# 路径点
t = [0, 1, 2, 3, 4]          # 时间
x = [0, 2, 3, 5, 7]          # x坐标
y = [0, 1, 3, 2, 4]          # y坐标

# 生成样条
cs_x = CubicSpline(t, x)
cs_y = CubicSpline(t, y)

# 在任意时刻求位置
t_query = 2.5
pos_x = cs_x(t_query)
pos_y = cs_y(t_query)

我曾经用B样条做过一个机械臂的轨迹规划。当时有个需求：用户拖拽机械臂示教几个点，然后机器人要平滑地重复运动。用B样条就特别合适——因为B样条有局部支撑性，修改一个点不会影响整条轨迹。你想想看，如果每次调整一个点都要重新算全局，那调试效率得多低？

4.4 三种方法的对比与选择

选哪种？我的建议是：

• 如果只是让机器人从A到B，中间不停——用多项式曲线就够了
• 如果路径点很多，且要求平滑——上样条曲线
• 如果只是做原型验证，不关心平滑性——路径点序列最快

4.5 知识体系结构图

下面这张图，帮你理清三种轨迹表示方法的关系和选择逻辑：

嗯，这张图把三种方法的核心特征和选择逻辑都串起来了。你对照着看，应该能快速找到适合自己场景的方法。

最后说一句：轨迹表示是调参的基础。轨迹本身不平滑，你后面调再多的PID、LQR都是白搭。先把轨迹搞对，再谈跟踪精度——这是我做了这么多年机器人控制最深的体会。