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

权利要求书

1.一种基于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约束因子包括陀螺仪误差因子约束项和加速度计误差因子约束项，其中，将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差，将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差；

S5、构建GNSS约束因子：GNSS约束因子包括GNSS伪距约束因子、GNSS多普勒频移约束因子和接收机时钟偏置约束因子；其中，GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差，GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差，接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差；

S6、以概率因子图优化的方式求解智能体位姿信息。

2.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法，其特征在于，步骤S2具体为：

S201、构建智能体状态向量χ，所述状态向量包含智能体当前的朝向速度位置陀螺仪偏置和加速度计偏置GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d₁,d₂,...,d_m和里程计坐标系与世界坐标系之间的Z轴偏置ψ，智能体状态向量χ表示为：

S202、利用PnP算法计算两个相邻图像帧的相对运动关系，同时利用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位置和初始朝向同时利用SPP算法粗略求解智能体的经纬度；

S203、将智能体的初始位置和经纬度对齐，获得里程计坐标系与世界坐标系之间的变换矩阵完成GNSS/IMU/光流信息联合初始化。

3.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法，其特征在于，步骤S3中将视觉误差因子项构建为特征点重投影残差的具体方法为：把空间特征点l从视频帧i的像素坐标系投影到世界坐标系，再从世界坐标系投影到视频帧j的像素坐标系，然后求解特征点l在视频帧j中的测量位置与实际位置的残差。