CN111366139A - 室内测绘点定位方法、装置、计算机设备及存储介质 - Google Patents

Abstract

本发明涉及室内测绘点定位方法、装置、计算机设备及存储介质，该方法包括在首个测绘点开启RGB‑D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；获取惯性测量数据及RGB‑D相机的工作状态，并判断测绘装置的运动状态是否是静置扫描状态；若是，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；发送初始位姿数据至服务器，以供服务器进行点云数据配准，以得到目标位姿数据，并反馈目标位姿数据；若否，则获取图像序列，并进行图像特征提取与位姿估计，得到移动数据；获取海拔高度，并融合移动数据、惯性测量数据、海拔高度，得到解算位姿数据。本发明有效降低成本，提高作业效率。

Description

室内测绘点定位方法、装置、计算机设备及存储介质

技术领域

本发明涉及定位方法，更具体地说是指室内测绘点定位方法、装置、计算机设备及存储介质。

背景技术

目前，室内三维全景重建技术已广泛应用于商品住宅、商场、医院、地铁站、写字楼等场景的测绘，公安刑侦、VR/AR等领域，相关产品也日趋成熟和完善，基于RGB-D相机的测绘装置便是其中之一。基于RGB-D相机的测绘装置的原理是采用结构光、TOF(飞行时间测距法，Time of flight)等技术实现拍摄图像兼具色彩和深度通道，即RGB-D图像，再利用图像算法根据相机位姿生成整体三维点云，实现三维重建。在一些精度要求高的室内场景RGB-D相机多采用静止多位置扫描的方式测绘，这就需要获取各测绘点的相对位置，便于点云数据配准。

目前所采用的室内测绘点的定位方式主要有三种，一种是推车式计程定位；另一种是增加激光雷达应用SLAM技术；最后一种是人工预先规划、测量各测绘点的位置。但是推车式计程定位方式无法在地面复杂和需要保护的现场使用；激光雷达成本高昂，人工规划效率低等。

因此，有必要设计一种新的方法，实现无需加入激光雷达进行SLAM(即时定位与地图构建，simultaneous localization and mapping)，无需预先规划相机架设点，基于自身设备导航无需过多接触现场，能有效降低成本，可在地面复杂和需要保护的现场使用，提高作业效率。

发明内容

本发明的目的在于克服现有技术的缺陷，提供室内测绘点定位方法、装置、计算机设备及存储介质。

为实现上述目的，本发明采用以下技术方案：室内测绘点定位方法，包括：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；

获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；

根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；

若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；

发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；

若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；

获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

其进一步技术方案为：所述在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值，包括：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并以处于前向且被用于视觉里程计的一个方位的RGB-D相机的光轴指向为方位角指向，并使加速度计和陀螺仪的x轴指向重合，y轴向右，z轴遵循右手定则向下，得到机体坐标系；

当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系，并设定此时的方位角指向为0；

设置当前的位置为导航坐标系为原点且初始方位角为0的位置，以得到导航初始值。

其进一步技术方案为：所述锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据，包括：

锁定移动状态下解算位姿数据中的位置和方位角，并根据惯性测量数据中的横滚角、俯仰角以及陀螺仪漂移输入状态方程内进行融合，以得到初始位姿数据。

其进一步技术方案为：所述状态方程为X(t)＝f[X(t),t]+G(t)w，其中X为18维的系统状态向量，所述系统状态向量包含三轴姿态角、三轴姿态角误差、三维速度、三维位置、三轴陀螺仪漂移、三轴加速度误差；f[X(t),t]为非线性的18维向量函数，由捷联惯导方程得到；w为系统白噪声；G(t)为噪声矩阵。

其进一步技术方案为：所述获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据，包括：

获取海拔高度；

根据所述移动数据、惯性测量数据、海拔高度输入组合导航系统量测方程，并对导航系统量测方程进行线性化，使用Kalman基本方程估计组合导航系统状态，以得到解算位姿数据。

其进一步技术方案为：所述组合导航系统量测方程为Z(t)＝h[X(t),t]+v(t)，其中，Z(t)为视觉里程计所采集的测绘装置的位置数据以及方位角数据、惯性测量数据中加速度的x轴、y轴数据组成的量测矢量，h[X(t),t]为非线性向量函数，由系统状态与量测值的关系得到；v(t)为量测白噪声。

其进一步技术方案为：所述发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据之后，还包括：

根据所述目标位姿数据消除位置和方位的累积误差，并重设所述初始位置。

本发明还提供了室内测绘点定位装置，包括：

位置设定单元，用于在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；

初始数据获取单元，用于获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；

判断单元，用于根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；

第一融合单元，用于若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；

第一反馈单元，用于发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；

估计单元，用于若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；

第二融合单元，用于获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

本发明还提供了一种计算机设备，所述计算机设备包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现上述的方法。

本发明还提供了一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时可实现上述的方法。

本发明与现有技术相比的有益效果是：本发明通过设置初始位置后，结合惯性测量数据以及RGB-D相机的工作状态判断测绘装置的运动状态，在测绘装置处于静置扫描状态时，利用惯性测量数据计算初始位姿数据，并上传至服务器辅助其对点云数据配准，并接收配准后反馈的位姿数据修正定位累积误差，在测绘装置处于移动状态时，结合多个数据进行实时位姿计算，充分利用测绘装置本身的RGB-D相机和点云配准计算结果，无需加入激光雷达进行SLAM，无需预先规划相机架设点，基于自身设备导航无需过多接触现场，能有效降低成本，可在地面复杂和需要保护的现场使用，提高作业效率。

下面结合附图和具体实施例对本发明作进一步描述。

附图说明

为了更清楚地说明本发明实施例技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的室内测绘点定位方法的应用场景示意图；

图2为本发明实施例提供的室内测绘点定位方法的流程示意图；

图3为本发明实施例提供的室内测绘点定位方法的子流程示意图；

图4为本发明实施例提供的室内测绘点定位方法的子流程示意图；

图5为本发明另一实施例提供的室内测绘点定位方法的流程示意图；

图6为本发明实施例提供的室内测绘点定位装置的示意性框图；

图7为本发明实施例提供的室内测绘点定位装置的位置设定单元的示意性框图；

图8为本发明实施例提供的室内测绘点定位装置的第二融合单元的示意性框图；

图9为本发明另一实施例提供的室内测绘点定位装置的示意性框图；

图10为本发明实施例提供的计算机设备的示意性框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

请参阅图1和图2，图1为本发明实施例提供的室内测绘点定位方法的应用场景示意图。图2为本发明实施例提供的室内测绘点定位方法的示意性流程图。该室内测绘点定位方法应用于测绘装置中。该测绘装置与服务器进行数据交互，由测绘装置进行测绘点的定位后，当测绘装置处于静置扫描状态，则定位后获取的数据会发送至服务器进行3D点云配准，以提升整个定位的准确率，并将配准后的结果发送至测绘装置，当测绘装置不是处于静置扫描状态，则融合多个数据后形成位姿。

图2是本发明实施例提供的室内测绘点定位方法的流程示意图。如图2所示，该方法包括以下步骤S110至S170。

S110、在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值。

在本实施例中，定位请求是指开启测绘装置时所构成的请求，也就是当开启测绘装置时，测绘装置会接收到一个定位请求，以进行室内各个测绘点的定位操作。

上述的RGB-D相机为测绘装置的其中一部分，且整个测绘装置使用脚架支撑在地面上，以配合测绘装置进行室内三维全景测绘作业。在进行室内测绘点的定位时，需要从首个测绘点开始，并对所有测绘点都进行定位。

在本实施例中，在RGB-D相机的测绘装置中加入定位子系统，定位子系统包括IMU(惯性测量单元，Inertial measurement unit)、单个RGB-D相机、气压高度计、处理器单元及TCP网络模块构成，测绘装置内集成了IMU、气压高度计、基于RGB-D相机的视觉里程计在内的多路感知模块，并结合EKF(扩展卡尔曼滤波器，Extended Kalman Filter)组合导航模块，融合上述多路感知模块的数据，估计位姿与IMU误差。

EKF组合导航融合IMU、视觉里程计及气压计输出多路感知数据解算出测绘装置的实时位姿数据。

其中，IMU有三轴陀螺仪和三轴加速度计，用于感知测绘装置移动时的空间角速度和线加速度，以此获取惯性测量数据。

基于RGB-D相机的视觉里程计用于估计测绘装置移动时的位置和姿态；气压高度计用于感知海拔高度，提供大范围垂向位置参考。

在本实施例中，导航初始值是指测绘装置所处的当前位置，用户向定位子系统设定EKF组合导航与视觉里程计的导航初始值。

在一实施例中，请参阅图3，上述的步骤S110可包括步骤S111～S113。

S111、在首个测绘点开启RGB-D相机扫描时，获取定位请求，并以RGB-D相机的光轴指向为方位角指向，并使加速度计和陀螺仪的x轴指向重合，y轴向右，z轴遵循右手定则向下，得到机体坐标系；

S112、当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系，并设定此时的方位角指向为0；

S113、设置当前的位置为导航坐标系为原点且初始方位角为0的位置，以得到导航初始值。

以视觉里程计使用的处于测绘装置定义的“前”向上的RGB-D相机光轴指向为方位角指向，使IMU中加速度计和陀螺仪的x轴该指向重合，y轴向右，z轴遵循右手定则向下，得到测绘装置的机体坐标系b-xyz，当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系e-xyz，并设定此时的方位角指向为0，在上述导航坐标系下实现EKF组合导航功能。设定当前位置为组合导航与视觉里程计的初始位置，即e-xyz坐标为(0，0，0)的点，设定方位指向此时的值为0即初始方位角等于0。

S120、获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据。

在本实施例中，初始数据是指惯性测量数据以及RGB-D相机的工作状态的综合。

其中，惯性测量数据是可表示测绘装置是否处于移动状态的加速度值，再结合RGB-D相机的工作状态是处于扫描状态等，便可判断当前测绘装置的运动状态。利用IMU数据和RGB-D相机的工作状态实时判断测绘装置的运动状态，据此切换工作模式。

S130、根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；

S140、若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据。

在本实施例中，初始位姿数据是指惯性测量数据中的横滚角、俯仰角以及陀螺仪漂移输入状态方程内进行融合所得的数据。

具体地，测绘装置处于静置扫描状态时，EKF组合导航锁定移动状态下最后输出的位置以及姿态中的方位角，停止更新这些数据，同时取消视觉里程计所采集的位置与气压高度数据融合，仅融合IMU估计姿态数据中的横滚角和俯仰角以及陀螺仪漂移。

具体地，锁定移动状态下解算位姿数据中的位置和方位角，并根据惯性测量数据中的横滚角、俯仰角以及陀螺仪漂移输入状态方程内进行融合，以得到初始位姿数据。

首先使用IMU根据捷联惯导方程建立惯性导航系统，以惯导导航参数和IMU误差作为组合导航系统的状态得到状态方程，其中，所述状态方程为X(t)＝f[X(t),t]+G(t)w，其中X为18维的系统状态向量，所述系统状态向量包含三轴姿态角、三轴姿态角误差、三维速度、三维位置、三轴陀螺仪漂移、三轴加速度误差；f[X(t),t]为非线性的18维向量函数，由捷联惯导方程得到；w为系统白噪声；G(t)为噪声矩阵。由此便可根据状态方程计算其他姿态和估计陀螺仪漂移数据，以此构成该测绘点的位置信息。

如果是首个测绘点则位置直接为(0，0，0)，方位角也为0，也就是初始位姿数据为位置为(0，0，0)，方位角也为0。

S150、发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据。

在本实施例中，目标位姿数据是指经过3D点云数据配准后形成的更加准确的位置数据。定位子系统检测到RGB-D相机本次扫描结束后，便将初始位姿数据上传给服务器。

测绘装置除了上传RGB-D图像数据至服务器外，测绘装置还能上传初始位姿数据，作为所在测绘点的3D点云数据配准的初始参考位姿，从而省去或优化服务器端通过图像特征点匹配算法估计位姿的环节，加速点云数据配准过程。

当视觉里程计估计的位姿与EKF组合导航解算的位姿会存在误差累积，需要及时修正，也就是初始位姿数据与实际的测绘点的位置数据存在一定的误差，而服务器使用ICP算法等算法对新一组3D点云数据配准后，能同时得到采集该组点云对应的RGB-D相机与其在上一点或第一点采集时的相对位姿变化量，即旋转矩阵R和平移矢量T。在设定初始位姿数据后，便可由相对位姿变化量求出该组点云对应的较精确的相机位姿。因此，增加服务器反馈消息，将配准后得到的位姿数据下发给测绘装置，即可修正上述误差，因此，可利用该目标位姿数据调整初始位置，以供供下一次移动状态下EKF组合导航和视觉里程计使用。

在一实施例中，当定位子系统检测测绘装置是否结束测绘，若测绘装置结束测绘，则进入结束步骤；若测绘装置不结束测绘，则执行所述步骤S130。

当用户开始移动测绘装置或开始在新的测绘点静置扫描时，切换工作模式，如此循环，直至测绘结束。

S160、若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据。

在本实施例中，移动数据是指根据采集的图像进行测绘点特征提取和位姿的估计所形成的位姿数据，也就是视觉里程计所输出的位置和方位角数据。

当测绘装置处于移动状态时，视觉里程计开始接收处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列并使用ORB(Oriented FAST and Rotated BRIEF)进行图像特征提取，使用PnP(Perspective-n-Point)算法进行位姿估计。

S170、获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

在本实施例中，解算位姿数据是指测绘点的位置和方位角。

EKF组合导航则根据融合惯性测量数据、视觉里程计及气压计输出多路感知数据解算出实时位姿。

在一实施例中，请参阅图4，上述的步骤S170可包括步骤S171～S172。

S171、获取海拔高度；

S172、根据所述移动数据、惯性测量数据、海拔高度输入组合导航系统量测方程，并对导航系统量测方程进行线性化，使用Kalman基本方程估计组合导航系统状态，以得到解算位姿数据。

具体地，所述组合导航系统量测方程为Z(t)＝h[X(t),t]+v(t)，其中，Z(t)为视觉里程计所采集的测绘装置的位置数据以及方位角数据、惯性测量数据中加速度的x轴、y轴数据组成的量测矢量，h[X(t),t]为非线性向量函数，由系统状态与量测值的关系得到；v(t)为量测白噪声。

在本实施例中，先根据捷联惯导方程建立惯性导航系统，以惯导导航参数和IMU所获得的惯性测量数据的误差作为组合导航系统的状态，得到状态方程；再建立组合导航系统量测方程，最后根据EKF算法，将上述导航系统量测方程离散化和线性化后使用Kalman基本方程估计组合导航系统状态，状态中就包括了所需的目标位姿数据，由组合导航提供较精确相机姿态角，可省去或优化服务器端通过图像特征点匹配的算法估计姿态环节，无需对相机架设的测绘点预先规划测量，减少服务器计算量，提高作业效率；无需过多接触现场，对现场保护较好；无需增加昂贵的激光雷达，在满足定位要求的同时可降低成本。利用单个RGB-D相机的图像数据，根据常规的ORB与PnP算法借助g2o图优化框架实现。

测绘装置依托TCP网络协议扩展一条向服务器上传数据的消息。测绘装置在监测到正常的RGB-D图像数据上传完成后，通过该拓展消息上传由EKF组合导航输出的位姿数据，该位姿数据，作为所在测绘点3D点云数据配准的初始参考位姿，辅助服务器加速点云数据配准过程。同样地，依托TCP网络协议，增加一条服务器发送，测绘装置接收的TCP消息。用于反馈服务器配准后得到的较精确的位姿数据。

将计算到的实时的目标位姿数据进行反馈，包括反馈至服务器，以供服务器进行存储等操作。

上述的室内测绘点定位方法适用于架站式RGB-D相机静置扫描，间歇工作，测绘点之间需要知道相对位置的作业场景，使用该方法构建的测绘装置在无预先规划测量、不允许过多接触现场的条件下，做到设备位置实时输出即可。

在一实施例中，当定位子系统检测测绘装置是否结束测绘，若测绘装置结束测绘，则进入结束步骤；若测绘装置不结束测绘，则执行所述步骤S130。

当用户开始移动测绘装置或开始在新的测绘点静置扫描时，切换工作模式，如此循环，直至测绘结束。

在测绘装置中加入定位子系统，在首个测绘点向子系统输入导航初始值。测绘装置在移动-静置扫描循环的工作过程中，定位子系统利用惯性测量数据和RGB-D相机测绘装置的工作状态实时判断测绘装置的运动状态。若测绘装置处于移动状态，则EKF组合导航融合IMU、视觉里程计和气压高度计数据解算出测绘装置的实时位姿数据。若测绘装置处于静止扫描状态，则EKF组合导航锁定位置以及姿态中的方位角更新，仅融合IMU数据解算其余姿态数据。上传EKF输出的位姿数据至服务器，以供服务器进行点云数据配准，并将配准后得到的位姿数据反馈回测绘装置的定位子系统，以修正定位误差。

上述的室内测绘点定位方法，通过设置初始位置后，结合惯性测量数据以及RGB-D相机的工作状态判断测绘装置的运动状态，在测绘装置处于静置扫描状态时，利用惯性测量数据计算初始位姿数据，并上传至服务器辅助其对点云数据配准，并接收配准后反馈的位姿数据修正定位累积误差，在测绘装置处于移动状态时，结合多个数据进行实时位姿计算，充分利用测绘装置本身的RGB-D相机和点云配准计算结果，无需加入激光雷达进行SLAM，无需预先规划相机架设点，基于自身设备导航无需过多接触现场，能有效降低成本，可在地面复杂和需要保护的现场使用，提高作业效率。

图5是本发明另一实施例提供的一种室内测绘点定位方法的流程示意图。如图5所示，本实施例的室内测绘点定位方法包括步骤S210-S280。其中步骤S210-S250与上述实施例中的步骤S110-S150类似，步骤S270-S280与上述实施例中的步骤S160-S170类似，在此不再赘述。下面详细说明本实施例中所增加的步骤S260。

S260、根据所述目标位姿数据消除位置和方位的累积误差，并重设所述初始位置。

EKF组合导航使用服务器的目标位姿数据中的位置与方位角执行重新对准，消除位置和方位的累积误差；即对EKF组合导航重新对准，消除位置和方位的累积误差；同样，视觉里程计使用反馈的位姿数据，重置导航初始值，也就是重设初始位置。

此后，当用户得到扫描完成提示开始移动测绘装置，或在新的测绘点开启扫描时，测绘装置会在判断测绘装置的运动状态的实时检测中切换新的工作状态，开始新的循环，直至测绘结束。

图6是本发明实施例提供的一种室内测绘点定位装置300的示意性框图。如图6所示，对应于以上室内测绘点定位方法，本发明还提供一种室内测绘点定位装置300。该室内测绘点定位装置300包括用于执行上述室内测绘点定位方法的单元，该装置可以被配置于服务器中。具体地，请参阅图6，该室内测绘点定位装置300包括位置设定单元301、初始数据获取单元302、判断单元303、第一融合单元304、第一反馈单元305、估计单元307以及第二融合单元308。

位置设定单元301，用于在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；初始数据获取单元302，用于获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；判断单元303，用于根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；第一融合单元304，用于若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；第一反馈单元305，用于发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；估计单元307，用于若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；第二融合单元308，用于获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

在一实施例中，如图7所示，所述位置设定单元301包括机体坐标系获取子单元3011、导航坐标系获取子单元3012以及设置子单元3013。

机体坐标系获取子单元3011，用于在首个测绘点开启RGB-D相机扫描时，获取定位请求，并以处于前向且被用于视觉里程计的一个方位的RGB-D相机的光轴指向为方位角指向，并使加速度计和陀螺仪的x轴指向重合，y轴向右，z轴遵循右手定则向下，得到机体坐标系；导航坐标系获取子单元3012，用于当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系，并设定此时的方位角指向为0；设置子单元3013，用于设置当前的位置为导航坐标系为原点且初始方位角为0的位置，以得到导航初始值。

在一实施例中，如图8所示，所述第二融合单元308包括高度获取子单元3081以及状态计算子单元3082。

高度获取子单元3081，用于获取海拔高度；状态计算子单元3082，用于根据所述移动数据、惯性测量数据、海拔高度输入组合导航系统量测方程，并对导航系统量测方程进行线性化，使用Kalman基本方程估计组合导航系统状态，以得到解算位姿数据。

图9是本发明另一实施例提供的一种室内测绘点定位装置300的示意性框图。如图9所示，本实施例的室内测绘点定位装置300是上述实施例的基础上增加了重设单元306。

重设单元306，用于根据所述目标位姿数据消除位置和方位的累积误差，并重设所述初始位置。

需要说明的是，所属领域的技术人员可以清楚地了解到，上述室内测绘点定位装置300和各单元的具体实现过程，可以参考前述方法实施例中的相应描述，为了描述的方便和简洁，在此不再赘述。

上述室内测绘点定位装置300可以实现为一种计算机程序的形式，该计算机程序可以在如图10所示的计算机设备上运行。

请参阅图10，图10是本申请实施例提供的一种计算机设备的示意性框图。该计算机设备500可以是服务器，服务器可以是独立的服务器，也可以是多个服务器组成的服务器集群。

参阅图10，该计算机设备500包括通过系统总线501连接的处理器502、存储器和网络接口505，其中，存储器可以包括非易失性存储介质503和内存储器504。

该非易失性存储介质503可存储操作系统5031和计算机程序5032。该计算机程序5032包括程序指令，该程序指令被执行时，可使得处理器502执行一种室内测绘点定位方法。

该处理器502用于提供计算和控制能力，以支撑整个计算机设备500的运行。

该内存储器504为非易失性存储介质503中的计算机程序5032的运行提供环境，该计算机程序5032被处理器502执行时，可使得处理器502执行一种室内测绘点定位方法。

该网络接口505用于与其它设备进行网络通信。本领域技术人员可以理解，图10中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备500的限定，具体的计算机设备500可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

其中，所述处理器502用于运行存储在存储器中的计算机程序5032，以实现如下步骤：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

在一实施例中，处理器502在实现所述在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值步骤时，具体实现如下步骤：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并以处于前向且被用于视觉里程计的一个方位的RGB-D相机的光轴指向为方位角指向，并使加速度计和陀螺仪的x轴指向重合，y轴向右，z轴遵循右手定则向下，得到机体坐标系；当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系，并设定此时的方位角指向为0；设置当前的位置为导航坐标系为原点且初始方位角为0的位置，以得到导航初始值。

在一实施例中，处理器502在实现所述锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据步骤时，具体实现如下步骤：

锁定移动状态下解算位姿数据中的位置和方位角，并根据惯性测量数据中的横滚角、俯仰角以及陀螺仪漂移输入状态方程内进行融合，以得到初始位姿数据。

其中，所述状态方程为X(t)＝f[X(t),t]+G(t)w，其中X为18维的系统状态向量，所述系统状态向量包含三轴姿态角、三轴姿态角误差、三维速度、三维位置、三轴陀螺仪漂移、三轴加速度误差；f[X(t),t]为非线性的18维向量函数，由捷联惯导方程得到；w为系统白噪声；G(t)为噪声矩阵。

在一实施例中，处理器502在实现所述获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据步骤时，具体实现如下步骤：

获取海拔高度；根据所述移动数据、惯性测量数据、海拔高度输入组合导航系统量测方程，并对导航系统量测方程进行线性化，使用Kalman基本方程估计组合导航系统状态，以得到解算位姿数据。

其中，所述组合导航系统量测方程为Z(t)＝h[X(t),t]+v(t)，其中，Z(t)为视觉里程计所采集的测绘装置的位置数据以及方位角数据、惯性测量数据中加速度的x轴、y轴数据组成的量测矢量，h[X(t),t]为非线性向量函数，由系统状态与量测值的关系得到；v(t)为量测白噪声。

在一实施例中，处理器502在实现所述发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据步骤之后，还实现如下步骤：

根据所述目标位姿数据消除位置和方位的累积误差，并重设所述初始位置。

应当理解，在本申请实施例中，处理器502可以是中央处理单元(CentralProcessing Unit，CPU)，该处理器502还可以是其他通用处理器、数字信号处理器(DigitalSignal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

本领域普通技术人员可以理解的是实现上述实施例的方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成。该计算机程序包括程序指令，计算机程序可存储于一存储介质中，该存储介质为计算机可读存储介质。该程序指令被该计算机系统中的至少一个处理器执行，以实现上述方法的实施例的流程步骤。

因此，本发明还提供一种存储介质。该存储介质可以为计算机可读存储介质。该存储介质存储有计算机程序，其中该计算机程序被处理器执行时使处理器执行如下步骤：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

在一实施例中，所述处理器在执行所述计算机程序而实现所述在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值步骤时，具体实现如下步骤：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并以处于前向且被用于视觉里程计的一个方位的RGB-D相机的光轴指向为方位角指向，并使加速度计和陀螺仪的x轴指向重合，y轴向右，z轴遵循右手定则向下，得到机体坐标系；当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系，并设定此时的方位角指向为0；设置当前的位置为导航坐标系为原点且初始方位角为0的位置，以得到导航初始值。

在一实施例中，所述处理器在执行所述锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据步骤时，具体实现如下步骤：

锁定移动状态下解算位姿数据中的位置和方位角，并根据惯性测量数据中的横滚角、俯仰角以及陀螺仪漂移输入状态方程内进行融合，以得到初始位姿数据。

其中，所述状态方程为X(t)＝f[X(t),t]+G(t)w，其中X为18维的系统状态向量，所述系统状态向量包含三轴姿态角、三轴姿态角误差、三维速度、三维位置、三轴陀螺仪漂移、三轴加速度误差；f[X(t),t]为非线性的18维向量函数，由捷联惯导方程得到；w为系统白噪声；G(t)为噪声矩阵。

在一实施例中，所述处理器在执行所述计算机程序而实现所述获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据步骤时，具体实现如下步骤：

获取海拔高度；根据所述移动数据、惯性测量数据、海拔高度输入组合导航系统量测方程，并对导航系统量测方程进行线性化，使用Kalman基本方程估计组合导航系统状态，以得到解算位姿数据。

其中，所述组合导航系统量测方程为Z(t)＝h[X(t),t]+v(t)，其中，Z(t)为视觉里程计所采集的测绘装置的位置数据以及方位角数据、惯性测量数据中加速度的x轴、y轴数据组成的量测矢量，h[X(t),t]为非线性向量函数，由系统状态与量测值的关系得到；v(t)为量测白噪声。

在一实施例中，所述处理器在执行所述计算机程序而实现所述发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据步骤之后，还实现如下步骤：

根据所述目标位姿数据消除位置和方位的累积误差，并重设所述初始位置。

所述存储介质可以是U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的计算机可读存储介质。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的。例如，各个单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。

本发明实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减。本发明实施例装置中的单元可以根据实际需要进行合并、划分和删减。另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。

该集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，终端，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.室内测绘点定位方法，其特征在于，包括：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；

获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；

根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；

若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；

发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；

若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；

获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

2.根据权利要求1所述的室内测绘点定位方法，其特征在于，所述在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值，包括：

在首个测绘点开启RGB-D相机扫描时，获取定位请求，并以处于前向且被用于视觉里程计的一个方位的RGB-D相机的光轴指向为方位角指向，并使加速度计和陀螺仪的x轴指向重合，y轴向右，z轴遵循右手定则向下，得到机体坐标系；

当测绘装置处于首个测绘点，且机体坐标系x轴和y轴水平时，则得到与机体坐标系重合的导航坐标系，并设定此时的方位角指向为0；

设置当前的位置为导航坐标系为原点且初始方位角为0的位置，以得到导航初始值。

3.根据权利要求1所述的室内测绘点定位方法，其特征在于，所述锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据，包括：

锁定移动状态下解算位姿数据中的位置和方位角，并根据惯性测量数据中的横滚角、俯仰角以及陀螺仪漂移输入状态方程内进行融合，以得到初始位姿数据。

4.根据权利要求3所述的室内测绘点定位方法，其特征在于，所述状态方程为X(t)＝f[X(t),t]+G(t)w，其中X为18维的系统状态向量，所述系统状态向量包含三轴姿态角、三轴姿态角误差、三维速度、三维位置、三轴陀螺仪漂移、三轴加速度误差；f[X(t),t]为非线性的18维向量函数，由捷联惯导方程得到；w为系统白噪声；G(t)为噪声矩阵。

5.根据权利要求4所述的室内测绘点定位方法，其特征在于，所述获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据包括：

获取海拔高度；

根据所述移动数据、惯性测量数据、海拔高度输入组合导航系统量测方程，并对导航系统量测方程进行线性化，使用Kalman基本方程估计组合导航系统状态，以得到解算位姿数据。

6.根据权利要求5所述的室内测绘点定位方法，其特征在于，所述组合导航系统量测方程为Z(t)＝h[X(t),t]+v(t)，其中，Z(t)为视觉里程计所采集的测绘装置的位置数据以及方位角数据、惯性测量数据中加速度的x轴、y轴数据组成的量测矢量，h[X(t),t]为非线性向量函数，由系统状态与量测值的关系得到；v(t)为量测白噪声。

7.根据权利要求1所述的室内测绘点定位方法，其特征在于，所述发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据之后，还包括：

根据所述目标位姿数据消除位置和方位的累积误差，并重设所述初始位置。

8.室内测绘点定位装置，其特征在于，包括：

位置设定单元，用于在首个测绘点开启RGB-D相机扫描时，获取定位请求，并设定初始位置，以得到导航初始值；

初始数据获取单元，用于获取惯性测量数据以及RGB-D相机的工作状态，以得到初始数据；

判断单元，用于根据所述初始数据判断所述测绘装置的运动状态是否是静置扫描状态；

第一融合单元，用于若所述测绘装置的运动状态是静置扫描状态，则锁定移动状态下解算位姿数据中的位置和方位角，并融合惯性测量数据，以得到初始位姿数据；

第一反馈单元，用于发送所述初始位姿数据至服务器，以供服务器将所述初始位姿作为本组点云数据配准的初始参考位姿，进行点云数据配准，以得到目标位姿数据，并反馈所述目标位姿数据；

估计单元，用于若所述测绘装置的运动状态不是静置扫描状态，则获取处于前向且被用于视觉里程计的一个方位的RGB-D相机采集的图像序列，并根据所述图像序列进行图像特征提取与位姿估计，以得到移动数据；

第二融合单元，用于获取海拔高度，并融合所述移动数据、惯性测量数据、海拔高度，以得到解算位姿数据。

9.一种计算机设备，其特征在于，所述计算机设备包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的方法。

10.一种存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时可实现如权利要求1至7中任一项所述的方法。

Images