JP2015087149A - 移動体位置姿勢角推定装置及び移動体位置姿勢角推定方法 - Google Patents

Abstract

【課題】エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定する。【解決手段】本発明の移動体位置姿勢角推定装置１０は、投影画像上の評価点に対して設定された評価値を補正する評価値補正部２５を備え、この評価値補正部２５は、投影画像上に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出し、投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点の数を増加させることで、評価値が増大するように評価値を補正する。【選択図】図１

Description

本発明は、パーティクルフィルタを用いて移動体の位置及び姿勢角を推定する移動体位置姿勢角推定装置及びその方法に関する。

三次元地図とカメラの撮像画像とを比較して移動体の位置を算出する技術として、特許文献１が開示されている。この技術では、移動体に搭載されたカメラで撮像した範囲内に映像的な特徴点の三次元情報を生成し、この三次元情報を記録した記録媒体と現実の映像とが一致する三次元座標上の地点と方向を求めて移動体の位置や姿勢角を求めていた。

この従来技術では、算出される移動体の位置や姿勢角の誤差分布が大きくなる特徴点群を抽出し、この特徴点群を除いた残りの特徴点を用いて再演算を行って位置や姿勢角の算出精度を向上させていた。

国際公開ＷＯ２００５／０３８４０２Ａ１号公報

しかしながら、上述した従来の技術では、特徴点であるエッジの配置を考慮せずに移動体の位置と姿勢角を推定していたので、エッジが偏って存在している場合には移動体の位置と姿勢角の推定結果に大きな誤差を含んでいる恐れがあるという問題点があった。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定することのできる移動体位置姿勢角推定装置及び移動体位置姿勢角推定方法を提供することを目的とする。

上述した課題を解決するために、本発明は、投影画像上に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出し、移動体の進行方向に第１方向の軸を設定し、移動体の進行方向と直交する移動体の横方向に第２方向の軸を設定する。そして、投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、第１方向成分の長さの総和が小さい場合に第１方向を向いたエッジ上の評価値が増大するように評価値を補正する。

本発明によれば、エッジの配置を考慮して評価値を補正できるので、エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定することができる。

図１は、本発明の第１実施形態に係る移動体位置姿勢角推定装置を備えた移動体位置姿勢角推定システムの構成を示すブロック図である。 図２は、本発明の第１実施形態に係る移動体位置姿勢角推定装置による移動体の位置姿勢角推定処理の処理手順を示すフローチャートである。 図３は、本発明の第１実施形態に係る移動体位置姿勢角推定装置による評価値の補正方法を説明するための図である。 図４は、本発明の第１実施形態に係る移動体位置姿勢角推定装置による評価値の補正方法を説明するための図である。 図５は、本発明の第１実施形態に係る移動体位置姿勢角推定装置による評価値の補正方法を説明するための図である。 図６は、本発明の第２実施形態に係る移動体位置姿勢角推定装置による評価値の補正方法を説明するための図である。

以下、本発明を適用した第１及び第２実施形態について図面を参照して説明する。

［第１実施形態］
［移動体位置姿勢角推定システムの構成］
図１は、本実施形態に係る移動体位置姿勢角推定装置を搭載した移動体位置姿勢角推定システムの構成を示すブロック図である。ただし、本実施形態では移動体の一例として、車両に適用した場合について説明する。

図１に示すように、本実施形態に係る移動体位置姿勢角推定システムは、ＥＣＵ１と、カメラ２と、三次元地図データベース３と、車両センサ群４とを備えている。

ここで、ＥＣＵ１は、ＲＯＭ、ＲＡＭ、演算回路等によって構成された電子制御ユニットであり、本実施形態に係る移動体位置姿勢角推定装置１０を備えている。尚、ＥＣＵ１は他の制御に用いるＥＣＵと兼用してもよい。

カメラ２は、例えばＣＣＤ等の固体撮像素子を用いたものであり、本実施形態では、車両のフロントバンパーに光軸が水平で、尚且つ車両前方が撮像可能となるように設置されている。撮像された画像はＥＣＵ１へ送信される。

三次元地図データベース３は、三次元位置情報を記憶しており、例えば路面表示を含む周囲環境のエッジ等を記憶している。本実施形態では、少なくとも道路端部を示す区画線や縁石等の位置とその向きの三次元情報が記録されており、この他に白線、停止線、横断歩道、路面マーク等の路面表示の他に、建物等の構造物のエッジ情報も含まれている。尚、三次元地図データベース３のデータフォーマットとして、道路端部等の各地図情報は、エッジの集合体として定義されている。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す三次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す三次元位置情報を持っている。

車両センサ群４は、ＧＰＳ受信機４１と、アクセルセンサ４２と、ステアリングセンサ４３と、ブレーキセンサ４４と、車速センサ４５と、加速度センサ４６と、車輪速センサ４７と、その他センサ４８とを備えている。車両センサ群４は、ＥＣＵ１に接続され、各センサ４１〜４８によって検出された各種の検出値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることによって、車両の概位置を算出したり、単位時間に車両が進んだ移動量を示すオドメトリを算出する。

移動体位置姿勢角推定装置１０は、パーティクルフィルタを用いて、車両の周囲環境を撮像した画像と三次元地図データのマッチングを行って車両の位置及び姿勢角を推定する装置である。そして、特定のプログラムを実行することにより、エッジ画像算出部１２、オドメトリ算出部１４、パーティクル存在分布範囲設定部１６、パーティクル散布部１８として動作する。さらに、投影画像作成部２０、評価点設定部２２、尤度算出部２４、評価値補正部２５、位置姿勢角推定部２６として動作する。

エッジ画像算出部１２は、カメラ２から車両の周囲環境を撮像した画像を取得し、この画像からエッジを検出してエッジ画像を算出する。カメラ２で撮像された画像には、自車両の位置及び姿勢角を推定するために必要な路面情報として、少なくとも道路端部を示す区画線や縁石が撮像されている。また、この他に白線、停止線、横断歩道、路面マーク等の路面表示が撮像されていてもよい。

オドメトリ算出部１４は、車両センサ群４から得られる各種センサ値を用いて、車両の単位時間当たりの移動量であるオドメトリを算出する。

パーティクル存在分布範囲設定部１６は、オドメトリ算出部１４で算出されたオドメトリの分だけ移動した位置と姿勢角の近傍に所定範囲のパーティクルの存在分布範囲を設定する。

パーティクル散布部１８は、パーティクル存在分布範囲設定部１６によって設定されたパーティクルの存在分布範囲内にパーティクルを散布する。

投影画像作成部２０は、パーティクル散布部１８によって散布されたパーティクルのそれぞれについて投影画像を作成する。例えば、三次元地図データベース３に記憶されたエッジ等の三次元位置情報を、各パーティクルの位置及び姿勢角からカメラ２で撮像した画像となるように投影変換して投影画像を作成する。

評価点設定部２２は、投影画像とエッジ画像とを比較するための評価点を投影画像上に設定する。

尤度算出部２４は、投影画像とエッジ画像とを比較して、エッジ画像上のエッジと一致する投影画像上の評価点を対象評価点とし、この対象評価点に対して所定の評価値を設定して、この評価値に基づいて尤度を算出する。ここで、尤度とは、各パーティクルの位置及び姿勢角が、どれぐらい実際の車両の位置及び姿勢角に対して尤もらしいかを示す指標であり、投影画像とエッジ画像との一致度が高いほど、尤度が高くなるように設定されている。

評価値補正部２５は、尤度算出部２４によって設定された評価値を補正する。具体的に説明すると、評価値補正部２５は、投影画像上に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出し、車両の進行方向に第１方向の軸を設定し、車両の進行方向と直交する車両の横方向に第２方向の軸を設定する。そして、投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価値が増大するように評価値を補正する。特に、本実施形態では、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点の数を増加させることで、評価値が増大するように補正する。

位置姿勢角推定部２６は、尤度算出部２４によって算出された尤度に基づいて車両の位置及び姿勢角を推定する。例えば、尤度が最も高いパーティクルの位置及び姿勢角を、車両の実際の位置及び姿勢角の推定結果として算出する。

尚、本実施形態では車両の６自由度の位置（前後方向，車幅方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を求めている。しかし、例えばサスペンション等を備えていない工場等で使用される無人搬送車のように３自由度の位置（前後方向，横方向）及び姿勢角（ヨー）を推定する場合でも本技術を適用することは可能である。具体的に、このような車両では、上下方向の位置と姿勢角のロール及びピッチは固定となるので、予めこれらのパラメータを計測しておいて利用すればよい。

［移動体の位置姿勢角推定処理の手順］
次に、本実施形態に係る移動体の位置姿勢角推定処理の手順を図２のフローチャートを参照して説明する。尚、本実施形態では、車両の位置情報として進行方向、横方向（車幅方向）、上下方向、姿勢角情報としてロール、ピッチ、ヨーの合計６自由度の位置と姿勢角を推定する。ただし、ロールは車両の進行方向を軸とした回転方向であり、ピッチは車両の横方向を軸とした回転方向であり、ヨーは車両の上下方向を軸とした回転方向である。

また、以下で説明する移動体の位置姿勢角推定処理は、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

図２に示すように、まずステップＳ１１０において、エッジ画像算出部１２は、カメラ２の画像からエッジ画像を算出する。本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指している。エッジの検出方法としては、例えばCanny法を用いることができる。この他にも微分エッジ検出など様々な手法を使用してもよい。

また、エッジ画像算出部１２は、カメラ２の画像からエッジの輝度変化の方向やエッジ近辺のカラーなどについても抽出することが望ましい。これにより、後述するステップＳ１６０及びステップＳ１７０において、三次元地図データベース３に記録されているこれらエッジ以外の情報についても利用して尤度を設定し、自車両の位置及び姿勢角を算出することが可能になる。

次に、ステップＳ１２０において、オドメトリ算出部１４は、車両センサ群４から得られるセンサ値に基づいて、１ループ前のステップＳ１２０で算出した時点から現在までの車両の移動量であるオドメトリを算出する。なお、プログラムを開始して最初のループの場合には、オドメトリをゼロとして算出する。

オドメトリ算出部１４は、オドメトリの算出方法として、車両運動を平面上に限定した上で、左右輪の車輪速センサ４７のエンコーダ値の差からヨー角方向の旋回量（旋回角）を算出する。そして、進行方向と横方向の移動量は、各車輪の車輪速センサ４７のエンコーダ値から平均の移動量を求め、この移動量に対してヨー角方向の旋回角の余弦と正弦を取ることで求めることができる。また、各車輪の車輪速とヨーレートから単位時間での移動量と回転量を算出してもよい。さらに、車輪速を車速やＧＰＳ受信機４１の測位値の差分で代用してもよく、ヨーレートを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリを算出できればどの手法を用いてもよい。

特に、横方向やヨー角方向の移動量をより正確に算出するために、例えば車速約１０ｋｍ／ｈ以下の極低速ではアッカーマンステアリングジオメトリ（自動車の運動と制御 第３章、安部正人著、山海堂発行）に従ってオドメトリを求めてもよい。また、それ以上の車速ではタイヤの横滑り角を考慮できる線形２輪モデルの運動方程式（自動車の運動と制御 第３章3.2.1節P.56（3.12）及び（3.13）式、安部正人著、山海堂発行）を用いて算出するとなお良い。ただし、線形２輪モデルの運動方程式を用いる場合には予め各車輪のタイヤパラメータ（コーナーリングパラメータ）を測定しておく必要がある。

次に、ステップＳ１３０において、パーティクル存在分布範囲設定部１６は、１ループ前のステップＳ１７０で推定した各パーティクルの位置及び姿勢角を、今回のステップＳ１２０で算出したオドメトリの分だけ移動させる。ただし、プログラムを開始して初めてのループの場合には、前回の車両位置情報がないので、車両センサ群４に含まれるＧＰＳ受信機４１からのデータを初期位置情報とする。また、前回の停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報としてもよい。

そして、オドメトリの分だけ移動させた車両の位置と姿勢角の近傍に車両の動特性や走行状態を考慮してパーティクルの存在分布範囲を設定する。

尚、パーティクル存在分布範囲設定部１６は、車両センサ群４の測定誤差や通信遅れによって生じるオドメトリの誤差や車両の動特性を考慮に入れて、車両の位置及び姿勢角の真値が取り得る可能性のある複数の予測位置と姿勢角の候補を生成する。この予測位置及び姿勢角の候補は、位置及び姿勢角の６自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で乱数表等を用いてランダムに設定する。本実施形態では、この予測位置及び姿勢角の候補を５００個作成している。そして、予測位置及び姿勢角の６自由度のパラメータについて誤差の上下限を、順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。

この予測位置及び姿勢角の候補を作成する数や、位置及び姿勢角の６自由度のパラメータについての誤差の上下限は、車両の運転状態や路面の状況を検出或いは推定して、適宜変更する。例えば、急旋回やスリップなどが発生している場合には、平面方向（進行方向、横方向、ヨー）の誤差が大きくなる可能性が高いので、これら３つのパラメータの誤差の上下限を大きくし、且つ予測位置の作成数を増やすようにする。

次に、ステップＳ１４０において、パーティクル散布部１８は、ステップＳ１３０で設定された存在分布範囲内にパーティクルを散布する。散布方法としては、パーティクルの位置及び姿勢角を規定する６自由度のパラメータに対して、それぞれステップＳ１３０で設定された範囲（上下限）の中で、乱数表等を用いてランダムに設定する。また、本実施形態では、パーティクルを常に５００個作成する。

尚、特開２０１０−２２４７５５号公報で開示された技術を用いてパーティクルを散布してもよい。この場合には、ステップＳ１３０でオドメトリの分だけ移動させた車両の位置と姿勢角が、ステップＳ１７０で算出した位置と姿勢角によってどれだけ補正されたかを算出し、この補正量に応じてパーティクルを散布する平均と分布を設定して散布すればよい。このとき、パーティクルを散布する範囲はステップＳ１３０で設定された存在分布範囲とする。また、存在分布範囲についても特開２０１０−２２４７５５号公報で開示された技術を用いて求め、ステップＳ１３０で設定された存在分布範囲とのＯＲをとってもよい。さらに、パーティクルを散布する数についても、特開２０１０−２２４７５５号公報に開示された技術を用いてステップＳ１３０で設定された存在分布範囲に応じて動的に決定してもよい。

次に、ステップＳ１５０において、投影画像作成部２０は、ステップＳ１４０で散布された複数のパーティクルについてそれぞれ投影画像（仮想画像）を作成する。このとき、評価点設定部２２は、例えば三次元地図データベース３に記憶されたエッジ等の三次元位置情報を、それぞれの予測位置及び姿勢角候補でのカメラ画像に投影変換して評価点を設定する。これにより、評価用の投影画像が作成される。この投影画像に投影された評価点群は、後述するステップＳ１６０において、ステップＳ１１０で算出したエッジ画像上のエッジと比較される。

尚、自車両周囲のエッジ情報の抽出方法は、三次元地図データベース３に記録された各エッジとカメラ２までの距離Ｄ（単位：ｍ）を算出し、この距離が閾値Ｄｔｈ（ｍ）以下のエッジ情報のみを抽出する。この距離の算出方法としては、厳密には各エッジの中央点とカメラ２の位置とで求めることが望ましいが、エッジの端点と車両位置で代替して計算コストを抑えてもよい。また、マッチングを行なうのはカメラ２の撮像範囲だけなので、カメラ２で撮像できる範囲のエッジのみを抽出してもよい。ただし、このとき用いる車両位置及び姿勢角は、ステップＳ１３０で求めた位置や姿勢角を用いる。すなわち、１ループ前のステップＳ１７０で推定した車両位置からステップＳ１２０で算出したオドメトリ分だけ移動させた位置や姿勢角を用いる。

また、投影変換では、カメラ２の位置を示す外部パラメータと、カメラ２の内部パラメータとが必要となる。外部パラメータは、車両からカメラ２までの相対位置を予め計測しておくことによって、予測位置及び姿勢角候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。

尚、ステップＳ１１０においてカメラ画像からエッジの輝度変化の方向やエッジ近辺のカラー等について抽出している場合には、それらを用いて投影画像を作成することが望ましい。

次に、ステップＳ１６０において、尤度算出部２４は、ステップＳ１４０で散布した複数のパーティクルのそれぞれについて、ステップＳ１１０で算出したエッジ画像と、ステップＳ１５０で作成した投影画像とを比較する。そして、比較した結果に基づいて、予測位置及び姿勢角候補であるパーティクルごとに尤度を算出する。この尤度とは、それぞれの予測位置及び姿勢角候補が、どれぐらい実際の車両の位置及び姿勢角に対して尤もらしいかを示す指標であり、投影画像とエッジ画像との一致度が高いほど、尤度が高くなるように設定されている。この尤度の求め方の一例を以下で説明する。

まず、投影画像上において評価点を特定し、この評価点にエッジ画像上のエッジが存在するか否かを判定する。そして、エッジの有無が一致する場合には評価点を対象評価点とし、この対象評価点に対して評価値１を設定する。一方、エッジの有無が一致しない場合には評価値として０を設定する。次に、設定された評価値を、評価値補正部２５によって補正し、このような処理を全評価点に対して行い、補正された評価値の総和である一致評価点の数を求める。そして、全ての予測位置及び姿勢角候補である各パーティクルについて一致評価点の数を算出したら、それぞれの合計値が１になるように正規化して尤度を算出する。尚、尤度の算出方法は、他にも多くの算出方法が考えられるので、それらの方法を用いてもよい。

ここで、評価値補正部２５による評価値の補正方法について説明する。評価値補正部２５は、投影画像上に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出し、車両の進行方向に第１方向の軸を設定し、車両の進行方向と直交する車両の横方向に第２方向の軸を設定する。そして、投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価値が増大するように補正する。特に、本実施形態では、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点の数を増加させることで、評価値が増大するように補正する。

ただし、以下に示す一例では、第１方向成分の長さの総和が小さい場合に第１方向を向いたエッジ上の評価値が増大するように補正し、特に第１方向を向いたエッジ上の評価点の数を増加させることで、評価値が増大するように補正する。

ここで、図３を参照して、評価値補正部２５による評価値の補正方法を具体的に説明する。図３に示すように、自車両３１が道路上を走行し、カメラ２の撮像範囲Ｗ内にエッジＥ１〜Ｅ５が存在している場合について説明する。

評価値補正部２５は、投影画像の撮像範囲Ｗ内に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出する。例えば、Ｅ１とＥ３やＥ２とＥ４等がエッジのペアとして抽出される。ここで、エッジのペアを構成する２つのエッジは平行ではなく１次独立であればよい。したがって、道路左端の白線によるエッジと道路右端の白線によるエッジがエッジのペアを構成することはない。

そして、図３では、車両の進行方向であるＸ軸方向に第１方向の軸を設定し、車両の横方向（車幅方向）であるＹ軸方向に第２方向の軸を設定する。ただし、地面と平行な南北方向を第１方向の軸とし、地面と平行な東西方向を第２方向の軸としてもよく、その他の方向であってもよい。

そして、投影画像内のすべてのエッジの第１方向成分の長さの総和Ｘａ（単位：ｍ）と第２方向成分の長さの総和Ｙａ（単位：ｍ）とを求める。図３では、エッジ画像内のエッジはＥ１〜Ｅ５となるので、
Ｘａ＝Ｘ１＋Ｘ２
Ｙａ＝Ｙ１＋Ｙ２＋Ｙ３
となる。

そして、評価値補正部２５は、ｉ番目のエッジＥｉの評価点の数Ｎｉ（小数点以下切り上げ）を、以下の式（１）に基づいて設定する。

Ｎｉ＝ｍａｘ［｛Ｘｉ÷（Ｘａ÷Ｎａ）｝，｛Ｙｉ÷（Ｙａ÷Ｎａ）｝，２]
＝ｍａｘ［｛（Ｘｉ・Ｎａ）／Ｘａ｝，｛（Ｙｉ・Ｎａ）／Ｙａ｝，２] （１）
ここで、式（１）において、Ｎａは各方向の評価点の総数であり、本実施形態では目安として１０００個とする。また、Ｘｉはｉ番目のエッジＥｉのＸ軸方向の長さ成分（単位：ｍ）であり、Ｙｉはｉ番目のエッジＥｉのＹ軸方向の長さ成分（単位：ｍ）である。尚、評価点の数Ｎｉを２以上としているのは、エッジの端点には必ず評価点を設定するためである。

具体的に、式（１）を用いて図３のエッジＥ１について評価点の数Ｎ１を計算すると、Ｘｉ＝Ｘ１、Ｙｉ＝０となるので、Ｎ１は
Ｎ１＝ｍａｘ［｛（Ｘ１・Ｎａ）／Ｘａ｝，｛（０・Ｎａ）／Ｙａ｝，２]
＝（Ｘ１・Ｎａ）／Ｘａ
となる。ただし、（Ｘ１・Ｎａ）／Ｘａ＜２の場合には、Ｎ１＝２となる。

このように、第１方向成分の長さの総和Ｘａと第２方向成分の長さの総和Ｙａのうち、Ｘａ、Ｙａの値が小さいほうが（Ｘｉ・Ｎａ）／Ｘａ、（Ｙｉ・Ｎａ）／Ｙａの値が大きくなる。したがって、評価点の数Ｎｉは、第１方向成分の長さの総和Ｘａと第２方向成分の長さの総和Ｙａとを比較して、長さの総和Ｘａ、Ｙａの小さいほうの値に基づいて設定されることになる。

その結果、長さの総和が小さい方向に多くの評価点が設定され、長さの総和が大きい方向に少ない評価点が設定されるので、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点の数を増加させることができる。これにより、長さの総和が小さい方向を向いたエッジ上の評価値が増大するように補正することができる。

図３の例では、第１方向成分の長さの総和が小さいので、第１方向を向いたエッジ上の評価値が増大するように補正するために、特に第１方向を向いたエッジ上の評価点の数を増加させる。尚、長さの総和が小さい方向の評価点の数を増加させるのではなく、長さの総和が大きい方向の評価点の数を減少させてもよい。

このように評価点の数を設定することによって、Ｘ軸方向のエッジとＹ軸方向のエッジには長さにかかわらず評価点の数を同等に配置することができる。

ただし、上述した式（１）を用いなくても、例えばＸａがＹａよりも小さい場合には、ＸｉがＹｉよりも長いエッジＥｉの評価点の数を１つだけ増やす、または所定数だけ増やすという簡便な方法で評価点の数を増加させてもよい。

そして、式（１）で算出した評価点の数Ｎｉで長さｌｉ（単位：ｍ）のエッジＥｉ上に評価点を設定する場合には、以下に示す式（２）で間隔Ｓｐｉ（単位：ｍ）を求め、その間隔ＳｐｉでエッジＥｉ上に評価点を配置する。

Ｓｐｉ＝ｌｉ÷（Ｎｉ−１） （２）
こうして評価値補正部２５は、エッジＥｉ上に評価点を設定することができる。

また、地面と平行な方向だけでなく、車両の鉛直方向にもう一つの軸（Ｚ軸）を設定し、Ｘ軸、Ｙ軸、Ｚ軸方向で評価点の数が均一になるように設定してもよい。この場合、式（１）を式（３）に示すように置き換えて評価点の数を求めればよい。

Ｎｉ＝ｍａｘ［｛Ｘｉ÷（Ｘａ÷Ｎａ）｝，｛Ｙｉ÷（Ｙａ÷Ｎａ）｝，｛Ｚｉ÷（Ｚａ÷Ｎａ）｝，２] （３）
ただし、Ｚａは投影画像内のすべてのエッジのＺ軸方向成分の長さの総和（単位:ｍ）、Ｚｉはｉ番目のエッジＥｉのＺ軸方向の長さ成分（単位：ｍ）である。

尚、上述の実施形態では、第１方向がＸ軸、第２方向がＹ軸の場合について説明したが、第１方向と第２方向がそれぞれＹ軸とＺ軸でもよいし、Ｘ軸とＺ軸でもよい。

また、評価点の密度が投影画像の解像度を上回るような場合には、解像度を上回った評価点を間引くようにしてもよい。

さらに、自車両（カメラ２）に近いエッジほど画面上では大きく写るので、式（１）、あるいは式（３）で求めた評価点の数を、自車両の近傍のエッジについては増加させ、遠方のエッジについては減少させるようにしてもよい。例えば、１０ｍ先のエッジについては評価点の数をその逆数である１／１０にし、その増減の倍率を縦軸にとり、距離を横軸にとったマップを参照して演算するようにしてもよい。

次に、図４、５を参照して、本実施形態に係る移動体位置姿勢角推定装置１０による効果を説明する。

図４は、屋外駐車場において、自車両の横方向に白線のエッジが多数あり、自車両の進行方向にはエッジがほとんど存在しない場合の一例を示している。この場合に、白線上のエッジに均等な間隔で評価点を設定し、自車両の位置と姿勢角を適当に変化させながら、その都度、投影画像とエッジ画像とをマッチングさせて位置と姿勢角を推定する。

このとき、マッチングした評価点を黒丸、マッチングしなかった評価点を白丸で示すと、図４（ａ）に示すように、位置と姿勢角が真値に近い状態で投影画像とエッジ画像が重畳した場合には、全ての評価点でマッチングしていることが分かる。

これに対し、自車両の位置が真値から前方向、または右方向に同じ距離だけずれた状態ではそれぞれ図４（ｂ）と図４（ｃ）に示すように変化する。すなわち、同じ距離だけずれているにもかかわらず、前方向にずれた図４（ｂ）では全ての評価点でマッチングしていない。これに対し、右方向にずれた図４（ｃ）ではほとんどの評価点でマッチングしている。

これは、エッジが横方向に極端に多く存在し、進行方向にはほとんど存在していないためである。したがって、エッジの長さに対して均等に評価点を設定していると、図４に示すようなエッジが偏って存在している状況では、自車両の位置と姿勢角を正確に推定することはできない。特に、図４では、横方向に誤差があっても検出しにくいので、横方向の位置を正確に検出することは困難であった。

そこで、本実施形態では、評価値補正部２５が、すべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、長さの総和が小さい方向の評価点の数を増加させることで、評価値が増大するように補正する。例えば、図４に示すように、車両の進行方向成分の長さの総和が小さい場合には、車両の進行方向の評価点の数を増加させるように補正する。

具体的に説明すると、図５（ａ）の拡大図に示すように、車両の進行方向にはエッジの端点のみに評価点が存在しているが、本実施形態では図５（ｂ）に示すように車両の進行方向を向いたエッジに対して評価点を２つ増加させている。

これにより、エッジが一定の方向に偏って存在している場合でも評価点の数を調整して移動体の位置と姿勢角を正確に推定することが可能となる。また、評価点の偏重を軽減できるので、車両の横方向の位置の変化に対するマッチングの感度が、車両の進行方向の感度に近づくことになり、マッチングの精度や安定性を向上させることができる。

次に、ステップＳ１７０において、位置姿勢角推定部２６は、ステップＳ１６０で算出した尤度情報を有する複数の予測位置及び姿勢角候補を用いて、最終的な車両の位置及び姿勢角を算出する。例えば、位置姿勢角推定部２６は、尤度が最も高い予測位置及び姿勢角候補を、車両の実際の位置及び姿勢角として算出する。また、各予測位置及び姿勢角候補の尤度を用いて、予測位置及び姿勢角の重み付き平均を求め、その値を最終的な車両の位置及び姿勢角としてもよい。こうして車両の位置と姿勢角の推定結果が算出されると、本実施形態に係る移動体の位置姿勢角推定処理を終了する。

［第１実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動体位置姿勢角推定装置では、投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較する。そして、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価値が増大するように評価値を補正する。これにより、エッジの配置を考慮して評価値を補正できるので、エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定することができる。

また、本実施形態に係る移動体位置姿勢角推定装置では、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点の数を増加させることで、評価値が増大するように評価値を補正する。これにより、長さの総和が大きい方向の評価点の数と小さい方向の評価点の数を均等にできるので、エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定することができる。

［第２実施形態］
次に、本発明の第２実施形態に係る移動体位置姿勢角推定装置について図面を参照して説明する。

［移動体位置姿勢角推定システムの構成］
本実施形態に係る移動体位置姿勢角推定装置を搭載した移動体位置姿勢角推定システムの構成は、図１に示した第１実施形態と同一なので、同一の番号を付して詳細な説明は省略する。

ただし、本実施形態では、評価値補正部２５の機能が第１実施形態と相違している。本実施形態の評価値補正部２５は、異なる方向を向いて交差するエッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点に対して加算点を設定することで、評価値が増大するように評価値を補正する。そして、この加算点は、エッジの長さの総和の逆数に基づいて設定する。

［移動体の位置姿勢角推定処理の手順］
本実施形態に係る移動体の位置姿勢角推定処理は、図２のステップＳ１６０で実行される評価値の補正方法が、第１実施形態と相違している。第１実施形態では、評価値補正部２５が評価点の数を増加させていたが、本実施形態では、長さの総和が小さい方向を向いたエッジ上の評価点に対して加算点を設定する。

図２のステップＳ１５０において、評価点設定部２２は、まず投影画像のエッジ上に評価点を設定する。このとき、本実施形態では評価点の数をエッジの長さに応じて均等に設定する場合について説明するが、上述した第１実施形態で説明したように、エッジの長さの総和が小さい方向の評価点の数を増加させるように設定してもよい。

そして、評価点を設定したら、ステップＳ１６０において、尤度算出部２４が、投影画像とエッジ画像とを比較して、エッジ画像上のエッジと一致する投影画像上の評価点に対して評価値を設定する。例えば、エッジの有無が一致する場合には１を設定し、一致しない場合には０を設定する。

ここで、設定された評価値を、評価値補正部２５が補正する。具体的に説明すると、評価値補正部２５は、投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点に対して加算点を設定する。

ただし、以下に示す一例では、第１方向成分の長さの総和が小さい場合に第１方向を向いたエッジ上の評価値が増大するように補正し、特に第１方向を向いたエッジ上の評価点に対して加算点を設定することで、評価値が増大するように補正する。

この加算点の設定方法を、第１実施形態の図３を例にして説明すると、評価値補正部２５は、以下に示す式（４）に基づいて尤度の加算点Δｉを設定する。

Δｉ＝ｍａｘ［（Ｘｉ÷Ｘａ），（Ｙｉ÷Ｙａ）］ （４）
ただし、Ｘａは投影画像内のすべてのエッジの第１方向成分の長さの総和（単位：ｍ）であり、Ｙａは第２方向成分の長さの総和（単位：ｍ）である。また、Ｘｉはｉ番目のエッジＥｉのＸ軸方向の長さ成分（単位：ｍ）であり、Ｙｉはｉ番目のエッジＥｉのＹ軸方向の長さ成分（単位：ｍ）である。

このように、第１方向成分の長さの総和Ｘａと第２方向成分の長さの総和Ｙａの逆数に応じて加算点が設定されるので、加算点Δｉは長さの総和Ｘａ、Ｙａの小さいほうの値に基づいて設定される。その結果、長さの総和が小さい方向の加算点が大きくなるので、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価値を増大させることができる。

図３の例では、第１方向成分の長さの総和が小さいので、第１方向を向いたエッジ上の評価値が増大するように補正するために、特に第１方向を向いたエッジ上の評価点に対して加算点を設定する。

ただし、上述した式（４）を用いなくても、例えばＸａがＹａよりも小さい場合には、ＸｉがＹｉよりも長いエッジＥｉ上の評価点における加算点を２倍にするという簡便な方法で評価値を補正してもよい。

こうして加算点Δｉが設定されると、尤度算出部２４は、各評価点に対して加算点Δｉを加算して尤度を算出する。

また、地面と平行な方向だけでなく、車両の鉛直方向にもう一つの軸（Ｚ軸）を設定してもよい。この場合、式（４）を式（５）に示すように置き換えて加算点Δｉを求めればよい。

Δｉ＝ｍａｘ［（Ｘｉ÷Ｘａ），（Ｙｉ÷Ｙａ），（Ｚｉ÷Ｚａ）］ （５）
ただし、Ｚａは投影画像内のすべてのエッジのＺ軸方向成分の長さの総和（単位:ｍ）、Ｚｉはｉ番目のエッジＥｉのＺ軸方向の長さ成分（単位：ｍ）である。

ここで、上述した加算点の設定方法について図６を参照して説明する。第１実施形態の図４で説明したように、エッジが車両の横方向に極端に多く存在し、車両の進行方向にほとんど存在していない場合には、エッジの長さに対して均等に評価点を設定すると、自車両の位置と姿勢角を正確に推定することができない。

そこで、本実施形態では、評価値補正部２５が、長さの総和が小さい方向を向いたエッジ上の評価値が増大するように加算点を設定する。

例えば、図６の拡大図に示すように、車両の進行方向を向いたエッジには、その端点に評価点Ｐ１、Ｐ２が存在しているだけなので、これらの評価点Ｐ１、Ｐ２に対して本実施形態では上述した式（４）に基づいて加算点Δｉを設定する。そして、尤度算出部２４は、尤度を算出する際に加算点Δｉを加算して尤度を算出する。

こうすることにより、長さの総和が小さい方向を向いたエッジ上の評価点において、尤度が大きく算出されることになる。したがって、エッジが一方向に偏って存在している場合でも正確に移動体の位置と姿勢角を推定することができる。

［第２実施形態の効果］
以上詳細に説明したように、本実施形態に係る移動体位置姿勢角推定装置では、エッジのペアのうち長さの総和が小さい方向を向いたエッジ上の評価点に対して加算点を設定することで、評価値が増大するように補正する。これにより、長さの総和が小さい方向を向いたエッジ上の評価点の尤度が大きくなるので、エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定することができる。

また、本実施形態に係る移動体位置姿勢角推定装置では、エッジの長さの総和の逆数に基づいて加算点を設定するので、長さの総和が小さい方向を向いたエッジ上にある評価点の尤度を大きくできる。これにより、エッジが偏って存在している場合でも移動体の位置と姿勢角を正確に推定することができる。

なお、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。

１ ＥＣＵ
２ カメラ
３ 三次元地図データベース
４ 車両センサ群
１０ 移動体位置姿勢角推定装置
１２ エッジ画像算出部
１４ オドメトリ算出部
１６ パーティクル存在分布範囲設定部
１８ パーティクル散布部
２０ 投影画像作成部
２２ 評価点設定部
２４ 尤度算出部
２５ 評価値補正部
２６ 位置姿勢角推定部
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ その他センサ

Claims (5)

1. 移動体の周囲環境を撮像した画像からエッジ画像を算出し、パーティクルフィルタを用いて散布されたパーティクルのそれぞれについて投影画像を作成し、前記投影画像と前記エッジ画像とを比較して尤度を算出することによって前記移動体の位置及び姿勢角を推定する移動体位置姿勢角推定装置であって、
   前記投影画像を作成する際に前記投影画像上に評価点を設定する評価点設定部と、
   前記投影画像と前記エッジ画像とを比較して、前記エッジ画像上のエッジと一致する前記投影画像上の評価点を対象評価点とし、前記対象評価点に対して所定の評価値を設定して前記評価値に基づいて尤度を算出する尤度算出部と、
   前記尤度算出部によって設定された評価値を補正する評価値補正部とを備え、
   前記評価値補正部は、前記投影画像上に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出し、前記移動体の進行方向に第１方向の軸を設定し、前記移動体の進行方向と直交する前記移動体の横方向に第２方向の軸を設定し、前記投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、前記第１方向成分の長さの総和が小さい場合に前記第１方向を向いたエッジ上の評価値が増大するように前記評価値を補正することを特徴とする移動体位置姿勢角推定装置。
2. 前記評価値補正部は、前記第１方向を向いたエッジ上の評価点の数を増加させることで、前記評価値が増大するように補正することを特徴とする請求項１に記載の移動体位置姿勢角推定装置。
3. 前記評価値補正部は、前記第１方向を向いたエッジ上の評価点に対して加算点を設定することで、前記評価値が増大するように補正することを特徴とする請求項１または２に記載の移動体位置姿勢角推定装置。
4. 前記加算点は、エッジの長さの総和の逆数に基づいて設定されることを特徴とする請求項３に記載の移動体位置姿勢角推定装置。
5. 移動体の周囲環境を撮像した画像からエッジ画像を算出し、パーティクルフィルタを用いて散布されたパーティクルのそれぞれについて投影画像を作成し、前記投影画像と前記エッジ画像とを比較して尤度を算出することによって前記移動体の位置及び姿勢角を推定する移動体位置姿勢角推定装置の移動体位置姿勢角推定方法であって、
   前記投影画像を作成する際に前記投影画像上に評価点を設定し、
   前記投影画像と前記エッジ画像とを比較して、前記エッジ画像上のエッジと一致する前記投影画像上の評価点を対象評価点とし、前記対象評価点に対して所定の評価値を設定して補正し、
   前記評価値を補正するときには、前記投影画像上に設定されたエッジの中から異なる方向を向いて交差するエッジのペアを抽出し、前記移動体の進行方向に第１方向の軸を設定し、前記移動体の進行方向と直交する前記移動体の横方向に第２方向の軸を設定し、前記投影画像上のすべてのエッジの第１方向成分の長さの総和と第２方向成分の長さの総和とを比較して、前記第１方向成分の長さの総和が小さい場合に前記第１方向を向いたエッジ上の評価値が増大するように前記評価値を補正することを特徴とする移動体位置姿勢角推定方法。