实施例提供一种室内环境二维与三维联合模型的构建方法，通过包括一个其底部沿水平方向安装的水平二维激光扫描仪、分别位于所述水平二维激光扫描仪两侧且其底部沿竖直方向安装的两个竖直二维激光扫描仪、以及惯性传感单元的室内移动三维测图平台实施。
[0029]如图1、图2及图3所示，该方法包括如下步骤S1-步骤S5:
[0030]S1、获得所述水平二维激光扫描仪输出的二维激光扫描数据，构建二维栅格地图；
[0031]SLAM (Simultaneous Localizat1n And Mapping，同步定位与地图创建)是指移动平台在未知环境中构建地图，并同时使用该地图进行定位与导航。因此要解决两个问题，一是地图创建问题，即根据获得的信息如何实现对环境的描述和模型构建，二是定位问题，即估计移动平台相对于地图的位置和姿态。在地图构建的过程中，首先，需要获得室内环境中的二维栅格地图，室内环境中的二维栅格地图是使用所述水平二维激光扫描仪输出的二维激光扫描数据基于二维的SLAM算法构建的。可选的，采用RBPF(Rao-BlackwellisedParticle Filter粒子滤波)方法为二维SLAM解决方案来创建二维栅格地图。RBPF方法可以精确地产生潜在的粒子分布和自适应重采样粒子，以避免粒子耗尽(基于里程表估计的概率模型和扫描匹配考虑二维范围内数据的粒子权重)。获取所述水平二维激光扫描仪输出的二维激光扫描数据后，该二维SLAM过程提供了环境的二维栅格地图，并且还提供了一个初始姿势确定的平台系统，同时该数据将被用作三维激光扫描的参考值。
[0032]S2、采用扩展卡尔曼滤波方法，根据所述惯性传感单元输出的数据与所述二维栅格地图，实现平台六自由度姿态估计；
[0033]在步骤S2中，惯性传感单元可准确地得到移动平台在各个方向的线加速度与角加速度，以获得更精确的三维位姿。采用EKF扩展卡尔曼滤波方法融合二维激光扫描数据和惯性传感单元输出的数据，实现平台六自由度姿态估计。
[0034]可选的，步骤S2具体通过以下方法实现:
[0035]S21、根据所述二维栅格地图估计出平台的二维位姿信息，该二维位姿信息包括平台在二维平面中的当前位置和朝向；
[0036]该步骤具体可采用2D SLAM算法系统，以获得平台的二维位姿估计。
[0037]S22、获得所述惯性传感单元输出的数据，该数据包括线加速度变化数据和角加速度变化数据；
[0038]S23、根据所述惯性传感单元输出的数据计算得到平台的三维位姿信息，该三维位姿信息包括平台高度、俯仰、偏航和横滚；
[0039]该步骤具体可采用惯性传感单元提供的驱动的算法系统，以获得平台的三维位姿?目息。
[0040]S24、采用扩展卡尔曼滤波方法，融合所述三维位姿信息和所述二维位姿信息实现平台六自由度姿态估计。
[0041]该步骤具体可采用6-D0F定位算法系统，以获得移动平台的六自由度姿态估计。其中，惯性传感单元可准确地得到移动平台在各个方向的线加速度与角加速度，6-D0F定位算法系统采用EKF扩展卡尔曼滤波方法融合二维栅格地图和惯性传感单元输出的数据实现平台六自由度姿态估计。所述六自由度姿态估计可以为该系统提供一个完整的三维位姿估计，并可以用来加速地图的配准过程，并且可在多地图之间没有重叠区域时配准两个地图。
[0042]S3、获得所述竖直二维激光扫描仪输出的二维激光扫描数据，根据S2中得到的平台六自由度姿态估计的结果，将所述竖直二维激光扫描仪输出的二维激光扫描数据配准到平台的三维坐标系中，得到室内环境的三维点云数据；
[0043]S4、将初始时刻的平台六自由度位姿估计的结果作为初始旋转平移矩阵并确定三维坐标系原点位置，将所获得的三维点云数据通过相应的旋转平移矩阵配准到平台的三维坐标系以生成三维地图；
[0044]S5、采用二维栅格地图构建中的闭合环路检测原理，对获得的二维栅格地图进行闭环检测；根据闭环检测结果对获得的二维位姿信息进行全局优化调整，根据调整结果对二维栅格地图进行优化，获得全局优化的二维栅格地图；再根据闭环检测结果对获得的平台六自由度位姿估计的结果进行全局优化调整，根据调整结果对三维地图进行优化，获得全局优化的三维地图。
[0045]通常平台在地图创建与位姿估计中都会有累积误差存在，将导致平台在完成同步定位与地图创建的过程中环路无法闭合，即无法判断是否回到已探索区域，同时使得环境中的同一个地方对应多个地图，导致地图的不一致性，进而妨碍平台完成SLAM任务。
[0046]该步骤中:在二维SLAM的闭环检测中，持续监测基于RBPF的二维SLAM的粒子滤波器的粒子权重；当平台系统遇到闭环，即当粒子权重比设定的阈值小时，粒子滤波器开始重新采样和复位所有粒子的权重，校正该平台系统的位姿，并更新二维栅格地图。在全局优化中，采用基于构图的优化方法，以消除构图过程中的累积误差，建立全局一致的地图。在建立全局三维地图的基础上，使用g2o的构图优化框架。所述g2o框架是一个开源图优化器，其被广泛应用于基于图的SLAM问题和BA的问题。该系统的位姿表示为图中的结构中的节点，并且所述扫描之间的配准误差被表示为图中结构的边缘。通过对全局三维地图所估计的平台姿态的变换矩阵进行更新，最终创建全局一致的三维地图。基于平台上二维与三维数据获取传感器间的刚性连接，二维SLAM闭环检测获得的同时，认为在三维地图上也检测到一个闭环。因此，应用基于图的优化框架为平台六自由度姿态进行优化调整，减少平台定位的不确定性，进而提高创建地图的精度。
[0047]本发明实施例通过获取平台六自由度位姿估计，并将平台六自由度位姿估计的结果作为初始旋转平移矩阵，然后将所述三维点云数据配准为三维地图，提高了地图构建的效率，解决了现有技术中移动平台在三维方向上空间中运动时，对三维空间的探索能力的限制，最终构建的三维地图准确性高，该方法尤其适用于路面不平整情况下的的轮式平台采集系统以及操作者背负采集系统，环境适用性强。此外，通过采用二维栅格地图构建中的闭合环路检测原理，进行二维栅格地图的闭环检测，并通过平台位姿调整，获得全局优化的二维栅格地图和三维地图，提高了二维与三维地图构建的效率与准确性，有效的在近距离、多转角、易遮挡、光照复杂、缺乏绝对定位等特点的室内准确构建室内环境的二维栅格地图，并提供与之对应的三维点云模型。
[0048]在上述实施例的基础上，可选的，在所述步骤S5之后，该方法还包括:
[0049]S6、将得到的二维栅格地图与三维地图进行空间坐标系一致性配准，最终得到空间坐标系一致的二维栅格地图与三维地图。
[0050]这样设置的好处是，通过空间一致性配准，使二维栅格地图与三维地图在空间上具有一致性。该步骤具体如下:将三维地图的三维坐标系变换到二维栅格地图的坐标系上，使二维栅格地图和三维地图的坐标系一致，即可使得三维地图与二维栅格地图在空间上具有一致性。
[0051]为实施如上所述室内环境二维与三维联合模型的构建方法本实施例还提供一种室内移动三维测图平台系统。
[0052]如图4所示，移动平台5上安装有水平二维激光扫描仪51、竖直二维激光扫描