CN114136315A - 一种基于单目视觉辅助惯性组合导航方法及系统 - Google Patents

Abstract

本发明提供了一种基于单目视觉辅助惯性组合导航方法，包括建立基于惯性导航坐标系下的状态模型；建立基于卫星导航系统、惯性导航系统和单目视觉系统的量测模型；基于所述状态模型和量测模型，获得载体当前状态信息。该方法是建立在传统的GNSS/INS组合导航方法的基础上，将单目视觉系统融入到组合导航算法中，有效的改善了在卫星信号有遮挡等复杂情况下由于惯导系统存在误差随时间累计的问题，很大限度的提升组合导航的精准性和应对复杂环境的能力。此外，由于单目摄像头价格比较低，而且计算机视觉算法相对比较简单，因此对原有系统成本和开销没有太大的影响。

Description

一种基于单目视觉辅助惯性组合导航方法及系统

技术领域

本发明涉及导航技术领域，尤其涉及一种基于单目视觉辅助惯性组合导航方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

在社会经济不断提升和信息化建设脚步加快的情况下，卫星导航定位技术为社会的建设和安全提供了有力的支持与保障。

针对全球导航卫星系统(Global Navigation Satellite System,GNSS)和惯性导航系统(InertialNavigationSystem,INS)而言，GNSS具有全球、全天候、高精度、实时定位等优点，它的误差不随时间积累，但是定位终端存在信号容易被遮挡的缺陷，特别是当车辆行驶到高大建筑物下或者隧道里面等场景，定位终端接收到的定位信号差或者干脆接收不到卫星信号，从而使定位误差瞬间增大甚至不能获得定位信息。而INS具有自主导航能力，不需要任何外界电磁信号的情况下根据惯性测量单元(Inertial Measurement Unit,IMU)就可以独立推算出载体的姿态、速度和位置信息，抗外界干扰能力强。但INS存在定位精度与IMU性能强相关的问题，如，IMU的积分效应会导致定位误差随时间的延续不断增大，即误差积累、漂移大，即定位存在误差无限累积的缺点。而在现有的导航系统中，最常用的技术就是基于GNSS/INS组合导航方法设计的卫星组合导航系统。

但随着组合导航的应用场景复杂度日益提高，这对组合系统的鲁棒性有了更加严格的要求，单纯的GNSS与INS组合的方式已经不适用于所有工程应用场景。

发明内容

本发明为了解决上述问题，提出了一种基于单目视觉辅助惯性组合导航方法及系统，本发明通过增加单目相机信息来辅助组合导航过程中的Kalman滤波过程，提升了组合导航算法在信息融合过程中的鲁棒性，提升了导航精度和稳定性。

根据一些实施例，本发明采用如下技术方案：

一种基于单目视觉辅助惯性组合导航方法，包括：

获取惯导系统参量数据；

根据获取的惯导系统参量数据，基于状态模型和量测模型，获得载体当前状态信息；

其中，基于状态模型和量测模型，采用Kalman滤波估计载体的姿态、速度和位置信息，对惯性器件的误差进行修正。

进一步地，所述建立基于卫星导航系统、惯性导航系统和单目视觉系统的量测模型，包括利用卫星导航系统、惯性导航系统解算获取载体的位置、速度、姿态信息以及位置增量信息。

进一步地，所述建立基于卫星导航系统、惯性导航系统和单目视觉系统的量测模型，还包括利用单目视觉系统获取关键帧之间载体的相对位移和航向变化量。

进一步地，所述利用卫星导航系统、惯性导航系统解算获取载体的位置、速度、姿态信息以及位置增量信息，包括利用卫星导航系统计算获取载体的位置、速度信息，对惯性导航系统进行初始化和校准。

进一步地，所述利用卫星导航系统、惯性导航系统解算获取载体的位置、速度、姿态信息以及位置增量信息，还包括利用IMU采集到的原始数据进行惯导更新，计算获取载体的位置、速度、姿态以及位置增量信息。

进一步地，所述利用单目视觉系统获取关键帧之间载体的相对位移和航向变化量，包括利用单目视觉系统对图像中的关键点进行提取和跟踪，通过连续帧中关键点的变化来推算载体相对位移和航向变化信息。

进一步地，所述通过连续帧中关键点的变化来推算载体相对位移和航向变化信息，还包括建立图像测距参考表格。

一种基于单目视觉辅助惯性组合导航系统，包括：

数据获取模块，被配置为，获取惯导系统参量数据；

量测模块，被配置为，根据获取的惯导系统参量数据，基于状态模型和量测模型，获得载体当前状态信息；

其中，基于状态模型和量测模型，采用Kalman滤波估计载体的姿态、速度和位置信息，对惯性器件的误差进行修正。

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行所述的一种基于单目视觉辅助惯性组合导航方法。

一种终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行所述的一种基于单目视觉辅助惯性组合导航方法。

与现有技术相比，本发明的有益效果为：

本发明提供的一种基于单目视觉辅助惯性组合导航方法及系统，具有如下优点：该方法是建立在传统的GNSS/INS组合导航方法的基础上，将单目视觉系统融入到组合导航算法中，有效的改善了在卫星信号有遮挡等复杂情况下由于惯导系统存在误差随时间累计的问题，很大限度的提升组合导航的精准性和应对复杂环境的能力。此外，由于单目摄像头价格比较低，而且计算机视觉算法相对比较简单，因此对原有系统成本和开销没有太大的影响。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1是本实施例1的流程图；

图2是本实施例1的单目视觉中关键点移动原理示意图；

图3是本实施例1的单目视觉测距原理图；

图4是本实施例1的航向变化量计算原理图。

具体实施方式：

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例1

如图1所示，一种基于单目视觉辅助惯性组合导航方法及系统，包括：

第一步，建立基于惯性导航坐标系下的状态模型；

第二步，分别利用卫星导航系统、惯性导航系统解算获取载体的位置、速度姿态信息以及位置增量信息，利用单目视觉系统获取载体相对位移和航向变化量。根据这些信息建立由卫星导航系统/惯性导航系统的速度和位置误差信息、单目视觉系统/惯性导航系统的相对位移和航向变化量误差信息组成的量测模型；

第三步，基于第一步中的状态模型和第二步中的量测模型，采用Kalman滤波估计载体的姿态、速度和位置信息，并对惯性器件的误差进行修正；

进一步的实施例中，该方法进一步具体步骤如下：

步骤1、建立基于惯性导航坐标系下的状态模型；在低精度惯导/卫星组合导航系统中，选择惯导系统的姿态失准角φ、速度误差δvⁿ、位置误差δpⁿ、陀螺漂移

加速度计相关偏差

作为状态(共15维)，如下：

系统状态空间模型为

其中，X_k是n维的状态向量，Z_k是m维的量测向量；Φ_k|k-1、Γ_k-1和H_k是已知的系统结构参数，分别称为n阶的状态一步转移矩阵、n*1阶的系统噪声分配矩阵、m*n阶的量测矩阵；W_k-1是1*1维的系统噪声向量，V_k是m维的量测噪声向量，两者都是零均值的高斯白噪声向量序列，且相互独立不相关，即满足

式中，δ_kj为克罗内克函数。根据Kalman滤波状态空间模型对噪声要求的基本假设，一般要求Q_k是非负定的且R_k是正定的，既有Q_k≥0且R_k＞0。在工程实践中我们将Q_k和R_k定义为对角矩阵。

步骤2、利用卫星导航系统计算获取载体的位置p_gnss、速度信息v_gnss。对IMU器件采集的数据进行误差补偿，包括零偏补偿、比例因子补偿等，同时对惯性导航系统进行初始化和校准。完成校准后利用IMU采集到的原始数据进行惯导更新，计算获取载体的位置p_ins、速度v_ins、姿态att_ins以及位置增量信息Δp_ins等。

步骤3、利用单目视觉系统对图像中的关键点进行提取和跟踪，通过连续帧中关键点的变化来推算载体相对位移和航向变化信息，如图2所示原理。对行驶方向的图像进行匹配和跟踪，即在图像里提取出一系列特征点并进行匹配和跟踪，假设在图像第F_i帧和第F_j帧中有n个特征点，即

和

其中a＝1，2，...，n，将这一系列特征点进行匹配，若匹配成功则该特征点跟踪成功，然后选取其中特征比较明显的一个特征点作为关键点

本方案特征点采用ORB与光流法结合的方式进行特征点匹配和跟踪，因为ORB具有较好的尺度不变性和旋转不变性，是一种较为稳健的特征点选取方法，但关键帧只占所有帧中的一部分，ORB算法比较耗时的部分，采用ORB提取关键帧特征点，对非关键帧使用光流法的策略，不需要提取所有帧的特征点，这样就可以有效提升运行效率。

利用光流法进行帧间位移定位，并根据经验设定每帧跟踪的最少特征点数，在跟踪特征点数低于设定的阈值后，利用改进的ORB算法进行帧间位移定位。克服了光流法定位精度差的缺点，突出了改进ORB算法定位准确性的优点，能够提供较准确的定位输出。利用最新的观测信息，可以得到载体运动的距离和航向的变化情况。

步骤4、建立图像测距参考表格，基于像素坐标系下的测距标点原理如图3所示。在前期对相机进行标定时，增加测距标定。在计算机视觉描述的世界坐标系中，距离相机镜头相同距离的点应该在同一个圆上，当将这些点投影到像素坐标系中时，随着距离的增加，由于投影透视原理会造成远处的弧形逐渐变为直线，因此为了减小标注工作量和标注误差的影响，可以选取像素平面中心区域作为测距的参考线区域，并根据实际测量距离对像素点进行等距离标注，最终标注的参考点集合可以表示为{D(y₀)，D(y₁)…，D(y_n)}，其中D(y_m)表示纵轴像素为y_m的点到相机坐标系中心点的位置，并且满足d＝D(y_m)-D(y_m-1)，d表示点集的固定间距差。

步骤5、根据对关键帧获取到的关键点

信息，计算载体相对位移，计算公式如下：假设关键点

中

其中Δp_i≤y_i-1-y_i，则该点距离载体的距离可以表示为：

同样的原理关键点

距离载体的距离可以表示为：

则载体移动的距离为：

假设第F_j帧时刻惯导输出的姿态信息att_ins＝[pitch_j，roll_j，yaw_j]，则根据单目视觉系统获得的载体相对位移在导航坐标系下的表达形式为：

Δp_vns＝[dis*sin(yaw_j)，dis*cos(yaw_j)，0]

步骤6、根据对关键帧获取到的关键点

信息，计算载体航向变化量，计算公式如下：

选取图像中C_o作为固定的基准点，C_o坐标为(C_x，C_y)。则C_o与关键帧F_i的关键点之间的方向可以表示为：

则关键帧F_i与关键帧F_j中关键点角度变化量为：

步骤7、把GNSS与INS的速度误差、位置误差，以及INS与单目视觉相对位移误差和航向误差作为测量信息，建立误差量测模型的表达式

其中v＝[v_E，v_N，v_U]表示载移动速度矢量，v_E，v_N，v_U分别表示导航坐标系下东、北、天向速度，单位m/s。p＝[L，B，H]表示载体的位置信息矢量，L，B，H分别表示导航坐标系纬度、经度和高度信息，单位分别为rad、rad和m。Δp表示相对位移矢量，单位m。Δyaw表示航向变化量，单位rad。

步骤8、进行Kalman滤波获得载体当前时刻的位置、速度及姿态估计如下。

实施例2

一种基于单目视觉辅助惯性组合导航系统，包括：

数据获取模块，被配置为，获取惯导系统参量数据；

量测模块，被配置为，根据获取的惯导系统参量数据，基于状态模型和量测模型，获得载体当前状态信息；

其中，基于状态模型和量测模型，采用Kalman滤波估计载体的姿态、速度和位置信息，对惯性器件的误差进行修正。

实施例3

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行本实施例1提供的一种基于单目视觉辅助惯性组合导航方法。

实施例4

一种终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行本实施例1提供的一种基于单目视觉辅助惯性组合导航方法。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种基于单目视觉辅助惯性组合导航方法，其特征在于，包括：

获取惯导系统参量数据；

根据获取的惯导系统参量数据，基于状态模型和量测模型获得载体当前状态信息；

其中，基于状态模型和量测模型获得载体当前状态信息，包括采用Kalman滤波估计载体的姿态、速度和位置信息，对惯性器件的误差进行修正。

2.如权利要求1所述的一种基于单目视觉辅助惯性组合导航方法，其特征在于，所述建立基于卫星导航系统、惯性导航系统和单目视觉系统的量测模型，包括利用卫星导航系统、惯性导航系统解算获取载体的位置、速度、姿态信息以及位置增量信息。

3.如权利要求2所述的一种基于单目视觉辅助惯性组合导航方法，其特征在于，所述建立基于卫星导航系统、惯性导航系统和单目视觉系统的量测模型，还包括利用单目视觉系统获取关键帧之间载体的相对位移和航向变化量。

4.如权利要求3所述的一种基于单目视觉辅助惯性组合导航方法，其特征在于，所述利用卫星导航系统、惯性导航系统解算获取载体的位置、速度、姿态信息以及位置增量信息，包括利用卫星导航系统计算获取载体的位置、速度信息，对惯性导航系统进行初始化和校准。

5.如权利要求4所述的一种基于单目视觉辅助惯性组合导航方法，其特征在于，所述利用卫星导航系统、惯性导航系统解算获取载体的位置、速度、姿态信息以及位置增量信息，还包括利用IMU采集到的原始数据进行惯导更新，计算获取载体的位置、速度、姿态以及位置增量信息。

6.如权利要求5所述的一种基于单目视觉辅助惯性组合导航方法，其特征在于，所述利用单目视觉系统获取关键帧之间载体的相对位移和航向变化量，包括利用单目视觉系统对图像中的关键点进行提取和跟踪，通过连续帧中关键点的变化来推算载体相对位移和航向变化信息。

7.如权利要求6所述的一种基于单目视觉辅助惯性组合导航方法，其特征在于，所述通过连续帧中关键点的变化来推算载体相对位移和航向变化信息，还包括建立图像测距参考表格。

8.一种基于单目视觉辅助惯性组合导航系统，包括：

数据获取模块，被配置为，获取惯导系统参量数据；

量测模块，被配置为，根据获取的惯导系统参量数据，基于状态模型和量测模型，获得载体当前状态信息；

其中，基于状态模型和量测模型，采用Kalman滤波估计载体的姿态、速度和位置信息，对惯性器件的误差进行修正。

9.一种计算机可读存储介质，其特征在于，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行权利要求1-7中任一项所述的一种基于单目视觉辅助惯性组合导航方法。

10.一种终端设备，其特征在于，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行权利要求1-7中任一项所述的一种基于单目视觉辅助惯性组合导航方法。

Images