CN117412895A - 跟踪装置、跟踪方法、跟踪程序 - Google Patents

跟踪装置、跟踪方法、跟踪程序 Download PDF

Info

Publication number
CN117412895A
CN117412895A CN202280034122.5A CN202280034122A CN117412895A CN 117412895 A CN117412895 A CN 117412895A CN 202280034122 A CN202280034122 A CN 202280034122A CN 117412895 A CN117412895 A CN 117412895A
Authority
CN
China
Prior art keywords
value
observation
state value
moving body
tracking
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
CN202280034122.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.)
Denso Corp
Original Assignee
Denso Corp
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 Denso Corp filed Critical Denso Corp
Publication of CN117412895A publication Critical patent/CN117412895A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
    • B60W40/04—Traffic conditions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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/66—Tracking systems using electromagnetic waves other than radio waves
    • 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
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0248—Filters characterised by a particular frequency response or filtering method
    • H03H17/0255—Filters based on statistics
    • H03H17/0257—KALMAN filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/10—Numerical modelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Hardware Design (AREA)
  • Electromagnetism (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • Mathematical Physics (AREA)
  • Mechanical Engineering (AREA)
  • Transportation (AREA)
  • Automation & Control Theory (AREA)
  • Probability & Statistics with Applications (AREA)
  • Human Computer Interaction (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明的跟踪装置的处理器构成为执行如下步骤：获取在观测时刻观测到的移动体的观测值；通过预测在观测时刻的移动体的状态值，来获取预测状态值；以及通过将预测状态值和在观测时刻的观测值作为变量的非线性滤波，来估计在观测时刻的状态值的真值。估计真值包含如下步骤：根据来自外界传感器系统的每个顶点(m)的视觉辨认度来设定对移动体进行建模而成的矩形模型中的多个顶点(m)中的每个顶点的权重系数(s)；基于预测状态值和在观测时刻的观测值来获取各顶点(m)处的观测误差；以及基于每个顶点(m)的权重系数(s)来获取观测误差的协方差。

Description

跟踪装置、跟踪方法、跟踪程序
相关申请的交叉引用
本申请以在2021年5月14日向日本申请的日本专利申请第2021-82666号为基础，通过参照整体上引用基础申请的内容。
技术领域
本公开涉及跟踪移动体的跟踪技术。
背景技术
基于外界传感器系统的观测值通过按时间序列估计移动体的状态值来跟踪移动体的跟踪技术已广为人知。作为跟踪技术的一种，在非专利文献1中提出了通过使用卡尔曼滤波器的滤波，按时间序列反复估计移动体的状态值的方法。
非专利文献1：3D Multi-Object Tracking:A Baseline and New EvaluationMetrics(URL：https://arxiv.org/pdf/1907.03961.pdf)
但是，非专利文献1提出的方法以根据外界传感器系统充分观测移动体的整体为前提。因此，在很难通过外界传感器系统观测移动体中的一部分的情况下，存在所估计的状态值偏离真值，跟踪精度降低的担忧。
发明内容
本公开的课题在于提供一种提高针对移动体的跟踪精度的跟踪装置。本公开的另一课题在于提供一种提高针对移动体的跟踪精度的跟踪方法。本公开的又一课题在于提供一种提高针对移动体的跟踪精度的跟踪程序。
以下，对用于解决课题的本公开的技术方案进行说明。
本公开的第一方式是一种跟踪装置，具有处理器，基于外界传感器系统的观测值通过按时间序列估计移动体的状态值来跟踪移动体，
处理器构成为执行如下步骤：
获取在观测时刻观测到的移动体的观测值；
通过预测在观测时刻的移动体的状态值，来获取预测状态值；以及
通过将预测状态值和在观测时刻的观测值作为变量的非线性滤波，来估计在观测时刻的状态值的真值，
估计真值包含如下步骤：
根据来自外界传感器系统的每个顶点的视觉辨认度来设定对移动体进行建模而成的矩形模型中的多个顶点中的每个顶点的权重系数；
基于预测状态值和在观测时刻的观测值来获取各顶点处的观测误差；以及
基于每个顶点的权重系数来获取观测误差的协方差。
本公开的第二方式是一种跟踪方法，是为了基于外界传感器系统的观测值通过按时间序列估计移动体的状态值来跟踪移动体，而由处理器估计的跟踪方法，
上述跟踪方法包含如下步骤：
获取在观测时刻观测到的移动体的观测值；
通过预测在观测时刻的移动体的状态值，来获取预测状态值，以及
通过将预测状态值和在观测时刻的观测值作为变量的非线性滤波，来估计在观测时刻的状态值的真值，
估计真值包含如下步骤：
根据来自外界传感器系统的每个顶点的视觉辨认度来设定对移动体进行建模而成的矩形模型中的多个顶点中的每个顶点的权重系数；
基于预测状态值和在观测时刻的观测值来获取各顶点处的观测误差；以及
基于每个顶点的权重系数来获取观测误差的协方差。
本公开的第三方式是一种跟踪程序，该跟踪程序存储于存储介质，且该跟踪程序包含为了基于外界传感器系统的观测值通过按时间序列估计移动体的状态值来跟踪移动体而使处理器执行的指令，
指令包含：
获取在观测时刻观测到的移动体的观测值；
通过预测在观测时刻的移动体的状态值，来获取预测状态值；以及
通过将预测状态值和在观测时刻的观测值作为变量的非线性滤波，来估计观测时刻的状态值的真值，
估计真值包含如下步骤：
根据来自外界传感器系统的每个顶点的视觉辨认度来设定对移动体进行建模而成的矩形模型中的多个顶点中的每个顶点的权重系数；
基于预测状态值和在观测时刻的观测值来获取各顶点处的观测误差；以及
基于每个顶点的权重系数来获取观测误差的协方差。
根据这些第一方式～第三方式，通过将预测状态值和在观测时刻的观测值作为变量的非线性滤波，来估计在观测时刻的状态值的真值。此时，基于预测状态值和在观测时刻的观测值来获取对移动体进行建模而成的矩形模型中的各顶点处的观测误差，并且基于这些每个顶点的权重系数来获取该观测误差的协方差。因此，根据按照来自外界传感器系统的视觉辨认度对每个顶点来设定权重系数的第一方式～第三方式，能够将该视觉辨认度反映到状态值的真值估计中。因此，能够精确地估计状态值的真值，提高针对移动体的跟踪精度。
附图说明
图1是表示一个实施方式的跟踪装置的整体结构的框图。
图2是用于对一个实施方式的观测值以及矩形模型进行说明的示意图。
图3是表示一个实施方式的跟踪装置的功能结构的框图。
图4是表示一个实施方式的跟踪方法的流程图。
图5是用于对一个实施方式的预测状态值以及矩形模型进行说明的示意图。
图6是表示一个实施方式的估计处理的流程图。
图7是表示一个实施方式的权重设定例的示意图。
图8是表示一个实施方式的权重设定子例程的流程图。
具体实施方式
以下，基于附图对一个实施方式进行说明。
如图1所示，一个实施方式的跟踪装置1通过基于外界传感器系统2的观测值按时间序列估计移动体3的状态值，来跟踪移动体3。因此，跟踪装置1与外界传感器系统2一起搭载于车辆4。
在车辆4中，通过与手动驾驶模式之间的切换暂时地、或者不实质执行该切换而稳定地提供自动驾驶模式。自动驾驶模式也可以通过有条件的驾驶自动化、高度驾驶自动化或者完全驾驶自动化这样的工作时的系统执行所有驾驶任务的自主行驶控制来实现。自动驾驶模式也可以通过驾驶辅助或者部分驾驶自动化这样的由乘员执行一部分或者所有驾驶任务的高度驾驶辅助控制来实现。自动驾驶模式也可以通过这些自主行驶控制和高度驾驶辅助控制中的任意一方、或组合、或切换来实现。
外界传感器系统2通过观测在车辆4的外界设定的感测区域AS内，输出该感测区域AS中的观测值。外界传感器系统2由例如LiDAR(Light Detection and Ranging/LaserImaging Detection and Ranging：光探测和测距/激光成像探测和测距)、雷达、相机、或者这些感测设备中的至少两种的融合构成。
外界传感器系统2被控制为以规定的跟踪周期反复观测。当在感测区域AS内存在移动体3的情况下，从外界传感器系统2按每个那样的跟踪周期输出针对该移动体3的在观测时刻k的观测值zk。在这里，观测值zk由使用图2中示意性地示出的物理量的式1来定义。在式1中，x、y分别是在观测空间中定义的正交坐标系中的移动体3的横向中心位置以及纵向中心位置。在式1中，θ是在观测空间中定义的正交坐标系中的移动体3的相对于横向的方位角。在式1中，l、w分别是在观测空间中定义的正交坐标系中的移动体3的前后方向长度以及左右方向宽度。
[式1]
zk＝[x,y,θ,l,w]T
图1所示的跟踪装置1例如经由LAN(Local Area Network：局域网)、线束、内部总线以及无线通信线路等中的至少一种与外界传感器系统2连接。跟踪装置1包含至少一个专用计算机而构成。构成跟踪装置1的专用计算机也可以是承担包含车辆4的自动驾驶模式的驾驶控制的驾驶控制ECU(Electronic Control Unit：电子控制单元)。构成跟踪装置1的专用计算机也可以是估计车辆4的自身状态量的定位器ECU。构成跟踪装置1的专用计算机也可以是导航车辆4的行驶路径的导航ECU。构成跟踪装置1的专用计算机也可以是例如构建能够与车辆4之间进行通信的外部中心或者移动终端等的至少一个外部计算机。
构成跟踪装置1的专用计算机至少具有一个存储器10以及一个处理器12。存储器10是非临时存储由计算机可读取的程序以及数据等的例如半导体存储器、磁介质以及光学介质等中的至少一种非过渡性实体存储介质(non-transitory tangible storagemedium)。处理器12包含例如CPU(Central Processing Unit：中央处理器)、GPU(GraphicsProcessing Unit：图形处理器)以及RISC(Reduced Instruction Set Computer：精简指令集计算机)－CPU等中的至少一种作为核心。
处理器12执行存储于存储器10的跟踪程序所包含的多个指令。由此，跟踪装置1构建多个用于跟踪移动体3的功能模块。这样，在跟踪装置1中，通过为了跟踪移动体3而存储于存储器10的跟踪程序使处理器12执行多个指令，而构建多个功能模块。如图3所示，由跟踪装置1构建的多个功能模块包括预测模块100、观测模块110以及估计模块120。
以下根据图4对通过这些预测模块100、观测模块110以及估计模块120的协作，跟踪装置1跟踪移动体3的跟踪方法的流程进行说明。本流程按每个跟踪周期来执行。此外，在本流程中，“S”分别是指由跟踪程序所包含的多个指令执行的多个步骤。
在跟踪方法的S100中，预测模块100通过预测在观测时刻k的移动体3的状态值，来获取图3所示的预测状态值Zk|k－1及其误差协方差Pk|k－1。在这里，预测状态值Zk|k－1通过使用图5中示意性地示出的物理量的式2来定义。在式2中，X、Y分别是在观测空间中定义的正交坐标系中的移动体3的横向中心位置以及纵向中心位置。在式2中，Θ是在观测空间中定义的正交坐标系中的移动体3的相对于横向的方位角。在式2中，L、W分别是在观测空间中定义的正交坐标系中的移动体3的前后方向长度以及左右方向宽度。在式2中，Vx、Vy(省略图5中的显示)分别是在观测空间中定义的正交坐标系中的移动体3的横向速度以及纵向中心速度。
[式2]
Zk|k-1＝[X，Y，Θ，L，W，Vx，Vy]T
S100中的预测模块100通过对估计状态值Zk-1|k-1及其误差协方差Pk-1|k-1执行时刻转换后的时间更新运算，来预测性地获取预测状态值Zk|k-1，其中，该估计状态值Zk-1|k-1为在比观测时刻k靠前的过去时刻k-1由估计模块120估计为真值的状态值。此时，预测状态值Zk|k-1以及误差协方差Pk|k-1分别使用在过去时刻k-1的估计状态值Zk-1|k-1以及误差协方差Pk-1|k-1，通过式3～式5来获取。在这里，式4中的Q是系统噪声(过程噪声)的协方差矩阵。
[式3]
Zk|k-1＝FZk-1|k-1
[式4]
Pk|k-1＝FPk-1|k-1FT+Q
[式5]
在图4所示的跟踪方法的S110中，观测模块110从外界传感器系统2获取在观测时刻k的移动体3的图3所示的观测值zk。S110的观测值zk的获取也可以伴随着从外界传感器系统2输出观测值zk来执行，也可以在该输出的观测值zk暂时缓存在存储器10中之后来执行。另外，这样的S110的观测值zk的获取相对于S100的预测状态值Zk|k-1的获取可以并列(即，同时)执行，也可以在时间上前后执行。
在图4所示的跟踪方法的S120中，估计模块120通过估计在观测时刻k的移动体3的状态值，获取作为图3所示的该状态值的真值的估计状态值Zk|k及其误差协方差Pk|k。此时，通过将预测状态值Zk|k-1和在观测时刻k的观测值zk作为变量来进行观测更新运算的非线性滤波来获取估计状态值Zk|k以及误差协方差Pk|k。
具体而言，S120中的估计模块120执行图6所示的估计处理。在估计处理的S200中，估计模块120对如图2、图5、图7所示那样将移动体3模式化为矩形而建模而成的矩形模型M中的多个顶点mfl、mbl、mfr、mbr的每个顶点分别设定权重系数sfl、sbl、sfr、sbr。
此时，S200中的估计模块120执行图8所示的权重设定子例程，以使根据来自外界传感器系统2的每个顶点mfl、mbl、mfr、mbr的视觉辨认度ωfl、ωbl、ωfr、ωbr来设定各个权重系数sfl、sbl、sfr、sbr。本子例程反复执行与顶点mfl、mbl、mfr、mbr的数量对应的次数。因此，将顶点mfl、mbl、mfr、mbr中的在本子例程中成为各次的执行对象的顶点称为对象顶点m。此外，在本子例程的说明中，除了基于图7的一部分例示说明之外，作为在各种变量中以下标字符表示的后缀，省略了分别表示移动体3的左前方、左后方、右前方、右后方的fl、bl、fr、br的标记，例如对象顶点m。
在图8所示的权重设定子例程的S300中，估计模块120判定在外界传感器系统2的观测原点O与对象顶点m之间是否存在图7所示那样的遮蔽物标ST。在这里，遮蔽物标ST也可以是有关右后方顶点mbr的如图7的例示那样在连结观测原点O与对象顶点m(在该图中为mbr)之间的线段上与移动体3分开存在的物体。遮蔽物标ST也可以是有关右前方顶点mfr的如图7的例示那样在连结观测原点O与对象顶点m(在该图中为mfr)之间的线段上存在的与矩形模型M的一边对应的移动体3的构成部分。此外，观测原点O也可以是对构成外界传感器系统2的单个传感器设备设定的感测原点，也可以是通过构成外界传感器系统2的多个传感器设备的融合而在观测空间中假定的空间原点。
如图8所示，当在S300中判定为存在遮蔽物标ST的情况下，权重设定子例程移至S310，从而估计模块120将视觉辨认度ω设定为最低值。此处，在图7的例示的情况下，隐藏于遮蔽物标ST的右后方顶点mbr以及右前方顶点mfr的视觉辨认度ωbr、ωfr被设定为作为最低值的“0”。
如图8所示，当在S300中判定为不存在遮蔽物标ST的情况下，权重设定子例程移至S320，从而估计模块120判定对象顶点m是否存在于外界传感器系统2的感测区域AS外。其结果时，在判定为对象顶点m存在于感测区域AS外的情况下，权重设定子例程移至S310，从而估计模块120将视觉辨认度ω设定为最低值。此外，感测区域AS也可以是在构成外界传感器系统2的单个传感器设备中设定的视角，也可以是构成外界传感器系统2的多个传感器设备的视角彼此的重复区域。
在S320中，在判定为对象顶点m不存在于感测区域AS外的情况下，权重设定子例程移至S330，从而估计模块120获取视觉辨认判定角在这里，如图7所示，视觉辨认判定角/>被定义为在对象顶点m中与对象顶点m连接的两边与连结观测原点O和对象顶点m之间的线段所成的角中的最小角度。在这里，在有关左后方顶点mbl的图7的例示中最小角度/>或者在有关左前方顶点mfl的图7的例示中最小角度/>分别对应于视觉辨认判定角/>
如图8所示，接着S330，在权重设定子例程的S340中，估计模块120判定视觉辨认判定角是否超过90度。其结果是，在视觉辨认判定角/>超过90度的情况下，权重设定子例程移至S350，从而估计模块120将视觉辨认度ω设定为最高值。另一方面，在视觉辨认判定角为90度以下的情况下，权重设定子例程移至S360，从而估计模块120通过式6设定最低值以上且最高值以下的视觉辨认度ω。在这里，在有关左后方顶点mbl的图7的例示中，根据超过90度的视觉辨认判定角/>将视觉辨认度ωbl设定为作为最高值的“1”，另一方面，在有关左前方顶点mfl的图7的例示中，根据90度以下的视觉辨认判定角/>将视觉辨认度ωfl设定为式6中的运算值/>
[式6]
ω＝sinφ
如图8所示，接着S310、S350、S360，在权重设定子例程的S370中，估计模块120判定视觉辨认度ω是否是作为最小值的“0”。其结果是，在视觉辨认度ω为0的情况下，权重设定子例程移至S380，从而估计模块120将权重系数s设定为最大值smax。另一方面，在视觉辨认度ω不是0的情况下，权重设定子例程移至S390，从而估计模块120将权重系数s设定为视觉辨认度ω的倒数(1/ω)和最大值smax中较小的值。特别是，在权重系数s被设定为视觉辨认度ω的倒数的情况下，越是视觉辨认度ω高的顶点m，权重系数s被设定得越小。
在这里，在有关隐藏于遮蔽物标ST的右后方顶点mbr以及右前方顶点mfr的图7的例示中，权重系数s被设定为最大值smax。另一方面，在有关左后方顶点mbl以及左前方顶点mfl的图7的例示中，设定为作为视觉辨认度ω的最高值或者相对于式6中的运算值的倒数(1或者1/)和最大值smax中较小的值。此外，权重系数s的最大值smax被定义为能够避免后述的观测误差enew的协方差Snew因非线性滤波而成为无限大的保护值，大于“1”。
对所有顶点mfl、mbl、mfr、mbr完成这样的S200的权重设定子例程，从而如图6所示，估计处理移至S210。在S210中，估计模块120基于预测状态值Zk|k－1和在观测时刻k的观测值zk来获取各顶点mfl、mbl、mfr、mbr处的观测误差enew。
此时，S210中的估计模块120通过式7的非线性函数hnew以及式8～式11的矩阵转换函数将观测值zk的物理量x、y、θ、l、w转换为对矩形模型M的各顶点mfl、mbl、mfr、mbr展开后的展开观测值znew。在这里，在式8～式11中，xfl、xbl、xfr、xbr为构成展开观测值znew的位置坐标，其中，该展开观测值znew是如图2所示那样将观测值zk的横向位置x展开到各顶点mfl、mbl、mfr、mbr而得到的值。在式8～式11中，yfl、ybl、yfr、ybr为构成展开观测值znew的位置坐标，其中，该展开观测值znew是如图2所示那样将观测值zk的纵向位置y展开到各顶点mfl、mbl、mfr、mbr而得到的值。
[式7]
znew＝hnew(x,y,θ,l,w)＝[xfl,yfl,xbl，ybl，xbr,ybr,xfr，yfr]T
[式8]
[式9]
[式10]
[式11]
S210中的估计模块120通过式12的非线性函数hnew以及式13～式16的矩阵转换函数将预测状态值Zk|k-1的物理量X、Y、Θ、L、W转换为对矩形模型M的各顶点mfl、mbl、mfr、mbr展开后的展开状态值Znew。在这里，在式13～式16中，Xfl、Xbl、Xfr、Xbr为构成展开状态值Znew的位置坐标，其中，该展开状态值Znew是如图5所示那样将预测状态值Zk|k-1的横向位置X展开到各顶点mfl、mbl、mfr、mbr而得到的值。在式13～式16中，Yfl、Ybl、Yfr、Ybr为构成展开状态值Znew的位置坐标，其中，该展开状态值Znew是如图5所示将预测状态值Zk|k-1的纵向位置Y展开到各顶点mfl、mbl、mfr、mbr而得到的值。
[式12]
Znew＝hnew(X，Y，Θ，L，W)＝[Xfl，Yfl，Xbl，Ybl，Xbr，Ybr，Xfr，Yfr，]T
[式13]
[式14]
[式15]
[式16]
在S210中的估计模块120中，通过使用这样转换而成的展开观测值znew以及展开状态值Znew的式17，如图6那样获取观测误差enew。
[式17]
enew＝znew-Znew
接着S210，在估计处理的S220中，估计模块120基于每个顶点mfl、mbl、mfr、mbr的权重系数sfl、sbl、sfr、sbr来获取观测误差enew的协方差Snew。此时，估计模块120通过使用权重系数sfl、sbl、sfr、sbr的式18，来获取对每个顶点mfl、mbl、mfr、mbr进行加权后的观测误差enew的8×8协方差矩阵Rnew。在这里，式18的R’是针对横向位置以及纵向位置的协方差矩阵，是能够通过预先设定进行调整的调整参数。进而，估计模块120通过式19获取针对式12的非线性函数的偏微分矩阵(雅可比式)Hnew。像这样，在估计模块120中，通过与观测误差enew的协方差矩阵Rnew以及偏微分矩阵Hnew一起，使用了预测状态值Zk|k-1的误差协方差Pk|k-1的式20，来获取观测误差enew的协方差Snew。
[式18]
Rnew＝diag(sflR′，sblR′，sbrR′，sfrR′)
[式19]
[式20]
接着S220，在估计处理的S230中，估计模块120通过使用了扩展卡尔曼滤波器的非线性滤波，将作为更新预测状态值Zk|k-1后的真值的估计状态值Zk|k与其误差协方差Pk|k一起获取。此时，估计模块120通过与观测误差enew的协方差Snew以及偏微分矩阵Hnew一起使用了预测状态值Zk|k-1的误差协方差Pk|k-1的式21，获取扩展卡尔曼滤波器的卡尔曼增益Knew。由此，在估计模块120中，通过与卡尔曼增益Knew以及观测误差enew一起使用了预测状态值Zk|k-1的式22，来获取估计状态值Zk|k。进而，在估计模块120中，通过与卡尔曼增益Knew以及偏微分矩阵Hnew一起使用了预测状态值Zk|k-1的误差协方差Pk|k-1的式23，来获取估计状态值Zk|k的误差协方差Pk|k。在这里，式23的I是单位矩阵。
[式21]
[式22]
Zk|k＝Zk|k-1+Knewenew
[式23]
Pk|k＝(I-KnewHnew)Pk|k-1
如图4所示，由本次的跟踪周期中的跟踪方法获取到的最新的在观测时刻k的估计状态值Zk|k通过被输出到驾驶控制ECU，而被用于车辆4的包含自动驾驶模式的驾驶控制。另外，由本次的跟踪周期中的跟踪方法获取到的在观测时刻k的估计状态值Zk|k在下次的跟踪周期中的跟踪方法中作为在过去时刻k-1的估计状态值Zk-1|k-1，被用于由预测模块100进行的S100中的预测。
(作用效果)
以下对以上说明的本实施方式的作用效果进行说明。此外，在作用效果的说明中，作为各种变量中用下标字符表示的后缀，省略了分别表示移动体3的左前方、左后方、右前方、右后方的fl、bl、fr、fr的标记。
根据本实施方式，通过将预测状态值Zk|k-1和在观测时刻k的观测值zk作为变量的非线性滤波，来估计在观测时刻k的状态值的真值。此时，基于预测状态值Zk|k-1和在观测时刻k的观测值zk来获取对移动体3进行建模而成的矩形模型M中的各顶点m处的观测误差enew，并且基于这些每个顶点m的权重系数s来获取该观测误差enew的协方差Snew。因此，根据按照来自外界传感器系统2的视觉辨认度ω对每个顶点设定权重系数s的本实施方式，可以将该视觉辨认度ω反映到状态值的真值估计中。因此，能够精确地估计作为状态值的真值的估计状态值Zk|k，提高对移动体3的跟踪精度。
根据本实施方式，越是视觉辨认度ω高的顶点m，权重系数s被设定为越小。由此，关于来自外界传感器系统2的高的视觉辨认度ω的顶点m，由于协方差Snew的矩阵分量变小，因此状态值对真值估计的贡献度变高。因此，能够估计作为反映出来自外界传感器系统2的视觉辨认度ω的精确的真值的估计状态值Zk|k，提高对移动体3的跟踪精度。
根据本实施方式，对于与外界传感器系统2之间存在遮蔽物标ST的顶点m，权重系数s被设定为最大值smax。由此，关于由于隐藏于遮蔽物标ST而来自外界传感器系统2的视觉辨认度ω被假定为实质为零的顶点m，由于协方差Snew的矩阵分量变大，因此状态值对真值估计的贡献度变低。因此，能够估计作为反映出来自外界传感器系统2的视觉辨认度ω的精确的真值的估计状态值Zk|k，提高对移动体3的跟踪精度。
根据本实施方式，对于存在于外界传感器系统2的感测区域AS外的顶点m，权重系数s被设定为最大值smax。由此，关于由于在感测区域AS外而来自外界传感器系统2的视觉辨认度ω被假定为实质为零的顶点m，由于协方差Snew的矩阵分量变大，因此状态值对真值估计的贡献度变低。因此，能够估计作为反映出来自外界传感器系统2的视觉辨认度ω的精确的真值的估计状态值Zk|k，提高对移动体3的跟踪精度。
(其他实施方式)
以上，对一个实施方式进行了说明，但本公开并不限定于该实施方式来解释，能够在不脱离本公开的主旨的范围内应用于各种实施方式。
在变形例中，构成跟踪装置1的专用计算机也可以包含数字电路以及模拟电路中的至少一方作为处理器。这里，所谓的数字电路，例如是ASIC(Application SpecificIntegrated Circuit：专用集成电路)、FPGA(Field Programmable Gate Array：现场可编程逻辑门阵列)、SOC(System on a Chip：片上系统)、PGA(Programmable Gate Array：可编程门阵列)以及CPLD(Complex Programmable Logic Device：复杂可编程逻辑器件)等中的至少一种。另外，这样的数字电路也可以具有存储有程序的存储器。
变形例的跟踪装置、跟踪方法以及跟踪程序也可以应用于车辆以外。在该情况下，特别是应用于车辆以外的跟踪装置也可以搭载或者设置于与外界传感器系统2相同的应用对象，也可以搭载或者设置于与外界传感器系统2不同的应用对象。

Claims (8)

1.一种跟踪装置，具有处理器(12)，基于外界传感器系统(2)的观测值通过按时间序列估计移动体(3)的状态值来跟踪上述移动体，
上述处理器构成为执行如下步骤：
获取在观测时刻观测到的上述移动体的上述观测值；
通过预测在上述观测时刻的上述移动体的上述状态值，来获取预测状态值；以及
通过将上述预测状态值和在上述观测时刻的上述观测值作为变量的非线性滤波，来估计在上述观测时刻的上述状态值的真值，
估计上述真值包含如下步骤：
根据来自上述外界传感器系统的每个上述顶点的视觉辨认度来设定对上述移动体进行建模而成的矩形模型中的多个顶点中的每个顶点的权重系数；
基于上述预测状态值和在上述观测时刻的上述观测值来获取各上述顶点处的观测误差；以及
基于每个上述顶点的上述权重系数来获取上述观测误差的协方差。
2.根据权利要求1所述的跟踪装置，其中，
设定上述权重系数包含：
越是上述视觉辨认度高的上述顶点，将上述权重系数设定得越小。
3.根据权利要求1或2所述的跟踪装置，其中，
设定上述权重系数包含：
对与上述外界传感器系统之间存在遮蔽物标的上述顶点，将上述权重系数设定为最大值。
4.根据权利要求1～3中任一项所述的跟踪装置，其中，
设定上述权重系数包含：
对存在于上述外界传感器系统的感测区域外的上述顶点，将上述权重系数设定为最大值。
5.根据权利要求1～4中任一项所述的跟踪装置，其中，
获取上述预测状态值包含：
基于在比上述观测时刻靠前的过去时刻估计出的上述真值，来获取在上述观测时刻的上述预测状态值。
6.根据权利要求1～5中任一项所述的跟踪装置，其中，
估计上述真值包含：
通过使用扩展卡尔曼滤波器的上述非线性滤波，来获取已经更新了上述预测状态值的上述真值。
7.一种跟踪方法，是为了基于外界传感器系统(2)的观测值通过按时间序列估计移动体(3)的状态值来跟踪上述移动体，而由处理器(12)估计的跟踪方法，上述跟踪方法包含如下步骤：
获取在观测时刻观测到的上述移动体的上述观测值；
通过预测在上述观测时刻的上述移动体的上述状态值，来获取预测状态值；以及
通过将上述预测状态值和在上述观测时刻的上述观测值作为变量的非线性滤波，来估计在上述观测时刻的上述状态值的真值，
估计上述真值包含如下步骤：
根据来自上述外界传感器系统的每个上述顶点的视觉辨认度来设定对上述移动体进行建模而成的矩形模型中的多个顶点中的每个顶点的权重系数；
基于上述预测状态值和在上述观测时刻的上述观测值来获取各上述顶点处的观测误差；以及
基于每个上述顶点的上述权重系数来获取上述观测误差的协方差。
8.一种跟踪程序，存储于存储介质(10)，上述跟踪程序包含为了基于外界传感器系统(2)的观测值通过按时间序列估计移动体(3)的状态值来跟踪上述移动体而使处理器(12)执行的指令，
上述指令包含：
获取在观测时刻观测到的上述移动体的上述观测值；
通过预测在上述观测时刻的上述移动体的上述状态值，来获取预测状态值；以及
通过将上述预测状态值和在上述观测时刻的上述观测值作为变量的非线性滤波，来估计在上述观测时刻的上述状态值的真值，
估计上述真值包含如下步骤：
根据来自上述外界传感器系统的每个上述顶点的视觉辨认度来设定对上述移动体进行建模而成的矩形模型中的多个顶点中的每个顶点的权重系数；
基于上述预测状态值和在上述观测时刻的上述观测值来获取各上述顶点处的观测误差；以及
基于每个上述顶点的上述权重系数来获取上述观测误差的协方差。
CN202280034122.5A 2021-05-14 2022-04-26 跟踪装置、跟踪方法、跟踪程序 Pending CN117412895A (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2021-082666 2021-05-14
JP2021082666A JP2022175896A (ja) 2021-05-14 2021-05-14 追跡装置、追跡方法、追跡プログラム
PCT/JP2022/018842 WO2022239637A1 (ja) 2021-05-14 2022-04-26 追跡装置、追跡方法、追跡プログラム

Publications (1)

Publication Number Publication Date
CN117412895A true CN117412895A (zh) 2024-01-16

Family

ID=84029583

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202280034122.5A Pending CN117412895A (zh) 2021-05-14 2022-04-26 跟踪装置、跟踪方法、跟踪程序

Country Status (4)

Country Link
US (1) US20240078358A1 (zh)
JP (1) JP2022175896A (zh)
CN (1) CN117412895A (zh)
WO (1) WO2022239637A1 (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0996521A (ja) * 1995-09-29 1997-04-08 Kansei Corp 車間距離測定装置
JP7197456B2 (ja) * 2019-10-15 2022-12-27 株式会社Soken 物体追跡装置
JP7351706B2 (ja) * 2019-10-15 2023-09-27 株式会社Soken 物体追跡装置
CN110861651B (zh) * 2019-12-02 2021-07-23 吉林大学 一种前车纵侧向运动状态估计方法
CN111667512B (zh) * 2020-05-28 2024-04-09 浙江树人学院（浙江树人大学） 基于改进卡尔曼滤波的多目标车辆轨迹预测方法
CN112099378B (zh) * 2020-09-24 2021-11-19 吉林大学 考虑随机测量时滞的前车侧向运动状态实时估计方法

Also Published As

Publication number Publication date
WO2022239637A1 (ja) 2022-11-17
JP2022175896A (ja) 2022-11-25
US20240078358A1 (en) 2024-03-07

Similar Documents

Publication Publication Date Title
US11073601B2 (en) Vehicle positioning system using LiDAR
EP3875985B1 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
US10757395B2 (en) Camera parameter set calculation method, recording medium, and camera parameter set calculation apparatus
Indelman et al. Information fusion in navigation systems via factor graph based incremental smoothing
CN108291813B (zh) 借助姿态图的位置数据融合
JP7297172B2 (ja) 物体の拡大状態を追跡するためのシステムおよび方法
JP6736412B2 (ja) 目標物監視システムおよび目標物監視方法
JP2010061655A (ja) 線形特徴を用いた対象追跡
JP7301245B2 (ja) モデル幾何学学習を用いて移動物体の拡大状態を追跡するためのシステムおよび方法
US11866056B2 (en) Ballistic estimation of vehicle data
CN114264301A (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN112487861A (zh) 车道线识别方法、装置、计算设备及计算机存储介质
CN117222915A (zh) 用于使用复合测量模型跟踪移动对象的扩张状态的系统和方法
US11935304B2 (en) Calibration system, calibration method, and calibration program
US8548194B1 (en) System and method for determining altitude
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
JP6916091B2 (ja) 位置姿勢推定システム及び位置姿勢推定装置
CN117412895A (zh) 跟踪装置、跟踪方法、跟踪程序
JP2024012160A (ja) 目標状態推定方法、装置、電子機器及び媒体
JP2024012162A (ja) 目標状態推定方法、装置、電子機器及び媒体
CN114076946A (zh) 一种运动估计方法及装置
CN112477868A (zh) 碰撞时间计算方法、装置、可读存储介质和计算机设备
CN111712855A (zh) 地面信息处理方法、装置和无人驾驶车辆
CN117475399B (zh) 车道线拟合方法、电子设备及可读介质
Winkens et al. Optical truck tracking for autonomous platooning

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