Tracking device, tracking method, storage medium

Abstract

The processor of the tracking device of the present application is configured to perform the steps of: acquiring an observation value of the moving body observed at an observation time; acquiring a predicted state value by predicting a state value of the moving body at the observation time; and estimating a true value of the state value at the observation time by a nonlinear filter using the predicted state value and the observation value at the observation time as variables. The estimating the true value includes the steps of: setting a weight coefficient (s) of each vertex of a plurality of vertices (m) in a rectangular model modeling the moving body according to a visual recognition degree of each vertex (m) from an external sensor system; acquiring an observation error at each vertex (m) based on the predicted state value and the observation value at the observation time; and acquiring a covariance of the observation error based on the weight coefficient (s) of each vertex (m).

Description

[0001] Cross-reference to related applications

[0002] This application is based on Japanese Patent Application No. 2021-82666, filed in Japan on May 14, 2021, and is incorporated herein by reference in its entirety. Technical Field

[0003] This disclosure relates to tracking technology for tracking moving objects. Background Technology

[0004] Tracking techniques that rely on observations from external sensor systems to estimate the state values ​​of a moving object over time are well-known. As one such tracking technique, Non-Patent Literature 1 proposes a method that uses a Kalman filter to repeatedly estimate the state values ​​of a moving object over time.

[0005] Non-patent literature 1: 3D Multi-Object Tracking: A Baseline and New Evaluation Metrics (URL: https: / / arxiv.org / pdf / 1907.03961.pdf)

[0006] However, the method proposed in Non-Patent Literature 1 is based on the premise of fully observing the entire moving body using an external sensor system. Therefore, when it is difficult to observe a part of the moving body using an external sensor system, there is a concern that the estimated state value may deviate from the true value, leading to a decrease in tracking accuracy. Summary of the Invention

[0007] The present disclosure addresses the problem of providing a tracking device for improving the tracking accuracy of a moving object. Another problem of the present disclosure is to provide a tracking method for improving the tracking accuracy of a moving object. Yet another problem of the present disclosure is to provide a tracking program for improving the tracking accuracy of a moving object.

[0008] The technical solution of this disclosure used to solve the problem is described below.

[0009] The first aspect of this disclosure is a tracking device having a processor that tracks a moving body by estimating its state values ​​over time based on observations from an external sensor system.

[0010] The processor is configured to perform the following steps:

[0011] Obtain the observation values ​​of the moving body observed at the observation time;

[0012] The predicted state value is obtained by predicting the state value of the moving body at the observation time; and

[0013] The true value of the state at the observation time is estimated by using nonlinear filtering with the predicted state value and the observed value at the observation time as variables.

[0014] Estimating the true value involves the following steps:

[0015] The weight coefficient of each vertex in the rectangular model used to model the moving body is set based on the visual recognition of each vertex from the external sensor system.

[0016] The observation error at each vertex is obtained based on the predicted state value and the observed value at the observation time; and

[0017] The covariance of the observation error is obtained based on the weight coefficient of each vertex.

[0018] The second aspect of this disclosure is a tracking method for tracking a moving body by estimating its state values ​​over time based on observations from an external sensor system. This tracking method, however, is based on the estimation by a processor.

[0019] The above tracking method includes the following steps:

[0020] Obtain the observation values ​​of the moving body observed at the observation time;

[0021] The predicted state value is obtained by predicting the state value of the moving body at the observation time, and

[0022] The true value of the state at the observation time is estimated by using nonlinear filtering with the predicted state value and the observed value at the observation time as variables.

[0023] Estimating the true value involves the following steps:

[0024] The weight coefficient of each vertex in the rectangular model used to model the moving body is set based on the visual recognition of each vertex from the external sensor system.

[0025] The observation error at each vertex is obtained based on the predicted state value and the observed value at the observation time; and

[0026] The covariance of the observation error is obtained based on the weight coefficient of each vertex.

[0027] The third aspect of this disclosure is a tracking program stored in a storage medium, which includes instructions executed by a processor to track a moving body by estimating its state values ​​over time based on observations from an external sensor system.

[0028] The instructions include:

[0029] Obtain the observation values ​​of the moving body observed at the observation time;

[0030] The predicted state value is obtained by predicting the state value of the moving body at the observation time; and

[0031] The true value of the state at the observation time is estimated by using nonlinear filtering with the predicted state value and the observed value at the observation time as variables.

[0032] Estimating the true value involves the following steps:

[0033] The weight coefficient of each vertex in the rectangular model used to model the moving body is set based on the visual recognition of each vertex from the external sensor system.

[0034] The observation error at each vertex is obtained based on the predicted state value and the observed value at the observation time; and

[0035] The covariance of the observation error is obtained based on the weight coefficient of each vertex.

[0036] According to these first to third methods, the true value of the state at the observation time is estimated by nonlinear filtering with the predicted state value and the observed value at the observation time as variables. At this time, the observation error at each vertex of the rectangular model that models the moving body is obtained based on the predicted state value and the observed value at the observation time, and the covariance of this observation error is obtained based on the weight coefficient of each vertex. Therefore, by setting the weight coefficient of each vertex according to the visual recognition from the external sensor system according to the first to third methods, this visual recognition can be reflected in the true value estimation of the state value. Therefore, the true value of the state value can be accurately estimated, improving the tracking accuracy for the moving body. Attached Figure Description

[0037] Figure 1 This is a block diagram showing the overall structure of a tracking device according to one embodiment.

[0038] Figure 2 This is a schematic diagram used to illustrate the observations and rectangular model of one implementation method.

[0039] Figure 3 This is a block diagram illustrating the functional structure of a tracking device in one embodiment.

[0040] Figure 4 This is a flowchart illustrating a tracking method in one implementation.

[0041] Figure 5 This is a schematic diagram used to illustrate the predicted state values ​​and rectangular model of one implementation method.

[0042] Figure 6This is a flowchart illustrating the estimation process of one implementation method.

[0043] Figure 7 This is a schematic diagram illustrating a weight setting example of one implementation method.

[0044] Figure 8 This is a flowchart representing a weight setting subroutine for one implementation method. Detailed Implementation

[0045] Hereinafter, one embodiment will be described with reference to the accompanying drawings.

[0046] like Figure 1 As shown, in one embodiment, the tracking device 1 tracks the moving body 3 by estimating the state value of the moving body 3 in a time series based on observations from the external sensor system 2. Therefore, the tracking device 1 is mounted on the vehicle 4 together with the external sensor system 2.

[0047] In vehicle 4, an autonomous driving mode is stably provided, either temporarily or without substantially performing the switch, by switching between manual driving and autonomous driving modes. The autonomous driving mode can also be achieved through autonomous driving control where the system performs all driving tasks during operation, such as conditional driving automation, high driving automation, or full driving automation. The autonomous driving mode can also be achieved through high driving assistance control, such as driver assistance or partial driving automation, where the occupant performs some or all of the driving tasks. The autonomous driving mode can also be achieved through any one of these autonomous driving controls and high driving assistance controls, or a combination thereof, or by switching between them.

[0048] The external sensor system 2 observes within a defined sensing area AS outside the vehicle 4 and outputs the observed values ​​in that sensing area AS. The external sensor system 2 consists of, for example, LiDAR (Light Detection and Ranging / Laser Imaging Detection and Ranging), radar, a camera, or a fusion of at least two of these sensing devices.

[0049] External sensor system 2 is controlled to repeatedly observe at a predetermined tracking period. When a moving object 3 is present within the sensing area AS, external sensor system 2 outputs the observed value z for the moving object 3 at observation time k according to each such tracking period. k Here, the observation z k By use Figure 2The physical quantities are defined using Equation 1, which is schematically shown in the diagram. In Equation 1, x and y are the lateral and longitudinal center positions of the moving body 3 in the orthogonal coordinate system defined in the observation space, respectively. In Equation 1, θ is the azimuth angle of the moving body 3 relative to the lateral direction in the orthogonal coordinate system defined in the observation space. In Equation 1, l and w are the longitudinal length and lateral width of the moving body 3 in the orthogonal coordinate system defined in the observation space, respectively.

[0050] [Formula 1]

[0051] z k = [x, y, θ, l, w] T

[0052] Figure 1 The tracking device 1 shown is connected to the external sensor system 2 via at least one of the following: LAN (Local Area Network), wiring harness, internal bus, and wireless communication line. The tracking device 1 is configured to include at least one dedicated computer. The dedicated computer constituting the tracking device 1 can also be a driving control ECU (Electronic Control Unit) that performs driving control including the autonomous driving mode of the vehicle 4. The dedicated computer constituting the tracking device 1 can also be a locator ECU that estimates the vehicle 4's own state variables. The dedicated computer constituting the tracking device 1 can also be a navigation ECU that navigates the vehicle 4's driving path. The dedicated computer constituting the tracking device 1 can also be at least one external computer, such as an external center or mobile terminal capable of communicating with the vehicle 4.

[0053] The dedicated computer constituting the tracking device 1 has at least one memory 10 and one processor 12. The memory 10 is a non-transitory tangible storage medium, such as semiconductor memory, magnetic media, and optical media, that stores computer-readable programs and data. The processor 12 includes at least one of the following as its core: CPU (Central Processing Unit), GPU (Graphics Processing Unit), and RISC (Reduced Instruction Set Computer) – CPU.

[0054] The processor 12 executes multiple instructions contained in the tracking program stored in the memory 10. Thus, the tracking device 1 constructs multiple functional modules for tracking the moving body 3. In this way, in the tracking device 1, multiple functional modules are constructed by the processor 12 executing multiple instructions through the tracking program stored in the memory 10 for tracking the moving body 3. For example... Figure 3 As shown, the multiple functional modules constructed by the tracking device 1 include a prediction module 100, an observation module 110, and an estimation module 120.

[0055] The following is based on Figure 4 The process of a tracking method in which the tracking device 1 tracks the moving body 3 through the cooperation of the prediction module 100, the observation module 110, and the estimation module 120 is described. This process is executed per tracking cycle. Furthermore, in this process, "S" refers to multiple steps executed by multiple instructions included in the tracking program.

[0056] In the tracking method S100, the prediction module 100 obtains the state value of the moving body 3 at observation time k. Figure 3 The predicted state value Z shown k|k－1 and its error covariance P k|k－1 Here, the predicted state value Z is... k|k－1 By using Figure 5 The physical quantities are defined by Equation 2, which is schematically shown in the diagram. In Equation 2, X and Y are the lateral and longitudinal center positions of the moving body 3 in the orthogonal coordinate system defined in the observation space, respectively. In Equation 2, Θ is the azimuth angle of the moving body 3 relative to the lateral direction in the orthogonal coordinate system defined in the observation space. In Equation 2, L and W are the front-to-back length and left-to-right width of the moving body 3 in the orthogonal coordinate system defined in the observation space, respectively. In Equation 2, Vx and Vy (omitted) Figure 5 The values ​​shown in the figures are the lateral velocity and longitudinal center velocity of the moving body 3 in the orthogonal coordinate system defined in the observation space.

[0057] [Equation 2]

[0058] Z k|k-1 =[X, Y, Θ, L, W, Vx, Vy] T

[0059] The prediction module 100 in S100 estimates the state value Z. k-1|k-1 and its error covariance P k-1|k-1 Perform time update operations after time transition to predictively obtain the predicted state value Z. k|k-1 Wherein, the estimated state value Z k-1|k-1 Let Z be the state value estimated as true by estimation module 120 at a past time k-1, which is earlier than the observation time k. At this time, the predicted state value Z...k|k-1 and error covariance P k|k-1 Using the estimated state value Z from the past time k-1 respectively k-1|k-1 and error covariance P k-1|k-1 The value is obtained through equations 3 to 5. Here, Q in equation 4 is the covariance matrix of the system noise (process noise).

[0060] [Formula 3]

[0061] Z k|k-1 =FZ k-1|k-1

[0062] [Formula 4]

[0063] P k|k-1 =FP k-1|k-1 F T +Q

[0064] [Formula 5]

[0065]

[0066] exist Figure 4 In the tracking method shown in S110, the observation module 110 acquires the position of the moving body 3 at observation time k from the external sensor system 2. Figure 3 The observed value z is shown k The observed value z of S110 k The acquisition can also be accompanied by the output of the observed value z from the external sensor system 2. k To execute, or in the output observation z k It is temporarily cached in memory 10 before execution. Additionally, the observation value z of such S110... k The predicted state value Z relative to S100 is obtained. k|k-1 The acquisition can be performed in parallel (i.e., simultaneously) or sequentially.

[0067] exist Figure 4 In the tracking method shown in S120, the estimation module 120 obtains the state value of the moving body 3 at observation time k as... Figure 3 The estimated state value Z, which represents the true value of the state, is shown. k|k and its error covariance P k|k At this point, by predicting the state value Z... k|k-1 and the observed value z at observation time k k Nonlinear filtering, used as a variable in the observation update operation, is employed to obtain the estimated state value Z. k|k and error covariance P k|k .

[0068] Specifically, the estimation module 120 in S120 performs... Figure 6 The estimation process is shown. In the estimation process S200, the estimation module 120 performs the estimation as follows: Figure 2 , Figure 5 , Figure 7 As shown, the multiple vertices m in the rectangular model M, which is modeled by modeling the moving body 3 as a rectangle, are... fl m bl m fr m br Each vertex is assigned a weight coefficient s. fl s bl s fr s br .

[0069] At this time, the estimation module 120 in S200 executes. Figure 8 The shown weight setting subroutine is used to set the weights according to each vertex m from the external sensor system 2. fl m bl m fr m br Visual recognition ω fl ω bl ω fr ω br To set the various weight coefficients s fl s bl s fr s br This subroutine is executed repeatedly with vertex m. fl m bl m fr m br The number of times corresponds to the number of times. Therefore, the vertex m fl m bl m fr m br The vertex that becomes the execution object in each iteration of this subroutine is called object vertex m. Furthermore, in the description of this subroutine, besides being based on... Figure 7 Apart from some examples, as suffixes represented by subscript characters in various variables, the markers fl, bl, fr, and br, which respectively represent the left front, left rear, right front, and right rear of the moving body 3, are omitted, for example, object vertex m.

[0070] exist Figure 8 In S300 of the weight setting subroutine shown, the estimation module 120 determines whether there exists a distance between the observation origin O of the external sensor system 2 and the object vertex m. Figure 7 The occlusion marker ST is shown in the image. Here, the occlusion marker ST could also be related to the right rear vertex m. br such as Figure 7As illustrated, connect the observation origin O to the object vertex m (m in this diagram). br An object existing separately from the moving body 3 on the line segment between ) . The occluded landmark ST can also be related to the right front vertex m fr such as Figure 7 As illustrated, connect the observation origin O to the object vertex m (m in this diagram). fr The moving body 3, which exists on the line segment between the two points, corresponds to one side of the rectangular model M. Furthermore, the observation origin O can be a sensing origin set for a single sensor device constituting the external sensor system 2, or a spatial origin assumed in the observation space through the fusion of multiple sensor devices constituting the external sensor system 2.

[0071] like Figure 8 As shown, when an occluded object ST is determined to exist in S300, the weight setting subroutine moves to S310, thereby estimating module 120 sets the visual recognition ω to the minimum value. Here, in Figure 7 In the example shown, the right rear vertex m of the occluded target ST is hidden. br and the right front vertex m fr Visual recognition ω br ω fr It is set to "0" as the minimum value.

[0072] like Figure 8 As shown, when it is determined in S300 that there is no occluded object ST, the weight setting subroutine moves to S320, thereby estimating module 120 determines whether object vertex m exists outside the sensing area AS of external sensor system 2. As a result, when it is determined that object vertex m exists outside the sensing area AS, the weight setting subroutine moves to S310, thereby estimating module 120 sets the visual recognition ω to the minimum value. Furthermore, the sensing area AS can be a viewing angle set in a single sensor device constituting external sensor system 2, or it can be an area where the viewing angles of multiple sensor devices constituting external sensor system 2 overlap.

[0073] In S320, if it is determined that the object vertex m does not exist outside the sensing area AS, the weight setting subroutine moves to S330, thereby estimating the module 120 to obtain the visual recognition determination angle. Here, as Figure 7 As shown, visual recognition judgment angle It is defined as the smallest angle among the angles formed by the two sides connecting to vertex m of the object and the line segment connecting the observation origin O and vertex m of the object. Here, regarding the left rear vertex m bl of Figure 7 The smallest angle in the example of Or regarding the left front vertex m fl of Figure 7 The smallest angle in the example of Corresponding to the visual recognition judgment angles

[0074] like Figure 8 As shown, following S330, in S340 of the weight setting subroutine, the estimation module 120 determines the visual recognition judgment angle. Does it exceed 90 degrees? The result is that, in visual recognition judgment angles... When the angle exceeds 90 degrees, the weight setting subroutine moves to S350, thereby estimating module 120 sets the visual recognition degree ω to its maximum value. On the other hand, at the visual recognition determination angle... When the angle is below 90 degrees, the weight setting subroutine is moved to S360 to estimate the visual recognition ω of module 120, which is above the minimum value and below the maximum value, by setting Equation 6. Here, regarding the left rear vertex m bl of Figure 7 In the example, the visual recognition angle is determined based on a visual recognition angle exceeding 90 degrees. Visual recognition ω bl Set to "1" as the highest value; on the other hand, regarding the left front vertex m... fl of Figure 7 In the example, the angle is determined based on visual recognition of less than 90 degrees. Visual recognition ω fl Set as the operation value in Equation 6

[0075] [Formula 6]

[0076] ω=sinφ

[0077] like Figure 8 As shown, following steps S310, S350, and S360, in step S370 of the weight setting subroutine, the estimation module 120 determines whether the visual recognition ω is "0" as the minimum value. As a result, if the visual recognition ω is 0, the weight setting subroutine moves to S380, whereby the estimation module 120 sets the weight coefficient s to the maximum value s. max On the other hand, when the visual recognition ω is not 0, the weight setting subroutine moves to S390, so that the estimation module 120 sets the weight coefficient s to the reciprocal (1 / ω) of the visual recognition ω and the maximum value s. max The smaller the value. In particular, when the weight coefficient s is set as the reciprocal of the visual recognition ω, the higher the visual recognition ω of a vertex m, the smaller the weight coefficient s is set.

[0078] Here, at the right rear vertex m of the hidden object ST.br and the right front vertex m fr of Figure 7 In the example, the weighting coefficient s is set to the maximum value s. max On the other hand, regarding the left rear vertex m bl and the left front vertex m fl of Figure 7 In the example, it is set as the highest value of visual recognition ω or the reciprocal of the value calculated in Equation 6 (1 or 1 / ) and maximum value s max The smaller of the values. Furthermore, the maximum value of the weighting coefficient s. max Defined as being able to avoid the observation error e described later. new covariance S new Due to nonlinear filtering, the protection value becomes infinitely large, greater than "1".

[0079] For all vertices m fl m bl m fr m br Complete the S200 weight setting subroutine to achieve this, thus... Figure 6 As shown, the estimation process is moved to S210. In S210, the estimation module 120 estimates the state value Z based on the predicted state value. k|k-1 and the observed value z at observation time k k To obtain each vertex m fl m bl m fr m br The observation error e at the location new .

[0080] At this point, the estimation module 120 in S210 uses the nonlinear function h from Equation 7. new And the matrix transformation functions of Equations 8 to 11 will transform the observed value z k The physical quantities x, y, θ, l, w are converted into the values ​​of each vertex m of the rectangular model M. fl m bl m fr m br Expanded observation z new Here, in equations 8 to 11, x fl x bl x fr x br To construct the expanded observation value Z new The position coordinates, where the expanded observation z new Is it like this? Figure 2 As shown, the observed value z k The horizontal position x expands to each vertex m fl mbl m fr m br The obtained value is y. In equations 8 to 11, y fl y bl y fr y br To construct the expanded observation value z new The position coordinates, where the expanded observation z new Is it like this? Figure 2 As shown, the observed value z k The vertical position y is expanded to each vertex m fl m bl m fr m br The value obtained.

[0081] [Formula 7]

[0082] z new =h new (x, y, θ, l, w) = [x fl y fl x bl y bl x br y br x fr y fr ] T

[0083] [Formula 8]

[0084]

[0085] [Formula 9]

[0086]

[0087] [Formula 10]

[0088]

[0089] [Equation 11]

[0090]

[0091] The estimation module 120 in S210 uses the nonlinear function h of Equation 12. new The matrix transformation functions in equations 13 to 16 will predict the state value Z. k|k-1 The physical quantities X, Y, Θ, L, W are converted into the values ​​of each vertex m of the rectangular model M. fl m bl m fr m br Unfolded state value Z newHere, in equations 13 to 16, X fl X bl X fr X br To construct the unfolded state value Z new The position coordinates, where the unfolded state value Z new Is it like this? Figure 5 As shown, predict the state value Z k|k-1 The lateral position X is expanded to each vertex m fl m bl m fr m br The obtained value. In equations 13 to 16, Y fl Y bl Y fr Y br To construct the unfolded state value Z new The position coordinates, where the unfolded state value Z new Is it like this? Figure 5 The figure shows the predicted state value Z. k|k-1 The vertical position Y is expanded to each vertex m fl m bl m fr m br The value obtained.

[0092] [Equation 12]

[0093] Z new =h new (X, Y, Θ, L, W) = [X fl Y fl X bl Y bl X br Y br X fr Y fr , T

[0094] [Equation 13]

[0095]

[0096] [Formula 14]

[0097]

[0098] [Formula 15]

[0099]

[0100] [Formula 16]

[0101]

[0102] In the estimation module 120 of S210, the expanded observation z obtained through this transformation is used. new and the unfolded state value Z new Equation 17, such as Figure 6 That way, the observation error e can be obtained. new .

[0103] [Equation 17]

[0104] e new =z new -Z new

[0105] Next, in S210, in the estimation process S220, the estimation module 120 estimates each vertex m... fl m bl m fr m br weighting coefficients s fl s bl s fr s br To obtain the observation error e new covariance S new At this point, the estimation module 120 uses weighting coefficients s fl s bl s fr s br Equation 18 is used to obtain the value for each vertex m. fl m bl m fr m br Weighted observation error e new The 8×8 covariance matrix R new Here, R' in Equation 18 is the covariance matrix for the lateral and longitudinal positions, which is an adjustment parameter that can be preset and adjusted. Furthermore, the estimation module 120 obtains the partial differential matrix (Jacobi form) H of the nonlinear function of Equation 12 using Equation 19. new In this way, in estimation module 120, by comparing with the observation error e new covariance matrix R new and the partial differential matrix H new Together, the predicted state value Z was used. k|k-1 Error covariance P k|k-1 Equation 20 is used to obtain the observation error e. new covariance S new .

[0106] [Formula 18]

[0107] R new =diag(s flR′,s bl R′,s br R′,s fr R′)

[0108] [Formula 19]

[0109]

[0110] [Formula 20]

[0111]

[0112] Next, in S220, in the estimation process S230, the estimation module 120 uses a nonlinear filter with an extended Kalman filter to update the predicted state value Z. k|k-1 The estimated state value Z after the true value k|k Its error covariance P k|k Together they are acquired. At this time, the estimation module 120 obtains the result by comparing it with the observation error e. new covariance S new and the partial differential matrix H new The predicted state value Z was used together. k|k-1 Error covariance P k|k-1 Equation 21 yields the Kalman gain K of the extended Kalman filter. new Therefore, in the estimation module 120, by comparing with the Kalman gain K... new and observation error e new The predicted state value Z was used together. k|k-1 Equation 22 is used to obtain the estimated state value Z. k|k Furthermore, in the estimation module 120, by comparing with the Kalman gain K... new and the partial differential matrix H new The predicted state value Z was used together. k|k-1 Error covariance P k|k-1 Equation 23 is used to obtain the estimated state value Z. k|k Error covariance P k|k Here, I in Equation 23 is the identity matrix.

[0113] [Equation 21]

[0114]

[0115] [Equation 22]

[0116] Z k|k =Z k|k-1 +K new e new

[0117] [Equation 23]

[0118] P k|k =(IK new H new )P k|k-1

[0119] like Figure 4 As shown, the latest estimated state value Z at observation time k is obtained by the tracking method in this tracking cycle. k|k The output is sent to the driving control ECU and used for driving control of vehicle 4, including the automatic driving mode. Additionally, the estimated state value Z at observation time k is obtained by the tracking method in this tracking cycle. k|k In the tracking method of the next tracking cycle, the estimated state value Z is used as the value at time k-1 in the past. k-1|k-1 It is used for prediction in S100 performed by prediction module 100.

[0120] (Effects)

[0121] The effects of this embodiment described above will now be explained. Furthermore, in the explanation of the effects, the markings fl, bl, fr, and fr, which respectively represent the left front, left rear, right front, and right rear of the moving body 3, are omitted as suffixes represented by subscript characters in various variables.

[0122] According to this embodiment, by predicting the state value Z k|k-1 and the observed value z at observation time k k Nonlinear filtering, as a variable, is used to estimate the true value of the state at observation time k. Then, based on the predicted state value Z... k|k-1 and the observed value z at observation time k k To obtain the observation error e at each vertex m in the rectangular model M formed by modeling the moving body 3. new And the observation error e is obtained based on the weight coefficients s of each vertex m. new covariance S new Therefore, according to this embodiment, which sets a weight coefficient s for each vertex based on the visual recognition degree ω from the external sensor system 2, the visual recognition degree ω can be reflected in the true value estimation of the state value. Thus, the estimated state value Z, which is the true value of the state value, can be accurately estimated. k|k This improves the tracking accuracy of the moving object 3.

[0123] According to this embodiment, the higher the visual recognition ω of a vertex m, the smaller the weight coefficient s is set. Therefore, for a vertex m with high visual recognition ω from the external sensor system 2, due to the covariance S... newThe matrix components decrease, thus the contribution of the state values ​​to the truth estimation increases. Therefore, it is possible to estimate the state value Z as the accurate truth value reflecting the visual recognition ω from the external sensor system 2. k|k This improves the tracking accuracy of the moving object 3.

[0124] According to this embodiment, for a vertex m that is obscured by a target ST that is connected to the external sensor system 2, the weight coefficient s is set to the maximum value s. max Therefore, the visual recognition ω from the external sensor system 2, due to being hidden in the occluded target ST, is assumed to be substantially zero at vertex m, due to the covariance S new The matrix components increase, thus the contribution of the state values ​​to the true value estimation decreases. Therefore, it is possible to estimate the state value Z as the accurate true value reflecting the visual recognition ω from the external sensor system 2. k|k This improves the tracking accuracy of the moving object 3.

[0125] According to this embodiment, for a vertex m existing outside the sensing area AS of the external sensor system 2, the weighting coefficient s is set to the maximum value s. max Therefore, regarding the vertex m, whose visual recognition ω is essentially zero due to its location outside the sensing region AS and originating from the external sensor system 2, the covariance S... new The matrix components increase, thus the contribution of the state values ​​to the true value estimation decreases. Therefore, it is possible to estimate the state value Z as the accurate true value reflecting the visual recognition ω from the external sensor system 2. k|k This improves the tracking accuracy of the moving object 3.

[0126] (Other implementation methods)

[0127] The above describes one embodiment, but this disclosure is not limited to this embodiment and can be applied to various embodiments without departing from the spirit of this disclosure.

[0128] In a variation, the dedicated computer constituting the tracking device 1 may also include at least one of digital circuitry and analog circuitry as a processor. Here, the so-called digital circuitry includes, for example, at least one of 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). Furthermore, such digital circuitry may also have a memory storing programs.

[0129] The tracking device, tracking method, and tracking program of the modified examples can also be applied to applications other than vehicles. In this case, the tracking device, in particular, applied to applications other than vehicles, can be mounted or installed on the same application as the external sensor system 2, or it can be mounted or installed on an application different from the external sensor system 2.

Claims

1. A tracking device comprising a processor, wherein the tracking device tracks a moving body by estimating its state values ​​over time based on observations from an external sensor system. The processor described above is configured to perform the following steps: Obtain the observed values ​​of the aforementioned moving body at the observation time; The predicted state value is obtained by predicting the state value of the moving body at the aforementioned observation time; and The true value of the state at the aforementioned observation time is estimated by applying a nonlinear filter that uses the predicted state value and the observed value at the aforementioned observation time as variables. Estimating the truth value above involves the following steps: The weight coefficient of each vertex is set based on the visual recognizability of each vertex in the rectangular model formed by modeling the moving body from the external sensor system. The observation error at each of the aforementioned vertices is obtained based on the predicted state values ​​and the observed values ​​at the aforementioned observation times; and The covariance of the observation error is obtained based on the weight coefficients of each of the aforementioned vertices.

2. The tracking device according to claim 1, wherein, The above weighting coefficients include: The higher the visual recognition of the aforementioned vertices, the smaller the weight coefficient should be.

3. The tracking device according to claim 1, wherein, The above weighting coefficients include: For the aforementioned vertices that are obstructed by the external sensor system, the aforementioned weighting coefficients are set to the maximum value.

4. The tracking device according to claim 1, wherein, The above weighting coefficients include: For the aforementioned vertices that exist outside the sensing area of ​​the aforementioned external sensor system, the aforementioned weighting coefficients are set to the maximum value.

5. The tracking device according to claim 1, wherein, Obtaining the above predicted state values ​​includes: The predicted state value at the aforementioned observation time is obtained based on the true value estimated at a past time earlier than the aforementioned observation time.

6. The tracking device according to any one of claims 1 to 5, wherein, Estimate the truth value of the above to include: The true value of the predicted state value that has been updated is obtained by using the aforementioned nonlinear filtering of the extended Kalman filter.

7. A tracking method for tracking a moving body by estimating its state value over time based on observations from an external sensor system, wherein the tracking method is estimated by a processor, the tracking method comprising the following steps: Obtain the observed values ​​of the aforementioned moving body at the observation time; The predicted state value is obtained by predicting the state value of the moving body at the aforementioned observation time; and The true value of the state at the aforementioned observation time is estimated by applying a nonlinear filter that uses the predicted state value and the observed value at the aforementioned observation time as variables. Estimating the truth value above involves the following steps: The weight coefficient of each vertex is set based on the visual recognizability of each vertex in the rectangular model formed by modeling the moving body from the external sensor system. The observation error at each of the aforementioned vertices is obtained based on the predicted state values ​​and the observed values ​​at the aforementioned observation times; and The covariance of the observation error is obtained based on the weight coefficients of each of the aforementioned vertices.

8. A storage medium storing a tracking program, the tracking program comprising instructions executed by a processor for tracking a moving body by estimating its state values ​​over time based on observations from an external sensor system. The above instructions include: Obtain the observed values ​​of the aforementioned moving body at the observation time; The predicted state value is obtained by predicting the state value of the moving body at the aforementioned observation time; and The true value of the state at the aforementioned observation time is estimated by applying a nonlinear filter that uses the predicted state value and the observed value at the aforementioned observation time as variables. Estimating the truth value above involves the following steps: The weight coefficient of each vertex is set based on the visual recognizability of each vertex in the rectangular model formed by modeling the moving body from the external sensor system. The observation error at each of the aforementioned vertices is obtained based on the predicted state values ​​and the observed values ​​at the aforementioned observation times; and The covariance of the observation error is obtained based on the weight coefficients of each of the aforementioned vertices.

Images