4、轨迹规划入门：全局路径规划 vs 局部路径规划、路径点插值

各位同学，欢迎来到轨迹规划的实战第一课。说实话，很多新手一上来就被各种规划算法搞懵了。什么A*、Dijkstra、RRT、DWA、TEB……听着就头大。

其实没那么复杂。你想想看，机器人要动起来，无非就两件事：去哪儿和怎么去。全局规划管的是“去哪儿”，局部规划管的是“怎么去”。今天我就带你把这两个概念彻底理清楚，再聊聊路径点插值这个让轨迹变平滑的实用技巧。

4.1 全局路径规划 vs 局部路径规划

先讲个我自己的经历。几年前我做AGV项目，一开始只做了全局规划，结果机器人每次遇到临时障碍物就卡死。后来加了局部规划，才真正跑起来。说白了，这两者缺一不可。

4.1.1 全局路径规划

全局规划，说白了就是“看地图，找路”。它依赖的是事先建好的静态地图，比如SLAM建出来的栅格地图。常见的算法有：

• Dijkstra：保证最短路径，但搜索范围大，效率低。适合小地图。
• A*：加了启发式函数，比Dijkstra快很多。我项目里最常用的就是它。
• RRT / RRT*：基于随机采样，适合高维空间或非结构化环境。

嗯，这里要注意：全局规划的输出是一系列离散的路径点，不是连续的轨迹。这些点之间怎么走，那是局部规划的事。

4.1.2 局部路径规划

局部规划处理的是“眼下的事”。它只关心机器人周围几米内的环境，实时感知障碍物，然后生成平滑、安全的运动指令。

我常用的局部规划算法有：

• DWA (Dynamic Window Approach)：在速度空间采样，选最优轨迹。适合差速轮机器人。
• TEB (Timed Elastic Band)：把轨迹看作弹性带，优化速度和避障。适合阿克曼底盘。
• MPC (Model Predictive Control)：带模型预测，效果好但计算量大。

4.1.3 两者对比

维度	全局路径规划	局部路径规划
地图范围	整个已知地图	机器人周围几米
地图类型	静态地图	动态实时感知
输出	离散路径点序列	速度指令 (v, ω)
计算频率	低 (0.1~1 Hz)	高 (10~50 Hz)
典型算法	A*, Dijkstra, RRT	DWA, TEB, MPC
避障能力	仅静态障碍物	动态障碍物

你看，全局规划像是一个“战略层”，告诉你从北京到上海走哪条高速；局部规划是“战术层”，在高速上遇到堵车时怎么变道。两者配合才能跑得稳。

4.2 路径点插值：让轨迹变平滑

全局规划出来的路径点，通常是离散的、带折角的。如果直接让机器人沿着这些点走，你会发现它每到一个点就急停、转向，再加速。不仅效率低，还容易损坏电机。

怎么办？插值。说白了，就是在两个路径点之间“补”出更多点，让轨迹连续平滑。

4.2.1 线性插值

线性插值是最简单的方法。假设两个点 P0 和 P1，中间任意位置可以表示为：

P(t) = P0 + t * (P1 - P0),  t ∈ [0, 1]

当 t=0 时在 P0，t=1 时到 P1。代码实现也很直接：

def linear_interpolate(p0, p1, num_points):
    points = []
    for i in range(num_points):
        t = i / (num_points - 1)
        x = p0[0] + t * (p1[0] - p0[0])
        y = p0[1] + t * (p1[1] - p0[1])
        points.append((x, y))
    return points

4.2.2 样条插值

样条插值能保证轨迹的连续性和平滑性。最常用的是三次样条，它保证位置、速度、加速度都连续。

三次样条的核心思想：每两个点之间用一个三次多项式连接，并且在节点处保证一阶导数和二阶导数连续。

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

# 原始路径点
x_points = [0, 1, 2, 3, 4]
y_points = [0, 2, 1, 3, 0]

# 创建三次样条
cs = CubicSpline(x_points, y_points)

# 生成密集插值点
x_dense = np.linspace(0, 4, 100)
y_dense = cs(x_dense)

你看，代码很简洁。但实际项目中，我更推荐用五次样条或B样条，因为它们能控制更高阶的连续性，比如加加速度（jerk）连续。这对高精度轨迹跟踪特别重要。

4.3 知识体系总览

下面这张图帮你理清本章的核心逻辑：

4.4 实战中的选择建议

说了这么多，到底怎么选？我根据项目经验给你几个参考：

• 室内差速轮机器人：A* + DWA，插值用三次样条。ROS里调好参数就能跑。
• 室外阿克曼底盘：A* + TEB，插值用五次样条。阿克曼转弯半径大，需要更平滑的轨迹。
• 机械臂轨迹规划：RRT* + 多项式插值。关节空间规划时，样条插值能保证末端平滑。
• 高速移动机器人：全局用A*，局部用MPC，插值用B样条。MPC能预测未来轨迹，适合高速场景。