CN115096309A - 融合定位方法、装置、电子设备及存储介质 - Google Patents

融合定位方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN115096309A
CN115096309A CN202210662635.2A CN202210662635A CN115096309A CN 115096309 A CN115096309 A CN 115096309A CN 202210662635 A CN202210662635 A CN 202210662635A CN 115096309 A CN115096309 A CN 115096309A
Authority
CN
China
Prior art keywords
data
module
imu
gnss
positioning
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210662635.2A
Other languages
English (en)
Inventor
肖天中
丁可
王瑜婵
李军
杨志坤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing Starnav Systems Co ltd
Original Assignee
Chongqing Starnav Systems Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing Starnav Systems Co ltd filed Critical Chongqing Starnav Systems Co ltd
Priority to CN202210662635.2A priority Critical patent/CN115096309A/zh
Publication of CN115096309A publication Critical patent/CN115096309A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Processing (AREA)

Abstract

本申请属于导航定位技术领域，公开了一种融合定位方法、装置、电子设备及存储介质，通过联合视觉相机和IMU模块获取被定位对象的第一位姿数据；联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；根据GNSS信号的质量确定权重值；根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值；从而实现了视觉定位技术、GNSS定位技术和IMU定位技术的有机融合，可提高在对GNSS信号具有长时间遮挡作用的复杂环境中的定位精度。

Description

融合定位方法、装置、电子设备及存储介质
技术领域
本申请涉及导航定位技术领域，具体而言，涉及一种融合定位方法、装置、电子设备及存储介质。
背景技术
目前，进行导航定位时多采用GNSS（Global Navigation Satellite System，全球卫星导航系统）技术和IMU（Inertial Measurement Unit，惯性测量单元）技术进行定位。若单独使用GNSS技术进行定位，当出现GNSS信号遮挡的情况，则无法正常进行定位，若单独使用IMU技术进行定位，由于累计误差的现象，长时间使用时，定位偏差过大；因此，常常会结合GNSS技术和IMU技术进行融合定位（以下称之为GNSS/IMU融合定位技术），在GNSS信号出现短暂中断时，通过IMU技术进行定位，当GNSS信号恢复时，根据GNSS定位结果对IMU的累计误差进行修正，从而提高在复杂环境中的定位能力。
然而，采用这种GNSS/IMU融合定位技术进行导航定位时，在长隧道、地下车库等对GNSS信号具有长时间遮挡作用的复杂环境中，由于GNSS信号被长时间遮挡，IMU定位模块长时间单独工作进行定位，导致累计误差过大，进而使定位结果出现较大偏差；因此，这种GNSS/IMU融合定位技术不适用于对GNSS信号具有长时间遮挡作用的复杂环境中。
目前，视觉SLAM（Simultaneous Localization and Mapping，同步定位与建图）技术在室内定位领域中得到了广泛使用，使用视觉SLAM技术进行导航定位，可以有效提高定位精度，但是，在初始化时存在尺度不确定和钝化问题。
因此，需要寻求一种综合视觉SLAM技术、GNSS技术和IMU技术的优点的融合定位方法，以提高在对GNSS信号具有长时间遮挡作用的复杂环境中的定位精度。
发明内容
本申请的目的在于提供一种融合定位方法、装置、电子设备及存储介质，可提高在对GNSS信号具有长时间遮挡作用的复杂环境中的定位精度。
第一方面，本申请提供了一种融合定位方法，应用于具有视觉相机、IMU模块和GNSS模块的定位系统，对被定位对象进行定位，包括以下步骤：
A1.联合所述视觉相机和所述IMU模块获取所述被定位对象的第一位姿数据；
A2.联合所述IMU模块和所述GNSS模块获取所述被定位对象的第二位姿数据；
A3.根据GNSS信号的质量确定权重值；
A4.根据所述权重值对所述第一位姿数据和所述第二位姿数据进行加权融合计算，得到所述被定位对象的位姿数据测量值。
该融合定位方法，先分别结合视觉相机和IMU模块得到第一位姿数据、结合IMU模块和GNSS模块得到第二位姿数据，再根据GNSS信号的质量好坏确定第一位姿数据和第二位姿数据融合时的权重值，最后对第一位姿数据和第二位姿数据进行加权融合计算得到最终的位姿数据，在GNSS信号被长时间遮挡时，依然能够根据视觉相机和IMU模块得到比较准确的定位结果，与现有技术中的GNSS/IMU融合定位技术相比，在对GNSS信号具有长时间遮挡作用的复杂环境中，具有更高的定位精度。
优选地，步骤A1包括：
获取所述视觉相机采集的多帧图像；
根据所述图像提取相邻两帧图像的匹配特征点的像素坐标；
根据所述匹配特征点的像素坐标进行对极几何求解，计算出所述被定位对象的粗略位姿数据；
获取所述IMU模块的测量数据的并进行预积分处理得到对应的预积分数据；所述测量数据包括加速度计偏置和陀螺仪偏置；
根据所述匹配特征点的像素坐标和所述预积分数据通过三角法进行深度估计，得到所述匹配特征点的深度数据；
根据所述粗略位姿数据、所述深度数据、所述IMU模块的测量数据、所述预积分数据、所述IMU模块的外参和所述视觉相机的外参生成状态变量，并对所述状态变量进行非线性优化处理，得到所述状态变量的最优估计值；
从所述状态变量的最优估计值中提取所述第一位姿数据。
通过采用预积分数据对匹配特征点进行深度估计，可以有效避免视觉定位在初始化时存在的尺度不确定和钝化问题，通过对状态变量进行非线性优化处理进而得到第一位姿数据，可提高第一位姿数据的精度。
优选地，所述状态变量为：
Figure 581539DEST_PATH_IMAGE001
；
其中，
Figure 701942DEST_PATH_IMAGE002
为所述状态变量，
Figure 934209DEST_PATH_IMAGE003
分别为第1至第n帧图像对应的状态估计数据，对于第k帧图像有：
Figure 671963DEST_PATH_IMAGE004
；
Figure 424018DEST_PATH_IMAGE005
为与第k帧图像对应的所述状态估计数据，
Figure 699011DEST_PATH_IMAGE006
为第k帧图像对应的所述粗略位姿数据中的位置，
Figure 903727DEST_PATH_IMAGE007
为第k帧图像对应的所述粗略位姿数据中的姿态，
Figure 555157DEST_PATH_IMAGE008
为第k帧图像对应的速度，
Figure 161719DEST_PATH_IMAGE009
为第k帧图像对应的所述加速度计偏置，
Figure 607613DEST_PATH_IMAGE010
为第k帧图像对应的所述陀螺仪偏置；
其中，
Figure 299625DEST_PATH_IMAGE011
；
Figure 226517DEST_PATH_IMAGE012
为外参矩阵，
Figure 687586DEST_PATH_IMAGE013
为所述视觉相机的外参，
Figure 445326DEST_PATH_IMAGE014
为所述IMU模块的外参；
Figure 405061DEST_PATH_IMAGE015
为第1至第n帧图像对应的所述深度数据；
所述对状态变量进行非线性优化处理，得到所述状态变量的最优估计值的步骤包括：
根据以下模型进行最大后验估计，得到所述状态变量的最优估计值：
Figure 617867DEST_PATH_IMAGE016
；
其中，
Figure 713868DEST_PATH_IMAGE017
为边缘化残差，
Figure 986718DEST_PATH_IMAGE018
为与协方差相关的马氏距离，
Figure 905520DEST_PATH_IMAGE019
为雅可比矩阵；
其中，
Figure 312230DEST_PATH_IMAGE020
为IMU残差，
Figure 13470DEST_PATH_IMAGE021
为IMU残差函数，
Figure 972068DEST_PATH_IMAGE022
为IMU测量值，
Figure 391548DEST_PATH_IMAGE023
为IMU预积分噪声项的协方差矩阵；
其中，
Figure 461004DEST_PATH_IMAGE024
为视觉重投影误差，
Figure 751171DEST_PATH_IMAGE025
为视觉重投影误差函数，
Figure 883599DEST_PATH_IMAGE026
为视觉特征点坐标，
Figure 649430DEST_PATH_IMAGE027
为视觉观测噪声的协方差矩阵。
上述模型是一种将视觉定位数据和IUM定位数据综合利用的紧耦合模型，其优势在于对位姿的状态变量的估算结果比较准确，从而能够得到比较准确的第一位姿数据。
优选地，步骤A2包括：
采用基于卡尔曼滤波的GNSS/IMU融合算法获取所述被定位对象的第二位姿数据。
优选地，步骤A3包括：
获取卫星定位系统的各卫星测得的所述被定位对象的第三位姿数据；
根据所述第三位姿数据计算伪距残差平方和；
根据所述伪距残差平方和计算最小二乘残差统计量和检测门限值；
对比所述最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏；
根据GNSS信号的质量好坏的判断结果，确定所述权重值。
优选地，所述对比所述最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏的步骤包括：
若
Figure 7730DEST_PATH_IMAGE028
，则判定所述GNSS信号的质量不好；
若
Figure 667251DEST_PATH_IMAGE029
，则判定所述GNSS信号的质量好；
若
Figure 718383DEST_PATH_IMAGE030
，则判定所述GNSS信号的质量中等；
其中，
Figure 361723DEST_PATH_IMAGE031
为所述最小二乘残差统计量，
Figure 436296DEST_PATH_IMAGE032
为所述检测门限值，
Figure 435476DEST_PATH_IMAGE033
为伪距残差的标准差；
所述根据GNSS信号的质量好坏的判断结果，确定所述权重值的步骤包括：
若所述GNSS信号的质量不好，则
Figure 906778DEST_PATH_IMAGE034
；
若所述GNSS信号的质量好，则
Figure 522567DEST_PATH_IMAGE035
；
若所述GNSS信号的质量中等，则
Figure 737517DEST_PATH_IMAGE036
；
Figure 122362DEST_PATH_IMAGE037
为所述权重值。
优选地，步骤A4包括：
根据以下公式计算所述被定位对象的位姿数据测量值：
Figure 639931DEST_PATH_IMAGE038
；
其中，
Figure 995213DEST_PATH_IMAGE039
为所述被定位对象的位姿数据测量值，
Figure 764586DEST_PATH_IMAGE040
为所述第一位姿数据，
Figure 253205DEST_PATH_IMAGE041
为所述第二位姿数据。
第二方面，本申请提供了一种融合定位装置，应用于具有视觉相机、IMU模块和GNSS模块的定位系统，对被定位对象进行定位，包括：
第一获取模块，用于联合所述视觉相机和所述IMU模块获取所述被定位对象的第一位姿数据；
第二获取模块，用于联合所述IMU模块和所述GNSS模块获取所述被定位对象的第二位姿数据；
第一计算模块，用于根据GNSS信号的质量确定权重值；
第二计算模块，用于根据所述权重值对所述第一位姿数据和所述第二位姿数据进行加权融合计算，得到所述被定位对象的位姿数据测量值。
该融合定位装置，先分别结合视觉相机和IMU模块得到第一位姿数据、结合IMU模块和GNSS模块得到第二位姿数据，再根据GNSS信号的质量好坏确定第一位姿数据和第二位姿数据融合时的权重值，最后对第一位姿数据和第二位姿数据进行加权融合计算得到最终的位姿数据，在GNSS信号被长时间遮挡时，依然能够根据视觉相机和IMU模块得到比较准确的定位结果，与现有技术中的GNSS/IMU融合定位技术相比，在对GNSS信号具有长时间遮挡作用的复杂环境中，具有更高的定位精度。
第三方面，本申请提供了一种电子设备，包括处理器以及存储器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如前文所述融合定位方法中的步骤。
第四方面，本申请提供了一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述融合定位方法中的步骤。
有益效果
本申请提供的融合定位方法、装置、电子设备及存储介质，通过联合所述视觉相机和所述IMU模块获取所述被定位对象的第一位姿数据；联合所述IMU模块和所述GNSS模块获取所述被定位对象的第二位姿数据；根据GNSS信号的质量确定权重值；根据所述权重值对所述第一位姿数据和所述第二位姿数据进行加权融合计算，得到所述被定位对象的位姿数据测量值；从而实现了视觉定位技术、GNSS定位技术和IMU定位技术的有机融合，可提高在对GNSS信号具有长时间遮挡作用的复杂环境中的定位精度。
本申请的其他特征和优点将在随后的说明书阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请实施例了解。本申请的目的和其他优点可通过在所写的说明书、以及附图中所特别指出的结构来实现和获得。
附图说明
图1为本申请实施例提供的融合定位方法的一种流程图。
图2为本申请实施例提供的融合定位装置的一种结构示意图。
图3为本申请实施例提供的电子设备的结构示意图。
具体实施方式
下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
请参照图1，本申请一些实施例中的一种融合定位方法，应用于具有视觉相机、IMU模块和GNSS模块的定位系统，对被定位对象进行定位，包括以下步骤：
A1.联合视觉相机和IMU模块获取被定位对象的第一位姿数据；
A2.联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；
A3.根据GNSS信号的质量确定权重值；
A4.根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值。
其中，被定位对象是安装有该定位系统且可移动的设备，例如移动机器人、车辆、无人机、船舶等。
该融合定位方法，先分别结合视觉相机和IMU模块得到第一位姿数据、结合IMU模块和GNSS模块得到第二位姿数据，再根据GNSS信号的质量好坏确定第一位姿数据和第二位姿数据融合时的权重值，最后对第一位姿数据和第二位姿数据进行加权融合计算得到最终的位姿数据，在GNSS信号被长时间遮挡时，依然能够根据视觉相机和IMU模块得到比较准确的定位结果，与现有技术中的GNSS/IMU融合定位技术相比，在对GNSS信号具有长时间遮挡作用的复杂环境中，具有更高的定位精度。此外，与传统的视觉SLAM定位技术相比，可省略SLAM建图的步骤，计算量更小。可见，该融合定位方法不但环境适应性高，定位精度好，且计算量相对较少。
在本实施例中，步骤A1包括：
获取视觉相机采集的多帧图像；
根据图像提取相邻两帧图像的匹配特征点的像素坐标；
根据匹配特征点的像素坐标进行对极几何求解，计算出被定位对象的粗略位姿数据；
获取IMU模块的测量数据的并进行预积分处理得到对应的预积分数据；测量数据包括加速度计偏置和陀螺仪偏置；
根据匹配特征点的像素坐标和预积分数据通过三角法进行深度估计，得到匹配特征点的深度数据；
根据粗略位姿数据、深度数据、IMU模块的测量数据、预积分数据、IMU模块的外参和视觉相机的外参生成状态变量，并对状态变量进行非线性优化处理，得到状态变量的最优估计值；
从状态变量的最优估计值中提取第一位姿数据。
通过采用预积分数据对匹配特征点进行深度估计，可以有效避免视觉定位在初始化时存在的尺度不确定和钝化问题，通过对状态变量进行非线性优化处理进而得到第一位姿数据，可提高第一位姿数据的精度。
其中，特征点为图像像素值发生剧烈变化的点，匹配特征点是两个图像中相互匹配的特征点。可采用现有的图像识别方法来从图像中提取匹配特征点的位置数据，此处不对其具体方法进行限定。
其中，极几何求解过程为现有技术，此处不对其进行详述。其中，求解过程中，深度做了归一化处理，因此，粗略位姿数据存在尺度不确定性问题，不能直接作为第一位姿数据使用。
其中，根据匹配特征点的像素坐标和预积分数据对特征点进行深度估计，得到匹配特征点的深度数据的步骤，具体包括：首先对IMU数据（即IMU模块的测量数据）进行预积分可以计算出两帧图像之间的相对距离和相对姿态，再通过三角法可以得到匹配特征点与成像平面的距离。
在本实施例中，状态变量为：
Figure 207255DEST_PATH_IMAGE042
；
其中，
Figure 797636DEST_PATH_IMAGE002
为状态变量，
Figure 354388DEST_PATH_IMAGE003
分别为第1至第n帧图像对应的状态估计数据（进行状态变量的生成之前，需要先对视觉相机采集的图像和IMU模块采集的的测量数据进行时间同步处理，具体的时间同步处理过程为现有技术，此处不对其进行详述；其中，此处的第1至第n帧图像是滑动窗口内的图像，滑动窗口越大表示n越大则约束条件越多，计算结果越准确，但计算量越大，n的大小可根据实际需要设置，一般地窗口大小范围为10-20，即n一般为10-20），对于第k帧图像有：
Figure 448246DEST_PATH_IMAGE004
；
Figure 700760DEST_PATH_IMAGE005
为与第k帧图像对应的状态估计数据，
Figure 247279DEST_PATH_IMAGE006
为第k帧图像对应的粗略位姿数据中的位置（一般地，第k帧图像对应的粗略位姿数据为通过第k-1帧图像和第k帧图像计算得到的粗略位姿数据，对于第1帧图像，可通过上一个窗口中的最后一帧图像与该第1帧图像计算得到对应的粗略位姿数据），
Figure 748668DEST_PATH_IMAGE007
为第k帧图像对应的粗略位姿数据中的姿态，
Figure 211879DEST_PATH_IMAGE043
为第k帧图像对应的速度（通过对加速度计偏置进行预积分得到，即预积分数据包括速度），
Figure 383097DEST_PATH_IMAGE009
为第k帧图像对应的加速度计偏置，
Figure 931759DEST_PATH_IMAGE044
为第k帧图像对应的陀螺仪偏置；
其中，
Figure 581046DEST_PATH_IMAGE011
；
Figure 636115DEST_PATH_IMAGE012
为外参矩阵，
Figure 712655DEST_PATH_IMAGE013
为视觉相机的外参，
Figure 483034DEST_PATH_IMAGE045
为IMU模块的外参；
Figure 326225DEST_PATH_IMAGE046
为第1至第n帧图像对应的深度数据；
对状态变量进行非线性优化处理，得到状态变量的最优估计值的步骤包括：
根据以下模型进行最大后验估计（可采用现有的最大后验估计方法进行计算，此处不对其进行限定），得到状态变量的最优估计值：
Figure 514761DEST_PATH_IMAGE016
；
其中，
Figure 11470DEST_PATH_IMAGE047
为边缘化残差，
Figure 285456DEST_PATH_IMAGE018
为与协方差相关的马氏距离，
Figure 681408DEST_PATH_IMAGE019
为雅可比矩阵；
其中，
Figure 973718DEST_PATH_IMAGE048
为IMU残差，
Figure 392061DEST_PATH_IMAGE049
为IMU残差函数，
Figure 137032DEST_PATH_IMAGE050
为IMU测量值，
Figure 587605DEST_PATH_IMAGE023
为IMU预积分噪声项的协方差矩阵；
其中，
Figure 485153DEST_PATH_IMAGE051
为视觉重投影误差，
Figure 592174DEST_PATH_IMAGE052
为视觉重投影误差函数，
Figure 575174DEST_PATH_IMAGE053
为视觉特征点，
Figure 422913DEST_PATH_IMAGE027
为视觉观测噪声的协方差矩阵。
上述模型是一种将视觉定位数据和IUM定位数据综合利用的紧耦合模型，其优势在于对位姿的状态变量的估算结果比较准确，从而能够得到比较准确的第一位姿数据。
一般地，
Figure 909389DEST_PATH_IMAGE054
代表当前时刻采集到的图像对应的状态估计数据（包含粗略位姿数据以及IMU偏置），优化后的
Figure 184381DEST_PATH_IMAGE002
中的
Figure 389098DEST_PATH_IMAGE054
所包括的被定位对象的位姿即为第一位姿数据；从而从状态变量的最优估计值中提取第一位姿数据的步骤包括：从状态变量的最优估计值中提取优化后的
Figure 915894DEST_PATH_IMAGE054
中的被定位对象的位姿，得到第一位姿数据。
其中，联合IMU模块和GNSS模块进行定位是成熟的现有技术，步骤A2中，可采用现有的GNSS/IMU融合算法来获取被定位对象的第二位姿数据。例如，在一些实施方式中，步骤A2包括：
采用基于卡尔曼滤波的GNSS/IMU融合算法获取被定位对象的第二位姿数据。
其中，基于卡尔曼滤波的GNSS/IMU融合算法为现有技术，此处不对其进行详述。在实际应用中，不限于采用基于卡尔曼滤波的GNSS/IMU融合算法获取第二位姿数据。
在一些实施方式中，步骤A3包括：
获取卫星定位系统的各卫星测得的被定位对象的第三位姿数据；
根据第三位姿数据计算伪距残差平方和；
根据伪距残差平方和计算最小二乘残差统计量和检测门限值；
对比最小二乘残差统计量和检测门限值以判断GNSS信号的质量好坏；
根据GNSS信号的质量好坏的判断结果，确定权重值。
一般地，卫星定位系统包括多颗卫星，各颗卫星分别可测得被定位对象的第三位姿数据，有时候，部分卫星可能存在故障或部分卫星的信号无法被被定位对象接收到，此处，获取卫星定位系统的各卫星测得的被定位对象的第三位姿数据是指获取能够接收到其信号的卫星测得的第三位姿数据。
实际上，GNSS伪距差（即实际伪距与GNSS模块测得的测量伪距值的差）与被定位对象的单位位姿误差之间的存在以下线性关系：
Figure 774653DEST_PATH_IMAGE055
（1）；
Figure 236858DEST_PATH_IMAGE056
；
其中，
Figure 443718DEST_PATH_IMAGE057
为GNSS伪距差，
Figure 118413DEST_PATH_IMAGE058
为被定位对象的单位位姿误差，
Figure 969694DEST_PATH_IMAGE059
为观测矩阵，
Figure 586489DEST_PATH_IMAGE060
分别为第1颗到第m颗卫星自身位置数据，
Figure 765798DEST_PATH_IMAGE061
、
Figure 761960DEST_PATH_IMAGE062
、
Figure 343114DEST_PATH_IMAGE063
分别为接收机位置，
Figure DEST_PATH_IMAGE064
为伪距，
Figure 68493DEST_PATH_IMAGE065
为服从零均值正态分布的伪距噪声。
根据最小二乘原理，
Figure 984366DEST_PATH_IMAGE058
最优估计解为：
Figure DEST_PATH_IMAGE066
（2）；
Figure 594339DEST_PATH_IMAGE067
为
Figure 536057DEST_PATH_IMAGE058
的最优估计解，
Figure 635600DEST_PATH_IMAGE068
为
Figure 55080DEST_PATH_IMAGE059
的转置矩阵；联立公式（1）和（2）有：
Figure 124536DEST_PATH_IMAGE069
；
则预测的GNSS伪距差为：
Figure 680282DEST_PATH_IMAGE070
；
Figure 809781DEST_PATH_IMAGE071
为预测的GNSS伪距差；
因此，可得伪距残差为：
Figure 450978DEST_PATH_IMAGE072
Figure 202421DEST_PATH_IMAGE073
为伪距残差，
Figure 861941DEST_PATH_IMAGE074
为单位矩阵。
可根据以下公式计算伪距残差平方和：
Figure 647495DEST_PATH_IMAGE075
；
其中，
Figure 25255DEST_PATH_IMAGE076
为伪距残差平方和，
Figure 187246DEST_PATH_IMAGE077
为
Figure 966852DEST_PATH_IMAGE073
的转置矩阵。
其中，
Figure 454466DEST_PATH_IMAGE073
服从均值为0，方差为
Figure 56873DEST_PATH_IMAGE078
的正态分布，从而
Figure 756975DEST_PATH_IMAGE079
服从自由度为
Figure 125509DEST_PATH_IMAGE080
的卡方分布，即
Figure 518444DEST_PATH_IMAGE081
。
在系统预设误警率
Figure 339638DEST_PATH_IMAGE082
（可根据实际需要设置）的情况下，根据卡方分布概率密度函数
Figure 843432DEST_PATH_IMAGE083
可得：
Figure 800893DEST_PATH_IMAGE084
（3）；
其中，
Figure 99150DEST_PATH_IMAGE085
为正确告警率，
Figure 676149DEST_PATH_IMAGE086
为伪距残差的标准差，
Figure 577109DEST_PATH_IMAGE087
为与
Figure 670967DEST_PATH_IMAGE086
对应的观测矩阵，
Figure 389393DEST_PATH_IMAGE088
为等效检测门限值。求解上述公式（3）可得到
Figure 670333DEST_PATH_IMAGE088
的值。
其中，可根据以下公式计算最小二乘残差统计量：
Figure 30776DEST_PATH_IMAGE089
；
Figure 510299DEST_PATH_IMAGE090
为最小二乘残差统计量。
其中，可根据以下公式计算检测门限值：
Figure 665206DEST_PATH_IMAGE091
；
Figure 433441DEST_PATH_IMAGE092
为检测门限值。
优选地，对比最小二乘残差统计量和检测门限值以判断GNSS信号的质量好坏的步骤包括：
若
Figure 815486DEST_PATH_IMAGE093
，则判定GNSS信号的质量不好；
若
Figure 883936DEST_PATH_IMAGE029
，则判定GNSS信号的质量好；
若
Figure 209744DEST_PATH_IMAGE094
，则判定GNSS信号的质量中等；
根据GNSS信号的质量好坏的判断结果，确定权重值的步骤包括：
若GNSS信号的质量不好，则
Figure 465276DEST_PATH_IMAGE095
；
若GNSS信号的质量好，则
Figure 901943DEST_PATH_IMAGE096
；
若GNSS信号的质量中等，则
Figure 824899DEST_PATH_IMAGE036
；
Figure 931396DEST_PATH_IMAGE037
为权重值。
进一步地，步骤A4包括：
根据以下公式计算被定位对象的位姿数据测量值：
Figure 923491DEST_PATH_IMAGE038
；
其中，
Figure 914581DEST_PATH_IMAGE097
为被定位对象的位姿数据测量值，
Figure 678662DEST_PATH_IMAGE098
为第一位姿数据，
Figure 97005DEST_PATH_IMAGE099
为第二位姿数据。
即，当GNSS信号的质量不好的时候，以第一位姿数据作为最终的位姿定位结果，当GNSS信号的质量好的时候，以第二位姿数据作为最终的位姿定位结果，当GNSS信号的质量中等（此时难以判断GNSS信号的质量好坏），则以第一位姿数据和第二位姿数据的加权和作为最终的位姿定位结果。通过上述方式判断GNSS信号的质量好坏，可靠性好。
由上可知，该融合定位方法，通过联合视觉相机和IMU模块获取被定位对象的第一位姿数据；联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；根据GNSS信号的质量确定权重值；根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值；从而实现了视觉定位技术、GNSS定位技术和IMU定位技术的有机融合，可提高在对GNSS信号具有长时间遮挡作用的复杂环境中的定位精度。
参考图2，本申请提供了一种融合定位装置，应用于具有视觉相机、IMU模块和GNSS模块的定位系统，对被定位对象进行定位，包括：
第一获取模块1，用于联合视觉相机和IMU模块获取被定位对象的第一位姿数据；
第二获取模块2，用于联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；
第一计算模块3，用于根据GNSS信号的质量确定权重值；
第二计算模块4，用于根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值。
其中，被定位对象是安装有该定位系统且可移动的设备，例如移动机器人、车辆、无人机、船舶等。
该融合定位装置，先分别结合视觉相机和IMU模块得到第一位姿数据、结合IMU模块和GNSS模块得到第二位姿数据，再根据GNSS信号的质量好坏确定第一位姿数据和第二位姿数据融合时的权重值，最后对第一位姿数据和第二位姿数据进行加权融合计算得到最终的位姿数据，在GNSS信号被长时间遮挡时，依然能够根据视觉相机和IMU模块得到比较准确的定位结果，与现有技术中的GNSS/IMU融合定位技术相比，在对GNSS信号具有长时间遮挡作用的复杂环境中，具有更高的定位精度。
在本实施例中，第一获取模块1用于在联合视觉相机和IMU模块获取被定位对象的第一位姿数据的时候，执行：
获取视觉相机采集的多帧图像；
根据图像提取相邻两帧图像的匹配特征点的像素坐标；
根据匹配特征点的像素坐标进行对极几何求解，计算出被定位对象的粗略位姿数据；
获取IMU模块的测量数据的并进行预积分处理得到对应的预积分数据；测量数据包括加速度计偏置和陀螺仪偏置；
根据匹配特征点的像素坐标和预积分数据通过三角法进行深度估计，得到匹配特征点的深度数据；
根据粗略位姿数据、深度数据、IMU模块的测量数据、预积分数据、IMU模块的外参和视觉相机的外参生成状态变量，并对状态变量进行非线性优化处理，得到状态变量的最优估计值；
从状态变量的最优估计值中提取第一位姿数据。
通过采用预积分数据对匹配特征点进行深度估计，可以有效避免视觉定位在初始化时存在的尺度不确定和钝化问题，通过对状态变量进行非线性优化处理进而得到第一位姿数据，可提高第一位姿数据的精度。
其中，特征点为图像像素值发生剧烈变化的点，匹配特征点是两个图像中相互匹配的特征点。可采用现有的图像识别方法来从图像中提取匹配特征点的位置数据，此处不对其具体方法进行限定。
其中，极几何求解过程为现有技术，此处不对其进行详述。其中，求解过程中，深度做了归一化处理，因此，粗略位姿数据存在尺度不确定性问题，不能直接作为第一位姿数据使用。
其中，第一获取模块1在根据特征点的位置数据和预积分数据对特征点进行深度估计，得到特征点的深度数据的时候，具体执行：首先对IMU数据（即IMU模块的测量数据）进行预积分可以计算出两帧图像之间的相对距离和相对姿态，再通过三角法可以得到匹配特征点与成像平面的距离。
在本实施例中，状态估计量包括被定位对象的位姿和速度、加速度计偏置和陀螺仪偏置；
在本实施例中，状态变量为：
Figure 310818DEST_PATH_IMAGE042
；
其中，
Figure 371178DEST_PATH_IMAGE002
为状态变量，
Figure 986836DEST_PATH_IMAGE003
分别为第1至第n帧图像对应的状态估计数据（进行状态变量的生成之前，需要先对视觉相机采集的图像和IMU模块采集的的测量数据进行时间同步处理，具体的时间同步处理过程为现有技术，此处不对其进行详述；其中，此处的第1至第n帧图像是滑动窗口内的图像，滑动窗口越大表示n越大则约束条件越多，计算结果越准确，但计算量越大，n的大小可根据实际需要设置，一般地窗口大小范围为10-20，即n一般为10-20），对于第k帧图像有：
Figure 435135DEST_PATH_IMAGE004
；
Figure 418134DEST_PATH_IMAGE005
为与第k帧图像对应的状态估计数据，
Figure 294DEST_PATH_IMAGE006
为第k帧图像对应的粗略位姿数据中的位置（一般地，第k帧图像对应的粗略位姿数据为通过第k-1帧图像和第k帧图像计算得到的粗略位姿数据，对于第1帧图像，可通过上一个窗口中的最后一帧图像与该第1帧图像计算得到对应的粗略位姿数据），
Figure 221191DEST_PATH_IMAGE007
为第k帧图像对应的粗略位姿数据中的姿态，
Figure 233534DEST_PATH_IMAGE043
为第k帧图像对应的速度（通过对加速度计偏置进行预积分得到，即预积分数据包括速度），
Figure 438250DEST_PATH_IMAGE009
为第k帧图像对应的加速度计偏置，
Figure 292943DEST_PATH_IMAGE044
为第k帧图像对应的陀螺仪偏置；
其中，
Figure 758559DEST_PATH_IMAGE011
；
Figure 345398DEST_PATH_IMAGE012
为外参矩阵，
Figure 365307DEST_PATH_IMAGE013
为视觉相机的外参，
Figure 305581DEST_PATH_IMAGE045
为IMU模块的外参；
Figure 750338DEST_PATH_IMAGE046
为第1至第n帧图像对应的深度数据；
第一获取模块1在对状态变量进行非线性优化处理，得到状态变量的最优估计值的时候，执行：
根据以下模型进行最大后验估计（可采用现有的最大后验估计方法进行计算，此处不对其进行限定），得到状态变量的最优估计值：
Figure 852286DEST_PATH_IMAGE016
；
其中，
Figure 283792DEST_PATH_IMAGE047
为边缘化残差，
Figure 621232DEST_PATH_IMAGE018
为与协方差相关的马氏距离，
Figure 795861DEST_PATH_IMAGE019
为雅可比矩阵；
其中，
Figure 317979DEST_PATH_IMAGE048
为IMU残差，
Figure 719004DEST_PATH_IMAGE049
为IMU残差函数，
Figure 250348DEST_PATH_IMAGE050
为IMU测量值，
Figure 686009DEST_PATH_IMAGE023
为IMU预积分噪声项的协方差矩阵；
其中，
Figure 104659DEST_PATH_IMAGE051
为视觉重投影误差，
Figure 258560DEST_PATH_IMAGE052
为视觉重投影误差函数，
Figure 62437DEST_PATH_IMAGE053
为视觉特征点，
Figure 618183DEST_PATH_IMAGE027
为视觉观测噪声的协方差矩阵。
上述模型是一种将视觉定位数据和IUM定位数据综合利用的紧耦合模型，其优势在于对位姿的状态变量的估算结果比较准确，从而能够得到比较准确的第一位姿数据。
一般地，
Figure 482103DEST_PATH_IMAGE054
代表当前时刻采集到的图像对应的状态估计数据（包含粗略位姿数据以及IMU偏置），优化后的
Figure 388879DEST_PATH_IMAGE002
中的
Figure 402971DEST_PATH_IMAGE054
所包括的被定位对象的位姿即为第一位姿数据；从而第一获取模块1在从状态变量的最优估计值中提取第一位姿数据的时候，执行：从状态变量的最优估计值中提取优化后的
Figure 406699DEST_PATH_IMAGE054
中的被定位对象的位姿，得到第一位姿数据。
其中，联合IMU模块和GNSS模块进行定位是成熟的现有技术，第二获取模块2可采用现有的GNSS/IMU融合算法来获取被定位对象的第二位姿数据。例如，在一些实施方式中，第二获取模块2用于在联合IMU模块和GNSS模块获取被定位对象的第二位姿数据的时候，执行：
采用基于卡尔曼滤波的GNSS/IMU融合算法获取被定位对象的第二位姿数据。
其中，基于卡尔曼滤波的GNSS/IMU融合算法为现有技术，此处不对其进行详述。在实际应用中，不限于采用基于卡尔曼滤波的GNSS/IMU融合算法获取第二位姿数据。
在一些实施方式中，第一计算模块3用于在根据GNSS信号的质量确定权重值的时候，执行：
获取卫星定位系统的各卫星测得的被定位对象的第三位姿数据；
根据第三位姿数据计算伪距残差平方和；
根据伪距残差平方和计算最小二乘残差统计量和检测门限值；
对比最小二乘残差统计量和检测门限值以判断GNSS信号的质量好坏；
根据GNSS信号的质量好坏的判断结果，确定权重值。
一般地，卫星定位系统包括多颗卫星，各颗卫星分别可测得被定位对象的第三位姿数据，有时候，部分卫星可能存在故障或部分卫星的信号无法被被定位对象接收到，此处，获取卫星定位系统的各卫星测得的被定位对象的第三位姿数据是指获取能够接收到其信号的卫星测得的第三位姿数据。
实际上，GNSS伪距差（即实际伪距与GNSS模块测得的测量伪距值的差）与被定位对象的单位位姿误差之间的存在以下线性关系：
Figure 710029DEST_PATH_IMAGE055
（1）；
Figure 572943DEST_PATH_IMAGE056
；
其中，
Figure 718622DEST_PATH_IMAGE057
为GNSS伪距差，
Figure 248961DEST_PATH_IMAGE058
为被定位对象的单位位姿误差，
Figure 454683DEST_PATH_IMAGE059
为观测矩阵，
Figure 539314DEST_PATH_IMAGE060
分别为第1颗到第m颗卫星自身位置数据，
Figure 754263DEST_PATH_IMAGE061
、
Figure 873529DEST_PATH_IMAGE062
、
Figure 253082DEST_PATH_IMAGE063
分别为接收机位置，
Figure 90588DEST_PATH_IMAGE064
为伪距，
Figure 578070DEST_PATH_IMAGE065
为服从零均值正态分布的伪距噪声。
根据最小二乘原理，
Figure 551842DEST_PATH_IMAGE058
最优估计解为：
Figure 364946DEST_PATH_IMAGE066
（2）；
Figure 17645DEST_PATH_IMAGE067
为
Figure 325129DEST_PATH_IMAGE058
的最优估计解，
Figure 668255DEST_PATH_IMAGE068
为
Figure 137413DEST_PATH_IMAGE059
的转置矩阵；联立公式（1）和（2）有：
Figure 936130DEST_PATH_IMAGE069
；
则预测的GNSS伪距差为：
Figure 516147DEST_PATH_IMAGE070
；
Figure 323566DEST_PATH_IMAGE071
为预测的GNSS伪距差；
因此，可得伪距残差为：
Figure 478472DEST_PATH_IMAGE072
Figure 246708DEST_PATH_IMAGE073
为伪距残差。
可根据以下公式计算伪距残差平方和：
Figure 145263DEST_PATH_IMAGE075
；
其中，
Figure 213713DEST_PATH_IMAGE076
为伪距残差平方和，
Figure 273942DEST_PATH_IMAGE077
为
Figure 263895DEST_PATH_IMAGE073
的转置矩阵。
其中，
Figure 793289DEST_PATH_IMAGE073
服从均值为0，方差为
Figure 716245DEST_PATH_IMAGE078
的正态分布，从而
Figure 822742DEST_PATH_IMAGE079
服从自由度为
Figure 955783DEST_PATH_IMAGE080
的卡方分布，即
Figure 540348DEST_PATH_IMAGE081
。
在系统预设误警率
Figure 567079DEST_PATH_IMAGE082
（可根据实际需要设置）的情况下，根据卡方分布概率密度函数
Figure 719842DEST_PATH_IMAGE083
可得：
Figure 464813DEST_PATH_IMAGE084
（3）；
其中，
Figure 728436DEST_PATH_IMAGE085
为正确告警概率，
Figure 878182DEST_PATH_IMAGE086
为伪距残差的标准差，
Figure 467426DEST_PATH_IMAGE087
为与
Figure 43901DEST_PATH_IMAGE086
对应的观测矩阵，
Figure 767006DEST_PATH_IMAGE088
为等效检测门限值。求解上述公式（3）可得到
Figure 112537DEST_PATH_IMAGE088
的值。
其中，可根据以下公式计算最小二乘残差统计量：
Figure 121950DEST_PATH_IMAGE089
；
Figure 326666DEST_PATH_IMAGE090
为最小二乘残差统计量。
其中，可根据以下公式计算检测门限值：
Figure 446938DEST_PATH_IMAGE091
；
Figure 646975DEST_PATH_IMAGE092
为检测门限值。
优选地，第一计算模块3在对比最小二乘残差统计量和检测门限值以判断GNSS信号的质量好坏的时候，执行：
若
Figure 578022DEST_PATH_IMAGE093
，则判定GNSS信号的质量不好；
若
Figure 663177DEST_PATH_IMAGE029
，则判定GNSS信号的质量好；
若
Figure 462506DEST_PATH_IMAGE094
，则判定GNSS信号的质量中等；
第一计算模块3在根据GNSS信号的质量好坏的判断结果，确定权重值的时候，执行：
若GNSS信号的质量不好，则
Figure 907263DEST_PATH_IMAGE095
；
若GNSS信号的质量好，则
Figure 9211DEST_PATH_IMAGE096
；
若GNSS信号的质量中等，则
Figure 172208DEST_PATH_IMAGE036
；
Figure 650594DEST_PATH_IMAGE037
为权重值。
进一步地，第二计算模块4用于在根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值的时候，执行：
根据以下公式计算被定位对象的位姿数据测量值：
Figure 215436DEST_PATH_IMAGE038
；
其中，
Figure 753865DEST_PATH_IMAGE097
为被定位对象的位姿数据测量值，
Figure 407088DEST_PATH_IMAGE098
为第一位姿数据，
Figure 282640DEST_PATH_IMAGE099
为第二位姿数据。
即，当GNSS信号的质量不好的时候，以第一位姿数据作为最终的位姿定位结果，当GNSS信号的质量好的时候，以第二位姿数据作为最终的位姿定位结果，当GNSS信号的质量中等（此时难以判断GNSS信号的质量好坏），则以第一位姿数据和第二位姿数据的加权和作为最终的位姿定位结果。通过上述方式判断GNSS信号的质量好坏，可靠性好。
由上可知，该融合定位装置，通过联合视觉相机和IMU模块获取被定位对象的第一位姿数据；联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；根据GNSS信号的质量确定权重值；根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值；从而实现了视觉定位技术、GNSS定位技术和IMU定位技术的有机融合，可提高在对GNSS信号具有长时间遮挡作用的复杂环境中的定位精度。
请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当计算设备运行时，处理器301执行该计算机程序，以执行时执行上述实施例的任一可选的实现方式中的融合定位方法，以实现以下功能：联合视觉相机和IMU模块获取被定位对象的第一位姿数据；联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；根据GNSS信号的质量确定权重值；根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值。
本申请实施例提供一种存储介质，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的融合定位方法，以实现以下功能：联合视觉相机和IMU模块获取被定位对象的第一位姿数据；联合IMU模块和GNSS模块获取被定位对象的第二位姿数据；根据GNSS信号的质量确定权重值；根据权重值对第一位姿数据和第二位姿数据进行加权融合计算，得到被定位对象的位姿数据测量值。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random AccessMemory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable ProgrammableRead-Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable ProgrammableRead Only Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory,简称PROM），只读存储器（Read-Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。
在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
另外，作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。
以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种融合定位方法，应用于具有视觉相机、IMU模块和GNSS模块的定位系统，对被定位对象进行定位，其特征在于，包括以下步骤：
A1.联合所述视觉相机和所述IMU模块获取所述被定位对象的第一位姿数据；
A2.联合所述IMU模块和所述GNSS模块获取所述被定位对象的第二位姿数据；
A3.根据GNSS信号的质量确定权重值；
A4.根据所述权重值对所述第一位姿数据和所述第二位姿数据进行加权融合计算，得到所述被定位对象的位姿数据测量值。
2.根据权利要求1所述的融合定位方法，其特征在于，步骤A1包括：
获取所述视觉相机采集的多帧图像；
根据所述图像提取相邻两帧图像的匹配特征点的像素坐标；
根据所述匹配特征点的像素坐标进行对极几何求解，计算出所述被定位对象的粗略位姿数据；
获取所述IMU模块的测量数据的并进行预积分处理得到对应的预积分数据；所述测量数据包括加速度计偏置和陀螺仪偏置；
根据所述匹配特征点的像素坐标和所述预积分数据通过三角法进行深度估计，得到所述匹配特征点的深度数据；
根据所述粗略位姿数据、所述深度数据、所述IMU模块的测量数据、所述预积分数据、所述IMU模块的外参和所述视觉相机的外参生成状态变量，并对所述状态变量进行非线性优化处理，得到所述状态变量的最优估计值；
从所述状态变量的最优估计值中提取所述第一位姿数据。
3.根据权利要求2所述的融合定位方法，其特征在于，所述状态变量为：
Figure DEST_PATH_IMAGE001
；
其中，
Figure DEST_PATH_IMAGE002
为所述状态变量，
Figure DEST_PATH_IMAGE003
分别为第1至第n帧图像对应的状态估计数据，对于第k帧图像有：
Figure DEST_PATH_IMAGE004
；
Figure DEST_PATH_IMAGE005
为与第k帧图像对应的状态估计数据，
Figure DEST_PATH_IMAGE006
为第k帧图像对应的所述粗略位姿数据中的位置，
Figure DEST_PATH_IMAGE007
为第k帧图像对应的所述粗略位姿数据中的姿态，
Figure DEST_PATH_IMAGE008
为第k帧图像对应的速度，
Figure DEST_PATH_IMAGE009
为第k帧图像对应的所述加速度计偏置，
Figure DEST_PATH_IMAGE010
为第k帧图像对应的所述陀螺仪偏置；
其中，
Figure DEST_PATH_IMAGE011
；
Figure DEST_PATH_IMAGE012
为外参矩阵，
Figure DEST_PATH_IMAGE013
为所述视觉相机的外参，
Figure DEST_PATH_IMAGE014
为所述IMU模块的外参；
Figure DEST_PATH_IMAGE015
为第1至第n帧图像对应的所述深度数据；
所述对所述状态变量进行非线性优化处理，得到所述状态变量的最优估计值的步骤包括：
根据以下模型进行最大后验估计，得到所述状态变量的最优估计值：
Figure DEST_PATH_IMAGE016
；
其中，
Figure DEST_PATH_IMAGE017
为边缘化残差，
Figure DEST_PATH_IMAGE018
为与协方差相关的马氏距离，
Figure DEST_PATH_IMAGE019
为雅可比矩阵；
其中，
Figure DEST_PATH_IMAGE020
为IMU残差，
Figure DEST_PATH_IMAGE021
为IMU残差函数，
Figure DEST_PATH_IMAGE022
为IMU测量值，
Figure DEST_PATH_IMAGE023
为IMU预积分噪声项的协方差矩阵；
其中，
Figure DEST_PATH_IMAGE024
为视觉重投影误差，
Figure DEST_PATH_IMAGE025
为视觉重投影误差函数，
Figure DEST_PATH_IMAGE026
为视觉特征点，
Figure DEST_PATH_IMAGE027
为视觉观测噪声的协方差矩阵。
4.根据权利要求1所述的融合定位方法，其特征在于，步骤A2包括：
采用基于卡尔曼滤波的GNSS/IMU融合算法获取所述被定位对象的第二位姿数据。
5.根据权利要求1所述的融合定位方法，其特征在于，步骤A3包括：
获取卫星定位系统的各卫星测得的所述被定位对象的第三位姿数据；
根据所述第三位姿数据计算伪距残差平方和；
根据所述伪距残差平方和计算最小二乘残差统计量和检测门限值；
对比所述最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏；
根据GNSS信号的质量好坏的判断结果，确定所述权重值。
6.根据权利要求5所述的融合定位方法，其特征在于，所述对比所述最小二乘残差统计量和所述检测门限值以判断GNSS信号的质量好坏的步骤包括：
若
Figure DEST_PATH_IMAGE028
，则判定所述GNSS信号的质量不好；
若
Figure DEST_PATH_IMAGE029
，则判定所述GNSS信号的质量好；
若
Figure DEST_PATH_IMAGE030
，则判定所述GNSS信号的质量中等；
其中，
Figure DEST_PATH_IMAGE031
为所述最小二乘残差统计量，
Figure DEST_PATH_IMAGE032
为所述检测门限值，
Figure DEST_PATH_IMAGE033
为伪距残差的标准差；
所述根据GNSS信号的质量好坏的判断结果，确定所述权重值的步骤包括：
若所述GNSS信号的质量不好，则
Figure DEST_PATH_IMAGE034
；
若所述GNSS信号的质量好，则
Figure DEST_PATH_IMAGE035
；
若所述GNSS信号的质量中等，则
Figure DEST_PATH_IMAGE036
；
Figure DEST_PATH_IMAGE037
为所述权重值。
7.根据权利要求6所述的融合定位方法，其特征在于，步骤A4包括：
根据以下公式计算所述被定位对象的位姿数据测量值：
Figure DEST_PATH_IMAGE038
；
其中，
Figure DEST_PATH_IMAGE039
为所述被定位对象的位姿数据测量值，
Figure DEST_PATH_IMAGE040
为所述第一位姿数据，
Figure DEST_PATH_IMAGE041
为所述第二位姿数据。
8.一种融合定位装置，应用于具有视觉相机、IMU模块和GNSS模块的定位系统，对被定位对象进行定位，其特征在于，包括：
第一获取模块，用于联合所述视觉相机和所述IMU模块获取所述被定位对象的第一位姿数据；
第二获取模块，用于联合所述IMU模块和所述GNSS模块获取所述被定位对象的第二位姿数据；
第一计算模块，用于根据GNSS信号的质量确定权重值；
第二计算模块，用于根据所述权重值对所述第一位姿数据和所述第二位姿数据进行加权融合计算，得到所述被定位对象的位姿数据测量值。
9.一种电子设备，其特征在于，包括处理器以及存储器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如权利要求1-7任一项所述融合定位方法中的步骤。
10.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1-7任一项所述融合定位方法中的步骤。
CN202210662635.2A 2022-06-13 2022-06-13 融合定位方法、装置、电子设备及存储介质 Pending CN115096309A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210662635.2A CN115096309A (zh) 2022-06-13 2022-06-13 融合定位方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210662635.2A CN115096309A (zh) 2022-06-13 2022-06-13 融合定位方法、装置、电子设备及存储介质

Publications (1)

Publication Number Publication Date
CN115096309A true CN115096309A (zh) 2022-09-23

Family

ID=83291083

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210662635.2A Pending CN115096309A (zh) 2022-06-13 2022-06-13 融合定位方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN115096309A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117473455A (zh) * 2023-12-27 2024-01-30 合众新能源汽车股份有限公司 多源定位数据的融合方法、装置和电子设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117473455A (zh) * 2023-12-27 2024-01-30 合众新能源汽车股份有限公司 多源定位数据的融合方法、装置和电子设备
CN117473455B (zh) * 2023-12-27 2024-03-29 合众新能源汽车股份有限公司 多源定位数据的融合方法、装置和电子设备

Similar Documents

Publication Publication Date Title
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
CN109887057B (zh) 生成高精度地图的方法和装置
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
US7769543B2 (en) Fault detection and reconfiguration of an automated refueling boom
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN103454650A (zh) 一种视觉辅助卫星完好性监测方法
CN111890373A (zh) 车载机械臂的感知定位方法
CN114396943A (zh) 一种融合定位方法与终端
CN111024067B (zh) 一种信息处理方法、装置、设备及计算机存储介质
CN115371665A (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN114459506B (zh) 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
CN115096309A (zh) 融合定位方法、装置、电子设备及存储介质
CN113436261B (zh) 一种面向封闭园区自动驾驶的单目视觉惯性定位方法
US11662472B2 (en) Integrity monitoring of odometry measurements within a navigation system
Chu et al. Performance comparison of tight and loose INS-Camera integration
CN114355393A (zh) 一种基于低成本接收机的三天线姿态估计方法
CN115291253B (zh) 一种基于残差检测的车辆定位完好性监测方法及系统
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
Ćwian et al. GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments
Ren et al. Adaptive sensor fusion of camera, GNSS and IMU for autonomous driving navigation
CN115930948A (zh) 一种果园机器人融合定位方法
Liu et al. 3D LiDAR aided GNSS NLOS mitigation for reliable GNSS-RTK positioning in urban canyons
EP3901650A1 (en) Integrity monitoring of odometry measurements within a navigation system
Panahandeh et al. IMU-camera data fusion: Horizontal plane observation with explicit outlier rejection

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination