JP4851874B2 - 自己位置推定プログラム、自己位置推定方法および自己位置推定装置 - Google Patents

Description

この発明は、自己位置推定プログラム、自己位置推定方法および自己位置推定装置に関する。

従来より、移動体の自己位置を推定する自己位置推定装置が知られている。このような自己位置推定装置では、移動体に搭載されているカメラにより撮影された画像から複数個のランドマークの位置情報を検索し、移動体に搭載されているカメラを中心とした各ランドマーク間の挟角を算出し、これをもとに位置標定原理である三角法を用いた演算処理を行い、自己位置を推定する。すなわち、このような自己位置推定装置では、２つのランドマークの位置情報と挟角から、自己位置が存在する可能性のある拘束円を算出し、このような拘束円を別の組み合わせの２つのランドマークにおいても算出し、２つ以上の拘束円の交点をもって自己位置と推定する（例えば、特許文献１や特許文献２を参照）。

ここで、自己位置推定装置は、ランドマーク間の挟角の算出に際しては、画像上のランドマークの代表ピクセルを選択し、そのピクセル間の距離を、カメラ素子や光学レンズのパラメータ情報を利用して、角度に変換することでランドマーク間の挟角を算出する。そして、自己位置推定装置では、代表ピクセルの選択を計算機に自動的に決定させることが高度な認識アルゴリズムを用いても非常に困難であるため、検出されたランドマーク画像領域の中心値を代表ピクセルとして簡便に利用することが一般的である。また、自己位置推定装置では、代表ピクセルから算出された挟角に基づき算出された２つ以上の拘束円の交点の算出に際しては、最小二乗法などの手法により算出する。

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

ところで、上記した従来の技術では、自己位置推定の精度が悪いという問題点があった。すなわち、上記した従来技術では、必然的に幅のあるランドマークの画像から中心値を代表ピクセルとして選択して挟角を算出し、拘束円を算出することから、代表ピクセルの選択を誤りやすく、自己位置推定の精度が悪くなるという問題点があった。

そして、２つ以上の拘束円の交点の算出には、最小二乗法などの解析的な方法が用いられているが、算出された挟角の精度が悪ければ、いかなる解析的な方法を用いても自己位置推定の精度は向上せず、依然として自己位置推定の精度向上にはならなかった。

また、自己位置の推定には、冗長な複数個ペアのランドマークから算出された複数個の拘束円が用いられているが、精度の悪い挟角から定まる拘束円が混入する確率が増えれば、ランドマークの組み合わせを増やしても自己位置推定の精度は向上せず、依然として自己位置推定の精度向上にはならなかった。

そこで、この発明は、上述した従来技術の課題を解決するためになされたものであり、自己位置推定精度を向上させることが可能な自己位置推定プログラム、自己位置推定方法および自己位置推定装置を提供することを目的とする。

上述した問題を解決し、目的を達成するため、請求項１に係る発明は、入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から前記自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定する自己位置推定方法をコンピュータに実行させる自己位置推定プログラムであって、前記複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する挟角算出手順と、前記複数のランドマーク間ごとに、前記挟角算出手順によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する拘束円算出手順と、前記複数のランドマーク間ごとに、前記拘束円算出手順によって算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、前記最大挟角と最小挟角との間で各角度における前記挟角としての尤度を規定した尤度モデルを適用して、前記領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出するとともに、前記複数のランドマーク間ごとに算出されたスコアを前記座標位置ごとに加算するスコア算出手順と、前記スコア算出手順によって算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を前記自己位置として推定する自己位置推定手順と、をコンピュータに実行させることを特徴とする。

また、請求項２に係る発明は、上記の発明において、前記スコア算出手順は、前記最大挟角と最小挟角との間の中心値である中心挟角が最大尤度となる放物線状の曲線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする。

また、請求項３に係る発明は、上記の発明において、前記スコア算出手順は、前記最大挟角と最小挟角との間の各挟角が一定尤度となる一様直線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする。

また、請求項４に係る発明は、上記の発明において、前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態が良好である場合には、前記最大挟角と最小挟角との間で前記尤度を単放性の曲線で規定した尤度モデルを適用して前記スコアを算出し、前記入力画像における前記ランドマークの撮像状態が良好でない場合には、前記最大挟角と最小挟角との間で前記尤度を一様直線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする。

また、請求項５に係る発明は、上記の発明において、前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態に応じて、前記尤度モデルを当該撮像状態に応じた重み付けで適用して前記スコアを算出することを特徴とする。

請求項１の発明によれば、複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出し、複数のランドマーク間ごとに、挟角算出手順によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出し、複数のランドマーク間ごとに、拘束円算出手順によって算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出するとともに、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算し、スコア算出手順によって算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を自己位置として推定するので、挟角の算出誤差の影響を低減させことができ、自己位置推定精度を向上させることが可能になる。

また、請求項２の発明によれば、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルを適用してスコアを算出するので、２つのランドマークの端によって規定される最大挟角拘束円もしくは最小挟角拘束円の円周部が自己位置とされることはなく、また前記最大挟角と前記最小挟角の中心値である中心挟角を最大尤度とした単放性の曲線に対応したスコアを算出することができ、例えば２つのランドマークの撮像状態が良好である場合には、特に自己位置推定精度を向上させることが可能になる。具体的には、２つのランドマークの撮像状態が良好である場合には、それぞれのランドマークの中心からなる中心挟角が自己位置を示す最大尤度をあたえるので、単放性の曲線で規定される尤度モデルを適用することで、自己位置推定精度を向上させることが可能になる。

また、請求項３の発明によれば、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルを適用してスコアを算出するので、２つのランドマークの端によって規定される最大挟角拘束円もしくは最小挟角拘束円の円周部が自己位置とされることはなく、また前記最大挟角と前記最小挟角の間の角度に対しては一様直線としてスコアを算出することができ、例えば２つのランドマークの撮像状態が良好でない場合には、特に自己位置推定精度を向上させることが可能になる。具体的には、２つのランドマークの撮像状態が良好でない場合には、算出された最大挟角と最小挟角との間にあるどの角度が最大尤度をあたえるか判定できないので、一様直線で規定される尤度モデルを適用することで、自己位置推定精度を向上させることが可能になる。

また、請求項４の発明によれば、入力画像におけるランドマークの撮像状態が良好である場合には、最大挟角と最小挟角との間で前記尤度を単放性の曲線で規定した尤度モデルを適用してスコアを算出し、入力画像におけるランドマークの撮像状態が良好でない場合には、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルを適用してスコアを算出するので、撮像状態に応じて自動的に尤度モデルを選択することができ、自己位置推定精度を向上させることが可能になる。

また、請求項５の発明によれば、入力画像におけるランドマークの撮像状態に応じて、尤度モデルを当該撮像状態に応じた重み付けで適用してスコアを算出するので、撮像状態に応じて自動的に尤度モデルから算出されるスコアに重み付けを適用でき、自己位置推定精度を向上させることが可能になる。

以下に添付図面を参照して、この発明に係る自己位置推定プログラム、自己位置推定方法および自己位置推定装置の実施例を詳細に説明する。なお、以下では、この発明に係る自己位置推定プログラムを含んで構成される自己位置推定装置を車両に搭載した実施例として説明する。また、以下では、実施例１における自己位置推定装置の構成および処理の手順、実施例１の効果を順に説明し、次に実施例１と同様に、実施例２に係る自己位置推定装置、実施例３に係る自己位置推定装置、実施例４に係る自己位置推定装置について順に説明する。

［実施例１における自己位置推定装置の概要および特徴］
まず最初に、図１を用いて、実施例１における自己位置推定装置の主たる特徴を具体的に説明する。図１は、実施例１における自己位置推定装置の概要および特徴を説明するための図である。

実施例１における自己位置推定装置は、入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定することを概要とする。ここで、「ランドマーク」とは、自己位置を推定するために目印として使用されるものであり、例えばビルディングや道路標識などが挙げられる。また、「自己位置」とは、撮像装置（もしくは撮像装置が搭載される車両）が存在する場所の情報であり、例えば車両が位置する緯度と経度などがこれに当たる。また、「挟角」とは、車両と２つのランドマークからなる三角形のうち、車両とランドマークを結ぶ２つの辺によって挟まれる角度のことである。また、「拘束円」とは、２つのランドマークの位置情報と、「挟角」によって規定される円であり、その円周上に撮像装置が存在する場所の情報である「自己位置」が存在する。

ここで、本発明は、自己位置推定精度を向上させることに主たる特徴がある。この主たる特徴について簡単に説明すると、実施例１における自己位置推定装置は、複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する。具体的には、図１の（Ａ）に示すように、ランドマーク間の挟角を、ランドマークの幅に基づいた最大挟角と最小挟角から規定される幅をもった角度として算出する。なお、図１では、車両に搭載されている撮像装置から自己位置推定装置に入力された入力画像に、ランドマーク１、ランドマーク２、ランドマーク３の３つのランドマークが検出された場合を例に説明しており、図１の（Ａ）では、ランドマーク１とランドマーク２とについて最大挟角と最小挟角とを求める場合を表しているが、同様にランドマーク１とランドマーク３、ランドマーク２とランドマーク３とについても最大挟角と最小挟角とを算出する。

続いて、実施例１における自己位置推定装置は、複数のランドマーク間ごとに、上記で算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する。具体的には、図１の（Ｂ）に示すように、算出された最大挟角と最小挟角とともに、後述するランドマークの絶対位置情報に基づき、最大挟角拘束円と最小挟角拘束円を算出する。この最大挟角拘束円と最小挟角拘束円により包囲される領域が、撮像位置である自己位置が存在する可能性のある領域となる。ここでランドマークの「絶対位置情報」とは、例えばランドマークが位置する緯度および経度がこれに対応する。

続いて、実施例１における自己位置推定装置は、複数のランドマーク間ごとに、上記で算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出する。具体的には、最大挟角と最小挟角との中心値である中心挟角から、中心挟角拘束円を算出し、この中心挟角拘束円を利用してスコアを算出する。すなわち、図１の（Ｃ）に示すように、最大挟角拘束円および最小挟角拘束円によって包囲される領域全体に、尤度モデルから算出されるスコアを算出する。なお、図１の（Ｃ）では、中心挟角拘束円の中心を通る直線と最大挟角拘束円との交点である交点Ａと最小挟角拘束円との交点である交点Ｂからなる線分ＡＢに沿って算出されたスコアをグラフで示す。また、ここで示すのは、単放性の曲線からなる尤度モデルから算出したスコアである。さらに、ｘ−ｙ平面は予めグリッドに分割されており、線分ＡＢが通る座標位置に対応するグリッドに対し、スコア付けを行っている。

続いて、実施例１における自己位置推定装置は、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算する。具体的には、図１の（Ｄ）に示すように、ランドマーク間ごとに算出されグリッドに対し対応付けられたスコアを、あらためてグリッドごとに加算する。なお、図１の（Ｄ）では、ランドマーク１とランドマーク２とから算出された最大挟角拘束円および最小挟角拘束円によって包囲される領域に対し算出されたスコアと、ランドマーク２とランドマーク３とから算出された最大挟角拘束円および最小挟角拘束円によって包囲される領域に対し算出されたスコアとを加算する場合を示している。また、図１の（Ｄ）では、ランドマーク１とランドマーク２とから規定される最大挟角拘束円とランドマーク２とランドマーク３とから規定される最大挟角拘束円の交点である交点Ｃと、ランドマーク１とランドマーク２とから規定される最小挟角拘束円とランドマーク２とランドマーク３とから規定される最小挟角拘束円の交点である交点Ｄからなる線分ＣＤに沿って算出したスコアと、その加算したスコアのパターンをグラフで示している。

続いて、実施例１における自己位置推定装置は、上記で加算されたスコアに基づいて、当該スコアが最大となる座標位置を自己位置として推定する。すなわち、実施例１における自己位置推定装置は、すべての座標位置（グリッド）において、複数のランドマーク間から算出し加算したスコアから、最大値を示す場所を自己推定位置として出力する。具体的には、図１の（Ｅ）では、ランドマーク１とランドマーク２とから算出されたスコアと、ランドマーク２とランドマーク３とから算出されたスコアを加算した結果に基づいて、最大値となった座標位置（グリッド）を自己推定位置として出力している。

このようなことから、実施例１における自己位置推定装置は、挟角の算出誤差の影響を低減させることができ、上記した主たる特徴の通り、自己位置推定精度を向上させることが可能になる。

［実施例１における自己位置推定装置の構成］
次に、図２〜１３を用いて、実施例１における自己位置推定装置を説明する。図２は、実施例１における自己位置推定装置の構成を示すブロック図であり、図３は、ランドマーク検出結果記憶部に記憶されるランドマーク検出結果を示す図であり、図４は、ランドマーク情報記憶部に記憶されるランドマーク情報を示す図であり、図５は、ランドマーク検索結果記憶部に記憶されるランドマーク検索結果を示す図であり、図６は、挟角算出処理部による挟角算出処理を説明するための図であり、図７は、挟角算出結果記憶部に記憶される挟角算出結果を示す図であり、図８は拘束円算出処理部による拘束円算出処理を説明するための図であり、図９は、拘束円算出結果記憶部に記憶される拘束円算出結果を示す図であり、図１０は、尤度モデル記憶部に記憶されている尤度モデルを示す図であり、図１１は、投票処理部による投票処理を説明するための図であり、図１２は、投票処理結果記憶部に記憶される投票処理結果を示す図であり、図１３は、自己位置推定部による自己位置推定を説明するための図である。

図２に示すように、実施例１における自己位置推定装置１０は、カメラ画像入力部１１と、自己位置推定出力部１２と、入出力部Ｉ／Ｆ部１３と、記憶部２０と、処理部３０とから構成され、さらに、カーナビ出力装置４０と、衝突回避車両制御装置５０とに接続される。

カメラ画像入力部１１は、処理部２０による各種処理に用いるカメラ画像データを入力する。具体的には、ランドマークが撮像されていて自己位置推定に用いられる画像データであって、例えば、ＪＰＥＧ（Ｊｏｉｎｔ Ｐｈｏｔｏｇｒａｐｈｉｃ Ｅｘｐｅｒｔｓ Ｇｒｏｕｐ）などの形式で保存された画像データなどをカメラから直接入力する。なお自己位置推定装置１０は、図示しない入力部として、キーボードや、タッチパネルを備えており、これにより自己位置推定要求を受け付けるようにしている。

自己推定位置出力部１２は、後述する自己位置推定部３６による処理結果として得られる自己推定位置を、カーナビ出力装置４０や衝突回避車両制御装置５０に出力する。具体的には、自己位置推定位置として地球上における座標、例えば経度、経度を出力する。

入出力制御Ｉ／Ｆ部１３は、カメラ画像入力部１１および自己推定位置出力部１２と、記憶部２０および処理部３０との間におけるデータ転送を制御する。

記憶部２０は、処理部３０による各種処理に用いるデータと、処理部３０による各種処理結果を記憶し、特に本発明に密接に関連するものとしては、図２に示すように、ランドマーク検出結果記憶部２１と、ランドマーク情報記憶部２２と、ランドマーク検索結果記憶部２３と、カメラパラメータ記憶部２４と、挟角算出結果記憶部２５と、拘束円算出結果記憶部２６と、尤度モデル記憶部２７と、投票処理結果記憶部２８とを備える。なお、各部については、後に詳述する。

処理部３０は、入出力制御Ｉ／Ｆ部１３から転送された画像データに基づき各種処理を実行し、特に本発明に密接に関連するものとしては、図２に示すように、ランドマーク検出処理部３１と、ランドマーク検索処理部３２と、挟角算出処理部３３と、拘束円算出処理部３４と、投票処理部３５と、自己位置推定部３６とを備える。ここで、挟角算出処理部３３は、特許請求の範囲に記載の「挟角算出手順」に対応し、拘束円算出処理部３４は、同じく「拘束円算出手順」に対応し、投票処理部３５は、同じく「スコア算出手順」に対応し、自己位置推定部３６は、同じく「自己位置推定手順」に対応する。なお、各部については、以下に詳述する。

ランドマーク検出処理部３１は、入力された画像データに含まれるランドマークを検出する。具体的には、公知のパターン認識の手法を用いて、画像データのなかからビルディングや標識などのランドマークを検出し、画像データにおけるランドマークの座標位置、幅、高さおよび画像特徴量などの情報を算出する。なお、「座標位置（Ｘ座標およびＹ座標）、幅および高さ」は、画面上の画像データにおいてランドマークが占める範囲を示すものであって、特にＸ座標はランドマークの左端の座標位置を示すものである。また、「画像特徴量」とは、検出されたランドマークの画像そのものでも、使用したパターン認識の手法によって算出される情報（例えば勾配ヒストグラムやＤＣＴ（Ｄｉｓｃｒｅｔｅ Ｃｏｓｉｎｅ Ｔｒａｎｓｆｏｒｍ）係数など)でも、どちらでも良く、検出されたランドマークを識別するために付与される情報である。これらの算出結果は、後述するランドマーク検出結果記憶部２１に記憶される。

ランドマーク検出結果記憶部２１は、上述したランドマーク検出処理部３１による処理結果を記憶する。具体的には、図３に示すように、「ランドマーク領域ＩＤ」、「Ｘ座標、Ｙ座標」、「幅」、「高さ」および「画像特徴量」を対応付けて記憶する。ここで図３における「ランドマーク領域ＩＤ」とは、検出されたランドマークごとに付与されるＩＤである。また、「Ｘ座標、Ｙ座標」、「幅」、「高さ」および「画像特徴量」とは、上記したランドマーク検出処理部３１により、検出されたランドマークの画像から算出されるものである。

ランドマーク情報記憶部２２は、後述するランドマーク検索処理部３２による処理に用いるデータを記憶する。具体的には、ランドマーク情報記憶部２２は、例えばランドマークとして使用可能なビルや標識を、あらかじめ高精度ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍｓ）装置による測位することで得られた情報と、撮像装置により撮影することにより得られた情報とを記憶する。すなわち、図４に示すように、「ランドマークＩＤ」、「Ｘ座標、Ｙ座標」、「画像データ」および「画像特徴量」を対応付けて記憶する。ここで、図４における「ランドマークＩＤ」とは、ランドマーク情報記憶部２２に保持されているランドマークごとに付与されるＩＤである。また、「Ｘ座標、Ｙ座標」は、高精度ＧＰＳ装置によって得られたランドマークの測位情報であり、例えば緯度および経度がこれらに対応する。また、「画像データ」は、撮像装置によって得られたランドマークの画像データであり、また「画像特徴量」は、「画像データ」から公知のパターン認識の手法によって算出される情報（例えば勾配ヒストグラムやＤＣＴ（Ｄｉｓｃｒｅｔｅ Ｃｏｓｉｎｅ Ｔｒａｎｓｆｏｒｍ）係数など)であり、上述したランドマーク検出結果記憶部２１に記憶するランドマークの画像特徴量と同質のものである。

ランドマーク検索処理部３２は、入力された画像データに撮像されているランドマークから、既知のランドマークを検索する。具体的には、ランドマーク検出結果記憶部２１に記憶されている画像特徴量と、ランドマーク情報記憶部２２に記憶されている画像特徴量とを照合して、画像特徴量が一致する「画像データに撮像されているランドマーク」と「既知のランドマーク」とを検索し対応付ける。その結果は、後述するランドマーク検索結果記憶部２３に記憶する。

ランドマーク検索結果記憶部２３は、ランドマーク検索処理部３２の処理結果を記憶する。具体的には、図５に示すように、「ランドマーク領域ＩＤ」および「ランドマークＩＤ」を対応付けて記憶する。ここで「ランドマーク領域ＩＤ」とは、ランドマーク検出結果記憶部２１に保持されている検出されたランドマークごとに付与されるＩＤであり、図３における「ランドマーク領域ＩＤ」に対応する。また、「ランドマークＩＤ」とは、ランドマーク情報記憶部２２に保持されているランドマークごとに付与されるＩＤであり、図４における「ランドマークＩＤ」に対応する。

カメラパラメータ記憶部２４は、撮像装置であるカメラが使用しているカメラレンズから定義されるパラメータを記憶する。具体的には、撮像装置であるカメラが使用しているカメラレンズから定義される１ピクセル当りの角度であり、後述する挟角算出処理部３３の挟角算出処理に用いられる。

挟角算出処理部３３は、複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出し、その結果を、後述する挟角算出結果記憶部２５に記憶する。具体的には、ランドマーク検出結果記憶部２１が記憶しているランドマークの「Ｘ座標と幅」およびカメラパラメータ記憶部２４が記憶している「カメラパラメータ」に基づいて、画像に含まれるランドマーク間の最大挟角と最小挟角とを算出する。すなわち、図６の（Ａ）に示すように、ランドマーク検出結果記憶部２１に記憶されているランドマークｉのＸ座標「ｘ＿ｉ」および幅「ｗ＿ｉ」、ランドマークｊのＸ座標「ｘ＿ｊ」および幅「ｗ＿ｊ」から、ランドマークｉとランドマークｊの最大挟角である「θ＿ｉｊ＿ｍａｘ」と、最小挟角である「θ＿ｉｊ＿ｍｉｎ」とを算出する。ここで、図６の（Ｂ）では、カメラパラメータ記憶部２４に格納されている撮像装置であるカメラが使用しているカメラレンズから定義されるパラメータである、１ピクセル当りの角度Δθ（デルタシータ）に基づき、最大挟角「θ＿ｉｊ＿ｍａｘ」と、最小挟角「θ＿ｉｊ＿ｍｉｎ」を算出する数式を示している。

挟角算出結果記憶部２５は、上述した挟角算出処理部３３の処理結果を記憶する。具体的には、図７に示すように、「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」、「最小挟角（°）」および「最大挟角（°）」を対応付けて記憶する。ここで図７における「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」とは、処理されている２つの検出されたランドマークの「ランドマーク領域ＩＤ」を入力画像において相対的に左（Ｌ）に位置するランドマーク領域ＩＤと、入力画像において相対的に右（Ｒ）にあるランドマーク領域ＩＤの順に表記したものである。また、「最小挟角（°）」および「最大挟角（°）」は、「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」について挟角算出処理部３３で算出された、最小挟角と最大挟角に対応する。

拘束円算出処理部３４は、複数のランドマーク間ごとに、上記で算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出し、その結果を、後述する拘束円算出結果記憶部２６に記憶する。具体的には、ランドマーク検索結果記憶部２３が記憶しているランドマークの「Ｘ座標、Ｙ座標（例えば緯度および経度）」および挟角算出結果記憶部２５が記憶している「最大挟角および最小挟角」に基づき、最大挟角拘束円と最小挟角拘束円を算出し、自己位置が存在する可能性のある領域を算出する。

すなわち、図８の（Ａ）に示すように、２つのランドマークの位置情報および挟角が与えられれば、自己位置が存在する拘束円を算出することができる。より詳細には、図８の（Ａ）に示すように、ランドマーク検索結果記憶部２３に記憶されている「ランドマーク領域ＩＤ」が「ＩＬ＿ｉ１」の絶対位置情報である「Ｌｏｃ＿ｉ＝（Ｘ＿ｉ、Ｙ＿ｉ）」と、「ランドマーク領域ＩＤ」が「ＩＬ＿ｉ１」の絶対位置情報である「Ｌｏｃ＿ｊ＝（Ｘ＿ｊ、Ｙ＿ｊ）」と「挟角（θ＿ｉｊ）」とに基づき、これらの条件を満たす拘束円をあらわす「拘束円パラメータ（λ＿ｉｊ）」を算出する。なお、「拘束円パラメータ（λ＿ｉｊ）」は、その円の中心である「ベクトルＯ（θ＿ｉｊ）」と半径である「ｒ（θ＿ｉｊ）」とのふたつの要素からなるパラメータである。図８の（Ｂ）では、拘束円パラメータの二つの要素を算出する数式を示す。なお「θ＿ｉｊ」は挟角算出結果記憶部２５が保持している最大挟角「θ＿ｉｊ＿ｍａｘ」と、最小挟角「θ＿ｉｊ＿ｍｉｎ」の間にある角度である。これによって、拘束円算出処理部３４は、最大挟角から定まる最大挟角拘束円と、最小挟角から定まる最小挟角拘束円とを算出する。なお、拘束円算出処理部３４は、後述する投票処理部３５においてスコア算出の際に利用され、最大挟角と最小挟角の中心値である中心挟角から定まる中心挟角拘束円の拘束円パラメータも予め算出する。

拘束円算出結果記憶部２６は、拘束円算出処理部３４によって算出された結果を記憶する。具体的には、図９に示すように、「最小値時半径」、「最小値時中心」、「最大値時半径」、「最大値時中心」、「中心値時半径」および「中心値時中心」を対応付けて記憶する。ここで、図９における「最小値時半径」および「最小値時中心」は、最小挟角拘束円の拘束円パラメータであり、「最大値時半径」および「最大値時中心」は、最大挟角拘束円の拘束円パラメータであり、「中心値時半径」および「中心値時中心」は、中心挟角拘束円の拘束円パラメータである。ここでは、予め算出された中心挟角拘束円の拘束円パラメータが、最小挟角拘束円の拘束円パラメータと最大挟角拘束円の拘束円パラメータとともに記憶されている。

尤度モデル記憶部２７は、後述する投票処理部３５のスコア算出に適用される尤度モデルを記憶する。具体的には、尤度モデル記憶部２７は、図１０の（Ｂ）に示すように単放性の曲線で規定される尤度モデルを記憶する。これについて詳細に説明すると、図１０の（Ａ）は、図６の（Ａ）で示したものと同じであり、ランドマークの幅に基づき最大挟角である「θ＿ｉｊ＿ｍａｘ」と最小挟角である「θ＿ｉｊ＿ｍｉｎ」とが与えられることを示している。図１０の（Ｂ）では、最大挟角と最小挟角との間の各角度に対し、中心挟角である「θ＿ｉｊ＿μ」が最大尤度である単放性の曲線からなる尤度モデルをグラフで示している。また、図１０の（Ｃ）では、前記尤度モデル関数（Ｐ（θ＿ｉｊ））を算出する数式を示している。

投票処理部３５は、複数のランドマーク間ごとに、上記で算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに自己位置としての可能性を示すスコアを算出する。具体的には、図１１に示すように、拘束円算出結果記憶部２６に記憶されている「最小挟角拘束円の拘束円パラメータ」、「最大挟角拘束円の拘束円パラメータ」および「中心挟角拘束円の拘束円パラメータ」と、尤度モデル記憶部２７に記憶されている「尤度モデル」とに基づき、最大挟角拘束円と最小挟角拘束円によって包囲される領域全体に、尤度モデルから算出されるスコアを算出する。すなわち、図１１の（Ａ）では、中心挟角拘束円の中心を通る直線と最大挟角拘束円との交点である交点Ａと最小挟角拘束円との交点である交点Ｂからなる線分ＡＢに沿って、尤度モデル記憶部２７に記憶されている尤度モデルを適用して算出したスコアを投票することを示している。図１１（Ｂ）では、線分ＡＢに図１０の（Ｂ）で示した尤度モデルを適応して算出されたスコアの形状をグラフで示し、また、図１１の（Ｃ）では、線分ＡＢからの距離ｕにおけるスコアを算出する関数Ｐ´（ｕ）を示している。

ここで、上記した投票処理は、最大挟角拘束円と最小挟角拘束円とによって包囲される領域全体に対し行われるが、その手順は以下のステップにより行われる。まずは、中心挟角拘束円の円周に均等間隔で点をあたえる。続いて、中心挟角拘束円の中心と上記の点からなる直線と最大挟角拘束円との交点である交点Ａと最小挟角拘束円との交点である交点Ｂとを算出する。続いて、線分ＡＢ上の点に対し、上記図１１の（Ｃ）で示した関数によりスコアを算出する。続いて、線分ＡＢが存在するグリッド（ＧＲＩＤ（ｘ，ｙ））を算出し、各グリッドに対し対応するスコアを対応付ける。この処理を、例えば中心挟角拘束円の円周に均等間隔で点が１００個あたえられたとすると、１００個の点すべてについてステップ２からステップ４の処理を行う。ただし、例えば３４個目の点の処理により既に投票されていたグリッドに対し、３５個目の点の処理でも投票することができる場合は、投票をしないこととする。これは、ひとつのランドマーク間での処理において、スコアが重複して投票されることを防ぐものである。ただし、別のランドマーク間の処理においての投票は許される。上記したようなスコア投票処理を行うことで、一つのランドマーク間において、図１２に示すような投票スコアが得られる。

また、投票処理部３５は、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算する。具体的には、図１３に示すように、ランドマーク間ごとに算出されグリッドに対し対応付けられたスコアを、あらためてグリッドごとに加算する。

投票処理結果記憶部２８は、投票処理部３５の処理結果を記憶する。具体的には、図１２や図１３の（Ｂ）に示すように、最大挟角拘束円と最小挟角拘束円とによって包囲される領域全体に対応するグリッドに対して投票されたスコアを記憶する。図１２の「ランドマーク１と２の投票スコア」や、図１３の（Ｂ）の「ランドマーク１と２の投票スコア」および「ランドマーク２と３の投票スコア」では、グリッドことにスコアが対応付けられている様子を表形式で示している。

自己位置推定部３６は、上記で算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を自己位置として推定する。具体的には、投票処理結果記憶部２８に記憶されている加算後の結果に基づき、最大値を示す位置を自己推定位置として出力する。図１３は、「ランドマーク１とランドマーク２」および「ランドマーク２とランドマーク３」を処理し、自己位置を推定する場合を示す。図１３の（Ａ）に示すように、投票処理部３５の処理によって、「ランドマーク１とランドマーク２」および「ランドマーク２とランドマーク３」について、それぞれの最大挟角拘束円および最小挟角拘束円によって規定される領域に対応するグリッドにスコアが算出され、加算されている。ここで、自己位置推定部３６は、図１３の（Ｂ）の「ランドマーク間ごとのスコア加算」の結果（投票処理結果記憶部２８に記憶されているもの）のうち、最大値を示すグリッドを自己位置として推定する。ここでは、ＧＲＩＤ（Ｘ，Ｙ）＝（３，４）の座標位置が自己位置として推定されている。なお、当該座標位置は、ランドマークの絶対位置情報から、具体的に緯度および経度に変換されてもよい。

カーナビ出力装置４０は、自己推定位置出力部１２の出力結果を、カーナビ装置（カーナビゲーション装置ともいう）に出力する。例えば、カーナビ装置の画面上に、自己推定位置として算出された緯度や経度の情報を、既存の地図情報とともに表示する。

衝突回避車両制御装置５０は、車両の動作を制御する。例えば、道路の位置情報および形状情報をカーナビ装置が保有する場合には、カーナビ装置から道路の位置情報および形状情報を取得するとともに、自己推定位置出力部１２の出力結果を取得し、道路の位置情報および形状情報と、自己推定位置出力部１２の出力結果との情報に基づいて、車両が道路外に突入する可能性があるかどうかを判定し、道路外に突入する可能性がありと判断した場合は、道路外への突入を回避するために、車両の動作停止制御を行う。

［実施例１における自己位置推定装置による処理の手順］
次に、図１４を用いて、実施例１における自己位置推定装置による処理を説明する。図１４は、実施例１における自己位置推定装置の処理の手順を示すフローチャートである。

まず、実施例１における自己位置推定装置１０は、キーボードやタッチパネルから自己位置推定要求を受け付けると（ステップＳ１４０１肯定）、ランドマーク検出処理部３１は、入力された画像データに含まれるランドマークを検出する（ステップＳ１４０２）。

そして、ランドマーク検索処理部３２は、入力された画像データに撮像されているランドマークから、既知のランドマークを検索する（ステップＳ１４０３）。

続いて、挟角算出処理部３３は、上記で検索された複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する挟角算出処理を行う（ステップＳ１４０４）。すなわち、ランドマーク検出結果記憶部２１が記憶しているランドマークの「Ｘ座標と幅」およびカメラパラメータ記憶部２４が記憶している「カメラパラメータ」に基づいて、画像に含まれるランドマーク間の最大挟角と最小挟角とを算出する。

そして、拘束円算出処理部３４は、複数のランドマーク間ごとに、上記で算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する（ステップＳ１４０５）。すなわち、ランドマーク検索結果記憶部２３が記憶しているランドマークの「Ｘ座標、Ｙ座標（例えば緯度および経度）」および挟角算出結果記憶部２５が記憶している「最大挟角および最小挟角」に基づき、最大挟角拘束円と最小挟角拘束円を算出し、自己位置が存在する可能性のある領域を算出する。

続いて、投票処理部３５は、複数のランドマーク間ごとに、上記で算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに自己位置としての可能性を示すスコアを算出するスコア算出処理を行う（ステップＳ１４０６）。すなわち、拘束円算出結果記憶部２６に記憶されている「最小挟角拘束円の拘束円パラメータ」、「最大挟角拘束円の拘束円パラメータ」および「中心挟角拘束円の拘束円パラメータ」と、尤度モデル記憶部２７に記憶されている「尤度モデル」とに基づき、最大挟角拘束円と最小挟角拘束円によって包囲される領域全体に、尤度モデルから算出されるスコアを算出する。

そして、上記投票処理部３５は、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算する（ステップＳ１４０７）。すなわち、ランドマーク間ごとに算出されグリッドに対し対応付けられたスコアを、あらためてグリッドごとに加算する。

そして、自己位置推定部３６は、上記で算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を自己位置として推定し（ステップＳ１４０８）、処理を終了する。すなわち、投票処理結果記憶部２８に記憶されている「ランドマーク間ごとのスコア加算」の結果のうち、最大値を示すグリッドを自己位置として推定し、その位置を自己位置として出力し、処理を終了する。

［実施例１の効果］
上記したように、実施例１によれば、複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出し、複数のランドマーク間ごとに、挟角算出手順によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出し、複数のランドマーク間ごとに、最大挟角拘束円および最小挟角拘束円で包囲される領域に、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに自己位置としての可能性を示すスコアを算出するとともに、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算し、当該スコアが最大となる座標位置を自己位置として推定するので、挟角の算出誤差の影響を低減させことができ、自己位置推定精度を向上させることが可能になる。

また、実施例１によれば、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに自己位置としての可能性を示すスコアを算出するので、例えば実施例１で適用した最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルの場合、２つのランドマークの端によって規定される最大挟角拘束円もしくは最小挟角拘束円の円周部が自己位置とされることはなく、自己位置推定の精度を向上させることが可能になる。

また、実施例１によれば、例えば人工衛星からの電波がＧＰＳ装置に届かないような場所を移動中の車両においても、撮動装置からの入力画像データだけで、精度の高い自己位置推定を行うことが可能になる。

また、実施例１によれば、撮動装置からの入力画像データだけで、ＧＰＳ装置による自己位置推定より高い解像度で自己位置推定を行うので、例えば、道路外への突入を回避することが可能になる。

上述した実施例１では、予め用意された一つの尤度モデルを適用していたが、実施例２では、複数の尤度モデルのなかから、ランドマークの撮像状態に応じて尤度モデルを選択し、選択した尤度モデルを適用する自己位置推定装置について説明する。

［実施例２における自己位置推定装置の概要および特徴］
まず最初に、図１５を用いて、実施例２における自己位置推定装置の主たる特徴を具体的に説明する。図１５は、実施例２における自己位置推定装置の概要および特徴を説明するための図である。

実施例２における自己位置推定装置は、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルを適用してスコアを算出し、入力画像におけるランドマークの撮像状態が良好でない場合には、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルを適用してスコアを算出する。

具体的には、図１５に示すように、入力画像に検出されたランドマークの撮像状態を評価し、スコア算出処理を行う２つのランドマークの評価がともに「良」の場合は、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルを適用してスコアを算出し、スコア算出処理を行う２つのランドマークの評価のどちらか一方が「悪」の場合は、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルを適用してスコアを算出する。

なお、図１５では、車両に搭載されている撮像装置から自己位置推定装置に入力された入力画像に、ランドマーク１、ランドマーク２、ランドマーク３の３つのランドマークが検出された場合を例に説明しており、入力画像に検出されたランドマーク１、ランドマーク２、ランドマーク３の画像データと、ランドマーク情報記憶部に記憶されていたランドマーク１、ランドマーク２、ランドマーク３の画像データとをそれぞれ比較して、評価を行っている。ここで、ランドマーク１およびランドマーク２の評価は「良」、ランドマーク３の評価は「悪」である。ランドマーク１およびランドマーク２のように、２つとも評価が「良」であるランドマーク間で処理を行う際には、単放性の曲線で規定した尤度モデルＰ１を適用し引き続きスコア算出処理を行う。ランドマーク２およびランドマーク３のように、少なくとも１つの評価が「悪」であるランドマーク間で処理を行う際には、一様直線で規定した尤度モデルＰ２を適用し引き続きスコアを算出する。

このようなことから、実施例２における自己位置推定装置は、撮像状態に応じて自動的に尤度モデルを選択することができ、自己位置推定精度を向上させることが可能になる。

［実施例２における自己位置推定装置の構成］
次に、図１６〜１８を用いて、実施例２における自己位置推定装置を説明する。図１６は、実施例２における自己位置推定装置の構成を示すブロック図であり、図１７は、尤度モデル記憶部に格納されている選択尤度モデルを説明するための図であり、図１８は、ランドマーク検出画像評価結果記憶部と選択尤度モデル記憶部に記憶されるランドマーク検出画像評価結果と選択尤度モデルとを説明するための図である。

図１６に示すように、実施例２における自己位置推定装置は、図２に示した実施例１における自己位置推定装置と基本的に同様であり、ランドマーク検出画像評価結果記憶部２９と、選択尤度モデル記憶部２１０と、ランドマーク検出画像評価部３７と、尤度モデル選択部３８とを新たに備えているところが相違する。以下、これらを中心に説明する。

尤度モデル記憶部２７は、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルと、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルとを記憶する。具体的には、図１７に示すように、尤度モデル記憶部２７は、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデル尤度モデルＰ１（図１７の（Ａ））と、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルＰ２（図１７の（Ｂ））とを記憶する。なお、ここでは尤度モデルＰ１を導く数式および尤度モデルＰ２を導く数式を、あわせて示す。

ランドマーク検出画像評価部３７は、入力画像に検出されたランドマークの撮像状態を評価する。具体的には、ランドマーク検出結果記憶部２１に記憶されているランドマーク検出画像データと、ランドマーク情報記憶部２２に記憶されているランドマーク画像データとを比較して、ランドマーク検出画像の撮像状態を評価する。すなわち、図１８に示すように、ランドマーク画像データ「Ｃｒｏｐ」と、ランドマーク検出画像「ＩＬ」との重なり具合を数値化する評価関数「ｄｕｐ」を用い、その数値があらかじめ設定した閾値（ＴＨ）より大きければ、「良」と評価し、小さければ「悪」と評価する。

ランドマーク検出画像評価結果記憶部２９は、ランドマーク検出画像評価部３７によって評価されたランドマーク検出画像の評価結果を記憶する。具体的には、図１８に示すように、「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」、「最小挟角（°）」、「最大挟角（°）」、「検出状態（Ｌ、Ｒ）」および「選択モデル」を対応付けて記憶する。ここで図１８における「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」、「最小挟角（°）」および「最大挟角（°）」は、図７に示す「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」、「最小挟角（°）」および「最大挟角（°）」と同じものであり、「検出状態（Ｌ、Ｒ）」は処理されている２つの検出されたランドマークのうち、相対的に左「Ｌ」に位置するランドマーク領域ＩＤの画像データの評価結果と、入力画像において相対的に右「Ｒ」にあるランドマーク領域ＩＤの画像データとの評価結果を順に表記したものである。ここでは、ランドマーク検出画像評価結果記憶部２９に記憶される結果を判りやすく説明するために、挟角算出結果記憶部２５に記憶される挟角算出結果とともに示している。なお、「選択モデル」については、選択尤度モデル記憶部２１０にて後述する。

尤度モデル選択部３８は、ランドマーク検出画像評価結果記憶部２９に記憶されているランドマーク検出画像評価結果に基づき、適用する尤度モデルを尤度モデル記憶部２７から選択する。すなわち、ランドマーク検出画像評価結果記憶部２９に記憶されているランドマーク検出画像評価結果において、スコア算出を行う２つのランドマークのうち両方とも「良」であれば、最大挟角と最小挟角の間で尤度を単放性の曲線で規定した尤度モデルＰ１を選択し、少なくともどちらか一方が「悪」ならば、最大挟角と最小挟角の間で尤度を一様直線で規定した尤度モデルＰ２を選択する。

選択尤度モデル記憶部２１０は、尤度モデル選択部３８により選択された尤度モデルを記憶する。具体的には、図１８に示すように、「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」、「最小挟角（°）」、「最大挟角（°）」、「検出状態（Ｌ、Ｒ）」および「選択モデル」を対応付けて記憶する。ここで、図１８において、「ランドマーク領域ＩＤの組み合わせ（Ｌ、Ｒ）」、「最小挟角（°）」、「最大挟角（°）」および「検出状態（Ｌ、Ｒ）」は、上記ランドマーク検出画像評価結果記憶部２９にて説明したものと同じであり、「選択モデル」は尤度モデル選択部３８で選択された尤度モデルを示している。例えば図１８に示すように、「ＩＬ＿ｉ１」と「ＩＬ＿ｊ１」では尤度モデルＰ１が選択され、「ＩＬ＿ｉ２」と「ＩＬ＿ｊ２」では尤度モデルＰ２が選択されたことを記憶する。なお、ここでは、選択尤度モデル記憶部２１０に記憶される結果を判りやすく説明するために、挟角算出結果記憶部２５に記憶される挟角算出結果とともに示している。

投票処理部３５は、入力画像におけるランドマークの撮像状態が良好である場合には、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルを適用してスコアを算出し、入力画像におけるランドマークの撮像状態が良好でない場合には、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルを適用してスコアを算出する。具体的には、ランドマーク間ごとに、尤度モデルＰ１、もしくは尤度モデルＰ２が２つのランドマークの撮像状態に応じて自動的に選択され、適用された尤度モデルに応じたスコアが算出される。

［実施例２における自己位置推定装置による処理の手順］
次に、図１９を用いて、実施例２における自己位置推定装置による処理を説明する。図１９は、実施例２における自己位置推定装置の処理の手順を示すフローチャートである。

まず、実施例２における自己位置推定装置１０は、キーボードやタッチパネルから自己位置推定要求を受け付けると（ステップＳ１９０１肯定）、実施例１と同様（図１４に示したステップＳ１４０２〜ステップＳ１４０５処理と同様）に、「ランドマーク検出」、「ランドマーク検索」、「挟角算出」、「拘束円算出」を行なう（ステップＳ１９０２〜ステップＳ１９０５）。

続いて、ランドマーク検出画像評価部３７は、入力画像に検出されたランドマークの撮像状態を評価する（ステップＳ１９０６）。すなわち、ランドマーク検出結果記憶部２１に記憶されているランドマーク検出画像データと、ランドマーク情報記憶部２２に記憶されているランドマーク画像データとを比較して、ランドマーク検出画像の撮像状態を評価する。

そして、尤度モデル選択部３８は、ランドマーク検出画像評価結果記憶部２９に記憶されているランドマーク検出画像評価結果に基づき、適用する尤度モデルを尤度モデル記憶部２７から選択する（ステップＳ１９０７）。すなわち、ランドマーク検出画像評価結果記憶部２９に記憶されているランドマーク検出画像評価結果において、スコア算出を行う２つのランドマークのうち両方とも「良」であれば、最大挟角と最小挟角の間で尤度を単放性の曲線で規定した尤度モデルＰ１を選択し、少なくともどちらか一方が「悪」ならば、最大挟角と最小挟角の間で尤度を一様直線で規定した尤度モデルＰ２を選択する。

続いて、投票処理部３５は、複数のランドマーク間ごとに、上記で算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、最大挟角と最小挟角との間で各角度における挟角としての尤度を規定した尤度モデルを適用して、領域内の座標位置ごとに自己位置としての可能性を示すスコアを算出するスコア算出処理を行う（ステップＳ１９０８）。すなわち、拘束円算出結果記憶部２６に記憶されている「最小挟角拘束円の拘束円パラメータ」、「最大挟角拘束円の拘束円パラメータ」および「中心挟角拘束円の拘束円パラメータ」と、選択尤度モデル記憶部２１０に記憶されている「尤度モデル」とに基づき、最大挟角拘束円と最小挟角拘束円によって包囲される領域全体に、尤度モデルから算出されるスコアを算出する。

そして、上記投票処理部３５は、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算する（ステップＳ１９０９）。すなわち、ランドマーク間ごとに算出されグリッドに対し対応付けられたスコアを、あらためてグリッドごとに加算する。

そして、自己位置推定部３６は、上記で算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を自己位置として推定し（ステップＳ１９１０）、処理を終了する。すなわち、投票処理結果記憶部２８に記憶されている「ランドマーク間ごとのスコア加算」の結果のうち、最大値を示すグリッドを自己位置として推定し、その位置を自己位置として出力し、処理を終了する。

［実施例２の効果］
上記したように、実施例２によれば、ランドマークの撮像状態に応じて尤度モデルを自動的に選択することができるので、自己位置推定精度を向上されることが可能になる。

また、実施例２によれば、最大挟角と最小挟角との中心値である中心挟角を最大尤度とした単放性の曲線に対応したスコアを算出することができ、例えば２つのランドマークの撮像状態が良好である場合には、特に自己位置推定精度を向上させることが可能になる。具体的には、２つのランドマークの撮像状態が良好である場合には、それぞれのランドマークの中心からなる中心挟角が自己位置を示す最大尤度をあたえるので、単放性の曲線で規定される尤度モデルを適用することで、自己位置推定精度を向上させることが可能になる。

また、実施例２によれば、最大挟角と最小挟角との間の角度に対しては一様直線としてスコアを算出することができ、例えば２つのランドマークの撮像状態が良好でない場合には、特に自己位置推定精度を向上させることが可能になる。具体的には、２つのランドマークの撮像状態が良好でない場合には、算出された最大挟角と最小挟角との間にあるどの角度が最大尤度をあたえるか判定できないので、一様直線で規定される尤度モデルを適用することで、自己位置推定精度を向上させることが可能になる。

上述した実施例２では、複数の尤度モデルのなかから、ランドマークの撮像状態に応じて尤度モデルを選択し、選択した尤度モデルを適用して自己位置としてのスコアを算出したが、実施例３では、ランドマークの撮像状態に応じて、選択した尤度モデルを当該撮像状態に応じた重み付けで適用しスコアを算出する自己位置推定装置について説明する。

［実施例３における自己位置推定装置の概要および特徴］
まず最初に、図２０を用いて、実施例３における自己位置推定装置の主たる特徴を具体的に説明する。図２０は、実施例３における自己位置推定装置の概要および特徴を説明するための図である。

実施例３における自己位置推定装置は、入力画像におけるランドマークの撮像状態が良好である場合には、最大挟角と最小挟角との間で尤度を単放性の曲線で規定した尤度モデルを適用してスコアを算出し、入力画像におけるランドマークの撮像状態が良好でない場合には、最大挟角と最小挟角との間で尤度を一様直線で規定した尤度モデルを適用してスコアを算出するとともに、入力画像におけるランドマークの撮像状態に応じて、尤度モデルを当該撮像状態に応じた重み付けで適用してスコアを算出する。

具体的には、図２０に示すように、ランドマーク検出画像評価により算出された評価値に基づき、選択された尤度モデルから算出されたスコアにさらに重み付けを適用してスコアを算出する。なお、図２０では、車両に搭載されている撮像装置から自己位置推定装置に入力された入力画像に、ランドマーク１、ランドマーク２、ランドマーク３の３つのランドマークが検出された場合を例に説明しており、入力画像に検出されたランドマーク１、ランドマーク２、ランドマーク３の画像データと、ランドマーク情報記憶部に保持されていたランドマーク１、ランドマーク２、ランドマーク３の画像データとをそれぞれ比較し評価する。

ここでは、ランドマーク１とランドマーク２の評価値は、ここで設定されている「閾値ＴＨが０．５」より高い値であるため両者とも同じ「良」であり、かつ尤度モデルとしてＰ１が選択される。しかし、ランドマーク１の評価値が最大評価値「１」であるのに対し、ランドマーク２の評価値が「０．６５」と低い値であるために、この評価値に応じ、尤度モデルＰ１を適用して算出したスコアに対し、さらに重み付けを適用して算出したスコアを算出する。また、ランドマーク２とランドマーク３の組み合わせでは、ランドマーク２の評価が「良」で、ランドマーク３の評価が「悪」であるので、尤度モデルＰ２が選択され、スコアが算出されるが、さらにこれらの評価値に基づいて、重み付けを適用したスコアが算出する。

このようなことから、実施例３における自己位置推定装置は、上記した主たる特徴の通り、撮像状態に応じて自動的に尤度モデルを選択するとともに、撮像状態に応じて自動的に選択された尤度モデルから算出されるスコアに重み付けを適用することができ、自己位置推定精度を向上させることが可能になる。

［実施例３における自己位置推定装置の構成］
次に、図２１と、図２２とを用いて、実施例３における自己位置推定装置を説明する。図２１は、実施例３における自己位置推定装置の構成を示すブロック図であり、図２２は、重み付け投票処理部を説明するための図である。

図２１に示すように、実施例３における自己位置推定装置は、図１６に示した実施例２における自己位置推定装置と基本的に同様であり、投票処理部３５の代わりに重み付け投票処理部３９を備えているところが相違する。以下、これを中心に説明する。

重み付け投票処理部３９は、入力画像におけるランドマークの撮像状態に応じて、尤度モデルを当該撮像状態に応じた重み付けで適用してスコアを算出する。具体的には、ランドマーク検出画像評価結果記憶部２９に記憶されている評価値と、選択尤度モデル記憶部に記憶されている選択された尤度モデルから算出されるスコアに基づき、さらに重み付けを適用したスコアを算出する。例えば、図２２に示すように、「選択された尤度モデルＰ」から「スコアを算出する関数Ｐ’」に「評価関数ｄｕｐ」から算出された「評価値ｓ」を乗ずることによって導かれる「関数Ｐ’’」により重み付けを適用したスコアが算出する。

［実施例３における自己位置推定装置による処理の手順］
次に、図２３を用いて、実施例３における自己位置推定装置による処理を説明する。図２３は、実施例３における自己位置推定装置の処理の手順を示すフローチャートである。

まず、実施例３における自己位置推定装置１０は、キーボードやタッチパネルから自己位置推定要求を受け付けると（ステップＳ２３０１肯定）、実施例２と同様（図１９に示したステップＳ１９０２〜ステップＳ１９０７処理と同様）に、「ランドマーク検出」、「ランドマーク検索」、「挟角算出」、「拘束円算出」、「ランドマーク検出画像評価」、「尤度モデル選択」を行なう（ステップＳ２３０２〜ステップＳ２３０７）。

続いて、重み付け投票処理部３９は、入力画像におけるランドマークの撮像状態に応じて、尤度モデルを当該撮像状態に応じた重み付けで適用してスコアを算出する（ステップＳ２３０８）。具体的には、ランドマーク検出画像評価結果記憶部２９に記憶されている評価値と、選択尤度モデル記憶部２１０に記憶されている選択された尤度モデルから算出されるスコアに基づき、さらに重み付けを適用したスコアを算出する。

続いて、上記重み付け投票処理部３９は、複数のランドマーク間ごとに算出されたスコアを座標位置ごとに加算する（ステップＳ２３０９）。すなわち、ランドマーク間ごとに算出されグリッドに対し対応付けられたスコアを、あらためてグリッドごとに加算する。

そして、自己位置推定部３６は、上記で算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を自己位置として推定し（ステップＳ２３１０）、処理を終了する。すなわち、投票処理結果記憶部２８に記憶されている「ランドマーク間ごとのスコア加算」の結果のうち、最大値を示すグリッドを自己位置として推定し、その位置を自己位置として出力し、処理を終了する。

［実施例３の効果］
上記したように、実施例３によれば、入力画像の撮像状態を評価して自動的に尤度モデルを選択しスコアを算出するとともに、さらに評価の基準となった評価値に応じた重み付けで適用してスコアを算出するので、自己位置推定精度を向上させることが可能になる。すなわち、検出されたランドマークの画像データの評価が同じものであっても、その評価値の程度で、さらにスコアに重み付けを適用できるので、自己位置推定精度を向上させることが可能になる。

さて、これまで実施例１〜３における自己位置推定装置について説明したが、本発明は上述した実施例以外にも、種々の異なる形態にて実施されてもよいものである。そこで、以下では、実施例４における自己位置推定装置として、種々の異なる実施例を（１）〜（５）に区分けして説明する。

（１）重み付け
上記の実施例３では、複数の尤度モデルのなかから、ランドマークの撮像状態に応じて尤度モデルを選択し、選択した尤度モデルを当該撮像状態に応じた重み付けで適用しスコアを算出する自己位置推定装置について説明したが、本発明はこれに限定されるものではなく、与えられた一つの尤度モデルを、ランドマークの撮像状態に応じた重み付けで適用しスコアを算出する自己位置推定装置であってもよい。すなわち、「ランドマーク検出画像評価部３７」および「重み付け投票処理３９」が実施例１の処理部３０にさらに備えられている自己位置推定装置１０であってもよい。

（２）尤度モデル
上記の実施例１では、尤度モデル記憶部２７に最大挟角と最小挟角の間で尤度を単放性の曲線で規定した尤度モデルが記憶されている自己位置推定装置１０を説明したが、本発明はこれに限定されるものではなく、最大挟角と最小挟角の間で尤度を一様直線で規定した尤度モデルであっても、別の尤度モデルであってもよい。

また、上記の実施例２および実施例３では、尤度モデル記憶部２７に最大挟角と最小挟角の間で尤度を単放性の曲線で規定した尤度モデルＰ１と、最大挟角と最小挟角の間で尤度を一様直線で規定した尤度モデルＰ２の２つの尤度モデルが記憶されている自己位置推定装置１０を説明したが、本発明はこれに限定されるものではなく、別の尤度モデルと置き換わった、もしくは加えた２つ以上の尤度モデルが記憶されていてもよい。ここで、別の尤度モデルとは、ランドマークの形状（丸みを帯びているビルディングや、四角錐のテレビ塔など）やランドマークの見え方（樹木などの遮蔽物により右半分しか見えないなど）に応じた尤度モデルである。

（３）適用対象
また、上記の実施例では、この発明に係る自己位置推定プログラムを含んで構成される自己位置推定装置を搭載した車両を適用対象として説明したが、本発明はこれに限定されるものではなく、例えばどこで撮像されたか不明な写真を処理して、その写真が撮像された場所を推定する自己位置推定装置として使用してもよい。

（４）システム構成等
また、上記の実施例において説明した各処理のうち、自動的におこなわれるものとして説明した処理の全部または一部を手動でおこなうこともでき（例えば、キーボードやタッチパネルから自己位置推定要求を受け付けるのではなく、一定時間ごと自動的に自己位置推定要求を生成するなど）、あるいは、手動的におこなうものとして説明した処理の全部または一部を公知の方法で自動的におこなうこともできる。例えば、この他、上記文章中や図面中で示した処理手順、具体的名称、各種のデータやパラメータを含む情報（例えば図９に示す情報）については、特記する場合を除いて任意に変更することができる。

また、図示した各装置の各構成要素は機能概念的なものであり、必ずしも物理的に図示の如く構成されていることを要しない。すなわち、各処理部および各記憶部の分散・統合の具体的形態（例えば、図２の形態など）は図示のものに限られず、例えば、投票処理部３５と自己位置推定部３６を統合するなど、その全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。さらに、各装置にて行なわれる各処理機能は、その全部または任意の一部が、ＣＰＵおよび当該ＣＰＵにて解析実行されるプログラムにて実現され、あるいは、ワイヤードロジックによるハードウェアとして実現され得る。

（５）自己位置推定プログラム
ところで上記の実施例１〜３では、ハードウェアロジックによって各種の処理を実現する場合を説明したが、本発明はこれに限定されるものではなく、あらかじめ用意されたプログラムをコンピュータで実行するようにしてもよい。そこで以下では、図２４を用いて、上記の実施例１に示した自己位置推定装置１０と同様の機能を有する自己位置推定プログラムを実行するコンピュータの一例を説明する。図２４は、実施例１における自己位置推定プログラムを実行するコンピュータを示す図である。

図２４に示すように、情報処理装置としてのコンピュータ２４０は、キーボード２４１、ディスプレイ２４２、ＣＰＵ２４３、ＲＯＭ２４４、ＨＤＤ２４５およびＲＡＭ２４６をバス２４７などで接続して構成され、さらにカーナビ出力装置４０と衝突回避車両制御装置５０に接続される。

ＲＯＭ２４４には、上記の実施例１に示した自己位置推定装置１０と同様の機能を発揮する自己位置推定プログラム、つまり、図２４に示すように、ランドマーク検出プログラム２４４ａ、ランドマーク検索プログラム２４４ｂ、挟角算出プログラム２４４ｃ、拘束円算出プログラム２４４ｄ、投票プログラム２４４ｅ、自己位置推定プログラム２４４ｆが予め記憶されている。なお、これらのプログラム２４４ａ〜２４４ｆについては、図２に示した自己位置推定装置１０の各構成要素と同様、適宜統合または分散してもよい。

そして、ＣＰＵ２４３が、これらのプログラム２４４ａ〜２４４ｆをＲＯＭ２４４から読みだして実行することで、図２４に示すように、各プログラム２４４ａ〜２４４ｆは、ランドマーク検出プロセス２４３ａ、ランドマーク検索プロセス２４３ｂ、挟角算出プロセス２４３ｃ、拘束円算出プロセス２４３ｄ、投票プロセス２４３ｅ、自己位置推定プロセス２４３ｆとして機能するようになる。なお、各プロセス２４３ａ〜２４３ｆは、図２に示した、ランドマーク検出処理部３１、ランドマーク検索処理部３２、挟角算出処理部３３、拘束円算出処理部３４、投票処理部３５、自己位置推定部３６にそれぞれ対応する。

また、ＨＤＤ２４５には、図２４に示すように、ランドマーク情報テーブル２４５ａと、カメラパラメータテーブル２４５ｂと、尤度モデルテーブル２４５ｃが設けられる。このランドマーク情報テーブル２４５ａは、図２に用いたランドマーク情報記憶部２２に対応し、カメラパラメータテーブル２４５ｂはカメラパラメータ記憶部２４に対応し、尤度モデルテーブル２４５ｃは尤度モデル記憶部２７に対応する。そしてＣＰＵ２４３は、ランドマーク情報データ２４６ｂをランドマーク情報テーブル２４５ａに対して登録し、カメラパラメータデータ２４６ｄをカメラパラメータテーブル２４５ｂに対して登録し、尤度モデルデータ２４６ｇを尤度モデルテーブル２４５ｃに対して登録し、このランドマーク情報データ２４６ｂと、カメラパラメータデータ２４６ｄと、尤度モデルデータ２４６ｇを読み出してＲＡＭ２４６に格納し、ＲＡＭ２４６に格納されたランドマーク検出データ２４６ａと、ランドマーク検索データ２４６ｃと、挟角算出データ２４６ｅと、拘束円算出データ２４６ｆと、投票処理データ２４６ｈとに基づいて自己位置推定処理を実行する。

なお、上記した各プログラム２４４ａ〜２４４ｆについては、必ずしも最初からＲＯＭ２４４に記憶させておく必要はなく、例えばコンピュータ２４０に挿入されるフレキシブルディスク（ＦＤ）、ＣＤ−ＲＯＭ、ＭＯディスク、ＤＶＤディスク、光磁気ディスク、ＩＣカードなどの「可搬用の物理媒体」、または、コンピュータ２４０の内外に備えられるＨＤＤなどの「固定用物理媒体」、さらには、公衆回線、インターネット、ＬＡＮ、ＷＡＮなどを介してコンピュータ２４０に接続される「他のコンピュータ（またはサーバ）」などに各プログラムを記憶させておき、コンピュータ２４０がこれらから各プログラムを読み出して実行するようにしてもよい。

（付記１）入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から前記自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定する自己位置推定方法をコンピュータに実行させる自己位置推定プログラムであって、
前記複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する挟角算出手順と、
前記複数のランドマーク間ごとに、前記挟角算出手順によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する拘束円算出手順と、
前記複数のランドマーク間ごとに、前記拘束円算出手順によって算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、前記最大挟角と最小挟角との間で各角度における前記挟角としての尤度を規定した尤度モデルを適用して、前記領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出するとともに、前記複数のランドマーク間ごとに算出されたスコアを前記座標位置ごとに加算するスコア算出手順と、
前記スコア算出手順によって算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を前記自己位置として推定する自己位置推定手順と、
をコンピュータに実行させることを特徴とする自己位置推定プログラム。

（付記２）前記スコア算出手順は、前記最大挟角と最小挟角との間で前記尤度を単放性の曲線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする付記１に記載の自己位置推定プログラム。

（付記３）前記スコア算出手順は、前記最大挟角と最小挟角との間で前記尤度を一様直線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする付記１に記載の自己位置推定プログラム。

（付記４）前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態に応じて、複数の尤度モデルのなかから当該撮像状態に応じた尤度モデルを選択し、当該選択した尤度モデルを適用して前記スコアを算出することを特徴とする付記１に記載の自己位置推定プログラム。

（付記５）前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態が良好である場合には、前記最大挟角と最小挟角との間で前記尤度を単放性の曲線で規定した尤度モデルを適用して前記スコアを算出し、前記入力画像における前記ランドマークの撮像状態が良好でない場合には、前記最大挟角と最小挟角との間で前記尤度を一様直線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする付記１に記載の自己位置推定プログラム。

（付記６）前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態に応じて、前記尤度モデルを当該撮像状態に応じた重み付けで適用して前記スコアを算出することを特徴とする付記１に記載の自己位置推定プログラム。

（付記７）入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から前記自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定する自己位置推定方法であって、
前記複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する挟角算出工程と、
前記複数のランドマーク間ごとに、前記挟角算出工程によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する拘束円算出工程と、
前記複数のランドマーク間ごとに、前記拘束円算出工程によって算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、前記最大挟角と最小挟角との間で各角度における前記挟角としての尤度を規定した尤度モデルを適用して、前記領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出するとともに、前記複数のランドマーク間ごとに算出されたスコアを前記座標位置ごとに加算するスコア算出工程と、
前記スコア算出工程によって算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を前記自己位置として推定する自己位置推定工程と、
を含んだことを特徴とする自己位置推定方法。

（付記８）入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から前記自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定する自己位置推定装置であって、
前記複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する挟角算出手段と、
前記複数のランドマーク間ごとに、前記挟角算出手段によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する拘束円算出手段と、
前記複数のランドマーク間ごとに、前記拘束円算出手段によって算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、前記最大挟角と最小挟角との間で各角度における前記挟角としての尤度を規定した尤度モデルを適用して、前記領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出するとともに、前記複数のランドマーク間ごとに算出されたスコアを前記座標位置ごとに加算するスコア算出手段と、
前記スコア算出手段によって算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を前記自己位置として推定する自己位置推定手段と、
を備えたことを特徴とする自己位置推定装置。

以上のように、本発明における自己位置推定プログラム、自己位置推定方法および自己位置推定装置は、入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定する場合に有用であり、特に、自己位置推定精度を向上させることに適する。

実施例１における自己位置推定装置の概要および特徴を説明するための図である。 実施例１における自己位置推定装置の構成を示すブロック図である。 ランドマーク検出結果記憶部を説明するための図である。 ランドマーク情報記憶部を説明するための図である。 ランドマーク検索結果記憶部を説明するための図である。 挟角算出処理部を説明するための図である。 挟角算出結果記憶部を説明するための図である。 拘束円算出処理部を説明するための図である。 拘束円算出結果記憶部を説明するための図である。 尤度モデル記憶部を説明するための図である。 投票処理部を説明するための図である。 投票処理結果記憶部を説明するための図である。 自己位置推定部を説明するための図である。 実施例１における自己位置推定装置の処理を説明するための図である。 実施例２における自己位置推定装置の概要および特徴を説明するための図である。 実施例２における自己位置推定装置の構成を示すブロック図である。 尤度モデル記憶部に格納されている選択尤度モデルを説明するための図である。 ランドマーク検出画像評価結果記憶部と選択尤度モデル記憶部を説明するための図である。 実施例２における自己位置推定装置の処理を説明するための図である。 実施例３における自己位置推定装置の概要および特徴を説明するための図である。 実施例３における自己位置推定装置の構成を示すブロック図である。 重み付け投票処理部を説明するための図である。 実施例３における自己位置推定装置の処理を説明するための図である。 実施例１の自己位置推定プログラムを実行するコンピュータを示す図である。

符号の説明

１０ 自己位置推定装置
１１ カメラ画像入力部
１２ 自己推定位置出力部
１３ 入出力制御Ｉ／Ｆ部
２０ 記憶部
２１ ランドマーク検出結果記憶部
２２ ランドマーク情報記憶部
２３ ランドマーク検索結果記憶部
２４ カメラパラメータ記憶部
２５ 挟角算出結果記憶部
２６ 拘束円算出結果記憶部
２７ 尤度モデル記憶部
２８ 投票処理結果記憶部
３０ 処理部
３１ ランドマーク検出処理部
３２ ランドマーク検索処理部
３３ 挟角算出処理部
３４ 拘束円算出処理部
３５ 投票処理部
３６ 自己位置推定部
４０ カーナビ出力装置
５０ 衝突回避車両制御装置

Claims (5)

1. 入力画像に含まれる複数のランドマーク間ごとに当該入力画像の撮像位置である自己位置を中心とした各ランドマーク間の挟角をそれぞれ算出し、当該算出した各挟角から前記自己位置として拘束される拘束円をそれぞれ算出して当該自己位置を推定する自己位置推定方法をコンピュータに実行させる自己位置推定プログラムであって、
   前記複数のランドマーク間ごとに、各ランドマーク間の最遠距離および最近距離から、各ランドマーク間の挟角の最大値を示す最大挟角および当該挟角の最小値を示す最小挟角を算出する挟角算出手順と、
   前記複数のランドマーク間ごとに、前記挟角算出手順によって算出された最大挟角および最小挟角それぞれに基づいて、当該最大挟角から定まる最大挟角拘束円および当該最小挟角から定まる最小挟角拘束円を算出する拘束円算出手順と、
   前記複数のランドマーク間ごとに、前記拘束円算出手順によって算出された最大挟角拘束円および最小挟角拘束円で包囲される領域に、前記最大挟角と最小挟角との間で各角度における前記挟角としての尤度を規定した尤度モデルを適用して、前記領域内の座標位置ごとに前記自己位置としての可能性を示すスコアを算出するとともに、前記複数のランドマーク間ごとに算出されたスコアを前記座標位置ごとに加算するスコア算出手順と、
   前記スコア算出手順によって算出されて加算されたスコアに基づいて、当該スコアが最大となる座標位置を前記自己位置として推定する自己位置推定手順と、
   をコンピュータに実行させることを特徴とする自己位置推定プログラム。
2. 前記スコア算出手順は、前記最大挟角と最小挟角との間の中心値である中心挟角が最大尤度となる放物線状の曲線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする請求項１に記載の自己位置推定プログラム。
3. 前記スコア算出手順は、前記最大挟角と最小挟角との間の各挟角が一定尤度となる一様直線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする請求項１に記載の自己位置推定プログラム。
4. 前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態が良好である場合には、前記最大挟角と最小挟角との間で前記尤度を単放性の曲線で規定した尤度モデルを適用して前記スコアを算出し、前記入力画像における前記ランドマークの撮像状態が良好でない場合には、前記最大挟角と最小挟角との間で前記尤度を一様直線で規定した尤度モデルを適用して前記スコアを算出することを特徴とする請求項１に記載の自己位置推定プログラム。
5. 前記スコア算出手順は、前記入力画像における前記ランドマークの撮像状態に応じて、前記尤度モデルを当該撮像状態に応じた重み付けで適用して前記スコアを算出することを特徴とする請求項１に記載の自己位置推定プログラム。

Images