本発明の実施の形態における自律移動ロボット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、移動空間グラフ情報85等の制御部7の各処理に用いられる各種パラメータ、各センサ等の出力値及び撮像画像等が含まれる。
目標対象物位置81は、管理装置102から受信した目標対象物Mの位置情報(座標:Xt,Yt,Zt)である。本実施の形態では、目標対象物位置81を目標対象物Mの足元位置、すなわち目標対象物Mが地面(床面)に接している位置とする。制御部7は、通信部9を介して目標対象物Mの位置を受信すると目標対象物位置81として記憶部8に記憶させる。
撮像条件情報82は、撮像部3の視野を表す情報である。撮像条件情報82として、撮像部3の俯角θ及び視野角φ等が記憶される。撮像条件情報82は、撮像部3の特性及び設置角度等に基づいて管理者等によって適宜設定される。なお、自律移動ロボット1に対して撮像部3の撮像方向を変更可能なカメラ制御装置を搭載した場合には、当該カメラ制御装置から俯角θの情報を取得するようにしてもよい。また、視野角φを変更可能な撮像部3を用いた場合には、当該撮像部3から視野角φの情報を取得するようにしてもよい。撮像条件情報82を参照することによって、目標対象物Mの位置との関係から自律移動ロボット1が移動空間内の所定の位置に存在するときに撮像部3によって目標対象物Mを撮像することが可能か否かを判定することができる。
なお、目標対象物Mに対して発揮する所定の機能を撮像部3による撮像機能とする場合には前述の撮像条件情報82を用いるが、撮像機能以外の機能を対象とする場合には撮像条件情報82に代わりに機能条件情報とすることができる。機能条件情報は、当該機能が十分に発揮できる範囲を画定するための条件を含むものとする。例えば、目標対象物Mに対して音を発したり、発光による警告を行ったりする等の機能である場合、機能条件情報は、音や光が有効に到達する範囲を特定できる情報を含むものとする。
撮像許可高度83は、撮像部3によって撮像することが許可されている高度を示す情報である。撮像許可高度83は、プライバシー等の観点から必要以上に高い高度からの撮像が許可されていない場合に設定される。一般的に、自律移動ロボット1が、高高度の位置から周囲を俯瞰するように撮像するほど、撮像画像内に様々な物体が映り込むことから、高高度に位置するほど自律移動ロボット1の利用者とは無関係な人のプライバシーを侵害する恐れがある。そのため、撮像部3で撮像可能な高度として撮像許可高度83が設定されたとき、当該撮像許可高度83以下でしか撮像できないように制限されるものとする。ただし、撮像許可高度83の設定は必須ではなく、必要に応じて設定されればよい。なお、撮像許可高度83は、地面(床面)からの距離として設定してもよいし、海抜高度として設定してもよい。
なお、撮像部3による撮像の場合には撮像許可高度83を用いるが、撮像以外の機能を対象とする場合には機能許可高度とすることができる。機能許可高度は、当該機能を利用することが許可されている高度とする。例えば、目標対象物Mに対して音を発したり、発光による警告を行ったりする等の機能である場合、機能許可高度は、音や光を発してよい高度とされる。
ボクセル情報84は、飛行空間をボクセル空間として複数のボクセルに分割して飛行空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部8に記憶される情報である。本実施の形態では、飛行空間を所定の大きさ(例えば15cm×15cm×15cm)のボクセルに等分割し、各ボクセルの識別子であるボクセルIDと、飛行空間におけるボクセルの位置(座標)と、ボクセル属性と、ボクセルコスト値とを対応付けてボクセル情報84として記憶する。ボクセル属性には、建造物等の障害物に位置するボクセルを「占有ボクセル」と定義して、自律移動ロボット1が移動できない空間とする。そして、占有ボクセルの近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルを「自由ボクセル」として定義する。
各ボクセルには、後述する経路探索手段73にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて登録される。ボクセルコスト値は、占有ボクセルにおいて最大値をとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、ボクセルコスト値=exp{−λ・(占有ボクセルからの距離)}の計算式からボクセルコスト値を算出することが好適である。ここでλは実験によって求めたパラメータである。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とする。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を0と設定する。なお、自律移動ロボット1が移動空間における予め定めた移動可能空間の外に出ないようにするため、移動可能空間とその外部との境界となるボクセルを占有ボクセルと設定することが好適である。
移動空間グラフ情報85は、ボクセル情報84に基づいて作成された3次元グラフ情報である。具体的には、ボクセル情報84に基づいて、各ボクセルの中心位置をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしたグラフ構造からなる情報である。図4は移動空間グラフ情報85のグラフ構造を説明する図であり、飛行空間におけるボクセルの一部(27個)を切り欠いたものである。図4において符号Bで表す個々の立方体はボクセルを表すものである。また、これらのボクセルBの中心位置にある黒又はハッチングにて塗りつぶした球はノードであり、ノード間を連結する点線で表示する線分はエッジである。なお、移動空間グラフ情報85におけるエッジの重みとして、隣接するノード間の距離に基づいて求められる後述する距離コストCMが設定されているものとする。
また、記憶部8には、各種パラメータ86として離間距離等も記憶される。離間距離は、目標対象物Mに追従飛行するにあたって、自律移動ロボット1と目標対象物Mとの水平面における維持すべき相対距離である。離間距離は、自律移動ロボット1の管理者等によって予め設定される。自律移動ロボット1を用いて所定の目標対象物Mを監視する場合、目標対象物Mに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Mから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施の形態の自律移動ロボット1は、目標対象物Mの詳細な撮像画像を取得でき、かつ、当該目標対象物Mから攻撃を受け難い距離に離間距離を予め、当該離間距離を保ちつつ追従飛行するように制御される。離間距離は、例えば3mとして設定される。
制御部7は、CPU等を備えたコンピュータで構成され、位置推定処理、速度推定処理、経路探索処理(機能可否判定処理及び移動経路生成処理)、移動制御処理(経路追従制御)を行う一連の処理として、位置推定手段71、速度推定手段72、経路探索手段73、移動制御手段74を含んでいる。
位置推定手段71は、位置検出センサ6の出力に基づいて、飛行空間における自律移動ロボット1の現在位置(自己位置)を推定する位置推定処理を行う。
具体的には、位置検出センサ6から得られた複数の航法衛星からの航法信号に基づいて既知の周知技術に基づいて推定した緯度・経度と、距離検出センサ5から得られた高度とから自己位置の座標(Xs,Ys,Zs)を計算する。さらに、電子コンパスやジャイロセンサなどの位置検出センサ6からの出力を受けて自己位置として姿勢YAWを求める。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律移動ロボット1の現在位置を推定してもよい。
位置推定手段71は、推定された自己位置(座標:Xs,Ys,Zs及び姿勢YAW)と管理装置102から受信した目標対象物Mの位置(座標:Xt,Yt,Zt)を経路探索手段73へ出力する。
なお、位置推定手段71は、目標対象物Mの位置に基づいてボクセル情報84を更新する処理を行う。具体的には、記憶部8のボクセル情報84に基づいたボクセル空間に目標対象物Mの位置を中心として予め定めた目標対象物Mの大きさと略同じ大きさの円柱モデル(例えば、監視対象の目標対象物Mを侵入者であるとしたとき、底面の半径0.3m、高さ1.7mの円柱モデル)を配置し、当該円柱モデルと干渉するボクセルを占有ボクセルとして設定することによりボクセル情報84を更新する。後述するように、自律移動ロボット1は、占有ボクセルには移動しないように飛行制御されるが、上記のように目標対象物Mの位置に基づいてボクセル情報84を更新することにより、自律移動ロボット1と目標対象物Mとの接触を回避することができる。
速度推定手段72は、後述する移動制御手段74における移動制御で利用するため、自律移動ロボット1の現在の飛行速度(vx,vy,vz,vyaw)を推定する処理を行う。本実施の形態では、位置推定手段71にて推定した自己位置(座標:Xs,Ys,Zs及び姿勢YAW)の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。この他にも、GNSSにおけるドップラー効果を利用した速度推定方法を用いてもよい。
経路探索手段73は、位置推定手段71で推定された自己位置及び目標対象物Mの位置と、記憶部8に記憶された各種情報とを用いて自律移動ロボット1の移動経路を算出する経路探索処理を行う。本実施の形態では、経路探索手段73における経路探索処理は、自己位置から目標対象物Mの近傍に設定した移動目標位置に至る移動経路を生成する。
経路探索処理では、まず、目標対象物位置81に基づいて移動目標位置を設定する処理を行う。本実施の形態では、(目標対象物Mの位置の時間変化により求めた)目標対象物Mの移動方向を目標対象物Mの正面方向として定め、当該正面方向に予め設定した離間距離(3m)だけ水平方向に離れた位置であって予め設定された高度(3m)の位置を移動目標位置として設定する。更に、移動目標位置として目標対象物Mの正面方向を撮像できる姿勢、すなわち目標対象物Mの正面方向と自律移動ロボット1の正面方向とが正対する姿勢となるよう設定する。
そして設定された移動目標位置と、記憶部8に記憶されたボクセル情報84及び移動空間グラフ情報85と、位置推定手段71にて算出された自己位置と、後述する機能可否判定処理の判定結果とを用いて自律移動ロボット1の移動経路を算出する移動経路生成処理を行う。移動経路生成処理では、ボクセル情報84及び移動空間グラフ情報85を参照し、自己位置に対応するボクセルのノード(以下「ゴールノード」という)から移動目標位置に対応するボクセルのノード(以下「スタートノード」という)に至る総コスト値Cが最も小さくなる経路をA*経路探索法により探索する。
A*経路探索法では、ある評価ノードn(ノードIDがnのノード)における総コスト値C(n)を数式(1)で表わす。
ここで、g(n)は、評価ノードnについてのA*経路探索法におけるgコスト値である。本実施の形態では、g(n)は、スタートノードから評価ノードnに至るまでの移動距離や障害物への接触危険性を考慮したコスト値として求める。すなわち、評価ノードnのgコスト値であるg(n)は、隣接ノードにおけるgコスト値であるg(n−1)と、当該隣接ノードから評価ノードに至る距離に基づいて設定された距離コストC
M、評価ノードにおけるボクセルコストC
v(n)との和として数式(2)によって算出される。
数式(1)におけるh(n)は、評価ノードnについてのA*経路探索法におけるhコスト値である。h(n)は、評価ノードnからゴールノードに至る推定距離L(n)と、撮像部3による撮像が可能か否かによって定まる機能コストV(n)に基づいて数式(3)によって算出される。
本実施の形態では、推定距離L(n)は、評価ノードnからゴールノードに至る直線距離により求められる。推定距離L(n)は、評価ノードnからゴールノードに至る直線距離が遠いほど大きい値となるように設定される。
機能コストV(n)は、撮像部3によって目標対象物Mが撮像できるか否かを判定する機能可否判定処理の判定結果に基づいて定められる。すなわち、経路探索手段73における機能可否判定処理は、撮像条件情報82に基づいて自律移動ロボット1が移動空間内の所定の位置において撮像部3が目標対象物Mに対して有効範囲で機能するか否かを判定する処理であって、自律移動ロボット1が第n番目のボクセルのノード(評価ノードn)の位置にあると仮定したときに撮像部3が設けられた位置から俯角θ及び視野角φで定められる画角内に目標対象物Mが収まって撮像が可能か否かを判定する処理である。具体的には、機能可否判定処理では、まず、撮像条件情報82に基づいて、評価ノードnの位置に自律移動ロボット1が存在していたと仮定した場合おける撮像部3の撮像範囲を求める。本実施の形態では、撮像範囲を地面(床面)における2次元の範囲を表す座標情報として算出する。そして、目標対象物位置81が求めた撮像範囲の内側に含まれているか否かを判定し、含まれている場合は撮像可能と判定する。なお、本実施の形態における機能可否判定処理では、「地面における撮像範囲」内に目標対象物位置81が含まれているか否かを判定しているが、これに限らず、「地面から所定の高さにおける撮像範囲」を求め、これを用いて判定してもよい。例えば、地面から人の顔の位置の高さ(例えば、1.5m)における投影面に撮像部3の撮像範囲を求める。そして、目標対象物位置81に当該「人の顔の位置の高さ」を加えた位置座標(以下、「目標対象物顔位置」という)を求める。そして、地面から人の顔の位置の高さにおける撮像範囲の内側に目標対象物顔位置が含まれているか否かを判定し、含まれている場合は撮像可能と判定してもよい。
移動経路生成処理では、機能可否判定処理の判定結果に基づいて、目標対象物Mが撮像可能であれば機能コストV(n)を最低値(例えば0)に設定し、不可能であれば最大値(例えば1)に設定する。また、画角内に目標対象物Mが収まる程度に応じて、すなわち撮像部3によって撮像された画像内の目標対象物Mの画像領域が占める割合が小さいほど機能コストV(n)が高い値となるように設定してもよい。推定距離L(n)及び機能コストV(n)には係数α及びβをそれぞれ乗算して適宜重み付けして加算してh(n)を算出する。
A*経路探索法では、数式(1)〜(3)を適用してスタートノードから順に隣接ノードの総コスト値C(n)を算出していくことを繰り返し、最終的にゴールノードに至る総コスト値C(n)が最も小さくなる経路を探索する。
以下に、A*経路探索法による移動経路の生成について簡単に説明する。A*経路探索法では、まず、スタートノードに隣接する一又は複数のノード(隣接ノード)を評価ノードとして設定するところから開始する。そして、それぞれの評価ノードについて数式(1)〜(3)により総コスト値を求める。次に、評価ノードの中で最小の総コスト値となるノードを注目ノードと称したとき、当該注目ノードの(これまで評価ノードと設定されていない新たな)隣接ノードを新たに評価ノードとして追加し、新たに追加された評価ノードに対しても同様に総コスト値を求める。続いて、同様に全ての評価ノードの中で最小の総コスト値となる評価ノードを注目ノードに再設定する。このように、評価ノードの総コスト値に基づいた注目ノードの再設定と、当該再設定に伴う新たな評価ノードの追加及び総コスト値の計算を繰り返し、最終的にゴールノードが注目ノードとして設定されたとき、経路の探索は終了する。
このようにして、起点となるスタートノードから終点となるゴールノードに至る最良の移動経路が生成される。経路探索手段73で生成された移動経路のデータは、経由点となるノードの位置(x,y,z)の集合データであり、この情報は記憶部8に一時的に記憶される。
ここで、従来の自律移動ロボットでは、図5(a)に示すように、撮像部3によって目標対象物Mが撮像できるか否か、すなわち撮像部3の俯角θ及び視野角φで定まる画角(図中、ハッチングした角度範囲)内に目標対象物Mが収まるか否かを考慮せず、自律移動ロボット1の自己位置から移動目標位置まで最短に到達できるように移動経路が探索されていた。
これに対して、本実施の形態における自律移動ロボット1では、機能コストV(n)を含む総コスト値C(n)に基づいて移動経路を生成する。これによって、目標対象物Mを撮像可能なノードを含む経路の総コスト値C(n)がより低く算出され、目標対象物Mを撮像できる経路がより優先的に探索される。したがって、図5(b)の実線で示すように、各ボクセルにおいて撮像部3で目標対象物Mが撮像可能であり、かつ、移動目標位置にできるだけ短い距離で到達できるような移動経路が生成される。また、自律移動ロボット1の管理者は、数式(3)における係数βの設定を変更することにより、撮像可否と移動経路長のどちらを重視するのかの設定変更をすることができる。すなわち、できるだけ撮像できるような移動経路となることを重視する管理者はβを大きめに設定し、できるだけ移動経路長が短くなるような移動経路となることを重視する管理者はβを小さめに設定することにより、どちらを重視するかの設定変更が可能となる。
なお、経路の探索方法は、A*経路探索法に限定されるものでなく、ダイクストラ法等の他の経路探索方法を適用してもよい。
移動制御手段74は、経路探索手段73にて算出された移動経路と位置推定手段71にて推定された自己位置と速度推定手段72で推定された飛行速度とを用いて、自律移動ロボット1が経路探索手段73で算出された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である速度指令値を求め、当該速度指令値に基づいてモータ4を制御し、ロータ2の回転数を制御する。
経路追従制御では、まず、各時刻での自律移動ロボット1が目標とすべき直近の位置(以下、「ローカル目標」と呼ぶ)を算出する処理を行う。図6はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段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・vψの式のようなPD制御により求める。なお、本実施の形態では、目標角度ψ'を目標対象物Mの方向、すなわち、目標対象物Mの位置の方向を向く角度とした。
このように、制御部7は、上述した位置推定手段71、速度推定手段72、経路探索手段73、移動制御手段74における各処理を逐次繰り返す。これにより、本実施の形態の自律移動ロボット1は、目標対象物Mから離間距離の位置に対して移動目標位置を逐次更新し、その都度移動経路についても逐次更新していくことによって、目標対象物Mを適切に追従することができる。
以上のように、自律移動ロボット1は、自己位置から移動目標位置までの経路を探索し、探索された経路に沿って自律的に移動する。このとき、距離コストCM、推定距離L(n)、ボクセルコストCv(n)のみならず機能コストV(n)をも含む総コスト値C(n)を考慮することによって、各ボクセルにおいて撮像部3での撮像が可能であり、かつ、移動目標位置にできるだけ短い距離で到達できる移動経路に沿って自律移動ロボット1が移動するように制御することができる。これにより、自律移動ロボット1が目標対象物Mに向かって移動している途中であっても撮像部3によって目標対象物Mを撮像できる機会を増やすことができ、証拠収集能力を高めることができる。
なお、撮像部3による撮像が許可されている高度が定められている場合には撮像許可高度83を考慮して移動経路を生成してもよい。この場合、撮像許可高度83を考慮して機能コストV(n)を設定することにより、撮像許可高度83への移動を制限するよう移動経路を生成する。例えば、経路探索手段73は、評価ノードnの高度が撮像許可高度83を超えている場合には機能コストV(n)を最大値に設定し、撮像許可高度83以下である場合には上記実施の形態と同様に目標対象物Mと撮像部3の画角との関係に基づいて撮像が可能か否かの機能可否判定を行って、その結果に応じて機能コストV(n)を設定する。この他にも、撮像許可高度83が設定されている場合において、自己位置における高度(Zs)が撮像許可高度83よりも大きいとき、撮像部3の撮像機能をOFFとすることにより、撮像許可高度83よりも上空においては撮像できないように制限することもできる。
また、自己位置と目標対象物Mとの間の距離が十分に離れている場合には撮像部3によって目標対象物Mが撮像できなかったり、たとえ撮像できたとしても十分な大きさ及び解像度の画像として写すことができなくなる。そこで、評価ノードnの位置から目標対象物位置81までの距離が所定の基準距離以上であれば撮像部3による撮像が可能か否かの機能可否判定処理を行わずに機能コストV(n)を最小値に設定し、一方、当該基準距離未満であれば上記実施の形態と同様に目標対象物位置81と撮像条件情報82との関係に基づいて撮像が可能か否かの機能可否判定を行って、その結果に応じて機能コストV(n)を設定するようにしてもよい。これにより、自律移動ロボット1は、目標対象物Mから基準距離以上離れている場合は、機能可否判定処理を省略することによる計算負荷を軽減でき、かつ、より短い距離の移動経路を求めることが可能となるため移動目標位置に素早く到着することができる。
なお、このときの基準距離は、固定値(例えば20m)として設定してもよいが、俯角θ及び視野角φを含む撮像条件情報82と撮像許可高度83とに基づいて設定してもよい。例えば、基準距離は、図7に示すように自律移動ロボット1の自己位置が撮像許可高度83の高度に位置していたと仮定したとき、目標対象物Mが撮像部3の画角内に収まらなくなる距離とされ、撮像許可高度83と撮像部3の俯角θ及び視野角φによって決定される。すなわち、撮像許可高度83の位置に自律移動ロボット1が存在していたとしても、目標対象物Mを撮像できないほど離れているならば、撮像可否判定処理を行うだけ処理の無駄が生じることになる。したがって、撮像条件情報82と撮像許可高度83に基づいて目標対象物Mを撮像可能な基準距離を求め、当該基準距離内でのみ撮像可否判定処理を行うことによって、より効果的に計算処理負荷を軽減することができる。
また、経路探索手段73は、移動経路生成処理において、評価ノードnの位置から目標対象物位置81までの距離に応じて機能コストV(n)の補正量を連続的に設定し、数式(3)における機能コストV(n)を当該補正量にて補正してもよい。一般的に、目標対象物位置81と自己位置との距離に応じて、目標対象物Mに発揮する機能の効果が異なる。例えば、本実施の形態のように撮像機能の場合は、目標対象物位置81に近い位置である程、目標対象物Mを大きく撮像可能であり、撮像画像の証拠能力は相対的に大きいものとなる。したがって、この場合、評価ノードnの位置から目標対象物位置81までの距離が遠くなるほど機能コストV(n)が高い値となるように補正量を定めることが好適である。すなわち、上述のように撮像可能と判定された評価ノードnであっても、目標対象物位置81に近い位置に存在する評価ノードnについては、目標対象物位置81に遠い位置に存在する他の評価ノードn’よりも機能コストV(n)がより小さくなるよう補正される。これにより、経路探索手段73は、自律移動ロボット1が目標対象物位置81に近づくほど、より目標対象物Mを撮像しやすい位置となるような移動経路を生成しやすくすることが可能となる。なお、自律移動ロボット1が目標対象物Mに発揮する機能によっては、反対に、評価ノードnの位置から目標対象物位置81までの距離が遠くなるほど機能コストV(n)が低い値となるよう補正量を定めるようにしてもよい。
ところで、本発明は、上記実施形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施形態で実施されてもよいものである。また、実施形態に記載した効果は、これに限定されるものではない。
上記実施の形態では、撮像部3による目標対象物Mの撮像の状況に応じて機能コストVを設定したが、撮像部3に限定されるものではなく、自律移動ロボット1から所定の範囲に対して所定の機能を発揮する手段であれば適用可能である。例えば、自律移動ロボット1から所定の範囲に対して音を発する指向性スピーカーや、所定の範囲に対して発光するスポットライトにより警告を行ったりする等の機能であっても適用対象となる。この場合、本実施の形態における撮像部3の俯角θ及び視野角φからなる撮像条件情報82に代わって、音や光などの機能を有効に発揮する範囲を規定するための情報(機能条件情報)を設定すればよい。例えば、音や光の放射方向θ’と指向特性を表す指標である半値幅(放射エネルギーが−3dBとなる放射角度)φ’を機能条件情報として設定すればよい。また、機能可否判定処理は、機能条件情報と目標対象物位置81との位置関係に応じて、評価ノードnにおいて目標対象物Mに対して有効範囲(指向性スピーカーやスポットライトの機能を有効に発揮する範囲)で機能するか否かを判定して機能コストV(n)を設定すればよい。また、撮像以外の機能について許可される機能許可高度が定められている場合、本実施の形態と同様に、当該機能許可高度に応じて機能コストVを設定するようにしてもよい。
上記実施の形態では、管理装置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処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。
また、上記実施の形態では、経路探索手段73の移動経路生成処理において、機能可否判定処理の判定結果に基づいて求めた評価ノードnの機能コストV(n)を、A*経路探索法におけるhコスト値の算出(数式(3)を参照)のために用いることにより、数式(1)により総コスト値C(n)を算出している。しかし、これに限らず、機能可否判定処理の判定結果に基づいて求めた評価ノードnの機能コストV(n)を、A*経路探索法におけるgコスト値の算出のために用いてもよい。すなわち、数式(2)のgコスト値の算出式を、g(n)=g(n−1)+CM+Cv(n)+γ×V(n)のように変更すると共に、数式(3)のhコスト値の算出式をh(n)=L(n)のように変更してもよい。あるいは、機能可否判定処理の判定結果に基づいて数式(2)における距離コストCMやボクセルコストCv(n)を補正してもよい。
また、上記実施の形態では、A*経路探索法において評価ノードnが新たに追加される都度、当該評価ノードnについて機能可否判定処理を行い、当該機能可否判定処理の判定結果に基づいて当該評価ノードnの機能コストV(n)を求めている。しかし、これに限らず、予め全てのノード(ボクセル)に対して機能可否判定処理を行っておき、当該判定結果に基づいて各ノード(ボクセル)における機能コストを求め、記憶部8にボクセル情報84(又は移動空間グラフ情報85)として記憶しておいてもよい。すなわち、目標対象物位置81が変更される都度、これらの情報及び撮像条件情報82に基づいて全てのノード(ボクセル)に対して機能可否判定処理を行い、ボクセル情報84(又は移動空間グラフ情報85)を更新する。この際、自律移動ロボット1の撮像部3は目標対象物Mの方向を常に向いていることとする。そして、経路探索手段73における移動経路生成処理は、当該ボクセル情報84と移動空間グラフ情報85と自己位置と目標対象物位置81とに基づいてA*経路探索法により同様に移動経路を生成してもよい。
あるいは、自己位置から移動目標位置に至る複数の移動経路を予め求めておき、それぞれの求めた移動経路上における各ノード(ボクセル)において機能可否判定処理を行い、それぞれの移動経路における移動経路長と撮像可能と判定されたノード数とに基づいて最終的に自律移動ロボット1が追従する移動経路を採用するように処理してもよい。例えば、複数の移動経路の中から最短の移動経路長を求め、当該最短の移動経路長から1.1倍以内の移動経路長からなる複数の移動経路の中から、撮像可能と判定されたノード数が最も多い移動経路を最終的に自律移動ロボット1が追従する移動経路として採用する。この場合に予め求めるべき複数の移動経路は、自己位置から移動目標位置に至る移動経路長に基づいて移動経路を算出するような経路探索アルゴリズムを複数種類用いることにより求めることができる。
また、上記実施の形態における経路探索処理では、自己位置を移動経路の起点とし、目標対象物Mの近傍に設定された移動目標位置を移動経路の終点とし、当該起点から当該終点に至る移動経路を生成している。しかし、これに限らず、自己位置の近傍の位置を起点とし、当該起点から移動目標位置に至る移動経路を生成してもよい。例えば、自己位置から所定時間(例えば1秒)だけ未来の時刻における自己位置(予想到達位置)を、現時点の自己位置及び移動速度から予想し、当該予想到達位置を移動経路の起点として設定してもよい。