柚子快報(bào)激活碼778899分享：【論文閱讀】FAST-LIO
柚子快報(bào)激活碼778899分享：【論文閱讀】FAST-LIO
FAST-LIO論文閱讀
零、摘要一、介紹二、相關(guān)工作三、方法A.框架概述1. 一些重要的符號(hào)2. 流程框架
B.系統(tǒng)描述1. 符號(hào)定義2. 連續(xù)模型a. LiDAR和IMU的外參b. IMU運(yùn)動(dòng)學(xué)模型c. 離散模型d.激光雷達(dá)測(cè)量數(shù)據(jù)預(yù)處理
3. 狀態(tài)估計(jì)a.前向傳播b.反向傳播和運(yùn)動(dòng)補(bǔ)償c. 殘差計(jì)算d.迭代狀態(tài)更新e.算法流程
4.地圖更新5.初始化
四、附錄A.
F
x
~
F_{\widetilde{x}}
Fx
?和
F
w
F_w
Fw?的計(jì)算B.兩種卡爾曼增益的等價(jià)證明
零、摘要
本文提出了一種計(jì)算效率高、魯棒的激光雷達(dá)-慣性里程計(jì)框架。我們利用緊密耦合的迭代擴(kuò)展卡爾曼濾波器將激光雷達(dá)特征點(diǎn)與IMU數(shù)據(jù)融合，從而在快速運(yùn)動(dòng)、噪聲或雜波環(huán)境中實(shí)現(xiàn)魯棒導(dǎo)航。為了降低在大量測(cè)量條件下的計(jì)算量，我們提出了一個(gè)新的計(jì)算卡爾曼增益的公式。新公式的計(jì)算量不再依賴(lài)于測(cè)量維數(shù)，而是依賴(lài)于狀態(tài)維數(shù)。提出的方法及其實(shí)現(xiàn)在各種室內(nèi)和室外環(huán)境中進(jìn)行了測(cè)試。在所有的測(cè)試中，我們的方法產(chǎn)生了可靠的實(shí)時(shí)導(dǎo)航結(jié)果:在一臺(tái)四旋翼機(jī)載計(jì)算機(jī)上運(yùn)行，它在一次掃描中融合了超過(guò)1200個(gè)有效特征點(diǎn)，并在25毫秒內(nèi)完成了iEKF步驟的所有迭代。我們的代碼在Github上是開(kāi)源的。
一、介紹
相機(jī)相比于激光雷達(dá)的一些缺陷，如：缺乏深度測(cè)量，對(duì)光照敏感。固態(tài)激光雷達(dá)的優(yōu)點(diǎn)：成本低，重量輕，性能高，適用于無(wú)人機(jī)。激光雷達(dá)+IMU的一些問(wèn)題：在嘈雜環(huán)境中容易退化，計(jì)算量大（點(diǎn)多），存在運(yùn)動(dòng)畸變，無(wú)人機(jī)螺旋槳和馬達(dá)的持續(xù)旋轉(zhuǎn)也會(huì)給IMU的測(cè)量帶來(lái)顯著的噪聲。FAST-LIO介紹： a. 防止退化采用緊耦合迭代卡爾曼濾波器來(lái)融合激光雷達(dá)特征點(diǎn)和IMU測(cè)量值，提出了 一種形式的反向傳播過(guò)程來(lái)補(bǔ)償運(yùn)動(dòng)失真； b. 提出了一種新的卡爾曼增益計(jì)算公式，降低計(jì)算量。
二、相關(guān)工作
LOAM及其變體：適用于結(jié)構(gòu)化環(huán)境和大視場(chǎng)的激光雷達(dá)，但非常容易受到無(wú)特征環(huán)境或小視場(chǎng)激光雷達(dá)的影響。松耦合LIO：掃描配準(zhǔn)與數(shù)據(jù)融合的分離降低了計(jì)算量。然而，它忽略了系統(tǒng)的其他狀態(tài)(例如速度)和新掃描的姿態(tài)之間的關(guān)系。此外，在無(wú)特征環(huán)境下，掃描配準(zhǔn)會(huì)在一定方向上退化，導(dǎo)致后期融合不可靠。緊耦合LIO：與松耦合方法不同，緊耦合的LIO通常將激光雷達(dá)的原始特征點(diǎn)(而不是掃描配準(zhǔn)結(jié)果)與IMU數(shù)據(jù)融合。緊耦合LIO有兩種主要方法:基于優(yōu)化和基于濾波器。論文的方法屬于緊耦合方法。采用迭代擴(kuò)展卡爾曼濾波器來(lái)減小線性化誤差?？柭鼮V波器(及其變體)的時(shí)間復(fù)雜度為
O
(
m
2
)
O(m^2)
O(m2)，其中m為測(cè)量維數(shù)，這可能導(dǎo)致在處理大量的LiDAR測(cè)量時(shí)計(jì)算量非常高。下采樣會(huì)減少測(cè)量的數(shù)量，但以信息損失為代價(jià)?？梢酝ㄟ^(guò)提取和擬合類(lèi)似的地平面，減少測(cè)量的數(shù)量。然而，這并不適用于地面可能不總是出現(xiàn)的空中應(yīng)用。
三、方法
A.框架概述
1. 一些重要的符號(hào)
2. 流程框架
B.系統(tǒng)描述
1. 符號(hào)定義
2. 連續(xù)模型
a. LiDAR和IMU的外參
I
T
L
=
(
I
R
L
,
I
p
L
)
{}^{I}\mathbf{T}_{L}=\left({}^{I}\mathbf{R}_{L},{}^{I}\mathbf{p}_{L}\right)
ITL?=(IRL?,IpL?)
b. IMU運(yùn)動(dòng)學(xué)模型
G
p
˙
I
=
G
v
I
G
v
˙
I
=
G
R
I
(
a
m
?
b
a
?
n
a
)
+
G
g
G
g
˙
=
0
G
R
˙
I
=
G
R
I
[
ω
m
?
b
ω
?
n
ω
]
∧
b
˙
ω
=
n
b
ω
b
˙
a
=
n
b
a
\begin{align*}^G\dot{\mathbf p}_I &= {^G\mathbf v}_I \\ {^G\dot{\mathbf v}_I}&= {^G\mathbf R}_I\left(\mathbf a_m-\mathbf b_\mathbf a-\mathbf n_\mathbf a\right)+{^G\mathbf g} \\ ^G\dot{\mathbf g} &= \mathbf0 \\ {}^{G}\dot{\mathbf{R}}_{I} & = {}^{G}\mathbf{R}_{I}\bigl[\boldsymbol{\omega}_{m}-\mathbf_{\boldsymbol{\omega}}-\mathbf{n}_{\boldsymbol{\omega}}\bigr]_{\wedge}\\ \dot{\mathbf}_{\boldsymbol{\omega}}&=\mathbf{n}_{\mathbf\boldsymbol{\omega}}\\ \dot{\mathbf}_{\mathbf{a}}&=\mathbf{n}_{\mathbf\mathbf{a}}\end{align*}
Gp˙?I?Gv˙I?Gg˙?GR˙I?b˙ω?b˙a??=GvI?=GRI?(am??ba??na?)+Gg=0=GRI?[ωm??bω??nω?]∧?=nbω?=nba??
c. 離散模型
M
=
S
O
(
3
)
×
R
15
,
dim
?
(
M
)
=
18
\mathcal{M}=SO(3)\times\mathbb{R}^{15},\dim(\mathcal{M})=18 \\
M=SO(3)×R15,dim(M)=18
x
?
[
G
R
I
T
G
p
I
T
G
v
I
T
b
ω
T
b
a
T
G
g
T
]
T
∈
M
u
?
[
ω
m
T
a
m
T
]
T
w
?
[
n
ω
T
n
a
T
n
b
ω
T
n
b
a
T
]
T
\begin{align*} \mathbf{x} &\doteq\begin{bmatrix}G\mathbf{R}_I^T&G\mathbf{p}_I^T&^G\mathbf{v}_I^T&\mathbf_\omega^T&\mathbf_\mathbf{a}^T&^G\mathbf{g}^T\end{bmatrix}^T\in\mathcal{M} \\ \mathbf{u}&\doteq\begin{bmatrix}\boldsymbol{\omega}_m^T&\mathbf{a}_m^T\end{bmatrix}^T\\\mathbf{w}&\doteq\begin{bmatrix}\mathbf{n}_{\boldsymbol{\omega}}^{T}&\mathbf{n}_{\mathbf{a}}^{T}&\mathbf{n}_{\mathbf\boldsymbol{\omega}}^{T}&\mathbf{n}_{\mathbf{ba}}^{T}\end{bmatrix}^{T}\end{align*}
xuw??[GRIT??GpIT??GvIT??bωT??baT??GgT?]T∈M?[ωmT??amT??]T?[nωT??naT??nbωT??nbaT??]T?
d.激光雷達(dá)測(cè)量數(shù)據(jù)預(yù)處理
激光雷達(dá)測(cè)量的是點(diǎn)在其自身坐標(biāo)系下的坐標(biāo)，由于原始的雷達(dá)點(diǎn)在非常高的速率下采集的，所以新點(diǎn)一接收到就處理是不可能的。更實(shí)際的方法是在一定時(shí)間內(nèi)積累這些點(diǎn)，并一次性處理它們。也就是每獲得一幀點(diǎn)云再進(jìn)行處理。處理方式是提取平面點(diǎn)和角點(diǎn)這樣的特征點(diǎn)。假設(shè)有m個(gè)特征點(diǎn)，每個(gè)特征點(diǎn)的采樣時(shí)間為
ρ
j
∈
(
t
k
?
1
,
t
k
]
ρ_j ∈ (t_{k?1}, t_k]
ρj?∈(tk?1?,tk?]并表示為
L
j
p
f
i
{}^{L_j}p_{f_i}
Lj?pfi??，其中
L
j
L_j
Lj?為
ρ
j
ρ_j
ρj?時(shí)刻的激光雷達(dá)坐標(biāo)。同時(shí)在一幀激光雷達(dá)中有多個(gè)IMU測(cè)量，每個(gè)在
τ
i
∈
[
t
k
?
1
，
t
k
]
τ_i ∈ [t_{k?1}，t_{k}]
τi?∈[tk?1?，tk?]處采樣，每個(gè)都有 離散模型中相應(yīng)的狀態(tài)
x
i
x_i
xi?。注意，最后一個(gè)LiDAR特征點(diǎn)是掃描的結(jié)束，即，
ρ
m
=
t
k
ρ_m = t_k
ρm?=tk?，而IMU測(cè)量可能不一定與掃描的開(kāi)始或結(jié)束對(duì)齊。
3. 狀態(tài)估計(jì)
論文使用IEKF實(shí)現(xiàn)狀態(tài)估計(jì)，并且在狀態(tài)估計(jì)的切空間中表征估計(jì)協(xié)方差。假設(shè)在
t
k
?
1
t_{k-1}
tk?1?時(shí)最后一次雷達(dá)掃描的最佳狀態(tài)估計(jì)為
x
￣
k
?
1
\overline{x}_{k-1}
xk?1?,協(xié)方差矩陣為
P
￣
k
?
1
\overline{P}_{k-1}
Pk?1?，其中
P
￣
k
?
1
\overline{P}_{k-1}
Pk?1?表示如下定義的隨機(jī)誤差狀態(tài)向量的協(xié)方差：
a.前向傳播
其中
?
t
=
τ
i
+
1
?
τ
i
?t = τ_{i+1} ? τ_{i}
?t=τi+1??τi? 為了傳播協(xié)方差，論文使用下面獲得的誤差狀態(tài)動(dòng)態(tài)模型：
F
x
~
F_{\widetilde{x}}
Fx
?和
F
w
F_w
Fw?的計(jì)算過(guò)程見(jiàn)附錄A
所以有協(xié)方差的傳播公式：
P
^
i
+
1
=
F
x
~
P
^
i
F
x
~
T
+
F
w
Q
F
w
T
;
P
^
0
=
P
ˉ
k
?
1
.
\widehat{\mathbf{P}}_{i+1}=\mathbf{F}_{\widetilde{\mathbf{x}}}\widehat{\mathbf{P}}_{i}\mathbf{F}_{\widetilde{\mathbf{x}}}^{T}+\mathbf{F}_{\mathbf{w}}\mathbf{Q}\mathbf{F}_{\mathbf{w}}^{T};\widehat{\mathbf{P}}_{0}=\bar{\mathbf{P}}_{k-1}.
P
i+1?=Fx
?P
i?Fx
T?+Fw?QFwT?;P
0?=Pˉk?1?. 這個(gè)傳播過(guò)程一直到新一幀到來(lái)。其中的傳播狀態(tài)和協(xié)方差分別表示為
x
^
k
,
P
^
k
\hat{x}_k,\hat{P}_k
x^k?,P^k?,其中
P
^
k
\hat{P}_k
P^k?表示真值
x
k
x_k
xk?和狀態(tài)傳播值
x
^
k
\hat{x}_k
x^k?之間誤差的協(xié)方差。
b.反向傳播和運(yùn)動(dòng)補(bǔ)償
由于特征點(diǎn)采集的時(shí)間不同（特征點(diǎn)采集時(shí)間
ρ
j
ρ_j
ρj?<
t
k
t_k
tk?），所以會(huì)產(chǎn)生運(yùn)動(dòng)畸變，這就需要進(jìn)行運(yùn)動(dòng)補(bǔ)償，反向傳播公式如下：
x
ˇ
j
?
1
=
x
ˇ
j
(
?
Δ
t
f
(
x
ˇ
j
,
u
j
,
0
)
)
\check{\mathbf{x}}_{j-1}=\check{\mathbf{x}}_{j}(-\Delta t\mathbf{f}(\check{\mathbf{x}}_{j},\mathbf{u}_{j},\mathbf{0}))
xˇj?1?=xˇj?(?Δtf(xˇj?,uj?,0)) 后向傳播是在特征點(diǎn)的采集頻率下執(zhí)行的，該頻率通常遠(yuǎn)高于IMU速率。對(duì)于在兩個(gè)IMU測(cè)量之間采樣的所有特征點(diǎn)，使用左側(cè)IMU測(cè)量作為反向傳播中的輸入(這句話(huà)為理解下面三個(gè)方程的關(guān)鍵)。此外，注意到
f
(
x
j
，
u
j
，
0
)
f(x_j，u_j，0)
f(xj?，uj?，0)的噪聲為零，反向傳播可以被簡(jiǎn)化為：
I
k
p
ˇ
I
j
+
1
=
I
k
p
ˇ
I
j
?
I
k
v
ˇ
I
j
Δ
t
,
s
.
f
.
I
k
p
ˇ
I
m
=
0
;
I
k
v
ˇ
I
j
?
1
=
I
k
v
ˇ
I
j
?
I
k
R
ˇ
I
j
(
a
m
i
?
1
?
b
^
a
k
)
Δ
t
?
I
k
g
^
k
Δ
t
,
s
.
f
.
I
k
v
ˇ
I
m
=
G
R
^
I
k
T
G
v
^
I
k
,
I
k
g
^
k
=
G
R
^
I
k
T
G
g
^
k
;
I
k
R
ˇ
I
j
?
1
=
I
k
R
ˇ
I
j
E
x
p
(
(
b
^
ω
k
?
ω
m
i
?
1
)
Δ
t
)
,
s
.
f
.
I
k
R
I
m
=
I
.
{}^{I_k}\check{\mathbf{p}}_{I_{j+1}}={}^{I_k}\check{\mathbf{p}}_{I_j}-{}^{I_k}\check{\mathbf{v}}_{I_j}\Delta t,\quad s.f.{}^{I_k}\check{\mathbf{p}}_{I_m}=\mathbf{0}; \\ \begin{gathered} {}^{I_{k}}\check{\mathbf{v}}_{I_{j-1}}={}^{I_{k}}\check{\mathbf{v}}_{I_{j}}-{}^{I_{k}}\check{\mathbf{R}}_{I_{j}}(\mathbf{a}_{m_{i-1}}-\widehat{\mathbf}_{\mathbf{a}_{k}})\Delta t-{}^{I_{k}}\widehat{\mathbf{g}}_{k}\Delta t, \\ s.f.{}^{I_{k}}\check{\mathbf{v}}_{I_{m}}={}^{G}\widehat{\mathbf{R}}_{I_{k}}^{T}{}^{G}\widehat{\mathbf{v}}_{I_{k}},{}^{I_{k}}\widehat{\mathbf{g}}_{k}={}^{G}\widehat{\mathbf{R}}_{I_{k}}^{T}{}^{G}\widehat{\mathbf{g}}_{k}; \end{gathered} \\ {}^{I_k}\check{\mathbf{R}}_{I_{j-1}}={}^{I_k}\check{\mathbf{R}}_{I_j}\mathrm{Exp}((\widehat{\mathbf}_{\mathbf{\omega}_k}-\mathbf{\omega}_{m_{i-1}})\Delta t),\quad s.f.{}^{I_k}\mathbf{R}_{I_m}=\mathbf{I}.
Ik?pˇ?Ij+1??=Ik?pˇ?Ij???Ik?vˇIj??Δt,s.f.Ik?pˇ?Im??=0;Ik?vˇIj?1??=Ik?vˇIj???Ik?RˇIj??(ami?1???b
ak??)Δt?Ik?g
?k?Δt,s.f.Ik?vˇIm??=GR
Ik?T?Gv
Ik??,Ik?g
?k?=GR
Ik?T?Gg
?k?;?Ik?RˇIj?1??=Ik?RˇIj??Exp((b
ωk???ωmi?1??)Δt),s.f.Ik?RIm??=I. 其中
ρ
j
?
1
∈
[
τ
i
?
1
,
τ
i
)
，?
t
=
ρ
j
?
ρ
j
?
1
，
s
.
f
.
ρ_{j?1} ∈ [τ_{i?1}, τ_i)，?t = ρ_j ? ρ_{j?1}，s.f.
ρj?1?∈[τi?1?,τi?)，?t=ρj??ρj?1?，s.f.means “starting from”. 通過(guò)上三個(gè)方程就能獲得激光雷達(dá)
ρ
j
ρ_j
ρj?時(shí)刻和
t
k
t_k
tk?時(shí)刻的相對(duì)位姿：
I
k
T
ˇ
I
j
=
(
I
k
R
ˇ
I
j
,
I
k
p
ˇ
I
j
)
{}^{I_k}\check{T}_{I_j}=({}^{I_k}\check{R}_{I_j},{}^{I_k}\check{p}_{I_j})
Ik?TˇIj??=(Ik?RˇIj??,Ik?pˇ?Ij??)， 這個(gè)相對(duì)姿態(tài)可以將局部測(cè)量
L
j
p
f
j
{}^{L_j}{p}_{f_j}
Lj?pfj??投影到掃描結(jié)束端
L
k
p
f
j
{}^{L_k}{p}_{f_j}
Lk?pfj??
L
k
p
f
j
=
I
T
L
?
1
I
k
T
ˇ
I
j
I
T
L
L
j
p
f
j
.
{}^{L_k}\mathbf{p}_{f_j}={}^{I}\mathbf{T}_{L}^{-1I_k}\check{\mathbf{T}}_{I_j}{}^{I}\mathbf{T}_{L}{}^{L_j}\mathbf{p}_{f_j}.
Lk?pfj??=ITL?1Ik??TˇIj??ITL?Lj?pfj??. 然后就可以利用
L
k
p
f
j
{}^{L_k}{p}_{f_j}
Lk?pfj??去構(gòu)造殘差。（去畸變的思路和LOAM差不多）
c. 殘差計(jì)算
通過(guò)上一步的運(yùn)動(dòng)補(bǔ)償，可以獲得所有在同一時(shí)刻
t
k
t_k
tk?采樣得到的特征點(diǎn)集 {
L
k
p
f
j
{}^{L_k}{p}_{f_j}
Lk?pfj??}，并用它來(lái)構(gòu)造殘差。假設(shè)當(dāng)前迭代卡爾曼濾波的迭代數(shù)為
k
k
k，其對(duì)應(yīng)的狀態(tài)為
x
^
k
k
\hat{x}_k^k
x^kk?。當(dāng)
k
=
0
,
x
^
k
k
=
x
^
k
k=0,\hat{x}_k^k=\hat{x}_k
k=0,x^kk?=x^k?，傳播的預(yù)測(cè)狀態(tài)用方程為 所以特征點(diǎn)集{
L
k
p
f
j
{}^{L_k}{p}_{f_j}
Lk?pfj??}可以通過(guò)如下方式變換到全局坐標(biāo)下。
G
p
^
f
j
κ
=
G
T
^
I
k
κ
I
T
L
L
k
p
f
j
;
j
=
1
,
?
,
m
.
{}^G\widehat{\mathbf{p}}_{f_j}^{\kappa}={}^G\widehat{\mathbf{T}}_{I_k}^{\kappa I}\mathbf{T}_L{}^{L_k}\mathbf{p}_{f_j};j=1,\cdots,m.
Gp
?fj?κ?=GT
Ik?κI?TL?Lk?pfj??;j=1,?,m. 接下來(lái)就是計(jì)算點(diǎn)—線，點(diǎn)—面的殘差。
z
j
κ
=
G
j
(
G
p
^
f
j
κ
?
G
q
j
)
\mathbf{z}_j^{\kappa}=\mathbf{G}_j\left(\mathbf{}^G\widehat{\mathbf{p}}_{f_j}^{\kappa}-\mathbf{}^G\mathbf{q}_j\right)
zjκ?=Gj?(Gp
?fj?κ??Gqj?)
G
p
^
f
j
κ
{}^G\widehat{\mathbf{p}}_{f_j}^{\kappa}
Gp
?fj?κ?表示特征點(diǎn)的全局坐標(biāo)系坐標(biāo)，
G
q
j
\mathbf{}^G\mathbf{q}_j
Gqj?表示線/面上的點(diǎn)，
G
j
\mathbf{G}_j
Gj?表示線/面特征，
z
j
κ
\mathbf{z}_j^{\kappa}
zjκ?表示殘差。 方法和LOAM中類(lèi)似。
d.迭代狀態(tài)更新
為了將殘差
z
j
κ
\mathbf{z}_j^{\kappa}
zjκ?和IMU預(yù)測(cè)狀態(tài)
x
^
k
\hat{x}_k
x^k?,協(xié)方差
P
^
k
\hat{P}_k
P^k?融合，需要將殘差
z
j
κ
\mathbf{z}_j^{\kappa}
zjκ?與真值
x
k
x_k
xk?和測(cè)量噪聲聯(lián)系起來(lái)的測(cè)量模型線性化。測(cè)量噪聲來(lái)源于測(cè)量點(diǎn)
L
j
p
f
j
{}^{L_j}{p}_{f_j}
Lj?pfj??時(shí)的雷達(dá)測(cè)距和定向噪聲
L
j
n
f
j
{}^{L_j}{n}_{f_j}
Lj?nfj??。從測(cè)量點(diǎn)
L
j
p
f
j
{}^{L_j}{p}_{f_j}
Lj?pfj??中去除這一噪聲，就得到了真實(shí)點(diǎn)的位置。
L
j
p
f
j
gt
=
L
j
p
f
j
?
L
j
n
f
j
{}^{L_j}\mathbf{p}_{f_j}^{\text{gt}}={}^{L_j}\mathbf{p}_{f_j}-{}^{L_j}\mathbf{n}_{f_j}
Lj?pfj?gt?=Lj?pfj???Lj?nfj?? 這個(gè)真實(shí)點(diǎn)帶入殘差方程理應(yīng)結(jié)果為0，即
0
=
h
j
(
x
k
,
L
j
n
f
j
)
=
G
j
(
G
T
I
k
I
k
T
ˇ
I
j
I
T
L
(
L
j
p
f
j
?
L
j
n
f
j
)
?
G
q
j
)
\mathbf{0=h}_j\bigl(\mathbf{x}_k,{}^{L_j}\mathbf{n}_{f_j}\bigr)\mathbf{=G}_j\bigl({}^G\mathbf{T}_{I_k}{}^{I_k}\check{\mathbf{T}}_{I_j}{}^I\mathbf{T}_L\bigl(^{L_j}\mathbf{p}_{f_j}-{}^{L_j}\mathbf{n}_{f_j}\bigr)\mathbf{-}^G\mathbf{q}_j\bigr)
0=hj?(xk?,Lj?nfj??)=Gj?(GTIk??Ik?TˇIj??ITL?(Lj?pfj???Lj?nfj??)?Gqj?) 在
x
^
k
k
\hat{x}_k^k
x^kk?處進(jìn)行一階近似(這一部分對(duì)應(yīng)于EKF的線性化)
0
=
h
j
(
x
k
,
L
j
n
f
j
)
?
h
j
(
x
^
k
κ
,
0
)
+
H
j
κ
x
~
k
κ
+
v
j
=
z
j
κ
+
H
j
κ
x
~
k
κ
+
v
j
\begin{aligned} 0& =\mathbf{h}_{j}\left(\mathbf{x}_{k},{}^{L_{j}}\mathbf{n}_{f_{j}}\right)\simeq\mathbf{h}_{j}\left(\widehat{\mathbf{x}}_{k}^{\kappa},\mathbf{0}\right)+\mathbf{H}_{j}^{\kappa}\widetilde{\mathbf{x}}_{k}^{\kappa}+\mathbf{v}_{j} \\ &=\mathbf{z}_j^\kappa+\mathbf{H}_j^\kappa\widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j \end{aligned}
0?=hj?(xk?,Lj?nfj??)?hj?(x
kκ?,0)+Hjκ?x
kκ?+vj?=zjκ?+Hjκ?x
kκ?+vj?? 其中，
H
j
k
H_j^k
Hjk?是
x
~
k
κ
\widetilde{\mathbf{x}}_k^\kappa
x
kκ?關(guān)于
h
j
h_j
hj?的雅可比矩陣，
x
~
k
κ
\widetilde{\mathbf{x}}_k^\kappa
x
kκ?取值為0，
v
j
∈
N
(
0
,
R
j
)
{\mathbf{v}}_{j}\in{\mathcal{N}}(\mathbf{0},\mathbf{R}_{j})
vj?∈N(0,Rj?)來(lái)自測(cè)量噪聲
L
j
n
f
j
{}^{L_j}\mathbf{n}_{f_j}
Lj?nfj?? 注意：先驗(yàn)
x
k
x_k
xk?通過(guò)正向傳播獲得，將其線性化 其中
J
k
J^k
Jk 構(gòu)建MAP 得到標(biāo)準(zhǔn)的迭代卡爾曼濾波器 定義
H
=
[
H
1
κ
T
,
?
,
H
m
κ
T
]
T
,
R
=
d
i
a
g
(
R
1
,
?
R
m
)
,
P
=
(
J
κ
)
?
1
P
^
k
(
J
κ
)
?
T
,
a
n
d
z
k
κ
=
[
z
1
κ
T
,
?
,
z
m
κ
T
]
T
\mathbf{H}=[\mathbf{H}_{1}^{\kappa^{T}},\cdots,\mathbf{H}_{m}^{\kappa^{T}}]^{T},\\ \mathbf{R=diag}(\mathbf{R}_{1},\cdots\mathbf{R}_{m}),\\P=\begin{aligned} (\mathbf{J}^{\kappa})^{-1}\widehat{\mathbf{P}}_{k}(\mathbf{J}^{\kappa})^{-T},\mathrm{and \quad}\mathbf{z}_{k}^{\kappa}=\left[\mathbf{z}_{1}^{\kappa^{T}},\cdots,\mathbf{z}_{m}^{\kappa^{T}}\right]^{T} \end{aligned}
H=[H1κT?,?,HmκT?]T,R=diag(R1?,?Rm?),P=(Jκ)?1P
k?(Jκ)?T,andzkκ?=[z1κT?,?,zmκT?]T? 這個(gè)更新后的估計(jì)值
x
^
k
k
+
1
\hat{x}_k^{k+1}
x^kk+1?為第k+1次迭代的結(jié)果，循環(huán)迭代直到收斂至。收斂后優(yōu)化的狀態(tài)估計(jì)和方差為：
x
ˉ
k
=
x
^
k
κ
+
1
,
P
ˉ
k
=
(
I
?
K
H
)
P
\bar{\mathbf{x}}_{k}=\widehat{\mathbf{x}}_{k}^{\kappa+1},\bar{\mathbf{P}}_{k}=\left(\mathbf{I}-\mathbf{KH}\right)\mathbf{P}
xˉk?=x
kκ+1?,Pˉk?=(I?KH)P 論文重點(diǎn) 常用的卡爾曼增益形式的一個(gè)問(wèn)題是，它需要對(duì)度量維度內(nèi)的矩陣
H
P
H
T
+
R
HPH^T+R
HPHT+R求逆。在實(shí)踐中，LiDAR特征點(diǎn)的數(shù)量是非常大的，這種大小的矩陣的逆是禁止的。因此，現(xiàn)有的一些方案只使用了少量的測(cè)量方法。論文證明了這種限制是可以避免的。直覺(jué)源于 ，其中代價(jià)函數(shù)在狀態(tài)之上，因此解決方案的計(jì)算復(fù)雜度取決于狀態(tài)維。通過(guò)該式可以得到新的Kalman增益形式，如下圖所示:
K
=
(
H
T
R
?
1
H
+
P
?
1
)
?
1
H
T
R
?
1
\mathbf{K=}\left(\mathbf{H}^T\mathbf{R}^{-1}\mathbf{H+P}^{-1}\right)^{-1}\mathbf{H}^T\mathbf{R}^{-1}
K=(HTR?1H+P?1)?1HTR?1 我們?cè)诟戒汢中證明了基于矩陣逆引理的兩種形式的卡爾曼增益確實(shí)是等價(jià)的。由于激光雷達(dá)測(cè)量是獨(dú)立的，因此協(xié)方差矩陣R是(塊)對(duì)角線，因此新公式只需要兩個(gè)矩陣的狀態(tài)維，而不是測(cè)量。由于在LIO中狀態(tài)維數(shù)通常遠(yuǎn)低于測(cè)量值(例如，在10 Hz掃描速率下一次掃描1000多個(gè)有效特征點(diǎn)，而狀態(tài)維數(shù)只有18)，新公式大大節(jié)省了計(jì)算量。
e.算法流程
4.地圖更新
G
p
ˉ
f
j
=
G
T
ˉ
I
k
I
T
L
L
k
p
f
j
;
j
=
1
,
?
,
m
.
^G\bar{\mathbf{p}}_{f_j}={}^G\bar{\mathbf{T}}_{I_k}{}^I\mathbf{T}_L{}^{L_k}\mathbf{p}_{f_j};j=1,\cdots,m.
Gpˉ?fj??=GTˉIk??ITL?Lk?pfj??;j=1,?,m.
5.初始化
為了獲得系統(tǒng)狀態(tài)的良好初始估計(jì)(例如重力矢量Gg、偏差和噪聲協(xié)方差)，從而加速狀態(tài)估計(jì)，需要初始化。在FAST-LIO中，初始化很簡(jiǎn)單:保持LiDAR靜態(tài)數(shù)秒(論文中所有實(shí)驗(yàn)均為2秒)，然后使用收集到的數(shù)據(jù)初始化IMU偏差和重力矢量。如果激光雷達(dá)支持非重復(fù)掃描(如Livox AVIA)，保持靜態(tài)也允許激光雷達(dá)捕獲初始的高分辨率地圖，這對(duì)后續(xù)導(dǎo)航是有益的。
四、附錄
A.
F
x
~
F_{\widetilde{x}}
Fx
?和
F
w
F_w
Fw?的計(jì)算
其中式子（5）為
B.兩種卡爾曼增益的等價(jià)證明
柚子快報(bào)激活碼778899分享：【論文閱讀】FAST-LIO
參考閱讀
本文內(nèi)容根據(jù)網(wǎng)絡(luò)資料整理，出于傳遞更多信息之目的，不代表金鑰匙跨境贊同其觀點(diǎn)和立場(chǎng)。
轉(zhuǎn)載請(qǐng)注明，如有侵權(quán)，聯(lián)系刪除。
Voghion時(shí)尚前沿
