WO2025112232A1 - 一种使用分段线性连续时间轨迹的激光雷达里程计方法 - Google Patents

一种使用分段线性连续时间轨迹的激光雷达里程计方法 Download PDF

Info

Publication number
WO2025112232A1
WO2025112232A1 PCT/CN2024/081903 CN2024081903W WO2025112232A1 WO 2025112232 A1 WO2025112232 A1 WO 2025112232A1 CN 2024081903 W CN2024081903 W CN 2024081903W WO 2025112232 A1 WO2025112232 A1 WO 2025112232A1
Authority
WO
WIPO (PCT)
Prior art keywords
point
time
trajectory
current
point cloud
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
PCT/CN2024/081903
Other languages
English (en)
French (fr)
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.)
Zhejiang University
Original Assignee
Zhejiang 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 Zhejiang University filed Critical Zhejiang University
Priority to US18/916,786 priority Critical patent/US20250180745A1/en
Publication of WO2025112232A1 publication Critical patent/WO2025112232A1/zh
Anticipated expiration legal-status Critical
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; 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/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/207—Analysis of motion for motion estimation over a hierarchy of resolutions
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; 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/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; 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 OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • 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

Definitions

  • the present invention relates to simultaneous positioning and mapping related technologies in the field of robotics, and more particularly to an odometer method that relies solely on input information from a laser radar and uses a piecewise linear continuous-time trajectory.
  • LiDAR odometry is a critical component in many robotic applications. Due to LiDAR’s excellent distance sensing capabilities, LiDAR odometry plays a key role in robot navigation and path planning in GPS-restricted environments.
  • LIO LiDAR-inertial Odometry
  • IMU inertial measurement unit
  • LiDAR is a streaming sensor that is accompanied by continuous motion and is used to capture scene structures.
  • Past research on LiDAR odometry has represented motion trajectories as a series of discrete time postures, breaking the inherent connection between the collected geometric information and sensor motion. Therefore, the accuracy of the current odometry method that only uses LiDAR information input is much lower than that of the multi-sensor fusion odometry method.
  • the present invention provides a novel continuous-time trajectory parameterization method for motion estimation of a laser radar odometer.
  • the present invention is applicable to various types of laser radars, including multi-line scanning and non-repetitive scanning radars, and different configurations of single or multiple radar systems.
  • S5 Repeat S3 and S4, perform iterative optimization of the pose in the current time window according to the updated continuous time trajectory, until the objective function is less than the preset threshold, obtain the optimal continuous time trajectory of the current time window and the corresponding lidar point cloud in the world coordinate system; and add the lidar point cloud in the world coordinate system of the current time window to the current local point cloud map;
  • S6 Repeat S2-S5, optimize the lidar pose for the lidar point cloud subsequences in each time window in turn, and obtain the total continuous time trajectory.
  • the step of calculating the point-to-plane error corresponding to each point is specifically as follows:
  • the current continuous-time trajectory is used to obtain the position Ti of the laser radar in the world coordinate system at the time corresponding to the current point p i .
  • Several points closest to the current point are found in the current local point cloud map and the nearest point among the several points is recorded as q i .
  • the principal component analysis method is used to calculate the normal vectors ni corresponding to the several points.
  • the point-to-plane error corresponding to each point is calculated using the following formula:
  • e i represents the point-to-plane error corresponding to the current point pi
  • T represents transpose
  • T k-1 and T k are the starting and ending poses of the current window k, respectively; and They are respectively the starting and ending poses after optimization within the previous time window k-1.
  • the objective function formula is as follows:
  • E represents the total trajectory error
  • e i represents the point-to-plane error corresponding to the current point p i in the current time window
  • T represents the transpose
  • the computer device comprises a memory and a processor, wherein the memory stores a computer program, and the steps of the method are implemented when the processor executes the computer program.
  • a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the steps of the method described are implemented.
  • the present invention When completing the point cloud registration part in the lidar odometer, the present invention additionally considers the timestamp information of each point, thereby utilizing the continuous time trajectory representation method proposed above, eliminating the additional point cloud dedistortion step and reducing dependence on other sensors.
  • the present invention introduces motion constraints according to the smoothness of the continuous trajectory, which can achieve stable registration when the system dynamics state exceeds the general IMU measurement range.
  • the continuous time representation method adopted by the present invention provides a unified odometer framework, which can be applicable to non-scanning type lidar sensors and can also simultaneously fuse multiple asynchronous lidar sensors.
  • FIG1 is an overall flow chart of a laser radar odometer method using a piecewise linear continuous-time trajectory according to an embodiment of the present invention.
  • FIG. 2 is a framework of a laser radar odometer method for continuous-time trajectory according to an embodiment of the present invention.
  • the present invention comprises the following steps:
  • the initial laser radar point cloud time series is represented as T(t), where t is the time variable, and the trajectory in the time range [t 0 ,t K ) is of interest.
  • Tt a laser radar posture
  • All postures are represented in the world coordinate system, where the world coordinate system is the starting position of the laser radar.
  • the posture is specifically a translation and rotation of a three-dimensional rigid body transformation.
  • the present invention divides the time window into K small segments with equal time intervals.
  • the initial laser mine The point cloud time series is divided into K point cloud subsets, each of which contains the lidar point cloud data collected during this time interval.
  • the point cloud data obtained in the first 3 seconds is directly used to build the initial local point cloud map.
  • the map structure uses a voxel hash table, and each point information is stored in the corresponding voxel according to the hash value of the 3D coordinates.
  • the current continuous time trajectory is used to obtain the position T i of the laser radar in the world coordinate system at the current point p i at the corresponding time.
  • the position T i is obtained by linear interpolation based on the posture T k-1 at the start time and the posture T k at the end time corresponding to the current time window.
  • the calculation formula is as follows:
  • Log and Exp are the logarithmic mapping and exponential mapping of the three-dimensional space rigid transformation respectively, ⁇ i represents the time scale coefficient, and ⁇ represents the tangent space vector corresponding to the posture transformation.
  • e i represents the point-to-plane error corresponding to the current point pi
  • T represents transpose
  • This error is used to measure the geometric relationship between the laser radar measurement data and the map, and is therefore used for motion estimation.
  • the point-surface error constraint takes into account the continuity of the continuous-time trajectory, so that the present invention can estimate the motion of the laser radar more accurately.
  • T k-1 and T k are the pose at the start time and the pose at the end time of the current window k, respectively, and will be updated during the optimization process; and They are the starting and ending poses after optimization in the previous time window k-1, and remain unchanged in the optimization of the current window k.
  • the objective function formula is as follows:
  • E represents the total trajectory error
  • e i represents the point-to-plane error corresponding to the current point p i in the current time window
  • T represents the transpose
  • the above objective function is minimized by using the iterative Gauss-Newton method to obtain the final pose estimation result.
  • the present invention lists the explicit Jacobian matrix required in the optimization process to improve the optimization efficiency.
  • the Jacobian ratio matrix corresponding to the plane error e i is:
  • Jr and Jl are the right Jacobi matrix and the left Jacobi matrix in three-dimensional space respectively, and Ri represents the rotation matrix.
  • S5 Repeat S3 and S4, perform iterative optimization of the pose in the current time window according to the updated continuous time trajectory, until the objective function is less than the preset threshold, obtain the optimal continuous time trajectory of the current time window and the corresponding lidar point cloud in the world coordinate system; and add the lidar point cloud in the world coordinate system of the current time window to the current local point cloud map;
  • S6 Repeat S2-S5 to perform laser radar on the subsequences of the laser radar point cloud in each time window.
  • the pose is optimized to obtain the total continuous time trajectory, that is, the pose within time tK .
  • the method is applicable to the fields of robot navigation, simultaneous localization and mapping, and autonomous driving.
  • the present invention has the function of high-precision laser radar motion estimation for robots, is applicable to various types of laser radars and motion situations, and has high robustness and real-time performance.
  • the present invention can be widely used in the fields of robot navigation, simultaneous localization and mapping, and autonomous driving.

Landscapes

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

Abstract

本发明公开了一种使用分段线性连续时间轨迹的激光雷达里程计方法。本发明包括以下步骤：首先，利用时间窗口将初始激光雷达点云时间序列进行划分后，获得多个时间窗口下的激光雷达点云子序列，以及构建初始局部点云地图；接着，基于初始局部点云地图，对各时间窗口下的激光雷达点云子序列进行激光雷达位姿的优化，获得总的连续时间轨迹以及最终的局部点云地图。本发明实现了激光雷达的高精度运动估计，具备高鲁棒性和实时性，适用于机器人导航、同时定位与建图和自动驾驶等领域，提供了一个相对简单且高效的解决方案，能够有效地处理激光雷达作为流式传感器的特性。

Description

一种使用分段线性连续时间轨迹的激光雷达里程计方法 技术领域
本发明涉及机器人领域的同时定位与建图相关技术，具体关于仅依赖激光雷达的输入信息，使用分段线性连续时间轨迹的里程计方法。
背景技术
在许多机器人应用中，激光雷达里程计是一个至关重要的组件。由于激光雷达出色的距离感测能力，激光雷达里程计在GPS受限环境中的机器人导航和路径规划中起到了关键作用。
经过近些年的探索和发展，仅依靠单一激光雷达传感器的方法缺乏精确性和鲁棒性。多传感器融合的方法被广泛认为是实际应用中的最佳解决方案。尤其是紧密耦合结合了来自惯性测量单元(IMU)的信息的激光雷达-惯性里程计(LiDAR-inertial Odometry，简称LIO)，在实践中有着大量应用。与现有仅依赖于激光雷达信息的方法相比，LIO方法在两个方面提供了优势：一是高频率的IMU状态有助于更准确的运动补偿；另一个优势是IMU通过直接施加运动约束来增强系统的鲁棒性。然而，稳定的LIO在很大程度上依赖于IMU参数的准确校准，而且IMU数据对温度变化和机械冲击非常敏感。此外，即使是目前最先进的LIO系统也难以处理动力学状态超出IMU测量范围的情况。
事实上，激光雷达是一个伴随着连续运动的流式传感器，用于捕获场景结构。过去关于激光雷达里程计的研究将运动轨迹表示为一系列的离散时间姿态，断开了采集到的几何信息和传感器运动之间的固有联系。因此当前仅使用激光雷达信息输入的里程计方法的精度远低于多传感器融合里程计方法。
发明内容
为了解决背景技术中存在的问题，本发明提供了一种新颖的连续时间轨迹参数化方法，用于激光雷达里程计的运动估计。本发明适用于各种类型的激光雷达，包括多线扫描和非重复扫描雷达，以及单个或多个雷达系统的不同配置。
本发明采用的技术方案包括如下步骤：
一、一种使用分段线性连续时间轨迹的激光雷达里程计方法
S1：利用时间窗口将初始激光雷达点云时间序列进行划分后，获得多个时间窗口下的激光雷达点云子序列，以及构建初始局部点云地图；
S2：对每个时间窗口下的激光雷达点云子序列进行线性参数化后，获得初始连续时间轨迹；
S3：根据当前连续时间轨迹和当前局部点云地图，计算当前时间窗口中的各点对应的点到平面误差并组成点面误差约束，并建立轨迹平滑性约束；
S4：基于点面误差约束和轨迹平滑性约束，使用高斯-牛顿法进行最小化求解目标函数后，估计获得当前时间窗口内的连续时间轨迹并更新；
S5：重复S3和S4，根据更新的连续时间轨迹进行当前时间窗口内的位姿迭代优化，直至目标函数小于预设阈值，获得当前时间窗口的最优连续时间轨迹以及对应的在世界坐标系下的激光雷达点云；并且将当前时间窗口的在世界坐标系下的激光雷达点云加入当前局部点云地图中；
S6：重复S2-S5，依次对各个时间窗口下的激光雷达点云子序列进行激光雷达位姿的优化，获得总的连续时间轨迹。
所述S3中，对于当前时间窗口中的每个点，每个点对应的点到平面误差的计算步骤具体为：
首先，利用当前连续时间轨迹获取到当前点pi对应时刻的激光雷达在世界坐标系下的位置Ti，在当前局部点云地图中寻找与当前点最近的若干点并将若干点中最近的点记为qi，利用主成分分析方法计算若干点对应的法向量ni，最后利用以下公式计算获得每个点对应的点到平面误差：
其中，ei表示当前点pi对应的点到平面误差，T表示转置。
所述S3中，轨迹平滑性约束的公式如下：
其中，表示时间窗口k内的轨迹平滑性约束，表示时间窗口k内的广义速度，表示时间窗口k的伪速度测量；Tk-1和Tk分别为当前窗口k的开始时刻位姿和结束时刻位姿；和分别为上一个时间窗口k-1内完成优化后的开始时刻位姿和结束时刻位姿。
所述目标函数的公式如下：
其中，E表示轨迹总误差，ei表示当前时间窗口中当前点pi对应的点到平面误差，表示当前时间窗口k内的轨迹平滑性约束，T表示转置。
二、一种计算机设备
计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述方法的步骤。
三、一种计算机可读存储介质
计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现所述的方法的步骤。
本发明的有益效果是：
本发明提出了分段线性连续时间轨迹的运动表示方法。整段时间区间内的运动可以根据运动情况，划分为多段时间区间，每段时间区间内的运动可以使用简单的线性插值表示。这种分段的轨迹表示方式解决了线性插值无法表示复杂运动的问题，从而为后续里程计的估计提供了简洁和高效的数学表示。
本发明在完成激光雷达里程计中点云注册部分时，额外考虑了每个点的时间戳信息，从而利用前面提出的连续时间轨迹表示方式，省去了额外点云去畸变的步骤，减少对其他传感器的依赖。
本发明在里程计优化部分，根据连续轨迹的平滑性引入了运动约束，可以在系统动力学状态超出一般IMU测量范围的情况下，实现稳定的注册。
本发明采用的连续时间表示方式提供了一种统一的里程计框架，可以适用于不用扫描类型的激光雷达传感器，也可以同时融合多个异步的激光雷达传感器。
附图说明
图1是本发明实施例的一种使用分段线性连续时间轨迹的激光雷达里程计方法的总体流程图。
图2是本发明实施例的连续时间轨迹的激光雷达里程计方法框架。
具体实施方式
下面将结合本发明中的附图，对本发明的技术方案进行清晰、完整的描述。基于本发明中的实施例，本领域普通技术人员在没有做创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的目的、技术方案及优点更加清楚明白，接下来将参照附图对本发明实施例进行详细的说明。
如图1和图2所示，本发明包括以下步骤：
S1：利用时间窗口将初始激光雷达点云时间序列进行划分后，获得多个时间窗口下的激光雷达点云子序列，以及构建初始局部点云地图；
具体地：将初始激光雷达点云时间序列表示为T(t)，其中t是时间变量，关注时间范围为[t0,tK)内的轨迹。每个时间点对应一个激光雷达的位姿，用Tt表示。所有的位姿都是世界坐标系下的表示，其中世界坐标系是激光雷达的起始位置。位姿具体为三维刚体变换地平移和旋转。为了处理快速运动，本发明将时间窗口分成了K个等时间间隔距的小段从而将初始激光雷 达点云时间序列划分成K个点云子集，每个点云子集中包含这段时间区间内采集到地激光雷达点云数据。在开始阶段，直接利用前3秒获得的点云数据构建初始局部点云地图。地图结构采用体素哈希表，每个点信息根据三维坐标的哈希值存储在对应的体素中。
S2：根据其初始位姿Tk-1和结束位姿Tk对每个时间窗口下的激光雷达点云子序列进行线性参数化后，获得初始连续时间轨迹；在每个分段[tk-1,kk)中，运动由初始位姿Tk-1和结束位姿Tk线性参数化轨迹，每个点可以根据时间戳信息线性插值出自身在世界坐标系下地位置。
S3：根据当前连续时间轨迹和当前局部点云地图，计算当前时间窗口k中的各点对应的点到平面误差并组成点面误差约束，并建立轨迹平滑性约束；
S3中，对于当前时间窗口k中的每个点，每个点对应的点到平面误差的计算步骤具体为：
首先，假设第k个时间窗口[tk-1，tk)中有一个点pi，对应采集时刻的时间戳为ti。利用当前连续时间轨迹获取到当前点pi对应时刻的激光雷达在世界坐标系下的位置Ti，位置Ti根据当前时间窗口对应的开始时刻位姿Tk-1和结束时刻位姿Tk线性插值获得，计算公式如下：
其中，Log和Exp分别为三维空间刚性变换的对数映射和指数映射，αi表示时间比例系数，τ表示位姿变换对应的切空间向量。
然后根据位置Ti，在当前局部点云地图中寻找与当前点最近的若干点，并将若干点中最近的点记为qi，利用主成分分析方法计算若干点对应的法向量ni，最后利用以下公式计算获得每个点对应的点到平面误差：
其中，ei表示当前点pi对应的点到平面误差，T表示转置。
这个误差用于衡量激光雷达测量数据与地图之间的几何关系，从而用于运动估计。点面误差约束考虑了连续时间轨迹的连续性，使得本发明可以更准确地估计激光雷达的运动。
S3中，轨迹平滑性约束的公式如下：
其中，表示时间窗口k内的轨迹平滑性约束，表示时间窗口k内的广义速度，表示时间窗口k的伪速度测量，由上一段的位 姿估计值组成，用于衡量相邻分段内的轨迹变化；Tk-1和Tk分别为当前窗口k的开始时刻位姿和结束时刻位姿，在优化过程中会进行更新；和分别为上一个时间窗口k-1内完成优化后的开始时刻位姿和结束时刻位姿，在当前窗口k的优化中保持不变。
S4：基于点面误差约束和轨迹平滑性约束，使用高斯-牛顿法进行最小化求解目标函数后，估计获得当前时间窗口k内的连续时间轨迹并更新；
目标函数的公式如下：
其中，E表示轨迹总误差，ei表示当前时间窗口中当前点pi对应的点到平面误差，表示当前时间窗口k内的轨迹平滑性约束，T表示转置。
利用迭代高斯牛顿方法最小化上述目标函数，获得最终的位姿估计结果。本发明列出了优化过程中需要的显形式雅阁比矩阵，用于提升优化效率。
其中，平面误差ei对应的雅阁比矩阵为：
其中，Jr和Jl分别为三维空间下的右雅阁比矩阵和左雅阁比矩阵，Ri表示旋转矩阵。
平滑约束的雅阁比矩阵为：
S5：重复S3和S4，根据更新的连续时间轨迹进行当前时间窗口内的位姿迭代优化，直至目标函数小于预设阈值，获得当前时间窗口的最优连续时间轨迹以及对应的在世界坐标系下的激光雷达点云；并且将当前时间窗口的在世界坐标系下的激光雷达点云加入当前局部点云地图中；
S6：重复S2-S5，依次对各个时间窗口下的激光雷达点云子序列进行激光雷 达位姿的优化，获得总的连续时间轨迹，即时间tK内的位姿。
该方法适用于机器人导航、同时定位与建图和自动驾驶等领域。本发明具备对机器人进行高精度的激光雷达运动估计的功能，适用于各种类型的激光雷达和运动情况，并具有较高的鲁棒性和实时性。本发明可在机器人导航、同时定位与建图和自动驾驶等领域得到广泛应用。
最后所应说明的是，以上实施例和阐述仅用以说明本发明的技术方案而非进行限制。本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，不脱离本发明技术方案公开的精神和范围的，其均应涵盖在本发明的权利要求保护范围之中。

Claims (6)

  1. 一种使用分段线性连续时间轨迹的激光雷达里程计方法，其特征在于，包括以下步骤：
    S1：利用时间窗口将初始激光雷达点云时间序列进行划分后，获得多个时间窗口下的激光雷达点云子序列，以及构建初始局部点云地图；
    S2：对每个时间窗口下的激光雷达点云子序列进行线性参数化后，获得初始连续时间轨迹；
    S3：根据当前连续时间轨迹和当前局部点云地图，计算当前时间窗口中的各点对应的点到平面误差并组成点面误差约束，并建立轨迹平滑性约束；
    S4：基于点面误差约束和轨迹平滑性约束，使用高斯-牛顿法进行最小化求解目标函数后，估计获得当前时间窗口内的连续时间轨迹并更新；
    S5：重复S3和S4，根据更新的连续时间轨迹进行当前时间窗口内的位姿迭代优化，直至目标函数小于预设阈值，获得当前时间窗口的最优连续时间轨迹以及对应的在世界坐标系下的激光雷达点云；并且将当前时间窗口的在世界坐标系下的激光雷达点云加入当前局部点云地图中；
    S6：重复S2-S5，依次对各个时间窗口下的激光雷达点云子序列进行激光雷达位姿的优化，获得总的连续时间轨迹。
  2. 根据权利要求1所述的一种使用分段线性连续时间轨迹的激光雷达里程计方法，其特征在于，所述S3中，对于当前时间窗口中的每个点，每个点对应的点到平面误差的计算步骤具体为：
    首先，利用当前连续时间轨迹获取到当前点pi对应时刻的激光雷达在世界坐标系下的位置Ti，在当前局部点云地图中寻找与当前点最近的若干点并将若干点中最近的点记为qi，利用主成分分析方法计算若干点对应的法向量ni，最后利用以下公式计算获得每个点对应的点到平面误差：
    其中，ei表示当前点pi对应的点到平面误差，T表示转置。
  3. 根据权利要求1所述的一种使用分段线性连续时间轨迹的激光雷达里程计方法，其特征在于，所述S3中，轨迹平滑性约束的公式如下：
    其中，表示时间窗口k内的轨迹平滑性约束，表示时间窗口k内的广义速度，表示时间窗口k的伪速度测量；Tk-1和Tk分 别为当前窗口k的开始时刻位姿和结束时刻位姿；和分别为上一个时间窗口k-1内完成优化后的开始时刻位姿和结束时刻位姿。
  4. 根据权利要求1所述的一种使用分段线性连续时间轨迹的激光雷达里程计方法，其特征在于，所述目标函数的公式如下：
    其中，E表示轨迹总误差，ei表示当前时间窗口中当前点pi对应的点到平面误差，表示当前时间窗口k内的轨迹平滑性约束，T表示转置。
  5. 一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至4中任一项所述方法的步骤。
  6. 一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至4中任一项所述的方法的步骤。
PCT/CN2024/081903 2023-11-30 2024-03-15 一种使用分段线性连续时间轨迹的激光雷达里程计方法 Pending WO2025112232A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US18/916,786 US20250180745A1 (en) 2023-11-30 2024-10-16 Lidar odometry using piecewise linear continuous-time trajectory

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202311627956.XA CN117665748A (zh) 2023-11-30 2023-11-30 一种使用分段线性连续时间轨迹的激光雷达里程计方法
CN202311627956.X 2023-11-30

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/916,786 Continuation-In-Part US20250180745A1 (en) 2023-11-30 2024-10-16 Lidar odometry using piecewise linear continuous-time trajectory

Publications (1)

Publication Number Publication Date
WO2025112232A1 true WO2025112232A1 (zh) 2025-06-05

Family

ID=90070756

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2024/081903 Pending WO2025112232A1 (zh) 2023-11-30 2024-03-15 一种使用分段线性连续时间轨迹的激光雷达里程计方法

Country Status (3)

Country Link
US (1) US20250180745A1 (zh)
CN (1) CN117665748A (zh)
WO (1) WO2025112232A1 (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN120403609A (zh) * 2025-06-19 2025-08-01 湖南大学 一种用于提高激光惯性里程计性能的分布自适应面元地图实现方法
CN121366510A (zh) * 2025-12-23 2026-01-20 飞友科技有限公司 一种航空器航迹数据的平滑处理方法及系统
CN121582889A (zh) * 2026-01-26 2026-02-27 山东大学 一种融合轨迹多特征回归的道路边界线重构方法及系统
CN121838102A (zh) * 2026-03-12 2026-04-10 华中科技大学 面向长时序点云场景流估计的方法及系统

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117665748A (zh) * 2023-11-30 2024-03-08 浙江大学 一种使用分段线性连续时间轨迹的激光雷达里程计方法
CN120299007B (zh) * 2025-06-12 2025-08-15 成都航空职业技术学院 一种智能汽车运行风险场预测方法及装置
CN121074096B (zh) * 2025-08-12 2026-07-07 华中科技大学 一种基于主从架构的多模态语义建图方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102020003662A1 (de) * 2020-06-19 2020-08-06 Daimler Ag Verfahren zur Bestimmung einer Eigenbewegung mittels Lidar-Odometrie und Fahrzeug
CN115407358A (zh) * 2022-08-08 2022-11-29 杭州极线科技有限公司 使用连续时间滤波配准的高效固态激光雷达里程计方法
WO2023016392A1 (en) * 2021-08-09 2023-02-16 Zhejiang Huaray Technology Co., Ltd. Systems and methods for pose determination of a mobile subject
CN117665748A (zh) * 2023-11-30 2024-03-08 浙江大学 一种使用分段线性连续时间轨迹的激光雷达里程计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102020003662A1 (de) * 2020-06-19 2020-08-06 Daimler Ag Verfahren zur Bestimmung einer Eigenbewegung mittels Lidar-Odometrie und Fahrzeug
WO2023016392A1 (en) * 2021-08-09 2023-02-16 Zhejiang Huaray Technology Co., Ltd. Systems and methods for pose determination of a mobile subject
CN115407358A (zh) * 2022-08-08 2022-11-29 杭州极线科技有限公司 使用连续时间滤波配准的高效固态激光雷达里程计方法
CN117665748A (zh) * 2023-11-30 2024-03-08 浙江大学 一种使用分段线性连续时间轨迹的激光雷达里程计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
XIN ZHENG; JIANKE ZHU: "Traj-LO: In Defense of LiDAR-Only Odometry Using an Effective Continuous-Time Trajectory", ARXIV.ORG, CORNELL UNIVERSITY LIBRARY, 201 OLIN LIBRARY CORNELL UNIVERSITY ITHACA, NY 14853, 25 September 2023 (2023-09-25), 201 Olin Library Cornell University Ithaca, NY 14853, XP091625481 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN120403609A (zh) * 2025-06-19 2025-08-01 湖南大学 一种用于提高激光惯性里程计性能的分布自适应面元地图实现方法
CN121366510A (zh) * 2025-12-23 2026-01-20 飞友科技有限公司 一种航空器航迹数据的平滑处理方法及系统
CN121582889A (zh) * 2026-01-26 2026-02-27 山东大学 一种融合轨迹多特征回归的道路边界线重构方法及系统
CN121838102A (zh) * 2026-03-12 2026-04-10 华中科技大学 面向长时序点云场景流估计的方法及系统

Also Published As

Publication number Publication date
CN117665748A (zh) 2024-03-08
US20250180745A1 (en) 2025-06-05

Similar Documents

Publication Publication Date Title
US20240312061A1 (en) High-precision odometry estimation method based on double-layer filtering framework
CN113376650B (zh) 移动机器人定位方法及装置、电子设备及存储介质
US20250180745A1 (en) Lidar odometry using piecewise linear continuous-time trajectory
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN111561923B (zh) 基于多传感器融合的slam制图方法、系统
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
CN114046800B (zh) 一种基于双层滤波框架的高精度里程估计方法
CN116661469B (zh) 机器人轨迹误差修正方法及系统
CN105096341B (zh) 基于三焦张量和关键帧策略的移动机器人位姿估计方法
CN116358525A (zh) 基于激光雷达的建图与定位方法、系统及工程车辆
CN114022553B (zh) 基于异构传感器信息融合的位姿跟踪系统及方法
CN118688748A (zh) 基于高斯过程的多激光雷达-惯性测量单元状态估计方法
CN117405105A (zh) 一种基于视觉惯性融合技术的无人机自主定位方法及系统
KR102800061B1 (ko) Ndt-pso를 이용한 이동로봇의 라이다 오도메트리 및 공분산 추정 방법 및 장치
CN111896946A (zh) 一种基于航迹拟合的连续时间目标跟踪方法
Wang et al. SW-LIO: A sliding window based tightly coupled LiDAR-inertial odometry
CN113280808A (zh) 一种移动机器人定位精度提升方法及系统
CN115930943A (zh) 基于图优化和ekf框架下融合单目视觉和imu的slam方法及系统
CN112067007B (zh) 地图生成方法、计算机存储介质及电子设备
CN115752507B (zh) 基于二维码导航的在线单舵轮agv参数标定方法及系统
CN116576868A (zh) 一种多传感器融合精确定位及自主导航方法
CN120176717A (zh) 轮式底盘人形机器人定位方法、装置、计算机设备及存储介质
CN109459769B (zh) 一种自主定位方法与系统
CN119268673A (zh) 一种基于足腿类机器人运动特征约束的定位建图方法及系统
CN118092451A (zh) 一种厂房机器人目标搜索过程的位置补偿方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 24895346

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE