CN116929343A - 位姿估计方法、相关设备及存储介质 - Google Patents

位姿估计方法、相关设备及存储介质 Download PDF

Info

Publication number
CN116929343A
CN116929343A CN202310803048.5A CN202310803048A CN116929343A CN 116929343 A CN116929343 A CN 116929343A CN 202310803048 A CN202310803048 A CN 202310803048A CN 116929343 A CN116929343 A CN 116929343A
Authority
CN
China
Prior art keywords
acquisition time
pose
point cloud
acquisition
optimization
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
CN202310803048.5A
Other languages
English (en)
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.)
Beijing Xinchi Semiconductor Technology Co ltd
Original Assignee
Beijing Xinchi Semiconductor 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 Beijing Xinchi Semiconductor Technology Co ltd filed Critical Beijing Xinchi Semiconductor Technology Co ltd
Priority to CN202310803048.5A priority Critical patent/CN116929343A/zh
Publication of CN116929343A publication Critical patent/CN116929343A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请公开了一种位姿估计方法、相关设备及存储介质，其中所述方法包括：基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量；基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量；基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位。能够实现轻量级的位姿估计。

Description

位姿估计方法、相关设备及存储介质
技术领域
本申请涉及定位领域，尤其涉及一种位姿估计方法、相关设备及存储介质。
背景技术
即时定位与地图构建方法(SLAM，Simultaneous Localization and Mapping)可解决待测物体(如机器人、无人机、物流车等)在未知环境中的定位问题，具体是通过利用传感器采集到的数据对待测物体周围环境进行地图构建，同时对待测物体的位姿进行估计。相关技术中的位姿估计算法对算力要求高，需要运算能力强的硬件才能支撑算法的正常运行。而往往运算能力强的硬件的造价高，不利于支出成本的节省。
发明内容
本申请提供了一种位姿估计方法、相关设备及存储介质，以至少解决现有技术中存在的以上技术问题。
根据本申请的第一方面，提供了一种位姿估计方法，包括：
基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量；
基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；
基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量；
基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位；
其中，所述第p采集时刻的位姿优化量是在满足优化条件时，基于M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、和所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的相对位姿量而得到；其中，M为大于或等于1的正整数，所述M个采集时刻早于第p采集时刻；所述目标优化量是通过对所述至少之一位姿估计量进行优化而得到。
根据本申请的第二方面，提供了一种位姿估计设备，包括：
第一获得单元，用于基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量；
第二获得单元，用于基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；
第三获得单元，用于基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量；
定位单元，用于基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位；
其中，所述第p采集时刻的位姿优化量是在满足优化条件时，基于M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、和所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的相对位姿量而得到；其中，M为大于或等于1的正整数，所述M个采集时刻早于第p采集时刻；所述目标优化量是通过对所述至少之一位姿估计量进行优化而得到。
根据本申请的第三方面，提供了一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行本申请所述的方法。
根据本申请的第四方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，所述计算机指令用于使所述计算机执行本申请所述的方法。
与相关技术相比，本申请中的位姿估计算法易于实现，对算力要求不高，为轻量级的算法，还可有效避免由于购买运算能力强的硬件而导致的成本增加的问题，节约成本。
应当理解，本部分所描述的内容并非旨在标识本申请的实施例的关键或重要特征，也不用于限制本申请的范围。本申请的其它特征将通过以下的说明书而变得容易理解。
附图说明
通过参考附图阅读下文的详细描述，本申请示例性实施方式的上述以及其他目的、特征和优点将变得易于理解。在附图中，以示例性而非限制性的方式示出了本申请的若干实施方式，其中：
在附图中，相同或对应的标号表示相同或对应的部分。
图1示出了本申请实施例中位姿估计方法的实现流程示意图一；
图2示出了本申请实施例中位姿估计方法的实现流程示意图二；
图3示出了本申请实施例中位姿估计方法的实现流程示意图三；
图4示出了本申请实施例中位姿估计方法的实现流程示意图四；
图5示出了本申请实施例中地图的效果图；
图6示出了本申请实施例中位姿估计设备的组成结构示意图；
图7示出了本申请实施例中电子设备的组成结构示意图。
具体实施方式
为使本申请的目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而非全部实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
为了使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请作进一步地详细描述，所描述的实施例不应视为对本申请的限制，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。
在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解,“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。
在以下的描述中，所涉及的术语“第一\第二”仅仅是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二”在允许的情况下可以互换特定的顺序或先后次序，以使这里描述的本申请实施例能够以除了在这里图示或描述的以外的顺序实施。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。
应理解，在本申请的各种实施例中，各实施过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
本申请中的位姿估计方法应用于任何需要估计位姿的设备中，如机器人、无人机、物流车、水下探测机器人、诸如汽车的驾驶设备等。本申请中的位姿估计算法，将第t采集时刻的相对位姿量和第p采集时刻的位姿优化量进行结合，得到第t采集时刻的位姿先验量，将第t采集时刻的位姿先验量和第t采集时刻的观测量进行结合，得到待测物体在第t采集时刻的位姿估计量。与相关技术中对算力要求高的位姿估计算法相比，本申请中的位姿估计算法为轻量级的算法，易于实现，对算力要求不高，还可有效避免由于购买运算能力强的硬件而导致的成本增加的问题，节约成本。
图1示出了本申请实施例中位姿估计方法的实现流程示意图一。如图1所示，所述方法包括：
S101：基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量。
待测物体可以为任何可以进行位姿估计的物体。如，机器人、无人机、物流车、水下探测机器人、诸如汽车的驾驶设备等。本申请中可优选待测物体为驾驶设备。
通常，待测物体上设置有惯性测量单元(IMU，Inertial Measurement Unit)。惯性测量单元(IMU，Inertial Measurement Unit)具有一定的采集频率。按照该采集频率，对IMU数据进行采集。采集频率的倒数为采集周期。每个采集周期所处的时刻视为采集时刻。本申请中的第t采集时刻为除了初始采集时刻之外的其他采集时刻。
本步骤中，将每个采集时刻采集到的IMU数据进行记录。通过IMU在第t采集时刻对IMU数据进行采集而获得待测物体在第t采集时刻的IMU数据。通过读取记录的第t采集时刻之前的所有时刻的IMU数据而获得待测物体在第t采集时刻的在前时刻的IMU数据。
通常，IMU包括加速度计和陀螺仪。其中，加速度计用于测量待测物体在空间中的加速度。陀螺仪用于测量待测物体在三维空间中的角速度。采用IMU原理，对第t采集时刻和在前时刻的加速度和角速度进行时间上的积分，可得到第t采集时刻的相对位姿量。其中，关于IMU原理的阐述详见相关说明。
可以理解，第t采集时刻的相对位姿量是一个相对量，指的是第t采集时刻相对于在前时刻在位姿量上产生的相对值。
通过第t采集时刻的IMU数据和在前时刻的IMU数据的结合，可得到准确的相对位姿量。
S102：基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；所述第p采集时刻的位姿优化量是在满足优化条件时，基于M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、和所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的相对位姿量而得到；其中，M为大于或等于1的正整数，所述M个采集时刻早于第p采集时刻；所述目标优化量是通过对所述至少之一位姿估计量进行优化而得到。
在实施时，可将第t采集时刻的相对位姿量和第p采集时刻(第t采集时刻的前一采集时刻)的位姿优化量进行相加操作得到的结果，作为第t采集时刻的位姿先验量。位姿先验量的计算简单，对算力要求低。
本申请中，满足优化条件指的是在每计算出q个采集时刻的位姿估计量的情况下进行一次优化，q为大于或等于2的正整数。示例性地，如每计算出30(如t1-t30)或50(如t1-t30)个采集时刻的位姿估计量的情况下进行一次优化。也就是说，优化条件约定了优化的周期，并非计算出一个采集时刻的位姿估计量就进行优化。优化条件的这种约束，可实现对优化效率的提升，避免频繁优化而带来的优化资源负担重的问题。
第p采集时刻的位姿优化量是在满足优化条件时，对第p采集时刻的位姿估计量进行优化而得到的。进一步地，将M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、与(所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的)相对位姿量进行相加操作，将相加而得的结果作为第p采集时刻的位姿优化量。
本申请中的目标优化量采用基于平滑和建图的因子图优化库(GTSAM，GeorgiaTech Smoothing And Mapping)方法对所述至少之一位姿估计量进行优化而得到。采用GTSAM方法进行优化，可保证目标优化量的准确性，从而保证第p采集时刻的位姿优化量的准确性，进而实现对第t采集时刻的位姿先验量的准确计算。
S103：基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量。
在实施时，将所述第t采集时刻的位姿先验量和第t采集时刻的观测量输入至误差迭代卡尔曼滤波***(ESKF，Error State Kalman Filter)，得到待测物体在第t采集时刻的位姿估计量。
其中，采用ESKF***在计算位姿估计量时，是计算基于误差状态的雅克比矩阵，与相关技术中的位姿估计算法相比，基于误差状态的雅克比矩阵计算简单，对算力要求不高，从而使得通过误差迭代卡尔曼滤波算法而计算出的位姿估计量是一种轻量级的估计算法。可有效节省算力成本和支出成本。
S104：基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位。
本申请中，位姿包括位置和朝向，计算出待测物体在第t采集时刻的位姿估计量，可认为就是计算出了待测物体在第t采集时刻所处环境中的位置和朝向，由此实现了待测物体在其所处环境中的定位。
S101～S104中，将第t采集时刻的相对位姿量和第p采集时刻的位姿优化量进行结合，得到第t采集时刻的位姿先验量的方案；以及，将第t采集时刻的位姿先验量和第t采集时刻的观测量进行结合，得到待测物体在第t采集时刻的位姿估计量的方案为一种对算力要求不高的算法。与相关技术中对算力要求高的位姿估计算法相比，本申请中的位姿估计算法对算力要求不高，为轻量级的算法，易于实现，还可有效避免由于购买运算能力强的硬件而导致的成本增加的问题，节约成本。
此外，由于采用了准确的第p采集时刻的位姿优化量和第t采集时刻的相对位姿量，所以，可得到准确的第t采集时刻的位姿先验量。并基于准确的位姿先验量和准确的观测量，得到准确的位姿估计量，从而实现对待测物体的位姿的准确估计。
在一些实施例中，前述S104基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位的方案通过图2所示的方案来实现：
S201：基于第t采集时刻的多个在前时刻的位姿估计量的目标优化量，对地图进行更新，所述地图为针对待测物体在多个所述在前时刻所处环境而建立。
在实施时，待测物体上设置有雷达(radar)或激光雷达(liar)。激光雷达可对待测物体所处环境中的点云数据进行采集，并根据点云数据进行(环境)地图的构建。如，可根据第t采集时刻的多个在前时刻采集到的点云数据，对地图的构建。待测物体是运动的，随着时间的推移，其所处的环境也是随之发生变化。在第t采集时刻到来之前，采用GTSAM方法对多个在前时刻的位姿估计量进行优化，得到各在前时刻的位姿估计量的目标优化量。在第t采集时刻到来时或到来之前，可根据多个在前时刻的位姿估计量的目标优化量，对地图进行更新。如，将原本显示在地图中的各在前时刻的位姿估计量调整为各在前时刻的位姿估计量的目标优化量，以使得待测物体在第t采集时刻到来之前或到来时，以优化后的位姿在地图中显示。如果将显示优化位姿后的地图视为更新后的地图，则在第t采集时刻到来之前或到来时，待测物体的各在前时刻的位姿是以各在前时刻的优化后位姿(位姿估计量的目标优化量)显示在更新后的地图中的。
S202：基于第t采集时刻的位姿估计量和更新后的地图，对待测物体进行定位。
本步骤中，计算出待测物体在第t采集时刻所处环境中的位置和朝向，实现了待测物体在其所处环境中的定位。此外，因为本申请可对待测物体所处环境进行建图、以及对地图进行更新，所以，在计算出待测物体在第t采集时刻的位姿估计量的情况下，还可将该位姿估计量显示于在第t采集时刻到来之前或到来时而得到的最新地图(更新后的地图)中，以进行第t采集时刻的位姿的显示。
从S201～S202中可看出，本申请技术方案，既是一种对待测物体进行位姿定位的方案，也是一种构建地图的方案。由此可见，本申请技术方案可视为一种SLAM方案，可解决待测物体在未知环境中的定位问题。这种方案，实现了地图的适时更新以及位姿估计量的优化，进而实现了对待测物体的位姿的准确定位。
在本申请的SLAM方案中，采用误差迭代卡尔曼滤波***计算位姿估计量，实现了轻量级的位姿估计。同时，采用GTSAM方法对位姿估计量进行优化，可提高优化准确性。由此可见，本申请的SLAM方案是一种算力低、精度高、鲁棒性好的即时定位与地图构建方案。
在一些实施例中，在执行S103基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量的步骤之前，还需要获得第t采集时刻的观测量。结合图3所示，第t采集时刻的观测量的获得方式包括：
S301：获得第t采集时刻的目标点云数据；
本申请中，采用雷达或激光雷达对待测物体所处环境进行点云数据的扫描。雷达或激光雷达具有一定的扫描频率。按照该扫描频率，对点云数据进行扫描。扫描频率的倒数为扫描周期。每个扫描周期所处的时刻视为扫描时刻。
在实施时，获得由雷达扫描到的第t采集时刻的初始点云数据。如，读取雷达在第t采集时刻采集到的点云数据，将该点云数据作为第t采集时刻的初始点云数据。将第t采集时刻的初始点云数据进行点云预处理以及点云畸变处理，得到第t采集时刻的目标点云数据。其中，点云预处理包括点云数据归一化、滤波(滤除噪声)、降采样等。点云畸变处理包括点云畸变检测、点云畸变校正等。
本申请中，经过点云预处理以及点云畸变处理后的点云数据，用于对地图进行构建。利用点云预处理以及点云畸变处理后的点云数据，可实现对地图的准确构建。
还可以，将第t采集时刻的初始点云数据进行点云预处理，得到第一点云数据。基于第t采集时刻的位姿先验量，从第一点云数据中筛选出第二点云数据。对第二点云数据进行点云畸变处理，得到第t采集时刻的目标点云数据。
本申请中，待测物体是运动的，雷达或激光雷达也是运动的，第一点云数据包括针对运动的待测物体的点云，也包括针对运动的雷达的点云。运动的待测物体的点云会对地图构建存在影响，所以需要从第一点云数据中，删除针对运动的待测物体的点云数据。运动的待测物体的点云数据是能够体现第t采集时刻的位姿先验量的点云数据。从第一点云数据中，删除能够体现第t采集时刻的位姿先验量的点云数据，得到的是针对运动的雷达的点云数据，即，得到第二点云数据。
S302：针对所述目标点云数据中的任意点，从地图中，确定与所述任意点对应的多个期望点；其中，所述地图为在早于第t采集时刻的时刻针对待测物体所处环境而建立，所述期望点为地图中与所述任意点满足第一条件和第二条件的点，所述第一条件包括各期望点与所述任意点之间的距离均小于或等于第一阈值，所述第二条件包括多个期望点能够构成平面。
从在第t采集时刻之前构建的地图中，查找出与任意点之间的距离均小于或等于第一阈值、且能够构成平面的多个期望点。
S303：基于所述目标点云数据中的各点以及所述各点对应的多个期望点，得到第t采集时刻的观测量。
将目标点云数据中的各点到由各点对应的多个期望点构成的平面的距离，作为第t采集时刻的观测量。
S301～S303所示的观测量获得方法，在工程上易行，可行性高。能够为本申请轻量级的位姿估计方案提供一定程度的辅助作用。
下面结合图4和图5对本申请技术方案做进一步说明。
为清楚说明，本应用场景中以待测物体为驾驶设备、优化条件为在每计算出q＝5个采集时刻的位姿估计量的情况下进行一次优化为例。本应用场景中的说明仅是一种举例而已，在实际应用中使用的q的取值可能远比5大，但方案执行类似，参见理解即可。
IMU测量驾驶设备在第tq＝t5采集时刻的加速度值和角速度值。读取第tq＝t5采集时刻之前的全部采集时刻(t1～t4)的加速度值和角速度值。根据IMU原理，对第t5采集时刻和在前时刻的加速度和角速度进行时间上的积分，可得到第t5采集时刻的相对位姿量。
将第t5采集时刻的相对位姿量和第tp＝tq-1＝t4采集时刻的位姿优化量进行相加，相加结果作为第t5采集时刻的位姿先验量。位姿先验量的这种计算简单，为本申请轻量级的位姿估计方案提供了一定的支持。其中，因为设定的优化条件是每计算出q＝5个采集时刻的位姿估计量的情况下进行一次优化，所以第t4采集时刻的位姿估计量还未到优化时刻、未被优化。基于此，将第t4采集时刻的位姿优化量取值为第t4采集时刻的位姿估计量，以完成第t5采集时刻的位姿先验量的计算。示例性地，将第t4采集时刻的位姿估计量和第t5采集时刻的相对位姿量相加，相加结果作为第t5采集时刻的位姿先验量使用。
以上位姿先验量的获得过程为图4中的正向传播模块所执行的流程。因为待测物体的先验量可体现待测物体的运动信息，通过图4中的反向传播模块实现某个采集时刻的待测物体的运动信息，从IMU侧到雷达侧的传播。以使得待测物体可根据某个采集时刻的待测物体的运动信息，从雷达扫描并经点云预处理后的点云数据中，将表征待测物体的运动信息的点云数据删除掉，以避免待测物体的运动对雷达运动的影响。
将第t5采集时刻的位姿先验量和第t5采集时刻的观测量输入至ESKF***。可以理解，ESKF***是一种迭代运算，通过迭代运算，来进行位姿估计量的计算。ESKF***的观测方程和计算误差状态的更新过程请参见公式(1)～公式(4)。
假定ESKF***的观测方程是抽象的h(x)：
z＝h(x)+v v～N(0，V) (1)
其中，z为观测量；v为观测噪声，符合随机分布概率N；V为该噪声的协方差矩阵。
ESKF***计算误差状态的更新过程是：
K＝PpredHT(HPpredHT+V)-1； (2)
Δx＝K(z-h(xt))； (3)
P＝(I-KH)Ppred； (4)
其中，K为卡尔曼增益；H为观测方程相对于误差状态的雅可比矩阵，通过链式法则计算而得到。Ppred为***的协方差矩阵。P通过对预测的***协方差矩阵Ppred进行修正后的***的协方差矩阵。
本申请中，ESKF***可根据第t采集时刻和在前采集时刻的IMU数据，计算出***的协方差矩阵Ppred以及雅克比矩阵H。在计算出协方差矩阵Ppred和H、以及已知V的情况下，根据公式(2)，计算出卡尔曼增益。将第t5采集时刻的位姿先验量作为公式(3)中的h(xt)、将第t5采集时刻的观测量作为公式(3)中的z，根据公式(3)更新***的状态量Δx。同时，按照公式(4)更新***的协方差矩阵。将***状态量Δx计算得到的四元数进行归一化处理，再将四元数转化为方便位姿显示的欧拉角，以此表示待测物体的位姿状态，ESFK***将位姿状态作为位姿估计量输出。
即，在将第t5采集时刻的位姿先验量和第t5采集时刻的观测量输入至ESKF***，ESKF***经如上过程的计算，得到并输出第t5采集时刻的位姿估计量。
可以看出，采用误差迭代卡尔曼滤波***在计算位姿估计量时，是计算基于误差状态的雅克比矩阵，与相关技术中的位姿估计算法相比，基于误差状态的雅克比矩阵计算简单，对算力要求不高，从而使得通过误差迭代卡尔曼滤波算法而计算出的位姿估计量是一种轻量级的估计算法。可有效节省算力成本和支出成本。
在前述方案中，第t5采集时刻的观测量的获得过程参见以下所述：
可以理解，在第t5采集时刻到来之前，如针对雷达扫描到的第t1采集时刻的初始点云数据，将该采集时刻的初始点云数据进行点云预处理、删除除雷达之外的其他运动物体如待测物体的运动点云、点云畸变处理等步骤之后，进行待测物体所处环境的地图的构建。如，将第t1采集时刻经过如上步骤之后而得到的点云数据映射到全局坐标系中，以实现对待测物体所处环境的地图的构建。此外，还可以将计算出的第t1采集时刻的位姿估计量显示在地图中，以实现待测物体在第t1采集时刻的定位。
第t5采集时刻到来时，使用的地图可以是根据第t1采集时刻经如上步骤处理后而构建的地图，还可以是通过第t2～t4采集时刻的点云数据对第t1采集时刻构建的地图进行更新后的地图。以第t5采集时刻到来时需要使用的地图是第t1采集时刻经如上步骤处理后而构建的地图为例。
雷达按照其扫描频率进行点云数据的扫描，从雷达扫描到的点云数据中，读取雷达扫描到的第t5采集时刻的点云数据作为第t5采集时刻的初始点云数据。将初始点云数据进行点云预处理，得到第t5采集时刻的第一点云数据。第t5采集时刻的位姿先验量能够体现待测物体的运动状态。从第一点云数据中，删除能够体现待测物体的运动状态的点云数据，从而得到是针对运动的雷达的(第二)点云数据。针对第t5采集时刻的第二点云数据中的各点云，从第t5采集时刻到来时可以使用的地图中，为各点云查找出一定数量的期望点。以第二点云数据中的点云1、搜索或查找出5个期望点为例，这5个期望点需要是地图中满足以下两个条件(第一条件和第二条件)的点。具体的，5个期望点中的每个点是与点云1距离近的点。即，每个点与点云1之间的距离均小于或等于第一阈值(满足第一条件)。5个期望点可构成一平面(满足第二条件)。
为第二点云数据中的各点云找出5个这样的期望点。将各点云到由各点云对应的5个期望点构成的平面的距离，作为第t采集时刻的观测量，输入至ESKF***中。
图4中的残差计算模块，用于计算各点云到由各点云对应的5个期望点构成的平面的距离，并将计算出的距离作为残差，输入至ESKF***中。
待测物体利用第t5采集时刻计算出的位姿估计量，对其在自身所处的环境中进行位置和朝向的定位。也可将第t5采集时刻的位姿估计量显示在地图中，通过地图来呈现待测物体的位姿和朝向，以实现对待测物体的定位。
前述方案是对待测物体的第t5采集时刻的位姿进行估计的方案。采用ESKF***对位姿进行估计时，ESKF***是计算基于误差状态的雅克比矩阵，计算简单，对算力要求不高，为本申请位姿估计的轻量级计算提供了支持。
此外，由于ESKF***的稳定性和准确性，可保证对位姿估计量的准确性，从而实现了对待测物体的精准定位。
按照优化条件的设置或约束，在利用前述方案，计算出第t5采集时刻的位姿估计量的情况下，可对第t1～t5采集时刻的位姿估计量进行优化。
对于第t1～t4采集时刻计算出的位姿估计量进行缓存。在计算出第t5采集时刻的位姿估计量的情况下，采用GTSAM方法对计算出的第t5采集时刻的位姿估计量和在前缓存的第t1～t4采集时刻的位姿估计量进行优化。
可以理解，GTSAM方法是基于因子图实现的优化。在因子图中，涉及到节点和边的概念。结合本方案，各采集时刻的位姿估计量可视为因子图中的各节点。采集时刻的位姿估计量之间的约束方程可视为因子图中的边。其中，约束方程包括但不限定于：相邻两个或多个采集时刻的位姿估计量相差较小，如小于或等于第二阈值。某个或某些采集时刻的位姿估计量的协方差较小，如小于或等于第三阈值。利用因子图实现优化，意在不断调整各采集时刻的位姿估计量，使得各采集时刻的位姿估计量能够满足约束方程的约束。在满足约束方程时，经调整而得到的各采集时刻的位姿估计量即可视为各采集时刻的位姿优化量。经调整而得到的各采集时刻的位姿优化量的协方差可视为各采集时刻的优化协方差。
本申请中，第t1～t5采集时刻的位姿估计量经过上述GTSAM方法的优化，可得到第t1～t5采集时刻的位姿优化量。将显示在地图中的各采集时刻的位姿估计量调整各采集时刻的位姿优化量，以实现对地图的更新。本申请中的第一阈值、第二阈值和第三阈值，根据实际情况而灵活设定。
在待测物体如驾驶设备包括前端和后端的情况下，采用ESKF***进行位姿估计的方案可以由驾驶设备的前端完成。采用GTSAM方法对位姿估计量进行优化的方案可以由驾驶设备的后端完成。如此，本申请提供了一种前端轻量级的位姿计算，后端轻量级的位姿优化的方案。该方案，对算力要求不高，可有效节省支出成本。且，考虑到ESKF***和GTSAM方法具有很强的稳定性，所以利用ESKF***和GTSAM方法可实现对位姿的准确估计和优化。
通俗来讲，本申请中，前端对位姿进行估计，后端对估计位姿进行优化。与前端进行位姿估计耗费的时间相比，后端对位姿估计进行一次优化的耗费时间长。如，在后端进行一次优化的时间里，前端可能完成了多个采集时刻的位姿估计。示例性地，在后端进行第t1～t5采集时刻的位姿估计量这样一次优化的时间里，前端可能完成了第t6～t9采集时刻的位姿估计量的计算。
如图4所示，后端记录来自前端的第t1～t5采集时刻的位姿估计量的同时，还记录各位姿估计量对应的时间戳。在已知各位姿估计量对应的时间戳和一次优化时间耗费时长的情况下，估算出一次优化完成后，前端完成几个采集时刻的位姿估计，后端需要为前端反馈哪个采集时刻的位姿优化量。
可以理解，接下来，前端应该计算第t10采集时刻的位姿估计量。计算第t10采集时刻的位姿估计量需要使用到第t9采集时刻的位姿优化量。本申请中，第t9采集时刻的位姿优化量是通过第t9采集时刻和第t5采集时刻的位姿估计量之间的相对(位姿)量、与第t5采集时刻的位姿优化量而得到。具体的，在已知第t9采集时刻和第t5采集时刻的位姿估计量的情况下，后端计算该两个时刻的位姿估计量的差值，该差值可视为该两个时刻的位姿估计量的相对(位姿)量。将该差值与第t5采集时刻的位姿优化量相加，得到的结果即可视为第t9采集时刻的位姿优化量。后端将第t9采集时刻的位姿优化量和第t9采集时刻的优化协方差反馈至前端。
其中，第t9采集时刻的优化协方差是这样得来的：该两个时刻的位姿估计量的协方差的差值，将该差值与第t5采集时刻的位姿优化量的协方差相加，得到的结果即可视为第t9采集时刻的优化协方差。
如图4所示，本申请中的ESKF***包括卡尔曼观测迭代更新模块和卡尔曼状态更新模块。其中，卡尔曼观测迭代更新模块用于对***状态量Δx进行更新。卡尔曼状态更新模块用于接收来自后端的优化协方差，并将该优化协方差提供至正向传播模块，以供进行下一时刻位姿先验量的计算。
前端读取IMU测量到的第t10采集时刻及其在前全部采集时刻(t1～t9)的加速度值和角速度值。根据IMU原理，对第t10采集时刻和在前时刻的加速度和角速度进行时间上的积分，可得到第t10采集时刻的相对位姿量。将第t10采集时刻的相对位姿量和第t9采集时刻的位姿优化量进行相加，相加结果作为第t10采集时刻的位姿先验量。将第t10采集时刻的位姿先验量和第t10采集时刻的观测量输入至ESKF***。ESKF***采用公式(1)～(4)计算，得到并输出第t10采集时刻的位姿估计量。
其中，第t10采集时刻的观测量的获得过程请参见前述对第t5采集时刻的观测量的获得过程的描述，重复之处不赘述。
可以理解，在前端计算出第t10采集时刻的位姿估计量的情况下，满足优化条件，后端采用GTSAM方法对第t6～t10采集时刻的位姿估计量进行优化，得到各采样时刻的位姿优化量，然后对地图进行更新。以此类推，前端完成各采集时刻的位姿估计量的计算，后端在满足优化条件下实现对位姿估计量的优化，并更新地图，使在地图中显示待测物体在各采样时刻的位姿。
可以理解，如果某个采样时刻的位姿优化量被计算出，则地图中可显示该采集时刻的位姿优化量。在未被计算出时，地图中可显示该采集时刻的位姿估计量。
图4中的回环检测模块，用于识别待测物体在某个采集时刻上是否回到曾经走过的位置，以实现回环检测技术。如果经识别发现，某个采集时刻上回到了曾经走过的位置，则可将曾经走过的位置和该采集时刻所处位置之间的距离尽量小，作为除前述约束条件之外的附加约束条件，加入至GTSAM方法的因子图中。以通过更多的约束条件，来实现对位姿优化量的计算。
本申请中，对待测物体所处环境进行建图，以及对已构建地图进行更新，以达到将待测物体的实时位姿显示在待测物体所处的实时环境中。采用本申请技术方案构建或更新的地图的效果如图5所示。考虑雷达扫描角度的有限性，所构建或更新的地图是待测物体所处环境周围一定范围内的地图。随着待测物体的运动，地图中显示的环境也随着待测物体运动的环境进行改变。即，无论何时，地图中显示的是待测物体在实时所处环境中的位姿。
从前述方案可看出，本申请是前端采用ESKF***进行轻量级位姿估计，后端是采用GTSAM方法对位姿进行准确优化的方案。这种方案，因为是轻量级，所以对算力要求没有相关技术中的高，可视为一种低算力方案。因为ESKF***和GTSAM方法具有很强的稳定性，所以，位姿估计和位姿优化的精度高，鲁棒性强。由此可见，本申请提供了一种低算力、高精度、强鲁棒性的位姿估计和优化方案。
本申请中，提供两个线程：第一线程和第二线程。其中，第一线程处于前端，至少用于采用ESKF***进行轻量级位姿估计。第二线程处于后端，用于采用GTSAM方法对位姿进行准确优化、对地图进行构建和更新、以及执行回环检测技术等。两个线程之间独立，互不干扰，可保证各自的准确性。
在硬件上，实现本申请的低算力、高精度、强鲁棒性的位姿估计和优化方案，需要图像处理器(GPU)、中央处理器(CPU)、DL加速器、视觉加速器、显存和存储器等硬件的支撑。由于本申请技术方案是轻量级的计算方案，所以，对这些硬件的性能和数量等均要求不高。普通性能和/或少数量的这些硬件即可实现本申请的技术方案。从硬件支撑来看，本申请技术方案也可视为轻量级的方案，可避免购买性能高、数量多的硬件而导致的购买支出增加的问题，节省支出成本。
当本申请技术方案应用于驾驶设备时，前端可对驾驶设备在驾驶过程中所处的位置和朝向(位姿)进行估计并定位。针对前端估计的位姿，每满足一次优化条件时，对前端估计出的位姿进行一次优化，以保证对地图构建或更新的准确化。同时，方便前端采用对在前采集时刻优化后的位姿，计算后续采集时刻的位姿，可提高对不同采集时刻的驾驶设备的定位精准性。
本申请提供一种位姿估计设备，如图6所示，所述设备包括：
第一获得单元701，用于基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量；
第二获得单元702，用于基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；
第三获得单元703，用于基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量；
定位单元704，用于基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位；
其中，所述第p采集时刻的位姿优化量是在满足优化条件时，基于M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、和所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的相对位姿量而得到；其中，M为大于或等于1的正整数，所述M个采集时刻早于第p采集时刻；所述目标优化量是通过对所述至少之一位姿估计量进行优化而得到。
在一些实施例中，所述定位单元704，用于：
基于第t采集时刻的多个在前时刻的位姿估计量的目标优化量，对地图进行更新，所述地图为针对待测物体在多个所述在前时刻所处环境而建立；
基于第t采集时刻的位姿估计量和更新后的地图，对待测物体进行定位。
在一些实施例中，所述第三获得单元703，用于：
获得第t采集时刻的目标点云数据；
针对所述目标点云数据中的任意点，从地图中，确定与所述任意点对应的多个期望点；
其中，所述地图为在早于第t采集时刻的时刻针对待测物体所处环境而建立，所述期望点为地图中与所述任意点满足第一条件和第二条件的点，所述第一条件包括各期望点与所述任意点之间的距离均小于或等于第一阈值，所述第二条件包括多个期望点能够构成平面；
基于所述目标点云数据中的各点以及所述各点对应的多个期望点，得到第t采集时刻的观测量。
在一些实施例中，所述第三获得单元703，用于：获得由雷达扫描到的第t采集时刻的初始点云数据；
将第t采集时刻的初始点云数据进行点云预处理以及点云畸变处理，得到第t采集时刻的目标点云数据。
在一些实施例中，所述第三获得单元703，用于：
将第t采集时刻的初始点云数据进行点云预处理，得到第一点云数据；
基于第t采集时刻的位姿先验量，从第一点云数据中筛选出第二点云数据；
对第二点云数据进行点云畸变处理，得到第t采集时刻的目标点云数据。
在一些实施例中，所述第二获得单元702，用于：
将相对位姿量和第p采集时刻的位姿优化量进行相加操作得到的结果，作为第t采集时刻的位姿先验量。
在一些实施例中，所述第三获得单元703，用于：
将所述第t采集时刻的位姿先验量和第t采集时刻的观测量输入至误差迭代卡尔曼滤波***，得到待测物体在第t采集时刻的位姿估计量。
在一些实施例中，所述目标优化量采用基于平滑和建图的因子图优化库(gtsam)方法对所述至少之一位姿估计量进行优化而得到。
需要说明的是，本申请实施例的位姿估计设备，由于该位姿估计设备解决问题的原理与前述的位姿估计方法相似，因此，位姿估计设备的实施过程及实施原理均可以参见前述方法的实施过程及实施原理描述，重复之处不再赘述。
根据本申请的实施例，本申请还提供了一种电子设备和一种可读存储介质。
所述电子设备包括：至少一个处理器；以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行前述的位姿估计方法。
针对存储有计算机指令的非瞬时计算机可读存储介质，所述计算机指令用于使所述计算机执行前述的位姿估计方法。
本申请提供一种驾驶设备，包括前述的位姿估计设备。
图7示出了可以用来实施本申请的实施例的示例电子设备800的示意性框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本申请的实现。
如图7所示，设备800包括计算单元801，其可以根据存储在只读存储器(ROM)802中的计算机程序或者从存储单元808加载到随机访问存储器(RAM)803中的计算机程序，来执行各种适当的动作和处理。在RAM 803中，还可存储设备800操作所需的各种程序和数据。计算单元801、ROM 802以及RAM 803通过总线804彼此相连。输入/输出(I/O)接口805也连接至总线804。
设备800中的多个部件连接至I/O接口805，包括：输入单元806，例如键盘、鼠标等；输出单元807，例如各种类型的显示器、扬声器等；存储单元808，例如磁盘、光盘等；以及通信单元809，例如网卡、调制解调器、无线通信收发机等。通信单元809允许设备800通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元801可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元801的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元801执行上文所描述的各个方法和处理，例如位姿估计方法。例如，在一些实施例中，位姿估计方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元808。在一些实施例中，计算机程序的部分或者全部可以经由ROM 802和/或通信单元809而被载入和/或安装到设备800上。当计算机程序加载到RAM 803并由计算单元801执行时，可以执行上文描述的位姿估计方法的一个或多个步骤。备选地，在其他实施例中，计算单元801可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行位姿估计方法。
本文中以上描述的***和技术的各种实施方式可以在数字电子电路***、集成电路***、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上***的***(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程***上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储***、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储***、该至少一个输入装置、和该至少一个输出装置。
用于实施本申请的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本申请的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行***、装置或设备使用或与指令执行***、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体***、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的***和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的***和技术实施在包括后台部件的计算***(例如，作为数据服务器)、或者包括中间件部件的计算***(例如，应用服务器)、或者包括前端部件的计算***(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的***和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算***中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将***的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机***可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式***的服务器，或者是结合了区块链的服务器。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims (12)

1.一种位姿估计方法，其特征在于，包括：
基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量；
基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；
基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量；
基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位；
其中，所述第p采集时刻的位姿优化量是在满足优化条件时，基于M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、和所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的相对位姿量而得到；其中，M为大于或等于1的正整数，所述M个采集时刻早于第p采集时刻；所述目标优化量是通过对所述至少之一位姿估计量进行优化而得到。
2.根据权利要求1所述的方法，其特征在于，所述基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位，包括：
基于第t采集时刻的多个在前时刻的位姿估计量的目标优化量，对地图进行更新，所述地图为针对待测物体在多个所述在前时刻所处环境而建立；
基于第t采集时刻的位姿估计量和更新后的地图，对待测物体进行定位。
3.根据权利要求1或2所述的方法，其特征在于，所述第t采集时刻的观测量的获得方式包括：
获得第t采集时刻的目标点云数据；
针对所述目标点云数据中的任意点，从地图中，确定与所述任意点对应的多个期望点；
其中，所述地图为在早于第t采集时刻的时刻针对待测物体所处环境而建立，所述期望点为地图中与所述任意点满足第一条件和第二条件的点，所述第一条件包括各期望点与所述任意点之间的距离均小于或等于第一阈值，所述第二条件包括多个期望点能够构成平面；
基于所述目标点云数据中的各点以及所述各点对应的多个期望点，得到第t采集时刻的观测量。
4.根据权利要求3所述的方法，其特征在于，所述获得第t采集时刻的目标点云数据，包括：
获得由雷达扫描到的第t采集时刻的初始点云数据；
将第t采集时刻的初始点云数据进行点云预处理以及点云畸变处理，得到第t采集时刻的目标点云数据。
5.根据权利要求4所述的方法，其特征在于，所述将第t采集时刻的初始点云数据进行点云预处理以及点云畸变处理，得到第t采集时刻的目标点云数据，包括：
将第t采集时刻的初始点云数据进行点云预处理，得到第一点云数据；
基于第t采集时刻的位姿先验量，从第一点云数据中筛选出第二点云数据；
对第二点云数据进行点云畸变处理，得到第t采集时刻的目标点云数据。
6.根据权利要求1或2所述的方法，其特征在于，所述基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量，包括：
将相对位姿量和第p采集时刻的位姿优化量进行相加操作得到的结果，作为第t采集时刻的位姿先验量。
7.根据权利要求1或2所述的方法，其特征在于，所述基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量，包括：
将所述第t采集时刻的位姿先验量和第t采集时刻的观测量输入至误差迭代卡尔曼滤波***，得到待测物体在第t采集时刻的位姿估计量。
8.根据权利要求1或2所述的方法，其特征在于，所述目标优化量采用基于平滑和建图的因子图优化库GTSAM方法对所述至少之一位姿估计量进行优化而得到。
9.一种位姿估计设备，包括：
第一获得单元，用于基于待测物体在第t采集时刻及其在前时刻的惯性测量单元IMU数据，得到待测物体在第t采集时刻的相对位姿量；
第二获得单元，用于基于所述相对位姿量和第p采集时刻的位姿优化量，得到第t采集时刻的位姿先验量；其中，t和p均为正数，且第p采集时刻为第t采集时刻的前一采集时刻；
第三获得单元，用于基于第t采集时刻的位姿先验量和第t采集时刻的观测量，得到待测物体在第t采集时刻的位姿估计量；
定位单元，用于基于待测物体在第t采集时刻的位姿估计量，对待测物体进行定位；
其中，所述第p采集时刻的位姿优化量是在满足优化条件时，基于M个采集时刻的位姿估计量中的至少之一位姿估计量的目标优化量、和所述至少之一位姿估计量与第p采集时刻的位姿估计量之间的相对位姿量而得到；其中，M为大于或等于1的正整数，所述M个采集时刻早于第p采集时刻；所述目标优化量是通过对所述至少之一位姿估计量进行优化而得到。
10.一种驾驶设备，其特征在于，包括权利要求9所述的位姿估计设备。
11.一种电子设备，其特征在于，所述电子设备包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-8中任一项所述的方法。
12.一种存储有计算机指令的非瞬时计算机可读存储介质，其特征在于，所述计算机指令用于使所述计算机执行根据权利要求1-8中任一项所述的方法。
CN202310803048.5A 2023-06-30 2023-06-30 位姿估计方法、相关设备及存储介质 Pending CN116929343A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310803048.5A CN116929343A (zh) 2023-06-30 2023-06-30 位姿估计方法、相关设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310803048.5A CN116929343A (zh) 2023-06-30 2023-06-30 位姿估计方法、相关设备及存储介质

Publications (1)

Publication Number Publication Date
CN116929343A true CN116929343A (zh) 2023-10-24

Family

ID=88374754

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310803048.5A Pending CN116929343A (zh) 2023-06-30 2023-06-30 位姿估计方法、相关设备及存储介质

Country Status (1)

Country Link
CN (1) CN116929343A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117270487A (zh) * 2023-11-17 2023-12-22 北京芯驰半导体科技有限公司 模拟信号的采样控制***、方法及芯片

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117270487A (zh) * 2023-11-17 2023-12-22 北京芯驰半导体科技有限公司 模拟信号的采样控制***、方法及芯片
CN117270487B (zh) * 2023-11-17 2024-01-23 北京芯驰半导体科技有限公司 模拟信号的采样控制***、方法及芯片

Similar Documents

Publication Publication Date Title
CN110178048B (zh) 交通工具环境地图生成和更新的方法和***
CN115265523B (zh) 机器人同时定位与建图方法、装置及可读介质
CN110197615B (zh) 用于生成地图的方法及装置
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
EP3852065A1 (en) Data processing method and apparatus
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
JP2014523572A (ja) 地図データの生成
CN116929343A (zh) 位姿估计方法、相关设备及存储介质
CN115457152A (zh) 外参标定方法、装置、电子设备及存储介质
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与***
CN113177980B (zh) 用于自动驾驶的目标对象速度确定方法、装置及电子设备
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及***
CN113495281B (zh) 可移动平台的实时定位方法及装置
CN115239899B (zh) 位姿图生成方法、高精地图生成方法和装置
CN115984463A (zh) 一种适用于狭窄巷道的三维重建方法和***
CN114861725A (zh) 一种目标感知跟踪的后处理方法、装置、设备及介质
CN112880675A (zh) 用于视觉定位的位姿平滑方法、装置、终端和移动机器人
CN112698374B (zh) 基于bds-3/磁力计的多频双天线车载测姿方法和设备
CN116817928B (zh) 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN117292118B (zh) 雷达引导光电跟踪坐标补偿方法、装置、电子设备及介质
CN117308925B (zh) 一种频谱地图惯导组合的导航方法、装置、设备和介质
CN117687006A (zh) 激光雷达到惯性测量单元的外参标定方法及其装置
CN118225110A (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