JP5821274B2 - 移動体位置検出装置 - Google Patents

Description

本発明は、移動体位置検出装置に関するものである。

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

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

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

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

ところで、移動体の位置情報は、移動体を上方から見た平面図上の位置つまり直交座標系によって示すことが一般的である。この一方、移動体としての例えば車両において、レーダやカメラによって道路周辺に存在する固定物までの距離と方位とを検出して、この固定物の地図データ上での位置情報に基づいて、直交座標系において車両の現在位置と方位を精度よく検出することが可能となる。

しかしながら、移動体としての例えば車両の進行方向がずれていると、つまり例えば車両がロールしているときやピッチングしていて車両が水平状態でないときは、水平状態である場合に比して、ある固定物の一定位置に対する距離と方位とが相違してくることになる。このことは、検出された固定物に対する車両からの距離および方位が変化しているのと同じことを意味しており、このようなずれを有する状態で車両の現在位置の測位をマップマッチングによって行っても、得られる現在位置の精度を十分に向上させることが難しいことになる。また、移動体の進行方向のずれを加味しつつ現在位置の測位をマップマッチングによって行うには、膨大な演算を行う必要があり、制御系の負担も大きくなる。

本発明は以上のような事情を勘案してなされたもので、その目的は、マップマッチングによる移動体の現在位置の測位を、車両の進行方向のずれを加味しつつ簡便かつ精度よく行えるようにした移動体位置検出装置を提供することにある。

前記目的を達成するため、本発明にあっては次のような解決手法を採択してある。
すなわち、特許請求の範囲における請求項１に記載のように、
移動体に周囲の物体を検出する物体検出手段が設けられ、
固定物の位置情報を含む３次元地図データと前記物体検出手段で検出された検出データとをマッチングさせることにより移動体の現在位置の測位を行う移動体位置検出装置であって、
前記物体検出手段によって検出される固定物の線分の向きを用いて、移動体の進行方向の補正が行われ、
補正された後の移動体の進行方向に基づいて、移動体の現在位置の測位のためのマッチングが行われ、
前記移動体の進行方向の補正が、前記検出データにおける固定物の線分の向きと前記地図データにおける固定物の線分の向きとをマッチングさせることにより行われ、
前記移動体の進行方向が、移動体のピッチング角とロール角との少なくとも一方とされている、
ようにしてある。上記解決手法によれば、あらかじめ移動体の進行方向の補正が行われるので、その後の現在位置の測位のためのマップマッチングの処理を簡便かつ精度よく行うことができる。

以上に加えて、前記移動体の進行方向の補正が、前記検出データにおける固定物の線分の向きと前記地図データにおける固定物の線分の向きとをマッチングさせることにより行われる、ようにしてあるので、線分というある長さをもった部分同士をマッチングさせることにより進行方向の補正を行うので、補正そのものを精度よく行う上で好ましいものとなる。また、線分を有する固定物は道路周辺に数多く存在するので、精度よく現在位置を測位する機会を十分に確保する上でも好ましいものとなる。

さらに、前記移動体の進行方向が、ピッチング角とロール角との少なくとも一方とされているので、移動体において生じやすいピッチングあるいはロールしている場合での進行方向のずれを補正することができる。

移動体の前方に存在する固定物の垂直な線分を用いて、移動体のロール角が補正される、ようにしてある（請求項２対応）。この場合、垂直線分に対する移動体の左右方向線の傾きを検出することによってロール角を直接的に知ることができ、ロール角の検出を容易かつ精度よく行う上で好ましいものとなる。

移動体の側方に存在する固定物の水平な線分を用いて、移動体のピッチング角が補正される、ようにしてある（請求項３対応）。この場合、水平線分に対する移動体の上下方向線の傾きを検出することによってピッチング角を直接的に知ることができ、ピッチング角の検出を容易かつ精度よく行う上で好ましいものとなる。

移動体の進行方向の補正に用いる前記固定物として、移動体の近くでかつ長いものが選択される、ようにしてある（請求項４対応）。この場合、進行方向の補正をより精度よく行う上で好ましいものとなる。

垂直線分を有する前記固定物が複数選択され、
前記選択された複数の固定物の線分の向きの平均値を用いて、移動体のロール角の補正が行われる、
ようにしてある（請求項５対応）。この場合、垂直線分の向きのデータを安定的に得る上で好ましいものとなる。

水平線分を有する前記固定物が複数選択され、
前記選択された複数の固定物の線分の向きの平均値を用いて、移動体のピッチング角の補正が行われる、
ようにしてある（請求項６対応）。この場合、水平線分の向きのデータを安定的に得る上で好ましいものとなる。

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

本発明によれば、マップマッチングによる移動体の現在位置の測位を簡便かつ精度よく行なうことができる。

本発明が適用された車両の簡略平面図。 本発明の制御系統例をブロック図的に示す図。 車両前方に存在する固定物の一例を示す図。 車両側方に存在する固定物の一例を示す図。 本発明の制御例を示すフローチャート。 本発明の制御例を示すフローチャート。 現在位置のマップマッチングの一例を示す図。 方位のマップマッチングの一例を示す図。

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

車両Ｖは、マイクロコンピュータを利用して構成されたコントローラ(制御ユニット）Ｕを有する。コントローラＵは、後述するように、車両Ｖの現在位置と進行方向とを精度よくマップマッチングする機能と、衝突回避のための運転支援等の制御を行う。このため、コントローラＵには、図２に示すように、前述の各種センサ等１〜７からの信号が入力される一方、表示画面５、警報装置８，車両制御装置９を制御するようになっている。コントローラＵは、表示画面５に対して、車両Ｖの現在位置を表示する制御を行う。物体検出センサ１で検出された物体が車両Ｖに接触する可能性があると判断したときに、警報装置８を作動させて、例えば音声によって運転者に警報を行う。また、コントローラＵは、物体検出センサ１によって、警報装置８が作動するよりもより近い距離にある物体を検出して衝突の危険性が極めて高いと判断したときに、車両制御装置９を作動させて、例えば自動操舵を行ったり自動ブレーキを行う等によって、衝突を回避する制御を行うようになっている。

また、コントローラＵは、車車間通信によって、車両Ｖの現在位置と進行方向とを前方付近に存在する他車両に知らせて、他車両との衝突等を未然に防止するようになっている。さらに、コントローラＵは、車両Ｖが車線を逸脱して危険である場合に、自動操舵を行って車線逸脱を防止するようになっている。コントローラＵは、この他、例えば車両Ｖと、停止線位置、交差点位置、死角域のガードレールとの距離に応じて車両Ｖについての適正な制御を行う。なお、車両Ｖの位置に応じたコントローラＵによる各種の制御は種々想定されるが、位置検出そのものとは直接関係ないので、これ以上の説明は省略する。

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

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

図３は、車両Ｖの前方に存在する固定物の一例が示される。この図３中、１１は車両Ｖが走行している走行レーン、１２は、その反対走行レーン、１３は中央車線である。また、走行レーン１１，１２に対して交差する道路が符合１４で示される。１５〜２０は、物体検出センサ１によって検出された固定物であり、それぞれ垂直な線分を有するものとなっている。すなわち、固定物１５は道路標識を保持する支柱であり、固定物１６と１８は信号機を保持する支柱であり、固定物１７は電柱であり、固定物１９は建物の柱であり、固定物２０は煙突である。これらの垂直線分を有する固定物のうち、例えば、１５，１６，１７，１８が、後述するロール角の補正に用いる垂直成分とされ、固定物１９と２０は、車両Ｖから遠い（例えば３０ｍ以上離間）ということで、ロール角の補正には用いられないものとされる。

図３において、符合Ｌ１は、車両Ｖの左右方向に伸びる水平方向線である。この水平方向線Ｌ１が、垂直な固定物となす角度がロール角を示すことになる。すなわち、水平方向線Ｌ１が垂直固定物と直角であれば、車両Ｖは水平状態であってロール角が０ということになる。また、水平方向線Ｌ１が例えば右上がりであって、垂直な固定物となす角度が例えば９０度に対して３０度ずれているときは、車両Ｖが右上がりに３０度ロールしていることになる。なお、水平方向線Ｌ１と複数の垂直固定物１５〜１８とのなす角度がそれぞれ検出されて、その平均値が最終的に車両Ｖのロール角とされる。

車両Ｖの現在位置を示す直交座標系においては、車両Ｖが水平状態を基準としていることから、車両Ｖのロール角が分かることにより、ロール状態で得られた直交座標系での現在位置が、水平状態での直交座標系に容易に変換されることになる（ロール角に基づく補正）。

図４は、車両Ｖの側方に存在する固定物の一例が示される。この図４中、１１〜１３は図３の場合と同じであり、走行レーンおよび中央線である。また、図４では、歩道３１、門３２を有する塀３３、建物３４、タンク３５を保持した枠状の構造物３６が示される。これらの固定物のうち、水平線分（水平方向に伸びる線分）は、門３２の上縁３２ａ、塀３３の上縁３３ａ、建物３４の屋根の軒先縁部３４ａ、構造物３６の梁部３６ａがある。そして、車両Ｖの上下方向線が符合Ｌ２で示される。

図４において、上下方向線Ｌ２が、水平な固定物となす角度がピッチング角を示すことになる。すなわち、上下方向線Ｌ２が水平固定物と直角であれば、車両Ｖは水平状態であってピッチング角が０ということになる。また、上下方向線Ｌ２が例えば前傾斜であって、水平な固定物となす角度が例えば９０度に対して１０度ずれているときは、車両Ｖが前下がりに１０度ピッチングしているということになる。なお、上下方向線Ｌ２と複数の水平固定物（３２ａ、３３ａ、３４ａ、３６ａ）とのなす角度がそれぞれ検出されて、その平均値が最終的に車両Ｖのピッチング角とされる。なお、相対的に遠方にある水平線分３４ａ、３６ａを除外して、水平線分３２ａ、３３ａのみを用いてピッチング角を検出するようにしてもよい。

車両Ｖの現在位置を示す直交座標系においては、車両Ｖが水平状態を基準としていることから、車両Ｖのピッチング角が分かることにより、ピッチング状態で得られた直交座標系での現在位置が、水平状態での直交座標系に容易に変換されることになる（ピッチング角に基づく補正）。なお、ロールとピッチングが共に生じている場合があるが、この場合は、前述のようにして検出されたロール角とピッチング角との両方でもって直交座標系が補正されることになる。

ここで、位置ずれと方位ずれの両方を含む場合に、そのずれ量を演算する手法例について説明する。まず、地図データ上での固定物体の位置を（ｘｉ，ｙｉ）とし、検出データ上での固定物体の位置を（ｕｉ、ｖｉ）とし（ロール角、ピッチング角によって補正された後の直交座標系での位置となる）、位置ずれをｑｘ、ｑｙとし、方位のずれをθとすると、数１が成立する。そして、ｎ個のデータを使用すると、数２が成立する。

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

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

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

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

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

次に、コントローラＵの詳細な制御内容について、図５、図６のフローチャートを参照しつつ説明する。なお、以下の説明でＱはステップを示す。まず、図５のＱ１において、図２に示す各種センサ等からの信号が入力される。この後、Ｑ２において、ＧＰＳ３での検出結果に基づいて、車両Ｖの暫定の現在位置が決定されると共に、この暫定現在位置の道路情報が地図データ４から読み込まれる。

Ｑ２の後、Ｑ３において、物体検出センサ１の検出範囲内で、車両Ｖの周辺の所定エリア（例えば車両Ｖを中心とする２５ｍ四方）が設定される。この後、Ｑ４において、上記所定エリアに対応して、物体検出センサ１で検出された検出データ（での地図データ）が作成されると共に、地図データ４に基づく地図データが読み出される。

Ｑ４の後、Ｑ５において、方向判定を行必要があるか否か、つまり車両Ｖの進行方向の補正が必要なときであるか否かが判別される。実施形態では、方向判定する必要がある場合の所定条件が、次のように設定されている。すなわち、（１）エンジン始動後でＧＰＳデータの取得時、（２）所定距離走行したとき(速度×時間が所定値に達したとき）、（３）操舵角度が所定角度以上のとき、（４）左右の車輪速の差が所定値以上のとき、（５）ウインカが作動されたとき、（６）地図データに記憶された所定の特徴点(道路に平行な固定物が存在するとき）に位置したとき、（７）ブレーキ時あるいは加速時、（８）下り坂あるいは登り道であるとき、とされている。

上記Ｑ５の判別でＹＥＳのときは、Ｑ６において、後述するように、方向判定が実行される。Ｑ６の後、あるいはＱ５の判別でＮＯのときは、Ｑ７において、方向判定された結果に基づいて(進行方向の補正が行われた状態で）、車両Ｖの現在位置の算出が行われる。Ｑ７の後、Ｑ８において、現在位置の算出が可能であったか否かが判別される。このＱ８の判別でＹＥＳのときは、Ｑ９において、Ｑ７で算出された現在位置が、最終的な現在位置として決定される。また、Ｑ８の判別でＮＯのときは、Ｑ９を経ることなくリターンされる。なお、Ｑ８の判別でＮＯのときは、現在位置として、Ｑ２での暫定現在位置をそのまま用いたり、あるいは前回決定された現在位置と車速と舵角とヨーレートに基づいて現在位置を推定する等のことが行われる。

図５のＱ６の詳細が図６に示される。すなわち、まずＱ２１において、検出データから、車両前方に存在する固定物のうち垂直線分を有するものが抽出される。この後Ｑ２２において、抽出された固定物のうち、車両Ｖに近くて長いもの（例えば車両Ｖから２０ｍ以内でかつ４ｍ以上の長さを有するもの）が選択される。この後、Ｑ２３において、選択された垂直線分に対してそれぞれロール角が検出される。そして、Ｑ２４において、検出された複数（１つの場合もある）のロール角を平均化して、最終的なロール角が決定される。

Ｑ２４の後、Ｑ２５において、検出データから、車両側方に存在する固定物のうち水平線分を有するものが抽出される。この後Ｑ２６において、抽出された固定物のうち、車両Ｖに近くて長いもの（例えば車両Ｖから２０ｍ以内でかつ４ｍ以上の長さを有するもの）が選択される。この後、Ｑ２７において、選択された水平線分に対してそれぞれピッチング角が検出される。そして、Ｑ２８において、検出された複数（１つの場合もある）のピッチング角を平均化して、最終的なピッチング角が決定される。

Ｑ２８の後は、Ｑ２９において、それぞれ平均値としてのロール角とピッチング角とに基づいて、検出されている固定物の位置情報が、車両Ｖが水平状態にあるときの直交座標系の値として補正される。そして、この補正後の車両Ｖが水平状態にあるときの直交座標系に基づいて、地図データとマップマッチングされて、車両Ｖの現在位置と方位とが精度よく決定されることになる。

以上実施形態について説明したが、本発明は、実施形態に限定されるものではなく、特許請求の範囲の記載された範囲において適宜の変更が可能であり、例えば次のような場合をも含むものである。移動体としては、自動車、二輪車等の車両や、歩行者等、道路を移動する任意のものを含むものである。車両Ｖの後方にある固定物の垂直線分をロール角検出のためのデータとして用いるようにしてもよい。勿論、本発明の目的は、明記されたものに限らず、実質的に好ましいあるいは利点として表現されたものを提供することをも暗黙的に含むものである。

本発明によれば、車両等の現在位置の測位を簡便に精度よく行う上で好ましいものとなる。

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

Claims (7)

1. 移動体に周囲の物体を検出する物体検出手段が設けられ、
   固定物の位置情報を含む３次元地図データと前記物体検出手段で検出された検出データとをマッチングさせることにより移動体の現在位置の測位を行う移動体位置検出装置であって、
   前記物体検出手段によって検出される固定物の線分の向きを用いて、移動体の進行方向の補正が行われ、
   補正された後の移動体の進行方向に基づいて、移動体の現在位置の測位のためのマッチングが行われ、
   前記移動体の進行方向の補正が、前記検出データにおける固定物の線分の向きと前記地図データにおける固定物の線分の向きとをマッチングさせることにより行われ、
   前記移動体の進行方向が、移動体のピッチング角とロール角との少なくとも一方とされている、
   ことを特徴とする移動体位置検出装置。
2. 請求項１において、
   移動体の前方に存在する固定物の垂直な線分を用いて、移動体のロール角が補正される、ことを特徴とする移動体位置検出装置。
3. 請求項１において、
   移動体の側方に存在する固定物の水平な線分を用いて、移動体のピッチング角が補正される、ことを特徴とする移動体位置検出装置。
4. 請求項２または請求項３において、
   移動体の進行方向の補正に用いる前記固定物として、移動体の近くでかつ長いものが選択される、ことを特徴とする移動体位置検出装置。
5. 請求項２において、
   垂直線分を有する前記固定物が複数選択され、
   前記選択された複数の固定物の線分の向きの平均値を用いて、移動体のロール角の補正が行われる、ことを特徴とする移動体位置検出装置。
6. 請求項３において、
   水平線分を有する前記固定物が複数選択され、
   前記選択された複数の固定物の線分の向きの平均値を用いて、移動体のピッチング角の補正が行われる、
   ことを特徴とする移動体位置検出装置。
7. 請求項１ないし請求項６のいずれか１項において、
   前記移動体が自動車とされている、ことを特徴とする移動体位置検出装置。
   

Images