移动激光雷达slam制图装置及数据处理方法与流程

本发明涉及一种激光雷达导航定位处理方法，尤其涉及一种移动激光雷达slam制图装置及数据处理方法。

背景技术：

slam(simultaneouslocalizationandmapping)技术是指即时定位与地图构建，即通过处理传感器采集的周围环境数据，实时反馈当前运动系统在未知环境的位置并同时绘制运动系统的周围环境地图。随着激光雷达技术的发展，移动slam制图系统在林业、交通、矿道、建筑等领域已显现出广阔的应用前景。

虽然slam技术在很多场景都有应用，但是通过slam技术来实现多传感器采集的进行制图及其数据处理方面却很少有这方面的技术公开，而且如何将移动激光雷达系统中各个传感器稳定的融合在一起，同时保证便携易操控，采集数据稳定可靠，以实现slam制图装置的模块化是目前需要解决的问题。

申请号为201610799842.7的中国发明专利申请中公开了“一种针对大尺度环境的多机器人协同制图与定位的方法”，其包括基于视觉检测闭环的单机器人激光slam算法、多机器人位姿约束估计算法以及多机器人地图融合算法；其中，基于视觉检测闭环的单机器人激光slam算法利用视觉传感器来辅助激光传感器，实现更为稳定鲁棒性的slam算法。

申请号为201910378821.1的中国发明专利申请中公开了“基于slam步态imu融合的室内人员自主定位方法”，其包括室内人员穿戴定位装置并将其启动，穿戴装置的人员在室内环境中自由移动，slam传感器将测得的数据传输给计算机，计算机使用slam算法对室内环境进行建图并对人员自身定位，同时，imu元件将测得的数据传输给计算机，计算机通过基于步态的imu定位算法计算人员的运动轨迹，再将融合slam定位结果与步态imu定位结果进行人员定位，以对环境依赖小的步态imu定位技术作为挑战性环境下的辅助定位手段，将两者融合起来的人员自主定位系统，不仅在正常环境中具备良好的导航定位性能，在火场等特殊室内环境中也具有较强的鲁棒性。

上述技术方案虽然都是基于slam技术的应用，但是其实现的技术方案并不能解决前述中提到的技术问题。

技术实现要素：

本发明的目的在于克服现有技术的缺点，提供了一种移动激光雷达slam制图装置及数据处理方法，基于移动激光雷达制图设备的模块化设计，可以在装置中灵活定制各类数据采集传感设备，便携方便的拆装设置，整体装置集成度高、稳定性好，故障率低，且模块易替换。

本发明的目的通过以下技术方案来实现：一种移动激光雷达slam制图装置，包括卫星信号接收单元、数据采集传感单元、imu惯性测量单元和制图采集计算单元：

卫星信号接收单元：用于接收卫星定位信号并分别与所述数据采集传感单元、imu惯性测量单元和制图采集计算单元连接，实现定位及时间同步信息的传输；

数据采集传感单元：用于捕捉某一时空区域下的影像信息，并将采集的影像信息发送给所述制图采集计算单元；

imu惯性测量单元：与所述制图采集计算单元连接将数据发送到所述制图采集计算单元，并获取所述卫星信号接收单元发送的精确gps时间信息；

制图采集计算单元：提供多种数据接口通过多线程方式获取所述数据采集传感单元的原始数据，实现多传感融合的slam制图。

所述数据采集传感单元包括高精度全景相机和激光雷达扫描仪；所述高精度全景相机用于在保持与所述数据采集传感单元中其他传感设备时间同步的同时，将捕捉到的高精度影像信息发送到所述制图采集计算单元；所述激光雷达扫描仪与所述卫星信号接收单元连接获取精确的gps时间，并将gps定位信息和时间信息发送到所述制图采集计算单元。

所述多传感融合的slam制图方法包括：

依据imu惯性测量单元和卫星信号接收单元接收的数据以及二者之间的标定信息，利用卫星定位信息、惯性导航系统和卡尔曼滤波原理求解得每一时刻的运动系统初始定位信息p0；

利用初始定位信息p0和高精度全景相机采集的影像数据，根据视觉slam技术生成高精度的运动系统定位信息p1；

根据所述运动系统定位信息p1和激光雷达扫描仪采集的点云数据，采用激光slam技术生成更高精度的运动系统定位信息p2；

利用各个传感设备提供的定位信息进行联合优化，得到当前运动系统的定位信息，再根据求解的高精度轨迹以及原始点云重新解算每一时刻的点位置，最终得到高精度地图。

所述装置内从上到下依次设置所述卫星信号接收单元、高精度相机、激光雷达扫描仪、imu惯性测量单元；所述制图采集计算单元设置于便携式背包的背负系统内。

所述数据采集传感单元还包括倾斜式激光扫描仪；设置于所述背包外，与所述制图采集计算单元和所述卫星信号接收单元连接，用于对所述激光雷达扫描仪在某些场景无法扫描到侧面或地面点云时，进行扫描以实现辅助定位及确保点云完整。

移动智能设备通过wifi网络或者有线网络对所述制图采集计算单元进行通信连接，访问控制所述制图采集计算单元并控制数据采集、数据拷贝以及实时点云显示。

一种数据处理方法，采用上述任一项所述的移动激光雷达slam制图装置，所述数据处理方法包括：

数据压缩转换：对制图采集计算单元每次生成的点云、轨迹点和影像数据通过预设压缩方式压缩后进入传输队列中；

数据还原显示：移动智能客户端与所述制图采集计算单元建立通信连接，将获取的压缩的二维数据还原为三位点云及影像，并通过webgl技术实现实时显示。

所述预设压缩方式包括：通过将降采样后的每帧点云的三维坐标点转换为极坐标形式后再通过一个旋转矩阵的方式来进行数据存储。

所述制图采集计算单元实时监测每个所述移动智能客户端，移动智能客户端发送指令数据实现控制数据采集传感单元中各个传感设备启动数据采集处理及数据拷贝，同时将指令处理的反馈结果发送到所述移动智能客户端。

一种存储介质，所述存储介质上存储有计算机控制程序指令，所述计算机控制程序指令被处理器执行时实现所述数据处理方法的步骤。

一种终端设备它包括处理器以及存储有计算机控制程序指令的存储器；在所述处理器内运行所述计算机控制程序指令，所述运行所述计算机控制程序指令时执行时实现所述数据处理方法的步骤。

本发明具有的有益效果为：一种移动激光雷达slam制图装置及数据处理方法；对移动激光雷达制图设备进行模块化设计，提供高集成化的采集系统，可以灵活定制各种采集传感设备，方便携带、拆装以及维修和替换处理，数据采集处理稳定性高，故障率低且易替换；

针对对外作业采集，不需要携带其他手持设备，只需要通过手机或者平板电脑连接本装置后点击start按钮即可开始数据采集，采集完成后点击stop即可；

同时通过移动激光雷达slam制图装置的数据处理方法，以确保智能客户端能接收到初步的采集好的点云数据，能够对采集作业进行质量判断，同时完整数据也通过u盘即可快速导出，该方案适用性广，以网页形式来调用，可在用户自有设备中使用而不需要按照其他软件，同时使用显卡加速方式，确保渲染速度能够达到实时要求。

附图说明

图1为本发明装置的结构示意图；

图2为本发明方法的流程图；

图3为本发明装置的数据处理结构图。

具体实施方式

为使本申请实施例的目的、技术方案和有点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的保护范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。下面结合附图对本发明做进一步的描述。

如图1所示，为发明实施例提供的一种移动激光雷达slam制图装置，包括卫星信号接收单元、数据采集传感单元、imu（测量物体三轴姿态角度及加速度的装置）惯性测量单元和制图采集计算单元：其中，数据采集传感单元包括高精度相机、激光雷达扫描仪和倾斜式激光扫描仪；

卫星信号接收单元：用于接收卫星定位信号并分别与所述数据采集传感单元、imu惯性测量单元和制图采集计算单元连接，实现定位及时间同步信息的传输；

进一步地，卫星信号接收单元可以为定制的gnss（全球导航卫星系统定位）板卡及天线主要用于接收卫星发射的定位信号，其设置于装置的最上一层，gnss板卡分别于激光雷达扫描仪、imu惯性测量单元以及制图采集计算单元连接，实现定位及时间同步信息的发送。

进一步地，高精度相机设置在gnss板卡的下面一层，用于捕捉高精度影像信息；可以通过高精度相机自带的gps天线来获取gps时间与其他的数据采集传感设备保持时间同步，或者也可以通过高精度相机自带的imu与本发明中的imu惯性测量单元进行时间同步。

进一步地，激光雷达扫描仪设置在高精度相机下面一层，与制图采集计算单元连接，同时与gnss板卡连接获取精确的gps时间，其主要用于实现定位。

进一步地，倾斜式激光扫描仪设置于所述背包外，与所述制图采集计算单元和gnss板卡连接获取精确的gps时间，用于对所述激光雷达扫描仪在某些场景无法扫描到侧面或地面点云时，进行扫描以实现辅助定位及确保点云完整。

进一步地，imu惯性测量单元：设置在激光雷达扫描仪的下面一层，与所述制图采集计算单元连接将数据发送到所述制图采集计算单元，并获取所述卫星信号接收单元发送的精确gps时间信息；

进一步地，在imu惯性测量单元下面为背包背负系统，主要确保能够稳定的承载上面的各种数据采集传输部件，同时保证便携；其内部设置有电源和制图采集计算单元等等。

进一步地，制图采集计算单元：提供多种数据接口通过多线程方式获取所述数据采集传感单元的原始数据，实现多传感融合的slam制图。

具体地，制图采集计算单元提供了usb接口，以太网接口及hdmi接口。usb接口用来拷贝数据；以太网接口通过有线连接外部平板电脑，或者通过发出的wifi信号使得手机设备方便连接；hdmi接口用来调试设备。制图采集计算单元通过多线程方式实时获取上述各个传感器的原始数据，确保数据稳定传输并存储，以及实时处理或后处理。处理的结果可通过usb接口导出到u盘中。

进一步地，所述多传感融合的slam制图方法包括：

s1、依据imu和gnss接收的数据以及二者之间的标定信息，利用gnss卫星定位信息、惯性导航系统和卡尔曼滤波原理求解得每一时刻t的运动系统初始定位信息p0；得到定位信息的初值，为后续步骤提供初始数据。

s2、利用初始定位信息p0和高精度全景相机采集的影像数据，根据视觉slam技术生成高精度的运动系统定位信息p1；结合前面定位信息给的初值，能更有效提取匹配点，并进行ransac筛点的操作。

具体地，首先提取对每一相邻的两帧图像关键帧，提取特征点集，其中包括orb特征点和harris角点等，根据第一步得到的初始定位信息，在已知当前两帧关键帧的初始位置信息的情况下，通过投影特征点在三维空间中估计的位置，可极大提升ransac匹配效率，提取有有效点，去除异常点，得到点集p^t,p^(t-1)并计算统一世界坐标系下每个特征点的空间坐标位置。这些特征匹配点对的变换关系可表示为如下式所示：

∀i,p_i^t=rp_i^(t-1)+t

其中r为相邻两帧位姿旋转变换矩阵，t为平移矩阵，p_i^t与p_i^(t-1)为t时刻到t+1时刻的特征点匹配点对。采用最小化重投影误差的方法来求解位姿r与t，如下式：

其中，p^t表示图像帧f_1的所有特征点集合，p^(t-1)表示图像帧f_1的所有特征点集合；r为载体的旋转矩阵，t为载体的平移向量,n表示特征点对的数目。

计算得到相邻关键帧的旋转平移矩阵，并依次得到所有关键帧的定位信息p1。

s3、根据所述运动系统定位信息p1和激光雷达扫描仪采集的点云数据，采用激光slam技术生成更高精度的运动系统定位信息p2；

具体地，对于当前帧激光点云数据pk，依据以下公式可计算得到其点特征fk1,线特征fk2，面特征fk3。

其中，i为pk中的一个点，xi为点i的坐标，p为点i的邻域点集合，j为p中的点，xi为点i的坐标，f为特征值；预先给定阈值m_1、m_2、m_3和m_4，针对当前点的特征值f小于m_1的点属于特征f_k1，大于m_2小于m_3的点属于f_k2，大于m4的点属于f_k3。

根据初始定位信息p1，将每帧的特征数据转换到p1定位信息对应的坐标系。获取相邻两帧点云数据p_t和p_(t-1)，对所有匹配对f_(t-1)在f_t中进行领域搜索，确定所有候选特征匹配对。根据匹配对并通过最小二乘法求解得到相邻两帧点云数据的旋转平移参数r和t。具体的，可通过如下公式求解参数：

其中，y表示从相邻的后一数据帧中提取的特征，x表示从相邻的前一数据帧中提取的特征，r为载体的旋转矩阵，t为载体的平移向量。然后根据得到的结果对匹配对进行优选，并重新计算特征点f_t'。对点云p_(t-1)中的特征点在f_t'中重新查找特征点对，重新计算得到新的旋转平移矩阵r和t，并更新之。最终得到相邻两帧的旋转平移位置信息r_(t-1)和t_(t-1)，并将当前的最优特征匹配对加入到匹配数据库k中。

最后，通过依据相邻帧的转换矩阵，得到依据激光点云信息的定位信息p2。

当前步骤的优势在于提出了一种较好的特征点提取方法，能够更加有效的表征线特征及面特征，使得在后面的特征匹配的步骤上，能跟有效的匹配两帧点云，提升匹配精度。

s4、利用各个传感设备提供的定位信息进行联合优化，得到当前运动系统的定位信息；

具体地，将之前求得的激光点特征z^laser，图像关键帧的特征点集z^img，gnss信息z^gnss以及imu预积分约束信息z^imu等数据集进行联合优化，通过对每一时刻的状态集合，求解联合概率分布的最大后验概率，得到每一个时间点的最优状态量：

其中，，进而求得当前时刻的高精度轨迹；再根据求解的高精度轨迹以及原始点云重新解算每一时刻的点位置，最终得到高精度地图。

本技术方案中，主要优势在于将激光点云特征和图像特征都加入到了联合优化中去，相比其他的方法只加入其中之一，本方案针对之前特征单一造成的稳定性差的问题提出了一套解决方案。同时，基于联合优化的方法通过融合gnss的定位信息，能获取到大地坐标系表示的点云，从而能够满足基于大地坐标的测绘要求。通过结合imu的预积分约束信息和图像关键帧提取的特征点，能提升成图过程的稳定性，防止出现在经过狭窄楼道等特征点比较低的情况下，误差较大，成图精度较低的问题。

进一步地，移动智能设备通过wifi网络或者有线网络对所述制图采集计算单元进行通信连接，访问控制所述制图采集计算单元并控制数据采集、数据拷贝以及实时点云显示。

具体地，移动智能设备能够控制采集设备开始/关闭采集，控制采集处理单元重启/关闭，拷贝数据等，查看采集传感器信息，更新系统固件及相关设置等，这些功能基本能保证采集工作能一键便携的完成，方便外业人员能够单人即可作业。由于模块化设计的便利性，相比之前的制图装置，本装置能更加灵活的根据客户需求定制化设备，包括gnss设备，全景相机及倾斜的激光扫描仪等。

如图2和图3所示，为发明实施例提供的一种数据处理方法，采用上述的移动激光雷达slam制图装置，所述数据处理方法包括：

s1、多传感设备数据采集：通过三维激光雷达扫描仪和/或倾斜式三维激光扫描仪以及高精度全景相机实时采集影像数据发送到制图采集计算单元，同时gnss板卡和imu惯性测量单元也将gnss数据和imu数据发送到制图采集计算单元；

s2、数据压缩转换：对制图采集计算单元每次生成的点云、轨迹点和影像数据通过预设压缩方式压缩后进入传输队列中；

具体地，制图采集计算单元根据接收到的数据每次生成一帧点云及轨迹点和影像数据，然后再对数据进行压缩；其压缩方式包括：通过将降采样后的每帧点云的三维坐标点转换为极坐标形式后再通过一个旋转矩阵的方式来进行数据存储，具体如下式所示：

其中，p为极坐标形式的数据，d为原始格式点云数据，r与t为进行旋转平移变换，将原始点云数据转到中心点为原点的坐标系。

具体为采用球面极坐标把三维图像中每个点的坐标转换到球面极坐标，然后把球面极坐标对应到相应的二维图像中去进而实现数据的压缩。

首先将三维图像数据实现球面极坐标转换，先确定球面极坐标系的坐标原点，选取点云曲面包围盒的中心作为坐标原点，对于任一点p[x，y，z]，其球面极坐标[r,φ,θ]可以通过笛卡尔坐标和球面极坐标间的转换关系得到，进而得到三维图像所有点的球面极坐标；其中，r表示空间中一点p到原点o的距离，φ是在xy平面上从x轴正向旋转得到的角度，即方位角，范围为0-2π；θ表示在zy平面上从z轴正向旋转得到的角度，即极角，范围为0-π；扫描点云曲面上所有的点，可以得到r,φ,θ的区间。

然后得到了球面极坐标后，把球面极坐标对应到二维图像上去，实现三维图像的从球面极坐标到二维图像的转换；具体地，把方位角φ与极角θ区间的叉乘对应到分辨率为m×n的图像上，r对应到g上；其中，m为二维图像宽度，n为二维图像高度，g为灰度；进而实现三维图像的球面极坐标到二维图像的压缩转换。

由于激光扫描结构限制，每一帧点云都在一定角度和距离范围内，这样存储能够极大的提高数据一致性，从而能在后面步骤提高压缩比。

s3、数据还原显示：移动智能客户端与所述制图采集计算单元建立通信连接，将获取的压缩的二维数据还原为三位点云及影像，并通过webgl技术实现实时显示。

进一步地，移动智能客户端（手机客户端）请求数据，并建立连接。每次将获取的压缩的二维数据还原到三维点云及影像，然后通过使用webgl技术，调用手机显卡来实时渲染三维点云及影像数据，从而确保设备的实时显示。手机客户端显示时，将每帧与之前的数据一起显示在手机或平板客户端，从而展示了制图效果。

s4、制图采集计算单元实时监听每个所述移动智能客户端，移动智能客户端发送指令数据实现控制数据采集传感单元中各个传感设备启动数据采集处理及数据拷贝，同时将指令处理的反馈结果发送到所述移动智能客户端。

本发明另一实施例包括一种存储介质，所述存储介质上存储有计算机控制程序指令，所述计算机控制程序指令被处理器执行时实现所述数据处理方法的步骤。

本发明又一实施例包括一种终端设备它包括处理器以及存储有计算机控制程序指令的存储器；在所述处理器内运行所述计算机控制程序指令，所述运行所述计算机控制程序指令时执行时实现所述数据处理方法的步骤。

以上所述仅是本发明的优选实施方式，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。