CN116772828B - 一种基于图优化的多传感器融合定位与建图方法 - Google Patents

一种基于图优化的多传感器融合定位与建图方法 Download PDF

Info

Publication number
CN116772828B
CN116772828B CN202311068502.3A CN202311068502A CN116772828B CN 116772828 B CN116772828 B CN 116772828B CN 202311068502 A CN202311068502 A CN 202311068502A CN 116772828 B CN116772828 B CN 116772828B
Authority
CN
China
Prior art keywords
gps
odo
coordinate system
imu
constraint
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
CN202311068502.3A
Other languages
English (en)
Other versions
CN116772828A (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.)
Changchun University of Technology
Original Assignee
Changchun University of Technology
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 Changchun University of Technology filed Critical Changchun University of Technology
Priority to CN202311068502.3A priority Critical patent/CN116772828B/zh
Publication of CN116772828A publication Critical patent/CN116772828A/zh
Application granted granted Critical
Publication of CN116772828B publication Critical patent/CN116772828B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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

  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于图优化的多传感器融合定位与建图方法，涉及机器人感知领域。首先该方法建立在图优化的基础上，基于滑动窗口实现激光雷达、惯性测量单元（IMU）、轮速里程计（ODO）及GPS的多源数据联合非线性优化。首先通过IMU与ODO采集的信息构建联合预积分模型，基于预积分模型修正激光雷达观测数据与重复积分的耗时问题；其次构建4种类型不同的传感器的残差项与约束方程；最后图优化的方式建立非线性最小二乘目标函数式联合优化关键帧与子地图的位姿。本发明应用于同步定位与地图构建，与现有技术相比，本发明具有良好的建图精度和鲁棒性，能够实现更高精度的定位与建图效果，完成更高精度的全局位姿图优化。

Description

一种基于图优化的多传感器融合定位与建图方法
技术领域
本发明涉及机器人感知领域，具体涉及一种基于图优化的多传感器融合定位与建图方法。
背景技术
同时定位与建图技术(Simultaneous localization and mapping，SLAM)突破传统定位方法的局限，它所关注的问题是载有传感器的移动机器人如何在未知环境汇总定位并构建出环境地图，是机器人估计自身状态和感知外部环境的关键技术。在SLAM发展过程中，各种传感器承担着至关重要的角色，为定位与建图算法提供全局或局部的测量信息，其中，激光雷达、摄像头、惯性测量单元（IMU）为该领域研究的核心传感器。
目前，SLAM算法主要分为激光SLAM算法和视觉SLAM算法，激光SLAM能够提供载有传感器的机器人本体与周围环境障碍物间的较为准确的距离信息，误差模型简单，对光照不敏感，点云的处理比较容易且理论研究也相对成熟，落地产品更丰富，但其重定位能力较差，激光SLAM在跟踪丢失后很难重新回到工作状态，对动态变化较大的场景适应性也较弱。视觉SLAM具有体积小、重量轻、非接触、小范围内定位精度高等优点。视觉SLAM技术信息量丰富、智能化高、探测范围广、信息完整，可提取语义信息，但在动态环境、显著特征过多或过少以及存在部分或全部遮挡的条件下工作时，会导致建图失败，且受天气、光照影响较大。面对日趋复杂的应用场景，依靠单一传感器的SLAM系统进行同时定位与建图存在一定的局限性，将多种传感器组合，利用不同传感器的优势克服其他传感器的缺陷来提高定位建图算法在不同场景中的适用性和对位姿估计的准确性是未来研究发展的趋势。因此，本发明提出一种基于图优化的多传感器融合定位与建图方法，通过将激光雷达、IMU、轮速里程计（ODO）、GPS四种传感器进行融合，可提高定位与建图的精度与SLAM系统的鲁棒性，对于提升SLAM系统定位与建图效果具有重要意义。
发明内容
本发明的目的在于提供了一种具有高精度、鲁棒性强、建图误差小的基于图优化的多传感器融合定位与建图方法。
本发明的技术方案如下：一种基于图优化的多传感器融合定位与建图方法，具体实施步骤为：
步骤一：采集目标区域的激光雷达、惯性测量单元（IMU）、轮速里程计（ODO），GPS的数据；
步骤二：将IMU与ODO采集的数据进行联合预积分处理，构建IMU/ODO联合预积分模型；
步骤三：利用IMU与ODO计算位姿变化得到的初始位姿信息对点云进行畸变校正，对点云数据进行预处理；
步骤四：采用正态分布点云算法（NDT）对预处理后的点云数据进行配准，构建激光里程计；
步骤五：将构建的IMU/ODO联合预积分模型与激光里程计结合，构建激光惯性里程计，并将上述配准后的点云数据经过激光惯性里程计进行状态估计，得到位姿信息；
步骤六：对上述位姿信息进行关键帧处理，并将点云当前帧与关键帧相结合构建子地图；
步骤七：引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS位置约束的残差项；
步骤八：采用滑动窗口边缘化的方式限制优化变量的数量，通过边缘化的方式，增加边缘化约束，构建滑动窗口边缘化约束方程；
步骤九：采用图优化的方式对构建的残差项与约束方程进行联合优化，通过建立非线性最小二乘目标函数式优化关键帧与子地图的位姿；
步骤十：将全局描述符LiDAR Iris用于闭环检测，修正累积误差，提高优化结果的精度，获得最终的状态估计及全局地图。
本发明的特点在于：
步骤二中，将IMU与ODO采集的数据进行联合预积分处理，构建IMU/ODO联合预积分模型，具体按照以下步骤实施：
机体坐标系为，轮速里程计（ODO）坐标系为，世界坐标系为。IMU/ODO联合状 态传递方程如下所示：
，
，
，
，
其中，
，，
式中，和分别为和时刻机体在世界坐标系下的位置向量，和分别为和时刻机体在世界坐标系下的速度向量，和为和时刻机 体系相对于世界坐标系下的姿态。为时间间隔，为时刻从机体系到世界坐标 系的旋转矩阵，为时刻从轮速里程计（ODO）坐标系到机体系的旋转矩阵，为 世界坐标系下的重力向量。和分别为时刻加速度及与陀螺输出，和为时刻 加速度计与陀螺的零偏，和分别为加速度计和陀螺仪的噪声。和分别为和时刻轮速里程计（ODO）坐标系到机体系的旋转矩阵，为时刻轮速里程计的标度 因数，为轮速里程计的噪声，为时刻下的速度。将陀螺仪的数值转换为4×4的 矩阵形式，为陀螺仪的读数。
将上式中的状态传播模型投影到时刻的机体系下，可得：
，
，
，
，
其中，
，
，
，
，
式中，为时刻从世界坐标系到机体系的旋转矩阵，和为与时刻机体在世界坐标系下的位置向量，为时刻的机体相对于机体系的预 积分位置变化量，与为与时刻机体在世界坐标系下的速度向量，为时刻的机体相对于系的预积分速度变化量，和为和时刻世界坐标系 下相对于机体系的姿态，为时刻的机体相对于机体系的预积分旋转变化量，为时刻的机体相对于系的预积分位置变化量，为时刻从世界坐标系到 机体系的旋转矩阵，为时刻从轮速里程计（ODO）坐标系到机体系的旋转矩阵。
构建联合预积分模型一方面提高系统运行效率，另一方面为激光雷达前端提供良好的位姿初值，并为SLAM系统提供位姿解算，通过联合预积分模型构建IMU/ODO预积分残差项为：
，
步骤七中，引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS位置约束的残差方程，具体按照以下步骤实施：
GPS解算出的绝对位置信息首先将其转换为世界坐标系下，最后在世界坐标系下形成GPS绝对位置约束的残差项：
，
式中，表示GPS绝对位置残差，表示GPS在世界坐标系的定位结果， 表示GPS的信息，表示GPS系下的定位结果，为GPS系转换为世界坐标系的矩阵变换 参数。
步骤八中，采用滑动窗口边缘化的方式限制优化变量的数量，通过边缘化的方式，增加边缘化约束，构建滑动窗口边缘化约束方程，滑动窗口边缘化约束方程为：
，
为滑动窗口保留的变量信息。
步骤九中：采用图优化的方式对构建的残差项与约束方程进行联合优化，通过建立非线性最小二乘目标函数式优化关键帧与子地图的位姿，具体按照以下步骤实施：
首先构建上述传感器的约束方程，将步骤二构建的IMU/ODO预积分残差项构建 IMU/ODO预积分约束方程，IMU/ODO预积分约束方程如下所示：
，
其中，为关键帧数，为当前关键帧数，表示滑动窗口内待估计的参数， 为IMU/ODO的预积分协方差矩阵，为IMU/ODO预积分残差项。
将步骤七中构建的GPS绝对位置的残差方程构建GPS绝对位置约束方程，GPS绝 对位置约束方程如下所示：
，
表示GPS系下的定位结果，为GPS系转换为世界坐标系的矩阵变换参数，为GPS解算位姿的协方差矩阵，表示当前GPS的位姿信息。
通过激光雷达当前帧与关键帧构成的子地图进行匹配，构建激光雷达匹配残差 项：，并通过激光雷达匹配残差项构建雷达相对位姿约束方程，雷达相对位姿 约束方程为：
，
与为当前雷达位姿信息和相对位姿信息， 为雷达相对位姿约束 协方差矩阵。
建立非线性最小二乘目标函数式优化关键帧与子地图的位姿：
，
其中，
，
，
，
，
约束条件包括IMU/ODO预积分约束，GPS绝对位置约束，雷达相对位姿约束，滑动窗口边缘化约束。
本发明相对于现有技术具有如下的优点和效果：
本发明提供了一种基于图优化的多传感器融合定位与建图方法，首先该方法建立在图优化的基础上，基于滑动窗口实现激光雷达、IMU/ODO、GPS的多源数据联合非线性优化，惯性测量单元（IMU）与轮速里程计（ODO）联合预积分约束修正激光雷达观测数据，经过相对位姿估计之后形成的LIDAR约束通过图优化的方式求解IMU的零偏及ODO比例因子，并将全局描述符LiDAR Iris用于闭环检测，形成紧耦合的多传感器融合系统，实现高精度的定位与建图。本发明应用于同步定位与地图构建，与现有技术相比，能够实现更高精度的定位与建图效果，具有更好的鲁棒性，实现更高精度的全局位姿图优化。
附图说明
图1是本发明的一种基于图优化的多传感器融合定位与建图方法的系统框图；
图2（a）是本发明在KITTI数据集00序列下的估计值和其他主流算法估计值与真实值的绝对轨迹误差值（APE）；
图2（b）是本发明在KITTI数据集02序列下的估计值和其他主流算法估计值与真实值的绝对轨迹误差值（APE）；
图2（c）是本发明在KITTI数据集05序列下的估计值和其他主流算法估计值与真实值的绝对轨迹误差值（APE）；
图2（d）是本发明在KITTI数据集07序列下的估计值和其他主流算法估计值与真实值的绝对轨迹误差值（APE）；
图3（a）是本发明在KITTI数据集00序列下的位姿估计值关于平移向量在x、y、z三个轴上的拟合曲线；
图3（b）是本发明在KITTI数据集02序列下的位姿估计值关于平移向量在x、y、z三个轴上的拟合曲线；
图4是本发明的位姿轨迹图；
图5（a）是本发明在KITTI数据集00序列下构建的地图；
图5（b）是本发明在KITTI数据集02序列下构建的地图；
图5（c）是本发明在KITTI数据集05序列下构建的地图；
图5（d）是本发明在KITTI数据集07序列下构建的地图。
具体实施方式
下面结合附图和实施例对本发明做进一步详细说明。
本发明公开了一种基于图优化的多传感器融合定位与建图方法，涉及机器人感知领域。首先该方法建立在图优化的基础上，基于滑动窗口实现激光雷达、惯性测量单元（IMU）、轮速里程计（ODO）及GPS的多源数据联合非线性优化。首先通过IMU与ODO采集的信息构建联合预积分模型，基于预积分模型修正激光雷达观测数据与重复积分的耗时问题；其次构建4种类型不同的传感器的残差项与约束方程；最后图优化的方式建立非线性最小二乘目标函数式联合优化关键帧与子地图的位姿。本发明应用于同步定位与地图构建，与现有技术相比，本发明具有良好的建图精度和鲁棒性，能够实现更高精度的定位与建图效果，完成更高精度的全局位姿图优化。
本发明提供的一种基于图优化的多传感器融合定位与建图方法的系统框图如图1所示,具体按照以下步骤实施:
步骤一：采集目标区域的激光雷达、惯性测量单元（IMU）、轮速里程计（ODO），GPS的数据；
步骤二：将IMU与ODO采集的数据进行联合预积分处理，构建IMU/ODO联合预积分模型；
在步骤二中，将IMU与ODO采集的数据进行联合预积分处理，构建IMU/ODO联合预积分模型，具体按照以下步骤实施：
机体坐标系为，轮速里程计（ODO）坐标系为，世界坐标系为。IMU/ODO联合状 态传递方程如下所示：
，
，
，
，
其中，
，，
式中，和分别为和时刻机体在世界坐标系下的位置向量，和分别为和时刻机体在世界坐标系下的速度向量，和为和时刻机 体系相对于世界坐标系下的姿态。为时间间隔，为时刻从机体系到世界坐标 系的旋转矩阵，为时刻从轮速里程计（ODO）坐标系到机体系的旋转矩阵，为 世界坐标系下的重力向量。和分别为时刻加速度及与陀螺输出，和为时刻 加速度计与陀螺的零偏，和分别为加速度计和陀螺仪的噪声。和分别为和时刻轮速里程计（ODO）坐标系到机体系的旋转矩阵，为时刻轮速里程计的标度 因数，为轮速里程计的噪声，为时刻下的速度。将陀螺仪的数值转换为4×4的 矩阵形式，为陀螺仪的读数。
将上式中的状态传播模型投影到时刻的机体系下，可得：
，
，
，
，
其中，
，
，
，
，
式中，为时刻从世界坐标系到机体系的旋转矩阵，和为与时刻机体在世界坐标系下的位置向量，为时刻的机体相对于机体系的预 积分位置变化量，与为与时刻机体在世界坐标系下的速度向量，为时刻的机体相对于系的预积分速度变化量，和为和时刻世界坐标系 下相对于机体系的姿态，为时刻的机体相对于机体系的预积分旋转变化量，为时刻的机体相对于系的预积分位置变化量，为时刻从世界坐标系到 机体系的旋转矩阵，为时刻从轮速里程计（ODO）坐标系到机体系的旋转矩阵。
步骤三：利用IMU与ODO计算位姿变化得到的初始位姿信息对点云进行畸变校正，对点云数据进行预处理；
步骤四：采用正态分布点云算法（NDT）对预处理后的点云数据进行配准，构建激光里程计；
步骤五：将构建的IMU/ODO联合预积分模型与激光里程计结合，构建激光惯性里程计，并将上述配准后的点云数据经过激光惯性里程计进行状态估计，得到位姿信息；
步骤六：对上述位姿信息进行关键帧处理，并将点云当前帧与关键帧相结合构建子地图；
步骤七：引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS位置约束的残差项；
在步骤七中，引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS位置约束的残差方程，具体按照以下步骤实施：
GPS解算出的绝对位置信息首先将其转换为世界坐标系下，最后在世界坐标系下形成GPS绝对位置约束的残差项：
，
式中，表示GPS绝对位置残差，表示GPS在在世界坐标系的定位结果，表示GPS的信息，表示GPS系下的定位结果，为GPS系转换为世界坐标系的矩阵变 换参数。
步骤八：采用滑动窗口边缘化的方式限制优化变量的数量，通过边缘化的方式，增加边缘化约束，构建滑动窗口边缘化约束方程；
在步骤八中，采用滑动窗口边缘化的方式限制优化变量的数量，通过边缘化的方式，增加边缘化约束，构建滑动窗口边缘化约束方程，滑动窗口边缘化约束方程为：
，
为滑动窗口保留的变量信息。
步骤九：采用图优化的方式对构建的残差项与约束方程进行联合优化，通过建立非线性最小二乘目标函数式优化关键帧与子地图的位姿；
在步骤九中：采用图优化的方式对构建的残差项与约束方程进行联合优化，通过建立非线性最小二乘目标函数式优化关键帧与子地图的位姿，具体按照以下步骤实施：
首先构建上述传感器的约束方程，将步骤二构建的IMU/ODO预积分残差项构建 IMU/ODO预积分约束方程，IMU/ODO预积分约束方程如下所示：
，
其中，为关键帧数，为当前关键帧数，表示滑动窗口内待估计的参数， 为IMU/ODO的预积分协方差矩阵，为IMU/ODO预积分残差项。
将步骤七中构建的GPS绝对位置的残差方程构建GPS绝对位置约束方程，GPS绝 对位置约束方程如下所示：
，
表示GPS系下的定位结果，为GPS系转换为世界坐标系的矩阵变换参数，为GPS解算位姿的协方差矩阵，表示当前GPS的位姿信息。
通过激光雷达当前帧与关键帧构成的子地图进行匹配，构建激光雷达匹配残差 项：，并通过激光雷达匹配残差项构建雷达相对位姿约束方程，雷达相对位姿 约束方程为：
，
与为当前雷达位姿信息和相对位姿信息， 为雷达相对位姿约束 协方差矩阵。
建立非线性最小二乘目标函数式优化关键帧与子地图的位姿：
，
其中，
，
，
，
，
约束条件包括IMU/ODO预积分约束，GPS绝对位置约束，雷达相对位姿约束，滑动窗口边缘化约束。
步骤十：将全局描述符LiDAR Iris用于闭环检测，修正累积误差，提高优化结果的精度，获得最终的状态估计及全局地图。
本发明方法对实验结果精度分析上，采用将本发明所提方法的轨迹估计值与其他算法的轨迹值进行对比。当系统运行所得轨迹误差与地面真实值之间的误差足够小时，即达到了本系统的高精度指标。误差指标描述主要为：绝对轨迹误差（APE）.
本发明以KITTI公开数据集的00、02、05、07序列为例，对本发明所提方法进行精度分析讨论，如图2所示，其中图中的箱线图表示本发明方法的估计值与其他主流算法的估计值的绝对轨迹误差（APE），图中纵轴为APE值，横轴为本发明所提出方法与其他主流算法，LIOG为本发明所提方法，图2（a）为KITTI公开数据集在00序列下本发明所提方法（LIOG）与其他主流算法的绝对轨迹误差值，图2（b）为KITTI公开数据集在02序列下本发明所提方法（LIOG）与其他主流算法的绝对轨迹误差值，图2（c）为KITTI公开数据集在05序列下本发明所提方法（LIOG）与其他主流算法的绝对轨迹误差值，图2（d）为KITTI公开数据集在07序列下本发明所提方法（LIOG）与其他主流算法的绝对轨迹误差值。本发明所提方法的绝对轨迹误差值均小于其他主流算法，具有更高的精度。
空间坐标轴上的移动分量图如图3所示，图3（a）为KITTI数据集00序列的位姿估计值关于平移向量在x、y、z三个轴上的拟合曲线，虚线代表真实值，与真实值高度拟合的为本发明所提方法（LIOG），图3（b）为KITTI数据集07序列的位姿估计值关于平移向量在x、y、z三个轴上的拟合曲线，虚线代表真实值，与真实值高度拟合的为本发明所提方法（LIOG）。本发明所提方法不仅在数量级上优于其他主流算法，而且在平移分量数值波动上也更趋于稳定，因此从位移分量图中体现出本发明的高精度性与鲁棒性。
图4为本发明的位姿轨迹图，本发明所绘轨迹为KITTI公开数据集的07序列，07序列轨迹全长694.7m，最大轨迹误差为0.908m，本发明所提方法在建图上也体现出高精度性。
图5（a）为本发明在KITTI公开数据集的00序列下所绘制的地图，图5（b）为本发明在KITTI公开数据集的00序列下所绘制的地图，图5（c）为本发明在KITTI公开数据集的00序列下所绘制的地图，图5（d）为本发明在KITTI公开数据集的00序列下所绘制的地图。
需要补充的是，上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (2)

1.一种基于图优化的多传感器融合定位与建图方法，其特征在于，包括以下步骤：
步骤1：采集目标区域的激光雷达、IMU、ODO，GPS的数据；
步骤2：将IMU与ODO采集的数据进行联合预积分处理，构建IMU/ODO联合预积分模型；具体按照以下步骤实施：
机体坐标系为b，ODO坐标系为o，世界坐标系为w，IMU/ODO联合状态传递方程如下所示：
其中，
式中，和/>分别为tk和tk+1时刻机体在世界坐标系w下的位置向量，/>和/>分别为tk和tk+1时刻机体在世界坐标系w下的速度向量，/>和/>为tk和tk+1时刻机体系b相对于世界坐标系下w的姿态；Δtk为时间间隔，/>为t时刻从机体系b到世界坐标系w的旋转矩阵，/>为t时刻从ODO坐标系o到机体系b的旋转矩阵，gw为世界坐标系w下的重力向量；/>和分别为t时刻加速度及与陀螺输出，/>和/>为t时刻加速度计与陀螺的零偏，na和nω分别为加速度计和陀螺仪的噪声；/>和/>分别为tk和tk+1时刻ODO坐标系o到机体系b的旋转矩阵，/>为t时刻轮速里程计的标度因数，ns为轮速里程计的噪声，/>为t时刻下的速度；Ω(ω)将陀螺仪的数值转换为4×4的矩阵形式，ω为陀螺仪的读数；
将上式中的状态传播模型投影到tk时刻的机体系bk下，得：
其中，
式中，为tk时刻从世界坐标系w到机体系bk的旋转矩阵，/>和/>为tk与tk+1时刻机体在世界坐标系w下的位置向量，/>为tk+1时刻的机体相对于机体系bk的预积分位置变化量，/>与/>为tk与tk+1时刻机体在世界坐标系w下的速度向量，/>为tk+1时刻的机体相对于bk系的预积分速度变化量，/>和/>为tk和tk+1时刻世界坐标系w下相对于机体系bk的姿态，/>为tk+1时刻的机体相对于机体系bk的预积分旋转变化量，/>为tk+1时刻的机体相对于bk系的预积分位置变化量，/>为t时刻从世界坐标系w到机体系bk的旋转矩阵，/>为t时刻从ODO坐标系o到机体系b的旋转矩阵；
通过联合预积分模型构建IMU/ODO预积分残差项为：
步骤3：利用IMU与ODO计算位姿变化得到的初始位姿信息对点云进行畸变校正，对点云数据进行预处理；
步骤4：采用NDT对预处理后的点云数据进行配准，构建激光里程计；
步骤5：将构建的IMU/ODO联合预积分模型与激光里程计结合，构建激光惯性里程计，并将上述配准后的点云数据经过激光惯性里程计进行状态估计，得到位姿信息；
步骤6：对上述位姿信息进行关键帧处理，并将点云当前帧与关键帧相结合构建子地图；
步骤7：引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS绝对位置约束的残差项；具体按照以下步骤实施：
GPS解算出的绝对位置信息首先将其转换为世界坐标系w下，最后在世界坐标系w下形成GPS绝对位置约束的残差项：
式中，表示GPS绝对位置残差，t表示GPS在世界坐标系w的定位结果，g表示GPS的信息，/>表示GPS系下的定位结果，/>为GPS系转换为世界坐标系w的矩阵变换参数；
步骤8：采用滑动窗口边缘化的方式限制优化变量的数量，通过边缘化的方式，增加边缘化约束，构建滑动窗口边缘化约束方程；
步骤9：采用图优化的方式对激光雷达、IMU、ODO、GPS构建的残差项与约束方程进行联合优化，通过建立非线性最小二乘目标函数式优化关键帧与子地图的位姿；具体按照以下步骤实施：
首先构建激光雷达、IMU、ODO、GPS的约束方程，根据步骤2构建的IMU/ODO联合预积分模型构建IMU/ODO预积分约束方程fi/o，IMU/ODO预积分约束方程如下所示：
其中，M为关键帧数，k为当前关键帧数，χ表示滑动窗口内待估计的参数，Dz为IMU/ODO的预积分协方差矩阵，为IMU/ODO预积分残差项；
根据步骤7中构建的GPS绝对位置约束的残差项构建GPS绝对位置约束方程fg，GPS绝对位置约束方程如下所示：
表示GPS系下的定位结果，/>为GPS系转换为世界坐标系w的矩阵变换参数，Dg为GPS解算位姿的协方差矩阵，i∈G表示当前GPS的位姿信息；
通过激光雷达当前帧与关键帧构成的子地图进行匹配，构建激光雷达匹配残差项：并通过激光雷达匹配残差项构建雷达相对位姿约束方程fl，雷达相对位姿约束方程为：
i∈L与j∈S为当前雷达位姿信息和相对位姿信息，Di,j为雷达相对位姿约束协方差矩阵；
建立非线性最小二乘目标函数式优化关键帧与子地图的位姿：
其中，
约束条件包括IMU/ODO预积分约束fi/o，GPS绝对位置约束fg，雷达相对位姿约束fl，滑动窗口边缘化约束fmarg；
步骤10：将全局描述符LiDAR Iris用于闭环检测，修正累积误差，提高优化结果的精度，获得最终的状态估计及全局地图。
2.根据权利要求1所述的一种基于图优化的多传感器融合定位与建图方法，其特征在于，根据步骤8中所述的采用滑动窗口边缘化的方式限制优化变量的数量，通过边缘化的方式，增加边缘化约束，构建滑动窗口边缘化约束方程，滑动窗口边缘化约束方程为：
Δχb为滑动窗口保留的变量信息。
CN202311068502.3A 2023-08-24 2023-08-24 一种基于图优化的多传感器融合定位与建图方法 Active CN116772828B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311068502.3A CN116772828B (zh) 2023-08-24 2023-08-24 一种基于图优化的多传感器融合定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311068502.3A CN116772828B (zh) 2023-08-24 2023-08-24 一种基于图优化的多传感器融合定位与建图方法

Publications (2)

Publication Number Publication Date
CN116772828A CN116772828A (zh) 2023-09-19
CN116772828B true CN116772828B (zh) 2023-12-19

Family

ID=87986395

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311068502.3A Active CN116772828B (zh) 2023-08-24 2023-08-24 一种基于图优化的多传感器融合定位与建图方法

Country Status (1)

Country Link
CN (1) CN116772828B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106272423A (zh) * 2016-08-31 2017-01-04 哈尔滨工业大学深圳研究生院 一种针对大尺度环境的多机器人协同制图与定位的方法
CN106323273A (zh) * 2016-08-26 2017-01-11 深圳微服机器人科技有限公司 一种机器人重定位方法及装置
CN107702722A (zh) * 2017-11-07 2018-02-16 云南昆船智能装备有限公司 一种激光导引agv自然导航定位方法
CN114136311A (zh) * 2021-11-08 2022-03-04 上海应用技术大学 一种基于imu预积分的激光slam定位方法
CN114812601A (zh) * 2021-01-29 2022-07-29 阿里巴巴集团控股有限公司 视觉惯性里程计的状态估计方法、装置、电子设备
CN115479598A (zh) * 2022-08-23 2022-12-16 长春工业大学 基于多传感器融合的定位与建图方法及紧耦合系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7541974B2 (en) * 2005-12-15 2009-06-02 Trimble Navigation Limited Managed traverse system and method to acquire accurate survey data in absence of precise GPS data

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106323273A (zh) * 2016-08-26 2017-01-11 深圳微服机器人科技有限公司 一种机器人重定位方法及装置
CN106272423A (zh) * 2016-08-31 2017-01-04 哈尔滨工业大学深圳研究生院 一种针对大尺度环境的多机器人协同制图与定位的方法
CN107702722A (zh) * 2017-11-07 2018-02-16 云南昆船智能装备有限公司 一种激光导引agv自然导航定位方法
CN114812601A (zh) * 2021-01-29 2022-07-29 阿里巴巴集团控股有限公司 视觉惯性里程计的状态估计方法、装置、电子设备
CN114136311A (zh) * 2021-11-08 2022-03-04 上海应用技术大学 一种基于imu预积分的激光slam定位方法
CN115479598A (zh) * 2022-08-23 2022-12-16 长春工业大学 基于多传感器融合的定位与建图方法及紧耦合系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
物流分拣机器人设计;王昱琪，等;科技风;1-3 *

Also Published As

Publication number Publication date
CN116772828A (zh) 2023-09-19

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN109520497A (zh) 基于视觉和imu的无人机自主定位方法
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN111288989B (zh) 一种小型无人机视觉定位方法
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
Chen et al. Rnin-vio: Robust neural inertial navigation aided visual-inertial odometry in challenging scenes
CN102155955A (zh) 立体视觉里程计及测量方法
CN108426582B (zh) 行人室内三维地图匹配方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN116429116A (zh) 一种机器人定位方法及设备
Jiang et al. 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments
CN113358117A (zh) 一种利用地图的视觉惯性室内定位方法
CN116772828B (zh) 一种基于图优化的多传感器融合定位与建图方法
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
CN113029138B (zh) 一种基于多传感器数据融合的小车实时姿态检测方法
CN116124161A (zh) 一种基于先验地图的LiDAR/IMU融合定位方法
CN115046543A (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