CN110658530B - 一种基于双激光雷达数据融合的地图构建方法、系统及地图 - Google Patents

一种基于双激光雷达数据融合的地图构建方法、系统及地图 Download PDF

Info

Publication number
CN110658530B
CN110658530B CN201910708030.0A CN201910708030A CN110658530B CN 110658530 B CN110658530 B CN 110658530B CN 201910708030 A CN201910708030 A CN 201910708030A CN 110658530 B CN110658530 B CN 110658530B
Authority
CN
China
Prior art keywords
point
point cloud
map
matrix
double
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.)
Active
Application number
CN201910708030.0A
Other languages
English (en)
Other versions
CN110658530A (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.)
Beijing Union University
Original Assignee
Beijing Union University
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 Beijing Union University filed Critical Beijing Union University
Priority to CN201910708030.0A priority Critical patent/CN110658530B/zh
Publication of CN110658530A publication Critical patent/CN110658530A/zh
Application granted granted Critical
Publication of CN110658530B publication Critical patent/CN110658530B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

Landscapes

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

Abstract

本发明提供一种基于双激光雷达数据融合的地图构建方法、系统及地图，其中方法包括构建点云地图，还包括以下步骤：对双激光雷达进行标定和数据融合；采用快速空间模型点云配准算法进行帧间点云粗配准；采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云精准的拼接。本发明首先对双激光雷达进行空间、时间同步，对激光雷达进行数据融合；然后采用一种快速空间模型点云配准算法，进行帧间点云粗配准，再利用最近邻迭代点云配准进行点云配准纠正，实现高精度的帧间点云拼接；最后完成点云地图的构建。

Description

一种基于双激光雷达数据融合的地图构建方法、系统及地图
技术领域
本发明涉及无人驾驶的技术领域，特别是一种基于双激光雷达数据融合的地图构建方法、系统及地图。
背景技术
车载激光雷达LIDAR能够自动快速地获取高精度、高密度的周围三维坐标信息，并在城市三维重建、地形测绘等领域具有重要的应用。随着无人车技术的发展，LIDAR作为无人车中感知领域重要的传感器之一，在无人车的SLAM(Simultaneous Localization andMapping，即时定位与地图构建)技术中具有重要的研究价值。
激光雷达是无人驾驶技术开发的重要传感器之一，可通过扫描周围环境构建点云地图。但单个激光雷达点云数据对周围环境信息表示不足。
公开号为CN107462897A的发明专利公开了一种基于激光雷达的三维建图的方法，首先获得带有坐标信息的点云；然后使用双边滤波算法对点云进行滤波处理；接着由激光雷达测距算法计算激光雷达点云中每一个特征点的位移；最后用绘图算法将扫描到的点云配准到世界坐标系上，构成三维点云图。该方法是基于激光雷达单个传感器进行三维建图，但单个激光雷达点云数据对周围环境信息表示不足；且由于测距算法要计算点云中每一个特征点的位移，当环境复杂、点云数量大，就会导致计算量大，而导致无法达到实时性要求。
发明内容
为了解决上述的技术问题，本发明提出一种基于双激光雷达数据融合的地图构建方法及系统，首先对双激光雷达进行空间、时间同步，对激光雷达进行数据融合；然后采用一种快速空间模型点云配准算法，进行帧间点云粗配准，再利用最近邻迭代点云配准进行点云配准纠正，实现高精度的帧间点云拼接；最后完成点云地图的构建。
本发明的第一目的是提供一种基于双激光雷达数据融合的地图构建方法，包括构建点云地图，还包括以下步骤：
步骤1：对双激光雷达进行标定和数据融合；
步骤2：采用快速空间模型点云配准算法进行帧间点云粗配准；
步骤3：采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云精准的拼接。
优选的是，所述双激光雷达标定是指对不同位置的两个激光雷达进行标定，包括空间同步和/或时间同步。
在上述任一方案中优选的是，所述空间同步是指进行物理标定，以其中的一个激光雷达为中心，另一个激光雷达调整角度值和方向值进行标定，以扫描同一个障碍物进行空间标定。
在上述任一方案中优选的是，所述空间标定就是利用三维坐标系的旋转矩阵及平移矩阵，三维坐标系的旋转表述为向量与合适尺寸的方阵的乘积。
在上述任一方案中优选的是，设定所述三维坐标系中的一个向量其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点Q、点N，/>绕Z轴旋转θ角，旋转矩阵为/> 绕X轴旋转θ角，旋转矩阵为：/> 绕Y轴旋转θ角，旋转矩阵为：在Z、X、Y轴的平移矩阵为：/>
在上述任一方案中优选的是，所述步骤2包括以下子步骤：
步骤21：设相邻帧间点云集为S和W，计算S点云集的正态分布；
步骤22：初始化坐标变换参数P；
步骤23：将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’；
步骤24：求每个点映射变换后的正态分布；
步骤25：将每个点的概率密度相加，评估坐标的变换参数；
步骤26：使用Hessian矩阵优化每个点云的概率密度之和s(p)；
步骤27：判断是否满足收敛条件，如果不满足则重新执行步骤23至26，如果满足收敛条件，则完成点云粗配准。
在上述任一方案中优选的是，所述坐标变换参数P的计算公式为：其中，P为包括旋转矩阵和平移矩阵的4*4矩阵，R是3*3的旋转矩阵，t是3*1的平移矩阵。
在上述任一方案中优选的是，所述正态分布表示为：其中，q是每个立方体内点的均值，∑是每个立方体内点的协方差矩阵。
在上述任一方案中优选的是，所述变换参数的公式为：
在上述任一方案中优选的是，所述步骤3包括以下子步骤：
步骤31：计算目标点云W’中的每一个点在S点集中的对应最近点；
步骤32：利用SVD分解计算求得旋转矩阵R和平移矩阵t，使得对应点平均距离E(R，t)最小；
步骤33：对W’使用步骤32求得的所述旋转矩阵R和平移矩阵t，得到新的变换点集W”；
步骤34：判断所述点集W”与S点集是否满足目标函数要求，如果满足则停止迭代，如果不满足，重新执行步骤33，直到满足要求为止。
在上述任一方案中优选的是，所述对应点平均距离E(R，t)的计算公式为：E(R，t)＝∑||W”-(S*R+t)||2。
在上述任一方案中优选的是，所述构建点云地图包括以下子步骤：
步骤41：设n时刻扫描周围环境得到的点云为Xn，n+1时刻扫描周围环境得到的点云为Xn+1，n时刻保存的有效地图为Mn；
步骤42：采用空间模型算法进行Xn与Xn+1的点云配准，经过平移与旋转得到了坐标的转换，把Xn+1的点云坐标与Xn的点云坐标转换为同一坐标系下，得到n+1时刻的有效地图为Mn+1；
步骤43：采用点到点最近邻迭代算法进行对Xn与Xn+1的点云之间的距离校正，校正后得到n+1时刻的有效地图为M’n+1；
步骤44：经过点云依次叠加，直至所有接收的点云进行坐标变换，最后得到精确的点云地图为M。
本发明的第二目的是提供一种基于双激光雷达数据融合的地图构建系统，包括用于构建点云地图的地图构建模块，还包括以下模块：
双雷达标定模块：用于对双激光雷达进行标定和数据融合；
粗配准模块：用于采用快速空间模型点云配准算法进行帧间点云粗配准；
点云拼接模块：用于采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云精准的拼接。
优选的是，所述双激光雷达标定是指对不同位置的两个激光雷达进行标定，包括空间同步和/或时间同步。
在上述任一方案中优选的是，所述空间同步是指进行物理标定，以其中的一个激光雷达为中心，另一个激光雷达调整角度值和方向值进行标定，以扫描同一个障碍物进行空间标定。
在上述任一方案中优选的是，所述空间标定就是利用三维坐标系的旋转矩阵及平移矩阵，三维坐标系的旋转表述为向量与合适尺寸的方阵的乘积。
在上述任一方案中优选的是，设定所述三维坐标系中的一个向量其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点Q、点N，/>绕Z轴旋转θ角，旋转矩阵为/> 绕X轴旋转θ角，旋转矩阵为：/> 绕Y轴旋转θ角，旋转矩阵为：在Z、X、Y轴的平移矩阵为：/>
在上述任一方案中优选的是，所述帧间点云粗配准包括以下子步骤：
步骤21：设相邻帧间点云集为S和W，计算S点云集的正态分布；
步骤22：初始化坐标变换参数P；
步骤23：将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’；
步骤24：求每个点映射变换后的正态分布；
步骤25：将每个点的概率密度相加，评估坐标的变换参数；
步骤26：使用Hessian矩阵优化每个点云的概率密度之和s(p)；
步骤27：判断是否满足收敛条件，如果不满足则重新执行步骤23至26，如果满足收敛条件，则完成点云粗配准。
在上述任一方案中优选的是，所述坐标变换参数P的计算公式为：其中，P为包括旋转矩阵和平移矩阵的4*4矩阵，R是3*3的旋转矩阵，t是3*1的平移矩阵。
在上述任一方案中优选的是，所述正态分布表示为：其中，q是每个立方体内点的均值，∑是每个立方体内点的协方差矩阵。
在上述任一方案中优选的是，所述变换参数的公式为：
在上述任一方案中优选的是，所述帧间点云精准的拼接包括以下子步骤：
步骤31：计算目标点云W’中的每一个点在S点集中的对应最近点；
步骤32：利用SVD分解计算求得旋转矩阵R和平移矩阵t，使得对应点平均距离E(R，t)最小；
步骤33：对W’使用步骤32求得的所述旋转矩阵R和平移矩阵t，得到新的变换点集W”；
步骤34：判断所述点集W”与S点集是否满足目标函数要求，如果满足则停止迭代，如果不满足，重新执行步骤33，直到满足要求为止。
在上述任一方案中优选的是，所述对应点平均距离E(R，t)的计算公式为：E(R，t)＝∑||W”-(S*R+t)||2。
在上述任一方案中优选的是，所述点云地图的构建包括以下子步骤：
步骤41：设n时刻扫描周围环境得到的点云为Xn，n+1时刻扫描周围环境得到的点云为Xn+1，n时刻保存的有效地图为Mn；
步骤42：采用空间模型算法进行Xn与Xn+1的点云配准，经过平移与旋转得到了坐标的转换，把Xn+1的点云坐标与Xn的点云坐标转换为同一坐标系下，得到n+1时刻的有效地图为Mn+1；
步骤43：采用点到点最近邻迭代算法进行对Xn与Xn+1的点云之间的距离校正，校正后得到n+1时刻的有效地图为M’n+1；
步骤44：经过点云依次叠加，直至所有接收的点云进行坐标变换，最后得到精确的点云地图为M。
本发明的第三目的是提供一种基于双激光雷达数据融合的地图，所述地图采用如权利要求1所述的方法构建。
本发明提出了一种基于双激光雷达数据融合的地图构建方法和系统，涉及一种精确度高的点云配准算法，能在提高算法精度的同时，也提高了算法的效率，实现高精度的帧间点云拼接，进而完成点云地图的构建，不仅可以丰富点云地图信息，而且提高了地图构建的精度，能够构建复杂环境的三维点云地图。
附图说明
图1为按照本发明的基于双激光雷达数据融合的地图构建方法的一优选实施例的流程图。
图1A为按照本发明的基于双激光雷达数据融合的地图构建方法的如图1所示实施例的帧间点云粗配准方法流程图。
图1B为按照本发明的基于双激光雷达数据融合的地图构建方法的如图1所示实施例的帧间点云精准拼接方法流程图。
图1C为按照本发明的基于双激光雷达数据融合的地图构建方法的如图1所示实施例的点云地图构建方法流程图。
图2为按照本发明的基于双激光雷达数据融合的地图构建系统的一优选实施例的模块图。
图3为按照本发明的基于双激光雷达数据融合的地图构建方法的另一优选实施例的直角坐标系图。
图4为按照本发明的基于双激光雷达数据融合的地图构建方法的如图3所示实施例的整体算法流程图。
图5为按照本发明的基于双激光雷达数据融合的地图构建方法的如图3所示实施例的无人接驳车实物图。
具体实施方式
下面结合附图和具体的实施例对本发明做进一步的阐述。
实施例一
如图1、2所示，执行步骤100，双雷达标定模块200对双激光雷达进行标定和数据融合。所述双激光雷达标定是指对不同位置的两个激光雷达进行标定，包括空间同步和/或时间同步。空间同步是指进行物理标定，以其中的一个激光雷达为中心，另一个激光雷达调整角度值和方向值进行标定，以扫描同一个障碍物进行空间标定。空间标定就是利用三维坐标系的旋转矩阵及平移矩阵，三维坐标系的旋转表述为向量与合适尺寸的方阵的乘积。设定所述三维坐标系中的一个向量其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点Q、点N，/>绕Z轴旋转θ角，旋转矩阵为/> 绕X轴旋转θ角，旋转矩阵为： 绕Y轴旋转θ角，旋转矩阵为：/> 在Z、X、Y轴的平移矩阵为：/>
执行步骤110，粗配准模块210采用快速空间模型点云配准算法进行帧间点云粗配准。在步骤110中，执行步骤111，设相邻帧间点云集为S和W，计算S点云集的正态分布。执行步骤112，初始化坐标变换参数P，坐标变换参数P的计算公式为：其中，P为包括旋转矩阵和平移矩阵的4*4矩阵，R是3*3的旋转矩阵，t是3*1的平移矩阵。执行步骤113，将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’。执行步骤114，求每个点映射变换后的正态分布，正态分布表示为：其中，q是每个立方体内点的均值，∑是每个立方体内点的协方差矩阵。执行步骤115，将每个点的概率密度相加，评估坐标的变换参数，变换参数的公式为：/>执行步骤116，使用Hessian矩阵优化每个点云的概率密度之和s(p)。执行步骤117，判断是否满足收敛条件。如果不满足则重新执行步骤113，将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’。如果满足收敛条件，则执行步骤118，完成帧间点云粗配准。
执行步骤120，点云拼接模块220采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云精准的拼接。在步骤120中，执行步骤121，计算目标点云W’中的每一个点在S点集中的对应最近点。执行步骤122，利用SVD分解计算求得旋转矩阵R和平移矩阵t，使得对应点平均距离E(R，t)最小，对应点平均距离E(R，t)的计算公式为：E(R，t)＝∑||W”-(S*R+t)||2。执行步骤123，对W’使用步骤122求得的所述旋转矩阵R和平移矩阵t，得到新的变换点集W”。执行步骤124，判断所述点集W”与S点集是否满足目标函数要求，，如果不满足，重新执行步骤123，对W’使用步骤122求得的所述旋转矩阵R和平移矩阵t，得到新的变换点集W”。如果满足则执行步骤125，停止迭代。
执行步骤130，地图构建模块230构建精确的点云地图。在步骤130中，执行步骤131，设n时刻扫描周围环境得到的点云为Xn，n+1时刻扫描周围环境得到的点云为Xn+1，n时刻保存的有效地图为Mn。执行步骤132，采用空间模型算法进行Xn与Xn+1的点云配准，经过平移与旋转得到了坐标的转换，把Xn+1的点云坐标与Xn的点云坐标转换为同一坐标系下，得到n+1时刻的有效地图为mn+1。执行步骤133，采用点到点最近邻迭代算法进行对Xn与Xn+1的点云之间的距离校正，校正后得到n+1时刻的有效地图为M’n+1。执行步骤134，经过点云依次叠加，直至所有接收的点云进行坐标变换，最后得到精确的点云地图为M。
根据上述方法中的步骤或上述系统中的模块能够构建一种基于双激光雷达数据融合的地图。
实施例二
本发明提出一种双激光雷达数据融合的进行地图构建方法，首先进行对双激光雷达进行标定、数据融合；然后，采用一种快速空间模型点云配准算法进行帧间点云粗配准，其次采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云的拼接；最后完成精确的点云地图构建。
1.双激光雷达标定、数据融合：
首先对不同位置的两个激光雷达进行标定，包括空间同步、时间同步。空间同步就是进行物理标定，以其中的一个激光雷达为中心，另一个激光雷达调整角度值和方向值进行标定，以扫描同一个障碍物进行空间标定。时间同步利用ROS(Robot Operating System,机器人操作系统)机制同时接收双激光雷达点云数据，然后进行数据的融合。其中空间标定就是利用三维坐标系的旋转矩阵及平移矩阵，三维的旋转可以表述为向量与合适尺寸的方阵的乘积。最终一个旋转等价于在另一个不同坐标系下对点位置的重新表述，而平移矩阵就是在不同方向的平移。坐标系旋转角度θ则等同于将目标点围绕坐标原点反方向旋转同样的角度θ。若以坐标系的三个坐标轴X、Y、Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上作二维旋转。假设三维坐标系中的某一向量其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点Q、点N，如图3所示。
绕Z轴旋转θ角，旋转矩阵为
绕X轴旋转θ角，旋转矩阵为：
绕Y轴旋转θ角，旋转矩阵为：
在Z、X、Y轴的平移矩阵为：
通过旋转矩阵及平移矩阵，把位置不同的激光雷达进行空间上的标定，使在同一个坐标系下进行表示。然后时间同步利用ROS机制同时接收双激光雷达点云数据，然后进行数据的融合，以上步骤实现对双激光雷达的标定。
2.提出一种两步法的点云配准算法实现帧间点云拼接：
本发明提出了一种两步法的点云配准实现帧间点云精确的拼接，首先提出一种快速空间模型技术，将点云投放在三维的单元格中，将每个三维的单元格内的点云数据转换成一个连续可微的概率密度分布函数，求出点云之间的配准，完成帧间点云粗配准；然后利用点到点最近邻迭代点云配准算法对已配准的点云进行校正，实现帧间点云精确的拼接。
设相邻帧间点云集为S和W，利用快速空间模型点云配准算法与点到点最近邻迭代点云配准算法实现帧间点云精确的拼接，
(1)计算S点云集的正态分布；
(2)初始化坐标变换参数P
其中，是3*3的旋转矩阵，t是3*1的平移矩阵；
(3)将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’；
(4)求每个点映射变换后的正态分布；
其中q和∑是每个立方体内点的均值和协方差矩阵；
(5)将每个点的概率密度相加，去评估坐标的变换参数：
(公式7)
(6)使用Hessian矩阵法优化s(p)；
(7)跳转到第3步继续执行，直到满足收敛条件为之。
至此，利用快速空间模型点云配准算法完成了对相邻帧之间的点云粗配准过程，并返回最优解的坐标变换参数P及变换后的点云集W’。下面将利用点到点最近邻迭代点云配准算法对已配准的点云进行校正：
(8)计算目标点云W’中的每一个点在S点集中的对应最近点；
(9)求得使对应点对平均距离最小的变换，利用SVD分解求得旋转矩阵R和平移矩阵t，使得E(R，t)最小：
E(R，t)＝∑||W”-(S*R+t)||2 (公式8)
(10)对W’使用上一步求得的所述旋转矩阵R和平移矩阵t，得到新的变换点集W”；
(11)如果变换后的点集W”与S点集满足目标函数要求，即两点集的平均距离小于给定的阈值，则停止迭代计算，否则重新计算(9)继续迭代，直到满足收敛条件为止。
通过以上两步法实现帧间精确的拼接。
3.完成精确的点云地图构建：
通过车载传感器返回的点云进行地图构建，如果以10Hz返回的点云数据量经过长时间的累积是很庞大的，本发明采用了一种以2Hz低频率方式进行点云地图的构建。
设n时刻扫描周围环境得到的点云为Xn，n+1时刻扫描周围环境得到的点云为Xn+1，n时刻保存的有效地图为Mn，本文采用空间模型算法进行Xn与Xn+1的点云配准，经过平移与旋转得到了坐标的转换，把Xn+1的点云坐标与Xn的点云坐标转换为同一坐标系下，得到n+1时刻的有效地图为Mn+1，然后本发明采用点到点最近邻迭代算法进行对Xn与Xn+1的点云之间的距离校正，校正后得到n+1时刻的有效地图为M’n+1，经过点云依次叠加，直至所有接收的点云进行坐标变换，最后得到精确的点云地图为M。
整体算法流程图如图4所示。
实施例三
本发明提出一种双激光雷达数据融合的进行地图构建方法，包括双激光雷达标定及数据的融合；基于一种精确度高的点云配准算法实现帧间点云拼接，完成精确度较高的点云地图的构建。
1、本发明采用双激光雷达数据融合进行无人驾驶车的地图构建，双激光雷达补充了单激光雷达点云信息不足的特点，为点云地图的构建提供了较为丰富的点云信息。为此本发明涉及双激光雷达的标定及标定后的点云信息融合方法。
2、本发明提出了一种两步法的帧间点云配准算法，实现帧间点云的精确拼接，进而完成精确点云地图的构建。首先进行对双激光雷达进行标定、数据融合；然后采用一种快速空间模型技术进行帧间点云粗配准，其次采用点到点最近邻迭代点云配准进行位姿的矫正，得到最优的帧间变换，实现帧间点云的拼接；最后完成精确的点云地图。
以下结合实例进行详细阐述。
1、场地无人车平台
本实例采用的是北京联合大学机器人学院自主研发的“小旋风”系列接驳车。无人车具体配置如下：
车体长2.78米，宽1.2米，高1.8米。车辆配备2个位置不同的16线激光雷达、围绕车身12个超声波雷达等传感设备，并配置i7四核高性能工业控制计算机，如图5所示，其中以A激光雷达为标准，B激光雷达变换参数，使A、B激光雷达标定在同一坐标系下。
本实例在Linux系统下的ROS操作平台上实现无人车激光点云地图的构建。
2、算法实现
设相邻帧间点云集为S和W，利用二步法，即快速空间模型技术和点到点最近邻迭代点云配准算法实现帧间点云精确的拼接，
(1)计算S点云集的正态分布；
(2)初始化坐标变换参数P：
其中R是3*3的旋转矩阵，t是3*1的平移矩阵；
(3)将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’；
(4)求每个点映射变换后的正态分布；
其中q和∑是每个立方体内点的均值和协方差矩阵；
(5)将每个点的概率密度相加，去评估坐标的变换参数：
(6)使用Hessian矩阵法优化s(p)；
(7)跳转到第3步继续执行，直到满足收敛条件为之。
此时，完成了对相邻帧之间的点云粗配准过程，并返回最优解的坐标变换参数P及变换后的点云集W’。
(8)计算目标点云W’中的每一个点在S点集中的对应最近点；
(9)求得使对应点对平均距离最小的变换，利用SVD分解求得旋转矩阵R和平移矩阵t，使得E(R，t)最小：
E(R，t)＝∑||W”-(S*R+t)||2 (公式8)
(10)对W’使用上一步求得的旋转矩阵R和平移矩阵t，得到新的变换点集W”；
(11)如果变换后的点集W”与S点集点集满足目标函数要求，即两点集的平均距离小于给定的阈值，则停止迭代计算，否则重新计算(9)继续迭代，直到满足收敛条件为止。
通过以上两步法实现帧间精确的拼接，最终实现精确的点云地图。
为了更好地理解本发明，以上结合本发明的具体实施例做了详细描述，但并非是对本发明的限制。凡是依据本发明的技术实质对以上实施例所做的任何简单修改，均仍属于本发明技术方案的范围。本说明书中每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似的部分相互参见即可。对于系统实施例而言，由于其与方法实施例基本对应，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

Claims (9)

1.一种基于双激光雷达数据融合的地图构建方法，包括构建点云地图，其特征在于，还包括以下步骤：
步骤1：对双激光雷达进行标定和数据融合；
步骤2：采用快速空间模型点云配准算法进行帧间点云粗配准；
步骤3：采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云精准的拼接，包括以下子步骤：
步骤31：计算目标点云W’中的每一个点在S点集中的对应最近点；
步骤32：利用SVD分解计算求得旋转矩阵R′和平移矩阵t′，使得对应点平均距离E(R′，t′)最小，所述对应点平均距离E(R′，t′)的计算公式为：E(R′，t′)＝∑||W’-(S*R′+t′)||2；
步骤33：对W’使用步骤32求得的所述旋转矩阵R′和平移矩阵t′，得到新的变换点集W”；
步骤34：判断所述点集W”与S点集是否满足目标函数要求，如果满足则停止迭代，如果不满足，重新执行步骤33，直到满足要求为止；所述构建点云地图包括以下子步骤：
步骤41：设n时刻扫描周围环境得到的点云为Xn，n+1时刻扫描周围环境得到的点云为Xn+1，n时刻保存的有效地图为Mn；
步骤42：采用空间模型算法进行Xn与Xn+1的点云配准，经过平移与旋转得到了坐标的转换，把Xn+1的点云坐标与Xn的点云坐标转换为同一坐标系下，得到n+1时刻的有效地图为Mn+1；
步骤43：采用点到点最近邻迭代算法进行对Xn与Xn+1的点云之间的距离校正，校正后得到n+1时刻的有效地图为M’n+1；
步骤44：经过点云依次叠加，直至所有接收的点云进行坐标变换，最后得到精确的点云地图为M。
2.如权利要求1所述的基于双激光雷达数据融合的地图构建方法，其特征在于，双激光雷达标定是指对不同位置的两个激光雷达进行标定，包括空间同步和/或时间同步。
3.如权利要求2所述的基于双激光雷达数据融合的地图构建方法，其特征在于，所述空间同步是指进行物理标定，以其中的一个激光雷达为中心，另一个激光雷达调整角度值和方向值进行标定，以扫描同一个障碍物进行空间标定。
4.如权利要求3所述的基于双激光雷达数据融合的地图构建方法，其特征在于，所述空间标定就是利用三维坐标系的旋转矩阵及平移矩阵，三维坐标系的旋转表述为向量与合适尺寸的方阵的乘积。
5.如权利要求4所述的基于双激光雷达数据融合的地图构建方法，其特征在于，设定所述三维坐标系中的一个向量其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点Q、点N，/>绕Z轴旋转θ角，旋转矩阵为/> 绕X轴旋转θ角，旋转矩阵为：/> 绕Y轴旋转θ角，旋转矩阵为：/>在Z、X、Y轴的平移矩阵为：/>
6.如权利要求5所述的基于双激光雷达数据融合的地图构建方法，其特征在于，所述步骤2包括以下子步骤：
步骤21：设相邻帧间点云集为S和W，计算S点云集的正态分布；
步骤22：初始化坐标变换参数P；
步骤23：将W点云集根据坐标转换参数映射到与S同一坐标系中，并得到映射后的点云集合W’；
步骤24：求每个点映射变换后的正态分布；
步骤25：将每个点的概率密度相加，评估坐标的变换参数；
步骤26：使用Hessian矩阵优化每个点云的概率密度之和s(p)；
步骤27：判断是否满足收敛条件，如果不满足则重新执行步骤23至26，如果满足收敛条件，则完成点云粗配准。
7.如权利要求6所述的基于双激光雷达数据融合的地图构建方法，其特征在于，所述坐标变换参数P的计算公式为：其中，P为包括旋转矩阵和平移矩阵的4*4矩阵，R是3*3的旋转矩阵，t是3*1的平移矩阵。
8.如权利要求7所述的基于双激光雷达数据融合的地图构建方法，其特征在于，所述正态分布表示为：
其中，q是每个立方体内点的均值，∑是每个立方体内点的协方差矩阵。
9.一种基于双激光雷达数据融合的地图构建系统，包括用于构建的点云地图的地图构建模块，其特征在于，还包括以下模块：
双雷达标定模块：用于对双激光雷达进行标定和数据融合；
粗配准模块：用于采用快速空间模型点云配准算法进行帧间点云粗配准；
点云拼接模块：用于采用最近邻迭代进行位姿的矫正，得到最优的帧间变换，实现帧间点云精准的拼接，所述帧间点云精准的拼接包括以下子步骤：
步骤31：计算目标点云W’中的每一个点在S点集中的对应最近点；
步骤32：利用SVD分解计算求得旋转矩阵R′和平移矩阵t′，使得对应点平均距离E(R′，t′)最小，所述对应点平均距离E(R′，t′)的计算公式为：E(R′，t′)＝∑||W’-(S*R′+t′)||2；
步骤33：对W’使用步骤32求得的所述旋转矩阵R′和平移矩阵t′，得到新的变换点集W”；
步骤34：判断所述点集W”与S点集是否满足目标函数要求，如果满足则停止迭代，如果不满足，重新执行步骤33，直到满足要求为止；所述点云地图的构建包括以下子步骤：
步骤41：设n时刻扫描周围环境得到的点云为Xn，n+1时刻扫描周围环境得到的点云为Xn+1，n时刻保存的有效地图为Mn；
步骤42：采用空间模型算法进行Xn与Xn+1的点云配准，经过平移与旋转得到了坐标的转换，把Xn+1的点云坐标与Xn的点云坐标转换为同一坐标系下，得到n+1时刻的有效地图为Mn+1；
步骤43：采用点到点最近邻迭代算法进行对Xn与Xn+1的点云之间的距离校正，校正后得到n+1时刻的有效地图为M’n+1；
步骤44：经过点云依次叠加，直至所有接收的点云进行坐标变换，最后得到精确的点云地图为M。
CN201910708030.0A 2019-08-01 2019-08-01 一种基于双激光雷达数据融合的地图构建方法、系统及地图 Active CN110658530B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910708030.0A CN110658530B (zh) 2019-08-01 2019-08-01 一种基于双激光雷达数据融合的地图构建方法、系统及地图

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910708030.0A CN110658530B (zh) 2019-08-01 2019-08-01 一种基于双激光雷达数据融合的地图构建方法、系统及地图

Publications (2)

Publication Number Publication Date
CN110658530A CN110658530A (zh) 2020-01-07
CN110658530B true CN110658530B (zh) 2024-02-23

Family

ID=69036375

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910708030.0A Active CN110658530B (zh) 2019-08-01 2019-08-01 一种基于双激光雷达数据融合的地图构建方法、系统及地图

Country Status (1)

Country Link
CN (1) CN110658530B (zh)

Families Citing this family (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111191426B (zh) * 2020-01-08 2023-03-14 山东大学 一种基于Gaussian程序输出文件的数据提取及处理方法
CN110850439B (zh) * 2020-01-15 2020-04-21 奥特酷智能科技(南京)有限公司 一种高精度三维点云地图构建方法
WO2021189479A1 (zh) * 2020-03-27 2021-09-30 深圳市速腾聚创科技有限公司 路基传感器的位姿校正方法、装置和路基传感器
CN111553937B (zh) * 2020-04-23 2023-11-21 东软睿驰汽车技术（上海）有限公司 激光点云地图构建方法、装置、设备及系统
CN111413684A (zh) * 2020-05-07 2020-07-14 沃行科技(南京)有限公司 一种基于多激光雷达数据融合的方法
CN111596659A (zh) * 2020-05-14 2020-08-28 福勤智能科技（昆山）有限公司 基于麦克纳姆轮的自动导引车及系统
CN111596665B (zh) * 2020-05-29 2021-06-22 浙江大学 一种适用于腿足机器人规划的稠密高度地图构建方法
CN111612829B (zh) * 2020-06-03 2024-04-09 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、系统、终端和存储介质
CN113866779A (zh) * 2020-06-30 2021-12-31 上海商汤智能科技有限公司 点云数据的融合方法、装置、电子设备及存储介质
CN112198491B (zh) * 2020-09-30 2023-06-09 广州赛特智能科技有限公司 一种基于低成本二维激光雷达的机器人三维感知系统及其方法
CN112305521B (zh) * 2020-11-03 2021-11-30 福勤智能科技（昆山）有限公司 双激光雷达相对位置标定方法、装置、设备和存储介质
CN112462758B (zh) * 2020-11-06 2022-05-06 深圳市优必选科技股份有限公司 一种建图方法、装置、计算机可读存储介质及机器人
CN112462381A (zh) * 2020-11-19 2021-03-09 浙江吉利控股集团有限公司 一种基于车路协同的多激光雷达融合方法
CN112733877B (zh) * 2020-11-27 2023-05-30 北京理工大学 一种多激光雷达立体成像人工智能矿石识别方法及装置
CN112415494B (zh) * 2020-12-11 2021-08-13 福勤智能科技（昆山）有限公司 Agv双激光雷达位置标定方法、装置、设备和存储介质
CN112781594B (zh) * 2021-01-11 2022-08-19 桂林电子科技大学 基于imu耦合的激光雷达迭代最近点改进算法
CN112837314B (zh) * 2021-03-08 2023-06-09 华南农业大学 基于2D-LiDAR和Kinect的果树冠层参数检测系统和方法
CN113345025B (zh) * 2021-04-26 2022-09-09 香港理工大学深圳研究院 一种基于背包式激光雷达系统的建图和地面分割方法
CN113436235B (zh) * 2021-05-25 2022-07-01 北京理工大学 一种激光雷达与视觉点云的初始化自动配准方法
CN113504543B (zh) * 2021-06-16 2022-11-01 国网山西省电力公司电力科学研究院 无人机LiDAR系统定位定姿系统及方法
CN113658256A (zh) * 2021-08-16 2021-11-16 智道网联科技(北京)有限公司 基于激光雷达的目标检测方法、装置及电子设备
CN113625301B (zh) * 2021-09-03 2024-01-19 国网山东省电力公司济宁供电公司 一种基于单扫描头激光雷达扩大建图视场的方法及系统
CN113866747B (zh) * 2021-10-13 2023-10-27 上海师范大学 一种多激光雷达的标定方法及装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106529573A (zh) * 2016-10-14 2017-03-22 北京联合大学 一种结合三维点云分割和局部特征匹配的实时物体检测方法
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
CN108509972A (zh) * 2018-01-16 2018-09-07 天津大学 一种基于毫米波和激光雷达的障碍物特征提取方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011127375A1 (en) * 2010-04-09 2011-10-13 Pochiraju Kishore V Adaptive mechanism control and scanner positioning for improved three-dimensional laser scanning

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106529573A (zh) * 2016-10-14 2017-03-22 北京联合大学 一种结合三维点云分割和局部特征匹配的实时物体检测方法
CN108509972A (zh) * 2018-01-16 2018-09-07 天津大学 一种基于毫米波和激光雷达的障碍物特征提取方法
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
杨飚 等.基于正态分布变换与迭代最近点的快速点云配准算法.《科学技术与工程》.2017,第17卷(第15期),摘要，第1.2节. *
林辉.基于车载多激光雷达的地图构建与障碍物检测.《中国优秀硕士学位论文全文数据库 信息科技辑》.2018,I136-229. *
王庆闪 等.基于激光雷达在无人车中的定位与建图的应用研究综述.《计算机科学》.2018,第45卷(第10A期),117-118，130. *
程效军 等.《海量点云数据处理理论与技术》.同济大学出版社,2014,42-43. *
程金龙 等.车载激光雷达外参数的标定方法.《光电工程》.2013,第40卷(第12期),第1、3节. *
陈宗海.《系统仿真技术及其应用（第17卷）》.中国科学技术大学出版社,2016,337. *
韩成 等.《基于结构光的计算机视觉》.国防工业出版社,2015,第99页. *

Also Published As

Publication number Publication date
CN110658530A (zh) 2020-01-07

Similar Documents

Publication Publication Date Title
CN110658530B (zh) 一种基于双激光雷达数据融合的地图构建方法、系统及地图
CN109974707B (zh) 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
CN111427026B (zh) 激光雷达的标定方法、装置、存储介质及自移动设备
WO2021139590A1 (zh) 基于蓝牙与slam的室内定位导航装置及其方法
Ho et al. Virtual occupancy grid map for submap-based pose graph SLAM and planning in 3D environments
CN105510913A (zh) 基于类光学像方改正的异源光学和sar遥感影像联合定位方法
CN111273312B (zh) 一种智能车辆定位与回环检测方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
CN111077907A (zh) 一种室外无人机的自主定位方法
CN112669354A (zh) 一种基于车辆非完整约束的多相机运动状态估计方法
CN110969665A (zh) 一种外参标定方法、装置、系统及机器人
Gong et al. Extrinsic calibration of a 3D LIDAR and a camera using a trihedron
Wang et al. Accurate mix-norm-based scan matching
Wei et al. Croon: Automatic multi-lidar calibration and refinement method in road scene
CN110187337B (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统
Ren et al. High precision calibration algorithm for binocular stereo vision camera using deep reinforcement learning
CN115079143A (zh) 一种用于双桥转向矿卡的多雷达外参快速标定方法及装置
Jensen et al. Laser range imaging using mobile robots: From pose estimation to 3D-models
CN116358517B (zh) 一种用于机器人的高度地图构建方法、系统以及存储介质
Li et al. Extrinsic calibration of non-overlapping multi-camera system with high precision using circular encoded point ruler
Wang et al. Visual pose measurement based on structured light for MAVs in non-cooperative environments
CN111121818A (zh) 一种无人车中相机与二维码的标定方法
Xu et al. A flexible 3D point reconstruction with homologous laser point array and monocular vision
CN112489118B (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
GR01 Patent grant
GR01 Patent grant