第1章：轨迹表示方法——路径与轨迹的区别
大家好，我是你们这节课的讲师。做机器人运动规划这些年，我踩过最大的坑，就是没搞清楚「路径」和「轨迹」的区别。记得刚入行那会儿，我拿着一条完美的路径给机器人，结果它跑起来一顿一顿的，像得了帕金森。后来才明白——路径只管「走哪条路」，轨迹才管「怎么走」。
1.1 路径 vs 轨迹：本质区别
说白了，路径是一串空间位置的集合。比如从A点到B点，中间经过哪些点，这就是路径。它不关心时间，不关心速度，只关心几何形状。
轨迹呢？轨迹是路径加上时间信息。每个点都绑定了「什么时候到」「速度多少」「加速度多少」。你想想看，同样的路径，用1秒走完和用10秒走完，轨迹完全不同。
| 维度 | 路径 | 轨迹 |
|---|---|---|
| 数学表示 | P(s)，s为弧长参数 | P(t)，t为时间参数 |
| 包含信息 | 位置、几何形状 | 位置、速度、加速度、加加速度 |
| 约束类型 | 避障、曲率限制 | 动力学、执行器限制、时间最优 |
| 典型应用 | 全局路径规划 | 局部运动控制 |
1.2 离散点路径——最朴素的方式
离散点路径就是一堆坐标点的列表。比如：[(0,0), (1,2), (3,3), (5,1)]。这种方式最简单，传感器直接采集，路径规划器直接输出。
但问题也很明显——点与点之间怎么插值？直线插值会有尖角，曲线插值又需要额外处理。我在项目中遇到过，直接用离散点做控制，机器人每到一个点就急停一下，效率极低。
# 离散点路径示例
waypoints = [
(0.0, 0.0),
(1.0, 2.0),
(3.0, 3.0),
(5.0, 1.0),
(6.0, 0.0)
]
# 直线插值——简单但不平滑
def linear_interpolate(p1, p2, t):
x = p1[0] + t * (p2[0] - p1[0])
y = p1[1] + t * (p2[1] - p1[1])
return (x, y)
1.3 多项式路径——数学之美
多项式路径用多项式函数来描述路径。比如三次多项式：y = ax³ + bx² + cx + d。好处是连续可导，曲率可以解析计算。
但多项式也有脾气。次数太低，拟合能力不够；次数太高，容易振荡（龙格现象）。我个人习惯用三次或五次多项式，既能保证C²连续，又不会太飘。
# 三次多项式路径
import numpy as np
def cubic_poly_path(x, coeffs):
a, b, c, d = coeffs
return a * x**3 + b * x**2 + c * x + d
# 计算曲率
def curvature(x, coeffs):
y = cubic_poly_path(x, coeffs)
y_prime = 3*coeffs[0]*x**2 + 2*coeffs[1]*x + coeffs[2]
y_double = 6*coeffs[0]*x + 2*coeffs[1]
return abs(y_double) / (1 + y_prime**2)**1.5
1.4 样条路径——工业标准
样条路径是分段多项式的组合。最常用的是三次样条——每段是三次多项式，段与段之间保证C²连续。为什么是C²？因为曲率连续，机器人跑起来才不抖。
B样条更灵活，可以局部调整而不影响全局。NURBS（非均匀有理B样条）还能精确表示圆弧和圆锥曲线。嗯，这里要注意，样条路径的节点分布直接影响平滑效果。
# 三次样条插值示例
from scipy.interpolate import CubicSpline
import numpy as np
# 离散点
x = np.array([0, 1, 3, 5, 6])
y = np.array([0, 2, 3, 1, 0])
# 构建三次样条
cs = CubicSpline(x, y, bc_type='natural')
# 在密集点上求值
x_dense = np.linspace(0, 6, 100)
y_dense = cs(x_dense)
1.5 混合路径表示——实战利器
实际项目中，单一表示方法往往不够用。我做过一个AGV项目，全局路径用样条，局部避障用离散点，最终轨迹用多项式拟合。这就是混合路径表示。
混合策略的核心思想：不同场景用不同表示。比如：
- 全局规划：样条路径，保证整体平滑
- 局部重规划：离散点+多项式拟合，快速响应障碍物
- 最终执行：轨迹（路径+时间），考虑动力学约束
1.6 知识体系总览
下面这张图是我自己整理的，把本章的核心逻辑串起来了。你看一遍应该就能明白各种表示方法之间的关系。
1.7 本章小结
路径和轨迹的区别，说白了就是「地图」和「导航」的区别。地图告诉你路怎么走，导航告诉你什么时候加速、什么时候转弯。
离散点路径简单直接，适合传感器数据；多项式路径数学优雅，适合理论分析；样条路径工业标准，适合实际部署；混合路径灵活强大，适合复杂场景。
我个人建议初学者先从样条路径入手，它最实用。等把样条玩熟了，再回头看其他表示方法，你会发现很多相通之处。