JP7440272B2 - 飛行体、３次元位置・姿勢計測装置、及び３次元位置・姿勢計測方法 - Google Patents

Description

本発明は、飛行体、３次元位置・姿勢計測装置、及び３次元位置・姿勢計測方法に関する。

従来から、飛行体に撮像装置を取り付けて当該撮像装置によって建物の外壁等を撮影し、生成された画像を合成して当該建物の全体像を把握することが行われている。画像の合成処理においては、撮像時の飛行体の３次元位置及び姿勢の情報が必要となる。そのために、飛行体の３次元位置及び姿勢を測定する技術が考案されている。

例えば、ＧＰＳ信号受信機や測距センサ等を飛行体に搭載することにより、飛行体の３次元位置及び姿勢を測定する技術が提案されている。また、例えば、飛行体に複数の光源を取り付けておき、固定された２次元イメージセンサによって当該飛行体を撮影して生成された画像に含まれる光源の位置に基づいて飛行体の３次元的な位置及び姿勢を計測する方法が提案されている（例えば、特許文献１）。

特開２００３－２５４７１６号公報

しかしながら、例えばボイラー炉の内部等の空間においては、ＧＰＳ信号の受信が困難な場合や、センサの搭載により大型化した飛行体が進入することが困難な場合がある。また、飛行体に取り付けられた光源の位置のみに基づいて当該飛行体の３次元位置・姿勢を計測する場合、多数の光源が必要になる。

そこで、本発明は、ＧＰＳ信号の受信が困難な環境や、大型の装置が進入することの困難な狭い環境においても、少ない数の光源によって３次元位置・姿勢を把握することの可能な飛行体と、当該飛行体の３次元的位置・姿勢を計測する３次元位置・姿勢計測装置及び３次元位置・姿勢計測方法を提供することを目的とする。

本発明の一態様に係る３次元位置・姿勢計測装置は、少なくとも１つの光源を有する飛行体の空間的な位置及び姿勢を算出する３次元位置・姿勢計測装置であって、飛行体を含むシーンを撮像することにより画像を生成する撮像部と、撮像部により生成された画像から、少なくとも１つの光源の発光態様を抽出する抽出部と、少なくとも１つの光源の画像における２次元座標と、抽出部により抽出された発光態様に基づいて、飛行体の３次元位置及び姿勢を算出する算出部と、を備える。

この態様によれば、ＧＰＳ信号の受信が困難な環境や、大型の装置が進入することの困難な狭い環境においても、少ない数の光源によって飛行体の３次元位置・姿勢を把握することが可能となる。そして、飛行体の３次元位置・姿勢の把握が可能となることにより、飛行体に設けられた撮像装置によって生成された画像が、観察対象である大型設備等のうちのどの部分をどの向きで撮像した画像であるのかを把握可能となり、以て大型設備等の観察が可能となる。

本発明によれば、少ない数の光源によって３次元位置・姿勢を把握することの可能な飛行体と、当該飛行体の３次元的位置・姿勢を計測する３次元位置・姿勢計測装置及び３次元位置・姿勢計測方法を提供することができる。そして、飛行体の３次元位置・姿勢の把握が可能となることにより、飛行体の飛行制御や、飛行体に設けられた撮像装置により大型設備等の画像診断等が可能となる。

第１実施形態に係る３次元位置・姿勢計測システム１の構成の一例を示す概略図である。 第１実施形態に係る飛行体１０ａの構成の一例を示すブロック図である。 第１実施形態に係る３次元位置・姿勢計測装置２０の構成の一例を示すブロック図である。 参照情報の一例について説明するための模式的な図である。 第１実施形態に係る３次元位置・姿勢計測システム１による飛行体１０ａの３次元位置・姿勢計測方法の動作フローの一例を示す図である。 カメラ２１ａ及び光源１３の位置関係の一例を模式的に示す図である。 第２実施形態に係る３次元位置・姿勢計測システム２の構成の一例を示す概略図である。 第２実施形態に係る飛行体１０ａの構成の一例を示すブロック図である。 第２実施形態に係る３次元位置・姿勢計測装置２０の構成の一例を示すブロック図である。 第２実施形態に係る３次元位置・姿勢計測システム２による飛行体１０ａの３次元位置・姿勢計測方法の動作フローの一例を示す図である。 カメラ２１ａ及び光源１３Ａ、１３Ｂの位置関係の一例を模式的に示す図である。 第３実施形態に係る３次元位置・姿勢計測システム３の構成の一例を示す概略図である。 第３実施形態に係る飛行体１０ａの構成の一例を示すブロック図である。 第３実施形態に係る３次元位置・姿勢計測装置２０の構成の一例を示すブロック図である。 第３実施形態に係る３次元位置・姿勢計測システム２による飛行体１０ａの３次元位置・姿勢計測方法の動作フローの一例を示す図である。 カメラ２１ａ及び光源１３Ａ、１３Ｂ、１３Ｃの位置関係の一例を模式的に示す図である。

添付図面を参照して、本発明の好適な実施形態について説明する。（なお、各図において、同一の符号を付したものは、同一又は同様の構成を有する。）

［第１実施形態］
以下では、第１実施形態に係る３次元位置・姿勢計測システム１について説明する。第１実施形態に係る３次元位置・姿勢計測システム１においては、飛行体１０ａは、飛行体１０ａの姿勢に応じた発光態様で発光する１つの光源を備える。第１実施形態に係る３次元位置・姿勢計測システム１では、カメラ２１ａが飛行体１０ａを撮像して得られた画像における光源１３の２次元座標及び輝度に基づいて、飛行体１０ａの３次元位置が算出される。また、当該画像に対して時系列的に前後する複数の画像に基づいて光源１３の発光態様が抽出された上で、当該発光態様に基づいて飛行体１０ａの３次元姿勢が算出される。

（１）構成
（１－１）３次元位置・姿勢計測システム１
図１は、第１実施形態に係る３次元位置・姿勢計測システム１の構成の一例を示す概略図である。３次元位置・姿勢計測システム１は、例えば、飛行体１０ａと、操縦装置１０ｂと、３次元位置・姿勢計測装置２０とを、備える。

飛行体１０ａには１つの光源１３が設けられている。飛行体１０ａは、操縦装置１０ｂから受信した操縦信号や撮像信号に応じた動作を行う。操縦装置１０ｂは、遠隔的に飛行体１０ａを操縦するための情報処理装置であり、例えば、メモリ、通信回路、及びプロセッサ等を含んで構成される。操縦装置１０ｂは、操作者による操作を受け付けると、当該操作に応じた操縦信号や撮像信号を例えば無線通信により飛行体１０ａに送信する。

３次元位置・姿勢計測装置２０は、飛行体１０ａの３次元位置及び姿勢を測定する装置であり、例えば、カメラ２１ａと、制御ユニット２１ｂとを備える。カメラ２１ａは、飛行体１０ａを含むシーンを撮像することにより画像を生成し、当該画像を制御ユニット２１ｂに供給する。制御ユニット２１ｂは、カメラ２１ａから供給された画像を解析して、当該画像における光源１３の２次元座標や発光態様に基づいて、飛行体１０ａの３次元位置及び姿勢を算出する。

（１－２）飛行体１０ａ
図２は、第１実施形態に係る飛行体１０ａの構成の一例を示すブロック図である。

図２に示すように、飛行体１０ａは、４つのモータ１１、カメラ１２、光源１３、制御ユニット１４等を備える。制御ユニット１４は、制御部１４０、メモリ１４１、モータコントローラ１４２、通信部１４３、姿勢センサ１４４、及びバッテリ１４５等を含む。

４つのモータ１１は、それぞれの出力軸に取り付けた回転翼を個別に回転駆動するモータである。各モータ１１はプロペラに接続されており、モータ１１が回転することによりプロペラが浮力を生じさせ、これにより飛行体１０ａが飛行可能となる。

カメラ１２は、ＣＣＤ（Charge-Coupled Device）やＣＭＯＳ（Complementary MOS）などのイメージセンサを備え、カメラ制御部１４０ｂからの制御データに基づいてイメージセンサにより対象物（例えば、大型設備の外壁等）を撮像し、撮像により得た画像を制御部１４０に出力する。

光源１３は、ＰＷＭ（Pulse Width Modulation）信号により駆動するＬＥＤ（Light Emitting Diode）発光部を有する。なお、光源１３は、ＬＥＤの他に、キセノンランプなどの他の発光部を用いてもよい。光源１３は、照明の明るさを設定する設定レジスタを有し、設定レジスタに設定された明るさにＰＷＭ信号のパルス幅を調節してＬＥＤ発光部を駆動する。設定レジスタに対する明るさの設定は、制御部１４０が行うものとし、パルス幅を調節する制御データを設定する。また、発光部の明るさの可変方式は、ＰＷＭ信号の他に、例えば発光部に流れる電流値を可変させるなど、適宜変形してもよい。

制御部１４０は、ＣＰＵ（Central Processing Unit）等により構成されるマイクロコンピュータを有する。制御部１４０は、メモリ１４１に記憶されている制御プログラムを実行することにより、飛行体１０ａの各部に制御データを出力して、飛行体１０ａ全体を統括的に制御する。制御部１４０は、例えば、モータ制御部１４０ａと、カメラ制御部１４０ｂと、光源制御部１４０ｃとを備える。

モータ制御部１４０ａは、通信部１４３が受信した操縦装置１０ｂからの操縦信号に対応する制御データをモータコントローラ１４２へ出力して各モータ１１を制御する。これにより、モータ制御部１４０ａは、飛行体１０ａの飛行に関する制御を行う。

カメラ制御部１４０ｂは、通信部１４３が受信した操縦装置１０ｂからの撮像信号に対応する制御データをカメラ１２へ出力して、カメラ１２の撮像に関する制御を行う。
カメラ制御部１４０ｂは、カメラ１２から出力された画像を所定形式に変換してメモリ１４１に保存する。

光源制御部１４０ｃは、飛行体１０ａの３次元姿勢を示す姿勢センサ１４４の出力をモニタし、それらの出力値に対応した発光態様で発光するように光源１３に制御データを出力して、光源１３の発光に関する制御を行う。

具体的には、光源制御部１４０ｃは、メモリ１４１に記憶された参照情報を参照して、姿勢センサ１４４の出力に基づき、飛行体１０ａの３次元姿勢であるＸ軸周りの回転角度α、Ｙ軸周りの回転角度β、及びＺ軸周りの回転角度γに対応する色情報や点滅周波数等の発光態様で、光源１３を発光させる。例えば、光源制御部１４０ｃは、回転角度αに対応する色情報Ｃ１、回転角度βに対応する色情報Ｃ２、及び回転角度γに対応する色情報Ｃ３で、光源１３を所定期間ずつ順次に発光させてもよい。また、光源制御部１４０ｃは、回転角度αに対応する点滅周波数ｆ１、回転角度βに対応する点滅周波数ｆ２、及び回転角度γに対応する点滅周波数ｆ３で、光源１３を所定期間ずつ順次に発光させてもよい。

メモリ１４１は、ＲＯＭやＲＡＭ等により構成され、ドライバプログラム、オペレーティングシステムプログラム、及びアプリケーションプログラム等の各種のプログラムや、各種のデータを記憶する。メモリ１４１は、例えば、制御部１４０がモータ制御部１４０ａ、光源制御部１４０ｃ、及びカメラ制御部１４０ｂ等を実現するためのプログラムを記憶する。また、メモリ１４１は、例えば、カメラ制御部１４０ｂから出力された画像を記憶する。また、メモリ１４１は、例えば、光源制御部１４０ｃが光源１３の発光を制御する際に参照する参照情報を記憶する。参照情報については後述する。メモリ１４１が記憶する各種のプログラムは、例えばＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ等のコンピュータ読取可能な可搬型記録媒体から、公知のセットアッププログラム等を用いてメモリ１４１にインストールされてもよい。

モータコントローラ１４２は、制御部１４０のモータ制御部１４０ａからの制御データに基づいて４つのモータ１１の回転数を個別に制御する。

通信部１４３は、ＷｉＦｉやＢｌｕｅｔｏｏｔｈ（登録商標）などの通信方式により無線通信を行う無線通信機である。例えば、通信部１４３は、電波を受信し、受信電波から受信データを復調して受信データ内の操縦信号や撮像信号に関するデータを制御部１４０に出力する。

姿勢センサ１４４は、例えば、３軸ジャイロセンサ等の角速度センサによって構成され、飛行体１０ａの３次元姿勢を検知し、検知結果を制御部１４０に出力する。ここで、姿勢センサ１４４が検知する飛行体１０ａの３次元姿勢は、例えば、Ｘ軸周りに回転角度αだけ回転させ、次にＹ軸周りに回転角度βだけ回転させ、次にＺ軸周りに回転角度γだけ回転させた場合の３次元姿勢として規定される。

バッテリ１４６は、リチウムイオン電池などであり、飛行体１０ａの各部に駆動電力を供給する。

（１－３）３次元位置・姿勢計測装置２０
図３は、第１実施形態に係る３次元位置・姿勢計測装置２０の構成の一例を示すブロック図である。

３次元位置・姿勢計測装置２０は、例えば、カメラ２１ａと、制御ユニット２１ｂ内に設けられた通信部２２と、メモリ２３と、制御部２４とを備える。

カメラ２１ａは、ＣＣＤ（Charge-Coupled Device）やＣＭＯＳ（Complementary MOS）などのイメージセンサを備える。カメラ２１ａは、カメラ制御部２５から供給される制御データに基づいて、当該イメージセンサにより所定のフレームレートによって飛行体１０ａを撮像して画像を生成し、メモリ２３に出力する。カメラ２１ａの画像生成におけるフレームレートは、光源１３の点滅周波数が把握可能ように十分大きいものであってよい。

通信部２２は、ＷｉＦｉやＢｌｕｅｔｏｏｔｈ（登録商標）などの通信方式により無線通信を行う無線通信機である。通信部２２は、飛行体１０ａを含む他の装置等との間で情報を送受信する。

メモリ２３は、ＲＯＭやＲＡＭ等により構成され、ドライバプログラム、オペレーティングシステムプログラム、及びアプリケーションプログラム等の各種のプログラムや、各種のデータを記憶する。また、メモリ２３は、例えば、カメラ２１ａが生成した画像を時系列的に記憶する。また、例えば、メモリ２３は、画像に基づいて飛行体１０ａの３次元位置や姿勢を算出するための各種の参照情報を記憶する。当該参照情報は、例えば、３次元姿勢算出部２７３が、解析対象の動画像から抽出された光源１３の発光態様に基づいて、飛行体１０ａの姿勢を算出する際に参照される。参照情報については後述する。

制御部２４は、ＣＰＵ（Central Processing Unit）等により構成されるマイクロコンピュータを有する。制御部２４は、メモリ２３に記憶されている制御プログラムを実行することにより、３次元位置・姿勢計測装置２０の各部に制御データを出力して、３次元位置・姿勢計測装置２０全体を統括的に制御する。

カメラ制御部２５は、制御データをカメラ２１ａへ出力して、カメラ２１ａの撮像に関する制御を行う。カメラ制御部２５は、カメラ２１ａから出力された画像にタイムスタンプを付加し、所定形式に変換してメモリ２３に保存する。

画像処理部２６は、メモリ２３に保存された画像から所定の画像を抽出し、当該画像を処理することにより、３次元位置・姿勢計測のための種々のパラメータを算出・抽出する。画像処理部２６は、例えば、光源特定部２６１、及び発光態様抽出部２６２を含む。

光源特定部２６１は、解析対象の画像から光源１３を特定する。光源特定部２６１が光源１３を特定する方法は特に限定されないが、例えば以下のように光源１３を特定してもよい。すなわち、光源特定部２６１は、画像に含まれる各画素の輝度及び／又は色情報（色差等）を参照して、輝度及び／又は色情報（色差等）が所定の閾値以上である画素を、光源１３に対応する画素であると特定してもよい。また、光源特定部２６１は、画像中の任意の領域（略円状、略矩形等の形状を有する領域であってよい）に含まれる複数の画素のうちの所定の割合以上について、輝度及び／又は色情報（色差等）が所定の閾値以上である場合に、当該領域を光源１３に対応する画素（領域）であると特定してもよい。そして、特定された画素（領域）の位置に応じて、光源１３の２次元座標を決定する。

発光態様抽出部２６２は、メモリ２３から解析対象の画像を含む所定期間（飛行体１０ａが移動していないと見なせる程度に十分に短い期間であってよい）内に撮像された複数の画像を抽出し、当該複数の画像から光源１３の発光態様を抽出する。ここで、発光態様は、光源の色情報や点滅周波数等によって規定される。例えば、発光態様は、光源１３の色情報が、色情報Ｃ１、色情報Ｃ２、及び色情報Ｃ３と順次に変化することであってもよい。また、例えば、発光態様は、光源１３による点滅の周波数が、所定期間ずつ、周波数ｆ１、周波数ｆ２、及び周波数ｆ３と順次に変化することであってもよい。

３次元位置・姿勢算出部２７は、画像処理部２６により算出・抽出された各種のパラメータを用いて、飛行体１０ａの３次元位置及び３次元姿勢を算出する。３次元位置・姿勢算出部２７は、例えば、３次元方向算出部２７１、３次元位置算出部２７２、及び３次元姿勢算出部２７３を含む。

３次元方向算出部２７１は、解析対象の画像における光源１３の２次元座標によって、光源１３の３次元方向を算出する。ここで、「３次元方向」は、光学的原点Ｏに対する方向である。当該算出方法については後述する。

３次元位置算出部２７２は、３次元方向算出部２７１により算出された光源１３の３次元方向と、解析対象の画像に含まれる光源１３の輝度とに基づいて、光源１３の３次元位置を算出する。当該算出方法については後述する。

３次元姿勢算出部２７３は、発光態様抽出部２７３により抽出された光源１３の発光態様に基づいて、飛行体１０ａの姿勢を算出する。当該算出方法については後述する。

ここで、図４を参照して、参照情報について説明する。図４は、参照情報の一例について説明するための模式的な図である。上述したとおり、参照情報は、例えば、飛行体１０ａのメモリ１４１に記憶され、飛行体１０ａの光源制御部１４０ｃが光源１３の発光を制御する際に参照される。また、上述したとおり、参照情報は、例えば、３次元位置・姿勢計測装置２０の３次元姿勢算出部２７３が、解析対象となる動画像から抽出された光源１３の発光態様に基づいて、飛行体１０ａの３次元姿勢を算出する際に参照される。

図４は、光源１３の色情報Ｃ又は点滅周波数ｆと、軸周りの回転角度θとの関係式が規定されたグラフが示されている。参照情報は、色情報Ｃ又は点滅周波数ｆによって回転角度θが一意的に定まるような関係式であれば特に限定されず、図４に示したような比例関係の他、比例関係ではない単調増加又は単調減少（一対一の写像関係）の関係式であればよい。図４には、一例として、色情報Ｃ又は点滅周波数ｆに対して回転角度θが単調増加するグラフが示されている。当該グラフを用いることにより、光源１３の発光態様に含まれる色情報Ｃや点滅周波数ｆによって、飛行体１０ａの３次元姿勢に含まれる回転角度θを表すことが可能となる。そして、飛行体１０ａは当該参照情報を用いて順次に光源１３を発光させることにより、姿勢センサ１４４が検知した飛行体１０ａの３次元姿勢を、光源１３の発光態様で表現できるようになる。また、３次元位置・姿勢計測装置２０は、飛行体１０ａを撮像することにより得られる画像から抽出された光源１３の発光態様に基づいて、飛行体１０ａの３次元姿勢を算出することが可能となる。

（２）３次元位置・姿勢計測方法
図５及び図６を参照して、第１実施形態に係る３次元位置・姿勢計測システム１による飛行体１０ａの３次元位置・姿勢計測方法の動作フローについて説明する。

なお、以下では、図６に示すとおり、カメラ２１ａの光学的原点をＯとし、３次元座標の軸を互いに直交するＸ軸、Ｙ軸、及びＺ軸とし、カメラ２１ａにより生成された解析対象の画像をＰとし、カメラ２１ａの焦点距離をＦとする。また、画像Ｐ内の２次元座標原点をＯ_Pとし、画像Ｐ内の２次元座標の軸をＸ軸に平行なｘ軸、及びＹ軸に平行なｙ軸とし、２次元座標原点Ｏ_Pの３次元位置座標を（０，０，Ｆ）とする。また、飛行体１０ａが備える光源１３の３次元位置を点Ａとし、点Ａの３次元座標を（Ｘ_A，Ｙ_A，Ｚ_A）とする。点Ａの画像Ｐにおける位置を点ａとし、点ａの画像Ｐにおける２次元座標を（ｘ_a，ｙ_a）とし、点aの輝度をI_aとする。

（Ｓ１１：光源を特定する）
まず、光源特定部２６１は、メモリ２３から取得された解析対象の画像Ｐから光源１３を特定する。光源特定部２６１は、例えば、画像に含まれる各画素の輝度を参照して、輝度が所定の閾値以上である画素を、光源に対応する画素であると特定する。そして、特定された画素に基づいて、光源１３の２次元座標を特定する。これにより、光源１３の２次元座標及び輝度が特定される。

（Ｓ１２：発光態様を抽出する）
次に、発光態様抽出部２６２は、解析対象の画像Ｐを含む所定期間内に撮像された複数の画像をメモリ２３から抽出し、当該複数の画像から光源１３の発光態様を抽出する。発光態様は、光源１３の色情報や点滅周波数等によって規定される。例えば、発光態様は、光源１３の色情報が、色情報Ｃ１、色情報Ｃ２、及び色情報Ｃ３と順次に変化することであってもよい。なお、この場合、発光態様抽出部２６２は例えば、画像に含まれる画素の色度情報に基づいて色情報を算出してよい。また、例えば、発光態様は、光源１３による点滅の周波数が、周波数ｆ１、周波数ｆ２、及び周波数ｆ３と順次に変化することであってもよい。

（Ｓ１３：光源の３次元方向を算出する）
次に、３次元方向算出部２７１は、解析対象の画像Ｐに基づいて点Ａの３次元方向を算出する。すなわち、点ＡのＸ軸方向及びＺ軸方向の３次元座標Ｘ_A及びＺ_A、点ａのｘ軸方向の２次元座標ｘ_a、及びカメラ１２の焦点距離Ｆについて式（１）が成り立つ。

式（１）をＸ_Aについて解くと、次の式（２）を得る。

同様にして、点ＡのＹ軸方向及びＺ軸方向の３次元座標Ｙ_A及びＺ_A、点ａのｙ軸方向の２次元座標ｙ_a、及びカメラ１２の焦点距離Ｆに基づいて、次の式（３）を得る。

式（２）及び式（３）より、点Ａの３次元座標（Ｘ_A，Ｙ_A，Ｚ_A）は、次の式（４）のように表される。

すなわち、点Ａの３次元方向は、次の式（５）に示すベクトルｌの方向に一致する。

（Ｓ１４：光源の３次元位置を算出する）
次に、３次元位置算出部２７２は、光源１３の３次元位置を算出する。まず、光源１３の解析対象の画像Ｐにおける輝度Ｉ_aと、光源１３の光学的原点Ｏからの距離Ｒとは、定数をＣとすると、次の式（６）を満たす。

式（６）に式（４）を代入すると、次の式（７）が成り立つ。

式（７）をＺ_Aについて解いて、次の式（８）を得る。

以上より、光源１３の３次元位置は、次の式（９）のように表される。

（Ｓ１５：光源の３次元姿勢を算出する）
次に、３次元姿勢算出部２７３は、Ｓ１２で抽出された光源１３の発光態様に基づいて、飛行体１０ａの３次元姿勢を算出する。

例えば、発光態様が光源１３の色情報Ｃ１、Ｃ２、及びＣ３で規定される場合に、メモリ２３に記憶された参照情報において、色情報Ｃ１は角度θ１に、色情報Ｃ２は角度θ２に、色情報Ｃ３は角度θ３にそれぞれ対応するものとする。この場合、３次元姿勢算出部２６３は、飛行体１０ａの姿勢を、Ｘ軸周りにθ１だけ回転し、Ｙ軸周りにθ２だけ回転し、Ｚ軸周りにθ３だけ回転した姿勢であると算出する。

また、例えば、発光態様が光源１３の点滅周波数ｆ１、ｆ２、及びｆ３で規定される場合に、メモリ２３に記憶された参照情報において、点滅周波数ｆ１は角度θ１に、点滅周波数ｆ２は角度θ２に、点滅周波数ｆ３は角度θ３度にそれぞれ対応するものとする。この場合、３次元姿勢算出部２６３は、飛行体１０ａの姿勢を、Ｘ軸周りにθ１だけ回転し、Ｙ軸周りにθ２だけ回転し、Ｚ軸周りにθ３だけ回転した姿勢であると算出する。以上で、飛行体１０ａの３次元位置及び姿勢が算出される。

（３）３次元位置・姿勢計測システム１の用途
第１実施形態に係る３次元位置・姿勢計測システム１は、例えば、以下のように利用され得る。まず、飛行体１０ａは、操縦装置１０ｂから供給される操縦信号や撮像信号に基づいて、大型設備の外壁等の対象物をカメラ１２により撮像しながら飛行する。このとき、３次元位置・姿勢計測装置２０のカメラ２１ａは、飛行中の飛行体１０ａを所定のフレームレートによって継続的に撮像する。

そして、飛行体１０ａのカメラ１２による対象物の撮像と、３次元位置・姿勢計測装置２０のカメラ２１ａによる飛行体１０ａの撮像とが終了した後で、３次元位置・姿勢計測２０は、上述したように、カメラ２１ａにより生成された画像に基づいて、当該画像が撮像された時点における飛行体１０ａの３次元位置及び姿勢を算出する。更に、このようにして算出された３次元位置及び姿勢の情報は、飛行体１０ａのカメラ１２により生成された対象物（大型設備の外壁等）の画像を合成する際に用いられる。

例えば、飛行体１０ａの３次元姿勢が、Ｘ軸周りに回転角度αだけ回転し、Ｙ軸周りに回転角度βだけ回転し、Ｚ軸周りに回転角度γだけ回転したものとすると、当該３次元姿勢に対応する回転行列は、次の式（Ｒ）のように表される。

したがって、飛行体１０ａのカメラ１２が生成した画像に対して、当該３次元姿勢に対応する回転行列の逆行列を掛けることで、飛行体１０ａの３次元姿勢の分が補正された補正画像を得ることができる。そして、飛行体１０ａの各３次元位置について、このように補正画像を生成した上で、生成された補正画像同士を合成することにより、大型設備の外壁等の対象物についての画像が生成される。

［第２実施形態］
以下では、第２実施形態に係る３次元位置・姿勢計測システム２について説明する。

（１）構成
図７は、第２実施形態に係る３次元位置・姿勢計測システム２の構成の一例を示す概略図である。図８は、第２実施形態に係る飛行体１０ａの構成の一例を示すブロック図である。図９は、第２実施形態に係る３次元位置・姿勢計測装置２０の構成の一例を示すブロック図である。

図７及び図８に示すとおり、第２実施形態に係る３次元位置・姿勢計測システム２においては、飛行体１０ａは、飛行体１０ａの姿勢に応じた発光態様で発光する２つの光源１３Ａ及び１３Ｂを備える。光源１３Ａ及び１３Ｂそれぞれの３次元位置が算出されれば、当該２つの３次元位置に基づいて、２つの光源１３Ａ及び１３Ｂを通るベクトルが算出可能である。そして、当該ベクトルが算出されれば、飛行体１０ａの３次元姿勢については、当該ベクトル周りの回転角度という１つのパラメータのみが未知のパラメータとして残る。そこで、第２実施形態に係る３次元位置・姿勢計測システム２における飛行体１０ａは、２つの光源１３Ａ及び１３Ｂを通るベクトル周りの飛行体１０ａの回転角度に基づいた発光態様（色情報や点滅周波数等）で発光する。これにより、３次元位置・姿勢計測装置２０は、光源１３Ａ及び／又は１３Ｂの発光態様を用いて当該１つのパラメータのみを特定すればよいため、光源１３Ａ及び／又は１３Ｂの発光の制御が簡素になる。

このため、図８に示すとおり、第２実施形態に係る飛行体１０ａでは、制御部１４０が含む光源制御部１４０ｃは、例えば、ベクトル算出部１４０ｃ１と、回転角度算出部１４０ｃ２とを含む。ここで、ベクトル算出部１４０ｃ１は、飛行体１０ａの３次元姿勢を示す姿勢センサ１４４の出力に基づいて、２つの光源１３Ａ及び１３Ｂを通るベクトルの方向を算出する。また、回転角度算出部１４０ｃ２は、飛行体１０ａの３次元姿勢を示す姿勢センサ１４４の出力、及びベクトル算出部１４０ｃ１が算出したベクトル（２つの光源１３Ａ及び１３Ｂを通るベクトル）の方向に基づいて、当該ベクトル周りの飛行体１０ａの回転角度を算出する。そして、光源制御部１４０ｃは、回転角度算出部１４０ｃ２によって算出された回転角度に応じた発光態様（色情報や点滅周波数）で光源１３Ａ及び／又は光源１３Ｂを発光させる。

図９に示すとおり、第２実施形態に係る３次元位置・姿勢計測装置２０の３次元姿勢算出部２７３は、例えば、ベクトル算出部２７３ａと、回転角度算出部２７３ｂとを含む。ベクトル算出部２７３ａは、２つの光源１３Ａ及び１３Ｂそれぞれの３次元位置に基づいて、２つの光源１３Ａ及び１３Ｂを通るベクトルの方向を算出する。回転角度算出部２７３ｂは、発光態様抽出部２６２により抽出された発光態様に基づいて、２つの光源１３Ａ及び１３Ｂを通るベクトルの周りの飛行体１０ａの回転角度を算出する。

（２）３次元位置・姿勢計測方法
図１０及び図１１を参照して、第２実施形態に係る３次元位置・姿勢計測システム２による飛行体１０ａの３次元位置・姿勢計測方法の動作フローについて説明する。

なお、以下では、図１１に示すとおり、飛行体１０ａが備える光源１３Ａの３次元位置を点Ａとし、点Ａの３次元座標を（Ｘ_A，Ｙ_A，Ｚ_A）とし、点Ａの画像Ｐにおける位置を点ａとし、点ａの画像Ｐにおける２次元座標を（ｘ_a，ｙ_a）とし、点aの輝度をI_aとする。また、飛行体１０ａが備える光源１３Ｂの３次元位置を点Ｂとし、点Ｂの３次元座標を（Ｘ_B，Ｙ_B，Ｚ_B）とし、点Ｂの画像Ｐにおける位置を点Ｂとし、点Ｂの画像Ｐにおける２次元座標を（ｘ_B，ｙ_B）とし、点Ｂの輝度をI_Bとする。

（Ｓ２１：光源を特定する）
まず、光源特定部２６１は、メモリ２３から取得された解析対象の画像Ｐから光源１３Ａ及び１３Ｂそれぞれを特定する。光源特定部２６１は、例えば、画像に含まれる各画素の輝度を参照して、輝度が所定の閾値以上である画素を、光源に対応する画素であると特定する。そして、特定された画素に基づいて、光源１３Ａ及び１３Ｂそれぞれの２次元座標を特定する。これにより、光源１３Ａ及び１３Ｂそれぞれの２次元座標及び輝度が特定される。

（Ｓ２２：発光態様を抽出する）
次に、発光態様抽出部２６２は、光源１３Ａの発光態様及び／又は光源１３Ｂの発光態様を抽出する。例えば、発光態様抽出部２６２は、解析対象の画像Ｐから、光源１３Ａの色情報及び／又は光源１３Ｂの色情報を抽出してもよい。或いは、発光態様抽出部２６２は、解析対象の画像Ｐを含む所定期間内に撮像された複数の画像をメモリ２３から抽出し、当該複数の画像から光源１３Ａの点滅周波数及び／又は光源１３Ｂの点滅周波数を抽出してもよい。

（Ｓ２３：光源の３次元方向を算出する）
次に、３次元方向算出部２７１は、解析対象の画像Ｐに基づいて点Ａ及び点Ｂそれぞれの３次元方向を算出する。すなわち、点ＡのＸ軸方向及びＺ軸方向の３次元座標Ｘ_A及びＺ_A、点ａのｘ軸方向の２次元座標ｘ_a、及びカメラ１２の焦点距離Ｆに基づいて、点Ａの３次元方向は、次の式（１０－１）に示すベクトルｌ１の方向に一致する。

また、点ＢのＸ軸方向及びＺ軸方向の３次元座標Ｘ_B及びＺ_B、点ｂのｘ軸方向の２次元座標ｘ_b、及びカメラ１２の焦点距離Ｆに基づいて、点Ｂの３次元方向は、次の式（１０－２）に示すベクトルｌ２の方向に一致する。

（Ｓ２４：光源の３次元位置を算出する）
次に、３次元位置算出部２７２は、光源１３Ａ及び１３Ｂそれぞれの３次元位置を算出する。まず、光源１３及び１３Ｂそれぞれの解析対象の画像Ｐにおける輝度Ｉ_a及びＩ_b、光源１３及び１３Ｂそれぞれの光学的原点Ｏからの距離Ｒ_A及びＲ_Bとは、定数をＣとすると、次の式（１１）を満たす。

式（１０－１）、式（１０－２）、及び式（１１）に基づいて、光源１３Ａ及び１３Ｂそれぞれの３次元位置は、次の式（１２）及び式（１３）のように表される。

（Ｓ２５：２つの光源を通るベクトルの方向を算出する）
次に、３次元姿勢算出部２７３のベクトル算出部２７３ａは、飛行体１０ａが備える光源１３Ａと光源１３Ｂとを通るベクトルを、次の式（１４）のように求める。

（Ｓ２６：ベクトル周りの回転角度を算出する）
次に、３次元姿勢算出部２７３の回転角度算出部２７３ｂは、メモリ２３に記憶された参照情報を参照して、Ｓ２２で抽出された光源１３Ａの発光態様及び／又は光源１３Ｂの発光態様に基づいて、飛行体１０ａのベクトルＡＢ周りの回転角度を算出する。例えば、光源１３Ａの色情報及び／又は光源１３Ｂの色情報がＣ４であり、参照情報において色情報Ｃ４が回転角度θ４に対応する場合、３次元姿勢算出部２７３は、飛行体１０ａの姿勢を、ベクトルＡＢ周りにθ４だけ回転した姿勢であると算出する。或いは、光源１３Ａの点滅周波数及び／又は光源１３Ｂの点滅周波数がｆ４であり、参照情報において点滅周波数ｆ４が回転角度θ４に対応する場合、３次元姿勢算出部２７３は、飛行体１０ａの姿勢を、ベクトルＡＢ周りにθ４だけ回転した姿勢であると算出する。以上で、飛行体１０ａの３次元位置及び姿勢が算出される。

［第３実施形態］
以下では、第３実施形態に係る３次元位置・姿勢計測システム３について説明する。

（１）構成
図１２は、第３実施形態に係る３次元位置・姿勢計測システム３の構成の一例を示す概略図である。図１３は、第３実施形態に係る飛行体１０ａの構成の一例を示すブロック図である。図１４は、第３実施形態に係る３次元位置・姿勢計測装置２０の構成の一例を示すブロック図である。

図１２及び図１３に示すとおり、第３実施形態に係る３次元位置・姿勢計測システム３においては、飛行体１０ａは３つの光源１３Ａ、１３Ｂ、及び１３Ｃを備える。飛行体１０ａの光源制御部１４０ｃは、光源１３Ａ、１３Ｂ、及び１３Ｃを、飛行体１０ａの３次元姿勢に基づいた発光態様で発光させる制御は実行しない。光源制御部１４０ｃによる光源１３Ａ、１３Ｂ、及び１３Ｃの発光の制御は、飛行体１０ａの３次元姿勢に基づいた発光態様で発光させる制御でなければ、特にその内容は限定されないが、例えば、所定の色情報や強度等で光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれを発光させてもよい。

第３実施形態に係る３次元位置・姿勢計測システム３においては、飛行体１０ａは、姿勢センサ１４４を備えなくてもよい。また、第３実施形態に係る３次元位置・姿勢計測システム３においては、飛行体１０ａのメモリ１４１は、光源１３Ａ、１３Ｂ、及び１３Ｃを飛行体１０ａの３次元姿勢に応じた発光態様で発光制御する際に参照する参照情報は記憶しなくてもよい。

図１４に示すとおり、第３実施形態に係る３次元位置・姿勢計測システム３においては、３次元位置・姿勢計測装置２０の画像処理部２６は、発光態様抽出部２６２を含まなくてもよい。また、第３実施形態に係る３次元位置・姿勢計測システム３においては、３次元方向算出部２７１は、解析対象となる画像における光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの２次元座標に基づいて、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元方向を算出する。また、第３実施形態に係る３次元位置・姿勢計測システム３においては、３次元位置算出部２７２は、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元方向と、解析対象となる画像における光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの輝度から算出される各光源１３までの距離とに基づいて、各光源１３の３次元位置を算出する。また、第３実施形態に係る３次元位置・姿勢計測システム３においては、３次元姿勢算出部２７３は、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元位置に基づいて、飛行体１０ａの３次元姿勢を算出する。

（２）３次元位置・姿勢計測方法
図１５及び図１６を参照して、第３実施形態に係る３次元位置・姿勢計測システム３による飛行体１０ａの３次元位置・姿勢計測方法の動作フローについて説明する。

なお、以下では、図１６に示すとおり、飛行体１０ａが備える光源１３Ａの３次元位置を点Ａとし、点Ａの３次元座標を（Ｘ_A，Ｙ_A，Ｚ_A）とし、点Ａの画像Ｐにおける位置を点ａとし、点ａの画像Ｐにおける２次元座標を（ｘ_a，ｙ_a）とし、点aの輝度をI_aとする。また、飛行体１０ａが備える光源１３Ｂの３次元位置を点Ｂとし、点Ｂの３次元座標を（Ｘ_B，Ｙ_B，Ｚ_B）とし、点Ｂの画像Ｐにおける位置を点Ｂとし、点Ｂの画像Ｐにおける２次元座標を（ｘ_B，ｙ_B）とし、点Ｂの輝度をI_Bとする。また、飛行体１０ａが備える光源１３Ｃの３次元位置を点Ｃとし、点Ｃの３次元座標を（Ｘ_C，Ｙ_C，Ｚ_C）とし、点Ｃの画像Ｐにおける位置を点Ｃとし、点Ｃの画像Ｐにおける２次元座標を（ｘ_C，ｙ_C）とし、点Ｃの輝度をI_Cとする。

（Ｓ３１：光源を特定する）
まず、光源特定部２６１は、メモリ２３から取得された解析対象の画像Ｐから光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれを特定する。光源特定部２６１は、例えば、画像に含まれる各画素の輝度を参照して、輝度が所定の閾値以上である画素を、光源に対応する画素であると特定する。そして、特定された画素に基づいて、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの２次元座標を特定する。これにより、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの２次元座標及び輝度が特定される。

（Ｓ３２：光源の３次元方向を算出する）
次に、３次元方向算出部２７１は、解析対象の画像Ｐに基づいて点Ａ、点Ｂ、及び点Ｃそれぞれの３次元方向を算出する。すなわち、点ＡのＸ軸方向及びＺ軸方向の３次元座標Ｘ_A及びＺ_A、点ａのｘ軸方向の２次元座標ｘ_a、及びカメラ１２の焦点距離Ｆに基づいて、点Ａの３次元方向は、次の式（１５－１）に示すベクトルｌ１の方向に一致する。

また、点ＢのＸ軸方向及びＺ軸方向の３次元座標Ｘ_B及びＺ_B、点ｂのｘ軸方向の２次元座標ｘ_b、及びカメラ１２の焦点距離Ｆに基づいて、点Ｂの３次元方向は、次の式（１５－２）に示すベクトルｌ２の方向に一致する。

また、点ＣのＸ軸方向及びＺ軸方向の３次元座標Ｘ_C及びＺ_C、点ｃのｘ軸方向の２次元座標ｘ_c、及びカメラ１２の焦点距離Ｆに基づいて、点Ｃの３次元方向は、次の式（１５－３）に示すベクトルｌ３の方向に一致する。

（Ｓ３３：光源の３次元位置を算出する）
次に、３次元位置算出部２７２は、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元位置を算出する。まず、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの解析対象の画像Ｐにおける輝度Ｉ_a、Ｉ_b、及びＩ_c、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの光学的原点Ｏからの距離Ｒ_A、Ｒ_B、Ｒ_Cは、定数をＣとすると、次の式（１６）を満たす。

式（１５－１）、式（１５－２）、式（１５－３）、及び式（１６）に基づいて、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元位置は、次の式（１７）、式（１８）、及び式（１９）のように表される。

（Ｓ３４：光源の３次元姿勢を算出する）
次に、３次元姿勢算出部２７３は、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元位置に基づいて、飛行体１０ａの３次元位置及び姿勢を求める。飛行体１０ａの３次元位置は、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元位置に基づいて規定されれば、その表記方法は特に限定されないが、例えば、光源１３Ａ、１３Ｂ、及び１３Ｃの重心等であってよい。また、飛行体１０ａの３次元姿勢は、例えば、光源１３Ａ、１３Ｂ、及び１３Ｃそれぞれの３次元位置に基づいて規定されれば、その表記方法は特に限定されないが、例えば、次の式（２０）のように、光源１３Ｂ及び光源１３Ｃの中点から光源１３Ａへ向くベクトルとして表されてもよい。

以上、様々な実施形態について説明した。上述した各実施形態に係る飛行体、３次元位置・姿勢計測装置、及び３次元位置・姿勢計測方法によれば、ＧＰＳ信号の受信が困難な環境や、大型の装置が進入することの困難な狭い環境においても、少ない数の光源によって飛行体の３次元位置・姿勢を把握することが可能となる。

以上説明した各実施形態は、本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。実施形態が備える各要素並びにその配置、材料、条件、形状及びサイズ等は、例示したものに限定されるわけではなく適宜変更することができる。また、異なる実施形態で示した構成同士を部分的に置換し又は組み合わせることが可能である。

１、２、３…３次元位置・姿勢計測システム、１０ａ…飛行体、１０ｂ…操縦装置、１１…モータ、１２…カメラ、１３、１３Ａ、１３Ｂ、１３Ｃ…光源、１４０…制御部、１４０ａ…モータ制御部、１４０ｂ…カメラ制御部、１４０ｃ…光源制御部、１４０ｃ１…ベクトル算出部、１４０ｃ２…回転角度算出部、１４１…メモリ、１４２…モータコントローラ、１４３…通信部、１４４…姿勢センサ、１４５…バッテリ、２０…３次元位置・姿勢計測装置、２１ａ…カメラ、２１ｂ…制御ユニット、２２…通信部、２３…メモリ、２４…制御部、２５…カメラ制御部、２６…画像処理部、２６１…光源特定部、２７…３次元位置・姿勢算出部、２７１…３次元方向算出部、２７２…３次元位置算出部、２７３…３次元姿勢算出部、２７３ａ…ベクトル算出部、２７３ｂ…回転角度算出部

Claims (9)

1. 飛行体であって、
   前記飛行体の姿勢を検出する姿勢検出部と、
   少なくとも１つの光源と、
   前記少なくとも１つの光源を、前記姿勢検出部が検出する姿勢に基づいた発光態様で発光させる光源制御部と、を備え、
   前記姿勢検出部は、前記飛行体の姿勢として、直交する３つの座標軸の各々を回転軸とする３つの回転角度を検出し、
   前記光源制御部は、前記少なくとも１つの光源を、前記姿勢検出部が検出する前記３つの回転角度の各々に応じた色情報及び／又は点滅周波数で発光させることを特徴とする、飛行体。
2. 前記光源制御部は、前記少なくとも１つの光源を、前記３つの回転角度の各々に応じた色情報で、順次に発光させる、請求項１に記載の飛行体。
3. 前記光源制御部は、前記少なくとも１つの光源を、前記３つの回転角度の各々に応じた点滅周波数で、順次に発光させる、請求項１に記載の飛行体。
4. 前記姿勢検出部は、角速度センサである、請求項１～３のいずれか一項に記載の飛行体。
5. 前記少なくとも１つの光源は、ＬＥＤである、請求項１～４のいずれか一項に記載の飛行体。
6. 前記少なくとも１つの光源の数は、１つ又は２つである、請求項１～５のいずれか一項に記載の飛行体。
7. 請求項１～６のいずれか一項に記載の飛行体の空間的な位置及び姿勢を算出する３次元位置・姿勢計測装置であって、
   前記飛行体を含むシーンを撮像することにより画像を生成する撮像部と、
   前記撮像部により生成された画像から、前記少なくとも１つの光源の発光態様を抽出する抽出部と、
   前記少なくとも１つの光源の前記画像における２次元座標と、前記抽出部により抽出された前記発光態様に基づいて、前記飛行体の３次元位置及び姿勢を算出する算出部と、を備える、３次元位置・姿勢計測装置。
8. 請求項１～７のいずれか一項に記載の飛行体を含むシーンを撮像することにより画像を生成することと、
   生成された前記画像から、前記少なくとも１つの光源の発光態様を抽出することと、
   前記少なくとも１つの光源の前記画像における２次元座標と、抽出された前記発光態様に基づいて、前記飛行体の３次元位置及び姿勢を算出することと、を含む、３次元位置・姿勢計測方法。
9. 少なくとも３つの光源を備える飛行体の空間的な位置及び姿勢を算出する３次元位置・姿勢計測装置であって、
   前記飛行体を含むシーンを撮像することにより画像を生成する撮像部と、
   前記少なくとも３つの光源の各々の前記画像における２次元座標及び輝度に基づいて、前記少なくとも３つの光源の各々の３次元位置を算出することにより、前記飛行体の位置及び姿勢を算出する算出部と、を備え、
   前記飛行体の姿勢は前記少なくとも３つの光源の各々の前記３次元位置に基づいて規定されることを特徴とする、３次元位置・姿勢計測装置。