Self-position estimation device, control method, program, and storage medium
The self-position estimating device corrects predicted positions using distance measurements and accuracy information to maintain accuracy despite errors in sensors or map data, enhancing vehicle positioning precision.
Patent Information
- Application Number
- JP2024145609
- Authority / Receiving Office
- JP · JP
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2017-05-19
- Filing Date
- 2024-08-27
- Publication Date
- 2024-11-15
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
Existing self-position estimation methods suffer from inaccuracies due to errors in measurement sensors or map data, particularly when there are high reflection objects, occlusions, or outdated map information, leading to decreased accuracy.
A self-position estimating device that corrects predicted self-position using distance measurements from a moving object to a target object, considering self-position accuracy information and measurement accuracy, employing a correction unit to adjust the predicted position based on a difference value and Kalman gain.
The device effectively maintains self-position estimation accuracy by correcting for errors in map or measurement data, ensuring precise vehicle positioning even in challenging conditions.
Smart Images

Figure 2024161130000001_ABST
Abstract
Description
[Technical field]
[0001] The present invention relates to a self-location estimation technology. [Background technology]
[0002] Conventionally, there has been known a technique for detecting features installed in the vehicle's travel direction using a radar or a camera, and calibrating the vehicle's position based on the detection result. For example, Patent Document 1 discloses a technique for estimating the vehicle's position by matching the output of a measurement sensor with position information of features registered in advance on a map. Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter. [Prior art documents] [Patent documents]
[0003] [Patent Document 1] JP 2013-257742 A [Patent Document 2] JP 2017-72422 A Summary of the Invention [Problem to be solved by the invention]
[0004] When estimating the self-location by comparing the output of a measurement sensor with the position information of a feature registered in advance on a map, if there is an error in either the map or the output of the measurement sensor, the self-location is erroneously corrected, and the accuracy of the self-location estimation decreases. Situations in which errors occur in the output of the measurement sensor include a case where there are many objects with high reflective strength, such as delineators and reflectors of other vehicles, which are mistakenly detected as the target feature, or a case 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. Situations in which errors occur in the map include a case where the latest information is not reflected on the map, and the position coordinates of the feature are inaccurate.
[0005] The present invention has been made to solve the above-mentioned problems, and has as its main object to provide a self-location estimation device that can effectively suppress a decrease in the accuracy of self-location estimation even when an error occurs in either the map or the measurement values. [Means for solving the problem]
[0006] The invention described in claim 1 is a self-location estimation device comprising a first acquisition unit that acquires predicted location information indicating a predicted self-location, a second acquisition unit that acquires first distance information indicating a distance measured by a measurement unit from a moving body to an object and second distance information indicating a distance from the moving body to the object predicted based on location information of the object, a third acquisition unit that acquires self-location accuracy information of the moving body, and a correction unit that corrects the predicted self-location based on the first distance information, the second distance information, and the self-location accuracy information. [Brief description of the drawings]
[0007] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Diagram 2] FIG. 2 is a block diagram showing a functional configuration of an in-vehicle device. [Diagram 3] 4 is an example of a data structure of a map DB. [Figure 4] This is a diagram showing state variable vectors in two-dimensional orthogonal coordinates. [Diagram 5] FIG. 1 illustrates a schematic relationship between a prediction step and a measurement update step. [Figure 6] 3 shows functional blocks of a vehicle position estimation unit. [Figure 7] This shows the positional relationship between the vehicle's estimated position and features. [Figure 8] 4 is a flowchart of a vehicle position estimation process. [Figure 9] 4 shows experimental results of the vehicle position estimation process based on this embodiment. [Figure 10] 4 shows experimental results of the vehicle position estimation process based on this embodiment. [Figure 11]The following shows the experimental results of a conventional vehicle position estimation process when the position coordinates of the feature information of a certain feature are shifted by 80 cm in the direction of travel. [Figure 12] 13 shows the experimental results of the vehicle position estimation process based on the embodiment when the position coordinates of the feature information of a certain feature are shifted by 80 cm in the traveling direction. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0008] According to a preferred embodiment of the present invention, a self-location estimation device includes a first acquisition unit that acquires predicted location information indicating a predicted self-location, a second acquisition unit that acquires first distance information indicating a distance measured by a measurement unit from a moving body to an object and second distance information indicating a distance from the moving body to the object predicted based on the position information of the object, a third acquisition unit that acquires self-location accuracy information of the moving body, and a correction unit that corrects the predicted self-location based on a difference value between the distances indicated by the first distance information and the second distance information and the self-location accuracy information. According to this aspect, the self-location estimation device can preferably correct the predicted self-location based on the difference value between the distances indicated by the first distance information and the second distance information and the self-location accuracy information.
[0009] In one aspect of the self-location estimation device, the correction unit corrects the predicted self-location based on the difference value, the self-location accuracy information, and the measurement accuracy information of the measurement unit. In this aspect, the self-location estimation device can appropriately correct the self-location by further taking into account the measurement accuracy of the measurement unit.
[0010] In another aspect of the self-location estimation device, the correction unit calculates an evaluation value for evaluating the difference value based on the difference value and the self-location accuracy information, and determines the degree to which the predicted self-location is corrected by the difference value based on the evaluation value. According to this aspect, the self-location estimation device can accurately determine the degree to which the predicted self-location is corrected by the difference value by evaluating the difference value based on the self-location accuracy information. This makes it possible to suitably suppress a decrease in the vehicle location estimation accuracy even when an error exists in the first distance information or the second distance information. In this case, preferably, the correction unit corrects the predicted self-location by a value obtained by multiplying the difference value by a predetermined gain, and the correction unit determines a correction coefficient for the gain based on the evaluation value. More preferably, the gain is a Kalman gain.
[0011] In another aspect of the self-location estimation device, the second acquisition unit calculates the distance indicated by the second distance information based on the predicted position information and the position information of the object recorded in map information. With this aspect, the self-location estimation device generates the second distance information based on the map information and can be suitably used for self-location estimation.
[0012] According to another preferred embodiment of the present invention, a control method executed by a self-location estimation device includes a first acquisition step of acquiring predicted position information indicating a predicted self-location, a second acquisition step of acquiring first distance information indicating a distance measured by a measurement unit from a moving body to an object and second distance information indicating a distance from the moving body to the object predicted based on the position information of the object, a third acquisition step of acquiring self-location accuracy information of the moving body, and a correction step of correcting the predicted self-location based on a difference value between the distances indicated by the first distance information and the second distance information and the self-location accuracy information. The self-location estimation device can preferably correct the predicted self-location by executing this control method.
[0013] According to another preferred embodiment of the present invention, a program executed by a computer includes a first acquisition unit that acquires predicted position information indicating a predicted self-position, a second acquisition unit that acquires first distance information indicating a distance measured by a measurement unit from a moving body to an object, and second distance information indicating a distance from the moving body to the object predicted based on the position information of the object, a third acquisition unit that acquires self-position accuracy information of the moving body, and a correction unit that corrects the predicted self-position based on a difference value between the distance indicated by the first distance information and the second distance information, and the self-position accuracy information. The computer can preferably correct the predicted self-position by executing this program. Preferably, the program is stored in a storage medium. EXAMPLES
[0014] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In this specification, for the sake of convenience, a character with "^" or "-" added to any symbol is referred to as "A ^ " or "A - ” (where “A” is any letter). [Schematic configuration]
[0015] Fig. 1 is a schematic diagram of a driving assistance system according to the present embodiment. The driving assistance system shown in Fig. 1 includes an on-board device 1 that is mounted on a vehicle and controls driving assistance for the vehicle, a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 2, a gyro sensor 3, a vehicle speed sensor 4, and a GPS receiver 5.
[0016] The vehicle-mounted device 1 is electrically connected to the lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and estimates the position of the vehicle on which the vehicle-mounted device 1 is mounted (also referred to as "vehicle position") based on the outputs of these sensors. Then, based on the estimation result of the vehicle position, the vehicle-mounted device 1 performs automatic driving control of the vehicle so that the vehicle travels along a route to a set destination. The vehicle-mounted device 1 stores a map database (DB: DataBase) 10 that stores road data and feature information, which is information on landmark features provided near the road. The above-mentioned landmark features are, for example, kilometer posts, 100m posts, delineators, traffic infrastructure facilities (e.g., signs, direction signs, traffic signals), utility poles, street lights, and other features that are periodically lined up on the side of the road. Then, the vehicle-mounted device 1 estimates the vehicle position based on this feature information by collating it with the output of the lidar 2, etc. The vehicle-mounted device 1 is an example of a "self-position estimation device" in the present invention.
[0017] The lidar 2 measures the distance to an object in the outside world discretely by emitting a pulsed laser in a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud information indicating the position of the object. In this case, the lidar 2 has an irradiation unit that irradiates a laser beam while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser beam, and an output unit that outputs scan data based on the light receiving signal output by the light receiving 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-mentioned light receiving signal. In general, the closer the distance to the object, the higher the accuracy of the distance measurement value of the lidar, and the farther the distance, the lower the accuracy. The lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the vehicle-mounted device 1. The lidar 2 is an example of a "measurement unit" in the present invention.
[0018] 2 is a block diagram showing the functional configuration of the vehicle-mounted device 1. The vehicle-mounted 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. These elements are connected to each other via a bus line.
[0019] The interface 11 acquires output data from sensors such as the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies the data to the control unit 15.
[0020] The storage unit 12 stores a program executed by the control unit 15 and information required for the control unit 15 to execute a predetermined process. In this embodiment, the storage unit 12 stores a map DB 10 including feature information. FIG. 3 shows an example of a 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 each feature is associated with the feature, and here includes at least a feature ID corresponding to an index of the feature, and position information indicating the absolute position of the feature represented by latitude and longitude (and altitude), etc. The map DB 10 may be updated periodically. 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 map information via a communication unit (not shown), and reflects the partial map information in the map DB 10.
[0021] The input unit 14 is a button operated by the user, a touch panel, a remote controller, a voice input device, etc. The information output unit 16 is, for example, a display, a speaker, etc. that perform output based on the control of the control unit 15.
[0022] The control unit 15 includes a CPU that executes a program and controls the entire vehicle-mounted device 1. In this embodiment, the control unit 15 has a vehicle position estimation unit 17. The control unit 15 is an example of a "first acquisition unit," a "second acquisition unit," a "third acquisition unit," a "correction unit," and a "computer" that executes a program in the present invention.
[0023] The vehicle position estimation unit 17 corrects the vehicle position estimated from the output data of the gyro sensor 3, the vehicle speed sensor 4, and / or the GPS receiver 5 based on the measurement values of the distance and angle of the lidar 2 to the feature and the position information of the feature extracted from the map DB 10. In this embodiment, as an example, the vehicle position estimation unit 17 alternately executes a prediction step of estimating the vehicle position from the output data of the gyro sensor 3, the vehicle speed sensor 4, etc. based on a state estimation method based on Bayesian estimation, and a measurement update step of correcting the estimated value of the vehicle position calculated in the immediately preceding prediction step. As the state estimation filter used in these steps, various filters developed to perform Bayesian estimation can be used, such as 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.
[0024] Fig. 4 is a diagram showing the state variable vector x in two-dimensional Cartesian coordinates. As shown in Fig. 4, the vehicle position on a plane defined on the two-dimensional Cartesian coordinates of xy is represented by coordinates "(x, y)" and the vehicle's orientation "Ψ". Here, the orientation Ψ is defined as the angle between the vehicle's traveling direction and the x-axis. The coordinates (x, y) indicate an absolute position equivalent to a combination of latitude and longitude, for example.
[0025] FIG. 5 is a diagram showing 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, the prediction step and the measurement update step are repeated to sequentially calculate and update an estimated value of a state variable vector "X" indicating the vehicle position. As shown in FIG. 6, the vehicle position estimation unit 17 has a position prediction unit 21 that executes the prediction step, and a position estimation unit 22 that executes the 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 (i.e., the current time) "t" to be calculated is calculated and updated. -(t)" or "X ^ (t)" ("State variable vector X(t) = (x(t), y(t), Ψ(t)) T "). Here, the provisional estimates (predictions) made in the prediction step are indicated by " - " is added to the character representing the value, and the more accurate estimates updated in the measurement update step are marked with " ^ " is added.
[0026] In the prediction step, the dead reckoning block 23 of the vehicle position estimation unit 17 calculates the vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as the "control value u(t)=(v(t), ω(t)) T "). The position prediction block 24 of the vehicle position estimation unit 17 calculates the travel distance and the change in direction from the previous time t-1 using the state variable vector X ^ Add the calculated travel distance and direction change to (t-1) to calculate the predicted vehicle position at time t (also called the predicted vehicle position). X - (t) is calculated. At the same time, the predicted vehicle position X - The covariance matrix P - (t)” is the covariance matrix “P ^ (t-1)".
[0027] 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, when this association is established, the feature search / extraction block 25 of the vehicle position estimation unit 17 calculates the measurement value (called the "feature measurement value") "Z(t)" of the associated feature by the LIDAR 2 and the predicted vehicle position X - (t) and the position vector of the feature registered in the map DB 10 are used to model the measurement process by the LIDAR 2, and the estimated measurement value of the feature (called the "predicted feature value") "Z -The feature measurement value Z(t) is a two-dimensional vector in the vehicle's body coordinate system, which is obtained by converting the distance and scan angle of the feature measured by the LIDAR 2 at time t into components whose axes are the traveling direction and lateral direction of the vehicle. Then, the position correction block 26 of the vehicle position estimation unit 17 calculates the feature measurement value Z(t) and the feature prediction value Z(t) as shown in the following equation (1). - The difference value with (t) is calculated.
[0028]
number
[0029]
number
[0030] When the vehicle position estimation unit 17 is able to associate the position vectors of multiple features registered in the map DB 10 with the scan data of the LIDAR 2, the vehicle position estimation unit 17 may perform the measurement update step based on any one selected feature measurement value, or may perform the measurement update step multiple times based on all the associated feature measurement values. When using multiple feature measurement values, the vehicle position estimation unit 17 may take into consideration that the LIDAR measurement accuracy deteriorates as the feature is farther from the LIDAR 2, and may reduce the weighting for the feature as the distance between the LIDAR 2 and the feature increases.
[0031] In this way, the prediction step and the measurement update step are repeated, and the predicted vehicle position X - (t) and estimated vehicle position X ^ By successively calculating (t), the most probable vehicle position is calculated.
[0032] In the above description, the feature measurement value Z(t) is an example of the "first distance information" of the present invention, and the feature prediction value Z - (t) is an example of the "second distance information" of the present invention.
[0033] [Vehicle position estimation details] Next, the vehicle position estimation in this embodiment will be described in detail. In summary, the vehicle position estimation unit 17 calculates a feature measurement value Z(t) and a feature prediction value Z - The vehicle position estimating unit 17 calculates an evaluation value (also called a "difference evaluation value") that evaluates a difference value between the vehicle position estimating unit 17 and the map DB 10 (t), and corrects the Kalman gain K(t) according to the difference evaluation value. In this way, even if an error occurs in either the map DB 10 or the measurement value of the LIDAR 2, the vehicle position estimating unit 17 suitably suppresses a decrease in the accuracy of estimating the vehicle position.
[0034] In the EKF (Extended Karman Filter) position estimation using road signs and other objects using a lidar, the ground object measurement value Z(t) and the ground object prediction value Z(t) are calculated as shown in the above equation (2). - As can be seen from equation (2), when the difference value shown in equation (1) becomes large, the predicted vehicle position X- The amount of correction for (t) becomes large. The reason why the difference value becomes large is due to one of the following reasons (A) to (C).
[0035] (A) There is an error in the feature measurement. This includes, for example, cases where the lidar's measurement accuracy is low, or cases where the lidar's measurement accuracy is not low but the measurement accuracy is low because a moving object such as another vehicle is present between the feature being measured and the vehicle at the time of measurement (i.e., when occlusion occurs).
[0036] (B) The estimated vehicle position used to calculate the feature prediction value is misaligned. This is the predicted vehicle position X - (t) or estimated vehicle position X ^ This is the case when the error in (t) is large.
[0037] (C) The location coordinates of features in the map data used to calculate feature prediction values are misaligned. This is a state in which the position of a feature has changed since the map data was created, for example, because a sign, which is a feature, has become tilted due to a vehicle collision or because a structure, which is a feature, has been removed or moved, and as a result, the position coordinates of the feature in the map data no longer match the actual position coordinates of the feature. In other words, this is a state in which a discrepancy has occurred between the position of the feature on the map data and the position of the feature in the real environment.
[0038] When the difference value is large due to the above (A) or (C), the estimated vehicle position X ^ When (t) is calculated, the vehicle position estimation accuracy decreases. Therefore, when either (A) or (C) above occurs, the predicted vehicle position X - It is preferable to make the degree of correction for (t) as low as possible.
[0039] FIG. 7 shows the positional relationship between the estimated vehicle position and features. The coordinate system shown in FIG. 7 is a body coordinate system based on the vehicle, with the x-axis indicating the traveling direction of the vehicle and the y-axis indicating the direction perpendicular thereto (the 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 indicates the position estimation accuracy in the x-direction, "σ P (x) and y-direction position estimation accuracy "σ P (y)”, where the position estimation accuracy σ P is obtained by transforming the covariance matrix P(t) into the body coordinate system using the Jacobian matrix “H(t)” according to the following equation (3).
[0040]
number
[0041] In Fig. 7, the feature prediction value Z - (t) is the predicted vehicle position X - The feature measurement value Z(t) is obtained by modeling the measurement process by the LIDAR 2 using the feature position vectors registered in the map DB 10 and the scan data of the LIDAR 2. The feature measurement value Z(t) is the measurement value of the feature by the LIDAR 2 when the position vectors of the feature registered in the map DB 10 can be associated with the scan data of the LIDAR 2. The feature measurement value Z(t) has an error range indicated by an error ellipse 43. The error ellipse 43 represents the measurement accuracy "σ L (x)" and y-direction measurement accuracy "σ LGenerally, the measurement error by a lidar increases in proportion to the square of the distance between the lidar and the feature to be measured. Therefore, the lidar measurement accuracy σ L (σ L (x),σ L (y)) is obtained. In addition, the feature prediction value Z - The difference between (t) and the feature measurement value Z(t) is “dx” in the x direction and “dy” in the y direction, as shown in equation (1).
[0042] In addition, the position estimation accuracy σ P is an example of the "self-position accuracy information" of the present invention, and the lidar measurement accuracy σ L is an example of the "measurement accuracy information" of the present invention.
[0043] Next, the feature measurement value Z(t) and the feature prediction value Z - A method for calculating an evaluation value for evaluating the difference value with (t) will be described. P (σ P (x),σ P (y)) and lidar measurement accuracy σ L (σ L (x),σ L (y)) to calculate the feature measurement value Z(t) and the feature prediction value Z - (t) is evaluated. In other words, the vehicle position estimation unit 17 judges the validity of the above-mentioned difference value based on the ratio of the difference value to the error range 40 of the estimated vehicle position and the LIDAR error range 43. Specifically, the vehicle position estimation unit 17 calculates the difference evaluation values "Ex" and "Ey" by the following evaluation formula (4). In this case, the absolute values of the difference values dx and dy are used.
[0044]
number
[0045]
number
[0046]
number
[0047] 8 is a flowchart of the vehicle position estimation process performed by the vehicle position estimation unit 17 of the vehicle-mounted device 1. The vehicle-mounted device 1 repeatedly executes the process of the flowchart in FIG.
[0048] First, the vehicle position estimation unit 17 sets an initial value of the vehicle position based on the output of the GPS receiver 5, etc. (step S101). Next, the vehicle position estimation unit 17 acquires the vehicle speed from the vehicle speed sensor 4 and the angular velocity in the yaw direction from the gyro sensor 3 (step S102). Then, the vehicle position estimation unit 17 calculates the travel distance of the vehicle and the change in the direction of the vehicle based on the results acquired in step S102 (step S103).
[0049] After that, the vehicle position estimation unit 17 calculates the estimated vehicle position X ^ (t-1) is added to the travel distance and direction change calculated in step S103 to obtain the predicted vehicle position X - (t) (step S104). Furthermore, in step S104, the vehicle position estimating unit 17 calculates the position estimation accuracy σ P (x),σ P Furthermore, the vehicle position estimation unit 17 calculates the predicted vehicle position X - Based on (t), the feature information in the map DB 10 is referenced to search for features within the measurement range of the LIDAR 2 (step S105).
[0050] Then, the vehicle position estimation unit 17 calculates the predicted vehicle position X - From the position coordinates indicated by the feature information of the feature searched for in step S105, the predicted positions of the feature in the vehicle's travel direction and lateral direction (i.e., predicted feature values Z - Thereafter, the vehicle position estimation unit 17 calculates the measured distances to the feature in each of the vehicle travel direction and lateral direction from the scan data of the LIDAR 2 (i.e., the feature measurement values Z(t)), and calculates the LIDAR measurement accuracy σ L (x),σ L (y) is calculated (step S107).
[0051] Next, the vehicle position estimation unit 17 calculates the difference values dx, dy between the feature measurement value and the feature prediction value based on the formula (1) (step S108).Then, the vehicle position estimation unit 17 calculates the difference evaluation values Ex, Ey based on the formula (4) (step S109).
[0052] If the difference evaluation values Ex, Ey are greater than the predetermined value (step S110; Yes), the vehicle position estimation unit 17 calculates a coefficient value a X (t), a Y For example, the vehicle position estimation unit 17 generates a coefficient value a X (t), a Y (t), the coefficient value a becomes smaller as the difference evaluation values Ex and Ey become larger in the range from 0 to 1. X (t), a Y If there is a difference evaluation value that does not exceed the predetermined value, the vehicle position estimation unit 17 calculates a coefficient value corresponding to the difference evaluation value (coefficient value a in the case of difference evaluation value Ex). X (t) may be set to 1 without using equation (5).
[0053] On the other hand, when the difference evaluation values Ex and Ey are equal to or smaller than the predetermined value (step S110; No), the vehicle position estimation unit 17 X (t), a Y (t) is set to "1" (step S112). That is, the coefficient value a X (t) is set, and the coefficient value a is calculated by comparing the difference evaluation value Ey with a predetermined value. Y (t) is set.
[0054] Next, the vehicle position estimation unit 17 adds a coefficient value a to the Kalman gain K(t) based on the equation (6). X (t), a Y (t) to generate a Kalman gain K(t)' (step S113). After that, the vehicle position estimation unit 17 uses the generated Kalman gain K(t)' instead of K(t) based on the formula (2) to calculate the predicted vehicle position X - (t) is corrected and the estimated vehicle position X^ (t) is calculated (step S114).
[0055] [Specific example] Next, the effect of the vehicle position estimation process according to this embodiment will be described.
[0056] Fig. 9(A) is a graph showing the transition of the position in the x direction and the y direction when a vehicle is driven on a certain driving test course while executing the vehicle position estimation process shown in Fig. 8. Note that Fig. 9(A) depicts the vehicle position (also called the "reference position") measured with high accuracy using RTK-GPS and the estimated vehicle position calculated by the vehicle position estimation process shown in Fig. 8, but they overlap to the extent that they are visually indistinguishable.
[0057] Fig. 9(B) is an enlarged view of data in the travel section within the frame 70 in Fig. 9(A). In Fig. 9(B), the positions of the features that were the measurement targets are plotted with circles. Fig. 10(A) is a graph showing the transition of the difference value dx in the travel section of the frame 70, Fig. 10(B) shows the difference in the travel direction between the estimated vehicle position based on the embodiment in the travel section of the frame 70 and the reference position, Fig. 10(C) shows the difference in the lateral direction between the estimated vehicle position based on the embodiment in the travel section of the frame 70 and the reference position, and Fig. 10(D) shows the azimuth difference between the estimated vehicle position based on the embodiment in the travel section of the frame 70 and the reference position.
[0058] As shown in Fig. 9(B), the vehicle position estimation unit 17 estimates the estimated vehicle position based on feature information for features that exist at irregular intervals, and corrects the predicted vehicle position based on the difference value shown in Fig. 10(A) etc. As a result, as shown in Fig. 10(B) to (D), the difference between the reference position and the estimated vehicle position in the vehicle's travel direction and lateral direction is within about 0.1 m, and the azimuth difference is also within about 0.1 degree.
[0059] 11(A) to (D) show the relationship between the Kalman gain K(t) and the coefficient value a when the position coordinates of the feature information for the feature shown in the plot 71 of FIG. 9(B) are shifted by 80 cm in the direction of travel. X (t), a Y 10(A) to (D) show experimental results similar to those in FIG. 10(A) to (D) when used without correction by (t).
[0060] As shown in Fig. 11(A), due to a deviation of the position coordinates of the feature information for the feature shown in the plot 71 in Fig. 9(B), the difference value dx (see circle 72) calculated using the position coordinates of the feature information for the feature shown in the plot 71 is significantly larger than the difference value dx calculated using the position coordinates of the feature information for other features. As a result, as shown in Fig. 11(B), the estimated vehicle position calculated using the position coordinates of the feature information for the feature shown in the plot 71 in Fig. 9(B) is deviated from the reference position in the traveling direction of the vehicle (see circle 73). The error in the estimated vehicle position thus generated continues until the next feature (see plot 74 in Fig. 9) used for vehicle position estimation is detected.
[0061] 12(A) to (D) show the relationship between the Kalman gain K(t) and the coefficient value a when the position coordinates of the feature information for the feature shown in the plot 71 of FIG. 9(B) are shifted by 80 cm in the direction of travel. X (t), a Y 11(A) to (D) show experimental results similar to those shown in FIG. 11(A) to (D) when corrected by (t).
[0062] In this case, the difference value dx at the time when the feature is detected, which is shown by the plot 71 in FIG. 9B, and the position estimation accuracy σ P (x), Lidar measurement accuracy σ L (x) becomes as follows: dx=-0.749 σ P (x)=0.048 σ L (x)=0.058
[0063] Therefore, these values are used as the coefficient value a in equation (5). X Substituting for (t), the coefficient value aX (t) is 0.854×10 -3 ≒0 In this case, the Kalman gain K(t)' is calculated as shown in the following equation (7).
[0064]
number
[0065] As described above, the vehicle position estimating unit 17 of the vehicle-mounted device 1 according to the present embodiment estimates the predicted vehicle position X - The position estimation unit 22 generates information indicating a feature measurement value Z(t) indicating a distance measured 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 calculates the difference values dx and dy between the feature measurement values and the feature prediction values, and the position estimation accuracy σ P and the lidar measurement accuracy σ L A coefficient value a for correcting the Kalman gain K(t) according to the differential evaluation values Ex and Ey based on X (t), a Y As a result, even if an error occurs in either the map DB 10 or the measurement value of the LIDAR 2, the vehicle-mounted device 1 can suitably suppress a decrease in the accuracy of estimating the vehicle's position.
[0066] [Variations] Preferred modified examples of the embodiment will be described below. The following modified examples may be applied to the embodiment in combination.
[0067] (Variation 1) Predicted vehicle position X based on feature information that is judged to be unreliable - The method for reducing the correction amount of (t) is to use the coefficient value a X (t), a Y(t) by the Kalman gain K(t).
[0068] Instead, in the first example, the vehicle position estimation unit 17 calculates a coefficient value a for the difference values dx and dy. X (t), a Y In this case, the vehicle position estimating unit 17 calculates the estimated vehicle position based on the following equation (8).
[0069]
number
[0070]
number
[0071]
number
[0072]
number
[0073]
number
[0074] (Variation 2) The configuration of the driving assistance system shown in Fig. 1 is an example, and the configuration of a driving assistance system to which the present invention can be applied is not limited to the configuration shown in Fig. 1. For example, instead of having the on-board device 1, the driving assistance system may have an electronic control device of the vehicle execute the processing of the vehicle position estimation unit 17 of the on-board device 1. In this case, the map DB 10 may be stored in, for example, a storage unit in the vehicle, and the electronic control device of the vehicle may receive update information for the map DB 10 from a server device (not shown). [Explanation of symbols]
[0075] 1 On-vehicle device 2. Rider 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB
Claims
[Claim 1] A first acquisition unit that acquires predicted location information indicating a predicted self-location; a second acquisition unit that acquires a first distance, which is a distance measured by a measurement unit from a moving body to an object, and a second distance, which is a distance predicted from the moving body to the object based on position information of the object; A third acquisition unit that acquires self-location accuracy information of the moving object; a correction unit that corrects the predicted self-position by a value obtained by multiplying a difference between the first distance and the second distance by a predetermined gain, The correction unit determines a correction coefficient for the gain based on the self-location accuracy information. Self-location estimation device.
Citation Information
Patent Citations
Self-position estimation device, control method, program, and storage medium
JP6968877B2
Moving body position estimation method and moving body
JP2013257742A
Information processing device, control method, program, and storage medium
JP2017072422A
