Position estimation device, estimation device, control method, program and storage medium

Abstract

To provide a position estimation device capable of suitably performing own vehicle position estimation even when the number of landmarks capable of being detected is small.SOLUTION: An own vehicle position estimation unit 17 of an on-vehicle device 1 acquires a predicted own vehicle position X-(t), which is a predicted value of the current position and direction of a moving body. The own vehicle position estimation unit 17 acquires a relative angle Lψ(t), which is first direction information indicating a direction of a landmark with respect to a vehicle based on measurement data of the landmark by a rider 2, and acquires a relative angle Lψ-(t), which is second direction information indicating a direction obtained by converting the direction of the landmark included in the landmark information into a direction with respect to the vehicle. The own vehicle position estimation unit 17 corrects the predicted own vehicle position X-(t) based on the difference between the relative angle Lψ(t) and the relative angle Lψ-(t).SELECTED DRAWING: Figure 9

Description

[Technical Field]

[0001] The present invention relates to location estimation technology. [Background technology]

[0002] Conventionally, there have been known techniques for detecting features installed ahead of a vehicle using a radar or a camera and calibrating the vehicle's position based on the detection results. For example, Patent Document 1 discloses a technique for estimating the vehicle's position by comparing the output of a measurement sensor with position information of features pre-registered on a map. Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter. Furthermore, Patent Document 3 discloses a technique for calculating a normal angle indicating the orientation of a sign based on point cloud data of the sign measured by a lidar. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2013-257742 [Patent Document 2] Japanese Patent Application Publication No. 2017-072422 [Patent Document 3] Japanese Patent Application Publication No. 2017-211307 Summary of the Invention [Problem to be solved by the invention]

[0004] On roads with road signs and white lines, such as expressways and major national highways, there are many landmarks whose measurements can be compared with the map. On the other hand, in suburban areas, there are roads with few landmarks, and in some places, for example, only one road sign or only one side of the white line can be detected. Also, while a vehicle equipped with a high-performance external sensor can measure a sufficient distance and angle range, relatively low-cost external sensors have a short measurable distance and a narrow detection angle. In such cases, even if there are many landmarks on the road, the number of landmarks that can be measured is limited.

[0005] The present invention has been made to solve the above-mentioned problems, and its main purpose is to perform suitable vehicle position estimation even in situations where the number of detectable landmarks is small. [Means for solving the problem]

[0006] The claimed invention is A position estimation device, comprising: an acquisition unit that acquires a predicted value including a current position of a moving object and an orientation of the moving object; a first calculation unit that calculates a first direction indicating an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measurement device mounted on the moving body; a second calculation unit that calculates a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; a correction unit that corrects the predicted value of the current position by a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and corrects the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain; It has.

[0007] The claimed invention is an estimation device, a first calculation unit that calculates a first direction that indicates an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measurement device mounted on the moving body; a second calculation unit that calculates a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; an estimation unit that estimates a current position of the moving object based on a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and that estimates a direction of the moving object based on a value obtained by multiplying the difference by a second Kalman gain; It has.

[0008] The claimed invention is a control method executed by a position estimation device, an acquisition step of acquiring predicted values ​​including a current position of the moving object and an orientation of the moving object; a first calculation step of calculating a first direction indicating an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measuring device mounted on the moving body; a second calculation step of calculating a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; a correction step of correcting the predicted value of the current position by a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and correcting the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain; It has.

[0009] The claimed invention also includes: A computer-executable program, an acquisition unit that acquires a predicted value including a current position of a moving object and an orientation of the moving object; a first calculation unit that calculates a first direction indicating an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measurement device mounted on the moving body; a second calculation unit that calculates a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; a correction unit that corrects the predicted value of the current position by a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and corrects the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain; The computer functions as follows. [Brief explanation of the drawings]

[0010] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Figure 2] FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device. [Figure 3] 1 is an example of a data structure of a map DB. [Figure 4] This is a diagram showing a state variable vector in two-dimensional orthogonal coordinates. [Figure 5] FIG. 10 is a diagram illustrating a schematic relationship between a prediction step and a measurement update step. [Figure 6] 1 is a diagram showing the positional relationship between a vehicle and landmarks in a world coordinate system and a vehicle coordinate system. FIG. [Figure 7] 3 shows an example of a functional block of a vehicle position estimation unit. [Figure 8] (A) A front view of the illuminated surface of a landmark, showing the lidar measurement points. (B) A diagram showing the scan lines and the center points of each scan line within the prediction window when the lane markings are dashed. (C) A diagram showing the scan lines and the center points of each scan line within the prediction window when the lane markings are deteriorated. [Figure 9] 10 is a flowchart of a vehicle position estimation process. [Figure 10] 1 is a diagram showing the positional relationship between a vehicle and landmarks in a world coordinate system and a vehicle coordinate system. FIG. [Figure 11] FIG. 10 is a diagram showing a simulation result of vehicle position estimation based on a Kalman filter according to a comparative example. [Figure 12] FIG. 10 is a plan view showing the trajectory of the estimated position of the vehicle based on the Kalman filter according to the embodiment, together with the correct position of the vehicle and the positions of landmarks. [Figure 13] (A) A graph showing the error in the x direction of the estimated value relative to the true value. (B) A graph showing the error in the y direction of the estimated value relative to the true value. (C) A graph showing the error in the direction ψ of the estimated value relative to the true value. DETAILED DESCRIPTION OF THE INVENTION

[0011] According to a preferred embodiment of the present invention, a position estimation device includes: an acquisition unit that acquires a predicted value including a current position of a mobile body and an orientation of the mobile body; a first calculation unit that calculates a first direction indicating the orientation of the feature in a coordinate system of the mobile body based on measurements of the feature by a measurement device mounted on the mobile body; a second calculation unit that calculates a second direction indicating the orientation obtained by converting the orientation of the feature included in map information into the coordinate system of the mobile body; and a correction unit that corrects the predicted value based on a difference between the first direction and the second direction. With this aspect, the self-location estimation device preferably corrects the predicted value including the current position of the mobile body and the orientation of the mobile body in accordance with the difference between the orientation of the feature based on the measurements of the feature by the measurement device and the orientation of the feature based on the map information, thereby enabling accurate position estimation even when the number of features to be measured is small.

[0012] In one aspect of the position estimation device, the correction unit corrects the predicted value based on the first difference, which is the difference, and a second difference between the distance measured from the moving object to the feature by the measurement device and a predicted distance from the moving object to the feature predicted based on position information of the feature included in the map information. According to this aspect, the position estimation device can uniquely determine a correction amount for the predicted values ​​of the current position and orientation of the moving object based on the first difference and the second difference, thereby enabling accurate position estimation even when there is only one feature to be measured by the measurement device.

[0013] In another aspect of the position estimation device, the correction unit corrects the predicted value by a value obtained by multiplying the difference by a Kalman gain. In a preferred example, the correction unit corrects the predicted value of the current position by a value obtained by multiplying the difference by a first Kalman gain, and corrects the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain. This aspect enables the position estimation device to accurately determine the amount of correction for the current position indicated by the predicted value and the amount of correction for the orientation indicated by the predicted value, based on the difference between the orientation of a feature based on a measurement value of the feature by a measurement device and the orientation of the feature based on map information.

[0014] In another aspect of the position estimation device, the position estimation device includes a third calculation unit that calculates the measurement accuracy of the first direction based on the distance measurement accuracy of the measurement device, and the correction unit determines an observation noise coefficient used to calculate the Kalman gain based on the measurement accuracy of the distance and orientation. With this aspect, the position estimation device can calculate a Kalman gain that appropriately reflects the distance and orientation accuracy of the measurement device.

[0015] According to another preferred embodiment of the present invention, an estimation device includes: a first calculation unit that calculates a first direction indicating an orientation of a feature in a coordinate system of the moving body based on measurements of the feature by a measuring device mounted on the moving body; a second calculation unit that calculates a second direction indicating an orientation obtained by converting an orientation of the feature included in map information into the coordinate system of the moving body; and an estimation unit that estimates at least one of a current position and orientation of the moving body based on a difference between the first direction and the second direction. According to this aspect, the estimation device preferably corrects a predicted value of at least one of the current position of the moving body or the orientation of the moving body in accordance with the difference between the orientation of the feature based on the measurements of the feature by the measuring device and the orientation of the feature based on the map information, thereby enabling accurate position estimation even when the number of features to be measured is small.

[0016] According to another preferred embodiment of the present invention, there is provided a control method executed by a position estimation device, the control method comprising: an acquisition step of acquiring a predicted value including a current position of a mobile body and an orientation of the mobile body; a first calculation step of calculating a first direction indicating the orientation of the feature in a coordinate system of the mobile body based on measurements of the feature by a measuring device mounted on the mobile body; a second calculation step of calculating a second direction indicating a direction obtained by converting the orientation of the feature included in map information into the coordinate system of the mobile body; and a correction step of correcting the predicted value based on a difference between the first direction and the second direction. By executing this control method, the position estimation device can preferably correct the predicted value including the current position of the mobile body and the orientation of the mobile body, and can perform accurate position estimation even when the number of features to be measured is small.

[0017] According to another preferred embodiment of the present invention, a computer-executable program causes the computer to function as an acquisition unit that acquires predicted values ​​including the current position and orientation of a mobile object, a first calculation unit that calculates a first direction indicating the orientation of the object in the coordinate system of the mobile object based on measurements of the object by a measuring device mounted on the mobile object, a second calculation unit that calculates a second direction indicating the orientation of the object obtained by converting the orientation of the object contained in map information into the coordinate system of the mobile object, and a correction unit that corrects the predicted values ​​based on the difference between the first direction and the second direction. By executing this program, the computer can preferably correct the predicted values ​​including the current position and orientation of the mobile object, thereby enabling accurate position estimation even when the number of objects to be measured is small. Preferably, the program is stored in a storage medium. [Example]

[0018] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, in this specification, a character with "^" or "-" above any symbol will be referred to as "A ^ " or "A - " (where "A" is any letter).

[0019] [Schematic configuration] Fig. 1 is a schematic configuration diagram of a driving assistance system according to this 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, an IMU (Inertial Measurement Unit) 3, a vehicle speed sensor 4, and a GPS receiver 5.

[0020] The vehicle-mounted device 1 is electrically connected to a lidar 2, an IMU 3, a vehicle speed sensor 4, and a GPS receiver 5, and estimates the position of the vehicle (also referred to as "host vehicle position") on which the vehicle-mounted device 1 is mounted based on the outputs of these devices. Based on the estimated host 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) 10 that stores landmark information, which is information on road data and landmark features (also referred to as "landmarks") located near roads. The landmarks mentioned above are, for example, kilometer posts, 100-meter posts, delineators, traffic infrastructure (e.g., signs, direction signs, traffic lights), utility poles, streetlights, and dividing lines (white lines) that are periodically lined along the side of the road. Note that the dividing lines are not limited to solid lines and may be dashed lines. Based on this landmark information, the vehicle-mounted device 1 compares it with the outputs of the lidar 2 and the like to estimate the host vehicle position. The vehicle-mounted device 1 is an example of the "position estimation device" of the present invention.

[0021] The lidar 2 emits a pulsed laser beam within a predetermined angular range in the horizontal and vertical directions to discretely measure the distance to an object in the external world and generate 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 light (scattered light) reflected from the irradiated laser light by the object, and an output unit that outputs scan data based on the light receiving signal output by the light receiving unit. The scan data is point cloud data and 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, which is determined based on the above-mentioned light receiving signal. The lidar 2, IMU 3, vehicle speed sensor 4, and GPS receiver 5 each supply output data to the onboard device 1. The lidar 2 is an example of a "measurement device" 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 interconnected via a bus line.

[0023] The interface 11 acquires output data from sensors such as the lidar 2, the IMU 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 landmark 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 landmark information.

[0025] Landmark information is information that associates information about each landmark feature with that feature, and here includes a landmark ID (equivalent to a landmark index), location information, orientation information, and size information. The location information indicates the absolute location of the landmark, expressed by latitude and longitude (and altitude), etc. If the landmark is a marking line, the corresponding location information includes at least coordinate information indicating the discrete positions of the marking line. The orientation information indicates the orientation (or direction) of the landmark. For landmarks with planar shapes, such as signs, the orientation information indicates the normal direction (normal vector) to the surface of the landmark, or the vector obtained by projecting the normal vector onto the xy plane. For marking lines on road surfaces, the orientation information indicates the direction in which the marking line extends. The size information indicates the size of the landmark, and may be, for example, information indicating the vertical and / or horizontal length (width) of the landmark, or information indicating the area of ​​the surface of the landmark.

[0026] 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's 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. Furthermore, instead of being stored in the vehicle-mounted device 1, the map DB 10 may be stored in an external storage device such as an external server. In this case, the vehicle-mounted device 1 acquires at least a part of the map DB 10 from the external storage device via wireless communication or the like.

[0027] The functional components of the in-vehicle device 1 will be described with reference to Fig. 2 again. The input unit 14 is a button, a touch panel, a remote controller, a voice input device, etc. that the user operates. The information output unit 16 is, for example, a display, a speaker, etc. that outputs information 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 that estimates the position of the vehicle on which the vehicle-mounted device 1 is placed (vehicle position) based on the output signals of each sensor supplied from the interface 11 and the map DB 10. The control unit 15 then performs control related to vehicle driving assistance, including automatic driving control, based on the result of estimating the vehicle position. The control unit 15 is an example of the "first calculation unit," "second calculation unit," "third calculation unit," "acquisition unit," "correction unit," "estimation unit," and "computer" that executes a program in the present invention.

[0029] [Outline of vehicle position estimation process] First, an overview of the process of estimating the vehicle position by the vehicle position estimating unit 17 will be described.

[0030] The vehicle position estimation unit 17 corrects the vehicle position estimated from the output data of the IMU 3, the vehicle speed sensor 4, and / or the GPS receiver 5 based on the distance and angle measurements from the LIDAR 2 relative to the landmarks and the landmark information extracted from the map DB 10. In this embodiment, as an example, the vehicle position estimation unit 17 alternately executes a prediction step in which the vehicle position is estimated from the output data of the IMU 3, the vehicle speed sensor 4, etc., based on a state estimation method based on Bayesian estimation, and a measurement update step in which the estimated value of the vehicle position calculated in the immediately preceding prediction step is corrected. Various filters developed for Bayesian estimation can be used as state estimation filters in these steps, such as an extended Kalman filter, an unscented Kalman filter, and a particle filter. As such, various methods have been proposed for position estimation based on Bayesian estimation. Below, a brief description is given of vehicle position estimation using an extended Kalman filter as an example.

[0031] 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 forward direction and the x-axis. The coordinates (x, y) are, for example, an absolute position equivalent to a combination of latitude and longitude, or world coordinates indicating a position with a predetermined point as the origin. Here, the vehicle's forward direction is the direction corresponding to the traveling direction when the vehicle is moving forward, and is an example of the orientation of a moving body.

[0032] FIG. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step. As shown in FIG. 5, the prediction step and the measurement update step are repeated to sequentially calculate and update the estimated value of the state variable vector "X" indicating the vehicle position. In FIG. 5, the state variable vector at the reference time (i.e., the current time) "t" to be calculated is calculated as "X - (t)" or "X ^ (t)" ("State variable vector X(t) = (x(t), y(t), ψ(t)) T "). Here, the provisional estimates (predicted values) estimated in the prediction step are indicated by " - " is added to the character representing the value, and the more accurate estimated value updated in the measurement update step is added with " ^ " is added.

[0033] In the prediction step, the vehicle position estimation unit 17 calculates the vehicle's moving speed "v" and angular velocity (yaw rate) "ω" (collectively referred to as the "control value u(t)=(v(t), ω(t)) T The vehicle position estimation unit 17 calculates the travel distance and change in direction from the previous time by using the state variable vector X ^ The calculated travel distance and direction change are added to (t-1) to obtain the predicted value of the 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 corresponding to the error distribution of (t) -(t)” is the covariance matrix “P ^ (t-1)".

[0034] In the measurement update step, the vehicle position estimation unit 17 associates the position vector of the landmark to be measured registered in the map DB 10 with the scan data of the LIDAR 2. Then, when the vehicle position estimation unit 17 has established this association, it calculates the measurement value "L(t)" of the landmark that has been associated by the LIDAR 2 and the predicted vehicle position X - (t) and the landmark position vector registered in the map DB 10, and the landmark measurement predicted value (referred to as the "measurement predicted value") "L - The measurement value L(t) is a vector value in a coordinate system (also called the "vehicle coordinate system") that is converted from the distance and scan angle of the landmark measured by LIDAR 2 at time t into components with axes in the front and lateral directions of the vehicle.

[0035] Then, in the measurement update step, the vehicle position estimation unit 17 calculates the measured value L(t) and the predicted measured value L as shown in the following equation (1): - The difference between (t) and (t) is multiplied by the Kalman gain "K(t)" and used as the predicted vehicle position X - (t) to obtain the updated state variable vector (also called the "estimated vehicle position") X ^ Calculate (t).

[0036]

number

[0037] When the vehicle position estimation unit 17 is able to associate the position vector of a landmark registered in the map DB 10 with the scan data of the LIDAR 2 for multiple landmarks, it may perform the measurement update step based on any selected set of measurement predicted values ​​and measurement values, etc., or may perform the measurement update step multiple times based on all the associated measurement predicted values ​​and measurement values, etc. When multiple measurement predicted values ​​and measurement values, etc. are used, the vehicle position estimation unit 17 may consider that the LIDAR measurement accuracy deteriorates as the landmark is farther from the LIDAR 2, and may decrease the weighting for the landmark as the distance between the LIDAR 2 and the landmark increases.

[0038] [Vehicle position estimation using landmark orientation information] Next, the vehicle position estimation process using landmark orientation information will be described. In summary, the vehicle position estimation unit 17 measures the orientation of the landmark to be measured and acquires the orientation information of the landmark registered in the map DB 10, thereby forming a Kalman filter using the difference between the measured and predicted values ​​of the orientation in addition to the position, and derives the estimated vehicle position X ^ As a result, even when the vehicle position estimation process is performed using one landmark as the measurement target, a highly accurate estimated vehicle position X ^ Calculate (t).

[0039] (1) Processing Overview FIG. 6A is a diagram showing the positional relationship between a vehicle and a landmark in the world coordinate system and the vehicle coordinate system when the landmark to be measured is a signboard. Here, the world coordinate system has a predetermined point as its origin and mutually perpendicular coordinate axes "x w " and coordinate axis "y w The vehicle coordinate system has a coordinate axis "x" along the front direction of the vehicle, with the center of the vehicle as the origin. b ” and the coordinate axis along the side of the vehicle, “y b In FIG. 6A, the predicted vehicle position in the world coordinate system is "X - =(x - , y -, ψ - ) T ”, and the position vector “M=(M x , M y , M ψ ) T ", the measurement value in the vehicle coordinate system "L = (L x , L y , L ψ ) T Each element of " is illustrated.

[0040] In this case, the vehicle position estimation unit 17 calculates the measured value L(t)=(L x (t), L y (t), L ψ (t)) T is calculated using the distance "r(t)" and direction "θ" of the measurement target relative to the landmark output by the lidar 2, using the following equation (2).

[0041]

number

[0042] Here, the vehicle position estimation unit 17 calculates the relative angle L in equation (2) that indicates the orientation of the measured landmark. ψ can be calculated from the point cloud data of the lidar 2 that measured the measurement surface of the landmark. Specifically, the vehicle position estimation unit 17 obtains an equation that represents the plane of the landmark by regression analysis from the coordinates of each measurement point on the plane of the landmark, obtains a normal vector to this plane, and projects it onto the xy plane to obtain the relative angle L of the landmark in the vehicle coordinate system. ψ The details of this process are disclosed in, for example, Patent Document 3. The relative angle L ψ is an example of the "first direction" in the present invention.

[0043] Furthermore, the vehicle position estimation unit 17 calculates the predicted measurement value L - (t)=(L- x (t), L - y (t), L - ψ (t)) T The predicted vehicle position X - (t)=(x - (t), y - (t), ψ - (t)) T and the landmark's position vector on the map M = (M x (t), M y (t), M ψ (t)) T and is calculated based on the following formula (3).

[0044]

number

[0045] Furthermore, the vehicle position estimation unit 17 calculates the relative angle L ψ The measured value L(t) and the relative angle L - ψ Measurement prediction value L including - The difference between the vehicle position and the target position is multiplied by the Kalman filter K(t) to obtain the predicted vehicle position X - (t) corrected estimated vehicle position X ^ That is, the vehicle position estimation unit 17 calculates the estimated vehicle position X ^ (t)=(x ^ (t), y ^ (t), ψ ^ (t)) T is calculated based on the following formula (4), which corresponds to formula (1).

[0046]

number

[0047] 6(B) is a diagram showing the positional relationship between the vehicle and the landmark in the world coordinate system and the vehicle coordinate system when the landmark to be measured is a lane marking. Similarly, when the landmark to be measured is a lane marking, the vehicle position estimation unit 17 calculates the measured value L(t) and the predicted measured value L based on the formulas (2) to (4). - (t), and estimated vehicle position X ^ (t) are calculated respectively.

[0048] Here, the predicted measurement value L based on Equation (3) when the landmark to be measured is a lane marking - A supplementary explanation will be given on the calculation method of (t). When the landmark is a lane marking, the corresponding landmark information includes, as orientation information, an orientation M indicating the direction along the lane marking in the world coordinate system (extension direction), as shown in FIG. 6(B). ψ Therefore, the vehicle position estimation unit 17 refers to the landmark information of the lane markings to calculate the predicted measurement value L - When calculating (t), this direction M ψ The calculation of formula (3) is performed using the direction M ψ If the landmark information does not include this information, the coordinate values ​​of several points before and after it are connected and the direction multiplication is calculated using the least squares method or the like.

[0049] Next, the measurement value L(t) = (Lx (t), L y (t), L ψ (t)) T Here, we will provide additional information on how to calculate the above formula. When the landmark is a latitude line, the landmark information includes, as location information, coordinate information such as latitude and longitude that indicates discrete positions (discrete points) of the latitude line, for example, at intervals of several meters.

[0050] In this case, first, the vehicle position estimation unit 17 sets a prediction window "Wp" that determines the range in which to detect the lane markings, with the predicted position of the closest discrete point of the lane markings from a position a predetermined distance away as the center in at least one of the directions of the left front, left rear, right front, and right rear of the vehicle (left front in FIG. 6(B)). Then, the vehicle position estimation unit 17 extracts measurement points within the prediction window Wp that are on the road surface and have a high reflectance that is equal to or greater than a predetermined threshold value from the measurement data output by the LIDAR 2, and calculates the x coordinate of the center position of each position indicated by the extracted measurement points. b Coordinates and y b The coordinates are L x (t), L y The vehicle position estimation unit 17 also calculates the center point of each scanning line on the lane marking (for example, the center position of the measurement point for each scanning line), and calculates the relative angle L based on the slope of a regression line obtained from these center points using the least squares method or the like. ψ Calculate (t).

[0051] (2) Block configuration Next, the functional block configuration of the vehicle position estimation unit 17 will be described.

[0052] Fig. 7 shows an example of functional blocks of the vehicle position estimation unit 17. As shown in Fig. 7, the vehicle position estimation unit 17 includes a dead reckoning block 20, a movement correction block 21, a landmark extraction block 22, and a localization EKF (Extended Kalman Filter) block 23.

[0053] The dead reckoning block 20 calculates the vehicle's moving speed v, yaw rate ω, and vehicle attitude information (e.g., the vehicle's yaw angle, pitch angle, and roll angle) based on the outputs of the IMU 3, the vehicle speed sensor 4, and the GPS receiver 5. The localization EKF block 23 then supplies the moving speed v and vehicle attitude information to the movement correction block 21, and also outputs a control value u(t)=(v(t), ω(t)) to the localization EKF block 23. T supply.

[0054] The movement correction block 21 corrects the measurement data output from the LIDAR 2 based on the movement speed v and attitude information supplied from the dead reckoning block 20. For example, if the scan period is 100 ms, the vehicle moves during that 100 ms, so even if the same object located ahead is measured, the measurement distance will be long at the start of scanning in one scan frame and short at the end. Therefore, the above correction is used to keep the measurement distance within one scan frame constant. The movement correction block 21 then supplies the corrected measurement data of the LIDAR 2 to the landmark extraction block 22.

[0055] The landmark extraction block 22 extracts the estimated vehicle position X calculated by the localization EKF block 23 at the previous time. ^ Based on (t-1), the position vector M of the landmarks around the vehicle that can be measured by Lidar 2 is calculated as M = (M x , M y , M ψ ) T is extracted from the map DB10, and the measurement value L(t)=(L x , L y , L ψ ) T and the position vector M=(M x , M y , M ψ ) T and are supplied to the localized EKF block 23.

[0056] In this case, the landmark extraction block 22 converts the measurement data of the lidar 2, which indicates the combination of (r(t), θ(t)), supplied from the movement correction block 21, into components with axes in the front direction and lateral direction of the vehicle, based on equation (2), into a measurement value L(t)=(L x , L y , L ψ ) T In this case, the landmark extraction block 22 may convert the measurement data of the LIDAR 2 expressed in a coordinate system based on the LIDAR 2 into a measurement value L(t) in the vehicle coordinate system, for example, based on information about the installation position and installation attitude of the LIDAR 2 relative to the vehicle stored in the storage unit 12 or the like.

[0057] In reality, the landmarks have a predetermined size, and data corresponding to a plurality of measurement points on the irradiated surface irradiated with the laser light of the lidar 2 is acquired as point cloud data. Therefore, for example, the landmark extraction block 22 calculates a measurement value L(t) corresponding to the center position of the target landmark by averaging the measurement values ​​at each measurement point where the target landmark is measured, and supplies this to the localization EKF block 23.

[0058] The localization EKF block 23 calculates the estimated vehicle position X by performing the calculation shown in Equation (4). ^ (t), and calculate the Kalman gain K(t) and covariance matrix P ^ In this case, the localization EKF block 23 updates the estimated vehicle position X (t) calculated at the previous time. ^ (t-1) and the control value u(t) supplied from the dead reckoning block 20. - (t) and calculates the position vector M and predicted vehicle position X supplied from the landmark extraction block 22. - (t) using equation (3) to calculate the predicted measurement value L - The method for updating the Kalman gain K(t) will be described later.

[0059] (3) How to update the Kalman gain Next, a method for updating the Kalman gain K(t) will be described. In summary, the vehicle position estimation unit 17 calculates diagonal components corresponding to the three elements of the x direction, y direction, and direction ψ of the observation noise matrix "R(t)" used when calculating the Kalman gain K(t) based on the measurement accuracy of the LIDAR 2 and the accuracy of the map DB 10 (i.e., landmark information). As a result, the vehicle position estimation unit 17 calculates the measured value L(t) and the predicted measurement value L for the three elements of the x direction, y direction, and direction ψ. - Each element of the Kalman gain K(t) to be multiplied by the difference value with (t) is suitably determined.

[0060] The Kalman gain K(t) is the sum of the observation noise matrix R(t) and the measurement prediction value L - (t) 3x3 Jacobian matrix "H(t)" and 3x3 covariance matrix P - (t) is calculated using the following general formula (5).

[0061]

number

[0062]

number

[0063] Next, the above-mentioned measurement accuracy σ Lψ (i) and noise coefficient a Lψ The method for setting the measurement accuracy σ Lx (i), σ Ly Regarding (i), for example, the measurement accuracy of the lidar 2 described in the specifications (or obtained based on experiments) is set to x b direction and y b It can be determined by decomposing the noise coefficient a Lx , a Ly may be set to a predetermined value, and the noise coefficient a Lψ It may be set to the same value as the map accuracy σ Mx (i), σ My (i), σ Mψ (i) and the noise coefficient of the map a Mx , a My , a Mψ For example, x may be determined based on the information about accuracy recorded in the map DB 10, or may be set to a predetermined value. Note that the map accuracy is based on the world coordinate system, so it must be converted into the accuracy of the vehicle coordinate system. w direction and y w If the accuracy is the same between the direction and the vehicle coordinate system, the accuracy will be the same even if converted to the vehicle coordinate system, so no conversion is necessary.

[0064] First, the measurement accuracy σ Lψ Regarding (i), for example, the vehicle position estimation unit 17 calculates the distance x to the landmark to be measured. bMeasurement accuracy of direction and y b The accuracy of the direction measurement is used to calculate the following equation (7).

[0065]

number

[0066] Next, the noise coefficient a Lψ The setting method for the case where the landmark to be measured is a feature other than a division line (for example, a sign or sign) will be described below. Fig. 8(A) shows a front view of the illuminated surface of the landmark, clearly indicating the measurement points of the lidar 2. In Fig. 8(A), the measurement points of the landmark are represented by dots, and points that hit the edge of the landmark or other measurement points that are not illuminated on the landmark are represented by circles.

[0067] The vehicle position estimation unit 17 calculates a noise coefficient a based on the number of measurement points of the point cloud data of the lidar 2 acquired for the landmark to be measured. Lψ Specifically, the vehicle position estimation unit 17 determines the noise coefficient a based on the following equation (8), where "Np" is the number of measurement points for the landmark to be measured and "C" is a predetermined constant. Lψ Calculate.

[0068]

number

[0069] Next, the noise coefficient a when the landmark to be measured is a lane marking Lψ This section explains how to set this up.

[0070] In this case, the vehicle position estimation unit 17 calculates a noise coefficient a based on the difference between the expected value and the measured value of the number of scanning lines in the prediction window Wp and the width of the lane marking. Lψ Specifically, the vehicle position estimation unit 17 sets the expected value of the number of scanning lines on the lane markings within the prediction window Wp (i.e., the number of scanning lines within the prediction window Wp) as "N M ”, and the measurement value of the number of scan lines on the partition line within the prediction window Wp is “N L ”, and the expected value of the width of each scanning line of the lane marking in the prediction window Wp (i.e., the width of the lane marking to be measured registered in the map DB 10) is defined as “W M ”, and the measurement value of the width of each scan line of the parcel line within the prediction window Wp is defined as “W L " Then, based on the following equation (9), the noise coefficient a Lψ Calculate.

[0071]

number

[0072] Fig. 8(B) is a diagram showing scan lines L1-L4 and center points P1-P4 of each scan line within the prediction window Wp when the lane markings are broken lines, and Fig. 8(C) is a diagram showing scan lines L1-L10 and center points P1-P10 of each scan line within the prediction window Wp when the lane markings are deteriorated.

[0073] In the example of FIG. 8B, the division line to be measured is a broken line, so there are discontinuous portions within the prediction window Wp. Therefore, in this case, the expected value N M (Here, 10) and the measured value N L (Here, the difference with 4) becomes large, and the noise coefficient a based on equation (9) Lψ is the measured value N L The expected value for N M In the example of FIG. 8(C), the lane markings to be measured are degraded, and the expected value W M The measured value W is L Therefore, in this case, the expected value for each scan line W M and the measured value W L The integrated value of the difference between Lψ increases according to the integrated value above.

[0074] Here, a supplementary explanation will be given for Equation (9). As described above, the vehicle position estimation unit 17 calculates the relative angle L in the vehicle coordinate system, which indicates the direction in which the lane marking extends, based on the regression line obtained by the least squares method or the like from the center points of each scanning line on the lane marking. ψ Therefore, the fewer the number of center points of the scanning line, the smaller the relative angle L ψ In consideration of the above, the vehicle position estimation unit 17 calculates the measured value N L The smaller the value (i.e., the measured value N L The expected value for N M The larger the ratio of Lψ Make it bigger.

[0075] In addition, if the lane markings to be measured are deteriorated, the relative angle L ψ The width of the lane markings used to calculate (t) varies due to the presence of peeling parts, etc., and the center points of each scanning line vary accordingly, resulting in a relative angle L ψ Taking the above into consideration, the vehicle position estimation unit 17 calculates the width W of the lane marking to be measured based on the equation (9). M and the width measurement of the lot line W L The larger the difference between Lψ Make it bigger.

[0076] (4) Processing flow 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.

[0077] 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 acquires the angular velocity in the yaw direction from the IMU 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).

[0078] Thereafter, 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), landmark information in the map DB 10 is referenced to search for landmarks that are within the measurement range of the LIDAR 2 (step S105).

[0079] Then, the vehicle position estimation unit 17 calculates the predicted vehicle position X based on the equation (3). -(t) and the position information and orientation information indicated by the landmark information of the landmark searched in step S105, a relative angle L indicating the predicted orientation of the landmark is calculated. - ψ Measurement prediction value L including - Furthermore, in step S106, the vehicle position estimation unit 17 calculates the relative angle L (t) indicating the predicted orientation of the landmark from the measurement data of the LIDAR 2 relative to the landmark searched for in step S105, based on the formula (2). ψ Measurement value L including ψ Calculate (t).

[0080] Then, the vehicle position estimation unit 17 calculates the observation noise matrix R(t) shown in equation (6) including the noise coefficient of the direction ψ based on the measurement accuracy of the LIDAR 2 and the map accuracy (step S107). Then, the vehicle position estimation unit 17 calculates a 3-row, 3-column Kalman gain K(t) using equation (5) based on the calculated observation noise matrix R(t) (step S108). This allows for the appropriate formation of a Kalman filter that uses the difference between the measured value and the predicted value of the direction ψ in addition to the position.

[0081] Then, the vehicle position estimation unit 17 calculates the predicted vehicle position X using the generated Kalman gain K(t) based on the equation (4). - By correcting (t), the estimated vehicle position X ^ (t) is calculated (step S109).

[0082] (5) effect Next, a supplementary explanation will be given of the effect of the vehicle position estimation process using landmark orientation information in this embodiment.

[0083] Here, for comparison with the Kalman filter according to this embodiment, a Kalman filter (also referred to as a "Kalman filter according to a comparative example") that does not use the difference between the measured value and the predicted value of the direction ψ will be considered. In this comparative example, the vehicle position estimation unit 17 calculates the measured value L(t) based on the following equation (10), and calculates the measured predicted value L based on the following equation (11). - Calculate (t).

[0084]

number

[0085]

number

[0086] Then, the vehicle position estimation unit 17 calculates the estimated vehicle position X based on the equation (12). ^ Calculate (t).

[0087]

number

[0088] In this case, in equation (4), the estimated vehicle position X ^ Each element of x ^ , y ^ , ψ ^ To calculate these three parameters, the measured value L(t) and the predicted value L for the two elements in the x and y directions are used. - Therefore, in this case, the difference value between the estimated vehicle position X ^ When calculating (t), the problem arises that the position and orientation cannot be uniquely determined.

[0089] Figure 10(A) and (B) show the same measured value L x , L y 10(A) and 10(B) are diagrams showing different positional relationships between a vehicle and landmarks in the world coordinate system and the vehicle coordinate system. x , L y Even if the calculated value L x , L y The positional relationship between the vehicle and the landmarks is not uniquely determined by the above alone, and the positional relationships between the vehicle and the landmarks are different from each other. Specifically, in the example of FIG. 10(B), x - is large, and y -is small and the direction ψ - is getting bigger.

[0090] Next, a description will be given of simulation results when the Kalman filter according to the present embodiment is applied and when the Kalman filter according to the comparative example is applied. As an example, the applicant intentionally calculated the predicted vehicle position X - The simulation was carried out for 60 seconds with error information (speed 4 [m / s], angular velocity 0.4 [rad / s]) added to (t).

[0091] 11(A) and 11(B) are diagrams showing simulation results of vehicle position estimation based on a Kalman filter according to a comparative example. Specifically, FIG. 11(A) shows the trajectory of the vehicle's estimated position (estimated value) based on the Kalman filter according to the comparative example, along with the vehicle's correct position (true value) and the positions of landmarks. Note that the arrows in the figure indicate the orientation of the landmark or vehicle. FIG. 11(B) is a graph showing the error of the estimated value relative to the true value for each of the x direction, y direction, and direction ψ.

[0092] As shown in Figures 11A and 11B, in the comparative example, the distance from the vehicle to the landmark remains constant, but the estimated position of the vehicle gradually deviates from the true value. ^ Each element of x ^ , y ^ , ψ ^ To calculate these three parameters, the measured value L(t) and the predicted value L for the two elements in the x and y directions are used. - (t) is used to calculate the difference between the predicted vehicle position X - (t) cannot be accurately corrected.

[0093] 12 and 13 are diagrams showing simulation results of vehicle position estimation based on the Kalman filter according to this embodiment. Specifically, FIG. 12 shows the trajectory of the estimated position (estimated value) of the vehicle based on the Kalman filter (see equation (4)) according to this embodiment, together with the actual position (true value) of the vehicle and the positions of landmarks. Also, FIG. 13(A) is a graph showing the error in the x direction of the estimated value relative to the true value, FIG. 13(B) is a graph showing the error in the y direction of the estimated value relative to the true value, and FIG. 13(C) is a graph showing the error of the direction ψ of the estimated value relative to the true value. Note that the scale of the vertical axis of the graphs shown in FIGS. 13(A) to (C) is much smaller than the scale of the vertical axis of the graph shown in FIG. 11(B), which shows the error related to the comparative example.

[0094] As shown in FIGS. 12 and 13, in this embodiment, the orientation of the landmark as seen from the vehicle is suitably fixed by applying a Kalman filter using the orientation information of the landmark. Therefore, the predicted vehicle position X - (t) is appropriately corrected, and there is almost no deviation in the position estimation. In this way, when the Kalman filter according to this embodiment is used, even when one landmark is the measurement target, an accurate estimated vehicle position X ^ (t) can be conveniently calculated.

[0095] As described above, the vehicle position estimation unit 17 of the vehicle-mounted device 1 according to this embodiment estimates the predicted vehicle position X - Then, the vehicle position estimation unit 17 obtains the relative angle L (t), which is first orientation information indicating the orientation of the landmark with respect to the vehicle based on the measurement data of the landmark by the LIDAR 2. ψ (t) is acquired, and a relative angle L, which is second orientation information indicating the orientation of the landmark included in the landmark information converted into the orientation relative to the vehicle, is acquired. - ψ Then, the vehicle position estimation unit 17 obtains the relative angle L ψ (t) and relative angle L - ψ Based on the difference with (t), the predicted vehicle position X -As a result, even in a situation where only one landmark can be detected, the vehicle position estimation unit 17 can estimate the predicted vehicle position X , which is a predicted value of the current position and orientation of the moving object. - (t) is accurately corrected to estimate the vehicle position X ^ (t) can be determined.

[0096] [Variations] The configuration of the driving assistance system shown in Fig. 1 is one 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 that executes 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 as needed from a server device (not shown). [Explanation of symbols]

[0097] 1 Onboard device 2 Rider 3. IMU 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims

1. an acquisition unit that acquires a predicted value including a current position of a moving object and an orientation of the moving object; a first calculation unit that calculates a first direction indicating an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measurement device mounted on the moving body; a second calculation unit that calculates a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; a correction unit that corrects the predicted value of the current position by a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and corrects the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain; A position estimation device having:

2. 2. The position estimation device according to claim 1, wherein the correction unit corrects the predicted value based on a first difference, which is the difference, and a second difference between a distance measured by the measurement device from the moving body to the feature and a predicted distance from the moving body to the feature that is predicted based on position information of the feature included in the map information.

3. The position estimation device according to claim 1 , wherein the correction unit corrects the predicted value by a value obtained by multiplying the difference by a Kalman gain.

4. a third calculation unit that calculates a measurement accuracy in the first direction based on the distance measurement accuracy of the measurement device; 4. The position estimation device according to claim 1, wherein the correction unit determines an observation noise coefficient used in calculating the Kalman gain based on measurement accuracy of the distance and direction.

5. a first calculation unit that calculates a first direction that indicates an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measurement device mounted on the moving body; a second calculation unit that calculates a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; an estimation unit that estimates a current position of the moving object based on a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and that estimates a direction of the moving object based on a value obtained by multiplying the difference by a second Kalman gain; An estimation device comprising:

6. A control method executed by a position estimation device, comprising: an acquisition step of acquiring predicted values ​​including a current position of the moving object and an orientation of the moving object; a first calculation step of calculating a first direction indicating an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measuring device mounted on the moving body; a second calculation step of calculating a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; a correction step of correcting the predicted value of the current position by a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and correcting the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain; A control method comprising:

7. A computer-executable program, an acquisition unit that acquires a predicted value including a current position of a moving object and an orientation of the moving object; a first calculation unit that calculates a first direction indicating an orientation of the feature in a coordinate system of the moving body based on a measurement value of the feature by a measurement device mounted on the moving body; a second calculation unit that calculates a second direction indicating a direction obtained by converting the orientation of the feature included in the map information into a coordinate system of the moving body; a correction unit that corrects the predicted value of the current position by a value obtained by multiplying a difference between the first direction and the second direction by a first Kalman gain, and corrects the predicted value of the orientation by a value obtained by multiplying the difference by a second Kalman gain; A program that causes the computer to function as a

8. A storage medium that stores the program according to claim 7.

Images