(57)【要約】      （修正有）【課題】移動体を安全に移動させるための情報を得ることができる装置を提供する。【解決手段】測距装置は、移動体１０に搭載される測距装置であって、移動体１０にジンバルを介して取り付けられ、対象物までの距離情報を測定する測距系と、移動体１０の移動に伴う測距系と移動体１０の相対的な姿勢変化に関する情報である姿勢変化情報を検出する検出系と、姿勢変化情報を用いて距離情報を補正する制御演算部５２０（補正系）と、を備えている。【選択図】図８
2017227516.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載される装置であって、
前記移動体にジンバルを介して取り付けられる測距系と、
前記移動体の移動に伴う前記測距系と前記移動体との相対的な姿勢変化に関する情報である姿勢変化情報を検出する検出系と、
前記姿勢変化情報を用いて前記測距系の測定距離を補正する補正系と、を備える装置。
【請求項２】
前記姿勢変化情報は、前記移動体の進行方向に対する前記測距系の光軸の傾きに関する情報であることを特徴とする請求項１に記載の装置。
【請求項３】
前記検出系は、前記移動体の移動に伴う前記ジンバルの揺動軸の傾斜情報を計測する少なくとも１つのジャイロセンサを含むことを特徴とする請求項１又は２に記載の装置。
【請求項４】
前記検出系は、
前記移動体の姿勢を検出する移動体姿勢検出部と、
前記測距系の姿勢を検出する測距系姿勢検出部と、を含むことを特徴とする請求項１又は２に記載の装置。
【請求項５】
前記測距系は、
複数の撮像部と、
前記複数の撮像部の撮像画像から視差画像を生成する処理部と、を含み、
前記検出系は、前記視差画像に基づいて前記姿勢変化情報を求めることを特徴とする請求項１又は２に記載の装置。
【請求項６】
前記複数の撮像部は、前記移動体の少なくとも一部が画角内に入ることを特徴とする請求項５に記載の装置。
【請求項７】
前記少なくとも一部には、少なくとも１つのマークが付されていることを特徴とする請求項６に記載の装置。
【請求項８】
前記複数の撮像部の撮像画像から輝度情報を得る輝度情報取得部を更に備えることを特徴とする請求項５〜７のいずれか一項に記載の装置。
【請求項９】
前記測距系は、ステレオカメラ法、ＴＯＦ法又はパターン投影法を用いることを特徴とする請求項１〜４のいずれか一項に記載の装置。
【請求項１０】
請求項１〜９のいずれか一項に記載の装置と、
前記装置の測距系が前記ジンバルを介して取り付けられる移動体と、を備える移動体装置。
【請求項１１】
移動体にジンバルを介して取り付けられる測距系を用いる測距方法であって、
前記測距系を用いて対象物までの距離情報を測定する工程と、
前記移動体の移動に伴う前記測距系と前記移動体との相対的な姿勢変化に関する情報である姿勢変化情報を検出する工程と、
前記姿勢変化情報を用いて前記距離情報を補正する工程と、含む測距方法。
【請求項１２】
前記検出する工程では、前記移動体の移動に伴う前記ジンバルの揺動軸の傾斜情報を計測することを特徴とする請求項１１に記載の測距方法。
【請求項１３】
前記検出する工程は、
前記移動体の姿勢を検出するサブ工程と、
前記測距系の姿勢を検出するサブ工程を、を含むことを特徴とする請求項１１に記載の測距方法。
【請求項１４】
前記測距系は、複数の撮像部を含み、
前記測定する工程では、前記複数の撮像部の撮像画像から視差画像を生成し、前記撮像画像に写る物体までの距離情報を算出することを特徴とする請求項１１に記載の測距方法。
【請求項１５】
前記複数の撮像部は、前記移動体の少なくとも一部が画角内に入り、
前記検出する工程では、前記視差画像に基づいて前記姿勢変化情報を求めることを特徴とする請求項１４に記載の測距方法。
【請求項１６】
前記少なくとも一部には、少なくとも１つのマークが付されていることを特徴とする請求項１５に記載の測距方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、装置、移動体装置及び測距方法に関する。
【背景技術】
【０００２】
近年、移動体に搭載される測距や撮像を行う装置の開発が盛んに行われている。
【０００３】
例えば、特許文献１には、移動体にジンバルを介して取り付けられる撮像を行う装置が開示されている。
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１に開示されている装置等の従来の装置では、移動体を安全に移動させるための情報を得ることに関して改善の余地があった。
【課題を解決するための手段】
【０００５】
本発明は、移動体に搭載される装置であって、前記移動体にジンバルを介して取り付けられる測距系と、前記移動体の移動に伴う前記測距系と前記移動体の相対的な姿勢変化に関する情報である姿勢変化情報を検出する検出系と、前記姿勢変化情報を用いて前記測距系の測定距離を補正する補正系と、を備える装置である。
【発明の効果】
【０００６】
本発明によれば、移動体を安全に移動させるための情報を得ることができる。
【図面の簡単な説明】
【０００７】
【図１】第１実施形態の移動体装置の一例を示す模式図である。
【図２】図１の測距系を示す斜視図である。
【図３】図１のジンバルの一例を模式的に示す斜視図である。
【図４】測距系のステレオカメラの模式図である。
【図５】移動体装置の構成の一例を示すブロック図である。
【図６】測距系の測距部のハードウェア構成の一例を示すブロック図である。
【図７】測距系の制御部のハードウェア構成の一例を示すブロック図である。
【図８】移動体のハードウェア構成の一例を示すブロック図である。
【図９】ステレオカメラの原理図である。
【図１０】ステレオカメラで得られる視差を説明するための図である。
【図１１】ステレオカメラで得られる視差と距離の関係図である。
【図１２】図１２（ａ）〜図１２（ｄ）は、それぞれ移動体にジンバルを介して取り付けられたステレオカメラの向き及び移動体の向きと、距離画像の関係を示す図（その１〜その４）である。
【図１３】測距処理１を説明するためのフローチャートである。
【図１４】図１４（ａ）〜図１４（ｃ）は、それぞれ第２実施形態の測距方法を説明するための図（その１〜その３）である。
【図１５】第３実施形態の移動体装置を説明するための図である。
【図１６】図１５の移動体のハードウェア構成を示すブロック図である。
【図１７】図１５の測距系のハードウェア構成を示すブロック図である。
【図１８】図１８（ａ）及び図１８（ｂ）は、それぞれ図１７の投光系及び受光系を説明するための図である。
【図１９】測距処理２を説明するためのフローチャートである。
【図２０】ＴＯＦ法を用いる測距系の変形例を説明するための図である。
【図２１】パターン投影法を用いる測距系の一例を説明するための図である。
【発明を実施するための形態】
【０００８】
［導入］
本発明の実施形態は、一例として移動体に測距装置を搭載することで自立して移動することを可能にした移動体装置に関する。移動体装置において、測距装置は、移動体の周囲に存在する物体（対象物）との距離を測距することで、移動体の移動を支援するものである。
【０００９】
例えば、移動体が自律して移動する移動体であれば、測距装置は、移動体の周囲に存在する物体（対象物）との距離を測定することで、移動体の自己位置を推定し、移動体は、測距装置の自己位置推定結果に基づいて自律移動を制御することができる。これにより、移動体は、障害物等を避けながら自律移動することができる。
【００１０】
以下の実施形態では、移動体が、自律飛行する無人航空機（例えば、ＤｒｏｎｅやＵＡＶ（Unmanned  aerial  vehicle））であり、測距装置が移動体の自己位置を推定し、移動体が自己位置推定結果に基づいて自律移動を制御する移動体装置を例にとって説明する。
【００１１】
《第１実施形態》
以下に、本発明の第１実施形態を図面を用いて詳細に説明する。
［基本構成］
図１は、第１実施形態の移動体装置１の一例を示す模式図である。図１に示されるように、移動体装置１は、移動体１０と、ジンバル２０と、測距系１００と、姿勢駆動部２００と、を備えている。ここでは、移動体１０は、無人航空機である。以下、移動体１０の前後方向をＸ軸方向、左右方向をＹ軸方向、高さ方向をＺ軸方向とする移動体に設定されたＸＹＺ３次元直交座標系（以下では「移動体座標系」とも呼ぶ）を適宜用いて説明する。ここでは、移動体座標系において移動体１０の前方向は＋Ｘ方向である。
【００１２】
移動体１０は、機体１１と、該機体１１の前後左右に設けられたプロペラ１２〜１５と、を含む。移動体１０は、プロペラ１２〜１５が回転駆動することにより飛行するように構成されている。また、機体１１には、ジンバル２０と測距系１００と４本の足（１６,１７,１８,１９）が設けられている。４本の足は機体１１が地面に着地した時に転倒しないように支えると共に測距系１００が地面に接触しないように保護する役割も担っている。
【００１３】
図２は、第１実施形態の測距系１００の一例を示す斜視図であり、図３はジンバル２０を模式的に示す斜視図であり、図４は測距系１００の一例を概略的に示す構成図である。
【００１４】
測距系１００は、図２〜図４に示されるように、筐体１２０と、筐体１２０に配置された撮像光学系１２１Ａ、１２１Ｂ及び撮像素子３１１Ａ、３１１Ｂを含むステレオカメラ３５０と、筐体１２０に固定された装着用のアタッチメント１１０と、を有している。
【００１５】
測距系１００は、撮像光学系１２１Ａ及び撮像光学系１２１Ａ用の撮像素子であるＣＭＯＳ（Complementary  Metal  Oxide  Semiconductor）３１１Ａにより撮像された撮像画像と、撮像光学系１２１Ｂ及び撮像光学系１２１Ｂ用の撮像素子であるＣＭＯＳ３１１Ｂにより撮像された撮像画像と、を用いた視差演算により測距を行う。
【００１６】
測距系１００は、シャフト２１、姿勢駆動部２００、ジンバル２０を介して移動体１０に取り付けられている（図１参照）。
【００１７】
詳述すると、筐体１２０に固定されたアタッチメント１１０は、シャフト２１の一端（−Ｚ側の端）に装着されている。シャフト２１の他端（＋Ｚ側の端）は、姿勢駆動部２００により保持されている。姿勢駆動部２００はジンバル２０を介して機体１１に接続されている。
【００１８】
ジンバル２０は、互いに直交する３つの揺動軸を持つ自由度３のジャイロであるジンバル機構を有し、測距系１００、シャフト２１及び姿勢駆動部２００を含む系を移動体１０に対して任意の方向に揺動自在に接続する。
【００１９】
ジンバル２０は、ジンバル機構に加えて、該ジンバル機構の各揺動軸の角速度を同時に検出する３軸ジャイロセンサ２０ａを有している。３軸ジャイロセンサ２０ａの検出結果すなわちジンバル機構の各揺度軸の傾斜情報は、姿勢駆動部２００に送られる。ジャイロセンサは、角度センサとも呼ばれる。
【００２０】
なお、ジンバルは、３軸ジャイロセンサの代わりに、各揺動軸の角速度を検出する３つの１軸ジャイロセンサを有していても良いし、２軸ジャイロセンサと１軸ジャイロセンサを組み合わせて各揺動軸の角速度を検出しても良い。
【００２１】
ステレオカメラ３５０の撮像光学系１２１Ａ、１２１Ｂの光軸を含む面は、シャフト２１に対して略垂直となっている（図４参照）。
【００２２】
姿勢駆動部２００は、３軸ジャイロセンサ２０ａの検出結果に基づいて、シャフト２１を介して測距系１００を駆動する。
【００２３】
詳述すると、姿勢駆動部２００は、移動体１０の移動（飛行）に伴うジンバル２０の動作による、移動体１０に対する測距系１００の相対的な姿勢変化（例えばＸ軸周りの回転、Ｙ軸周りの回転、Ｚ軸周りの回転等）をキャンセルするよう、すなわち測距系１００が移動体１０の移動に追従するようシャフト２１を駆動する。姿勢駆動部２００は、駆動源として小型のモータを有している。
【００２４】
図５は、第１実施形態の移動体装置１の構成の一例を示すブロック図である。図５に示されるように、測距系１００は、測距部３００と、制御部４００と、を備える。測距部３００はステレオカメラ３５０による測距に係る構成であり、制御部４００は、測距部３００の制御、並びにジンバル２０、移動体１０との通信に係る構成である。
【００２５】
図６は、第１実施形態の測距部３００のハードウェア構成の一例を示すブロック図である。撮像光学系１２１Ａを通過した被写体光は、ＣＭＯＳ３１１Ａに結像され、撮像光学系１２１Ｂを通過した被写体光は、ＣＭＯＳ３１１Ｂに結像される。ＣＭＯＳ３１１Ａは、撮像面に結像された光学像を電気信号に変換し、アナログの画像データとして出力し、ＣＭＯＳ３１１Ｂは、撮像面に結像された光学像を電気信号に変換し、アナログの画像データとして出力する。なお、撮像素子としてＣＭＯＳ３１１Ａ及びＣＭＯＳ３１１Ｂの代わりにＣＣＤ（Charge  Coupled  Device）を採用してもよい。
【００２６】
ＣＭＯＳ３１１Ａから出力された画像データは、ＣＤＳ（Correlated  Double  Sampling：相関２重サンプリング）回路３１２Ａによりノイズ成分を除去され、Ａ／Ｄ変換器３１３Ａによりデジタル値に変換され、画像処理回路３１４Ａに出力される。ＣＭＯＳ３１１Ｂから出力された画像データは、ＣＤＳ回路３１２Ｂによりノイズ成分を除去され、Ａ／Ｄ変換器３１３Ｂによりデジタル値に変換され、画像処理回路３１４Ｂに出力される。
【００２７】
なお、ＣＭＯＳ３１１Ａ、３１１Ｂ、ＣＤＳ回路３１２Ａ、３１２Ｂ、及びＡ／Ｄ変換器３１３Ａ、３１３Ｂは、タイミング信号発生器３１６により、動作するタイミングが制御されている。画像処理回路３１４Ａ、３１４Ｂは、それぞれ、画像データを一時格納するＳＤＲＡＭ（Synchronous  Dynamic  Random  Access  Memory）３１７を用いて、ＲＧＢ→ＹＣｒＣｂ変換処理、ホワイトバランス制御処理、コントラスト補正処理、エッジ強調処理、及び色変換処理などの各種画像処理を行う。なお、ホワイトバランス制御処理は、画像情報の色濃さを調整する画像処理であり、コントラスト補正処理は、画像情報のコントラストを調整する画像処理であり、エッジ強調処理は、画像情報のシャープネスを調整する画像処理であり、色変換処理は、画像情報の色合いを調整する画像処理である。
【００２８】
また、ＳＤＲＡＭ３１７に記憶された画像処理が施された画像データは、画像圧縮伸張回路３１８により圧縮され、メモリカード３１９に記録される。画像圧縮伸張回路３１８は、画像処理回路３１４Ａ、３１４Ｂから出力される画像データを圧縮してメモリカード３１９に出力するとともに、メモリカード３１９から読み出した画像データを伸張して画像処理回路３１４Ａ、３１４Ｂに出力する回路である。
【００２９】
タイミング信号発生器３１６、画像処理回路３１４Ａ、３１４Ｂ、画像圧縮伸張回路３１８、及びメモリカード３１９は、ＣＰＵ（Central  Processing  Unit）３１５により制御されている。ＲＯＭ（Read  Only  Memory）３２１は、ＣＰＵ３１５により実行されるプログラムなどを格納した読み出し専用メモリであり、ＲＡＭ（Random  Access Memory）３２２は、ＣＰＵ３１５が各種の処理過程で利用するワークエリア、及び各種データ格納エリアなどを有する読み出し書き込み自在のメモリである。
【００３０】
ＣＰＵ３１５は、画像処理回路３１４Ａにより各種画像処理が施された画像データ、及び画像処理回路３１４Ｂにより各種画像処理が施された画像データ、即ち、ステレオ画像を用いた視差演算を行って視差画像を生成し、生成した視差画像に基づいて、視差画像に写る物体までの距離を求める。なお、視差は、画素毎又は画素ブロック毎に算出され、視差画像では、画素又は画素ブロック毎に視差情報が対応づけられる。画素ブロックは、１以上の画素で構成されるブロックである。これにより、視差画像は、視差画像に写る物体（対象物）の３次元空間上での３次元位置（３次元座標）を表すことができる。測距部３００の主要な構成は、バスライン１９によって相互接続されている。
【００３１】
図７は、第１実施形態の制御部４００のハードウェア構成の一例を示すブロック図である。制御部４００は、ＣＰＵ４１１、ＲＡＭ４１２、フラッシュＲＯＭ４１３、Ｉ／Ｏ４１４、及びネットワークＩ／Ｆ４１５を備える。
【００３２】
ＣＰＵ４１１は、フラッシュＲＯＭ４１３に記憶されたプログラム４２０を実行して制御部４００の全体の動作を制御する。プログラム４２０は、メモリカード４１６に記憶された状態で配布されてもよいし、サーバから携帯電話網や無線ＬＡＮ（Local  Area  Network）網などの通信網を経由してダウンロードされることで配布されてもよい。ＲＡＭ４１２は、ＣＰＵ４１１がプログラムを実行する際のワークエリア（プログラムやデータが一時的に記憶される）として使用される。
【００３３】
ＣＰＵ４１１は、測距部３００の測距結果に基づいて、自己位置推定を行ったり、測距部３００に再測距を行わせたりする。Ｉ／Ｏ４１４は、Ｉ２ＣやＵＡＲＴなどの入出力インターフェースである。ネットワークＩ／Ｆ４１５は、イーサネット（登録商標）などの車載ネットワーク上で通信するための通信装置である。
【００３４】
図８は、第１実施形態の移動体１０のハードウェア構成の一例を示すブロック図である。移動体１０は、主に、制御装置５１９、ＧＰＳアンテナ５１２、ＧＰＳ受信器５２５、及びプロペラモータ５４５Ａ〜５４５Ｄから構成される。
【００３５】
ＧＰＳアンテナ５１２及びＧＰＳ受信器５２５は、移動体１０の絶対座標を測定する。なお、ＧＰＳアンテナ５１２及びＧＰＳ受信器５２５としては、後処理キネマティックＧＰＳ又はリアルタイムキネマティックＧＰＳ（ＲＴＫ−ＧＰＳ）が用いられるのが好ましい。ＲＴＫ−ＧＰＳは、高精度の測定が可能である。
【００３６】
プロペラモータ５４５Ａ〜５４５Ｄは、それぞれ、プロペラ１２〜１５を回転駆動するためのモータである。
【００３７】
制御装置５１９は、主に、制御演算部５２０、記憶部５２２、撮像制御部５２３、飛行制御部５２６、方位センサ５２７、ジャイロユニット５２８、及び通信部５２９を、備える。
【００３８】
記憶部５２２は、例えば、ＲＡＭやＲＯＭであり、プログラム格納部とデータ格納部とを含む。プログラム格納部には、ステレオカメラ３５０の撮像を制御する為の撮像プログラム、自律飛行する為の飛行制御プログラム、ステレオカメラ３５０の姿勢を制御する為の姿勢制御プログラム、及び飛行計画プログラムなどのプログラムが格納されている。データ格納部には、ステレオカメラ３５０で撮像した撮像データ、並びにＧＰＳアンテナ５１２及びＧＰＳ受信器５２５によって取得した絶対座標（地表座標）などのデータが格納される。
【００３９】
制御演算部５２０は、記憶部５２２に格納されたプログラムに基づいて、移動体１０の全体の動作を制御する。
【００４０】
ここで、ステレオカメラ３５０は、静止画及び動画を撮像可能であり、取得した画像をデジタル信号の画像データとして撮像制御部５２３に出力可能である。
【００４１】
撮像制御部５２３は、ステレオカメラ３５０による静止画や動画の撮像に関する制御を行う。撮像制御部５２３には、測距部３００から画素又は画素ブロック毎の距離情報、すなわち視差画像から得られた距離画像が送られる。撮像制御部５２３は、ステレオカメラ３５０からの画像データから輝度情報を得ることも可能である。
【００４２】
方位センサ５２７は、移動体１０の向きを検出し、その検出結果を制御演算部５２０に出力する。
【００４３】
ジャイロユニット５２８は、移動体１０の飛行状態での姿勢を検出し、その検出結果を制御演算部５２０に出力する。
【００４４】
また、３軸ジャイロセンサ２０ａの検出結果も制御演算部５２０に出力される。さらに、測距部３００の測定結果（距離画像）も制御演算部５２０に出力される。
【００４５】
制御演算部５２０は、測距部３００の測定結果及び３軸ジャイロセンサ２０ａの検出結果に基づいて、後述する演算処理を行い、その演算結果を飛行制御部５２６に出力する。
【００４６】
飛行制御部５２６は、測距系１００の制御部４００により推定された自己位置、方位センサ５２７により検出された移動体１０の向き、ジャイロユニット５２８により検出された移動体１０の姿勢、制御演算部５２０からの上記演算結果等に基づいて、プロペラモータ５４５Ａ〜５４５Ｄの駆動を制御することにより、移動体１０の自律飛行を制御する。
【００４７】
なお、飛行制御部５２６は、ＧＰＳアンテナ５１２及びＧＰＳ受信器５２５を含むＧＰＳ装置によって取得した絶対座標と制御演算部５２０からの上記演算結果を用いて、移動体１０の自律飛行を制御しても良い。
【００４８】
但し、ＧＰＳ装置は絶対位置を求めることができるものの、環境によって結果が不安定になったり、観測不能になったりすることがある。そのため、方位センサ２７、ジャイロユニット５２８等が配備されるメリットは大きい。
【００４９】
通信部５２９は、測距系１００と通信して、測距系１００により推定された自己位置を示す自己位置情報を取得したり、ステレオカメラ３５０で撮像した撮像データやＧＰＳアンテナ５１２及びＧＰＳ受信器５２５により測定された移動体１０の絶対座標を基地局などに無線送信したりする等の機能を有する。
【００５０】
［測距の原理］
従来から、例えばステレオカメラ、ミリ波レーダ、レーザレーダ等を含む測距系を備える測距装置が知られており、これらの測距装置の測距結果を用いて対象物までの距離を測定する技術が知られている（例えば、特許文献１（特開２００６−３１１０１号公報）参照）。
【００５１】
例えばステレオカメラを含む測距系は、計測対象である物体を２つのカメラで撮影して、得られた２つの画像間に生じる視差を利用して、三角測量の原理により物体までの距離を算出する。
【００５２】
視差を利用して距離を算出する原理を図９、図１０に基づいて説明する。図９では、計測対象物１０１を同一構成の光学系（レンズ、撮像素子）からなる２つのカメラ１０２ａ、１０２ｂを含むステレオカメラ１０６を用いて撮影している。レンズ１０３ａを通して得た計測対象物像１０４ａと、レンズ１０３ｂを通して得た計測対象物像１０４ｂは２次元撮像素子１０５ａ、１０５ｂ上にそれぞれ写る。このとき２次元撮像素子上で計測対象物の写る位置の違いが視差Δである。
【００５３】
ここでレンズ１０３ａ、１０３ｂの光軸間の距離は基線長と呼ばれ、これをＢとし、レンズと計測対象物との距離をＡ、レンズ１０３ａ、１０３ｂの焦点距離をｆとし、Ａ≫ｆであるときには次の（式０）が成り立つ。
Ａ＝Ｂｆ/Δ    ・・・（式０）
【００５４】
上記（式０）より、基線長Ｂ、およびレンズの焦点距離ｆは既知であるから、視差Δを検出すれば計測対象物までの距離Ａを算出することができる。上記（式０）からわかるように距離Ａが大きい（＝遠い）場合には視差Δは小さく、距離Ａが小さい（＝近い）場合には視差Δは大きくなる。ここで求められる距離Ａは基線長Ｂの方向に対して垂直方向の距離である。
【００５５】
図１０を用いて視差Δの求め方を説明する。計測対象物１０１は２次元撮像素子１０５ｂ上に写っている場合、２次元撮像素子１０５ａ上では２次元撮像素子１０５ｂと視差Δだけ異なる位置に写っている。どれくらい異なっているかを検出するために「パターンマッチング」と呼ばれる手法が用いられる。これは対象物が一方の画像（基準画像という。ここでは２次元撮像素子１０５ｂの画像）に対して、他方の画像（比較画像という。ここでは２次元撮像素子１０５ａの画像）内のどこに存在するかを探索することであり、この探索には、ＳＡＤ（Ｓｕｍ  ｏｆ  Ａｂｓｏｌｕｔｅ  Ｄｉｆｆｅｒｅｎｃｅ）法等の相関処理が一般的によく用いられている。
【００５６】
相関処理は、基準画像の中にあるブロック（例えば７×７画素）をテンプレートとして、比較画像上の探索範囲内でテンプレートと同サイズのウインドウを動かし、テンプレートと最も相関のあるブロックを決定する処理である。この同じ画像（ブロック）を探すときは、１画素ずつずらしながら１画素、２画素・・・と順に探すため、探索画素数を大きくするほど測定できる距離は拡大する。
【００５７】
一例として、基線長Ｂ＝１００ｍｍ、レンズ焦点距離１０ｍｍ、撮像素子の画素サイズ５μｍとした場合は、上記（式０）から計測対象物までの距離Ａと視差Δの関係は図１１のようになる。計測対象物までの距離Ａが１３００ｍｍでは視差は１５４画素、距離が１０００ｍｍでは視差は２００画素、距離７００ｍｍでは視差は２８６画素発生する。
【００５８】
以上のようにステレオカメラを用いて対象物までの距離を測定する測距装置は既に知られている。
【００５９】
［問題点］
ところで、現状のステレオカメラが搭載される移動体としての自律飛行する無人航空機は、単に自律飛行するだけではなく飛行しながらステレオカメラにより対象物の画像を撮影する役割も担っている。そのため、安定した画像が得られるようステレオカメラは、第１実施形態のように、ジンバルと姿勢駆動部を含むジンバル装置と呼ばれる映像安定化・追従装置を介して移動体に取り付けられることが一般的となっている。このジンバル装置により、移動体の細かい振動や急な方向転換などに対しても、カメラの向きを安定させることができ、画面ぶれのない見やすい画像を提供することができる。
【００６０】
しかしながら、逆に言うと、移動体の動きに対してステレオカメラは俊敏には追従していないことになる。ステレオカメラを含む測距系をジンバル装置を介して移動体に取り付けた場合、撮像手段としては安定した画像が得られる反面、測距手段としては本来移動体が向いている方向（進行方向）の距離を測るべきなのに、移動体が向いている方向とはやや異なるステレオカメラが向いている方向の距離を測ってしまうことになる。そのため移動体が方向転換したときなどはステレオカメラの向きが移動体の向きに遅れて追従するために、その間は移動体が向いている方向（進行方向）の距離ではなく、ステレオカメラが向いている方向の距離を測ってしまうことになる。
【００６１】
例えば室内のような狭い空間を高速に自律飛行するような移動体では、移動体の向きが変わってから、カメラの向きが移動体の向きに一致するまでの時間における測距誤差により移動体が周囲の物体に衝突することが懸念される。
【００６２】
［考察］
ジンバルは、一の機器を例えばＰｉｔｃｈｉｎｇ（ピッチング）、Ｒｏｌｌｉｎｇ（ローリング）、Ｙａｗｉｎｇ（ヨーイング）の直交する３軸周りに互いに回転自在に他の機器に接続するよう構成されている（２つの機器を２軸周りに回転自在に接続するジンバルもある）。
【００６３】
ジンバルを介してステレオカメラが移動体に取り付けられる場合、移動体に対するステレオカメラの各軸周りの振動などによる高速で細かい傾きを角度センサ（ジャイロセンサ）で検知して、検知した傾きを打ち消すことでステレオカメラの移動体に対する姿勢を一定に保つことができる。
【００６４】
この検知した傾きをステレオカメラで得られた視差にフィードバックすることで、ステレオカメラが向いている方向の距離ではなく移動体が向いている方向（進行方向）の距離を算出することができる。
その原理を、図１２を用いて説明する。
【００６５】
図１２（ａ）では移動体とステレオカメラが同じ方向を向いている場合、すなわち移動体の進行方向とステレオカメラの光軸方向が一致している場合を示している。ここでは、便宜上、「ステレオカメラの光軸」を該ステレオカメラの２眼（２つの撮像部）の光軸の中間の軸としている。この場合、移動体の前方に平面（物体の一面）があれば、ステレオカメラは移動体の進行方向と垂直な平面があることを検出する。このとき図１２（ａ）に示されるステレオカメラを含む測距部（以下では「ステレオ測距部」とも呼ぶ）で生成された距離画像は、平面全体で均一（一定の濃さ）となる。なお、「距離画像」は、ステレオ測距部で生成された視差画像から得られた画素毎の距離データを総合したものである。
【００６６】
一方、図１２（ｂ）のように移動体が紙面に垂直な軸（例えば鉛直軸）周りの一方向（例えば左周り）に急回転（急旋回）し移動体が角度θだけ傾いても、ジンバルにより安定に保たれているステレオカメラは急には傾かない。そのため、ステレオカメラは進行方向と垂直な平面があることを検出したままである。このとき図１２（ｂ）に示されるステレオ測距部で生成された距離画像は、図１２（ａ）の距離画像と同じである。
【００６７】
この場合、移動体から見ると平面は進行方向と垂直ではなく、θだけ傾いた状態で存在している。移動体がこのまま進行すると、移動体の平面に近い右側の部分が該平面に衝突することになる。
【００６８】
そこで、移動体が角度θだけ傾いたことを検出し、ステレオ測距部で生成された各距離データに角度θを加味する、すなわち該距離データを角度θを用いて補正する。
【００６９】
すると、図１２（ｃ）のようにステレオ測距部から出力された平面の距離データを、移動体から見た移動体の右端に近いほど距離が短く、左端に近いほど距離が長い距離データに補正することが可能である。このとき図１２（ｃ）に示されるステレオ測距部で生成された距離画像には、濃い部分ほど距離が短く、淡い部分ほど距離が長いことを示す濃淡のグラデーションがある。
【００７０】
移動体の急回転に対して、ステレオカメラは緩やかに追従する。移動体の急回転が終わると、図１２（ｄ）のように移動体の向きとステレオカメラの向きが一致し、図１２（ａ）と同様に移動体とステレオカメラは同じ方向を向くことになる。このとき図１２（ｄ）に示されるステレオ測距部で生成された距離画像は、図１２（ｃ）に示される距離画像と同じであり、図１２（ｃ）において距離データに角度θが加味されていることが確認できる。
【００７１】
このようにしてステレオカメラからの距離データに移動体の向きとステレオカメラの向きの違いを加味することでリアルタイムに移動体の向きの距離データを得ることができる。
【００７２】
なお、距離データは振動に対して安定ではなくなるが距離データを人が見て観察するわけではないため問題はない。ジンバルにより安定な撮影画像が得られるととともに、リアルタイムに距離画像が得られることで室内のような狭い場所でも移動体を周囲のものに衝突することなく精度よく飛行させることができる。
【００７３】
なお、ここでは、ステレオカメラの向きと移動体の向きが異なる場合の一例として、移動体が急回転する場合について説明したが、例えば移動体が急加速する場合や急減速する場合もステレオカメラの向きと移動体の向きが異なることが想定される。また、例えば移動体１０が振動する場合や移動体が緩やかに曲がる場合や緩やかに加速する場合や緩やかに減速する場合も、ステレオカメラの向きと移動体の向きが大なり小なり異なることが想定される。
【００７４】
［改善点］
以下では、簡単のために、移動体１０の向き（前進方向）が水平方向に一致し、かつ測距系１００、シャフト２１及び姿勢駆動部２００を含む系に重力のみが作用している状態をジンバル２０の初期状態と定義したときに、該初期状態において移動体１０の向きとステレオカメラ３５０の向きが一致していることを前提に説明を進める。
【００７５】
上述したように、ジンバル２０を介して移動体１０に取り付けられたステレオカメラ３５０は姿勢駆動部２００によって移動体１０の移動に遅れて（緩やかに）追従するため、移動体１０の進行方向とステレオカメラ３５０の光軸には一時的にずれ（傾斜）が生じる。
【００７６】
そこで、第１実施形態では、ジンバル２０の３軸ジャイロセンサ２０ａの検出結果が、移動体１０の向きに対するステレオカメラ３５０の向きの違い、すなわち移動体１０の進行方向に対するステレオカメラ３５０の光軸の傾斜情報（傾斜方向と傾斜角）を示すことに着目し、この傾斜情報を用いて測距部３００で測定された距離データ（距離情報）移動体１０から見た距離データに変換（補正）する。以下に、その方法を説明する。
【００７７】
先ず、移動体１０にジンバル２０を介して取り付けられたステレオカメラ３５０の２つの撮像画像（２つのＣＭＯＳ３１１Ａ、３１１Ｂによる撮像画像）から、ステレオカメラ３５０の光軸に平行な方向をｚ軸とした距離情報（ｘ、ｙ、ｚ）、すなわちステレオカメラ３５０の光軸に直交する平面内の２次元座標（ｘ、ｙ）と（ｘ、ｙ）でのステレオカメラ３５０からの距離ｚで表される、ステレオカメラ３５０から見た（カメラ座標系における）画素又は画素ブロック毎の距離情報を含む距離画像が得られる。
【００７８】
そして、３軸ジャイロセンサ２０ａの検出結果である、移動体１０の進行方向に対するステレオカメラ３５０の光軸の傾斜（以下では「カメラ傾斜」とも呼ぶ）がｘ軸まわりの角度αの回転である場合は、移動体１０から見た距離情報（ｘ´、ｙ´、ｚ´）は次の（式１）で表される。
【数１】
2017227516.tif 000003
【００７９】
また、カメラ傾斜がｙ軸まわりの角度βの回転である場合は、移動体１０から見た距離情報（ｘ´、ｙ´、ｚ´）は次の（式２）で表される。
【数２】
2017227516.tif 000004
【００８０】
また、カメラ傾斜がｚ軸まわりに角度γの回転である場合は、移動体１０から見た距離情報（ｘ´、ｙ´、ｚ´）は次の（式３）で表される。
【数３】
2017227516.tif 000005
【００８１】
また、カメラ傾斜がｘ軸、ｙ軸、ｚ軸まわりにそれぞれ角度α、β、γの回転（複合回転）である場合は、移動体１０から見た距離情報（ｘ’、ｙ’、ｚ’）は次の（式４）で表される。
【数４】
2017227516.tif 000006
【００８２】
以上の説明から分かるように、移動体１０にジンバル２０を介して取り付けられた測距系１００から得られる距離情報（ｘ、ｙ、ｚ）を、３軸ジャイロセンサ２０ａの検出結果から求めた、ステレオカメラ３５０の光軸と移動体の進行方向の傾斜角α、β、γを用いて、移動体１０から見た距離情報（ｘ´、ｙ´、ｚ´）すなわち移動体座標系における距離情報に変換することができる。この変換は、制御演算部５２０での演算処理で実施できる。そこで、測距系１００と３軸ジャイロセンサ２０ａと制御演算部５２０を含んで「測距装置１０００」が構成される。
【００８３】
これにより、ステレオカメラ３５０がジンバル２０を介して移動体１０に取り付けられている場合でも、常に移動体１０の向き（進行方向）を基準とした距離情報（ｘ´、ｙ´、ｚ´）をリアルタイムに得ることが可能になり、移動体１０を例えば室内のような狭い場所でも周囲のものに衝突することなく精度よく飛行させることができる。
【００８４】
なお、ジンバル２０の初期状態において移動体１０の向きに対してステレオカメラ３５０の向きが傾斜している場合には、その傾斜情報も加味して距離データを補正すれば良い。
【００８５】
以下に、第１実施形態の測距方法を実現する測距処理１について図１３を参照して説明する。図１３のフローチャートは、測距部３００と制御装置５１９によって実行される処理アルゴリズムに従う。
【００８６】
最初のステップＳ１では、移動体１０が移動中か否かを判断する。具体的には、ＧＰＳ受信器５２５等からの情報を基に移動体１０の移動の有無を判定する。ここでの判断が肯定されるとステップＳ２に移行し、否定されるとフローは終了する。
【００８７】
ステップＳ２では、ステレオカメラ３５０で画角内に入る物体を撮像する。
【００８８】
次のステップＳ３では、ステレオカメラ３５０の２つの撮像画像（ステレオ画像）から物体の視差画像を生成する。
【００８９】
次のステップＳ４では、視差画像から物体の距離画像を生成する。
【００９０】
次のステップＳ５では、移動体１０に対する測距系１００の相対的な姿勢変化に関する情報である姿勢変化情報（α、β、γ）を検出する。
【００９１】
次のステップＳ６では、姿勢変化情報（α、β、γ）を用いて距離画像を補正する。具体的には、距離画像の画素又は画素ブロック毎の距離情報に対して、上記式（１）〜式（４）のいずれかを用いた演算を行う。
【００９２】
補正後の距離画像は飛行制御部５２６に送られ、移動体１０の制御に用いられる。
【００９３】
以上説明した第１実施形態の測距装置１０００は、移動体１０に搭載される測距装置（装置）であって、移動体１０にジンバル２０を介して取り付けられ、対象物までの距離情報を測定する測距系１００と、移動体１０の移動に伴う測距系１００と移動体１０との相対的な姿勢変化に関する情報である姿勢変化情報を検出する検出系と、姿勢変化情報を用いて距離情報を補正する制御演算部５２０（補正系）と、を備えている。なお、「移動体１０の移動」は、移動体１０の前進移動、後退移動、上昇移動、下降移動、旋回移動、回転移動（姿勢変化）及び任意の方向の振動を含む。
【００９４】
この場合、移動体１０の移動に伴って測距系１００と移動体１０の相対的な姿勢が変化しているときの測距系１００の測定結果を、移動体１０から見た距離情報に補正（変換）できる。
【００９５】
結果として、測距装置１０００によれば、移動体１０を安全に移動させるための情報を得ることができる。
【００９６】
また、測距系はステレオカメラ法を用いることが好ましい。
【００９７】
この場合、ステレオカメラ３５０からジンバル２０による安定した輝度画像を得ることができるとともに、ステレオカメラ３５０からジンバル２０の安定効果を除去した（移動体１０から見た）リアルタイムの距離画像を得ることができる。すなわち、１つのステレオカメラ３５０を用いて画質の安定化と測距の信頼性向上を両立できる。
【００９８】
また、姿勢変化情報は、移動体１０の進行方向に対する測距系１００の光軸の傾きに関する情報であることが好ましい。
【００９９】
また、検出系は、移動体１０の移動に伴うジンバル２０の揺動軸の傾斜情報を計測する少なくとも１つのジャイロセンサを含むことが好ましい。
【０１００】
この場合、移動体１０の移動に伴う測距系１００の相対的な姿勢変化を制御するための制御情報として用いられるジンバル２０の揺動軸の傾斜情報から姿勢変化情報を容易に得ることができる。
【０１０１】
また、検出系は、移動体１０の姿勢を検出するジャイロユニット５２８（移動体姿勢検出部）と、測距系１００の姿勢を検出する測距系姿勢検出部（例えば少なくとも１つのジャイロセンサ）と、を含んでいても良い。
【０１０２】
この場合、移動体１０の姿勢に関する情報と測距系の姿勢に関する情報を用いて姿勢変化情報を求めることができる。すなわち、３軸ジャイロセンサ２０ａの検出結果を用いることなく、姿勢変化情報を求めることができる。
【０１０３】
具体的には、ジャイロユニット５２８の検出結果であるワールド座標系の各軸周りの回転角を（φ１、σ１、ε１）、測距系姿勢検出部の検出結果であるワールド座標系の各軸周りの回転角を（φ２、σ２、ε２）とすると、姿勢変化情報（α、β、γ）＝（φ２−φ１、σ２−σ１、ε２−ε１）となる。
【０１０４】
さらに、検出系は、移動体１０の進行方向を検出する方位センサ５２７（移動体進行方向検出部）を含むことが好ましい。
【０１０５】
この場合、姿勢変化情報をより精度良く求めることができる。
【０１０６】
また、測距系１００と、該測距系１００がジンバル２０を介して取り付けられる移動体１０と、を備える移動体装置１によれば、移動体を安全に移動させることができる移動体装置を提供できる。
【０１０７】
また、第１実施形態の測距方法は、第１の観点からすると、移動体１０にジンバル２０を介して取り付けられる測距系１００を用いる測距方法であって、測距系１００を用いて対象物までの距離情報を測定する工程と、移動体１０の移動に伴う測距系１００と移動体１０との相対的な姿勢変化に関する姿勢変化情報を検出する工程と、該姿勢変化情報に基づいて距離情報を補正する工程と、含む。
【０１０８】
この場合、移動体１０の移動に伴って測距系１００の移動体１０に対する姿勢が変化しているときの測距系の測定結果を、移動体１０から見た距離情報に補正できる。
【０１０９】
結果として、測距系１００によれば、移動体１０を安全に移動させるための情報を得ることができる。
【０１１０】
ここで、第１実施形態の測距系１００を、測距機能を持つ撮像装置と捉えることもできる。
【０１１１】
そこで、第１実施形態の測距機能を持つ撮像装置は、移動体１０に搭載される撮像装置（装置）であって、移動体１０にジンバルを介して取り付けられ、撮像光学系及び撮像素子（例えばＣＭＯＳ）をそれぞれが含む複数（例えば２つ）の撮像部を有するステレオカメラ３５０を含む撮像系と、複数の撮像部の撮像結果から視差画像を生成し、複数の撮像部で撮像された物体までの距離情報を算出するＣＰＵ３１５（演算手段）と、移動体１０の移動に伴う撮像系と移動体１０の相対的な姿勢変化に関する情報である姿勢変化情報を検出する検出系と、姿勢変化情報を用いて距離情報を補正する制御演算部５２０（補正手段）と、を備えている。
【０１１２】
この場合、移動体１０の移動に伴って撮像系と移動体１０の相対的な姿勢が変化しているときのＣＰＵ３１５の算出結果を、移動体１０から見た距離情報に補正（変換）できる。
【０１１３】
結果として、第１実施形態の撮像装置によれば、移動体１０を安全に移動させるための情報を得ることができる。
【０１１４】
また、第１実施形態の撮像装置は、複数の撮像部の撮像結果から輝度情報を得る撮像制御部５２３（輝度情報取得部）を更に備えることが好ましい。
【０１１５】
この場合、ステレオカメラ３５０からジンバル２０による安定した輝度画像を得ることができるとともに、ステレオカメラ３５０からジンバル２０の安定効果を除去した（移動体１０から見た）リアルタイムの距離画像を得ることができる。すなわち、１つのステレオカメラ３５０を用いて画質の安定化と測距の信頼性向上を両立できる。
【０１１６】
また、姿勢変化情報は、移動体１０の進行方向に対するステレオカメラ３５０の光軸の傾きに関する情報であることが好ましい。
【０１１７】
また、検出系は、移動体１０の移動に伴うジンバル２０の揺動軸の傾斜情報を検出する少なくとも１つのジャイロセンサを含むことが好ましい。
【０１１８】
この場合、移動体１０の移動に伴う測距系の相対的な姿勢変化を制御するための制御情報として用いられるジンバル２０の揺動軸の傾斜情報から姿勢変化情報を容易に得ることができる。
【０１１９】
また、検出系は、移動体１０の姿勢を検出する移動体姿勢検出部（例えば少なくとも１つのジャイロセンサ）と、ステレオカメラ３５０の姿勢を検出するステレオカメラ姿勢検出部（例えば少なくとも１つのジャイロセンサ）と、を含んでいても良い。
【０１２０】
この場合、移動体１０の姿勢に関する情報とステレオカメラ３５０の姿勢に関する情報を用いて姿勢変化情報を求めることができる。
【０１２１】
また、第１実施形態の撮像装置と、該撮像装置がジンバル２０を介して取り付けられる移動体１０と、を備える移動体装置によれば、この場合、移動体１０の移動に伴ってステレオカメラ３５０の移動体１０に対する姿勢が変化しているときのステレオカメラ３５０の測定結果を、移動体１０から見た距離情報に補正できる。
【０１２２】
結果として、第１実施形態の撮像装置によれば、移動体１０を安全に移動させるための情報を得ることができる。
【０１２３】
また、第１実施形態の測距方法は、第２の観点からすると、移動体１０にジンバル２０を介して取り付けられ、複数の撮像部を有するステレオカメラ３５０を含む撮像系を用いる測距方法であって、複数の撮像部の撮像結果から視差画像を生成し、複数の撮像部で撮像された物体までの距離情報を算出する工程と、移動体１０の移動に伴う撮像系と移動体１０の相対的な姿勢変化に関する情報である姿勢変化情報を検出する工程と、該姿勢変化情報を用いて距離情報を補正する工程と、を含む。
【０１２４】
この場合、移動体１０の移動に伴って撮像系と移動体１０の相対的な姿勢が変化しているときに算出された距離情報を、移動体１０から見た距離情報に補正（変換）できる。
【０１２５】
結果として、第１実施形態の測距方法によれば、移動体１０を安全に移動させるための情報を得ることができる。
【０１２６】
以上の説明から分かるように、発明者は、測距系や撮像系をジンバル２０を介して移動体１０に取り付けた場合でも常に移動体が向いている方向（進行方向）の距離を得ることができるようにすることで、安定した画像撮影と信頼性の高い測距を両立し、特に移動体１０を室内のような狭い空間でも衝突することなく正確かつ高速に移動させることができる技術として、第１実施形態を発案した。
【０１２７】
なお、上記第１実施形態では、姿勢変化情報（α、β、γ）等を用いて距離情報を補正する演算処理を移動体１０の制御演算部５２０が行っているが、これに限らず、例えば測距部３００のＣＰＵ３１５や制御部４００のＣＰＵ４１１が行っても良い。この場合も、姿勢変化情報（α、β、γ）等を測距部３００のＣＰＵ３１５や制御部４００のＣＰＵ４１１に入力する必要がある。
【０１２８】
以下に、本発明の別の実施形態を説明する。以下の実施形態では、主に上記第１実施形態とは異なる点を説明し、上記第１実施形態と同様の構成、機能を有する部材等には、同一の符号を付して詳細な説明を省略する。
【０１２９】
《第２実施形態》
上記第１実施形態では、ステレオカメラ３５０の向きと移動体の向きの違い（α、β、γ）を、３軸ジャイロセンサ２０ａの検出結果から求めたが、第２実施形態では、ステレオカメラ３５０のステレオ画像から求める。
【０１３０】
図１に示されるように機体１１は４本の足１６、１７、１８、１９で支えられている。測距系１００に用いられるステレオカメラ３５０のレンズの画角が広角の場合、ステレオカメラ３５０の前方にある足１６と足１７はステレオカメラ３５０の画角内に入る。
【０１３１】
そこで、足１６に２つのマーク５１、５２を、足１７に２つのマーク５３、５４を付しておき、ステレオカメラ３５０で撮像された画像（図１４（ａ））からマーク検出を行うようにすればステレオカメラ３５０と移動体１０の足１６、１７の向き、ステレオカメラ３５０と移動体１０の足１６、１７までの距離を検出することができる。
【０１３２】
足１６、足１７は移動体１０の動きに俊敏に追従して動くが、ステレオカメラ３５０を含む測距系１００はジンバル２０によって安定化されるため、移動体１０の動きに俊敏には追従しない。
【０１３３】
そのため、ステレオカメラ３５０の画角内に入る足１６と足１７のマークは移動体１０の動きにより小刻みに動くことになる。この動きを検知すればステレオカメラ３５０の向きと移動体１０の向きの違い（α、β、γ）を検知することができる。
【０１３４】
移動体１０の向きに対してステレオカメラ３５０の向きがＹ軸回りに回転（ピッチ）すれば、図１４（ｂ）のようにマークはＹ軸方向にΔＹだけずれる。移動体１０の向きに対してステレオカメラ３５０の向きがＺ軸回りに回転（ヨー）すれば、図１４（ｃ）のようにマークはＸ軸方向にΔＸ方向にずれる。
【０１３５】
さらに、ステレオカメラ３５０は、足１６、足１７までの距離を測定できるため移動体１０の向きだけではなく、ステレオカメラ３５０と移動体１０の相対位置（距離）も検出することができる。
【０１３６】
ステレオカメラ３５０と足１６と足１７の位置関係は初期状態（図１４（ａ）参照）ではある一定の関係となっている。そのためステレオカメラ３５０の画角内に入るマーク５１とマーク５３までの距離は初期状態では一定の値となっている。
【０１３７】
しかしながら、ジンバル２０の動作によりステレオカメラ３５０から足１６、足１７のまでの距離が変われば、ステレオカメラ３５０からマーク５１、５３までの距離が変わる。距離が近づけばマーク５１とマーク５３までの視差は大きくなり、距離が遠ざかればマーク５１とマーク５３までの視差は小さくなる。そこで、図１４（ａ）のＡの値より、ステレオカメラ３５０から足１６、足１７のまでの距離がわかることになる。ステレオカメラと足１６、１７までの距離が初期状態よりも遠いか近いかが判れば、移動体の周囲の対象物までの距離からその分を足すもしくは引くことで移動体１０から周囲の対象物までの距離を正確に測定することができる。
【０１３８】
以上のように、ステレオカメラ３５０の画角に入る機体１１の足の画像からステレオカメラ３５０の向きと移動体１０の向きの違いや距離の違いを検出できる。これにより３軸ジャイロセンサ２０ａの出力を用いるときよりもステレオカメラ３５０の位置がわかる分だけ正確に移動体からの距離情報をリアルタイムに得ることが可能になり、室内のような狭い場所でも周囲のものに衝突することなく精度よく飛行することができるようになる。
【０１３９】
なお、ここでは、移動体１０の足に付される「マーク」として略バタフライ形状のものが用いられているが、これに限らず、他の形状のものを用いても良い。また、マークが付される箇所は、移動体の足に限らず、要は、移動体の一部であってステレオカメラの画角内に入る部分であれば良い。
【０１４０】
以上説明した第２実施形態では、測距系１００は、複数の撮像部を有するステレオカメラ３５０を含む撮像系と、該複数の撮像部の撮像画像から視差画像を生成するＣＰＵ３１５（処理部）と、を含み、制御演算部５２０（検出系）は、視差画像に基づいて、移動体１０に対する撮像系の相対的な姿勢変化情報である姿勢変化情報（α、β、γ）を求める。なお、検出系として制御演算部５２０に代えて、測距部３００のＣＰＵ３１５や制御部４００のＣＰＵ４１１が姿勢変化情報（α、β、γ）を求めても良い。
【０１４１】
この場合、ステレオカメラ３５０を用いて姿勢変化情報を求めることができる。
【０１４２】
また、複数の撮像部は、移動体１０の少なくとも一部が画角内に入ることが好ましい。
【０１４３】
また、移動体１０の複数の撮像部の画角内に入る少なくとも一部には、少なくとも１つのマークが付されていること好ましい。
【０１４４】
なお、上記第１及び第２実施形態では、測距系や測距機能を持つ撮像系は、ステレオカメラ法を用いているが、ＴＯＦ法やパターン投影法を用いても良い。
【０１４５】
また、複数の撮像部は、移動体１０の少なくとも一部が画角内に入り、視差画像に基づいて、姿勢変化情報を求める工程を更に含むことが好ましい。
【０１４６】
この場合、ステレオカメラ３５０を用いて姿勢変化情報も求めることができる。具体的には、移動体１０に対するステレオカメラ３５０の姿勢変化後の撮像画像と姿勢変化前の画像（基準画像）を比較することにより、姿勢変化情報を求めることができる。
【０１４７】
なお、上記第２実施形態において、検出系は、ステレオカメラ３５０の撮影画像における移動体１０のステレオカメラ３５０の画角内に入る部分の画像と、予め取得された該部分の基準輝度画像のピクセル値の差分の総和を用いて、姿勢変化情報を求めても良い。
【０１４８】
この場合、単純なアルゴリズムでズレを計測することができる。
【０１４９】
また、上記第２実施形態において、検出系は、ステレオカメラ３５０の複数の撮像部の撮影画像から得られた移動体１０のステレオカメラ３５０の画角内に入る部分の視差画像と、予め取得された該部分の基準視差画像の視差値の差分の総和を用いて、姿勢変化情報を求めても良い。
【０１５０】
この場合、照明変化に対してロバストに、姿勢変化情報を計測することができる
【０１５１】
《第３実施形態》
以下に、本発明の第３実施形態の移動体装置として、ＴＯＦ法を用いる測距装置（もしくは撮像装置）を備える移動体装置について説明する。
【０１５２】
図１５には、第３実施形態の移動体１０´を含む移動体装置３の全体構成が概略的に示されている。
【０１５３】
移動体装置３は、図１５に示されるように、上記第１及び第２実施形態の移動体装置の測距系１００（もしくは測距機能を持つ撮像系）を測距・撮像ユニット６００で置き換えた構成を有している。
【０１５４】
図１６は、移動体１０´と測距・撮像ユニット６００のハードウェア構成の一例を示すブロック図である。
【０１５５】
測距・撮像ユニット６００は、図１５及び図１６に示されるように、レーザレーダ３５５を含む測距手段６００ａと、カメラ５２４（ここでは単眼カメラ）を含む撮像手段６００ｂを有している。
【０１５６】
測距手段６００ａと撮像手段６００ｂは、上下に並べて一体的に設けられている。ここでは、測距手段６００ａが上側で撮像手段６００ｂが下側とされているが、逆であっても良い。また、測距手段６００ａと撮像手段６００ｂは、それぞれ筐体を有していても良いし、共通の筐体を有していても良い。
【０１５７】
レーザレーダ３５５を用いて測距手段６００ａで生成された距離画像は、制御演算部５２０に送られる。
【０１５８】
カメラ５２４を用いて撮像手段６００ｂで生成された輝度画像は、撮像制御部５２３に送られる。なお、撮像手段６００ｂの構成は、図６に示されるステレオカメラ３５０を含む測距部３００の構成を単眼カメラ用に置き換えたもの（すなわち撮像光学系、ＣＭＯＳ等を１つずつにしたもの）に相当する。
【０１５９】
第３実施形態の姿勢駆動部５４１は、制御演算部５２０を介した３軸ジャイロセンサ２０ａの検出結果に基づいて、移動体１０´の移動に伴う測距・撮像ユニット６００の移動体１０´に対する姿勢変化をキャンセルするよう測距・撮像ユニット６００を駆動する。姿勢駆動部５４１の構成は、姿勢駆動部２００と同様である。
【０１６０】
図１７は、測距手段６００ａのハードウェア構成の一例を示すブロック図である。
【０１６１】
測距手段６００ａは、レーダレーダ３５５に加えて、測定制御部、時間測定回路、距離演算部を有している。
【０１６２】
レーザレーダ３５５は、ＬＤ、ＬＤからの光を導光する投光光学系及び測定制御部から発光開始信号が入力されたときにＬＤを点灯させるＬＤ駆動回路を含む投光系と、該投光系から投光され物体で反射された光を導光する受光光学系、該受光光学系を介した光を受光する受光素子）及び該受光素子の出力電流を処理する処理回路を含む受光系と、処理回路からの処理後の信号を二値化する二値化回路と、を備えている。なお、受光素子と処理回路を含んで光検出器が構成される。
【０１６３】
二値化回路からの二値化信号は、検出信号として時間測定回路に出力される。時間測定回路は、測定制御部からの発光開始信号と二値化回路からの検出信号に基づいて、ＬＤの発光タイミングと受光素子の受光タイミングの時間差を算出し、距離演算部に出力する。距離演算部は、時間計測回路からの時間差から物体までの距離を算出する。
【０１６４】
図１８（ａ）及び図１８（ｂ）には、レーザレーダ３５５の投光系及び受光系の構成が示されている。
【０１６５】
レーザレーダ３５５は、投光系が、ＬＤからの光を有効走査領域に向けて偏向走査する走査ミラーを有している。
【０１６６】
ここでは、この走査ミラーにＭＥＭＳ機構でミラー部を駆動するＭＥＭＳミラーを用いているが、モータによって多面鏡を回転させるポリゴンミラー、その他ガルバノミラーなど種々の変形が可能である。
【０１６７】
ここでは、ＭＥＭＳミラーによって走査される光（走査光）を検出する光検出部（例えばフォトダイオードやフォトトランジスタ）が有効走査領域を含む投光範囲（検出領域）に設けられ、測定制御部は、光検出部の出力に基づいて走査光の位置（走査位置）を検出し、該走査位置に基づいて投光系のＬＤを制御する。なお、ＭＥＭＳミラーにミラー部の振れ角を検知する振れ角検知部が設けられる場合には、測定制御部は該振れ角検知部の出力に基づいて走査位置を検出しても良い。
【０１６８】
また、走査する構成としては、投光系、受光系をＺ軸方向に重ねた構成にしており、ともに図１８のようにＺ軸方向から見たときに同軸で走査ミラーに入射して走査することにより、投光系で光を投射している領域と、受光系で光を取り込む領域を一致させ、安定した物体検出、測距を実現している。
【０１６９】
第３実施形態の変形例として、投光系においてのみ走査ミラーで走査し、受光系は走査ミラーを介さず、有効走査領域全体を結像光学系によって観察する構成もとりうる。
【０１７０】
この場合、受光系において走査ミラーの設置スペースが不要となり、投光系において走査ミラーのサイズを小さくすることによって、高速駆動や広角駆動とすることも可能である。
【０１７１】
図１８では、座標軸は、有効走査領域がＹＺ平面となるようにとっており、走査ミラーは、図１８では図示されていないが、Ｙ軸方向とＺ軸方向の２軸方向に独立して走査可能な構成とされている。走査ミラーをポリゴンミラーなどで代用する場合は、複数の反射面を回転軸に対してそれぞれ異なった角度で傾けて配置して、走査・検出する領域をＺ軸方向に切り替えることも可能である。
【０１７２】
以下に、第３実施形態の測距方法を実現する測距処理２について図１９を参照して説明する。図１９のフローチャートは、測距・撮像ユニット６００と制御装置５１９によって実行される処理アルゴリズムに従う。
【０１７３】
最初のステップＳ１１では、移動体１０が移動中か否かを判断する。具体的には、ＧＰＳ受信器５２５等からの情報を基に移動体１０の移動の有無を判定する。ここでの判断が肯定されるとステップＳ２に移行し、否定されるとフローは終了する。
【０１７４】
ステップＳ１２では、レーザレーダ３５５を用いて有効走査領域に存在する物体の距離画像を生成する。
【０１７５】
次のステップＳ１３では、移動体１０に対する測距・撮像ユニット６００の相対的な姿勢変化に関する情報である姿勢変化情報を３軸ジャイロセンサ２０ａの検出結果（α、β、γ）から得る。
【０１７６】
次のステップＳ１４では、姿勢変化情報を用いて距離画像を補正する。具体的には、距離画像の画素又は画素ブロック毎の距離情報に対して、上記式（１）〜式（４）のいずれかを用いた演算を行う。
【０１７７】
補正後の距離画像は飛行制御部５２６に送られ、移動体１０´の制御に用いられる。
【０１７８】
なお、一例として図２０に示されるＴＯＦ（Ｔｉｍｅ−Ｏｆ−Ｆｌｉｇｈｔ）法を用いる測距系の変形例のように、投光系１３１は、所定の周波数で強度変調された光を射出しても良い。ここでは、受光系１３２は、１つの位相差計測可能な２次元撮像素子と撮像光学系を備えている。このとき、受光系１３２は、撮像対象物で反射されて位相がずれた光を撮像する。そして、演算部１３３は、投光系１３１から射出された光と、受光系１３２で撮像された光とを比較し、時間差もしくは位相差に基づいてデプスマップを求める。
【０１７９】
また、上記第１〜第３実施形態では、姿勢駆動部によって、測距系や撮像系の姿勢を制御して移動体の向きに追従させているが、移動体から見た距離画像を精度良く生成するという観点からは、姿勢駆動部は必須ではない。すなわち、移動体の向きと測距系や撮像系の向きが継続的に又は常時変化しても、姿勢変化情報（α、β、γ）を用いて距離画像を補正することにより、移動体から見た距離画像を精度良く生成できるからである。この場合、３軸ジャイロセンサ２０ａも必須ではない。すなわち、本発明の装置において、ジンバルは、ジンバル機構のみで構成することも可能である。
【０１８０】
また、上記第１〜第３実施形態では、測距系にステレオカメラやレーダレーダを用いているが、これに限らず、例えばミリ波レーダ、赤外線センサを用いることもできる。
【０１８１】
また、レーザレーダに限らず、要は、ＴＯＦを利用するライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ  ｄｅｔｅｃｔｉｎｇ  ａｎｄ  ｒａｎｇｉｎｇ）全般（光源がレーザでないものも含む）を測距系に用いることができる。
【０１８２】
また、測距系や測距機能を持つ撮像系は、パターン投影法を用いても良い。
【０１８３】
一例として図２１には、パターン投影法を用いる測距系の構成例が示されている。ここでは、投光系１３１は、ある構造化された光を射出する。なお、構造化された光とは、Ｓｔｒｕｃｔｕｒｅｄ  Ｌｉｇｈｔ法として知られている方法に適した光であって、例えば、ストライプ状の光やマトリックス状の光などがある。受光系１３２は、撮像対象物で反射されて変形した構造化された光を撮像する。そして、演算部１３３は、投光系１３１から射出された光と、受光系１３２で撮像された光とを比較し、三角測量法に基づいてデプスマップを求める。ここでは、受光系１３２の焦点距離を、便宜上、測距系の焦点距離と呼ぶ。
【０１８４】
また、測距系や測距機能を持つ撮像系は、同種又は異種の複数の測距部を有していても良い。
【０１８５】
例えば、撮影用のカメラ（単眼カメラもしくはステレオカメラ）と測距用のステレオカメラを併設しても良い。
【０１８６】
また、測距系や撮像系は、複数の撮像部を有していれば良く、例えば単眼カメラを２つ組み合わせたものであっても良い。
【０１８７】
また、少なくとも１つの測距系と少なくとも１つの測距機能を持つ撮像系を含む複数の系を、それぞれ複数のジンバルを介して移動体に取り付けても良い。
【０１８８】
また、上記第１〜第３実施形態の測距系や撮像系を、自律して移動可能なロボット、車両、船舶、有人航空機等の移動体にジンバルを介して取り付けても良い。この場合も、上記第１〜第３実施形態と同様の効果が得られる。
【０１８９】
そして、移動体と測距系や測距機能を持つ撮像系を接続するジンバルは、自由度３のジンバルに限らず、自由度２もしくは自由度１のジンバルであっても良い。自由度２のジンバルを用いる場合には互いに直交する２つの揺動軸の角速度を検出する少なくとも１つのジャイロセンサを設ければ良く、自由度１のジンバルを用いる場合には１つの揺動軸の角速度を検出する１つのジャイロセンサを設ければ良い。
【０１９０】
また、上記各実施形態の説明で用いた数値、形状等は、本発明の趣旨を逸脱しない範囲で適宜変更可能である。
【０１９１】
以下に、発明者が上記第１〜第３実施形態を発案した思考プロセスを説明する。
【０１９２】
先ず、特許文献１（特開２０１４−６２７８９号公報）について説明する。特許分文献１では、撮影用のカメラとステレオカメラとをジンバルを介して移動体に取り付けることで、撮影用のカメラで農作物の包括的な観察を行い、ステレオカメラで農作物の局部観察を行うことを目的としている。
【０１９３】
この目的を達成するために、移動体に取り付けた撮影用のカメラを用いて広範囲の農作物を包括的に観察し、広範囲の中で特に成長状態などを詳細に観察したい農作物については移動体をその上方で静止（ホバリング）させて、シャフトを伸ばしてステレオカメラを近づけ、農作物の高さを計測して成長状態を観察して局部観察を行っている。このように撮影用のカメラとステレオカメラとをジンバルを介して移動体に取り付けることが開示されている。
【０１９４】
しかしながら、特許文献１ではステレオカメラは農作物の観察に使われているだけで、移動体が自律飛行するために周囲の物体までの距離を計測することには使われていない。
【０１９５】
このため、特に室内のような狭い空間を自律飛行する移動体では、ジンバルの追従遅れの時間による測距誤差により周囲の物体に衝突してしまうという課題がある。
【０１９６】
移動体が周囲の物体に衝突せずに自律飛行するためには３次元計測装置（具体的にはステレオカメラ）を用いて周囲にある物体までの距離をリアルタイムに計測しながら飛行する必要がある。周囲の物体の距離を測定するためにはステレオカメラの画角はできるだけ広画角にして、広範囲の物体の距離測定ができることが望ましい。
【０１９７】
しかしながら、先述したように映像安定化・追従装置であるジンバル装置にステレオカメラを取り付けると安定した画像が得られる反面、移動体の動きに対してカメラがリアルタイムには追従しなくなる。移動体が向いている方向（進行方向）ではなく、カメラが向いている方向の距離を測ってしまうことになる。
【０１９８】
そこで、発明者は、鋭意検討の結果、ステレオカメラを含む測距部で得られた視差にステレオカメラの向きと移動体の向きの違いを加味して距離を算出する方法を見出した。例えばジンバルの揺動軸の傾きをリアルタイムに検出しその検出結果を用いて距離情報を算出することで、ジンバルを介して移動体に取り付けられたステレオカメラを用いてもリアルタイムに正確な距離を出力できる。これにより、移動体が周囲の物体に衝突せずに自律飛行することができる。
【符号の説明】
【０１９９】
１、３…移動体装置、１０、１０´…移動体、２０…ジンバル、２０ａ…３次元ジャイロセンサ（検出系）、１００…測距系、３１５…ＣＰＵ（処理部）、６００ｂ…撮像系、５２０…制御演算部（補正系）、５２３…撮像制御部（輝度情報取得部）、５２８…ジャイロユニット（移動体姿勢検出部）、６００…測距・撮像ユニット（測距系）、１０００…測距装置（装置）。
【先行技術文献】
【特許文献】
【０２００】
【特許文献１】特開２０１４−６２７８９号公報
【図１】
2017227516.tif 000007
【図２】
2017227516.tif 000008
【図３】
2017227516.tif 000009
【図４】
2017227516.tif 000010
【図５】
2017227516.tif 000011
【図６】
2017227516.tif 000012
【図７】
2017227516.tif 000013
【図８】
2017227516.tif 000014
【図９】
2017227516.tif 000015
【図１０】
2017227516.tif 000016
【図１１】
2017227516.tif 000017
【図１２】
2017227516.tif 000018
【図１３】
2017227516.tif 000019
【図１４】
2017227516.tif 000020
【図１５】
2017227516.tif 000021
【図１６】
2017227516.tif 000022
【図１７】
2017227516.tif 000023
【図１８】
2017227516.tif 000024
【図１９】
2017227516.tif 000025
【図２０】
2017227516.tif 000026
【図２１】
2017227516.tif 000027