一种基于视觉惯性GNSSPPP耦合的定位方法

一种基于视觉惯性gnss ppp耦合的定位方法
技术领域
1.本发明属于卫星定位系统领域，特别是涉及一种基于视觉惯性gnss ppp耦合的定位方法。


背景技术：

2.随着自动驾驶技术的兴起，城市环境下高精度定位需求不断增长。卫星定位技术虽然能够在全球范围内为用户提供全天候、全时段、高精度的位置服务，但在城市环境下卫星信号容易被遮挡，其定位性能会急剧下降。
3.为解决上述问题，提出了gnss与ins融合构成组合导航系统和视觉slam两种解决方案，一是将gnss与ins融合构成组合导航系统能够实现优势互补，但其过度依赖gnss，当卫星信号被频繁遮挡中断时，组合导航系统退化为ins惯性导航系统；惯性导航系统可通过机械编排获取载体状态信息，但受惯性测量单元噪声影响，其定位误差会随时间累积，无法提供长时高精度位置信息。上述方案虽然解决了定位精准的问题，但还存在卫星无信号时不能提供长时高精度位置信息的问题。二是视觉slam能够通过跟踪遮挡物上丰富的视觉特征为载体提供高精度局部位置信息，减少惯性导航系统的误差漂移。
4.为此，公开号为cn115523920a的专利公开了一种基于视觉惯性gnss紧耦合的无缝定位方法，该方法将上述两种方案结合起来，用于改善现有定位中断和长时误差积累导致的定位不准确的缺陷。该方案的优化模型是以所有残差项最小为优化目标，以此来得到载体的精准位姿。但是该优化模型是以所有残差项最优为目标的，导致模型在建立过程较为复杂。


技术实现要素：

5.本发明的目的在于提供一种基于视觉惯性gnss ppp耦合的定位方法，以解决现有技术中定位优化模型较为复杂技术问题。
6.为实现上述目的，本发明所提供的一种基于视觉惯性gnss ppp耦合的定位方法的技术方案是：
7.一种基于视觉惯性gnss ppp耦合的定位方法，该方法包括有如下步骤：1)对gnss量测值进行处理得到多普勒频移、伪距和载波相位，对imu量测值进行预积分处理，对图像数据进行特征提取和匹配，利用gnss量测值构成的gnss误差方程、利用图像数据进行特征提取和匹配的结果构成视觉重投影误差方程、利用imu预积分结果构成imu误差方程；2)利用当地地理坐标系来实现gnss坐标系和视觉惯性坐标系转换；3)以图像帧时刻载体状态量为待估参数，利用gnss误差方程、视觉重投影误差方程、imu误差方程和滑动窗口产生的边缘化误差方程基于gnss坐标系和视觉惯性坐标系的转换构建图优化模型，图优化模型根据待估参数完成载体的状态估计，实现载体的定位。
8.有益效果是：通过将gnss和视觉惯性进行结合的方式解决卫星信号中断时，无法提供长时高精度位置信息。通过对数据进行预处理获取需要的数据类型，再通过视觉初始
化，以当地地理坐标系为桥梁完成gnss坐标系与视觉惯性坐标系的转化，简化了两个坐标系的转化过程，使得gnss坐标系与视觉惯性坐标系的转化过程相比现有技术更简洁。通过多项误差方程建立图优化模型，完成载体的状态估计，图优化模型相比现有技术优化问题更简单。
9.作为进一步地改进，通过图优化模型完成载体状态估计的过程：检测滑动窗口内的新图像是不是关键帧，若是关键帧，则用保留新图像时刻的相关数据，并删除滑窗内最早图像时刻的相关数据，若不是关键帧，则删除新图像时刻的相关数据，保留滑窗内原有各图像时刻的相关数据；所述相关数据指的是gnss量测值、imu量测值、图像数据、gnss误差方程、视觉重投影误差方程、imu误差方程和滑动窗口产生的边缘化误差方程。
10.有益效果是：通过识别关键帧，确定是否保留新增图片时刻对应的相关数据，这种方式在长时间的获取图像过程中能够保证图像数据的准确性，避免了在长时间的定位中，积累的各种误差，保障了定位的精准度。
11.作为进一步地改进，在构建cnss误差方程时，需要通过时间内插，得到与gnss一致的视觉惯性量测时刻。
12.有益效果是：由于cnss和视觉惯性获取数据频率不一致，cnss量测数据的时刻对应的视觉惯性不一定有量测值，通过时间内插的简单操作，实现了cnss和视觉惯性量测数据的对应性。
13.作为进一步地改进，gnss坐标系和视觉惯性坐标系转换过程：选定一个定位点，将定位点作为当地地理坐标的原点建立一个gnss坐标系与当地地理坐标系的转换矩阵，并利用该转换矩阵实现gnss坐标系与当地地理坐标系的转换；再使得视觉惯性坐标系与当地地理坐标系内的载体速度一致，建立一个航向角优化函数，根据航向角的优化函数得到航向角，利用航向角建立一个视觉惯性坐标系与当地地理坐标系转换矩阵，并利用该转换矩阵实现视觉惯性坐标系与当地地理坐标系的转换；从而实现gnss坐标系和视觉惯性坐标系转换。
14.有益效果是：利用定位点得到gnss坐标系与当地地理坐标系的旋转矩阵；利用航向角的当地地理坐标系与视觉惯性坐标系的转转矩阵，利用旋转矩阵的实现对应的两个坐标系的转化，以当地地理坐标系为桥梁实现gnss和视觉惯性坐标系的转换。提高了gnss和视觉惯性坐标系的转换的效率。
15.作为进一步地改进，航向角优化函数为：
[0016][0017]
其中，为卫星j在gnss坐标系下的速度，为定位点在视觉惯性坐标系下的速度，和分别为卫星钟漂和定位点钟漂，c为光速，λ为载波的波长，为多普勒频移的量测值，为卫星和定位点位置计算得到，为当地地理坐标系与视觉惯性坐标系之间的旋转矩阵，为gnss坐标系与当地地理坐标系的旋转矩阵。
[0018]
有益效果是：通过gnss和视觉惯性两个系统的数据构建以运行速度为目标的优化函数，通过求解优化函数实现得到最优航向角，使得利用航向角得到的当地地理坐标系与
视觉惯性坐标系的统一更准确。
[0019]
作为进一步地改进，视觉惯性采用基于滑窗优化的vins-mono算法。
[0020]
作为进一步地改进，待估参数为：
[0021][0022][0023][0024]
其中，m为滑动窗口中观测历元个数，s为地物点的个数，k为涉及到的卫星模糊度的个数，f为地物点在相机坐标系下的逆深度，n为载波相位中卫星的模糊度，φ分别为定位点和航向角，xm为m时刻载体的状态量，dtm、dt分别为位置、速度、姿态、加速度计和陀螺仪零偏、接收机钟差和对流层湿延迟，dtm为m时刻接收机时钟钟差。
[0025]
有益效果是：通过得到的待估参数，精准确定载体的实际状态，该待估参数相较于现有技术的待估参数更全面，用待估参数描述的载体位置越准确。
附图说明
[0026]
图1为基于视觉惯性gnss ppp耦合的定位方法的结构示意图；
[0027]
图2为基于视觉惯性gnss ppp耦合的定位方法中视觉与gnss采样时间示意图；
[0028]
图3为基于视觉惯性gnss ppp耦合的定位方法中的优化模型示意图；
[0029]
图4为基于视觉惯性gnss ppp耦合的定位方法中的实验轨迹1图；
[0030]
图5为基于视觉惯性gnss ppp耦合的定位方法中的误差对比图1；
[0031]
图6为基于视觉惯性gnss ppp耦合的定位方法中的误差分布图1；
[0032]
图7为基于视觉惯性gnss ppp耦合的定位方法中的实验轨迹2图；
[0033]
图8为基于视觉惯性gnss ppp耦合的定位方法中的误差对比图2；
[0034]
图9为基于视觉惯性gnss ppp耦合的定位方法中的误差分布图2。
具体实施方式
[0035]
为了使本发明的目的、技术方案及优点更加清楚明了，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明，即所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
[0036]
基于视觉惯性gnss ppp耦合的定位方法实施例：
[0037]
如图1所示，基于视觉惯性gnss ppp耦合的定位方法，该方法采用gnss和视觉惯性slam结合的框架，该框架包括有数据预处理、初始化和图优化三部分。
[0038]
其中，预处理包括有对gnss量测值进行处理得到多普勒频移、伪距和载波相位，对imu量测值进行预积分处理，对图像数据进行特征提取和匹配。利用gnss量测值构成的gnss误差方程、利用图像数据进行特征提取和匹配的结果构成视觉重投影误差方程、利用imu预积分结果构成imu误差方程；通过预处理为后续的初始化提供数据支持。
[0039]
初始化时，以当地地理坐标系为桥梁，实现gnss和视觉惯性坐标系的统一。
[0040]
图优化时，以图像帧时刻载体状态量为待估参数，利用gnss误差方程、视觉重投影误差方程、imu误差方程和滑动窗口产生的边缘化误差方程基于gnss坐标系和视觉惯性坐标系的转换构建图优化模型，图优化模型根据待估参数完成载体的状态估计，实现载体的定位。下面对数据预处理、初始化、图优化部分进行具体说明。
[0041]
1)数据预处理。
[0042]
数据预处理主要对各项误差方程进行说明。
[0043]
以i历元卫星j观测值构成的伪距误差方程e
p,i,j
、载波相位误差方程e
φ,i,j
和多普勒频移误差方程e
d,i,j
为例，gnss误差方程的具体表达形式可分别表示为：
[0044][0045][0046][0047]
其中，
[0048][0049]
其中，和分别为卫星j在i历元的位置、速度、卫星钟漂和接收机钟漂，可通过广播星历计算得到；为接收机钟差的系数矩阵，可根据卫星所属系统设置。
[0050]
伪距误差仅与当前时刻状态量x相关，而载波相位误差不仅与当前时刻状态量相关，还与模糊度n相关。当转换颗卫星被连续观测且没有周跳时，模糊度是不变的，当有新卫星出现或产生周跳时，会出现新的模糊度变量，所以多个历元的载波相位误差因子可能链接转换个模糊度变量。因此，系统滑窗时会把载波相位对待边缘化历元载体状态和模糊度的约束转换为模糊度间的约束，并把这种约束引入先验误差方程。
[0051]
该重投影误差方程为：
[0052][0053]
其中，f
x
,fy,c
x
,cy分别为相机的内参数，为空间点j在局部世界系的坐标，pj＝[u
j vj]
t
为空间点对应的像素坐标，为空间点在相机系的坐标，ri,ti分别为i历元载体的姿态和位置。
[0054]
该预积分误差方程为：
[0055][0056]
其中，分别为i时刻载体相对于局部世界系的位置、速度和姿态，gw为地球重力在局部世界系的矢量，δt为相机采样间隔，为i时刻到i+1时刻imu预积分量，分别为i时刻、i+1时刻加速度计和陀螺仪的零偏。
[0057]
该边缘化误差方程为：
[0058]er
＝[b-ec-1et
]x-(v-ec-1
w)
[0059]
式中，b为未被边缘化参数的海森矩阵，c为待边缘化参数的海森矩阵，e为未被边缘化和待边缘化参数间的海森矩阵，v和w分别为的滑窗时增量方程中未被边缘化参数和待边缘化参数雅可比矩阵与误差的乘积。
[0060]
2)初始化。
[0061]
初始化的主要目的是实现gnss和视觉惯性坐标系的转化。利用当地地理坐标系来实现gnss坐标系和视觉惯性坐标系转换。通过滑窗内图像帧的位姿来估计加速度计与陀螺仪的零偏、惯导本身的噪声和运动速度、视觉惯性里程计参考坐标系与gnss参考坐标系之间的变换矩阵来实现上述目的。
[0062]
其中，gnss与vio视觉初始化需要获得gnss参考坐标系与视觉惯性里程计参考坐标系间的旋转矩阵和平移向量求解旋转矩阵和平移向量可以分成两部分，先获取gnss参考坐标系与当地地理坐标系之间旋转矩阵和平移向量再当地地理坐标系与视觉惯性里程计参考坐标系之间旋转矩阵通过gnss参考坐标系求解当地地理坐标系(东-北-天)时，只需一个定位点就可得到以该锚点为原点的当地地理坐标系；由于当地地理坐标系与视觉惯性里程计世界系的z轴均与重力方向平行，即当地地理坐标系与世界系之间仅相差一个航向角φ。综上，只需求解定位点和航向角就能实现gnss和视觉惯性坐标系的统一。
[0063]
定位点的确定过程：在第一个滑动窗口内进行精密单点定位时，需要选择一个历元的位置坐标作为当地地理坐标系的原点。本发明以滑窗内第五个精密单点定位结果为定位点坐标。设定位点的地理坐标为则以该定位点为原点的当地地理系与gnss参考坐标系的转换矩阵为：
[0064][0065]
据此，可将地理坐标系的结果转换至gnss参考坐标系：
[0066]
[0067]
航向角的确定过程：获取当地地理系与视觉惯性里程计参考坐标系间的航向角时，使滑窗内载体的运动速度与多普勒频移解算的运动速度一致，得到航向角φ，用公式表示为：
[0068][0069]
其中，为卫星j在gnss坐标系下的速度，为定位点在视觉惯性坐标系下的速度，和分别为卫星钟漂和定位点钟漂，c为光速，λ为载波的波长，为多普勒频移的量测值，为卫星和定位点位置计算得到，为当地地理坐标系与视觉惯性坐标系之间的旋转矩阵，为gnss坐标系与当地地理坐标系的旋转矩阵。
[0070]
依据航向角φ建立视觉惯性里程计参考坐标系与当地地理坐标系之间的旋转矩阵
[0071][0072]
gnss与vio初始化得到的锚点与航向角只是滑窗内基于现有条件得到的最优估值，并不是真正意义上的最优估值，需要在后端图优化过程中，以初始化结果作为初值，不断迭代优化调整，实现vio视觉惯性里程计参考坐标系与gnss参考坐标系的统一。
[0073]
3)图优化
[0074]
以图像帧时刻载体状态量为待估参数，利用gnss量测量构成的误差方程、视觉重投影误差方程、imu预积分误差方程和滑动窗口产生的边缘化误差方程构建图优化模型，图优化模型根据待估参数完成载体的状态估计。
[0075]
确定待估参数为：
[0076][0077][0078][0079]
其中，m为滑动窗口中观测历元个数，s为地物点的个数，k为涉及到的卫星模糊度的个数，f为地物点在相机坐标系下的逆深度，n为载波相位中卫星的模糊度，φ分别为定位点和航向角，xm为m时刻载体的状态量，dtm、dt分别为位置、速度、姿态、加速度计和陀螺仪零偏、接收机钟差和对流层湿延迟，dtm为m时刻接收机时钟钟差。上述的时钟钟差包括gps、glonass、galileo和bds四个系统的接收机时钟钟差。
[0080]
与现有技术相比本发明新引入的待估参数包括接收机钟差、对流层湿延迟、模糊度和视觉惯性里程计参考坐标系gnss参考坐标系变换所需的航向角和定位点。
[0081]
系统代价函数c(χ)为
[0082][0083]
其中，cc(χ)和ci(χ)分别为重投影误差方程和imu预积分误差方程构成的关于视觉与惯性的代价函数，具体可参照vins-mono算法。
[0084]
获取各个误差方程，er为先验约束误差方程，主要是由滑动窗口过程中的边缘化产生，wr为先验约束对应的信息矩阵；m,n分别为滑动窗口中观测历元的个数和每个历元卫星的个数；其余部分属于新添加的gnss误差方程。
[0085]
如图2所示的视觉与gnss采样时间示意图所示，本发明以相机采样时刻载体状态量为待估参数，然而通常情况下，相机与gnss采样时间并不同步。因此，在构建gnss误差方程时，需要通过时间内插出gnss采样时刻的位置和速度
[0086][0087][0088]
其中，
[0089][0090]
式中，ti为i历元gnss对应的时间戳，t
m-1
,tm为图像帧m-1和m对应的时间戳，为i历元gnss相位中心相对于局部世界系运动的位置和速度；为图像帧m-1和m时刻载体相对于局部世界系运动的位置和速度。
[0091]
具体的图优化模型如图3所示，为了保持图形整洁，这里并未将地图点、定位点、航向角和视觉惯性产生的先验误差显示出来。考虑到图像关键帧选取问题，当新增一个历元的状态量时，以窗口中次新帧图像是否为关键帧为依据，决定抛弃和边缘化哪一历元相关数据。若滑动窗口内次新帧图像为关键帧，则将窗口内只与第一帧图像相关数据边缘化，并抛弃第一帧图像时刻对应的相关数据；若窗口中次新帧图像不是关键帧，则当接收到新一帧图像时，抛弃采集次新帧图像时刻对应的观测值，并将对应时刻的相关数据边缘化。上述的相关数据指的是gnss量测值、imu量测值、图像数据、gnss误差方程、视觉重投影误差方程、imu误差方程和滑动窗口产生的边缘化误差方程。
[0092]
为验证基于视觉惯性gnss ppp耦合的定位方法的效果，设计两个对比实验如下：
[0093]
(1)实验一
[0094]
在河南省郑州市开展实测车载实验，将解算结果与ppp、gvins定位结果进行对比分析。其中，ppp解算时使用gfz的多系统精密产品，包括卫星精密轨道、精密钟差和地球自转参数产品，并按照igs推荐的标准模型对天线相位中心、对流层干延迟、相位缠绕、海潮固体潮等误差进行了改正。
[0095]
实验所用数据采集平台搭载战术级惯导、多频多模gnss和工业相机等多种传感器，通过高强度铝合金材料和高精度数控机床精密加工、含有高精度fpga系统和精密晶振的高精度时间同步板卡，实现传感器间毫米级空间同步和微秒级时间同步。
[0096]
主传感器参数见表1，参考轨迹为商业软件ie提供的厘米级gnss/imu松组合定位结果。
[0097]
表1
[0098][0099][0100]
实验轨迹如图4所示，实验时车辆沿校园环道行驶2560s，道路两旁有树木、建筑物等遮挡，部分道路狭窄，是城市中较为常见的场景。在这种场景下，根据道路宽度、树木和建筑物高度的不同，卫星信号会受到不同程度的影响，为卫星定位和组合导航带来挑战，但同时为视觉里程计提供丰富的视觉特征。
[0101]
图5给出了ppp、gvins和本文定位误差对比图。从图中可知，gvins不存在收敛过程，这是因为gvins只融合了gnss原始数据中的伪距和多普勒频移，没有载波相位模糊度；而ppp和pvins虽然融合了载波相位，并且具有收敛过程，但相较而言，pvins收敛时间较短，因为视觉惯性里程计中多余观测的存在一定程度上辅助了模糊度的快速收敛。另外，在卫星信号易受遮挡区域视觉特征较为丰富，利于视觉里程计定位，但gvins系统绝对位置依靠伪距，所以当卫星信号受到遮挡而中断时，gvins和ppp整体定位误差会显著增大；而pvins不仅能够利用遮挡物的视觉特征完成特征跟踪，还能通过边缘化理论将历史观测值对模糊度的观测转换为模糊度间的约束作为当前时刻位姿估计的先验信息，实现视觉惯性里程计与精密单点定位的优势互补，因此pvins系统在卫星遮挡区域依然能够获得较高的定位信息。
[0102]
为进一步评估三者的定位性能，图6和表2对比了定位误差分布及其统计值。可以看出，对于转换组数据，pvins系统定位误差最大值约1.42米，而gvins和ppp定位误差小于该值的比例分别为89.61％、55.93％。而且pvins系统定位精度最高，其对应的均方根误差、平均绝对误差和标准差分别为0.68m、0.43m和0.49m，相较于定位精度较高的gvins，pvins定位精度分别提升56.96％、55.67％和60.16％。这说明在卫星信号易受遮挡的环境，含有视觉里程计的pvins系统和gvins系统定位精度要远高于ppp，且相较而言pvins系统定位精度最高，gvins次之，ppp最差。
[0103]
表2ppp、gvins和pvins定位误差(m)
[0104]
[0105][0106]
(2)实验二
[0107]
图7为车载实验二中车辆行驶4030秒的轨迹图，道路两旁几乎没有高大的树木和建筑物，只有交叉分布的路灯和部分低矮小树，场景中卫星信号几乎不受遮挡，而且相机采集的图像中具有满足视觉里程计定位的特征信息。
[0108]
如图8所示，对比了ppp、gvins和pvins在东-北-天三个方向的定位误差。由图可知，与实验一结果一致，ppp和pvins存在收敛过程，但pvins收敛更快，而gvins则不存在模糊度收敛过程。不同的是，gvins算法在北向和天向的定位误差明显降低，但并非由视觉跟踪丢失或卫星信号中断引起的跳变，而是误差整体偏移。这是因为gvins本身绝对定位精度不高，再加上初始时刻场景中视觉特征不太丰富且距离载体自身较远，视觉惯性里程计对绝对位置不敏感，导致通过两者对齐解算出的坐标系间转换矩阵并不准确，但pvins并未出现这种情况，说明在视觉特征不丰富或质量不高时，pvins能够依靠其绝对定位精度高的优势，为系统提供更准确的坐标系间转换矩阵。另外，对比ppp和pvins定位误差图可以发现，载波相位模糊度收敛后，两者定位误差相差不大，只是pvins定位误差更为平滑，这是因为pvins中视觉惯性里程计本质上是一种递推的定位方式，对相邻时刻载体状态量约束，而ppp几乎没有这种限制。
[0109]
如图9所示，统计了三个系统的定位误差分布及其统计值。由表3可知，pvins定位精度最高，均方根误差、平均绝对误差、标准差分别为0.23m、0.19m、0.13m，相较于定位精度较高的ppp，pvins定位精度分别提升54.90％、51.28％、58.06％。由图可知，ppp、gvins和pvins三个系统概率分布为90％时对应的定位误差分别小于0.49m、1.45m和0.28m。整体而言，pvins算法定位精度最高，ppp算法次之，gvins算法最差。这说明在卫星信号质量较好时，利用精密单点定位原理解算载体绝对位置的系统能够获得更高的绝对定位精度，而且即使视觉特征不丰富，视觉惯性里程计累积误差较大，将其与ppp从原始观测值层面紧耦合构成pvins后，系统不仅能够获得更高的绝对定位精度，而且能够在视觉惯性里程计的辅助下快速收敛，整体定位误差更加连续和平滑。
[0110]
表3ppp、gvins和pvins定位误差(m)
[0111][0112][0113]
实验结果，为满足城市环境高精度定位需求，解决卫星信号易受遮挡、视觉惯性里程计累积误差大的问题，本发明提出的利用精密单点定位和多普勒频移测速原理将gnss原始观测值中的伪距、载波相位和多普勒频移以误差方程的形式引入vins-mono，构建基于图
优化的gnss ppp/ins/视觉紧耦合定位方法。同时在方法中考虑了视觉图像与gnss观测值不同时采集的特点和图像关键帧筛选对边缘化的影响。实测实验结果表明，相较于标准ppp和gvins，本文所提方法能够更好地适应不同城市环境，充分利用所有观测信息，间接辅助模糊度快速收敛，提升多源融合定位方法的精确性和鲁棒性。
[0114]
最后需要说明的是，以上仅为本发明的优选实施例，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细地说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行不需付出创造性劳动地修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于视觉惯性gnss ppp耦合的定位方法，其特征在于，该方法包括有如下步骤：1)对gnss量测值进行处理得到多普勒频移、伪距和载波相位，对imu量测值进行预积分处理，对图像数据进行特征提取和匹配，利用gnss量测值构成的gnss误差方程、利用图像数据进行特征提取和匹配的结果构成视觉重投影误差方程、利用imu预积分结果构成imu误差方程；2)利用当地地理坐标系来实现gnss坐标系和视觉惯性坐标系转换；3)以图像帧时刻载体状态量为待估参数，利用gnss误差方程、视觉重投影误差方程、imu误差方程和滑动窗口产生的边缘化误差方程基于gnss坐标系和视觉惯性坐标系的转换构建图优化模型，图优化模型根据待估参数完成载体的状态估计，实现载体的定位。2.根据权利要求1所述的基于视觉惯性gnss ppp耦合的定位方法，其特征在于，通过图优化模型完成载体状态估计的过程：检测滑动窗口内的新图像是不是关键帧，若是关键帧，则用保留新图像时刻的相关数据，并删除滑窗内最早图像时刻的相关数据，若不是关键帧，则删除新图像时刻的相关数据，保留滑窗内原有各图像时刻的相关数据；所述相关数据指的是gnss量测值、imu量测值、图像数据、gnss误差方程、视觉重投影误差方程、imu误差方程和滑动窗口产生的边缘化误差方程。3.根据权利要求1所述的基于视觉惯性gnss ppp耦合的定位方法，其特征在于，在构建cnss误差方程时，需要通过时间内插，得到与视觉惯性一致的gnss量测时刻。4.根据权利要求1所述的基于视觉惯性gnss ppp耦合的定位方法，其特征在于，gnss坐标系和视觉惯性坐标系转换过程：选定一个定位点，将定位点作为当地地理坐标的原点建立一个gnss坐标系与当地地理坐标系的转换矩阵，并利用该转换矩阵实现gnss坐标系与当地地理坐标系的转换；再使得视觉惯性坐标系与当地地理坐标系内的载体速度一致，建立一个航向角优化函数，根据航向角的优化函数得到航向角，利用航向角建立一个视觉惯性坐标系与当地地理坐标系转换矩阵，并利用该转换矩阵实现视觉惯性坐标系与当地地理坐标系的转换；从而实现gnss坐标系和视觉惯性坐标系转换。5.根据权利要求4所述的基于视觉惯性gnss ppp耦合的定位方法，其特征在于，所述航向角优化函数为：其中，为卫星j在gnss坐标系下的速度，为定位点在视觉惯性坐标系下的速度，和分别为卫星钟漂和定位点钟漂，c为光速，λ为载波的波长，为多普勒频移的量测值，为卫星和定位点位置计算得到，为当地地理坐标系与视觉惯性坐标系之间的旋转矩阵，为gnss坐标系与当地地理坐标系的旋转矩阵。6.根据权利要求1所述的基于视觉惯性gnss ppp耦合的定位方法，其特征在于，所述视觉惯性采用基于滑窗优化的vins-mono算法。7.根据权利要求1-6任意一项所述的基于视觉惯性gnss ppp耦合的定位方法，其特征在于，所述待估参数用集合表示为：
其中，m为滑动窗口中观测历元个数，s为地物点的个数，k为涉及到的卫星模糊度的个数，f为地物点在相机坐标系下的逆深度，n为载波相位中卫星的模糊度，φ分别为定位点和航向角，x
m
为m时刻载体的状态量，dt
m
、dt分别为位置、速度、姿态、加速度计和陀螺仪零偏、接收机钟差和对流层湿延迟，dt
m
为m时刻接收机时钟钟差。

技术总结
本发明设计一种基于视觉惯性GNSS PPP耦合的定位方法，属于卫星定位系统领域，一种基于视觉惯性GNSS PPP耦合的定位方法，将GNSS和视觉惯性SLAM进行结合作为整体框架；进行数据预处理、进行初始化、进行图优化三个部分，通过预处理得到初始化需要的数据类型，在初始化时以当地地理坐标系为桥梁实现GNSS坐标系和视觉惯性坐标系的统一，利用定位点实现GNSS坐标系与当地地理坐标系的转换，利用航向角实现当地地理坐标系与视觉惯性坐标系的统一，通过对航向角设置优化函数保障GNSS坐标系和视觉惯性坐标系的统一的准确性。通过构建的图优化模型，完成载体的状态估计。通过将GNSS和视觉惯性SLAM进行结合的方式解决卫星信号中断时，无法提供长时高精度位置信息的问题。法提供长时高精度位置信息的问题。法提供长时高精度位置信息的问题。


技术研发人员：肖国锐 赵冬青 贾晓雪
受保护的技术使用者：中国人民解放军战略支援部队信息工程大学
技术研发日：2023.06.16
技术公布日：2023/9/16