JP5821276B2 - 移動体の位置および進行方向の検出装置 - Google Patents

Description

本発明は、移動体の位置および進行方向の検出装置に関するものである。

ＧＰＳ(Global Positioning System）を用いたナビゲーションシステムにおいては、ＧＰＳの測位誤差等を考慮して、表示装置における地図上の道路上に自車両を表示するべく、特許文献１に示すように、走行軌跡と道路形状によりマップマッチングが行われている。

ところで、近時は、ナビゲーションシステムと移動体の走行制御等とを関連づけて移動体の運転支援を高める等のことが考えられており、このため移動体の位置情報に関してマップマッチングを行っている場合の誤差数ｍ程度の精度では足りず、より高精度な位置情報が求められるようになっている。

特許文献２には、絶対位置が既知とされた外部の固定対象物に対する自己の相対位置を検出手段により検出して、その検出結果に基づいてＧＰＳ検出に基づく自己の位置を補正するものが提案されている。

特開平６−１０２０５２号公報 特開２００７−２１８８４８号公報

ところで、移動体の位置情報として、移動体を上方から見た平面図上の位置つまり直交座標系によって示すことに加えて、方位つまり進行方向(自動車の場合においてヨー方向）を精度よく知ることが求められる。例えば、移動体としての自車両と他車両との衝突回避等や種々の運転支援等のために、移動体の進行方向を精度よく知ることが必要になる。そして、この進行方向の検出に際しては、複雑な演算を行うことなく極力簡単に行えるようにすることが望まれる。

本発明は以上のような事情を勘案してなされたもので、その目的は、移動体の現在位置および進行方向を、簡便な手法によって精度よく検出することができるようにした移動体の位置および進行方向の検出装置を提供することにある。

前記目的を達成するため、本発明にあっては次のような第１の解決手法を採択してある。すなわち、特許請求の範囲における請求項１に記載のように
移動体に設けられ、周囲の物体を検出する物体検出手段と、
道路周辺にある固定物体の位置情報を含めて記憶した３次元の地図データと、
前記地図データと前記物体検出手段により検出された検出データとを直交座標系におけるＸ軸およびＹ軸方向に相対変位させたときに一致点がもっとも多くなる状態において、該両データのＸ軸方向およびＹ軸方向のずれに基づいて移動体の現在位置の測位を行う現在位置決定手段と、
前記地図データと前記検出データとを極座標系における原点を中心にして相対回転させたときに一致点がもっとも多くなる状態の相対回転角度に基づいて、移動体の進行方向を決定する進行方向決定手段と、
を備えているようにしてある。上記解決手法によれば、極座標系における位置は、距離ｒと角度αとをパラメータとして(ｒ、α）として示されるが、角度αは、極座標系の原点に移動体が位置するものとしたとき、移動体の進行方向を示すことになるので、複雑な演算を用いることなく、移動体の進行方向を決定することができる。また、直交座標系を用いた位置決定についても、複雑な演算を行う必要がないので、制御系の負担軽減の上でも好ましいものとなる。

上記解決手法を前提とした好ましい態様は、特許請求の範囲における請求項２以下に記載のとおりである。

前記移動体の進行方向の決定後に、前記移動体の現在位置の測位が行われる、ようにしてある（請求項２対応）。この場合、進行方向を加味して直交座標系での移動体の位置を精度よく決定することができる。

前記移動体が直進移動しているときは、直交座標系を用いて該移動体の現在位置の測位と進行方向との決定が行われる、ようにしてある（請求項３対応）。この場合、移動体の直進時という進行方向に変化がないときは、直交座標系のみによって移動体の位置と進行方向とを決定して、制御系の負担を軽減する上で好ましいものとなる。

前記移動体の進行方向の決定は、所定エリアにおける前記地図データの線分方向と前記検出データの線分方向とを対比させることにより行われる、ようにしてある（請求項４対応）。この場合、例えば直線となるガードレールや建物等の直線部分を対比用の線分として利用して、進行方向を精度よく検出することができる。

前記検出データで得られる道路に沿って直線状に伸びる検出固定物体の位置情報と、該検出固定物体に対応して前記地図データに記憶されている直線状の記憶固定物体の位置情報とを、直交座標系においてＸ軸方向およびＹ軸方向に相対変位させて、該両位置情報同士がもっとも一致する相対変位量が直交座標系における移動体の位置ずれ量として決定され、
前記検出固定物体の位置情報と前記記憶固定物体の位置情報とを前記極座標系において相対回転させて、該両位置情報同士がもっとも一致する相対回転量が移動体の進行方向のずれ量として決定される、
ようにしてある（請求項５対応）。この場合、直交座標系を用いた移動体の位置ずれ量と、極座標系を用いた移動体の進行方向のずれ量とを、極力簡便な演算によって決定することができる。

前記移動体が自動車とされている、ようにしてある（請求項６対応）。この場合、ＧＰＳの利用度が極めて高くしかも衝突回避等の安全が要求される自動車について、種々の運転支援等の上で好ましいものとなる。

本発明によれば、移動体の位置と進行方向とを精度よく検出することができ、しかも進行方向の決定のための演算も極力簡単化されて制御系の負担も軽減できる。

本発明が適用された車両の簡略平面図。 本発明の制御系統例をブロック図的に示す図。 移動体としての車両の周辺にある固定物体の例を示す平面図。 図３に対応した固定物体に対応したボクセルデータを示す図。 図３の状態から、固定物体のうち道路側の部分の輪郭を抽出した状態を示す図。 車両に搭載された物体検出センサによって図３の固定物体を検出した状態を示す図。 図５の状態から複数の特徴点を抽出する様子を示す図。 図６の状態から、図５で抽出した特典部分に対応した特徴点を抽出する様子を示す図。 図７，図８で抽出された特徴点の位置情報を直交座標系で示す図。 直交座標系において、地図データでの抽出特徴点と検出データでの抽出特徴点とをＸ軸方向、Ｙ軸方向にずらして一致レベルをみるときの様子を示す図。 極座標系において、地図データでの抽出特徴点と検出データでの抽出特徴点とを原点を中心に相対回転させて、一致レベルをみるときの様子を示す図。 本発明の制御例を示すフローチャート。 本発明の制御例を示すフローチャート。 本発明の制御例を示すフローチャート。 本発明の制御例を示すフローチャート。 本発明の制御例を示すフローチャート。 本発明の制御例を示すフローチャート。

図１において、Ｖは、移動体としての車両(実施形態では自動車）である。車両Ｖは、左右４隅にそれぞれ、レーダ(例えばレーザレーダ）あるいはカメラの少なくとも一方からなる物体検出センサ１が装備されている。前２個の物体検出センサ１は、車両前方の物体の方位と相対位置を検出するものであり、後ろ２個の物体検出センサ１は車両後方の物体の方位と相対距離を検出するものである。車両Ｖは、前後左右の車輪の速度を検出する車輪速センサ２を装備している。各車輪速センサ２で検出された車輪速の平均値が車速とされる。車両Ｖは、ナビゲーション装置を有しており、ナビゲーション装のＧＰＳ(ＧＰＳセンサ）が符号３で示され、地図データベースが符号４で示され、表示画面が符号５で示される。コントローラＵによる制御によって、表示画面５に表示された地図上に、ＧＰＳ３で検出された車両Ｖの現在位置が表示される。車両Ｖはさらに、ジャイロ６，舵角センサ７を有する。

車両Ｖは、マイクロコンピュータを利用して構成されたコントローラ(制御ユニット）Ｕを有する。コントローラＵは、後述するように、車両Ｖの現在位置と進行方向とを精度よくマップマッチングする機能と、衝突回避のための運転支援等の制御を行う。このため、コントローラＵには、図２に示すように、前述の各種センサ等１〜７からの信号が入力される一方、警報装置８，車両制御装置９を制御するようになっている。コントローラＵは、物体検出センサ１で検出された物体が車両Ｖに接触する可能性があると判断したときに、警報装置８を作動させて、例えば音声によって運転者に警報を行う。また、コントローラＵは、物体検出センサ１によって、警報装置８が作動するよりもより近い距離にある物体を検出して衝突の危険性が極めて高いと判断したときに、車両制御装置９を作動させて、例えば自動操舵を行ったり自動ブレーキを行う等によって、衝突を回避する制御を行うようになっている。また、コントローラＵは、車車間通信によって、車両Ｖの現在位置と進行方向とを前方付近に存在する他車両に知らせて、他車両との衝突等を未然に防止するようになっている。さらに、コントローラＵは、車両Ｖが車線を逸脱して危険である場合に、自動操舵を行って車線逸脱を防止するようになっている。コントローラＵは、この他、例えば車両Ｖと、停止線位置、交差点位置、死角域のガードレールとの距離に応じて車両Ｖについての適正な制御を行う。なお、車両Ｖの位置に応じたコントローラＵによる各種の制御は種々想定されるが、位置検出そのものとは直接関係ないので、これ以上の説明は省略する。

車両Ｖの現在位置と進行方向とを精度よく検出するために、次のような制御が行われる。まず、車両Ｖの現在位置は、車両Ｖを上方から見た平面図上、つまり例えば前後方向をＸ軸、左右方向をＹ軸とする直交座標系で示される。また、車両Ｖの進行方向は、車両Ｖの左右方向(ヨー方向）となるものであって、例えば直交座標系における１次直線の傾きとして示すことができる。

地図データベース４には、三次元地図データとしてのボクセルデータが記憶されている。表示画面５上に表示される地図は、実際の地形を上方から見た平面図であり、地図上の各位置は実際の地形に応じて精度よく設定されている。また、ボクセルデータは、三次元空間を例えば一辺１ｍの立方体に分割したものであり、この分割された多数のボクセルには、ボクセルの中心位置の位置座や、ボクセル内に含まれる物体、例えば電柱、ガードレール、建物、電源ボックス等の正確な位置情報が位置データとして含まれる。なお、以下の説明では、三次元地図データを単に地図データと称する。そして、車両Ｖの位置決定等のために用いられる地図データ中の位置情報は、物体検出センサ１でのセンシング平面と同じ高さ位置でのデータが用いられることになる。

図３は、車両Ｖの前方にある実際の固定物体の一例が示される。この図３中、１１、１２，１３はガードレール、１４，１５、１６は電柱、１７、１８，１９は建物、２０は歩行者である。実際の状態を示す図３の位置に対応した地図データが、図４に示される（地図データであるため、歩行者は示されない）。この図４において、図３に対応した固定物体に対応した地図上の固定物体に付した符合に対して、符合「Ａ」を付記して示してある。例えば、図３のガードレール１１に対応した地図データ（ボクセルデータ）上のガードレールが、符合１１Ａで示される。なお、図４において、多数の四角形で示す個々の格子は、１つ１つのボクセルを示す（四角形の１辺が例えば１ｍ）。

図５は、図４の地図データ上の固定物体について、車両Ｖが走行している道路に沿った輪郭部分を抽出した状態が示される。この図５において、図４の固定物体に対応した固定物体には、図４で示す符合「Ａ」に代えて「Ｂ」の符合を用いて対応関係を示してある。

図６は、図３の状態において、物体検出センサ１によって検出された結果が示される。図３に示す固定物体に対応した検出物体について、図３で示す符合に対して「Ｃ」を付してある。物体検出センサ１は、図３に示す固定物体のうち、一部の固定物体のみを検出している状態となっており、しかも一部の固定物体についてはその一部分のみを検出している状態ともなっている。

図７〜図９は、後述するようにして車両Ｖの位置と方位（進行方向）とが決定された際に、位置と方位が精度よく決定されているか否かを判定する際に用いるデータの抽出手法を示してある。まず、図７は、図５の地図データ上から、複数の領域（特徴点）を抽出した状態が示される。この領域の抽出に際しては、車両Ｖに近い固定物体や、道路に沿って直線状の固定物体が選択され、さらに方位と距離とが互いに相違するように選択される。図７では、４つの領域（抽出部分）Ｘ１〜Ｘ４となっている。すなわち、領域Ｘ１は、ガードレール１１Ｂのうち車両Ｖに近い部分であり、領域Ｘ２は電柱１５Ｂであり、領域Ｘ３はガードレール１３Ｂのうち車両Ｖに近い部分であり、領域Ｘ４は建物１８Ｂのうち車両Ｖに近い部分である。

図８は、図７に対応して、物体検出センサ１２による検出データ上において、図７で抽出した領域と同一部分を領域として抽出した状態が示される。図７の領域に対応した部分に、図７で用いた「Ｘ」の代わりに「Ｙ」の符合を用いて示してある（例えばＹ１がＸ１に対応）。そして、図９は、図７，図８での抽出点同士を直交座標系でもって位置表示したものである。

図１０は、直交座標系において、位置を正確に求める手法を説明するための図である。図１０に示す黒塗り部分が、地図データと検出データとの比較対象である固定物体（の部分）であり、四角形の枠体が要求位置精度に対応したメッシュ（升目）である（例えば１０ｃｍ四方）。なお、図１０では、抽出された特徴部分（領域）としては、特に、道路に沿って直線状に長く伸びる（例えば５ｍ〜１０ｍ）固定物体（例えば建物やガードレール）を含むようにするのが好ましく、線分としての直線状であれば建物やガードレールの一部であってもよい。

地図データと検出データとの抽出された各特徴部分同士を、例えば地図データ上の特徴部分を固定しつつ、検出データ上の特徴部分をＸ軸方向およびＹ軸方向に１メッシュづつ移動させて、その一致度合が判定される。すなわち、地図データ上の抽出特徴部分に対して、検出データ上の抽出特徴部分をＸ軸方向およびＹ軸方向に１メッシュ移動させる毎に、その一致度合をみて、最終的に、もっとも一致度合が高い状態が、地図データと検出データとの直交座標系での位置ずれ量となる。このような手法によって、地図データと検出データとの位置ずれ量を簡便に知ることができる。この位置ずれ量に基づいて、地図データ上の車両Ｖの位置を補正することによって、直交座標系における車両Ｖの絶対位置が精度よく検出されることになる。

車両Ｖの進行方向（左右の向き）の検出は、極座標系を用いて行われる。すなわち、図１０に示す直交座標系が、図１１に示すように、車両Ｖ（の前端部）を原点とする極座標系に変換される（ｒが距離、αが角度つまり方位）。そして、この極座標系において、多数の枠体からなるメッシュは、距離が要求位置精度（例えば１０ｃｍ）とされ、回転方向に隣合うメッシュ同士の角度は進行方向についての要求精度（例えば１度）とされる。この場合も、例えば地図データを固定しつつ検出データを極座標系の原点を中心に１メッシュづつ相対回転させて、その一致度合をみて、最終的に、もっとも一致度合が高い状態が、地図データと検出データとの極座標系での進行方向のずれ量となる。

次に、コントローラＵの詳細な制御内容について、図１２以下のフローチャートを参照しつつ説明する。なお、以下の説明でＱはステップを示す。まず、Ｑ１において、車速と舵角とヨーレートから、車両Ｖの走行軌跡が演算される。次いでＱ２において、ＧＰＳ３での受信信号に基づいて、車両Ｖの現在位置と方位が検出される。Ｑ２の後、Ｑ３において、車両Ｂ周辺の道路構造情報が地図データベース４から読み込まれる。この後Ｑ４において、物体検出センサ１によって、車両Ｖ周辺の道路構造情報が検出される。

Ｑ４の後、Ｑ５において、ＧＰＳ３に基づく車両Ｖの位置が直交座標系として変換される（車両Ｖを上方から見た直交座標系に変換）。この後、Ｑ７において、Ｑ１で得た走行軌跡とＧＰＳ３で得られている位置情報とから、車両Ｖの運動が解析される。この車両Ｖの運動解析は、位置と進行方向との決定をどのような手法で行うかを区別するためのもので、この点については後述する。

Ｑ７の後、Ｑ８において、Ｑ７での解析結果に基づいて、車両Ｖが直線走行していて、かつ車両挙動が安定しているか否かが判別される。このＱ８の判別でＹＥＳのときは、Ｑ９において、後述するように、補間による位置検出処理が行われる。この後、Ｑ１０において、Ｑ９で検出された車両Ｖの位置と方位の精度の良否が判断される。この後、Ｑ１１において、Ｑ１０での精度判定の結果に基づいて、精度が不十分であるか否かが判別される。なお。Ｑ１０の詳細については後述する。

前記Ｑ１１の判別でＹＥＳのとき、つまりＱ９の処理では精度が不十分なときは、Ｑ１２において、道路構造が解析される。この後、Ｑ１３において、Ｑ１２での解析の結果、車両Ｖの進行方向に平行な直線状の構造物が存在するか否かが判別される。このＱ１３の判別でＹＥＳのときは、Ｑ１４において、高精度の方位検出処理が行われ、次いでＱ１５において、Ｑ１０と同様に、位置と方位の精度についての判定処理が行われる。この後、Ｑ１６において、Ｑ１５での判定処理の結果に基づいて、精度不十分であるか否かが判別される。

前記Ｑ１６の判別でＹＥＳのとき、つまり精度不十分な場合は、Ｑ１７において、周辺の道路構造物に形状的な特徴があるか否かが判別される。Ｑ１７の判別でＮＯのときは、Ｑ１８において、後述するように、舵角、ヨーレートおよび車速から、車両Ｖの位置と方位が推定される。また、Ｑ１７の判別でＹＥＳのときは、Ｑ１９において、高精度の位置検出処理が行われ、この後、Ｑ２０において、高精度の方位検出処理が行われる。なお、Ｑ１９，Ｑ２０の詳細については後述する。

図１２のＱ９の詳細が図１３に示される。すなわち、Ｑ３１において、車両Ｖの挙動（特に舵角）と走行距離とから、車両Ｖの位置、方位が推定される。この図１３の処理は、車両Ｖが直線走行していてかつ挙動が安定しているため、車両Ｖの位置の推定が容易かつ精度よく決定できるという想定に基づく処理となる。

図１２のＱ１９の詳細が、図１４に示される。すなわち、図１４は、前述した図１０で説明したように、直交座標系において、地図データと検出データとをＸ軸方向およびＹ軸方向に１メッシュづつ相対変位させて、両データ同士の一致度合のもっとも高い位置が車両Ｖの位置とされる処理となる。具体的には、Ｑ４１において、地図データと物体データ（検出データ）とが直交座標系における位置として変換される。次いで、Ｑ４２において、直交座標系が多数のメッシュデータに変換される。この後、Ｑ４３において、両データを１メッシュづつ移動させて、一致しているメッシュ数が最大となる位置が車両Ｖの位置とされる。つまり、メッシュ数が最大となる位置と地図データ上での位置とのずれが、ＧＰＳ３での検出誤差となるので、この誤差を用いて、地図データ上での車両Ｖの位置を精度よく決定することができる。

図１２のＱ１４の詳細が、図１５に示される。すなわち、図１５は、前述した図１１で説明したように、極座標系において、地図データと検出データとを極座標系の原点を中心に相対回転させて、もっとも一致するメッシュ数が最大となる位置での角度が方位として決定される。具体的には、Ｑ５１において、地図データ及び検出データ（物体データ）とが極座標系に変換される。次いでＱ５２において、各極座標系が多数のメッシュデータに変換される。この後、Ｑ５３において、両データを１メッシュづつ相対回転させて、一致しているメッシュ数が最大となる相対回転角度位置が方位として決定される。つまり、メッシュ数が最大となる位置と地図データ上での位置との角度のずれが、ＧＰＳ３での検出誤差となるので、この誤差を用いて、地図データ上での車両Ｖの方位を精度よく決定することができる。

図１２のＱ２０の詳細が図１６に示される。すなわち、Ｑ６１において、地図データおよび検出データから、車両Ｖの進行方向に平行な直線状の物体（例えば建物やガードレール）を抽出して、この抽出された各直線状物体に対する車両Ｖの傾きが演算される。次いでＱ６２において、地図データと検出データにおいて、車両Ｖの近傍にある直線状物体に対する傾きの差分が検出される。この後、Ｑ６３において、Ｑ６２で検出された傾きの差分の絶対値を加算した値を最小化して、もっとも一致する位置を検出する。この図１６の処理は、つまるところ、車両Ｖの進行方向に沿って平行に直線状の伸びる複数の固定物体について、地図データ上での固定物体に対する傾きと検出データ上での固定物体に対する傾きとの差分を方位誤差として決定するものである。

図１２のＱ１０，Ｑ１５の詳細が、図１７に示される。すなわち、Ｑ７１において、地図データついて、図７に示すように複数の領域が抽出され、同様に、図８に示すように、検出データについて地図データに対応した複数の領域が抽出される。この後、Ｑ７２において、対応する領域間で、地図データ上での特徴点の位置と検出データ上での特徴点を比較して、そのずれが検出される。この後、Ｑ７３において、複数の領域についてのずれ量から、位置と方位についてずれの良否が決定される。

図１７の制御の詳細、つまり図９に示す地図データと検出データとの間での複数の領域での比較にもとづくずれの良否は、具体的記には次のようにして行うことができる。まず、地図データ上での固定物体の位置を（ｘｉ，ｙｉ）とし、検出データ上での固定物体の位置を（ｕｉ、ｖｉ）とし、位置ずれをｑｘ、ｑｙとし、方位のずれをθとすると、数１が成立する。そして、ｎ個のデータを使用すると、数２が成立する。

数２に示す方程式群は、数３の行列として置くことができる。

ここで、Ａの転置行列をＡt としたときに、数４を解いて、ｑｘ、ｑｙ、θを求める。そして、求められたｑｘ、ｑｙ、θが所定値以下の場合（ずれが小さい場合に）に、精度が十分とされる。なお、数４中の「ＡtＡ」、「ＡtＹ」を展開すると、数５になる。

以上実施形態について説明したが、本発明は、実施形態に限定されるものではなく、特許請求の範囲の記載された範囲において適宜の変更が可能であり、例えば次のような場合をも含むものである。移動体としては、自動車、二輪車等の車両や、歩行者等、道路を移動する任意のものを含むものである。あらかじめ進行方向を決定した後、この決定された後の進行方向を考慮して（ずれ補正された後の進行方向に基づいて）、位置決定を行うようにしてもよく、この場合は、位置精度をより一層高める上で好ましいものとなる。車両Ｖの後方にある固定物体を検出データとして用いるようにしてもよい。勿論、本発明の目的は、明記されたものに限らず、実質的に好ましいあるいは利点として表現されたものを提供することをも暗黙的に含むものである。

本発明は車両等の位置や進行方向を精度よく決定することができる。

Ｖ：車両
Ｕ：コントローラ
１：物体検出センサ
２：車輪速センサ
３：ＧＰＳ
４：地図データ
５：表示画面
６：ジャイロ
７：舵角センサ

Claims (6)

1. 移動体に設けられ、周囲の物体を検出する物体検出手段と、
   道路周辺にある固定物体の位置情報を含めて記憶した３次元の地図データと、
   前記地図データと前記物体検出手段により検出された検出データとを直交座標系におけるＸ軸およびＹ軸方向に相対変位させたときに一致点がもっとも多くなる状態において、該両データのＸ軸方向およびＹ軸方向のずれに基づいて移動体の現在位置の測位を行う現在位置決定手段と、
   前記地図データと前記検出データとを極座標系における原点を中心にして相対回転させたときに一致点がもっとも多くなる状態の相対回転角度に基づいて、移動体の進行方向を決定する進行方向決定手段と、
   を備えていることを特徴とする移動体の位置および進行方向の検出装置。
2. 請求項１において、
   前記移動体の進行方向の決定後に、前記移動体の現在位置の測位が行われる、ことを特徴とする移動体の位置および進行方向の検出装置。
3. 請求項１において、
   前記移動体が直進移動しているときは、直交座標系を用いて該移動体の現在位置の測位と進行方向との決定が行われる、ことを特徴とする移動体の位置および進行方向の検出装置。
4. 請求項１において、
   前記移動体の進行方向の決定は、所定エリアにおける前記地図データの線分方向と前記検出データの線分方向とを対比させることにより行われる、ことを特徴とする移動体の位置および進行方向の検出装置。
5. 請求項１ないし請求項４のいずれか１項において、
   前記検出データで得られる道路に沿って直線状に伸びる検出固定物体の位置情報と、該検出固定物体に対応して前記地図データに記憶されている直線状の記憶固定物体の位置情報とを、直交座標系においてＸ軸方向およびＹ軸方向に相対変位させて、該両位置情報同士がもっとも一致する相対変位量が直交座標系における移動体の位置ずれ量として決定され、
   前記検出固定物体の位置情報と前記記憶固定物体の位置情報とを前記極座標系において相対回転させて、該両位置情報同士がもっとも一致する相対回転量が移動体の進行方向のずれ量として決定される、
   ことを特徴とする移動体の位置および進行方向の検出装置。
6. 請求項１ないし請求項５のいずれか１項において、
   前記移動体が自動車とされている、ことを特徴とする移動体の位置および進行方向の検出装置。
   

Images