(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210891419.5 (22)申请日 2022.07.27 (71)申请人 徐州徐工矿业机 械有限公司 地址 221000 江苏省徐州市徐州经济技 术 开发区和平大道169号 申请人 中国矿业大 学 (72)发明人 李志鹏　李会军　李凯　叶宾　 史继江　刘凯　徐明亮　 (74)专利代理 机构 徐州市三联专利事务所 32220 专利代理师 史海涛 (51)Int.Cl. G06T 17/20(2006.01) G06T 5/00(2006.01) G06T 7/73(2017.01) (54)发明名称 一种用于移动机械彩色点云地图构建的感 知系统及方法 (57)摘要 本发明属于一种点云地图构建技术领域， 具 体是一种用于移动机械彩色点云地图构建的感 知系统及方法， 感知系统包括固态激光雷达模 块、 双目相机模块、 惯性里程计模块和计算机， 计 算机包括时间戳同步模块、 动态障碍物检测模 块、 数据融合模块和显示器， 本发明通过固态激 光雷达模块、 双目相机模块采集环境的点云、 光 学图像， 通过惯性测量单元模块检测机械移动的 惯性数据； 并通过计算机进行数据处理、 校正、 插 值、 RGB赋值、 图像叠加构建彩色三维点云地图。 本发明获得的彩色三维点云地图， 相比于没有色 彩的点云和缺乏直接距离信息的二维图像而言， 三维彩色点 云表示的环境数据维度更多， 同时更 加直观的可视化反馈， 为三维重建技术提供数据 基础。 权利要求书3页 说明书7页 附图1页 CN 115222919 A 2022.10.21 CN 115222919 A 1.一种用于移动机械彩色点云地图构建的感知系统， 其特征在于， 在移动机械上安装 感知系统， 所述感知系统包括 用于采集周围环境的点云图像的固态激光雷达模块、 用于采集周围环境的光学图像的双目相机模块、 用于检测机 械移动时的惯性数据的惯性里程计模块、 用于接收固态激光雷达模块、 双目相机模块、 惯性里程计模块数据的计算机； 所述计算机包括 用于将固态激光雷达的时钟与计算机的主时钟同步的时间戳同步模块、 用于检测双目相机模块采集的带有动态障碍物的光学图像的动态障碍物检测模块、 用于将点云图像和光学图像融合形成彩色点云地图的数据融合模块 和用于彩色点云地图显示的显示器。 2.根据权利要求1所述感知系统 的的彩色点云地图构建方法， 其特征在于： 具体彩色点 云地图构建的步骤为 (1)预先标定双目相机模块的内参及畸变系数， 以及固态激光雷达到双目相机模块的 外参、 惯性里程计模块到双目相机模块的外参和固态激光雷达模块到惯性里程计模块的外 参， 并存储到数据融合模块中； (2)通过时间戳同步模块将固态激光雷达的时钟与计算机的主时钟同步； (3)计算机同步接收固态激光雷达模块采集周围环境的点云图像、 双目相机模块采集 周围环境的光学图像和惯性测量单 元模块检测机 械移动时的惯性数据； (4)计算机将获得的双目相机光学图像通过步骤(1)标定的内参与畸变系数进行校正， 接着动态障碍物检测模块将校正后的每帧图像采用深度神经网络Y OLOv3进行目标检测， 得 到带有动态障碍物的那一帧图像； (5)计算机将获得点云图像数据和惯性数据， 利用时间戳同步模块将两组数据时间同 步对齐， 然后将惯性数据插值获得当前帧固态激光雷达在世界坐标系 下的位姿， 基于得到 的当前帧固态激光雷达的运动状态对当前帧所有点云进行坐标变换， 从而对点云图像数据 的畸变进行 校正； (6)对校正后的点云图像数据进行点云插值， 得到稠密化后的点云图像； (7)将稠密化后的点云图像结合步骤(1)标定的固态激光雷达到双目相机模块的外参 与双目相 机模块的内参， 使点云图像投影到双目相 机的uv坐标系下 的成像平面； 结合步骤 (4)获得的带有动态障碍物的那一帧图像， 将与其对应的像素坐标 处的点云图像数据剔除， 接着将成像平面上光学图像各个像素位置的RGB信息赋值给对应的点云图像， 进而获得周 围环境的每一帧彩色稠密点云图像； 最后将得到的每一帧彩色稠密点云图像叠加， 从而获 得周围环境的彩色三维点云地图。 3.根据权利要求2所述的彩色点云地图构建方法， 其特征在于： 所述步骤(1)中双目相 机模块的内参为K及畸变系数为k1， k2， k3， p1， p2； 固态激光雷达到双目相机模块的外参为 惯性里程计模块到双目相机模块的外参为 固态激光雷达模块到惯性里程计模块的 外参为 4.根据权利要求2所述的彩色点云地图构建方法， 其特征在于： 所述步骤(4)中校正过权　利　要　求　书 1/3 页 2 CN 115222919 A 2程为： 设未校正前双目相机模块获得的图像在uv坐标系下的各个像素坐标为(u， v)， 结合步 骤(1)标定的内参与畸变系数并根据下式对各个 像素坐标进行 校正： 其中， (u‘,v')为图像校正后的各像素坐标； r为像素点距离成像中心的距离 。 5.根据权利要求2所述的彩色点云地图构建方法， 其特征在于： 所述步骤(5)的具体过 程为： 设激光雷达第k帧的点云图像数据表示 为： 通过第k帧帧头前后两帧的惯性数据位姿插值得到的第k帧帧头对应的惯性数据位姿 表示为： 其中 为点云图像数据第k帧第i 点到相应局部 世界坐标系的旋转矩阵， 为点云图像 数据第k帧第i 点到相应局部世界坐标系的平 移矩阵； 利用同样的方法能得到第k帧帧中和帧尾的惯性数据位姿为 进行二次曲线拟 合得到关于时间的点云位姿曲线拟合方程： 依据此方程能得到校正 点云畸变的公式为： 从而对点云图像数据的畸变进行 校正。 6.根据权利要求2所述的彩色点云地图构建方法， 其特征在于： 所述步骤(6)的具体过 程为： 根据固态激光雷达非重复式扫描的特性， 隔行选取四个邻近点， 进行插值处 理： 在固态激光雷达三根扫描线的第一根和第三根上分别选取两个邻近点记为Q11(x1,y1, z1),Q12(x1,y2,z2),Q21(x2,y1,z3),Q22(x2,y2,z4)， 记R1,R2为在x方向上插值得到的两个点， P 为最终插值得到的点， 则有： 总结成为多 项式的形式为: z＝a0+a1x+a2y+a3xy, a0＝f(Q11), a1＝f(Q21)‑f(Q11), a2＝f(Q12)‑f(Q11), a3＝f(Q22)‑f(Q21)‑f(Q12)+f(Q11),权　利　要　求　书 2/3 页 3 CN 115222919 A 3