(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221098515 5.X (22)申请日 2022.08.17 (71)申请人 南京航空航天大 学 地址 210016 江苏省南京市秦淮区御道街 29号 (72)发明人 张驰　杨忠　廖禄伟　朱傥　 李国涛　杨欣　 (74)专利代理 机构 南京苏高专利商标事务所 (普通合伙) 32204 专利代理师 徐红梅 (51)Int.Cl. G01C 21/16(2006.01) G06T 7/269(2017.01) G06T 7/73(2017.01) (54)发明名称 一种基于GNSS/IMU/光流信息融合的智能体 位姿估计方法 (57)摘要 本发明公开了一种基于GNSS/IMU/光流信息 融合的智能体位姿估计方法， 其步骤为： 首先， 分 别对传智能体机载感器原始数据进行预处理， 包 括IMU预积分、 视觉 特征提取及 跟踪、 GNSS信号滤 波； 然后， 分别构建视觉、 IMU、 GNSS代价函数及智 能体状态向量， 并对GNSS/IMU/光流信息进行联 合初始化， 获取智 能体位姿估计器的所有初值； 最后， 将智能体位姿求解过程构建为状态估计问 题， 并设计相应的概率因子图模型及边缘化策 略， 以图优化的方式求解智能体位姿。 与现有的 机器人位姿估计方法相比， 本发 明具有更高的计 算效率及自适应性， 能够准确、 鲁棒、 无漂地获取 智能体位姿信息 。 权利要求书4页 说明书14页 附图3页 CN 115435779 A 2022.12.06 CN 115435779 A 1.一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法， 其特征在于， 包括以下步 骤： S1、 分别对智能体原始测量数据进行预处理， 包括对相机的RGB输出进行Shi ‑Tomasi角 点特征提取， 并对随后的RGB视频帧执行KLT光流跟踪； 在机器人坐标系下对惯性测量单元 (IMU)的原 始测量数据进行 预积分； 对捕获到的GNS S原始信号进行 滤波； 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′、 视觉特征 点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏 置 ψ， 智能体状态向量χ表示 为： S202、 利用PnP算法计算两个相邻图像帧的相对运动关系， 同时利用BA算法优化滑动窗 内所有角点特征的重投影误差， 获得智能体的初始位置 和初始朝向 同时利用SPP算 法粗略求 解智能体的经纬度； S203、 将智能体的初始位置和经纬度对齐， 获得里程计坐标系与世界坐标系之间的变 换矩阵 完成GNSS/IMU/光 流信息联合初始化。 3.根据权利 要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法， 其 特征在于， 步骤S3中将视觉误差因子项构建为特征点重投影残差的具体方法为： 把空间特 征点l从视频帧i的像素坐标系投影到世界坐标系， 再从世界坐标系投影到视频帧j的像素权　利　要　求　书 1/4 页 2 CN 115435779 A 2坐标系， 然后求 解特征点l在视频帧j中的测量 位置与实际位置的残差 。 4.根据权利 要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法， 其 特征在于， 步骤S4具体为： S401、 将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差， 分别用智能体在 两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法， 即获取陀螺仪的 测量残差； S402、 将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差； 速度偏 置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获 得； 位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项 做差获得； S403、 陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差 获得； S404、 联立 步骤S401、 步骤S402和步骤S40 3即完成IMU约束因子的构建； 构建的IMU约束因子为： 其中， χI表示智能体状态向量χ中与IMU相关的子向量， 为IMU朝向的预积分项、 为IMU速度的预积分项、 为IMU位置的预积分项， 为IMU朝向预积分项的测量值、 为IMU速度预积分项的测量值、 为IMU位置预积分项的测量值， δbω和 δba分别为陀螺仪偏 置和加速度计偏置的残差， 代表tk时刻世界坐标系到机器人坐标系之间的旋转， 分别为世界坐标系下tk+1时刻智能体的朝向、 速度和位置， 为tk时刻世 界坐标系与机器人坐标系 之间的变换矩阵， 符号 代表四元数乘法， 和 分别为tk+1 时刻的陀螺仪偏置和 加速度计偏置， 和 分别为tk时刻的陀螺仪偏置和 加速度计偏置 。 5.根据权利 要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法， 其 特征在于， 步骤S5具体为：权　利　要　求　书 2/4 页 3 CN 115435779 A 3