CN115655302B - 激光里程计的实现方法、计算机设备、存储介质及车辆 - Google Patents

激光里程计的实现方法、计算机设备、存储介质及车辆 Download PDF

Info

Publication number
CN115655302B
CN115655302B CN202211569521.XA CN202211569521A CN115655302B CN 115655302 B CN115655302 B CN 115655302B CN 202211569521 A CN202211569521 A CN 202211569521A CN 115655302 B CN115655302 B CN 115655302B
Authority
CN
China
Prior art keywords
state quantity
vehicle
motion state
point cloud
value
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
CN202211569521.XA
Other languages
English (en)
Other versions
CN115655302A (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.)
Anhui Weilai Zhijia Technology Co Ltd
Original Assignee
Anhui Weilai Zhijia Technology Co Ltd
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 Anhui Weilai Zhijia Technology Co Ltd filed Critical Anhui Weilai Zhijia Technology Co Ltd
Priority to CN202211569521.XA priority Critical patent/CN115655302B/zh
Publication of CN115655302A publication Critical patent/CN115655302A/zh
Application granted granted Critical
Publication of CN115655302B publication Critical patent/CN115655302B/zh
Priority to EP23209458.1A priority patent/EP4382954A3/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • 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/497—Means for monitoring or calibrating
    • 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/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/28—Details of pulse systems
    • G01S7/285—Receivers
    • G01S7/295—Means for transforming co-ordinates or for evaluating data, e.g. using computers
    • 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/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/28—Details of pulse systems
    • G01S7/285—Receivers
    • G01S7/295—Means for transforming co-ordinates or for evaluating data, e.g. using computers
    • G01S7/2955—Means for determining the position of the radar coordinate system for evaluating the position data of the target in another coordinate system

Landscapes

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

Abstract

本发明涉及自动驾驶技术领域，具体提供一种激光里程计的实现方法、计算机设备、存储介质及车辆，旨在解决提高建图和定位的实时性与精度的问题。为此目的，本发明提供的方法包括：根据上一周期车辆上IMU的测量数据，预测当前周期车辆的运动状态量预测值；根据当前周期车辆上激光雷达的点云数据帧，获取当前周期车辆的第一运动状态量观测值；根据当前周期车辆上轮速计的测量数据，获取当前周期车辆的第二运动状态量观测值；根据第一运动状态量观测值与第二运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。通过上述方法可以快速且准确地得到车辆的最优运动状态量，从而可以显著提高建图和定位的实时性、精度与鲁棒性。

Description

激光里程计的实现方法、计算机设备、存储介质及车辆
技术领域
本发明涉及自动驾驶技术领域，具体涉及一种激光里程计的实现方法、计算机设备、存储介质及车辆。
背景技术
基于激光雷达的建图方法主要包括前端和后端两个部分，前端部分主要是利用激光里程计进行增量式局部地图（incremental local mapping）构建，后端部分主要是进行全局地图（global mapping）构建。其中，前端侧重于建图的局部一致性和实时性，后端侧重于建图的全局一致性和高精度。但是，目前常规的建图方法无法很好地兼顾实时性和高精度，为了保证高精度通常需要较高的算力，必然会影响建图的实时性。
同时，在目前的自动驾驶车辆的相对定位方案中，多采样轮速-IMU（InertialMeasurement Unit）融合的方案，这种方案缺少对高程、横向位移以及偏航角的约束，针对里程大于10米或者时间超过1秒钟的相对定位需求，精度非常有限。其他采用激光里程计的方案也过于依赖激光雷达的点云匹配，没有充分融合多传感器信息，在一些FOV（Field OfView）偏小的激光雷达硬件上出现精度和鲁棒性退化的情况。
相应地，本领域需要一种新的技术方案来解决上述问题。
发明内容
为了克服上述缺陷，提出了本发明，以提供解决或至少部分地解决在保证建图和定位高精度的前提下如何降低建图和定位的算力需求，提高建图和定位的实时性与鲁棒性的技术问题的激光里程计的实现方法、计算机设备、存储介质及车辆。
在第一方面，提供一种激光里程计的实现方法，所述方法包括：
根据上一周期车辆上IMU的测量数据，预测当前周期所述车辆的运动状态量预测值；
根据当前周期所述车辆上激光雷达的点云数据帧，获取当前周期所述车辆的第一运动状态量观测值；
根据当前周期所述车辆上轮速计的测量数据，获取当前周期所述车辆的第二运动状态量观测值；
根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
在上述激光里程计的实现方法的一个技术方案中，“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤具体包括：
根据所述第一运动状态量观测值中的位姿观测值，对所述运动状态量预测值中的位姿预测值进行更新，并且
根据所述第二运动状态量观测值中的位移观测值，对所述位姿预测值中的位置预测值进行更新，以确定所述最优运动状态量。
在上述激光里程计的实现方法的一个技术方案中，所述方法还包括通过下列方式获取所述位移观测值：
根据当前周期所述车辆上轮速计的测量数据，获取当前周期的轮速；
根据当前周期所述车辆上IMU的测量数据与当前周期内每个轮速的时刻，分别获取在每个时刻的车辆姿态；
根据所述车辆姿态，分别获取每两个相邻时刻之间的相对姿态；
根据每两个相邻时刻之间的相对姿态，分别对每两个相邻时刻的轮速进行积分，以获取在世界坐标系下每两个相邻时刻之间的相对位移；
根据所述每两个相邻时刻之间的相对位移，获取所述位移观测值。
在上述激光里程计的实现方法的一个技术方案中，“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤还包括：
根据所述第一运动状态量观测值中的位姿观测值，对所述运动状态量预测值中的位姿预测值进行更新，并且
根据所述第二运动状态量观测值中的速度观测值，对所述运动状态量预测值中的速度预测值进行更新，以确定所述最优运动状态量。
在上述激光里程计的实现方法的一个技术方案中，所述方法还包括通过下列方式对IMU、激光雷达和轮速计进行外参标定：
根据所述车辆的最优运动状态量，确定所述车辆的行驶轨迹；
判断所述行驶轨迹是否包含旋转角度大于预设角度阈值的旋转轨迹并且所述IMU的零偏在预设时长内的变化量是否小于预设变化量阈值；
若是，则对IMU、激光雷达和轮速计进行外参标定；
若否，则不对IMU、激光雷达和轮速计进行外参标定。
在上述激光里程计的实现方法的一个技术方案中，当所述轮速计发生异常时，所述方法还包括：
不再执行“根据当前周期所述车辆上轮速计的测量数据，获取当前周期所述车辆的第二运动状态量观测值”的步骤；
将“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤替换成下列步骤：根据所述第一运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
在上述激光里程计的实现方法的一个技术方案中，当所述IMU发生异常时，所述方法还包括：
将“根据上一周期车辆上IMU的测量数据，预测当前周期所述车辆的运动状态量预测值”的步骤替换成下列步骤：将上一周期车辆的最优运动状态量，作为当前周期所述车辆的运动状态量预测值。
在上述激光里程计的实现方法的一个技术方案中，当激光雷达发生异常时，所述方法还包括：
不再执行“根据当前周期所述车辆上激光雷达的点云数据帧，获取当前周期所述车辆的第一运动状态量观测值”的步骤；
将“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤替换成下列步骤：根据所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
在上述激光里程计的实现方法的一个技术方案中，当所述IMU和所述轮速计发生异常时，所述方法还包括：
将“根据上一周期车辆上IMU的测量数据，预测当前周期所述车辆的运动状态量预测值”的步骤替换成下列步骤：将上一周期车辆的最优运动状态量，作为当前周期所述车辆的运动状态量预测值；
不再执行“根据当前周期所述车辆上轮速计的测量数据，获取当前周期所述车辆的第二运动状态量观测值”的步骤；
将“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤替换成下列步骤：根据所述第一运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
在上述激光里程计的实现方法的一个技术方案中，“根据当前周期所述车辆上激光雷达的点云数据帧，获取当前周期所述车辆的第一运动状态量观测值”的步骤具体包括：
将所述运动状态量预测值中的位姿预测值，作为所述第一运动状态量观测值中位姿观测值的初始值；
根据所述初始值对所述点云数据帧与局部点云地图进行点云匹配；
根据点云匹配的结果，获取所述位姿观测值的最终值；
其中，所述局部点云地图是根据当前周期之前多个周期的历史点云数据帧建立的点云地图。
在上述激光里程计的实现方法的一个技术方案中，“根据所述初始值对所述点云数据帧与局部点云地图进行点云匹配”的步骤具体包括：
对所述点云数据帧进行第一下采样处理，得到候选匹配点云；
根据所述初始值，对所述候选匹配点云与所述局部点云地图进行点云匹配。
在上述激光里程计的实现方法的一个技术方案中，在“对所述点云数据帧进行第一下采样处理，得到候选匹配点云”的步骤之前，所述方法还包括：
根据所述运动状态量预测值中的位姿预测值，对所述点云数据帧进行点云去畸变。
在上述激光里程计的实现方法的一个技术方案中，所述方法还包括通过下列方式对所述局部点云地图进行更新：
对所述点云数据帧进行第二下采样处理，得到候选地图点云；
将所述候选地图点云叠加至所述局部点云地图上，以更新所述局部点云地图。
在上述激光里程计的实现方法的一个技术方案中，在“对所述局部点云地图进行更新”的步骤之前，所述方法还包括：
判断当前周期的点云数据帧是否为点云数据关键帧；
若是，则继续执行“对所述局部点云地图进行更新”的步骤；
若否，则不再执行“对所述局部点云地图进行更新”的步骤。
在上述激光里程计的实现方法的一个技术方案中，在对“所述点云数据帧进行第一下采样处理，得到候选地图点云”的步骤之前，所述方法还包括：根据所述运动状态量预测值中的位姿预测值，对所述点云数据帧进行点云去畸变。
在上述激光里程计的实现方法的一个技术方案中，所述方法还包括通过下列方式确定所述局部点云地图的地图范围：
获取所述激光雷达的视场范围；
根据所述视场范围，确定所述局部点云地图的地图范围。
在上述激光里程计的实现方法的一个技术方案中，所述方法还包括：根据所述车辆上轮速计的测量数据，对车辆的速度状态量进行初始化。
在第二方面，提供一种计算机设备，该计算机设备包括处理器和存储装置，所述存储装置适于存储多条程序代码，所述程序代码适于由所述处理器加载并运行以执行上述激光里程计的实现方法的技术方案中任一项技术方案所述的激光里程计的实现方法。
在第三方面，提供一种计算机可读存储介质，该计算机可读存储介质其中存储有多条程序代码，所述程序代码适于由处理器加载并运行以执行上述激光里程计的实现方法的技术方案中任一项技术方案所述的激光里程计的实现方法。
在第四方面，提供一种车辆，该车辆包括上述计算机设备的技术方案上述的计算机设备。
本发明上述一个或多个技术方案，至少具有如下一种或多种有益效果：
在实施本发明的激光里程计的实现方法的技术方案中，可以根据上一周期车辆上IMU的测量数据，预测当前周期车辆的运动状态量预测值；根据当前周期车辆上激光雷达的点云数据帧，获取当前周期车辆的第一运动状态量观测值；根据当前周期车辆上轮速计的测量数据，获取当前周期车辆的第二运动状态量观测值；最后，根据第一运动状态量观测值与第二运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。
通过融合IMU、激光雷达和轮速计的测量数据，共同实现激光里程计，可以准确地确定出车辆在每个周期内的运动状态量。同时，上述实施方式对算力的需求较小，不会带来较大的计算资源压力。因此，在利用上述实施方式得到的激光里程计进行建图和定位时，能够很好地兼顾建图和定位的实时性和高精度，克服了现有技术为了保证高精度通常需要较高的算力，必然会影响建图和定位的实时性的缺陷。
进一步，在实施本发明的激光里程计的实现方法的技术方案中，当IMU、激光雷达和轮速计中的一个或多个发生异常时，还可以继续融合剩余设备的测量数据，确定车辆的最优运动状态量，因而显著提高了激光里程计的鲁棒性。
附图说明
参照附图，本发明的公开内容将变得更易理解。本领域技术人员容易理解的是：这些附图仅仅用于说明的目的，而并非意在对本发明的保护范围组成限制。其中：
图1是根据本发明的一个实施例的融合IMU、激光雷达和轮速计的激光里程计实现方法的主要步骤流程示意图；
图2是根据本发明的一个实施例的获取第一运动状态量观测值的方法的主要步骤流程示意图；
图3是根据本发明的一个实施例的获取第二运动状态量观测值的方法的主要步骤流程示意图；
图4是根据本发明的一个实施例的轮速示意图；
图5是根据本发明的一个实施例的对IMU、激光雷达和轮速计进行外参标定的方法的主要步骤流程示意图；
图6是根据本发明的一个实施例的融合IMU和激光雷达的激光里程计实现方法的主要步骤流程示意图；
图7是根据本发明的一个实施例的融合激光雷达和轮速计的激光里程计实现方法的主要步骤流程示意图；
图8是根据本发明的一个实施例的融合IMU和轮速计的激光里程计实现方法的主要步骤流程示意图；
图9是根据本发明的一个实施例的仅融合激光雷达的激光里程计实现方法的主要步骤流程示意图；
图10是根据本发明的一个实施例的计算机设备的主要结构示意图。
具体实施方式
下面参照附图来描述本发明的一些实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。
在本发明的描述中，“处理器”可以包括硬件、软件或者两者的组合。处理器可以是中央处理器、微处理器、图像处理器、数字信号处理器或者其他任何合适的处理器。处理器具有数据和/或信号处理功能。处理器可以以软件方式实现、硬件方式实现或者二者结合方式实现。计算机可读存储介质包括任何合适的可存储程序代码的介质，比如磁碟、硬盘、光碟、闪存、只读存储器、随机存取存储器等等。
下面对本发明提供的激光里程计的实现方法实施例进行说明。
参阅附图1，图1是根据本发明的一个实施例的融合了IMU、激光雷达和轮速计的激光里程计的实现方法的主要步骤流程示意图。如图1所示，该激光里程计的实现方法主要包括下列步骤S101至步骤S104。
步骤S101：根据上一周期车辆上IMU的测量数据，预测当前周期车辆的运动状态量预测值。
IMU（惯性测量单元，Inertial measurement unit）至少包括陀螺仪和加速度计，陀螺仪可以得到角速度，加速度计可以得到加速度，因此，IMU的测量数据至少包括上述角速度和加速度。
根据角速度和加速度可以计算得到车辆的姿态，对加速度分别进行一次积分和两次积分可以得到车辆的速度和位置，因此，运动状态量预测值包括但不限于姿态、速度和位置，姿态和位置可以描述成位姿。
需要说明的是，本领域技术人员可以采用IMU技术领域中常规的姿态计算方法，根据IMU测量到的加速度与角速度计算得到车辆的姿态，本发明实施例在此不对上述方法作具体限定。
步骤S102：根据当前周期车辆上激光雷达的点云数据帧，获取当前周期车辆的第一运动状态量观测值。
第一运动状态量观测值至少包括车辆的位置和姿态，即至少包括位姿观测值。
步骤S103：根据当前周期车辆上轮速计的测量数据，获取当前周期车辆的第二运动状态量观测值。
第二运动状态量观测值至少包括车辆的速度，即至少包括速度观测值。
步骤S104：根据第一运动状态量观测值与第二运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。
本发明实施例可以基于卡尔曼滤波（Kalman Filtering）方法，根据第一运动状态量观测值与第二运动状态量观测值，对运动状态量预测值进行更新，将更新后的预测值作为最优运动状态量。
具体而言，可以建立公式（1）所示的IMU预测方程，通过该IMU预测方程预测得到第
Figure DEST_PATH_IMAGE001
个周期的运动状态量预测值，然后通过公式（2）所示的状态更新方程对公式（1）得到的运 动状态量预测值进行更新。
Figure 144693DEST_PATH_IMAGE002
（1）
Figure DEST_PATH_IMAGE003
（2）
上述公式（1）和（2）中各参数的含义分别如下：
Figure 229324DEST_PATH_IMAGE004
表示第
Figure DEST_PATH_IMAGE005
个周期的运动状态量预测值，
Figure 444273DEST_PATH_IMAGE006
表示第
Figure DEST_PATH_IMAGE007
个周期的运动状态 量预测值，
Figure 360277DEST_PATH_IMAGE008
表示第
Figure 707207DEST_PATH_IMAGE005
个周期的状态转移矩阵，
Figure 138188DEST_PATH_IMAGE009
表示第
Figure 845244DEST_PATH_IMAGE005
个周期的车辆控制信号的信号 向量，
Figure DEST_PATH_IMAGE010
表示第
Figure 333863DEST_PATH_IMAGE005
个周期的控制矩阵；
Figure 225596DEST_PATH_IMAGE011
表示第
Figure 284818DEST_PATH_IMAGE005
个周期的协方差矩阵，
Figure DEST_PATH_IMAGE012
表示第
Figure 80386DEST_PATH_IMAGE007
个周期的协方差矩阵，
Figure 502140DEST_PATH_IMAGE013
表示转置符号，
Figure DEST_PATH_IMAGE014
表示协方差为
Figure 158249DEST_PATH_IMAGE014
的噪声；
Figure 298244DEST_PATH_IMAGE015
表示对
Figure 878261DEST_PATH_IMAGE004
进行更新后的结果，
Figure DEST_PATH_IMAGE016
表示第
Figure 154521DEST_PATH_IMAGE005
个周期的观测值（如第一运动状态量 观测值与第二运动状态量观测值），
Figure 279734DEST_PATH_IMAGE017
表示第
Figure 907025DEST_PATH_IMAGE005
个周期的变换矩阵，
Figure DEST_PATH_IMAGE018
表示卡尔曼增益；
Figure 556312DEST_PATH_IMAGE019
表示对
Figure DEST_PATH_IMAGE020
进行更新后的结果。
需要说明的是，上述公式（1）和（2）是对卡尔曼滤波方法的简要说明，本发明实施例在此不对卡尔曼滤波方法的步骤流程进行赘述。
在本发明实施例中对各个运动状态量进行初始化时可以根据车辆上轮速计的测量数据，对速度状态量进行初始化，此外可以根据预设的位置先验值和姿态先验值分别对位置状态量和姿态状态量进行初始化。
基于上述步骤S101至步骤S104所述的方法，可以融合IMU、激光雷达和轮速计的测量数据，准确地确定出车辆在每个周期内的运动状态量。同时，由于对算力的需求较小，上述方法不会带来较大的计算资源压力。因此，在利用上述方法得到的激光里程计进行建图和定位时，能够很好地兼顾建图和定位的实时性和高精度，克服了现有技术为了保证高精度通常需要较高的算力，必然会影响建图和定位的实时性的缺陷。
下面分别对上述步骤S102至步骤S104作进一步说明。
一、对步骤S102进行说明。
在本发明实施例中可以采用点云匹配的方法，利用激光雷达的点云数据帧，获取第一运动状态量观测值。
具体而言，参阅附图2，在本发明实施例中可以通过下列步骤S1021至步骤S1023，获取第一运动状态量观测值。
步骤S1021：将运动状态量预测值中的位姿预测值，作为第一运动状态量观测值中位姿观测值的初始值。
位姿预测值是指运动状态量预测值中车辆的位置和姿态，位姿观测值是指第一运动状态量观测值中车辆的位置和姿态。
步骤S1022：根据初始值对点云数据帧与局部点云地图进行点云匹配。
基于激光雷达的建图方法主要包括前端和后端两个部分，前端部分主要是利用激光里程计进行增量式局部地图构建，后端部分主要是进行全局地图构建，本步骤中的局部点云地图就是上述增量式局部地图，其主要是根据当前周期之前多个周期的历史点云数据帧建立的点云地图。
在实际应用中不同的激光雷达可能具有不同的视场范围。比如，一些激光雷达的视场范围的视场角（Field of view）是360°，另一些激光雷达的视场范围的视场角是120°。为了提高点云匹配的效率，可以根据激光雷达的视场范围，确定局部点云地图的地图范围，然后对位于这个地图范围内的局部点云地图与激光雷达的点云数据帧进行点云匹配。比如，对于视场角是360°的激光雷达，地图范围可以是整个局部点云地图；对于视场角是120°的激光雷达，地图范围可以是位于视场范围内的局部点云地图。在一些优选实施方式中地图范围可以略大于视场范围，将视场范围之外的点云地图作为冗余部分，以提高点云匹配的准确性。
需要说明的是，在本发明实施例中可以采用自动驾驶技术领域中常规的点云匹配方法对点云数据帧与局部点云地图进行点云匹配。例如，可以采用基于ICP（IterativeClosest Point）算法的点云匹配方法对上述局部地图进行点云匹配，基于ICP算法的点云匹配方法可以是基于ICP算法中point-to-plane的方法的点云匹配方法。本发明实施例不对上述点云匹配的方法作具体限定。
为了提高点云匹配的准确性，在本发明实施中当IMU正常工作时可以根据利用IMU得到的运动状态量预测值中的位姿预测值，对点云数据帧进行点云去畸变，然后再利用点云去畸变后的点云数据帧进行点云匹配。需要说明的是，在本发明实施例中可以采用激光雷达点云技术领域中常规的点云去畸变方法对点云数据帧进行点云去畸变，本发明实施例不对上述点云去畸变方法进行具体限定。而在一些实施方式中，若IMU发生异常，无法正常工作，则可以不对当前周期的点云数据帧进行点云去畸变。
进一步，为了提高点云匹配的效率，在本发明实施例中在对点云数据帧进行点云去畸变之后，还可以对点云数据帧进行第一下采样处理得到候选匹配点云，然后利用候选匹配点云进行点云匹配，即根据上述步骤S102中的初始值，对候选匹配点云与局部点云地图进行点云匹配。需要说明的是，在本发明实施例中可以采用数据采样技术领域中常规的下采样方法对点云数据帧进行第一下采样处理得到候选匹配点云，本发明实施例不对上述下采样方法进行赘述。根据前述描述可知，IMU发生异常时可以不对当前周期的点云数据帧进行点云去畸变，此时，可以直接对原始（未进行点云去畸变）的点云数据帧进行第一下采样处理得到候选匹配点云。
步骤S1023：根据点云匹配的结果，获取位姿观测值的最终值。通过点云匹配可以对位姿观测值的初始值进行优化，优化后的结果就是位姿观测值的最终值。
基于上述步骤S1021至步骤S1023所述的方法，可以利用激光雷达的点云数据帧准确得到车辆的位置和姿态（位姿观测值）。
进一步，在本发明实施例中还可以利用当前周期的点云数据帧更新局部点云地图，以便在下一个周期可以利用更新后的局部点云地图得到准确的位姿观测值。下面对更新局部点云地图的方法进行说明。
具体地，首先，根据运动状态量预测值中的位姿预测值，对点云数据帧进行点云去畸变。然后，对点云去畸变之后的点云数据帧进行第二下采样处理得到候选地图点云。最后，将候选地图点云叠加至局部点云地图上，以更新局部点云地图。在本发明实施例中同样可以采用激光雷达点云技术领域中常规的点云去畸变方法对点云数据帧进行点云去畸变，本发明实施例不对上述云去畸变方法进行具体限定。根据前述描述可知，IMU发生异常时可以不对当前周期的点云数据帧进行点云去畸变，此时，可以直接对原始（未进行点云去畸变）的点云数据帧进行第二下采样处理得到候选地图点云。
此外，在本发明实施例中也可以采用数据采样技术领域中常规的下采样方法对点云数据帧进行第二下采样处理得到候选地图点云，本发明实施例不对上述下采样方法进行赘述。
根据上述描述可知，在本发明实施例中并没有将对点云数据帧进行第一下采样处理得到的候选匹配点云作为候选地图点云使用，而是单独进行两次下采样分别得到候选匹配点云和候选地图点云。通过这种方式，有利于分别控制候选匹配点云和候选地图点云的数量，本领域技术人员可以根据实际需求灵活地设定第一下采样处理和第二次下采样处理的下采样参数，以获取满足实际需求的候选匹配点云和候选地图点云。
进一步，在本发明实施例的一些实施方式中，为了进一步提高地图更新的效率，可以选取点云数据关键帧进行更新，摒除掉非点云数据关键帧。具体而言，在得到当前周期的点云数据帧之后，先判断这个点云数据帧是否为点云数据关键帧；若是，则根据这个点云数据帧获取候选地图点云，然后将候选地图点云叠加至局部点云地图上进行地图更新；若否，则不再利用当前周期的点云数据帧进行地图更新。
在本发明实施例可以通过下列方式判断当前周期的点云数据帧是否为点云数据关键帧。
具体而言，在启动本发明提供的激光里程计的实现方法后，为了快速完成局部点云地图的初始化，可以将启动之后连续多个周期的点云数据帧都作为点云数据关键帧。其中，对局部点云地图的初始化可以包括：先根据第1个点云数据关键帧上的候选地图点云形成初始的局部点云地图，然后根据第2个点云数据关键帧上的候选地图点云对上述初始的局部点云地图进行地图更新，进而根据第3个点云数据关键帧上的候选地图点云对上述更新后的局部点云地图再次进行地图更新，以此类推，直至根据最后1个点云数据关键帧进行地图更新，即可完成局部点云地图的初始化。需要说明的是，本领域技术人员可以根据实际需求灵活设置上述连续多个周期的周期数量，比如在一些实施方式中周期数量可以是30，即将启动之后连续30个周期的点云数据帧都作为点云数据关键帧。本发明实施例不对上述周期数量的具体数值进行限定。
在完成局部点云地图的初始化之后，针对后续每个周期的点云数据帧，可以分别按照下列方式判断各个周期的点云数据帧是否为点云数据关键帧。
根据当前周期的点云数据帧，并采用前述步骤S101至步骤S104所述的方法，确定当前周期车辆的最优运动状态量，并获取这个最优运动状态量中的最优位置和最优姿态；同时，获取上一个周期车辆的最优运动状态量，并获取这个最优运动状态量中的最优位置和最优姿态。
根据上述最优位置，计算当前周期相对于上一个周期的位置变化量；根据上述最优姿态，计算当前周期相对于上一个周期的姿态变化量；此外，还可以计算当前周期与上一个周期之间的时间间隔。当上述位置变化量、姿态变化量和时间间隔中任意一个大于各自对应的设定阈值时，就可以判定当前周期的点云数据帧是点云数据关键帧。需要说明的是，本领域技术人员可以根据实际需求灵活设置上述位置变化量、姿态变化量和时间间隔各自对应的设定阈值的具体数值，比如在一些实施方式中上述位置变化量、姿态变化量和时间间隔各自对应的设定阈值分别是5米、5°和2秒。
此外，还需要说明的是，上述地图更新的方法是以当前周期的点云数据帧为例进行说明的，对于后续的每一个点云数据帧，都会按照上述地图更新的方法对局部点云地图进行地图更新。由前述描述可知，在对局部点云地图进行初始化时也是采用上述地图更新的方法完成的初始化。也就是说，在本发明实施例中无论是在初始化阶段，还是在初始化完成之后的阶段，局部点云地图都是根据每个点云数据关键帧上的候选地图点云建立形成的。
二、对步骤S103进行说明。
在本发明实施例中第二运动状态量观测值除了可以包括车辆的速度，还可以包括车辆的位移，即位移观测值。下面结合附图3和附图4对位移观测值的获取方法进行说明。
参阅附图3，在本发明实施例中可以通过下列步骤S1031至步骤S1035，并根据当前周期车辆上轮速计的测量数据，获取位移观测值
步骤S1031：根据当前周期车辆上轮速计的测量数据，获取当前周期的轮速。
步骤S1032：根据当前周期车辆上IMU的测量数据与当前周期内每个轮速的时刻，分别获取在每个时刻的车辆姿态。
具体地，可以先根据IMU的测量数据获取当前周期内每个时刻的车辆姿态，然后根 据每个轮速的时刻对这些车辆姿态进行插值计算，就可以得到在每个轮速时刻的车辆姿 态。如图4所示，在当前周期内，按照时间由先至后的顺序，轮速包括
Figure 874030DEST_PATH_IMAGE021
、
Figure DEST_PATH_IMAGE022
、
Figure 153832DEST_PATH_IMAGE023
和
Figure DEST_PATH_IMAGE024
共 四个轮速，根据IMU的测量数据得到的车辆姿态包括
Figure 422746DEST_PATH_IMAGE025
、
Figure DEST_PATH_IMAGE026
、
Figure 610145DEST_PATH_IMAGE027
、
Figure DEST_PATH_IMAGE028
、
Figure 239095DEST_PATH_IMAGE029
、
Figure DEST_PATH_IMAGE030
和
Figure 971690DEST_PATH_IMAGE031
共七个姿 态。根据
Figure 432627DEST_PATH_IMAGE021
、
Figure 17193DEST_PATH_IMAGE022
、
Figure DEST_PATH_IMAGE032
和
Figure 794656DEST_PATH_IMAGE024
的时刻，通过对这七个车辆姿态进行插值计算可以得到在
Figure 429643DEST_PATH_IMAGE021
、
Figure 518822DEST_PATH_IMAGE022
、
Figure 782444DEST_PATH_IMAGE032
和
Figure 211151DEST_PATH_IMAGE024
的时刻下的车辆姿态
Figure 393871DEST_PATH_IMAGE033
、
Figure DEST_PATH_IMAGE034
、
Figure 891717DEST_PATH_IMAGE035
和
Figure DEST_PATH_IMAGE036
。
步骤S1033：根据车辆姿态，分别获取每两个相邻之间的相对姿态。相对姿态是指两个时刻各自对应的车辆姿态之间的变化量。
步骤S1034：根据每两个相邻时刻之间的相对姿态，分别对每两个相邻时刻的轮速进行积分，以获取在世界坐标系下每两个相邻时刻之间的相对位移。
根据上述相对姿态轮速进行积分，可以将每两个相邻时刻之间的相对位移都转换到世界坐标系下，即具有相同的坐标系，这样就可以直接对这些相对位移进行相加得到位移观测值。
在一些实施方式中可以根据上述相对姿态并通过下列公式（3）对两个相邻时刻的轮速进行积分，得到这两个相邻时刻之间的相对位移。
Figure 427872DEST_PATH_IMAGE037
（3）
公式（3）中各参数含义分别如下：
Figure DEST_PATH_IMAGE038
、
Figure 665080DEST_PATH_IMAGE039
和
Figure DEST_PATH_IMAGE040
分别表示当前周期内的第
Figure 690805DEST_PATH_IMAGE041
、第
Figure DEST_PATH_IMAGE042
和第
Figure 941527DEST_PATH_IMAGE043
个轮速，
Figure DEST_PATH_IMAGE044
表示在第
Figure 281373DEST_PATH_IMAGE043
个轮速对应的时刻，由车辆坐标系转换到世界坐标系的转换姿态，
Figure 481410DEST_PATH_IMAGE045
表 示第
Figure 888821DEST_PATH_IMAGE042
和第
Figure 377571DEST_PATH_IMAGE043
个轮速各自对应的时刻之间的相对姿态，
Figure DEST_PATH_IMAGE046
，
Figure 52266DEST_PATH_IMAGE047
表示在第
Figure 372389DEST_PATH_IMAGE042
个轮速对应的时刻，由车辆坐标系转换到世界坐标系的转换姿态，
Figure 458025DEST_PATH_IMAGE013
表示转置符号，
Figure DEST_PATH_IMAGE048
表示第
Figure 699651DEST_PATH_IMAGE041
和第
Figure 646878DEST_PATH_IMAGE042
个轮速之间的时间间隔。需要说明的是，上述 转换姿态
Figure 87087DEST_PATH_IMAGE044
和
Figure 579510DEST_PATH_IMAGE047
的具体数值可以由IMU得到，本发明实施例不对获取上述转换姿态
Figure 42853DEST_PATH_IMAGE044
和
Figure 652826DEST_PATH_IMAGE047
的方法进行说明，只要能够通过IMU得到上述转换姿态
Figure 557328DEST_PATH_IMAGE044
和
Figure 860133DEST_PATH_IMAGE047
即可。
步骤S1035：根据每两个相邻时刻之间的相对位移，获取位移观测值。具体地，将当 前周期内每两个相邻时刻之间的相对位移相加就可以得到当前周期的位移观测值，即位移 观测值
Figure 732143DEST_PATH_IMAGE049
。
继续参阅附图4，利用公式（3）可以计算得到当前周期的位移观测值
Figure DEST_PATH_IMAGE050
如下式（4） 所示。
Figure 552331DEST_PATH_IMAGE051
（4）
在对轮速进行积分的过程中，轮速计是存在旋转运动的，这就导致同一个周期内不同时刻的轮速方向不一致，而基于上述步骤S1031至步骤S1035所述的方法可以在对轮速进行积分的过程中利用两个时刻之间的相对姿态进行积分补偿，对同一个周期内不同时刻的轮速进行坐标系统一，消除轮速方向不一致的问题。
三、对步骤S104进行说明。
根据前述步骤S103的描述可知，第二运动状态量观测值可以包括位移观测值，也可以包括速度观测值。在此情况下，在根据第二运动状态量观测值对运动状态量预测值进行更新时可以择一使用位移观测值或速度观测值进行更新。下面分别对使用位移观测值或速度观测值进行更新的方法进行说明。
（一）使用位移观测值进行更新的方法
在本发明实施例中可以根据第一运动状态量观测值中的位姿观测值，对运动状态 量预测值中的位姿预测值进行更新，同时根据第二运动状态量观测值中的位移观测值，对 位姿预测值中的位置预测值进行更新。也就是说，公式（2）中第
Figure 435974DEST_PATH_IMAGE001
个周期的观测值
Figure 112943DEST_PATH_IMAGE016
包括上 述位姿观测值和位移观测值。
（二）使用速度观测值进行更新的方法
在本发明实施例中可以根据第一运动状态量观测值中的位姿观测值，对运动状态 量预测值中的位姿预测值进行更新，同时根据第二运动状态量观测值中的速度观测值，对 位姿预测值中的速度预测值进行更新。也就是说，公式（2）中第
Figure 705205DEST_PATH_IMAGE005
个周期的观测值
Figure 656980DEST_PATH_IMAGE016
包括上 述位姿观测值和速度预测值。
以上是对步骤S102至步骤S104的进一步说明。
由于本发明实施例是融合IMU、激光雷达和轮速计多种设备的测量数据，来确定车辆的最优运动状态量，因此IMU、激光雷达和轮速计之间外参是否准确，会极大地影响最优运动状态量的准确性。对此，为了避免由于外参不准确而影响最优运动状态量的准确性，在本发明实施例中可以通过图5所示的步骤S201至步骤S204对IMU、激光雷达和轮速计之间的外参进行外参标定。
步骤S201：根据车辆的最优运动状态量，确定车辆的行驶轨迹。最优运动状态量至少包括车辆的位置和姿态。
步骤S202：判断行驶轨迹是否包含旋转角度大于预设角度阈值的旋转轨迹并且IMU的零偏（Bias）在预设时长内的变化量是否小于预设变化量阈值；若包含该旋转轨迹且IMU的零偏在预设时长内的变化量小于预设变化量阈值，则表明当前运动状态量的估计处于约束性较好的稳定状态，适合进行外参标定，因而转至步骤S203；否则，转至步骤S204。需要说明的是，本领域技术人员可以根据实际需求灵活设置预设角度阈值和预设时长的具体数值，本发明实施例不进行具体限定。例如，预设角度阈值可以是45°。
步骤S203：对IMU、激光雷达和轮速计进行外参标定。
步骤S204：不对IMU、激光雷达和轮速计进行外参标定。
需要说明的是，在本发明实施例中可以采用外参标定技术领域中常规的多设备外参标定方法对IMU、激光雷达和轮速计之间的外参进行联合标定，本发明实施例不对上述多设备外参标定方法作具体限定。
基于上述步骤S201至步骤S204所述的方法，可以及时地判断出运动状态量估计是否处于约束性较好的稳定状态，并在处于上述稳定状态的情况下进行外参标定，从而可以保证最优运动状态量的准确性。
在实际应用中IMU、激光雷达和轮速计可能无法同时正常运行，在此情况下，可以排除异常的设备，继续融合正常运行的设备，获取车辆的最优运动状态量。下面结合附图6至附图8分别对轮速计、IMU和激光雷达发生异常时获取车辆最优运动状态量的方法进行说明，同时结合附图9对轮速计和IMU都发生异常时获取车辆最优运动状态量的方法进行说明。
一、轮速计发生异常
在此情况下可以继续融合IMU和激光雷达的测量数据，获取车辆的最优运动状态量，即激光里程计运行在IMU与激光雷达的融合模式。
如图6所示，当激光里程计运行在IMU与激光雷达的融合模式时可以通过下列步骤S301至步骤S303，获取车辆的最优运动状态量。
步骤S301：根据上一周期车辆上IMU的测量数据，预测当前周期车辆的运动状态量预测值。
步骤S302：根据当前周期车辆上激光雷达的点云数据帧，获取当前周期车辆的第一运动状态量观测值。
步骤S303：根据第一运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。
基于上述步骤S301至步骤S303，可以继续基于卡尔曼滤波方法，根据第一运动状态量观测值，对运动状态量预测值进行更新，将更新后的预测值作为最优运动状态量，避免了在轮速计发生异常时无法获取车辆最优运动状态量的问题。
二、IMU发生异常
在此情况下可以继续融合轮速计和激光雷达的测量数据，获取车辆的最优运动状态量，即激光里程计运行在轮速计与激光雷达的融合模式。
如图7所示，当激光里程计运行在轮速计与激光雷达的融合模式时可以通过下列步骤S401至步骤S404，获取车辆的最优运动状态量。
步骤S401：将上一周期车辆的最优运动状态量，作为当前周期车辆的运动状态量预测值，即不再通过IMU获取运动状态量预测值。
步骤S402：根据当前周期车辆上激光雷达的点云数据帧，获取当前周期车辆的第一运动状态量观测值。
步骤S403：根据当前周期车辆上轮速计的测量数据，获取当前周期车辆的第二运动状态量观测值。
步骤S404：根据第一运动状态量观测值与第二运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。
基于上述步骤S401至步骤S404，可以继续基于卡尔曼滤波方法，根据第一运动状态量观测值与第二运动状态量观测值，对运动状态量预测值进行更新，将更新后的预测值作为最优运动状态量，避免了在IMU发生异常时无法获取车辆最优运动状态量的问题。
三、激光雷达发生异常
在此情况下可以继续融合IMU和轮速计的测量数据，获取车辆的最优运动状态量，即激光里程计运行在IMU与轮速计的融合模式。
如图8所示，当激光里程计运行在IMU与轮速计的融合模式时可以通过下列步骤S501至步骤S503，获取车辆的最优运动状态量。
步骤S501：根据上一周期车辆上IMU的测量数据，预测当前周期车辆的运动状态量预测值。
步骤S502：根据当前周期车辆上轮速计的测量数据，获取当前周期车辆的第二运动状态量观测值。
步骤S503：根据第二运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。
基于上述步骤S501至步骤S503，可以继续基于卡尔曼滤波方法，根据第二运动状态量观测值，对运动状态量预测值进行更新，将更新后的预测值作为最优运动状态量，避免了在激光雷达发生异常时无法获取车辆最优运动状态量的问题。
四、IMU和轮速计发生异常
在此情况下可以继续根据激光雷达的测量数据（点云数据帧），获取车辆的最优运动状态量，即激光里程计运行在激光雷达模式。
如图9所示，当激光里程计运行在激光雷达模式时可以通过下列步骤S601至步骤S603，获取车辆的最优运动状态量。
步骤S601：将上一周期车辆的最优运动状态量，作为当前周期车辆的运动状态量预测值，即不再通过IMU获取运动状态量预测值。
步骤S602：根据当前周期车辆上激光雷达的点云数据帧，获取当前周期车辆的第一运动状态量观测值。
步骤S603：根据第一运动状态量观测值，对运动状态量预测值进行更新，以确定车辆的最优运动状态量。
基于上述步骤S601至步骤S603，可以继续基于卡尔曼滤波方法，根据第一运动状态量观测值，对运动状态量预测值进行更新，将更新后的预测值作为最优运动状态量，避免了在IMU和轮速计都发生异常时无法获取车辆最优运动状态量的问题。
需要指出的是，尽管上述实施例中将各个步骤按照特定的先后顺序进行了描述，但是本领域技术人员可以理解，为了实现本发明的效果，不同的步骤之间并非必须按照这样的顺序执行，其可以同时（并行）执行或以其他顺序执行，这些变化都在本发明的保护范围之内。
本领域技术人员能够理解的是，本发明实现上述一实施例的方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器、随机存取存储器、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。
进一步，本发明还提供了一种计算机设备。
参阅附图10，图10是根据本发明的一个计算机设备实施例的主要结构示意图。如图10所示，本发明实施例中的计算机设备主要包括存储装置和处理器，存储装置可以被配置成存储执行上述方法实施例的激光里程计的实现方法的程序，处理器可以被配置成用于执行存储装置中的程序，该程序包括但不限于执行上述方法实施例的激光里程计的实现方法的程序。为了便于说明，仅示出了与本发明实施例相关的部分，具体技术细节未揭示的，请参照本发明实施例方法部分。
在本发明实施例中计算机设备可以是包括各种电子设备形成的控制装置设备。在一些可能的实施方式中，计算机设备可以包括多个存储装置和多个处理器。而执行上述方法实施例的激光里程计的实现方法的程序可以被分割成多段子程序，每段子程序分别可以由处理器加载并运行以执行上述方法实施例的激光里程计的实现方法的不同步骤。具体地，每段子程序可以分别存储在不同的存储装置中，每个处理器可以被配置成用于执行一个或多个存储装置中的程序，以共同实现上述方法实施例的激光里程计的实现方法，即每个处理器分别执行上述方法实施例的激光里程计的实现方法的不同步骤，来共同实现上述方法实施例的激光里程计的实现方法。
上述多个处理器可以是部署于同一个设备上的处理器，例如上述计算机设备可以是由多个处理器组成的高性能设备，上述多个处理器可以是该高性能设备上配置的处理器。此外，上述多个处理器也可以是部署于不同设备上的处理器
进一步，本发明还提供了一种计算机可读存储介质。
在根据本发明的一个计算机可读存储介质的实施例中，计算机可读存储介质可以被配置成存储执行上述方法实施例的激光里程计的实现方法的程序，该程序可以由处理器加载并运行以实现上述激光里程计的实现方法。为了便于说明，仅示出了与本发明实施例相关的部分，具体技术细节未揭示的，请参照本发明实施例方法部分。该计算机可读存储介质可以是包括各种电子设备形成的存储装置设备，可选的，本发明实施例中计算机可读存储介质是非暂时性的计算机可读存储介质。
进一步，本发明还提供了一种车辆。在根据本发明的一个车辆的实施例中，车辆可以包括上述计算机设备实施例所述的计算机设备。在本实施例中车辆可以是自动驾驶车辆、无人车等车辆。此外，按照动力源类型划分，本实施例中车辆可以是燃油车、电动车、电能与燃油混合的混动车或使用其他新能源的车辆等。
至此，已经结合附图所示的一个实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (18)

1.一种激光里程计的实现方法，其特征在于，所述方法包括：
根据上一周期车辆上IMU的测量数据，预测当前周期所述车辆的运动状态量预测值；
根据当前周期所述车辆上激光雷达的点云数据帧，获取当前周期所述车辆的第一运动状态量观测值；
根据当前周期所述车辆上轮速计的测量数据，获取当前周期所述车辆的第二运动状态量观测值；
根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量，具体包括根据所述第一运动状态量观测值中的位姿观测值，对所述运动状态量预测值中的位姿预测值进行更新，并且根据所述第二运动状态量观测值中的位移观测值，对所述位姿预测值中的位置预测值进行更新，以确定所述最优运动状态量；
其中，所述方法还包括通过下列方式获取所述位移观测值：根据当前周期所述车辆上轮速计的测量数据，获取当前周期的轮速；根据当前周期所述车辆上IMU的测量数据与当前周期内每个轮速的时刻，分别获取在每个时刻的车辆姿态；根据所述车辆姿态，分别获取每两个相邻时刻之间的相对姿态；根据每两个相邻时刻之间的相对姿态，分别对每两个相邻时刻的轮速进行积分，以获取在世界坐标系下每两个相邻时刻之间的相对位移；根据所述每两个相邻时刻之间的相对位移，获取所述位移观测值。
2.根据权利要求1所述的激光里程计的实现方法，其特征在于，“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤还包括：
根据所述第一运动状态量观测值中的位姿观测值，对所述运动状态量预测值中的位姿预测值进行更新，并且
根据所述第二运动状态量观测值中的速度观测值，对所述运动状态量预测值中的速度预测值进行更新，以确定所述最优运动状态量。
3.根据权利要求1所述的激光里程计的实现方法，其特征在于，所述方法还包括通过下列方式对IMU、激光雷达和轮速计进行外参标定：
根据所述车辆的最优运动状态量，确定所述车辆的行驶轨迹；
判断所述行驶轨迹是否包含旋转角度大于预设角度阈值的旋转轨迹并且所述IMU的零偏在预设时长内的变化量是否小于预设变化量阈值；
若是，则对IMU、激光雷达和轮速计进行外参标定；
若否，则不对IMU、激光雷达和轮速计进行外参标定。
4.根据权利要求1所述的激光里程计的实现方法，其特征在于，当所述轮速计发生异常时，所述方法还包括：
不再执行“根据当前周期所述车辆上轮速计的测量数据，获取当前周期所述车辆的第二运动状态量观测值”的步骤；
将“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤替换成下列步骤：根据所述第一运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
5.根据权利要求1所述的激光里程计的实现方法，其特征在于，当所述IMU发生异常时，所述方法还包括：
将“根据上一周期车辆上IMU的测量数据，预测当前周期所述车辆的运动状态量预测值”的步骤替换成下列步骤：将上一周期车辆的最优运动状态量，作为当前周期所述车辆的运动状态量预测值。
6.根据权利要求1所述的激光里程计的实现方法，其特征在于，当激光雷达发生异常时，所述方法还包括：
不再执行“根据当前周期所述车辆上激光雷达的点云数据帧，获取当前周期所述车辆的第一运动状态量观测值”的步骤；
将“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤替换成下列步骤：根据所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
7.根据权利要求1所述的激光里程计的实现方法，其特征在于，当所述IMU和所述轮速计发生异常时，所述方法还包括：
将“根据上一周期车辆上IMU的测量数据，预测当前周期所述车辆的运动状态量预测值”的步骤替换成下列步骤：将上一周期车辆的最优运动状态量，作为当前周期所述车辆的运动状态量预测值；
不再执行“根据当前周期所述车辆上轮速计的测量数据，获取当前周期所述车辆的第二运动状态量观测值”的步骤；
将“根据所述第一运动状态量观测值与所述第二运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量”的步骤替换成下列步骤：根据所述第一运动状态量观测值，对所述运动状态量预测值进行更新，以确定所述车辆的最优运动状态量。
8.根据权利要求1所述的激光里程计的实现方法，其特征在于，“根据当前周期所述车辆上激光雷达的点云数据帧，获取当前周期所述车辆的第一运动状态量观测值”的步骤具体包括：
将所述运动状态量预测值中的位姿预测值，作为所述第一运动状态量观测值中位姿观测值的初始值；
根据所述初始值对所述点云数据帧与局部点云地图进行点云匹配；
根据点云匹配的结果，获取所述位姿观测值的最终值；
其中，所述局部点云地图是根据当前周期之前多个周期的历史点云数据帧建立的点云地图。
9.根据权利要求8所述的激光里程计的实现方法，其特征在于，“根据所述初始值对所述点云数据帧与局部点云地图进行点云匹配”的步骤具体包括：
对所述点云数据帧进行第一下采样处理，得到候选匹配点云；
根据所述初始值，对所述候选匹配点云与所述局部点云地图进行点云匹配。
10.根据权利要求9所述的激光里程计的实现方法，其特征在于，在“对所述点云数据帧进行第一下采样处理，得到候选匹配点云”的步骤之前，所述方法还包括：
根据所述运动状态量预测值中的位姿预测值，对所述点云数据帧进行点云去畸变。
11.根据权利要求8所述的激光里程计的实现方法，其特征在于，所述方法还包括通过下列方式对所述局部点云地图进行更新：
对所述点云数据帧进行第二下采样处理，得到候选地图点云；
将所述候选地图点云叠加至所述局部点云地图上，以更新所述局部点云地图。
12.根据权利要求11所述的激光里程计的实现方法，其特征在于，在“对所述局部点云地图进行更新”的步骤之前，所述方法还包括：
判断当前周期的点云数据帧是否为点云数据关键帧；
若是，则继续执行“对所述局部点云地图进行更新”的步骤；
若否，则不再执行“对所述局部点云地图进行更新”的步骤。
13.根据权利要求11所述的激光里程计的实现方法，其特征在于，在“对所述点云数据帧进行第二下采样处理，得到候选地图点云”的步骤之前，所述方法还包括：
根据所述运动状态量预测值中的位姿预测值，对所述点云数据帧进行点云去畸变。
14.根据权利要求8所述的激光里程计的实现方法，其特征在于，所述方法还包括通过下列方式确定所述局部点云地图的地图范围：
获取所述激光雷达的视场范围；
根据所述视场范围，确定所述局部点云地图的地图范围。
15.根据权利要求1所述的激光里程计的实现方法，其特征在于，所述方法还包括：
根据所述车辆上轮速计的测量数据，对车辆的速度状态量进行初始化。
16.一种计算机设备，包括处理器和存储装置，所述存储装置适于存储多条程序代码，其特征在于，所述程序代码适于由所述处理器加载并运行以执行权利要求1至15中任一项所述的激光里程计的实现方法。
17.一种计算机可读存储介质，其中存储有多条程序代码，其特征在于，所述程序代码适于由处理器加载并运行以执行权利要求1至15中任一项所述的激光里程计的实现方法。
18.一种车辆，其特征在于，所述车辆包括权利要求16所述的计算机设备。
CN202211569521.XA 2022-12-08 2022-12-08 激光里程计的实现方法、计算机设备、存储介质及车辆 Active CN115655302B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202211569521.XA CN115655302B (zh) 2022-12-08 2022-12-08 激光里程计的实现方法、计算机设备、存储介质及车辆
EP23209458.1A EP4382954A3 (en) 2022-12-08 2023-11-13 Lidar odometry implementation method, computer device, storage medium, and vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211569521.XA CN115655302B (zh) 2022-12-08 2022-12-08 激光里程计的实现方法、计算机设备、存储介质及车辆

Publications (2)

Publication Number Publication Date
CN115655302A CN115655302A (zh) 2023-01-31
CN115655302B true CN115655302B (zh) 2023-03-21

Family

ID=85017562

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211569521.XA Active CN115655302B (zh) 2022-12-08 2022-12-08 激光里程计的实现方法、计算机设备、存储介质及车辆

Country Status (2)

Country Link
EP (1) EP4382954A3 (zh)
CN (1) CN115655302B (zh)

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3109589B1 (en) * 2015-06-23 2019-01-30 Volvo Car Corporation A unit and method for improving positioning accuracy
DE102019207448A1 (de) * 2019-05-21 2020-11-26 Robert Bosch Gmbh Simultane Lokalisierung und Kartenerstellung in 2D unter Verwendung eines 3D-Scanners
CN113819905B (zh) * 2020-06-19 2024-07-12 北京图森未来科技有限公司 一种基于多传感器融合的里程计方法及装置
CN111707272B (zh) * 2020-06-28 2022-10-14 湖南大学 一种地下车库自动驾驶激光定位系统
CN113945206B (zh) * 2020-07-16 2024-04-19 北京图森未来科技有限公司 一种基于多传感器融合的定位方法及装置
CN114088093B (zh) * 2020-11-09 2024-07-16 北京京东乾石科技有限公司 一种点云地图构建方法、装置、系统及存储介质
CN115290071A (zh) * 2022-08-08 2022-11-04 爱驰汽车有限公司 相对定位融合方法、装置、设备及存储介质
CN115200588B (zh) * 2022-09-14 2023-01-06 煤炭科学研究总院有限公司 移动机器人的slam自主导航方法及其装置
CN115307646B (zh) * 2022-10-08 2023-03-24 浙江光珀智能科技有限公司 一种多传感器融合的机器人定位方法、系统及装置

Also Published As

Publication number Publication date
EP4382954A3 (en) 2024-08-14
EP4382954A2 (en) 2024-06-12
CN115655302A (zh) 2023-01-31

Similar Documents

Publication Publication Date Title
CN113465628B (zh) 惯性测量单元数据补偿方法及系统
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN112432644B (zh) 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN113252048A (zh) 一种导航定位方法、导航定位系统及计算机可读存储介质
US20080177495A1 (en) Automated measurement derivation
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN113720349A (zh) 一种基于卡尔曼滤波的里程计信息平滑方法
CN115655302B (zh) 激光里程计的实现方法、计算机设备、存储介质及车辆
CN111044053B (zh) 单舵轮无人车的导航方法、装置和单舵轮无人车
CN109459769B (zh) 一种自主定位方法与系统
CN114897942B (zh) 点云地图的生成方法、设备及相关存储介质
JP4517298B2 (ja) 方位センサの補正係数演算装置及び演算プログラム
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN115388914A (zh) 传感器的参数标定方法、装置、存储介质和电子装置
JP4366660B2 (ja) 方位センサの補正係数演算装置及び演算プログラム
JP4831441B2 (ja) 方位センサの補正係数演算装置及び演算プログラム
CN113483762A (zh) 一种位姿优化方法及设备
CN118654681B (zh) 一种误差状态rts平滑组合导航后处理方法
JP7491065B2 (ja) 状態推定装置、及び状態推定方法、状態推定プログラム
CN116793343A (zh) 车辆姿态初始化方法、装置、自动驾驶车辆及存储介质
CN117553794A (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