Estimation device

Abstract

To accurately estimate angular velocity of an object without using a road shape.SOLUTION: An estimation device 10 comprises: a sensor section 11 that transmits and receives sensor waves to detect positions and relative velocities of a plurality of reflection points on an object; a velocity vector calculation section 23 that calculates a velocity vector at an arbitrary reference point in the object; a reflection point distance calculation section 21 that calculates a distance from the reference point to a straight line inclined in a reflection point azimuth; a rotational relative velocity calculation section 25 that calculates a rotational relative velocity of each reflection point by subtracting a relative velocity in the reflection point azimuth due to the velocity vector of the reference point from a relative velocity of the reflection point; and an angular velocity calculation section 27 that calculates an angular velocity of the object on the basis of the reflection point distances and the rotational relative velocities calculated by the reflection point distance calculation section and the rotational relative velocity calculation section.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present disclosure relates to an estimation device mounted on a moving body to estimate the state of a surrounding object. [Background technology]

[0002] When tracking an object using an onboard radar, tracking can be delayed if the object's behavior changes from moving straight to turning. This is because object observation by onboard radar requires filtering that combines predictions and observations to suppress observation errors. In other words, when the object transitions from moving straight to turning, filtering makes predictions assuming straight movement, and the observation results deviate from the object's actual behavior.

[0003] A common technique for alleviating this problem is to ensure tracking performance by adjusting the filter gain of a filter, but this approach may result in a decrease in stability.In response to this, Patent Document 1 proposes improving tracking performance when tracking an object by estimating the object's traveling direction and angular velocity based on the road shape. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Patent Publication No. 2021-60245 Summary of the Invention [Problem to be solved by the invention]

[0005] The estimation device described in Patent Document 1 estimates the direction of travel and angular velocity of an object based on the road shape, so even if the object's state changes from moving straight to turning, the change can be estimated instantaneously and the object can be tracked with high accuracy.

[0006] However, the estimation device described in Patent Document 1 has a problem in that when an object passes a fork in the road, for example, when the object enters an intersection, it is not possible to accurately predict the direction of travel of the object, which results in a decrease in the accuracy of estimating the angular velocity.

[0007] An object of one aspect of the present disclosure is to enable accurate estimation of the angular velocity of an object without using the shape of a road. [Means for solving the problem]

[0008] An estimation device according to one aspect of the present disclosure is an estimation device mounted on a moving body to estimate the state of surrounding objects, and includes a sensor unit (11), a velocity vector calculation unit (23), a reflection point distance calculation unit (21), a rotational relative velocity calculation unit (25), and an angular velocity calculation unit (27).

[0009] The sensor unit transmits sensor waves to the surrounding area and detects the positions and relative velocities of multiple reflection points on the object from the reflected waves. The velocity vector calculation unit calculates a velocity vector at an arbitrary reference point on the object.

[0010] The reflection point distance calculation unit calculates the distance from the reference point to a line whose slope is the reflection point azimuth detected by the sensor unit as the reflection point distance. The rotational relative velocity calculation unit calculates the rotational relative velocity of the reflection point, which is the value obtained by subtracting the relative velocity of the velocity vector calculated by the velocity vector calculation unit in the reflection point azimuth, from the relative velocity of the reflection point detected by the sensor unit.

[0011] The angular velocity calculation unit calculates the angular velocity of the object based on the reflection point distance and the rotational relative velocity of at least one reflection point calculated by the reflection point distance calculation unit and the rotational relative velocity calculation unit.

[0012] In this way, the estimation device of the present disclosure estimates the angular velocity of an object based on the positions and relative velocities of multiple reflection points of surrounding objects detected by transmitting and receiving sensor waves, without using the road shape.

[0013] This is because when a turning object is observed by transmitting and receiving sensor waves, a different relative velocity is detected at each reflection point. In other words, when a turning object is rotating, a rotational velocity occurs at each reflection point according to the distance from the center of rotation, and the magnitude of this rotational velocity changes depending on the angular velocity of the object.

[0014] Therefore, in the estimation device of the present disclosure, the rotational velocity of the reflection point is obtained as a rotational relative velocity by subtracting the relative velocity of the velocity vector of the reference point in the reflection point direction from the relative velocity of the reflection point, and the angular velocity is calculated based on the rotational relative velocity and the reflection point distance.

[0015] Therefore, according to the estimation device of the present disclosure, it is possible to estimate the angular velocity of an object without using the road shape, and it is possible to accurately estimate the angular velocity even when the object passes through a fork in the road. [Brief explanation of the drawings]

[0016] [Figure 1] FIG. 2 is a block diagram illustrating a hardware configuration of the estimation device according to the embodiment. [Figure 2] FIG. 2 is a block diagram illustrating a functional configuration of the estimation device according to the embodiment. [Figure 3] 3 is a diagram showing an example of a mounting position and detection range of a sensor unit in a vehicle; FIG. [Figure 4] 10 is a diagram showing another example of the mounting position and detection range of the sensor unit in the vehicle. FIG. [Figure 5] FIG. 1 is an explanatory diagram showing the relative velocity and rotational velocity at a plurality of reflection points of an object. [Figure 6] 10A and 10B are explanatory diagrams illustrating the calculation operation of the reflection point distance in the reflection point distance calculation unit. [Figure 7] 10A and 10B are explanatory diagrams illustrating a calculation operation of a rotational relative velocity in a rotational relative velocity calculation unit. [Figure 8] FIG. 10 is an explanatory diagram illustrating a method for calculating the angular velocity of an object. [Figure 9]10 is a flowchart showing a control process executed in the processing device. [Figure 10] 10 is a flowchart showing an angular velocity calculation process executed in S50 of FIG. 9. [Figure 11] 11 is a flowchart showing a target object determination process executed in S110 of FIG. [Figure 12] 11 is a flowchart showing a target reflection point determination process executed in S140 of FIG. 10. [Figure 13] 10 is a flowchart illustrating a first example of the state estimation process executed in S60 of FIG. 9. [Figure 14] 10 is a flowchart illustrating a second example of the state estimation process executed in S60 of FIG. 9. [Figure 15] 10A and 10B are explanatory diagrams illustrating a filter gain switching operation based on a calculated value of an angular velocity. [Figure 16] 10 is a flowchart illustrating a third example of the state estimation process executed in S60 of FIG. 9. [Figure 17] 10A and 10B are explanatory diagrams illustrating a correction operation of a predicted travel angle value based on a calculated value of an angular velocity. [Figure 18] 10 is a flowchart showing a control process executed by a processing device according to a second embodiment. [Figure 19] 19 is a flowchart showing a collision determination process executed in S180 of FIG. 18. DETAILED DESCRIPTION OF THE INVENTION

[0017] [First embodiment] [composition] The configuration of an estimation device 10 of this embodiment will be described with reference to Figures 1 and 2. The estimation device 10 includes a sensor unit 11 and a processing device 20, and is mounted on a vehicle 50, which is a moving body.

[0018] The sensor unit 11 is a radar, a lidar, a sonar, or the like. A radar transmits radio waves such as millimeter waves as sensor waves and receives reflected waves generated when the radar waves are reflected by an object 60. A lidar transmits light as sensor waves and receives reflected waves generated when the light is reflected by an object. A sonar transmits sound waves as sensor waves and receives reflected waves generated when the sound waves are reflected by an object.

[0019] As shown in FIG. 3, the sensor unit 11 may be mounted in the front center of the vehicle 50 (for example, in the center of the front bumper) and have a detection area A1 in the front center of the vehicle 50. Alternatively, as shown in FIG. 4, the sensor unit 11 may be mounted on the left front, right front, left rear, and right rear of the vehicle 50 (for example, the left and right ends of the front bumper and the left and right ends of the rear bumper) in addition to the front center of the vehicle 50. That is, the sensor unit 11 may have detection areas A2 in the left front, right front, left rear, and right rear of the vehicle 50 in addition to the detection area A1. The sensor unit 11 may be mounted in at least one location among the front center, left front, right front, left rear, and right rear of the vehicle 50.

[0020] The sensor unit 11 acquires a plurality of observation values ​​by transmitting and receiving sensor waves to and from the periphery of the vehicle 50. The sensor unit 11 is a high-resolution sensor, and by transmitting a sensor wave once, it can acquire a plurality of reflected waves reflected at different reflecting points on a single object 60 and acquire an observation value based on each reflected wave. Therefore, each of the plurality of observation values ​​corresponds to a different reflecting position on the single object 60.

[0021] Each of the multiple observation values ​​includes, as physical quantities, the position of the reflection point (i.e., the distance from the sensor unit 11 to the reflection point and the orientation of the reflection point relative to the sensor unit 11) and the relative speed between the reflection point and the vehicle 50 (more specifically, the sensor unit 11).

[0022] Next, the processing device 20 includes a microcomputer including a CPU, a ROM, a RAM, etc. The processing device 20 realizes the functions of a reflection point distance calculation unit 21, a velocity vector calculation unit 23, a rotational relative velocity calculation unit 25, and an angular velocity calculation unit 27 by the CPU executing a program stored in a non-transient tangible recording medium. Note that this function is a function for calculating the angular velocity, in other words, the yaw rate, of a single object 60 based on the observed values ​​of a plurality of reflection points of the object 60 acquired by the sensor unit 11.

[0023] In addition to the angular velocity calculation function by the calculation units 21, 23, 25, and 27, the processing device 20 also realizes the function of a state estimation unit 30 that estimates the state of the object 60 based on the observation values ​​of multiple reflection points acquired by the sensor unit 11.

[0024] In this embodiment, the function of the state estimation unit 30 is realized by performing extended object tracking using a shape model of the object 60. Then, with this function, physical quantities that represent the motion state of the object 60, such as the x-direction position, y-direction position, velocity, direction of travel, and angular velocity of the reference point of the object 60, are calculated as estimated values.

[0025] The x direction corresponds to the length direction of the vehicle 50, and the y direction corresponds to the width direction of the vehicle 50. Assuming that the object 60 is an automobile, the center positions of the left and right rear wheels (hereinafter referred to as the rear wheel axle centers) are set as the reference points of the object 60.

[0026] Extended object tracking is a method of modeling a target by assuming that the target has a shape, and estimating the motion state of the target over time, and in this embodiment, this method is performed in accordance with the procedure described in the following document A. The specific estimation procedure will be explained in the state estimation process described later.

[0027] Reference A: Soichiro Tokizawa, Keisuke Yoneda, Naoki Suganuma, "Extended Object Tracking for Autonomous Driving with Stability and Real-Time Performance," Transactions of the Society of Automotive Engineers of Japan, Vol. 52, No. 5, September 2021. Next, the reflection point distance calculation unit 21, velocity vector calculation unit 23, rotational relative velocity calculation unit 25, and angular velocity calculation unit 27, which realize the angular velocity calculation function, will be described.

[0028] First, the reflection point distance calculation unit 21 calculates the distance between the positions of the multiple reflection points of the object 60 detected by the sensor unit 11 and the reference point of the object 60 as the reflection point distance. Furthermore, the velocity vector calculation unit 23 calculates the velocity vector at the reference point of the object 60 from the latest state of the object 60 detected by the state estimation unit 30. Here, the velocity vector is a predicted value obtained by predicting the velocity vector at the current time from the velocity vector of the past reference point estimated in the state estimation process. For example, if the behavior of the object is assumed to be uniform linear motion, the predicted value of the velocity vector at the current time is used as the previously estimated velocity vector. On the other hand, if turning motion is assumed, for example, the predicted value of the velocity vector at the current time is used as the previously estimated velocity vector and the previously estimated angular velocity, and the change in direction according to the angular velocity is added.

[0029] Furthermore, the rotational relative velocity calculation unit 25 calculates the rotational relative velocity of each reflection point by subtracting the relative velocity of the velocity vector in the reflection point direction calculated by the velocity vector calculation unit 23 from the relative velocities of the multiple reflection points detected by the sensor unit 11. Note that the relative velocity of the velocity vector in the reflection point direction represents the velocity vector of the reference point calculated by the velocity vector calculation unit 23 converted into the relative velocity with respect to the vehicle 50.

[0030] Then, the angular velocity calculation unit 27 calculates the angular velocity of the object 60 based on the reflection point distance calculated by the reflection point distance calculation unit 21 and the rotational relative velocity calculated by the rotational relative velocity calculation unit 25 .

[0031] The result of the angular velocity calculation by the angular velocity calculation unit 27 is output to the state estimation unit 30, and the state estimation unit 30 reflects the calculated value of the angular velocity calculated by the angular velocity calculation unit 27 in the result of the state estimation of the object 60. As a result, even if a delay occurs in the state estimation by the state estimation unit 30 when the object 60 turns due to filtering processing such as a Kalman filter used in the state estimation, the delay can be corrected by the angular velocity calculated by the angular velocity calculation unit 27. This correction method will be described later in the state estimation processing section.

[0032] [Angular velocity calculation principle] Next, the principles of calculation of angular velocity by reflection point distance calculation section 21, velocity vector calculation section 23, rotational relative velocity calculation section 25, and angular velocity calculation section 27 will be described.

[0033] As shown in Fig. 5A, when the object 60 is moving straight, the relative speeds of the multiple reflection points detected by the sensor unit 11 are approximately the same. In contrast, when the object 60 is turning, the relative speeds of the multiple reflection points differ for each reflection point, as shown in Fig. 5B.

[0034] The reason why the relative speed of each reflection point differs while the object 60 is turning is that a rotational speed (V) corresponding to the distance (r) from the center of rotation occurs at each reflection point due to the rotational motion of the object 60. As is clear from the equation for the speed of circular motion, "V = r ω," this rotational speed (V) changes according to the angular speed (ω) of the object 60, in other words, the yaw rate.

[0035] Therefore, the yaw rate can be derived from the relationship between the position and velocity vector of an arbitrary reference point Pr within the object and the relative velocity and direction of a reflection point at a position different from the reference point Pr. Therefore, in this embodiment, the angular velocity of the object 60 is calculated from the position and velocity vector of the reference point Pr and the relative velocity and direction of the reflection point.

[0036] Here, the translational velocity components due to translational movement are the same for the reference point Pr and the reflection point, but the rotational velocity components due to rotational movement differ due to the difference in position. That is, if the position of the reflection point of the object is x, y, the position of an arbitrary reference point Pr within the object is x0, y0, the velocities of the reference point in the x and y directions are Vx0, Vy0, and the angular velocity of the object is ω, the velocities Vx, Vy of the reflection point in the x and y directions can be written as in the following equations (1) and (2), respectively.

[0037] Vx=Vx0-ω(y-y0) … (1) Vy=Vy0+ω(x-x0) … (2) The relative velocity Vr of the reflection point can be expressed as the following equation (3):

[0038] Vr=Vxcosθ+Vysinθ … (3) Then, substituting equations (1) and (2) into equation (3), we obtain the following equation (4). Vr=(Vx0-ω(y-y0))cosθ+(Vy0+ω(x-x0))sinθ =Vx0cosθ+Vy0sinθ +(-ycosθ+xsinθ)ω +(y0cosθ-x0sinθ)ω … (4) Here, since x=rcosθ and y=rsinθ, xsinθ=ycosθ, and so equation (4) can be written as the following equation (5): And, from equation (5), the following equation (6) holds.

[0039] Vr=Vx0cosθ+Vy0sinθ+(y0cosθ-x0sinθ)ω … (5) (y0cosθ-x0sinθ)ω=Vr-(Vx0cosθ+Vy0sinθ) … (6) In equation (6), the value in parentheses on the left side corresponds to the distance from the reference point Pr to the line whose slope is the reflection point azimuth, in other words, the circumferential positional deviation from the reference point Pr to the reflection point.The right side corresponds to the velocity (hereinafter referred to as the rotational relative velocity) obtained by subtracting the relative velocity of the velocity vector of the reference point Pr in the reflection point azimuth from the relative velocity Vr of the reflection point, in other words, the velocity difference between the reference point Pr and the reflection point with the circumferential velocity component cancelled out. [Angular velocity calculation operation] In this embodiment, the state estimation unit 30 calculates the velocity vector of the reference point Pr of the object 60, and then calculates the angular velocity of the object 60.

[0040] That is, as shown in FIG. 6, the reflection point distance calculation unit 21 calculates the distance between a line having a gradient equal to the direction of each reflection point detected by the sensor unit 11 and the reference point Pr as the reflection point distance. 6, the reflection point distance represents the length of a line perpendicular to the line in the reflection point azimuth when viewed from the reference point Pr. However, as shown by the dotted line in FIG. 6, the reflection point distance may also be calculated as the length of an arc whose radius is the distance from the sensor unit 11, which is the origin of azimuth measurement, to the reference point Pr. This also makes it possible to calculate the distance from the reference point Pr to the line in the reflection point azimuth. In other words, the reflection point distance may be calculated approximately as the length of a line perpendicular to the line in the reflection point azimuth, or the length of an arc whose radius is the distance to the reference point Pr. Furthermore, the rotational relative velocity calculation unit 25 calculates the rotational relative velocity by subtracting the relative velocity (Vx0cosθ+Vy0sinθ) of the velocity vector of the reference point Pr shown in FIG. 7 in the reflection point azimuth from the relative velocity Vr of each reflection point.

[0041] The angular velocity calculation unit 27 identifies the position corresponding to each reflection point in a coordinate space with the reflection point distance as the horizontal axis and the relative rotational velocity as the vertical axis, as indicated by the x marks in Fig. 8. Then, the regression processing unit 27A shown in Fig. 2 performs linear regression from the distribution of each reflection point in the coordinate space, and calculates the angular velocity ω of the object 60 from the gradient of the distribution of each reflection point.

[0042] Therefore, the angular velocity calculation unit 27 can calculate the angular velocity of the object 60 without using a filter process such as a Kalman filter, and can instantly obtain the angular velocity of the object 60 without a response delay. In other words, the angular velocity calculation unit 27 can calculate the instantaneous angular velocity of the object 60, and therefore the instantaneous yaw rate.

[0043] [Control processing] Next, a description will be given of the control processing executed to realize the angular velocity calculation function and the state estimation function in the processing device 20. Note that this control processing is repeatedly executed in the processing device 20 at a predetermined processing cycle.

[0044] 9, when the control process is started in the processing device 20, first, in S10 (S represents step), the sensor unit 11 is caused to transmit and receive sensor waves, thereby acquiring a plurality of observation values ​​of the object 60 to be tracked from the sensor unit 11. The object 60 to be tracked is an object located ahead of the vehicle 50 in the traveling direction, and is set in advance based on the estimation result of the state estimation process executed in S60, which will be described later.

[0045] Next, in S20, the process as the velocity vector calculation unit 23 is executed to calculate the velocity vector of the reference point Pr from the velocity vector of the reference point Pr of the object 60 previously estimated in the state estimation process of S60.

[0046] In S30, based on the multiple observation values ​​of the object 60 acquired from the sensor unit 11 in S10, the reflection point distance calculation unit 21 performs processing to calculate the distance from the reference point Pr of the object 60 to a line whose slope is the reflection point orientation of the multiple reflection points as the reflection point distance of each reflection point.

[0047] Then, in S40, the rotational relative velocity of each reflection point is calculated by subtracting the relative velocity of the velocity vector of the reference point Pr in the reflection point direction from the relative velocity of each reflection point based on the relative velocity of each reflection point acquired from the sensor unit 11 in S10 and the velocity vector calculated in S20. The processing of S40 realizes the function of the rotational relative velocity calculation unit 25.

[0048] In this way, when the reflection point distance and the rotational relative velocity are calculated for each reflection point of the object 60 detected by the sensor unit 11, the process proceeds to S50, and the angular velocity of the object 60 is calculated in accordance with the above-mentioned principle of angular velocity calculation, and processing as the angular velocity calculation unit 27 is executed.

[0049] In the next step S60, the state estimation unit 30 performs processing to estimate the latest state of the object 60 based on the observation value acquired from the sensor unit 11 in S10, the angular velocity (i.e., the instantaneous angular velocity) calculated in S50, and the state of the object 60 previously estimated in S60.

[0050] Then, after the state estimation process is executed in S60, the process proceeds to S10, whereby the above series of processes are repeatedly executed at a predetermined processing cycle. [Angular velocity calculation process] Next, the angular velocity calculation process executed in S50 will be described with reference to FIGS.

[0051] 10, in the angular velocity calculation process, first, in S110, a target object determination process is executed to determine whether or not the object 60 detected by the sensor unit 11 is a target object for which angular velocity is to be calculated. This target object determination process is executed, for example, according to the procedure shown in FIG.

[0052] 11, in the target object determination process, first, in S210, it is determined whether the distance between the object 60 and the vehicle 50 is equal to or less than a preset threshold value for determining short distance. If it is determined in S210 that the distance is equal to or less than the threshold value, the process proceeds to S250, where the object 60 is determined to be a non-target object for which angular velocity calculation is not performed.

[0053] On the other hand, if it is determined in S210 that the distance is greater than the threshold, the process proceeds to S220, where it is determined whether the number of reflection points detected by the sensor unit 11 is equal to or less than a preset threshold for determining the number of reflection points. If it is determined in S220 that the number of reflection points is equal to or less than the threshold, the process proceeds to S250, where it is determined that the object 60 is a non-target object, and if it is determined in S220 that the number of reflection points is greater than the threshold, the process proceeds to S240.

[0054] In S240, it is determined whether the absolute value of the acceleration of object 60 is equal to or greater than a preset threshold value for determining acceleration or deceleration. If it is determined in S240 that the absolute value of the acceleration is equal to or greater than the threshold value and that object 60 is accelerating or decelerating, the process proceeds to S250, where it is determined that object 60 is not a target object. If it is determined in S240 that the acceleration is smaller than the threshold value and that object 60 is not accelerating or decelerating, the process proceeds to S240. In S240, it is determined that object 60 is a target object for which angular velocity calculation is performed, and the target object determination process ends.

[0055] In this way, in the target object determination process, when the object 60 and the host vehicle 50 are far apart, the number of reflection points detected by the sensor unit 11 is greater than a predetermined threshold, and the object 60 is not accelerating or decelerating, the object 60 is determined to be a target object. This is because, under conditions where the angular velocity cannot be calculated accurately, the object 60 is determined to be a non-target object, and the angular velocity is not calculated.

[0056] That is, when the distance between the object 60 and the host vehicle 50 is short, the reflected waves from the object 60 tend to spread, and unnecessary signals are included in the received signal, which may reduce the accuracy of calculating the angular velocity. Also, when the number of reflection points is small, the linear regression described above may become unstable, which may reduce the accuracy of calculating the angular velocity. Also, when the object 60 is accelerating or decelerating, the velocity vector of the reference point Pr obtained by the state estimation process performed by the state estimation unit 30 may deviate from the actual velocity vector, which may reduce the accuracy of calculating the angular velocity.

[0057] Therefore, in this embodiment, under such conditions, the object 60 is determined to be a non-target object, and the angular velocity is not calculated. Note that the determination conditions used to determine whether the object 60 is a target object or a non-target object are not limited to the above three determination conditions, and two or one of the above three determination conditions may be used. Furthermore, other determination conditions may be added as determination conditions.

[0058] 10, after the target object determination process described above is executed in S110, the process proceeds to S120, where it is determined whether or not the target object 60 is determined to be a processing target (i.e., a target object) in the target object determination process. If it is determined in S120 that the object 60 is not a processing target, the angular velocity calculation process is terminated without calculating the angular velocity in the subsequent processes.

[0059] On the other hand, if it is determined in S120 that the object 60 is a processing target, the process proceeds to S130, where it is determined whether or not the target reflection point determination process of S140 has been executed for all reflection points acquired from the sensor unit 11. Then, if it is determined in S130 that the target reflection point determination process of S140 has been executed for all reflection points, the process proceeds to S170.

[0060] Furthermore, if it is determined in S130 that the target reflection point determination process of S140 has not been executed for all reflection points, the process proceeds to S140, where the target reflection point determination process is executed. The target reflection point determination process of S140 is a process for determining whether the reflection point of the object 60 detected by the sensor unit 11 is a target reflection point suitable for calculating angular velocity, or a non-target reflection point unsuitable for calculating angular velocity.

[0061] In this target reflection point determination process, a reflection point for which the determination process has not been performed is obtained from among the multiple reflection points detected by the sensor unit 11, and it is determined whether or not that reflection point is a target reflection point using the procedure shown in Figure 12.

[0062] That is, in the target reflection point determination process, in S310, it is determined whether the received power of the reflected wave from the reflection point is equal to or less than a preset threshold for determining received power. If it is determined in S310 that the received power of the reflection point is equal to or less than the threshold, it is considered that the received signal from the reflection point is affected by noise due to micro-Doppler, multipath, etc., so the process proceeds to S350. Then, in S350, the reflection point is determined to be a non-target reflection point for which angular velocity calculation is not performed.

[0063] On the other hand, if it is determined in S310 that the received power of the reflection point exceeds the threshold, the process proceeds to S320, where it is determined whether the speed difference between the reflection point and the reference point Pr is equal to or greater than a preset threshold for determining the speed difference. If it is determined in S320 that the speed difference between the reflection point and the reference point Pr is equal to or greater than the threshold, it is possible that the speed of the reflection point has not been detected normally by micro Doppler or the like, so the process proceeds to S350, where it is determined that the reflection point is a non-target reflection point.

[0064] If it is determined in S320 that the speed difference between the reflection point and the reference point Pr is less than the threshold, the process proceeds to S330, where it is determined whether or not the reflected waves are multiple waves in the direction estimation of the reflection point by the sensor unit 11.

[0065] Here, multiple waves in azimuth estimation refers to a state in which multiple reflected waves are detected at the same distance during azimuth estimation processing. For example, in azimuth estimation processing that extracts azimuths with peaks above a threshold from an angle FFT spectrum, this corresponds to a case in which multiple peaks are above a threshold.

[0066] If the direction estimation of the reflected wave indicates that there are multiple reflected waves, it is possible that the detection accuracy of the direction of the reflection point, in other words, the position of the reflection point, is low, so the process proceeds to S350 and the reflection point is determined to be a non-target reflection point.

[0067] Next, if it is determined in S330 that there are not multiple reflected waves in the direction estimation of the reflection point, the process proceeds to S340, where the reflection point is determined to be a target reflection point to be used in calculating the angular velocity. That is, in the target reflection point determination process, it is confirmed that the received power exceeds a threshold value for each reflection point, the velocity difference with the reference point Pr is less than a threshold value, and the direction of the reflection point can be correctly estimated from the reflected waves from the reflection point. When these three conditions are met, the reflection point is determined to be a target reflection point.

[0068] In the target reflection point determination process, after the process of S340 or S350 is executed, the target reflection point determination process is terminated and the process proceeds to S150 in Fig. 10. Then, when the target reflection point determination process is started next time, reflection points that have not yet been determined as target reflection points are determined as target reflection points in the same procedure as above.

[0069] In the above-mentioned target reflection point determination process, by performing the determination processes of S310 to S330, when the above-mentioned three determination conditions are met, the reflection point is determined to be a target reflection point. However, in the target reflection point determination process, at least one of the determination processes of S310 to S330 may be executed. In other words, in the target reflection point determination process, when one or two of the above-mentioned three determination conditions are met, the reflection point may be determined to be a target reflection point.

[0070] 10, it is determined whether the reflection point is determined to be a processing target (i.e., a target reflection point) in the target reflection point determination process of S140. If it is determined in S150 that the reflection point is not a processing target, the process proceeds to S130.

[0071] Furthermore, if it is determined in S150 that the reflection point is a processing target, the process proceeds to S160. In S160, angular velocity calculation information for the reflection point determined to be a target reflection point in S140 is stored in a recording medium such as RAM as reflection point information, and the process proceeds to S130. Note that the reflection point information includes the reflection point distance and rotational relative velocity calculated in the reflection point distance calculation process in S30 and the rotational relative velocity calculation process in S40.

[0072] Next, in S170, which is executed when it is determined in S130 that the target reflection point determination process has been performed for all reflection points, the angular velocity of the object 60 is calculated based on the reflection point distance and rotational relative velocity of each reflection point stored on the recording medium in S160.

[0073] That is, in S170, as described above, the angular velocity of the object 60 is calculated by specifying the position corresponding to each reflection point in a coordinate space having the reflection point distance and the rotational relative velocity as parameters, and then finding the gradient of the distribution of each reflection point by linear regression. Then, once the angular velocity is calculated in S170, the angular velocity calculation process of S50 shown in Fig. 9 is terminated, and the state estimation process of S60 is executed.

[0074] [State estimation process] The state estimation process of S60 is executed, for example, according to the procedure shown in FIG. 13, in the state estimation process, a prediction process is first executed in S410. In this prediction process, the contour of the object 60 (hereinafter referred to as a predicted contour) is predicted based on a predetermined shape model of the object 60 and an estimated value calculated in a past processing cycle (for example, the previous processing cycle).

[0075] The shape model of the object 60 is determined by selecting a model that is suitable for the size and shape of the object 60 estimated from the observation values ​​of multiple reflection points from multiple pre-prepared models, such as a circular model, an elliptical model, a rectangular model, etc.

[0076] The predicted contour corresponds to the area where the target exists, predicted from the state of the reference point Pr, which is a past estimated value. Next, in S420, an association process is executed in which each of the plurality of predicted values ​​calculated in S410 is associated with the observed value acquired from the sensor unit 11 in S10, and an association set is generated.

[0077] Next, in S430, an estimation process is executed in which a filter such as a Kalman filter is applied to the association set calculated in S40 to calculate a current estimated value P2. In the estimation process of S430, an update amount of the estimated value based on the observed value associated with the predicted value is calculated using a nonlinear filter such as an extended Kalman filter. The estimated value is then updated based on the predicted value and the update amount of the estimated value. This updated estimated value becomes the estimated value for the current processing cycle. The estimated value also includes at least a parameter representing the velocity vector of the reference point Pr. For example, this could be a combination of the longitudinal velocity and lateral velocity in an arbitrary coordinate system, or the magnitude and direction of the velocity.

[0078] Next, in S440, an angular velocity mixing process is performed to mix the angular velocity of the reference point Pr included in the estimated value updated in the estimation process of S430 (hereinafter referred to as the angular velocity estimated value) with the angular velocity calculated in the angular velocity calculation process of S50 (hereinafter referred to as the angular velocity calculated value).

[0079] In this angular velocity mixing process, the angular velocity estimated value and the angular velocity calculated value are mixed together using the following equation (7) including a coefficient α smaller than 1, to update the angular velocity estimated value. Note that in the following equation (7), ωfil represents the angular velocity estimated value, and ωobs represents the angular velocity calculated value.

[0080] ωfil=(1-α)·ωfil+α·ωobs … (7) Then, when the angular velocity mixing process of S440 is performed, the state estimation process of S60 is terminated and the process proceeds to S10 in FIG. 9, whereby angular velocity calculation and state estimation in the next processing cycle are started.

[0081] [effect] As described above, in the estimation device 10 of this embodiment, the angular velocity calculation unit 27 calculates the angular velocity of the object 60 based on the reflection point distance between at least one reflection point of the object 60 and the reference point Pr and the relative rotational velocity of the reflection point.

[0082] Therefore, the angular velocity calculation unit 27 of this embodiment can calculate the angular velocity of the object 60 (i.e., the calculated angular velocity value) without using the road shape and without using filtering such as a Kalman filter. Therefore, the angular velocity of the object 60 can be calculated accurately in accordance with the actual moving state of the object 60 and without a response delay.

[0083] Furthermore, by executing the target object determination process, angular velocity calculation unit 27 determines whether any of the following conditions is met: the distance from vehicle 50 is equal to or less than a threshold, the number of reflection points detected by sensor unit 11 is equal to or less than a threshold, and the absolute value of the acceleration of the object is equal to or greater than a threshold. If any of these conditions is met, it is determined that the accuracy of calculating the angular velocity will decrease, and the angular velocity of object 60 is not calculated.

[0084] Furthermore, the angular velocity calculation unit 27 executes a target reflection point determination process for the object 60 determined to be a target object in the target object determination process. In the target reflection point determination process, among the reflection points detected by the sensor unit 11, reflection points whose received power is equal to or less than a threshold, reflection points whose velocity difference with the reference point Pr is equal to or more than a threshold, and reflection points whose reflected waves are multiple waves in the direction estimation are excluded from the objects to be calculated for angular velocity.

[0085] Therefore, the angular velocity calculation unit 27 can select a reflection point from which the reflected signal is received well by the sensor unit 11 and whose position and velocity are detected with high accuracy, from among the multiple reflection points detected by the sensor unit 11. Then, the angular velocity is calculated based on the reflection point distance and relative rotational velocity of the selected reflection point, thereby improving the accuracy of the calculation of the angular velocity.

[0086] Furthermore, in this embodiment, the calculation result of the angular velocity by the angular velocity calculation unit 27 (i.e., the calculated angular velocity value) is reflected in the state estimation result of the object 60 estimated by the state estimation unit 30. Specifically, in the angular velocity mixing process described above, the estimated angular velocity value of the state of the object 60 estimated by the state estimation unit 30 is corrected by the calculated angular velocity value.

[0087] As a result, the angular velocity estimation value estimated using a filter process such as a Kalman filter can be corrected using the angular velocity calculation value, which is an instantaneous value, and the estimation delay included in the final estimation result of the vehicle state can be suppressed. As a result, the vehicle 50 can more appropriately control the vehicle 50 based on the estimation result.

[0088] [Variations] In the above embodiment, as a first example of reflecting the angular velocity calculation value calculated by the angular velocity calculation unit 27 in the vehicle state estimation result by the state estimation unit 30, the angular velocity mixing process is described in which the angular velocity estimation value and the angular velocity calculation value are mixed to correct the angular velocity estimation value.

[0089] However, in order to reflect the angular velocity calculation value in the vehicle state estimation result and perform more appropriate estimation of the vehicle state, it is not necessarily necessary to correct the angular velocity estimation value obtained by the state estimation unit 30 with the angular velocity calculation value.

[0090] Therefore, in this modification, a second example and a third example of a method for reflecting the angular velocity calculated by angular velocity calculation unit 27 in the estimation result of the vehicle state by state estimation unit 30 will be described.

[0091] 14, in the state estimation process of the second example, first, in S400, it is determined whether the absolute value of the calculated angular velocity value calculated in the angular velocity calculation process of S50 is equal to or greater than a preset threshold. If it is determined in S400 that the absolute value of the calculated angular velocity value is equal to or greater than the threshold, that is, if the yaw rate of object 60 is large, the process proceeds to S405, where the filter gain is increased from the initial value, and then the process proceeds to S410.

[0092] On the other hand, if it is determined in S400 that the absolute value of the calculated angular velocity is less than the threshold value, that is, if the yaw rate of the object 60 is small, the filter gain is not increased and the process proceeds to S410.

[0093] This is because, when the angular velocity of the object 60 is large, it is considered that the reliability of the prediction by S410 is low, and therefore, as shown in FIG. 15, the observed values ​​of each reflection point are trusted and the filter gain is made larger than when the angular velocity of the object 60 is small, thereby improving tracking ability.

[0094] After S410, the process of S410 to S430 shown in FIG. 13 is executed, and the state estimation process ends. In this way, in the state estimation process of the second example, the filter gain used in the state estimation unit 30 is corrected based on the angular velocity calculation value calculated by the angular velocity calculation unit 27. Even in this way, the accuracy of the state estimation of the object 60 by the state estimation unit 30 can be improved.

[0095] Next, in the state estimation process of the third example, as shown in FIG. 16, first, the above-mentioned prediction process is executed in S410, and then the speed vector prediction correction process is executed in S415, and the process proceeds to S420.

[0096] In the velocity vector prediction correction process, as shown in FIG. 17, the velocity vector prediction value for the current processing cycle is corrected by correcting the change in the velocity direction of the reference point Pr of the object 60 (hereinafter referred to as the velocity direction change) based on the angular velocity calculation value calculated by the angular velocity calculation unit 27.

[0097] After S420, the processes of S420 and S430 shown in FIG. 13 are executed, and the state estimation process ends. In this way, in the state estimation process of the third example, the velocity vector predicted value is corrected by correcting the velocity direction change amount of the velocity of the reference point Pr estimated in the previous processing cycle based on the angular velocity calculation value, and therefore it is possible to more appropriately estimate the velocity and traveling direction of the reference point Pr of the object 60. Therefore, it is possible to improve the accuracy of the state estimation of the object 60 by the state estimation unit 30.

[0098] The correction of the filter gain in the second example and the correction of the velocity vector predicted value in the third example may be performed in combination with other corrections, such as the correction of the angular velocity estimated value in the first example. [Second embodiment] The second embodiment has the same basic configuration as the first embodiment, so differences from the first embodiment will be described below. Note that the same reference numerals as those in the first embodiment indicate the same configuration, and reference is made to the preceding description.

[0099] The second embodiment differs from the first embodiment in that the processing device 20 is provided with a function as a collision determination unit 32, as indicated by the dotted line in Fig. 2. The function as the collision determination unit 32 is realized by an angular velocity calculation process executed by the processing device 20.

[0100] That is, as shown in FIG. 18, in the angular velocity calculation process of this embodiment, if the angular velocity is calculated in S170 or if it is determined in S120 that the object 60 is not a processing target, collision determination processing is performed in S180, and then the angular velocity calculation process is terminated.

[0101] 19, in the collision determination process of S180, first, in S510, it is determined whether or not the direction of the velocity vector of the object 60 estimated in the state estimation process by the state estimation unit 30, more specifically, the traveling direction of the reference point Pr, intersects with the path of the host vehicle 50. Then, if the direction of the velocity vector of the object 60 does not intersect with the path of the host vehicle 50, in S560, it is determined that the host vehicle 50 will not collide with the object 60, that is, a "non-collision," and the collision determination process is terminated.

[0102] Next, if it is determined in S510 that the direction of the velocity vector of the object 60 intersects with the path of the host vehicle 50, the process proceeds to S520. In S520, it is determined whether the time to collision (hereinafter referred to as TTC), calculated by dividing the distance between the host vehicle 50 and the object 60 by the difference in speed (i.e., relative speed), is equal to or less than a preset threshold value for collision determination. TTC is an abbreviation for Time to Collision.

[0103] If it is determined in S520 that the TTC is greater than the threshold, then in S560 it is determined that there is no collision and the collision determination process is terminated. If it is determined in S520 that the TTC is equal to or less than the threshold, then the process proceeds to S530.

[0104] In S530, it is determined whether or not the angular velocity of the object 60 has been calculated in the angular velocity calculation process. If it is determined in S530 that the angular velocity has not been calculated, there is a high possibility that the host vehicle 50 will collide with the object 60, so the process proceeds to S550, a "collision" is determined, and the collision determination process is terminated.

[0105] If it is determined in S530 that the angular velocity has been calculated, the process proceeds to S540, where it is determined whether the absolute value of the calculated angular velocity is equal to or less than a threshold value for collision determination. If the absolute value of the angular velocity is equal to or less than the threshold value, there is a high possibility that the host vehicle 50 will collide with the object 60, so the process proceeds to S550, where a "collision" is determined, and the collision determination process is terminated.

[0106] On the other hand, when it is determined in S540 that the absolute value of the angular velocity is greater than the threshold value, it is determined that the object 60 has changed course and the possibility of the host vehicle 50 colliding with the object 60 has decreased, and the process proceeds to S560, where it is determined that there is no collision, and the collision determination process is terminated.

[0107] In this way, in the estimation device 10 of this embodiment, similar to the estimation device 10 of the first embodiment, not only can it calculate the instantaneous acceleration of the surrounding object 60 and estimate the state of the object 60, but it can also determine whether or not there will be a collision with the object 60 based on the estimation results.

[0108] Furthermore, since the instantaneous angular velocity calculated in the angular velocity calculation process is used for the collision determination, it is possible to prevent an erroneous determination of a "collision" when the instantaneous angular velocity of the object 60 determined by the TTC to have a high collision possibility is large and the possibility of a collision is low. Therefore, it is possible to prevent an erroneous determination of a "collision" from causing an alarm to be output to the occupants of the vehicle 50 or the automatic driving device, and to prevent erroneous control of the steering device of the vehicle 50.

[0109] [Other embodiments] Although the embodiments of the present disclosure have been described above, the present disclosure is not limited to the above-described embodiments and can be implemented in various modified forms.

[0110] In the above embodiment, the state estimation unit 30 is described as modeling the shape of an object and performing state estimation including angular velocity in accordance with the extended object tracking technology described in document A. However, the object state estimation does not necessarily have to be performed according to the procedure described in the above embodiment and can be changed as appropriate. Even in this case, the tracking accuracy of the object can be improved by correcting the estimation results of the angular velocity, etc. by the state estimation unit 30 with the angular velocity calculation value described above.

[0111] In the above embodiment, the estimation device 10 has been described as being mounted on a vehicle 50 such as an automobile, but the estimation device 10 may be mounted on a moving body other than an automobile. For example, the estimation device 10 may be mounted on a moving body such as a ship, an aircraft, a motorcycle, or a drone.

[0112] The estimation device 10 and the method described herein may be implemented by a special-purpose computer configured by configuring a processor and memory programmed to execute one or more functions embodied in a computer program. Alternatively, the estimation device 10 and the method described herein may be implemented by a special-purpose computer configured by configuring a processor with one or more dedicated hardware logic circuits. Alternatively, the estimation device 10 and the method described herein may be implemented by one or more special-purpose computers configured by combining a processor and memory programmed to execute one or more functions with a processor configured with one or more hardware logic circuits. Furthermore, the computer program may be stored as instructions executed by a computer on a computer-readable non-transitory tangible storage medium. The method for implementing the functions of each unit included in the estimation device 10 does not necessarily need to include software; all of the functions may be implemented using one or more hardware devices.

[0113] In the above embodiments, multiple functions of one component may be realized by multiple components, or one function of one component may be realized by multiple components. Furthermore, multiple functions of multiple components may be realized by one component, or one function realized by multiple components may be realized by one component. Furthermore, part of the configuration of the above embodiments may be omitted. Furthermore, at least part of the configuration of the above embodiments may be added to or substituted for the configuration of another of the above embodiments.

[0114] In addition to the above-described estimation device 10, the present disclosure can also be realized in various forms, such as a system including the estimation device 10 as a component, a program for causing a computer to function as the estimation device, a non-transient physical recording medium such as a semiconductor memory on which the program is recorded, and a method for estimating an object.

[0115] [Technical idea disclosed in this specification] [Item 1] An estimation device mounted on a moving body to estimate a state of a surrounding object, a sensor unit (11) configured to transmit sensor waves to the surroundings and detect the positions and relative velocities of a plurality of reflection points on the object from the reflected waves; a velocity vector calculation unit (23) configured to calculate a velocity vector of an arbitrary reference point of the object; a reflection point distance calculation unit (21) configured to calculate, as a reflection point distance, a distance from the reference point to a line having a slope equal to the reflection point azimuth detected by the sensor unit; a rotational relative velocity calculation unit (25) configured to calculate a rotational relative velocity of the reflection point, which is a value obtained by subtracting the relative velocity of the reference point in the reflection point azimuth according to the velocity vector of the reference point calculated by the velocity vector calculation unit from the relative velocity of the reflection point detected by the sensor unit; an angular velocity calculation unit (27) configured to calculate an angular velocity of the object based on the reflection point distance and the rotational relative velocity of at least one of the reflection points calculated by the reflection point distance calculation unit and the rotational relative velocity calculation unit; An estimation device comprising:

[0116] [Item 2] Item 1, an estimation device comprising: The estimation device, wherein the angular velocity calculation unit is configured to calculate the angular velocity based on a magnitude of the relative rotational velocity with respect to the reflection point distance for at least one of the reflection points.

[0117] [Item 3] Item 2. The estimation device according to item 2, The angular velocity calculation unit a regression processing unit (27A) configured to perform linear regression from the distribution of the reflection points in a coordinate space using the reflection point distance and the rotation relative speed as parameters, and to calculate a gradient within the coordinate space; The estimation device is configured to calculate the angular velocity from the slope calculated by the regression processing unit.

[0118] [Item 4] The estimation device according to any one of items 1 to 3, The angular velocity calculation unit performing an exclusion determination on the plurality of reflection points detected by the sensor unit based on at least one of the following determination conditions a) to c); a) The received power of the reflected wave is equal to or less than a threshold value.

[0119] b) The speed difference from the reference point is equal to or greater than a threshold value. c) The reflected waves are multiple waves. The estimation device is configured to exclude the reflection points that are determined to satisfy at least one of the determination conditions a) to c) in the exclusion determination from the calculation targets for the angular velocity.

[0120] [Item 5] The estimation device according to any one of items 1 to 4, The angular velocity calculation unit The estimation device is configured to not calculate the angular velocity or not output the calculation result of the angular velocity when any of the following conditions is met: the distance to the object is less than a predetermined value, the number of reflection points is less than a predetermined value, or the absolute value of the acceleration of the object is greater than or equal to a threshold value.

[0121] [Item 6] The estimation device according to any one of items 1 to 5, a state estimation unit (30) configured to estimate a state of the object corresponding to the plurality of reflection points; the state estimation unit is configured to reflect an angular velocity calculation value, which is the calculation value of the angular velocity calculated by the angular velocity calculation unit, in a state estimation result of the object.

[0122] [Item 7] Item 6. The estimation device according to item 6, the state estimation unit is configured to estimate a state including the angular velocity by a method different from that of the angular velocity calculation unit, and is configured to mix an angular velocity estimation value that is an estimation result of the angular velocity included in a state estimation result of the object with the angular velocity calculation value calculated by the angular velocity calculation unit, to update the angular velocity estimation value.

[0123] [Item 8] Item 6 or Item 7, the estimation device the state estimation unit is configured to estimate a state including the angular velocity by a method different from that of the angular velocity calculation unit, and is configured to increase a filter gain of a filter used to estimate the state of the object by the state estimation unit when an absolute value of the angular velocity calculation value calculated by the angular velocity calculation unit is equal to or greater than a predetermined value.

[0124] [Item 9] The estimation device according to any one of items 6 to 8, the state estimation unit is configured to correct a predicted value of a current velocity vector of the object, the predicted value being calculated based on a past velocity vector of the object estimated by the state estimation unit as one of the states of the object, by the angular velocity calculation value calculated by the angular velocity calculation unit.

[0125] [Item 10] The estimation device according to any one of items 1 to 9, The estimation device includes a collision determination unit (32) configured to determine a collision between the moving body and the object using the angular velocity calculation value calculated by the angular velocity calculation unit. [Explanation of symbols]

[0126] 10...estimation device, 11...sensor unit, 21...reflection point distance calculation unit, 23...velocity vector calculation unit, 25...rotational relative velocity calculation unit, 27...angular velocity calculation unit, 27A...regression processing unit, 30...state estimation unit, 32...collision determination unit.

Claims

1. An estimation device mounted on a moving body to estimate a state of a surrounding object, a sensor unit (11) configured to transmit sensor waves to the surroundings and detect the positions and relative velocities of a plurality of reflection points on the object from the reflected waves; a velocity vector calculation unit (23) configured to calculate a velocity vector of an arbitrary reference point of the object; a reflection point distance calculation unit (21) configured to calculate, as a reflection point distance, a distance from the reference point to a line having a slope equal to the reflection point azimuth detected by the sensor unit; a rotational relative velocity calculation unit (25) configured to calculate a rotational relative velocity of the reflection point, which is a value obtained by subtracting a relative velocity in the reflection point direction according to the velocity vector of the reference point calculated by the velocity vector calculation unit from the relative velocity of the reflection point detected by the sensor unit; an angular velocity calculation unit (27) configured to calculate an angular velocity of the object based on the reflection point distance and the rotational relative velocity of at least one of the reflection points calculated by the reflection point distance calculation unit and the rotational relative velocity calculation unit; An estimation device comprising:

2. The estimation device according to claim 1, The estimation device, wherein the angular velocity calculation unit is configured to calculate the angular velocity based on a magnitude of the relative rotational velocity with respect to the reflection point distance for at least one of the reflection points.

3. 3. The estimation device according to claim 2, The angular velocity calculation unit a regression processing unit (27A) configured to perform linear regression from the distribution of the reflection points in a coordinate space using the reflection point distance and the rotational relative speed as parameters, and to calculate a gradient within the coordinate space; The estimation device is configured to calculate the angular velocity from the slope calculated by the regression processing unit.

4. The estimation device according to any one of claims 1 to 3, The angular velocity calculation unit An exclusion determination is made for the plurality of reflection points detected by the sensor unit based on at least one of the following determination conditions a) to c): a) The received power of the reflected wave is equal to or less than a threshold value. b) The speed difference from the reference point is equal to or greater than a threshold value. c) The reflected wave is a plurality of waves. The estimation device is configured to exclude the reflection points that are determined to satisfy at least one of the determination conditions a) to c) in the exclusion determination from the calculation of the angular velocity.

5. The estimation device according to any one of claims 1 to 3, The angular velocity calculation unit The estimation device is configured to not calculate the angular velocity or not output the calculation result of the angular velocity when any of the following conditions is met: the distance to the object is less than a predetermined value, the number of reflection points is less than a predetermined value, or the absolute value of the acceleration of the object is greater than or equal to a threshold value.

6. The estimation device according to any one of claims 1 to 3, a state estimation unit (30) configured to estimate a state of the object corresponding to the plurality of reflection points; the state estimation unit is configured to reflect an angular velocity calculation value, which is the calculation value of the angular velocity calculated by the angular velocity calculation unit, in a state estimation result of the object.

7. The estimation device according to claim 6, the state estimation unit is configured to estimate a state including the angular velocity by a method different from that of the angular velocity calculation unit, and is configured to mix an angular velocity estimation value that is an estimation result of the angular velocity included in a state estimation result of the object with the angular velocity calculation value calculated by the angular velocity calculation unit, to update the angular velocity estimation value.

8. The estimation device according to claim 6, the state estimation unit is configured to estimate a state including the angular velocity by a method different from that of the angular velocity calculation unit, and is configured to increase a filter gain of a filter used to estimate the state of the object by the state estimation unit when an absolute value of the angular velocity calculation value calculated by the angular velocity calculation unit is equal to or greater than a predetermined value.

9. The estimation device according to claim 6, the state estimation unit is configured to correct a predicted value of a current velocity vector of the object, the predicted value being calculated based on a past velocity vector of the object estimated by the state estimation unit as one of the states of the object, with the calculated angular velocity value calculated by the angular velocity calculation unit.

10. The estimation device according to any one of claims 1 to 3, The estimation device includes a collision determination unit (32) configured to determine a collision between the moving body and the object using the angular velocity calculation value calculated by the angular velocity calculation unit.

Images