Self-position estimation device

Abstract

By utilizing lane markings such as white lines, the system accurately estimates the vehicle's position in the direction of travel. [Solution] The self-position estimation device is mounted on a moving object and acquires the predicted position of the moving object. The self-position estimation device calculates the difference between the predicted position of the endpoint of the lane line obtained based on the endpoint information of the lane line obtained from map information and the measured position of the endpoint of the lane line measured by a measuring unit mounted on the moving object scanning light in a predetermined direction, and estimates the self-position of the moving object by correcting the predicted position with a value obtained by multiplying the difference value by a coefficient. The self-position estimation device then corrects the coefficient based on the interval of the scanning position of the measuring unit at the position where the endpoint of the lane line was detected.

Description

Technical Field

[0001] The present invention relates to self-position estimation technology.

Background Art

[0002] Conventionally, a technique has been known in which a ground object installed in the traveling direction of a vehicle is detected using a radar or a camera, and the position of the host vehicle is corrected based on the detection result. For example, Patent Document 1 discloses a technique for estimating the self-position by comparing the output of a measurement sensor with the position information of a ground object registered in advance on a map. Further, Patent Document 2 discloses a self-position estimation technique using a Kalman filter.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Patent Document 2

Summary of the Invention

Problems to be Solved by the Invention

[0004] In self-position estimation based on the Bayesian method shown in Patent Document 2, when measuring continuously provided structures such as white lines, curbs, and guardrails, the lateral distance from the host vehicle can be measured, but there is a problem that the distance in the traveling direction cannot be accurately specified due to the continuity in the traveling direction. Further, in the case of a broken-line type lane line, since the length in the front-rear direction of the detected lane line changes, the position in the traveling direction cannot be accurately specified in the same manner.

[0005] The present invention has been made to solve the above problems, and mainly aims to provide a self-position estimation device that can accurately estimate the position in the traveling direction as well as the lateral direction, with a lane line such as a white line as the measurement target.

Means for Solving the Problems

[0006] The invention according to the claims is a self-position estimation device mounted on a moving body, comprising: an acquisition means for acquiring a predicted position of the moving body; a difference value calculation means for calculating a difference value between a predicted position of an end point of a section line obtained based on information of end points of the section line acquired from map information and a measured position of the end point of the section line measured by scanning light in a predetermined direction by a measurement unit mounted on the moving body; and an estimation means for estimating the self-position of the moving body by correcting the predicted position of the moving body with a value obtained by multiplying the difference value by a coefficient.

Brief Description of the Drawings

[0007] [Figure 1] It is a schematic configuration diagram of a driving support system. [Figure 2] It is a block diagram showing a functional configuration of an in-vehicle device. [Figure 3] It is a diagram showing a state variable vector in a two-dimensional orthogonal coordinate system. [Figure 4] It is a diagram showing a schematic relationship between a prediction step and a measurement update step. [Figure 5] It shows a functional block of a host vehicle position estimation unit. [Figure 6] It shows a method for converting lidar measurement values into a Cartesian coordinate system. [Figure 7] It shows a method for detecting a white line by a lidar mounted on a vehicle. [Figure 8] It shows positions of a plurality of scan lines within a window. [Figure 9] It shows a method for detecting a position of an end point at a start portion of a white line. [Figure 10] It shows a method for detecting a position of an end point at an end portion of a white line. [Figure 11] It shows a method for determining a position of an end point when the reflection intensity changes in the middle of a scan line. [Figure 12] It shows a method for calculating a landmark prediction value of an end point of a white line. [Figure 13] It shows a correction coefficient of a Kalman gain. [Figure 14] The correction factor for measurement noise is shown. [Figure 15] This is a flowchart of the vehicle position estimation process. [Modes for carrying out the invention]

[0008] One preferred embodiment of the present invention is a self-position estimation device mounted on a moving body, comprising: acquisition means for acquiring a predicted position of the moving body; difference value calculation means for calculating a difference between a predicted position of the endpoint of a road mark obtained based on information of the endpoint of a road mark obtained from map information and a measured position of the endpoint of the road mark measured by a measuring unit mounted on the moving body scanning light in a predetermined direction; and estimation means for estimating the self-position of the moving body by correcting the predicted position of the moving body with a value obtained by multiplying the difference value by a coefficient, wherein the estimation means corrects the coefficient based on the interval of the scanning position of the measuring unit at the position where the endpoint of the road mark is detected.

[0009] The self-position estimation device described above is mounted on a moving object and acquires the predicted position of the moving object. The self-position estimation device calculates the difference between the predicted position of the boundary line endpoints obtained from map information and the measured position of the boundary line endpoints measured by a measuring unit mounted on the moving object scanning light in a predetermined direction. The predicted position of the moving object is corrected by multiplying the difference value by a coefficient to estimate the self-position of the moving object. The self-position estimation device then corrects the coefficient based on the interval of the scanning position of the measuring unit at the position where the boundary line endpoints are detected. This self-position estimation device makes it possible to improve the accuracy of estimating the self-position of the moving object in the direction of travel by utilizing the position of the boundary line endpoints. In a preferred example, the coefficient is the Kalman gain.

[0010] In one embodiment of the self-position estimation device described above, the estimation means corrects the coefficient by the reciprocal of the interval between the scanning positions. In another embodiment, the estimation means corrects the coefficient based on the ratio of the interval between the scanning positions to the shortest interval between the scanning positions measured by the measurement unit. In this embodiment, the smaller the interval between the scanning positions, the more the coefficient is corrected so that the difference value is reflected in the estimation of the self-position.

[0011] In another embodiment of the self-position estimation device described above, the measurement unit detects demarcation lines within a window defined at a predetermined position based on the position of the moving object, and the estimation means corrects the coefficient based on the ratio of the number of scan lines present on the demarcation lines to the number of scan lines predetermined based on the size of the window. In this embodiment, the coefficient is corrected so that the greater the number of scan lines present on the detected demarcation lines, the greater the difference value is reflected in the estimation of the self-position.

[0012] In another embodiment of the self-position estimation device described above, the measurement unit detects the midpoint between an adjacent scan line present on the boundary line and a scan line not present on the boundary line as the endpoint of the boundary line. This makes it possible to determine the endpoint of the boundary line from the positions of the scan lines present on the boundary line and the scan lines not present on the boundary line.

[0013] Another preferred embodiment of the present invention is a self-position estimation method performed by a self-position estimation device mounted on a moving body, comprising: an acquisition step of acquiring a predicted position of the moving body; a difference calculation step of calculating a difference between a predicted position of the endpoint of a road mark obtained based on information of the endpoint of a road mark obtained from map information and a measured position of the endpoint of the road mark measured by a measuring unit mounted on the moving body scanning light in a predetermined direction; and an estimation step of estimating the self-position of the moving body by correcting the predicted position of the moving body with a value obtained by multiplying the difference value by a coefficient, wherein the estimation step corrects the coefficient based on the interval of the scanning position of the measuring unit at the position where the endpoint of the road mark is detected. According to this self-position estimation method, the accuracy of estimating the self-position of the moving body in the direction of travel can be improved by utilizing the position of the endpoint of the road mark.

[0014] Another preferred embodiment of the present invention is a program executed by a self-position estimation device mounted on a mobile body and equipped with a computer, wherein the computer functions as an acquisition means for acquiring the predicted position of the mobile body, a difference value calculation means for calculating the difference between the predicted position of the endpoint of a road mark obtained based on information of the endpoint of a road mark obtained from map information and the measured position of the endpoint of the road mark measured by a measuring unit mounted on the mobile body scanning light in a predetermined direction, and an estimation means for estimating the self-position of the mobile body by correcting the predicted position of the mobile body with a value obtained by multiplying the difference value by a coefficient, and the estimation means corrects the coefficient based on the interval of the scanning position of the measuring unit at the position where the endpoint of the road mark is detected. By executing this program on the computer, the above self-position estimation device can be realized. This program can be stored and handled on a storage medium. [Examples]

[0015] Preferred embodiments of the present invention will be described below with reference to the drawings. Note that any character with a "^" or "-" above it will be referred to as "A" in this specification for convenience. ^ " or "A - This is represented as (where "A" is any letter). [Driving assistance system]

[0016] Figure 1 is a schematic diagram of the driver assistance system according to this embodiment. The driver assistance system shown in Figure 1 includes an on-board unit 1 mounted on the vehicle that performs control related to driver assistance, 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.

[0017] The on-board unit 1 is electrically connected to the lidar 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5, and estimates the position of the vehicle on which the on-board unit 1 is mounted (also called "self-position") based on the outputs of these sensors. Based on the estimated self-position, the on-board unit 1 performs automatic driving control of the vehicle so that it travels along a set route to a destination. The on-board unit 1 stores a map database (DB:DataBase) 10 in which road data and information on landmarks and road markings located near the road are registered. The aforementioned landmarks include, for example, kilometer posts, 100m posts, delineators, traffic infrastructure equipment (e.g., signs, directional signs, traffic lights), utility poles, and streetlights that are periodically lined up along the side of the road. Based on this map DB 10, the on-board unit 1 estimates the self-position by comparing it with the outputs of the lidar 2, etc. The on-board unit 1 is an example of a "self-position estimation device" in the present invention.

[0018] The lidar 2 discretely measures the distance to an object in the external environment by emitting a pulsed laser within 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 laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data based on the received signal output by the light receiving unit. In this embodiment, the irradiation range of the laser emitted by the lidar 2 includes at least the road surface. The scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the distance to the object in that irradiation direction of the laser light, which is determined based on the received signal described above. Generally, the closer the distance to the object, the higher the accuracy of the lidar's distance measurement, and the farther the distance, the lower the accuracy. The lidar 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 each supply output data to the in-vehicle unit 1. The lidar 2 is an example of a "measurement unit" in the present invention.

[0019] Figure 2 is a block diagram showing the functional configuration of the in-vehicle unit 1. The in-vehicle unit 1 mainly consists of 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 interconnected via a bus line.

[0020] Interface 11 acquires output data from sensors such as the rider 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5, and supplies it to the control unit 15.

[0021] The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to perform predetermined processing. In this embodiment, the storage unit 12 stores a map DB 10 containing road marking information and feature information. Here, road marking information is information about road markings (white lines) provided on each road, and each road marking includes coordinate information indicating the discrete position of the road marking. Note that road marking information may also be information incorporated into the road data for each road. Furthermore, in this embodiment, dashed road markings include coordinate information indicating the position of their endpoints. Feature information is information about features other than road markings, and here, each feature is associated with at least a feature ID corresponding to the feature index and position information indicating the absolute position of the feature, expressed by latitude and longitude (and elevation), etc. Note that the map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information relating to the area to which the vehicle's position belongs from a server device that manages map information via a communication unit (not shown) and reflects it in the map DB 10.

[0022] The input unit 14 includes buttons, a touch panel, a remote controller, a voice input device, etc., for user operation. The information output unit 16 includes, for example, a display or speaker that outputs information based on the control of the control unit 15.

[0023] The control unit 15 includes a CPU that executes programs and controls the entire in-vehicle unit 1. In this embodiment, the control unit 15 has a vehicle position estimation unit 17. The control unit 15 is an example of the acquisition means, difference value calculation means, and estimation means in the present invention.

[0024] The vehicle position estimation unit 17 corrects the vehicle position estimated from the output data of the gyro sensor 3, vehicle speed sensor 4, and / or GPS receiver 5, based on the distance and angle measurements taken by the lidar 2 to landmarks and the location information of landmarks extracted from the map DB 10. In this embodiment, as an example, the vehicle position estimation unit 17 alternately performs a prediction step, which predicts the vehicle position from the output data of the gyro sensor 3, vehicle speed sensor 4, etc., based on a state estimation method based on Bayesian estimation, and a measurement update step, which corrects the predicted value of the vehicle position calculated in the previous prediction step. Various filters developed for Bayesian estimation can be used as state estimation filters in these steps, such as extended Kalman filters, unscented Kalman filters, and particle filters. Thus, various methods have been proposed for position estimation based on Bayesian estimation.

[0025] The following briefly describes the vehicle position estimation using an extended Kalman filter. As will be described later, in this embodiment, the vehicle position estimation unit 17 performs the vehicle position estimation process uniformly using an extended Kalman filter, regardless of whether the vehicle position estimation is performed on a feature or a lane marking.

[0026] Figure 3 shows the state variable vector x represented in two-dimensional Cartesian coordinates. As shown in Figure 3, the position of the vehicle on a plane defined on the two-dimensional Cartesian coordinate system xy is represented by the coordinates "(x, y)" and the vehicle's bearing "Ψ". Here, the bearing Ψ is defined as the angle between the vehicle's direction of travel and the x-axis. The coordinates (x, y) represent the absolute position in a coordinate system with a certain reference point as the origin, which corresponds, for example, to a combination of latitude and longitude.

[0027] FIG. 4 is a diagram showing a schematic relationship between a prediction step and a measurement update step. FIG. 5 shows an example of a functional block of the host vehicle position estimation unit 17. As shown in FIG. 4, by repeating the prediction step and the measurement update step, the calculation and update of the estimated value of the state variable vector “X” indicating the host vehicle position are sequentially executed. As shown in FIG. 5, the host vehicle position estimation unit 17 includes 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 landmark search / extraction unit 25 and a position correction block 26. In FIG. 4, the state variable vector at the reference time (i.e., the current time) “t” to be calculated is represented as “X - (t)” or “X ^ (t)” (denoted as “state variable vector X(t)=(x(t), y(t), Ψ(t)) T ”). Here, a temporary estimated value (predicted value) estimated in the prediction step is denoted by putting “ - ” above the character representing the predicted value, and a more accurate estimated value updated in the measurement update step is denoted by putting “ ^ ” above the character representing the value.

[0028] In the prediction step, the dead reckoning block 23 of the host vehicle position estimation unit 17 uses the moving speed “v” and the angular velocity “ω” of the vehicle (collectively denoted as “control value u(t)=(v(t), ω(t)) T ”) to obtain the moving distance and the azimuth change from the previous time. The position prediction block 24 of the host vehicle position estimation unit 17 adds the obtained moving distance and azimuth change to the state variable vector X ^ (t - 1) calculated in the immediately preceding measurement update step to calculate the predicted value of the host vehicle position at time t (also referred to as “predicted host vehicle position”) X - (t). At the same time, the covariance matrix “P - (t)” corresponding to the error distribution of the predicted host vehicle position X - (t) is calculated from the covariance matrix “P ^ (t - 1)” at time t - 1 calculated in the immediately preceding measurement update step.

[0029] In the measurement update step, the landmark search and extraction unit 25 of the vehicle position estimation unit 17 associates the position vectors of landmarks registered in the map DB 10 with the scan data of the lidar 2. Then, if this association is successful, the landmark search and extraction unit 25 of the vehicle position estimation unit 17 extracts the measured value of the associated landmark by the lidar 2 (referred to as the "landmark measurement value") "Z(t)" and the predicted vehicle position X - (t) and the estimated landmark measurement values ​​(referred to as "landmark prediction values") obtained by modeling the measurement process by LIDA2 using the location vectors of landmarks registered in map DB10. - The landmark measurement value Z(t) is obtained from the distance and scan angle of the landmark measured by the rider 2 at time t, and is converted into components with the direction of travel and lateral direction as axes, and is a two-dimensional vector in the vehicle's body coordinate system. Then, the position correction block 26 of the self-position estimation unit 17 obtains the landmark measurement value Z(t) and the landmark prediction value Z as shown in equation (1) below. - Calculate the difference value from (t).

[0030]

number

[0031] Then, the position correction block 26 of the vehicle position estimation unit 17 uses the landmark measurement value Z(t) and the landmark prediction value Z as shown in equation (2) below. - The difference value from (t) is multiplied by the Kalman gain "K(t)", and this is used to predict the vehicle's position X - By adding this to (t), the updated state variable vector (also called "estimated vehicle position") X is obtained. ^ Calculate (t).

[0032]

number

[0033] Furthermore, in the measurement update step, the position correction block 26 of the vehicle position estimation unit 17 performs the same as in the prediction step, the estimated vehicle position X ^ The covariance matrix P corresponds to the error distribution of (t). ^ (t) (also simply denoted as P(t)) is the covariance matrix P - It is determined from (t). Parameters such as the Kalman gain K(t) can be calculated in the same way as known self-localization techniques using extended Kalman filters.

[0034] In this way, the prediction step and the measurement update step are repeatedly performed, and the predicted vehicle position X - (t) and estimated vehicle position X ^ The most likely position of the vehicle is calculated by sequentially calculating (t).

[0035] [Vehicle position estimation using lane markings] Next, we will explain the vehicle position estimation method using lane markings, which is a feature of this embodiment. In this embodiment, vehicle position estimation is performed using lane markings as landmarks. In the following explanation, we will describe an example using white lines as lane markings, but the same method can be applied to yellow lane markings as well.

[0036] (1) Measurement of white lines using a lidar First, let's explain how to measure white lines using a ridiculous scanner. (Rider's measurements) Now, as shown in Figure 6, consider a Cartesian coordinate system (hereinafter referred to as the "vehicle coordinate system") where the vehicle's own position is the origin and the vehicle's direction of travel is the x-axis. The measured values ​​to the target measured by the lidar include the horizontal angle α, the vertical angle β, and the distance r. When these are converted to the Cartesian coordinate system, the measured values ​​by the lidar are expressed by the following equation.

[0037]

number

[0038] (Detection of white lines) Figure 7 shows how a white line is detected by a lidator 2 mounted on the vehicle. White lines have a high reflectivity because they are coated with retroreflective material. Therefore, the on-board unit 1 has a window positioned to easily detect white lines on the road surface in front of the vehicle, and recognizes the area with high reflectivity within that window as a white line.

[0039] Specifically, as shown in Figure 7, emitted light EL is emitted from lidars 2 located on the left and right sides of the front of the vehicle toward the road surface in front of the vehicle. A virtual window W is defined at a predetermined position in front of the vehicle. The position of the window W relative to the vehicle's center O is predetermined. That is, the horizontal distance L1 from the vehicle's center O to the window W, the lateral distance L2 from the vehicle's center O to the left and right windows W, and the length L3 of the window W are all predetermined. The in-vehicle unit 1 extracts scan data belonging to the window W from the scan data output from the lidars 2, and determines that a white line has been detected if the reflection intensity of that data is higher than a predetermined threshold.

[0040] (Line spacing) By using a multi-layered lidar as lidar 2, multiple scan lines s are formed within window W. Figure 8 shows the positions of the multiple scan lines s within window W. The distance between the multiple scan lines s within window W increases as they move further away from the vehicle (hereinafter referred to as "line spacing"). The upper right of Figure 8 shows a plan view of the scan lines within window W as seen from above. Scan lines s1~s within window W 10 A diagram is formed, showing the line spacing d1 to d9 for each scanline. The line spacing d increases for scanlines further from the vehicle. Also, the lower right of Figure 8 shows a side view of the scanlines within window W. Similarly, the line spacing d increases for scanlines further from the vehicle.

[0041] If we denote the number of an arbitrary scanline as "i", the horizontal angle of the i-th scanline as α(i), the vertical angle as β(i), and the distance from the vehicle as r(i), then the line spacing d(i) between the i-th scanline and the (i+1)-th scanline can be calculated using the following equation (4). Similarly, the line spacing d(i+1) between the (i+1)-th scanline and the (i+2)-th scanline can be calculated using the following equation (5), and the line spacing d(i+2) between the (i+2)-th scanline and the (i+3)-th scanline can be calculated using the following equation (6).

[0042]

number

[0043] In this way, the line spacing d of multiple scan lines s within window W can be determined based on the scan data from rider 2.

[0044] (Measurement of the endpoint position of the white line) Next, we will explain how to measure the endpoint position of the white line using a lidar. Figure 9 shows how to measure the position of the endpoint of the beginning of the white line (the starting point of the white line). Within window W are s1~s 10 Ten scan lines are formed. As mentioned above, the in-vehicle device 1 detects white lines based on the reflection intensity of the scan data obtained by the lidar. Now, let's assume that the reflection intensity of the scan data changes between scan lines s2 and s3, as shown in Example 1. That is, the reflection intensity of scan line s2 is above a predetermined threshold, and the reflection intensity of scan line s3 is below a predetermined threshold. In this case, it can be seen that the endpoint of the white line is located between scan lines s2 and s3, but its exact position cannot be determined. Therefore, the in-vehicle device 1 considers the midpoint of scan lines s2 and s3 to be the endpoint of the white line.

[0045] Similarly, as shown in Example 2, if the reflection intensity of the scan data changes between scan lines s4 and s5, the in-vehicle unit 1 considers the midpoint of scan lines s4 and s5 as the endpoint of the white line. Also, as shown in Example 3, if the reflection intensity of the scan data changes between scan lines s7 and s8, the in-vehicle unit 1 considers the midpoint of scan lines s7 and s8 as the endpoint of the white line.

[0046] The method for detecting the endpoint position of the end of the white line is basically the same. Figure 10 shows a method for measuring the position of the endpoint (end point of the white line) of the end of the white line. As shown in Example 4, assume that the reflection intensity of the scan data changes between scan lines s2 and s3. That is, assume that the reflection intensity of scan line s2 is below a predetermined threshold, and the reflection intensity of scan line s3 is above a predetermined threshold. In this case, the in-vehicle device 1 considers the midpoint of scan lines s2 and s3 to be the endpoint of the white line.

[0047] Similarly, as shown in Example 5, if the reflection intensity of the scan data changes between scan lines s4 and s5, the in-vehicle unit 1 considers the midpoint of scan lines s4 and s5 as the endpoint of the white line. Also, as shown in Example 6, if the reflection intensity of the scan data changes between scan lines s7 and s8, the in-vehicle unit 1 considers the midpoint of scan lines s7 and s8 as the endpoint of the white line.

[0048] Now, the i-th scanline s i And the (i+1)th scanline s i+1 Assuming that the reflection intensity of the scan data changes between the two points, the ridiculous measurement L indicates the position of the endpoint of the white line in the direction of vehicle travel. x (t) is the i-th scanline s in the direction of travel of the vehicle. i Position r x (i) and the (i+1)th scanline s i+1 Position r x It is calculated using (i+1) and the following formula.

[0049]

number

[0050] The measurement value of the white line by LIDA2 is given by the following formula.

[0051]

number

[0052] Note that while the examples in Figures 9 and 10 show a case where a single scan line could be detected as a set of scan data with equal reflectance, the reflectance of the scan data may change in the middle of a single scan line. In this case, it is difficult to distinguish whether the scan line is actually the endpoint of a white line, or whether it is not actually the endpoint of a white line but was detected as such due to dirt, smudges, etc.

[0053] Figure 11 shows an example where the reflectivity of the scan data changes in the middle of a scan line. In Examples 7 and 8, the reflectivity of the scan data changes in the middle of scan line s3, making it difficult to determine that scan line s3 is the endpoint of the white line. Therefore, when the reflectivity of the scan data changes in the middle of a scan line, the in-vehicle unit 1 considers the midpoint between the scan line that was correctly detected as a white line and the scan line that was not detected as a white line at all as the endpoint position of the white line. In Example 7 of Figure 11, the in-vehicle unit 1 considers the midpoint between scan line s2, which was correctly detected as a white line, and scan line s4, which was not detected as a white line at all, as the endpoint position of the white line. Similarly, in Example 8, the in-vehicle unit 1 considers the midpoint between scan line s4, which was correctly detected as a white line, and scan line s2, which was not detected as a white line at all, as the endpoint position of the white line.

[0054] Now, the (i-1)th scanline s iー1 And the (i+1)th scanline s i+1 The i-th scanline s located between and i Assuming that the reflection intensity of the scan data changes during the process, the ridiculous measurement L indicates the position of the endpoint of the white line in the direction of the vehicle's movement. x(t) is the (i-1)th scanline s in the direction of vehicle travel. i-1 Position r x (i-1) and (i+1)th scanlines s i+1 Position r x It is calculated using (i+1) and the following equation (9). The line spacing d(t) at that time is calculated using equation (10).

[0055]

number

[0056] Based on the above, if the endpoint of the white line is detected, the measured value L in the direction of vehicle travel is x (t) is the value given by equation (7) or equation (9). If the endpoint of the white line cannot be detected, the measured value L in the direction of travel of the vehicle is used. x (t) is not included. On the other hand, the lateral measurement value L of the vehicle y (t) is the average value of the center points of multiple scanlines detected within window W. In this way, the measured value of the white line by the lidar is obtained according to the white line detection result.

[0057] (2) Method for estimating the position of the vehicle Next, we will describe a method for estimating the vehicle's position using an example. (Calculation of landmark prediction values) First, we will explain how to calculate the predicted landmark values ​​of the endpoints of the white lines using map data. Figure 12 shows the method for calculating the predicted landmark values ​​of the endpoints of the white lines. In Figure 12, a vehicle V exists in the world coordinate system, and the vehicle coordinate system is defined with the center of the vehicle V as the origin. The coordinates of the white line endpoint k stored in map DB10 are (m x (k),m y Let (k)) be the coordinates of the white line endpoint k stored in map DB10 are in the world coordinate system, and the onboard unit 1 transforms these coordinates to the vehicle coordinate system. Specifically, the estimated self-position of vehicle V is x - (t), y - (t), estimated vehicle direction Ψ -If (t) is the case, then using a rotation matrix that transforms the coordinates from the world coordinate system to the vehicle coordinate system, the predicted landmark value of the white line endpoint k can be expressed by the following formula.

[0058]

number

[0059] (Correction of Kalman gain) Next, we will explain how to correct the Kalman gain according to the detection status of the endpoints of the white lines. Figure 13(A) shows the correction coefficient for the Kalman gain when the in-vehicle unit 1 detects the endpoints of the white lines. The landmark measurements used for estimating the vehicle's position include measurements in the direction of travel and measurements in the lateral direction of the vehicle. Therefore, the correction coefficient for the direction of travel is denoted as the direction correction coefficient a(t), and the correction coefficient for the lateral direction of the vehicle is denoted as the lateral coefficient b(t).

[0060] When the endpoint of a white line is detected within window W, the on-board unit 1 uses the position of the white line endpoint as the measured value for the direction of travel. However, as mentioned above, the measured value of the white line endpoint position is obtained as the midpoint of two adjacent scan lines before and after the change in reflection intensity, so the measurement accuracy of the vehicle's direction of travel differs depending on the line spacing of the scan lines. That is, the error is large when the line spacing is wide, and small when the line spacing is narrow. Therefore, the Kalman gain for the direction of travel is corrected using the length of the line spacing of the scan lines. Specifically, the reciprocal of the line spacing d(t) is taken as the direction of travel coefficient a(t). That is, the direction of travel coefficient a(t) is, a(t) = 1 / d(t) This is how it is set. When an endpoint of a white line is detected, the narrower the line spacing, the higher the accuracy of the measured endpoint position of the white line, so a correction is made to increase the Kalman gain.

[0061] The lateral measurement of the vehicle uses the average value of the center points of multiple scan lines detected within window W. If the endpoint of a white line is detected within window W, the number of scan lines present within window W is N, as shown in Figure 13(A). MLet N be the number of scan lines measured on the white line by the lidar. L Therefore, the horizontal coefficient b(t) is, b(t) = N L (t) / N M This is how it is set. When an endpoint of a white line is detected, the more scan lines detected on the white line, the higher the measurement accuracy in the lateral direction of the vehicle, so a correction is made to increase the Kalman gain.

[0062] Figure 13(B) shows the correction coefficient for the Kalman gain when the in-vehicle unit 1 detects a white line but does not detect the endpoint of the white line. When a white line is not detected, measurement values ​​in the direction of vehicle travel cannot be obtained, so the direction coefficient a(t) is: a(t)=0 This is how it is set. Also, if the endpoints of the white lines are not detected, the center points of all scanlines within window W can be measured, and the measurement accuracy in the lateral direction of the vehicle is high, so the lateral coefficient b(t) is, b(t)=1 This is how it is set.

[0063] The vehicle position estimation unit 17 corrects the Kalman gain using the direction of travel coefficient a(t) and lateral direction coefficient b(t) obtained in this way. Specifically, the vehicle position estimation unit 17 multiplies the Kalman gain K(t) shown in equation (12) by the direction of travel coefficient a(t) and lateral direction coefficient b(t) to generate the adaptive Kalman gain K(t)' shown in equation (13).

[0064]

number

[0065]

number

[0066] Then, the vehicle position estimation unit 17 applies the obtained adaptive Kalman gain K(t)' to the Kalman gain K(t) in equation (2) and calculates the estimated vehicle position X^(t) using the following equation (14).

[0067]

number

[0068] (Correction of measurement noise) Next, we will explain how to correct measurement noise according to the detection status of the endpoints of the white lines. Figure 14(A) shows the correction coefficient for measurement noise when the in-vehicle unit 1 detects the endpoints of the white lines. The landmark measurement values ​​used for estimating the vehicle's position include measurement values ​​in the direction of the vehicle's movement and measurement values ​​in the lateral direction of the vehicle. Therefore, the correction coefficient for the direction of the vehicle's movement is denoted as the direction correction coefficient a(t), and the correction coefficient for the lateral direction of the vehicle is denoted as the lateral coefficient b(t).

[0069] When the endpoint of a white line is detected within window W, the on-board unit 1 uses the position of the white line endpoint as the measured value for the direction of travel. However, as mentioned above, the measured value of the white line endpoint position is obtained as the midpoint of two adjacent scan lines before and after the change in reflection intensity, so the measurement accuracy of the direction of travel of the vehicle differs depending on the line spacing of the scan lines. That is, the error is large when the line spacing is wide, and small when the line spacing is narrow. Therefore, the measurement noise for the direction of travel is corrected using the length of the line spacing of the scan lines. Specifically, the ratio of the line spacing d(t) to the shortest line spacing among the multiple detected line spacings (line spacing d9 in Figure 9) is defined as the direction of travel coefficient a(t). That is, the direction of travel coefficient a(t) is, a(t) = d(t) / d9 This setting is applied. When an endpoint of a white line is detected, the accuracy of the measured endpoint position of the white line is higher when the line spacing is narrower, so a correction is made to reduce measurement noise.

[0070] The lateral measurement of the vehicle uses the average value of the center points of multiple scan lines detected within window W. If the endpoint of a white line is detected within window W, the number of scan lines present within window W is N, as shown in Figure 14(A). M Let N be the number of scan lines measured on the white line by the lidar.L Therefore, the horizontal coefficient b(t) is, b(t) = N M / N L (t) This setting is applied. When an endpoint of a white line is detected, the more scan lines detected on the white line, the higher the measurement accuracy in the lateral direction of the vehicle, so a correction is made to reduce measurement noise.

[0071] Figure 14(B) shows the measurement noise correction coefficient when the in-vehicle unit 1 detects a white line but does not detect the endpoint of the white line. When a white line is not detected, measurement values ​​in the direction of vehicle travel cannot be obtained, so a large value is set for the direction of travel coefficient a(t). For example, a(t) = 10000 This is how it is set. Also, if the endpoints of the white lines are not detected, the center points of all scanlines within window W can be measured, and the measurement accuracy in the lateral direction of the vehicle is high, so the lateral coefficient b(t) is, b(t)=1 This is how it is set.

[0072] The vehicle position estimation unit 17 multiplies the obtained direction coefficient a(t) and lateral coefficient b(t) by the measurement noise to generate adaptive measurement noise. Then, the vehicle position estimation unit 17 uses this adaptive measurement noise to determine the estimated vehicle position.

[0073] Specifically, the basic measurement noise R(t) is given by the following formula. Note that for the white line measurement in question, σ Lx 2 σ is the measurement noise in the direction of travel of the vehicle coordinate system, Ly 2 This represents the measurement noise in the lateral direction of the vehicle coordinate system.

[0074]

number

[0075] Therefore, the vehicle position estimation unit 17 multiplies the measurement noise by the direction of travel coefficient a(t) and the lateral direction coefficient b(t) to obtain the following adaptive measurement noise R(t)'.

[0076]

number

[0077] Substituting this into the Kalman gain equation (12), we obtain the following adapted Kalman gain K(t)'.

[0078]

number

[0079] Then, the vehicle position estimation unit 17 applies the obtained adaptive Kalman gain K(t)' to the Kalman gain K(t) in equation (2) and calculates the estimated vehicle position X^(t) using equation (14).

[0080] This allows for position estimation in the direction of travel using endpoint information of dashed lines, and enables appropriate processing according to the detection status of white lines by processing with adaptive measurement noise corresponding to the detection accuracy of white line endpoints and the number of detected lines.

[0081] (3) Vehicle position estimation process Figure 15 is a flowchart of the vehicle position estimation process performed by the vehicle position estimation unit 17. The vehicle position estimation unit 17 repeatedly executes the process shown in the flowchart of Figure 15.

[0082] First, the vehicle position estimation unit 17 determines whether or not it has detected the vehicle speed and the vehicle's yaw angular velocity (step S11). For example, the vehicle position estimation unit 17 detects the vehicle speed based on the output of the vehicle speed sensor 4 and detects the yaw angular velocity based on the output of the gyro sensor 3. If the vehicle position estimation unit 17 has detected the vehicle speed and the vehicle's yaw angular velocity (step S11: Yes), it uses the detected vehicle speed and angular velocity to estimate the vehicle position X from one time point ago. ^ Predicted vehicle position X from (t-1)- (t) is calculated. Furthermore, the vehicle position estimation unit 17 calculates the covariance matrix at the current time from the covariance matrix from one time step ago (step S12). Note that if the vehicle position estimation unit 17 does not detect the vehicle speed and the angular velocity of the vehicle in the yaw direction (step S11: No), the estimated vehicle position X^(t-1) from one time step ago is used as the predicted vehicle position X-(t), and the covariance matrix from one time step ago is used as the covariance matrix at the current time.

[0083] Next, the vehicle position estimation unit 17 determines whether or not it has detected a white line as a landmark to be used for estimating the vehicle's position (step S13). Specifically, the vehicle position estimation unit 17 sets a window W as shown in Figure 7 and determines whether or not it has detected a white line within the window W based on the reflection intensity of the scan data from the lidar 2. If no white line is detected (step S13: No), vehicle position estimation using the white line is not possible, so the process ends.

[0084] On the other hand, if a white line is detected (Step S13: Yes), the vehicle position estimation unit 17 calculates a predicted landmark value for the white line based on the map data in the map DB 10, and also calculates a measured landmark value for the white line by the lidar 2 (Step S14). Specifically, the vehicle position estimation unit 17 converts the coordinates of the endpoints of the white line included in the map data to the vehicle coordinate system to calculate a predicted landmark value for the white line endpoint, and uses the measured value of the white line by the lidar as the landmark value.

[0085] Furthermore, as explained in Figures 13 and 14, the vehicle position estimation unit 17 calculates a direction coefficient a(t) and a lateral coefficient b(t) depending on whether or not it was able to detect the endpoint of the white line (step S15).

[0086] The vehicle position estimation unit 17 then calculates the Kalman gain K(t) based on equation (10) above using the covariance matrix, generates an adaptive Kalman gain K(t)' shown in equation (13) or equation (17) using the direction coefficient a(t) and the lateral coefficient b(t), and calculates the estimated vehicle position using equation (14). Furthermore, the vehicle position estimation unit 17 updates the covariance matrix using the adaptive Kalman gain K(t)' as shown in equation (18) (step S16).

[0087]

number

[0088] As explained above, when the vehicle position estimation unit 17 detects the endpoint of the white line, it uses the endpoint of the white line as a landmark and performs vehicle position estimation processing based on a Kalman filter. Furthermore, it calculates a direction coefficient based on the line spacing of the scan lines by the lidar 2 when the endpoint of the white line is detected and corrects the Kalman gain. This makes it possible to estimate the vehicle position with high accuracy even in the direction of vehicle movement.

[0089] [Differentiation] In the above embodiment, the in-vehicle unit 1 detects white lines by providing windows W at the left and right positions in front of the vehicle. However, if a pair of lidars 2 are also provided at the rear of the vehicle, windows W may also be provided at the left and right positions at the rear of the vehicle to detect white lines. [Explanation of Symbols]

[0090] 1 On-vehicle device 2 Riders 3. Gyroscope sensor 4. Vehicle speed sensor 5 GPS receivers 10 Map Database

Claims

[Claim 1] A self-position estimation device mounted on a mobile vehicle, An acquisition means for acquiring the predicted position of the moving object, A difference value calculation means calculates the difference between the predicted position of the endpoint of the lane line obtained based on information of the endpoint of the lane line acquired from map information and the measured position of the endpoint of the lane line measured by a measuring unit mounted on the mobile body scanning light in a predetermined direction, Estimation means for estimating the self-position of the moving object by correcting the predicted position of the moving object with a value obtained by multiplying the difference value by a coefficient, A self-localization device equipped with the following features.

Images