移動体位置検出装置
【課題】位置情報精度を高めることができると共に、常時、その高い位置情報精度を維持できる移動体位置検出装置を提供する。
【解決手段】レーザレーダ３の検出データに基づく周辺物の複数の節点ｓと、地図データ上の当該周辺物の複数の節点ｍとを対応付け、その対応付けられた各組の節点ｓ、ｍを用いて、座標変換式に基づき誤差θ、ｑｘ、ｑｙを算出し、その誤差θ、ｑｘ、ｑｙに基づきＧＰＳにより検出された自車１位置を補正する。これにより、ありふれて存在する周辺物に関する複数の節点に基づき誤差算出を可能とし、誤差算出精度を高めると共に、位置補正頻度を高める。
【解決手段】レーザレーダ３の検出データに基づく周辺物の複数の節点ｓと、地図データ上の当該周辺物の複数の節点ｍとを対応付け、その対応付けられた各組の節点ｓ、ｍを用いて、座標変換式に基づき誤差θ、ｑｘ、ｑｙを算出し、その誤差θ、ｑｘ、ｑｙに基づきＧＰＳにより検出された自車１位置を補正する。これにより、ありふれて存在する周辺物に関する複数の節点に基づき誤差算出を可能とし、誤差算出精度を高めると共に、位置補正頻度を高める。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体位置検出装置に関する。
【背景技術】
【０００２】
ＧＰＳ（Global Positioning System)を用いたナビゲーションシステムにおいては、ＧＰＳの測位誤差等を考慮し、表示装置における地図上の道路上に自車を表示するべく、特許文献１に示すように、走行軌跡と道路形状とによりマップマッチングが行われている。
【０００３】
ところで、近時、ナビゲーションシステムと移動体の走行制御等とを関連付けて移動体の運転支援を高めることが考えられている。このため、当該移動体の位置情報に関し、マップマッチングを行っている場合の誤差±２〜３ｍ程度の精度では足りず、位置情報精度をより高めることが望まれている。
【０００４】
これに対しては、特許文献２において、絶対位置が既知とされた外部の固定対象物に対する自己の相対位置を、検出手段により検出し、その検出結果に基づき、ＧＰＳ検出に基づく自己の位置を補正するものが提案されている。これにより、補正後の自己の位置情報の精度は高まる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−１０２０５２号公報
【特許文献２】特開２００７−２１８８４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、上記特許文献２に係るものにおいては、絶対位置が既知とされた外部の固定対象物の数自体が少なく、位置補正する頻度は低いものとならざるを得ない。このため、常時、高い位置情報精度を維持することはできない。
【０００７】
また、上記特許文献２に係るものは、１地点（絶対位置が既知とされた外部の固定対象物）に対する相対的距離を検出して、それを用いて誤差算出することにより、位置補正するものであることから、誤差算出精度は高いとはいえず、これに伴い、高い精度の位置情報自体を得ることは難しい。
【０００８】
本発明は、このような事情に鑑みてなされたもので、その技術的課題は、位置情報精度を高めると共に、常時、その高い位置情報精度を維持できる移動体位置検出装置を提供することにある。
【課題を解決するための手段】
【０００９】
前記技術的課題を達成するために本発明（請求項１に係る発明）においては、
移動体に搭載されて、該移動体の位置を検出する移動体位置検出装置において、
前記移動体の周囲における周辺物の節点の位置、又は周辺物の位置および形状を記憶する地図データ記憶手段と、
前記移動体の位置を検出する位置検出手段と、
前記移動体の周囲における周辺物を検出する周辺物検出手段と、
前記周辺物検出手段が検出した周辺物の複数の節点の相対的位置を検出する第１節点検出手段と、
前記第１節点検出手段が検出した各節点に対応する節点を、前記地図データ記憶手段が記憶する地図データからそれぞれ検出する第２節点検出手段と、
前記第１節点検出手段が検出した各節点と、該各節点に対応する前記第２節点検出手段が検出した各節点とに基づき、該第２節点検出手段が検出した節点に対する該第１節点検出手段が検出した節点の誤差を算出する誤差算出手段と、
前記誤差算出手段が算出した誤差に基づき、前記位置検出手段が検出した位置を補正する補正手段と、
を備えている、
ことを特徴とする移動体位置検出装置とした構成としてある。この請求項１の好ましい態様としては、請求項２以下の記載の通りとなる。
【発明の効果】
【００１０】
本発明（請求項１に係る発明）によれば、周辺物検出手段が検出する周辺物の複数の各節点と、地図データ上の各節点、又は周辺物の位置および形状から求められる各節点とをそれぞれ対応させ、その複数組の対応する節点を用いることにより、誤差を算出することから、１組の対応する節点を用いる場合に比べて、誤差算出の精度を高めることができる。このため、当該移動体の位置補正精度を高めることができ、当該移動体の位置情報に関し、その精度を高めることができる。
【００１１】
また、移動体搭載の周辺物検出手段が検出する周辺物の節点と、地図データ記憶手段が記憶する移動体周囲における当該周辺物の節点又は周辺物の位置および形状から求められる各節点と、を利用して両節点の誤差を算出し、その誤差に基づき、位置検出手段が検出した位置を補正することから、位置補正（誤差算出）に際して、移動体周囲にありふれて存在する周辺物を利用できることになり、位置補正の頻度を高めることができる。このため、当該移動体の位置情報に関し、常時、高い精度を維持できることになる。
【００１２】
請求項２に係る発明によれば、第２節点検出手段が、地図データ記憶手段が記憶する地図データから移動体の進行域における地図データを切り出す地図データ切出し処理部と、地図データ切出し処理部が切り出した地図データから周辺物の節点を抽出する節点抽出部と、第１節点検出手段が検出した各節点と節点抽出部が抽出した節点とを対応付ける節点マッチング処理部と、を備えていることから、具体的構成をもって、前記請求項１と同様の作用効果を得ることができる。
【００１３】
請求項３に係る発明によれば、節点マッチング処理部が、節点抽出部が抽出した節点間を結ぶ線分と、第１節点検出手段が検出する節点間を結ぶ線分とを比較することにより、第１節点検出手段が検出した各節点に対応する節点を、節点抽出部が抽出した節点から検出するように設定されていることから、節点間を結ぶ線分（リンクデータ）を利用することにより、第１節点検出手段が検出した各節点と、節点抽出部が抽出した節点とを的確に対応付けることができる。このため、誤差算出を確実に行うことができる。
【００１４】
請求項４に係る発明によれば、節点マッチング処理部は、第１節点検出手段が周辺物の複数の節点を検出する度に、第１節点検出手段が検出する節点間を結ぶ線分と節点抽出部が抽出した節点間を結ぶ線分とが一致するものを一組の一致線分として、その一致線分の差分の絶対値を順次、保存すると共に、その差分の絶対値の総和の正規化値を算出して、該正規化値が該正規化値の算出以前の正規化値よりも減少しているか否かを判別するように設定され、誤差算出手段は、節点マッチング処理部が正規化値の最小値を判定したときに限り、各組の一致線分における節点を節点マッチング処理部から受け入れて、誤差算出を行うように設定されていることから、全体として、一致線分の一致性が高い場合に限り（節点の対応性が高い場合に限り）、その新たな各組の一致線分の始点、終点をなす節点を用いて誤差処理がなされ、それ以外の場合には、誤差処理がなされず、位置検出手段が検出した位置を補正する際には、以前の対応性の高い節点が用いられる。このため、誤差算出の精度が低下して、当該移動体の高い位置情報精度が低下することを防止できる。
【００１５】
請求項５に係る発明によれば、第２節点検出手段が、さらに死角データ除去部を備え、死角データ除去部が、地図データ切出し部が切り出した地図データから、周辺物検出手段の死角領域に存在すると推定される死角データを除去するように設定されていることから、第１節点検出手段が検出した各節点と、節点抽出部が抽出した各節点とを対応付ける対応付け負荷（節点マッチング処理負荷）を軽減できる。
【００１６】
請求項６に係る発明によれば、地図データ記憶手段が、三次元空間を所定の大きさのボクセルに分割して、その各ボクセルの三次元位置座標と、その各ボクセルに含まれる前記周辺物種別の属性データと、該周辺物の節点の位置データと、を記憶していることから、移動体の進行域にありふれて存在する周辺物の複数の節点を誤差算出に利用できることになり、誤差算出精度を高めると共に、位置補正の頻度を高めることができる。このため、ボクセル地図データを用いることにより、具体的に、当該移動体の位置情報精度を高めると共に、常時、その高い位置情報精度を維持できる。
【００１７】
請求項７に係る発明によれば、前記地図データ記憶手段が、前記周辺物の位置データおよび形状情報を記憶し、前記節点が、前記周辺物の位置データおよび形状情報より求められることから、移動体の進行域にありふれて存在する周辺物の複数の節点を誤差算出に利用できることになり、誤差算出精度を高めると共に、位置補正の頻度を高めることができる。このため、地図データを用いることにより、具体的に、当該移動体の位置情報精度を高めると共に、常時、その高い位置情報精度を維持できる。
【００１８】
請求項８に係る発明によれば、ボクセルの大きさが、道路領域に配置するものに関し、道路種別に異なるように設定されていることから、ボクセルの大きさを、移動体の移動速度に応じたものにでき、移動体の移動速度が異なる場合でも、位置情報精度を高く維持できる。
【００１９】
請求項９に係る発明によれば、地図データ記憶手段が、所定高さ以上に位置する周辺物の節点の位置データを含み、周辺物検出手段が、所定高さ以上の周辺物を検出するように設定されていることから、他の移動体等の影響を受けることなく周辺物を検出して誤差算出を行うことができ、高い精度の移動体の位置情報を得ることができる。
【図面の簡単な説明】
【００２０】
【図１】第１実施形態に係る移動体位置検出装置を搭載した車両を平面的に示す図。
【図２】実施形態に係る移動体位置検出装置を示す構成図。
【図３】ボクセル地図データに基づく周辺物の節点と、それに対応するレーザレーダの検出データに基づく周辺部の節点との位置ずれを概念的に示す説明図。
【図４】ボクセルを概念的に説明する説明図。
【図５】レーザレーダの検出データにおける節点検出処理、遮蔽・空間判定処理を説明する説明図。
【図６】地図データにおける節点検出処理、遮蔽・空間判定処理を説明する説明図。
【図７】地図データの切り出しを説明する説明図。
【図８】地図データに基づく節点と、それに対応するレーザレーダの検出データに基づく節点とのマッチング処理を説明する説明図。
【図９】方位誤差をθ、位置誤差をｑｘ、ｑｙとした場合におけるＧＰＳ座標系と世界座標系との関係を示す説明図。
【図１０】節点のマッチング処理を説明する説明図。
【図１１】移動体位置検出装置の制御例を示すフローチャート。
【図１２】節点のマッチング処理例を示すフローチャート。
【図１３】図１２の続きを示すフローチャート。
【図１４】図１３の続きを示すフローチャート。
【図１５】ステップＱ１４の処理を説明する説明図。
【図１６】ステップＱ１３の処理を説明する説明図。
【図１７】ステップＱ１６の処理を説明する説明図。
【図１８】ステップＱ１８の処理を説明する説明図。
【図１９】ステップＱ２０の処理を説明する説明図。
【図２０】第２実施形態に係るボクセル地図データを概念的に示す説明図。
【図２１】第３実施形態を説明する説明図。
【発明を実施するための形態】
【００２１】
以下、本発明の実施形態について、図面に基づいて説明する。
【００２２】
図１〜図１９は第１実施形態を示す。この第１実施形態を示す図１において、符号１は移動体としての車両（自車）であり、この車両１には、自車位置、方位を精度良く検出できる実施形態に係る移動体位置検出装置２が搭載されている。この移動体位置検出装置２は、周辺物検出手段としてのレーザレーダ３と、位置検出手段としてのＧＰＳセンサ４と、地図データ記憶手段としてのナビゲーション装置６と、それらからの各種情報を入力情報として、自車１位置の検出処理を実行する処理ユニットＵ（図２参照）と、が備えられている。
【００２３】
前記レーザレーダ３は、レーザの照射及びその反射波の検出に基づいて、自車（車両）１周囲の周辺物の方位、相対的距離を検出するものである。図３は、車両前方における周辺物（周辺物を節点（丸印をもって示す）ｓ（１）・・・ｓ（nums）をもって示す）がレーザレーダ３により検出されたことを概念的に示している。
【００２４】
前記ＧＰＳセンサ４は、ＧＰＳ衛星からの信号を受信して自車１位置を検出するものである。これにより、自車１位置だけでなく、その検出された自車１位置を利用して、レーザレーダ３が検出した周辺物の位置をも算出することができることになる。この場合、自車1位置については、ＧＰＳに基づく測位誤差が生じ、周辺物の位置については、ＧＰＳ、レーザレーダ３の誤差等が生じることになる。
【００２５】
前記ナビゲーション装置６は、図４に示すボクセルＢを用いて、三次元地図データを記憶している。ボクセルＢは、三次元空間を所定の大きさ、例えば一辺１ｍの立方体に分割したものであり、その各ボクセルＢには、その三次元位置座標は勿論、その各ボクセルＢに含まれる物体、具体的には、電柱、ガードレール、電源ボックス等の節点（ノード又は端点）の正確な位置座標（世界座標系のもの）が位置データとして含まれている。図４は、概念的に、ボクセルＢ内に、ボクセルＢの中心位置データOと、物体の節点ｍの位置データとが存在することを示している。また、図３は、各ボクセルＢに自車の前方側における物体としての周辺物の節点ｍ（１）・・・ｍ（numm）の正確な位置情報が含まれ、その各ボクセルＢから周辺物の節点ｍ（１）・・・ｍ（numm）の位置情報が抽出可能であることを示している。
【００２６】
前記処理ユニットＵは、図２に示す構成を有しており、その検出処理に基づき、自車１位置を精度良く検出処理できることになっている。
【００２７】
処理ユニットＵは、図２に示すように、節点検出処理部７、遮蔽・空間判定処理部８、自車位置検出処理部９、進行方向検出処理部１０、地図データ切出し等処理部１１、節点抽出及び遮蔽・空間判定処理部１２を備えている。
【００２８】
節点検出処理部７には、レーザレーダ３からの出力信号（周辺物の方位、距離等）が入力されており、その節点検出処理部７においては、図３，図５に示すように、レーザレーダ３からの出力信号に基づき、自車１前方における周辺物（電柱、ガードレール、電源ボックス等）の節点（図３では白丸、図５では黒丸をもって示す）ｓ（１）・・・ｓ（nums）の検出処理が行われる。図３，図５において、節点ｓ（１）・・・ｓ（nums）の各間、節点ｍ（１）・・・ｍ（numm）の各間が線分をもって結ばれた状態が示されているが、この各線分は、節点間の部分をリンクとして規定したリンクデータになることを示している。図３中、符号Ｌmは、地図データに基づく節点間の線分の長さ、Ｌｓは、レーザレーダ３の検出結果に基づく節点間の線分の長さを示している。
【００２９】
遮蔽・空間判定処理部８には、節点検出処理部７からの出力信号が入力されており、その遮蔽・空間判定処理部８においては、判定すべき節点間が遮蔽状態か又は空間状態かの判定処理が行われる。具体的に図５をもって説明すれば、節点ｓ間のうち、周辺物が存在する部分（線分をもって結ばれている部分）が遮蔽状態と判定され、節点ｓ間のうち、周辺物が存在しない部分（線分をもって結ばれていない部分）が空間状態と判定されることになる。
【００３０】
自車位置検出処理部９には、前記ＧＰＳセンサ４からの出力信号が入力されており、自車位置検出処理部９においては、ＧＰＳセンサ４からの出力信号に基づき自車１位置が検出される。
【００３１】
進行方向検出処理部１０には、上記自車位置検出処理部９における自車位置データが入力されており、進行方向検出処理部１０においては、自車位置データに基づき自車１位置の変化を捉えて自車１の進行方向（方位）が検出される。
【００３２】
地図データ切出し等処理部１１には、前記ナビゲーション装置６からのボクセル三次元地図データ、前記自車位置検出処理部９からの自車位置データ、前記進行方向検出処理部１０からの自車の進行方向データがそれぞれ入力される。この地図データ切出し等処理部１１は、具体的には、地図データ切出し処理部、死角データ除去処理部を備えている。地図データ切出し処理部は、ナビゲーション装置６における地図データ（ボクセルデータ）から自車１位置周辺の地図データを切出す機能を有している。この場合、自車位置・方位推定誤差を考慮して、図７に示すように、地図データ切出し領域Ａｍは、レーザレーダ３の検出領域Ａｓよりも大きめに設定される。死角データ除去処理部は、自車位置・方位検出誤差を考慮して、想定されるいずれの自車位置・方位からも死角になる死角データを切出し地図データから削除する機能を有している。後述の図６中、×印を付されたものは、削除される死角データを示している。
【００３３】
節点抽出及び遮蔽・空間判定処理部１２には、前記地図データ切出し等処理部１１からの出力データが入力される。この節点抽出及び遮蔽・空間判定処理部１２は、切り出した地図データから自車１の進行域におけるガードレール１７，電柱１８，電源ボックス１９等の周辺物（図７参照）の節点（ノード又は端点）ｍを抽出する節点抽出部と、その節点抽出部が抽出した節点ｍ間の遮蔽・空間の判定処理を行う遮蔽・空間判定処理部と、を備えている。図６は、切り出された地図データから周辺物の節点ｍ（黒丸をもって示す）を抽出した状態を示しており、その節点m間のうち、周辺物が存在する部分（線分をもって結ばれている部分）が遮蔽状態と判定され、節点m間のうち、周辺物が存在しない部分（線分をもって結ばれていない部分）が空間状態と判定されることになる。
【００３４】
この場合、地図データに基づく節点ｍ（１）・・・ｍ（numm）は、レーザレーダの検出データに基づく節点ｓ（１）・・・ｓ（nums）に対して対応関係を有するものであるが、図３，図８に示すように、ＧＰＳ、レーザレーダ３等による誤差のため、両者は合致しないものとなる。
【００３５】
また、処理ユニットＵは、図２に示すように、節点マッチング処理部１３、自車位置／方位誤差算出処理部１４、自車位置算出処理部１５を備えている。節点マッチング処理部１３には、遮蔽・空間判定処理部８からの節点データと、節点抽出及び遮蔽・空間判定処理部１２からの節点データとが入力される。節点マッチング処理部１３は、図８に示すように、遮蔽・空間判定処理部８からの節点データと、節点抽出及び遮蔽・空間判定処理部１２からの節点データとに基づき、レーザレーダ３の検出データに基づく節点ｓに対応する節点を、地図データに基づく節点ｍから検出する処理である。この両節点ｍ、ｓの対応付けの検出に際しては、節点間の線分の長さを距離としてＤＰマッチング処理が行われ、そのとき、線分の角度（自車方位の推定誤差）、空間・遮蔽の状態、線分中の点群密度等が考慮され、レーザレーダ３により検出されない点を含む経路については削除される。
【００３６】
尚、図８中、両方向を向いた矢印は、レーザレーダ３の検出データに基づく線分と、それに対応する地図データに基づく線分の対比を示している。
【００３７】
自車位置／方位誤差算出処理部１４には、節点マッチング処理部１３から、対応関係があるとされるレーザレーダ３の検出データに基づく節点ｓ及び地図データに基づく節点ｍに関するデータが入力される。自車位置／方位誤差算出処理部１４は、レーザレーダ３の検出データに基づく各節点ｓと、それに対応する地図データに基づく各節点ｍとを（数１）に示す座標変換式に用いて、自車位置及び方位の誤差が算出する。
【００３８】
すなわち、ＧＰＳを用いて車両（自車）１位置を検出する場合、測位誤差を含んでおり、これに伴い、その車両からレーザレーダ３等を用いて周辺物の位置（距離、方位等）を検出したとしても、その周辺物の位置は、その車両の位置（ＧＰＳによる検出位置）を基礎として算出する限り（ＧＰＳ座標系の下では）、レーザレーダ３の誤差に加えて、ＧＰＳの誤差を含むことになる。
【００３９】
一方、誤差を含むＧＰＳ座標系で表される点（ｕｉ，ｖｉ）と、それに対応する世界座標系の点（ｘｉ，ｙｉ）との間には、ＧＰＳ座標系で表される点（ｕｉ，ｖｉ）の方位誤差をθ、位置誤差をｑｘ、ｑｙとすると、（数１）に示す一般的な座標変換式が成立する。この（数１）に示す関係を図式的に示せば、図９に示す通りである。
【００４０】
【数１】
【００４１】
このため、ｎ個の物標の位置（ＧＰＳ座標系）が車両搭載のレーザレーダ３により検出されたときには、それらと、そのｎ個の物標の地図データに基づく位置（世界座標系）とを、（数１）に用いて、（数２）に示す方程式群を得ることができる。
【００４２】
【数２】
【００４３】
この（数２）に示す方程式群は、（数３）の行列として置くことができる。
【００４４】
【数３】
【００４５】
ここで、Ａの転置行列をtＡとすると、tＡＡに逆行列が存在する場合、（数３）は、（tＡＡ）-1ＡＢ＝（tＡＡ）-1Ｃ ，（tＡ）-1Ｂ＝（tＡＡ）-1Ｃ，tＡ（tＡ）-1Ｂ＝tＡ（tＡＡ）-1Ｃを経て、（数４）を得ることができる。
【００４６】
【数４】
【００４７】
このように、自車位置／方位誤差算出処理部１４では、上記（数４）を解く処理が行われ、方位誤差θ、位置誤差ｑｘ、ｑｙが算出される。尚、（数４）中のtＡＡ、tＡＣを展開すると、（数５）となる。
【００４８】
【数５】
【００４９】
自車位置算出処理部１５には、上記自車位置／方位誤差算出処理部１４から、方位誤差θ、位置誤差ｑｘ、ｑｙの各情報が入力されると共に、自車位置検出処理部９から自車位置情報（ＧＰＳによる検出）が入力される。自車位置算出処理部１５は、その各情報を（数１）に用いて演算し、これにより、位置精度の高い自車１位置を得る。
【００５０】
この精度の高い自車位置情報は、自車位置算出処理部１５から利用対象１６に出力されて種々のものに利用される。例えば、各車両１の正確な位置情報を車車間通信することによって追突を防止したり、地図データより求める対象物（停止線位置、交差点位置、死角域のガードレール等）と自車１との正確な距離を算出して適正な車両制御を行うことが行われる。
【００５１】
次に、前記処理ユニットＵの制御内容を、図１１に示すフローチャートに基づいて具体的に説明する。尚、図１１における符号Ｓは、ステップを示す。
【００５２】
Ｓ１において、ＧＰＳを利用して、自車位置及び方位が検出され、この後、Ｓ２におけるエゴモーションによる補間を経て、Ｓ３において、図７に示すように、自車位置周辺の地図データの切出しが行われる。次のＳ５において、その地図データ中から、死角データが除去される。この死角データを除去するに際しては、自車位置・方位の検出誤差を考慮して、想定されるいずれの自車位置・方位からも死角になる死角データのみが削除される（図６の×印参照）。
【００５３】
この後、図６に示すように、Ｓ６において、切り出した地図データ（各ボクセルＢ）から周辺物の節点ｍが抽出され、次のＳ７において、その各隣り合う節点ｍ間が遮蔽状態にあるか、又は空間状態にあるかが判定される。
【００５４】
次に、Ｓ８において、レーザレーダ３により周辺物が検出され、次のＳ１０において、図５に示すように、周辺物の節点ｓが抽出され、その次のＳ１１において、各隣り合う節点ｓ間が遮蔽状態にあるか又は空間状態にあるかが判定される。この後、Ｓ１２において、地図データに基づく節点ｍと、レーザレーダ３の検出データに基づく節点ｓとのマッチング処理（対応付け）が行われる。
【００５５】
上記節点のマッチング処理について、図１０、図１２〜図１９に基づいて具体的に説明する。尚、図１２〜図１４における符号Ｑは、ステップを示す。
【００５６】
地図データに基づく節点ｍは世界測地系座標系あるいは平面直角座標系（上述の世界座標系）で表されている。一方、レーザレーダ３の検出データに基づく節点ｓは車両１を基準とした直角座標系（上述のＧＰＳ座標系）で表されている。地図データに基づく節点ｍと、レーザレーダ３の検出データに基づく節点ｓとのマッチング処理を行うため、Ｑ０において、世界測地系座標系あるいは平面直角座標系（世界座標系）で表されている地図データに基づく節点ｍの座標を、ＧＰＳで検出した車両１の位置座標に基づき、車両１を基準とした直角座標系（ＧＰＳ座標系）に変換する。
【００５７】
上記、座標変換処理は、車両１を基準とした直角座標系（ＧＰＳ座標系）で表されているレーザレーダ３の検出データに基づく節点ｓを、ＧＰＳで検出した車両１の位置座標に基づき、世界測地系座標系あるいは平面直角座標系（世界座標系）に変換しても良い。
【００５８】
地図データに基づく節点ｍと、レーザレーダ３の検出データに基づく節点ｓとのマッチング処理は、地図データに基づく節点ｍ同士を結んで線分（リンクデータ）を形成すると共に、レーザレーダ３の検出データに基づく節点ｓ同士を結んで線分を形成し、その両者の線分の比較を通じて、対応付けが行われる。このため、先ず、Ｑ１において、線分の初期値設定が行われる。具体的には、図１０に示すように、地図データに基づく節点ｍであって、自車１左側において自車１に最も近い節点ｍを線分Ｌｍの始点ｓｍとして捉え、その始点ｓｍをｓｍ←１に設定すると共に、そのｍ（１）を基準にして、図１０中、時計方向に隣り合う節点ｍを線分Lmの終点ｅｍとして捉え、ｅｍ←ｓｍ＋１（＝２）と設定する。同様に、レーザレーダ３の検出データに基づく節点ｓであって、自車１左側において自車１に最も近い節点ｓを線分Ｌｓの始点ｓｓとして捉え、そのｓｓをｓｓ←１に設定すると共に、そのｓ（１）を基準にして、図１０中、時計方向に隣り合う節点ｓを線分Lｓの終点ｅｓとして捉え、ｅｓ←ｓｓ＋１（＝２）と設定する。
【００５９】
次に、Ｑ２，Ｑ３，Ｑ４において、評価値Ｆの最小値Fmin、評価値F、一致した線分数linknumが、順次、初期化される。具体的には、Ｑ２において、Fmin←−１と設定され、Ｑ３において、F←０と設定され、Ｑ４において、linknum←０と設定される。
【００６０】
Ｑ４の処理を終えると、地図データの節点ｍ同士を結んだ線分Lm(sm,em)と、レーザレーダ３の検出データの節点同士を結んだ線分Ls(ss,es)とが一致するか否かの処理が、自車１の左側に一番近いものから図１０中、時計方向（矢印方向）に向けて順次、行われる。具体的には、先ず最初に、節点ｍ（１）と節点ｍ（２）との間のLm(１,２)と、節点ｓ（１）と節点ｓ（２）との間のLs(１,２)とについて、両線分の状態（空間状態又は遮蔽状態）の同一性、点群密度の差、長さの差、傾きの差の観点から、線分の一致性が判定されることになる。
【００６１】
より具体的に説明すれば、Ｑ５において、線分Lm(１,２)及び線分Ls(１,２)が、空間状態であるか又は遮蔽状態であるかの点で同一か否かが判別され、このＱ５の判別がＮＯのときには、同一でないとして、Ｑ１４に進み、そこにおいて、線分の更新として、図１５に示すように、地図データに基づく線分Lm(sm,em)の終点emはem←em+1、すなわち、Lm(１,３)に更新される。そして、このＱ１４における更新後、Ｑ１５に進み、そのＱ１５において、地図データに基づく線分Lm(１,３)の終点em（＝３）が、切り出した地図データ中から抽出した節点ｍの個数nummを越えたか否かが判別される。このＱ１５の判別がＮＯのときには、線分Lm(１,３)の終点em（＝３）が地図データ中から抽出した節点ｍの個数nummに達しておらず、未だ前述の処理を行う必要があるとして、前記Ｑ５に戻され、このＱ１５の判別がＹＥＳのときには、Ｑ１６に進む。
【００６２】
前記Ｑ５がＹＥＳのときには、Ｑ６において、線分Lm(sm,em)とLs(ss,es)中の点群密度の差、すなわち、Lm(１,２)とLs(１,２)との点群密度の差が所定値以下か否かが判別される。そのＱ６がＮＯのときには、点群密度の観点から両線分Lm(１,２)、Ls(１,２)が同一でないとして前記Ｑ１４に進み、Ｑ６がＹＥＳのときには、Ｑ７において、線分Lm(sm,em)とLs(ss,es)との長さの差、すなわちLm(１,２)とLs(１,２)との長さの差が所定値以下か否かが判別される。このＱ７がＮＯのときには、線分の長さの観点から両線分Lm(１,２)、Ls(１,２)が一致しないとして前記Ｑ１４に進み、Ｑ７がＹＥＳのときには、Ｑ８において、線分Lm(sm,em)とLs(ss,es)の傾きの差、すなわちLm(１,２)とLs(１,２)との傾きの差が所定値以下か否かが判別される。このＱ８がＮＯのときには、傾きの観点から、両線分Lm(１,２)、Ls(１,２)が一致しないとして前記Ｑ１４に進む一方、Ｑ８がＹＥＳのときには、両線分Lm(１,２)、Ls(１,２)が一致するとして（一組の一致線分）、Ｑ９において、両線分の差分の絶対値ｄ（Lm(sm,em),Ls(ss,es))、すなわちｄ（Lm(１,２),Ls(１,２))を評価値Fに加算したものを評価値Fとする。この場合、上記Ｑ６〜Ｑ８における各所定値には、両線分Lm(sm,em)、Ls(ss,es)が一致するとして許容できるものが適宜設定される。
【００６３】
次いで、Ｑ１０において、一致した線分数linknum（当初は０）に１が加算されて、それが一致した線分数linknumとされ、次のＱ１１においては、線分が一致する毎に評価値Fが保存される。次のＱ１２においては、一致した線分情報を保存すべく、地図データと一致したレーザレーダ３検出に基づく線分の始点ssがstrss(linknum)、地図データと一致したレーザレーダ３検出に基づく線分の終点esがstres(linknum)、レーザレーダ３の検出データと一致した地図データの線分の始点smがstrsm(linknum)、レーザレーダ３の検出データと一致した地図データの線分の終点emがstrem(linknum)としてそれぞれ保存される（線分が１本一致する毎に一致した線分の対応関係の情報を評価値メモリに保存）。
【００６４】
そして、次のＱ１３においては、図１６に示すように、新たな線分同士で比較すべく、地図データに基づく線分Lm(sm,em)の始点smが終点em位置に移動され（sm←em)、線分Lm(sm,em)の終点emが新たな始点emの次の点（節点）に移動される(em←sm+1)。レーザレーダ３の検出データに基づく線分Ls(ss,es)についても、同様に、線分Ls(ss,es)の始点ssが終点es位置に移動され（ss←es)、線分Ls(ss,es)の終点esが新たな始点esの次の点（節点）に移動される(es←ss+1)。このＱ１３の処理を終えると、前記Ｑ１５に進み、地図データに基づく比較すべき線分Lm(sm,em)の終点emが、切出した地図データの最後（地図データに基づく節点の個数numm） に達しないときには、同じ処理を繰り返すべく、前記Ｑ５に戻され、その線分Lm(sm,em)の終点emが、切出した地図データの最後に達したときには、Ｑ１６に進む。
【００６５】
Ｑ１６においては、図１７に示すように、地図データに基づく線分Lm(sm,em)の終点がデータの最後に達したときにレーザレーダ３の検出データに基づく新たな線分Ls(ss,es)で比較すべく、線分Ls(ss,es)の終点esが次の節点に移動され(es←es+1)、その新たな線分Ls(ss,es)と比較すべき線分Lm(sm,em)の終点emは、始点smの次の節点に移動される(em←sm+1)。このＱ１６の処理を終えると、Ｑ１７において、レーザレーダ３の検出データに基づく線分Ls(ss,es)の終点esが、そのレーザレーダ３の検出データの最後（レーザレーダ３の検出データに基づく節点の個数nums）に達したか否かが判別される。このＱ１７の判別がＮＯのときには、同じ処理を繰り返すべく、前記Ｑ５に戻され、Ｑ１７がＹＥＳのときには、Ｑ１８において、図１８に示すように、地図データに基づく線分Lm(sm,em)の始点smが次の節点に移動され(sm←sm+1)、その線分Lm(sm,em)の終点emは新たな始点sm次の節点に移動される（em←sm+1)。このＱ１８の処理を終えると、Ｑ１９において、地図データに基づく線分Lm(sm,em)の始点smが地図データの最後(numm)から２個目に達したか否かが判別される。このＱ１９がＮＯのときには、同じ処理を繰り返すべく、前記Ｑ５に戻される一方、Ｑ１９がＹＥＳのときには、Ｑ２０において、図１９に示すように、レーザレーダ３の検出データに基づく線分Ls(ss,es)の始点ssが、次の節点に移動され(ss←ss+1)、その線分Ls(ss,es)の終点esが新たな始点ssの次の節点に移動される(es←ss+1)。また、地図データに基づく線分Lm(sm,em)の始点smは、最後に一致した地図データに基づく線分Lm(sm,em)の終点に移動され(sm←strsm(linknum))、その地図データに基づく線分Lm(sm,em)の終点emはその新たな始点smの次の節点に移動される(em←sm+1)。
【００６６】
上記Ｑ２０の処理を終えると、Ｑ２１において、レーザレーダ３の検出データに基づく線分Ls(ss,es)の始点ssが、その検出データの最後(節点の個数nums)から２個目に達したか否かが判別される。このＱ２１がＮＯのときには、同じ処理を繰り返すべく、前記Ｑ５に戻される一方、Ｑ２１がＹＥＳのときには、Ｑ２２において、両線分の差分の絶対値を加算した評価値Ｆを、一致した線分数linknumで除し、F/linknumを算出して、線分数に依存することのない一致した線分1本あたりの評価値を求め（すなわち、一致した線分数で正規化する）、このF/linknumが正規化した評価値Fsとして読み込まれる。なお、上記の例は、評価値Ｆを一致した線分数linknumで除して正規化した評価値Fsとしたが、両線分の差分の値を２乗した上で相加平均し平方根をとった二乗平均平方根（RMS (Root Mean Square)）を正規化した評価値Fsとしても良い。
【００６７】
Ｑ２２の処理を終えると、Ｑ２３において、Fmin=−１又はFs<Fminであるか否かが判別される。Fsが評価値の最小値Fminよりも小さい場合には、一致している線分の対応関係の情報を更新するためである。この場合、最初は、Fmin=−１より必ずＹＥＳとなって、次のＱ２４に進み、２回目以降はFs<Fminであれば、次のＱ２４に進むことになる。Ｑ２３がＹＥＳのときには、Ｑ２４において、評価値の最小値FminがＱ２２のFsとされた上で、Ｑ２５において、一致した線分数が更新され(linknummin←linknum)、Ｑ２６において、一致した線分情報が更新される(strssmin(1,・・,linknum)←strss(1,・・,linknum),stresmin(1,・・,linknum)←stres(1,・・,linknum),strsmmin(1,・・,linknum)←strsm(1,・・,linknum),stremmin(1,・・,linknum)←strem(1,・・,linknum))。
【００６８】
Ｑ２３がＮＯと判定された場合又はＱ２６の処理を終えた場合には、Ｑ２７に進み、そのＱ２７において、一致した線分の数linknumが１を越えているか否かが判別される。このＱ２７がＮＯのときには、リターンされる一方、Ｑ２７がＹＥＳのときには、一致したとしてＱ１２において記憶している線分以降にさらに、近似するものがないかをチェックするべく、Ｑ２８〜Ｑ３０において、記憶しているss,es,sm,emを読み出し、そこを始点、終点として、さらに近似する線分がないかをチェックすることになる。このため、Ｑ２８では一致したとして記憶し、それ以降のチェックが行われていないss,es,sm,emを読み出し、チェックを開始する線分が更新され、Ｑ２９では一致した線分数から1本が減算され、Ｑ３０においてＱ１１で一旦、記憶された評価値Fが読み出され、その後、前記Ｑ３に戻されることで、ss,es,sm,emを始点、終点とする線分以降に対して、上記と同様にＱ５からＱ８によって線分の一致度がチェックされ、上記読み出された評価値Ｆより、更に小さい評価値が検出された場合は、Ｑ２６にてその線分情報が更新記憶される。
【００６９】
上記節点のマッチング処理を終えると、Ｓ１３において、自車位置・方位の誤差算出処理が行われる。この誤差算出処理は、前述した如く、Ｓ１２の節点マッチング処理により得た地図データに基づく複数の節点ｍと、それに対応するレーザレーダ３の検出データに基づく各節点ｓと車両情報を、（数２）〜（数５）に用いることにより算出される処理であり、この誤差算出処理により、方位誤差θ、位置誤差ｑｘ、ｑｙが得られる。
【００７０】
この場合、誤差算出処理は、Ｆｓが最小になる節点の組合せとしてＱ２６にて更新された線分情報に基づき、新たな方位誤差θ、位置誤差ｑｘ、ｑｙが算出される。
【００７１】
上記Ｓ１３の誤差算出処理を終えると、Ｓ１４において、自車位置算出処理が行われる。この自車位置算出処理は、上述の誤差算出処理により得た方位誤差θ、位置誤差ｑｘ、ｑｙと、自車位置検出処理部９が検出した自車位置（ＧＰＳの検出位置）とを用いることにより、その自車位置検出処理部９が検出した自車位置を補正して、正確な自車１位置を算出するものであり、方位誤差θ、位置誤差ｑｘ、ｑｙ、さらには、ＧＰＳに基づく自車位置を（数１）に用いることにより、世界座標系の下での正確な自車位置が得られることになる。これにより、正確な自車１位置情報に基づき、車車間通信による衝突防止、地図データ、提供情報より得られる対象物と自車との距離を正確に算出して、適切な車両制御が行われる。
【００７２】
したがって、本実施形態においては、ありふれて存在する周辺物の複数の各節点（レーザレーダ３の検出に基づくもの）と、それらに対応する地図データ上の各節点とをそれぞれ対応付け、その複数組の対応する節点を用いることにより、誤差を算出することから、１組の対応する節点を用いる場合等に比べて、方位誤差θ、位置誤差ｑｘ、ｑｙの誤差算出精度を高めることができる。このため、車両１の位置補正精度を高めて、車両１の位置情報を精度の高いものにできる。
【００７３】
また、誤差算出に用いる対応する節点として、車両１周囲にありふれて存在する周辺物（多数存在）の節点を利用できることになり、位置補正の頻度を高めることができる。このため、当該車両１の上記高い位置情報精度を、常時、維持できる。
【００７４】
なお、上記実施形態においては、道路周辺物の節点を検出し、これと地図データとして記憶されている節点を直接、比較するものであったが、これに代えて、道路周辺の道路構造物の位置およびその形状情報を地図データに記憶させ、これらの位置と形状情報から道路周辺構造物のコーナや端点などの特徴点を節点として抽出し、レーザレーダにより検出された道路周辺の構造物の特徴点である節点とを比較するようにしても良い。
【００７５】
図２０は第２実施形態、図２１は第３実施形態を示す。この各実施形態において、前記第１実施形態と同一構成要素については同一符号を付してその説明を省略する。
【００７６】
図２０に示す第２実施形態は、三次元地図データとしてのボクセルＢの変形例を示す。前記第１実施形態においては、ボクセルＢを縦（道路の延びる方向）、横、高さが等しいもの（例えば１辺が１ｍ）が用いられていたが、本実施形態においては、走行道路に応じてボクセルＢの大きさが変えられている。
【００７７】
すなわち、市街道路（低速走行する場所）においては、ボクセルＢとして、縦、横、高さが等しいものが用いられる一方、高速道路や自専道の平均速度の速いところでは、ボクセルとしては、高さ＞縦＞横の関係をもって形成されたものが用いられる。例えば、高さに対して、縦が１／２，横が１／４のもの等が用いられる。また、郊外道路では、ボクセルとしては、高さ＞縦＝横の関係をもって形成されたものが用いられる。例えば、高さに対して、縦及び横が共に１／２のもの等が用いられる。これにより、ボクセルＢは、車両が高速走行する場合には、車幅方向、車両前後方向で小さい長さのボクセルによるデータが利用されることとなるため、データの更新速度が速くなり、車幅方向の車線逸脱や車両前後方向の衝突回避を精度よく実施することが出来る。
【００７８】
図２１に示す第３実施形態は、所定高さの対象物（信号機、標識、歩道橋等）２０の節点ｓを利用することにより自車１位置を算出する内容を示している。
【００７９】
すなわち、三次元地図データとしてのボクセルＢに、所定高さの対象物についての節点の位置情報をも含ませておく一方、レーザレーダ３によりその所定高さの対象物２０の節点ｓの位置を検出するようにすれば、レーザレーダ３による節点の検出が、周囲に存在する車両等１’の影響を受けなくなり、前述の検出方法に基づき自車１位置を的確に検出できることになる。尚、水平方向に対してαの角度をなす矢印は、レーザを示す。
【００８０】
以上実施形態について説明したが本発明にあっては、次の態様を包含する。
（１）レーザレーダ３に代えて、監視カメラ（例えばステレオカメラ）を用いること。
（２）自車１の方位については、実施形態においては、ＧＰＳセンサ４により検出する自車位置の変化を利用して処理ユニットＵ（進行方向検出部１０）が検出することになっているが、ジャイロセンサ等の方位センサを設けてもよいこと。
【符号の説明】
【００８１】
１ 車両（移動体）
２ 移動体位置検出装置
３ レーザレーダ（周辺物検出手段）
４ ＧＰＳセンサ（位置検出手段）
６ ナビゲーション装置（地図データ記憶手段）
７ 節点検出処理部（第１節点検出手段）
８ 遮蔽・空間判定処理部（第１節点検出手段）
９ 自車位置検出処理部（位置検出手段）
１１ 地図データ切出し等処理部（第２節点検出手段）
１２ 節点抽出及び遮蔽・空間判定処理部（第２節点検出手段）
１３ 節点マッチング処理部（第２節点検出手段）
１４ 自車位置／方位誤差算出処理部（誤差算出手段）
１５ 自車位置算出処理部（補正手段）
ｍ 地図データ上の節点
ｓ レーザレーダの検出データ上の節点
Ｕ 処理ユニット（第１節点検出手段、第２節点検出手段、誤差算出手段、補正手段）
【技術分野】
【０００１】
本発明は、移動体位置検出装置に関する。
【背景技術】
【０００２】
ＧＰＳ（Global Positioning System)を用いたナビゲーションシステムにおいては、ＧＰＳの測位誤差等を考慮し、表示装置における地図上の道路上に自車を表示するべく、特許文献１に示すように、走行軌跡と道路形状とによりマップマッチングが行われている。
【０００３】
ところで、近時、ナビゲーションシステムと移動体の走行制御等とを関連付けて移動体の運転支援を高めることが考えられている。このため、当該移動体の位置情報に関し、マップマッチングを行っている場合の誤差±２〜３ｍ程度の精度では足りず、位置情報精度をより高めることが望まれている。
【０００４】
これに対しては、特許文献２において、絶対位置が既知とされた外部の固定対象物に対する自己の相対位置を、検出手段により検出し、その検出結果に基づき、ＧＰＳ検出に基づく自己の位置を補正するものが提案されている。これにより、補正後の自己の位置情報の精度は高まる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−１０２０５２号公報
【特許文献２】特開２００７−２１８８４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、上記特許文献２に係るものにおいては、絶対位置が既知とされた外部の固定対象物の数自体が少なく、位置補正する頻度は低いものとならざるを得ない。このため、常時、高い位置情報精度を維持することはできない。
【０００７】
また、上記特許文献２に係るものは、１地点（絶対位置が既知とされた外部の固定対象物）に対する相対的距離を検出して、それを用いて誤差算出することにより、位置補正するものであることから、誤差算出精度は高いとはいえず、これに伴い、高い精度の位置情報自体を得ることは難しい。
【０００８】
本発明は、このような事情に鑑みてなされたもので、その技術的課題は、位置情報精度を高めると共に、常時、その高い位置情報精度を維持できる移動体位置検出装置を提供することにある。
【課題を解決するための手段】
【０００９】
前記技術的課題を達成するために本発明（請求項１に係る発明）においては、
移動体に搭載されて、該移動体の位置を検出する移動体位置検出装置において、
前記移動体の周囲における周辺物の節点の位置、又は周辺物の位置および形状を記憶する地図データ記憶手段と、
前記移動体の位置を検出する位置検出手段と、
前記移動体の周囲における周辺物を検出する周辺物検出手段と、
前記周辺物検出手段が検出した周辺物の複数の節点の相対的位置を検出する第１節点検出手段と、
前記第１節点検出手段が検出した各節点に対応する節点を、前記地図データ記憶手段が記憶する地図データからそれぞれ検出する第２節点検出手段と、
前記第１節点検出手段が検出した各節点と、該各節点に対応する前記第２節点検出手段が検出した各節点とに基づき、該第２節点検出手段が検出した節点に対する該第１節点検出手段が検出した節点の誤差を算出する誤差算出手段と、
前記誤差算出手段が算出した誤差に基づき、前記位置検出手段が検出した位置を補正する補正手段と、
を備えている、
ことを特徴とする移動体位置検出装置とした構成としてある。この請求項１の好ましい態様としては、請求項２以下の記載の通りとなる。
【発明の効果】
【００１０】
本発明（請求項１に係る発明）によれば、周辺物検出手段が検出する周辺物の複数の各節点と、地図データ上の各節点、又は周辺物の位置および形状から求められる各節点とをそれぞれ対応させ、その複数組の対応する節点を用いることにより、誤差を算出することから、１組の対応する節点を用いる場合に比べて、誤差算出の精度を高めることができる。このため、当該移動体の位置補正精度を高めることができ、当該移動体の位置情報に関し、その精度を高めることができる。
【００１１】
また、移動体搭載の周辺物検出手段が検出する周辺物の節点と、地図データ記憶手段が記憶する移動体周囲における当該周辺物の節点又は周辺物の位置および形状から求められる各節点と、を利用して両節点の誤差を算出し、その誤差に基づき、位置検出手段が検出した位置を補正することから、位置補正（誤差算出）に際して、移動体周囲にありふれて存在する周辺物を利用できることになり、位置補正の頻度を高めることができる。このため、当該移動体の位置情報に関し、常時、高い精度を維持できることになる。
【００１２】
請求項２に係る発明によれば、第２節点検出手段が、地図データ記憶手段が記憶する地図データから移動体の進行域における地図データを切り出す地図データ切出し処理部と、地図データ切出し処理部が切り出した地図データから周辺物の節点を抽出する節点抽出部と、第１節点検出手段が検出した各節点と節点抽出部が抽出した節点とを対応付ける節点マッチング処理部と、を備えていることから、具体的構成をもって、前記請求項１と同様の作用効果を得ることができる。
【００１３】
請求項３に係る発明によれば、節点マッチング処理部が、節点抽出部が抽出した節点間を結ぶ線分と、第１節点検出手段が検出する節点間を結ぶ線分とを比較することにより、第１節点検出手段が検出した各節点に対応する節点を、節点抽出部が抽出した節点から検出するように設定されていることから、節点間を結ぶ線分（リンクデータ）を利用することにより、第１節点検出手段が検出した各節点と、節点抽出部が抽出した節点とを的確に対応付けることができる。このため、誤差算出を確実に行うことができる。
【００１４】
請求項４に係る発明によれば、節点マッチング処理部は、第１節点検出手段が周辺物の複数の節点を検出する度に、第１節点検出手段が検出する節点間を結ぶ線分と節点抽出部が抽出した節点間を結ぶ線分とが一致するものを一組の一致線分として、その一致線分の差分の絶対値を順次、保存すると共に、その差分の絶対値の総和の正規化値を算出して、該正規化値が該正規化値の算出以前の正規化値よりも減少しているか否かを判別するように設定され、誤差算出手段は、節点マッチング処理部が正規化値の最小値を判定したときに限り、各組の一致線分における節点を節点マッチング処理部から受け入れて、誤差算出を行うように設定されていることから、全体として、一致線分の一致性が高い場合に限り（節点の対応性が高い場合に限り）、その新たな各組の一致線分の始点、終点をなす節点を用いて誤差処理がなされ、それ以外の場合には、誤差処理がなされず、位置検出手段が検出した位置を補正する際には、以前の対応性の高い節点が用いられる。このため、誤差算出の精度が低下して、当該移動体の高い位置情報精度が低下することを防止できる。
【００１５】
請求項５に係る発明によれば、第２節点検出手段が、さらに死角データ除去部を備え、死角データ除去部が、地図データ切出し部が切り出した地図データから、周辺物検出手段の死角領域に存在すると推定される死角データを除去するように設定されていることから、第１節点検出手段が検出した各節点と、節点抽出部が抽出した各節点とを対応付ける対応付け負荷（節点マッチング処理負荷）を軽減できる。
【００１６】
請求項６に係る発明によれば、地図データ記憶手段が、三次元空間を所定の大きさのボクセルに分割して、その各ボクセルの三次元位置座標と、その各ボクセルに含まれる前記周辺物種別の属性データと、該周辺物の節点の位置データと、を記憶していることから、移動体の進行域にありふれて存在する周辺物の複数の節点を誤差算出に利用できることになり、誤差算出精度を高めると共に、位置補正の頻度を高めることができる。このため、ボクセル地図データを用いることにより、具体的に、当該移動体の位置情報精度を高めると共に、常時、その高い位置情報精度を維持できる。
【００１７】
請求項７に係る発明によれば、前記地図データ記憶手段が、前記周辺物の位置データおよび形状情報を記憶し、前記節点が、前記周辺物の位置データおよび形状情報より求められることから、移動体の進行域にありふれて存在する周辺物の複数の節点を誤差算出に利用できることになり、誤差算出精度を高めると共に、位置補正の頻度を高めることができる。このため、地図データを用いることにより、具体的に、当該移動体の位置情報精度を高めると共に、常時、その高い位置情報精度を維持できる。
【００１８】
請求項８に係る発明によれば、ボクセルの大きさが、道路領域に配置するものに関し、道路種別に異なるように設定されていることから、ボクセルの大きさを、移動体の移動速度に応じたものにでき、移動体の移動速度が異なる場合でも、位置情報精度を高く維持できる。
【００１９】
請求項９に係る発明によれば、地図データ記憶手段が、所定高さ以上に位置する周辺物の節点の位置データを含み、周辺物検出手段が、所定高さ以上の周辺物を検出するように設定されていることから、他の移動体等の影響を受けることなく周辺物を検出して誤差算出を行うことができ、高い精度の移動体の位置情報を得ることができる。
【図面の簡単な説明】
【００２０】
【図１】第１実施形態に係る移動体位置検出装置を搭載した車両を平面的に示す図。
【図２】実施形態に係る移動体位置検出装置を示す構成図。
【図３】ボクセル地図データに基づく周辺物の節点と、それに対応するレーザレーダの検出データに基づく周辺部の節点との位置ずれを概念的に示す説明図。
【図４】ボクセルを概念的に説明する説明図。
【図５】レーザレーダの検出データにおける節点検出処理、遮蔽・空間判定処理を説明する説明図。
【図６】地図データにおける節点検出処理、遮蔽・空間判定処理を説明する説明図。
【図７】地図データの切り出しを説明する説明図。
【図８】地図データに基づく節点と、それに対応するレーザレーダの検出データに基づく節点とのマッチング処理を説明する説明図。
【図９】方位誤差をθ、位置誤差をｑｘ、ｑｙとした場合におけるＧＰＳ座標系と世界座標系との関係を示す説明図。
【図１０】節点のマッチング処理を説明する説明図。
【図１１】移動体位置検出装置の制御例を示すフローチャート。
【図１２】節点のマッチング処理例を示すフローチャート。
【図１３】図１２の続きを示すフローチャート。
【図１４】図１３の続きを示すフローチャート。
【図１５】ステップＱ１４の処理を説明する説明図。
【図１６】ステップＱ１３の処理を説明する説明図。
【図１７】ステップＱ１６の処理を説明する説明図。
【図１８】ステップＱ１８の処理を説明する説明図。
【図１９】ステップＱ２０の処理を説明する説明図。
【図２０】第２実施形態に係るボクセル地図データを概念的に示す説明図。
【図２１】第３実施形態を説明する説明図。
【発明を実施するための形態】
【００２１】
以下、本発明の実施形態について、図面に基づいて説明する。
【００２２】
図１〜図１９は第１実施形態を示す。この第１実施形態を示す図１において、符号１は移動体としての車両（自車）であり、この車両１には、自車位置、方位を精度良く検出できる実施形態に係る移動体位置検出装置２が搭載されている。この移動体位置検出装置２は、周辺物検出手段としてのレーザレーダ３と、位置検出手段としてのＧＰＳセンサ４と、地図データ記憶手段としてのナビゲーション装置６と、それらからの各種情報を入力情報として、自車１位置の検出処理を実行する処理ユニットＵ（図２参照）と、が備えられている。
【００２３】
前記レーザレーダ３は、レーザの照射及びその反射波の検出に基づいて、自車（車両）１周囲の周辺物の方位、相対的距離を検出するものである。図３は、車両前方における周辺物（周辺物を節点（丸印をもって示す）ｓ（１）・・・ｓ（nums）をもって示す）がレーザレーダ３により検出されたことを概念的に示している。
【００２４】
前記ＧＰＳセンサ４は、ＧＰＳ衛星からの信号を受信して自車１位置を検出するものである。これにより、自車１位置だけでなく、その検出された自車１位置を利用して、レーザレーダ３が検出した周辺物の位置をも算出することができることになる。この場合、自車1位置については、ＧＰＳに基づく測位誤差が生じ、周辺物の位置については、ＧＰＳ、レーザレーダ３の誤差等が生じることになる。
【００２５】
前記ナビゲーション装置６は、図４に示すボクセルＢを用いて、三次元地図データを記憶している。ボクセルＢは、三次元空間を所定の大きさ、例えば一辺１ｍの立方体に分割したものであり、その各ボクセルＢには、その三次元位置座標は勿論、その各ボクセルＢに含まれる物体、具体的には、電柱、ガードレール、電源ボックス等の節点（ノード又は端点）の正確な位置座標（世界座標系のもの）が位置データとして含まれている。図４は、概念的に、ボクセルＢ内に、ボクセルＢの中心位置データOと、物体の節点ｍの位置データとが存在することを示している。また、図３は、各ボクセルＢに自車の前方側における物体としての周辺物の節点ｍ（１）・・・ｍ（numm）の正確な位置情報が含まれ、その各ボクセルＢから周辺物の節点ｍ（１）・・・ｍ（numm）の位置情報が抽出可能であることを示している。
【００２６】
前記処理ユニットＵは、図２に示す構成を有しており、その検出処理に基づき、自車１位置を精度良く検出処理できることになっている。
【００２７】
処理ユニットＵは、図２に示すように、節点検出処理部７、遮蔽・空間判定処理部８、自車位置検出処理部９、進行方向検出処理部１０、地図データ切出し等処理部１１、節点抽出及び遮蔽・空間判定処理部１２を備えている。
【００２８】
節点検出処理部７には、レーザレーダ３からの出力信号（周辺物の方位、距離等）が入力されており、その節点検出処理部７においては、図３，図５に示すように、レーザレーダ３からの出力信号に基づき、自車１前方における周辺物（電柱、ガードレール、電源ボックス等）の節点（図３では白丸、図５では黒丸をもって示す）ｓ（１）・・・ｓ（nums）の検出処理が行われる。図３，図５において、節点ｓ（１）・・・ｓ（nums）の各間、節点ｍ（１）・・・ｍ（numm）の各間が線分をもって結ばれた状態が示されているが、この各線分は、節点間の部分をリンクとして規定したリンクデータになることを示している。図３中、符号Ｌmは、地図データに基づく節点間の線分の長さ、Ｌｓは、レーザレーダ３の検出結果に基づく節点間の線分の長さを示している。
【００２９】
遮蔽・空間判定処理部８には、節点検出処理部７からの出力信号が入力されており、その遮蔽・空間判定処理部８においては、判定すべき節点間が遮蔽状態か又は空間状態かの判定処理が行われる。具体的に図５をもって説明すれば、節点ｓ間のうち、周辺物が存在する部分（線分をもって結ばれている部分）が遮蔽状態と判定され、節点ｓ間のうち、周辺物が存在しない部分（線分をもって結ばれていない部分）が空間状態と判定されることになる。
【００３０】
自車位置検出処理部９には、前記ＧＰＳセンサ４からの出力信号が入力されており、自車位置検出処理部９においては、ＧＰＳセンサ４からの出力信号に基づき自車１位置が検出される。
【００３１】
進行方向検出処理部１０には、上記自車位置検出処理部９における自車位置データが入力されており、進行方向検出処理部１０においては、自車位置データに基づき自車１位置の変化を捉えて自車１の進行方向（方位）が検出される。
【００３２】
地図データ切出し等処理部１１には、前記ナビゲーション装置６からのボクセル三次元地図データ、前記自車位置検出処理部９からの自車位置データ、前記進行方向検出処理部１０からの自車の進行方向データがそれぞれ入力される。この地図データ切出し等処理部１１は、具体的には、地図データ切出し処理部、死角データ除去処理部を備えている。地図データ切出し処理部は、ナビゲーション装置６における地図データ（ボクセルデータ）から自車１位置周辺の地図データを切出す機能を有している。この場合、自車位置・方位推定誤差を考慮して、図７に示すように、地図データ切出し領域Ａｍは、レーザレーダ３の検出領域Ａｓよりも大きめに設定される。死角データ除去処理部は、自車位置・方位検出誤差を考慮して、想定されるいずれの自車位置・方位からも死角になる死角データを切出し地図データから削除する機能を有している。後述の図６中、×印を付されたものは、削除される死角データを示している。
【００３３】
節点抽出及び遮蔽・空間判定処理部１２には、前記地図データ切出し等処理部１１からの出力データが入力される。この節点抽出及び遮蔽・空間判定処理部１２は、切り出した地図データから自車１の進行域におけるガードレール１７，電柱１８，電源ボックス１９等の周辺物（図７参照）の節点（ノード又は端点）ｍを抽出する節点抽出部と、その節点抽出部が抽出した節点ｍ間の遮蔽・空間の判定処理を行う遮蔽・空間判定処理部と、を備えている。図６は、切り出された地図データから周辺物の節点ｍ（黒丸をもって示す）を抽出した状態を示しており、その節点m間のうち、周辺物が存在する部分（線分をもって結ばれている部分）が遮蔽状態と判定され、節点m間のうち、周辺物が存在しない部分（線分をもって結ばれていない部分）が空間状態と判定されることになる。
【００３４】
この場合、地図データに基づく節点ｍ（１）・・・ｍ（numm）は、レーザレーダの検出データに基づく節点ｓ（１）・・・ｓ（nums）に対して対応関係を有するものであるが、図３，図８に示すように、ＧＰＳ、レーザレーダ３等による誤差のため、両者は合致しないものとなる。
【００３５】
また、処理ユニットＵは、図２に示すように、節点マッチング処理部１３、自車位置／方位誤差算出処理部１４、自車位置算出処理部１５を備えている。節点マッチング処理部１３には、遮蔽・空間判定処理部８からの節点データと、節点抽出及び遮蔽・空間判定処理部１２からの節点データとが入力される。節点マッチング処理部１３は、図８に示すように、遮蔽・空間判定処理部８からの節点データと、節点抽出及び遮蔽・空間判定処理部１２からの節点データとに基づき、レーザレーダ３の検出データに基づく節点ｓに対応する節点を、地図データに基づく節点ｍから検出する処理である。この両節点ｍ、ｓの対応付けの検出に際しては、節点間の線分の長さを距離としてＤＰマッチング処理が行われ、そのとき、線分の角度（自車方位の推定誤差）、空間・遮蔽の状態、線分中の点群密度等が考慮され、レーザレーダ３により検出されない点を含む経路については削除される。
【００３６】
尚、図８中、両方向を向いた矢印は、レーザレーダ３の検出データに基づく線分と、それに対応する地図データに基づく線分の対比を示している。
【００３７】
自車位置／方位誤差算出処理部１４には、節点マッチング処理部１３から、対応関係があるとされるレーザレーダ３の検出データに基づく節点ｓ及び地図データに基づく節点ｍに関するデータが入力される。自車位置／方位誤差算出処理部１４は、レーザレーダ３の検出データに基づく各節点ｓと、それに対応する地図データに基づく各節点ｍとを（数１）に示す座標変換式に用いて、自車位置及び方位の誤差が算出する。
【００３８】
すなわち、ＧＰＳを用いて車両（自車）１位置を検出する場合、測位誤差を含んでおり、これに伴い、その車両からレーザレーダ３等を用いて周辺物の位置（距離、方位等）を検出したとしても、その周辺物の位置は、その車両の位置（ＧＰＳによる検出位置）を基礎として算出する限り（ＧＰＳ座標系の下では）、レーザレーダ３の誤差に加えて、ＧＰＳの誤差を含むことになる。
【００３９】
一方、誤差を含むＧＰＳ座標系で表される点（ｕｉ，ｖｉ）と、それに対応する世界座標系の点（ｘｉ，ｙｉ）との間には、ＧＰＳ座標系で表される点（ｕｉ，ｖｉ）の方位誤差をθ、位置誤差をｑｘ、ｑｙとすると、（数１）に示す一般的な座標変換式が成立する。この（数１）に示す関係を図式的に示せば、図９に示す通りである。
【００４０】
【数１】
【００４１】
このため、ｎ個の物標の位置（ＧＰＳ座標系）が車両搭載のレーザレーダ３により検出されたときには、それらと、そのｎ個の物標の地図データに基づく位置（世界座標系）とを、（数１）に用いて、（数２）に示す方程式群を得ることができる。
【００４２】
【数２】
【００４３】
この（数２）に示す方程式群は、（数３）の行列として置くことができる。
【００４４】
【数３】
【００４５】
ここで、Ａの転置行列をtＡとすると、tＡＡに逆行列が存在する場合、（数３）は、（tＡＡ）-1ＡＢ＝（tＡＡ）-1Ｃ ，（tＡ）-1Ｂ＝（tＡＡ）-1Ｃ，tＡ（tＡ）-1Ｂ＝tＡ（tＡＡ）-1Ｃを経て、（数４）を得ることができる。
【００４６】
【数４】
【００４７】
このように、自車位置／方位誤差算出処理部１４では、上記（数４）を解く処理が行われ、方位誤差θ、位置誤差ｑｘ、ｑｙが算出される。尚、（数４）中のtＡＡ、tＡＣを展開すると、（数５）となる。
【００４８】
【数５】
【００４９】
自車位置算出処理部１５には、上記自車位置／方位誤差算出処理部１４から、方位誤差θ、位置誤差ｑｘ、ｑｙの各情報が入力されると共に、自車位置検出処理部９から自車位置情報（ＧＰＳによる検出）が入力される。自車位置算出処理部１５は、その各情報を（数１）に用いて演算し、これにより、位置精度の高い自車１位置を得る。
【００５０】
この精度の高い自車位置情報は、自車位置算出処理部１５から利用対象１６に出力されて種々のものに利用される。例えば、各車両１の正確な位置情報を車車間通信することによって追突を防止したり、地図データより求める対象物（停止線位置、交差点位置、死角域のガードレール等）と自車１との正確な距離を算出して適正な車両制御を行うことが行われる。
【００５１】
次に、前記処理ユニットＵの制御内容を、図１１に示すフローチャートに基づいて具体的に説明する。尚、図１１における符号Ｓは、ステップを示す。
【００５２】
Ｓ１において、ＧＰＳを利用して、自車位置及び方位が検出され、この後、Ｓ２におけるエゴモーションによる補間を経て、Ｓ３において、図７に示すように、自車位置周辺の地図データの切出しが行われる。次のＳ５において、その地図データ中から、死角データが除去される。この死角データを除去するに際しては、自車位置・方位の検出誤差を考慮して、想定されるいずれの自車位置・方位からも死角になる死角データのみが削除される（図６の×印参照）。
【００５３】
この後、図６に示すように、Ｓ６において、切り出した地図データ（各ボクセルＢ）から周辺物の節点ｍが抽出され、次のＳ７において、その各隣り合う節点ｍ間が遮蔽状態にあるか、又は空間状態にあるかが判定される。
【００５４】
次に、Ｓ８において、レーザレーダ３により周辺物が検出され、次のＳ１０において、図５に示すように、周辺物の節点ｓが抽出され、その次のＳ１１において、各隣り合う節点ｓ間が遮蔽状態にあるか又は空間状態にあるかが判定される。この後、Ｓ１２において、地図データに基づく節点ｍと、レーザレーダ３の検出データに基づく節点ｓとのマッチング処理（対応付け）が行われる。
【００５５】
上記節点のマッチング処理について、図１０、図１２〜図１９に基づいて具体的に説明する。尚、図１２〜図１４における符号Ｑは、ステップを示す。
【００５６】
地図データに基づく節点ｍは世界測地系座標系あるいは平面直角座標系（上述の世界座標系）で表されている。一方、レーザレーダ３の検出データに基づく節点ｓは車両１を基準とした直角座標系（上述のＧＰＳ座標系）で表されている。地図データに基づく節点ｍと、レーザレーダ３の検出データに基づく節点ｓとのマッチング処理を行うため、Ｑ０において、世界測地系座標系あるいは平面直角座標系（世界座標系）で表されている地図データに基づく節点ｍの座標を、ＧＰＳで検出した車両１の位置座標に基づき、車両１を基準とした直角座標系（ＧＰＳ座標系）に変換する。
【００５７】
上記、座標変換処理は、車両１を基準とした直角座標系（ＧＰＳ座標系）で表されているレーザレーダ３の検出データに基づく節点ｓを、ＧＰＳで検出した車両１の位置座標に基づき、世界測地系座標系あるいは平面直角座標系（世界座標系）に変換しても良い。
【００５８】
地図データに基づく節点ｍと、レーザレーダ３の検出データに基づく節点ｓとのマッチング処理は、地図データに基づく節点ｍ同士を結んで線分（リンクデータ）を形成すると共に、レーザレーダ３の検出データに基づく節点ｓ同士を結んで線分を形成し、その両者の線分の比較を通じて、対応付けが行われる。このため、先ず、Ｑ１において、線分の初期値設定が行われる。具体的には、図１０に示すように、地図データに基づく節点ｍであって、自車１左側において自車１に最も近い節点ｍを線分Ｌｍの始点ｓｍとして捉え、その始点ｓｍをｓｍ←１に設定すると共に、そのｍ（１）を基準にして、図１０中、時計方向に隣り合う節点ｍを線分Lmの終点ｅｍとして捉え、ｅｍ←ｓｍ＋１（＝２）と設定する。同様に、レーザレーダ３の検出データに基づく節点ｓであって、自車１左側において自車１に最も近い節点ｓを線分Ｌｓの始点ｓｓとして捉え、そのｓｓをｓｓ←１に設定すると共に、そのｓ（１）を基準にして、図１０中、時計方向に隣り合う節点ｓを線分Lｓの終点ｅｓとして捉え、ｅｓ←ｓｓ＋１（＝２）と設定する。
【００５９】
次に、Ｑ２，Ｑ３，Ｑ４において、評価値Ｆの最小値Fmin、評価値F、一致した線分数linknumが、順次、初期化される。具体的には、Ｑ２において、Fmin←−１と設定され、Ｑ３において、F←０と設定され、Ｑ４において、linknum←０と設定される。
【００６０】
Ｑ４の処理を終えると、地図データの節点ｍ同士を結んだ線分Lm(sm,em)と、レーザレーダ３の検出データの節点同士を結んだ線分Ls(ss,es)とが一致するか否かの処理が、自車１の左側に一番近いものから図１０中、時計方向（矢印方向）に向けて順次、行われる。具体的には、先ず最初に、節点ｍ（１）と節点ｍ（２）との間のLm(１,２)と、節点ｓ（１）と節点ｓ（２）との間のLs(１,２)とについて、両線分の状態（空間状態又は遮蔽状態）の同一性、点群密度の差、長さの差、傾きの差の観点から、線分の一致性が判定されることになる。
【００６１】
より具体的に説明すれば、Ｑ５において、線分Lm(１,２)及び線分Ls(１,２)が、空間状態であるか又は遮蔽状態であるかの点で同一か否かが判別され、このＱ５の判別がＮＯのときには、同一でないとして、Ｑ１４に進み、そこにおいて、線分の更新として、図１５に示すように、地図データに基づく線分Lm(sm,em)の終点emはem←em+1、すなわち、Lm(１,３)に更新される。そして、このＱ１４における更新後、Ｑ１５に進み、そのＱ１５において、地図データに基づく線分Lm(１,３)の終点em（＝３）が、切り出した地図データ中から抽出した節点ｍの個数nummを越えたか否かが判別される。このＱ１５の判別がＮＯのときには、線分Lm(１,３)の終点em（＝３）が地図データ中から抽出した節点ｍの個数nummに達しておらず、未だ前述の処理を行う必要があるとして、前記Ｑ５に戻され、このＱ１５の判別がＹＥＳのときには、Ｑ１６に進む。
【００６２】
前記Ｑ５がＹＥＳのときには、Ｑ６において、線分Lm(sm,em)とLs(ss,es)中の点群密度の差、すなわち、Lm(１,２)とLs(１,２)との点群密度の差が所定値以下か否かが判別される。そのＱ６がＮＯのときには、点群密度の観点から両線分Lm(１,２)、Ls(１,２)が同一でないとして前記Ｑ１４に進み、Ｑ６がＹＥＳのときには、Ｑ７において、線分Lm(sm,em)とLs(ss,es)との長さの差、すなわちLm(１,２)とLs(１,２)との長さの差が所定値以下か否かが判別される。このＱ７がＮＯのときには、線分の長さの観点から両線分Lm(１,２)、Ls(１,２)が一致しないとして前記Ｑ１４に進み、Ｑ７がＹＥＳのときには、Ｑ８において、線分Lm(sm,em)とLs(ss,es)の傾きの差、すなわちLm(１,２)とLs(１,２)との傾きの差が所定値以下か否かが判別される。このＱ８がＮＯのときには、傾きの観点から、両線分Lm(１,２)、Ls(１,２)が一致しないとして前記Ｑ１４に進む一方、Ｑ８がＹＥＳのときには、両線分Lm(１,２)、Ls(１,２)が一致するとして（一組の一致線分）、Ｑ９において、両線分の差分の絶対値ｄ（Lm(sm,em),Ls(ss,es))、すなわちｄ（Lm(１,２),Ls(１,２))を評価値Fに加算したものを評価値Fとする。この場合、上記Ｑ６〜Ｑ８における各所定値には、両線分Lm(sm,em)、Ls(ss,es)が一致するとして許容できるものが適宜設定される。
【００６３】
次いで、Ｑ１０において、一致した線分数linknum（当初は０）に１が加算されて、それが一致した線分数linknumとされ、次のＱ１１においては、線分が一致する毎に評価値Fが保存される。次のＱ１２においては、一致した線分情報を保存すべく、地図データと一致したレーザレーダ３検出に基づく線分の始点ssがstrss(linknum)、地図データと一致したレーザレーダ３検出に基づく線分の終点esがstres(linknum)、レーザレーダ３の検出データと一致した地図データの線分の始点smがstrsm(linknum)、レーザレーダ３の検出データと一致した地図データの線分の終点emがstrem(linknum)としてそれぞれ保存される（線分が１本一致する毎に一致した線分の対応関係の情報を評価値メモリに保存）。
【００６４】
そして、次のＱ１３においては、図１６に示すように、新たな線分同士で比較すべく、地図データに基づく線分Lm(sm,em)の始点smが終点em位置に移動され（sm←em)、線分Lm(sm,em)の終点emが新たな始点emの次の点（節点）に移動される(em←sm+1)。レーザレーダ３の検出データに基づく線分Ls(ss,es)についても、同様に、線分Ls(ss,es)の始点ssが終点es位置に移動され（ss←es)、線分Ls(ss,es)の終点esが新たな始点esの次の点（節点）に移動される(es←ss+1)。このＱ１３の処理を終えると、前記Ｑ１５に進み、地図データに基づく比較すべき線分Lm(sm,em)の終点emが、切出した地図データの最後（地図データに基づく節点の個数numm） に達しないときには、同じ処理を繰り返すべく、前記Ｑ５に戻され、その線分Lm(sm,em)の終点emが、切出した地図データの最後に達したときには、Ｑ１６に進む。
【００６５】
Ｑ１６においては、図１７に示すように、地図データに基づく線分Lm(sm,em)の終点がデータの最後に達したときにレーザレーダ３の検出データに基づく新たな線分Ls(ss,es)で比較すべく、線分Ls(ss,es)の終点esが次の節点に移動され(es←es+1)、その新たな線分Ls(ss,es)と比較すべき線分Lm(sm,em)の終点emは、始点smの次の節点に移動される(em←sm+1)。このＱ１６の処理を終えると、Ｑ１７において、レーザレーダ３の検出データに基づく線分Ls(ss,es)の終点esが、そのレーザレーダ３の検出データの最後（レーザレーダ３の検出データに基づく節点の個数nums）に達したか否かが判別される。このＱ１７の判別がＮＯのときには、同じ処理を繰り返すべく、前記Ｑ５に戻され、Ｑ１７がＹＥＳのときには、Ｑ１８において、図１８に示すように、地図データに基づく線分Lm(sm,em)の始点smが次の節点に移動され(sm←sm+1)、その線分Lm(sm,em)の終点emは新たな始点sm次の節点に移動される（em←sm+1)。このＱ１８の処理を終えると、Ｑ１９において、地図データに基づく線分Lm(sm,em)の始点smが地図データの最後(numm)から２個目に達したか否かが判別される。このＱ１９がＮＯのときには、同じ処理を繰り返すべく、前記Ｑ５に戻される一方、Ｑ１９がＹＥＳのときには、Ｑ２０において、図１９に示すように、レーザレーダ３の検出データに基づく線分Ls(ss,es)の始点ssが、次の節点に移動され(ss←ss+1)、その線分Ls(ss,es)の終点esが新たな始点ssの次の節点に移動される(es←ss+1)。また、地図データに基づく線分Lm(sm,em)の始点smは、最後に一致した地図データに基づく線分Lm(sm,em)の終点に移動され(sm←strsm(linknum))、その地図データに基づく線分Lm(sm,em)の終点emはその新たな始点smの次の節点に移動される(em←sm+1)。
【００６６】
上記Ｑ２０の処理を終えると、Ｑ２１において、レーザレーダ３の検出データに基づく線分Ls(ss,es)の始点ssが、その検出データの最後(節点の個数nums)から２個目に達したか否かが判別される。このＱ２１がＮＯのときには、同じ処理を繰り返すべく、前記Ｑ５に戻される一方、Ｑ２１がＹＥＳのときには、Ｑ２２において、両線分の差分の絶対値を加算した評価値Ｆを、一致した線分数linknumで除し、F/linknumを算出して、線分数に依存することのない一致した線分1本あたりの評価値を求め（すなわち、一致した線分数で正規化する）、このF/linknumが正規化した評価値Fsとして読み込まれる。なお、上記の例は、評価値Ｆを一致した線分数linknumで除して正規化した評価値Fsとしたが、両線分の差分の値を２乗した上で相加平均し平方根をとった二乗平均平方根（RMS (Root Mean Square)）を正規化した評価値Fsとしても良い。
【００６７】
Ｑ２２の処理を終えると、Ｑ２３において、Fmin=−１又はFs<Fminであるか否かが判別される。Fsが評価値の最小値Fminよりも小さい場合には、一致している線分の対応関係の情報を更新するためである。この場合、最初は、Fmin=−１より必ずＹＥＳとなって、次のＱ２４に進み、２回目以降はFs<Fminであれば、次のＱ２４に進むことになる。Ｑ２３がＹＥＳのときには、Ｑ２４において、評価値の最小値FminがＱ２２のFsとされた上で、Ｑ２５において、一致した線分数が更新され(linknummin←linknum)、Ｑ２６において、一致した線分情報が更新される(strssmin(1,・・,linknum)←strss(1,・・,linknum),stresmin(1,・・,linknum)←stres(1,・・,linknum),strsmmin(1,・・,linknum)←strsm(1,・・,linknum),stremmin(1,・・,linknum)←strem(1,・・,linknum))。
【００６８】
Ｑ２３がＮＯと判定された場合又はＱ２６の処理を終えた場合には、Ｑ２７に進み、そのＱ２７において、一致した線分の数linknumが１を越えているか否かが判別される。このＱ２７がＮＯのときには、リターンされる一方、Ｑ２７がＹＥＳのときには、一致したとしてＱ１２において記憶している線分以降にさらに、近似するものがないかをチェックするべく、Ｑ２８〜Ｑ３０において、記憶しているss,es,sm,emを読み出し、そこを始点、終点として、さらに近似する線分がないかをチェックすることになる。このため、Ｑ２８では一致したとして記憶し、それ以降のチェックが行われていないss,es,sm,emを読み出し、チェックを開始する線分が更新され、Ｑ２９では一致した線分数から1本が減算され、Ｑ３０においてＱ１１で一旦、記憶された評価値Fが読み出され、その後、前記Ｑ３に戻されることで、ss,es,sm,emを始点、終点とする線分以降に対して、上記と同様にＱ５からＱ８によって線分の一致度がチェックされ、上記読み出された評価値Ｆより、更に小さい評価値が検出された場合は、Ｑ２６にてその線分情報が更新記憶される。
【００６９】
上記節点のマッチング処理を終えると、Ｓ１３において、自車位置・方位の誤差算出処理が行われる。この誤差算出処理は、前述した如く、Ｓ１２の節点マッチング処理により得た地図データに基づく複数の節点ｍと、それに対応するレーザレーダ３の検出データに基づく各節点ｓと車両情報を、（数２）〜（数５）に用いることにより算出される処理であり、この誤差算出処理により、方位誤差θ、位置誤差ｑｘ、ｑｙが得られる。
【００７０】
この場合、誤差算出処理は、Ｆｓが最小になる節点の組合せとしてＱ２６にて更新された線分情報に基づき、新たな方位誤差θ、位置誤差ｑｘ、ｑｙが算出される。
【００７１】
上記Ｓ１３の誤差算出処理を終えると、Ｓ１４において、自車位置算出処理が行われる。この自車位置算出処理は、上述の誤差算出処理により得た方位誤差θ、位置誤差ｑｘ、ｑｙと、自車位置検出処理部９が検出した自車位置（ＧＰＳの検出位置）とを用いることにより、その自車位置検出処理部９が検出した自車位置を補正して、正確な自車１位置を算出するものであり、方位誤差θ、位置誤差ｑｘ、ｑｙ、さらには、ＧＰＳに基づく自車位置を（数１）に用いることにより、世界座標系の下での正確な自車位置が得られることになる。これにより、正確な自車１位置情報に基づき、車車間通信による衝突防止、地図データ、提供情報より得られる対象物と自車との距離を正確に算出して、適切な車両制御が行われる。
【００７２】
したがって、本実施形態においては、ありふれて存在する周辺物の複数の各節点（レーザレーダ３の検出に基づくもの）と、それらに対応する地図データ上の各節点とをそれぞれ対応付け、その複数組の対応する節点を用いることにより、誤差を算出することから、１組の対応する節点を用いる場合等に比べて、方位誤差θ、位置誤差ｑｘ、ｑｙの誤差算出精度を高めることができる。このため、車両１の位置補正精度を高めて、車両１の位置情報を精度の高いものにできる。
【００７３】
また、誤差算出に用いる対応する節点として、車両１周囲にありふれて存在する周辺物（多数存在）の節点を利用できることになり、位置補正の頻度を高めることができる。このため、当該車両１の上記高い位置情報精度を、常時、維持できる。
【００７４】
なお、上記実施形態においては、道路周辺物の節点を検出し、これと地図データとして記憶されている節点を直接、比較するものであったが、これに代えて、道路周辺の道路構造物の位置およびその形状情報を地図データに記憶させ、これらの位置と形状情報から道路周辺構造物のコーナや端点などの特徴点を節点として抽出し、レーザレーダにより検出された道路周辺の構造物の特徴点である節点とを比較するようにしても良い。
【００７５】
図２０は第２実施形態、図２１は第３実施形態を示す。この各実施形態において、前記第１実施形態と同一構成要素については同一符号を付してその説明を省略する。
【００７６】
図２０に示す第２実施形態は、三次元地図データとしてのボクセルＢの変形例を示す。前記第１実施形態においては、ボクセルＢを縦（道路の延びる方向）、横、高さが等しいもの（例えば１辺が１ｍ）が用いられていたが、本実施形態においては、走行道路に応じてボクセルＢの大きさが変えられている。
【００７７】
すなわち、市街道路（低速走行する場所）においては、ボクセルＢとして、縦、横、高さが等しいものが用いられる一方、高速道路や自専道の平均速度の速いところでは、ボクセルとしては、高さ＞縦＞横の関係をもって形成されたものが用いられる。例えば、高さに対して、縦が１／２，横が１／４のもの等が用いられる。また、郊外道路では、ボクセルとしては、高さ＞縦＝横の関係をもって形成されたものが用いられる。例えば、高さに対して、縦及び横が共に１／２のもの等が用いられる。これにより、ボクセルＢは、車両が高速走行する場合には、車幅方向、車両前後方向で小さい長さのボクセルによるデータが利用されることとなるため、データの更新速度が速くなり、車幅方向の車線逸脱や車両前後方向の衝突回避を精度よく実施することが出来る。
【００７８】
図２１に示す第３実施形態は、所定高さの対象物（信号機、標識、歩道橋等）２０の節点ｓを利用することにより自車１位置を算出する内容を示している。
【００７９】
すなわち、三次元地図データとしてのボクセルＢに、所定高さの対象物についての節点の位置情報をも含ませておく一方、レーザレーダ３によりその所定高さの対象物２０の節点ｓの位置を検出するようにすれば、レーザレーダ３による節点の検出が、周囲に存在する車両等１’の影響を受けなくなり、前述の検出方法に基づき自車１位置を的確に検出できることになる。尚、水平方向に対してαの角度をなす矢印は、レーザを示す。
【００８０】
以上実施形態について説明したが本発明にあっては、次の態様を包含する。
（１）レーザレーダ３に代えて、監視カメラ（例えばステレオカメラ）を用いること。
（２）自車１の方位については、実施形態においては、ＧＰＳセンサ４により検出する自車位置の変化を利用して処理ユニットＵ（進行方向検出部１０）が検出することになっているが、ジャイロセンサ等の方位センサを設けてもよいこと。
【符号の説明】
【００８１】
１ 車両（移動体）
２ 移動体位置検出装置
３ レーザレーダ（周辺物検出手段）
４ ＧＰＳセンサ（位置検出手段）
６ ナビゲーション装置（地図データ記憶手段）
７ 節点検出処理部（第１節点検出手段）
８ 遮蔽・空間判定処理部（第１節点検出手段）
９ 自車位置検出処理部（位置検出手段）
１１ 地図データ切出し等処理部（第２節点検出手段）
１２ 節点抽出及び遮蔽・空間判定処理部（第２節点検出手段）
１３ 節点マッチング処理部（第２節点検出手段）
１４ 自車位置／方位誤差算出処理部（誤差算出手段）
１５ 自車位置算出処理部（補正手段）
ｍ 地図データ上の節点
ｓ レーザレーダの検出データ上の節点
Ｕ 処理ユニット（第１節点検出手段、第２節点検出手段、誤差算出手段、補正手段）
【特許請求の範囲】
【請求項１】
移動体に搭載されて、該移動体の位置を検出する移動体位置検出装置において、
前記移動体の周囲における周辺物の節点の位置、又は周辺物の位置および形状を記憶する地図データ記憶手段と、
前記移動体の位置を検出する位置検出手段と、
前記移動体の周囲における周辺物を検出する周辺物検出手段と、
前記周辺物検出手段が検出した周辺物の複数の節点の相対的位置を検出する第１節点検出手段と、
前記第１節点検出手段が検出した各節点に対応する節点を、前記地図データ記憶手段が記憶する地図データからそれぞれ検出する第２節点検出手段と、
前記第１節点検出手段が検出した各節点と、該各節点に対応する前記第２節点検出手段が検出した各節点とに基づき、該第２節点検出手段が検出した節点に対する該第１節点検出手段が検出した節点の誤差を算出する誤差算出手段と、
前記誤差算出手段が算出した誤差に基づき、前記位置検出手段が検出した位置を補正する補正手段と、
を備えている、
ことを特徴とする移動体位置検出装置。
【請求項２】
請求項１において、
前記第２節点検出手段が、前記地図データ記憶手段が記憶する地図データから前記移動体の進行域における地図データを切り出す地図データ切出し処理部と、該地図データ切出し処理部が切り出した地図データから周辺物の節点を抽出する節点抽出部と、前記第１節点検出手段が検出した各節点と前記節点抽出部が抽出した節点とを対応付ける節点マッチング処理部と、を備えている、
ことを特徴とする移動体位置検出装置。
【請求項３】
請求項２において、
前記節点マッチング処理部が、前記節点抽出部が抽出した節点間を結ぶ線分と、前記第１節点検出手段が検出する節点間を結ぶ線分とを比較することにより、該第１節点検出手段が検出した各節点に対応する節点を、該節点抽出部が抽出した節点から検出するように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項４】
請求項３において、
前記節点マッチング処理部は、前記第１節点検出手段が周辺物の複数の節点を検出する度に、該第１節点検出手段が検出する節点間を結ぶ線分と前記節点抽出部が抽出した節点間を結ぶ線分とが一致するものを一組の一致線分として、その一致線分の差分の絶対値を順次、保存すると共に、その差分の絶対値の総和の正規化値を算出して、該正規化値が該正規化値の算出以前の正規化値よりも減少しているか否かを判別するように設定され、
前記誤差算出手段は、前記節点マッチング処理部が前記正規化値の最小値を判定したときに限り、前記各組の一致線分における節点を前記節点マッチング処理部から受け入れて、誤差算出を行うように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項５】
請求項２において、
前記第２節点検出手段が、さらに死角データ除去部を備え、
前記死角データ除去部が、前記地図データ切出し部が切り出した地図データから、前記周辺物検出手段の死角領域に存在すると推定される死角データを除去するように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項６】
請求項１において、
前記地図データ記憶手段が、三次元空間を所定の大きさのボクセルに分割して、その各ボクセルの三次元位置座標と、その各ボクセルに含まれる前記周辺物種別の属性データと、該周辺物の節点の位置データと、を記憶している、
ことを特徴とする移動体位置検出装置。
【請求項７】
請求項１において、
前記地図データ記憶手段が、前記周辺物の位置データおよび形状情報を記憶し、前記節点が、前記周辺物の位置データおよび形状情報より求められる、
ことを特徴とする移動体位置検出装置。
【請求項８】
請求項６において、
前記ボクセルの大きさが、道路領域に配置するものに関し、道路種別に異なるように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項９】
請求項１において、
前記地図データ記憶手段が、所定高さ以上に位置する周辺物の節点の位置データを含み、
前記周辺物検出手段が、所定高さ以上の周辺物を検出するように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項１】
移動体に搭載されて、該移動体の位置を検出する移動体位置検出装置において、
前記移動体の周囲における周辺物の節点の位置、又は周辺物の位置および形状を記憶する地図データ記憶手段と、
前記移動体の位置を検出する位置検出手段と、
前記移動体の周囲における周辺物を検出する周辺物検出手段と、
前記周辺物検出手段が検出した周辺物の複数の節点の相対的位置を検出する第１節点検出手段と、
前記第１節点検出手段が検出した各節点に対応する節点を、前記地図データ記憶手段が記憶する地図データからそれぞれ検出する第２節点検出手段と、
前記第１節点検出手段が検出した各節点と、該各節点に対応する前記第２節点検出手段が検出した各節点とに基づき、該第２節点検出手段が検出した節点に対する該第１節点検出手段が検出した節点の誤差を算出する誤差算出手段と、
前記誤差算出手段が算出した誤差に基づき、前記位置検出手段が検出した位置を補正する補正手段と、
を備えている、
ことを特徴とする移動体位置検出装置。
【請求項２】
請求項１において、
前記第２節点検出手段が、前記地図データ記憶手段が記憶する地図データから前記移動体の進行域における地図データを切り出す地図データ切出し処理部と、該地図データ切出し処理部が切り出した地図データから周辺物の節点を抽出する節点抽出部と、前記第１節点検出手段が検出した各節点と前記節点抽出部が抽出した節点とを対応付ける節点マッチング処理部と、を備えている、
ことを特徴とする移動体位置検出装置。
【請求項３】
請求項２において、
前記節点マッチング処理部が、前記節点抽出部が抽出した節点間を結ぶ線分と、前記第１節点検出手段が検出する節点間を結ぶ線分とを比較することにより、該第１節点検出手段が検出した各節点に対応する節点を、該節点抽出部が抽出した節点から検出するように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項４】
請求項３において、
前記節点マッチング処理部は、前記第１節点検出手段が周辺物の複数の節点を検出する度に、該第１節点検出手段が検出する節点間を結ぶ線分と前記節点抽出部が抽出した節点間を結ぶ線分とが一致するものを一組の一致線分として、その一致線分の差分の絶対値を順次、保存すると共に、その差分の絶対値の総和の正規化値を算出して、該正規化値が該正規化値の算出以前の正規化値よりも減少しているか否かを判別するように設定され、
前記誤差算出手段は、前記節点マッチング処理部が前記正規化値の最小値を判定したときに限り、前記各組の一致線分における節点を前記節点マッチング処理部から受け入れて、誤差算出を行うように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項５】
請求項２において、
前記第２節点検出手段が、さらに死角データ除去部を備え、
前記死角データ除去部が、前記地図データ切出し部が切り出した地図データから、前記周辺物検出手段の死角領域に存在すると推定される死角データを除去するように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項６】
請求項１において、
前記地図データ記憶手段が、三次元空間を所定の大きさのボクセルに分割して、その各ボクセルの三次元位置座標と、その各ボクセルに含まれる前記周辺物種別の属性データと、該周辺物の節点の位置データと、を記憶している、
ことを特徴とする移動体位置検出装置。
【請求項７】
請求項１において、
前記地図データ記憶手段が、前記周辺物の位置データおよび形状情報を記憶し、前記節点が、前記周辺物の位置データおよび形状情報より求められる、
ことを特徴とする移動体位置検出装置。
【請求項８】
請求項６において、
前記ボクセルの大きさが、道路領域に配置するものに関し、道路種別に異なるように設定されている、
ことを特徴とする移動体位置検出装置。
【請求項９】
請求項１において、
前記地図データ記憶手段が、所定高さ以上に位置する周辺物の節点の位置データを含み、
前記周辺物検出手段が、所定高さ以上の周辺物を検出するように設定されている、
ことを特徴とする移動体位置検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【公開番号】特開２０１１−１９１２３９（Ｐ２０１１−１９１２３９Ａ）
【公開日】平成２３年９月２９日（２０１１．９．２９）
【国際特許分類】
【出願番号】特願２０１０−５８９８８（Ｐ２０１０−５８９８８）
【出願日】平成２２年３月１６日（２０１０．３．１６）
【出願人】（０００００３１３７）マツダ株式会社 (6,115)
【Ｆターム（参考）】
【公開日】平成２３年９月２９日（２０１１．９．２９）
【国際特許分類】
【出願日】平成２２年３月１６日（２０１０．３．１６）
【出願人】（０００００３１３７）マツダ株式会社 (6,115)
【Ｆターム（参考）】
[ Back to top ]