本发明涉及数据处理技术领域，特别涉及一种基于3D激光的3D地图创建方法。
背景技术：
随着传感器技术的发展以及机器人技术的应用，装备3D激光雷达的变电站巡检机器人应用越来越多，通过融合3D激光数据获取高精度点云地图是机器人的基础功能，是三维模型构建和空间可视化的核心。
目前，现有的变电站3D地图创建方法大多数精度交低，重复性较差，未获取稠密数据需重复多次扫描同一环境区域，但会造成地图重影等问题。其中，应用最多的多传感器方法，需3D激光配合高精度IMU和差分GPS，成本较高。
技术实现要素：
本发明的目的在于提供一种基于3D激光的3D地图创建方法，以提高点云地图的精度。
为实现以上目的，本发明采用的技术方案为：
采用一种基于3D激光的3D地图创建方法，包括如下步骤：
以3D激光传感器起始位置建立全局坐标系，并对3D激光传感器获取的点云数据进行富信息处理，得到有序化的激光点云数据帧；
对所述点云数据帧进行特征提取，得到点云数据帧的特征点；
根据LM算法对变换矩阵进行优化，将使得所有特征点匹配的距离误差平方和最小的变换矩阵作为激光雷达的位姿；
根据激光雷达的位姿，将每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图。
优选地，对3D激光传感器获取的点云数据进行富信息处理，得到有序化的激光点云数据帧，具体包括：
对3D激光传感器获取的每个扫描点信息进行封装处理，得到单个扫描点的数据格式，其中每个激光点信息包括时间戳信息、发射线通道信息、标准三维空间坐标信息以及强度信息；
将覆盖3D激光传感器视场角的一次扫描的所有扫描点封装为一帧点云数据；
基于每个扫描点的发射线通道信息，将所述每帧点云数据中每个扫描点的数据进行排序；
基于每个扫描点的时间戳信息，将在同一线通道中的扫描点数据进行排序，得到有序化的激光点云数据帧。
优选地，对点云数据帧进行特征提取，得到点云数据帧的特征点，具体包括：
根据所述每一帧点云数据中的任一扫描点，得到该扫描点的邻域范围内的所有扫描点；
对该扫描点邻域范围内所有扫描点进行PCA主成成分分析，得到该扫描点邻域曲面的主方向V以及曲率值L；
将该扫描点邻域的曲率值L与预设的平面阈值C1、C2进行比较，判断该扫描点是否为特征点，其中，C1、C2均为常数且C1＜C2。
优选地，将该扫描点邻域的曲率值L与预设的平面阈值C1、C2进行比较，判断该扫描点是否为特征点，具体包括：
a1、判断该扫描点邻域的曲率值L是否满足L≤C1，若是则执行步骤a2，若否则执行步骤a4；
a2、判断该扫描点邻域范围内所有扫描点的领域曲面的曲率值是否均小于平面阈值C1，若是执行步骤a3，若否则执行步骤a9；
a3、确定该扫描点为平面特征点；
a4、判断该扫描点邻域的曲率值L是否满足C1＜L≤C2，若是则执行步骤a5，若否则执行步骤a7；
a5、判断该扫描点邻域范围内所有扫描点的领域曲面的曲率值是否均满足大于C1且小于等于C2，若是则执行步骤a6，若否则执行步骤a9；
a6、判断该扫描点为圆柱面特征点；
a7、判断该扫描点邻域范围内所有扫描点的领域曲面的曲率值是否均大于C2，若是则执行步骤a8，若否则执行步骤a9；
a8、确定该扫描点为边缘特征点；
a9、判断该扫描点为非特征点。
优选地，根据LM算法对变换矩阵进行优化，将使得所有特征点匹配的距离误差平方和最小的变换矩阵作为激光雷达的位姿，具体包括：
将所述提取的特征点的类型和坐标信息，分别将边缘特征点、平面特征点以及圆柱面特征点应用到点到线的ICP匹配、点到平面的ICP匹配以及点到柱面的ICP匹配；
利用LM算法对变换矩阵进行优化，将使得点到线的ICP匹配的距离误差、点到平面的ICP匹配的距离误差以及点到柱面的ICP匹配的距离误差的平方和最小的变换矩阵作为激光雷达的位姿。
优选地，该方法还包括：
在利用LM算法得到激光雷达的位姿之后，判断函数是否收敛和/或当前迭代次数是否超过最大迭代次数；
在函数收敛时或当前迭代次数超过最大迭代次数时，对激光雷达的位姿进行更新。
优选地，该方法还包括：
将激光雷达的位姿和所述特征点加入poseGraph，通过现有开源库的非线性求解器，计算全局最优位姿。
优选地，根据所述每个扫描点的时间戳信息，对所述激光雷达的位姿进行插值，得到所述点云数据帧中扫描点在各时刻的位姿变换插值比例系数；
根据比例系数，计算插值后的变换矩阵；
根据插值后的变换矩阵，对所述点云数据帧中的扫描点数据进行校正，得到校正后的点云数据帧；
相应地，所述根据激光雷达的位姿，将每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图，具体包括：
根据激光雷达的位姿，将校正后的每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图。
优选地，该方法还包括：
还包括对体素内的信息进行更新，具体为：
计算从位姿到观测点坐标穿过的所有体；
判断体素是否被穿过；
若是则体素穿过次数增加1，若否则体素观测次数增加1；
在观测点为特征点时，更新体素特征估计次数；
并根据观测点的特征类型将该特征类型的个数进行更新。
优选地，所述体素地图通过八叉树结构进行存储。
与现有技术相比，本发明存在以下技术效果：本方案中首先，通过对原始激光数据进行富信息处理，传统的激光数据仅表达扫描点的距离或空间点位置，本方案通过富信息处理之后，增加了激光线通道、扫描时间、有序化等信息，为下一步3D地图的创建提供了数据基础。其次，可针对变电站场景的特点，进行不同类型特征点的提取，基于特征的匹配优化得到空间变换矩阵并对点云进行扭曲校正，快速准确的得到空间定位信息，提高了点云地图的精度。通过采用体素地图表达形式，避免了采用点云地图不方便滤波去噪的问题，提高了3D地图的清晰度。
附图说明
下面结合附图，对本发明的具体实施方式进行详细描述：
图1是一种基于3D激光的3D地图创建方法的流程示意图；
图2是点云数据帧的示意图；
图3是对点云数据进行特征点提取的流程示意图；
图4是特征匹配及位姿更新的流程示意图；
图5是地图更新融合的流程示意图。
具体实施方式
为了更进一步说明本发明的特征，请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用，并非用来对本发明的保护范围加以限制。
如图1所示，本实施例公开了一种基于3D激光的3D地图创建方法，包括如下步骤S101至S104：
S101、以3D激光传感器起始位置建立全局坐标系，并对3D激光传感器获取的点云数据进行富信息处理，得到有序化的激光点云数据帧；
需要说明的是，本实施例中得到的有序化的激光点云数据帧指的是帧内的数据是有序的。
S102、对所述点云数据帧进行特征提取，得到点云数据帧的特征点；
S103、根据LM算法对变换矩阵进行优化，将使得所有特征点匹配的距离误差平方和最小的变换矩阵作为激光雷达的位姿；
S104、根据激光雷达的位姿，将每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图。
作为进一步优选的方案，步骤S101中，对3D激光传感器获取的点云数据进行富信息处理，得到有序化的激光点云数据帧，具体包括如下步骤：
(1)对3D激光传感器获取的每个扫描点信息进行封装处理，得到单个扫描点的数据格式，其中每个激光扫描点信息包括时间戳信息、发射线通道信息、标准三维空间坐标信息以及强度信息；
(2)将覆盖3D激光传感器视场角的一次扫描的所有扫描点封装为一帧点云数据；
(3)基于每个扫描点的发射线通道信息，将所述每帧点云数据中每个扫描点的数据进行排序；
(4)基于每个扫描点的时间戳信息，将在同一线通道中的扫描点数据进行排序，得到有序化的激光点云数据帧。
需要说明的是，目前的激光传感器至提供原始的扫描距离信息和强度信息，标准的点云数据格式只包含每个扫描点在激光坐标系下的(x,y,z)三维坐标信息和intensity强度信息。本实施例中将每个激光扫描点的时间戳信息time、发射线通道信息ring、标准的(x,y,z)空间坐标信息以及intensity强度信息一起封装为单个扫描点的数据格式。然后将覆盖激光视场角一次扫描的所有激光扫描点封装为一帧数据。对点云数据进行了富信息处理，提高了点云数据的丰富性，为后面的3D地图创建提供了数据基础。
在单个扫描点的数据格式中，每个激光扫描点的标准三维空间坐标信息和强度信息为原始激光传感器可提供，激光扫描点的时间戳信息和发射线通道信息计算过程如下：
时间戳信息：由于激光传感器内部激光发射器是按其硬件设计原理和规律发射激光束，因此每个激光扫描点的时间信息可以由帧头的时间戳和该点的发射顺序索引插值计算得到，以velodyne 16线激光为例：
记完成所有线的一次扫描过程为一个发射周期，传感器一次传输的每个数据包包含很多周期的数据，而每个数据包只有一个帧头时间，每个点的估算时间为：
time＝Timestamp+(TimePeriod×SequenceIndex)+(TimeLaser×PointIndex)；
式中：
TimePeriod为所有激光线完成一次发射和接受的时间周期，该参数数值由传感器手册获取；
TimeLaser为两次相邻的激光发射和接受的时间周期，该参数数值由传感器手册获取；
SequenceIndex为发射周期在该数据包的顺序索引，该值由传感器数据传输协议获取；
PointIndex为该点在发射周期内的发射顺序索引，该值由传感器数据传输协议获取；
Timestamp为该数据包的时间帧头，该值由传感器数据传输协议获取；
time为该扫描点时间戳信息估计值。
发射线通道信息：3D激光通常分为8线，16线，32线等，其中相邻两根线之间的夹角都是固定的，对于每根线的信息称之为线通道信息。以水平方向为0做基准，对3D激光传感器的所有发射器按发射竖直角从小到大排序，顺序索引记为ring，每个激光点的线通道信息由ring表示，其值可以由原始数据提取处理或根据坐标信息推算：
ring＝(atan(point.z/sqrt(point.x×point.x+point.y×point.y))–angleMin)/angleDiff；
式中：
point(x,y,z)为该点的空间坐标；
angleMin为最下面发射器的发射角；
angleDiff为相邻两个发射器的角度差；
ring为该点在发射周期内的发射顺序索引，该值由传感器数据传输协议获取。
需要说明的是，每个激光扫描点的线通道信息用于点云数据的有序化处理，由于在同一线通道上的扫描数据点比较稠密类似于2D激光扫描数据，而在不同线通道上的数据比较稀疏，因此可基于激光扫描点线通道信息将每帧激光数据分组。由于激光发射器的原理设计，直接得到的点云数据并不是有序化的，本实施例中将点云按照类似图像像素点排列的方式重新排序组装得到有序化后的激光点云数据，有序化处理过程如下：
将点云数据按照线通道信息ring排序，在同一线通道中按时间排序。由于激光的设计原理，一帧数据中每个线通道的扫描点数相同记为width，线通道数量记为height，因此得到类似图像中像素排列结构的数据帧，如图2所示。在该数据帧中，所有点存储到一维数组中，数组的大小即为所有点的数量，记为size＝height×width。因此，通过点在数组的索引index可以得到ring＝index/width。
作为进一步优选的方案，上述步骤S102中，对点云数据帧进行特征提取，得到点云数据帧的特征点，具体包括：
根据所述每一帧点云数据中的任一扫描点，得到该扫描点的邻域范围内的所有扫描点；
对该扫描点邻域范围内所有扫描点进行PCA主成成分分析，得到该扫描点邻域曲面的主方向V以及曲率值L；
将该扫描点邻域的曲率值L与预设的平面阈值C1、C2进行比较，判断该扫描点是否为特征点，其中，C1、C2均为常数且C1＜C2。
需要说明的是，由于在变电站环境中，有结构化物体，如围墙、监控楼、配电室、电线杆、部分电线架等，也有非结构化物体树木、绿化灌木层、崎岖不平的草地、外廓复杂或横截面积较小的物体等。本实施例结合变电站环境的特点，主要采用平面特征点、圆柱面特征点、外廓边缘点特征点这三类特征点。这三类特征点用于表达邻域范围内的特征信息，由于激光点云数据帧已经有序化处理，对任一扫描点P可以快速查询到P点邻域范围内的所有扫描点，通过对P点邻域周围点进行主成分分析(Principal components analysis，PCA)可以得到P点邻域曲面的一个主方向v(三维向量)以及曲率值L。
PCA主成分分析过程如下：
计算扫描点P的中心点：
计算协方差矩阵：
对协方差矩阵进行特征分解：Cov·vj＝λj·vj,j∈{0,1,2}；
计算曲率值：
其中，i代表邻域内点的索引，λj为第j个特征值，vj为主方向v对应的第j个特征向量，pi表示第i个邻域特征点，p-表示邻域中心，k表示邻域内点的索引个数且取值为常数，T为转置符号。
如图3所示，判别曲率是否满足L<＝C1，若是则该点可能平面特征点，则按照上述过程检验邻域内其他扫描点曲率是否满足上述条件，若所有邻域点均满足条件则标记该点为平面特征点同时标记邻域内其他所有点为已处理，若存在一个邻域点不满足条件则标记为非特征点。
如果C1<L<＝C2，则该点可能为圆柱面特征点，则按照上述过程检验邻域内其他点曲率是否满足上述条件，若所有邻域点均满足条件则标记该点为圆柱面特征点同时标记邻域内其他所有点为已处理，若存在一个邻域点不满足条件则标记为非特征点。
如果L>C2，则该点可能为边缘特征点。判断该点是否为孤立点(即该点周围没有邻接其他点)，如果该点是孤立点则标记该点为杂乱点。如果该点非孤立点，则判断该点的邻域点是否都在该点的一侧，是则标记该点为外廓边缘特征点同时标记邻域内其他所有点为已处理，否则标记该点为非特征点，其中C1<C2且C1、C2均为常数，一般可取C1为0.01，C2为1.0，本领域技术人员也可根据实际应用的需要对C1、C2取值进行适当调整。
作为进一步优选的方案，上述步骤S103，根据LM算法对变换矩阵进行优化，将使得所有特征点匹配的距离误差平方和最小的变换矩阵作为激光雷达的位姿，具体包括：
首先，以激光传感器的起始位置建立全局坐标系或以输入参数为起点建立全局坐标系。将上述提取的边缘特征点、平面特征点以及圆柱面特征点的类型和坐标信息分别应用点到线的ICP匹配、点到平面的ICP匹配、点到柱面的ICP匹配，通过列文伯格算法(Levenberg-Marquardt，LM)优化计算变换矩阵T使得所有特征点匹配的距离误差的平方和最小(即边缘特征点的匹配距离误差、平面特征点的匹配距离误差以及圆柱面特征点的匹配距离误差这个三个距离误差的平方和最小)。过程如下：
记当前帧点云数据所有特征点集为P{Ppi∈P，i＝0,1,2...n}，n为当前帧特征点的个数；
历史帧点云数据所有特征点集为Q{Qqi∈Q，i＝0,1,2...m}，m为历史帧特征点的个数；
两次相邻数据帧之间激光雷达的移动变换矩阵为Tk，下标k代表迭代次数，上标i代表第i个特征点，Pk为点集P经过k次迭代计算后的点集，记T为待估计的变换矩阵，Tsum为从初始时刻到当前时刻的变换矩阵，也即激光雷达的位姿，如图4所示，雷达位姿的迭代求解过程为：
(1)基于当前激光雷达的位姿Tsum，上一帧点云数据估计得到的Tk、历史帧特征点集Q以及当前帧点集P经过第k次迭代计算后的点集Pk，对Pk中每个特征点pi，计算pi＝Tk·pi，判断第i个特征点pi的类别，若为平面特征点，则在Q中查询与pi最近的3个平面特征点，将pi投影到这3个特征点所在平面上，得到投影点p′i；若为柱面特征点，则在Q中查询与pi最近的3个柱面特征点，将pi投影到这3个特征点所在圆柱面上，得到投影点p′i；若为外廓边缘特征点，则在Q中查询与pi最近的2个外廓边缘特征点，将pi投影到这2个特征点所在直线上，得到投影点p′i；记pi与p′i的距离为1i，记映射函数为f，则fi(Tk)＝1i。
(2)因此优化目标为求Tk+1使得fi(Tk+1)＝0，考虑所有点的误差，即求最小二乘解。
(3)计算雅可比矩阵J及更新Tk+1：
Tk+1＝Tk-(JTJ+λdiag(JTJ))-1JTJ)|。
(4)积分迭代增量T＝Tk+1·T。
(5)判断迭代是否收敛或当前迭代次数是否大于等于最大迭代次数，若是执行步骤(6)，若否则重复步骤(1)-(4)；
(6)更新雷达位姿Tsum＝T·Tsum。
其中，λ为LM方法计算的参数。
需要说明的是，对于每两帧数据通过特征匹配计算变换矩阵进行积分得到初步的位姿估计，为减少误差累积，将位姿Tsum＝T·Tsum和特征点P加入poseGraph，通过现有开源的非线性求解器如g20库、google ceres solver库等计算全局的最优位姿估计。
需要说明的是，激光原始数据表达的是在传感器坐标系下的观测点坐标，然而由于建图的数据扫描过程是一个运动累积过程，而每一帧原始激光数据的表达都是在传感器坐标系下，位姿更新模块计算每一帧的位姿。因此每一帧内的原始数据都有不同程度的扭曲，运动速度越大则数据扭曲程度越大。为了更好的地图融合处理，需要进行点云数据校正。由于在数据富信息处理时，每个扫描点的时间信息已经得到，在位姿态更新模块两帧之间的位姿变换矩阵也已经得到，因此根据每个扫描点的时间信息对位姿变换矩阵进行插值得到每个点位姿补偿量。作为进一步优选的方案，本实施例中在上述实施例公开的内容的基础上增加点云数据的校正，具体过程为：
根据所述每个扫描点的时间戳信息，对所述激光雷达的位姿进行插值，得到所述点云数据帧中扫描点在各时刻的位姿变换插值比例系数：
记点云数据帧中第g个扫描点的坐标cg，to为每帧数据的第一个扫描点的时间，t1为每帧数据的最后一个点扫描的时间，经过特征匹配及位姿更新所得T为to时刻到时刻t1的位姿变换矩阵，tg为第g个扫描点的时间，则tg时刻的位姿变换插值比例系数为：
根据比例系数εg，计算插值后的变换矩阵Tg；
根据插值后的变换矩阵Tg，对所述点云数据帧中的扫描点数据进行校正，得到校正后的点云数据帧，校正后的点云数据坐标为
相应地，步骤S104：根据激光雷达的位姿，将每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图，具体包括：
根据激光雷达的位姿，将校正后的每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图。
作为进一步优选的方案，步骤与S104中，根据激光雷达的位姿，将校正后的每帧点云数据转换为全局坐标系下的点云地图，并将点云地图转换为体素表达的地图，具体包括：
将经过位姿校正后的每帧数据为传感器坐标系下，转换为全局坐标系下得到点云地图，然后对每一帧数据的每个点，根据其坐标数据映射到体素地图中，将全局坐标系下的点云地图转换为体素表达的地图。传统的点云地图表达形式简单，不方便滤波去噪等处理，本实施例中通过转换成体素地图可对地图中的需要滤除的动态物体进行过滤，提高了创建的地图的清晰度。
需要说明的是，在实际应用中，由于传感器本身存在一定的误差，以及位姿计算存在误差，直接将点云数据映射到体素地图后会存在很多噪点。同时在数据扫描过程中，环境中会存在动态物体移动的情况，如过往的人流、车辆等，它们不应该表达在地图中因而需要过滤掉动态物体。因此，本实施例中的每个体素除了保存点云地图的映射关系之外还要记录一些额外信息，包括该体素被观测到的次数z，被穿过的次数s，该点为特征点次数h，该点特征类型次数hw；其中w为第w种类型的特征索引w＝0代表平面特征，w＝1代表柱面特征，w＝2代表边缘特征。因此每计算一阵点云数据后，将其映射到体素地图中，并对体素内信息进行更新。
如图5所示，对体素内的信息进行更新，具体为：
计算从位姿到观测点坐标穿过的所有体；
判断体素是否被穿过；
若是则体素穿过次数增加1，若否则体素观测次数增加1；
在观测点为特征点时，更新体素特征估计次数；
并根据观测点的特征类型将该特征类型的个数进行更新。
作为进一步优选的方案，在体素地图中，将多次扫描的数据进行融合滤波以过滤掉动态物体。本实施例采用的滤波方法为高通滤波器，即对每个体素计算其概率：fp＝n/(m+n)，若fp大于等于滤波阈值FP，则该体素保留，若fp小于滤波阈值FP，则该点被舍弃。其中滤波阈值FP的设定根据需求和经验值决定，其范围在0～1内，阈值越大，噪声点过滤越多，构建的地图越清晰，但小物体或物体细节部分会受影响；FP阈值越小，则噪点过滤较少，构建的地图细节较明显，但有可能会有重影。
需要说明的是，在扫描数据过程中，动态物体在不在同一个地方停留时间小于该区域的扫描时间的一半。通过上述阈值过滤过程，动态的障碍物会被过滤掉，得到更加清晰的3D地图。
较为优选地，本实施例通过八叉树结构存储体素地图。由于采用体素表达地图时，当地图表达粒度较细时需要大量存储，针对变电站环境较稀疏的特点，本实施例采用八叉树结构存储体素地图可减少算法的内存消耗。
本实施例公开的基于3D激光的3D地图创建方法，具有如下有益效果：
(1)传统的激光数据仅表达一个扫描点的距离或空间点位置，本专利提出的方法在此基础上增加了激光线通道，扫描时间等信息，并且通过有序化处理以便于特征提取，为后面的处理过程提供了数据基础。
(2)针对变电站场景的特点，提取3类特征点，基于特征的匹配优化得到空间变换矩阵并对点云进行扭曲校正，提高了点云地图精度。
(3)地图的表达采用基于八叉树的体素表达方式，除表达空间信息之外记录了与历史观测状态相关的统计信息，通过进行数据融合滤波，能够过滤由于传感器的误差和定位估计误差导致的噪声，同时能够消除动态移动物体的影响，最终得到清晰一致的3D地图。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。