车辆行驶路径平滑方法

　　笔者在工作中遇到了一个问题：需要车辆平滑的行驶路径来完成建图，但是实际采集的路径数据不太理想，总是免不了包含倒车等带“尖角”的路径。如何对其进行平滑呢？我想到了一个简单的方法。
　　思路是计算相邻点间向量的夹角，如果超过一定阈值就令中间的点取前后两个点的平均值。其本质是限制曲线的曲率。但是曲率不仅与角度有关，还与弧长有关，所以仅仅计算角度还不够，还要不断对点进行下采样，确保其弧长不至于太小。
　　Mathematica代码如下：
　　首先是采样函数（Sample），其按照一定的分辨率（resolution）对点列（pts）进行下采样，去除太密集的点。

Sample[pts_, resolution_] := Module[{ptsa = {}},
  Do[
   If[Length[ptsa] < 1,
     AppendTo[ptsa, pt],
     If[Norm[ptsa[[-1]] - pt] > resolution, AppendTo[ptsa, pt]]
     ];
   , {pt, pts}];
  ptsa]

　　平滑的代码如下，就是两个循环，外层的循环可以用一个while替换。

pts = pts0;
maxangle = 5.0/180*Pi;
Do[
 ptsa = Sample[pts, 0.5];
 n = Length[ptsa];
 pts = ptsa;
 Do[
  angle = VectorAngle[pts[[i]] - pts[[i - 1]], pts[[i + 1]] - pts[[i]]];
  If[angle > maxangle, pts[[i]] = Mean[{pts[[i + 1]], pts[[i - 1]]}]];
  , {i, 2, n - 1}];
 , {j, 100}]
Graphics[{AbsolutePointSize[1], Black, Point[ptsa0], AbsolutePointSize[1.5], Red, Point[pts]}]

　　我们用一些数据来验证一下，假设汽车的行驶路径的坐标点如下图中左侧的黑点。数据在最后给出。平滑后的坐标点见右图中的红点，已经重新采样了所以看起来更均匀。可以看到，车辆倒车形成的尖角都被抹平了。

　　车辆实际的路径坐标数据如下（点击下面的三角即可展开）：