(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210331425.5 (22)申请日 2022.03.30 (71)申请人 广州导远电子科技有限公司 地址 广东省广州市高新 技术产业开发区南 翔二路1号自编一栋B101室 (72)发明人 曾毓菁　王军厚　吴翰宇　李楠　 (74)专利代理 机构 北京超凡宏宇专利代理事务 所(特殊普通 合伙) 11463 专利代理师 徐叶馨 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/16(2006.01) G01C 21/20(2006.01) G06V 20/10(2022.01) G06V 10/44(2022.01) (54)发明名称 一种视觉惯性SLAM方法及装置 (57)摘要 本申请提供一种视觉惯性SLAM方法及装置， 该视觉惯性SLA M方法包括： 获取图像特征点和状 态估计信息； 根据特征点量化模 型对图像特征点 进行量化， 得到特征点向量； 计算IMU运动模型的 运动信息增益， 根据运动信息增益对状态估计信 息进行增益， 得到第一增益特征； 根据线性测量 模型计算特征点信息增益， 根据特征点信息增益 对特征点向量进行增益， 得到第二增益特征； 根 据贪婪算法对第一增益特征和第二增益特征进 行选择， 得到选定特征； 将选定特征放入视觉惯 性SLAM后端进行非线性优化， 得到SLAM结果。 可 见， 实施这种实施方式， 能够对通过将IMU信息增 益、 特征点信息增益、 特征选择方法融入到现有 的视觉惯性SLA M后端优化框架中， 从而减少视觉 惯性SLAM后端 优化的计算 量。 权利要求书4页 说明书13页 附图3页 CN 114646310 A 2022.06.21 CN 114646310 A 1.一种视 觉惯性SLAM方法， 其特 征在于， 所述方法包括： 获取图像特 征点和状态估计信息； 所述图像特 征点与所述状态估计信息相关联； 根据特征点量化模型对所述图像特 征点进行量 化， 得到特 征点向量； 计算预先建立的IMU运动模型的运动信息增益， 并根据所述运动信息增益对所述状态 估计信息进行增益， 得到第一增益特 征； 根据线性测量模型计算特征点信 息增益， 并根据 所述特征点信 息增益对所述特征点向 量进行增益， 得到第二增益特 征； 根据贪婪算法对所述第一增益特 征和第二增益特 征进行选择， 得到 选定特征； 将所述选 定特征放入视 觉惯性SLAM后端 进行非线性优化， 得到SLAM结果。 2.根据权利要求1所述的视觉惯性SLAM方法， 其特征在于， 所述根据线性测量模型计算 特征点信息增 益， 并根据所述特征点信息增益对所述特征点向量进行增 益， 得到第二增益 特征的步骤 包括： 根据线性测量模型计算特征点信 息增益， 并根据 所述特征点信 息增益对所述特征点向 量和所述状态估计信息进行增益， 得到第二增益特 征。 3.根据权利要求1所述的视觉惯性SLAM方法， 其特征在于， 所述获取图像特征点和状态 估计信息的步骤 包括： 获取图像特 征点和真实运动数据； 对所述真实运动数据进行状态估计， 得到状态估计信息 。 4.根据权利要求1所述的视觉惯性SLAM方法， 其特征在于， 所述根据 特征点量化模型对 所述图像特 征点进行量 化， 得到特 征点向量的步骤之前， 所述方法还 包括： 建立用于量 化特征点可行性的特 征点量化模型。 5.根据权利要求4所述的视觉惯性SLAM方法， 其特征在于， 所述建立用于量化特征点可 行性的特 征点量化模型的步骤 包括： 根据特征点量化方程建立用于量化特征点可行性的特征点量化模型； 所述特征点量化 方程为： 其中， 是IMU运动模型相关的信息矩阵； k用于表示 k时刻； k+H用于表示 k时刻后经 过H时长后的另一时刻； S是特征子集； Δl是第l个特 征的信息矩阵； pl是第l个特 征被跟踪到的概 率， 所述概率是通过对特 征检测得分进行归一 化得到。 6.根据权利要求1所述的视觉惯性SLAM方法， 其特征在于， 所述计算预先建立的IMU运 动模型的运动信息增益的步骤 包括： 根据增益计算矩阵计算预先建立的IMU运动模型的运动信息增益； 所述增益计算矩阵 为：权　利　要　求　书 1/4 页 2 CN 114646310 A 2其中， 用于表示所述 运动信息增益； h和h′用于表示两个连续的时刻； 用于表示 IMU的运动噪声； 用于表示离 散加速度噪声； 用于表示 IMU的离散测量偏差值； m用于表示测量时刻的数量； i用于表示测量时刻； δ用于表示 IMU的采样周期； R0＝I用于表示初始时刻时IMU测量的增量旋转矩阵。 7.根据权利要求1所述的视觉惯性SLAM方法， 其特征在于， 所述根据 所述运动信息增益 对所述状态估计信息进行增益， 得到第一增益特 征的步骤 包括： 根据所述运动信息增益对所述状态估计信息进行增益， 并加上来自所述视觉惯性SLAM 后端的先验信息， 得到第一增益特 征。 8.根据权利要求1所述的视觉惯性SLAM方法， 其特征在于， 所述根据线性测量模型计算 特征点信息增 益， 并根据所述特征点信息增益对所述特征点向量进行增 益， 得到第二增益 特征的步骤之前， 所述方法还 包括： 建立相机非线性透 视投影模型； 对所述相机非线性透 视投影模型进行简化， 得到线性测量模型。 9.根据权利要求1所述的视 觉惯性SLAM方法， 其特 征在于， 所述线性测量模型为： 其中， uhl为第l个特 征h时刻相机姿态的归一 化向量； 是特征相对于世界坐标系的位置； 是相机在h时刻相机的预测的位姿。 10.一种视 觉惯性SLAM装置， 其特 征在于， 所述视 觉惯性SLAM装置包括： 获取单元， 用于获取图像特征点和状态估计信息； 所述图像特征点与所述状态估计信 息相关联；权　利　要　求　书 2/4 页 3 CN 114646310 A 3