一种自适应融合单频RTK的视觉惯性定位方法和设备

一种自适应融合单频rtk的视觉惯性定位方法和设备
技术领域
1.本发明涉及定位技术领域，尤其是涉及一种自适应融合单频rtk的视觉惯性定位方法和设备。


背景技术：

2.自主定位是移动机器人领域的关键技术，准确实时的定位结果是实现后续感知、规划和控制的最基本前提。移动机器人的自主定位依赖所使用的传感器，为提升整体性能，多传感器融合是一种通用且有效的手段。基于单目相机和惯性测量单元(inertial measurement unite,imu)融合的视觉惯性定位方法，拥有局部精度高、计算实时性好，以及尺寸小、重量轻、功耗低、成本低等优点，在移动机器人领域得到了广泛应用。
3.中国专利申请号cn202210737648.1公开了一种基于视觉-imu融合的多移动机器人协同定位方法及系统。本方法，包括：步骤s1、对相机与imu进行参数标定；步骤s2、计算imu预积分，进行数据对齐；步骤s3、提取图像orb特征，根据brief描述子进行特征匹配；步骤s4、建立优化问题，求解获得机器人终端的局部坐标系下最优的位姿估计；步骤s5、将优化得到的位姿与提取的图像orb特征数据发送给服务器端；步骤s6、使用词袋模型进行回环检测；步骤s7、在全局统一坐标下进行位姿图优化，将优化结果发送至机器人终端。
4.上述申请采用词袋模型进行回环检测，最终可以输出多机器人在全局统一坐标系下的位姿。然而，视觉惯性定位方法存在全局累积漂移的问题，定位误差会随时间逐渐累积，直至完全失效。一种常见的解决方法是增加额外的回环检测步骤，但该方法的计算成本较高，会影响实时性。而且需要注意的是，在实际应用中，移动机器人的路径是不一定存在回环的，这就大大限制了该方法的通用性。


技术实现要素：

5.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种自适应融合单频实时动态(real-time kinematic,rtk)的视觉惯性定位方法和设备，以解决或部分解决现有的视觉惯性定位方法通用性低的问题。
6.本发明的目的可以通过以下技术方案来实现：
7.本发明的一个方面，提供了一种自适应融合单频rtk的视觉惯性定位方法，包括如下步骤：
8.基于滑动窗口对多个关键帧进行多次滑动测量，在一次测量过程中，利用基于因子图的非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据，实现视觉惯性定位，通过单频rtk算法计算在全局坐标系下的轨迹，计算本次测量下的局部坐标系与全局坐标系的变换数据，基于状态数据以及变换数据构建本次测量的测量状态信息；
9.基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入所述非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。
10.作为优选的技术方案，所述的rtk残差的获取包括如下步骤：
11.判断所述单频rtk算法是否有固定解，若是，基于变换数据、rtk算法得到的固定解以及视觉惯性定位得到的局部坐标系下的坐标，通过解最小二乘问题计算松耦合的位置约束残差，作为所述的rtk残差，若否，所述的rtk残差包括紧耦合的多普勒测量残差和双差伪距测量残差。
12.作为优选的技术方案，计算局部坐标系与全局坐标系的变换数据的过程包括如下步骤：
13.判断所述单频rtk算法是否有固定解，若是，通过对应点集配准对局部坐标系和全局坐标系下的轨迹进行对齐，计算坐标系间旋转变换项和平移变换项，作为所述的变换数据，若否，首先使用粗略位置坐标和多普勒测量对齐旋转，然后使用双差伪距测量对齐平移，计算坐标系间旋转变换项和平移变换项，作为所述的变换数据。
14.作为优选的技术方案，所述的残差项还包括点特征重投影残差、线特征重投影残差、imu预积分残差中的至少一个。
15.作为优选的技术方案，通过单频rtk算法计算在全局坐标系下的轨迹具体包括如下步骤：
16.流动站接收机将高度角小于预设角度的卫星剔除，并在gps和bds中各选择一颗最高仰角卫星作为参考卫星；
17.流动站接收机将自身对某卫星的测量，与来自基准站接收机对同卫星的测量作差，得到卫星的站间单差伪距测量和单差载波相位测量，将各星座内其余所有卫星的单差测量，与其参考卫星的单差测量作差，得到双差测量；
18.基于双差测量执行单频rtk算法，得到在全局坐标系下3自由度位置的轨迹。
19.作为优选的技术方案，利用基于因子图的非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据的过程具体包括如下步骤：
20.获取多帧图像，通过点特征提取和跟踪，基于预设的关键帧选取规则选取关键帧，计算imu预积分项；
21.基于视觉sfm获取位姿信息并与imu(inertial measurement unit)预积分项对齐；
22.构建基于因子图的非线性优化模型，通过最小化滑动窗口中的所有因子，得到车辆位姿、速度、点特征位置和imu偏差最优佳估计，作为所述的状态数据，实现视觉惯性定位。
23.作为优选的技术方案，所述的关键帧的选取过程中还包括线特征提取，通过点特征和线特征的提取及跟踪，基于预设的关键帧选取规则选取关键帧。
24.作为优选的技术方案，所述的最佳估计的测量状态信息包括车辆位姿、速度、点特征位置、imu偏差、rtk接收机时钟漂移以及全局坐标系与局部坐标系之间的变换。
25.作为优选的技术方案，所述的局部坐标系为imu坐标系的初始位姿，所述的全局坐标系为东北天坐标系。
26.本发明的另一个方面，提供了一种自适应融合单频rtk的视觉惯性定位设备，包括工控机以及与所述工控机连接的单目相机、imu和rtk流动站接收机及接收天线，所述单目相机和imu层叠设置且并位于车身纵向中心线上，所述rtk接收天线与rtk流动站接收机连
接，所述的工控机用于从所述单目相机获取多帧图像并筛选多个关键帧，基于滑动窗口对多个关键帧进行多次滑动测量，构建本次测量的测量状态信息，基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。
27.与现有技术相比，本发明具有以下优点：
28.(1)通用性高，使用场景广：不同于现有的通过增加额外的回环检测步骤以减少全局累积漂移的方案，本发明首先利用在非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据，即局部坐标系下的轨迹，实现基本的视觉惯性定位，然后利用rtk算法计算在全局坐标系下的轨迹，得到变换数据，最后计算rtk残差并根据单频rtk算法是否有固定解自适应融入非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。通过根据单频rtk算法是否有固定解将rtk残差自适应融入在基于因子图的非线性优化模型中，再通过最小化包括rtk残差的残差项实现定位，由于rtk不要求移动的路径存在回环，解决或部分解决了现有的采用回环检测存在的通用性低的问题。
29.(2)定位精度高：当单频rtk算法有固定解时，将松耦合的位置约束残差作为rtk残差，否则将紧耦合的多普勒测量残差和双差伪距测量残差作为rtk残差，通过单频rtk解的状态选择不同的初始化方法和融合方法，这种策略可以最大程度发挥单频rtk的优势，从而提高本方法的定位精度和实时性。
附图说明
30.图1为实施例中自适应融合单频rtk的视觉惯性定位方法的实现步骤流程图；
31.图2为实施例中传感器设备安装示意图；
32.图3为单频rtk与视觉惯性之间的初始化示意图；
33.图4为单频rtk与视觉惯性之间的自适应初始化流程图；
34.图5为单频rtk与视觉惯性之间的因子图优化示意图；
35.图6为单频rtk与视觉惯性之间的自适应融合流程图。
具体实施方式
36.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
37.实施例1
38.本实施例针对现有视觉惯性定位方法存在累计漂移的问题，以及传统的使用回环检测步骤计算成本高、不具备通用性的问题，提供一种自适应融合单频rtk的视觉惯性定位方法，实现流程如图1所示，具体步骤如下：
39.步骤s1：设备安装。
40.将单目相机、imu和rtk接收天线安装在汽车顶部，如图2所示。其中，相机和imu的
安装位置近似重合，并位于车身纵向中心线上。rtk接收天线有两个，对称布置在车身纵向中心线两侧，并与相机和imu位于一条直线上，左侧的用来定位，右侧的用来定向，本实施例中仅使用左侧的定位天线。rtk流动站接收机安装在汽车后备箱中，使用一台工控机作为计算平台，同样安装在汽车后备箱中。使用汽车自身电池为所有设备供电。
41.相机坐标系的原点位于光心处，定义x轴指向车辆正右方，y轴指向车辆正下方，z轴指向车辆正前方。imu坐标系的原点位于imu质心处，定义x轴指向正右方，y轴指向正前方，z轴指向正上方。rtk定位天线坐标系的原点位于天线相位中心，各轴的指向可任意定义。通过校准得到相机坐标系与imu坐标系之间的变换矩阵t
cb
，以及rtk定位天线坐标系与imu坐标系之间的平移p
rb
。统一使用imu坐标系作为定位本体坐标系，也称为载体坐标系或汽车车辆坐标系。rtk基准站接收机及其天线安装在汽车行驶区域附近的某一空旷处，并借助当地基准站服务商实现静态多频rtk测量算法，得到自身的精准位置坐标。
42.步骤s2：数据采集。
43.相机采集车辆行驶过程中正前方的图像，采集频率为20hz；imu采集车辆行驶过程中的线加速度和角速度，采集频率为200hz。rtk流动站接收机采集自身与卫星之间的多普勒测量、伪距测量和单频载波相位测量，同时接收基准站接收机采集的它自身与卫星之间的伪距测量和单频载波相位测量，频率均为1hz。卫星包括两个星座，美国的gps以及中国的北斗，具体频点为gps l1c和bds-2b1i。所有测量数据统一传入工控机，工控机安装ubuntu系统，基于ros(robot operating system)进行数据之间的通信。
44.步骤s3：基于相机与imu实现视觉惯性定位功能。
45.该步骤的目的是将相机和imu进行融合，从而得到车辆在局部坐标系下的轨迹，实现基本的视觉惯性定位功能。
46.(1)相机与imu的预处理
47.首先，使用shi-tomasi算法对第一帧图像进行点特征提取，数量上限设为300。之后，使用lucas-kanade稀疏光流算法对下一帧图像进行点特征跟踪，如果跟踪到的点特征数量少于50，则继续使用shi-tomasi算法对该帧图像进行点特征提取，保证每帧图像中点特征的总数处于100～300之间。在采集的图像中选取关键帧，选取原则是某帧图像中跟踪到的点特征数小于20个，或跟踪到的点特征的平均视差大于10像素。对上述选取的每两个关键帧之间的所有imu测量进行预积分，得到imu预积分项。
48.(2)相机与imu的初始化
49.该步骤的目的，是将本步骤中基于纯视觉sfm(structure from motion)得到的相机(车辆)位姿与上个步骤中的imu预积分项对齐。首先，算法检查当前关键帧和滑动窗口中的所有先前关键帧之间的点特征对应关系，如果点特征的跟踪数量均超过30，并且平均视差均超过20个像素，则基于对极几何约束，得到当前帧与上一帧之间的位姿变换。之后，对两帧图像中所有共视点特征进行三角测量，恢复点特征的深度，得到其三维位置坐标。接下来，使用pnp(perspective-n-point)算法得到窗口中所有关键帧的位姿。然后，最小化所有点特征重投影残差，得到最优的相机位姿和点特征位置。由于使用的是imu坐标系来表示车辆坐标系，所以需要将其改用imu坐标系表示。最后将位姿变换结果与imu预积分项对齐，以恢复其中平移部分的尺度，并得到车辆速度、重力向量和陀螺仪偏差的初始值。
50.(3)相机与imu的融合
51.构建基于因子图的非线性优化模型，通过最小化滑动窗口中的所有因子(点特征重投影残差和imu预积分残差)，得到所有状态(车辆位姿、速度、点特征位置和imu偏差)的最优估计，实现基本的视觉惯性定位功能，这也被称为视觉惯性里程计(vio)，可以得到自动驾驶汽车在局部世界(world)坐标系下轨迹(6自由度位姿)。
52.步骤s4：单频rtk接收机的预处理。
53.首先，流动站接收机将高度角小于15度的卫星剔除，并在gps和bds中各选择一颗最高仰角卫星作为参考卫星。之后，流动站接收机将自身对某卫星的测量，与来自基准站接收机对同卫星的测量作差，得到该卫星的站间单差伪距测量和单差载波相位测量。接着将各星座内其余所有卫星的单差测量，与其参考卫星的单差测量作差，得到双差测量。最后，基于双差测量执行单频rtk算法，可以得到自动驾驶汽车在全局地心地固(ecef)坐标系下的轨迹(3自由度位置)。进而可以通过设置锚点，将其变换到东北天(enu)坐标系下。
54.步骤s5：单频rtk与视觉惯性之间的自适应初始化
55.初始化的目的是将上述步骤3得到的world坐标系下的轨迹与步骤4得到的enu坐标系下的轨迹对齐，即求解world坐标系和enu坐标系之间的旋转和平移如图3所示。
56.(1)单频rtk得到固定解的情况
57.当单频rtk得到固定解时，由于其精度相对较高(亚米级)，选择使用对应点集配准的方法进行轨迹对齐。
58.选择第三个单频rtk固定解对应的位置坐标点作为参考点，定义它为锚点并记为在锚点处建立enu坐标系，则后续自动驾驶汽车从rtk接收机获取的位置坐标便可以通过锚点，从ecef坐标系变换到enu坐标系：
[0059][0060]
其中，是ecef坐标系到enu坐标系的旋转矩阵，具体形式为：
[0061][0062]
其中，lon和lat分别是锚点在ecef坐标系中的经度和纬度，可以从接收机中直接获取，所以可以视为已知量。现在考虑自动驾驶汽车从vio中获取的位置坐标将它从world坐标系变换到enu坐标系：
[0063][0064]
理论上，上述两个enu坐标系下的位置坐标应该相等，但由于vio存在漂移，所以还需要同时估计一个尺度参数s。在滑动窗口中将它们建模成一个最小二乘问题，代价函数为：
[0065][0066]
通过解上述最小二乘问题，得到world坐标系和enu坐标系之间的旋转和平移
[0067]
(2)单频rtk没有得到固定解的情况
[0068]
当单频rtk没有得到固定解时，由于其精度相对较低(米级)，选择使用逐级细化的方法进行对齐。即首先使用粗略的位置坐标和噪声较小的多普勒测量对齐旋转，之后再使用双差伪距测量对齐平移。
[0069]
首先，在不依靠任何先验信息的情况下，联合滑动窗口中的所有双差伪距测量，得到窗口的平均位置坐标，定义它为锚点并记为同样的，在锚点处建立enu坐标系。
[0070]
(a)使用多普勒测量对齐旋转
[0071]
将来自vio的车辆速度作为先验，通过最小化窗口内的所有多普勒测量残差，可以得到world坐标系到enu坐标系的旋转
[0072][0073]
其中，
[0074][0075][0076]
是k时刻，汽车本体bk和卫星sj之间的多普勒测量残差，q是协方差矩阵，m是卫星总数。是多普勒测量值，λ是卫星载波波长，是卫星速度。锚点坐标用于计算以及本体到卫星的单位向量因为它们对位置不敏感，所以利用粗略的锚点坐标计算精度足够。是卫星位置坐标，是卫星时钟漂移，是接收机时钟漂移。与卫星有关的参数均可通过星历得到，式中仅和未知。
[0077]
(b)使用双差伪距测量对齐平移
[0078]
将来自vio的车辆位置坐标作为先验，通过最小化窗口内的所有双差伪距测量残差，可以得到world坐标系到enu坐标系的平移并能进一步细化步骤s5-(2)开头部分计算的粗略的锚点坐标：
[0079][0080]
其中，
[0081][0082][0083]
是k时刻，汽车本体bk和基准站ref对gps卫星和的双差伪距测
量残差，是参考卫星。是对bds卫星和的双差伪距测量残差，是参考卫星。和分别是两个星座中的双差伪距测量值，和分别是基准站ref对两个星座中的卫星单差几何距离。与卫星和基准站有关的参数均已知，式中仅和未知。
[0084]
至此，全局enu坐标系与局部world坐标系之间的变换被完全校准，单频rtk和视觉惯性之间的初始化完成。
[0085]
步骤s6：单频rtk与视觉惯性之间的自适应融合。
[0086]
本发明通过在步骤s3-(3)所述的基于因子图的非线性优化模型中，增加与单频rtk相关的因子，如图5所示，通过最小化滑动窗口中的所有因子(点特征重投影残差、imu预积分残差以及与单频rtk相关的残差)，得到所有状态(车辆位姿、速度、点特征位置和imu偏差，以及rtk接收机时钟漂移、world坐标系与enu坐标系之间的变换)的最优估计。滑动窗口内的状态为：
[0087][0088]
其中，
[0089][0090][0091]
n+1和m+1分别是滑动窗口中关键帧和点特征的总数。f
l
是第l个点特征在其第一次被观测到时的逆深度。是world坐标系到enu坐标系的变换矩阵，和分别是旋转和平移。xk表示第k个关键帧的状态，包括自动驾驶汽车本体在world坐标系中的位置姿态和速度以及加速度计偏差ba和陀螺仪偏差b
ω
。由于双差测量消去了接收机时钟偏差，所以不再需要估计δtk，但由于仍用到了多普勒测量，所以需要估计时钟漂移它对不同星座是相同的。
[0092]
最优状态估计是一个最大后验概率问题，是在给定所有测量的情况下，求解满足最大后验概率的状态。假定所有测量相互独立，且噪声为零均值的高斯分布，则可进一步转化为最小化一系列代价函数之和的问题，每个代价函数对应一个特定的测量残差。具体形式如下：
[0093][0094]
其中，r
marg
是来自边缘化的先验信息，表示传感器的测量残差。其中，是imu预积分残差，是点特征重投影残差，是多普勒测量残差，是双差伪距测量残差，是rtk固定解位置约束残差。m是跟踪到的卫星总数，u和v是gps和bds中跟踪到的卫星
数，和是各自的参考卫星。表示求马氏距离，q是协方差矩阵。
[0095]
本发明提供了一种自适应融合策略，如图6所示，可以根据单频rtk的状态，即是否得到固定解，来选择不同的融合方法：当单频rtk得到固定解时，松耦合位置约束残差当单频rtk没有得到固定解时，紧耦合多普勒测量残差和双差伪距测量残差
[0096]
(1)单频rtk得到固定解的情况
[0097]
当单频rtk得到固定解时，选择松耦合它提供的位置约束。对于车辆本体从固定解获取的ecef坐标系下的坐标可以通过下式变换到enu坐标系：
[0098][0099]
同样，车辆本体从vio中获取的位置坐标系也可以通过下式变换到enu坐标系：
[0100][0101]
所以在k时刻，来自单频rtk固定解的位置约束残差为：
[0102][0103]
(2)单频rtk没有得到固定解的情况
[0104]
当单频rtk没有得到固定解时，选择紧耦合它提供的原始测量中的双差伪距测量和多普勒测量，而附带模糊度的双差载波相位测量则被忽略，以避开模糊度固定难题，提高算法效率。多普勒测量和双差伪距测量的残差模型如下：
[0105]
(a)多普勒测量残差
[0106]
在k时刻，车辆本体bk和卫星sj的多普勒测量模型为：
[0107][0108][0109]
其中，可通过以下方式实现与world坐标系之间的变换：
[0110][0111]
所以在k时刻，车辆本体bk和卫星sj之间的多普勒测量残差为：
[0112][0113][0114]
(b)双差伪距测量残差
[0115]
本发明的单频rtk算法包括gps和bds两个星座，并且在两个星座中分别选择参考卫星和以gps为例，在k时刻，车辆本体bk和基准站ref同时跟踪卫星和双差伪距测量模型为：
[0116][0117][0118]
其中，可通过锚点实现与world坐标系之间的变换：
[0119][0120]
所以在k时刻，车辆本体bk和基准站ref对gps卫星和的双差伪距测量残差为：
[0121][0122]
同理，车辆本体bk和基准站ref对bds卫星和的双差伪距测量残差为：
[0123][0124]
至此，基于自适应融合策略，可以将上述与单频rtk相关的残差选择性的融入非线性优化模型，从而实现对自动驾驶汽车的定位。
[0125]
本发明提供的一种融合单频rtk的视觉惯性定位方法，解决了目前视觉惯性定位方法存在的累计漂移的问题，以及传统的使用回环检测步骤所带来的计算成本高、不具备通用性的问题，可以实现局部精确和全局无漂移的定位功能。通过单频rtk的状态选择不同的初始化方法和融合方法。这种策略可以最大程度发挥单频rtk的优势，从而提高本方法的定位精度和实时性。本发明所融合的单频rtk兼顾精度和成本，硬件设备尺寸小、重量轻、功耗低，安装方便，使用简单，与视觉惯性设备的优点相匹配，非常适合推广。
[0126]
实施例2
[0127]
与实施例1相比，本实施例在于步骤s3，除了提取图像中的点特征外，还额外提取线特征，从而能更好的应对点特征退化的情况，提升整体性能。关于额外增加的线特征部分的操作步骤如下：
[0128]
(1)基于edlines算法进行线特征提取，之后通过设置长度阈值，将过短的线段剔除；通过设置角度和距离阈值，筛选过分靠近的线段并将其中较短的剔除。
[0129]
(2)基于lbd描述子进行线特征匹配，之后通过设置比例验证步骤，以及距离和角度阈值，剔除错误匹配。
[0130]
(3)在实施例1步骤s3-(3)所述的基于因子图的非线性优化模型中，增加与线特征相关的因子，即线特征重投影残差：
[0131][0132]
其中，为第h个线特征在第j帧图像中的匹配结果，和分别为起点和终点坐标。为第h个线特征在第j帧图像中的重投影，d表示点到直线的距离。
[0133]
实施例3
[0134]
参见图2，本实施例提供了一种自适应融合单频rtk的视觉惯性定位设备，包括工控机以及与工控机连接的单目相机、imu和rtk流动站接收机及接收天线，单目相机和imu层
叠设置且并位于车身纵向中心线上，rtk接收天线与rtk流动站接收机连接，工控机用于从单目相机获取多帧图像并筛选多个关键帧，基于滑动窗口对多个关键帧进行多次滑动测量，构建本次测量的测量状态信息，基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。
[0135]
相机和imu的安装位置近似重合，并位于车身纵向中心线上。rtk接收天线有两个，对称布置在车身纵向中心线两侧，并与相机和imu位于一条直线上，左侧的用来定位，右侧的用来定向，本实施例中仅使用左侧的定位天线。rtk流动站接收机安装在汽车后备箱中，使用一台工控机作为计算平台，同样安装在汽车后备箱中。使用汽车自身电池为所有设备供电。
[0136]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

技术特征：
1.一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，包括如下步骤：基于滑动窗口对多个关键帧进行多次滑动测量，在一次测量过程中，利用基于因子图的非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据，实现视觉惯性定位，通过单频rtk算法计算在全局坐标系下的轨迹，计算本次测量下的局部坐标系与全局坐标系的变换数据，基于状态数据以及变换数据构建本次测量的测量状态信息；基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入所述非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。2.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，所述的rtk残差的获取包括如下步骤：判断所述单频rtk算法是否有固定解，若是，基于变换数据、rtk算法得到的固定解以及视觉惯性定位得到的局部坐标系下的坐标，通过解最小二乘问题计算松耦合的位置约束残差，作为所述的rtk残差，若否，所述的rtk残差包括紧耦合的多普勒测量残差和双差伪距测量残差。3.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，计算局部坐标系与全局坐标系的变换数据的过程包括如下步骤：判断所述单频rtk算法是否有固定解，若是，通过对应点集配准对局部坐标系和全局坐标系下的轨迹进行对齐，计算坐标系间旋转变换项和平移变换项，作为所述的变换数据，若否，首先使用粗略位置坐标和多普勒测量对齐旋转，然后使用双差伪距测量对齐平移，计算坐标系间旋转变换项和平移变换项，作为所述的变换数据。4.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，所述的残差项还包括点特征重投影残差、线特征重投影残差、imu预积分残差中的至少一个。5.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，通过单频rtk算法计算在全局坐标系下的轨迹具体包括如下步骤：流动站接收机将高度角小于预设角度的卫星剔除，并在gps和bds中各选择一颗最高仰角卫星作为参考卫星；流动站接收机将自身对某卫星的测量，与来自基准站接收机对同卫星的测量作差，得到卫星的站间单差伪距测量和单差载波相位测量，将各星座内其余所有卫星的单差测量，与其参考卫星的单差测量作差，得到双差测量；基于双差测量执行单频rtk算法，得到在全局坐标系下3自由度位置的轨迹。6.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，利用基于因子图的非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据的过程具体包括如下步骤：获取多帧图像，通过点特征提取和跟踪，基于预设的关键帧选取规则选取关键帧，计算imu预积分项；基于视觉sfm获取位姿信息并与imu预积分项对齐；构建基于因子图的非线性优化模型，通过最小化滑动窗口中的所有因子，得到车辆位姿、速度、点特征位置和imu偏差最优佳估计，作为所述的状态数据，实现视觉惯性定位。
7.根据权利要求6所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，所述的关键帧的选取过程中还包括线特征提取，通过点特征和线特征的提取及跟踪，基于预设的关键帧选取规则选取关键帧。8.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，所述的最佳估计的测量状态信息包括车辆位姿、速度、点特征位置、imu偏差、rtk接收机时钟漂移以及全局坐标系与局部坐标系之间的变换。9.根据权利要求1所述的一种自适应融合单频rtk的视觉惯性定位方法，其特征在于，所述的局部坐标系为imu坐标系的初始位姿，所述的全局坐标系为东北天坐标系。10.一种自适应融合单频rtk的视觉惯性定位设备，其特征在于，包括工控机以及与所述工控机连接的单目相机、imu和rtk流动站接收机及接收天线，所述单目相机和imu层叠设置且并位于车身纵向中心线上，所述rtk接收天线与rtk流动站接收机连接，所述的工控机用于从所述单目相机获取多帧图像并筛选多个关键帧，基于滑动窗口对多个关键帧进行多次滑动测量，构建本次测量的测量状态信息，基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。

技术总结
本发明涉及一种自适应融合单频RTK的视觉惯性定位方法和设备，首先利用非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据，即局部坐标系下的轨迹，实现基本的视觉惯性定位，然后利用单频RTK算法计算在全局坐标系下的轨迹，得到变换数据，最后计算RTK残差并根据单频RTK算法是否有固定解自适应融入非线性优化模型中，通过最小化包括RTK残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频RTK的视觉惯性定位。通过根据单频RTK算法是否有固定解将RTK残差自适应融入在基于因子图的非线性优化模型中，由于RTK不要求移动的路径存在回环，解决或部分解决了现有的采用回环检测存在的通用性低的问题。有的采用回环检测存在的通用性低的问题。有的采用回环检测存在的通用性低的问题。


技术研发人员：田应仲 刘伊铭 杨晓东 倪雨嘉 王洁羽 李龙 金滔 林杨乔 张泉
受保护的技术使用者：上海大学
技术研发日：2023.06.05
技术公布日：2023/8/23