WO2018212301A1

WO2018212301A1 - Self-position estimation device, control method, program, and storage medium

Info

Publication number: WO2018212301A1
Application number: PCT/JP2018/019173
Authority: WO
Inventors: 加藤　正浩; 岩井　智昭; 多史藤谷
Original assignee: パイオニア株式会社
Priority date: 2017-05-19
Filing date: 2018-05-17
Publication date: 2018-11-22
Also published as: JP2022031266A; JP6968877B2; JP2022176322A; JPWO2018212301A1

Abstract

A host vehicle position estimation unit 17 of an on-vehicle device 1 is provided with: a position prediction unit 21 which generates information indicating a predicted host vehicle position X-(t); and a position estimation unit 22. The position estimation unit 22 calculates a planimetric feature predicted value Z-(t) predicted on the basis of planimetric feature information and a planimetric feature measured value Z(t) indicating the distance from a moving body to an object measured by a lidar 2. Furthermore, the position estimation unit 22 determines coefficient values aX(t),aY(t) for correcting the Kalman gain K(t), in accordance with difference evaluation values Ex, Ey based on difference values dx, dy between the planimetric feature measured value and the planimetric feature predicted value, the position estimation accuracy σP, and the lidar measurement accuracy σL.

Description

Self-position estimation apparatus, control method, program, and storage medium

The present invention relates to a self-position estimation technique.

2. Description of the Related Art Conventionally, a technique for detecting a feature installed at a destination of a vehicle using a radar or a camera and calibrating the position of the vehicle based on the detection result is known. For example, Patent Document 1 discloses a technique for estimating a self-position by collating the output of a measurement sensor with the position information of a feature registered in advance on a map. Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.

JP 2013-257742 A JP 2017-72422 A

When estimating the self-position by collating the output of the measurement sensor with the position information of the feature registered in advance on the map, if there is an error in either the map or the output of the measurement sensor, the self-position is incorrect. And the self-position estimation accuracy decreases. There are many situations where errors occur in the output of the measurement sensor, such as when there are many objects with high reflection strength, such as line-of-sight guides and reflectors of other vehicles, and they are mistakenly detected as target features. There are cases where there is another vehicle between the vehicle and the target feature, and all or part of the scan by the measurement sensor is occluded. In addition, as a situation where an error occurs in the map, there is a case where the latest information is not reflected on the map and the position coordinates of the feature are inaccurate.

The present invention has been made to solve the above-described problems, and suitably suppresses a decrease in self-position estimation accuracy even when an error occurs in either a map or a measurement value. It is a main object to provide a self-position estimation apparatus capable of performing the above.

The invention according to claim 1 is a self-position estimation device, and shows a measurement distance by a first acquisition unit that acquires predicted position information indicating a predicted self-position and a measurement unit from a moving object to an object. A second acquisition unit configured to acquire first distance information and second distance information indicating a distance from the moving object to the object predicted based on position information of the object; and self-position accuracy of the moving object A third acquisition unit that acquires information; a correction unit that corrects the predicted self-position based on the difference value between the distances indicated by the first distance information and the second distance information; and the self-position accuracy information; .

The invention according to claim 7 is a control method executed by the self-position estimation device, the first obtaining step for obtaining the predicted position information indicating the predicted self-position, and the measurement unit from the moving body to the object A second acquisition step of acquiring first distance information indicating a measurement distance according to the first object, and second distance information indicating a distance from the moving body to the object predicted based on position information of the object; A third acquisition step of acquiring self-position accuracy information of the body, a distance difference value indicated by the first distance information and the second distance information, and the predicted self-position based on the self-position accuracy information And a correction step for correcting.

The invention according to claim 8 is a program executed by a computer, and includes a first acquisition unit that acquires predicted position information indicating a predicted self-position, and a measurement distance by a measurement unit from a moving object to an object. A second acquisition unit that acquires first distance information that indicates and second distance information that indicates a distance from the moving object to the object predicted based on position information of the object; and a self-position of the moving object A third acquisition unit that acquires accuracy information, and a correction unit that corrects the predicted self-position based on the difference value between the distances indicated by the first distance information and the second distance information, and the self-position accuracy information To make the computer function.

It is a schematic block diagram of a driving assistance system. It is a block diagram which shows the functional structure of a vehicle equipment. It is an example of the data structure of map DB. It is the figure which represented the state variable vector by the two-dimensional orthogonal coordinate. It is a figure which shows the schematic relationship between a prediction step and a measurement update step. The functional block of the own vehicle position estimation part is shown. The positional relationship between the estimated vehicle position of the vehicle and the feature is shown. It is a flowchart of the own vehicle position estimation process. The experimental result of the own vehicle position estimation process based on a present Example is shown. The experimental result of the own vehicle position estimation process based on a present Example is shown. The experimental result of the conventional own vehicle position estimation process at the time of shifting the position coordinate of the feature information of a certain feature by 80 cm in the traveling direction is shown. The experiment result of the own vehicle position estimation process based on the Example when the position coordinates of the feature information of a certain feature are shifted by 80 cm in the traveling direction is shown.

According to a preferred embodiment of the present invention, the self-position estimation device indicates a measurement distance from a first acquisition unit that acquires predicted position information indicating a predicted self-position and a measurement unit from the moving body to the object. A second acquisition unit configured to acquire first distance information and second distance information indicating a distance from the moving object to the object predicted based on position information of the object; and self-position accuracy of the moving object A third acquisition unit that acquires information; a correction unit that corrects the predicted self-position based on the difference value between the distances indicated by the first distance information and the second distance information; and the self-position accuracy information; . According to this aspect, the self-position estimation device can suitably correct the predicted self-position based on the distance difference value indicated by the first distance information and the second distance information and the self-position accuracy information. .

In one aspect of the self-position estimation apparatus, the correction unit corrects the predicted self-position based on the difference value, the self-position accuracy information, and the measurement accuracy information of the measurement unit. In this aspect, the self-position estimating apparatus can appropriately correct the self-position by further considering the measurement accuracy of the measurement unit.

In another aspect of the self-position estimation device, the correction unit calculates an evaluation value for evaluating the difference value based on the difference value and the self-position accuracy information, and calculates the predicted self-position. The degree of correction based on the difference value is determined based on the evaluation value. According to this aspect, the self-position estimation device can accurately determine the degree at which the predicted self-position is corrected by the above-described difference value by evaluating the above-described difference value based on the self-position accuracy information. it can. Thereby, even if it is a case where an error exists in the 1st distance information or the 2nd distance information, it can control suitably that self-vehicle position estimation accuracy falls. In this case, preferably, the correction unit corrects the predicted self-position by a value obtained by multiplying the difference value by a predetermined gain, and the correction unit corrects the gain based on the evaluation value. The correction coefficient may be determined. More preferably, the gain is a Kalman gain.

In another aspect of the self-position estimation apparatus, the second acquisition unit may calculate a distance indicated by the second distance information based on the predicted position information and the position information of the object recorded in map information. calculate. According to this aspect, the self-position estimation device can generate the second distance information based on the map information and can be suitably used for self-position estimation.

According to another preferred embodiment of the present invention, there is provided a control method executed by the self-position estimating device, the first obtaining step of obtaining predicted position information indicating the predicted self-position, and the object from the moving object. The second acquisition step of acquiring the first distance information indicating the measurement distance by the measuring unit until and the second distance information indicating the distance from the moving body to the object predicted based on the position information of the object A third acquisition step of acquiring the self-position accuracy information of the mobile body, the difference value between the distances indicated by the first distance information and the second distance information, and the prediction based on the self-position accuracy information. And a correction step for correcting the self-position. The self-position estimating apparatus can suitably correct the predicted self-position by executing this control method.

According to another preferred embodiment of the present invention, a program executed by a computer, a first acquisition unit that acquires predicted position information indicating a predicted self-position, and a measurement unit from a moving object to an object A second acquisition unit that acquires first distance information indicating a measured distance by the second object and second distance information indicating a distance from the moving object to the object predicted based on position information of the object; A third acquisition unit for acquiring self-position accuracy information of the body; a distance difference value indicated by the first distance information and the second distance information; and the predicted self-position based on the self-position accuracy information. The computer is caused to function as a correction unit for correction. The computer can suitably correct the predicted self-position by executing this program. Preferably, the program is stored in a storage medium.

Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For convenience, a character with “^” or “-” attached on an arbitrary symbol is represented as “A ^{^} ” or “A ⁻ ” (“A” is an arbitrary character) in this specification.
[Schematic configuration]

FIG. 1 is a schematic configuration diagram of a driving support system according to the present embodiment. The driving support system shown in FIG. 1 is mounted on a vehicle and has an in-vehicle device 1 that performs control related to driving support of the vehicle, a lidar (Lidar: Light Detection and Ranging, or Laser Illuminated Detection And Ranging) 2, and a gyro sensor 3. And a vehicle speed sensor 4 and a GPS receiver 5.

The in-vehicle device 1 is electrically connected to the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and based on these outputs, the position of the vehicle on which the in-vehicle device 1 is mounted ("own vehicle position"). Also called.) And the vehicle equipment 1 performs automatic driving | operation control etc. of a vehicle so that it drive | works along the path | route to the set destination based on the estimation result of the own vehicle position. The in-vehicle device 1 stores a map database (DB: DataBase) 10 that stores road data and feature information that is information about a feature that is a landmark provided near the road. The features that serve as the above-mentioned landmarks are, for example, features such as kilometer posts, 100 m posts, delineators, traffic infrastructure facilities (for example, signs, direction signs, signals), utility poles, street lamps, and the like that are periodically arranged on the side of the road. And the vehicle equipment 1 estimates the own vehicle position by making it collate with the output of the lidar 2 etc. based on this feature information. The in-vehicle device 1 is an example of the “self-position estimation device” in the present invention.

The lidar 2 emits a pulse laser in a predetermined angle range in the horizontal direction and the vertical direction, thereby discretely measuring the distance to an object existing in the outside world, and a three-dimensional point indicating the position of the object Generate group information. In this case, the lidar 2 includes an irradiation unit that emits laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and scan data based on a light reception signal output by the light receiving unit. Output unit. The scan data is generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the response delay time of the laser beam specified based on the above-described received light signal. Generally, the accuracy of the lidar distance measurement value is higher as the distance to the object is shorter, and the accuracy is lower as the distance is longer. The rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the in-vehicle device 1. The lidar 2 is an example of the “measurement unit” in the present invention.

FIG. 2 is a block diagram showing a functional configuration of the in-vehicle device 1. The in-vehicle device 1 mainly includes an interface 11, a storage unit 12, an input unit 14, a control unit 15, and an information output unit 16. Each of these elements is connected to each other via a bus line.

The interface 11 acquires output data from sensors such as the lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies the output data to the control unit 15.

The storage unit 12 stores a program executed by the control unit 15 and information necessary for the control unit 15 to execute a predetermined process. In a present Example, the memory | storage part 12 memorize | stores map DB10 containing feature information. FIG. 3 shows an example of the data structure of the map DB 10. As shown in FIG. 3, the map DB 10 includes facility information, road data, and feature information. The feature information is information in which information related to the feature is associated with each feature, and is represented by a feature ID corresponding to the feature index, latitude and longitude (and altitude), and the like. And at least position information indicating the absolute position of the feature. The map DB 10 may be updated regularly. In this case, for example, the control unit 15 receives partial map information related to the area to which the vehicle position belongs from a server device that manages the map information via a communication unit (not shown), and reflects it in the map DB 10.

The input unit 14 is a button for operation by the user, a touch panel, a remote controller, a voice input device, or the like. The information output unit 16 is, for example, a display or a speaker that outputs based on the control of the control unit 15.

The control unit 15 includes a CPU that executes a program and controls the entire vehicle-mounted device 1. In the present embodiment, the control unit 15 includes a host vehicle position estimation unit 17. The control unit 15 is an example of a “first acquisition unit”, “second acquisition unit”, “third acquisition unit”, “correction unit”, and “computer” that executes a program in the present invention.

The own vehicle position estimation unit 17 is based on the distance and angle measurement values by the lidar 2 for the feature and the position information of the feature extracted from the map DB 10, and the gyro sensor 3, the vehicle speed sensor 4, and / or the GPS receiver. The vehicle position estimated from the output data of 5 is corrected. In the present embodiment, as an example, the vehicle position estimation unit 17 estimates a vehicle position from output data from the gyro sensor 3 and the vehicle speed sensor 4 based on a state estimation method based on Bayesian estimation, The measurement update step for correcting the estimated value of the vehicle position calculated in the prediction step is executed alternately. Various filters developed to perform Bayesian estimation can be used as the state estimation filter used in these steps, and examples thereof include an extended Kalman filter, an unscented Kalman filter, and a particle filter. As described above, various methods have been proposed for position estimation based on Bayesian estimation. In the following, vehicle position estimation using an extended Kalman filter will be briefly described as an example.

FIG. 4 is a diagram showing the state variable vector x in two-dimensional orthogonal coordinates. As shown in FIG. 4, the vehicle position on a plane defined on the two-dimensional orthogonal coordinates of xy is represented by coordinates “(x, y)” and the direction “Ψ” of the vehicle. Here, the direction Ψ is defined as an angle formed by the traveling direction of the vehicle and the x axis. The coordinates (x, y) indicate an absolute position corresponding to a combination of latitude and longitude, for example.

FIG. 5 is a diagram illustrating a schematic relationship between the prediction step and the measurement update step. FIG. 6 shows an example of a functional block of the vehicle position estimation unit 17. As shown in FIG. 5, by repeating the prediction step and the measurement update step, calculation and update of the estimated value of the state variable vector “X” indicating the vehicle position are sequentially executed. Moreover, as shown in FIG. 6, the own vehicle position estimation part 17 has the position estimation part 21 which performs a prediction step, and the position estimation part 22 which performs a measurement update step. The position prediction unit 21 includes a dead reckoning block 23 and a position prediction block 24, and the position estimation unit 22 includes a feature search / extraction block 25 and a position correction block 26. In FIG. 5, the state variable vector of the reference time (ie, current time) “t” to be calculated is represented as “X ⁻ (t)” or “X ^{^} (t)” (“state variable Vector X (t) = (denoted as x (t), y (t), Ψ (t)) ^T ”). Here, the provisional estimated value (predicted value) estimated in the predicting step is appended with “ ^- ” on the character representing the predicted value, and the estimated value with higher accuracy updated in the measurement updating step. Is appended with “ ^{^} ” on the character representing the value.

In the prediction step, the dead reckoning block 23 of the vehicle position estimation unit 17 moves the vehicle moving speed “v” and the angular velocity “ω” (collectively, “control value u (t) = (v (t), ω ( t)) ^T ”is used to determine the movement distance and azimuth change from the previous time. The position prediction block 24 of the vehicle position estimation unit 17 adds the obtained moving distance and azimuth change to the state variable vector X ^{^} (t-1) at time t-1 calculated in the immediately preceding measurement update step. Then, the predicted value of the vehicle position at time t (also referred to as “predicted vehicle position”) X ⁻ (t) is calculated. At the same time, the covariance matrix “P ⁻ (t)” corresponding to the error distribution of the predicted vehicle position X ⁻ (t) is converted into the covariance at time t−1 calculated in the immediately preceding measurement update step. It is calculated from the matrix “P ^{^} (t−1)”.

In the measurement update step, the feature search / extraction block 25 of the vehicle position estimation unit 17 associates the position vector of the feature registered in the map DB 10 with the scan data of the lidar 2. Then, the feature search / extraction block 25 of the vehicle position estimation unit 17 calls the measured value (hereinafter referred to as “feature measurement value”) by the lidar 2 of the feature that can be associated when the association is made. ) Feature estimation value obtained by modeling the measurement process by the lidar 2 using “Z (t)”, the predicted vehicle position X ⁻ (t) and the location vector of the feature registered in the map DB 10. (Referred to as “feature predicted value”) “Z ⁻ (t)” is acquired. The feature measurement value Z (t) is a two-dimensional vehicle body coordinate system obtained by converting the feature distance and scan angle measured by the rider 2 at time t into components with the vehicle traveling direction and the lateral direction as axes. Is a vector. Then, the position correction block 26 of the vehicle position estimation unit 17 calculates a difference value between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) as shown in the following equation (1). To do.

Further, the position correction block 26 of the vehicle position estimation unit 17 calculates the Kalman to the difference value between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) as shown in the following equation (2). By multiplying the gain “K (t)” and adding this to the predicted own vehicle position X ⁻ (t), the updated state variable vector (also referred to as “estimated own vehicle position”) X ^{^} (t) calculate.

Further, in the measurement update step, the position correction block 26 of the vehicle position estimating section 17, similarly to the prediction step, the covariance matrix corresponding to the error distribution of the estimated vehicle position ^{^{X ^ (t) P ^ (}} t) ( simply P (t)) is obtained from the covariance matrix P ⁻ (t). Parameters such as the Kalman gain K (t) can be calculated in the same manner as a known self-position estimation technique using an extended Kalman filter, for example.

In addition, the own vehicle position estimation unit 17 can select any one selected ground when the position vector registered in the map DB 10 and the scan data of the lidar 2 can be associated with a plurality of features. The measurement update step may be performed based on the object measurement value or the like, or the measurement update step may be performed a plurality of times based on all the feature measurement values or the like that can be associated. When using a plurality of feature measurement values or the like, the vehicle position estimation unit 17 considers that the rider measurement accuracy deteriorates as the feature farther from the rider 2, and the distance between the rider 2 and the feature becomes smaller. The longer it is, the smaller the weight related to the feature is.

In this way, the prediction step and the measurement update step are repeatedly performed, and the predicted vehicle position X ⁻ (t) and the estimated vehicle position X ^{^} (t) are sequentially calculated, so that the most likely vehicle position Is calculated.

In the above description, the feature measurement value Z (t) is an example of the “first distance information” of the present invention, and the predicted feature value Z ⁻ (t) of the “second distance information” of the present invention. It is an example.

[Details of vehicle position estimation]
Next, details of the vehicle position estimation in the present embodiment will be described. Schematically, the vehicle position estimation unit 17 evaluates a difference value between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) (also referred to as “difference evaluation value”). And the Kalman gain K (t) is corrected according to the difference evaluation value. Thereby, the own vehicle position estimation part 17 suppresses the fall of the own vehicle position estimation precision suitably even if it is a case where an error arises in either the map DB10 or the measured value of the lidar 2.

In EKF (Extended Karman Filter) position estimation by road signs using a lidar, as shown in the above equation (2), the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) The vehicle position is calculated using the difference value. As understood from the equation (2), when the difference value shown in the equation (1) increases, the correction amount for the predicted host vehicle position X ⁻ (t) increases. Here, the difference value becomes large due to one of the following causes (A) to (C).

(A) There is an error in the feature measurement value.
For example, if the measurement accuracy of the lidar is low, or the measurement accuracy of the lidar is not low, there is a moving object such as another vehicle between the measurement target feature and the vehicle at the time of measurement. Or the like becomes low (that is, when occlusion occurs).

(B) The estimated vehicle position used for obtaining the feature prediction value is shifted.
This is a case where the error of the predicted own vehicle position X ⁻ (t) or the estimated own vehicle position X ^{^} (t) is large.

(C) The position coordinates of the feature of the map data used for obtaining the feature prediction value are shifted.
This is because, for example, after the map data is created, the sign of the feature is tilted due to the collision of the vehicle, the structure of the feature is removed or moved, and the position of the feature is changed. This is a state in which the position coordinates of the feature in the data do not match the actual position coordinates of the feature. That is, there is a difference between the position of the feature on the map data and the position of the feature in the real environment.

If the estimated vehicle position X ^{^} (t) is calculated based on the equation (2) when the difference value is large due to the above (A) or (C), the vehicle position estimation accuracy is lowered. Therefore, when either of the above (A) or (C) occurs, it is preferable to make the degree of correction with respect to the predicted host vehicle position X ⁻ (t) as low as possible.

FIG. 7 shows the positional relationship between the estimated vehicle position of the vehicle and the feature. The coordinate system shown in FIG. 7 is a body coordinate system based on the vehicle, the x-axis indicates the traveling direction of the vehicle, and the y-axis indicates the direction perpendicular to the traveling direction (lateral direction of the vehicle). In FIG. 7, the estimated vehicle position has an error range indicated by an error ellipse 40. The error ellipse 40 is defined by the position estimation accuracy “σ _P (x)” in the x direction and the position estimation accuracy “σ _P (y)” in the y direction. Here, the position estimation accuracy σ _P is obtained by converting the covariance matrix P (t) into the body coordinate system using the Jacobian matrix “H (t)” by the following equation (3).

In Expression (3), “σ _P (x) ² ” is the variance in the x direction, and “σ _P (y) ² ” is the variance in the y direction, and the position estimation accuracy in the x direction σ _P (x ) And y-direction position estimation accuracy σ _P (y). Thus, the position estimation accuracy σ _P (σ _P (x), σ _P (y)) is obtained.

In FIG. 7, the predicted feature value Z ⁻ (t) is obtained by modeling the measurement process by the lidar 2 using the predicted vehicle position X ⁻ (t) and the position vector of the feature registered in the map DB 10. Is. The feature measurement value Z (t) is a measurement value obtained by the rider 2 of the feature when the feature position vector registered in the map DB 10 can be associated with the scan data of the rider 2. The feature measurement value Z (t) has an error range indicated by an error ellipse 43. The error ellipse 43 is defined by the measurement accuracy “σ _L (x)” in the x direction and the measurement accuracy “σ _L (y)” in the y direction. In general, the measurement error due to the rider increases in proportion to the square of the distance between the rider and the feature to be measured, and therefore, based on the distance from the estimated vehicle position to the feature measurement value Z (t). By calculating the error ellipse 43, the lidar measurement accuracy σ _L (σ _L (x), σ _L (y)) is obtained. Further, the difference value between the predicted feature value Z ⁻ (t) and the measured feature value Z (t) is “dx” in the x direction and “dy” in the y direction, as shown in the equation (1).

The position estimation accuracy σ _P is an example of “self-position accuracy information” of the present invention, and the lidar measurement accuracy σ _L is an example of “measurement accuracy information” of the present invention.

Next, an evaluation value calculation method for evaluating the difference value between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) will be described. The own vehicle position estimation unit 17 uses the position estimation accuracy σ _P (σ _P (x), σ _P (y)) and the lidar measurement accuracy σ _L (σ _L (x), σ _L (y)), The difference value between the feature measurement value Z (t) and the feature prediction value Z ⁻ (t) is evaluated. In other words, the vehicle position estimation unit 17 determines the validity of the above-described difference value based on the ratio of the difference value with respect to the error range 40 and the lidar error range 43 of the estimated vehicle position. Specifically, the vehicle position estimation unit 17 calculates the difference evaluation values “Ex” and “Ey” using the following evaluation formula (4). In this case, absolute values of the difference values dx and dy are used.

In this case, it is estimated that the higher the difference evaluation values Ex and Ey, the higher the possibility that either (A) or (C) has occurred. Therefore, if at least one of the difference evaluation values Ex and Ey is larger than the predetermined value, the host vehicle position estimation unit 17 determines that the above (A) or (C) has occurred as a cause of the difference value being increased. . Therefore, in this case, the own vehicle position estimation unit 17 calculates coefficient values “a _X (t)” and “a _Y (t)” for reducing the Kalman gain according to the difference evaluation values Ex and Ey. Multiply by Kalman gain. In this case, the coefficient values a _X (t) and a _Y (t) are values from 0 to 1, and the smaller the evaluation values Ex and Ey, the smaller the values, using an expression or a table. Is set. For example, the coefficient values a _X (t) and a _Y (t) are set based on the following equation (5).

Then, the vehicle position estimation unit 17 calculates the updated Kalman gain “K (t) ′” based on the following equation (6) using the coefficient values a _X (t) and a _Y (t). .

By setting the Kalman gain in this way, when the reliability of the predicted feature value is low, the vehicle position estimation unit 17 reduces the Kalman gain according to the reliability and predicts the vehicle position X ⁻ ( The correction amount for t) can be reduced. Therefore, in this case, inaccurate correction can be prevented, so that the estimated vehicle estimation accuracy is improved. The coefficient values a _X (t) and a _Y (t) are examples of the “correction coefficient” in the present invention.

FIG. 8 is a flowchart of the vehicle position estimation process performed by the vehicle position estimation unit 17 of the in-vehicle device 1. The in-vehicle device 1 repeatedly executes the process of the flowchart of FIG.

First, the vehicle position estimation unit 17 sets an initial value of the vehicle position based on the output of the GPS receiver 5 or the like (step S101). Next, the host vehicle position estimation unit 17 acquires the vehicle body speed from the vehicle speed sensor 4 and the angular velocity in the yaw direction from the gyro sensor 3 (step S102). And the own vehicle position estimation part 17 calculates the moving distance of a vehicle and the azimuth | direction change of a vehicle based on the acquisition result of step S102 (step S103).

Then, the vehicle position estimating section 17, one time before the estimated vehicle position X ^{^ (t-1),} by adding the moving distance and direction change calculated in step S103, the predicted vehicle position X ^- (t) Is calculated (step S104). Furthermore, in step S104, the host vehicle position estimation unit 17 obtains position estimation accuracy σ _P (x), σ _P (y) from the covariance matrix using Equation (3). Further, the own vehicle position estimating unit 17 refers to the feature information in the map DB 10 based on the predicted own vehicle position X ⁻ (t), and searches for the feature that is the measurement range of the rider 2 (step S105).

Then, the vehicle position estimation unit 17 predicts the features in the traveling direction and the lateral direction of the vehicle from the predicted vehicle position X ⁻ (t) and the position coordinates indicated by the feature information of the feature searched in step S105. The position (that is, the predicted feature value Z ⁻ (t)) is calculated (step S106). Thereafter, the vehicle position estimation unit 17 calculates a measurement distance (that is, a feature measurement value Z (t)) from the scan data of the rider 2 to each feature in the traveling direction and the lateral direction of the vehicle. Rider measurement accuracy σ _L (x), σ _L (y) is calculated (step S107).

Next, the vehicle position estimation unit 17 calculates difference values dx and dy between the feature measurement value and the feature prediction value based on the equation (1) (step S108). And the own vehicle position estimation part 17 calculates difference evaluation value Ex and Ey based on Formula (4) (step S109).

Then, when the difference evaluation values Ex and Ey are larger than the predetermined values (step S110; Yes), the vehicle position estimation unit 17 obtains the coefficient values a _X (t) and a _Y (t) from the difference evaluation values Ex and Ey. Generate (step S111). For example, the vehicle position estimation unit 17 calculates the coefficient values a _X (t) and a _Y (t) based on the equation (5), so that the difference evaluation values Ex and Ey are in the range from 0 to 1. The coefficient values a _X (t) and a _Y (t) are calculated such that the larger the value, the smaller the value. When there is a difference evaluation value that does not exceed the predetermined value, the vehicle position estimation unit 17 determines a coefficient value corresponding to the difference evaluation value (in the case of the difference evaluation value Ex, the coefficient value a _X (t )) May be set to 1 regardless of the equation (5).

On the other hand, the vehicle position estimation unit 17 sets the coefficient values a _X (t) and a _Y (t) to “1” when the difference evaluation values Ex and Ey are equal to or smaller than the predetermined values (step S110; No) ( Step S112). That is, the coefficient value a _X (t) is set by comparing the difference evaluation value Ex and a predetermined value, and the coefficient value a _Y (t) is set by comparing the difference evaluation value Ey and the predetermined value.

Next, the vehicle position estimation unit 17 multiplies the Kalman gain K (t) by the coefficient values a _X (t) and a _Y (t) based on the equation (6), thereby obtaining the Kalman gain K (t) ′. Is generated (step S113). Thereafter, the host vehicle position estimation unit 17 corrects the predicted host vehicle position X ⁻ (t) using the generated Kalman gain K (t) ′ instead of K (t) based on the equation (2), and estimates The own vehicle position X ^{^} (t) is calculated (step S114).

[Concrete example]
Next, the effect of the own vehicle position estimation process based on a present Example is demonstrated.

FIG. 9 (A) is a graph showing the transition of the position in the x direction and the y direction when the vehicle is running while executing the vehicle position estimation process shown in FIG. 8 in a certain driving test course. In FIG. 9A, the vehicle position (also referred to as “reference position”) measured with high accuracy using RTK-GPS and the estimated vehicle position calculated by the vehicle position estimation process shown in FIG. Are drawn to the extent that they cannot be distinguished visually.

FIG. 9B is an enlarged view of data in the travel section in the frame 70 of FIG. 9A. In FIG. 9B, the positions of the features that are the measurement targets are plotted with circles. 10A is a graph showing the transition of the difference value dx in the travel section of the frame 70, and FIG. 10B shows the estimated vehicle position based on the embodiment in the travel section of the frame 70. FIG. 10C illustrates the difference in the lateral direction between the estimated vehicle position based on the embodiment in the traveling section of the frame 70 and the reference position, and FIG. These show the azimuth | direction difference of the estimated own vehicle position based on the Example in the driving | running | working area of the frame 70, and a reference position.

As shown in FIG. 9B, the vehicle position estimation unit 17 estimates the estimated vehicle position based on the feature information for features that exist at irregular intervals, and FIG. The predicted vehicle position is corrected based on the difference value shown in FIG. As a result, as shown in FIGS. 10B to 10D, the difference between the reference position in the traveling direction and the lateral direction of the vehicle and the estimated own vehicle position is within about 0.1 m. The difference is also within about 0.1 degree.

11A to 11D show the Kalman gain K (t) as a coefficient value a when the position coordinates of the feature information with respect to the feature indicated by the plot 71 in FIG. 9B are shifted by 80 cm in the traveling direction. Experimental results similar to those shown in FIGS. 10A to 10D when used without correction by _X (t) and a _Y (t) are shown.

As shown in FIG. 11A, the position coordinates of the feature information of the feature indicated by the plot 71 are derived from the shift in the position coordinates of the feature information with respect to the feature indicated by the plot 71 of FIG. 9B. The difference value dx calculated using the reference (see the round frame 72) has a significantly large deviation compared to the difference value dx calculated using the position coordinates of the feature information of other features. As a result, as shown in FIG. 11B, the estimated own vehicle position calculated using the position coordinates of the feature information with respect to the feature indicated by the plot 71 in FIG. 9B is the reference position in the traveling direction of the vehicle. (See a round frame 73). The error of the estimated own vehicle position generated in this way continues until the next feature used for the own vehicle position estimation (see the plot 74 in FIG. 9) is detected.

12A to 12D show the Kalman gain K (t) as the coefficient value a when the position coordinates of the feature information with respect to the feature indicated by the plot 71 in FIG. 9B are shifted by 80 cm in the traveling direction. Experimental results similar to those in FIGS. 11A to 11D when corrected by _X (t) and a _Y (t) are shown.

In this case, the difference value dx, the position estimation accuracy σ _P (x), and the lidar measurement accuracy σ _L (x) when the feature indicated by the plot 71 in FIG. 9B is detected are as follows.
dx = −0.749
σ _P (x) = 0.048
σ _L (x) = 0.058

Therefore, when these values are substituted into Equation (5) the coefficient value _a X (t) shown in, the coefficient value _a X (t) is 0.854 × ^{10 -3} ≒ 0
It becomes. In this case, the Kalman gain K (t) ′ is obtained as shown in the following equation (7).

Therefore, in this case, since the predicted vehicle position X ⁻ (t) is not corrected in the traveling direction of the vehicle, accurate position estimation is maintained.

As described above, the vehicle position estimation unit 17 of the vehicle-mounted device 1 according to the present embodiment includes the position prediction unit 21 that generates information indicating the predicted vehicle position X ⁻ (t), and the position estimation unit 22. Have. The position estimation unit 22 calculates a feature measurement value Z (t) indicating the measurement distance by the lidar 2 from the moving body to the target object and a feature prediction value Z ⁻ (t) predicted based on the feature information. Then, the position estimation unit 22 responds to the difference evaluation values Ex and Ey based on the difference values dx and dy between the feature measurement value and the feature prediction value, the position estimation accuracy σ _P, and the lidar measurement accuracy σ _L. The coefficient values a _X (t) and a _Y (t) for correcting the Kalman gain K (t) are determined. Thereby, the vehicle equipment 1 can suppress suitably the fall of the own vehicle position estimation precision even if it is a case where an error arises in either the map DB10 or the measured value of the lidar 2.

[Modification]
Hereinafter, modified examples suitable for the embodiments will be described. The following modifications may be applied to the embodiments in combination.

(Modification 1)
The method of reducing the correction amount of the predicted vehicle position X ⁻ (t) based on the feature information determined to have low reliability is to use the coefficient values a _X (t) and a _Y (t) as the Kalman gain K (t ) Is not limited.

Instead, in the first example, the vehicle position estimation unit 17 may multiply the difference values dx and dy by coefficient values a _X (t) and a _Y (t). That is, in this case, the host vehicle position estimation unit 17 calculates the estimated host vehicle position based on the following equation (8).

In this case, since the Kalman gain K (t) is generated by the equation (10) and is not updated based on the equation (6), the covariance matrix P ( The update of t) does not reflect the use of feature information with low reliability. That is, while the position estimation reflects the reliability, the covariance matrix does not reflect the reliability, and thus processing that considers the difference is necessary.

In the second example, the vehicle position estimation unit 17 uses the diagonal component of the observation noise matrix “R (t)” used when calculating the Kalman gain K (t) by the following general formula (10) as follows: As shown in equation (11), coefficient values a _X (t) and a _Y (t) may be multiplied.

In this case, the vehicle position estimation unit 17 sets the coefficient values a _X (t) and a _Y (t) so that the coefficient values a _X (t) and a _Y (t) become larger as the evaluation values Ex and Ey shown in Expression (4) are larger. For example, the vehicle position estimation unit 17 sets the coefficient values a _X (t) and a _Y (t) as shown in the following formula (12).

In this case, since the possible range of the coefficient values a _X (t) and a _Y (t) is from 1 to infinity, a process for avoiding overflow, such as setting an upper limit value, is required.

(Modification 2)
The configuration of the driving support system shown in FIG. 1 is an example, and the configuration of the driving support system to which the present invention is applicable is not limited to the configuration shown in FIG. For example, in the driving support system, instead of having the in-vehicle device 1, the electronic control device of the vehicle may execute the processing of the vehicle position estimation unit 17 of the in-vehicle device 1. In this case, map DB10 is memorize | stored in the memory | storage part in a vehicle, for example, and the electronic control apparatus of a vehicle may receive the update information of map DB10 from the server apparatus which is not shown in figure.

DESCRIPTION OF SYMBOLS 1 In-vehicle apparatus 2 Rider 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims

A first acquisition unit that acquires predicted position information indicating the predicted self-position;
Acquires first distance information indicating a distance measured by the measurement unit from the moving object to the object, and second distance information indicating a distance from the moving object to the object predicted based on the position information of the object. A second acquisition unit to perform,
A third acquisition unit for acquiring self-position accuracy information of the moving body;
A correction unit that corrects the predicted self-position based on the difference value of the distance indicated by the first distance information and the second distance information, and the self-position accuracy information;
A self-position estimation apparatus comprising:
The self-position estimation apparatus according to claim 1, wherein the correction unit corrects the predicted self-position based on the difference value, the self-position accuracy information, and measurement accuracy information of the measurement unit.
The correction unit calculates an evaluation value for evaluating the difference value based on the difference value and the self-position accuracy information, and sets the degree of correction of the predicted self-position by the difference value as the evaluation value. The self-position estimation apparatus according to claim 1, wherein the self-position estimation apparatus is determined based on the determination.
The correction unit corrects the predicted self-position by a value obtained by multiplying the difference value by a predetermined gain,
The self-position estimating apparatus according to claim 3, wherein the correction unit determines a correction coefficient for the gain based on the evaluation value.
The self-position estimation apparatus according to claim 4, wherein the gain is a Kalman gain.
6. The second acquisition unit according to claim 1, wherein the second acquisition unit calculates a distance indicated by the second distance information based on the predicted position information and position information of the object recorded in map information. The self-position estimation apparatus described in 1.
A control method executed by the self-position estimation apparatus,
A first acquisition step of acquiring predicted position information indicating the predicted self-position;
First distance information indicating a distance measured by a measurement unit from a moving object to an object and second distance information indicating a distance from the moving object to the object predicted based on position information of the object are acquired. A second acquisition step,
A third acquisition step of acquiring self-position accuracy information of the moving body;
A correction step of correcting the predicted self-position based on a difference value between distances indicated by the first distance information and the second distance information, and the self-position accuracy information;
A control method.
A program executed by a computer,
A first acquisition unit that acquires predicted position information indicating the predicted self-position;
Acquires first distance information indicating a distance measured by the measurement unit from the moving object to the object, and second distance information indicating a distance from the moving object to the object predicted based on the position information of the object. A second acquisition unit to perform,
A third acquisition unit for acquiring self-position accuracy information of the moving body;
A program that causes the computer to function as a correction unit that corrects the predicted self-position based on a difference value between distances indicated by the first distance information and the second distance information and the self-position accuracy information.
A storage medium storing the program according to claim 8.