融合点线特征的视觉-惯性-GNSS紧耦合导航定位方法

Visual-Inertial-GNSS Tightly Coupled Navigation and Positioning Method with Fusion of Point and Line Features

摘要

针对复杂环境下单一传感器定位的局限性问题，提出一种多传感器融合的定位方法.在视觉方面，通过在点特征的基础上增加线特征，以克服视觉图像中重复纹理的干扰；在GNSS（global navigation satellite system）方面，通过引入精度更高的载波相位对伪距观测值进行平滑处理，以提高单点定位精度.利用公开数据集和实测数据分别对算法的精度和稳定性进行了验证.结果表明，在公开数据集和实测数据中，所提方法相比于GVINS（视觉-惯性-GNSS紧耦合的算法）在地心地固坐标系下的X，Y，Z 3个方向上，定位精度分别提高了32.2%，23.3%，24.5%和25.7%，25.8%，14.1%.此外，在卫星信号被严重遮挡的环境下，所提方法在一定时间内仍具有良好的定位性能，平面定位精度达到0.74 m，高程定位精度达到0.91 m.研究成果为复杂环境下的多传感器融合定位提供新思路.

Abstract

A multi-sensor fusion positioning method was proposed to address the limitations of single-sensor localization in complex environments. In terms of vision， line features were added to point features to overcome the interference caused by repetitive textures in visual images. In the global navigation satellite system （GNSS）， the introduction of carrier phase with higher accuracy was used to smooth the pseudorange observations， which improved the accuracy of single point positioning. The accuracy and stability of the algorithm were validated by using both public datasets and measured data. In both public datasets and actual data， the accuracy of the proposed method is improved by 32.2%， 23.3%， 24.5%， and 25.7%， 25.8%， and 14.1% in the X， Y， and Z directions， respectively， compared to the GVINS （visual inertial GNSS tightly coupled algorithm） in the geocentric coordinate system. In addition， in the environments where satellite signals are severely obstructed， the proposed method still has good positioning performance for a certain period of time， with a positioning accuracy of 0.74 m in plane and 0.91m in elevation. Research results provide new insights for multi-sensor fusion position in complex environments.

Graphical abstract

关键词

视觉惯性里程计 / 线特征 / 载波相位平滑伪距 / 图优化 / 紧耦合

Key words

visual-inertial odometry / line feature / carrier phase smoothed pseudorange / graph optimization / tightly coupled

近年来，基于多传感器融合的定位方法被广泛研究.视觉传感器具有价格低廉、能够提供丰富视觉和语义信息的优点.视觉同步定位和建图（simultaneous localization and mapping， SLAM）是一种仅使用相机就能实现自我运动估计的技术.MONO-SLAM（monocular-SLAM）^［1］算法作为最早提出的一种视觉SLAM算法，使用间接法和卡尔曼滤波估计位姿，但该方法只适用于小场景中，且缺乏回环检测和全局优化.ORB-SLAM（oriented FAST and rotated BRIEF-SLAM）^［2］算法使用基于特征点的间接方法，可以在纹理良好的环境中提供精确的定位.SVO（semi-direct visual odometry）^［3］算法利用半直接的方法进行运动估计和建图，其主要缺点是采用短期数据关联，并且无法进行回环检测和全局优化.DSO（direct sparse odometry）^［4］和LSD-SLAM（large-scale direct monocular SLAM）^［5］算法使用直接法，利用平面中所有点进行大量计算，得到一个精确的结果.

当环境中存在大量重复纹理时，基于点特征的视觉方法不能很好地表达环境结构信息，相比之下，线特征包含更多的环境特征信息，能更好地克服重复纹理的干扰^［6］.PL-VIO^［7］算法是一种使用点、线特征的视觉惯性里程计（visual-inertial odometry， VIO），其使用OpenCV中的LSD线段提取算法进行特征线检测，但是LSD计算复杂，影响了系统的实时性.PLF-VINS^［8］算法是另一种利用点线特征结合的方法，它引入了2种融合点线特征的方法.第1种方法基于点线特征的位置相似度来搜索点线特征之间的关系；第2种方法融合三维平行线.然后将这2种方法形成的残差添加到后端优化估计中.实验表明，相比于经典的仅使用特征点的方法，点线特征结合的方法具有更高的精度.

虽然线特征可以提高视觉SLAM在重复纹理场景下的稳定性以及系统精度，但是在纹理稀疏和黑暗场景下，难以提取到足够的点、线特征，影响定位的精度.将视觉与IMU（inertial measurement unit）结合，构成视觉惯性里程计VIO，可以很好地解决这一问题.VIO利用相机获取的视觉数据和IMU测量的加速度和角速度信息来估计相机的运动和场景的三维结构，相比于纯视觉的导航系统，VIO可以提供更加稳定和精确的定位结果.VINS-MONO^［9］算法作为一种紧耦合IMU和相机的非线性优化算法，即使在大场景下，利用回环检测和四自由度位姿图优化，也可以达到很高的精度.多状态约束卡尔曼滤波器（multi-state constraint Kalman filter，MSCFK）^［10-11］是一种基于滤波的状态估计器，在扩展卡尔曼滤波（extended Kalman filter， EKF）框架下融合IMU和视觉信息，相较于单纯的VO（visual odometry）算法，能够适应更剧烈的运动、一定时间内的纹理缺失等，具有更高的鲁棒性.ORB-SLAM3^［12］在ORB-SLAM2^［13］和ORB-SLAM^［2］算法的基础上增加了IMU测量，具有更高的稳定性和精度.相比于VO，虽然VIO具有更好的精度和稳定性，但是VIO系统已经被证明存在4个不可观测的方向——X，Y，Z和一个偏航角^［8］，因此，VIO方法的误差累计是不可避免的.

相比之下，全球卫星导航系统GNSS提供全局的、无漂移的定位结果，被广泛应用于导航场景，只要有4颗可观测的卫星，就能获取接收机在全局坐标系中的位置.GNSS主要有4种定位模式：单点定位（single point positioning，SPP）；实时差分（real time differential，RTD）；实时动态（real time kinematic，RTK）^［14］以及精密单点定位（precise point positioning，PPP）^［15］.其中，SPP和PPP不需要基站即可获取定位结果；PPP利用了精度更高的载波相位，可以获取分米级的定位结果，但收敛时间长^［16］，需要考虑整周模糊度的问题，增加了问题的复杂性和计算量.载波相位平滑伪距的方法将两者折中，使用载波相位信息优化伪距的精度，同时不需要考虑整周模糊度的问题.

综上所述，本文提出一种紧耦合的视觉-惯性-GNSS非线性优化系统.对视觉图像提取点线特征，与IMU融合构成视觉惯性系统；然后将其与载波相位平滑后的伪距结合，实现了一种精度更高的GNSS-VIO系统.分别利用公开数据集和实测数据进行了实验验证，结果表明本文提出的算法具有更高的精度.

1 系统状态量

为方便后续算法介绍，给出本文所涉及的参考坐标系如下：

1） 地心地固坐标系（ECEF）.原点为地球质心，X轴延伸通过本初子午线（0°经度）和赤道的交点；Z轴延伸通过北极（与地球旋转轴重合）；Y轴构成右手坐标系，穿过赤道和90°经度.本文中使用WGS-84作为地心地固坐标系，用{(·)}^{\mathrm{E}}表示.

2） 局部世界坐标系.即VIO系统运行所在的坐标系，其原点为任意点，Z轴选择与重力方向对齐，用{(·)}^{\mathrm{w}}表示.

3） 站心坐标系（ENU）.为了进行局部坐标系到地心地固坐标系的转换，引入站心坐标系.站心坐标系也被称为东-北-天坐标系，X轴指向东方向，Y轴指向北方向，Z轴指向上方向，每个点有一个唯一确定的站心坐标系.本文的站心坐标系和局部世界坐标系具有相同的原点，两者之间存在一个\phi角度的旋转，用{(·)}^{\mathrm{N}}表示站心坐标系.

4） 传感器坐标系.用{(·)}^{\mathrm{r}}，{(·)}^{\mathrm{c}}，{(·)}^{\mathrm{i}}分别表示GNSS接收机、相机、IMU所在的坐标系.

2 算法基本框架

本文提出的算法在GVINS^［17］的基础上，加入了线特征以提升视觉定位性能，同时对GVINS中的GNSS伪距定位进行了优化，使用精度更高的载波相位平滑伪距以提高GNSS定位的精度.算法分为4个部分：数据输入，数据预处理，系统初始化以及后端的滑动窗口非线性优化器.通过输入GNSS原始数据、IMU数据以及相机原始图像，在预处理阶段分别进行载波相位平滑伪距、IMU预积分以及图像中点、线特征的提取和跟踪.初始化部分执行与VINS-MONO相同的视党惯性（VI）初始化，VI初始化完成后进行GNSS的初始化——锚点定位、偏移角\phi的确定以及锚点的精细化.最后，在一个滑动窗口中利用各种因子的残差和雅可比矩阵进行联合优化.本文提出的算法流程如图2所示，其中虚线框内的部分为本文相比于GVINS的改进.

2.1 线特征

本文利用PL-VIO^［7］中改进的LSD方法提取线特征，在提取线特征后，使用普吕克坐标法（Plücker）来描述空间中的直线，给定空间中一条直线\mathit{L}\in \mathit{\pi }，其普吕克坐标可以表示成\mathit{L}=({\mathit{n}}^{\mathrm{T}},{\mathit{d}}^{\mathrm{T}}{)}^{\mathrm{T}}\in {\mathit{R}}^{\mathrm{6}}，其中 n 是平面的法向量， d 是直线的方向向量.假设局部世界坐标系下的直线转换到相机坐标系下的变换矩阵{\mathit{T}}_{\mathrm{w}}^{\mathrm{c}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathrm{w}}^{\mathrm{c}}& {\mathit{P}}_{\mathrm{w}}^{\mathrm{c}}\\ \mathrm{0}& \mathrm{1}\end{array}\right]，其中{\mathit{R}}_{\mathrm{w}}^{\mathrm{c}}和{\mathit{P}}_{\mathrm{w}}^{\mathrm{c}}分别是局部世界坐标系到相机坐标系的旋转矩阵和平移矩阵.线特征的投影矩阵 K 将直线的相机坐标{\mathit{L}}_{\mathrm{c}}转换到线段的像素平面l_{\mathrm{c}}.定义普吕克线特征的坐标变换为

{\mathit{L}}_{\mathrm{c}}={\mathit{T}}_{\mathrm{w}}^{\mathrm{c}}{\mathit{L}}_{\mathrm{w}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathrm{w}}^{\mathrm{c}}& {\left[{\mathit{P}}_{\mathrm{w}}^{\mathrm{c}}\right]}_{\times }{\mathit{R}}_{\mathrm{w}}^{\mathrm{c}}\\ \mathrm{0}& {\mathit{R}}_{\mathrm{w}}^{\mathrm{c}}\end{array}\right]{\mathit{L}}_{\mathrm{w}} .(1)

对于空间中的一条直线\mathit{L}，如图3所示，被两个相机c_{\mathrm{1}}，c_{\mathrm{2}}观测到，该直线用起点和终点来表示.相机c_{\mathrm{1}}观测到的直线两个端点分别是{\mathit{S}}_{\mathrm{L}}^{\mathrm{c}\mathrm{1}}={\left[{\mathit{u}}_{\mathrm{s}}^{\mathrm{c}\mathrm{1}},{\mathit{v}}_{\mathrm{s}}^{\mathrm{c}\mathrm{1}},\mathrm{1}\right]}^{\mathrm{T}}，{\mathit{E}}_{\mathrm{L}}^{c\mathrm{1}}={\left[{\mathit{u}}_{\mathrm{e}}^{\mathrm{c}\mathrm{1}},{\mathit{v}}_{\mathrm{e}}^{\mathrm{c}\mathrm{1}},\mathrm{1}\right]}^{\mathrm{T}}，同理相机c_{\mathrm{2}}观测到的直线两个端点分别是{\mathit{S}}_{\mathrm{L}}^{\mathrm{c}\mathrm{2}}={\left[{\mathit{u}}_{\mathrm{s}}^{\mathrm{c}\mathrm{2}},{\mathit{v}}_{\mathrm{s}}^{\mathrm{c}\mathrm{2}},\mathrm{1}\right]}^{\mathrm{T}}，{\mathit{E}}_{\mathrm{L}}^{\mathrm{c}\mathrm{2}}={\left[{\mathit{u}}_{\mathrm{e}}^{\mathrm{c}\mathrm{2}},{\mathit{v}}_{\mathrm{e}}^{\mathrm{c}\mathrm{2}},\mathrm{1}\right]}^{\mathrm{T}}.三维平面\mathit{\pi }可以建模成{\pi }_{x}x+{\pi }_{y}y+{\pi }_{z}z+{\pi }_{\omega }=\mathrm{0}，其中{\mathit{n}}_{\pi }={\left[{\pi }_x+{\pi }_y+{\pi }_z\right]}^{\mathrm{T}}是平面\mathit{\pi }的法向量，对于平面上一点{\mathit{p}}_{\mathrm{0}}={\left[x,y,z\right]}^{\mathrm{T}}：

{\mathit{\pi }}_{\omega }=-\left({\pi }_x{x}_{\mathrm{0}}+{\pi }_y{y}_{\mathrm{0}}+{\pi }_z{z}_{\mathrm{0}}\right)=-{{\mathit{n}}_{\pi }}^{\mathrm{T}}·{\mathit{p}}_{\mathrm{0}} .(2)

根据式（1）和式（2），可以表示直线投影所在的两个平面{\mathit{\pi }}_{\mathrm{1}},{\mathit{\pi }}_{\mathrm{2}}，根据普吕克对偶矩阵{\mathit{L}}_{\mathrm{w}}^{*}，可以得到普吕克坐标：

{\mathit{L}}_w^{*}=\left[\begin{array}{cc}[{\mathit{d}}_w{]}_{\times }& {\mathit{n}}_w\\ -{\mathit{n}}_w^{\mathrm{T}}& \mathrm{0}\end{array}\right]={\mathit{\pi }}_{\mathrm{1}}{\mathit{\pi }}_{\mathrm{2}}^{\mathrm{T}}-{\mathit{\pi }}_{\mathrm{2}}{\mathit{\pi }}_{\mathrm{1}}^{\mathrm{T}}.(3)

由于空间中的直线只有4个自由度，普吕克坐标由6个参数组成，利用正交表示的紧凑性对其进行优化^［18］，假设\mathit{U}=\mathit{R}\left(\mathit{\psi }\right),\mathit{W}=\mathit{R}(\mathit{\varphi })分别表示3D和2D的旋转矩阵，可以得

\mathit{U}=\mathit{R}\left(\mathit{\psi }\right)=\left[{\mathit{u}}_{\mathrm{1}},{\mathit{u}}_{\mathrm{2}},{\mathit{u}}_{\mathrm{3}}\right]=\left[\frac{{\mathit{n}}_{\mathrm{w}}}{‖{\mathit{n}}_{\mathrm{w}}‖},\frac{{\mathit{d}}_{\mathrm{w}}}{‖{\mathit{d}}_{\mathrm{w}}‖},\frac{{\mathit{n}}_{\mathrm{w}}\times {\mathit{d}}_{\mathrm{w}}}{‖{\mathit{n}}_{\mathrm{w}}\times {\mathit{d}}_{\mathrm{w}}‖}\right]，

\mathit{W}=\mathit{R}\left(\mathit{\varphi }\right)=\left[\begin{array}{cc}{\mathit{w}}_{\mathrm{1}}& -{\mathit{w}}_{\mathrm{2}}\\ {\mathit{w}}_{\mathrm{2}}& {\mathit{w}}_{\mathrm{1}}\end{array}\right]=\left[\begin{array}{cc}\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathit{\varphi }\right)& -\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathit{\varphi }\right)\\ \mathrm{s}\mathrm{i}\mathrm{n}\left(\mathit{\varphi }\right)& \mathrm{c}\mathrm{o}\mathrm{s}\left(\mathit{\varphi }\right)\end{array}\right]=

\frac{\mathrm{1}}{\sqrt[]{{‖{\mathit{n}}_w‖}^{\mathrm{2}}+{‖{\mathit{d}}_w‖}^{\mathrm{2}}}}\left[\begin{array}{cc}‖{\mathit{n}}_w‖& -‖{\mathit{d}}_w‖\\ ‖{\mathit{d}}_w‖& ‖{\mathit{n}}_w‖\end{array}\right] . (4)

其中：\mathit{\psi }={\left[{\psi }_{\mathrm{1}},{\psi }_{\mathrm{2}},{\psi }_{\mathrm{3}}\right]}^{\mathrm{T}}和\mathit{\varphi }是围绕相机坐标系的x，y，z轴三维旋转角度和二维旋转角度.用一个四维向量来定义一条空间线的正交表示：

\mathit{o}={(\mathit{\psi },\mathit{\varphi })}^{\mathrm{T}} .

给定一个标准正交表示(\mathit{U},\mathit{W})，对应的普吕克坐标：

{\mathit{L}}_{\mathrm{w}}^{\text{'}}={\left[{\mathit{w}}_{\mathrm{1}}{\mathit{u}}_{\mathrm{1}}^{\mathrm{T}},{\mathit{w}}_{\mathrm{2}}{\mathit{u}}_{\mathrm{2}}^{\mathrm{T}}\right]}^{\mathrm{T}}=\frac{\mathrm{1}}{\sqrt[]{{‖\mathit{n}‖}^{\mathrm{2}}+{‖\mathit{d}‖}^{\mathrm{2}}}}{\mathit{L}}_{\mathrm{w}} .(5)

其中：{\mathit{w}}_{\mathrm{1}}=\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathit{\varphi }\right)，{\mathit{w}}_{\mathrm{2}}=\mathrm{s}\mathrm{i}\mathrm{n}(\mathit{\varphi })；{\mathit{u}}_{\mathrm{1}}和{\mathit{u}}_{\mathrm{2}}表示\mathit{U}的第1列和第2列；{\mathit{L}}_{\mathrm{w}}和{\mathit{L}}_{\mathrm{w}}^{\text{'}}之间有一个比例因子，它们表示同一条空间线.

2.2 GNSS测量

2.2.1 伪距测量

GNSS伪距测量是一种用于确定GNSS接收机与卫星之间距离的技术.伪距是指接收机接收到卫星发射信号所经历的时间与信号传播速度的乘积.由于信号传播速度为光速，可以将传播时间转换为距离.伪距测量是一种被动定位方法，接收机通过同时接收多颗卫星发射的信号并测量信号的传播时间来计算与每颗卫星的距离.为了进行GNSS伪距测量，接收机需要同时接收至少4颗卫星的信号.然后，通过空间距离后方交会计算接收机的空间三维坐标.对于卫星s，其在k时刻与接收机r的伪距{\mathit{P}}_{r_k}^s可以建模为

{\mathit{P}}_{r_k}^s=‖{\mathit{P}}_s^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}‖+c\left(\delta t_{r_k}-\delta t_s\right)+\mathit{T}+\mathit{I}+{\epsilon }_{\mathrm{P}} .(6)

其中：{\mathit{P}}_s^{\mathrm{E}}和{\mathit{P}}_{r_k}^{\mathrm{E}}分别表示卫星和接收机在k时刻地心地固坐标系下的坐标；c为光速；\delta t_{r_k}和\delta t_s分别为接收机钟差和卫星钟差；\mathit{T}为对流层误差；\mathit{I}为电离层误差；卫星钟差、电离层误差都可以从星历中计算得到，对流层误差通过Sasstamoinen模型计算；{\epsilon }_{\mathrm{P}}代表多径误差和随机误差，服从零均值的高斯分布.

2.2.2 载波相位测量

与伪距测量不同，在载波相位测量中，接收机测量的是卫星信号的电磁波载波相位.通过测量接收信号与发射信号之间的相对相位差，可以计算出接收机与卫星之间更高精度的距离.然而，由于载波相位是周期性的正弦或余弦信号，GNSS接收机只能测量不到一个波长的部分，存在整周模糊的问题.与伪距测量类似，载波相位测量也需要获取至少4颗卫星的载波相位观测值，对于卫星s，其在k时刻与接收机r的载波相位观测值{\mathit{\Phi }}_{r_k}^s与卫星和GNSS接收机位置的关系可以建模为

{\mathit{\Phi }}_{r_k}^s=‖{\mathit{P}}_s^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}‖+c\left(\delta t_{r_k}-\delta t_s\right)+\mathit{T}-

\mathit{I}+\lambda N+{\epsilon }_{\mathrm{\Phi }} .(7)

其中：\lambda为载波的波长；N为载波相位的整周模糊度；{\epsilon }_{\mathrm{\Phi }}是载波相位观测中的卫星多径误差和随机误差，服从零均值的高斯分布.

2.2.3 多普勒频移测量

多普勒频移（Doppler shift，DS）是指当信号源（如卫星）与接收机作相对运动时，信号的发射频率与接收频率发生变化的现象.这种频率变化是由于接收机与信号源之间的相对运动（靠近或远离）引起的.GNSS观测中也存在多普勒效应，卫星在椭圆形轨道上运行时，其与接收机之间的距离是相对变化的，因此接收机接收到的卫星信号频率也会产生相应的变化.多普勒频移可以建模为

\mathrm{\Delta }{\mathit{f}}_{r_k}^{\mathrm{s}}=-\frac{\mathrm{1}}{\lambda }[{\mathit{u}}_k^{\mathrm{T}}({\mathit{V}}_{\mathrm{s}}^{\mathrm{E}}-{\mathit{V}}_{r_k}^{\mathrm{E}})+c(\stackrel{.}{\delta t_{r_k}}-\stackrel{.}{\delta t_s})]+{\epsilon }_{\mathrm{\Delta }f}.(8)

其中：{\mathit{u}}_k^{\mathrm{T}}代表卫星s到接收机r_k的单位矢量，表示为{\mathit{u}}_k^{\mathrm{T}}=\frac{({\mathit{P}}_s^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}})}{‖{\mathit{P}}_s^E-{\mathit{P}}_{r_k}^E‖}；{\mathit{V}}_s^{\mathrm{E}}和{\mathit{V}}_{r_k}^{\mathrm{E}}分别代表k时刻在地心地固坐标系下卫星和接收机的速度；\stackrel{.}{\delta t_{r_k}}和\stackrel{.}{\delta t_s}分别代表卫星和接收机的时钟漂移率；{\epsilon }_{\mathrm{\Delta }f}代表多普勒频移测量中的多径和随机误差，服从零均值高斯分布.

2.2.4 载波相位平滑伪距测量

上述的载波相位测量和伪距测量，伪距观测值只能达到米级精度，而载波相位观测值虽然可以达到厘米级，但是存在整周模糊度的计算问题.因此，通过将载波相位观测与伪距观测进行结合，利用载波相位观测平滑伪距来提高伪距的精度，同时避开载波相位观测中的整周模糊度计算，可以实现两者优势互补.为实现动态的载波相位平滑伪距计算，本文采用Hatch滤波方法.对于单频接收机，平滑后k时刻的伪距\tilde{\mathit{P}}\left(k\right)可以建模为

\tilde{\mathit{P}}\left(k\right)=\frac{\mathrm{1}}{N}\mathit{P}\left(k\right)+\frac{N-\mathrm{1}}{N}\left[\tilde{\mathit{P}}\left(k-\mathrm{1}\right)+\mathit{\Phi }\left(k\right)-\mathit{\Phi }\left(k-\mathrm{1}\right)\right]+{\epsilon }_{\mathrm{P}}.(9)

其中：N表示平滑间隔，一般在20~200之间；\mathit{P}\left(k\right)表示k时刻的未平滑伪距；\mathit{\Phi }\left(k\right)表示k时刻的载波相位观测值；{\epsilon }_{\mathrm{P}}表示载波相位平滑伪距中的多路径误差和随机误差，符合零均值的高斯分布.

3 因子图表示

本文系统采用的因子图如图4所示，采用滑动窗口优化方法，窗口内的状态\mathit{\chi }表示为

\mathit{\chi }=[{\mathit{x}}_{\mathrm{0}},{\mathit{x}}_{\mathrm{1}},\cdots ,{\mathit{x}}_n,{\mathit{o}}_{\mathrm{0}},{\mathit{o}}_{\mathrm{1}},\cdots ,{\mathit{o}}_i,{\mathit{\rho }}_{\mathrm{0}},{\mathit{\rho }}_{\mathrm{1}},\cdots ,{\mathit{\rho }}_j,\phi ]，

{\mathit{x}}_k=\left[{\mathit{p}}_{b_k}^{\mathrm{w}},{\mathit{v}}_{b_k}^{\mathrm{w}},{\mathit{q}}_{b_k}^{\mathrm{w}},b_{\mathrm{a}},b_{\mathrm{g}},\delta t_{r_k}\stackrel{.}{\delta t_{r_k}}\right].(10)

其中：k=\mathrm{0,1},\mathrm{2}； n表示滑动窗口的大小；i表示滑动窗口中特征点的数量；j表示滑动窗口中特征线的数量；{\mathit{\rho }}_j表示滑动窗口中的第j个点特征的逆深度；\phi表示局部世界坐标系{(·)}^{\mathrm{w}}和站心坐标系{(·)}^{\mathrm{N}}之间的偏航角；{\mathit{p}}_{b_k}^w，{\mathit{v}}_{b_k}^w，{\mathit{q}}_{b_k}^w分别表示传感器坐标系在局部世界坐标系中的位置、速度以及方向；b_{\mathrm{a}}，b_{\mathrm{g}}分别表示加速度计的偏置和陀螺仪的偏置；\delta t_{r_k}，\stackrel{.}{\delta t_{r_k}}分别表示接收机的时钟偏差和时钟漂移率.

3.1 线特征因子

本文中，线特征的重投影残差是根据点到线的距离来定义的.给定空间中的一条线\mathit{L}=({\mathit{n}}_{\mathrm{c}}^{\mathrm{T}},{\mathit{d}}_{\mathrm{c}}^{\mathrm{T}}{)}^{\mathrm{T}}，按照式（11）投影到相机平面：

\mathit{l}=[{\mathit{l}}_{\mathrm{1}},{\mathit{l}}_{\mathrm{2}},{\mathit{l}}_{\mathrm{3}}{]}^{\mathrm{T}}={\mathit{K}}_{\mathrm{L}}{\mathit{n}}_{\mathrm{c}}.(11)

其中，{\mathit{K}}_{\mathrm{L}}是线特征的投影矩阵.假设\mathit{L}代表一条被相机c观测到的空间直线，该直线的重投影残差可以建模为

{\mathit{r}}_{\mathrm{L}}\left({\tilde{\mathit{z}}}_{\mathrm{L}}^{\mathrm{c}},\mathit{\chi }\right)=\left[\begin{array}{c}d\left({\mathit{s}}_{\mathrm{L}}^{\mathrm{c}},{\mathit{l}}_{\mathrm{L}}^{\mathrm{c}}\right)\\ d\left({\mathit{e}}_{\mathrm{L}}^{\mathrm{c}},{\mathit{l}}_{\mathrm{L}}^{\mathrm{c}}\right)\end{array}\right].(12)

其中：{\mathit{s}}_{\mathrm{L}}^{\mathrm{c}}={\left[{\mathit{u}}_{\mathrm{s}}^{\mathrm{c}},{\mathit{v}}_{\mathrm{s}}^{\mathrm{c}},\mathrm{1}\right]}^{\mathrm{T}}和{\mathit{e}}_{\mathrm{L}}^{\mathrm{c}}={\left[{\mathit{u}}_{\mathrm{e}}^{\mathrm{c}},{\mathit{v}}_{\mathrm{e}}^{\mathrm{c}},\mathrm{1}\right]}^{\mathrm{T}}分别表示投影在归一化图像平面上的线段的两个端点；{\tilde{\mathit{z}}}_{\mathrm{L}}^{\mathrm{c}}为观测数据；d(\mathit{s},\mathit{l})表示点到线的距离，

d\left(\mathit{s},\mathit{l}\right)=\frac{{\mathit{s}}^{\mathrm{T}}\mathit{l}}{\sqrt[]{{\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}}}.(13)

通过最小化线重投影误差，相应的雅可比矩阵可以通过链式法则得到

{\mathit{J}}_L=\frac{\partial {\mathit{r}}_{\mathrm{l}}}{\partial {\mathit{l}}^{\mathrm{c}}}\frac{\partial {\mathit{l}}^{\mathrm{c}}}{\partial {\mathit{L}}^{\mathrm{c}}}\left[\frac{\partial {\mathit{L}}^{\mathrm{c}}}{\partial \delta {\mathit{x}}^i}\frac{\partial {\mathit{L}}^{\mathrm{c}}}{\partial {\mathit{L}}^{\mathrm{w}}}\frac{\partial {\mathit{L}}^{\mathrm{w}}}{\partial \delta \mathit{o}}\right].(14)

其中：

\frac{\partial {\mathit{r}}_{\mathrm{l}}}{\partial {\mathit{l}}^{\mathrm{c}}}={\left[\begin{array}{cc}\frac{-{\mathit{l}}_{\mathrm{1}}({\mathit{s}}_{\mathrm{L}}^{\mathrm{c}}{)}^{\mathrm{T}}\mathit{l}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{3}}{\mathrm{2}})}}+\frac{{\mathit{u}}_{\mathrm{s}}^{\mathrm{c}}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{1}}{\mathrm{2}})}}& \frac{-{\mathit{l}}_{\mathrm{2}}({\mathit{s}}_{\mathrm{L}}^{\mathrm{c}}{)}^{\mathrm{T}}\mathit{l}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{3}}{\mathrm{2}})}}+\frac{{\mathit{v}}_{\mathrm{s}}^{\mathrm{c}}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{1}}{\mathrm{2}})}}\frac{\mathrm{1}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{1}}{\mathrm{2}})}}\\ \frac{-{\mathit{l}}_{\mathrm{1}}({\mathit{e}}_{\mathrm{L}}^{\mathrm{c}}{)}^{\mathrm{T}}\mathit{l}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{3}}{\mathrm{2}})}}+\frac{{\mathit{u}}_{\mathrm{e}}^{\mathrm{c}}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{1}}{\mathrm{2}})}}& \frac{-{\mathit{l}}_{\mathrm{2}}({\mathit{e}}_{\mathrm{L}}^{\mathrm{c}}{)}^{\mathrm{T}}\mathit{l}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{3}}{\mathrm{2}})}}+\frac{{\mathit{v}}_{\mathrm{e}}^{\mathrm{c}}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{1}}{\mathrm{2}})}}\frac{\mathrm{1}}{({\mathit{l}}_{\mathrm{1}}^{\mathrm{2}}+{\mathit{l}}_{\mathrm{2}}^{\mathrm{2}}{)}^{(\frac{\mathrm{1}}{\mathrm{2}})}}\end{array}\right]}_{\mathrm{2}\times \mathrm{3}};

\frac{\partial {\mathit{l}}^{\mathrm{c}}}{\partial {\mathit{L}}^{\mathrm{c}}}={\left[\begin{array}{cc}{\mathit{K}}_{\mathrm{L}}& \mathrm{0}\end{array}\right]}_{\mathrm{3}\times \mathrm{6}}；\frac{\partial {\mathit{L}}^{\mathrm{c}}}{\partial \delta {\mathit{x}}^i}={\left[{{\mathit{T}}_{\mathrm{b}}^{\mathrm{c}}}^{-\mathrm{1}}\left[\begin{array}{c}{{\mathit{R}}_{\mathrm{b}}^{\mathrm{w}}}^{\mathrm{T}}{\left[{\mathit{d}}^{\mathrm{w}}\right]}_{\times }\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}\end{array}\right]{{\mathit{T}}_{\mathrm{b}}^{\mathrm{c}}}^{-\mathrm{1}}\left[\begin{array}{c}{{\mathit{R}}_{\mathrm{b}}^{\mathrm{w}}}^{\mathrm{T}}({\mathit{n}}^{\mathrm{w}}+{\left[{\mathit{d}}^{\mathrm{w}}\right]}_{\times }{\mathit{P}}_{\mathrm{b}}^{\mathrm{w}}{)]}_{\times }\\ {\left[{{\mathit{R}}_{\mathrm{b}}^{\mathrm{w}}}^{\mathrm{T}}{\mathit{d}}^{\mathrm{w}}\right]}_{\times }\end{array}\right]\mathrm{0}\mathrm{0}\mathrm{0}\right]}_{\mathrm{6}\times \mathrm{15}};

\frac{\partial {\mathit{L}}^{\mathrm{c}}}{\partial {\mathit{L}}^{\mathrm{w}}}\frac{\partial {\mathit{L}}^{\mathrm{w}}}{\partial \delta \mathit{o}}={{\mathit{T}}_{\mathrm{c}}^{\mathrm{w}}}^{-\mathrm{1}}{\left[\begin{array}{cc}\begin{array}{l}\mathrm{0}\\ {\mathit{w}}_{\mathrm{2}}{\mathit{u}}_{\mathrm{3}}\end{array}& \begin{array}{l}-{\mathit{w}}_{\mathrm{1}}{\mathit{u}}_{\mathrm{3}}\\ \mathrm{0}\end{array}\end{array}\begin{array}{cc}\begin{array}{l}{\mathit{w}}_{\mathrm{1}}{\mathit{u}}_{\mathrm{2}}\\ -{\mathit{w}}_{\mathrm{2}}{\mathit{u}}_{\mathrm{1}}\end{array}& \begin{array}{l}-{\mathit{w}}_{\mathrm{2}}{\mathit{u}}_{\mathrm{1}}\\ {\mathit{w}}_{\mathrm{1}}{\mathit{u}}_{\mathrm{2}}\end{array}\end{array}\right]}_{\mathrm{6}\times \mathrm{4}}.

3.2 载波相位平滑伪距因子

接收机在地心地固坐标系中k时刻的位置可以表示为

{\mathit{P}}_{r_k}^{\mathrm{E}}={\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{P}}_{r_k}^{\mathrm{w}}+{\mathit{P}}_{\mathrm{N}}^{\mathrm{E}}.(15)

其中，{\mathit{P}}_{r_k}^{\mathrm{w}}表示接收机在局部世界坐标系中的位置，可以表示为

{\mathit{P}}_{r_k}^{\mathrm{w}}={\mathit{R}}_{b_k}^{\mathrm{w}}{\mathit{P}}_r^{\mathrm{b}}+{\mathit{P}}_{b_k}^{\mathrm{w}}.(16)

{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}表示从局部世界坐标系到站心坐标系的旋转矩阵.由于站心坐标系的Z轴方向与重力方向平行，且局部世界坐标系的Z轴也平行于重力方向，所以两者之间只有一个自由度的变化，存在一个偏航角\phi，因此：

{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}=\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].(17)

而{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}表示从站心坐标系到地心地固坐标系的旋转矩阵.在一个地心地固坐标系下的坐标\mathit{P}={[X,Y,Z]}^{\mathrm{T}}，可以由式（18）得到其在大地坐标系下的坐标\mathit{P}={[B,L,H]}^{\mathrm{T}}：

\left[\begin{array}{c}L\\ B\\ H\end{array}\right]=\left[\begin{array}{c}\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{Y}{X}\right)\\ \mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left[\frac{Z\left(N+H\right)}{\sqrt[]{X^{\mathrm{2}}+Y^{\mathrm{2}}\left[N\left(\mathrm{1}-e^{\mathrm{2}}\right)+H\right]}}\right]\\ \frac{\sqrt[]{X^{\mathrm{2}}+Y^{\mathrm{2}}}}{\mathrm{c}\mathrm{o}\mathrm{s}B}-N\end{array}\right],

N=\frac{a}{\sqrt[]{\mathrm{1}-e^{\mathrm{2}}\mathrm{s}\mathrm{i}{\mathrm{n}}^{\mathrm{2}}B}}，e=\frac{\sqrt[]{a^{\mathrm{2}}-b^{\mathrm{2}}}}{a}.(18)

其中：a为地球椭球的长半轴；b为短半轴；N为\mathit{P}点的曲率半径；e是与a，b相关的偏心率.根据\mathit{P}点的大地坐标，可以得到唯一确定的{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}：

{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}=\left[\begin{array}{ccc}-\mathrm{s}\mathrm{i}\mathrm{n}L& -\mathrm{s}\mathrm{i}\mathrm{n}BcosL& \mathrm{c}\mathrm{o}\mathrm{s}B\mathrm{c}\mathrm{o}\mathrm{s}L\\ \mathrm{c}\mathrm{o}\mathrm{s}L& -\mathrm{s}\mathrm{i}\mathrm{n}BsinL& \mathrm{c}\mathrm{o}\mathrm{s}B\mathrm{s}\mathrm{i}\mathrm{n}L\\ \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}B& \mathrm{s}\mathrm{i}\mathrm{n}B\end{array}\right].(19)

根据式（9），可以得到载波相位平滑伪距的残差为

r_{\rho }\left({\tilde{z}}_{r_k}^{\mathrm{s}},\mathit{\chi }\right)=\frac{\mathrm{1}}{N}{\mathit{P}}_{r_k}^{\mathrm{s}}+\left(\frac{N-\mathrm{1}}{N}\right)\left[{\tilde{\mathit{P}}}_{r_{k-\mathrm{1}}}^{\mathrm{s}}+{\mathit{\Phi }}_{r_k}^{\mathrm{s}}-{\mathit{\Phi }}_{r_{k-\mathrm{1}}}^{\mathrm{s}}\right]-{\tilde{\mathit{P}}}_{r_k}^{\mathrm{s}} .(20)

根据链式法则，得到载波相位平滑伪距的雅可比矩阵为

{\mathit{J}}_{\rho }=\left[\frac{\partial r_{\rho }}{\partial {\mathit{p}}_{b_{k-\mathrm{1}}}^w}\frac{\partial r_{\rho }}{\partial {\mathit{R}}_{b_{k-\mathrm{1}}}^w}\frac{\partial r_{\rho }}{\partial {\mathit{p}}_{b_k}^w}\frac{\partial r_{\rho }}{\partial {\mathit{R}}_{b_k}^w}\frac{\partial r_{\rho }}{\partial \delta {\mathit{t}}_{r_{k-\mathrm{1}}}}\frac{\partial r_{\rho }}{\partial \delta {\mathit{t}}_{r_k}}\frac{\partial r_{\rho }}{\partial \phi }\frac{\partial r_{\rho }}{\partial {\mathit{P}}_N^E}\frac{\partial r_{\rho }}{\partial {\mathit{P}}_r^b}\right] .(21)

其中：

\frac{\partial r_{\rho }}{\partial {\mathit{p}}_{b_{k-\mathrm{1}}}^{\mathrm{w}}}=\left(\mathrm{1}-\frac{\mathrm{1}}{N}\right)\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}‖}{\mathit{R}}_N^{\mathrm{E}}{\mathit{R}}_w^{\mathrm{N}}；

\frac{\partial r_{\rho }}{\partial {\mathit{R}}_{b_{k-\mathrm{1}}}^{\mathrm{w}}}=-\left(\mathrm{1}-\frac{\mathrm{1}}{N}\right)\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{R}}_{b_{k-\mathrm{1}}}^{\mathrm{w}}{\left[{\mathit{p}}_r^b\right]}_{\times }；

\frac{\partial r_{\rho }}{\partial {\mathit{p}}_{b_k}^{\mathrm{w}}}=-\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}；

\frac{\partial r_{\rho }}{\partial {\mathit{R}}_{b_k}^{\mathrm{w}}}=\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{R}}_{b_k}^{\mathrm{w}}{\left[{\mathit{p}}_r^b\right]}_{\times }；

\frac{\partial r_{\rho }}{\partial \delta {\mathit{t}}_{r_{k-\mathrm{1}}}}=-c(\mathrm{1}-\frac{\mathrm{1}}{N})；\frac{\partial r_{\rho }}{\partial \delta {\mathit{t}}_{r_k}}=c；

\frac{\partial r_{\rho }}{\partial \phi }=-\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^E‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\dot{\mathit{R}}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{p}}_{r_k}^{\mathrm{w}}+

(\mathrm{1}-\frac{\mathrm{1}}{N})\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\dot{\mathit{R}}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{p}}_{r_{k-\mathrm{1}}}^{\mathrm{w}};

\frac{\partial r_{\rho }}{\partial {\mathit{P}}_{\mathrm{N}}^{\mathrm{E}}}=-\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}‖}+(\mathrm{1}-\frac{\mathrm{1}}{N})\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}‖}，

\frac{\partial r_{\rho }}{\partial {\mathit{P}}_r^b}=-\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_k}^E‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\dot{\mathit{R}}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{p}}_{b_k}^{\mathrm{w}}+

(\mathrm{1}-\frac{\mathrm{1}}{N})\frac{{\left({\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}\right)}^{\mathrm{T}}}{‖{\mathit{P}}_{s_j}^{\mathrm{E}}-{\mathit{P}}_{r_{k-\mathrm{1}}}^{\mathrm{E}}‖}{\mathit{R}}_{\mathrm{N}}^{\mathrm{E}}{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}{\mathit{R}}_{b_{k-\mathrm{1}}}^{\mathrm{w}}.

{\dot{\mathit{R}}}_{\mathrm{w}}^{\mathrm{N}}为{\mathit{R}}_{\mathrm{w}}^{\mathrm{N}}求导后的矩阵：

{\dot{\mathit{R}}}_{\mathrm{w}}^{\mathrm{N}}=\left[\begin{array}{ccc}-\mathrm{s}\mathrm{i}\mathrm{n}\phi & -\mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{0}\\ \mathrm{c}\mathrm{o}\mathrm{s}\phi & -\mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}\end{array}\right].

4 实 　验

为验证本文所提算法的精度和稳定性，分别利用公开数据集和校园实测数据集进行实验，并与现有算法进行对比分析.

4.1 公开数据集实验结果

使用文献［16］中的sport_field公开数据集进行验证，该数据集为香港科技大学体育场的采集数据，设备采用VI传感器采集相机和IMU数据，采用ublox-F9p采集GNSS原始数据.设备沿着运动跑道跑了5圈.运动场是一个室外环境，周边无遮挡，对空卫星观测环境很好，RTK可持续保持固定解.将本文提出的方法与GVINS进行对比，在谷歌地图上绘制的轨迹如图5所示.从图5可以看出，GVINS作为一种紧耦合视觉-惯性- GNSS的算法，具有较好的平滑性和稳定性，定位精度较高.本文提出的方法同样紧耦合3种传感器，具有相同平滑性和稳定性，同时相比于GVINS算法，更加接近真值轨迹.

本文方法和GVINS都能直接输出地心地固坐标系下的定位结果，同时，文献［16］提供了厘米级定位精度的RTK结果，将其作为地面真值与本文方法和GVINS进行比较.通过式（22）得到本文方法与GVINS在地心地固坐标系下定位的均方根误差（RMSE），如表1所示，可以看出，本文提出的方法相比于GVINS算法，在X，Y，Z 3个轴上都具有更高的精度.相应的误差折线图如图6所示.

\mathrm{R}\mathrm{M}\mathrm{S}\mathrm{E}\left(\tilde{X},X\right)=\sqrt[]{\frac{\mathrm{1}}{m}{\sum }_{i=\mathrm{1}}^m(\tilde{X}\left(i\right)-X(i))} . (22)

本文提出的方法和GVINS算法的误差累积分布图（CDF）如图7所示.从图中可以看出，本文提出的方法误差累积分布曲线位于图中的左上角，表明相比于GVINS算法，本文方法在每个误差概率分布中都是优于GVINS的.

为了验证卫星数量在不同退化情况下对本文方法的影响，在后端优化中设置不同的接收卫星数量，来模拟卫星数量退化情况.在Sports_field数据集中，G02，G05和G13 卫星最为稳定，为此将G02，G05，G13逐步加入到系统中，模拟出系统接收到0，1，2，3颗卫星的情况.在接收不同数量卫星的情况下，本文方法与GVINS算法在ENU坐标系下相比于真值的误差，如图8所示.

通过图8可以看出，当卫星数量从3颗逐渐减少到0颗时，本文方法和GVINS在东、北方向的误差都逐渐增加，当卫星数量为0时，由于没有GNSS数据，本文的方法和GVINS都退化至纯VIO定位.从图8d中可以看出，当卫星数量减少到3颗时，本文提出的方法和GVINS仍然能有效抑制东、北和偏航方向的漂移.从图8c可以看出，当卫星数量减少到2颗时，本文方法和GVINS的水平定位误差随距离增加而逐渐增大.从图8a、图8b可以看出，当卫星数量为0或1时，误差呈正弦波分布，卫星数量为1时，相比于没有卫星数据，振幅更小，说明即使只有一颗卫星观测，也能对本文方法提供支撑，且本文方法相比于GVINS振幅更小，表明在这种情况下，本文方法具有更高的精度.对于高程方向，无论是本文方法还是GVINS，误差均以相同方式累计，说明3颗或更少的卫星无法消除高程方向上的误差.

4.2 实地实验

为了进一步验证本文提出方法的精度和稳定性，在东北大学校园内选择一个开阔区域以及一个信号遮挡严重区域进行数据采集.相机数据由Intel 公司的RealsenseD435提供，使用Ublox-F9P获取GNSS原始数据，IMU数据采用战术级姿态传感器CH108获取.轨迹的真实值使用测量型GNSS RTK获取其高精度绝对位置，中途有一部分卫星信号遮挡严重，RTK不可用，在RTK可用的开阔区域对本文方法和GVINS进行对比.GVINS算法和本文方法以及RTK的轨迹如图9所示.在地心地固坐标系下的X，Y，Z 3轴误差如表2和图10所示.从结果中可以看出，通过引入线特征和使用载波相位平滑伪距代替原始伪距，算法的精度和平滑性有明显提升.

另外，本文提出的方法和GVINS算法在实地进行实验的误差累积分布（CDF）如图11所示.从图中可以看出，本文方法的误差累积分布曲线位于GVINS算法误差累积分布的左上方，表明相比于GVINS算法，本文方法在每一个误差概率分布中都是优于GVINS的.

此外，在图9中的RTK退化区域为树木遮挡区域，由于树木遮挡严重导致卫星信号差，RTK在此路段失效，退化成单点定位.但在采集数据时路线为沿着中间车道分界线采集，因此该区域的路径应为一条直线.通过获取RTK退化区域两端的坐标，可以得到该路程的真值，并将本文算法与RTK退化时的误差进行比较如图12所示.从图中可以看出，当RTK退化时，本文方法在一定时间内仍然可以保持良好的精度，平面精度达到0.74 m，高程精度达到0.91 m.

5 结 语

本文提出了一种结合点线特征的视觉、惯性、GNSS紧耦合定位方法.该方法在GVINS算法的基础上，加入了包含更多环境结构信息的线特征，使得系统在重复纹理场景下的精度得到提高.另外，由于伪距测量的精度只能达到米级，而载波相位的精度可以达到厘米级，但需要求解整周模糊度，所以利用载波相位平滑伪距的方法以提高伪距测量的精度.通过公开数据集和实测数据集分别进行实验，验证了提出的方法可以实现稳定、高精度、实时的定位.在公开数据集中，本文提出的方法相比于GVINS算法，在地心地固坐标系下的X，Y，Z 3个方向上的定位精度分别提高了32.2%，23.3%，24.5%；在实测数据集中分别为25.7%，25.8%，14.1%.同时，在卫星信号遮蔽环境下，RTK不可用，退化为单点定位时，本文提出的方法仍可以在一定时间内保持良好的定位性能，平面精度达到0.74 m，高程精度达到0.91 m.

参考文献

[1] Davison A J， Reid I D， Molton N D， et al. MonoSLAM： real-time single camera SLAM［J］. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2007， 29（6）： 1052-1067.

[2] Mur-Artal R， Montiel J M M， Tardós J D. ORB-SLAM： a versatile and accurate monocular SLAM system［J］. IEEE Transactions on Robotics， 2015， 31（5）： 1147-1163.

[3] Forster C， Pizzoli M， Scaramuzza D. SVO： fast semi-direct monocular visual odometry［C］// 2014 IEEE International Conference on Robotics and Automation （ICRA）. Hong Kong， 2014： 15-22.

[4] Engel J， Koltun V， Cremers D. Direct sparse odometry［J］. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2018， 40（3）： 611-625.

[5] Engel J， Schöps T， Cremers D. LSD-SLAM： large-scale direct monocular SLAM［M］//Computer Vision-ECCV 2014. Cham： Springer International Publishing， 2014： 834-849.

[6] 王丹， 黄鲁， 李垚. 基于点线特征的单目视觉同时定位与地图构建算法［J］. 机器人， 2019， 41（3）： 392-403.

[7] Wang Dan， Huang Lu， Li Yao. A monocular visual SLAM algorithm based on point-line feature［J］. Robot， 2019， 41（3）： 392-403.

[8] He Y J， Zhao J， Guo Y， et al. PL-VIO： tightly-coupled monocular visual-inertial odometry using point and line features［J］. Sensors， 2018， 18（4）： 1159.

[9] Lee J， Park S Y. PLF-VINS： real-time monocular visual-inertial SLAM with point-line fusion and parallel-line fusion［J］. IEEE Robotics and Automation Letters， 2021， 6（4）： 7033-7040.

[10] Qin T， Li P L， Shen S J. VINS-mono： a robust and versatile monocular visual-inertial state estimator［J］. IEEE Transactions on Robotics， 2018， 34（4）： 1004-1020.

[11] Mourikis A I， Roumeliotis S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］//Proceedings 2007 IEEE International Conference on Robotics and Automation. Rome， 2007： 3565-3572.

[12] Sun K， Mohta K， Pfrommer B， et al. Robust stereo visual inertial odometry for fast autonomous flight［J］. IEEE Robotics and Automation Letters， 2018， 3（2）： 965-972.

[13] Campos C， Elvira R， Rodríguez J J G， et al. ORB-SLAM3： an accurate open-source library for visual， visual-inertial， and multimap SLAM［J］. IEEE Transactions on Robotics， 2021， 37（6）： 1874-1890.

[14] Mur-Artal R， Tardós J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo， and RGB-D cameras［J］. IEEE Transactions on Robotics， 2017， 33（5）： 1255-1262.

[15] Langley R B. RTK GPS［J］. GPS World， 1998， 9（9）：70-76.

[16] Zumberge J F， Heflin M B， Jefferson D C， et al. Precise point positioning for the efficient and robust analysis of GPS data from large networks［J］. Journal of Geophysical Research—Solid Earth， 1997， 102： 5005-5017.

[17] Xiang Y， Gao Y， Li Y. Reducing convergence time of precise point positioning with ionospheric constraints and receiver differential code bias modeling［J］. Journal of Geodesy， 2020， 94（1）： 8.

[18] Cao S Z， Lu X Y， Shen S J. GVINS： tightly coupled GNSS-visual-inertial fusion for smooth and consistent state estimation［J］. IEEE Transactions on Robotics， 2022， 38（4）： 2004-2021.

[19] 林利蒙， 王梅. 改进点线特征的双目视觉SLAM算法［J］. 计算机测量与控制， 2019， 27（9）： 156-162.

[20] Lin Li-meng， Wang Mei. An improved stereo SLAM system through combination of points and line segments［J］. Computer Measurement & Control， 2019， 27（9）： 156-162.

基金资助

国家自然科学基金资助项目(41974028)

中央高校基本科研业务费专项资金资助项目(N2201013)