第三章：路径表示与平滑处理
各位同学，今天我们来聊聊路径的表示与平滑。说实话，这是无人车控制里最容易被忽视、却又最影响最终效果的一环。我见过太多团队在控制算法上死磕，结果发现路径本身就有问题——毛刺多、曲率突变，再好的控制器也救不回来。
3.1 路径点插值：从离散到连续
传感器采集的路径点，通常是离散的。比如GPS每隔0.1秒给一个点，或者规划层输出了一系列稀疏的路径点。但控制器需要连续的位置信息，怎么办？插值。
3.1.1 线性插值
最简单的方法，就是两点之间连直线。数学上就是：
// 线性插值公式
P(t) = (1 - t) * P0 + t * P1, t ∈ [0, 1]代码实现也很直接：
def linear_interpolate(p0, p1, num_points):
"""线性插值，在p0和p1之间生成num_points个点"""
points = []
for i in range(num_points):
t = i / (num_points - 1)
x = (1 - t) * p0[0] + t * p1[0]
y = (1 - t) * p0[1] + t * p1[1]
points.append((x, y))
return points
注意：线性插值在连接处只有C0连续（位置连续），一阶导数不连续。这意味着曲率会突变，车辆转向时会有冲击感。我曾经在早期项目中用线性插值做路径平滑，结果实车测试时方向盘一直在抖——嗯，从那以后我再也不敢只用线性插值了。
3.1.2 样条插值
为了解决连续性问题，我们需要更高阶的插值方法。我个人最常用的是三次样条插值。它保证C2连续（位置、速度、加速度都连续），非常适合路径表示。
# 使用scipy的三次样条插值
from scipy.interpolate import CubicSpline
import numpy as np
# 原始路径点
x = np.array([0, 1, 2, 3, 4, 5])
y = np.array([0, 0.5, 2, 1.5, 3, 4])
# 以路径长度s为参数进行样条插值
s = np.cumsum(np.sqrt(np.diff(x)**2 + np.diff(y)**2))
s = np.insert(s, 0, 0) # 第一个点s=0
cs_x = CubicSpline(s, x)
cs_y = CubicSpline(s, y)
# 在更密集的s上采样
s_dense = np.linspace(0, s[-1], 100)
x_dense = cs_x(s_dense)
y_dense = cs_y(s_dense)
我的经验：用路径长度s作为参数，比直接用x或y作为参数更稳定。因为路径可能有回环或垂直段，直接用x或y会导致多值问题。这个坑我踩过，分享给大家。
3.2 路径平滑算法
插值解决了连续性问题，但原始路径点本身可能就有噪声。这时候需要平滑算法。说白了，就是既要贴近原始路径，又要让路径足够光滑。
3.2.1 B样条曲线
B样条曲线是我在项目中用得最多的平滑方法。它的核心思想是：用一组控制点来定义曲线，但曲线不一定要通过所有控制点。
import numpy as np
def bspline_basis(i, k, t, knots):
"""计算B样条基函数"""
if k == 0:
return 1.0 if knots[i] <= t < knots[i+1] else 0.0
# 递归计算
denom1 = knots[i+k] - knots[i]
denom2 = knots[i+k+1] - knots[i+1]
term1 = 0
if denom1 != 0:
term1 = (t - knots[i]) / denom1 * bspline_basis(i, k-1, t, knots)
term2 = 0
if denom2 != 0:
term2 = (knots[i+k+1] - t) / denom2 * bspline_basis(i+1, k-1, t, knots)
return term1 + term2
def bspline_curve(control_points, num_samples=100, degree=3):
"""生成B样条曲线上的点"""
n = len(control_points)
knots = np.linspace(0, 1, n + degree + 1)
curve_points = []
for t in np.linspace(0, 1, num_samples):
point = np.zeros(2)
for i in range(n):
basis = bspline_basis(i, degree, t, knots)
point += basis * control_points[i]
curve_points.append(point)
return np.array(curve_points)
B样条的优势：
- 局部性：移动一个控制点只影响附近区域，不会全局变形
- 光滑性：三次B样条保证C2连续
- 凸包性：曲线始终在控制点构成的凸包内
3.2.2 贝塞尔曲线
贝塞尔曲线是另一种经典方法。它的特点是：曲线始终通过第一个和最后一个控制点，中间控制点起牵引作用。
def bezier_curve(control_points, num_samples=100):
"""生成贝塞尔曲线上的点"""
n = len(control_points) - 1
curve_points = []
for t in np.linspace(0, 1, num_samples):
point = np.zeros(2)
for i in range(n + 1):
# 伯恩斯坦基函数
coeff = np.math.comb(n, i) * (t ** i) * ((1 - t) ** (n - i))
point += coeff * control_points[i]
curve_points.append(point)
return np.array(curve_points)
注意：贝塞尔曲线是全局的——移动一个控制点会影响整条曲线。对于长路径，我建议分段使用低阶贝塞尔曲线（3阶或4阶），而不是用高阶曲线。高阶曲线容易出现振荡，你想想看，一个10阶贝塞尔曲线，控制点稍微动一下，曲线可能就扭成麻花了。
3.3 曲率计算
曲率是路径平滑度的核心指标。对于无人车来说，曲率直接决定了最大通过速度。曲率越大，转弯越急，速度就得越低。
3.3.1 离散点曲率计算
对于离散路径点，我们用三点法估算曲率：
def compute_curvature(p1, p2, p3):
"""计算三点确定的圆的曲率"""
# 向量
v1 = p2 - p1
v2 = p3 - p2
# 叉积的模（2倍三角形面积）
cross_product = np.cross(v1, v2)
# 边长
a = np.linalg.norm(v1)
b = np.linalg.norm(v2)
c = np.linalg.norm(p3 - p1)
# 曲率 = 4 * 面积 / (a * b * c)
if a * b * c == 0:
return 0.0
curvature = 2 * abs(cross_product) / (a * b * c)
return curvature
# 计算整条路径的曲率
def path_curvature(path_points):
"""计算路径上每个点的曲率"""
curvatures = []
n = len(path_points)
for i in range(1, n - 1):
k = compute_curvature(path_points[i-1],
path_points[i],
path_points[i+1])
curvatures.append(k)
# 首尾点曲率设为0
curvatures.insert(0, 0.0)
curvatures.append(0.0)
return np.array(curvatures)3.3.2 曲率约束检查
在实际项目中，我通常会做曲率检查：
def check_curvature_constraint(path_points, max_curvature):
"""检查路径是否满足最大曲率约束"""
curvatures = path_curvature(path_points)
violations = np.where(curvatures > max_curvature)[0]
if len(violations) > 0:
print(f"警告：发现 {len(violations)} 个点曲率超标")
print(f"最大曲率：{np.max(curvatures):.4f}")
print(f"允许最大曲率：{max_curvature:.4f}")
return False
return True
避坑指南：我曾经在一个项目中，路径曲率计算一直偏大，排查了半天发现是坐标系单位搞错了——经纬度直接当米来算。曲率对尺度非常敏感，一定要确保单位统一。建议所有路径点先转换到笛卡尔坐标系（比如UTM坐标），再做曲率计算。
3.4 知识体系总览
下面这张图总结了本章的核心内容：
从这张图可以看出，路径表示是基础，插值和平滑是核心方法，曲率是评价指标，最终服务于路径跟踪控制。每一步都有坑，每一步也都有技巧。
本章小结：
- 线性插值简单但连续性差，适合快速原型验证
- 三次样条插值C2连续，是工程中最常用的插值方法
- B样条曲线局部可控，适合长路径平滑
- 贝塞尔曲线全局可控，适合短路径或曲线设计
- 曲率计算要特别注意单位一致性
好了，关于路径表示与平滑处理就讲到这里。这些方法我在实际项目中反复用过，踩过坑也填过坑。希望你们能少走弯路，直接用到自己的项目里。