CN117437770A - 一种目标状态估计方法、装置、电子设备和介质 - Google Patents

一种目标状态估计方法、装置、电子设备和介质 Download PDF

Info

Publication number
CN117437770A
CN117437770A CN202210837617.3A CN202210837617A CN117437770A CN 117437770 A CN117437770 A CN 117437770A CN 202210837617 A CN202210837617 A CN 202210837617A CN 117437770 A CN117437770 A CN 117437770A
Authority
CN
China
Prior art keywords
time window
orientation
target
loss
state quantity
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
CN202210837617.3A
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 Tusimple Technology Co Ltd
Original Assignee
Beijing Tusimple 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 Tusimple Technology Co Ltd filed Critical Beijing Tusimple Technology Co Ltd
Priority to CN202210837617.3A priority Critical patent/CN117437770A/zh
Priority to EP23184287.3A priority patent/EP4307244A1/en
Priority to AU2023204634A priority patent/AU2023204634A1/en
Priority to JP2023114661A priority patent/JP2024012160A/ja
Priority to US18/351,931 priority patent/US20240020870A1/en
Publication of CN117437770A publication Critical patent/CN117437770A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/01—Detecting movement of traffic to be counted or controlled
    • G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
    • G08G1/0125—Traffic data processing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/62—Extraction of image or video features relating to a temporal dimension, e.g. time-based feature extraction; Pattern tracking
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/52—Surveillance or monitoring of activities, e.g. for recognising suspicious objects
    • G06V20/54—Surveillance or monitoring of activities, e.g. for recognising suspicious objects of traffic, e.g. cars on the road, trains or boats
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/01—Detecting movement of traffic to be counted or controlled
    • G08G1/052—Detecting movement of traffic to be counted or controlled with provision for determining speed or overspeed
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V2201/00—Indexing scheme relating to image or video recognition or understanding
    • G06V2201/07—Target detection

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Chemical & Material Sciences (AREA)
  • Analytical Chemistry (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Image Analysis (AREA)

Abstract

本公开涉及一种目标状态估计方法，包括：获取对应于多个时刻的数据帧序列；在数据帧序列中确定第一时间窗口，第一时间窗口包括一组待优化的第一状态量集合；从第一时间窗口内确定多个第二时间窗口，第二时间窗口内的数据帧数小于第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；通过同时对多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。本公开的目标状态估计方法能够得到足够精确的状态估计。此外，一种目标状态估计装置、电子设备和介质亦被提出。

Description

一种目标状态估计方法、装置、电子设备和介质
技术领域
本公开涉及计算机领域，特别涉及自动驾驶、数据处理技术领域，具体涉及一种目标状态估计方法、装置、电子设备、计算机可读存储介质和计算机程序产品。
背景技术
在对目标进行识别或观测过程中，通常需要根据传感器已获得的目标测量数据对目标状态进行精确的估计。目标在运动过程中随着速度、角度、加速度等参数不断变化，使得目标的位置具有很强的相关性。例如，无人驾驶中一个重要的环节是对道路上其他车辆的位置、速度、大小、朝向等状态进行实时估计，这项技术很大程度上决定了无人驾驶的安全系数。因此，为提高对目标的识别或观测性能，迫切需要研究更为优越的状态估计方法。
在此部分中描述的方法不一定是之前已经设想到或采用的方法。除非另有指明，否则不应假定此部分中描述的任何方法仅因其包括在此部分中就被认为是现有技术。类似地，除非另有指明，否则此部分中提及的问题不应认为在任何现有技术中已被公认。
发明内容
根据本公开的一个方面，提供了一种目标状态估计方法，包括：获取对应于多个时刻的数据帧序列；在所述数据帧序列中确定第一时间窗口，所述第一时间窗口包括一组待优化的第一状态量集合；从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
根据本公开的另一方面，提供了一种目标状态估计装置，包括：获取单元，配置为：获取对应于多个时刻的数据帧序列；确定单元，配置为：确定所述数据序列中的第一时间窗口，所述第一时间窗口包括一组待优化的第一状态量集合；选取单元，配置为：从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；优化单元，配置为：通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
根据本公开的另一方面，提供了一种电子设备，包括：至少一个处理器；以及与至少一个处理器通信连接的存储器；存储器存储有可被至少一个处理器执行的指令，该指令被至少一个处理器执行，以使至少一个处理器能够执行本公开所述的方法。
根据本公开的另一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，该计算机指令用于使计算机执行本公开所述的方法。
根据本公开的一个或多个实施例，在对在第一时间窗口内的状态量进行优化时，通过设置多个第二时间窗口并通过同时优化第二时间窗口内的状态量，来保持第一时间窗口内状态量的全局平滑性，从而提高了目标状态估计的精确性。
应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图示例性地示出了实施例并且构成说明书的一部分，与说明书的文字描述一起用于讲解实施例的示例性实施方式。所示出的实施例仅出于例示的目的，并不限制权利要求的范围。在所有附图中，相同的附图标记指代类似但不一定相同的要素。
图1是示出根据示例性实施例的目标状态估计方法的流程图；
图2是示出根据示例性实施例的用于目标状态估计的时间窗口的示意图；
图3是示出根据示例性实施例的卡车运动模型的示意图；
图4是示出根据示例性实施例的只包含第一部件的车辆的运动模型示意图；
图5是示出根据示例性实施例的目标状态估计装置的结构框图；以及
图6是示出能够应用于示例性实施例的示例性计算设备的结构框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
在本公开中，除非另有说明，否则使用术语“第一”、“第二”等来描述各种要素不意图限定这些要素的位置关系、时序关系或重要性关系，这种术语只是用于将一个元件与另一元件区分开。在一些示例中，第一要素和第二要素可以指向该要素的同一实例，而在某些情况下，基于上下文的描述，它们也可以指代不同实例。
在本公开中对各种所述示例的描述中所使用的术语只是为了描述特定示例的目的，而并非旨在进行限制。除非上下文另外明确地表明，如果不特意限定要素的数量，则该要素可以是一个也可以是多个。此外，本公开中所使用的术语“和/或”涵盖所列出的项目中的任何一个以及全部可能的组合方式。
无人驾驶中一个重要的环节是对道路上其他车辆的位置、速度、大小、朝向等状态进行实时估计，这很大程度上决定了无人驾驶的安全系数。可以通过相应的观测模型来对车辆的速度、位置等信息进行观测，观测模型所观测到的数据通常存在噪声等影响，以使得所观测数据与车辆的实际行驶数据存在误差。从而需要对该观测数据进行修正，即根据观测数据对车辆的行驶过程中的物理状态进行估计。
通常，在状态估计过程中，基于相对应的观测数据进行某种状态的估计，例如，基于所观测到的速度信息对车辆速度状态进行估计、基于所观测到的车辆中心点坐标对车辆位置状态进行估计等。通常，在对车辆的物理状态进行估计时，可以通过构建时间窗口(例如滑动时间窗口)，以实现时间窗口内的一组状态量的优化。可以看出，时间窗口内的一组状态量的优化效果成为了影响无人驾驶的安全系数的关键。
因此，本公开的实施例提供了一种目标状态估计方法，包括：获取对应于多个时刻的数据帧序列；在所述数据帧序列中确定第一时间窗口，所述第一时间窗口包括一组待优化的第一状态量集合；从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
根据本公开的实施例，在对在第一时间窗口内的状态量进行优化时，通过设置多个第二时间窗口并通过同时优化第二时间窗口内的状态量，来保持第一时间窗口内状态量的全局平滑性，从而提高了目标状态估计的精确性和鲁棒性。
图1示出了根据本公开的实施例的目标状态估计方法的流程图。如图1所示，在步骤110中，获取对应于多个时刻的数据帧序列。
在本公开的实施例中，所述目标可以包括车辆。因此，数据帧序列可以对应于该目标车辆的多个时刻下的观测量。示例地，该观测量可以包括：目标车辆在各时刻下的速度、位置、朝向中的至少一种；以及目标车辆的尺寸，所述尺寸可以包括长度、宽度和高度中的至少一种。
在一些实施例中，该数据帧序列所对应的观测量可以基于多种传感器获得。示例地，多种传感器可以包括以下至少一种：图像采集装置、点云采集装置。示例地，该图像采集装置可以包括多种多样的设备，如视觉摄像头、红外摄像头、进行紫外或X光成像的相机，等等。不同的设备可以提供不同的检测精度和范围。视觉摄像头可以实时捕获目标的运行状态等信息。红外摄像头可以在夜视情况下捕捉目标。紫外或X光成像的相机可以在各种复杂环境(夜间、恶劣气候、电磁干扰等)下对目标进行成像。点云采集装置也可以包括多种多样的设备，如激光雷达(LiDAR)、毫米波雷达、超声波传感器，等等。不同的设备可以提供不同的检测精度和范围。激光雷达可以用于检测目标边缘、形状信息，从而进行目标识别和追踪。毫米波雷达可以用于利用电磁波的特性测量与目标的距离。超声波传感器可以用于利用超声波方向性强等特点来测量与目标的距离。由于多普勒效应，雷达装置还可以测量与移动目标的速度变化。
根据一些实施例，多种传感器可以位于至少一个观测车辆上或路侧设备上。示例地，自动驾驶车辆在行驶过程中，多种传感器可以安装在车辆的前方、后方或其他位置，以实现对周围车辆的实时观测。或者，多种传感器位于至路侧设备上，以对途经该路侧设备的车辆、行人等目标进行实时观测。
在一些示例中，该路侧设备可以包括电子设备、通信设备等，电子设备可以和通信设备一体集成，也可以分体设置。电子设备可以获取多种传感器所观测到的数据，从而进行数据处理和计算，以获得相应的观测量，再经由通信设备向计算设备传送处理和计算结果。可选的，电子设备也可以设置在云端，以通过通信设备获取路侧设备上的多种传感器所观测到的数据，并通过数据分析和计算获得相应的观测量。
根据一些实施例，根据本公开的目标状态估计方法可以在计算设备中实现，计算设备通过每种传感器获取至少一种观测量。也就是说，通过多种传感器所获取目标在各时刻下的观测量可以通过计算设备进行在线或离线分析。计算设备可以驻留在至少一个观测车辆上、驻留在路侧设备上或者驻留在云端，在此不作限制。
根据一些实施例，观测量可以通过各传感器所分别对应的观测模型得到。示例地，该观测模型包括以下至少一种：基于图像的双目测距算法、基于图像的单目测距算法、基于点云的测距算法、基于图像和地图的投射测距算法、以及基于点云和地图的投射测距算法。
在本公开中，观测模型可以基于传感器所获取的数据进行分析和计算，以输出与该目标相对应的各时刻下的观测量。具体地，在一些示例中，基于投射测距算法，可以获得周围车辆的中心点坐标和检测框的四个角点坐标等；基于双目测距算法、单目测距算法等的测距算法，可以获得周围车辆的中心点坐标、速度等。
在一些实施例中，在通过多种传感器获取目标在各时刻下的观测量后，可以对所获取到的观测量进行数据预处理。示例地，可以删除异常观测量、保留可用观测量，统一数据格式等，在此不作限制。
在步骤120中，在数据帧序列中确定第一时间窗口，第一时间窗口包括一组待优化的第一状态量集合。
在驾驶过程中，车辆例如可以通过多个传感器对周围车辆进行实时观测，从而不断产生观测数据，即数据帧序列。基于该观测数据，即可确定待优化的状态量。在一些实施例中，可以通过构建时间窗口以实现对车辆的物理状态的优化。
具体地，获取由至少一个观测模型在第一时间窗口内观测到的针对目标车辆的观测量，以基于该观测量构建描述目标车辆在该第一时间窗口内的物理状态的状态量。在一些示例中，该第一时间窗口可以为滑动时间窗口，第一时间窗口的长度和滑动步长可以任意设置，当然第一时间窗口也可以不滑动，在此不作限制。
根据一些实施例，第一时间窗口所对应的第一状态量集合包括以下至少一种：所述目标在第一时间窗口内各时刻下的速度、位置、朝向、尺寸中的至少一种；目标在各时刻下的速度、位置、朝向等状态量即为瞬时状态量。另外，第一状态量集合还可以包括所述目标在第一时间窗口内的平均速度、平均位置、平均朝向中的至少一种。
图2示出了根据本公开的实施例的用于目标状态估计的时间窗口的示意图。如图2所示，第一时间窗口包括n个待优化状态量，构成第一状态量集合，亦即S0、S1、…、Sn-1。其中，第一状态量集合还可以包括目标在第一时间窗口内的平均速度、平均位置、平均朝向中的至少一种，在此不作限制。
示例地，可以根据公式(1)构建第一时间窗口中的待优化状态量。
其中，该第一时间窗口中的第i帧状态量si例如可以包括公式(2)所示的状态量。
其中，vi、θi、oi分别表示目标车辆的速度大小、速度方向以及车体朝向。此外，关于及/>将于后述步骤130的相关描述进行说明。
根据一些实施例，所述目标为车辆，并且该车辆包括第一部件和至少一个第二部件，第二部件能够围绕第一部件旋转。因此，在一些示例中，目标的位置可以包括以下至少一项：第一部件的位置、各第二部件的位置、车辆的位置；目标的尺寸包括以下至少一项：第一部件的尺寸、各第二部件的尺寸、车辆的尺寸。目标的朝向包括以下至少一项：速度的朝向、第一部件的朝向、所述车辆所在车道的朝向。
示例地，在所述目标为车辆的实施例中，该目标车辆可以为卡车，卡车包括前后两个部件，即该卡车的第一部件为牵引车、第二部件为拖车，牵引车和拖车之间连接的转轴(或铰链)结构形成其两者之间的结构约束。
在一些示例中，在该目标车辆为例如卡车的结构时，该目标车辆的速度大小、速度方向可以为牵引车的速度大小和速度方向。另外，上述车体朝向为牵引车朝向，并且，第i帧状态量si还可以包括拖车朝向βi，即可以理解的是，公式(1)和(2)所示的时间窗口内的待优化状态量仅仅是示例性的，在此不作限制。
图3示出了根据本公开的实施例的卡车运动模型的示意图。如图3所示，牵引车301和拖车302通过转轴结构303连接。在一些实施例中，牵引车301可以基于只包含第一部件的车辆的运动模型进行处理，但是拖车的运动观测对牵引车的运动观测产生约束。该只包括第一部件的车辆例如可以为独轮车、普通四轮车辆等。
图4示出了根据本公开的实施例的只包含第一部件的车辆运动模型的示意图。在一些示例中，将车辆的速度方向与车头方向进行区分，以提高车辆状态估计的准确性。在如图4所示的运动模型中，ο为车辆的朝向(即车头方向)，θ为车辆的速度方向。假设车辆在ti时刻到ti+1时刻之间具有速度vi，因此具有如下公式(3)和公式(4)所示的变换公式。
pxi+1＝pxi+vi·cosθi 公式(3)
pyi+1＝pyi+vi·sinθi 公式(4)
其中，pxi和pyi分别表示ti时刻的车辆中心点坐标；pxi+1和pyi+1分别表示ti+1时刻的车辆中心点坐标；θi表示ti时刻的车辆速度方向与参考坐标系中的x方向的夹角。
在本公开中，参考坐标系为基于多个传感器所在的观测车辆或路测设备所确定坐标系。示例地，当多个传感器位于观测车辆上时，参考坐标系用于描述车辆周边物体与车辆关系的坐标系。根据定义不同，其原点也不一样，例如可以将重心作为原点，其延伸出来的右手坐标系即为参考坐标系；或者以IMU(惯性测量单元)为定义的参考坐标系则以IMU位置为原点。
可以理解的是，任何合适的参考坐标系均是可能的，例如该参考坐标系还可以以车道中心线为横向坐标轴，偏离于车道中心线为纵向坐标轴，垂直于车道中心线为竖直坐标轴，在此不作限制。
如上所述，基于投射测距算法，可以获得车辆检测框的四个角点坐标，即实现车辆轮廓检测。因此，在如图4所示的车体框架中，车辆中心到第i个车辆角点的向量可表示为公式(5)所示。
其中，L和W分别为车辆的长度和宽度；[δi,ηi]表示第i个车辆角点在参考坐标系中相对于车辆中心点的偏移量，对于每个车辆角点来说为常量；Rbw表示参考坐标系到ENU(East-North-Up，东北天)坐标系的旋转矩阵，其中，Rbw表示为公式(6)所示。
从而，基于车辆的速度、朝向、尺寸以及中心点位置等信息，足以确定出一个车辆。
继续参考图3，在一些实施例中，拖车302和转轴结构303通常具有相同的朝向，因此可以作为刚体结构进行处理。另外，可以假设转轴结构303链接与牵引车301和拖车302的接触面的中心位置。一旦牵引车301的中心点坐标p0、长度L0、宽度W0已知，即可获得拖车302的中心点坐标p1，如公式(7)-(9)所示。
p1＝p0+offset0-offset1 公式(7)
其中，L1为拖车的长度，Lh为转轴结构的长度，o和β分别为牵引车和拖车相对于参考坐标系的x轴方向的夹角。
在一些示例中，可以通过例如激光雷达的传感器来同时获得牵引车和拖车的检测框。将拖车的检测框的位置从h1(当i为1时，图3中hi的位置)移动到h0(图3中h0的位置)，假设是牵引车的另一个观测，使得拖车的观测对牵引车的观测产生约束，如公式(10)所示。
拖车的角速度可以如公式(11)所示：
其中，v表示牵引车的速度，表示拖车的角速度。已知牵引车的速度、朝向、尺寸、转轴长度、牵引车的位置，可以确定出一辆卡车的各种状态。
如上所述，已经描述了目标车辆包括两级结构的模型，即目标车辆包括第一部件和一个第二部件。在一些实施例中，第二部件也可以为多个，例如火车、多车挂的货车等等，其运动模型可以参考上面所描述的卡车模型，在此不再赘述。
在图1的步骤130中，从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合。
继续参考图2，在第一时间窗口内包括多个第二时间窗口，即第二时间窗口1、第二时间窗口2、…、第二时间窗口(n-m+1)。每个第二时间窗口具有一组待优化的第二状态量集合。在图2中每个第二时间窗口包括m个状态量，相邻两个第二时间窗口具有(m-1)个重复的数据帧。可以理解的是，这里的相邻第二时间窗口之间重复的数据帧数以及第二状态量集合中的状态量个数仅仅是示例性的，在此不作限制。
根据一些实施例，所述第二状态量集合包括以下至少一种：所述目标在所述第二时间窗口内各时刻下的速度、位置、朝向、尺寸中的至少一种；所述目标在所述第二时间窗口内的平均速度、平均位置、平均朝向中的至少一种。
请参考前述的公式(1)，表示在第一个第二时间窗口内的平均速度，/>表示在第(n-m)个第二时间窗口内的平均速度，/>表示在第(n-m+1)个第二时间窗口内的平均速度。/>表示在第一个第二时间窗口内的平均朝向，/>表示在第(n-m)个第二时间窗口内的平均朝向，/>表示在第(n-m+1)个第二时间窗口内的平均朝向。
在图1的步骤140中，通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
根据一些实施例，所述优化通过最小化损失函数实现。所述损失函数基于所述多个第二时间窗口内各时刻下所述目标的状态量和所述目标的观测量确定得到；所述观测量通过至少一个观测模型观测得到，所述至少一个观测模型基于至少一个传感器得到。
具体地，在一些实施例中，该损失函数可以包括目标的位置损失、朝向损失、速度损失、尺寸损失、结构约束中的至少一项。通过最小化损失函数，实现对目标各时刻下的状态量的优化。示例地，在损失函数包括位置损失、朝向损失、速度损失、尺寸损失时，可以基于公式(12)构建损失函数。
E＝Ep+Ev+Eo+Es 公式(12)
其中，Ep、Ev、Eo和Es分别表示位置损失、朝向损失、速度损失和尺寸损失。损失函数基于待优化的状态量进行确定。具体地，位置损失、朝向损失、速度损失和尺寸损失中的每一项可以基于待优化状态量、与该状态量相对应的观测量以及可以对该状态量提供约束条件的其他观测量确定。
在根据本公开的示例性场景中，目标车辆的速度观测量、目标车辆的位置等可以对该目标车辆的速度大小、速度方向提供约束条件；另外，目标车辆的速度先验、平均速度等也可以对该目标车辆的速度大小、速度方向提供约束条件；车道线方向、速度方向、激光雷达传感器所观测到的目标车辆朝向、目标车辆的朝向先验、平均朝向等可以对该目标车辆的车体朝向提供约束条件；等等。这将在下文进行详细描述。
在本公开中，通过多种传感器获取目标在各时刻下的观测量并构建相应的损失函数，实现了从单传感器识别过渡到多传感器融合。从而，在车辆行驶过程中，可以结合多种传感器的感知结果来对周围车辆进行建模，并实时更新周围车辆的状态信息，以便无人驾驶系统基于此结果做出安全的路径规划，从而避免交通事故发生。
在一些实施例中，所述损失函数包括基于多个第二时间窗口确定的所述状态量的平滑损失；平滑损失根据所述目标在每个第二时间窗口内各时刻下的状态量以及所述目标在所述每个第二时间窗口内的所述状态量的平均值计算得到。
通过基于多个第二时间窗口确定的状态量的平滑损失，使得在对当前时刻的状态估计时，充分地利用了当前时刻及之前、之后各时刻的数据，从而使得对第一时间窗口进行优化后所得到的该状态量更加平滑，提高了目标状态估计的精确度。
具体地，根据一些实施例，损失函数包括与所述目标的速度相关联的速度损失。速度损失包括速度平滑损失，速度平滑损失根据所述每个第二时间窗口内各时刻、所述目标在相应第二时间窗口内各时刻下的速度状态量、以及所述目标在所述相应第二时间窗口内的平均速度状态量计算得到。
在一些实施例中，在所述目标为车辆时，状态量包括目标车辆在第二时间窗口内的各时刻下的速度。为了保证第二时间窗口内的速度平滑，可以使用公式(13)所示的速度平滑损失将第二时间窗口内每一时刻的速度限制为平均值。
其中，wa为该速度平滑损失所对应的权重值，Rbw如上参考公式(6)所述，为当前第二时间窗口内的平均速度。
可以注意的是，可以对每一个第二时间窗口应用如公式(13)所示的速度平滑约束。
根据一些实施例，速度平滑损失进一步根据目标在第一时间窗口内各时刻下的速度状态量、以及目标在所述第一时间窗口内的平均速度状态量计算得到。
在一些实施例中，状态量包括目标车辆在第一时间窗口内的各时刻下的速度。为了保证第一时间窗口内的速度平滑，可以使用公式(14)所示的速度平滑损失将第一时间窗口内每一时刻的速度限制为平均值。
其中，wb为该速度平滑损失所对应的权重值，这里的为第一时间窗口内的平均速度。
在一些实施例中，速度平滑损失所对应的权重值wa和wb可以基于目标车辆与所述多个传感器所在车辆或路侧设备之间的距离确定。示例地，当该距离大于预设阈值时，该权重值与该距离正相关；当该距离不大于该预设阈值时，该权重值为一固定值。
在一些实施例中，速度平滑损失所对应的权重值wa和wb可以进一步基于目标车辆的速度变化率确定，所述速度变化率根据目标车辆在滑动时间窗口内的每一时刻下的速度计算得到。具体地，速度变化率大于另一预设阈值时的权重值小于速度变化率不大于该另一预设阈值时的权重值。
根据一些实施例，速度损失进一步包括速度先验损失，速度先验损失根据以下各项计算得到：当前第一时间窗口与前一第一时间窗口的重叠区间内各时刻下的速度状态量、以及在针对前一第一时间窗口进行的状态量优化过程中所述重叠区间内各时刻下的已优化速度状态量。
在一些实施例中，第一时间窗口为滑动时间窗口，且其滑动步长小于第一时间窗口的长度，则速度损失可以基于速度先验损失确定。具体地，为了保留每一当前时刻在之前所优化得到的优化信息，可以使用公式(15)所示的速度先验损失项将第一时间窗口内每一时刻的速度限制为与该时刻上一次优化后的速度相接近。
其中，为当前时刻上一次优化后的速度，此时k的取值从0到n-2，表示此时第一时间窗口的滑动步长为1，而对于v0、v1、…、vn-2，其最优解已在上一次优化(前一个第一时间窗口)中获得；wp为速度先验损失所对应的权重值。
在一些实施例中，速度先验损失所对应的权重值可以基于目标车辆与多个传感器所在车辆或路侧设备之间的距离确定。当该距离大于预设阈值时，该权重值与该距离正相关；当该距离不大于该预设阈值时，该权重值为一固定值。
根据一些实施例，速度损失进一步包括速度残差，速度残差根据所述目标在第一时间窗口内各时刻下的速度观测量和所述目标在第一时间窗口内各时刻下的速度状态量计算得到。
当一个观测模型可提供速度观测量时，例如雷达模型，该速度损失中还可以灵活添加速度残差损失。假设第l个观测模型的观测为：公式(16)中的速度损失项eov需要加入到速度损失公式中，其中/>代表可提供速度观测的模型个数。
在一些实施例中，例如雷达模型所观测到的速度矢量是不可靠的，但是，可以采用速度范数。如果此时只有速度范数可用，则速度损失项eov可以如公式(17)所示。
综上所述，完整的速度损失项则可以表示为公式(18)所示。
其中，基于第二时间窗口所确定的平滑约束项为多项，即等于第二时间窗口的个数。
根据一些实施例，损失函数包括与所述目标的朝向相关联的朝向损失。朝向损失包括朝向平滑损失，朝向平滑损失根据每个第二时间窗口各时刻、所述目标在相应第二时间窗口内各时刻下的朝向状态量、以及所述目标在相应第二时间窗口内的平均朝向状态量计算得到。
正如速度损失，朝向损失也有相似的平滑损失。在一些实施例中，状态量包括目标车辆在第二时间窗口内的各时刻下的朝向。为了保证第二时间窗口内的朝向平滑，可以使用公式(19)所示的朝向平滑损失将第二时间窗口内每一时刻的朝向限制为平均值。
其中，wc为该朝向平滑损失所对应的权重值，这为当前第二时间窗口内的平均朝向。
可以注意的是，可以对任意一个第二时间窗口应用如公式(19)所示的速度平滑约束。
根据一些实施例，朝向平滑损失进一步根据所述目标在第一时间窗口内各时刻下的朝向状态量、以及所述目标在第一时间窗口内的平均朝向状态量计算得到。
在一些实施例中，状态量包括目标车辆在第一时间窗口内的各时刻下的朝向。为了保证第一时间窗口内的朝向平滑，可以使用公式(20)所示的朝向平滑损失将第一时间窗口内每一时刻的朝向限制为平均值。
其中，wd为该朝向平滑损失所对应的权重值，这里的为第一时间窗口内的平均朝向。
根据一些实施例，朝向损失进一步包括朝向先验损失，朝向先验损失根据以下各项计算得到：第一时间窗口与前一第一时间窗口的重叠区间内各时刻下的朝向状态量、以及在针对前一第一时间窗口进行的状态量优化过程中所述重叠区间内各时刻下的已优化朝向状态量。
在一些实施例中，第一时间窗口为滑动时间窗口，且其滑动步长小于第一时间窗口的长度，则朝向损失可以基于朝向先验损失确定。具体地，为了保留每一当前时刻在之前所优化得到的优化信息，可以使用公式(21)所示的朝向先验损失项将第一时间窗口内每一时刻的朝向限制为与该时刻上一次优化后的朝向相接近。
其中，wp为朝向先验损失所对应的权重值，为当前时刻上一次优化后的朝向(此时假设第一时间窗口的滑动步长为1)。
在一些实施例中，状态量包括目标在第一时间窗口内每一时刻下的朝向。此时，朝向损失可以包括基于朝向残差，朝向残差根据目标在第一时间窗口内每一时刻下的朝向状态量和目标在该第一时间窗口内每一时刻下的朝向观测值计算得到。
具体地，朝向观测量可以直接构成朝向的约束，因此朝向损失项可以如公式(22)所示。
其中，是一组不同的观测源，/>是第l个观测源所对应的权重，其计算方式可以如下参考位置损失所述的公式(32)所示。
在一些实施例中，朝向观测值可以为至少一个观测模型观测到的目标车辆的车体朝向、车道线朝向、或目标车辆的速度方向。在一些实施例中，为了优化朝向，当没有给出可靠的朝向观测时，车辆应该沿着车道走，则此时车道的车道线航向可以被视为具有固定方差的朝向观测值；再者，速度方向也可以被认为是一种朝向观测，且速度越高，速度方向与车辆朝向的差异越小。
在一些示例中，对于基于速度的朝向观测，权重λk可根据公式(23)计算得到，
其中，wv和a为超参数。
根据一些实施例，所述损失函数包括与所述目标的朝向相关联的朝向损失；所述朝向损失包括朝向平滑损失，所述朝向平滑损失根据所述每个第二时间窗口各时刻、所述目标在相应第二时间窗口内各时刻下的朝向状态量、以及所述目标在所述相应第二时间窗口内的平均朝向状态量计算得到。所述朝向损失进一步包括朝向残差，所述朝向残差包括第一部件朝向残差和/或第二部件朝向残差；所述第一部件朝向残差根据所述第一部件在所述第一时间窗口内各时刻下的朝向状态量和所述第一部件在所述第一时间窗口内各时刻下的朝向观测量计算得到；所述第二部件朝向残差根据所述第二部件在所述第一时间窗口内各时刻下的朝向状态量和所述第二部件在所述第一时间窗口内各时刻下的朝向观测量计算得到。
在一些实施例中，目标车辆为例如卡车的包括第一部件和第二部件的车辆，如图3所示。所述第一部件和第二部件可以通过转轴结构(或铰链)形成两者之间的结构约束。状态量包括第一部件在滑动时间窗口内每一时刻下的朝向和第二部件在所述滑动时间窗口内每一时刻下的朝向。
因此，在一些实施例中，朝向损失可基于第一部件朝向残差和第一部件朝向残差，其中，第一部件朝向残差根据第一部件在滑动时间窗口内每一时刻下的朝向和第一部件在滑动时间窗口内每一时刻下的朝向观测值计算得到，并且第二部件朝向残差根据第二部件在滑动时间窗口内每一时刻下的朝向和第二部件在滑动时间窗口内每一时刻下的朝向观测值计算得到。第一部件朝向残差和第一部件朝向残差可以参考上面所述，在此不再赘述。
在一些实施例中，所述第一部件的朝向观测值为至少一个观测模型观测到的第一部件的朝向、车道线朝向、或第一部件的速度方向，并且第二部件的朝向观测值为所述至少一个观测模型观测到的第二部件的朝向、车道线朝向、或第二部件的速度方向。
在一些实施例中，在目标车辆为包括第一部件和第二部件的车辆时，状态量包括第一部件在滑动时间窗口内的平均朝向。因此，朝向损失可以包括第一部件的朝向平滑损失，第一部件的朝向平滑损失根据第一部件在每个第二时间窗口各时刻、第一部件在相应第二时间窗口内各时刻下的朝向状态量、以及第一部件在所述相应第二时间窗口内的平均朝向状态量计算得到。
在一些实施例中，在目标车辆为包括第一部件和第二部件的车辆时，状态量包括第一部件在滑动时间窗口内的平均朝向。因此，第一部件的朝向平滑损失还可以根据第一部件在第一时间窗口内各时刻下的朝向状态量、以及第一部件在第一时间窗口内的平均朝向状态量计算得到。
可以理解的是，朝向损失还可以包括第二部件的朝向平滑损失，在此不做赘述。
在一些实施例中，第一时间窗口的滑动步长小于所述第一时间窗口的长度。因此，在目标车辆为包括第一部件和第二部件的车辆时，朝向损失可以包括第一部件的朝向先验损失，第一部件的朝向先验损失根据以下各项计算得到：第一时间窗口与前一第一时间窗口的重叠区域内各时刻下第一部件的朝向、以及在针对所述前一第一时间窗口进行的状态量优化过程中所述重叠区域内各时刻下第一部件的已优化朝向。
根据一些实施例，所述损失函数包括与所述目标的朝向相关联的朝向损失；朝向损失包括朝向平滑损失，朝向平滑损失根据所述每个第二时间窗口各时刻、所述目标在相应第二时间窗口内各时刻下的朝向状态量、以及所述目标在所述相应第二时间窗口内的平均朝向状态量计算得到。朝向损失进一步包括角速度约束，角速度约束根据以下各项计算得到：卡车在第一时间窗口内各时刻下的速度状态量、在第一时间窗口内第二部件的长度、在第一时间窗口内在第一部件与第二部件之间的铰链的长度、第一部件在第一时间窗口内各时刻下的朝向状态量、以及第二部件在第一时间窗口内各时刻下的朝向状态量。
具体地，第二部件的朝向观测还存在一个公式(11)所示的运动约束。则角速度损失可以如公式(24)所示。
其中，Lt和Lh分别为第一部件和转轴结构的长度，其计算方式将在下面参考尺寸损失进行描述。
综上所述，完整的朝向损失项例如可以表示为公式(25)所示。
其中，基于第二时间窗口所确定的平滑约束项为多项，即等于第二时间窗口的个数。
根据一些实施例，所述损失函数进一步包括与所述目标的位置相关联的位置损失。位置损失包括至少一个基准点残差；基准点残差包括以下至少一种：中心点残差和轮廓角点残差。
根据一些实施例，位置包括至少一个基准点的位置，所述基准点包括以下至少一种：中心点和轮廓角点(例如车辆检测框的四角点)。位置损失包括至少一个基准点残差，基准点残差包括以下至少一种：中心点残差和轮廓角点残差，所述中心点残差代表对于中心点的观测量和状态量的差异，轮廓角点残差代表对于轮廓角点的观测量和状态量的差异。
具体地，假设基于L个观测模型所获得的观测数据对目标车辆的状态量进行优化，L为正整数。如果第l个观测模型的中心点观测量为则可以基于中心点的观测量和状态量之间的差异构建中心点残差/>如果第l个观测模型还提供轮廓观测，轮廓观测量为/>则可以基于轮廓角点的观测量和状态量之间的差异构建轮廓角点残差
在一些实施例中，中心点的状态量可以基于速度进行刻画，以实现通过中心点残差进一步优化速度状态量。具体地，在观测量包括目标车辆在滑动时间窗口内每一时刻下的中心点坐标、状态量包括目标车辆在该滑动时间窗口内每一时刻下的速度时，中心点残差可以根据目标车辆在该滑动时间窗口内每一时刻下的中心点坐标和目标车辆在该滑动时间窗口内每一时刻下的速度计算得到。
具体地，假设基于L个观测模型所获得的观测数据对目标车辆的状态量进行优化，L为正整数。如果第l个观测模型的中心点观测量为确定目标车辆所对应的第一帧位置坐标为p0，/>代表第l个观测模型在tk时刻的中心点残差矢量，如公式(26)所示：
其中，
vi＝[vicos(θi),visin(θi)]T 公式(28)
在一些实施例中，轮廓角点的状态量可以基于中心点的状态量进行刻画，以实现通过轮廓角点残差进一步优化中心点状态量。具体地，在观测量包括目标车辆在滑动时间窗口内每一时刻下的轮廓角点坐标时，基准点残差可以根据以下各项计算得到：目标车辆在该滑动时间窗口内的初始时刻下的中心点坐标、目标车辆在该滑动时间窗口内每一时刻下的速度、目标车辆在该滑动时间窗口内每一时刻下的轮廓角点坐标、以及目标车辆在该滑动时间窗口内每一时刻下的中心点坐标到轮廓角点坐标的相应矢量。
具体地，如果第l个观测模型还提供轮廓观测，轮廓观测为则可以得到轮廓角点残差，如公式(29)所示。
其中，
其中，φm表示车辆中心点到车辆轮廓角点的矢量。
如上所述，在参考图3所述的卡车运动模型中，拖车对牵引车的轮廓观测产生约束。因此，在对牵引车的相应状态量(例如上面所述的速度状态量)进行优化时，在上面所描述的基准点残差的基础上，可以进一步引入拖车对牵引车的轮廓观测的约束条件。
根据一些实施例，中心点残差和轮廓角点残差分别具有对应的权重，且权重均为对角矩阵；中心点残差和轮廓角点残差的每一种包括横向残差分量和纵向残差分量，且横向残差分量和纵向残差分量分别具有对应的权重。
在根据本公开的示例中，当目标为车辆时，横向方向可以为垂直于目标车辆的近似朝向的水平方向；纵向方向可以为平行于目标车辆的近似朝向的水平方向。具体地，该“近似朝向”例如可以包括所观测到的目标车辆的车体朝向、目标车辆所在车道的车道朝向(即车道线航向)，等等。
因此，根据一些实施例，在目标为车辆时，所述横向残差分量垂直于所述车辆所在的车道朝向，所述纵向残差分量平行于所述车辆所在的车道线航向；或者所述横向残差分量垂直于所述车辆的车体朝向，所述纵向残差分量平行于所述车辆的车体朝向。
在本公开中，在状态量进行估计时关注横向方向和纵向方向，并且为了方便模型调优，可以将横向方向与纵向方向进行解耦。在一些示例中，例如已知雷达传感器所观测到的车体朝向或车道朝向，因此可以将ENU坐标系下的残差通过Rbw矩阵旋转到参考坐标系下，则包括中心点残差和轮廓角点残差的位置损失函数可以如公式(31)所示。
其中，ρ(·)是鲁棒函数；为权重矩阵(对角线矩阵)，分别为横向残差和纵向残差设置有不同权重；Rbw如上参考公式(6)所述。
在本公开中，鲁棒函数ρ(·)可以为基于任何合适的损失函数的鲁棒函数，包括但不限于Cauchy(Lorentzian)、Charbonnier(pseudo-Huber、L1-L2)、Huber、Geman-McClure、smooth truncated quadratic、truncated quadratic、Tukey’s biweight，等等。示例地，可以选择如Huber的凸损失函数，以保持凸优化问题。然而，凸损失函数可能对异常值的鲁棒性有限。因此，在一些示例中，可以选择非凸损失函数。
根据一些实施例，当中心点残差和轮廓角点残差的其中一个的横向方差小于预定阈值时，对应的横向残差分量的权重取第一固定值；当中心点残差和轮廓角点残差的其中一个的纵向方差小于预定阈值时，对应的纵向残差分量的权重取第一固定值。
在一些示例中，以中心点残差为例，如果中心点方差的横向中心点方差分量和纵向中心点方差分量中的至少一项小于相应的第一阈值时，横向中心点残差分量和纵向中心点残差分量的相应的权重为第一固定值。并且，当横向中心点方差分量和纵向中心点方差分量中的至少一项不小于相应的第一阈值时，横向中心点残差分量和纵向中心点残差分量中的至少一项的权重与横向中心点方差分量和纵向中心点方差分量中的所述至少一项负相关。
在一些示例中，轮廓角点残差可以与上述中心点残差类似，即基于轮廓角点方差确定轮廓角点残差所对应的权重。
具体地，权重矩阵与方差负相关，已知横向方差和纵向方差，权重矩阵可以表示为公式(32)所示：
其中，wlong、wlat、a、b均为超参数。受限于观测模型的准确度，方差很小时不能准确反映真实误差，因此通过公式(32)，当方差低于阈值时，使用固定的权重。在本公开中，对于所有观测损失项，均可以使用与公式(32)类似的权重公式。
根据一些实施例，所述损失函数进一步包括与所述目标的尺寸相关联的尺寸损失。尺寸损失项包括以下至少一项：尺寸先验损失、以及各时刻下已优化过的尺寸累计损失。尺寸先验损失包括所述目标在每一当前时刻的尺寸量与在针对所述前一第一时间窗口进行的状态量优化过程中已优化的所述目标的尺寸量的残差；尺寸累计损失包括所述目标在初始时刻到上一优化时刻之间的各尺寸损失的总和。
在一些实施例中，第一时间窗口的滑动步长小于第一时间窗口的长度。因此，尺寸损失项可以包括尺寸先验损失，尺寸先验损失根据以下各项计算得到：第一时间窗口与前一第一时间窗口的重叠区域内各时刻下目标的尺寸、以及在针对所述前一第一时间窗口进行的状态量优化过程中所述重叠区域内各时刻下目标的已优化尺寸。
根据一些实施例，尺寸累计损失采用增量式更新方法计算得到；所述目标在各时刻下的观测量为所述目标在第一时间窗口内的各时刻下的观测量；所述目标在各时刻下的状态量为所述目标在第一时间窗口内的各时刻下的状态量；所述第一时间窗口包括多个数据时刻，所述各时刻为所述多个数据时刻中的至少两个时刻。
具体地，尺寸累计损失包括所述目标在初始时刻到上一优化时刻之间的各尺寸损失的总和。该初始时刻为最初开始进行状态量优化的时刻，例如所获得的第一帧数据时刻。上一次优化时刻例如可以为前一第一时间窗口内的最后一个时刻。示例地，目标车辆轮廓观测可以提供目标车辆的尺寸信息，因此，尺寸累计损失可以根据以下各项计算得到：未落入当前第一时间窗口内且落入前一第一时间窗口内的各时刻对应的基于基准点残差所确定的尺寸损失、以及在针对所述前一第一时间窗口进行的状态量优化过程中使用的尺寸累计损失。
在一些实施例中，所述观测量包括目标车辆在所述第一时间窗口内每一时刻下的轮廓角点坐标和目标车辆在第一时间窗口内每一时刻下的中心点坐标。状态量包括目标车辆在所述第一时间窗口内每一时刻下的速度，并且每一时刻对应的基准点残差根据以下各项计算得到：目标车辆在该时刻下的中心点坐标观测量、目标车辆在该时刻下的轮廓角点坐标观测量、以及目标车辆在该时刻下的中心点坐标观测量到轮廓角点坐标观测量的相应矢量，该相应矢量可以根据公式(30)确定。
具体地，在优化框架中，车体尺寸或者卡车的牵引车尺寸被认为是一个待优化的全局变量。当更新过当前状态后，最旧的一帧将被剔除并不会再更新。尽管滑动窗口外的状态量是固定的，但它们也能提供一些关于全局尺寸变量的信息。具体地，当第i帧剔除掉后，如果轮廓观测可用，则可以生成一个新的尺寸损失，如公式(33)所示。
其中，和/>是常量，/>是公式(32)中根据方差计算出的权重。
由于拉普拉斯分布可以等价地表示为高斯分布与逆高斯分布的乘积，在一些示例中，则可以使用γ＝diag(γ0,γ1)来近似Huber损失函数的L2项，如公式(34)所示，以具有更好的鲁棒性。
其中，δ表示预设参数，ri表示公式(33)中的
尺寸损失项的数目会随着时间推移而变多，为了避免冗余计算，在根据本公开的实施例中，以增量形式将它们合并为一项，因此Ti时刻的损失项可表示为公式(35)所示。
/>
其中，Ai可以通过SVD分解方法计算，如公式(36)-(38)所示：
Ai＝(UΛVT)T 公式(38)
其中，是对称矩阵，因此U＝V。bi可以如公式(39)所示。
在一些实施例中，在目标为包括第一部件和第二部件的示例中，例如如图3所示的卡车模型，拖车以及连接该拖车和牵引车的转轴结构尺寸可以通过观测量计算得到，如下公式(40)-(42)所示。
其中，公式(40)-(42)是优化问题的解，如公式(43)所示。
综上所述，已知L和W的先验损失，总的尺寸损失项可以如公式(44)所示。
其中，Es中的第一项即为尺寸累计损失，第二项为尺寸先验损失。
在本公开中，基于包括目标的位置损失、朝向损失、速度损失、尺寸损失、结构约束中的至少一项的损失函数，通过最小化该损失函数，即可对该目标在各时刻下的状态量进行优化，从而获得优化后的各项状态量。在无人驾驶领域中，根据本公开方法可以更精准地更新周围车辆的状态信息，以便无人驾驶系统基于此结果做出安全的路径规划，从而避免交通事故发生。
根据本公开的实施例，如图5所示，还提供了一种目标状态估计装置500，包括：获取单元510，配置为：获取对应于多个时刻的数据帧序列；确定单元520，配置为：确定所述数据序列中的第一时间窗口，所述第一时间窗口包括一组待优化的第一状态量集合；选取单元530，配置为：从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；优化单元540，配置为：通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
参照图6，现将描述计算设备2000，其是可以应用于本公开的各方面的硬件设备的示例。计算设备2000可以是被配置为执行处理和/或计算的任何机器，可以是但不限于工作站、服务器、台式计算机、膝上型计算机、平板计算机、个人数字助理、智能电话、车载计算机或其任何组合。上述目标状态估计装置可以全部或至少部分地由计算设备2000或类似设备或系统实现。
计算设备2000可以包括(可能经由一个或多个接口)与总线2002连接或与总线2002通信的元件。例如，计算设备2000可以包括总线2002、一个或多个处理器2004、一个或多个输入设备2006以及一个或多个输出设备2008。一个或多个处理器2004可以是任何类型的处理器，并且可以包括但不限于一个或多个通用处理器和/或一个或多个专用处理器(例如特殊处理芯片)。输入设备2006可以是能向计算设备2000输入信息的任何类型的设备，并且可以包括但不限于鼠标、键盘、触摸屏、麦克风和/或遥控器。输出设备2008可以是能呈现信息的任何类型的设备，并且可以包括但不限于显示器、扬声器、视频/音频输出终端、振动器和/或打印机。计算设备2000还可以包括非暂时性存储设备2010或者与非暂时性存储设备2010连接，非暂时性存储设备可以是非暂时性的并且可以实现数据存储的任何存储设备，并且可以包括但不限于磁盘驱动器、光学存储设备、固态存储器、软盘、柔性盘、硬盘、磁带或任何其他磁介质，光盘或任何其他光学介质、ROM(只读存储器)、RAM(随机存取存储器)、高速缓冲存储器和/或任何其他存储器芯片或盒、和/或计算机可从其读取数据、指令和/或代码的任何其他介质。非暂时性存储设备2010可以从接口拆卸。非暂时性存储设备2010可以具有用于实现上述方法和步骤的数据/程序(包括指令)/代码。计算设备2000还可以包括通信设备2012。通信设备2012可以是使得能够与外部设备和/或与网络通信的任何类型的设备或系统，并且可以包括但不限于调制解调器、网卡、红外通信设备、无线通信设备和/或芯片组，例如蓝牙TM设备、1302.11设备、WiFi设备、WiMax设备、蜂窝通信设备和/或类似物。
计算设备2000还可以包括工作存储器2014，其可以是可以存储对处理器2004的工作有用的程序(包括指令)和/或数据的任何类型的工作存储器，并且可以包括但不限于随机存取存储器和/或只读存储器设备。
软件要素(程序)可以位于工作存储器2014中，包括但不限于操作系统2016、一个或多个应用程序2018、驱动程序和/或其他数据和代码。用于执行上述方法和步骤的指令可以被包括在一个或多个应用程序2018中，并且上述目标状态估计装置的各个单元等可以通过由处理器2004读取和执行一个或多个应用程序2018的指令来实现。更具体地，另前述目标状态估计装置的获取单元510可以例如通过处理器2004执行具有执行步骤110的指令的应用程序2018而实现。另前述目标状态估计装置的构建单元520可以例如通过处理器2004执行具有执行步骤120的指令的应用程序2018而实现。此外，前述目标状态估计装置的优化单元530可以例如通过处理器2004执行具有执行步骤130的指令的应用程序2018而实现。软件要素(程序)的指令的可执行代码或源代码可以存储在非暂时性计算机可读存储介质(例如上述存储设备2010)中，并且在执行时可以被存入工作存储器2014中(可能被编译和/或安装)。软件要素(程序)的指令的可执行代码或源代码也可以从远程位置下载。
还应该理解，可以根据具体要求而进行各种变型。例如，也可以使用定制硬件，和/或可以用硬件、软件、固件、中间件、微代码，硬件描述语言或其任何组合来实现特定元件。例如，所公开的方法和设备中的一些或全部可以通过使用根据本公开的逻辑和算法，用汇编语言或硬件编程语言(诸如VERILOG，VHDL，C++)对硬件(例如，包括现场可编程门阵列(FPGA)和/或可编程逻辑阵列(PLA)的可编程逻辑电路)进行编程来实现。
还应该理解，前述方法可以通过服务器-客户端模式来实现。例如，客户端可以接收用户输入的数据并将所述数据发送到服务器。客户端也可以接收用户输入的数据，进行前述方法中的一部分处理，并将处理所得到的数据发送到服务器。服务器可以接收来自客户端的数据，并且执行前述方法或前述方法中的另一部分，并将执行结果返回给客户端。客户端可以从服务器接收到方法的执行结果，并例如可以通过输出设备呈现给用户。
还应该理解，计算设备2000的组件可以分布在网络上。例如，可以使用一个处理器执行一些处理，而同时可以由远离该一个处理器的另一个处理器执行其他处理。计算系统2000的其他组件也可以类似地分布。这样，计算设备2000可以被解释为在多个位置执行处理的分布式计算系统。
虽然已经参照附图描述了本公开的实施例或示例，但应理解，上述的方法、系统和设备仅仅是示例性的实施例或示例，本发明的范围并不由这些实施例或示例限制，而是仅由授权后的权利要求书及其等同范围来限定。实施例或示例中的各种要素可以被省略或者可由其等同要素替代。此外，可以通过不同于本公开中描述的次序来执行各步骤。进一步地，可以以各种方式组合实施例或示例中的各种要素。重要的是随着技术的演进，在此描述的很多要素可以由本公开之后出现的等同要素进行替换。

Claims (20)

1.一种目标状态估计方法，包括：
获取对应于多个时刻的数据帧序列；
在所述数据帧序列中确定第一时间窗口，所述第一时间窗口包括一组待优化的第一状态量集合；
从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；
通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
2.如权利要求1所述的方法，其中，所述第一状态量集合包括以下至少一种：
所述目标在所述第一时间窗口内各时刻下的速度、位置、朝向、尺寸中的至少一种；
所述目标在所述第一时间窗口内的平均速度、平均位置、平均朝向中的至少一种。
3.如权利要求1所述的方法，其中，所述第二状态量集合包括以下至少一种：
所述目标在所述第二时间窗口内各时刻下的速度、位置、朝向、尺寸中的至少一种；
所述目标在所述第二时间窗口内的平均速度、平均位置、平均朝向中的至少一种。
4.如权利要求1所述的方法，其中，
所述优化通过最小化损失函数实现；
所述损失函数基于所述多个第二时间窗口内各时刻下所述目标的状态量和所述目标的观测量确定得到；
所述观测量通过至少一个观测模型观测得到，所述至少一个观测模型基于至少一个传感器得到。
5.如权利要求4所述的方法，其中，
所述损失函数包括基于所述多个第二时间窗口确定的所述状态量的平滑损失；
所述平滑损失根据所述目标在每个第二时间窗口内各时刻下的状态量以及所述目标在所述每个第二时间窗口内的所述状态量的平均值计算得到。
6.如权利要求4所述的方法，其中，
所述损失函数包括与所述目标的速度相关联的速度损失；
所述速度损失包括速度平滑损失，所述速度平滑损失根据所述每个第二时间窗口内各时刻、所述目标在相应第二时间窗口内各时刻下的速度状态量、以及所述目标在所述相应第二时间窗口内的平均速度状态量计算得到。
7.如权利要求6所述的方法，其中，
所述速度平滑损失进一步根据所述目标在所述第一时间窗口内各时刻下的速度状态量、以及所述目标在所述第一时间窗口内的平均速度状态量计算得到。
8.如权利要求6所述的方法，其中，
所述速度损失进一步包括速度先验损失，所述速度先验损失根据以下各项计算得到：当前所述第一时间窗口与前一第一时间窗口的重叠区间内各时刻下的速度状态量、以及在针对所述前一第一时间窗口进行的状态量优化过程中所述重叠区间内各时刻下的已优化速度状态量。
9.如权利要求6所述的方法，其中，
所述速度损失进一步包括速度残差，所述速度残差根据所述目标在所述第一时间窗口内各时刻下的速度观测量和所述目标在所述第一时间窗口内各时刻下的速度状态量计算得到。
10.如权利要求4所述的方法，其中，
所述损失函数包括与所述目标的朝向相关联的朝向损失；
所述朝向损失包括朝向平滑损失，所述朝向平滑损失根据所述每个第二时间窗口各时刻、所述目标在相应第二时间窗口内各时刻下的朝向状态量、以及所述目标在所述相应第二时间窗口内的平均朝向状态量计算得到。
11.如权利要求10所述的方法，其中，
所述朝向平滑损失进一步根据所述目标在所述第一时间窗口内各时刻下的朝向状态量、以及所述目标在所述第一时间窗口内的平均朝向状态量计算得到。
12.如权利要求10所述的方法，其中，
所述朝向损失进一步包括朝向先验损失，所述朝向先验损失根据以下各项计算得到：所述第一时间窗口与前一第一时间窗口的重叠区间内各时刻下的朝向状态量、以及在针对所述前一第一时间窗口进行的状态量优化过程中所述重叠区间内各时刻下的已优化朝向状态量。
13.如权利要求2或3所述的方法，其中，所述目标为车辆，所述车辆包括第一部件和至少一个第二部件，所述第二部件能够围绕所述第一部件旋转；
所述目标的位置包括以下至少一项：所述第一部件的位置、各所述第二部件的位置、所述车辆的位置；
所述目标的尺寸包括以下至少一项：所述第一部件的尺寸、各所述第二部件的尺寸、所述车辆的尺寸；
所述目标的朝向包括以下至少一项：所述速度的朝向、所述第一部件的朝向、所述车辆所在车道的朝向。
14.如权利要求13所述的方法，其中，
所述损失函数包括与所述目标的朝向相关联的朝向损失；
所述朝向损失包括朝向平滑损失，所述朝向平滑损失根据所述每个第二时间窗口各时刻、所述目标在相应第二时间窗口内各时刻下的朝向状态量、以及所述目标在所述相应第二时间窗口内的平均朝向状态量计算得到，其中，
所述朝向损失进一步包括朝向残差，所述朝向残差包括第一部件朝向残差和/或第二部件朝向残差；
所述第一部件朝向残差根据所述第一部件在所述第一时间窗口内各时刻下的朝向状态量和所述第一部件在所述第一时间窗口内各时刻下的朝向观测量计算得到；
所述第二部件朝向残差根据所述第二部件在所述第一时间窗口内各时刻下的朝向状态量和所述第二部件在所述第一时间窗口内各时刻下的朝向观测量计算得到。
15.如权利要求13所述的方法，其中，
所述损失函数包括与所述目标的朝向相关联的朝向损失；
所述朝向损失包括朝向平滑损失，所述朝向平滑损失根据所述每个第二时间窗口各时刻、所述目标在相应第二时间窗口内各时刻下的朝向状态量、以及所述目标在所述相应第二时间窗口内的平均朝向状态量计算得到，其中，
所述朝向损失进一步包括角速度约束，所述角速度约束根据以下各项计算得到：所述卡车在所述第一时间窗口内各时刻下的速度状态量、在所述第一时间窗口内所述第二部件的长度、在所述第一时间窗口内在所述第一部件与所述第二部件之间的铰链的长度、所述第一部件在所述第一时间窗口内各时刻下的朝向状态量、以及所述第二部件在所述第一时间窗口内各时刻下的朝向状态量。
16.如权利要求4所述的方法，其中，
所述损失函数进一步包括与所述目标的位置相关联的位置损失；
所述位置损失包括至少一个基准点残差；
所述基准点残差包括以下至少一种：中心点残差和轮廓角点残差。
17.如权利要求4所述的方法，其中，
所述损失函数进一步包括与所述目标的尺寸相关联的尺寸损失；
所述尺寸损失项包括以下至少一项：尺寸先验损失、以及各时刻下已优化过的尺寸累计损失；
所述尺寸先验损失包括所述目标在每一当前时刻的尺寸量与在针对所述前一第一时间窗口进行的状态量优化过程中已优化的所述目标的尺寸量的残差；
所述尺寸累计损失包括所述目标在初始时刻到上一优化时刻之间的各尺寸损失的总和。
18.一种目标状态估计装置，包括：
获取单元，配置为：获取对应于多个时刻的数据帧序列；
确定单元，配置为：确定所述数据序列中的第一时间窗口，所述第一时间窗口包括一组待优化的第一状态量集合；
选取单元，配置为：从所述第一时间窗口内确定多个第二时间窗口，所述第二时间窗口内的数据帧数小于所述第一时间窗口内的数据帧数，相邻两个第二时间窗口具有重复的数据帧，且每个第二时间窗口具有一组待优化的第二状态量集合；
优化单元，配置为：通过同时对所述多个第二时间窗口内的第二状态量集合进行优化，得到优化后的第一状态量集合。
19.一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-17中任一项所述的方法。
20.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-17中任一项所述的方法。
CN202210837617.3A 2022-07-15 2022-07-15 一种目标状态估计方法、装置、电子设备和介质 Pending CN117437770A (zh)

Priority Applications (5)

Application Number Priority Date Filing Date Title
CN202210837617.3A CN117437770A (zh) 2022-07-15 2022-07-15 一种目标状态估计方法、装置、电子设备和介质
EP23184287.3A EP4307244A1 (en) 2022-07-15 2023-07-07 Method, apparatus, electronic device and medium for target state estimation
AU2023204634A AU2023204634A1 (en) 2022-07-15 2023-07-12 Method, apparatus, electronic device and medium for target state estimation
JP2023114661A JP2024012160A (ja) 2022-07-15 2023-07-12 目標状態推定方法、装置、電子機器及び媒体
US18/351,931 US20240020870A1 (en) 2022-07-15 2023-07-13 Method, electronic device and medium for target state estimation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210837617.3A CN117437770A (zh) 2022-07-15 2022-07-15 一种目标状态估计方法、装置、电子设备和介质

Publications (1)

Publication Number Publication Date
CN117437770A true CN117437770A (zh) 2024-01-23

Family

ID=87196382

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210837617.3A Pending CN117437770A (zh) 2022-07-15 2022-07-15 一种目标状态估计方法、装置、电子设备和介质

Country Status (5)

Country Link
US (1) US20240020870A1 (zh)
EP (1) EP4307244A1 (zh)
JP (1) JP2024012160A (zh)
CN (1) CN117437770A (zh)
AU (1) AU2023204634A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US12008037B2 (en) * 2020-11-03 2024-06-11 Samsung Electronics Co., Ltd. Method of video search in an electronic device

Also Published As

Publication number Publication date
JP2024012160A (ja) 2024-01-25
EP4307244A1 (en) 2024-01-17
AU2023204634A1 (en) 2024-02-01
US20240020870A1 (en) 2024-01-18

Similar Documents

Publication Publication Date Title
KR102581263B1 (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
JP2021523443A (ja) Ｌｉｄａｒデータと画像データの関連付け
EP3654064B1 (en) Apparatus and method for characterizing an object based on measurement samples from one or more location sensors
RU2767949C2 (ru) Способ (варианты) и система для калибровки нескольких лидарных датчиков
US11893496B2 (en) Method for recognizing objects in an environment of a vehicle
Daraei et al. Velocity and shape from tightly-coupled LiDAR and camera
Yap et al. A particle filter for monocular vision-aided odometry
WO2021102676A1 (zh) 物体状态获取方法、可移动平台及存储介质
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
US20240025428A1 (en) Method, electronic device and medium for target state estimation
US20240020870A1 (en) Method, electronic device and medium for target state estimation
CN114964212A (zh) 面向未知空间探索的多机协同融合定位与建图方法
US11080562B1 (en) Key point recognition with uncertainty measurement
Xu et al. Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs
US11488391B2 (en) Method and apparatus for estimating position
Dichgans et al. Robust Vehicle Tracking with Monocular Vision using Convolutional Neuronal Networks
EP3654065B1 (en) Apparatus and method for characterizing an object based on measurement samples from one or more location sensors
CN114428251A (zh) 一种雷达测量精度的确定方法及装置
CN117405118B (zh) 一种多传感器融合建图方法、系统、设备及存储介质
US20230009736A1 (en) Adaptive motion compensation of perception channels
US20230085898A1 (en) Automatic cross-sensor calibration using object detections
US20230025579A1 (en) High-definition mapping
CN115471520A (zh) 测量方法及装置
CN118131212A (zh) 一种识别环境对象的方法和电子设备
Itu et al. Machine learning based automatic extrinsic calibration of an onboard monocular camera for driving assistance applications on smart mobile devices

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