Self-location estimation device, control method, program, and storage medium

The self-position estimating device corrects vehicle positioning errors by using feature orientation and size information to adjust the predicted position, enhancing accuracy in self-position estimation.

JP2024161105AInactive Publication Date: 2024-11-15PIONEER IP
View PDF 6 Cites 0 Cited by

Patent Information

Application Number
JP2024144149
Authority / Receiving Office
JP · JP
Patent Type
Applications
Current Assignee / Owner
Priority Date
2017-05-19
Filing Date
2024-08-26
Publication Date
2024-11-15
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The accuracy of self-position estimation in vehicles is compromised due to variations in the size and orientation of physical objects like road signs and direction signboards, leading to measurement errors that affect the precision of vehicle positioning.

Method used

A self-position estimating device that acquires predicted position information, traveling direction, normal line, and size information of features, and corrects the predicted self-position using a correction unit that adjusts based on the acquired information, including a Kalman gain for precise estimation.

Benefits of technology

The device enhances the accuracy of self-position estimation by reducing measurement errors through targeted corrections based on feature orientation and size, improving the precision of vehicle positioning.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure 2024161105000001_ABST
    Figure 2024161105000001_ABST
Patent Text Reader

Abstract

To provide a self-location estimation device capable of suitably suppressing deterioration of self-location estimation precision even in the case of a possible feature measurement error.SOLUTION: An own vehicle location estimation part 17 of an on-vehicle machine 1 generates information for showing a predicted own vehicle location X-(t), and acquires normal line information and size information of a feature as a measurement object from a map DB10 by a lidar 2. The own vehicle location estimation part 17 corrects the predicted own vehicle location X-(t) on the basis of an angle difference Δθ calculated from information on a progress direction of a vehicle and normal line information of a feature, and a size S shown by size information of a feature.SELECTED DRAWING: Figure 7
Need to check novelty before this filing date? Find Prior Art

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] Road signs, direction signs, and other features that are the subject of measurement in the self-location estimation process are created and installed so that drivers can easily recognize them, but they are not all the same size or orientation; the size varies depending on the display content, and the orientation varies depending on the installation location. As a result, there are cases where the size or orientation of the feature that is the subject of measurement by the system that performs self-location estimation is difficult to measure, and in such cases, the measurement value of the feature that is the subject of measurement may contain a large error. If self-location estimation is performed using measurement values ​​that contain such errors, the accuracy of the self-location estimation will decrease.

[0005] The present invention has been made to solve the above-mentioned problems, and its main object is to provide a self-location estimation device that can effectively suppress deterioration of the self-location estimation accuracy even if there is a possibility of measurement errors of geographical features. [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 information on the direction of travel of a moving body, a third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves together with the moving body, and a correction unit that corrects the predicted self-location based on the information on the direction of travel, the normal information, and the size information.

[0007] The invention described in claim 8 is a control method executed by a self-location estimation device, comprising a first acquisition step of acquiring predicted location information indicating a predicted self-location, a second acquisition step of acquiring information on the direction of travel of a moving body, a third acquisition step of acquiring normal information and size information of a feature to be measured by a measurement unit moving together with the moving body, and a correction step of correcting the predicted self-location based on the information on the direction of travel, the normal information, and the size information.

[0008] The invention described in claim 9 is a program executed by a computer, which causes the computer to function as a first acquisition unit that acquires predicted position information indicating a predicted self-position, a second acquisition unit that acquires information on the direction of travel of a moving body, a third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves together with the moving body, and a correction unit that corrects the predicted self-position based on the information on the direction of travel, the normal information, and the size information. [Brief description of the drawings]

[0009] [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] 1 illustrates an example of the positional relationship between a feature that is a measurement target of a lidar and a vehicle. [Figure 8] FIG. 11 is a diagram for explaining an example of setting coefficient values. [Figure 9] 4 is a flowchart of a vehicle position estimation process. [Figure 10] FIG. 2 is a diagram showing a surface of an object on the ground that is irradiated with laser light by a lidar. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] 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 information on a moving direction of a moving object, a third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves with the moving object, and a correction unit that corrects the predicted self-location based on the information on the moving direction, the normal information, and the size information. With this aspect, the self-location estimation device can appropriately correct the predicted self-location based on the orientation and size of the feature with respect to the moving direction of the moving object.

[0011] In one aspect of the self-location estimation device, the self-location estimation device further includes a fourth acquisition unit that acquires first distance information indicating a distance measured by the measurement unit from the moving body to the feature, and second distance information indicating a distance from the moving body to the feature predicted based on the location information of the feature, and the correction unit determines a degree to which the predicted self-location is corrected by a difference value between the distance indicated by the first distance information and the second distance information based on the information of the traveling direction, the normal information, and the size information. With this aspect, the self-location estimation device can suitably adjust a degree to which the predicted self-location is corrected by a difference value between the distance indicated by the first distance information and the second distance information based on an orientation or size of the feature relative to the traveling direction of the moving body.

[0012] In another aspect of the self-location estimation device, 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 information on the traveling direction, the normal information, and the size information. With this aspect, the self-location estimation device can determine a correction coefficient for a gain that determines the degree to which the predicted self-location is corrected by the difference value of the distance indicated by the first distance information and the second distance information, based on the orientation and size of a feature relative to the traveling direction of the moving body. Preferably, the gain is a Kalman gain.

[0013] In another aspect of the self-location estimation device, the correction unit reduces the degree of correction of the predicted self-location in the traveling direction as the angular difference between the traveling direction and the normal direction of the feature indicated by the normal information increases, and reduces the degree of correction of the predicted self-location in the lateral direction of the moving body as the angular difference decreases. In this way, the self-location estimation device can reduce the degree of correction of the self-location in a direction in which an error is likely to occur in the position measurement of the feature, and can suitably improve the position estimation accuracy.

[0014] In another aspect of the self-location estimation device, the correction unit reduces the degree to which the predicted self-location is corrected as the size of the feature indicated by the size information becomes smaller. In general, the smaller the size of the feature to be measured, the fewer the measurement points for the feature, and the lower the position measurement accuracy of the feature. Therefore, with this aspect, the self-location estimation device can preferably suppress the decrease in position estimation accuracy.

[0015] In another aspect of the self-location estimation device, the correction unit reduces the degree of correction of the predicted self-location as the size of the feature measured by the measurement unit is smaller than the size of the feature indicated by the size information. In general, when only a portion of a feature can be measured due to occlusion or the like, the position measurement accuracy of the feature is low. Therefore, with this aspect, the self-location estimation device can preferably suppress a decrease in the self-location estimation accuracy by reducing the degree of correction of the predicted self-location when only a portion of the feature can be measured due to occlusion or the like.

[0016] 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 location information indicating a predicted self-location, a second acquisition step of acquiring information on a moving direction of a moving body, a third acquisition step of acquiring normal information and size information of a feature to be measured by a measurement unit moving together with the moving body, and a correction step of correcting the predicted self-location based on the information on the moving direction, the normal information, and the size information. By executing this control method, the self-location estimation device can appropriately correct the predicted self-location based on the orientation and size of the feature relative to the moving direction of the moving body.

[0017] According to another preferred embodiment of the present invention, a program executed by a computer causes the computer to function as a first acquisition unit that acquires predicted position information indicating a predicted self-position, a second acquisition unit that acquires information on the moving direction of a moving body, a third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves with the moving body, and a correction unit that corrects the predicted self-position based on the moving direction information, the normal information, and the size information. By executing this program, the computer can preferably correct the predicted self-position based on the orientation and size of the feature relative to the moving direction of the moving body. Preferably, the above program is stored in a storage medium. EXAMPLES

[0018] 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).

[0019] [Schematic configuration] 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.

[0020] 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.

[0021] 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.

[0022] 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.

[0023] 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.

[0024] The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to execute predetermined processes. In this embodiment, the storage unit 12 stores a map DB 10 including 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.

[0025] The feature information is information in which information about each feature is associated with the feature, and here includes a feature ID equivalent to an index of the feature, position information, normal information, and size information. The position information indicates the absolute position of the feature expressed by latitude and longitude (and altitude), etc. The normal information and size information are information provided for features having a planar shape such as a signboard. The normal information is information that indicates the orientation of the feature, and indicates, for example, a normal vector for a surface formed on the feature. The size information is information that indicates the size of the feature, and may be, for example, information indicating the area of ​​the surface formed on the feature, or information indicating the vertical and horizontal widths of the surface formed on the feature.

[0026] The map DB 10 may be periodically updated. 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.

[0027] 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.

[0028] 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 "fourth acquisition unit," a "correction unit," and a "computer" that executes a program in the present invention.

[0029] 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.

[0030] 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.

[0031] 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.

[0032] 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)".

[0033] 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.

[0034]

number

[0035]

number

[0036] 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.

[0037] 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.

[0038] 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.

[0039] [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 corrects the Kalman gain based on normal information and size information included in the feature information of the feature to be measured by the LIDAR 2. As a result, the vehicle position estimation unit 17 estimates the accuracy of the feature measurement values ​​in the vehicle travel direction and lateral direction according to the orientation and size of the feature to be measured, and calculates the predicted vehicle position X - The amount of correction (t) is determined accurately.

[0040] Fig. 7 is a diagram showing an example of the positional relationship between the vehicle and a feature 50 that is the object of measurement by the LIDAR 2. In Fig. 7, an arrow 40 indicates the traveling direction of the vehicle, and an arrow 41 indicates the normal direction of the feature 50. Measurement points "P1" to "P7" indicate the positions of each point of the point cloud data output by the LIDAR 2. Furthermore, "Δθ" indicates the angle difference between the traveling direction of the vehicle indicated by the arrow 40 and the normal direction of the feature 50 indicated by the arrow 41. The angle difference Δθ is assumed to be in the range of 0 to 90 degrees.

[0041] 7, the vehicle position estimation unit 17 acquires point cloud data indicating measurement points P1 to P7 from the LIDAR 2, and calculates each of the feature measurement values ​​Z(t) in the x direction and y direction from the acquired point cloud data. In this case, the vehicle position estimation unit 17 calculates the center of gravity coordinate of the x coordinate of the body coordinate system indicated by the measurement points P1 to P7 for the feature 50 (i.e. the average value of the coordinates indicated by each measurement point) as the feature measurement value Z(t) in the x direction, and calculates the center of gravity coordinate of the y coordinate of the body coordinate system indicated by the measurement points P1 to P7 for the feature 50 as the feature measurement value Z(t) in the y direction.

[0042] At this time, the smaller the angle difference Δθ (i.e., closer to parallel), the higher the accuracy of the traveling direction component of the feature measurement value and the lower the accuracy of the lateral direction component. In other words, the larger the angle difference Δθ (i.e., closer to perpendicular), the higher the accuracy of the lateral direction component of the feature measurement value and the lower the accuracy of the traveling direction component. Also, the larger the size of the feature 50, the more measurement points the lidar 2 has on the feature 50, and therefore the higher the accuracy of the feature measurement value.

[0043] Taking the above into consideration, the vehicle position estimation unit 17 determines a coefficient value "a X (t)," "a Y Then, as shown in the following equation (3), these are multiplied by the Kalman gain K(t) to calculate the corrected Kalman gain “K(t)’’.

[0044]

number

[0045] In this case, the coefficient value a X (t), a Y (t) is a value between 0 and 1, and is determined so that the larger the size of the feature to be measured, the larger the value becomes. Furthermore, the coefficient a X (t) becomes smaller as the angle difference Δθ becomes larger, and a Y The coefficient a(t) is determined so that it becomes larger as the angle difference Δθ becomes larger. In other words, X (t) becomes larger as the angle difference Δθ becomes smaller, and a Y The coefficient a(t) is determined so that it becomes smaller as the angle difference Δθ becomes smaller. X (t), a Y (t) is set using an equation or a table. For example, if the size (e.g., area) of the feature to be measured is "S", then (t) is set based on the following equation (4).

[0046]

number

[0047] Next, referring to FIG. 8, the coefficient value a X (t), a Y An example of setting (t) will be explained.

[0048] FIG. 8(A) shows an overhead view of a vehicle undergoing scanning by the lidar 2. In FIG. 8(A), features 51 and 52 with different normal directions are present within the measurement range of the lidar 2. FIG. 8(B) is an enlarged view of the feature 51 showing the measurement points "P11" to "P15" of the lidar 2. FIG. 8(C) is an enlarged view of the feature 52 showing the measurement points "P16" to "P19" of the lidar 2. In FIG. 8(B) and (C), the center of gravity coordinates in the lateral direction and the traveling direction of the vehicle calculated from the position coordinates of each irradiation point are shown by broken lines. Note that, for convenience of explanation, FIG. 8(A) to (C) illustrate a case where the lidar 2 emits laser light along a predetermined scanning plane, but the laser light may be emitted along multiple scanning planes with different heights.

[0049] As shown in Figures 8(A) and 8(B), the normal direction of the feature 51 is approximately aligned with the lateral direction of the vehicle. Therefore, the illuminated points P11 to P15 are aligned along the traveling direction of the vehicle, with large variation in the traveling direction and small variation in the lateral direction. Therefore, in position estimation using the feature 51, it is predicted that the accuracy of the feature measurement values ​​of the lateral components of the illuminated points P11 to P15, which have small variation, will be high. In this case, the angle difference Δθ is approximately 90 degrees, and "cos Δθ ≒ 0" and "sin Δθ ≒ 1", so the coefficient value a based on equation (4) is X (t) is close to 0, and the coefficient value a Y (t) is a value according to the size S. Therefore, in this case, the predicted vehicle position X - The amount of correction to (t) can be made relatively small.

[0050] On the other hand, as shown in Figures 8(A) and 8(C), the normal direction of the feature 52 is approximately the same as the traveling direction of the vehicle. Therefore, the illuminated points P16 to P19 are aligned along the lateral direction of the vehicle, with large variation in the lateral direction and small variation in the traveling direction. Therefore, in position estimation using the feature 52, it is predicted that the accuracy of the feature measurement value of the traveling direction component with small variation of the illuminated points P16 to P19 will be high. In this case, the angle difference Δθ is approximately 0 degrees, and "cos Δθ ≒ 1" and "sin Δθ ≒ 0", so the coefficient value a based on formula (4) X (t) is a value according to the size S, and the coefficient value a Y (t) is close to 0. Therefore, in this case, the predicted vehicle position X - The amount of correction to (t) can be made relatively small. Note that the coefficient value a X (t) and a Y A lower limit may be set for (t). In that case, if the result calculated by formula (4) is equal to or less than a predetermined lower limit, the lower limit is set. This makes it possible to avoid a situation in which a direction that is not corrected by Δθ continues.

[0051] 9 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.

[0052] 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).

[0053] 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, 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).

[0054] Then, the vehicle position estimation unit 17 calculates the predicted vehicle position X - From (t) and the position coordinates indicated by the feature information of the feature searched for in step S105, the feature prediction value Z - Furthermore, in step S106, the vehicle position estimation unit 17 calculates a feature measurement value Z(t) from the measurement data of the LIDAR 2 for the feature searched for in step S105.

[0055] Then, the vehicle position estimation unit 17 refers to the size information and normal information of the feature information of the feature searched for in step S105, and calculates the size S of the illuminated surface of the feature and the angle difference Δθ between the vehicle's traveling direction and the normal vector of the target feature (step S107). In this case, the vehicle position estimation unit 17 calculates the angle difference Δθ by multiplying it by the predicted vehicle position X - The calculation is based on the vehicle's heading Ψ indicated by (t) and the normal direction of the feature indicated by the normal information included in the feature information.

[0056] Next, the vehicle position estimation unit 17 calculates a coefficient value a based on the size S and the angle difference Δθ by using, for example, equation (4). X (t), a Y Then, the vehicle position estimation unit 17 calculates the coefficient value a(t) based on the equation (3). X (t), a Y (t) to generate a Kalman gain K(t)' (step S109). 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 S110).

[0057] 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 vehicle position estimation unit 17 generates information indicating the vehicle position X(t) and obtains normal line information and size information of the feature to be measured by the LIDAR 2 from the map DB 10. The vehicle position estimation unit 17 then calculates a predicted vehicle position X(t) based on the information on the vehicle's traveling direction and the angle difference Δθ calculated from the normal line information of the feature, and the size S indicated by the size information of the feature. - In this way, the vehicle position estimation unit 17 can estimate the accuracy of the feature measurement values ​​in the vehicle travel direction and lateral direction according to the orientation and size of the feature to be measured, and can accurately determine the correction amount of the predicted vehicle position in each direction according to the estimated accuracy.

[0058] [Variations] Preferred modified examples of the embodiment will be described below. The following modified examples may be applied to the embodiment in combination.

[0059] (Variation 1) Predicted vehicle position X - The method for adjusting the correction amount of (t) is to use the coefficient value a X (t), a Y (t) by the Kalman gain K(t).

[0060] Instead, in the first example, the vehicle position estimation unit 17 calculates a coefficient value a for the difference values ​​dx and dy shown in the formula (1). X (t), a Y In this case, the vehicle position estimating unit 17 calculates the estimated vehicle position based on the following equation (5).

[0061]

number

[0062]

number

[0063]

number

[0064]

number

[0065]

number

[0066] (Variation 2) The vehicle position estimation unit 17 may correct the Kalman gain by taking into account the ratio between the size of the feature to be measured, which is identified from the size information registered in the map DB 10, and the size, which is identified from the measurement data of the LIDAR 2.

[0067] Fig. 10 is a diagram showing an irradiated surface of a feature 55 onto which the laser light from the lidar 2 is irradiated. In the example of Fig. 10, measurement points on the irradiated surface of the feature 55 are clearly shown. Note that a part of the left side of the feature 55 is not irradiated with the laser light from the lidar 2 due to occlusion by an obstacle or the like.

[0068] In this case, the vehicle position estimation unit 17 calculates the height direction width “H M " and the horizontal width "W M Based on the point cloud data output by the LIDAR 2, the vehicle position estimation unit 17 also determines the width "H L " and the horizontal width "W L " respectively.

[0069] Then, the vehicle position estimation unit 17 generates reliability information “a(t)” indicating the accuracy of the distance measurement for the feature 55 by using the following formula (10).

[0070]

number

[0071] Then, the vehicle position estimation unit 17 calculates the calculated reliability information a(t) as a coefficient value aX (t), a Y By multiplying the Kalman gain by (t), it is possible to determine the amount of correction to the predicted vehicle position according to the reliability indicating the accuracy of the distance measurement to the feature.

[0072] (Variation 3) 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]

[0073] 1 Onboard 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 information about a traveling direction of the moving object; a third acquisition unit that acquires normal information and size information of a feature to be measured by a measurement unit that moves together with the moving object; a correction unit that corrects the predicted self-position based on the information on the traveling direction, the normal information, and the size information; A self-location estimation device comprising:

Citation Information

Patent Citations

  • Positioning device

    JP2008002906A

  • Position locating device, position locating system, user interface device of the position locating system, locating server device of the position locating system, and position locating method

    JP2009264983A

  • Position specification device and data structure

    JP2016050840A

  • Estimation apparatus, control method, program, and storage medium

    WO2017060947A1

  • Moving body position estimation method and moving body

    JP2013257742A