[发明专利]一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法

说明书

本发明公开了一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法，其步骤为：首先，分别对传智能体机载感器原始数据进行预处理，包括IMU预积分、视觉特征提取及跟踪、GNSS信号滤波；然后，分别构建视觉、IMU、GNSS代价函数及智能体状态向量，并对GNSS/IMU/光流信息进行联合初始化，获取智能体位姿估计器的所有初值；最后，将智能体位姿求解过程构建为状态估计问题，并设计相应的概率因子图模型及边缘化策略，以图优化的方式求解智能体位姿。与现有的机器人位姿估计方法相比，本发明具有更高的计算效率及自适应性，能够准确、鲁棒、无漂地获取智能体位姿信息。

技术领域

本发明涉及智能体位姿估计技术，特别是涉及一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法。

背景技术

智能体是一种能够凭借传感器感知其周围环境，并借助于执行器作用于该环境的智能机器人。目前，智能体的自主导航能力已经成为衡量其智能化水平的重要指标。而解决智能体自主导航问题的难点是如何实时、准确地获取智能体当前的位姿。相比于基于单一传感器的智能体位姿估计器，多传感器融合位姿估计技术能够充分利用不同种类传感器之间的互补特性，以获得更为准确且鲁棒的智能体位姿估计结果。立体相机和惯性测量单元(IMU)能够为智能体提供机体坐标系下厘米级精度的位姿，但局部坐标系下的位姿会随着智能体的运动而漂移。全球导航卫星系统(GNSS)为能够为智能体提供无漂移的位置信息，已广泛应用于各种移动机器人导航任务。但基于GNSS的智能体导航方法难以在室内使用，且易受噪声和多径效应的影响，导致其只能达到米级的定位精度。为了充分利用不同传感器之间的互补特性，基于GNSS/IMU/光流信息融合的智能体位姿估计算法能够利用导航卫星、加速度计、陀螺仪、立体相机各自的优势，以获取准确、无漂的智能体位姿估计。

然而，基于GNSS/IMU/光流信息融合的位姿估计方法将会面临如下问题：首先，不同传感器的输出频率并不相同(GNSS接收机约为10HZ；相机约为30HZ；IMU约为250HZ)，如何同步并对齐这些来自不同传感器的原始测量数据将会是一个难题；其次，视觉/惯性里程计通常可以达到厘米级的定位精度，而GNSS的定位精度比视觉/惯性里程计要低两个数量级；最后，当位姿估计系统的某个传感器突然失效，然后又重新正常工作时，位姿估计器怎样才能快速恢复到之前的状态。因此，如何利用GNSS/IMU/光流信息快速、准确、无漂地获取智能体位姿已成为智能机器人导航领域亟需解决的难题。

发明内容

发明目的：本发明的目的是提供一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法。

技术方案：本发明的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法，包括以下步骤：

S1、分别对智能体原始测量数据进行预处理，包括对相机的RGB输出进行Shi-Tomasi角点特征提取，并对随后的RGB视频帧执行KLT光流跟踪；在机器人坐标系下对惯性测量单元(IMU)的原始测量数据进行预积分；对捕获到的GNSS原始信号进行滤波；

S2、位姿估计器GNSS/IMU/光流信息联合初始化：构建智能体状态向量χ，利用PnP算法和BA算法求解智能体的初始位置和初始朝向，同时利用SPP算法粗略求解智能体的经纬度；将智能体的初始位置和经纬度对齐，获得里程计坐标系与世界坐标系之间的变换矩阵，完成GNSS/IMU/光流信息联合初始化；

S3、构建视觉约束因子：将相机投影模型构建为针孔相机模型，建立空间特征点l从视频帧i到视频帧j的重投影函数，将视觉误差因子项构建为特征点重投影残差；

S4、构建IMU约束因子：IMU约束因子包括陀螺仪误差因子约束项和加速度计误差因子约束项，其中，将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差，将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差；