CN113670316A - 基于双雷达的路径规划方法、***、存储介质及电子设备 - Google Patents

基于双雷达的路径规划方法、***、存储介质及电子设备 Download PDF

Info

Publication number
CN113670316A
CN113670316A CN202110996364.XA CN202110996364A CN113670316A CN 113670316 A CN113670316 A CN 113670316A CN 202110996364 A CN202110996364 A CN 202110996364A CN 113670316 A CN113670316 A CN 113670316A
Authority
CN
China
Prior art keywords
point cloud
laser radar
map
coordinate system
rotation matrix
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
CN202110996364.XA
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.)
Guangzhou Industry&trade Technician College
Original Assignee
Guangzhou Industry&trade Technician College
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 Guangzhou Industry&trade Technician College filed Critical Guangzhou Industry&trade Technician College
Priority to CN202110996364.XA priority Critical patent/CN113670316A/zh
Publication of CN113670316A publication Critical patent/CN113670316A/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/20—Instruments for performing navigational calculations
    • 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
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Computer Graphics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明涉及雷达探测领域，尤其涉及基于双雷达的路径规划方法、***、存储介质及电子设备。该方法包括：获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；步骤2，基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图；通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；基于建图结果对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；根据所述融合建图实现路径规划。本发明能够达到提高定位匹配精确度，并提高路径规划准确度的效果。

Description

基于双雷达的路径规划方法、***、存储介质及电子设备
技术领域
本发明涉及雷达探测领域，尤其涉及基于双雷达的路径规划方法、***、存储介质及电子设备。
背景技术
为了实现移动机器人智能化，配备多个激光雷达传感器***已逐渐成为主流，与单激光雷达传感器相比，可以获取更丰富的环境视图及更密集的测量，避免视野盲区，增强更健壮的环境感知能力。现有技术中，载物机器人在载物过程中可能会因为货物面积过大，导致视野盲区，导致定位匹配精度降低，影响路径规划，导致意外发生。
发明内容
本发明所要解决的技术问题是提供基于双雷达的路径规划方法、***、存储介质及电子设备。
本发明解决上述技术问题的技术方案如下：一种基于双雷达的路径规划方法，其特征在于，包括：
步骤1，获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；
步骤2，基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Mapt以及辅助激光雷达坐标系的全局点云图Maps；
步骤3，通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；
步骤4，基于所述主激光雷达坐标系的全局点云图Mapt以及所述辅助激光雷达坐标系的全局点云图Maps，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；
步骤5，根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；
步骤6，根据所述融合建图实现路径规划。
本发明的有益效果是：先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
在上述技术方案的基础上，本发明还可以做如下改进。
进一步，步骤1具体为：
获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点Pi,Pi'，其中，Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。
进一步，步骤3具体为：
通过ICP算法计算Pi以及Pi'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；
其中，所述姿态变换关系为：
Figure BDA0003234271340000021
pi＝Rp'i+t，其中，i为1，2...n，
Figure BDA0003234271340000022
为任意一个i。
进一步，步骤4具体为：
对所述辅助激光雷达坐标系的全局点云图Maps转换为主激光雷达坐标系下的全局点云图Map's，转换公式为：
Map's＝Maps*R+t
通过所述主激光雷达坐标系下的全局点云图Map's、所述主激光雷达坐标系的全局点云图Mapt、所述旋转矩阵R以及所述平移向量t构建误差优化函数，通过对所述误差优化函数的计算，得到优化后的旋转矩阵R'以及优化后的平移向量t'，
其中，所述误差优化函数E(R',t')公式为：
Figure BDA0003234271340000031
其中，N为主激光雷达坐标系下的全局点云图Map's以及所述主激光雷达坐标系的全局点云图Mapt中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，Si为主激光雷达坐标系的全局点云图Mapt包含的点云坐标集合S中的第i个点云坐标，Ti为主激光雷达坐标系下的全局点云图Map's包含的点云坐标合集T的第i个点云坐标。
进一步，步骤5具体为：
通过优化后的旋转矩阵R'以及优化后的平移向量t'构建变换矩阵，实时获取辅助激光雷达输出的点云坐标，通过所述变换矩阵对所述点云坐标进行变换处理，将变换处理后的结果与主激光雷达获取的点云坐标组合成融合点云坐标，将所述融合点云坐标导入建图软件，得到融合建图。
本发明解决上述技术问题的另一种技术方案如下：一种基于双雷达的路径规划***，其特征在于，包括：
获取模块，用于获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；
转换模块，用于基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Mapt以及辅助激光雷达坐标系的全局点云图Maps；
计算模块，用于通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；
优化模块，用于基于所述主激光雷达坐标系的全局点云图Mapt以及所述辅助激光雷达坐标系的全局点云图Maps，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；
融合模块，用于根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；
规划模块，用于根据所述融合建图实现路径规划。
本发明的有益效果是：先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
进一步，获取模块具体用于：
获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点Pi,Pi'，其中，Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。
进一步，计算模块具体用于:
通过ICP算法计算Pi以及Pi'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；
其中，所述姿态变换关系为:
Figure BDA0003234271340000051
pi＝Rp′i+t，其中，i为1，2...n，
Figure BDA0003234271340000052
为任意一个i。
进一步，优化模块具体用于：
对所述辅助激光雷达坐标系的全局点云图Maps转换为主激光雷达坐标系下的全局点云图Map's，转换公式为：
Map's＝Maps*R+t
通过所述主激光雷达坐标系下的全局点云图Map's、所述主激光雷达坐标系的全局点云图Mapt、所述旋转矩阵R以及所述平移向量t构建误差优化函数，通过对所述误差优化函数的计算，得到优化后的旋转矩阵R'以及优化后的平移向量t'，
其中，所述误差优化函数E(R',t')公式为：
Figure BDA0003234271340000053
其中，N为主激光雷达坐标系下的全局点云图Map's以及所述主激光雷达坐标系的全局点云图Mapt中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，Si为主激光雷达坐标系的全局点云图Mapt包含的点云坐标集合S中的第i个点云坐标，Ti为主激光雷达坐标系下的全局点云图Map's包含的点云坐标合集T的第i个点云坐标。
进一步，融合模块具体用于：
通过优化后的旋转矩阵R'以及优化后的平移向量t'构建变换矩阵，实时获取辅助激光雷达输出的点云坐标，通过所述变换矩阵对所述点云坐标进行变换处理，将变换处理后的结果与主激光雷达获取的点云坐标组合成融合点云坐标，将所述融合点云坐标导入建图软件，得到融合建图。
本发明解决上述技术问题的另一种技术方案如下：一种存储介质，所述存储介质中存储有指令，当计算机读取所述指令时，使所述计算机执行如上述任一项所述的一种基于双雷达的路径规划方法。
本发明的有益效果是：先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
本发明解决上述技术问题的另一种技术方案如下：一种电子设备，包括存储器、处理器及存储在所述存储器上并在所述处理器上运行的程序，所述处理器执行所述程序时实现如上述任一项所述的一种基于双雷达的路径规划方法。
本发明的有益效果是：先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
附图说明
图1为本发明一种基于双雷达的路径规划方法实施例提供的流程示意图；
图2为本发明一种基于双雷达的路径规划***实施例提供的***框架图。
具体实施方式
以下对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
如图1所示，一种基于双雷达的路径规划方法，其特征在于，包括：
步骤1，获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；
步骤2，基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Mapt以及辅助激光雷达坐标系的全局点云图Maps；
步骤3，通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；
步骤4，基于所述主激光雷达坐标系的全局点云图Mapt以及所述辅助激光雷达坐标系的全局点云图Maps，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；
步骤5，根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；
步骤6，根据所述融合建图实现路径规划。
在一些可能的实施方式中，先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
需要说明的是，预设条件指的是选择点云边缘或平面特征点丰富的场景，分别进行主辅激光雷达建图，将两个三维激光雷达固定在无人小车的对角方向，同时在辅助激光雷达与主激光雷达找到一组配对好的三维点云坐标，通过ICP算法，得到粗标定的旋转矩阵R以及粗标定的平移向量t；这里的R表示两帧点云之间的旋转变换，t表示两帧点云之间的位移变换；
在两个三维激光雷达固定在无人小车的对角方向的前提条件下，控制搭载两个三维激光雷达的移动平台运动，在运动中扫描环境轮廓并采集离线数据包，通过两个激光雷达离线建图计算，得到两帧分别基于主激光雷达坐标系和基于辅助激光雷达坐标系的全局三维点云地图，由于两个三维激光雷达处于同一环境下进行数据采集和建图工作，因此两帧全局三维点云地图存在一定程度的视野重复区域，这是全局匹配精确标定的核心；粗标定的旋转矩阵R以及粗标定的平移向量t，将其作为全局匹配的初始值，匹配两个全局三维点云地图，建立非线性最小二乘误差优化函数，求解得到优化后的旋转矩阵R'以及优化后的平移向量t'；这里的R'表示两帧全局点云地图之间的旋转变换，t'表示两帧全局点云地图之间的位移变换；双雷达点云坐标变换则是基于上一部分得到优化的R'和t'，将辅助雷达的点云坐标数据逐点转换映射到主雷达坐标系下，实现主辅双雷达点云坐标系的统一。最后基于主激光雷达坐标系下的双雷达融合点云数据，实现双雷达的融合数据建图过程。整个处理流程可参考实施例1进行辅助理解。
实施例1，
选择无人小车到特征点丰富的标定场景，分别从主辅激光雷达坐标系下的若干单帧三维点云中，选取主辅两个激光雷达共同视野最多的两帧三维点云，从中选取一组配对好的三维点云坐标：Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，假设存在一个旋转变换R和位移变换t，使得：
Figure BDA0003234271340000081
pi＝Rp'i+t
因此，使用ICP算法计算Pi以及Pi'之间的姿态变换关系，求解得到粗标定的旋转矩阵R以及粗标定的平移向量t。
选择点云边缘或平面特征点丰富的场景，分别进行主辅激光雷达建图，得到两帧分别基于主辅激光雷达坐标系的全局点云地图Mapt和Maps，其中Maps表示辅助激光雷达坐标系下的全局点云地图，Mapt表示主激光雷达坐标系下的全局点云地图。
根据粗标定的旋转矩阵R以及粗标定的平移向量t，将辅助激光雷达坐标系下的全局点云地图Maps通过坐标系变换计算，将所有的点云坐标变换到主激光雷达坐标系下，具体如下所示：
Map's＝Maps*R+t
经过上述计算得到的Map's已经处于主激光雷达坐标系下，和Mapt的坐标系一致。
基于上述步骤，为了求得更精确的点云姿态变换关系优化后的旋转矩阵R'以及优化后的平移向量t'，需要构建基于点云坐标点的非线性最小二乘误差优化函数，误差优化函数E(R',t')公式为：
Figure BDA0003234271340000091
其中，N为主激光雷达坐标系下的全局点云图Map's以及所述主激光雷达坐标系的全局点云图Mapt中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，Si为主激光雷达坐标系的全局点云图Mapt包含的点云坐标集合S中的第i个点云坐标，Ti为主激光雷达坐标系下的全局点云图Map's包含的点云坐标合集T的第i个点云坐标；
通过计算上述误差优化函数，得到的位姿变换关系优化后的旋转矩阵R'以及优化后的平移向量t'组合在一起，就可以构成全局地图精确匹配后的变换矩阵，可以直接用于辅助激光雷达坐标系点云坐标映射到主激光雷达坐标系。
基于上述计算得到的主辅激光雷达坐标系变换矩阵，融合主辅激光雷达两个坐标系下的实时点云坐标，实时输出融合后的点云坐标，实现主辅激光雷达点云坐标的融合计算；辅助激光雷达实时输出该坐标系下的点云坐标至主激光雷达驱动中，经过与上述变换矩阵进行坐标变换后，与主激光雷达坐标系下的点云坐标一起重新组合成新的一帧融合点云坐标再输出，此时的点云坐标数据克服了单激光雷达的视野不足问题，实现360°的全方位视野观测效果。基于上述融合的双雷达点云坐标数据，可直接导入任一激光雷达SLAM建图软件中进行建图工作，有效避免了单激光雷达建图高错误率和失败率的问题。
优选地，在上述任意实施例中，步骤1具体为：
获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点Pi,Pi'，其中，Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。
需要说明的是，已配对成功的3D点为，通过点云帧之间最近点迭代匹配算法匹配后筛选出来的。
优选地，在上述任意实施例中，步骤3具体为：
通过ICP算法计算Pi以及Pi'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；
其中，所述姿态变换关系为：
Figure BDA0003234271340000101
pi＝Rp'i+t，其中，i为1，2...n，
Figure BDA0003234271340000102
为任意一个i。
优选地，在上述任意实施例中，步骤4具体为：
对所述辅助激光雷达坐标系的全局点云图Maps转换为主激光雷达坐标系下的全局点云图Map's，转换公式为：
Map's＝Maps*R+t
通过所述主激光雷达坐标系下的全局点云图Map's、所述主激光雷达坐标系的全局点云图Mapt、所述旋转矩阵R以及所述平移向量t构建误差优化函数，通过对所述误差优化函数的计算，得到优化后的旋转矩阵R'以及优化后的平移向量t'，
其中，所述误差优化函数E(R',t')公式为：
Figure BDA0003234271340000103
其中，N为主激光雷达坐标系下的全局点云图Map's以及所述主激光雷达坐标系的全局点云图Mapt中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，Si为主激光雷达坐标系的全局点云图Mapt包含的点云坐标集合S中的第i个点云坐标，Ti为主激光雷达坐标系下的全局点云图Map's包含的点云坐标合集T的第i个点云坐标。
优选地，在上述任意实施例中，步骤5具体为：
通过优化后的旋转矩阵R'以及优化后的平移向量t'构建变换矩阵，实时获取辅助激光雷达输出的点云坐标，通过所述变换矩阵对所述点云坐标进行变换处理，将变换处理后的结果与主激光雷达获取的点云坐标组合成融合点云坐标，将所述融合点云坐标导入建图软件，得到融合建图。
如图2所示，一种基于双雷达的路径规划***，其特征在于，包括：
获取模块100，用于获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；
转换模块200，用于基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Mapt以及辅助激光雷达坐标系的全局点云图Maps；
计算模块300，用于通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；
优化模块400，用于基于所述主激光雷达坐标系的全局点云图Mapt以及所述辅助激光雷达坐标系的全局点云图Maps，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；
融合模块500，用于根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；
规划模块600，用于根据所述融合建图实现路径规划。
在一些可能的实施方式中，先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
优选地，在上述任意实施例中，获取模块100具体用于：
获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点Pi,Pi'，其中，Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。
优选地，在上述任意实施例中，计算模块300具体用于:
通过ICP算法计算Pi以及Pi'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；
其中，所述姿态变换关系为:
Figure BDA0003234271340000121
pi＝Rp'i+t，其中，i为1，2...n，
Figure BDA0003234271340000122
为任意一个i。
优选地，在上述任意实施例中，优化模块400具体用于：
对所述辅助激光雷达坐标系的全局点云图Maps转换为主激光雷达坐标系下的全局点云图Map's，转换公式为：
Map's＝Maps*R+t
通过所述主激光雷达坐标系下的全局点云图Map's、所述主激光雷达坐标系的全局点云图Mapt、所述旋转矩阵R以及所述平移向量t构建误差优化函数，通过对所述误差优化函数的计算，得到优化后的旋转矩阵R'以及优化后的平移向量t'，
其中，所述误差优化函数E(R',t')公式为：
Figure BDA0003234271340000123
其中，N为主激光雷达坐标系下的全局点云图Map's以及所述主激光雷达坐标系的全局点云图Mapt中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，Si为主激光雷达坐标系的全局点云图Mapt包含的点云坐标集合S中的第i个点云坐标，Ti为主激光雷达坐标系下的全局点云图Map's包含的点云坐标合集T的第i个点云坐标。
优选地，在上述任意实施例中，融合模块500具体用于：
通过优化后的旋转矩阵R'以及优化后的平移向量t'构建变换矩阵，实时获取辅助激光雷达输出的点云坐标，通过所述变换矩阵对所述点云坐标进行变换处理，将变换处理后的结果与主激光雷达获取的点云坐标组合成融合点云坐标，将所述融合点云坐标导入建图软件，得到融合建图。
本发明解决上述技术问题的另一种技术方案如下：一种存储介质，所述存储介质中存储有指令，当计算机读取所述指令时，使所述计算机执行如上述任一项所述的一种基于双雷达的路径规划方法。
在一些可能的实施方式中，先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
本发明解决上述技术问题的另一种技术方案如下：一种电子设备，包括存储器、处理器及存储在所述存储器上并在所述处理器上运行的程序，所述处理器执行所述程序时实现如上述任一项所述的一种基于双雷达的路径规划方法。
在一些可能的实施方式中，先通过对双雷达之间的单帧匹配粗标定的处理求解得到粗标定的旋转矩阵R以及粗标定的平移向量t，在两个激光雷达完成建图后，进行全局匹配求解最优解优化后的旋转矩阵R'以及优化后的平移向量t'，地图信息融合。更健壮的环境感知能力及环境视图，定位精度更高SLAM得到点云地图更加精细及稠密。
读者应理解，在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的方法实施例仅仅是示意性的，例如，步骤的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个步骤可以结合或者可以集成到另一个步骤，或一些特征可以忽略，或不执行。
上述方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，RandomAccessMemory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种基于双雷达的路径规划方法，其特征在于，包括：
步骤1，获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；
步骤2，基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Mapt以及辅助激光雷达坐标系的全局点云图Maps；
步骤3，通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；
步骤4，基于所述主激光雷达坐标系的全局点云图Mapt以及所述辅助激光雷达坐标系的全局点云图Maps，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；
步骤5，根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；
步骤6，根据所述融合建图实现路径规划。
2.根据权利要求1所述的一种基于双雷达的路径规划方法，其特征在于，步骤1具体为：
获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点Pi,Pi'，其中，Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。
3.根据权利要求2所述的一种基于双雷达的路径规划方法，其特征在于，步骤3具体为：
通过ICP算法计算Pi以及Pi'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；
其中，所述姿态变换关系为：
Figure FDA0003234271330000021
其中，i为1，2...n，
Figure FDA0003234271330000022
为任意一个i。
4.根据权利要求3所述的一种基于双雷达的路径规划方法，其特征在于，步骤4具体为：
对所述辅助激光雷达坐标系的全局点云图Maps转换为主激光雷达坐标系下的全局点云图Map's，转换公式为：
Map's＝Maps*R+t
通过所述主激光雷达坐标系下的全局点云图Map's、所述主激光雷达坐标系的全局点云图Mapt、所述旋转矩阵R以及所述平移向量t构建误差优化函数，通过对所述误差优化函数的计算，得到优化后的旋转矩阵R'以及优化后的平移向量t'，
其中，所述误差优化函数E(R',t')公式为：
Figure FDA0003234271330000023
其中，N为主激光雷达坐标系下的全局点云图Map's以及所述主激光雷达坐标系的全局点云图Mapt中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，Si为主激光雷达坐标系的全局点云图Mapt包含的点云坐标集合S中的第i个点云坐标，Ti为主激光雷达坐标系下的全局点云图Map's包含的点云坐标合集T的第i个点云坐标。
5.根据权利要求4所述的一种基于双雷达的路径规划方法，其特征在于，步骤5具体为：
通过优化后的旋转矩阵R'以及优化后的平移向量t'构建变换矩阵，实时获取辅助激光雷达输出的点云坐标，通过所述变换矩阵对所述点云坐标进行变换处理，将变换处理后的结果与主激光雷达获取的点云坐标组合成融合点云坐标，将所述融合点云坐标导入建图软件，得到融合建图。
6.一种基于双雷达的路径规划***，其特征在于，包括：
获取模块，用于获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；
转换模块，用于基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Mapt以及辅助激光雷达坐标系的全局点云图Maps；
计算模块，用于通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；
优化模块，用于基于所述主激光雷达坐标系的全局点云图Mapt以及所述辅助激光雷达坐标系的全局点云图Maps，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；
融合模块，用于根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；
规划模块，用于根据所述融合建图实现路径规划。
7.根据权利要求6所述的一种基于双雷达的路径规划***，其特征在于，获取模块具体用于：
获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点Pi,Pi'，其中，Pi＝{P1,P2…Pn}，Pi'＝{P1',P2'…Pn'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。
8.根据权利要求7所述的一种基于双雷达的路径规划***，其特征在于，计算模块具体用于:
通过ICP算法计算Pi以及Pi'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；
其中，所述姿态变换关系为:
Figure FDA0003234271330000041
其中，i为1，2...n，
Figure FDA0003234271330000042
为任意一个i。
9.一种存储介质，其特征在于，所述存储介质中存储有指令，当计算机读取所述指令时，使所述计算机执行如权利要求1至6中任一项所述的一种基于双雷达的路径规划方法。
10.一种电子设备，包括存储器、处理器及存储在所述存储器上并在所述处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至6任一项所述的一种基于双雷达的路径规划方法。
CN202110996364.XA 2021-08-27 2021-08-27 基于双雷达的路径规划方法、***、存储介质及电子设备 Pending CN113670316A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110996364.XA CN113670316A (zh) 2021-08-27 2021-08-27 基于双雷达的路径规划方法、***、存储介质及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110996364.XA CN113670316A (zh) 2021-08-27 2021-08-27 基于双雷达的路径规划方法、***、存储介质及电子设备

Publications (1)

Publication Number Publication Date
CN113670316A true CN113670316A (zh) 2021-11-19

Family

ID=78547031

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110996364.XA Pending CN113670316A (zh) 2021-08-27 2021-08-27 基于双雷达的路径规划方法、***、存储介质及电子设备

Country Status (1)

Country Link
CN (1) CN113670316A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115031718A (zh) * 2022-05-25 2022-09-09 安徽中科合鼎科技发展有限公司 一种多传感器融合的无人船同步定位与建图方法(slam)及***
CN115236690A (zh) * 2022-09-20 2022-10-25 图达通智能科技(武汉)有限公司 用于激光雷达***的数据融合方法、装置及可读存储介质

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115031718A (zh) * 2022-05-25 2022-09-09 安徽中科合鼎科技发展有限公司 一种多传感器融合的无人船同步定位与建图方法(slam)及***
CN115031718B (zh) * 2022-05-25 2023-10-31 合肥恒淏智能科技合伙企业(有限合伙) 一种多传感器融合的无人船同步定位与建图方法(slam)及***
CN115236690A (zh) * 2022-09-20 2022-10-25 图达通智能科技(武汉)有限公司 用于激光雷达***的数据融合方法、装置及可读存储介质
CN115236690B (zh) * 2022-09-20 2023-02-10 图达通智能科技(武汉)有限公司 用于激光雷达***的数据融合方法、装置及可读存储介质

Similar Documents

Publication Publication Date Title
WO2021233029A1 (en) Simultaneous localization and mapping method, device, system and storage medium
CN112132972B (zh) 一种激光与图像数据融合的三维重建方法及***
WO2019127445A1 (zh) 三维建图方法、装置、***、云端平台、电子设备和计算机程序产品
JP4685313B2 (ja) 任意の局面の受動的な体積画像の処理方法
Borrmann et al. Globally consistent 3D mapping with scan matching
CN110568447A (zh) 视觉定位的方法、装置及计算机可读介质
JP2009145314A (ja) 異種センサ統合モデリングによるデジタル写真測量方法及びその装置
CN112258590B (zh) 一种基于激光的深度相机外参标定方法、设备及其存储介质
CN113670316A (zh) 基于双雷达的路径规划方法、***、存储介质及电子设备
CN110487274B (zh) 用于弱纹理场景的slam方法、***、导航车及存储介质
JP2016217941A (ja) ３次元データ評価装置、３次元データ測定システム、および３次元計測方法
CN112465849B (zh) 一种无人机激光点云与序列影像的配准方法
CN114022542A (zh) 一种基于三维重建的3d数据库制作方法
CN115410167A (zh) 目标检测与语义分割方法、装置、设备及存储介质
CN116246119A (zh) 3d目标检测方法、电子设备及存储介质
CN114543787A (zh) 基于条纹投影轮廓术的毫米级室内建图定位方法
Zhang et al. Primitive-based building reconstruction by integration of Lidar data and optical imagery
CN116704112A (zh) 一种用于对象重建的3d扫描***
CN112146647B (zh) 一种地面纹理的双目视觉定位方法及芯片
CN114638898A (zh) 一种小型飞行目标检测方法和装置
Li et al. 3D reconstruction of oil refinery buildings using a depth camera
Singandhupe et al. Single frame lidar and stereo camera calibration using registration of 3d planes
CN114663596B (zh) 基于无人机实时仿地飞行方法的大场景建图方法
CN114638894B (zh) 机器人行走的定位方法、***、电子装置及存储介质
Guo et al. Research on Floating Object Ranging and Positioning Based on UAV Binocular System

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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20211119