视觉轮速滤波融合理论推导
文章目录
- 视觉轮速滤波融合理论推导
- 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
本文参考博客