一种实时雷达视觉融合的室内场景采集建模方法及系统与流程

本发明属于计算机视觉的同步定位与地图构建领域，具体涉及一种实时雷达视觉融合的室内场景采集建模方法及系统。

背景技术：

1、同步定位与地图构建(simultaneous localization and mapping，简称slam)指的是：机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的环境特征定位自身位置和姿态，再根据自身位置构建周围环境的增量式地图，从而达到同时定位和地图构建的目的。slam技术分为传感器数据采集、前端里程计、后端非线性优化、回环检测以及建图五个部分。根据传感器数据采集的类型不同，现有slam方案主要包括激光slam方案以及视觉slam方案。(1)视觉slam方案。以rtab-map为代表的视觉slam方案从功能角度上分为图像数据采集、视觉里程计、后端非线性优化、回环检测以及建图五个部分，其中图像数据采集模块通过相机对环境的2d视觉数据进行获取，视觉里程计通过不同时刻和不同位置的2d图像(由于运动产生的图像变化)对3d立体信息进行预测计算，通过后端非线性优化和回环检测进而估计自身位姿的过程。输入为图像、视频序列，输出相机运动轨迹和局部地图。在建图过程中将当前计算得到的相机运动轨迹和局部地图匹配拼接到原有地图中。地图融合将这一轮来自激光雷达的新数据拼接到原始地图当中，最终完成地图的更新。(2)激光slam方案。以lio-sam为代表的激光slam，与视觉slam方案不同，激光slam方案以3d点云作为直接输入数据。从功能角度上分为点云数据采集、激光里程计、后端非线性优化、回环检测以及建图五个部分。其中点云数据采集通过激光雷达或其他传感器获取所在位置的环境信息，然后对激光雷达原始数据进行优化，剔除一些有问题的数据，或者进行滤波。激光里程计不再预测3d立体信息，而是直接将当前局部环境的点云数据在已建立的地图上寻找对应的位置，匹配的好坏对slam构建地图的精度有直接的影响。在slam过程中，需要将激光雷达当前采集的点云匹配拼接到原有地图中。后端非线性优化、回环检测以及建图模块与视觉slam方案一致。视觉slam方案因为直接采集数据为2d图像，因此经过计算获得的3d立体信息精度较低、计算成本高、速度较慢；激光slam方案由于缺少视觉数据作为输入，因此最终生成的地图缺少颜色信息，对于实际户外环境的应用产生较大的限制。此外两种技术方案实际使用时需要实施繁琐的标定实现传感器之间的外参标定。

技术实现思路

1、本发明要解决的技术问题：针对现有技术的上述问题，提供一种实时雷达视觉融合的室内场景采集建模方法及系统，本发明旨在利用雷达数据和图像数据实现雷达激光点云的3d信息和相机的2d图像信息相互配准与融合，实现具有颜色信息的3d实时重建，克服视觉slam中的速度慢和3d精度差的问题，补充激光slam方案中缺乏颜色信息的缺点，达到兼顾重建速度、精度与信息丰富度的目的。

2、为了解决上述技术问题，本发明采用的技术方案为：

3、一种实时雷达视觉融合的室内场景采集建模方法，包括：

4、s101，确定室内场景中初始的当前位置i；

5、s102，在当前位置i分别采集雷达数据和图像数据；

6、s103，基于雷达数据和图像数据，获取当前位置i的雷达位姿δti和图像位姿δki，将当前位置i的雷达位姿δti和图像位姿δki融合得到当前位置i的融合位姿变换矩阵δi，并将当前位置i的雷达数据、图像数据进行位姿变化后融合建立当前位置i的局部三维模型pic；

7、s104，根据当前位置i的融合位姿变换矩阵δi，将当前位置i的局部三维模型pic增加到上一位置i-1的全局地图pi-1all中，从而得到当前位置i的全局地图piall；

8、s105，移动到新的当前位置i，判断新的当前位置i为初始的当前位置i是否成立，若成立，则此时当前位置i的全局地图piall为室内场景的采集建模结果，结束并退出，否则，跳转步骤s102以继续对新的当前位置i的采集建模。

9、可选地，步骤s103中获取当前位置i的雷达位姿δti包括：

10、s201，对雷达数据进行特征点提取得到边缘特征点和平面特征点，边缘特征点是指位于平面边缘的特征点，平面特征点是指位于平面中部的特征点，得到边缘特征点集fei和平面特征点集fpi两类稀疏的特征点云；

11、s202，针对两类稀疏的特征点云，分别将当前位置i的特征点云与当前位置往前位置的特征点云进行匹配，基于点云匹配的结果通过高斯-牛顿迭代法求解下式的优化问题得到的解ti作为当前位置i的雷达位姿δti：

12、

13、上式中，min表示取最小值，ti为优化问题的解，pe为当前位置i的雷达数据中边缘特征点集fei中的特征点，pp为当前位置i的雷达数据中平面特征点集fpi中的特征点，de(pe)为特征点pe到上一位置i-1的雷达数据中相应点的距离，dp(pp)为特征点pp到上一位置i-1的雷达数据中相应点的距离。

14、可选地，特征点pe到上一位置i-1的雷达数据中相应点的距离de(pe)以及特征点pp到上一位置i-1的雷达数据中相应点的距离dp(pp)的计算函数表达式为：

15、

16、

17、上式中，pi为作为自变量的当前位置i的雷达数据中的特征点，pj为上一位置i-1的雷达数据中与特征点pi距离最近的特征点，pk为上上位置i-2的雷达数据中与特征点pj距离最近的特征点，pl为当前位置i的雷达数据中与特征点pj距离最近的特征点。

18、可选地，步骤s103中获取当前位置i的图像位姿δki包括：跟踪确定当前位置i的图像数据中每一个特征点在上一位置i-1的图像数据中的对应位置，并根据下式进行图像数据的帧间里程计更新：

19、δki＝(htr-1h+p-1)-1htr-1

20、上式中，h为图像位姿转移的可比矩阵，r为当前位置i的图像数据、上一位置i-1的图像数据的位姿之间的旋转矩阵，p为在图像数据中需要跟踪的特征点集合。

21、可选地，步骤s103中融合得到当前位置i的融合位姿变换矩阵δi的函数表达式为：

22、δi＝αδti+βδki

23、上式中，α和β为权重系数，δti为雷达位姿，δki为图像位姿。

24、可选地，步骤s103中将当前位置i的雷达数据、图像数据进行位姿变化后融合建立当前位置i的局部三维模型pic包括：

25、s201，针对当前位置i的图像数据ii中的每一个像素点(u,v,1)，首先通过像素坐标系-相机坐标系-世界坐标系下的空间转换得到该像素点在世界坐标系下的坐标(xc,yc,zc)；然后将该像素点在世界坐标系下的坐标(xc,yc,zc)转换为雷达坐标系下的坐标(x,y,z)；

26、s202，针对当前位置i的雷达数据pi中的每一个点(xl,yl,zl)，寻找与点(xl,yl,zl)的距离小于设定值的坐标(x,y,z)对应的像素点，并根据找到的像素点的rgb颜色值取平均值后作为赋予点(xl,yl,zl)的rgb颜色值(r,g,b)，从而使得当前位置i的雷达数据pi中的每一个点(xl,yl,zl)具有六维属性(xl,yl,zl,r,g,b),从而得到当前位置i的局部三维模型pic。

27、可选地，步骤s104中将局部三维模型pic增加到上一位置i-1的全局地图中的函数表达式为：

28、piall＝pi-1all+δi*pic

29、上式中，piall为当前位置i的全局地图，pi-1all为上一位置i-1的全局地图，δi为当前位置i的融合位姿变换矩阵，pic为当前位置i的局部三维模型。

30、可选地，步骤s104中将局部三维模型pic增加到上一位置i-1的全局地图中时，上一位置i-1的全局地图pi-1all的尺寸为n×6，当前位置i得到的局部三维模型pic的尺寸为m×6，当前位置i的全局地图piall的尺寸为(n+m)×6，其中n为上一位置i-1的全局地图中的点数，m为当前位置i得到的局部三维模型中的点数，×6表示点具有三维坐标和三个颜色通道信息共六个维度的数据。

31、此外，本发明还提供一种实时雷达视觉融合的室内场景采集建模系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行所述实时雷达视觉融合的室内场景采集建模方法。

32、此外，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序用于被微处理器编程或配置以执行所述实时雷达视觉融合的室内场景采集建模方法。

33、和现有技术相比，本发明主要具有下述优点：本发明方法包括在每一个当前位置i基于雷达数据和图像数据，获取雷达位姿和图像位姿并融合得到融合位姿变换矩阵δi，将当前位置i的雷达数据、图像数据进行位姿变化后融合建立当前位置i的局部三维模型pic；根据融合位姿变换矩阵δi将局部三维模型pic增加到全局地图以实现全局地图的更新。本发明利用雷达数据和图像数据能实现雷达激光点云的3d信息和相机的2d图像信息相互配准与融合，实现具有颜色信息的3d实时重建，克服视觉slam中的速度慢和3d精度差的问题，补充激光slam方案中缺乏颜色信息的缺点，达到兼顾重建速度、精度与信息丰富度的目的。