WO2022239637A1 - 追跡装置、追跡方法、追跡プログラム - Google Patents

追跡装置、追跡方法、追跡プログラム Download PDF

Info

Publication number
WO2022239637A1
WO2022239637A1 PCT/JP2022/018842 JP2022018842W WO2022239637A1 WO 2022239637 A1 WO2022239637 A1 WO 2022239637A1 JP 2022018842 W JP2022018842 W JP 2022018842W WO 2022239637 A1 WO2022239637 A1 WO 2022239637A1
Authority
WO
WIPO (PCT)
Prior art keywords
value
observation
state value
observation time
observed
Prior art date
Application number
PCT/JP2022/018842
Other languages
English (en)
French (fr)
Inventor
健太郎 新井
Original Assignee
株式会社デンソー
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 株式会社デンソー filed Critical 株式会社デンソー
Priority to CN202280034122.5A priority Critical patent/CN117412895A/zh
Publication of WO2022239637A1 publication Critical patent/WO2022239637A1/ja
Priority to US18/506,646 priority patent/US20240078358A1/en

Links

Images

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

Definitions

  • the present disclosure relates to tracking technology for tracking mobile objects.
  • Non-Patent Document 1 proposes a method of repeating state value estimation of a moving object in time series by filtering using a Kalman filter.
  • Non-Patent Document 1 assumes that the entire moving object is sufficiently observed from the external sensor system. Therefore, when it is difficult to observe a part of the moving object from the external sensor system, the estimated state value deviates from the true value, and there is a risk of deterioration in tracking accuracy.
  • An object of the present disclosure is to provide a tracking device that improves tracking accuracy for moving objects. Another object of the present disclosure is to provide a tracking method that increases tracking accuracy for a moving object. Still another object of the present disclosure is to provide a tracking program that improves tracking accuracy for a moving object.
  • a first aspect of the present disclosure is A tracking device that has a processor and tracks a moving object by estimating state values of the moving object in time series based on values observed by an external sensor system,
  • the processor Acquiring an observed value of a moving object observed at an observation time; obtaining a predicted state value by predicting the state value of the moving object at the observation time; estimating the true value of the state value at the observation time by nonlinear filtering using the observed value and the predicted state value at the observation time as variables, Estimating the true value is setting a weighting factor for each of a plurality of vertices in a rectangular model modeling a moving object according to the visibility of each vertex from an external sensor system; Obtaining an observation error at each vertex based on the observed value and the predicted state value at the observation time; obtaining a covariance of the observation error based on the weighting factor for each vertex.
  • a second aspect of the present disclosure is A tracking method estimated by a processor for tracking a moving object by estimating state values of the moving object in time series based on values observed by an external sensor system, Acquiring an observed value of a moving object observed at an observation time; obtaining a predicted state value by predicting the state value of the moving object at the observation time; estimating the true value of the state value at the observation time by nonlinear filtering using the observed value and the predicted state value at the observation time as variables, Estimating the true value is setting a weighting factor for each of a plurality of vertices in a rectangular model modeling a moving object according to the visibility of each vertex from an external sensor system; Obtaining an observation error at each vertex based on the observed value and the predicted state value at the observation time; obtaining a covariance of the observation error based on the weighting factor for each vertex.
  • a third aspect of the present disclosure is A tracking program stored in a storage medium and containing instructions to be executed by a processor to track a moving object by estimating state values of the moving object in time series based on values observed by an external sensor system, the instruction is Acquiring an observed value of a moving object observed at an observation time; obtaining a predicted state value by predicting the state value of the moving object at the observation time; estimating the true value of the state value at the observation time by nonlinear filtering using the observed value and the predicted state value at the observation time as variables, Letting the true value be estimated is setting a weighting factor for each of a plurality of vertices in a rectangular model modeling a moving object according to the degree of visibility of each vertex from an external sensor system; Obtaining an observation error at each vertex based on the observed value and the predicted state value at the observation time; and obtaining a covariance of the observation error based on the weighting factor for each vertex.
  • the true value of the state value at the observation time is estimated by non-linear filtering using the observed value and the predicted state value at the observation time as variables.
  • the observation error at each vertex in the rectangular model modeling the moving body is obtained based on the observed value and the predicted state value at the observation time, and the covariance of the observation error based on the weighting factor for each vertex is obtained. Therefore, according to the first to third modes in which the weighting factor is set according to the visibility from the external sensor system for each vertex, the visibility can be reflected in the true value estimation of the state value. Therefore, it is possible to accurately estimate the true value of the state value and improve the accuracy of tracking the moving object.
  • FIG. 1 is a block diagram showing the overall configuration of a tracking device according to one embodiment
  • FIG. FIG. 4 is a schematic diagram for explaining observation values and a rectangle model according to one embodiment
  • It is a block diagram showing a functional configuration of a tracking device according to one embodiment.
  • 4 is a flowchart illustrating a tracking method according to one embodiment
  • FIG. 4 is a schematic diagram for explaining predicted state values and a rectangular model according to an embodiment
  • 4 is a flow chart illustrating an estimation process according to one embodiment
  • FIG. 4 is a schematic diagram showing an example of weight setting according to one embodiment
  • 4 is a flow chart illustrating a weight setting subroutine according to one embodiment.
  • the tracking device 1 tracks the moving body 3 by estimating the state values of the moving body 3 in time series based on the values observed by the external sensor system 2 .
  • the tracking device 1 is mounted on the vehicle 4 together with the external sensor system 2 .
  • the vehicle 4 is temporarily given the automatic driving mode by switching to the manual driving mode, or constantly without the switching being substantially executed.
  • Autonomous driving modes may be achieved by autonomous cruise control, such as conditional driving automation, advanced driving automation, or full driving automation, in which the system performs all driving tasks when activated.
  • Autonomous driving modes may be provided by advanced driving assistance controls, such as driving assistance or partial driving automation, in which the occupant performs some or all driving tasks.
  • the automatic driving mode may be realized by either one, combination, or switching of the autonomous driving control and advanced driving support control.
  • the external sensor system 2 observes the inside of the sensing area AS set in the external world of the vehicle 4, and outputs the observed value in the sensing area AS.
  • the external sensor system 2 is composed of, for example, LiDAR (Light Detection and Ranging/Laser Imaging Detection and Ranging), radar, camera, or fusion of at least two of these sensing devices.
  • the external sensor system 2 is controlled so as to repeat observations at predetermined tracking intervals.
  • the external sensor system 2 outputs the observed value zk at the observation time k for the moving object 3 at each tracking cycle.
  • the observed value zk is defined by Equation 1 using physical quantities schematically shown in FIG.
  • x and y are the horizontal center position and the vertical center position, respectively, of the moving body 3 in the orthogonal coordinate system defined in the observation space.
  • ⁇ is the azimuth angle of the moving body 3 with respect to the horizontal direction in the orthogonal coordinate system defined in the observation space.
  • l and w are the longitudinal length and lateral width, respectively, of the moving body 3 in the orthogonal coordinate system defined in the observation space.
  • the tracking device 1 shown in FIG. 1 is connected to the external sensor system 2 via at least one of, for example, a LAN (Local Area Network), wire harness, internal bus, wireless communication line, and the like.
  • the tracking device 1 comprises at least one dedicated computer.
  • the dedicated computer that configures the tracking device 1 may be a driving control ECU (Electronic Control Unit) that controls driving including the automatic driving mode of the vehicle 4 .
  • a dedicated computer that constitutes the tracking device 1 may be a locator ECU that estimates the self-state quantity of the vehicle 4 .
  • a dedicated computer that configures the tracking device 1 may be a navigation ECU that navigates the travel route of the vehicle 4 .
  • the dedicated computer that constitutes the tracking device 1 may be at least one external computer that constructs an external center or mobile terminal that can communicate with the vehicle 4, for example.
  • the dedicated computer that constitutes the tracking device 1 has at least one memory 10 and one processor 12 .
  • the memory 10 stores computer-readable programs and data non-temporarily, for example, at least one type of non-transitory physical storage medium (non-transitory storage medium) among semiconductor memory, magnetic medium, optical medium, etc. tangible storage medium).
  • the processor 12 includes at least one of a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), and a RISC (Reduced Instruction Set Computer)-CPU as a core.
  • a CPU Central Processing Unit
  • GPU Graphics Processing Unit
  • RISC Reduced Instruction Set Computer
  • Processor 12 executes a plurality of instructions contained in a trace program stored in memory 10 .
  • the tracking device 1 constructs a plurality of functional blocks for tracking the moving object 3 .
  • the tracking program stored in the memory 10 causes the processor 12 to execute multiple instructions to track the moving body 3, thereby constructing multiple functional blocks.
  • the functional blocks constructed by the tracking device 1 include a prediction block 100, an observation block 110 and an estimation block 120.
  • the prediction block 100 predicts the state value of the moving object 3 at the observation time k to obtain the predicted state value Z k
  • k ⁇ 1 is defined by Equation 2 using physical quantities schematically shown in FIG.
  • X and Y are the horizontal center position and the vertical center position, respectively, of the moving body 3 in the orthogonal coordinate system defined in the observation space.
  • ⁇ is the azimuth angle of the moving body 3 with respect to the horizontal direction in the orthogonal coordinate system defined in the observation space.
  • Equation 2 L and W are the longitudinal length and the lateral width of the moving body 3 in the orthogonal coordinate system defined in the observation space.
  • Vx and Vy (not shown in FIG. 5) are the lateral velocity and vertical central velocity of the moving body 3 in the orthogonal coordinate system defined in the observation space, respectively.
  • the prediction block 100 in S100 obtains an estimated state value Z k ⁇ 1
  • k-1 is predictively obtained by performing a time update operation after time conversion on the variance P k-1
  • k ⁇ 1 and the error covariance P k ⁇ 1 are the estimated state value Z k ⁇ 1
  • Q in Expression 4 is a covariance matrix of system noise (process noise).
  • the observation block 110 acquires from the external sensor system 2 the observed value zk shown in FIG.
  • Acquisition of the observed value zk in S110 may be executed as the observed value zk is output from the external sensor system 2, or the output observed value zk is temporarily buffered in the memory 10. may be executed from Also, the acquisition of the observed value z k in S110 may be executed in parallel (that is, simultaneously) with the acquisition of the predicted state value Z k
  • the estimation block 120 estimates the state value of the moving object 3 at the observation time k, and the estimated state value Zk
  • k are obtained by nonlinear filtering that performs an observation update operation using the observed value z k and the predicted state value Z k
  • the estimation block 120 in S120 executes the estimation process shown in FIG. In S200 of the estimation process , the estimation block 120 performs a rectangular model model of the moving object 3 as shown in FIGS. , set weighting factors s fl , s bl , s fr , s br respectively.
  • the estimation block 120 in S200 performs weight A weight setting subroutine shown in FIG. 8 is executed to set the coefficients s fl , s bl , s fr , s br .
  • This subroutine is repeatedly executed the number of times corresponding to the number of vertices m fl , m bl , m fr , m br . Therefore, among the vertices m fl , m bl , m fr , and m br , the vertex to be executed each time in this subroutine is called the target vertex m.
  • suffixes indicated by subscripts to various variables indicate left front, left rear, right front, and right rear of the moving body 3, respectively.
  • the notations fl, bl, fr, and br are omitted, for example, the target vertex m.
  • the estimation block 120 determines whether or not the shielding target ST shown in FIG. 7 exists between the observation origin O of the external sensor system 2 and the target vertex m. do.
  • the shielding target ST is located on a line segment connecting the observation origin O and the target vertex m (m br in FIG. 7). It may be an object that exists separately.
  • the shielding target ST is one side of the rectangular model M that exists on the line segment connecting the observation origin O and the target vertex m (m fr in the figure). It may be a constituent part of the moving body 3 corresponding to .
  • the observation origin O may be a sensing origin set in a single sensor device that constitutes the external sensor system 2, or may be assumed in the observation space by fusion of a plurality of sensor devices that constitute the external sensor system 2. It may be the spatial origin.
  • the weight setting subroutine proceeds to S310, whereby the estimation block 120 sets the visibility ⁇ to the lowest value.
  • the visibility levels ⁇ br and ⁇ fr of the right rear vertex m br and the right front vertex m fr hidden by the shielding target ST are set to “0” as the lowest value. .
  • the weight setting subroutine proceeds to S320 so that the target vertex m is outside the sensing area AS of the external sensor system 2.
  • Inference block 120 determines whether there is a .
  • the weight setting subroutine proceeds to S310, and the estimation block 120 sets the visibility ⁇ to the lowest value.
  • the sensing area AS may be a viewing angle set for a single sensor device that constitutes the external sensor system 2, or may be an overlapping area between the viewing angles of a plurality of sensor devices that constitute the external sensor system 2. There may be.
  • the weight setting subroutine proceeds to S330, and the estimation block 120 acquires the visual recognition determination angle ⁇ .
  • the visual recognition determination angle ⁇ is the smallest angle between two sides connected to the target vertex m and a line segment connecting the observation origin O and the target vertex m. Defined as an angle.
  • the minimum angle ⁇ bl in the illustration of FIG. 7 regarding the left rear vertex m bl and the minimum angle ⁇ fl in the illustration of FIG. 7 regarding the left front vertex m fl respectively correspond to the visual recognition determination angle ⁇ .
  • the estimation block 120 determines whether or not the visual recognition determination angle ⁇ exceeds 90 degrees. As a result, when the visibility determination angle ⁇ exceeds 90 degrees, the weight setting subroutine proceeds to S350, and the estimation block 120 sets the visibility ⁇ to the maximum value. On the other hand, if the visibility determination angle ⁇ is 90 degrees or less, the weight setting subroutine proceeds to S360, and the estimation block 120 sets the visibility ⁇ that is equal to or more than the minimum value and equal to or less than the maximum value by Equation 6. .
  • the visibility determination angle ⁇ is 90 degrees or less
  • the visibility determination angle ⁇ bl exceeding 90 degrees sets the visibility ⁇ bl to “1” as the maximum value, while the left front vertex m fl in FIG. 2, the visibility ⁇ fl is set to the calculated value (sin ⁇ fl ) in Expression 6 with the visibility determination angle ⁇ fl of 90 degrees or less.
  • the estimation block 120 determines whether or not the visibility ⁇ is "0" as the minimum value. As a result, when the visibility ⁇ is 0, the weight setting subroutine proceeds to S380, and the estimation block 120 sets the weighting factor s to the maximum value smax . On the other hand, if the visibility ⁇ is not 0, the weight setting subroutine proceeds to S390, and the estimation block 120 sets the smaller value of the reciprocal of the visibility ⁇ (1/ ⁇ ) and the maximum value smax . sets the weighting factor s. In particular, when the weighting factor s is set to the reciprocal of the visibility ⁇ , the weighting factor s is set smaller for the vertex m with a higher visibility ⁇ .
  • the weighting factor s is set to the maximum value smax .
  • the maximum value as the visibility ⁇ or the reciprocal of the calculated value in Equation 6 (1 or 1/sin ⁇ fl ) and the maximum value s max and is set to the smaller value.
  • the maximum value s_max of the weighting factor s is defined to be a guard value larger than "1" that can prevent the covariance S_new of the observation error e_new described later from becoming infinite due to nonlinear filtering.
  • the estimation process proceeds to S210 as shown in FIG.
  • the estimation block 120 obtains the observation error e new at each vertex m fl , m bl , m fr , m br based on the observed value z k and the predicted state value Z k
  • the estimation block 120 in S210 converts the physical quantities x, y, ⁇ , l , w of the observed value zk to each vertex m Transform to the expanded observation z new expanded for fl , m bl , m fr , and m br .
  • x fl , x bl , x fr , and x br in Equations 8 to 11 represent the lateral position x of observed value z k to each vertex m fl , m bl , m fr , and m br as shown in FIG. It becomes the position coordinate that constitutes the expanded observed value z new .
  • Equations 8 to 11 y fl , y bl , y fr , y br expand the vertical position y of the observed value z k to each vertex m fl , m bl , m fr , m br as shown in FIG. , are the position coordinates that constitute the expanded observed value z new .
  • the estimation block 120 in S210 converts the physical quantities X, Y, ⁇ , L, and W of the predicted state value Zk
  • X fl , X bl , X fr , and X br in Equations 13 to 16 represent the horizontal position X of the predicted state value Z k
  • Equations 13 to 16 correspond to the vertical position Y of the predicted state value Z k
  • the observed error e new is obtained as shown in FIG.
  • the estimation block 120 calculates the covariance S new of the observation error e new as weighting coefficients s fl , s bl , s fr , s fr , s bl for each vertex m fl , m bl , m fr , m br . s-- br .
  • the estimation block 120 then generates the observed error e new Obtain the 8 ⁇ 8 covariance matrix R new of .
  • R' in Equation 18 is a covariance matrix for horizontal position and vertical position, and is an adjustment parameter that can be adjusted by presetting.
  • the estimation block 120 acquires the partial differential matrix (Jacobian) H new for the nonlinear function of Equation 12 by Equation 19.
  • the estimation block 120 along with the covariance matrix R new and the partial differential matrix H new of the observation error e new , using Equation 20 using the error covariance P k
  • the covariance S_new of the observed error e_new is obtained.
  • the estimation block 120 performs non-linear filtering using an extended Kalman filter to update the estimated state value Z k
  • the estimation block 120 uses Equation 21 using the covariance S new and the partial differential matrix H new of the observed error e new and the error covariance P k
  • the estimation block 120 acquires the estimated state value Z k
  • I in Expression 23 is a unit matrix.
  • k at the latest observation time k acquired by the tracking method in the current tracking cycle is output to the operation control ECU, and the automatic driving mode of the vehicle 4 It is used for operation control including Also, the estimated state value Z k
  • the true value of the state value at observation time k is estimated by non-linear filtering using the observed value z k and predicted state value Z k
  • the observation error e new at each vertex m in the rectangular model M modeling the moving body 3 is obtained based on the observed value z k and the predicted state value Z k
  • the weighting factor s is set according to the visibility ⁇ from the external sensor system 2 for each vertex, the visibility ⁇ can be reflected in the true value estimation of the state value. Therefore, it is possible to accurately estimate the estimated state value Zk
  • the weighting factor s is set smaller for the vertex m with a higher visibility ⁇ . According to this, with regard to the vertex m with a high visibility ⁇ from the external sensor system 2, the matrix component of the covariance S new becomes small, so that the degree of contribution to the true value estimation of the state value increases. Therefore, by estimating the estimated state value Zk
  • the weighting factor s is set to the maximum value s max for the vertex m at which the shielding target ST exists between the external sensor system 2 and the vertex m.
  • the matrix component of the covariance S new increases, and the state value Contribution to true value estimation becomes low. Therefore, by estimating the estimated state value Zk
  • the weighting factor s is set to the maximum value s max for the vertex m existing outside the sensing area AS of the external sensor system 2 .
  • the matrix component of the covariance S new becomes large, so that the true value of the state value less contribution to value estimation. Therefore, by estimating the estimated state value Zk
  • the dedicated computer that constitutes the tracking device 1 may include at least one of a digital circuit and an analog circuit as a processor.
  • Digital circuits here include, for example, ASIC (Application Specific Integrated Circuit), FPGA (Field Programmable Gate Array), SOC (System on a Chip), PGA (Programmable Gate Array), and CPLD (Complex Programmable Logic Device). , at least one Such digital circuits may also have a memory that stores the program.
  • the modified tracking device, tracking method, and tracking program may be applied to other than vehicles.
  • the tracking device that is applied to something other than the vehicle may be mounted or installed on the same application target as the external sensor system 2, or may be mounted or installed on a different application target from the external sensor system 2. good.

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

追跡装置のプロセッサは、観測時刻に観測された移動体の観測値を取得することと、観測時刻における移動体の状態値を予測することにより、予測状態値を取得することと、観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定することとを、実行するように構成される。真値を推定することは、移動体をモデリングした矩形モデルにおける複数頂点（ｍ）毎の重み係数（ｓ）を、外界センサ系からの各頂点（ｍ）毎の視認度に応じて設定することと、各頂点（ｍ）での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得することと、観測誤差の共分散を、各頂点（ｍ）毎の重み係数（ｓ）に基づき取得することとを、含む。

Description

追跡装置、追跡方法、追跡プログラム 関連出願の相互参照
　この出願は、２０２１年５月１４日に日本に出願された特許出願第２０２１－８２６６６号を基礎としており、基礎の出願の内容を、全体的に、参照により援用している。
　本開示は、移動体を追跡する追跡技術に、関する。
　外界センサ系による観測値に基づき移動体の状態値を時系列に推定して移動体を追跡する追跡技術は、広く知られている。追跡技術の一種として非特許文献１には、カルマンフィルタを用いたフィルタリングにより、移動体の状態値推定を時系列に繰り返す手法が、提案されている。
3D Multi-Object Tracking: A Baseline and New Evaluation Metrics（ＵＲＬ：https://arxiv.org/pdf/1907.03961.pdf）
　しかし、非特許文献１の提案手法は、外界センサ系から移動体の全体が十分に観測されることを、前提としている。そのため、移動体のうち一部の観測が外界センサ系からは困難となる場合、推定される状態値が真値からずれてしまい、追跡精度の低下するおそれがあった。
　本開示の課題は、移動体に対する追跡精度を高める追跡装置を、提供することにある。本開示の別の課題は、移動体に対する追跡精度を高める追跡方法を、提供することにある。本開示のさらに別の課題は、移動体に対する追跡精度を高める追跡プログラムを、提供することにある。
　以下、課題を解決するための本開示の技術的手段について、説明する。
　本開示の第一態様は、
　プロセッサを有し、外界センサ系による観測値に基づき移動体の状態値を時系列に推定して移動体を追跡する追跡装置であって、
　プロセッサは、
　観測時刻に観測された移動体の観測値を取得することと、
　観測時刻における移動体の状態値を予測することにより、予測状態値を取得することと、
　観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定することとを、実行するように構成され、
　真値を推定することは、
　移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、外界センサ系からの各頂点毎の視認度に応じて設定することと、
　各頂点での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得することと、
　観測誤差の共分散を、各頂点毎の重み係数に基づき取得することとを、含む。
　本開示の第二態様は、
　外界センサ系による観測値に基づき移動体の状態値を時系列に推定して移動体を追跡するために、プロセッサにより推定される追跡方法であって、
　観測時刻に観測された移動体の観測値を取得することと、
　観測時刻における移動体の状態値を予測することにより、予測状態値を取得することと、
　観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定することとを、含み、
　真値を推定することは、
　移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、外界センサ系からの各頂点毎の視認度に応じて設定することと、
　各頂点での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得することと、
　観測誤差の共分散を、各頂点毎の重み係数に基づき取得することとを、含む。
　本開示の第三態様は、
　記憶媒体に記憶され、外界センサ系による観測値に基づき移動体の状態値を時系列に推定して移動体を追跡するために、プロセッサに実行させる命令を含む追跡プログラムであって、
　命令は、
　観測時刻に観測された移動体の観測値を取得させることと、
　観測時刻における移動体の状態値を予測させることにより、予測状態値を取得させることと、
　観測時刻における観測値及び予測状態値を変数とした非線形フィルタリングにより、観測時刻における状態値の真値を推定させることとを、含み、
　真値を推定させることは、
　移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、外界センサ系からの各頂点毎の視認度に応じて設定させることと、
　各頂点での観測誤差を、観測時刻における観測値及び予測状態値に基づき取得させることと、
　観測誤差の共分散を、各頂点毎の重み係数に基づき取得させることとを、含む。
　これら第一～第三態様によると、観測時刻における観測値及び予測状態値を変数とする非線形フィルタリングにより、観測時刻における状態値の真値が推定される。このとき、移動体をモデリングした矩形モデルにおける各頂点での観測誤差が、観測時刻における観測値及び予測状態値に基づき取得されると共に、それら各頂点毎の重み係数に基づき当該観測誤差の共分散が取得される。そこで、各頂点毎に外界センサ系からの視認度に応じて重み係数を設定する第一～第三態様によれば、状態値の真値推定に当該視認度が反映され得る。故に、状態値の真値を精確に推定して、移動体に対する追跡精度を高めることが可能となる。
一実施形態による追跡装置の全体構成を示すブロック図である。 一実施形態による観測値及び矩形モデルを説明するための模式図である。 一実施形態による追跡装置の機能構成を示すブロック図である。 一実施形態による追跡方法を示すフローチャートである。 一実施形態による予測状態値及び矩形モデルを説明するための模式図である。 一実施形態による推定処理を示すフローチャートである。 一実施形態による重み設定例を示す模式図である。 一実施形態による重み設定サブルーチンを示すフローチャートである。
　以下、一実施形態を図面に基づいて説明する。
　図１に示すように一実施形態による追跡装置１は、外界センサ系２による観測値に基づき移動体３の状態値を時系列に推定することで、移動体３を追跡する。そのために追跡装置１は、外界センサ系２と共に、車両４に搭載される。
　車両４には、手動運転モードとの間での切り替えにより一時的に、又は当該切り替えが実質実行されずに定常的に、自動運転モードが与えられる。自動運転モードは、条件付運転自動化、高度運転自動化、又は完全運転自動化といった、作動時のシステムが全ての運転タスクを実行する自律走行制御により、実現されてもよい。自動運転モードは、運転支援、又は部分運転自動化といった、乗員が一部若しくは全ての運転タスクを実行する高度運転支援制御により、実現されてもよい。自動運転モードは、それら自律走行制御と高度運転支援制御とのいずれか一方、組み合わせ、又は切り替えにより実現されてもよい。
　外界センサ系２は、車両４の外界に設定されるセンシングエリアＡＳ内を観測することで、当該センシングエリアＡＳでの観測値を出力する。外界センサ系２は、例えＬｉＤＡＲ（Light Detection and Ranging / Laser Imaging Detection and Ranging）、レーダ、カメラ、又はそれらセンシングデバイスのうち少なくとも二種類のフュージョンにより、構成される。
　外界センサ系２は、所定の追跡周期で観測を繰り返すように、制御される。センシングエリアＡＳ内に移動体３が存在する場合に外界センサ系２からは、当該移動体３に対する観測時刻ｋでの観測値ｚｋが、そうした追跡周期毎に出力される。ここで観測値ｚｋは、図２に模式的に示す物理量を用いた数１により、定義される。数１においてｘ，ｙは、観測空間に定義される直交座標系での移動体３の、それぞれ横方向中心位置及び縦方向中心位置である。数１においてθは、観測空間に定義される直交座標系での移動体３の、横方向に対する方位角である。数１においてｌ，ｗは、観測空間に定義される直交座標系での移動体３の、それぞれ前後方向長さ及び左右方向幅である。
Figure JPOXMLDOC01-appb-M000001
　図１に示す追跡装置１は、例えばＬＡＮ（Local Area Network）、ワイヤハーネス、内部バス、及び無線通信回線等のうち、少なくとも一種類を介して外界センサ系２と接続される。追跡装置１は、少なくとも一つの専用コンピュータを含んで構成される。追跡装置１を構成する専用コンピュータは、車両４の自動運転モードを含む運転制御を担う、運転制御ＥＣＵ（Electronic Control Unit）であってもよい。追跡装置１を構成する専用コンピュータは、車両４の自己状態量を推定する、ロケータＥＣＵであってもよい。追跡装置１を構成する専用コンピュータは、車両４の走行経路をナビゲートする、ナビゲーションＥＣＵであってもよい。追跡装置１を構成する専用コンピュータは、例えば車両４との間で通信可能な外部センタ又はモバイル端末等を構築する、少なくとも一つの外部コンピュータであってもよい。
　追跡装置１を構成する専用コンピュータは、メモリ１０及びプロセッサ１２を、少なくとも一つずつ有している。メモリ１０は、コンピュータにより読み取り可能なプログラム及びデータ等を非一時的に記憶する、例えば半導体メモリ、磁気媒体、及び光学媒体等のうち、少なくとも一種類の非遷移的実体的記憶媒体（non-transitory tangible storage medium）である。プロセッサ１２は、例えばＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphics Processing Unit）、及びＲＩＳＣ（Reduced Instruction Set Computer）－ＣＰＵ等のうち、少なくとも一種類をコアとして含む。
　プロセッサ１２は、メモリ１０に記憶された追跡プログラムに含まれる複数の命令を、実行する。これにより追跡装置１は、移動体３を追跡するための機能ブロックを、複数構築する。このように追跡装置１では、移動体３を追跡するためにメモリ１０に記憶された追跡プログラムが複数命令をプロセッサ１２に実行させることで、複数機能ブロックが構築される。図３に示すように、追跡装置１により構築される複数の機能ブロックには、予測ブロック１００、観測ブロック１１０、及び推定ブロック１２０が含まれる。
　これら予測ブロック１００、観測ブロック１１０、及び推定ブロック１２０の共同により、追跡装置１が移動体３を追跡する追跡方法のフローを、図４に従って以下に説明する。本フローは、追跡周期毎に実行される。尚、本フローにおいて「Ｓ」とは、追跡プログラムに含まれた複数命令により実行される、複数ステップをそれぞれ意味する。
　追跡方法のＳ１００において予測ブロック１００は、観測時刻ｋにおける移動体３の状態値を予測することで、図３に示す予測状態値Ｚｋ｜ｋ－１及びその誤差共分散Ｐｋ｜ｋ－１を取得する。ここで予測状態値Ｚｋ｜ｋ－１は、図５に模式的に示す物理量を用いた数２により、定義される。数２においてＸ，Ｙは、観測空間に定義される直交座標系での移動体３の、それぞれ横方向中心位置及び縦方向中心位置である。数２においてΘは、観測空間に定義される直交座標系での移動体３の、横方向に対する方位角である。数２においてＬ，Ｗは、観測空間に定義される直交座標系での移動体３の、それぞれ前後方向長さ及び左右方向幅である。数２においてＶｘ，Ｖｙ（図５での表示は省略）は、観測空間に定義される直交座標系での移動体３の、それぞれ横方向速度及び縦方向中心速度である。
Figure JPOXMLDOC01-appb-M000002
　Ｓ１００における予測ブロック１００は、観測時刻ｋよりも前の過去時刻ｋ－１において推定ブロック１２０により真値として推定された状態値となる推定状態値Ｚｋ－１｜ｋ－１、及びその誤差共分散Ｐｋ－１｜ｋ－１に対して、時刻変換後の時間更新演算を実行することで、予測状態値Ｚｋ｜ｋ－１を予測的に取得する。このとき予測状態値Ｚｋ｜ｋ－１及び誤差共分散Ｐｋ｜ｋ－１は、過去時刻ｋ－１での推定状態値Ｚｋ－１｜ｋ－１及び誤差共分散Ｐｋ－１｜ｋ－１をそれぞれ用いて、数３～５により取得される。ここで数４におけるＱは、システムノイズ（プロセスノイズ）の共分散行列である。
Figure JPOXMLDOC01-appb-M000003
Figure JPOXMLDOC01-appb-M000004
Figure JPOXMLDOC01-appb-M000005
　図４に示す追跡方法のＳ１１０において観測ブロック１１０は、観測時刻ｋにおける移動体３の図３に示す観測値ｚｋを、外界センサ系２から取得する。Ｓ１１０による観測値ｚｋの取得は、外界センサ系２から観測値ｚｋが出力されるのに伴って実行されてもよいし、当該出力の観測値ｚｋがメモリ１０に一旦バッファリングされてから実行されてもよい。また、こうしたＳ１１０による観測値ｚｋの取得は、Ｓ１００による予測状態値Ｚｋ｜ｋ－１の取得に対して、並列的（即ち、同時的）に実行されてもよいし、時間を前後して実行されてもよい。
　図４に示す追跡方法のＳ１２０において推定ブロック１２０は、観測時刻ｋにおける移動体３の状態値を推定することで、図３に示す当該状態値の真値としての推定状態値Ｚｋ｜ｋ及びその誤差共分散Ｐｋ｜ｋを取得する。このとき推定状態値Ｚｋ｜ｋ及び誤差共分散Ｐｋ｜ｋは、観測時刻ｋでの観測値ｚｋ及び予測状態値Ｚｋ｜ｋ－１を変数として観測更新演算をする非線形フィルタリングによって、取得される。
　具体的にＳ１２０における推定ブロック１２０は、図６に示す推定処理を、実行する。推定処理のＳ２００において推定ブロック１２０は、図２，５，７に示すように移動体３を矩形に模式化してモデリングした矩形モデルＭにおける複数頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒ毎に、それぞれ重み係数ｓｆｌ，ｓｂｌ，ｓｆｒ，ｓｂｒを設定する。
　このときＳ２００における推定ブロック１２０は、外界センサ系２からの各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒ毎の視認度ωｆｌ，ωｂｌ，ωｆｒ，ωｂｒに応じて、それぞれの重み係数ｓｆｌ，ｓｂｌ，ｓｆｒ，ｓｂｒを設定するように、図８に示す重み設定サブルーチンを実行する。本サブルーチンは、頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒの数に対応した回数分、繰り返し実行される。そこで、頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒのうち、本サブルーチンにおいて各回の実行対象となる頂点を、対象頂点ｍという。尚、本サブルーチンの説明では、図７に基づく一部の例示説明を除き、各種変数に下付き文字で示されるサフィックスとして、移動体３の左前方、左後方、右前方、右後方をそれぞれ示すｆｌ，ｂｌ，ｆｒ，ｂｒの表記が、例えば対象頂点ｍの如く省略されている。
　図８に示す重み設定サブルーチンのＳ３００において推定ブロック１２０は、外界センサ系２の観測原点Ｏと対象頂点ｍとの間に、図７に示すような遮蔽物標ＳＴが存在するか否かを判定する。ここで遮蔽物標ＳＴは、右後方頂点ｍｂｒに関する図７の例示の如く、観測原点Ｏと対象頂点ｍ（同図ではｍｂｒ）との間を結ぶ線分上に、移動体３とは別個に存在する物体であってもよい。遮蔽物標ＳＴは、右前方頂点ｍｆｒに関する図７の例示の如く、観測原点Ｏと対象頂点ｍ（同図ではｍｆｒ）との間を結ぶ線分上に存在する、矩形モデルＭの一辺に対応した移動体３の構成部分であってもよい。尚、観測原点Ｏは、外界センサ系２を構成する単一センサデバイスに設定されるセンシング原点であってもよいし、外界センサ系２を構成する複数センサデバイスのフュージョンによって観測空間に想定される空間原点であってもよい。
　図８に示すように、Ｓ３００において遮蔽物標ＳＴが存在するとの判定を下した場合には、重み設定サブルーチンがＳ３１０へ移行することで、推定ブロック１２０が視認度ωを最低値に設定する。ここで図７の例示の場合には、遮蔽物標ＳＴに隠れた右後方頂点ｍｂｒ及び右前方頂点ｍｆｒの視認度ωｂｒ，ωｆｒが、最低値としての「０」に設定される。
　図８に示すように、Ｓ３００において遮蔽物標ＳＴが存在しないとの判定を下した場合には、重み設定サブルーチンがＳ３２０へ移行することで、対象頂点ｍが外界センサ系２のセンシングエリアＡＳ外に存在するか否かを、推定ブロック１２０が判定する。その結果、対象頂点ｍはセンシングエリアＡＳ外に存在するとの判定が下された場合には、重み設定サブルーチンがＳ３１０へ移行することで、推定ブロック１２０が視認度ωを最低値に設定する。尚、センシングエリアＡＳは、外界センサ系２を構成する単一センサデバイスに設定される視野角であってもよいし、外界センサ系２を構成する複数センサデバイスの視野角同士での重複エリアであってもよい。
　Ｓ３２０において対象頂点ｍはセンシングエリアＡＳ外に存在しないとの判定を下した場合には、重み設定サブルーチンがＳ３３０へ移行することで、推定ブロック１２０が視認判定角φを取得する。ここで図７に示すように視認判定角φは、対象頂点ｍにおいて対象頂点ｍに接続されている二辺と、観測原点Ｏ及び対象頂点ｍ間を結ぶ線分との、なす角のうち最小角度に定義される。ここで、左後方頂点ｍｂｌに関する図７の例示では最小角度φｂｌ、また左前方頂点ｍｆｌに関する図７の例示では最小角度φｆｌが、それぞれ視認判定角φに対応する。
　図８に示すように、Ｓ３３０に続く重み設定サブルーチンのＳ３４０において推定ブロック１２０は、視認判定角φが９０度超過であるか否かを、判定する。その結果、視認判定角φが９０度超過である場合には、重み設定サブルーチンがＳ３５０へ移行することで、推定ブロック１２０が視認度ωを最高値に設定する。一方、視認判定角φが９０度以下である場合には、重み設定サブルーチンがＳ３６０へ移行することで、最低値以上且つ最高値以下となる視認度ωを、推定ブロック１２０が数６により設定する。ここで、左後方頂点ｍｂｌに関する図７の例示では９０度超過の視認判定角φｂｌにより視認度ωｂｌが最高値としての「１」に設定される一方、左前方頂点ｍｆｌに関する図７の例示では９０度以下の視認判定角φｆｌにより視認度ωｆｌが数６での演算値（ｓｉｎφｆｌ）に設定される。
Figure JPOXMLDOC01-appb-M000006
　図８に示すように、Ｓ３１０，Ｓ３５０，Ｓ３６０に続く重み設定サブルーチンのＳ３７０において推定ブロック１２０は、視認度ωが最小値としての「０」であるか否かを、判定する。その結果、視認度ωが０である場合には、重み設定サブルーチンがＳ３８０へ移行することで、推定ブロック１２０が重み係数ｓを最大値ｓｍａｘに設定する。一方、視認度ωが０でない場合には、重み設定サブルーチンがＳ３９０へ移行することで、視認度ωの逆数（１／ω）と、最大値ｓｍａｘとうち小さい側の値に、推定ブロック１２０が重み係数ｓを設定する。特に、重み係数ｓが視認度ωの逆数に設定される場合、視認度ωが高い頂点ｍほど、重み係数ｓが小さく設定されることになる。
　ここで、遮蔽物標ＳＴに隠れた右後方頂点ｍｂｒ及び右前方頂点ｍｆｒに関する図７の例示では、重み係数ｓが最大値ｓｍａｘに設定される。一方、左後方頂点ｍｂｌ及び左前方頂点ｍｆｌに関する図７の例示では、視認度ωとしての最高値又は数６での演算値に対する逆数（１又は１／ｓｉｎφｆｌ）と、最大値ｓｍａｘとのうち小側の値に、設定されている。尚、重み係数ｓの最大値ｓｍａｘは、後述する観測誤差ｅｎｅｗの共分散Ｓｎｅｗが非線形フィルタリングによって無限大となることを回避可能なガード値に、「１」よりは大きく定義される。
　このようなＳ２００による重み設定サブルーチンが全頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒに対して完了することで、図６に示ように推定処理がＳ２１０へ移行する。Ｓ２１０において推定ブロック１２０は、各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒでの観測誤差ｅｎｅｗを、観測時刻ｋにおける観測値ｚｋ及び予測状態値Ｚｋ｜ｋ－１に基づき取得する。
　このときＳ２１０における推定ブロック１２０は、数７の非線形関数ｈｎｅｗ及び数８～１１の行列変換関数により観測値ｚｋの物理量x，ｙ，θ，ｌ，ｗを、矩形モデルＭの各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒに対して展開した、展開観測値ｚｎｅｗへと変換する。ここで数８～１１においてｘｆｌ，ｘｂｌ，ｘｆｒ，ｘｂｒは、図２に示すように観測値ｚｋの横方向位置ｘを各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒへ展開した、展開観測値ｚｎｅｗを構成する位置座標となる。数８～１１においてｙｆｌ，ｙｂｌ，ｙｆｒ，ｙｂｒは、図２に示すように観測値ｚｋの縦方向位置ｙを各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒへ展開した、展開観測値ｚｎｅｗを構成する位置座標となる。
Figure JPOXMLDOC01-appb-M000007
Figure JPOXMLDOC01-appb-M000008
Figure JPOXMLDOC01-appb-M000009
Figure JPOXMLDOC01-appb-M000010
Figure JPOXMLDOC01-appb-M000011
　Ｓ２１０における推定ブロック１２０は、数１２の非線形関数ｈｎｅｗ及び数１３～１６の行列変換関数により予測状態値Ｚｋ｜ｋ－１の物理量X，Ｙ，Θ，Ｌ，Ｗを、矩形モデルＭの各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒに対して展開した、展開状態値Ｚｎｅｗへと変換する。ここで数１３～１６においてＸｆｌ，Ｘｂｌ，Ｘｆｒ，Ｘｂｒは、図５に示すように予測状態値Ｚｋ｜ｋ－１の横方向位置Ｘを各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒへ展開した、展開状態値Ｚｎｅｗを構成する位置座標となる。数１３～１６においてＹｆｌ，Ｙｂｌ，Yｆｒ，Ｙｂｒは、図５に示すように予測状態値Ｚｋ｜ｋ－１の縦方向位置Ｙを各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒへ展開した、展開状態値Ｚｎｅｗを構成する位置座標となる。
Figure JPOXMLDOC01-appb-M000012
Figure JPOXMLDOC01-appb-M000013
Figure JPOXMLDOC01-appb-M000014
Figure JPOXMLDOC01-appb-M000015
Figure JPOXMLDOC01-appb-M000016
　Ｓ２１０における推定ブロック１２０では、このようにして変換された展開観測値ｚｎｅｗ及び展開状態値Ｚｎｅｗを用いる数１７により、図６の如く観測誤差ｅｎｅｗが取得されることとなる。
Figure JPOXMLDOC01-appb-M000017
　Ｓ２１０に続く推定処理のＳ２２０において推定ブロック１２０は、観測誤差ｅｎｅｗの共分散Ｓｎｅｗを、各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒ毎の重み係数ｓｆｌ，ｓｂｌ，ｓｆｒ，ｓｂｒに基づき取得する。このとき推定ブロック１２０は、重み係数ｓｆｌ，ｓｂｌ，ｓｆｒ，ｓｂｒを用いた数１８により、各頂点ｍｆｌ，ｍｂｌ，ｍｆｒ，ｍｂｒ毎に重み付けされた、観測誤差ｅｎｅｗの８×８共分散行列Ｒｎｅｗを取得する。ここで数１８のＲ’は、横方向位置及び縦方向位置に対する共分散行列であって、予設定により調整可能な調整パラメータである。さらに推定ブロック１２０は、数１２の非線形関数に対する偏微分行列（ヤコビアン）Ｈｎｅｗを、数１９により取得する。こうして推定ブロック１２０では、観測誤差ｅｎｅｗの共分散行列Ｒｎｅｗ及び偏微分行列Ｈｎｅｗと共に、予測状態値Ｚｋ｜ｋ－１の誤差共分散Ｐｋ｜ｋ－１を用いた数２０により、観測誤差ｅｎｅｗの共分散Ｓｎｅｗが取得される。
Figure JPOXMLDOC01-appb-M000018
Figure JPOXMLDOC01-appb-M000019
Figure JPOXMLDOC01-appb-M000020
　Ｓ２２０に続く推定処理のＳ２３０において推定ブロック１２０は、拡張カルマンフィルタを用いた非線形フィルタリングにより、予測状態値Ｚｋ｜ｋ－１を更新した真値としての推定状態値Ｚｋ｜ｋを、その誤差共分散Ｐｋ｜ｋと共に取得する。このとき推定ブロック１２０は、観測誤差ｅｎｅｗの共分散Ｓｎｅｗ及び偏微分行列Ｈｎｅｗと共に、予測状態値Ｚｋ｜ｋ－１の誤差共分散Ｐｋ｜ｋ－１を用いた数２１により、拡張カルマンフィルタのカルマンゲインＫｎｅｗを取得する。これにより推定ブロック１２０では、カルマンゲインＫｎｅｗ及び観測誤差ｅｎｅｗと共に、予測状態値Ｚｋ｜ｋ－１を用いた数２２により、推定状態値Ｚｋ｜ｋが取得される。さらに推定ブロック１２０では、カルマンゲインＫｎｅｗ及び偏微分行列Ｈｎｅｗと共に、予測状態値Ｚｋ｜ｋ－１の誤差共分散Ｐｋ｜ｋ－１を用いた数２３により、推定状態値Ｚｋ｜ｋの誤差共分散Ｐｋ｜ｋが取得される。ここで数２３のＩは、単位行列である。
Figure JPOXMLDOC01-appb-M000021
Figure JPOXMLDOC01-appb-M000022
Figure JPOXMLDOC01-appb-M000023
　図４に示すように、今回の追跡周期での追跡方法により取得された最新の観測時刻ｋにおける推定状態値Ｚｋ｜ｋは、運転制御ＥＣＵへ出力されることで、車両４の自動運転モードを含む運転制御に活用される。また、今回の追跡周期での追跡方法により取得された観測時刻ｋにおける推定状態値Ｚｋ｜ｋは、次回の追跡周期での追跡方法では過去時刻ｋ－１における推定状態値Ｚｋ－１｜ｋ－１として、予測ブロック１００によるＳ１００での予測に使用される。
　（作用効果）
　以上説明した本実施形態の作用効果を、以下に説明する。尚、作用効果の説明では、各種変数に下付き文字で示されるサフィックスとして、移動体３の左前方、左後方、右前方、右後方をそれぞれ示すｆｌ，ｂｌ，ｆｒ，ｆｒの表記が、省略されている。
　本実施形態によると、観測時刻ｋにおける観測値ｚｋ及び予測状態値Ｚｋ｜ｋ－１を変数とする非線形フィルタリングにより、観測時刻ｋにおける状態値の真値が推定される。このとき、移動体３をモデリングした矩形モデルＭにおける各頂点ｍでの観測誤差ｅｎｅｗが、観測時刻ｋにおける観測値ｚｋ及び予測状態値Ｚｋ｜ｋ－１に基づき取得されると共に、それら各頂点ｍ毎の重み係数ｓに基づき当該観測誤差ｅｎｅｗの共分散Ｓｎｅｗが取得される。そこで、各頂点毎に外界センサ系２からの視認度ωに応じて重み係数ｓを設定する本実施形態によれば、状態値の真値推定に当該視認度ωが反映され得る。故に、状態値の真値である推定状態値Ｚｋ｜ｋを精確に推定して、移動体３に対する追跡精度を高めることが可能となる。
　本実施形態によると、視認度ωが高い頂点ｍほど、重み係数ｓが小さく設定される。これによれば、外界センサ系２から高い視認度ωの頂点ｍに関しては、共分散Ｓｎｅｗの行列成分が小さくなることで、状態値の真値推定に対する寄与度が高くなる。故に、外界センサ系２からの視認度ωを反映させた精確な真値としての推定状態値Ｚｋ｜ｋを推定して、移動体３に対する追跡精度を高めることが可能となる。
　本実施形態によると、外界センサ系２との間に遮蔽物標ＳＴが存在する頂点ｍに対して、重み係数ｓが最大値ｓｍａｘに設定される。これによれば、遮蔽物標ＳＴに隠れるために外界センサ系２からの視認度ωが実質零と想定される頂点ｍに関しては、共分散Ｓｎｅｗの行列成分が大きくなることで、状態値の真値推定に対する寄与度が低くなる。故に、外界センサ系２からの視認度ωを反映させた精確な真値としての推定状態値Ｚｋ｜ｋを推定して、移動体３に対する追跡精度を高めることが可能となる。
　本実施形態によると、外界センサ系２のセンシングエリアＡＳ外に存在する頂点ｍに対して、重み係数ｓが最大値ｓｍａｘに設定される。これによれば、センシングエリアＡＳ外のために外界センサ系２からの視認度ωが実質零と想定される頂点ｍに関しては、共分散Ｓｎｅｗの行列成分が大きくなることで、状態値の真値推定に対する寄与度が低くなる。故に、外界センサ系２からの視認度ωを反映させた精確な真値としての推定状態値Ｚｋ｜ｋを推定して、移動体３に対する追跡精度を高めることが可能となる。
　（他の実施形態）
　以上、一実施形態について説明したが、本開示は、当該実施形態に限定して解釈されるものではなく、本開示の要旨を逸脱しない範囲内において種々の実施形態に適用することができる。
　変形例において追跡装置１を構成する専用コンピュータは、デジタル回路、及びアナログ回路のうち、少なくとも一方をプロセッサとして含んでいてもよい。ここでデジタル回路とは、例えばＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）、ＳＯＣ（System on a Chip）、ＰＧＡ（Programmable Gate Array）、及びＣＰＬＤ（Complex Programmable Logic Device）等のうち、少なくとも一種類である。またこうしたデジタル回路は、プログラムを記憶したメモリを、有していてもよい。
　変形例による追跡装置、追跡方法、及び追跡プログラムは、車両以外へ適用されてもよい。この場合、特に車両以外へ適用される追跡装置は、外界センサ系２と同一の適用対象に搭載又は設置されていてもよいし、外界センサ系２と異なる適用対象に搭載又は設置されていてもよい。 

Claims (8)

  1. 　プロセッサ（１２）を有し、外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して前記移動体を追跡する追跡装置であって、
    　前記プロセッサは、
    　観測時刻に観測された前記移動体の前記観測値を取得することと、
    　前記観測時刻における前記移動体の前記状態値を予測することにより、予測状態値を取得することと、
    　前記観測時刻における前記観測値及び前記予測状態値を変数とした非線形フィルタリングにより、前記観測時刻における前記状態値の真値を推定することとを、実行するように構成され、
    　前記真値を推定することは、
    　前記移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、前記外界センサ系からの各前記頂点毎の視認度に応じて設定することと、
    　各前記頂点での観測誤差を、前記観測時刻における前記観測値及び前記予測状態値に基づき取得することと、
    　前記観測誤差の共分散を、各前記頂点毎の前記重み係数に基づき取得することとを、含む追跡装置。
  2. 　前記重み係数を設定することは、
    　前記視認度が高い前記頂点ほど、前記重み係数を小さく設定することを、含む請求項１に記載の追跡装置。
  3. 　前記重み係数を設定することは、
    　前記外界センサ系との間に遮蔽物標が存在する前記頂点に対して、前記重み係数を最大値に設定することを、含む請求項１又は２に記載の追跡装置。
  4. 　前記重み係数を設定することは、
    　前記外界センサ系のセンシングエリア外に存在する前記頂点に対して、前記重み係数を最大値に設定することを、含む請求項１～３のいずれか一項に記載の追跡装置。
  5. 　前記予測状態値を取得することは、
    　前記観測時刻よりも前の過去時刻に推定された前記真値に基づき、前記観測時刻における前記予測状態値を取得することを、含む請求項１～４のいずれか一項に記載の追跡装置。
  6. 　前記真値を推定することは、
    　拡張カルマンフィルタを用いた前記非線形フィルタリングにより、前記予測状態値を更新した前記真値を取得することを、含む請求項１～５のいずれか一項に記載の追跡装置。
  7. 　外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して前記移動体を追跡するために、プロセッサ（１２）により推定される追跡方法であって、
    　観測時刻に観測された前記移動体の前記観測値を取得することと、
    　前記観測時刻における前記移動体の前記状態値を予測することにより、予測状態値を取得することと、
    　前記観測時刻における前記観測値及び前記予測状態値を変数とした非線形フィルタリングにより、前記観測時刻における前記状態値の真値を推定することとを、含み、
    　前記真値を推定することは、
    　前記移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、前記外界センサ系からの各前記頂点毎の視認度に応じて設定することと、
    　各前記頂点での観測誤差を、前記観測時刻における前記観測値及び前記予測状態値に基づき取得することと、
    　前記観測誤差の共分散を、各前記頂点毎の前記重み係数に基づき取得することとを、含む追跡方法。
  8. 　記憶媒体（１０）に記憶され、外界センサ系（２）による観測値に基づき移動体（３）の状態値を時系列に推定して前記移動体を追跡するために、プロセッサ（１２）に実行させる命令を含む追跡プログラムであって、
    　前記命令は、
    　観測時刻に観測された前記移動体の前記観測値を取得させることと、
    　前記観測時刻における前記移動体の前記状態値を予測させることにより、予測状態値を取得させることと、
    　前記観測時刻における前記観測値及び前記予測状態値を変数とした非線形フィルタリングにより、前記観測時刻における前記状態値の真値を推定させることとを、含み、
    　前記真値を推定させることは、
    　前記移動体をモデリングした矩形モデルにおける複数頂点毎の重み係数を、前記外界センサ系からの各前記頂点毎の視認度に応じて設定させることと、
    　各前記頂点での観測誤差を、前記観測時刻における前記観測値及び前記予測状態値に基づき取得させることと、
    　前記観測誤差の共分散を、各前記頂点毎の前記重み係数に基づき取得させることとを、含む追跡プログラム。 
PCT/JP2022/018842 2021-05-14 2022-04-26 追跡装置、追跡方法、追跡プログラム WO2022239637A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202280034122.5A CN117412895A (zh) 2021-05-14 2022-04-26 跟踪装置、跟踪方法、跟踪程序
US18/506,646 US20240078358A1 (en) 2021-05-14 2023-11-10 Tracking device, tracking method, and computer-readable non-transitory storage medium storing tracking program

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2021-082666 2021-05-14
JP2021082666A JP2022175896A (ja) 2021-05-14 2021-05-14 追跡装置、追跡方法、追跡プログラム

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/506,646 Continuation US20240078358A1 (en) 2021-05-14 2023-11-10 Tracking device, tracking method, and computer-readable non-transitory storage medium storing tracking program

Publications (1)

Publication Number Publication Date
WO2022239637A1 true WO2022239637A1 (ja) 2022-11-17

Family

ID=84029583

Family Applications (1)

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

Country Status (4)

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

Citations (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 車間距離測定装置
CN110861651A (zh) * 2019-12-02 2020-03-06 吉林大学 一种前车纵侧向运动状态估计方法
CN111667512A (zh) * 2020-05-28 2020-09-15 浙江树人学院（浙江树人大学） 基于改进卡尔曼滤波的多目标车辆轨迹预测方法
CN112099378A (zh) * 2020-09-24 2020-12-18 吉林大学 考虑随机测量时滞的前车侧向运动状态实时估计方法
WO2021075349A1 (ja) * 2019-10-15 2021-04-22 株式会社デンソー 物体追跡装置
WO2021075331A1 (ja) * 2019-10-15 2021-04-22 株式会社デンソー 物体追跡装置

Patent Citations (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 車間距離測定装置
WO2021075349A1 (ja) * 2019-10-15 2021-04-22 株式会社デンソー 物体追跡装置
WO2021075331A1 (ja) * 2019-10-15 2021-04-22 株式会社デンソー 物体追跡装置
CN110861651A (zh) * 2019-12-02 2020-03-06 吉林大学 一种前车纵侧向运动状态估计方法
CN111667512A (zh) * 2020-05-28 2020-09-15 浙江树人学院（浙江树人大学） 基于改进卡尔曼滤波的多目标车辆轨迹预测方法
CN112099378A (zh) * 2020-09-24 2020-12-18 吉林大学 考虑随机测量时滞的前车侧向运动状态实时估计方法

Also Published As

Publication number Publication date
CN117412895A (zh) 2024-01-16
JP2022175896A (ja) 2022-11-25
US20240078358A1 (en) 2024-03-07

Similar Documents

Publication Publication Date Title
US11073601B2 (en) Vehicle positioning system using LiDAR
CN111223135B (zh) 通过使用雷达和运动数据的单目相机来增强距离估计的系统和方法
CN111208492B (zh) 车载激光雷达外参标定方法及装置、计算机设备及存储介质
EP3384360B1 (en) Simultaneous mapping and planning by a robot
CN108291813B (zh) 借助姿态图的位置数据融合
KR102628778B1 (ko) 위치결정을 위한 방법, 컴퓨팅 기기, 컴퓨터 판독가능 저장 매체 및 컴퓨터 프로그램
JP7297172B2 (ja) 物体の拡大状態を追跡するためのシステムおよび方法
JP7301245B2 (ja) モデル幾何学学習を用いて移動物体の拡大状態を追跡するためのシステムおよび方法
KR20180038154A (ko) 라이다 센서를 이용한 차량의 자세 추정 방법
CN111060125A (zh) 碰撞检测方法、装置、计算机设备和存储介质
CN114943952A (zh) 多相机重叠视域下障碍物融合方法、系统、设备和介质
CN117222915A (zh) 用于使用复合测量模型跟踪移动对象的扩张状态的系统和方法
CN113139696A (zh) 一种轨迹预测模型构建方法及轨迹预测方法、装置
CN116449356A (zh) 基于聚集的lidar数据对准
WO2022239637A1 (ja) 追跡装置、追跡方法、追跡プログラム
JP2024012160A (ja) 目標状態推定方法、装置、電子機器及び媒体
US11047673B2 (en) Method, device, apparatus and storage medium for detecting a height of an obstacle
JP2024012162A (ja) 目標状態推定方法、装置、電子機器及び媒体
WO2022037370A1 (zh) 一种运动估计方法及装置
CN111833395B (zh) 一种基于神经网络模型的测向体制单目标定位方法和装置
CN110308724B (zh) 自动驾驶控制方法、装置、车辆、存储介质及电子设备
US11544899B2 (en) System and method for generating terrain maps
JP7409037B2 (ja) 推定装置、推定方法、推定プログラム
CN113470342B (zh) 一种自运动估计的方法及装置
CN118310501A (zh) 地图构建方法、装置、服务器及存储介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22807339

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 202280034122.5

Country of ref document: CN

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22807339

Country of ref document: EP

Kind code of ref document: A1