JP6664162B2

JP6664162B2 - 自律飛行ロボット

Info

Publication number: JP6664162B2
Application number: JP2015151739A
Authority: JP
Inventors: 晴之天本
Original assignee: Secom Co Ltd
Current assignee: Secom Co Ltd
Priority date: 2015-07-31
Filing date: 2015-07-31
Publication date: 2020-03-13
Anticipated expiration: 2035-07-31
Also published as: JP2017033232A

Description

本発明は、移動空間内を自律的に飛行する自律飛行ロボットに関する。

従来、現在位置から所定の移動目標位置に至る移動経路を算出し、当該移動経路に沿うよう自律的に飛行可能な自律飛行ロボットが提案されている。例えば、特許文献１には、賊等の移動物体から所定の離間距離だけ離れた位置に移動目標位置を設定し、現在位置から当該移動目標位置に至る移動経路を生成し、当該移動経路に沿うよう飛行制御される自律飛行ロボットが開示されている。

当該従来技術では、生成した移動経路と現在の自己位置との差異が小さくなるようモータの出力（回転数）を制御することによって、移動経路に追従するよう自律飛行している。そのため、移動空間内に風が生じていた場合、風に抗して機体が正しく進むよう各モータの出力が制御される。例えば、横風の場合は、当該横風に流されないよう逆らう方向に対してモータ出力が大きくなるよう飛行制御される。

特開２０１４−１１９９０１号公報

しかしながら、このような飛行制御の場合、突風が生じたときに対応が追い付かず障害物に衝突してしまう恐れがあった。また、急に風向きが変化した場合も、機体の傾斜する方向が前後方向や左右方向に大きく変化して、機体動揺してしまい、障害物に衝突してしまう恐れがあった。

そこで、本発明は、風速が大きいほど障害物などの飛行禁止エリアから大きく離れるような移動経路を設定することにより、自律飛行ロボットが風によって飛行禁止エリアに進入する危険性を少なくすることを目的とする。

本発明の１つの態様は、移動空間内を自律飛行する自律飛行ロボットであって、前記移動空間内を複数のボクセルに分割し、前記ボクセルの各々について飛行禁止エリアか否かを示す属性及び自律移動ロボットの移動の障害となる度合いを示すボクセルコスト値を関連付けて記憶した記憶部と、前記移動空間における前記自律飛行ロボットの自己位置を推定する位置推定手段と、前記移動空間における風速を計測する風測定手段と、前記属性および前記ボクセルコスト値に基づいて、前記飛行禁止エリアを通過せずに移動目標位置に到達するまでに通過する前記ボクセルの前記ボクセルコスト値を加算した総コスト値が最も小さくなる移動経路を探索する経路探索手段と、前記移動経路に沿って移動するよう制御する移動制御手段と、を有し、前記経路探索手段は、前記飛行禁止エリアの近傍に前記風速が大きいほど広い範囲に存在する前記ボクセルの前記ボクセルコスト値を大きな値に更新して前記移動経路を探索することを特徴とする自律飛行ロボットである。

ここで、前記風測定手段は、前記移動空間における風向を更に測定し、前記経路探索手段は、前記風向を参照し、前記範囲に存在するボクセルのうちの風上側よりも風下側のボクセルの前記ボクセルコスト値を大きな値に更新することが好適である。

また、前記経路探索手段は、現在から過去の所定期間における最大瞬間風速が大きいほど前記範囲に存在するボクセルの前記ボクセルコスト値を大きな値に更新することが好適である。

また、前記経路探索手段は、現在から過去の所定期間内における最大瞬間風速と該所定期間における平均風速との比から突風率を求め、該突風率が大きくなるほど前記範囲に存在するボクセルの前記ボクセルコスト値を大きな値に更新することが好適である。

本発明によれば、移動空間内に生じている風によって、飛行禁止エリアへの意図しない侵入の危険性を少なくすることができる自律飛行ロボットを提供することができる。

本発明の実施形態における自律飛行ロボットの構成を示す図である。本発明の実施形態における自律飛行ロボットシステムの構成を示す図である。本発明の実施形態における自律飛行ロボットの構成を示す機能ブロック図である。本発明の実施形態における移動経路の生成に用いられるボクセルのグラフ構造を示す図である。本発明の実施形態における風速に応じて更新されたボクセル情報を説明する図である。本発明の実施形態における風速に応じて生成された移動経路を説明する図である。本発明の実施形態におけるローカル目標の算出処理を説明する図である。本発明の実施形態における風速・風向に応じて更新されたボクセル情報を説明する図である。

本発明の実施形態における自律飛行ロボット１は、図１の概観図に示すように、クアッドロータ型の小型無人ヘリコプタである。なお、本発明の適用範囲は、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタについても同様に適用することができる。

自律飛行ロボット１は、図２のシステム構成図に示すように、外部の警備センタ１００や管理装置１０２と通信し、移動空間内に存在する所定の移動物体を目標対象物Ｍとして追跡し、当該目標対象物Ｍに対して所定の機能を発揮するように構成されている。目標対象物Ｍとなる移動物体は、例えば、監視領域内に侵入した人物（賊等）や車両等である。本実施形態では、所定の機能として、目標対象物Ｍを撮像する機能を例に説明するが、特に限定されるものではなく、目標対象物Ｍに対して音を発したり、発光による警告を行ったりする等の機能であってもよい。

警備センタ１００と管理装置１０２とはインターネット等の情報通信網１１０を介して情報伝達可能に接続される。また、自律飛行ロボット１と管理装置１０２は、無線通信等によって情報伝達可能に接続される。

警備センタ１００は、管理装置１０２を介して自律飛行ロボット１と通信を行い、自律飛行ロボット１によって撮像された目標対象物Ｍの撮像画像を受信する。警備センタ１００は、撮像画像に対して画像処理を行い、警備センタ１００にて異常監視している管理者等（図示しない）に警告を発するような機能を備えていてもよい。また、管理装置１０２から目標対象物Ｍの位置（座標）に関する情報を受信し、当該目標対象物Ｍと自律飛行ロボット１によって撮像された撮像画像とを関連付けて管理するというような機能を備えてもよい。

管理装置１０２は、地面や壁面等に設置された固定型の目標対象物検出センサ１０４（１０４ａ，１０４ｂ・・・）、風速計１０５（１０５ａ，１０５ｂ・・・）を備え、目標対象物Ｍの位置を検知する。目標対象物検出センサ１０４は、例えば、レーザセンサとすることができる。レーザセンサは、一定の角度サンプル間隔の角度毎にレーザを二次元的にスキャンすることによって、地面（又は床面）から一定の高さの水平面における検知範囲内に存在する物体（障害物）との距離情報を極座標値として取得する。レーザセンサは、放射状にレーザ光である探査信号を走査し、物体に反射して戻ってきた探査信号を受信して、送信と受信の時間差から物体までの距離を算出し、レーザセンサの設置位置の座標及び探査信号を送信した方向と算出した距離から当該物体の位置の極座標値を求め、当該極座標値から３次元の直交座標値（Ｘ_t，Ｙ_t，Ｚ_t）を求める。管理装置１０２は、目標対象物検出センサ１０４によって求められた物体の位置を目標対象物Ｍの位置として自律飛行ロボット１へ送信する。自律飛行ロボット１は、目標対象物Ｍの位置を受信すると、その位置に基づいて自らの移動経路を算出し、当該移動経路に沿って移動する。なお、管理装置１０２は、レーザセンサの検知範囲が重複する領域に存在する目標対象物Ｍの同一性を検証することで複数のレーザセンサの検知範囲に渡る目標対象物Ｍの追跡を行う。すなわち、レーザセンサ１０４ａの検知範囲に存在する目標対象物Ｍがレーザセンサ１０４ｂの検知範囲に移動したとしても、管理装置１０２は、当該目標対象物Ｍが同一の物体であると判定することができる。

また、管理装置１０２は、地面や壁面等に設置された固定型の風速計１０５（風測定手段）を複数備え、移動空間内の各位置における風速（風の強さ）を測定することができる。本実施形態では、風速計１０５は、図２に表すようにプロペラ等の回転子を利用し、プロペラの回転数により風速を換算する機械式の風速計１０５を利用している。しかし、これに限らず、超音波方式やレーザードップラー方式などを利用した電気式の風速計を用いてもよい。また、風速計１０５を自律飛行ロボット１自身が搭載してもよい。この場合、自らの移動速度などの移動制御に係る情報を考慮して風速計１０５の出力を補正し、実際の風速に近い値を求めることが好適である。さらに、自律飛行ロボット１が風速計１０５を備えずに、自律飛行ロボット１の無風時におけるモータ４の制御指令値とこの場合における移動距離・移動速度の値とを予め測定しておき、現時点のモータ４の制御指令値と実際の移動距離・移動速度の値とを求め、両者を比較することにより、現時点の風速を推測してもよい。管理装置１０２は、風速計１０５によって求められた風速を自律飛行ロボット１へ送信する。自律飛行ロボット１は、風速を受信（または計測）すると、後述するように記憶部８に風速情報８２として記憶し、風速の大きさに基づいて移動経路を算出し、当該移動経路に沿って移動する。

以下、図１の概観図及び図３の機能ブロック図を参照して、自律飛行ロボット１の構成及び機能について説明する。

自律飛行ロボット１は、図１に示すように、４枚のロータ（プロペラ）２（２ａ〜２ｄ）を一平面上に有する。各ロータ２は、バッテリ（二次電池：図示しない）により駆動されるモータ４（４ａ〜４ｄ）を用いて回転させられる。一般的に、シングルロータ型のヘリコプタでは、メインロータによって発生する反トルクをテールロータが生み出すモーメントで相殺することによって方位角を保っている。一方、自律飛行ロボット１のようなクアッドロータ型のヘリコプタでは、前後・左右で異なる方向に回転するロータ２を用いることで反トルクの相殺を行っている。そして、各ロータ２の回転数（ｆａ〜ｆｄ）を制御することにより、様々な機体の移動や姿勢の調節を行うことができる。例えば、機体をヨー方向に回転させたいときは、前後のロータ２ａ、２ｃと左右ロータ２ｄ、２ｂの回転数に差を与えればよい。

撮像部３は、例えばレンズなどの光学系および所定画素（例えば６４０×４８０画素）のＣＣＤやＣＭＯＳなどの２次元アレイ素子を有する二次元イメージセンサで構成され、移動空間の撮像画像を所定の時間間隔で取得するいわゆるカラーカメラである。本実施形態では、撮像部３は、その光軸が自律飛行ロボット１の正面方向を撮像するよう筐体部分に設置され、かつ、水平面（ＸＹ平面）から予め定めた俯角θにより斜め下方の空間を視野角φにおいて撮像するよう設置されている。取得した撮像画像は後述する制御部７に出力され、制御部７により記憶部８に記憶されたり、後述する通信部９を介して管理装置１０２に送信されたりする。

距離検出センサ５は、自律飛行ロボット１の周囲に存在する障害物と自律飛行ロボット１との間の距離を検出し、センサ検出範囲内に存在する障害物の相対的な位置を取得するセンサである。本実施形態では、距離検出センサ５としてマイクロ波センサを備える。マイクロ波センサは、空間にマイクロ波を放出し、その反射波を検知することによって、自律飛行ロボット１の周囲にある物体を探知し、その物体までの距離を求める。距離検出センサ５は、例えば、自律飛行ロボット１の前方に向けて設け、目標対象物Ｍまでの距離を測定するために用いることができる。また、距離検出センサ５は、例えば、自律飛行ロボット１の下部に下向きに設け、地面との距離（高度）を測定するために用いることもできる。また、距離検出センサ５は、例えば、撮像部３の光軸方向に向けて設け、撮像部３の撮像対象物である目標対象物Ｍまでの距離を測定するために用いることができる。

位置検出センサ６は、自律飛行ロボット１の現在位置を取得するためのセンサである。位置検出センサ６は、例えば、ＧＮＳＳ(Global Navigation Satellite System)等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する。位置検出センサ６は、複数の航法衛星（人工衛星）から送信される航法信号を受信して制御部７へ入力する。なお、位置検出センサ６は、レーザスキャナ、ジャイロセンサ、電子コンパス、気圧センサ等の他のセンサを用いて既知の従来技術により自己位置を得るための情報を取得するものとしてもよい。

通信部９は管理装置１０２との間で、例えば無線ＬＡＮや携帯電話回線等により無線通信するための通信モジュールである。本実施形態では、撮像部３によって取得した撮像画像を通信部９により管理装置１０２に送信し、当該撮像画像を管理装置１０２から警備センタ１００に送信することにより、警備員等が遠隔から侵入者を監視することを可能にする。また、通信部９は、管理装置１０２から目標対象物Ｍの位置（座標：Ｘ_t，Ｙ_t，Ｚ_t）を受信することにより、後述するような移動経路の設定を可能にする。

記憶部８は、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）等の情報記憶装置である。記憶部８は、各種プログラムや各種データを記憶し、制御部７との間でこれらの情報を入出力する。各種データには、目標対象物位置８１、風速情報８２、ボクセル情報８３、移動空間グラフ情報８４、各種パラメータ８５等の制御部７の各処理に用いられる情報、各センサ等の出力値及び撮像画像等が含まれる。

目標対象物位置８１は、管理装置１０２から受信した目標対象物Ｍの位置情報（座標：Ｘ_t，Ｙ_t，Ｚ_t）である。本実施形態では、目標対象物位置８１を目標対象物Ｍの足元位置、すなわち目標対象物Ｍが地面（床面）に接している位置とする。制御部７は、通信部９を介して目標対象物Ｍの位置を受信すると目標対象物位置８１として記憶部８に記憶させる。なお、管理装置１０２は、目標対象物Ｍの位置（座標：Ｘ_t，Ｙ_t，Ｚ_t）の時間変化に基づいて目標対象物Ｍの姿勢（例えば、目標対象物Ｍの正面方向を示した情報）を推定し、目標対象物位置８１として記憶してもよい。

風速情報８２は、管理装置１０２から受信した風速の大きさを表す情報である。本実施形態では、移動空間に複数の風速計１０５を設置し、各風速計１０５の設置位置と各風速計１０５によって測定された風速値とを対応付けて記憶している。しかし、これに限らず、移動空間に一つの風速計１０５を設置し、当該風速計１０５によって測定された風速値を、当該移動空間における風速の大きさとみなしてもよい。また、風速計１０５を自律飛行ロボット１が搭載している場合は、搭載した風速計１０５によって測定された風速値と自己位置とを対応付けて記憶する。

ボクセル情報８３は、移動空間をボクセル空間として複数のボクセルに分割して移動空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部８に記憶され、また、後述するように位置推定手段７１及び経路探索手段７３にて更新される情報である。本実施形態では、移動空間を所定の大きさ（例えば１５ｃｍ×１５ｃｍ×１５ｃｍ）のボクセルに等分割し、各ボクセルの識別子であるボクセルＩＤと、移動空間におけるボクセルの位置（三次元座標）と、ボクセル属性と、ボクセルコスト値とを対応付けてボクセル情報８３として記憶する。ボクセル属性には、飛行禁止エリアに相当するボクセルを「占有ボクセル」と定義して、自律飛行ロボット１が移動できない空間とする。なお飛行禁止エリアには、例えば建造物等の障害物に相当するエリア、飛行が許可された敷地外のエリア、飛行が許可された高度よりも高いエリアなどが挙げられる。そして、占有ボクセルの近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルを「自由ボクセル」として定義する。

各ボクセルには、後述する経路探索手段７３にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて登録される。ボクセルコスト値は、占有ボクセルにおいて最大値Ｃ_maxをとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、あるボクセル（評価ボクセル）のボクセルコスト値は、ボクセルコスト値＝Ｃ_max × ｅｘｐ｛−λ×Ｒ｝…式（１）の計算式により算出することが好適である。ここでλは実験によって求めたパラメータとし、Ｒは評価ボクセルに最も近い占有ボクセルからの距離とする。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とする。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を０と設定する。

移動空間グラフ情報８４は、ボクセル情報８３に基づいて作成された３次元グラフ情報である。具体的には、ボクセル情報８３に基づいて、各ボクセルの中心位置をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしたグラフ構造からなる情報である。図４は移動空間グラフ情報８４のグラフ構造を説明する図であり、移動空間におけるボクセルの一部（２７個）を切り欠いたものである。図４において符号Ｂで表す個々の立方体はボクセルを表すものである。また、これらのボクセルＢの中心位置にある黒又はハッチングにて塗りつぶした球はノードであり、ノード間を連結する点線で表示する線分はエッジである。なお、移動空間グラフ情報８４におけるエッジの重みとして、隣接するノード間の距離に基づいて求められる後述する距離コストＣＭが設定されているものとする。

また、記憶部８には、各種パラメータ８５として離間距離等も記憶される。離間距離は、目標対象物Ｍに追従飛行するにあたって、自律飛行ロボット１と目標対象物Ｍとの水平面における維持すべき相対距離である。離間距離は、自律飛行ロボット１の管理者等によって予め設定される。自律飛行ロボット１を用いて所定の目標対象物Ｍを監視する場合、目標対象物Ｍに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Ｍから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施形態の自律飛行ロボット１は、目標対象物Ｍの詳細な撮像画像を取得でき、かつ、当該目標対象物Ｍから攻撃を受け難い距離に離間距離を予め、当該離間距離を保ちつつ追従飛行するように制御される。離間距離は、例えば３ｍとして設定される。

制御部７は、ＣＰＵ等を備えたコンピュータで構成され、位置推定処理、速度推定処理、ボクセル情報更新処理、移動経路生成処理、移動制御処理（経路追従制御）を行う一連の処理として、位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４を含んでいる。

位置推定手段７１は、位置検出センサ６の出力に基づいて、移動空間における自律飛行ロボット１の現在位置（自己位置）を推定する位置推定処理を行う。

具体的には、位置検出センサ６から得られた複数の航法衛星からの航法信号に基づいて既知の周知技術に基づいて推定した緯度・経度と、距離検出センサ５から得られた高度とから自己位置の座標（Ｘ_s，Ｙ_s，Ｚ_s）を計算する。さらに、電子コンパスやジャイロセンサなどの位置検出センサ６からの出力を受けて自己位置として姿勢ＹＡＷを求める。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律飛行ロボット１の現在位置を推定してもよい。

位置推定手段７１は、推定された自己位置（座標：Ｘ_s，Ｙ_s，Ｚ_s及び姿勢ＹＡＷ）と管理装置１０２から受信した目標対象物Ｍの位置（座標：Ｘ_t，Ｙ_t，Ｚ_t）を経路探索手段７３へ出力する。

なお、位置推定手段７１は、目標対象物Ｍの位置に基づいてボクセル情報８３を更新する処理を行う。具体的には、記憶部８のボクセル情報８３に基づいたボクセル空間に目標対象物Ｍの位置を中心として予め定めた目標対象物Ｍの大きさと略同じ大きさの円柱モデル（例えば、監視対象の目標対象物Ｍを侵入者であるとしたとき、底面の半径０．３ｍ、高さ１．７ｍの円柱モデル）を配置し、当該円柱モデルと干渉するボクセルを占有ボクセルとして設定することによりボクセル情報８３を更新する。後述するように、自律飛行ロボット１は、占有ボクセルには移動しないように飛行制御されるが、上記のように目標対象物Ｍの位置に基づいてボクセル情報８３を更新することにより、自律飛行ロボット１と目標対象物Ｍとの接触を回避することができる。

速度推定手段７２は、後述する移動制御手段７４における移動制御で利用するため、自律飛行ロボット１の現在の飛行速度（ｖ_x，ｖ_y，ｖ_z，ｖ_yaw）を推定する処理を行う。本実施形態では、位置推定手段７１にて推定した自己位置（座標：Ｘ_s，Ｙ_s，Ｚ_s及び姿勢ＹＡＷ）の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。この他にも、ＧＮＳＳにおけるドップラー効果を利用した速度推定方法を用いてもよい。

経路探索手段７３は、自律飛行ロボットの現在位置（自己位置）から所定の移動目標位置に至る移動経路を算出する処理を行う。特に、本実施形態における経路探索手段７３は、移動経路を算出する際、風速情報８２に基づいて風速が大きいほど占有ボクセル（飛行禁止エリア）からより遠くに離れた位置に移動経路を生成する。

まず、経路探索手段７３は、記憶部８に記憶された風速情報８２に基づいてボクセル情報８３を更新するボクセル情報更新処理を行う。以下、ボクセル情報更新処理の詳細について説明する。ボクセル情報更新処理では、移動空間における風速情報８２の風速値が大きいほど、近接ボクセルの範囲が大きく（広く）設定されるように、ボクセルコスト値を算出する。なお、本実施形態におけるボクセル情報更新処理では、自己位置に最も近い位置に設置された風速計１０５の風速値を利用することとして以下で説明する。自律飛行ロボット１自体が風速計１０５を搭載している他の実施形態においては、当該搭載している風速計１０５の風速値をボクセル情報更新処理にて利用することとし、搭載している風速計１０５が故障等によって利用できないときは、自己位置に近い位置に設置された風速計１０５の風速値を利用することとする。

本実施形態では、経路探索手段７３は、風速情報８２から風速値を読み出し、式（１）におけるλの値を、風速が大きいほど小さな値となるように更新し、更新した当該λの値を用いて、式（１）を用いて同様に各ボクセルのボクセルコスト値を算出するものとする。例えば、風速１０ｍの時はλ＝０．１、風速８ｍの時はλ＝０．１５、風速６ｍの時はλ＝０．２などといったように、予め風速の大きさが大きくなるほどλの値が小さくなるよう風速に対応付けてλの値を記憶しておき、ボクセル情報更新処理の実行時における風速値に基づいて計算に用いるλの値を読み出して、式（１）によりボクセルコストを計算する。このようにして各ボクセルについて求めたボクセルコスト値によってボクセル情報８３を更新する。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルのボクセル属性を「近接ボクセル」、当該閾値未満のボクセルコスト値を有するボクセルのボクセル属性を「自由ボクセル」としてボクセル情報８３を更新する。

図５は、ボクセル情報更新処理によって再設定されたボクセル情報８３を説明する図であり、時刻ｔのときにボクセルで表示した移動空間の一部を真上から見下ろしたときの図について表している。図５において、符号Ｂ１で表わした黒色で塗られた領域は「占有ボクセル」を示し、例えば、建造物などを構成する物体が占有する空間である。また、符号Ｂ２で表わしたハンチングで塗られた領域は「近接ボクセル」であり、符号Ｂ３で表わした白色の領域（何も塗られていない領域）は「自由ボクセル」である。図５（ａ）は、風速が小さい（弱風）のときに設定されたボクセル情報８３を表しており、図５（ｂ）は、風速が大きい（強風）のときに設定されたボクセル情報８３を表している。図５に表すように、移動空間における風速が大きくなるほど、占有ボクセルから離れた位置にあるボクセルが、（自由ボクセルではなく）近接ボクセルとして設定されることになる。

なお、ボクセル情報更新処理は、経路探索手段７３によって移動経路を生成する際（後述する移動経路生成処理を実行する際）に、その都度実行してもよいし、所定期間毎に実行してもよいし、風速が所定範囲以上変化する度に実行してもよい。或いは、複数通りの風速値における全ボクセルのボクセルコスト値を式（１）によって予め算出した上で、風速値に対応付けて記憶しておき、ボクセル情報更新処理の実行時における風速値に対応する全ボクセルのボクセルコスト値を読み出して更新してもよい。

続いて、経路探索手段７３は、位置推定手段７１で推定された自己位置及び目標対象物位置８１と、記憶部８に記憶された各種情報とを用いて自律飛行ロボット１の移動経路を算出する移動経路生成処理を行う。以下に、移動経路生成処理の詳細について説明する。

移動経路生成処理では、まず、目標対象物位置８１から予め設定した離間距離（３ｍ）だけ水平方向に離れた位置であって予め設定された高度（３ｍ）の位置に移動目標位置を設定する処理を行う。この際、経路探索手段７３は、ボクセル情報８３を参照し、占有ボクセル又は近接ボクセルに含まれる位置には移動目標位置を設定しないようにする。なお、（目標対象物位置８１の時間変化により求めた）目標対象物Ｍの移動方向を目標対象物Ｍの正面方向として定め、当該正面方向に近い位置に移動目標位置を設定することが好適である。

次に移動経路生成処理では、設定された移動目標位置Ｑと、記憶部８に記憶されたボクセル情報８３及び移動空間グラフ情報８４と、位置推定手段７１にて推定された自己位置と、を用いて自律飛行ロボット１の移動経路を算出する。この際、空間情報８２及び移動空間グラフ情報８４を参照し、移動目標位置に対応するボクセルのノード（以下「スタートノード」という）から自己位置に対応するボクセルのノード（以下「ゴールノード」という）に至る総コスト値Ｃが最も小さくなる経路をＡ＊経路探索法により探索する。

Ａ＊経路探索法では、ある評価ノードｎ（ノードＩＤがｎのノード）における総コスト値Ｃ（ｎ）＝ｇ（ｎ）＋ｈ（ｎ）…式（２）で表わす。

ここで、ｇ（ｎ）は、評価ノードｎについてのＡ＊経路探索法におけるｇコスト値であり、本実施形態では、スタートノードから評価ノードｎに至るまでの移動距離や障害物への接触危険性を考慮したコスト値として求める。すなわち、評価ノードｎのｇコスト値であるｇ（ｎ）は、隣接ノードにおけるｇコスト値であるｇ（ｎ−１）と、当該隣接ノードから評価ノードに至る距離に基づいて設定された距離コストＣＭ、評価ノードにおけるボクセルコストＣｖ（ｎ）との和とし、ｇ（ｎ）＝ｇ（ｎ−１）＋ＣＭ＋Ｃｖ（ｎ）により求める。また、ｈ（ｎ）は、評価ノードｎについてのＡ＊経路探索法におけるｈコスト値であり、評価ノードｎからゴールノードに至る距離の推定コスト値である。本実施形態では、評価ノードｎからゴールノードに至る直線距離によりｈ（ｎ）を求める。Ａ＊経路探索法では、スタートノードから順に隣接ノードの総コスト値Ｃ（ｎ）を算出していくことを繰り返し、最終的にゴールノードに至る総コスト値Ｃ（ｎ）が最も小さくなる経路を探索していく。

以下に、Ａ＊経路探索法による移動経路の生成について簡単に説明する。Ａ＊経路探索法では、まず、スタートノードに隣接する一又は複数のノード（隣接ノード）を評価ノードとして設定するところから開始する。この際、ボクセル情報８３のボクセル属性を参照し、占有ボクセル又は近接ボクセルに相当する位置にある隣接ノードは評価ノードから除外し、占有ボクセル又は近接ボクセルに移動経路が生成されないようにする。そして、それぞれの評価ノードについて式（２）により総コスト値を求める。次に、評価ノードの中で最小の総コスト値となるノードを注目ノードと称したとき、当該注目ノードの（これまで評価ノードと設定されていない新たな）隣接ノードを新たに評価ノードとして追加し、新たに追加された評価ノードに対しても同様に総コスト値を求める。続いて、同様に全ての評価ノードの中で最小の総コスト値となる評価ノードを注目ノードに再設定する。このように、評価ノードの総コスト値に基づいた注目ノードの再設定と、当該再設定に伴う新たな評価ノードの追加及び総コスト値の計算を繰り返し、最終的にゴールノードが注目ノードとして設定されたとき、経路の探索は終了する。

このようにして、起点となるスタートノードから終点となるゴールノードに至る最良の移動経路が生成される。経路探索手段７３で生成された移動経路のデータは、経由点となるノードの位置（ｘ，ｙ，ｚ）の集合データであり、この情報は記憶部８に一時的に記憶される。図６は、風速に応じて生成された移動経路の違いについて説明する図であり、図６（ａ）は、風速が小さい（弱風）のときに設定された移動経路を表しており、図６（ｂ）は、風速が大きい（強風）のときに設定された移動経路を表している。このように、経路探索手段７３は、風速が大きいほど占有ボクセル（飛行禁止エリア）からより遠くに離れた位置に移動経路を生成する。

なお、経路の探索方法は、Ａ＊経路探索法に限定されるものでなく、ダイクストラ法等の他の経路探索方法を適用してもよい。

移動制御手段７４は、経路探索手段７３にて算出された移動経路と位置推定手段７１にて推定された自己位置と速度推定手段７２で推定された飛行速度とを用いて、自律飛行ロボット１が経路探索手段７３で算出された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である制御指令値を求め、当該制御指令値に基づいてモータ４を制御し、ロータ２の回転数を制御する。

経路追従制御では、まず、各時刻での自律飛行ロボット１が目標とすべき直近の位置（以下、「ローカル目標」と呼ぶ）を算出する処理を行う。図７はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段７４は、経路探索手段７３で生成された移動経路を記憶部８から読出し、自律飛行ロボット１が現在時刻で目指している経由点Ｗｐ１と前回通過済みである経由点Ｗｐ０との２点間を繋げた直線Ｗを求める。そして、移動制御手段７４は、求めた直線Ｗと自律飛行ロボット１の自己位置を中心とした球Ｓとの交点Ｌｐ’、Ｌｐを算出し、目指している経由点Ｗｐ１に近い交点Ｌｐをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律飛行ロボット１が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Ｐｏに向かって移動していき、自律飛行ロボット１は移動経路に沿って飛行していくことになる。

次に、経路追従制御では、算出したローカル目標に向かって飛行するようＸ、Ｙ、Ｚ、ヨー角の各方向毎に制御指令値ｕ_x，ｕ_y，ｕ_z，ｕ_ψを算出する処理を行う。この際、現在の自己位置とローカル目標の位置との差異が小さくなるような制御指令値を求める。具体的には、ＸＹＺ軸方向の制御指令値ｕ＝（ｕ_x，ｕ_y，ｕ_z）は、位置推定手段７１で求められた自己位置ｒ＝（Ｘ_s，Ｙ_s，Ｚ_s）と速度推定手段７２で推定した速度ｖ＝（ｖ_x，ｖ_y，ｖ_z）とを利用し、ＰＩＤ制御により求める。ＸＹＺ軸方向の各制御指令値をｕ＝（ｕ_x，ｕ_y，ｕ_z）、ローカル目標をｒ’＝（ｘ，ｙ，ｚ）としたとき、制御指令値は、ｕ＝Ｋp（ｒ’−ｒ）＋Ｋd・ｖ＋Ｋi・ｅの式で算出される。ここで、Ｋp、Ｋd、ＫiはそれぞれＰＩＤ制御のゲインのことであり、ｅ＝（ｅ_x，ｅ_y，ｅ_z）は誤差の積分値である。一方、ヨー角方向の制御指令値ｕ_ψは、ψ'を目標角度、ψを位置推定手段７１にて推定した自律飛行ロボット１の姿勢（角度）、ｖ_yawを速度推定手段７２で推定した角速度とすると、ｕ_ψ＝Ｋp（ψ’−ψ）＋Ｋd・ｖ_yawの式のようなＰＤ制御により求める。なお、本実施形態では、目標角度ψ'を目標対象物Ｍの方向、すなわち、目標対象物Ｍの位置の方向を向く角度とした。

このように、制御部７は、上述した位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４における各処理を逐次繰り返す。これにより、本実施形態の自律飛行ロボット１は、目標対象物Ｍから離間距離だけ離れた位置に対して移動目標位置を逐次更新し、その都度移動経路についても逐次更新していくことによって、目標対象物Ｍを撮像しながら追従移動することができる。特に本実施形態における自律飛行ロボット１は、風速が大きいほど占有ボクセル（飛行禁止エリア）から大きく離れるように移動経路を生成することにより、風による障害物への衝突する危険性を少なくすることができる。

ところで、本発明は、上記実施形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施形態で実施されてもよいものである。また、実施形態に記載した効果は、これに限定されるものではない。

上記実施形態におけるボクセル情報更新処理では、風速情報８２に基づいて式（１）におけるλの値を変更して、各ボクセルのボクセルコストを算出した上で、閾値判定によりボクセル属性（近接ボクセル／自由ボクセル）を判定している。しかし、これに限定されるものではなく、風速が大きくなるほど占有ボクセル（飛行禁止エリア）からより遠くに離れた位置に移動経路が生成されるよう、他の方法を用いてボクセル情報８３を更新してもよい。例えば、全てのボクセルについての無風時におけるボクセルコストを式（１）によって予め算出してボクセル情報８３として記憶し、風速の大きさが大きくなるほど大きい値となる補正コスト値を別途算出して、予め記憶している無風時のボクセルコスト値に当該補正コスト値を加算することによりボクセルコストを算出し、ボクセル情報８３を更新してもよい。そして、当該ボクセルコスト値を閾値判定することによりボクセル属性を判定してもよい。

また、ボクセル情報更新処理では、ボクセルコストを更新せずに（一定のまま）、ボクセル属性のみを更新してもよい。例えば、風速の大きさに応じて近接ボクセルと判定するための閾値を変更し、当該変更した閾値を用いて各ボクセルが近接ボクセルであるか否かを判定し、ボクセル情報８３のボクセル属性を更新してもよい。すなわち、風速の大きさが大きいほど、近接ボクセルと判定するための閾値を小さくなるように設定することにより、あるボクセルのボクセルコスト値が風速に応じて変わらなくても、風速が大きくなるほど近接ボクセルとして設定され易くなる。他にも、風速が大きくなるほど大きな値となる離間距離を求め、あるボクセル（評価ボクセル）から当該離間距離の範囲内に占有ボクセルが存在していたときは、当該評価ボクセルのボクセル属性を近接ボクセルとして設定してもよい。この際、例えば、離間距離＝α×風速値により求めてもよい（ここでαは実験により求めたパラメータとする）。

また、上記実施形態におけるボクセル情報更新処理では、風速の大きさからなる風速情報８２に基づいてボクセル情報８３を更新している。しかし、これに限定されるものではなく、風速だけでなく風向も測定可能な風速計１０５を用い、当該風速計１０５によって出力された風速・風向に基づいてボクセル情報８３を更新してもよい。すなわち、風速計１０５にて測定された風向を参照して、風速が大きいほど風向の風下に位置する飛行禁止エリアからより遠くに離れた位置に移動経路を生成する。具体的には、生成される移動経路から見て風上側に位置する飛行禁止エリアからよりも、生成される移動経路から見て風下側に位置する飛行禁止エリアからの方が、より遠くに離れるような位置となるように移動経路を生成するものとする。例えば、あるボクセル（評価ボクセル）のボクセルコストを求めるのに際し、まず、評価ボクセルから占有ボクセルに向かう直線方向と風向とがなす角δが小さいほど、式（１）におけるλの値が小さくなるように設定する。例えば、式（１）におけるλ値を、λ＝λ₀−β×ｃｏｓδ（０°≦δ＜９０°）…式（３）により求めることができる。ここで、λ₀は無風時におけるλ値として予め設定された値であり、βは風速の大きさが大きくなるほど大きな値となる値であって風速に対応付けて予め設定された値である。なお、式（３）においてδ≧９０°の場合はλ＝λ₀を用いるものとする。また、風向を考慮してボクセルコストを算出する場合、式（１）及び式（３）で対象とする占有ボクセルを、評価ボクセルに最も近い占有ボクセルではなく、評価ボクセルの所定範囲内の周囲に存在する占有ボクセルについて其々ボクセルコストを算出し、求めた各ボクセルコストの中で最も大きい値のボクセルコストを採用するのが好適である。図８は、式（１）及び式（３）により風速・風向に基づいてボクセル情報８３を更新した例を表す図である。図８（ａ）は、移動空間に東風が発生していた時のボクセル情報８３の例を表しており、図８（ｂ）は、西風が発生していた時のボクセル情報８３の例を表しており、図８（ａ）に表すように、東風が発生している場合、評価ボクセルから見て当該風向（東風）と同じ方向（東側）に存在する占有ボクセルの影響を強く受ける。したがって、占有ボクセルの西側に位置するボクセルの多くが近接ボクセルとして設定されることになる。一方、図８（ｂ）に表すように、西風が発生している場合は、占有ボクセルの東側に位置するボクセルの多くが近接ボクセルとして設定されることになる。このように風速だけでなく風向も考慮してボクセルコストを算出することにより、経路探索手段７３は、風速が大きいほど、風向と逆の方向（風上）にある占有ボクセルからよりも、風向と同じ方向（風下）にある占有ボクセルから大きく（より遠くに）離れるように移動経路を生成する。これにより、自律飛行ロボット１は、移動自由度を確保しつつ、より安全に飛行することが可能となる。なお式（３）におけるλ₀は、風速に関係なく固定値としているが、当該λ₀についても風速に応じて小さくなるように更新してもよい。

また、上記実施形態におけるボクセル情報更新処理では、ボクセル情報更新処理の実行時（現在）における風速の大きさに基づいて、式（１）の計算に用いるλの値を決定している。しかし、これに限定されるものではなく、現在から過去の所定期間（例えば、１０分間）における最大瞬間風速に基づいてλの値を決定してもよい。すなわち、現在から過去の所定期間における最大瞬間風速値が大きいほど、式（１）の計算に用いるλの値が小さくなるように決定する。したがって、経路探索手段７３は、現在から過去の所定期間における最大瞬間風速値が大きいほど、占有ボクセル（飛行禁止エリア）から大きく離れるような移動経路を生成するようになる。これにより、一時的に風が弱くなったタイミングで設定されたボクセルコスト（及びボクセル属性）に基づいて移動経路が生成され、再び風が強くなることによって障害物に衝突する危険性を低下させることができる。

また、上記各実施形態におけるボクセル情報更新処理では、風速（及び風向）に基づいてボクセル情報８３を更新している。しかし、これに限定されるものではなく、現在から過去の所定期間内における最大瞬間風速と平均風速との比から突風率（突風率＝最大瞬間風速÷平均風速）を更に求め、突風率も考慮してボクセル情報８３を更新してもよい。すなわち、突風率が大きいほど式（１）の計算に用いるλの値が小さくなるように決定する。或いは、突風率が大きいほど各ボクセルのボクセルコストが大きくなるように補正したり、近接ボクセルを判定する閾値が小さくなるように補正した上で、ボクセル属性を更新してもよい。このようにしてボクセル情報８３を更新することにより、経路探索手段７３は、突風率が大きいほど、占有ボクセル（飛行禁止エリア）から大きく離れるような移動経路を生成するようになる。したがって、突風の影響を考慮して移動経路を生成することができ、突風により障害物に衝突する危険性を低下させることができる。

また、上記各実施形態における移動経路生成処理では、Ａ＊経路探索法によって移動経路を生成する際、ボクセル情報８３のボクセル属性を参照することで近接ボクセルに移動経路が生成されないよう処理している。しかし、これに限定されるものではなく、ボクセル属性を参照せずにボクセルコストのみを用いて移動経路を生成してもよい。すなわち、この場合においても、風速に基づいてボクセルコストを更新することにより、風速が大きいほど占有ボクセル（飛行禁止エリア）から大きく離れるよう移動経路を生成されることに変わりない。

また、上記実施の形態では、管理装置１０２に接続された目標対象物検出センサ１０４を用いて目標対象物Ｍを検出している。しかしながら、これに限定されるものではなく、撮像部３で取得した撮像画像を画像解析することにより、目標対象物Ｍの位置を推定してもよい。例えば、撮像画像の各フレームを画像処理して目標対象物Ｍの画像領域を抽出する処理を行う。この際、既知の従来技術（特開２００６−１４６５５１号公報を参照）であるオプティカルフロー法、ブースティング学習（例えば、Ｈａａｒ−ｌｉｋｅ特徴を用いているＡｄａＢｏｏｓｔベース識別器による顔検出手法）による識別器等を用いて目標対象物Ｍの画像領域（人物領域）を抽出する。次に、当該抽出された画像領域の位置に基づいて目標対象物Ｍと自律飛行ロボット１との距離を推定する。具体的には、抽出した目標対象物Ｍの画像領域の頭頂部の（撮像画像における）ｙ座標位置と距離との対応表を予め飛行高度毎に作成しておき、現在の飛行高度及び目標対象物Ｍの頭頂部のｙ座標位置を当該対応表に照らし合わせて自律飛行ロボット１との距離を推定する。しかし、これに限らず、抽出した目標対象物Ｍの頭部の大きさから距離を算出してもよい。すなわち、頭部の大きさと距離との対応表を予め作成しておき、抽出された目標対象物Ｍの頭部の大きさを当該対応表に照らし合わせて自律飛行ロボット１との距離を推定してもよい。

また、自律飛行ロボット１に撮像部３としてカラーカメラの代わりに距離画像センサを搭載して、当該距離画像センサから取得した距離画像を用いて、既知の移動物体抽出技術により目標対象物Ｍを抽出して、抽出した目標対象物Ｍと自律飛行ロボット１との距離値と自己位置とから目標対象物Ｍの位置を推定してもよい。また、自律飛行ロボット１にレーザスキャナを搭載し、当該レーザスキャナの出力値と自己位置とを用いて目標対象物Ｍの位置を推定してもよい。

また、上記実施の形態では、制御部７において位置推定処理、速度推定処理、ボクセル情報更新処理、移動経路生成処理、移動制御処理（経路追従制御）の一連の処理を行っている。しかし、これに限らず、図示しない制御用のＰＣを用意し、当該ＰＣにこれらの一連の処理を実施させてもよい。すなわち、自律飛行ロボット１は、ＰＣによって行われた位置推定処理、速度推定処理、ボクセル情報更新処理、移動経路生成処理、移動制御処理によって得られた制御指令値を無線通信又は有線通信によりＰＣから受信し、当該制御指令値に基づいてモータ４の回転数を制御することにより、目的の位置に飛行するようにしてもよい。このように、外部ＰＣを用いて上記の一連の処理を分担することにより、自律飛行ロボット１のＣＰＵ処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。

１自律飛行ロボット、２（２ａ〜２ｄ）ロータ、３撮像部、４（４ａ〜４ｄ）モータ、５距離検出センサ、６位置検出センサ、７制御部、８記憶部、９通信部、７１位置推定手段、７２速度推定手段、７３経路探索手段、７４移動制御手段、８１目標対象物位置、８２風速情報、８３ボクセル情報、８４移動空間グラフ情報、８５各種パラメータ、１００警備センタ、１０２管理装置、１０４目標対象物検出センサ、１０５風速計、１１０情報通信網。

Claims

移動空間内を自律飛行する自律飛行ロボットであって、
前記移動空間内を複数のボクセルに分割し、前記ボクセルの各々について飛行禁止エリアか否かを示す属性及び自律移動ロボットの移動の障害となる度合いを示すボクセルコスト値を関連付けて記憶した記憶部と、
前記移動空間における前記自律飛行ロボットの自己位置を推定する位置推定手段と、
前記移動空間における風速を計測する風測定手段と、
前記属性および前記ボクセルコスト値に基づいて、前記飛行禁止エリアを通過せずに移動目標位置に到達するまでに通過する前記ボクセルの前記ボクセルコスト値を加算した総コスト値が最も小さくなる移動経路を探索する経路探索手段と、
前記移動経路に沿って移動するよう制御する移動制御手段と、を有し、
前記経路探索手段は、前記飛行禁止エリアの近傍に前記風速が大きいほど広い範囲に存在する前記ボクセルの前記ボクセルコスト値を大きな値に更新して前記移動経路を探索することを特徴とする自律飛行ロボット。
前記風測定手段は、前記移動空間における風向を更に測定し、
前記経路探索手段は、前記風向を参照し、前記範囲に存在するボクセルのうちの風上側よりも風下側のボクセルの前記ボクセルコスト値を大きな値に更新する請求項１に記載の自律飛行ロボット。
前記経路探索手段は、現在から過去の所定期間における最大瞬間風速が大きいほど前記範囲に存在するボクセルの前記ボクセルコスト値を大きな値に更新する請求項１又は請求項２に記載の自律飛行ロボット。
前記経路探索手段は、現在から過去の所定期間内における最大瞬間風速と該所定期間における平均風速との比から突風率を求め、該突風率が大きくなるほど前記範囲に存在するボクセルの前記ボクセルコスト値を大きな値に更新する請求項１から請求項３の何れか一項に記載の自律飛行ロボット。