JP2021047099A - Vehicle own-location estimation device and own-location estimation method - Google Patents
Vehicle own-location estimation device and own-location estimation method Download PDFInfo
- Publication number
- JP2021047099A JP2021047099A JP2019170154A JP2019170154A JP2021047099A JP 2021047099 A JP2021047099 A JP 2021047099A JP 2019170154 A JP2019170154 A JP 2019170154A JP 2019170154 A JP2019170154 A JP 2019170154A JP 2021047099 A JP2021047099 A JP 2021047099A
- Authority
- JP
- Japan
- Prior art keywords
- self
- map
- position estimation
- unit
- dimensional
- 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
- 238000000034 method Methods 0.000 title claims description 39
- 238000000605 extraction Methods 0.000 claims abstract description 58
- 238000012937 correction Methods 0.000 claims abstract description 8
- 239000000284 extract Substances 0.000 claims description 9
- 230000006399 behavior Effects 0.000 description 19
- 238000011156 evaluation Methods 0.000 description 19
- 238000010586 diagram Methods 0.000 description 12
- 230000007613 environmental effect Effects 0.000 description 12
- 238000012545 processing Methods 0.000 description 10
- 238000012217 deletion Methods 0.000 description 7
- 230000037430 deletion Effects 0.000 description 7
- 230000006870 function Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 6
- 238000009434 installation Methods 0.000 description 6
- 238000012795 verification Methods 0.000 description 6
- 239000002245 particle Substances 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 239000003550 marker Substances 0.000 description 4
- 230000033228 biological regulation Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000003384 imaging method Methods 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000010801 machine learning Methods 0.000 description 2
- 230000011218 segmentation Effects 0.000 description 2
- YREOLPGEVLLKMB-UHFFFAOYSA-N 3-methylpyridin-1-ium-2-amine bromide hydrate Chemical compound O.[Br-].Cc1ccc[nH+]c1N YREOLPGEVLLKMB-UHFFFAOYSA-N 0.000 description 1
- 206010034719 Personality change Diseases 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010790 dilution Methods 0.000 description 1
- 239000012895 dilution Substances 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
Description
本発明は、自車が走行する周囲環境構造と、地図情報に記録されている構造情報を照合し、自車が地図中のどこにいるのかを示す詳細位置を推定する、車両の自己位置推定装置および自己位置推定方法に関する。 The present invention is a vehicle self-position estimation device that collates the surrounding environment structure in which the vehicle travels with the structural information recorded in the map information and estimates a detailed position indicating where the vehicle is on the map. And self-position estimation method.
従来、測位装置により検出された車両の大凡の現在位置を補正する技術として、車両に搭載されたカメラを用いて車両から交差点までの距離を求め、地図情報における車両の位置を特定することにより、車両の大凡の現在位置を補正する技術が提案されている。 Conventionally, as a technology for correcting the approximate current position of a vehicle detected by a positioning device, the distance from the vehicle to the intersection is obtained by using a camera mounted on the vehicle, and the position of the vehicle in the map information is specified. Techniques have been proposed to correct the approximate current position of the vehicle.
例えば、特許文献1の車両位置判定方法では、晴天時や積雪時、高速走行時や左右走行車線などの走行条件に基づいて認識対象とする領域を変更することで、地物認識部の処理負荷を低減した自己位置推定技術について記載がある。
For example, in the vehicle position determination method of
特許文献1では、積雪によって走行環境の形状が変わる場合や、圧雪路で路面標示が見えない道路が続く場合には、同文献の段落0028や図2に示されるように、カメラが撮影した画像の上半分の領域に含まれる道路標識や案内標識を認識することで、車両の現在位置の推定精度を数m程度の誤差まで補正することができる。
In
しかしながら、特許文献1の技術では、自己位置推定誤差を、自動運転に必要とされる数cm以下に抑制することができず、自動運転には利用できないという問題があった。
However, the technique of
そこで、本発明では、積雪によって走行環境に存在する環境構造物が遮蔽される状況や、環境構造物の形状が変わる状況が継続する場合、また、圧雪路で路面標示が見えない状況が継続する場合であっても、自動運転の実行時に求められる高精度な自己位置推定が可能な自己位置推定装置を提供することを目的とする。 Therefore, in the present invention, the situation where the environmental structure existing in the traveling environment is shielded by the snowfall, the situation where the shape of the environmental structure changes continues, and the situation where the road markings cannot be seen on the snow-packed road continue. Even in this case, it is an object of the present invention to provide a self-position estimation device capable of highly accurate self-position estimation required at the time of executing automatic operation.
上記課題を解決するために、本発明の自己位置推定装置は、地図情報を記憶する地図データベースと、周囲環境の三次元点群データを取得する周囲環境観測センサと、に接続され、車両の自己位置を推定する自己位置推定装置であって、大凡の自己位置を推定する粗位置推定部と、詳細な自己位置を推定する三次元照合部と、を有し、前記三次元照合部は、前記地図情報から積雪時に観測可能な外形情報を抽出した三次元地図データを生成する照合用マップ抽出部と、前記周囲環境観測センサから前記三次元点群データを得るセンシング結果抽出部と、前記三次元地図データと前記三次元点群データを三次元照合することで、前記粗位置推定部が推定した自己位置を補正する位置補正部と、を有するものとした。 In order to solve the above problems, the self-position estimation device of the present invention is connected to a map database that stores map information and an ambient environment observation sensor that acquires three-dimensional point group data of the surrounding environment, and is connected to the vehicle's self. It is a self-position estimation device that estimates a position, and has a rough position estimation unit that estimates a rough self-position and a three-dimensional collation unit that estimates a detailed self-position. A matching map extraction unit that generates three-dimensional map data that extracts observable external shape information during snowfall from map information, a sensing result extraction unit that obtains the three-dimensional point group data from the surrounding environment observation sensor, and the three-dimensional It is assumed that the map data and the three-dimensional point group data are three-dimensionally collated to have a position correction unit that corrects the self-position estimated by the rough position estimation unit.
本発明の自己位置推定装置によれば、積雪によって走行環境に存在する環境構造物が遮蔽されたり、環境構造物の形状が変わる状況が継続したり、また、圧雪路で路面標示が見えない状況が継続したりする場合にでも、自動運転の実行時に求められる高精度な自己位置推定が可能にとなる。 According to the self-position estimation device of the present invention, the environmental structure existing in the traveling environment is shielded by the snow cover, the shape of the environmental structure continues to change, and the road marking is not visible on the snow-packed road. It is possible to estimate the self-position with high accuracy required at the time of executing the automatic operation even when the above is continued.
以下、本発明に係る自己位置推定装置の実施例を、図面を用いて説明する。 Hereinafter, examples of the self-position estimation device according to the present invention will be described with reference to the drawings.
図1は、本発明の実施例1に係る自己位置推定装置10の構成とそれに接続されるセンサ等を説明するブロック図である。ここに示すように、自己位置推定装置10は、車両挙動観測センサ1、絶対位置取得センサ2、地図データベース3、周囲環境観測センサ4、積雪判定部5、車両制御部6と接続されている。また、自己位置推定装置10は、粗位置推定部11、マップマッチング部12、三次元照合部13を備えており、三次元照合部13は、汎用照合部14と、積雪対応照合部15と、を有している。なお、自己位置推定装置10は、具体的には、CPU等の演算装置、半導体メモリ等の主記憶装置、補助記憶装置、および、通信装置などのハードウェアを備えた計算機であり、主記憶装置にロードされたプログラムを演算装置が実行することで、粗位置推定部11等の各機能を実現するが、以下では、このような計算機分野での周知技術を適宜省略しながら、各部の詳細を説明する。
FIG. 1 is a block diagram illustrating a configuration of a self-
車両挙動観測センサ1は、例えば加速度センサ、角速度センサ、角加速度センサ、車輪パルスセンサ、車速センサなどの自車の相対運動を観測するための1つ以上からなるセンサである。これらの車両挙動観測センサ1から得られたセンシング結果を時系列で累積することにより、自車の相対的な移動および姿勢変化を連続的に推定することが可能である。なお、センシング結果に混入する誤差やノイズも累積してしまうため、一般的に自車挙動観測センサ1による推定結果は徐々に精度が悪化する特性がある。
The vehicle
絶対位置取得センサ2は、例えばGNSS(Global Navigation Satellite System:全球測位衛星システム)の受信装置、スードライド(擬似衛星)やビーコン(無線標識)などの電波の受信装置、環境側に設置したバーコードなどの可視マーカや磁気マーカの認識装置などの、自車の絶対位置を認識するための1つ以上からなるセンサである。これらの絶対位置取得センサ2から得られたセンシング結果から、その時点の自車位置および姿勢を推定することが可能である。なお、絶対位置取得センサ2による自車位置および姿勢の推定は、時間的または空間的な観点で離散的であり、例えばGNSSの測位間隔は1秒から5秒程度であることが多く、マーカ認識装置はマーカが設置されている地点近傍の場所に限定されている。 The absolute position acquisition sensor 2 includes, for example, a GNSS (Global Navigation Satellite System) receiver, a radio wave receiver such as a sudride (pseudo satellite) or a beacon (radio beacon), a bar code installed on the environment side, or the like. It is a sensor consisting of one or more for recognizing the absolute position of the own vehicle, such as a recognition device for a visible marker or a magnetic marker. From the sensing results obtained from these absolute position acquisition sensors 2, it is possible to estimate the position and posture of the own vehicle at that time. The estimation of the vehicle position and posture by the absolute position acquisition sensor 2 is discrete from the viewpoint of time or space. For example, the positioning interval of GNSS is often about 1 to 5 seconds, and marker recognition. The device is limited to a location near the point where the marker is installed.
粗位置推定部11では、車両挙動観測センサ1と絶対位置取得センサ2のセンシング結果を用いて、時々刻々と変化する大凡の自己位置(粗位置)を推定する。また、ここでの自己位置推定の初期値として、前時刻に三次元照合部13で算出された自己位置推定結果を用いることができる。なお、粗位置推定部11で推定する自己位置(粗位置)には、数m程度の誤差がある可能性があるため、目的地への経路を案内するカーナビゲーション等には利用できるが、自動運転には利用できない精度のものである。
The rough
マップマッチング部12では、地図データベース3からの地図情報と、粗位置推定部11からの自己位置推定結果を照合することで、自己位置推定結果の精度改善をおこなう。例えば、自車が交差点を通過する状況を想定すると、自車は地図データベース3に格納されている交差点の形状に沿って走行しているはずであるので、粗位置推定部11による自己位置推定結果が交差点形状に合わない場合には、自己位置推定結果に誤差があるものとし扱い、地図に合わせて自己位置を補正する。
The map matching
周囲環境観測センサ4は、例えばステレオカメラ、単眼カメラ、LiDAR(Laser Imaging Detection and Ranging:レーザによる画像検出と測距)、ミリ波、超音波ソナー、TOFカメラ(Time of Flight)などの自車周囲を観測するための1つ以上からなるセンサである。これらを用いて自車周囲の障害物や環境構造物の形状、模様、色、大きさ、あるいは三次元点群の集合などの情報を取得するとともに、自車に対する相対的な位置、姿勢を取得する。 The ambient environment observation sensor 4 is, for example, a stereo camera, a monocular camera, a LiDAR (Laser Imaging Detection and Ranging), a millimeter wave, an ultrasonic sonar, a TOF camera (Time of Flight), or the like around the vehicle. It is a sensor consisting of one or more for observing. Using these, information such as the shape, pattern, color, size, or set of three-dimensional point clouds of obstacles and environmental structures around the vehicle is acquired, and the position and attitude relative to the vehicle are acquired. To do.
積雪判定部5は、周囲環境観測センサ4からの情報を入力とし、観測している領域内に積雪があるか否かを判定する。この判定は公知の技術が複数提案されているため、それらを用いても良い。例えば、単眼カメラの場合、昼間は積雪表面で太陽光をよく反射するため、撮像時の明度が高くなり白色領域として観測できること、自動露光機能がある場合にはシャッタ時間が短くなること、といった特徴がある。そこで、これらを実験的に求めた閾値求めておき、所定の閾値よりも大きい輝度値をもつ画素数が撮像領域内に一定以上あり、かつ所定の閾値よりもシャッタ時間が短い場合に、積雪があると判定することができる。なお、この積雪判定部5は、専用のハードウェアを用意しても良いし、自己位置推定装置10を実現するとのと同じ計算機により実現されるものであっても良い。
The snow
三次元照合部13は、時々刻々と変化する自己位置を数cm程度の精度で推定するものであり、汎用照合部14と、積雪対応照合部15を有し、積雪判定部5の判定結果に応じて、使用する照合部を切り替える。例えば、判定結果が積雪なしの場合は、汎用照合部14の使用が選択され、判定結果が積雪ありの場合は、積雪対応照合部15の使用が選択される。なお、積雪が少ないと判断されたときなどには、両照合部を併用しても良い。
The three-
汎用照合部14は、地図データベース3からの地図情報と、マップマッチング部12からの自己位置推定結果と、周囲環境観測センサ4からの障害物や環境構造物の形状、模様、色、大きさ、あるいは三次元点群などの情報を取得する。そして、地図情報から、自己位置推定結果付近の三次元構造物を含む周辺地図を抽出する。次に、抽出された三次元構造物の位置や姿勢と、周囲環境観測センサ4で得られた障害物や環境構造物の位置や姿勢を比較し、照合することで地図データベース3に対する周囲環境観測センサ4の視点を算出する。この照合技術は公知の技術が複数提案されているため、それらを用いても良い。
The general-purpose collation unit 14 includes map information from the
例えば、ICP(Iterative Closest Point)という技術がある。これは2つの三次元点群A,Bを照合するときに、まず仮視点を仮定して点群を重ね合わせ、点群Aのある点aに対し、点群B中で最も近い点bを算出し、点aと点bの二乗距離を算出する。この操作を点群A中のすべての点に対しておこない、ペアとなった点間の距離を合計する。次に、任意の方向に仮視点を移動させ、同様にペアとなった点間の距離を合計する。合計距離が小さくなる方向に仮視点を移動させていくことで、最終的に最も点群Aと点群Bが近くなる視点を求めることができる。このような技術を用いることにより、汎用照合部14では、自己位置を数cm程度の精度で推定できる。 For example, there is a technology called ICP (Iterative Closest Point). This is because when collating two three-dimensional point clouds A and B, the point clouds are first superimposed on the assumption of a tentative viewpoint, and the closest point b in the point cloud B is set with respect to a certain point a in the point cloud A. Calculate and calculate the squared distance between points a and b. This operation is performed on all the points in the point cloud A, and the distances between the paired points are totaled. Next, the temporary viewpoint is moved in an arbitrary direction, and the distances between the paired points are summed in the same manner. By moving the tentative viewpoint in the direction in which the total distance becomes smaller, it is possible to finally find the viewpoint where the point group A and the point group B are closest to each other. By using such a technique, the general-purpose collation unit 14 can estimate the self-position with an accuracy of about several cm.
積雪対応照合部15では、基本的に、汎用照合部14と同等の処理が実施され、詳細な自己位置を推定する。ただし、積雪対応照合部15は、積雪判定部5で積雪があると判定された場合に利用されるものであるため、積雪状況に特化した処理も実施される。この処理の詳細は後述する。
The snow
三次元照合部13で算出され、更新された自己位置は、粗位置推定部11の入力にフィードバックされ、次の時刻の初期の自己位置として利用することができる。
The self-position calculated and updated by the three-
車両制御部6は、運転者の運転操作に対して、運転者の操作を要しない自動運転を含む運転支援を可能とするものであり、先行車両に対する追い越し、車線維持、高速道路自動合流等を含む適応走行制御、障害物への自動回避制御、一時停止などの運転支援制御を実施する。
The
以下、本実施例の、粗位置推定部11、マップマッチング部12、三次元照合部13の詳細を順次説明する。
Hereinafter, the details of the rough
<粗位置推定部11>
図2に粗位置推定部11で実施される処理のフローチャートを示す。
<Rough
FIG. 2 shows a flowchart of the process executed by the rough
前述したように、粗位置推定部11では、車両挙動観測センサ1と絶対位置取得センサ2のセンシング結果から、時々刻々と変化する大凡の自己位置(粗位置)を推定する。車両挙動観測センサ1からは連続的に推定結果が得られるが、時間経過とともに誤差が累積する。一方、絶対位置取得センサ2は誤差が累積しないが、離散的にしか自己位置が得られない。このため、絶対位置取得センサ2の推定結果が得られ、かつ信頼できる状態である場合にのみ、絶対位置取得センサ2の出力に基づいて自己位置を更新し、そうでない場合には、車両挙動観測センサ1の出力に基づいて自己位置と方位を相対的に変化させるよう更新する。粗位置推定部11での処理は、具体的には、次のステップからなる。
As described above, the rough
ステップS1:車両挙動観測センサ1、絶対位置取得センサ2からセンサ時刻を付加した上でセンサ出力値を受信する。センサ時刻は内部に実時間時計を持つセンサについてはその時刻を利用し、加速度センサや角速度センサのような内部に実時間時計を持たないセンサの場合は、センシング時刻とセンサ出力を受信した装置との間の伝送時間を加味したオフセット時間を考慮して、センサ出力を受信する装置で時刻を付与し、記憶または蓄積する。
Step S1: The sensor output value is received from the vehicle
ステップS2:絶対位置取得センサ2の出力は時間または空間の観点で離散的であるため、常に出力が得られているとは限らない。そこで、前回値から更新されたか否かを判定する必要がある。出力が更新されていない場合には、ステップS5にすすむ。更新された場合にはステップS3にすすむ。 Step S2: Since the output of the absolute position acquisition sensor 2 is discrete in terms of time or space, the output is not always obtained. Therefore, it is necessary to determine whether or not the value has been updated from the previous value. If the output has not been updated, the process proceeds to step S5. If it is updated, proceed to step S3.
ステップS3:絶対位置取得センサ2の出力は状況によって精度が悪化する場合がある。例えば、GNSS受信装置においては、測位衛星からの電波が届きにくいトンネル内や地下駐車場などでは精度良い位置情報が得られない場合がある。また、多くの高層ビルが建ち並ぶ市街地では、衛星電波が建物で反射および回折して受信されるマルチパス現象が起きることで誤った位置情報を得る場合がある。こうした状況を判断するために、測位ステータス、DOP値(Dilution Of Precision:精度劣化度)や、誤差楕円半径、使用衛星数、速度精度、方位精度などの指標がある。これらの指標が所定の閾値より良好であることをもって、精度が一定以上であると判定する。所定の閾値よりも良好であること、とは、当該業務従事者であれば自明ではあるが、例えば、測位ステータスであれば2D測位、3D測位、RTK-FLOAT測位、RTK-FIX測位の順に良好さが増す。また、DOP値と誤差楕円半径は値が小さいほど良好であり、使用衛星数は多いほど良好である。精度が一定以上でない場合には、ステップS5にすすむ。精度が一定以上の良好な状態であればステップS4にすすむ。なお、精度が一定以上であるか否かを判定する際、複数の指標値をAND条件とOR条件を適宜使用して結合してもよい。 Step S3: The accuracy of the output of the absolute position acquisition sensor 2 may deteriorate depending on the situation. For example, in a GNSS receiving device, accurate position information may not be obtained in a tunnel or an underground parking lot where radio waves from a positioning satellite are difficult to reach. Further, in an urban area where many high-rise buildings are lined up, incorrect position information may be obtained due to a multipath phenomenon in which satellite radio waves are reflected and diffracted by the buildings and received. In order to judge such a situation, there are indexes such as positioning status, DOP value (Dilution Of Precision), error elliptical radius, number of satellites used, speed accuracy, and azimuth accuracy. When these indexes are better than a predetermined threshold value, it is determined that the accuracy is above a certain level. It is obvious to the worker concerned that it is better than a predetermined threshold value, but for example, in the case of positioning status, it is better in the order of 2D positioning, 3D positioning, RTK-FLOAT positioning, and RTK-FIX positioning. Increases. The smaller the DOP value and the radius of the error ellipse, the better, and the larger the number of satellites used, the better. If the accuracy is not above a certain level, the process proceeds to step S5. If the accuracy is in a good state above a certain level, the process proceeds to step S4. When determining whether or not the accuracy is above a certain level, a plurality of index values may be combined by appropriately using the AND condition and the OR condition.
ステップS4:絶対位置取得センサ2による最新の出力が良好な精度で得られている場合には、この出力結果を自己位置に反映させることで、車両挙動観測センサ1による累積誤差をリセットすることができる。自己位置への反映は、十分に信頼性が高い場合には、絶対位置取得センサ2の値をそのまま自己位置に設定してもよいが、カルマンフィルタやパーティクルフィルタを用いて安定化させた出力値を自己位置に設定してもよい。
Step S4: When the latest output by the absolute position acquisition sensor 2 is obtained with good accuracy, the cumulative error by the vehicle
ステップS5:絶対位置取得センサ2の出力が古い場合、もしくは出力精度が劣化している場合には、車両挙動観測センサ1の出力を加算することで、短時間に変化した自己位置を更新する。その際、車両挙動観測センサ1の出力をそのまま自己位置に加算してもよいが、カルマンフィルタやパーティクルフィルタを用いて安定化させてもよい。
Step S5: When the output of the absolute position acquisition sensor 2 is old or the output accuracy is deteriorated, the self-position changed in a short time is updated by adding the output of the vehicle
ステップS6:絶対位置取得センサ2の出力が良好な精度で得られている場合には、自己位置推定の信頼度を高くなるように設定し、車両挙動観測センサ1の出力を累積するに従い、信頼度が徐々に低くなるように設定する。例えば、次の式1のように設定できる。
Step S6: When the output of the absolute position acquisition sensor 2 is obtained with good accuracy, the reliability of the self-position estimation is set to be high, and the reliability is increased as the output of the vehicle
(信頼度)=α÷(絶対位置取得センサの精度指標)
−β×(車両挙動観測センサによる自己位置更新回数) … (式1)
または、カルマンフィルタを用いる場合には共分散行列の項を、信頼度算出の式に加えてもよい。
(Reliability) = α ÷ (Accuracy index of absolute position acquisition sensor)
−β × (Number of self-position updates by vehicle behavior observation sensor)… (Equation 1)
Alternatively, when the Kalman filter is used, the term of the covariance matrix may be added to the formula for calculating the reliability.
具体的な数値例を挙げる。(絶対位置取得センサ2の精度指標)を出力周期1[Hz]のGNSS受信機の誤差楕円半径とし、その値域が1〜50[m]とすると、小さい値であるほど精度が良好となる。(車両挙動観測センサ1による自己位置更新回数)を出力周期20[Hz]加速度センサと各加速度センサとする。また、α=1000、β=2とする。この場合、GNSS受信機の出力周期に合わせて、20〜1000の信頼度が設定され、その後1秒かけて徐々に信頼度が低下していく挙動となる。 A concrete numerical example is given. Assuming that (the accuracy index of the absolute position acquisition sensor 2) is the error elliptical radius of the GNSS receiver having an output period of 1 [Hz] and the range is 1 to 50 [m], the smaller the value, the better the accuracy. (Number of self-position updates by the vehicle behavior observation sensor 1) is defined as an output cycle 20 [Hz] acceleration sensor and each acceleration sensor. Further, α = 1000 and β = 2. In this case, the reliability is set from 20 to 1000 according to the output cycle of the GNSS receiver, and then the reliability gradually decreases over 1 second.
<マップマッチング部12>
次にマップマッチング部12の処理の流れを説明する。
<
Next, the processing flow of the
マップマッチング部12では、粗位置推定部11で推定された自己位置が、地図上のどの道路を走行しているかを算出し、道路から逸脱していない場合には道路上に乗るように自己位置を補正する。このマップマッチング方法は公知の方法が複数提案されている。以下に具体例を1つ示す。
The
まず、道路情報を地図データベース3から取得する。この道路情報は、道路ノード番号、ノード位置、道路リンク番号、道路リンクの幅員などのうち1つ以上を含む。
First, road information is acquired from the
次に、粗位置推定部11が推定した自己位置の周囲に存在する各道路リンクに対して、自己位置からの垂線をおろし、各道路リンクとの交点を道路リンク候補点として求める。さらに、垂線の長さと、道路リンク方位と自車方位θVとの差を考慮して評価量を算出する。
Next, for each road link existing around the self-position estimated by the rough
図3は、道路リンクに対して評価値を算出する場合の一例を説明するための説明図である。例えば、自車の位置(自車位置PV)および姿勢(自車方位θV)とし、自車位置から所定の距離内にある複数の道路リンクを、道路ノードNA1から道路ノードNA2への道路リンクLA、道路ノードNB1から道路ノードNB2への道路リンクLBとする。さらに、自車位置PVから道路リンクLAに対して下ろした垂線の長さをDA、道路リンクとの交点をPAとする。同様に、自車位置PVから道路リンクLBに対して下ろした垂線の長さをDB、道路リンクとの交点をPBとする。そして、各道路リンク候補点PAおよびPBに対して評価量を算出する。道路リンク方位θA、θBと、自車方位θVの差を方位角誤差と呼ぶ。また、道路リンクまで距離DA、DBを位置誤差と呼ぶ。この方位角誤差と位置誤差に関して共分散誤差を計算し、これに基づいて各道路リンク候補点の評価量を計算する。この計算結果により、道路を走行している確率が最も高い評価量を持つ道路リンク候補点を第一道路リンク候補点とする。 FIG. 3 is an explanatory diagram for explaining an example in the case of calculating the evaluation value for the road link. For example, the position of the vehicle and (vehicle position P V) and attitude (vehicle direction theta V), a plurality of road links at the vehicle position within a predetermined distance from the road node N A1 to road node N A2 road link L a of which the road node N B1 and road link L B to road node N B2. Further, the length of the perpendicular drawn from the vehicle position P V relative to the road link L A to D A, the intersection of the road link and P A. Similarly, the length of the perpendicular drawn from the vehicle position P V relative to the road link L B D B, an intersection between the road link and P B. Then, to calculate the evaluation value for each road link candidate points P A and P B. The difference between the road link directions θ A and θ B and the vehicle direction θ V is called the azimuth error. The distances D A and D B to the road link are called position errors. The covariance error is calculated for this azimuth error and the position error, and the evaluation amount of each road link candidate point is calculated based on this. Based on this calculation result, the road link candidate point having the highest evaluation amount with the highest probability of traveling on the road is set as the first road link candidate point.
次に、道路リンクの位置、方位及びそれらの分散と、幅員情報と、推定位置及び推定方位及びそれらの共分散とに基づいて、自車が道路を逸脱しているかどうかを判定する。逸脱していないと判定された場合には、自車位置PVを第一道路リンク候補点に近づけるように補正する。その際、第一道路リンク候補点の位置をそのまま自車位置PVに設定してもよいが、カルマンフィルタやパーティクルフィルタを用いて安定化させた出力値を自車位置PVに設定してもよい。 Next, it is determined whether or not the own vehicle deviates from the road based on the position and direction of the road link and their variances, the width information, and the estimated position and the estimated direction and their covariance. If it is determined not to deviate it is corrected so as to approach the vehicle position P V in the first road link candidate points. At that time, the position of the first road link candidate points may be directly set on the vehicle position P V, but the output value was stabilized using a Kalman filter or a particle filter is set to the vehicle position P V Good.
<三次元照合部13>
三次元照合部13は、前述したように、その内部に、汎用照合部14と積雪対応照合部15をもち、積雪判定部5からの結果に応じて、いずれか一方、もしくは両方を使用するかを切り替える。例えば、積雪判定部5の判定結果が所定の時間以上「積雪なし」の状態であれば、汎用照合部14の結果のみ使用し、所定の時間以上「積雪あり」の状態であれば、積雪対応照合部15の結果を使用する。積雪判定部5の結果が変動する場合は、部分的に積雪が残っており、除雪が不完全である道路状況であると予想されるため、その場合には汎用照合部14と積雪対応照合部15の両方を使用する。両方を使用する場合、照合結果として出力される、地図中の周囲環境観測センサ4の視点が複数出てくるため、重み付き平均を用いて統合する。なお、カルマンフィルタやパーティクルフィルタを用いて安定化させた出力値を用いてもよい。
<Three-
As described above, the three-
<積雪対応照合部15>
図4は、積雪対応照合部15の機能ブロック図である。ここに示すように、積雪対応照合部15は、外形情報抽出部16、照合用マップ抽出部17、センシング結果抽出部18、センシング結果照合部19、位置補正部20を有する。
<Snow
FIG. 4 is a functional block diagram of the snow
<外形情報抽出部16>
外形情報抽出部16は、マップマッチング部12から出力される自己位置(図3参照)に基づき、自己位置近傍の所定の範囲に該当する地図データベース3に格納されている地図情報を読み込み、立体形状に係る外形情報に変換する。例えば、地図データベース3に格納されている地図情報は、道路自体の形状である舗装路面の領域形状や、ガードレールや防音壁の道路に対する設置位置、ガードレールや防音壁の高さ、制限速度・走行規制・道路案内などを表す標識が設置される棒状・L字状・跨線状などの形状をとる支柱の道路に対する形状種別や設置位置、標識支柱の高さ、標識看板の取付高さと形状、電柱の道路に対する設置位置と高さ、カーブミラーの道路に対する設置位置・形状・鏡部の取付高さ、横断歩道や走行路区分線などの路面標示の位置と形状、信号機の形状や取付高さ、などの1つ以上を含む。また、道路だけでなく、路外にあるビルや塀、樹木などの建造物や構造物の形状を含んでいてもよい。さらに、走行車両に搭載した外界センサで得られた3D形状や点群情報、テクスチャ情報などを、サーバに収集し、通信手段や記録メディアにより配布された情報を含んでいてもよい。外形情報抽出部16は、これらの格納された地図情報を読み込み、含まれる物体の外形情報に変換する。
<External
Based on the self-position (see FIG. 3) output from the
例えば、地図情報から、高さ2mで制限速度50km/hの交通標識の所在情報を読み込んだ場合、その情報を、路外1mの地点に太さ0.1mで長さ2mの円柱が存在するという外形情報、路外1m高さ2mの同じ地点に半径0.6mの円盤状の板が存在するという外形情報、さらにその円盤には50という文字が掲示されているという外形情報、といった3つの外形情報に変換し出力する。 For example, when the location information of a traffic sign with a height of 2 m and a speed limit of 50 km / h is read from map information, a cylinder with a thickness of 0.1 m and a length of 2 m exists at a point 1 m outside the road. External information that there is a disk-shaped plate with a radius of 0.6 m at the same point 1 m outside the road and 2 m high, and external information that the character 50 is posted on the disk. Converts to external shape information and outputs.
また、この外形情報は、3D点群の座標値で出力する方法のほか、円柱や円盤の中心軸とその半径といったベクトル形式での出力方法や、表現する空間を一定サイズの直方体に区切って物体有無の情報を付加するボクセル形式での出力方法など、多くの出力形式がある。本実施例中では3D点群の座標値として出力することと仮定するが、本発明としてはこの形式に限定されない。 In addition to the method of outputting the coordinate values of the 3D point cloud, this external information is also output in the vector format such as the central axis of the cylinder or disk and its radius, and the space to be expressed is divided into rectangular parallelepipeds of a certain size and the object is displayed. There are many output formats, such as a voxel format output method that adds presence / absence information. In this embodiment, it is assumed that the coordinate value is output as a 3D point cloud, but the present invention is not limited to this format.
<照合用マップ抽出部17>
図5に、照合用マップ抽出部17のブロック図を示す。ここに示すように、照合用マップ抽出部17は、支柱抽出部17a、低位外形情報削除部17b、鉛直面抽出部17cから構成される。
<Map extraction unit for
FIG. 5 shows a block diagram of the collation
支柱抽出部17aは、外形情報抽出部16から出力された外形情報から、速度制限標識や規制標識、注意標識などの標識が設置されている支柱に相当する外形情報や、自動車用や歩行者用、路面電車用などの信号機が設置されている支柱に相当する外形情報を抽出する。なお、標識や信号機は、道路交通上の重要度が高く、積雪時であっても最低限これらが見えるように除雪されていることが期待できる。このため、標識や信号機の支柱の外形情報を照合用マップとして抽出することで、センシング結果照合部19における成功率が向上することが期待できる。
From the external shape information output from the external shape
さらに、低位外形情報削除部17bは、周囲環境観測センサ4で観測された積雪高さ、三次元照合部13から過去に出力された地図中の視点の高さと車両に搭載時の取付高さの差分として表される積雪高さ、または、一般的な積雪高さの何れかに基づき、路面から一定の高さを除外した残りの外形情報(すなわち、積雪の上に出ていると思われる外形情報)を抽出してもよい。これにより、積雪時に観測されることのない外形情報を除外した照合用マップを抽出することが可能となる。
Further, the low-level external information deletion unit 17b determines the snow cover height observed by the surrounding environment observation sensor 4, the height of the viewpoint in the map output in the past from the three-
あるいは、低位外形情報削除部17bは、ガードレールや防音壁など、一定の高さで連続的に存在する遮蔽物の向こう側にある外形情報について、路面から一定の高さを除外した残りの外形情報を抽出してもよい。これにより、遮蔽により観測されることのない外形情報を除外した照合用マップを抽出することが可能となる。 Alternatively, the low-level external shape information deletion unit 17b excludes the remaining external shape information from the road surface with respect to the external shape information on the other side of the shield that continuously exists at a constant height such as a guardrail or a soundproof wall. May be extracted. This makes it possible to extract a collation map excluding external information that is not observed due to shielding.
鉛直面抽出部17cは、外形情報抽出部16が出力した外形情報から、積雪時にも照合が可能な鉛直面に関する情報を抽出する。具体的には、水平に近い物体の面の上には雪が積もることが想定され、地図データベース3に格納されている形状情報と、積雪時の実際の環境での観測結果の形状とは異なる可能性がある。このため、外形情報抽出部16が出力する外形情報のうち、鉛直方向から所定の角度閾値以内の鉛直に近い面を抽出することで、雪が積もる可能性が少なく、積雪時に見かけの形状が変わりにくい物体を抽出することができる。
The vertical
以上で説明した支柱抽出部17a等を用いることで、照合用マップ抽出部17では、積雪時であっても照合可能な、支柱や壁面等の外形情報群からなる照合用マップを抽出する。
By using the support
<センシング結果抽出部18>
図6に、センシング結果抽出部18のブロック図を示す。センシング結果抽出部18は、周囲環境観測センサ4から得られた障害物や環境構造物の形状、模様、色、大きさなどの情報から、照合用マップ抽出部17で抽出された外形情報に対応する可能性の高い形状情報を抽出する。なお、周囲環境観測センサ4で取得された形状情報は、既に積雪や他構造物に遮蔽されて対象の全ての部分が観測できていない可能性があるため、観測結果の下方を除外する必要はない。
<Sensing
FIG. 6 shows a block diagram of the sensing
図6に示すように、センシング結果抽出部18は、支柱抽出部18a、低位外形情報削除部18b、鉛直面抽出部18c、積雪領域除去部18d、移動体除去部18eから構成される。
As shown in FIG. 6, the sensing
支柱抽出部18aは、周囲環境観測センサ4から出力されたセンシング結果から、速度制限標識や規制標識、注意標識などの標識が設置されている支柱や、自動車用や歩行者用、路面電車用などの信号機が設置されている支柱を抽出する。具体的には、周囲環境観測センサ4において、機械学習などを利用したパターン識別技術を用いることで、観測している対象がどの種別であるかを認識することが可能となる。また、ステレオカメラやLiDARなどの距離情報が認識できるセンサにおいては、距離情報を利用して観測対象物を背景物から抽出することができる。距離情報が取得できない単眼カメラであっても公知のSemantic Segmentation技術などを用いることで、信号機や標識の支柱の抽出は可能である。前述したように、標識と信号機は、道路交通上の重要度が高く、積雪時であっても最低限これらが見えるように除雪されていることが期待できる。このため、標識と信号機、およびそれらの支柱を抽出することで、センシング結果照合部19における成功率が向上することが期待できる。
From the sensing result output from the ambient environment observation sensor 4, the support column extraction unit 18a is used for columns on which signs such as speed limit signs, regulation signs, and caution signs are installed, for automobiles, pedestrians, trams, and the like. Extract the pillars on which the traffic lights are installed. Specifically, the ambient environment observation sensor 4 can recognize which type of object is being observed by using a pattern identification technique using machine learning or the like. Further, in a sensor such as a stereo camera or LiDAR that can recognize distance information, the observation target can be extracted from the background object by using the distance information. Even with a monocular camera that cannot acquire distance information, it is possible to extract traffic lights and sign posts by using known Semantic Segmentation technology. As mentioned above, signs and traffic lights are of high importance in road traffic, and it can be expected that snow will be removed so that they can be seen at least even during snowfall. Therefore, it can be expected that the success rate in the sensing
低位外形情報削除部18bは、ガードレールや防音壁など、一定の高さで連続的に存在する遮蔽物の向こう側にある外形情報について、支柱上端から一定の高さを除外した残りの外形情報を抽出してもよい。これにより、遮蔽により観測されることのない外形情報を除外した照合用マップを抽出することが可能となる。 The low-level external shape information deletion unit 18b extracts the remaining external shape information excluding a certain height from the upper end of the column for the external shape information on the other side of the shield that continuously exists at a constant height such as a guardrail or a soundproof wall. It may be extracted. This makes it possible to extract a collation map excluding external information that is not observed due to shielding.
鉛直面抽出部18cは、周囲環境観測センサ4から得られたセンシング結果から、積雪によって形状変化を受けにくい鉛直面を抽出する。具体的には、観測結果中の水平に近い面は積雪によって生成された面の可能性があるため、地図データベース3に格納されている形状情報と、観測結果の形状とは異なる可能性がある。このため、周囲環境観測センサ4から得られたセンシング結果のうち、鉛直方向から所定の角度閾値以内の鉛直に近い面を抽出することで、積雪によって生成された面である可能性が少ない物体を抽出することができる。これにより、積雪によって生成された照合対象の地図には含まれていないセンシング結果で誤って照合することを抑止し、センシング結果照合部19における成功率が向上する効果が期待できる。
The vertical
積雪領域除去部18dは、周囲環境観測センサ4から得られたセンシング結果から、積雪によって生成された面を除去する。具体的には、公知のSemantic Segmentation技術などを用いて積雪領域からなる観測結果を除去してもよいし、LiDARにおいてはレーザの反射率が所定の閾値よりも高い場合に積雪から得られた領域であると判定して除外してもよい。これにより、積雪によって生成された照合対象の地図には含まれていない面で誤って照合することを抑止し、センシング結果照合部19における成功率が向上する効果が期待できる。
The snow cover
移動体除去部18eは、周囲環境観測センサ4から得られたセンシング結果から、走行車両や歩行者などの移動体によって得られた観測情報を除去する。具体的には、複数回センシングをおこない、自車移動量を考慮したセンシング結果の差分を抽出することで、環境中の移動体を抽出して除去してもよいし、Deep Learningをはじめとした機械学習などを利用して車両や歩行者の領域を抽出し、対応する領域をセンシング結果から除去してもよい。これにより、照合対象の地図に含まれていない移動体からの観測結果で誤って照合することを抑止し、センシング結果照合部19における成功率が向上する効果が期待できる。
The moving
なお、自車が水平な路面を走行していると記載したが、自車方位θVは車両挙動観測センサ1として搭載されている加速度センサを用いて求めることも可能である。この場合、センシング結果抽出部18に車両挙動観測センサ1の出力値を入力し、自車方位θVを推定した上で、鉛直方向から所定の角度閾値以内の鉛直に近い面を抽出する際の角度計算を補正することができ、抽出の正確性が増す。なお、この車両挙動観測センサ1との接続は図3には図示していない。
Although described as the vehicle is traveling on a horizontal road surface, the vehicle direction theta V can also be be obtained by using an acceleration sensor mounted as a vehicle
<センシング結果照合部19>
センシング結果照合部19は、照合用マップ抽出部17の出力と、センシング結果抽出部18の出力を照合し、地図情報中の自車位置PVおよび自車方位θVを算出する。この照合技術は公知の技術が複数提案されているため、それらを用いても良い。例えば、前述したICPという技術を用いてもよい。
<Sensing
Sensing
センシング結果照合部19では、形状情報の下部は遮蔽などの影響で、照合用マップ抽出部17の出力とセンシング結果抽出部18からの出力の、一方のみで観測されることが起こりうる。このため、地上高の高い部分で重みづけが強くなるように改良したICPを用いても良い。例えば、具体的には以下のようにすればよい。
In the sensing
2つの三次元点群A,Bを照合するときに、まず仮視点を仮定して点群を重ね合わせ、点群Aのある点aに対し、点群B中で最も近い点bを算出し、点aと点bの二乗距離D(a,b)を算出する。この操作を点群A中のすべての点に対しておこない、ペアとなった点間の距離を算出する。通常はこの距離をそのまま合計してマッチングの誤差評価値とするが、改良したICPでは路面からの点a,点bの高さをそれぞれHa,Hbとし、単調増加する変換関数T(x)とおいて、誤差評価値をΣ{D(a,b)×T(Ha×Hb)}として計算する。これにより、路面からの高さの高い点のずれが大きい場合には、路面からの高さが低い点のずれが大きい場合よりも、誤差評価値が大きくなる。 When collating two three-dimensional point clouds A and B, first, assuming a tentative viewpoint, the point clouds are overlapped, and the closest point b in the point cloud B is calculated with respect to a certain point a in the point cloud A. , The squared distance D (a, b) of the point a and the point b is calculated. This operation is performed for all the points in the point cloud A, and the distance between the paired points is calculated. Normally, this distance is summed as it is and used as the matching error evaluation value, but in the improved ICP, the heights of points a and b from the road surface are set to Ha and Hb, respectively, and the conversion function T (x) increases monotonically. Therefore, the error evaluation value is calculated as Σ {D (a, b) × T (Ha × Hb)}. As a result, when the deviation of the point having a high height from the road surface is large, the error evaluation value becomes larger than when the deviation of the point having a low height from the road surface is large.
あとは通常のICPと同様に、任意の方向に仮視点を移動させ、同様にペアとなった点間の距離を算出し、誤差評価値を計算する。誤差評価値が小さくなる方向に仮視点を移動させていくことで、最終的に最も点群Aと点群Bが近くなる視点を求めることができる。この視点位置は周囲環境観測センサ4のセンサ原点に対応しているため、周囲環境観測センサ4の搭載位置と車両原点との相対関係を用いて自車位置PVに変換する。なお、この相対関係は車両構築時に既知の情報となっている。 After that, as in the case of normal ICP, the temporary viewpoint is moved in an arbitrary direction, the distance between the paired points is calculated, and the error evaluation value is calculated. By moving the tentative viewpoint in the direction in which the error evaluation value becomes smaller, it is possible to finally obtain the viewpoint where the point group A and the point group B are closest to each other. The viewpoint position is because it corresponds to the sensor origin of the surrounding environment observation sensors 4, converts the vehicle position P V using the relative relationship between the mounting position and the vehicle origin of the surrounding environment observation sensors 4. This relative relationship is known information when the vehicle is constructed.
センシング結果照合部19では、前述の方法で求めた自車位置PVと、誤差評価値、点群Aと点群Bの各点が所定の距離内で対応付けが得られた点の数を一致面積数として、出力する。
In sensing
<位置補正部20>
位置補正部20では、センシング結果照合部19の出力から照合信頼度を計算し、自己位置を補正する。この照合信頼度は、誤差評価値が小さいほど良好であり、一致面積数が大きいほど良好になるよう設定する。例えば、照合信頼度は次の式2のように定義できる。
<
The
(照合信頼度)=γ÷(誤差評価値)+δ×(一致面積数) … 式2
照合信頼度に応じた重みをつけて、マップマッチング部12で出力された自己位置を、センシング結果照合部19で出力された自車位置PVに近づけるように補正して、車両制御部6に出力する自己位置を生成する。
(Collation reliability) = γ ÷ (error evaluation value) + δ × (number of matching areas)… Equation 2
With a weight corresponding to the matching reliability, the self-position output by the
なお、照合信頼度が十分に大きい所定の閾値以上であれば、車両制御部6に出力する自己位置をセンシング結果照合部19で得られた自車位置PVで置き換えて出力してもよい。また、前述では重みづけで補正すると記載したが、カルマンフィルタやパーティクルフィルタを用いて安定化させてもよい。
Incidentally, verification if reliability is a sufficiently large predetermined threshold or more, may be outputted replaced by the vehicle position P V obtained its own position to be output to the
以上のように、本実施例の自己位置推定装置10によれば、地図とセンサから得られた情報から、それぞれ所定の時間および領域を抽出し、環境情報が記録された地図と、観測された周囲環境構造とで、照合可能な共通領域を抽出することで、積雪によって走行環境に存在する環境構造物が遮蔽されたり、あるいは環境構造物の形状が変わる状況が継続する場合や、圧雪路で路面標示が見えない状況が継続する場合でも、自己位置の詳細な推定が可能になる。
As described above, according to the self-
次に、図7のフローチャートを用いて、本発明の実施例2に係る自己位置推定装置10を説明する。なお、実施例1との共通点は重複説明を省略する。
Next, the self-
実施例1の粗位置推定部11では、図2のフローチャートのステップS2以降の処理から明らかなように、絶対位置取得センサ2の出力が更新されたときに、絶対位置取得センサ2の出力に基づいて自己位置を更新したが、本実施例の粗位置推定部11では、絶対位置取得センサ2の出力更新のタイミングに依存せずに自己位置を更新することで、処理負荷により処理周期が変動しても精度良く自己位置推定をおこなうことが可能になる。以下、実施例1から変更のあるステップのみを図7を用いて説明する。
The rough
例えば、GNSSの受信装置が測位衛星電波を用いて測位を行う周期が処理負荷によって変動する場合や、センシング周期と自己位置推定を行う周期が異なる場合においては、各センサが出力した最終出力値をそのまま使うと、センサ間の時刻整合性がとれず、自己位置推定の位置精度が悪化する。そこで、本実施例では、センサ出力を蓄積しておき、自己位置推定をおこなうタイミングで、蓄積されたセンシング結果を利用する。 For example, when the cycle in which the GNSS receiver performs positioning using the positioning satellite radio waves fluctuates depending on the processing load, or when the sensing cycle and the self-position estimation cycle are different, the final output value output by each sensor is used. If it is used as it is, the time consistency between the sensors cannot be obtained and the position accuracy of the self-position estimation deteriorates. Therefore, in this embodiment, the sensor output is accumulated, and the accumulated sensing result is used at the timing of self-position estimation.
その場合、ステップS1のあとに、以下に述べるステップS11、S12を追加することで、自己位置推定をおこなうことが可能になる。 In that case, self-position estimation can be performed by adding steps S11 and S12 described below after step S1.
ステップS11:自己位置推定の起動タイミングであるか否かを判定し、起動タイミングでなければ、ステップS1に進む。起動タイミングであればステップS12に進む。 Step S11: It is determined whether or not it is the start timing of self-position estimation, and if it is not the start timing, the process proceeds to step S1. If it is the start timing, the process proceeds to step S12.
ステップS12:ステップS1にて時刻を付与されたセンサ出力は過去のタイミングにおける出力値であるため、自己位置推定の計算時の出力値を補間、または過去値から予測して複数センサ間の時刻同期を取る必要がある。例えば、センサ毎にカルマンフィルタを用いたり、線形モデルや多項式モデルを用いたりして当該時刻のセンサ値を予測する。 Step S12: Since the sensor output to which the time is given in step S1 is the output value at the past timing, the output value at the time of calculating the self-position estimation is interpolated or predicted from the past value and the time is synchronized between the plurality of sensors. Need to take. For example, a Kalman filter is used for each sensor, or a linear model or a polynomial model is used to predict the sensor value at that time.
このように、センサ出力時刻と非同期で自己位置推定を行う場合には、上記ステップを追加することで、精度よく自己位置推定をおこなうことが可能になる。また、処理負荷により処理周期が変動しても精度良く自己位置推定をおこなうことが可能になる。 In this way, when self-position estimation is performed asynchronously with the sensor output time, it is possible to perform self-position estimation with high accuracy by adding the above step. Further, even if the processing cycle fluctuates due to the processing load, it becomes possible to accurately estimate the self-position.
次に、図8のブロック図を用いて、本発明の実施例3に係る自己位置推定装置10を説明する。なお、上記の実施例との共通点は重複説明を省略する。
Next, the self-
実施例1の積雪対応照合部15では、積雪判定部5の出力に基づいて、照合用マップ抽出部17やセンシング結果抽出部18が所定の情報を抽出したが、本実施例の積雪対応照合部15では、地図データベース3と周囲環境観測センサ4の照合残差、すなわち過去の地図情報と現在観測された環境情報との差異をフィードバックして活用することで、照合精度を向上し、自己位置推定精度を向上させるものである。
In the snow-covered
このため、本実施例では積雪判定部5の出力に加えて、周囲環境認識センサ4の出力を用いて、照合用マップ抽出部17およびセンシング結果抽出部18の出力を制御する。これにより、積雪状況により適合した照合用マップ抽出およびセンシング結果抽出ができるようになる。
Therefore, in this embodiment, in addition to the output of the snow
図8において、センシング結果照合部19は、前述した照合用マップ抽出部17の出力と、センシング結果抽出部18からの出力を照合し、地図情報中の自車位置PVおよび自車方位θVを算出機能のほかに、照合されずに残った点群の情報から積雪状態を積雪状態記録部7へ出力する。
8, the sensing
例えば、センシング結果照合部19では、2つの三次元点群A,Bを照合するときに、まず仮視点を仮定して点群を重ね合わせ、点群Aのある点aに対し、点群B中で最も近い点bを算出し、点aと点bの二乗距離D(a,b)を算出する。この操作を点群A中のすべての点に対しておこない、ペアとなった点間の距離を算出する。通常はこの距離をそのまま合計してマッチングの誤差評価値とするが、改良したICPでは路面からの点a,点bの高さをそれぞれHa,Hbとし、単調増加する変換関数T(x)とおいて、誤差評価値をΣ{D(a,b)×T(Ha×Hb)}として計算する。これにより、路面からの高さの高い点のずれが大きい場合には、路面からの高さが低い点のずれが大きい場合よりも、誤差評価値が大きくなる。
For example, in the sensing
通常のICPと同様に、任意の方向に仮視点を移動させ、同様にペアとなった点間の距離を算出し、誤差評価値を計算する。誤差評価値が小さくなる方向に仮視点を移動させていくことで、最終的に最も点群Aと点群Bが近くなる視点を求めることができる。 Similar to normal ICP, the temporary viewpoint is moved in an arbitrary direction, the distance between the paired points is calculated, and the error evaluation value is calculated. By moving the tentative viewpoint in the direction in which the error evaluation value becomes smaller, it is possible to finally obtain the viewpoint where the point group A and the point group B are closest to each other.
この際、点aと点bとの二乗距離D(a,b)が所定の距離以内である場合には、点aと点bとが対応付いているものとしてフラグをつけ、二乗距離D(a,b)が所定の距離よりも大きい場合には、点aと点bが対応づかなかったとしてフラグをつけない。こうすることで、最終的にどの点とも対応付けられない点を、フラグがつけられていない点として抽出できる。このどの点とも対応づけられていない点は、前述の変換関数T(x)により、路面からの高さが低い点に多く抽出されることが期待できる。照合用マップ抽出部17から出力された点群のうちで、どの点とも対応づけられていない点であり、かつ高さが低い点は、積雪などによりセンシングできない点であると見なせるため、これらの点の高さ情報をもとに積雪高さを推定することができる。
At this time, if the squared distance D (a, b) between the points a and b is within a predetermined distance, a flag is added assuming that the points a and b correspond to each other, and the squared distance D ( When a and b) are larger than a predetermined distance, the point a and the point b are not flagged as not corresponding to each other. By doing so, the points that cannot be finally associated with any points can be extracted as unflagged points. It can be expected that many points that are not associated with any of these points are extracted by the above-mentioned conversion function T (x) at points having a low height from the road surface. Of the point clouds output from the collation
例えば、高さ2.0mのポールがあり、照合用マップ抽出部17により上部から1.8m分が点群として抽出されているとする。また、センシング結果抽出部17から、当該領域に対応する高さ1.0m分のポール状の点群が出力されているとすると、前述の変換関数T(x)により、センシング結果の点群はマップから抽出された点群の上部にマッチングすることになる。仮に前述の二乗距離の閾値を0.1mとすると、下部の0.7m分の点群がどの点とも対応づけられない点として抽出されることになる。このことから積雪高さが現在のシステムの想定よりも0.7m分高いことが分かる。これを積雪状態記録部7に記録する。
For example, suppose that there is a pole with a height of 2.0 m, and 1.8 m from the top is extracted as a point cloud by the collation
また、センシング結果照合部19では、積雪状態記録部7に記録されている積雪高さの差異を読み出し、所定の値よりも大きい場合には、照合用マップの点群の高さが低い点を削除することにより、抽出された照合用マップとセンシング結果との誤マッチングを抑制し、安定した自己位置推定をできるようにする。
Further, the sensing
なお、積雪高さという観点で、積雪状態記録部7への出力を述べたが、これに限定される必要はない。 Although the output to the snow cover state recording unit 7 has been described from the viewpoint of the snow cover height, it is not necessary to limit the output to this.
図9に、積雪のない環境E1と、積雪後の環境E2の例を示す。この図を用いて、積雪状態記録部7へ出力する内容の別の例を説明する。 FIG. 9 shows an example of the environment E1 without snow and the environment E2 after snow. Another example of the content to be output to the snow cover state recording unit 7 will be described with reference to this figure.
例えば、積雪のない環境E1において、路側帯と車道との区切りを示す路側帯白線L1と、車線区分線L2があるものとする。一般に、積雪時には除雪されるが、積雪量が多い場合には路肩に雪が残り、路側帯白線L1は雪に隠され、車線区分線2のみが可視状態となる場合がある。 For example, in an environment E1 without snow, it is assumed that there is a roadside zone white line L1 indicating a division between a roadside zone and a roadway, and a lane dividing line L2. Generally, snow is removed when snow is accumulated, but when the amount of snow is large, snow may remain on the shoulder of the road, the white line L1 on the roadside may be hidden by snow, and only the lane dividing line 2 may be visible.
センシング結果照合部19では、一定時間走行中に、特定の種別の対象が、所定の照合回数以下でしか照合されない場合、その種別の対象がセンシングできない状態の道路であることを積雪状態記録部7に出力する。また、センシング結果照合部19では、積雪状態記録部7から、照合できない種別の対象については、センシング結果抽出部18からの出力から削除することで、抽出された照合用マップとセンシング結果との誤マッチングを抑制し、安定した自己位置推定をできるようにする。
In the sensing
なお、積雪状態記録部7に記録された情報は、一定時間後に削除して、当該種別の対象に関して再度照合を試行してもよい。 The information recorded in the snow cover state recording unit 7 may be deleted after a certain period of time, and verification may be attempted again for the target of the type.
1 車両挙動観測センサ
2 絶対位置取得センサ
3 地図データベース
4 周囲環境観測センサ
5 積雪判定部
6 車両制御部
7 積雪状態記録部
10 自己位置推定装置
11 粗位置推定部
12 マップマッチング部
13 三次元照合部
14 汎用照合部
15 積雪対応照合部
16 外形情報抽出部
17 照合用マップ抽出部
17a 支柱抽出部
17b 低位外形情報削除部
17c 鉛直面抽出部
18 センシング結果抽出部
18a 支柱抽出部
18b 低位外形情報削除部
18c 鉛直面抽出部
18d 積雪領域除去部
18e 移動体除去部
19 センシング結果照合部
20 位置補正部
1 Vehicle behavior observation sensor 2 Absolute
Claims (10)
周囲環境の三次元点群データを取得する周囲環境観測センサと、に接続され、
車両の自己位置を推定する自己位置推定装置であって、
大凡の自己位置を推定する粗位置推定部と、
詳細な自己位置を推定する三次元照合部と、を有し、
前記三次元照合部は、
前記地図情報から積雪時に観測可能な外形情報を抽出した三次元地図データを生成する照合用マップ抽出部と、
前記周囲環境観測センサから前記三次元点群データを得るセンシング結果抽出部と、
前記三次元地図データと前記三次元点群データを三次元照合することで、前記粗位置推定部が推定した自己位置を補正する位置補正部と、
を有することを特徴とする自己位置推定装置。 A map database that stores map information and
Connected to the ambient environment observation sensor, which acquires 3D point cloud data of the ambient environment,
It is a self-position estimation device that estimates the self-position of the vehicle.
A rough position estimater that estimates the approximate self-position,
It has a three-dimensional collation unit that estimates the detailed self-position, and
The three-dimensional collation unit
A collation map extraction unit that generates three-dimensional map data that extracts observable external shape information during snowfall from the map information, and
A sensing result extraction unit that obtains the three-dimensional point cloud data from the ambient environment observation sensor,
A position correction unit that corrects the self-position estimated by the rough position estimation unit by three-dimensionally collating the three-dimensional map data with the three-dimensional point cloud data.
A self-position estimation device characterized by having.
前記照合用マップ抽出部は、前記地図情報に含まれる支柱の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定装置。 In the self-position estimation device according to claim 1,
The collation map extraction unit is a self-position estimation device characterized by generating the three-dimensional map data by extracting the outer shape information of the columns included in the map information.
前記照合用マップ抽出部は、前記地図情報に含まれる所定の高さ以上の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定装置。 In the self-position estimation device according to claim 1,
The collation map extraction unit is a self-position estimation device characterized by generating the three-dimensional map data by extracting external shape information having a height equal to or higher than a predetermined height included in the map information.
前記照合用マップ抽出部は、前記地図情報に含まれる鉛直面の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定装置。 In the self-position estimation device according to claim 1,
The collation map extraction unit is a self-position estimation device characterized by generating the three-dimensional map data by extracting the outline information of the vertical plane included in the map information.
さらに前記三次元照合の結果を記録する積雪状態記録部を有することを特徴とする自己位置推定装置。 In the self-position estimation device according to any one of claims 1 to 4.
Further, a self-position estimation device including a snow cover state recording unit that records the result of the three-dimensional collation.
大凡の自己位置を推定する第一ステップと、
地図情報から積雪時に観測可能な外形情報を抽出した三次元地図データを生成する第二ステップと、
周囲環境観測センサから三次元点群データを得る第三ステップと、
前記三次元地図データと前記三次元点群データを三次元照合することで、推定した大凡の自己位置を補正する第四ステップと、
を有することを特徴とする自己位置推定方法。 It is a self-position estimation method that estimates the self-position of the vehicle.
The first step in estimating the approximate self-position,
The second step of generating 3D map data by extracting the external shape information that can be observed during snowfall from the map information,
The third step of obtaining 3D point cloud data from the ambient environment observation sensor,
The fourth step of correcting the estimated self-position by three-dimensionally collating the three-dimensional map data with the three-dimensional point cloud data,
A self-position estimation method characterized by having.
前記第二ステップでは、前記地図情報に含まれる支柱の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定方法。 In the self-position estimation method according to claim 6,
The second step is a self-position estimation method characterized in that the three-dimensional map data is generated by extracting the outer shape information of the columns included in the map information.
前記第二ステップでは、前記地図情報に含まれる所定の高さ以上の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定方法。 In the self-position estimation method according to claim 6,
The second step is a self-position estimation method characterized in that the three-dimensional map data is generated by extracting external shape information having a height equal to or higher than a predetermined height included in the map information.
前記第二ステップでは、前記地図情報に含まれる鉛直面の外形情報を抽出することで、前記三次元地図データを生成することを特徴とする自己位置推定方法。 In the self-position estimation method according to claim 6,
The second step is a self-position estimation method characterized in that the three-dimensional map data is generated by extracting the external shape information of the vertical plane included in the map information.
さらに前記三次元照合の結果を記録する第五ステップを有することを特徴とする自己位置推定方法。 In the self-position estimation method according to any one of claims 6 to 9.
Further, a self-position estimation method comprising a fifth step of recording the result of the three-dimensional collation.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2019170154A JP7289761B2 (en) | 2019-09-19 | 2019-09-19 | Vehicle self-position estimation device and self-position estimation method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2019170154A JP7289761B2 (en) | 2019-09-19 | 2019-09-19 | Vehicle self-position estimation device and self-position estimation method |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2021047099A true JP2021047099A (en) | 2021-03-25 |
JP7289761B2 JP7289761B2 (en) | 2023-06-12 |
Family
ID=74876257
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2019170154A Active JP7289761B2 (en) | 2019-09-19 | 2019-09-19 | Vehicle self-position estimation device and self-position estimation method |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP7289761B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP7448304B2 (en) | 2021-11-15 | 2024-03-12 | 飛島建設株式会社 | Structure shape confirmation system |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2018116014A (en) * | 2017-01-20 | 2018-07-26 | パイオニア株式会社 | Own vehicle position estimation device, control method, program and storage medium |
JP2019144066A (en) * | 2018-02-20 | 2019-08-29 | 日産自動車株式会社 | Method and device for estimating road structure |
-
2019
- 2019-09-19 JP JP2019170154A patent/JP7289761B2/en active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2018116014A (en) * | 2017-01-20 | 2018-07-26 | パイオニア株式会社 | Own vehicle position estimation device, control method, program and storage medium |
JP2019144066A (en) * | 2018-02-20 | 2019-08-29 | 日産自動車株式会社 | Method and device for estimating road structure |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP7448304B2 (en) | 2021-11-15 | 2024-03-12 | 飛島建設株式会社 | Structure shape confirmation system |
Also Published As
Publication number | Publication date |
---|---|
JP7289761B2 (en) | 2023-06-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11768959B2 (en) | Anonymizing navigation information for use in autonomous vehicle navigation | |
US11953340B2 (en) | Updating road navigation model using non-semantic road feature points | |
US11774251B2 (en) | Systems and methods for identifying landmarks | |
US12030502B2 (en) | Road vector fields | |
RU2668459C1 (en) | Position evaluation device and method | |
JP6492469B2 (en) | Own vehicle travel lane estimation device and program | |
US20220035378A1 (en) | Image segmentation | |
JP6171612B2 (en) | Virtual lane generation apparatus and program | |
AU2017300097A1 (en) | Crowdsourcing and distributing a sparse map, and lane measurements for autonomous vehicle navigation | |
EP4016115A1 (en) | Vehicle localization based on radar detections | |
JP2007303842A (en) | Vehicle position estimation device, and map information preparation device | |
WO2021240884A1 (en) | Vehicle control device and host vehicle position estimation method | |
US20180208197A1 (en) | Lane keeping assistance system | |
JP7289761B2 (en) | Vehicle self-position estimation device and self-position estimation method | |
JP7298882B2 (en) | Vehicle self-localization device and vehicle | |
KR102137043B1 (en) | Positioning accuracy improvement system | |
EP4016129A1 (en) | Radar reference map generation | |
JP6837626B1 (en) | Feature data generation system, feature database update system, and feature data generation method | |
US20240255291A1 (en) | Sparse map for autonomous vehicle navigation | |
WO2024154231A1 (en) | Environment recognition device, travelable-area determination method, and electronic control device | |
JP2022136145A (en) | Data structure, information processing device, and map data generator | |
JP2022065044A (en) | Information processing apparatus |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220401 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20230224 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20230228 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20230323 |
|
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: 20230531 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7289761 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |