多传感器融合定位二-3D激光里程计其二：NDT
Reference:
文章跳转：
- 多传感器融合定位一-3D激光里程计其一：ICP
- 多传感器融合定位二-3D激光里程计其二：NDT
- 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
- 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
- 多传感器融合定位五-点云地图构建及定位
- 多传感器融合定位六-惯性导航原理及误差分析
- 多传感器融合定位七-惯性导航解算及误差分析其一
- 多传感器融合定位八-惯性导航解算及误差分析其二
- 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
- 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
- 多传感器融合定位十一-基于滤波的融合方法Ⅱ
- 多传感器融合定位十二-基于图优化的建图方法其一
- 多传感器融合定位十三-基于图优化的建图方法其二
- 多传感器融合定位十四-基于图优化的定位方法
- 多传感器融合定位十五-多传感器时空标定(综述)
1. 经典NDT
NDT(Normal Distributions Transform，正态分布变换) 核心思想：基于概率的匹配。目标是将点集
Y
Y
Y 匹配到固定的点集
X
X
X 中。这里的联合概率说的是将
X
X
X 划分成栅格(如下图)，每个栅格里面都有很多点，这些点可以计算一个 均值&&协方差，这是一个概率的概念。而联合概率是：
Y
Y
Y 往
X
X
X 上旋转的时候，它落在栅格中的哪个格子里是知道的。根据落在格子里的点，本身
X
X
X 格子已经有了一个 均值/协方差，而
Y
Y
Y 的这些落在格子里的点可以形成一个联合概率。这个联合概率会作为有没匹配好的一个指标。
点集：
X
=
{
x
1
,
x
2
,
⋯
,
x
N
x
}
Y
=
{
y
1
,
y
2
,
⋯
,
y
N
y
}
\begin{aligned} & X=\left\{x_1, x_2, \cdots, x_{N_x}\right\} \\ & Y=\left\{y_1, y_2, \cdots, y_{N_y}\right\} \end{aligned}
X={x1,x2,⋯,xNx}Y={y1,y2,⋯,yNy}目标：
max
Ψ
=
max
∏
i
=
1
N
y
f
(
X
,
T
(
p
,
y
i
)
)
\max \Psi=\max \prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right)
maxΨ=max∏i=1Nyf(X,T(p,yi))(与 ICP 这里有些区别，ICP 的是点到点的距离，这里变成了联合概率)
2D模型：
p
=
p
3
=
[
t
x
t
y
ϕ
z
]
T
p=p_3=\left[\begin{array}{lll}t_x & t_y & \phi_z\end{array}\right]^{\mathrm{T}}
p=p3=[txtyϕz]T
3D模型：
p
=
p
6
=
[
t
x
t
y
t
z
ϕ
x
ϕ
y
ϕ
z
]
T
p=p_6=\left[\begin{array}{llllll}t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}\right]^{\mathrm{T}}
p=p6=[txtytzϕxϕyϕz]T
2. 计算方式
均值和协方差：
μ
=
1
N
x
∑
i
=
1
N
x
x
i
,
Σ
=
1
N
x
−
1
∑
i
=
1
N
x
(
x
i
−
μ
)
(
x
i
−
μ
)
T
\mu=\frac{1}{N_x} \sum_{i=1}^{N_x} x_i, \quad \boldsymbol{\Sigma}=\frac{1}{N_x-1} \sum_{i=1}^{N_x}\left(x_i-\mu\right)\left(x_i-\mu\right)^{\mathrm{T}}
μ=Nx1i=1∑Nxxi,Σ=Nx−11i=1∑Nx(xi−μ)(xi−μ)T根据预测的位姿，对点进行旋转和平移(这里的旋转和平移是一个初始预测值)：
y
i
′
=
T
(
p
,
y
i
)
=
R
y
i
+
t
y_i^{\prime}=T\left(p, y_i\right)=R y_i+t
yi′=T(p,yi)=Ryi+t旋转和平移后的点与目标点集中的点在同一坐标系下，此时可计算各点的联合概率：
f
(
X
,
y
i
′
)
=
1
2
π
∣
Σ
∣
exp
(
−
(
y
i
′
−
μ
)
T
Σ
−
1
(
y
i
′
−
μ
)
2
)
f\left(X, y_i^{\prime}\right)=\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right)
f(X,yi′)=2π∣Σ∣1exp(−2(yi′−μ)TΣ−1(yi′−μ))所有点的联合概率(就是所有点的概率乘一起)：
Ψ
=
∏
i
=
1
N
y
f
(
X
,
T
(
p
,
y
i
)
)
=
∏
i
=
1
N
y
1
2
π
∣
Σ
∣
exp
(
−
(
y
i
′
−
μ
)
T
Σ
−
1
(
y
i
′
−
μ
)
2
)
\begin{aligned} \Psi & =\prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) \\ & =\prod_{i=1}^{N_y} \frac{1}{\sqrt{2 \pi} \sqrt{|\mathbf{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) \end{aligned}
Ψ=i=1∏Nyf(X,T(p,yi))=i=1∏Ny2π∣Σ∣1exp(−2(yi′−μ)TΣ−1(yi′−μ))目标是让所有点的联合概率最大。但是上式中的 R 和 t 是在
y
i
′
y'_i
yi′ 内的。这里还有一个exp，这个指数项会让公式变得复杂，需要消掉指数项。这时去对数可以解决。
取对数，简化问题(目标是让所有点的联合概率最大)：
ln
Ψ
=
∑
i
=
1
N
y
(
−
(
y
i
′
−
μ
)
T
Σ
−
1
(
y
i
′
−
μ
)
2
+
ln
(
1
2
π
∣
Σ
∣
)
)
\ln \Psi=\sum_{i=1}^{N_y}\left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}+\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)\right)
lnΨ=i=1∑Ny(−2(yi′−μ)TΣ−1(yi′−μ)+ln(2π∣Σ∣1))去除常数项
ln
(
1
2
π
∣
Σ
∣
)
\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)
ln(2π∣Σ∣1)，得到：
max
Ψ
=
max
ln
Ψ
=
min
Ψ
1
=
min
∑
i
=
1
N
y
(
y
i
′
−
μ
)
T
Σ
−
1
(
y
i
′
−
μ
)
\max \Psi=\max \ln \Psi=\min \Psi_1=\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)
maxΨ=maxlnΨ=minΨ1=mini=1∑Ny(yi′−μ)TΣ−1(yi′−μ)因为之前是负数，我们上式只需要求最小值就行。(这里又变成马氏距离了)
这时就变成了一个优化问题了：
目标函数：
min
∑
i
=
1
N
y
(
y
i
′
−
μ
)
T
Σ
−
1
(
y
i
′
−
μ
)
\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)
min∑i=1Ny(yi′−μ)TΣ−1(yi′−μ)
y
i
′
=
T
(
p
,
y
i
)
=
R
y
i
+
t
\quad \quad \quad \quad y_i^{\prime}=T\left(p, y_i\right)=R y_i+t
yi′=T(p,yi)=Ryi+t
待求参数：
R
,
t
R, t
R,t
定义残差函数：
f
i
(
p
)
=
y
i
′
−
μ
f_i(p)=y_i^{\prime}-\mu
fi(p)=yi′−μ
按照高斯牛顿法的流程，只需计算残差函数关于待求参数的雅可比，便可迭代优化。
J
i
=
d
f
i
(
p
)
d
p
J_i=\frac{d f_i(p)}{d p}
Ji=dpdfi(p)
2.1 2D场景求解:
p
=
[
t
x
t
y
ϕ
z
]
T
y
i
′
=
T
(
p
,
y
i
)
=
[
cos
ϕ
z
−
sin
ϕ
z
sin
ϕ
z
cos
ϕ
z
]
y
i
+
[
t
x
t
y
]
\begin{aligned} p & =\left[\begin{array}{lll} t_x & t_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ \\ y_i^{\prime} & =T\left(p, y_i\right) \\ & =\left[\begin{array}{cc} \cos \phi_z & -\sin \phi_z \\ \sin \phi_z & \cos \phi_z \end{array}\right] y_i+\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \end{aligned}
pyi′=[txtyϕz]T=T(p,yi)=[cosϕzsinϕz−sinϕzcosϕz]yi+[txty]雅可比：
J
i
=
d
f
i
(
p
)
d
p
=
[
1
0
−
y
i
1
sin
ϕ
z
−
y
i
2
cos
ϕ
z
0
1
y
i
1
cos
ϕ
z
−
y
i
2
sin
ϕ
z
]
J_i=\frac{d f_i(p)}{d p}=\left[\begin{array}{ccc}1 & 0 & -y_{i 1} \sin \phi_z-y_{i 2} \cos \phi_z \\ 0 & 1 & y_{i 1} \cos \phi_z-y_{i 2} \sin \phi_z\end{array}\right]
Ji=dpdfi(p)=[1001−yi1sinϕz−yi2cosϕzyi1cosϕz−yi2sinϕz]
2.2 3D场景求解：
p
=
[
t
x
t
y
t
z
ϕ
x
ϕ
y
ϕ
z
]
T
y
i
′
=
T
(
p
,
y
i
)
=
R
x
R
y
R
z
y
i
+
t
=
[
c
y
c
z
−
c
y
s
z
s
y
c
x
s
z
+
s
x
s
y
c
z
c
x
c
z
−
s
x
s
y
s
z
−
s
x
c
y
s
x
s
z
−
c
x
s
y
c
z
c
x
s
y
s
z
+
s
x
c
z
c
x
c
y
]
y
i
+
[
t
x
t
y
t
z
]
\begin{aligned} & p=\left[\begin{array}{llllll} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ & y_i^{\prime}=T\left(p, y_i\right)=R_x R_y R_z y_i+t=\left[\begin{array}{ccc} c_y c_z & -c_y s_z & s_y \\ c_x s_z+s_x s_y c_z & c_x c_z-s_x s_y s_z & -s_x c_y \\ s_x s_z-c_x s_y c_z & c_x s_y s_z+s_x c_z & c_x c_y \end{array}\right] y_i+\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \end{aligned}
p=[txtytzϕxϕyϕz]Tyi′=T(p,yi)=RxRyRzyi+t=
cyczcxsz+sxsyczsxsz−cxsycz−cyszcxcz−sxsyszcxsysz+sxczsy−sxcycxcy
yi+
txtytz
雅可比：
J
i
=
[
1
0
0
0
c
f
0
1
0
a
d
g
0
0
1
b
e
h
]
\begin{aligned} J_i=\left[\begin{array}{llllll} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{array}\right] \end{aligned}
Ji=
1000100010abcdefgh
其中：
a
=
y
i
1
(
−
s
x
s
z
+
c
x
s
y
c
z
)
+
y
i
2
(
−
s
x
c
z
−
c
x
s
y
s
z
)
+
y
i
3
(
−
c
x
c
y
)
b
=
y
i
1
(
c
x
s
z
+
s
x
s
y
c
z
)
+
y
i
2
(
−
s
x
s
y
s
z
+
c
x
c
z
)
+
y
i
3
(
−
s
x
c
y
)
c
=
y
i
1
(
−
s
y
c
z
)
+
y
i
2
(
s
y
s
z
)
+
y
i
3
(
c
y
)
d
=
y
i
1
(
s
x
c
y
c
z
)
+
y
i
2
(
−
s
x
c
y
s
z
)
+
y
i
3
(
s
x
s
y
)
e
=
y
i
1
(
−
c
x
c
y
c
z
)
+
y
i
2
(
c
x
c
y
s
z
)
+
y
i
3
(
−
c
x
s
y
)
f
=
y
i
1
(
−
c
y
s
z
)
+
y
i
2
(
−
c
y
c
z
)
g
=
y
i
1
(
c
x
c
z
−
s
x
s
y
s
z
)
+
y
i
2
(
−
c
x
s
z
−
s
x
s
y
c
z
)
h
=
y
i
1
(
s
x
c
z
+
c
x
s
y
s
z
)
+
y
i
2
(
c
x
s
y
c
z
−
s
x
s
z
)
\begin{aligned} & a=y_{i 1}\left(-s_x s_z+c_x s_y c_z\right)+y_{i 2}\left(-s_x c_z-c_x s_y s_z\right)+y_{i 3}\left(-c_x c_y\right) \\ & b=y_{i 1}\left(c_x s_z+s_x s_y c_z\right)+y_{i 2}\left(-s_x s_y s_z+c_x c_z\right)+y_{i 3}\left(-s_x c_y\right) \\ & c=y_{i 1}\left(-s_y c_z\right)+y_{i 2}\left(s_y s_z\right)+y_{i 3}\left(c_y\right) \\ & d=y_{i 1}\left(s_x c_y c_z\right)+y_{i 2}\left(-s_x c_y s_z\right)+y_{i 3}\left(s_x s_y\right) \\ & e=y_{i 1}\left(-c_x c_y c_z\right)+y_{i 2}\left(c_x c_y s_z\right)+y_{i 3}\left(-c_x s_y\right) \\ & f=y_{i 1}\left(-c_y s_z\right)+y_{i 2}\left(-c_y c_z\right) \\ & g=y_{i 1}\left(c_x c_z-s_x s_y s_z\right)+y_{i 2}\left(-c_x s_z-s_x s_y c_z\right) \\ & h=y_{i 1}\left(s_x c_z+c_x s_y s_z\right)+y_{i 2}\left(c_x s_y c_z-s_x s_z\right) \end{aligned}
a=yi1(−sxsz+cxsycz)+yi2(−sxcz−cxsysz)+yi3(−cxcy)b=yi1(cxsz+sxsycz)+yi2(−sxsysz+cxcz)+yi3(−sxcy)c=yi1(−sycz)+yi2(sysz)+yi3(cy)d=yi1(sxcycz)+yi2(−sxcysz)+yi3(sxsy)e=yi1(−cxcycz)+yi2(cxcysz)+yi3(−cxsy)f=yi1(−cysz)+yi2(−cycz)g=yi1(cxcz−sxsysz)+yi2(−cxsz−sxsycz)h=yi1(sxcz+cxsysz)+yi2(cxsycz−sxsz)上面是使用旋转矩阵求导，也可以使用李代数求解。
3. 其他 NDT
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
