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

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

背景技术：

1、自主定位是移动机器人领域的关键技术，准确实时的定位结果是实现后续感知、规划和控制的最基本前提。移动机器人的自主定位依赖所使用的传感器，为提升整体性能，多传感器融合是一种通用且有效的手段。基于单目相机和惯性测量单元(inertialmeasurement unite,imu)融合的视觉惯性定位方法，拥有局部精度高、计算实时性好，以及尺寸小、重量轻、功耗低、成本低等优点，在移动机器人领域得到了广泛应用。

2、中国专利申请号cn202210737648.1公开了一种基于视觉-imu融合的多移动机器人协同定位方法及系统。本方法，包括：步骤s1、对相机与imu进行参数标定；步骤s2、计算imu预积分，进行数据对齐；步骤s3、提取图像orb特征，根据brief描述子进行特征匹配；步骤s4、建立优化问题，求解获得机器人终端的局部坐标系下最优的位姿估计；步骤s5、将优化得到的位姿与提取的图像orb特征数据发送给服务器端；步骤s6、使用词袋模型进行回环检测；步骤s7、在全局统一坐标下进行位姿图优化，将优化结果发送至机器人终端。

3、上述申请采用词袋模型进行回环检测，最终可以输出多机器人在全局统一坐标系下的位姿。然而，视觉惯性定位方法存在全局累积漂移的问题，定位误差会随时间逐渐累积，直至完全失效。一种常见的解决方法是增加额外的回环检测步骤，但该方法的计算成本较高，会影响实时性。而且需要注意的是，在实际应用中，移动机器人的路径是不一定存在回环的，这就大大限制了该方法的通用性。

技术实现思路

1、本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种自适应融合单频实时动态(real-time kinematic,rtk)的视觉惯性定位方法和设备，以解决或部分解决现有的视觉惯性定位方法通用性低的问题。

2、本发明的目的可以通过以下技术方案来实现：

3、本发明的一个方面，提供了一种自适应融合单频rtk的视觉惯性定位方法，包括如下步骤：

4、基于滑动窗口对多个关键帧进行多次滑动测量，在一次测量过程中，利用基于因子图的非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据，实现视觉惯性定位，通过单频rtk算法计算在全局坐标系下的轨迹，计算本次测量下的局部坐标系与全局坐标系的变换数据，基于状态数据以及变换数据构建本次测量的测量状态信息；

5、基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入所述非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。

6、作为优选的技术方案，所述的rtk残差的获取包括如下步骤：

7、判断所述单频rtk算法是否有固定解，若是，基于变换数据、rtk算法得到的固定解以及视觉惯性定位得到的局部坐标系下的坐标，通过解最小二乘问题计算松耦合的位置约束残差，作为所述的rtk残差，若否，所述的rtk残差包括紧耦合的多普勒测量残差和双差伪距测量残差。

8、作为优选的技术方案，计算局部坐标系与全局坐标系的变换数据的过程包括如下步骤：

9、判断所述单频rtk算法是否有固定解，若是，通过对应点集配准对局部坐标系和全局坐标系下的轨迹进行对齐，计算坐标系间旋转变换项和平移变换项，作为所述的变换数据，若否，首先使用粗略位置坐标和多普勒测量对齐旋转，然后使用双差伪距测量对齐平移，计算坐标系间旋转变换项和平移变换项，作为所述的变换数据。

10、作为优选的技术方案，所述的残差项还包括点特征重投影残差、线特征重投影残差、imu预积分残差中的至少一个。

11、作为优选的技术方案，通过单频rtk算法计算在全局坐标系下的轨迹具体包括如下步骤：

12、流动站接收机将高度角小于预设角度的卫星剔除，并在gps和bds中各选择一颗最高仰角卫星作为参考卫星；

13、流动站接收机将自身对某卫星的测量，与来自基准站接收机对同卫星的测量作差，得到卫星的站间单差伪距测量和单差载波相位测量，将各星座内其余所有卫星的单差测量，与其参考卫星的单差测量作差，得到双差测量；

14、基于双差测量执行单频rtk算法，得到在全局坐标系下3自由度位置的轨迹。

15、作为优选的技术方案，利用基于因子图的非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据的过程具体包括如下步骤：

16、获取多帧图像，通过点特征提取和跟踪，基于预设的关键帧选取规则选取关键帧，计算imu预积分项；

17、基于视觉sfm获取位姿信息并与imu(inertial measurement unit)预积分项对齐；

18、构建基于因子图的非线性优化模型，通过最小化滑动窗口中的所有因子，得到车辆位姿、速度、点特征位置和imu偏差最优佳估计，作为所述的状态数据，实现视觉惯性定位。

19、作为优选的技术方案，所述的关键帧的选取过程中还包括线特征提取，通过点特征和线特征的提取及跟踪，基于预设的关键帧选取规则选取关键帧。

20、作为优选的技术方案，所述的最佳估计的测量状态信息包括车辆位姿、速度、点特征位置、imu偏差、rtk接收机时钟漂移以及全局坐标系与局部坐标系之间的变换。

21、作为优选的技术方案，所述的局部坐标系为imu坐标系的初始位姿，所述的全局坐标系为东北天坐标系。

22、本发明的另一个方面，提供了一种自适应融合单频rtk的视觉惯性定位设备，包括工控机以及与所述工控机连接的单目相机、imu和rtk流动站接收机及接收天线，所述单目相机和imu层叠设置且并位于车身纵向中心线上，所述rtk接收天线与rtk流动站接收机连接，所述的工控机用于从所述单目相机获取多帧图像并筛选多个关键帧，基于滑动窗口对多个关键帧进行多次滑动测量，构建本次测量的测量状态信息，基于多次测量的测量状态信息，计算rtk残差并根据单频rtk算法是否有固定解自适应融入非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。

23、与现有技术相比，本发明具有以下优点：

24、(1)通用性高，使用场景广：不同于现有的通过增加额外的回环检测步骤以减少全局累积漂移的方案，本发明首先利用在非线性优化模型获取局部坐标系下滑动窗口中各个关键帧的状态数据，即局部坐标系下的轨迹，实现基本的视觉惯性定位，然后利用rtk算法计算在全局坐标系下的轨迹，得到变换数据，最后计算rtk残差并根据单频rtk算法是否有固定解自适应融入非线性优化模型中，通过最小化包括rtk残差的残差项，得到最佳估计的测量状态信息，实现自适应融合单频rtk的视觉惯性定位。通过根据单频rtk算法是否有固定解将rtk残差自适应融入在基于因子图的非线性优化模型中，再通过最小化包括rtk残差的残差项实现定位，由于rtk不要求移动的路径存在回环，解决或部分解决了现有的采用回环检测存在的通用性低的问题。

25、(2)定位精度高：当单频rtk算法有固定解时，将松耦合的位置约束残差作为rtk残差，否则将紧耦合的多普勒测量残差和双差伪距测量残差作为rtk残差，通过单频rtk解的状态选择不同的初始化方法和融合方法，这种策略可以最大程度发挥单频rtk的优势，从而提高本方法的定位精度和实时性。