CN113701749A - 一种基于立体视觉惯性里程计的车辆定位导航方法及系统 - Google Patents

Abstract

本发明公开了一种基于立体视觉惯性里程计的车辆定位导航方法及系统，该方法包括如下步骤：捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息；将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。

Description

一种基于立体视觉惯性里程计的车辆定位导航方法及系统

技术领域

本发明涉及一种基于立体视觉惯性里程计的车辆定位导航方法及系统，涉及卫星导航、人工智能、无人驾驶以及时间频率应用等领域。

背景技术

车辆的准确和连续定位是自动驾驶汽车和移动地图系统的主要要求，全球导航卫星系统(GNSS)是满足户外环境中这一需求的关键技术；然而，在建筑密集的地区、城市峡谷和隧道中，GNSS无法很好利用，准确和可靠的车辆定位成为一个挑战。

惯性传感器通常与GNSS集成，可以在没有GNSS观测的情况下提供位置估计，但惯性导航系统(INS,Inertial Navigation System)定位会随着时间的推移定位误差会大量地积累和增长，因此INS不适合在GNSS信号丢失时长时间的定位。

惯性传感器和视觉传感器具有互补的特性，INS可以在每次图像采集时提供相机姿态的初始估计，视觉特征可以限制INS漂移；此外，无法保证纯粹基于视觉的导航系统可以提供连续定位，因为它们高度依赖视觉特征，因此在无特征的环境中表现不佳。

基于相机与惯性传感器集成的一般导航方法被称为视觉惯性里程计(VIO,Visual-Inertial Odometry)。在机器人和计算机视觉方面对VIO进行了大量研究；用于将惯性测量与图像观测相结合的算法主要分为两类：基于优化的方法和滤波方法；优化算法通过对最小二乘误差函数的迭代最小化来估计未知参数，尽管这些算法提供了更好的估计，但它们必须限制可用的测量值，以实现实时姿态估计；与优化方法相反，滤波算法一有图像观测结果就以递归方式估计参数；在过去的几十年里，人们提出了许多滤波方法，用来解决实时问题；这些方法要么计算成本高，因此不适合实时定位，要么没有充分利用跟踪特性为提供的约束。

多态约束卡尔曼滤波(MSCKF,Multi-State Constraint Kalman Filter)使用了一个特征从多个相机姿势提供的最大几何约束，很好地解决了实时性和跟踪特征的约束问题，但该方法的一个挑战是单图像序列跟踪特征可靠性低，影响姿态估计的准确性。

发明内容

本发明的目的是提供一种基于立体视觉惯性里程计的车辆定位导航方法及系统，解决现有技术中单图像序列跟踪特征可靠性低，姿态估计的准确性低等问题。

本发明是通过下述技术方案实现的：

第一方面，提供了一种基于立体视觉惯性里程计的车辆定位导航方法，包括如下步骤：

捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息，将所述位姿信息、特征数据、位姿偏差信息和状态信息进行传播；

将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；

通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；

根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。

结合第一方面，进一步的，通过INS捕获待观测点的位姿信息。

结合第一方面，进一步的，通过左侧相机捕获待观测点的特征数据。

结合第一方面，进一步的，预校准包括比对和分析。

结合第一方面，进一步的，INS状态向量通过如下公式预设：

其中，

是4×1单位四元数，表示在框架坐标系{B}中解析的INS的旋转量；^εV_B和^εP_B分别表示INS相对于地心地固坐标系{ε}的线性速度和位置；b_g和b_a分别表示3×1的陀螺仪和加速度计的偏差。

结合第一方面，进一步的，滤波处理是通过扩展卡尔曼滤波进行的。

第二方面，本发明还提供了一种基于立体视觉惯性里程计的车辆定位导航系统，包括MSCKF传播部分、MSCKF增强部分、相机部分和MSCKF更新部分；

MSCKF传播部分：用于捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息；

MSCKF增强部分：用于将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；

相机部分：包括左侧相机和右侧相机，通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；

MSCKF更新部分：用于根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。

与现有技术相比，本发明所达到的有益效果是：

本发明提供的一种基于立体视觉惯性里程计的车辆定位导航方法及系统，利用MSCKF集成待观测点的位姿信息和特征数据，然后进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵，增加了相机位姿估计的约束，提高了精确度；在相机部分，本发明采用左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息，增加对目标特征的估计；在MSCKF更新部分，根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，增加了对目标特征的滤波约束，提高了特征跟踪的有效性和准确性。

附图说明

为了更清楚地说明本方实施例的技术方案，下面将对本发明实施例中所需要使用的附图作简单地介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种基于立体视觉惯性里程计的车辆定位导航方法的流程图；

图2是本发明实施例提供的单声道MSCKF、立体声MSCKF、INS/GNSS组合与地面真实数据的比较图；

图3是本发明实施例提供的单声道MSCKF、立体声MSCKF、INS/GNSS组合的误差累计分布图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例一

本发明提供了一种基于立体视觉惯性里程计的车辆定位导航方法，包括如下步骤：

捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息，将所述位姿信息、特征数据、位姿偏差信息和状态信息进行传播

通过INS捕获待观测点的位姿信息，通过左侧相机捕获待观测点的特征数据，INS获得的位姿信息与左侧相机的特征数据进行比对、分析，即预校准，获得待观测点的位姿偏差信息和状态信息，将位姿信息、特征数据、位姿偏差信息和状态信息进行传播。

INS误差状态传播所涉及的误差包括姿态误差、速度误差、位置误差和偏差。

在车载坐标系中姿态误差传播可表示为：

其中，

是姿态误差的微分，向量

在车身坐标系中解析的角速度估计值，可从传感器角速度ω_m得到，

为陀螺仪偏差估计，

运算符

表示向量x的倾斜对称矩阵，可以表示为：

在车身坐标系{ε}位置误差的时间导数可简化为速度误差，可表示为：

假设加速度计和陀螺仪的偏差不会随时间而变化，即

将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵

获得的位姿偏差信息和状态信息经过传播，送入MSCKF增强部分；将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，并对INS状态和相机姿态的协方差矩阵进行增广。

INS的状态向量可表示为：

式中，

是4×1单位四元数，表示在框架坐标系{B}中解析的INS的旋转量；^εV_B和^εP_B分别表示INS相对于地心地固坐标系{ε}的线性速度和位置；b_g和b_a分别表示3×1的陀螺仪和加速度计的偏差。

上式INS误差状态方程可写成：

式中，

定义了车身框架中INS姿态的扰动。这个向量可以通过四元数误差演化线性近似为

完整的EKF状态向量包括INS的状态向量以及视觉特征的M个相机姿势。(16+7M)×1个EKF状态方程可表示为：

在这个表达式中，其余参数是包含相机四元数的M个相机姿势，

表示在相机坐标系{C_i}中解析的INS的旋转量；

为对应在地心地固坐标系(ECEF){ε}中的转换。

INS误差状态方程可近似表示成如下所示的(15+6M)×1维误差状态向量：

测量向量y被描述为状态和相应的白噪声源的线性模型：

y＝HδX+n

其中，n是白噪声，H是由系统的已知性质确定的测量雅可比矩阵。

其中，

是为特征点f_j在第i相机坐标系下的位置估计向量，

是从地球坐标系到内部相机坐标系的旋转矩阵，

为特征点f_j在坐标系{ε}中的位置，假设摄像机已进行了校准，则视觉测量模型描述为：

其中，

为第j个特征点在第i帧图像中的观测向量，η是图像的噪声向量，其协方差：

估计了特征的全局位置后，在状态中增强的左相机的测量残余描述为：

式中，

为左相机坐标系C_li下计算得到的特征点f_j的观测值；对相机姿态参数和特征三维位置残差的线性化，上式可表示为

式中，

是特征点f_j的位置估计误差，

分别表示观测值

的状态和特征点位置的雅可比矩阵，可表示为：

雅可比矩阵

可表示为：

定义右侧相机位姿为

关于左侧相机的姿势和它们之间的相对校准参数：

其中，

左右相机之间的旋转矩阵和位移向量，

表示左侧相机坐标系和地球坐标系之间的旋转矩阵。

其中，

为地球坐标系{ε}到右侧相机坐标系的旋转单位四元数，

表示从左侧相机坐标系向右侧的旋转单位四元数，可以获得特征点f_j在第i对相机的右侧相机坐标系的位置。

左侧相机和特征位置的估计的测量残差可线性化定义为：

其中，

表示从右侧相机姿态观测特征点f_j的雅可比矩阵，分别对应于特征点的误差状态和全局位置。

可获得残差向量

其中，

因此，误差状态δX、与特征位置的误差相关

可由下式解算出残差向量r^(j)：

式中，A为具有H_f的左零空间的基的列的酉矩阵，EKF形式为：

其中，

一旦图像被记录下来，相机的姿势参数就会被添加到状态中以增强状态。这些参数与INS的姿态估计有关：

其中，

是从车身坐标系{ε}到相机坐标系{C}旋转的单位四元数，

定义从车身坐标系到相机坐标系的旋转得到的相机和INS之间的离线校准量，

为四元数乘法。

照相机相对于ECEF帧的位置也被定义为：

其中，

相机在ECEF坐标系中的位置，

为相机坐标系与车身坐标系之间的位移向量，可通过校准处理得到。

最终，扩展卡尔曼滤波(EKF)的协方差矩阵被增强，可得：

其中，J是照相机相对于ECEF帧的位置定义公式对应的第k步状态向量的雅可比亚矩阵：

通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息

左侧相机和右侧相机对特征点进行观测，观察每张图像中的所有特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息。

根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果

整个残差的块向量被描述为：

r₀＝H₀δX+n₀

最后，滤波更新过程中卡尔曼增益为：

式中，P为状态协方差矩阵，R_N是噪声分量的协方差矩阵，

由不确定性的传播计算出来的，

下一步状态：

进一步简化：后验误差协方差被更新为：

P_k+1|k+1＝(I-KR_H)P_k+1|k(I-KR_H)^T+KR_NK^T

其中，I为单位矩阵。

实施例二

本发明提供了一种基于立体视觉惯性里程计的车辆定位导航系统，包括MSCKF传播部分、MSCKF增强部分、相机部分和MSCKF更新部分。

MSCKF传播部分：用于捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息。

MSCKF增强部分：用于将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵。

相机部分：包括左侧相机和右侧相机，通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息。

MSCKF更新部分：用于根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。

本发明利用序列图像匹配特征约束估计姿态，为组合系统提供了一个额外的约束，当特征值失效或者附加位姿达到最大阈值时，执行更新程序MSCKF维护一个可变的车辆姿势窗口，并同时更新窗口中的每个姿势，在不额外添加状态向量新特征的前提下，提高了姿态估计精度。

实施例三

为了评估本发明提供的一种基于立体视觉惯性里程计的车辆定位导航方法及系统的定位性能，使用已有的观测数据集进行验证实验；数据集包含相机拍摄的154对图像，大小的1392x512像素，时间分辨率约0.1秒；此外，这些数据集还包括由INS和GNSS导航系统的测量值，这些数据以RTKGNSS观测数据为参考基准；使用均方根误差(RMSE)评估系统定位误差性能；开展与单声道视觉-惯性里程计的组合系统、地面真实运动轨迹分析比较，验证其定位性能。

图2显示了单声道MSCKF、立体声MSCKF、INS/GNSS组合与地面真实情况比较分析；虽然MSCKF在两种模式下的性能都比惯性定位(INS/GNSS)要好得多，但立体声MSCKF在两个数据集中都提供了比单声道MSCKF更精确的轨迹。

图3显示了与INS/GNSS相比，单声道MSCKF和立体声MSCKF的平移误差的累积分布；这里的表演是为了最佳的相机姿势窗口，立体声MSCKF的性能优于其他方法，达到轨迹长度的0.9％，而单声道MSCKF和INS/GNSS集成的平均精度分别为1.3％和2.5％。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (7)

1.一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，包括如下步骤：

捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息，将所述位姿信息、特征数据、位姿偏差信息和状态信息进行传播；

将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；

通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；

根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。

2.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，通过INS捕获待观测点的位姿信息。

3.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，通过左侧相机捕获待观测点的特征数据。

4.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，所述预校准包括比对和分析。

5.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，所述INS状态向量通过如下公式预设：

其中，

是4×1单位四元数，表示在框架坐标系{B}中解析的INS的旋转量；^εV_B和^εP_B分别表示INS相对于地心地固坐标系{ε}的线性速度和位置；b_g和b_a分别表示3×1的陀螺仪和加速度计的偏差。

6.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，所述滤波处理是通过扩展卡尔曼滤波进行的。

7.一种基于立体视觉惯性里程计的车辆定位导航系统，其特征在于，包括MSCKF传播部分、MSCKF增强部分、相机部分和MSCKF更新部分；

MSCKF传播部分：用于捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息；

MSCKF增强部分：用于将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；

相机部分：包括左侧相机和右侧相机，通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；

MSCKF更新部分：用于根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。

Images