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

技术特征：
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)选取合适分辨率，对稠密点云中每一个点按照公式(2)(3)分配空间平面坐标{hor，ver}，并根据平面坐标对整个点云进行排序。3.4)通过计算点p
i，j
的周围邻域法线角度和xy平面欧氏距离关系来归属当前点的类别，即墙壁点、顶层点、煤料点和噪声点。3.5)利用处于二维平面的基于类别和欧氏距离的方法完成聚类，对每一个聚类结果的邻域类别进行下式权重*聚类数量的判断，来修正当前点的类别误差。
其中w
i
代表当前i点的聚类权重依据，s
i，w
代表利用w计算当前i点的权重，k代表权重系数。3.6)基于已构建的kd树数据结构，利用全局点云类别信息通过领域类别信息修正点的归属类别。遍历所有点，基于已构建的kd树数据结构，以一定范围的三维空间结构来搜索该点的周围邻域点，通过邻域信息来修正该点的归属类别。3.7)利用栅格地图来优化点云地图，在得到叠加分类后的点云后，将每一帧点云作为先验数据输入到栅格地图中：data＝{x1，t1，x2，t2，...，x
n
，t
n
}
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)data代表当前点云帧信息，x
n
，t
n
代表第n个点的坐标和位姿。以生成符合当前帧和历史帧信息数据最大概率的栅格地图：m
*
＝arg max
m p(m|data)
ꢀꢀꢀꢀꢀꢀꢀꢀ
(11)其中p(m|data)是在当前先验数据data下的栅格地图概率，m
*
代表符合当前帧和历史帧的最大概率地图。3.8)在对二维雷达信息叠加时，并同时更新栅格地图所属栅格概率值：其中是模型观测值，是固定的更新步长，s-和s 代表先验栅格概率以及更新后的栅格概率。以公式(12)同步更新栅格地图中每一个栅格的极大似然估计，同时更新公式(10)中当前帧点云数据，并且构建公式(11)中的最大栅格概率地图。3.9)利用栅格概率来去除因前端里程计匹配误差而建图所形成的噪声点，并修正所有栅格类别点。利用当前帧信息更新历史帧信息构建的栅格地图后，设定占据概率阈值，通过栅格地图中的栅格概率去判断占据栅格内的点是否属于占据点，若是，则保存为相应类别点，若不是，则标记空闲栅格内的点为噪声点，并去除。3.10)但是由于单线雷达发射的激光是线束形状，所以在遇到一定高度坡度的煤料时，会在其后方形成一个激光扫射盲区，需要进一步填充。在填充之前需要确定点云外边界，利用公式(2)确定外边界搜寻范围。利用knn离群点处理 均值滤波优化误差以及填充部分缺失边界，来获得完整的外边界。3.11)构建二维栅格矩阵，将所有煤料点投影到二维栅格矩阵内，设定近邻点数目nums和方差阈值threshold，计算当前点到所有近邻点的距离和的均值d
i
和标准方差stddev：和标准方差stddev：通过判断d
i
＞threshold*stddev来确定该点为离群点，标记该点，并将之从一维数组中去除，然后确定该栅格内部最小z值作为当前栅格高度值。
3.12)利用二维栅格的cv算子去搜寻盲区栅格的近邻点，用公式(9)距离权重*近邻点栅格高度值填充盲区栅格高度值：s
i，d
代表利用距离d计算当前i点的权重，z
nums
代表当前近邻点栅格高度，e代表盲区栅格高度值。3.13)处理完毕后，将二维栅格重新投影回三维空间中，并利用kd树空间划分对三维点云做栅格处理。对于得到的完整煤料的三维栅格高度数据，累加所有栅格体积即为煤料总体积：其中size
x
，size
y
代表栅格划分的分辨率，e
i
代表当前i栅格的高度，v代表累加栅格体积。2.如权利要求1所述的一种基于三维定位和二维建图的煤仓建模方法，其特征是以三维激光雷达信息和惯性测量单元来对煤仓环境进行感知，对信息进行特征语义分割和聚类预处理，并且在历史信息中利用位姿欧氏距离作为依据来实时更新全局地图，从而对进行清舱作业的推耙机进行实时定位。3.如权利要求1所述的一种基于三维定位和二维建图的煤仓建模方法，其特征是利用单线雷达、直流伺服电机组成的固定旋转传感器感知煤仓环境，以前端三维定位的位姿结果和串口反馈的旋转角度作为单帧匹配依据，来获得完整的煤舱内部点云。4.如权利要求1所述的一种基于三维定位和二维建图的煤仓建模方法，其特征是对叠加点云地图结果进行语义融合分割，分类出煤料点云，补全点云外边界，填充内部空缺点云，并根据煤料三维栅格地图信息来计算煤料体积。

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


技术研发人员：禹鑫燚 羊俊华 唐浩凯 冯远静 欧林林 沈炳华 冯宇 周利波 翁建明
受保护的技术使用者：杭州登元科技有限公司
技术研发日：2022.02.25
技术公布日：2022/6/4