JP2009288255A - Estimating device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To accurately estimate shape and motion state of a preceding vehicle. <P>SOLUTION: The estimating device measures the position coordinates of a plurality of points of the preceding vehicle surface with a radar wave S110, and approximates the plurality of points by a single line and two lines S120, S1300. For each line, a prior probability where the line is a line for approximating the front, rear and side surfaces of the vehicle is calculated based on the determined line length and approximation error S1400. Further, at the same time, likelihood is calculated based on the information on the position, direction, speed, traveling direction and angular velocity of the preceding vehicle estimated in the past S1500. For each line, a posterior probability where the line is a line for approximating the front, rear and side surfaces of the vehicle is calculated based on the determined prior probability and likelihood by Bayes estimation S1600, and the size, position, and direction of the preceding vehicle are estimated S1700. The speed, traveling direction, and angular velocity of the vehicle are together estimated with an unscented Kalman filter based on the information S1800. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to an estimation device that estimates the state of a vehicle located ahead.

  Conventionally, as a device for estimating the state of a vehicle, a radar device that estimates the state of a vehicle at a distant position using a radar wave is known. As this type of radar device, for example, on a straight road Is installed and periodically irradiates a millimeter wave as a radar wave toward a predetermined range (see, for example, Patent Document 1).

  This radar apparatus captures a reflected wave returned from a radar wave reflected by the vehicle, and estimates the state of the vehicle according to the reception result of the reflected wave. Specifically, assuming that the vehicle is moving linearly, the vehicle speed and acceleration are obtained using a Kalman filter designed based on a linear motion model, and the vehicle motion state is predicted. According to this radar apparatus, the current position of the vehicle traveling on the road can be known, and if the vehicle on the road is in motion, the future motion state of the vehicle can be predicted.

  Therefore, by using this radar device, it is possible to improve the accuracy of determining whether or not a vehicle or the like is stopped on the road, the accuracy of calculating the vehicle speed, etc., using the observed value and the predicted value in the future. A process for predicting a collision between vehicles and avoiding this can be executed.

JP 2002-99986 A

Conventionally, however, there are the following two problems.
The first problem is that there is no technology to accurately detect the size and direction of the vehicle located in the front, so that information such as the size and direction of the vehicle located in the front is obtained to a degree necessary and sufficient for collision avoidance. It is a problem that cannot be done. If the shape of the vehicle is not known, it is difficult to accurately predict a collision. For this reason, in the prior art, there has been an accuracy limit regarding the execution of vehicle control for collision avoidance based on the detection result of the radar device.

  The second problem is that there is no technology for predicting the movement of the vehicle when turning. In other words, in the prior art, when predicting the future state of the vehicle, it is assumed that the vehicle is moving linearly, and the vehicle motion is predicted by the Kalman filter based on the linear motion model. In some cases, the vehicle motion cannot be predicted by accurately capturing the turning motion.

  The present invention has been made in view of these problems, and an object of the present invention is to provide an estimation device that can accurately estimate the state of a vehicle.

  The estimation apparatus according to claim 1, which has been made to achieve such an object, emits a radar wave, receives a reflected wave that is reflected back by a vehicle positioned in front of the emitted wave, and receives a radar wave. Positioning means for measuring the position coordinates of a plurality of points on the vehicle surface reflected from the vehicle, and according to the positioning result, an estimation device for estimating the motion state of the vehicle located in front of the vehicle surface, the position of the vehicle surface measured by the positioning means Based on the position coordinates of a plurality of points, the motion state of the vehicle including the angular velocity of the vehicle is estimated by the estimating means according to a predetermined nonlinear motion model.

  Conventionally, since the motion state of the vehicle was estimated using a linear motion model, there was a problem that the motion state of the vehicle could not be accurately estimated when the vehicle was turning, but with this device, Since the motion state of the vehicle including the angular velocity of the vehicle is estimated based on the nonlinear motion model, the turning motion of the vehicle can be accurately captured. Therefore, this estimation apparatus is very useful when performing vehicle control related to vehicle collision avoidance.

  The estimating means is configured to estimate at least one of the position of the vehicle, the direction of the vehicle, the speed of the vehicle, and the traveling direction of the vehicle, in addition to the angular velocity of the vehicle, as the motion state of the vehicle. (Claim 2). Thus, if the estimation device is configured, the turning motion of the vehicle can be known in detail, which is very useful when performing vehicle control related to collision avoidance.

  According to a third aspect of the present invention, there is provided an estimating apparatus in which the estimating means estimates the state of the vehicle using a Kalman filter employing a nonlinear motion model. If the vehicle motion state is estimated using the Kalman filter, there is an advantage that the motion state of the vehicle can be estimated with high accuracy while suppressing the influence of the observation error.

  In addition, the estimation device according to claim 4 is characterized in that the Kalman filter is an unscented Kalman filter. The unscented Kalman filter is very good for estimating the motion state of nonlinear motion. Therefore, if the estimation device is configured to estimate the motion state of the vehicle using the unscented Kalman filter, the motion state can be estimated with high accuracy.

  Moreover, as a nonlinear motion model employ | adopted as the said estimation apparatus, a constant velocity circular motion model can be mentioned (Claim 5). If the Kalman filter is designed by adopting a constant velocity circular motion model and the above estimation device is configured to estimate the motion state of the vehicle based on the Kalman filter, a simple motion model can be used to accurately turn the vehicle. Can be estimated.

  Further, when the previously estimated angular velocity is zero, the estimation device according to claim 6 switches the motion model used for estimating the vehicle state from the nonlinear motion model to a predetermined linear motion model, and performs the linear motion. According to the model, the vehicle state is estimated. By configuring the estimation device in this way, it is possible to accurately estimate not only the turning motion but also the motion state of the straight motion, and it is possible to estimate a wide range of vehicle motion with high accuracy.

2 is a block diagram illustrating a configuration of an estimation device 1. FIG. It is a flowchart showing the vehicle state estimation process which CPU25a performs. It is explanatory drawing which showed the measuring principle of the distance by a radar wave, and the calculation method of a position coordinate. It is explanatory drawing showing the specific example about distribution of a reflective point. It is a flowchart showing the two-segment calculation process which CPU25a performs. It is a figure showing the specific example of a two-segment calculation process. It is a figure showing the specific example of the approximation by a single line segment and two line segments with respect to a reflective point. It is a flowchart showing the prior probability calculation process which CPU25a performs. It is a figure showing the specific example of the function data for prior probability calculation. It is a figure showing the flowchart of likelihood calculation processing. It is a figure showing the specific example of the function data for likelihood calculation. It is a flowchart showing the individual likelihood calculation process which CPU25a performs. It is a figure showing the flowchart showing the posterior probability calculation process which CPU25a performs. It is a figure showing the flowchart showing the selection process which CPU25a performs. It is explanatory drawing showing the method of the calculation using a Kalman filter. It is explanatory drawing which showed the calculation method of the position coordinate of the center point of a vehicle. It is a flowchart showing the state estimation process which CPU25a performs.

Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram showing the configuration of an estimation apparatus 1 to which the present invention is applied, and is mounted on the front part of a four-wheeled vehicle.
As shown in FIG. 1, the estimation apparatus 1 of the present embodiment drives a laser diode 11 that emits laser light, a collimator lens 12 that collimates the laser light emitted from the laser diode, and the laser diode 11. The laser diode drive circuit 13, a mirror 15 that reflects the laser light emitted from the laser diode 11, a motor 17 that rotates the mirror 15, a condenser lens 19 that condenses the laser light, and a condenser lens 19 collect the laser light. A photodiode 21 that receives the emitted laser light and generates an electrical signal; an amplifier 22 that amplifies the electrical signal from the photodiode 21; a laser diode drive signal and a signal output from the amplifier 22; The laser light emitted from the diode 11 is reflected and detected by the photodiode 21. The detection circuit 23 for detecting the time until the laser beam is output, the laser diode drive signal for the laser diode drive circuit 13 and the motor drive signal for driving the motor 17 are output, and laser light is emitted and the motor 17 is rotated. The control unit 25 is configured to perform various processes based on the information on the motor rotation position input from the motor 17 and the information on the flight time ΔT of the laser beam input from the detection circuit 23.

  The laser diode 11 is a device for emitting laser light as a radar wave for detecting a vehicle ahead. Since laser light has high directivity, according to the present apparatus, high spatial resolution can be obtained. The collimator lens 12 refracts the laser light emitted from the laser diode 11 so as to be a parallel light beam.

  The mirror 15 is configured as a polygon mirror having a hexagonal reflecting surface for reflecting light, and the motor 17 is configured to rotate the mirror 15 in accordance with a motor drive signal from the CPU 25a. With this configuration, the estimation apparatus 1 of the present embodiment can emit the laser light that has passed through the collimator lens 12 in an arbitrary direction.

  The control unit 25 includes a CPU 25a, a ROM 25b, a RAM 25c, and the like. The CPU 25a executes various programs stored in the ROM 25b, thereby controlling the laser diode 11 and the motor 17, and information on the motor rotation position. Based on the information on the flight time ΔT of the laser beam, the state of the object reflecting the laser beam is estimated by the program stored in the ROM 25b.

  FIG. 2 is a flowchart showing a vehicle state estimation process that is repeatedly executed mainly by the CPU 25a. In this apparatus, the forward object detected by the laser beam is regarded as a four-wheeled vehicle and the subsequent processing is performed.

  When the vehicle state estimation process is started, the CPU 25a emits laser light, positions the reflection point of the laser light, and obtains its position coordinates (S110). Specifically, the laser radar drive circuit 13 and the motor 17 are driven to rotate the mirror 15 and emit pulsed light intermittently from the laser diode 11. By controlling the laser radar drive circuit 13 and the motor 17 as described above, the laser beam is emitted toward the vehicle positioned ahead while changing the irradiation angle in the horizontal direction.

  The laser light reflected by the front object passes through the condenser lens 19 and is condensed on the photodiode 21. The condensed laser light is converted into an electrical signal by the photodiode 21, amplified by the amplifier 22, and then sent to the detection circuit 23.

  The detection circuit 23 is configured to be able to acquire a laser diode drive signal as a drive signal of the laser diode drive circuit 13 output from the CPU 25a. From the input timing of this drive signal and the light reception timing of the reflected light, As shown in FIG. 3 (a), a time from laser light emission to light reception (delay time: ΔT) is detected as the flight time ΔT of the laser light.

  The CPU 25a obtains the distance D to the reflection point as D = v · ΔT / 2 by using the speed v of the laser beam based on the delay time ΔT input from the detection circuit 23 every time pulse light is emitted. . Then, from the calculated distance D and the emission angle Φ of the pulsed light that causes the distance D to be measured, (x, y) = (D sin φ, D cos φ) as the position coordinates (x, y) of the reflection point. ) The xy coordinate system used here is a relative coordinate system based on the estimation device 1 fixed to the vehicle. Hereinafter, this coordinate system is expressed as “coordinate system A”.

  The emission angle Φ of the pulsed light can be uniquely obtained from the motor rotation position. Here, as shown in FIG. 3B, assuming that the estimation device 1 is installed on the longitudinal axis of the vehicle, the y-axis is set forward from the axis, and the y-axis is used as a reference. The angle is defined as φ, and the position coordinates (x, y) = (Dsinφ, Dcosφ) of the reflection point are obtained.

  FIG. 4 shows a specific example of the reflection point from which the position coordinates (x, y) can be obtained by this positioning. FIG. 4A is a diagram illustrating the distribution of reflection points when the front vehicle 50 is tilted with respect to the estimation device 1. FIG. 4B is a diagram illustrating the rear surface 50 a of the front vehicle 50 with respect to the estimation device 1. It is a figure showing distribution of the reflective point in the case of facing directly. Even when the vehicle side surface 50b or the front surface 50c faces the estimation device 1, the distribution of reflection points is as shown in FIG.

  When the position coordinates of a plurality of points on the front object surface are obtained in S110, the CPU 25a next displays the contour pattern of the front object represented by the position coordinates (position coordinates of each reflection point) of the plurality of points obtained in S110. Are fitted with a straight line using the least squares method. Then, by obtaining the end point of the approximate straight line, the contour pattern of the front object is approximated by a single line segment. Hereinafter, this approximate line segment is referred to as a line segment L1. When the contour pattern of the front object is approximated by a single line segment, the length, the position coordinates of the midpoint, and the approximation error are obtained for the obtained line segment L1 (S120). The approximate error here is the sum of the distances between the obtained line segment L1 and each reflection point constituting the contour pattern.

  However, if the position coordinates of four or more points cannot be obtained as the position coordinates of the reflection point in S110, the CPU 25a once ends the vehicle state estimation process without moving to S120, and again, By executing the process of S110, positioning is performed again.

  On the other hand, when the process in S120 is finished, the CPU 25a executes an L-shaped two-line segment calculation process (S1300). In other words, the contour pattern of the front object represented by a plurality of reflection points is fitted with two L-shaped straight lines orthogonal to each other using the least square method.

  Then, by determining the end points of each straight line, the contour pattern of the front object is an L-shaped figure pattern formed by connecting a pair of orthogonal line segments that model the front and rear surfaces and side surfaces of the vehicle at the end points. Approximate. In the coordinate system A, the length of each line segment constituting the approximate figure pattern, the direction of each line segment (the angle formed by the x axis of the coordinate system A and the line segment), and the position of the midpoint of each line segment Find the coordinates.

FIG. 5 is a flowchart showing the L-shaped two-line segment calculation process executed by the CPU 25a in S1300.
When the two-line segment calculation process is started, the CPU 25a sets the number of reflection points obtained in S110 to n (n> 3), and assigns a number N to the reflection points in descending order of the angle φ (S1310). Specifically, in S1310, a process of associating the number N with the position coordinates of the reflection point held as data is performed.

  The method of assigning the number N here is as shown in FIG. That is, the numbers N are assigned sequentially in the order of the reflection points with the larger angles φ, such as N = 1, 2, 3,..., N−1, n. For example, N = 1 is assigned to the first point, and N = 2 is assigned to the second point.

  In addition, when the processing in S1310 is completed in this way, the CPU 25a sets the value of the variable m to 2 (m = 2) (S1320). Thereafter, the reflection point assigned the number from N = 1 to N = m is set as the segment SEG1, and the reflection point assigned the number from N = m + 1 to N = n is set as the segment SEG2 (S1330).

  When the processing in S1330 is completed, the CPU 25a approximates the group of reflection points belonging to the segment SEG1 with a straight line by the least square method, calculates the approximate line L21 of the group of reflection points belonging to the segment SEG1, and also calculates the segment. The distance from each reflection point belonging to SEG1 to the approximate straight line L21 is obtained, and the sum of the distances from each reflection point to the approximate straight line L21 is obtained as an approximation error (S1340).

  FIG. 6B is an explanatory diagram showing how to obtain the distance between the approximate straight line L21 and the reflection point. FIG. 6B shows an approximate straight line L21 together with a specific example of the distribution of reflection points belonging to the segment SEG1. In S1340, for example, when the reflection points belonging to the segment SEG1 have the distribution shown in FIG. 6B, the distance between the approximate straight line L21 indicated by the arrow and each reflection point in the drawing is obtained. Then, the sum is obtained as an approximation error of the segment SEG1.

  When the processing of S1340 is completed, the CPU 25a approximates the group of reflection points belonging to the segment SEG2 by a straight line orthogonal to the straight line obtained in S1340 by the least square method, and approximates the group of reflection points belonging to the segment SEG2. In addition to calculating the straight line L22, the distance from each reflection point belonging to the segment SEG2 to the calculated approximate straight line L22 is obtained, and the sum of the distances from each reflection point to the approximate straight line L22 is obtained as an approximation error (S1350).

  FIG. 6A is an explanatory diagram showing how to obtain approximate straight lines L21 and L22 when n = 12 and m = 6. As shown in FIG. 6A, in this embodiment, first, an approximate straight line is obtained by the least square method for the first to sixth reflection points which are the reflection points of the segment SEG1. Assuming that the approximate straight line obtained here is y = ax + b, the straight line approximated with respect to the reflection point group of the segment SEG2 is y = − (1 / a) x + c because the condition is orthogonal to the segment SEG1. . Accordingly, parameters a and b are calculated in S1340, and parameter c is calculated in S1350.

  Then, by calculating the position coordinates of the end points of the calculated pair of approximate lines L21 and L22, the approximate line is segmented and extracted from the approximate lines L21 and L22 as an approximate figure pattern corresponding to the contour pattern of the front object. An L-shaped figure pattern formed by connecting line segments is obtained. Then, the length and direction of each line segment constituting the approximate figure pattern and the position coordinates of the midpoint are calculated by the coordinate system A (S1360).

  The method for obtaining the position coordinates of the end points is as follows. First, the position coordinates of the intersection of the straight lines L21 and L22 are obtained as the position coordinates of the end points of the orthogonal straight lines L21 and L22. Then, as shown in FIG. 6B, the position coordinates of the intersection of the perpendicular line from the reflection point assigned N = 1 to the straight line L21 and the straight line L21 are obtained as the position coordinates of the other end point of the straight line L21. . Similarly, the position coordinate of the intersection of the perpendicular line from the reflection point to which N = n is assigned to the straight line L22 and the straight line L22 is obtained as the position coordinate of the other end point of the straight line L22. In this way, in the present embodiment, the position coordinates of the end points are obtained, the approximate lines L21 and L22 are segmented, and the length, direction, and midpoint position coordinates of each line segment are calculated (S1360). .

  When the process in S1360 is finished, it is determined whether m = n−2 (S1370). If it is determined that m = n−2 is not satisfied (S1370: N), the process proceeds to S1380, 1 is added to m (m ← m + 1), and then the process returns to S1330.

  On the other hand, if it is determined that m = n−2 (S1370: Y), the approximate error of the segment SEG1 and the approximate error of the segment SEG2 are calculated from the n−3 pairs of line segments L21 and L22 calculated so far. A pair of line segments L21 and L22 having the smallest sum of the two is selected, and an L-shaped figure pattern composed of the selected pair of line segments L21 and L22 is applied to the reflection point group (contour pattern of the front object) measured in S110. Finally, an L-shaped approximate graphic pattern is determined (S1390). Thereafter, the two-line segment calculation process ends.

  In this way, in this embodiment, the contour pattern of the front object represented by the reflection point group measured in S110, the graphic pattern composed of a single line segment modeling one surface of the vehicle, and the front and rear of the vehicle Approximating with a L-shaped figure pattern formed by connecting a pair of orthogonal line segments that model the face and the side face at the end points, and the approximate figure pattern (specifically, the length and direction of each line segment) , And midpoint position coordinates).

  FIG. 7 is a diagram illustrating specific examples of the approximate figure pattern (line segment L1) obtained in S120 and the L-shaped approximate figure pattern (line segments L21 and L22) obtained in S1300. .

  Specifically, FIG. 7A shows the left figure of the approximate graphic pattern (line segment L1) obtained in S120 when the distribution of reflection points shown in FIG. 4A is obtained as a result of positioning. 7 (a) shows the L-shaped approximate figure pattern (S1300) obtained in S1300 when the distribution of the reflection points shown in FIG. 4 (a) is obtained as a result of positioning. This represents the line segments L21, L22).

  7B shows an approximate figure pattern (line segment L1) obtained by S120 when the reflection point distribution shown in FIG. 4B is obtained as a result of positioning. FIG. 7B shows the L-shaped approximate figure pattern (line segment) obtained in S1300 when the distribution of reflection points shown in FIG. 4B is obtained as a result of positioning. L21, L22).

  When the two-segment calculation processing in S1300 is finished, the CPU 25a proceeds to S1400 and executes the prior probability calculation processing shown in FIG. FIG. 8 is a flowchart showing a prior probability calculation process executed by the CPU 25a.

  In this embodiment, each line segment L1, L21, and L22 obtained in S120 and S1300 using the Bayesian estimation method is a line segment that approximates the vehicle front-rear surface and a line segment that approximates the vehicle side surface. A certain probability and a probability that is a line segment that approximates the side surface or a line segment that does not approximate the front and back surfaces (hereinafter referred to as other line segments) are obtained. In S1400, the probability is determined. The prior probability that each of the line segments L1, L21, and L22 used for the input of Bayesian estimation is a line segment that approximates the vehicle front-rear surface, the prior probability that is the line segment that approximates the vehicle side surface, and other line segments Is calculated according to function data (such as a map) recorded in the ROM 25b.

  Therefore, before describing the prior probability calculation process with reference to FIG. 8, here, the configuration of the function data used in the prior probability calculation process will be described with reference to FIG. FIG. 9 is a diagram visualizing the structure of the function data recorded in the ROM 25b as a graph.

  Specifically, in FIG. 9A, the line segment length is input, the probability that the input line segment is a line segment approximating the vehicle front-rear surface, and the input line segment of the above length is A graph of the structure of function data that outputs the probability of a line segment approximating the vehicle side surface and the probability that the input line segment is another line segment as a graph. is there.

  FIG. 9B shows an input of the approximation error of the line segment, the probability that the input line segment having the approximation error is a line segment approximating the vehicle front-rear surface, and the input line segment having the approximation error. Is a graphical representation of the structure of the function data that outputs the probability that is a line segment approximating the vehicle side surface and the probability that the input line segment having the above approximation error is another line segment. It is.

  The graph shown in FIG. 9A is specifically a graph in which the vertical axis is probability and the horizontal axis is length, and in the graph shown in FIG. 9B, the vertical axis is probability and the horizontal axis is a line segment. It is a graph of an approximation error. Each of FIGS. 9A and 9B represents a change in the probability that the dotted line is “side face”, the solid line is “front and back face”, and the alternate long and short dash line is “other”.

  The function data can be designed freely by the designer, but since the vertical axis is the probability, the function data shown in FIG. 9A and the function data shown in FIG. The probability of being a line segment approximating a surface, the probability of being a line segment approximating a vehicle side surface, and the sum of the probabilities of other line segments are designed to be constant. However, it is not necessary to set the sum to 1 here. This is because even if the sum is not set to 1, if the sum is made constant, an operation similar to the narrowly defined probability that the sum is set to 1 can be performed in Bayesian estimation described later by adding a normalization process. .

  As shown in FIG. 9A, in this embodiment, when the line segment length is short in consideration of the shape of the vehicle, there is a high probability that the line segment is a line segment approximating the vehicle front-rear surface. Thus, when the line segment length is long, the function data is designed so that the probability that the line segment is a line segment approximating the vehicle side surface is high. In addition, when the line segment length is longer than the total length of the vehicle that is normally considered, the function data is designed so that the probability that the line segment is another line segment is high.

  Specifically, the probability that the line segment is a line segment that approximates the vehicle front-rear surface is set to reach a peak when the line segment length becomes a length corresponding to the full width of a general vehicle. The probability that the line segment is a line segment that approximates the side surface of the vehicle is set to reach a peak when the line segment length becomes a length corresponding to the overall length of a general vehicle.

  In addition, as shown in FIG. 9B, in this embodiment, the smaller the approximation error, the more likely the corresponding line segment is to approximate the vehicle front and back planes, and the line segment that approximates the vehicle side surface. The function data is designed so that the probability increases and the probability that the corresponding line segment is another line segment increases as the approximation error increases.

  The CPU 25a of this embodiment reads such function data from the ROM 25b, and for each line segment L1, L21, L22, the line segment approximates the vehicle front-rear surface based on the line segment length and approximation error. A prior probability, a prior probability that the line segment approximates the vehicle side surface, and an a priori probability that the line segment is another line segment.

Next, the contents of the prior probability calculation process executed by the CPU 25a in S1400 will be described with reference to FIG.
When the prior probability calculation process is started, the CPU 25a first sets a line segment (hereinafter referred to as “target line segment”) to be calculated later as the line segment L1 obtained in S120 (S1405). Further, the vehicle part (hereinafter referred to as “target part”) to be calculated thereafter is set as “side face” (S1410).

  Then, using the function data shown in FIG. 9A, based on the length of the target line segment, a probability P (1415) that the target line segment is a line segment approximating the target part is derived (S1415). Next, using the function data shown in FIG. 9B, based on the approximation error of the target line segment, a probability P (1420) that the target line segment is a line segment approximating the target part is derived (S1420). . Then, the probability P (1425) = P (1415) × P (1420) is calculated (S1425).

  Next, it is determined whether or not the probability P (1425) having the “front and back plane” as the target region is derived for the current target line segment (S1430), and the “front and back plane” is still targeted for the current target line segment. If the probability P (1425) as a part has not been derived (S1430: N), the target part is changed to “front-rear surface” (S1435), and the process returns to S1415.

  On the other hand, if the probability P (1425) with the “front and back plane” as the target part has already been derived (S1430: Y), the probability P (1425) with “other” as the target part is obtained for the current target segment. It is determined whether or not it has been derived (S1440). If the probability P (1425) having “other” as the target part has not yet been derived for the current target line segment (S1440: N), the target part is determined as “ Change to “Other” (S1445), and return to S1415. If the target part is “other”, the probability that the target line segment is another line segment is obtained.

  On the other hand, if the probability P (1425) having “other” as the target part has already been derived (S1440: Y), the probability P (1425) having “side” as the target part for the current target line segment. , Normalization is performed so that the sum of the probability P (1425) with “front and back plane” as the target part and the probability P (1425) with “other” as the target part is 1, and the target line segment is the vehicle. Prior probability P (1425) which is a line segment approximating “front and back planes”, prior probability P (1425) where the target line segment is a line segment approximating the vehicle “side surface”, and the target line segment is another line segment Prior probability P (1425) is obtained (S1450).

  That is, in this embodiment, the normalized value of the probability P (1425) with the “front and back plane” thus obtained as the target portion is represented by a line segment that approximates the vehicle “front and back plane”. A certain prior probability P (1425) is handled in subsequent processing. Similarly, the normalized value of the probability P (1425) with “side” as the target part is treated as a prior probability P (1425) in which the target line is a line approximating the vehicle “side”. Is treated as a prior probability P (1425) in which the target line segment is another line segment.

  When the processing in S1450 is completed, the CPU 25a sets the line segment L21 as the target line segment and determines whether or not the probability P (1425) has been obtained (S1455), and the probability P for the line segment L21 still remains. If (1425) is not obtained (S1455: N), the target line segment is changed to the line segment L21 (S1460), and the process returns to S1410.

  On the other hand, if the probability P (1425) of the line segment L21 has already been obtained (S1455: Y), it is determined whether or not the probability P (1425) has been obtained by setting the line segment L22 as the target line segment (S1465). ) If the probability P (1425) for the line segment L22 has not yet been obtained (S1465: N), the target line segment is changed to the line segment L22 (S1470), and the process returns to S1410.

If the probability P (1425) of the line segment L22 has already been obtained (S1465: Y), the prior probability calculation process ends.
Hereinafter, the prior probability P (1425) in which the line segment L1 obtained in this way by the prior probability calculation process is a line segment that approximates the vehicle side surface will be referred to as prior probability P (1 side). Also, the prior probability P (1425) in which the line segment L1 is a line segment that approximates the vehicle front-rear surface is referred to as prior probability P (around 1), and the prior probability P (1425) in which the line segment L1 is another line segment. ) Is expressed as prior probability P (1 other).

  Similarly, for the line segments L21 and L22, the prior probability P (1425) in which the line segment L21 is a line segment approximating the vehicle side surface is expressed as the prior probability P (21 side), and the line segment L21 is the front and rear of the vehicle. Prior probability P (1425) that is a line segment approximating the surface is expressed as prior probability P (around 21), and prior probability P (1425) in which line segment L21 is another line segment is referred to as prior probability P (21 Other)), the prior probability P (1425) in which the line segment L22 approximates the vehicle side surface is expressed as the prior probability P (22 side), and the line segment L22 approximates the vehicle front-rear surface. The prior probability P (1425) is expressed as a prior probability P (around 22), and the prior probability P (1425) in which the line segment L22 is another line segment is expressed as a prior probability P (22 other).

  When the prior probability calculation process is finished in S1400, the CPU 25a proceeds to S1500 and executes the likelihood calculation process shown in FIG. FIG. 10 is a flowchart illustrating likelihood calculation processing executed by the CPU 25a.

  In the present embodiment, in this likelihood calculation process, prior probabilities P (1 side), P (around 1), P (1 other) P (21 side), P (around 21) used for input of Bayesian estimation ), P (21 other), P (22 side), P (22 around), P (22 other) likelihood L (1 side), L (1 around), L (1 other), L ( 21 side), L (around 21), L (21 other), L (22 side), L (around 22), L (22 other) are obtained. In this case, the likelihood recorded in the ROM 25b is derived. Use function data for. Therefore, prior to describing the likelihood calculation process, the configuration of function data for derivation of likelihood recorded in the ROM 25b will be described with reference to FIG.

  FIG. 11 is a diagram showing the structure of the function data recorded in the ROM 25b for derivation of the likelihood, visualized by a graph. All graphs shown in FIGS. 11A to 11F have likelihoods set on the vertical axis, with the dotted line indicating “side surface”, the solid line indicating “front and back surfaces”, and the alternate long and short dash line indicating “other”. It represents the change of. This function data can be freely designed by the designer.

  Specifically, in FIG. 11A, the distance between the midpoint position coordinates of the line segment and the predicted position (details will be described later) of the vehicle side surface is input, and the line segment corresponding to the input distance approximates the vehicle front-rear surface. The likelihood that the line segment corresponding to the input distance is a line segment approximating the vehicle side surface, and the line segment corresponding to the input distance is another line segment. It is the figure which visualized and represented the structure of the function data which output likelihood as a graph. As shown in FIG. 11A, the function data is designed so that the likelihood of being a line segment approximating the vehicle side surface increases as the distance of the line segment decreases.

  FIG. 11B is a line segment in which the distance between the midpoint position coordinates of the line segment and the predicted position of the vehicle front-rear surface is input, and the line segment corresponding to the input distance approximates the vehicle front-rear surface. The likelihood, the likelihood that the line segment corresponding to the input distance is a line segment approximating the vehicle side surface, and the likelihood that the line segment corresponding to the input distance is another line segment are output. It is the figure which visualized and represented the structure of the function data to perform as a graph. As shown in FIG. 11B, the function data is designed so that the likelihood of being a line segment that approximates the vehicle front-rear surface increases as the distance decreases.

  FIG. 11C shows the difference (however, absolute value) between the line segment length and the predicted value of the vehicle side length (vehicle total length) as an input, and the line segment corresponding to the input difference is the vehicle front-rear surface. , The likelihood that the line segment corresponding to the input difference is a line segment approximating the vehicle side surface, and the line segment corresponding to the input difference is the other line segment It is the figure which visualized and represented the structure of the function data which output the likelihood which is as a graph. As shown in FIG. 11C, the function data is designed such that the likelihood that the line segment approximates the vehicle side surface increases as the difference decreases.

  In addition, in FIG. 11D, the difference (however, absolute value) between the line segment length and the predicted value of the vehicle front-rear surface length (vehicle total width) is input, and the line segment corresponding to the input difference is the vehicle. Likelihood that is a line segment that approximates the front and back planes, likelihood that the line segment that corresponds to the input difference is a line segment that approximates the vehicle side surface, and the line segment that corresponds to the input difference is other It is the figure which visualized and represented the structure of the function data which output the likelihood which is a line segment as a graph. This function data is designed such that the likelihood of a line segment that approximates the vehicle front-rear plane increases as the difference decreases.

  Further, FIG. 11E shows the likelihood that the difference between the direction of the line segment and the predicted value of the vehicle direction is input, and the line segment corresponding to the input difference is a line segment that approximates the vehicle front-rear surface, Function data that outputs the likelihood that the line segment corresponding to the input difference is a line segment approximating the vehicle side surface, and the likelihood that the line segment corresponding to the input difference is another line segment. It is the figure which visualized and represented this structure as a graph. This function data is designed so that the likelihood that the line segment approximates the vehicle front-and-rear plane is increased as the difference is closer to 90 degrees, and the difference is closer to 0 degrees or 180 degrees. It is designed so that the likelihood that is a line segment approximating the vehicle side surface is increased.

  FIG. 11F shows the likelihood that the difference between the direction of the line segment and the predicted value of the traveling direction of the vehicle is input, and the line segment corresponding to the input difference is a line segment that approximates the vehicle front-rear surface. A function that outputs a likelihood that the line segment corresponding to the input difference is a line segment approximating the vehicle side surface, and a likelihood that the line segment corresponding to the input difference is another line segment. It is the figure which visualized and represented the structure of data as a graph. This function data is designed so that the likelihood that the line segment approximates the vehicle front-and-rear plane is increased as the difference is closer to 90 degrees, and the difference is closer to 0 degrees or 180 degrees. It is designed so that the likelihood that is a line segment approximating the vehicle side surface is increased.

  Next, the likelihood calculation process executed by the CPU 25a in S1500 will be described using FIG. When the likelihood calculation process is started, the CPU 25a first performs an operation based on an input from a sensor group connected to the estimation device 1 for detecting the motion state of the host vehicle with respect to the previous execution of the vehicle state estimation process. A displacement amount (δx, δy, δθ) of the host vehicle is obtained (S1505). δx is the origin of the current coordinate system A in the x direction of the coordinate system A when the previous vehicle state estimation process is executed with reference to the origin position of the coordinate system A when the previous vehicle state estimation process is executed (when S1505 is executed). It is an evaluation of how much the position has been moved, and δy is a coordinate at the time of executing the previous vehicle state estimation process on the basis of the origin position of the coordinate system A at the time of execution of the previous vehicle state estimation process (when executing S1505). This is an evaluation of how much the origin of the current coordinate system A is moved in the y direction of the system A, and δθ is the time when the y axis of the current coordinate system A is executed in the previous vehicle state estimation process. This is an evaluation of how much the position is rotated with respect to the y-axis of the coordinate system A (when executing S1505) using the coordinate system A when executing the previous vehicle state estimation process (when executing S1505).

  When this process is finished, the CPU 25a sets the target line segment to the line segment L1 (S1510). Further, the target part is set to “side” (S1515). Next, the likelihood L (3000) that the target line segment is a line segment that approximates the target part is obtained by the individual likelihood calculation process shown in FIG. 12 (S3000). A detailed description of the individual likelihood calculation process using FIG. 12 will be described later.

  When the individual likelihood calculation process is completed, the CPU 25a next determines whether or not the likelihood L (3000) having the “front and back plane” as the target part for the current target line segment has been obtained (S1530). If it is determined that the likelihood L (3000) with “front and back planes” as the target part is not obtained (S1530: N), the target part is changed to “front and back planes” (S1535), and the process returns to S3000. The individual likelihood calculation process is executed for the part.

  On the other hand, for the current target line segment, if it is determined that the likelihood L (3000) having “front and back planes” as the target site is obtained (S1530: Y), “other” is set as the target site for the current target line segment. It is determined whether or not likelihood L (3000) has been obtained (S1540).

  If it is determined that the likelihood L (3000) with “other” as the target part is not obtained (S1540: N), the target part is changed to “other” (S1545), and the process returns to S3000. Then, an individual likelihood calculation process is executed for this target part. When the target part is “other”, the likelihood L (3000) that the target line segment is another line segment is obtained by the individual likelihood calculation process.

  If it is determined that the likelihood L (3000) with “others” as the target part has been obtained (S1540: Y), the CPU 25a sets the line segment L21 as the target line segment and obtains the likelihood L (3000). If the likelihood L (3000) is not obtained with the line segment L21 as the target line segment (S1555: N), the target line segment is changed to the line segment L21 (S1560). The process returns to S1515.

  On the other hand, if the likelihood L (3000) has already been obtained with the line segment L21 as the target line segment (S1555: Y), it is determined whether or not the likelihood L (3000) has been obtained with the line segment L22 as the target line segment. (S1565). If the likelihood L (3000) is not obtained with the line segment L22 as the target line segment (S1565: N), the target line segment is changed to the line segment L22 (S1570), and the process returns to S1515. On the other hand, if the likelihood L (3000) is obtained with the line segment L22 as the target line segment (S1565: Y), the likelihood calculation process is terminated.

  In the following, the likelihood L (3000) in which the line segment L1 obtained in this way by the likelihood calculation process is a line segment approximating the vehicle side surface is denoted as likelihood L (1 side). Likelihood L (3000) in which line segment L1 is a line segment approximating the vehicle front-rear surface is denoted as likelihood L (around 1), and likelihood L (3000) in which line segment L1 is another line segment. ) Is expressed as likelihood L (1 other).

  Similarly, for the line segments L21 and L22, the likelihood L (3000), which is a line segment that approximates the vehicle side surface, is represented as the likelihood L (21 side), and the line segment L21 is the front and rear of the vehicle. Likelihood L (3000), which is a line segment approximating a surface, is expressed as likelihood L (around 21), and likelihood L (3000), where line segment L21 is another line segment, is represented by likelihood L (21 The likelihood L (3000) in which the line segment L22 approximates the vehicle side surface is denoted as likelihood L (22 side), and the line segment L22 approximates the vehicle front-rear surface. Likelihood L (3000) is expressed as likelihood L (around 22), and likelihood L (3000) where line segment L22 is another line segment is expressed as likelihood L (22 other).

Next, the individual likelihood calculation process executed by the CPU 25a in S3000 will be described. FIG. 12 is a flowchart showing an individual likelihood calculation process executed by the CPU 25a.
When the individual likelihood calculation process is started, the CPU 25a first obtains the predicted value of the vehicle state obtained in the process of S1800 in the previous cycle (when the previous vehicle state estimation process is executed) and stored in the RAM 25c in S190 (S3010). Although details of the processing in S1800 will be described later, in S1800, using the unscented Kalman filter, the position coordinates (x1, y1) of the midpoint of the vehicle side surface in the next cycle, the position coordinates of the midpoint of the vehicle front-rear plane ( x2, y2), vehicle side length = N1, vehicle front / rear surface length = N2, vehicle orientation (angle) = Θ1, and vehicle travel direction (angle) = Θ2. In S190, these predicted values are stored in the RAM 25c in the coordinate system A at that time.

  Therefore, in S3010, as the predicted value of the vehicle state, the position coordinate (x1, y1) of the midpoint of the vehicle side surface, the position coordinate (x2, y2) of the midpoint of the vehicle front-rear surface, the length of the vehicle side surface = N1, Information on the length of the vehicle front-rear surface = N2, the direction (angle) of the vehicle = Θ1, and the traveling direction (angle) of the vehicle = Θ2 is read from the RAM 25c and acquired.

  When the processing in S3010 is completed in this way, the CPU 25a is then expressed in the coordinate system A at the time of the previous vehicle state estimation processing execution based on the displacement amounts (δx, δy, δθ) obtained in advance in S1505. The predicted value of the vehicle state is projected onto the current coordinate system A, and the predicted value is coordinate-transformed (S3015).

  In addition, when the processing in S3015 is completed in this way, the CPU 25a determines the position coordinate (x1, y1) of the vehicle side midpoint predicted in the previous cycle (when the previous vehicle state estimation processing is executed) and the current S120 or The distance from the position coordinates of the midpoint of the target line segment calculated in S1300 is calculated, and the target line segment approximates the target portion using the calculated distance as input and the function data shown in FIG. A likelihood L (3020) as a minute is derived (S3020).

  Further, the distance between the midpoint position coordinates (x2, y2) predicted in the previous cycle and the midpoint position coordinates of the target line segment calculated in S120 or S1300 this time is calculated, and the calculated distance is calculated. As the input, the function data shown in FIG. 11B is used to derive a likelihood L (3030) in which the target line segment is a line segment approximating the target portion (S3030).

  When the processing in S3030 is finished, the CPU 25a calculates the absolute value of the difference between the vehicle side length N1 predicted in the previous cycle and the length of the target line segment, and inputs the difference (absolute value) as an input. Using the function data shown in 11 (c), a likelihood L (3040) that the target line segment is a line segment approximating the target part is derived (S3040).

  In addition, the absolute value of the difference between the length N2 of the vehicle front-rear surface predicted in the previous cycle and the length of the target line segment is calculated, and the function data shown in FIG. Is used to derive the likelihood L (3050) that the target line segment is a line segment approximating the target part (S3050).

  Further, a difference (angle) between the vehicle orientation Θ1 predicted in the previous cycle and the direction of the target line segment is calculated, and the target line segment is input using the difference as an input and the function data shown in FIG. Is derived (S3060), and the difference (angle) between the vehicle traveling direction Θ2 predicted in the previous cycle and the direction of the target line segment is calculated. Then, using the difference as an input, the likelihood L (3070) that the target line segment is a line segment approximating the target part is derived using the function data shown in FIG. 11F (S3070).

When the above processing is completed, the likelihoods calculated in S3020 to S3070 are multiplied with each other to calculate a final likelihood L (3000) (S3080).
L (3000) = L (3020) × L (3030) × L (3040)
× L (3050) × L (3060) × L (3070)
Thereafter, the CPU 25a ends the individual likelihood calculation process. In addition, when such individual likelihood calculation processing is executed and the likelihood calculation processing in S1500 is completed, the CPU 25a proceeds to S1600 and executes the posterior probability calculation processing shown in FIG. FIG. 13 is a flowchart showing the posterior probability calculation process executed by the CPU 25a.

  In the present embodiment, by executing this posterior probability calculation process, the line probability L1, L21, L22 is calculated for each line segment L1, L21, L22 by the Bayesian estimation method using the prior probability obtained in S1400 and the likelihood obtained in S1500. Probability (posterior probability) that the line segment is a line segment that approximates the vehicle side surface, probability (posterior probability) that the line segment is a line segment that approximates the vehicle front-rear surface, and the line segment is another line segment Find probability (posterior probability).

  Next, the contents of this posterior probability calculation process will be described in detail with reference to FIG. When the posterior probability calculation process is started, the CPU 25a first sets the target line segment to the line segment L1 (S1605). Subsequently, the target part is set to “side surface” (S1610).

  Next, Bayesian estimation is performed using the prior probability P (1425) in which the target line segment approximates the target part and the likelihood L (3000) in which the target line segment approximates the target part. The posterior probability P (1615) that the target line segment is a line segment approximating the target part is obtained from the equation (S1615).

  In S1615, when the target line segment is the line segment L1 and the target part is the “side surface”, the posterior probability P (1615) is calculated according to the following equation. Here, the posterior probability P (1615) in the case where the target line segment is the line segment L1 and the target part is the “side surface”, that is, the posterior probability P (16) is the line segment that approximates the vehicle side surface. 1615) is expressed in particular as Po (1 side).

When this process is finished, it is determined whether or not the posterior probability P (1615) that the target line segment is a line segment approximating the vehicle front-rear surface has already been calculated for the current target line segment (S1630). If the posterior probability P (1615) that the target line segment is a line segment approximating the vehicle front-rear surface has not yet been calculated (S1630: N), the target part is changed to “front-rear surface” (S1635), and the process proceeds to S1615. Return. Then, the posterior probability P (1615) is obtained, in which the target line segment is a line segment that approximates the vehicle “front and back plane”.

  When the target line segment is the line segment L1 and the target part is the “front and back plane”, the posterior probability P (1615) is calculated according to the following equation. In this embodiment, the a posteriori probability P (1615) when the target line segment is the line segment L1 and the target part is the “front-rear plane”, that is, the line segment L1 is a line segment approximating the vehicle front-rear plane. The posterior probability P (1615) is particularly expressed as Po (around 1).

On the other hand, if it is determined that the posterior probability P (1615) that the target line segment is a line segment approximating the vehicle front-rear plane has already been calculated for the current target line segment (S1630: Y), the CPU 25a It is determined whether the posterior probability P (1615) that the target line segment is another line segment has already been calculated (S1640), and the target line segment is the other line segment for the current target line segment. If a certain posterior probability P (1615) has not yet been calculated (S1640: N), the target part is changed to “others” (S1645), and the process returns to S1615. Then, the posterior probability P (1615) that the target line segment is another line segment is obtained.

  When the target line segment is the line segment L1 and the target part is “other”, the posterior probability P (1615) is calculated according to the following equation. In this embodiment, the posterior probability P (1615) when the target line segment is the line segment L1 and the target part is “other”, that is, the posterior probability P (1615) that the line segment L1 is another line segment. ) Is expressed in particular as Po (1 other).

If the CPU 25a determines that the posterior probability P (1615) that the target line segment is another line segment has already been calculated for the current target line segment (S1640: Y), the CPU 25a sets the line segment L21 as the target line segment. It is set and it is determined whether or not the posterior probability P (1615) has been calculated (S1655). If it is determined that the posterior probability P (1615) has not been calculated (S1655: N), the target line segment is determined as the line segment L21. (S1660), and the process returns to S1610.

  Then, by repeatedly executing the process of S1615 in the above-described procedure, the posterior probability P (1615) = Po (21 side) that the line segment L21 is a line segment approximating the vehicle side surface, and the line segment L21 is the vehicle front-rear surface. A posteriori probability P (1615) = Po (around 21) that is an approximate line segment and a posteriori probability P (1615) = Po (21 other) that the line segment L21 is another line segment are calculated.

If it is determined that the line segment L21 is set as the target line segment and the posterior probability P (1615) is calculated (S1655: Y), the line segment L22 is set as the target line segment and the posterior probability P (1615) is calculated. If it is determined whether or not the line segment L22 is set as the target line segment and the posterior probability P (1615) is not calculated (S1665: N), the target line segment is determined as a line segment. Change to L22 (S1670), and return to S1610.

  Then, by repeating the processing of S1615 in the above-described procedure, the posterior probability P (1615) = Po (22 side) that the line segment L22 is a line segment that approximates the vehicle side surface, and the line segment L22 is the vehicle front-rear surface. A posteriori probability P (1615) = Po (around 22) that is an approximate line segment and a posteriori probability P (1615) = Po (22 other) that the line segment L22 is another line segment are calculated.

If it is determined that the line segment L22 is set as the target line segment and the posterior probability P (1615) is calculated (S1665: Y), the posterior probability calculation process is terminated. In S1600, when the posterior probability calculation process ends in this way, the CPU 25a executes a selection process shown in FIG. 14 (S1700). FIG. 14 is a flowchart showing the selection process executed by the CPU 25a.

  When the selection process is started, the CPU 25a determines the posterior probability Po (1 side), the posterior probability Po (around 1), or the posterior probability Po (21 side) and the posterior probability Po (around 22) obtained in S1600. It is determined whether or not both or both of the posterior probability Po (around 21) and the posterior probability Po (22 side) are equal to or greater than a predetermined threshold Th (S1710). That is, here, the conditional expression “Po (1 side) ≧ Th”, the conditional expression “Po (around 1) ≧ Th”, the conditional expression “Po (21 side), Po (around 22) ≧ Th”, the conditional expression “ It is determined whether or not at least one of the four conditional expressions “Po (around 21), Po (22 side) ≧ Th” is satisfied. The threshold Th may be set freely by the designer within a range satisfying 0 <Th <1.

If it is determined that it is not equal to or greater than the threshold Th (S1710: N), the selection process is terminated without executing the processes of S1720 to S1780.
On the other hand, if it is determined that the threshold value Th is equal to or greater than the threshold Th (S1710: Y), the CPU 25a determines that the posterior probability Po (1 side) is the posterior probability Po (1 side), Po (around 1), Po (21 side), Po ( 22), Po (around 21), and Po (22 side) are determined to be the maximum (S1720).

  When it is determined that the posterior probability Po (1 side) is the maximum (S1720: Y), it is determined that the line segment L1 is a line segment that approximates the vehicle side surface, and the approximate figure pattern that consists of the single line segment is The most probable figure pattern is selected as the contour of the vehicle located ahead (S1730). Thereafter, the selection process ends.

  On the other hand, when determining that the posterior probability Po (1 side) is not the maximum (S1720: N), the CPU 25a determines whether the posterior probability Po (around 1) is the maximum in the set (S1740). . When it is determined that the posterior probability Po (around 1) is the maximum (S1740: Y), it is determined that the line segment L1 is a line segment approximating the vehicle front-rear surface, and an approximate figure pattern composed of the single line segment is The most probable figure pattern is selected as the contour of the vehicle located in front (S1750). Thereafter, the selection process ends.

  In addition, if it is determined that the posterior probability Po (around 1) is not the maximum (S1740: N), either the posterior probability Po (21 side) or the posterior probability Po (around 22) is the maximum in the set. It is determined whether or not there is (S1760).

  When it is determined that either the posterior probability Po (21 side) or the posterior probability Po (around 22) is the maximum (S1760: Y), the line segment L21 is a line segment that approximates the vehicle side surface, and the line segment L22 is determined to be a line segment that approximates the vehicle front-rear surface, and the L-shaped approximate graphic pattern is selected as the most probable graphic pattern as the contour of the vehicle positioned ahead (S1770). Thereafter, the selection process ends.

  On the other hand, if it is determined that neither the posterior probability Po (21 side) or the posterior probability Po (around 22) is the maximum in the above set (S1760: N), the CPU 25a approximates the vehicle front-rear surface. It is a line segment, and it is determined that the line segment L22 is a line segment that approximates the vehicle side surface, and the L-shaped approximate graphic pattern is selected as the most probable graphic pattern as the contour of the vehicle located ahead (S1780). . Thereafter, the selection process ends.

When the selection process ends in S1700 as described above, the CPU 25a proceeds to S1800 and executes the state estimation process shown in FIG.
In the present embodiment, by executing the current past state estimation process, the current vehicle state and the vehicle state at the time of executing the next vehicle state estimation process (that is, the next cycle) are estimated based on the positioning result in S110. . In the following, this current past state estimation process will be outlined before it will be described in detail with reference to FIG.

In the state estimation process, first, according to the determination result of the selection process executed in S1700 and the calculation results of the position coordinates, orientations, and lengths of the midpoints of the line segments L1, L21, and L22 calculated in S120 and S1300, forward The object is regarded as a vehicle, and in the coordinate system A, the position coordinates (x0 _m , y0 _m ) of the center point of the preceding vehicle, the direction of the vehicle (the angle between the vehicle longitudinal axis and the coordinate system Ax axis) θ _m Then, the total length L _m and the total width W _m of the vehicle are obtained.

  And these observations z

And, after replacing the coordinate system B to be described later, the observed value z after the coordinate transformation, the input to the Kalman filter of the forward vehicle in the coordinate system B state quantities x _s

Is obtained as the output of the Kalman filter. Here x0 is the coordinate value on the coordinate system Bx axis of the forward vehicle center point, v _x is the velocity x-axis component of the forward vehicle, y0 is the coordinate system By on-axis coordinate value of the forward vehicle center point, v _y is The forward vehicle speed y-axis component, θ, is the angle formed by the x-axis and the vehicle side surface (or the vehicle longitudinal axis) (however, normalized so that 0 ≦ θ <π), and ω is the angle Time differentiation of θ (that is, angular velocity), W is the front vehicle full width, and L is the front vehicle full length.

Specifically, in this embodiment, an unscented Kalman filter (UKF) applicable to a nonlinear motion model is used as the Kalman filter, and the state quantity x _s of the preceding vehicle is obtained. FIG. 15 is an explanatory diagram showing a procedure for estimating the state quantity x _s from the observed value z.

The estimation apparatus 1 according to the present embodiment includes a program for causing the CPU 25a to realize the function as the unscented Kalman filter in the ROM 25b. In S1800, the CPU 25a executes the program to obtain the state quantity x _s . Ask.

  The unscented Kalman filter used here is constructed based on a well-known theory, and calculates an output with respect to an input by a transfer function determined by a motion model and an observation model set by a designer.

  Therefore, in the following, description of the principles of the Kalman filter and the unscented Kalman filter is omitted, and as an explanation of the unscented Kalman filter (UKF), the design of the unscented Kalman filter is implemented in realizing the operation of the estimation device 1 of the present embodiment. The motion model, observation model, and parameters input to and output from the unscented Kalman filter will be described.

As shown in FIG. 15, in the unscented Kalman filter of the present embodiment, the observed value z (t) calculated based on the positioning result of the latest cycle t and the observed value z ( The state quantity x _s (t, t−1) calculated and output by the unscented Kalman filter based on t−1) is input. Then, from the unscented Kalman filter by the calculation based on this input, the state quantity x _s (t, t) indicating the current vehicle state and the state quantity x _s (t + 1, t) indicating the vehicle state of the next cycle t + 1 are obtained. , Is output. However, in this embodiment, the vehicle state quantity x _s at the time (cycle) t ₁ obtained based on the input of the observation value z (t ₂ ) at the time (cycle) t ₂ , in particular, x _s ( t ₁ , t ₂ ).

The state quantity x _s (t, t) representing the current vehicle state output from the unscented Kalman filter is sent to the in-vehicle network connected to the control unit 25 of the estimation device 1 by the operation of the CPU 25a. Further, the state quantity x _s (t, t) representing the current vehicle state and x _s (t + 1, t) representing the vehicle state of the next cycle are stored in the RAM 25c. Then, x _s (t + 1, t) representing the vehicle state of the next cycle is used when the state estimation process of the next cycle is executed as described above. In addition, the state quantity x _s (t, t) stored in the RAM 25c is used when the input observation value z (t) is replaced with the coordinate system B when the next cycle estimation process is executed.

The coordinate system B used in the unscented Kalman filter is an xy coordinate system set with reference to the position and orientation of the preceding vehicle in the previous cycle, and the origin is estimated by the state estimation process in the previous cycle t-1. The position coordinate (x0, y0) of the vehicle center point indicated by the state quantity x _s (t−1, t−1) is set, and the y axis is estimated in the state estimation process of the previous cycle t−1. Xy coordinates that are set in the direction of the vehicle indicated by the state quantity x _s (t−1, t−1) (along the longitudinal axis of the vehicle) and the x axis is set to be orthogonal to the y axis It is a system. In the conversion from the coordinate system A to the coordinate system B, the parameter of the displacement amount of the host vehicle from the time t-1 to the time t is necessary. These parameters are the parameters (δx, δy, δθ) obtained in S1505. ) Is used. Further, Helmat transformation is used for coordinate transformation. In this case, the coordinate system is set based on the preceding vehicle 50, but any position may be used as a reference.

  The input / output of the unscented Kalman filter has been described above. Next, a motion model of the unscented Kalman filter provided in the estimation apparatus 1 of the present embodiment will be described. The estimation apparatus 1 of the present embodiment includes an unscented Kalman filter (hereinafter referred to as “UKF1”) designed based on a nonlinear motion model described later, and an unscented Kalman filter designed based on a linear motion model described later. (Hereinafter referred to as “UKF2”), and the vehicle state is estimated by switching between UKF1 and UKF2.

  UKF1 is designed based on a constant velocity circular motion model as a non-linear motion model, and is used when it is assumed that the preceding vehicle is turning. Specifically, the UKF 1 is designed based on the following motion model and mounted on the estimation device 1. The reason for using the uniform circular motion is that it is suitable for capturing the turning motion.

The modifier symbol * in the above expression indicates a state after one cycle T _s of the vehicle state estimation process that is repeatedly executed, and T _s is the repetition cycle. The parameters x and y used here are the center positions of the forward vehicle 50 and correspond to the inputs x0 _m and y0 _m to the unscented Kalman filter.

X ^* shown in the above _{^{equation, y *, v x *,}} v y *, the relationship between the θ ^* and ω ^*, x, y, and v _x, v _y, θ and ω is, the condition that constant velocity circular motion Naturally required from. Further, it is defined that the full width W and the full length L of the vehicle are independent of time and do not change over time.

However, in this motion model, since there is ω in the denominator, calculation is not possible when ω is zero. Therefore, when ω is zero, the motion model is changed from the constant-velocity circular motion model to the constant-velocity linear motion model by using UKF2 for estimating the state variable x _s , and the state variable x _s is estimated.

  That is, UKF2 of the present embodiment is designed based on a constant velocity linear motion model as a linear motion model, and is used when it is assumed that the preceding vehicle is linearly moving. Specifically, the UKF 1 is designed based on the following motion model and mounted on the estimation device 1.

Also, the observation model in UKF1 and UKF2 of this embodiment, with respect to z (t), a theta _m is obtained by normalizing such that _{0 ° ≦ θ m <180 °} . The observation model is not limited to this, and the designer can freely design the observation model.

In this embodiment, by using the UKF1, UKF2 having such a configuration, estimates the state quantity X _s of the forward vehicle.
Next, a method for setting the observation value z (t) used for the input of these unscented Kalman filters will be briefly described. This setting process is specifically realized by the processes from S1805 to S1842 of the state estimation process.

As described above, in the state estimation process, in order to set z (t), the determination result of the selection process, the position coordinates of the midpoints of the line segments L1, L21, and L22 calculated in S120 and S1300, the orientation, and According to the length calculation result, the front object is regarded as a vehicle, the position coordinates (x0 _m , y0 _m ) of the center point of the preceding vehicle, the direction of the vehicle (the angle formed by the coordinate system Ax axis of the vehicle longitudinal axis) θ _m , the total length L _m and the total width W _m of the vehicle are obtained, and the vehicle parts that can be measured depending on whether the preceding vehicle is in the state shown in FIG. 4A or the state shown in FIG. Of course, the processing procedure for obtaining the position coordinates (x0 _m , y0 _m ) of the center point of the preceding vehicle changes according to the positioning result. Therefore, here, a method for setting z (t) will be described with a specific example.

  In FIG. 16, z (t) in the case where it is determined in the selection process that the line segment L21 is a line segment that approximates the vehicle side surface and the line segment L22 is a line segment that approximates the vehicle front-rear surface. It is explanatory drawing which showed the derivation method.

As shown in FIG. 16, in this case, the length of the line segment L21, and set the length L _m of the vehicle, the length of the line segment L22, sets a full width W _m of the vehicle. Moreover, setting the angle between the x-axis and the line segment L21 in the direction theta _m of the vehicle. And the position of the midpoint of the line segment L3 formed by connecting the end point of the line segment L21 not connected to the line segment L22 and the end point of the line segment L22 not connected to the line segment L21 is set as the center point of the preceding vehicle, and the center the coordinate values of the x-axis of the point x _m, sets the coordinate values of the y-axis to the y _m. In this way, z (t) is set.

Further, when it is determined in the selection process that the line segment L21 is a line segment that approximates the vehicle side surface, and the line segment L22 is determined to be a line segment that approximates the vehicle front-rear surface, In the description regarding the setting of z (t) in the case where it is determined that L21 is a line segment approximating the vehicle side surface and the line segment L22 is a line segment approximating the vehicle front-rear surface, X _m , y _m , θ _m , W _m , and L _m are set by a method in which the line segment L22 is read and the line segment L22 is read as the line segment L21.

On the other hand, if it is determined in the selection process that the line segment L1 is a line segment approximating the vehicle side surface or the vehicle front-rear surface, an affirmative determination is made in S1805, and x _m , y _m , θ _m , W _m , and L _m are set. Next, the state estimation process executed by the CPU 25a in S1800 will be described in detail with reference to FIG.

  When this state estimation process is started, the CPU 25a determines in S1700 whether or not the approximate graphic pattern consisting of a single line segment has been selected as the most probable graphic pattern as the contour of the vehicle located ahead (S1805). If it is determined that an approximate figure pattern consisting of minutes has been selected (S1805: Y), the flow proceeds to S1810. On the other hand, when the L-shaped approximate graphic pattern is selected as the most probable graphic pattern as the contour of the vehicle located ahead in S1700 (S1805: N), the processing of S1810 to S1840 is not executed. , The process proceeds to S1842.

  In step S1810, the CPU 25a selects the L-shaped approximate graphic pattern as the most probable graphic pattern as the contour of the vehicle ahead by the selection process of the cycle ahead of the current cycle for the current target forward vehicle 50. It is determined whether it has been selected (S1810).

If an L-shaped approximate graphic pattern has been selected (S1810: Y), whether or not the line segment L1 is determined to be a line segment that approximates the vehicle front-rear surface in this selection process. Is determined (S1815). If it is determined that the line segment L1 is a line segment that approximates the vehicle front-rear surface (S1815: Y), the length of the vehicle side surface for the vehicle estimated in the past is set to L _m . Then, the length of the line segment L1 is set to W _m so that these values L _m and W _m are used for z (t) (S1820). Thereafter, the flow proceeds to S1842.

On the other hand, when it is determined that the line segment L1 is a line segment that approximates the vehicle side surface (S1815: N), the length of the vehicle front-rear surface for the vehicle estimated in the past is set to W _m . Then, the length of the line segment L1 is set to L _m (S1825). Thereafter, the flow proceeds to S1842.

  On the other hand, the L-shaped approximate graphic pattern is selected as the most probable graphic pattern as the contour of the vehicle located ahead in the selection process of the cycle ahead of the current cycle for the forward vehicle 50 currently targeted. If not (S1810: N), the process proceeds to S1830, and it is determined whether or not it is determined in this selection process that the line segment L1 is a line segment that approximates the vehicle front-rear surface (S1830).

When it is determined that the line segment L1 is a line segment that approximates the vehicle front-rear surface (S1830: Y), the length of the line segment L1 is set to W _m and the length of the vehicle side surface is set. A predetermined length (for example, the length at which the probability of the side surface reaches a peak in FIG. 9A) is set in L _m (S1835). Thereafter, the flow proceeds to S1842.

Further, if the line segment L1 is determined that the line segment approximating the vehicle side (S1830: N), the length of the line segment L1, and sets the L _m, the length of the vehicle longitudinal plane A predetermined length (for example, the length at which the probability of the front and rear surfaces peaks in FIG. 9A) is set in W _m (S1840). Thereafter, the flow proceeds to S1842.

In S1842, the CPU 25a sets an observation value z (t) for input to the unscented Kalman filter.
Specifically, when the L-shaped approximate graphic pattern is selected as the most probable graphic pattern as the contour of the vehicle located ahead in the selection process of S1700 (that is, the line segment L21 is the vehicle in the selection process). When the line segment L22 is determined to be a line segment that approximates the side surface and the line segment L22 is determined to be a line segment that approximates the vehicle front-rear surface, or the line segment L21 is determined to be a line segment that approximates the vehicle side surface, When it is determined that the line segment L22 is a line segment that approximates the vehicle front-rear surface), the observation value z (t) is set by the above-described method.

On the other hand, when the approximate graphic pattern consisting of a single line segment is selected as the most probable graphic pattern as the contour of the vehicle located ahead in the selection process of S1700 (that is, the line segment L1 is the vehicle side surface or the vehicle front-rear direction in the selection process). If it is determined that the surface is an approximated line segment), based on the information of W _m and L _m set in any of S1820, S1825, S1835, and S1840 and the information of the line segment L1, Z (t) including the position coordinates (x0 _m , y0 _m ) of the center point is calculated, and the observed value z (t) is set (S1842). Specifically, the vehicle full width W _m or the vehicle full length L along the direction in which the y value in the coordinate system A (xy coordinate system) increases in the direction orthogonal to the line segment L1 with the midpoint of the line segment L1 as the base point. _The position moved half the length of _m is regarded as the center point of the preceding vehicle, and the position coordinates (x0 _m , y0 _m ) of the center point of the preceding vehicle are calculated. Further, the vehicle orientation θ _m is obtained, and the observation value z (t) is set.

Further, when the observation value z (t) in the coordinate system A is set in S1842, the observation value z (t) is coordinate-converted into the coordinate system B described above. That is, the coordinate (x0, y0) of the vehicle center point indicated by the state quantity x _s (t−1, t−1) estimated by the previous vehicle state estimation process is the observation value z (t) of the coordinate system A. Projection is performed on the coordinate system B serving as the origin (S1845).

Further, the state quantity x _s (t, t−1), which is the predicted value of the current vehicle state estimated by the previous vehicle state estimation process, is used as the previous coordinate system B (that is, the state quantity x _s (t−2). , T-2), the vehicle indicated by the current coordinate system B (that is, the state quantity x _s (t-1, t-1)) from the coordinate system B whose origin is the coordinate (x0, y0) of the vehicle center point indicated by The coordinates are converted into a coordinate system B) where the coordinates (x0, y0) of the center point are the origin (S1847).

Next, it is determined whether the angular velocity ω indicated by the state quantity x _s (t−1, t−1) as an estimation result in the previous vehicle state estimation process is zero (S1850), and the angular velocity ω is zero. If (S1850: Y), the mobility model used in S1865 is set to a constant velocity linear motion model (in other words, the Kalman filter to be used is set to UKF2) (S1860), and the angular velocity ω is not zero (S1850: N), The motion model used in S1865 is set to a constant velocity circular motion model (in other words, the Kalman filter to be used is set to UKF1) (S1855). afterwards,
The process moves to S1865.

In S 1865, the observed value Z (t) and state quantity x _s (t, t−1) after conversion to the coordinate system B are input to the unscented Kalman filter designed based on the set motion model. The state quantity x _s (t, t) representing the current vehicle state and the state quantity x _s (t + 1, t) representing the vehicle state of the next cycle are obtained from the unscented Kalman filter.

That is, when it is determined that the angular velocity ω is zero (S1850: Y), the state quantity x _s (t, t) and the state quantity x _s (t + 1, t) are calculated using UKF2, and the angular velocity ω is If it is determined that it is not zero (S1850: N), the state quantity x _s (t, t) and the state quantity x _s (t + 1, t) are calculated using UKF1.

In S1865, the calculated state quantity x _s (t, t) and state quantity x _s (t + 1, t) in the coordinate system B are stored in the RAM 25c. Thereafter, the state estimation process ends.
Then, when the process proceeds to S1800, the process proceeds to S190, and the predicted value of the vehicle state used in the next likelihood calculation process is calculated based on the state quantity x _s (t + 1, t), and similarly stored in the RAM 25c.

That is, the state quantities _{x s (t + 1, t} ) vehicle overall length L indicated, set the parameters N1, the state quantities _{x s (t + 1, t} ) vehicle overall width W indicated, and sets the parameter N2, state quantity x _The vehicle orientation θ indicated by _s (t + 1, t) is replaced with the current coordinate system A, the vehicle orientation θ in the coordinate system A is set to the parameter Θ1, and the state quantity x _s (t + 1, t) the velocity vector of the vehicle indicated by (v _x, v _y), substituting the coordinate system a, the velocity vector (v _x, v _y) in the coordinate system a is the angle formed between the x-axis of the coordinate system a, the parameter Set to Θ2.

Further, the position coordinate (x0, y0) of the center point of the vehicle indicated by the state quantity x _s (t + 1, t) is replaced with the current coordinate system A, and the position coordinate ( x1, y1) and the position coordinates (x2, y2) of the midpoint of the vehicle front-rear surface are obtained as follows.

  That is, a distance half of the vehicle total length L is set so that the y value in the coordinate system A decreases along the longitudinal axis of the vehicle with the vehicle center point (x0, y0) in the coordinate system A as a base point. The coordinates of the moved position are obtained as the position coordinates (x2, y2) of the midpoint of the vehicle front-rear surface. In addition, the vehicle full width W in a direction in which the value of y in the coordinate system A decreases along a straight line that is based on the center point (x0, y0) of the vehicle in the coordinate system A and is perpendicular to the longitudinal axis of the vehicle. The coordinates of the position moved by half the distance are obtained as the position coordinates (x1, y1) of the midpoint of the vehicle side surface. However, if the value of y does not change in any direction, it can be advanced in any direction.

These values (x1, y1), (x2, y2), N1, N2, Θ1, and Θ2 are stored in the RAM 25c as predicted vehicle state values. Finally, x _s (t, t), which is the current information of the preceding vehicle 50 obtained in S1800, is output to the in-vehicle network (S200). Then, a series of processing is finished.

  As described above, the vehicle state estimation process according to the present embodiment has a configuration that can cope with a turning motion, and therefore the use range is significantly wider than that of the conventional technique. Therefore, if it is mounted on a vehicle, it can be used as information for collision avoidance, and even when it is installed on a road, it can be installed in the vicinity of a curve that has not been possible in the past.

  In the above embodiment, measurement of four or more reflection points is a condition for executing the subsequent processing. However, the estimation device 1 may be configured so that the estimation device 1 can estimate the state of the forward vehicle 50 even if the number of measured reflection points is two or three. Explaining the method, in the case of two points, the CPU 25a obtains the length, direction, and midpoint position of the line segment connecting these two points instead of the least square method in the process of S120. Then, the CPU 25a does not perform the two-segment calculation processing (S1300), and does not perform the processing based on the result of the two-segment calculation processing in the processing from the prior probability calculation processing (S1400) to the selection processing (S1700). . That is, only whether the line segment calculated | required by S120 approximates the vehicle front-back surface or a side surface is selected. Subsequent processing is the same as the processing described above.

  In the case of three points, the CPU 25a performs the following process instead of the two-segment calculation process. In this process, first, since there are three line segments that connect two of the three points, the three line segments are calculated. Then, by selecting two of the three line segments, two line segments are calculated. As a method of selecting two lines from among the three line segments, for example, a method of combining two line segments having a subordinate angle closest to 90 degrees among angles formed by the two line segments is conceivable. The subsequent processing after S1400 is the same as the processing described above.

In addition, although the subsequent processing content when it is determined No in S1710 has not been described above, in this case, the vehicle state estimation processing may be interrupted and positioning may be performed again assuming that the front object is not a vehicle. Alternatively, x _s (t, t−1) stored in the previous vehicle state estimation process may be output as a state quantity x _s (t, t) representing the current vehicle state.

Further, the processing of S1845 and S1847 cannot be performed unless the value of x _s (t, t−1) is stored in the RAM 25c. Therefore, in this case, it is preferable that the series of processing is completed after the information of the observation value z (t) is stored in the RAM 25c without performing the subsequent processing. Then, in the next vehicle state estimation process, the processes of S1845 and S1847 can be executed. When the initial calculation process by the unscented Kalman filter is executed, predetermined initial values are set as the speeds v _x , v _y , and the angular speed ω, and the necessary predicted values are substituted with the observed values to obtain the state quantity x _s. Good.

  In addition, in the likelihood calculation process, a predicted value of the vehicle state is required, but when the predicted value of the vehicle state is not recorded in the RAM 25c, the likelihood calculation process and the posterior probability calculation process are not executed. The prior probability P may be set as it is as the posterior probability Po, and the process proceeds to selection processing.

  In addition, the estimation means described in the claims is realized by the processing from S120 to S1865 of the present embodiment. In addition, the positioning means is realized by the processing of S110 of the present embodiment. Moreover, it goes without saying that the present invention can take various forms without being limited to the above embodiments.

DESCRIPTION OF SYMBOLS 1 ... Estimator, 11 ... Laser diode, 12 ... Collimator lens, 13 ... Laser diode drive circuit, 15 mirror ..., 17 motor ..., 19 ... Condensing lens, 21 ... Photo-dye auto, 22 ... Amplifier, 23 ... Detection circuit , 25 ... control unit, 25a ... CPU, 25b ... ROM, 25c ... RAM, 50 ... front vehicle

Claims (6)

An estimation device for estimating a motion state of a vehicle located ahead,
Positioning means for emitting a radar wave, receiving a reflected wave that is reflected back by a vehicle located in front of the vehicle, and measuring position coordinates of a plurality of points on the surface of the vehicle reflecting the radar wave; ,
Estimating means for estimating a motion state of the vehicle including an angular velocity of the vehicle based on position coordinates of a plurality of points on the vehicle surface measured by the positioning means, according to a predetermined nonlinear motion model;
An estimation apparatus comprising: The estimation means estimates at least one of the position of the vehicle, the direction of the vehicle, the speed of the vehicle, and the traveling direction of the vehicle, in addition to the angular velocity of the vehicle, as the motion state of the vehicle. The estimation device according to claim 1, wherein the estimation device is configured as follows. Said estimating means uses a Kalman filter to the non-linear motion model is employed, the estimation apparatus according to claim 1 or claim 2, characterized in that it is in the configuration of estimating the state of the vehicle. The estimation apparatus according to claim 3, wherein the Kalman filter is an unscented Kalman filter. The nonlinear motion model estimation device according to claim 3 or claim 4, characterized in that a constant-velocity circular movement model. When the previously estimated angular velocity is zero, the estimating means switches the motion model used for estimating the state of the vehicle from the nonlinear motion model to a predetermined linear motion model, according to the linear motion model, estimating apparatus according to any one of claims 5 to claim 3, characterized in that it is in the configuration of estimating the state of the vehicle.