(57)【要約】【課題】  移動体が使用される環境によらず、移動体とカメラの相対的な位置ずれを検出できる装置を提供する。【解決手段】  撮像装置は、移動体１０に搭載される装置であって、移動体１０の少なくとも一部が画角内に入るステレオカメラ（カメラ）と、ステレオカメラの撮影画像に基づいて、移動体１０とステレオカメラの相対的な位置ずれ（カメラ機体相対位置ずれ）を検出する処理装置３０（検出手段）と、を備えている。この場合、移動体が使用される環境によらず、移動体とカメラの相対的な位置ずれを検出可能な撮像装置を提供できる。【選択図】図６
2018004420.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載される装置であって、
前記移動体の少なくとも一部が画角内に入る撮像系と、
前記撮像系の撮影画像に基づいて、前記移動体と前記撮像系の相対的な位置ずれを検出する検出手段と、を備える装置。
【請求項２】
前記撮像系は、ジンバルを介して前記移動体に搭載されていることを特徴とする請求項１に記載の装置。
【請求項３】
前記少なくとも一部には、少なくとも１つのマークが付され、
前記検出手段は、前記撮像系の撮影画像と予め取得された前記マークが写る基準画像に基づいて、前記位置ずれを検出することを特徴とする請求項１又は２に記載の装置。
【請求項４】
前記基準画像の前記少なくとも一部が写る領域にはマスクが施されることを特徴とする請求項３に記載の装置。
【請求項５】
前記検出手段は、前記撮像系の撮影画像における前記少なくとも一部の画像と、予め取得された前記少なくとも一部の基準輝度画像のピクセル値の差分の総和を用いて、前記位置ずれを検出する特徴とする請求項１又は２に記載の装置。
【請求項６】
前記撮像系は、複数の撮像部を有し、
前記検出手段は、前記複数の撮像部の撮影画像から得られた前記少なくとも一部の視差画像と、予め取得された前記少なくとも一部の基準視差画像の視差値の差分の総和を用いて、前記位置ずれを検出することを特徴とする請求項１又は２に記載の装置。
【請求項７】
請求項１〜６のいずれか一項に記載の装置と、
前記装置が搭載される移動体と、を備える移動体装置。
【請求項８】
前記装置の検出手段の検出結果に基づいて、前記移動体の移動許否を決定する制御手段を更に備えることを特徴とする請求項７に記載の移動体装置。
【請求項９】
前記制御手段は、前記装置への電源投入時又は前記移動体の移動開始時に前記移動許否の決定を行うことを特徴とする請求項８に記載の移動体装置。
【請求項１０】
前記撮像系は、
複数の撮像部と、
前記複数の撮像部の撮影画像から視差画像を生成し、該視差画像に基づいて、前記撮影画像に写る物体までの距離情報を算出する演算手段を更に備えることを特徴とする請求項７〜９のいずれか一項に記載の移動体装置。
【請求項１１】
前記装置の検出手段の検出結果に基づいて、前記演算手段の算出結果を補正する補正手段を更に備えることを特徴とする請求項１０に記載の移動体装置。
【請求項１２】
前記移動体は、無人航空機であることを特徴とする請求項７〜１１のいずれか一項に記載の移動体装置。
【請求項１３】
移動体に搭載される、撮像部を含む装置と前記移動体の相対的な位置ずれを検出する位置ずれ検出方法であって、
前記移動体の少なくとも一部が前記撮像部の画角内に入るように前記装置を前記移動体に搭載する工程と、
前記撮像部の撮影画像に基づいて、前記移動体に対する前記装置の位置ずれを検出する工程と、を含む位置ずれ検出方法。
【請求項１４】
移動体に搭載される、複数の撮像部を含む装置を用いる測距方法であって、
前記移動体の少なくとも一部が前記複数の撮像部の画角内に入るように前記装置を前記移動体に搭載する工程と、
前記複数の撮像部の撮影画像から視差画像を生成し、前記撮影画像に写る物体までの距離情報を算出する工程と、
前記複数の撮像部の撮影画像に基づいて、前記移動体と前記装置の相対的な位置ずれ情報を検出する工程と、を含む測距方法。
【請求項１５】
前記位置ずれ情報に基づいて前記距離情報を補正する工程を更に含む請求項１４に記載の測距方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、装置、移動体装置、位置ずれ検出方法及び測距方法に関する。
【背景技術】
【０００２】
近年、移動体に搭載される、撮像系を含む装置の開発が盛んに行われている。
【０００３】
例えば、特許文献１に開示されている装置では、撮像系が道路上の路面表示を撮像して得られた撮像情報から特徴点又は特徴線を抽出し、路面表示の特徴量から移動体と撮像系の相対的な位置ずれを求めている。
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１に開示されている装置等の従来の装置では、移動体が使用される環境によっては（例えば屋内では）、移動体と撮像系の相対的な位置ずれを検出することが困難であった。
【課題を解決するための手段】
【０００５】
本発明は、移動体に搭載される装置であって、前記移動体の少なくとも一部が画角内に入る撮像系と、前記撮像系の撮影画像に基づいて、前記移動体と前記撮像系の相対的な位置ずれを検出する検出手段と、を備える装置である。
【発明の効果】
【０００６】
本発明によれば、移動体が使用される環境によらず、移動体と撮像系の相対的な位置ずれを検出することができる。
【図面の簡単な説明】
【０００７】
【図１】本発明の一実施形態の移動体装置の外観を概略的に示す図である。
【図２】移動体装置の構成の一例を示すブロック図である。
【図３】図２の撮像手段のハードウェア構成の一例を示すブロック図である。
【図４】図２の制御部のハードウェア構成の一例を示すブロック図である。
【図５】図２の移動体のハードウェア構成の一例を示すブロック図である。
【図６】図２の処理装置の構成の一例を示すブロック図である。
【図７】カメラ位置姿勢計測処理と機体位置姿勢計測処理について説明するための図である。
【図８】環境地図生成処理について説明するためのフローチャートである。
【図９】カメラ機体相対位置ずれ検出処理について説明するためのフローチャートである。
【図１０】撮像系で撮影した画像（基準撮影画像）について説明するための図である。
【図１１】カメラ機体相対位置ずれ計測用の基準マスク画像について説明するための図である。
【図１２】カメラ機体相対位置ずれ計測用のマークについて説明するための図である。
【図１３】カメラ機体相対位置ずれ計測処理１について説明するためのフローチャートである。
【図１４】カメラ機体相対位置ずれ計測処理２について説明するためのフローチャートである。
【図１５】カメラ機体相対位置ずれ計測処理３について説明するためのフローチャートである。
【図１６】図１６（ａ）及び図１６（ｂ）は、それぞれ移動体とステレオカメラの相対位置と、撮像系で得られる距離画像の関係を示す図（その１及びその２）である。
【図１７】測距処理を説明するためのフローチャートである。
【図１８】変形例の移動体装置を説明するための図である。
【図１９】図１９（ａ）〜図１９（ｄ）は、それぞれ変形例の移動体装置による作用、効果を説明するための図（その１〜その４）である。
【発明を実施するための形態】
【０００８】
［導入］
本発明の実施形態は、一例として自立して移動することが可能な移動体と該移動体に搭載される撮像装置とを備える移動体装置に関する。ここでは、移動体として飛行体（航空機）を例にとって説明をする。
【０００９】
ところで、カメラで撮影された一連の静止画像や動画像から得られる移動情報を用いて自動飛行を行う飛行体において、カメラの持つカメラ座標系と飛行体が持つ飛行体座標系の相対的な位置姿勢を予め求めておき、該位置姿勢を用いてカメラの移動情報を飛行体の移動情報に変換し、変換後の移動情報を用いて自動的に移動（自立移動）する飛行体が知られている。
【００１０】
しかし、カメラ座標系と飛行体座標系の相対的な位置姿勢が、カメラのレンズなどの部品の経年劣化や外的な衝撃によるカメラ筐体の変形やカメラ筐体を飛行体に取り付ける部材の変形などにより、予め求められた相対的な位置姿勢からずれた場合、カメラの移動情報を飛行体の移動情報に変換した結果が真値からずれてしまう。
【００１１】
この場合、飛行体を安全に飛行させるのが困難となる。
【００１２】
そこで、例えば特許文献１では、校正用のパターンが設置された環境で移動体が使用される場合に移動体とカメラの相対的な位置や姿勢のずれを計測する技術が提案されている。
【００１３】
しかし、特許文献１等の従来技術では、移動体が使用される環境が校正用のパターンが設置された環境に制限される課題があった。
【００１４】
そこで、発明者は、この課題を解決すべく、鋭意検討の末、以下の実施形態を発案した。
【００１５】
以下の実施形態では、移動体が、自律飛行する無人航空機（例えば、ＤｒｏｎｅやＵＡＶ（Unmanned  aerial  vehicle））であり、移動体が自己位置を推定し、自己位置推定結果に基づいて自律移動を制御する移動体装置を例にとって説明する。
【００１６】
以下に、本発明の一実施形態を図面を用いて詳細に説明する。
［基本構成］
図１は、第１実施形態の移動体装置１の一例を示す模式図である。図１に示されるように、移動体装置１は、移動体１０、撮像系２０、処理装置３０を備えている。ここでは、移動体１０は、無人航空機である。
【００１７】
以下、移動体１０の左右方向をＸ軸方向、前後方向をＹ軸方向、高さ方向をＺ軸方向とする移動体に設定されたＸＹＺ３次元直交座標系（以下では「移動体座標系」とも呼ぶ）を適宜用いて説明する。ここでは、移動体座標系において移動体１０の前方向は＋Ｙ方向である。
【００１８】
移動体１０は、機体１１と、該機体１１の前後左右にそれぞれアームを介して設けられた４つのプロペラ１２〜１５と、を含む。移動体１０は、マルチコプターとも呼ばれる。
【００１９】
移動体１０は、プロペラ１２〜１５が回転駆動することにより飛行するように構成されている。
【００２０】
機体１１は、円板状の胴部１５と、該胴部１５から−Ｚ側に延在するＴ字状の２つの脚部１６、１７とを有する。
【００２１】
胴部１５には、撮像系２０が支持部材１８、１９を介して宙吊り状態で取り付けられている。
【００２２】
２つの脚部１６、１７は移動体１０が地面に着地したときに転倒しないように支えると共に撮像系２０が地面に接触しないように保護する役割も担っている。
【００２３】
図２は、移動体装置１の構成の一例を示すブロック図である。図２に示されるように、撮像系２０は、撮像手段３００と、制御部４００と、を備える。
【００２４】
撮像手段３００は、ステレオカメラ２１による測距に係る構成であり、ステレオカメラ２１と、その静止画像や動画像を処理する処理回路（論理回路）とを有する。ステレオカメラ２１は、光軸が互いに平行となるように配置された左右のカメラ（以下では「左右眼」とも呼ぶ）を有する。
【００２５】
制御部４００は、撮像手段３００の制御、並びに移動体１０、処理装置３０との通信に係る構成である。
【００２６】
図３は、撮像手段３００のハードウェア構成の一例を示すブロック図である。撮像光学系１２１Ａを通過した被写体光は、ＣＭＯＳ３１１Ａに結像され、撮像光学系１２１Ｂを通過した被写体光は、ＣＭＯＳ３１１Ｂに結像される。ＣＭＯＳ３１１Ａは、撮像面に結像された光学像を電気信号に変換し、アナログの画像データとして出力し、ＣＭＯＳ３１１Ｂは、撮像面に結像された光学像を電気信号に変換し、アナログの画像データとして出力する。なお、撮像素子としてＣＭＯＳ３１１Ａ及びＣＭＯＳ３１１Ｂの代わりにＣＣＤ（Charge  Coupled  Device）を採用してもよい。
【００２７】
ＣＭＯＳ３１１Ａから出力された画像データは、ＣＤＳ（Correlated  Double  Sampling：相関２重サンプリング）回路３１２Ａによりノイズ成分を除去され、Ａ／Ｄ変換器３１３Ａによりデジタル値に変換され、画像処理回路３１４Ａに出力される。ＣＭＯＳ３１１Ｂから出力された画像データは、ＣＤＳ回路３１２Ｂによりノイズ成分を除去され、Ａ／Ｄ変換器３１３Ｂによりデジタル値に変換され、画像処理回路３１４Ｂに出力される。
【００２８】
なお、ＣＭＯＳ３１１Ａ、３１１Ｂ、ＣＤＳ回路３１２Ａ、３１２Ｂ、及びＡ／Ｄ変換器３１３Ａ、３１３Ｂは、タイミング信号発生器３１６により、動作するタイミングが制御されている。画像処理回路３１４Ａ、３１４Ｂは、それぞれ、画像データを一時格納するＳＤＲＡＭ（Synchronous  Dynamic  Random  Access  Memory）３１７を用いて、ＲＧＢ→ＹＣｒＣｂ変換処理、ホワイトバランス制御処理、コントラスト補正処理、エッジ強調処理、及び色変換処理などの各種画像処理を行う。なお、ホワイトバランス制御処理は、画像情報の色濃さを調整する画像処理であり、コントラスト補正処理は、画像情報のコントラストを調整する画像処理であり、エッジ強調処理は、画像情報のシャープネスを調整する画像処理であり、色変換処理は、画像情報の色合いを調整する画像処理である。
【００２９】
また、ＳＤＲＡＭ３１７に記憶された画像処理が施された画像データは、画像圧縮伸張回路３１８により圧縮され、メモリカード３１９に記録される。画像圧縮伸張回路３１８は、画像処理回路３１４Ａ、３１４Ｂから出力される画像データを圧縮してメモリカード３１９に出力するとともに、メモリカード３１９から読み出した画像データを伸張して画像処理回路３１４Ａ、３１４Ｂに出力する回路である。
【００３０】
タイミング信号発生器３１６、画像処理回路３１４Ａ、３１４Ｂ、画像圧縮伸張回路３１８、及びメモリカード３１９は、ＣＰＵ（Central  Processing  Unit）３１５により制御されている。ＲＯＭ（Read  Only  Memory）３２１は、ＣＰＵ３１５により実行されるプログラムなどを格納した読み出し専用メモリであり、ＲＡＭ（Random  Access Memory）３２２は、ＣＰＵ３１５が各種の処理過程で利用するワークエリア、及び各種データ格納エリアなどを有する読み出し書き込み自在のメモリである。
【００３１】
ＣＰＵ３１５は、画像処理回路３１４Ａにより各種画像処理が施された画像データ、及び画像処理回路３１４Ｂにより各種画像処理が施された画像データ、即ち、ステレオ画像を用いた視差演算を行って視差画像を生成し、生成した視差画像に基づいて、視差画像に写る物体までの距離を求める。なお、視差は、画素毎又は画素ブロック毎に算出され、視差画像では、画素又は画素ブロック毎に視差情報が対応づけられる。画素ブロックは、１以上の画素で構成されるブロックである。これにより、視差画像は、視差画像に写る物体（対象物）の３次元空間上での３次元位置（３次元座標）を表すことができる。撮像手段３００の主要な構成は、バスライン１９によって相互接続されている。
【００３２】
図４は、制御部４００のハードウェア構成の一例を示すブロック図である。制御部４００は、ＣＰＵ４１１、ＲＡＭ４１２、フラッシュＲＯＭ４１３、Ｉ／Ｏ４１４、及びネットワークＩ／Ｆ４１５を備える。
【００３３】
ＣＰＵ４１１は、フラッシュＲＯＭ４１３に記憶されたプログラム４２０を実行して制御部４００の全体の動作を制御する。プログラム４２０は、メモリカード４１６に記憶された状態で配布されてもよいし、サーバから携帯電話網や無線ＬＡＮ（Local  Area  Network）網などの通信網を経由してダウンロードされることで配布されてもよい。ＲＡＭ４１２は、ＣＰＵ４１１がプログラムを実行する際のワークエリア（プログラムやデータが一時的に記憶される）として使用される。
【００３４】
ＣＰＵ４１１は、撮像手段３００の出力に基づいて、自己位置推定を行ったり、撮像手段３００に再測距を行わせたりする。Ｉ／Ｏ４１４は、Ｉ２ＣやＵＡＲＴなどの入出力インターフェースである。ネットワークＩ／Ｆ４１５は、イーサネット（登録商標）などの車載ネットワーク上で通信するための通信装置である。
【００３５】
図５は、移動体１０のハードウェア構成の一例を示すブロック図である。移動体１０は、主に、制御装置５１９、ＧＰＳアンテナ５１２、ＧＰＳ受信器５２５、及びプロペラモータ５４５Ａ〜５４５Ｄから構成される。
【００３６】
ＧＰＳアンテナ５１２及びＧＰＳ受信器５２５は、移動体１０の絶対座標を測定する。なお、ＧＰＳアンテナ５１２及びＧＰＳ受信器５２５としては、後処理キネマティックＧＰＳ又はリアルタイムキネマティックＧＰＳ（ＲＴＫ−ＧＰＳ）が用いられるのが好ましい。ＲＴＫ−ＧＰＳは、高精度の測定が可能である。
【００３７】
プロペラモータ５４５Ａ〜５４５Ｄは、それぞれ、プロペラ１２〜１５を回転駆動するためのモータである。
【００３８】
制御装置５１９は、主に、制御演算部５２０、記憶部５２２、撮像制御部５２３、飛行制御部５２６、方位センサ５２７、ジャイロユニット５２８、通信部５２９を、備える。
【００３９】
記憶部５２２は、例えば、ＲＡＭやＲＯＭであり、プログラム格納部とデータ格納部とを含む。プログラム格納部には、ステレオカメラ２１の撮像を制御する為の撮像プログラム、自律飛行する為の飛行制御プログラム、ステレオカメラ２１の姿勢を制御する為の姿勢制御プログラム、及び飛行計画プログラムなどのプログラムが格納されている。データ格納部には、ステレオカメラ２１で撮像した撮像データ、並びにＧＰＳアンテナ５１２及びＧＰＳ受信器５２５によって取得した絶対座標（地表座標）などのデータが格納される。
【００４０】
制御演算部５２０は、記憶部５２２に格納されたプログラムに基づいて、移動体１０の全体の動作を制御する。
【００４１】
ここで、ステレオカメラ２１は、静止画及び動画を撮像可能であり、取得した画像をデジタル信号の画像データとして撮像制御部５２３に出力可能である。
【００４２】
撮像制御部５２３は、ステレオカメラ２１による静止画や動画の撮像に関する制御を行う。撮像制御部５２３には、撮像手段３００から画素又は画素ブロック毎の距離情報、すなわち視差画像から得られた距離画像が送られる。
【００４３】
方位センサ５２７は、移動体１０の向きを検出し、その検出結果を制御演算部５２０に出力する。
【００４４】
ジャイロユニット５２８は、移動体１０の飛行状態での姿勢を検出し、その検出結果を制御演算部５２０に出力する。
【００４５】
飛行制御部５２６は、撮像系２０の制御部４００により推定された自己位置、方位センサ５２７により検出された移動体１０の向き、ジャイロユニット５２８により検出された移動体１０の姿勢等に基づいて、プロペラモータ５４５Ａ〜５４５Ｄの駆動を制御することにより、移動体１０の自律飛行を制御する。
【００４６】
なお、飛行制御部５２６は、ＧＰＳアンテナ５１２及びＧＰＳ受信器５２５を含むＧＰＳ装置によって取得した絶対座標と制御演算部５２０からの上記演算結果を用いて、移動体１０の自律飛行を制御しても良い。
【００４７】
但し、ＧＰＳ装置は絶対位置を求めることができるものの、環境によって結果が不安定になったり、観測不能になったりすることがある。そのため、方位センサ２７、ジャイロユニット５２８等が配備されるメリットは大きい。
【００４８】
通信部５２９は、撮像系２０と通信して、撮像系２０により推定された自己位置を示す自己位置情報を取得したり、ステレオカメラ２１で撮像した撮像データやＧＰＳアンテナ５１２及びＧＰＳ受信器５２５により測定された移動体１０の絶対座標を基地局などに無線送信したりする等の機能を有する。また、通信部５２９は、処理装置３０とも通信を行う。その詳細は後述する。
【００４９】
図６には、処理装置３０の構成の一例がブロック図で示されている。
【００５０】
処理装置３０は、移動計測を行うための種々のソフトウェアで動作するＣＰＵ３１と、撮像系２０で撮影された画像を処理する画像処理装置３２と、ＤＲＡＭ３３と、通信部３４とを含む。
【００５１】
画像処理装置３２は、視差計算処理部３２ａ、画像特徴点抽出処理部３２ｂ、画像特徴点追跡処理部３２ｃ、画像リマップ処理部３２ｄを含む。
【００５２】
画像リマップ処理部３２ｄは、視差計算の前段処理として、撮像系２０で撮影された画像に対して画像リマップ処理、すなわち平行化処理と魚眼カメラ画像をピンホールカメラモデルで投影した画像へ変換する処理を行う。
【００５３】
画像リマップ処理された画像は、画像特徴点抽出処理部３２ｂ、画像特徴点追跡処理部３２ｃ、視差計算処理部３２ａに入力される。
【００５４】
画像特徴点抽出処理部３２ｂは、画像リマップ処理された画像から特徴点の抽出を行う。
【００５５】
画像特徴点追跡処理部３２ｃは、画像特徴点抽出処理部３２ｂによって過去に抽出された画像特徴点を画像リマップ処理された画像において探索する。
【００５６】
画像特徴点抽出処理部３２ｂは、Harrisオペレータ、FASTオペレータ、SIFT特徴量記述子、SURF特徴量記述子などの公知の技術を利用することができる。
【００５７】
画像特徴点追跡処理部３２ｃは、ブロックマッチングやLucas-KanadeTrackerを用いた対応点探索や、上記特徴量を用いた特徴点の内応付などの公知の技術を利用して実現することができる。
【００５８】
視差計算処理部３２ａは、ステレオ測距法により、ステレオカメラ２１の左右のカメラ（左右眼）の視差を元に各画素の距離を計測する。
【００５９】
視差計算処理部３２ａは、Semi-Global-Mating法などの既知の手法を利用し実現できる。(非特許文献 Hirschmuller, Heiko:”Accurate and efficient stereo processing by semi-global matching and mutual information”, Computer Vision and Pattern Recognition, 2005. CVPR 2005. IEEE Computer So-ciety Conference on, Vol. 2, pp.807 - 814, 2005.。
【００６０】
画像特徴点抽出処理部３２ｂ、画像特徴点追跡処理部３２ｃ及び視差計算処理部３２ａの処理結果はＤＲＡＭ３３に画像特徴点情報３３ｂとして記録される。画像特徴点情報３３ｂには、特徴点の左右眼の画像（ステレオ画像）上の座標とその視差値が含まれる。
【００６１】
ＤＲＡＭ３３には、環境地図生成プログラム３３ａ、画像特徴点情報３３ｂ、カメラ機体相対位置基準データ３３ｃ、カメラ機体相対位置データ３３ｄ、カメラパラメータ３３ｅ、機体移動情報３３ｆ、カメラ機体相対位置ずれ検出プログラム３３ｇ、自己位置計測プログラム３３ｈ、カメラ移動機体移動変換プログラム３３ｊ、カメラ位置姿勢計測プログラム３３ｋ、環境地図３３ｌ、及びカメラ機体相対位置ずれ情報計測プログラム３３ｍ（カメラ機体相対位置ずれ検出プログラム３３ｇの一部）が格納されている。
【００６２】
ここで、本明細書において「ステレオカメラと機体の相対的な位置（相対位置）」は、ステレオカメラと機体の並進方向の相対的な位置、及びステレオカメラと機体の回転方向の相対的な位置を含む。また、本明細書において「ステレオカメラと機体の相対的な位置ずれ（相対位置ずれ）」は、ステレオカメラと機体の並進方向の相対的な位置ずれである並進位置ずれ、及びステレオカメラと機体の回転方向の相対的な位置ずれである回転位置ずれ（姿勢ずれ）を含む。
【００６３】
カメラ位置姿勢計測プログラム３３ｋは、ＤＲＡＭ３３に記録された環境地図３３ｌ、画像特徴点情報３３ｂ及びカメラパラメータ３３ｅから、ステレオカメラ２１の位置及び姿勢であるカメラ位置姿勢を計測する処理を行い、ステレオカメラ２１の位置姿勢情報であるカメラ位置姿勢情報を出力する。このカメラ位置姿勢計測プログラム３３ｋの処理内容（カメラ位置姿勢計測処理）は後述する。
【００６４】
環境地図生成プログラム３３ａは、画像特徴点情報３３ｂと、深度計測部の自己位置姿勢情報から、周囲の環境の３次元地図を生成する。
【００６５】
カメラ移動機体移動変換プログラム３３ｊは、ＤＲＡＭ３３に記録されているカメラ機体相対位置データ３３ｄとカメラ位置姿勢計測プログラム３３ｋで算出されたカメラ位置姿勢情報を、機体座標系での移動に変換する。この処理は後述する。
【００６６】
カメラ機体相対位置ずれ検出プログラム３３ｇは、ステレオカメラ２１と機体１１の相対位置姿勢を、カメラ機体相対位置基準データ３３ｃと撮像系２０の撮影画像を元に観測し、該相対位置姿勢が予め取得、保持されているカメラ機体相対位置データ３３ｄからずれていると判定した場合には、通信部３４を通じて移動体１０の制御装置５１９へ通知する。
【００６７】
そこで、撮像系２０及び処理装置３０を含んで、本発明の装置の一例である「撮像装置１００」が構成されている。
【００６８】
処理装置３０の通信部３４は、機体移動情報３３ｆを、制御装置５１９の通信部５２９を介して飛行制御部５２６に送る。飛行制御部５２６は機体移動情報３３ｆに基づいてプロペラ１２〜１５をそれぞれ駆動する４つのモータの駆動パラメータを生成し、各モータを対応する駆動パラメータに従って駆動する。飛行制御部５２６は、移動体１０のＧＰＳアンテナ５１２及びＧＰＳ受信器５２５を含むＧＰＳ装置から移動体１０の絶対位置情報を取得する。
【００６９】
次に、図７を用いて、カメラ位置姿勢計測処理と機体位置姿勢計測処理について説明する。なお、図７の機体座標系は、便宜上、図１の移動体座標系と異なる表記としている。
【００７０】
《カメラ位置姿勢計測処理》
先ず、カメラ位置姿勢計測処理について説明する。カメラ位置姿勢計測処理は、ＤＲＡＭ３３に格納されたカメラ位置姿勢計測プログラム３３ｋに従ってＣＰＵ３１により実行される。
【００７１】
カメラ位置姿勢計測処理は、以下のパラメータを求める処理である。
・カメラの座標系とワールド座標系とを変換する回転行列Ｒ
・カメラの座標系とワールド座標系とを変換する平行移動ベクトルｔ
【数１】
2018004420.tif 000003
【００７２】
また、これらを求めるのに、以下のパラメータを利用する。すなわち、画像内にある特徴点のワールド座標系での３次元座標ｘｉＷ、カメラの内部パラメータＰ、Ｘ方向の焦点距離ｆｘ、Ｙ方向の焦点距離ｆｙ、Ｘ方向の画像中心Ｏｘ、Ｙ方向の画像中心Ｏｙとしたときに、次の（２）式、（３）式で表されるパラメータを利用する。
【数２】
2018004420.tif 000004
【００７３】
特徴点の３次元座標については、前述の環境地図生成処理で予め求めておく。また、カメラの内部パラメータについてはカメラキャリブレーションによって予め求めておく。カメラキャリブレーションについては、Zhangの方法などの既知の手法が利用できる(A flexible new technique for camera calibration)。
【００７４】
また、これ以外にカメラの歪補正も行う必要があるが、歪のパラメータについても上記Zhangの手法で得ることができる。歪の補正についても、特徴点抽出を行う前に画像自体の歪補正を行うか、または、特徴点抽出した点の座標について歪のパラメータを用いてリマップしているかのどちらかを行えば、この位置計測の方式が利用できる。
【００７５】
カメラ位置姿勢計測処理では、これらのパラメータを利用して、再投影誤差を最小化にするＲ、ｔを求める。
【００７６】
再投影誤差は、画像特徴点の３次元点を画像上に投影した点の座標と、実際にそれが観測された座標の差分により定義される。
【００７７】
再投影座標は、以下の手順により行うことができる。
・次元点のカメラ座標系への変換
・想スクリーンへの投影
【００７８】
３次元点のカメラ座標系への変換は、次の（４）式で求めることができる。
【数３】
2018004420.tif 000005
【００７９】
ここで、カメラ座標系での３次元点の座標をｘｉｃ、ワールド座標系での３次元点の座標をｘｉＷ、として定義している。このようにして、カメラ座標系での３次元点の座標を得ることができる。
【００８０】
仮想スクリーンへの次元点の投影を再投影と呼び、次の（５）式で行うことができる。
【数４】
2018004420.tif 000006
【００８１】
ここで、再投影点の座標をｘｉｒ、ｙｉｒとする。
【００８２】
この再投影誤差は、最投影点の座標及び、観測座標の２乗誤差の総和として次の（６）式で定義される。
【数５】
2018004420.tif 000007
【００８３】
ここで、nはあるフレームで撮影された、３次元座標が既知の特徴点の点数である。
ポーズ推定は、この再投影誤差を最小化するRとtを求める問題であり、次の（７）式のように定式化できる。
【数６】
2018004420.tif 000008
【００８４】
上記の最小化問題はPnP問題として知られており、この式をそのまま解くと非線形最小化問題となり、非線形最小化問題を解くLevenberg-Marquardt法などを利用すれば解くことができる。
【００８５】
また、ステレオカメラを用いた手法では、次の（８）式を用いることで、ポーズを推定しても良い。
【数７】
2018004420.tif 000009
b:ステレオカメラの基線長の距離
【００８６】
《機体位置姿勢計測処理》
機体位置姿勢計測処理は、ＤＲＡＭ３３に記録されたカメラ移動機体移動変換プログラム３３ｊに従ってＣＰＵ３１により実行される。
【００８７】
ワールド座標系から機体座標系への変換は、以下の式で行うことができる。ワールド座標系からカメラ座標系へ変換する回転行列と平行移動ベクトルをＲｗ２ｃ、ｔｗ２ｃとし、機体座標系からカメラ座標系へ変換する回転行列と平行移動ベクトルをＲｃ２ｐ、ｔｃ２ｐとする。このとき、ワールド座標系から機体座標系へ変換する回転行列と平行移動ベクトルＲｗ２ｐ、ｔｗ２とについて、以下の（１２）式が成り立つ。
【数８】
2018004420.tif 000010
【００８８】
これらにより、ワールド座標系における移動体１０の移動をステレオカメラ２１を用いて計測することができる。
【００８９】
以下に、図８のフローチャートを用いて、環境地図生成処理について説明する。環境地図生成処理は、ＤＲＡＭ３３に格納された環境地図生成プログラム３３ａに従ってＣＰＵ３１により実行される。
【００９０】
環境地図生成処理では、以下の３つの処理を主に行う。
・地図全体の再投影誤差の最小化処理（ステップＳ１）
・追跡に複数回成功した画像特徴点の環境地図への登録（ステップＳ２）
・環境地図にあるアウトライアの点の排除（ステップＳ３）
【００９１】
ステップＳ１において、再投影誤差の最小化処理はバンドル調整と言われ、過去に撮影したNj個の撮影ポーズ(Rj,tj)とNi個の３次元点の座標xiをパラメータとして、再投影誤差が最小となるポーズとパラメータを求める処理である。
【００９２】
この問題は、次の（１３）式のように、非線形の最小二乗問題に定式化することができる。
【数９】
2018004420.tif 000011
【００９３】
よって、これらの最適パラメータはLevenberg-Marquardt法などの手法を用いて求めることができる。
【００９４】
また、カメラ機体位置姿勢計測の場合と同様にステレオカメラの観測を追加してバンドル調整を行っても良い。
【００９５】
ステップＳ２において、追跡に複数回成功した画像特徴点の環境地図への登録は、３次元再構成処理と、信頼度計測処理からなり、この３次元再構成に成功し、かつ信頼度が高い点のみを環境地図へ追加する。
【００９６】
３次元再構成は、画像特徴点の３次元位置（Ｘ、Ｙ、Ｚ）を求める処理である。
３次元位置の算出は、ステレオカメラの測距情報から次の（１４）式〜（１７）式で求めることができる。
【００９７】
【数１０】
2018004420.tif 000012
xr:：計測対象の右カメラの画像上の観測座標X[pixel]
ｙｒ:計測対象の右カメラの画像上の観測座標Y[pixel]
ｂ:ステレオカメラの基線長（左右眼の光軸間距離）の距離[mm]
ｄ:視差[pixel]
ｆ：焦点距離[mm]
ｃｘ:画像のX方向の中心座標 [pixel]
ｃｙ:画像のY方向の中心座標 [pixel]
【００９８】
信頼度は再投影誤差のFisher情報量の逆数とし、これが閾値より高い３次元点については、信頼できる点として環境地図へ登録する。
【００９９】
この閾値は予め設定された値としても良いし、マップへの追加候補点の全点の上位のみをマップへ追加するようにこの処理の度に設定しても良い。
【０１００】
ステップＳ３において、環境地図にあるアウトライアの点の削除処理では、マップにある全点について上記信頼度を算出し、信頼度が閾値を小さい点をマップから削除する。
【０１０１】
この閾値については予め設定された値としても良いし、マップにある３次元点の全点の下位をマップから削除するように、処理の度に設定しても良い。
【０１０２】
次に、カメラ機体相対位置ずれ検出処理について図９のフローチャートを用いて説明する。カメラ機体相対位置ずれ検出処理は、ＤＲＡＭ３３に格納されたカメラ機体相対位置ずれ検出プログラム３３ｇに従ってＣＰＵ３１により実行される。
【０１０３】
ここで、カメラ機体相対位置ずれ検出処理に先立って、移動体１０の一部がステレオカメラ２１の画角内に入るようにステレオカメラ２１を移動体１０に搭載する工程が実施される。具体的には、ステレオカメラ２１を含む撮像系２０を機体１１の胴部１５に支持部材１８、１９を介して吊り下げ状態となるよう（例えばステレオカメラ２１の光軸が水平方向もしくは斜め下方を向くよう）取り付ける（図１参照）。このとき、ステレオカメラ２１の画角が比較的広角であれば、機体１１の一部（例えば脚部１６、１７やアーム９）を画角内に入れることができる。
【０１０４】
最初のステップＳ１１では、ステレオカメラ２１で画像（静止画像又は動画像）の撮影を行う。
【０１０５】
次のステップＳ１２では、カメラ機体相対位置ずれ情報計測処理を行う。具体的には、ステレオカメラ２１で撮影した画像を利用してステレオカメラ２１と機体１１の相対位置ずれ情報を計測する。詳述すると、ステレオカメラ２１と機体１１の並進位置ずれ（ΔＸ、ΔＹ、ΔＺ）と、回転位置ずれ（α、β、γ）を計測する。但し、ΔＸはＸ軸方向の並進位置ずれ量、ΔＹはＹ軸方向の並進位置ずれ量、ΔＺはＺ軸方向の並進位置ずれ量、αはＸ軸周りの回転位置ずれ量、βはＹ軸周りの回転位置ずれ量、γはＺ軸周りの回転位置ずれ量である。ステップＳ１２の更なる詳細は、後述する。
【０１０６】
次のステップＳ１３では、ステレオカメラ２１と機体１１の相対的な位置ずれのずれ量が閾値以上か否かを判断する。具体的には、（ΔＸ、ΔＹ、ΔＺ）がそれぞれ閾値（ΔＸｔｈ、ΔＹｔｈ、ΔＺｔｈ）以上であるか否か、及び（α、β、γ）がそれぞれ閾値（αｔｈ、βｔｈ、γｔｈ）以上であるか否かを判定する。この判定の結果、少なくとも１つの方向のずれが閾値を超えた場合に、ステップＳ１３での判断が肯定される。ステップＳ１３での判断が肯定されるとステップＳ１４に移行し、否定されるとフローは終了する。
【０１０７】
ステップＳ１４では、異常を通知する。異常の通知は、移動体１０の制御装置５１９に通信部３４を介して伝えられる。
【０１０８】
このとき、制御装置５１９は、移動体１０の自動運転を停止する。
【０１０９】
以上説明したカメラ機体相対位置ずれ検出処理は、撮像装置１００の起動時、移動体１０の自動運転開始時、又は定期的に実行され、ステレオカメラ２１の向きに異常がないかを確認する。
【０１１０】
図１０には、撮像系２０によって撮影された基準撮影画像、すなわちステレオカメラ２１と機体１１の相対的な位置ずれが生じていないときの撮影画像が示されている。図１０に示される基準撮影画像は、ステレオカメラ２１が機体１１に対して４５°斜め下向きに取り付けられたときの撮影画像である。
【０１１１】
この基準撮影画像において、右側、左側にある略Ｔ字状の黒い部分が機体１１の脚部１６、１７であり、上側にある黒いテーパ形状の部分が移動体１０のプロペラ１２（回転翼）を支持するアーム９である（図１参照）。
【０１１２】
移動体装置１では、ステレオカメラ２１に広画角のものを用いることで、自身の機体１１の一部を画角内に入れることができ、該一部を撮影することができる。
【０１１３】
なお、ステレオカメラ２１として視野角（画角）が１８０°以上のものを用いることで、前方領域を撮像しながら機体１１も撮像することができる。
【０１１４】
図１１には、カメラ機体位置ずれ計測用の基準マスク画像、すなわちステレオカメラ２１と機体１１の相対的な位置ずれが生じていないときのマスク画像が示されている。
【０１１５】
図１１において、基準マスク画像は、ハッチングされた領域が想定される移動体１０におけるステレオカメラ２１で撮影される領域である。この領域は、ステレオカメラ２１の設置時に画像を取得し人手で抽出しても良いし、クロマキーを使って機体色から抽出するなどしても良い。
【０１１６】
図１２には、カメラ機体位置ずれ計測用のマークが示されている。
【０１１７】
機体１１の脚部１６、１７にそれぞれ２つのマークが付され、アーム９に３つのマークが付されている。ここでは、マークは、白丸の中に黒色のバタフライ形状部が入ったものとされているが、その他のものであっても良い。また、マークの個数や大きさや設置箇所も適宜変更可能である。
【０１１８】
なお、図１２では、上記基準撮影画像における機体領域（機体１１が写っている領域）にマークが写っているが、上記基準撮影画像に代えて上記基準マスク画像を用いる場合には該基準マスク画像における機体領域にマークが写ることになる。
【０１１９】
撮像系２０で撮影した画像において、これらのマークの位置関係によって機体１１とステレオカメラ２１の位置や姿勢のずれを計測できる。
【０１２０】
次に、図９のステップＳ１２のカメラ機体相対位置ずれ情報計測処理の一例であるカメラ機体相対位置ずれ情報計測処理１について、図１３のフローチャートを用いて説明する。カメラ機体相対位置ずれ情報計測処理１は、カメラ機体相対位置ずれ情報計測プログラム３３ｍの一例であるカメラ機体相対位置ずれ情報計測プログラム１に従ってＣＰＵ３１によって実行される。
【０１２１】
最初のステップＳ２１では、ステレオカメラ２１で撮影された画像（撮影画像）から、機体領域（機体１１が写っている領域）を抽出する。
【０１２２】
次のステップＳ２２では、機体領域について、予め取得した基準視差画像の視差値と撮影画像の視差値の差分を算出する。なお、視差画像は、画素毎又は複数の画素を含む画素ブロック毎の視差データを総合したものである。
【０１２３】
ここでは、ステレオカメラ２１と機体１１の位置ずれの計測を移動体１０が使用される環境に依存させないために、予め上記基準マスク画像（図１１参照）を用意しておき、撮影画像中の、基準マスク画像においてマスクされている領域内のマークの座標を抽出する。
【０１２４】
詳述すると、マークが機体１１以外にもある場合、例えば機体１１及び地面にマークがある場合、どのマークを基準とすればよいのかを移動体装置側で認識できなくなる。そこで、予めマークを探す領域（マスクされている領域）を決めておき、その領域の中で検出したマークの撮影画像中の座標を抽出する。なお、上記基準マスク画像に代えて上記基準撮影画像を用いても良い。
【０１２５】
マークの画像中の座標の抽出は、図１２のような白部と黒部を有するマークを利用すれば、上記Harrisの特徴点抽出などにより行うことができる。
【０１２６】
上記基準マスク画像におけるマークの３次元位置を機体座標系で事前に計測しておき、さらに撮影画像におけるマークの３次元位置を機体座標系で計測する。
【０１２７】
基準マスク画像及び撮影画像におけるマークの３次元位置と画像特徴点の対応により機体座標系からカメラ座標系へ座標変換する３自由度の回転パラメータ（α、β、γ）及び３自由度の並進パラメータ（ΔＸ、ΔＹ、ΔＺ）の計６自由度のパラメータを、上記PnP問題を解くことによって求めることができる。
【０１２８】
そこで、撮影画像の機体座標系からカメラ座標系への変換を行う６自由度のパラメータと、基準マスク画像の機体座標系からカメラ座標系への変換を行う６自由度のパラメータを比較し、少なくとも１自由度のパラメータの差が閾値より大きい場合は、ずれを検出したとする。パラメータの比較は、６自由度のパラメータの差分の2-ノルムを使って算出することができる。
【０１２９】
ステップＳ２２が実行されるとフローは終了する。
【０１３０】
次に、図９のステップＳ１２のカメラ機体相対位置ずれ情報計測処理の一例であるカメラ機体相対位置ずれ情報計測処理２について、図１４のフローチャートを用いて説明する。カメラ機体位置ずれ情報計測処理２は、カメラ機体相対位置ずれ情報計測プログラム３３ｍの一例であるカメラ機体相対位置ずれ情報計測プログラム２に従ってＣＰＵ３１によって実行される。
【０１３１】
最初のステップＳ３１では、ステレオカメラ２１で撮影された画像（撮影画像）から、基準マスク画像を用いて機体領域を抽出する。
【０１３２】
次のステップＳ３２では、機体領域について基準撮影画像の輝度値と撮影画像の輝度値の差分を算出する。ここでは、機体領域として抽出された領域に含まれる各ピクセルについて、撮影画像の輝度値と予め取得、保持している基準撮影画像の輝度値を比較しその差分の総和をずれ量とする。ステレオカメラ２１と機体１１の位置関係がずれていれば、背景が機体領域に映り込み、輝度値の差分が大きくなるため、ずれ量が大きくなる。このずれ量が閾値を超えている場合にずれていると判定する。
【０１３３】
この場合、画像の特定の領域について、ピクセルの輝度値の差分を総和を求めるのみの計算なので、論理回路によって実現が可能であり、撮影画像についてのフレームバッファも必要ないため、低遅延低コストでずれ検出を行うことができる。
【０１３４】
ステップＳ３２が実行されるとフローは終了する。
【０１３５】
次に、図９のステップＳ１２のカメラ機体位置ずれ情報計測処理の一例であるカメラ機体相対位置ずれ情報計測処理３について、図１５のフローチャートを用いて説明する。カメラ機体相対位置ずれ情報計測処理３は、カメラ機体相対位置ずれ情報計測プログラム３３ｍの一例であるカメラ機体相対位置ずれ情報計測プログラム３に従ってＣＰＵ３１によって実行される。
【０１３６】
最初のステップ４１では、ステレオカメラ２１で撮影された視差画像（撮影画像）から、基準マスク画像を用いて機体領域を抽出する。
【０１３７】
次のステップＳ４２では、機体領域について基準視差画像の視差値と撮影画像の視差値の差分を算出する。ここでは、機体領域として抽出された領域に含まれる各ピクセルについて、撮影画像の視差値と予め取得、保持している基準視差画像の視差値を比較しその差分の総和をずれ量とする。ステレオカメラ２１と機体１１の位置関係がずれていれば、背景が機体領域に映り込み、視差値の差分が大きくなるため、ずれ量が大きくなる。このずれ量が閾値を超えている場合にずれていると判定する。
【０１３８】
この場合、視差画像を用いることで、照明条件にロバストなズレ検出が可能になる。
【０１３９】
ステップＳ４２が実行されるとフローは終了する。
【０１４０】
ここで、ステレオカメラを用いて移動体から障害物までの距離を測定することにより、移動体を障害物に接触や衝突させることなく飛行させることができる。
【０１４１】
一般に、ステレオカメラを含む撮像装置は、ステレオカメラと移動体の設計時の相対位置姿勢に基づいて距離情報を算出するよう、すなわちステレオカメラから見た距離情報を移動体から見た距離情報に変換するよう演算処理する。このため、ステレオカメラと移動体の相対位置が例えば筐体の歪み等により設計時から変化した場合には、距離情報の信頼性が低下することになる。
【０１４２】
以下に、図１２を用いて詳細に説明する。ここでは、一例として移動体とステレオカメラの回転位置ずれによる測定距離の誤差について説明する。
【０１４３】
簡単のため、移動体とステレオカメラの設計時の相対位置を、移動体とステレオカメラの向きが同じ状態（移動体の進行方向とステレオカメラの光軸方向が一致している状態）とする。
【０１４４】
図１６（ａ）に示されるように移動体とステレオカメラの相対位置に設計時からのずれが生じていない場合、移動体の前方に移動体の進行方向に垂直な平面（物体の一面）があれば、ステレオカメラは移動体の進行方向に垂直な平面があることを検出する。この場合、ステレオカメラを含む撮像系（以下では「ステレオ撮像系」とも呼ぶ）で生成された距離画像は、平面全体で均一（一定の濃さ）となる。このとき、移動体は、ステレオ撮像系からの距離画像を頼りに例えば旋回すれば平面との衝突を回避することができる。なお、「距離画像」は、ステレオ撮像系で生成された視差画像から得られた画素毎又は複数の画素を含む画素ブロック毎の距離データを総合したものである。
【０１４５】
一方、図１６（ｂ）のように移動体とステレオカメラの相対位置に角度θの回転位置ずれが有る場合には、ステレオカメラは移動体の進行方向に垂直な平面を該進行方向から角度θだけ傾いた平面であると検出する。この場合、ステレオカメラから出力された平面の距離データは、移動体から見て移動体の右端に近いほど距離が短く、左端に近いほど距離が長い距離データである。図１６（ｂ）に示されるステレオ撮像系で生成された距離画像には、濃い部分ほど距離が短く、淡い部分ほど距離が長いことを示す濃淡のグラデーションがある。
【０１４６】
一方、移動体から見ると平面は進行方向に対して垂直である。このため、移動体がステレオ撮像系からの距離データを頼りに例えば右旋回すると、特に移動体の平面に近い左側の部分が該平面に衝突することが懸念される。
【０１４７】
そこで、移動体とステレオカメラに角度θの回転位置ずれがあることを検出し、ステレオ撮像系で生成された距離データに角度θを加味することで、すなわち該距離データを角度θを用いて補正することで、移動体から見た距離データに変換することができる。この変換後の距離データを頼りに移動体が例えば右旋回すれば、平面と衝突することなく安全に移動することができる。
【０１４８】
以上を総括すると、カメラ機体相対位置ずれ（ΔＸ、ΔＹ、ΔＺ）、（α、β、γ）を用いて、撮像系２０での演算結果（測定距離）を補正することで、すなわちカメラ座標系における距離情報を機体座標系における距離情報に変換することで、移動体１０から見た測定距離を精度良く算出することが可能である。この補正（変換）は、例えば制御演算部５２０やＣＰＵ３１５やＣＰＵ４１１によって公知の座標変換方法を用いて実施可能である。
【０１４９】
この場合、移動体１０を安全に飛行させることができるため、カメラ機体相対位置ずれのずれ量に関わらず、すなわち該ずれ量と閾値との大小関係に関わらず、移動体１０を飛行させることができる。
【０１５０】
以下に、本実施形態の測距方法を実現するための測距処理について図１７を参照して説明する。図１７のフローチャートは、移動体１０と撮像系２０と処理装置３０によって実行される処理アルゴリズムに基づいている。
【０１５１】
ここで、測距処理に先立って、移動体１０の一部がステレオカメラ２１の画角内に入るようにステレオカメラ２１を移動体１０に搭載する工程が実施される。具体的には、ステレオカメラ２１を含む撮像系２０を機体１１の胴部１５に支持部材１８、１９を介して吊り下げ状態となるよう（例えばステレオカメラ２１の光軸が水平方向もしくは斜め下方を向くよう）取り付ける（図１参照）。このとき、ステレオカメラ２１の画角が比較的広角であれば、機体１１の一部（例えば脚部１６、１７やアーム９）を画角内に入れることができる。
【０１５２】
最初のステップＳ５１では、移動体１０が移動中か否かを判断する。具体的には、ＧＰＳ等からの情報を基に移動体１０の移動の有無を判定する。ここでの判断が肯定されるとステップＳ５２に移行し、否定されるとフローは終了する。
【０１５３】
ステップＳ５２では、ステレオカメラ２１で画角内の物体を撮像する。
【０１５４】
次のステップＳ５３では、ステレオカメラの２つの撮影画像（ステレオ画像）から物体の視差画像を生成する。
【０１５５】
次のステップＳ５４では、視差画像から物体の距離画像を生成する。
【０１５６】
次のステップＳ５５では、移動体１０とステレオカメラ２１の相対位置ずれ情報（ΔＸ、ΔＹ、ΔＺ）、（α、β、γ）を検出する。
【０１５７】
次のステップＳ５６では、相対位置ずれ情報を用いて距離画像を補正する。すなわち、距離画像の画素又は画素ブロック毎の距離情報を相対位置ずれ情報を用いて補正する。
【０１５８】
補正後の距離画像は移動体１０の制御装置５１９に送られ、移動体１０の制御に用いられる。
【０１５９】
以上説明した本実施形態の撮像装置１００は、移動体１０に搭載される撮像装置であって、移動体１０の少なくとも一部が画角内に入る、ステレオカメラ２１（カメラ）を含む撮像系２０と、ステレオカメラ２１の撮影画像に基づいて、移動体１０と撮像系２０の相対的な位置ずれ（カメラ機体相対位置ずれ）を検出する処理装置３０（検出手段）と、を備えている。
【０１６０】
この場合、移動体１０が使用される環境によらず、移動体１０の少なくとも一部をステレオカメラ２１で撮影できる。
【０１６１】
この結果、移動体が使用される環境によらず、移動体とカメラの相対的な位置ずれを検出できる。
【０１６２】
なお、ステレオカメラ２１に超広画角なものを用いれば、移動体１０の全体を撮影することも可能である。
【０１６３】
なお、本実施形態の撮像装置１００のカメラとして、ステレオカメラ２１に限らず、要は、少なくとも１つの撮像部を有するカメラであれば良く、少なくとも１つの単眼カメラを用いても良い。
【０１６４】
一方、特許文献１（特開２００８−０１１１７４号公報）には、充車両が走行中であっても、高い頻度で、車載カメラの光軸補正を可能とする目的で、道路上の路面標示の撮像情報から、特徴点または特徴線を抽出し、車載カメラの光軸補正パラメータを路面標示の特徴量から求める構成が開示されている。
【０１６５】
しかし、特許文献１では、例えば飛行体において、屋内を飛行する場合などでは路面標示のような標示があるケースは少なく、この場合には、飛行体とカメラの相対的な位置ずれを検出できない。
【０１６６】
また、移動体１０のステレオカメラ２１の画角内に入る部分には、少なくとも１つのマークが付され、検出手段は、ステレオカメラ２１の撮影画像におけるマークの画像と予め取得されたマークの基準画像に基づいて、カメラ機体相対位置ずれを検出することが好ましい。
【０１６７】
この場合、移動体１０とステレオカメラ２１のずれを数値的に計測することができる。
【０１６８】
また、検出手段は、カメラの撮影画像における移動体１０のステレオカメラ２１の画角内に入る部分の画像と、予め取得された該部分の基準輝度画像のピクセル値の差分の総和を用いて、カメラ機体相対位置ずれを検出することが好ましい。
【０１６９】
この場合、単純なアルゴリズムでズレを計測することができる。
【０１７０】
また、検出手段は、ステレオカメラ２１の複数の撮像部の撮影画像から得られた移動体１０のステレオカメラ２１の画角内に入る部分の視差画像と、予め取得された該部分の基準視差画像の視差値の差分の総和を用いて、カメラ機体相対位置ずれを検出することが好ましい。
【０１７１】
この場合、照明変化に対してロバストに、カメラ機体相対位置ずれを計測することができる
【０１７２】
また、撮像装置１００と、該撮像装置１００が搭載される移動体と、を備える移動体装置１によれば、撮像装置１００によって検出されるカメラ機体相対位置ずれに基づいて移動体１０を制御することができる。この結果、移動体１０を使用環境によらず安全に移動可能な移動体装置を提供できる。
【０１７３】
また、移動体装置１は、撮像装置１００の検出手段の検出結果に基づいて移動体１０の移動許否を決定する制御装置５１９（制御手段）を更に備えることが好ましい。
【０１７４】
また、制御装置５１９は、移動体装置１への電源投入時又は移動体１０の移動開始時（例えば自動飛行開始時）に移動体１０の移動許否の決定を行うことが好ましい。
【０１７５】
この場合、例えば移動体１０の移動開始前や移動体開始時に移動体１０とステレオカメラ２１の相対位置ずれが閾値を超えたときに、安全性を考慮して移動を禁止させることができる。
【０１７６】
また、処理装置３０は、ステレオカメラ２１の複数の撮像部の撮影画像から得られた視差画像に基づいて、撮影画像に写る物体までの距離情報を算出するＣＰＵ３１５（処理部）と、撮像装置１００の検出手段の検出結果に基づいて、演算手段の算出結果を補正する制御演算部５２０（補正手段）と、を更に備えることが好ましい。なお、補正手段として、制御演算部５２０に代えて、ＣＰＵ３１５やＣＰＵ４１１を用いても良い。
【０１７７】
この場合、ステレオカメラ２１を用いて物体までの距離を測定できる。
【０１７８】
また、移動体装置１は、移動体１０に搭載され、物体までの距離情報を測定する測距装置を更に備え、撮像装置１００の検出手段の検出結果に基づいて、測距装置の測定結果を補正する補正手段（例えば制御演算部５２０やＣＰＵ３１５やＣＰＵ４１１）と、を更に備えていても良い。
【０１７９】
この場合、物体までの距離情報を測定できる専用の測距装置が設けられているため、撮像装置１００のカメラとして、ステレオカメラ２１に代えて単眼カメラを用いることもできる。
【０１８０】
また、移動体１０が無人航空機である場合には、移動体１０の使用環境によらず、安全に飛行させることができる。
【０１８１】
また、本実施形態の撮像装置１００は、撮像機能を有する測距装置と捉えることもできる。
【０１８２】
すなわち、本実施形態の測距装置は、移動体１０に搭載される測距装置であって、移動体１０の少なくとも一部が画角内に入る撮像部を複数含むステレオカメラ２１（カメラ）と、複数の撮像部の撮影画像から視差画像を生成し、撮影画像に写る物体までの距離情報を算出する処理回路と、複数の撮像部の撮影画像に基づいて、移動体１０とステレオカメラ２１の相対的な位置ずれ情報（カメラ機体相対位置ずれ情報）を検出する処理装置３０（検出手段）と、を備えている。
【０１８３】
この場合、移動体１０が使用される環境によらず、移動体１０の少なくとも一部をステレオカメラ２１で撮影できる。
【０１８４】
この結果、移動体が使用される環境によらず、移動体とカメラの相対的な位置ずれを検出できる。
【０１８５】
なお、本実施形態の測距装置のカメラとして、ステレオカメラ２１に限らず、要は、少なくとも１つの撮像部を有するカメラであれば良く、少なくとも１つの単眼カメラを用いても良い。
【０１８６】
また、本実施形態の測距装置は、カメラ機体相対位置ずれ情報に基づいて距離情報を補正する制御演算部５２０（補正手段）を更に備えることが好ましい。
【０１８７】
この場合、移動体１０を安全に移動させることができる。
【０１８８】
また、移動体１０のステレオカメラ２１の画角内に入る部分には、少なくとも１つのマークが付され、検出手段は、ステレオカメラ２１の撮影画像におけるマークの画像と予め取得されたマークの基準画像に基づいて、カメラ機体相対位置ずれを検出することが好ましい。
【０１８９】
この場合、移動体１０とステレオカメラ２１のずれを数値的に計測することができる。
【０１９０】
また、検出手段は、カメラの撮影画像における移動体１０のステレオカメラ２１の画角内に入る部分の画像と、予め取得された該部分の基準輝度画像のピクセル値の差分の総和を用いて、カメラ機体相対位置ずれを検出することが好ましい。
【０１９１】
この場合、単純なアルゴリズムでズレを計測することができる。
【０１９２】
また、検出手段は、ステレオカメラ２１の複数の撮像部の撮影画像から得られた移動体１０のステレオカメラ２１の画角内に入る部分の視差画像と、予め取得された該部分の基準視差画像の視差値の差分の総和を用いて、カメラ機体相対位置ずれを検出することが好ましい。
【０１９３】
この場合、照明変化に対してロバストに、カメラ機体相対位置ずれを計測することができる
【０１９４】
また、本実施形態の測距装置と、該測距装置が搭載される移動体１０と、を備える移動体装置１によれば、測距装置によって検出されるカメラ機体相対位置ずれに基づいて移動体１０を制御することができる。この結果、移動体１０を使用環境によらず安全に移動可能な移動体装置を提供できる。
【０１９５】
また、移動体装置１は、測距装置の検出手段の検出結果に基づいて移動体１０の移動許否を決定する制御装置５１９（制御手段）を更に備えることが好ましい。
【０１９６】
また、制御装置５１９は、移動体１０への電源投入時又は移動体１０の移動開始時（例えば自動飛行開始時）に移動体１０の移動許否の決定を行う。
【０１９７】
この場合、例えば移動体１０の移動開始前や移動体開始時に移動体１０とステレオカメラ２１の相対位置ずれが閾値を超えたときに、安全性を考慮して移動を禁止させることができる。
【０１９８】
また、本実施形態の位置ずれ検出方法は、移動体１０に搭載される、ステレオカメラ２１（カメラ）を含む装置（撮像装置や測距装置）と移動体１０の相対的な位置ずれを検出する位置ずれ検出方法であって、移動体１０の少なくとも一部がステレオカメラ２１の画角内に入るようにステレオカメラ２１を移動体１０に搭載する工程と、ステレオカメラ２１の撮影画像に基づいて、移動体１０とステレオカメラ２１の相対的な位置ずれを検出する工程と、を含む。
【０１９９】
この場合、移動体１０の使用環境によらず、移動体１０の少なくとも一部をステレオカメラ２１で撮影できる。
【０２００】
この結果、移動体が使用される環境によらず、移動体とカメラの相対的な位置ずれを検出できる。
【０２０１】
なお、本実施形態の位置ずれ検出方法に用いられるカメラとして、ステレオカメラ２１に限らず、要は、少なくとも１つの撮像部を有するカメラであれば良く、少なくとも１つの単眼カメラを用いても良い。
【０２０２】
また、本実施形態の測距方法は、移動体１０に搭載される、複数の撮像部を有する装置（撮像装置や測距装置）を用いる測距方法であって、移動体１０の少なくとも一部がステレオカメラ２１（カメラ）の画角内に入るようにステレオカメラ２１を移動体１０に搭載する工程と、複数の撮像部の撮影画像から視差画像を生成し、撮影画像に写る物体までの距離情報を算出する工程と、複数の撮像部の撮影画像に基づいて、移動体１０とステレオカメラ２１の相対的な位置ずれ情報（カメラ機体相対位置ずれ情報）を検出する工程と、を含む。
【０２０３】
この場合、移動体１０が使用される環境によらず、移動体１０の少なくとも一部をステレオカメラ２１で撮影できる。
【０２０４】
この結果、移動体が使用される環境によらず、移動体とカメラの相対的な位置ずれを検出できる。
【０２０５】
なお、本実施形態の測距方法に用いられるカメラとして、ステレオカメラ２１に限らず、要は、少なくとも１つの撮像部を有するカメラであれば良く、少なくとも１つの単眼カメラを用いても良い。
【０２０６】
また、本実施形態の測距方法は、カメラ機体相対位置ずれ情報に基づいて距離情報を補正する工程を更に含むことが好ましい。
【０２０７】
この場合、移動体１０を安全に移動させることができる。
【０２０８】
なお、上記実施形態では、カメラ機体相対位置ずれ情報を用いて距離情報を補正する演算処理を移動体１０の制御演算部５２０が行っているが、これに限らず、例えば撮像手段３００のＣＰＵ３１５や制御部４００のＣＰＵ４１１が行っても良い。この場合も、カメラ機体相対位置ずれ情報を撮像手段３００のＣＰＵ３１５や制御部４００のＣＰＵ４１１に入力する必要がある。
【０２０９】
なお、上記実施形態では、撮像系２０を支持部材１８、１９を介して機体１１に取り付けているが、映像安定化及び防振の観点から、図１８に示される変形例の移動体装置１´のように撮像系２０をジンバル装置５０を介して機体１１に取り付けても良い。
【０２１０】
ジンバル装置５０は、ジンバル機構と、姿勢駆動部と、ジンバル機構の揺動軸の角速度を検知する、少なくとも１つのジャイロセンサを含むジャイロユニットを有する。
【０２１１】
ジンバル装置５０では、移動体１０が移動（例えば急旋回、急加減速）しているときに、ジンバル機構の動作による移動体１０と撮像系２０の相対位置ずれをジャイロユニットで検出し、該ずれをキャンセルするよう、すなわち移動体１０の向きに撮像系２０の向きを追従させるよう姿勢駆動部が撮像系２０を駆動する。
【０２１２】
この際、移動体１０と撮像系２０の相対位置が正常状態（初期状態）から一時的にずれてしまい、そのずれている間に撮像系２０で生成された距離画像は、信頼性の低いものとなってしまう。
【０２１３】
そこで、変形例の移動体装置１´でも、撮像系２０で生成された距離画像をカメラ機体相対位置ずれ情報に基づいて補正することで、信頼性の高い距離画像を得ることができる。
【０２１４】
以下に、その原理を説明する。
【０２１５】
ジンバルは、例えばＰｉｔｃｈｉｎｇ（ピッチング）、Ｒｏｌｌｉｎｇ（ローリング）、Ｙａｗｉｎｇ（ヨーイング）の直交する３軸周りに回転自在に構成されている（２軸周りに回転自在なジンバルもある）。
【０２１６】
ジンバルを介してステレオカメラが移動体に取り付けられる場合、移動体に対するステレオカメラの、ジンバルの各軸周りの振動などによる高速で細かい傾きを角度センサ（ジャイロセンサ）で検知して、検知した傾きを打ち消すことでステレオカメラの移動体に対する姿勢を一定に保つことができる。
【０２１７】
この検知した傾きをステレオカメラで得られた視差にフィードバックすることで、ステレオカメラが向いている方向の距離ではなく移動体が向いている方向（進行方向）方向の距離を算出することができる。
【０２１８】
図１９（ａ）では移動体とステレオカメラが同じ方向を向いている場合、すなわち移動体の進行方向とステレオカメラの光軸方向が一致している場合を示している。この場合、移動体の前方に平面（物体の一面）があれば、ステレオカメラは移動体の進行方向と垂直な平面があることを検出する。この場合、図１９（ａ）に示されるようにステレオ撮像系で生成された距離画像は、平面全体で均一（一定の濃さ）となる。
【０２１９】
一方、図１９（ｂ）のように移動体が紙面に垂直な軸（例えば鉛直軸）周りの一方向（例えば左周り）に急回転（急旋回）し移動体が角度θだけ傾いても、ジンバルにより安定に保たれているステレオカメラは急には傾かない。そのため、ステレオカメラは進行方向と垂直方向に平面があることを検出したままである。このとき、図１９（ｂ）に示されるステレオ撮像系で生成された距離画像は、図１９（ａ）の距離データと同じである。
【０２２０】
この場合、移動体から見ると平面は進行方向と垂直方向ではなく、該進行方向からθだけ傾いた状態で存在している。移動体がこのまま進行すると、移動体の平面に近い右側の部分が該平面に衝突することになる。
【０２２１】
そこで、移動体が角度θだけ傾いたことを検出し、ステレオ撮像系で生成された距離データに角度θを加味する、すなわち該距離データを角度θを用いて補正する。
【０２２２】
すると、図１９（ｃ）のようにステレオ撮像系から出力された平面の距離データを、移動体から見た移動体の右端に近いほど距離が短く、左端に近いほど距離が長い距離データに補正することが可能である。図１９（ｃ）に示されるステレオ撮像系で生成された距離画像には、濃い部分ほど距離が短く、淡い部分ほど距離が長いことを示す濃淡のグラデーションがある。
【０２２３】
移動体の急回転に対して、ステレオカメラは緩やかに追従する。移動体の急回転が終わると、図１９（ｄ）のように移動体の向きとステレオカメラの向きが一致し、図１９（ｂ）と同様に移動体とステレオカメラは同じ方向を向くことになる。図１９（ｄ）に示されるステレオ撮像系で生成された距離画像は、図１９（ｃ）に示される距離画像と同じであり、図１９（ｃ）において距離データに角度θが加味されていることが確認できる。
【０２２４】
このようにしてステレオ撮像系で生成された距離データに移動体の向きとステレオカメラの向きの違いの傾きを加味することでリアルタイムに移動体の向きの距離データを得ることができる。
【０２２５】
この場合、距離データは振動に対して安定ではなくなるが距離データを人が見て観察するわけではないため問題はない。ジンバル装置により安定な撮影画像が得られるとともに、リアルタイムに距離画像が得られることで室内のような狭い場所でも移動体を周囲のものに衝突することなく精度よく飛行させることができる。
【０２２６】
なお、変形例では、姿勢駆動部によって、測距系や撮像系の姿勢を制御して移動体の向きに追従させているが、移動体から見た距離画像を精度良く生成するという観点からは、姿勢駆動部は必須ではない。すなわち、移動体の向きと撮像装置の向きが継続的に異なっていても、相対位置ずれ情報（ΔＸ、ΔＹ、ΔＺ）、（α、β、γ）を用いて距離画像を補正することにより、移動体から見た距離画像を精度良く生成できるからである。
【０２２７】
また、上記実施形態及び変形例において、撮影用のカメラ（単眼カメラもしくはステレオカメラ）と測距用のステレオカメラを併設しても良い。
【０２２８】
また、上記実施形態及び変形例において、撮像系は、ステレオカメラに代えて、単眼カメラを２つ組み合わせたものを用いても良い。
【０２２９】
また、上記実施形態及び変形例の撮像装置や測距装置を自律して移動可能なロボット、車両、船舶、有人航空機等にジンバルを介して又は介さずに取り付けても良い。この場合も、上記実施形態及び変形例と同様の効果が得られる。
【０２３０】
また、上記実施形態及び変形例の説明で用いた数値、形状等は、本発明の趣旨を逸脱しない範囲で適宜変更可能である。
【符号の説明】
【０２３１】
１…移動体装置、１０…移動体、２０…撮像系（装置の一部）、３０…処理装置（装置の一部、検出手段）、５０…ジンバル、１００…撮像装置（装置）、３１５…ＣＰＵ（演算手段）、５２０…制御演算部（制御手段、補正手段）。
【先行技術文献】
【特許文献】
【０２３２】
【特許文献１】特開２００８−０１１１７４号公報
【図１】
2018004420.tif 000013
【図２】
2018004420.tif 000014
【図３】
2018004420.tif 000015
【図４】
2018004420.tif 000016
【図５】
2018004420.tif 000017
【図６】
2018004420.tif 000018
【図７】
2018004420.tif 000019
【図８】
2018004420.tif 000020
【図９】
2018004420.tif 000021
【図１０】
2018004420.tif 000022
【図１１】
2018004420.tif 000023
【図１２】
2018004420.tif 000024
【図１３】
2018004420.tif 000025
【図１４】
2018004420.tif 000026
【図１５】
2018004420.tif 000027
【図１６】
2018004420.tif 000028
【図１７】
2018004420.tif 000029
【図１８】
2018004420.tif 000030
【図１９】
2018004420.tif 000031