本発明の実施の形態における自律飛行ロボット1は、図1の概観図に示すように、クアッドロータ型の小型無人ヘリコプタである。なお、本発明の適用範囲は、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタについても同様に適用することができる。
自律飛行ロボット1は、図2のシステム構成図に示すように、外部の警備センタ100や管理装置102と通信し、移動空間内に存在する所定の移動物体を目標対象物Mとして追跡し、当該目標対象物Mに対して所定の機能を発揮するように構成されている。目標対象物Mとなる移動物体は、例えば、監視領域内に侵入した人物(賊等)である。本実施の形態では、所定の機能として、目標対象物Mを撮像する機能を例に説明するが、特に限定されるものではなく、目標対象物Mに対して音を発したり、発光による警告を行ったりする等の機能であってもよい。
警備センタ100と管理装置102とはインターネット等の情報通信網110を介して情報伝達可能に接続される。また、自律飛行ロボット1と管理装置102は、無線通信等によって情報伝達可能に接続される。警備センタ100は、管理装置102を介して自律飛行ロボット1と通信を行い、自律飛行ロボット1によって撮像された目標対象物Mの撮像画像を受信する。警備センタ100は、撮像画像に対して画像処理を行い、警備センタ100にて異常監視している管理者等(図示しない)に警告を発するような機能を備えていてもよい。また、管理装置102から目標対象物Mの位置(座標)に関する情報を受信し、当該目標対象物Mと自律飛行ロボット1によって撮像された撮像画像とを関連付けて管理するというような機能を備えてもよい。
管理装置102は、地面や壁面等に設置された固定型の移動物体検出センサ104(104a,104b・・・)を備え、各目標対象物Mの位置を検知する。移動物体検出センサ104は、例えば、レーザセンサとすることができる。レーザセンサは、一定の角度サンプル間隔の角度毎にレーザを二次元的にスキャンすることによって、地面(又は床面)から一定の高さの水平面における検知範囲内に存在する物体(障害物)との距離情報を極座標値として取得する。レーザセンサは、放射状にレーザ光である探査信号を走査し、物体に反射して戻ってきた探査信号を受信して、送信と受信の時間差から物体までの距離を算出し、レーザセンサの設置位置の座標及び探査信号を送信した方向と算出した距離から当該物体の位置の極座標値を求め、当該極座標値から3次元の直交座標値(Xt,Yt,Zt)を求める。管理装置102は、移動物体検出センサ104によって求められた目標対象物Mの位置を自律飛行ロボット1へ送信する。自律飛行ロボット1は、目標対象物Mの位置を受信すると、その位置に基づいて自らの移動経路を算出し、当該移動経路に沿って移動する。なお、管理装置102は、レーザセンサの検知範囲が重複する領域に存在する目標対象物Mの同一性を検証することで複数のレーザセンサの検知範囲に渡る目標対象物Mの追跡を行う。すなわち、レーザセンサ104aの検知範囲に存在する目標対象物Mがレーザセンサ104bの検知範囲に移動したとしても、管理装置102は、当該目標対象物Mが同一の物体であると判定することができる。
以下、図1の概観図及び図3の機能ブロック図を参照して、自律飛行ロボット1の構成及び機能について説明する。
自律飛行ロボット1は、図1に示すように、4枚のロータ(プロペラ)2(2a〜2d)を一平面上に有する。各ロータ2は、バッテリ(二次電池:図示しない)により駆動されるモータ4(4a〜4d)を用いて回転させられる。一般的に、シングルロータ型のヘリコプタでは、メインロータによって発生する反トルクをテールロータが生み出すモーメントで相殺することによって方位角を保っている。一方、自律飛行ロボット1のようなクアッドロータ型のヘリコプタでは、前後・左右で異なる方向に回転するロータ2を用いることで反トルクの相殺を行っている。そして、各ロータ2の回転数(fa〜fd)を制御することにより、様々な機体の移動や姿勢の調節を行うことができる。例えば、機体をヨー方向に回転させたいときは、前後のロータ2a、2cと左右ロータ2d、2bの回転数に差を与えればよい。
撮像部3は、例えばレンズなどの光学系および所定画素数(例えば640×480画素からなる解像度)のCCDやCMOSなどの2次元アレイ素子を有する二次元イメージセンサで構成され、移動空間の撮像画像を所定の時間間隔で取得するいわゆるカラーカメラである。本実施の形態では、撮像部3は、その光軸が自律飛行ロボット1の正面方向を撮像するよう筐体部分に設置され、かつ、水平面(XY平面)から予め定めた俯角θにより斜め下方の空間を視野角φ(画角)において撮像するよう設置されている。取得した撮像画像は後述する制御部7に出力され、制御部7により記憶部8に記憶されたり、後述する通信部9を介して管理装置102に送信されたりする。
距離検出センサ5は、自律飛行ロボット1の周囲に存在する障害物と自律飛行ロボット1との間の距離を検出し、センサ検出範囲内に存在する障害物の相対的な位置を取得するセンサである。本実施の形態では、距離検出センサ5としてマイクロ波センサを備える。マイクロ波センサは、空間にマイクロ波を放出し、その反射波を検知することによって、自律飛行ロボット1の周囲にある物体を探知し、その物体までの距離を求める。距離検出センサ5は、例えば、自律飛行ロボット1の前方に向けて設け、目標対象物Mまでの距離を測定するために用いることができる。また、距離検出センサ5は、例えば、自律飛行ロボット1の下部に下向きに設け、地面との距離(高度)を測定するために用いることもできる。また、距離検出センサ5は、例えば、撮像部3の光軸方向に向けて設け、撮像部3の撮像対象物である目標対象物Mまでの距離を測定するために用いることができる。
位置検出センサ6は、自律飛行ロボット1の現在位置を取得するためのセンサである。位置検出センサ6は、例えば、GNSS(Global Navigation Satellite System)等の航法衛星から送信される電波(航法信号)を受信する。位置検出センサ6は、複数の航法衛星から送信される航法信号を受信して制御部7へ入力する。なお、位置検出センサ6は、レーザスキャナ、ジャイロセンサ、電子コンパス、気圧センサ等の他のセンサを用いて既知の技術により自己位置を得るための情報を取得するものとしてもよい。
通信部9は管理装置102との間で、例えば無線LANや携帯電話回線等により無線通信するための通信モジュールである。本実施の形態では、撮像部3によって取得した撮像画像を通信部9により管理装置102に送信し、当該撮像画像を管理装置102から警備センタ100に送信することにより、警備員等が遠隔から侵入者を監視することを可能にする。また、通信部9は、管理装置102から目標対象物Mの位置(座標:Xt,Yt,Zt)を受信することにより、後述するような移動経路の設定を可能にする。
記憶部8は、ROM(Read Only Memory)、RAM(Random Access Memory)、HDD(Hard Disk Drive)等の情報記憶装置である。記憶部8は、各種プログラムや各種データを記憶し、制御部7との間でこれらの情報を入出力する。各種データには、目標対象物位置81、変化履歴情報82、ボクセル情報83、各種パラメータ84等の制御部7の各処理に用いられる情報、各センサ等の出力値及び撮像画像等が含まれる。
目標対象物位置81は、管理装置102から受信した目標対象物Mの位置情報(座標:Xt,Yt,Zt)である。本実施の形態では、目標対象物位置81を目標対象物Mの足元位置、すなわち目標対象物Mが地面(床面)に接している位置とする。制御部7は、通信部9を介して目標対象物Mの位置を受信すると目標対象物位置81として記憶部8に記憶させる。なお、管理装置102は、目標対象物Mの位置(座標:Xt,Yt,Zt)の時間変化に基づいて目標対象物Mの姿勢(例えば、目標対象物Mの正面方向を示した情報)を推定し、目標対象物位置81として記憶してもよい。
変化履歴情報82は、自律飛行ロボット1の飛行速度と当該飛行速度が推定された時刻とを対応付けた飛行速度の過去から現在までの変化履歴を表す情報である。本実施の形態では、後述する速度推定手段72にて飛行速度を推定する度に、当該飛行速度と推定時刻とを対応付けて変化履歴情報82として記憶部8に記憶されるものとする。
ボクセル情報83は、移動空間をボクセル空間として複数のボクセルに分割して移動空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部8に記憶され、また、後述するように位置推定手段71にて更新される情報である。本実施の形態では、移動空間を所定の大きさ(例えば15cm×15cm×15cm)のボクセルに等分割し、各ボクセルの識別子であるボクセルIDと、移動空間におけるボクセルの位置(三次元座標)と、ボクセル属性と、ボクセルコスト値とを対応付けてボクセル情報83として記憶する。飛行禁止エリアに相当するボクセルは、そのボクセル属性を「占有ボクセル」と定義して、自律飛行ロボット1が移動できない空間とする。なお飛行禁止エリアには、例えば建造物等の障害物に相当するエリア、飛行が許可された敷地外のエリア、飛行が許可された高度よりも高いエリアなどが挙げられる。そして、占有ボクセルの近くに存在する空間に位置するボクセルのボクセル属性を「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルのボクセル属性を「自由ボクセル」と定義する。
各ボクセルには、後述する経路探索手段73にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて登録される。ボクセルコスト値は、占有ボクセルにおいて最大値Cmaxをとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、あるボクセル(評価ボクセル)のボクセルコスト値は、ボクセルコスト値=Cmax × exp{−λ×R}の計算式により算出することが好適である。ここでλは実験によって求めたパラメータとし、Rは評価ボクセルに最も近い占有ボクセルからの距離とする。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とする。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を0と設定する。
また、記憶部8には、各種パラメータ84として離間距離等も記憶される。離間距離は、目標対象物Mに追従飛行するにあたって、自律飛行ロボット1と目標対象物Mとの水平面における維持すべき相対距離である。離間距離は、自律飛行ロボット1の管理者等によって予め設定される。自律飛行ロボット1を用いて所定の目標対象物Mを監視する場合、目標対象物Mに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Mから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施の形態の自律飛行ロボット1は、目標対象物Mの詳細な撮像画像を取得でき、かつ、当該目標対象物Mから攻撃を受け難い距離に離間距離を予め、当該離間距離を保ちつつ追従飛行するように制御される。離間距離は、例えば3mと設定される。離間距離の他にも、後述する回数閾値や振幅閾値などの振動状態を判定するための条件や式なども、各種パラメータ84として記憶部8に記憶される。
制御部7は、CPU等を備えたコンピュータで構成され、位置推定処理、速度推定処理、経路探索処理、振動状態検出処理、経路追従制御、飛行停止制御等を行う一連の処理として、位置推定手段71、速度推定手段72、経路探索手段73、移動制御手段74、振動状態検出手段75を含んでいる。
位置推定手段71は、位置検出センサ6の出力に基づいて、移動空間における自律飛行ロボット1の現在位置である自己位置を推定する位置推定処理を行う。
具体的には、位置検出センサ6から得られた複数の航法衛星からの信号に基づいて周知技術によって推定した緯度・経度と、距離検出センサ5から得られた高度とから自己位置の座標(Xs,Ys,Zs)を計算する。さらに、電子コンパスやジャイロセンサなどの位置検出センサ6からの出力を受けて自己位置として姿勢YAWを求める。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律飛行ロボット1の現在位置を推定してもよい。
位置推定手段71は、推定された自己位置(座標:Xs,Ys,Zs及び姿勢YAW)と管理装置102から受信した目標対象物Mの位置(座標:Xt,Yt,Zt)を経路探索手段73へ出力する。また、位置推定手段71は、推定された自己位置を速度推定手段72及び移動制御手段74へ出力する。
なお、位置推定手段71は、目標対象物Mの位置に基づいてボクセル情報83を更新する処理を行う。具体的には、記憶部8のボクセル情報83に基づいたボクセル空間に目標対象物Mの位置を重心として予め定めた目標対象物Mの大きさと略同じ大きさの円柱モデル(例えば、監視対象の目標対象物Mを侵入者であるとしたとき、底面の半径0.3m、高さ1.7mの円柱モデル)を配置し、当該円柱モデルと干渉するボクセルを占有ボクセルとして設定することによりボクセル情報83を更新する。後述するように、自律飛行ロボット1は、占有ボクセルには移動しないように飛行制御されるが、上記のように目標対象物Mの位置に基づいてボクセル情報83を更新することにより、自律飛行ロボット1と目標対象物Mとの接触を回避することができる。
速度推定手段72は、後述する移動制御手段74における移動制御で利用するため、自律飛行ロボット1の現在の飛行速度(vx,vy,vz,vyaw)を推定する速度推定処理を行う。本実施の形態では、位置推定手段71にて推定した自己位置(座標:Xs,Ys,Zs及び姿勢YAW)の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。また、推定した飛行速度は、大きさだけでなく速度の向きを考慮したベクトル値であり、向きに応じた正と負の符号を有している。例えば、北方向及び東方向への飛行速度を正とし、反対に南方向及び西方向への飛行速度を負として設定する。なお、本実施の形態では、自己位置の時間変化から飛行速度を求めているが、これに限らず、GNSSにおけるドップラー効果を利用した速度推定方法を用いてもよい。速度推定手段72は、飛行速度を推定すると、推定時刻と対応付けて変化履歴情報82として記憶部8に逐次記憶する。また、速度推定手段72は、推定した飛行速度を移動制御手段74へ出力する。
経路探索手段73は、位置推定手段71で推定された自己位置と、記憶部8に記憶された移動物***置81及び各種情報とを用いて、自己位置から所定の移動目標位置に至る移動経路を算出する経路探索処理を行う。
経路探索処理では、まず、目標対象物位置81から離間距離だけ離れた位置に移動候補位置を設定する処理を行う。すなわち、目標対象物位置81から離間距離だけ離れた周囲の領域に複数の移動候補位置を設定する。本実施の形態では、移動候補位置を、目標対象物位置81を中心として、予め設定された離間距離を半径とした円状の位置で、かつ、管理者等により予め定められた飛行高度(例えば3m)の領域に、それぞれ等間隔になるように設定する。
次に、経路探索処理では、設定した移動候補位置の其々を評価し、最も高い評価値となる移動候補位置を移動目標位置として設定する処理を行う。この際、まず、自律飛行ロボット1が障害物に接触することを防止するため、ボクセル情報83を参照して占有ボクセル又は近接ボクセルに含まれる位置には移動目標位置を設定しないよう、最低の評価値となるようにする。また、自律飛行ロボット1の自己位置から各移動候補位置までの距離が小さいほど評価値が高くなるようにする。更に、前回設定した移動目標位置から移動候補位置までの距離が小さいほど評価値が高くなるようにする。
次に、経路探索処理では、位置推定手段71で推定された自己位置と、設定した移動目標位置とを用いて自律飛行ロボット1の移動経路を算出する移動経路生成処理を行う。具体的には、移動経路生成処理では、まず、記憶部8からボクセル情報82を読出し、分割した移動可能な空間である自由ボクセル及び近接ボクセルの各ボクセルの中心をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしてグラフ構造を生成する。この際、エッジの重みとして、隣接するノード間の距離に基づいて求められる距離コストが設定されているものとする。このようにボクセル情報82に基づいて全ての移動可能なボクセルについてグラフ構造を生成した後、自律飛行ロボット1が存在しているボクセルのノードから移動目標位置が存在しているボクセルのノードまでの移動経路を生成する。移動経路の生成方法については、さまざまな経路生成方法が適用可能であるが、本実施の形態ではA*(エースター)経路探索法を用いて移動経路を探索する。この際、ボクセル情報82として記憶されたボクセルコストと距離コストとを、A*経路探索法における移動コストとして利用する。経路探索手段73で生成された生成された移動経路のデータは、経由点(x,y,z)の集合データであり、この情報は記憶部8に一時的に記憶される。
振動状態検出手段75は、速度推定手段72にて推定した飛行速度の過去の変化履歴を示した変化履歴情報82を用いて、現在の飛行状態が自律的な飛行制御が機能しない振動状態であるか否かを検出する振動状態検出処理を行う。振動状態検出処理では、現在時刻から所定時間前までの過去期間において飛行速度が符号反転を繰り返しているか否かに基づいて振動状態を検出する。以下、本実施の形態の自律飛行ロボット1に係る振動状態検出手段75が実行する振動状態検出処理の流れの一例について、図4を参照しながら詳細に説明する。なお、振動状態検出処理は、予め設定されたタイミング(例えば5秒毎)に実施されるものとする。
振動状態検出処理では、まず、振動状態であるか否かを判定するための期間である過去期間を設定する(ST1)。本実施の形態では、変化履歴情報82を参照して現在の直近における飛行速度の振幅値が大きいほど、長い過去期間を設定する。図5と図6は、飛行速度の変化をグラフによって表した図であり、図5は飛行速度の振幅値が増加傾向にある変化を示した図であり、図6は飛行速度の振幅値が減少傾向にある変化を示した図である。図5において、現在時刻がt’であるとき、振動状態検出手段75は、変化履歴情報82を参照して現在時刻から所定時間だけ過去の過去期間における飛行速度の変化履歴を読み出し、直近の飛行速度の振幅値a3を求める。そして、例えば、直近の飛行速度の振幅値a3に基づいて、過去期間の長さTを、T=α×a3+T0の計算式により算出することが好適である。ここでαは実験によって求めたパラメータとし、T0は基準となる過去期間の長さとして実験によって設定した期間長とする。図6では、現在時刻がt’’であるとき、直近の飛行速度の振幅値はb3であり、時刻t’の直近の振幅値a3よりも大きい振幅値であるため(b3>a3)、過去期間の長さT’は、過去期間の長さTよりも長い期間長(T’>T)に設定していることを表している。
次に、振動状態検出処理では、変化履歴情報82を参照し、現在時刻からST1にて設定した過去期間における飛行速度の符号の変化回数を計数し、当該符号の変化回数が予め設定した回数閾値以上であるか否かを判定する(ST2)。本実施の形態では、回数閾値を3回として以下を説明する。図5において、現在時刻がt’であるとき、振動状態検出手段75は、変化履歴情報82から過去期間Tにおける飛行速度の変化履歴を読み出す。そして、過去期間Tにおける飛行速度の符号の変化回数を計数する。図5では、過去期間Tの間に、飛行速度の符号が変化しているのはt1、t2、t3の3回であり、回数閾値以上であるため(ST2−Yes)、処理をST3へ進める。図6の場合も同様に、過去期間T’の間に、飛行速度の符号が変化しているのはt1’、t2’、t3’の3回であり、回数閾値以上であるため(ST2−Yes)、処理をST3へ進める。一方、飛行速度の符号の変化回数が回数閾値以上でないとき(ST2−No)、現在は振動状態ではないと判定して振動状態検出処理を終了する。
次に、振動状態検出処理では、ST2にて求めた振幅の平均値を求め、当該平均値が予め設定した振幅閾値以上であるか否かを判定する(ST3)。振幅閾値Th1としたとき、図5の例では、振幅の平均値AをA=(a1+a2+a3)/3により求め、求めた平均値AがTh1以上であるか否かを判定する。求めた平均値が予め設定した振幅閾値Th1以上であるとき(ST3−Yes)、処理をST4へ進める。一方、平均値が予め設定した振幅閾値以上でないとき(ST3−No)、現在は振動状態ではないと判定して振動状態検出処理を終了する。このように、過去期間における振幅値を閾値判定することにより、通常時の飛行制御においても生じうる速度が小さい振動移動を異常状態である振動状態として誤判定されないようにすることができる。なお、本実施の形態では、ST3にて、ST2にて求めた振幅の平均値Aと振幅閾値Th1との比較により上記判定を行っているが、これに限らず、ST2にて求めた振幅値が全て振幅閾値Th1以上であるか否かに基づいて上記判定を行ってもよい。
次に、振動状態検出処理では、過去期間における振幅値の変化が減少傾向にあるか否かを判定する(ST4)。図5の例では、過去期間Tにおける振幅値の変化は、a1、a2、a3と時間が進むにつれて大きくなっており、減少傾向ではないと判定されて(ST4−No)、処理をST7に進める。一方、図6の例では、過去期間T’における振幅値の変化は、b1、b2、b3と時間が進むにつれて小さくなっており、減少傾向であると判定されて(ST4−Yes)、処理をST5に進める。
ST4にて振幅値の変化が減少傾向であると判定されると(ST4−Yes)、振動状態検出処理では、予め設定した判定時間だけ処理を待機させる(ST5)。そして、判定時間が経過した直後の振幅値が振幅閾値Th1以上となっているか否かを判定する(ST6)。図6の例では、判定時間T’’が経過した時刻t’’’の直後の振幅値b6と振幅閾値Th1とを比較する。判定時間が経過した直後の振幅値が振幅閾値Th1以上となっていないとき(ST6−No)、現在は振動状態ではないと判定して振動状態検出処理を終了する。一方、判定時間が経過した時刻の直後の振幅値が振幅閾値Th1以上となっているとき(ST6−Yes)、処理をST7に進める。なお、本実施の形態では、ST6にて、判定時間が経過した直後の振幅値が振幅閾値Th1以上となっているか否かを判定しているが、これに限らず、判定時間内の振幅値の平均値が振幅閾値Th1以上となっているか否かに基づいて上記判定を行ってもよい。また、本実施の形態では、判定時間を予め設定した値としているが、これに限らず、所定回数(例えば3回)以上の振幅値が求められるまでの時間を判定時間としてもよい。例えば、図6の例では、現在時刻t=t’’から3回目の振幅値であるb6が求まるまでの時間を判定時間とし、当該b6と振幅閾値Th1とを比較して、上記判定を行ってもよい。
振動状態検出処理では、過去期間における振幅値の変化が減少傾向にないと判定されたとき(ST4−No)、又は、判定時間の経過後の振幅値が振幅閾値Th1以上となっているとき(ST6−Yes)、現在の飛行状態が自律的な飛行制御が機能しない振動状態であると判定する(ST7)。そして、振動状態であると判定すると、振動状態検出手段75は、振動状態である旨を示す情報を移動制御手段74に出力すると共に、変化履歴情報82に基づいて飛行速度(絶対値)が最小となるタイミング(すなわち、現在を基準として飛行速度が最小となる時間長)を求め、当該タイミングを表すタイミング値を移動制御手段74に出力する。例えば、現在の飛行速度がちょうどゼロであるとき、移動制御手段74にタイミング値として“0”を出力する。また、変化履歴情報82を参照して、飛行速度の変化の振幅値と周期(振動周波数)とに基づいて飛行速度(絶対値)が最小(例えば、飛行速度がゼロ)となる未来時刻を推測し、現在時刻から当該未来時刻までの時間長をタイミング値として出力する(例えば、“0.5秒後”など)。移動制御手段74にタイミング値を出力すると振動状態検出処理を終了する。
移動制御手段74(本発明における飛行制御手段)は、経路探索手段73にて算出された移動経路と位置推定手段71にて推定された自己位置と速度推定手段72で推定された飛行速度とを用いて、自律飛行ロボット1が経路探索手段73で算出された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である制御指令値を求め、当該制御指令値に基づいてモータ4を制御し、ロータ2の回転数を制御する。また、移動制御手段74は、振動状態検出手段75にて振動状態であると判定されたとき、振動状態検出手段75から受けたタイミング値に基づいてモータ4を制御し、飛行停止するようロータ2の回転数を制御する飛行停止制御を行う。
経路追従制御では、まず、各時刻での自律飛行ロボット1が目標とすべき直近の位置(以下、「ローカル目標」と呼ぶ)を算出する処理を行う。図7はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段74は、経路探索手段73で生成された移動経路を記憶部8から読出し、自律飛行ロボット1が現在時刻で目指している経由点Wp1と前回通過済みである経由点Wp0との2点間を繋げた直線Wを求める。そして、移動制御手段74は、求めた直線Wと自律飛行ロボット1の自己位置を中心とした球Sとの交点Lp’、Lpを算出し、目指している経由点Wp1に近い交点Lpをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律飛行ロボット1が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Poに向かって移動していき、自律飛行ロボット1は移動経路に沿って飛行していくことになる。
次に、経路追従制御では、算出したローカル目標に向かって飛行するようX、Y、Z、ヨー角の各方向毎に制御指令値ux,uy,uz,uψを算出する処理を行う。この際、現在の自己位置とローカル目標の位置との差異が小さくなるような制御指令値を求める。具体的には、XYZ軸方向の制御指令値u=(ux,uy,uz)は、位置推定手段71で求められた自己位置r=(Xs,Ys,Zs)と速度推定手段72で推定した速度v=(vx,vy,vz)とを利用し、PID制御により求める。XYZ軸方向の各制御指令値をu=(ux,uy,uz)、ローカル目標をr’=(x,y,z)としたとき、制御指令値は、u=Kp(r’−r)+Kd・v+Ki・eの式で算出される。ここで、Kp、Kd、KiはそれぞれPID制御のゲインのことであり、e=(ex,ey,ez)は誤差の積分値である。一方、ヨー角方向の制御指令値uψは、ψ'を目標角度、ψを位置推定手段71にて推定した自律飛行ロボット1の姿勢(角度)、vyawを速度推定手段72で推定した角速度とすると、uψ=Kp(ψ’−ψ)+Kd・vyawの式のようなPD制御により求める。なお、本実施の形態では、目標角度ψ'を目標対象物Mの方向、すなわち、目標対象物Mの位置の方向を向く角度とした。
飛行停止制御では、振動状態検出手段75から受けたタイミング値に基づいてモータ4の回転を停止させ、ロータ2の回転を停止させる。すなわち、現在時刻を基準としてタイミング値に応じた期間が経過した後にモータ4の回転を停止させる。なお、本実施の形態では、振動状態検出手段75から受けたタイミング値に基づいてモータ4の回転を急停止させることにより飛行停止(緊急着陸)させているが、これに限らず、タイミング値に基づいてモータ4の回転速度を徐々に低下させることによって飛行停止させてもよい。
このように、本実施の形態における自律飛行ロボット1は、振動状態検出手段75にて所定の過去期間における飛行速度の変化履歴に基づいて、当該飛行速度の符号反転を繰り返していることから振動状態を検出し、振動状態検出手段75にて振動状態であると検出したとき、移動制御手段74は、飛行速度が最小となるタイミングで飛行停止(緊急着陸)させる。したがって、本実施の形態における自律飛行ロボット1は、現在の飛行状態が自律的な飛行制御が機能しない振動状態であるとき、飛行速度が最小となるタイミングで飛行停止させることができ、迅速かつ安全に緊急着陸させることが可能となる。すなわち、飛行速度が最小であるため、着陸(落下)時の慣性移動を最小にすることができ、落下距離を最小にできることから周囲の物体への衝突の可能性を少なくすることができる。また、着陸(落下)時における運動エネルギーを最小にできるため、衝突時における周囲の物体や自律飛行ロボット1自体に対する被害を最小にすることができる。
また、本実施の形態における自律飛行ロボット1は、振動状態検出手段75にて、所定の過去期間において飛行速度の符号が回数閾値以上変化しているときに振動状態と判定する。このように、飛行速度の変化によって振動状態を検知することにより、簡易かつ確実に振動状態を検出することができる。
また、本実施の形態における自律飛行ロボット1は、振動状態検出手段75にて、現在時刻から直近(直前)の振幅値が大きいほど長い期間となるよう過去期間を設定し、当該過去期間における飛行速度の変化に基づいて振動状態であるか否かを判定する。すなわち、自律飛行ロボット1が振動状態となっているとき、直前の振幅値が大きいほど、振動の周期が長くなる傾向がある。そのため、直近の振幅値が大きいほど過去期間を長期間とすることにより、振動の状況に応じた適切な過去期間を設定することができ、振動の検出精度を向上させることができる。
また、本実施の形態における自律飛行ロボット1は、過去期間において速度の符号が回数閾値以上変化し、当該過去期間における振幅値の最大値が時間の経過と共に減少傾向であるとき、現在から所定の判定時間だけ判定を保留し、当該判定時間の経過後における振幅値が振幅閾値よりも大きいならば振動状態であると判定する。これにより、たとえ、過去期間における飛行速度の符号が回数閾値以上であったとしても、振動移動が収束傾向(振幅値が減少傾向)にある場合は振動状態とはみなさない。したがって、飛行停止制御によって着陸しなくても、飛行制御が回復するまで様子を見ることが可能となり、通常飛行へ迅速に復帰させることが可能となる。
ところで、本発明は、上記実施の形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施の形態で実施されてもよいものである。また、実施の形態に記載した効果は、これに限定されるものではない。
上記実施の形態では、速度推定手段72は本発明における飛行状態の計測値である自己位置の時間変化から求めた飛行速度を求め、振動状態検出手段75は、当該飛行速度の過去の変化履歴を示した変化履歴情報82に基づいて、過去期間における飛行速度の符号反転の繰り返しから振動状態を検出している。しかし、これに限らず、飛行状態の計測値として、例えば、別途備えた加速度センサやジャイロセンサによる計測値を積分した値を飛行速度としてを求め、当該飛行速度が符号反転を繰り返している振動状態を検出してもよい。これにより、自律飛行ロボット1の姿勢の時間変化についても飛行速度として求めることができ、姿勢の振動状態を検出することも可能となる。
また、上記実施の形態における、移動制御手段74は、振動状態検出手段75にて出力されたタイミング値に基づいて、モータ4の回転を停止させる飛行停止制御を行っている。しかし、これに限らず、ボクセル情報83(本発明における障害物情報)を参照することによって、モータ4の回転を停止させるタイミングにおいて自己位置の下方に地面以外の障害物(地面以外の占有ボクセル)が存在するか否かを判定し、障害物が存在しないときにモータ4の回転を停止させる飛行停止制御を行ってもよい。または、目標対象物位置81を参照することによって、自己位置の下方に目標対象物Mが存在しないときにモータ4の回転を停止させる飛行停止制御を行ってもよい。これにより、自律飛行ロボット1の下方に人や物体などの障害物が存在するとき、飛行停止(落下)させないため、より安全なタイミングで振動状態を解消して着陸することができる。
また、上記実施の形態では、振動状態検出手段75は、予め設定された回数閾値及び振幅閾値を用いて振動状態を検知している。しかし、これに限らず、移動目標位置の位置変化に基づいて、これらの閾値の少なくとも一方をを変更してもよい。具体的には、移動目標位置が大きく変化しているとき、回数閾値や振幅閾値を大きい値に変更し、反対に移動目標位置の変化が小さいとき回数閾値や振幅閾値を小さい値に変更する。すなわち、移動目標位置が大きく変化している場合、振動状態らしい変化をする可能性が高くなるため、振動状態の判定条件を厳しくすることにより、振動状態の誤検出を少なくすることができる。反対に、移動目標位置の変化が小さい場合については、振動状態の判定条件を緩和することにより、振動状態の失検出を少なくすることができる。したがって、振動状態の検出精度を向上させることができる。
また、上記実施の形態では、移動制御手段74は、振動状態検出手段75から振動状態である旨と、モータ4の回転を停止させるタイミング値を出力されたとき、当該タイミング値に基づいてモータ4の回転を停止させる。しかしこれに限らず、移動制御手段74は、振動状態検出手段75から振動状態である旨を出力されたとき、経路探索手段73に対して、タイミング値が示す時間における自己位置と当該自己位置の直下の着陸位置を出力し、経路探索手段73に対して当該自己位置から着陸位置に至る移動経路(着陸経路)を求めさせるようにしてもよい。
また、上記実施の形態では、管理装置102に接続された移動物体検出センサ104を用いて目標対象物Mを検出している。しかしながら、これに限定されるものではなく、撮像部3で取得した撮像画像を画像解析することにより、目標対象物Mの位置を推定してもよい。例えば、撮像画像の各フレームを画像処理して目標対象物Mの画像領域を抽出する処理を行う。この際、既知の技術(特開2006−146551号公報を参照)であるオプティカルフロー法、ブースティング学習(例えば、Haar−like特徴を用いているAdaBoostベース識別器による顔検出手法)による識別器等を用いて目標対象物Mの画像領域(人物領域)を抽出する。次に、当該抽出された画像領域の位置に基づいて目標対象物Mと自律飛行ロボット1との距離を推定する。具体的には、抽出した目標対象物Mの画像領域の頭頂部の(撮像画像における)y座標位置と距離との対応表を予め飛行高度毎に作成しておき、現在の飛行高度及び目標対象物Mの頭頂部のy座標位置を当該対応表に照らし合わせて自律飛行ロボット1との距離を推定する。しかし、これに限らず、抽出した目標対象物Mの頭部の大きさから距離を算出してもよい。すなわち、頭部の大きさと距離との対応表を予め作成しておき、抽出された目標対象物Mの頭部の大きさを当該対応表に照らし合わせて自律飛行ロボット1との距離を推定してもよい。
また、自律飛行ロボット1に撮像部3としてカラーカメラの代わりに距離画像センサを搭載して、当該距離画像センサから取得した距離画像を用いて、既知の移動物体抽出技術により目標対象物Mを抽出して、抽出した目標対象物Mと自律飛行ロボット1との距離値と自己位置とから目標対象物Mの位置を推定してもよい。また、自律飛行ロボット1にレーザスキャナを搭載し、当該レーザスキャナの出力値と自己位置とを用いて目標対象物Mの位置を推定してもよい。
また、上記実施の形態では、制御部7において位置推定処理、速度推定処理、移動経路生成処理、経路追従制御の一連の処理を行っている。しかし、これに限らず、図示しない制御用のPCを用意し、当該PCにこれらの一連の処理を実施させてもよい。すなわち、自律飛行ロボット1は、PCによって行われた位置推定処理、速度推定処理、移動経路生成処理、経路追従制御によって得られた制御指令値を無線通信又は有線通信によりPCから受信し、当該制御指令値に基づいてモータ4の回転数を制御することにより、目的の位置に飛行するようにしてもよい。このように、外部PCを用いて上記の一連の処理を分担することにより、自律飛行ロボット1のCPU処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。