JP2020204501A - Self position estimation device of vehicle, and vehicle - Google Patents
Self position estimation device of vehicle, and vehicle Download PDFInfo
- Publication number
- JP2020204501A JP2020204501A JP2019111748A JP2019111748A JP2020204501A JP 2020204501 A JP2020204501 A JP 2020204501A JP 2019111748 A JP2019111748 A JP 2019111748A JP 2019111748 A JP2019111748 A JP 2019111748A JP 2020204501 A JP2020204501 A JP 2020204501A
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- unit
- self
- probability
- distribution
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000009826 distribution Methods 0.000 claims abstract description 90
- 230000003068 static effect Effects 0.000 claims abstract description 32
- 230000002093 peripheral effect Effects 0.000 claims abstract description 28
- 238000009795 derivation Methods 0.000 claims abstract description 6
- 230000001186 cumulative effect Effects 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 10
- 238000012937 correction Methods 0.000 abstract description 12
- 238000000034 method Methods 0.000 description 24
- 238000012360 testing method Methods 0.000 description 20
- 238000010586 diagram Methods 0.000 description 15
- 230000000052 comparative effect Effects 0.000 description 13
- 238000011156 evaluation Methods 0.000 description 9
- 238000013507 mapping Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 6
- 230000033001 locomotion Effects 0.000 description 5
- 238000001514 detection method Methods 0.000 description 4
- 238000000605 extraction Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- YLZOPXRUQYQQID-UHFFFAOYSA-N 3-(2,4,6,7-tetrahydrotriazolo[4,5-c]pyridin-5-yl)-1-[4-[2-[[3-(trifluoromethoxy)phenyl]methylamino]pyrimidin-5-yl]piperazin-1-yl]propan-1-one Chemical compound N1N=NC=2CN(CCC=21)CCC(=O)N1CCN(CC1)C=1C=NC(=NC=1)NCC1=CC(=CC=C1)OC(F)(F)F YLZOPXRUQYQQID-UHFFFAOYSA-N 0.000 description 3
- 238000012805 post-processing Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000036544 posture Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000007429 general method Methods 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004043 responsiveness Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
Description
本発明は、車両の自己位置をリアルタイムに推定する車両の自己位置推定装置、及び当該自己位置推定装置を搭載した車両に関する。 The present invention relates to a vehicle self-position estimation device that estimates the vehicle self-position in real time, and a vehicle equipped with the self-position estimation device.
近年、交通事故の削減や交通渋滞の緩和、誰もが利用できる移動手段の提供等を目的として、車両の運転が自動的に行われる自動運転システムの開発が盛んに行われている。自動運転自動車が目的地へ向かって正しく移動するためには、当該車両の位置をリアルタイムに推定する自己位置推定が必要である。
自己位置推定を行うための一般的な手法として、あらかじめ定義した地図（地図画像）を用いたマップマッチングがあり、その手法では主にＬｉＤＡＲ（Light Detection and Ranging）やカメラといったセンサが用いられる。
例えば、非特許文献１では、ＬｉＤＡＲを用いたマップマッチング手法が開示されている。ＬｉＤＡＲを用いた手法は、測定精度が高く、昼夜の変化や環境の変化に対して高いロバスト性を備えているため、デシメートルレベルの精度で車両の自己位置推定を行うことができる。
また、非特許文献２では、カメラで検出した道路面の特徴をベクトル地図（白線、道路標識、信号機等の位置を記録したデジタル地図）とマッチングすることで、車両の自己位置を、車両横方向で０．１ｍ程度、車両前後方向で０．５ｍ程度の精度で推定することが開示されている。
In recent years, the development of an automatic driving system in which a vehicle is automatically driven has been actively developed for the purpose of reducing traffic accidents, alleviating traffic congestion, and providing a means of transportation that can be used by anyone. In order for an autonomous vehicle to move correctly toward a destination, self-position estimation is required to estimate the position of the vehicle in real time.
As a general method for self-position estimation, there is map matching using a predefined map (map image), and in this method, sensors such as LiDAR (Light Detection and Ranging) and a camera are mainly used.
For example, Non-Patent Document 1 discloses a map matching method using LiDAR. Since the method using LiDAR has high measurement accuracy and high robustness against changes in day and night and changes in the environment, it is possible to estimate the self-position of the vehicle with an accuracy of decimeter level.
Further, in Non-Patent Document 2, by matching the characteristics of the road surface detected by the camera with a vector map (digital map recording the positions of white lines, road signs, traffic lights, etc.), the self-position of the vehicle can be set in the lateral direction of the vehicle. It is disclosed that the estimation is performed with an accuracy of about 0.1 m and about 0.5 m in the front-rear direction of the vehicle.
しかしながら、ＬｉＤＡＲやカメラを用いた自己位置推定は、路面が雪で覆われている場合、降雪によって車両周辺の物体の形状が変化している場合、又は雨によって路面が正確に把握できない場合などには、精度が安定しないという問題がある。
非特許文献３では、このような問題を解決するために、過去のフレームを使うことで、雨で濡れた道路や雪道においてＬｉＤＡＲの反射率が低減することを防ぐ手法が開示されている。しかし、この手法では、雪で完全に覆われている道路や、雪壁によって道路形状が変化する場所において、自己位置を精度よく推定することができない。
However, self-position estimation using LiDAR or a camera is performed when the road surface is covered with snow, when the shape of objects around the vehicle is changed due to snowfall, or when the road surface cannot be accurately grasped due to rain. Has a problem that the accuracy is not stable.
Non-Patent Document 3 discloses a method for preventing the reflectance of LiDAR from being reduced on a rain-wet road or a snowy road by using a past frame in order to solve such a problem. However, with this method, it is not possible to accurately estimate the self-position on a road completely covered with snow or a place where the road shape changes due to a snow wall.
また、非特許文献４には、ミリ波レーダーとカメラを用いて、道路面の情報に基づき自己位置推定を行う手法が開示されている。しかし、この手法も積雪時に精度よく自己位置推定を行うことはできない。
また、非特許文献５には、クラスタリングしたミリ波レーダーの観測情報を用いて地図生成を行い、さらにパーティクルフィルタを用いて自己位置推定を行う手法が開示されている。しかし、ミリ波レーダーによる観測点が疎らであることから、誤差が最大３ｍを超える推定精度であり、かつメートルレベルの精度でしか自己位置推定ができなかったと記載されている。
Further, Non-Patent Document 4 discloses a method of performing self-position estimation based on road surface information using a millimeter-wave radar and a camera. However, this method also cannot accurately estimate the self-position during snowfall.
Further, Non-Patent Document 5 discloses a method of generating a map using observation information of a clustered millimeter-wave radar and further estimating a self-position using a particle filter. However, since the observation points by the millimeter-wave radar are sparse, it is stated that the error is an estimation accuracy of more than 3 m at the maximum, and the self-position estimation can be performed only with the accuracy of the meter level.
また、特許文献１には、横位置補正処理部は、降雪等の影響によりカメラユニットで自車進行路の左右区画線が認識されていないと判定した場合、自車両の道路地図上の走行車線に記憶されている地図曲率を読込み、追従対象先行車の走行軌跡に基づいて先行曲率を求め、両曲率と自車速とに基づいて推定横位置偏差を求め、推定横位置偏差を目標ハンドル角のフィードバック項である横位置ハンドル角の自車横位置に加算して、新たな自車横位置を設定することで、自車両を先行車の走行軌跡に沿って走行させる車両の運転支援装置が開示されている。
しかし、特許文献１の運転支援装置は、降雪時においても車両の自己位置を精度よく推定するものではない。また、先行車の走行軌跡を利用するものであるから、先行車が存在しない場合には、降雪等の影響によりカメラユニットで自車進行路の左右区画線が認識されない状況における自動運転が困難となる。
Further, in Patent Document 1, when the lateral position correction processing unit determines that the left and right lane markings of the own vehicle's traveling path are not recognized by the camera unit due to the influence of snowfall or the like, the traveling lane on the road map of the own vehicle is determined. The map curvature stored in is read, the leading curvature is obtained based on the traveling locus of the preceding vehicle to be followed, the estimated lateral position deviation is obtained based on both curvatures and the own vehicle speed, and the estimated lateral position deviation is the target handle angle. A vehicle driving support device that allows the vehicle to travel along the traveling locus of the preceding vehicle by adding a feedback term to the lateral position of the vehicle and setting a new lateral position of the vehicle is disclosed. Has been done.
However, the driving support device of Patent Document 1 does not accurately estimate the self-position of the vehicle even during snowfall. In addition, since the traveling locus of the preceding vehicle is used, if the preceding vehicle does not exist, it is difficult to automatically drive in a situation where the camera unit does not recognize the left and right lane markings of the own vehicle due to the influence of snowfall or the like. Become.
そこで本発明は、精度よく自己位置を推定できる車両の自己位置推定装置、及び当該装置を備えた車両を提供することを目的とする。 Therefore, an object of the present invention is to provide a vehicle self-position estimation device capable of accurately estimating a self-position, and a vehicle equipped with the device.
請求項１記載の本発明の車両の自己位置推定装置は、車両の周辺に存在する物体の観測情報に基づきリアルタイムに作成した観測画像と予め作成された地図画像とのマッチングを行うことにより車両の位置を推定する自己位置推定装置であって、ＧＮＳＳ衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部と、車両の周辺に存在する物体の観測情報を取得する周辺物体情報取得センサ部と、ＧＮＳＳ／ＩＭＵデータ取得部で取得したデータに基づいてデッドレコニングにより車両の位置を推定するＤＲ部と、ＤＲ部で推定した車両の位置及び周辺物体情報取得センサ部で取得した観測情報に基づき、物体のうち静的物体の位置がマッピングされた観測画像を生成する観測画像生成部と、観測画像と地図画像とのマッチングを行うマッチング部と、マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部と、確率分布に基づいてオフセット量を更新するオフセット更新部と、ＤＲ部で推定した車両の位置をオフセット量で補正することにより車両の補正後位置を導出する補正後位置導出部とを備えることを特徴とする。 The vehicle self-position estimation unit of the present invention according to claim 1 matches an observation image created in real time based on observation information of an object existing around the vehicle with a map image created in advance. It is a self-position estimation device that estimates the position, and is a GNSS / IMU data acquisition unit that acquires data from the GNSS satellite and inertial measurement unit, and a peripheral object information acquisition sensor that acquires observation information of objects existing around the vehicle. The DR unit that estimates the vehicle position by dead reckoning based on the data acquired by the GNSS / IMU data acquisition unit, and the observation information acquired by the vehicle position and peripheral object information acquisition sensor unit estimated by the DR unit. Based on this, the observation image generation unit that generates the observation image in which the position of the static object is mapped among the objects, the matching unit that matches the observation image and the map image, and the offset probability distribution based on the matching result. The probability update unit to be updated, the offset update unit to update the offset amount based on the probability distribution, and the corrected position derivation to derive the corrected position of the vehicle by correcting the vehicle position estimated by the DR unit with the offset amount. It is characterized by having a part.
請求項２記載の本発明は、請求項１に記載の車両の自己位置推定装置において、周辺物体情報取得センサ部は、雪を透過して物体の観測情報を取得可能であり、マッチング部は、テンプレートマッチングを行い、確率更新部は、テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、尤度分布を用いて確率分布を更新することを特徴とする。 According to the second aspect of the present invention, in the vehicle self-position estimation device according to the first aspect, the peripheral object information acquisition sensor unit can acquire the observation information of the object through the snow, and the matching unit can acquire the observation information of the object. The probability update unit performs template matching, gamma-corrects the correlation distribution obtained by the template matching to derive the likelihood distribution, and updates the probability distribution using the likelihood distribution.
請求項３記載の本発明は、請求項２に記載の車両の自己位置推定装置において、確率更新部は、各時刻で得られた尤度分布を積算し、積算した尤度分布を正規化して累積分布を導出し、累積分布に基づいて確率分布を更新することを特徴とする。 According to the third aspect of the present invention, in the vehicle self-position estimation device according to the second aspect, the probability update unit integrates the likelihood distributions obtained at each time and normalizes the integrated likelihood distributions. It is characterized in that the cumulative distribution is derived and the probability distribution is updated based on the cumulative distribution.
請求項４記載の本発明は、請求項２に記載の車両の自己位置推定装置において、確率更新部は、尤度分布を用いて事後確率の対数オッズ値を導出し、対数オッズ値を用いて確率分布を更新することを特徴とする。 According to the fourth aspect of the present invention, in the vehicle self-position estimation device according to the second aspect, the probability update unit derives a posterior probability log odds value using a likelihood distribution, and uses the log odds value. It is characterized by updating the probability distribution.
請求項５記載の本発明は、請求項３又は請求項４に記載の車両の自己位置推定装置において、オフセット更新部は、確率分布の重み付け平均を用いてオフセット量を更新することを特徴とする。 The present invention according to claim 5 is characterized in that, in the vehicle self-position estimation device according to claim 3 or 4, the offset update unit updates the offset amount by using the weighted average of the probability distribution. ..
請求項６記載の本発明は、請求項４に記載の車両の自己位置推定装置において、オフセット更新部は、確率分布の最大値をマッチング状態の不確実性とし、不確実性に従った重み付けによりオフセット量を更新することを特徴とする。 According to the sixth aspect of the present invention, in the vehicle self-position estimation device according to the fourth aspect, the offset update unit sets the maximum value of the probability distribution as the uncertainty of the matching state, and weights the vehicle according to the uncertainty. It is characterized by updating the offset amount.
請求項７記載の本発明の車両は、請求項１から請求項６のいずれか１項に記載の車両の自己位置推定装置を搭載したことを特徴とする。 The vehicle of the present invention according to claim 7 is characterized by incorporating the vehicle self-position estimation device according to any one of claims 1 to 6.
本発明によれば、車両の自己位置推定の精度を向上させることができる。 According to the present invention, the accuracy of self-position estimation of the vehicle can be improved.
本発明の第１の実施の形態による車両の自己位置推定装置は、ＧＮＳＳ衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部と、車両の周辺に存在する物体の観測情報を取得する周辺物体情報取得センサ部と、ＧＮＳＳ／ＩＭＵデータ取得部で取得したデータに基づいてデッドレコニングにより車両の位置を推定するＤＲ部と、ＤＲ部で推定した車両の位置及び周辺物体情報取得センサ部で取得した観測情報に基づき、物体のうち静的物体の位置がマッピングされた観測画像を生成する観測画像生成部と、観測画像と地図画像とのマッチングを行うマッチング部と、マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部と、確率分布に基づいてオフセット量を更新するオフセット更新部と、ＤＲ部で推定した車両の位置をオフセット量で補正することにより車両の補正後位置を導出する補正後位置導出部とを備えるものである。
本実施の形態によれば、デッドレコニングにより推定した車両の位置をマッチングの結果に基づいて補正することで、自己位置推定の精度を向上させることができる。
The vehicle self-position estimation device according to the first embodiment of the present invention acquires the GNSS / IMU data acquisition unit that acquires data from the GNSS satellite and the inertial measurement unit, and the observation information of objects existing around the vehicle. Peripheral object information acquisition sensor unit, DR unit that estimates the vehicle position by dead reckoning based on the data acquired by the GNSS / IMU data acquisition unit, and the vehicle position and peripheral object information acquisition sensor unit estimated by the DR unit. Based on the observation information acquired in, the observation image generation unit that generates the observation image in which the position of the static object is mapped among the objects, the matching unit that matches the observation image and the map image, and the matching result. The position after correction of the vehicle is corrected by correcting the position of the vehicle estimated by the DR unit, the probability update unit that updates the probability distribution of the offset, the offset update unit that updates the offset amount based on the probability distribution, and the DR unit. It is provided with a corrected position derivation unit to be derived.
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 matching result.
本発明の第２の実施の形態は、第１の実施の形態による車両の自己位置推定装置において、周辺物体情報取得センサ部は、雪を透過して物体の観測情報を取得可能であり、マッチング部は、テンプレートマッチングを行い、確率更新部は、テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、尤度分布を用いて確率分布を更新するものである。
本実施の形態によれば、積雪条件下においても精度よく自己位置を推定することができる。また、テンプレートマッチングを行うことにより、処理の高速化が可能となる。
In the second embodiment of the present invention, in the vehicle self-position estimation device according to the first embodiment, the peripheral object information acquisition sensor unit can acquire the observation information of the object through the snow and is matched. The unit performs template matching, and the probability updating unit gamma-corrects the correlation distribution obtained by template matching to derive a likelihood distribution, and updates the probability distribution using the likelihood distribution.
According to this embodiment, the self-position can be estimated accurately even under snowy conditions. Further, by performing template matching, it is possible to speed up the processing.
本発明の第３の実施の形態は、第２の実施の形態による車両の自己位置推定装置において、確率更新部は、各時刻で得られた尤度分布を積算し、積算した尤度分布を正規化して累積分布を導出し、累積分布に基づいて確率分布を更新するものである。
本実施の形態によれば、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部による物体観測が疎らであっても精度よく自己位置を推定することができる。
In the third embodiment of the present invention, in the vehicle self-position estimation device according to the second embodiment, the probability update unit integrates the likelihood distributions obtained at each time and calculates the integrated likelihood distribution. It normalizes to derive the cumulative distribution and updates the probability distribution based on the cumulative distribution.
According to the present embodiment, since the contribution of the correlation can be lowered and the probability can be updated, the self-position can be estimated accurately even if the object observation by the peripheral object information acquisition sensor unit is sparse.
本発明の第４の実施の形態は、第２の実施の形態による車両の自己位置推定装置において、確率更新部は、尤度分布を用いて事後確率の対数オッズ値を導出し、対数オッズ値を用いて確率分布を更新するものである。
本実施の形態によれば、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部による物体観測が疎らであっても精度よく自己位置を推定することができる。
In the fourth embodiment of the present invention, in the vehicle self-position estimation device according to the second embodiment, the probability update 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, since the contribution of the correlation can be lowered and the probability can be updated, the self-position can be estimated accurately even if the object observation by the peripheral object information acquisition sensor unit is sparse.
本発明の第５の実施の形態は、第３又は第４の実施の形態による車両の自己位置推定装置において、オフセット更新部は、確率分布の重み付け平均を用いてオフセット量を更新するものである。
本実施の形態によれば、道路脇に同じパターンの静的物体である柵やガードレール等が連続して存在する場合であっても精度よく自己位置を推定することができる。
In the fifth embodiment of the present invention, in the vehicle self-position estimation device according to the third or fourth embodiment, the offset update unit updates the offset amount by using the weighted average of the probability distribution. ..
According to this embodiment, even when fences, guardrails, and the like, which are static objects having the same pattern, are continuously present on the side of the road, the self-position can be estimated accurately.
本発明の第６の実施の形態は、第４の実施の形態による車両の自己位置推定装置において、オフセット更新部は、確率分布の最大値をマッチング状態の不確実性とし、不確実性に従った重み付けによりオフセット量を更新するものである。
本実施の形態によれば、不確実性に従って重み付けすることによって、不安定な状況における過度のオフセット更新を防ぎ、より精度よく自己位置を推定することができる。
In the sixth embodiment of the present invention, in the vehicle self-position estimation device according to the fourth embodiment, the offset update unit sets the maximum value of the probability distribution as the uncertainty of the matching state, and follows the uncertainty. The offset amount is updated by the weighting.
According to the present embodiment, by weighting according to the uncertainty, it is possible to prevent excessive offset update in an unstable situation and estimate the self-position more accurately.
本発明の第７の実施の形態による車両は、第１から第６の実施の形態のいずれか一つの車両の自己位置推定装置を搭載したものである。
本実施の形態によれば、自己位置をリアルタイムに精度よく推定し、安全かつ的確に目的地へ移動する自動運転車両を提供することができる。
The vehicle according to the seventh embodiment of the present invention is equipped with a self-position estimation device for any one of the first to sixth embodiments.
According to the present embodiment, it is possible to provide an autonomous driving vehicle that accurately estimates its own position in real time and safely and accurately moves to a destination.
本発明の第一の実施例による車両の自己位置推定装置について説明する。
図１は本実施例による車両の自己位置推定装置のブロック図である。
自己位置推定装置１０は、ＧＮＳＳ（Global Navigation Satellite System）衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部１１と、車両の周辺に存在する物体の観測情報をリアルタイムに取得する周辺物体情報取得センサ部１２と、ＧＮＳＳ／ＩＭＵデータ取得部１１で取得したデータに基づいてデッドレコニングにより車両の位置を推定するＤＲ部１３と、ＤＲ部１３で推定した車両の位置及び周辺物体情報取得センサ部１２が取得した観測情報に基づき、物体のうち静的物体の位置がマッピングされた観測画像を生成する観測画像生成部１４と、観測画像と地図画像とのマッチングを行うマッチング部１５と、マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部１６と、確率分布に基づいてオフセット量を更新するオフセット更新部１７と、ＤＲ部１３で推定した車両の位置をオフセット量で補正することにより車両の補正後位置を導出する補正後位置導出部１８と、地図画像が記憶されている地図画像記憶部１９を備える。
観測画像生成部１４は、車両の周辺に存在する物体を追跡し、その物体が静的物体か動的物体かを推定する物体追跡部１４Ａと、追跡した物体の中から静的物体を抽出する静的物体抽出部１４Ｂと、抽出した静的物体のマッピングを行うマッピング部１４Ｃを有する。
The vehicle self-position estimation device according to the first embodiment of the present invention will 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 a GNSS (Global Navigation Satellite System) satellite and an inertial measurement unit, and a peripheral area that acquires observation information of objects existing around the vehicle in real time. Object information acquisition sensor unit 12, DR unit 13 that estimates the vehicle position by dead reckoning based on the data acquired by the GNSS / IMU data acquisition unit 11, and vehicle position and peripheral object information acquisition estimated by the DR unit 13. Based on the observation information acquired by the sensor unit 12, the observation image generation unit 14 that generates an observation image in which the position of a static object is mapped among the objects, the matching unit 15 that matches the observation image and the map image, and the matching unit 15. The probability update unit 16 that updates the offset probability distribution based on the matching result, the offset update unit 17 that updates the offset amount based on the probability distribution, and the vehicle position estimated by the DR unit 13 are corrected by the offset amount. A corrected position deriving unit 18 for deriving the corrected position of the vehicle and a map image storage unit 19 for storing the map image are provided.
The observation image generation unit 14 tracks an object existing around the vehicle, estimates whether the object is a static object or a dynamic object, and extracts a static object from the tracked objects. It has a static object extraction unit 14B and a mapping unit 14C for mapping the extracted static object.
自己位置推定装置１０は、車両に搭載され、搭載された車両（以下、「自車両」という）の周辺に存在する物体の観測情報に基づき、リアルタイムに作成した観測画像と予め作成された地図画像とのマッチングを行うことにより自己位置を推定する。図２は自己位置推定装置を搭載した自車両の外観図である。
自車両１には、上部にＧＮＳＳ衛星からの信号を受信するＧＮＳＳ受信部（ＧＮＳＳアンテナ）３０が取り付けられ、後輪に慣性計測装置であるＩＭＵ（Inertial Measurement Unit）４０が取り付けられている。ＧＮＳＳ及びＩＭＵを用いることで、自車両１の位置（緯度、経度、高度）、姿勢（ピッチ、ヨー、ロール）、速度及び加速度等を１００Ｈｚで取得できる。
また、自車両１は、全方位照射可能なレーザーであるＬｉＤＡＲ（Light Detection and Ranging）５０を備えており、自己位置推定装置１０による自己位置推定とは別に、ＬｉＤＡＲ５０から得られる赤外線反射率を利用した自己位置推定を行うことができる。
また、自車両１には、周辺物体情報取得センサ部１２として、複数個の７６ＧＨｚ帯ミリ波レーダー（ＭＷＲ）を取り付けている。図３は自車両におけるミリ波レーダーの取り付け位置と観測領域を示す図である。図３に示すように、本実施例のミリ波レーダーは、視野角が約４０度であり、自車両１の前方バンパーに７個、後方バンパーに２個設置することで全方位を検出可能にしている。本実施例のミリ波レーダーは、車両周辺を２０Ｈｚで観測可能であり、自車両１の周辺に存在する人、他車両、道路設置物等といった周辺に存在する物体までの相対距離、相対角度、相対速度といった観測情報を取得する。ミリ波レーダーは、環境変化に強く、雪を透過して物体検出が可能である。
The self-position estimation device 10 is mounted on a vehicle, and is an observation image created in real time and a map image created in advance based on observation information of an object existing in the vicinity of the mounted vehicle (hereinafter referred to as “own vehicle”). The self-position is estimated by matching with. FIG. 2 is an external view of the own vehicle equipped with the self-position estimation device.
A GNSS receiving unit (GNSS antenna) 30 that receives a signal from a GNSS satellite is attached to the upper part of the own vehicle 1, and an IMU (Inertial Measurement Unit) 40 that is an inertial measurement unit is attached to the rear wheels. By using GNSS and IMU, the position (latitude, longitude, altitude), attitude (pitch, yaw, roll), speed, acceleration, etc. of the own vehicle 1 can be acquired at 100 Hz.
Further, the own vehicle 1 is provided with LiDAR (Light Detection and Ranging) 50, which is a laser capable of omnidirectional irradiation, and uses the infrared reflectance obtained from the LiDAR 50 separately from the self-position estimation by the self-position estimation device 10. It is possible to estimate the self-position.
Further, a plurality of 76 GHz band millimeter wave radars (MWRs) are attached to the own vehicle 1 as peripheral object information acquisition sensor units 12. FIG. 3 is a diagram showing the mounting position and the 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 can detect all directions by installing seven on the front bumper and two on the rear bumper of the own vehicle 1. ing. The millimeter-wave radar of this embodiment can observe the periphery of the vehicle at 20 Hz, and has a relative distance, a relative angle, and the like, relative distances and relative angles to objects existing in the vicinity such as a person, another vehicle, a road installation, etc. Acquire observation information such as relative velocity. Millimeter-wave radar is resistant to changes in the environment and can detect objects through snow.
図４はデッドレコニングによる自己位置推定とオフセット補正の説明図である。
自己位置推定装置１０は、自己位置推定を開始すると、ＤＲ部１３において、ＧＮＳＳ／ＩＭＵデータ取得部１１が取得したデータに基づいてデッドレコニングにより自車両１の位置を推定する。
デッドレコニングとは、車両の速度を時間で積分し、車両の位置を推定する手法である。本実施例では、線速度とヨーレートの時間積分によって自車両１の推定位置［ｘd,t，ｙd,t］Tを更新する。しかしながら、ＧＮＳＳ／ＩＭＵデータ取得部１１が取得する位置や速度には誤差が含まれるため、デッドレコニングでは、逐次移動量を計算した際に発生する誤差が累積してしまう。
FIG. 4 is an explanatory diagram of self-position estimation and offset correction by dead reckoning.
When the self-position estimation device 10 starts the self-position estimation, the DR unit 13 estimates the position of the own 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 own vehicle 1 is updated by the time integration of the linear velocity and the yaw rate. However, since the position and speed acquired by the GNSS / IMU data acquisition unit 11 include an error, in the dead reckoning, the error generated when the sequential movement amount is calculated is accumulated.
そこで、自己位置推定装置１０は、マッチング部１６において、地図画像記憶部１９に記憶されている地図画像と、観測画像生成部１４で生成された観測画像とのマッチング（照合）を行うことによりデッドレコニングの誤差（オフセット）［Δｘd,t，Δｙd,t ]Tを推定し、デッドレコニングにより推定した自車両１の推定位置［ｘd,t，ｙd,t］をオフセットで補正する。自車両１の位置Ｘt，Ｙtは下式（１）で表される。
オフセット量［Δｘd,t，Δｙd,t ]Tは、以下の手順（１）〜（４）で推定する。なお、本実施例では、自車両１の位置と姿勢のうち、姿勢についてはＧＮＳＳ／ＩＭＵデータ取得部１１から直接取得し、位置に関するオフセット量［Δｘd,t，Δｙd,t ]Tのみを推定する。このように位置のみを推定することで、計算の簡易化及び高速化を図ることができる。
（１）周辺物体情報取得センサ部１２で取得した最新のＮフレームのデータを用いて観測画像生成部１４で観測画像を生成する。
（２）地図画像記憶部１９に記憶されている地図画像の中から、自車両１周辺の地図画像を抽出する。
（３）マッチング部１５における観測画像と抽出した地図画像とのマップマッチングを行い、確率更新部１６でオフセットの確率Ｐｔを推定する。
（４）得られた確率Ｐｔに基づいてオフセット更新部１７でオフセット量［Δｘd,t，Δｙd,t ]Tを更新する。
以下、各手順について説明する。
The offset amount [Δ x d, t , Δ y d, t ] T is estimated by the following procedures (1) to (4). In this embodiment, of the positions and postures of the own vehicle 1, the postures are directly acquired from the GNSS / IMU data acquisition unit 11, and only the offset amount [Δx d, t , Δyd , t ] T related to the positions 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-frame data acquired by the peripheral object information acquisition sensor unit 12.
(2) From the map image stored in the map image storage unit 19, a map image around the own vehicle 1 is extracted.
(3) performs a map matching between the map image and the extracted observation image in the matching unit 15, it estimates the probability P t offset probability update unit 16.
(4) The offset amount [Δ x d, t , Δ y d, t ] T is updated by the offset update unit 17 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 generation means.
The map image generation means 20 tracks the RTK-GNSS unit 21 of the real-time kinematic positioning method and the object tracking unit 14A that tracks an object existing around the own vehicle 1 and estimates whether the object is a static object or a dynamic object. (Shared with the observation image generation unit 10 in this embodiment) and the static object extraction unit 14B (shared with the observation image generation unit 10 in this example) for extracting a static object from the tracked objects. It has a map image mapping unit 22 that maps static objects, and generates a map image.
The map image is generated by mapping the observed values acquired by the peripheral object information acquisition sensor unit 12 using RTK-GNSS in post-processing. By using the GNSS position after post-processing correction, a position accuracy of about 0.03 m can be realized.
Here, the detection accuracy of the measurement angle in the 76 GHz band millimeter wave radar is generally not accurate as compared with LiDAR and the 79 GHz band millimeter wave radar. Therefore, it is necessary to consider the measurement accuracy when mapping the map image. The map image is generated by the following procedures (A) to (C). The map generated by the map image generation unit 24 is stored in the map image generation unit 19.
(A) Object tracking: Estimate static / dynamic objects.
(B) Static object extraction: Deletes a dynamic object.
(C) Mapping: Update the probability of a static object at each pixel.
物体追跡部２２は、ミリ波レーダーによる観測情報を用いて静的／動的物体を推定する。なお、本実施例では、自車両１を用いて地図画像を生成するため、周辺物体情報取得センサ部１２（ミリ波レーダー）を使用して観測情報を取得する。
ミリ波レーダーは、静的物体だけでなく、移動する物体や、移動し得る静的物体など様々な物体を観測する。そこで、観測情報から地図画像の生成に使用する静的物体のみを抽出するために、（i）ミリ波レーダー照射方向の絶対速度が閾値以下であること、（ii）道路の車線外に存在すること、及び（iii）動的物体と対応付けられていない非追跡物体であること［非特許文献６：永野聖巳, et al.“全方位ミリ波レーダを用いた自動運転自動車のための周辺移動車両追跡に関する研究”自動車技術会論文集 vol.48,no.2,pp.411-418,2017.］の条件をすべて満たす観測情報を静的物体と判断する。
The object tracking unit 22 estimates a static / dynamic object using the observation information by the millimeter wave radar. In this embodiment, in order to generate a map image using the own vehicle 1, observation information is acquired using the peripheral object information acquisition 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 the static objects used to generate the map image from the observation information, (i) the absolute velocity in the millimeter-wave radar irradiation direction is below the threshold, and (ii) it exists outside the lane of the road. That, and (iii) a non-tracking object that is not associated with a dynamic object [Non-Patent Document 6: Seimi Nagano, et al. "Around for an autonomous vehicle using an omnidirectional millimeter-wave radar" Observation information that satisfies all the conditions of the research on moving vehicle tracking "Proceedings of the Society of Automotive Engineers of Japan vol.48, no.2, pp.411-418, 2017." is judged as a static object.
ＩＭＭ（interactive multiple model）［非特許文献７：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.］は、観測対象物体の位置、速度、及び加速度を推定するために複数種類の運動を統合するために採用される。等加速度、等速、及び停止モデルは、運動モデルとして定義される。状態変数ｘmwrは、下式（２）で定義される。
下式（３）のｗmwrは、対応する変数に対するプロセスノイズベクトルである。
W mwr in the following equation (3) is a process noise vector for the corresponding variable.
等速モデルの場合は、加速度値はゼロに設定される。さらに、一定のヨーレートを有するモデルは、非ホロノミックシステムを用いて車両運動を表現するように定義される。ＩＭＭは、自車両１の周囲に存在する自動車や自転車などの動的物体を追跡できる。したがって、動的物体は、地図画像生成では無視することができる。
地図画像は、観測した物体のうち動的物体を除去することによって得られた静的物体を使用して生成される。グローバル座標内の観測点は、２−Ｄ画像座標に変換され、対応するピクセルにマッピングされる。ミリ波レーダーの低い検知精度を考慮するために、存在尤度は、観測誤差伝播に基づいて共分散行列Ｐを使用して定義される。
図６は地図画像生成の例を示す図であり、図６（ａ）はマッピング処理の一例を示し、図６（ｂ）はミリ波レーダーを用いて生成した地図画像の一例を示し、図６（ｃ）はＬｉＤＡＲを用いて生成した地図画像の一例を示している。図６（ａ）においては、色が薄いほど静的物体Ａの存在確率が高いことを示している。なお、図６（ａ）の右図に示すように、静的物体の存在確率は誤差を考慮した確率密度分布とするため、９５％の確率で静的物体が存在する領域とする。
存在尤度は、周辺物体情報取得センサ部１２の照射方向に対して垂直な分散を生成するように計算される。各ピクセルに対する尤度値は、尤度分布の対応する領域における積分によって得られる。そして、存在確率は、尤度値に基づいて更新される。得られた地図画像である図６（ｂ）の各ピクセルは、緯度と経度に関する情報を有する。図６（ｂ）の地図画像には、図６（ｃ）に示されるＬｉＤＡＲを用いた地図画像と比較すると、歩道上又は道路脇に位置する電信柱やガードレール等の静的物体をランドマークとして描写できていることが分かる。
For the constant velocity model, the acceleration value is set to zero. In addition, models with constant yaw rate are defined to represent vehicle motion using a nonholonomic system. The IMM can track dynamic objects such as automobiles and bicycles that exist around the own 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 converted to 2-D image coordinates and mapped to the corresponding pixels. To take into account the low detection accuracy of millimeter-wave radar, the existence likelihood is defined using the covariance matrix P based on the observation error propagation.
FIG. 6 is a diagram showing an example of map image generation, FIG. 6 (a) shows an example of mapping processing, and FIG. 6 (b) shows an example of a map image generated by using a millimeter wave radar. (C) shows an example of a map image generated by using LiDAR. In FIG. 6A, the lighter the color, the higher the probability of existence of the static object A. As shown in the right figure of FIG. 6A, the existence probability of the static object is a probability density distribution in consideration of the error, so that the region has a 95% probability of the existence of the static object.
The existence likelihood is calculated so as to generate a dispersion perpendicular to the irradiation direction of the peripheral object information acquisition sensor unit 12. The likelihood value for each pixel is obtained by integration in the corresponding region of the likelihood distribution. Then, the existence probability is updated based on the likelihood value. Each pixel of FIG. 6B, which is the obtained map image, has information on latitude and longitude. Compared with the map image using LiDAR shown in FIG. 6 (c), the map image of FIG. 6 (b) uses static objects such as telephone poles and guardrails located on the sidewalk or the side of the road as landmarks. You can see that it can be described.
ここで、図７を用いて、観測誤差伝播に基づく共分散行列Ｐを使用した存在尤度の定義について説明する。
ミリ波レーダーのデータから提ｖ供される観測値は、下式（５）として表される。
The observed values provided from the millimeter-wave radar data are expressed by the following equation (5).
照射方向における車両の絶対速度ｖego mwrは、車両速度ｖegoを用いて下式（９）のように計算される。
したがって、観測値は下式（１０）で表される。
したがって、観測値ｚは、下式（１１）のように、１０個の変数を有する関数ｈによって表される。
Therefore, the observed value z is represented by the function h having 10 variables as shown in the following equation (11).
誤差伝播の理論に基づき、観測値ｚの誤差Δzは下式（１２）のように近似される。
そして、下式（１３）〜（１９）のように、その二乗誤差の期待値を解くことにより、観測用の共分散行列Ｐが得られる。
手順（１）における観測画像の生成においては、地図画像の生成と同様に、周辺物体情報取得センサ手段１２（ミリ波レーダー）から取得された静的物体が、北東（ＮＥ）座標で観測画像を生成するためにマッピングされる。
なお、観測画像の生成はリアルタイムで行う必要があるため、後処理補正は行わない。また、地図画像の生成では、道路の車線外に存在することを静的物体か否かを判断する条件の一つとしていたが、観測画像を生成する際は、誤差のあるデッドレコニングによる推定位置を用いるため、本来道路外にある静止物体を誤って除去してしまう可能性がある。そこで、例えば動的物体の位置とその大きさを認識できるＬｉＤＡＲによる移動物体の追跡情報をミリ波レーダーによる観測情報と対応付けることで、道路の車線内の観測情報を除去することが好ましい。
In the generation of the observation image in the procedure (1), the static object acquired from the peripheral object information acquisition sensor means 12 (millimeter wave radar) produces the observation image in the northeast (NE) coordinates as in the generation of the map image. Mapped to generate.
Since the observation image needs to be generated in real time, no post-processing correction is performed. Also, in the generation of the map image, the existence outside the lane of the road was one of the conditions for judging whether or not it was a static object, but in the generation of the observation image, the estimated position by dead reckoning with an error. Therefore, there is a possibility that a stationary object originally outside the road may be accidentally removed. Therefore, for example, it is preferable to remove the observation information in the lane of the road by associating the tracking information of the moving object by LiDAR, which can recognize the position and the size of the dynamic object, with the observation information by the millimeter wave radar.
図８は生成された観測画像の例を示す図である。
図８（ａ）は観測画像の一例である。観測画像とは、リアルタイムに走行している際に周辺物体情報取得センサ手段１２で観測した静的物体の位置を真上から見たような画像であり、自車両１の移動量に応じてパノラマ画像のように結合することで生成される。観測画像の大きさは２４ｍ×２４ｍであり、観測画像の中央は常にＤＲ部１３で推定した自車両１の位置になるようにしている。観測画像は、観測点が領域全体をカバーするように、観測した複数フレームを使用して生成される。
手順（２）における自車両１周辺の地図画像の抽出では、図８（ｂ）に示すように、観測画像とのマッチングに用いる地図画像が、デッドレコニングにより推定した自車両１の位置に基づき、地図画像記憶部１９に記憶されている地図画像から抽出される。地図画像は、観測画像と同様にＮＥ座標で生成されている。抽出された地図画像の大きさは、３２ｍ×３２ｍの範囲をカバーしている。
FIG. 8 is a diagram showing an example of the generated observation image.
FIG. 8A is an example of the observed image. The observation image is an image as if the position of a static object observed by the peripheral object information acquisition sensor means 12 is viewed from directly above while traveling in real time, and is a panorama according to the amount of movement of the own vehicle 1. 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 set to the position of the own vehicle 1 estimated by the DR unit 13. The observed image is generated using multiple observed frames so that the observation point covers the entire area.
In the extraction of the map image around the own vehicle 1 in the procedure (2), as shown in FIG. 8 (b), the map image used for matching with the observed image is based on the position of the own vehicle 1 estimated by dead reckoning. It is extracted from the map image stored in the map image storage unit 19. The map image is generated in NE coordinates like the observed image. The size of the extracted map image covers a range of 32 m × 32 m.
手順（３）のマッチング部１５における観測画像と抽出した地図画像とのマップマッチングにおいては、現在位置周辺の相関分布を得るためにテンプレートマッチングが適用される。
マップマッチングでは、特定位置の地図画像と観測画像が類似しているほど自車両１がその位置にいる確率が高いとすることで、オフセット量及び自車両１の位置を推定する。また、テンプレートマッチングとは、テンプレートと呼ばれる画像が探索対象画像内に存在するか否かを調べるアルゴリズムである。本実施例では、テンプレートマッチングのテンプレート画像に該当するのが観測画像であり、抽出した地図画像がテンプレート画像と照合される画像となる。テンプレート画像となる観測画像を地図画像の中で順番に移動させながら、観測画像と抽出した地図画像の類似度を計算する。
In the map matching between the observed image and the extracted map image in the matching unit 15 of the procedure (3), template matching is applied in order to obtain the correlation distribution around the current position.
In map matching, the offset amount and the position of the own vehicle 1 are estimated by assuming that the probability that the own vehicle 1 is at that position is higher as the map image of the specific position and the observed image are similar. Further, template matching is an algorithm for checking whether or not an image called a template exists in the image to be searched. In this embodiment, the observation image corresponds to the template image of the template matching, and the extracted map image is the image to be collated with the template image. The similarity between the observed image and the extracted map image is calculated while moving the observed image as the template image in order in the map image.
下式（２０）のように、テンプレートマッチングのコスト関数として、正規化相互相関（ＺＮＣＣ）Ｒt,ref(Δｘ)を使用する。
類似度の計算には、ＳＳＤ（Sum of Squared Difference）やＳＡＤ（Sum of Absolute Difference）等を使用することもできるが、他の手法と比べて画像の明るさの変動に強いという特徴を持つ正規化相互相関（ＺＮＣＣ）を使用することが好ましい。
しかし、ミリ波レーダーによる観測は、観測点が疎らであるため、ＺＮＣＣから高い相関値を得ることが困難である。したがって、各フレームにおける相関分布の結果は信頼性が高くない可能性がある。そこで、下式（２１）のように、確率に対する過度の応答を回避するためにガンマ補正が適用される。
SSD (Sum of Squared Difference), SAD (Sum of Absolute Difference), etc. can be used to calculate the degree of similarity, but it is a normalization that is more resistant to fluctuations in image brightness than other methods. It is preferable to use chemical cross-correlation (ZNCC).
However, it is difficult to obtain a high correlation value from ZNCC in the observation by the millimeter wave radar because the observation points are sparse. Therefore, the results of the correlation distribution at each frame may not be reliable. Therefore, gamma correction is applied in order to avoid an excessive response to the probability as in the following equation (21).
手順（４）において、確率更新部１６でオフセット［Δｘd,t，Δｙd,t ]Tの確率分布Ｐ（Δx）は尤度分布を用いて更新される。図９は尤度フィルタリングを示す図である。
確率分布とは、尤度分布と同様に自車両１の位置がどこに存在する可能性が大きいかを確率で示した分布である。
図９の上図に示すように、各フレームで得られる尤度分布は不安定である。明確なピークを示すフレームがいくつかあるが、それらは突然異なる分布を示すことがある。 したがって、累積分布Ｐt(Δｘ)は、尤度分布の累積処理によって計算される。累積分布とは、尤度分布と同様に自車両１の位置がどこに存在する可能性が大きいかを示した分布である。下式（２２）、（２３）のように、確率分布Ｐ(Δｘ)は、Ｐt(Δｘ)を正規化することにより得られる。これにより、安定した信頼のある分布を得ることができる。
The probability distribution is a distribution showing the probability of where the position of the own vehicle 1 is likely to exist, similar to the likelihood distribution.
As shown in the upper figure of FIG. 9, the likelihood distribution obtained in each frame is unstable. There are several frames that show distinct peaks, but they can suddenly show different distributions. Therefore, the cumulative distribution P t (Δx) is calculated by the cumulative processing of the likelihood distribution. The cumulative distribution is a distribution showing where the position of the own vehicle 1 is likely to exist, similar to the likelihood distribution. The probability distribution P (Δx) can be 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.
図１０は複数のピークが発生した場合の処理についての説明図である。
自車両１の進行方向に沿って例えば道路脇に同じパターンの静的物体である柵やガードレール等が連続して存在する場合、図１０（ａ）（ｂ）に示すように複数のピークが発生する可能性がある。複数のピークが発生すると、オフセット量を正確に計算することができないため、正しいピークだけを抽出する必要がある。そこで、図１０（ｃ）に示すようにオフセット更新部１７ではピーク位置を追跡することで誤った位置でのピークを棄却し、下式（２４）、（２５）のように、得られた確率の重み付け平均値を用いてオフセット量を推定する。
補正後位置導出部１８は、ＤＲ部１３で推定した自車両１の位置をオフセット量で補正することにより自車両１の補正後位置を導出し、導出した補正後位置を自車両１の運転制御装置（図示無し）へ出力する。出力された補正後位置を用いて運転制御装置が自車両１を制御することで、自車両１の自動運転をより安全かつ的確なものとすることができる。
FIG. 10 is an explanatory diagram of processing when a plurality of peaks occur.
When, for example, fences and guardrails, which are static objects of the same pattern, are continuously present along the traveling direction of the own vehicle 1, a plurality of peaks occur as shown in FIGS. 10A and 10B. there's a possibility that. When 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 update unit 17 tracks the peak position to reject the peak at the wrong position, and the probabilities obtained as shown in the following equations (24) and (25). The offset amount is estimated using the weighted average value of.
The corrected position derivation unit 18 derives the corrected position of the own vehicle 1 by correcting the position of the own vehicle 1 estimated by the DR unit 13 with an offset amount, and controls the operation of the own vehicle 1 by using the derived corrected position. Output to the device (not shown). By controlling the own vehicle 1 by the operation control device using the output corrected position, the automatic operation of the own vehicle 1 can be made safer and more accurate.
次に、公道で実施した第一の実施例による車両の自己位置推定装置の性能評価試験の結果について説明する。
性能評価試験においては、自己推定位置と実際の位置との間における車両前後方向及び横方向の誤差を計算した。ＧＮＳＳの情報は、自車両１の初期位置の初期化に使用した。ＲＴＫ−ＧＮＳＳの情報は、自己位置推定の精度を評価する基準となる実際の位置を求めるために使用した。また、比較例１として、ＬｉＤＡＲを用いた従来の自己位置推定［非特許文献９：N. Suganuma, D. Yamamoto and K. Yoneda, “Localization for Au- tonomous Driving on Urban Road”, Journal of Advanced Control, Automation and Robotics, Vol. 1, No. 1, pp. 47-53, 2016.］も併せて行い、第一の実施例による自己位置推定の結果と比較した。
第一の実施例による自己位置推定と、比較例１との主な相違点は、周辺物体情報取得センサ部１２としてのセンサ（すなわち、ミリ波レーダーかＬｉＤＡＲか）、及びマッチングに用いる画像（すなわち、ミリ波レーダー画像かＬｉＤＡＲ画像か）である。比較例１では、路面のレーザー反射率のマップマッチングによってオフセット量を推定する。
性能評価試験は、２種類の走行エリアで行った。
一つ目のエリアは、石川県の金沢市内の公道である。このルートの全長は約２．１５ｋｍであり、夏季（積雪無し）と冬季（部分積雪：路側帯や歩道に積雪が有り車道路面の積雪は殆ど無し）に試験を行った。
二つ目のエリアは、北海道の網走市内の公道である。このルートの全長は約２５ｋｍであり、冬季（完全積雪：路側帯や歩道に積雪が有り車道路面も雪で完全に覆われている）に試験を行った。二つ目のエリアでは、路面が雪で覆われており比較例１のＬｉＤＡＲを用いた従来の自己位置推定は使用できないため、第一の実施例による自己位置推定についてのみ試験を行った。
Next, the result of the performance evaluation test of the vehicle self-position estimation device according to the first embodiment carried out on the public road will be described.
In the performance evaluation test, the error in the vehicle front-rear direction and the lateral direction between the self-estimated position and the actual position was calculated. The GNSS information was used to initialize the initial position of the own vehicle 1. The RTK-GNSS information was used to determine the actual position as a reference for evaluating the accuracy of self-position estimation. 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 Au- tonomous Driving on Urban Road”, Journal of Advanced Control , Automation and Robotics, Vol. 1, No. 1, pp. 47-53, 2016.] was also performed and compared with the result of self-position estimation by the first example.
The main difference between the self-position estimation according to the first embodiment and the comparative example 1 is the sensor as the peripheral object information acquisition sensor unit 12 (that is, millimeter wave radar or LiDAR), and the image used for matching (that is, that is). , 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.
The performance evaluation test was conducted in two types of driving areas.
The first area is a public road 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: there is snow on the roadside belt and sidewalks, and there is almost no snow on the road surface).
The second area is a public road in Abashiri City, Hokkaido. The total length of this route is about 25 km, and the test was conducted in winter (complete snowfall: there is snow on the roadside belt and sidewalks, and the road surface is completely covered with snow). In the second area, since the road surface is covered with snow and the conventional self-position estimation using LiDAR of Comparative Example 1 cannot be used, only the self-position estimation by the first example was tested.
下表１に試験結果を示す。表１では、「積雪無し」、「部分積雪」、及び「完全積雪」のそれぞれの条件下における車両前後方向及び横方向の二乗平均平方根（ＲＭＳ）誤差を示している。
図１１は夏季（積雪無し）の一つ目のエリアにおける試験結果を示すグラフであり、図１２は冬季（部分積雪）の一つ目のエリアにおける試験結果を示すグラフである。
図１１及び図１２において、横軸は経過時間［ｓｅｃ］であり、縦軸は、図１１（ａ）及び図１２（ａ）が車両前後方向の誤差［ｍ］、図１１（ｂ）及び図１２（ｂ）が車両横方向の誤差［ｍ］、図１１（ｃ）及び図１２（ｃ）がヨーレート［ｒａｄ／ｓ］である。
図１１から、積雪無しの場合は、交差点や直線道路に関係なく、第一の実施例においても比較例１においても１ｍ以内の誤差に自己位置推定の精度が維持されていることが分かる。
しかし、図１２から分かるように、比較例１においては降雪時には、車両横方向の誤差はすぐに２ｍ以上に増加し、その誤差は解消されなかった。これは、比較例１においては、ＬｉＤＡＲが道路脇の積雪を反射率の高い地域として観測するため、道路脇を車線と混同することにより車両横方向のずれが発生したためである。
一方、第１の実施例においては、環境変化に強く、雪を透過して物体検出が可能なミリ波レーダーを周辺物体情報取得センサ部１２として用いることで、降雪等の悪天候条件の下であっても、積雪無しの条件下と同じ程度の誤差で自車両１の位置を推定することができた。
FIG. 11 is a graph showing the test results in the first area in the summer (no snow cover), and FIG. 12 is a graph showing the test results in the first area in the winter season (partial snow cover).
In FIGS. 11 and 12, the horizontal axis is the elapsed time [sec], and the vertical axes are the error [m] in the vehicle front-rear direction in FIGS. 11 (a) and 12 (a), FIGS. 11 (b) and FIG. 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 in the case of no snow, the accuracy of self-position estimation is maintained within an error of 1 m in both the first embodiment and the comparative example 1 regardless of the intersection or the straight road.
However, as can be seen from FIG. 12, in Comparative Example 1, when it snowed, 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, since LiDAR observes the snow cover on the side of the road as an area with high reflectance, the lateral deviation of the vehicle occurs by confusing the side of the road with the lane.
On the other hand, in the first embodiment, by using a millimeter-wave radar that is resistant to environmental changes and can detect an object through snow as the peripheral object information acquisition sensor unit 12, it is under bad weather conditions such as snowfall. However, the position of the own vehicle 1 could be estimated with the same error as under the condition without snow.
図１３は冬季（完全積雪）の二つ目のエリアにおける試験結果を示すグラフである。図１３において、横軸は経過時間［ｓｅｃ］であり、縦軸は、図１３（ａ）が車両前後方向の誤差［ｍ］、図１３（ｂ）が車両横方向の誤差［ｍ］、図１３（ｃ）がヨーレート［ｒａｄ／ｓ］である。
図１３により、第一の実施例による自己位置推定は、降雪量に関わらず精度を維持していることが分かる。
FIG. 13 is a graph showing the test results in the second area in winter (complete snow cover). In FIG. 13, the horizontal axis is the elapsed time [sec], and the vertical axis is the error [m] in the vehicle front-rear direction in FIG. 13 (a) and the error [m] in the vehicle lateral direction in FIG. 13 (b). 13 (c) is the yaw rate [rad / s].
From FIG. 13, it can be seen that the self-position estimation according to the first embodiment maintains the 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 snow conditions.
次に、本発明の第二の実施例による車両の自己位置推定装置について説明する。なお、上記した第一の実施例と同一構成要素については重複説明を省略する。
上記した第一の実施例では、得られた相関を累積し、それらを正規化することによって事後確率を計算した。事後確率の応答性を低下させるためには、対数オッズスケールで値を徐々に更新することがより適切である（対数オッズ：ｌ＝ｌｏｇ（Ｐ／（１−Ｐ）））。そこで本実施例では、得られた相関を対数オッズスケールでの尤度値として、マップマッチングにより得られた相関分布を累積して事後確率を計算する。
これにより、相関を累積して得られた分布を対数オッズスケールの事後確率として扱い、マッチング状態の安定性を推定し、得られた確率の大きさに応じてオフセット量（Δｘd,t、Δｙd,t）を更新してより安定した自己位置推定を実現することができる。
Next, the vehicle self-position estimation device according to the second embodiment of the present invention will be described. The duplicate description will be omitted for the same components as those in the first embodiment described above.
In the first embodiment described above, posterior probabilities were calculated by accumulating the obtained correlations and normalizing them. In order to reduce the responsiveness of posterior probabilities, it is more appropriate to gradually update the values on the 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 log 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 correlations is treated as the posterior probability of the log odds scale, the stability of the matching state is estimated, and the offset amount (Δx d, t , Δy) depends on the magnitude of the obtained probability. It is possible to update d, t ) to realize more stable self-position estimation.
事後確率ｌtの対数オッズ値は、下式（２６）を用いて更新される。
マップマッチングを適用するために、観測画像と地図画像を用意する。観測画像と地図画像の生成又は抽出方法は、第一の実施例と同様である。
現在位置周辺の相関分布を得るためにテンプレートマッチングが適用される。下式（２７）のように、テンプレートマッチングのコスト関数として、正規化相互相関（ＺＮＣＣ）Ｒt,zncc(Δｘ)を使用する。
しかし、ミリ波レーダーによる観測は観測点が疎らであるため、ＺＮＣＣから高い相関値を得ることが困難である。したがって、各フレームにおける相関分布の結果は信頼性が高くない可能性がある。そこで、下式（２１）のように、確率に対する過度の応答を回避するためにガンマ補正が適用される。
Template matching is applied to obtain the correlation distribution around the current position. Normalized cross-correlation (ZNCC) R t, zncc (Δx) is used as the cost function of template matching as in the following equation (27).
However, it is difficult to obtain a high correlation value from ZNCC because the observation points by the millimeter wave radar are sparse. Therefore, the results of the correlation distribution at each frame may not be reliable. Therefore, gamma correction is applied in order to avoid an excessive response to the probability as in the following equation (21).
オフセット量［Δｘd,t，Δｙd,t ]Tの確率分布Ｐ（Δｘ）は、尤度分布を用いて更新される。本実施例では、確率分布Ｐ（Δｘ）は、式（２６）を用いて更新したｌt（Δｘ）から求めることができる。 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 the equation (26).
最後に、得られた確率の重みづけ平均値を用いてオフセット量を推定する。
上述のように、事後確率を理論的に正しく計算することで、得られた確率の最大値をマッチング状態の不確実性とみなすことができる。したがって、下式（２９）のように不確実性に従って重み付けすることによって、不安定な状況における過度のオフセット更新を防ぐことが可能である。
なお、第一の実施例と同様に式（２４）及び式（２５）を用いてオフセット量を更新することもできる。
As described above, by calculating the posterior probability theoretically correctly, the maximum value of the obtained probability can be regarded as the uncertainty of the matching state. Therefore, it is possible to prevent excessive offset update in an unstable situation by weighting according to the uncertainty as in the following equation (29).
The offset amount can also be updated using the equations (24) and (25) as in the first embodiment.
次に、公道で実施した第二の実施例による車両の自己位置推定装置の性能評価試験について説明する。
性能評価試験においては、自己推定位置と実際の位置との間における車両前後方向及び横方向の誤差を計算した。ＧＮＳＳの情報は、自車両１の初期位置の初期化に使用した。ＲＴＫ−ＧＮＳＳの情報は、自己位置推定の精度を評価する基準となる実際の位置を求めるために使用した。
性能評価試験の運転データは、北海道の網走市と大空町の公道において、秋季（積雪無し）と冬季（完全積雪：路側帯や歩道に積雪が有り車道路面も雪で完全に覆われている）に記録された。試験を行った経路の全長は、約８．５ｋｍである。試験に用いた自車両１に搭載されている機器は、第一の実施例と同様である。
ミリ波レーダー地図である地図画像は、秋季に取得した運転データを用いて生成して地図画像記憶部１９に記憶させ、自己位置推定の精度は、冬季に取得した運転データについて評価した。そのため、雪の影響により、地図画像と観測画像とでは道路脇のランドマークが異なる場合がある。
Next, the performance evaluation test of the vehicle self-position estimation device according to the second embodiment carried out on the public road will be described.
In the performance evaluation test, the error in the vehicle front-rear direction and the lateral direction between the self-estimated position and the actual position was calculated. The GNSS information was used to initialize the initial position of the own vehicle 1. The RTK-GNSS information was used to determine the actual position as a reference for evaluating the accuracy of self-position estimation.
The driving data of the performance evaluation test is for public roads in Abashiri City and Ozora Town in Hokkaido in autumn (no snow) and winter (complete snow: there is snow on the roadside belt and sidewalks, and the road surface is completely covered with snow). It was recorded in. The total length of the tested route is about 8.5 km. The equipment mounted on the own vehicle 1 used in the test is the same as that in the first embodiment.
The map image, which is a millimeter-wave radar map, was generated using the operation data acquired in the autumn season and stored in the map image storage unit 19, and the accuracy of the self-position estimation was evaluated for the operation data acquired in the winter season. Therefore, due to the influence of snow, the landmarks on the side of the road may differ between the map image and the observed image.
第二の実施例による自己位置推定結果を、デッドレコニングによる自己位置推定結果（比較例２）と、ＬｉＤＡＲを用いた従来の推定方法［非特許文献９］による自己位置推定結果（比較例３）と比較した。
なお、第二の実施例による自己位置推定は、式（２４）及び式（２５）を用いる場合（実施例２−１）と、式（２９）を用いる場合（実施例２−２）の二つのパターンで実施し、オフセット値更新時の不確実性を利用するか否かの有効性についても評価した。
The self-position estimation result according to the second embodiment is 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 with.
The self-position estimation according to the second embodiment is performed when the equations (24) and (25) are used (Example 2-1) and when the equation (29) is used (Example 2-2). One pattern was used, and the effectiveness of whether or not to utilize the uncertainty when updating the offset value was also evaluated.
試験結果を、下表２及び図１４に示す。
図１４において、横軸は経過時間［ｓｅｃ］であり、縦軸は、図１４（ａ）が車両前後方向の誤差［ｍ］、図１４（ｂ）が車両横方向の誤差［ｍ］、図１４（ｃ）においてはヨーレートとピーク確率を示しており、左縦軸がヨーレート［ｒａｄ／ｓ］、右縦軸がピーク確率である。ピーク確率は、式（２９）の不確実性に対応する。
表２においては、車両前後方向及び横方向についての二乗平均平方根（ＲＭＳ）誤差、標準偏差（Ｓ．Ｄ．）、及び最大誤差の絶対値についての結果を示している。
In FIG. 14, the horizontal axis is the elapsed time [sec], and the vertical axis is an error [m] in the vehicle front-rear direction in FIG. 14 (a) and an error [m] in the vehicle lateral direction in FIG. 14 (b). In 14 (c), the yaw rate and the peak probability are shown, the left vertical axis is the yaw rate [rad / s], and the right vertical axis is the peak probability. The peak probability corresponds to the uncertainty of equation (29).
Table 2 shows the results for the root mean square (RMS) error, standard deviation (SD), and absolute value of the maximum error in the front-rear and lateral directions of the vehicle.
試験結果から、式（２４）及び式（２５）を用いて自己位置推定を行う実施例２−１の場合は、比較例３と同等の精度を有することが分かる。図１４に示すように、実２−１と比較例３について、２２０秒から２８０秒の間に、メートルレベルの車両横方向誤差が確認された。この区間では、ピーク確率が０．７前後で振動しており、マッチング状態は不安定であると言える。
一方、式（２９）を用いて自己位置推定を行う実施例２−２では、この区間での車両横方向誤差を小さく出来ることが確認された。したがって、式（２９）を用いてオフセット更新に不確実性を導入することで、測位精度の急激な低下を防ぐことができる。また、特に実施例２−２では、すべての評価指標に対して安定した性能を示している。
From the test results, it can be seen that the case of Example 2-1 in which the self-position estimation is performed using the formulas (24) and (25) has the same accuracy as that of the comparative example 3. As shown in FIG. 14, a metric-level vehicle lateral error was confirmed between 220 seconds and 280 seconds for Actual 2-1 and Comparative Example 3. 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, in Example 2-2 in which the self-position estimation is performed using the equation (29), it was confirmed that the vehicle lateral error in this section can be reduced. Therefore, by introducing uncertainty into the offset update using the equation (29), it is possible to prevent a sharp decrease in positioning accuracy. Further, in particular, in Example 2-2, stable performance is shown for all the evaluation indexes.
さらに、本実施例の有効性を検証するために、同じく北海道の網走市と大空町の公道において雪道での自動運転評価を行った。結果、殆どの区間で自動運転が成功していることが確認され、本実施例による自己位置推定装置が安定したパフォーマンスを発揮することが実証され、吹雪等の悪天候の条件下において自車両１の自己位置推定を精度よく行えることが、自動運転評価によって示された。 Furthermore, in order to verify the effectiveness of this example, automatic driving evaluation was conducted on snowy roads on public roads in Abashiri City and Ozora Town, Hokkaido. As a result, it was confirmed that the automatic driving was successful in most sections, and it was proved that the self-position estimation device according to this embodiment exhibited stable performance, and the own vehicle 1 under bad weather conditions such as a snowstorm. The automatic driving evaluation showed that the self-position estimation can be performed accurately.
以上のように、本発明によれば、デッドレコニングにより推定した車両１の位置をマッチングの結果に基づいて補正することで、自己位置推定の精度を向上させることができる。
また、周辺物体情報取得センサ部１２は、雪を透過して物体の観測情報を取得可能であり、マッチング部１５は、テンプレートマッチングを行い、確率更新部１６は、テンプレートマッチングにより得られた相関分布をガンマ補正して尤度分布を導出し、尤度分布を用いて確率分布を更新することで、積雪条件下においても精度よく自己位置を推定することができる。また、テンプレートマッチングを行うことにより、処理の高速化が可能となる。
また、確率更新部１６は、各時刻で得られた尤度分布を積算し、積算した尤度分布を正規化して累積分布を導出し、累積分布に基づいて確率分布を更新することで、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部１２による物体観測が疎らであっても精度よく自己位置を推定することができる。
また、確率更新部１６は、尤度分布を用いて事後確率の対数オッズ値を導出し、対数オッズ値を用いて確率分布を更新することで、相関の寄与度を低くし確率更新できるので、周辺物体情報取得センサ部１２による物体観測が疎らであっても精度よく自己位置を推定することができる。
また、オフセット更新部１７は、確率分布の重み付け平均を用いてオフセット量を更新することで、道路脇に同じパターンの静的物体である柵やガードレール等が連続して存在する場合であっても精度よく自己位置を推定することができる。
また、オフセット更新部１７は、確率分布の最大値をマッチング状態の不確実性とし、不確実性に従った重み付けによりオフセット量を更新することで、不安定な状況における過度のオフセット更新を防ぎ、より精度よく自己位置を推定することができる。
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 matching result.
Further, the peripheral object information acquisition sensor unit 12 can acquire the observation information of the object through the snow, the matching unit 15 performs template matching, and the probability update unit 16 performs the correlation distribution obtained by the template matching. Is gamma-corrected to derive the likelihood distribution, and the probability distribution is updated using the likelihood distribution, so that the self-position can be estimated accurately even under snow conditions. Further, by performing template matching, it is possible to speed up the processing.
Further, the probability update unit 16 integrates the probability distributions obtained at each time, normalizes the integrated probability distributions, derives a cumulative distribution, and updates the probability distribution based on the cumulative distributions to correlate with each other. Since the contribution of the above can be lowered and the probability can be updated, the self-position can be estimated accurately even if the object observation by the peripheral object information acquisition sensor unit 12 is sparse.
Further, the probability update unit 16 derives the log odds value of the posterior probability using the likelihood distribution and updates the probability distribution using the log odds value, so that the contribution of the correlation can be lowered and the probability can be updated. Even if the object observation by the peripheral object information acquisition sensor unit 12 is sparse, the self-position can be estimated accurately.
Further, the offset updating unit 17 updates the offset amount by using the weighted average of the probability distribution, so that even if fences, guardrails, etc., which are static objects of the same pattern, continuously exist on the side of the road. The self-position can be estimated accurately.
Further, the offset update unit 17 sets 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 to prevent excessive offset update in an unstable situation. The self-position can be estimated more accurately.
１ 車両（自車両）
１０ 自己位置推定装置
１１ ＧＮＳＳ／ＩＭＵデータ取得部
１２ 周辺物体情報取得センサ部
１３ ＤＲ部
１４ 観測画像生成部
１５ マッチング部
１６ 確率更新部
１７ オフセット更新部
１８ 補正後位置導出部
４０ 慣性計測装置（ＩＭＵ）
1 vehicle (own vehicle)
10 Self-position estimation device 11 GNSS / IMU data acquisition unit 12 Peripheral object information acquisition sensor unit 13 DR unit 14 Observation image generation unit 15 Matching unit 16 Probability update unit 17 Offset update unit 18 Corrected position derivation unit 40 Inertial measurement unit (IMU) )
Claims (7)
ＧＮＳＳ衛星及び慣性計測装置からのデータを取得するＧＮＳＳ／ＩＭＵデータ取得部と、
前記車両の周辺に存在する前記物体の前記観測情報を取得する周辺物体情報取得センサ部と、
前記ＧＮＳＳ／ＩＭＵデータ取得部で取得した前記データに基づいてデッドレコニングにより前記車両の位置を推定するＤＲ部と、
前記ＤＲ部で推定した前記車両の位置及び前記周辺物体情報取得センサ部で取得した前記観測情報に基づき、前記物体のうち静的物体の位置がマッピングされた前記観測画像を生成する観測画像生成部と、
前記観測画像と前記地図画像とのマッチングを行うマッチング部と、
前記マッチングの結果に基づいてオフセットの確率分布を更新する確率更新部と、
前記確率分布に基づいてオフセット量を更新するオフセット更新部と、
前記ＤＲ部で推定した前記車両の位置を前記オフセット量で補正することにより前記車両の補正後位置を導出する補正後位置導出部とを備えることを特徴とする車両の自己位置推定装置。 It is a self-position estimation device that estimates the position of the vehicle by matching the observation image created in real time with the map image created in advance based on the observation information of the objects existing around the vehicle.
GNSS / IMU data acquisition unit that acquires data from GNSS satellites and inertial measurement units,
Peripheral object information acquisition sensor unit that acquires the observation information of the object existing around the vehicle, and
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 DR unit.
An observation image generation unit that generates the observation image in which the position of a static object among the objects is 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. When,
A matching unit that matches the observed image with the map image,
A probability update unit that updates the offset probability distribution based on the matching result,
An offset update unit that updates the offset amount based on the probability distribution,
A vehicle self-position estimation device including a corrected position derivation unit that derives a corrected 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 the observation information of the object through the snow.
The matching unit performs template matching and
The first aspect of claim 1, wherein the probability updating 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. Vehicle self-position estimation device.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019111748A JP7298882B2 (en) | 2019-06-17 | 2019-06-17 | Vehicle self-localization device and vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019111748A JP7298882B2 (en) | 2019-06-17 | 2019-06-17 | Vehicle self-localization device and vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2020204501A true JP2020204501A (en) | 2020-12-24 |
| JP7298882B2 JP7298882B2 (en) | 2023-06-27 |
Family
ID=73838341
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2019111748A Active JP7298882B2 (en) | 2019-06-17 | 2019-06-17 | Vehicle self-localization device and vehicle |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7298882B2 (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113465607A (en) * | 2021-06-30 | 2021-10-01 | 上海西井信息科技有限公司 | Port vehicle positioning method, port vehicle positioning device, electronic equipment and storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008002906A (en) * | 2006-06-21 | 2008-01-10 | Toyota Motor Corp | Positioning device |
| JP2011215056A (en) * | 2010-03-31 | 2011-10-27 | Aisin Aw Co Ltd | Own-vehicle position recognition system |
| JP2018116653A (en) * | 2017-01-20 | 2018-07-26 | 株式会社デンソーテン | Identification apparatus, identification system and identification method |
-
2019
- 2019-06-17 JP JP2019111748A patent/JP7298882B2/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008002906A (en) * | 2006-06-21 | 2008-01-10 | Toyota Motor Corp | Positioning device |
| JP2011215056A (en) * | 2010-03-31 | 2011-10-27 | Aisin Aw Co Ltd | Own-vehicle position recognition system |
| JP2018116653A (en) * | 2017-01-20 | 2018-07-26 | 株式会社デンソーテン | Identification apparatus, identification system and identification method |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113465607A (en) * | 2021-06-30 | 2021-10-01 | 上海西井信息科技有限公司 | Port vehicle positioning method, port vehicle positioning device, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| JP7298882B2 (en) | 2023-06-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Yoneda et al. | Vehicle localization using 76GHz omnidirectional millimeter-wave radar for winter automated driving | |
| US11048267B2 (en) | Mobile robot system and method for generating map data using straight lines extracted from visual images | |
| Levinson et al. | Map-based precision vehicle localization in urban environments. | |
| US20080033645A1 (en) | Pobabilistic methods for mapping and localization in arbitrary outdoor environments | |
| JP5162849B2 (en) | Fixed point position recorder | |
| CN112074885A (en) | Lane sign positioning | |
| Wang et al. | Intelligent vehicle self-localization based on double-layer features and multilayer LIDAR | |
| US20220197301A1 (en) | Vehicle Localization Based on Radar Detections | |
| JP4984659B2 (en) | Own vehicle position estimation device | |
| US20230194269A1 (en) | Vehicle localization system and method | |
| Kellner et al. | Road curb detection based on different elevation mapping techniques | |
| Moras et al. | Drivable space characterization using automotive lidar and georeferenced map information | |
| Vora et al. | Aerial imagery based lidar localization for autonomous vehicles | |
| Suganuma et al. | Localization for autonomous vehicle on urban roads | |
| Suganuma et al. | Map based localization of autonomous vehicle and its public urban road driving evaluation | |
| JP7298882B2 (en) | Vehicle self-localization device and vehicle | |
| AU2017254915B2 (en) | Information processing system and information processing method | |
| EP4016129A1 (en) | Radar reference map generation | |
| US20220196829A1 (en) | Radar Reference Map Generation | |
| JP7289761B2 (en) | Vehicle self-position estimation device and self-position estimation method | |
| RU2807978C1 (en) | Method for visual localization of urban rail transport based on kalman particle filters and mapping data | |
| Gu et al. | Correction of vehicle positioning error using 3D-map-GNSS and vision-based road marking detection | |
| Kreibich et al. | Lane-level matching algorithm based on GNSS, IMU and map data | |
| US20230101472A1 (en) | Methods and Systems for Estimating Lanes for a Vehicle | |
| Britt | Lane detection, calibration, and attitude determination with a multi-layer lidar for vehicle safety systems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A80 | Written request to apply exceptions to lack of novelty of invention |
Free format text: JAPANESE INTERMEDIATE CODE: A80 Effective date: 20190703 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220523 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20230309 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20230418 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20230515 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20230530 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230608 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7298882 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |