本発明の実施の形態における自律移動ロボット1は、図1の概観図に示すように、クアッドロータ型の小型無人ヘリコプタである。なお、本発明の適用範囲は、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタや、自律移動する走行型のロボットについても同様に適用することができる。
自律移動ロボット1は、図2のシステム構成図に示すように、外部の警備センタ100や管理装置102と通信し、監視空間(移動空間)内に存在する所定の移動物体を目標対象物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の前方に向けて設け、障害物までの距離を測定するために用いることができる。また、距離検出センサ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、移動空間グラフ情報85、離間条件86、各種パラメータ87等の制御部7の各処理に用いられる情報、各センサ等の出力値及び撮像画像等が含まれる。
目標対象物位置81は、管理装置102から受信した目標対象物Mの位置情報(座標:Xt,Yt,Zt)である。本実施の形態では、目標対象物位置81を目標対象物Mの足元位置、すなわち目標対象物Mが地面(床面)に接している位置とする。制御部7は、通信部9を介して目標対象物Mの位置を受信すると目標対象物位置81として記憶部8に記憶させる。なお、管理装置102は、目標対象物Mの位置(座標:Xt,Yt,Zt)の時間変化に基づいて目標対象物Mの姿勢(例えば、目標対象物Mの正面方向を示した情報)を推定し、目標対象物位置81として記憶してもよい。
空間情報82は、飛行空間をボクセル空間として複数のボクセル(単位空間)に分割して飛行空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部8に記憶される情報である。本実施の形態では、飛行空間を所定単位の大きさ(例えば15cm×15cm×15cm)のボクセルに等分割し、図4に示すように、ボクセル毎に識別子であるボクセルIDと、飛行空間におけるボクセルの位置(座標)とを対応付けて空間情報82として記憶する。
空間情報82は、各ボクセルの属性を示す情報を含む。属性は、移動障害属性を含む。移動障害属性は、自律移動ロボット1の移動の障害となるか否かを示した属性である。例えば、建造物等の障害物に位置するボクセルは、自律移動ロボット1が移動できない空間として移動障害属性として設定される。また、障害物の近くに存在する空間に位置するボクセルについても、当該障害物に必要以上に接近しないよう移動障害属性として設定されるものとする。このように、空間情報82としてボクセルの位置と属性を用いることにより飛行空間に存在する障害物等の物体の位置や大きさ(形状)を表現することができる。なお、本明細書では以下において障害物に位置するボクセルを「占有ボクセル」、障害物の近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外のボクセルを「自由ボクセル」と称する。
透過/非透過属性は、自律移動ロボット1の有する撮像の障害となるか否かを示した属性である。透光性を有するボクセルであれば透過/非透過属性は透過となり、遮光性を有するボクセルであれば非透過となる。
例えば、一般的な壁や屋根等の遮光性を有する障害物は、移動障害属性であり、透過/非透過属性は「非透過」である。一方、ガラス窓、ネット、フェンス等の透光性を有する障害物は、移動障害属性であるが、透過/非透過属性は「透過」である。また、霧や煙等は、透過/非透過属性は「非透過」あるが、移動障害属性ではない。
各ボクセルには、後述する経路探索手段73にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて記憶部8に登録されるものとする。ボクセルコスト値は、占有ボクセルにおいて最大値Cmaxをとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、あるボクセル(評価ボクセル)のボクセルコスト値は、ボクセルコスト値=Cmax×exp{−λ・(R)}の計算式からボクセルコスト値を算出することが好適である。ここでλは実験によって求めたパラメータとし、Rは評価ボクセルに最も近い占有ボクセルからの距離とする。なお、自律移動ロボット1が移動空間における予め定めた移動可能空間の外に出ないようにするため、移動可能空間とその外部との境界となるボクセルを占有ボクセルとみなして、当該ボクセルのボクセルコスト値を最大値と設定することが好適である。
光源情報83は、移動空間内に照射される光の光源の位置を示す情報であり、移動空間内の照明条件を示した本発明における照明条件情報の一形態である。光源情報83は、図5に示すように、各光源のIDに対応付けて位置情報(座標:Xi ',Yi ',Zi ':ただしiは光源のIDに対応する番号)を登録したものである。
撮像条件情報84は、撮像部3の視野を表す情報である。撮像条件情報として、撮像部3の俯角θ及び視野角φ等が記憶される。撮像条件情報は、撮像部3の特性及び設置角度等に基づいて管理者等によって適宜設定される。なお、自律移動ロボット1に対して撮像部3の撮像方向を変更可能なカメラ制御装置を搭載した場合には、当該カメラ制御装置から俯角θの情報を取得するようにしてもよい。また、視野角φを変更可能な撮像部3を用いた場合には、当該撮像部3から視野角φの情報を取得するようにしてもよい。
移動空間グラフ情報85は、空間情報82に基づいて作成された3次元グラフ情報である。具体的には、空間情報82に基づいて、各ボクセルの中心位置をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしたグラフ構造からなる情報である。図6は移動空間グラフ情報85のグラフ構造を説明する図であり、飛行空間におけるボクセルの一部(27個)を切り出したものである。図6において符号Bで表す個々の立方体はボクセルを表すものである。また、これらのボクセルBの中心位置にある黒又はハッチングにて塗りつぶした球はノードであり、ノード間を連結する点線で表示する線分はエッジである。なお、移動空間グラフ情報85におけるエッジの重みとして、隣接するノード間の距離に基づいて求められる後述する距離コストCMが設定されているものとする。
離間条件86は、目標対象物Mに追従飛行するにあたって、自律移動ロボット1と目標対象物Mとの維持すべき位置関係を示す情報である。離間条件86は、自律移動ロボット1の管理者等によって予め設定される。自律移動ロボット1を用いて所定の目標対象物Mを監視する場合、目標対象物Mに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Mから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施の形態の自律移動ロボット1は、目標対象物Mの詳細な撮像画像を取得でき(撮像部3の撮像範囲内に目標対象物Mが位置し)、かつ、当該目標対象物Mから攻撃を受け難い距離となるように離間条件86を予め求めて記憶部8に記憶させ、当該離間条件86を満たすように追従飛行するように制御される。離間条件86は、例えば、平面内で目標対象物Mから3mの離間距離、及び、目標対象物Mから3mの離間高度として設定される。また、離間条件86は、ある程度の幅を持たせた範囲を示す条件としてもよい。
各種パラメータ87は、自律移動ロボット1の制御や機能を発揮させるために必要な他のパラメータである。例えば、自律移動ロボット1が撮像機能以外の機能を有する場合、当該機能を発揮するための機能条件情報とすることができる。機能条件情報は、当該機能が十分に発揮できる範囲を画定するための条件を含むものとする。例えば、目標対象物Mに対して音を発したり、発光による警告を行ったりする等の機能である場合、機能条件情報は、音や光が有効に到達する範囲を特定できる情報を含むものとする。例えば、音や光の放射方向θ’と指向特性を表す指標である半値幅(放射エネルギーが−3dBとなる放射角度)φ’を機能条件情報として設定すればよい。
制御部7は、CPU等を備えたコンピュータで構成され、位置推定処理、速度推定処理、経路探索処理、移動制御処理(経路追従制御)を行う一連の処理として、位置推定手段71、速度推定手段72、経路探索手段73、移動制御手段74を含んでいる。
位置推定手段71は、位置検出センサ6の出力に基づいて、飛行空間における自律移動ロボット1の現在位置(自己位置)を推定する位置推定処理を行う。
具体的には、位置検出センサ6から得られた複数の航法衛星からの航法信号に基づいて既知の周知技術に基づいて推定した緯度・経度と、距離検出センサ5から得られた高度とから自己位置の座標(Xs,Ys,Zs)を計算する。さらに、電子コンパスやジャイロセンサなどの位置検出センサ6からの出力を受けて自己位置として姿勢YAWを求める。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律移動ロボット1の現在位置を推定してもよい。
位置推定手段71は、推定された自己位置(座標:Xs,Ys,Zs及び姿勢YAW)と管理装置102から受信し記憶部8に目標対象物位置81として記憶した目標対象物Mの位置(座標:Xt,Yt,Zt)を速度推定手段72や経路探索手段73へ出力する。
なお、位置推定手段71は、目標対象物Mの位置に基づいて空間情報82を更新する処理を行う。具体的には、記憶部8の空間情報82に基づいたボクセル空間に目標対象物Mの位置を中心として予め定めた目標対象物Mの大きさと略同じ大きさの円柱モデル(例えば、監視対象の目標対象物Mを侵入者であるとしたとき、底面の半径0.3m、高さ1.7mの円柱モデル)を配置し、当該円柱モデルと干渉するボクセルを「移動障害属性」を有する占有ボクセルとして設定することにより空間情報82を更新する。後述するように、自律移動ロボット1は、占有ボクセルには移動しないように飛行制御されるが、上記のように目標対象物Mの位置に基づいて空間情報82を更新することにより、自律移動ロボット1と目標対象物Mとの接触を回避することができる。
速度推定手段72は、後述する移動制御手段74における移動制御で利用するため、自律移動ロボット1の現在の飛行速度(vx,vy,vz,vyaw)を推定する処理を行う。本実施の形態では、位置推定手段71にて推定した自己位置(座標:Xs,Ys,Zs及び姿勢YAW)の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。この他にも、GNSSにおけるドップラー効果を利用した速度推定方法を用いてもよい。
経路探索手段73は、位置推定手段71で推定された自己位置及び目標対象物Mの位置と、記憶部8に記憶された各種情報とを用いて自律移動ロボット1の移動経路を算出する経路探索処理を行う。本実施の形態では、経路探索手段73における経路探索処理は、自己位置から目標対象物Mの近傍に設定した移動目標位置に至る移動経路を生成する。
経路探索手段73は、図7に示すように、移動候補位置設定手段731、移動目標位置設定手段732及び移動経路生成手段733を含んで構成される。
移動候補位置設定手段731では、目標対象物位置81と離間条件86とに基づいて移動候補位置を設定する処理を行う。移動候補位置設定手段731は、目標対象物位置81から離間条件86として定められた離間距離及び離間高度だけ離れた周囲の領域に複数の移動候補位置を設定する処理を行う。例えば、移動候補位置を、目標対象物Mを中心として記憶部8に記憶された離間距離(例えば3m)を半径とした円状の位置で、かつ、管理者等により予め定められた離間高度(例えば3m)の領域に、それぞれ等間隔になるように設定する。
図8(a)〜図8(c)は、移動候補位置の設定についての説明図である。図8(a)〜図8(c)は、時刻tのときにボクセルで表示した飛行空間の一部を真上から見下ろしたときの図である。
符号B1で表わした黒塗りされた領域は、移動障害属性を有し及び非透過な占有ボクセルを示す。例えば、当該ボクセルは、建物等の遮光性の物体が占有する空間を示す。符号B2で表わしたハッチングで塗られた領域は移動障害属性を有するが、透明である占有ボクセルを示す。例えば、当該ボクセルは、ガラス、ネット、フェンス等の透光性を有する物体が占有する空間を示す。符号B3で表わしたドットで塗られた領域は、移動障害属性を有するが、透明な近接ボクセルを示す。符号B4で表わした白色の領域(何も塗られていない領域)は、移動障害属性を有さず、透明な自由ボクセルを示す。
まず、移動候補位置設定手段731は、図8(a)に示すように、目標対象物Mを中心として離間条件86を半径とした円周上に等角度の間隔で複数個の移動候補位置Pcを設置する。図8(a)の例では、5°おきに72個の移動候補位置Pcを設定している。
また、管理者により予め設定された離間条件86(固定値)に基づいて移動候補位置Pcを設定するのではなく、自律移動ロボット1の撮像部3(機能部)の有効範囲を示す機能条件情報を用いて目標対象物Mが自律移動ロボット1から当該有効範囲となる移動空間内の位置又は範囲を求め、当該位置又は範囲内に移動候補位置Pcを設定するようにしてもよい。
例えば、自律移動ロボット1の機能が撮像である場合ならば、撮像部3の画角等の撮像条件を示した機能条件情報を記憶部8から読み出して、機能条件情報と目標対象物Mの位置とに基づいて、目標対象物Mが撮像部3の撮像範囲内となる移動空間内の位置又は範囲を幾何学的に求め、当該位置又は範囲を示した離間条件86により記憶部8を更新する。例えば、目標対象物Mから所定の水平距離(例えば3m)だけ離れた位置において目標対象物Mを撮像可能(撮像範囲内となる)な高度を機能条件情報に基づいて幾何学的に算出し、当該水平距離及び高度からなる値により離間条件86を更新する。又は、所定の高度の位置において目標対象物Mを撮像可能(撮像範囲内となる)な水平距離を機能条件情報に基づいて幾何学的に算出し、当該水平距離及び高度からなる値により離間条件86を更新する。そして、更新した離間条件86を満たす位置に複数の移動候補位置Pcを設定する。
さらに、目標対象物Mの特徴部位についての目標対象物位置81からの相対位置を示した特徴位置を記憶部8に記憶させておき、当該特徴位置と目標対象物位置81とを用いて複数の移動候補位置Pcを設定してもよい。例えば、目標対象物Mが人(賊)の場合、特徴部位は、顔等の人(賊)を識別できる部位とし、特徴位置は略顔の位置となる高さ方向(Z軸方向)に150cmの位置として記憶すればよい。そして、目標対象物位置81と特徴位置とから求まる特徴部位の位置を用いて、当該特徴部位が機能部(撮像部3)の機能の有効範囲内に入るための位置関係を求め当該位置関係(例えば、目標対象物位置81から水平距離3m離れた位置であって高度3mの位置など)を離間条件86として記憶部8に記憶し、当該離間条件86を満たす位置に移動候補位置Pcを設定する。この際、目標対象物Mの特徴部位が自律移動ロボット1の機能部(撮像部3)の有効範囲内に入る位置又は範囲を幾何学的に求め、当該位置又は範囲内に複数の移動候補位置Pcを設定してもよい。なお、上記の他にも、目標対象物Mが車両である場合、その特徴部位をナンバープレートとし、目標対象物Mの姿勢(正面方向)を含む目標対象物位置81と特徴位置とを用いて、当該特徴部位(ナンバープレート)の位置を求めてもよい。
次に、移動目標位置設定手段732は、設定された移動候補位置Pcのうち移動障害属性を有するボクセルに含まれる移動候補位置Pcを除外する(図8(b))。すなわち、設定された移動候補位置Pcのうち「占有ボクセル」及び「近接ボクセル」に含まれる移動候補位置Pcを候補から除外する。
また、移動目標位置設定手段732は、残った移動候補位置Pcのうち目標対象物Mを撮像できる移動候補位置Pcを選択する。まず、残った移動候補位置Pcの各々を順に着目する移動候補位置Pcして、着目した移動候補位置Pcと目標対象物位置81とを結ぶ直線を求める。次に、空間情報82を参照して、当該直線上に存在する各ボクセルを抽出し、抽出されたボクセルのボクセルIDを求める。そして、空間情報82を参照して、求められたボクセルIDで特定されるボクセルの属性を調べ、非透過であるボクセルが存在する場合に着目した移動候補位置Pcを候補から除外する。非透過であるボクセルが存在する場合、着目した移動候補位置Pcからは目標対象物Mを撮像できないからである。一方、非透過であるボクセルが存在しない場合には、着目した移動候補位置Pcを候補として残す。着目した移動候補位置Pcから目標対象物Mが撮像できるからである。
なお、目標対象物Mの特徴位置が設定されている場合、着目した移動候補位置Pcと目標対象物Mの特徴位置とを結ぶ直線を求め、当該直線上のボクセルについて不透過であるか否かを判断することが好適である。
また、一般的な3次元コンピュータグラフィックス処理における仮想視点からのレンダリング技術を用いて目標対象物Mが撮像可能な移動候補位置Pcを求めてもよい。すなわち、空間情報82が非透過であるボクセルについては移動候補位置Pcにある仮想視点(仮想的に設けた撮像部3)からみたときに撮像の障害物(非透過の物体)として存在することとする。反対に、空間情報82が透過であるボクセルについては移動候補位置Pcにある仮想視点(仮想的に設けた撮像部3)からみたときに撮像の障害物とはならない(透明)ものとして存在することとする。そして、移動候補位置Pcの各々から目標対象物位置81に設置した目標対象物Mの3次元モデルをレンダリング(撮像)したときに当該目標対象物Mが撮像できるか否かを目標対象物位置81及び撮像条件を用いて幾何学的に算出する。
さらに、移動目標位置設定手段732は、残った移動候補位置Pcのうち撮像部3から目標対象物Mを撮像した場合に逆光になっていない移動候補位置Pcを選択する。移動目標位置設定手段732は、移動候補位置Pc、空間情報82、光源情報83及び撮像条件情報84を用いて幾何学的な演算をして撮像画角内に光源の位置が含まれているか否かを判定する。すなわち、残った移動候補位置Pcから撮像条件情報84によって定められた撮像画角で目標対象物Mを撮像したときに、光源情報83に含まれる各光源の位置が撮像画像内に写り込むか否かを判定する。具体的には、移動候補位置Pcを撮像部3の光学中心として撮像条件情報84によって規定される撮像画角を表す立体形状(視錐体)を求め、当該立体形状内に光源情報83によって定められる光源の位置が含まれるか否かを判定する。
そして、移動目標位置設定手段732は、撮像画角(を表す立体形状内)内に光源の位置が含まれていた場合、その光源の位置が他の障害物によって遮蔽されるか否かを判定する。すなわち、移動目標位置設定手段732は、当該光源の位置と当該移動候補位置Pcとを結ぶ直線を求め、空間情報82を参照してその直線上に非透過なボクセルが存在するか否かを判定する。直線上に非透過のボクセルが存在しているとき、当該光源からの光が当該非透過のボクセルをなす障害物によって遮られるため当該移動候補位置Pcは逆光状態ではないと判定する。反対に直線上に非透過のボクセルが存在していないとき、当該移動候補位置Pcは当該光源の位置が撮像画像内に写り込む条件を満たしている状態(逆光状態)にあると判定する。なお、本実施形態の移動目標位置設定手段732は、光源の位置が障害物によって遮られるか否かについて空間情報82を用いて判定しているが、これに限定されず、当該判定を行わない簡易的な処理により、各移動候補位置Pcが逆光状態となるか否かを判定してもよい。また、ボクセルの属性(透過/非透過)を考慮せずに、移動候補位置Pcと光源の位置とを結ぶ直線上に障害物等の物体に相当するボクセルが存在しているか否かを判定し、存在していないときに当該移動候補位置Pcは当該光源の位置が撮像画像内に写り込む条件を満たしている逆光状態にあると簡易的に判定してもよい。これらの簡易的な処理の場合、逆光状態の判定精度は低下するものの処理負荷を軽減させることができる。
移動目標位置設定手段732は、残った移動候補位置Pcのうち、当該移動候補位置Pcから目標対象物Mを撮像した場合に逆光状態になっていない移動候補位置Pcを選択する。図8(c)において、破線で囲まれていない移動候補位置Pcは逆光状態ではない移動候補位置Pcを表しており、これらの移動候補位置Pcが選択される。そして、選択された移動候補位置Pcのうち自己位置に最も近い、すなわち当該自己位置からの直線距離がもっと小さい移動候補位置Pcを移動目標位置に設定する。これにより、図8(c)に示すように移動目標位置Qが設定される。
なお、すべての移動候補位置Pcについて目標対象物Mを撮像できない又は目標対象物Mを撮像できるが逆光状態であると判定された場合、移動障害属性を有するボクセルに含まれるとして除外された後に残った移動候補位置Pcのうち自己位置に最も近い移動候補位置Pcを移動目標位置Qに設定すればよい。
すなわち、本実施の形態では、経路探索手段73は、飛行空間にある障害物の3次元形状のみならず、逆光状態を含めて自律移動ロボット1による撮像が可能か否かを考慮し、自律移動ロボット1の撮像機能が発揮可能な移動目標位置Qを優先的に設定する。
移動経路生成手段733は、設定された移動目標位置Qと、記憶部8に記憶された空間情報82及び移動空間グラフ情報85と、位置推定手段71にて推定された自己位置と、を用いて自律移動ロボット1の移動経路を算出する移動経路生成処理を行う。移動経路生成処理では、空間情報82及び移動空間グラフ情報85を参照し、移動目標位置に対応するボクセルのノード(以下「スタートノード」という)から自己位置に対応するボクセルのノード(以下「ゴールノード」という)に至る総コスト値Cが最も小さくなる経路をA*経路探索法により探索する。
A*経路探索法では、ある評価ノードn(ノードIDがnのノード)における総コスト値C(n)を数式(1)で表わす。
ここで、g(n)は、評価ノードnについてのA*経路探索法におけるgコスト値であり、本実施の形態では、スタートノードから評価ノードnに至るまでの移動距離や障害物への接触危険性を考慮したコスト値として求める。すなわち、評価ノードnのgコスト値であるg(n)は、隣接ノードにおけるgコスト値であるg(n−1)と、当該隣接ノードから評価ノードに至る距離に基づいて設定された距離コストC
M、評価ノードにおけるボクセルコストC
v(n)との和とし、g(n)=g(n−1)+C
M+C
v(n)により求める。また、h(n)は、評価ノードnについてのA*経路探索法におけるhコスト値であり、評価ノードnからゴールノードに至る距離の推定コスト値である。本実施形態では、評価ノードnからゴールノードに至る直線距離によりh(n)を求める。A*経路探索法では、スタートノードから順に隣接ノードの総コスト値C(n)を算出していくことを繰り返し、最終的にゴールノードに至る総コスト値C(n)が最も小さくなる経路を探索していく。
以下に、A*経路探索法による移動経路の生成について簡単に説明する。A*経路探索法では、まず、スタートノードに隣接する一又は複数のノード(隣接ノード)を評価ノードとして設定するところから開始する。そして、それぞれの評価ノードについて式(1)により総コスト値を求める。次に、評価ノードの中で最小の総コスト値となるノードを注目ノードと称したとき、当該注目ノードの(これまで評価ノードと設定されていない新たな)隣接ノードを新たに評価ノードとして追加し、新たに追加された評価ノードに対しても同様に総コスト値を求める。続いて、同様に全ての評価ノードの中で最小の総コスト値となる評価ノードを注目ノードに再設定する。このように、評価ノードの総コスト値に基づいた注目ノードの再設定と、当該再設定に伴う新たな評価ノードの追加及び総コスト値の計算を繰り返し、最終的にゴールノードが注目ノードとして設定されたとき、経路の探索は終了する。
このようにして、起点となるスタートノードから終点となるゴールノードに至る最良の移動経路が生成される。移動経路生成手段733で生成された移動経路のデータは、経由点となるノードの位置(x,y,z)の集合データであり、この情報は記憶部8に一時的に記憶される。
なお、経路の探索方法は、A*経路探索法に限定されるものでなく、ダイクストラ法等の他の経路探索方法を適用してもよい。
移動制御手段74は、移動経路生成手段733にて生成された移動経路と位置推定手段71にて推定された自己位置と速度推定手段72で推定された飛行速度とを用いて、自律移動ロボット1が移動経路生成手段733で生成された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である制御指令値を求め、当該制御指令値に基づいてモータ4を制御し、ロータ2の回転数を制御する。
経路追従制御では、まず、各時刻での自律移動ロボット1が目標とすべき直近の位置(以下、「ローカル目標」と呼ぶ)を算出する処理を行う。図9はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段74は、経路探索手段73で生成された移動経路を記憶部8から読出し、自律移動ロボット1が現在時刻で目指している経由点Wp1と前回通過済みである経由点Wp0との2点間を繋げた直線Wを求める。そして、移動制御手段74は、求めた直線Wと自律移動ロボット1の自己位置を中心とした球Sとの交点Lp’、Lpを算出し、目指している経由点Wp1に近い交点Lpをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律移動ロボット1が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Qに向かって移動していき、自律移動ロボット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の位置の方向を向く角度とした。
このように、制御部7は、上述した位置推定手段71、速度推定手段72、経路探索手段73、移動制御手段74における各処理を逐次繰り返す。これにより、本実施の形態の自律移動ロボット1は、目標対象物Mから離間条件86を満たす位置であって、撮像部3による撮像時に逆光にならない位置を優先的に選択した移動目標位置Qを逐次更新し、その都度移動経路についても逐次更新していくことによって、目標対象物Mを適切に撮像しながら追従することができる。
<変形例1>
上記実施の形態では、移動目標位置設定手段732において光源情報83及び撮像条件情報84を用いて幾何学的な演算をして撮像画角内に光源の位置が含まれているか否かを判定するものとしたが、各ボクセルの重心位置における各光源からの光の照射方向及び照射強度を予め算出しておき、それらの値を各ボクセルのボクセルIDと対応付けた情報である光照射情報として登録しておいてもよい。すなわち、光照射情報は移動空間内の照明条件を示した本発明における照明条件情報の一形態である。
移動目標位置設定手段732は、光照射情報を用いて移動候補位置Pcから撮像条件情報84に基づいて撮像するときに撮像画角が光の照射方向に向いているか否かによって逆光状態となるか否かを判定することができる。ここでは、撮像画角の光学中心を中心とした点対象の方向範囲内に、光照射情報における当該光学中心に相当するボクセルに対応付けられた照射方向が含まれているとき「撮像画角が光の照射方向に向いている」と判定する。また、撮像画角が光の照射方向に向いているときに光の照射強度によって逆光であるか否かの判定をしてもよい。例えば、移動候補位置Pcから目標対象物Mに向かって撮像するときに撮像画角が光の照射方向に向いており、かつその光の照射強度が所定の基準値以上である場合に逆光状態であり、そうでない場合には逆光状態ではないと判定することができる。
なお、この場合、光源の位置を考慮して各ボクセルへの光の照射方向及び照射強度を予め求めておくので光源情報83として光源の位置を記憶しておく必要はなくなる。
<変形例2>
光源の位置によって1つのボクセルから他のボクセルをみたときに逆光の位置になるか否かを光源の位置及び当該2つのボクセルの位置関係に基づいて予め算出し、ボクセル間の関係を空間情報82として登録しておいてもよい。すなわち、空間情報82に登録されているある1つのボクセルから撮像条件情報84によって定められている撮像画角により他のボクセルに存在する目標対象物Mに向かって撮像を行ったときに当該撮像画角内に光源の位置が収まるか否かを求め、撮像画角内に光源の位置が収まれば逆光状態であり、撮像画角内に光源の位置が収まらなければ逆光状態にならないとして2つのボクセルの関係を求める。これを空間情報82に存在するすべてのボクセルの組み合わせについて予め求めて空間情報82として記憶させる。
これによって、移動目標位置設定手段732は、空間情報82を参照することによって、移動候補位置Pcとなるボクセルと目標対象物Mとなるボクセルの位置関係から移動候補位置Pcから目標対象物Mを撮像したときに逆光状態になるか否かを判定することができる。
なお、本変形例においても光源の位置を考慮して2つのボクセル間の位置関係として逆光になるか否かを予め求めておくので、光源情報83として光源の位置を記憶しておく必要はなくなる。
<変形例3>
空間情報82として、移動障害属性及び透過/非透過属性に加えて、さらに物体を構成する構造物の光の反射属性を記憶させてもよい。例えば、完全に光を反射する場合を100%、光を反射しない場合を0%として各物体の反射率を反射属性として記憶させる。
自律移動ロボット1は、反射属性に基づいて、光源からの光が反射した場合に目標対象物Mに対して逆光になる位置を移動目標位置Qの候補から除外する処理を行う。移動目標位置設定手段732は、移動候補位置Pcから移動目標位置を設定する際、光源情報83の光源の位置と空間情報82の物対の大きさと位置とを用いて、光源からの光の照射範囲(到達範囲)を求める。すなわち、光源から照射された光が物体によって遮られずに到達するボクセル(以下、照射ボクセルという)を求める。また、移動目標位置設定手段732は、空間情報82に登録されている照射ボクセルの反射属性(反射率)を参照し、照射ボクセルに到達した光が反射して撮像部3によって撮像されるときの光の強度を求める。そして、移動目標位置設定手段732は、移動候補位置Pcからの撮像画角内に照射ボクセルが含まれるか否かを判定し、含まれている場合には当該照射ボクセルからの反射光の光強度が所定の基準値以上であるか否かを判定する。基準値以上の光強度を有する照射ボクセルが存在するとき、その移動候補位置Pcは逆光状態であると判定し、当該移動候補位置Pcを移動目標位置Qの候補から除外する。また、基準値以上となる照射ボクセルが所定数以上存在するときに逆光状態であると判定し、移動候補位置Pcを移動目標位置Qの候補から除外してもよい。
なお、反射属性として、鏡面反射や拡散反射(乱反射)等の条件を付加してもよい。この場合、反射属性として登録されている反射特性を用いてレイトレース等のシミュレーションによって各移動候補位置Pcが逆光状態であるか否かを判定すればよい。
<変形例4>
本変形例では、光源情報83の属性値として各光源の明るさを示す光強度をさらに記憶する。例えば、それぞれの光源の明るさをルーメン等の単位で表わした数値を光強度として記憶させる。また、所定の光源の明るさを基準として相対的な明るさを示す数値を光強度して記憶させてもよい。
移動目標位置設定手段732は、移動候補位置Pcと各光源との距離に応じた光源からの影響を考慮して移動目標位置Qを設定する。移動目標位置設定手段732は、空間情報82及び光源情報83を参照して、着目する移動候補位置Pcとの間に不透過の属性を有する障害物が存在しない光源の位置と当該移動候補位置Pcの位置との距離を算出する。移動目標位置設定手段732は、算出された距離に応じて、当該光源の光強度が減衰するものとして移動候補位置Pcへ届く光の強度を求める。例えば、算出された距離の2乗に反比例して光強度が減衰するものとする。そして、移動目標位置設定手段732は、移動候補位置Pcのうち撮像部3から目標対象物Mを撮像した場合に光源が逆光の位置にあり、当該光源から移動候補位置Pcへ届く光の強度が所定の基準値以上である場合に逆光であると判定する。
なお、光源情報83として各光源の光の拡散性や指向性を示す情報をさらに記憶させ、当該情報を用いて各光源から移動候補位置Pcへ到達する光の強度を算出するようにしてもよい。
以上のように、上記実施の形態及び変形例における自律移動ロボット1は、自己位置から移動目標位置までの経路を探索し、探索された経路に沿って自律的に移動することができる。また、本実施の形態では、自律移動ロボット1と光源との相対的な位置関係によって撮像時に逆光となる位置を移動目標位置Qとしないよう考慮して、目標対象物Mをより確実に撮像できる位置に自律移動ロボット1の移動目標位置を設定することができる。したがって、自律移動ロボット1によって目標対象物Mを撮像しながら追従飛行することが可能となる。
なお、上記実施の形態では、管理装置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処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。
また、上記実施の形態における経路探索処理では、自己位置を移動経路の終点とし、目標対象物Mの近傍に設定された移動目標位置を移動経路の起点とし、当該起点から当該終点に至る移動経路を生成している。しかし、これに限らず、自己位置の近傍の位置を起点とし、当該起点から移動目標位置に至る移動経路を生成してもよい。例えば、自己位置から所定時間(例えば1秒)だけ未来の時刻における自己位置(予想到達位置)を、現時点の自己位置及び移動速度から予想し、当該予想到達位置を移動経路の起点として設定してもよい。