CN113129377B - 一种三维激光雷达快速鲁棒slam方法和装置 - Google Patents

一种三维激光雷达快速鲁棒slam方法和装置 Download PDF

Info

Publication number
CN113129377B
CN113129377B CN202110440733.7A CN202110440733A CN113129377B CN 113129377 B CN113129377 B CN 113129377B CN 202110440733 A CN202110440733 A CN 202110440733A CN 113129377 B CN113129377 B CN 113129377B
Authority
CN
China
Prior art keywords
pose
point cloud
frame
laser radar
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.)
Active
Application number
CN202110440733.7A
Other languages
English (en)
Other versions
CN113129377A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202110440733.7A priority Critical patent/CN113129377B/zh
Publication of CN113129377A publication Critical patent/CN113129377A/zh
Application granted granted Critical
Publication of CN113129377B publication Critical patent/CN113129377B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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
    • 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
    • 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
    • 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
    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开一种三维激光雷达快速鲁棒SLAM方法和装置，采用高度传感器、惯性测量单元对激光雷达进行信息辅助，补偿点云配准前激光雷达点云的初始位姿，并通过解耦三维激光雷达点云配准算法实现UAV的六自由度导航解算。本发明相比于传统激光SLAM算法，有效抑制了点云过度匹配带来的高度解算误差以及水平耦合误差，提高了复杂环境下UAV的位姿估计精度，并且具有更高的计算效率。

Description

一种三维激光雷达快速鲁棒SLAM方法和装置
技术领域
本发明属于组合导航领域，尤其涉及一种三维激光雷达快速鲁棒SLAM方法和装置，具体涉及一种复杂环境下微小型飞行器(UAV)三维激光雷达解耦SLAM方法和装置。
背景技术
同步定位与地图(SLAM，Simultaneous Localization and Mapping)技术是机器人自主导航技术领域中的一大研究热点。在无GPS(Global Positioning System)环境中，SLAM方法能够帮助机器人实现在陌生环境中的导航与定位，是机器人实际应用中的关键技术。
在SLAM算法中，采取迭代匹配两帧点云，寻找两帧点云之间的相对位姿关系的方式来确定机体位姿，通常采用基于概率统计的多元正态变换算法(NDT)进行无人机的位姿估计。然而UAV在高程方向飞行过程中，机载三维激光雷达点云的感知能力因为环境的结构特性被弱化，以NDT作为位姿估计的传统SLAM算法无法只对两帧点云实际重合部分进行匹配从而为其相对位姿关系的确定引入高程误差以及水平耦合误差。
发明内容
针对上述现有技术的缺陷，本发明提供一种三维激光雷达快速鲁棒SLAM方法和装置，通过高度传感器/IMU的信息辅助以及点云配准算法的解耦解算实现机载激光雷达点云实际重合部分的匹配。相比传统算法具有精度高、速度快的特点。
本发明为解决上述技术问题采用以下技术方案：
一种三维激光雷达快速鲁棒SLAM方法，包括如下步骤：
步骤1、采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
步骤2、采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
步骤3、利用步骤2中位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位；。
步骤4、根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至步骤2。
作为优选，步骤1具体包括：
步骤11、根据惯性传感器获得三轴角速度以及加速度信息，通过航姿算法得到惯性传感器相对于全局系的姿态角得到无人机机体相对于全局系的姿态角/>
其中，为机体与惯性传感器之间安装误差对应的旋转矩阵；
步骤12、获取高度传感器测距信息ldis，并根据惯性传感器解算的机体相对于全局坐标系的姿态角对测距信息ldis进行修正，得到机体距离地面高度/>
其中：分别为机体系相对于全局系的横滚角与俯仰角，/>为高度传感器在机体系下的高度坐标值；
步骤13、分别定义机体外部位姿向量内部位姿向量/>为：
其中，为第k帧点云对应的无人机外部位姿向量，由机体系在全局系下的横滚角俯仰角/>及高度/>组成；/>为第k帧点云对应的无人机内部位姿向量，由机体系在全局系下水平位置/>以及航向角组成/>
作为优选，步骤2具体包括：
步骤21、将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
步骤22、将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
作为优选，步骤3具体包括：
步骤31、构建两帧点云之间的关联
对空间进行网格化处理，根据空间网格中的点云分布及其坐标数据，对关键帧点云进行多元正态分布函数表示：
其中：μk、Σk分别为关键帧第k个网格中空间点云的均值与协方差；为输入点云中第i个点；/>为/>在该网格中对应的多元正态分布函数的概率值；d1、d2为常数；
步骤32、构建待目标函数
待优化函数为输入点云概率值之和，目标为求解输入点云的最优变换，使得该待优化函数值最大，待优化变量即为无人机内部位姿变化量
其中，为根据转换矩阵/>计算出的输入点云总概率值，/>为输入点云中的第k个点，/>为第k-1至k帧点云无人机内部位姿变化量：
步骤33、求解目标函数
位姿变化量解算采用高斯牛顿迭代法，求解无人机内部位姿变化量的正规方程如下式：
其中，H、g为待优化函数的Hessian矩阵和Jacobian矩阵，
令:
其中，Tcal表示位姿解算过程中第k帧点云第i次迭代后机体系下激光雷达点云坐标；表示第i次迭代后机体内部位姿向量相对于第k-1帧点云对应的机体内部位姿向量的变化量，定义/>为/>的第i个元素，则有：
其中，gi、Hij分别为g、H的元素；Tcal的Jacobian矩阵与Hessian矩阵分别为：
作为优选，步骤4具体包括：
设第k+m-1与k+m帧点云解算出的位姿增量为则初值补偿矩阵更新方法为：
其中，表示第k帧关键帧点云的位姿，/>为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，/>为第k+m帧与关键帧的外部位姿向量，/>第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
将结果传递至步骤2。
本发明提供一种三维激光雷达快速鲁棒SLAM装置，包括：
获取模块，用于采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
补偿模块，用于采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
计算模块，用于利用位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿；
更新模块，用于根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至补偿模块。
作为优选，所述补偿模块包括：
转换单元，用于将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
补偿单元，用于将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
作为优选，所述更新模块具体为：
设第k+m-1与k+m帧点云解算出的位姿增量为则初值补偿矩阵更新方法为：
其中，表示第k帧关键帧点云的位姿，/>为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，/>为第k+m帧与关键帧的外部位姿向量，/>第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
将结果传递至补偿模块。
与现有技术相比，本发明具有以下有益效果：
通过本发明，可以解决由于环境特征退化使得点云过度配准，从而带来的导航精度降低的问题。与传统SLAM相比，本发明有效抑制了点云过度匹配带来的高度解算误差以及水平耦合误差，提高了复杂环境下的位姿估计精度，并且具有更高的计算效率。
附图说明
图1为本发明三维激光雷达快速鲁棒SLAM方法的流程示意图；
图2为本发明一种三维激光雷达快速鲁棒SLAM装置的结构示意图。
具体实施方式
如图1所示，本发明提供一种三维激光雷达快速鲁棒SLAM方法，包括以下步骤：
步骤1、采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
步骤2、采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
步骤3、利用步骤二中位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿；
步骤4：根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至步骤2。
进一步，步骤1具体包括：
步骤11、根据惯性传感器与高度传感器数据，计算机体系相对于全局系的位姿矩阵。从惯性传感器获得三轴角速度以及加速度信息，通过航姿算法可得惯性传感器相对于全局系的姿态角可得无人机机体相对于全局系的姿态角/>
其中，为机体与惯性传感器之间安装误差对应的旋转矩阵。
步骤12、获取激光测距仪测距信息ldis，并根据惯性传感器解算的机体相对于全局坐标系的姿态角对测距信息ldis进行修正，可得机体距离地面高度/>
其中：分别为机体系相对于全局系的横滚角与俯仰角。/>为高度传感器在机体系下的高度坐标值。
步骤13、分别定义机体外部位姿向量内部位姿向量/>为：
其中，为第k帧点云对应的无人机外部位姿向量，由机体系在全局系下的横滚角/>俯仰角/>及高度/>组成；/>为第k帧点云对应的无人机内部位姿向量，由机体系在全局系下水平位置/> 以及航向角组成/>
进一步，步骤2具体包括：
步骤21、初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成，首先将其转换为转换矩阵
其中，表示第k帧点云的初始位姿向量对应的转换矩阵。
步骤22、将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
其中：为第k帧点云对应的机体系下输入点云坐标；/>为其经过初始位姿补偿后的点云坐标。
进一步，步骤3具体包括：
步骤31、构建两帧点云(输入点云、目标点云)之间的关联
采用NDT算法，对空间进行网格化处理，随后根据空间网格中的点云分布及其坐标数据，对关键帧点云进行多元正态分布函数表示：
其中：μk、Σk分别为关键帧第k个网格中空间点云的均值与协方差；为输入点云中第i个点；/>为/>在该网格中对应的多元正态分布函数的概率值；d1、d2为常数。
步骤32、构建待目标函数
待优化函数为输入点云概率值之和，目标为求解输入点云的最优变换，使得该待优化函数值最大，待优化变量即为无人机内部位姿变化量
其中：为根据转换矩阵/>计算出的输入点云总概率值，/>为输入点云中的第k个点，/>为第k-1至k帧点云无人机内部位姿变化量：
步骤33、求解目标函数
位姿变化量解算采用高斯牛顿迭代法，求解无人机内部位姿变化量的正规方程如下式：
其中：H、g为待优化函数的Hessian矩阵和Jacobian矩阵。
令:
其中，Tcal表示位姿解算过程中第k帧点云第i次迭代后机体系下激光雷达点云坐标；表示第i次迭代后机体内部位姿向量相对于第k-1帧点云对应的机体内部位姿向量的变化量。定义/>为/>的第i个元素，则有：
其中，gi、Hij分别为g、H的元素；Tcal的Jacobian矩阵与Hessian矩阵分别为：
进一步，步骤4具体为：采用Hdl_Graph_Slam算法中的导航框架进行导航解算，改进后框架内引入惯性/高度传感器的位姿解算。定义第k+m-1与k+m帧点云解算出的位姿增量为则初值补偿矩阵更新方法为：
其中，表示第k帧关键帧点云的位姿，/>为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，/>为第k+m帧与关键帧的外部位姿向量，/>第k+m-1帧与关键帧的内部位姿向量。
计算机体当前位姿为：
将结果传递至步骤2。
本发明的三维激光雷达快速鲁棒SLAM方法，采用高度传感器、惯性测量单元对激光雷达进行信息辅助，补偿点云配准前激光雷达点云的初始位姿，并通过解耦三维激光雷达点云配准算法实现UAV的六自由度导航解算。本发明相比于传统激光SLAM算法，有效抑制了点云过度匹配带来的高度解算误差以及水平耦合误差，提高了复杂环境下UAV的位姿估计精度，并且具有更高的计算效率。
如图2所示，本发明提供一种三维激光雷达快速鲁棒SLAM装置，实现三维激光雷达快速鲁棒SLAM方法，包括：
获取模块，用于采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
补偿模块，用于采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
计算模块，用于利用位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿。
更新模块，用于根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至补偿模块。
进一步，所述补偿模块包括：
转换单元，用于将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
补偿单元，用于将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
进一步，所述更新模块具体为：
设第k+m-1与k+m帧点云解算出的位姿增量为则初值补偿矩阵更新方法为：
其中，表示第k帧关键帧点云的位姿，/>为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，/>为第k+m帧与关键帧的外部位姿向量，/>第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
将结果传递至补偿模块。
以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (7)

1.一种三维激光雷达快速鲁棒SLAM方法，其特征在于，包括如下步骤：
步骤1、采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
步骤2、采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
步骤3、利用步骤2中位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿；
步骤4、根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至步骤2；
其中，步骤3具体包括：
步骤31、构建两帧点云之间的关联
对空间进行网格化处理，根据空间网格中的点云分布及其坐标数据，对关键帧点云进行多元正态分布函数表示：
其中：μk、Σk分别为关键帧第k个网格中空间点云的均值与协方差；为输入点云中第i个点；/>为/>在该网格中对应的多元正态分布函数的概率值；d1、d2为常数；
步骤32、构建待目标函数
待优化函数为输入点云概率值之和，目标为求解输入点云的最优变换，使得该待优化函数值最大，待优化变量即为无人机内部位姿变化量
其中，为根据转换矩阵/>计算出的输入点云总概率值，/>为输入点云中的第k个点，/>为第k-1至k帧点云无人机内部位姿变化量：
步骤33、求解目标函数
位姿变化量解算采用高斯牛顿迭代法，求解无人机内部位姿变化量的正规方程如下式：
其中，H、g为待优化函数的Hessian矩阵和Jacobian矩阵，
令:
其中，Tcal表示位姿解算过程中第k帧点云第i次迭代后机体系下激光雷达点云坐标；表示第i次迭代后机体内部位姿向量相对于第k-1帧点云对应的机体内部位姿向量的变化量，定义/>为/>的第i个元素，则有：
其中，gi、Hij分别为g、H的元素；Tcal的Jacobian矩阵与Hessian矩阵分别为：
。
2.如权利要求1所述的三维激光雷达快速鲁棒SLAM方法，其特征在于，步骤1具体包括：
步骤11、根据惯性传感器获得三轴角速度以及加速度信息，通过航姿算法得到惯性传感器相对于全局系的姿态角得到无人机机体相对于全局系的姿态角/>
其中，为机体与惯性传感器之间安装误差对应的旋转矩阵；
步骤12、获取高度传感器测距信息ldis，并根据惯性传感器解算的机体相对于全局坐标系的姿态角对测距信息ldis进行修正，得到机体距离地面高度/>
其中：分别为机体系相对于全局系的横滚角与俯仰角，/>为高度传感器在机体系下的高度坐标值；
步骤13、分别定义机体外部位姿向量内部位姿向量/>为：
其中，为第k帧点云对应的无人机外部位姿向量，由机体系在全局系下的横滚角俯仰角/>及高度/>组成；/>为第k帧点云对应的无人机内部位姿向量，由机体系在全局系下水平位置/>以及航向角组成/>
3.如权利要求1或2所述的三维激光雷达快速鲁棒SLAM方法，其特征在于，步骤2具体包括：
步骤21、将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
步骤22、将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
4.如权利要求3所述的三维激光雷达快速鲁棒SLAM方法，其特征在于，步骤4具体包括：
设第k+m-1与k+m帧点云解算出的位姿增量为则初值补偿矩阵更新方法为：
其中，表示第k帧关键帧点云的位姿，/>为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，/>为第k+m帧与关键帧的外部位姿向量，/>第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
将结果传递至步骤2。
5.一种实现权利要求1至4任意一项所述三维激光雷达快速鲁棒SLAM方法的三维激光雷达快速鲁棒SLAM装置，其特征在于，包括：
获取模块，用于采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
补偿模块，用于采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
计算模块，用于利用位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿；
更新模块，用于根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至补偿模块。
6.如权利要求5所述的三维激光雷达快速鲁棒SLAM装置，其特征在于，所述补偿模块包括：
转换单元，用于将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
补偿单元，用于将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
7.如权利要求6所述的三维激光雷达快速鲁棒SLAM装置，其特征在于，所述更新模块具体为：
设第k+m-1与k+m帧点云解算出的位姿增量为则初值补偿矩阵更新方法为：
其中，表示第k帧关键帧点云的位姿，/>为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，/>为第k+m帧与关键帧的外部位姿向量，/>第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
将结果传递至补偿模块。
CN202110440733.7A 2021-04-23 2021-04-23 一种三维激光雷达快速鲁棒slam方法和装置 Active CN113129377B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110440733.7A CN113129377B (zh) 2021-04-23 2021-04-23 一种三维激光雷达快速鲁棒slam方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110440733.7A CN113129377B (zh) 2021-04-23 2021-04-23 一种三维激光雷达快速鲁棒slam方法和装置

Publications (2)

Publication Number Publication Date
CN113129377A CN113129377A (zh) 2021-07-16
CN113129377B true CN113129377B (zh) 2024-05-03

Family

ID=76779263

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110440733.7A Active CN113129377B (zh) 2021-04-23 2021-04-23 一种三维激光雷达快速鲁棒slam方法和装置

Country Status (1)

Country Link
CN (1) CN113129377B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114858226B (zh) * 2022-07-05 2022-10-25 武汉大水云科技有限公司 一种无人机山洪流量测量方法、装置及设备
CN116734830A (zh) * 2023-05-17 2023-09-12 中山大学 一种激光雷达同时定位与建图的方法及系统
CN117689698B (zh) * 2024-02-04 2024-04-19 安徽蔚来智驾科技有限公司 点云配准方法、智能设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
一种基于图优化的实时3D激光SLAM算法;柯艳国;王雄奇;魏新;吴贤斌;朱仲贤;董翔宇;黄杰;董二宝;;机电一体化;20200415(Z1);第94-101页 *
合作环境下基于惯性/里程计/Lidar融合的UGV导航方法;付相可;《导航定位与授时》;20201112;第94-101页 *
复杂环境下微小飞行器惯性/激光雷达Robust-SLAM方法;石鹏;《导航定位与授时》;20181224;第14-21页 *

Also Published As

Publication number Publication date
CN113129377A (zh) 2021-07-16

Similar Documents

Publication Publication Date Title
CN109709801B (zh) 一种基于激光雷达的室内无人机定位系统及方法
CN113066105B (zh) 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113129377B (zh) 一种三维激光雷达快速鲁棒slam方法和装置
CN101598556B (zh) 一种未知环境下无人机视觉/惯性组合导航方法
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN112347840A (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN111366148B (zh) 适用于机载光电观瞄系统多次观察的目标定位方法
CN106871927A (zh) 一种无人机光电吊舱安装误差标校方法
CN110081878B (zh) 一种多旋翼无人机的姿态及位置确定方法
CN106123890A (zh) 一种多传感器数据融合的机器人定位方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
JP2020530569A (ja) 車両センサの較正及び位置特定
CN106871928A (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN112781594B (zh) 基于imu耦合的激光雷达迭代最近点改进算法
CN108151737A (zh) 一种动态互观测关系条件下的无人机蜂群协同导航方法
CN107728182A (zh) 基于相机辅助的柔性多基线测量方法和装置
CN108458709B (zh) 基于视觉辅助测量的机载分布式pos数据融合方法和装置
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
WO2023226155A1 (zh) 多源数据融合定位方法、装置、设备及计算机存储介质
CN112985421B (zh) 一种基于角度约束辅助测量的航天器自主天文导航方法
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN115902930A (zh) 一种面向船舶检测的无人机室内建图与定位方法
CN103438907A (zh) 一种星敏感器六自由度像平面误差的在轨标定方法
CN110849349A (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