JP4215871B2

JP4215871B2 - Position detection device

Info

Publication number: JP4215871B2
Application number: JP29529798A
Authority: JP
Inventors: 一哉森田
Original assignee: Kenwood KK; Sumitomo Electric Industries Ltd
Current assignee: Kenwood KK; Sumitomo Electric Industries Ltd
Priority date: 1998-10-16
Filing date: 1998-10-16
Publication date: 2009-01-28
Anticipated expiration: 2018-10-16
Also published as: JP2000121373A

Description

【０００１】
【発明の属する技術分野】
この発明は位置検出装置に関し、特に推測航法により求めた走行軌跡に基づき地図データの道路リンク上の位置を検出する位置検出装置に関する。
【０００２】
【従来の技術】
従来より走行している車両の道路地図上の位置を検出する装置として、距離センサと方位センサの出力に基づき、車両の走行に伴って生ずる距離変化量と方位変化量とを積算しながら車両の道路地図上の位置を得る推測航法を用いた位置検出装置が知られている。この位置検出装置は、距離センサおよび方位センサが必然的に有している誤差が走行継続に伴って累積され、車両の道路地図上の位置に含まれる誤差も累積されてしまうという問題があった。
【０００３】
このような問題を考慮し、特開昭６０−４５２８５号公報には、移動軌跡記録装置に記憶保持されている移動体の走行軌跡データと地図上における各道路のデータに基づいて、移動体の現在位置から所定の範囲内で、かつその走行軌跡の方向とほぼ一致する地図上における候補道路を探索する手段と、その探索された候補道路データと走行軌跡のデータとの比較をなして候補道路と走行軌跡との類似度を求めて、そのうち最も類似度の高い候補道路を選定する手段と、その選定された候補道路上に移動体の現在位置が表示されるようにその表示位置の修正を行なわせる手段とを備えた移動体の走行位置表示装置が開示されている。
【０００４】
【発明が解決しようとする課題】
しかしながら、従来の位置表示装置が車両の現在位置を表示する基準となる地図データは、地図上の座標データであるノードデータとそのノードの接続関係のデータとから構成されており、ノードの座標データは、地図座標の分解能に伴う誤差を有するものであった。たとえば、地図データ中のノードＡとノードＡと接続関係を有するノードＢとがる場合、ノードＡとノードＢとを接続する道路リンクＡＢが示す方位は、地図座標の分解能に伴う誤差により、実際の道路が示す方位と異なるものとなってしまい、これが方位の誤差として現われてしまう。この方位の誤差は、道路リンクＡＢが短いほど大きくなる。したがって、実際の道路を走行している車両の方位センサから得られる方位と地図データの道路リンクの示す方位との間に違いが出て、走行軌跡と道路リンクとの類似度を正確に求めることができなかった。
【０００５】
また、特開昭６０−４５２８５号公報に開示の位置検出装置は、車両が属する道路リンクを検出することもできるが、検出された道路リンク上のどこに位置するのかを正確に求めることはできず、車両の現在位置を正確に表示することができなかった。
【０００６】
この発明は上述の問題点を解決するためになされたもので、道路リンク上の車両の位置のずれを少なくし、車両が位置する道路リンク上の正確な位置を求めることが可能な位置検出装置を提供することを目的とする。さらに、この発明は累積される方位誤差を小さくすることが可能な位置検出装置を提供することを目的とする。
【０００７】
【課題を解決するための手段】
この発明のある局面による位置検出装置は、走行距離を検出する距離検出手段と、走行方位を検出する方位検出手段と、距離検出手段と方位検出手段との出力に基づき自己が位置する推測座標を所定のタイミングで順次検出し、推測点列を求める手段と、距離検出手段の出力に基づき地図データから得られる道路リンク上の点列であって、推測点列に対応する複数の対応点列を求める手段と、推測点列から得られる走行軌跡と道路リンクとの類似度であって、複数の対応点列のそれぞれに基づいて算出される複数の類似度を求める類似度算出手段と、複数の対応点列のうち類似度算出手段で求められた類似度が最小となるときの対応点列をもとに道路リンク上の位置を特定する特定手段とを備える。
【０００８】
この発明に従うと、走行軌跡と道路リンクとの類似度を車両が位置する道路リンク上の位置をずらして求め、類似度が最小となるときの道路リンク上の位置に基づき、車両の位置を特定するので、車両の道路リンク上の位置のずれを少なくし、正確な位置を求めることができる位置検出装置を提供することができる。
【０００９】
好ましくは位置検出装置の特定手段は、複数の対応点列の道路リンク上の位置と類似度とから多次元方程式を導き出し、導き出された多次元方程式の類似度が最小となるときの点をもとに道路リンク上の位置を特定することを特徴とする。
【００１０】
この発明に従うと、多次元方程式の類似度が最小となるときの対応点と近傍点との道路リンク上の距離をもとに道路リンク上の位置を特定するので、より正確に道路リンク上の位置を特定することが可能となる。
【００１１】
さらに好ましくは位置検出装置は、道路リンクが複数ある場合は、類似度算出手段は、各々のリンクで類似度を求め、特定手段は、各々のリンクで最小となる類似度である相関度を求め、複数の道路リンクのうち相関度が最小となる道路リンクを選択する選択手段をさらに備える。
【００１２】
この発明に従うと、複数の道路リンクがある場合は、相関度が最小となる道路リンクが選択されるので、車両の道路リンク上の位置のずれを考慮して道路リンクを選択可能な位置検出装置を提供することができる。
【００１３】
さらに好ましくは、位置検出装置の類似度算出手段は、推測点列のうち第１の推測座標を終点とし第１の推測座標よりも前の第２の推測座標を始点とする第１ベクトルと、前記対応点列のうち第１の推測座標に対応する点を終点とし第２の推測座標に対応する点を始点とする第２ベクトルとのなす角を第１の推測座標の角度差とし、複数の推測座標の角度差をもとに類似度を算出することを特徴とする。
【００１４】
この発明に従うと、走行軌跡と道路リンクとの類似度を走行軌跡上の２点を結ぶベクトルと道路リンク上の２点を結ぶベクトルとのなす角に基づき求めるので、道路リンクの有する方位の誤差による影響を少なくして類似度の低下を防止することができる。その結果、道路リンク上の位置の特定や道路リンクの選択を正確に行なうことができる位置検出装置を提供することができる。
【００１５】
この発明の他の局面による位置検出装置は、走行距離を検出する距離検出手段と、走行方位を検出する方位検出手段と、距離検出手段と方位検出手段との出力に基づき自己が位置する推測座標を所定のタイミングで順次検出し、推測点列を求める手段と、距離検出手段の出力に基づき地図データから得られる道路リンク上の点列であって、推測点列に対応する複数の対応点列を求める手段と、推測点列のうち第１の推測座標を終点とし第１の推測座標よりも前の第２の推測座標を始点とする第１ベクトルと、対応点列のうち第１の推測座標に対応する点を終点とし第２の推測座標に対応する点を始点とする第２ベクトルとのなす角を第１の推測座標の角度差とし、複数の推測座標の角度差から角度差の平均値と類似度とを、複数の対応点列のそれぞれに基づいて算出する角度差算出手段と、類似度が最小のときの平均値に基づき方位検出手段の出力を補正する補正手段とを備える。
【００１６】
この発明に従うと、類似度が最小となるときの角度差の平均値に基づき方位検出手段の出力が補正されるので、累積される方位誤差を小さくした位置検出装置を提供することができる。
【００１７】
【発明の実施の形態】
以下この発明の実施の形態を図面を参照して詳細に説明する。
【００１８】
［第１の実施の形態］
図１は、この発明の実施の形態の１つにおける位置検出装置の構成を示すブロック図である。図を参照して、位置検出装置は装置全体の制御を行なうＣＰＵ１０１と、複数のノードとノードの接続関係より構成される地図データを記憶するＣＤ−ＲＯＭ１０３と、プログラムなどを記憶するＲＯＭ１０５と、位置検出動作のための各種定数を記憶するＲＡＭ１０７と、地図表示を行なうディスプレイ１０９と、ユーザから目的地などの情報を入力する入力装置１１１と、衛星からの電波を受信することにより車両の現在位置を識別するＧＰＳ（グローバルポジショニングシステム）装置１１３と、車両の方位を検出するジャイロセンサ１１５と、車両の走行距離を検出する測距センサ１１７とから構成される。
【００１９】
まず、位置検出装置で行なわれる位置検出処理で用いられる走行軌跡と道路リンクとの類似度について説明する。図２は、道路リンクと走行軌跡とを示す図である。道路リンクとは、ＣＤ−ＲＯＭ１０３に記憶されている地図データから導き出されるものである。地図データは、ノードの座標とそれぞれのノードの接続関係からなる。図２に示される地図データは、ノードＡとノードＢとが接続関係を有し、ノードＢとノードＣとが接続関係を有する。これにより、ノードＡとノードＢとを結ぶ道路リンクＡＢと、ノードＢとノードＣとを結ぶ道路リンクＢＣとが導き出される。走行軌跡とは、ＣＰＵ１０１において、ジャイロセンサ１１５の出力である方位と測距センサ１１７の出力である走行距離とに基づき車両が２［ｍ］進むごとに求められる道路地図上の推測座標（Ｑ（−１）〜Ｑ（７））を結んでできた軌跡をいう。推測座標は、ＲＡＭ１０７に記憶される。
【００２０】
図２は、前回の位置検出処理における推測座標をＱ（０）とし、位置検出処理により特定された道路リンク上の座標をＰ（０）として表す。そして、前回の位置検出処理時から車両が２［ｍ］進んだとき（１回目）の推測座標をＱ（１）とし、４［ｍ］進んだとき（２回目）の推測座標をＱ（２）とし、以下同様にして１４［ｍ］進んだとき（７回目）の推測座標Ｑ（７）までを表わす。ここでは、車両が推測座標Ｑ（７）に来たときに、位置検出処理を行なうのに用いる類似度について説明する。
【００２１】
道路リンクＢＣ上の点Ｐ（７）は、推測座標Ｑ（７）の対応点であり、前回の位置検出処理において特定された道路リンクＡＢ上の点Ｐ（０）を基準に１４［ｍ］だけ離れた点として求められる。道路リンクＢＣ上の点Ｐ（６）は、推測座標Ｑ（６）の対応点であり、前回の位置検出処理において求められた点Ｐ（０）を基準に１２［ｍ］離れた距離に位置する点として求められる。同様にして、道路リンク上の点Ｐ（１）〜Ｐ（５）、Ｐ（−１）が求められる。なお、Ｑ（−１）は、前回の位置検出処理を行なったときより１つ前に求められた推測座標である。
【００２２】
このようにして求められた推測座標Ｑ（−１）〜Ｑ（７）の集まりが推測点列であり、道路リンク上の点Ｐ（−１）〜Ｐ（７）の集まりが対応点列である。
【００２３】
図３は、点Ｐ（７）における類似度を求める際に用いられるベクトルを表わす図である。図を参照して、ベクトルＶは、１つの推測座標を終点とし、その推測座標より２つ前に推測された推測座標を始点とするベクトルである。たとえば、ベクトルＶ₀は推測座標Ｑ（５）を始点としＱ（７）を終点とするベクトルである。このようにして、ベクトルＶは、推測座標Ｑ（７）〜Ｑ（１）をそれぞれ終点とするベクトルＶ₀〜Ｖ₆が求められる。
【００２４】
ベクトルＴは、道路リンク上の対応点を終点とし、その対応点よりベクトルＶの始点と終点との走行軌跡上の距離だけ道路リンク上を遡った点を始点とするベクトルである。たとえばベクトルＴ₀は、点Ｐ（７）を終点とし、それよりベクトルＶ₀の始点と終点との走行軌跡上の距離、すなわち４［ｍ］道路リンク上遡った点Ｐ（５）を始点とするベクトルである。道路リンク上の対応点Ｐ（７）〜Ｐ（１）をそれぞれ終点とするベクトルＴ₀〜Ｔ₆が求められる。
【００２５】
図４は、ベクトルＶとベクトルＴとのなす角を示す図である。図４（Ａ）は、ベクトルＶ₀とベクトルＴ₀を示し、それらのベクトルのなす角θ₀は０である。図４（Ｂ）は、ベクトルＶ₁とベクトルＴ₁とのなす角を示し、それらのベクトルのなす角はθ₁である。図４（Ｃ）は、ベクトルＶ₂とベクトルＴ₂とを示し、それらのベクトルのなす角はθ₂である。図４（Ｄ）は、ベクトルＶ₃〜Ｖ₆とベクトルＴ₃〜Ｔ₆とを示し、それらのベクトルのなす角θ₃〜θ₆は、それぞれ０である。
【００２６】
このようにして求めたベクトルＶとベクトルＴとのなす角θ₀〜θ₆の平均値と分散値を求めることができる。そして求めた分散値が道路リンク上の対応点Ｐ（７）における類似度である。この対応点Ｐ（７）における類似度は、推測座標Ｑ（１）からＱ（７）までの走行軌跡と、道路リンクの点Ｐ（１）からＰ（７）までの道路リンクとが類似している度合いを示すものである。
【００２７】
なお、本実施の形態においては、ベクトルＶを、１つの推測座標を終点とし、その推測座標より２つ前に推測された推測座標を始点とするベクトルとしたが、これに限られず、１つ前または３つ以上前に推測された推測座標を始点とするベクトルとしてもよい。また、類似度をＶ₀〜Ｖ₆とＴ₀〜Ｔ₆のそれぞれ７つのベクトルのなす角θ₀〜θ₆の分散値として求めたが、ベクトルの数はこれより多くてもよく、また少なくてもよい。さらに、類似度を分散値としてではなく、平均値または総和としてもよい。
【００２８】
次に位置検出装置のＣＰＵ１０１で行なわれる位置検出処理について説明する。図５は、ＣＰＵ１０１で行なわれる位置検出処理の流れを示すメインフロー図である。図を参照して、位置検出処理は、候補リンクを抽出するステップＳ０１と、抽出された候補リンクが１つの場合（ステップＳ０２でＮＯ）、１つの候補リンクについて候補リンク処理を行なうステップＳ０３と、抽出された候補リンクが複数の場合（ステップＳ０２でＹＥＳ）、それぞれの候補リンクに対して候補リンク処理を行ない（ステップＳ０４，Ｓ０５）、候補リンク処理の結果を用いて１つの候補リンクを特定するステップＳ０６と、候補リンク処理（ステップＳ０３またはステップＳ０４）の結果を用いて道路リンク上の位置を決定するステップＳ０７と、推測航法の方位を補正するステップＳ０８とを含む。ステップＳ０１における候補リンクの抽出処理は、従前の方法を用いることができ、たとえば、推測航法により得られた地図上の現在位置である推測座標から所定の範囲内にある道路リンクを候補リンクとして抽出することができる。その他の処理については後で具体的に説明する。
【００２９】
次に図５のステップＳ０３とステップＳ０４において行なわれる候補リンク処理について説明する。候補リンク処理は、推測座標Ｑ（ｎ）の道路リンク上の対応点Ｐ（ｎ）を道路リンク上で前後にずらした近傍点において上述の類似度を求め、その最小値を相関度として求める処理である。ここで、相関度について説明する。
【００３０】
図６は、推測座標Ｑ（−１）〜Ｑ（７）のそれぞれの道路リンク上の対応点Ｐ（−１）〜Ｐ（７）を、道路リンク上で前方向にずらした近傍点Ｐ（−１）′〜Ｐ（７）′を示す図である。ここでは、対応点Ｐ（−１）〜Ｐ（７）をそれぞれ前方向にＬだけずらした近傍点Ｐ（−１）′〜Ｐ（７）′を示している。したがって、対応点Ｐ（７）と近傍点Ｐ（７）′とは距離Ｌだけ離れている。同様に対応点Ｐ（−１）と近傍点Ｐ（−１）′とは距離Ｌだけ離れている。
【００３１】
先の類似度の説明では、推測座標Ｑ（７）に対応する道路リンク上の対応点Ｐ（７）における類似度を求めた。Ｐ（７）を前方向に距離Ｌだけずらした近傍点Ｐ（７）′においても、Ｐ（７）における類似度を求めたのと同様にして、近傍点Ｐ（７）′における類似度を求めることができる。このとき、対応点Ｐ（７）の類似度を求める場合に、求めたベクトルＴ₀は、対応点Ｐ（５）を始点とし対応点Ｐ（７）を終点とするベクトルとして表わしたが、近傍点Ｐ（７）′における類似度を求める場合には、近傍点Ｐ（５）′を始点とし近傍点Ｐ（７）′を終点とするベクトルＴ₀′となる。近傍点Ｐ（７）′における類似度は、推測座標Ｑ（１）からＱ（７）までの走行軌跡と道路リンク上の近傍点Ｐ（１）′からＰ（７）′とが類似している度合いを示す。
【００３２】
推測座標の道路リンク上の対応点を前後にずらした近傍点を複数設け、それらの近傍点ごとに類似度を求め、求めた類似度のうち最小となる類似度を相関度として決定し、類似度が最小となる場合の近傍点と対応点との距離（ずらし量）を求める処理が候補リンク処理である。
【００３３】
図７は、候補リンク処理の流れを示すフロー図である。図を参照して、まず走行軌跡上の推測座標Ｑ（ｎ）が、ＲＡＭ１０７より取得される（ステップＳ１０）。ここでは、図２における推測座標Ｑ（７）が取得された場合について説明する。次に、推測座標Ｑ（７）に対応する道路リンク上の対応点Ｐ（７）が取得される（ステップＳ１１）。対応点Ｐ（７）は、前回の位置検出処理において特定された道路リンクＡＢ上の対応点Ｐ（０）から１４［ｍ］離れた点として取得される。
【００３４】
そして変数ｉに−３が代入される（ステップＳ１２）。変数ｉは、対応点Ｐ（７）を道路リンク上でずらす量を定める変数であり、ステップＳ１５においてインクリメントされ、ステップＳ１６において３以下か否かが判断されることにより、ステップＳ１３とステップＳ１４の処理が、変数ｉを−３，−２，−１，０，１，２，３のそれぞれの値について処理するようにしている。ステップＳ１３においては、対応点Ｐ（７）が定数Ｌ［ｍ］に変数ｉを乗じた値だけ道路リンク上でずらす処理が行なわれる。変数ｉの値がマイナスの場合には対応点Ｐ（７）は後方向にずらされ、変数ｉが正の場合には道路リンク上で前方向にずらされる。定数Ｌは、本実施の形態においては説明を簡単にするため２［ｍ］としているが、これに限定されるわけではない。
【００３５】
ステップＳ１４においては、対応点Ｐ（７）を前後にずらしたそれぞれの近傍点において、ベクトルＶとベクトルＴとのなす角の平均値Ａ（ｉ）と分散値Ｓ（ｉ）とが計算される。この処理については後で具体的に説明する。
【００３６】
ステップＳ１２からステップＳ１６の間で、対応点Ｐ（７）における類似度と、対応点Ｐ（７）を前方向と後方向にそれぞれＬ，２Ｌ，３Ｌずらした６つの近傍点における類似度が求められる。
【００３７】
ステップＳ１７においては、求められた類似度のうち最小の類似度が相関度とされる。そのときの変数ｉの値、すなわちＬ×ｉで求められるずらし量も併せて求められる。ただし、前方向および後方向にそれぞれずらしたずらし量が最も多い近傍点における類似度が最小となる場合（ｉ＝３ｏｒ−３）には、相関度は求めることができず、そのまま処理を終了する。これについては後で具体的に説明する。
【００３８】
図８は、図７のステップＳ１４において行なわれるベクトルＶとベクトルＴのなす角の平均値と分散値とを求める処理の流れを示すフロー図である。図を参照して、まず変数ｒに０が代入される。変数ｒは、推測座標から求められるベクトルＶと対応点から求められるベクトルＴのそれぞれのベクトルの数を定めるもので、ステップＳ２５においてインクリメントされ、ステップＳ２６において変数ｒが６以下の場合、ステップＳ２１からステップＳ２４までの処理が繰返される。本実施の形態においては、ベクトルＶとベクトルＴとをそれぞれ７個求めてそれぞれのベクトルのなす角の分散と平均を求めるようにしている。
【００３９】
ステップＳ２１において、推測座標Ｑ（ｎ−ｒ）とそれより２つ前に推測された推測座標Ｑ（ｎ−ｒ−２）とが求められ、推測座標Ｑ（ｎ−ｒ）に対応する道路リンク上の対応点Ｐ（ｎ−ｒ）と推測座標Ｑ（ｎ−ｒ−２）に対応する道路リンク上の対応点Ｐ（ｎ−ｒ−２）とが求められる。そしてステップＳ２２において、Ｑ（ｎ−ｒ−２）を始点としＱ（ｎ−ｒ）を終点とするベクトルＶｒが求められ、ステップＳ２３において、対応点Ｐ（ｎ−ｒ−２）を始点とし対応点Ｐ（ｎ−ｒ）を終点とするベクトルＴｒが求められる。次にステップＳ２４において、ＶｒとＴｒとのなす角θｒが求められる。ステップＳ２１からステップＳ２４までの処理を変数ｒが０〜６まで行なわれることにより、図３に示すベクトルＶ₀〜Ｖ₆とベクトルＴ₀〜Ｔ₆とそれぞれのベクトルのなす角θ₀〜θ₆とが求められる。
【００４０】
次にステップＳ２７において、ステップＳ２０からステップＳ２６までに求められたベクトルＶとベクトルＴとのなす角θ₀〜θ₆の平均値Ａｉが次の式（１）に従って求められ、分散値Ｓｉが式（２）に従って求められる。
【００４１】
【数１】

ただし、ｉは、図７に示す変数ｉであり、対応点Ｐを道路リンク上で前後にずらした量を示す。
【００４２】
なお、図８においては、説明の便宜上、対応点Ｐ（ｎ）を道路リンク上で前後にずらした近傍点をＰ（ｎ）として表わしている。
【００４３】
以上説明した候補リンク処理を図２に示す走行軌跡と道路リンクに対して行なった結果の一例を図９に示す。
【００４４】
図９は、横軸に対応点と近傍点の位置を示し、縦軸に対応点と近傍点のそれぞれにおける分散値Ｓｉを示す。対応点における分散値はＳ₀として表わされている。また、「前１（ｉ＝１）」は、対応点を前方向にＬずらした近傍点であり、分散値はＳ₁で表わされる。後１〜後３の近傍点と前１〜前３の近傍点における分散値Ｓｉが示されている。これらの分散値Ｓ_-3〜Ｓ₃は、対応点Ｐ（７）とその近傍点における類似度として表わされ、その類似度の最小値Ｓ₂が、相関度とされる（図７のステップＳ１７）。
【００４５】
ただし、相関度が導き出されるのは、算出された類似度のうち最小となる類似度が、その前後においてそれより大となる類似度を有する場合に限られる。たとえば、後３と前３の近傍点における類似度が最小となる場合には、最小となる類似度は相関度とされない。そのような場合には道路リンクと走行軌跡との類似度を判断することができないからである。
【００４６】
ここで図５に戻って、ステップＳ０２において候補リンクが複数ある場合には、ステップＳ０４の候補リンク処理がすべての候補リンクについて行なわれる。すなわち、すべての候補リンクについて相関度が求められ、ステップＳ０６において、相関度が最も小さい値の候補リンクが特定される。相関度が小さいということは、道路リンクと走行軌跡の類似する度合いが高いことを意味するからである。
【００４７】
ステップＳ０７では、道路リンク位置の決定がなされるが、これは、相関度が示す分散値Ｓｉとなる対応点または近傍点に道路リンク上の位置が決定されるものである。たとえば、候補リンク処理の結果が図９に示す場合であるとき、相関度を示す分散値はＳ₂であり、そのときの近傍点は前２（ｉ＝２）である。したがって、推測座標Ｑ（７）に対応する道路リンク上の位置は、道路リンク上の近傍点である前２（ｉ＝２）の位置に特定される。
【００４８】
ステップＳ０８においては、推測航法の方位が補正される。これについて具体的に説明する。上述したとおり、図２に示す走行軌跡と道路リンクとに候補リンク処理を施すと、図９に示す結果となる。このときの相関度はＳ₂であり、相関度がＳ₂となるときのベクトルＶとベクトルＴとのなす角の平均値はＡ₂である。この平均値Ａ₂は、方位の回転を示すものである。したがって、これを用いて方位センサの出力を補正することができる。その補正方法としては、方位センサの出力をθとしたとき、補正後の方位は、
θ−Ａ₂
として表わされる。ただし、θは、東方向を０とし、時計回りを正とする角度で表わされ、平均値Ａ₂は、ベクトルＶを時計回りに回転させてベクトルＴと重なるまでの回転角度で表わされる。
【００４９】
以上説明したとおり、本実施の形態における位置検出装置は、所定の区間における走行軌跡とそれに対応する道路リンクとの類似度を、道路リンク上の対応点とその対応点をずらした近傍点に基づいてそれぞれ類似度を求め、求められた類似度が最小となるときのずれ量をもとに道路リンク上の位置を特定するので、道路リンク上の位置を正確に求めることができる。
【００５０】
また、走行軌跡と道路リンクとの類似度を、走行軌跡中の２点を結ぶベクトルとその２点に対応する道路リンク上の２点を結ぶベクトルとのなす角に基づき求めるので、道路リンクの有する方位の誤差、たとえば、地図座標の分解能に基づく道路リンクの方位誤差による影響を少なくした類似度を求めることができる。
【００５１】
さらに、候補リンクが複数ある場合であっても、相関度が最も小さい道路リンクを選択するので、道路リンク上の前後位置のずれを考慮した相関度に基づき道路リンクを選択することができる。したがって、道路リンクの選択の精度を向上させることができる。
【００５２】
さらに、類似度が最小となるときのベクトルＶとベクトルＴのなす角の平均値に基づいて方位センサの方位を補正するので、累積された方位誤差を小さくすることができる。
【００５３】
［第２の実施の形態］
第２の実施の形態における位置検出装置は、第１の実施の形態における候補リンク処理に改良に加えて位置検出精度を向上させたものである。その他の点については第１の実施の形態における位置検出装置と同様であるので、ここでの説明は繰返さない。
【００５４】
図７を参照して、第１の実施の形態における位置検出装置は、ステップＳ１７において、ステップＳ１３からステップＳ１６までの処理で求められた分散値Ｓｉのうち最小となる分散値を相関度としたが、第２の実施の形態における位置検出装置は、近傍点と対応点との距離と分散値に対して２次曲線を近似し、この２次曲線の最小点を求め、最小点が示す分散値を相関度とし、最小点が示す対応点との距離に基づき道路リンク上の位置を特定するものである。
【００５５】
図１０は、図７に示すステップＳ１３〜ステップＳ１６において求められた、対応点および近傍点における分散値をプロットした一例を示す図である。プロットした点Ｓ_-3〜Ｓ₃までの点に対して次の式（３）で表される２次関数を近似する。
【００５６】
【数２】

この２次関数は、式（４）に示す評価関数を式（５）（６）（７）を用いて解くことにより求められる。
【００５７】
【数３】

ここで、Ｘ_iは対応点と近傍点との距離を示し、Ｙ_iは、分散値を示す。式（５）（６）（７）に対して、Ｓ_-3〜Ｓ₃までの７つの点の値を代入することにより、変数ａ，ｂ，ｃを求めることができる。
【００５８】
このようにして求めた関数ｆ（ｘ）の最小値を求めることにより、相関度と対応点からの距離（ずれ量）を求めることができる。対応点からの距離（ずれ量）は、−ｂ／（２ａ）から求められ、この値を式（３）に代入すれば相関度を求めることができる。
【００５９】
以上説明したとおり、第２の実施の形態における位置検出装置は、対応点と近傍点との距離と分散値とに２次関数を近似してその最小値を求めることにより、相関度と対応点からの距離（ずれ量）をもとに道路リンク上の位置を特定するので、より正確に道路リンク上の位置を特定することができる。
【００６０】
なお、今回開示された実施の形態は全ての点で例示であって、制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【図面の簡単な説明】
【図１】第１の実施の形態における位置検出装置の構成を示すブロック図である。
【図２】道路リンクと走行軌跡とを示す図である。
【図３】類似度を求める際に用いられるベクトルを示す図である。
【図４】ベクトルＶとベクトルＴとのなす角を示す図である。
【図５】位置検出処理の流れを示すメインフロー図である。
【図６】対応点を道路リンク上でずらした近傍点を示す図である。
【図７】候補リンク処理の流れを示すフロー図である。
【図８】ベクトルのなす角の平均値と分散値との計算処理の流れを示すフロー図である。
【図９】候補リンク処理の結果の一例を示す図である。
【図１０】第２の実施の形態における候補リンク処理の結果の一例を示す図である。
【符号の説明】
１０１ＣＰＵ
１０３ＣＤ−ＲＯＭ
１０７ＲＡＭ
１０９ディスプレイ
１１３ＧＰＳ
１１５ジャイロセンサ
１１７測距センサ[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a position detection device, and more particularly to a position detection device that detects a position of map data on a road link based on a travel locus obtained by dead reckoning navigation.
[0002]
[Prior art]
As a device for detecting the position of a vehicle traveling on a road map from the past, based on the output of a distance sensor and a direction sensor, the distance change amount and the direction change amount that accompany the traveling of the vehicle are integrated and the vehicle A position detection apparatus using dead reckoning navigation that obtains a position on a road map is known. This position detection device has a problem in that errors that the distance sensor and the direction sensor inevitably have are accumulated as travel continues and errors included in the position of the vehicle on the road map are also accumulated. .
[0003]
In consideration of such a problem, Japanese Patent Application Laid-Open No. 60-45285 discloses a moving object based on the traveling locus data of the moving object stored in the moving locus recording apparatus and the data of each road on the map. Means for searching for a candidate road on a map within a predetermined range from the current position and almost coincident with the direction of the travel locus, and comparing the searched candidate road data with the data of the travel locus Is used to select the candidate road with the highest similarity, and the display position is corrected so that the current position of the moving object is displayed on the selected candidate road. There is disclosed a traveling position display device for a moving body including means for performing the operation.
[0004]
[Problems to be solved by the invention]
However, the map data that is a reference for displaying the current position of the vehicle by the conventional position display device is composed of node data that is coordinate data on the map and data of connection relations between the nodes. Had an error associated with the resolution of the map coordinates. For example, when the node A in the map data and the node B having a connection relationship with the node A are found, the direction indicated by the road link AB connecting the node A and the node B is actually due to an error associated with the resolution of the map coordinates. This is different from the direction indicated by the road, and this appears as an error in direction. This azimuth error increases as the road link AB is shorter. Therefore, there is a difference between the direction obtained from the direction sensor of the vehicle traveling on the actual road and the direction indicated by the road link in the map data, and the similarity between the travel locus and the road link can be accurately obtained. I could not.
[0005]
Further, the position detection device disclosed in Japanese Patent Laid-Open No. 60-45285 can detect the road link to which the vehicle belongs, but cannot accurately determine where the vehicle is located on the detected road link. The vehicle's current position could not be accurately displayed.
[0006]
The present invention has been made to solve the above-described problems, and is a position detection device that can reduce the displacement of the position of the vehicle on the road link and can determine the exact position on the road link where the vehicle is located. The purpose is to provide. Furthermore, an object of the present invention is to provide a position detection device capable of reducing the accumulated azimuth error.
[0007]
[Means for Solving the Problems]
A position detection device according to an aspect of the present invention includes a distance detection unit that detects a travel distance, an orientation detection unit that detects a travel direction, and an estimated coordinate where the device is located based on outputs from the distance detection unit and the direction detection unit. Means for sequentially detecting at a predetermined timing and obtaining an estimated point sequence; and a point sequence on the road link obtained from the map data based on the output of the distance detecting unit, and a plurality of corresponding point sequences corresponding to the estimated point sequence A degree of similarity, a degree of similarity between the travel trajectory obtained from the estimated point sequence and the road link, and a degree of similarity calculating means for obtaining a plurality of similarities calculated based on each of the plurality of corresponding point sequences; A specifying unit that specifies a position on the road link based on the corresponding point sequence when the similarity calculated by the similarity calculating unit is minimum among the corresponding point sequence.
[0008]
According to the present invention, the similarity between the travel locus and the road link is obtained by shifting the position on the road link where the vehicle is located, and the position of the vehicle is specified based on the position on the road link when the similarity is minimum. Therefore, it is possible to provide a position detection device that can reduce the displacement of the position on the road link of the vehicle and obtain an accurate position.
[0009]
Preferably, the specifying means of the position detection device derives a multidimensional equation from the position and similarity of the plurality of corresponding point sequences on the road link, and has a point when the similarity of the derived multidimensional equation is minimized. And a position on the road link is specified.
[0010]
According to the present invention, since the position on the road link is specified based on the distance on the road link between the corresponding point and the neighboring point when the similarity of the multidimensional equation is minimum, the position on the road link is more accurately detected. The position can be specified.
[0011]
More preferably, in the case where there are a plurality of road links, the position detection device obtains a similarity for each link, and the specifying means obtains a correlation that is a minimum similarity for each link. And a selection means for selecting a road link having a minimum degree of correlation among the plurality of road links.
[0012]
According to the present invention, when there are a plurality of road links, the road link having the minimum correlation is selected, so that the position detection device capable of selecting the road link in consideration of the position shift on the road link of the vehicle. Can be provided.
[0013]
More preferably, the similarity calculation means of the position detection device includes a first vector having a first estimated coordinate in the estimated point sequence as an end point and a second estimated coordinate before the first estimated coordinate as a start point; The angle formed by the second vector starting from a point corresponding to the first estimated coordinate and starting from the point corresponding to the second estimated coordinate in the corresponding point sequence is defined as an angle difference of the first estimated coordinate. The similarity is calculated based on the angle difference between the estimated coordinates.
[0014]
According to the present invention, the similarity between the travel track and the road link is obtained based on the angle formed by the vector connecting the two points on the travel track and the vector connecting the two points on the road link. It is possible to reduce the influence due to the decrease in similarity. As a result, it is possible to provide a position detection device capable of accurately specifying a position on a road link and selecting a road link.
[0015]
According to another aspect of the present invention, there is provided a position detection apparatus comprising: a distance detection unit that detects a travel distance; an orientation detection unit that detects a travel direction; and an estimated coordinate where the device is located based on outputs of the distance detection unit and the direction detection unit Is a sequence of points on the road link obtained from the map data based on the output of the distance detection unit, and a plurality of corresponding point sequences corresponding to the estimated point sequence , A first vector having the first estimated coordinate in the estimated point sequence as the end point and the second estimated coordinate preceding the first estimated coordinate as the starting point, and the first estimated in the corresponding point sequence The angle formed by the second vector starting from the point corresponding to the coordinate and the point corresponding to the second estimated coordinate as the starting point is the angle difference of the first estimated coordinate, and the angle difference is calculated from the angle difference of the plurality of estimated coordinates. The average value and the similarity are calculated for multiple corresponding point sequences. It comprises an angle difference calculating means for calculating, based on respectively, and correction means for similarity to correct the output of the direction detection means on the basis of the average value at the minimum.
[0016]
According to the present invention, since the output of the azimuth detecting means is corrected based on the average value of the angle difference when the similarity is minimized, it is possible to provide a position detecting device that reduces the accumulated azimuth error.
[0017]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
[0018]
[First Embodiment]
FIG. 1 is a block diagram showing a configuration of a position detection device according to one embodiment of the present invention. Referring to the figure, the position detection apparatus includes a CPU 101 that controls the entire apparatus, a CD-ROM 103 that stores map data composed of a plurality of nodes and node connections, a ROM 105 that stores programs, and the like. A RAM 107 for storing various constants for detection operation, a display 109 for displaying a map, an input device 111 for inputting information such as a destination from a user, and a current position of the vehicle by receiving radio waves from a satellite. A GPS (global positioning system) device 113 for identifying, a gyro sensor 115 for detecting the direction of the vehicle, and a distance measuring sensor 117 for detecting the travel distance of the vehicle are included.
[0019]
First, the degree of similarity between the travel locus and the road link used in the position detection process performed by the position detection device will be described. FIG. 2 is a diagram illustrating road links and travel trajectories. The road link is derived from map data stored in the CD-ROM 103. Map data consists of the coordinates of the nodes and the connection relationship between the nodes. In the map data shown in FIG. 2, node A and node B have a connection relationship, and node B and node C have a connection relationship. As a result, a road link AB connecting the node A and the node B and a road link BC connecting the node B and the node C are derived. The travel trajectory is the estimated coordinates (Q () on the road map obtained every time the vehicle travels 2 [m] based on the azimuth that is the output of the gyro sensor 115 and the travel distance that is the output of the distance measuring sensor 117. -1) to Q (7)). The estimated coordinates are stored in the RAM 107.
[0020]
FIG. 2 represents the estimated coordinates in the previous position detection process as Q (0) and the coordinates on the road link specified by the position detection process as P (0). Then, the estimated coordinate when the vehicle has advanced 2 [m] (first time) since the previous position detection process is Q (1), and the estimated coordinate when 4 m has advanced (second time) is Q (2 ), And in the same manner, it represents up to the estimated coordinate Q (7) when the vehicle advances 14 [m] (seventh time). Here, the similarity used for performing the position detection process when the vehicle comes to the estimated coordinate Q (7) will be described.
[0021]
The point P (7) on the road link BC is a corresponding point of the estimated coordinate Q (7), and 14 [m] based on the point P (0) on the road link AB specified in the previous position detection process. It is required as a point that is only separated. A point P (6) on the road link BC is a corresponding point of the estimated coordinate Q (6), and is located at a distance of 12 [m] from the point P (0) obtained in the previous position detection process. It is required as a point to do. Similarly, points P (1) to P (5) and P (-1) on the road link are obtained. Note that Q (−1) is an estimated coordinate obtained one time before the previous position detection process.
[0022]
The group of estimated coordinates Q (-1) to Q (7) obtained in this way is the estimated point string, and the group of points P (-1) to P (7) on the road link is the corresponding point string. is there.
[0023]
FIG. 3 is a diagram showing a vector used when obtaining the similarity at the point P (7). Referring to the figure, vector V is a vector having one estimated coordinate as an end point and an estimated coordinate estimated two times before the estimated coordinate as a start point. For example, the vector V₀Is a vector starting from the estimated coordinate Q (5) and ending at Q (7). In this way, the vector V is a vector V whose end points are estimated coordinates Q (7) to Q (1), respectively.₀~ V₆Is required.
[0024]
The vector T is a vector having a corresponding point on the road link as an end point, and a point starting from the corresponding point on the road link by a distance on the travel locus between the start point and the end point of the vector V. For example, vector T₀Takes the point P (7) as the end point and the vector V₀This is a vector whose starting point is the distance on the travel locus between the starting point and the ending point of, that is, the point P (5) that goes back on the 4 [m] road link. A vector T with corresponding points P (7) to P (1) on the road links as end points₀~ T₆Is required.
[0025]
FIG. 4 is a diagram illustrating an angle formed by the vector V and the vector T. FIG. 4A shows the vector V₀And vector T₀And the angle θ between these vectors₀Is 0. FIG. 4B shows the vector V₁And vector T₁And the angle between those vectors is θ₁It is. FIG. 4C shows the vector V₂And vector T₂The angle between these vectors is θ₂It is. FIG. 4D shows the vector V_Three~ V₆And vector T_Three~ T₆And the angle θ between those vectors_Three~ Θ₆Are each 0.
[0026]
The angle θ formed by the vector V and the vector T thus obtained₀~ Θ₆The average value and the variance value can be obtained. The obtained variance value is the similarity at the corresponding point P (7) on the road link. The degree of similarity at the corresponding point P (7) is similar between the travel locus from the estimated coordinates Q (1) to Q (7) and the road link from the points P (1) to P (7) of the road link. It shows the degree of being.
[0027]
In the present embodiment, the vector V is a vector having one estimated coordinate as an end point and an estimated coordinate estimated two times before the estimated coordinate as a start point. However, the present invention is not limited to this. It is good also as a vector which makes the starting point the presumed coordinate estimated three times or before. Also, the degree of similarity is V₀~ V₆And T₀~ T₆The angle θ between each of the seven vectors₀~ Θ₆The number of vectors may be larger or smaller than this. Furthermore, the similarity may be an average value or a total sum instead of a variance value.
[0028]
Next, position detection processing performed by the CPU 101 of the position detection device will be described. FIG. 5 is a main flowchart showing a flow of position detection processing performed by the CPU 101. Referring to the figure, the position detection processing includes step S01 for extracting candidate links, step S03 for performing candidate link processing for one candidate link when there is one extracted candidate link (NO in step S02), and If there are a plurality of extracted candidate links (YES in step S02), candidate link processing is performed for each candidate link (steps S04, S05), and one candidate link is specified using the result of the candidate link processing. Step S06, Step S07 for determining the position on the road link using the result of the candidate link process (Step S03 or Step S04), and Step S08 for correcting the direction of dead reckoning navigation are included. For the candidate link extraction processing in step S01, a conventional method can be used. For example, a road link within a predetermined range is extracted as a candidate link from the estimated coordinates which are the current position on the map obtained by dead reckoning navigation. can do. Other processing will be specifically described later.
[0029]
Next, the candidate link process performed in step S03 and step S04 in FIG. 5 will be described. The candidate link process is a process for obtaining the above-mentioned similarity at a neighboring point obtained by shifting the corresponding point P (n) on the road link of the estimated coordinate Q (n) back and forth on the road link, and obtaining the minimum value as the correlation degree. It is. Here, the correlation degree will be described.
[0030]
FIG. 6 shows neighboring points P () in which the corresponding points P (-1) to P (7) on the road links of the estimated coordinates Q (-1) to Q (7) are shifted forward on the road links. -1) 'to P (7)'. Here, neighboring points P (−1) ′ to P (7) ′ are shown by shifting the corresponding points P (−1) to P (7) by L in the forward direction. Therefore, the corresponding point P (7) and the neighboring point P (7) ′ are separated by a distance L. Similarly, the corresponding point P (−1) and the neighboring point P (−1) ′ are separated by a distance L.
[0031]
In the above description of the similarity, the similarity at the corresponding point P (7) on the road link corresponding to the estimated coordinate Q (7) is obtained. At the neighboring point P (7) ′ obtained by shifting P (7) by the distance L in the forward direction, the similarity degree at the neighboring point P (7) ′ is calculated in the same manner as the similarity degree at P (7). Can be sought. At this time, when obtaining the similarity of the corresponding point P (7), the obtained vector T₀Is represented as a vector having the corresponding point P (5) as the starting point and the corresponding point P (7) as the ending point. However, in the case of obtaining the similarity at the neighboring point P (7) ′, the neighboring point P (5) ′ Is a vector T starting from a neighboring point P (7) ′₀It becomes ′. The degree of similarity at the neighboring point P (7) ′ is similar between the traveling locus from the estimated coordinates Q (1) to Q (7) and the neighboring points P (1) ′ to P (7) ′ on the road link. It shows the degree.
[0032]
Establish multiple neighboring points by shifting corresponding points on the road link of the estimated coordinates back and forth, calculate the similarity for each neighboring point, determine the minimum similarity among the calculated similarities as the correlation, The process of obtaining the distance (shift amount) between the neighboring point and the corresponding point when the degree is minimum is the candidate link process.
[0033]
FIG. 7 is a flowchart showing the flow of candidate link processing. Referring to the figure, first, estimated coordinates Q (n) on the travel locus are acquired from RAM 107 (step S10). Here, a case where the estimated coordinate Q (7) in FIG. 2 is acquired will be described. Next, the corresponding point P (7) on the road link corresponding to the estimated coordinate Q (7) is acquired (step S11). The corresponding point P (7) is acquired as a point 14 [m] away from the corresponding point P (0) on the road link AB specified in the previous position detection process.
[0034]
Then, -3 is substituted for the variable i (step S12). The variable i is a variable that determines the amount by which the corresponding point P (7) is shifted on the road link. The variable i is incremented in step S15, and in step S16, it is determined whether or not it is 3 or less. The processing processes the variable i for each value of -3, -2, -1, 0, 1, 2, 3. In step S13, the corresponding point P (7) is shifted on the road link by a value obtained by multiplying the constant L [m] by the variable i. When the value of the variable i is negative, the corresponding point P (7) is shifted backward, and when the variable i is positive, it is shifted forward on the road link. The constant L is set to 2 [m] in the present embodiment for the sake of simplicity, but is not limited to this.
[0035]
In step S14, the average value A (i) and the variance value S (i) of the angles formed by the vector V and the vector T are calculated at the respective neighboring points obtained by shifting the corresponding point P (7) back and forth. . This process will be specifically described later.
[0036]
Between step S12 and step S16, the similarity at the corresponding point P (7) and the similarity at six neighboring points obtained by shifting the corresponding point P (7) by L, 2L, and 3L in the forward and backward directions are obtained. It is done.
[0037]
In step S17, the minimum similarity among the obtained similarities is set as the correlation. The value of the variable i at that time, that is, the shift amount obtained by L × i is also obtained. However, when the degree of similarity at the nearest point with the largest amount of shift shifted forward and backward is minimum (i = 3 or −3), the correlation cannot be obtained, and the process ends. . This will be specifically described later.
[0038]
FIG. 8 is a flowchart showing a flow of processing for obtaining the average value and the variance value of the angles formed by the vector V and the vector T performed in step S14 of FIG. Referring to the figure, first, 0 is assigned to variable r. The variable r determines the number of vectors V and T obtained from the estimated coordinates, and is incremented in step S25. If the variable r is 6 or less in step S26, the variable r starts from step S21. The process up to step S24 is repeated. In this embodiment, seven vectors V and T are obtained, and the variance and average of the angles formed by the vectors are obtained.
[0039]
In step S21, the estimated coordinate Q (n−r) and the estimated coordinate Q (n−r−2) estimated two times before are obtained, and the road link corresponding to the estimated coordinate Q (n−r). An upper corresponding point P (n−r) and a corresponding point P (n−r−2) on the road link corresponding to the estimated coordinate Q (n−r−2) are obtained. In step S22, a vector Vr having Q (n−r−2) as a starting point and Q (n−r) as an end point is obtained. In step S23, a corresponding point P (n−r−2) is set as a starting point and corresponding. A vector Tr whose end point is the point P (n−r) is obtained. Next, in step S24, an angle θr formed by Vr and Tr is obtained. When the process from step S21 to step S24 is performed from 0 to 6 for the variable r, the vector V shown in FIG.₀~ V₆And vector T₀~ T₆And the angle θ between each vector₀~ Θ₆Is required.
[0040]
Next, in step S27, the angle θ formed by the vector V and the vector T obtained from step S20 to step S26.₀~ Θ₆Is obtained in accordance with the following equation (1), and the dispersion value Si is obtained in accordance with equation (2).
[0041]
[Expression 1]

However, i is the variable i shown in FIG. 7, and shows the amount by which the corresponding point P is shifted back and forth on the road link.
[0042]
In FIG. 8, for convenience of explanation, a neighboring point obtained by shifting the corresponding point P (n) back and forth on the road link is represented as P (n).
[0043]
FIG. 9 shows an example of a result obtained by performing the candidate link process described above on the travel locus and the road link shown in FIG.
[0044]
In FIG. 9, the horizontal axis indicates the positions of the corresponding points and the neighboring points, and the vertical axis indicates the variance value Si at each of the corresponding points and the neighboring points. The variance at the corresponding point is S₀It is expressed as Further, “front 1 (i = 1)” is a neighboring point obtained by shifting the corresponding point by L in the forward direction, and the variance value is S₁It is represented by Dispersion values Si at the rear points 1 to 3 and the front points 1 to 3 are shown. These variance values S_-3~ S_ThreeIs expressed as the similarity between the corresponding point P (7) and its neighboring points, and the minimum value S of the similarity₂Is the correlation (step S17 in FIG. 7).
[0045]
However, the degree of correlation is derived only when the smallest degree of similarity among the calculated degrees of similarity has a degree of similarity that is greater before and after that. For example, when the degree of similarity at the neighboring points of the rear 3 and the front 3 is minimum, the minimum similarity is not a correlation. This is because in such a case, the degree of similarity between the road link and the travel locus cannot be determined.
[0046]
Returning to FIG. 5, if there are a plurality of candidate links in step S02, the candidate link process in step S04 is performed for all candidate links. That is, the degree of correlation is obtained for all candidate links, and the candidate link having the smallest value of the degree of correlation is specified in step S06. This is because a small degree of correlation means that the degree of similarity between the road link and the travel locus is high.
[0047]
In step S07, the position of the road link is determined. In this case, the position on the road link is determined at a corresponding point or a neighboring point having the variance value Si indicated by the correlation. For example, when the result of the candidate link process is the case shown in FIG. 9, the variance value indicating the correlation is S₂The neighboring point at that time is front 2 (i = 2). Therefore, the position on the road link corresponding to the estimated coordinate Q (7) is specified as the previous 2 (i = 2) position, which is a neighboring point on the road link.
[0048]
In step S08, the direction of dead reckoning navigation is corrected. This will be specifically described. As described above, when candidate link processing is performed on the travel locus and road link shown in FIG. 2, the result shown in FIG. 9 is obtained. The degree of correlation at this time is S₂And the degree of correlation is S₂The average value of the angle formed by the vector V and the vector T is₂It is. This average value A₂Indicates the rotation of the bearing. Therefore, the output of the direction sensor can be corrected using this. As the correction method, when the output of the direction sensor is θ, the corrected direction is
θ-A₂
Is represented as However, θ is represented by an angle in which the east direction is 0 and the clockwise direction is positive, and the average value A₂Is represented by a rotation angle until the vector V is rotated clockwise and overlapped with the vector T.
[0049]
As described above, the position detection apparatus according to the present embodiment is based on the similarity between the travel locus in a predetermined section and the corresponding road link based on the neighboring points obtained by shifting the corresponding points on the road links and the corresponding points. Thus, the degree of similarity is obtained, and the position on the road link is specified based on the amount of deviation when the obtained degree of similarity is minimum, so that the position on the road link can be obtained accurately.
[0050]
In addition, since the similarity between the travel locus and the road link is obtained based on the angle formed by the vector connecting the two points in the travel locus and the vector connecting the two points on the road link corresponding to the two points, It is possible to obtain a degree of similarity that is less affected by the direction error of the road link, for example, the road link direction error based on the resolution of the map coordinates.
[0051]
Furthermore, even when there are a plurality of candidate links, the road link with the smallest correlation is selected, so that the road link can be selected based on the degree of correlation in consideration of the shift of the front and rear positions on the road link. Therefore, the accuracy of selection of road links can be improved.
[0052]
Furthermore, since the azimuth | direction of an azimuth | direction sensor is correct | amended based on the average value of the angle which the vector V and the vector T when a similarity degree becomes the minimum, the accumulated azimuth | direction error can be made small.
[0053]
[Second Embodiment]
The position detection apparatus according to the second embodiment is obtained by improving the position detection accuracy in addition to the improvement of the candidate link process according to the first embodiment. Since other points are the same as those of the position detection device in the first embodiment, description thereof will not be repeated here.
[0054]
Referring to FIG. 7, in step S17, the position detection apparatus in the first embodiment uses the smallest variance value among the variance values Si obtained in the processing from step S13 to step S16 as the degree of correlation. However, the position detection apparatus in the second embodiment approximates a quadratic curve to the distance between the neighboring points and the corresponding points and the variance value, obtains the minimum point of the quadratic curve, and the variance indicated by the minimum point The value is used as the degree of correlation, and the position on the road link is specified based on the distance from the corresponding point indicated by the minimum point.
[0055]
FIG. 10 is a diagram illustrating an example in which the variance values at corresponding points and neighboring points obtained in steps S13 to S16 illustrated in FIG. 7 are plotted. Plotted point S_-3~ S_ThreeA quadratic function represented by the following expression (3) is approximated to the points up to:
[0056]
[Expression 2]

This quadratic function is obtained by solving the evaluation function shown in Expression (4) using Expressions (5), (6), and (7).
[0057]
[Equation 3]

Where X_iIndicates the distance between the corresponding point and the neighboring point, and Y_iIndicates a variance value. For equations (5), (6) and (7), S_-3~ S_ThreeBy substituting the values of up to seven points, the variables a, b, and c can be obtained.
[0058]
By obtaining the minimum value of the function f (x) obtained in this way, the correlation degree and the distance (deviation amount) from the corresponding point can be obtained. The distance from the corresponding point (deviation amount) is obtained from -b / (2a), and the degree of correlation can be obtained by substituting this value into equation (3).
[0059]
As described above, the position detection apparatus according to the second embodiment approximates the quadratic function to the distance between the corresponding point and the neighboring point and the variance value to obtain the minimum value, thereby obtaining the correlation degree and the corresponding point. Since the position on the road link is specified based on the distance (shift amount) from the road, the position on the road link can be specified more accurately.
[0060]
It should be understood that the embodiment disclosed this time is illustrative in all respects and not restrictive. The scope of the present invention is defined by the terms of the claims, rather than the description above, and is intended to include any modifications within the scope and meaning equivalent to the terms of the claims.
[Brief description of the drawings]
FIG. 1 is a block diagram illustrating a configuration of a position detection device according to a first embodiment.
FIG. 2 is a diagram showing road links and travel trajectories.
FIG. 3 is a diagram showing vectors used when obtaining similarity.
FIG. 4 is a diagram illustrating an angle formed by a vector V and a vector T;
FIG. 5 is a main flowchart showing a flow of position detection processing.
FIG. 6 is a diagram illustrating neighboring points obtained by shifting corresponding points on a road link.
FIG. 7 is a flowchart showing a flow of candidate link processing.
FIG. 8 is a flowchart showing a flow of calculation processing of an average value and a variance value of angles formed by vectors.
FIG. 9 is a diagram illustrating an example of a result of candidate link processing;
FIG. 10 is a diagram illustrating an example of a result of candidate link processing according to the second embodiment.
[Explanation of symbols]
101 CPU
103 CD-ROM
107 RAM
109 display
113 GPS
115 Gyro sensor
117 Ranging sensor

Claims

A distance detecting means for detecting a travel distance;
Azimuth detecting means for detecting a traveling azimuth;
Means for sequentially detecting the estimated coordinates where the self is located based on the outputs of the distance detecting means and the azimuth detecting means at a predetermined timing, and obtaining an estimated point sequence composed of a plurality of estimated points ;
A plurality of points corresponding to the estimated point sequence, each of which is a point sequence on a road link obtained from map data based on the output of the distance detecting means, each of which is composed of corresponding points respectively corresponding to the plurality of estimated points A means for obtaining a corresponding point sequence of
A first vector starting from a first estimated coordinate corresponding to the estimated point in the estimated point sequence and having a second estimated coordinate preceding the first estimated coordinate as a starting point; and each of the corresponding point sequences The angle formed by the second vector starting from the point corresponding to the first estimated coordinate and the point corresponding to the second estimated coordinate as the starting point is the angle difference of the first estimated coordinate, Angle difference calculating means for calculating an average value and a variance value of the angle difference of the first estimated coordinates at each estimated point ;
A specifying unit that specifies a position on the road link based on the corresponding point sequence when the variance value obtained by the angle difference calculating unit is minimum among the plurality of corresponding point sequences ;
A position detecting device comprising: a correcting unit that corrects an output of the azimuth detecting unit based on the average value when the variance value obtained by the angle difference calculating unit is minimum among the plurality of corresponding point sequences. .

The specifying means derives a multidimensional equation from the position of the plurality of corresponding point sequences on the road link and the variance value, and based on the point at which the value of the derived multidimensional equation is minimized The position detection device according to claim 1, wherein an upper position is specified.

The means for obtaining the plurality of corresponding point sequences, when there are a plurality of the road links, obtains a plurality of corresponding point sequences corresponding to each road link,
The angle difference calculation means calculates an average value and a variance value of the angle difference of the first estimated coordinates at each of the plurality of estimated points for the plurality of corresponding point sequences corresponding to each road link,
The specifying means obtains a correlation value with a minimum variance value for the plurality of corresponding point sequences corresponding to each link,
The position detection apparatus according to claim 1, further comprising a selection unit that selects a road link having a minimum correlation among the plurality of road links.