一种室内定位与建图方法及系统

技术领域

本发明涉及实时定位与建图技术技术领域，特别是涉及一种室内定位与建图方法及系统。

背景技术

激光雷达SLAM技术(Simultaneous Localization and Mapping，SLAM)用于移动机器人只依赖自身的激光Lidar传感器进行自我定位以及对环境的感知。激光雷达SLAM技术一般与IMU(Inertial Measurement Unit，IMU)联合使用，IMU传感器主要用于激光点云的矫正，解决运动过程中的雷达点云产生的畸变问题。

然而，利用现有的激光SLAM技术在在短时间内能够保证移动机器人的较高定位精度，若长时间运行，以及在结构单一的走廊环境将不可避免地产生定位累计误差和回环检测失败，导致长时间定位时定位精度的大幅下降。

发明内容

为了克服现有技术的不足，本发明的目的是提供一种室内定位与建图方法及系统。

为实现上述目的，本发明提供了如下方案：

一种室内定位与建图方法，包括：

获取目标空间内的激光雷达数据和惯性传感器数据；

分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；

基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；

基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；

基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；

基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；

根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。

优选地，所述分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据，包括：

记录数据采集周期内惯性传感器的运动轨迹；

利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；

通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；

对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：

所述惯性传感器数据约束因子的公式为：

优选地，所述基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息，包括：

判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；

利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；

基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。

优选地，所述基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息，包括：

基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。

优选地，所述基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架，包括：

基于七参数模型对所述第一位姿信息进行转换，得到所述第二位姿信息坐标框架。

优选地，所述利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息，包括：

基于图优化工具对所述载体的位姿进行全局位姿优化，得到所述优化求解后的位姿信息。

一种室内定位与建图系统，包括：

获取模块，用于获取目标空间内的激光雷达数据和惯性传感器数据；

预处理模块，用于分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；

第一位姿计算模块，用于基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；

第二位姿计算模块，用于基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；

坐标转换模块，用于基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；

求解模块，用于基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；

配准模块，用于根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。

优选地，所述预处理模块具体包括：

记录单元，用于记录数据采集周期内惯性传感器的运动轨迹；

坐标确定单元，用于利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；

转换单元，用于通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；

预积分处理单元，用于对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：

所述惯性传感器数据约束因子的公式为：

优选地，所述基于所述第一位姿计算模块具体包括：

判断单元，用于判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；

筛选单元，用于利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；

第一计算单元，用于基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。

优选地，所述第二位姿计算模块具体包括：

第二计算单元，用于基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供了一种室内定位与建图方法及系统，通过获取目标空间内的激光雷达数据和惯性传感器数据；分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，最终生成了室内的三维点云地图。本发明将UWB与激光SLAM相结合，利用激光SLAM在短时间内的高精度定位优势，UWB的定位精度不随时间变化的优势，利用滑动窗口优化该组合，实现了在短时间或长时间内移动机器人在所处空间内较高的实时定位精度，且不随时间的变化产生累计误差，不会对定位精度产生较大影响。

附图说明

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

图1为本发明提供的实施例中的方法流程图；

图2为本发明提供的实施例中的流程框图。

具体实施方式

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

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

本申请的说明书和权利要求书及所述附图中的术语“第一”、“第二”、“第三”和“第四”等是用于区别不同对象，而不是用于描述特定顺序。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤、过程、方法等没有限定于已列出的步骤，而是可选地还包括没有列出的步骤，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤元。

激光SLAM技术指的是物体在运动过程中依赖所携带的激光与IMU传感器实现对自身的定位的同时对周围环境进行二维或三维的地图构建，是移动机器人定位导航技术的重要方向。激光SLAM具有精度高，鲁棒性好等特点。但在结构化环境下存在退化等情况，因此大多数激光SLAM技术都会融合惯性测量单元(Inertial MeasurementUnit，IMU)，即本申请中的惯性传感器，短时间内提供运动的初值，改善匹配效果。这种方式可有效缓解短时间的定位精度和鲁棒性，但是IMU传感器存在零偏误差，长时间的定位效果误差积累严重，因此本发明设计利用超宽带(UltraWide Band，UWB)与激光SLAM融合的空间定位方式，不仅能够有效解决激光SLAM长距离误差累积的问题，也能提供可复用的具有统一坐标系统的二维或三维地图。对室内外无缝定位导航系统提供可行的技术方案。

本发明的目的是提供一种室内定位与建图方法及系统，将UWB与激光SLAM相结合，利用激光SLAM在短时间内的高精度定位优势，UWB的定位精度不随时间变化的优势，利用滑动窗口优化该组合，实现了在短时间或长时间内移动机器人在所处空间内较高的实时定位精度，且不随时间的变化产生累计误差，不会对定位精度产生较大影响。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明提供的实施例中的方法流程图，如图1所示，本发明提供了一种室内定位与建图方法，包括：

步骤100：获取目标空间内的激光雷达数据和惯性传感器数据；

步骤200：分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；

步骤300：基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；

步骤400：基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；

步骤500：基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；

步骤600：基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；

步骤700：根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。

图2为本发明提供的实施例中的流程框图，如图2所示，其中，第一个流程是需要先实时获取空间内载体2D或3D激光雷达、IMU数据。在该过程中，激光雷达数据指安置在运动载体上的单线程或多线程激光雷达运动采集到的2D点云坐标数据，以及实时获取的IMU采集到的加速度和角速度数据。此外还需要，实时检测UWB脉冲信号，由于UWB只需要在关键部位进行布设，因此UWB信号不是实时都会检测到。

优选地，所述步骤200包括：

记录数据采集周期内惯性传感器的运动轨迹；

利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；

通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；

对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：

所述惯性传感器数据约束因子的公式为：

具体的，本实施例中第二个流程为对激光Lidar数据进行去畸变处理，需要对2D激光雷达点云数据进行去畸变处理，记录数据采集周期内IMU的运动轨迹；利用所述时间戳和IMU的运动轨迹得到在采集各个所述点云时激光雷达的位置和姿态；通过外参将得到的每帧点云的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，即实现了点云去畸变。

进一步地，本实施例中第三个流程为对激光Lidar关键帧间的IMU点云数据进行预积分处理，对应下文预积分因子；

本发明为减少计算量和计算机存储空间只存储激光关键帧数据；单线程激光Lidar每扫描一圈为一帧数据，有时候激光Lidar并没有运动，或者运动距离很少，就会造成数据冗余，造成不必要的计算，基于此，下文位姿优化与局部建图过程只对激光Lidar关键帧数据进行处理。

激光Lidar关键帧的判别根据距离信息以及旋转角度大小，运动超过5m或者旋转超过10°会被认为关键帧。

激光Lidar扫描频率在10-30HZ之间，IMU接受数据的频率在100-500之间，为了防止重复积分，节约计算量，需要对雷达关键帧数据之间的IMU加速度数据，和角速度数据进行积分处理(公式1)，得到激光Lidar关键帧之间的位置、速度、和旋转约束。也就是IMU约束因子。

优选地，所述基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息，包括：

判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；

利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；

基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。

可选地，本实施例中第四个流程为：基于去畸变的激光Lidar数据进行地面点的去除，也就是将Z坐标设置一个高程阈值，小于0.2米的点云进行删除，减少点云数据，减少计算量，为下一步增加匹配速度做准备。

进一步地，本实施例中第五个流程为：利用当前去除地面点的激光Lidar点云数据与激光Lidar关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到位姿变换(R,T)(Lidar约束因子)。因为两帧之间点云进行匹配鲁棒性不好，因此本发明使用关键帧子集的概念，即：使用当前帧之前的连续时间的几个关键帧数据形成的点云与当前关键帧点云进行ICP匹配，提高匹配的鲁棒性。

具体的，本实施例中第六个流程为：基于激光Lidar约束因子与IMU约束因子联合优化计算得到第一位姿；

r

优选地，所述基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息，包括：

基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。

具体的，本实施例中第七个流程为：在有UWB信号的地方进行基于UWB信号进行基于TDOP(Time Difference ofArrival)算法计算得到第二位姿信息，形成基于UWB的位姿约束(UWB约束因子)；

优选地，所述基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架，包括：

基于七参数模型对所述第一位姿信息进行转换，得到所述第二位姿信息坐标框架。

具体的，本实施例中第八个流程为：对第一位姿信息通过7参数转换将基于激光Lidar的坐标信息转换为第二位姿信息坐标框架；

[X

本实施例中的第九个流程为：在UWB坐标系框架下，根据IMU预积分因子、激光Lidar因子，UWB因子，回环检测因子(有闭环如公式6，无闭环如公式5)，构建误差方程，当检测不出来UWB信号时，重新使用公式3，利用图优化工具ceres-solver进行实施位姿求解。为保持计算效率，本发明使用基于滑动窗口的方式进行优化，保持十个激光Lidar关键帧及这一时间段的IMU预积分以及UWB数据构建的误差方程。(局部滑动窗口优化)

上式r

优选地，所述利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息，包括：

基于图优化工具对所述载体的位姿进行全局位姿优化，得到所述优化求解后的位姿信息。

可选地，本实施例中的第十个流程为：单独开辟一个线程，ceres-solver进行一个全局位姿优化，根据全局优化求解的位姿信息。

进一步地，本实施例中的最后一个流程为：对关键帧的激光点云数据进行坐标转换，即可得到以UWB坐标系下的室内三维点云模型(稠密3D地图)。

本实施例中还公开了一种UWB/激光Lidar/IMU融合的室内定位与建图装置，其中具体包括：

IMU处理单元，位于所述载体的外表面或内部，用于实时获取所述的IMU数据，利用该数据对激光雷达点云数据进行去畸变，并对所述IMU数据做预积分处理。

激光雷达处理单元，位于所述载体的外表面，用于实时获取所述激光雷达点云数据，利于该数据进行点云匹配并与IMU的预积分误差方程共同构建误差方程，进行联合优化，得到载体的第一位姿信息。

UWB处理单元，位于所述载体所在空间内，用于获取UWB信号，利用所述UWB信号计算所述载体的第二位姿信息；利用UWB的位姿信息，对第一载体位姿信息进行坐标变换，统一到UWB坐标框架内。其次利用UWB计算的位姿信息辅助激光SLAM进行回环检测并构建所述第一位姿信息和所述第二位姿信息以及回环检测的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息。

本实施例中还公开了一种室内定位与建图系统，包括：

获取模块，用于获取目标空间内的激光雷达数据和惯性传感器数据；

预处理模块，用于分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；

第一位姿计算模块，用于基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；

第二位姿计算模块，用于基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；

坐标转换模块，用于基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；

求解模块，用于基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；

配准模块，用于根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。

优选地，所述预处理模块具体包括：

记录单元，用于记录数据采集周期内惯性传感器的运动轨迹；

坐标确定单元，用于利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；

转换单元，用于通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；

预积分处理单元，用于对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：

所述惯性传感器数据约束因子的公式为：

优选地，所述基于所述第一位姿计算模块具体包括：

判断单元，用于判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；

筛选单元，用于利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；

第一计算单元，用于基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。

优选地，所述第二位姿计算模块具体包括：

第二计算单元，用于基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。

本发明的有益效果如下：

1)将UWB与激光SLAM相结合，利用激光SLAM在短时间内的高精度定位优势，UWB的定位精度不随时间变化的优势，利用滑动窗口优化该组合，实现了在短时间或长时间内移动机器人在所处空间内较高的实时定位精度，且不随时间的变化产生累计误差，不会对定位精度产生较大影响。

2)UWB能够辅助激光SLAM技术进行回环检测并进行累计误差的消除，提升实时定位的精度。

3)UWB能够辅助激光SLAM技术进行坐标系的转换，将激光SLAM的局部坐标系转化为大场景的基于场景布设的UWB的稳定的坐标系框架。

4)与IMU融合的激光SLAM系统融合UWB设备能够实现大场景的高精度二维或者三维地图的制作，为室内等轻量级设备定位导航提供数据基础。

5)仅需在空间内关键位置布设少量的UWB处理模块即可实现对空间内移动机器人的高精度定位，进而实现对空间内环境的高精度定位，其布设所需面积以及建设成本较小。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。