(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211055396.0 (22)申请日 2022.08.31 (71)申请人 华中农业大 学 地址 430070 湖北省武汉市洪山区狮子山 街1号 (72)发明人 韩明兴　孔哲　杨天乐　王树才　 熊利荣　马欢歌　李淼　徐坤　 (74)专利代理 机构 武汉开元知识产权代理有限 公司 42104 专利代理师 樊戎　张继巍 (51)Int.Cl. G06T 17/05(2011.01) G06T 5/50(2006.01) G06T 7/30(2017.01) G06V 10/74(2022.01) (54)发明名称 一种基于双俯仰旋转2D激光雷达数据融合 的地图构建方法 (57)摘要 本发明提供一种一种基于双俯仰旋转2D激 光雷达数据融合的地图构建方法， 对 前置雷达和 后置雷达进行标定； 采用RANSAS算 法分别对前置 雷达和后置雷达进行帧间点云数据粗配准； 采用 点云相似度匹配算法进行雷达帧间点云数据的 补充， 获得融合后帧间点云数据； 重复步骤得到 下一时刻的融合后帧间点云数据； 利用ICP算法 对融合后帧间点云数据进行精确配准。 使用双激 光雷达， 即前置雷达和后置雷达， 前置雷达受限 于车身长度和雷达安装位置无法采集到的堆肥 背坡完整的信息， 所以利用后置雷达数据进行补 充， 从而解决前置雷达视野盲区问题， 同时实现 精确的帧间点云配准。 权利要求书3页 说明书7页 附图1页 CN 115496868 A 2022.12.20 CN 115496868 A 1.一种基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征在于： 所述构建 方法如下： 步骤1： 对前置雷达和后置雷达进行 标定； 步骤2： 采用RANSAS算法分别对前置雷达和后置雷达进行帧间点云数据粗配准； 步骤3： 采用点云相似度匹配算法进行雷达帧间点云数据的补充， 获得融合后帧间点云 数据； 步骤4： 重复步骤2 ～步骤3得到下一时刻的融合后帧间点云数据； 步骤5： 利用ICP算法对步骤3和步骤4得到的融合后帧间点云数据进行精确配准。 2.根据权利要求1所述基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征 在于： 所述 步骤2的具体过程 为： 采集前置雷达在时刻t的m个帧间点云数据p1、 p2、 ...、 pm， 采集前置雷达在时刻t+ △t 的m个帧间点云数据q1、 q2、 ...、 qm； 采用RANSAS算法对时刻t和时刻t+ △t的帧间点云数据 进行粗配准， 得到粗配准后前置雷达t+ △t时刻z个帧间点云数据L1、 L2、 ...、 Lz； 同理， 采集 后置雷达在时刻t+δt的m个帧间点云数据p1 ’、 p2’、 ...、 pm’， 采集后置雷达在时刻t+δt+ △t 的m个帧间点 云数据q1 ’、 q2’、 ...、 qm’； 利用RANSA C算法对时刻 t+δt和时刻t+δt+ △t的帧间 点云进行点云粗配准， 得到粗配准后后置雷达t+δt+ △t时刻z个帧间点云数据L ’1、 L’ 2、 ....、 L’z。 3.根据权利要求2所述基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征 在于： 所述 步骤3的具体过程 为： 3a)将t+△t时刻的前置雷达帧间点云数据L1、 L1、 ...、 Lz划分为n个区域即A1、 A2、 ...、 An， 分别计算前置雷达n个区域帧间点云数据的协方差V1、 V2、 ...、 Vn， 然后计算每个协方差 对应的马氏距离d1、 d2、 . ..、 dn； 将t+δt+ △t时刻的后置雷达帧间点云数据L ’1、 L’2、 ...、 L’z划分为n个区域即B1、 B2、 ...、 Bn， 且B1、 B2、 ...、 Bn的大小与A1、 A2、 ...、 An呈一一对应相同； 按照计算马氏距 离d1 的方法依次计算B1、 B2、 . ..、 Bn对应的马氏距离d1 ’、 d2’、 ...、 dn’； 3b)将前置雷达的每 个区域分别与后置雷达的n个区域进行一 一配对， 具体过程 为： 计算B1的马氏距离d1 ’与A1的马氏距离d1的差值的绝对值即D11＝|d1 ’ ‑d1|， 依次类推 计算D12＝|d1 ’ ‑d2|、 D13＝|d1 ’ ‑d3|、…、 D1n＝|d1’ ‑dn|； 同理依次计算D21、 D22、 …、 D2n， D21、 D22、 …、 D2n，……， Dn1、 Dn2、 …、 Dnn， 并将D11、 D12、…、 Dnn分别与预设相似度阈值S2进行比较， 大于预设相似度阈值S2则匹配成功， 并记 录匹配成功的区域数目； 若匹配成功的区域数目小于预设的匹配数目阈值S3则执行步骤 3e)， 若匹配成功的区域数目大于预设的匹配数目阈值S3则执 行步骤3c)； 3c)计算点云补充变换矩阵H1； 3d)将后置雷达t+δt+ △t时刻z个帧间点云数据L ’1、 L’2、 ....、 L ’z通过点云补充转换 矩阵H1补 充到前置雷达t+ △t时刻z个帧间点云数据L1、 L2、 ...、 Lz中完成融合获得融合的k 个帧间点云数据O1、 O2、 . ..Ok； 3e)当迭代次数小于等于预设的迭代次数时， 将后 置雷达t+δt+ △t时刻z个帧间点云数 据L’1、 L’2、 ....、 L ’z和前置雷达t+ △t时刻z个帧间点云数据L1、 L2、 ...、 Lz进行重新划分 区域重复步骤3a)～3b)； 当迭代次数大于预设 的迭代次数则停止迭代， 直接将前置雷达t+权　利　要　求　书 1/3 页 2 CN 115496868 A 2△t时刻z个帧间点云数据L1、 L2、 . ..、 Lz进行精确配准， 同时迭代次数清零。 4.根据权利要求3所述基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征 在于： 所述 步骤3a)中， 马氏距离d1具体 计算过程如下： d1＝[(x1 ‑μ1)TV1‑1(x1‑μ1)]1/2， μ1＝( μ1 1, μ12,..., μ1e)T； x1为均值 μ1、 协方差V1的向量， 即x1＝(x1 1,x12,...,x1e)T 同理依次计算马氏距离d2、 d3、 . ..、 dn。 5.根据权利要求3所述基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征 在于： 所述 步骤3c)中， 点云补充变换矩阵H1计算过程如下： 设定一个三维坐标向量 在三维坐标系中 绕z轴旋转θ1角得到旋转矩阵 在三维坐标系中 绕x轴旋转θ1 ’角得到旋转矩阵 在三维坐标系中 绕y轴旋转θ1 ”角得到旋转矩阵 获得点云补充旋转矩阵 在x轴 上平移x1、 在y轴上平 移y1、 在z轴上平 移z1获得点云补充平 移矩阵 帧间点云数据只存在旋转和平移变换， 不存在形变， 比例因子为1， 即获得点云补充转 换矩阵 6.根据权利要求3所述基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征 在于： 所述 步骤4的具体过程 为： 重复步骤2～步骤3得到t+δt+ △t’时刻融合后的k个帧间点云数据W1、 W2、 ...Wk， △t’> △t。 7.根据权利要求6所述基于双俯仰旋转2D激光雷达数据融合的地图构建方法， 其特征 在于： 所述 步骤5的具体过程 为： 5a)计算精确配准变换矩阵H2；权　利　要　求　书 2/3 页 3 CN 115496868 A 3