CN115371689A - 自动驾驶车辆的融合定位方法、装置及电子设备 - Google Patents

Abstract

本申请公开了一种自动驾驶车辆的融合定位方法、装置及电子设备，该方法包括：获取第一传感器量测数据，并以此从共享缓冲区获取对应的第一滤波器状态数据，共享缓冲区用于缓存预设时间长度的滤波器状态数据，第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；根据第一传感器量测数据和第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行融合更新得到第二滤波器状态数据，以此确定最终融合定位结果，预设更新策略包括对延时的传感器量测数据的融合策略。本申请通过设置共享缓冲区作为数据传输通道，提高了融合定位效率，采用预设更新策略进行融合更新，充分利用了各种延时的传感器量测数据，提高了融合定位精度和稳定性。

Description

自动驾驶车辆的融合定位方法、装置及电子设备

技术领域

本申请涉及自动驾驶技术领域，尤其涉及一种自动驾驶车辆的融合定位方法、装置及电子设备。

背景技术

自动驾驶定位系统一般会对多个传感器的量测信息进行融合实现定位，例如IMU(Inertial measurement unit，惯性测量单元)、GNSS(Global Navigation SatelliteSystem，全球导航卫星系统)、激光雷达、视觉相机等。由于自动驾驶处理器需要同时处理感知、决策和规划等其他非定位模块的数据，定位模块也会由于算力的影响，增加计算时间，从而给到融合定位算法的量测信息往往是延时滞后的定位信息。

目前针对延时的量测信息的处理方式为：若收到量测信息的时间比量测信息的触发时间大一定时间(比如0.5s)以上，则放弃本次收到的量测信息，等待下一次量测信息的到来，再次循环判断。

然而，这种暴力移除的方法，虽然避免了时间延迟带来的影响，但其在占用了自动驾驶处理器算力的基础上，并没有充分利用到所有的传感器量测信息，进而会影响融合定位系统的精度以及稳定性。

发明内容

本申请实施例提供了一种自动驾驶车辆的融合定位方法、装置及电子设备，以提高自动驾驶车辆的融合定位精度和稳定性。

本申请实施例采用下述技术方案：

第一方面，本申请实施例提供一种自动驾驶车辆的融合定位方法，其中，所述方法包括：

获取第一传感器量测数据；

根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

可选地，在获取第一传感器量测数据之后，所述方法还包括：

确定所述第一传感器量测数据的接收时刻和产生时刻；

根据所述接收时刻与所述产生时刻之间的时间间隔，确定所述第一传感器量测数据是否可用；

在所述第一传感器量测数据不可用且不可用的持续时间达到预设持续时间阈值的情况下，关闭所述第一传感器量测数据对应的传感器模块；

在所述第一传感器量测数据可用的情况下，根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据。

可选地，在根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据之前，所述方法包括：

获取IMU原始测量数据；

基于所述IMU原始测量数据，利用所述第一扩展卡尔曼滤波器进行状态预测，得到所述第一滤波器状态数据。

可选地，所述根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据包括：

确定所述第一传感器量测数据的接收时刻和产生时刻；

从所述共享缓冲区获取所述第一传感器量测数据的接收时刻和产生时刻之间对应的第一滤波器状态数据。

可选地，所述根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据包括：

在将时间更新到当前时刻的过程中，确定是否获取到第二传感器量测数据；

若获取到所述第二传感器量测数据，则确定所述第二传感器量测数据的产生时刻，并根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新；

所述根据所述第二滤波器状态数据确定当前时刻的融合定位结果包括：

若没有获取到所述第二传感器量测数据，则直接根据所述第二滤波器状态数据更新所述共享缓冲区。

可选地，所述根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新包括：

若所述第二传感器量测数据的产生时刻晚于所述第一传感器量测数据的产生时刻，则确定需要重新进行量测更新；

若所述第二传感器量测数据的产生时刻早于所述第一传感器量测数据的产生时刻，则确定不需要重新进行量测更新。

可选地，所述根据所述第二滤波器状态数据确定当前时刻的融合定位结果包括：

根据所述第二滤波器状态数据，通过所述第二扩展卡尔曼滤波器更新所述共享缓冲区；

通过所述第一扩展卡尔曼滤波器从更新后的共享缓冲区中获取更新后的滤波器状态数据；

根据更新后的滤波器状态数据更新所述第一滤波器状态数据，并输出当前时刻的融合定位结果。

第二方面，本申请实施例还提供一种自动驾驶车辆的融合定位装置，其中，所述装置包括：

第一获取单元，用于获取第一传感器量测数据；

第二获取单元，用于根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

第一更新单元，用于根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

第一确定单元，用于根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

第三方面，本申请实施例还提供一种电子设备，包括：

处理器；以及

被安排成存储计算机可执行指令的存储器，所述可执行指令在被执行时使所述处理器执行前述之任一所述方法。

第四方面，本申请实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储一个或多个程序，所述一个或多个程序当被包括多个应用程序的电子设备执行时，使得所述电子设备执行前述之任一所述方法。

本申请实施例采用的上述至少一个技术方案能够达到以下有益效果：本申请实施例的自动驾驶车辆的融合定位方法，先获取第一传感器量测数据；然后根据第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，共享缓冲区用于缓存预设时间长度的滤波器状态数据，第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；之后根据第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，预设更新策略包括对延时的传感器量测数据的融合策略；最后根据第二滤波器状态数据确定当前时刻的融合定位结果。本申请实施例的自动驾驶车辆的融合定位方法通过设置共享缓冲区作为数据传输通道，提高了融合定位效率，且采用一定的更新策略进行量测更新和时间更新，充分利用了各种延时的传感器量测数据，避免了暴力融合或者移除带来的观测量损失，提高了融合定位的精度和稳定性。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本申请实施例中一种自动驾驶车辆的融合定位方法的流程示意图；

图2为本申请实施例中一种多传感器量测数据的融合时序示意图；

图3为本申请实施例中一种自动驾驶车辆的融合定位流程示意图；

图4为本申请实施例中一种自动驾驶车辆的融合定位装置；

图5为本申请实施例中一种电子设备的结构示意图。

具体实施方式

为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

以下结合附图，详细说明本申请各实施例提供的技术方案。

本申请实施例提供了一种自动驾驶车辆的融合定位方法，如图1所示，提供了本申请实施例中一种自动驾驶车辆的融合定位方法的流程示意图，所述方法至少包括如下的步骤S110至步骤S140：

步骤S110，获取第一传感器量测数据。

本申请实施例在进行自动驾驶车辆的融合定位时，需要先获取第一传感器量测数据，这里的第一传感器量测数据可以是自动驾驶车辆上安装的各种传感器实际观测到的定位数据，例如GNSS的定位数据、激光雷达的定位数据、视觉相机的定位数据等等。这些传感器的定位数据都可以看作是量测数据进行后续基于第二扩展卡尔曼滤波器(EKF-2)实现的融合更新过程。

步骤S120，根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到。

在基于扩展卡尔曼滤波器实现的融合定位方案中，除了需要有各个传感器实际的量测数据，还需要提供状态预测数据，也即本申请实施例的第一滤波器状态数据，状态预测数据是指基于前一时刻的最优定位数据以及惯性测量数据来预测当前时刻的定位状态，具体可以通过第一扩展卡尔曼滤波器(EKF-1)对这些数据进行处理得到。

由于时间延迟的原因，当前接收到的第一传感器量测数据往往是在一段时间之前就已经产生的量测数据，因此在融合定位时，需要根据第一传感器量测数据的延迟时间从共享缓冲区获取对应的第一滤波器状态数据，从而便于后续将量测数据与同一时刻的第一滤波器状态数据进行融合，保证时间同步精度，提高融合定位精度。

上述共享缓冲区可以看作是EKF-1和EKF-2共用的一个数据传输通道，主要用于存储一段时间内如1s内的滤波器状态数据，提高融合定位效率和精度。一方面EKF-1可以实时将自己预测的滤波器状态数据存储到共享缓冲区中，另一方面EKF-2可以从共享缓冲区中获取EKF-1预测的滤波器状态数据，从而进行量测更新。

步骤S130，根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略。

在得到第一传感器量测数据和对应的第一滤波器状态数据后，可以利用EKF-2按照预设更新策略进行量测更新和时间更新，由于第一传感器量测数据往往存在延时，且在基于第一传感器量测数据进行量测更新和时间更新的过程中，可能还会获取到新的其他的传感器量测数据，因此这里可以针对延时的各种传感器量测数据采用一定的融合策略进行融合，从而保证各种传感器量测数据均能够被充分利用，提高融合定位精度和稳定性。

步骤S140，根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

EKF-2输出的定位结果可以看作是对状态预测数据和传感器实际的量测数据的融合结果，相当于利用传感器实际的量测数据对EKF-1的状态预测数据进行了校正和更新，因此可以基于EKF-2输出的定位结果确定最终的融合定位结果。

本申请实施例的自动驾驶车辆的融合定位方法通过设置共享缓冲区作为数据传输通道，提高了融合定位效率，且采用一定的更新策略进行量测更新和时间更新，充分利用了各种延时的传感器量测数据，避免了暴力融合或者移除带来的观测量损失，提高了融合定位的精度和稳定性。

在本申请的一些实施例中，在获取第一传感器量测数据之后，所述方法还包括：确定所述第一传感器量测数据的接收时刻和产生时刻；根据所述接收时刻与所述产生时刻之间的时间间隔，确定所述第一传感器量测数据是否可用；在所述第一传感器量测数据不可用且不可用的持续时间达到预设持续时间阈值的情况下，关闭所述第一传感器量测数据对应的传感器模块；在所述第一传感器量测数据可用的情况下，根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据。

本申请实施例在获取到第一传感器量测数据之后，可以先对第一传感器量测数据的延时情况进行判断，确定当前接收到的第一传感器量测数据是否可用。具体地，第一传感器量测数据中会携带有第一传感器量测数据产生时刻的时间戳，因此根据第一传感器量测数据当前的接收时刻与其产生时刻之间的时间间隔可以确定第一传感器量测数据的延时时间，然后将第一传感器量测数据的延时时间与预设延时时间阈值进行比较，如果超过该预设延时时间阈值，说明当前的第一传感器量测数据延时时间过长，继续用于后续量测更新将导致定位结果出现较大偏差，因此可以认为该第一传感器量测数据不可用，直接丢弃即可。而如果第一传感器量测数据的延时时间没有超过预设延时时间阈值，说明当前的第一传感器量测数据延时时间仍可接受，可以用于后续的量测更新将，因此可以认为该第一传感器量测数据可用。

对于上述第一传感器量测数据不可用的情况，还可以进一步对第一传感器量测数据不可用的持续时间进行统计，如果连续一段时间如1分钟以上接收到的第一传感器量测数据都不可用，此时说明该第一传感器量测数据对应的传感器模块受算力影响较大，可以暂时关闭该传感器模块的定位功能，释放其在自动驾驶处理器上的所占算力，从而保证自动驾驶系统的其他节点的正常算力需求。当然，一段时间如半小时之后可以再次启动该传感器模块的定位功能，并重复以上判断过程。

需要说明的是，上述预设延时时间阈值以及预设持续时间阈值等均可以根据实际需求灵活设置大小，在此不作具体限定。

在本申请的一些实施例中，在根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据之前，所述方法包括：获取IMU原始测量数据；基于所述IMU原始测量数据，利用所述第一扩展卡尔曼滤波器进行状态预测，得到所述第一滤波器状态数据。

本申请实施例的EKF-1和EKF-2对于数据的处理是两个独立的过程，对于EKF-1来说，其主要用于实现自动驾驶车辆的状态预测，在进行状态预测时，可以先获取IMU原始测量数据，EKF-1依赖IMU原始测量数据以及前一时刻的最终定位结果就可以实现状态预测并输出状态量，这里的状态量主要包括3维位置、3维速度、3维姿态、3维IMU陀螺偏置，3维IMU加速度计偏置，共计15维变量。此外，进一步结合IMU原始数据、协方差和状态转移矩阵等信息构成第一滤波器状态数据缓存到共享缓冲区中。

EKF-1的状态预测过程可以看作是一个实时进行的过程，只要有IMU原始测量数据即可实现，而EKF-2的量测更新过程依赖于量测数据的产生和接收，相比于EKF-1的状态预测过程，更像一个瞬时发生的过程。

在本申请的一些实施例中，所述根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据包括：确定所述第一传感器量测数据的接收时刻和产生时刻；从所述共享缓冲区获取所述第一传感器量测数据的接收时刻和产生时刻之间对应的第一滤波器状态数据。

本申请实施例在从共享缓冲区获取对应的第一滤波器状态数据时，可以先根据第一传感器量测数据中携带的时间戳确定第一传感器量测数据的产生时刻t1，然后从共享缓冲区获取第一传感器量测数据的接收时刻t2和产生时刻t1之间所对应的第一滤波器状态数据进行后续的量测更新过程。

由于共享缓冲区中存储了最近1s内的滤波器状态数据，而第一传感器量测数据可用的要求是第一传感器量测数据的产生时刻与接收时刻之间的时间间隔最长不能超过1s，因此能够看出本申请对于共享缓冲区缓存状态数据的时间设置以及对于量测数据延时时间的设置是相适配的，目的是为了保证后续量测数据能够与同一时刻的第一滤波器状态数据进行融合，保证时间同步精度，提高融合定位精度。

在本申请的一些实施例中，所述根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据包括：在将时间更新到当前时刻的过程中，确定是否获取到第二传感器量测数据；若获取到所述第二传感器量测数据，则确定所述第二传感器量测数据的产生时刻，并根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新；所述根据所述第二滤波器状态数据确定当前时刻的融合定位结果包括：若没有获取到所述第二传感器量测数据，则直接根据所述第二滤波器状态数据更新所述共享缓冲区。

本申请实施例在利用EKF-2对第一传感器量测数据和对应的第一滤波器状态数据进行融合更新时，可以判断更新过程中是否接收到新的传感器量测数据也即第二传感器量测数据，并根据判断结果的不同采取不同的策略。例如，如果在基于第一传感器量测数据进行融合更新的过程中，又接收到了第二传感器量测数据，那么这时可以先确定第二传感器量测数据的产生时刻，将其与第一传感器量测数据的产生时刻进行比较，并根据比较结果确定是否需要重新进行量测更新。而如果在进行时间更新到当前时刻的过程中都没有获取到新的传感器量测数据，那么根据量测更新后得到的第二滤波器状态数据直接更新共享缓冲区即可，并等待下一次量测数据的到来。

上述过程针对融合过程中多传感器量测数据的时序情况采取了不同的融合策略，从而能够充分利用各种传感器的量测数据进行融合定位，保证了融合定位精度和稳定性。

在本申请的一些实施例中，所述根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新包括：若所述第二传感器量测数据的产生时刻晚于所述第一传感器量测数据的产生时刻，则确定需要重新进行量测更新；若所述第二传感器量测数据的产生时刻早于所述第一传感器量测数据的产生时刻，则确定不需要重新进行量测更新。

在将第二传感器量测数据的产生时刻与第一传感器量测数据的产生时刻进行比较时，如果第二传感器量测数据在第一传感器量测数据之后产生，但又在第一传感器量测数据的时间更新过程中被接收到，说明原来的量测更新过程中对于有延时但是可用的传感器量测数据并没有利用上，那么这时可以重新进行量测更新，保证可用的第一传感器量测数据和第二传感器量测数据均可以在量测更新过程中被利用。

而如果第二传感器量测数据在第一传感器量测数据之前产生，且在第一传感器量测数据的时间更新过程中被接收到，那么说明原来的量测更新过程已经将所有可用的传感器量测数据融合进来，那么也就不需要重新进行量测更新。

为了便于对上述实施例的理解，如图2所示，提供了本申请实施例中一种多传感器量测数据的融合时序示意图。可以看出，对于EKF-1来说，其能够根据稳定获取到的IMU原始测量数据稳定输出第一滤波器状态数据，并实时缓存到共享缓冲区中。对于EKF-2来说，传感器量测数据由于会存在不同程度的延时情况，并且不同传感器自身的数据输出频率不同以及外界因素影响等原因导致各自输出的数据也不够稳定。

图2中列举了几种常见的传感器量测数据的形式，包括激光雷达(Lidar)观测量、GPS观测量以及视觉观测量，每个传感器量测数据对应有其产生时刻T_occur和接收时刻T_rec，例如，对于Lidar观测量-1，其产生时刻为t1，但是在t7时刻才被接收到，在基于Lidar观测量-1进行更新的过程中，又接收到了GPS观测量-1和视觉观测量-1，且GPS观测量-1的产生时刻t2和视觉观测量-1的产生时刻t5均晚于Lidar观测量-1的产生时刻t1，因此这时就需要重新进行量测更新。

又例如，对于Lidar观测量-2，其产生时刻为t4，但是在t8时刻才被接收到，在基于Lidar观测量-2进行更新的过程中，又接收到了视觉观测量-1和Lidar观测量-1，由于视觉观测量-1的产生时刻t5晚于Lidar观测量-2的产生时刻t4，因此同样需要重新进行量测更新，而由于Lidar观测量-1的产生时刻t1早于Lidar观测量-2的产生时刻为t4，此时不存在遗漏的问题，也就不需要重新进行量测更新。因此，通过上述过程保证了量测更新过程能够对各个传感器量测数据的充分利用，提高了融合定位精度和稳定性。

在本申请的一些实施例中，所述根据所述第二滤波器状态数据确定当前时刻的融合定位结果包括：根据所述第二滤波器状态数据，通过所述第二扩展卡尔曼滤波器更新所述共享缓冲区；通过所述第一扩展卡尔曼滤波器从更新后的共享缓冲区中获取更新后的滤波器状态数据；根据更新后的滤波器状态数据更新所述第一滤波器状态数据，并输出当前时刻的融合定位结果。

EKF-2在完成所有量测更新之后，会将得到的第二滤波器状态数据更新到共享缓冲区，共享缓冲区被EKF-2更新后，EKF-1会获取共享缓冲区中的所有信息，并更新其自身状态，以更新后的状态，继续在当前时刻利用接收到的IMU原始测量数据进行状态预测。

此外，由于EKF-2的输出频率主要依赖于当前是否有接收到新的传感器量测数据，如果没有接收到新的传感器量测数据，则不会输出定位数据，而EKF-1的输出频率则较为稳定，因此为了保证融合定位结果输出的稳定性，本申请实施例最终可以以EKF-1在更新自身状态后输出的当前定位结果作为最终融合定位结果。

为了便于对本申请各实施例的理解，如图3所示，提供了本申请实施例中一种自动驾驶车辆的融合定位流程示意图。本申请实施例的自动驾驶车辆的融合定位流程主要分为两部分，第一部分是基于EKF-1实现的状态预测，第二部分是基于EKF-2实现的量测更新，EKF-1和EKF-2之间通过共享缓冲区进行数据传输，共享缓冲区内用于存储一段时间如1s内的滤波器状态数据。EKF-1通过IMU原始测量数据进行状态预测，得到第一滤波器状态数据并缓存到共享缓冲区，EKF-2在接收到传感器量测数据如Lidar定位数据、GPS定位数据或者视觉定位数据后，从共享缓冲区中获取对应的滤波器状态数据并进行量测更新，量测更新完成后，将得到的第二滤波器状态数据更新至共享缓冲区中，共享缓冲区被EKF-2触发更新后，EKF-1会从共享缓冲区中获取第二滤波器状态数据来更新自身的状态数据，并输出最终的融合定位结果，不断重复此过程，从而实现自动驾驶车辆的实时定位。

综上所述，本申请的自动驾驶车辆的融合定位方法至少取得了如下的技术效果：

1)利用共享缓冲区缓存一定时间段内的滤波器状态数据，基于此将传感器量测数据与同一时刻的IMU原始测量数据进行融合，保证了时间同步精度，提高了融合定位效率和精度；

2)充分利用了各种传感器量测数据，避免了暴力融合或者移除带来的观测信息损失，提高了融合定位稳定性；

3)使用两个扩展卡尔曼滤波器，提高了定位鲁棒性，理论上可以融合任意数量的量测信息源，如Lidar的延时定位信息，视觉的延时定位信息，同时融合无延时或者低延时的定位信息，比如GPS定位信息；

4)具备长时间延时监测功能，若传感器量测数据的延时时间过长，并且该状态持续一段时间以上，则证明该传感器模块受算力影响较大，且定位信息不可用，此时会自动关闭该传感器模块的定位功能，释放其在自动驾驶处理器上所占用的算力，从而保证其他模块的正常算力需求。

本申请实施例还提供了一种自动驾驶车辆的融合定位装置400，如图4所示，提供了本申请实施例中一种自动驾驶车辆的融合定位装置，所述装置400包括：第一获取单元410、第二获取单元420、第一更新单元430以及第一确定单元440，其中：

第一获取单元410，用于获取第一传感器量测数据；

第二获取单元420，用于根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

第一更新单元430，用于根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

第一确定单元440，用于根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

在本申请的一些实施例中，所述装置还包括：第二确定单元，用于确定所述第一传感器量测数据的接收时刻和产生时刻；第三确定单元，用于根据所述接收时刻与所述产生时刻之间的时间间隔，确定所述第一传感器量测数据是否可用；关闭单元，用于在所述第一传感器量测数据不可用且不可用的持续时间达到预设持续时间阈值的情况下，关闭所述第一传感器量测数据对应的传感器模块；第二获取单元，用于在所述第一传感器量测数据可用的情况下，根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据。

在本申请的一些实施例中，所述装置包括：第三获取单元，用于获取IMU原始测量数据；状态预测单元，用于基于所述IMU原始测量数据，利用所述第一扩展卡尔曼滤波器进行状态预测，得到所述第一滤波器状态数据。

在本申请的一些实施例中，所述第二获取单元420具体用于：确定所述第一传感器量测数据的接收时刻和产生时刻；从所述共享缓冲区获取所述第一传感器量测数据的接收时刻和产生时刻之间对应的第一滤波器状态数据。

在本申请的一些实施例中，所述第一更新单元430具体用于：在将时间更新到当前时刻的过程中，确定是否获取到第二传感器量测数据；若获取到所述第二传感器量测数据，则确定所述第二传感器量测数据的产生时刻，并根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新；所述第一确定单元440具体用于：若没有获取到所述第二传感器量测数据，则直接根据所述第二滤波器状态数据更新所述共享缓冲区。

在本申请的一些实施例中，所述第一更新单元430具体用于：若所述第二传感器量测数据的产生时刻晚于所述第一传感器量测数据的产生时刻，则确定需要重新进行量测更新；若所述第二传感器量测数据的产生时刻早于所述第一传感器量测数据的产生时刻，则确定不需要重新进行量测更新。

在本申请的一些实施例中，所述第一确定单元440具体用于：根据所述第二滤波器状态数据，通过所述第二扩展卡尔曼滤波器更新所述共享缓冲区；通过所述第一扩展卡尔曼滤波器从更新后的共享缓冲区中获取更新后的滤波器状态数据；根据更新后的滤波器状态数据更新所述第一滤波器状态数据，并输出当前时刻的融合定位结果。

能够理解，上述自动驾驶车辆的融合定位装置，能够实现前述实施例中提供的自动驾驶车辆的融合定位方法的各个步骤，关于自动驾驶车辆的融合定位方法的相关阐释均适用于自动驾驶车辆的融合定位装置，此处不再赘述、

图5是本申请的一个实施例电子设备的结构示意图。请参考图5，在硬件层面，该电子设备包括处理器，可选地还包括内部总线、网络接口、存储器。其中，存储器可能包含内存，例如高速随机存取存储器(Random-Access Memory，RAM)，也可能还包括非易失性存储器(non-volatile memory)，例如至少1个磁盘存储器等。当然，该电子设备还可能包括其他业务所需要的硬件。

处理器、网络接口和存储器可以通过内部总线相互连接，该内部总线可以是ISA(Industry Standard Architecture，工业标准体系结构)总线、PCI(PeripheralComponent Interconnect，外设部件互连标准)总线或EISA(Extended Industry StandardArchitecture，扩展工业标准结构)总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。

存储器，用于存放程序。具体地，程序可以包括程序代码，所述程序代码包括计算机操作指令。存储器可以包括内存和非易失性存储器，并向处理器提供指令和数据。

处理器从非易失性存储器中读取对应的计算机程序到内存中然后运行，在逻辑层面上形成自动驾驶车辆的融合定位装置。处理器，执行存储器所存放的程序，并具体用于执行以下操作：

获取第一传感器量测数据；

根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

上述如本申请图1所示实施例揭示的自动驾驶车辆的融合定位装置执行的方法可以应用于处理器中，或者由处理器实现。处理器可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器可以是通用处理器，包括中央处理器(CentralProcessing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific IntegratedCircuit，ASIC)、现场可编程门阵列(Field－Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。

该电子设备还可执行图1中自动驾驶车辆的融合定位装置执行的方法，并实现自动驾驶车辆的融合定位装置在图1所示实施例的功能，本申请实施例在此不再赘述。

本申请实施例还提出了一种计算机可读存储介质，该计算机可读存储介质存储一个或多个程序，该一个或多个程序包括指令，该指令当被包括多个应用程序的电子设备执行时，能够使该电子设备执行图1所示实施例中自动驾驶车辆的融合定位装置执行的方法，并具体用于执行：

获取第一传感器量测数据；

根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

本领域技术人员应明白，本申请的实施例可提供为方法、系统或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims (10)

1.一种自动驾驶车辆的融合定位方法，其中，所述方法包括：

获取第一传感器量测数据；

根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

2.如权利要求1所述方法，其中，在获取第一传感器量测数据之后，所述方法还包括：

确定所述第一传感器量测数据的接收时刻和产生时刻；

根据所述接收时刻与所述产生时刻之间的时间间隔，确定所述第一传感器量测数据是否可用；

在所述第一传感器量测数据不可用且不可用的持续时间达到预设持续时间阈值的情况下，关闭所述第一传感器量测数据对应的传感器模块；

在所述第一传感器量测数据可用的情况下，根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据。

3.如权利要求1所述方法，其中，在根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据之前，所述方法包括：

获取IMU原始测量数据；

基于所述IMU原始测量数据，利用所述第一扩展卡尔曼滤波器进行状态预测，得到所述第一滤波器状态数据。

4.如权利要求1所述方法，其中，所述根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据包括：

确定所述第一传感器量测数据的接收时刻和产生时刻；

从所述共享缓冲区获取所述第一传感器量测数据的接收时刻和产生时刻之间对应的第一滤波器状态数据。

5.如权利要求1所述方法，其中，所述根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据包括：

在将时间更新到当前时刻的过程中，确定是否获取到第二传感器量测数据；

若获取到所述第二传感器量测数据，则确定所述第二传感器量测数据的产生时刻，并根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新；

所述根据所述第二滤波器状态数据确定当前时刻的融合定位结果包括：

若没有获取到所述第二传感器量测数据，则直接根据所述第二滤波器状态数据更新所述共享缓冲区。

6.如权利要求5所述方法，其中，所述根据所述第二传感器量测数据的产生时刻与所述第一传感器量测数据的产生时刻的比较结果确定是否需要重新进行量测更新包括：

若所述第二传感器量测数据的产生时刻晚于所述第一传感器量测数据的产生时刻，则确定需要重新进行量测更新；

若所述第二传感器量测数据的产生时刻早于所述第一传感器量测数据的产生时刻，则确定不需要重新进行量测更新。

7.如权利要求1所述方法，其中，所述根据所述第二滤波器状态数据确定当前时刻的融合定位结果包括：

根据所述第二滤波器状态数据，通过所述第二扩展卡尔曼滤波器更新所述共享缓冲区；

通过所述第一扩展卡尔曼滤波器从更新后的共享缓冲区中获取更新后的滤波器状态数据；

根据更新后的滤波器状态数据更新所述第一滤波器状态数据，并输出当前时刻的融合定位结果。

8.一种自动驾驶车辆的融合定位装置，其中，所述装置包括：

第一获取单元，用于获取第一传感器量测数据；

第二获取单元，用于根据所述第一传感器量测数据，从共享缓冲区获取对应的第一滤波器状态数据，所述共享缓冲区用于缓存预设时间长度的滤波器状态数据，所述第一滤波器状态数据通过第一扩展卡尔曼滤波器得到；

第一更新单元，用于根据所述第一传感器量测数据和对应的第一滤波器状态数据，通过第二扩展卡尔曼滤波器按照预设更新策略进行量测更新和时间更新，得到第二滤波器状态数据，所述预设更新策略包括对延时的传感器量测数据的融合策略；

第一确定单元，用于根据所述第二滤波器状态数据确定当前时刻的融合定位结果。

9.一种电子设备，包括：

处理器；以及

被安排成存储计算机可执行指令的存储器，所述可执行指令在被执行时使所述处理器执行所述权利要求1～7之任一所述方法。

10.一种计算机可读存储介质，所述计算机可读存储介质存储一个或多个程序，所述一个或多个程序当被包括多个应用程序的电子设备执行时，使得所述电子设备执行所述权利要求1～7之任一所述方法。

Images