(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202211123604.6 (22)申请日 2022.09.15 (65)同一申请的已公布的文献号 申请公布号 CN 115235454 A (43)申请公布日 2022.10.25 (73)专利权人 中国人民解 放军国防科技大 学 地址 410073 湖南省长 沙市开福区德雅路 109号 (72)发明人 潘献飞　涂哲铭　陈昶昊　张礼廉　 褚超群　陈宗阳　毛军　王茂松　 (74)专利代理 机构 长沙国科天河知识产权代理 有限公司 432 25 专利代理师 邱轶 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/16(2006.01) G06T 7/73(2017.01) G06T 7/55(2017.01)(56)对比文件 CN 112985392 A,2021.0 6.18 CN 114608561 A,202 2.06.10 CN 114964276 A,202 2.08.30 CN 114966734 A,202 2.08.30 CN 111024070 A,2020.04.17 CN 113091738 A,2021.07.09 US 20193 01871 A1,2019.10.0 3 WO 2022079292 A1,202 2.04.21 US 2022018962 A1,202 2.01.20 US 20172 27363 A1,2017.08.10 CN 114545472 A,202 2.05.27 毛军 等.惯性/ 视觉/激光雷达SLAM技 术综 述. 《导航定位与授时》 .202 2,第9卷(第4期),第 17-30页. 李倩 等.基 于因子图的I NS/UWB室内行 人紧 组合定位 技术. 《仪器仪表学报》 .202 2,第43卷 (第5期),第32-45页. (续) 审查员 章慧敏 (54)发明名称 行人运动约束的视觉惯性融合定位与建图 方法和装置 (57)摘要 本申请涉及一种行人运动约束的视觉惯性 融合定位与建图方法和装置。 所述方法包括： 对 足绑式IMU进行因子图建模， 得到因子图， 根据足 绑式IMU的速度预测值以及零速之间的误差， 构 建零速修正因子； 根据视觉惯 性里程计观测的位 置信息与足绑式IMU观测的位置信息， 构建视觉 惯性里程计因子； 将零速修正因子和视觉惯性里 程计因子作为状态变量的约束条件， 对因子图进 行优化， 得到优化因子图； 根据视觉传感器采集 的图像数据以及通过足绑式IMU的零速检测结 果， 筛选关键帧图像， 根据关键帧图像进行稠密 重建， 得到建图结果； 对优化因子图和建图结果 进行融合， 实现行人运动的定位与建图。 采用本 方法能够复杂环境下的行 人导航定位与建图。 [转续页] 权利要求书3页 说明书13页 附图3页 CN 115235454 B 2022.12.30 CN 115235454 B (56)对比文件 Mang Wang 等.An Optimal Co operative Navigation Algorithm based o n Factor Graph for Pedest rians. 《2021 3rd Internati onal Conference o n Intelligent Control, Measurement and Signal Processing and I ntelligent Oi l Field (ICMSP)》 .2021,第1- 6页.2/2 页 2[接上页] CN 115235454 B1.一种行人运动约束的视觉惯性融合定位与建图方法， 其特征在于， 应用于行人导航 中， 其中足绑式IMU设置在行人足部， 视觉惯性里程计设置在行人腿部以上的躯干部分； 所 述视觉惯性里程计包括： 视 觉传感器和惯性传感器， 所述方法包括： 对所述足绑式IMU进行因子图建模， 得到因子图； 其中， 所述因子图中的因子节点包括： 偏差因子节点和IMU因子节点， 所述偏差因子节点连接相邻的偏差变量， 所述IMU因子节点 连接当前时刻的状态变量和偏差变量与下一时刻的状态变量； 根据所述足绑式IMU的速度预测值以及零速之间的误差， 构建零速修 正因子； 根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息， 构建视 觉惯性里程计因子； 将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件， 对所述因 子图进行优化， 得到优化因子图； 根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果， 筛选关键 帧图像， 根据所述关键帧图像进行稠密重建， 得到建图结果； 对所述优化因子图和所述建图结果进行融合， 实现行 人运动的定位与建图。 2.根据权利要求1所述的方法， 其特 征在于， 所述方法还 包括： 构建坐标系方向为右 ‑前‑上的载体坐标系以及东 ‑北‑天的导航坐标系， 通过对所述足 绑式IMU进行初始化； 利用所述足绑式IMU静止时的三轴加速度测量得到横滚角和俯仰角； 根据所述横滚角和所述俯仰角， 将足绑式IMU的测量数据从载体坐标系转化至导航坐 标系， 并计算得到在导 航坐标系中的初始航向角。 3.根据权利要求1所述的方法， 其特 征在于， 所述方法还 包括： 通过足绑式IMU进行零速检测之后， 进行足绑式IMU和所述视觉惯性里程计时间同步的 步骤包括： 将足绑式IMU和所述视觉惯性里程计的传感器信息通过话题通讯机制上传至数据处理 中心， 并通过 数据处理中心对所述传感器数据打上时间戳； 进行足绑式IMU和所述视 觉惯性里程计空间上同步的步骤 包括： 确定足绑式IMU和所述视觉惯性里程计在水平方向上初始航向角偏差值， 根据所述初 始航向角偏差值， 对足绑式IMU和所述视 觉惯性里程计进行空间对齐。 4.根据权利要求1所述的方法， 其特 征在于， 视 觉惯性里程计进行定位的步骤 包括： 对所述视觉惯性里程计进行因子 图建模， 得到定位因子 图； 所述定位因子 图中的因子 包括： 视觉因子和惯性因子； 所述视觉因子是根据视觉传感器对运动对 象的像素点位置进 行测量产生的测量 位置误差确定的； 所述惯性因子是通过 预积分方式计算得到的； 对所述定位因子图进行计算， 得到 视觉惯性里程计的定位数据。 5.根据权利要求 4所述的方法， 其特 征在于， 计算视 觉因子的步骤 包括： 根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差， 得到视觉因 子为： 权　利　要　求　书 1/3 页 2 CN 115235454 B 3