(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111633902.5 (22)申请日 2021.12.2 9 (71)申请人 北京易航远智科技有限公司 地址 100015 北京市朝阳区酒仙桥北路7号 66幢一层0 07 (72)发明人 郑林飞　 (74)专利代理 机构 北京庚致知识产权代理事务 所(特殊普通 合伙) 11807 代理人 韩德凯 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/11(2017.01) G06T 5/00(2006.01) (54)发明名称 栅格地图更新方法、 装置、 电子设备及存储 介质 (57)摘要 本公开提供了基于位姿优化的栅格地图更 新方法， 包括： 对于测距传感器获取的载具的视 野场景的点云数据及IMU获取的载具的位姿数 据， 进行时间对齐以至少获取当前帧的载具位 姿， 去除载具运动导致的点云数据畸变； 进行点 云数据分割， 获取地面点云和非地面点云； 对当 前帧的载具位姿进行第一优化处理； 对第一优化 处理后的当前帧的载具位姿进行第二优化处理； 将当前帧的优化后的载具位姿与IMU获取的载具 的位姿数据进行时间对齐并进行融合处理； 基于 融合处理后的载具位姿， 使用地面点云以及非地 面点云对栅格地图进行更新。 本公开还提供了一 种栅格地图更新装置、 电子设备以及可读存储介 质。 权利要求书3页 说明书16页 附图12页 CN 114299145 A 2022.04.08 CN 114299145 A 1.一种基于位姿优化的栅格地图更新方法， 其特 征在于， 包括： S110、 对于测距传感器获取的载具的视野场景的点云数据及IMU获取的载具的位姿数 据， 进行时间对齐以至少获取当前帧的载 具位姿， 去除载 具运动导 致的点云数据畸变； S120、 将进行了时间对齐以及点云数据畸变去除的当前帧的点云数据分割为地面点云 和非地面点云； S130、 基于地面点云数据对当前帧的载具位姿进行第一优化处理， 以获得第一优化处 理后的载 具位姿； S140、 对进行了所述第一优化处理后的当前帧的载具位姿进行基于非地面点云的第二 优化处理， 以获得第二优化处 理后的载 具位姿； S150、 将当前帧的优化后的载具位姿与IMU获取的载具的位姿数据进行时间对齐并进 行融合处 理， 输出当前帧的融合处 理后的载 具位姿； 以及 S160、 基于所述融合处理后的载具位姿， 使用所述地面点云以及所述非地面点云对栅 格地图进行 更新。 2.根据权利要求1所述的基于位姿优化的栅格地图更新方法， 其特征在于， 步骤S110 中， 进行时间对齐以至少获取当前帧的载 具位姿， 包括： S1102、 基于IMU获取的载具的位姿数据获取载具在测距传感器获取当前帧点云数据时 刻的位姿数据。 3.根据权利要求2所述的基于位姿优化的栅格地图更新方法， 其特征在于， 步骤S110 中， 去除载 具运动导 致的点云数据畸变， 包括： S1104、 将当前帧点云数据转换到载具坐标系下， 基于当前帧的点云数据中每个点的时 间戳信息， 通过差值的方式获取每个点对应的IMU位姿， 并将每个点对应的IMU位姿转换到 当前帧的点云数据中初始点对应的IMU位姿下。 4.根据权利要求1所述的基于位姿优化的栅格地图更新方法， 其特征在于， S120、 将进 行了时间对齐以及点云数据畸变去除的当前帧的点云数据分割为地面点和非地面点， 包 括： 基于点云中各个点的高度值， 将当前帧的点云数据分割为 地面点和非地 面点。 5.根据权利要求4所述的基于位姿优化的栅格地图更新方法， 其特征在于， S130、 基于 地面点云数据对当前帧的载具位姿进行第一优化处理， 以获得第一优化处理后的载具位 姿， 包括： S1302、 对当前帧的地 面点云数据进行体素 滤波； S1304、 将滤波之后的当前帧的地面点云向栅格地图中作投影， 基于点云中投影到目标 栅格的各点的高度值与所述目标栅格的高程值的差值以及各点的高度值在所述目标栅格 中的概率值构建第一代价 函数； 以及 S1306、 对所述第一代价 函数进行迭代优化， 以获得优化后的载 具位姿。 6.根据权利 要求5所述的基于位姿优化的栅格地图更新方法， 其特征在于， S1304、 将滤 波之后的当前帧的地面点云向栅格地图中作投影， 基于点云中投影到目标栅格的各点的高 度值与所述目标栅格的高程值的差值以及各点的高度值在所述目标栅格中的概率值构建 第一代价 函数， 包括： 用当前帧时刻测距传感器获取的地面的高程值与当前帧时刻之前的至少一个帧时刻权　利　要　求　书 1/3 页 2 CN 114299145 A 2获取的地 面的高程 值的差值作为代价 值， 构建所述第一代价 函数。 7.根据权利要求6所述的基于位姿优化的栅格地图更新方法， 其特征在于， 所述第 一代 价函数为最小二乘函数； 优选地， S140、 对进行了所述第一优化处理后的当前帧的载具位姿进行基于非地面点 云的第二优化处 理， 以获得第二优化处 理后的载 具位姿， 包括： S1402、 对当前帧的非地 面点云数据进行体素 滤波； S1404、 将滤波之后的当前帧的非地面点云向栅格地图中作投影， 获取每个栅格中的高 程特征值， 基于当前帧的每个栅格中的高程特征值与当前帧之前帧的每个栅格中的高程特 征值的差值构建第二代价 函数； 以及 S1406、 基于CSM算法对第二代价 函数求解， 以获得优化后的载 具位姿； 优选地， 所述高程特 征值包括高程 最大值、 高程中值及高程 最小值； 优选地， S150、 将当前帧的优化后的载具位姿与 IMU获取的载具的位姿数据进行时间对 齐并进行融合处 理， 输出当前帧的融合处 理后的载 具位姿， 包括： S1502、 基于IMU传感器的时间戳对当前帧优化后的载 具位姿进行时间对齐处 理； 以及 S1504、 以当前优化后的载具位姿为基准， 对后续IMU传感器获取的位姿进行积分， 获取 当前帧的融合处 理后的载 具位姿； 优选地， S160中、 基于所述融合处理后的载具位姿， 使用所述地面点云对栅格地图进行 更新， 包括： A、 若点云对应的栅格中高程值为NAN， NAN表示未有高程值存储， 则将当前点的高程值 存入栅格中； B、 当点云对应的栅格 中已有高程值存储且栅格仅更新过一次时， 若当前点的高程值与 栅格中已存储的高程值的差值大于差值阈值， 则将当前栅格高程值置为NA N， 若差值小于差 值阈值， 则用当前值与已有值更新当前栅格的高程 值概率分布； C、 当点云对应的栅格中已有高程值存储且更新次数大于2时， 若当前点的高程值与栅 格中已存储的高程值的差值大于差值阈值， 则认 为当前点为异常点， 直接舍去； 若差值小于 差值阈值， 则用当前值与已有值更新当前栅格的高程 值概率分布； 优选地， S160中、 基于所述融合处理后的载具位姿， 使用所述非地面点云对栅格地图进 行更新， 包括： a、 对栅格地图进行全局设置： 采用四叉树数据结构对栅格地图中数据进行存储， 明确 树的深度、 明确栅格地图中每 个栅格的大小及设置栅格地图要建模的场景的大小； b、 将当前帧点云向栅格地图中投影： 根据每个点的坐标计算其对应的栅格的索引， 在 四叉树中与当前栅格对应的子节点， 进行递归查找直到找到叶子节点； 以及 c、 根据栅格中新加入的点， 更新当前栅格中存 储的统计量。 8.一种基于位姿优化的栅格地图更新装置， 其特 征在于， 包括： 预处理模块， 所述预处理模块对于测距传感器获取的载具的视野场景的点云数据及 IMU获取的载具的位姿 数据， 进行时间对齐以至少获取当前帧的载具位姿， 去除载具运动导 致的点云数据畸变； 点云数据分割模块， 所述点云数据分割模块将进行了时间对齐以及点云数据畸变去除 的当前帧的点云数据分割为 地面点云和非地 面点云；权　利　要　求　书 2/3 页 3 CN 114299145 A 3