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

摘要

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

主权项

一种激光测距移动制图中闭合环误差纠正方法，其特征是，包括步骤：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逆向机械编排定位；δx_i‑1、δx_i分别表示历元i‑1、i的状态误差向量，表示历元i状态转移矩阵Φ_i的逆矩阵，w_i表示历元i的驱动白噪声；S6分别将(1‑i/n)和i/n作为正向组合定位结果和逆向组合定位结果的权值，正向组合定位结果和逆向组合定位结果的加权求和即误差纠正后的定位结果，i表示当前闭合环扫描过程中历元；S7采用误差纠正后的定位结果更新当前栅格地图；上述k‑n历元和k历元分别为当前闭合环扫描开始历元和结束历元。