(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210929111.5 (22)申请日 2022.08.03 (71)申请人 广州高新兴机 器人有限公司 地址 510530 广东省广州市黄埔区科 学城 开创大道 2819号A6 08房 （仅限办公） (72)发明人 李瀚文　柏林　刘彪　舒海燕　 袁添厦　沈创芸　祝涛剑　方映峰　 (74)专利代理 机构 广州国鹏知识产权代理事务 所(普通合伙) 44511 专利代理师 葛红 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/20(2006.01) G01S 17/86(2020.01) G01S 17/88(2006.01)G06T 7/73(2017.01) (54)发明名称 一种基于动态场景的定位方法、 装置、 机器 人及介质 (57)摘要 本发明提供了一种基于动态场景的定位方 法， 其包括如下步骤： S1， 使用获取的3D激光雷达 数据， 并基于先验地图进行定位； S2， 在基于先验 地图进行定位结果不正确时， 使用激光里程计获 取机器人的姿态Tm； S3， 将当前帧和上一帧激光 里程计得到的姿态变换加入到因子图， 因子图中 存在上一时刻的位姿， 通过激光里程计的预测位 姿变换转换得到当前时刻位姿初值， 利用此初值 作为观测ND T配准的初 始值， 最终ND T配准的结果 为当前的准确位姿。 本发明首先使用激光雷达的 先验地图进行姿态匹配， 在匹配失效的情况， 使 用激光里程计获取当前帧与上一帧姿态的姿态 变换， 并使用激光里程计获取的姿态变换作为观 测的初始值获取最终姿态。 权利要求书2页 说明书9页 附图2页 CN 115326051 A 2022.11.11 CN 115326051 A 1.一种基于动态场景的定位方法， 其包括如下步骤： S1， 使用获取的3D激光雷达数据， 并基于先验地图进行定位； S2， 在基于先验地图进行定位结果 不正确时， 使用激光里程计获取机器人的姿态Tm； S3， 将当前帧和上一帧激光里程计得到的姿态变换加入到因子图， 因子图中存在上一 时刻的位姿， 通过激光里程计的预测位姿变换转换得到当前时刻位姿初值， 利用此初值作 为观测NDT配准的初始值， 最终NDT配准的结果 为当前的准确位姿。 2.根据权利要求1所述的方法， 所述基于先验地图进行定位是基于NDT配准。 3.根据权利要求2所述的方法， 所述步骤S1具体包括： S11， 将先验地图map划分为立方 体； S12， 计算每一个立方体的概 率分布模型； S13， 将激光雷达得到的当前帧根据先验姿态变换到map下对应的立方体中得到对应的 转换点x′i； S14， 根据所述 概率分布模型计算每 个转换点 落在对应立方体中的概 率密度； S15， 通过每 个立方体 计算出的概 率密度相加得到NDT配准得分 score； S16， 根据牛 顿优化算法对score进行优化， 找到最优姿态使得score的值 最大； S17， 判断所述score是否超过阈值， 如 未超过阈值， 则基于先验地图进行定位结果不正 确。 4.根据权利要求1所述的方法， 所述 步骤S2具体包括： S21， 获取关键帧， 并将所述关键帧加入关键帧队列， 同时保存所述关键帧的激光姿态； S22， 获取子图submap； S23， 对当前帧与上一帧进行GICP算法配准， 得到 两帧之间的相对姿态变换Ts； S24， 将Ts与上一帧的姿态相乘， 得到当前帧在先验地图的坐标系下的姿态预测值 S25， 利用姿态预测值 并进行GICP算法进行当前帧与子图submap的配准， 得到当前 帧在先验地图的坐标系的姿态Tm。 5.根据权利要求 4所述的方法， 所述 步骤S21具体包括： 判断激光里程计获取当前帧是否为第一帧， 如果是第一帧， 则将当前帧加入到关键帧 队列， 并保存当前激光姿态； 否则将判断当前帧与上一帧是否超过了第一阈值， 或角度转过了第一阈值度， 如果是， 则将当前帧加入到关键帧队列中。 6.一种基于动态场景的定位装置， 其包括如下 单元： 先验地图定位单 元， 用于使用获取的3D激光雷达数据， 并基于先验地图进行定位； 激光姿态计算单元， 用于在基于先验地图进行定位结果不正确时， 使用激光里程计获 取机器人的姿态Tm； 姿态修正单元， 用于将当前帧和上一帧激光里程计得到的姿态变换加入到因子 图中， 因子图中存在上一时刻的位姿， 通过激光里程计的预测位姿变换转换得到 当前时刻位姿初 值， 利用此初值作为观测NDT配准的初始值， 最终NDT配准的结果 为当前的准确位姿。 7.根据权利要求6所述的装置， 所述先验地图定位单 元还包括： 立方体划分单 元， 用于将先验地图map划分为 立方体；权　利　要　求　书 1/2 页 2 CN 115326051 A 2概率分布模型获取 单元， 用于计算每一个立方体的概 率分布模型； 当前帧转换单元， 用于将激光雷达得到的当前帧根据先验姿态变换到map下对应的立 方体中得到对应的转换点x ′i； 概率密度计算单元， 用于根据 所述概率分布模型计算每个转换点落在对应立方体 中的 概率密度； NDT配准得分计算单元， 用于通过每个立方体计算出的概率密度相加得到NDT配准得分 score； NDT配准得分优化单元， 用于根据牛顿优化算法对score进行优化， 找到最优姿态使得 score的值 最大； 先验地图定位结果判断单元， 用于判断所述score是否超过阈值， 如未超过阈值， 则基 于先验地图进行定位结果 不正确。 8.根据权利要求6所述的装置， 所述激光姿态计算单 元还包括： 关键帧获取单元， 用于获取关键帧， 并将所述关键帧加入关键帧 队列， 同时保存所述关 键帧的激光姿态； 子图获取 单元， 用于获取子图submap； 帧间姿态获取单元， 用于对当前帧与上一帧进行GICP算法配准， 得到两帧之间的相对 姿态变换Ts； 帧间姿态预测单元， 用于将 Ts与上一帧的姿态相乘， 得到当前帧在先验地图的坐标系下 的姿态预测值 子图姿态获取单元， 用于利用姿态预测值 并进行GICP算法进行当前帧与子图 submap的配准， 得到当前帧在先验地图的坐标系的姿态Tm。 9.根据权利要求8所述的装置， 关键帧获取 单元还包括： 判断激光里程计获取当前帧是否为第一帧， 如果是第一帧， 则将当前帧加入到关键帧 队列， 并保存当前激光姿态； 否则将判断当前帧与上一帧是否超过了第一阈值， 或角度转过了第一阈值度， 如果是， 则将当前帧加入到关键帧队列中。 10.一种机器人， 所述机器人包括一中央处理器， 一存储器， 一3D激光雷达， 一激光里程 计， 所述所述存储器上存储有指 令， 所述处理器在执行所述指 令时用以实现权利要求 1‑5中 任一项所述的方法。权　利　要　求　书 2/2 页 3 CN 115326051 A 3