。
[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日