JP2020091580A - 飛行装置制御システム - Google Patents

Abstract

【課題】複雑な構成を必要とすることなく、飛行装置までの距離にかかわらず飛行装置の飛行姿勢の精度の高い推定が容易な飛行装置制御システムを提供する。【解決手段】飛行装置制御システム１０は、設置部１５、反射部材駆動装置１６、変化量取得部３６および飛行姿勢推定部６５を備える。設置部１５は、無人の飛行装置１１と一体または別体に設けられ、再帰反射部材１４が設置されている。反射部材駆動装置１６は、設置部１５に対して、再帰反射部材１４を相対移動可能に駆動する。変化量取得部３６は、飛行装置１１の飛行姿勢の変化量を取得する。飛行姿勢推定部６５は、測量部１６で測量した測量結果から取得される再帰反射部材１４の移動軌跡、および変化量取得部３６で取得した飛行装置１１の飛行姿勢の変化量から、飛行装置１１の最新の飛行姿勢を推定する。【選択図】図１

Description

本発明は、飛行装置制御システムに関する。

近年、いわゆるドローンと称される小型の飛行装置が普及している。飛行装置は、主に地上の操作者による無線もしくは有線での遠隔操作、または予め設定された飛行計画に沿って操作者の操作によらない自立的な制御によって飛行する。いずれにしても、飛行装置は、地上の基地や操作者から離れた位置で飛行位置や飛行姿勢が監視される。このように飛行装置は、遠隔から監視されることから、例えば特許文献１のようにカメラを用いることが提案されている。特許文献１では、飛行装置に設けられているカメラを用いて、飛行装置の外部に設置した再帰反射部材や特定の画像が表示された表示板などのマーカを認識し、カメラで認識したマーカの画像に基づいて飛行装置の飛行姿勢を推定している。

しかしながら、マーカと飛行装置との距離が大きくなると、飛行装置に搭載したカメラはマーカの捕捉が困難になる。例えばマーカと飛行装置との間の距離の拡大にともなってカメラの倍率を上げると、視野が狭くなる。そのため、マーカとの飛行装置との間の距離が大きくなるほど、カメラによるマーカの安定した捕捉は困難になるという問題がある。

また、飛行装置に搭載したマーカを地上の測量機器で測量し、飛行装置の極座標を取得し、取得した極座標に基づいて飛行装置の飛行位置および飛行姿勢を制御することも提案されている。マーカが１つの場合、飛行位置や飛行姿勢の厳密な推定は困難であることから、加速度センサや角速度センサなどの内界センサを用いたレート積分により飛行位置や飛行姿勢を推定している。しかし、この場合、内界センサの積分時の誤差によって、推定した飛行位置や飛行姿勢の誤差が増大するという問題がある。一方、捕捉性の向上や推定精度の向上のためにマーカの数を増加すると、構成の複雑化を招くという問題がある。

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

そこで、複雑な構成を必要とすることなく、飛行装置までの距離にかかわらず飛行装置の飛行姿勢の精度の高い推定が容易な飛行装置制御システムを提供することを目的とする。

請求項１記載の発明では、飛行姿勢推定部は、飛行装置の最新の飛行姿勢を推定する。すなわち、飛行姿勢推定部は、測量部で測量した移動する再帰反射部材までの距離および角度を含む測量結果から再帰反射部材の移動軌跡を取得する。そして、飛行姿勢推定部は、変化量取得部を通して飛行装置の飛行姿勢の変化量を取得する。このとき、変化量取得部は、飛行装置に設けられ、例えば加速度センサや角速度センサを含む内界センサによって飛行装置の飛行姿勢の変化量を取得する。飛行姿勢推定部は、これら取得した再帰反射部材の移動軌跡および飛行装置の飛行姿勢の変化量から、飛行装置の最新の飛行姿勢を推定する。このように、飛行姿勢推定部は、再帰反射部材の移動軌跡に基づいて飛行装置の飛行姿勢を推定する。そのため、測量部は、１つの再帰反射部材を追尾すれば足りる。つまり、追尾の対象となる再帰反射部材は、反射部材駆動装置によって、既知の設置部に対して相対的に駆動される。そのため、再帰反射部材は、既知の設置部の座標系において移動軌跡が取得される。これにより、飛行装置の飛行位置を一定に維持することによって、設置部の座標系に変化は生じない。その結果、飛行姿勢推定部は、既知の設置部の座標系において１つの再帰反射部材を追尾することによって、飛行装置の飛行位置および飛行姿勢を推定する。また、再帰反射部材は、入射した光を照射元へ反射するため、測量部までの距離にかかわらず追尾が容易である。そして、飛行姿勢推定部は、再帰反射部材の追尾によって取得した移動軌跡を、変化量取得部で取得した飛行姿勢の変化量を用いて補正する。そのため、飛行装置に風や障害物といった外部からの影響が加わる場合でも、既知の設置部における座標系の誤差は補正される。したがって、複雑な構成を必要とすることなく、飛行装置までの距離にかかわらず容易に飛行装置の飛行姿勢を高い精度で推定することができる。

第１実施形態による飛行装置制御システムの概略的な構成を示す模式図 第１実施形態による飛行装置制御システムの構成を示すブロック図 第１実施形態による飛行装置制御システムの座標系を説明するための図 第１実施形態による飛行装置制御システムの処理の流れを示す概略図 第２実施形態による飛行装置制御システムの処理の流れを示す概略図 第３実施形態による飛行装置制御システムの処理の流れを示す概略図 第４実施形態による飛行装置制御システムの概略的な構成を示す模式図 第４実施形態による飛行装置制御システムの処理の流れを示す概略図 第５実施形態による飛行装置制御システムの構成を示すブロック図 第５実施形態による飛行装置制御システムの処理の流れを示す概略図 第６実施形態による飛行装置制御システムの構成を示すブロック図 第６実施形態による飛行装置制御システムの処理の流れを示す概略図 第７実施形態による飛行装置制御システムの概略的な構成を示す模式図 その他の実施形態による飛行装置制御システムの概略的な構成を示す模式図

以下、飛行装置を用いた飛行装置制御システムの複数の実施形態を図面に基づいて説明する。なお、複数の実施形態において実質的に同一の構成部位には同一の符号を付し、説明を省略する。
（第１実施形態）
図１に示すように第１実施形態による飛行装置制御システム１０は、飛行装置１１および地上設備１２を備える。飛行装置１１は、本体１３、再帰反射部材１４、設置部１５および反射部材駆動装置１６を備えている。また、地上設備１２は、測量部１７および制御装置１８を備えている。飛行装置１１は、地上設備１２の測量部１７から照射された光を再帰反射部材１４で反射する。地上設備１２の測量部１７は、再帰反射部材１４で反射した光を用いて飛行装置１１を追尾して飛行装置１１の飛行データを取得する。設置部１５は、再帰反射部材１４が設置されている。

飛行装置１１の本体１３は、腕部２１およびスラスタ２２を有している。腕部２１は、本体１３において放射状に延びて設けられ、先端にスラスタ２２が設けられている。なお、本体１３は、腕部２１が放射状に延びる構成に限らず、円環状に形成し、周方向へ複数のスラスタ２２を設ける構成など、任意の構成とすることができる。また、腕部２１およびスラスタ２２の数は、２つ以上であれば任意に設定することができる。

スラスタ２２は、いずれもモータ２３およびプロペラ２４を有している。モータ２３は、プロペラ２４を駆動する駆動源である。モータ２３は、バッテリ２５などの電源から供給される電力によって駆動される。プロペラ２４は、モータ２３によって回転駆動される。また、プロペラ２４は、図示しないピッチ変更機構部によってピッチが変更される。スラスタ２２は、モータ２３でプロペラ２４を駆動することによって推進力を発生する。このとき、スラスタ２２から発生する推進力の大きさおよび推進力の向きは、モータ２３の回転数およびプロペラ２４のピッチを変更することによって制御される。

飛行装置１１は、制御ユニット３０および通信部３１を備えている。制御ユニット３０は、図２に示すように制御演算部３２および記憶部３３を有している。制御演算部３２は、ＣＰＵ、ＲＯＭおよびＲＡＭを有するマイクロコンピュータで構成されている。制御演算部３２は、ＣＰＵでＲＯＭに記憶されているコンピュータプログラムを実行することにより飛行装置１１の全体を制御する。制御演算部３２は、コンピュータプログラムを実行することにより、状態取得部３４、飛行制御部３５および変化量取得部３６をソフトウェア的に実現している。なお、状態取得部３４、飛行制御部３５および変化量取得部３６は、ソフトウェア的に限らず、ハードウェア的、あるいはソフトウェアとハードウェアとの協働によって実現してもよい。記憶部３３は、例えば不揮発性メモリなどを有している。記憶部３３は、予め設定された飛行計画をデータとして記憶している。飛行計画は、例えば飛行装置１１が飛行する飛行ルートや飛行高度などが含まれている。通信部３１は、地上設備１２との間で無線または有線で通信する。

状態取得部３４は、飛行装置１１の本体１３の傾きや本体１３に加わる加速度などから飛行装置１１の飛行状態を取得する。具体的には、状態取得部３４は、ＧＰＳ（Global Positioning System）センサ４１、加速度センサ４２、角速度センサ４３、地磁気センサ４４および高度センサ４５などと接続している。状態取得部３４を構成する加速度センサ４２、角速度センサ４３、地磁気センサ４４および高度センサ４５は、いずれも飛行装置１１の外部からの情報を必要とすることなく飛行装置１１の飛行状態およびその変化量を自立的に取得する内界センサに相当する。ＧＰＳセンサ４１は、ＧＰＳ衛星から出力されるＧＰＳ信号を受信する。そのため、ＧＰＳセンサ４１は、飛行装置１１の外部からの情報を取得する外界センサに相当する。また、加速度センサ４２は、ｘ軸、ｙ軸およびｚ軸の３次元の３つの軸方向において飛行装置１１の本体１３に加わる加速度を検出する。角速度センサ４３は、３次元の３つの軸方向において本体１３に加わる角速度を検出する。地磁気センサ４４は、３次元の３つの軸方向における地磁気を検出する。高度センサ４５は、気圧などに基づいて天地方向における高度を検出する。状態取得部３４を構成するＧＰＳセンサ４１、加速度センサ４２および角速度センサ４３は、速度情報取得部に相当する。また、ＧＰＳセンサ４１は、並進位置情報取得部に相当する。

状態取得部３４は、これらＧＰＳセンサ４１で受信したＧＰＳ信号、加速度センサ４２で検出した加速度、角速度センサ４３で検出した角速度、地磁気センサ４４で検出した地磁気などから本体１３の飛行姿勢、飛行方向および飛行速度を検出する。また、状態取得部３４は、ＧＰＳセンサ４１で検出したＧＰＳ信号と各種センサによる検出値から本体１３の飛行位置を検出する。さらに、状態取得部３４は、高度センサ４５で検出した高度から本体１３の飛行高度を検出する。このように、状態取得部３４は、本体１３の飛行姿勢、飛行位置および飛行高度など、本体１３の飛行に必要な情報を飛行状態として検出する。状態取得部３４は、これらに加え、本体１３の周囲における可視的な画像を取得する図示しないカメラ、あるいは本体１３の周囲の物体まで距離を測定する図示しないＬＩＤＡＲ（Light Detection And Ranging）などに接続してもよい。これら図示しないカメラやＬＩＤＡＲも、外界センサに相当する。

飛行制御部３５は、モータ２３の回転数およびプロペラ２４のピッチを変更することにより、スラスタ２２の推進力を制御する。飛行制御部３５は、本体１３の飛行を、自立制御モードおよび遠隔制御モードによって制御する。自立制御モードは、操作者の操作または地上設備１２からの誘導によらずに、飛行装置１１を自立的に飛行させる飛行モードである。自立制御モードのとき、飛行制御部３５は、記憶部３３に記憶されている飛行計画に沿って、飛行装置１１の飛行を自動的に制御する。すなわち、飛行制御部３５は、この自立制御モードのとき、飛行計画および状態取得部３４で検出した飛行装置１１の飛行状態に基づいて、スラスタ２２の推進力を制御する。これにより、飛行制御部３５は、操作者の操作および地上設備１２からの誘導によらず、飛行装置１１を飛行計画に沿って自動的に飛行させる。一方、遠隔制御モードは、操作者の操作または地上設備１２からの誘導にしたがって飛行装置１１を飛行させる飛行モードである。遠隔制御モードのとき、地上設備１２は、遠隔から飛行装置１１の飛行状態を制御する。操作者が飛行装置１１の飛行状態を操作する場合、操作者は地上設備１２を通して操作の意思を入力する。また、地上設備１２が飛行装置１１を誘導する場合、地上設備１２は予め設定されている飛行計画に沿って飛行装置１１を誘導する。飛行制御部３５は、地上設備１２による誘導、および状態取得部３４で取得した飛行状態に基づいてスラスタ２２の推進力を制御する。これにより、飛行制御部３５は、操作者の意思による操作または地上設備１２からの誘導に基づいて飛行装置１１を飛行させる。
変化量取得部３６は、飛行装置１１の飛行姿勢の変化量を取得する。具体的には、変化量取得部３６は、状態取得部３４で取得した飛行装置１１の飛行姿勢の経時的な変化に基づいて、単位時間あたりの変化量または基準となる時期から任意の時期までの変化量を、飛行姿勢の変化量として取得する。

第１実施形態の場合、図１に示すように反射部材駆動装置１６は、設置部１５に設けられている再帰反射部材１４を駆動する。反射部材駆動装置１６は、制御ユニット３０の制御演算部３２からの指示によって再帰反射部材１４を駆動する。具体的には、反射部材駆動装置１６は、支持部５１およびアクチュエータ５２を有している。支持部５１は、飛行装置１１の設置部１５と再帰反射部材１４との間に設けられている。支持部５１は、設置部１５と反対側の端部に再帰反射部材１４を支持している。アクチュエータ５２は、支持部５１の本体１３側に設けられており、制御演算部３２からの指示に基づいて支持部５１を通して再帰反射部材１４を駆動する。これにより、第１実施形態の場合、再帰反射部材１４は、反射部材駆動装置１６を挟んで飛行装置１１の設置部１５と一体に設けられている。再帰反射部材１４は、地上設備１２の測量部１７から照射された光を、この測量部１７に向けて反射する。すなわち、再帰反射部材１４は、測量部１７から照射された光を、光源である測量部１７に向けて反射する。

地上設備１２は、上述のように測量部１７および制御装置１８を備えている。制御装置１８は、図２に示すように制御演算部６１、記憶部６２、地上通信部６３、測量制御部６４および飛行姿勢推定部６５を有している。制御演算部６１は、ＣＰＵ、ＲＯＭおよびＲＡＭを有するマイクロコンピュータで構成されている。記憶部６２は、不揮発性メモリや光磁気ディスクなどの任意の記憶媒体を有している。制御演算部６１は、ＣＰＵでＲＯＭや記憶部６２に記憶されているコンピュータプログラムを実行することにより、地上設備１２の全体を制御する。制御演算部６１は、コンピュータプログラムを実行することにより、測量制御部６４および飛行姿勢推定部６５をソフトウェア的に実現している。なお、これら測量制御部６４および飛行姿勢推定部６５は、ソフトウェア的に限らず、ハードウェア的、あるいはソフトウェアとハードウェアとの協働によって実現してもよい。地上通信部６３は、飛行装置１１の通信部３１との間において有線または無線で通信する。

測量部１７は、照射部６６、受光部６７およびデータ処理部６８を有している。照射部６６は、例えばレーザ光などの光を照射する。照射部６６は、連続的または所定の間隔で定期的にレーザ光を照射する。受光部６７は、飛行装置１１に設けられている再帰反射部材１４で反射した光を受光する。すなわち、受光部６７は、照射部６６から照射され、飛行装置１１の再帰反射部材１４で反射したレーザ光を受光する。

測量制御部６４は、測量部１７の制御を実行する。具体的には、測量制御部６４は、例えば図示しないモータやアクチュエータなどを用いて測量部１７を任意の方向へ駆動し、飛行する飛行装置１１に向けて測量部１７を追尾させる。これとともに、測量制御部６４は、照射部６６を制御して光の照射を実行するとともに、受光部６７を制御して光の受光を制御する。このように、測量制御部６４は、飛行装置１１へ向けて測量部１７を追尾させながら、飛行装置１１への光の照射および再帰反射部材１４で反射した光の受光を制御する。

測量部１７は、飛行装置１１の再帰反射部材１４で反射した光に基づいて、飛行装置１１までの距離と、飛行装置１１の角度とを取得する。飛行装置１１の角度とは、地上設備１２の測量部１７に基準点として地上原点ｐ０を設定し、この地上原点ｐ０を中心とする水平方向の角度および垂直方向の角度である。つまり、測量部１７に地上原点ｐ０を設定したとき、水平方向には０〜３６０°の水平角度が設定され、垂直方向には０〜９０°の垂直角度が設定される。この場合、水平角度の基準となる「０°」は、例えば地図座標における「北」をはじめとして任意に設定される。また、垂直角度の基準となる「０°」は、例えば地面と平行な面に設定される。測量部１７は、再帰反射部材１４で反射した光から、飛行装置１１の水平角度および垂直角度、ならびに飛行装置１１までの距離を取得する。

飛行姿勢推定部６５は、再帰反射部材１４の移動軌跡、および変化量取得部３６で取得した飛行装置１１の飛行姿勢の変化量から、飛行装置１１の最新の飛行姿勢を推定する。すなわち、飛行姿勢推定部６５は、測量部１７を通して取得した飛行装置１１までの距離、水平角度および垂直角度から飛行装置１１の移動軌跡、つまり再帰反射部材１４の移動軌跡を取得する。また、飛行姿勢推定部６５は、飛行装置１１の通信部３１および地上設備１２の地上通信部６３を通して、飛行装置１１の変化量取得部３６から飛行装置１１の飛行姿勢の変化量を取得する。飛行姿勢推定部６５は、これら取得した移動軌跡および飛行姿勢から、飛行装置１１の最新の飛行姿勢を推定する。

次に、飛行姿勢推定部６５による飛行装置１１の飛行姿勢の推定について詳細に説明する。
まず、本実施形態における飛行装置制御システム１０における座標系について図３に基づいて説明する。本実施形態の飛行装置制御システム１０では、機体座標系ΣＡと、測量部座標系ΣＷとを設定している。機体座標系ΣＡは、飛行装置１１に設定されている座標系である。一方、測量部座標系ΣＷは、地上設備１２の測量部１７に設定されている座標系である。

機体座標系ΣＡは、ｘ軸、ｙ軸、ｚ軸の３次元の３つの軸が規定される。これらは、ロール軸、ピッチ軸およびヨー軸にそれぞれ対応する。機体座標系ΣＡにおいて、ｘ軸であるロール軸を中心とする回転角度φはロール角である。同様に、ｙ軸であるピッチ軸を中心とする回転角度θはピッチ角であり、ｚ軸であるヨー軸を中心とする回転角度ψはヨー角である。機体座標系ΣＡにおいて、基準となる原点は機体原点ｐ０である。また、測量部座標系ΣＷは、機体座標系ΣＡと同様にＸ軸、Ｙ軸、Ｚ軸の３次元の３つの軸が規定される。測量部座標系ΣＷにおいて、基準となる原点は地上原点Ｐ０である。

ここで、機体原点ｐ０に対する再帰反射部材１４の座標は、座標^Ａｘ_ｉとして設定される。この座標^Ａｘ_ｉは、飛行装置１１に設けられた再帰反射部材１４の座標であるから実測値を取得可能な既知の値である。また、地上原点Ｐ０に対する再帰反射部材１４の座標は、座標^ＷＸ_ｉとして設定される。この座標^ＷＸ_ｉは、測量部１７で測定した再帰反射部材１４までの実測値であることから取得可能な既知の値である。一方、地上原点Ｐ０に対する機体原点ｐ０の位置^Ｗｐ_ＡＷは、飛行装置１１の飛行位置および飛行姿勢によって変化する。そのため、機体座標系ΣＡの原点である機体原点ｐ０の位置、および機体座標系ΣＡにおける各軸の回転角度φ、θ、ψは、それぞれ未知であり、飛行姿勢推定部６５による推定の対象となる。すなわち、

・ΣＡ 機体原点位置^Ｗｐ_ＡＷ＝（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）^ｔ
・ΣＡ 各軸回転角度（φ，θ，ψ）^ｔ
は、いずれも飛行姿勢推定部６５による推定の対象となる。
ここで、ｔは、（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）の転置行列であることを意味する。
これにより、飛行姿勢推定部６５は、既知の値として、
・座標^Ａｘ_ｉ＝（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）^ｔ
・座標^ＷＸ_ｉ＝（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）^ｔ
・推定開始から計測された各軸回転量（Δφ_ｉ，Δθ_ｉ，Δψ_ｉ）^ｔ
・推定開始から計測された並進変位^ＷΔｐ_ＡＷｉ＝（Δｐ_ｘｉ，Δｐ_ｙｉ，Δｐ_ｚｉ）^ｔ
を用いて飛行装置１１の飛行姿勢を推定する。
なお、上記において、座標^Ａｘ_ｉ、座標^ＷＸ_ｉ、位置^Ｗｐ_ＡＷは、並進変位^ＷΔｐ_ＡＷｉは、いずれもベクトルである。

次に、飛行姿勢推定部６５による具体的な推定の手法について説明する。
飛行姿勢推定部６５は、複数の異なる時期に取得した検出データを用いて、既存のＤＬＴ（Direct Liner Transformation）法を用いて飛行装置１１の姿勢を推定する。
任意の計測ステップｉにおける２つの座標系の間における同時変換行列は、以下の式１の通りである。

ここで、Ｃ_ｚ（ψ）、Ｃ_ｙ（θ）、Ｃ_ｘ（φ）は、各軸の回転行列のベクトルである。
それぞれの計測ステップｉで計測した値に対しては、以下の式２、式３が成立する。

飛行装置１１の姿勢の不安定性を考慮すると、この式２および式３による連立方程式を解き、飛行装置１１の姿勢を推定するためには、最低でも３つの計測ステップが必要となる。しかし、それぞれの計測ステップで取得された^ｗΔｐ_ＡＷｉおよび各軸回転量（Δφ_ｉ，Δθ_ｉ，Δψ_ｉ）^ｔは、計測誤差を含んでいる。そのため、より多くの計測ステップで^ｗΔｐ_ＡＷｉおよび各軸回転量（Δφ_ｉ，Δθ_ｉ，Δψ_ｉ）^ｔを取得し、誤差が最も小さな飛行姿勢を推定する。
そこで、上記の複数の式から、以下の式４に変形する。

ここで、Ｍは３ｉ行７列のパラメータ行列のベクトルである。したがって、飛行姿勢推定部６５は、次の式５を満たすベクトルである飛行姿勢νを推定する。なお、式４示されるＭνもベクトルである。

次に、上記の構成による飛行姿勢推定部６５における処理の流れについて図４に基づいて説明する。
飛行姿勢推定部６５は、飛行姿勢の推定を開始する姿勢推定開始信号を受け取ると（Ｓ１０１）、計測ステップの数をステップ数ｎとして設定する（Ｓ１０２）。飛行姿勢推定部６５は、単位時間あたりのデータ検出回数と検出時間との積としてステップ数ｎを設定する。この場合、単位時間あたりのデータ検出回数および検出時間は、いずれも任意に設定することができる。ステップ数ｎが大きくなるほど、精度は向上するものの処理が煩雑になる。そのため、飛行姿勢推定部６５は、飛行装置１１に要求される飛行姿勢の推定精度に基づいてステップ数ｎを任意に設定する。

飛行姿勢推定部６５は、ステップ数ｎを設定すると、反射部材駆動装置１６により再帰反射部材１４を駆動する（Ｓ１０３）。これにより、再帰反射部材１４は、反射部材駆動装置１６によって駆動される。この場合、反射部材駆動装置１６は、再帰反射部材１４を、設置部１５に対して相対的に移動させる。

飛行姿勢推定部６５は、再帰反射部材１４の駆動が開始されると、各種パラメータの取得を開始する（Ｓ１０４）。すなわち、飛行姿勢推定部６５は、計測ステップｉについてｉ＝１として、計測ステップｉごとに各種のパラメータを取得する。飛行姿勢推定部６５は、飛行装置１１から計測ステップｉにおける機体側パラメータとして、座標^Ａｘ_ｉ、並進変位^ＷΔｐ_ＡＷｉ、各軸回転量（Δφ_ｉ，Δθ_ｉ，Δψ_ｉ）^ｔを取得して記憶する（Ｓ１０５）。また、飛行姿勢推定部６５は、計測ステップｉにおける地上側パラメータとして、座標^ＷＸ_ｉを取得して記憶する（Ｓ１０６）。このとき、飛行姿勢推定部６５は、測量部１７で追尾する飛行装置１１の再帰反射部材１４の移動軌跡に基づいて地上側パラメータを取得する。

飛行姿勢推定部６５は、Ｓ１０５およびＳ１０６において計測ステップｉにおけるパラメータの取得が終了すると、計測ステップｉがＳ１０２で設定したステップ数ｎに到達したか否かを判断する（Ｓ１０７）。すなわち、飛行姿勢推定部６５は、設定したステップ数ｎの検出データが取得できたか否かを判断する。飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達していないとき（Ｓ１０７：Ｎｏ）、計測ステップｉをｉ＝ｉ＋１とインクリメントして（Ｓ１０８）、Ｓ１０５およびＳ１０６以降の処理を繰り返す。
一方、飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達すると（Ｓ１０７：Ｙｅｓ）、上記の「式４」となるＭνを導出する（Ｓ１０９）。さらに、飛行姿勢推定部６５は、Ｓ１０９で導出したＭνから、「式５」を満たす飛行姿勢νを推定する（Ｓ１１０）。

以上説明した第１実施形態では、飛行姿勢推定部６５は、飛行装置１１の最新の飛行姿勢を推定する。すなわち、飛行姿勢推定部６５は、測量部１７で測量した移動する再帰反射部材１４の座標^ＷＸ_ｉを含む測量結果から再帰反射部材１４の移動軌跡を取得する。そして、飛行姿勢推定部６５は、変化量取得部３６を通して飛行装置１１の飛行姿勢の変化量を各軸回転量（Δφ_ｉ，Δθ_ｉ，Δψ_ｉ）^ｔ、および並進変位^ＷΔｐ_ＡＷｉ＝（Δｐ_ｘｉ，Δｐ_ｙｉ，Δｐ_ｚｉ）^ｔとして取得する。変化量取得部３６は、飛行装置１１に設けられ、状態取得部３４で取得した加速度センサ４２や角速度センサ４３を含む内界センサの出力値に基づいて、飛行装置１１の飛行姿勢の変化量を取得する。飛行姿勢推定部６５は、これら取得した再帰反射部材１４の座標^Ａｘ_ｉ＝（ｘ_ｉ，ｙ_ｉ，ｚ_ｉ）^ｔおよび座標^ＷＸ_ｉ、ならびに飛行装置１１の飛行姿勢の変化量である各軸回転量および並進変位から、飛行装置１１の最新の飛行姿勢を推定する。このように、飛行姿勢推定部６５は、再帰反射部材１４の移動軌跡に相当する再帰反射部材１４の座標^Ａｘ_ｉおよび座標^ＷＸ_ｉに基づいて飛行装置１１の飛行姿勢を推定する。そのため、測量部１７は、飛行装置１１に設けられている１つの再帰反射部材１４を追尾すれば足りる。このとき、追尾の対象となる再帰反射部材１４は、反射部材駆動装置１６によって、既知の設置部１５に対して相対的に駆動される。そのため、再帰反射部材１４は、既知の設置部１５の座標系である機体座標系ΣＡにおいて移動軌跡が取得される。これにより、飛行装置１１の飛行位置が既知であれば、座標系ΣＡに変化は生じない。その結果、飛行姿勢推定部６５は、既知の設置部１５の座標系において１つの再帰反射部材１４を追尾することによって、飛行装置１１の飛行位置および飛行姿勢を推定する。また、再帰反射部材１４は、入射した光を照射元へ反射するため、測量部１７までの距離にかかわらず追尾が容易である。そして、飛行姿勢推定部６５は、再帰反射部材１４の追尾によって取得した座標を、変化量取得部３６で取得した飛行姿勢の変化量を用いて補正する。そのため、飛行装置１１に風や障害物といった外部からの影響が加わる場合でも、既知の設置部１５における機体座標系ΣＡの誤差は補正される。したがって、複雑な構成を必要とすることなく、飛行装置１１までの距離にかかわらず容易に飛行装置１１の飛行姿勢を高い精度で推定することができる。

第１実施形態では、再帰反射部材１４は飛行装置１１に設けられている。また、測量部１７は、飛行装置１１とは別体に設けられている。これにより、既存の飛行装置制御システム１０の構成の活用が可能である。したがって、新たな構成を追加することなく、飛行装置１１の飛行姿勢を高い精度で推定することができる。

（第２実施形態）
第２実施形態による飛行装置制御システムについて説明する。
第２実施形態による飛行装置制御システム１０は、概略的な構成が第１実施形態と共通するのに対し、処理に用いるデータおよび処理の流れの一部が第１実施形態と相違する。

一般に、状態取得部３４に接続する加速度センサ４２や角速度センサ４３などの内界センサで構成される慣性航法装置は、飛行装置１１に加わる加速度や角速度に基づいて飛行装置１１のロール軸の回転角度φおよびピッチ軸の回転角度θを検出する。そのため、これら加速度センサ４２や角速度センサ４３の検出値を用いることにより、飛行姿勢推定部６５は飛行装置１１の回転角度φおよび回転角度θをより高い精度で推定することができる。また、状態取得部３４に接続するＧＰＳセンサ４１は、ＲＴＫ（Real Time Kinetic）−ＧＰＳにより、３次元における飛行装置１１の位置座標（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）を検出する。そのため、ＧＰＳセンサ４１の検出値を用いることにより、飛行姿勢推定部６５は飛行装置１１の位置座標をより高い精度で推定することができる。そこで、飛行姿勢推定部６５は、これら得られた回転角度φ、回転角度θおよび位置座標（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）を上記の式４に代入する。これにより、次の式６が得られる。

ここで、Ｍは３ｉ行２列のパラメータ行列である。その結果、飛行姿勢推定部６５が推定する変数は、ヨー方向の回転のみとなる。したがって、飛行姿勢推定部６５は、より正確な飛行姿勢νの推定が可能となる。

次に、第２実施形態による飛行姿勢推定部６５における処理の流れについて図５に基づいて説明する。第１実施形態と共通する処理については、説明を省略する。
飛行姿勢推定部６５は、姿勢推定開始信号を受け取ると（Ｓ２０１）、計測ステップの数をステップ数ｎとして設定する（Ｓ２０２）。飛行姿勢推定部６５は、ステップ数ｎを設定すると、反射部材駆動装置１６により再帰反射部材１４を駆動する（Ｓ２０３）。飛行姿勢推定部６５は、再帰反射部材１４の駆動が開始されると、各種パラメータの取得を開始する（Ｓ２０４）。飛行姿勢推定部６５は、機体側パラメータを取得するとともに（Ｓ２０５）、地上側パラメータを取得する（Ｓ２０６）。

飛行姿勢推定部６５は、Ｓ２０５およびＳ２０６において計測ステップｉにおけるパラメータの取得が終了すると、計測ステップｉがステップ数ｎに到達したか否かを判断する（Ｓ２０７）。飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達していないとき（Ｓ２０７：Ｎｏ）、計測ステップｉをｉ＝ｉ＋１とインクリメントして（Ｓ２０８）、Ｓ２０５およびＳ２０６以降の処理を繰り返す。

一方、飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達すると（Ｓ２０７：Ｙｅｓ）、状態取得部３４の各種センサの出力値を取得する（Ｓ２０９）。すなわち、飛行姿勢推定部６５は、状態取得部３４の加速度センサ４２および角速度センサ４３からロール軸の回転角度φおよびピッチ軸の回転角度ψを取得する。これとともに、飛行姿勢推定部６５は、状態取得部３４のＧＰＳセンサ４１から位置座標（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）を取得する。飛行姿勢推定部６５は、Ｓ２０４におけるパラメータの取得を開始してからＳ２０７でパラメータの取得が完了したと判断したときまでの回転角度φ、回転角度ψ、および位置座標（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）の実測した変化量を取得する。この変化量は、再帰反射部材１４の駆動開始から駆動完了までの各値の変化量に相当する。

飛行姿勢推定部６５は、取得したデータを上記の「式２」における（φ，ψ）、（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）にそれぞれ代入する（Ｓ２１０）。そして、飛行姿勢推定部６５は、上記「式６」となるＭνを導出する（Ｓ２１１）。さらに、飛行姿勢推定部６５は、Ｓ２１１で導出したＭνから「式５」を満たす飛行姿勢νを推定する（Ｓ２１２）。

第２実施形態では、状態取得部３４の各種センサで取得したデータを用いてパラメータを低減している。そのため、回転角度φ、回転角度ψ、位置座標（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）は、飛行姿勢推定部６５で推定することなく、より精度の高い実測データとして取得される。したがって、飛行装置１１の飛行姿勢の推定精度をより向上することができる。
なお、第２実施形態では、飛行装置１１に搭載されている例えば図示しないカメラで撮影した画像などを用いて回転角度φ、ψ、位置座標（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）を推定または検出する構成としてもよい。

（第３実施形態）
第３実施形態による飛行装置制御システムについて説明する。
第３実施形態による飛行装置制御システム１０は、概略的な構成が第１実施形態と共通するのに対し、処理の流れの一部が第１実施形態と相違する。

第３実施形態では、飛行制御部３５は、飛行姿勢推定部６５において飛行姿勢の推定を行なう前に、飛行装置１１を姿勢安定モードに移行する。ここで、姿勢安定モードは、飛行装置１１の各軸における回転角度、および並進変位をいずれも０に近づけるものである。すなわち、飛行制御部３５は、姿勢安定モードにあるとき、飛行装置１１の各軸の回転および飛行位置の変化を抑える制御を行なう。飛行制御部３５は、状態取得部３４の各種センサやカメラなどで検出したデータに基づいて、スラスタ２２の推進力を制御する。これにより、飛行装置１１は、飛行姿勢および飛行位置の変化が抑えられる。

具体的には、飛行制御部３５は、各軸の回転角度が式７を満たすとともに、並進変位^ＷΔｐ_ＡＷｉが式８を満たすようにスラスタ２２を制御する。この場合、飛行制御部３５は、スラスタ２２のモータ２３の回転数およびプロペラ２４のピッチを制御することにより、スラスタ２２の推進力を制御する。

このように、飛行制御部３５が飛行装置１１の飛行姿勢および飛行位置を制御することにより、飛行装置１１の各軸の回転および飛行位置の変化に起因する誤差の低減が図られる。

次に、第３実施形態による飛行姿勢推定部６５における処理の流れについて図６に基づいて説明する。第１実施形態と共通する処理については、説明を省略する。
飛行姿勢推定部６５は、姿勢推定開始信号を受け取ると（Ｓ３０１）、計測ステップの数をステップ数ｎとして設定する（Ｓ３０２）。飛行制御部３５は、Ｓ３０２において飛行姿勢推定部６５がステップ数ｎを設定すると、飛行装置１１の飛行モードを姿勢安定モードへ移行する（Ｓ３０３）。すなわち、飛行制御部３５は、状態取得部３４の各種センサおよびカメラなどから取得した情報に基づいて、スラスタ２２におけるモータ２３の回転数およびプロペラ２４のピッチを制御する。これにより、飛行制御部３５は、飛行装置１１の各軸における回転および飛行位置の変化を制限する。

飛行姿勢推定部６５は、Ｓ３０３において飛行制御部３５が飛行装置１１を姿勢安定モードへ移行させると、反射部材駆動装置１６により再帰反射部材１４を駆動する（Ｓ３０４）。飛行姿勢推定部６５は、再帰反射部材１４の駆動が開始されると、各種パラメータの取得を開始する（Ｓ３０５）。飛行姿勢推定部６５は、機体側パラメータを取得するとともに（Ｓ３０６）、地上側パラメータを取得する（Ｓ３０７）。

飛行姿勢推定部６５は、Ｓ３０６およびＳ３０７において計測ステップｉにおけるパラメータの取得が終了すると、計測ステップｉがステップ数ｎに到達したか否かを判断する（Ｓ３０８）。飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達していないとき（Ｓ３０８：Ｎｏ）、計測ステップｉをｉ＝ｉ＋１とインクリメントして（Ｓ３０９）、Ｓ３０６およびＳ３０７以降の処理を繰り返す。

一方、飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達すると（Ｓ３０８：Ｙｅｓ）、「式４」となるＭνを導出する（Ｓ３１０）。さらに、飛行姿勢推定部６５は、Ｓ３１０で導出したＭνから、「式５」を満たす飛行姿勢νを推定する（Ｓ３１１）。

第３実施形態では、飛行姿勢推定部６５が飛行姿勢を推定するとき、飛行制御部３５は飛行装置１１を姿勢安定モードへ移行する。これにより、飛行装置１１は、飛行制御部３５によって、各軸の回転および飛行位置の変化が抑えられた状態で飛行する。そのため、飛行姿勢の推定のために取得される各種データは、飛行装置１１の各軸の回転および飛行位置の変化に起因する誤差が低減される。したがって、飛行装置１１の飛行姿勢の推定精度をより向上することができる。

（第４実施形態）
第４実施形態による飛行装置制御システムについて説明する。
第４実施形態による飛行装置制御システム１０は、概略的な構成が第１実施形態と共通するのに対し、処理の流れの一部が第１実施形態と相違する。
飛行装置１１の飛行姿勢を推定する場合、一般に姿勢アライメント工程およびレート積分工程を含んでいる。姿勢アライメント工程は、飛行装置１１が離陸する前に機体座標系ΣＡと地上座標系ΣＷとの関係の整合を図るいわゆるキャリブレーションを実行する工程である。また、レート積分工程は、状態取得部３４で取得した加速度、角速度およびＧＰＳによる位置情報などのデータを逐次演算することにより、機体座標系ΣＡと地上座標系ΣＷとの関係を更新する工程である。第４実施形態では、飛行装置１１は、図７に示すように脚部７９を備えている。脚部７９は、天地方向においてスラスタ２２の下方に設けられ、飛行装置１１を地面８０に支持する。脚部７９は、折り畳み可能とすることができる。飛行姿勢推定部６５は、離陸前において飛行装置１１が地面８０に静止した姿勢アライメント工程において、飛行姿勢を推定する。すなわち、飛行姿勢推定部６５は、飛行姿勢が地面８０で安定している離陸前の状態のとき姿勢アライメント工程を実行する。換言すると、この姿勢アライメント工程は、飛行装置１１の飛行姿勢が安定した姿勢安定モードに相当する。これにより、姿勢アライメント工程の開始から検出された各軸の回転量は式９、姿勢アライメント工程の開始から検出された並進変位^ＷΔｐ_ＡＷｉは式１０に示す関係が確約される。

このように、飛行装置１１の離陸前の安定した状態で姿勢アライメント工程を実行することにより、飛行装置１１の各軸の回転および飛行位置の変化に起因する誤差がほぼ「０」となる。

次に、第４実施形態による飛行姿勢推定部６５における処理の流れについて図８に基づいて説明する。第１実施形態と共通する処理については、説明を省略する。
飛行姿勢推定部６５は、姿勢推定開始信号を受け取ると（Ｓ４０１）、以降の処理に先立って姿勢アライメント工程に移行する（Ｓ４０２）。すなわち、飛行姿勢推定部６５は、飛行制御部３５を通して飛行装置１１を離陸する前の地面８０における安定した状態とする。これにより、飛行装置１１は、飛行することなく、地面８０に待機している。

飛行姿勢推定部６５は、飛行装置１１が飛行を停止している姿勢アライメント工程に移行すると、計測ステップの数をステップ数ｎとして設定する（Ｓ４０３）。飛行姿勢推定部６５は、ステップ数ｎが設定されると、反射部材駆動装置１６により再帰反射部材１４を駆動する（Ｓ４０４）。飛行姿勢推定部６５は、再帰反射部材１４の駆動が開始されると、各種パラメータの取得を開始する（Ｓ４０５）。飛行姿勢推定部６５は、機体側パラメータを取得するとともに（Ｓ４０６）、地上側パラメータを取得する（Ｓ４０７）。

飛行姿勢推定部６５は、Ｓ４０６およびＳ４０７において計測ステップｉにおけるパラメータの取得が終了すると、計測ステップｉがステップ数ｎに到達したか否かを判断する（Ｓ４０８）。飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達していないとき（Ｓ４０８：Ｎｏ）、計測ステップｉをｉ＝ｉ＋１とインクリメントして（Ｓ４０９）、Ｓ４０６およびＳ４０７以降の処理を繰り返す。

一方、飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達すると（Ｓ４０８：Ｙｅｓ）、「式４」となるＭνを導出する（Ｓ４１０）。さらに、飛行姿勢推定部６５は、Ｓ４１０で導出したＭνから、「式５」を満たす飛行姿勢νを推定する（Ｓ４１１）。

第４実施形態では、飛行姿勢推定部６５は、飛行装置１１が離陸する前に飛行姿勢の推定を実行する。これにより、飛行装置１１は、地面８０において各軸の回転および飛行位置の変化が抑えられた状態となる。そのため、飛行姿勢の推定のために取得されるデータは、飛行装置１１の各軸の回転および飛行位置の変化に起因する誤差がほぼ「０」になる。したがって、飛行装置１１の飛行姿勢の推定精度をより向上することができる。

（第５実施形態）
第５実施形態による飛行装置制御システムについて説明する。
第５実施形態による飛行装置制御システム１０は、図９に示すように信頼度算出部８１を備えている。信頼度算出部８１は、制御装置１８において制御演算部６１によるコンピュータプログラムの実行によってソフトウェア的に実現されている。なお、信頼度算出部８１は、ソフトウェア的に限らず、ハードウェア的、またはソフトウェアとハードウェアとの協働によって実現してもよい。

信頼度算出部８１は、飛行姿勢推定部６５で推定された飛行姿勢の信頼度Ｅを算出する。これとともに、信頼度算出部８１は、算出した信頼度Ｅを操作者の五感に訴えることによって操作者に報知する。信頼度算出部８１は、例えば飛行装置１１の姿勢変化や図示しないディスプレイの表示などによる視覚的な手法、ブザーの鳴動などによる聴覚的な手法、あるいは飛行装置１１の一部の振動など触覚的な手法によって信頼度Ｅを報知する。また、信頼度算出部８１は、算出した信頼度Ｅが予め設定した設定範囲外であるとき、算出した信頼度Ｅを破棄する。本実施形態の場合、信頼度算出部８１は、算出した信頼度Ｅが下限値Ｅｔより大きいとき、飛行姿勢推定部６５で推定された飛行姿勢νを破棄する。下限値Ｅｔは、例えば飛行装置１１に求める性能などに基づいて任意に設定することができる。

具体的には、飛行姿勢推定部６５は、上述の第１実施形態で説明したように式４および式５を用いて飛行姿勢νを推定する。ここで、信頼度算出部８１は、以下のように式１１に基づいて信頼度Ｅを算出する。

この信頼度Ｅは、値が小さくなるほど精度の高い推定であることを示す指標である。飛行装置１１を用いる操作者は、この信頼度Ｅに対して下限値Ｅｔを任意に設定することができる。信頼度算出部８１は、算出した信頼度Ｅが設定された下限値Ｅｔよりも大きいとき、推定した飛行姿勢νに誤差が大きく含まれていると判断する。そのため、信頼度算出部８１は、算出した信頼度Ｅが下限値Ｅｔよりも大きいとき、飛行姿勢推定部６５において推定された飛行姿勢νを破棄する。例えば風などの外乱があると、推定した飛行姿勢の信頼度に影響が生じる。信頼度算出部８１において算出した信頼度Ｅが下限値Ｅｔ以下であるか否かを判断することにより、推定の精度の高い信頼度Ｅに基づく飛行姿勢νが用いられる。その結果、飛行姿勢の推定の精度が向上する。

次に、第５実施形態による飛行姿勢推定部６５における処理の流れについて図１０に基づいて説明する。第１実施形態と共通する処理については、説明を省略する。
飛行姿勢推定部６５は、姿勢推定開始信号を受け取ると（Ｓ５０１）、計測ステップの数をステップ数ｎとして設定する（Ｓ５０２）。飛行姿勢推定部６５は、ステップ数ｎを設定すると、下限値Ｅｔの設定を受け付ける（Ｓ５０３）。飛行姿勢推定部６５は、操作者が任意に設定する下限値Ｅｔを受け付ける。操作者は、例えば地上設備１２の制御装置１８を通して下限値Ｅｔを入力する。飛行姿勢推定部６５は、下限値Ｅｔの設定を受け付けると、反射部材駆動装置１６により再帰反射部材１４を駆動する（Ｓ５０４）。飛行姿勢推定部６５は、再帰反射部材１４の駆動が開始されると、各種パラメータの取得を開始する（Ｓ５０５）。飛行姿勢推定部６５は、機体側パラメータを取得するとともに（Ｓ５０６）、地上側パラメータを取得する（Ｓ５０７）。

飛行姿勢推定部６５は、Ｓ５０６およびＳ５０７において計測ステップｉにおけるパラメータの取得が終了すると、計測ステップｉがステップ数ｎに到達したか否かを判断する（Ｓ５０８）。飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達していないとき（Ｓ５０８：Ｎｏ）、計測ステップｉをｉ＝ｉ＋１とインクリメントして（Ｓ５０９）、Ｓ５０６およびＳ５０６以降の処理を繰り返す。

一方、飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達すると（Ｓ５０８：Ｙｅｓ）、「式４」となるＭνを導出する（Ｓ５１０）。信頼度算出部８１は、Ｓ５１０で導出されたＭνを用いて、式１１から信頼度Ｅを算出する（Ｓ５１１）。この場合、信頼度算出部８１は、Ｓ５１１で算出した信頼度Ｅを、五感を通して操作者に報知してもよい。信頼度算出部８１は、Ｓ５１１で算出した信頼度Ｅと、Ｓ５０３で設定した下限値Ｅｔとを比較する（Ｓ５１２）。すなわち、信頼度算出部８１は、算出した信頼度Ｅが下限値Ｅｔ以下であるか否かを判断する。

信頼度算出部８１は、Ｓ５１２において信頼度Ｅが下限値Ｅｔ以下であると判断すると（Ｓ５１２：Ｙｅｓ）、Ｓ５１０で導出したＭνから、「式５」を満たす飛行姿勢νを推定する（Ｓ５１３）。一方、信頼度算出部８１は、Ｓ５１２において信頼度Ｅが下限値Ｅｔより大きいと判断すると（Ｓ５１２：Ｎｏ）、Ｓ５１０で導出したＭνを破棄して飛行装置１１の姿勢推定の処理を終了する（Ｓ５１４）。

第５実施形態では、信頼度算出部８１は、導出したＭνに基づいて信頼度Ｅを算出し、算出した信頼度Ｅが予め設定した範囲にあるか否かを判断する。そして、信頼度算出部８１は、信頼度Ｅに基づいて信頼性が確保できないとき、飛行装置１１の姿勢推定の処理を終了する。これにより、例えば風などの外乱などの影響が排除され、推定した飛行姿勢の信頼度が高められる。したがって、飛行姿勢の推定の精度をより向上することができる。

（第６実施形態）
第６実施形態による飛行制御システムについて説明する。
第６実施形態による飛行装置制御システム１０は、第５実施形態の変形であり、図１１に示すようにセンサ補正部８２を備えている。センサ補正部８２は、制御装置１８の制御演算部６１によるコンピュータプログラムの実行によってソフトウェア的に実現されている。なお、センサ補正部８２は、ソフトウェア的に限らず、ハードウェア的、またはソフトウェアとハードウェアとの協働によって実現してもよい。

飛行装置１１に搭載されている各種センサは、飛行する周囲の状況によって予期しない誤差が生じることがある。例えば地磁気センサ４４の場合、飛行装置１１の周囲に鉄骨などの磁場の不安定化を招く構造物などがあると、誤差を生じ、信頼度が低下する。一方、飛行姿勢推定部６５による飛行装置１１の飛行姿勢の推定は、条件によって高い精度が得られる。そこで、センサ補正部８２は、飛行姿勢推定部６５において推定された飛行姿勢の信頼度Ｅが高いと判断されているとき、飛行装置１１の各種センサで検出したデータに誤差があるとして、状態取得部３４を構成する各種センサから取得した出力値を補正する。例えば上記のように地磁気センサ４４に誤差が生じているとき、センサ補正部８２は、飛行姿勢推定部６５で推定された飛行姿勢に基づいて、地磁気センサ４４の出力値を補正する。具体的には、センサ補正部８２は、飛行姿勢の信頼度Ｅが下限値Ｅｔ以下であるとき、信頼度Ｅが予め設定された補正必要値Ｅｃ以下であるか否かを判断する。そして、センサ補正部８２は、信頼度Ｅが補正必要値Ｅｃ以下であるとき、各種センサからの出力値を補正する。この場合、補正必要値Ｅｃは、操作者が飛行装置１１の信頼性を基準として任意の値に設定することができる。

次に、第６実施形態による飛行姿勢推定部６５における処理の流れについて図１２に基づいて説明する。第５実施形態と共通する処理については、説明を省略する。
飛行姿勢推定部６５は、姿勢推定開始信号を受け取ると（Ｓ６０１）、計測ステップの数をステップ数ｎとして設定する（Ｓ６０２）。飛行姿勢推定部６５は、ステップ数ｎを設定すると、下限値Ｅｔの設定を受け付ける（Ｓ６０３）。これとともに、センサ補正部８２は、補正必要値Ｅｃの設定を受け付ける（Ｓ６０４）。センサ補正部８２は、操作者が任意に設定する補正必要値Ｅｃを受け付ける。操作者は、例えば地上設備１２の制御装置１８を通して補正必要値Ｅｃを入力する。飛行姿勢推定部６５は、下限値Ｅｔおよび補正必要値Ｅｃの設定を受け付けると、反射部材駆動装置１６により再帰反射部材１４を駆動する（Ｓ６０５）。飛行姿勢推定部６５は、再帰反射部材１４の駆動が開始されると、各種パラメータの取得を開始する（Ｓ６０６）。飛行姿勢推定部６５は、機体側パラメータを取得するとともに（Ｓ６０７）、地上側パラメータを取得する（Ｓ６０８）。

飛行姿勢推定部６５は、Ｓ６０７およびＳ６０８において計測ステップｉにおけるパラメータの取得が終了すると、計測ステップｉがステップ数ｎに到達したか否かを判断する（Ｓ６０９）。飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達していないとき（Ｓ６０９：Ｎｏ）、計測ステップｉをｉ＝ｉ＋１とインクリメントして（Ｓ６１０）、Ｓ６０７およびＳ６０８以降の処理を繰り返す。

一方、飛行姿勢推定部６５は、計測ステップｉがステップ数ｎに到達すると（Ｓ６０９：Ｙｅｓ）、「式４」となるＭνを導出する（Ｓ６１１）。信頼度算出部８１は、Ｓ６１１で導出されたＭνを用いて、式１１から信頼度Ｅを算出する（Ｓ６１２）。信頼度算出部８１は、Ｓ６１２で算出した信頼度Ｅと、Ｓ６０３で設定した下限値Ｅｔとを比較する（Ｓ６１３）。信頼度算出部８１は、Ｓ６１３において信頼度Ｅが下限値Ｅｔ以下であると判断すると（Ｓ６１３：Ｙｅｓ）、Ｓ６１１で導出したＭνから、「式５」を満たす飛行姿勢νを推定する（Ｓ６１４）。一方、信頼度算出部８１は、Ｓ６１３において信頼度Ｅが下限値Ｅｔより大きいと判断すると（Ｓ６１３：Ｎｏ）、Ｓ６１２で算出した信頼度Ｅが低いとして、飛行装置１１の姿勢推定の処理を終了する（Ｓ６１５）。

センサ補正部８２は、Ｓ６１４で飛行姿勢νが推定されると、Ｓ６１２で算出した信頼度ＥとＳ６０４で設定した補正必要値Ｅｃとを比較する（Ｓ６１６）。センサ補正部８２は、Ｓ６１６において信頼度Ｅが補正必要値Ｅｃ以下であると判断すると（Ｓ６１６：Ｙｅｓ）、推定した飛行姿勢νから状態取得部３４を構成する各種センサの出力値を補正する（Ｓ６１７）。一方、センサ補正部８２は、Ｓ６１６において信頼度Ｅが補正必要値Ｅｃより大きいと判断すると（Ｓ６１６：Ｎｏ）、処理を終了する。

第６実施形態では、センサ補正部８２は、算出した信頼度Ｅが信頼できる範囲にあるとき、状態取得部３４を構成する各種センサの出力値を補正する。これにより、飛行装置１１の周囲に各種センサの誤差を招く原因があるときでも、各種センサの補正が可能となる。したがって、飛行姿勢の推定の精度の向上とともに、状態取得部３４を構成する各種センサの信頼性の向上を図ることができる。

（第７実施形態）
第７実施形態による飛行制御システムの飛行装置について説明する。
第７実施形態による飛行装置制御システム１０に用いられる飛行装置１１は、図１３に示すように反射部材駆動装置１６の構成が第１実施形態と異なる。第７実施形態の場合、反射部材駆動装置１６は、飛行装置１１のヨー軸つまり機体座標系ΣＡのＺ軸と平行な軸Ａを中心として再帰反射部材１４を回転駆動する。すなわち、再帰反射部材１４は、設置部１５に対して軸Ａを中心に回転する。第７実施形態の場合、反射部材駆動装置１６の支持部５１は、アクチュエータ５２によって、飛行装置１１のヨー軸方向に対して垂直な軸Ａを中心に回転する。これにより、反射部材駆動装置１６は、支持部５１の先端に支持された再帰反射部材１４を、軸Ａを中心に回転駆動する。なお、ヨー軸は、飛行装置１１の本体１３の中心を貫く軸である。これにより、反射部材駆動装置１６は、複雑な駆動機構を必要とせず、構成の簡略化を図ることができる。

第７実施形態では、反射部材駆動装置１６は、飛行装置１１のヨー軸と垂直な軸Ａを中心に再帰反射部材１４を回転駆動する。これにより、再帰反射部材１４を駆動するアクチュエータは１軸である。したがって、再帰反射部材１４の軸が低減され、精度の向上を図ることができる。

また、第７実施形態の場合、再帰反射部材１４は、機体座標系ΣＡにおけるＸ−Ｙ平面を円運動する。そのため、飛行姿勢推定部６５は、再帰反射部材１４の移動を円運動とみなして飛行姿勢を推定する処理を行なう。したがって、演算処理の簡略化および誤差の低減を図ることができ、飛行姿勢の推定の精度をより向上することができる。

（その他の実施形態）
以上説明した複数の実施形態では、再帰反射部材１４を飛行装置１１に設け、この再帰反射部材１４を地上設備１２の測量部１７で追尾する例について説明した。しかし、再帰反射部材１４とこれを追尾する測量部１７の相対的な関係が維持されていれば、再帰反射部材１４または測量部１７の設置位置は上述の複数の実施形態に限られない。例えば、再帰反射部材１４および測量部１７は、以下のように構成を変更することができる。

図１４に示す例の場合、測量部１７は飛行装置１１に設けられ、再帰反射部材１４は地上設備１２に設けられている。この場合、再帰反射部材１４を駆動する反射部材駆動装置１６および設置部１５は、地上設備１２に設けられている。

また、第１実施形態による再帰反射部材１４の駆動に代えて、測量部１７を設置部１５に対して相対的に駆動する構成としてもよい。この場合でも、設置部１５に対する測量部１７の座標は既知であることから、精度よく飛行姿勢の推定が可能となる。測量部１７を駆動することにより、飛行装置１１の飛行姿勢によって再帰反射部材１４が飛行装置１１の影になることが低減される。したがって、再帰反射部材１４を追従する精度をより向上ことができる。

以上説明した本発明は、上記実施形態に限定されるものではなく、その要旨を逸脱しない範囲で種々の実施形態に適用可能である。例えば上記の実施形態では、各実施形態を個別に適用する例について説明した。しかし、飛行装置制御システム１０は、複数の実施形態を組み合わせて適用してもよい。また、例えば上記の複数の実施形態では、演算処理を行なう飛行姿勢推定部６５は地上設備１２の制御装置１８で実行する例について説明したが、飛飛行姿勢推定部６５は飛行装置１１に設けてもよい。

本開示は、実施例に準拠して記述されたが、本開示は当該実施例や構造に限定されるものではないと理解される。本開示は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素のみ、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範疇や思想範囲に入るものである。

図面中、１０は飛行装置制御システム、１１は飛行装置、１４は再帰反射部材、１５は設置部、１６は反射部材駆動装置、１７は測量部、１８は制御装置、３４は状態取得部、３５は飛行制御部、３６は変化量取得部、４１はＧＰＳセンサ（並進位置情報取得部）、４２は加速度センサ（速度情報取得部）、４３は角速度センサ（速度情報取得部）、５１は支持部、６５は飛行姿勢推定部、８１は信頼度算出部、８２はセンサ補正部（補正部）を示す。

Claims (11)

1. 無人の飛行装置（１１）と、
   光を照射元へ反射する再帰反射部材（１４）と、
   前記再帰反射部材（１４）で反射した光から、前記再帰反射部材（１４）を追尾して、前記再帰反射部材（１４）までの距離および前記再帰反射部材（１４）の角度を測量する測量部（１７）と、
   前記測量部（１７）で測量した測量結果、および予め設定された目標位置を、前記飛行装置（１１）へ提供し、前記飛行装置（１１）の飛行を制御する制御装置（１８）と、を備える飛行装置制御システムであって、
   前記飛行装置（１１）と一体または別体に設けられ、前記再帰反射部材（１４）が設置されている設置部（１５）と、
   前記設置部（１５）に対して、前記再帰反射部材（１４）を相対移動可能に駆動する反射部材駆動装置（１６）と、
   前記飛行装置（１１）の飛行姿勢の変化量を取得する変化量取得部（３６）と、
   前記測量結果から取得される前記再帰反射部材（１４）の移動軌跡、および前記変化量取得部（３６）で取得した前記飛行装置（１１）の飛行姿勢の変化量から、前記飛行装置（１１）の最新の飛行姿勢を推定する飛行姿勢推定部（６５）と、
   を備える飛行装置制御システム。
2. 前記反射部材駆動装置（１６）は、前記再帰反射部材（１４）をヨー軸方向に対して垂直な軸を中心に回転可能であり、前記再帰反射部材（１４）を支持する支持部（５１）を有する請求項１記載の飛行装置制御システム。
3. 前記再帰反射部材（１４）は、前記飛行装置（１１）に設けられ、
   前記測量部（１７）は、前記飛行装置（１１）とは別体に離れて設けられている請求項１または２記載の飛行装置制御システム。
4. 前記測量部（１７）は、前記飛行装置（１１）に設けられ、
   前記再帰反射部材（１４）は、前記飛行装置（１１）とは別体に離れて設けられている請求項１または２記載の飛行装置制御システム。
5. 前記飛行装置（１１）の飛行状態を取得する飛行状態取得部（３４）をさらに備え、
   前記飛行姿勢推定部（６５）は、前記飛行状態取得部（３４）で取得した前記飛行装置（１１）の飛行状態を用いて前記飛行装置（１１）の飛行姿勢を推定する請求項１から４のいずれか一項記載の飛行装置制御システム。
6. 前記状態取得部（３４）は、前記飛行装置（１１）の角速度および並進加速度を取得する速度情報取得部（４２、４３）を有する請求項１から５のいずれか一項記載の飛行装置制御システム。
7. 前記状態取得部（３４）は、前記飛行装置（１１）の並進位置情報を取得する位置情報取得部（４１）を有する請求項１から６のいずれか一項記載の飛行装置制御システム。
8. 前記飛行装置（１１）は、前記状態取得部（３４）で取得した前記飛行装置（１１）の飛行姿勢の変化に基づいて前記飛行装置（１１）の飛行姿勢を制御する飛行制御部（３５）を有し、
   前記飛行姿勢推定部（６５）は、前記飛行制御部（３５）において前記飛行装置（１１）の飛行姿勢の変化を制限する姿勢安定モードのとき、前記飛行装置（１１）の飛行姿勢を推定する請求項１から７のいずれか一項記載の飛行装置制御システム。
9. 前記再帰反射部材（１４）または前記測量部（１７）の移動軌跡から推定された前記飛行装置（１１）の飛行姿勢の信頼度を算出し、算出した前記信頼度を報知する信頼度算出部（８１）をさらに備える請求項１から８のいずれか一項記載の飛行装置制御システム。
10. 前記信頼度算出部（８１）は、前記信頼度が予め設定した設定範囲外であるとき、前記飛行姿勢推定部（６５）で推定した飛行姿勢を破棄する請求項９記載の飛行装置制御システム。
11. 前記信頼度算出部（８１）で算出した前記信頼度に応じて前記飛行姿勢推定部（６５）で推定に用いる前記状態取得部（３４）で取得した値を補正する補正部（８２）を、さらに備える請求項９または１０記載の飛行装置制御システム。

Images