JP2019113992A - 飛行装置、飛行装置の制御方法及びプログラム - Google Patents

Abstract

【課題】飛行装置に対する移動制御の状況に応じて、一定速度にする制御と、目的位置に滑らかに到達させる制御とを適切に切替え可能とする。【解決手段】飛行装置のコントローラは、現在距離が所定の距離閾値に比較して大きいときには、速度ＰＩＤ制御で出力される操作量を飛行推進部に指示する。コントローラは、現在距離が所定の距離閾値に比較して小さくなると、その時点での現在位置と目的位置との距離を残り行程距離として、速度ＰＩＤ制御の出力に対しては現在距離の残り行程距離に対する比に比例した大きさの重み付けを行い、位置ＰＩＤ制御の出力に対しては上記比に反比例した大きさの重み付けを行い、速度ＰＩＤ制御の出力に重み付けをして得られる操作量と位置ＰＩＤ制御の出力に対して重み付けをして得られる操作量とを加算して得られる操作量を、飛行推進部に指示する。【選択図】図６

Description

本発明は、自律飛行する飛行装置、その制御方法及びプログラムに関する。

モータで駆動されるロータブレードによる駆動推進装置を搭載して自律飛行を行う、いわゆる「ドローン」又は「マルチコプター」と総称される小型無人飛行装置（以下「ドローン」と呼ぶ）が知られている（例えば、特許文献１、２）。

このような飛行装置が自律飛行を行う場合の移動制御として、位置に対してＰＩＤ制御（Ｐｒｏｐｏｒｔｉｏｎａｌ−Ｉｎｔｅｇｒａｌ−Ｄｉｆｆｅｒｅｎｔｉａｌ Ｃｏｎｔｒｏｌｌｅｒ）と呼ばれるフィードバック制御を行うものが知られている。ＰＩＤ制御は、制御の目標値と現在の制御量との差分に対応する偏差に関する比例動作、積分動作、及び微分動作を組み合わせたフィードバック制御方法である。位置に対するＰＩＤ制御では、例えば図７（ａ）に示されるように、飛行装置において例えばＧＰＳセンサから検出されている現在位置７００と、飛行装置が飛行する目的地である目的位置７０１とについて、例えば地面に水平な平面内で定義される互いに直交する２次元のＸＹ座標軸のうち、Ｘ軸方向の位置偏差７０２とＹ軸方向の位置偏差７０３のそれぞれについて、各偏差を小さくするような各軸方向の操作量が算出されて、飛行装置の飛行推進部に対してその操作量が指示される。図７（ａ）で、飛行装置の現在位置７００に示される４つの丸印は、飛行装置の推進装置を示しており、上記各軸方向の操作量が黒丸で示される２つの推進装置に対する２つの推進量に変換されて各推進装置が駆動されることを示している。なお、地面に垂直な高さ方向の制御も、ＰＩＤ制御によって別途実施される。

また、上記移動制御として、速度に対してＰＩＤ制御を行うものも知られている。速度に対するＰＩＤ制御では、例えば図７（ｂ）に示されるように、図７（ａ）と同様の飛行装置の現在位置７００と目的位置７０１とについて、図７（ｂ）と同様に地面に平行な平面で、まず、目的位置７０１に飛行するための目的速度ベクトル７０４が算出される。そして、この目的速度ベクトル７０４と、飛行装置において例えば加速度センサの出力を積分して得られる上記平面内での現在の速度を示す現在速度ベクトル７０５との偏差が算出される。具体的には、現在速度ベクトル７０５が、例えば現在位置７００から目的位置７０１に向かう軸（以下「水平軸」）方向の成分値である現在速度ベクトル７０５の水平方向分解成分７０６と、上記水平軸に垂直な軸（以下「垂直軸」）方向の成分値である現在速度ベクトル７０５の垂直分解成分７０７とに分解される。次に、これらの各成分と、目的速度ベクトル７０４も同様に水平及び垂直方向に分解した各成分との間の偏差として、水平方向偏差７０８及び垂直方向偏差７０９が算出される。そして、これらの各偏差を小さくするような各軸方向の操作量が算出されて、飛行装置の飛行推進部に対してその各操作量が指示される。図７（ｂ）において、飛行装置の現在位置７００に示される４つの丸印は、図７（ａ）の場合と同様に、飛行装置の推進装置を示しており、上記各軸方向の操作量が黒丸で示される２つの推進装置に対する２つの推進量に変換されて各推進装置が駆動されることを示している。

特許第５４３２２７７号 特開２０１３−１２９３０１号公報

上述の位置に対するＰＩＤ制御は、飛行装置が最終的に目的位置に到達するようにきめの細かい制御を行うことが可能である。しかし、位置に対するＰＩＤ制御では、現在位置と目的位置の偏差が大きい場合に、過度に速度を出すような制御が行われてしまっていた。また、現在位置と目的位置の差異により操作量が決まるため、移動中に一定の速度を保つことができなかった。

一方、上述の速度に対するＰＩＤ制御は、制御途中の飛行装置の速度を、目的位置に対応する目的速度に近づくように、ほぼ一定に保つことが可能である。しかし、速度に対するＰＩＤ制御では、飛行装置が最終的な目的位置に近づいたときに、目的位置の周辺を一定の速度で行ったり来たり往復してしまい、その目的位置に滑らかに到達させて停止させる制御が難しかった。

そこで、本発明は、飛行装置を、一定速度にする制御と、目的位置に滑らかに到達させる制御とを適切に制御することを目的とする。

態様の一例は、飛行推進部を備える飛行装置であって、少なくとも現在位置と現在速度を検出するセンサ部と、センサ部が検出した現在位置と目的位置との現在距離が遠いほどセンサ部が検出した現在速度と目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、現在距離が近くなるほどセンサ部が検出した現在位置と目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行う制御部と、を備える。

本発明によれば、飛行装置を、一定速度にする制御と、目的位置に滑らかに到達させる制御とを適切に制御することが可能となる。

本実施形態による飛行装置の構造例を示す横断面図である。 本実施形態による飛行装置の構造例を示す上面図である。 本実施形態による飛行装置のシステムの例を示すブロック図である。 本実施形態におけるＰＩＤ制御制御機構を示すブロック図である。 コントローラの移動制御の処理例を示すフローチャートである。 本実施形態の動作説明図である。 位置によるＰＩＤ制御と速度によるＰＩＤ制御の説明図である。

以下、本発明を実施するための形態について図面を参照しながら詳細に説明する。本実施形態では、飛行推進部と少なくとも現在位置と現在速度を検出するセンサ部とを備えた飛行装置の移動制御において、センサ部が検出した現在位置と目的位置との現在距離が遠いほどセンサ部が検出した現在速度と目的位置に対応する目的速度とに基づく例えば速度ＰＩＤ制御が強くかかり、現在距離が近くなるほど現在位置と目的位置とに基づく例えば位置ＰＩＤ制御が強くかかるフィードバック制御を、飛行推進部の操作量に関して行う制御部を備える。この場合、制御部は、速度ＰＩＤ制御と位置ＰＩＤ制御に対してそれぞれ現在距離に応じた重み付けを行って得られる各操作量を加算して得られる操作量を、飛行推進部に指示する。より具体的には、制御部は、現在距離が所定の距離閾値に比較して大きいときには、速度ＰＩＤ制御で出力される操作量を飛行推進部に指示する。また、制御部は、現在距離が所定の距離閾値に比較して小さくなると、その時点での現在位置と目的位置との距離を残り行程距離として、速度ＰＩＤ制御の出力に対しては現在距離の残り行程距離に対する比に比例した大きさの重み付けを行い、位置ＰＩＤ制御の出力に対しては上記比に反比例した大きさの重み付けを行い、速度ＰＩＤ制御の出力に重み付けをして得られる操作量と位置ＰＩＤ制御の出力に対して重み付けをして得られる操作量とを加算して得られる操作量を、飛行推進部に指示する。このような制御により、本実施形態では、飛行装置の現在位置が目的位置から遠い場合には、速度ＰＩＤ制御のみのフィードバック制御がかかることにより、飛行装置ができるだけ一定速度になるような制御を実施し、飛行装置が目的位置に近づいたときには、速度ＰＩＤ制御から位置ＰＩＤ制御に徐々に移行するようなフィードバック制御がかかることにより、飛行装置を目的位置に滑らかに到達させて停止（ホバリング）させる制御を実施することが可能となる。

図１及び図２はそれぞれ、本実施形態による飛行装置１００の構造例を示す横断面図及び上面図である。図２（ａ）は図１の破線枠Ａの部分を飛行装置１００の上方から下方を見た場合の上面図、図２（ｂ）は図１の破線枠Ｂの部分を飛行装置１００の上方から下方を見た場合の上面図である。なお、破線枠Ａ及びＢは、説明のために付加した線である。この飛行装置１００は、本実施形態を、空中から写真撮影を行うことができるデジタルカメラユニットを搭載したドローンとして実施したものである。

本体部である筒状のフレーム１０１は、上方（空側）と下方（地面側）にそれぞれ開口部を有する。上方の開口部内には、図１及び図２（ａ）に示されるように、バッテリ１０４と、バッテリ１０４により駆動されるロータモータ１０２と、ロータモータ１０２の回転軸が接続されロータモータ１０２によって回転させられるロータ１０３が設置される。ロータモータ１０２及びロータ１０３は、飛行推進部の一部である。

フレーム１０１の内部には、ステータ１０７の中央部から降ろされたロッド１０８と、図２（ｂ）に示されるように、フレーム１０１の４カ所に設置された＃１から＃４のベーンモータ１０６の各回転軸とで支持される＃１から＃４のベーン１０５が設置される。各ベーン１０５は、それぞれに接続されたベーンモータ１０６の回転軸の回転によってそれぞれの羽の角度が制御されることにより、ロータ１０３から送風されてきて各ベーン１０５間の４カ所の間隙を流れる空気の各流入量を制御する流入弁の役割を有する。＃１から＃４のベーン１０５とベーンモータ１０６の組は、飛行推進部の一部である。

図１に示されるように、ステータ１０７の中央から降ろされたロッドの最下部（ベーン１０５の下側）には、検出部であるフライトセンサ１０９（フライトセンサ部）が設置される。フライトセンサ１０９は例えば、ジャイロセンサ（角速度センサ）、加速度センサ、地磁気センサ（方位センサ）、ＧＰＳ（全地球測位システム）センサ、気圧センサ、超音波センサ、レーザドップラセンサ、等を含んでよいが、少なくとも、例えば飛行装置１００の現在位置を検出するためのＧＰＳセンサと、飛行装置１００の現在速度を検出するための加速度センサとそれが出力する加速度を積分して速度を算出する回路を搭載する。また、飛行装置１００の高さを検出する気圧センサも搭載する。

フレーム１０１の外側表面には、情報取得装置の一部であるデジタルカメラユニット１１０と、制御部である回路ボックス１１１が設置される。デジタルカメラユニット１１０は、画像を撮像する。回路ボックス１１１には、図１又は図２のロータモータ１０２、＃１から＃４のベーンモータ１０６、フライトセンサ１０９、デジタルカメラユニット１１０、及びバッテリ１０４を制御するための回路群が格納される。

図３は、図１の回路ボックス１１１内の回路とそれらの回路に接続される周辺装置からなるシステムの例を示すブロック図である。回路ボックス１１１内には、コントローラ３０１、ロータモータドライバ３０２、＃１から＃４のベーンモータドライバ３０３、及びパワーセンサ３０４が格納される。

ロータモータドライバ３０２は、コントローラ３０１からの指示に基づいて図１のロータモータ１０２を駆動する。＃１から＃４のベーンモータドライバ３０３はそれぞれ、コントローラ３０１からの指示に基づいて、図１又は図２（ｂ）の＃１から＃４のベーンモータ１０６を駆動する。

パワーセンサ３０４は、バッテリ１０４の電圧をモニタしながら、ロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３に電力を供給する。なお、特には図示しないが、バッテリ１０４の電力の一部は、コントローラ３０１のほか、図１のフライトセンサ１０９及びデジタルカメラユニット１１０にも供給される。

コントローラ３０１は、フライトセンサ１０９から、飛行装置１００の機体の位置及び速度等に関する情報をリアルタイムで取得する。また、コントローラ３０１は、パワーセンサ３０６を介して、バッテリ１０４の電圧をモニタしながら、ロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３にそれぞれ、パルス幅変調に基づくデューティ比による電力指示信号を送信する。これにより、ロータモータドライバ３０２はロータモータ１０２の回転速度を制御し、＃１から＃４のロータモータドライバ３０２はそれぞれ＃１から＃４のロータモータ１０２の回転角度を制御する。また、コントローラ３０１は、デジタルカメラユニット１１０（図１）による撮影動作を制御する。

次に、本実施形態において、コントローラ３０１がロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３を制御する場合の、基本的な制御原理について説明する。本実施形態では、下記（１）式で示されるＰＩＤ制御が用いられる。

上記（１）式において、ｅ（ｔ）は、時刻ｔにおいて、後述するコントローラ３０１の制御処理で算出される目標値からフライトセンサ１０９より得られる現在の制御量を減算して得られる偏差である。また、ｕ（ｔ）は、時刻ｔにおいて、ロータモータドライバ３０２又は＃１から＃４のベーンモータドライバ３０３に対して与えられるべき操作量である。

上記（１）式で示されるＰＩＤ制御は、上記偏差に関する比例動作、積分動作、及び微分動作を組み合わせたフィードバック制御方法である。即ち、（１）式の右辺第１項で、操作量を制御量と目標値の偏差ｅ（ｔ）の一次関数として操作量ｕ（ｔ）を制御する比例制御（Ｐ制御：Ｐｒｏｐｏｒｔｉｏｎａｌ Ｃｏｎｔｒｏｌｌｅｒ）が実行される。この第１項で乗算される係数Ｋ_p は比例ゲイン（Ｐゲイン）と呼ばれる。このＰ制御により、操作量ｕ（ｔ）が目標値と現在の制御量との偏差ｅ（ｔ）に比例した大きさで徐々に調節されることになり、操作量ｕ（ｔ）を目標値にきめ細かく近づけることが可能となる。

また、（１）式の右辺第２項で、上記偏差ｅ（ｔ）の時間積分に比例して操作量ｕ（ｔ）を制御する積分制御（Ｉ制御：Ｉｎｔｅｇｒａｌ Ｃｏｎｔｒｏｌｌｅｒ）が実行される。この第２項で乗算される係数Ｋ_i は、積分ゲイン（Ｉゲイン）と呼ばれる。上記Ｐ制御のみだと、現在の制御量が目標値に近づくと操作量ｕ（ｔ）が小さくなりすぎ、それ以上細かくは制御できない状態が発生し、目標値に極めて近い現在の制御量の状態で安定した状態になってしまう。この僅かな誤差は「残留偏差」と呼ばれる。そこで、Ｐ制御に上記Ｉ制御を加えたＰＩ制御により、残留偏差を時間的に累積し、或る大きさになった所で操作量ｕ（ｔ）を増加させて残留偏差を無くすように動作させることが可能となる。

更に、（１）式の右辺第３項で、上記偏差ｅ（ｔ）の微分に比例して操作量ｕ（ｔ）を制御する微分制御（Ｄ制御：Ｄｉｆｆｅｒｅｎｔｉａｌ Ｃｏｎｔｒｏｌｌｅｒ）が実行される。この第３項で乗算される係数Ｋ_d は、微分ゲイン（Ｄゲイン）と呼ばれる。上記ＰＩ制御により現在の制御量を目標値に近づける制御が実現される。しかし、この制御には一定の時間（時定数）が必要となり、この時定数が大きいと外乱があった時の応答性能が悪くなり、すぐには元の目標値には戻せないという状態が発生する。そこで、ＰＩ制御に上記Ｄ制御を加えたＰＩＤ制御により、偏差ｅ（ｔ）に関して前回偏差との差、即ち微分値が大きい時には操作量を多くすることにより、急激に起きる外乱に対して機敏に反応するようなフィードバック制御が可能となる。

このように、操作量ｕ（ｔ）を偏差ｅ（ｔ）に関する比例項、積分項、及び微分項からなる３項の和として制御するＰＩＤ制御によって、ロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３において、それぞれの制御量を目標値に滑らかに到達させることができ、精度が高く、かつ応答性能の良い制御が可能となる。

コントローラ３０１は、上述のＰＩＤ制御を、例えばプログラム制御により実現する。この場合、コントローラ３０１は、一定時間間隔の離散時刻毎に、フライトセンサ１０９から得られる制御量の離散値から算出される偏差を用いて、下記（２）式及び（３）式に従って、今回の離散時刻における操作量を算出する。そして、コントローラ３０１は、このＰＩＤ制御をベースとするフィードバック制御処理により算出された各操作量を、ロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３に与えて、ロータモータ１０２及び＃１から＃４のベーンモータ１０６を駆動させる。

上記（２）式において、ｕ（ｎ）は今回の離散時刻ｎにおいて算出されるべき操作量、ｕ（ｎ−１）は前回の離散時刻ｎ−１において算出されている操作量、Δｕ（ｎ）は今回の離散時刻ｎにおいて算出されるべき操作量差分値である。また、この操作量差分値Δｕ（ｎ）を算出する演算を示す上記（３）式において、ｅ（ｎ）は目標値から今回の離散時刻ｎにおける制御量を減算して得られる今回の離散時刻ｎにおける偏差、ｅ（ｎ−１）は目標値から前回の離散時刻ｎ−１における制御量を減算して得られている前回の離散時刻ｎ−１における偏差、ｅ（ｎ−２）は目標値から前々回の離散時刻ｎ−２において得られている制御量を減算して得られる前々回の離散時刻ｎ−２における偏差である。

上記（３）式において、右辺第１項の比例制御の演算は、目標値から今回の離散時刻ｎにおける制御量を減算して得られる今回の離散時刻ｎにおける偏差ｅ（ｎ）から前回の離散時刻ｎ−１において算出されている偏差ｅ（ｎ−１）を減算して得られる結果にＰゲインＫ_p を乗算するという簡単な演算で算出できる。また、右辺第２項の積分制御の演算は、今回の離散時刻ｎにおける偏差ｅ（ｎ）にＩゲインＫ_i を乗算するという簡単な演算で算出できる。更に、右辺第３項の微分制御の演算は、今回の離散時刻ｎにおいて算出される偏差ｅ（ｎ）から前回の離散時刻ｎ−１において算出されている偏差ｅ（ｎ−１）を減算した結果から、前記の離散時刻ｎ−１において算出されている偏差ｅ（ｎ−１）から前々回の離散時刻ｎ−２において算出されている偏差ｅ（ｎ−２）を減算した結果を減算した結果にＤゲインＫ_d を乗算するという簡単な演算で算出できる。このようにして、コントローラ３０１は、目標値から今回の離散時刻ｎにおいてフライトセンサ１０９から得られる制御量を減算して得られる偏差ｅ（ｎ）と、前回及び前々回の各離散時刻ｎ−１及びｎ−２でそれぞれ算出されている偏差ｅ（ｎ−１）及びｅ（ｎ−２）と、予め算出されているＰゲインＫ_p 、ＩゲインＫ_i 、及びＤゲインＫ_d とを用いて、ＰＩＤ制御の離散時間演算を高速に実行することが可能となる。

図４は、コントローラ３０１がロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３を制御する場合の、上述のＰＩＤ制御を用いた本実施形態によるＰＩＤ制御機構を示すブロック図である。

コントローラ３０１が後述する制御処理を実行する動作であるアルゴリズム４０１において、飛行装置１００の位置を変更する要求が発生した場合には、まず、アルゴリズム４０１において、目的位置４１１が決定される。目的位置４１１は例えば、ユーザが飛行装置１００を例えば投げ上げた後に、飛行装置１００が到達すべき位置である。目的位置４１１は、緯度データ、経度データ、及び高度データとからなる。これに対して、ユーザが飛行装置１００を例えば投げ上げた後、アルゴリズム４０１には、フライトセンサ１０９中の例えばＧＰＳセンサ及び気圧センサから、現在の位置を示す現在位置４１２が逐次入力される。現在位置４１２は、ＧＰＳセンサから得られる緯度データ、経度データ、及び気圧センサから得られる高度データとからなる。

アルゴリズム４０１は、速度ＰＩＤ制御を実行すると決定した場合には、目的位置４１１の緯度データ及び経度データを、地面に水平な平面内で例えばフライトセンサ１０９中のＧＰＳセンサから得られている現在位置から目的位置４１１に向かう軸（以下「水平軸」）方向の成分値と、上記水平軸に垂直な軸（以下「垂直軸」）方向の成分値（前述した図７（ｂ）の説明を参照）とかなるベクトルデータである目的２次元速度４１３に変換し出力する。

以下に説明する減算部４０２と４０６、ＰＩＤ制御部４０３と４０７、減算部４０６、及び操作量混合部４０４は、目的２次元速度４１３の２つの成分値及び後述する目的２次元位置４１９の２つの成分値に対応して２系統あるが、図４及び以下の記述では、説明の簡単のために１系統のみについて示す。

アルゴリズム４０１が出力される目的２次元速度４１３の１つの成分値は、減算部４０５に入力する。減算部４０２には、また、現在速度４１４の上記水平軸方向の成分値又は上記垂直軸方向の成分値（前述した図７（ｂ）の７０６及び７０７に対応）の何れかの対応する成分値が入力する。この現在速度４１４は、フライトセンサ１０９中のＧＰＳセンサから検出される緯度データ及び経度データを、上記水平軸方向と垂直軸方向の各成分値に変換したデータである。減算部４０２は、コントローラ３０１が制御プログラム中で減算処理を実行することにより実現される機能である。減算部４０２は、前述した離散時刻ｎ毎に、目的２次元速度４１３の成分値から現在速度４１４の成分値を減算することにより、上記水平軸又は垂直軸に対応する２次元速度偏差４１５の成分値を算出する。

上記離散時刻ｎ毎に算出される２次元速度偏差４１５の成分値は、前述した（３）式における離散時刻ｎにおける偏差ｅ（ｎ）として、ＰＩＤ制御部４０３に入力する。ＰＩＤ制御部４０３は、コントローラ３０１が制御プログラム中で前述した（３）式及び（２）式のＰＩＤ制御演算を実行することにより実現される機能である。ＰＩＤ制御部４０３は、前述したように、離散時刻ｎにおいて減算部４０２で算出される２次元速度偏差４１５の上記水平軸又は垂直軸に対応する成分値である偏差ｅ（ｎ）と、前回及び前々回の各離散時刻ｎ−１及びｎ−２でそれぞれ算出されている２次元速度偏差４１５の上記水平軸又は垂直軸に対応する成分値である偏差ｅ（ｎ−１）及びｅ（ｎ−２）と、予め算出されているＰゲインＫ_p 、ＩゲインＫ_i 、及びＤゲインＫ_d と、前回の離散時刻ｎ−１において算出されている２次元速度操作量４１６の上記水平軸又は垂直軸に対応する成分値である操作量ｕ（ｎ−１）とを用いて、前述した（３）式及び（２）式で示される演算を実行することで、今回の離散時刻ｎにおける操作量ｕ（ｎ）として２次元速度操作量４１６の上記水平軸又は垂直軸に対応する成分値を算出する。コントローラ３０１は、上記水平軸又は垂直軸に対応する各系統のＰＩＤ制御部４０２で算出された２次元速度操作量４１６の上記水平軸及び垂直軸に対応する各成分値を、操作量混合部４０４に出力する。

アルゴリズム４０１は、上記速度ＰＩＤ制御と並行して位置ＰＩＤ制御を実行すると決定した場合には、目的位置４１１の緯度データ及び経度データを、前述した水平軸方向の成分値と、前述した垂直軸方向の成分値とかなるベクトルデータである目的２次元位置４１９に変換し出力する。速度ＰＩＤ制御と並行して位置ＰＩＤ制御を実行する処理は、ハイブリッドＰＩＤ制御処理と呼ぶ。

アルゴリズム４０１が出力される目的２次元位置４１９の１つの成分値は、減算部４０６に入力する。減算部４０６には、また、前述した現在位置４１２の上記水平軸方向の成分値又は上記垂直軸方向の成分値の何れかの対応する成分値が入力する。この現在位置４１２は、フライトセンサ１０９中のＧＰＳセンサから検出される緯度データ及び経度データを、上記水平軸方向と垂直軸方向の各成分値に変換したデータである。減算部４０６は、減算部４０２と同様に、コントローラ３０１が制御プログラム中で減算処理を実行することにより実現される機能である。減算部４０６は、前述した離散時刻ｎ毎に、目的２次元位置４１９の成分値から現在位置４１２の成分値を減算することにより、上記水平軸又は垂直軸に対応する２次元位置偏差４２０の成分値を算出する。

上記離散時刻ｎ毎に算出される２次元位置偏差４２０の成分値は、前述した（３）式における離散時刻ｎにおける偏差ｅ（ｎ）として、ＰＩＤ制御部４０７に入力する。ＰＩＤ制御部４０７は、ＰＩＤ制御部４０３と同様に、コントローラ３０１が制御プログラム中で前述した（３）式及び（２）式のＰＩＤ制御演算を実行することにより実現される機能である。ＰＩＤ制御部４０７は、前述したように、離散時刻ｎにおいて減算部４０２で算出される２次元位置偏差４２０の上記水平軸又は垂直軸に対応する成分値である偏差ｅ（ｎ）と、前回及び前々回の各離散時刻ｎ−１及びｎ−２でそれぞれ算出されている２次元位置偏差４２０の上記水平軸又は垂直軸に対応する成分値である偏差ｅ（ｎ−１）及びｅ（ｎ−２）と、予め算出されているＰゲインＫ_p 、ＩゲインＫ_i 、及びＤゲインＫ_d と、前回の離散時刻ｎ−１において算出されている２次元位置操作量４２１の上記水平軸又は垂直軸に対応する成分値である操作量ｕ（ｎ−１）とを用いて、前述した（３）式及び（２）式で示される演算を実行することで、今回の離散時刻ｎにおける操作量ｕ（ｎ）である２次元位置操作量４２１の上記水平軸又は垂直軸に対応する成分値を算出する。コントローラ３０１は、上記水平軸又は垂直軸に対応する各系統のＰＩＤ制御部４０７で算出された２次元位置操作量４２１の上記水平軸及び垂直軸に対応する各成分値を、操作量混合部４０４に出力する。

アルゴリズム４０１が速度ＰＩＤ制御のみを実行すると決定している場合には、上記水平軸又は垂直軸に対応する各系統の操作量混合部４０４は、ＰＩＤ制御部４０３から出力される２次元速度操作量４１６の上記水平軸又は垂直軸に対応する各成分値をそのまま、最終的な操作量４１７の上記水平軸又は垂直軸に対応する各成分値として操作量変換部４０５に出力する。一方、アルゴリズム４０１が速度ＰＩＤ制御に加えて位置ＰＩＤ制御も実行すると決定している場合には、上記水平軸又は垂直軸に対応する各系統の操作量混合部４０４は、ＰＩＤ制御部４０３から出力される２次元速度操作量４１６の上記水平軸又は垂直軸に対応する各成分値と、ＰＩＤ制御部４０７から出力される２次元位置操作量４２１の上記水平軸又は垂直軸に対応する各成分値とにそれぞれ、後述するコントローラ３０１による制御処理で決定される各重み値を乗算し、各乗算結果を加算し、各加算結果を最終的な操作量４１７の上記水平軸又は垂直軸に対応する各成分値として操作量変換部４０５に出力する。

操作量変換部４０５は、上記水平軸又は垂直軸に対応する各系統の操作量混合部４０４からそれぞれ入力する最終的な操作量４１７の上記水平軸又は垂直軸に対応する各成分値に基づいて、＃１から＃４のベーンモータ１０６（図１、図２（ｂ）参照）を駆動するための＃１から＃４のベーンモータ回転角４１８を生成し、それぞれ＃１から＃４のベーンモータドライバ３０３（図３参照）に出力する。

一方、アルゴリズム４０１から出力される目的高さ４２２は、減算部４０８に入力する。減算部４０８にはまた、現在高さ４２３が入力する。この現在高さ４２３は例えば、フライトセンサ１０９中の気圧センサの出力データである。減算部４０８は、減算部４０２等と同様に、コントローラ３０１が制御プログラム中で減算処理を実行することにより実現される機能である。減算部４０８は、前述した離散時刻ｎ毎に、目的高さ４２２から現在高さ４２３を減算することにより、高さ偏差４２４を算出する。この離散時刻ｎ毎に算出される高さ偏差４２４は、前述した（３）式における離散時刻ｎにおける偏差ｅ（ｎ）として、ＰＩＤ制御部４０９に入力する。ＰＩＤ制御部４０９は、ＰＩＤ制御部４０３等と同様の、コントローラ３０１が制御プログラム中で前述した（３）式及び（２）式のＰＩＤ制御演算を実行することにより実現される機能である。ＰＩＤ制御部４０９は、前述のように、離散時刻ｎにおいて減算部４０８で算出される高さ偏差４２４である偏差ｅ（ｎ）と、前回及び前々回の各離散時刻ｎ−１及びｎ−２でそれぞれ算出されている高さ偏差４２４である偏差ｅ（ｎ−１）及びｅ（ｎ−２）と、予め算出されているＰゲインＫ_p 、ＩゲインＫ_i 、及びＤゲインＫ_d と、前回の離散時刻ｎ−１において算出されている高さ操作量４２５である操作量ｕ（ｎ−１）とを用いて、前述した（３）式及び（２）式で示される演算を実行することで、今回の離散時刻ｎにおける操作量ｕ（ｎ）である高さ操作量４２５を算出する。

操作量変換部４１０は、ＰＩＤ制御部４０９から入力する高さ操作量４２５に基づき、ロータモータ１０２（図１参照）を駆動するためのロータモータ回転数４２６を生成し、ロータモータドライバ３０２（図３参照）に出力する。

アルゴリズム４０１において更に目的位置４１１の変更が必要ならば、上述と同様のＰＩＤ制御をベースとするフィードバック制御処理が繰り返し実行される。

図５は、図３のコントローラ３０１の移動制御の処理例を示すフローチャートである。この処理は、コントローラ３０１が内蔵するＣＰＵが、同じく内蔵する特には図示しないメモリに記憶された制御プログラムを実行する処理として実現することができる。

コントローラ３０１は、ユーザが飛行装置１００を例えば投げ上げる処理（図示せず）を行った後、他の制御処理（図示せず）に基づいて、図４の目的位置４１１を設定する（ステップＳ５０１）。目的位置４１１は例えば、ユーザが飛行装置１００を例えば投げ上げた後に、飛行装置１００が到達すべき位置である。目的位置４１１は、緯度データ、経度データ、及び高度データとからなる。

次に、コントローラ３０１は、ステップＳ５０１で設定した目的位置４１１の緯度データ及び経度データを、図４の説明で前述したように、水平軸方向の成分値と垂直軸方向の成分値とかなるベクトルデータである目的２次元速度４１３に変換し、設定する（ステップＳ５０２）。

その後、コントローラ３０１は、下記のステップＳ５０３からＳ５０６の一連の処理を繰り返し実行する。コントローラ３０１はまず、フライトセンサ１０９中の例えばＧＰＳセンサ及び気圧センサから、現在位置４１２を検出する（ステップＳ５０３）。現在位置４１２は、ＧＰＳセンサから得られる緯度データ、経度データ、及び気圧センサから得られる高度データとからなる。

次に、コントローラ３０１は、ステップＳ５０３で検出した現在位置４１２からステップＳ５０１で設定した目的位置４１１までの直線距離を、現在距離として算出する（ステップＳ５０４）。

コントローラ３０１は、ステップＳ５０４で算出した現在距離が、所定の距離閾値よりも大きいか否かを判定する（ステップＳ５０５）。

ステップＳ５０５の判定がＹＥＳならば、コントローラ３０１は、目的２次元速度４１３に関して、図４の減算部４０２及びＰＩＤ制御部４０３に関して説明した速度ＰＩＤ制御のみの処理を実行する。このとき、図４の減算部４０６及びＰＩＤ制御部４０７に関して説明した位置ＰＩＤ制御の処理は実行しない。この結果、コントローラ３０１は、ＰＩＤ制御部４０３から出力される２次元速度操作量４１６の各成分値をそのまま、最終的な操作量４１７の各成分値として操作量変換部４０５に出力する。更に、コントローラ３０１は、操作量変換部４０５で＃１から＃４のベーンモータ回転角４１８を生成し、それぞれ＃１から＃４のベーンモータドライバ３０３（図３参照）に出力する（以上、ステップＳ５０６）。その後、コントローラ３０１は、ステップＳ５０３の処理に戻って、ステップＳ５０３からＳ５０６の処理を繰り返し実行する。

上記繰返し処理の結果、飛行装置１００が目的位置４１１に近づいて、現在距離が所定の距離閾値以下（ステップＳ５０５の判定がＮＯ）になると、コントローラ３０１は、以下のようにし、速度ＰＩＤ制御と位置ＰＩＤ制御の並行処理であるハイブリッドＰＩＤ制御処理を実行する。ハイブリッドＰＩＤ制御処理において、コントローラ３０１はまず、目的位置４１１と現在位置４１２の距離（現在距離）を残り行程距離として設定する（ステップＳ５０７）。

次に、コントローラ３０１は、ステップＳ５１２で飛行装置１００が目的位置４１１に到達したと判定するまで、ステップＳ５０８からＳ５１２の一連の制御処理を実行する。コントローラ３０１はまず、ステップＳ５０３と同様の現在位置４１２の検出（ステップＳ５０８）と、ステップＳ５０４と同様の現在距離の算出の処理（ステップＳ５０９）を実行する。

続いて、コントローラ３０１は、ステップＳ５０９で算出した現在距離をステップＳ５０７で算出した残り行程距離で除算して得られる結果、即ち、現在距離の残り行程距離に対する比を、重み値として算出する（ステップＳ５１０）。

そして、コントローラ３０１は、目的２次元速度４１３に関して、図４の減算部４０２及びＰＩＤ制御部４０３に関して説明した速度ＰＩＤ制御の処理を実行する。これと並行し、目的２次元位置４１９に関して、図４の減算部４０６及びＰＩＤ制御部４０７に関して説明した位置ＰＩＤ制御の処理を実行する。更に、コントローラ３０１は、各系統の操作量混合部４０４の処理で、下記（４）式の演算で示されるように、ＰＩＤ制御部４０３から出力される２次元速度操作量４１６の各成分値にステップＳ５１０で算出した重み値を乗算した各結果と、ＰＩＤ制御部４０７から出力される２次元位置操作量４２１の各成分値に（１―重み値）を乗算した各結果をそれぞれ、操作量４１７の各成分値として操作量変換部４０５に出力する。

操作量４１７の成分値＝２次元速度操作量４１６の成分値×重み値＋
２次元位置操作量４２１の成分値×（１−重み値）
・・・（４）

コントローラ３０１は、操作量変換部４０５の処理として、各系統の操作量混合部４０４における上記（４）式の演算結果としてそれぞれ入力する操作量４１７の各成分値に基づいて、＃１から＃４のベーンモータ１０６（図１、図２（ｂ）参照）を駆動するための＃１から＃４のベーンモータ回転角４１８を生成し、それぞれ＃１から＃４のベーンモータドライバ３０３（図３参照）に出力する（以上、ステップＳ５１１）。

その後、コントローラ３０１は、ステップＳ５０９で算出した現在距離がほぼ０になったか否かを判定することにより、飛行装置１００がステップＳ５０１で設定した目的位置４１１に到達したか否かを判定する（ステップＳ５１２）。

ステップＳ５１２の判定がＮＯならば、コントローラ３０１は、ステップＳ５０８の処理に戻って、ステップＳ５０８からＳ５１２の処理を繰り返し実行する。

図６は、以上説明した図１から図５に基づくコントローラ３０１の制御処理により実現される本実施形態の動作説明図である。例えば、ユーザが飛行装置１００を投げ上げた後に、飛行装置１００が自律飛行によって或る軌道を描いて飛行し、手元に戻ってくるような移動を行う場合を想定する。このとき、飛行装置１００が、図６に示される軌道中の＃１から＃４として例示される複数の中間位置６０２を経由して、終着点である目的位置６０１に到達するような場合に、本実施形態では、＃１から＃４の移動制御６０３では、速度ＰＩＤ制御のみによってほぼ一定の速度を保ち、＃４の中間位置６０２から目的位置６０１までの移動制御６０４では、速度ＰＩＤ制御から位置ＰＩＤ制御に徐々に移行する制御により目的位置４１１でのなめらかな停止を両立することが可能となる。

以上説明した実施形態では、現在距離が所定の距離閾値に比較して小さくなったときにハイブリッドＰＩＤ制御処理が実行されるようにしたが、飛行開始直後からハイブリッドＰＩＤ制御処理が実行されるようにしてもよく、その他様々な条件でハイブリッドＰＩＤ制御処理が実行されるようにしてもよい。
また、以上説明した実施形態では、速度ＰＩＤ制御と位置ＰＩＤ制御の強さが徐々に変化するようにしたが、２段階または複数段階で強さが切り替わるようにしても良い。

以上説明した実施形態は、飛行装置にデジタルカメラユニットが搭載される例について説明したが、そのほか、例えば温度分布や大気成分の分布を収集するセンサによって構成される測定装置をはじめとする、各種センサ装置類が飛行装置に搭載されてもよく、これらのものが搭載されなくてもよい。

上述の実施形態は、ロータモータ１０２を１つ搭載し、ベーンモータ１０６を＃１から＃４の４個搭載した、いわゆるダクテッドファン型の装置であるが、このほか、ロータモータ１０２を複数（４個又は６個等）搭載したマルチコプター型の装置であってもよい。或いは、空気圧やエンジン出力により推進される機構により飛行推進部が実現されてもよい。

以上の実施形態に関して、更に以下の付記を開示する。
（付記１）
飛行推進部を備える飛行装置であって、
少なくとも現在位置と現在速度を検出するセンサ部と、
前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行う制御部と、
を備えることを特徴とする飛行装置。
（付記２）
前記制御部は、前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が前記位置フィードバック制御よりも強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が前記速度フィードバック制御より強くかかるフィードバック制御を行なう、付記１に記載の飛行装置。
（付記３）
前記制御部は、前記速度フィードバック制御と前記位置フィードバック制御に対してそれぞれ前記現在距離に応じた重み付けを行って得られる各操作量を加算して得られる操作量を前記飛行推進部に指示する、付記１又は２に記載の飛行装置。
（付記４）
前記制御部は、前記現在距離が所定の距離閾値に比較して大きいときには、前記速度フィードバック制御で出力される操作量を前記飛行推進部に指示し、前記現在距離が前記所定の距離閾値に比較して小さくなると、該時点での前記現在位置と前記目的位置との距離を残り行程距離として、前記速度フィードバック制御に対しては前記現在距離の前記残り行程距離に対する比に比例した大きさの重み付けを行い、前記位置フィードバック制御に対しては前記比に反比例した大きさの重み付けを行い、前記速度フィードバック制御に重み付けをして得られる操作量と前記位置フィードバック制御に対して重み付けをして得られる操作量とを加算して得られる操作量を前記飛行推進部に指示する、付記３に記載の飛行装置。
（付記５）
前記フィードバック制御は、現在の制御量と目標値との差として得られる偏差に比例する操作量と、前記偏差を積分して得られる操作量と、前記偏差を微分して得られる操作量とを加算して得られる操作量を出力する制御である、付記１乃至４の何れかに記載の飛行装置。
（付記６）
飛行推進部を備える飛行装置の制御方法であって、
センサ部によって少なくとも現在位置と現在速度を検出し、
前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行う、
ことを特徴とする飛行装置の制御方法。
（付記７）
飛行推進部を備える飛行装置を制御するコンピュータに、
センサ部によって少なくとも現在位置と現在速度を検出するステップと、
前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行うステップと、
を実行させるためのプログラム。

１００ 飛行装置
１０１ フレーム
１０２ ロータモータ
１０３ ロータ
１０４ バッテリ
１０５ ベーン
１０６ ベーンモータ
１０７ ステータ
１０８ ロッド
１０９ フライトセンサ
１１０ カメラ
３０１ コントローラ
３０２ ロータモータドライバ
３０３ ベーンモータドライバ
３０４ パワーセンサ
４０１ アルゴリズム
４０２、４０６、４０８ 減算部
４０３、４０７、４０９ ＰＩＤ制御部
４０４ 操作量混合部
４０５、４１０ 操作量変換部
４１１ 目的位置
４１２ 現在位置
４１３ 目的２次元速度
４１４ 現在速度
４１５ ２次元速度偏差
４１６ ２次元速度操作量
４１７ 操作量
４１８ ベーンモータ回転角
４１９ 目的２次元位置
４２０ ２次元位置偏差
４２１ ２次元位置操作量
４２２ 目的高さ
４２３ 現在高さ
４２４ 高さ偏差
４２５ 高さ操作量
４２６ ロータモータ回転数

Claims (7)

1. 飛行推進部を備える飛行装置であって、
   少なくとも現在位置と現在速度を検出するセンサ部と、
   前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行う制御部と、
   を備えることを特徴とする飛行装置。
2. 前記制御部は、前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が前記位置フィードバック制御よりも強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が前記速度フィードバック制御より強くかかるフィードバック制御を行なう、請求項１に記載の飛行装置。
3. 前記制御部は、前記速度フィードバック制御と前記位置フィードバック制御に対してそれぞれ前記現在距離に応じた重み付けを行って得られる各操作量を加算して得られる操作量を前記飛行推進部に指示する、請求項１又は２に記載の飛行装置。
4. 前記制御部は、前記現在距離が所定の距離閾値に比較して大きいときには、前記速度フィードバック制御で出力される操作量を前記飛行推進部に指示し、前記現在距離が前記所定の距離閾値に比較して小さくなると、該時点での前記現在位置と前記目的位置との距離を残り行程距離として、前記速度フィードバック制御に対しては前記現在距離の前記残り行程距離に対する比に比例した大きさの重み付けを行い、前記位置フィードバック制御に対しては前記比に反比例した大きさの重み付けを行い、前記速度フィードバック制御に重み付けをして得られる操作量と前記位置フィードバック制御に対して重み付けをして得られる操作量とを加算して得られる操作量を前記飛行推進部に指示する、請求項３に記載の飛行装置。
5. 前記フィードバック制御は、現在の制御量と目標値との差として得られる偏差に比例する操作量と、前記偏差を積分して得られる操作量と、前記偏差を微分して得られる操作量とを加算して得られる操作量を出力する制御である、請求項１乃至４の何れかに記載の飛行装置。
6. 飛行推進部を備える飛行装置の制御方法であって、
   センサ部によって少なくとも現在位置と現在速度を検出し、
   前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行う、
   ことを特徴とする飛行装置の制御方法。
7. 飛行推進部を備える飛行装置を制御するコンピュータに、
   センサ部によって少なくとも現在位置と現在速度を検出するステップと、
   前記センサ部が検出した現在位置と目的位置との現在距離が遠いほど前記センサ部が検出した現在速度と前記目的位置に対応する目的速度とに基づく速度フィードバック制御が強くかかり、前記現在距離が近くなるほど前記現在位置と前記目的位置とに基づく位置フィードバック制御が強くかかるフィードバック制御を行うステップと、
   を実行させるためのプログラム。