CN114660641B - 一种自适应gps融合定位系统、方法及介质 - Google Patents
一种自适应gps融合定位系统、方法及介质 Download PDFInfo
- Publication number
- CN114660641B CN114660641B CN202210189088.0A CN202210189088A CN114660641B CN 114660641 B CN114660641 B CN 114660641B CN 202210189088 A CN202210189088 A CN 202210189088A CN 114660641 B CN114660641 B CN 114660641B
- Authority
- CN
- China
- Prior art keywords
- pose
- image
- area
- fusion
- matching
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 37
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000000007 visual effect Effects 0.000 claims abstract description 23
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 230000011218 segmentation Effects 0.000 claims abstract description 13
- 230000010365 information processing Effects 0.000 claims abstract description 11
- 238000007499 fusion processing Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 40
- 238000005259 measurement Methods 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 230000003044 adaptive effect Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 description 9
- 238000006073 displacement reaction Methods 0.000 description 3
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (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融合定位系统，包括：
GNSS/INS信息采集模块：包括全球卫星导航系统和惯性导航系统；
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
上述模块均设置在同一载体上。
进一步，所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
进一步，所述图像信息采集单元平行于地面安装，且安装高度应大于60cm。
进一步，所述图像信息采集单元包括双目相机。
一种基于自适应GPS融合定位系统的融合定位方法，包括：
获得载体在世界坐标系下的运动坐标及惯性导航系统获得载体运动学信息；
实时采集运动载体前方区域的多帧图像，并记录每一帧图像的序列号；
将图像按照序列号顺序进行语义分割，将分割后的天空区域存储于MASK矩阵中；不存在天空区域，则矩阵存为空白RGB矩阵；
对MASK矩阵进行膨胀处理，计算天空区域的图像面积，及其在原始图像面积的区域占比；
依据图像序列号对双目图像进行特征点匹配，获得匹配点；
将匹配点与前一帧图像的特征点以及地图特征点进行匹配，计算得到一个图像帧的位姿变换矩阵，进一步通过对前n帧的位姿变换矩阵进行累乘得到第n帧图像的位姿数据；
通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿。
进一步，所述通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿，具体为：
对载体的初始速度和位姿进行初始化；
定义十二维的速度状态量和位姿状态量
根据GNSS/INS信息采集模块中陀螺仪与加速度计构成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，得到K时刻的最优速度状态估计值
根据K时刻的最优速度状态估计值、视觉位姿估计值、GPS位置测量值对位姿状态量进行测量更新，得到k时刻的位姿状态观测量
通过k时刻的先验位姿状态量以及k时刻的位姿状态观测量对K时刻的位姿进行最优估计。
进一步，所述通过k时刻的先验位姿状态量以及k时刻的位姿状态观测量对K时刻的位姿进行最优估计，具体为：
通过如下公式计算得到K时刻的位姿最优估计值：
其中为卡尔曼增益
其中，
其中为测量误差协方差矩阵，其表示为：
r2～r4为经过调参后得到的常值，在相机所得位姿的测量误差协方差矩阵中r1为常值，在GPS所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rD，rD由天空区域占比Ra决定。
进一步，所述rD由天空区域占比Ra决定，具体为：
根据天空区域占比Ra以及GNSS/INS信息采集模块所捕获到的卫星数目x对动态参数rD进行加权取值得到动态参数rD；
其中λ为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(地球坐标系)下的坐标，其可转换为世界坐标系(东北天坐标系)下的坐标值：该坐标后续可通过Sophus库和Eigen库转换到载体坐标系下；所述的惯性导航系统中，通过三轴加速度计、三轴陀螺仪、三轴磁罗盘所得到的载体运动学信息可表示为：
所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
所述图像信息采集单元为双目相机，其平行于地面安装，且安装高度应大于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；通过视差计算出匹配点的深度信息，计算公式如下：
并使用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帧图像的位姿数据。
所述自适应融合模块主要选择扩展卡尔曼滤波对上述数据进行融合，具体实现融合步骤如下：
首先是运动学变量的预测和更新部分，包括：
S4-1对载体的速度进行初始化，
初始化速度其中分别表示载体在载体坐标系中x、y、z轴方向的线速度，分别表示载体在载体坐标系中x、y、z轴方向的角速度；初始化载体的初始位姿其中xb、yb、zb分别表示载体在载体坐标系中x、y、z轴方向的坐标值，φb、θb、ψb分别表示载体在载体坐标系中的横滚角Roll、俯仰角Pitch和偏航角Yaw。
S4-2对GNSS/INS信息采集模块中陀螺仪与加速度计组成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，具体的更新步骤为：
S4-2-1：首先定义12维状态量：
分别有三轴速度三轴线加速度三轴角速度三轴角加速度
S4-2-2：定义6维观测量：
分别有三轴线加速度观测值三轴角速度观测值
S4-2-3：对步骤S4-2-1中的状态量及其对应的状态转移矩阵进行初始化：
S4-2-4：不考虑外部控因素对载体的影响，对状态量进行预测更新，预测更新公式如下：
其中状态转移方程为：
表示由k-1时刻状态量的最优估计值通过经典运动学方程计算而来的k时刻状态量的预测值；
表示在姿态角改变情况下加速度ab在原坐标系下的旋转矩阵，其表示为：
对状态转移方程求偏导得到Jacobi矩阵，即状态转移矩阵：
其中I为3×3的单位矩阵，δa,k-1以如下矩阵形式表示：
S4-2-5：通过上述步骤S4-2-3和S4-2-4得到的k时刻的先验速度状态量以及步骤S4-2中得到的k时刻的观测量通过以下公式计算得到K时刻的最优估计值：
其中为卡尔曼增益
其中
对位姿的预测及更新，具体如下：
S4-3根据步骤S4-2所得的K时刻的最优速度状态估计值对步骤S4-1中所述的位姿状态量进行测量更新，更新步骤如下：
S4-3-1:首先定义12维状态量：
分别有三维坐标三轴姿态角αb＝[Φ θ ψ]T，三轴线速度三轴角速度
S4-3-2：定义12维观测量：
分别有三维坐标观测值三轴姿态角观测值三轴线速度观测值三轴角速度观测值
S4-3-3：对步骤S4-3-1中的状态量及其对应的状态转移矩阵进行初始化：
状态量初始化：
状态转移矩阵初始化：
S4-3-4：不考虑外部控制因素对载体的影响，对状态量进行预测更新，有预测更新公式：
其中状态转移方程为：
表示由k-1时刻状态量的最优估计值通过经典运动学方程计算而来的k时刻状态量的预测值；
对状态转移方程求偏导得到Jacobi矩阵，即状态转移矩阵：
S4-4：通过上述步骤S4-3-3和S4-3-4得到的k时刻的先验位姿状态量以及所述观测量合并得到的k时刻的位姿状态观测量(注意这两个观测量会分别进行下述步骤，且其中频率较低的观测量的值为线性拟合计算得到)
通过以下公式计算得到K时刻的最优估计值：
其中，为卡尔曼增益
其中，
其中为测量误差协方差矩阵，其表示为：
r2～r4为经过调参后得到的常值，由于r1对应着状态量中三维位移的测量更新，而相机的视觉跟踪算法和GPS都会对k时刻的位移值进行测量，因而原十二轴状态量中的三维位移量会分别对相机和GPS进行两次测量更新，对于相机所得位姿的测量误差协方差矩阵中r1同样为调参后得到的常值，也就是在相机，所得位姿的测量误差协方差矩阵中r1为常值，在GPS所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rD，其由S3-4得到的天空区域占比Ra决定。
进一步，动态参数rD由天空区域占比Ra决定，具体为：
根据天空区域占比Ra以及步骤Step1中所述GNSS/INS信息采集模块所捕获到的卫星数目x对动态参数rD进行加权取值；Ra用于判定载体位于GPS信号较差的城市峡谷中还是GPS信号较好的开阔地带。
则有其中λ为Ra的加权系数，这里取λ＝4。
得到的动态参数rD用于GPS数据的动态融合，最终可用于实现精度更高、鲁棒性更强的自适应GPS数据融合定位。
本发明针对上述问题提出一种依据图像的天空区域语义分割改进的自适应GPS融合方法，该方法可利用语义分割网络对天空区域进行分割，去除掉视觉算法中天空区域的误匹配，从而一定程度上改进视觉算法的精度，并根据天空区域占比Ra判断GPS所在载体所处的环境，从而对GPS得到的数据进行自适应融合。
实施例2
一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的融合定位方法，包括
GNSS/INS信息采集模块：包括全球卫星导航系统和惯性导航系统；
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
上述模块均设置在同一载体上。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受所述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (7)
1.一种基于自适应GPS融合定位系统的融合定位方法，其特征在于，
所述自适应GPS融合定位系统，包括：
GNSS/INS信息采集模块：包括全球卫星导航系统和惯性导航系统；
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
所述GNSS/INS信息采集模块、视觉信息处理模块及自适应融合模块设置在同一载体上；
融合定位方法，包括：
获得载体在世界坐标系下的运动坐标及惯性导航系统获得载体运动学信息；
实时采集运动载体前方区域的多帧图像，并记录每一帧图像的序列号；
将图像按照序列号顺序进行语义分割，将分割后的天空区域存储于MASK矩阵中；不存在天空区域，则矩阵存为空白RGB矩阵；
对MASK矩阵进行膨胀处理，计算天空区域的图像面积，及其在原始图像面积的区域占比；
依据图像序列号对双目图像进行特征点匹配，获得匹配点；
将匹配点与前一帧图像的特征点以及地图特征点进行匹配，计算得到一个图像帧的位姿变换矩阵，进一步通过对前n帧的位姿变换矩阵进行累乘得到第n帧图像的位姿数据；
通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
所述通过对天空区域在原始图像中的区域占比动态调整GPS数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿，具体为：
对载体的初始速度和位姿进行初始化；
定义十二维的速度状态量和位姿状态量
其中，pb为三维坐标，wb为三轴角速度，αb是三轴姿态角，vb是三轴线速度；是三轴角加速度；ab是三轴线加速度；
根据GNSS/INS信息采集模块中陀螺仪与加速度计构成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，得到K时刻的最优速度状态估计值
根据K时刻的最优速度状态估计值、视觉位姿估计值、GPS位置测量值对位姿状态量进行测量更新，得到k时刻的位姿状态观测量
通过k时刻的先验位姿状态量以及k时刻的位姿状态观测量对K时刻的位姿进行最优估计；
所述通过k时刻的先验位姿状态量以及k时刻的位姿状态观测量对K时刻的位姿进行最优估计，具体为：
通过如下公式计算得到K时刻的位姿最优估计值：
其中为卡尔曼增益
其中
其中为测量误差协方差矩阵，其表示为：
r2～r4为经过调参后得到的常值，在相机所得位姿的测量误差协方差矩阵中r1为常值，在GPS所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rD，rD由天空区域占比Ra决定。
2.根据权利要求1所述的融合定位方法，其特征在于，所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
3.根据权利要求2所述的融合定位方法，其特征在于，所述图像信息采集单元平行于地面安装，且安装高度大于60cm。
4.根据权利要求2所述的融合定位方法，其特征在于，所述图像信息采集单元包括双目相机。
5.根据权利要求1所述的融合定位方法，其特征在于，所述rD由天空区域占比Ra决定，具体为：
根据天空区域占比Ra以及GNSS/INS信息采集模块所捕获到的卫星数目x对动态参数rD进行加权取值得到动态参数rD；
其中λ为Ra的加权系数。
6.根据权利要求1所述的融合定位方法，其特征在于，所述依据图像序列号对双目图像进行特征点匹配，获得匹配点，具体为：
按照图像序列号对左右双目图像进行灰度处理，并对双目图像帧进行特征点提取，将左右图像中的特征点进行匹配得到双目图像之间的视差，通过视差计算匹配点的深度信息，并使用SAD算法对匹配误差大的点进行剔除；
对经过SAD算法提出后剩余的匹配点进行遍历，判断匹配点是否处于膨胀处理后的MASK矩阵上，若处于，则对该匹配点对应的深度值Depth进行判断，若小于设定的深度阈值bf，bf＝baseline×fx，则认为有误匹配，将该匹配点去除。
7.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-6任一项所述的融合定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210189088.0A CN114660641B (zh) | 2022-02-28 | 2022-02-28 | 一种自适应gps融合定位系统、方法及介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210189088.0A CN114660641B (zh) | 2022-02-28 | 2022-02-28 | 一种自适应gps融合定位系统、方法及介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114660641A CN114660641A (zh) | 2022-06-24 |
| CN114660641B true CN114660641B (zh) | 2024-07-12 |
Family
ID=82026808
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210189088.0A Active CN114660641B (zh) | 2022-02-28 | 2022-02-28 | 一种自适应gps融合定位系统、方法及介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114660641B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115171030B (zh) * | 2022-09-09 | 2023-01-31 | 山东省凯麟环保设备股份有限公司 | 基于多级特征融合的多模态图像分割方法、系统及器件 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107229063A (zh) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法 |
| CN108036785A (zh) * | 2017-11-24 | 2018-05-15 | 浙江大学 | 一种基于直接法与惯导融合的飞行器位姿估计方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108592914A (zh) * | 2018-04-08 | 2018-09-28 | 河南科技学院 | 无gps情况下的复杂区域巡视机器人定位、导航及授时方法 |
| WO2021121306A1 (zh) * | 2019-12-18 | 2021-06-24 | 北京嘀嘀无限科技发展有限公司 | 视觉定位方法和系统 |
| CN111121767B (zh) * | 2019-12-18 | 2023-06-30 | 南京理工大学 | 一种融合gps的机器人视觉惯导组合定位方法 |
-
2022
- 2022-02-28 CN CN202210189088.0A patent/CN114660641B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107229063A (zh) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法 |
| CN108036785A (zh) * | 2017-11-24 | 2018-05-15 | 浙江大学 | 一种基于直接法与惯导融合的飞行器位姿估计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114660641A (zh) | 2022-06-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109931926B (zh) | 一种基于站心坐标系的小型无人机无缝自主式导航方法 | |
| CN112639502B (zh) | 机器人位姿估计 | |
| CN113340295B (zh) | 一种多个测距传感器的无人艇近岸实时定位与建图方法 | |
| CN107782321B (zh) | 一种基于视觉和高精度地图车道线约束的组合导航方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN112967392A (zh) | 一种基于多传感器触合的大规模园区建图定位方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及系统 | |
| CN114396943B (zh) | 一种融合定位方法与终端 | |
| CN117739972B (zh) | 一种无全球卫星定位系统的无人机进近阶段定位方法 | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| CN111829514A (zh) | 一种适用于车辆底盘集成控制的路面工况预瞄方法 | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
| CN114660641B (zh) | 一种自适应gps融合定位系统、方法及介质 | |
| CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下slam方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
| CN108827287B (zh) | 一种复杂环境下的鲁棒视觉slam系统 | |
| CN112883134A (zh) | 数据融合建图方法、装置、电子设备及存储介质 | |
| CN114608560B (zh) | 一种基于智能终端传感器的无源组合室内定位系统及方法 | |
| CN113850864B (zh) | 一种面向室外移动机器人的gnss/lidar回环检测方法 | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN115290073A (zh) | 一种矿山井下非结构化特征下的slam方法及系统 | |
| CN118334259B (zh) | 融合增强多线激光雷达与imu的建图方法及系统 | |
| CN117760417B (zh) | 一种基于4d毫米波雷达与imu的融合定位方法及系统 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |