基于激光测距的室内平面地图制图方法
基于激光测距的室内平面地图制图方法
【技术领域】
[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定位的姿态 。
[0056] 在有LiDAR匹配定位信息时，通过EKF估计融合校正获得融合后传感器平台的位 置、速度和姿态，LiDAR匹配定位获得的传感器平台的位置和姿态分别为 、4咖。同时将补偿误差估计值Sx = [Srn，Svn，en，Sba，Sbg]反馈给MU的机 械编排过程，提高后续推导定位和姿态估计精度。实际操作过程中，可根据所用传感器本身 特性设置合适的EKF滤波误差参数。
[0057] 其中相关主要计算公式描述如下：
[0058] 惯性系统状态方程为：
[0064] 其中，，=以乂/小表示传感器平台在导航坐标系下的位置估计值，f是炜度，入 是经度，h为高程；vn表示传感器平台移动速度；是传感器平台坐标系到导航坐标系的变 换矩阵，G则是传感器平台坐标系到导航坐标系的变换矩阵；fb表示比力； < 是在传感器 坐标系下由陀螺仪测量得到的角速度； < 和 < 表示地球自转在导航坐标系下的角速度和 导航坐标系相对于地固系下的角速度；么x)是的反对称矩阵；gn表示当地重力向量； M和N分别表示子午圈和夕P酉圈曲率半径；；bJPb8分别表示加速度计和陀螺仪漂移量。
[0065] 误差状态方程如下：
[0066] 8x= [Sr11, 8vn,en, 8ba, 8bg] (6)
[0073] 其中，SX表示状态误差向量，6，表示位置误差，Svn表示速度误差，en表示姿 态误差，8匕表示加速度漂移误差，6、表示陀螺仪漂移误差，SXk、SXk+1分别表示第k、 (k+1)历元时的状态误差向量；u表示噪声向量，F是动态矩阵，G是设计矩阵，〇k表示状态 转移矩阵，wk表示驱动白噪声，服从wk~N(0,Qk)高斯分布；Qk表示协方差矩阵，Q表示谱 密度矩阵；Sg、S3分别表示加速度和陀螺仪的误差标准方差。At为相邻历元时间间隔。
[0074] 系统观测方程如下：
[0075] zk=Hk 8xk+vk (13)
[0079] 其中，匕u是IMU机械编排预测位置；表示LiDAR匹配定位观测位置； 和分别表示欧拉角表示的预测姿态和观测姿态；Hk是观测矩阵，vk为观测量驱动白噪 声，服从vk~N(0,Rk)高斯分布，Rk表不观测协方差矩阵。
[0080] 误差反馈方程如下：
[0084] 其中，/T、（、cr分别表示滤波前的位置、速度和姿态；（0)是误差角度< 的 反对称矩阵。
[0085] 步骤4,栅格地图的更新。
[0086] 根据当前历元传感器平台的位置和姿态，计算出当前历元LiDAR扫描帧中各个点 在栅格地图中具体位置。在栅格地图中，逐分辨率图层写入LiDAR点云的位置和相似度信 息，从而达到建立或更新测图信息的目的。
[0087] 相似度值表示栅格点为地物点的概率值，其值为0~1，为栅格点存储的信息，栅 格点存储的相似度值采用高斯随机模型预设。如图2所示，具体实施中，依据高斯随机模 型，将栅格点相似度值，依据距离中心点的远近预设为〇.l、〇. 3、0. 6、0. 9四级的范例。
[0088] 步骤5,剔除栅格地图中动态物体。
[0089] 数据采集中，不可避免会出现环境中因为行人经过等产生的动态点云。这些点云 虽然在匹配定位过程中有一定帮助，但作为制图而言，这些动态物体不是需要成图的环境 特征，应从栅格地图中剔除。具体实施中采用栅格加权投票方法来剔除动态物体，见图5所 示，具体为：为各栅格点设置计数量N，每当有点云点扫到该位置时，N加1。定位和制图过 程完成后，检查各栅格点的计数量N。若N值小于预设阈值F，则认为是动态物体。具体实 施时，阈值F可根据数据采集传感器平台的移动速度和观察制图效果设置，若为人工推扫， 阈值F可取2或3。
[0090] 步骤6,生成并输出室内地图。
[0091] 在栅格地图中，将最大分辨率图层做地图输出，其他分辨率图层用作辅助匹配定 位，导出本地坐标系下的室内平面地图原型。
[0092] 本发明核心在于采集数据的后处理，主要包括多分辨率栅格室内平面图表达，基 于最大相似度的LiDAR点云特征匹配算法，MU融合定位算法及动态物体剔除算法几大部 分组成。
[0093] 主要数据流程包括：
[0094](1)预设置栅格地图大小和最大分辨率，如1平方公里范围和最大栅格分辨率为1 厘米，系统通过栅格金字塔算法，自动分层，以便于在后期点云匹配中快速调度和匹配；
[0095] (2)逐历元LiDAR点云扫描帧与多层栅格地图进行相似度匹配，进行特征匹配定 位；
[0096] (3)IMU自身导航的机械编排定位；
[0097] (4)扩展卡尔曼滤波（EKF)对LiDAR匹配定位结果和MU定位结果进行数据融合， 得到传感器中心当前历元的最佳的位置和姿态；
[0098] (5)以该最佳定位位置和姿态，及LiDAR测距信息为基础，计算LiDAR扫描帧中各 个点在相对地图中位置，在该点写入相似度预设信息，更新栅格地图；
[0099] (6)在数据采集过程中，碰到的行人及车辆等动态物体，不应作为室内地图环境特 征，需要被剔除。本发明使用一种栅格加权投票的算法，可快速滤除大部分简单动态信息；
[0100] (7)导出最大相似度表达的制图结果。
[0101] 本发明可采用计算机软件技术自动运行，包括数据采集记录软件和后处理定位制 图软件。下面将结合应用实例，进一步说明本发明。
[0102] 以地下停车场为例，传感器平台为消费级IMU和商用LiDAR。首先，在停车场中任 意起点开始，启动数据采集平台，静置3-5分钟。开始沿任意路径推动平台移动，数据采集 记录软件则记录LiDAR和MU的测量数据，并生成数据记录文件，如图6所示。
[0103] 数据采集结束后，将数据记录文件导入后处理定位制图软件，如图7所示。根据停 车场范围，设置栅格地图范围约为1平方公里，栅格分辨率为1厘米。设置匹配搜索距离为 0.5米，搜索角度为5度，搜索角度间隔0.25度。然后开始定位和同步制图。直到数据处理 结束。
[0104] 在数据处理过程中，数据处理系统自动30秒执行一次，动态信息清除。无需用户 操作。制图结果见图8。
[0105] 以上所述为本发明的最佳实施例，并非对本发明作任何形式上的限制，凡是依据 本发明的技术实质对以上实例所作的任何简单修改、等同变化与修饰，均仍落入本发明的 保护范围内。
【主权项】
1. 基于激光测距的室内平面地图制图方法，其特征是，包括步骤： 步骤1，移动传感器平台采集室内环境数据，所述的传感器平台包括水平放置的惯性测 量单元和LiDAR传感器，所述的室内环境数据包括导航数据和室内环境点云扫描帧，导航 数据利用惯性测量单元获得，室内环境点云扫描帧利用LiDAR传感器获得； 步骤2,根据设定的室内地图的范围和最大分辨率，创建多分辨率的栅格地图； 步骤3,结合室内环境数据和栅格地图，获得各历元下传感器平台最佳的位置和姿态， 本步骤进一步包括： 3. 1遍历栅格地图中栅格点位置，以预设角度分辨率遍历传感器平台姿态角度，找出与 当前历元扫描帧相似度最大的栅格点的位置和姿态，该位置和姿态即当前历元传感器平台 的基于室内环境点云匹配定位的位置和姿态； 3. 2IMU自身利用导航数据的机械编排定位，获得当前历元传感器平台基于导航数据的 位置和姿态； 3. 3融合基于室内环境点云匹配定位和基于导航数据的位置和姿态，获得传感器平台 最佳的位置和姿态，同时将补偿误差估计值反馈给MU机械编排定位； 步骤4,根据最佳的位置和姿态，计算当前历元扫描帧中各点在栅格地图中的栅格点位 置，将预设的多级相似度值逐分辨率图层写入对应栅格点； 步骤5,将最大分辨率图层做地图输出，其他分辨率图层用作辅助匹配定位，在本地坐 标系下到导出栅格地图。2. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 步骤2中采用金字塔四叉树法创建多分辨率的栅格地图。3. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 子步骤3. 1中遍历栅格地图中栅格点位置和以预设角度分辨率遍历传感器平台姿态 时，根据传感器平台的移动速度和采样间隔设置最大搜索范围和搜索角度，根据实际情况 和LiDAR传感器角度分辨率参数设置搜索角度；最大搜索范围和搜索角度的设定要保证下 一历元传感器平台的基于室内环境点云匹配定位的位置和姿态落在最大搜索范围内。4. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 子步骤3. 1中当前历元扫描帧与栅格点的相似度其中，i为 扫描帧中当前点编号，η为扫描帧中点总数，P(SiIM)表示点i在栅格地图的预设相似度值。5. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 子步骤3. 3中采用扩展卡尔曼滤波法融合基于室内环境点云匹配定位和基于导航数 据的位置和姿态。6. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 步骤4中所述的多级相似度值通过高斯随机模型获得。7. 如权利要求6所述的基于激光测距的室内平面地图制图方法，其特征是： 所述的多级相似度值可预设为〇. l、〇. 3、0. 6、0. 9四级。8. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 步骤5前还包括剔除栅格地图中动态物体，具体为： 为各栅格点设置计数量N，每当有点云点扫到该栅格点，N加1 ;导出栅格地图前，检查 栅格点计数量N，若计数量值小于预设阈值F，则剔除该栅格点；预设阈值F根据试验验证获 得。
【专利摘要】本发明公开了一种基于激光测距的室内平面地图制图方法，包括步骤：步骤1，移动传感器平台采集室内环境数据；步骤2，创建多分辨率的栅格相似度地图；步骤3，结合室内环境数据和栅格相似度地图，获得各历元下传感器平台最佳的位置和姿态；步骤4，根据最佳的位置和姿态更新栅格相似度地图；步骤5，在本地坐标系下到导出栅格相似度地图。本发明数据采集操作简单，且无需预先了解环境空间结构；成本低廉，成图速度快，可实现室内地图的多次和快速更新，有效解决室内制图的效率瓶颈问题。
【IPC分类】G01C21/20, G01C3/00, G01C21/16
【公开号】CN104897161
【申请号】CN201510295778
【发明人】唐健, 陈育伟
【申请人】武汉大学
【公开日】2015年9月9日
【申请日】2015年6月2日
【技术领域】
[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定位的姿态 。
[0056] 在有LiDAR匹配定位信息时，通过EKF估计融合校正获得融合后传感器平台的位 置、速度和姿态，LiDAR匹配定位获得的传感器平台的位置和姿态分别为 、4咖。同时将补偿误差估计值Sx = [Srn，Svn，en，Sba，Sbg]反馈给MU的机 械编排过程，提高后续推导定位和姿态估计精度。实际操作过程中，可根据所用传感器本身 特性设置合适的EKF滤波误差参数。
[0057] 其中相关主要计算公式描述如下：
[0058] 惯性系统状态方程为：
[0064] 其中，，=以乂/小表示传感器平台在导航坐标系下的位置估计值，f是炜度，入 是经度，h为高程；vn表示传感器平台移动速度；是传感器平台坐标系到导航坐标系的变 换矩阵，G则是传感器平台坐标系到导航坐标系的变换矩阵；fb表示比力； < 是在传感器 坐标系下由陀螺仪测量得到的角速度； < 和 < 表示地球自转在导航坐标系下的角速度和 导航坐标系相对于地固系下的角速度；么x)是的反对称矩阵；gn表示当地重力向量； M和N分别表示子午圈和夕P酉圈曲率半径；；bJPb8分别表示加速度计和陀螺仪漂移量。
[0065] 误差状态方程如下：
[0066] 8x= [Sr11, 8vn,en, 8ba, 8bg] (6)
[0073] 其中，SX表示状态误差向量，6，表示位置误差，Svn表示速度误差，en表示姿 态误差，8匕表示加速度漂移误差，6、表示陀螺仪漂移误差，SXk、SXk+1分别表示第k、 (k+1)历元时的状态误差向量；u表示噪声向量，F是动态矩阵，G是设计矩阵，〇k表示状态 转移矩阵，wk表示驱动白噪声，服从wk~N(0,Qk)高斯分布；Qk表示协方差矩阵，Q表示谱 密度矩阵；Sg、S3分别表示加速度和陀螺仪的误差标准方差。At为相邻历元时间间隔。
[0074] 系统观测方程如下：
[0075] zk=Hk 8xk+vk (13)
[0079] 其中，匕u是IMU机械编排预测位置；表示LiDAR匹配定位观测位置； 和分别表示欧拉角表示的预测姿态和观测姿态；Hk是观测矩阵，vk为观测量驱动白噪 声，服从vk~N(0,Rk)高斯分布，Rk表不观测协方差矩阵。
[0080] 误差反馈方程如下：
[0084] 其中，/T、（、cr分别表示滤波前的位置、速度和姿态；（0)是误差角度< 的 反对称矩阵。
[0085] 步骤4,栅格地图的更新。
[0086] 根据当前历元传感器平台的位置和姿态，计算出当前历元LiDAR扫描帧中各个点 在栅格地图中具体位置。在栅格地图中，逐分辨率图层写入LiDAR点云的位置和相似度信 息，从而达到建立或更新测图信息的目的。
[0087] 相似度值表示栅格点为地物点的概率值，其值为0~1，为栅格点存储的信息，栅 格点存储的相似度值采用高斯随机模型预设。如图2所示，具体实施中，依据高斯随机模 型，将栅格点相似度值，依据距离中心点的远近预设为〇.l、〇. 3、0. 6、0. 9四级的范例。
[0088] 步骤5,剔除栅格地图中动态物体。
[0089] 数据采集中，不可避免会出现环境中因为行人经过等产生的动态点云。这些点云 虽然在匹配定位过程中有一定帮助，但作为制图而言，这些动态物体不是需要成图的环境 特征，应从栅格地图中剔除。具体实施中采用栅格加权投票方法来剔除动态物体，见图5所 示，具体为：为各栅格点设置计数量N，每当有点云点扫到该位置时，N加1。定位和制图过 程完成后，检查各栅格点的计数量N。若N值小于预设阈值F，则认为是动态物体。具体实 施时，阈值F可根据数据采集传感器平台的移动速度和观察制图效果设置，若为人工推扫， 阈值F可取2或3。
[0090] 步骤6,生成并输出室内地图。
[0091] 在栅格地图中，将最大分辨率图层做地图输出，其他分辨率图层用作辅助匹配定 位，导出本地坐标系下的室内平面地图原型。
[0092] 本发明核心在于采集数据的后处理，主要包括多分辨率栅格室内平面图表达，基 于最大相似度的LiDAR点云特征匹配算法，MU融合定位算法及动态物体剔除算法几大部 分组成。
[0093] 主要数据流程包括：
[0094](1)预设置栅格地图大小和最大分辨率，如1平方公里范围和最大栅格分辨率为1 厘米，系统通过栅格金字塔算法，自动分层，以便于在后期点云匹配中快速调度和匹配；
[0095] (2)逐历元LiDAR点云扫描帧与多层栅格地图进行相似度匹配，进行特征匹配定 位；
[0096] (3)IMU自身导航的机械编排定位；
[0097] (4)扩展卡尔曼滤波（EKF)对LiDAR匹配定位结果和MU定位结果进行数据融合， 得到传感器中心当前历元的最佳的位置和姿态；
[0098] (5)以该最佳定位位置和姿态，及LiDAR测距信息为基础，计算LiDAR扫描帧中各 个点在相对地图中位置，在该点写入相似度预设信息，更新栅格地图；
[0099] (6)在数据采集过程中，碰到的行人及车辆等动态物体，不应作为室内地图环境特 征，需要被剔除。本发明使用一种栅格加权投票的算法，可快速滤除大部分简单动态信息；
[0100] (7)导出最大相似度表达的制图结果。
[0101] 本发明可采用计算机软件技术自动运行，包括数据采集记录软件和后处理定位制 图软件。下面将结合应用实例，进一步说明本发明。
[0102] 以地下停车场为例，传感器平台为消费级IMU和商用LiDAR。首先，在停车场中任 意起点开始，启动数据采集平台，静置3-5分钟。开始沿任意路径推动平台移动，数据采集 记录软件则记录LiDAR和MU的测量数据，并生成数据记录文件，如图6所示。
[0103] 数据采集结束后，将数据记录文件导入后处理定位制图软件，如图7所示。根据停 车场范围，设置栅格地图范围约为1平方公里，栅格分辨率为1厘米。设置匹配搜索距离为 0.5米，搜索角度为5度，搜索角度间隔0.25度。然后开始定位和同步制图。直到数据处理 结束。
[0104] 在数据处理过程中，数据处理系统自动30秒执行一次，动态信息清除。无需用户 操作。制图结果见图8。
[0105] 以上所述为本发明的最佳实施例，并非对本发明作任何形式上的限制，凡是依据 本发明的技术实质对以上实例所作的任何简单修改、等同变化与修饰，均仍落入本发明的 保护范围内。
【主权项】
1. 基于激光测距的室内平面地图制图方法，其特征是，包括步骤： 步骤1，移动传感器平台采集室内环境数据，所述的传感器平台包括水平放置的惯性测 量单元和LiDAR传感器，所述的室内环境数据包括导航数据和室内环境点云扫描帧，导航 数据利用惯性测量单元获得，室内环境点云扫描帧利用LiDAR传感器获得； 步骤2,根据设定的室内地图的范围和最大分辨率，创建多分辨率的栅格地图； 步骤3,结合室内环境数据和栅格地图，获得各历元下传感器平台最佳的位置和姿态， 本步骤进一步包括： 3. 1遍历栅格地图中栅格点位置，以预设角度分辨率遍历传感器平台姿态角度，找出与 当前历元扫描帧相似度最大的栅格点的位置和姿态，该位置和姿态即当前历元传感器平台 的基于室内环境点云匹配定位的位置和姿态； 3. 2IMU自身利用导航数据的机械编排定位，获得当前历元传感器平台基于导航数据的 位置和姿态； 3. 3融合基于室内环境点云匹配定位和基于导航数据的位置和姿态，获得传感器平台 最佳的位置和姿态，同时将补偿误差估计值反馈给MU机械编排定位； 步骤4,根据最佳的位置和姿态，计算当前历元扫描帧中各点在栅格地图中的栅格点位 置，将预设的多级相似度值逐分辨率图层写入对应栅格点； 步骤5,将最大分辨率图层做地图输出，其他分辨率图层用作辅助匹配定位，在本地坐 标系下到导出栅格地图。2. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 步骤2中采用金字塔四叉树法创建多分辨率的栅格地图。3. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 子步骤3. 1中遍历栅格地图中栅格点位置和以预设角度分辨率遍历传感器平台姿态 时，根据传感器平台的移动速度和采样间隔设置最大搜索范围和搜索角度，根据实际情况 和LiDAR传感器角度分辨率参数设置搜索角度；最大搜索范围和搜索角度的设定要保证下 一历元传感器平台的基于室内环境点云匹配定位的位置和姿态落在最大搜索范围内。4. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 子步骤3. 1中当前历元扫描帧与栅格点的相似度其中，i为 扫描帧中当前点编号，η为扫描帧中点总数，P(SiIM)表示点i在栅格地图的预设相似度值。5. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 子步骤3. 3中采用扩展卡尔曼滤波法融合基于室内环境点云匹配定位和基于导航数 据的位置和姿态。6. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 步骤4中所述的多级相似度值通过高斯随机模型获得。7. 如权利要求6所述的基于激光测距的室内平面地图制图方法，其特征是： 所述的多级相似度值可预设为〇. l、〇. 3、0. 6、0. 9四级。8. 如权利要求1所述的基于激光测距的室内平面地图制图方法，其特征是： 步骤5前还包括剔除栅格地图中动态物体，具体为： 为各栅格点设置计数量N，每当有点云点扫到该栅格点，N加1 ;导出栅格地图前，检查 栅格点计数量N，若计数量值小于预设阈值F，则剔除该栅格点；预设阈值F根据试验验证获 得。
【专利摘要】本发明公开了一种基于激光测距的室内平面地图制图方法，包括步骤：步骤1，移动传感器平台采集室内环境数据；步骤2，创建多分辨率的栅格相似度地图；步骤3，结合室内环境数据和栅格相似度地图，获得各历元下传感器平台最佳的位置和姿态；步骤4，根据最佳的位置和姿态更新栅格相似度地图；步骤5，在本地坐标系下到导出栅格相似度地图。本发明数据采集操作简单，且无需预先了解环境空间结构；成本低廉，成图速度快，可实现室内地图的多次和快速更新，有效解决室内制图的效率瓶颈问题。
【IPC分类】G01C21/20, G01C3/00, G01C21/16
【公开号】CN104897161
【申请号】CN201510295778
【发明人】唐健, 陈育伟
【申请人】武汉大学
【公开日】2015年9月9日
【申请日】2015年6月2日
最新回复(0)