多传感器融合定位五-点云地图构建及定位

多传感器融合定位五-点云地图构建及定位

  • 1. 回环检测
    • 1.1 基于Scan Context
    • 1.2 基于直方图
  • 2. 后端优化
    • 2.1 后端优化基本原理
    • 2.2 李群、李代数基本知识
    • 2.3 李群、李代数基本知识
      • 2.3.1 S O ( 3 ) \mathrm{SO}(3) SO(3) 对应的 B C H \mathrm{BCH} BCH 公式
      • 2.3.2 S E ( 3 ) \mathrm{SE}(3) SE(3) 对应的 B C H \mathrm{BCH} BCH 公式
    • 2.4 伴随矩阵
    • 2.3 基于回环的位姿修正
    • 2.4 基于先验观测的位姿修正
  • 3. 点云地图建立
    • 3.1 整体流程
  • 4. 基于地图的定位
    • 4.1 整体流程
    • 4.2 位姿初始化
      • 4.2.1 整体流程
      • 4.2.2 位姿初始化
  • 5. LeGO-LOAM介绍
    • 5.1 主要特点
    • 5.2 特征提取
    • 5.3 位姿解算
    • 5.4 回环检测与修正

前面做出了一个比较好的里程计,但是在实际项目使用的时候,都是基于地图的。原因如下:

  1. 基于地图有先验的信息,地图的质量是经过检查了的,它没有太多的不可控在里面。而里程计有很多的干扰因素在里面,会导致里程计的不稳定。
  2. 地图更多是一个全局优化得到的结果,误差相比于里程计会更小。

建图流程如下:
多传感器融合定位五-点云地图构建及定位_第1张图片

1. 回环检测

  • 目的:消除累计误差,并提高地图一致性。
  • 方法:从历史帧中找出相似帧(即物理世界中相近位置且有充足共视区域的帧),并给出二者的相对位姿。(注意,不需要完全重合)
  • 用法:在后端优化中,作为一个相对位姿约束的边,加入图优化系统中。

目前比较可靠的方法还是ICP和NDT两个比较基础的方法(小范围可以使用里程计确定是不是在之前帧周边,大范围就需要用到GPS了)。
多传感器融合定位五-点云地图构建及定位_第2张图片

1.1 基于Scan Context

论文:Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map
多传感器融合定位五-点云地图构建及定位_第3张图片
基本思想为,将一圈点云分成很多的扇形,每个扇形在纵向上再切割成小段。三维点云展开,就成了二维数组。下次再走到这里的时候,根据二维数组的匹配相似性,就能判断是不是同一区域了。
缺点:旋转可以计算,但是平移计算不准。

应用案例: LeGO-LOAM + scan context 闭环检测

1.2 基于直方图

论文:[A fast, complete, point cloud based loop closure for LiDAR odometry and mapping]
应用案例:loam_livox
多传感器融合定位五-点云地图构建及定位_第4张图片
将点云按三维网格划分,每个网格求特征值,会有三个特征值。如果将每帧点云跟之前帧匹配计算相似性,可以大致判断出来是不是走到了同一位置。

2. 后端优化

2.1 后端优化基本原理

目的:利用 回环检测结果 和 惯导先验位姿 修正里程计误差,得到一个高质量的地图。回环在此处提供的是两帧之间的相对位姿。
观测

  1. 连续两帧的相对位姿观测;
  2. 回环匹配得到的相对位姿观测;
  3. 组合导航提供的先验位姿观测。

使用方式:
a. 1)和2)的观测构成了基于回环的位姿修正(LeGO-LOAM);
b. 1)和3)的观测构成了基于先验观测的位姿修正;
c. 1) 2) 3) 也可以同时使用。

多传感器融合定位五-点云地图构建及定位_第5张图片

2.2 李群、李代数基本知识

多传感器融合定位五-点云地图构建及定位_第6张图片
常用转换:

  • R = exp ⁡ ( ϕ ∧ ) R=\exp \left(\boldsymbol{\phi}^{\wedge}\right) R=exp(ϕ)
    R − 1 = exp ⁡ ( ( − ϕ ) ∧ ) R^{-1}=\exp \left((-\boldsymbol{\phi})^{\wedge}\right) R1=exp((ϕ))
    ϕ = ln ⁡ ( exp ⁡ ( ϕ ∧ ) ) ∨ = ln ⁡ ( R ) ∨ \boldsymbol{\phi}=\ln \left(\exp \left(\boldsymbol{\phi}^{\wedge}\right)\right)^{\vee}=\ln (R)^{\vee} ϕ=ln(exp(ϕ))=ln(R)
  • T = exp ⁡ ( ξ ∧ ) T=\exp \left(\boldsymbol{\xi}^{\wedge}\right) T=exp(ξ)
    T − 1 = exp ⁡ ( ( − ξ ) ∧ ) T^{-1}=\exp \left((-\boldsymbol{\xi})^{\wedge}\right) T1=exp((ξ))
    ξ = ln ⁡ ( exp ⁡ ( ξ ∧ ) ) ∨ = ln ⁡ ( T ) ∨ \boldsymbol{\xi}=\ln \left(\exp \left(\boldsymbol{\xi}^{\wedge}\right)\right)^{\vee}=\ln (T)^{\vee} ξ=ln(exp(ξ))=ln(T)

2.3 李群、李代数基本知识

在推导里永远无法避免使用 BCH 公式:
ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + ⋯ \ln (\exp (A) \exp (B))=A+B+\frac{1}{2}[A, B]+\frac{1}{12}[A,[A, B]]-\frac{1}{12}[B,[A, B]]+\cdots ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]121[B,[A,B]]+这个公式用来解决一个问题:
已知 T = T 1 ⋅ T 2 T=T_1\cdot T_2 T=T1T2,又知道 T = exp ⁡ ( ξ ∧ ) T=\exp \left(\boldsymbol{\xi}^{\wedge}\right) T=exp(ξ),那么 exp ⁡ ( ξ 1 ∧ ) exp ⁡ ( ξ 2 ∧ ) \exp \left(\boldsymbol{\xi_1}^{\wedge}\right)\exp \left(\boldsymbol{\xi_2}^{\wedge}\right) exp(ξ1)exp(ξ2) 这两项相乘能得到什么呢,或者说,这个 T T T 所对应的李代数,是什么呢?

BCH公式,就是解决的这个问题。这里公式中使用 A A A B B B,是因为这两下既可能是在so(3)下的表达,又可能是在 s e ( 3 ) se(3) se(3)下的表达。这里使用 ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) \ln (\exp (A) \exp (B)) ln(exp(A)exp(B)),就是求它对应的李代数。可以看出来,公式不是只有 A + B A+B A+B,而是多了后面的几项。

  • S O ( 3 ) \mathrm{SO}(3) SO(3) 下的李括号定义为:
    [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ \left[\boldsymbol{\phi}_1, \boldsymbol{\phi}_2\right]=\left(\Phi_1 \Phi_2-\Phi_2 \Phi_1\right)^{\vee} [ϕ1,ϕ2]=(Φ1Φ2Φ2Φ1)其中,
    Φ = ϕ ∧ = [ 0 − ϕ ( 3 ) ϕ ( 2 ) ϕ ( 3 ) 0 − ϕ ( 1 ) − ϕ ( 2 ) ϕ ( 1 ) 0 ] \Phi=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi(3) & \phi(2) \\ \phi(3) & 0 & -\phi(1) \\ -\phi(2) & \phi(1) & 0 \end{array}\right] Φ=ϕ= 0ϕ(3)ϕ(2)ϕ(3)0ϕ(1)ϕ(2)ϕ(1)0

  • S E ( 3 ) \mathrm{SE}(3) SE(3) 下的李括号定义为:
    [ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ \left[\boldsymbol{\xi}_1, \boldsymbol{\xi}_2\right]=\left(\boldsymbol{\xi}_1^{\wedge} \boldsymbol{\xi}_2^{\wedge}-\boldsymbol{\xi}_2^{\wedge} \boldsymbol{\xi}_1^{\wedge}\right)^{\vee} [ξ1,ξ2]=(ξ1ξ2ξ2ξ1)

但是很显然,这个BCH公式太复杂了,那么我们有没有简化的方式来解决它呢?这里根据 so(3) 和 se(3),给出了两个简化的表达。

2.3.1 S O ( 3 ) \mathrm{SO}(3) SO(3) 对应的 B C H \mathrm{BCH} BCH 公式

(小量可以想像成,第 k k k帧和第 k + 1 k+1 k+1帧的相对旋转,而另一个即为坐标系下当前角度)
ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 , 当  ϕ 1  为小量  J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 , 当  ϕ 2  为小量  \ln \left(\exp \left(\boldsymbol{\phi}_1^{\wedge}\right) \exp \left(\phi_2^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} J_l\left(\boldsymbol{\phi}_2\right)^{-1} \boldsymbol{\phi}_1+\boldsymbol{\phi}_2 \text {, 当 } \boldsymbol{\phi}_1 \text { 为小量 } \\ J_r\left(\boldsymbol{\phi}_1\right)^{-1} \boldsymbol{\phi}_2+\boldsymbol{\phi}_1 \text {, 当 } \boldsymbol{\phi}_2 \text { 为小量 } \end{array}\right. ln(exp(ϕ1)exp(ϕ2)){Jl(ϕ2)1ϕ1+ϕ2 ϕ1 为小量 Jr(ϕ1)1ϕ2+ϕ1 ϕ2 为小量 其中左乘雅可比为:(左乘意味着在左边乘上一个小旋转)
J l = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ J_l=\frac{\sin \theta}{\theta} I+\left(1-\frac{\sin \theta}{\theta}\right) a a^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} a^{\wedge} Jl=θsinθI+(1θsinθ)aaT+θ1cosθa即:
J l − 1 = θ 2 cot ⁡ θ 2 I + ( 1 − θ 2 cot ⁡ θ 2 ) a a T − θ 2 a ∧ J_l^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} I+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) a a^T-\frac{\theta}{2} a^{\wedge} Jl1=2θcot2θI+(12θcot2θ)aaT2θa右乘雅可比仅需要在左乘雅可比的基础上对自变量取负号,即:
J r ( ϕ ) = J l ( − ϕ ) J_r(\boldsymbol{\phi})=J_l(-\boldsymbol{\phi}) Jr(ϕ)=Jl(ϕ)

2.3.2 S E ( 3 ) \mathrm{SE}(3) SE(3) 对应的 B C H \mathrm{BCH} BCH 公式

ln ⁡ ( exp ⁡ ( ξ 1 ∧ ) exp ⁡ ( ξ 2 ∧ ) ) ∨ ≈ { J ℓ ( ξ 2 ) − 1 ξ 1 + ξ 2  ,当  ξ 1  为小量  J r ( ξ 1 ) − 1 ξ 2 + ξ 1  ,当  ξ 2  为小量  \ln \left(\exp \left(\boldsymbol{\xi}_1^{\wedge}\right) \exp \left(\boldsymbol{\xi}_2^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} \mathcal{J}_{\ell}\left(\boldsymbol{\xi}_2\right)^{-1} \boldsymbol{\xi}_1+\boldsymbol{\xi}_2 \text { ,当 } \boldsymbol{\xi}_1 \text { 为小量 } \\ \mathcal{J}_r\left(\boldsymbol{\xi}_1\right)^{-1} \boldsymbol{\xi}_2+\boldsymbol{\xi}_1 \text { ,当 } \boldsymbol{\xi}_2 \text { 为小量 } \end{array}\right. ln(exp(ξ1)exp(ξ2)){J(ξ2)1ξ1+ξ2 ,当 ξ1 为小量 Jr(ξ1)1ξ2+ξ1 ,当 ξ2 为小量 由于SE(3)上的雅可比形式过于复杂,此处直接给出本章所用到的近似形式如下。详细内容可参考《机器人中的状态估计》 中公式7.83的推导过程。( J l J_l Jl更复杂)
J r ( ξ ) − 1 ≈ I + 1 2 [ ϕ ∧ ρ ∧ 0 ϕ ∧ ] \mathcal{J}_r(\boldsymbol{\xi})^{-1} \approx I+\frac{1}{2}\left[\begin{array}{cc} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho}^{\wedge} \\ 0 & \boldsymbol{\phi}^{\wedge} \end{array}\right] Jr(ξ)1I+21[ϕ0ρϕ] ξ 1 \boldsymbol{\xi}_1 ξ1 ξ 2 \boldsymbol{\xi}_2 ξ2 非常小,则左右雅可比均可以直接约等于单位阵,即 J r ( ξ ) − 1 ≈ I \mathcal{J}_r(\boldsymbol{\xi})^{-1} \approx I Jr(ξ)1I,此时有
exp ⁡ ( ξ 1 ∧ ) exp ⁡ ( ξ 2 ∧ ) ∨ ≈ exp ⁡ ( ξ 1 ∧ + ξ 1 ∧ ) ∨ \exp \left(\boldsymbol{\xi}_1^{\wedge}\right) \exp \left(\boldsymbol{\xi}_2^{\wedge}\right)^{\vee} \approx \exp \left(\boldsymbol{\xi}_1^{\wedge}+\boldsymbol{\xi}_1^{\wedge}\right)^{\vee} exp(ξ1)exp(ξ2)exp(ξ1+ξ1)

2.4 伴随矩阵

S O ( 3 ) \mathrm{SO}(3) SO(3) 上的伴随性质
exp ⁡ ( p ∧ ) R = R exp ⁡ ( ( R − 1 p ) ∧ ) \exp \left(p^{\wedge}\right) R=R \exp \left(\left(R^{-1} p\right)^{\wedge}\right) exp(p)R=Rexp((R1p)) S E ( 3 ) S E(3) SE(3) 上的伴随性质
exp ⁡ ( ξ ∧ ) T = T exp ⁡ ( ( Ad ⁡ ( T − 1 ) ξ ) ∧ ) \exp \left(\xi^{\wedge}\right) T=T \exp \left(\left(\operatorname{Ad}\left(T^{-1}\right) \xi\right)^{\wedge}\right) exp(ξ)T=Texp((Ad(T1)ξ))其中伴随矩阵定义如下:
Ad ⁡ ( T ) = [ R t ∧ R 0 R ] \operatorname{Ad}(T)=\left[\begin{array}{cc} R & t^{\wedge} R \\ 0 & R \end{array}\right] Ad(T)=[R0tRR]

伴随矩阵的目的是,假设我们有 exp ⁡ ( p ∧ ) R \exp \left(p^{\wedge}\right) R exp(p)R,想对 p p p求偏导,但是 p p p在左边是很难求偏导的,转到右边之后,偏导就比较容易求了。

2.3 基于回环的位姿修正

位姿图优化是把所有的观测和状态放在一起优化,残差项是前面所讲的残差项的总和。在实际使用中,各残差会被分配一个权重,也就是信息矩阵,它相当于对残差进行加权。考虑信息矩阵后,总的残差项可以表示为: F ( x ) = ∑ ⟨ i , j ⟩ ∈ C e i j T Ω i j e i j ⏟ F i j \mathbf{F}(\mathbf{x})=\sum_{\langle i, j\rangle \in \mathcal{C}} \underbrace{\mathbf{e}_{i j}^T \boldsymbol{\Omega}_{i j} \mathbf{e}_{i j}}_{\mathbf{F}_{i j}} F(x)=i,jCFij eijTΩijeij

此时优化问题可以表示为: x ∗ = argmin ⁡ x F ( x ) \mathbf{x}^*=\underset{\mathbf{x}}{\operatorname{argmin}} \mathbf{F}(\mathbf{x}) x=xargminF(x)

多传感器融合定位五-点云地图构建及定位_第7张图片 i \mathrm{i} i 帧和第 j \mathrm{j} j 帧之间的相对位姿,在李群SE(3)上可以表示为: T i j = T i − 1 T j \boldsymbol{T}_{i j}=\boldsymbol{T}_i^{-1} \boldsymbol{T}_j Tij=Ti1Tj
也可以在李代数上表示为:
ξ i j = ln ⁡ ( T i − 1 T j ) ∨ = ln ⁡ ( exp ⁡ ( ( − ξ i ) ∧ ) exp ⁡ ( ξ j ∧ ) ) ∨ \begin{aligned} \boldsymbol{\xi}_{i j} & =\ln \left(\boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee} \\ & =\ln \left(\exp \left(\left(-\boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_j^{\wedge}\right)\right)^{\vee} \end{aligned} ξij=ln(Ti1Tj)=ln(exp((ξi))exp(ξj))若位姿没有误差,则上面两个式子是精确相等的,但当位姿有误差存在时,便可以使用等式的左右两端计算残差项。(也就是,第 i \mathrm{i} i 帧和第 j \mathrm{j} j 帧之间的相对位姿,要添加一个什么样的转换才能到 T i j T_{ij} Tij,要是相等,这里就是一个单位阵 I I I)
e i j = ln ⁡ ( T i j − 1 T i − 1 T j ) ∨ = ln ⁡ ( exp ⁡ ( ( − ξ i j ) ∧ ) exp ⁡ ( ( − ξ i ) ∧ ) exp ⁡ ( ξ j ∧ ) ) ∨ \begin{aligned} \boldsymbol{e}_{i j} & =\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee} \\ & =\ln \left(\exp \left(\left(-\boldsymbol{\xi}_{i j}\right)^{\wedge}\right) \exp \left(\left(-\boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_j^{\wedge}\right)\right)^{\vee} \end{aligned} eij=ln(Tij1Ti1Tj)=ln(exp((ξij))exp((ξi))exp(ξj))位姿图优化的思想是通过调整状态量(即位姿),使残差项的值最小化,这就需要用残差项对位姿求雅可比,才能使用高斯牛顿方法进行优化。(需要对矩阵求导了)

求雅可比的方式是对位姿添加扰动,此时残差表示为:
e ^ i j = ln ⁡ ( T i j − 1 T i − 1 exp ⁡ ( ( − δ ξ i ) ∧ ) exp ⁡ ( δ ξ j ∧ ) T j ) ∨ \hat{\boldsymbol{e}}_{i j}=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \exp \left(\left(-\boldsymbol{\delta} \boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) \boldsymbol{T}_j\right)^{\vee} e^ij=ln(Tij1Ti1exp((δξi))exp(δξj)Tj)这里的扰动都是左扰动 T j \boldsymbol{T}_j Tj 的扰动是 exp ⁡ ( δ ξ j ∧ ) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) exp(δξj),而 T i \boldsymbol{T}_i Ti的在右边是因为这里是求的它的逆。

进一步对前面的式子进行化简:
e ^ i j = ln ⁡ ( T i j − 1 T i − 1 exp ⁡ ( ( − δ ξ i ) ∧ ) exp ⁡ ( δ ξ j ∧ ) T j ) ∨ = ( 1 ) ln ⁡ ( T i j − 1 T i − 1 exp ⁡ ( ( − δ ξ i ) ∧ ) T j exp ⁡ ( ( Ad ⁡ ( T j − 1 ) δ ξ j ) ∧ ) ∨ = ( 2 ) ln ⁡ ( T i j − 1 T i − 1 T j exp ⁡ ( ( − Ad ⁡ ( T j − 1 ) δ ξ i ) ∧ ) exp ⁡ ( ( Ad ⁡ ( T j − 1 ) δ ξ j ) ∧ ) ∨ ≈ ( 3 ) ln ⁡ ( T i j − 1 T i − 1 T j exp ⁡ ( ( − Ad ⁡ ( T j − 1 ) δ ξ i ) ∧ + ( Ad ⁡ ( T j − 1 ) δ ξ j ) ∧ ) ∨ = ( 4 ) ln ⁡ ( exp ⁡ ( e i j ∧ ) exp ⁡ ( ( − Ad ⁡ ( T j − 1 ) δ ξ i ) ∧ + ( Ad ⁡ ( T j − 1 ) δ ξ j ) ∧ ) ∨ ≈ ( 5 ) e i j − J r − 1 ( e i j ) Ad ⁡ ( T j − 1 ) δ ξ i + J r − 1 ( e i j ) Ad ⁡ ( T j − 1 ) δ ξ j \begin{aligned} \hat{e}_{i j}&=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \exp \left(\left(-\delta \boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) \boldsymbol{T}_j\right)^{\vee} \\ &\stackrel{(1)}{=} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \exp \left(\left(-\delta \boldsymbol{\xi}_i\right)^{\wedge}\right) \boldsymbol{T}_j \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(2)}{=} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(3)}{\approx} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}+\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(4)}{=} \ln \left(\exp \left(\boldsymbol{e}_{i j}^{\wedge}\right) \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}+\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(5)}{\approx} \boldsymbol{e}_{i j}-\mathcal{J}_r^{-1}\left(e_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i+\mathcal{J}_r^{-1}\left(e_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_j \end{aligned} e^ij=ln(Tij1Ti1exp((δξi))exp(δξj)Tj)=(1)ln(Tij1Ti1exp((δξi))Tjexp((Ad(Tj1)δξj))=(2)ln(Tij1Ti1Tjexp((Ad(Tj1)δξi))exp((Ad(Tj1)δξj))(3)ln(Tij1Ti1Tjexp((Ad(Tj1)δξi)+(Ad(Tj1)δξj))=(4)ln(exp(eij)exp((Ad(Tj1)δξi)+(Ad(Tj1)δξj))(5)eijJr1(eij)Ad(Tj1)δξi+Jr1(eij)Ad(Tj1)δξj

首先要将两个exp放到最右边去:
(1) 三维变换下的伴随性质,先将 exp ⁡ ( δ ξ j ∧ ) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) exp(δξj) 移到右边去;
(2) 三维变换下的伴随性质,这次是将 exp ⁡ ( δ ξ i ∧ ) \exp \left(\delta \boldsymbol{\xi}_i^{\wedge}\right) exp(δξi) 移到右边去;
(3) B C H \mathrm{BCH} BCH 公式,且是两个李代数都比较小情况下的近似。就是两个李代数对应都非常小的时候,可以使用 exp ⁡ ( ξ 1 ∧ ) exp ⁡ ( ξ 2 ∧ ) ∨ ≈ exp ⁡ ( ξ 1 ∧ + ξ 1 ∧ ) ∨ \exp \left(\boldsymbol{\xi}_1^{\wedge}\right) \exp \left(\boldsymbol{\xi}_2^{\wedge}\right)^{\vee} \approx \exp \left(\boldsymbol{\xi}_1^{\wedge}+\boldsymbol{\xi}_1^{\wedge}\right)^{\vee} exp(ξ1)exp(ξ2)exp(ξ1+ξ1)的形式。
(4) 残差的定义,即 e i j = ln ⁡ ( T i j − 1 T i − 1 T j ) ∨ \boldsymbol{e}_{i j} =\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee} eij=ln(Tij1Ti1Tj)
(5) BCH 公式

上面的式子表明,残差关于 T i T_i Ti 的雅可比为:
A i j = ∂ e i j ∂ δ ξ i = − J r − 1 ( e i j ) Ad ⁡ ( T j − 1 ) A_{i j}=\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_i}=-\mathcal{J}_r^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) Aij=δξieij=Jr1(eij)Ad(Tj1)残差关于 T j T_j Tj 的雅可比为:
B i j = ∂ e i j ∂ δ ξ j = J r − 1 ( e i j ) Ad ⁡ ( T j − 1 ) B_{i j}=\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_j}=\mathcal{J}_r^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) Bij=δξjeij=Jr1(eij)Ad(Tj1)其中:
J r − 1 ( e i j ) ≈ I + 1 2 [ ϕ e ∧ ρ e ∧ 0 ϕ e ∧ ] \mathcal{J}_r^{-1}\left(\boldsymbol{e}_{i j}\right) \approx I+\frac{1}{2}\left[\begin{array}{cc} \boldsymbol{\phi}_e^{\wedge} & \boldsymbol{\rho}_e^{\wedge} \\ 0 & \boldsymbol{\phi}_e^{\wedge} \end{array}\right] Jr1(eij)I+21[ϕe0ρeϕe]

现在有了雅克比了,剩下的就是高斯牛顿的流程了。
按照高斯牛顿法的流程,需要对残差进行一阶泰勒展开, 即求雅可比:
e i j ( x i + Δ x i , x j + Δ x j ) = e i j ( x + Δ x ) ≈ e i j + J i j Δ x \begin{array}{l} \mathbf{e}_{i j}\left(\mathbf{x}_i+\Delta \mathbf{x}_i, \mathbf{x}_j+\Delta \mathbf{x}_j\right) \\ =\mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ \approx \mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x} \end{array} eij(xi+Δxi,xj+Δxj)=eij(x+Δx)eij+JijΔx
其中 J i j \mathbf{J}_{i j} Jij 即为前面推导的残差关于位姿的雅可比组成的矩阵:
J i j = ( 0 ⋯ 0 A i j ⏟ node  i 0 ⋯ 0 B i j ⏟ node  j 0 ⋯ 0 ) \mathbf{J}_{i j}=(0 \cdots 0 \underbrace{\mathbf{A}_{i j}}_{\text {node } i} \mathbf{0} \cdots 0 \underbrace{\mathbf{B}_{i j}}_{\text {node } j} \mathbf{0} \cdots 0) Jij=(00node i Aij00node j Bij00)对于每一个残差块,便有:
F i j ( x + Δ x ) = e i j ( x + Δ x ) T Ω i j e i j ( x + Δ x ) ≈ ( e i j + J i j Δ x ) T Ω i j ( e i j + J i j Δ x ) = e i j T Ω i j e i j ⏟ c i j + 2 e i j T Ω i j J i j ⏟ b i j T Δ x + Δ x T J i j T Ω i j J i j ⏟ H i j Δ x = c i j + 2 b i j T Δ x + Δ x T H i j Δ x \begin{array}{l} \mathbf{F}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ =\mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x})^T \Omega_{i j} \mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ \approx\left(\mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x}\right)^T \Omega_{i j}\left(\mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x}\right) \\ =\underbrace{\mathbf{e}_{i j}^T \Omega_{i j} \mathbf{e}_{i j}}_{c_{i j}}+2 \underbrace{\mathbf{e}_{i j}^T \Omega_{i j} \mathbf{J}_{i j}}_{\mathbf{b}_{i j}^T} \Delta \mathbf{x}+\Delta \mathbf{x}^T \underbrace{\mathbf{J}_{i j}^T \Omega_{i j} \mathbf{J}_{i j}}_{\mathbf{H}_{i j}} \Delta \mathbf{x} \\ =c_{i j}+2 \mathbf{b}_{i j}^T \Delta \mathbf{x}+\Delta \mathbf{x}^T \mathbf{H}_{i j} \Delta \mathbf{x} \\ \end{array} Fij(x+Δx)=eij(x+Δx)TΩijeij(x+Δx)(eij+JijΔx)TΩij(eij+JijΔx)=cij eijTΩijeij+2bijT eijTΩijJijΔx+ΔxTHij JijTΩijJijΔx=cij+2bijTΔx+ΔxTHijΔx其中:
H i j = ( ⋱ A i j T Ω i j   A i j ⋯ A i j T Ω i j   B i j ⋮ ⋱ ⋮ B i j T Ω i j   A i j ⋯ B i j T Ω i j   B i j ⋱ ) b i j = ( ⋮ A i j T Ω i j e i j ⋮ B i j T Ω i j e i j ⋮ ) \mathbf{H}_{i j}=\left(\begin{array}{cccc} \ddots & & & \\ & \mathbf{A}_{i j}^T \Omega_{i j} \mathrm{~A}_{i j} & \cdots & \mathbf{A}_{i j}^T \Omega_{i j} \mathrm{~B}_{i j} \\ \vdots & \ddots & \vdots & \\ & \mathrm{B}_{i j}^T \Omega_{i j} \mathrm{~A}_{i j} & \cdots & \mathrm{B}_{i j}^T \Omega_{i j} \mathrm{~B}_{i j} \\ \ddots & & & \end{array}\right) \quad \mathrm{b}_{i j}=\left(\begin{array}{c} \vdots \\ \mathrm{A}_{i j}^T \Omega_{i j} \mathrm{e}_{i j} \\ \vdots \\ \mathrm{B}_{i j}^T \Omega_{i j} \mathrm{e}_{i j} \\ \vdots \end{array}\right) Hij= AijTΩij AijBijTΩij AijAijTΩij BijBijTΩij Bij bij= AijTΩijeijBijTΩijeij
此后便可以使用高斯牛顿法进行优化。

2.4 基于先验观测的位姿修正

先验观测是一元边,它不像前面所述的帧间观测连接两个 位姿状态,而是只连接一个位姿状态量,它直接给出的就 是该状态量的观测值,因此它对应的残差就是观测值与状 态量之间的差异,即
e i = ln ⁡ ( Z i − 1 T i ) ∨ = ln ⁡ ( exp ⁡ ( ( − ξ z i ) ∧ ) exp ⁡ ( ξ i ∧ ) ) ∨ \begin{aligned} \boldsymbol{e}_i & =\ln \left(\boldsymbol{Z}_i^{-1} \boldsymbol{T}_i\right)^{\vee} \\ & =\ln \left(\exp \left(\left(-\boldsymbol{\xi}_{z i}\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_i^{\wedge}\right)\right)^{\vee} \end{aligned} ei=ln(Zi1Ti)=ln(exp((ξzi))exp(ξi))
对残差添加扰动,可得
e ^ i = ln ⁡ ( Z i − 1 exp ⁡ ( δ ξ i ∧ ) T i ) ∨ \hat{\boldsymbol{e}}_i=\ln \left(\boldsymbol{Z}_i^{-1} \exp \left(\delta \boldsymbol{\xi}_i^{\wedge}\right) \boldsymbol{T}_i\right)^{\vee} e^i=ln(Zi1exp(δξi)Ti)
利用伴随性质和 B C H \mathrm{BCH} BCH 公式进行化简,可得
e ^ i = ln ⁡ ( Z i − 1 T i exp ⁡ ( ( Ad ⁡ ( T i − 1 ) δ ξ i ) ∧ ) ) ∨ = ln ⁡ ( exp ⁡ ( e i ∧ ) exp ⁡ ( ( Ad ⁡ ( T i − 1 ) δ ξ i ) ∧ ) ) ∨ ≈ e i + J r − 1 ( e i ) Ad ⁡ ( T i − 1 ) δ ξ i \begin{aligned} \hat{\boldsymbol{e}}_i & =\ln \left(\boldsymbol{Z}_i^{-1} \boldsymbol{T}_i \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}\right)\right)^{\vee} \\ & =\ln \left(\exp \left(\boldsymbol{e}_i^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}\right)\right)^{\vee} \\ & \approx e_i+\mathcal{J}_r^{-1}\left(\boldsymbol{e}_i\right) \operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) \delta \boldsymbol{\xi}_i \end{aligned} e^i=ln(Zi1Tiexp((Ad(Ti1)δξi)))=ln(exp(ei)exp((Ad(Ti1)δξi)))ei+Jr1(ei)Ad(Ti1)δξi因此残差关于 T i T_i Ti 的雅可比为
∂ e i ∂ δ ξ i = J r − 1 ( e i ) Ad ⁡ ( T i − 1 ) \frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_i}=\mathcal{J}_r^{-1}\left(\boldsymbol{e}_i\right) \operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) δξiei=Jr1(ei)Ad(Ti1)其中:
J r − 1 ( e i ) ≈ I + 1 2 [ ϕ e ∧ ρ e ∧ 0 ϕ e ∧ ] \mathcal{J}_r^{-1}\left(e_i\right) \approx I+\frac{1}{2}\left[\begin{array}{cc} \phi_e^{\wedge} & \boldsymbol{\rho}_e^{\wedge} \\ 0 & \phi_e^{\wedge} \end{array}\right] Jr1(ei)I+21[ϕe0ρeϕe]后面的推导过程,便与相对位姿做观测的推导过程完全一致。

此外,部分场合提供的观测只有位置,没有姿态,比如只 有RTK,而没有组合导航,这里的残差便只剩下位置误差。 相应的雅可比公式,可自行推导。

3. 点云地图建立

3.1 整体流程

建图流程设计的核心原则是准确、高效地把里程计相对位姿、回环相对位姿、惯导先验位姿进行融合。
多传感器融合定位五-点云地图构建及定位_第8张图片
轨迹对齐:比如有个GPS轨迹和一个激光里程计,两个轨迹没有在一起,是因为初始的激光里程计是以单位阵为原点的,它与GPS之间没有产生过任何相对位姿联系。解决这个问题的方法是,把整个轨迹做调整,把激光里程计的轨迹旋转到GPS轨迹附近。
添加先验约束:如果没有检测到回环,需要添加GPS的先验位姿

4. 基于地图的定位

4.1 整体流程

在地图匹配中,鲁棒性和运行速度更加重要,因此实际使用中,基于NDT的匹配使用更广泛。
多传感器融合定位五-点云地图构建及定位_第9张图片
获取初始信息:假如有一个地图,这个地图很大,有可能在任何位置,需要有一个东西告诉你你在地图的哪个大致位置,如有GPS告诉我在一个位置,这时就需要把这附近一块区域的地图索引出来,索引出来后将当前雷达的点云和地图做匹配。为什么叫索引呢?因为不索引的话,就需要和整张地图做匹配了,这时不现实的,所以需要和局部地图做匹配,因此就加载一小块局部地图。

位姿初始化:在正式匹配之前,需要做一个初始化。第一次匹配前,需要多这么一个步骤。后面再做匹配时,就不需要这样做了,因为有了上一帧的位姿做参考。

更新局部地图:在地图上肯定是要沿着轨迹走的,上次加载的局部地图在一个位置,走到后面时,跟前面的局部地图就匹配不上了,因此需要拿另一块局部地图来做匹配。

4.2 位姿初始化

4.2.1 整体流程

由于NDT匹配需要较准确的初始位姿,因此在定位之前需要初始化环节,给出载体的初始位姿。
按照难度由低到高,常见的初始化需求有这样几种:

  1. 已知位姿的初始化
    2)位置已知而姿态位置的初始化
    3)位置和姿态均未知的初始化

4.2.2 位姿初始化

由于NDT匹配需要较准确的初始位姿,因此在定位之前需要初始化环节,给出载体的初始位姿。

按照难度由低到高,常见的初始化需求有这样几种:
1)已知位姿的初始化
2)位置已知而姿态位置的初始化
3)位置和姿态均未知的初始化(非常有难度)

5. LeGO-LOAM介绍

论文:LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

5.1 主要特点

  1. 对地面做了分割,减小了特征搜索范围(环境中找线特征,地面找面特征);
  2. 提取特征之前做了聚类,提高了特征质量(很多曲率大的地方都是杂点,甚至树叶上的点都当成线特征来使用了),这里删除掉了环境中比较杂乱的点,即不形成足够数量的点;
  3. 以帧为单位进行优化,使得全局地图可以多次调整,而不像LOAM那样不可修改;
  4. 增加了回环修正。

多传感器融合定位五-点云地图构建及定位_第10张图片

5.2 特征提取

  1. 根据线与线之间的夹角,以及点的曲率,筛选出地面点。所有用于匹配的平面点仅使用地面点。
  2. 在非地面点中,使用广度优先搜索(BFS)做聚类,聚类中点的数量大于30,才用来筛选线特征。
  3. 筛选线特征方法,与LOAM中相同。

多传感器融合定位五-点云地图构建及定位_第11张图片

5.3 位姿解算

LeGO-LOAM 相较于之前的 LOAM 有个很重要的区别,它把6dof拆成了两个3dof。从理论上讲,面特征既然都提自于地面,那么它确实只能约束两个水平角和z向的距离;线特征也一样,它不在地面只分配在周围,而且大多数线特征都是竖直向上的,那它也确实只能约束航向角以及x、y的距离。这个理论上,从6dof拆成3dof和周围环境结合起来分析,它在精度上是没有损失的(或者说损失较少?),但是效率上会大幅提升。

  1. 使用地面面特征优化高度和水平角;
  2. 使用线特征优化水平位移和航向角。

注意:实际代码中,只有odom环节(scan-to-scan)使用了该模式,在mapping环节(scan-to-map)仍使用的是六自由度模型
多传感器融合定位五-点云地图构建及定位_第12张图片

5.4 回环检测与修正

  1. 回环检测使用的初始位姿,采用前端里程计位姿
  2. 匹配采用 ICP 方法
  3. 优化库采用 gtsam

你可能感兴趣的:(SLAM,激光雷达,人工智能,网络,算法)