CN114674311A - 一种室内定位与建图方法及系统 - Google Patents

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

Info

Publication number
CN114674311A
CN114674311A CN202210285226.5A CN202210285226A CN114674311A CN 114674311 A CN114674311 A CN 114674311A CN 202210285226 A CN202210285226 A CN 202210285226A CN 114674311 A CN114674311 A CN 114674311A
Authority
CN
China
Prior art keywords
data
laser radar
frame
inertial sensor
pose
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210285226.5A
Other languages
English (en)
Inventor
刘振彬
赵玉玲
李增科
业迟星
陈王琪
胡一帆
陈昭冰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China University of Mining and Technology CUMT
Original Assignee
China University of Mining and Technology CUMT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China University of Mining and Technology CUMT filed Critical China University of Mining and Technology CUMT
Priority to CN202210285226.5A priority Critical patent/CN114674311A/zh
Publication of CN114674311A publication Critical patent/CN114674311A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/383—Indoor data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3848—Data obtained from both position sensors and additional sensors
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G06T5/80—
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Abstract

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

Description

一种室内定位与建图方法及系统
技术领域
本发明涉及实时定位与建图技术技术领域，特别是涉及一种室内定位与建图方法及系统。
背景技术
激光雷达SLAM技术(Simultaneous Localization and Mapping，SLAM)用于移动机器人只依赖自身的激光Lidar传感器进行自我定位以及对环境的感知。激光雷达SLAM技术一般与IMU(Inertial Measurement Unit，IMU)联合使用，IMU传感器主要用于激光点云的矫正，解决运动过程中的雷达点云产生的畸变问题。
然而，利用现有的激光SLAM技术在在短时间内能够保证移动机器人的较高定位精度，若长时间运行，以及在结构单一的走廊环境将不可避免地产生定位累计误差和回环检测失败，导致长时间定位时定位精度的大幅下降。
发明内容
为了克服现有技术的不足，本发明的目的是提供一种室内定位与建图方法及系统。
为实现上述目的，本发明提供了如下方案：
一种室内定位与建图方法，包括：
获取目标空间内的激光雷达数据和惯性传感器数据；
分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；
基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；
基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；
基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；
基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；
根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。
优选地，所述分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据，包括：
记录数据采集周期内惯性传感器的运动轨迹；
利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；
对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：
Figure BDA0003557935300000021
其中，，
Figure BDA0003557935300000022
和
Figure BDA0003557935300000023
分别表示惯性传感器测量的加速度与角速度测量值，i和i+1分别表示第k个关键帧k和第k+1个关键帧之间惯性测量数据的两个时刻，δt表示两个惯性测量数据之间的时间间隔，
Figure BDA0003557935300000024
Figure BDA0003557935300000031
分别代表加速度零偏和角速度零偏。
Figure BDA0003557935300000032
和
Figure BDA0003557935300000033
分别代表基于bk关键帧坐标系下的位置、速度和四元数表示的姿态；
所述惯性传感器数据约束因子的公式为：
Figure BDA0003557935300000034
其中，
Figure BDA0003557935300000035
为位置和速度的三维零向量；
Figure BDA0003557935300000036
是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]T；
Figure BDA0003557935300000037
表示世界坐标系到bk关键帧的旋转矩阵；
Figure BDA0003557935300000038
表示bk关键帧在世界坐标系的位置；Δtk表示bk与bk+1关键帧的时间差；
Figure BDA0003557935300000039
表示bk关键帧在世界坐标系下的速度；
Figure BDA00035579353000000310
表示
Figure BDA00035579353000000311
的逆矩阵，世界坐标系到bk关键帧的四元数旋转；
Figure BDA00035579353000000312
表示bk+1关键帧到世界坐标系的旋转的四元数表示；
Figure BDA00035579353000000313
表示第k帧的加速度零偏；
Figure BDA00035579353000000314
表示第k帧的角速度零偏；所述惯性传感器数据约束因子为所述预积分处理后的数据。
优选地，所述基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息，包括：
判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；
利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；
基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。
优选地，所述基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息，包括：
基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。
优选地，所述基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架，包括：
基于七参数模型对所述第一位姿信息进行转换，得到所述第二位姿信息坐标框架。
优选地，所述利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息，包括：
基于图优化工具对所述载体的位姿进行全局位姿优化，得到所述优化求解后的位姿信息。
一种室内定位与建图系统，包括：
获取模块，用于获取目标空间内的激光雷达数据和惯性传感器数据；
预处理模块，用于分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；
第一位姿计算模块，用于基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；
第二位姿计算模块，用于基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；
坐标转换模块，用于基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；
求解模块，用于基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；
配准模块，用于根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。
优选地，所述预处理模块具体包括：
记录单元，用于记录数据采集周期内惯性传感器的运动轨迹；
坐标确定单元，用于利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
转换单元，用于通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；
预积分处理单元，用于对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：
Figure BDA0003557935300000051
其中，，
Figure BDA0003557935300000052
和
Figure BDA0003557935300000053
分别表示惯性传感器测量的加速度与角速度测量值，i和i+1分别表示第k个关键帧k和第k+1个关键帧之间惯性测量数据的两个时刻，δt表示两个惯性测量数据之间的时间间隔，
Figure BDA0003557935300000054
Figure BDA0003557935300000055
分别代表加速度零偏和角速度零偏。
Figure BDA0003557935300000056
和
Figure BDA0003557935300000057
分别代表基于bk关键帧坐标系下的位置、速度和四元数表示的姿态；
所述惯性传感器数据约束因子的公式为：
Figure BDA0003557935300000058
其中，
Figure BDA0003557935300000059
为位置和速度的三维零向量；
Figure BDA00035579353000000510
是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]T；
Figure BDA00035579353000000511
表示世界坐标系到bk关键帧的旋转矩阵；
Figure BDA00035579353000000512
表示bk关键帧在世界坐标系的位置；Δtk表示bk与bk+1关键帧的时间差；
Figure BDA00035579353000000513
表示bk关键帧在世界坐标系下的速度；
Figure BDA00035579353000000514
表示
Figure BDA00035579353000000515
的逆矩阵，世界坐标系到bk关键帧的四元数旋转；
Figure BDA00035579353000000516
表示bk+1关键帧到世界坐标系的旋转的四元数表示；
Figure BDA00035579353000000517
表示第k帧的加速度零偏；
Figure BDA00035579353000000518
表示第k帧的角速度零偏；所述惯性传感器数据约束因子为所述预积分处理后的数据。
优选地，所述基于所述第一位姿计算模块具体包括：
判断单元，用于判断所述去畸变激光雷达数据的中的各个点云数据的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点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；
对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：
Figure BDA0003557935300000091
其中，，
Figure BDA0003557935300000092
和
Figure BDA0003557935300000093
分别表示惯性传感器测量的加速度与角速度测量值，i和i+1分别表示第k个关键帧k和第k+1个关键帧之间惯性测量数据的两个时刻，δt表示两个惯性测量数据之间的时间间隔，
Figure BDA0003557935300000094
Figure BDA0003557935300000095
分别代表加速度零偏和角速度零偏。
Figure BDA0003557935300000096
和
Figure BDA0003557935300000097
分别代表基于bk关键帧坐标系下的位置、速度和四元数表示的姿态；
所述惯性传感器数据约束因子的公式为：
Figure BDA0003557935300000101
其中，
Figure BDA0003557935300000102
为位置和速度的三维零向量；
Figure BDA0003557935300000103
是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]T；
Figure BDA0003557935300000104
表示世界坐标系到bk关键帧的旋转矩阵；
Figure BDA0003557935300000105
表示bk关键帧在世界坐标系的位置；Δtk表示bk与bk+1关键帧的时间差；
Figure BDA0003557935300000106
表示bk关键帧在世界坐标系下的速度；
Figure BDA0003557935300000107
表示
Figure BDA0003557935300000108
的逆矩阵，世界坐标系到bk关键帧的四元数旋转；
Figure BDA0003557935300000109
表示bk+1关键帧到世界坐标系的旋转的四元数表示；
Figure BDA00035579353000001010
表示第k帧的加速度零偏；
Figure BDA00035579353000001011
表示第k帧的角速度零偏；所述惯性传感器数据约束因子为所述预积分处理后的数据。
具体的，本实施例中第二个流程为对激光Lidar数据进行去畸变处理，需要对2D激光雷达点云数据进行去畸变处理，记录数据采集周期内IMU的运动轨迹；利用所述时间戳和IMU的运动轨迹得到在采集各个所述点云时激光雷达的位置和姿态；通过外参将得到的每帧点云的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，即实现了点云去畸变。
进一步地，本实施例中第三个流程为对激光Lidar关键帧间的IMU点云数据进行预积分处理，对应下文预积分因子；
本发明为减少计算量和计算机存储空间只存储激光关键帧数据；单线程激光Lidar每扫描一圈为一帧数据，有时候激光Lidar并没有运动，或者运动距离很少，就会造成数据冗余，造成不必要的计算，基于此，下文位姿优化与局部建图过程只对激光Lidar关键帧数据进行处理。
激光Lidar关键帧的判别根据距离信息以及旋转角度大小，运动超过5m或者旋转超过10°会被认为关键帧。
激光Lidar扫描频率在10-30HZ之间，IMU接受数据的频率在100-500之间，为了防止重复积分，节约计算量，需要对雷达关键帧数据之间的IMU加速度数据，和角速度数据进行积分处理(公式1)，得到激光Lidar关键帧之间的位置、速度、和旋转约束。也就是IMU约束因子。
Figure BDA0003557935300000111
Figure BDA0003557935300000112
表示IMU测量的加速度与角速度测量值，i，i+1，表示关键帧k和k+1之间IMU测量数据的两个时刻，δt表示两个IMU数据之间的时间间隔，
Figure BDA0003557935300000113
Figure BDA0003557935300000114
分别代表加速度零偏和角速度零偏。
Figure BDA0003557935300000115
代表基于bk关键帧坐标系下的位置，速度和四元数表示的姿态。两关键帧之间预积分开始时刻，
Figure BDA0003557935300000116
是3维零向量，
Figure BDA0003557935300000117
是单位四元数。由预积分项构建的测量模型如公式2，其中gw表示世界坐标系下的重力向量gw＝[0,0,g]T。
Figure BDA0003557935300000118
优选地，所述基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息，包括：
判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；
利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；
基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。
可选地，本实施例中第四个流程为：基于去畸变的激光Lidar数据进行地面点的去除，也就是将Z坐标设置一个高程阈值，小于0.2米的点云进行删除，减少点云数据，减少计算量，为下一步增加匹配速度做准备。
进一步地，本实施例中第五个流程为：利用当前去除地面点的激光Lidar点云数据与激光Lidar关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到位姿变换(R,T)(Lidar约束因子)。因为两帧之间点云进行匹配鲁棒性不好，因此本发明使用关键帧子集的概念，即：使用当前帧之前的连续时间的几个关键帧数据形成的点云与当前关键帧点云进行ICP匹配，提高匹配的鲁棒性。
具体的，本实施例中第六个流程为：基于激光Lidar约束因子与IMU约束因子联合优化计算得到第一位姿；
Figure BDA0003557935300000121
rL，rB分别代表抽象形式的误差模型(Lidar约束因子与IMU约束因子)，通过调整姿态X(R,T)，使得整个误差最小，此时的X(R,T)就是第一位姿的最优解。
优选地，所述基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息，包括：
基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。
具体的，本实施例中第七个流程为：在有UWB信号的地方进行基于UWB信号进行基于TDOP(Time Difference ofArrival)算法计算得到第二位姿信息，形成基于UWB的位姿约束(UWB约束因子)；
Figure BDA0003557935300000122
(x,y,z)表示移动站坐标，待求量，(xi,yi,zi)表示固定基站坐标为已知量，n是测量噪声，di,1表示移动站到观测站的测量距离，三个未知数，建立三个观测方程即可求解移动站的三维坐标，当接收的UWB基站信号多于三个时，可采用最小二乘法进行求解。
优选地，所述基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架，包括：
基于七参数模型对所述第一位姿信息进行转换，得到所述第二位姿信息坐标框架。
具体的，本实施例中第八个流程为：对第一位姿信息通过7参数转换将基于激光Lidar的坐标信息转换为第二位姿信息坐标框架；
Figure BDA0003557935300000131
[XU YU ZU]T,[XL YL ZL]T分别表示同一位置在UWB坐标系，激光Lidar定义的世界坐标系下的坐标。m代表比例因子，R代表激光Lidar定义的世界坐标系到UWB坐标系下的转换关系，[ΔX0ΔY0ΔZ0]T激光Lidar定义的世界坐标系的原点在UWB下的坐标。[XU YU ZU]T,[XLYL ZL]T已知，其余量未知，通过三个点的两套坐标即可将剩余量计算出来，多余的数据可以构建最小二乘求解。计算出七参数，将Lidar与IMU计算的第一位姿根据七参数全部转化到UWB坐标系下的坐标。
本实施例中的第九个流程为：在UWB坐标系框架下，根据IMU预积分因子、激光Lidar因子，UWB因子，回环检测因子(有闭环如公式6，无闭环如公式5)，构建误差方程，当检测不出来UWB信号时，重新使用公式3，利用图优化工具ceres-solver进行实施位姿求解。为保持计算效率，本发明使用基于滑动窗口的方式进行优化，保持十个激光Lidar关键帧及这一时间段的IMU预积分以及UWB数据构建的误差方程。(局部滑动窗口优化)
Figure BDA0003557935300000132
Figure BDA0003557935300000133
上式rU代表UWB误差方程，rC代表激光雷达当前关键帧与数据库存储的关键帧进行匹配形成的闭环约束。通过最小化上述方程得到最优的位姿估计。
优选地，所述利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息，包括：
基于图优化工具对所述载体的位姿进行全局位姿优化，得到所述优化求解后的位姿信息。
可选地，本实施例中的第十个流程为：单独开辟一个线程，ceres-solver进行一个全局位姿优化，根据全局优化求解的位姿信息。
进一步地，本实施例中的最后一个流程为：对关键帧的激光点云数据进行坐标转换，即可得到以UWB坐标系下的室内三维点云模型(稠密3D地图)。
本实施例中还公开了一种UWB/激光Lidar/IMU融合的室内定位与建图装置，其中具体包括：
IMU处理单元，位于所述载体的外表面或内部，用于实时获取所述的IMU数据，利用该数据对激光雷达点云数据进行去畸变，并对所述IMU数据做预积分处理。
激光雷达处理单元，位于所述载体的外表面，用于实时获取所述激光雷达点云数据，利于该数据进行点云匹配并与IMU的预积分误差方程共同构建误差方程，进行联合优化，得到载体的第一位姿信息。
UWB处理单元，位于所述载体所在空间内，用于获取UWB信号，利用所述UWB信号计算所述载体的第二位姿信息；利用UWB的位姿信息，对第一载体位姿信息进行坐标变换，统一到UWB坐标框架内。其次利用UWB计算的位姿信息辅助激光SLAM进行回环检测并构建所述第一位姿信息和所述第二位姿信息以及回环检测的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息。
本实施例中还公开了一种室内定位与建图系统，包括：
获取模块，用于获取目标空间内的激光雷达数据和惯性传感器数据；
预处理模块，用于分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；
第一位姿计算模块，用于基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；
第二位姿计算模块，用于基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；
坐标转换模块，用于基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；
求解模块，用于基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；
配准模块，用于根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。
优选地，所述预处理模块具体包括：
记录单元，用于记录数据采集周期内惯性传感器的运动轨迹；
坐标确定单元，用于利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
转换单元，用于通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；
预积分处理单元，用于对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：
Figure BDA0003557935300000151
其中，，
Figure BDA0003557935300000152
和
Figure BDA0003557935300000153
分别表示惯性传感器测量的加速度与角速度测量值，i和i+1分别表示第k个关键帧k和第k+1个关键帧之间惯性测量数据的两个时刻，δt表示两个惯性测量数据之间的时间间隔，
Figure BDA0003557935300000154
Figure BDA0003557935300000155
分别代表加速度零偏和角速度零偏。
Figure BDA0003557935300000156
和
Figure BDA0003557935300000157
分别代表基于bk关键帧坐标系下的位置、速度和四元数表示的姿态；
所述惯性传感器数据约束因子的公式为：
Figure BDA0003557935300000161
其中，
Figure BDA0003557935300000162
为位置和速度的三维零向量；
Figure BDA0003557935300000163
是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]T；
Figure BDA0003557935300000164
表示世界坐标系到bk关键帧的旋转矩阵；
Figure BDA0003557935300000165
表示bk关键帧在世界坐标系的位置；Δtk表示bk与bk+1关键帧的时间差；
Figure BDA0003557935300000166
表示bk关键帧在世界坐标系下的速度；
Figure BDA0003557935300000167
表示
Figure BDA0003557935300000168
的逆矩阵，世界坐标系到bk关键帧的四元数旋转；
Figure BDA0003557935300000169
表示bk+1关键帧到世界坐标系的旋转的四元数表示；
Figure BDA00035579353000001610
表示第k帧的加速度零偏；
Figure BDA00035579353000001611
表示第k帧的角速度零偏；所述惯性传感器数据约束因子为所述预积分处理后的数据。
优选地，所述基于所述第一位姿计算模块具体包括：
判断单元，用于判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；
筛选单元，用于利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；
第一计算单元，用于基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。
优选地，所述第二位姿计算模块具体包括：
第二计算单元，用于基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。
本发明的有益效果如下：
1)将UWB与激光SLAM相结合，利用激光SLAM在短时间内的高精度定位优势，UWB的定位精度不随时间变化的优势，利用滑动窗口优化该组合，实现了在短时间或长时间内移动机器人在所处空间内较高的实时定位精度，且不随时间的变化产生累计误差，不会对定位精度产生较大影响。
2)UWB能够辅助激光SLAM技术进行回环检测并进行累计误差的消除，提升实时定位的精度。
3)UWB能够辅助激光SLAM技术进行坐标系的转换，将激光SLAM的局部坐标系转化为大场景的基于场景布设的UWB的稳定的坐标系框架。
4)与IMU融合的激光SLAM系统融合UWB设备能够实现大场景的高精度二维或者三维地图的制作，为室内等轻量级设备定位导航提供数据基础。
5)仅需在空间内关键位置布设少量的UWB处理模块即可实现对空间内移动机器人的高精度定位，进而实现对空间内环境的高精度定位，其布设所需面积以及建设成本较小。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种室内定位与建图方法，其特征在于，包括：
获取目标空间内的激光雷达数据和惯性传感器数据；
分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；
基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；
基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；
基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；
基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；
根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。
2.根据权利要求1所述的室内定位与建图方法，其特征在于，所述分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据，包括：
记录数据采集周期内惯性传感器的运动轨迹；
利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；
对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：
Figure FDA0003557935290000021
其中，，
Figure FDA0003557935290000022
和
Figure FDA0003557935290000023
分别表示惯性传感器测量的加速度与角速度测量值，i和i+1分别表示第k个关键帧k和第k+1个关键帧之间惯性测量数据的两个时刻，δt表示两个惯性测量数据之间的时间间隔，
Figure FDA0003557935290000024
Figure FDA0003557935290000025
分别代表加速度零偏和角速度零偏。
Figure FDA0003557935290000026
和
Figure FDA0003557935290000027
分别代表基于bk关键帧坐标系下的位置、速度和四元数表示的姿态；
所述惯性传感器数据约束因子的公式为：
Figure FDA0003557935290000028
其中，
Figure FDA0003557935290000029
为位置和速度的三维零向量；
Figure FDA00035579352900000210
是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]T；
Figure FDA00035579352900000211
表示世界坐标系到bk关键帧的旋转矩阵；
Figure FDA00035579352900000212
表示bk关键帧在世界坐标系的位置；Δtk表示bk与bk+1关键帧的时间差；
Figure FDA00035579352900000213
表示bk关键帧在世界坐标系下的速度；
Figure FDA00035579352900000214
表示
Figure FDA00035579352900000215
的逆矩阵，世界坐标系到bk关键帧的四元数旋转；
Figure FDA00035579352900000216
表示bk+1关键帧到世界坐标系的旋转的四元数表示；
Figure FDA00035579352900000217
表示第k帧的加速度零偏；
Figure FDA00035579352900000218
表示第k帧的角速度零偏；所述惯性传感器数据约束因子为所述预积分处理后的数据。
3.根据权利要求1所述的室内定位与建图方法，其特征在于，所述基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息，包括：
判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；
利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；
基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。
4.根据权利要求1所述的室内定位与建图方法，其特征在于，所述基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息，包括：
基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。
5.根据权利要求1所述的室内定位与建图方法，其特征在于，所述基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架，包括：
基于七参数模型对所述第一位姿信息进行转换，得到所述第二位姿信息坐标框架。
6.根据权利要求1所述的室内定位与建图方法，其特征在于，所述利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息，包括：
基于图优化工具对所述载体的位姿进行全局位姿优化，得到所述优化求解后的位姿信息。
7.一种室内定位与建图系统，其特征在于，包括：
获取模块，用于获取目标空间内的激光雷达数据和惯性传感器数据；
预处理模块，用于分别对所述激光雷达数据和所述激光雷达数据关键帧间的惯性传感器数据进行数据预处理，得到去畸变激光雷达数据和预积分处理后的数据；
第一位姿计算模块，用于基于所述去畸变激光雷达数据和所述预积分处理后的数据计算载体的位姿，得到所述载体的第一位姿信息；
第二位姿计算模块，用于基于获取到的超宽带信号计算所述载体的位姿，得到所述载体的第二位姿信息；
坐标转换模块，用于基于所述第二位姿信息对所述第一位姿信息进行坐标变换，得到基于超宽带信号坐标的第二位姿信息坐标框架；
求解模块，用于基于所述第二位姿信息坐标框架，根据所述去畸变激光雷达数据、所述预积分处理后的数据、所述第一位姿信息和所述第二位姿信息构建联合误差方程，并利用所述联合误差方程对所述载体的位姿进行优化求解，得到优化求解后的位姿信息；
配准模块，用于根据所述优化求解后的位姿信息对所述所述激光雷达数据关键帧间的激光点云数据进行配准，生成室内的三维点云地图。
8.根据权利要求7所述的室内定位与建图系统，其特征在于，所述预处理模块具体包括：
记录单元，用于记录数据采集周期内惯性传感器的运动轨迹；
坐标确定单元，用于利用时间戳和所述惯性传感器的运动轨迹确定在采集各个所述激光雷达数据时激光雷达的位置和姿态，得到每帧所述激光雷达数据的坐标值；
转换单元，用于通过外参将得到的每帧所述激光雷达数据的坐标值转换至该帧点云中第一个2D点采集时刻的激光雷达坐标系下，得到转换后的所述去畸变激光雷达数据；
预积分处理单元，用于对激光雷达关键帧间的惯性传感器数据进行预积分处理，得到惯性传感器数据约束因子；所述惯性传感器数据约束因子包括：激光雷达关键帧之间的位置、速度和旋转约束；所述预积分处理的公式为：
Figure FDA0003557935290000041
其中，，
Figure FDA0003557935290000042
和
Figure FDA0003557935290000043
分别表示惯性传感器测量的加速度与角速度测量值，i和i+1分别表示第k个关键帧k和第k+1个关键帧之间惯性测量数据的两个时刻，δt表示两个惯性测量数据之间的时间间隔，
Figure FDA0003557935290000051
Figure FDA0003557935290000052
分别代表加速度零偏和角速度零偏。
Figure FDA0003557935290000053
和
Figure FDA0003557935290000054
分别代表基于bk关键帧坐标系下的位置、速度和四元数表示的姿态；
所述惯性传感器数据约束因子的公式为：
Figure FDA0003557935290000055
其中，
Figure FDA0003557935290000056
为位置和速度的三维零向量；
Figure FDA0003557935290000057
是单位四元数；gw表示世界坐标系下的重力向量gw＝[0,0,g]T；
Figure FDA0003557935290000058
表示世界坐标系到bk关键帧的旋转矩阵；
Figure FDA0003557935290000059
表示bk关键帧在世界坐标系的位置；Δtk表示bk与bk+1关键帧的时间差；
Figure FDA00035579352900000510
表示bk关键帧在世界坐标系下的速度；
Figure FDA00035579352900000511
表示
Figure FDA00035579352900000512
的逆矩阵，世界坐标系到bk关键帧的四元数旋转；
Figure FDA00035579352900000513
表示bk+1关键帧到世界坐标系的旋转的四元数表示；
Figure FDA00035579352900000514
表示第k帧的加速度零偏；
Figure FDA00035579352900000515
表示第k帧的角速度零偏；所述惯性传感器数据约束因子为所述预积分处理后的数据。
9.根据权利要求7所述的室内定位与建图系统，其特征在于，所述基于所述第一位姿计算模块具体包括：
判断单元，用于判断所述去畸变激光雷达数据的中的各个点云数据的Z轴坐标是否小于预设高度阈值，若是，则对所述点云数据进行删除；
筛选单元，用于利用删除后的所述去畸变激光雷达数据与激光雷达关键帧子集生成的三维点云地图进行基于ICP算法的匹配，得到激光雷达约束因子；
第一计算单元，用于基于所述激光雷达约束因子和所述预积分处理后的数据进行联合优化计算，得到所述第一位姿信息。
10.根据权利要求7所述的室内定位与建图系统，其特征在于，所述第二位姿计算模块具体包括：
第二计算单元，用于基于TDOP算法，根据所述超宽带信号计算得到所述第二位姿信息。
CN202210285226.5A 2022-03-22 2022-03-22 一种室内定位与建图方法及系统 Pending CN114674311A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210285226.5A CN114674311A (zh) 2022-03-22 2022-03-22 一种室内定位与建图方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210285226.5A CN114674311A (zh) 2022-03-22 2022-03-22 一种室内定位与建图方法及系统

Publications (1)

Publication Number Publication Date
CN114674311A true CN114674311A (zh) 2022-06-28

Family

ID=82074368

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210285226.5A Pending CN114674311A (zh) 2022-03-22 2022-03-22 一种室内定位与建图方法及系统

Country Status (1)

Country Link
CN (1) CN114674311A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115164887A (zh) * 2022-08-30 2022-10-11 中国人民解放军国防科技大学 基于激光雷达与惯性组合的行人导航定位方法和装置
WO2024041464A1 (zh) * 2022-08-22 2024-02-29 先临三维科技股份有限公司 回环路径的预测方法及装置、非易失性存储介质、处理器

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024041464A1 (zh) * 2022-08-22 2024-02-29 先临三维科技股份有限公司 回环路径的预测方法及装置、非易失性存储介质、处理器
CN115164887A (zh) * 2022-08-30 2022-10-11 中国人民解放军国防科技大学 基于激光雷达与惯性组合的行人导航定位方法和装置

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
Zhao et al. A robust laser-inertial odometry and mapping method for large-scale highway environments
CN114674311A (zh) 一种室内定位与建图方法及系统
CN103412565B (zh) 一种具有全局位置快速估计能力的机器人定位方法
CN113781582A (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN109443351A (zh) 一种稀疏环境下的机器人三维激光定位方法
CN112950781A (zh) 特种场景的多传感器动态加权融合的点云地图构建方法
CN109648558B (zh) 机器人曲面运动定位方法及其运动定位系统
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN114608561B (zh) 一种基于多传感器融合的定位与建图方法及系统
Fang et al. A real-time 3d perception and reconstruction system based on a 2d laser scanner
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN113252033A (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113674412A (zh) 基于位姿融合优化的室内地图构建方法、系统及存储介质
CN115183762A (zh) 一种机场仓库内外建图方法、系统、电子设备及介质
CN115046540A (zh) 一种点云地图构建方法、系统、设备和存储介质
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
Wang et al. A robust lidar state estimation and map building approach for urban road
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN116608873A (zh) 一种自动驾驶车辆的多传感器融合定位建图方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination