【OpenVINS】（一）ZUPT

参考：Measurement Update Derivations » Zero Velocity Update

---

在典型的自主汽车场景中，传感器系统将在停止灯处变得静止，其中动态物体（例如交叉路口的其他车辆）可能会快速损坏系统，通过零速检测识别当前车辆处于静止状态，以暂停特征跟踪，来避免定位系统崩溃

---

1. Inertial-based Detection

1.1 误差构造

当车辆静止（对于车辆匀速直线运动这种case可以通过速度幅值检测剔除），加速度和角速度先验值都是0，先验也是一种观测，所以构造如下观测方程：
$$\tilde{\mathbf{z}}=\left[\begin{array}{c}\mathbf{a}-\left({\mathbf{a}}_m-{\mathbf{b}}_a-{}_G^{I_k}{\mathbf{R}}^G\mathbf{g}-{\mathbf{n}}_a\right)\\ \mathbf{\omega }-\left({\mathbf{\omega }}_m-{\mathbf{b}}_g-{\mathbf{n}}_g\right)\end{array}\right]=\left[\begin{array}{c}-\left({\mathbf{a}}_m-{\mathbf{b}}_a-{}_G^{I_k}{\mathbf{R}}^G\mathbf{g}-{\mathbf{n}}_a\right)\\ -\left({\mathbf{\omega }}_m-{\mathbf{b}}_g-{\mathbf{n}}_g\right)\end{array}\right]$$

openvins代码也利用速度为0的先验，构造误差方程，
$$0=v_{k+1}=v_k-g\ast dt+R^T\ast (a_m-b_a-n_a)\ast dt$$

此误差与上面的 $\tilde{\mathbf{z}}$ 中以加速度为 $0$ 的先验构造的误差项可认为是等价的，只需要使用一个即可

1.2 计算雅可比

$$\begin{array}{rl}\mathbf{a}& ={\mathbf{a}}_m-{\mathbf{b}}_a-{}_G^{I_k}{\mathbf{R}}^G\mathbf{g}-{\mathbf{n}}_a\\ \mathbf{\omega }& ={\mathbf{\omega }}_m-{\mathbf{b}}_g-{\mathbf{n}}_g\end{array}$$

对上式一阶泰勒展开，即可得到雅可比
$$\begin{array}{rl}\frac{\partial \tilde{\mathbf{z}}}{{\partial }_G^{I_k}\mathbf{R}}& =-\lfloor {}_G^{I_k}{\mathbf{R}}^G\mathbf{g}\times \rfloor \\ \frac{\partial \tilde{\mathbf{z}}}{\partial {\mathbf{b}}_a}& =\frac{\partial \tilde{\mathbf{z}}}{\partial {\mathbf{b}}_g}=-{\mathbf{I}}_{3\times 3}\end{array}$$

注意，这里使用的JPL四元数，所以关于${}_G^{I_k}\mathbf{R}$的雅可比前面会多一个负号

1.3 卡方分布来确定当前车辆是否处于静止状态

$${\tilde{\mathbf{z}}}^{\top }{\left({\mathbf{HPH}}^{\top }+\alpha \mathbf{R}\right)}^{-1}\tilde{\mathbf{z}}<{\chi }^2$$

• $\mathbf{P}$表示状态${}_G^{I_k}{\mathbf{R}}^G,{\mathbf{b}}_g,{\mathbf{b}}_a$的协方差，观测方程会使用两帧图像中间所有的imu测量值构造（每个测量值构造一组观测方程），方程中关于状态的协方差并非都是常量，
  • $\mathbf{P}$中关于${}_G^{I_k}{\mathbf{R}}^G$的协方差是常量，就是$k$时刻姿态的协方差
  • 而${\mathbf{b}}_g,{\mathbf{b}}_a$的协方差则需要从$k$时刻积分到当前imu测量值时刻$t$，这段时间记为$\Delta t$，则${\Sigma }_t^{{\mathbf{b}}_g,{\mathbf{b}}_a}={\Sigma }_{continuous}^{{\mathbf{b}}_g,{\mathbf{b}}_a}\Delta t$，也就是说ZUPT中不只是有观测方程，还隐含了一个这样的state propagation的过程
  • openvins为了简便做了近似，使用两帧图像间的时间差$\Delta t_{k,k+1}$替代$\Delta t$
• 实际使用中，IMU的噪声$\mathbf{R}$需要放大50-100倍，因为我们有可能低估了我们的IMU的噪声，以及IMU挂在的车体本身存在振动

1.4 状态更新

• 对状态${\mathbf{b}}_g,{\mathbf{b}}_a$进行state propagation，即更新状态协方差
• 执行正常的KF更新步骤

---

2. Disparity-based Detection

$$\frac{1}{N}\sum _{i=0}^N\Vert {\mathbf{uv}}_{k,i}-{\mathbf{uv}}_{k-1,i}\Vert <\Delta d$$

注意，这种方式在动态环境中将会失效，所以需要配合第一种方式一起使用

---

@leatherwang

二零二一年十月三日

---