视觉轮速滤波融合理论推导
文章目录
- 视觉轮速滤波融合理论推导
- 1 坐标系
- 2 轮速计
- 2.1 运动学模型
- 2.2 外参
- 3 状态和协方差矩阵
- 3.1 状态
- 3.2 协方差矩阵
- 4 Wheel Propagation
- 4.1 连续运动学
- 4.2 离散积分
- 4.2.1 状态均值递推
- 4.2.2 协方差递推
- 5 Visual update
- 5.1 视觉残差与雅可比
- 5.2 左零空间投影
- 5.3 降低计算量
- 5.4 边缘化
- 6 平面约束Update
- 6.1 平面约束本质
- 6.2 如何进行平面约束
1 坐标系
2 轮速计
2.1 运动学模型
这里介绍下轮速计的一个运动学模型，以阿克曼为例。如果一个小车，它沿着直线运动，那么它的左右轮速相等。如果不相等，意味着发生了旋转：
V
X
=
V
L
+
V
R
2
，
ω
=
V
X
/
R
V_{X}=\frac{V_{L}+V_{R}}{2}，ω = V_{X}/R
VX=2VL+VR，ω=VX/R
左右轮速的速度我们是可以计算得到的，但是半径R一开始肯定未知，所以利用下面公式计算，其中W是轮距，H是轴距！
V
X
=
V
L
+
V
R
2
,
ω
=
V
R
−
V
L
W
V_{X}=\frac{V_{L}+V_{R}}{2},ω =\frac{V_{R}-V_{L}}{W}
VX=2VL+VR,ω=WVR−VL
有了角速度和线速度，就可以计算出旋转半径R，然后利用轴距、轮距、R来计算左右前轮的偏角（θ=ωt）
A
n
g
l
e
L
=
tan
−
1
(
H
R
−
0.5
W
)
A
n
g
l
e
R
=
tan
−
1
(
H
R
+
0.5
W
)
\begin{aligned}Angle_L&=\tan^{-1}\big(\frac{H}{R-0.5W}\big)\\\\Angle_R&=\tan^{-1}\big(\frac{H}{R+0.5W}\big)\end{aligned}
AngleLAngleR=tan−1(R−0.5WH)=tan−1(R+0.5WH)
2.2 外参
与相机的姿态变换外参
O
C
R
与平移
C
p
O
_O^CR\text{ 与平移 }^Cp_O
OCR 与平移 CpO
3 状态和协方差矩阵
3.1 状态
类似于MSCKF，状态由两部分组成，一个是Odometry的位姿：
O
G
T
=
{
O
G
R
,
G
p
O
}
∈
SE
3
_O^GT=\{_O^GR,^Gp_O\}\in\text{SE}3
OGT={OGR,GpO}∈SE3
另一个是相机位姿（多帧）：
C
G
T
=
{
C
G
R
,
G
p
C
}
∈
SE
3
_C^GT=\{_C^GR,^Gp_C\}\in\text{SE}3
CGT={CGR,GpC}∈SE3
滑窗内状态变量即当前时刻Odometry位姿+N帧的相机位姿
KaTeX parse error: Got group of unknown type: 'internal'
3.2 协方差矩阵
里程计状态（姿态）协方差矩阵维度6*6，相机状态协方差矩阵维度6N*6N
P
k
:
k
=
[
P
O
O
k
:
k
P
O
C
k
:
k
P
O
C
k
:
k
T
P
C
C
k
:
k
]
P_{k:k}=\begin{bmatrix}P_{OO_{k:k}}&P_{OC_{k:k}}\\P_{OC_{k:k}}^T&P_{CC_{k:k}}\end{bmatrix}
Pk:k=[POOk:kPOCk:kTPOCk:kPCCk:k]
1 卡尔曼预测阶段更新里程计状态协方差，同时更新里程计与相机交叉部分状态协方差
其中，
P
O
O
k
+
1
∣
k
=
Φ
P
O
O
k
:
k
Φ
T
+
F
Q
F
T
\mathbf{P}_{OO_{k+1|k}} = \Phi P_{OO_{k:k}}\Phi^T+FQF^T
POOk+1∣k=ΦPOOk:kΦT+FQFT
P
k
+
1
∣
k
=
(
P
O
O
k
+
1
∣
k
Φ
k
P
O
C
k
∣
k
P
O
C
k
∣
k
⊤
Φ
k
⊤
P
C
C
k
∣
k
)
\left.\mathbf{P}_{k+1|k}=\left(\begin{matrix}\mathbf{P}_{OO_{k+1|k}}&\mathbf{\Phi}_{k}\mathbf{P}_{OC_{k|k}}\\\mathbf{P}_{OC_{k|k}}^\top\mathbf{\Phi}_{k}^\top&\mathbf{P}_{CC_{k|k}}\end{matrix}\right.\right)
Pk+1∣k=(POOk+1∣kPOCk∣k⊤Φk⊤ΦkPOCk∣kPCCk∣k)
2 状态增广：当新的frame到来，利用里程计位姿，通过外参预测相机位姿，更新 P k : k P_{k:k} Pk:k, 整体维度
（6+6N + 6）*（6+6N + 6）
C G R = O G R C O R G p C = G p O + O G R O p C \begin{array}{c}_C^GR={}_O^GR{}_C^OR\\^Gp_C={}^Gp_O+{}_O^GR{}^Op_C\end{array} CGR=OGRCORGpC=GpO+OGROpC
P ( 6 + 6 ( N + 1 ) ) × ( 6 + 6 ( N + 1 ) ) = [ I 6 + 6 N J 6 × ( 6 + 6 N ) ] P ( 6 + 6 N ) × ( 6 + 6 N ) [ I 6 + 6 N J 6 × ( 6 + 6 N ) ] T = [ P P J T J P J P J T ] \begin{gathered} P^{(6+6(N+1))\times(6+6(N+1))} \left.=\left[\begin{array}{c}I_{6+6N}\\J_{6\times(6+6N)}\end{array}\right.\right]P^{(6+6N)\times(6+6N)}\left[\begin{array}{c}I_{6+6N}\\J_{6\times(6+6N)}\end{array}\right]^T \\ \left.=\left[\begin{array}{cc}P&PJ^T\\JP&JPJ^T\end{array}\right.\right] \end{gathered} P(6+6(N+1))×(6+6(N+1))=[I6+6NJ6×(6+6N)]P(6+6N)×(6+6N)[I6+6NJ6×(6+6N)]T=[PJPPJTJPJT]
其中，雅可比是相机姿态对状态扰动量的雅可比，因为odometry预测相机姿态，所以和之前N帧相机无关，导数为0！
J
=
[
C
O
R
T
0
3
×
3
0
6
N
×
6
N
−
O
G
R
[
O
p
C
]
×
I
0
6
N
×
6
N
]
J=\begin{bmatrix}^O_CR^T&0_{3\times3}&0_{6N\times6N}\\-_O^GR{\left[^Op_C\right]}_\times&I&0_{6N\times6N}\end{bmatrix}
J=[CORT−OGR[OpC]×03×3I06N×6N06N×6N]
重点补充下第一列的雅可比求解推导（要搞清楚是左/右扰动）
1 平移 G p C ^Gp_C GpC对 O G R _O^GR OGR的雅可比，注意是右扰动！若按左扰动求导，结果应该是 − [ O G R O p C ] -{\left[_O^GR^Op_C\right]} −[OGROpC]
推导过程如下
4 Wheel Propagation
论文Localization for Ground Robots: On Manifold Representation, Integration
4.1 连续运动学
VIO里面使用IMU去做预测这部分，这里是利用轮速计去做propagation。
O G R ˙ = O G R [ O ω O ] × G p O ˙ = G v O = O G R O v O \begin{aligned}\dot{_O^GR}&= ^G_OR[^O\omega_O]_\times\\^G\dot{p_O}&={}^Gv_O={}_O^GR^Ov_O\end{aligned} OGR˙GpO˙=OGR[OωO]×=GvO=OGROvO
其中
O
ω
O
^O\omega_O
OωO是轮速坐标系下的顺时角速度，等同IMU测量的角速度。但是因为轮速计只有2D方面的旋转，一般只有偏航角，即只能测到绕z轴的角速度。下面式子中b是轮距，
k
l
,
k
r
k_l,k_r
kl,kr分别是左右轮速系数。
O
ω
O
=
[
0
0
k
r
⋅
v
r
−
k
l
⋅
v
l
b
]
\left.^O\omega_O=\left[\begin{array}{c}0\\0\\\frac{k_r\cdot v_r-k_l\cdot v_l}b\end{array}\right.\right]
OωO=
00bkr⋅vr−kl⋅vl
O
v
O
^Ov_O
OvO是瞬时速度，轮速本身只能测量沿x轴方向的速度（在世界系并不是）。
O
v
O
=
[
k
l
⋅
v
l
+
k
r
⋅
v
r
2
0
0
]
\left.^Ov_O=\left[\begin{array}{c}\frac{k_l\cdot v_l+k_r\cdot v_r}{2}\\0\\0\end{array}\right.\right]
OvO=
2kl⋅vl+kr⋅vr00
4.2 离散积分
4.2.1 状态均值递推
对上面得到的常微分方程进行离散化，使用欧拉积分，认为dt时间内变量未改变，根据定积分公式，以平移p为例，可以得到下式：
常见的一种运动学积分公式（欧拉积分）
O G R k + 1 = O G R k E x p ( O ω O Δ t ) ⏟ Δ R G p O k + 1 = G p O k + O G R k O v O Δ t ⏟ Δ p \begin{gathered} _{O}^{G}R_{k+1} ={}_O^GR_k\underbrace{\mathrm{Exp}({}^O\omega_O\Delta t)}_{\Delta R} \\ ^Gp_{O_{k+1}} ={}^Gp_{Ok}+{}_O^GR_k\underbrace{^Ov_O\Delta t}_{\Delta p} \end{gathered} OGRk+1=OGRkΔR Exp(OωOΔt)GpOk+1=GpOk+OGRkΔp OvOΔt
4.2.2 协方差递推
对于协方差的Propgation，我们先求雅克比。
这里先引出论文中的公式，貌似顺序写反了，δp在前，δθ在后，更多详细推导可以参考论文
我们这里先不管后面那个m，因为这个递推公式是连续的，所以我们把它离散化。在MSCKF中，利用泰勒展开去近似
Fc得到状态转移矩阵Φ，简单起见，用一阶泰勒展开δx' = (I + Fc*dt)δx + G,Q是噪声矩阵。下图展示了状态转移矩阵的推导。
Φ
=
[
Exp
(
O
ω
O
Δ
t
)
T
0
−
O
G
R
k
[
O
v
O
Δ
t
]
×
I
]
\Phi=\begin{bmatrix}\operatorname{Exp}(^O\omega_O\Delta t)^T&0\\-_O^GR_k[^Ov_O\Delta t]_\times&I\end{bmatrix}
Φ=[Exp(OωOΔt)T−OGRk[OvOΔt]×0I]
G ′ = [ I 0 0 O G R k ] \left.G'=\left[\begin{array}{cc}I&0\\0&{}_{O}^{G}R_{k}\end{array}\right.\right] G′=[I00OGRk]
卡尔曼预测阶段协方差更新
P O O k + 1 ∣ k = Φ P O O k : k Φ T + G ′ Q G ′ T \mathbf{P}_{OO_{k+1|k}} = \Phi P_{OO_{k:k}}\Phi^T+G'QG'^T POOk+1∣k=ΦPOOk:kΦT+G′QG′T
5 Visual update
5.1 视觉残差与雅可比
MSCKF的边缘化操作：没有把特征点放到状态向量里面，降低了计算量。
当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：
- 一种是跟踪丢失，也就是当前帧跟踪不到的那些特征点；
- 另一种是边缘化的时候，也就是把最后一帧滑出窗口的时候，把在这一帧里面新建的特征点都丢弃，也就是都拿来做更新。
这里更详细的推导，可以参考之前MSCKF的博客
r i j = z i j − z ^ i j = H C i j x ~ C i + H f i j G p ~ j + n i j \mathbf{r}_i^j=\mathbf{z}_i^j-\hat{\mathbf{z}}_i^j=\mathbf{H}_{C_i}^j\tilde{\mathbf{x}}_{C_i}+\mathbf{H}_{f_i}^j{}^G\tilde{\mathbf{p}}_j+\mathbf{n}_i^j rij=zij−z^ij=HCijx~Ci+HfijGp~j+nij
5.2 左零空间投影
左零空间投影—QR分解
r 0 ( 2 M − 3 M L ) × 1 = A T r 2 M × 1 ≅ A T H X 2 M × ( 15 + 6 N ) ⏟ H 0 X ~ ( 15 + 6 N ) × 1 + A T n 2 M × 1 ⏟ n 0 = H 0 ( 2 M − 3 ) × ( 15 + 6 N ) X ~ ( 15 + 6 N ) × 1 + n 0 ( 2 M − 3 ) × 1 \begin{aligned}r_0^{(2M-3M_L)\times1}&=\mathrm{A}^Tr^{2M\times1}\cong\underbrace{\mathrm{A}^TH_X^{2M\times(15+6N)}}_{H_0}\tilde{X}^{(15+6N)\times1}+\underbrace{\mathrm{A}^Tn^{2M\times1}}_{n_0}\\&={H_0}^{(2M-3)\times(15+6N)}\tilde{X}^{(15+6N)\times1}+{n_0}^{(2M-3)\times1}\end{aligned} r0(2M−3ML)×1=ATr2M×1≅H0 ATHX2M×(15+6N)X~(15+6N)×1+n0 ATn2M×1=H0(2M−3)×(15+6N)X~(15+6N)×1+n0(2M−3)×1
5.3 降低计算量
QR分解降低计算量
投影完之后，我们就可以得到新的残差和相应的雅可比矩阵
H
0
H_{0}
H0。但是
H
0
H_{0}
H0矩阵通常维度很大，为了降低计算量，对
H
0
H_{0}
H0进行QR分解：
QR分解会得到一个正交矩阵和一个上三角矩阵！正交矩阵的转置和其本身的乘积是单位矩阵，且该矩阵的所有列向量彼此正交（内积为0）
H
0
(
2
M
−
3
)
×
(
15
+
6
N
)
=
[
Q
1
Q
2
]
[
T
H
0
]
{H_0}^{(2M-3)\times(15+6N)}=[Q_1\quad Q_2]\begin{bmatrix}T_H\\0\end{bmatrix}
H0(2M−3)×(15+6N)=[Q1Q2][TH0]
对上面新的残差左边乘以
[
Q
1
T
Q
2
T
]
\left.\left[\begin{array}{cc}\mathbf{Q}_1^T\\\mathbf{Q}_2^T\end{array}\right.\right]
[Q1TQ2T],得到
[
Q
1
T
r
0
Q
2
T
r
0
]
=
[
T
H
0
]
X
~
+
[
Q
1
T
n
0
Q
2
T
n
0
]
\begin{bmatrix}Q_1^Tr_0\\Q_2^Tr_0\end{bmatrix}=\begin{bmatrix}T_H\\0\end{bmatrix}\tilde{X}+\begin{bmatrix}Q_1^Tn_0\\Q_2^Tn_0\end{bmatrix}
[Q1Tr0Q2Tr0]=[TH0]X~+[Q1Tn0Q2Tn0]
5.4 边缘化
边缘化，就是说如何删除滑窗里的状态，这里使用最简单的策略，把最老的一帧去掉，删掉协方差矩阵中对应块，去掉的这帧里的所有特征点都被拿来做更新。
6 平面约束Update
参考论文：VINS on wheels," 2017 (ICRA)
当车辆在在平面上运动时，在更新的时候，我们引入一个平面约束.
6.1 平面约束本质
为了解释这个问题，首先解释下旋转矩阵的含义，更多请参考之前博客
刚体坐标系B相对于世界坐标系A的的姿态—旋转矩阵。分别把B下的三个轴分别投影到世界坐标系下的三个轴，下面矩阵给出了对应的投影分量。
B
A
R
=
[
∣
∣
∣
A
X
^
B
A
Y
^
B
A
Z
^
B
∣
∣
∣
]
=
[
X
^
B
⋅
X
^
A
Y
^
B
⋅
X
^
A
Z
^
B
⋅
X
^
A
X
^
B
⋅
Y
^
A
Y
^
B
⋅
Y
^
A
Z
^
B
⋅
Y
^
A
X
^
B
⋅
Z
^
A
Y
^
B
⋅
Z
^
A
Z
^
B
⋅
Z
^
A
]
{ }^A _B{R}=\left[\begin{array}{ccc} \mid & \mid & \mid \\ { }^A \widehat{X}_B & { }^A \widehat{Y}_B & { }^A \hat{Z}_B \\ \mid & \mid & \mid \end{array}\right]=\left[\begin{array}{ccc} \hat{X}_B \cdot \hat{X}_A & \hat{Y}_B \cdot \hat{X}_A & \hat{Z}_B \cdot \hat{X}_A \\ \hat{X}_B \cdot \hat{Y}_A & \hat{Y}_B \cdot \hat{Y}_A & \hat{Z}_B \cdot \hat{Y}_A \\ \hat{X}_B \cdot \hat{Z}_A & \hat{Y}_B \cdot \hat{Z}_A & \hat{Z}_B \cdot \hat{Z}_A \end{array}\right]
BAR=
∣AX
B∣∣AY
B∣∣AZ^B∣
=
X^B⋅X^AX^B⋅Y^AX^B⋅Z^AY^B⋅X^AY^B⋅Y^AY^B⋅Z^AZ^B⋅X^AZ^B⋅Y^AZ^B⋅Z^A
也就是说，矩阵
B
A
R
^A_BR
BAR的第3列就是坐标系B的z轴在A系下的投影。对于
O
G
R
^G_OR
OGR来讲，前面定义世界系与初始轮速坐标系重合，轮速只有绕z轴的旋转，所以坐标系{G}和坐标系{O}的z轴是平行的。平面约束的本质就是让
O
G
R
^G_OR
OGR的右上角2*1向量为0。
6.2 如何进行平面约束
平面约束是Odometry的坐标的X-O-Y面与一个平面重合。我们这里设定平面 {Π} 为初始时刻Odometry坐标系的X-O-Y平面，即世界系。
0 2 × 1 = Λ G π R O G R ⏟ O π R e 3 = Λ O G R e 3 0_{2\times1}=\Lambda\underbrace{_G^\pi R_{O}^GR}_{_{O}^{\pi}R}e_3 = \Lambda^G_ORe_3 02×1=ΛOπR GπROGRe3=ΛOGRe3
其中，
Λ
\Lambda
Λ是为了将一个三维列向量取出前面2*1向量，
e
3
e_3
e3就是Odometry坐标系z下轴单位向量，投影到世界系后，取出前面二维，应该是一个0向量！
Λ = [ 1 0 0 0 1 0 ] , e 3 = [ 0 0 1 ] \Lambda=\begin{bmatrix}1&0&0\\0&1&0\end{bmatrix},e_3=\begin{bmatrix}0\\0\\1\end{bmatrix} Λ=[100100],e3= 001
本文参考博客
