JP6658001B2

JP6658001B2 - 位置推定装置、プログラム、位置推定方法

Info

Publication number: JP6658001B2
Application number: JP2016013860A
Authority: JP
Inventors: 康宏梶原; 靖子白鷹; 和史松下; 多聞貞末; 和寛 ▲高▼澤
Original assignee: Ricoh Co Ltd
Current assignee: Ricoh Co Ltd
Priority date: 2016-01-27
Filing date: 2016-01-27
Publication date: 2020-03-04
Anticipated expiration: 2036-01-27
Also published as: JP2017134617A

Description

本発明は、位置推定装置、プログラム及び位置推定方法に関する。

運転者が移動体を操縦したり外部から操縦しなくても移動体が自立的に決められた経路に沿って移動したりする技術の開発が進められている。移動体が経路に沿って移動するには移動中も移動体が位置を正確に推定する必要がある。そして、移動体の位置や姿勢を移動体が備えるカメラを用いて推定するＶｉｓｕａｌ−ＳＬＡＭ（Simultaneous Localization and Mapping；同時位置推定地図構築）が知られている。Ｖｉｓｕａｌ−ＳＬＡＭでは、時系列の映像から周囲の静止物体の三次元位置情報を含む環境地図の生成と、移動体の位置や姿勢の推定とを同時に行うことができる。

カメラを用いた位置と姿勢の検出では、時系列に撮像された画像から複数の特徴点とその三次元座標を求めておき、次のタイミングで撮像された画像の特徴点との対応付けを行う。次のタイミングの画像のうち追跡された特徴点の三次元座標が分かっているので、該三次元座標と画像内の特徴点との対応を利用して自己位置と姿勢の推定が行われる。

このようにカメラを用いて位置と姿勢が検出される場合、位置と姿勢の精度が撮像された映像の品質に大きく依存することが知られている。そこで、カメラによる映像の品質に対しロバストな位置推定技術が従来から考案されている（例えば、特許文献１参照。）。特許文献１には、異なるタイミングで撮像された複数の画像のそれぞれから対応した対応点を検索し、検索された対応点の全ての組から対応点の複数の組を繰り返し選択し、繰り返し選択された対応点の複数の組に基づいて自車両の運動推定候補を各々算出する運動推定装置が開示されている。

しかしながら、従来の技術では、特徴点の対応付けが困難なほど画像が大きく変化した場合に、推定される位置や姿勢の精度が低下するおそれがあるという問題がある。例えば、移動体は目的地に早く到着することが望ましいと考えられるが、移動体が早く移動するために位置や姿勢の変化が大きくなると異なるタイミングで撮像された画像内の特徴点の対応付けが困難になる場合がある。この場合、過去に対応付けが可能であった画像を用いて位置や姿勢が推定されるので、推定される位置や姿勢の精度が低下するおそれがある。

上術した課題に鑑み、本発明は、推定される位置の精度を向上できる位置推定装置を提供することを目的とする。

本発明は、被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する位置推定装置であって、前記撮像装置が撮像した前記画像から特徴点を抽出する画像特徴点抽出部と、前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡する画像特徴点追跡部と、前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成する環境地図生成部と、前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出する位置検出部と、前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、前記特徴点を追跡可能な前記画像を撮像できる運動を定義する許容運動定義部と、を有する。

推定される位置の精度を向上できる位置推定装置を提供することができる。

移動体の概略斜視図の一例である。位置計測部と移動体制御部のハードウェア構成図の一例である。位置計測部の機能ブロック図の一例である。ピンホールカメラモデルにおける画像内の特徴点の座標とワールド座標系の三次元座標の対応を説明する図の一例である。記憶部に記憶される情報を模式的に示す図の一例である。本実施形態における位置計測部の全体的な処理手順を示すフローチャート図の一例である。位置姿勢計測処理を説明するための図の一例である。環境地図生成処理の一例示すフローチャートである。許容運動定義部が許容運動を定義する手順を示すフローチャート図の一例である。図９の許容運動判定の処理を説明するフローチャート図の一例である。移動体制御部の動作手順を示すフローチャート図の一例である。経路コストの算出方法を説明する図の一例である。

以下、本発明を実施するための形態について図面を参照しながら説明する。
図１は、移動体の概略斜視図の一例である。図１の移動体はマルチコプターやドローンなどと呼ばれる飛行物体１００である。この飛行物体１００は放射状に配置される複数のロータ（回転翼）９を回転させることによって上昇・下降し、また、前後左右等に移動する。上昇時には４つのロータ９の回転数を増大させ、下降時には４つのロータ９の回転数を減少させる。また、前進時又は後退時には対称にあるロータ９の回転数に変化を付け機体を傾けることで前進又は後退する。

本実施形態の飛行物体１００は、Ｖｉｓｕａｌ−ＳＬＡＭ（Simultaneous Localization and Mapping；同時位置推定地図構築）を使用して、時系列の映像から周囲の静止物体（三次元点）の三次元座標を含む環境地図の生成と、自己位置や姿勢の推定とを同時に行う。

図１の飛行物体１００は、位置を計測する位置計測部１０及び移動に関する制御を行う移動体制御部３０を有している。位置計測部１０は飛行物体１００の現在の位置と姿勢を推定、計測又は算出等するものであり、移動体制御部３０は現在の位置を参考にしながら目的地まで飛行物体１００を移動させる。また、飛行物体１００はステレオカメラ５０を有している。ステレオカメラ５０は、被写体との距離を検出できるという特徴を有する。距離を検出可能な手段を別に設ければ単眼のカメラでもよいし、距離情報を取得できる単眼カメラでもよい。距離を検出可能な手段の一例としてレーザーレーダが挙げられる。

図２は、位置計測部１０と移動体制御部３０のハードウェア構成図の一例である。位置計測部１０は、例えば、スマートフォン、タブレット端末、ノートＰＣ、デジタルカメラ、又は、飛行物体１００や車両、移動式ロボット等の各種移動体に設けられた制御装置を本体とすることができる。したがって、位置計測部１０は情報処理装置としての機能を備える。

位置計測部１０は、入力装置１０１と、出力装置１０２と、ドライブ装置１０３と、カメラＩ/Ｆ１０４と、位置計測装置１０５と、移動体制御部Ｉ/Ｆ１０６、補助記憶装置１０７と、主記憶装置１０８と、各種制御を行うＣＰＵ（Central Processing Unit）１０９と、ネットワーク接続装置１１０と、姿勢センサ１１１と、論理回路１１２とを有し、これらはシステムバスＢで相互に接続されている。

入力装置１０１は、ユーザ等が操作するキーボード、マウス等のポインティングデバイスやマイク等の音声入力デバイス等を有し、ユーザ等からの各種操作信号の入力を受け付ける。また、入力装置１０１として出力装置１０２と一体のタッチパネルを備えていてもよい。

出力装置１０２は、位置計測部１０の状態を示すＬＥＤ、ＣＵＩ（Character User Interface）やＧＵＩ（Graphical User Interface）などで各種の情報を表示するディスプレイなどであり、位置計測部１０の処理結果や状態を外部に出力する。更に、音声を出力するスピーカの機能を備えていてよい。

ドライブ装置１０３には、例えば記録媒体１０３ａ等が着脱自在に装着され、ＣＰＵ１０９からの制御で記録媒体１０３ａに記録された各種情報を読み込んだり、各種情報を記録媒体１０３ａに書き込んだりすることができる。位置計測部１０が実行するプログラムは、例えば記録媒体１０３ａ等に記憶された状態で配布されたり、プログラムを配信したりするサーバから配布される。

記録媒体１０３ａは、各種のプログラムやデータ等を記憶するコンピュータで読み取り可能な媒体である。記録媒体１０３ａは、例えばＵＳＢ（Universal Serial Bus）メモリやＳＤメモリ、ＣＤ−ＲＯＭ等の可搬型である。

カメラＩ/Ｆ１０４はステレオカメラ５０とのインタフェースである。カメラＩ/Ｆ１０４はステレオカメラ５０にカメラパラメータ（主に内部パラメータをいい、例えば焦点距離、露光時間、画像中心、絞り値、ＩＳＯ感度、アスペクト比、スキュー補正量、ゆがみ補正量、フレーム間隔など）を送信し、ステレオカメラ５０から画像データやカメラパラメータを受信したりする。

位置計測装置１０５は、ＧＰＳ（Global Positioning System；全地球測位システム）機能等を用いて、飛行物体１００の位置情報（座標：緯度、経度、標高）を取得する。室内や地下ではＩＭＥＳ（Indoor Messaging System）やビーコンから位置情報を取得してもよい。

移動体制御部Ｉ/Ｆ１０６は、移動体制御部３０と通信するためのインタフェースである。具体的にはＵＳＢケーブル、Ｉ^２Ｃ，ＳＰＩ、MicroWireなどが接続される。

補助記憶装置１０７は、例えばＳＳＤ（Solid State Drive）やＨＤＤ（Hard Disk Drive）等のストレージ装置等である。補助記憶装置１０７は、ＣＰＵ１０９が図２の各ブロックへ出力する制御信号を生成するためのプログラム、オペレーティングシステム等が記憶されている。

主記憶装置１０８には、補助記憶装置１０７から読み出されたプログラム等を展開され、ＣＰＵ１０９により実行される。主記憶装置１０８は、例えばＲＡＭ（Random Access Memory）等であるが、これに限定されるものではない。

ＣＰＵ１０９は、オペレーティングシステムやプログラムに基づいて、各種演算や図２の各部に対し制御信号を出力し、位置計測部１０の全体の処理を制御する。プログラムの実行結果等を補助記憶装置１０７に格納することができる。

ネットワーク接続装置１１０は、ＣＰＵ１０９からの制御信号に基づき、各種のネットワークに接続する通信装置である。例えば、無線ＬＡＮ（Local Area Network）や３Ｇ、４Ｇの公共無線通信網に接続する。あるいは有線ＬＡＮに接続してもよい。これにより、ネットワーク接続装置１１０は、プログラムや設定情報等をネットワークに接続されている外部装置等から取得できる。また、ネットワーク接続装置１１０は、プログラムを実行することで得られた実行結果等を外部装置等に提供することができる。

姿勢センサ１１１は、飛行物体１００の姿勢を検出するための各種のセンサである。例えば、３軸のジャイロセンサ、３軸の加速度センサ、及び、磁気方位センサを有する。

論理回路１１２は、特定の情報処理を行うハードウェアである。具体的には、ＡＮＤ、ＯＲ、ＮＡＮＤ、ＮＯＲ回路などを要素として、加算器、乗算器、メモリ、ラッチなど、及び、これらが組み合わされたＩＣ、ＬＳＩ、ＡＳＩＣ、ＦＰＧＡなどである。本実施形態では、後述する特徴点の抽出、追跡、及び、視差演算などを論理回路１１２が行う。

本実施形態では、上述したコンピュータ本体のハードウェア構成に実行プログラム（例えば、位置姿勢計測プログラム等）をインストールすることで、ハードウェア資源とソフトウェアとが協働して本実施形態における位置姿勢計測処理等を実現することができる。また、上述した位置姿勢計測処理に対応するプログラムは、例えば装置上で常駐している状態であってもよく、起動指示により起動させてもよい。

続いて、移動体制御部３０について説明する。移動体制御部３０は飛行物体１００の運動を制御する。運動とは、位置、移動速度、及び、姿勢の少なくとも１つをいう。移動体制御部３０は、位置計測部Ｉ/Ｆ２０１、演算制御装置２０２、モータコントローラ２０３及び４つのモータ２０４を有している。

位置計測部Ｉ/Ｆ２０１は、位置計測部１０と通信するためのインタフェースである。具体的にはＵＳＢケーブル、Ｉ２Ｃ，ＳＰＩ、MicroWireなどが接続される。例えば、位置計測部１０が推定した位置、移動速度及び姿勢、並びに後述する許容運動マップが位置計測部Ｉ/Ｆ２０１を介して送信される。

演算制御装置２０２は、４つのモータ２０４の回転数を制御することでロータ９の回転数を制御し、位置（三次元空間の位置）、移動速度、姿勢などを制御する。４個のロータ９の１つ以上を揚力発生用のロータ９や姿勢制御用のロータ９として動作するように制御する。更に、移動の際には、左右又は前後のロータ９の回転数をアンバランス（回転数差を設けて）にして機体を傾けるように制御することにより移動させる。なお、ロータ９の回転により生じる飛行物体１００への回転トルクを抑制するため、４個のロータ９のうち、２つを時計回り方向へ回転させ、残りの２つを反時計回り方向へ回転させる。

演算制御装置２０２には予め経由地を含む目的地の座標が登録されており、位置計測部１０が検出する位置と姿勢に基づいて、経由地又は目的地に接近するように飛行物体１００の移動方向を決定する。適切な移動速度は予め定められているものとする。演算制御装置２０２は移動方向と移動速度等により決定した４つのロータ９の回転数を指示する回転数指示信号をモータコントローラ２０３に出力する。

モータコントローラ２０３は回転数指示信号に応じてモータ２０４の回転数を制御する。回転数指示信号に応じた電流がモータ２０４に流れるように例えばＰＷＭ信号を生成しモータ２０４に出力する。モータコントローラ２０３はモータ２０４の回転数を検出又は取得してフィードバック制御を行う。また、同一回転数で回転すべきモータ２０４に微小な回転数の違いがあれば、同一回転数となるように電流を制御する。更に、２つのロータ９の間で回転数指示信号に応じた回転数差が得られない場合、指示された回転数差が得られるように少なくとも１つのモータ２０４に流れる電流を制御する。

＜位置計測部１０の機能構成＞
続いて、図３を用いて位置計測部１０の機能について説明する。図３は、位置計測部１０の機能ブロック図の一例である。位置計測部１０は、画像特徴点抽出部１１、画像特徴点追跡部１２、深度計測部１３、撮像部制御部１４、環境地図生成部１５、移動体位置計測部１６、及び、許容運動定義部１７を有する。

画像特徴点抽出部１１、画像特徴点追跡部１２及び深度計測部１３は上述の論理回路１１２により実現される。撮像部制御部１４、環境地図生成部１５、移動体位置計測部１６、及び、許容運動定義部１７は、補助記憶装置１０７から主記憶装置１０８に展開されたプログラムをＣＰＵ１０９が実行することで実現される機能又は手段である。ただし、画像特徴点抽出部１１、画像特徴点追跡部１２及び深度計測部１３が、ＣＰＵ１０９がプログラムを実行することで実現されてもよい。

また、位置計測部１０は記憶部１０００を有している。記憶部１０００は、補助記憶装置１０７又は主記憶装置１０８の少なくとも一方により構築される情報の記憶手段である。

撮像部制御部１４は、カメラパラメータをステレオカメラ５０に出力して、ステレオカメラ５０を制御する。本実施形態では、カメラパラメータのうち、少なくとも露光時間とフレーム間隔が出力される。フレーム間隔（撮像間隔）は固定でもよいし撮像される環境などに応じて撮像部制御部１４が制御してもよい。固定の場合、フレーム間隔は飛行物体１００の補助記憶装置１０７などに記録されている。また、撮像部制御部１４は、ステレオカメラ５０が撮像する画像のヒストグラムを利用して、例えば飽和する画素がなく低輝度から高輝度までの使用されるように露光時間を決定する。

深度計測部１３は、ステレオ計測法により各画素又は画素ブロックごとにステレオカメラ５０から被写体までの距離を計測する。いわゆる、ブロックマッチング法により画像を一定の大きさのブロックに分割し、それぞれのブロックの水平方向のずれを視差として算出する。更に、ブロックコストを算出した後、周囲との滑らかさを考慮して視差を最適化するSemi-Global-Mating法などを適用することが好ましい。詳細は例えば非特許文献１に開示されている。

画像特徴点抽出部１１は、ステレオカメラ５０が撮像した画像から特徴点（画像特徴点）を抽出する。画像特徴点抽出部１１は、撮像した画像毎に特徴点を抽出する。画像特徴点抽出部１１は、例えば画像の輝度値を用いて特徴点を抽出したり、画像中の物体の形状等から特徴点を抽出したりする手法があるが、これらに限定されるものではない。

特徴点の抽出手法としては、例えばＨａｒｒｉｓオペレータやＦＡＳＴ（Features from Accelerated Segment Test）オペレータを用いることができる。Ｈａｒｒｉｓオペレータは、２つのエッジの交点をコーナーとして検出する画像処理である。ＦＡＳＴオペレータは、注目画素の周囲の円周上における１６個の画素の画素値が連続してｎ個以上、注目画素より明るいか暗い場合にコーナーを検出する画像処理である。その他、特徴点の抽出手法として、例えばＳＩＦＴ（Scale Invariant Feature Transform）特徴量記述子、ＳＵＲＦ（Speeded Up Robust Features）特徴量記述子を用いることができる。画像特徴点抽出部１１は、画像上の特徴点の座標とその特徴量（画素値、濃度、輝度など）とを画像特徴点情報として記憶部１０００に記録する。

画像特徴点追跡部１２は、時系列の（異なるタイミングで撮像された）画像の特徴点を追跡する。画像特徴点追跡部１２は、例えば画像特徴点抽出部１１が抽出した特徴点を、その特徴点を抽出した画像より後にステレオカメラ５０が撮像した画像の中から探索することで各特徴点の追跡を行う。また、画像特徴点追跡部１２は、探索結果から追跡の成否（成功、失敗）を判定し、その判定結果に基づき記憶部１０００の画像特徴点情報に登録等を行う。すなわち、画像の特徴点ごとに追跡の成否が登録される。

画像特徴点追跡部１２は、例えば、ブロックマッチングや特徴量等による探索手法を用いて特徴点の追跡を行うことができる。ブロックマッチングを用いて特徴点の追跡を行う場合、画像特徴点追跡部１２は、時系列の１つ前の画像の特徴点を含むブロックを取り出し、時系列の１つ後の画像に対しブロックマッチングを行う。

また、上述した特徴量を用いた探索を行う場合には、画像特徴点追跡部１２は、画像特徴点抽出部１１と同様の特徴点記述処理を行い、画像特徴点情報として記録されている特徴点と、相関の高い特徴点を探索する。また、画像特徴点追跡部１２は、上述の探索の結果、相関が所定の閾値を超えている場合には追跡成功と判定し、閾値以下の場合には追跡失敗と判定する。

環境地図生成部１５は、画像特徴点情報、カメラパラメータ、及び、深度計測部１３の計測結果から、周囲の環境の三次元地図を生成する。例えば、飛行物体１００の初期位置（後述するワールド座標系の三次元座標）は、位置計測装置１０５により既知である。この初期位置において、カメラパラメータの焦点距離と深度計測部１３の結果により特徴点までの距離が分かるので、特徴点の三次元座標は図４のようにして求めることができる。

図４（ａ）は、ピンホールカメラモデルにおける画像内の特徴点の座標とワールド座標系の三次元座標の対応を説明する図の一例である。（ｘ，ｙ）は特徴点の画像上の座標で、（Ｘ，Ｙ，Ｚ）は特徴点のワールド座標系の三次元座標である。ｆは焦点距離である。深度計測部１３により計測された深度（距離）がＺであるので、図４（ｂ）に示すｘとＸ、ｆとＺの比例関係からＸ＝Ｚ・（ｘ-cx）/ｆが得られる。同様に、Ｙ＝Ｚ・（ｙ-cy）/ｆが得られる。cxとcyは画像中である（画像内の特徴点の座標が左上コーナーを原点としているため）。

したがって、飛行物体１００の初期位置とそこで撮像された画像に含まれる三次元点（ワールド座標系における特徴点）の三次元座標が分かることになる。環境地図生成部１５はこれにより簡易的な環境地図を生成しておき、後に最適化を行う。

図３に戻り、移動体位置計測部１６は、環境地図と画像特徴点情報から、自身の位置と姿勢を計測する処理を行う。初期位置から飛行物体１００が移動して周囲を撮像した場合、異なる場所から同じ特徴点が撮像される。移動体位置計測部１６は、特徴点のワールド座標系における三次元座標をカメラ座標系に変換する回転行列Ｒ,平行移動ベクトルｔを後述するように求めることで、移動後の位置と姿勢を計測する。そして、この移動後の位置と姿勢において図４のようにして特徴点の三次元座標が分かる。したがって、ワールド座標系において撮像時の飛行物体１００の位置と姿勢を常に更新し、かつ、環境地図を生成しながら移動できる。

許容運動定義部１７は、露光時間とフレーム間隔を元に、飛行物体１００に許容される又は制限される運動を定義し移動体制御部３０に出力する。移動体制御部３０は制約の範囲内で飛行物体１００の運動を制御する。詳細は後述される。

<<記憶部に記憶される情報>>
図５は記憶部１０００に記憶される情報を模式的に示す図の一例である。記憶部１０００には、環境地図１００１、画像特徴点情報１００２、カメラパラメータ１００３、許容運動情報１００４、移動体位置姿勢情報１００５、及び、運動パラメータ使用運動１００６が記憶される。

環境地図１００１とは、特徴点の三次元座標が登録された地図である。特徴点ごとに三次元座標が登録されており、飛行物体１００が移動した空間の地図が再現される。これにより、建物や壁等の物体、道路等がどこにあるか等の周辺環境の情報が得られる。なお、環境地図は、周囲の静止物体等の２次元の情報であってもよい。

画像特徴点情報１００２は、画像ごとに以下の情報を有する。
画像上の特徴点の座標
特徴点の特徴量（画素値、濃度、輝度など）
時系列の１つ前の画像からの追跡の成否
カメラパラメータ１００３については上述のように主に内部パラメータであり、すくなくとも露光時間とフレーム間隔を含む。露光時間とはＣＣＤやＣＭＯＳの撮像素子が被写体光に晒されている時間である。フレーム間隔とは、ステレオカメラ５０が１つの画像を撮像してから次の画像を撮像するまでの時間的な間隔である。

許容運動情報１００４は、複数の運動パラメータのうち、許容運動定義部１７により許容された運動パラメータ又は運動パラメータを特定するための情報である。運動パラメータは、飛行物体１００の運動の内容を規定するパラメータである。例えば、剛体の自由度は６つ（ピッチ角θ、ロール角φ、ヨー角ψの回転、Ｘ，Ｙ，Ｚ方向の移動）であるが、飛行物体１００が例えば一定方向へ移動する場合でも移動体制御部３０は複数のモータ２０４を制御する。このため、飛行物体の運動を制御しやすいように、運動パラメータがいくつか設定されている。運動パラメータは例えば、上昇、下降、右旋回、左旋回、前進、後進、右移動、左移動などであるがこれらに限られない。

移動体位置姿勢情報１００５は、特徴点が検出された画像を撮像したステレオカメラ５０の位置と姿勢に関する情報である。移動体位置姿勢情報１００５には撮像した画像ごとに位置と姿勢が登録されている。

運動パラメータ使用運動１００６は各運動パラメータで使用する運動を示す。例えば、上昇であれば、回転は伴わないので、ピッチ角θ、ロール角φ、及び、ヨー角ψが変化する運動は行われない。また、移動を伴わない姿勢変化では、位置が変化しない（後述する平行移動ベクトルがゼロ）。Ｘ方向への移動であれば、移動の原理に基づく姿勢の変化とＸ座標が変化する。運動パラメータ使用運動１００６には運動パラメータごとにピッチ角θ、ロール角φ、ヨー角ψ、Ｘ座標、Ｙ座標、Ｚ座標が変化するかどうかが対応付けて登録されている。

＜遺影計測部の処理＞
次に、図６、７を用いて、本実施形態における位置姿勢計測処理の一例について、フローチャートを用いて説明する。図６は、本実施形態における位置計測部１０の全体的な処理手順を示すフローチャート図の一例である。

ステレオカメラ５０は、撮像部制御部１４から通知されたカメラパラメータ等により画像を取得する（Ｓ１０）。ステレオカメラ５０は一定の周期で撮像を行い、時系列に画像を生成する。例えば、飛行物体１００が移動している場合、ステレオカメラ５０は、移動中の時系列な画像を生成する。ステレオカメラ５０は、撮像した画像を、画像特徴点抽出部１１、画像特徴点追跡部１２、及び深度計測部１３に出力する。

次に、画像特徴点抽出部１１は、ステップＳ１０で得られた画像から特徴点を抽出する（Ｓ２０）。特徴点は記憶部１０００の画像特徴点情報１００２に記憶される。

次に、画像特徴点追跡部１２は、連続する画像間において、Ｓ２０の処理で得られた特徴点を追跡する（Ｓ３０）。追跡の成否は記憶部１０００の画像特徴点情報１００２に記憶される。

深度計測部１３は画像の画素又は画素ブロック毎に深度（距離情報）を計測する（Ｓ４０）。深度は画像の画素又は画素ブロックに埋め込まれるので画像と共に参照される。あるいは、記憶部１０００の画像特徴点情報１００２に特徴点と対応付けて記憶してもよい。

なお、環境地図生成部１５は、図４に示したように、記憶部１０００に記憶されている画像の特徴点（座標）と、飛行物体の位置と姿勢及び深度を用いて環境地図を随時作成しておく。

次に、移動体位置計測部１６は、飛行物体１００（又は、ステレオカメラ５０）の位置と姿勢の計測を行う（Ｓ５０）。位置と姿勢は記憶部１０００の移動体位置姿勢情報１００５に記憶される。

次に、環境地図生成部１５は、画像特徴点情報１００２、及び、Ｓ５０の処理で得られる移動体位置姿勢情報１００５等を用いて環境地図生成処理を行う（Ｓ６０）。なお、Ｓ６０の処理では、環境地図の生成だけでなく、環境地図の更新（例えば、補正、削除、拡張）等を行うこともできる。

次に、許容運動定義部１７はカメラパラメータ１００３と移動体位置計測部１６が算出する移動速度と回転速度に基づいて、画像における特徴点の移動量を算出し、許容できる運動パラメータを決定する（Ｓ７０）。

＜処理の詳細＞
以下では、図６のステップＳ５０、Ｓ６０、Ｓ７０について詳細に説明する。

<<Ｓ５０位置姿勢計測処理>>
上述した位置計測部１０における位置姿勢計測処理（Ｓ５０）について具体的に説明する。図７は、位置姿勢計測処理を説明するための図の一例である。移動体位置計測部１６は、環境地図の生成で得られた特徴点の三次元座標と画像における特徴点の座標の対応関係を用いて位置と姿勢を計測する。

飛行物体１００（ステレオカメラ５０）と共に移動する座標系をカメラ座標系と称し、扱う三次元空間の全体の座標系をワールド座標系と称す。カメラ座標系は例えばカメラの撮像素子の中央を原点とする座標系である。ワールド座標系はＧＰＳで使用されるWGS84座標系、世界測地系又は日本測地系などである。カメラ座標系のＺ方向が飛行物体１００の正面、Ｘ方向が側面、Ｙ方向が下面であるとする。Ｙ軸周りの回転により生じる姿勢はヨー角（ψ）で指定され、Ｚ軸周りの回転により生じる姿勢はロール角（ρ）で指定され、Ｘ軸周りの回転により生じる姿勢はピッチ角（θ）で指定される。

位置計測部１０は、ワールド座標系からカメラ座標系へ変換する回転行列Ｒ、平行移動ベクトルｔを、位置と姿勢の推定により求める。カメラ座標系とワールド座標系とを変換する行列のうち回転成分の行列を回転行列Ｒ、平行移動成分の行列を平行移動ベクトルｔと称する。

ここで、Ｒ、ｔは、それぞれ以下の（１）式で表される。

なお、以下では、右側の右カメラ（ステレオカメラ５０の撮像方向と同じ向きを見た場合に右側）により撮像された画像を用いるものとして説明するが、左カメラの画像を用いてもよい。

回転行列Ｒ、平行移動ベクトルｔを求めるために、画像内にある特徴点のワールド座標系での三次元座標Ｘ_i ^w、及び、ステレオカメラ５０の透視投影行列を利用する。透視投影行列には以下のカメラパラメータ１００３が使用される。
・Ｘ方向の焦点距離fx
・Ｙ方向の焦点距離fy
・Ｘ方向の画像中心ox
・Ｙ方向の画像中心oy
三次元座標Ｘ_i ^w、透視投影行列は、それぞれ以下に示す（２）式、（３）式で表される。

透視投影行列Ｐは、三次元点を撮像面に投影する行列である。三次元点が画像のどの位置に撮像されるかを算出することが可能になる。なお、透視投影行列Ｐは同次座標系で表されている。

特徴点の三次元座標については、上述した環境地図１００１に登録されている。また、透視投影行列Ｐで使用される焦点距離や画像中心などはカメラパラメータ１００３に登録されている。カメラパラメータ１００３については、例えばカメラキャリブレーションによって担当者等が予め求めおくことができる。カメラキャリブレーションについては、例えばZhangの手法（Z.Zhang,"A flexible new technique for camera calibration",IEEE Transactions on Pattern Analysis and Machine Intelligence,22（１４）:1330-1334,2000）等を用いることができるが、これに限定されるものではない。

また、本実施形態において、移動体位置計測部１６は、上述の処理以外に、ステレオカメラ５０の歪み補正を行ってもよい。なお、歪みのパラメータについてもZhangの手法で得ることができる。また、歪み補正については、特徴点抽出を行う前に画像自体の歪み補正を行ってもよく、特徴点を抽出した後に、歪みのパラメータ等を用いて補正してもよい。移動体位置計測部１６は、上述したいずれかの処理を行った後に、位置と姿勢の推定を行うことで、より高精度にステレオカメラ５０又は飛行物体１００の位置と姿勢を計測することができる。

移動体位置計測部１６は、三次元座標Ｘ_i ^w、及び、透視投影行列Ｐを利用して、再投影誤差を最小化にするＲ，ｔ及び三次元点の座標を求める。再投影誤差は、特徴点の三次元座標を画像上に投影した点の座標（再投影座標）と、実際にそれがステレオカメラ５０で観測された座標との差分により定義される。ステレオカメラ５０で観測された座標とは、画像特徴点情報１００２に登録された特徴点の画像内の座標である。

再投影座標は、例えば「三次元点のカメラ座標系への変換」及び「仮想スクリーンへの投影」により求めることができる。

まず、三次元点のカメラ座標系への変換は、以下に示す（４）式で求めることができる。（４）式では、カメラ座標系での三次元点の座標をＸ_i ^cとした。

上述した（４）式により、カメラの座標系での三次元点の座標を得ることができる。

次に、仮想スクリーンへ再投影された三次元点の座標を求める。ここで、再投影点の座標をx_i ^r、y_i ^rとする。x_i ^rは右カメラの画像のｘ座標、y_i ^rは右カメラの画像のｙ座標である。再投影点の座標x_i ^r、y_i ^rは以下により求めることができる。

（５）式は、（４）式で得られたカメラ座標系での三次元点の座標Ｘ_i ^cを透視投影行列Ｐで２次元の座標に変換する（投影する）処理を表している。また、同次座標系で表されている。

次に、再投影誤差は、（５）式で求めた再投影点の座標、及び、実際にステレオカメラ５０で観測された特徴点の座標の２乗誤差の総和として、以下に示す（６）式で定義される。

上述した（６）式において、ｎはあるフレームで撮像された三次元座標が既知の特徴点の点数である。位置と姿勢の推定は、再投影誤差を最小化するＲとｔを求める問題であり、以下に示す（７）式のように定式化することができる。

この最小化問題は、ＰｎＰ問題として知られており、この式をそのまま解くと非線形最小化問題となる。ＰｎＰ問題は、ｎ点の対応から飛行物体１００の位置と姿勢を推定する問題である。

一般に、非線形最小化問題は算出コストが高く、また適切な初期値を設定しないと適切な解が求まらないという課題がある。非線形最小化問題を解くには、線形化する方法、Levenberg-Marquardt法など利用する方法が知られている。

また、ステレオカメラ５０が用いられているため、左右の画像を使用して、以下のように再投影誤差を最小化するＲとｔを求めてもよい。

（８）式は右カメラの画像と左カメラの画像のそれぞれで再投影誤差を求め、その合計が最も少なくなるＲとｔを求めることを示す。

以上のようにして回転行列Ｒ、平行移動ベクトルｔを求めることができれば、飛行物体１００の位置と姿勢を求めることができる。回転行列Ｒはワールド座標系に対する回転行列なので、ステレオカメラ５０と一体の飛行物体１００がワールド座標系に対しどのくらい傾いているかを示す情報となる。

（１２）式は、剛体がロール角φ、ピッチ角θ、ヨー角ψで回転する場合の回転行列を示す。r31の要素が-sinθであるためピッチ角θを求めることができる。ピッチ角θが分かるとr32又はr33からヨー角ψを求めることができる。同様に、r11又はr21からロール角φを求めることができる。したがって、飛行物体１００の姿勢を決定できる。

位置については、平行移動ベクトルｔがワールド座標系からの平行移動量を表すので、画像内にある特徴点のワールド座標系での三次元座標Ｘ_i ^wに平行移動ベクトルｔを適用することで、カメラと共に移動する飛行物体１００のワールド座標系における位置を決定できる。

なお、本実施形態では、再投影誤差が最小になる回転行列Ｒ、平行移動ベクトルｔを求めたが、オブジェクトスペースエラー（Object space error）が最小になる回転行列Ｒ、平行移動ベクトルｔを求めてもよい。オブジェクトスペースエラーは三次元座標の誤差である。この場合、移動体位置計測部１６はオブジェクトスペースエラーが最小になるように回転行列Ｒ、平行移動ベクトルｔを求める。すなわち、透視投影行列Ｐ、回転行列Ｒ及び平行移動ベクトルｔを画像の特徴点の座標に適用し三次元座標（変換座標）を求め、環境地図の三次元座標との差が最小になるように回転行列Ｒ、平行移動ベクトルｔを求める。この場合、線形に解が求まるため算出コストを低減できるという利点がある。

<<Ｓ６０環境地図生成処理>>
次に、図８を用いて、上述した環境地図生成部１５における環境地図生成処理について、フローチャートを用いて説明する。図８は、環境地図生成処理の一例示すフローチャートである。

図８において、環境地図生成部１５は、環境地図全体の再投影誤差の最小化処理を行う（Ｓ６０１）。この再投影誤差の最小化処理はバンドル調整ともいう。姿勢（Ｒｊ，ｔｊ）は１回の撮像で１つ得られる（ｊは画像の撮像順）。また、各撮像（画像）でｎ個の三次元点の座標Ｘ_iが得られている。したがって、ステップＳ６０１の処理では、例えば、過去の撮像で得られたＮｊ個の位置と姿勢（Ｒｊ，ｔｊ）と各撮像で得られたｎ個の三次元点の座標Ｘ_iをパラメータとして、再投影誤差が最小となる位置と姿勢を求める処理である。この処理は（１３）式により表すことができる。

（１３）式は、例えば非線形の最小二乗問題である。したがって、再投影誤差が最小となる位置と姿勢及び三次元点の座標Ｘ_iは、例えばＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法等を用いて求めることができるが、これに限定されるものではない。

（１３）式は、Ｎｊ個の撮像と各撮像で得られたｎ個の三次元点の座標Ｘ_iから再投影誤差が最も小さくなるように位置と姿勢（Ｒｊ，ｔｊ）と座標Ｘ_iを求めるので、（１３）式を最小化する姿勢（Ｒｊ，ｔｊ）と三次元点の座標Ｘ_iが分かると、環境地図全体の再投影誤差を最小化したことになる。

次に、環境地図生成部１５は、複数回の追跡に成功した特徴点を環境地図１００１に登録する（Ｓ６０２）。ステップＳ６０２の処理は、例えば三次元再構成処理と、信頼度計測処理とを有する。三次元再構成処理とは、例えば特徴点の三次元座標を求める処理である。三次元座標の算出は、図４と同様に求めることができる。具体的には以下の（１４）〜（１６）式で求める。カメラ座標系において、画像における特徴点の座標が分かっているので視差Ｚと焦点距離ｆから測距情報が得られる（（１６）式）。測距情報が得られると、ピンホールカメラモデルにより、カメラ座標系のＸ座標とＹ座標を求めることができる（（１４）（１５）式）。

（１４）〜（１６）式において、Ｚ、x_r、y_r、b、d、f、c_x、及び、c_yの意味は下記のようになる。
Ｚ：測距情報
x_r:特徴点の右カメラの画像上の観測座標[pixel]
y_r:特徴点の右カメラの画像上の観測座標[pixel]
b: ステレオカメラ５０のベースラインの距離[mm]
d: 視差[pixel]
f: 焦点距離 [mm]
c_x:画像のX方向の中心座標 [pixel]
c_y:画像のY方向の中心座標 [pixel]
なお、ステレオカメラ５０でなく単眼カメラである場合、２つの単眼カメラを用いてＤＬＴ（Direct Linear Transformation）法等を用いて三次元位置を求めることができる（ＤＬＴ法を用いた手法については非特許文献２を参照）。

次に、上述した信頼度計測処理とは、再投影誤差のフィッシャー（Fisher）情報量の逆数を信頼度として、これが閾値より高い三次元点を信頼できる点として環境地図１００１に登録する処理である。フィッシャー情報量は、確率変数がパラメータに関して持つ情報量のことである。なお、上述の閾値は、予め設定された値としてもよく、また環境地図１００１への追加候補点の全点のうち、上位から所定数の点を環境地図１００１に追加するようにしてもよい。上述したような追加する点の抽出条件は、処理毎に設定してもよい。つまり、Ｓ６０２の処理において、環境地図生成部１５は、三次元再構成処理に成功し、かつ信頼度計測により信頼度が高い点のみを環境地図１００１に登録する。

次に、環境地図生成部１５は、環境地図１００１にあるアウトライアの三次元点を削除する（Ｓ６０３）。アウトライアとは統計において他の値から大きく外れた値をいう。ステップＳ６０３の処理では、環境地図１００１にある全点について、上述した信頼度を算出し、算出した信頼度が閾値より小さい点を環境地図１００１から削除する。この閾値についても予め設定された値としてもよく、環境地図１００１にある三次元点の全点のうち、下位から所定数の点を環境地図１００１から削除してもよい。上述したような削除する点の抽出条件は、処理毎に設定してもよい。

<<Ｓ７０許容運動定義>>
次に、許容運動定義について説明する。許容運動定義とは、飛行物体１００の運動を表す複数のパラメータセットのそれぞれについて、その時点で、どのパラメータセットが許容されるかが記述された情報である。

図９は、許容運動定義部１７が許容運動を定義する手順を示すフローチャート図の一例である。図９の処理は、運動パラメータＩＤの全てについて繰り返し実行される。ここでは運動パラメータＩＤの数をｎ個（０〜ｎ−１）とする。運動パラメータＩＤは運動パラメータを区別するための識別情報である。識別情報とは、複数の対象からある特定の対象を一意的に区別するために用いられる名称、符号、文字列、数値又はこれらの組み合わせをいう。

図９の処理では、まず、許容運動定義部１７が許容運動判定を行う（Ｓ７０１）。許容運動判定とは、許容できる運動かどうかを判定することをいうが詳細は図１０にて説明される。

そして、許容できる運動である場合（Ｓ７０２のＹｅｓ）、ｉ番目の運動パラメータに関する運動を許容運動として登録する（Ｓ７０３）。

許容運動定義部１７が有限の数の運動パラメータから、特徴点を追跡可能な運動パラメータを選択するので、実用的な時間内に許容できる運動を決定できる。

図１０は、図９の許容運動判定の処理を説明するフローチャート図の一例である。許容運動判定の処理は、ある運動パラメータで飛行物体１００が運動した場合に、環境地図に含まれる三次元点のうち最新の画像で撮像された特徴点が、撮像中（露光中）の画像で観測可能かを判定する処理である。露光時間が長ければ撮像中の画像での観測は困難になり、フレーム間隔が長ければ特徴点が大きく移動するので撮像中の画像での観測は困難になる。したがって、撮像中（露光中）の画像において、環境地図に含まれる点のうち最新の画像で撮像された点が撮像中（露光中）の画像でどのくらい移動するかを予測することで、撮像中（露光中）の画像で撮像できるかどうかを判定可能となる。図１０では、一定数以上の特徴点が観測可能であると予想される運動パラメータ（運動）を許容運動であると判定する。

まず、許容運動定義部１７は、記憶部１０００から環境地図１００１を読み出す（Ｓ７７０１）。

また、許容運動定義部１７は、記憶部１０００からカメラパラメータ１００３を読み出す（Ｓ７７０２）。カメラパラメータ１００３には露光時間とフレーム間隔が含まれている。

次に、許容運動定義部１７は、最新の画像で撮像された特徴点かつ環境地図に含まれる特徴点の数をＭ個として、以下の処理を繰り返す。

まず、許容運動定義部１７は、露光中の特徴点の移動距離を予測する（Ｓ７７０３）。微小時間の間に飛行物体１００が、回転行列dRで表される回転を行い、平行移動ベクトルdtで表される移動を行う場合、画像上の移動量ｖは以下の（１７）式で求めることができる。

（１７）式の左辺は、微小時間が経過した後に露光中の特徴点が撮像される画像内の座標と、最新の画像で特徴点が撮像されている画像内の座標との差分を表している。（１７）式から、露光中の画像における移動量ｖを求めることができる。なお、移動距離は（１７）式のv_i ^xとv_i ^ｙの二乗和の平方根である。（１７）式では最新の画像で撮像されたカメラ座標系の特徴点の座標をＸ_i ^cとした。

回転行列dRは、最新の画像に撮像されている特徴点のワールド座標系の三次元座標をカメラ座標系に変換する回転行列Ｒの各要素の微小量である。

まず、回転行列dRと平行移動ベクトルdtは以下のように求める。移動体位置計測部１６は位置と姿勢を計測しているため、移動速度と各回転軸の回転速度を算出している。露光時間は既知なので、移動速度で飛行物体１００が露光時間、移動する際の平行移動ベクトルｄｔを算出できる。同様に、各回転軸の回転速度は、ヨー角ψ、ピッチ角θ、ロール角φの時間的変化なので、移動体位置計測部１６に取って既知である。各回転軸の回転速度に露光時間を乗じれば露光時間における回転量が分かる。

そして、許容運動定義部１７は、現在着目している運動パラメータで使用する運動を運動パラメータ使用運動１００６から読み出す。そして、使用しない運動に対応する回転行列ｄＲと平行移動ベクトルｄｔの要素をゼロに置き換える。例えば、上昇する場合は回転行列ｄＲの各要素をゼロにしてよい。また、移動を伴わない姿勢変化では、平行移動ベクトルｄｔの各要素をゼロにしてよい。また、Ｘ方向（のみ）への移動であれば、移動の原理に基づく回転角以外は変化しないので、回転行列ｄＲの要素のうち変化しない要素をゼロにしてよい。この場合、Ｙ座標とＺ座標は変化しないので、平行移動ベクトルｄｔうちＹ座標とＺ座標の変化分をゼロにしてよい。このようにして、運動パラメータに応じて回転行列ｄＲと平行移動ベクトルdtを算出できる。

許容運動定義部１７は、このようにして求めた回転行列ｄＲと平行移動ベクトルｄｔを（１７）式に設定することで、露光時間における飛行物体１００の画像上の移動量ｖを予測できる。

なお、回転行列ｄＲと平行移動ベクトルｄｔの一方だけと露光時間から飛行物体１００の画像上の移動量ｖを予測してもよい。

次に、許容運動定義部１７は、露光時間における移動距離が閾値Ｌ１〔ピクセル数〕より小さいか否かを判定する（Ｓ７０１４）。露光時間中の移動距離が閾値Ｌ１以上の場合、被写体ブレ（モーションブラー）が発生する可能性が高いと判定する。すなわち、観測可能な特徴点でないと判定する。理論上は、１ピクセル以上の動きがある場合は被写体ブレが発生する。このため、閾値Ｌ１は１ピクセル（画素と画素の１ピッチ）となり、露光時間中の移動距離が１ピクセル未満の場合はステップＳ７０１４の判定がＹｅｓとなる。若干の被写体ブレを許容して数ピクセルとしてもよい。

次に、運動制約定義１７部は、フレーム間隔における画像上の飛行物体１００の移動距離を予測する（Ｓ７０１５）。フレームとフレームの間における画像上の移動体の運動をベクトルで表すものをオプティカルフローと呼ぶ。本実施形態では、フレーム間のオプティカルフローは、ステップＳ７０１３と同様に求められる。すなわち、露光時間の代わりにフレーム間隔を用いればよい。

許容運動定義部１７は、このようにして求め得た回転行列ｄＲと平行移動ベクトルｄｔを（１７）式に設定することで、フレーム間隔における飛行物体１００の画像上の移動距離を予測できる。

次に、運動制約定義は、フレーム間の移動距離が閾値Ｌ２〔ピクセル数〕より小さいか否かを判定する（Ｓ７０１６）。閾値Ｌ２は、特徴点追跡アルゴリズムの性能の限界に考慮して予め定められている。よって、閾値Ｌ２は、特徴点の追跡アルゴリズムによって異なることが好ましい。例えば、特徴点追跡アルゴリズムが事前に探索範囲を指定するブロックマッチングである場合、探索範囲を指定する画素サイズが閾値Ｌ２となる。特徴点追跡アルゴリズムにPyramidal KLT （又はLucas-Kanade法など）が用いられた場合を説明する。特徴点追跡のPyramidal（ピラミッド）とは、元の画像から解像度の異なる複数の画像を作成してそれぞれから特徴点を探索することをいう。また、KLTは、微小時間においては平行移動のみしか生じず、特徴量の変化がないことを仮定して特徴点を追跡することをいう。Pyramidal KLT （又はLucas-Kanade法など）が用いられた場合、最高レベル（最も解像度が高い）のピラミッド画像においてKLTのコストが算出される局所ウインドウのサイズが閾値Ｌ２となる。

また、特徴量記述が用いられた手法の場合、画像のほぼ全体が探索範囲となるので、ステレオカメラ５０の撮像範囲を超えない範囲が閾値Ｌ２となる。こうすることで、特徴点の追跡の性能を超えない運動で飛行物体１００が移動することができる。

なお、フレーム間の移動距離が閾値Ｌ２より小さくても、次のフレームで特徴点が画像範囲外に移動してしまうおそれがある。このため、許容運動定義部１７は、最新の画像における特徴点の位置にオプティカルフローの移動ベクトルを加えた座標が、縦横ともに０より大きくかつ画像の幅と高さを超えない範囲に収まっているかを判定する。この範囲に収まっていない場合は、フレーム間の移動距離が閾値Ｌ２より小さくても観測可能な特徴点としては登録されない。

ステップＳ７０１６の判定がＹｅｓの場合、許容運動定義部１７は特徴点を可観測特徴点として登録する（Ｓ７０１７）。

Ｍ個の特徴点について可観測特徴点かどうかの判定が行われると処理はステップＳ７０１８に進む。

許容運動定義部１７は、可観測特徴点がＫ個以上あるか否かを判定する（Ｓ７０１８）。

ステップＳ７０１８の判定がＹｅｓの場合、許容運動定義部１７は、図９で着目しているｉ番目の運動パラメータが許容運動であると判定する（Ｓ７０１９）。許容運動定義部１７は、運動パラメータのうち許容運動であると判定された運動パラメータを許容運動マップに登録する。

＜移動体制御部３０の処理＞
次に、図１１を用いて移動体制御部３０の処理について説明する。図１１は、移動体制御部３０の動作手順を示すフローチャート図の一例である。移動体制御部３０は、Ｎ個の許容運動マップから最も経路コストが小さい運動を採用する。許容運動マップは、運動パラメータのうち許容運動として登録された運動パラメータである。すなわち、許容運動マップの運動パラメータの運動を飛行物体１００が行った場合、次の画像にＮ個以上の可観測特徴点が得られると判定されている。

移動体制御部３０は、各運動パラメータごとに予定移動先に到達するための経路コストを算出する（Ｓ１１０）。経路コストは時間、燃費、機体への負荷などである。本実施形態では説明の便宜上、経路コストが時間であるとして説明する。

図１２は、経路コストの算出方法を説明する図の一例である。例えば、飛行物体１００が移動する経路にＡ座標とＢ座標が登録されている場合、飛行物体１００はＡ座標に到達したが次に予定移動先であるＢ座標に移動する必要がある。ここで、Ａ座標からＢ座標に移動する運動パラメータが複数ある場合がある。通常は、直線で到達できる経路ｃを移動する運動パラメータを採用するが、本実施形態では、許容運動定義部１７が経路ｃを移動する運動パラメータを許容しないことができる。したがって、本実施形態の飛行物体１００は、位置の推定を継続することができる。

この場合、移動体制御部３０はＢ座標に移動するため経路ａや経路ｂを通過する別の運動パラメータで経路コストを算出する（Ｓ１１０）。

そして、許容されている全ての運動パラメータで経路コストを算出すると、移動体制御部３０は最も経路コストが低い運動パラメータを採用する（Ｓ１２０）。

したがって、許容運動定義部１７で定義された許容運動の運動パラメータのいずれかのうち、最も低い経路コストで移動する運動パラメータを採用できる。例えば、最も短い時間で移動する運動パラメータを採用できる。

なお、経路はＡ*アルゴリズムやダイクストラ法などを用いて自動生成してもよいし、ユーザが移動体に記録させたものでも良い。

以上説明したように、本実施形態の飛行物体１００は、画像の撮像条件に基づいて、被写体ブレが起こらず、かつ、フレーム間で追跡が可能な特徴点が閾値以上の画像が撮像される場合に、着目している運動パラメータの運動が可能であると判定する。したがって、本実施形態の飛行物体１００は位置と姿勢を継続して検出しやすい。また、位置と姿勢を継続して検出しやすい運動パラメータのうち、最も低い経路コストで移動する運動パラメータを採用するので、移動に必要な時間の増大を最小限に抑制できる。

＜その他の適用例＞
以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。

本実施形態では移動体として飛行物体１００を例に説明したが、車両、移動式ロボット、船、潜水艦等の各種移動体で適用することができる。また、移動体に搭載されたスマートフォンなどの情報処理装置に適用することができる。

また、図１０などの構成例は、飛行物体１００による処理の理解を容易にするために、主な機能に応じて分割したものである。処理単位の分割の仕方や名称によって本願発明が制限されることはない。飛行物体１００の処理は、処理内容に応じて更に多くの処理単位に分割することもできる。また、１つの処理単位が更に多くの処理を含むように分割することもできる。

また、上述した各実施形態で述べたフローチャートは、矛盾の無い限り順序を入れ替えてもよい。

なお、ステレオカメラ５０は撮像装置の一例であり、移動体位置計測部１６は位置検出部の一例であり、カメラパラメータは撮像条件の一例である。位置計測部１０は位置推定装置の一例であり、位置計測部１０が行う処理や方法は位置推定方法の一例である。

１０位置計測部
１１画像特徴点抽出部
１２画像特徴点追跡部
１３深度計測部
１４撮像部制御部
１５環境地図生成部
１６移動体位置計測部
１７許容運動定義部
３０移動体制御部
５０ステレオカメラ
１００飛行物体

特開２０１２−１５０６５５号公報

Computer Vision and Pattern Recognition, 2005. CVPR 2005. IEEE Computer So-ciety Conference on, Vol. 2, pp.807 - 814, 2005.。佐藤智和、"複数の動画像を用いたカメラパラメータ推定に基づく屋外環境の三次元モデル化に関する研究"、奈良先端科学技術大学院大学、博士論文、2003年3月24日。

Claims

被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する位置推定装置であって、
前記撮像装置が撮像した前記画像から特徴点を抽出する画像特徴点抽出部と、
前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡する画像特徴点追跡部と、
前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成する環境地図生成部と、
前記特徴点の前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記特徴点の前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出する位置検出部と、
前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、前記特徴点を追跡可能な前記画像を撮像できる前記移動体の運動を定義する許容運動定義部と、を有する位置推定装置。
前記撮像条件は露光時間に関する情報を含み、
前記許容運動定義部は、前記移動体が前記移動速度及び前記回転速度で運動し、前記撮像装置が前記露光時間で前記画像を撮像した場合に、前記特徴点を追跡可能な前記運動を定義する請求項１に記載の位置推定装置。
前記許容運動定義部は、前記移動体が前記移動速度及び前記回転速度で運動し、前記撮像装置が前記露光時間で前記画像を撮像した場合に、前記画像における前記特徴点の移動量が１ピクセル未満である前記運動を定義する請求項２に記載の位置推定装置。
前記撮像条件は撮像間隔に関する情報を含み、
前記許容運動定義部は、前記移動体が前記移動速度及び前記回転速度で運動し、前記撮像装置が前記撮像間隔で次の前記画像を撮像した場合に、前記特徴点を追跡可能な前記運動を定義する請求項１〜３のいずれか１項に記載の位置推定装置。
前記許容運動定義部は、前記撮像間隔における前記特徴点の移動ベクトルを算出し、
前記移動ベクトルの長さが閾値よりも小さい場合に、前記特徴点を追跡可能であると判定する請求項４に記載の位置推定装置。
前記画像特徴点追跡部は、事前に設定された探索範囲の中でのみ前記特徴点の探索を行い、
前記許容運動定義部は前記移動ベクトルで移動した前記特徴点が前記探索範囲に含まれる場合、前記特徴点を追跡可能であると判定する請求項５に記載の位置推定装置。
前記画像特徴点追跡部は、Pyramidal KLTを用いて前記特徴点の追跡を行い、
前記許容運動定義部は、前記移動ベクトルで移動した前記特徴点がPyramidal KLTで利用する最高レベルのピラミッド画像におけるKLTの局所ウインドウ内にある場合、前記特徴点を追跡可能であると判定する請求項６に記載の位置推定装置。
前記許容運動定義部は、前記移動体が採用することが可能な運動を規定する複数の運動パラメータ及び前記運動パラメータに対応付けられた変化しうる自由度を記憶部から読み出し、
前記許容運動定義部は、前記運動パラメータごとに前記自由度を参照して、前記運動パラメータが規定する運動が、前記特徴点を追跡可能な前記画像を撮像できる運動であるかどうかを判定する請求項１〜７のいずれか１項に記載の位置推定装置。
被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する情報処理装置を、
前記撮像装置が撮像した前記画像から特徴点を抽出する画像特徴点抽出部と、
前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡する画像特徴点追跡部と、
前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成する環境地図生成部と、
前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出する位置検出部と、
前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、前記特徴点を追跡可能な前記画像を撮像できる運動を定義する許容運動定義部、として機能させるためのプログラム。
被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する位置推定装置の位置推定方法であって、
画像特徴点抽出部が、前記撮像装置が撮像した前記画像から特徴点を抽出するステップと、
画像特徴点追跡部が、前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡するステップと、
環境地図生成部が、前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成するステップと、
位置検出部が、前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出するステップと、
前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、許容運動定義部が、前記特徴点を追跡可能な前記画像を撮像できる運動を定義するステップと、を有する位置推定方法。