多传感器融合定位

传感器融合定位"/>

多传感器融合定位

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

• 整体流程
• • 激光里程计方案
  • • 基于SVD的ICP
    • • 介绍
      • 进一步推导
      • 旋转部分求解
      • 平移部分求解
    • 基于优化的ICP
    • NDT

整体流程

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

激光里程计方案

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

基于SVD的ICP

介绍

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

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

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

进一步推导

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

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

旋转部分求解

在旋转部分求解中，使 x i ′ = x i − u x x'_i = x_i - u_x xi′​=xi​−ux​， y i ′ = y i − u y y'_i = y_i - u_y yi′​=yi​−uy​，则E1可拆分为3项。最终与R有关的仅为第三项， x i ′ T R y i ′ x_{i}^{'T}Ry'_i xi′T​Ryi′​。即求E1 的最小值问题转化为求该项的最大。

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

E 1 ′ = Σ x i ′ T R y i ′ = Σ T r a c e ( x i ′ T R y i ′ ) = Σ T r a c e ( R y i ′ x i ′ T ) = T r a c e ( Σ R y i ′ x i ′ T ) E'_1 = \Sigma x_{i}^{'T}Ry'_i =\Sigma Trace(x_{i}^{'T}Ry'_i) = \Sigma Trace(Ry'_ix_{i}^{'T}) = Trace (\Sigma Ry'_ix_{i}^{'T} ) E1′​=Σxi′T​Ryi′​=ΣTrace(xi′T​Ryi′​)=ΣTrace(Ryi′​xi′T​)=Trace(ΣRyi′​xi′T​)

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

因此，SVD分解用于转化为正定矩阵 A A T AA^T AAT 的形式。
令 H = y i ′ x i ′ T H = y'_ix_{i}^{'T} H=yi′​xi′T​，则有 H = U Σ V T H = U \Sigma V^T H=UΣVT （SVD分解）
令 R = V U T R = VU^T R=VUT
则有

平移部分求解

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

基于优化的ICP

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

雅克比为：

NDT

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

联合概率如下

其中，



即NDT问题中，目标函数为

残差可定义为：

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

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