如果我们有“优衣库”的精确位置，而且能够测量出“优衣库”距离自己的精确距离，那么以“优衣库”为中心画一个圆，自己一定处于圆周之上。这时候如果我们还能确定“目标1”的精确位置和距离自己的精确距离，而且知道朝向，就可以唯一确定自己的精确位置了。这就是自定位方案最基本的原理。

　　而上述方案的精度主要取决于目标位置的精度和距离的精度。

　　目标位置的精度主要取决于地图数据的质量。如果地图本身就是不精确的，那么很难让人相信自定位能够达到10cm左右的精度。因此，直播，精确自定位的首要要求就是高精度地图的制作；高精度地图通常要求相对精度在厘米量级。

　　而距离的精度则取决于感知设备的选择和自定位算法的设计。首先，通过感知设备精确采集、分割、识别目标本身就是一件困难的事情；其次，将目标和高精度地图中制作的目标进行匹配和对应也存在一定的误差。

　　当然，如果给定的目标越多，例如上图中增加“目标2”等，由于误差的平均减少原理，自然能够增加自定位的精度。

　　从上面的分析不难看出，自定位方案依赖于高精地图，而感知设备的选择和自定位算法的设计决定了最终的定位精度。下面将对几种典型的精确车辆自定位方案进行探讨（包括感知设备的选择和相应的自定位算法的设计）。

　　感知设备的选择和自定位算法的设计

　　常用的感知设备和相应的自定位算法可归纳如下：

　　1. 激光点云数据+3D配准

　　最开始也是最直观的方案是直接对激光点云数据进行3D配准。激光点云数据不会受到光照变化、天气、遮挡等因素的影响，而且能够直接得到和目标的物理距离，因此利用多视角几何（multi-view geometry）即可完成目标的自定位。在下图中，左侧为实时采集的激光点云数据，而右侧为高精度地图中存储的局部激光点云数据。

　　根据多视角几何理论，上述两个场景的相机存在姿态上的差异，而这个差异可以通过3D点的坐标变换来补偿；变换矩阵主要包括3D旋转变换和3D平移变换共6个自由度。如果实时采集的点云数据 和高精地图中存储的局部点云数据能够匹配上，那么满足：

　　一般来说，4组点对就可以唯一的确定上述变换矩阵。

　　但上述分析面临两个难题：

4/8线激光点云的数据是稀疏的，每个点的描述也只有反射率和相机映射过去的彩色值，那么如何确定对应点对？

通常点对精度不够，而且存在大量的误匹配点对，如何滤除存在的噪声？

　　一种解决方法是利用ICP算法（Iterative Closest Point，迭代最近点），它的主要思路是先利用某些几何特征来假设一些点对，并计算变换矩阵；再对所有点进行变换，并更新假设的匹配点对；也就是利用这种不断迭代的方式来优化自定位结果。当然，还有很多其他的非线性优化算法可以解决上述问题。

　　激光点云数据+3D配准方案存在的缺点包括：

初值非常重要，不合适的初值极大的影响了自定位结果。

上述优化过程很难收敛到全局最优解，而局部最优解难以保证自定位的精度和稳定性。

变换矩阵的自由度较高，而且匹配失败的时候难以及时得到确定。

　　2. 激光点云数据+2D配准