JP2019159488A - 飛行制御装置、方法、プログラム、及び記憶媒体 - Google Patents

Abstract

【課題】無人飛行機を自由自在に操作し、かつ、安定的に飛行させるように制御できる飛行制御装置を提供する。【解決手段】位置方位検出部３０が、位置計測センサ１０によって計測されたマーカの各々の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点とに基づいて、マーカの各々の三次元座標の中点、中点からの目標点までの距離、及びグローバル座標系における目標点に対するマーカの各々の三次元座標を結んだ線分の方位角を算出する。速度制御部３２が、目標点までの距離に基づいて、ＵＡＶの速度ベクトルを制御するための制御データを、中点及び目標点から定められた速度ベクトルとなるように更新する。飛行コマンド変換部３４が、更新された制御データと、算出された方位角とに基づいて、ＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御する。【選択図】図１

Description

本発明は、飛行制御装置、方法、プログラム、及び記憶媒体に係り、特に、無人飛行機を制御する飛行制御装置、方法、プログラム、及び記憶媒体に関する。

クアッドロータ型ＵＡＶ（Unmanned Aerial Vehicle）は４つのプロペラを持ち、それぞれのプロペラに与える揚力を制御することにより飛行操縦する無人飛行機である。ドローンと呼ばれる無人飛行機の多くは、クアッドロータ型ＵＡＶの一種である。以降で扱う無人飛行機をＵＡＶと略称する。

一般的に、ＵＡＶの姿勢を計測するために、図１１に示すように機体にローカル座標系が設定される。ＵＡＶの前進方向をＸ軸、Ｘ軸と垂直な方向をＹ軸、重力とは逆方向をＺ軸とする。

また、ＵＡＶの三次元位置を計測するため、図１２に示すように、ある基準となるグローバル座標系を設定する。ＧＰＳでは世界座標系とした三次元座標となり、モーションキャプチャシステムではその計測座標系がグローバル座標系となる。ＵＡＶの重心すなわちＵＡＶの位置（ローカル座標系の原点）はグローバル座標系の点Ｐ＝（Ｘ，Ｙ，Ｚ）として表現する。また、ＵＡＶの姿勢はグローバル座標系に対するローカル座標系の回転角で表現し、Ｘ軸周りの回転はロール回転（回転角φ）、Ｙ軸周りの回転はピッチ回転（回転角ω）、Ｚ軸周りの回転はヨー回転（回転角θ）と呼ばれる。ＵＡＶの飛行運動は、４つのプロペラに与える揚力を変化させることにより、Ｘ軸周りの回転、Ｙ軸周りの回転、Ｚ軸周りの回転を発生させる。Ｙ軸周りの回転はＸ軸方向の並進運動を生み出し、Ｘ軸周りの回転はＹ軸方向の並進運動を生み出す。Ｚ軸周りの回転は方位の回転を生み出し、同じ揚力が同時に４つのプロペラに与えられたとき、その強弱によってＺ軸方向の並進運動（高度の昇降）を生み出す。

グローバル座標系において所定の座標値（Ｘ_ｄ，Ｙ_ｄ，Ｚ_ｄ）と方位θ_ｄが与えられたとき、ＵＡＶの現在位置Ｐ＝（Ｘ，Ｙ，Ｚ）と現在方位θから、その所定の座標値の位置へ飛行して所定方位に機体を向けるためには、ＵＡＶの姿勢と高度を制御する必要がある。非特許文献１には、ＵＡＶの運動方程式と角運動方程式に基づいたバックステッピング制御が公開されている。非特許文献２では、AR Drone 2.0（市販の低価格なUAV）に関する飛行運動制御法が公開されている。従来技術の多くは、グローバル座標系における飛行の軌跡として離散的な所定位置と所定方位が与えられ、ＵＡＶが各点と各方位を追跡するようにその運動が制御される。

さらに、ＵＡＶには空間中を自由自在に飛行する能力があるため、人の代わりとなるロボットとして活躍することができる．非特許文献３では，ＵＡＶを人と協調作業するための道具として使われており、非特許文献４には、複数のＵＡＶを使って空間中に網を張り、人とＵＡＶがボールをやり取りする動作として複数のＵＡＶの運動を制御する方法が公開されている。

T. Madani and A. Benallegue: "Backstepping Control for a Quadrotor Helicopter", IEEE Conference on Intelligent Robots and Systems, 2006. L. V. Santana, A. S. Brandao, M. S-Filho, and R. Carelli: "A Trajectory Tracking and 3D Positioning Controller for the AR.Drone Quadrotor", International Conference on Unmanned Aircraft Systems (ICUAS) May 27-30, 2014. W. S. Ng and E. Sharlin: "Collocated Interaction with Flying Robots", IEEE International Symposium on Robot and Human Interactive Communication, 2011. R. Ritz, M. W. Muller, M. Hehn, and R. D'Andrea: "Cooperative Quadrocopter Ball Throwing and Catching", IEEE/RSJ International Conference on Intelligent Robots and Systems, 2012.

ＵＡＶの飛行を自由自在に制御する課題は、大まかに３つに大別でき、ホバリング（hovering）、軌跡追跡（trajectory tracking）、パスフォロー（path following）と呼ばれる。ホバリングは、与えられた位置と方位に留まるよう飛行を維持する。軌跡追跡では、与えられた経路（空間中の離散的な三次元位置と方位）に沿ってリアルタイムに追跡するよう制御する。これに対して、パスフォローは、リアルタイム性は問わないが、与えられた空間中の三次元位置と方位において、何らかの空間中の拘束条件を与えて飛行を制御する。非特許文献１と非特許文献２によれば、軌跡追跡の課題に対して、所定の経路に従って飛行するように、ＵＡＶの飛行運動を適切に制御することができる。しかしながら、これらの技術は所定の経路に沿って目標地に到達することを目的とした制御であるため、ＵＡＶの安定した飛行が保証されていない。

これに対して、パスフォローは障害物あるいは人との衝突を避けるため、特定の場所を飛行しないように空間中に拘束条件を設定することができる。また、飛行運動中のトラブルを回避するため、ＵＡＶには多様なセンサやカメラが取り付けられている場合がある。例えば、超音波センサは地面あるいは床からの高度を計測し、ジャイロセンサと加速度センサによって機体の速度と姿勢を計測することができる。また、移動物体を検出しながら、空間中を自律的に飛行させることにカメラが利用されている。これらの機体から計測した空間情報や画像情報を使うことも考えられるが、大半のＵＡＶに内蔵されているセンサの精度はミリ単位での正確な精度で計測できることを保証しておらず、別の手段を使って機体の正確な位置と方位を計測する必要がある。さらに、これらの技術も目標地に到達することを目的とした制御であるため、ＵＡＶの安定した飛行が保証されているものではなかった。

本発明は、上記問題点を解決するために成されたものであり、無人飛行機を自由自在に操作し、かつ、安定的に飛行させるように制御できる飛行制御装置、方法、プログラム、及び記憶媒体を提供することを目的とする。

上記目的を達成するために、第１の発明に係る飛行制御装置は、ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測する位置計測センサと、前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出する位置方位検出部と、前記目標点までの距離に基づいて、前記ＵＡＶの速度ベクトルを制御するための制御データを、前記中点及び前記目標点から定められた速度ベクトルとなるように更新する速度制御部と、更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御する飛行コマンド変換部と、を含んで構成されている。

第２の発明に係る飛行制御方法は、位置計測センサが、ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測するステップと、位置方位検出部が、前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出するステップと、速度制御部が、前記目標点までの距離に基づいて、前記ＵＡＶの速度ベクトルを制御するための制御データを、前記中点及び前記目標点から定められた速度ベクトルとなるように更新するステップと、飛行コマンド変換部が、更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御するステップと、を含んで実行することを特徴とする。

第３の発明に係るプログラムは、コンピュータを、第１の発明に係る飛行制御装置の各部として機能させるためのプログラムである。

第４の発明に係る記憶媒体は、コンピュータを、第１の発明に係る飛行制御装置の各部として機能させるためのプログラムを格納する記憶媒体である。

本発明の飛行制御装置、方法、プログラム、及び記憶媒体によれば、位置計測センサによって計測されたマーカの各々の三次元座標と、予め設定されたＵＡＶの目標地の目標点とに基づいて、マーカの各々の三次元座標の中点、中点からの目標点までの距離、及びグローバル座標系における目標点に対するマーカの各々の三次元座標を結んだ線分の方位角を算出し、目標点までの距離に基づいて、ＵＡＶの速度ベクトルを制御するための制御データを、中点及び目標点から定められた速度ベクトルとなるように更新し、更新された制御データと、算出された方位角とに基づいて、ＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御することにより、無人飛行機を自由自在に操作し、かつ、安定的に飛行させるように制御できる、という効果が得られる。

本発明の実施形態に係る飛行制御装置の構成を示すブロック図である。 ＵＡＶの飛行を制御する状況の一例を示す図である。 本発明の実施形態に係る飛行制御装置における飛行制御処理ルーチンを示すフローチャートである。 飛行制御処理ルーチンにおける位置方位検出処理のフローチャートである。 点ｑ_１、及び点ｑ_２、並びに、目標点ｐをＺ_ｗ軸方向から視た位置と方位の関係を示す図である。 飛行制御処理ルーチンにおける速度制御処理のフローチャートである。 飛行コマンド変換処理ルーチンにおける飛行コマンド変換処理のフローチャートである。 飛行速度を加速又は減速させる場合の一例を示す図である。 第２の実施形態の飛行制御処理ルーチンにおける速度制御処理のフローチャートである。 複数のＵＡＶの空間配置の一例を示す図である。 ＵＡＶの概観とＵＡＶ固定のローカル座標系の一例を示す図である。 グローバル座標系とＵＡＶ固定のローカル座標系の関係の一例を示す図である。

以下、図面を参照して本発明の実施形態を詳細に説明する。本発明の実施形態に係る手法は、ＵＡＶの飛行を自由自在に操作し、かつ、安定的にＵＡＶを飛行させる、という課題を解決するものである。

＜本発明の第１の実施形態に係る飛行制御装置の構成＞

本発明の第１の実施形態に係るクアッドロータ型ＵＡＶの飛行制御装置の構成について説明する。本実施形態は１台のＵＡＶの飛行速度を制御する形態である。図１に示すように、本発明の第１の実施形態に係る飛行制御装置１００は、ＣＰＵと、ＲＡＭと、後述する飛行制御処理ルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この飛行制御装置１００は、機能的には図１に示すように位置計測センサ１０と、演算部２０と、通信部５０とを備えている。この構成において、位置計測センサ１０は必ずしも構成要素として接続している必要はなく、処理に必要なデータを取得すればよく、演算部２０における位置方位検出部３０、速度制御部３２、及び飛行コマンド変換部３４からそれぞれの矢印へのデータの流れは、ハードディスク、ＲＡＩＤ装置、ＣＤ−ＲＯＭなどの記録媒体を利用する、または、ネットワークを介してリモートなデータ資源を利用する形態でもどちらでも構わない。

位置計測センサ１０は、ＵＡＶ１４に付与され、かつ、マーカ間の距離が既知の複数のマーカ（点ｑ_１、及び点ｑ_２とする）との各々の三次元座標を計測データとして計測する。本実施形態では、位置計測センサ１０の例として、モーションキャプチャ装置を利用する。ＵＡＶ１４の目標地１２の目標点ｐの位置は飛行計画に応じて任意に設定し、ＵＡＶ１４には点ｑ_１、及び点ｑ_２の位置にセンシング用のマーカが取り付けられている．図２に示す状況において、位置計測センサ１０はグローバル座標系において３次元座標を計測するようにセットアップされており、各マーカの３次元座標が、ある一定間隔で逐次計測される。本実施形態は、ＵＡＶ１４に取り付けたマーカ点ｑ_１とマーカ点ｑ_２の中点が目標点ｐに到達するまでに、ＵＡＶ１４を一定速度で飛行するようにその運動を制御する。なお、本実施形態では、マーカの数は２つとしたが、これに限定されるものではなく、３つ以上として、物体間の相対的な位置関係を求めるようにしてもよい。

演算部２０は、位置計測センサ１０によりグローバル座標系におけるＵＡＶ１４の位置と方位を検出する位置方位検出部３０と、位置と方位に基づき、ＵＡＶ１４の飛行速度を制御する速度制御部３２と、使用するＵＡＶ１４に応じて指令を送出するための飛行コマンド変換部３４とを含んで構成されている。なお、位置方位検出部３０、速度制御部３２、及び飛行コマンド変換部３４の具体的な処理内容は後述の作用の説明において説明する。

位置方位検出部３０は、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ_１、及び点ｑ_２の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカの点ｑ_１、及び点ｑ_２の三次元座標の中点ｑ、中点ｑからの目標点ｐまでのベクトルＴから求まる距離||Ｔ||、及びグローバル座標系における目標点ｐに対するマーカの点ｑ_１、及び点ｑ_２の中点ｑを三次元座標で結んだ線分の方位角θを算出する。

速度制御部３２は、目標点ｐまでの距離に基づいて、ＵＡＶ１４の速度ベクトルを制御するための制御データｕを、中点ｑ及び目標点ｐから定められた速度ベクトルＶ_０となるように更新する。

飛行コマンド変換部３４は、更新された制御データｕと、算出された方位角θとに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転速度Ｖ_x、Ｙ軸周りの回転速度Ｖ_y、Ｚ軸に沿った速度Ｖ_ｚ、及びＺ軸周りの回転速度Ｖ_θを飛行指令データとして算出し、飛行指令データを通信部５０を介してＵＡＶ１４に送信することで、算出した飛行指令データに基づいてＵＡＶ１４の運動を制御する。

＜本発明の第１の実施形態に係る飛行制御装置の作用＞

次に、本発明の第１の実施形態に係る飛行制御装置１００の作用について説明する。ＵＡＶ１４のマーカの点ｑ_１、及び点ｑ_２の三次元座標の計測を開始すると、飛行制御装置１００は、図３に示す飛行制御処理ルーチンを実行する。

まず、ステップＳ１００では、位置方位検出部３０が、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ_１、及び点ｑ_２の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカの点ｑ_１、及び点ｑ_２の三次元座標の中点ｑ、中点ｑからの目標点ｐまでのベクトルＴから求まる距離||Ｔ||、及びグローバル座標系における目標点ｐに対するマーカの点ｑ_１、及び点ｑ_２の中点ｑを三次元座標で結んだ線分の方位角θを算出する。

次に、ステップＳ１０２では、速度制御部３２が、目標点ｐまでの距離に基づいて、ＵＡＶ１４の速度ベクトルを制御するための制御データｕを、中点ｑ及び目標点ｐから定められた速度ベクトルＶ_０となるように更新する。

ステップＳ１０４では、飛行コマンド変換部３４が、更新された制御データｕと、算出された方位角θとに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転速度Ｖ_x、Ｙ軸周りの回転速度Ｖ_y、Ｚ軸に沿った速度Ｖ_ｚ、及びＺ軸周りの回転速度Ｖ_θを飛行指令データとして算出し、飛行指令データを通信部５０を介してＵＡＶ１４に送信することで、算出した飛行指令データに基づいてＵＡＶ１４の運動を制御する。

次に、ステップＳ１００の位置方位検出部３０の位置方位検出処理の詳細について説明する。

図４は位置方位検出部３０の処理のフロー図である。位置方位検出部３０は処理を開始すると、ステップＳ１０００で、グローバル座標系における目標地１２の目標点ｐを設定する。目標点ｐは操作者が決定した任意の三次元座標でよい。

ステップＳ１００２で、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ_１、及び点ｑ_２の三次元座標を取得する。

ステップＳ１００４で、マーカの点ｑ_１及び点ｑ_２の中点ｑから目標点ｐまでの距離を算出する。中点ｑの座標は、ｑ＝(ｑ_１＋ｑ_２)/２により計算する。位置計測センサ１０で得た現在のＵＡＶ１４の位置（中点ｑ）と目標地１２の目標点ｐ間の距離を、ベクトルＴから||Ｔ||＝||ｐ−ｑ||の計算により求める。||・||はベクトルのノルム（大きさ）を表す。

ステップＳ１００６で、グローバル座標系における目標点ｐに対するマーカの点ｑ_１、及び点ｑ_２の中点ｑを三次元座標で結んだ線分の方位角θを算出する。図５に、マーカの点ｑ_１、及び点ｑ_２、並びに、目標地１２の目標点ｐをＺ_ｗ軸方向から視た位置と方位の関係を示す。位置方位検出部３０では、マーカの点ｑ_１及び点ｑ_２の三次元座標の線分が、目標点ｐの線分に対して直交する線分となるように回転角（方位角θ）をベクトル内積の関係を用いて算出する。

ステップＳ１００８では、位置方位検出の処理を停止するかを判定し、処理を停止する場合には位置方位検出部３０の処理を終了し、終了しない場合にはステップＳ１０００に戻って処理を繰り返す。なお、処理を停止する場合とは、ここでは操作者がＵＡＶ１４の飛行制御を終了する場合とし、以下の処理においても同様である。

位置方位検出部３０では、以上の処理により、中点ｑから目標点ｐまでの距離||Ｔ||、及び方位角θを算出する。

次に、ステップＳ１０２の速度制御部３２の速度制御処理の詳細について説明する。

図６は速度制御部３２の処理のフロー図である。速度制御部３２は処理を開始すると、ステップＳ１１００で、中点ｑから目標点ｐまでの距離||Ｔ||を取得する。

ステップＳ１１０２で、距離||Ｔ||が許容距離ΔＬ未満（||Ｔ||＜ΔＬ）であるか否かを判定する。||Ｔ||＜ΔＬであればステップＳ１１０４へ移行する。||Ｔ||＜ΔＬでなく、||Ｔ||≧ΔＬであれば運動を制御するためステップＳ１１０６へ移行する。

ステップＳ１１０４で、ホバリングにより飛行を維持するように制御する。

ステップＳ１１０６で、ＵＡＶ１４の速度ベクトルｖを算出する。本実施形態では、位置計測センサ１０によって点ｑ_１から点ｑ_２の三次元位置が入力として与えられるため、逐次、中点ｑが入力として与えられる。中点ｑを用いて、時間間隔Δｔの間に中点ｑがベクトルｍだけ移動したときの速度ベクトルｖ＝ｍ/Δｔを算出する。

ステップＳ１１０８で、ＵＡＶ１４を、中点ｑから目標点ｐまでのベクトルＴから求まる一定速度の速度ベクトルＶ_０で飛行させるため制御データｕを更新するか否かを判定する。中点ｑから目標点ｐまでのベクトルＴから求まる一定速度の速度ベクトルＶ_ｏと現在の速度ベクトルｖとの絶対差分||Ｖ_０−ｖ||を求め、一定速度を満たすための許容誤差をΔｖとする。このとき、||Ｖ_０−ｖ||＞Δｖのであるか否かを判定する。条件を満たす場合はステップＳ１１１０へ移行し、条件を満たさない場合は、ステップＳ１１００へ移行する。

ステップＳ１１１０で、制御データｕを更新する。制御データｕは

により更新する。

はフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。更新された制御データｕは、飛行コマンド変換部３４に出力する。

ステップＳ１１１２では、速度制御の処理を停止するかを判定し、処理を停止する場合には速度制御部３２の処理を終了し、終了しない場合にはステップＳ１１００に戻って処理を繰り返す。

以上の処理によって、速度制御部３２は、||Ｔ||＜ΔＬ、かつ、||Ｖ_０−ｖ||≦Δｖになるまで，フィードバック制御によって制御データｕを更新する。

次に、ステップＳ１０４の飛行コマンド変換部３４の飛行コマンド変換処理の詳細について説明する。

飛行コマンド変換部３４の処理により、速度制御部３２より与えられる制御データｕと位置方位検出部３０で得た方位θを、使用するＵＡＶ１４に応じて飛行コマンドの飛行指令データへ変換して通信部５０から無線経由で指令する。ＵＡＶ１４への飛行コマンドには様々なデータ形式が存在するが、本実施形態では、市販製品のAR Drone 2.0を例にした場合を示す。ただし、それ以外のＵＡＶ１４の飛行を制御する場合にも、本実施形態を利用できることはもちろんである。

図７は飛行コマンド変換部３４の処理のフロー図である。飛行コマンド変換部３４は処理を開始すると、ステップＳ１２００で、更新された制御データｕと、算出された方位角θとの入力を受け付ける。

ステップＳ１２０２で、制御データｕと、方位角θとを、ＵＡＶ１４へ送信するための飛行指令データへ変換する。ＵＡＶ１４への飛行指令データは、機体に設定されたｘ軸周りの回転速度Ｖ_ｘ、ｙ軸周りの回転速度Ｖ_ｙ、Ｚ軸に沿った速度Ｖ_ｚと、Ｚ軸周りの回転速度Ｖ_θになる。制御データｕがｕ＝(ｕ_ｘ,ｕ_ｙ,ｕ_ｚ)と与えられるとする。ＵＡＶ１４では、ｘ軸周りの回転がｙ軸の並進運動を生み出し、ｙ軸周りの回転がｘ軸の並進運動を生み出するため、本処理では、ＵＡＶ１４に与える飛行指令データを以下（１）〜（４）式により変換する。

係数α_ｘ、α_ｙ、α_ｚ、α_θはAR Drone 2.0が扱うことができる変換係数であり、パラメータとしてユーザが決めてよく、例えば、α_ｘ＝α_ｙ＝α_ｚ＝α_θ＝０．１と与える。

ステップＳ１２０４で、通信部５０を介して無線通信により（１）〜（４）式で算出した飛行指令データをＵＡＶ１４へ送信する。

ステップＳ１２０６で、飛行コマンド変換の処理を停止するかを判定し、処理を停止する場合には飛行コマンド変換部３４の処理を終了し、終了しない場合にはステップＳ１２００に戻って処理を繰り返す。

以上により、本実施形態は、位置計測センサ１０によってＵＡＶ１４の位置と方位を算出し、目的地に向かって所定速度で飛行するように、ＵＡＶ１４の飛行運動を制御することができる。なお、目的地の座標を連続的に設定することにより、ＵＡＶ１４の飛行ナビゲーションを実現することができる。

以上説明したように、第１の発明の実施形態に係る飛行制御装置によれば、位置計測センサによって計測されたマーカの各々の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点とに基づいて、マーカの各々の三次元座標の中点、中点からの目標点までの距離、及びグローバル座標系における目標点に対するマーカの各々の三次元座標を結んだ線分の方位角を算出し、目標点までの距離に基づいて、ＵＡＶの速度ベクトルを制御するための制御データを、中点及び目標点から定められた速度ベクトルとなるように更新し、更新された制御データと、算出された方位角とに基づいて、ＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御することにより、無人飛行機を自由自在に操作し、かつ、安定的に飛行させるように制御できる。

＜本発明の第２の実施形態に係る飛行制御装置の構成及び作用＞

本発明の第２の実施形態は、上記図２の構成において、ＵＡＶ１４の制限速度をＶ_０に固定するのでなく、任意の制限速度が与えられたときに、所定の速度で飛行するようＵＡＶ１４を加速又は減速させて制御する場合の例である。なお、第１の実施形態と同様となる箇所には同一符号を付して説明を省略する。

図８に、本実施形態の手法により制御されたＵＡＶ１４の飛行速度を示す。横軸は時間ｔであり、縦軸は速度ｖである。図８の左図は、加速した場合（||Ｖ_０||＜||Ｖ_１||）であり、図８の右図は、減速した場合（||Ｖ_０||＞||Ｖ_１||）である。なお、制限速度の入力として、時系列に制限速度を与えてよい。あるいは、距離||Ｔ||の大きさに応じて、ある一定距離に離れた場合は加速させ、ある一定距離以内になったときに減速させるようにしてもよい。

図９は本実施形態の速度制御部３２の速度制御処理のフロー図である。

ステップＳ１１０２で||Ｔ||≧ΔＬと判定されると、ステップＳ２１００で、所定の制限速度の入力があるか否かを判定する。制限速度の入力がある場合には、ステップＳ２１０２へ移行する。制限速度の入力がある場合には、ステップＳ２１０４へ移行する。

ステップＳ２１０２で、制限速度の入力に基づいて、Ｖ_ｔ−１の速度に対して加速、又は減速するように、制限速度と中点ｑから目標点ｐまでのベクトルＴとから定まる速度ベクトルＶ_tを設定する。ここでは、Ｖ_ｔ−１の速度に対し制限速度が速ければ加速させるようにＶ_tを設定し、Ｖ_ｔ−１の速度に対し制限速度が遅ければ減速させるようにＶ_tを設定する。

ステップＳ２１０４で、ＵＡＶ１４の速度ベクトルｖを算出する。算出方法はステップＳ１１０６と同様である。

ステップＳ２１０６で、ＵＡＶ１４を、ステップＳ２１０２で設定した制限速度の速度ベクトルＶ_ｔで飛行させるため制御データｕを更新する。任意の速度の速度ベクトルＶ_ｔと現在の速度ベクトルｖとの絶対差分||Ｖ_ｔ−ｖ||を求め、任意の速度を満たすための許容誤差をΔｖとして、||Ｖ_ｔ−ｖ||＞Δｖのであるか否かを判定する。条件を満たす場合はステップＳ２１０８へ移行し、条件を満たさない場合は、ステップＳ１１０４へ移行する。

ステップＳ２１０８で、制御データｕを更新する。制御データｕは

により更新する。

以上のようにして本実施形態では、加速又は減速の制限速度の入力に応じて任意の速度となるようにＵＡＶ１４の飛行を制御する。

＜本発明の第３の実施形態に係る飛行制御装置の構成及び作用＞

本発明の第３の実施形態は、上記図２の構成において、Ｎ台のＵＡＶ１４の飛行を同時に制御する例である。図１０に、ＵＡＶ１４＃１を先頭にしてＮ台のＵＡＶ１４が編成を組んで飛行する状況を示す。それぞれのＵＡＶ１４には位置計測センサ１０で検出可能な２つのマーカが設置されている。ＵＡＶ１４＃１の点Ａ_１と点Ｂ_１の中点が第１の実施形態の目標点ｐに対応するものとし、ＵＡＶ１４＃２の点Ａ_２と点Ｂ_２の中点が第１の実施形態の点ｑ_１と点ｑ_２に対応するものとする。以下、Ｎ台目のＵＡＶ１４の中点をｑ、Ｎ−１台目のＵＡＶ１４の目標点をｐとして説明する。各ＵＡＶ１４が互いに衝突しないように、第１の実施形態と同様に速度制御部３２において、ΔＬ＝Ｌと与えて、ＵＡＶ１４間の距離をＬとするようにＵＡＶ１４＃２の運動を制御する。

この割り当てにより、第１の実施形態１と同様に、ＵＡＶ１４＃１の点Ａ_１と点Ｂ_１の中点座標と距離Ｌの設定に従って、ＵＡＶ１４＃２の点Ａ_２と点Ｂ_２の位置と方位、並びにＵＡＶ＃２の速度が制御される。

同様にして、ＵＡＶ１４＃Ｎの点Ａ_Ｎと点Ｂ_Ｎを実施形態１の点ｑ_１と点ｑ_２に対応させ、目標点ｐについては、１つ前を飛行するＵＡＶ１４＃Ｎ−１の点Ａ_Ｎ−１と点Ｂ_Ｎ−１の中点を目標点ｐとすることにより、ＵＡＶ１４＃Ｎの位置、方位、並びに速度を制御する。

本実施形態において、一定速度で飛行させるように複数のＵＡＶ１４の運動を同時に制御することができる。本実施形態により、ＵＡＶ１４＃１を先頭にしてＮ台のＵＡＶ１４の編成飛行を可能とする。

本実施形態の位置方位検出部３０、速度制御部３２、及び飛行コマンド変換部３４は１台目のＵＡＶ１４について上記第１の実施形態と同様に処理を行う。以下、２台目以降のＵＵＡＶ１４について以下の処理を行う。

本実施形態の位置方位検出部３０は、位置計測センサ１０によって計測されたＮ台目のマーカの各々の三次元座標と、予め求められたＮ−１台目のＵＡＶ１４の中点とに基づいて、Ｎ台目のＵＡＶ１４のマーカの各々の三次元座標の中点、中点からＮ−１台目のＵＡＶ１４の中点である目標点ｐまでの距離||Ｔ||、及びグローバル座標系におけるＮ−１台目のＵＡＶ１４の中点である目標点ｐに対するＮ台目のＵＡＶ１４のマーカの各々の三次元座標を結んだ線分の方位角θを算出する。

速度制御部３２は、Ｎ台目のＵＡＶ１４の中点ｑに対するＮ−１台目のＵＡＶ１４の目標点ｐまでの距離に基づいて、Ｎ台目のＵＡＶ１４に対する制御データｕを、中点ｑ及び目標点ｐから定められた速度ベクトルＶ_０となるように更新する。

飛行コマンド変換部３４は、Ｎ台目のＵＡＶ１４について更新された制御データｕと、Ｎ台目のＵＡＶ１４について算出された方位角θとに基づいて、Ｎ台目のＵＡＶ１４における、飛行指令データを算出し、算出した飛行指令データに基づいてＮ台目のＵＡＶ１４の運動を制御する。

なお、本実施形態の速度制御部３２において、速度ベクトルＶ_０とするのではなく、第２の実施形態のように、速度ベクトルＶ_ｔとして加速又は減速による制限速度を与えて制御するようにしてもよい。

なお、本発明は、上述した実施形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

１０ 位置計測センサ
１２ 目標地
１４ ＵＡＶ
２０ 演算部
３０ 位置方位検出部
３２ 速度制御部
３４ 飛行コマンド変換部
５０ 通信部
１００ 飛行制御装置

Claims (8)

1. ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測する位置計測センサと、
   前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出する位置方位検出部と、
   前記目標点までの距離に基づいて、前記ＵＡＶの速度ベクトルを制御するための制御データを、前記中点及び前記目標点から定められた速度ベクトルとなるように更新する速度制御部と、
   更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御する飛行コマンド変換部と、
   を含む飛行制御装置。
2. 前記速度制御部は、前記中点及び前記目標点から定められた速度ベクトルを一定とし、あるいは、前記中点及び前記目標点から定められた速度ベクトルを加速又は減速させた任意の速度ベクトルとして、前記制御データを更新する請求項１に記載の飛行制御装置。
3. 前記ＵＡＶをＮ台のＵＡＶとし、Ｎ−１台目のＵＡＶの中点をＮ台目のＵＡＶの前記目標点とし、
   前記位置方位検出部は、前記位置計測センサによって計測された前記Ｎ台目の前記マーカの各々の三次元座標と、予め求められた前記Ｎ−１台目の前記ＵＡＶの中点とに基づいて、前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標の中点、前記中点からＮ−１台目のＵＡＶの中点である前記目標点までの距離、及びグローバル座標系におけるＮ−１台目のＵＡＶの中点である前記目標点に対する前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標を結んだ線分の方位角を算出し、
   前記速度制御部は、前記Ｎ台目の前記ＵＡＶの前記中点に対する前記目標点までの距離に基づいて、前記Ｎ台目の前記ＵＡＶに対する前記制御データを、前記中点及び前記目標点から定められた速度ベクトルとなるように更新し、
   前記飛行コマンド変換部は、前記Ｎ台目のＵＡＶについて更新された前記制御データと、前記Ｎ台目のＵＡＶについて算出された前記方位角とに基づいて、前記Ｎ台目の前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記Ｎ台目の前記ＵＡＶの運動を制御する請求項１又は請求項２に記載の飛行制御装置。
4. 位置計測センサが、ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測するステップと、
   位置方位検出部が、前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出するステップと、
   速度制御部が、前記目標点までの距離に基づいて、前記ＵＡＶの速度ベクトルを制御するための制御データを、前記中点及び前記目標点から定められた速度ベクトルとなるように更新するステップと、
   飛行コマンド変換部が、更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御するステップと、
   を含む飛行制御方法。
5. 前記速度制御部が更新するステップは、前記中点及び前記目標点から定められた速度ベクトルを一定とし、あるいは、前記中点及び前記目標点から定められた速度ベクトルを加速又は減速させた任意の速度ベクトルとして、前記制御データを更新する請求項４に記載の飛行制御方法。
6. 前記ＵＡＶをＮ台のＵＡＶとし、Ｎ−１台目のＵＡＶの中点をＮ台目のＵＡＶの前記目標点とし、
   前記位置方位検出部が算出するステップは、前記位置計測センサによって計測された前記Ｎ台目の前記マーカの各々の三次元座標と、予め求められた前記Ｎ−１台目の前記ＵＡＶの中点とに基づいて、前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標の中点、前記中点からＮ−１台目のＵＡＶの中点である前記目標点までの距離、及びグローバル座標系におけるＮ−１台目のＵＡＶの中点である前記目標点に対する前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標を結んだ線分の方位角を算出し、
   前記速度制御部が更新するステップは、前記Ｎ台目の前記ＵＡＶの前記中点に対する前記目標点までの距離に基づいて、前記Ｎ台目の前記ＵＡＶに対する前記制御データを、前記中点及び前記目標点から定められた速度ベクトルとなるように更新し、
   前記飛行コマンド変換部が制御するステップは、前記Ｎ台目のＵＡＶについて更新された前記制御データと、前記Ｎ台目のＵＡＶについて算出された前記方位角とに基づいて、前記Ｎ台目の前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記Ｎ台目の前記ＵＡＶの運動を制御する請求項４又は請求項５に記載の飛行制御方法。
7. コンピュータを、請求項１〜請求項３のいずれか１項に記載の飛行制御装置の各部として機能させるためのプログラム。
8. コンピュータを、請求項１〜請求項３のいずれか１項に記載の飛行制御装置の各部として機能させるためのプログラムを格納する記憶媒体。