高斯牛顿
问题描述
给定一个目标函数，把激光帧间匹配问题转换为求解目标函数极值的问题：
E ( T ) = a r g min T ∑ [ 1 − M ( S i ( T ) ) ] 2 E(T) = arg\min_T\sum[1-M(S_i(T))]^2 E(T)=argTmin∑[1−M(Si(T))]2
其中，
T
=
(
T
x
,
T
y
,
T
θ
)
T= (T_x, T_y, T_{\theta})
T=(Tx,Ty,Tθ)表示机器人的位姿
p
i
=
(
p
i
x
,
p
i
y
)
p_i = (p_{ix}, p_{iy})
pi=(pix,piy)表示第
i
i
i个激光点的坐标
S
i
(
T
)
S_i(T)
Si(T)表示第
i
i
i个激光点位姿变换
T
T
T后的坐标
S
i
(
T
)
=
[
c
o
s
T
θ
−
s
i
n
T
θ
T
x
s
i
n
T
θ
c
o
s
T
θ
T
y
0
0
1
]
[
p
i
x
p
i
y
1
]
S_i (T)= \begin{bmatrix} cosT_\theta & -sinT_\theta & T_x \\ sinT_\theta & cosT_\theta & T_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} p_{ix} \\ p_{iy} \\ 1 \end{bmatrix}
Si(T)=⎣⎡cosTθsinTθ0−sinTθcosTθ0TxTy1⎦⎤⎣⎡pixpiy1⎦⎤
M
(
x
)
M(x)
M(x)表示似然厂地图中坐标
x
x
x处的值。
对待优化函数进行泰勒展开，最后可以得到，
Δ T = H − 1 ∑ [ ∇ M ( S i ( T ) ∂ S i ( T ) ∂ T ] T [ 1 − M ( S i ( T ) ) ] \Delta T = H^{-1}\sum[\nabla M(S_i(T)\frac{\partial S_i(T)}{\partial T}]^T[1 - M(S_i(T))] ΔT=H−1∑[∇M(Si(T)∂T∂Si(T)]T[1−M(Si(T))]
H = ∑ [ ∇ M ( S i ( T ) ) ∂ S i ( T ) ∂ T ] T [ ∇ M ( S i ( T ) ) ∂ S i ( T ) ∂ T ] H = \sum[\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}]^T[\nabla M(S_i(T))\frac{\partial S_i(T)}{\partial T}] H=∑[∇M(Si(T))∂T∂Si(T)]T[∇M(Si(T))∂T∂Si(T)]
未知量： ∇ M ( S i ( T ) ) \nabla M(S_i(T)) ∇M(Si(T))， ∂ S i ( T ) ∂ T \frac{\partial S_i(T)}{\partial T} ∂T∂Si(T)
∂ S i ( T ) ∂ T = [ 1 0 − s i n T θ ∗ p i x − c o s T θ ∗ p i y 0 1 c o s T θ ∗ p i x − s i n T θ ∗ p i y ] \frac{\partial S_i(T)}{\partial T} = \begin{bmatrix} 1 & 0 & -sinT_\theta * p_{ix} - cosT_\theta * p_{iy} \\ 0 & 1 & cosT\theta * p_{ix} - sinT_\theta * p_{iy} \end{bmatrix} ∂T∂Si(T)=[1001−sinTθ∗pix−cosTθ∗piycosTθ∗pix−sinTθ∗piy]
∇ M ( S i ( T ) ) \nabla M(S_i(T)) ∇M(Si(T))表示似然场地图中坐标 S i ( T ) S_i(T) Si(T)处的值， ∇ M ( S i ( T ) ) \nabla M(S_i(T)) ∇M(Si(T))表示似然场对坐标位置的导数，需要通过差值求解。
地图双线性差值
对于2D激光雷达SLAM，建立的概率栅格地图是二维的，因此需要进行双线性插值。
Z
1
=
f
(
x
0
,
y
0
)
,
Z
2
=
f
(
x
1
,
y
0
)
Z_1 = f(x_0, y_0),\ \ Z_2 = f(x_1, y_0)
Z1=f(x0,y0), Z2=f(x1,y0)
Z
3
=
f
(
x
1
,
y
1
)
,
Z
4
=
f
(
x
0
,
y
1
)
Z_3 = f(x_1, y_1), \ \ Z_4 = f(x_0, y_1)
Z3=f(x1,y1), Z4=f(x0,y1)
令：
u = x − x 0 x 1 − x 0 , v = y − y 0 y 1 − y 0 u = \frac{x - x_0}{x_1 - x_0}, \ v = \frac{y - y_0}{y _1 - y_0} u=x1−x0x−x0, v=y1−y0y−y0
构造基函数：
l
1
(
u
,
v
)
=
(
1
−
u
)
(
1
−
v
)
l_1(u, v) = (1 - u )(1 - v)
l1(u,v)=(1−u)(1−v)
l
2
(
u
,
v
)
=
u
(
1
−
v
)
l_2(u, v) = u(1 - v)
l2(u,v)=u(1−v)
l
3
(
u
,
v
)
=
u
v
l_3(u, v) = uv
l3(u,v)=uv
l
4
(
u
,
v
)
=
(
1
−
u
)
v
l_4(u, v) = (1 - u)v
l4(u,v)=(1−u)v
差值函数为，
L 4 ( u , v ) = Z 1 l 1 ( u , v ) + Z 2 l 2 ( u , v ) + Z 3 l 3 ( u , v ) + Z 4 l 4 ( u , v ) L_4(u, v) = Z_1l_1(u, v) + Z_2l_2(u, v) + Z_3l_3(u ,v) + Z_4l_4(u, v) L4(u,v)=Z1l1(u,v)+Z2l2(u,v)+Z3l3(u,v)+Z4l4(u,v)
NDT方法
数学描述
T
=
(
T
x
,
T
y
,
T
θ
)
T = (T_x, T_y, T_\theta)
T=(Tx,Ty,Tθ)表示需要求解的位姿变换
X
i
=
[
x
i
y
i
]
X_i = \begin{bmatrix} x_i \\ y_i \end{bmatrix}
Xi=[xiyi]表示第
i
i
i个激光点的坐标。
X
i
′
X_i^{'}
Xi′表示第
i
i
i个激光点经过位姿变换
T
T
T后的坐标
X
i
′
=
(
c
o
s
T
θ
−
s
i
n
T
θ
s
i
n
T
θ
c
o
s
T
θ
)
(
x
i
y
i
)
+
(
T
x
T
y
)
X_i^{'} = \begin{pmatrix} cosT_\theta & -sinT_\theta \\ sinT_\theta & cosT_\theta \end{pmatrix} \begin{pmatrix} x_i \\ y_i \end{pmatrix} +\begin{pmatrix} T_x \\ T_y \end{pmatrix}
Xi′=(cosTθsinTθ−sinTθcosTθ)(xiyi)+(TxTy)
q i , ∑ i q_i, \sum_i qi,∑i表示点 X i X_i Xi对应的高斯分布均值和方差
当前激光点 i i i的得分为：
s c o r e i = e x p ( − X i ′ − q i T ∑ i − 1 ( X i ′ − q i ) 2 ) {score}_i = exp (-\frac{{X_i^{'}-q_i}^T\sum_i^{-1}(X'_i - q_i)}{2}) scorei=exp(−2Xi′−qiT∑i−1(Xi′−qi))
令
q
=
X
i
′
−
q
i
q = X'_i - q_i
q=Xi′−qi，则
s
o
r
c
e
i
=
e
x
p
(
−
q
T
∑
i
−
1
q
2
)
sorce_i = exp(-\frac{q^T\sum_i^{-1}q}{2})
sorcei=exp(−2qT∑i−1q)
牛顿法
假设目标函数为：
m i n f ( x ) minf(x) minf(x)
等价于求一介导数的零点：
g
(
x
)
=
f
′
(
x
)
=
0
g(x) = f'(x) = 0
g(x)=f′(x)=0
泰勒展开可以得到：
g
(
x
+
Δ
x
)
=
g
(
x
)
+
∂
g
(
x
)
∂
x
=
0
g(x + \Delta x) = g(x) + \frac{\partial g(x)}{\partial x} = 0
g(x+Δx)=g(x)+∂x∂g(x)=0
∂
g
(
x
)
∂
x
Δ
x
=
−
g
(
x
)
\frac{\partial g(x)}{\partial x} \Delta x = -g(x)
∂x∂g(x)Δx=−g(x)
等价于
H
Δ
x
=
−
J
H\Delta x = -J
HΔx=−J
其中
H
(
x
)
H(x)
H(x)是海塞矩阵,
J
J
J是雅克比矩阵。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
