JP7298882B2 - Vehicle self-localization device and vehicle - Google Patents

Description

Application of Article 30, Paragraph 2 of the Patent Law 1. June 25, 2018, The 29th IEEE Intelligent Vehicles Symposium FINAL PROGRAM, pp. 971-977, Institute of Electrical and Electronics Engineers (IEEE)/IEEE Intelligent Transportation Systems Society (ITSS)2. Presented at The 29th IEEE Intelligent Vehicles Symposium held on June 28, 2018

The present invention relates to a vehicle self-position estimation device for estimating the self-position of a vehicle in real time, and a vehicle equipped with the self-position estimation device.

BACKGROUND ART In recent years, for the purpose of reducing traffic accidents, alleviating traffic congestion, providing means of transportation that anyone can use, and the like, the development of an automatic driving system that automatically drives a vehicle has been actively carried out. Self-localization, which estimates the position of the vehicle in real time, is necessary for an autonomous vehicle to correctly move toward its destination.
As a general method for estimating self-location, there is map matching using a predefined map (map image), and sensors such as LiDAR (Light Detection and Ranging) and cameras are mainly used in this method.
For example, Non-Patent Document 1 discloses a map matching method using LiDAR. The method using LiDAR has high measurement accuracy and is highly robust against changes in day and night and environmental changes, so it is possible to estimate the self-localization of a vehicle with decimeter-level accuracy.
In addition, in Non-Patent Document 2, by matching the characteristics of the road surface detected by a camera with a vector map (a digital map that records the positions of white lines, road signs, traffic lights, etc.), the self-position of the vehicle is calculated in the lateral direction of the vehicle. It is disclosed that estimation is performed with an accuracy of about 0.1 m in , and an accuracy of about 0.5 m in the longitudinal direction of the vehicle.

However, self-localization using LiDAR and cameras cannot be used when the road surface is covered with snow, when the shape of objects around the vehicle changes due to snowfall, or when the road surface cannot be accurately grasped due to rain. has the problem of unstable accuracy.
In order to solve such a problem, Non-Patent Document 3 discloses a method of using past frames to prevent the reflectance of LiDAR from decreasing on roads wet with rain or snow. However, this method cannot accurately estimate the self-position on roads that are completely covered with snow or where snow walls change the shape of the road.

Non-Patent Document 4 discloses a method of estimating a self-position based on road surface information using a millimeter wave radar and a camera. However, this method also cannot accurately estimate the self-position when it is snowing.
Non-Patent Document 5 discloses a method of generating a map using clustered millimeter-wave radar observation information and estimating the self-position using a particle filter. However, due to the sparse observation points by the millimeter-wave radar, it is described that the estimation accuracy exceeds the maximum error of 3m, and self-position estimation could only be performed with an accuracy of the meter level.

Further, in Japanese Patent Application Laid-Open No. 2002-100003, when it is determined that the camera unit does not recognize the left and right demarcation lines of the course of the vehicle due to the influence of snowfall or the like, the lateral position correction processing unit detects the driving lane of the vehicle on the road map. read the map curvature stored in , determine the leading curvature based on the trajectory of the preceding vehicle to be followed, determine the estimated lateral position deviation based on both curvatures and the vehicle speed, and calculate the estimated lateral position deviation from the target steering wheel angle. Disclosed is a vehicle driving support device that adds the lateral position steering wheel angle, which is a feedback term, to the lateral position of the own vehicle to set a new lateral position of the own vehicle, thereby causing the own vehicle to travel along the trajectory of the preceding vehicle. It is
However, the driving support device of Patent Document 1 does not accurately estimate the self-position of the vehicle even when it is snowing. In addition, since it uses the trajectory of the preceding vehicle, it is difficult to operate autonomous driving in situations where there is no preceding vehicle and the camera unit cannot recognize the left and right lane markings of the vehicle's course due to the effects of snowfall, etc. Become.

JP 2019-38396 A

J. Levinson and S. Thrun, “Robust Vehicle Localization in Urban Environments Using Probabilistic Maps”, Proceedings of 2010 IEEE International Conference on Robotics and Automation, pp. 4372-4378, 2010. Jo, Kichun, et al. "Precise Localization of an Autonomous Car Based on Probabilistic Noise Models of Road Surface Marker Features Using Multiple Cameras." IEEE Trans. Intelligent Transportation Systems vol.16, no.6, pp. 3377-3392, 2015 . M. Aldibaja, N. Suganuma and K. Yoneda, “Robust Intensity Based Localization Method for Autonomous Driving on Snow-wet Road Surface”, IEEE Transactions on Industrial Informatics, vol. 13, no. 5, pp. 2369-2378, 2017 . S. Park, D. Kim and K. Yi, “Vehicle Localization using an AVM camera for An Automated Urban Driving”, Proceedings of 2016 IEEE Intelligent Vehicles Symposium, pp. 871-876, 2016. F.Schuster, M.Worner, C.G. Keller, M. Haueis and C. Curio, “Robust localization based on radar signal clustering”, Proceedings of 2016 IEEE Intelligent Vehicles Symposium, pp. 839-844, 2016.

SUMMARY OF THE INVENTION Accordingly, it is an object of the present invention to provide a vehicle self-position estimation device capable of estimating the self-position with high accuracy, and a vehicle equipped with the device.

According to claim 1, the vehicle self-position estimation apparatus of the present invention performs matching between an observation image created in real time based on observation information of objects existing around the vehicle and a pre-created map image, thereby determining the position of the vehicle. A self-localization device for estimating a position, comprising a GNSS/IMU data acquisition unit that acquires data from GNSS satellites and an inertial measurement device, and a peripheral object information acquisition sensor that acquires observation information on objects existing around the vehicle. , a DR unit that estimates the position of the vehicle by dead reckoning based on the data acquired by the GNSS/IMU data acquisition unit, and the vehicle position estimated by the DR unit and observation information acquired by the peripheral object information acquisition sensor unit. Based on this, an observed image generation unit generates an observed image in which the positions of static objects among objects are mapped, a matching unit performs matching between the observed image and the map image, and an offset probability distribution is calculated based on the matching result. A probability update unit for updating, an offset update unit for updating the offset amount based on the probability distribution, and a post-correction position derivation for deriving the post-correction position of the vehicle by correcting the position of the vehicle estimated by the DR unit with the offset amount. The peripheral object information acquisition sensor unit can acquire observation information of the object through snow, the matching unit performs template matching, and the probability update unit calculates the correlation distribution obtained by template matching. is gamma-corrected to derive the likelihood distribution, and the likelihood distribution is used to update the probability distribution .

According to the second aspect of the present invention , in the vehicle self-position estimation device according to the first aspect, the probability updating unit integrates the likelihood distribution obtained at each time and normalizes the integrated likelihood distribution. is used to derive the cumulative distribution, and the probability distribution is updated based on the cumulative distribution.

According to a third aspect of the present invention, in the vehicle self-localization apparatus according to the first aspect, the probability updating unit derives a logarithmic odds value of the posterior probability using the likelihood distribution, and uses the logarithmic odds value to It is characterized by updating the probability distribution.

According to a fourth aspect of the present invention, in the vehicle self-position estimation device according to the second or third aspect, the offset updating unit updates the offset amount using a weighted average of the probability distribution. .

According to the fifth aspect of the present invention, in the vehicle self-position estimation device according to the third aspect, the offset updating unit sets the maximum value of the probability distribution as the uncertainty of the matching state, and weights according to the uncertainty. It is characterized by updating the offset amount.

According to a sixth aspect of the present invention, there is provided a vehicle equipped with the vehicle self-position estimating device according to any one of the first to fifth aspects.

ADVANTAGE OF THE INVENTION According to this invention, the precision of self-position estimation of a vehicle can be improved.

1 is a block diagram of a vehicle self-localization apparatus according to an embodiment of the present invention; FIG. Appearance of own vehicle equipped with the same self-localization device Diagram showing the installation position and observation area of the millimeter-wave radar in the vehicle Explanatory diagram of self-position estimation and offset correction by dead reckoning Block diagram of the same map image generation means A diagram showing an example of generating the same map image Illustration of existence likelihood definition using covariance matrix based on same observation error propagation Diagram showing an example of the generated observation image Diagram showing same-likelihood filtering Explanatory diagram of processing when multiple peaks of the same type occur A diagram showing the test results in the first area in the summer (no snow cover) of the first embodiment Figure showing the test results in the first area in the same winter (partial snow cover) Diagram showing the test results in the second area in the same winter (full snow cover) The figure which shows the test result of a 2nd Example

A vehicle self-position estimation device according to the first embodiment of the present invention includes a GNSS/IMU data acquisition unit that acquires data from GNSS satellites and an inertial measurement device, and acquires observation information on objects existing around the vehicle. a surrounding object information acquisition sensor unit, a DR unit that estimates the position of the vehicle by dead reckoning based on the data acquired by the GNSS/IMU data acquisition unit, and a vehicle position and surrounding object information acquisition sensor unit estimated by the DR unit Based on the observation information acquired in step 2, an observation image generation unit generates an observation image in which the positions of static objects are mapped, a matching unit performs matching between the observation image and the map image, and based on the matching result a probability updating unit that updates the probability distribution of the offset based on the probability distribution; an offset updating unit that updates the offset amount based on the probability distribution; and a post-correction position deriving unit for deriving the position.
According to the present embodiment, the accuracy of self-position estimation can be improved by correcting the position of the vehicle estimated by dead reckoning based on the result of matching.

According to a second embodiment of the present invention, in the vehicle self-position estimation device according to the first embodiment, the surrounding object information acquisition sensor unit can acquire observation information of the object through snow, and matching is performed. The section performs template matching, and the probability updating section performs gamma correction on the correlation distribution obtained by the template matching to derive a likelihood distribution, and updates the probability distribution using the likelihood distribution.
According to this embodiment, it is possible to accurately estimate the self-position even under snow conditions. Further, by performing template matching, it is possible to speed up the processing.

According to a third embodiment of the present invention, in the vehicle self-localization apparatus according to the second embodiment, the probability updating unit integrates the likelihood distribution obtained at each time, and calculates the integrated likelihood distribution as A cumulative distribution is derived by normalization, and the probability distribution is updated based on the cumulative distribution.
According to the present embodiment, the probability can be updated by reducing the degree of contribution of the correlation. Therefore, even if the surrounding object information acquisition sensor unit observes sparse objects, it is possible to accurately estimate the self-position.

According to a fourth embodiment of the present invention, in the vehicle self-localization apparatus according to the second embodiment, the probability updating unit derives the log odds value of the posterior probability using the likelihood distribution, and the log odds value is used to update the probability distribution.
According to the present embodiment, the probability can be updated by reducing the degree of contribution of the correlation. Therefore, even if the surrounding object information acquisition sensor unit observes sparse objects, it is possible to accurately estimate the self-position.

According to a fifth embodiment of the present invention, in the vehicle self-localization apparatus according to the third or fourth embodiment, the offset updating unit updates the offset amount using a weighted average of the probability distribution. .
According to this embodiment, it is possible to accurately estimate the self-position even when static objects such as fences and guardrails of the same pattern are continuously present on the side of the road.

According to a sixth embodiment of the present invention, in the vehicle self-localization apparatus according to the fourth embodiment, the offset updating unit uses the maximum value of the probability distribution as the uncertainty of the matching state, and follows the uncertainty. The offset amount is updated by weighting.
According to the present embodiment, weighting according to uncertainty prevents excessive offset updating in an unstable situation, and the self-position can be estimated more accurately.

A vehicle according to the seventh embodiment of the present invention is equipped with the vehicle self-position estimation device according to any one of the first to sixth embodiments.
According to the present embodiment, it is possible to provide an automatic driving vehicle that accurately estimates its own position in real time and moves to a destination safely and accurately.

A vehicle self-position estimation device according to a first embodiment of the present invention will now be described.
FIG. 1 is a block diagram of a vehicle self-position estimation device according to this embodiment.
The self-position estimation device 10 includes a GNSS/IMU data acquisition unit 11 that acquires data from GNSS (Global Navigation Satellite System) satellites and inertial measurement devices, and a peripheral device that acquires real-time observation information on objects existing around the vehicle. An object information acquisition sensor unit 12, a DR unit 13 that estimates the position of the vehicle by dead reckoning based on the data acquired by the GNSS/IMU data acquisition unit 11, and the vehicle position estimated by the DR unit 13 and peripheral object information acquisition. Based on the observation information acquired by the sensor unit 12, an observed image generation unit 14 that generates an observed image in which the positions of static objects among objects are mapped, a matching unit 15 that performs matching between the observed image and the map image, A probability updating unit 16 that updates the offset probability distribution based on the matching result, an offset updating unit 17 that updates the offset amount based on the probability distribution, and a DR unit 13 that corrects the vehicle position estimated by the offset amount. A post-correction position deriving unit 18 for deriving a post-correction position of the vehicle by means of the above-mentioned information, and a map image storage unit 19 in which the map image is stored.
Observation image generation unit 14 tracks an object existing around the vehicle, and an object tracking unit 14A estimates whether the object is static or dynamic, and extracts static objects from the tracked objects. It has a static object extraction unit 14B and a mapping unit 14C that maps the extracted static object.

The self-position estimation device 10 is mounted on a vehicle, and based on the observation information of objects existing around the mounted vehicle (hereinafter referred to as "self-vehicle"), an observation image created in real time and a map image created in advance. The self-position is estimated by matching with . FIG. 2 is an external view of a vehicle equipped with a self-position estimation device.
A GNSS receiver (GNSS antenna) 30 for receiving signals from GNSS satellites is attached to the top of the vehicle 1, and an inertial measurement unit (IMU) 40 is attached to the rear wheels. By using GNSS and IMU, the position (latitude, longitude, altitude), attitude (pitch, yaw, roll), velocity, acceleration, etc. of the own vehicle 1 can be obtained at 100 Hz.
In addition, the own vehicle 1 is equipped with a LiDAR (Light Detection and Ranging) 50, which is a laser that can irradiate in all directions. localization can be performed.
In addition, a plurality of 76 GHz band millimeter wave radars (MWR) are attached to the own vehicle 1 as the peripheral object information acquisition sensor unit 12 . FIG. 3 is a diagram showing the mounting position and observation area of the millimeter wave radar in the own vehicle. As shown in FIG. 3, the millimeter-wave radar of this embodiment has a viewing angle of about 40 degrees, and by installing seven radars on the front bumper and two on the rear bumper of the own vehicle 1, it is possible to detect all directions. ing. The millimeter-wave radar of this embodiment can observe the surroundings of the vehicle at 20 Hz. Acquire observation information such as relative velocity. Millimeter-wave radar is resistant to environmental changes and can detect objects through snow.

FIG. 4 is an explanatory diagram of self-position estimation and offset correction by dead reckoning.
When the self-position estimation device 10 starts self-position estimation, the DR unit 13 estimates the position of the vehicle 1 by dead reckoning based on the data acquired by the GNSS/IMU data acquisition unit 11 .
Dead reckoning is a method of estimating the position of a vehicle by integrating the speed of the vehicle over time. In this embodiment, the estimated position [x _d,t , y _d,t ] ^T of the host vehicle 1 is updated by time integration of the linear velocity and the yaw rate. However, since the positions and velocities acquired by the GNSS/IMU data acquisition unit 11 contain errors, dead reckoning accumulates errors that occur when the sequential movement amount is calculated.

Therefore, in the self-position estimation device 10, the matching unit 16 matches the map image stored in the map image storage unit 19 with the observation image generated by the observation image generation unit 14, thereby performing dead A reckoning error (offset) [Δx _d,t , Δy _d,t ] ^T is estimated, and the estimated position [x _d,t , y _d,t ] of the own vehicle 1 estimated by dead reckoning is corrected by the offset. Positions X _t and Y _t of own vehicle 1 are represented by the following equation (1).

The offset amount [Δx _d,t , Δy _d,t ] ^T is estimated by the following procedures (1) to (4). In this embodiment, out of the position and orientation of the host vehicle 1, the orientation is directly acquired from the GNSS/IMU data acquisition unit 11, and only the offset amount [Δx _d,t ,Δy _d,t ] ^T related to the position is obtained. presume. By estimating only the position in this way, it is possible to simplify and speed up the calculation.
(1) The observation image generation unit 14 generates an observation image using the latest N frames of data acquired by the surrounding object information acquisition sensor unit 12 .
(2) A map image around the vehicle 1 is extracted from the map images stored in the map image storage unit 19 .
(3) The matching unit 15 performs map matching between the observed image and the extracted map image, and the probability updating unit 16 estimates the offset probability _Pt .
(4) The offset update unit 17 updates the offset amount [Δx _d,t , Δy _d,t ] ^T based on the obtained probability P _t .
Each procedure will be described below.

First, a method of generating a map image stored in the map image storage unit 19 will be described.
FIG. 5 is a block diagram of the map image generating means.
The map image generating means 20 includes an RTK-GNSS unit 21 of the real-time kinematic positioning system, and an object tracking unit 14A that tracks an object existing around the own vehicle 1 and estimates whether the object is static or dynamic. (shared with the observation image generation unit 10 in this embodiment), a static object extraction unit 14B (shared with the observation image generation unit 10 in this embodiment) that extracts static objects from the tracked objects, and It has a map image mapping unit 22 that maps a static object, and generates a map image.
A map image is generated by mapping observation values acquired by the peripheral object information acquisition sensor unit 12 using RTK-GNSS in post-processing. A position accuracy of about 0.03 m can be achieved by using the GNSS position after post-processing correction.
Here, the detection accuracy of the measurement angle in the 76 GHz band millimeter wave radar is generally not accurate compared to LiDAR and 79 GHz band millimeter wave radar. Therefore, it is necessary to consider the measurement accuracy when mapping the map image. A map image is generated by the following procedures (A) to (C). The map generated by the map image generator 24 is stored in the map image generator 19 .
(A) Object Tracking: Estimating static/dynamic objects.
(B) Static object extraction: remove dynamic objects.
(C) Mapping: Update static object probabilities at each pixel.

The object tracking unit 22 estimates a static/dynamic object using observation information obtained by millimeter wave radar. In this embodiment, since the map image is generated using the own vehicle 1, observation information is obtained using the peripheral object information obtaining sensor unit 12 (millimeter wave radar).
Millimeter-wave radar observes not only static objects but also various objects such as moving objects and static objects that can move. Therefore, in order to extract only static objects used for map image generation from observation information, (i) the absolute velocity in the millimeter-wave radar irradiation direction must be less than a threshold, and (ii) the object must be outside the lane of the road. and (iii) a non-tracking object that is not associated with a dynamic object [Non-Patent Document 6: Kiyoshi Nagano, et al. Observed information that satisfies all the conditions of "Study on Tracking Moving Vehicles" Society of Automotive Engineers of Japan, vol.48, no.2, pp.411-418, 2017.] is judged as a static object.

ここで、ｐ^xとｐ^yは、ＵＴＭ（Universal Transverse Mercator）のグローバル座標における物体位置である［非特許文献８：J. P. Snyder, “Map Projections: A Working Manual”, Geological Survey(U.S.), 1987.］。ｖ^x、ａ^x、ｖ^y及びａ^yは、それぞれｘ座標とｙ座標の速度と加速度である。
下式（３）のｗ^mwrは、対応する変数に対するプロセスノイズベクトルである。

例えば、独立定加速度モデルの状態方程式は下式（４）のように定式化できる。

IMM (interactive multiple model) [Non-Patent Document 7: R. Helmick, “IMM Estimator with Nearest-Neighbor Joint Probabilistic Data Association”, Multiagent-Multisensor Tracking: Applications and Advances Volume III, Artech House Publishers, pp. 161-198, 2000.] are employed to integrate multiple types of motion to estimate the position, velocity, and acceleration of the observed object. Uniform acceleration, uniform velocity, and stationary models are defined as motion models. The state variable x ^mwr is defined by the following equation (2).

Here, p ^x and p ^y are object positions in global coordinates of UTM (Universal Transverse Mercator) [Non-Patent Document 8: JP Snyder, “Map Projections: A Working Manual”, Geological Survey (US), 1987. ]. v ^x , a ^x , v ^y and a ^y are the velocity and acceleration in the x and y coordinates respectively.
w ^mwr in equation (3) below is the process noise vector for the corresponding variable.

For example, the state equation of the independent constant acceleration model can be formulated as the following equation (4).

For constant velocity models, the acceleration value is set to zero. Additionally, a model with constant yaw rate is defined to represent vehicle motion using a non-holonomic system. The IMM can track dynamic objects such as automobiles and bicycles around the vehicle 1 . Therefore, dynamic objects can be ignored in map image generation.
Map images are generated using static objects obtained by removing dynamic objects from the observed objects. Observation points in global coordinates are transformed to 2-D image coordinates and mapped to corresponding pixels. To account for the low detection accuracy of millimeter-wave radar, the existence likelihood is defined using a covariance matrix P based on observational error propagation.
FIG. 6 is a diagram showing an example of map image generation, FIG. 6A shows an example of mapping processing, FIG. 6B shows an example of a map image generated using a millimeter wave radar, and FIG. (c) shows an example of a map image generated using LiDAR. In FIG. 6A, the lighter the color, the higher the presence probability of the static object A. In FIG. As shown in the right diagram of FIG. 6(a), the presence probability of a static object is a probability density distribution in consideration of errors, so the region is defined as a region where the static object exists with a probability of 95%.
The existence likelihood is calculated so as to generate a variance perpendicular to the illumination direction of the surrounding object information acquisition sensor unit 12 . A likelihood value for each pixel is obtained by integration over the corresponding region of the likelihood distribution. The existence probabilities are then updated based on the likelihood values. Each pixel in the resulting map image of FIG. 6(b) has information on latitude and longitude. Compared to the LiDAR-based map image shown in FIG. 6(c), the map image shown in FIG. It can be seen that the description is possible.

ミリ波レーダーのデータは、距離ｒ^mwr、照射方向θ^mwr、照射方向の相対速度ｖ^mwrである。図７によれば、下式（６）、（７）、（８）のようにして、物体の位置をセンサ座標からグローバル座標に変換することができる。

ここで、[ｘ^mwr，ｙ^mwr]^Tと[ｘ_v ^mwr，ｙ_v ^mwr]は、それぞれセンサ座標と車両座標における物体の位置である。［ｘ_s，ｙ_s，θ_s］^Tは、車両座標におけるセンサ設置位置である。［ｘ_ego，ｙ_ego，θ_ego］はグローバル座標での車両位置である。 Here, the definition of the existence likelihood using the covariance matrix P based on observation error propagation will be described with reference to FIG.
Observed values provided from millimeter wave radar data are expressed as the following equation (5).

The data of the millimeter wave radar are the distance r ^mwr , the irradiation direction θ ^mwr , and the relative velocity v ^mwr in the irradiation direction. According to FIG. 7, the position of the object can be converted from the sensor coordinates to the global coordinates using the following equations (6), (7), and (8).

where [x ^mwr , y ^mwr ] ^T and [x _v ^mwr , y _v ^mwr ] are the positions of the object in sensor coordinates and vehicle coordinates, respectively. [x _s , y _s , θ _s ] ^T is the sensor installation position in vehicle coordinates. [x _ego , y _ego , θ _ego ] is the vehicle position in global coordinates.

The absolute velocity v _ego ^mwr of the vehicle in the irradiation direction is calculated by the following equation (9) using the vehicle velocity v _ego .

ここで、φ＝θ_ego＋θ_s＋θ^mwr，θ＝θ_s＋θ^mwrであり、Ｓ_ego＋ｓｉｎθ_ego，Ｃ_ego＝ｃｏｓθ_ego，Ｓ_φ＝ｓｉｎφ，Ｃ_φ＝ｃｏｓφである。
したがって、観測値ｚは、下式（１１）のように、１０個の変数を有する関数ｈによって表される。

Therefore, the observed value is represented by the following formula (10).

Here, φ=θ _ego +θ _s +θ ^mwr , θ=θ _s +θ ^mwr , S _ego +sin θ _ego , C _ego =cos θ _ego , S _φ =sin φ, C _φ =cos φ.
Therefore, the observed value z is represented by a function h having 10 variables, as in the following equation (11).

Based on the theory of error propagation, the error Δz of the observed value z is approximated by the following equation (12).

σ_*は、対応する変数の標準偏差である。 Then, the covariance matrix P for observation is obtained by solving the expected value of the squared error as in the following equations (13) to (19).

σ _* is the standard deviation of the corresponding variable.

In the generation of the observation image in procedure (1), as in the generation of the map image, the static object acquired from the surrounding object information acquisition sensor means 12 (millimeter wave radar) generates the observation image at the northeast (NE) coordinates. Mapped to produce.
Since the observed image must be generated in real time, no post-processing correction is performed. In addition, when generating map images, one of the conditions for judging whether an object is static or not is that it exists outside the lane of the road. , there is a possibility of erroneously removing stationary objects that are originally outside the road. Therefore, it is preferable to remove the observation information in the lane of the road, for example, by associating the moving object tracking information by LiDAR, which can recognize the position and size of the moving object, with the observation information by the millimeter wave radar.

FIG. 8 is a diagram showing an example of a generated observation image.
FIG. 8(a) is an example of an observed image. An observation image is an image of the position of a static object observed by the surrounding object information acquisition sensor means 12 while traveling in real time, as seen from directly above. It is generated by combining like an image. The size of the observed image is 24 m×24 m, and the center of the observed image is always the position of the own vehicle 1 estimated by the DR unit 13 . Observed images are generated using the observed frames such that the observation points cover the entire area.
In the extraction of the map image around the own vehicle 1 in procedure (2), as shown in FIG. It is extracted from the map image stored in the map image storage unit 19 . The map image is generated with NE coordinates, like the observed image. The size of the extracted map image covers a range of 32m x 32m.

In the map matching between the observed image and the extracted map image in the matching unit 15 in step (3), template matching is applied to obtain the correlation distribution around the current position.
In map matching, the offset amount and the position of the vehicle 1 are estimated by assuming that the more similar the map image and the observation image at a specific position are, the higher the probability that the vehicle 1 is at that position. Template matching is an algorithm for checking whether an image called a template exists in a search target image. In this embodiment, the observation image corresponds to the template image for template matching, and the extracted map image is the image to be matched with the template image. The similarity between the observed image and the extracted map image is calculated while sequentially moving the observed image serving as the template image in the map image.

ここでＲ_t,ref(Δｘ)は、オフセットΔｘ＝［Δｘ，Δｙ］^TでのＺＮＣＣ特徴である。
しかし、ミリ波レーダーによる観測は、観測点が疎らであるため、ＺＮＣＣから高い相関値を得ることが困難である。したがって、各フレームにおける相関分布の結果は信頼性が高くない可能性がある。そこで、下式（２１）のように、確率に対する過度の応答を回避するためにガンマ補正が適用される。

ここで、γは、ガンマ補正用のガンマ値である。Ｒ_t（Δｘ）は、Δｘにおける尤度分布である。 We use the normalized cross-correlation (ZNCC) R _t,ref (Δx) as the cost function for template matching, as in Equation (20) below.
SSD (Sum of Squared Difference), SAD (Sum of Absolute Difference), etc., can also be used to calculate the similarity, but the regular method, which has the feature of being more resistant to fluctuations in image brightness than other methods, can be used. It is preferred to use the Zincized Cross-Correlation (ZNCC).

where R _t,ref (Δx) is the ZNCC feature at offset Δx=[Δx,Δy] ^T .
However, it is difficult to obtain a high correlation value from ZNCC because observation points are sparse in observation by millimeter wave radar. Therefore, the correlation distribution results at each frame may not be reliable. Gamma correction is then applied to avoid over-response to probability, as in equation (21) below.

Here, γ is a gamma value for gamma correction. R _t (Δx) is the likelihood distribution at Δx.

ここで、βは累積過程の逓減係数である（０≦β≦１）。 In procedure (4), the probability distribution P(Δx) of the offset [Δx _d,t , Δy _d,t ] ^T is updated by the probability updating unit 16 using the likelihood distribution. FIG. 9 is a diagram illustrating likelihood filtering.
The probability distribution, like the likelihood distribution, is a distribution that indicates, in terms of probability, where the vehicle 1 is most likely to exist.
As shown in the upper diagram of FIG. 9, the likelihood distribution obtained in each frame is unstable. There are some frames that show distinct peaks, but they can suddenly show different distributions. Therefore, the cumulative distribution P _t (Δx) is calculated by cumulative processing of the likelihood distribution. The cumulative distribution, like the likelihood distribution, is a distribution indicating where the vehicle 1 is most likely to exist. The probability distribution P(Δx) is obtained by normalizing P _t (Δx) as in the following equations (22) and (23). This makes it possible to obtain a stable and reliable distribution.

where β is the decay factor of the accumulation process (0≤β≤1).

ここで、ｇ_posは、オフセットを更新する際のゲインである（０≦ｇ_pos≦１）。
補正後位置導出部１８は、ＤＲ部１３で推定した自車両１の位置をオフセット量で補正することにより自車両１の補正後位置を導出し、導出した補正後位置を自車両１の運転制御装置（図示無し）へ出力する。出力された補正後位置を用いて運転制御装置が自車両１を制御することで、自車両１の自動運転をより安全かつ的確なものとすることができる。 FIG. 10 is an explanatory diagram of processing when a plurality of peaks occur.
For example, when stationary objects such as fences and guardrails of the same pattern are continuously present along the traveling direction of the own vehicle 1 on the side of the road, a plurality of peaks occur as shown in FIGS. there's a possibility that. If multiple peaks occur, the offset amount cannot be calculated accurately, so it is necessary to extract only the correct peaks. Therefore, as shown in FIG. 10(c), the offset updating unit 17 tracks the peak position to reject peaks at erroneous positions, and the obtained probability is used to estimate the offset amount.

Here, g _pos is the gain when updating the offset (0≦g _pos ≦1).
A post-correction position derivation unit 18 derives a post-correction position of the vehicle 1 by correcting the position of the vehicle 1 estimated by the DR unit 13 with an offset amount, and applies the derived post-correction position to the operation control of the vehicle 1. Output to a device (not shown). By having the operation control device control the own vehicle 1 using the output post-correction position, the automatic operation of the own vehicle 1 can be made safer and more accurate.

Next, the results of the performance evaluation test of the vehicle self-position estimation device according to the first embodiment, which were carried out on public roads, will be described.
In the performance evaluation test, errors in the longitudinal and lateral directions of the vehicle between the self-estimated position and the actual position were calculated. GNSS information was used to initialize the initial position of the vehicle 1 . The RTK-GNSS information was used to determine the actual position as a reference for evaluating the accuracy of localization. In addition, as Comparative Example 1, conventional self-position estimation using LiDAR [Non-Patent Document 9: N. Suganuma, D. Yamamoto and K. Yoneda, “Localization for Autonomous Driving on Urban Road”, Journal of Advanced Control , Automation and Robotics, Vol. 1, No. 1, pp. 47-53, 2016.] was also performed, and the results of self-localization according to the first embodiment were compared.
The main differences between the self-position estimation according to the first embodiment and the comparative example 1 are the sensor as the peripheral object information acquisition sensor unit 12 (i.e. millimeter wave radar or LiDAR) and the image used for matching (i.e. , millimeter wave radar image or LiDAR image). In Comparative Example 1, the offset amount is estimated by map matching of the laser reflectance of the road surface.
Performance evaluation tests were conducted in two types of running areas.
The first area is public roads in Kanazawa City, Ishikawa Prefecture. The total length of this route is about 2.15 km, and tests were conducted in summer (no snow) and winter (partial snow: snow on roadsides and sidewalks, almost no snow on the road surface).
The second area is public roads in Abashiri City, Hokkaido. The total length of this route is about 25 km, and the test was conducted in the winter season (completely covered with snow: roadside strips and sidewalks are covered with snow, and the road surface is completely covered with snow). In the second area, the road surface is covered with snow and the conventional self-localization using LiDAR of Comparative Example 1 cannot be used, so only the self-localization according to the first embodiment was tested.

表１から分かるように、第一の実施例も比較例１も、積雪無しの条件下においては妥当な推定位置精度を得ることができる。しかし、部分積雪又は完全積雪の条件下においては、第一の実施例は雪の無い条件下と同程度の精度を有するのに対し、比較例１は精度がかなり悪化した。 Table 1 below shows the test results. Table 1 shows the root-mean-square (RMS) errors in the longitudinal and lateral directions of the vehicle under the conditions of "no snow,""partialsnow," and "full snow."

As can be seen from Table 1, both the first embodiment and the comparative example 1 can obtain reasonable estimated position accuracy under the condition of no snow cover. However, under conditions of partial snow cover or full snow cover, while the first example had similar accuracy as under no-snow conditions, the accuracy of Comparative Example 1 was significantly worse.

FIG. 11 is a graph showing test results in the first area in summer (no snow cover), and FIG. 12 is a graph showing test results in the first area in winter (partial snow cover).
11 and 12, the horizontal axis is the elapsed time [sec], and the vertical axis is the error [m] in the longitudinal direction of the vehicle in FIGS. 12(b) is the error [m] in the lateral direction of the vehicle, and FIGS. 11(c) and 12(c) are the yaw rate [rad/s].
From FIG. 11, it can be seen that when there is no snow cover, the accuracy of self-position estimation is maintained within 1 m in both the first embodiment and the comparative example 1 regardless of intersections or straight roads.
However, as can be seen from FIG. 12, in Comparative Example 1, when snow fell, the error in the lateral direction of the vehicle immediately increased to 2 m or more, and the error was not eliminated. This is because, in Comparative Example 1, LiDAR observes the snow on the side of the road as an area with high reflectance, and thus the side of the road is confused with the lane, resulting in a deviation in the lateral direction of the vehicle.
On the other hand, in the first embodiment, a millimeter-wave radar that is resistant to environmental changes and capable of detecting objects through snow is used as the peripheral object information acquisition sensor unit 12, so that even under bad weather conditions such as snowfall. However, the position of the own vehicle 1 could be estimated with the same degree of error as under the condition of no snow cover.

FIG. 13 is a graph showing test results in the second area in winter (full snow cover). 13, the horizontal axis represents elapsed time [sec], and the vertical axis represents error [m] in the longitudinal direction of the vehicle in FIG. 13(a), error [m] in the lateral direction of the vehicle in FIG. 13(c) is the yaw rate [rad/s].
It can be seen from FIG. 13 that the self-position estimation according to the first embodiment maintains accuracy regardless of the amount of snowfall.

From the above test results, the effectiveness of the self-position estimation device 10 according to the first embodiment was confirmed even under snowy conditions.

Next, a vehicle self-position estimation device according to a second embodiment of the present invention will be described. Duplicate descriptions of the same components as those of the first embodiment will be omitted.
In the first example described above, the posterior probabilities were calculated by accumulating the obtained correlations and normalizing them. To reduce the responsiveness of the posterior probabilities, it is more appropriate to gradually update the values on a log-odds scale (log-odds: l=log(P/(1−P))). Therefore, in this embodiment, the obtained correlation is used as the likelihood value on the logarithmic odds scale, and the correlation distribution obtained by map matching is accumulated to calculate the posterior probability.
As a result, the distribution obtained by accumulating the correlation is treated as the posterior probability of the logarithmic odds scale, the stability of the matching state is estimated, and the offset amount (Δx _d,t , Δy _d,t ) can be updated to achieve more stable localization.

ここで、α₁は確率更新の逓減係数である（０．０≦α≦１．０）。α₁は、経験的に０．９９５と決定した。Ｒ_t（Δｘ）は、自車周辺で離散化した位置決めオフセットの変位量Δｘ＝［Δｘ，Δｙ］^Ｔにおけるマップマッチングの相関分布から得られる尤度分布である。 The log-odds value of the posterior probability l _t is updated using equation (26) below.

where α ₁ is the probability update taper factor (0.0≦α≦1.0). α ₁ was empirically determined to be 0.995. R _t (Δx) is the likelihood distribution obtained from the correlation distribution of map matching in the displacement amount Δx=[Δx, Δy] ^T of the discretized positioning offset around the own vehicle.

ここでＲ_t,zncc(Δｘ)は、ＺＮＣＣ特徴である。
しかし、ミリ波レーダーによる観測は観測点が疎らであるため、ＺＮＣＣから高い相関値を得ることが困難である。したがって、各フレームにおける相関分布の結果は信頼性が高くない可能性がある。そこで、下式（２１）のように、確率に対する過度の応答を回避するためにガンマ補正が適用される。

ここで、βは非負のスケール因子である。γは、ガンマ補正用のガンマ値である。これらのパラメータは、β＝ ０．１及びγ＝ ３．０とした。 To apply map matching, we prepare an observed image and a map image. The method of generating or extracting the observed image and map image is the same as in the first embodiment.
Template matching is applied to obtain the correlation distribution around the current position. We use the normalized cross-correlation (ZNCC) R _t,zncc (Δx) as the cost function for template matching, as in Equation (27) below.

where R _t,zncc (Δx) is the ZNCC feature.
However, it is difficult to obtain a high correlation value from ZNCC because observation points by millimeter-wave radar are sparse. Therefore, the correlation distribution results at each frame may not be reliable. Gamma correction is then applied to avoid over-response to probability, as in equation (21) below.

where β is a non-negative scaling factor. γ is a gamma value for gamma correction. These parameters were β=0.1 and γ=3.0.

The probability distribution P(Δx) of the offset amount [Δx _d,t , Δy _d,t ] ^T is updated using the likelihood distribution. In this embodiment, the probability distribution P(Δx) can be obtained from l _t (Δx) updated using equation (26).

Finally, the weighted average of the obtained probabilities is used to estimate the offset amount.
As mentioned above, by calculating the posterior probabilities theoretically and correctly, the maximum value of the obtained probabilities can be taken as the uncertainty of the matching state. Therefore, by weighting according to uncertainty as in Equation (29), it is possible to prevent excessive offset updates in unstable situations.
Note that the offset amount can also be updated using equations (24) and (25) as in the first embodiment.

Next, a performance evaluation test of the vehicle self-position estimation device according to the second embodiment, which was carried out on public roads, will be described.
In the performance evaluation test, errors in the longitudinal and lateral directions of the vehicle between the self-estimated position and the actual position were calculated. GNSS information was used to initialize the initial position of the vehicle 1 . The RTK-GNSS information was used to determine the actual position as a reference for evaluating the accuracy of localization.
The driving data of the performance evaluation test is on public roads in Abashiri City and Ozora Town, Hokkaido, in autumn (no snow) and winter (full snow: roadside strips and sidewalks are covered with snow, and the road surface is completely covered with snow). was recorded in The total length of the tested route is approximately 8.5 km. The devices mounted on the own vehicle 1 used for the test are the same as those of the first embodiment.
A map image, which is a millimeter-wave radar map, was generated using driving data acquired in autumn and stored in the map image storage unit 19, and the accuracy of self-position estimation was evaluated with respect to driving data acquired in winter. Therefore, roadside landmarks may differ between the map image and the observation image due to the influence of snow.

The self-position estimation result by the second embodiment is divided into the self-position estimation result by dead reckoning (comparative example 2) and the self-position estimation result by the conventional estimation method [non-patent document 9] using LiDAR (comparative example 3). compared to
It should be noted that the self-position estimation according to the second embodiment is performed in two cases: the case of using Equations (24) and (25) (Example 2-1) and the case of using Equation (29) (Example 2-2). We also evaluated the effectiveness of whether or not to use the uncertainty at the time of updating the offset value.

The test results are shown in Table 2 below and FIG.
In FIG. 14, the horizontal axis is the elapsed time [sec], and the vertical axis is the error [m] in the longitudinal direction of the vehicle in FIG. 14(a), the error [m] in the lateral direction of the vehicle in FIG. 14(c) shows the yaw rate and the peak probability, the left vertical axis being the yaw rate [rad/s] and the right vertical axis being the peak probability. The peak probability corresponds to the uncertainty in equation (29).
Table 2 shows results for root mean square (RMS) errors, standard deviations (SD), and absolute values of maximum errors in the longitudinal and lateral directions of the vehicle.

From the test results, it can be seen that Example 2-1, in which self-position estimation is performed using Equations (24) and (25), has an accuracy equivalent to that of Comparative Example 3. As shown in FIG. 14, for Example 2-1 and Comparative Example 3, a meter-level vehicle lateral error was confirmed between 220 seconds and 280 seconds. In this section, the peak probability oscillates around 0.7, and it can be said that the matching state is unstable.
On the other hand, it was confirmed that the vehicle lateral direction error in this section can be reduced in Example 2-2 in which the self-position is estimated using equation (29). Therefore, by introducing uncertainty into the offset update using Equation (29), it is possible to prevent a sudden drop in positioning accuracy. In particular, Example 2-2 shows stable performance for all evaluation indexes.

Furthermore, in order to verify the effectiveness of this embodiment, automatic driving evaluations on snowy roads were also conducted on public roads in Abashiri City and Ozora Town, Hokkaido. As a result, it was confirmed that automatic driving was successful in most sections, and it was demonstrated that the self-position estimation device according to the present embodiment exhibited stable performance. Self-positioning can be estimated with high accuracy, which was shown by automated driving evaluations.

As described above, according to the present invention, the accuracy of self-position estimation can be improved by correcting the position of the vehicle 1 estimated by dead reckoning based on the result of matching.
Further, the surrounding object information acquisition sensor unit 12 can acquire observation information of objects through snow, the matching unit 15 performs template matching, and the probability update unit 16 performs correlation distribution obtained by template matching. is gamma-corrected to derive the likelihood distribution, and the likelihood distribution is used to update the probability distribution. Further, by performing template matching, it is possible to speed up the processing.
Further, the probability update unit 16 integrates the likelihood distributions obtained at each time, normalizes the integrated likelihood distributions to derive a cumulative distribution, updates the probability distribution based on the cumulative distribution, and updates the correlation can be updated by reducing the contribution of , the self-position can be estimated with high accuracy even if the surrounding object information acquisition sensor unit 12 observes the object sparsely.
Further, the probability update unit 16 derives the logarithmic odds value of the posterior probability using the likelihood distribution, and updates the probability distribution using the logarithmic odds value, thereby reducing the contribution of correlation and updating the probability. Even if the object observation by the peripheral object information acquisition sensor unit 12 is sparse, the self-position can be estimated with high accuracy.
In addition, the offset update unit 17 updates the offset amount using the weighted average of the probability distribution. Self-location can be estimated with high accuracy.
In addition, the offset updating unit 17 uses the maximum value of the probability distribution as the uncertainty of the matching state, and updates the offset amount by weighting according to the uncertainty, thereby preventing excessive offset updating in an unstable situation, Self-position can be estimated more accurately.

1 vehicle (own vehicle)
10 Self-position estimation device 11 GNSS/IMU data acquisition unit 12 Surrounding object information acquisition sensor unit 13 DR unit 14 Observed image generation unit 15 Matching unit 16 Probability update unit 17 Offset update unit 18 Post-correction position derivation unit 40 Inertial measurement unit (IMU )

Claims (6)

A self-position estimation device for estimating the position of the vehicle by matching an observation image created in real time based on observation information of objects existing around the vehicle with a map image created in advance,
a GNSS/IMU data acquisition unit that acquires data from GNSS satellites and inertial measurement units;
a peripheral object information acquisition sensor unit that acquires the observation information of the object existing around the vehicle;
a DR unit that estimates the position of the vehicle by dead reckoning based on the data acquired by the GNSS/IMU data acquisition unit;
An observation image generation unit that generates the observation image in which the positions of static objects among the objects are mapped based on the position of the vehicle estimated by the DR unit and the observation information acquired by the peripheral object information acquisition sensor unit. and,
a matching unit that performs matching between the observed image and the map image;
a probability updating unit that updates the offset probability distribution based on the matching result;
an offset updating unit that updates an offset amount based on the probability distribution;
a post-correction position derivation unit for deriving a post-correction position of the vehicle by correcting the position of the vehicle estimated by the DR unit with the offset amount ;
The peripheral object information acquisition sensor unit is capable of acquiring the observation information of the object through snow,
The matching unit performs template matching,
The probability update unit gamma-corrects the correlation distribution obtained by the template matching to derive a likelihood distribution, and updates the probability distribution using the likelihood distribution. estimation device. The probability updating unit integrates the likelihood distributions obtained at each time, normalizes the integrated likelihood distributions to derive a cumulative distribution, and updates the probability distribution based on the cumulative distribution. 2. The vehicle self-position estimation device according to claim 1 . 2. The probability updater uses the likelihood distribution to derive a log-odds value of the posterior probability, and uses the log-odds value to update the probability distribution. Position estimator. 4. The vehicle self-position estimation device according to claim 2 , wherein the offset update unit updates the offset amount using a weighted average of the probability distribution. 4. The vehicle according to claim 3 , wherein the offset update unit uses the maximum value of the probability distribution as the uncertainty of the matching state, and updates the offset amount by weighting according to the uncertainty. Position estimator. A vehicle equipped with the vehicle self-position estimation device according to any one of claims 1 to 5 .

Images