JP6263453B2 - Momentum estimation device and program - Google Patents

Description

  The present invention relates to a momentum estimation device and a program.

  2. Description of the Related Art Conventionally, a technique for estimating the amount of vehicle movement based on vehicle information such as vehicle speed information, a yaw rate sensor, and a steering angle is known (for example, Patent Document 1).

  A technique for estimating the amount of vehicle movement by time-series verification of laser radar data is known (for example, Patent Documents 2 and 3).

JP 2007-310595 A JP 2006-160116 A JP 2012-133696 A

  However, in the technique described in Patent Document 1 described above, when a moving object and a stationary object are determined using a laser radar measurement result, a time delay or a response delay is generated between the laser radar data and the vehicle information. Therefore, an error in the estimated value of the own vehicle movement is likely to occur particularly during acceleration / deceleration or turning. In this case, it may cause an erroneous determination of a moving object and a stationary object.

  Further, in the techniques described in Patent Documents 2 and 3, the time-series collation in the traveling direction becomes indefinite at a place where the left and right roadside ends are configured with a flat surface without unevenness, such as a tunnel, The own vehicle motion estimation deteriorates, the discrimination between the moving object and the stationary object by the distance measuring sensor deteriorates, and the false detection increases.

  The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a momentum estimation device and a program capable of detecting a poor point of estimation of the momentum of a vehicle.

  In order to achieve the above object, a momentum estimation device according to a first aspect of the present invention is a position based on the moving body of each of a plurality of points on an object existing around the moving body, and the movement An acquisition means for acquiring the position measured by the measurement means mounted on the body for each time t, each of the positions corresponding to the point at the time t acquired by the acquisition means, and the acquired in the past A momentum estimating means for estimating the momentum of the moving body at time t based on each of the positions corresponding to the points; and a momentum of the moving body based on the momentum of the moving body estimated by the momentum estimating means. The position of a point obtained by generating a plurality of trial quantities for each of the plurality of trial quantities and performing coordinate transformation using the trial quantity for each of the positions corresponding to the point at the time t. of And comparing each of the positions corresponding to the points measured in the past by the measuring means, calculating a score representing the probability of the trial amount, and calculating each of the plurality of trial amounts. Based on the score and the plurality of trial amounts, a failure point likelihood calculation unit is configured to calculate the failure point likelihood of the estimation of the momentum of the moving body at time t.

  According to a second aspect of the invention, there is provided a program for measuring, on a computer, a position on the basis of the moving body at each of a plurality of points on an object existing around the moving body and mounted on the moving body Acquisition means for acquiring the position measured by each time t, each of the positions corresponding to the point at the time t acquired by the acquisition means, and each of the positions corresponding to the points acquired in the past Based on the momentum estimating means for estimating the momentum of the moving body at time t, and generating a plurality of trial quantities of the momentum of the moving body based on the momentum of the moving body estimated by the momentum estimating means, For each of the generated trial quantities, each of the positions of points obtained by performing coordinate transformation using the trial quantity with respect to each of the positions corresponding to the point at the time t, Comparing each of the positions corresponding to the points measured in the past by means to calculate a score representing the probability of the trial amount, the score calculated for each of the plurality of trial amounts, and This is a program for functioning as a bad spot likelihood calculating means for calculating the bad spot likelihood of estimating the momentum of the moving body at time t based on a plurality of trial amounts.

  According to the first and second inventions, the measuring means mounted on the moving body is a position based on the moving body of each of a plurality of points on the object existing around the moving body by the acquiring means. The position measured by is acquired for each time t.

  Then, based on each of the positions corresponding to the point at time t acquired by the acquisition unit and each of the positions corresponding to points acquired in the past by the momentum estimation unit, the momentum of the moving body at time t is calculated. presume.

  Then, the failure point likelihood calculating means generates a plurality of trial amounts of the moving body based on the momentum of the moving body estimated by the momentum estimating means, and for each of the generated plurality of trial quantities, at time t. Each point position obtained by performing coordinate conversion using the trial amount with respect to each position corresponding to the point is compared with each position corresponding to the point measured in the past by the measuring means. A score representing the certainty of the amount is calculated, and based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts, the likelihood of a bad point in estimating the momentum of the moving object at time t is calculated.

  As described above, the position of each of the plurality of points on the object existing around the moving body as a reference is acquired for each time t, and each of the positions corresponding to the acquired point of the time t Based on each of the positions corresponding to the acquired points, the momentum of the moving body at time t is estimated, and a plurality of trial amounts of the momentum of the moving body are generated based on the estimated momentum of the moving body, For each of the generated trial quantities, each of the positions corresponding to the point corresponding to the point of time t is measured in the past by each of the positions of the points obtained by performing coordinate conversion using the trial quantity and the distance measuring sensor. Each of the positions corresponding to the points is calculated, a score representing the probability of the trial amount is calculated, and based on the scores calculated for each of the plurality of trial amounts and the plurality of trial amounts, the time t In the estimation of momentum of moving objects The by calculating, it is possible to detect the failure point of the estimated momentum of the moving body is.

  In addition, the present invention further includes an exercise amount calculation unit and an output unit, wherein the acquisition unit further acquires the exercise state of the moving body at each time t, and the exercise amount calculation unit is acquired by the acquisition unit. The momentum of the moving body at the time t is calculated based on the motion state of the moving body at the time t, and the output means determines in advance the likelihood of a poor point of estimation of the momentum of the moving body at the estimated time t. If the threshold value is equal to or greater than the threshold value, the momentum of the moving object calculated by the momentum calculating means is output, and the estimated badness point of the momentum of the moving object at the estimated time t is less than a predetermined threshold value. In this case, the momentum of the moving body estimated by the momentum estimation means can be output.

  According to a third aspect of the present invention, there is provided a momentum estimation apparatus, which is a position based on the moving body at each of a plurality of points on an object existing around the moving body, and is measured by a measuring unit mounted on the moving body. The acquisition means for acquiring the measured position and the movement state of the moving body for each time t, each of the positions corresponding to the point at the time t acquired by the acquisition means, and acquired in the past Based on each of the positions corresponding to the points, based on the momentum estimating means for estimating the momentum of the moving body at time t, and based on the motion state of the moving body at time t obtained by the obtaining means, Based on the momentum calculating means for calculating the momentum of the moving body at time t, and the momentum of the moving body estimated by the momentum estimating means, a plurality of trial amounts of the momentum of the moving body are generated and generated For each of the plurality of trial amounts, each of the positions of the points obtained by performing coordinate transformation using the trial amount with respect to each of the positions corresponding to the point at the time t, and the past by the measuring means Are compared with each of the positions corresponding to the points measured to calculate a score representing the probability of the trial amount, and the score calculated for each of the plurality of trial amounts and the plurality of trials Dispersion matrix calculation means for calculating a weighted covariance matrix of the momentum of the moving object based on the amount, and observation noise used in the Kalman filter based on the weighted covariance matrix calculated by the dispersion matrix calculation means The Kalman gain is calculated based on the determined covariance of the observed noise, the momentum of the moving body estimated by the momentum estimation means, and the luck State estimation means for estimating the momentum of the moving body at time t according to a Kalman filter using the calculated Kalman gain, with the momentum of the moving body calculated by the quantity calculation means as an observation value. The

  According to the third aspect of the invention, the acquisition means measures the position of the plurality of points on the object existing around the moving body with respect to the moving body as a reference and is measured by the measuring means mounted on the moving body. The obtained position and the motion state of the moving body are acquired for each time t.

  The momentum estimation means estimates the momentum of the moving body at time t based on each of the positions corresponding to the point of time t acquired by the acquisition means and each of the positions corresponding to points acquired in the past. .

  Then, the momentum calculating means calculates the momentum of the moving body at time t based on the movement state of the moving body at time t acquired by the acquiring means.

  Then, the variance matrix calculation means generates a plurality of trial amounts of the momentum of the moving body based on the momentum of the moving body estimated by the momentum estimation means, and for each of the generated trial quantities, a point at time t Each of the positions of the points obtained by performing coordinate conversion using the trial amount for each of the positions corresponding to the point is compared with each of the positions corresponding to the points measured in the past by the measuring means. Is calculated, and a weighted covariance matrix of the momentum of the moving object is calculated based on the scores calculated for each of the plurality of trial amounts and the plurality of trial amounts.

  Then, the state estimation unit determines the covariance of the observation noise used in the Kalman filter based on the weighted covariance matrix calculated by the variance matrix calculation unit, and based on the determined covariance of the observation noise, The moving body at time t is calculated according to a Kalman filter using the calculated Kalman gain, with the gain calculated and the momentum of the moving body estimated by the momentum estimating means and the momentum of the moving body calculated by the momentum calculating means as observation values. Estimate the momentum.

  Thus, based on the momentum of the moving body estimated by the momentum estimation means, a plurality of trial quantities of the momentum of the moving body are generated, and the position corresponding to the point of time t for each of the generated plurality of trial quantities. Compare each of the positions of the points obtained by performing coordinate transformation using each of the trial amounts with each of the positions corresponding to the points measured in the past by the measuring means, and determine the probability of the trial amount. Calculating a score to be expressed, calculating a weighted covariance matrix of the plurality of trial amounts based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts, and calculating the weighted covariance matrix And determining the covariance of the observation noise used in the Kalman filter, calculating the Kalman gain based on the determined covariance of the observation noise, and calculating the momentum and the control of the moving object estimated by the momentum estimation means. Using the momentum of the moving body calculated by the quantity calculation means as an observed value, and estimating the momentum of the moving body at time t according to the Kalman filter using the calculated Kalman gain, accurately estimating the momentum of the moving body Can do.

  Further, the defect point likelihood calculating means in the present invention is based on the momentum of the moving body estimated by the momentum estimating means, and by sampling from a predetermined range including the momentum of the moving body, the momentum of the moving body A plurality of trial amounts can be generated.

  Further, the present invention is based on each moving body of a plurality of line-of-sight guidance signs existing around the moving body based on each of the positions corresponding to the point at time t acquired by the acquiring means. Further comprising eye gaze guidance mark candidate extraction means for acquiring a position for each time t, wherein the momentum estimation means each of the positions corresponding to the eye gaze guidance mark at time t obtained by the eye gaze guidance mark candidate extraction means; The momentum of the moving body at time t can be estimated based on each of the positions corresponding to the line-of-sight guidance sign acquired in the past.

  The program of the present invention can also be provided by being stored in a storage medium.

  As described above, according to the momentum estimation device of the first invention and the program of the second invention, the position of each of the plurality of points on the object existing around the moving object is set at each time point. acquired for t, and based on each of the positions corresponding to the acquired point of time t and each of the positions corresponding to the points acquired in the past, the momentum of the moving body at time t is estimated and estimated. Based on the momentum of the moving body, a plurality of trial amounts of the momentum of the moving body are generated, and for each of the generated trial quantities, the trial amount is used for each of the positions corresponding to the point of time t. A plurality of trials are calculated by comparing each point position obtained by coordinate conversion with each position corresponding to a point measured in the past by the distance measuring sensor, and calculating a score representing the probability of the trial amount. The score calculated for each quantity and the Based on the trial quantity, by calculating the failure point likelihood estimate of the momentum of the moving body at time t, it is possible to detect the failure point of the estimated momentum of the moving body, the effect is obtained that.

  Moreover, according to the momentum estimation device and the program of the third invention, a plurality of trial amounts of the momentum of the moving body are generated based on the momentum of the moving body estimated by the momentum estimation means, and the generated plurality of trial quantities For each of the points corresponding to the point at time t, and each of the points corresponding to the points measured in the past by the measuring means. And a score representing the probability of the trial amount is calculated, and a weighted covariance matrix of the plurality of trial amounts is calculated based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts. Based on the calculated weighted covariance matrix, determine the covariance of the observation noise used in the Kalman filter, calculate the Kalman gain based on the determined covariance of the observation noise, and estimate the momentum By estimating the momentum of the moving body at time t according to the Kalman filter using the calculated Kalman gain, using the momentum of the moving body estimated by the stage and the momentum of the moving body calculated by the momentum calculating means as the observed values The effect that the momentum of the moving body can be estimated with high accuracy is obtained.

It is a figure which shows the detection example of the vehicle periphery by a laser radar when a vehicle drive | works an urban area etc., and the detection example of the vehicle periphery by a laser radar when a vehicle drive | works a tunnel etc. It is a figure for demonstrating dispersion | distribution of momentum estimation. It is a block diagram which shows the momentum estimation apparatus which concerns on the 1st Embodiment of this invention. It is an example of the data detected by the laser radar. It is a figure which shows an example of the distance image and reflection luminance image which were produced | generated by the detection by a laser radar. It is a flowchart which shows the content of the momentum estimation processing routine in the momentum estimation apparatus which concerns on the 1st Embodiment of this invention. It is a block diagram which shows the momentum estimation apparatus which concerns on the 2nd Embodiment of this invention. It is a figure which shows an example which extracts a gaze guidance mark candidate from the reflected luminance image produced | generated by the detection by a laser radar. It is a figure which shows an example of the gaze guidance mark candidate extracted from the reflected luminance image. It is a figure for demonstrating an example of the method of preventing the misdetection of a gaze guidance mark candidate. It is a figure which shows the example of relationship between the momentum of a vehicle, and a vehicle body coordinate system. It is a flowchart which shows the content of the momentum estimation processing routine in the momentum estimation apparatus which concerns on the 2nd Embodiment of this invention. It is a block diagram which shows the momentum estimation apparatus which concerns on the 3rd Embodiment of this invention. It is a flowchart which shows the content of the momentum estimation process routine in the momentum estimation apparatus which concerns on the 3rd Embodiment of this invention. It is a flowchart which shows the content of the state estimation process routine in the momentum estimation apparatus which concerns on the 3rd Embodiment of this invention.

<Outline of Embodiment of the Present Invention>
In order to reduce collision and departure accidents on the highway due to driver's falling asleep or abnormalities in large vehicles (buses, trucks), when the driver is unable to drive, the road edge is detected with a sensor, A driving support system that automatically lays the vehicle on the edge of the runway and stops is useful.

  In order to construct such a driving support system, it is necessary to discriminate between a stationary object and a moving object based on sensor information, and it is necessary to estimate the amount of movement of the host vehicle as a precondition.

  As a conventional method, there has been proposed a method for calculating the amount of movement of the host vehicle by time series collation of distance measurement information such as laser radar.

  When calculating the momentum of the host vehicle by time-series verification of the distance measurement information of the laser radar, as shown in FIG. 1A, when the traveling scene is an urban area, time-series verification of the scan data of the laser radar is easy. I know that there is.

  However, as shown in FIG. 1B, in monotonous driving scenes such as tunnels and guardrails that frequently appear on highways, the change in roadside object shape measured by the distance measuring sensor is poor, and the scan data of the laser radar is Since series collation becomes difficult, there has been a problem that the estimation accuracy of the momentum of the vehicle deteriorates.

  Therefore, in the embodiment of the present invention, the good / bad state of the momentum estimation of the host vehicle by the distance measuring sensor such as a laser radar is estimated from the variance of the estimated momentum estimation values. An example of dispersion (X-axis direction and Z-axis direction) of vehicle momentum estimation by laser radar is shown in FIG. Note that the X-axis direction represents the lateral direction of the vehicle, and the Z-axis direction represents the vehicle traveling direction.

  As shown in FIG. 1 (A), when the vehicle traveling scene is an urban area, the time series collation of the scan data of the laser radar is easy. The distribution is as shown. On the other hand, as shown in FIG. 1B, in a monotonous driving scene such as a tunnel or a guardrail that frequently appears on a highway, the variance of the vehicle momentum estimation is, for example, a distribution as shown in FIG.

  Therefore, in the embodiment of the present invention, the good / bad state of the momentum estimation of the own vehicle by the distance sensor such as a laser radar is estimated from the variance of the estimated momentum value of the own vehicle. ) Substitute with information.

  Embodiments of the present invention will be described below with reference to the drawings. In the embodiment of the present invention, the case where the present invention is applied to a momentum estimation device that calculates the amount of badness of estimation of the momentum of the host vehicle and estimates the momentum of the host vehicle according to the likelihood of the defective point will be described as an example. .

[First Embodiment]
<System configuration of the momentum estimation device>
As illustrated in FIG. 3, the momentum estimation apparatus 10 according to the first embodiment includes a vehicle speed sensor 12 that sequentially detects a speed V [m / s] of the host vehicle, and a yaw rate ω [rad / s] of the host vehicle. A yaw rate sensor 14 that sequentially detects a distance, a distance measuring sensor 16 that sequentially measures a position of each of a plurality of points on an object existing around the vehicle as a reference, a computer 18 that estimates a momentum of the vehicle, and an estimation And a traveling environment recognition device 32 that recognizes the traveling environment based on the amount of movement of the vehicle. The distance measuring sensor 16 is an example of a measuring unit.

  The vehicle speed sensor 12 counts the number of wheel rotations N [times / second] per second, and calculates the formula V = π × N from the wheel rotation number N [times / second] and the known wheel diameter R [m]. The vehicle speed V [m / s] is detected according to xR.

  The distance measuring sensor 16 is mounted on the host vehicle and measures a position of each of a plurality of points on an object existing around the host vehicle with respect to the host vehicle at each time t. In the embodiment of the present invention, a case where a laser radar is used as a distance measuring sensor will be described as an example. The laser radar is incorporated at the front end of the vehicle and acquires scan data of the traveling environment around the host vehicle. In the following, for convenience of explanation, it is assumed that the laser radar is horizontally installed with a height h from the road surface and a direction of the vehicle traveling direction.

  The laser radar is composed of a light projecting unit and a light receiving unit, and measures the TOF (Time of Flight) until the laser light emitted from the light projecting unit is reflected by the object and detected by the light receiving unit. Measure distance. As a result of the distance measurement, a distance image and a reflected luminance image are obtained. FIG. 4 shows an example of distance measurement information by the laser radar. FIG. 5 shows an example of a distance image and a reflected luminance image obtained by measurement with a laser radar.

  The computer 18 includes a CPU, a RAM, and a ROM that stores a program for executing a later-described exercise amount estimation processing routine, and is functionally configured as follows. The computer 18 is measured by the vehicle information acquisition unit 20 that sequentially acquires the vehicle speed detected by the vehicle speed sensor 12 and the yaw rate of the vehicle detected by the yaw rate sensor 14 as vehicle information, and the distance measuring sensor 16. A distance measurement information acquisition unit 22 that acquires each of positions corresponding to a plurality of points on an object existing around the host vehicle, and a vehicle momentum conversion unit 24 that calculates a momentum of the host vehicle based on the acquired vehicle information. And based on the distance measurement information acquired by the distance measurement information acquisition unit 22, based on the momentum of the host vehicle estimated by the time series verification unit 26 and the time series verification unit 26 that estimates the momentum of the host vehicle, A defect estimation point determination unit 28 that calculates the likelihood of a bad point of estimation of the momentum of the vehicle at time t and determines whether or not it is a bad point of estimation. The vehicle momentum conversion unit 24 is an example of a vehicle momentum calculation unit, the time series verification unit 26 is an example of a vehicle momentum estimation unit, and the defect estimation point determination unit 28 is an example of a defect point likelihood calculation unit. is there.

  The vehicle information acquisition unit 20 acquires the speed of the host vehicle detected by the vehicle speed sensor 12 and the yaw rate of the host vehicle detected by the yaw rate sensor 14 for each time t as the motion state of the host vehicle.

  The ranging information acquisition unit 22 acquires a position corresponding to each of a plurality of points on an object existing around the host vehicle measured by the ranging sensor 16 at each time t.

The vehicle momentum conversion unit 24 calculates the amount of movement of the host vehicle at the time t based on the movement state of the host vehicle at the time t acquired by the vehicle information acquisition unit 20.
Specifically, the vehicle momentum conversion unit 24 calculates the following from the speed V [m / s] and yaw rate ω [rad / s] of the host vehicle acquired as the motion state of the host vehicle by the vehicle information acquisition unit 20. According to the calculation formula, the amount of movement Δx [m], Δz [m], Δθ [rad] of the host vehicle per measurement cycle time T is calculated.

Δx = 0.5 × ω × V × T ²
Δz = V × T
Δθ = ω × T

The calculated values calculated by the above calculation formula are predicted values of Δx, Δz, and Δθ based on the geometric relationship.
In the embodiment of the present invention, the amount of movement of the vehicle is calculated by the above calculation formula, but a more accurate predicted value may be calculated by applying a vehicle motion equation.

  Based on each of the positions corresponding to the plurality of points at the time t acquired by the ranging information acquisition unit 22 and each of the positions corresponding to the plurality of points acquired in the past, The momentum of the host vehicle at time t is estimated.

  Specifically, the time series collation unit 26 collates the past scan data acquired by the distance measurement information acquisition unit 22 with the latest scan data, and assumes that the roadside object is fixed to the ground. Momentum Δx [m], Δz [m], and Δθ [rad] are estimated. The time series collation of the scan data can be calculated by a known iterative closest point algorithm or the like. Alternatively, the scan data of the past time may be accumulated in a two-dimensional map corresponding to the travel plane, and the latest scan data and the two-dimensional map may be collated to estimate the momentum of the host vehicle.

  The defect estimation point determination unit 28 generates a plurality of trial amounts of the amount of movement of the host vehicle based on the amount of movement of the host vehicle estimated by the time-series matching unit 26. Next, the defect estimation point determination unit 28, for each of the generated plurality of trial amounts, each of the positions of the points obtained by performing coordinate conversion using the trial amount with respect to each of the positions corresponding to the point at time t. Are compared with each of the positions corresponding to the points measured in the past by the distance measuring sensor 16, and a score representing the probability of the trial amount is calculated.

  Next, the defective estimated point determination unit 28 calculates the likelihood of a bad point in estimating the amount of movement of the host vehicle at time t based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts.

  Then, the estimated defect point determination unit 28 determines that the estimated defect point is an estimated defect point when the estimated defect point estimation of the momentum of the vehicle at the time t is equal to or greater than a predetermined threshold. When the probability of a bad point of estimation of the momentum of the host vehicle at time t is less than a predetermined threshold, it is determined that the point is not a bad point of estimation.

  Specifically, the defect estimation point determination unit 28 estimates the variance of the estimated values Δx, Δz, Δθ of the own vehicle estimated by the time-series matching unit 26, and increases the variance of the tunnel and the like. It is determined whether or not the own vehicle exists at the defective point.

  The defect estimation point determination unit 28 calculates variances of the estimated values Δx, Δz, Δθ of the own vehicle's momentum estimated by the time-series matching unit 26 according to the following procedures (1) to (4).

(1) Sampling First, the defect estimation point determination unit 28 calculates the amount of momentum of the host vehicle by sampling from a predetermined range including the momentum of the host vehicle based on the momentum of the host vehicle estimated by the time-series matching unit 26. A plurality of trial amounts are generated as follows.

First, the defect estimated point determination unit 28 sets search ranges Δx _a , Δz _a , and Δθ _a . Search ranges Δx _a , Δz _a , and Δθ _a are set in advance based on vehicle speed, acceleration, and the like. Then, M is sampled from the range of Δx−Δx _{a to} Δx + Δx _a for the estimated value Δx of the own vehicle, and Δx _i is an element of the trial amount (i is 0 to 0). M-1 number).

Next, the defect estimation point determination unit 28 similarly samples M of the estimated value Δz of the own vehicle from the range of Δz−Δz _{a to} Δz + Δz _a based on the uniform distribution probability, and Δz _i Is an element of the trial amount.

Then, the defect estimation point determination unit 28 similarly samples M from the range of Δθ−Δθ _{a to} Δθ + Δθ _a based on the uniform distribution probability with respect to the estimated value Δθ of the own vehicle's momentum, and calculates Δθ _i . It is an element of trial amount.

A vector x _i ^ representing the trial amount of the vehicle's momentum is

x _i ^ = [Δx + Δx _i , Δz + Δz _i , Δθ + Δθ _i ]

M is obtained as follows. Note that “＾” attached to a symbol indicates that the symbol is a matrix, a multidimensional array, or a vector.

(2) Cost Calculation Next, the defect estimation point determination unit 28 uses the latest laser radar measurement point group (ranging information) based on the trial amount x _i ^ of the momentum of the host vehicle sampled in (1) above. A coordinate conversion is performed on each of the positions corresponding to a plurality of points at time t acquired by the acquisition unit 22, and a laser radar measurement point group one time before (time t− acquired by the distance measurement information acquisition unit 22). Each of the positions corresponding to a plurality of points is collated and the collation degree is obtained as M costs c _i . Incidentally, the measurement point group of the latest laser radar and the coordinate transformation calculates the distance between the measurement point group of one time before the laser radar, the distance may be determined as the M cost c _i.

As another method, the latest laser radar measurement point group is coordinate-converted based on the trial amount x _i ^, and the past scan data is collated with the accumulated two-dimensional map corresponding to the traveling plane, the sum of the matching value may be calculated as the cost c _i. For example, the three-dimensional object region of the latest laser radar measurement point group is coordinate-converted based on the trial amount x _i ^, and the past scan data is collated with the three-dimensional object region of the two-dimensional map stored corresponding to the traveling plane. Te may be calculated the sum of the matching value as a cost c _i.

(3) Cost Normalization Next, the defect estimation point determination unit 28 divides each cost value c _i by the total cost so that the total cost c _i obtained in (2) is 1. Normalization is performed to calculate a score p _i representing the probability of the trial amount. The score p _i indicates the likelihood of the own vehicle momentum Δx _i , Δz _i , Δθ _i .

p _i = c _i / sum (c _i ) (1)

Here, sum (c _i ) represents the total cost of each i.

(4) Calculation of Weighted Covariance Matrix V ^ Next, the defect estimation point determination unit 28 calculates the weight of x _i ^ by the following equation (2) based on the score p _i calculated in (3) above. The attached covariance matrix V ^ is calculated.

Then, the defect estimation point determination unit 28 pays attention to the term of Σ _i (p _i Δz _i ² ) of the weighted covariance matrix V ^ in the above formula (2), and the value of Σ _i (p _i Δz _i ² ). Is a bad spot for estimating the momentum of the own vehicle, and if the bad spot for estimating the momentum of the own vehicle is greater than a predetermined threshold, it is determined that the vehicle is a bad spot for estimating the momentum. . In the embodiment of the present invention, the vehicle speed variance is used as a predetermined threshold. The vehicle speed dispersion value is measured and set in advance.

  The output unit 30 outputs the momentum of the host vehicle calculated by the vehicle momentum conversion unit 24 when the failure estimation point determination unit 28 determines that the momentum of the host vehicle is estimated to be poor, and the defect estimation is performed. When the point determination unit 28 determines that the point is not a poor point for estimating the amount of movement of the own vehicle, the amount of movement of the own vehicle estimated by the time-series matching unit 26 is output. As described above, the output unit 30 is calculated by the vehicle momentum conversion unit 24 instead of the estimated value of the amount of movement of the host vehicle estimated by the time-series matching unit 26 when the host vehicle exists at the estimated defective point. Replaced with the estimated value of momentum of the own vehicle.

  The traveling environment recognition device 32 is configured to determine the position of each of a plurality of points on an object existing around the host vehicle measured by the distance measuring sensor 16 and the momentum of the host vehicle output by the output unit 30. Based on the above, it is recognized whether an object existing around the host vehicle is moving or stationary. Specifically, the traveling environment recognition device 32 recognizes whether an object existing around the host vehicle is moving or stationary by canceling the momentum of the host vehicle output by the output unit 30. .

<10 operations of the momentum estimation device>
Next, 10 operations of the momentum estimation apparatus according to the first embodiment will be described. First, the host vehicle travels by the driving operation of the driver of the vehicle, the speed of the host vehicle is sequentially detected by the vehicle speed sensor 12, the yaw rate of the host vehicle is sequentially detected by the yaw rate sensor 14, and around the host vehicle by the distance measuring sensor 16. A momentum estimation processing routine shown in FIG. 6 is executed in the computer 18 when the positions of the plurality of points on the existing object are sequentially measured with respect to the own vehicle.

  In step S100, the vehicle information acquisition unit 20 receives the speed of the host vehicle detected by the vehicle speed sensor 12 and the yaw rate of the host vehicle detected by the yaw rate sensor 14 as the motion state of the host vehicle.

  In step S <b> 102, the distance measurement information acquisition unit 22 receives a position corresponding to each of a plurality of points on an object existing around the host vehicle measured by the distance measurement sensor 16.

  In step S104, the vehicle momentum conversion unit 24 calculates the amount of movement of the host vehicle based on the movement state of the host vehicle received in step S100.

  In step S106, the current time t is determined based on each of the positions corresponding to the plurality of points received in step S102 and each of the positions corresponding to the plurality of points acquired in the past by the time-series matching unit 26. Estimate the momentum of the vehicle at.

In step S108, the trial amount x _i of the own vehicle's momentum is sampled by sampling from a predetermined range including the momentum of the own vehicle based on the momentum of the own vehicle estimated by the defect estimation point determination unit 28 in step S106. Generate multiple ^ s.

In step S110, the defect estimation point determination unit 28 uses the trial amount for each of the plurality of trial amounts x _i ^ generated in step S108 to position corresponding to the point at the current time t. A score representing the probability of the trial amount according to the above equation (1) by comparing each of the positions of the points obtained by conversion with each of the positions corresponding to the points measured in the past by the distance measuring sensor 16. Calculate p _i .

In step S112, the defective estimating point determination unit 28, based a score _{p i} calculated in step S110 for each of a plurality of trial amount _{x i} ^, into a plurality of trial amount _{x i} ^ and the above formula (2 ), A weighted covariance matrix V ^ including the likelihood of a bad spot for estimating the momentum of the host vehicle is calculated.

In step S114, among the weighted covariance matrix V ^ calculated in step S112 by the defective estimated point determination unit 28, the probability Σ _i (p _i Δz _i ² ) of the estimated bad moment of the host vehicle is It is determined whether or not a predetermined threshold value is exceeded. If the likelihood Σ _i (p _i Δz _i ² ) of the estimation of the momentum of the host vehicle is equal to or greater than a predetermined threshold, the estimation of the momentum of the host vehicle estimated at the current time t is If it is, the process proceeds to step S116. On the other hand, when the likelihood Σ _i (p _i Δz _i ² ) of the estimation of the momentum of the host vehicle is less than a predetermined threshold, the estimation of the momentum of the host vehicle estimated at the current time t is The process proceeds to step S118 because it is not at the defective point.

  In step S116, the output unit 30 outputs the momentum of the host vehicle calculated in step S104 as a result.

  In step S118, the output unit 30 outputs the momentum of the host vehicle estimated in step S106 as a result.

  In step S120, time t is incremented by 1, and the process returns to step S100.

  The traveling environment recognition device 32 outputs the position of the plurality of points on the object existing around the own vehicle measured by the distance measuring sensor 16 with reference to the own vehicle and the result in step S116 or step S118. Based on the amount of movement of the subject vehicle, it is recognized whether an object existing around the subject vehicle is moving or stationary.

  As described above, according to the momentum estimation device according to the first embodiment, the position of each of a plurality of points on an object existing around the vehicle is obtained for each time t and acquired. The momentum of the vehicle at time t is estimated based on each of the positions corresponding to the point of time t and the positions corresponding to points acquired in the past, and based on the estimated momentum of the vehicle The position of the point obtained by generating a plurality of trial amounts of the momentum of the vehicle, and for each of the generated trial quantities, coordinate conversion is performed using the trial amount with respect to each of the positions corresponding to the point at time t. Is compared with each of the positions corresponding to points measured in the past by the distance measuring sensor, a score representing the probability of the trial amount is calculated, and a score calculated for each of the plurality of trial amounts , Based on multiple trial amount and By calculating the failure point likelihood estimate of the momentum of the vehicle at t, it is possible to detect the failure point of the estimated motion of the vehicle.

  Further, the vehicle motion state is acquired for each time t, the vehicle momentum at time t is calculated based on the acquired vehicle motion state at time t, and the estimated vehicle momentum at time t is estimated. When the failure point likelihood is equal to or greater than a predetermined threshold, the vehicle momentum calculated based on the vehicle motion state at time t is output, and the estimated momentum of the moving body at the estimated time t is estimated. When the likelihood is less than a predetermined threshold value, the momentum of the vehicle estimated based on the distance measurement information acquired by the distance measurement sensor is output, so that the momentum of the vehicle can be accurately estimated.

  In addition, it is possible to prevent deterioration of the vehicle momentum estimation in a monotonous traveling scene such as a highway tunnel or a guardrail.

  In addition, by constantly monitoring the variance value of the vehicle momentum estimation by the distance measuring sensor, it is possible to determine the failure estimation state and to switch to the vehicle CAN information at an appropriate timing. This makes it possible to distinguish between a stationary object and a moving object that are robust on an expressway, and to reduce misrecognition.

<Second Embodiment>
Next, a second embodiment will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  The second embodiment is different from the first embodiment in that the position of the gaze guidance mark is detected and the momentum of the host vehicle is estimated based on the detected position of the gaze guidance mark.

  As shown in FIG. 7, the momentum estimation apparatus 210 according to the second embodiment includes a vehicle speed sensor 12 that sequentially detects a speed V [m / s] of the host vehicle, and a yaw rate ω [rad / s] of the host vehicle. A yaw rate sensor 14 that sequentially detects the distance, a distance measuring sensor 16 that sequentially measures a position of each of a plurality of points on an object existing around the own vehicle, and a computer 218 that estimates the amount of movement of the own vehicle. And a traveling environment recognition device 32 for recognizing the traveling environment based on the estimated momentum of the host vehicle.

  The computer 218 extracts line-of-sight guidance target candidates based on the distance information acquired by the vehicle information acquisition unit 20, the distance measurement information acquisition unit 22, the vehicle momentum conversion unit 24, and the distance measurement information acquisition unit 22. Based on the line-of-sight guidance target candidate extraction unit 225, the line-of-sight guidance target candidate extraction unit 225, the time series verification unit 226 that estimates the amount of movement of the host vehicle, the defect estimation point determination unit 28, And an output unit 30.

The distance measurement information acquisition unit 22 acquires positions corresponding to each of a plurality of points on an object existing around the vehicle measured by the distance measurement sensor 16 for each time t and is output by the distance measurement sensor 16. The reflected luminance image is acquired for each time t.
Based on each of the positions corresponding to the point of time t acquired by the distance measurement information acquisition unit 22 and the reflected luminance image, the line-of-sight guidance target candidate extraction unit 225 detects each of the plurality of line-of-sight guidance targets present around the host vehicle. A position based on the host vehicle is acquired for each time t.

  Specifically, the line-of-sight guidance candidate extraction unit 225 extracts a point where the reflection intensity value locally increases from the reflection intensity image acquired by the distance measurement information acquisition unit 22 as a line-of-sight guidance target candidate point. For example, the line-of-sight guide candidate extraction unit 225 scans the reflection intensity image in the horizontal direction by a one-dimensional Laplacian operator, and extracts a point where the value obtained by the scan is equal to or greater than a set threshold as a line-of-sight guide candidate. FIG. 8 shows an example in which a reflection intensity image is scanned in the horizontal direction by a one-dimensional Laplacian operator. FIG. 9 shows an example of a gaze guidance target candidate extracted from the reflection intensity image.

Further, the line-of-sight guidance target candidate extraction unit 225 generates a pair by combining the line-of-sight guidance marks on the same scan line of the reflection intensity image. Then, the line-of-sight guidance target candidate extraction unit 225 has an interval between the pair of line-of-sight guidance marks that is about the vehicle width (eg, 1.48 to 2.5 m), and the pair of line-of-sight guidance marks are at the same distance. As shown in FIG. 10, when there is a three-dimensional object having the same distance as the line-of-sight guide candidate between the line-of-sight guides, it is determined that the vehicle lamp part is erroneously detected, and the line-of-sight guide candidate is exclude.
Then, the line-of-sight guide candidate extraction unit 225 acquires each of the positions corresponding to the line-of-sight guide at time t from each of the positions corresponding to the point at time t acquired by the ranging information acquisition unit 22.

  The time-series matching unit 226 is based on each of the positions corresponding to the line-of-sight guidance target at the time t acquired by the line-of-sight guide target extraction unit 225 and each of the positions corresponding to the line-of-sight guide target acquired in the past. The momentum of the host vehicle at time t is estimated.

  Specifically, as illustrated in FIG. 11, the time-series matching unit 226 calculates the coordinate value of the vehicle coordinate system from the distance measurement value of the visual guide target candidate extraction unit 225 extracted from the visual guide target candidate extraction unit 225. The time-series matching unit 226 associates the line-of-sight guide target candidate at the current time t with the line-of-sight guide target candidate at the time t−1 one frame before by a known RANSAC method. That is, the time-series matching unit 226 assumes that the position of the line-of-sight guide is fixed to the ground, and estimates the amount of movement Δx, Δz, Δθ of the own vehicle by a known RANSAC method.

<Operation of 210 of momentum estimation device>
Next, the operation of the momentum estimation apparatus 210 according to the second embodiment will be described. First, the vehicle travels by the driving operation of the driver of the host vehicle, the speed of the host vehicle is sequentially detected by the vehicle speed sensor 12, the yaw rate of the host vehicle is sequentially detected by the yaw rate sensor 14, and the surrounding sensor 16 When the position of each of a plurality of points on the existing object with respect to the own vehicle is sequentially measured and the reflected luminance images are sequentially output, the computer 218 executes the momentum estimation processing routine shown in FIG. The In addition, about the process similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

In step S <b> 102, the position corresponding to each of a plurality of points on the object existing around the host vehicle measured by the distance measurement sensor 16 by the distance measurement information acquisition unit 22 and the reflection luminance output by the distance measurement sensor 16. Accept images.
In step S205, based on each of the positions corresponding to the point of time t acquired in step S102 and the reflection luminance image by the visual line guidance target candidate extraction unit 225, each of a plurality of visual guide signs existing around the host vehicle. For each time t.

  In step S206, based on each of the positions corresponding to the visual guidance target at time t acquired in step S205 and each of the positions corresponding to the visual guidance guide acquired in the past by the time-series matching unit 226. The momentum of the host vehicle at time t is estimated.

  In addition, about the other structure and effect | action of the momentum estimation apparatus 210 which concern on 2nd Embodiment, since it is the same as that of 1st Embodiment, description is abbreviate | omitted.

  As described above, according to the vehicle behavior predicting apparatus according to the second embodiment, the position of each of the plurality of line-of-sight guidance signs existing around the vehicle is acquired for each time t and acquired. The vehicle momentum is estimated by estimating the vehicle momentum at time t based on each of the positions corresponding to the line-of-sight guide at time t and each of the positions corresponding to the line-of-sight guide obtained in the past. It can be estimated with high accuracy.

<Third Embodiment>
Next, a third embodiment will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the third embodiment, the vehicle momentum calculated from the movement state of the host vehicle detected by each sensor and the momentum of the host vehicle estimated from the distance measurement information detected by the distance measuring sensor 16 are combined. Thus, the point of estimating the momentum of the host vehicle is different from the first embodiment.

  In the third embodiment, the speed of the host vehicle detected by the vehicle speed sensor 12, the momentum of the host vehicle calculated from the yaw rate detected by the yaw rate sensor 14, and the distance measurement information detected by the distance sensor 16. A case will be described as an example in which the momentum of the host vehicle estimated from the above is calculated in combination using a Kalman filter.

  Specifically, based on the calculated weighted covariance matrix V ^, the amount of movement of the host vehicle calculated from the speed and yaw rate of the host vehicle detected by each sensor, and the distance measurement detected by the distance measuring sensor 16. The momentum of the host vehicle estimated from the information is fused by a known Kalman filter. The third embodiment will be described below with reference to the drawings.

  As illustrated in FIG. 13, the momentum estimation apparatus 310 according to the third embodiment includes a vehicle speed sensor 12 that sequentially detects a speed V [m / s] of the host vehicle, and a yaw rate ω [rad / s] of the host vehicle. A yaw rate sensor 14 that sequentially detects the distance, a distance measuring sensor 16 that sequentially measures the position of each of a plurality of points on an object existing around the own vehicle, and a computer 318 that estimates the momentum of the own vehicle. And a traveling environment recognition device 32 for recognizing the traveling environment based on the estimated momentum of the host vehicle.

  The computer 318 is based on the momentum of the host vehicle estimated by the vehicle information acquisition unit 20, the distance measurement information acquisition unit 22, the vehicle momentum conversion unit 24, the time series verification unit 26, and the time series verification unit 26. A variance matrix calculation unit 327 that calculates a weighted covariance matrix, a vehicle momentum calculated by the vehicle momentum conversion unit 24, a vehicle momentum estimated by the time-series matching unit 26, and a variance matrix calculation unit 327 Is provided with a state estimation unit 328 for estimating the amount of movement of the host vehicle based on the weighted covariance matrix calculated by (1) and an output unit 30.

  Similar to the defect estimation point determination unit 28 of the first embodiment, the variance matrix calculation unit 327 determines a plurality of trial amounts of the own vehicle's momentum based on the own vehicle's momentum estimated by the time-series matching unit 26. Generate.

  Next, the variance matrix calculation unit 327 uses the trial amount with respect to each of the positions corresponding to the point at the current time t for each of the plurality of generated trial amounts, similarly to the defect estimation point determination unit 28. Each point position obtained by the coordinate conversion is compared with each position corresponding to a point measured in the past by the distance measuring sensor 16 to calculate a score representing the probability of the trial amount.

  Then, the variance matrix calculation unit 327 is weighted as shown in the above equation (2) based on the scores calculated for each of the plurality of trial amounts and the plurality of trial amounts, similarly to the defect estimation point determination unit 28. A covariance matrix V ^ is calculated.

  The state estimation unit 328 determines the covariance of the observation noise used in the Kalman filter based on the weighted covariance matrix V ^ calculated by the variance matrix calculation unit 327, and based on the determined covariance of the observation noise Calculate the Kalman gain.

  Next, the state estimation unit 328 uses the calculated Kalman gain by using the amount of movement of the host vehicle estimated by the vehicle momentum conversion unit 24 and the amount of movement of the host vehicle calculated by the time-series matching unit 26 as observation values. According to the Kalman filter, the momentum of the host vehicle at time t is estimated. And the state estimation part 328 outputs the momentum of the own vehicle in the estimated time t.

  Specifically, the state estimation unit 328 estimates the momentum of the host vehicle by the Kalman filter from the momentum of the host vehicle calculated by the vehicle momentum conversion unit 24 and the momentum of the host vehicle estimated by the time-series matching unit 26. To do.

  An example of a motion model used in the Kalman filter is shown below.

In this case, x ^ is a state vector, F ^ is an update matrix, and w ^ is a system noise term whose covariance matrix is represented by Q ^.
The state vector x ^ is expressed by the following equation.

Each element of the state vector x ^ includes a movement amount Δx (lateral movement amount), Δz (traveling direction movement amount), Δθ (azimuth direction movement amount) of the own vehicle, and first-order differential terms Δx ^· ,. Δz ^· , Δθ ^· . Here, the update matrix F ^ is set as the following equation, considering it as a simple mass point model. If the vehicle motion model is known, the estimation accuracy can be further improved by applying it. T is the update time [sec].

  The observation model is expressed by the following formula. At this time, z ^ is an observation vector, and H ^ is an observation matrix. v ^ is observation noise.

At this time, the observed value vector z ^ includes the own vehicle's momentum Δx _C , Δz _C , Δθ _C calculated by the vehicle momentum conversion unit 24 and the own vehicle's momentum Δx _L , Δz estimated by the time-series matching unit 26. _L and Δθ _L are elements.

  At this time, the observation matrix H ^ is expressed by the following equation.

  The dispersion matrix R ^ of the observation noise v ^ is set by the following equation.

At this time, V ^ _car and V ^ _lidar are matrices of 3 rows _× 3 columns. 0 ^ is a zero matrix of 3 rows x 3 columns. The weighted covariance matrix V ^ calculated by the variance matrix calculation unit 327 may be substituted for V ^ _lidar . Further, V ^ _car is the variance of the measured values from the measured values of the vehicle's momentums Δx [m], Δz [m], Δθ [rad] per measurement cycle time T acquired by the vehicle momentum conversion unit 24. Calculate and set.

Then, the vehicle momentum Δx _C , Δz _C , Δθ _C calculated by the vehicle momentum conversion unit 24 and the momentum of the host vehicle estimated by the time-series matching unit 26 are calculated by a known update formula of the Kalman filter as described below. The momentum of the two vehicles, Δx _L , Δz _L , and Δθ _L , is taken as the observed value z ^, and the weights of V ^ _car and V ^ _lidar are taken into consideration and updated sequentially, so that An estimated value x ^ of the vehicle momentum is obtained. In the third embodiment, k corresponds to time. P ^ is an error covariance matrix, and K ^ is a Kalman gain. As the initial value of P ^, for example, a relatively large value is set in consideration of the range of variation of the initial value of the observed value z ^.

  Here, the value to which the subscript “p, k” is given represents a pre-estimated value of time k predicted based on data available up to time k−1. Further, the value to which the subscript “k” is added represents a post-estimation value at time k estimated based on data available up to time k.

  The estimation process using the Kalman filter includes a prediction step and a filtering step.

(1) Prediction Step First, the process in the prediction step will be described. In the prediction step, the state estimation unit 328 calculates the time k according to the above equation (10) based on the state vector x ^ _k-1 at the time k-1 calculated in the previous filtering step and the update matrix F ^. The state vector x ^ _{p, k} is calculated.

Then, the state estimation unit 328 calculates the above formula (^) based on the update matrix F ^, the covariance matrix P ^ _k-1 estimated in the previous filtering step, and the system noise w ^ covariance matrix Q ^. 11) Calculate the prior error covariance matrix P ^ _{p, k} .

(2) Filtering Step Next, processing in the filtering step will be described. The state estimation unit 328 assigns the weighted covariance matrix V ^ calculated by the variance matrix calculation unit 327 to V ^ _lidar of the variance matrix R ^ of the observation noise v ^, and the variance matrix R ^ of the observation noise v ^ To decide.

Then, in the filtering step, the state estimation unit 328 converts the prior error covariance matrix P ^ _{p, k} calculated in the prediction step, the observation matrix H ^, and the determined variance matrix R ^ of the observation noise v ^. Based on the above equation (12), the Kalman gain K ^ is calculated.

Next, the state estimation unit 328 calculates the calculated Kalman gain K ^, the state vector x ^ _{p, k} at the time k calculated in the prediction step, and z ^ = [Δx _C shown in the above equation (7). , Δz _C , Δθ _C , Δx _L , Δz _L , Δθ _L ] and the observation matrix H ^, the state vector x ^ _k at time _k is estimated according to the above equation (13).

Then, the state estimation unit 328 follows the above equation (14) based on the covariance matrix P ^ _k−1 estimated in the previous filtering step, the calculated Kalman gain K ^, and the observation matrix H ^. Then, the posterior error covariance matrix P ^ _k is calculated.

Each value (Kalman gain K ^, state vector x ^ _k , posterior error covariance matrix P ^ _k ) calculated in the filtering step is used in the process in the next prediction step.

Further, the state estimation unit 328 outputs the amount of movement [Δx, Δz, Δθ] of the own vehicle among the state vectors x ^ _{k at} the estimated time k.

  The output unit 30 outputs the amount of movement of the host vehicle output by the state estimation unit 328.

  The traveling environment recognition device 332 includes a position of each of a plurality of points on an object existing around the own vehicle measured by the distance measuring sensor 16 and a momentum of the own vehicle output by the output unit 30. Based on the above, it is recognized whether an object existing around the host vehicle is moving or stationary. Specifically, the traveling environment recognition device 32 recognizes whether an object existing around the host vehicle is moving or stationary by canceling the momentum of the vehicle output by the output unit 30.

<Operation of the momentum estimation apparatus 310>
Next, the operation of the momentum estimation apparatus 310 according to the third embodiment will be described. First, the vehicle travels by the driving operation of the driver of the host vehicle, the speed of the host vehicle is sequentially detected by the vehicle speed sensor 12, the yaw rate of the host vehicle is sequentially detected by the yaw rate sensor 14, and the surrounding sensor 16 A momentum estimation processing routine shown in FIG. 14 is executed in the computer 318 when the positions of the plurality of points on the existing object are sequentially measured with respect to the own vehicle.

In step S308, based on the momentum of the host vehicle estimated in step S106 by the variance matrix calculation unit 327, sampling is performed from a predetermined range including the momentum of the host vehicle by sampling the trial amount x _i ^ Generate multiple.

In step S310, the variance matrix calculation unit 327 performs coordinate transformation on each of the plurality of trial amounts x _i ^ generated in step S308 using the trial amount for each of the positions corresponding to the point at the current time t. Each point position obtained in this manner is compared with each position corresponding to a point measured in the past by the distance measuring sensor 16, and a score p representing the probability of the trial amount according to the above equation (1). _i is calculated.

In step S312, based on the score p _i calculated in step S310 and the plurality of trial amounts x _i ^ by the variance matrix calculation unit 327 for each of the plurality of trial amounts x _i ^, the above formula (2) As shown, a weighted covariance matrix V ^ is calculated.

  In step S314, the state estimation unit 328 determines the covariance of the observation noise used in the Kalman filter based on the weighted covariance matrix calculated in step S312, and based on the determined covariance of the observation noise. Calculate the Kalman gain. Then, the state estimation unit 328 uses the momentum of the host vehicle calculated in step S104 and the momentum of the host vehicle estimated in step S106 as observation values according to a Kalman filter using the calculated Kalman gain. Estimate the momentum of the vehicle at. Step S314 is realized by the state estimation processing routine shown in FIG.

<State estimation processing routine>
First, in step S400, the state estimation unit 328 obtains the weighted covariance matrix V ^ calculated in step S312.

In step S402, based on the state vector x ^ _{k-1 at} time k-1 calculated in the previous step S410 and the update matrix F ^ by the state estimation unit 328, the time k according to the above equation (10). The state vector x ^ _{p, k} is calculated.

In step S404, based on the update matrix F ^, the covariance matrix P ^ _k-1 estimated in the previous step S412 and the system noise w ^ covariance matrix Q ^ by the state estimation unit 328, Prior error covariance matrix P ^ _{p, k} is calculated according to equation (11).

In step S406, the state estimation unit 328 assigns the weighted covariance matrix V ^ calculated by the variance matrix calculation unit 327 to V ^ _lidar of the variance matrix R ^ of the observation noise v ^, and the observation noise v ^ The variance matrix R ^ is determined.

In step S408, the state estimation unit 328 calculates the prior error covariance matrix P ^ _{p, k} calculated in step S404, the observation matrix H ^, and the dispersion matrix R of the observation noise v ^ determined in step S406. Based on {circumflex over ()}, Kalman gain K ^ is calculated according to the above equation (12).

In step S410, the Kalman gain K ^ calculated in step S408 by the state estimation unit 328, the state vector x ^ _{p, k at} time k calculated in step S402, and the above equation (7) are shown. Based on z ^ = [Δx _C , Δz _C , Δθ _C , Δx _L , Δz _L , Δθ _L ] and the observation matrix H ^, the state vector x ^ _k at time _k is estimated according to the above equation (13). To do.

In step S412, the state estimation unit 328 estimates the covariance matrix _{{circumflex over (} P) _{} k-1} calculated in the previous filtering step calculated in step S412; the Kalman gain K ^ calculated in step S408; and the observation matrix H Based on _{circumflex over () _} , the posterior error covariance matrix P ^ _k is calculated according to the above equation (14).

In step S414, the momentum [Δx, Δz, Δθ] of the host vehicle is output as a result of the state vector x ^ _k at time k calculated in step S410, and the state estimation processing routine is terminated.

  Next, returning to the momentum estimation processing routine, in step S316, the output unit 30 outputs the momentum of the host vehicle output in step S314 as a result. In step S318, the time k is incremented by 1, Return to S100.

  The travel environment recognition device 332 includes the position of the plurality of points on the object existing around the host vehicle measured by the distance measuring sensor 16 as a reference and the host vehicle output as a result in step S316. Based on the amount of momentum, it is recognized whether an object existing around the vehicle is moving or stationary.

  In addition, about the other structure and effect | action of the momentum estimation apparatus which concern on 3rd Embodiment, since it is the same as that of 1st Embodiment, description is abbreviate | omitted.

  As described above, according to the momentum estimation apparatus 310 according to the third embodiment, a plurality of trial amounts of the vehicle momentum are generated based on the vehicle momentum estimated by the time-series matching unit 26 and generated. For each of the plurality of trial quantities, the position corresponding to each point corresponding to the point at time k is coordinate-transformed using the trial quantity, and the distance sensor 16 measures in the past. Each of the positions corresponding to the points is calculated, a score representing the probability of the trial amount is calculated, and a plurality of trial amounts are calculated based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts. Calculate the weighted covariance matrix of the trial amount, determine the covariance of the observed noise used in the Kalman filter based on the calculated weighted covariance matrix, and determine the Kalman based on the determined covariance of the observed noise gain The vehicle momentum calculated by the time-series matching unit 26 and the vehicle momentum calculated by the vehicle momentum conversion unit 24 are observed values, and the vehicle at time k is calculated according to the Kalman filter using the calculated Kalman gain. By estimating the momentum, the momentum of the vehicle can be accurately estimated.

  By estimating the vehicle momentum at time k according to the Kalman filter, the vehicle momentum calculated by the vehicle momentum conversion unit 24 and the vehicle momentum estimated by the time-series matching unit 26 are positively switched. There is no need.

  In the above-described embodiment, the case where a vehicle is targeted as a moving body has been described as an example. However, the present invention is not limited to this, and another moving body may be targeted.

  In the above-described embodiment, the case where the laser radar is used as the distance measuring sensor has been described as an example. However, the present invention is not limited to this. For example, an on-vehicle stereo camera may be used. In this case, a position corresponding to each of a plurality of points on an object existing around the own vehicle may be calculated based on a stereo image taken by the stereo camera.

  Further, in the above-described embodiment, an example has been described in which the vehicle speed detected by the vehicle speed sensor 12 and the yaw rate of the host vehicle detected by the yaw rate sensor 14 are acquired as the vehicle motion state. However, the present invention is not limited to this, and the vehicle motion state obtained by other sensors may be acquired. For example, the steering angle of the steering wheel may be acquired as the motion state of the vehicle by a steering wheel angle sensor.

  In the above-described embodiment, the case where the present invention is applied to the momentum estimation device that estimates the momentum of the vehicle has been described as an example. However, the present invention is not limited to this, and the estimation of the momentum of the vehicle at time t is performed. The present invention may be applied to an apparatus that calculates only the likelihood of a defective point.

In addition, the defect estimation point determination unit 28 in the first and second embodiments focuses on the term of Σ _i (p _i Δz _i ² ) of the weighted covariance matrix V ^ in the above equation (2). , Σ _i (p _i Δz _i ² ) has been described as an example where it is likely to be a bad spot for estimating the momentum of the vehicle, but is not limited to this, and the weight of the above equation (2) is added. The values of the other elements of the covariance matrix V ^ may be considered to be bad points for estimating the momentum of the host vehicle.

  The program of the present invention can be provided by being stored in a recording medium.

10, 210, 310 Momentum amount estimation device 12 Vehicle speed sensor 14 Yaw rate sensor 16 Distance sensor 18, 218, 318 Computer 20 Vehicle information acquisition unit 22 Distance information acquisition unit 24 Vehicle momentum conversion unit 26 Time series verification unit 28 Defect estimation point determination Unit 30 Output unit 32, 332 Traveling environment recognition device 225 Gaze guidance target candidate extraction unit 226 Time series verification unit 327 Dispersion matrix calculation unit 328 State estimation unit

Claims (6)

The position of each of a plurality of points on the object existing around the moving body is obtained with respect to each time t, and the position measured by the measuring means mounted on the moving body is obtained for each time t. Acquisition means to
The momentum for estimating the momentum of the moving body at time t based on each position corresponding to the point at time t acquired by the acquisition means and each position corresponding to the point acquired in the past. An estimation means;
Based on the momentum of the moving body estimated by the momentum estimation means, a plurality of trial amounts of the momentum of the moving body are generated, and each of the generated trial quantities corresponds to the point at the time t. Comparing each of the positions of the points obtained by coordinate conversion using the trial amount with respect to each of the positions, and each of the positions corresponding to the points measured in the past by the measuring means, A score representing the probability of the trial amount is calculated, and based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts, a bad point of estimation of the momentum of the moving body at time t A defect point likelihood calculating means for calculating the likelihood;
A momentum estimation device including: Further including an exercise amount calculating means and an output means;
The acquisition means further acquires the movement state of the moving body for each time t,
The momentum calculating means calculates the momentum of the moving body at time t based on the movement state of the moving body at time t acquired by the acquiring means,
The output means outputs the momentum of the moving body calculated by the momentum calculation means when the likelihood of a bad point of estimation of the momentum of the moving body at the estimated time t is greater than or equal to a predetermined threshold, The momentum of the mobile body estimated by the momentum estimation means is output when the likelihood of a bad point of estimation of the momentum of the mobile body at an estimated time t is less than a predetermined threshold. Estimating device. A position based on the moving body of each of a plurality of points on an object existing around the moving body, and the position measured by a measuring unit mounted on the moving body; and Obtaining means for obtaining the exercise state for each time t;
The momentum for estimating the momentum of the moving body at time t based on each position corresponding to the point at time t acquired by the acquisition means and each position corresponding to the point acquired in the past. An estimation means;
Momentum calculating means for calculating the momentum of the moving body at time t based on the movement state of the moving body at time t acquired by the acquiring means;
Based on the momentum of the moving body estimated by the momentum estimation means, a plurality of trial amounts of the momentum of the moving body are generated, and each of the generated trial quantities corresponds to the point at the time t. Comparing each of the positions of the points obtained by coordinate conversion using the trial amount with respect to each of the positions, and each of the positions corresponding to the points measured in the past by the measuring means, A score representing the probability of the trial amount is calculated, and a weighted covariance matrix of the momentum of the mobile object is calculated based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts A dispersion matrix calculating means for
Based on the weighted covariance matrix calculated by the variance matrix calculation means, determine the covariance of the observation noise used in the Kalman filter, calculate the Kalman gain based on the determined covariance of the observation noise, The movement at time t according to a Kalman filter using the calculated Kalman gain, with the momentum of the moving body estimated by the momentum estimation means and the momentum of the moving body calculated by the momentum calculation means as observation values State estimation means for estimating body momentum;
A momentum estimation device including: The defect point likelihood calculating means calculates a plurality of trial amounts of the momentum of the moving body by sampling from a predetermined range including the momentum of the moving body based on the momentum of the moving body estimated by the momentum estimating means. The momentum estimation apparatus according to claim 1 or 2. Based on each of the positions corresponding to the point at time t acquired by the acquisition means, the position of each of the plurality of visual guidance indicators existing around the moving body with respect to the moving body is set for each time t. It further includes eye gaze guidance mark candidate extraction means for acquiring,
The momentum estimation means includes a position corresponding to the visual guidance sign at time t acquired by the visual guidance target candidate extraction means and a position corresponding to the visual guidance sign acquired in the past. The momentum estimation device according to any one of claims 1 to 4, wherein the momentum of the moving body at time t is estimated based on the momentum. On the computer,
The position of each of a plurality of points on the object existing around the moving body is obtained with respect to each time t, and the position measured by the measuring means mounted on the moving body is obtained for each time t. Acquisition means,
The momentum for estimating the momentum of the moving body at time t based on each position corresponding to the point at time t acquired by the acquisition means and each position corresponding to the point acquired in the past. Based on the momentum of the moving body estimated by the estimation means and the momentum estimation means, a plurality of trial amounts of the momentum of the moving body are generated, and for each of the generated trial quantities, the time t Comparing each of the positions of the points obtained by coordinate transformation using the trial amount with respect to each of the positions corresponding to the points, and each of the positions corresponding to the points measured in the past by the measuring means Then, a score representing the probability of the trial amount is calculated, and based on the score calculated for each of the plurality of trial amounts and the plurality of trial amounts, the mobile body at time t is calculated. A program for functioning as a bad spot-likeness calculating means for calculating the likelihood of a bad spot for estimating momentum.