(57)【要約】【課題】少ない計算コストで互いに干渉関係にない飛行計画を求めることができるようにする。【解決手段】干渉判定手段１３６が、複数の飛行体の各々についての現在位置から目標位置に至る飛行計画に基づいて、飛行計画間の空間的な近さから互いに干渉関係にある飛行体である干渉飛行体と干渉位置とを求める。局所空間設定手段１３８が、三次元空間内における干渉位置を含む局所空間であって、干渉飛行体の各々の加速度及び重量の少なくとも一方により決定される大きさの前記局所空間を設定する。経路修正手段１４２が、局所空間において干渉飛行体が互いに干渉関係にない回避計画を、干渉飛行体の各々について算出して飛行計画を修正する。【選択図】図３
2020057312.tif 000002
【特許請求の範囲】
【請求項１】
三次元空間を飛行する複数の飛行体の、互いに干渉関係にない飛行計画を求める飛行計画算出装置であって、
前記複数の飛行体の各々についての現在位置から目標位置に至る飛行計画に基づいて、前記飛行計画間の空間的な近さから互いに干渉関係にある前記飛行体である干渉飛行体と干渉位置とを求める干渉判定手段と、
前記三次元空間内における前記干渉位置を含む局所空間であって、前記干渉飛行体の各々の加速度及び重量の少なくとも一方により決定される大きさの前記局所空間を設定する局所空間設定手段と、
前記局所空間において前記干渉飛行体が互いに干渉関係にない回避計画を、前記干渉飛行体の各々について算出して前記飛行計画を修正する飛行計画修正手段と、
を備えることを特徴とする飛行計画算出装置。
【請求項２】
前記局所空間設定手段は、前記干渉飛行体の大きさ及び台数のうち少なくとも一方を更に用いて前記局所空間の大きさを決定する請求項１に記載の飛行計画算出装置。
【請求項３】
前記局所空間設定手段は、前記干渉飛行体以外の前記飛行体の前記飛行計画に干渉しない前記局所空間を設定する請求項１又は請求項２に記載の飛行計画算出装置。
【請求項４】
前記局所空間設定手段は、前記干渉位置が前記局所空間の略中央となるよう前記局所空間を設定する請求項１から請求項３の何れか一項に記載の飛行計画算出装置。
【請求項５】
前記局所空間設定手段は、前記三次元空間における風速を測定する風速測定手段によって測定された前記風速が大きいほど前記局所空間が大きくなるよう設定する請求項１から請求項４の何れか一項に記載の飛行計画算出装置。
【請求項６】
三次元空間を飛行する複数の飛行体の、互いに干渉関係にない飛行計画を求めるためのプログラムであって、
コンピュータを、
前記複数の飛行体の各々についての現在位置から目標位置に至る飛行計画に基づいて、前記飛行計画間の空間的な近さから互いに干渉関係にある前記飛行体である干渉飛行体と干渉位置とを求める干渉判定手段、
前記三次元空間内における前記干渉位置を含む局所空間であって、前記干渉飛行体の各々の加速度及び重量の少なくとも一方により決定される大きさの前記局所空間を設定する局所空間設定手段、及び
前記局所空間において前記干渉飛行体が互いに干渉関係にない回避計画を、前記干渉飛行体の各々について算出して前記飛行計画を修正する飛行計画修正手段
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、飛行計画算出装置及びプログラムに係り、特に、複数の飛行体の飛行計画を求める飛行計画算出装置及びプログラムに関する。
【背景技術】
【０００２】
近年、複数のドローンが干渉することなくスムーズに協調飛行することが可能となるよう、各ドローンの飛行経路を求める技術が提案されている。例えば、非特許文献１には、各自律飛行ロボットの飛行経路おける速度変化（又は加速度変化）を積分した目的関数を定め、それを最小化する最適化問題を解くことにより、複数の自律飛行ロボットが互いに干渉せずにスムーズに飛行経路を算出する技術が開示されている。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】F. Augugliaro, A. P. Schoellig and R. D'Andrea, "Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach," 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura, 2012, pp. 1917-1922.7
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記の従来技術は、自律飛行ロボットの台数が増えるほど、また、飛行空間が大きくなるほど、飛行経路の算出までに要する計算コストが膨大となるため、リアルタイムに移動経路を算出する用途に利用することが困難であった。
【０００５】
そこで、本発明は、少ない計算コストで互いに干渉関係にない飛行計画を求めることができる飛行計画算出装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記の目的を達成するために本発明に係る飛行計画算出装置は、三次元空間を飛行する複数の飛行体の、互いに干渉関係にない飛行計画を求める飛行計画算出装置であって、前記複数の飛行体の各々についての現在位置から目標位置に至る飛行計画に基づいて、前記飛行計画間の空間的な近さから互いに干渉関係にある前記飛行体である干渉飛行体と干渉位置とを求める干渉判定手段と、前記三次元空間内における前記干渉位置を含む局所空間であって、前記干渉飛行体の各々の加速度及び重量の少なくとも一方により決定される大きさの前記局所空間を設定する局所空間設定手段と、前記局所空間において前記干渉飛行体が互いに干渉関係にない回避計画を、前記干渉飛行体の各々について算出して前記飛行計画を修正する飛行計画修正手段と、を備えて構成されている。
【０００７】
本発明に係る飛行計画算出装置によれば、干渉判定手段が、前記複数の飛行体の各々についての現在位置から目標位置に至る飛行計画に基づいて、前記飛行計画間の空間的な近さから互いに干渉関係にある前記飛行体である干渉飛行体と干渉位置とを求める。また、局所空間設定手段が、前記三次元空間内における前記干渉位置を含む局所空間であって、前記干渉飛行体の各々の加速度及び重量の少なくとも一方により決定される大きさの前記局所空間を設定する。
【０００８】
そして、飛行計画修正手段が、前記局所空間において前記干渉飛行体が互いに干渉関係にない回避計画を、前記干渉飛行体の各々について算出して前記飛行計画を修正する。
【０００９】
また、前記局所空間設定手段は、前記干渉飛行体の大きさ及び台数のうち少なくとも一方を更に用いて前記局所空間の大きさを決定することが好適である。
【００１０】
また、前記局所空間設定手段は、前記干渉飛行体以外の前記飛行体の前記飛行計画に干渉しない大きさの前記局所空間を設定することが好適である。
【００１１】
また、前記局所空間設定手段は、前記干渉位置が前記局所空間の略中央となるよう前記局所空間を設定することが好適である。
【００１２】
また、前記局所空間設定手段は、前記三次元空間における風速を測定する風速測定手段によって測定された前記風速が大きいほど前記局所空間が大きくなるよう設定する
ことが好適である。
【００１３】
本発明に係るプログラムは、三次元空間を飛行する複数の飛行体の、互いに干渉関係にない飛行計画を求めるためのプログラムであって、コンピュータを、前記複数の飛行体の各々についての現在位置から目標位置に至る飛行計画に基づいて、前記飛行計画間の空間的な近さから互いに干渉関係にある前記飛行体である干渉飛行体と干渉位置とを求める干渉判定手段、前記三次元空間内における前記干渉位置を含む局所空間であって、前記干渉飛行体の各々の加速度及び重量の少なくとも一方により決定される大きさの前記局所空間を設定する局所空間設定手段、及び前記局所空間において前記干渉飛行体が互いに干渉関係にない回避計画を、前記干渉飛行体の各々について算出して前記飛行計画を修正する飛行計画修正手段として機能させるためのプログラムである。
【発明の効果】
【００１４】
以上説明したように、本発明の飛行計画算出装置及びプログラムによれば、少ない計算コストで互いに干渉関係にない飛行計画を求めることができる、という効果が得られる。
【図面の簡単な説明】
【００１５】
【図１】本発明の実施の形態に係る移動制御システムの構成を示す概略図である。
【図２】本発明の実施の形態に係る自律飛行ロボットの構成を示す概略図である。
【図３】本発明の実施の形態に係る管理装置の構成を示す概略図である。
【図４】経路情報の構成を示す図である。
【図５】個別経路の例を示す図である。
【図６】干渉位置を説明するための図である。
【図７】風の影響範囲を表す円柱の例を示す図である。
【図８】局所空間の例を示す図である。
【図９】干渉飛行体の台数に応じて局所空間の大きさを設定する例を示す図である。
【図１０】複数の局所空間を包含する局所空間の例を示す図である。
【図１１】複数の局所空間を包含する局所空間の他の例を示す図である。
【図１２】（Ａ）個別経路の例を示す図、及び（Ｂ）個別経路を修正した結果の例を示す図である。
【図１３】本発明の実施の形態に係る管理装置による経路探索処理の動作を示すフローチャートである。
【図１４】本発明の実施の形態に係る管理装置による局所空間設定処理の動作を示すフローチャートである。
【発明を実施するための形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、複数のドローンの移動を制御する移動制御システムに本発明を適用した場合を例に説明する。
【００１７】
＜システム構成＞
以下、本発明を適用した移動制御システム１０００の概略構成を示した図１を参照し、本発明の実施の形態の構成を説明する。
【００１８】
（移動制御システム１０００）
移動制御システム１０００は、管理装置１００、及び複数台の自律飛行ロボット２００を有する。管理装置１００と自律飛行ロボット２００の間は所定の閉域網（無線ＬＡＮ等）を介して通信する。なお、管理装置１００が、飛行計画算出装置の一例である。
【００１９】
（自律飛行ロボット２００）
自律飛行ロボット２００は、例えば、クアッドロータ型の小型無人ヘリコプタである。なお、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタについても同様に適用することができる。
【００２０】
自律飛行ロボット２００は、管理装置１００から、飛行計画の一例である飛行経路に係る情報（後述する経路情報）を受信し、当該飛行経路に沿って飛行するよう制御する。以下、自律飛行ロボット２００を構成する位置・姿勢センサ２１０、通信部２２０、記憶部２３０、制御部２４０、モータ２５０、及びロータ２６０の各部について、詳細に説明する（図２参照）。
【００２１】
（位置・姿勢センサ２１０）
位置・姿勢センサ２１０は、自律飛行ロボット２００の現在位置および姿勢を取得するためのセンサである。位置・姿勢センサ２１０は、例えば、ＧＮＳＳ（Global Navigation Satellite System）等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する受信機、加速度を計測する加速度センサ、方位を計測する電子コンパス、および角速度を計測するジャイロセンサを備えている。具体的には、位置・姿勢センサ２１０において、受信機が複数の航法衛星から送信される航法信号を受信して制御部２４０に出力するとともに電子コンパスおよびジャイロセンサによる計測信号を制御部２４０に出力する。
【００２２】
現在位置を取得するために受信機に代えてレーザスキャナおよび気圧センサを用いるなど、他の既知のセンサを用いて既知の従来技術により現在位置および姿勢を得るための情報を取得するものとしてもよい。
【００２３】
（通信部２２０）
通信部２２０は、管理装置１００との間で通信するための通信モジュールである。
【００２４】
（記憶部２３０）
記憶部２３０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。
【００２５】
記憶部２３０は、各種プログラムや各種データを記憶し、制御部２４０との間でこれらの情報を入出力する。
【００２６】
各種データには、位置・姿勢情報２３２、経路情報２３４等の制御部２４０の各処理に用いられる情報が含まれる。
【００２７】
位置・姿勢情報２３２は、位置・姿勢センサ２１０によって取得された、自律飛行ロボット２００の現在位置および姿勢を予め定められた回数だけ循環記憶した位置と姿勢の履歴である。ここで、履歴内の最新の位置及び姿勢を特に現在位置及び現在姿勢と称し、それ以外の位置及び姿勢を過去位置及び過去姿勢と称する。
【００２８】
経路情報２３４は、自律飛行ロボット２００が移動する予定である飛行経路に関する情報である。具体的には、飛行経路上における座標列（ｘ,ｙ,ｚ）と、各位置（座標）における時刻、速度、加速度とを対応付けた情報である。経路情報２３４は、管理装置１００から受信する。
【００２９】
制御部２４０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、記憶部２３０からプログラムやデータを読み出し、プログラムに従って、位置・姿勢算出手段２４２および飛行制御手段２４４等として機能する。
【００３０】
位置・姿勢算出手段２４２は、位置・姿勢センサ２１０の出力から飛行空間における自律飛行ロボット２００の現在位置および姿勢を算出し、位置・姿勢情報２３２として記憶させる。
【００３１】
位置・姿勢算出手段２４２は、例えば、位置・姿勢センサ２１０によって出力された航法信号から緯度・経度・高度を求め、予め記憶した変換規則を用いて飛行空間の座標系における位置に変換する。
【００３２】
また、位置・姿勢算出手段２４２は、位置・姿勢センサ２１０によって出力された加速度センサおよびジャイロセンサの計測信号から、飛行空間の座標系における現在姿勢を求める。
【００３３】
なお、位置・姿勢センサ２１０によって出力された電子コンパスの計測信号から、飛行空間の座標系における方位を求め、他のセンサからの計測信号を更に用いて現在姿勢を算出してもよい。
【００３４】
以上のように、位置・姿勢センサ２１０と通信部２２０と位置・姿勢算出手段２４２（制御部２４０）とが共働して自律飛行ロボット２００の現在位置及び現在姿勢を検出する。
【００３５】
なお、位置・姿勢算出手段２４２は、現在位置を算出する都度、通信部２２０を介して当該現在位置を管理装置１００に送信する。
【００３６】
飛行制御手段２４４は、経路情報２３４、位置・姿勢情報２３２を参照し、自律飛行ロボット２００を経路情報２３４に記された飛行経路に追従して移動するようモータ２５０の回転速度を制御する。
【００３７】
具体的には、飛行制御手段２４４は、経路情報２３４に記された現在時刻の位置（座標）と、位置・姿勢情報２３２に記された現在位置との誤差が小さくなるようにモータ２５０の回転速度を制御する。
【００３８】
また、飛行制御手段２４４は、位置・姿勢情報２３２に基づいて自律飛行ロボット２００の現在の速度及び加速度を算出し、これらの値が経路情報２３４に記された現在時刻の速度及び加速度との誤差が小さくなるようにモータ２５０の回転速度を制御する。この際、位置・姿勢情報２３２における姿勢に係る情報も考慮する。
【００３９】
（モータ２５０、ロータ２６０）
４つのロータ２６０と、その回転軸がそれぞれ対応するロータ２６０に連結された４つのモータ２５０からなる４つのユニットが自律飛行ロボット２００に搭載されている。
【００４０】
各モータ２５０は、制御部２４０に接続されて制御部２４０から回転速度を指示される。４つのロータ２６０が独立して回転することにより自律飛行ロボット２００に任意方向の加速度を発生させる。
【００４１】
（管理装置１００）
管理装置１００は、飛行空間内の所定位置に設置され、自律飛行ロボット２００の飛行先である目標位置を決定し、自律飛行ロボット２００から受信した現在位置に基づいて、現在位置から目標位置に至る飛行経路を求め、自律飛行ロボット２００に送信する。
【００４２】
なお、管理装置１００は、遠隔の監視センタに設置してある図示しないセンタ装置と広域網（インターネット、携帯電話網等）を介して通信し、センタ装置からの指示を受け、当該指示に基づいて目標位置を設定したり、センタ装置に自律飛行ロボット２００に係る各種情報を送信するようにしてもよい。
【００４３】
以下、管理装置１００を構成する通信部１１０、記憶部１２０、及び制御部１３０の各部について、詳細に説明する（図３参照）。
【００４４】
（通信部１１０）
通信部１１０は、自律飛行ロボット２００との間で通信するための通信モジュールである。
【００４５】
（記憶部１２０）
記憶部１２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。
【００４６】
記憶部１２０は、各種プログラムや各種データを記憶し、制御部１３０との間でこれらの情報を入出力する。
【００４７】
各種データには、位置情報１２２、空間情報１２４、機体情報１２６、経路情報１２８等の制御部１３０の各処理に用いられる情報が含まれる。
【００４８】
（位置情報１２２）
位置情報１２２は、自律飛行ロボット２００の現在位置及び目標位置を示した情報である。
【００４９】
自律飛行ロボット２００から現在位置を受信すると、当該自律飛行ロボット２００の識別子（機体ＩＤ）と対応付けて記憶する。
【００５０】
また、目標位置設定手段１３２によって、ある自律飛行ロボット２００の目標位置が設定されると、当該自律飛行ロボット２００の機体ＩＤに対応付けて目標位置が記憶される。
【００５１】
（空間情報１２４）
空間情報１２４は、飛行空間の３次元構造等を表した情報である。本実施形態における空間情報１２４は、飛行空間をボクセル空間として複数のボクセル（単位空間）に分割して飛行空間の障害物の構造等を表した情報である。空間情報１２４は、システムの管理者によって予め設定され記憶される。
【００５２】
本実施形態では、飛行空間を所定単位の大きさ（例えば１５ｃｍ×１５ｃｍ×１５ｃｍ）のボクセルに等分割し、ボクセル毎に識別子であるボクセルＩＤと、飛行空間におけるボクセルの位置（座標）とを対応付けて空間情報１２４として記憶する。
【００５３】
また、空間情報１２４は、各ボクセルの属性を示す情報を含む。例えば、属性として、自律飛行ロボット２００の移動の障害となるか否かを示した障害属性が少なくとも付与される。
【００５４】
すなわち、建造物等の障害物に位置するボクセルは、自律飛行ロボット２００が移動できない空間として障害属性として設定される。
【００５５】
このように、空間情報１２４としてボクセルの位置と属性を用いることにより飛行空間に存在する障害物等の物体の位置や大きさ（形状）を表現することができる。
【００５６】
（機体情報１２６）
機体情報１２６は、各自律飛行ロボット２００の特性（性能）を示した情報である。機体情報１２６は、システムの管理者によって予め設定され記憶される。
【００５７】
具体的には、機体情報１２６は、各自律飛行ロボット２００の機体ＩＤと、特性値（機体重量、大きさ、最大速度、最大加速度、及び最大加加速度）とを対応付けた情報である。
【００５８】
（経路情報１２８）
経路情報１２８は、各自律飛行ロボット２００の飛行経路に関する情報である。具体的には、経路情報１２８は、自律飛行ロボット２００の機体ＩＤと、経路上の座標値（x、y、z）、当該座標値における通過時間、速度、加速度、加加速度（Jerk）、加加速度の変化量（Snap）の値とが対応付けて記憶される。
【００５９】
例えば、図４に示す経路情報１２８では、機体ＩＤが「Ａ」である自律飛行ロボット２００について、経路上の座標値（x1、y1、z1）の通過時間がt1であり、加速度がa1であり、加加速度がj1であり、加加速度の変化量がs1であることが記憶され、また、経路上の座標値（x2、y2、z2）の通過時間がt2であり、加速度がa2であり、加加速度がj2であり、加加速度の変化量がs2であることが記憶されている。ここで、加速度a1、a2は、加速度のｘ軸成分、ｙ軸成分、ｚ軸成分を要素とするベクトルであり、加加速度j1、j2は、加加速度のｘ軸成分、ｙ軸成分、ｚ軸成分を要素とするベクトルであり、加加速度の変化量s1、s2は、加加速度の変化量のｘ軸成分、ｙ軸成分、ｚ軸成分を要素とするベクトルである。
【００６０】
経路情報１２８は、経路算出手段１３４及び経路修正手段１４２によって更新される。
【００６１】
（制御部１３０）
制御部１３０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、記憶部１２０からプログラムやデータを読み出し、プログラムに従って、目標位置設定手段１３２、経路算出手段１３４、干渉判定手段１３６、局所空間設定手段１３８、及び経路修正手段１４２等として機能する。なお、経路修正手段１４２が、飛行計画修正手段の一例である。
【００６２】
（目標位置設定手段１３２）
目標位置設定手段１３２は、各自律飛行ロボット２００の目標位置を設定し、機体ＩＤに対応付けて位置情報１２２として記憶部２３０に記憶させる。
【００６３】
目標位置は、例えば、巡回警備をタスクとする自律飛行ロボット２００であれば、予め定めた時刻になったときに所定の巡回位置が目標位置となるよう設定する。また、侵入者への対応をタスクとする自律飛行ロボット２００であれば、図示しないセンサからの侵入検知出力に基づいて、当該センサの検知位置の近傍を目標位置に設定してもよい。
【００６４】
他にも、図示しないセンタ装置からの指示に基づいて、目標位置を設定してもよい。
【００６５】
（経路算出手段１３４）
経路算出手段１３４は、各自律飛行ロボット２００の現在位置から目標位置に至る飛行経路を算出し、算出した経路を経路情報１２８として記憶部２３０に記憶させる個別経路算出処理を行う。この際、算出される飛行経路は、他の自律飛行ロボット２００との干渉を考慮せずに、障害物と干渉関係にないことのみを考慮して算出した飛行経路であり、以下、経路算出手段１３４で算出した飛行経路を個別経路と称する。
【００６６】
後述するように、経路算出手段１３４は、Ａ＊経路探索法などの経路探索法によって基本経路を算出し、当該基本経路に基づいて経由点を設定し、当該経由点等を制約条件とした最適化問題を解くことにより個別経路を算出している。個別経路算出処理の詳細については後述する。
【００６７】
（干渉判定手段１３６）
干渉判定手段１３６は、個別経路間の空間的な近さから自律飛行ロボット２００同士が干渉しうる（他の自律飛行ロボット２００の飛行に悪影響を及ぼしうる）位置である干渉位置と、干渉しうる自律飛行ロボット２００（以下、干渉飛行体と称する）を求める干渉判定処理を行う。干渉判定処理の詳細については後述する。また、干渉とは、所定基準を超えて近接しないことをいう。
【００６８】
（局所空間設定手段１３８）
局所空間設定手段１３８は、干渉判定手段１３６にて求めた干渉位置を含む局所空間を設定する局所空間設定処理を行う。局所空間設定処理の詳細については後述する。
【００６９】
（経路修正手段１４２）
経路修正手段１４２は、局所空間において干渉飛行体が互いに干渉関係にない回避経路を、最適化計算を行うことによって算出し、算出された回避経路に基づいて経路情報を修正する個別経路修正処理を行う。個別経路修正処理の詳細については後述する。なお、回避経路が、回避計画の一例である。
【００７０】
（個別経路算出処理について）
ここで、個別経路算出処理の詳細について説明する。
【００７１】
まず、空間情報に基づいて、障害属性が付与されていないボクセルの中心位置をノードとして求める。そして求めた各ノードと隣接するノード間を連結した線分をエッジとしたグラフ構造を求める。なお、グラフ構造は経路探索処理の前に予め生成し、記憶しておいてよい。
【００７２】
生成したグラフ構造及び位置・姿勢情報２３２を用いて、現在位置に対応するボクセルのノード（以下「開始ノード」という）から目標位置に対応するボクセルのノード（以下「目標ノード」という）に至る経路の長さ等によって求まる総コスト値が最も小さくなる経路をＡ＊経路探索法により探索する。この探索によって求まる経路を「基本経路」という。なお、基本経路の探索方法は、Ａ＊経路探索法に限定されるものでなく、ダイクストラ法等の他の経路探索方法を適用してもよい。Ａ＊経路探索法による経路探索手法については、例えば、参考文献１に記載の手法と同様であるため、詳細な説明を省略する。
【００７３】
［参考文献１］：特開2017-016359号公報
【００７４】
そして、基本経路に基づいて経由点を設定する。例えば、基本経路を構成するノードの中で直線上に配置されているノードを除いた場合に残ったノードを経由点として選択する。
【００７５】
位置情報、経由点の位置、機体性能（最大速度、最大加速度、最大加加速度等）を制約条件として設定し、目的関数を最小化する経路（多項式経路）を個別経路として算出する。
【００７６】
多項式経路とは、時間の変数と係数で構成される多項式で経路を表現したものである。各XYZ軸それぞれについて多項式経路がある。以下に、X軸について多項式経路を表す式を示す。
ｘ（ｔ）＝ｃ5ｔ5＋ｃ4ｔ4＋ｃ3ｔ3＋ｃ2ｔ2＋ｃ1ｔ＋ｃ0
【００７７】
また、多項式経路は基本経路の経由点間毎に設定される。例えば、図５に示すように、基本経路Ｂに対して、始点Ｓと経由点Ｎ１との間の多項式経路と、経由点Ｎ１、Ｎ２間の多項式経路と、経由点Ｎ２と終点Ｇの間の多項式経路とが、個別経路Ｒとして設定される。
【００７８】
自律飛行ロボット２００が追従し易い個別経路を算出するため、ここでは以下の制約条件を満たした上で、加加速度変化が最小となる個別経路を算出する。したがって、目的関数は経路の加加速度変化を積分したものとし、これを最小化する多項式経路を求める。また、加速度変化が最小となる個別経路を求めてもよい。
【００７９】
＜制約条件＞
a. 個別経路の始点は現在位置である。また、始点の速度、加速度、加加速度、加加速度の変化は0である。
b. 個別経路の終点は目標位置である。また、終点の速度、加速度、加加速度、加加速度の変化（snap）は0である。
c. 経由点の位置は上記で設定された位置である。そして、経由点間前後の経路を滑らかに接続させるため、経由点の速度、加速度、加加速度は経由点間前後の多項式経路で同じ値をとる。
d. 機体性能を満たす経路とするため、経路の速度、加速度、加加速度の最大値が指定値以内となるようにする。
【００８０】
上記のa.〜d.の制約条件を満たした上で、目的関数を最小化する経路を求めることで、機体動作制約を満たした個別経路、かつ、経路通過時の時間等が明確な経路を算出する。制約条件を満たした個別経路の算出方法は、下記の参考文献２に記載の手法と同様であるため、詳細な説明を省略する。
【００８１】
［参考文献２］：D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors," 2011 IEEE International Conference on Robotics and Automation, Shanghai, 2011, pp. 2520-2525.
【００８２】
（干渉判定処理について）
次に、干渉判定処理の詳細について説明する。
【００８３】
干渉判定処理は、個別経路間の空間的な近さから自律飛行ロボット２００同士が干渉しうる位置である干渉位置と、干渉しうる自律飛行ロボット２００を求める処理である。例えば、図６に示すように、自律飛行ロボット２００ａ〜２００ｅの個別経路Ｒａ〜Ｒｅの間の空間的な近さから、自律飛行ロボット２００ａ〜２００ｅ同士が干渉しうる干渉位置と、自律飛行ロボット２００ａ〜２００ｅのうち、干渉しうる自律飛行ロボット２００を求める。
【００８４】
本実施形態では、自律飛行ロボット２００の移動によって生じる風の影響範囲を考慮して「空間的な近さ」の判定を行う。
【００８５】
まず、以下に説明するように、風の影響範囲を求める。
【００８６】
機体情報１２６に記された機体重量、大きさ、最大加速度に基づいて、風の影響範囲を円柱に近似することによって簡易的に求める。
【００８７】
具体的には、自律飛行ロボット２００の大きさが大きいほど円柱の底面の半径が大きくなるよう設定し、自律飛行ロボット２００の機体重量と最大加速度が大きいほど円柱の高さが高くなるよう設定する。例えば、図７に示すように、自律飛行ロボット２００ａ、２００ｂに対して、風の影響範囲を表す円柱Ｃａ、Ｃｂを設定する。
【００８８】
上記の方法（円柱近似）の他にも、機体の最大傾け角度や、ロータ２６０の配置とロータ２６０の大きさ、ブレード傾き、最大回転速度等を考慮して流体力学的に解析することによって風の影響範囲を求めてもよい。また、自律飛行ロボット２００の機体重量のみに応じて円柱の高さを決定してもよいし、自律飛行ロボット２００の最大加速度のみに応じて円柱の高さを決定してもよい。
【００８９】
そして、以下に説明するように、経路情報１２８に基づいて風の影響範囲を移動させ干渉位置を求める。
【００９０】
まず、経路情報１２８を参照し、全ての自律飛行ロボット２００に対して、それぞれの自律飛行ロボット２００の個別経路上の所定時刻における位置（座標）を求め、当該位置に、上記で求めた円柱を配置する。また、時刻を所定間隔で変化させ、各時刻においても同様に円柱を配置する。例えば、図７に示すように、自律飛行ロボット２００ａ、２００ｂの個別経路Ｒａ、Ｒｂ上の位置Ｐａ、Ｐｂに、円柱Ｃａ、Ｃｂを配置する。
【００９１】
そして、ある自律飛行ロボット２００の円柱と、他の自律飛行ロボット２００の円柱とが重なる位置に基づいて干渉位置を求める。例えば、図７に示すように、自律飛行ロボット２００ａ、２００ｂの位置Ｐａ、Ｐｂで、円柱Ｃａ、Ｃｂが重なる場合には、自律飛行ロボット２００ａ、２００ｂの位置Ｐａ、Ｐｂの中間地点Ｉａｂを干渉位置として求める。なお、円柱が重なった部分の重心位置を干渉位置として求めてもよい。
【００９２】
なお、干渉し合った各自律飛行ロボット２００を「干渉飛行体」として特定する。
【００９３】
（局所空間設定処理について）
次に、局所空間設定処理の詳細について説明する。
【００９４】
まず、干渉位置を含む局所空間を設定する。本実施形態では、干渉飛行体である各自律飛行ロボット２００の機体重量や最大加速度等に基づいて、自律飛行ロボットの移動によって生じる風の影響範囲を考慮した大きさの局所空間を設定する。
【００９５】
例えば、図８に示すように、自律飛行ロボット２００ｃ、２００ｄの円柱が重なる位置に基づいて求められた干渉位置Ｉｃｄを中心位置とした立方体Ｌｃｄを設定する。このとき、自律飛行ロボット２００ｃ、２００ｄの機体重量が重いほど、局所空間を大きく設定したり、自律飛行ロボット２００ｃ、２００ｄの最大加速度が大きいほど、局所空間を大きく設定する。これは、自律飛行ロボット２００の最大加速度と機体重量が大きいほどロータ２６０によって生じる風が大きくなるため、自律飛行ロボット２００と干渉しやすいためである。一方、自律飛行ロボットの最大加速度や機体重量が小さい場合には、他の自律飛行ロボット２００と干渉しにくいため、局所空間を小さく設定することで回避経路の計算量をより少なくすることが可能となる。
【００９６】
また、干渉飛行体である各自律飛行ロボット２００の台数を更に考慮して、局所空間の大きさを設定してもよい。例えば、図９（Ａ）に示すように、局所空間Ｌａｃについての干渉飛行体が、自律飛行ロボット２００ａ〜２００ｃの３台である場合と、図９（Ｂ）に示すように、局所空間Ｌａｅについての干渉飛行体が、自律飛行ロボット２００ａ〜２００ｅの５台である場合とでは、局所空間Ｌａｅの方が大きく設定される。これは、干渉飛行体である自律飛行ロボット２００の台数が多いほど、各自律飛行ロボット２００が互いに干渉しやすい状況であるためである。
【００９７】
また、干渉飛行体である各自律飛行ロボット２００の大きさを更に考慮して、局所空間の大きさを設定してもよい。これは、干渉飛行体である自律飛行ロボット２００の大きさが大きいほど、各自律飛行ロボット２００が互いに干渉しやすい状況であるためである。
【００９８】
また、立方体の中心位置が干渉位置となるように配置したときに当該立方体が障害物などの飛行不可領域と重なる場合は、重ならない位置まで立方体の位置をずらす。あるいは、飛行不可領域と重ならない大きさであり干渉位置を含む直方体を局所空間として設定する。
【００９９】
また、複数の干渉位置が検出されたときは、干渉位置ごとに局所空間を設定する。この際、ある局所空間と他の局所空間との一部が重なり合っている場合、重なり合った局所空間を包含する大きさからなる直方体を局所空間として設定する。例えば、図１０に示すように、干渉位置Ｉ１を中心とする立方体Ｌ１と、干渉位置Ｉ２を中心とする立方体Ｌ２との一部が重なり合っている場合、立方体Ｌ１、Ｌ２を包含する直方体Ｌ１２を、局所空間として設定する。また、図１１に示すように、干渉位置Ｉ３を中心とする立方体Ｌ３を、干渉位置Ｉ４を中心とする立方体Ｌ４が包含している場合には、立方体Ｌ３、Ｌ４を包含する直方体Ｌ３４と、立方体Ｌ４とが等しいため、立方体Ｌ４が、局所空間として設定される。
【０１００】
（回避経路算出処理について）
次に、回避経路算出処理の詳細について説明する。
【０１０１】
前のステップにて決定した干渉飛行体について、対象の局所空間内における互いに干渉関係にない回避経路を、以下のように算出する。
【０１０２】
個別経路、局所空間、機体性能（最大速度、最大加速度、最大加加速度等）に基づく制約条件を設定し、目的関数を最小化する経路を回避経路として算出する。
【０１０３】
各自律飛行ロボット２００の回避経路は多項式経路で表される。
【０１０４】
目的関数は各自律飛行ロボット２００の回避経路の速度変化を積分したものとし、以下の制約条件を満たした上で、これを最小化する多項式経路を求める最適化問題を計算する。なお、加速度変化や加加速度変化を最小とする経路を求めてもよい。
【０１０５】
まず、各干渉飛行体について、対象の局所空間での始点と終点に関する制約条件がある。これは、対象の局所空間内の回避経路と当該局所空間外の個別経路を滑らかに接続させるための制約条件である。具体的には、個別経路が局所空間へ入る位置（始点）とその速度、加速度、加加速度、加加速度の変化を求め、それらの値を回避経路の始点の制約条件とする。さらに、個別経路が局所空間から出る位置（終点）とその速度、加加速度、加加速度の変化を求め、それらの値を回避経路の終点の制約条件とする。
【０１０６】
また、回避経路における速度、加速度、加加速度の制約条件を設定する。すなわち、制約条件は、機体性能に示す速度、加速度、加加速度の最大値の範囲内であることとする。
【０１０７】
また、各干渉飛行体同士の離間距離が一定以上である制約条件も設定してもよい。例えば、上記で設定した各移動体の影響範囲が重ならないことを制約条件としてもよい。
【０１０８】
以下の式に示すように、上記の制約条件を満たした上で、目的関数を最小化する回避経路を求める。
【０１０９】
【数１】
2020057312.tif 000003
【０１１０】
ただし、Ｘは、回避経路を表す多項式経路の係数からなるベクトルであり、ＡeqＸ＝ｂeqは、始点と終点とに関する制約条件を表し、ＡinＸ≦ｂinは、機体性能に関する制約条件を表す。
【０１１１】
上記の目的関数をｆ0とすると、目的関数ｆ0は、例えば、以下の式で表される。
【０１１２】
【数２】
2020057312.tif 000004
【０１１３】
ただし、Ｎは、干渉飛行体となる自律飛行ロボット２００の数であり、Ｋは、離散時間の数であり、ａi［ｋ］は、ｉ番目の自律飛行ロボット２００の時刻ｋにおける加速度を表す。
【０１１４】
制約条件を満たしかつ、目的関数を最小化する回避経路の求め方は、下記の参考文献３に記載の手法と同様であるため、詳細な説明を省略する。
【０１１５】
［参考文献３］：F. Augugliaro, A. P. Schoellig and R. D'Andrea, "Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach," 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura, 2012, pp. 1917-1922.7
【０１１６】
（個別経路修正処理について）
次に、個別経路修正処理の詳細について説明する。
【０１１７】
個別経路修正処理では、干渉飛行体については、それぞれ算出した回避経路により個別経路を修正する。
【０１１８】
例えば、図１２（Ａ）、（Ｂ）に示すように、干渉飛行体である、自律飛行ロボット２００ａ、２００ｄ、２００ｅの個別経路Ｒａ、Ｒｄ、Ｒｅについて、局所空間内の経路が、自律飛行ロボット２００ａ、２００ｄ、２００ｅの回避経路ｒａ、ｒｄ、ｒｅとなるように修正することにより、修正後の個別経路Ｒａ’、Ｒｄ’、Ｒｅ’を求める。
【０１１９】
＜移動制御システムの動作＞
以下、図１３〜図１４に示したフローチャートを参照しつつ、本発明を適用した移動制御システム１０００による経路探索処理の動作を説明する。なお、管理装置１００の記憶部１２０に、位置情報１２２、空間情報１２４、及び機体情報１２６が予め格納されている場合を例に説明する。
【０１２０】
複数の自律飛行ロボット２００の中で、少なくとも１台の自律飛行ロボット２００の目標位置が変更されたときに、図１３に示す経路探索処理が実行される。
【０１２１】
図１３に示す管理装置１００の経路探索処理では、最初に、目標位置が変更された１又は複数の自律飛行ロボット２００を経路更新対象として選択し、ループ１のステップＳ１００〜Ｓ１０２の処理が、各経路更新対象に対して実行される。以下、ループ１において選択された経路更新対象を「注目ロボット」と称する。
【０１２２】
経路算出手段１３４は、位置情報１２２から、注目ロボットの現在位置、現在姿勢、目標位置、目標姿勢を読み出す（ステップＳ１００）。次に、注目ロボットについて、現在位置から目標位置に至る飛行経路、すわなち個別経路を算出する個別経路算出処理を行う（ステップＳ１０２）。
【０１２３】
そして、干渉判定手段１３６は、干渉判定処理を行い、個別経路間の空間的な近さから自律飛行ロボット２００同士が干渉しうる位置である干渉位置と、干渉しうる自律飛行ロボット２００を求める干渉判定処理を行う（ステップＳ１０４）。
【０１２４】
そして、局所空間設定手段１３８は、上記ステップＳ１０４での干渉判定処理の結果に基づいて、干渉位置が存在するか否かを判定する（ステップＳ１０６）。干渉位置が存在しない場合には、経路探索処理ルーチンを終了する。
【０１２５】
一方、干渉位置が存在する場合に、局所空間設定手段１３８は、干渉位置を含む局所空間を設定する局所空間設定処理を行う（ステップＳ１０８）。局所空間設定処理の詳細については後述する。
【０１２６】
そして、ループ２のステップＳ１１２の処理が、各局所空間に対して実行される。以下、ループ２において処理の対象となる局所空間を「注目局所空間」と称する。
【０１２７】
経路修正手段１４２は、注目局所空間において各干渉飛行体が互いに干渉関係にない回避経路を、最適化計算を行うことによって算出する（ステップＳ１１２）。
【０１２８】
経路修正手段１４２は、算出された回避経路に基づいて経路情報を修正する個別経路修正処理を行って（ステップＳ１１４）、経路探索処理を終了する。
【０１２９】
上記ステップＳ１０８の処理は、図１４に示す局所空間設定処理により実現される。
【０１３０】
図１４に示す管理装置１００の局所空間設定処理では、最初に、局所空間設定手段１３８は、複数の干渉位置が存在するか否かを判定する（ステップＳ１２０）。干渉位置が複数ある場合、局所空間設定手段１３８は、各干渉位置間の距離（干渉位置間距離）を求める（ステップＳ１２２）。そして、局所空間設定手段１３８は、当該干渉位置間距離が予め定めた閾値Ｔｈ２以下であるか否かを判定する（ステップＳ１２４）。当該干渉位置間距離が閾値Ｔｈ２以下である場合、局所空間設定手段１３８は、２つの干渉位置の中間位置を新たな干渉位置として統合して（ステップＳ１２６）、上記ステップＳ１２２へ戻る。
【０１３１】
一方、干渉位置が１つだけの場合や、当該干渉位置間距離が閾値Ｔｈ２より大きい場合には、ループ４のステップＳ１２８〜Ｓ１３２の処理が、各干渉位置に対して実行される。干渉位置を１つ選択し、ループ４における処理の対象とする。以下、ループ４において選択された干渉位置を「注目干渉位置」と称する。
【０１３２】
局所空間設定手段１３８は、注目干渉位置に対応する干渉飛行体に係る機体情報（重量、大きさ、最大加速度）を読み出す（ステップＳ１２８）。
【０１３３】
そして、局所空間設定手段１３８は、注目干渉位置に対応する各干渉飛行体の機体情報に基づいて影響範囲（円柱）を算出する（ステップＳ１３０）。なお、干渉判定処理にて影響範囲を既に算出し、記憶している場合はその値を読み出すだけでもよい。
【０１３４】
そして、局所空間設定手段１３８は、各干渉飛行体の影響範囲を示す円柱の体積を考慮して、局所空間の大きさを決定する（ステップＳ１３２）。例えば、各干渉飛行体の影響範囲を示す円柱の体積を求め、当該円柱の体積の総和を求める。求めた体積の総和が局所空間の体積の所定割合（例えば５％）となるよう、局所空間の体積を決定する。
【０１３５】
局所空間設定手段１３８は、求めた局所空間の体積となるよう、局所空間の立体の大きさを決定し、当該局所空間の略中心が干渉位置となるような位置に局所空間を設定し（ステップＳ１３４）、局所空間設定処理を終了する。
【０１３６】
なお、干渉飛行体の影響範囲を算出することなく、影響範囲の算出に用いる値として例示した値から直接的に局所空間の大きさを決定してもよい。例えば、干渉飛行体である各自律飛行ロボット２００の機体重量、最大加速度、大きさ、及び台数に応じて局所空間の大きさを決定してもよい。
【０１３７】
以上説明してきたように、本発明の実施の形態に係る移動制御システム１０００では、複数の自律飛行ロボットの各々についての飛行経路から求められる干渉位置を含む局所空間であって、干渉飛行体の各々の最大加速度及び機体重量の少なくとも一方により決定される大きさの局所空間を設定し、局所空間において干渉飛行体が互いに干渉関係にない回避経路を、干渉飛行体の各々について算出して飛行経路を修正することにより、少ない計算コストで互いに干渉関係にない飛行経路を求めることができる。
【０１３８】
＜変形例＞
以上、本発明の好適な実施形態について説明してきたが、本発明はこれらの実施形態に限定されるものではない。例えば、本実施形態では、干渉判定処理にて、経路情報１２８に記憶された各個別経路における各時刻における座標値（x、y、z）に基づいて干渉を判定している。しかし、個別経路における時刻を用いずに座標値（x、y、z）を用いて干渉を判定してもよい。すなわち、ある自律飛行ロボットの個別経路上における各位置と、他の自律飛行ロボットの個別経路上における各位置との空間的な距離の近さから干渉を判定する。また、本実施形態では、個別経路算出処理にてＡ＊経路探索法などの経路探索法によって基本経路を算出し、当該基本経路に基づいて経由点を設定し、当該経由点等を制約条件とした最適化問題を解くことにより個別経路を算出している。しかし、これに限らず、Ａ＊経路探索法などの経路探索法のみによって個別経路を算出してもよい。この場合、算出される個別経路の経路情報は、自律飛行ロボットの機体ＩＤと、経路上の位置を示す座標値（x、y、z）のリストとから構成される情報となる。そして、この場合における干渉判定処理は、ある自律飛行ロボットの個別経路上における各位置と、他の自律飛行ロボットの個別経路上における各位置との空間的な距離の近さから干渉を判定する。
【０１３９】
例えば、ある自律飛行ロボットの個別経路上における各位置に風の影響範囲を表す円柱を設置し、同様に他の自律飛行ロボットの個別経路上における各位置に風の影響範囲を表す円柱を設置したとき、各円柱が重なり合う場合に干渉関係にあると判定する。そして、前述と同様の方法で干渉位置を求め、局所空間を設定する。さらに、個別経路修正処理では、個別経路と自律飛行ロボットの平均移動速度とから局所空間に入る時刻及び位置と出る時刻及び位置とを求め、これらの値を制約条件として、前述した個別経路修正処理と同様の方法で最適化問題を解くことにより、回避経路を算出する。
【０１４０】
また、風速に応じて局所空間の大きさを変更してもよい。例えば、管理装置１００は、地面や壁面等に設置された固定型の風速計（不図示）と閉域網を介して接続され、飛行空間内の風速（風の強さ）に基づいて、局所空間の大きさを修正してもよい。例えば、飛行空間内の風速が大きい場合は、安全を見て通常時よりも大きい局所空間となるように局所空間の大きさを修正してもよい。
【０１４１】
また、干渉飛行体以外の自律飛行ロボットの飛行経路に干渉しない範囲で局所空間を設定するようにしてもよい。局所空間の形状は立方体であるのが好適であるが、求めた局所空間が干渉飛行体以外の自律飛行ロボットの飛行経路に干渉する場合、当該自律飛行ロボットの飛行経路についても回避経路を算出する必要性が生じる。したがって、求めた局所空間（立方体）が干渉飛行体以外の自律飛行ロボットの飛行経路に干渉する場合、例えば、干渉しない形状からなる略同一体積からなる直方体として、局所空間を設定するのが好適である。また、求めた局所空間（立方体）が干渉飛行体以外の自律飛行ロボットの飛行経路に干渉する場合、干渉飛行体以外の自律飛行ロボットの飛行経路に干渉しない大きさ（体積）からなる局所空間（立方体）として設定したり、干渉飛行体以外の自律飛行ロボットの飛行経路に干渉しない位置に平行移動した局所空間として設定してもよい。
【０１４２】
また、管理装置が備える全ての機能を、特定の自律飛行ロボットが備えるように構成してもよい。この場合、特定の自律飛行ロボットが他の自律飛行ロボットと通信し、全ての自律飛行ロボットの飛行経路を生成してもよい。
【０１４３】
また、管理装置の経路算出手段が、各自律飛行ロボットの個別経路を算出する場合を例に説明したが、これに限定されるものではなく、管理装置以外の装置で算出された各自律飛行ロボットの個別経路を用いてもよい。
【０１４４】
また、飛行体の一例として自律飛行ロボットを用いる場合を例に説明したが、これに限定されるものではなく、自律飛行ロボット以外の飛行体を対象としてもよい。
【０１４５】
以上のように、当業者は本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。
【符号の説明】
【０１４６】
１００ 管理装置
１１０ 通信部
１２０ 記憶部
１２２ 位置情報
１２４ 空間情報
１２６ 機体情報
１２８ 経路情報
１３０ 制御部
１３２ 目標位置設定手段
１３４ 経路算出手段
１３６ 干渉判定手段
１３８ 局所空間設定手段
１４２ 経路修正手段
２００ 自律飛行ロボット
２１０ 位置・姿勢センサ
２２０ 通信部
２３０ 記憶部
２３２ 位置・姿勢情報
２３４ 経路情報
２４０ 制御部
２４２ 位置・姿勢算出手段
２４４ 飛行制御手段
２５０ モータ
２６０ ロータ
１０００      移動制御システム
【図１】
2020057312.tif 000005
【図２】
2020057312.tif 000006
【図３】
2020057312.tif 000007
【図４】
2020057312.tif 000008
【図５】
2020057312.tif 000009
【図６】
2020057312.tif 000010
【図７】
2020057312.tif 000011
【図８】
2020057312.tif 000012
【図９】
2020057312.tif 000013
【図１０】
2020057312.tif 000014
【図１１】
2020057312.tif 000015
【図１２】
2020057312.tif 000016
【図１３】
2020057312.tif 000017
【図１４】
2020057312.tif 000018