激光测距移动制图中闭合环误差纠正方法技术

本发明专利技术公开了一种激光测距移动制图中闭合环误差纠正方法，主要包括步骤：S1移动传感器平台采集时间同步的导航数据和点云扫描帧；S2导航数据和点云扫描帧的正向组合定位；S3闭合环检测和点云扫描帧的初始匹配纠正；S4导航数据和点云扫描帧的逆向组合定位；S5将正向组合定位结果和逆向组合定位结果进行加权求和，得误差纠正后的定位结果；S6采用误差纠正后的定位结果更新栅格地图。本发明专利技术方法在无其它外部参考信息条件下，通过多次闭合环误差纠正，可有效消除传统SLAM特征匹配制图产生的累积误差，从而进一步提高大范围激光测距移动制图的精度和可靠性。

【技术实现步骤摘要】

本专利技术属于移动测量制图
，尤其涉及一种激光测距移动制图中闭合环误差纠正方法。
技术介绍
基于激光测距同步定位与制图(Simultaneously Localization And Mapping，SLAM)的移动制图(Mobile Mapping，MM)技术在室内等GNSS信号无法定位的区域提供了一种新的导航与快速制图方法，得到了学术界和工业界的广泛关注。然而在基于SLAM特征匹配方法的移动制图方法从本质上来说是一种航位推算方法，随着时间的推移，定位和制图累积误差逐渐增大，若无外部信息校正，会造成定位和制图的误差，甚至错误。因此，非常有必要针对激光测距移动制图中累积误差的问题，提出一种行之有效的解决方案，从而提高制图的精度和可靠性。申请号为CN 201510295778、名称为《基于激光测距的室内平面地图制图方法》的中国专利提出了一种基于LiDAR和IMU组合的室内移动制图方法。本专利技术在此基础上提出了一种针对累积误差的闭合环纠正方法。公告号为CN 103337221 A、名称为《一种室内地图制作方法》的中国专利，其公开了一种基于GIS要素分类的室内地图的设计和表达，及数据管理方法，然而并未针对LiDAR测距点云数据的处理。申请号为CN201510609208.8、名称为《一种基于扫地机器人定位视频采集和制图的系统及方法》的中国专利，其提出了一种基于视频方式移动制图的方法，未涉及本专利技术提出的LiDAR数据处理和误差纠正方法。
技术实现思路
本专利技术的目的是提供一种激光测距移动制图中闭合环误差纠正方法，从而进一步提高大范围激光测距移动制图的精度和可靠性。为达到上述目的，本专利技术采用如下技术方案：一种激光测距移动制图中闭合环误差纠正方法，包括步骤：S1移动传感器平台按闭合环路径扫描室内，采集时间同步的导航数据和点云扫描帧，移动传感器平台包括惯性测量单元和LiDAR传感器；并创建栅格地图；对各闭合环扫描过程采集的导航数据和点云扫描帧执行S2～7：S2从k-n历元开始，对导航数据和点云扫描帧进行组合定位，得当前闭合环扫描过程的正向组合定位结果；采用正向组合定位结果更新当前栅格地图；S3人工判断k-n历元和k历元的点云扫描帧是否重叠，若无重叠，执行S4；否则，对下一闭合环扫描过程采集的导航数据和点云扫描帧重新执行S2～7；S4在当前栅格地图中保留k-n历元前的点云扫描帧，清除k-n历元到k历元间的点云扫描帧；通过人工观察采集的点云扫描帧与当前栅格地图的差异，确定移动传感器平台历元k的大致位置，以大致位置为起点，通过点云匹配搜索，确定历元k时移动传感器平台的实际位置，搜素范围即历元k时移动传感器平台大致位置的附近区域，该范围人为确定；S5将S4所确定实际位置作为历元k时移动传感器平台的初始位置，对导航数据和点云扫描帧进行逆向组合定位，本步骤进一步包括：5.1从k历元开始，将点云扫描帧和栅格地图进行逆向匹配定位，得当前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果；5.2从k历元开始，IMU自身利用逆向机械编排定位，得当前闭合环扫描过程中移动传感器平台基于导航数据的定位结果；5.3从k历元开始，基于模型采用基于扩展卡尔曼滤波的定位融合方法融合子步骤5.1和5.2的定位结果，得逆向组合定位结果，同时将补偿误差估计值反馈给IMU逆向机械编排定位；δxi-1、δxi分别表示历元i-1、i的状态误差向量，表示历元i状态转移矩阵Φi的逆矩阵，wi表示历元i的驱动白噪声；S6分别将(1-i/n)和i/n作为正向组合定位结果和逆向组合定位结果的权值，正向组合定位结果和逆向组合定位结果的加权求和即误差纠正后的定位结果，i表示当前闭合环扫描过程中历元；S7采用误差纠正后的定位结果更新当前栅格地图；上述k-n历元和k历元分别为当前闭合环扫描开始历元和结束历元。步骤2进一步包括：2.1从k-n历元开始，将点云扫描帧和栅格地图进行匹配定位，得当前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果；2.2从k-n历元开始，IMU自身利用机械编排定位，得当前闭合环扫描过程中移动传感器平台基于导航数据的定位结果；2.3从k-n历元开始，采用基于扩展卡尔曼滤波的定位融合方法融合子步骤2.1和2.2的定位结果，得正向组合定位结果，同时将补偿误差估计值反馈给IMU机械编排定位。与现有技术相比，本专利技术具有以下优点：1、断点接续处理，处理效率高。数据处理过程中保留数据纠正前后历史状态信息，数据纠正后可从纠正点开始继续后续制图数据处理，无需再从起始点重新开始，提高了地图数据处理的效率。2、实用性高。在无其它外部参考信息条件下，通过多次闭合环检测纠正，可有效消除传统SLAM特征匹配制图产生的累积误差，提高了该技术在大范围快速移动制图应用中的可用性。附图说明图1是本专利技术具体流程示意图；图2是本专利技术逆向组合定位流程示意图；图3是实施例中数据采集闭合环路径示意图；图4是实施例中闭合环误差检测和初始纠正范例；图5是实施例中制图结果对比范例。具体实施方式下面将结合附图和具体实施例进一步说明本专利技术技术方案。本专利技术具体步骤见图1，包括：步骤1，移动传感器平台采集时间同步的导航数据和点云扫描帧。移动传感器平台包括惯性测量单元(IMU)和LiDAR传感器，分别用来采集导航数据和点云扫描帧。移动传感器平台置于手推车或移动机器人等运动载台，沿水平方向推扫采集室内环境数据。IMU采样频率为100～200Hz，LiDAR传感器采样频率为30～40Hz。为利用闭合环误差纠正提高大范围测图时的定位和制图精度，采集数据时，移动传感器平台按闭合环路径逐层向外扩展移动，并扫描。各闭合环扫描结束历元时进行闭合环误差局部纠正，从而保证全局精度。步骤2，创建栅格地图，用以存储点云扫描帧。在各闭合环扫描结束时，对该闭合环(后文记为“当前闭合环”)扫描过程所采集的导航数据和点云扫描帧执行步骤3～9。步骤3，从当前闭合环扫描开始历元开始，根据导航数据和点云扫描帧进行正向组合定位，得正向组合定位结果，即闭合环扫描过程中移动传感器平台正向组合定位的位置和姿态。所述的正向组合定位即，从闭合环扫描开始历元到结束历元，采用基于栅格地图的最大相似度LiDAR点云特征匹配法及IMU融合定位法处理室内环境数据，以获得移动传感器平台的位置和姿态。本步骤进一步包括：3.1从当前闭合环扫描开始历元开始，根据点云扫描帧和栅格地图的匹配定位，获得当前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果。遍历栅格地图中栅格点位置，并以预设角度分辨率遍历移动传感器平台姿态，找出与点云扫描帧相似度最大的栅格点的位置和姿态，该位置和姿态即移动传感器平台的位置和姿态3.2从当前闭合环扫描开始历元开始，IMU自身利用机械编排定位，获得当前闭合环扫描过程中移动传感器平台基于导航数据的定位结果。3.3融合子步骤3.1和3.2的定位结果，获得正向组合定位结果，同时将补偿误差估计值反馈给IMU机械编排定位。在环境特征信息不足情况下，点云扫描帧匹配会造成不可修正的匹配定位误差累积。可利用导航数据的短时高精度位移和姿态推算对点云扫描帧进行校正，以提高定位和制图精度。一般情况下，从闭合环扫描开始历元开始，使用基于扩展卡尔曼滤波(EK本文档来自技高网...

【技术保护点】
一种激光测距移动制图中闭合环误差纠正方法，其特征是，包括步骤：S1移动传感器平台按闭合环路径扫描室内，采集时间同步的导航数据和点云扫描帧，移动传感器平台包括惯性测量单元和LiDAR传感器；并创建栅格地图；对各闭合环扫描过程采集的导航数据和点云扫描帧执行S2～7：S2从k‑n历元开始，对导航数据和点云扫描帧进行组合定位，得当前闭合环扫描过程的正向组合定位结果；采用正向组合定位结果更新当前栅格地图；S3人工判断k‑n历元和k历元的点云扫描帧是否重叠，若无重叠，执行S4；否则，对下一闭合环扫描过程采集的导航数据和点云扫描帧重新执行S2～7；S4在当前栅格地图中保留k‑n历元前的点云扫描帧，清除k‑n历元到k历元间的点云扫描帧；通过人工观察采集的点云扫描帧与当前栅格地图的差异，确定移动传感器平台历元k的大致位置，以大致位置为起点，通过点云匹配搜索，确定历元k时移动传感器平台的实际位置，搜素范围即历元k时移动传感器平台大致位置的附近区域，该范围人为确定；S5将S4所确定实际位置作为历元k时移动传感器平台的初始位置，对导航数据和点云扫描帧进行逆向组合定位，本步骤进一步包括：5.1从k历元开始，将点云扫描帧和栅格地图进行逆向匹配定位，得当前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果；5.2从k历元开始，IMU自身利用逆向机械编排定位，得当前闭合环扫描过程中移动传感器平台基于导航数据的定位结果；5.3从k历元开始，基于模型采用基于扩展卡尔曼滤波的定位融合方法融合子步骤5.1和5.2的定位结果，得逆向组合定位结果，同时将补偿误差估计值反馈给IMU逆向机械编排定位；δxi‑1、δxi分别表示历元i‑1、i的状态误差向量，表示历元i状态转移矩阵Φi的逆矩阵，wi表示历元i的驱动白噪声；S6分别将(1‑i/n)和i/n作为正向组合定位结果和逆向组合定位结果的权值，正向组合定位结果和逆向组合定位结果的加权求和即误差纠正后的定位结果，i表示当前闭合环扫描过程中历元；S7采用误差纠正后的定位结果更新当前栅格地图；上述k‑n历元和k历元分别为当前闭合环扫描开始历元和结束历元。...

【技术特征摘要】
1.一种激光测距移动制图中闭合环误差纠正方法，其特征是，包括步骤：S1移动传感器平台按闭合环路径扫描室内，采集时间同步的导航数据和点云扫描帧，移动传感器平台包括惯性测量单元和LiDAR传感器；并创建栅格地图；对各闭合环扫描过程采集的导航数据和点云扫描帧执行S2～7：S2从k-n历元开始，对导航数据和点云扫描帧进行组合定位，得当前闭合环扫描过程的正向组合定位结果；采用正向组合定位结果更新当前栅格地图；S3人工判断k-n历元和k历元的点云扫描帧是否重叠，若无重叠，执行S4；否则，对下一闭合环扫描过程采集的导航数据和点云扫描帧重新执行S2～7；S4在当前栅格地图中保留k-n历元前的点云扫描帧，清除k-n历元到k历元间的点云扫描帧；通过人工观察采集的点云扫描帧与当前栅格地图的差异，确定移动传感器平台历元k的大致位置，以大致位置为起点，通过点云匹配搜索，确定历元k时移动传感器平台的实际位置，搜素范围即历元k时移动传感器平台大致位置的附近区域，该范围人为确定；S5将S4所确定实际位置作为历元k时移动传感器平台的初始位置，对导航数据和点云扫描帧进行逆向组合定位，本步骤进一步包括：5.1从k历元开始，将点云扫描帧和栅格地图进行逆向匹配定位，得当前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果；5.2从k历元开始，IMU自身利用逆...

【专利技术属性】
技术研发人员：唐健，牛小骥，施闯，
申请(专利权)人：武汉大学，
类型：发明
国别省市：湖北;42