CN114660641A - 一种自适应gps融合定位系统、方法及介质 - Google Patents

一种自适应gps融合定位系统、方法及介质 Download PDF

Info

Publication number
CN114660641A
CN114660641A CN202210189088.0A CN202210189088A CN114660641A CN 114660641 A CN114660641 A CN 114660641A CN 202210189088 A CN202210189088 A CN 202210189088A CN 114660641 A CN114660641 A CN 114660641A
Authority
CN
China
Prior art keywords
pose
image
area
estimation
fusion
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
CN202210189088.0A
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.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
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 South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202210189088.0A priority Critical patent/CN114660641A/zh
Publication of CN114660641A publication Critical patent/CN114660641A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • 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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G01C21/1656—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 combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种自适应GPS融合定位系统、方法及介质，包括GNSS/INS信息采集模块、视觉信息处理模块及自适应融合模块；所述GNSS/INS信息采集模块包括全球卫星导航系统和惯性导航系统；所述视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；所述自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；本发明不仅可以去除视觉位姿估计中对天空区域的误匹配，而且可以使得移动机器人在处于城市峡谷状态下时可以获得更好的位姿估计。

Description

一种自适应GPS融合定位系统、方法及介质
技术领域
本发明涉及数据融合定位领域，具体涉及一种自适应GPS融合定位系统、方法及介质。
背景技术
在多传感器信息融合系统中，各个传感器会对不同参数进行测量，然而得到的测量值不仅会受传感器本身制造结构以及信息采集方法的影响，而且会受到环境因素的影响。GPS是一个易受到环境影响的传感器，其在室内无法接收到卫星信号从而无法对当前位置进行计算，而其处于室外时仍然会有信号被遮挡或被干扰的可能性出现，这种情况往往会出现在城市峡谷的环境中，由于城市中的高楼会对GPS的接收信号进行遮挡，致使卫星的跟踪精度差；相机是一个相对比较稳定的传感器，但其定位精度会受到视觉算法本身的一些错误影响，其中特征点误匹配就是一个重要的影响因素，实际应用过程中天空区域是一个较容易出现误匹配的区域。
发明内容
为了克服现有技术中存在的缺点与不足，本发明提供一种自适应GPS融合定位系统、方法及介质。
本发明克服两个现有技术存在的问题，一个是在不同环境下GPS所得数据精度不同对整个多传感器信息融合系统精度的影响，另一个是视觉位姿估计算法中对特定区域进行错误匹配问题，提供了一种鲁棒性强的自适应GPS数据融合方法，不仅可以去除视觉位姿估计中对天空区域的误匹配，而且可以使得移动机器人在处于城市峡谷状态下时可以获得更好的位姿估计。
本发明采用如下技术方案：
一种自适应GPS融合定位系统，包括：
GNSS/INS信息采集模块：包括全球卫星导航系统和惯性导航系统；
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
上述模块均设置在同一载体上。
进一步，所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
进一步，所述图像信息采集单元平行于地面安装，且安装高度应大于60cm。
进一步，所述图像信息采集单元包括双目相机。
一种基于自适应GPS融合定位系统的融合定位方法，包括：
获得载体在世界坐标系下的运动坐标及惯性导航系统获得载体运动学信息；
实时采集运动载体前方区域的多帧图像，并记录每一帧图像的序列号；
将图像按照序列号顺序进行语义分割，将分割后的天空区域存储于MASK矩阵中；不存在天空区域，则矩阵存为空白RGB矩阵；
对MASK矩阵进行膨胀处理，计算天空区域的图像面积，及其在原始图像面积的区域占比；
依据图像序列号对双目图像进行特征点匹配，获得匹配点；
将匹配点与前一帧图像的特征点以及地图特征点进行匹配，计算得到一个图像帧的位姿变换矩阵，进一步通过对前n帧的位姿变换矩阵进行累乘得到第n帧图像的位姿数据；
通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿。
进一步，所述通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿，具体为：
对载体的初始速度和位姿进行初始化；
定义十二维的速度状态量
Figure BDA0003523906390000021
和位姿状态量
Figure BDA0003523906390000022
根据GNSS/INS信息采集模块中陀螺仪与加速度计构成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，得到K时刻的最优速度状态估计值
Figure BDA0003523906390000023
根据K时刻的最优速度状态估计值、视觉位姿估计值、GPS位置测量值对位姿状态量进行测量更新，得到k时刻的位姿状态观测量
Figure BDA0003523906390000024
通过k时刻的先验位姿状态量
Figure BDA0003523906390000025
以及k时刻的位姿状态观测量
Figure BDA0003523906390000026
对K时刻的位姿进行最优估计。
进一步，所述通过k时刻的先验位姿状态量
Figure BDA0003523906390000031
以及k时刻的位姿状态观测量
Figure BDA0003523906390000032
对K时刻的位姿进行最优估计，具体为：
通过如下公式计算得到K时刻的位姿最优估计值：
Figure BDA0003523906390000033
其中
Figure BDA0003523906390000034
为卡尔曼增益
Figure BDA0003523906390000035
其中，
Figure BDA0003523906390000036
其中
Figure BDA0003523906390000037
为测量误差协方差矩阵，其表示为：
Figure BDA0003523906390000038
r2～r4为经过调参后得到的常值，在相机所得位姿的测量误差协方差矩阵中r1为常值，在GPS所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rD，rD由天空区域占比Ra决定。
进一步，所述rD由天空区域占比Ra决定，具体为：
根据天空区域占比Ra以及GNSS/INS信息采集模块所捕获到的卫星数目x对动态参数rD进行加权取值得到动态参数rD；
Figure BDA0003523906390000039
其中λ为Ra的加权系数。
进一步，所述依据图像序列号对双目图像进行特征点匹配，获得匹配点，具体为：
按照图像序列号对左右双目图像进行灰度处理，并对双目图像帧进行特征点提取，将左右图像中的特征点进行匹配得到双目图像之间的视差，通过视差计算匹配点的深度信息，并使用SAD算法对匹配误差大的点进行剔除；
对上一步骤得到匹配点进行遍历，判断匹配点是否处于膨胀处理后的MASK矩阵上，若处于，则对该匹配点对应的深度值Depth进行判断，若小于设定的深度阈值bf，bf＝baseline×fx，则认为有误匹配，将该匹配点去除。
一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的融合定位方法。
本发明的有益效果：
本发明的视觉位姿估计算法部分在保留天空区域内大于深度阈值的匹配点的情况下消除了天空区域的误匹配的特征点对位姿估计的影响，其次通过对天空区域在图像中的占比情况动态调整GPS数据在卡尔曼滤波的融合过程中的估计权重，使得最终滤波出的估计位姿更为准确。
附图说明
图1是本发明的结构示意图；
图2是本发明天空区域特征点匹配示意图；
图3是本发明语义分割效果图。
具体实施方式
下面结合实施例及附图，对本发明作进一步地详细说明，但本发明的实施方式不限于此。
实施例1
如图1所示，一种自适应GPS融合定位方法，硬件系统包括三个模块，分别是GNSS/INS信息采集模块、视觉信息处理模块及自适应融合模块，上述所有模块位于同一载体上，所述自适应融合模块通过滤波算法融合处理上述两个模块的数据，计算得到载体的最优位姿结果。
所述GNSS/INS信息采集模块，本实施例使用xsense MTi-700GPS/INS模块进行数据采集。
进一步，包括全球卫星导航系统和惯性导航系统，其中全球卫星导航系统通过GNSS接收机捕获并跟踪卫星获得导航解，亦即接收器所在经度和纬度数据，其中惯性导航系统通过三轴加速度计、三轴陀螺仪、三轴磁罗盘获得载体的运动学信息。
具体可以获得如下信号：
GNSS接收器捕获并跟踪卫星时可能有信号遮挡或受干扰的情况，比如在城市峡谷中，城市中的高楼会遮挡信号，致使卫星无法被跟踪或跟踪精度差。GNSS接收器得到的经度和纬度数据为WGS84(地球坐标系)下的坐标，其可转换为世界坐标系(东北天坐标系)下的坐标值：
Figure BDA0003523906390000051
该坐标后续可通过Sophus库和Eigen库转换到载体坐标系下；所述的惯性导航系统中，通过三轴加速度计、三轴陀螺仪、三轴磁罗盘所得到的载体运动学信息可表示为：
Figure BDA0003523906390000052
Figure BDA0003523906390000053
所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
所述图像信息采集单元为双目相机，其平行于地面安装，且安装高度应大于60cm。
如图2及图3所示，所述视觉信息处理模块包括如下步骤：
S3-1：通过双目相机实时采集多帧图像，并记录每一帧图像的序列号；
S3-2：将步骤S3-1所得图像按序列号顺序输入语义分割模块，对图像中包含的“天空”的语义信息进行分割，将分割后的天空区域的图像存储于MASK矩阵中、若图像不存在天空区域，则矩阵为空白RGB矩阵。
S3-3：通过OpenCV下的函数cvDilate()对Step3-2中得到的MASK矩阵进行膨胀处理。
S3-4：计算天空区域的图像面积并与原始图像面积进行比较得到区域占比Ra。
S3-5：按步骤S3-1所得的图像序列号对左右目图像进行灰度处理，并对左右目图像帧进行特征点提取，将左右目图像中的特征点进行匹配得到左右目之间的视差disparity；通过视差计算出匹配点的深度信息，计算公式如下：
Figure BDA0003523906390000054
并使用SAD(Sum of absolute differences)算法对匹配误差较大的点进行剔除；
S3-6：对步骤S3-5中得到的匹配点进行遍历，判断匹配点是否处于步骤S3-3中得到的膨胀处理后的MASK上，若处于，则对该匹配点对应的深度值Depth进行判断，若小于设定的深度阈值bf，bf＝baseline×fx，则认为有误匹配，将该匹配点去除。
S3-7：对图像帧进行跟踪，将步骤S3-6得到的每一帧图像上的特征点与前一帧图像的特征点以及地图点进行匹配，计算得到一个图像帧之间的变换矩阵Tcw，对前n帧的位姿变换矩阵Tcw进行累乘得到第n帧图像的位姿数据。
Figure BDA0003523906390000055
所述自适应融合模块主要选择扩展卡尔曼滤波对上述数据进行融合，具体实现融合步骤如下：
首先是运动学变量的预测和更新部分，包括：
S4-1对载体的速度进行初始化，
初始化速度
Figure BDA0003523906390000061
其中
Figure BDA0003523906390000062
分别表示载体在载体坐标系中x、y、z轴方向的线速度，
Figure BDA0003523906390000063
分别表示载体在载体坐标系中x、y、z轴方向的角速度；初始化权利要求书1所述载体的初始位姿
Figure BDA0003523906390000064
其中xb、yb、zb分别表示载体在载体坐标系中x、y、z轴方向的坐标值，φb、θb、ψb分别表示载体在载体坐标系中的横滚角Roll、俯仰角Pitch和偏航角Yaw。
S4-2对GNSS/INS信息采集模块中陀螺仪与加速度计组成的速度变量信息
Figure BDA0003523906390000065
对载体的速度通过扩展卡尔曼滤波进行测量更新，具体的更新步骤为：
S4-2-1：首先定义12维状态量：
Figure BDA0003523906390000066
分别有三轴速度
Figure BDA0003523906390000067
三轴线加速度
Figure BDA0003523906390000068
三轴角速度
Figure BDA0003523906390000069
三轴角加速度
Figure BDA00035239063900000610
S4-2-2：定义6维观测量：
Figure BDA00035239063900000611
分别有三轴线加速度观测值
Figure BDA00035239063900000612
三轴角速度观测值
Figure BDA00035239063900000613
S4-2-3：对步骤S4-2-1中的状态量及其对应的状态转移矩阵进行初始化：
Figure BDA00035239063900000614
Figure BDA00035239063900000615
S4-2-4：不考虑外部控因素对载体的影响，对状态量进行预测更新，预测更新公式如下：
其中状态转移方程为：
Figure BDA0003523906390000071
Figure BDA0003523906390000072
表示由k-1时刻状态量的最优估计值通过经典运动学方程计算而来的k时刻状态量的预测值；
Figure BDA0003523906390000073
表示在姿态角改变情况下加速度ab在原坐标系下的旋转矩阵，其表示为：
Figure BDA0003523906390000074
对状态转移方程求偏导得到Jacobi矩阵，即状态转移矩阵：
Figure BDA0003523906390000075
其中I为3×3的单位矩阵，δa,k-1以如下矩阵形式表示：
Figure BDA0003523906390000076
S4-2-5：通过上述步骤S4-2-3和S4-2-4得到的k时刻的先验速度状态量
Figure BDA0003523906390000077
以及步骤S4-2中得到的k时刻的观测量
Figure BDA0003523906390000078
通过以下公式计算得到K时刻的最优估计值：
Figure BDA0003523906390000079
其中
Figure BDA00035239063900000710
为卡尔曼增益
Figure BDA00035239063900000711
其中
Figure BDA00035239063900000712
对位姿的预测及更新，具体如下：
S4-3根据步骤S4-2所得的K时刻的最优速度状态估计值
Figure BDA00035239063900000713
对步骤S4-1中所述的位姿状态量
Figure BDA00035239063900000714
进行测量更新，更新步骤如下：
S4-3-1:首先定义12维状态量：
Figure BDA0003523906390000081
分别有三维坐标
Figure BDA0003523906390000082
三轴姿态角αb＝[Φ θ ψ]T，三轴线速度
Figure BDA0003523906390000083
三轴角速度
Figure BDA0003523906390000084
S4-3-2：定义12维观测量：
Figure BDA0003523906390000085
分别有三维坐标观测值
Figure BDA0003523906390000086
三轴姿态角观测值
Figure BDA0003523906390000087
三轴线速度观测值
Figure BDA0003523906390000088
三轴角速度观测值
Figure BDA0003523906390000089
S4-3-3：对步骤S4-3-1中的状态量及其对应的状态转移矩阵进行初始化：
状态量初始化：
Figure BDA00035239063900000810
状态转移矩阵初始化：
Figure BDA00035239063900000811
S4-3-4：不考虑外部控制因素对载体的影响，对状态量进行预测更新，有预测更新公式：
其中状态转移方程为：
Figure BDA00035239063900000812
Figure BDA00035239063900000813
表示由k-1时刻状态量的最优估计值通过经典运动学方程计算而来的k时刻状态量的预测值；
对状态转移方程求偏导得到Jacobi矩阵，即状态转移矩阵：
Figure BDA00035239063900000814
S4-4：通过上述步骤S4-3-3和S4-3-4得到的k时刻的先验位姿状态量
Figure BDA00035239063900000815
以及所述观测量合并得到的k时刻的位姿状态观测量(注意这两个观测量会分别进行下述步骤，且其中频率较低的观测量的值为线性拟合计算得到)
Figure BDA0003523906390000091
通过以下公式计算得到K时刻的最优估计值：
Figure BDA0003523906390000092
其中，
Figure BDA0003523906390000093
为卡尔曼增益
Figure BDA0003523906390000094
其中，
Figure BDA0003523906390000095
其中
Figure BDA0003523906390000096
为测量误差协方差矩阵，其表示为：
Figure BDA0003523906390000097
r2～r4为经过调参后得到的常值，由于r1对应着状态量中三维位移的测量更新，而相机的视觉跟踪算法和GPS都会对k时刻的位移值进行测量，因而原十二轴状态量中的三维位移量会分别对相机和GPS进行两次测量更新，对于相机所得位姿的测量误差协方差矩阵中r1同样为调参后得到的常值，也就是在相机，所得位姿的测量误差协方差矩阵中r1为常值，在GPS所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rD，其由S3-4得到的天空区域占比Ra决定。
进一步，动态参数rD由天空区域占比Ra决定，具体为：
根据天空区域占比Ra以及步骤Step1中所述GNSS/INS信息采集模块所捕获到的卫星数目x对动态参数rD进行加权取值；Ra用于判定权利要求1所述载体位于GPS信号较差的城市峡谷中还是GPS信号较好的开阔地带。
则有
Figure BDA0003523906390000098
其中λ为Ra的加权系数，这里取λ＝4。
得到的动态参数rD用于GPS数据的动态融合，最终可用于实现精度更高、鲁棒性更强的自适应GPS数据融合定位。
本发明针对上述问题提出一种依据图像的天空区域语义分割改进的自适应GPS融合方法，该方法可利用语义分割网络对天空区域进行分割，去除掉视觉算法中天空区域的误匹配，从而一定程度上改进视觉算法的精度，并根据天空区域占比Ra判断GPS所在载体所处的环境，从而对GPS得到的数据进行自适应融合。
实施例2
一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现所述的融合定位方法，包括
GNSS/INS信息采集模块：包括全球卫星导航系统和惯性导航系统；
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
上述模块均设置在同一载体上。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受所述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种自适应GPS融合定位系统，其特征在于，包括：
GNSS/INS信息采集模块：包括全球卫星导航系统和惯性导航系统；
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
上述模块均设置在同一载体上。
2.根据权利要求1所述的自适应GPS融合定位系统，其特征在于，所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
3.根据权利要求1所述的自适应GPS融合定位系统，其特征在于，所述图像信息采集单元平行于地面安装，且安装高度应大于60cm。
4.根据权利要求2所述的自适应GPS融合定位系统，其特征在于，所述图像信息采集单元包括双目相机。
5.一种基于权利要求1-4任一项所述的自适应GPS融合定位系统的融合定位方法，其特征在于，包括：
获得载体在世界坐标系下的运动坐标及惯性导航系统获得载体运动学信息；
实时采集运动载体前方区域的多帧图像，并记录每一帧图像的序列号；
将图像按照序列号顺序进行语义分割，将分割后的天空区域存储于MASK矩阵中；不存在天空区域，则矩阵存为空白RGB矩阵；
对MASK矩阵进行膨胀处理，计算天空区域的图像面积，及其在原始图像面积的区域占比；
依据图像序列号对双目图像进行特征点匹配，获得匹配点；
将匹配点与前一帧图像的特征点以及地图特征点进行匹配，计算得到一个图像帧的位姿变换矩阵，进一步通过对前n帧的位姿变换矩阵进行累乘得到第n帧图像的位姿数据；
通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿。
6.根据权利要求5所述的融合定位方法，其特征在于，所述通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿，具体为：
对载体的初始速度和位姿进行初始化；
定义十二维的速度状态量
Figure FDA0003523906380000021
和位姿状态量
Figure FDA0003523906380000022
根据GNSS/INS信息采集模块中陀螺仪与加速度计构成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，得到K时刻的最优速度状态估计值
Figure FDA0003523906380000023
根据K时刻的最优速度状态估计值、视觉位姿估计值、GPS位置测量值对位姿状态量进行测量更新，得到k时刻的位姿状态观测量
Figure FDA0003523906380000024
通过k时刻的先验位姿状态量
Figure FDA0003523906380000025
以及k时刻的位姿状态观测量
Figure FDA0003523906380000026
对K时刻的位姿进行最优估计。
7.根据权利要求6所述的融合定位方法，其特征在于，所述通过k时刻的先验位姿状态量
Figure FDA0003523906380000027
以及k时刻的位姿状态观测量
Figure FDA0003523906380000028
对K时刻的位姿进行最优估计，具体为：
通过如下公式计算得到K时刻的位姿最优估计值：
Figure FDA0003523906380000029
其中
Figure FDA00035239063800000210
为卡尔曼增益
Figure FDA00035239063800000211
其中
Figure FDA00035239063800000212
其中
Figure FDA00035239063800000213
为测量误差协方差矩阵，其表示为：
Figure FDA00035239063800000214
r2～r4为经过调参后得到的常值，在相机所得位姿的测量误差协方差矩阵中r1为常值，在GPS所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rD，rD由天空区域占比Ra决定。
8.根据权利要求7所述的融合定位方法，其特征在于，所述rD由天空区域占比Ra决定，具体为：
根据天空区域占比Ra以及GNSS/INS信息采集模块所捕获到的卫星数目x对动态参数rD进行加权取值得到动态参数rD；
Figure FDA0003523906380000031
其中λ为Ra的加权系数。
9.根据权利要求5所述的融合定位方法，其特征在于，所述依据图像序列号对双目图像进行特征点匹配，获得匹配点，具体为：
按照图像序列号对左右双目图像进行灰度处理，并对双目图像帧进行特征点提取，将左右图像中的特征点进行匹配得到双目图像之间的视差，通过视差计算匹配点的深度信息，并使用SAD算法对匹配误差大的点进行剔除；
对上一步骤得到匹配点进行遍历，判断匹配点是否处于膨胀处理后的MASK矩阵上，若处于，则对该匹配点对应的深度值Depth进行判断，若小于设定的深度阈值bf，bf＝baseline×fx，则认为有误匹配，将该匹配点去除。
10.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求5-9任一项所述的融合定位方法。
CN202210189088.0A 2022-02-28 2022-02-28 一种自适应gps融合定位系统、方法及介质 Pending CN114660641A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210189088.0A CN114660641A (zh) 2022-02-28 2022-02-28 一种自适应gps融合定位系统、方法及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210189088.0A CN114660641A (zh) 2022-02-28 2022-02-28 一种自适应gps融合定位系统、方法及介质

Publications (1)

Publication Number Publication Date
CN114660641A true CN114660641A (zh) 2022-06-24

Family

ID=82026808

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210189088.0A Pending CN114660641A (zh) 2022-02-28 2022-02-28 一种自适应gps融合定位系统、方法及介质

Country Status (1)

Country Link
CN (1) CN114660641A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115171030A (zh) * 2022-09-09 2022-10-11 山东省凯麟环保设备股份有限公司 基于多级特征融合的多模态图像分割方法、系统及器件

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115171030A (zh) * 2022-09-09 2022-10-11 山东省凯麟环保设备股份有限公司 基于多级特征融合的多模态图像分割方法、系统及器件
CN115171030B (zh) * 2022-09-09 2023-01-31 山东省凯麟环保设备股份有限公司 基于多级特征融合的多模态图像分割方法、系统及器件

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN106780699B (zh) 一种基于sins/gps和里程计辅助的视觉slam方法
CN111486845B (zh) 基于海底地形匹配的auv多策略导航方法
CN109631887B (zh) 基于双目、加速度与陀螺仪的惯性导航高精度定位方法
CN111583369B (zh) 一种基于面线角点特征提取的激光slam方法
CN113340295B (zh) 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN107451593B (zh) 一种基于图像特征点的高精度gps定位方法
US8401783B2 (en) Method of building map of mobile platform in dynamic environment
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN108931245A (zh) 移动机器人的局部自定位方法及设备
CN112639502A (zh) 机器人位姿估计
CN111288989B (zh) 一种小型无人机视觉定位方法
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN114648584B (zh) 一种用于多源融合定位的鲁棒性控制方法和系统
CN114217665A (zh) 一种相机和激光雷达时间同步方法、装置及存储介质
CN113740871A (zh) 一种在高动态环境下的激光slam方法、系统设备及存储介质
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN115183762A (zh) 一种机场仓库内外建图方法、系统、电子设备及介质
CN114660641A (zh) 一种自适应gps融合定位系统、方法及介质
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
Gupta et al. Terrain‐based vehicle orientation estimation combining vision and inertial measurements
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN111829514A (zh) 一种适用于车辆底盘集成控制的路面工况预瞄方法
CN115930948A (zh) 一种果园机器人融合定位方法

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