CN117518196A - 一种激光雷达的运动补偿方法、装置、系统、设备及介质 - Google Patents

一种激光雷达的运动补偿方法、装置、系统、设备及介质 Download PDF

Info

Publication number
CN117518196A
CN117518196A CN202311758567.0A CN202311758567A CN117518196A CN 117518196 A CN117518196 A CN 117518196A CN 202311758567 A CN202311758567 A CN 202311758567A CN 117518196 A CN117518196 A CN 117518196A
Authority
CN
China
Prior art keywords
point
point cloud
cloud data
holder
coordinate system
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
CN202311758567.0A
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.)
Zoomlion Heavy Industry Science and Technology Co Ltd
Original Assignee
Zoomlion Heavy Industry Science and Technology Co Ltd
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 Zoomlion Heavy Industry Science and Technology Co Ltd filed Critical Zoomlion Heavy Industry Science and Technology Co Ltd
Priority to CN202311758567.0A priority Critical patent/CN117518196A/zh
Publication of CN117518196A publication Critical patent/CN117518196A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/481—Constructional features, e.g. arrangements of optical elements
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本申请涉及人工智能技术领域，尤其涉及一种激光雷达的运动补偿方法、装置、系统、设备及介质，用以对剧烈运动场景下采集的激光雷达点云数据进行位置补偿。其中，方法包括：获取激光雷达当前帧的点云数据、定位装置当前帧对应的定位数据、以及目标时间段内云台的角度变化数据；基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻的云台角度，并根据预先确定的激光雷达和云台之间的位置标定参数，将每个空间点转换到第一坐标系中；基于定位数据，确定目标旋转偏移矩阵；基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。

Description

一种激光雷达的运动补偿方法、装置、系统、设备及介质
技术领域
本申请涉及人工智能技术领域，尤其涉及一种激光雷达的运动补偿方法、装置、系统、设备及介质。
背景技术
伴随人工智能和半导体技术的迅速发展，智慧施工已经逐步映入人们的眼帘，相对传统工地的施工管理，智慧施工融合了更多的传感器、人工智能和虚拟现实等高精尖技术。智慧工地运用信息化手段，对工程项目进度的精准把控和施工场景模拟，并通过管理施工过程数据进行深层次的数据挖掘分析，提供项目施工趋势预测和项目专家预案，建立一个互联协同、智慧生产、科学管理的智慧工地信息化生态圈。
智慧工地的主要信息来源为三维重建点云模型，而获取大场景(半径≥300米)三维重建点云模型，无法通过固定式的激光雷达(测距精度≤100m)旋转获取。现阶段最优方法是：通过移动载具搭载旋转式的激光雷达进行不同点位不同角度的点云数据采集，并根据姿态信息对点云数据进行拼接，实现大场景范围的三维重建。但由于激光雷达在每帧点云数据的采集过程中，可能处于旋转状态，且受到移动载具运动等因素的影响而产生剧烈运动，降低了点云数据的采集精度，进而影响利用点云数据三维重建时的模型精度。
因此，需要针对剧烈运动场景，设计一种激光雷达的运动补偿方法，以克服上述缺陷。
发明内容
本申请实施例提供一种激光雷达的运动补偿方法、装置、系统、设备及介质，用以对剧烈运动场景下采集的激光雷达点云数据进行位置补偿。
第一方面，本申请实施例提供一种激光雷达的运动补偿方法，应用于与云台转动部件连接的激光雷达，且所述云台装载于移动载具上，所述云台的非转动部件连接有定位装置，所述方法包括：
获取所述激光雷达当前帧的点云数据、所述定位装置当前帧对应的定位数据、以及目标时间段内所述云台的角度变化数据；
基于所述点云数据和所述角度变化数据，确定所述点云数据中每个空间点采样时刻的云台角度，并根据预先确定的所述激光雷达和所述云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，所述第一坐标系为以所述云台当前位置为原点建立的坐标系；
基于所述定位数据，确定用于表征所述云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵；
基于所述目标旋转偏移矩阵，对所述第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。
第二方面，本申请实施例提供一种激光雷达的运动补偿装置，应用于与云台转动部件连接的激光雷达，且所述云台装载于移动载具上，所述云台的非转动部件连接有定位装置，包括：
获取单元，用于获取所述激光雷达当前帧的点云数据、所述定位装置当前帧对应的定位数据、以及目标时间段内所述云台的角度变化数据；
第一位置补偿单元，用于基于所述点云数据和所述角度变化数据，确定所述点云数据中每个空间点采样时刻的云台角度，并根据预先确定的所述激光雷达和所述云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，所述第一坐标系为以所述云台当前位置为原点建立的坐标系；
处理单元，用于基于所述定位数据，确定用于表征所述云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵；
第二位置补偿单元，用于基于所述目标旋转偏移矩阵，对所述第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。
可选的，所述第一位置补偿单元，具体用于：
将所述点云数据的时间戳信息，作为所述点云数据中最后一个空间点的采集时刻，并依据所述激光雷达的属性参数，基于所述最后一个空间点的采样时刻，确定所述点云数据中每个空间点对应的雷达采样时刻；
依据所述云台角度变化的粒度，对所述角度变化数据进行拆分，确定各个云台采样时刻的云台角度；
针对每个空间点，分别执行以下操作：
若空间点对应的雷达采样时刻存在相应的云台采样时刻，则将云台采样时刻的云台角度，确定为空间点采样时刻对应的云台角度；
若空间点对应的雷达采样时刻不存在相应的云台采样时刻，则利用预设算法，基于雷达采样时刻相邻的两个云台采样时刻的云台角度，计算空间点采样时刻对应的云台角度。
可选的，所述第一位置补偿单元，利用预设算法计算空间点采样时刻对应的云台角度时，使用以下公式计算空间点采样时刻对应的云台角度：
其中，为空间点采样时刻对应的云台角度，/>为第j个云台采样时刻，/>为第j-1个云台采样时刻，T为两个相邻云台采样时刻之间的时间差，/>为第i个雷达采样时刻，且/>位于/>和/>之间，/>为/>时刻对应的云台角度，/>为/>时刻对应的云台角度。
可选的，所述第一位置补偿单元，具体用于：
使用以下公式，将每个空间点转换到第一坐标系中：
其中，为空间点在第一坐标系中的位置，/>为空间点采样时刻对应的云台角度，prRl和prTl为预先确定的所述激光雷达和所述云台之间的位置标定参数，/>为空间点的采样位置。
可选的，所述定位装置包括：惯性测量单元(Inertial Measurement Unit，IMU)和全球导航卫星系统(Global Navigation Satellite System，GNSS)。
可选的，所述处理单元，具体用于：
对所述IMU的当前帧的IMU数据进行累积积分，确定所述云台当前位置相对于所述云台初始位置位姿变化的估计值，基于所述GNSS的当前帧的GNSS数据，确定所述云台当前位置相对于所述云台初始位置位姿变化的观测值，使用扩展式卡尔曼滤波将所述预测值和所述观测值进行融合，得到估计旋转偏移矩阵；
对所述第二坐标系中的全局点云数据进行K-D树索引，计算所述全局点云数据中每个空间点邻域点云的协方差矩阵，并基于所述协方差矩阵，计算每个空间点邻域点云的平面单位法向量，所述全局点云数据为已进行位置补偿的点云数据；
基于所述平面单位法向量，构造用于计算所述第一坐标系中空间点和全局点云数据中空间点到平面的距离误差的目标函数，以所述估计旋转偏移矩阵作为所述目标函数的初始位置补偿参数，使用最优化算法通过多次迭代计算，确定所述目标函数的最小值，并将所述目标函数值最小值对应的位置补偿参数，确定为目标旋转偏移矩阵。
可选的，所述目标函数为：
其中，为所述第一坐标系中的空间点，/>为所述全局点云数据中的第i个空间点，wRptz和wTptz为位置补偿参数，所述位置补偿参数的初始值为估计旋转偏移矩阵的值，/>为所述全局点云数据中第i个空间点的平面单位法向量。
可选的，所述第二位置补偿单元具体用于：
使用以下公式，对所述第一坐标系中的每个空间点进行位置补偿：
其中，为空间点在所述第二坐标系中的位置坐标，/>为空间点在所述第一坐标系中的位置坐标，wRptz和wTptz的值根据所述目标旋转偏移矩阵确定。
可选的，所述处理单元还用于：
基于所述第二坐标系中部分或全部全局点云数据，进行三维点云重建，生成三维地图。
第三方面，本申请实施例提供一种三维点云重建系统，所述系统包括：激光雷达，云台、移动载具、定位装置，其中，所述激光雷达与所述云台的转动部件连接，所述定位装置与所述云台的非转动部件连接，且所述云台装载于移动载具上，所述激光雷达采集点云数据，采用如上述第一方面中任意一种激光雷达的运动补偿方法对采集的点云数据进行位置补偿，确定所述点云数据的位置坐标，并基于部分或全部点云数据的位置坐标进行三维点云重建。
第四方面，本申请实施例提供一种电子设备，其包括处理器和存储器，其中，所述存储器存储有计算机程序，当所述计算机程序被所述处理器执行时，使得所述处理器执行上述第一方面中任意一种激光雷达的运动补偿方法的步骤。
第五方面，本申请实施例提供一种机器可读存储介质，该机器可读存储介质上存储有指令，该指令在被处理器执行时使得所述处理器被配置成执行上述第一方面中任意一种激光雷达的运动补偿方法。
本申请有益效果如下：
本申请实施例提供的激光雷达的运动补偿方法、装置、系统、设备及介质，获取激光雷达当前帧的点云数据、定位装置当前帧对应的定位数据、以及目标时间段内云台的角度变化数据之后，在对激光雷达采集的点云数据进行位置补偿时，将运动补偿过程分解为云台带动激光雷达旋转的旋转补偿和移动载具带动云台进行移动的移动补偿。
首先，基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻的云台角度，并根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，对激光雷达采集的点云数据进行旋转补偿；其次，基于定位数据确定目标旋转偏移矩阵，并基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在第二坐标系中的位置坐标，对激光雷达采集的点云数据进行移动补偿，与现有技术中相比，实现了激光雷达随云台转动，云台随移动载具移动场景下，也即激光雷达剧烈运动场景下，对激光雷达采集的点云数据的运动补偿。
本申请的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请而了解。本申请的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1为本申请实施例中的一种应用场景示意图；
图2为本申请实施例中的一种激光雷达的运动补偿方法的实施流程图；
图3为本申请实施例中另一激光雷达的运动补偿方法的具体实施流程的实施流程图；
图4为本申请实施例中的一种激光雷达的运动补偿装置的组成结构示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请技术方案的一部分实施例，而不是全部的实施例。基于本申请文件中记载的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请技术方案保护的范围。
下面对本申请实施例中涉及的部分概念进行介绍。
激光雷达：是指以发射激光束探测目标的位置、速度等特征量的雷达系统，其工作原理是通过向目标发射测量信号，然后将接到从目标反射回来的信号与发射信号进行比较，作适当处理后，获取目标有关信息，如目标距离、方位、高度、速度、姿态等参数。
运动补偿：是一种在移动载具上使用激光雷达，来获取环境数据的必备需求，因激光雷达固定在车辆或者机器人上，而车辆或机器人可能处于移动状态，通过引入运动补偿，将激光雷达采集的点云数据校正为参考坐标系中的静态地图和环境数据。
下面对本申请实施例的设计思想进行简要介绍：
伴随人工智能和半导体技术的迅速发展，智慧施工已经逐步映入人们的眼帘，相对传统工地的施工管理，智慧施工融合了更多的传感器、人工智能和虚拟现实等高精尖技术。智慧工地运用信息化手段，对工程项目进度的精准把控和施工场景模拟，并通过管理施工过程数据进行深层次的数据挖掘分析，提供项目施工趋势预测和项目专家预案，建立一个互联协同、智慧生产、科学管理的智慧工地信息化生态圈。
智慧工地的主要信息来源为三维重建点云模型，而获取大场景(例如：半径≥300米)三维重建点云模型，无法通过固定式的激光雷达(通常测距精度≤100m)旋转获取。现阶段最优方法是：通过移动载具搭载旋转式的激光雷达进行不同点位不同角度的点云数据采集，并根据姿态信息对点云数据进行拼接，实现大场景范围的三维重建。但由于激光雷达在每帧点云数据的采集过程中，可能处于旋转状态，且受到移动载具运动等因素的影响而产生剧烈运动，降低了点云数据的采集精度，进而影响利用点云数据三维重建时的模型精度。
鉴于此，本申请实施例提供一种激光雷达的运动补偿方法、装置、系统、设备及介质，获取激光雷达当前帧的点云数据、定位装置当前帧对应的定位数据、以及目标时间段内云台的角度变化数据之后，在对激光雷达采集的点云数据进行位置补偿时，将运动补偿过程分解为云台带动激光雷达旋转的旋转补偿和移动载具带动云台进行移动的移动补偿。
首先，基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻的云台角度，并根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，对激光雷达采集的点云数据进行旋转补偿；其次，基于定位数据确定目标旋转偏移矩阵，并基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在第二坐标系中的位置坐标，对激光雷达采集的点云数据进行移动补偿，与现有技术中相比，实现了激光雷达随云台转动，云台随移动载具移动场景下，也即激光雷达剧烈运动场景下，对激光雷达采集的点云数据的运动补偿。
以下结合说明书附图对本申请的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本申请，并不用于限定本申请，并且在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
如图1所示，其为本申请实施例的应用场景示意图。该应用场景图中包括激光雷达10、云台11和移动载具(图中未示出)。其中，云台11的转动部件110与激光雷达10连接，且云台11装载于移动载具上，云台11的非转动部件120连接有定位装置。
在本申请实施例中，移动载具搭载云台11在采集场景中进行空间旋转平移，云台11带动激光雷达10进行高精度的角分辨率(例如1°)旋转，通过控制云台11水平旋转360°的同时控制激光雷达10获取采集场景的三维点云。
需要说明的是，本申请实施例中的激光雷达的运动补偿方法，可以由激光雷达10自身的处理器执行，也可以由与激光雷达10、云台11、以及定位装置通信连接的控制器或者处理器执行，本申请实施例对此不做限定。
以与激光雷达10通信连接的控制器或者处理器执行为例，获取激光雷达当前帧的点云数据、定位装置当前帧对应的定位数据、以及目标时间段内云台的角度变化数据；基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻的云台角度，并根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，然后基于定位数据确定用于表征云台相对于云台初始位置位姿变化的目标旋转偏移矩阵，并基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在第二坐标系中的位置坐标，获得当前帧的点云数据在第二坐标系中的位置坐标，也即获得补偿后的点云数据，实现对当前帧的点云数据的运动补偿。
下面结合上述描述的应用场景，参考附图来描述本申请示例性实施方式提供的激光雷达的运动补偿方法，需要注意的是，上述应用场景仅是为了便于理解本申请的精神和原理而示出，本申请的实施方式在此方面不受任何限制。
需要说明的是，本申请实施例实施之前，需要对激光雷达、云台和定位装置进行时间和空间的同步，且对激光雷达、云台和定位装置进行时间和空间的同步，可以离线进行。
定位装置以IMU和GNSS为例，同步过程具体包括以下四个部分：(1)通过旋转式激光雷达外参标定方法，对激光雷达与云台旋转中心进行标定；(2)利用激光雷达与IMU标定技术进行外参数标定，本申请实施例对此方式不做限定；(3)利用GNSS与激光雷达标定技术进行外参数标定，本申请实施例对此方式不做限定；(4)对云台、GNSS、IMU和激光雷达进行时间同步，例如，通过在同一时间触发云台、GNSS、IMU和激光雷达的时钟系统。
其中，对激光雷达与云台旋转中心进行标定时，首先，控制云台带动多线激光雷达以分辨率1°进行360°范围内旋转，采集点云数据采集并保存，以180度为分割点将点云分成上下半圆两部分(划分为第一点云集合和第二点云集合，其中，上半圆点云为第一点云集合，下半圆点云为第二点云集合)，利用旋转性质(云台的旋转中心与激光雷达的旋转中心重合，则前后半圆模型一致无偏移)将激光雷达三维重建系统外参标定转换为点云配准算法；其次，在上半圆点云建立K-D树索引，对上半圆点云进行处理，聚类每个特征点的邻域范围实现动态邻域选择，并计算每个特征点的邻域范围内协方差，利用协方差估计出点云聚类平面的邻域内平面法向量，同时使用协方差矩阵的特征值对点云邻域内平面法向量进行平面度评估；最后，通过K-D树索引，寻找前后半圆的对应点，并基于前后对应点，以两点与对应邻域内平面单位法向量的距离之差作为误差，以对应邻域的邻域内平面质量度加权构造出一个似配准的非线性最小二乘的损失函数，通过优化求解得旋转平移矩阵，即旋转式激光雷达重建系统的外参矩阵，通过此类方法可解决无标定物的激光雷达三维重建系统外参标定，实现大场景的重建。
在对激光雷达、云台和定位装置进行时间和空间的同步之后，即可通过激光雷达采集场景中的点云数据，并对采集的点云数据进行运动补偿，然后基于运动补偿后的点云数据进行三维重建，下面结合附图对本申请实施例提供的激光雷达的运动补偿方法进行详细说明。
参阅图2所示，其为本申请实施例中的一种激光雷达的运动补偿方法的实施流程图，该方法的具体实施流程如下S201-S204：
S201：获取激光雷达当前帧的点云数据、定位装置当前帧对应的定位数据、以及目标时间段内云台的角度变化数据。
具体实施时，激光雷达当前帧的点云数据和定位装置当前帧对应的定位数据，都可以分别从激光雷达传感器和定位装置中直接获取，而云台的角度变化数据可以从云台角度编码器中获取，本申请实施例对此不做限定。其中，目标时间段具体是指当前帧的采样时刻与前一帧的采样时刻之间的时间段。
需要说明的是，本申请实施例中定位装置用于获取激光雷达的定位数据(包括位姿数据)，定位装置可以是IMU和GNSS，也可以是实时差分(Real-time kinematic)定位装置，当然，还可以是其他定位装置，此处并不用于具体限定。
S202：基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻的云台角度，并根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，第一坐标系为以云台当前位置为原点建立的坐标系。
具体实施时，基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻对应的云台角度时，首先将点云数据的时间戳信息，作为点云数据中最后一个空间点的采集时刻，并依据激光雷达的属性参数，基于最后一个空间点的采样时刻，确定点云数据中每个空间点对应的雷达采样时刻。其中，激光雷达的属性参数中，包括针对每一行中每个空间点或者每一列中每个空间点的时间误差补偿系数，因此，根据针对每一行中每个空间点或者每一列中每个空间点的时间误差补偿系数，以及最后一个空间点的采样时刻，能够确定出点云数据中每个空间点的雷达采样时刻。
然后，依据云台角度变化的粒度，对角度变化数据进行拆分，确定各个云台采样时刻的云台角度。
实际应用中，由于将激光雷达的点云数据的帧频率，分解成空间点的点频率(≥2000HZ)，而云台水平角度反馈频率(≤500HZ)，因此，点云数据的点频率大于云台水平角度的反馈频率，换句话说，雷达采样时刻的数量大于云台采样时刻的数量，部分雷达采样时刻无对应的云台采样时刻。因此，在确定点云数据中每个空间点采样时刻的云台角度时，会存在以下两种情况：
情况1、若空间点对应的雷达采样时刻存在相应的云台采样时刻，则将云台采样时刻的云台角度，确定为空间点采样时刻对应的云台角度。
情况2、若空间点对应的雷达采样时刻不存在相应的云台采样时刻，则利用预设算法，基于雷达采样时刻相邻的两个云台采样时刻的云台角度，计算空间点采样时刻对应的云台角度。
其中，预设算法可以为双线性差值算法，具体可以使用以下公式(1)计算空间点采样时刻对应的云台角度：
其中，为空间点采样时刻对应的云台角度，/>为第j个云台采样时刻，/>为第j-1个云台采样时刻，T为两个相邻云台采样时刻之间的时间差，/>为第i个雷达采样时刻，且/>位于/>和/>之间，/>为/>时刻对应的云台角度，/>为/>时刻对应的云台角度。
具体实施时，在确定每个空间点采样时刻对应的云台角度之后，即可根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，对激光雷达采集的点云数据进行旋转补偿。其中，第一坐标系为以云台当前位置为原点建立的坐标系。
具体根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中时，可以使用以下公式(2)，将每个空间点转换到第一坐标系中：
其中，为空间点在第一坐标系中的坐标位置，/>为空间点采样时刻对应的云台角度，prRl和prTl为预先确定的激光雷达和云台之间的位置标定参数，该位置标定参数可以在对激光雷达与云台旋转中心进行标定时获得，也即根据激光雷达与云台旋转中心标定时计算得到的外参矩阵确定，/>为空间点的采样位置，也即空间点在以激光雷达为原点的坐标系中的位置。
S203：基于定位数据确定用于表征云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵。
本申请下述实施例中以定位装置包括IMU和GNSS为例，则定位数据包括IMU数据和GNSS数据，在基于IMU数据和GNSS数据，确定用于表征云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵，首先利用预积分方法对IMU数据进行累积积分，确定云台当前位置相对于云台初始位置(也即移动载具未发生移动且云台未发生位姿变化时的位置)位姿变化的估计值，基于GNSS数据，确定云台当前位置相对于云台初始位置位姿变化的观测值，然后计算拓展式卡尔曼滤波的卡尔曼增益，并以卡尔曼增益作为权值，进行观测值和估计值融合，并更新估计值，最后，更新当前位置的协方差矩阵，得到最优的估计旋转偏移矩阵。
上述确定估计旋转偏移矩阵的方式，通过引入GNSS数据，减少因IMU数据积分偏移问题，避免后续初始值偏移较大，迭代求解陷入局部最优无法完成第二阶段运动补偿(也即S204中的位置补偿)。
在得到估计旋转偏移矩阵之后，对第二坐标系中的全局点云数据进行K-D树索引，计算全局点云数据中每个空间点邻域点云的协方差矩阵，并基于协方差矩阵，计算每个空间点邻域点云的平面单位法向量，全局点云数据为已进行位置补偿的点云数据；其中，第二坐标系是预先定义的坐标系，其可以以任意点为原点，例如，以云台初始位置为原点，云台初始位置是指移动载具未发生移动且云台未发生位姿变化时的位置，第二坐标系也可以是大地坐标系、世界坐标系、或者地图坐标系等。
全局点云数据是指第二坐标系中的点云数据，也即已进行位置补偿的点云数据。举例来说，对于第二帧点云数据而言，全局点云数据为已进行位置补偿的第一帧的点云数据，对于第三帧点云数据而言，全局点云数据为已进行位置补偿的第一帧的点云数据和第二帧的点云数据。
需要说明的是，每个空间点的领域点云是指每个空间点阈值范围(可以根据经验值设定，本申请实施例对此不做限定，例如，2米)内预设数量(可以根据经验值设定，本申请实施例对此不做限定，例如：50个)的领域点组成的空间。
具体计算全局点云数据中每个空间点邻域点云的协方差矩阵时，可以采用以下公式(3)：
其中，N(x,r)表示空间点x阈值范围(半径r)内的点云集合，xi表示点云集合N(x,r)内空间点的坐标，表示点云集合N(x,r)的质心点。
基于协方差矩阵，计算每个空间点邻域点云的平面单位法向量时，可以使用如下公式(4)：
其中，nT为平面单位法向量，λ2和λ1为公式(3)中协方差矩阵的特征值，且λ2≥λ1，trace(∑(x,r))为公式(3)中协方差矩阵的迹。
在得到每个空间点领域点云的平面单位法向量之后，基于平面单位法向量，构造用于计算第一坐标系中空间点和全局点云数据中空间点到平面的距离误差的目标函数，以估计旋转偏移矩阵作为目标函数的初始位置补偿参数，使用最优化算法通过多次迭代计算，确定目标函数的最小值，并将目标函数值最小值对应的位置补偿参数，确定为目标旋转偏移矩阵。
具体地，目标函数如以下公式(5)：
其中，为第一坐标系中的空间点，也即当前帧的点云数据中待进行位置补偿的空间点，/>为全局点云数据中的第i个空间点，wRptz和wTptz为位置补偿参数，位置补偿参数的初始值通过估计旋转偏移矩阵确定，/>为全局点云数据中第i个空间点的平面单位法向量。
具体实施时，对上述公式(5)中的目标函数，使用最优化算法(例如，高斯-牛顿算法、L-M算法或者SVD算法等等，本申请实施例对此不做限定)不断迭代求解出目标函数的最小值，并确定目标函数最小值对应的位置补偿参数，经该位置补偿参数确定为目标旋转偏移矩阵的值。
S204：基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。
具体地，基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿时，可以使用如下公式(6)：
其中，为空间点在第二坐标系中的位置坐标，/>为空间点在第一坐标系中的位置坐标，通过上述公式(2)计算得到，wRptz和wTptz的值根据目标旋转偏移矩阵确定。
本申请实施例对激光雷达采集的点云数据进行运动补偿时，将运动补偿过程进行解耦，分解为云台带动激光雷达旋转的旋转补偿和移动载具带动云台进行移动的移动补偿。首先，通过双线性插值算法，对点云数据中的每个空间点进行水平角度补偿，并结合激光雷达与云台之间的标定旋转偏移矩阵，进行每个空间点的旋转补偿，使得云台在不断旋转的过程中，每一帧点云数据内的空间点与云台角度一一对应，解决因云台旋转导致激光雷达采集畸变。其次，利用拓展式卡尔曼滤波，融合IMU数据和GNSS数据作为估计旋转偏移矩阵，建立表征点云数据中空间点与全局点云数据中空间点到平面的距离误差的目标函数，最后，通过以估计旋转偏移矩阵为初始值，迭代求解得出最优的旋转偏移矩阵，也即目标旋转偏移矩阵，带入运动补偿模型，完成移动旋转式激光雷达的运动补偿。
因此，本申请实施例在对激光雷达采集的点云数据进行运动补偿时，解耦了复杂的姿态变化矩阵求解，简化了求解过程，提高算法的实用性。同时，引入了基于拓展式的卡尔曼滤波，融合IMU数据和GNSS数据作为初始姿态(也即估计旋转偏移矩阵)，提供较优的初始姿态，使得求解最优值存在初始值附近，避免求解目标函数陷入局部最优点，提高了算法的实用性。
另外，采用基于拓展式的卡尔曼滤波融合IMU数据和GNSS数据，当GNSS信号被遮挡存在数据异常时，系统通过计算卡尔曼增益，对异常的数据实现低权值分配，计算结果偏向状态估计，保证系统的稳定运行。同理，当IMU设备处于长时间运行状态过程中，观测值会约束状态估计值，避免因IMU传感器长时间运行积累时飘，导致无法迭代求解出全局最优的旋转偏移矩阵。
实际应用中，在对激光雷达采集的点云数据进行位置补偿之后，即可基于第二坐标系中部分或全部全局点云数据，进行三维点云重建，生成三维地图。
本申请实施例，通过利用云台旋转带动低线束激光雷达水平转动，模拟360°×360°视野的大线束激光雷达，移动载具带动激光雷达扩大感知范围，结合移动旋转式激光雷达的运动补偿方法，实现大场景(半径≥300米)下的三维重建。本申请实施例极大程度上降低了大场景建模(半径≥300米)所需的超高线束激光雷达(≥128线)硬件门槛，通过使用低线束(16线)便可实现满足超高线束的硬件需求，有效降低成本。同时本申请实施例不限于机械式、MESH和重复式扫描激光雷达类型，适用性较强。
另外，本申请实施例不依赖于特征提取，所以在无明显特征情况下(长走廊等应用场景)运动补偿效果更佳，适用性更广泛。
下面结合图3对本申请实施例提供的激光雷达的运动补偿方法的具体实施流程进行详细说明，定位装置仍以IMU和GNSS为例。本申请实施例实施之前，可以预先对激光雷达、云台、IMU和GNSS进行时间和空间同步，然后对激光雷达采集的点云数据进行运动补偿，具体地，如图3所示，包括：
S301，获取当前帧的IMU数据、GNSS数据、激光雷达当前帧的点云数据、以及目标时间段内云台的角度变化数据。其中，目标时间段为当前帧采样时刻与前一帧采样时刻之间的时间段。
S302，将点云数据的时间戳信息，作为点云数据中最后一个空间点的采集时刻，并确定点云数据中每个空间点对应的雷达采样时刻。
具体地，可以根据激光雷达的属性参数中包括的，针对每一行中每个空间点或者每一列中每个空间点的时间误差补偿系数，基于最后一个空间点的采样时刻，确定点云数据中每个空间点的雷达采样时刻。
S303，依据云台角度变化的粒度，对角度变化数据进行拆分，确定各个云台采样时刻的云台角度。
S304，计算点云数据中每个空间点采样时刻的云台角度。
具体实施时，由于点云数据的采样频率大于云台水平转动的频率，因此，雷达采样时刻的数量大于云台采样时刻的数量，在确定点云数据中每个空间点采样时刻的云台角度时，会存在以下两种情况：
情况1、若空间点对应的雷达采样时刻存在相应的云台采样时刻，则将云台采样时刻的云台角度，确定为空间点采样时刻对应的云台角度。
情况2、若空间点对应的雷达采样时刻不存在相应的云台采样时刻，则利用双线性插值算法，基于雷达采样时刻相邻的两个云台采样时刻的云台角度，计算空间点采样时刻对应的云台角度。
S305，根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中时，其中，第一坐标系为以云台当前位置为原点建立的坐标系。
S306，对当前帧的IMU数据和GNSS数据进行融合，得到估计旋转偏移矩阵。
具体地，首先利用预积分方法对IMU数据进行累积积分，确定云台当前位置相对于云台初始位置(也即移动载具未发生移动且云台未发生位姿变化时的位置)位姿变化的估计值，基于GNSS数据，确定云台当前位置相对于云台初始位置位姿变化的观测值，然后计算拓展式卡尔曼滤波的卡尔曼增益，并以卡尔曼增益作为权值，进行观测值和估计值融合，并更新估计值，最后，更新当前位置的协方差矩阵，得到最优的估计旋转偏移矩阵。
S307，对第二坐标系中的全局点云数据进行K-D树索引，计算全局点云数据中每个空间点邻域点云的协方差矩阵，并基于协方差矩阵，计算每个空间点邻域点云的平面单位法向量；其中，第二坐标系为以云台初始位置为原点建立的坐标系，云台初始位置是指移动载具未发生移动且云台未发生位姿变化时的位置，全局点云数据为已进行位置补偿的点云数据。
S308，基于平面单位法向量，构造用于计算第一坐标系中空间点和全局点云数据中空间点到平面的距离误差的目标函数，以估计旋转偏移矩阵作为目标函数的初始位置补偿参数，使用最优化算法进行迭代计算。
S309，判断是否达到迭代终止条件，如果达到迭代终止条件(例如，目标函数收敛，达到最小值)，则执行S310，否则，继续执行S308进行迭代计算。
S310，确定目标函数的最小值，并将目标函数值最小值对应的位置补偿参数，确定为目标旋转偏移矩阵。
S311，基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在第二坐标系中的位置坐标，获得补偿后的点云数据。
基于相同的发明构思，本申请实施例还提供一种激光雷达的运动补偿装置。如图4所示，其为激光雷达的运动补偿装置400的结构示意图，可以包括：
获取单元401，用于获取激光雷达当前帧的点云数据、定位装置当前帧对应的定位数据、以及目标时间段内云台的角度变化数据；
第一位置补偿单元402，用于基于点云数据和角度变化数据，确定点云数据中每个空间点采样时刻的云台角度，并根据预先确定的激光雷达和云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，第一坐标系为以云台当前位置为原点建立的坐标系；
处理单元403，用于基于定位数据，确定用于表征云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵；
第二位置补偿单元404，用于基于目标旋转偏移矩阵，对第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。
可选的，第一位置补偿单元402，具体用于：
将点云数据的时间戳信息，作为点云数据中最后一个空间点的采集时刻，并依据激光雷达的属性参数，基于最后一个空间点的采样时刻，确定点云数据中每个空间点对应的雷达采样时刻；
依据云台角度变化的粒度，对角度变化数据进行拆分，确定各个云台采样时刻的云台角度；
针对每个空间点，分别执行以下操作：
若空间点对应的雷达采样时刻存在相应的云台采样时刻，则将云台采样时刻的云台角度，确定为空间点采样时刻对应的云台角度；
若空间点对应的雷达采样时刻不存在相应的云台采样时刻，则利用预设算法，基于雷达采样时刻相邻的两个云台采样时刻的云台角度，计算空间点采样时刻对应的云台角度。
可选的，第一位置补偿单元402，利用预设算法计算空间点采样时刻对应的云台角度时，使用以下公式计算空间点采样时刻对应的云台角度：
其中，为空间点采样时刻对应的云台角度，/>为第j个云台采样时刻，/>为第j-1个云台采样时刻，T为两个相邻云台采样时刻之间的时间差，/>为第i个雷达采样时刻，且/>位于/>和/>之间，/>为/>时刻对应的云台角度，/>为/>时刻对应的云台角度。
可选的，第一位置补偿单元402，具体用于：
使用以下公式，将每个空间点转换到第一坐标系中：
其中，为空间点在第一坐标系中的位置，/>为空间点采样时刻对应的云台角度，prRl和prTl为预先确定的激光雷达和云台之间的位置标定参数，/>为空间点的采样位置。
可选的，定位装置包括：IMU和GNSS。
可选的，处理单元403，具体用于：
对IMU的当前帧的IMU数据进行累积积分，确定云台当前位置相对于云台初始位置位姿变化的估计值，基于GNSS的当前帧的GNSS数据，确定云台当前位置相对于云台初始位置位姿变化的观测值，使用扩展式卡尔曼滤波将预测值和观测值进行融合，得到估计旋转偏移矩阵；
对第二坐标系中的全局点云数据进行K-D树索引，计算全局点云数据中每个空间点邻域点云的协方差矩阵，并基于协方差矩阵，计算每个空间点邻域点云的平面单位法向量，全局点云数据为已进行位置补偿的点云数据；
基于平面单位法向量，构造用于计算第一坐标系中空间点和全局点云数据中空间点到平面的距离误差的目标函数，以估计旋转偏移矩阵作为目标函数的初始位置补偿参数，使用最优化算法通过多次迭代计算，确定目标函数的最小值，并将目标函数值最小值对应的位置补偿参数，确定为目标旋转偏移矩阵。
可选的，目标函数为：
其中，为第一坐标系中的空间点，/>为全局点云数据中的第i个空间点，wRptz和wTptz为位置补偿参数，位置补偿参数的初始值为估计旋转偏移矩阵的值，/>为全局点云数据中第i个空间点的平面单位法向量。
可选的，所述第二位置补偿单元404具体用于：
使用以下公式，对第一坐标系中的每个空间点进行位置补偿：
其中，为空间点在第二坐标系中的位置坐标，/>为空间点在第一坐标系中的位置坐标，wRptz和wTptz的值根据目标旋转偏移矩阵确定。/>
可选的，处理单元403还用于：
基于第二坐标系中部分或全部全局点云数据，进行三维点云重建，生成三维地图。
基于相同的发明构思，本申请实施例提供一种三维点云重建系统，系统包括：激光雷达，云台、移动载具、定位装置，其中，激光雷达与云台的转动部件连接，定位装置与云台的非转动部件连接，且云台装载于移动载具上，激光雷达采集点云数据，采用本申请实施例提供的任意一种激光雷达的运动补偿方法，对采集的点云数据进行位置补偿，确定点云数据的位置坐标，并基于部分或全部点云数据的位置坐标进行三维点云重建。
本申请实施例还提供一种电子设备，其包括处理器和存储器，其中，存储器存储有计算机程序，当计算机程序被处理器执行时，使得处理器执行本申请实施例中任意一种激光雷达的运动补偿方法的步骤。
本申请实施例提供一种机器可读存储介质，该机器可读存储介质上存储有指令，该指令在被处理器执行时使得处理器被配置成执行本申请实施例中任意一种激光雷达的运动补偿方法。
在一些可能的实施方式中，本申请提供的激光雷达的运动补偿方法的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当程序产品在计算机设备上运行时，程序代码用于使计算机设备执行本说明书上述描述的根据本申请各种示例性实施方式的激光雷达的运动补偿方法中的步骤，例如，计算机设备可以执行如图2所示的步骤。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
本申请的实施方式的程序产品可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在计算装置上运行。然而，本申请的程序产品不限于此，在本申请实施例中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被命令执行系统、装置或者器件使用或者与其结合使用。
可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由命令执行系统、装置或者器件使用或者与其结合使用的程序。
可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本申请操作的程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算装置上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算装置上部分在远程计算装置上执行、或者完全在远程计算装置或服务器上执行。在涉及远程计算装置的情形中，远程计算装置可以通过任意种类的网络包括局域网(LAN)或广域网(WAN)连接到用户计算装置，或者，可以连接到外部计算装置(例如利用因特网服务提供商来通过因特网连接)。
本申请实施例中，术语“模块”或“单元”是指有预定功能的计算机程序或计算机程序的一部分，并与其他相关部分一起工作以实现预定目标，并且可以通过使用软件、硬件(如处理电路或存储器)或其组合来全部或部分实现。同样的，一个处理器(或多个处理器或存储器)可以用来实现一个或多个模块或单元。此外，每个模块或单元都可以是包含该模块或单元功能的整体模块或单元的一部分。
应当注意，尽管在上文详细描述中提及了装置的若干单元或子单元，但是这种划分仅仅是示例性的并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之，上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。
此外，尽管在附图中以特定顺序描述了本申请方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序命令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序命令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的命令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序命令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的命令产生包括命令装置的制造品，该命令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序命令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的命令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本申请的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (13)

1.一种激光雷达的运动补偿方法，应用于与云台转动部件连接的激光雷达，且所述云台装载于移动载具上，所述云台的非转动部件连接有定位装置，其特征在于，所述方法包括：
获取所述激光雷达当前帧的点云数据、所述定位装置当前帧对应的定位数据、以及目标时间段内所述云台的角度变化数据；
基于所述点云数据和所述角度变化数据，确定所述点云数据中每个空间点采样时刻的云台角度，并根据预先确定的所述激光雷达和所述云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，所述第一坐标系为以所述云台当前位置为原点建立的坐标系；
基于所述定位数据，确定用于表征所述云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵；
基于所述目标旋转偏移矩阵，对所述第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。
2.根据权利要求1所述的方法，其特征在于，所述基于所述点云数据和所述角度变化数据，确定所述点云数据中每个空间点采样时刻对应的云台角度，包括：
将所述点云数据的时间戳信息，作为所述点云数据中最后一个空间点的采集时刻，并依据所述激光雷达的属性参数，基于所述最后一个空间点的采样时刻，确定所述点云数据中每个空间点对应的雷达采样时刻；
依据所述云台角度变化的粒度，对所述角度变化数据进行拆分，确定各个云台采样时刻的云台角度；
针对每个空间点，分别执行以下操作：
若空间点对应的雷达采样时刻存在相应的云台采样时刻，则将云台采样时刻的云台角度，确定为空间点采样时刻对应的云台角度；
若空间点对应的雷达采样时刻不存在相应的云台采样时刻，则利用预设算法，基于雷达采样时刻相邻的两个云台采样时刻的云台角度，计算空间点采样时刻对应的云台角度。
3.根据权利要求2所述的方法，其特征在于，所述预设算法为使用以下公式计算空间点采样时刻对应的云台角度：
其中，为空间点采样时刻对应的云台角度，/>为第j个云台采样时刻，/>为第j-1个云台采样时刻，T为两个相邻云台采样时刻之间的时间差，/>为第i个雷达采样时刻，且/>位于/>和/>之间，/>为/>时刻对应的云台角度，/>为/>时刻对应的云台角度。
4.根据权利要求3所述的方法，其特征在于，所述根据预先确定的所述激光雷达和所述云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，包括：
使用以下公式，将每个空间点转换到第一坐标系中：
其中，为空间点在第一坐标系中的位置，/>为空间点采样时刻对应的云台角度，prRl和prTl为预先确定的所述激光雷达和所述云台之间的位置标定参数，/>为空间点的采样位置。
5.根据权利要求1-4中任一项所述的方法，其特征在于，所述定位装置包括：惯性测量单元IMU和全球导航卫星系统GNSS。
6.根据权利要求5所述的方法，其特征在于，所述基于所述定位数据，确定用于表征所述云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵，包括：
对所述IMU的当前帧的IMU数据进行累积积分，确定所述云台当前位置相对于所述云台初始位置位姿变化的估计值，基于所述GNSS的当前帧的GNSS数据，确定所述云台当前位置相对于所述云台初始位置位姿变化的观测值，使用扩展式卡尔曼滤波将所述预测值和所述观测值进行融合，得到估计旋转偏移矩阵；
对所述第二坐标系中的全局点云数据进行K-D树索引，计算所述全局点云数据中每个空间点邻域点云的协方差矩阵，并基于所述协方差矩阵，计算每个空间点邻域点云的平面单位法向量，所述全局点云数据为已进行位置补偿的点云数据；
基于所述平面单位法向量，构造用于计算所述第一坐标系中空间点和全局点云数据中空间点到平面的距离误差的目标函数，以所述估计旋转偏移矩阵作为所述目标函数的初始位置补偿参数，使用最优化算法通过多次迭代计算，确定所述目标函数的最小值，并将所述目标函数值最小值对应的位置补偿参数，确定为目标旋转偏移矩阵。
7.根据权利要求6所述的方法，其特征在于，所述目标函数为：
其中，为所述第一坐标系中的空间点，/>为所述全局点云数据中的第i个空间点，wRptz和wTptz为位置补偿参数，所述位置补偿参数的初始值为估计旋转偏移矩阵的值，/>为所述全局点云数据中第i个空间点的平面单位法向量。
8.根据权利要求1-4中任一项所述的方法，其特征在于，所述基于所述目标旋转偏移矩阵，对所述第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，包括：
使用以下公式，对所述第一坐标系中的每个空间点进行位置补偿：
其中，为空间点在所述第二坐标系中的位置坐标，/>为空间点在所述第一坐标系中的位置坐标，wRptz和wTptz的值根据所述目标旋转偏移矩阵确定。
9.根据权利要求1-4中任一项所述的方法，其特征在于，所述还包括：
基于所述第二坐标系中部分或全部全局点云数据，进行三维点云重建，生成三维地图。
10.一种激光雷达的运动补偿装置，应用于与云台转动部件连接的激光雷达，且所述云台装载于移动载具上，所述云台的非转动部件连接有定位装置，其特征在于，包括：
获取单元，用于获取所述激光雷达当前帧的点云数据、所述定位装置当前帧对应的定位数据、以及目标时间段内所述云台的角度变化数据；
第一位置补偿单元，用于基于所述点云数据和所述角度变化数据，确定所述点云数据中每个空间点采样时刻的云台角度，并根据预先确定的所述激光雷达和所述云台之间的位置标定参数，利用每个空间点采样时刻对应的云台角度，将每个空间点转换到第一坐标系中，所述第一坐标系为以所述云台当前位置为原点建立的坐标系；
处理单元，用于基于所述定位数据，确定用于表征所述云台当前位置相对于云台初始位置位姿变化的目标旋转偏移矩阵；
第二位置补偿单元，用于基于所述目标旋转偏移矩阵，对所述第一坐标系中的每个空间点进行位置补偿，得到每个空间点在预先定义的第二坐标系中的位置坐标，获得补偿后的点云数据。
11.一种三维点云重建系统，其特征在于，所述系统包括：激光雷达，云台、移动载具、定位装置，其中，所述激光雷达与所述云台的转动部件连接，所述定位装置与所述云台的非转动部件连接，且所述云台装载于移动载具上，所述激光雷达采集点云数据，采用如权利要求1-9中任一项所述的方法，对采集的点云数据进行位置补偿，确定所述点云数据的位置坐标，并基于部分或全部点云数据的位置坐标进行三维点云重建。
12.一种电子设备，其特征在于，其包括处理器和存储器，其中，所述存储器存储有计算机程序，当所述计算机程序被所述处理器执行时，使得所述处理器执行权利要求1～9中任一所述方法的步骤。
13.一种机器可读存储介质，该机器可读存储介质上存储有指令，其特征在于，该指令在被处理器执行时使得所述处理器被配置成执行根据权利要求1-9中任一项所述的方法。
CN202311758567.0A 2023-12-19 2023-12-19 一种激光雷达的运动补偿方法、装置、系统、设备及介质 Pending CN117518196A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311758567.0A CN117518196A (zh) 2023-12-19 2023-12-19 一种激光雷达的运动补偿方法、装置、系统、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311758567.0A CN117518196A (zh) 2023-12-19 2023-12-19 一种激光雷达的运动补偿方法、装置、系统、设备及介质

Publications (1)

Publication Number Publication Date
CN117518196A true CN117518196A (zh) 2024-02-06

Family

ID=89744089

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311758567.0A Pending CN117518196A (zh) 2023-12-19 2023-12-19 一种激光雷达的运动补偿方法、装置、系统、设备及介质

Country Status (1)

Country Link
CN (1) CN117518196A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117788592A (zh) * 2024-02-26 2024-03-29 北京理工大学前沿技术研究院 一种矿井车辆的雷达点云处理装置、方法、设备及介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117788592A (zh) * 2024-02-26 2024-03-29 北京理工大学前沿技术研究院 一种矿井车辆的雷达点云处理装置、方法、设备及介质
CN117788592B (zh) * 2024-02-26 2024-06-07 北京理工大学前沿技术研究院 一种矿井车辆的雷达点云处理装置、方法、设备及介质

Similar Documents

Publication Publication Date Title
CN108717710B (zh) 室内环境下的定位方法、装置及系统
CN110178048B (zh) 交通工具环境地图生成和更新的方法和系统
KR102548282B1 (ko) 고정밀도 맵 작성 방법 및 장치
US10937191B2 (en) Predictive simultaneous localization and mapping system using prior user session positional information
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN111429574A (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
JP2019532433A (ja) リアルタイムオンラインエゴモーション推定を有するレーザスキャナ
CN110298914B (zh) 一种建立果园中果树冠层特征地图的方法
CN110375739B (zh) 一种移动端视觉融合定位方法、系统及电子设备
CN110197615B (zh) 用于生成地图的方法及装置
Zhao et al. Learning-based bias correction for time difference of arrival ultra-wideband localization of resource-constrained mobile robots
CN111338383B (zh) 基于gaas的自主飞行方法及系统、存储介质
Tulldahl et al. Accuracy evaluation of 3D lidar data from small UAV
CN117518196A (zh) 一种激光雷达的运动补偿方法、装置、系统、设备及介质
Popov et al. Optical flow and inertial navigation system fusion in the UAV navigation
CN114111776A (zh) 定位方法及相关装置
Kaijaluoto Precise indoor localization for mobile laser scanner
CN115900712A (zh) 一种信源可信度评价组合定位方法
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
CN114529585A (zh) 基于深度视觉和惯性测量的移动设备自主定位方法
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及系统
Wu et al. AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry
Jiang et al. 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments
Tiozzo Fasiolo et al. Experimental evaluation and comparison of LiDAR SLAM algorithms for mobile robotics
Ali et al. Multi-sensor depth fusion framework for real-time 3D reconstruction

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