多传感器融合定位-3D激光里程计（一）

整体流程

点云地图构建过程分为前端后端两部分，其中的前端过程可以看做激光里程计过程。

激光里程计方案

前端里程计中，又分为基于直接匹配、基于特征、多传感器融合里程计、基于栅格、基于面元以及基于语义几类。一般来说，基于直接匹配（ICP、NDT）、基于特征（LOAM系列）与多传感器融合里程计（LINS、LIO-SAM等）较为常见。

基于SVD的ICP

介绍

ICP，寻求两个点集的最近变换。

一般来说，两个点集（两部分点云）可以表示如下，

假定点集Y可以通过旋转、平移变换至点云X，则ICP目标为优化R、t使变换后距离最小。

进一步推导

为了简便推导，这里增加了$u_x$, $R{u}_y$两项。目的是使得R、t的优化分离开，简化后面的优化流程。这里的$u_x$, $u_y$是两坨点云的质心（均值）。

这样，我们可以发现，前一项只与R有关，后一项与R、t有关。因此，我们可以将先利用E1求出最合适的R，再将R带入E2，得到相应的t。

旋转部分求解

在旋转部分求解中，使$x_i^{\prime }=x_i-u_x$，$y_i^{\prime }=y_i-u_y$，则E1可拆分为3项。最终与R有关的仅为第三项，$x_i^{{}^{\prime }T}R{y}_i^{\prime }$。即求E1 的最小值问题转化为求该项的最大。

这里有一个定理，即常数的迹等于他本身，以及tr（AB）=tr（BA）。
由此可得，

$E_1^{\prime }=\Sigma x_i^{{}^{\prime }T}R{y}_i^{\prime }=\Sigma Trace(x_i^{{}^{\prime }T}R{y}_i^{\prime })=\Sigma Trace(R{y}_i^{\prime }{x}_i^{{}^{\prime }T})=Trace(\Sigma R{y}_i^{\prime }{x}_i^{{}^{\prime }T})$

问题转化为，求解R使得E1’最大。
而又有定理：

因此，SVD分解用于转化为正定矩阵$A{A}^T$ 的形式。
令$H=y_i^{\prime }{x}_i^{{}^{\prime }T}$，则有$H=U\Sigma V^T$ （SVD分解）
令$R=V{U}^T$
则有

平移部分求解

经过优化，得到R之后，可以利用下式求取t

基于优化的ICP

李代数下，残差可以表示为

雅克比为：

NDT

仍然是两坨点云，求取最大联合概率，

联合概率如下

其中，



即NDT问题中，目标函数为

残差可定义为：

雅克比推导分为2D场景和3D场景两种，

————————————————————————————————
P.S. 本文内容来源于深蓝学院课程《多传感器融合定位》第二期。