基于可观性分析的GNSS/IMU/视觉融合定位算法

摘要

作为决策、规划和控制的基础，精确的位姿估计对于智能汽车极为重要。为提高车辆的位姿中的航向估计精度，本文提出一种基于可观性的全球导航卫星系统（GNSS）、惯性测量单元（IMU）的视觉融合定位算法。首先，为分析GNSS/IMU组合定位误差状态的可观性，提出一种新的相对可观性分析方法，结果表明传统的GNSS/INS组合定位算法存在4个弱可观状态；随后，基于相对可观性理论，利用视觉惯性里程计估计的相对航向角，提出了一种基于可观性的融合定位算法；最后，实验验证了相对可观性分析的有效性，实验结果显示所提定位算法的最大航向误差为2.76°，航向误差RMS为1°，表明所提算法可以有效提高弱可观状态下车辆的航向估计精度。

关键词

融合定位; 全球导航卫星系统（GNSS）; 惯性测量单元（IMU）; 视觉惯性里程计; 可观性

精确的姿态、速度和位置估计对于智能车辆尤为重要^［1］。目前，全球导航卫星系统/惯性测量单元（GNSS/IMU）组合定位对车辆的位姿具有良好的精度和鲁棒性^［2-4］。但GNSS/IMU系统的位姿估计误差与系统的可观性相关，在特定运动下系统存在不可观的缺陷^［5-7］。为达到更好的可观性，要求车辆进行充分的加速或转向运动^［8-10］。加速度变化可以提高陀螺零偏和姿态误差的可观性^［7-8］。但在充分运动之前，车辆的某些状态始终处于弱可观状态，这会对车辆姿态的估计产生不良的影响^［11］。因此，研究提高系统可观性的多传感器融合定位算法具有显著的意义。

传统的可观性分析可观性矩阵的秩，只能得到一个可观测或不可观测的结果，而不能得到可观性的强度^［12］。为定量地描述系统可观性大小，Ham^［13］提出用卡尔曼滤波器估计误差协方差矩阵的特征值和特征向量来描述系统状态的可观性。这种方法对误差协方差矩阵的初始值非常敏感，可观性与误差协方差之间的关系可能会产生误导性^［14］。为了反映估计精度，有学者使用了一种基于奇异值分解（SVD）的可观性分析方法^［15-17］。但SVD方法导致不同量纲的状态可观性差异较大，有些状态的可观测性甚至可能大于1^［18］，与实际情况不符。马艳红等^［19］指出，SVD方法进行无量纲化处理后，得到系统状态可观度的大小关系有可能发生变化，此方法在理论上存在缺陷。对于GNSS/IMU系统，有学者提出瞬时可观度概念^［20］，但没有考虑姿态误差，同时没有区分加速度计和陀螺的零偏。

为避免量纲差异的不良影响，针对GNSS/INS组合定位系统，本文提出一种全新的相对可观性分析方法，分析了各个状态量的可观性，结论表明传统GNSS/IMU系统存在4个运动相关的弱可观误差状态。同时，根据系统的可观性缺陷，使用视觉惯性里程计估计车辆的相对航向角，设计了融合GNSS/IMU/视觉信息的卡尔曼滤波算法。最后通过实验证明，相比于传统GNSS/IMU组合定位算法，本文所提算法能有效提高弱可观条件下车辆航向估计精度。

1 GNSS/IMU相对可观性分析

在本节中，针对GNSS/IMU系统提出了一种新的相对可观性分析方法。与传统方法不同的是，本研究通过可观性矩阵的秩，分析系统整体的可观性，再通过将可观性矩阵解耦，分析相同或相似量纲的状态量的可观性差异。

1.1　GNSS/IMU模型

本文中用到的坐标系定义如下：以地球为中心的惯性系用i表示；以地球为中心的地心地固系用E表示；当地导航坐标系用N表示；载体坐标系用b表示。采用姿态角误差、速度误差、位置误差、角速度零偏和加速度计零偏作为状态向量：

$$\begin{array}{l}\mathit{X}=[{\varphi }_E,{\varphi }_N,{\varphi }_U,\mathrm{\delta }{V}_E,\mathrm{\delta }{V}_N,\mathrm{\delta }{V}_U,\mathrm{\delta }L,\mathrm{\delta }\lambda ,\mathrm{\delta }h,\\ {\epsilon }_x,{\epsilon }_y,{\epsilon }_z,{\nabla }_x,{\nabla }_y,{\nabla }_z{]}^{\mathrm{T}}\end{array}$$

（1）

式中： $\varphi$和$\mathrm{\delta }V$表示当地导航坐标系中的姿态误差和速度误差； $\mathrm{\delta }L\mathrm{、}\mathrm{\delta }\lambda \mathrm{、}\mathrm{\delta }h$分别是纬度、经度和高度的误差； $\epsilon$和$\nabla$分别代表载体坐标系下的陀螺和加速度计的零偏。

选择GNSS与IMU之间的位置误差作为系统的观测量，GNSS/IMU的状态方程与观测方程可以表示为：

$$\left\{\begin{array}{l}\dot{\mathit{X}}=\mathit{F}\mathit{X}+\mathit{w}\\ \mathit{Z}=\mathit{H}\mathit{X}+\mathit{v}\end{array}\right.$$

（2）

式中： F是15×15维状态转移矩阵，H是测量矩阵；w是包含陀螺和加速度计随机噪声的系统噪声；v是GNSS与IMU位置误差的测量噪声。

测量矩阵可以表示为：

$$\mathit{H}=\left[\begin{array}{ccc}{\mathrm{0}}_{\mathrm{3}\times \mathrm{6}}& {\mathit{I}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{6}}\end{array}\right]$$

（3）

状态转移矩阵可以表示为：

$$\mathit{F}=\left[\begin{array}{ccccc}-w_{in}^n\times & F_{\mathrm{12}}^n& F_{\mathrm{13}}^n& {\widehat{C}}_b^n& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ -{\widehat{C}}_b^n{f}_{ib}^b\times & F_{\mathrm{22}}^n& F_{\mathrm{23}}^n& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\widehat{C}}_b^n\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n& F_{\mathrm{33}}^n& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\end{array}\right]$$

（4）

对于消费级IMU，地球自转角速度$w_{ie}^n$和转移角速度$w_{en}^n$远小于陀螺噪声，因而其对系统可观性几乎没有影响。因此，在重力建模准确的情况下，重力误差、地球角速度和转移角速度可以忽略，经离散化和简化后的状态转移矩阵可以表示为：

$$\mathit{F}=\left[\begin{array}{ccccc}{I}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\widehat{C}}_b^n\mathrm{\delta }t& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ -{\widehat{C}}_b^n{f}_{ib}^b\times \delta t& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\widehat{C}}_b^n\mathrm{\delta }t\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n\delta t& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}\end{array}\right]$$

$${\mathit{F}}_{\mathrm{32}}^n=\left[\begin{array}{ccc}\mathrm{1}/(R_N+h_b)& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{1}/(R_E+h_b)\mathrm{c}\mathrm{o}\mathrm{s}{L}_b& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& -\mathrm{1}\end{array}\right]$$

（5）

式中： $R_N$是子午圈半径；$R_E$是卯酉圈半径；h和L是车辆的高度和纬度。

1.2　可观性矩阵解耦分析

根据可观性理论^［21］，对于某个时间段，可以得到式（2）所表示的GNSS/IMU系统的可观性矩阵的表达式：

$${\mathit{Q}}_i{\mathit{X}}_{\mathrm{0}}=\mathit{Z}$$

$${\mathit{Q}}_i=\left[H_i,H_i{F}_{\mathrm{1}},H_i{F}_{\mathrm{1}}{F}_{\mathrm{2}},\dots ,H_i{F}_{\mathrm{1}}{F}_{\mathrm{2}}\cdots F_n\right]$$

（6）

在可观性矩阵中代入状态转移矩阵和观测矩阵表达式，并对矩阵进行解耦：

$${\mathit{Q}}_i={\left[\begin{array}{ccccc}{\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n\delta t& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ -F_{\mathrm{32}}^n{f}_{ib}^n\times \delta t^{\mathrm{2}}& \mathrm{2}{F}_{\mathrm{32}}^n\delta t& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n{C}_b^n\delta t^{\mathrm{2}}\\ -\mathrm{3}{F}_{\mathrm{32}}^n{f}_{ib}^n\times \delta t^{\mathrm{2}}& \mathrm{3}{F}_{\mathrm{32}}^n\delta t& I_{\mathrm{3}\times \mathrm{3}}& -F_{\mathrm{32}}^n{f}_{ib}^n\times C_b^n\delta t^{\mathrm{3}}& \mathrm{3}{F}_{\mathrm{32}}^n{C}_b^n\delta t^{\mathrm{2}}\\ -\mathrm{6}{F}_{\mathrm{32}}^n{f}_{ib}^n\times \delta t^{\mathrm{2}}& \mathrm{4}{F}_{\mathrm{32}}^n\delta t& I_{\mathrm{3}\times \mathrm{3}}& -\mathrm{4}{F}_{\mathrm{32}}^n{f}_{ib}^n\times C_b^n\delta t^{\mathrm{3}}& \mathrm{6}{F}_{\mathrm{32}}^n{C}_b^n\delta t^{\mathrm{2}}\\ -\mathrm{10}{F}_{\mathrm{32}}^n{f}_{ib}^n\times \delta t^{\mathrm{2}}& \mathrm{5}{F}_{\mathrm{32}}^n\delta t& I_{\mathrm{3}\times \mathrm{3}}& -\mathrm{10}{F}_{\mathrm{32}}^n{f}_{ib}^n\times C_b^n\delta t^{\mathrm{3}}& \mathrm{10}{F}_{\mathrm{32}}^n{C}_b^n\delta t^{\mathrm{2}}\\ ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]}_{n\times \mathrm{15}}={\mathit{M}}_{n\times \mathrm{15}}{\mathit{D}}_{\mathrm{15}\times \mathrm{15}}$$

$${\mathit{M}}_{n\times \mathrm{15}}=$$$${\left[\begin{array}{ccccc}{\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ I_{\mathrm{3}\times \mathrm{3}}& \mathrm{2}{I}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}\\ \mathrm{3}{I}_{\mathrm{3}\times \mathrm{3}}& \mathrm{3}{I}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& \mathrm{3}{I}_{\mathrm{3}\times \mathrm{3}}\\ \mathrm{6}{I}_{\mathrm{3}\times \mathrm{3}}& \mathrm{4}{I}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& \mathrm{4}{I}_{\mathrm{3}\times \mathrm{3}}& \mathrm{6}{I}_{\mathrm{3}\times \mathrm{3}}\\ \mathrm{10}{I}_{\mathrm{3}\times \mathrm{3}}& \mathrm{5}{I}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& \mathrm{10}{I}_{\mathrm{3}\times \mathrm{3}}& \mathrm{10}{I}_{\mathrm{3}\times \mathrm{3}}\\ ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]}_{n\times \mathrm{15}}$$

$${\mathit{D}}_{\mathrm{15}\times \mathrm{15}}={\left[\begin{array}{ccccc}-F_{\mathrm{32}}^n{f}_{ib}^n\times \delta t^{\mathrm{2}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n\delta t& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& I_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n{f}_{ib}^n\times C_b^n\delta t^{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& F_{\mathrm{32}}^n{C}_b^n\delta t^{\mathrm{2}}\end{array}\right]}_{\mathrm{15}\times \mathrm{15}}$$

（7）

式（7）中： ${\mathit{M}}_{n\times \mathrm{15}}$代表由单位阵构成的系数矩阵； ${\mathit{D}}_{\mathrm{15}\times \mathrm{15}}$则代表对角阵。系数矩阵${\mathit{M}}_{n\times \mathrm{15}}$的秩为12，第一列与第五列相同，因此在某一时刻系统至少具有3个弱可观状态量。

根据${\mathit{M}}_{n\times \mathrm{15}}$和状态向量X，可知位置误差和速度误差具有较强的可观性，但是姿态误差$\varphi$、陀螺零偏$\epsilon$和加速度计零偏$\nabla$存在耦合。纬度误差$\mathrm{\delta }L$、经度误差$\mathrm{\delta }\lambda$和高度误差$\mathrm{\delta }h$的量纲不同，经纬度误差以弧度表示，而高度误差通常以米表示，这导致$F_{\mathrm{32}}^n$对角线上的数值大小差异很大。但位置误差的可观性应当是相同的，量纲的差异实际上并不影响可观性的大小，因此后续分析中不考虑对角阵$F_{\mathrm{32}}^n$的影响。

由于${\mathit{M}}_{n\times \mathrm{15}}$不影响相同量纲状态量的可观性，因而相同量纲状态量的可观性差异来源于${\mathit{D}}_{\mathrm{15}\times \mathrm{15}}$，其中的$F_{\mathrm{32}}^n$不影响各状态量的可观性。比较${\mathit{D}}_{\mathrm{15}\times \mathrm{15}}$和状态向量X，可以清楚地表明$f_{ib}^n\times$、$f_{ib}^n\times C_b^n$和$C_b^n$分别是导致姿态误差、陀螺零偏和加速度计零偏的可观性因子。

1.3　相对可观性分析

根据1.2节结论，将可观性因子与对应状态相乘，分别分析姿态误差、陀螺零偏和加速度零偏的相对可观性。姿态误差的相对可观性矩阵为：

$${\mathit{f}}_{ib}^n\times \mathit{\varphi }=\left[\begin{array}{ccc}\mathrm{0}& -f_{ib,z}^n& f_{ib,y}^n\\ f_{ib,z}^n& \mathrm{0}& -f_{ib,x}^n\\ -f_{ib,y}^n& f_{ib,x}^n& \mathrm{0}\end{array}\right]\left[\begin{array}{c}{\varphi }_E\\ {\varphi }_N\\ {\varphi }_U\end{array}\right]=\left[\begin{array}{c}-f_{ib,z}^n{\varphi }_N+f_{ib,y}^n{\varphi }_U\\ f_{ib,z}^n{\varphi }_E-f_{ib,x}^n{\varphi }_U\\ -f_{ib,y}^n{\varphi }_E+f_{ib,x}^n{\varphi }_N\end{array}\right]$$

（8）

${\mathit{f}}_{ib}^n\times$是秩为2的反对称矩阵，这表明姿态误差不是完全可观的，因此侧倾角误差和俯仰角误差相比于横摆角误差有更好的可观度。加速度计零偏的相对可观性矩阵为：${\mathit{C}}_b^n=\left[\begin{array}{ccc}{\nabla }_x& {\nabla }_y& {\nabla }_z\end{array}\right]$。式（9）表明姿态与加速度计零偏耦合，其中航向角$\phi$仅与${\nabla }_x$和${\nabla }_y$相关。由于航向角误差存在弱可观性，航向角$\phi$的估计误差较之于侧倾角/俯仰角更大，因而${\nabla }_z$比${\nabla }_x$和${\nabla }_y$有更好的可观性。

$${\mathit{C}}_b^n=\left[\begin{array}{ccc}\mathrm{1}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}\psi & \mathrm{s}\mathrm{i}\mathrm{n}\psi \\ \mathrm{0}& -\mathrm{s}\mathrm{i}\mathrm{n}\psi & \mathrm{c}\mathrm{o}\mathrm{s}\psi \end{array}\right]\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\theta & \mathrm{0}& -\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ \mathrm{0}& \mathrm{1}& \mathrm{0}\\ \mathrm{s}\mathrm{i}\mathrm{n}\theta & \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}\theta \end{array}\right]\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{0}\\ -\mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]$$

（9）

陀螺零偏的相对可观性矩阵为：

$$\begin{array}{l}{\mathit{f}}_{ib}^n\times {\mathit{C}}_b^n\left[\begin{array}{c}{\epsilon }_x\\ {\epsilon }_y\\ {\epsilon }_z\end{array}\right]={\mathit{C}}_b^n{\mathit{f}}_{ib}^b\times {\mathit{C}}_b^n\left[\begin{array}{c}{\epsilon }_x\\ {\epsilon }_y\\ {\epsilon }_z\end{array}\right]=\tilde{C}\left[\begin{array}{c}{\epsilon }_x\\ {\epsilon }_y\\ {\epsilon }_z\end{array}\right]\\ \tilde{\mathit{C}}={\mathit{C}}_b^n{\mathit{f}}_{ib}^b\times {\mathit{C}}_b^n={\mathit{C}}_b^n\left[\begin{array}{ccc}\mathrm{0}& -f_{ib,z}^b& f_{ib,y}^b\\ f_{ib,z}^b& \mathrm{0}& -f_{ib,x}^b\\ -f_{ib,y}^b& f_{ib,x}^b& \mathrm{0}\end{array}\right]{\mathit{C}}_b^n\approx \\ \left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{0}\\ -\mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]\left[\begin{array}{ccc}\mathrm{0}& -f_{ib,z}^b& f_{ib,y}^b\\ f_{ib,z}^b& \mathrm{0}& -f_{ib,x}^b\\ -f_{ib,y}^b& f_{ib,x}^b& \mathrm{0}\end{array}\right]\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{0}\\ -\mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]=\\ \left[\begin{array}{ccc}\mathrm{2}\mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{c}\mathrm{o}\mathrm{s}\phi f_{ib,z}^b& (\mathrm{s}\mathrm{i}{\mathrm{n}}^{\mathrm{2}}\phi -\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\phi )f_{ib,z}^b& \mathrm{c}\mathrm{o}\mathrm{s}\phi f_{ib,y}^b-\mathrm{s}\mathrm{i}\mathrm{n}\phi f_{ib,x}^b\\ (\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\phi -\mathrm{s}\mathrm{i}{\mathrm{n}}^{\mathrm{2}}\phi )f_{ib,z}^b& \mathrm{2}\mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{c}\mathrm{o}\mathrm{s}\phi f_{ib,z}^b& -\mathrm{s}\mathrm{i}\mathrm{n}\phi f_{ib,y}^b-\mathrm{c}\mathrm{o}\mathrm{s}\phi f_{ib,x}^b\\ \mathrm{c}\mathrm{o}\mathrm{s}\phi f_{ib,y}^b-\mathrm{s}\mathrm{i}\mathrm{n}\phi f_{ib,x}^b& -\mathrm{s}\mathrm{i}\mathrm{n}\phi f_{ib,y}^b+\mathrm{c}\mathrm{o}\mathrm{s}\phi f_{ib,x}^b& \mathrm{0}\end{array}\right]\\ \end{array}$$

（10）

由于$f_{ib,z}^n\approx g\gg f_{ib,x/y}^n$，导致观测量对${\epsilon }_z$的影响不如${\epsilon }_x$和${\epsilon }_y$；此外，式10中矩阵$\tilde{\mathit{C}}$最后一行只对${\epsilon }_x$和${\epsilon }_y$进行约束，而对${\epsilon }_z$的估计完全没有作用，因而${\epsilon }_z$的可观性不如${\epsilon }_x$和${\epsilon }_y$。

2 GNSS/IMU/视觉融合定位算法

根据之前的分析，航向角误差、X轴加速度零偏、Y轴加速度零偏、Z轴陀螺零偏为GNSS/IMU系统的弱可观状态。为降低GNSS/IMU航向角误差弱可观导致的航向误差过大，这里引入视觉信息，通过视觉惯性里程计提高组合定位系统的可观性。本文使用视觉惯性里程计（港科大的VIMU-mono）^［22］提供的相对航向角来提高定位系统的可观度。

对于IMU而言，k+1时刻的航向可以表示为：

$${\psi }_i^{k+\mathrm{1}}={\psi }_i^k+\mathrm{\Delta }{\psi }_i^k$$

（11）

同样，对视觉惯性里程计，其k+1时刻的相对航向角可以表示为：

$${\psi }_v^{k+\mathrm{1}}={\psi }_v^k+\mathrm{\Delta }{\psi }_v^k$$

（12）

由于VIMU-mono估计的是相对航向角而非绝对的航向角信息，可以将前一时刻二者的航向对齐 ，则航向误差的观测量可以表示为：

$$z_{\varphi U}={\psi }_i^k-{\psi }_v^k=\mathrm{\Delta }{\psi }_i^k-\mathrm{\Delta }{\psi }_v^k$$

（13）

将航向误差添加到观测量中，系统的观测量可以表示为：

$${\mathit{z}}_k=\left[\begin{array}{c}\mathrm{\Delta }{\psi }_i^k-\mathrm{\Delta }{\psi }_v^k\\ \mathrm{L}\mathrm{o}\mathrm{n}\mathrm{g}\mathrm{i}\mathrm{t}\mathrm{u}\mathrm{d}{\mathrm{e}}_{\mathrm{i}\mathrm{m}\mathrm{u}}-\mathrm{L}\mathrm{o}\mathrm{n}\mathrm{g}\mathrm{i}\mathrm{t}\mathrm{u}\mathrm{d}{\mathrm{e}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}\\ \mathrm{L}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{t}\mathrm{u}\mathrm{d}{\mathrm{e}}_{\mathrm{i}\mathrm{m}\mathrm{u}}-\mathrm{L}\mathrm{a}\mathrm{t}\mathrm{i}\mathrm{t}\mathrm{u}\mathrm{d}{\mathrm{e}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}\\ \mathrm{A}\mathrm{l}\mathrm{t}\mathrm{i}\mathrm{t}\mathrm{u}\mathrm{d}{\mathrm{e}}_{\mathrm{i}\mathrm{m}\mathrm{u}}-\mathrm{A}\mathrm{l}\mathrm{t}\mathrm{i}\mathrm{t}\mathrm{u}\mathrm{d}{\mathrm{e}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}\end{array}\right]$$

（14）

整体算法如图1所示。

图1  融合定位算法

Fig.1  Fusion Localization Algorithm

加速度计和陀螺提供加速度 、角速度 ，根据加速度进行姿态更新，再利用加速度进行速度更新和位置更新。IMU的航向增量与位置分别与GNSS位置、视觉里程计航向增量相减，得到航向误差和位置误差的观测值 。采用15维误差状态卡尔曼滤波，对姿态误差、速度误差、位置误差、加速度计零偏和陀螺零偏进行最优估计。最后使用估计的误差状态对IMU的姿态、速度、位置、陀螺零偏和加速度零偏进行补偿。其中，15维误差状态卡尔曼滤波算法流程如图2所示。

图2  卡尔曼滤波算法

Fig.2  Kalman Filter

3 实验验证

算法的实验平台如图3所示。实验采用的IMU是Bosch公司的BMI088低成本六轴IMU，GNSS为Trimble982定位板卡，定位模式采用单点定位，摄像头是小觅的标准彩色版双目摄像头，其图像分辨率为1280×400。此外，采用具有RTK模式的NovAtel公司OEM_718D的双天线GNSS板卡作为航向角与位置参考。BMI088与OEM_718D相关参数如表1、表2所示。

图3  实验设备

Fig.3  Experimental equipment

表1  BMI088参数

Tab.1  Parameters of the BMI088

IMU性能	陀螺	加速度计
动态范围	±125°/s	±6g
输出频率	200 Hz	200 Hz
零偏	1°/s	20 mg
输出噪声	0.014（°/s）/Hz	190 μg/Hz

表2  OEM_718D参数

Tab.2  Parameters of the OEM_718D

OEM_718D性能	RTK状态精度
航向角	0.08°（2 m基线）
速度	0.03 m/s （RMS）
位置	1 cm+1 ppm

为验证算法的有效性，本文进行了实验验证。实验包含了平稳转向、匀速直线行驶、剧烈转向、直线加速、直线减速等多种工况，具体路线如图4所示。

图4  实验路线

Fig.4  Experimental route

实验验证了15维GNSS/IMU组合导航算法状态量估计的可观性：GNSS/IMU算法对于陀螺零偏的估计，Z轴陀螺零偏的收敛明显滞后于其他两轴，如图5所示。对于加速度计零偏，则Z轴的估计速度明显更快，估计结果也更加稳定，如图6所示。传统GNSS/IMU组合定位算法估计的航向也有较大的波动，与参考值之间存在较大的误差，如图7所示。这与之前的相对可观性分析结论相符。

图5  陀螺零偏估计

Fig.5  Gyro bias estimation

图6  加速度计零偏估计

Fig.6  Accelerometer bias estimation

图7  航向误差对比

Fig.7  Comparison of Heading Error

本文提出的算法由于增加了航向误差的观测值，能显著提升航向误差的可观性。在传统组合定位的航向弱可观阶段，本算法估计的航向角相对于传统组合定位算法优势明显，如图7所示。本算法与传统GNSS/IMU组合定位算法比较，其航向估计结果对比如表3所示。实验结果表明，本算法能有效降低GNSS/IMU组合定位弱可观阶段航向估计最大误差至2.76°，同时提高航向估计的稳定性。

表3  实验结果比较

Tab.3  Comparison of experimental results

算法	航向误差RMS	航向误差最大值
传统组合定位	2.23°	7.00°
本文所提算法	1.00°	2.76°

4 结语

传统GNSS/IMU组合定位存在弱可观状态量，导致车辆的航向估计不稳定，存在较大误差。本文针对GNSS/IMU算法提出了相对可观性的分析方法，结果表明传统组合定位系统存在4个弱可观状态量。为降低航向误差弱可观对航向估计的影响，本研究使用了视觉信息，利用视觉惯性里程计的相对航向角观测信息，建立了航向增量约束的融合定位算法。最后通过实验，验证了相对可观性分析的有效性，同时表明所提算法能有效提升弱可观阶段航向估计的精度。

本研究使用了单目视觉惯性里程计与GNSS/IMU进行融合定位。单目视觉惯性里程计对速度、加速度计零偏的估计能力相对较弱，无法提升其余的弱可观状态估计精度。未来的工作可考虑采用双目视觉里程计，利用相对准确的速度和加速度计零偏估计来提高整个系统的位姿精度。