一种基于三维定位和二维建图的煤仓建模方法与流程

1.本发明涉及一种港口煤船煤仓的三维建模方法，主要利用基于三维激光的定位和单线雷达的旋转二维建模技术对煤仓环境进行点云重建和分析。


背景技术：

2.近年来随着我国煤炭供求格局的变化，我国的“国内煤炭北煤南运 外贸煤炭进口”的海运煤炭服务范围由南方七省扩大到了环渤海地区、长江中游四省。面对能源结构调整力度的加大，港口煤炭运输仍保持稳定增长，煤炭下水港格局加速调整，有力地保障了国家能源需求和经济建设健康发展。在现有的煤炭运输方式下，港口作为一个非常重要的中转节点，对煤炭运输具有重要作用，同时面对着煤炭运输的压力下，港口作业能力受到严峻的考验。
3.现如今码头作业的主要清舱作业工具是卸船机和推耙机。一般来说，首先由人工操作的卸船机向煤船舱中抓取煤堆到皮带机上，抓取部分周围煤料会因为自身的安息角自由滑落下来；直到煤料堆积成了一个较大坡度的环境，此时由吊机将推耙机吊到船舱内，由人工操作的推耙机将周围高坡度的煤料推耙出来，将其聚集在固定中心，由卸船机进一步抓取；最后直到最后阶段，推耙机进行一次全舱清理，由人工操作卸船机抓取剩余煤料。
4.在上述的清舱作业中，卸船机工作人员主要基于目视的方式来重复地抓耙船舱内部中心区域，其他区域的散落煤料则由工作在煤仓内部的推耙机完成聚拢工作，而且作为非闭环作业和人工目视的能力的限制，整个推耙作业没有详细感知煤仓内部环境和三维重建的能力，导致无法计算剩余煤料体积和清仓作业剩余工作时间。另一方面由于阴雨冰雪天等天气的影响，再加上全天作业的要求，这使得基于人工目视的感知方法的准确性降低，且效率也会进一步降低。
5.因此为了解决三维定位问题，于兴虎提出一种基于旋转二维激光雷达的三维定位方法(于兴虎,孟令波.基于激光雷达的三维点云重建方法[p].中国专利：cn110223379a,2019-09-10)。但是煤仓工作环境下，绝大部分环境为随着作业而不断运动的煤料，所以这种方法并没有处理动态物体对定位匹配所采生的影响，而且不适用于运动幅度较大、工作时间很长的工况下。
[0006]
同时为了解决三维重建问题，李长安提出一种基于旋转二维激光雷达的三维重建技术(李长安,姚同建,师伟,潘攀,李靖宇,马磊.一种料堆建模方法和料堆建模装置[p].中国专利：cn106094702b,2020-07-07)。该方法基于固定环境下进行对平面上的三维重建，但是由于码头煤船煤仓的尺寸、数量是不固定的，无法安装固定的旋转二维雷达。


技术实现要素：

[0007]
本发明克服现有技术上的问题，提出一种基于三维定位和旋转单线雷达的煤仓三维建模。
[0008]
本发明为解决现有技术问题所采用的技术方案是：
[0009]
一种基于三维定位和旋转单线雷达的三维重建方法，其中包括以下步骤：
[0010]
1)三维定位：针对于复杂的舱内环境以及高强度的工作模式，采用一个32线激光雷达和imu惯导的定位单元，对载体的位姿进行实时优化计算，具体包括：
[0011]
1.1)首先对输入点云进行去畸变处理，对由imu传感器输入的运动估计和激光雷达信息进行时间戳匹配，然后根据匀速模型对雷达点云进行插值处理矫正。
[0012][0013]
p
k i
,p
k i
'代表第k帧内的第i个畸变点和矫正点，w，j是第k帧内的所有imu的运动估计帧数和当前点所处的第j帧，t
k j
,t
k j 1
是当前第k帧的变化矩阵叠加当前imu的运动估计的变化矩阵以及后一帧的变化矩阵。
[0014]
1.2)然后对由激光雷达输入的32线雷达点云划分空间，生成scan context,划分出每一个点归属的空间平面坐标{hor,ver}，将三维点云空间降维到二维数据格式。
[0015][0016][0017]
其中xi,yi,zi为当前帧第i个点的三维坐标，δα,δβ分别为水平和垂直的划分分辨率，hori,veri为当前帧第i个点处理后的归属平面坐标。
[0018]
1.3)遍历二维平面空间，通过计算当前点的周围邻域曲率和水平线垂直线的夹角，并设定相应角度，来判断该点的类别。
[0019][0020]
其中p
i,j
代表空间平面坐标{i,j}的三维坐标点，n和k代表周围领域点数目和累加下标，通过比较θ和所设阈值来确定当前点的类别。
[0021]
1.4)然后对于分类后的点云进行基于类别和欧氏距离的聚类处理：以未被访问的点中挑选一个作为初始点，以该点开始搜索一定半径的临近点云，满足预设置的类别和欧氏距离条件后，标记为该类别点，并以近邻点作为圆心进一步聚类，否则就标记为噪声点，这时判断已标记数目，若小于阈值，则舍弃，若大于阈值，则从未标记点选取新的起始点开始新的一轮聚类操作。
[0022]
1.5)特征提取和匹配中步骤中，将平面将xy平面六等分，每一份的主要平面特征点是从预处理后的静态墙壁点云中按照下式曲率计算公式选取。
[0023][0024]
其中c代表当前特征点的领域曲率，n代表领域点数目，代表当前l点云帧的第k个激光束下的第i个三维点。
[0025]
1.6)提取出特征点后，通过两帧之间的特征点匹配得到特征点之间的距离，联立构建一个非线性约束方程组，通过lm方法求解此方程组，得到求解后的雷达位姿；并通过帧与地图的匹配，联立构建一个帧与地图的非线性约束方程组，再次用lm方法求解，得到优化后的运动估计。
[0026]
1.7)将从历史点云信息中选定的关键帧的作为二维坐标保存到历史信息中，并根据每一次的前端匹配结果对当前姿态附近的临近历史信息做删减，这样可以在动态变化场景中去除历史帧的过时信息，保证历史帧的动态更新。
[0027]
2)二维重建，利用到旋转电机和标定安装的单线雷达和三维定位中计算的位姿，具体包括：
[0028]
2.1)由于位姿姿态叠加，无法只利用imu数据对点云插值去畸变处理，所以根据匀速模型，采用下式的多线雷达优化计算的位姿变化矩阵来对单线雷达点云做去畸变处理。
[0029][0030]
其中
k i
pk代表当前k帧中下标为i的点坐标，kpk'代表下标i的点矫正到k帧起始时间下的点坐标，代表由多线雷达计算出来的k帧起始下标到i下标的帧间位姿变化矩阵。
[0031]
2.2)由于前一步已经去除了车体运动畸变，所以第二部分是去除由电机匀速旋转带来的单线雷达畸变问题，通过同步串口反馈数据，可以预估当前帧帧尾和下一帧帧头之间的姿态变换矩阵，再通过时间戳对每一个点做插值处理，通过下式可以求得帧内每一个点对应的旋转矩阵，并通过矩阵矫正，来将点投影在绝对世界坐标系下：
[0032][0033]
p
k,i
代表单线雷达第k帧下标为i的点，p
k,i
'代表着k帧下标i的点矫正到k帧起始时间下的点，代表着绕电机和雷达旋转轴旋转角度的矫正矩阵。
[0034]
2.3)最后根据去畸变的点云和时间戳匹配的运动估计进行坐标系变换，输出叠加后的点云地图。
[0035]
p
t,i
＝t
t
p
t,i
ꢀꢀꢀ
(8)
[0036]
p
t,i
和p
t,i
'代表第i个激光点的位姿以及变换后位姿，t
t
代表第i个激光点对应的相对时间t的插值变换矩阵。
[0037]
3)后端分析，对叠加矫正后的单线雷达点云地图做后端处理，实现对点云的语义识别、煤料的填充和体积参数计算；具体包括：
[0038]
3.1)首先构建点云数据庞大，需要快速搜索稠密的三维空间点，所以首先将输入点云构建kd树数据结构。
[0039]
3.2)由于硬件和环境因素影响成像结果，所以利用滑动最小二乘法(moving least squares)对每一个点做平滑处理。
[0040]
3.3)选取合适分辨率，对稠密点云中每一个点分配空间平面坐标{hor,ver}，并根据平面坐标对整个点云进行排序。
[0041]
3.4)遍历二维平面空间，利用煤料的安息角不超过45度的先验知识来设定一个合适角度阈值，并通过计算点p
i,j
的周围邻域法线角度和xy平面欧氏距离关系来归属当前点
的类别，即墙壁点、顶层点、煤料点和噪声点。
[0042]
3.5)因为单线激光雷达的分辨率很高和煤料分布具有随机性，所以利用处于二维平面的基于类别和欧氏距离的聚类方法来去修正当前点的类别误差：设定当前vel的第一个点作为起始点，根据类别和欧式距离标记该点为聚类点，并以该点继续向下判断；若不满足条件，则判断聚类点数目是否满足聚类数目阈值要求，若不满足，则舍弃该聚类；完成聚类操作后，对每一个聚类结果的邻域类别进行下式权重*数目的判断，来确定当前聚类中所有点的类别。
[0043][0044]
其中wi代表当前i点的聚类权重依据，s
i,w
代表利用w计算当前i点的权重，k代表权重系数。
[0045]
3.6)利用单线上所有点的信息来确定了点的类别，接下来利用整个点云信息来确定每一个点的类别：遍历所有点，基于已构建的kd树数据结构，以一定范围的三维空间结构来搜索该点的周围邻域点，通过邻域信息来修正该点的归属类别。
[0046]
3.7)覆盖栅格地图处理主要是利用栅格地图的建图方式来确定栅格当前状态，从而达到动态更新点云地图、去除动态物体产生的鬼影点和减少点云建图误差的效果。在得到叠加分类后的点云后，将每一帧点云作为先验数据输入到栅格地图中：
[0047]
data＝{x1,t1,x2,t2,
…
,xn,tn}
ꢀꢀꢀ
(10)
[0048]
data代表当前点云帧信息，xn,tn代表第n个点的坐标和位姿。
[0049]
以生成符合当前帧和历史帧信息数据最大概率的栅格地图：
[0050]m*
＝arg maxmp(m|data)
ꢀꢀꢀ
(11)
[0051]
其中p(m|data)是在当前先验数据data下的栅格地图概率，m
*
代表符合当前帧和历史帧的最大概率地图。
[0052]
3.8)在对二维雷达信息叠加时，并同时更新栅格地图所属栅格概率值：
[0053][0054]
其中是模型观测值，是固定的更新步长，s-和s

代表先验栅格概率以及更新后的栅格概率。以公式(12)同步更新栅格地图中每一个栅格的极大似然估计，同时更新公式(10)中当前帧点云数据，并且构建公式(11)中的最大栅格概率地图。
[0055]
3.9)利用当前帧信息更新历史帧信息构建的栅格地图后，设定占据概率阈值，通过栅格地图中的栅格概率去判断占据栅格内的点是否属于占据点，若是，则保存为相应类别点，若不是，则标记空闲栅格内的点为噪声点，并处理去除。
[0056]
3.10)但是由于单线雷达发射的激光是线束形状，所以在遇到一定高度坡度的煤料时，会在其后方形成一个激光扫射盲区，所以煤料的叠加点云并不是完整的，需要进一步填充。在填充之前需要确定点云外边界，利用公式(2)确定外边界搜寻范围，利用knn离群点处理 均值滤波处理外边界大小以及填充部分缺失边界，来获得完整的外边界。
[0057]
3.11)在得到分类优化的点云后，构建二维栅格矩阵，将所有煤料点投影到二维栅格矩阵内，保存同一栅格内部所有点的z值为一维数组，设定近邻点数目nums和方差阈值threshold，通过kd树搜索nums个边界内部的近邻点，计算当前点到所有近邻点的距离和的均值di和标准方差stddev：
[0058][0059][0060]
通过判断di》threshold*stddev来确定该点为离群点，标记该点，并将之从一维数组中去除，然后确定该栅格内部最小z值作为当前栅格高度值；
[0061]
3.12)利用二维栅格的cv算子去搜寻盲区栅格的近邻点，用公式(9)距离权重*近邻点栅格高度值填充盲区栅格高度值：
[0062][0063]si,d
代表利用距离d计算当前i点的权重，z
nums
代表当前近邻点栅格高度，e代表盲区栅格高度值。
[0064]
3.13)处理完毕后，将二维栅格重新投影回三维空间中，并利用kd树空间划分对三维点云做栅格处理。对于得到的完整煤料的二维栅格高度数据，累加所有栅格体积即为煤料总体积：
[0065][0066]
其中size
x
,sizey代表栅格划分的分辨率，ei代表当前i栅格的高度，v代表累加栅格体积。
[0067]
本发明的优点是：
[0068]
1.定位优势。以全球定位系统(gps)为例的绝对定位方式存在着较大的测量误差，对低水平信号不友好，无法达到高精度的工程需求。而以轮式编码器、imu传感器为例的相对定位存在的长期累计误差，容易受到磁场影响等多种问题，无法很好地解决长时间定位要求。所以以多线激光雷达感知煤仓环境并同步定位和建图的相对定位方式可以很好地满足高精度、闭环计算的工程要求。
[0069]
2.建图优势。由于煤料存在低反射高吸收的特性，多线雷达扫描得到的点云会被煤料吸收一部分，导致整体点云是不完整的，并且由于多线雷达在scan维度的分辨率太低，所以对于整体煤仓而言多线雷达点云是稀疏的。三维重建采用旋转单线雷达，水平分辨率可以通过电机角速度来调节，竖直分辨率可以达到0.06，能够完整地三维重建出整体煤仓的点云地图。
[0070]
3.后端分析。后端分析中主要采用了点云分类、融合和栅格处理，将三维重建的点云地图转化为了语义地图，并通过栅格地图处理去除动态物体噪声的影响，进一步优化了点云和位姿匹配的误差，实现长时间的动态更新地图。并且通过处理后的点云计算出上位机需要的体积、高层图等可视化信息，提高了上位机操作卸船机清仓作业效率。
附图说明
[0071]
图1是本发明的流程结构示意图。
[0072]
图2是本发明建立的去畸变仓库三维点云图。
[0073]
图3是本发明建立的煤仓三维语义图。
[0074]
图4是本发明煤仓外边界分布图
[0075]
图5是本发明外边界修正图
[0076]
图6是本发明切割得到的煤料点云图。
[0077]
图7是本发明建立的煤料三维栅格图。
具体实施方式
[0078]
参照图1～图7，一种基于三维定位和二维建图的煤仓建模方法，如图1所示，通过串口和网口接收两个雷达感知信息和imu测量信息，利用ouster雷达的三维激光信息和imu实时测量信息，通过三维建图定位获得推耙机实时位姿；利用quanergy雷达的三维激光信息、三维扫描建图计算得到的实时位姿、modbus通信获取yz-acsd608电机的旋转角度和imu实时测量信息来构建煤船煤仓内部环境，建立关于煤料仓库的点云地图。
[0079]
具体步骤如下：
[0080]
1)三维定位：采用一个32线激光雷达和imu惯导的定位单元，对载体的位姿进行实时优化计算，具体包括：
[0081]
1.1)根据匀速模型利用同步imu插值去除动态畸变的影响：
[0082][0083]
p
k i
,p
k i
代表第k帧内的第i个畸变点和矫正点，w，j是第k帧内的所有imu的运动估计帧数和当前点所处的第j帧，t
k j
,t
k j 1
是当前第k帧的变化矩阵叠加当前imu的运动估计的变化矩阵以及后一帧的变化矩阵。
[0084]
1.2)将三维空间点投影到二维平面，对激光雷达点云进行平面划分：
[0085][0086][0087]
其中xi,yi,zi为当前帧第i个点的三维坐标，δα,δβ分别为水平和垂直的划分分辨率，hori,veri为当前帧第i个点处理后的归属平面坐标。
[0088]
1.3)遍历二维平面空间，计算邻域曲率和水平线垂直线的夹角来归属点的类别：
[0089][0090]
其中p
i,j
代表空间平面坐标{i,j}的三维坐标点，n和k代表周围领域点数目和累加下标，通过比较θ和所设阈值来确定当前点的类别。
[0091]
1.4)然后对于分类后的点云进行处于三维空间的基于类别和欧氏距离的聚类处理。这样根据类别和欧式距离条件后，去除了噪声点，并且基于类别聚类可以修正每一个点的类别。
[0092]
1.5)特征提取和匹配中步骤中，将平面将xy平面六等分，去除因为煤料环境的多态导致的特征点不均匀的因素，每一份的主要平面特征点是从预处理后的静态墙壁点云中按照下式曲率计算公式选取。
[0093][0094]
其中c代表当前特征点的领域曲率，n代表领域点数目，代表当前l点云帧的第k个激光束下的第i个三维点。
[0095]
1.6)提取出特征点后，通过两帧之间的特征点匹配得到特征点之间的距离，联立构建一个非线性约束方程组，通过lm方法求解此方程组，得到求解后的雷达位姿；并通过帧与地图的匹配，联立构建一个帧与地图的非线性约束方程组，再次用lm方法求解，得到优化后的运动估计。
[0096]
1.7)将从历史点云信息中选定的关键帧的作为二维坐标保存到历史信息中，并根据每一次的前端匹配结果对当前姿态附近的临近历史信息做删减，保证历史帧的动态更新。
[0097]
2)二维重建，利用旋转单线雷达的信息和三维定位中计算的位姿来构建煤舱内部稠密点云地图，具体包括：
[0098]
2.1)由于位姿姿态叠加，无法只利用imu数据对点云插值去畸变处理，所以根据匀速模型，采用下式的多线雷达优化计算的位姿变化矩阵来对单线雷达点云做去畸变处理：
[0099][0100]
其中
k i
pk代表当前k帧中下标为i的点坐标，kpk'代表下标i的点矫正到k帧起始时间下的点坐标，代表由多线雷达计算出来的k帧起始下标到i下标的帧间位姿变化矩阵。
[0101]
2.2)通过同步串口反馈数据，预估当前帧帧尾和下一帧帧头之间的姿态变换矩阵，利用插值处理可以求得帧内每一个点对应的旋转矩阵：
[0102][0103]
p
k,i
代表单线雷达第k帧下标为i的点，p
k,i
'代表着k帧下标i的点矫正到k帧起始时间下的点，代表着绕电机和雷达旋转轴旋转角度的矫正矩阵。
[0104]
2.3)最后根据去畸变的点云和时间戳匹配的运动估计进行坐标系变换，输出叠加后的点云地图。
[0105]
p
t,i’＝t
t
p
t,i
ꢀꢀꢀ
(8)
[0106]
p
t,i
和p
t,i
'代表第i个激光点的位姿以及变换后位姿，t
t
代表第i个激光点对应的相对时间t的插值变换矩阵。
[0107]
去除畸变后，利用电机旋转获取仓库点云图如图2所示，可以看到电机旋转到达一
定速度后，整个构建出来的点云地图仍保持精准的叠加形态。
[0108]
3)后端分析，对叠加矫正后的单线雷达点云地图做后端处理，实现对点云的语义识别、煤料的填充和体积参数计算，具体包括：
[0109]
3.1)由于所构建的点云数据庞大，需要快速搜索稠密的三维空间点，所以首先将输入点云构建kd树数据结构。
[0110]
3.2)由于硬件和环境因素影响成像结果，所以利用滑动最小二乘法(moving least squares)对每一个点做平滑处理。
[0111]
3.3)选取合适分辨率，对稠密点云中每一个点按照公式(2)(3)分配空间平面坐标{hor,ver}，并根据平面坐标对整个点云进行排序。
[0112]
3.4)通过计算点p
i,j
的周围邻域法线角度和xy平面欧氏距离关系来归属当前点的类别，即墙壁点、顶层点、煤料点和噪声点。
[0113]
3.5)利用处于二维平面的基于类别和欧氏距离的方法完成聚类，对每一个聚类结果的邻域类别进行下式权重*聚类数量的判断，来修正当前点的类别误差。
[0114][0115]
其中wi代表当前i点的聚类权重依据，s
i,w
代表利用w计算当前i点的权重，k代表权重系数。
[0116]
3.6)基于已构建的kd树数据结构，利用全局点云类别信息通过领域类别信息修正点的归属类别。
[0117]
3.7)利用栅格地图来优化点云地图，在得到叠加分类后的点云后，将每一帧点云作为先验数据输入到栅格地图中：
[0118]
data＝{x1,t1,x2,t2,
…
,xn,tn}
ꢀꢀꢀ
(10)
[0119]
data代表当前点云帧信息，xn,tn代表第n个点的坐标和位姿。
[0120]
以生成符合当前帧和历史帧信息数据最大概率的栅格地图：
[0121]m*
＝arg maxmp(m|data)
ꢀꢀꢀ
(11)
[0122]
其中p(m|data)是在当前先验数据data下的栅格地图概率，m
*
代表符合当前帧和历史帧的最大概率地图。
[0123]
3.8)在对二维雷达信息叠加时，并同时更新栅格地图所属栅格概率值：
[0124][0125]
其中是模型观测值，是固定的更新步长，s-和s

代表先验栅格概率以及更新后的栅格概率。以公式(12)同步更新栅格地图中每一个栅格的极大似然估计，同时更新公式(10)中当前帧点云数据，并且构建公式(11)中的最大栅格概率地图。
[0126]
3.9)利用栅格概率来去除因前端里程计匹配误差而建图所形成的噪声点，并修正所有栅格类别点。最终得到语义分割图如图3所示，红色点云代表上平面，绿色点云代表船舱墙壁，紫色点云代表煤仓煤料。
[0127]
3.10)但是由于单线雷达发射的激光是线束形状，所以在遇到一定高度坡度的煤料时，会在其后方形成一个激光扫射盲区，所以在图3所示，下方红色区域点云是不完整的，需要进一步填充。在填充之前需要确定点云外边界，利用公式(2)确定外边界搜寻范围，如图4点云外边界点线图所示，外边界范围同样也是不完整的，而且有很多毛刺线。利用knn离群点处理 均值滤波优化误差以及填充部分缺失边界，来获得完整的外边界，实现图5的完整外边界效果。
[0128]
3.11)构建二维栅格矩阵，将所有煤料点投影到二维栅格矩阵内，设定近邻点数目nums和方差阈值threshold，计算当前点到所有近邻点的距离和的均值di和标准方差stddev：
[0129][0130][0131]
通过判断di》threshold*stddev来确定该点为离群点，标记该点，并将之从一维数组中去除，然后确定该栅格内部最小z值作为当前栅格高度值。
[0132]
3.12)利用二维栅格的cv算子去搜寻盲区栅格的近邻点，用公式(9)距离权重*近邻点栅格高度值填充盲区栅格高度值：
[0133][0134]si,d
代表利用距离d计算当前i点的权重，z
nums
代表当前近邻点栅格高度，e代表盲区栅格高度值。
[0135]
得到完整的煤料分割图6所示，煤料区域已经完整且平滑。
[0136]
3.13)处理完毕后，将二维栅格重新投影回三维空间中，并利用kd树空间划分对三维点云做栅格处理。所得到的结果如图7所示。对于得到的完整煤料的三维栅格高度数据，累加所有栅格体积即为煤料总体积：
[0137][0138]
其中size
x
,sizey代表栅格划分的分辨率，ei代表当前i栅格的高度，v代表累加栅格体积。
[0139]
本发明实施例子不限于码头船舱的三维建模，同样适用于室内仓库、地下煤矿等场景建模。