3D激光雷达SLAM算法学习03——前端里程计方案

1.本篇大纲

3D激光雷达SLAM算法学习03——前端里程计方案_第1张图片

2.前端里程计综述

解决的问题:由原始点云信息求解得到两个关键帧之间的相对位姿约束
输入:原始点云信息
输出:两个时刻之间的位姿关系 P = [R T] (4x4)
主流流程: 点云预处理(提取特征)→建立Loss模型→最优化模型求解→得到P矩阵

3.直接匹配的ICP系列

**1)思路:**对原始点云进行采样→寻找最近关联点建立Loss模型→求解R,T
2)主要方法框图:
3D激光雷达SLAM算法学习03——前端里程计方案_第2张图片
3)模型推导
下面以其中点到点+SVD分解求解为例,进行模型建立和推导
loss模型:
① E ( R , T ) = 1 N ∑ i = 1 N ∣ ∣ x i − R y i − T ∣ ∣ 2 ① E(R,T) = \frac{1}{N} \sum_{i =1}^{N}||x_{i}-Ry_{i}-T||^{2} E(R,T)=N1i=1N∣∣xiRyiT2
X = x 1 , x 2 , . . . , x N X = {x_{1},x_{2},...,x_{N}} X=x1,x2,...,xN Y = y 1 , y 2 , . . . , y N Y = {y_{1},y_{2},...,y_{N}} Y=y1,y2,...,yN
其中N表示点云预处理后点云个数,X,Y为前后两个关键帧的点云,其中假设x1和y1为最邻近点
Loss模型的思路:是找一个位姿变换关系(用R,T体现)使两个点云中各对对应点尽可能重合,也就是让E(R,T)最小。
求解推导:
② E ( R , T ) = 1 N ∑ i = 1 N ∣ ∣ x i − R y i − T − u x + R u y + u x − R y ∣ ∣ 2 ②E(R,T) = \frac{1}{N} \sum_{i =1}^{N}||x_{i}-Ry_{i}-T-u_{x}+Ru_{y}+u_x-R_{y}||^{2} E(R,T)=N1i=1N∣∣xiRyiTux+Ruy+uxRy2
③ E ( R , T ) = 1 N ∑ i = 1 N ( ∣ ∣ x i − u x − R ( y i − u y ) ∣ ∣ 2 + ∣ ∣ u x − R u y − T ∣ ∣ 2 ) ③E(R,T) = \frac{1}{N} \sum_{i =1}^{N}(||x_{i}-u_{x}-R(y_{i}-u_{y})||^{2}+||u_{x}-Ru_{y}-T||^{2}) E(R,T)=N1i=1N(∣∣xiuxR(yiuy)2+∣∣uxRuyT2)

由①到②是一步构造,目的就是为了转化成③,展开说说:
其中 u x u_{x} ux表示X点云的均值,直观理解就是中心,最后转化成③的形式可以看出来,Loss模型分为前后两部分,前部分只与R有关,后部分当已知R总可以找到一个T使该部分为0,所以整体思路变成先求解前部分求出R,然后用已知R令后部分为0求出T
③中令 x i ′ = x i − u x x_{i}^{'} = x_i-u_x xi=xiux 表示“去除”自身中心后的点云,这个 x 1 ′ x_{1}^{'} x1可以求出来
可以得到: ④ E 1 ( R , T ) = 1 N ∑ i = 1 N ∣ ∣ x i ′ − R ( y i ′ ) ∣ ∣ 2 ④E_1(R,T) =\frac{1}{N} \sum_{i =1}^{N}||x_{i}^{'}-R(y_{i}^{'})||^{2} E1(R,T)=N1i=1N∣∣xiR(yi)2
至此这个问题被完全转化为数学问题,剩下的公式推导基本就是SVD分解的部分,只写出公式不做讲解
m i n ( E 1 ( R , T ) ) = m i n ( 1 N ∑ i = 1 N ∣ ∣ x i ′ − R ( y i ′ ) ∣ ∣ 2 ) = m i n ( 1 N ∑ i = 1 N ( x i ′ T x i ′ − y i ′ T R T R y i ′ − 2 x i T ′ R y i ′ ) ) = m a x ( 1 N ∑ i = 1 N ( 2 x i T ′ R y i ′ ) ) = m a x ( ∑ i = 1 N ( x i T ′ R y i ′ ) ) = m a x ( ∑ i = 1 N T r a c e ( x i T ′ R y i ′ ) ) = m a x ( ∑ i = 1 N T r a c e ( R y i ′ x i T ′ ) ) = m a x ( T r a c e ( R H ) ) H = ∑ i = 1 N y i ′ x i min(E_1(R,T)) =min(\frac{1}{N} \sum_{i =1}^{N}||x_{i}^{'}-R(y_{i}^{'})||^{2}) \newline=min(\frac{1}{N} \sum_{i =1}^{N}(x_{i}^{'T}x_{i}^{'}-y_{i}^{'T}R^TRy_{i}^{'}-2x_i^{T'}Ry_i^{'})) \newline=max(\frac{1}{N} \sum_{i =1}^{N}(2x_i^{T'}Ry_i^{'})) \newline = max(\sum_{i =1}^{N}(x_i^{T'}Ry_i^{'})) \newline = max(\sum_{i =1}^{N}Trace(x_i^{T'}Ry_i^{'})) \newline = max(\sum_{i =1}^{N}Trace(Ry_i^{'}x_i^{T'})) \newline= max(Trace(RH)) \newline H = \sum_{i =1}^{N}y_i^{'}x_i min(E1(R,T))=min(N1i=1N∣∣xiR(yi)2)=min(N1i=1N(xiTxiyiTRTRyi2xiTRyi))=max(N1i=1N(2xiTRyi))=max(i=1N(xiTRyi))=max(i=1NTrace(xiTRyi))=max(i=1NTrace(RyixiT))=max(Trace(RH))H=i=1Nyixi
至此问题转化为找到一个R使得Trace(RH)最大,那其实就是矩阵分析的内容,就是让RH可以分解成 A T A A^TA ATA的形式,这部分就不写了
最后 T = u x − R u y T = u_{x}-Ru_{y} T=uxRuy

4.直接匹配的NDT系列

1)思路:点云预处理→空间划分栅格,计算每个栅格内点云的均值+协方差→由联合概率构建Loss函数→求解R,T
思路讲解:NDT的整体思路也是,找到一个位姿关系R,T,使得Y点云在R旋转T平移后和X点云进可能的重合,与ICP不同的是描述重合的Loss函数,NDT用点云分布的概率密度来描述,将联合概率作为Loss函数,举一个简单的一维例子:
10个点云,由四个分布在<0,六个分布>0,如果你求解的RT是对的,那么对应点可以对应上,那么联合概率相当于0.40.4+0.60.6 = 0.52
如果你求解RT正好“完全不对”,那么联合概率相当于0.40.6+0.60.4 = 0.48<0.52
(这个一个简化理解的案例,不代表公式推导)
2)主要方法框图:
3D激光雷达SLAM算法学习03——前端里程计方案_第3张图片

3)模型推导
loss模型
X = x 1 , x 2 , . . . , x N X = {x_{1},x_{2},...,x_{N}} X=x1,x2,...,xN
Y = y 1 , y 2 , . . . , y N Y = {y_{1},y_{2},...,y_{N}} Y=y1,y2,...,yN
NDT的推导部分主要是方便理解思路,忽略数学概率部分的推导。
划分栅格的目的是将点云分布用正态分布的形式进行描述,假设将点云X分为一个3X3X3的栅格(像一个最简单的魔方),那么假设第一个栅格内点云个数为 n u m 1 num_1 num1,注意 ∑ j = 1 27 ( n u m i ) = N \sum_{j=1}^{27}( {num_i})=N j=127(numi)=N,在第一个栅格内,进行均值和协方差求解
u 1 = 1 n u m i ∑ i = 1 n u m 1 x i u_1 = \frac{1}{num_i}\sum_{i=1}^{num_1}x_i u1=numi1i=1num1xi
∑ 1 = 1 n u m 1 − 1 ∑ i = 1 n u m 1 ( x i − u ) ( x i − u ) T {\sum}_1 = \frac{1}{num_1-1}\sum_{i=1}^{num_1}(x_i-u)(x_i-u)^T 1=num111i=1num1(xiu)(xiu)T

最直观的思路是对Y点云也进行如下求解,然后对 u 1 x u_1^x u1x, u 1 y u_1^y u1y, ∑ 1 x {\sum}_1^x 1x ∑ 1 y {\sum}_1^y 1y做联合概率计算,但是实际上不是这样处理,在求出第一个栅格内 u 1 x u_1^x u1x ∑ 1 x {\sum}_1^x 1x后,直接用Y点云内第一个栅格的点云求解概率公式 如下
f ( X , y i ′ ) = 1 2 π ∣ ∑ ∣ e x p ( − ( y i ′ − u 1 x ) T ∑ − 1 ( y i ′ − u 1 x ) 2 ) f(X,y_i^{'}) = \frac{1}{\sqrt{2\pi }\sqrt{\left | \sum \right |}}exp(-\frac{(y_i^{'}-u_1^x)^T\sum ^{-1}(y_i^{'}-u_1^x)}{2}) f(X,yi)=2π 1exp(2(yiu1x)T1(yiu1x))
上边的公式是一个激光点的,那么对于第一个栅格的联合概率就是把 n u m 1 num_1 num1 个点的 f ( X , y i ′ ) f(X,y_i^{'}) f(X,yi)乘在一起,那么在把27个栅格的概率乘在一起,最终联合概率为
Ψ = ∏ i = 1 N f ( X , y i ′ ) = ∏ i = 1 N 1 2 π ∣ ∑ ∣ e x p ( − ( y i ′ − u 1 x ) T ∑ − 1 ( y i ′ − u 1 x ) 2 ) \Psi = \prod_{i=1}^{N}f(X,y_i^{'}) = \prod_{i=1}^{N}\frac{1}{\sqrt{2\pi }\sqrt{\left | \sum \right |}}exp(-\frac{(y_i^{'}-u_1^x)^T\sum ^{-1}(y_i^{'}-u_1^x)}{2}) Ψ=i=1Nf(X,yi)=i=1N2π 1exp(2(yiu1x)T1(yiu1x))

m a x ( Ψ = m i n ( ∑ i = 1 N y i ′ − u j x ) T ∑ j − 1 ( y i ′ − u j x ) max(\Psi= min(\sum _{i=1}^{N}y_i^{'}-u_j^x)^T\sum^{-1}_j(y_i^{'}-u_j^x) max(Ψ=min(i=1Nyiujx)Tj1(yiujx)
u j x u_j^x ujx, ∑ j − 1 \sum^{-1}_j j1表示点云 y i ′ y_i^{'} yi所在栅格j对应的均值和方差,这个等式省略了中间推导过程。
求解推导
最终得到Loss模型为:
目标函数 m i n ( ∑ i = 1 N y i ′ − u j x ) T ∑ j − 1 ( y i ′ − u j x ) min(\sum _{i=1}^{N}y_i^{'}-u_j^x)^T\sum^{-1}_j(y_i^{'}-u_j^x) min(i=1Nyiujx)Tj1(yiujx),求解合适的RT使得loss模型最小。
其中: y i ′ = R y i + t y_i^{'} = Ry_i+t yi=Ryi+t 将Y点云通过RT变换后的对应位置
从loss函数和ICP的Loss函数对比,可以看出NDT的计算量小于ICP系列,因为NDP只需要遍历一个Y点云的所有激光点,X点云用u和 ∑ \sum 的形式描述。
e i ( p ) = y i ′ − u e_i(p) = y_i^{'}-u ei(p)=yiu
F i ( p ) = e i T ( p ) ∑ − 1 e i ( p ) F_i(p) = e_i^T(p)\sum^{-1} e_i(p) Fi(p)=eiT(p)1ei(p)
m i n ( ∑ i = 1 N y i ′ − u j x ) T ∑ j − 1 ( y i ′ − u j x ) = m i n ∑ i = 1 N F i ( p ) min(\sum _{i=1}^{N}y_i^{'}-u_j^x)^T\sum^{-1}_j(y_i^{'}-u_j^x)= min\sum _{i=1}^{N}F_i(p) min(i=1Nyiujx)Tj1(yiujx)=mini=1NFi(p)
采取迭代优化的方法
m i n ∑ i = 1 N F i ( p + Δ p ) = m i n ( ∑ i = 1 N e i T ( p + Δ p ) ∑ − 1 e i ( p + Δ p ) ) min\sum _{i=1}^{N}F_i(p+\Delta p)=min(\sum _{i=1}^{N} e_i^T(p+\Delta p)\sum^{-1} e_i(p+\Delta p)) mini=1NFi(p+Δp)=min(i=1NeiT(p+Δp)1ei(p+Δp))
F i ( p + Δ p ) = F i ( p ) + 2 b i T Δ p + Δ p T H i Δ p F_i(p+\Delta p)=F_i(p)+2b_i^T\Delta p+\Delta p^TH_i\Delta p Fi(p+Δp)=Fi(p)+2biTΔp+ΔpTHiΔp
其中 b i T = e i T ∑ − 1 J i b_i^T = e_i^T\sum^{-1}J_i biT=eiT1Ji , H i = J i T ∑ − 1 J i H_i = J_i^T\sum^{-1}J_i Hi=JiT1Ji

迭代优化的更新值为:
Δ F i ( p ) = F i ( p + Δ p ) − F i ( p ) = 2 b i T Δ p + Δ p T H i Δ p \Delta F_i(p) = F_i(p+\Delta p)- F_i(p)=2b_i^T\Delta p+\Delta p^TH_i\Delta p ΔFi(p)=Fi(p+Δp)Fi(p)=2biTΔp+ΔpTHiΔp

5.参考

参考:深蓝学院任乾老师课程

你可能感兴趣的:(3D激光雷达SLAM,算法,3d,学习,Slam,激光雷达)