(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210562354.X (22)申请日 2022.05.23 (65)同一申请的已公布的文献号 申请公布号 CN 114648584 A (43)申请公布日 2022.06.21 (73)专利权人 北京理工大 学前沿技术研究院 地址 250000 山东省济南市高新区经十路 7000号汉峪金融商务中心五区1号楼 601 (72)发明人 刘晓慧　杨东清　张宝庭　 (74)专利代理 机构 济南诚智商标专利事务所有 限公司 3710 5 专利代理师 朱晓熹 (51)Int.Cl. G06T 7/73(2017.01) G06T 5/00(2006.01) G06K 9/62(2022.01)G01S 17/86(2020.01) G01C 21/30(2006.01) G01C 21/16(2006.01) G06V 10/74(2022.01) (56)对比文件 CN 110261870 A,2019.09.20 CN 111595333 A,2020.08.28 CN 111983639 A,2020.1 1.24 CN 113721260 A,2021.1 1.30 CN 112347840 A,2021.02.09 CN 111207774 A,2020.0 5.29 CN 10946 5832 A,2019.0 3.15 CN 110706279 A,2020.01.17 Thien-Minh Nguyen et al. .VIRAL SLAM: Tightly Coupled Camera-IMU-UWB-L idar SLAM. 《arXiv:210 5.03296v3》 .2021,第1-8页. 审查员 谢晶 (54)发明名称 一种用于多源融合定位的鲁棒性控制方法 和系统 (57)摘要 本发明提出了一种用于多源融合定位的鲁 棒性控制方法和系统， 该方法包括： 针对同一场 景同一帧图像数据进行视觉特征点提取， 通过视 觉惯导融合得到第一位姿， 根据第一位姿计算视 觉特征点误差和第一重投影误差； 针对同一场景 同一帧的点云数据进行惯导与激光雷达特征点 提取， 通过惯导激光雷达融合得到第二位姿； 根 据第二位姿计算点云特征点误差和第二重投影 误差； 计算激光雷达和相机的第三位姿， 根据第 三位姿计算相机位置特征点误差和第三重投影 误差； 对比各重投影误差分别对 各特征点误差进 行权重排名， 进行质量等级划分。 基于该方法， 还 提出了鲁棒性控制系统。 本发明不仅能够提高系 统的解算速度， 提高系统环境适应性及精度， 而 且提高系统的鲁棒 性。 权利要求书5页 说明书15页 附图4页 CN 114648584 B 2022.08.30 CN 114648584 B 1.一种用于多源融合定位的鲁棒 性控制方法， 其特 征在于， 包括以下步骤： 针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐 标系下的像素位置， 以及通过视觉惯导融合得到第一位姿， 根据所述第一位姿计算视觉特 征点误差和第一重投影误差； 针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特 征点在世界坐标下三维坐标， 以及通过惯导激光雷达融合得到第二位姿； 根据所述第二位 姿计算点云特征点误差和第二重投影误差； 基于所述视觉特征点在图像像素坐标系下的像素位置和点云特征点在世界坐标下三 维坐标， 采用PNP方法计算激光雷达和相机的第三位姿， 根据所述第三位姿计算相机位置特 征点误差和第三重投影误差； 对比第一重投影误差、 第 二重投影误差和第三重投影误差， 分别对视觉特征点误差、 点 云特征点误差和相位位置特征点误差进行权重排名， 在环境特征匹配之后， 进行质量等级 划分； 所述质量等级划分的详细过程包括： 根据应用场景设置相应的阈值， 将特征点根据投 票结果进行分类， 对视觉与雷达提供 的场景时空关联信息进行信息匹配质量等级划分， 将 视觉与雷达提供的场景时空关联信息分成三种信息集合： 质量一级集合、 质量二级集合、 质 量三级集合； 质量二级集合包含质量一级集合， 质量三级集合包含质量一、 二级集合； 采用 投票机制策略， 分别 利用不同质量 等级的关联信息集 合进行融合定位； 在进行质量等级划分之后， 控制参与下一帧解算特征点的数量和质量， 保证以最少特 征点数量参与解算， 使系统达到最高精度； 所述控制参与下一帧解算特征点的数量和质量 的过程为： 采用同一场景 的当前帧处理的过程， 将当前帧的最少特征点数量参与下一帧解 算的过程， 所述最少特征点为当前帧质量最优的特征点， 再次得到最少特征点数量参与再 下一帧解 算， 循环执 行后使系统达 到最高精度， 提高多元融合定位的鲁棒 性。 2.根据权利要求1所述的一种用于多源融合定位的鲁棒性控制方法， 其特征在于， 在特 征点提取之前还 包括： 初始化组合 导航系统； 所述初始化组合导航系统 的过程为： 在姿态更新 时不考虑由载体速度引起的导航坐标 系相对于地球坐标系的角速度， 在速度更新时不考虑载体速度引起的导航坐标系的速度前 提下， 开环初始对准时姿态更新的公式为： ； （6） 开环初始对准时速度更新的公式为： ； （7） 其中： 表示载体坐标系在导航坐标系 下的姿态矩阵， 表示载体坐标系相对于地心 惯性坐标系的角速度， 表示地球自转引起的导航系的旋 转角速度； 表示比力在导航坐 标系下的投影； 表示重力加速度； 组合导航系统的误差方程 为： ； （8）权　利　要　求　书 1/5 页 2 CN 114648584 B 2； （9） 其中： 表示姿态矩阵的误差， 表示导航系相对于惯性系的角速度， 表示陀螺仪 的零漂误差， 表示速度误差， 表示比力在导航坐标系下的投影， 表示加速度计测量 的比力值， 加速度计常值偏置 。 3.根据权利要求1所述的一种用于多源融合定位的鲁棒性控制方法， 其特征在于， 所述 视觉特征点提取的过程包括： 采用改进式FAST角点提取图像数据中有明显差异的像素点， 所述有明显差异的像素点 为与周围相邻的像素 灰度值差别大于设定阈值； 采用灰度质心算法提高特征点的旋转不变性， 再通过BRIEF描述子判断像素点的特性 来确定图像数据中ORB特 征点的位置 。 4.根据权利要求1所述的一种用于多源融合定位的鲁棒性控制方法， 其特征在于， 所述 视觉惯导融合的详细过程包括： 定义滑动窗口内所有状态向量： ； （14） 其中， 表示第i帧惯导与世界坐 标系之间的旋转矩阵； 表示平移偏差； 表示惯导 在世界坐标系中的速度向量； 表示加速度计偏置； 表示旋转偏差； n表示第i帧惯导与 世界坐标系之间的旋转矩阵的索引； m为路标点的索引； o表 示路标线的索引； N、 M和O分别表 示滑窗中关键帧的数量、 被滑动窗口中所有关键帧观测到的路标点和路标线数量； 表示 第k个路标点第一次被观测到时的逆深度， 表示第l个特 征线的正交表达； 通过最小化滑动窗口内所有状态量的残差完成基于滑动窗口的非线性优化， 其中目标 函数表示 为： ； （15） 其中， 是滑动窗口中因时间长或者共视特征少而被剔除的帧的先验信息； 表示残差； 表示雅克比矩阵； 是滑动窗口内所有状态向量； 为柯西鲁棒函数用于 迭代约束过程中的惩罚项； 为第i帧与第i+1帧状态量之间的惯导预积分， 代表惯导帧之间的误差； 代表惯导帧观测值；权　利　要　求　书 2/5 页 3 CN 114648584 B 3