JP2017182691A - 自律飛行ロボット - Google Patents

Abstract

【課題】飛行制御ができなくなる振動状態を迅速に解消して安全を確保すること。【解決手段】移動空間内を自律飛行する自律飛行ロボットであって、現在時刻から所定の過去期間における飛行速度の変化履歴に基づいて飛行速度が符号反転を繰り返している振動状態を検出し、前記振動状態を検出したとき、飛行速度が最小となるタイミングで飛行停止させる。【選択図】図４

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 (6)

1. 移動空間内を自律飛行する自律飛行ロボットであって、
   前記自律飛行ロボットの飛行状態を計測した計測値から飛行速度を推定する速度推定手段と、
   現在時刻から所定時間前までの過去期間における前記飛行速度の変化履歴に基づいて該飛行速度が符号反転を繰り返している振動状態を検出する振動状態検出手段と、
   前記振動状態検出手段にて前記振動状態を検出したとき、前記飛行速度が最小となるタイミングで飛行停止させる飛行制御手段と、
   を有することを特徴とする自律飛行ロボット。
2. 前記振動状態検出手段は、前記過去期間における前記飛行速度の符号が所定の回数閾値以上変化しているとき、前記振動状態と判定する請求項１に記載の自律飛行ロボット。
3. 前記振動状態検出手段は、現在時刻の直近における前記飛行速度の振幅値が大きいほど長い前記過去期間を設定する請求項１又は請求項２の何れか一項に記載の自律飛行ロボット。
4. 前記振動状態検出手段は、前記過去期間において前記飛行速度の符号が前記所定回数以上変化し、該振幅値が時間の経過と共に減少傾向であるとき、現在時刻から所定の判定時間の経過後における前記振幅値が所定の振幅閾値よりも大きいならば前記振動状態であると判定する請求項１〜３の何れか一項に記載の自律飛行ロボット。
5. 前記飛行空間内に存在する障害物の位置を示す障害物情報を記憶し、
   前記自律飛行ロボットの自己位置を推定する自己位置推定部を更に備え、
   前記飛行制御部は、前記障害物情報を用いて前記自己位置の下方に障害物が存在しないとき前記飛行停止を実行する請求項１〜４の何れか一項に記載の自律飛行ロボット。
6. 前記飛行空間内に移動目標位置を設定し、自己位置から当該移動目標位置に至る飛行経路を求める経路探索手段を更に有し、
   前記飛行制御手段は、前記飛行経路に沿って飛行するよう制御し、
   振動状態検出手段は、前記移動目標位置の変化の大きさに応じて前記回数閾値又は前記振幅閾値の少なくとも一方を変更する請求項１〜５の何れか一項に記載の自律飛行ロボット。
   
   
   

Images