一种用于移动机械彩色点云地图构建的感知系统及方法

1.本发明属于一种点云地图构建技术领域，具体是一种用于移动机械彩色点云地图构建的感知系统及方法。


背景技术：

2.在机械领域，基于单目相机的环境感知与地图构建，成本低且效果基本满足需求，但无法提供可靠的空间深度信息。深度相机可以提供空间信息，但同样受光照影响程度大，且给出的空间深度信息可靠性并不高，仅适合室内等小范围的应用。而固态激光雷达成本低，受光照条件影响较小，且测量距离远，能提供高精度的空间信息。
3.因此，固态激光雷达与双目相机融合感知，可以提高感知系统的性能与可靠性。在移动机械领域，机械常被用来进行探测搜救等任务，节省人力且能提高作业安全性。其中三维点云的特征提取、分类识别技术是智能移动机械环境感知研究领域的一项关键前沿技术，在自动驾驶、ar、数字城市、古迹修复、智慧矿山等领域发挥着越来越重要的作用。以往在智能机械点云数据处理领域，主要通过提取点云的空间几何特征作为环境特征，加入颜色信息后，可以在几何特征的基础上，综合颜色和纹理特征对点云数据进行理解；但是目前机械在室外移动进行地图构建过程中，面临的一个很大问题是动态障碍物极大影响建图精度，这在室外环境中不可避免。另外固态激光雷达相比机械激光雷达获取的点云稠密度虽有所提升，但是稠密度相较于相机来讲还是较低，因此现有的固态激光雷达和双目相机拍摄的图像融合效果不好；故如何提供一种方法能在地图构建过程中对动态障碍物去除，从而提升建图精度，同时能对固态激光雷达获取的点云进行处理提升其稠密度，从而与双目相机的图像融合后能获得高精度的彩色点云图像，是本行业的研究方向。


技术实现要素：

4.针对上述现有技术存在的问题，本发明提供一种用于移动机械彩色点云地图构建的感知系统及方法，对动态障碍物去除，从而提升建图精度，同时能对固态激光雷达获取的点云进行处理提升其稠密度，最终与双目相机的图像融合后能获得高精度的彩色点云图像。
5.为了实现上述目的，本发明采用的技术方案是：
6.一种用于移动机械彩色点云地图构建的感知系统，包括在移动机械上安装感知系统，所述感知系统包括用于采集周围环境的点云图像的固态激光雷达模块、用于采集周围环境的光学图像的双目相机模块、用于检测机械移动时的惯性数据的惯性里程计模块、用于接收固态激光雷达模块、双目相机模块、惯性里程计模块数据的计算机；
7.所述计算机包括用于将固态激光雷达的时钟与计算机的主时钟同步的时间戳同步模块、用于检测双目相机模块采集的带有动态障碍物的光学图像的动态障碍物检测模块、用于将点云图像和光学图像融合形成彩色点云地图的数据融合模块和用于彩色点云地图显示的显示器。
8.进一步的，所述感知系统的的彩色点云地图构建方法，具体步骤为
9.(1)预先标定双目相机模块的内参及畸变系数，以及固态激光雷达到双目相机模块的外参、惯性里程计模块到双目相机模块的外参和固态激光雷达模块到惯性里程计模块的外参，并存储到数据融合模块中；
10.(2)通过时间戳同步模块将固态激光雷达的时钟与计算机的主时钟同步；
11.(3)计算机同步接收固态激光雷达模块采集周围环境的点云图像、双目相机模块采集周围环境的光学图像和惯性测量单元模块检测机械移动时的惯性数据；
12.(4)计算机将获得的双目相机光学图像通过步骤(1)标定的内参与畸变系数进行校正，接着动态障碍物检测模块将校正后的每帧图像采用深度神经网络yolov3进行目标检测，得到带有动态障碍物的那一帧图像；
13.(5)计算机将获得点云图像数据和惯性数据，利用时间戳同步模块将两组数据时间同步对齐，然后将惯性数据插值获得当前帧固态激光雷达在世界坐标系下的位姿，基于得到的当前帧固态激光雷达的运动状态对当前帧所有点云进行坐标变换，从而对点云图像数据的畸变进行校正；
14.(6)对校正后的点云图像数据进行点云插值，得到稠密化后的点云图像；
15.(7)将稠密化后的点云图像结合步骤(1)标定的固态激光雷达到双目相机模块的外参与双目相机模块的内参，使点云图像投影到双目相机的uv坐标系下的成像平面；结合步骤(4)获得的带有动态障碍物的那一帧图像，将与其对应的像素坐标处的点云图像数据剔除，接着将成像平面上光学图像各个像素位置的rgb信息赋值给对应的点云图像，进而获得采集周围环境的每一帧彩色稠密点云图像；最后将得到的每一帧彩色稠密点云图像叠加，从而获得周围环境的彩色三维点云地图。
16.进一步，所述步骤(1)中双目相机模块的内参为k及畸变系数为k1，k2，k3，p1，p2；固态激光雷达到双目相机模块的外参为惯性里程计模块到双目相机模块的外参为固态激光雷达模块到惯性里程计模块的外参为
17.进一步，所述步骤(4)中校正过程为：
18.设未校正前双目相机模块获得的图像在uv坐标系下的各个像素坐标为(u，v)，结合步骤(1)标定的内参与畸变系数并根据下式对各个像素坐标进行校正：
[0019][0020]
其中，(u
‘
,v')为图像校正后的各像素坐标；r为像素点距离成像中心的距离。
[0021]
进一步，所述步骤(5)的具体过程为：
[0022]
设激光雷达第k帧的点云图像数据表示为：
[0023]
通过第k帧帧头前后两帧的惯性数据位姿插值得到的第k帧帧头对应的惯性数据位姿表示为：
[0024][0025]
其中为点云图像数据第k帧第i点到相应局部世界坐标系的旋转矩阵，为点
云图像数据第k帧第i点到相应局部世界坐标系的平移矩阵；
[0026]
利用同样的方法能得到第k帧帧中和帧尾的惯性数据位姿为进行二次曲线拟合得到关于时间的点云位姿曲线拟合方程：
[0027]
依据此方程能得到校正点云畸变的公式为：
[0028]
从而对点云图像数据的畸变进行校正。
[0029]
进一步，所述步骤(6)的具体过程为：
[0030]
根据固态激光雷达非重复式扫描的特性，隔行选取四个邻近点，进行插值处理：
[0031]
在固态激光雷达三根扫描线的第一根和第三根上分别选取两个邻近点记为q
11
(x1，y1，z1)，q
12
(x1，y2，z2)，q
21
(x2，y1，z3)，q
22
(x2，y2，z4)，记r1，r2为在x方向上插值得到的两个点，p为最终插值得到的点，则有：
[0032][0033][0034][0035]
总结成为多项式的形式为：
[0036]
z＝a0+a1x+a2y+a3xy，
[0037]
a0＝f(q
11
)，
[0038]
a1＝f(q
21
)-f(q
11
)，
[0039]
a2＝f(q
12
)-f(q
11
)，
[0040]
a3＝f(q
22
)-f(q
21
)-f(q
12
)+f(q
11
)，
[0041]
将选取四个邻近点坐标带入上述多项式，计算出插值点p(x，y，z)的坐标，并对z值设定阈值为0.3，若计算获得的z值大于此阈值则认为插值点与所选四个点深度值误差过大，不进行后续插值处理，并重新选取四个邻近点重复步骤(6)；若计算获得的z值小于等于此阈值，则根据计算出插值点p(x,y,z)的坐标进行插值处理；从而得到稠密化后的点云图像，保证插值点云数据的精度。
[0042]
进一步，所述步骤(7)的具体过程为：
[0043]
设稠密化后的每一帧点云图像数据为p
l
(x0,y0,z0)
t
，每一帧点云图像投影到双目相机的uv坐标系下的成像平面后的每一帧图像数据为pc(x1,y1)
t
；
[0044]
双目相机模块的内参
[0045]
固态激光雷达到双目相机模块的外参
[0046]
则有：
[0047]
即
[0048]
从而使每一帧点云图像投影到双目相机的uv坐标系下的成像平面；结合步骤(4)获得的带有动态障碍物的那一帧图像，将与其对应的像素坐标处的点云图像数据剔除，接着将成像平面上光学图像各个像素位置的rgb信息赋值给对应的每一帧点云图像，赋值后的点云图像数据依据上述公式反投影到三维空间中得到每一帧彩色稠密点云图像p’c
(x,y,z,r,g,b)
t
；最后将得到的每一帧彩色稠密点云图像叠加，从而获得周围环境的彩色三维点云地图。
[0049]
与现有技术相比，本发明先通过固态激光雷达模块采集周围环境的点云图像、双目相机模块采集周围环境的光学图像和惯性测量单元模块检测机械移动时的惯性数据；并通过标定的内参与畸变系数对光学图像进行校正，接着将校正后的每帧图像采用深度神经网络yolov3进行目标检测，得到带有动态障碍物的那一帧图像；然后将获得点云图像数据结合惯性数据进行坐标变换，实现对点云图像数据的畸变进行校正；接着根据固态激光雷达非重复式扫描的特性，设计隔行的点云插值算法，减小了运算量，保证了实时性，使得点云数据稠密，并将稠密后的点云图像数据投影到双目相机的uv坐标系下的成像平面，此时去除带有动态障碍物的那一帧图像；最后将成像平面上光学图像各个像素位置的rgb信息赋值给对应的点云图像，获得周围环境的每一帧彩色稠密点云图像；并将每一帧彩色稠密点云图像叠加，从而获得周围环境的彩色三维点云地图。本发明获得的彩色三维点云地图，相比于没有色彩的点云和缺乏直接距离信息的二维图像而言，三维彩色点云表示的环境数据维度更多，也更接近于人类生活的世界。因此，移动机械在执行任务时，基于此本发明地图构建方法可以给出一个更为直观的可视化反馈，为数字城市，智慧矿山等场景中的三维重建技术提供数据基础。
附图说明
[0050]
图1是本发明中感知系统的结构示意图；
[0051]
图2是本发明中彩色点云地图构建整体流程图。
具体实施方式
[0052]
下面将对本发明作进一步说明。
[0053]
如图1所示，本发明在移动机械上安装感知系统，所述感知系统包括固态激光雷达模块、双目相机模块、惯性里程计模块和计算机，计算机包括时间戳同步模块、动态障碍物检测模块、数据融合模块和显示器，固态激光雷达模块用于采集周围环境的点云图像并传递给计算机；双目相机模块用于采集周围环境的光学图像并传递给计算机；惯性测量单元模块用于检测机械移动时的惯性数据并传递给计算机；所述时间戳同步模块用于将固态激光雷达的时钟与计算机的主时钟同步；动态障碍物检测模块用于检测双目相机模块采集的光学图像中带有动态障碍物的图像；数据融合模块用于将点云图像和光学图像融合形成彩
色点云地图，并通过显示器进行显示。
[0054]
如图2所示，具体彩色点云地图构建的步骤为：
[0055]
(1)预先标定双目相机模块的内参为k及畸变系数为k1，k2，k3，p1，p2；固态激光雷达到双目相机模块的外参为惯性里程计模块到双目相机模块的外参为固态激光雷达模块到惯性里程计模块的外参为并存储到数据融合模块中；
[0056]
(2)通过时间戳同步模块将固态激光雷达的时钟与计算机的主时钟同步，同步过程使用ieee 1588v2.0 ptp的delay request-response机制(two steps)，livox设备作为slave端，和计算机(master)时钟设备进行ptp时间同步。
[0057]
(3)计算机同步接收固态激光雷达模块采集周围环境的点云图像、双目相机模块采集周围环境的光学图像和惯性测量单元模块检测机械移动时的惯性数据；
[0058]
(4)计算机将获得的双目相机光学图像通过步骤(1)标定的内参与畸变系数进行校正，接着动态障碍物检测模块将校正后的每帧图像采用深度神经网络yolov3进行目标检测，得到带有动态障碍物的那一帧图像；具体的校正过程：
[0059]
设未校正前双目相机模块获得的图像在uv坐标系下的各个像素坐标为(u，v)，结合步骤(1)标定的内参与畸变系数并根据下式对各个像素坐标进行校正：
[0060][0061]
其中，(u
‘
,v')为图像校正后的各像素坐标；r为像素点距离成像中心的距离。
[0062]
(5)计算机将获得点云图像数据和惯性数据，利用时间戳同步模块将两组数据时间同步对齐，然后将惯性数据插值获得当前帧固态激光雷达在世界坐标系下的位姿，基于得到的当前帧固态激光雷达的运动状态对当前帧所有点云进行坐标变换，从而对点云图像数据的畸变进行校正，具体过程为：
[0063]
设激光雷达第k帧的点云图像数据表示为：
[0064]
通过第k帧帧头前后两帧的惯性数据位姿插值得到的第k帧帧头对应的惯性数据位姿表示为：
[0065][0066]
其中为点云图像数据第k帧第i点到相应局部世界坐标系的旋转矩阵，为点云图像数据第k帧第i点到相应局部世界坐标系的平移矩阵；
[0067]
利用同样的方法能得到第k帧帧中和帧尾的惯性数据位姿为进行二次曲线拟合得到关于时间的点云位姿曲线拟合方程：
[0068]
依据此方程能得到校正点云畸变的公式为：
[0069]
从而对点云图像数据的畸变进行校正。
[0070]
(6)对校正后的点云图像数据进行点云插值，得到稠密化后的点云图像，具体过程为：
[0071]
根据固态激光雷达非重复式扫描的特性，隔行选取四个邻近点，采用双线性插值法在三维空间进行插值处理：
[0072]
在固态激光雷达三根扫描线的第一根和第三根上分别选取两个邻近点记为q
11
(x1，y1，z1)，q
12
(x1，y2，z2)，q
21
(x2，y1，z3)，q
22
(x2，y2，z4)，记r1，r2为在x方向上插值得到的两个点，p为最终插值得到的点，则有：
[0073][0074][0075][0076]
总结成为多项式的形式为：
[0077]
z＝a0+a1x+a2y+a3xy，
[0078]
a0＝f(q
11
)，
[0079]
a1＝f(q
21
)-f(q
11
)，
[0080]
a2＝f(q
12
)-f(q
11
)，
[0081]
a3＝f(q
22
)-f(q
21
)-f(q
12
)+f(q
11
)，
[0082]
将选取四个邻近点坐标带入上述多项式，计算出插值点p(x，y，z)的坐标，并对z值设定阈值为0.3，若计算获得的z值大于此阈值则认为插值点与所选四个点深度值误差过大，不进行后续插值处理，并重新选取四个邻近点重复步骤(6)；若计算获得的z值小于等于此阈值，则根据计算出插值点p(x,y,z)的坐标进行插值处理；从而得到稠密化后的点云图像，保证插值点云数据的精度。
[0083]
(7)将稠密化后的点云图像结合步骤(1)标定的固态激光雷达到双目相机模块的外参与双目相机模块的内参，使点云图像投影到双目相机的uv坐标系下的成像平面；结合步骤(4)获得的带有动态障碍物的那一帧图像，将与其对应的像素坐标处的点云图像数据剔除，接着将成像平面上光学图像各个像素位置的rgb信息赋值给对应的点云图像，进而获得采集周围环境的每一帧彩色稠密点云图像；最后将得到的每一帧彩色稠密点云图像叠加，从而获得周围环境的彩色三维点云地图，具体过程为：
[0084]
设稠密化后的每一帧点云图像数据为p
l
(x0,y0,z0)
t
，每一帧点云图像投影到双目相机的uv坐标系下的成像平面后的每一帧图像数据为pc(x1,y1)
t
；
[0085]
双目相机模块的内参
[0086]
固态激光雷达到双目相机模块的外参
[0087]
则有：
[0088]
即
[0089]
从而使每一帧点云图像投影到双目相机的uv坐标系下的成像平面；结合步骤(4)获得的带有动态障碍物的那一帧图像，将与其对应的像素坐标处的点云图像数据剔除，接着将成像平面上光学图像各个像素位置的rgb信息赋值给对应的每一帧点云图像，赋值后的点云图像数据依据上述公式反投影到三维空间中得到每一帧彩色稠密点云图像p’c
(x,y,z,r,g,b)
t
；最后将得到的每一帧彩色稠密点云图像叠加，从而获得周围环境的彩色三维点云地图。
[0090]
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。