(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210951441.4 (22)申请日 2022.08.09 (71)申请人 杭州云深处科技有限公司 地址 310063 浙江省杭州市西湖区三墩镇 欣然街36号紫金梦想广场3幢6层601 室 (72)发明人 胡雪亮　贺晓伟　李超　朱秋国　 (74)专利代理 机构 浙江杭州金通专利事务所有 限公司 3 3100 专利代理师 金杭 (51)Int.Cl. G06T 7/73(2017.01) G06T 17/05(2011.01) G06V 10/75(2022.01) G06T 5/50(2006.01) (54)发明名称 一种融合激光雷达和深度相机点云的建图 定位方法 (57)摘要 本发明公开了一种融合激光雷达和深度相 机点云的建图定位方法， 包括以下步骤： S1， 获取 待处理的激光点云和深度相机点云； S2， 进行外 参标定， 确定激光雷达与深度相机坐标轴关系， 将深度相机点云转换到激光雷达坐标系； S3， 对 转换得到的激光雷达坐标系的激光雷达和深度 相机点云按照激光雷达扫描线进行重新分割， 并 对深度相机点云按照激光雷达的点云格式进行 转换， 融合形成一帧相同格式的点云； S4， 得到环 境的点云图； S5， 得到四足机器人在环境中的绝 对位姿， 融合IMU传感器和足式里程计得到四足 机器人在运动过程中的位姿。 本发 明可以得到准 确的定位信息， 可 以适应换流站的复杂环境； 使 用的计算资源少， 可以将算法搭载在一般的计算 平台上； 重复定位高。 权利要求书3页 说明书8页 附图3页 CN 115330866 A 2022.11.11 CN 115330866 A 1.一种融合激光雷达和深度相机点云的建图定位方法， 其特 征在于， 包括以下步骤： S1， 获取待处理的激光点云和深度相机点云， 包括其属于同一空间范围和不属于同一 空间范围的点云； S2， 对激光雷达和深度相机进行外参标定， 确定激光雷达与深度相机坐标轴关系， 将深 度相机点云转换到 激光雷达坐标系； S3， 根据深度相机与激光雷达的视场角， 对转换得到的激光雷达坐标系的激光雷达和 深度相机点云按照激光雷达扫描线进 行重新分割， 并对深度相机点云按照激光雷达的点云 格式进行转换， 融合形成一帧相同格式的点云； S4， 基于S3中融合后的点云与IMU传感器数据融合， 进行环境的建图得到环境的点云 图； S5， 基于S3中融合的点云匹配S4中建立的点云图得到四足机器人在环境中的绝对位 姿， 融合IMU传感器和足式里程计得到四足机器人在运动过程中的位姿。 2.根据权利要求1所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S1步骤 包括以下子步骤， S11， 设置使采集激光雷达点云与采集深度相机点云时间进行同步的装置； S12， 滤除深度相机设定范围内测量 不准确的深度点云； S13， 滤除激光雷达在 设定范围内测量不准确的激光雷达点云， 以及打在安装激光雷达 设备上的点云； S14， 滤除激光雷达与深度相机噪点。 3.根据权利要求1所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S2步骤 包括以下子步骤， S21， 标定所述激光雷达和所述深度相机之间的外参， 包括深度相机到激光雷达坐标系 的旋转矩阵R和平移向量t； 准备好黑白棋盘格， 采集若干张深度相 机和激光雷达均能观察 到完整棋盘格的相机图像和 激光雷达点云， 将其按时间近似同步进行配对， 导入Matlab中 的Lidar Toolbox工具箱中， 让工具箱检测图像中的棋盘格， 在激光雷达点云帧中手动 指定 落在棋盘格上的点云， 使工具箱进行标定， 得到深度相机到激光雷达坐标系的旋转矩阵R和 平移向量t； S22， 构建深度相机坐标系到 激光雷达坐标系的转换函数， 该转换函数为， 其中， Xl， Yl， Zl表示激光雷达坐标系的三个坐标轴， Xc， Yc， Zc表示深度相机坐标系； S23， 根据步骤S2 1中标定得到的深度相机到激光雷达坐标系的旋转矩阵R和平移向量t 以及步骤S2 2中的转换函数， 将 深度相机点云转换至 激光雷达坐标系； S24， 根据激光雷达与深度相机的视场角， 确定获取得到的深度相机与激光雷达属于同 一空间范围的点云与不属于同一空间范围的点云。 4.根据权利要求3所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S3步骤 包括以下子步骤， S31， 根据激光雷达与各个深度相机 融合的视场角， 按照激光雷达扫描线的概念以激光权　利　要　求　书 1/3 页 2 CN 115330866 A 2雷达垂直角分辨 率对激光雷达点云和各个深度相机点云进行线 束划分； S32， 对步骤S31中重新划分的深度相机点云转化为激光雷达格式点云， 再融合激光雷 达点云， 形成相同格式的融合 点云进行输出。 5.根据权利要求4所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S31步骤 包括以下子步骤， S311， 根据步骤S24中确定的深度相机与激光雷达点云属于同一空间范围的点云， 以深 度相机点云补足激光雷达线 束之间空位 点， 并重新计算属于该线 束的点云； S312， 根据步骤S24中确定的深度相机与激光雷达点云不属于同一空间范围的点云， 对 于只含有激光雷达点云空间保留原有的线束定义， 对于只含有深度相机点云的空间以激光 雷达角分辨 率向上以及向下扩充 线束， 在线束周围的点云 即认定为该线 束点云。 6.根据权利要求5所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S311步骤中， 对于同一空间范围内的相机点云和激光雷达点云线束的分割， 根据激光 雷达分辨率， 在激光雷达线束正负二分之一激光雷达角分辨率的相机点云划分为该线束点 云。 7.根据权利要求5所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S312步骤中， 对于只含有深度相机点云的空间， 根据激光雷达 分辨率， 以扩充线束正负 二分之一激光雷达角分辨 率的相机点云划分为该线 束点云。 8.根据权利要求4所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S32步骤 包括以下子步骤， S321， 根据步骤S31对激光雷达和相机点云重新划分线束之后， 按照所划分的线束确定 以及分配 每个激光点以及深度相机点的线 束信息； S322， 增加深度相机点云反射强度信息； S323， 以激光雷达点云发布的时间戳作为融合 点云的时间戳。 9.根据权利要求8所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在于： 所述S322步骤中， 对于和 激光雷达点云同一空间范围内的深度相机点云， 按据深度相机最 近激光雷达点的反射 强度确定反射强度； 对于和激光雷达点云不同空间范围内的深度相机 点云， 在激光雷达区域下方的相 机点云， 以激光雷达照射地面的反射强度 统一为深度相 机 点云分配反射强度， 在激光雷达区域上方的相 机点云， 以激光雷达照射白墙的反射强度 统 一为深度相机点云 分配反射强度信息 。 10.根据权利要求1所述的融合激光雷达和深度相机点云的建图定位方法， 其特征在 于： 所述S4 步骤包括以下子步骤， S41， 对融合点云进行预处理， 利用惯导辅助融合点云的非线性运动畸变去除， 对每一 帧内的点云进行补偿校正； S42， 对融合点云进行特征提取， 通过提取融合点云的面特征和线特征， 其中面特征又 包括地面特征和非地 面特征， 过滤非特征点数据； S43， 通过NDT匹配方法对点云进行帧图匹配， 得到四足机器人两帧点云之间的运动位 姿信息， 初始点云图是第一帧融合点云数据， 直到点云关键帧累积到五帧再逐渐淘汰前面 的点云帧， 通过惯导积分的位姿变化当超过某一阈值时认定点云数据为关键帧点云数据； 其中NDT假设点云服从正态分布， 假设当前扫描得到的点云为 用空间转权　利　要　求　书 2/3 页 3 CN 115330866 A 3