(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210576187.4 (22)申请日 2022.05.24 (71)申请人 广东人工智能与先进计算研究院 地址 510530 广东省广州市黄埔区开源大 道11号B2栋801室 申请人 芯跳科技 （广州） 有限公司 (72)发明人 蒿杰　周怡　孙亚强　迟鹏　梁俊　 詹恒泽　 (74)专利代理 机构 北京路浩知识产权代理有限 公司 11002 专利代理师 霍秋红 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/246(2017.01) G06T 5/00(2006.01)G06T 7/10(2017.01) G06T 7/66(2017.01) (54)发明名称 自主定位方法、 装置、 设备及计算机可读存 储介质 (57)摘要 本发明提供一种自主定位方法、 装置、 设备 及计算机可读存储介质， 所述方法包括： 采集定 位目标的点 云数据和惯性数据， 并根据所述惯性 数据确定 所述定位目标的预估位姿； 利用所述惯 性数据校正所述点云数据的运动畸变， 得到去畸 变数据； 根据所述预估位姿融合所述去畸变数据 和所述惯性数据， 得到融合定位数据， 并根据所 述融合定位数据对所述定位目标进行定位。 通过 融合惯性数据和点云数据， 实现对定位目标的不 依赖于外界的自主定位， 同时， 通过校正点云数 据的运动畸变， 并将去畸变后的点云数据与惯性 数据融合后对定位目标进行定位， 提高了定位鲁 棒性和定位目标的建图准确性， 从而提高了自主 定位的精确度。 权利要求书2页 说明书11页 附图2页 CN 115115702 A 2022.09.27 CN 115115702 A 1.一种自主定位方法， 其特 征在于， 包括以下步骤： 采集定位目标的点云数据和惯性数据， 并根据所述惯性数据确定所述定位目标的预估 位姿； 利用所述惯性数据校正所述 点云数据的运动畸变， 得到去畸变数据； 根据所述预估位姿融合所述去畸变数据和所述惯性数据， 得到融合定位数据， 并根据 所述融合定位数据对所述定位目标进行定位。 2.根据权利要求1所述的自主定位方法， 其特征在于， 所述根据 所述惯性数据确定所述 定位目标的预估位姿的步骤， 包括： 基于所述惯性数据中的首帧数据创建全局坐标系； 根据所述惯性数据中前若干帧数据计算所述定位目标的运动 参数的平均值和 协方差， 以确定所述定位目标相对于所述全局 坐标系的初始位姿信息， 其中， 所述运动参数包括加 速度和角速度； 基于所述初始位姿信 息对所述惯性数据执行状态误差迭代和滤波处理， 得到所述定位 目标在所述惯性数据的每帧数据中， 相对于所述全局坐标系的预估位姿。 3.根据权利要求2所述的自主定位方法， 其特征在于， 所述利用所述惯性数据校正所述 点云数据的运动畸变， 得到去畸变数据的步骤， 包括： 从所述惯性数据中确定与 所述点云数据中的一帧目标点云对应的目标惯性数据， 所述 目标惯性数据包括多帧数据； 计算所述目标点云与 所述目标惯性数据扫描结束时刻 之间的第 一时间差， 并根据 所述 第一时间差确定所述目标点云中最后一个数据点相对于所述全局坐标系的相对位姿； 基于所述相对位姿对所述目标惯性数据进行插值处理， 以校正所述目标点云的运动畸 变， 得到去畸变数据。 4.根据权利要求3所述的自主定位方法， 其特征在于， 所述基于所述相对位姿对所述惯 性数据进行插值处 理， 以校正所述目标点云的运动畸变的步骤， 包括： 将所述目标惯性数据中连续的两帧数据按照时间顺序标记为帧头和帧尾； 按照时间顺序反 向遍历所述目标点云中的各个数据点， 确定所述目标点云落入所述目 标惯性数据帧间的目标数据点， 并计算所述目标数据点与所述帧头对应的时刻之 间的第二 时间差； 根据所述第 二时间差对所述目标惯性数据进行插值， 并计算各个插值点和所述相对位 姿之间的位姿变换， 根据所述 位姿变换 校正所述目标点云的运动畸变。 5.根据权利要求2至4任一项所述的自主定位方法， 其特征在于， 所述根据所述预估位 姿融合所述去畸变数据和所述惯性数据， 得到融合定位数据的步骤， 包括： 基于所述全局坐标系将所述去畸变数据划分到单位体素； 根据所述单位体素和所述预估位姿构建最大后验 估计； 根据所述最大后验估计确定所述去畸变数据中每个数据点对应的最优位姿变换信 息， 并根据所述最优位姿变换信息将所述去畸变数据配准到所述全局坐标系中， 得到融合定位 数据。 6.根据权利要求5所述的自主定位方法， 其特征在于， 所述根据 所述单位体素和所述预 估位姿构建最大后验 估计的步骤， 包括：权　利　要　求　书 1/2 页 2 CN 115115702 A 2计算所述去畸变数据在所述单位体素内的数据点的协方差矩阵的最小特 征值； 若所述最小特征值大于或等于预设阈值， 则继续由粗到细分割所述全局坐标系， 直到 所述最小特征值大于预设阈值 为止； 若所述最小特征值小于预设阈值， 则将所述单位体素内的数据点划分到同一目标平面 内， 并计算所述目标平面的平面 参数， 所述平面 参数包括法向量和中心点； 根据所述平面参数计算所述单位体素内的数据点的残差方程， 并根据 所述残差方程和 所述预估位姿生成最大后验 估计。 7.根据权利要求1所述的自主定位方法， 其特征在于， 所述根据 所述预估位姿融合所述 去畸变数据和所述惯性数据， 得到融合定位数据的步骤之后， 还 包括： 生成所述去畸变数据的全局描述符， 并获取 所述去畸变数据的法向量 集合； 获取历史点云数据， 基于所述全局描述符从所述历史点云数据中搜索最佳候选帧， 并 根据所述法向量 集合计算所述去畸变数据与所述 最佳候选帧的相似度； 若所述相似度小于预设的相似度阈值， 则根据所述最佳候选帧校正所述定位目标的里 程计数据。 8.一种自主定位装置， 其特 征在于， 包括： 位姿预估模块， 用于采集定位目标的点云数据和惯性数据， 并根据所述惯性数据确定 所述定位目标的预估位姿； 运动补偿模块， 用于利用所述惯性数据校正所述点云数据的运动畸变， 得到去畸变数 据； 融合定位模块， 用于根据所述预估位姿融合所述去畸变数据和所述惯性数据， 得到融 合定位数据， 并根据所述融合定位数据对所述定位目标进行定位。 9.一种电子设备， 包括存储器、 处理器及存储在所述存储器上并可在所述处理器上运 行的计算机程序， 其特征在于， 所述处理器执行所述程序时实现如权利要求1至7任一项所 述的自主定位方法。 10.一种非暂态计算机可读存储介质， 其上存储有计算机程序， 其特征在于， 所述计算 机程序被处 理器执行时实现如权利要求1至7任一项所述的自主定位方法。权　利　要　求　书 2/2 页 3 CN 115115702 A 3