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

一种自适应gps融合定位系统、方法及介质
技术领域
1.本发明涉及数据融合定位领域，具体涉及一种自适应gps融合定位系统、方法及介质。


背景技术：

2.在多传感器信息融合系统中，各个传感器会对不同参数进行测量，然而得到的测量值不仅会受传感器本身制造结构以及信息采集方法的影响，而且会受到环境因素的影响。gps是一个易受到环境影响的传感器，其在室内无法接收到卫星信号从而无法对当前位置进行计算，而其处于室外时仍然会有信号被遮挡或被干扰的可能性出现，这种情况往往会出现在城市峡谷的环境中，由于城市中的高楼会对gps的接收信号进行遮挡，致使卫星的跟踪精度差；相机是一个相对比较稳定的传感器，但其定位精度会受到视觉算法本身的一些错误影响，其中特征点误匹配就是一个重要的影响因素，实际应用过程中天空区域是一个较容易出现误匹配的区域。


技术实现要素：

3.为了克服现有技术中存在的缺点与不足，本发明提供一种自适应gps融合定位系统、方法及介质。
4.本发明克服两个现有技术存在的问题，一个是在不同环境下gps所得数据精度不同对整个多传感器信息融合系统精度的影响，另一个是视觉位姿估计算法中对特定区域进行错误匹配问题，提供了一种鲁棒性强的自适应gps数据融合方法，不仅可以去除视觉位姿估计中对天空区域的误匹配，而且可以使得移动机器人在处于城市峡谷状态下时可以获得更好的位姿估计。
5.本发明采用如下技术方案：
6.一种自适应gps融合定位系统，包括：
7.gnss/ins信息采集模块：包括全球卫星导航系统和惯性导航系统；
8.视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
9.自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整gps数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
10.上述模块均设置在同一载体上。
11.进一步，所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
12.进一步，所述图像信息采集单元平行于地面安装，且安装高度应大于60cm。
13.进一步，所述图像信息采集单元包括双目相机。
14.一种基于自适应gps融合定位系统的融合定位方法，包括：
15.获得载体在世界坐标系下的运动坐标及惯性导航系统获得载体运动学信息；
16.实时采集运动载体前方区域的多帧图像，并记录每一帧图像的序列号；
17.将图像按照序列号顺序进行语义分割，将分割后的天空区域存储于mask矩阵中；不存在天空区域，则矩阵存为空白rgb矩阵；
18.对mask矩阵进行膨胀处理，计算天空区域的图像面积，及其在原始图像面积的区域占比；
19.依据图像序列号对双目图像进行特征点匹配，获得匹配点；
20.将匹配点与前一帧图像的特征点以及地图特征点进行匹配，计算得到一个图像帧的位姿变换矩阵，进一步通过对前n帧的位姿变换矩阵进行累乘得到第n帧图像的位姿数据；
21.通过对天空区域在原始图像中的区域占比动态调整gps数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿。
22.进一步，所述通过对天空区域在原始图像中的区域占比动态调整gps数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿，具体为：
23.对载体的初始速度和位姿进行初始化；
24.定义十二维的速度状态量和位姿状态量
25.根据gnss/ins信息采集模块中陀螺仪与加速度计构成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，得到k时刻的最优速度状态估计值
26.根据k时刻的最优速度状态估计值、视觉位姿估计值、gps位置测量值对位姿状态量进行测量更新，得到k时刻的位姿状态观测量
27.通过k时刻的先验位姿状态量以及k时刻的位姿状态观测量对k时刻的位姿进行最优估计。
28.进一步，所述通过k时刻的先验位姿状态量以及k时刻的位姿状态观测量对k时刻的位姿进行最优估计，具体为：
29.通过如下公式计算得到k时刻的位姿最优估计值：
[0030][0031]
其中为卡尔曼增益
[0032]
其中，
[0033]
其中为测量误差协方差矩阵，其表示为：
[0034]
[0035]
r2～r4为经过调参后得到的常值，在相机所得位姿的测量误差协方差矩阵中r1为常值，在gps所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rd，rd由天空区域占比ra决定。
[0036]
进一步，所述rd由天空区域占比ra决定，具体为：
[0037]
根据天空区域占比ra以及gnss/ins信息采集模块所捕获到的卫星数目x对动态参数rd进行加权取值得到动态参数rd；
[0038][0039]
其中λ为ra的加权系数。
[0040]
进一步，所述依据图像序列号对双目图像进行特征点匹配，获得匹配点，具体为：
[0041]
按照图像序列号对左右双目图像进行灰度处理，并对双目图像帧进行特征点提取，将左右图像中的特征点进行匹配得到双目图像之间的视差，通过视差计算匹配点的深度信息，并使用sad算法对匹配误差大的点进行剔除；
[0042]
对上一步骤得到匹配点进行遍历，判断匹配点是否处于膨胀处理后的mask矩阵上，若处于，则对该匹配点对应的深度值depth进行判断，若小于设定的深度阈值bf，bf＝baseline
×fx
，则认为有误匹配，将该匹配点去除。
[0043]
一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的融合定位方法。
[0044]
本发明的有益效果：
[0045]
本发明的视觉位姿估计算法部分在保留天空区域内大于深度阈值的匹配点的情况下消除了天空区域的误匹配的特征点对位姿估计的影响，其次通过对天空区域在图像中的占比情况动态调整gps数据在卡尔曼滤波的融合过程中的估计权重，使得最终滤波出的估计位姿更为准确。
附图说明
[0046]
图1是本发明的结构示意图；
[0047]
图2是本发明天空区域特征点匹配示意图；
[0048]
图3是本发明语义分割效果图。
具体实施方式
[0049]
下面结合实施例及附图，对本发明作进一步地详细说明，但本发明的实施方式不限于此。
[0050]
实施例1
[0051]
如图1所示，一种自适应gps融合定位方法，硬件系统包括三个模块，分别是gnss/ins信息采集模块、视觉信息处理模块及自适应融合模块，上述所有模块位于同一载体上，所述自适应融合模块通过滤波算法融合处理上述两个模块的数据，计算得到载体的最优位姿结果。
[0052]
所述gnss/ins信息采集模块，本实施例使用xsense mti-700gps/ins模块进行数据采集。
[0053]
进一步，包括全球卫星导航系统和惯性导航系统，其中全球卫星导航系统通过gnss接收机捕获并跟踪卫星获得导航解，亦即接收器所在经度和纬度数据，其中惯性导航系统通过三轴加速度计、三轴陀螺仪、三轴磁罗盘获得载体的运动学信息。
[0054]
具体可以获得如下信号：
[0055]
gnss接收器捕获并跟踪卫星时可能有信号遮挡或受干扰的情况，比如在城市峡谷中，城市中的高楼会遮挡信号，致使卫星无法被跟踪或跟踪精度差。gnss接收器得到的经度和纬度数据为wgs84(地球坐标系)下的坐标，其可转换为世界坐标系(东北天坐标系)下的坐标值：该坐标后续可通过sophus库和eigen库转换到载体坐标系下；所述的惯性导航系统中，通过三轴加速度计、三轴陀螺仪、三轴磁罗盘所得到的载体运动学信息可表示为：表示为：
[0056]
所述视觉信息处理模块包括图像信息采集单元，语义分割单元、图像特征提取与匹配单元及视觉位姿估计单元。
[0057]
所述图像信息采集单元为双目相机，其平行于地面安装，且安装高度应大于60cm。
[0058]
如图2及图3所示，所述视觉信息处理模块包括如下步骤：
[0059]
s3-1：通过双目相机实时采集多帧图像，并记录每一帧图像的序列号；
[0060]
s3-2：将步骤s3-1所得图像按序列号顺序输入语义分割模块，对图像中包含的“天空”的语义信息进行分割，将分割后的天空区域的图像存储于mask矩阵中、若图像不存在天空区域，则矩阵为空白rgb矩阵。
[0061]
s3-3：通过opencv下的函数cvdilate()对step3-2中得到的mask矩阵进行膨胀处理。
[0062]
s3-4：计算天空区域的图像面积并与原始图像面积进行比较得到区域占比ra。
[0063]
s3-5：按步骤s3-1所得的图像序列号对左右目图像进行灰度处理，并对左右目图像帧进行特征点提取，将左右目图像中的特征点进行匹配得到左右目之间的视差disparity；通过视差计算出匹配点的深度信息，计算公式如下：
[0064][0065]
并使用sad(sum of absolute differences)算法对匹配误差较大的点进行剔除；
[0066]
s3-6：对步骤s3-5中得到的匹配点进行遍历，判断匹配点是否处于步骤s3-3中得到的膨胀处理后的mask上，若处于，则对该匹配点对应的深度值depth进行判断，若小于设定的深度阈值bf，bf＝baseline
×fx
，则认为有误匹配，将该匹配点去除。
[0067]
s3-7：对图像帧进行跟踪，将步骤s3-6得到的每一帧图像上的特征点与前一帧图像的特征点以及地图点进行匹配，计算得到一个图像帧之间的变换矩阵t
cw
，对前n帧的位姿变换矩阵t
cw
进行累乘得到第n帧图像的位姿数据。
[0068][0069]
所述自适应融合模块主要选择扩展卡尔曼滤波对上述数据进行融合，具体实现融
合步骤如下：
[0070]
首先是运动学变量的预测和更新部分，包括：
[0071]
s4-1对载体的速度进行初始化，
[0072]
初始化速度其中分别表示载体在载体坐标系中x、y、z轴方向的线速度，分别表示载体在载体坐标系中x、y、z轴方向的角速度；初始化权利要求书1所述载体的初始位姿其中xb、yb、zb分别表示载体在载体坐标系中x、y、z轴方向的坐标值，φb、θb、ψb分别表示载体在载体坐标系中的横滚角roll、俯仰角pitch和偏航角yaw。
[0073]
s4-2对gnss/ins信息采集模块中陀螺仪与加速度计组成的速度变量信息对载体的速度通过扩展卡尔曼滤波进行测量更新，具体的更新步骤为：
[0074]
s4-2-1：首先定义12维状态量：
[0075][0076]
分别有三轴速度三轴线加速度三轴角速度三轴角加速度
[0077]
s4-2-2：定义6维观测量：
[0078][0079]
分别有三轴线加速度观测值三轴角速度观测值
[0080]
s4-2-3：对步骤s4-2-1中的状态量及其对应的状态转移矩阵进行初始化：
[0081][0082][0083]
s4-2-4：不考虑外部控因素对载体的影响，对状态量进行预测更新，预测更新公式如下：
[0084]
其中状态转移方程为：
[0085][0086]
表示由k-1时刻状态量的最优估计值通过经典运动学方程计算而来的k时刻状态量的预测值；
[0087]
表示在姿态角改变情况下加速度ab在原坐标系下的旋转矩阵，其表示为：
[0088][0089]
对状态转移方程求偏导得到jacobi矩阵，即状态转移矩阵：
[0090][0091]
其中i为3
×
3的单位矩阵，δ
a,k-1
以如下矩阵形式表示：
[0092][0093]
s4-2-5：通过上述步骤s4-2-3和s4-2-4得到的k时刻的先验速度状态量以及步骤s4-2中得到的k时刻的观测量通过以下公式计算得到k时刻的最优估计值：
[0094][0095]
其中为卡尔曼增益
[0096]
其中
[0097]
对位姿的预测及更新，具体如下：
[0098]
s4-3根据步骤s4-2所得的k时刻的最优速度状态估计值对步骤s4-1中所述的位姿状态量进行测量更新，更新步骤如下：
[0099]
s4-3-1:首先定义12维状态量：
[0100]
[0101]
分别有三维坐标三轴姿态角αb＝[φ θ ψ]
t
，三轴线速度三轴角速度
[0102]
s4-3-2：定义12维观测量：
[0103][0104]
分别有三维坐标观测值三轴姿态角观测值三轴线速度观测值三轴角速度观测值
[0105]
s4-3-3：对步骤s4-3-1中的状态量及其对应的状态转移矩阵进行初始化：
[0106]
状态量初始化：
[0107]
状态转移矩阵初始化：
[0108]
s4-3-4：不考虑外部控制因素对载体的影响，对状态量进行预测更新，有预测更新公式：
[0109]
其中状态转移方程为：
[0110][0111]
表示由k-1时刻状态量的最优估计值通过经典运动学方程计算而来的k时刻状态量的预测值；
[0112]
对状态转移方程求偏导得到jacobi矩阵，即状态转移矩阵：
[0113][0114]
s4-4：通过上述步骤s4-3-3和s4-3-4得到的k时刻的先验位姿状态量以及所述观测量合并得到的k时刻的位姿状态观测量(注意这两个观测量会分别进行下述步骤，且其中频率较低的观测量的值为线性拟合计算得到)通过以下公式计算得到k时刻的最优估计值：
[0115][0116]
其中，为卡尔曼增益
[0117]
其中，
[0118]
其中为测量误差协方差矩阵，其表示为：
[0119][0120]
r2～r4为经过调参后得到的常值，由于r1对应着状态量中三维位移的测量更新，而相机的视觉跟踪算法和gps都会对k时刻的位移值进行测量，因而原十二轴状态量中的三维位移量会分别对相机和gps进行两次测量更新，对于相机所得位姿的测量误差协方差矩阵中r1同样为调参后得到的常值，也就是在相机，所得位姿的测量误差协方差矩阵中r1为常值，在gps所得的位置的测量误差协方差矩阵中r1为动态参数，令其为rd，其由s3-4得到的天空区域占比ra决定。
[0121]
进一步，动态参数rd由天空区域占比ra决定，具体为：
[0122]
根据天空区域占比ra以及步骤step1中所述gnss/ins信息采集模块所捕获到的卫星数目x对动态参数rd进行加权取值；ra用于判定权利要求1所述载体位于gps信号较差的城市峡谷中还是gps信号较好的开阔地带。
[0123]
则有其中λ为ra的加权系数，这里取λ＝4。
[0124]
得到的动态参数rd用于gps数据的动态融合，最终可用于实现精度更高、鲁棒性更强的自适应gps数据融合定位。
[0125]
本发明针对上述问题提出一种依据图像的天空区域语义分割改进的自适应gps融合方法，该方法可利用语义分割网络对天空区域进行分割，去除掉视觉算法中天空区域的误匹配，从而一定程度上改进视觉算法的精度，并根据天空区域占比ra判断gps所在载体所处的环境，从而对gps得到的数据进行自适应融合。
[0126]
实施例2
[0127]
一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现所述的融合定位方法，包括
[0128]
gnss/ins信息采集模块：包括全球卫星导航系统和惯性导航系统；
[0129]
视觉信息处理模块：用于采集载体前方区域的图像，进行语义分割后获得天空区域在原始图像中的区域占比，进一步获得位姿数据；
[0130]
自适应融合模块：通过对天空区域在原始图像中的区域占比动态调整gps数据在卡曼尔滤波的融合过程的估计权重，获得滤波后的估计位姿；
[0131]
上述模块均设置在同一载体上。
[0132]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受所述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。