基于激光测距的室内平面地图制图方法
【技术领域】
[0001] 本发明属于室内测量制图技术领域，尤其涉及一种基于激光测距的室内平面地图 制图方法。
【背景技术】
[0002] 随着计算机技术、无线定位技术、GIS及移动互联技术的飞速发展，基于位置的服 务（Location-BasedService)成为现实并在实际中得到了大量应用。在室内环境中，如机 场大厅、展厅、仓库、超市、图书馆、地下停车场、矿井等环境中，常常需要确定移动终端或其 持有者、设施与物品在室内的位置信息，并提供相应的附加诸如导航，搜索查询等基于室内 位置的应用服务。然而，由于室内建筑数量巨大且室内环境复杂多变，如超市，展厅装修布 局的周期性改变，对室内位置服务的地图更新的时效性提出了严峻的挑战。
[0003] 因此，非常有必要针对室内地图的时效性问题，提出一种行之有效的快速更新解 决方案。
[0004] 公告号为CN103337221A、名称为《一种室内地图制作方法》的中国专利公开了一 种基于GIS要素分类的室内地图的设计和表达，及数据管理方法，然而并未针对室内地图 的数据采集手段和快速更新提出相应的方法，在实际应用中仍然十分不便。
[0005] 其余与室内位置服务相应的专利主要集中在室内导航定位技术的应用。如《室内 定位方法》（申请号200810043032. 4)、《导航方法及其手持导航装置和导航系统》（申请号 200810035934. 3)、《提供大型建筑物内部的定位和导航的方法》（申请号200980129150. X)、《一种机场乘客定位系统及装置》（申请号201110177845. 4)、《一种移动空间四维信息 服务系统及其终端和定位方法》（申请号201210156494. 3)、《一种室内外定位系统》（申请 号201220517515. 5)，上述专利所涉及的室内定位技术或系统，其所采用的室内地图为建筑 结构图或平面示意图，并未涉及室内地图制作方法。
【发明内容】
[0006] 针对现有技术存在的不足，本发明提供了一种可靠有效、操作简单、高精度的基于 激光测距的室内平面地图制图方法，该方法可绘制大范围的室内地图，服务于室内位置月艮 务应用。
[0007] 为解决上述技术问题，本发明采用如下的技术方案：
[0008] 基于激光测距的室内平面地图制图方法，包括步骤：
[0009] 步骤1，移动传感器平台采集室内环境数据，所述的传感器平台包括水平放置的惯 性测量单元和LiDAR传感器，所述的室内环境数据包括导航数据和室内环境点云扫描帧， 导航数据利用惯性测量单元获得，室内环境点云扫描帧利用LiDAR传感器获得；
[0010] 步骤2,根据设定的室内地图的范围和最大分辨率，创建多分辨率的栅格地图；
[0011] 步骤3,结合室内环境数据和栅格地图，获得各历元下传感器平台最佳的位置和姿 态，本步骤进一步包括：
[0012] 3. 1遍历栅格地图中栅格点位置，并以预设角度分辨率遍历传感器平台姿态，找出 与当前历元扫描帧相似度最大的栅格点的位置和姿态，该位置和姿态即当前历元传感器平 台的基于室内环境点云匹配定位的位置和姿态；
[0013] 3. 2IMU自身利用机械编排定位，获得当前历元传感器平台基于导航数据的位置和 姿态；
[0014] 3. 3融合基于室内环境点云匹配定位和基于导航数据的位置和姿态，获得传感器 平台最佳的位置和姿态，同时将补偿误差估计值反馈给IMU机械编排定位；
[0015] 步骤4,根据最佳的位置和姿态，计算当前历元扫描帧中各点在栅格地图中栅格点 位置，将预设的多级相似度值逐分辨率图层写入对应栅格点；
[0016] 步骤5,将最大分辨率图层做地图输出，其他分辨率图层用作辅助匹配定位，在本 地坐标系下到导出栅格地图。
[0017] 步骤2中采用金字塔四叉树法创建多分辨率的栅格地图。
[0018] 子步骤3. 1中遍历栅格地图中栅格点和传感器平台姿态时，根据传感器平台的移 动速度和采样间隔设置最大搜索范围和搜索角度，根据实际情况和LiDAR传感器角度分辨 率设置搜索角度；最大搜索范围和搜索角度的设定要保证下一历元传感器平台的基于室内 环境点云匹配定位的位置和姿态落在最大搜索范围内。
[0019] 子步骤3. 1中当前历元扫描帧与栅格点的相似度
，其中， i为扫描帧中当前点编号，n为扫描帧中点总数，P(S」M)表示点i在栅格地图的预设相似 度值。
[0020] 步骤4中所述的多级相似度值通过高斯随机模型获得，所述的多级相似度值可预 设为 〇.l、〇. 3、0. 6、0. 9 四级。
[0021] 步骤5前还包括剔除栅格地图中动态物体，具体为：
[0022] 为各栅格点设置计数量N，每当有点云点扫到该栅格点，N加1 ;导出栅格地图前， 检查栅格点计数量N，若计数量值小于预设阈值F，则剔除该栅格点；预设阈值F根据试验验 证获得。
[0023] 与现有技术相比，本发明具有以下优点：
[0024]1、操作简单。
[0025] 数据采集操作简单，无需预先了解环境空间结构，只需从室内任意起点开始，连续 推扫室内环境即可。数据采集传感器可搭载在任意移动载体，如推车或自主机器人平台等， 对操作人员稍加培训即可。
[0026] 2、成本低。
[0027] 使用低成本的消费级IMU和低成本LiDAR，即可制成可用于室内基于位置服务所 需的室内平面地图。
[0028] 3、数据后处理速度快。
[0029] 数据采集后可现场近实时进行定位和制图处理，可实现室内地图的多次、定期、快 速更新，有效解决用于室内基于位置服务所需平面地图的制图效率瓶颈问题。
【附图说明】
[0030] 图1是本发明的具体流程示意图；
[0031]图2是本发明的多分辨率相似度地图结构图；
[0032]图3是本发明的LiDAR点云相似度匹配定位算法流程；
[0033] 图4是本发明的扩展卡尔曼滤波组合定位算法流程；
[0034] 图5是本发明的动态物体剔除算法流程；
[0035] 图6是本发明的数据采集软件；
[0036] 图7是本发明的后处理制图软件；
[0037] 图8是本发明的制图结果范例。
【具体实施方式】
[0038] 下面将结合附图和具体实施例进一步说明本发明技术方案。
[0039]本发明具体步骤见图1，包括：
[0040]步骤1，数据采集。
[0041] 采用传感器平台采集制图数据，传感器平台由水平放置的IMU(惯性测量单元）和 LiDAR传感器组成，由电池供电，并连接至计算机。传感器平台置于手推车或移动机器人等 运动载体上。
[0042] 数据采集时，启动传感器平台，静置3-5分钟，利用LiDAR传感器水平扫描采集室 内环境点云，以便后期进行頂U零偏修正，提高定位精度。然后，将运动载体在室内环境随 意移动，移动时应尽可能匀速平稳行驶并避免高密度的移动人群和物体，以保证LiDAR传 感器尽可能扫描到室内环境点云，如墙面、家具布置等。
[0043] 步骤2,创建栅格地图M，用于存储LiDAR传感器采集的环境相似度信息。
[0044] 为绘制大范围的室内地图，具体实施中，首先预估室内范围大小，然后根据预估室 内范围大小和室内地图所需精度设置地图范围和栅格格网精度（即地图分辨率），并通过 金字塔四叉树算法进行动态数据调度，作为后续点云匹配定位和制图的基础，利用预设的 多级相似度值生成多分辨率的栅格地图。
[0045] 步骤3,基于室内环境点云获得传感器平台的位置和姿态。
[0046] 采用基于栅格地图的最大相似度LiDAR点云特征匹配算法及MU融合定位算法处 理室内环境点云，获得传感器平台最佳的位置和姿态。
[0047] 本步骤进一步包括：
[0048] 3.ILiDAR点云和栅格地图的匹配定位。
[0049] LiDAR点云匹配定位的目的在于从上一历元位置和姿态开始，通过遍历各个可能 的候选位置和姿态，计算栅格地图中与当前扫描帧相似度最大的栅格位置和平台姿态，进 而计算下一历元的位置和姿态。
[0050] 为了兼顾搜索效率和定位精度，具体实施中设置了多层搜索策略，见图4,从低分 辨率栅格地图向高分辨率栅格地图逐层搜索，并设置最大搜索范围、搜素角度以及搜索角 度分辨率，以尽可能快的速度搜索到最佳位置。
[0051] 最大搜素范围和搜素角度根据传感器平台行进速度和采样间隔进行设置，设置原 则是保证历元间隔下，下一历元位置和姿态落在最大搜索范围内。例如，人工推扫情况下， 传感器平台行进速度约0. 5m/s，最大搜素范围可设为0. 5-lm，搜索角度可设为5度-10度； 根据实际效果和LiDAR传感器角度分辨率参数设置搜索角度分辨率，如设置为0. 25度。
[0052] 本步骤基于高斯随机模型实现。当接收到新扫描帧St，将其与栅格地图M进 行搜索查找，在最大搜索范围内获得扫描帧St与各栅格点的相似度P(St|M)，以找到最 佳匹配栅格点，最佳匹配栅格点的判断条件为与扫描帧相似度最大的栅格点。理论上
，即在扫描帧中，各点与最大搜索范围内各栅格点相似度的乘积。 i为扫描帧中当前点编号，n为扫描帧中点总数，P(S」M)表示点i在栅格地图的预设相似 度值。实际系统中，因为点数量是固定的，可采用求和代替表达最大相似值。
[0053] 3. 2扩展卡尔曼滤波组合定位，获得当前历元传感器平台最佳的位置和姿态。
[0054] 有些状态下，LiDAR点云会由于环境特征信息不足，造成不可修正的匹配定位误差 累积。通过惯性导航和LiDAR匹配的融合定位，可利用IMU的短时高精度位移和姿态推算 对LiDAR进行校正，以获得最佳定位位置。
[0055] 本发明提出了基于扩展卡尔曼滤波（EKF)的定位融合方法，以获得高精度的定位 结果，其具体流程见图4。MU采样频率高，例如100Hz或200Hz，LiDAR采样频率低，例如 30~40Hz。融合定位过程中，无LiDAR匹配定位信息的时刻，IMU自身利用机械编排定位， 即利用IMU提供的加速度计和陀螺仪进行导航定位，获得传感器平台的基于导航数据的位 置、速度和姿态，其中，表示頂U定位的位置，表示頂u定位的速 度，表不IMU定位的姿态