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

Abstract

【課題】自律飛行する飛行装置の制御技術に関し、地面又は床面方向を撮影して得られる画像から水平移動量を算出する場合の計算量を削減する。【解決手段】処理部は、フライトセンサ部が検出した飛行装置の高度が所定の高度閾値に比較して高い場合には、撮影部が撮影した地面又は床面の方向の画像に対して画角を狭くするトリミング処理を実行する。また、処理部は、飛行装置の高度が所定の高度閾値に比較して低い場合には、上記画像に対して画角は変えずに画像サイズを小さくする縮小処理を実行する。処理部は、トリミング処理又は縮小処理の後の画像から飛行装置の水平移動量を算出する。処理部は、算出した水平移動量に基づいて、飛行装置が空中でホバリングを維持するように飛行装置の飛行推進部を制御する。地面又は床面方向を撮影して得られる画像から水平移動量を算出する場合の計算量が、飛行装置の高度によらずほぼ一定となるように制御することが可能となる。【選択図】図４

Description

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

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

このような無人飛行機を移動状態から空中で停止するいわゆるホバリング状態に自動制御するときに、水平方向に移動（並進）したことを検知してその並進量を補正するために従来、オプティカルフローと呼ばれる技術が知られている（例えば特許文献３）。オプティカルフロー技術では、下向きのカメラにより撮影した画像から適切な特徴点が抽出され、画像の連続するフレーム間の特徴点の位置の差分から動きを表すベクトルが推定され、その動きベクトルに基づいて水平移動量が算出される。そして、この水平移動量を補正するように、無人飛行機の飛行推進装置の推力がサーボ制御される。

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

無人飛行機の水平移動量を上述したオプティカルフローを用いて算出する場合には、画像から適切な特徴点を抽出するための演算処理が必要となる。しかし、無人飛行機における下向き画像は、無人飛行機の高度によって撮影範囲が変わるため、オプティカルフローを算出するために画像内に写る特徴点の範囲も変わってくる。このため、無人飛行機の高度が例えば高い場合には、オプティカルフローを算出するための計算量が増加してしまうという課題があった。

本発明は、地面又は床面方向を撮影して得られる画像から水平移動量を算出する場合の計算量を削減することを目的とする。

態様の一例は、飛行推進部と処理部とを備える飛行装置であって、地面又は床面の方向の画像を撮影する下向き撮影部と、飛行装置の少なくとも姿勢及び高度を検出するフライトセンサ部と、を備え、処理部は、フライトセンサ部が検出した飛行装置の姿勢及び高度に応じて、撮影部が撮影した画像に対してトリミング処理又は縮小処理を実行し、トリミング処理又は縮小処理の後の画像から飛行装置の水平移動量を算出する。

本発明は、地面又は床面方向を撮影して得られる画像から水平移動量を算出する場合の計算量を削減することが可能となる。

本実施形態による飛行装置の構造例を示す横断面図である。 本実施形態による飛行装置の構造例を示す上面図である。 本実施形態による飛行装置のシステムの例を示すブロック図である。 本実施形態の動作説明図（その１）である。 本実施形態の動作説明図（その２）である。 コントローラがロータモータドライバ及びベーンモータドライバを制御する場合の基本的な制御機構を示すブロック図である。 コントローラによる水平移動量算出のための制御処理例を示すフローチャートである。

以下、本発明を実施するための形態について図面を参照しながら詳細に説明する。本実施形態では、処理部は、フライトセンサ部が検出した飛行装置の高度が所定の高度閾値に比較して高い場合には、撮影部が撮影した地面又は床面の方向の画像に対して画角を狭くするトリミング処理を実行する。また、処理部は、飛行装置の高度が所定の高度閾値に比較して低い場合には、上記画像に対して画角は変えずに画像サイズを小さくする縮小処理を実行する。この場合、処理部は、フライトセンサ部内のジャイロセンサによって飛行装置の姿勢を検出し、検出した姿勢に基づいて飛行装置の地面又は床面に対する傾きを算出し、その傾きに従って画像の画角内で地面又は床面に対して垂直な点の座標を算出し、その垂直な点の座標を中心座標として上述のトリミング処理を実行する。処理部は、上述のトリミング処理又は縮小処理の後の画像から飛行装置の水平移動量を算出する。そして、処理部は、算出した水平移動量に基づいて、飛行装置が空中でホバリングを維持するように飛行装置の飛行推進部を制御する。
このような制御処理により、地面又は床面方向を撮影して得られる画像から水平移動量を算出する場合の計算量が、飛行装置の高度によらずほぼ一定となるように制御することが可能となる。この結果、飛行装置を自動でホバリングさせるときに、補正のための水平移動量を計算する際の演算量を減らすことができるため、処理時間の短縮につながり、消費電力の低減に効果がある。また、少ないプロセッサ負荷で水平移動量を計算できるため、専用のハードウェアを備ける必要がなく、計算能力の低いプロセッサのみでも処理が可能となる。
更に、ジャイロセンサにより地面又は床面方向を撮影した画像の画角内に地面又は床面に対して垂直な点を中心座標を設定し、トリミング処理を行うことで、飛行装置の傾きに対する影響を排除することが可能となる。

図１及び図２はそれぞれ、本実施形態による飛行装置１００の構造例を示す横断面図及び上面図である。図２（ａ）は図１の破線枠Ａの部分を飛行装置１００の上方から下方を見た場合の上面図、図２（ｂ）は図１の破線枠Ｂの部分を飛行装置１００の上方から下方を見た場合の上面図である。なお、破線枠Ａ及びＢは、説明のために付加した線である。

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

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

図１に示されるように、ステータ１０７の中央から降ろされたロッドの最下部（ベーン１０５の下側）には、フライトセンサ１０９（フライトセンサ部）及び下向きカメラユニット１１２（撮影部）が設置される。フライトセンサ１０９は例えば、ジャイロセンサ（角速度センサ）、加速度センサ、地磁気センサ（方位センサ）、ＧＰＳ（全地球測位システム）センサ、気圧センサ、超音波センサ、レーザドップラセンサ、等を含む。下向きカメラユニット１１２は、地面又は床面方向を撮影するデジタルカメラユニットである。

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

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

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

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

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

このとき特に、コントローラ３０１は、下向きカメラユニット１１２が撮影した地面又は床面方向の画像を取得し、この画像とフライトセンサ１０９中の超音波センサから得ている高度のデータとから、水平移動量を算出する。そして、コントローラ３０１は、この水平移動量を補正するような制御信号を、＃１から＃４のロータモータドライバ３０２に対して与える。

図４及び図５は、上述の水平移動量の算出処理の詳細を説明するための本実施形態の動作説明図である。本実施形態では、コントローラ３０１は、図４（ａ）に示されるように、フライトセンサ１０９中の超音波センサ４０１が検出する高度が所定の閾値４０４（高度閾値）に比較して高い場合には、下向きカメラユニット１１２が撮影した画像４０３に対してトリミング処理を実行する。このトリミング処理により、例えば横６４０ｐｉｘ（ピクセル）×縦４８０ｐｉｘの画像４０３において、解像度は変えられずに、例えば横８０ｐｉｘ×６０ｐｉｘの画角の範囲のみが切り出される。

一方、コントローラ３０１は、図４（ｂ）に示されるように、超音波センサ４０１が検出する高度が所定の閾値４０４に比較して低い場合には、下向きカメラユニット１１２が撮影した画像４０３に対して縮小処理を実行する。この縮小処理により、例えば横６４０ｐｉｘ×縦４８０ｐｉｘの画像４０３が、画角は変えられずに解像度が低下させられることにより、例えば横８０ｐｉｘ×６０ｐｉｘのサイズの画像に変換される。

ここで、コントローラ３０１は、上述のトリミング処理を実行する際に、図５に示されるように、フライトセンサ１０９中のジャイロセンサ４０２によって飛行装置１００の姿勢（図５の方向Ｂ）を検出し、検出した姿勢に基づいて飛行装置の地面又は床面に対する傾きを算出し、その傾きに従って画像４０３の画角内で地面又は床面に対して垂直な点の座標Ａ（図５）を算出し、その垂直な点の座標Ａを中心座標として上述のトリミング処理を実行する。このとき、コントローラ３０１は例えば、飛行装置１００の姿勢をヨー軸、ロール軸、及びピッチ軸まわりを基準として算出する。

コントローラ３０１は、上述のようにしてトリミング処理又は縮小処理を行って得られる画像に対して、既存のオプティカルフロー処理による水平移動量の算出処理を実行する。コントローラ３０１は、算出した水平移動量に基づいて、飛行装置１００が空中でホバリングを維持するように、＃１から＃４のベーンモータドライバ３０３を制御する。

このような水平移動量算出のための制御処理により、画像４０３から水平移動量を算出する場合の計算量が、飛行装置の高度によらずほぼ一定となるように制御することが可能となる。図４（ａ）及び（ｂ）の例では、画角及び解像度がほぼ一定の縦８０ｐｉｘ×横６０ｐｉｘの画像に対してオプティカルフローの処理が実行されることになる。この結果、飛行装置を自動でホバリングさせるときに、補正のための水平移動量を計算する際の演算量を減らすことができるため、処理時間の短縮につながり、消費電力の低減に効果がある。また、少ないプロセッサ負荷で水平移動量を計算できるため、専用のハードウェアを備ける必要がなく、計算能力の低いプロセッサのみでも処理が可能となる。

更に、現在の姿勢から算出される地面又は床面に対して垂直な点Ａを中心としてトリミング処理を行うことで、飛行装置１００の傾きに対する影響を排除することが可能となる

次に、本実施形態において、上述の水平移動量による補正も含めて、コントローラ３０１がロータモータドライバ３０２及び＃１から＃４のベーンモータドライバ３０３を制御する場合の、基本的な制御原理について説明する。本実施形態では、下記（１）式で示されるＰＩＤ制御（Ｐｒｏｐｏｒｔｉｏｎａｌ−Ｉｎｔｅｇｒａｌ−Ｄｉｆｆｅｒｅｎｔｉａｌ Ｃｏｎｔｒｏｌｌｅｒ）と呼ばれるフィードバック制御が用いられる。

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

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

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

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

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

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

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

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

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

コントローラ３０１が、後述する水平移動量算出のための制御処理等のアルゴリズム６０１において、飛行装置１００の位置を変更する、例えば水平移動量を補正する要求が発生した場合には、まず、アルゴリズム６０１に、フライトセンサ１０９中の例えばＧＰＳセンサ及び気圧センサから、現在の位置を示す現在位置６１１が入力される。現在位置６１１は、ＧＰＳセンサから得られる緯度データ、経度データ、及び気圧センサから得られる高度データとからなる。アルゴリズム６０１においては、変更要求後の位置と上記現在位置６１１とから、、目標軌道６１０が決定される。目標軌道６１０は例えば、上記現在位置６１１から、下向きカメラユニット１１２から得られる画像から決定されるホバリング位置までの飛行軌道である。次に、アルゴリズム６０１は、目標軌道６１０に対応する目標位置６１２と目標高さ６１３を出力する。目標位置６１２は、緯度データ及び経度データとからなる。

アルゴリズム６０１から出力される目標位置６１２は、差分演算部６０５に入力する。差分演算部６０５には、また、現在位置６１１が入力する。この現在位置６１１は例えば、フライトセンサ１０９中のＧＰＳセンサから出力される緯度データと経度データ、又はフライトセンサ１０９中のジャイロセンサ（角速度センサ）の出力を累算しその累算結果をフライトセンサ１０９中の加速度センサの出力で補正したデータである。差分演算部６０５は、コントローラ３０１が制御プログラム中で目標位置６１２と現在位置６１１の差分を演算する処理を実行することにより実現される機能である。差分演算部６０５は、前述した離散時刻ｎ毎に、目標位置６１２と現在位置６１１の差分を演算することにより、位置偏差６１６を算出する。この離散時刻ｎ毎に算出される位置偏差６１６は、前述した（３）式における離散時刻ｎにおける偏差ｅ（ｎ）として、ＰＩＤ制御部６０２に入力する。ＰＩＤ制御部６０２は、コントローラ３０１が制御プログラム中で前述した（３）式及び（２）式のＰＩＤ制御演算を実行することにより実現される機能である。ＰＩＤ制御部６０２は、前述したように、離散時刻ｎにおいて差分演算部６０５で算出される位置偏差６１６である偏差ｅ（ｎ）と、前回及び前々回の各離散時刻ｎ−１及びｎ−２でそれぞれ算出されている位置偏差６１６である偏差ｅ（ｎ−１）及びｅ（ｎ−２）と、予め算出されているＰゲインＫ_p 、ＩゲインＫ_i 、及びＤゲインＫ_d と、前回の離散時刻ｎ−１において算出されている操作量ｕ（ｎ−１）とを用いて、前述した（３）式及び（２）式で示される演算を実行することで、今回の離散時刻ｎにおける操作量ｕ（ｎ）を算出する。コントローラ３０１は、ＰＩＤ制御部６０２で算出された操作量ｕ（ｎ）を算出し、更に、その操作量ｕ（ｎ）に対応する目標姿勢６１７を出力する。

上記出力される目標姿勢６１７は、差分演算部６０６に入力する。差分演算部６０６には、また、上記１つのベーン１０５に対応する現在姿勢６１８が入力する。この現在姿勢６１８は例えば、フライトセンサ１０９中のジャイロセンサ４０２(図４参照)の出力データである。差分演算部６０６は、コントローラ３０１が制御プログラム中で目標姿勢６１７と現在姿勢６１８の差分を演算する処理を実行することにより実現される機能である。差分演算部６０６は、前述した離散時刻ｎ毎に、目標姿勢６１７と現在姿勢６１８の差分を演算することにより、姿勢偏差６１９を算出する。この離散時刻ｎ毎に算出される姿勢偏差６１９は、前述した（３）式における離散時刻ｎにおける偏差ｅ（ｎ）として、ＰＩＤ制御部６０３に入力する。ＰＩＤ制御部６０３は、ＰＩＤ制御部６０２と同様の、コントローラ３０１が制御プログラム中で前述した（３）式及び（２）式のＰＩＤ制御演算を実行することにより実現される機能である。ＰＩＤ制御部６０３は、前述したように、離散時刻ｎにおいて差分演算部６０６で算出される姿勢偏差６１９である偏差ｅ（ｎ）と、前回及び前々回の各離散時刻ｎ−１及びｎ−２でそれぞれ算出されている姿勢偏差６１９である偏差ｅ（ｎ−１）及びｅ（ｎ−２）と、予め算出されているＰゲインＫ_p 、ＩゲインＫ_i 、及びＤゲインＫ_d と、前回の離散時刻ｎ−１において算出されている操作量ｕ（ｎ−１）とを用いて、前述した（３）式及び（２）式で示される演算を実行することで、今回の離散時刻ｎにおける操作量ｕ（ｎ）を算出する。コントローラ３０１は、ＰＩＤ制御部６０３で算出された操作量ｕ（ｎ）を、位置・姿勢操作量６２５として出力する。

操作量変換部６２３は、上記位置・姿勢操作量６２５に基づいて、＃１から＃４のベーンモータ１０６（図１、図２（ｂ）参照）を駆動するための＃１から＃４のベーンモータ回転角６２１を生成し、それぞれ＃１から＃４のベーンモータドライバ３０３（図３参照）に出力する。

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

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

図７は、図３のコントローラ３０１が実行する水平移動量算出のための制御処理例を示すフローチャートである。この処理は、コントローラ３０１が内蔵するＣＰＵが、同じく内蔵する特には図示しないメモリに記憶された制御プログラムを実行する処理として実現することができる。この制御処理は、下向きカメラユニット１１２が撮影する動画像のフレーム期間毎に繰り返し起動されて実行される。

コントローラ３０１はまず、下向きカメラユニット１１２から、現在のフレームの画像４０３（図４、図５参照）（現画像）を入力する（ステップＳ７０１）。

次に、図５を用いて前述したように、コントローラ３０１は、フライトセンサ１０９中のジャイロセンサ４０２によって飛行装置１００の姿勢（図５の方向Ｂ）を検出する（ステップＳ７０２）。

更に、コントローラ３０１は、ステップＳ７０２で算出した姿勢から、下向きカメラユニット１１２により撮影された画像４０３から、飛行装置１００の地面又は床面に対して垂直となる座標点Ａ（図５参照）を算出する（ステップＳ７０３）。

コントローラ３０１は、ステップＳ７０３で算出した座標Ａが、画像４０３の画角内に入っているか否かを判定する（ステップＳ７０４）。

座標Ａが画角から外れた場合（ステップＳ７０４の判定がＮＯの場合）は、コントローラ３０１は、飛行装置１００がホバリング中では無いと判定し、今回のフレームにおける図７のフローチャートで示される水平移動量算出のための制御処理を終了する。

座標Ａが画角内にある場合（ステップＳ７０４の判定がＹＥＳの場合）は、コントローラ３０１は、フライトセンサ１０９中の超音波センサ４０１（図４参照）によって、飛行装置１００の高度を検出する（ステップＳ７０５）。

次に、コントローラ３０１は、ステップＳ７０５で検出した高度が、閾値４０４（図４参照）に比較して高いか低いかを判定する（ステップＳ７０６）。

コントローラ３０１は、ステップＳ７０６で上記高度が閾値４０４に比較して高いと判定した場合は、図４（ａ）を用いて前述したように、ステップＳ７０３で算出した座標Ａを中心とする所定の画像範囲（図４（ａ）の横８０ｐｉｘ×縦６０ｐｉｘの範囲）を設定する（ステップＳ７０７）。

そして、コントローラ３０１は、ステップＳ７０７で設定した画像範囲で、ステップＳ７０１で入力した画像４０３の現画像に対してトリミングを行う（ステップＳ７０８）。

飛行装置１００の高度が高い場合に、下向きカメラユニット１１２が撮影した画像４０３そのものに対してオプティカルフローの処理を実行すると、飛行装置１００の水平移動量に対して画像の変化が少ないため、縮小してぼやけてしまうと、注目すべき特徴や、また画像４０３の連続するフレーム間の画像の差分が小さくなり、水平移動量を算出するための特徴点をうまく抽出できない場合がある。そこで、飛行装置１００の高度が高い場合には、画像４０３に対してトリミング処理を実行して切り出した狭い範囲に対してオプティカルフロー処理を実行することにより、特徴量を適切に抽出することが可能となる。

一方、コントローラ３０１は、ステップＳ７０６で高度が閾値４０４に比較して低いと判定した場合は、図４（ｂ）を用いて前述したように、ステップＳ７０１で入力した画像４０３の現画像に対して縮小する倍率（横６４０ｐｉｘ×縦４８０ｐｉｘから横８０ｐｉｘ×縦６０ｐｉｘへの８分の１の縮小）を設定する（ステップＳ７０９）。

そして、コントローラ３０１は、ステップＳ７０９で設定した倍率で、ステップＳ７０１で入力した画像４０３の現画像に対して縮小を行う（ステップＳ７１０）。縮小処理は例えば、各画素の周囲８近傍画素の平均加算処理である。

飛行装置１００の高度が低い場合に、下向きカメラユニット１１２が撮影した画像４０３そのものに対してオプティカルフローの処理を実行すると、水平移動量に対して画像の変化が大きすぎ、画像４０３の連続するフレーム間で特徴点の対応付けができなくなる可能性がある。そこで、飛行装置１００の低い場合には、画像４０３に対して縮小処理を実行して得た小サイズの画像に対してオプティカルフロー処理を実行することにより、特徴量を適切に対応付けることが可能となる。

次に、コントローラ３０１は、ステップＳ７０８でトリミング処理又はステップＳ７１０で縮小処理した画像を入力し、その画像から特徴点を抽出する（ステップＳ７１２）。特徴点は例えば、画像エッジに対するベクトル情報である。

コントローラ３０１は、ステップＳ７１２で特徴点が抽出できたか否かを判定する（ステップＳ７１３）。

ステップＳ７１３の判定がＮＯの場合（特徴点が抽出できなかった場合）には、コントローラ３０１はそのまま、今回のフレームにおける図７のフローチャートで示される水平移動量算出のための制御処理を終了する。

ステップＳ７１３の判定がＹＥＳの場合（特徴点が抽出できた場合）には、コントローラ３０１は、１フレーム前の後述するステップ７１６で保存されている処理済みの画像を入力する（ステップＳ７１４）。

そして、コントローラ３０１は、ステップＳ７１１で入力した現在のフレームの画像とステップＳ７１４で入力した１フレーム前の画像とで、フレーム間で特徴点の対応付け及びオプティカルフロー（動きベクトル）の算出を行い、全体的な水平移動量を算出する（ステップＳ７１５）。オプティカルフロー処理は、連続するフレーム間で動きベクトルを算出するための一般的な処理であるため、その詳細は省略する。

コントローラ３０１は、ステップＳ７１１で入力した現在のフレームの処理済みの現画像を、次のフレームのステップＳ７１４で使用される１フレーム前の処理済み画像として保存する（ステップＳ７１６）。なお、コントローラ３０１は、ステップＳ７１２で算出した特徴点についても、次のフレームにおけるステップＳ７１５で参照される１フレーム前の特徴点として保存する。その後、コントローラ３０１は、図７のフローチャートで示される今回のフレームにおける水平移動量算出のための制御処理を終了する。

以上説明した実施形態において、コントローラ３０１は、画像４０３又はフライトセンサ１０９中の超音波センサ４０１の出力に基づいて地面又は床面の高度に対して所定閾値以上高度の異なる物体である障害物を検知すると、障害物から水平方向に所定距離離れるように飛行装置１００を水平移動させるような制御を行う、又はトリミング処理した後の画像に障害物が含まれなくなるように飛行装置１００を水平移動させる制御を行ってもよい。
また、上述の実施形態は、コントローラ３０１が全体の動作を制御しているが、処理の一部分を受け持つ専用のハードウェア又はプロセッサを設けてもよい。例えば専用のＰＩＤ制御回路や水平移動量算出回路を設けることもできる。

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

以上の実施形態に関して、更に以下の付記を開示する。
（付記１）
飛行推進部と処理部とを備える飛行装置であって、
地面又は床面の方向の画像を撮影する下向き撮影部と、
前記飛行装置の少なくとも姿勢又は、地面もしくは床面に対する高度を検出するフライトセンサ部と、
を備え、
前記処理部は、
前記フライトセンサ部が検出した前記飛行装置の姿勢及び高度に応じて、前記撮影部が撮影した前記画像に対してトリミング処理又は縮小処理を実行し、
前記トリミング処理又は縮小処理の後の前記画像から前記飛行装置の水平移動量を算出する
ことを特徴とする飛行装置。
（付記２）
前記処理部は、前記算出した水平移動量に基づいて、前記飛行装置が空中で定置するように前記飛行推進部を制御する、
ことを特徴とする付記１に記載の飛行装置。
（付記３）
前記フライトセンサ部はジャイロセンサを含み、
前記処理部は、
前記ジャイロセンサによって前記飛行装置の姿勢を検出し、
前記検出した姿勢に基づいて前記飛行装置の前記地面又は床面に対する傾きを算出し、
前記傾きに従って前記画像の画角内で前記地面又は床面に対して垂直な点の座標を算出し、
前記垂直な点の座標を中心座標として前記トリミング処理又は縮小処理を実行する、
ことを特徴とする付記１又は２に記載の飛行装置。
（付記４）
前記処理部は、
前記フライトセンサ部が検出する高度が所定の高度閾値に比較して高い場合には、前記画像に対して前記トリミング処理を実行し、
前記フライトセンサ部が検出する高度が所定の高度閾値に比較して低い場合には、前記画像に対して前記縮小処理を実行する、
ことを特徴とする付記１乃至３の何れかに記載の飛行装置。
（付記５）
前記処理部は、前記画像又は前記フライトセンサ部の出力に基づいて前記地面又は床面の高度に対して所定閾値以上高度の異なる物体である障害物を検知すると、前記障害物から水平方向に所定距離離れるように前記飛行装置を水平移動させ、又は前記トリミング処理した後の前記画像に前記障害物が含まれなくなるように前記飛行装置を水平移動させる、
ことを特徴とする付記１乃至４の何れかに記載の飛行装置。
（付記６）
前記処理部は、前記飛行装置の姿勢をヨー軸、ロール軸、及びピッチ軸まわりで制御する、ことを特徴とする付記１乃至５の何れかに記載の飛行装置。
（付記７）
飛行推進部と、地面又は床面の方向の画像を撮影する下向き撮影部と、少なくとも姿勢及び高度を検出するフライトセンサ部とを備える飛行装置の処理部が、
前記フライトセンサ部が検出した前記飛行装置の姿勢及び高度に応じて、前記撮影部が撮影した前記画像に対してトリミング処理又は縮小処理を実行し、
前記トリミング処理又は縮小処理の後の前記画像から前記飛行装置の水平移動量を算出し、
ことを特徴とする飛行装置の制御方法。
（付記８）
飛行推進部と、地面又は床面の方向の画像を撮影する下向き撮影部と、少なくとも姿勢及び高度を検出するフライトセンサ部とを備える飛行装置を制御するコンピュータに、
前記フライトセンサ部が検出した前記飛行装置の姿勢及び高度に応じて、前記撮影部が撮影した前記画像に対してトリミング処理又は縮小処理を実行するステップと、
前記トリミング処理又は縮小処理の後の前記画像から前記飛行装置の水平移動量を算出するステップと、
を実行させるためのプログラム。

１００ 飛行装置
１０１ フレーム
１０２ ロータモータ
１０３ ロータ
１０４ バッテリ
１０５ ベーン
１０６ ベーンモータ
１０７ ステータ
１０８ ロッド
１０９ フライトセンサ
１１０ デジタルカメラユニット
１１１ 回路ボックス
１１２ 下向きカメラユニット
３０１ コントローラ
３０２ ロータモータドライバ
３０３ ベーンモータドライバ
３０４ パワーセンサ
４０１ 超音波センサ
４０２ ジャイロセンサ
４０３ 画像
６０１ アルゴリズム
６０２、６０３、６０４ ＰＩＤ制御部
６０５、６０６、６０７ 差分演算部
６１０ 目標軌道
６１１ 現在位置
６１２ 目標位置
６１３ 目標高さ
６１５ 現在高さ
６１６ 位置偏差
６１７ 目標姿勢
６１８ 現在姿勢
６１９ 姿勢偏差
６２０ 高さ偏差
６２１ ベーンモータ回転角
６２２ ロータモータ回転数
６２３、６２４ 操作量変換部
６２５ 位置・姿勢操作量
６２６ 高さ操作量

Claims (8)

1. 飛行推進部と処理部とを備える飛行装置であって、
   地面又は床面の方向の画像を撮影する下向き撮影部と、
   前記飛行装置の少なくとも姿勢又は、地面若しくは床面に対する高度を検出するフライトセンサ部と、
   を備え、
   前記処理部は、
   前記フライトセンサ部が検出した前記飛行装置の姿勢又は高度に応じて、前記撮影部が撮影した前記画像に対してトリミング処理又は縮小処理を実行し、
   前記トリミング処理又は縮小処理の後の前記画像から前記飛行装置の水平移動量を算出する、
   ことを特徴とする飛行装置。
2. 前記処理部は、前記算出した水平移動量に基づいて、前記飛行装置が空中で定置するように前記飛行推進部を制御する、
   ことを特徴とする請求項１に記載の飛行装置。
3. 前記フライトセンサ部はジャイロセンサを含み、
   前記処理部は、
   前記ジャイロセンサによって前記飛行装置の姿勢を検出し、
   前記検出した姿勢に基づいて前記飛行装置の前記地面又は床面に対する傾きを算出し、
   前記傾きに従って前記画像の画角内で前記地面又は床面に対して垂直な点の座標を算出し、
   前記垂直な点の座標を中心座標として前記トリミング処理又は縮小処理を実行する、
   ことを特徴とする請求項１又は２に記載の飛行装置。
4. 前記処理部は、
   前記フライトセンサ部が検出する高度が所定の高度閾値に比較して高い場合には、前記画像に対して前記トリミング処理を実行し、
   前記フライトセンサ部が検出する高度が所定の高度閾値に比較して低い場合には、前記画像に対して前記縮小処理を実行する、
   ことを特徴とする請求項１乃至３の何れかに記載の飛行装置。
5. 前記処理部は、前記画像又は前記フライトセンサ部の出力に基づいて前記地面又は床面の高度に対して所定閾値以上高度の異なる物体である障害物を検知すると、前記障害物から水平方向に所定距離離れるように前記飛行装置を水平移動させ、又は前記トリミング処理した後の前記画像に前記障害物が含まれなくなるように前記飛行装置を水平移動させる、
   ことを特徴とする請求項１乃至４の何れかに記載の飛行装置。
6. 前記処理部は、前記飛行装置の姿勢をヨー軸、ロール軸、及びピッチ軸まわりで制御する、ことを特徴とする請求項１乃至５の何れかに記載の飛行装置。
7. 飛行推進部と、地面又は床面の方向の画像を撮影する下向き撮影部と、少なくとも姿勢及び高度を検出するフライトセンサ部とを備える飛行装置の処理部が、
   前記フライトセンサ部が検出した前記飛行装置の姿勢又は、地面若しくは床面に対する高度に応じて、前記撮影部が撮影した前記画像に対してトリミング処理又は縮小処理を実行し、
   前記トリミング処理又は縮小処理の後の前記画像から前記飛行装置の水平移動量を算出する、
   ことを特徴とする飛行装置の制御方法。
8. 飛行推進部と、地面又は床面の方向の画像を撮影する下向き撮影部と、少なくとも姿勢及び高度を検出するフライトセンサ部とを備える飛行装置を制御するコンピュータに、
   前記フライトセンサ部が検出した前記飛行装置の姿勢又は、地面若しくは床面に対する高度に応じて、前記撮影部が撮影した前記画像に対してトリミング処理又は縮小処理を実行するステップと、
   前記トリミング処理又は縮小処理の後の前記画像から前記飛行装置の水平移動量を算出するステップと、
   を実行させるためのプログラム。