激光测距移动制图中闭合环误差纠正方法
【专利摘要】本发明公开了一种激光测距移动制图中闭合环误差纠正方法，主要包括步骤：S1移动传感器平台采集时间同步的导航数据和点云扫描帧；S2导航数据和点云扫描帧的正向组合定位；S3闭合环检测和点云扫描帧的初始匹配纠正；S4导航数据和点云扫描帧的逆向组合定位；S5将正向组合定位结果和逆向组合定位结果进行加权求和，得误差纠正后的定位结果；S6采用误差纠正后的定位结果更新栅格地图。本发明方法在无其它外部参考信息条件下，通过多次闭合环误差纠正，可有效消除传统SLAM特征匹配制图产生的累积误差，从而进一步提高大范围激光测距移动制图的精度和可靠性。
【专利说明】
激光测距移动制图中闭合环误差纠正方法
技术领域
[0001] 本发明属于移动测量制图技术领域，尤其涉及一种激光测距移动制图中闭合环误 差纠正方法。
【背景技术】
[0002] 基于激光测距同步定位与制图（Simultaneously Localization And Mapping， SLAM)的移动制图（Mobile Mapping，MM)技术在室内等GNSS信号无法定位的区域提供了一 种新的导航与快速制图方法，得到了学术界和工业界的广泛关注。然而在基于SLAM特征匹 配方法的移动制图方法从本质上来说是一种航位推算方法，随着时间的推移，定位和制图 累积误差逐渐增大，若无外部信息校正，会造成定位和制图的误差，甚至错误。
[0003] 因此，非常有必要针对激光测距移动制图中累积误差的问题，提出一种行之有效 的解决方案，从而提尚制图的精度和可靠性。
[0004] 申请号为CN 201510295778、名称为《基于激光测距的室内平面地图制图方法》的 中国专利提出了一种基于LiDAR和頂U组合的室内移动制图方法。本发明在此基础上提出了 一种针对累积误差的闭合环纠正方法。
[0005] 公告号为CN 103337221 A、名称为《一种室内地图制作方法》的中国专利，其公开 了一种基于GIS要素分类的室内地图的设计和表达，及数据管理方法，然而并未针对LiDAR 测距点云数据的处理。申请号为CN201510609208.8、名称为《一种基于扫地机器人定位视频 采集和制图的系统及方法》的中国专利，其提出了一种基于视频方式移动制图的方法，未涉 及本发明提出的LiDAR数据处理和误差纠正方法。
【发明内容】
[0006] 本发明的目的是提供一种激光测距移动制图中闭合环误差纠正方法，从而进一步 提尚大范围激光测距移动制图的精度和可靠性。
[0007] 为达到上述目的，本发明采用如下技术方案：
[0008] -种激光测距移动制图中闭合环误差纠正方法，包括步骤：
[0009] S1移动传感器平台按闭合环路径扫描室内，采集时间同步的导航数据和点云扫描 帧，移动传感器平台包括惯性测量单元和LiDAR传感器;并创建栅格地图；
[0010] 对各闭合环扫描过程米集的导航数据和点云扫描帧执行S2~7:
[0011] S2从k-n历元开始，对导航数据和点云扫描帧进行组合定位，得当前闭合环扫描过 程的正向组合定位结果;采用正向组合定位结果更新当前栅格地图；
[0012] S3人工判断k-n历元和k历元的点云扫描帧是否重叠，若无重叠，执行S4;否则，对 下一闭合环扫描过程采集的导航数据和点云扫描帧重新执行S2~7;
[0013] S4在当前栅格地图中保留k-n历元前的点云扫描帧，清除k-n历元到k历元间的点 云扫描帧;通过人工观察采集的点云扫描帧与当前栅格地图的差异，确定移动传感器平台 历元k的大致位置，以大致位置为起点，通过点云匹配搜索，确定历元k时移动传感器平台的 实际位置，搜素范围即历元k时移动传感器平台大致位置的附近区域，该范围人为确定；
[0014] S5将S4所确定实际位置作为历元k时移动传感器平台的初始位置，对导航数据和 点云扫描帧进行逆向组合定位，本步骤进一步包括：
[0015] 5.1从k历元开始，将点云扫描帧和栅格地图进行逆向匹配定位，得当前闭合环扫 描过程中移动传感器平台基于匹配定位的定位结果；
[0016] 5.2从k历元开始，頂U自身利用逆向机械编排定位，得当前闭合环扫描过程中移动 传感器平台基于导航数据的定位结果；
[0017] 5.3从k历元开始，基于模型J.v, , = W.v,.十采用基于扩展卡尔曼滤波的定位融 合方法融合子步骤5.1和5.2的定位结果，得逆向组合定位结果，同时将补偿误差估计值反 馈给IMU逆向机械编排定位;5Xi-i、5Xi分别表示历元i-1、i的状态误差向量，表示历元i 状态转移矩阵? i的逆矩阵，^表示历元i的驱动白噪声；
[0018] S6分别将（1-i/n)和i/n作为正向组合定位结果和逆向组合定位结果的权值，正向 组合定位结果和逆向组合定位结果的加权求和即误差纠正后的定位结果，i表示当前闭合 环扫描过程中历元；
[0019] S7采用误差纠正后的定位结果更新当前栅格地图；
[0020]上述k-n历元和k历元分别为当前闭合环扫描开始历元和结束历元。
[0021] 步骤2进一步包括：
[0022] 2.1从k-n历元开始，将点云扫描帧和栅格地图进行匹配定位，得当前闭合环扫描 过程中移动传感器平台基于匹配定位的定位结果；
[0023] 2.2从k-n历元开始，頂U自身利用机械编排定位，得当前闭合环扫描过程中移动传 感器平台基于导航数据的定位结果；
[0024] 2.3从k-n历元开始，采用基于扩展卡尔曼滤波的定位融合方法融合子步骤2.1和 2.2的定位结果，得正向组合定位结果，同时将补偿误差估计值反馈给IMU机械编排定位。
[0025] 与现有技术相比，本发明具有以下优点：
[0026] 1、断点接续处理，处理效率高。
[0027]数据处理过程中保留数据纠正前后历史状态信息，数据纠正后可从纠正点开始继 续后续制图数据处理，无需再从起始点重新开始，提高了地图数据处理的效率。
[0028] 2、实用性高。
[0029]在无其它外部参考信息条件下，通过多次闭合环检测纠正，可有效消除传统SLAM 特征匹配制图产生的累积误差，提高了该技术在大范围快速移动制图应用中的可用性。
【附图说明】
[0030]图1是本发明具体流程不意图；
[0031 ]图2是本发明逆向组合定位流程示意图；
[0032] 图3是实施例中数据采集闭合环路径示意图；
[0033] 图4是实施例中闭合环误差检测和初始纠正范例；
[0034] 图5是实施例中制图结果对比范例。
【具体实施方式】
[0035]下面将结合附图和具体实施例进一步说明本发明技术方案。
[0036]本发明具体步骤见图1，包括：
[0037]步骤1，移动传感器平台采集时间同步的导航数据和点云扫描帧。
[0038] 移动传感器平台包括惯性测量单元(MU)和LiDAR传感器，分别用来采集导航数据 和点云扫描帧。移动传感器平台置于手推车或移动机器人等运动载台，沿水平方向推扫采 集室内环境数据。頂U采样频率为100~200Hz，LiDAR传感器采样频率为30~40Hz。
[0039] 为利用闭合环误差纠正提高大范围测图时的定位和制图精度，采集数据时，移动 传感器平台按闭合环路径逐层向外扩展移动，并扫描。各闭合环扫描结束历元时进行闭合 环误差局部纠正，从而保证全局精度。
[0040] 步骤2,创建栅格地图，用以存储点云扫描帧。
[0041] 在各闭合环扫描结束时，对该闭合环(后文记为"当前闭合环"）扫描过程所采集的 导航数据和点云扫描帧执行步骤3~9。
[0042]步骤3,从当前闭合环扫描开始历元开始，根据导航数据和点云扫描帧进行正向组 合定位，得正向组合定位结果，即闭合环扫描过程中移动传感器平台正向组合定位的位置 和姿态。
[0043] 所述的正向组合定位即，从闭合环扫描开始历元到结束历元，采用基于栅格地图 的最大相似度LiDAR点云特征匹配法及頂U融合定位法处理室内环境数据，以获得移动传感 器平台的位置和姿态。
[0044] 本步骤进一步包括：
[0045] 3.1从当前闭合环扫描开始历元开始，根据点云扫描帧和栅格地图的匹配定位，获 得当前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果。
[0046] 遍历栅格地图中栅格点位置，并以预设角度分辨率遍历移动传感器平台姿态，找 出与点云扫描帧相似度最大的栅格点的位置和姿态，该位置和姿态即移动传感器平台的位 置<_和姿态_。
[0047] 3.2从当前闭合环扫描开始历元开始，IMU自身利用机械编排定位，获得当前闭合 环扫描过程中移动传感器平台基于导航数据的定位结果。
[0048] 3.3融合子步骤3.1和3.2的定位结果，获得正向组合定位结果，同时将补偿误差估 计值反馈给IMU机械编排定位。
[0049]在环境特征信息不足情况下，点云扫描帧匹配会造成不可修正的匹配定位误差累 积。可利用导航数据的短时高精度位移和姿态推算对点云扫描帧进行校正，以提高定位和 制图精度。
[0050] -般情况下，从闭合环扫描开始历元开始，使用基于扩展卡尔曼滤波(EKF)的定位 融合方法将MU正向机械编排和LiDAR点云的正向最大相似度匹配方法做数据融合，获得正 向数据处理定位和制图结果。
[0051 ] 頂U采样频率高，LiDAR传感器采样频率低。融合定位过程中，无点云扫描帧时刻， MU自身利用机械编排定位，即利用IMU提供的加速度计和陀螺仪进行导航定位，获得移动 传感器平台基于导航数据的定位结果(',^ /;,,<_)，/^、',、￡^/[,.分别表示頂1]定 位的位置、速度和姿态。
[0052]有点云扫描帧时刻，通过子步骤3.1获得移动传感器平台的基于匹配定位的位置 和姿态通过基于扩展卡尔曼滤波的定位融合法获得融合后移动传感器平台的 位置以《、速度 和姿态，即正向组合定位结果，其中，i表示闭合环扫描过程中任意 历元;采用正向组合定位结果更新栅格地图。同时，将补偿误差估计值心=[8，，8，，￡"]反 馈给頂U机械编排过程，提高后续推导定位和姿态估计精度。实际操作过程中，可根据所用 传感器本身特性设置合适的EKF滤波误差参数。
[0053]其中相关主要计算公式描述如下：
[0054]惯性系统状态方程如下：
[0060]其中，表示移动传感器平台在导航坐标系下的位置估计值，免是炜 度，A是经度，h为高程;vn表示移动传感器平台移动速度;是移动传感器平台坐标系到导 航坐标系的变换矩阵，Cf则是移动传感器平台坐标系到导航坐标系的变换矩阵;f b表示比 力；以是在传感器坐标系下由陀螺仪测量得到的角速度; < 和< 表示地球自转在导航坐 标系下的角速度和导航坐标系相对于地固系下的角速度；(《><)是6么的反对称矩阵;8"表 示当地重力向量;M和N分别表示子午圈和夕卩酉圈曲率半径;b4Pb g分别表示加速度计和陀螺 仪漂移量。
[0061 ] 误差状态方程如下：
[0062] 8x= [8rn, 8vn, en] (6)
[0063] it=[?] (7)
[0064] Sx -- FSx -I- Gu (' 8 )
[0065] 8xi+i= 〇 i8xi+Wi (9)
[0066] 〇k = exp(F A t)^I+F A t (10)
[0067] Q^diag(S：,S^) (.11.)
[0068] 〇k ^<\y,GQG' <\ykM (12)
[0069] 其中，SX表示状态误差向量，Srn表示位置误差，S vn表示速度误差，en表示姿态误 差，5xi、5xi+i分别表示i、（i + 1)历元时的状态误差向量;u表示噪声向量，5fb表示加速度噪 声，表示陀螺仪误差，F是动态矩阵，G是设计矩阵，〇 i表示状态转移矩阵，Wl表示驱动白 噪声，服从wi~N(0，Qi)高斯分布;Qi表不协方差矩阵，Q表不谱密度矩阵;5 g、5a分别表不加速 度和陀螺仪的误差标准方差。A t为相邻历元时间间隔。
[0070] 系统观测方程如下：
[0071] zi = Hi8xi+Vi (13)
[0075]其中，21表示历元i的观测量误差;'/是MU机械编排预测位置;心通表示LiDAR匹 配定位观测位置；4&和分别表示欧拉角表示的预测姿态和观测姿态是观测矩阵， Vl为观测量驱动白噪声，服从高斯分布，心表示观测协方差矩阵；之和之为 LiDAR观测误差方差量。
[0076]误差反馈方程如下：
[0077] r；=r；--S4 (17)
[0078] v'l = v'^ -Sv" (18)
[0079] q -(/ + ?x))C； (19)
[0080] 其中，CT分别表示滤波前的位置、速度和姿态；(<x)是误差角度矽的反 对称矩阵。
[0081 ] 本步骤为已有技术，其具体实施可参见申请号为CN 201510295778、名称为《基于 激光测距的室内平面地图制图方法》的中国专利。
[0082]步骤4,采用正向组合定位结果更新栅格地图。
[0083] 步骤5,闭合环误差检测。
[0084]将当前闭合环扫描开始历元和结束历元分别记为k-n历元、k历元。通过人工判读 方式判断k_n历元和k历元的点云扫描帧是否重叠，若无重叠，则认为运动过程中累积误差 产生了闭合环误差，启动闭合环误差纠正，即执行步骤6;否则，当前闭合环无需进行闭合环 误差纠正，当前闭合环扫描过程的数据处理结束，对下一闭合环扫描过程所采集的导航数 据和点云扫描帧重新执行步骤3~9。
[0085]步骤6,点云扫描帧初始匹配纠正。
[0086 ]当前栅格地图中保留k-n历元前的点云扫描帧，清除k-n历元到k历元间的点云扫 描帧，所保留的点云扫描帧记为保留帧。设定保留帧的目的在于，认为数据处理前若干帧， 尚未存在较大的累积误差，即认为保留帧是正确帧。本发明中，采用k历元的点云扫描帧和 保留帧的特征匹配法进行点云扫描帧初始匹配纠正。
[0087]进行点云扫描帧初始匹配纠正，即确定移动传感器平台在当前闭合环扫描结束历 元k的正确位置，以消除平台运动过程中的累积误差，理论上，闭合环初始匹配搜索是地图 全局匹配搜索。本发明中，可通过人工观察采集的点云扫描帧与当前栅格地图的差异，确定 移动传感器平台在历元k的大致位置，以大致位置为起点，通过点云匹配搜索，确定历元k时 移动传感器平台的实际位置，搜素范围即历元k时移动传感器平台大致位置的附近区域，该 范围人为确定。
[0088]本发明中，将点云扫描帧初始匹配纠正所得实际位置作为移动传感器平台在k历 元的初始位置，通过导航数据和点云扫描帧的逆向组合定位，传递至当前闭合环扫描过程 各历元的定位结果。
[0089] 步骤7,从当前闭合环扫描结束历元开始，根据导航数据和点云扫描帧进行逆向组 合定位，得逆向组合定位结果，即当前闭合环扫描过程中移动传感器平台逆向组合定位的 位置和姿态。
[0090] 见图2,导航数据和点云扫描帧的逆向组合定位过程与步骤3过程类似。
[0091] 7.1从闭合环扫描结束历元开始，根据点云扫描帧和栅格地图的匹配定位，获得当 前闭合环扫描过程中移动传感器平台基于匹配定位的定位结果。
[0092] 7.2从闭合环扫描结束历元开始，IMU自身利用机械编排定位，得当前闭合环扫描 过程中移动传感器平台基于导航数据的定位结果(r'LmV:，：）。
[0093] 7.3从闭合环扫描结束历元开始，采用基于扩展卡尔曼滤波的定位融合方法融合 子步骤7.1和7.2的定位结果，得当前闭合环扫描过程中移动传感器平台的逆向组合定位结 果，同时将补偿误差估计值反馈给MU机械编排定位。
[0094] 頂U采样频率高，LiDAR传感器采样频率低。无点云扫描帧历元，頂U以k历元到k-n 历元开始进行机械编排定位，得移动传感器平台基于导航数据的定位结果 (4?^;，"腿>、r:、vl、c'L分别表示MJ定位的位置、速度和姿态。
[0095]有点云扫描帧历元（例如k历元、k-5历元等），将基于导航数据的定位结果 (r ，_ CU与基于匹配定位的定位结果.LiDAR^C bliDAR. )，通过基于扩展卡尔曼滤波 的定位融合方法得到逆向组合定位结果(<;)，)。
[0096] 在逆向组合定位中，所采用公式与正向组合定位一致，只是将式(9)中状态转移矩 阵〇:取逆矩阵賞1，即历元i、（ i-1)的状态误差向量SXl、具有如下关系：
[0097] <)'.v , =?：!)_ + vi;. 120 )
[0098] 边:1表示历元i时状态转移矩阵〇:的逆矩阵。
[0099] 步骤8,正向组合定位结果和逆向组合定位结果求加权和。
[0100] 通过正向组合定位和反向组合定位，得到移动传感器平台的两组位置和姿态，对 两组位置和姿态进行加权平均，得移动传感器平台误差纠正后的位置r。⑴和姿态
[0101] rc(i) = rf(i)(l-i/n)+rr(i)(i/n) (21)
[0102] Clc{l)=Clf(l){\-iln) + ClHi]{iln) (22)
[0103] 式（21)~（22)中，k、k-n分别表示当前闭合环扫描结束历元和开始历元，i表示闭 合环扫描过程中历元，k-nSiSk。
[0104] 根据式（21)~（22)可知，闭合环扫描过程中越靠近k历元的时刻越相信反向组合 定位结果，越靠近k-n历元的时刻越相信正向组合定位结果。
[0105] 步骤9,采用误差纠正后的位置⑴和姿态邱更新当前栅格地图。
[0106] 一个闭合环误差纠正结束后，采用误差纠正后的定位结果当前更新栅格地图，并 将系统状态量更新至k时刻。并从k时刻开始下一轮的正向组合定位，这样即可保证数据处 理的完整性和系统可用性。
[0107] 重复步骤2~9,至所有闭合环扫描过程采集数据处理完毕，输出制图结果。
[0108] 本发明从闭合环扫描过程开始历元的导航数据和点云扫描帧开始执行步骤2~9， 若未检测到闭合环误差，则仅进行步骤3~4的正向组合定位;若检测到闭合环误差，则还需 要进一步执行步骤6~9的闭合环误差纠正过程。所有数据历元处理完毕后，则可导出本地 坐标系下的室内平面栅格地图。
[0109]本发明采用人工和计算机软件交互处理技术，下面将结合应用实例，进一步说明 本发明。
[0110] 以某购物商场楼层平面为例，面积约5000平方米，数据采集过程中，移动传感器平 台在楼层平面内中任意起点开始记录数据。并以环状方式逐块推动移动传感器平台采集数 据，如图3所示推扫轨迹示例。通过此方式，可在独立在每个环状区域闭合处进行闭合环检 测和纠正，消除区域内的航位推算累积误差。并可通过逐块的闭合环纠正传递，提高整个楼 层的内的定位及制图精度。
[0111] 数据采集结束后，将数据记录的IMU和LiDAR文件导入后处理定位制图软件，根据 数据处理范围，设置栅格地图范围约为1平方公里，栅格分辨率为1厘米;设置历元间LiDAR 匹配搜索距离为0.3米，搜索角度为3度，搜索角度间隔0.25度；设置闭合环初始纠正时， LiDAR匹配搜索距离为1米，搜索角度为30度，搜索角度间隔0.25度。从起始历元开始正向定 位和同步制图。在每个闭合环路径结束处，人工检查判读是否存在闭合环误差，见图4(a)所 示，正方形方框表示闭合环结束时的定位和姿态结果，与闭合环开始历元的结果未重合，产 生了闭合差。指定保留帧历元号，清除从当前历元到保留帧历元间的制图结果；启动闭合环 纠正LiDAR初始搜索匹配定位，图4(b)为闭合环纠正初始匹配定位的结果。从当前历元开始 逆向定位和同步制图，并与正向定位结果进行加权平均，更新定位结果，进而更新制图结 果。纠正结束后，从当前历元处继续下一个闭合环的正向定位和同步制图，重复上述步骤， 直至所有历元数据处理完毕。如图5所示，测试商场楼层平面图的经闭合环纠正（白色点云） 和未经过闭合环纠正(浅灰色点云）的对比结果。
[0112]以上所述为本发明的最佳实施例，并非对本发明作任何形式上的限制，凡是依据 发明技术实质对以上实例所作的任何简单修改、等同变化与修饰，均仍落入本发明的保护 范围内。
【主权项】
1. 一种激光测距移动制图中闭合环误差纠正方法，其特征是，包括步骤： S1移动传感器平台按闭合环路径扫描室内，采集时间同步的导航数据和点云扫描帧， 移动传感器平台包括惯性测量单元和LiDAR传感器;并创建栅格地图； 对各闭合环扫描过程采集的导航数据和点云扫描帧执行S2~7: S2从k-n历元开始，对导航数据和点云扫描帧进行组合定位，得当前闭合环扫描过程的 正向组合定位结果;采用正向组合定位结果更新当前栅格地图； S3人工判断k-n历元和k历元的点云扫描帧是否重叠，若无重叠，执行S4;否则，对下一 闭合环扫描过程采集的导航数据和点云扫描帧重新执行S2~7; S 4在当前栅格地图中保留k - n历元前的点云扫描帧，清除k - n历元到k历元间的点云扫 描帧;通过人工观察采集的点云扫描帧与当前栅格地图的差异，确定移动传感器平台历元k 的大致位置，以大致位置为起点，通过点云匹配搜索，确定历元k时移动传感器平台的实际 位置，搜素范围即历元k时移动传感器平台大致位置的附近区域，该范围人为确定； S5将S4所确定实际位置作为历元k时移动传感器平台的初始位置，对导航数据和点云 扫描帧进行逆向组合定位，本步骤进一步包括： 5.1从k历元开始，将点云扫描帧和栅格地图进行逆向匹配定位，得当前闭合环扫描过 程中移动传感器平台基于匹配定位的定位结果； 5.2从k历元开始，IMU自身利用逆向机械编排定位，得当前闭合环扫描过程中移动传感 器平台基于导航数据的定位结果； 5.3从k历元开始，基于模型私^ 啦卩1# + % ,，采用基于扩展卡尔曼滤波的定位融合方 法融合子步骤5.1和5.2的定位结果，得逆向组合定位结果，同时将补偿误差估计值反馈给 IMU逆向机械编排定位;5xi-i、5xi分别表示历元i-l、i的状态误差向量，0, 1表示历元i状态 转移矩阵? i的逆矩阵，^表示历元i的驱动白噪声； S6分别将（1-i/n)和i/n作为正向组合定位结果和逆向组合定位结果的权值，正向组合 定位结果和逆向组合定位结果的加权求和即误差纠正后的定位结果，i表示当前闭合环扫 描过程中历元； S7采用误差纠正后的定位结果更新当前栅格地图； 上述k-n历元和k历元分别为当前闭合环扫描开始历元和结束历元。2. 如权利要求1所述的激光测距移动制图中闭合环误差纠正方法，其特征是： 步骤2进一步包括： 2.1从k-n历元开始，将点云扫描帧和栅格地图进行匹配定位，得当前闭合环扫描过程 中移动传感器平台基于匹配定位的定位结果； 2.2从k-n历元开始，IMU自身利用机械编排定位，得当前闭合环扫描过程中移动传感器 平台基于导航数据的定位结果； 2.3从k-n历元开始，采用基于扩展卡尔曼滤波的定位融合方法融合子步骤2.1和2.2的 定位结果，得正向组合定位结果，同时将补偿误差估计值反馈给MU机械编排定位。
【文档编号】G01S17/08GK106052691SQ201610326882
【公开日】2016年10月26日
【申请日】2016年5月17日
【发明人】唐健, 牛小骥, 施闯
【申请人】武汉大学