曲线匹配 (二维) 的数学定义和工程实现

曲线匹配

在自动驾驶, 机器人导航和地理信息系统等领域, 曲线匹配是核心的基础技术. 典型应用场景包括:

1. 高精地图匹配: 将车辆实时轨迹与高精度地图道路网进行匹配
2. 多传感器融合: 对齐不同传感器采集的轨迹数据（如GPS IMU 视觉SLAM）
3. 运动模式识别: 比较不同驾驶员的转向操作模式特征

本文以自动驾驶中的典型场景为例: 假设我们有两个曲线集合:

• 集合A: 高精地图中的道路中心线（参考曲线）
• 集合B: 车辆实时采集的行驶轨迹（查询曲线）

目标: 快速找到集合B中每条轨迹在集合A中对应的最优匹配道路

二 曲线相似度评价标准

以下是对点集曲线各种距离的数学形式化定义和程序

设曲线 P = { p 1 , p 2 , . . . , p m } P = \{p_1,p_2,...,p_m\} P={p1​,p2​,...,pm​} 和 Q = { q 1 , q 2 , . . . , q n } Q = \{q_1,q_2,...,q_n\} Q={q1​,q2​,...,qn​}, 其中 $p_i,q_j\in {\mathbb{R}}^d$

2.1 逐点距离范数

2.1.1 欧氏距离 (L2范数)

要求 $m=n$, 对齐对应点:
$$D_{L2}(P,Q)=\sqrt{\sum _{k=1}^n\Vert p_k-q_k{\Vert }_2^2}$$

def euclidean_distance(curve1, curve2):
    min_len = min(len(curve1), len(curve2))
    return np.sqrt(np.sum((curve1[:min_len] - curve2[:min_len])**2))

2.1.2 曼哈顿距离 (L1范数)

$$D_{L1}(P,Q)=\sum _{k=1}^{\min (m,n)}\Vert p_k-q_k{\Vert }_1$$

def manhattan_distance(curve1, curve2):
    min_len = min(len(curve1), len(curve2))
    return np.sum(np.abs(curve1[:min_len] - curve2[:min_len]))

2.2 动态时间规整 (DTW)

允许非线性对齐, 定义规整路径 $\pi =({\pi }_1,...,{\pi }_K)$, 其中 ${\pi }_k=(i_k,j_k)$ 满足:

• ${\pi }_1=(1,1)$
• ${\pi }_K=(m,n)$
• $i_{k+1}-i_k\le 1$
• $j_{k+1}-j_k\le 1$

DTW距离为:
$$D_{DTW}(P,Q)=\underset{\pi }{\min }\sqrt{\sum _{(i,j)\in \pi }\Vert p_i-q_j{\Vert }^2}$$

from dtaidistance import dtw

def dtw_distance(curve1, curve2):
    return dtw.distance(curve1, curve2)

2.3 弗雷歇距离 (Frétchet Distance)

考虑连续参数化 $\alpha :[0,1]\to P$, $\beta :[0,1]\to Q$, 定义:
$$D_{Fr\acute{e}tchet}(P,Q)=\underset{\alpha ,\beta }{\inf }\underset{t\in [0,1]}{\max }\Vert \alpha (t)-\beta (t)\Vert$$
离散形式近似为:
$$D_{discFr\acute{e}tchet}=\underset{i}{\max }\underset{j}{\min }\Vert p_i-q_j\Vert +\underset{j}{\max }\underset{i}{\min }\Vert q_j-p_i\Vert$$

def frechet_distance(P, Q):
    # 实现递归计算或使用矩阵加速
    # 详见论文 [Computing the Fréchet distance between two polygonal curves]

Computing the Fréchet distance between two polygonal curves

2.4 方向相似度

对于方向向量序列 V P = { p i + 1 − p i } i = 1 m − 1 V_P = \{p_{i+1}-p_i\}_{i=1}^{m-1} VP​={pi+1​−pi​}i=1m−1​, V Q = { q j + 1 − q j } j = 1 n − 1 V_Q = \{q_{j+1}-q_j\}_{j=1}^{n-1} VQ​={qj+1​−qj​}j=1n−1​

余弦相似度:
$$S_{cos}(P,Q)=\frac{1}{K}\sum _{k=1}^K\frac{v_k^P\cdot v_k^Q}{\Vert v_k^P\Vert \Vert v_k^Q\Vert }$$
其中 $K=\min (m-1,n-1)$

def cosine_similarity(curve1, curve2):
    vec1 = curve1[1:] - curve1[:-1]  # 计算方向向量
    vec2 = curve2[1:] - curve2[:-1]
    return np.dot(vec1, vec2.T)/(norm(vec1)*norm(vec2))

2.5 豪斯多夫距离 (Hausdorff Distance)

定义两个点集间的最大最小距离:
$$D_{Hausdorff}(P,Q)=\max \left(\underset{p\in P}{\sup }\underset{q\in Q}{\inf }\Vert p-q\Vert ,\underset{q\in Q}{\sup }\underset{p\in P}{\inf }\Vert q-p\Vert \right)$$

2.6 面积差异度量

对平面曲线 ($d=2$), 定义包围区域面积差:
$$D_{Area}=\left|{\iint }_{{\Omega }_P}dxdy-{\iint }_{{\Omega }_Q}dxdy\right|$$
其中 ${\Omega }_P$ 为曲线P的闭合区域

关键公式对比表

工程应用中的变形公式

加权DTW

$$D_{WDTW}(P,Q)=\underset{\pi }{\min }\sum _{k=1}^K{w}_k\cdot \Vert p_{i_k}-q_{j_k}\Vert$$
其中权重 $w_k$ 可根据应用场景设计（如更关注起始段匹配）

多尺度距离

结合不同粒度的距离计算:
$$D_{Multi}(P,Q)=\alpha D_{DTW}(P_{down},Q_{down})+\beta D_{Frechet}(P,Q)$$
其中 $P_{down}$ 为降采样后的曲线

这些数学形式为实际工程实现提供了理论基准, 实际编码时需根据计算资源 实时性要求进行适当近似（如使用快速DTW算法代替全矩阵计算）.

三 工程化解决方案架构

3.1 预处理阶段

1. 重采样: 统一曲线采样频率

   from scipy.interpolate import interp1d
   
   def resample_curve(curve, num_points=100):
       t = np.linspace(0, 1, len(curve))
       f = interp1d(t, curve, axis=0, kind='linear')
       return f(np.linspace(0, 1, num_points))
   

2. 坐标归一化: 消除绝对位置差异

   def normalize_curve(curve):
       return (curve - np.mean(curve, axis=0)) / np.std(curve, axis=0)
   

3.2 多级匹配策略

3.3 实时匹配优化

• 滑动窗口缓存: 保留最近N个匹配结果作为匹配起点
• 并行计算: 利用CUDA加速矩阵运算
• 早期终止: 设置距离阈值提前终止计算

3.2 混合匹配算法实现

from scipy.spatial.distance import cdist

def hybrid_matcher(query, candidates, weights=(0.4, 0.6)):
    """
    混合匹配算法: DTW + 方向相似度
    :param weights: (dtw_weight, cosine_weight)
    """
    scores = []
    for ref in candidates:
        # DTW距离
        dtw_dist = dtw.distance(query, ref)
        
        # 方向相似度
        cos_sim = cosine_similarity(query, ref)
        
        # 归一化组合得分
        score = weights[0]*dtw_dist + weights[1]*(1 - cos_sim)
        scores.append(score)
    
    return np.argmin(scores)

3.4 优化建议

1. 降采样策略: 在粗匹配阶段使用1/4采样率
2. 空间索引加速: 使用KD-Tree进行快速邻域搜索

   from sklearn.neighbors import KDTree
   
   tree = KDTree(ref_points)
   indices = tree.query_radius(query_points, r=5.0)
   

3. 增量式计算: 对连续轨迹只计算最新窗口差异
4. 非均匀采样: 处理不同采样率的曲线数据
5. 部分匹配: 识别长曲线中的局部相似片段
6. 深度学习: 使用CNN/LSTM学习曲线特征表示

   # 示例使用PyTorch构建轨迹特征提取器
   class TrajEncoder(nn.Module):
       def __init__(self):
           super().__init__()
           self.lstm = nn.LSTM(input_size=2, hidden_size=64)
           self.fc = nn.Linear(64, 32)
           
       def forward(self, x):
           _, (h_n, _) = self.lstm(x)
           return self.fc(h_n[-1])
   

注: 实际工程中需考虑坐标系转换, 道路拓扑约束等额外因素, 建议结合具体业务场景调整算法参数.