一种基于三维定位和二维建图的煤仓建模方法技术

一种基于三维定位和二维建图的煤仓建模方法，为了加快码头清舱效率，利用传感器获取煤舱内部信息，以三维激光雷达信息和惯性测量单元来对进行清舱作业的推耙机进行实时定位；利用单线雷达、直流伺服电机组成的固定旋转传感器感知煤仓环境，以前端定位和串口旋转角度作为单帧匹配依据，来获得完整的煤舱内部点云；对叠加点云进行语义融合分割，分类出煤料点云，补全点云外边界，填充内部空缺点云，并根据煤料三维栅格点云来计算煤料体积。将所计算得到信息反馈至上位机控制逻辑中，提高整个推耙清舱作业效率。耙清舱作业效率。耙清舱作业效率。

【技术实现步骤摘要】
一种基于三维定位和二维建图的煤仓建模方法


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

技术介绍

[0002]近年来随着我国煤炭供求格局的变化，我国的“国内煤炭北煤南运+外贸煤炭进口”的海运煤炭服务范围由南方七省扩大到了环渤海地区、长江中游四省。面对能源结构调整力度的加大，港口煤炭运输仍保持稳定增长，煤炭下水港格局加速调整，有力地保障了国家能源需求和经济建设健康发展。在现有的煤炭运输方式下，港口作为一个非常重要的中转节点，对煤炭运输具有重要作用，同时面对着煤炭运输的压力下，港口作业能力受到严峻的考验。
[0003]现如今码头作业的主要清舱作业工具是卸船机和推耙机。一般来说，首先由人工操作的卸船机向煤船舱中抓取煤堆到皮带机上，抓取部分周围煤料会因为自身的安息角自由滑落下来；直到煤料堆积成了一个较大坡度的环境，此时由吊机将推耙机吊到船舱内，由人工操作的推耙机将周围高坡度的煤料推耙出来，将其聚集在固定中心，由卸船机进一步抓取；最后直到最后阶段，推耙机进行一次全舱清理，由人工操作卸船机抓取剩余煤料。
[0004]在上述的清舱作业中，卸船机工作人员主要基于目视的方式来重复地抓耙船舱内部中心区域，其他区域的散落煤料则由工作在煤仓内部的推耙机完成聚拢工作，而且作为非闭环作业和人工目视的能力的限制，整个推耙作业没有详细感知煤仓内部环境和三维重建的能力，导致无法计算剩余煤料体积和清仓作业剩余工作时间。另一方面由于阴雨冰雪天等天气的影响，再加上全天作业的要求，这使得基于人工目视的感知方法的准确性降低，且效率也会进一步降低。
[0005]因此为了解决三维定位问题，于兴虎提出一种基于旋转二维激光雷达的三维定位方法(于兴虎,孟令波.基于激光雷达的三维点云重建方法[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]其中x
i
,y
i
,z
i
为当前帧第i个点的三维坐标，Δα,Δβ分别为水平和垂直的划分分辨率，hor
i
,ver
i
为当前帧第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
P
k
代表当前k帧中下标为i的点坐标，
k
P
k
'代表下标i的点矫正到k帧起始时间下的点坐标，代表由多线雷达计算出来的k帧起始下标到i下标的帧间位姿变化矩阵。
[0031]2.2)由于前一步已经去除了车体运动畸变，所以第二部分是去除由电机匀速旋转带来的单线雷达畸变问题，通过同步串口反馈数据，可以预估当前帧帧尾和下一帧帧头之间的姿态变换矩阵，再通过时间戳对每一个点做插值处理，通过下式可以求得帧内每一个点对应的旋转矩阵，并通过矩阵矫正，来将点本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于三维定位和二维建图的煤仓建模方法，其特征在于：所述煤仓建模方法包括如下步骤：1)三维定位：采用一个32线激光雷达和IMU惯导的定位单元，对载体的位姿进行实时优化计算，具体包括：1.1)根据匀速模型利用同步IMU插值去除动态畸变的影响：P
k+i
，P
k+i
′
代表第k帧内的第i个畸变点和矫正点，w，j是第k帧内的所有IMU的运动估计帧数和当前点所处的第j帧，T
k+j
，T
k+j+1
是当前第k帧的变化矩阵叠加当前IMU的运动估计的变化矩阵以及后一帧的变化矩阵。1.2)将三维空间点投影到二维平面，对激光雷达点云进行平面划分：1.2)将三维空间点投影到二维平面，对激光雷达点云进行平面划分：其中x
i
，y
i
，z
i
为当前帧第i个点的三维坐标，Δα，Δβ分别为水平和垂直的划分分辨率，hor
i
，ver
i
为当前帧第i个点处理后的归属平面坐标。1.3)遍历二维平面空间，计算邻域曲率和水平线垂直线的夹角来归属点的类别：其中P
i，j
代表空间平面坐标{i，j}的三维坐标点，n和k代表周围领域点数目和累加下标，通过比较θ和所设阈值来确定当前点的类别。1.4)然后对于分类后的点云进行基于类别和欧氏距离的聚类处理：以未被访问的点中挑选一个作为初始点，以该点开始搜索一定半径的临近点云，满足预设置的类别和欧氏距离条件后，标记为该类别点，并以近邻点作为圆心进一步聚类，否则就标记为噪声点，这时判断已标记数目，若小于阈值，则舍弃，若大于阈值，则从未标记点选取新的起始点开始新的一轮聚类操作。这样根据类别和欧式距离条件后，去除了噪声点，并且基于类别聚类可以修正每一个点的类别。1.5)特征提取和匹配中步骤中，将平面将XY平面六等分，去除因为煤料环境的多态导致的特征点不均匀的因素，每一份的主要平面特征点是从预处理后的静态墙壁点云中按照下式曲率计算公式选取。其中c代表当前特征点的领域曲率，n代表领域点数目，代表当前L点云帧的第k个激光束下的第i个三维点。
1.6)提取出特征点后，通过两帧之间的特征点匹配得到特征点之间的距离，联立构建一个非线性约束方程组，通过LM方法求解此方程组，得到求解后的雷达位姿；并通过帧与地图的匹配，联立构建一个帧与地图的非线性约束方程组，再次用LM方法求解，得到优化后的运动估计。1.7)将从历史点云信息中选定的关键帧的作为二维坐标保存到历史信息中，并根据每一次的前端匹配结果对当前姿态附近的临近历史信息做删减，保证历史帧的动态更新。2)二维重建，利用旋转单线雷达的信息和三维定位中计算的位姿来构建煤舱内部稠密点云地图，具体包括：2.1)由于位姿姿态叠加，无法只利用IMU数据对点云插值去畸变处理，所以根据匀速模型，采用下式的多线雷达优化计算的位姿变化矩阵来对单线雷达点云做去畸变处理：其中
k+i
P
k
代表当前k帧中下标为i的点坐标，
k
P
k
′
代表下标i的点矫正到k帧起始时间下的点坐标，代表由多线雷达计算出来的k帧起始下标到i下标的帧间位姿变化矩阵。2.2)通过同步串口反馈数据，预估当前帧帧尾和下一帧帧头之间的姿态变换矩阵，利用插值处理可以求得帧内每一个点对应的旋转矩阵：P
k，i
代表单线雷达第k帧下标为i的点，P
k，i
′
代表着k帧下标i的点矫正到k帧起始时间下的点，代表着绕电机和雷达旋转轴旋转角度的矫正矩阵。2.3)最后根据去畸变的点云和时间戳匹配的运动估计进行坐标系变换，输出叠加后的点云地图：P
t，i
′
＝T
t
P
t，i
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)P
t，i
和P
t，i
′
代表第i个激光点的位姿以及变换后位姿，T
t
代表第i个激光点对应的相对时间t的插值变换矩阵。3)后端分析，对叠加矫正后的单线雷达点云地图做后端处理，实现对点云的语义识别、煤料的填充和体积参数计算，具体包括：3.1)由于所构建的点云数据庞大，需要快速搜索稠密的三维空间点，所以首先将输入点云构建KD树数据结构。3.2)由于硬件和环境因素影响成像结果，所以利用滑动最小二乘法(Moving Least Squares)对每一个点做平滑处理。3.3)选取合适分辨率，对稠密点云中每一...

【专利技术属性】
技术研发人员：禹鑫燚，羊俊华，唐浩凯，冯远静，欧林林，沈炳华，冯宇，周利波，翁建明，
申请(专利权)人：杭州登元科技有限公司，
类型：发明
国别省市：