多传感器融合定位十二-基于图优化的建图方法其一
- 1. 基于预积分的融合方案流程
- 1.1 优化问题分析
- 1.2 预积分的作用
- 1.3 基于预积分的建图方案流程
- 2. 预积分模型设计
- 3. 预积分在优化中的使用
- 3.1 使用方法
- 3.2 残差设计
- 3.3 残差雅可比的推导
- 3.3.1 姿态残差的雅可比
- 3.3.2 速度残差的雅可比
- 3.3.3 位置残差的雅可比
Reference:
- 深蓝学院-多传感器融合
- 多传感器融合定位理论基础
文章跳转:
- 多传感器融合定位一-3D激光里程计其一:ICP
- 多传感器融合定位二-3D激光里程计其二:NDT
- 多传感器融合定位三-3D激光里程计其三:点云畸变补偿
- 多传感器融合定位四-3D激光里程计其四:点云线面特征提取
- 多传感器融合定位五-点云地图构建及定位
- 多传感器融合定位六-惯性导航原理及误差分析
- 多传感器融合定位七-惯性导航解算及误差分析其一
- 多传感器融合定位八-惯性导航解算及误差分析其二
- 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
- 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
- 多传感器融合定位十一-基于滤波的融合方法Ⅱ
- 多传感器融合定位十二-基于图优化的建图方法其一
- 多传感器融合定位十三-基于图优化的建图方法其二
- 多传感器融合定位十四-基于图优化的定位方法
- 多传感器融合定位十五-多传感器时空标定(综述)
1. 基于预积分的融合方案流程
1.1 优化问题分析
优化问题可以等效为如下形式:
三种约束分别通过以下方式获得:
- 激光里程计约束:使用激光里程计,计算每个关键帧位姿,进而得到相对位姿;
- IMU约束:在上一个关键帧位姿基础上,进行惯性积分,从而得到两关键帧相对位姿;
- RTK约束:直接测量得到。
1.2 预积分的作用
问题:位姿每次优化后会发生变化,其后的IMU惯性积分就要重新进行,运算量过大。
解决思路:直接计算两帧之间的相对位姿,而不依赖初始值影响,即所谓的预积分
。
预积分不是为了提高精度(与粗暴方法比,而非不融合方法),而是为了提高效率。关于预积分的文章,这一篇从理解上讲的非常清晰明了:imu预积分原理的个人理解
1.3 基于预积分的建图方案流程
由于此处讨论的优化方案包含组合导航系统,且认为外参已标定,因此会和常见的lio/vio中的方案有所不同,它不包含以下内容:
- 初始化lidar和IMU之间的外参(有外参的配置文件,已经标定好了,有了这个配置文件,外参的估计就没有意义了);
- 初始化速度、陀螺仪bias等(因为这里有组合导航,组合导航解决了大部分的问题,组合导航完成后有初始速度、初始速度和初始姿态,同时一般的组合导航会告诉初始的bias是多少,所以在这种情况下这两项也变成了已知的问题了,不需要做初始化来估计了);
- 初始化重力(如果知道经纬度就知道了精确的重力,只有在不知道经纬度的情况下才需要初始化去优化重力,因为组合导航经纬度都有了,经纬度是可以通过直接计算得到的而不需要通过优化得到);
- 世界坐标系对齐(组合导航已经对齐)(组合导航有了姿态,这个姿态就是在世界坐标系下的一个姿态)。
2. 预积分模型设计
在第8讲中,已知导航的微分方程如下:
p
˙
w
b
t
=
v
t
w
v
˙
t
w
=
a
t
w
q
˙
w
b
t
=
q
w
b
t
⊗
[
0
1
2
ω
b
t
]
\begin{aligned} & \dot{\mathbf{p}}_{w b_t}=\mathbf{v}_t^w \\ & \dot{\mathbf{v}}_t^w=\mathbf{a}_t^w \\ & \dot{\mathbf{q}}_{w b_t}=\mathbf{q}_{w b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \end{aligned}
p˙wbt=vtwv˙tw=atwq˙wbt=qwbt⊗[021ωbt]根据该微分方程,可知从
i
i
i 时刻到
j
j
j 时刻 IMU的积分结果为
p
w
b
j
=
p
w
b
i
+
v
i
w
Δ
t
+
∬
t
∈
[
i
,
j
]
(
q
w
b
t
a
b
t
−
g
w
)
δ
t
2
v
j
w
=
v
i
w
+
∫
t
∈
[
i
,
j
]
(
q
w
b
t
a
b
t
−
g
w
)
δ
t
q
w
b
j
=
∫
t
∈
[
i
,
j
]
q
w
b
t
⊗
[
0
1
2
ω
b
t
]
δ
t
\begin{gathered} \mathbf{p}_{w b_j}=\mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t+\iint_{t \in[i, j]}\left(\mathbf{q}_{w b_t} \mathbf{a}^{b_t}-\mathbf{g}^w\right) \delta t^2 \\ \mathbf{v}_j^w=\mathbf{v}_i^w+\int_{t \in[i, j]}\left(\mathbf{q}_{w b_t} \mathbf{a}^{b_t}-\mathbf{g}^w\right) \delta t \\ \mathbf{q}_{w b_j}=\int_{t \in[i, j]} \mathbf{q}_{w b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{gathered}
pwbj=pwbi+viwΔt+∬t∈[i,j](qwbtabt−gw)δt2vjw=viw+∫t∈[i,j](qwbtabt−gw)δtqwbj=∫t∈[i,j]qwbt⊗[021ωbt]δt根据预积分的要求,需要求相对结果,而且不依赖于上一时刻位姿,因此需要对上式做转换。
由于
q
w
b
t
=
q
w
b
i
⊗
q
b
i
b
t
\mathbf{q}_{w b_t}=\mathbf{q}_{w b_i} \otimes \mathbf{q}_{b_i b_t}
qwbt=qwbi⊗qbibt,把它带入(4)-(6)式可得
p
w
b
j
=
p
w
b
i
+
v
i
w
Δ
t
−
1
2
g
w
Δ
t
2
+
q
w
b
i
[
∬
t
∈
[
i
,
j
]
(
q
b
i
b
t
a
b
t
)
δ
t
2
v
j
w
=
v
i
w
−
g
w
Δ
t
+
q
w
b
i
∫
t
∈
[
i
,
j
]
(
q
b
i
b
t
a
b
t
)
δ
t
]
q
w
b
j
=
q
w
b
i
∫
t
∈
[
i
,
j
]
q
b
i
b
t
⊗
[
0
1
2
ω
b
t
]
δ
t
\begin{gathered} \mathbf{p}_{w b_j}=\mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t-\frac{1}{2} \mathbf{g}^w \Delta t^2+\mathbf{q}_{w b_i}\left[\iint_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t^2\right. \\ \left.\mathbf{v}_j^w=\mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \int_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t\right] \\ \mathbf{q}_{w b_j}=\mathbf{q}_{w b_i} \int_{t \in[i, j]} \mathbf{q}_{b_i b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{gathered}
pwbj=pwbi+viwΔt−21gwΔt2+qwbi[∬t∈[i,j](qbibtabt)δt2vjw=viw−gwΔt+qwbi∫t∈[i,j](qbibtabt)δt]qwbj=qwbi∫t∈[i,j]qbibt⊗[021ωbt]δt可见,此时需要积分的项,就完全和
i
i
i 时刻的状态无关了。
为了整理公式,把积分相关的项用下面的式子代替:
α
b
i
b
j
=
∬
t
∈
[
i
,
j
]
(
q
b
i
b
t
a
b
t
)
δ
t
2
β
b
i
b
j
=
∫
t
∈
[
i
,
j
]
(
q
b
i
b
t
a
b
t
)
δ
t
q
b
i
b
j
=
∫
t
∈
[
i
,
j
]
q
b
i
b
t
⊗
[
0
1
2
ω
b
t
]
δ
t
\begin{aligned} & \boldsymbol{\alpha}_{b_i b_j}=\iint_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t^2 \\ & \boldsymbol{\beta}_{b_i b_j}=\int_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t \\ & \mathbf{q}_{b_i b_j}=\int_{t \in[i, j]} \mathbf{q}_{b_i b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{aligned}
αbibj=∬t∈[i,j](qbibtabt)δt2βbibj=∫t∈[i,j](qbibtabt)δtqbibj=∫t∈[i,j]qbibt⊗[021ωbt]δt实际使用中使用离散形式,而非连续形式,由于在解算中,一般采用中值积分方法,即:
ω
=
1
2
[
(
ω
b
k
−
b
k
g
)
+
(
ω
b
k
+
1
−
b
k
g
)
]
a
=
1
2
[
q
b
i
b
k
(
a
b
k
−
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
−
b
k
a
)
]
\begin{aligned} & \boldsymbol{\omega}=\frac{1}{2}\left[\left(\boldsymbol{\omega}^{b_k}-\mathbf{b}_k^g\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_k^g\right)\right] \\ & \mathbf{a}=\frac{1}{2}\left[\mathbf{q}_{b_i b_k}\left(\mathbf{a}^{b_k}-\mathbf{b}_k^a\right)+\mathbf{q}_{b_i b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_k^a\right)\right] \end{aligned}
ω=21[(ωbk−bkg)+(ωbk+1−bkg)]a=21[qbibk(abk−bka)+qbibk+1(abk+1−bka)]那么预积分的离散形式可以表示为:
α
b
i
b
k
+
1
=
α
b
i
b
k
+
β
b
i
b
k
δ
t
+
1
2
a
δ
t
2
β
b
i
b
k
+
1
=
β
b
i
b
k
+
a
δ
t
q
b
i
b
k
+
1
=
q
b
i
b
k
⊗
[
1
1
2
ω
δ
t
]
\begin{aligned} & \boldsymbol{\alpha}_{b_i b_{k+1}}=\boldsymbol{\alpha}_{b_i b_k}+\boldsymbol{\beta}_{b_i b_k} \delta t+\frac{1}{2} \mathbf{a} \delta t^2 \\ & \boldsymbol{\beta}_{b_i b_{k+1}}=\boldsymbol{\beta}_{b_i b_k}+\mathbf{a} \delta t \\ & \mathbf{q}_{b_i b_{k+1}}=\mathbf{q}_{b_i b_k} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\omega} \delta t \end{array}\right] \end{aligned}
αbibk+1=αbibk+βbibkδt+21aδt2βbibk+1=βbibk+aδtqbibk+1=qbibk⊗[121ωδt]经过以上的推导,此时状态更新的公式可以整理为:
[
p
w
b
j
v
j
w
q
w
b
j
b
j
a
b
j
g
]
=
[
p
w
b
i
+
v
i
w
Δ
t
−
1
2
g
w
Δ
t
2
+
q
w
b
i
α
b
i
b
j
v
i
w
−
g
w
Δ
t
+
q
w
b
i
β
b
i
b
j
q
w
b
i
q
b
i
b
j
b
i
a
b
i
g
]
\left[\begin{array}{c} \mathbf{p}_{w b_j} \\ \mathbf{v}_j^w \\ \mathbf{q}_{w b_j} \\ \mathbf{b}_j^a \\ \mathbf{b}_j^g \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t-\frac{1}{2} \mathbf{g}^w \Delta t^2+\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ \mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{q}_{w b_i} \mathbf{q}_{b_i b_j} \\ \mathbf{b}_i^a \\ \mathbf{b}_i^g \end{array}\right]
pwbjvjwqwbjbjabjg
=
pwbi+viwΔt−21gwΔt2+qwbiαbibjviw−gwΔt+qwbiβbibjqwbiqbibjbiabig
需要注意的是,陀螺仪和加速度计的模型为:
b
k
+
1
a
=
b
k
a
+
n
b
k
a
δ
t
b
k
+
1
g
=
b
k
g
+
n
b
k
g
δ
t
\begin{array}{r} \mathbf{b}_{k+1}^a=\mathbf{b}_k^a+\mathbf{n}_{\mathbf{b}_k^a} \delta t \\ \mathbf{b}_{k+1}^g=\mathbf{b}_k^g+\mathbf{n}_{\mathbf{b}_k^g} \delta t \end{array}
bk+1a=bka+nbkaδtbk+1g=bkg+nbkgδt即认为bias是在变化的,这样便于估计不同时刻的bias值,而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但在预积分时,由于两个关键帧之间的时间较短,因此认为
i
i
i 和
j
j
j 时刻的bias相等。
需要注意的一点是,预积分的结果中包含了bias,在优化过程中,bias作为状态量也会发生变化,从而引起预积分结果变化。
为了避免bias变化后,重新做预积分,可以把预积分结果在bias处泰勒展开,表达成下面的形式,这样就可以根据bias的变化量直接算出新的预积分结果。
α
b
i
b
j
=
α
‾
b
i
b
j
+
J
b
i
a
α
δ
b
i
a
+
J
b
i
g
α
δ
b
i
g
β
b
i
b
j
=
β
‾
b
i
b
j
+
J
b
i
a
β
δ
b
i
a
+
J
b
i
g
β
δ
b
i
g
q
b
i
b
j
=
q
‾
b
i
b
j
⊗
[
1
1
2
J
b
i
q
q
δ
b
i
g
]
\begin{aligned} & \boldsymbol{\alpha}_{b_i b_j}=\overline{\boldsymbol{\alpha}}_{b_i b_j}+\mathbf{J}_{b_i^a}^\alpha \delta \mathbf{b}_i^a+\mathbf{J}_{b_i^g}^\alpha \delta \mathbf{b}_i^g \\ & \boldsymbol{\beta}_{b_i b_j}=\overline{\boldsymbol{\beta}}_{b_i b_j}+\mathbf{J}_{b_i^a}^\beta \delta \mathbf{b}_i^a+\mathbf{J}_{b_i^g}^\beta \delta \mathbf{b}_i^g \\ & \mathbf{q}_{b_i b_j}=\overline{\mathbf{q}}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^q}^q \delta \mathbf{b}_i^g \end{array}\right] \end{aligned}
αbibj=αbibj+Jbiaαδbia+Jbigαδbigβbibj=βbibj+Jbiaβδbia+Jbigβδbigqbibj=qbibj⊗[121Jbiqqδbig]其中:
J
b
i
a
α
=
∂
α
b
i
b
j
∂
δ
b
i
a
J
b
i
g
α
=
∂
α
b
i
b
j
∂
δ
b
i
j
J
b
i
a
β
=
∂
β
b
i
b
j
∂
δ
b
i
a
J
b
i
g
β
=
∂
β
b
i
b
j
∂
δ
b
i
a
J
b
i
g
q
=
q
b
i
b
j
∂
b
i
q
\begin{aligned} \mathbf{J}_{b_i^a}^\alpha & =\frac{\partial \alpha_{b_i b_j}}{\partial \delta \mathbf{b}_i^a} \\ \mathbf{J}_{b_i^g}^\alpha & =\frac{\partial \boldsymbol{\alpha}_{b_i b_j}}{\partial \delta \mathbf{b}_i^j} \\ \mathbf{J}_{b_i^a}^\beta & =\frac{\partial \beta_{b_i b_j}}{\partial \delta \mathbf{b}_i^a} \\ \mathbf{J}_{b_i^g}^\beta & =\frac{\partial \beta_{b_i b_j}}{\partial \delta \mathbf{b}_i^a} \\ \mathbf{J}_{b_i^g}^q & =\frac{\mathbf{q}_{b_i b_j}}{\partial \mathbf{b}_i^q} \end{aligned}
JbiaαJbigαJbiaβJbigβJbigq=∂δbia∂αbibj=∂δbij∂αbibj=∂δbia∂βbibj=∂δbia∂βbibj=∂biqqbibj注: 此处暂时不直接给出以上各雅可比的结果,它的推导放在后面进行。
3. 预积分在优化中的使用
3.1 使用方法
- 凸优化回顾
损失函数由残差函数组成:
min x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \min _x F(x)=\frac{1}{2}\|f(x)\|_2^2 xminF(x)=21∥f(x)∥22当考虑方差时,可以写为:
min x F ( x ) = f ( x ) T Ω f ( x ) \min _x F(x)=f(x)^T \Omega f(x) xminF(x)=f(x)TΩf(x)利用高斯牛顿方法,求解该优化问题,需要解下面的方程:
J T Ω J ⏟ H Δ x = − J T Ω f ( x ) ⏟ g \underbrace{J^T \Omega J}_H \Delta x=\underbrace{-J^T \Omega f(x)}_g H JTΩJΔx=g −JTΩf(x)即,在优化中使用一项信息,需要设计残差, 并推导它的雅可比和方差。 - 预积分的使用
按照优化的套路,在优化中使用预积分,需要:
a. 设计残差
b. 推导残差关于待优化变量的雅可比
c. 计算残差的方差
除此以外,预积分中还多一项计算,即推导bias变化时,预积分量重新计算的方法。
3.2 残差设计
在优化时,需要知道残差关于状态量的雅可比。由于已知姿态位姿更新的方法如下:
[
p
w
b
j
q
w
b
j
v
j
w
b
j
a
b
j
g
]
=
[
p
w
b
i
+
v
i
w
Δ
t
−
1
2
g
w
Δ
t
2
+
q
w
b
i
α
b
i
b
j
q
w
b
i
q
b
i
b
j
v
i
w
−
g
w
Δ
t
+
q
w
b
i
β
b
i
b
j
b
i
a
b
i
g
]
\left[\begin{array}{c} \mathbf{p}_{w b_j} \\ \mathbf{q}_{w b_j} \\ \mathbf{v}_j^w \\ \mathbf{b}_j^a \\ \mathbf{b}_j^g \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t-\frac{1}{2} \mathbf{g}^w \Delta t^2+\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ \mathbf{q}_{w b_i} \mathbf{q}_{b_i b_j} \\ \mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_i^a \\ \mathbf{b}_i^g \end{array}\right]
pwbjqwbjvjwbjabjg
=
pwbi+viwΔt−21gwΔt2+qwbiαbibjqwbiqbibjviw−gwΔt+qwbiβbibjbiabig
因此,可以很容易写出一种残差形式如下:
[
r
p
r
q
r
v
r
b
a
r
b
g
]
=
[
p
w
b
j
−
p
w
b
i
−
v
i
w
Δ
t
+
1
2
g
w
Δ
t
2
−
q
w
b
i
α
b
i
b
j
2
[
q
b
i
b
j
∗
⊗
(
q
w
b
i
∗
⊗
q
w
b
j
)
]
x
y
z
v
j
w
−
v
i
w
+
g
w
Δ
t
−
q
w
b
i
β
b
i
b
j
b
j
a
−
b
i
a
b
j
g
−
b
i
g
]
\left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \mathbf{r}_v \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_j}-\mathbf{p}_{w b_i}-\mathbf{v}_i^w \Delta t+\frac{1}{2} \mathbf{g}^w \Delta t^2-\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ 2\left[\mathbf{q}_{b_i b_j}^* \otimes\left(\mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z} \\ \mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t-\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_j^a-\mathbf{b}_i^a \\ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right]
rprqrvrbarbg
=
pwbj−pwbi−viwΔt+21gwΔt2−qwbiαbibj2[qbibj∗⊗(qwbi∗⊗qwbj)]xyzvjw−viw+gwΔt−qwbiβbibjbja−biabjg−big
但是和预积分相关的量,仍然与上一时刻的姿态有关,无法直接加减,因此,把残差修正为以下形式:
[
r
p
r
q
r
v
r
b
a
r
b
g
]
=
[
q
w
b
i
∗
(
p
w
b
j
−
p
w
b
i
−
v
i
w
Δ
t
+
1
2
g
w
Δ
t
2
)
−
α
b
i
b
j
2
[
q
q
b
j
∗
⊗
(
q
w
i
∗
⊗
q
w
b
j
)
]
x
y
z
q
w
b
i
∗
(
v
j
w
−
v
i
w
+
g
w
Δ
t
)
−
β
b
i
b
j
b
j
a
−
b
i
a
b
j
g
−
b
i
g
]
\left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \mathbf{r}_v \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{q}_{w b_i}^*\left(\mathbf{p}_{w b_j}-\mathbf{p}_{w b_i}-\mathbf{v}_i^w \Delta t+\frac{1}{2} \mathbf{g}^w \Delta t^2\right)-\boldsymbol{\alpha}_{b_i b_j} \\ 2\left[\mathbf{q}_{\mathbf{q}_{b_j}^*} \otimes\left(\mathbf{q}_{w_i}^* \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z} \\ \mathbf{q}_{w b_i}^*\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)-\boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_j^a-\mathbf{b}_i^a \\ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right]
rprqrvrbarbg
=
qwbi∗(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj2[qqbj∗⊗(qwi∗⊗qwbj)]xyzqwbi∗(vjw−viw+gwΔt)−βbibjbja−biabjg−big
待优化的变量是
[
p
w
b
i
q
w
b
i
v
i
w
b
i
a
b
i
g
]
[
p
w
b
j
q
w
b
j
v
j
w
b
j
a
b
j
g
]
\left[\begin{array}{lllll}\mathbf{p}_{w b_i} & \mathbf{q}_{w b_i} & \mathbf{v}_i^w & \mathbf{b}_i^a & \mathbf{b}_i^g\end{array}\right]\left[\begin{array}{lllll}\mathbf{p}_{w b_j} & \mathbf{q}_{w b_j} & \mathbf{v}_j^w & \mathbf{b}_j^a & \mathbf{b}_j^g\end{array}\right]
[pwbiqwbiviwbiabig][pwbjqwbjvjwbjabjg]
但在实际使用中,往往都是使用扰动量,因此实际是对以下变量求雅可比
[
δ
p
w
b
i
δ
θ
w
b
i
δ
v
i
w
δ
b
i
a
δ
b
i
g
]
[
δ
p
w
b
j
δ
θ
w
b
j
δ
v
j
w
δ
b
j
a
δ
b
j
g
]
\begin{aligned} & {\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_i} & \delta \theta_{w b_i} & \delta \mathbf{v}_i^w & \delta \mathbf{b}_i^a & \delta \mathbf{b}_i^g \end{array}\right]} \\ & {\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_j} & \delta \theta_{w b_j} & \delta \mathbf{v}_j^w & \delta \mathbf{b}_j^a & \delta \mathbf{b}_j^g \end{array}\right]} \end{aligned}
[δpwbiδθwbiδviwδbiaδbig][δpwbjδθwbjδvjwδbjaδbjg]此处只对几个比较复杂的雅可比进行推导,其余比较简单,感兴趣的可自行完成。
3.3 残差雅可比的推导
3.3.1 姿态残差的雅可比
-
对 i i i 时刻姿态误差的雅可比:
∂ r q ∂ δ θ b i b i ′ = ∂ 2 [ q b j b i ⊗ ( q b i w ⊗ q w b j ) ] x y z ∂ δ θ b i b i ′ = ∂ 2 [ q b i , b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ∗ ⊗ q w b j ] x y z ∂ δ θ b i b i ′ = ∂ − 2 [ ( q b i b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ∗ ⊗ q w b j ) ∗ ] x y z ∂ δ θ b i b i = ∂ − 2 [ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ⊗ q b i b j ] x y z ∂ δ θ b i b i ′ \begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} & =\frac{\partial 2\left[\mathbf{q}_{b_j b_i} \otimes\left(\mathbf{q}_{b_i w} \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial 2\left[\mathbf{q}_{b_i, b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_j}\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial-2\left[\left(\mathbf{q}_{b_i b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i} b_i^{\prime} \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_j}\right)^*\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i}} \\ & =\frac{\partial-2\left[\mathbf{q}_{w b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right) \otimes \mathbf{q}_{b_i b_j}\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \end{aligned} ∂δθbibi′∂rq=∂δθbibi′∂2[qbjbi⊗(qbiw⊗qwbj)]xyz=∂δθbibi′∂2[qbi,bj∗⊗(qwbi⊗[121δθbibi′])∗⊗qwbj]xyz=∂δθbibi∂−2[(qbibj∗⊗(qwbi⊗[121δθbibi′])∗⊗qwbj)∗]xyz=∂δθbibi′∂−2[qwbj∗⊗(qwbi⊗[121δθbibi′])⊗qbibj]xyz上式可以化简为:
∂ r q ∂ δ θ b i b i ′ = − 2 [ 0 I ] ∂ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ⊗ q b i b j ∂ δ θ b i b i ′ = − 2 [ 0 I ] ∂ [ q w b j ∗ ⊗ q w b i ] L [ q b i b j ] R [ 1 1 2 δ θ b i b i ′ ] ∂ δ θ b i b i ′ = − 2 [ 0 I ] [ q w b j ∗ ⊗ q w b i ] L [ q b i b j ] R [ 0 1 2 I ] \begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \frac{\partial \mathbf{q}_{w b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right) \otimes \mathbf{q}_{b_i b_j}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \frac{\partial\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i}\right]_L\left[\mathbf{q}_{b_i b_j}\right]_R\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i}\right]_L\left[\mathbf{q}_{b_i b_j}\right]_R\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \end{aligned} ∂δθbibi′∂rq=−2[0I]∂δθbibi′∂qwbj∗⊗(qwbi⊗[121δθbibi′])⊗qbibj=−2[0I]∂δθbibi′∂[qwbj∗⊗qwbi]L[qbibj]R[121δθbibi′]=−2[0I][qwbj∗⊗qwbi]L[qbibj]R[021I] -
对 j j j 时刻姿态误差的雅可比:
∂ r q ∂ δ θ b j b j ′ = ∂ 2 [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j ′ ] ] x y z ∂ δ θ b j b j ′ = ∂ 2 [ [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ] L [ 1 1 2 δ θ b j b j ′ ] ] x y z ∂ δ θ b j b j ′ = 2 [ 0 I ] [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ] L [ 0 1 2 I ] \begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} & =\frac{\partial 2\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_j b_j^{\prime}} \end{array}\right]\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} \\ & =\frac{\partial 2\left[\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_L\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_j b_j^{\prime}} \end{array}\right]\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} \\ & =2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_L\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \end{aligned} ∂δθbjbj′∂rq=∂δθbjbj′∂2[qbibj∗⊗qwbi∗⊗qwbj⊗[121δθbjbj′]]xyz=∂δθbjbj′∂2[[qbibj∗⊗qwbi∗⊗qwbj]L[121δθbjbj′]]xyz=2[0I][qbibj∗⊗qwbi∗⊗qwbj]L[021I] -
对 i i i 时刻陀螺仪bias误差的雅可比:
∂ r q ∂ δ b i g = ∂ 2 [ ( q b i b j ⊗ [ 1 1 2 J b i q δ b i g q ] ) ∗ ⊗ q w b i ∗ ⊗ q w b j ] x y z ∂ δ b i g = ∂ − 2 [ ( ( q b i b j ⊗ [ 1 1 2 J b i q q δ b i g ] ) ∗ ⊗ q w b i ∗ ⊗ q w b j ) ∗ ] x y z ∂ δ b i g = ∂ − 2 [ q w b j ∗ ⊗ q w b i ⊗ ( q b i b j ⊗ [ 1 1 2 J b i o δ b i g ] ) ] x y z ∂ δ b i g = − 2 [ 0 I ] [ q w b j ∗ ⊗ q w b i ⊗ q b i b j ] L [ 0 1 2 J b i q q ] \begin{aligned} & \frac{\partial \mathbf{r}_q}{\partial \delta \mathbf{b}_i^g}=\frac{\partial 2\left[\left(\mathbf{q}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^q \delta \mathbf{b}_i^g}^q \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =\frac{\partial-2\left[\left(\left(\mathbf{q}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^q}^q \delta \mathbf{b}_i^g \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right)^*\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =\frac{\partial-2\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i} \otimes\left(\mathbf{q}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^o} \delta \mathbf{b}_i^g \end{array}\right]\right)\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i} \otimes \mathbf{q}_{b_i b_j}\right]_L\left[\begin{array}{c} 0 \\ \frac{1}{2} \mathbf{J}_{b_i^q}^q \end{array}\right] \\ & \end{aligned} ∂δbig∂rq=∂δbig∂2[(qbibj⊗[121Jbiqδbigq])∗⊗qwbi∗⊗qwbj]xyz=∂δbig∂−2[((qbibj⊗[121Jbiqqδbig])∗⊗qwbi∗⊗qwbj)∗]xyz=∂δbig∂−2[qwbj∗⊗qwbi⊗(qbibj⊗[121Jbioδbig])]xyz=−2[0I][qwbj∗⊗qwbi⊗qbibj]L[021Jbiqq]
3.3.2 速度残差的雅可比
- 对
i
i
i 时刻姿态误差的雅可比:
∂ r v ∂ δ θ b i b i = ∂ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ] ) − 1 ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ ( R w b i exp ( [ δ θ b i b i ′ ] × ) ) − 1 ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ exp ( [ − δ θ b i b i ] × ) R b i w ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ ( I − [ δ θ b i b i ] × ) R b i w ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ − [ δ θ b i b i ] × R b i w ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = [ R b i w ( v j w − v i w + g w Δ t ) ] × \begin{aligned} \frac{\partial \mathbf{r}_v}{\partial \delta \boldsymbol{\theta}_{b_i b_i}} & =\frac{\partial\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i} \end{array}\right]\right)^{-1}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial\left(\mathbf{R}_{w b_i} \exp \left(\left[\delta \boldsymbol{\theta}_{b_i b_i^{\prime}}\right]_{\times}\right)\right)^{-1}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial \exp \left(\left[-\delta \boldsymbol{\theta}_{b_i b_i}\right]_{\times}\right) \mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i} b_i^{\prime}} \\ & =\frac{\partial\left(\mathbf{I}-\left[\delta \boldsymbol{\theta}_{b_i b_i}\right]_{\times}\right) \mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial-\left[\delta \boldsymbol{\theta}_{b_i b_i}\right]_{\times} \mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\left[\mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)\right]_{\times} \end{aligned} ∂δθbibi∂rv=∂δθbibi′∂(qwbi⊗[121δθbibi])−1(vjw−viw+gwΔt)=∂δθbibi′∂(Rwbiexp([δθbibi′]×))−1(vjw−viw+gwΔt)=∂δθbibi′∂exp([−δθbibi]×)Rbiw(vjw−viw+gwΔt)=∂δθbibi′∂(I−[δθbibi]×)Rbiw(vjw−viw+gwΔt)=∂δθbibi′∂−[δθbibi]×Rbiw(vjw−viw+gwΔt)=[Rbiw(vjw−viw+gwΔt)]× - 对
i
i
i 时刻速度误差的雅可比:
∂ r v ∂ δ v i w = − R b i w \frac{\partial \mathbf{r}_v}{\partial \delta \mathbf{v}_i^w}=-\mathbf{R}_{b_i w} ∂δviw∂rv=−Rbiw - 对
i
i
i 时刻加速度biasi吴差的雅可比:
∂ r v ∂ δ b i a = − ∂ β b i b j ∂ δ b i a = − J b i a β \frac{\partial \mathbf{r}_v}{\partial \delta \mathbf{b}_i^a}=-\frac{\partial \boldsymbol{\beta}_{b_i b_j}}{\partial \delta \mathbf{b}_i^a}=-\mathbf{J}_{b_i^a}^\beta ∂δbia∂rv=−∂δbia∂βbibj=−Jbiaβ
3.3.3 位置残差的雅可比
由于位置残差的形式与速度残差极其相似,因此不再重复推导。