測位装置及び測位方法
【課題】移動体の位置及び姿勢を継続的に且つより精度良く推定すること。
【解決手段】周辺領域の画像を経時的に撮像する撮像部１と、移動体２００の移動量を検出する慣性センサ２と、を備える移動体２００の位置及び姿勢を推定する測位装置１００に、撮像部１によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、移動体２００の並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとを算出する画像処理部３と、慣性センサ２によって検出された移動量に基づいて、移動体２００の並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとを算出する慣性算出部４と、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとに基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定するカルマンフィルタ５と、を備え、誤差量に基づいて、慣性センサ２によって検出された移動量を補正するように構成した。
【解決手段】周辺領域の画像を経時的に撮像する撮像部１と、移動体２００の移動量を検出する慣性センサ２と、を備える移動体２００の位置及び姿勢を推定する測位装置１００に、撮像部１によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、移動体２００の並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとを算出する画像処理部３と、慣性センサ２によって検出された移動量に基づいて、移動体２００の並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとを算出する慣性算出部４と、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとに基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定するカルマンフィルタ５と、を備え、誤差量に基づいて、慣性センサ２によって検出された移動量を補正するように構成した。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置及び姿勢を推定する測位装置及び測位方法に関する。
【背景技術】
【０００２】
車両等の移動体は、ジャイロセンサや加速度センサ等の慣性センサを用いて、当該移動体の位置及び姿勢の算出を行っている。
例えば、特許文献１には、慣性センサとして、機械式ジャイロを用いる技術が記載されている。そして、特許文献１には、機械式ジャイロによって衛星の角速度を検出し、恒星センサによって衛星の姿勢を検出する技術が記載されている。
また、特許文献２には、動揺体の上に設置されたカメラの座標系及び座面を修正するために、動揺体の回転角を検出する技術が記載されている。
【０００３】
また、移動体にカメラを搭載して、カメラによって取得された画像を用いて、移動体の位置及び姿勢を推定する方法もある。
例えば、非特許文献１には、ステレオ画像から静止領域を取得し、当該静止領域を仮想的に上方から見た画像（Ｖｉｒｔｕａｌ−Ｐｒｏｊｅｃｔｉｏｎ−Ｐｌａｎｅ ｉｍａｇｅ：ＶＰＰ画像）に変換し、当該ＶＰＰ画像を時系列に沿ってマッチングすることにより、移動体の運動を推定する技術が記載されている。
また、非特許文献２には、画像から特徴点を生成して、時系列に沿って、各画像内の特徴点を比較することにより、移動体の運動を推定する技術が記載されている。
また、特許文献３には、車両の操舵角と、当該車両に搭載されたＣＣＤカメラによって検出される車両の横方向の相対位置とに基づいて、カルマンフィルタによる処理を行って、補正操舵角を演算する技術が記載されている。特許文献３では、ＣＣＤカメラ画像内の路面に描かれたセンターライン等に基づいて走行目標ラインを想定し、当該走行目標ラインと車両の前後方向に延びる基準線との間隔を、車両の横方向の相対位置として検出している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−１４２５９４号公報
【特許文献２】実開平０１−３０４５１０号公報
【特許文献３】特開平１０−２９７５２１号公報
【非特許文献】
【０００５】
【非特許文献１】関 晃仁、奥富 正敏、ステレオ動画像を利用した道路面領域の抽出と追跡による自車両の運動推定、情報処理学会論文誌：コンピュータビジョンとイメージメディア、日本、社団法人情報処理学会、２００６年３月、Ｖｏｌ．４７、Ｎｏ．ＳＩＧ５（ＣＶＩＭ１３）、９０−９９ページ
【非特許文献２】Ｎｉｋｌａｓ Ｋａｒｌｓｓｏｎ、他５名、Ｔｈｅ ｖＳＬＡＭ Ａｌｇｏｒｉｔｈｍ ｆｏｒ Ｒｏｂｕｓｔ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ、Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ） ２００５、２００５年４月、２４−２９ページ
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１、２に記載の慣性センサにおいては、温度などの影響によりドリフト誤差が発生する。そのため、これらの慣性センサの検出精度は、時間が経過すると劣化してしまう。なお、慣性センサとして、光ファイバジャイロ（ＦＯＧ；Ｆｉｂｅｒ Ｏｐｔｉｃ Ｇｙｒｏ）等を用いることにより、精度を向上することができるが、コストが大幅に高くなってしまう。
【０００７】
また、非特許文献１のように、ステレオ画像を用いる場合、オクルージョンが発生してしまう。また、移動する障害物が存在する環境下では、非特許文献２に記載の特徴点の算出や特徴点の対応付けを安定して行うことができない。同様に、移動障害物の存在下では、特許文献３に記載の走行目標ラインの想定を安定して行うことが難しい。したがって、これらの方法によって、移動体の位置及び姿勢を継続的に且つ精度良く算出することは難しい。
【０００８】
従って、本発明は、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる測位装置及び測位方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明にかかる測位装置は、周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置である。前記測位装置は、画像処理手段と、慣性算出手段と、誤差量推定手段と、を備える。前記画像処理手段は、前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する。前記慣性算出手段は、前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する。前記誤差量推定手段は、前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する。そして、前記測位装置は、前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する。
【００１０】
本発明に係る測位装置においては、誤差量推定手段が、画像並進量と画像回転量とに基づいて、慣性並進量と慣性回転量とに含まれる誤差量を推定する。通常、画像から移動体の移動量を推定すると、オクルージョンや画像内に存在する移動障害物等の影響により誤差が発生する場合がある。しかし、時刻ｔにおける画像と、時刻ｔからΔｔ（１エポック（１時刻））経過した時刻ｔ＋Δｔにおける画像と、を比較することによって移動体の移動量を算出する場合、比較的精度良く移動体の移動量を算出することができる。具体的には、オクルージョンが毎回（Δｔ毎に）発生するわけではないので、時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像との対応が取れれば、精度良く移動体の移動量を算出することができる。換言すれば、画像処理手段によって移動体の移動量を算出する方法の短時間における検出精度は、比較的優れている。一方、慣性検出部によって、移動体の移動量を検出すると、温度などの影響によりドリフト誤差が発生する。そのため、慣性検出部の検出精度は、時間が経過すると劣化してしまう。換言すれば、慣性検出部によって移動体の移動量を検出する方法の長時間における検出精度は、比較的劣っている。本発明に係る測位装置においては、短時間における検出精度に比較的優れている画像処理手段によって算出される移動体の移動量によって、長時間における検出精度に比較的劣っている慣性検出部によって検出される移動量を補正する。これにより、測位装置は、移動体の移動量である並進量及び回転量を長時間にわたって精度良く推定することができる。換言すれば、測位装置は、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる。
【００１１】
本発明に係る測位方法は、周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置における測位方法である。当該測位方法は、前記測位装置が、画像処理と、慣性算出処理と、誤差量推定処理と、補正処理と、を実行するものである。前記画像処理において、前記測位装置は、前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する。前記慣性算出処理において、前記測位装置は、前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する。前記誤差量推定処理において、前記測位装置は、前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する。前記補正処理において、前記測位装置は、前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する。
【００１２】
本発明に係る測位方法においては、誤差量推定処理において、画像並進量と画像回転量とに基づいて、慣性並進量と慣性回転量とに含まれる誤差量を推定する。通常、画像から移動体の移動量を推定すると、オクルージョンや画像内に存在する移動障害物等の影響により誤差が発生する場合がある。しかし、時刻ｔにおける画像と、時刻ｔからΔｔ（１エポック（１時刻））経過した時刻ｔ＋Δｔにおける画像と、を比較することによって移動体の移動量を算出する場合、比較的精度良く移動体の移動量を算出することができる。換言すれば、画像処理によって移動体の移動量を算出する方法の短時間における検出精度は、比較的優れている。一方、慣性検出部によって、移動体の移動量を検出すると、温度などの影響によりドリフト誤差が発生する。そのため、慣性検出部の検出精度は、時間が経過すると劣化してしまう。換言すれば、慣性検出部によって移動体の移動量を検出する方法の長時間における検出精度は、比較的劣っている。本発明に係る測位方法においては、短時間における検出精度に比較的優れている画像処理によって算出される移動体の移動量によって、長時間における検出精度に比較的劣っている慣性検出部によって検出される移動量を補正する。これにより、移動体の移動量である並進量及び回転量を長時間にわたって精度良く推定することができる。換言すれば、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる。
【発明の効果】
【００１３】
本発明によれば、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施の形態に係る測位装置を模式的に示す構成図である。
【図２】時刻ｔにおける画像内の特徴点と時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを説明する図である。
【図３】時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像との間の幾何学的関係を説明する図である。
【図４】本発明の実施の形態に係る測位装置の測位方法を示すフローチャートである。
【図５】本発明の実施の形態に係る測位装置における基礎行列の算出方法を示すフローチャートである。
【図６】本発明に係る実施例１におけるヨー方向の姿勢の検出精度を示すグラフである。
【図７】本発明に係る実施例１におけるロール方向の姿勢の検出精度を示すグラフである。
【図８】本発明に係る実施例１におけるピッチ方向の姿勢の検出精度を示すグラフである。
【発明を実施するための形態】
【００１５】
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本発明の実施の形態に係る測位装置１００を模式的に示す構成図である。図１に示すように、測位装置１００は、移動体２００に備えられ、移動体２００の位置及び姿勢を推定する。
移動体２００は、測位装置１００の他に、撮像部１、慣性センサ２（慣性検出部）等を備えている。
【００１６】
撮像部１は、移動体２００の周辺領域の画像（画像データ）を経時的に撮像する。撮像部１は、例えば、ＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）、ＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）等の光電変換素子を備えるカメラ等である。
【００１７】
また、慣性センサ２は、移動体２００の移動量を検出する。移動体２００は、慣性センサ２として、例えば、加速度センサ２１、ジャイロセンサ２２等を備えている。そして、加速度センサ２１は、移動体２００の加速度を経時的に検出する。また、ジャイロセンサ２２は、移動体２００の角速度を検出する。なお、慣性センサ２は、移動体２００の移動量（回転行列、並進ベクトル）を検出するものであれば、どんなものであってもよい。例えば、移動体２００が車輪により駆動されるものである場合、慣性センサ２として当該車輪の回転を計測するエンコーダを備えていてもよい。
【００１８】
測位装置１００は、画像処理部３（画像処理手段）、慣性算出部４（慣性算出手段）、カルマンフィルタ５（誤差量推定手段）等を備えている。測位装置１００は、制御部（図示省略）、メモリ（図示省略）等を有している。当該メモリは、測位装置１００を制御するための各種プログラム（図示省略）及びデータ（図示省略）などを格納している。そして、当該制御部は、当該メモリに格納されている各種プログラムを実行することにより、測位装置１００の各部を制御する。例えば、当該制御部は、画像処理部３、慣性算出部４、カルマンフィルタ５等を制御することにより、画像処理手段、慣性算出手段、誤差量推定手段等として機能する。
【００１９】
画像処理部３には、撮像部１によって撮像された画像データが入力される。そして、画像処理部３は、撮像部１によって撮像された時刻ｔにおける画像と、時刻ｔ＋Δｔにおける画像とを比較することにより、移動体２００の並進ベクトルｈｉｍｇ（画像並進量）と回転行列Ｒｉｍｇ（画像回転量）とを算出する。
具体的には、画像処理部３は、特徴点検出部３１、特徴点対応付け部３２、誤対応除去部３３、停止判定部３４、基礎行列算出部３５等を備えている。
【００２０】
特徴点検出部３１には、撮像部１によって撮像された画像データが入力される。そして、特徴点検出部３１は、撮像部１によって撮像された画像から特徴点を検出する。具体的には、特徴点検出部３１は、撮像部１によって撮像された画像から特徴量を算出することによって特徴点を検出する。本実施形態では、特徴点検出部３１は、画像からＳＨＩＦＴ特徴量を算出することにより特徴点を検出する。ＳＨＩＦＴ特徴量は、画像の回転、スケール変化、照明変化等によって影響を受けにくい点で優れている。なお、特徴点検出部３１が算出する特徴量はＳＨＩＦＴ特徴量に限定されるものではなく、他の特徴量であってもよい。
【００２１】
特徴点対応付け部３２には、特徴点検出部３１によって検出された特徴点の特徴量が入力される。そして、特徴点対応付け部３２は、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行う。例えば、本実施形態では、特徴点対応付け部３２は、最近傍探索と呼ばれる解法を用いて、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行う。なお、特徴点対応付け部３２は、他の解法を用いて特徴点の対応付けを行ってもよい。
【００２２】
誤対応除去部３３には、特徴点対応付け部３２から、特徴点の対応付けに関する情報と各特徴点の特徴量とが入力される。そして、誤対応除去部３３は、特徴点対応付け部３２によって対応付けられた時刻ｔにおける画像内の特徴点と時刻ｔ＋Δｔにおける画像内の特徴点とのうち、誤って対応付けられた特徴点を除去する。
一般に、特徴量のみに基づいて、特徴点の対応付けを行うと、本来対応すべきでない特徴点同士を対応するものとする結果が含まれてしまう。例えば、図２に、このような誤対応の例を示す。図２の上側の図は時刻ｔにおける画像を示し、図２の下側の図は時刻ｔ＋Δｔにおける画像を示す。また、図２において、黒丸は特徴点を示し、点線は本来対応すべき特徴点同士の対応を示し、破線は本来対応すべきでない特徴点同士の対応を示す。特徴点対応付け部３２による特徴点の対応付け処理では、図２に示すような誤対応を生じる可能性がある。そのため、誤対応除去部３３は、図２に示すような、本来対応すべきでない特徴点同士を対応点から除外する処理を行う。
【００２３】
具体的には、誤対応除去部３３には、後述する慣性算出部４によって算出された並進ベクトルｈｉｎｓ（慣性並進量）及び回転行列Ｒｉｎｓ（慣性回転量）が入力される。
そして、まず、誤対応除去部３３は、並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓに基づいて、時刻ｔ＋Δｔにおける画像内の特徴点を時刻ｔにおける画像内に戻す。次いで、誤対応除去部３３は、時刻ｔ＋Δｔにおける画像内の特徴点が、時刻ｔにおける画像内において、当該時刻ｔ＋Δｔにおける画像内の特徴点に対応する時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、当該時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする。
これにより、時刻ｔ＋Δｔにおける画像内の特徴点を時刻ｔにおける画像内に戻した場合に、時刻ｔ＋Δｔにおける画像内の特徴点が、当該時刻ｔ＋Δｔにおける画像内の特徴点に対応する時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方の範囲内にない場合、当該時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点と、を、互いに対応しない特徴点として、対応点から除去することができる。
なお、本実施形態では、Ｎ＝２０とした。Ｎの値を適宜変更することにより、特徴点の対応付けの精度を必要に応じて調節することができる。
【００２４】
停止判定部３４には、特徴点対応付け部３２及び誤対応除去部３３によって対応付けられた特徴点（対応点）の特徴量が入力される。停止判定部３４は、移動体２００が停止しているか否かを判定する。具体的には、停止判定部３４は、撮像部１によって撮像された画像データに基づいて、移動体２００が停止しているか否かを判定する。より具体的には、停止判定部３４は、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とを比較することにより、移動体２００が停止しているか否かを判定する。
そして、停止判定部３４は、移動体２００が停止していないと判定した場合には、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とを基礎行列算出部３５に入力する。
一方、停止判定部３４は、移動体２００が停止していると判定した場合には、加速度センサ２１及びジャイロセンサ２２から慣性算出部４への入力を停止させる。加速度センサ２１及びジャイロセンサ２２は、移動体２００が停止している場合であっても、ドリフトに基づくノイズを出力する。これが蓄積されることにより、ドリフト誤差が発生する。そこで、停止判定部３４は、移動体２００が停止していると判定した場合に、加速度センサ２１及びジャイロセンサ２２から慣性算出部４に入力される信号を強制的にゼロにする。これにより、ドリフト誤差を低減することができる。
なお、停止判定部３４は、測位装置１００に設けられていなくてもよい。移動体２００が停止している際に蓄積されるドリフト誤差を含めて、後述するカルマンフィルタ５により誤差量を推定することができるためである。
【００２５】
基礎行列算出部３５は、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とに基づいて、最急勾配法により、並進ベクトルｈｉｍｇ（画像並進量）と回転行列Ｒｉｍｇ（画像回転量）を算出する。なお、最急勾配法以外の繰り返し演算により並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとが算出されても良い。例えば、繰り返し演算としては、ニュートン法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法等を用いることができる。
一般に、画像から基礎行列（並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇ）を算出する場合、時間の異なる画像間の幾何学的関係から基礎行列を求める。図３に、時間の異なる画像間の幾何学的関係を示す。図３において、点Ｐ１は、時刻ｔにおける撮像部１のレンズ中心を表し、点Ｐ２は、時刻ｔ＋Δｔにおける撮像部１のレンズ中心を表し、点Ｐ３は、空間における注視点（特徴点）を表す。そして、点Ｐ１、Ｐ２、Ｐ３により形成される平面をエピポーラ平面と呼ぶ。また、図３において、平面Ｇ１は、時刻ｔにおける画像平面を表し、平面Ｇ２は、時刻ｔ＋Δｔにおける画像平面を表す。また、画像平面Ｇ１とエピポーラ平面との交線Ｌ１と、画像平面Ｇ２とエピポーラ平面との交線Ｌ２と、をエピポーラ線と呼ぶ。また、図３において、点線Ｌ３は、画像平面Ｇ１と他のエピポーラ平面との交線（エピポーラ線）であり、点線Ｌ４は、画像平面Ｇ２と他のエピポーラ平面との交線（エピポーラ線）である。そして、図３において、点Ｐ１から点Ｐ２に向かうベクトルが並進ベクトルｈｉｍｇである。また、図３において、点Ｐ１から点Ｐ３に向かうベクトルと点Ｐ２から点Ｐ３に向かうベクトルとの間の回転から回転行列Ｒｉｍｇが表される。従って、図３に示す幾何学的関係から、並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを求めることができる。
【００２６】
図３に示す幾何学的関係を用いて、画像から並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する場合、最小二乗法を用いる解析により並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する方法（固有値の導出）と、最急勾配法により並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する方法とがある。
最小二乗法を用いる解析により算出する場合、撮像部１から入力される画像によって計算誤差が生じてしまう可能性がある。
一方、最急勾配法等の繰り返し演算により算出する場合、安定して精度の良い解が得られるが、計算時間が長くなってしまうという問題がある。
本実施形態では、基礎行列算出部３５は、初期値として、後述する慣性算出部４によって算出された並進ベクトルｈｉｎｓ（慣性並進量）及び回転行列Ｒｉｎｓ（慣性回転量）を利用して、最急勾配法等の繰り返し演算により、並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する。これにより、計算時間を短縮することができる。なお、基礎行列算出部３５には、停止判定部３４を介して、誤対応除去部３３から、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓが入力される。
【００２７】
慣性算出部４には、加速度センサ２１から移動体２００の加速度が入力され、ジャイロセンサ２２から移動体２００の角速度が入力される。そして、慣性算出部４は、加速度センサ２１から入力される加速度及びジャイロセンサ２２から入力される角速度に基づいて、移動体２００の並進ベクトルｈｉｎｓ（慣性並進量）と回転行列Ｒｉｎｓ（慣性回転量）とを算出する。
具体的には、慣性算出部４は、第１の演算器４１、第２の演算器４２、第３の演算器４３、第４の演算器４４等を備えている。
【００２８】
第１の演算器４１には、ジャイロセンサ２２から角速度が入力される。そして、第１の演算器４１は、当該角速度を時間で積分することにより角度を算出し、当該角度を出力する。また、第１の演算器４１により算出された角度は、回転行列Ｒｉｎｓとして、外部又はカルマンフィルタ５へ入力される。
【００２９】
第２の演算器４２には、加速度センサ２１から加速度が入力される。また、第２の演算器４２には、第１の演算器から出力された角度に前回の姿勢（角度）が加算されて得られる角度が入力される。そして、第２の演算器４２は、入力された当該角度（姿勢）に基づいて、加速度センサ２１から入力された加速度の姿勢を変換し、姿勢変換した加速度を出力する。
【００３０】
第３の演算器４３には、第２の演算器４２から姿勢変換された加速度が入力される。そして、第３の演算器４３は、当該加速度を時間で積分することにより速度を算出し、当該速度を出力する。
【００３１】
第４の演算器４４には、第３の演算器４３から出力された速度に前回の速度が加算して得られる速度が入力される。そして、第４の演算器４４は、入力された当該速度を時間で積分することにより距離を算出する。また、第４の演算器４４に算出された当該距離は、並進ベクトルｈｉｎｓとして、外部又はカルマンフィルタ５へ入力される。
【００３２】
カルマンフィルタ５は、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとに基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。具体的には、カルマンフィルタ５には、並進ベクトルｈｉｍｇから並進ベクトルｈｉｎｓが減算された値と、回転行列Ｒｉｍｇから回転行列Ｒｉｎｓが減算された値と、が入力される。そして、カルマンフィルタ５は、並進ベクトルｈｉｍｇと並進ベクトルｈｉｎｓとが等しく、且つ、回転行列Ｒｉｍｇと回転行列Ｒｉｎｓとが等しいという拘束条件によって作成された観測方程式に基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。
【００３３】
以下、カルマンフィルタ５に適用される観測方程式の作成について説明する。ここで、移動体２００の前後方向をｘ軸方向、左右方向をｙ軸方向、鉛直方向をｚ軸方向とする。さらに、ｘ軸周りの回動方向をロール（ｒｏｌｌ）方向、ｙ軸周りの回動方向をピッチ（ｐｉｔｃｈ）方向、ｚ軸周りの回動方向をヨー（ｙａｗ）方向とする。このとき、加速度センサ２１から出力される値は（１）式で表され、ジャイロセンサ２２から出力される値は、（２）式で表される。
【数１】
【数２】
（１）式において、ｇは加速度センサ２１の出力値であり、δｇはｇに含まれる誤差量であり、上バーつきのｇはｇの真値である。また、（２）式において、ωはジャイロセンサ２２の出力値であり、δωはωに含まれる誤差量であり、上バーつきのωはωの真値である。従って、（１）式は、加速度センサ２１の出力値を「真値＋誤差量」で表している。同様に、（２）式は、ジャイロセンサ２２の出力値を「真値＋誤差量」で表している。
【００３４】
一般に、誤差量に含まれる高周波成分は、ＬＰＦ（Ｌｏｗ Ｐａｓｓ Ｆｉｌｔｅｒ）により除去することができる。しかし、温度などの環境に依存した時間変化に対して比較的緩やかな誤差（低周波成分）はＬＰＦでは除去することができない。本実施形態では、カルマンフィルタ５によって当該低周波成分を推定する。
【００３５】
また、ジャイロセンサ２２の出力値における回転座標変換は、（３）式〜（７）式に従う。
【数３】
【数４】
【数５】
【数６】
【数７】
（３）式において、ｘ、ｙ、ｚは、時刻ｔ＋Δｔにおける座標を表し、ｘ'、ｙ'、ｚ'は、時刻ｔにおける座標を表す。
【００３６】
また、加速度センサ２１の出力値における回転座標変換は、（８）式に従う。
【数８】
（８）式において、ｖｂｏｄｙは、移動体２００に設定された座標系であり、撮像部１の座標系と同じとしてもよい。このようにした場合、実際にはｖｂｏｄｙにオフセットは含まれることになるが、問題はない。なお、移動体２００が車両である場合、ｖｂｏｄｙは、車軸に設定された座標系であり、カメラ座標系（撮像部１の座標系）と同じとしてよい。また、（８）式において、ｇは加速度センサ２１の出力値である。
【００３７】
そして、並進ベクトルｈｉｍｇと並進ベクトルｈｉｎｓとが等しく、且つ、回転行列Ｒｉｍｇと回転行列Ｒｉｎｓとが等しいとすると、観測方程式は（９）式で表される。
【数９】
（９）式において、Ｘは状態量であり、求めたい誤差量に相当する。また、Ｓは観測量（１２次元）であり、画像から求めた回転行列Ｒｉｍｇ及び並進ベクトルｈｉｍｇが代入される。また、並進ベクトルｈｉｍｇは、方向情報のみであり、スケール情報を含まないため、（９）式では、並進ベクトルｈｉｍｇの上限値として、並進ベクトルｈｉｎｓのノルムを利用した。なお、並進ベクトルｈｉｍｇ及び並進ベクトルｈｉｎｓは、３×３の行列であり、並進ベクトルｈｉｍｇ及び並進ベクトルｈｉｎｓは、３×１の行列である。そのため、（９）式は、全てで１２個のパラメータを含んでいる。そして、（９）式における各要素とは、これらの行列の要素のことである。すなわち、（９）式における各要素とは、当該各パラメータを意味する。
【００３８】
次に、本実施形態に係る測位装置１００における移動体２００の位置（並進ベクトルｈｉｎｓ）及び姿勢（回転行列Ｒｉｎｓ）を推定する測位方法について、図４を参照しながら説明する。
まず、測位装置１００において、慣性算出部４が、ジャイロセンサ２２の出力値に基づいて回転行列Ｒｉｎｓを算出し、加速度センサ２１の出力値に基づいて並進ベクトルｈｉｎｓを算出する。また、画像処理部３が、撮像部１から入力される画像データに基づいて、回転行列Ｒｉｍｇ及び並進ベクトルｈｉｍｇを算出する（ステップＳ１）。
【００３９】
次に、測位装置１００において、カルマンフィルタ５が、回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓに含まれる誤差量を推定する（ステップＳ２）。
【００４０】
次に、測位装置１００は、ステップＳ２で推定した誤差量に基づいて、回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓを補正する（ステップＳ３）。具体的には、カルマンフィルタ５から出力された回転行列Ｒｉｎｓの誤差量がジャイロセンサ２２の出力値に加算されて、慣性算出部４の第１の演算器４１に入力されることにより、回転行列Ｒｉｎｓが補正される。同様に、カルマンフィルタ５から出力された並進ベクトルｈｉｎｓの誤差量が加速度センサ２１の出力値に加算されて、慣性算出部４の第２の演算器４２に入力されることにより、並進ベクトルｈｉｎｓが補正される。
【００４１】
次に、慣性算出部４が補正後の回転行列Ｒｉｎｓ及び補正後の並進ベクトルｈｉｎｓを外部に出力する（ステップＳ４）。すなわち、測位装置１００は、補正後の回転行列Ｒｉｎｓを姿勢変位として、補正後の並進ベクトルｈｉｎｓを位置変位として出力する。
【００４２】
次に、本実施形態に係る測位装置１００における基礎行列（回転行列Ｒｉｍｇ、並進ベクトルｈｉｍｇ）の算出方法について、図５を参照しながら説明する。
まず、撮像部１から画像処理部３の特徴点検出部３１に画像データが入力される（ステップＳ１０１）。
【００４３】
次に、特徴点検出部３１は、撮像部１によって撮像された画像から特徴点を検出する（ステップＳ１０２）。そして、特徴点検出部３１は、当該特徴点の特徴量を特徴点対応付け部３２に入力する。
【００４４】
次に、特徴点対応付け部３２は、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行う（ステップＳ１０３）。そして、特徴点対応付け部３２は、特徴点の対応付けに関する情報と各特徴点の特徴量とを、誤対応除去部３３に入力する。
【００４５】
次に、慣性算出部４が、回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓを算出する（ステップＳ１０４）。そして、慣性算出部４は、当該回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓを誤対応除去部３３に入力する。
【００４６】
次に、誤対応除去部３３は、ステップＳ１０３において対応付けられた時刻ｔにおける画像内の特徴点と時刻ｔ＋Δｔにおける画像内の特徴点とのうち、誤って対応付けられた特徴点を除去する（ステップＳ１０５）。そして、誤対応除去部３３は、特徴点（対応点）の特徴量と、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓと、を停止判定部３４に入力する。
【００４７】
次に、停止判定部３４は、移動体２００が停止しているか否かを判定する（ステップＳ１０６）。
ステップＳ１０６において、停止判定部３４は、移動体２００が停止していると判定した場合（ステップＳ１０６；Ｙｅｓ）、加速度センサ２１及びジャイロセンサ２２から慣性算出部４への検出信号の入力を停止させ、ステップＳ１０１に戻る。
ステップＳ１０６において、停止判定部３４は、移動体２００が停止していないと判定した場合（ステップＳ１０６；Ｎｏ）、特徴点（対応点）の特徴量と、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓと、を基礎行列算出部３５に入力する。
【００４８】
次に、基礎行列算出部３５は、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とに基づいて、最急勾配法を含む繰り返し演算により、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇを算出する（ステップＳ１０７）。
【００４９】
以上に説明した本実施形態に係る測位装置１００及び測位方法によれば、カルマンフィルタ５が、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとに基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。通常、画像から移動体２００の移動量を推定すると、オクルージョンや画像内に存在する移動障害物等の影響により誤差が発生する場合がある。しかし、時刻ｔにおける画像と、時刻ｔからΔｔ（１エポック（１時刻））経過した時刻ｔ＋Δｔにおける画像と、を比較することによって移動体２００の移動量を算出する場合、比較的精度良く移動体２００の移動量を算出することができる。具体的には、オクルージョンが毎回（Δｔ毎に）発生するわけではないので、時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像との対応が取れれば、精度良く移動体の移動量を算出することができる。換言すれば、画像処理部３によって移動体２００の移動量（並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇ）を算出する方法の短時間における検出精度は、比較的優れている。一方、慣性センサ２によって、移動体２００の移動量（並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓ）を検出すると、温度などの影響によりドリフト誤差が発生する。そのため、慣性センサ２の検出精度は、時間が経過すると劣化してしまう。換言すれば、慣性センサ２によって移動体２００の移動量を検出する方法の長時間における検出精度は、比較的劣っている。本実施形態に係る測位装置１００及び測位方法においては、短時間における検出精度に比較的優れている画像処理部３によって算出される移動体２００の移動量によって、長時間における検出精度に比較的劣っている慣性センサ２によって検出される移動量を補正する。これにより、測位装置１００は、移動体２００の移動量である並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓを長時間にわたって精度良く推定することができる。換言すれば、測位装置１００は、移動体２００の位置及び姿勢を継続的に且つより精度良く推定することができる。なお、オクルージョンが発生している場合には、直近にカルマンフィルタ５により推定された誤差を用いて慣性センサ（加速度センサ２１及びジャイロセンサ２２）の検出値を補正する。オクルージョンが発生する頻度は比較的少ないため、直近にカルマンフィルタ５により推定した誤差によって慣性センサの検出値を補正する場合があっても、精度良く移動体２００の位置及び姿勢を推定することができる。
【００５０】
また、誤対応除去部３３は、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓに基づいて、時刻ｔ＋Δｔにおける画像内の特徴点を時刻ｔにおける画像内に戻した場合に、時刻ｔ＋Δｔにおける画像内の特徴点が、時刻ｔ＋Δｔにおける画像内の特徴点に対応する時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする。
これにより、特徴点の対応付けの精度を向上することができる。
【００５１】
また、基礎行列算出部３５は、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓを初期値として用いて、対応点から最急勾配法により、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇを算出する。
これにより、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇを安定して精度の良く算出することができるとともに、計算時間を短縮することができる。
【００５２】
また、並進ベクトルｈｉｍｇと並進ベクトルｈｉｎｓとが等しく、且つ、回転行列Ｒｉｍｇと回転行列Ｒｉｎｓとが等しいという拘束条件によって作成された観測方程式を適用したカルマンフィルタ５によって、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。
これにより、時間Δｔが経過するたびに、推定された誤差量によって、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとが補正されるため、ドリフト誤差が蓄積するのを防止することができる。
【００５３】
［実施例１］
次に、本実施形態に係る測位装置１００を適用した実施例について説明する。実施例１では、移動体２００として車両を用いた。そして、当該車両に、慣性センサ２としてジャイロ・加速度センサ（ＳＭＧ０７４／２２５）を搭載した。また、評価用に、当該車両に、光ファイバジャイロ（ＦＯＧ）・サーボ加速度計（ＪＭ３４０３／ＪＡ−５ＶＣ４）を搭載した。また、当該車両に、撮像部１としてカメラを搭載した。そして、当該車両を１５００ｍｍ／ｓ以下の移動速度で走行させ、計測を行った。
【００５４】
また、比較例でも、実施例１と同様の車両を用いて計測を行う。しかし、比較例では、カメラによって撮像される画像から算出される並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを用いて並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとを補正しない。すなわち、比較例において計測される値は、慣性センサ２としてのジャイロ・加速度センサ（ＳＭＧ０７４／２２５）そのものの値である。
【００５５】
ヨー方向の姿勢の計測結果を図６に示し、ロール方向の姿勢の計測結果を図７に示し、ピッチ方向の姿勢の計測結果を図８に示す。図６〜図８において、縦軸に姿勢（°）を示し、横軸にフレーム数を示す。また、図６〜図８において、実線は評価用の光ファイバジャイロ（ＦＯＧ）・サーボ加速度計（ＪＭ３４０３／ＪＡ−５ＶＣ４）の計測値を示し、三角印は比較例としてのジャイロ・加速度センサ（ＳＭＧ０７４／２２５）の計測値を示し、大きい丸印は実施例１の計測値を示し、小さい丸印はカメラによって撮像された画像から算出した値を示す。
図６〜図８に示すように、比較例に比べて、実施例１の計測値は、評価用の計測値に近い値となった。従って、本実施形態に係る測位装置１００を適用することにより、車両の位置及び姿勢を継続的に且つより精度良く推定することができるといえる。
【００５６】
尚、本発明は上述した各実施の形態に限定されず、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００５７】
１ 撮像部
２ 慣性センサ（慣性検出部）
２１ 加速度センサ（慣性検出部）
２２ ジャイロセンサ（慣性検出部）
３ 画像処理部（画像処理手段）
４ 慣性算出部（慣性算出手段）
５ カルマンフィルタ（誤差量推定手段）
１００ 測位装置
２００ 移動体
【技術分野】
【０００１】
本発明は、移動体の位置及び姿勢を推定する測位装置及び測位方法に関する。
【背景技術】
【０００２】
車両等の移動体は、ジャイロセンサや加速度センサ等の慣性センサを用いて、当該移動体の位置及び姿勢の算出を行っている。
例えば、特許文献１には、慣性センサとして、機械式ジャイロを用いる技術が記載されている。そして、特許文献１には、機械式ジャイロによって衛星の角速度を検出し、恒星センサによって衛星の姿勢を検出する技術が記載されている。
また、特許文献２には、動揺体の上に設置されたカメラの座標系及び座面を修正するために、動揺体の回転角を検出する技術が記載されている。
【０００３】
また、移動体にカメラを搭載して、カメラによって取得された画像を用いて、移動体の位置及び姿勢を推定する方法もある。
例えば、非特許文献１には、ステレオ画像から静止領域を取得し、当該静止領域を仮想的に上方から見た画像（Ｖｉｒｔｕａｌ−Ｐｒｏｊｅｃｔｉｏｎ−Ｐｌａｎｅ ｉｍａｇｅ：ＶＰＰ画像）に変換し、当該ＶＰＰ画像を時系列に沿ってマッチングすることにより、移動体の運動を推定する技術が記載されている。
また、非特許文献２には、画像から特徴点を生成して、時系列に沿って、各画像内の特徴点を比較することにより、移動体の運動を推定する技術が記載されている。
また、特許文献３には、車両の操舵角と、当該車両に搭載されたＣＣＤカメラによって検出される車両の横方向の相対位置とに基づいて、カルマンフィルタによる処理を行って、補正操舵角を演算する技術が記載されている。特許文献３では、ＣＣＤカメラ画像内の路面に描かれたセンターライン等に基づいて走行目標ラインを想定し、当該走行目標ラインと車両の前後方向に延びる基準線との間隔を、車両の横方向の相対位置として検出している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−１４２５９４号公報
【特許文献２】実開平０１−３０４５１０号公報
【特許文献３】特開平１０−２９７５２１号公報
【非特許文献】
【０００５】
【非特許文献１】関 晃仁、奥富 正敏、ステレオ動画像を利用した道路面領域の抽出と追跡による自車両の運動推定、情報処理学会論文誌：コンピュータビジョンとイメージメディア、日本、社団法人情報処理学会、２００６年３月、Ｖｏｌ．４７、Ｎｏ．ＳＩＧ５（ＣＶＩＭ１３）、９０−９９ページ
【非特許文献２】Ｎｉｋｌａｓ Ｋａｒｌｓｓｏｎ、他５名、Ｔｈｅ ｖＳＬＡＭ Ａｌｇｏｒｉｔｈｍ ｆｏｒ Ｒｏｂｕｓｔ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ、Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ） ２００５、２００５年４月、２４−２９ページ
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１、２に記載の慣性センサにおいては、温度などの影響によりドリフト誤差が発生する。そのため、これらの慣性センサの検出精度は、時間が経過すると劣化してしまう。なお、慣性センサとして、光ファイバジャイロ（ＦＯＧ；Ｆｉｂｅｒ Ｏｐｔｉｃ Ｇｙｒｏ）等を用いることにより、精度を向上することができるが、コストが大幅に高くなってしまう。
【０００７】
また、非特許文献１のように、ステレオ画像を用いる場合、オクルージョンが発生してしまう。また、移動する障害物が存在する環境下では、非特許文献２に記載の特徴点の算出や特徴点の対応付けを安定して行うことができない。同様に、移動障害物の存在下では、特許文献３に記載の走行目標ラインの想定を安定して行うことが難しい。したがって、これらの方法によって、移動体の位置及び姿勢を継続的に且つ精度良く算出することは難しい。
【０００８】
従って、本発明は、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる測位装置及び測位方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明にかかる測位装置は、周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置である。前記測位装置は、画像処理手段と、慣性算出手段と、誤差量推定手段と、を備える。前記画像処理手段は、前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する。前記慣性算出手段は、前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する。前記誤差量推定手段は、前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する。そして、前記測位装置は、前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する。
【００１０】
本発明に係る測位装置においては、誤差量推定手段が、画像並進量と画像回転量とに基づいて、慣性並進量と慣性回転量とに含まれる誤差量を推定する。通常、画像から移動体の移動量を推定すると、オクルージョンや画像内に存在する移動障害物等の影響により誤差が発生する場合がある。しかし、時刻ｔにおける画像と、時刻ｔからΔｔ（１エポック（１時刻））経過した時刻ｔ＋Δｔにおける画像と、を比較することによって移動体の移動量を算出する場合、比較的精度良く移動体の移動量を算出することができる。具体的には、オクルージョンが毎回（Δｔ毎に）発生するわけではないので、時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像との対応が取れれば、精度良く移動体の移動量を算出することができる。換言すれば、画像処理手段によって移動体の移動量を算出する方法の短時間における検出精度は、比較的優れている。一方、慣性検出部によって、移動体の移動量を検出すると、温度などの影響によりドリフト誤差が発生する。そのため、慣性検出部の検出精度は、時間が経過すると劣化してしまう。換言すれば、慣性検出部によって移動体の移動量を検出する方法の長時間における検出精度は、比較的劣っている。本発明に係る測位装置においては、短時間における検出精度に比較的優れている画像処理手段によって算出される移動体の移動量によって、長時間における検出精度に比較的劣っている慣性検出部によって検出される移動量を補正する。これにより、測位装置は、移動体の移動量である並進量及び回転量を長時間にわたって精度良く推定することができる。換言すれば、測位装置は、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる。
【００１１】
本発明に係る測位方法は、周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置における測位方法である。当該測位方法は、前記測位装置が、画像処理と、慣性算出処理と、誤差量推定処理と、補正処理と、を実行するものである。前記画像処理において、前記測位装置は、前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する。前記慣性算出処理において、前記測位装置は、前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する。前記誤差量推定処理において、前記測位装置は、前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する。前記補正処理において、前記測位装置は、前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する。
【００１２】
本発明に係る測位方法においては、誤差量推定処理において、画像並進量と画像回転量とに基づいて、慣性並進量と慣性回転量とに含まれる誤差量を推定する。通常、画像から移動体の移動量を推定すると、オクルージョンや画像内に存在する移動障害物等の影響により誤差が発生する場合がある。しかし、時刻ｔにおける画像と、時刻ｔからΔｔ（１エポック（１時刻））経過した時刻ｔ＋Δｔにおける画像と、を比較することによって移動体の移動量を算出する場合、比較的精度良く移動体の移動量を算出することができる。換言すれば、画像処理によって移動体の移動量を算出する方法の短時間における検出精度は、比較的優れている。一方、慣性検出部によって、移動体の移動量を検出すると、温度などの影響によりドリフト誤差が発生する。そのため、慣性検出部の検出精度は、時間が経過すると劣化してしまう。換言すれば、慣性検出部によって移動体の移動量を検出する方法の長時間における検出精度は、比較的劣っている。本発明に係る測位方法においては、短時間における検出精度に比較的優れている画像処理によって算出される移動体の移動量によって、長時間における検出精度に比較的劣っている慣性検出部によって検出される移動量を補正する。これにより、移動体の移動量である並進量及び回転量を長時間にわたって精度良く推定することができる。換言すれば、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる。
【発明の効果】
【００１３】
本発明によれば、移動体の位置及び姿勢を継続的に且つより精度良く推定することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施の形態に係る測位装置を模式的に示す構成図である。
【図２】時刻ｔにおける画像内の特徴点と時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを説明する図である。
【図３】時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像との間の幾何学的関係を説明する図である。
【図４】本発明の実施の形態に係る測位装置の測位方法を示すフローチャートである。
【図５】本発明の実施の形態に係る測位装置における基礎行列の算出方法を示すフローチャートである。
【図６】本発明に係る実施例１におけるヨー方向の姿勢の検出精度を示すグラフである。
【図７】本発明に係る実施例１におけるロール方向の姿勢の検出精度を示すグラフである。
【図８】本発明に係る実施例１におけるピッチ方向の姿勢の検出精度を示すグラフである。
【発明を実施するための形態】
【００１５】
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本発明の実施の形態に係る測位装置１００を模式的に示す構成図である。図１に示すように、測位装置１００は、移動体２００に備えられ、移動体２００の位置及び姿勢を推定する。
移動体２００は、測位装置１００の他に、撮像部１、慣性センサ２（慣性検出部）等を備えている。
【００１６】
撮像部１は、移動体２００の周辺領域の画像（画像データ）を経時的に撮像する。撮像部１は、例えば、ＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）、ＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）等の光電変換素子を備えるカメラ等である。
【００１７】
また、慣性センサ２は、移動体２００の移動量を検出する。移動体２００は、慣性センサ２として、例えば、加速度センサ２１、ジャイロセンサ２２等を備えている。そして、加速度センサ２１は、移動体２００の加速度を経時的に検出する。また、ジャイロセンサ２２は、移動体２００の角速度を検出する。なお、慣性センサ２は、移動体２００の移動量（回転行列、並進ベクトル）を検出するものであれば、どんなものであってもよい。例えば、移動体２００が車輪により駆動されるものである場合、慣性センサ２として当該車輪の回転を計測するエンコーダを備えていてもよい。
【００１８】
測位装置１００は、画像処理部３（画像処理手段）、慣性算出部４（慣性算出手段）、カルマンフィルタ５（誤差量推定手段）等を備えている。測位装置１００は、制御部（図示省略）、メモリ（図示省略）等を有している。当該メモリは、測位装置１００を制御するための各種プログラム（図示省略）及びデータ（図示省略）などを格納している。そして、当該制御部は、当該メモリに格納されている各種プログラムを実行することにより、測位装置１００の各部を制御する。例えば、当該制御部は、画像処理部３、慣性算出部４、カルマンフィルタ５等を制御することにより、画像処理手段、慣性算出手段、誤差量推定手段等として機能する。
【００１９】
画像処理部３には、撮像部１によって撮像された画像データが入力される。そして、画像処理部３は、撮像部１によって撮像された時刻ｔにおける画像と、時刻ｔ＋Δｔにおける画像とを比較することにより、移動体２００の並進ベクトルｈｉｍｇ（画像並進量）と回転行列Ｒｉｍｇ（画像回転量）とを算出する。
具体的には、画像処理部３は、特徴点検出部３１、特徴点対応付け部３２、誤対応除去部３３、停止判定部３４、基礎行列算出部３５等を備えている。
【００２０】
特徴点検出部３１には、撮像部１によって撮像された画像データが入力される。そして、特徴点検出部３１は、撮像部１によって撮像された画像から特徴点を検出する。具体的には、特徴点検出部３１は、撮像部１によって撮像された画像から特徴量を算出することによって特徴点を検出する。本実施形態では、特徴点検出部３１は、画像からＳＨＩＦＴ特徴量を算出することにより特徴点を検出する。ＳＨＩＦＴ特徴量は、画像の回転、スケール変化、照明変化等によって影響を受けにくい点で優れている。なお、特徴点検出部３１が算出する特徴量はＳＨＩＦＴ特徴量に限定されるものではなく、他の特徴量であってもよい。
【００２１】
特徴点対応付け部３２には、特徴点検出部３１によって検出された特徴点の特徴量が入力される。そして、特徴点対応付け部３２は、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行う。例えば、本実施形態では、特徴点対応付け部３２は、最近傍探索と呼ばれる解法を用いて、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行う。なお、特徴点対応付け部３２は、他の解法を用いて特徴点の対応付けを行ってもよい。
【００２２】
誤対応除去部３３には、特徴点対応付け部３２から、特徴点の対応付けに関する情報と各特徴点の特徴量とが入力される。そして、誤対応除去部３３は、特徴点対応付け部３２によって対応付けられた時刻ｔにおける画像内の特徴点と時刻ｔ＋Δｔにおける画像内の特徴点とのうち、誤って対応付けられた特徴点を除去する。
一般に、特徴量のみに基づいて、特徴点の対応付けを行うと、本来対応すべきでない特徴点同士を対応するものとする結果が含まれてしまう。例えば、図２に、このような誤対応の例を示す。図２の上側の図は時刻ｔにおける画像を示し、図２の下側の図は時刻ｔ＋Δｔにおける画像を示す。また、図２において、黒丸は特徴点を示し、点線は本来対応すべき特徴点同士の対応を示し、破線は本来対応すべきでない特徴点同士の対応を示す。特徴点対応付け部３２による特徴点の対応付け処理では、図２に示すような誤対応を生じる可能性がある。そのため、誤対応除去部３３は、図２に示すような、本来対応すべきでない特徴点同士を対応点から除外する処理を行う。
【００２３】
具体的には、誤対応除去部３３には、後述する慣性算出部４によって算出された並進ベクトルｈｉｎｓ（慣性並進量）及び回転行列Ｒｉｎｓ（慣性回転量）が入力される。
そして、まず、誤対応除去部３３は、並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓに基づいて、時刻ｔ＋Δｔにおける画像内の特徴点を時刻ｔにおける画像内に戻す。次いで、誤対応除去部３３は、時刻ｔ＋Δｔにおける画像内の特徴点が、時刻ｔにおける画像内において、当該時刻ｔ＋Δｔにおける画像内の特徴点に対応する時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、当該時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする。
これにより、時刻ｔ＋Δｔにおける画像内の特徴点を時刻ｔにおける画像内に戻した場合に、時刻ｔ＋Δｔにおける画像内の特徴点が、当該時刻ｔ＋Δｔにおける画像内の特徴点に対応する時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方の範囲内にない場合、当該時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点と、を、互いに対応しない特徴点として、対応点から除去することができる。
なお、本実施形態では、Ｎ＝２０とした。Ｎの値を適宜変更することにより、特徴点の対応付けの精度を必要に応じて調節することができる。
【００２４】
停止判定部３４には、特徴点対応付け部３２及び誤対応除去部３３によって対応付けられた特徴点（対応点）の特徴量が入力される。停止判定部３４は、移動体２００が停止しているか否かを判定する。具体的には、停止判定部３４は、撮像部１によって撮像された画像データに基づいて、移動体２００が停止しているか否かを判定する。より具体的には、停止判定部３４は、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とを比較することにより、移動体２００が停止しているか否かを判定する。
そして、停止判定部３４は、移動体２００が停止していないと判定した場合には、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とを基礎行列算出部３５に入力する。
一方、停止判定部３４は、移動体２００が停止していると判定した場合には、加速度センサ２１及びジャイロセンサ２２から慣性算出部４への入力を停止させる。加速度センサ２１及びジャイロセンサ２２は、移動体２００が停止している場合であっても、ドリフトに基づくノイズを出力する。これが蓄積されることにより、ドリフト誤差が発生する。そこで、停止判定部３４は、移動体２００が停止していると判定した場合に、加速度センサ２１及びジャイロセンサ２２から慣性算出部４に入力される信号を強制的にゼロにする。これにより、ドリフト誤差を低減することができる。
なお、停止判定部３４は、測位装置１００に設けられていなくてもよい。移動体２００が停止している際に蓄積されるドリフト誤差を含めて、後述するカルマンフィルタ５により誤差量を推定することができるためである。
【００２５】
基礎行列算出部３５は、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とに基づいて、最急勾配法により、並進ベクトルｈｉｍｇ（画像並進量）と回転行列Ｒｉｍｇ（画像回転量）を算出する。なお、最急勾配法以外の繰り返し演算により並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとが算出されても良い。例えば、繰り返し演算としては、ニュートン法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法等を用いることができる。
一般に、画像から基礎行列（並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇ）を算出する場合、時間の異なる画像間の幾何学的関係から基礎行列を求める。図３に、時間の異なる画像間の幾何学的関係を示す。図３において、点Ｐ１は、時刻ｔにおける撮像部１のレンズ中心を表し、点Ｐ２は、時刻ｔ＋Δｔにおける撮像部１のレンズ中心を表し、点Ｐ３は、空間における注視点（特徴点）を表す。そして、点Ｐ１、Ｐ２、Ｐ３により形成される平面をエピポーラ平面と呼ぶ。また、図３において、平面Ｇ１は、時刻ｔにおける画像平面を表し、平面Ｇ２は、時刻ｔ＋Δｔにおける画像平面を表す。また、画像平面Ｇ１とエピポーラ平面との交線Ｌ１と、画像平面Ｇ２とエピポーラ平面との交線Ｌ２と、をエピポーラ線と呼ぶ。また、図３において、点線Ｌ３は、画像平面Ｇ１と他のエピポーラ平面との交線（エピポーラ線）であり、点線Ｌ４は、画像平面Ｇ２と他のエピポーラ平面との交線（エピポーラ線）である。そして、図３において、点Ｐ１から点Ｐ２に向かうベクトルが並進ベクトルｈｉｍｇである。また、図３において、点Ｐ１から点Ｐ３に向かうベクトルと点Ｐ２から点Ｐ３に向かうベクトルとの間の回転から回転行列Ｒｉｍｇが表される。従って、図３に示す幾何学的関係から、並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを求めることができる。
【００２６】
図３に示す幾何学的関係を用いて、画像から並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する場合、最小二乗法を用いる解析により並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する方法（固有値の導出）と、最急勾配法により並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する方法とがある。
最小二乗法を用いる解析により算出する場合、撮像部１から入力される画像によって計算誤差が生じてしまう可能性がある。
一方、最急勾配法等の繰り返し演算により算出する場合、安定して精度の良い解が得られるが、計算時間が長くなってしまうという問題がある。
本実施形態では、基礎行列算出部３５は、初期値として、後述する慣性算出部４によって算出された並進ベクトルｈｉｎｓ（慣性並進量）及び回転行列Ｒｉｎｓ（慣性回転量）を利用して、最急勾配法等の繰り返し演算により、並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを算出する。これにより、計算時間を短縮することができる。なお、基礎行列算出部３５には、停止判定部３４を介して、誤対応除去部３３から、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓが入力される。
【００２７】
慣性算出部４には、加速度センサ２１から移動体２００の加速度が入力され、ジャイロセンサ２２から移動体２００の角速度が入力される。そして、慣性算出部４は、加速度センサ２１から入力される加速度及びジャイロセンサ２２から入力される角速度に基づいて、移動体２００の並進ベクトルｈｉｎｓ（慣性並進量）と回転行列Ｒｉｎｓ（慣性回転量）とを算出する。
具体的には、慣性算出部４は、第１の演算器４１、第２の演算器４２、第３の演算器４３、第４の演算器４４等を備えている。
【００２８】
第１の演算器４１には、ジャイロセンサ２２から角速度が入力される。そして、第１の演算器４１は、当該角速度を時間で積分することにより角度を算出し、当該角度を出力する。また、第１の演算器４１により算出された角度は、回転行列Ｒｉｎｓとして、外部又はカルマンフィルタ５へ入力される。
【００２９】
第２の演算器４２には、加速度センサ２１から加速度が入力される。また、第２の演算器４２には、第１の演算器から出力された角度に前回の姿勢（角度）が加算されて得られる角度が入力される。そして、第２の演算器４２は、入力された当該角度（姿勢）に基づいて、加速度センサ２１から入力された加速度の姿勢を変換し、姿勢変換した加速度を出力する。
【００３０】
第３の演算器４３には、第２の演算器４２から姿勢変換された加速度が入力される。そして、第３の演算器４３は、当該加速度を時間で積分することにより速度を算出し、当該速度を出力する。
【００３１】
第４の演算器４４には、第３の演算器４３から出力された速度に前回の速度が加算して得られる速度が入力される。そして、第４の演算器４４は、入力された当該速度を時間で積分することにより距離を算出する。また、第４の演算器４４に算出された当該距離は、並進ベクトルｈｉｎｓとして、外部又はカルマンフィルタ５へ入力される。
【００３２】
カルマンフィルタ５は、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとに基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。具体的には、カルマンフィルタ５には、並進ベクトルｈｉｍｇから並進ベクトルｈｉｎｓが減算された値と、回転行列Ｒｉｍｇから回転行列Ｒｉｎｓが減算された値と、が入力される。そして、カルマンフィルタ５は、並進ベクトルｈｉｍｇと並進ベクトルｈｉｎｓとが等しく、且つ、回転行列Ｒｉｍｇと回転行列Ｒｉｎｓとが等しいという拘束条件によって作成された観測方程式に基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。
【００３３】
以下、カルマンフィルタ５に適用される観測方程式の作成について説明する。ここで、移動体２００の前後方向をｘ軸方向、左右方向をｙ軸方向、鉛直方向をｚ軸方向とする。さらに、ｘ軸周りの回動方向をロール（ｒｏｌｌ）方向、ｙ軸周りの回動方向をピッチ（ｐｉｔｃｈ）方向、ｚ軸周りの回動方向をヨー（ｙａｗ）方向とする。このとき、加速度センサ２１から出力される値は（１）式で表され、ジャイロセンサ２２から出力される値は、（２）式で表される。
【数１】
【数２】
（１）式において、ｇは加速度センサ２１の出力値であり、δｇはｇに含まれる誤差量であり、上バーつきのｇはｇの真値である。また、（２）式において、ωはジャイロセンサ２２の出力値であり、δωはωに含まれる誤差量であり、上バーつきのωはωの真値である。従って、（１）式は、加速度センサ２１の出力値を「真値＋誤差量」で表している。同様に、（２）式は、ジャイロセンサ２２の出力値を「真値＋誤差量」で表している。
【００３４】
一般に、誤差量に含まれる高周波成分は、ＬＰＦ（Ｌｏｗ Ｐａｓｓ Ｆｉｌｔｅｒ）により除去することができる。しかし、温度などの環境に依存した時間変化に対して比較的緩やかな誤差（低周波成分）はＬＰＦでは除去することができない。本実施形態では、カルマンフィルタ５によって当該低周波成分を推定する。
【００３５】
また、ジャイロセンサ２２の出力値における回転座標変換は、（３）式〜（７）式に従う。
【数３】
【数４】
【数５】
【数６】
【数７】
（３）式において、ｘ、ｙ、ｚは、時刻ｔ＋Δｔにおける座標を表し、ｘ'、ｙ'、ｚ'は、時刻ｔにおける座標を表す。
【００３６】
また、加速度センサ２１の出力値における回転座標変換は、（８）式に従う。
【数８】
（８）式において、ｖｂｏｄｙは、移動体２００に設定された座標系であり、撮像部１の座標系と同じとしてもよい。このようにした場合、実際にはｖｂｏｄｙにオフセットは含まれることになるが、問題はない。なお、移動体２００が車両である場合、ｖｂｏｄｙは、車軸に設定された座標系であり、カメラ座標系（撮像部１の座標系）と同じとしてよい。また、（８）式において、ｇは加速度センサ２１の出力値である。
【００３７】
そして、並進ベクトルｈｉｍｇと並進ベクトルｈｉｎｓとが等しく、且つ、回転行列Ｒｉｍｇと回転行列Ｒｉｎｓとが等しいとすると、観測方程式は（９）式で表される。
【数９】
（９）式において、Ｘは状態量であり、求めたい誤差量に相当する。また、Ｓは観測量（１２次元）であり、画像から求めた回転行列Ｒｉｍｇ及び並進ベクトルｈｉｍｇが代入される。また、並進ベクトルｈｉｍｇは、方向情報のみであり、スケール情報を含まないため、（９）式では、並進ベクトルｈｉｍｇの上限値として、並進ベクトルｈｉｎｓのノルムを利用した。なお、並進ベクトルｈｉｍｇ及び並進ベクトルｈｉｎｓは、３×３の行列であり、並進ベクトルｈｉｍｇ及び並進ベクトルｈｉｎｓは、３×１の行列である。そのため、（９）式は、全てで１２個のパラメータを含んでいる。そして、（９）式における各要素とは、これらの行列の要素のことである。すなわち、（９）式における各要素とは、当該各パラメータを意味する。
【００３８】
次に、本実施形態に係る測位装置１００における移動体２００の位置（並進ベクトルｈｉｎｓ）及び姿勢（回転行列Ｒｉｎｓ）を推定する測位方法について、図４を参照しながら説明する。
まず、測位装置１００において、慣性算出部４が、ジャイロセンサ２２の出力値に基づいて回転行列Ｒｉｎｓを算出し、加速度センサ２１の出力値に基づいて並進ベクトルｈｉｎｓを算出する。また、画像処理部３が、撮像部１から入力される画像データに基づいて、回転行列Ｒｉｍｇ及び並進ベクトルｈｉｍｇを算出する（ステップＳ１）。
【００３９】
次に、測位装置１００において、カルマンフィルタ５が、回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓに含まれる誤差量を推定する（ステップＳ２）。
【００４０】
次に、測位装置１００は、ステップＳ２で推定した誤差量に基づいて、回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓを補正する（ステップＳ３）。具体的には、カルマンフィルタ５から出力された回転行列Ｒｉｎｓの誤差量がジャイロセンサ２２の出力値に加算されて、慣性算出部４の第１の演算器４１に入力されることにより、回転行列Ｒｉｎｓが補正される。同様に、カルマンフィルタ５から出力された並進ベクトルｈｉｎｓの誤差量が加速度センサ２１の出力値に加算されて、慣性算出部４の第２の演算器４２に入力されることにより、並進ベクトルｈｉｎｓが補正される。
【００４１】
次に、慣性算出部４が補正後の回転行列Ｒｉｎｓ及び補正後の並進ベクトルｈｉｎｓを外部に出力する（ステップＳ４）。すなわち、測位装置１００は、補正後の回転行列Ｒｉｎｓを姿勢変位として、補正後の並進ベクトルｈｉｎｓを位置変位として出力する。
【００４２】
次に、本実施形態に係る測位装置１００における基礎行列（回転行列Ｒｉｍｇ、並進ベクトルｈｉｍｇ）の算出方法について、図５を参照しながら説明する。
まず、撮像部１から画像処理部３の特徴点検出部３１に画像データが入力される（ステップＳ１０１）。
【００４３】
次に、特徴点検出部３１は、撮像部１によって撮像された画像から特徴点を検出する（ステップＳ１０２）。そして、特徴点検出部３１は、当該特徴点の特徴量を特徴点対応付け部３２に入力する。
【００４４】
次に、特徴点対応付け部３２は、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行う（ステップＳ１０３）。そして、特徴点対応付け部３２は、特徴点の対応付けに関する情報と各特徴点の特徴量とを、誤対応除去部３３に入力する。
【００４５】
次に、慣性算出部４が、回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓを算出する（ステップＳ１０４）。そして、慣性算出部４は、当該回転行列Ｒｉｎｓ及び並進ベクトルｈｉｎｓを誤対応除去部３３に入力する。
【００４６】
次に、誤対応除去部３３は、ステップＳ１０３において対応付けられた時刻ｔにおける画像内の特徴点と時刻ｔ＋Δｔにおける画像内の特徴点とのうち、誤って対応付けられた特徴点を除去する（ステップＳ１０５）。そして、誤対応除去部３３は、特徴点（対応点）の特徴量と、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓと、を停止判定部３４に入力する。
【００４７】
次に、停止判定部３４は、移動体２００が停止しているか否かを判定する（ステップＳ１０６）。
ステップＳ１０６において、停止判定部３４は、移動体２００が停止していると判定した場合（ステップＳ１０６；Ｙｅｓ）、加速度センサ２１及びジャイロセンサ２２から慣性算出部４への検出信号の入力を停止させ、ステップＳ１０１に戻る。
ステップＳ１０６において、停止判定部３４は、移動体２００が停止していないと判定した場合（ステップＳ１０６；Ｎｏ）、特徴点（対応点）の特徴量と、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓと、を基礎行列算出部３５に入力する。
【００４８】
次に、基礎行列算出部３５は、特徴点対応付け部３２及び誤対応除去部３３によって対応するとされた、時刻ｔにおける画像内の特徴点の特徴量と時刻ｔ＋Δｔにおける画像内の特徴点の特徴量とに基づいて、最急勾配法を含む繰り返し演算により、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇを算出する（ステップＳ１０７）。
【００４９】
以上に説明した本実施形態に係る測位装置１００及び測位方法によれば、カルマンフィルタ５が、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇとに基づいて、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。通常、画像から移動体２００の移動量を推定すると、オクルージョンや画像内に存在する移動障害物等の影響により誤差が発生する場合がある。しかし、時刻ｔにおける画像と、時刻ｔからΔｔ（１エポック（１時刻））経過した時刻ｔ＋Δｔにおける画像と、を比較することによって移動体２００の移動量を算出する場合、比較的精度良く移動体２００の移動量を算出することができる。具体的には、オクルージョンが毎回（Δｔ毎に）発生するわけではないので、時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像との対応が取れれば、精度良く移動体の移動量を算出することができる。換言すれば、画像処理部３によって移動体２００の移動量（並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇ）を算出する方法の短時間における検出精度は、比較的優れている。一方、慣性センサ２によって、移動体２００の移動量（並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓ）を検出すると、温度などの影響によりドリフト誤差が発生する。そのため、慣性センサ２の検出精度は、時間が経過すると劣化してしまう。換言すれば、慣性センサ２によって移動体２００の移動量を検出する方法の長時間における検出精度は、比較的劣っている。本実施形態に係る測位装置１００及び測位方法においては、短時間における検出精度に比較的優れている画像処理部３によって算出される移動体２００の移動量によって、長時間における検出精度に比較的劣っている慣性センサ２によって検出される移動量を補正する。これにより、測位装置１００は、移動体２００の移動量である並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓを長時間にわたって精度良く推定することができる。換言すれば、測位装置１００は、移動体２００の位置及び姿勢を継続的に且つより精度良く推定することができる。なお、オクルージョンが発生している場合には、直近にカルマンフィルタ５により推定された誤差を用いて慣性センサ（加速度センサ２１及びジャイロセンサ２２）の検出値を補正する。オクルージョンが発生する頻度は比較的少ないため、直近にカルマンフィルタ５により推定した誤差によって慣性センサの検出値を補正する場合があっても、精度良く移動体２００の位置及び姿勢を推定することができる。
【００５０】
また、誤対応除去部３３は、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓに基づいて、時刻ｔ＋Δｔにおける画像内の特徴点を時刻ｔにおける画像内に戻した場合に、時刻ｔ＋Δｔにおける画像内の特徴点が、時刻ｔ＋Δｔにおける画像内の特徴点に対応する時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、時刻ｔにおける画像内の特徴点と、時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする。
これにより、特徴点の対応付けの精度を向上することができる。
【００５１】
また、基礎行列算出部３５は、慣性算出部４によって算出された並進ベクトルｈｉｎｓ及び回転行列Ｒｉｎｓを初期値として用いて、対応点から最急勾配法により、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇを算出する。
これにより、並進ベクトルｈｉｍｇと回転行列Ｒｉｍｇを安定して精度の良く算出することができるとともに、計算時間を短縮することができる。
【００５２】
また、並進ベクトルｈｉｍｇと並進ベクトルｈｉｎｓとが等しく、且つ、回転行列Ｒｉｍｇと回転行列Ｒｉｎｓとが等しいという拘束条件によって作成された観測方程式を適用したカルマンフィルタ５によって、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとに含まれる誤差量を推定する。
これにより、時間Δｔが経過するたびに、推定された誤差量によって、並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとが補正されるため、ドリフト誤差が蓄積するのを防止することができる。
【００５３】
［実施例１］
次に、本実施形態に係る測位装置１００を適用した実施例について説明する。実施例１では、移動体２００として車両を用いた。そして、当該車両に、慣性センサ２としてジャイロ・加速度センサ（ＳＭＧ０７４／２２５）を搭載した。また、評価用に、当該車両に、光ファイバジャイロ（ＦＯＧ）・サーボ加速度計（ＪＭ３４０３／ＪＡ−５ＶＣ４）を搭載した。また、当該車両に、撮像部１としてカメラを搭載した。そして、当該車両を１５００ｍｍ／ｓ以下の移動速度で走行させ、計測を行った。
【００５４】
また、比較例でも、実施例１と同様の車両を用いて計測を行う。しかし、比較例では、カメラによって撮像される画像から算出される並進ベクトルｈｉｍｇ及び回転行列Ｒｉｍｇを用いて並進ベクトルｈｉｎｓと回転行列Ｒｉｎｓとを補正しない。すなわち、比較例において計測される値は、慣性センサ２としてのジャイロ・加速度センサ（ＳＭＧ０７４／２２５）そのものの値である。
【００５５】
ヨー方向の姿勢の計測結果を図６に示し、ロール方向の姿勢の計測結果を図７に示し、ピッチ方向の姿勢の計測結果を図８に示す。図６〜図８において、縦軸に姿勢（°）を示し、横軸にフレーム数を示す。また、図６〜図８において、実線は評価用の光ファイバジャイロ（ＦＯＧ）・サーボ加速度計（ＪＭ３４０３／ＪＡ−５ＶＣ４）の計測値を示し、三角印は比較例としてのジャイロ・加速度センサ（ＳＭＧ０７４／２２５）の計測値を示し、大きい丸印は実施例１の計測値を示し、小さい丸印はカメラによって撮像された画像から算出した値を示す。
図６〜図８に示すように、比較例に比べて、実施例１の計測値は、評価用の計測値に近い値となった。従って、本実施形態に係る測位装置１００を適用することにより、車両の位置及び姿勢を継続的に且つより精度良く推定することができるといえる。
【００５６】
尚、本発明は上述した各実施の形態に限定されず、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００５７】
１ 撮像部
２ 慣性センサ（慣性検出部）
２１ 加速度センサ（慣性検出部）
２２ ジャイロセンサ（慣性検出部）
３ 画像処理部（画像処理手段）
４ 慣性算出部（慣性算出手段）
５ カルマンフィルタ（誤差量推定手段）
１００ 測位装置
２００ 移動体
【特許請求の範囲】
【請求項１】
周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置であって、
前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する画像処理手段と、
前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する慣性算出手段と、
前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する誤差量推定手段と、
を備え、
前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する測位装置。
【請求項２】
前記画像処理手段は、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を算出し、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行って、前記慣性算出手段によって算出された前記慣性並進量及び前記慣性回転量に基づいて、前記時刻ｔ＋Δｔにおける画像内の特徴点を前記時刻ｔにおける画像内に戻した場合に、前記時刻ｔ＋Δｔにおける画像内の特徴点が、前記時刻ｔ＋Δｔにおける画像内の特徴点に対応する前記時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする請求項１に記載の測位装置。
【請求項３】
前記画像処理手段は、
前記慣性算出手段によって算出された前記慣性並進量及び前記慣性回転量を初期値として用いて、前記対応点から最急勾配法を含む繰り返し演算により、前記画像並進量と前記画像回転量を算出する請求項２に記載の測位装置。
【請求項４】
前記誤差量推定手段は、
前記画像並進量と前記慣性並進量とが等しく、且つ、前記画像回転量と前記慣性回転量とが等しいという拘束条件によって作成された観測方程式を適用したカルマンフィルタによって、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する請求項１乃至３の何れか一項に記載の測位装置。
【請求項５】
周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置における測位方法であって、
前記測位装置は、
前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する画像処理と、
前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する慣性算出処理と、
前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する誤差量推定処理と、
前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する補正処理と、
を実行する測位方法。
【請求項６】
前記画像処理において、前記測位装置は、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を算出し、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行って、前記慣性算出処理において算出した前記慣性並進量及び前記慣性回転量に基づいて、前記時刻ｔ＋Δｔにおける画像内の特徴点を前記時刻ｔにおける画像内に戻した場合に、前記時刻ｔ＋Δｔにおける画像内の特徴点が、前記時刻ｔ＋Δｔにおける画像内の特徴点に対応する前記時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする請求項５に記載の測位方法。
【請求項７】
前記画像処理において、前記測位装置は、
前記慣性算出処理において算出した前記慣性並進量及び前記慣性回転量を初期値として用いて、前記対応点から最急勾配法を含む繰り返し演算により、前記画像並進量と前記画像回転量を算出する請求項６に記載の測位方法。
【請求項８】
前記誤差量推定処理において、前記測位装置は、
前記画像並進量と前記慣性並進量とが等しく、且つ、前記画像回転量と前記慣性回転量とが等しいという拘束条件によって作成された観測方程式を適用したカルマンフィルタによって、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する請求項５乃至７の何れか一項に記載の測位方法。
【請求項１】
周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置であって、
前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する画像処理手段と、
前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する慣性算出手段と、
前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する誤差量推定手段と、
を備え、
前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する測位装置。
【請求項２】
前記画像処理手段は、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を算出し、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行って、前記慣性算出手段によって算出された前記慣性並進量及び前記慣性回転量に基づいて、前記時刻ｔ＋Δｔにおける画像内の特徴点を前記時刻ｔにおける画像内に戻した場合に、前記時刻ｔ＋Δｔにおける画像内の特徴点が、前記時刻ｔ＋Δｔにおける画像内の特徴点に対応する前記時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする請求項１に記載の測位装置。
【請求項３】
前記画像処理手段は、
前記慣性算出手段によって算出された前記慣性並進量及び前記慣性回転量を初期値として用いて、前記対応点から最急勾配法を含む繰り返し演算により、前記画像並進量と前記画像回転量を算出する請求項２に記載の測位装置。
【請求項４】
前記誤差量推定手段は、
前記画像並進量と前記慣性並進量とが等しく、且つ、前記画像回転量と前記慣性回転量とが等しいという拘束条件によって作成された観測方程式を適用したカルマンフィルタによって、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する請求項１乃至３の何れか一項に記載の測位装置。
【請求項５】
周辺領域の画像を経時的に撮像する撮像部と、移動体の移動量を検出する慣性検出部と、を備える前記移動体の位置及び姿勢を推定する測位装置における測位方法であって、
前記測位装置は、
前記撮像部によって撮像された時刻ｔにおける画像と時刻ｔ＋Δｔにおける画像とを比較することにより、前記移動体の画像並進量と画像回転量とを算出する画像処理と、
前記慣性検出部によって検出された移動量に基づいて、前記移動体の慣性並進量と慣性回転量とを算出する慣性算出処理と、
前記画像並進量と前記画像回転量とに基づいて、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する誤差量推定処理と、
前記誤差量に基づいて、前記慣性検出部によって検出された移動量を補正する補正処理と、
を実行する測位方法。
【請求項６】
前記画像処理において、前記測位装置は、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を算出し、
前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点との対応付けを行って、前記慣性算出処理において算出した前記慣性並進量及び前記慣性回転量に基づいて、前記時刻ｔ＋Δｔにおける画像内の特徴点を前記時刻ｔにおける画像内に戻した場合に、前記時刻ｔ＋Δｔにおける画像内の特徴点が、前記時刻ｔ＋Δｔにおける画像内の特徴点に対応する前記時刻ｔにおける画像内の特徴点を中心としたＮピクセル四方（Ｎは、正の整数）の範囲内にある場合に、前記時刻ｔにおける画像内の特徴点と、前記時刻ｔ＋Δｔにおける画像内の特徴点と、を互いに対応する対応点とする請求項５に記載の測位方法。
【請求項７】
前記画像処理において、前記測位装置は、
前記慣性算出処理において算出した前記慣性並進量及び前記慣性回転量を初期値として用いて、前記対応点から最急勾配法を含む繰り返し演算により、前記画像並進量と前記画像回転量を算出する請求項６に記載の測位方法。
【請求項８】
前記誤差量推定処理において、前記測位装置は、
前記画像並進量と前記慣性並進量とが等しく、且つ、前記画像回転量と前記慣性回転量とが等しいという拘束条件によって作成された観測方程式を適用したカルマンフィルタによって、前記慣性並進量と前記慣性回転量とに含まれる誤差量を推定する請求項５乃至７の何れか一項に記載の測位方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−１４１２２１（Ｐ２０１１−１４１２２１Ａ）
【公開日】平成２３年７月２１日（２０１１．７．２１）
【国際特許分類】
【出願番号】特願２０１０−２６４３（Ｐ２０１０−２６４３）
【出願日】平成２２年１月８日（２０１０．１．８）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２３年７月２１日（２０１１．７．２１）
【国際特許分類】
【出願日】平成２２年１月８日（２０１０．１．８）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]