(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210768447.8 (22)申请日 2022.07.01 (71)申请人 白犀牛智达 （北京） 科技有限公司 地址 100084 北京市海淀区信息路15号3层 315-2 (72)发明人 袁睿　徐晨　 (74)专利代理 机构 北京睿驰通 程知识产权代理 事务所(普通 合伙) 11604 专利代理师 张文平 (51)Int.Cl. G06T 7/70(2017.01) G06T 7/30(2017.01) G06T 17/00(2006.01) G06T 17/05(2011.01) (54)发明名称 一种位姿初始化方法 (57)摘要 本公开提供了一种位姿初始化方法， 本公开 利用扫描帧点云和局部点云地图， 生成候选位姿 集的八叉树， 进而利用预设收敛算法确定当前最 佳节点的候选位姿。 不依赖与高精度地图， 而是 通过点云数据便能够获得当前位姿， 大大降低计 算量。 不依赖 特殊场景约束， 适用于多种场景。 实 现了定位 设备在GNSS信号弱环境下的定位。 利用 旋转直方图匹配筛选错误旋转候选结果， 利用预 设收敛算法对所述八叉树的节点进行搜索和剪 枝， 避免了现有技术中的暴力搜索， 降低了算法 复杂度， 减少了 计算量， 提高了数据处 理效率。 权利要求书2页 说明书12页 附图2页 CN 115147482 A 2022.10.04 CN 115147482 A 1.一种位姿初始化方法， 其特 征在于， 包括： 获取当前的估计位置和扫描帧点云； 在预设点云地图中基于所述估计位置截取局部点云地图； 基于至少两种预设分辨率分别对所述局部点云地图进行滤波， 生成对应预设分辨率的 概率格栅地图， 其中， 所述概率格栅地图包括三 维空间中的多个网格， 至少一个网格包括所 述局部点云地图中点数据的投影坐标， 所述网格的网格值等于所述投影坐标在所述网格中 分布的概 率值； 基于所述扫描帧点云生成候选位姿集， 其中， 所述候选位姿集包括多组组合的候选位 姿； 基于所述至少两种预设分辨率的概率格栅地图共同构建所述候选位姿集的八叉树， 其 中， 所述八叉树中每层深度表征一种 预设分辨率且深度越小对应的预设分辨率越高， 所述 八叉树的每个节点有八个子节点或零个子节点， 每个节点包括候选位姿和节点评分， 每个 节点的节点评分等于对应节点的候选位姿在所述概率格栅地图中投影坐标所在 网格的网 格值的平均值， 所述八叉树中父节点的节点评分等于其子节点中最大的节点评分； 基于预设收敛算法对所述八叉树的节点进行搜索和剪枝， 确定当前最佳节点的候选位 姿。 2.根据权利要求1所述的方法， 其特征在于， 所述基于预设 收敛算法对所述八叉树的节 点进行搜索和剪枝， 确定当前最佳节点的候选位姿， 包括： 基于预设收敛算法和预设自适应阈值对所述八叉树的节点进行搜索和剪枝， 确定当前 最佳节点的候选位姿。 3.根据权利要求2所述的方法， 其特征在于， 所述基于预设 收敛算法和预设自适应 阈值 对所述八叉树的节点进行搜索和剪枝， 确定当前最佳节点的候选位姿， 包括： 对所述八叉树中的节点进行搜索， 获取搜索到的多个节点； 当所述多个节点均为叶子节点时， 当所述多个节点的深度均为预设深度， 且所述多个 节点的节点评分均大于预设自适应阈值时， 确定所述多个节点均为目标节点； 从所述多个目标节点的节点评分中确定最大值所对应的目标节点为所述当前最佳节 点， 以及所述当前最佳节点的候选位姿。 4.根据权利要求3所述的方法， 其特征在于， 所述对所述八叉树中的节点进行搜索， 获 取搜索到的多个节点之后， 还 包括： 当所述多个节点均为叶子节点时， 当所述多个节点的深度均小于预设深度， 且所述多 个节点的节点评分均大于预设自适应阈值时， 基于所述多个节点中任一节点的下一深度所 对应的预设分辨率和对应节点的候选位姿分裂对应节点， 生成对应节点的八个子节点， 直 至分裂后的子节点的深度等于预设深度。 5.根据权利要求3所述的方法， 其特征在于， 所述对所述八叉树中的节点进行搜索， 获 取搜索到的多个节点之后， 还 包括： 当所述多个节点均为父节点时， 当所述多个节点的深度均小于预设深度， 且所述多个 节点的节点评分均小于或等于预设自适应阈值时， 对所述多个节点分别进行剪枝。 6.根据权利要求1所述的方法， 其特征在于， 所述基于所述扫描帧点云生成候选位姿 集， 包括：权　利　要　求　书 1/2 页 2 CN 115147482 A 2针对所述扫描帧点云， 以预设角度步长生成候选旋转集， 且以预设位置步长生成候选 位置集， 其中， 所述候选旋转集包括多组候选旋转角度， 所述候选位置集包括多个候选位 置； 将所述候选旋转集中的各组候选旋转角度与所述候选位置集中的各个候选位置进行 两两组合， 生成所述 候选位姿集， 其中， 所述 候选位姿集包括多组候选位姿。 7.根据权利要求6所述的方法， 其特 征在于， 所述方法还 包括： 基于所述局部点云地图生成第一旋转直方图， 其中， 所述第一旋转直方图中记录着局 部点云地图中点数据的第一投影坐标； 相应地， 所述针对所述扫描帧点云， 以预设角度步长生成候选 旋转集， 包括： 基于所述扫描帧点云生成第二旋转直方图， 其中， 所述第二旋转直方图中记录着扫描 帧点云中点数据的第二投影坐标； 基于所述第一旋转直方图中的第一投影坐标对所述第二旋转直方图中的第二投影坐 标进行筛 选， 获取匹配的第二投影坐标； 从所述扫描帧点云中获取与匹配的第二投影坐标相关的点数据； 针对所述扫描帧点云中与匹配的第 二投影坐标相关的点数据， 以预设角度步长生成候 选旋转集。 8.根据权利要求1所述的方法， 其特征在于， 所述确定当前最佳节点的候选位姿之后， 还包括： 以所述当前最佳节点的候选位姿为初始位姿， 利用依赖初始位姿的预设点云配准算法 获取评估距离值或优化评分； 当所述评估距离值或所述优化评分不满足预设精度 条件时， 重新确定另一最佳节点的 候选位姿。 9.根据权利要求8所述的方法， 其特征在于， 所述当所述评估距离值或所述优化评分不 满足预设精度条件时， 重新确定另一 最佳节点的候选位姿， 包括： 当所述评估距离值或所述优化评分大于预设第 一精度阈值， 且小于或等于预设第 二精 度阈值时， 减少所述候选位姿集中的候选位姿， 生成新的候选位姿集， 且触发所述基于所述 至少两种预设 分辨率的概率格栅地图共同构建所述 候选位姿集的八叉树的操作执 行。 10.根据权利要求8所述的方法， 其特征在于， 所述当所述评估距离值或所述优化评分 不满足预设精度条件时， 重新确定另一 最佳节点的候选位姿， 包括： 当所述评估距离值或所述优化评分大于预设第 二精度阈值 时， 触发所述获取当前的估 计位置和扫描帧点云的操作执 行。 11.根据权利要求8所述的方法， 其特 征在于， 所述方法还 包括： 当所述评估距离值或所述优化评分小于或等于预设第 一精度阈值 时， 确定所述当前最 佳节点的候选位姿为当前位姿。 12.根据权利要求1所述的方法， 其特 征在于， 所述获取当前的估计位置， 包括： 通过全球导 航卫星系统获取当前多个返回位置分别与实际位置的方差值； 确定任一满足预设差距条件的方差值所对应的返回位置为当前的估计位置 。权　利　要　求　书 2/2 页 3 CN 115147482 A 3