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

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

Info

Publication number
CN113129377A
CN113129377A CN202110440733.7A CN202110440733A CN113129377A CN 113129377 A CN113129377 A CN 113129377A CN 202110440733 A CN202110440733 A CN 202110440733A CN 113129377 A CN113129377 A CN 113129377A
Authority
CN
China
Prior art keywords
pose
point cloud
frame
matrix
laser radar
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
CN202110440733.7A
Other languages
English (en)
Other versions
CN113129377B (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
Priority claimed from CN202110440733.7A external-priority 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

Images

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

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、根据惯性传感器获得三轴角速度以及加速度信息，通过航姿算法得到惯性传感器相对于全局系的姿态角
Figure BDA0003034873640000021
得到无人机机体相对于全局系的姿态角
Figure BDA0003034873640000022
Figure BDA0003034873640000023
其中，
Figure BDA0003034873640000024
为机体与惯性传感器之间安装误差对应的旋转矩阵；
步骤12、获取高度传感器测距信息ldis，并根据惯性传感器解算的机体相对于全局坐标系的姿态角
Figure BDA0003034873640000025
对测距信息ldis进行修正，得到机体距离地面高度
Figure BDA0003034873640000026
Figure BDA0003034873640000027
其中：
Figure BDA0003034873640000028
分别为机体系相对于全局系的横滚角与俯仰角，
Figure BDA0003034873640000029
为高度传感器在机体系下的高度坐标值；
步骤13、分别定义机体外部位姿向量
Figure BDA00030348736400000210
内部位姿向量
Figure BDA00030348736400000211
为：
Figure BDA00030348736400000212
Figure BDA0003034873640000031
其中，
Figure BDA0003034873640000032
为第k帧点云对应的无人机外部位姿向量，由机体系在全局系下的横滚角
Figure BDA0003034873640000033
俯仰角
Figure BDA0003034873640000034
及高度
Figure BDA0003034873640000035
组成；
Figure BDA0003034873640000036
为第k帧点云对应的无人机内部位姿向量，由机体系在全局系下水平位置
Figure BDA0003034873640000037
以及航向角组成
Figure BDA0003034873640000038
作为优选，步骤2具体包括：
步骤21、将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
步骤22、将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
作为优选，步骤3具体包括：
步骤31、构建两帧点云之间的关联
对空间进行网格化处理，根据空间网格中的点云分布及其坐标数据，对关键帧点云进行多元正态分布函数表示：
Figure BDA0003034873640000039
其中：μk、Σk分别为关键帧第k个网格中空间点云的均值与协方差；
Figure BDA00030348736400000310
为输入点云中第i个点；
Figure BDA00030348736400000311
为
Figure BDA00030348736400000312
在该网格中对应的多元正态分布函数的概率值；d1、d2为常数；
步骤32、构建待目标函数
待优化函数为输入点云概率值之和，目标为求解输入点云的最优变换，使得该待优化函数值最大，待优化变量即为无人机内部位姿变化量
Figure BDA00030348736400000313
Figure BDA0003034873640000041
其中，
Figure BDA0003034873640000042
为根据转换矩阵
Figure BDA0003034873640000043
计算出的输入点云总概率值，
Figure BDA0003034873640000044
为输入点云中的第k个点，
Figure BDA0003034873640000045
为第k-1至k帧点云无人机内部位姿变化量：
Figure BDA0003034873640000046
Figure BDA0003034873640000047
步骤33、求解目标函数
位姿变化量解算采用高斯牛顿迭代法，求解无人机内部位姿变化量的正规方程如下式：
Figure BDA0003034873640000048
其中，H、g为待优化函数的Hessian矩阵和Jacobian矩阵，
令:
Figure BDA0003034873640000049
其中，Tcal表示位姿解算过程中第k帧点云第i次迭代后机体系下激光雷达点云坐标；
Figure BDA00030348736400000410
表示第i次迭代后机体内部位姿向量相对于第k-1帧点云对应的机体内部位姿向量的变化量，定义
Figure BDA00030348736400000411
为
Figure BDA00030348736400000412
的第i个元素，则有：
Figure BDA00030348736400000413
Figure BDA00030348736400000414
其中，gi、Hij分别为g、H的元素；Tcal的Jacobian矩阵与Hessian矩阵分别为：
Figure BDA0003034873640000051
Figure BDA0003034873640000052
作为优选，步骤4具体包括：
设第k+m-1与k+m帧点云解算出的位姿增量为
Figure BDA0003034873640000053
则初值补偿矩阵更新方法为：
Figure BDA0003034873640000054
其中，
Figure BDA0003034873640000055
表示第k帧关键帧点云的位姿，
Figure BDA0003034873640000056
为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，
Figure BDA0003034873640000057
为第k+m帧与关键帧的外部位姿向量，
Figure BDA0003034873640000058
第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
Figure BDA0003034873640000059
将结果传递至步骤2。
本发明提供一种三维激光雷达快速鲁棒SLAM装置，包括：
获取模块，用于采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
补偿模块，用于采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
计算模块，用于利用位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿；
更新模块，用于根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至补偿模块。
作为优选，所述补偿模块包括：
转换单元，用于将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
补偿单元，用于将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
作为优选，所述更新模块具体为：
设第k+m-1与k+m帧点云解算出的位姿增量为
Figure BDA0003034873640000061
则初值补偿矩阵更新方法为：
Figure BDA0003034873640000062
其中，
Figure BDA0003034873640000063
表示第k帧关键帧点云的位姿，
Figure BDA0003034873640000064
为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，
Figure BDA0003034873640000065
为第k+m帧与关键帧的外部位姿向量，
Figure BDA0003034873640000066
第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
Figure BDA0003034873640000071
将结果传递至补偿模块。
与现有技术相比，本发明具有以下有益效果：
通过本发明，可以解决由于环境特征退化使得点云过度配准，从而带来的导航精度降低的问题。与传统SLAM相比，本发明有效抑制了点云过度匹配带来的高度解算误差以及水平耦合误差，提高了复杂环境下的位姿估计精度，并且具有更高的计算效率。
附图说明
图1为本发明三维激光雷达快速鲁棒SLAM方法的流程示意图；
图2为本发明一种三维激光雷达快速鲁棒SLAM装置的结构示意图。
具体实施方式
如图1所示，本发明提供一种三维激光雷达快速鲁棒SLAM方法，包括以下步骤：
步骤1、采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
步骤2、采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
步骤3、利用步骤二中位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿；
步骤4：根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至步骤2。
进一步，步骤1具体包括：
步骤11、根据惯性传感器与高度传感器数据，计算机体系相对于全局系的位姿矩阵。从惯性传感器获得三轴角速度以及加速度信息，通过航姿算法可得惯性传感器相对于全局系的姿态角
Figure BDA0003034873640000081
可得无人机机体相对于全局系的姿态角
Figure BDA0003034873640000082
Figure BDA0003034873640000083
其中，
Figure BDA0003034873640000084
为机体与惯性传感器之间安装误差对应的旋转矩阵。
步骤12、获取激光测距仪测距信息ldis，并根据惯性传感器解算的机体相对于全局坐标系的姿态角
Figure BDA0003034873640000085
对测距信息ldis进行修正，可得机体距离地面高度
Figure BDA0003034873640000086
Figure BDA0003034873640000087
其中：
Figure BDA0003034873640000088
分别为机体系相对于全局系的横滚角与俯仰角。
Figure BDA0003034873640000089
为高度传感器在机体系下的高度坐标值。
步骤13、分别定义机体外部位姿向量
Figure BDA00030348736400000810
内部位姿向量
Figure BDA00030348736400000811
为：
Figure BDA00030348736400000812
Figure BDA00030348736400000813
其中，
Figure BDA00030348736400000814
为第k帧点云对应的无人机外部位姿向量，由机体系在全局系下的横滚角
Figure BDA00030348736400000815
俯仰角
Figure BDA00030348736400000816
及高度
Figure BDA00030348736400000817
组成；
Figure BDA00030348736400000818
为第k帧点云对应的无人机内部位姿向量，由机体系在全局系下水平位置
Figure BDA00030348736400000819
Figure BDA00030348736400000820
以及航向角组成
Figure BDA00030348736400000821
进一步，步骤2具体包括：
步骤21、初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成，首先将其转换为转换矩阵
Figure BDA00030348736400000822
Figure BDA0003034873640000091
其中，
Figure BDA0003034873640000092
表示第k帧点云的初始位姿向量对应的转换矩阵。
步骤22、将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
Figure BDA0003034873640000093
其中：
Figure BDA0003034873640000094
为第k帧点云对应的机体系下输入点云坐标；
Figure BDA0003034873640000095
为其经过初始位姿补偿后的点云坐标。
进一步，步骤3具体包括：
步骤31、构建两帧点云(输入点云、目标点云)之间的关联
采用NDT算法，对空间进行网格化处理，随后根据空间网格中的点云分布及其坐标数据，对关键帧点云进行多元正态分布函数表示：
Figure BDA0003034873640000096
其中：μk、Σk分别为关键帧第k个网格中空间点云的均值与协方差；
Figure BDA0003034873640000097
为输入点云中第i个点；
Figure BDA0003034873640000098
为
Figure BDA0003034873640000099
在该网格中对应的多元正态分布函数的概率值；d1、d2为常数。
步骤32、构建待目标函数
待优化函数为输入点云概率值之和，目标为求解输入点云的最优变换，使得该待优化函数值最大，待优化变量即为无人机内部位姿变化量
Figure BDA00030348736400000910
Figure BDA00030348736400000911
其中：
Figure BDA0003034873640000101
为根据转换矩阵
Figure BDA0003034873640000102
计算出的输入点云总概率值，
Figure BDA0003034873640000103
为输入点云中的第k个点，
Figure BDA0003034873640000104
为第k-1至k帧点云无人机内部位姿变化量：
Figure BDA0003034873640000105
Figure BDA0003034873640000106
步骤33、求解目标函数
位姿变化量解算采用高斯牛顿迭代法，求解无人机内部位姿变化量的正规方程如下式：
Figure BDA0003034873640000107
其中：H、g为待优化函数的Hessian矩阵和Jacobian矩阵。
令:
Figure BDA0003034873640000108
其中，Tcal表示位姿解算过程中第k帧点云第i次迭代后机体系下激光雷达点云坐标；
Figure BDA0003034873640000109
表示第i次迭代后机体内部位姿向量相对于第k-1帧点云对应的机体内部位姿向量的变化量。定义
Figure BDA00030348736400001010
为
Figure BDA00030348736400001011
的第i个元素，则有：
Figure BDA00030348736400001012
Figure BDA00030348736400001013
其中，gi、Hij分别为g、H的元素；Tcal的Jacobian矩阵与Hessian矩阵分别为：
Figure BDA0003034873640000111
Figure BDA0003034873640000112
进一步，步骤4具体为：采用Hdl_Graph_Slam算法中的导航框架进行导航解算，改进后框架内引入惯性/高度传感器的位姿解算。定义第k+m-1与k+m帧点云解算出的位姿增量为
Figure BDA0003034873640000113
则初值补偿矩阵更新方法为：
Figure BDA0003034873640000114
其中，
Figure BDA0003034873640000115
表示第k帧关键帧点云的位姿，
Figure BDA0003034873640000116
为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，
Figure BDA0003034873640000117
为第k+m帧与关键帧的外部位姿向量，
Figure BDA0003034873640000118
第k+m-1帧与关键帧的内部位姿向量。
计算机体当前位姿为：
Figure BDA0003034873640000119
将结果传递至步骤2。
本发明的三维激光雷达快速鲁棒SLAM方法，采用高度传感器、惯性测量单元对激光雷达进行信息辅助，补偿点云配准前激光雷达点云的初始位姿，并通过解耦三维激光雷达点云配准算法实现UAV的六自由度导航解算。本发明相比于传统激光SLAM算法，有效抑制了点云过度匹配带来的高度解算误差以及水平耦合误差，提高了复杂环境下UAV的位姿估计精度，并且具有更高的计算效率。
如图2所示，本发明提供一种三维激光雷达快速鲁棒SLAM装置，实现三维激光雷达快速鲁棒SLAM方法，包括：
获取模块，用于采集惯性传感器以及高度传感器数据，并据此计算当前时刻机体坐标系相对于全局坐标系的横滚角、俯仰角与高度；
补偿模块，用于采集激光雷达点云数据，并对激光雷达点云进行位姿补偿；
计算模块，用于利用位姿补偿后的点云，进行解耦点云配准算法的计算，解算当前时刻的机体六自由度位姿。
更新模块，用于根据SLAM框架的更新条件对位姿矩阵进行更新，并将结果传递至补偿模块。
进一步，所述补偿模块包括：
转换单元，用于将初始位姿向量转换为转换矩阵，所述初始位姿向量由第k帧点云对应的无人机外部位姿向量以及第k-1帧点云解算出的无人机内部位姿向量组成；
补偿单元，用于将滤波去噪后的三维激光雷达点云根据初始位姿矩阵进行补偿，获得初始位姿较好的激光雷达点云。
进一步，所述更新模块具体为：
设第k+m-1与k+m帧点云解算出的位姿增量为
Figure BDA0003034873640000121
则初值补偿矩阵更新方法为：
Figure BDA0003034873640000131
其中，
Figure BDA0003034873640000132
表示第k帧关键帧点云的位姿，
Figure BDA0003034873640000133
为解算第k+m帧与关键帧相对位姿的初始位姿矩阵，
Figure BDA0003034873640000134
为第k+m帧与关键帧的外部位姿向量，
Figure BDA0003034873640000135
第k+m-1帧与关键帧的内部位姿向量，
计算机体当前位姿为：
Figure BDA0003034873640000136
将结果传递至补偿模块。
以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (8)

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

Priority Applications (1)

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

Applications Claiming Priority (1)

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

Publications (2)

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

Family

ID=

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114858226A (zh) * 2022-07-05 2022-08-05 武汉大水云科技有限公司 一种无人机山洪流量测量方法、装置及设备
CN117689698A (zh) * 2024-02-04 2024-03-12 安徽蔚来智驾科技有限公司 点云配准方法、智能设备及存储介质

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
付相可: "合作环境下基于惯性/里程计/Lidar融合的UGV导航方法", 《导航定位与授时》, 12 November 2020 (2020-11-12), pages 94 - 101 *
柯艳国;王雄奇;魏新;吴贤斌;朱仲贤;董翔宇;黄杰;董二宝;: "一种基于图优化的实时3D激光SLAM算法", 机电一体化, no. 1, 15 April 2020 (2020-04-15), pages 94 - 101 *
石鹏: "复杂环境下微小飞行器惯性/激光雷达Robust-SLAM方法", 《导航定位与授时》, 24 December 2018 (2018-12-24), pages 14 - 21 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114858226A (zh) * 2022-07-05 2022-08-05 武汉大水云科技有限公司 一种无人机山洪流量测量方法、装置及设备
CN114858226B (zh) * 2022-07-05 2022-10-25 武汉大水云科技有限公司 一种无人机山洪流量测量方法、装置及设备
CN117689698A (zh) * 2024-02-04 2024-03-12 安徽蔚来智驾科技有限公司 点云配准方法、智能设备及存储介质
CN117689698B (zh) * 2024-02-04 2024-04-19 安徽蔚来智驾科技有限公司 点云配准方法、智能设备及存储介质

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN109709801B (zh) 一种基于激光雷达的室内无人机定位系统及方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN111426318B (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN111337943B (zh) 一种基于视觉引导激光重定位的移动机器人定位方法
CN107728182B (zh) 基于相机辅助的柔性多基线测量方法和装置
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN112083725A (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN111366148B (zh) 适用于机载光电观瞄系统多次观察的目标定位方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN108458709B (zh) 基于视觉辅助测量的机载分布式pos数据融合方法和装置
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN109443354B (zh) 基于萤火虫群优化pf的视觉-惯性紧耦合组合导航方法
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN110686684A (zh) 一种小天体环绕探测器光学协同定轨方法
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN111696155A (zh) 一种基于单目视觉的多传感融合机器人定位方法
WO2023226155A1 (zh) 多源数据融合定位方法、装置、设备及计算机存储介质
Tao et al. SLAM Method Based on Multi-Sensor Information Fusion
CN113129377B (zh) 一种三维激光雷达快速鲁棒slam方法和装置
CN113129377A (zh) 一种三维激光雷达快速鲁棒slam方法和装置
Xiaoqian et al. Nonlinear extended Kalman filter for attitude estimation of the fixed-wing UAV

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