JP6411917B2 - 自己位置推定装置および移動体 - Google Patents

Description

本発明は、カメラを搭載した移動体が自己位置推定するときの、自己位置推定技術に関する。

従来から、ロボットや自動車などの移動体にロータリエンコーダやジャイロセンサなどの内界センサ、カメラやレーザ距離センサなどの外界センサを搭載し、それらのセンサから取得した情報をもとに、移動体の自己位置を推定する装置が種々提案されている。ロータリエンコーダやジャイロセンサなどの内界センサで取得した移動体の移動量の積算により基準点からの相対的な自己位置を推定する手法はデットレコニングといい、計算処理が低負荷でありリアルタイム性に優れるが、移動量に応じて位置誤差が蓄積する。一方、カメラやレーザレンジファインダなどの外界センサを用い、自己位置の目印となる周囲走行環境の特徴を検出し、地図参照（マップマッチング）により把握した前記特徴の絶対位置をもとに自己位置を推定する手法があり、位置精度を向上するが、計算処理が高負荷である。このように、デットレコニングを用いた手法とマップマッチングを用いた手法は各々精度と処理負荷のトレードオフとなっており、高精度かつ低処理負荷な自己位置推定を実現するため、デットレコニングとマップマッチングを時系列で複合的に利用し、さらに屋外ではＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）に代表される衛星測位も複合的に利用する装置も種々提案されている。

例えば、下記特許文献１（特開２００８−１６５２７５）では、位置が変動しないランドマークの地図を用意し、カメラやレーザ距離センサなどの外界センサによりランドマーク候補を検出する。また、ランドマーク候補と地図を毎回照合するのは高処理負荷であるため、前回に地図と照合した前ランドマーク候補と現ランドマーク候補とを対応付けることにより、現ランドマーク候と地図とを直接照合する数を減らしている。

例えば、下記特許文献２（特開２００４−５５９３）では、自己位置推定用の環境情報を取得し、取得した環境情報と地図情報に含まれる環境情報とを照合し、照合結果にもとづいて自己位置推定値の修正を行う。環境情報と地図情報に含まれる環境情報との照合に際し、自己位置推定値にもとづいて、予め設定されている所定範囲内で地図との照合を行い、この照合で自己位置の修正情報を得られない時は、前記所定範囲を広げて再度地図との照合を行うことで、自己位置の見失いを回避している。

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

以上のように、自己位置を推定する手法として、内界センサを利用したデットレコニングや、外界センサによるマップマッチングを利用した手法、ＧＰＳなどの衛星測位がある。さらに、特許文献１のように、これらの自己位置推定手法を時系列で複合的に利用することにより、自己位置推定を精度よく、かつ低処理負荷で実施することができる。

しかし、デットレコニングやマップマッチングを利用した手法や衛星測位は、いずれも自己位置推定の確度を保障するものではないため、これらの自己位置推定手法を複合的に利用しても、自己位置推定の確度は向上しない。そのため、しばしば推定自己位置の誤差増大による自己位置の見失いが起きる。すなわち特許文献１に代表されるような、デットレコニングとマップマッチングを時系列で複合的に利用する従来装置の場合、過去の自己位置の推定誤差が現在の自己位置推定結果に反映されるため、移動量が増大するほどマップマッチングの確度が低下し、やがて自己位置を見失うことがある。また、屋外ではＧＰＳなどの衛星測位が利用可能であるが、衛星測位は天候や時間帯により精度劣化や測位信号を受信できず測位できないことがあるため、自己位置を見失ったときの代替手段となりにくい。

これに対して特許文献２は、自己位置の見失いを回避するため、マップマッチングに成功するまで地図の走査範囲を徐々に広げている。しかし、マップマッチングの際の地図の捜査範囲を広げるほど、地図の照合対象が増加するため、照合対象が明示的なマーカでない限りマップマッチングの確度は向上せず、よって、自己位置推定の確度は向上しない。

このように、明示的なマーカが存在しない任意の走行環境において、天候や時間帯により精度が容易に変動する衛星測位に頼らずに、自己位置推定の確度を向上することが課題となっている。

本発明は、カメラを搭載する移動体において、処理負荷の増加を抑えつつ自己位置を見失った状態から回復し、自己位置推定の確度を向上できる自己位置推定装置等を提供することを目的とする。

上記課題を解決するために、代表的な本発明の自己位置推定装置等の１つは、複数の参照用画像と、前記複数の参照用画像それぞれの撮像位置とを対応して記憶する記憶部と、移動体に備えるセンサから得られた情報に基づき、周期的に前記移動体の自己位置を推定する演算部と、を備え、前記演算部は、自己位置の推定に失敗すると、当該失敗以前に自己位置の推定に成功した最新自己位置から移動距離を、前記センサからの情報を用いて求め、前記最新自己位置から前記移動距離の範囲内に属する前記参照用画像を複数抽出し、抽出した複数の前記参照用画像から、前記移動体に備える撮像装置によって撮像された現在画像と類似する前記参照用画像を複数検索し、検索した複数の前記参照用画像に対応付けられた撮像位置の分布をもとに撮像位置の集合から外れた点を除去して、前記移動体の自己位置を推定するものである。

上記態様の自己位置推定装置等によれば、処理負荷の増加を抑えつつ自己位置を見失った状態から回復し、自己位置推定の確度を向上することができる。

実施例１の形態による移動体の構成を示すブロック図である。 実施例１の形態による自己位置推定装置のフローチャートである。 実施例１の形態による類似画像検索部のフローチャートである。 実施例１の形態による暫定自己位置推定部のフローチャートである。 実施例１の形態による自己位置推定装置の内容を示す図である。 実施例１の形態による自己位置推定装置の内容を示す図である。 実施例１の形態による自己位置推定装置の内容を示す図である。 実施例１の形態による自己位置推定装置の内容を示す図である。 実施例１の形態による自己位置推定装置の内容を示す図である。

以下、図面を用いて実施例を説明する。

本実施例では、自己位置推定する自動車やロボットなどの移動体において、カメラを搭載し、自己位置を見失ったとき、本発明の自己位置推定装置により、自己位置を見失った状態から回復する。

図１は、本実施例による移動体１００の構成を示し、移動体１００は、内外界センサ２００と、本発明の自己位置推定装置で利用するカメラ２０１、さらに、ＣＰＵ１０１と記憶装置１０２を搭載している。尚、図１ではＣＰＵ１０１が実行するプログラムとして通常自己位置推定部２０２と、自己位置推定部２０４を機能ブロック図として示している。

内外界センサ２００は、デットレコニングを実施するためのロータリエンコーダやジャイロセンサや慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）、マップマッチングを実施するためのレーザ距離センサやカメラやレーザレーダ、衛星測位のためのＧＰＳセンサなど、後述の通常自己位置推定部２０２で実施する自己位置推定手法に必要となるセンサである。後述の通常自己位置推定部２０２がカメラ画像を用いた処理を実施する場合は、本発明で利用するカメラ２０１を内外界センサ２００としても良い。

カメラ２０１は、走行環境を広く撮像するため、複数の標準カメラを搭載したり、広角カメラや超広角カメラを搭載したりするのが望ましい。さらに、夜間などの暗所を走行する場合は、赤外カメラを搭載するのが望ましい。また、複数の標準カメラを搭載する場合は、移動体の周囲を満遍なく撮像できるような位置に搭載するのが望ましい。広角カメラや超広角カメラを搭載する場合は、移動体の頂点に上向きに搭載するのが望ましい。さらに、搭載時はカメラの姿勢が動かないように固定するのが望ましいが、カメラの姿勢を常に把握するシステムを搭載している場合は、カメラの姿勢を固定しなくても良い。さらに、カメラ２０１で撮像する画像はカラー画像でもグレースケール画像でもよい。 通常自己位置推定部２０２は、内外界センサ２００で取得した情報をもとに、自己位置を推定する。内界センサ２００がロータリエンコーダやジャイロセンサ、または慣性計測装置である場合は、デットレコニングにより自己位置を推定する。内外界センサ２００がレーザ距離センサやカメラやレーザレーダの場合は、内外界センサ２００で検出した走行環境の特徴情報と地図２０５とを照合するマップマッチングを利用し、自己位置を推定する。通常自己位置推定部２０２により推定される自己位置の精度向上および処理負荷低減のため、様々な内外界センサ２００を搭載し、前記デットレコニングとマップマッチング、さらに屋外の場合は、ＧＰＳなどの衛星測位を時系列で複合的に利用するのが望ましい。デットレコニングやマップマッチングを利用した手法や衛星測位の結果を時系列で複合的に利用する手法として、例えば、拡張カルマンフィルタやパーティクルフィルタがあるが、それ以外の手法を用いても良い。

条件分岐２０３では、通常自己位置推定部２０２で自己位置推定に成功した場合、通常自己位置推定部２０２で推定した自己位置を最新自己位置２０６として記憶装置１０２に記憶させ、通常自己位置推定部２０２に移動する。通所自己位置推定部２０２で自己位置推定に失敗した場合、本発明の自己位置推定部２０４に移動する。

自己位置推定部２０４は、通常自己位置推定部２０２で自己位置推定に失敗し、自己位置を見失ったとき、自己位置を見失った状態から回復するために参照する自己位置推定回復用ＤＢ２０７と、地図２０５と、通常自己位置推定部２０２で自己位置推定に成功した最新自己位置２０６をもとに、自己位置を推定し、その結果を通常自己位置推定部２０２に返す。自己位置推定部２０４の処理の詳細は後述する。

地図２０５は、過去に撮像した走行環境の画像（参照用画像）と、前記参照用画像の撮像位置姿勢の情報を含んでいる。前記参照用画像を撮像するカメラは、移動体１００に搭載したカメラ２０１が標準カメラであれば標準カメラが望ましく、カメラ２０１が広角カメラであれば広角カメラが望ましく、超広角カメラであれば超広角カメラが望ましい。また、カメラ２０１と地図２０５の参照用画像を撮像するカメラの解像度に違いがあっても良い。すなわち、地図２０５には、複数の参照用画像と、それぞれの参照用画像の撮像位置が対応させて記憶されており、カメラで撮像した画像と参照用画像を照合して、類似するものを検索することで移動体の自己位置を推定することができる。

図２は、図１の自己位置推定部２０４のフローチャートを示している。図３は、図２の類似画像検索部Ｓ１０２の詳細を示している。図４は、図２の暫定自己位置推定部Ｓ１０４の詳細を示している。図５、図６、図７、図８および図９は、本発明の自己位置推定部２０４の処理内容を示している。

撮像部Ｓ１００は、通常自己位置推定部２０２で最新自己位置２０６（図５のＤ１００）を更新できなくなり、自己位置を見失った地点（図５のＤ１０１）において、移動体１００に搭載したカメラ２０１で走行環境を撮像し、撮像した前記画像を現在画像２０８として記憶装置１０２に記憶させる。

デットレコニング部Ｓ１０１は、最新自己位置２０６（図５のＤ１００）からの移動体１００の移動量（図５のＤ１０３）を算出し、移動距離２０９を記憶装置１０２に記憶させる。デットレコニングには、移動体１００の車輪の回転数を取得するロータリエンコーダを利用したホイールオドメトリや、移動体１００の移動加速度や角加速度を取得するジャイロセンサや慣性計測装置を利用した慣性航法装置（ＩＮＳ：Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）、さらに、カメラで走行環境を継続的に撮像し、撮像した画像の変化分から移動体１００の移動量を算出するビジュアルオドメトリなどがあり、それぞれの手法を複合的に利用しても良く、どれか１つの手法のみ利用してもよい。ホイールオドメトリを利用する場合、車輪の回転数から即座に移動量が算出できるため低処理負荷でありリアルタイム処理に向いているが、路面が未舗装または起伏が大きい環境では、車輪のスリップや車輪径の変動の影響を受けやすいため、精度は低い。慣性航法装置を利用する場合、路面の舗装状態やその他の環境外乱の影響を受けにくい、ホイールオドメトリより精度は高いが、移動体１００がサスペンションを搭載している場合は、坂道などで移動体が揺れると、移動体１００の３方向姿勢の角加速度が変化するため、精度は悪化する。また、慣性航法装置はホイールオドメトリに比べ、高処理負荷である。ビジュアルオドメトリを利用する場合、路面の舗装状態の影響を受けにくいが、視野の開けた環境ではカメラを路面に向け、できるだけ多くの特徴点を検出する必要がある。いずれの手法も移動量を積算するため、本発明の自己位置推定装置が開始した時点では移動距離２０９はゼロであるが、本発明の自己位置推定装置が終了するまで、移動距離２０９は積算され続ける。さらに、いずれの手法も基準地点からの相対的な並進移動量と回転量を算出する手法であるが、並進移動量が大きいほど、基準地点からの相対位置は回転量の誤差の影響を受けるため、デットレコニングにより算出した基準地点からの移動方向は移動距離よりも誤差が増大しやすい。

類似画像検索部Ｓ１０２は、デットレコニング部Ｓ１０１で算出した移動体１００の移動距離２０９をもとに、移動体１００の記憶装置に記憶された地図１０２の中から現在画像２０８の類似画像を検索する。

まず、図３のＳ３００では、地図２０５（図５のＤ１０２）の中から現在画像２０８の類似画像を検索する領域（図６のＤ２００）を設定する。類似画像の検索領域Ｄ２００は、最新自己位置２０６（Ｄ１００）を中心とし、移動距離２０９の２倍の値を半径とする領域とする。デットレコニング部Ｓ１０１で算出した移動量のうち、移動方向の情報を検索領域Ｄ２００の設定に用いない理由は、先述のように、
次に、図３のＳ３０１では、地図２０５（Ｄ１０２）に記憶された撮像位置が既知の参照用画像Ｄ１０２のうち、前記検索領域Ｄ２００に含まれる参照用画像Ｄ１０２を検索対象画像（図６のＤ２０１）とし、撮像部Ｓ１００で撮像した現在画像２０８の類似画像（図６のＤ２０２）を前記検索対象画像Ｄ２０１から検索する。このとき、現在画像２０８と検索した類似画像Ｄ２０２との画像類似度Ｍ（ｉ）（ｉ＝１、２、・・・）を算出する。類似画像検索の手法として、Ｂａｇ ｏｆ Ｋｅｙｐｏｉｎｔｓ（ｗｏｒｄｓ）や特徴点マッチングなどがあるが、類似画像Ｄ２０２の前記画像類似度を定量化できる手法であれば、それ以外の手法でも良い。例えば、Ｂａｇ ｏｆ Ｋｅｙｐｏｉｎｔｓ（ｗｏｒｄｓ）は予め地図に登録された参照用画像２０５群をもとに輝度値を数種の基底（ボキャブラリ）に分解し、画像検索時、現在画像２０８の各ボキャブラリの出現頻度（ヒストグラム）と検索対象画像Ｄ２０１の前記ヒストグラムの距離を算出することにより、画像類似度Ｍ（ｉ）が得られる。このように画像類似度を算出することで、自己位置を推定するための情報が増えるため、より精確な自己位置推定を可能とする。

条件分岐Ｓ１０３では、類似画像検索部Ｓ１０２で類似画像が検索されなかった場合、撮像部Ｓ１００に移動し、類似画像撮像部Ｓ１０２で類似画像が検索された場合、暫定自己位置推定部Ｓ１０４に移動する。このように、類似画像が検索されるまで繰り返し処理を行うため、より高確度な自己位置推定を可能とする。

暫定自己位置推定部Ｓ１０４は、類似画像Ｄ２０２の撮像位置の分布と類似画像Ｄ２０２の前記画像類似度Ｍ（ｉ）をもとに、自己位置存在エリアを算出し、前記自己位置存在エリアを暫定自己位置２１０として記憶装置１０２に記憶させる。

まず、図４のＳ４００では、類似画像検索部Ｓ１０２で検索された類似画像Ｄ２０２のうち、撮像位置が外れている不要類似画像（図７のＤ３００）を取り除き、取り除かなかった類似画像を要類似画像（図７のＤ３０１）とし以降の処理で利用する。位置が集合から外れた点を抽出する手法として、Ｋ平均法などのデータクラスタリングや、独立成分分析などの成分分析法、スミノルフ・グラブス検定などの仮説検定があるが、どの手法を用いても良い。データクラスタリングの場合は、非常に高速であるためリアルタイム性に優れるが、初期値の設定によっては１つの集合が複数の集合に分割されることがあるため、クラスタの統合処理が必要である。成分分析の場合は、主成分分析であれば低処理負荷であるが直交性の拘束条件により正確な外れ値除去ができない可能性があり、独立成分分析であれば直交性の拘束条件がないため、より正確な外れ値除去が可能であるが、非線形最小化処理のため、値の収束に時間を要することがある。仮説検定は、データクラスタリングと同等の処理負荷であり、外れ値除去の確度が最も高いが、データ数が少ない場合は利用することができない。

次に、図４のＳ４０１では、類似画像検索部Ｓ１０２の処理Ｓ３０１で算出したそれぞれの要類似画像Ｄ３０１の周りに自己位置存在尤度（図８のＤ４００）を設定する。前記存在尤度Ｄ４００は、要類似画像Ｄ３０１を中心（図８のＤ４０１）とする半径Ｒ（ｉ）（図８のＤ４０２）の円として設定する。ここで、画像類似度Ｍ（ｉ）（ｉ＝１、２、３・・・）の逆数１／Ｍ（ｉ）を前記半径Ｒ（ｉ）とする。

図４のＳ４０２では、それぞれの要類似画像Ｄ３０１の周辺に設定した前記自己位置存在尤度Ｄ４００を融合し、自己位置の平均と分散共分散行列で表される誤差楕円（図９のＤ５００）を算出し、前記平均（図９のＤ５０１）を自己位置の最良推定値（μｘ、μｙ）とする。前記最良推定値は、要類似画像Ｄ３０１の座標（ｘ（ｉ）、ｙ（ｉ））と前記画像類似度Ｍ（ｉ）を用いて、μｘ＝（Σｘ（ｉ）Ｍ（ｉ））／（ΣＭ（ｉ））、μｙ＝（Σｙ（ｉ）Ｍ（ｉ））／（ΣＭ（ｉ））で算出する。つまり、Ｍ（ｉ）／（ΣＭ（ｉ））を重みとする重み付き平均が前記最良推定値となる。よって、前記分散共分散行列は、要類似画像Ｄ３０１の座標（ｘ（ｉ）、ｙ（ｉ））にＭ（ｉ）／（ΣＭ（ｉ））の重みを付けたときの分散を対角成分に、共分散を非対角成分にもつ。前記誤差楕円Ｄ５００は、まず信頼区間χ２を設定し、前記分散共分散行列の２つの固有ベクトルを軸とし、それぞれの固有値と前記信頼区間χ２の積の２乗根をとった値を、長軸および短軸の長さとして算出する。前記誤差楕円における中心（μｘ、μｙ）が暫定自己位置２１０の最良推定値であり、前記誤差楕円で表されるエリアが暫定自己位置２１０の存在エリアとなる。このように画像類似度の逆数を撮像位置に重み付けし、重み付けした撮像位置の平均と分散共分散行列を算出することで、自己位置の存在エリアを確定することができる。これによって、確率過程を考慮した自己位置推定システムに組み込むことが可能となり、また、従来の自己位置推定システムのような非線形最適化を一切実施しないため、低処理負荷で自己位置推定が可能となる。

以上のような構成により、本発明の自己位置推定装置および移動体は、最新自己位置を中心に地図参照エリアを設定し、前記エリア内で現在の画像の類似画像を地図中の画像から検索し、その分布をもとに自己位置を推定することで、任意の走行環境において、自己位置推定の確度を向上し、処理負荷を低減できる。そのため、自己位置を見失うことが多い自己位置推定装置に組み込むことで、自己位置を見失った状態から回復できる。

すなわち、本実施例に記載の自己位置推定装置は、カメラを搭載し、撮像位置が既知の参照用画像を地図として持つ移動体が自己位置推定するための自己位置推定装置であって、自己位置推定装置は、現在の走行環境の画像（現在画像）を前記カメラで撮像する撮像部と、最後に推定した最新自己位置からの移動距離を算出するデットレコニング部と、前記最新自己位置を中心とし、前記移動距離を半径とするエリアにある参照用画像を地図から取り出し、前記の現在画像の類似画像を前記参照用画像から検索する類似画像検索部と、前記類似画像の撮像位置の分布をもとに自己位置を推定する暫定自己位置推定部と、を有することを特徴とする。これによって、従来の自己位置推定の確度を向上できる。また、高精度ゆえに自己位置を見失いやすい自己位置推定システムに組み込むことで、自己位置を見失っても、その状態から回復することができる。

なお、本発明は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。

１００ 移動体
２０１ カメラ
２０２ 通常自己位置推定部
２０４ 自己位置推定装置
２０５ 地図
２０６ 最新自己位置

Claims (5)

1. 複数の参照用画像と、前記複数の参照用画像それぞれの撮像位置とを対応して記憶する記憶部と、
   移動体に備えるセンサから得られた情報に基づき、周期的に前記移動体の自己位置を推定する演算部と、を備え、
   前記演算部は、
   自己位置の推定に失敗すると、当該失敗以前に自己位置の推定に成功した最新自己位置から移動距離を、前記センサからの情報を用いて求め、
   前記最新自己位置から前記移動距離の範囲内に属する前記参照用画像を複数抽出し、抽出した複数の前記参照用画像から、前記移動体に備える撮像装置によって撮像された現在画像と類似する前記参照用画像を複数検索し、検索した複数の前記参照用画像に対応付けられた撮像位置の分布をもとに撮像位置の集合から外れた点を除去して、前記移動体の自己位置を推定する
   自己位置推定装置。
2. 請求項１において、
   前記演算部は、前記現在画像と類似する前記参照用画像を検索する際、前記現在画像と前記参照用画像の画像類似度を算出する
   自己位置推定装置。
3. 請求項２において、
   前記演算部は、算出した前記画像類似度を前記参照用画像の撮像位置に重み付けし、重み付けした撮像位置の平均と分散共分散行列を算出して自己位置を推定する
   自己位置推定装置。
4. 請求項１において、
   前記演算部は、前記現在画像と類似する前記参照用画像が検索されなかったとき、前記撮像装置で再度撮像した、前記現在画像とは異なる現在画像を用いて自己位置推定を行う
   自己位置推定装置。
5. 請求項１の自己位置推定装置を備えた移動体。