CN113570715A - 基于传感器融合的旋转激光实时定位建模系统及方法 - Google Patents

基于传感器融合的旋转激光实时定位建模系统及方法 Download PDF

Info

Publication number
CN113570715A
CN113570715A CN202110835171.6A CN202110835171A CN113570715A CN 113570715 A CN113570715 A CN 113570715A CN 202110835171 A CN202110835171 A CN 202110835171A CN 113570715 A CN113570715 A CN 113570715A
Authority
CN
China
Prior art keywords
laser
imu
laser radar
data
dimensional
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.)
Granted
Application number
CN202110835171.6A
Other languages
English (en)
Other versions
CN113570715B (zh
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.)
Northeastern University China
63983 Troops of PLA
Original Assignee
Northeastern University China
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 Northeastern University China filed Critical Northeastern University China
Priority to CN202110835171.6A priority Critical patent/CN113570715B/zh
Publication of CN113570715A publication Critical patent/CN113570715A/zh
Application granted granted Critical
Publication of CN113570715B publication Critical patent/CN113570715B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/003—Navigation within 3D models or images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/20—Editing of 3D images, e.g. changing shapes or colours, aligning objects or positioning parts
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G06T7/85—Stereo camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2200/00—Indexing scheme for image data processing or generation, in general
    • G06T2200/08—Indexing scheme for image data processing or generation, in general involving all processing steps from image acquisition to 3D model generation
    • 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/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2219/00—Indexing scheme for manipulating 3D models or images for computer graphics
    • G06T2219/20—Indexing scheme for editing of 3D models
    • G06T2219/2004—Aligning objects, relative positioning of parts
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2219/00—Indexing scheme for manipulating 3D models or images for computer graphics
    • G06T2219/20—Indexing scheme for editing of 3D models
    • G06T2219/2016—Rotation, translation, scaling
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Graphics (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Geometry (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Architecture (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开一种基于传感器融合的旋转激光实时定位建模系统及方法，系统包括三维激光雷达、IMU惯性测量单元、激光雷达驱动器、云台电机、感知处理单元、电源模块、通信模块和底层控制器，通过电路连接组成的一个小型三维建模系统，系统可实现实时移动式旋转建模。系统方法是一种融合IMU信息的，旋转激光雷达的实时定位和建图方法，通过融合三维激光雷达和IMU数据，实现激光雷达点云的有效去畸变处理。同时通过将三维激光进行旋转的方式，扩大激光雷达的有效视野，提高视野特征提取和匹配的可靠性，同时增加地图点云密度，基于鲁棒、准确的位姿估计，从而可以实时构建周围环境稠密、高精度的点云地图。

Description

基于传感器融合的旋转激光实时定位建模系统及方法
技术领域
本发明涉及机器人建模技术领域，尤其涉及一种基于传感器融合的旋转激光实时定位建模系统及方法。
背景技术
对于移动机器人的自主导航，机器人首先需要对所处环境进行感知，其中重要的是机器人可以知道其在环境中的位置，并且感知周围环境的状态，机器人的即时定位和建图，是这一能力的实现技术。机器人的即时定位和建图，也即SLAM(SimultaneousLocalization and Mapping)，是指机器人通过其自身携带的传感器获取周围环境的状态信息，将获得的传感器状态信息进行计算处理，可以估计出机器人在环境中的位置姿态，同时获得机器人所处周围环境的情况。
对于机器人即时定位和建图技术的研究，根据采用传感器的不同，可以分为基于视觉传感器和基于激光传感器的方法。基于视觉传感器的SLAM方法，具有的优点是视觉摄像头体积小、成本低，在结构特征少的场景也可以有良好的效果，然而视觉SLAM具有受光照变化影响大，在弱纹理情况下容易失败的缺点。基于激光雷达的SLAM方法，可以应对光照变化的影响，在弱纹理条件下可靠工作的特点，然而其具有传感器体积大，在结构特征少的情况下容易失败的缺点。
基于激光雷达的即时定位和建图方法的研究，已有一些文献和专利进行了研究说明。中国专利“CN107462897A基于激光雷达的三维建图的方法”提出一种基于二维激光雷达的三维建图方法，其仅使用二维激光雷达和电机的组合，就可以完成实时三维点云地图的建立，可用于移动机器人对地形数据的测绘。该方法在环境特征少的情况下，单纯使用二维激光点云进行匹配，由于激光本身数量比较少，会出现相应特征点无法正确匹配的情况，从而使得建图失败。中国专利“CN106652028A一种环境三维建图方法及装置”提出了一种环境三维建图方法及装置，使用二维测距传感器和三维测距传感器构建三维环境地图，利用二维测距传感器得到可移动装置在其所处环境的位姿，利用三维测距传感器采集所处环境的三维环境信息，从而避免了进行三维特征提取和特征匹配，减少了计算量。由于该装置和方法也只使用二维激光器来进行位姿估计，其同样也会在特征比较少的场景下出现匹配失败的情况，因此也会受限于本身二维激光测距器的缺陷。
因此，目前的研究仍然存在一些问题，由于激光传感器数据频率低，使得在快速运动和大幅度旋转情况下激光雷达接收的点云数据具有较大的运动畸变，使得运动估计出错，同时对于狭长场景中，由于激光雷达传感器的视野有限，单纯的使用激光雷达进行定位和建图，容易因特征缺少出现匹配失败的情况的，从而导致整个定位和建图模块失败。
发明内容
针对上述现有技术的不足，本发明提供一种基于传感器融合的旋转激光实时定位建模系统及方法。
为解决上述技术问题，本发明所采取的技术方案是：一种基于传感器融合的旋转激光实时定位建模系统，系统包括三维激光雷达、IMU惯性测量单元、激光雷达驱动器、云台电机、感知处理单元、电源模块、通信模块和底层控制器；
所述三维激光雷达与IMU惯性测量单元固定连接后利用连接件与所述云台电机进行连接，使得云台电机带动固定连接好的三维激光雷达与IMU惯性测量单元一起旋转；
所述激光雷达驱动器负责将激光雷达原始数据处理，然后通过RJ45接口发送给感知处理单元；
所述感知处理单元通过其内部运行的定位与建图系统算法程序，对三维激光雷达与IMU惯性测量单元的数据进行处理，并通过通信模块采用无线通讯的方式实现与远程交互端的信息交互；
所述底层控制器包括微控制单片机和电平转换模块，微控制单片机的控制信号经电平转换模块转换后发送到云台电机，实现对云台电机的工作模式以及具体参数的控制；
所述电源模块通过外接直流源来实现，并通过降压处理模块实现对三维激光雷达、云台电机、通信模块的供电。
进一步的，所述感知处理单元、激光雷达驱动器与固定连接好的三维激光雷达与IMU惯性测量单元之间通过滑环连接，避免转动带来的线路缠绕；所述固定连接好的三维激光雷达与IMU惯性测量单元连接滑环转子，所述感知处理单元与激光雷达驱动器连接滑环定子。
所述微控制单片机通过电平转换模块将TTL电平的控制信号转换为485电平传输给云台电机。
所述IMU惯性测量单元通过USB连接的方式与所述感知处理单元进行数据传输，用于反馈IMU惯性测量单元的数据。
另一方面，本发明还提供一种采用上述系统进行建模的方法，包括如下步骤：
步骤1：对相对位置固定的三维激光雷达与IMU惯性测量单元进行空间位置标定，得到三维激光雷达与IMU惯性测量单元各自坐标系之间的相对位置变换，即外参变换；
步骤2：利用IMU惯性测量单元的输出数据对三维激光雷达的点云数据去畸变处理，去除三维激光雷达点云由于装置运动产生的运动畸变，过程如下：
步骤2.1：将三维激光雷达数据中第一个激光点对应时刻的位姿作为当前激光帧的位姿
Figure BDA0003176963580000031
步骤2.2：当前帧激光中任意一激光点所对应时刻为tc，其中时刻tc是全局下的绝对时间，用于和IMU惯性测量单元输出数据的时间标准一致；由于激光雷达和IMU的数据频率不同，所以无法直接找到当前时刻激光点的IMU数据，因此对IMU惯性测量单元输出数据进行插值处理：
查找时刻tc前后两帧IMU惯性测量单元输出数据，相应的IMU位姿分别为
Figure BDA0003176963580000032
计算时刻tc和前后两帧IMU数据时间的时间比值，前后两帧IMU数据时间为ti、ti+1，时刻tc相对于前后两帧IMU数据时间的时间比值分别为
Figure BDA0003176963580000033
和
Figure BDA0003176963580000034
相应当前时刻对应的IMU位姿为：
Figure BDA0003176963580000035
步骤2.3：由于三维激光雷达和IMU惯性测量单元进行固定连接，因此三维激光雷达和IMU惯性测量单元具有相同的运动变化，通过当前时刻激光点的IMU位姿乘以步骤1得到的外参变换，从而得到当前时刻的激光点位姿；
Figure BDA0003176963580000036
其中，
Figure BDA0003176963580000037
为三维激光雷达和IMU惯性测量单元的外参变换；
步骤2.4：计算当前激光点转换到当前激光帧坐标系下的变换：
Figure BDA0003176963580000038
从而消除装置运动产生的运动畸变。
步骤3：对去运动畸变处理后的激光点云数据进行特征提取和特征匹配，实现对装置运动姿态的实时估计，过程如下：
步骤3.1：获取去运动畸变处理后的激光点云数据所在激光线的线束标号，选取当前激光点周围同一标号上一定数量的激光点，来计算当前激光点附近的曲率：
Figure BDA0003176963580000039
其中，
Figure BDA00031769635800000310
表示三维激光雷达坐标系下第k帧激光中，当前第i个激光点的三维坐标，相应
Figure BDA00031769635800000311
是第i个激光点周围第j个激光点的三维坐标，M为周围点集；
步骤3.2：将计算的曲率作为激光点云数据的特征，将当前帧激光提取得到的特征，和上一帧激光做特征匹配，计算当前帧激光相对于上一帧的运动状态。
进一步的，所述将计算的曲率作为激光点云数据的特征具体分为直线特征和平面特征两类；将曲率值大的归为直线特征点，曲率值小的归为平面特征点；
所述特征匹配的过程为：将当前帧提取得到的直线特征点和上一帧的直线特征点进行匹配，当前帧的平面特征点和上一帧的平面特征点进行匹配。
所述计算当前帧激光相对于上一帧的运动状态的过程为：
S1：基于IMU的测量数据，计算得到两帧间的相对变换，以IMU计算的相对变换作为初始值，将当前帧特征点转换到上一帧坐标系下；
S2：当前帧中的直线特征点，上一帧直线特征点中寻找距离其最近的两个特征点，计算当前帧激光点到相应两个最近特征点构成直线的距离dl；当前帧平面特征点，在上一帧平面特征点中寻找距离其最近的三个特征点，计算当前帧平面特征点到相应最近三个特征点构成平面的距离dp；
S3：将上述计算的距离问题，转换为当前帧激光位姿使匹配距离值最小的一个非线性最小二乘问题，因此通过将匹配的距离值进行趋于零的迭代优化，求解得到两帧间的运动变化，从而得出里程计的运动估计。
步骤4：基于装置运动姿态的估计，将激光点云数据进行拼接，从而得到基于三维激光雷达采样数据的周围环境模型，得到物理环境对应的三维点云地图，实现对环境的实时三维重建，过程如下：
步骤4.1：将三维激光雷达点云基于里程计数据进行拼接，获得所在环境的局部特征点云地图，从而实时进行地图构建；
步骤4.2：为了消除一定的里程计累积误差，进行激光帧与局部地图的地图匹配，优化里程计的位姿估计值；将局部特征点云地图进行体素降采样处理，同时限制局部特征点云地图的范围，只选取特定长度空间范围的局部特征点云；
步骤4.3：基于地图的匹配计算，将当前帧激光数据的特征点，和步骤4.2处理之后的局部特征点云地图进行匹配，减少由于里程计的运动估计带来的累积漂移；
步骤4.4：考虑到基于特征地图匹配的计算复杂性，为了能够达到实时的运动估计，降低当前帧激光数据的特征点与地图的匹配执行频率，从而实现实时、高精度的三维地图重建。
采用上述技术方案所产生的有益效果在于：
1、本发明提供的基于传感器融合的旋转激光实时定位建模系统使用云台电机将三维激光雷达和IMU惯性测量单元的组合装置进行旋转，可以实现平稳可靠的旋转；
2、本发明利用微型处理器对采集数据进行处理和与底层控制器的交互，实现了装置的小巧、轻便，满足多种场景的应用需求，同时也具有基于无线的远程通信，提高了系统的交互能力和使用效率；
3、本发明通过融合三维激光雷达和IMU数据，实现激光雷达点云的有效去畸变处理，从而实现装置运动状态的准确估计；
4、本发明采用将三维激光进行旋转的方式，扩大激光雷达的有效视野，提高视野特征提取和匹配的可靠性，同时增加地图点云密度，基于鲁棒、准确的位姿估计，从而可以实时构建周围环境稠密、高精度的点云地图；
5、本发明通过将所有系统装置进行整合封装，使得整个结构成为一个整体，可装载于机器人系统，也可用于人员手持，方便了系统在多种场景下的应用。可以实现移动机器人的实时鲁棒运动估计，可以实现在狭长环境下的可靠定位和建图，同时能够构建高精度、稠密的点云地图，不仅可以用于机器人的自主导航，同时可用于室内和室外的移动三维测绘。
附图说明
图1为本发明实施例中提供的基于传感器融合的旋转激光实时定位建模系统的结构示意爆炸图；
其中，1-三维激光雷达，2-IMU惯性测量单元，3-微控制单片机，4-电源模块，5-网关模块，6-感知处理单元，7-激光雷达驱动器，8-封装外壳，9-云台电机固定结构，10-云台电机，11-通信模块。
图2为本发明实施例中三维激光雷达和云台电机采用连接件连接的示意图；
图3为本发明实施例中基于传感器融合的旋转激光实时定位建模系统的整体封装结构图；
图4为本发明实施例中采用基于传感器融合的旋转激光实时定位建模系统进行建模的方法流程图；
图5为本发明实施例中需要室内建模的真实环境图；
图6为本发明实施例中采用本发明的系统和方法进行建模的效果图。
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
如图1所示，本实施例中基于传感器融合的旋转激光实时定位建模系统如下所述：
系统包括三维激光雷达1、IMU惯性测量单元2、激光雷达驱动器7、云台电机10、感知处理单元6、电源模块4、通信模块11和底层控制器；
本实施例中，采用16线三维激光雷达，型号为Velodyne VLP-16。
所述三维激光雷达1与IMU惯性测量单元2固定连接后利用连接件与所述云台电机10进行连接，使得云台电机10带动固定连接好的三维激光雷达1与IMU惯性测量单元2一起旋转，如图2所示；
所述激光雷达驱动器7负责将激光雷达原始数据处理，然后通过RJ45接口发送给感知处理单元；
所述感知处理单元6通过其内部运行的定位与建图系统算法程序，对三维激光雷达1与IMU惯性测量单元2的数据进行处理，并通过通信模块11采用无线通讯的方式实现与远程交互端的信息交互；
所述底层控制器包括微控制单片机3和电平转换模块，微控制单片机3的控制信号经电平转换模块转换后发送到云台电机10，实现对云台电机的工作模式以及具体参数的控制；
所述电源模块4通过外接直流源来实现，并通过降压处理模块实现对三维激光雷达1、云台电机10、通信模块11的供电。
本实施例中，如图3所示，基于传感器融合的旋转激光实时定位建模系统中的感知处理单元6、微控制单片机3、无线AP模块(即通讯模块11)、电源模块4、网关模块5和激光雷达驱动器7整体封装于封装外壳8内，并将云台电机使用螺丝与云台电机固定结构9进行连接固定，然后再将云台电机固定结构9通过螺丝跟设备封装外壳8进行固定。
进一步的，所述感知处理单元6、激光雷达驱动器7与固定连接好的三维激光雷达1与IMU惯性测量单元2之间通过滑环连接，避免转动带来的线路缠绕；所述固定连接好的三维激光雷达1与IMU惯性测量单元2连接滑环转子，所述感知处理单元6与激光雷达驱动器7连接滑环定子。
所述微控制单片机3通过电平转换模块将TTL电平的控制信号转换为485电平传输给云台电机10。
所述IMU惯性测量单元2通过USB连接的方式与所述感知处理单元6进行数据传输，用于反馈IMU惯性测量单元2的数据。
另一方面，本发明还提供一种采用上述系统进行建模的方法，如图4所示，包括如下步骤：
步骤1：对相对位置固定的三维激光雷达1与IMU惯性测量单元2进行空间位置标定，得到三维激光雷达1与IMU惯性测量单元2各自坐标系之间的相对位置变换，即外参变换；
步骤2：利用IMU惯性测量单元2的输出数据对三维激光雷达1的点云数据去畸变处理，去除三维激光雷达点云由于装置运动产生的运动畸变，过程如下：
步骤2.1：将三维激光雷达数据中第一个激光点对应时刻的位姿作为当前激光帧的位姿
Figure BDA0003176963580000071
步骤2.2：当前帧激光中任意一激光点所对应时刻为tc，其中时刻tc是全局下的绝对时间，用于和IMU惯性测量单元输出数据的时间标准一致；由于激光雷达和IMU的数据频率不同，所以无法直接找到当前时刻激光点的IMU数据，因此对IMU惯性测量单元输出数据进行插值处理：
查找时刻tc前后两帧IMU惯性测量单元输出数据，相应的IMU位姿分别为
Figure BDA0003176963580000072
计算时刻tc和前后两帧IMU数据时间的时间比值，前后两帧IMU数据时间为ti、ti+1，时刻tc相对于前后两帧IMU数据时间的时间比值分别为
Figure BDA0003176963580000073
和
Figure BDA0003176963580000074
相应当前时刻对应的IMU位姿为：
Figure BDA0003176963580000075
步骤2.3：由于三维激光雷达和IMU惯性测量单元进行固定连接，因此三维激光雷达和IMU惯性测量单元具有相同的运动变化，通过当前时刻激光点的IMU位姿乘以步骤1得到的外参变换，从而得到当前时刻的激光点位姿；
Figure BDA0003176963580000076
其中，
Figure BDA0003176963580000077
为三维激光雷达和IMU惯性测量单元的外参变换；
步骤2.4：计算当前激光点转换到当前激光帧坐标系下的变换：
Figure BDA0003176963580000078
从而消除装置运动产生的运动畸变。
步骤3：对去运动畸变处理后的激光点云数据进行特征提取和特征匹配，实现对装置运动姿态的实时估计，过程如下：
步骤3.1：获取去运动畸变处理后的激光点云数据所在激光线的线束标号，选取当前激光点周围同一标号上一定数量的激光点，来计算当前激光点附近的曲率：
Figure BDA0003176963580000079
其中，
Figure BDA00031769635800000710
表示三维激光雷达坐标系下第k帧激光中，当前第i个激光点的三维坐标，相应
Figure BDA0003176963580000081
是第i个激光点周围第j个激光点的三维坐标，M为周围点集；
步骤3.2：将计算的曲率作为激光点云数据的特征，将当前帧激光提取得到的特征，和上一帧激光做特征匹配，计算当前帧激光相对于上一帧的运动状态。
进一步的，所述将计算的曲率作为激光点云数据的特征具体分为直线特征和平面特征两类；将曲率值大的归为直线特征点，曲率值小的归为平面特征点；
所述特征匹配的过程为：将当前帧提取得到的直线特征点和上一帧的直线特征点进行匹配，当前帧的平面特征点和上一帧的平面特征点进行匹配。
所述计算当前帧激光相对于上一帧的运动状态的过程为：
S1：基于IMU的测量数据，计算得到两帧间的相对变换，以IMU计算的相对变换作为初始值，将当前帧特征点转换到上一帧坐标系下；
S2：当前帧中的直线特征点，上一帧直线特征点中寻找距离其最近的两个特征点，计算当前帧激光点到相应两个最近特征点构成直线的距离dl；当前帧平面特征点，在上一帧平面特征点中寻找距离其最近的三个特征点，计算当前帧平面特征点到相应最近三个特征点构成平面的距离dp；
S3：将上述计算的距离问题，转换为当前帧激光位姿使匹配距离值最小的一个非线性最小二乘问题，因此通过将匹配的距离值进行趋于零的迭代优化，求解得到两帧间的运动变化，从而得出里程计的运动估计。
步骤4：基于装置运动姿态的估计，将激光点云数据进行拼接，从而得到基于三维激光雷达采样数据的周围环境模型，得到物理环境对应的三维点云地图，实现对环境的实时三维重建，过程如下：
步骤4.1：将三维激光雷达点云基于里程计数据进行拼接，获得所在环境的局部特征点云地图，从而实时进行地图构建；
步骤4.2：为了消除一定的里程计累积误差，进行激光帧与局部地图的地图匹配，优化里程计的位姿估计值；将局部特征点云地图进行体素降采样处理，同时限制局部特征点云地图的范围，只选取特定长度空间范围的局部特征点云；
步骤4.3：基于地图的匹配计算，将当前帧激光数据的特征点，和步骤4.2处理之后的局部特征点云地图进行匹配，减少由于里程计的运动估计带来的累积漂移；
步骤4.4：考虑到基于特征地图匹配的计算复杂性，为了能够达到实时的运动估计，降低当前帧激光数据的特征点与地图的匹配执行频率，从而实现实时、高精度的三维地图重建。
本实施例中，由实验人员手持基于传感器融合的旋转激光实时定位建模系统在室内走一圈，室内情况如图5所示。通过本实施例的方法进行建模，得到的建模效果如图6所示。

Claims (10)

1.一种基于传感器融合的旋转激光实时定位建模系统，其特征在于：系统包括三维激光雷达、IMU惯性测量单元、激光雷达驱动器、云台电机、感知处理单元、电源模块、通信模块和底层控制器；
所述三维激光雷达与IMU惯性测量单元固定连接后利用连接件与所述云台电机进行连接，使得云台电机带动固定连接好的三维激光雷达与IMU惯性测量单元一起旋转；
所述激光雷达驱动器负责将激光雷达原始数据处理，然后通过RJ45接口发送给感知处理单元；
所述感知处理单元通过其内部运行的定位与建图系统算法程序，对三维激光雷达与IMU惯性测量单元的数据进行处理，并通过通信模块采用无线通讯的方式实现与远程交互端的信息交互；
所述底层控制器包括微控制单片机和电平转换模块，微控制单片机的控制信号经电平转换模块转换后发送到云台电机，实现对云台电机的工作模式以及具体参数的控制；
所述电源模块通过外接直流源来实现，并通过降压处理模块实现对三维激光雷达、云台电机、通信模块的供电。
2.根据权利要求1所述的基于传感器融合的旋转激光实时定位建模系统，其特征在于：所述感知处理单元、激光雷达驱动器与固定连接好的三维激光雷达与IMU惯性测量单元之间通过滑环连接，避免转动带来的线路缠绕；所述固定连接好的三维激光雷达与IMU惯性测量单元连接滑环转子，所述感知处理单元与激光雷达驱动器连接滑环定子。
3.根据权利要求1所述的基于传感器融合的旋转激光实时定位建模系统，其特征在于：所述微控制单片机通过电平转换模块将TTL电平的控制信号转换为485电平传输给云台电机。
4.根据权利要求1所述的基于传感器融合的旋转激光实时定位建模系统，其特征在于：所述IMU惯性测量单元通过USB连接的方式与所述感知处理单元进行数据传输，用于反馈IMU惯性测量单元的数据。
5.采用权利要求1至4中任意一项所述的基于传感器融合的旋转激光实时定位建模系统进行建模的方法，其特征在于，包括如下步骤：
步骤1：对相对位置固定的三维激光雷达与IMU惯性测量单元进行空间位置标定，得到三维激光雷达与IMU惯性测量单元各自坐标系之间的相对位置变换，即外参变换；
步骤2：利用IMU惯性测量单元的输出数据对三维激光雷达的点云数据去畸变处理，去除三维激光雷达点云由于装置运动产生的运动畸变；
步骤3：对去运动畸变处理后的激光点云数据进行特征提取和特征匹配，实现对装置运动姿态的实时估计；
步骤4：基于装置运动姿态的估计，将激光点云数据进行拼接，从而得到基于三维激光雷达采样数据的周围环境模型，得到物理环境对应的三维点云地图，实现对环境的实时三维重建。
6.根据权利要求5所述的基于传感器融合的旋转激光实时定位建模系统进行建模的方法，其特征在于：所述步骤2的过程如下：
步骤2.1：将三维激光雷达数据中第一个激光点对应时刻的位姿作为当前激光帧的位姿
Figure FDA0003176963570000021
步骤2.2：当前帧激光中任意一激光点所对应时刻为tc，其中时刻tc是全局下的绝对时间，用于和IMU惯性测量单元输出数据的时间标准一致；由于激光雷达和IMU的数据频率不同，所以无法直接找到当前时刻激光点的IMU数据，因此对IMU惯性测量单元输出数据进行插值处理：
查找时刻tc前后两帧IMU惯性测量单元输出数据，相应的IMU位姿分别为
Figure FDA0003176963570000022
计算时刻tc和前后两帧IMU数据时间的时间比值，前后两帧IMU数据时间为ti、ti+1，时刻tc相对于前后两帧IMU数据时间的时间比值分别为
Figure FDA0003176963570000023
和
Figure FDA0003176963570000024
相应当前时刻对应的IMU位姿为：
Figure FDA0003176963570000025
步骤2.3：由于三维激光雷达和IMU惯性测量单元进行固定连接，因此三维激光雷达和IMU惯性测量单元具有相同的运动变化，通过当前时刻激光点的IMU位姿乘以步骤1得到的外参变换，从而得到当前时刻的激光点位姿；
Figure FDA0003176963570000026
其中，
Figure FDA0003176963570000027
为三维激光雷达和IMU惯性测量单元的外参变换；
步骤2.4：计算当前激光点转换到当前激光帧坐标系下的变换：
Figure FDA0003176963570000028
从而消除装置运动产生的运动畸变。
7.根据权利要求5所述的基于传感器融合的旋转激光实时定位建模系统进行建模的方法，其特征在于：所述步骤3的过程如下：
步骤3.1：获取去运动畸变处理后的激光点云数据所在激光线的线束标号，选取当前激光点周围同一标号上一定数量的激光点，来计算当前激光点附近的曲率：
Figure FDA0003176963570000031
其中，
Figure FDA0003176963570000032
表示三维激光雷达坐标系下第k帧激光中，当前第i个激光点的三维坐标，相应
Figure FDA0003176963570000033
是第i个激光点周围第j个激光点的三维坐标，M为周围点集；
步骤3.2：将计算的曲率作为激光点云数据的特征，将当前帧激光提取得到的特征，和上一帧激光做特征匹配，计算当前帧激光相对于上一帧的运动状态。
8.根据权利要求7所述的基于传感器融合的旋转激光实时定位建模系统进行建模的方法，其特征在于：所述将计算的曲率作为激光点云数据的特征具体分为直线特征和平面特征两类；将曲率值大的归为直线特征点，曲率值小的归为平面特征点；
所述特征匹配的过程为：将当前帧提取得到的直线特征点和上一帧的直线特征点进行匹配，当前帧的平面特征点和上一帧的平面特征点进行匹配。
9.根据权利要求8所述的基于传感器融合的旋转激光实时定位建模系统进行建模的方法，其特征在于：所述计算当前帧激光相对于上一帧的运动状态的过程为：
S1：基于IMU的测量数据，计算得到两帧间的相对变换，以IMU计算的相对变换作为初始值，将当前帧特征点转换到上一帧坐标系下；
S2：当前帧中的直线特征点，上一帧直线特征点中寻找距离其最近的两个特征点，计算当前帧激光点到相应两个最近特征点构成直线的距离dl；当前帧平面特征点，在上一帧平面特征点中寻找距离其最近的三个特征点，计算当前帧平面特征点到相应最近三个特征点构成平面的距离dp；
S3：将上述计算的距离问题，转换为当前帧激光位姿使匹配距离值最小的一个非线性最小二乘问题，因此通过将匹配的距离值进行趋于零的迭代优化，求解得到两帧间的运动变化，从而得出里程计的运动估计。
10.根据权利要求5所述的基于传感器融合的旋转激光实时定位建模系统进行建模的方法，其特征在于：所述步骤4的过程如下：
步骤4.1：将三维激光雷达点云基于里程计数据进行拼接，获得所在环境的局部特征点云地图，从而实时进行地图构建；
步骤4.2：为了消除一定的里程计累积误差，进行激光帧与局部地图的地图匹配，优化里程计的位姿估计值；将局部特征点云地图进行体素降采样处理，同时限制局部特征点云地图的范围，只选取特定长度空间范围的局部特征点云；
步骤4.3：基于地图的匹配计算，将当前帧激光数据的特征点，和步骤4.2处理之后的局部特征点云地图进行匹配，减少由于里程计的运动估计带来的累积漂移；
步骤4.4：考虑到基于特征地图匹配的计算复杂性，为了能够达到实时的运动估计，降低当前帧激光数据的特征点与地图的匹配执行频率，从而实现实时、高精度的三维地图重建。
CN202110835171.6A 2021-07-23 2021-07-23 基于传感器融合的旋转激光实时定位建模系统及方法 Active CN113570715B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110835171.6A CN113570715B (zh) 2021-07-23 2021-07-23 基于传感器融合的旋转激光实时定位建模系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110835171.6A CN113570715B (zh) 2021-07-23 2021-07-23 基于传感器融合的旋转激光实时定位建模系统及方法

Publications (2)

Publication Number Publication Date
CN113570715A true CN113570715A (zh) 2021-10-29
CN113570715B CN113570715B (zh) 2023-10-13

Family

ID=78166591

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110835171.6A Active CN113570715B (zh) 2021-07-23 2021-07-23 基于传感器融合的旋转激光实时定位建模系统及方法

Country Status (1)

Country Link
CN (1) CN113570715B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114942421A (zh) * 2022-05-30 2022-08-26 西安电子科技大学 一种全向扫描的多线激光雷达自主定位装置及方法
CN115421125A (zh) * 2022-11-07 2022-12-02 山东富锐光学科技有限公司 一种基于数据融合的雷达点云数据惯性校正方法
CN115435784A (zh) * 2022-08-31 2022-12-06 中国科学技术大学 高空作业平台激光雷达与惯导融合定位建图装置及方法
WO2023226154A1 (zh) * 2022-05-24 2023-11-30 广东人工智能与先进计算研究院 自主定位方法、装置、设备及计算机可读存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303710A (zh) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维激光雷达的无人机多场景定位建图方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303710A (zh) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维激光雷达的无人机多场景定位建图方法
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李海标;时君;田春月;: "一种基于深度特征的室外环境下激光地图辅助视觉定位方法", 科学技术与工程, no. 13, pages 173 - 178 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023226154A1 (zh) * 2022-05-24 2023-11-30 广东人工智能与先进计算研究院 自主定位方法、装置、设备及计算机可读存储介质
CN114942421A (zh) * 2022-05-30 2022-08-26 西安电子科技大学 一种全向扫描的多线激光雷达自主定位装置及方法
CN115435784A (zh) * 2022-08-31 2022-12-06 中国科学技术大学 高空作业平台激光雷达与惯导融合定位建图装置及方法
CN115421125A (zh) * 2022-11-07 2022-12-02 山东富锐光学科技有限公司 一种基于数据融合的雷达点云数据惯性校正方法

Also Published As

Publication number Publication date
CN113570715B (zh) 2023-10-13

Similar Documents

Publication Publication Date Title
CN113570715B (zh) 基于传感器融合的旋转激光实时定位建模系统及方法
US10427304B2 (en) Robotic charger alignment
CN110446159B (zh) 一种室内无人机精确定位与自主导航的系统及方法
CN205219101U (zh) 一种家庭服务机器人
EP3171334B1 (en) Pose estimation apparatus and vacuum cleaner system
CN105783913A (zh) 一种融合车载多传感器的slam装置及其控制方法
CN109238240A (zh) 一种顾及地形的无人机倾斜摄影方法及其摄影系统
CN106898022A (zh) 一种手持式快速三维扫描系统及方法
CN109358342B (zh) 基于2d激光雷达的三维激光slam系统及控制方法
JP2017537484A (ja) 可動物体を検出し、追跡するシステム及び方法
CN205426175U (zh) 一种融合车载多传感器的slam装置
CN114088087B (zh) 无人机gps-denied下高可靠高精度导航定位方法和系统
WO2019100219A1 (zh) 输出影像生成方法、设备及无人机
Holz et al. Towards multimodal omnidirectional obstacle detection for autonomous unmanned aerial vehicles
CN105974456A (zh) 一种自主式水下机器人组合导航系统
CN110751123A (zh) 一种单目视觉惯性里程计系统及方法
CN113807435B (zh) 一种基于多传感器的遥感图像特征点高程获取方法
CN115307646B (zh) 一种多传感器融合的机器人定位方法、系统及装置
CN115933718A (zh) 一种融合全景slam与目标识别的无人机自主飞行技术方法
CN114923477A (zh) 基于视觉与激光slam技术的多维度空地协同建图系统和方法
CN113075686A (zh) 一种基于多传感器融合的电缆沟智能巡检机器人建图方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN214409706U (zh) 一种基于机器视觉的室内无人机定位系统
Kakillioglu et al. 3D sensor-based UAV localization for bridge inspection
CN114529585A (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
TA01 Transfer of patent application right

Effective date of registration: 20230912

Address after: 110819 No. 3 lane, Heping Road, Heping District, Shenyang, Liaoning 11

Applicant after: Northeastern University

Applicant after: 63983 FORCES, PLA

Address before: 110819 No. 3 lane, Heping Road, Heping District, Shenyang, Liaoning 11

Applicant before: Northeastern University

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant