以下、本発明の実施形態について図面を参照しながら説明する。
(第1実施形態)
図1には、第1実施形態に係るロボット10の動作制御に関連する全体構成のみを概略的に表したブロック図が示されている。この図において、前記ロボット10は、予め設定された出発地(開始位置)から目的地(目標位置)まで自律的に移動する自律ロボットとして機能し、移動途中で障害物との干渉が想定される場合、当該干渉を回避するための各種動作を行いながら移動可能となっている。
ここでの障害物としては、ロボット10の移動経路の周辺に存在する壁等の固定的な障害物(固定障害物)の他に、経時的に位置が変化する移動障害物も含まれており、以下の各実施形態においては、移動障害物を歩行中若しくは走行中の人間とし、以下に説明する。
また、干渉回避のための前記各種動作としては、人間の干渉回避動作を期待せずに、ロボット10のみが行う干渉回避のための移動の他に、干渉回避のために人間の移動を促すことを意図して、ロボット10から人間に働きかけを行う働きかけ行動が含まれる。これら各種動作をなすロボット10の行動は、後述する通り、種々の状況等に応じて適宜選択される。
ここで、前記働きかけ行動としては、人間にもある程度の干渉回避行動を期待するように、人間に対して何等かの主張を行う主張行動が挙げられ、当該主張行動としては、ロボット10の進行方向を人間に気付かせる進路示唆等を行う移動の他に、人間に対して回避を促す音声を出す声掛けの行動や、人間に意図的に接触する行動等がある。
前記ロボット10には、図1に示されるように、各種動作を可能に構成された機構や機器からなる動作部11と、ロボット10の周囲の環境情報を検出する検出装置12と、検出装置12の検出結果に基づき、ロボット10の動作制御を行う制御装置13とが設けられている。
前記動作部11は、所定範囲内を自律移動させるための機構やその動力源からなる移動手段14と、所定空間内を動作するための機構やその動力源からなるアーム15と、前記声掛け時等において所定の音声を発するスピーカー16とを含む。これら移動手段14、アーム15及びスピーカー16は、全て公知の部材、機構、装置類等からなり、本発明の本質部分でないため、各構成についての詳細な図示説明を省略する。
前記検出装置12は、ロボット10の周囲に存在する人間や物体等の障害物の位置情報を検出する周囲環境検出センサ18と、ロボット10に対する人間Hの認識状態を検出する認識状態検出センサ19とからなり、それぞれのセンサ18,19での検出結果は、制御装置13に逐次送信される。
前記周囲環境検出センサ18としては、特に限定されるものではないが、ロボット10の周囲へのレーザ光の照射による人間を含む物体の反射状態に基づいて、ロボット10の周囲の障害物の各表面部分の位置を検出する公知のレーザレンジファインダ等の測距センサが用いられる。つまり、この周囲環境検出センサ18では、各種障害物の表面部分を構成する点群における各点の平面視での2次元位置が、ロボット10を基準として測定される。
なお、周囲環境検出センサ18としては、ロボット10の周囲に存在する人間等の障害物の位置検出が可能な限りにおいて、GPS等を利用したセンサ等、他のセンサや装置類を適用することも可能である。
前記認識状態検出センサ19としては、特に限定されるものではないが、KINECT(登録商標)等の公知のデプスセンサが用いられ、人間の顔面部分が特定されるとともに、ロボット10に対する人間の顔面部分の向きが検出される。
なお、認識状態検出センサ19としては、同様の検出が可能な他のセンサや装置類を利用しても良い。
前記制御装置13では、予め指定された目的地まで、人間や他の障害物との干渉を回避しながらロボット10が自律移動可能となるように、動作部11に対し、後述する各種の動作指令が行われる。
この制御装置13は、ロボット10に一体的に或いは別体として設けられており、CPU等の演算処理装置及びメモリやハードディスク等の記憶装置等からなるコンピュータによって構成されている。当該コンピュータには、以下の各手段として機能させるためのプログラムがインストールされている。
次に、前記制御装置13の具体的構成について説明する。
前記制御装置13は、周囲環境検出センサ18での検出結果を用いて、将来的に人間がロボット10に干渉する可能性を判定する干渉度判定手段21と、認識状態検出センサ19での検出結果を用いて、現時点でのロボット10に対する人間の認知度を推定する認識度推定手段22と、干渉度判定手段21での判定結果と認識度推定手段22での推定結果を利用し、周囲の人間や障害物の状況に応じてロボット10を目的地に移動させるように、前記働きかけ行動を含む各種行動を計画する行動計画手段23と、前記働きかけ行動による人間への意図伝達の達成度を推定する達成度推定手段24と、達成度の推定結果により、ロボット10の行動計画を変更する計画調整手段25と、決定された行動計画に基づいてロボット10が動作するように、動作部11に動作指令する動作指令手段26とを備えている。
従って、この制御装置13は、周囲に存在する人間の位置情報の検出結果に応じて、当該人間との将来的な干渉回避のための各種の行動(動作)を計画する行動計画装置として機能することになる。
(干渉度判定手段)
前記干渉度判定手段21では、図2のように、ロボット10と人間Hがそれぞれ移動しながら接近する状況下において、次のようにして、周囲環境検出センサ18での検出結果から人間Hの現在の移動状況を取得し、ロボット10が将来的に人間Hに干渉する可能性の有無が判定される。
先ず前提として、ロボット10の位置情報と速度情報(速度及び方向)からなる移動情報は、目的地までの位置情報である経路を時刻に対応させた軌道情報して特定されている。ここで、ロボット10の現在位置は、特に限定されるものではないが、その平面視中央部分(以下、「ロボット10の体幹中心」と称する)の位置とし、平面視における絶対座標系での2次元座標(xR,yR)として特定される。また、絶対座標系におけるロボット10の速度情報は、前記2次元座標方向の速度成分(vRx、vRy)からなる速度ベクトルVRとして特定される。なお、ここでの説明に用いる前記絶対座標系は、ロボット10の通路P内の所定位置を原点とし、当該通路Pに沿った図2中上下方向をx軸とし、同左右方向をy軸とする。
一方、人間Hの移動情報は、前記周囲環境検出センサ18で検出された人間Hの位置情報に基づいて求められる。
すなわち、周囲環境検出センサ18では、ロボット10を基準とした相対座標系において、ロボット10の周囲に存在する人間Hや物体等の各障害物の表面部分を構成する点群の各点の位置が検出される。そして、この干渉度判定手段21では、障害物の表面を表す点群の中から、当該各点のピッチの大きさにより連続性が判断され、当該連続性に応じて各障害物の存在と種類が特定される。ここで、人間Hの肩幅(以下、「人間Hの体幹幅」と称する)に相当する幅を備えた障害物が、人間Hとして特定され、当該人間Hの表面の点群を楕円近似することにより、その平面視中央部分(以下、「人間Hの体幹中心」と称する)の位置が人間Hの現在位置とされる。このように検出された相対座標系での人間Hの現在位置が、ロボット10の現在位置における絶対座標系での座標(xR,yR)から、当該絶対座標系での座標(xH,yH)に変換される。また、周囲環境検出センサ18での所定時間毎の計測により、当該人間Hを構成する同一形状の表面部分が変位することから、この人間Hの座標(xH,yH)の経時的な変位により、速度成分(vHx、vHy)からなる絶対座標系における人間Hの速度ベクトルVHが算出される。
次に、これらロボット10及び人間Hの現在位置(xR,yR)、(xH,yH)から、所定時間毎に、現時点でのロボット10と人間Hの各体幹中心のx軸方向における縦方向の離間距離lHRが算出される。
そして、ロボット10と人間Hのそれぞれの速度情報から、ロボット10と人間Hとが将来的に横並びになるまでの時間Stが次式により計算される。
次に、時間Stから、図3に示されるように、ロボット10と人間Hとが横並びになった際におけるそれら各体幹中心の横方向(y軸方向)の離間距離DHRが次式により計算される。
最後に、前記離間距離DHRに基づいて、ロボット10や人間Hが現時点で回避動作を行わない場合に、それらが将来的に干渉する可能性の有無を表す干渉度が判定される。
具体的に、図3に示されるように、ロボット10の横幅(以下「ロボット10の体幹幅」と称する)S
LRと人間Hの体幹幅S
LHに対して、それぞれ定数K(K>1)倍した直径を有する円形のパーソナルスペースPS
R,PS
H(同図中破線部分)を仮想的に設定する。そして、当該各パーソナルスペースPS
R,PS
Hの半径R
LR,R
LHを加算した距離D
Lを干渉判定用の閾値として干渉度が判定される。
つまり、D
Lは次式で表される。
ここで、前記定数Kは、人間Hがある物体に接近したときに回避行動を取れるパーソナルスペースの横幅(マージン)に相当する倍数(例えば、1.3倍)とされる。そして、前記離間距離D
HRが、閾値D
L以上の場合は、干渉度が「無」とされ、逆に、当該離間距離D
HRが、閾値D
Lよりも小さい場合は、干渉度が「有」とされる。つまり、本実施形態においては、前記各パーソナルスペースPS
R,PS
Hが重なり合う状態が、「干渉」と称される状態となる。
なお、前記干渉度判定手段21では、横方向の前記離間距離DHRの代わりに、ロボット10及び人間Hの現時点における移動情報に基づいて、それらが将来的に最接近する時刻におけるそれらの最短直線距離である相対距離を利用し、前述と同様に閾値を設定して干渉度の有無を判定してもよい。
(認識度推定手段)
前記認識度推定手段22では、人間Hがロボット10に近づいた際に、認識状態検出センサ19で検知した人間Hの顔の向きに基づき、ロボット10に対する人間Hの視覚的な認知度を「大」、「小」、「無」の3段階で判定するようになっている。
具体的に、先ず、ロボット10の現在位置(xR,yR)と、周囲環境検出センサ18によって検出された人間Hの現在位置(xH,yH)とから、現在のロボット10と人間Hの各体幹中心間の直線距離となる相対距離が演算により求められる。なお、人間Hの現在位置(xH,yH)の検出は、認識状態検出センサ19の測距機能を用いることもできる。
そして、前記相対距離DSHRが、予め設定された距離(例えば、5m)以下になった場合に、次のようにして認知度の推定がなされる。
すなわち、図4(A)に示されるように、ロボット10が、人間Hの顔の向きに対し、第1の角度範囲θR1(例えば、人間Hの顔の向きに沿って延びる中心線Cを挟んでプラスマイナス15度)以内に存在する場合には、人間Hがロボット10を中心視野で捉えていると判断され、認知度「大」とされる。また、同図(B)に示されるように、ロボット10が、人間Hの顔の向きに対し、第1の角度範囲θR1を超え、第2の角度範囲θR2(例えば、前記中心線Cを挟んでプラスマイナス100度)以内に存在する場合には、人間Hがロボット10を周辺視野で捉えていると判断され、認知度「小」とされる。更に、同図(C)に示されるように、ロボット10が、人間Hの顔の向きに対し、第2の角度範囲θR2を超えて存在する場合には、ロボット10が人間Hの周辺視野外に存在すると判断され、認知度「無」とされる。
具体的には、次の演算により認知度が判定される。
前提として、人間Hの顔の向きに対応する頭角θ
HEADが、認識状態検出センサ19で検出される。この頭角θ
HEADは、図5に示されるように、人間Hの顔の向きに延びる中心線Cがy軸方向に対してなす角度となる傾き角度である。また、ロボット10の現在位置(x
R,y
R)と、周囲環境検出センサ18によって取得された人間の現在位置(x
H,y
H)とから、人間Hとロボット10の相対角度θ
HRが次式(4)により求められる。そして、次式(5)が成立する場合、すなわち、相対角度θ
HRと頭角θ
HEADとの差分の絶対値の大きさが、第1の角度範囲θ
R1の1/2以下の場合には、認知度が「大」とされる。また、次式(6)が成立する場合、すなわち、前記絶対値の大きさが、第1の角度範囲θ
R1の1/2超え、第2の角度範囲θ
R2の1/2以下の場合には、認知度が「小」とされる。更に、次式(7)が成立する場合、すなわち、前記絶対値の大きさが、第2の角度範囲θ
R2の1/2を超える場合には、認知度が「無」とされる。
本実施形態での認知度は、判断時点の瞬間において、人間Hがロボット10を視覚的に認識している可能性を表す瞬間的な認知度として判断しているが、本発明はこれに限らず、経時的な認知の状態により認知度の「大」、「小」、「無」を判断することも可能である。
この経時的な認知度は、例えば、次のように判断される。すなわち、前述した瞬間的な認知度「大」の状態が、現時点までの所定時間内に所定回数以上存在した場合に、認知度「大」とされる。また、瞬間的な認知度「大」の状態が、現在ではなく過去において所定時間内に所定回数以上存在した場合に、認知度「小」とされる。更に、これら以外の場合には、認知度「無」とされる。以上の各種の認知度は、ロボット10の用途や性質に応じて適宜選択される。
(行動計画手段)
前記行動計画手段23は、図1に示されるように、干渉度判定手段21で干渉度「無」と判定されたときに、現在位置から目的地までのロボット10の経路を計画する無干渉時計画部28と、同干渉度「有」と判定されたときに、人間との干渉回避をしながら、現在位置から目的地までのロボット10の経路及び動作を計画する干渉時計画部29とからなる。
前記無干渉時計画部28では、現時点において、移動障害物である人間との干渉が想定されないことから、検出装置12により検出され若しくは予め把握された壁等の固定障害物との衝突を回避する最短距離の経路が計画される。そして、当該経路について、位置情報と時間を対応させた軌道が生成され、当該軌道が、ロボット10を移動させるための実行軌道として特定される。
前記干渉時計画部29は、図1及び図6に示されるように、ロボット10が人間の横を通り抜ける際に必要となる幅である通り抜け幅を有する通り抜けスペースの有無を判定する通り抜けスペース判定部31と、通り抜けスペースの有無に応じ、干渉回避のためのロボット10の行動である干渉回避行動を決定する干渉回避行動決定部32とを備えている。
前記通り抜けスペース判定部31では、次のように、通り抜けスペースの有無が判定される。
ここでの判定に際しては、周囲環境検出センサ18での検出結果による前述の演算と同様に、将来的に人間Hがロボット10に横並びになる時間において、図3に示される横方向(y軸方向)の幅lsが計算される。つまり、この幅lsは、人間Hの横方向に位置する通路Pの壁W等の固定障害物と人間Hとの間の幅とされる。更に、この幅lsは、ロボット10の通過に必要となるロボット通過幅となるが、ここでは、当該ロボット通過幅lsが、ロボット10のパーソナルスペースPSRの幅となるその直径(2RLR)よりも広いか否かが判定される。そして、ロボット通過幅lsがパーソナルスペースPSRの幅よりも広い場合には、「通り抜けスペース有」とされ、そうでない場合には、「通り抜けスペース無」とされる。
なお、前記通り抜けスペース判定部31においては、特定された固定障害物等の位置情報と、ロボット10及び人間Hの現時点における移動情報とに基づいて、ロボット10が将来的に人間Hの横を通り抜けることが物理的に可能か否かを推定できる限りにおいて、他の演算手法を採用することもできる。
前記干渉回避行動決定部32は、図6に示されるように、前記通り抜けスペースが有る場合の干渉回避行動を特定する広大スペース用行動決定部34と、前記通り抜けスペースが無い場合の干渉回避行動を特定する狭小スペース用行動決定部35とを備えて構成される。
1.広大スペース用行動決定部の構成及び処理内容
以下、前記広大スペース用行動決定部34の構成及び処理内容について説明する。
前記広大スペース用行動決定部34は、図6に示されるように、現在時刻におけるロボット10と人間の相対関係に基づき、人間に対する干渉回避の行動内容を特定する行動内容特定部37と、行動内容特定部37で特定されたロボット10の行動内容に対応するように、現在位置から目的地までのロボット10の移動経路に係る実行軌道を特定する実行軌道特定部38とを備えている。
1−A 行動内容特定部37の説明
前記行動内容特定部37は、現在時刻において、人間Hとの相対位置及び相対速度に応じて設定される複数の領域のどこにロボット10が存在するかを特定する現存領域特定機能40と、ロボット10と人間との干渉時におけるロボット10及び人間それぞれの干渉角度を算出する干渉角度算出機能41と、先に求めた認知度及び干渉角度算出機能41で算出された各干渉角度に基づき、人間に対してロボット10が進路を譲る度合いとなる譲合度を前記領域毎に決定することで、干渉回避のためのロボット10の行動を特定する譲合度決定機能42とを有する。
先ず、前記現存領域特定機能40について説明する。
前記現存領域特定機能40では、先ず、現在時刻におけるロボット10と人間の判定用相対距離LRHが、後述するように演算によって求められ、判定用相対距離LRHに応じて、図7に示された3つの領域の何れかに存在するかが特定される。これら領域は、干渉までに要する時間が短い順、すなわち、人間H側から遠方に向って順に、第1、第2、第3の領域A1,A2,A3に区分される。ここで、第1の領域A1は、人間Hの干渉回避行動を期待せずに、人間Hの動きに合せてロボット10のみで干渉回避を行う同調行動がなされる同調行動領域とされる。また、第2の領域A2は、ロボット10の進行方向を人間Hに示唆することにより、人間Hの干渉回避行動をも期待してロボット10に干渉回避のための働きかけ行動を含む主張行動をさせる主張行動領域とされる。更に、第3の領域A3は、現時点で、ロボット10と人間Hとの間に距離があって、ロボット10が人間Hに干渉回避のための働きかけ行動を行っても、人間Hに干渉回避の意図が伝わり難いと考えられる領域である。このため、第3の領域A3は、ロボット10に働きかけ行動を含む干渉回避行動を未だ意識しない干渉回避行動外領域とされる。
前記判定用相対距離LRHは、ロボット10と人間Hの干渉までに要する現在時刻からの干渉推定時間をロボット10と人間Hの相対方向の距離に換算したものであり、次のようにして求められる。
先ず、検出装置12の検出結果により特定されたロボット10と人間Hのそれぞれの速度ベクトルV
R、V
Hに基づき、前記干渉推定時間T
ifが演算により求められる。そして、ロボット10と人間Hの各体幹中心間を結ぶ直線L(図8参照)の延出方向となる前記相対方向におけるロボット10と人間Hの各速度v
Rr,v
Hrから、それらの相対速度に干渉推定時間T
ifを乗じた距離が、判定用相対距離L
RHとして求められる。すなわち、この判定用相対距離L
Rは、次式(8),(9)により算出される。ここで、φ
Rは、図8に示されるように、ロボット10の速度ベクトルV
Rの方向とロボット10と人間Hを結ぶ直線Lとの間でなす角度を表し、φ
Hは、人間Hの速度ベクトルV
Hの方向と前記直線Lとの間でなす角度を表す。
前記同調行動領域A1は、判定用相対距離L
RHが、人間Hに対して所定の境界距離以内の距離となる領域である。この境界距離は、図7に示されるように、後述するように、パーソナルスペース臨界距離L
PSよりも長い最短回避距離L
PSEに、予め設定された働きかけ準備時間T
laを加味した臨界回避距離L
ILとされる。この臨界回避距離L
ILは、次式(10)〜(12)により求められる。
前記パーソナルスペース臨界距離LPSは、ロボット10がこれ以上近づいて欲しくないという人間Hの心理的な距離を意味し、ロボット10及び人間の各パーソナルスペースの半径RLR,RLHに対応して決定される一定値となり、例えば、1mとされる。
また、前記最短回避距離LPSEは、ロボット10が人間Hのパーソナルスペースに侵入してしまった場合に、不意の衝突を回避するためにロボット10が何等かの行動を採るために要する衝突回避準備時間Tppを考慮し、衝突回避準備時間Tppに前記相対速度を乗じた距離に、パーソナルスペース臨界距離LPSを加算した距離を意味する。ここで、衝突回避準備時間Tppは、予め設定された一定値を採る。
更に、前記臨界回避距離LILは、人間Hから最短回避距離LPSEまでの領域に入る前に、人間Hへの干渉回避行動を期待した何等かの働きかけ行動をロボット10にさせるために必要となる人間Hからの最低限の距離を意味する。すなわち、この臨界回避距離LILは、前記働きかけ行動に必要な最低限の時間である働きかけ準備時間Tlaが予め一定値として設定され、当該働きかけ準備時間Tlaに前記相対速度を乗じた距離に、最短回避距離LPSEを加算した距離を意味する。
前記主張行動領域A2は、判定用相対距離LRHが、人間Hから同調行動領域A1の境界までの距離である臨界回避距離LILよりも長く、且つ、ロボット10の働きかけ行動が有効となる最遠の人間Hからの距離である最遠働きかけ距離よりも短い範囲の領域とされる。当該最遠働きかけ距離は、一定値(例えば、7m)として予め設定される。
前記干渉回避行動外領域A3は、人間Hから前記主張行動領域A2よりも遠方となる領域である。
次に、前記干渉角度算出機能41について説明する。
この干渉角度算出機能41では、将来的にロボット10と人間の各パーソナルスペースが接する干渉時刻において、ロボット10と人間のそれぞれについて、干渉角度が算出される。当該干渉角度は、図9(A)に示されるように、ロボット10及び人間Hそれぞれの進行方向(同図中の各矢印方向)と、それらの体幹中心CR,CHから各パーソナルスペースPSR,PSHの接点となる干渉点Pへの方向との間でなす角度である。以下、ロボット10の干渉角度を角度θRとし、人間Hの干渉角度を角度θHとする。
これら各干渉角度θ
R、θ
Hは、前記検出装置12での検出結果に基づいて特定された各値、すなわち、ロボット10と人間Hのそれぞれの速度ベクトルV
R、V
Hと、干渉点Pと、ロボット10及び人間Hの各体幹中心C
R,C
Hとの間の方向における位置ベクトルS
R、S
Hとから、次式により算出される。なお、次式において、干渉点Pの座標を(p,q)とし、前記干渉時刻における各体幹中心C
R,C
Hの座標を(x
R,y
R)、(x
H,y
H)とする。
次に、前記譲合度決定機能42について説明する。
この譲合度決定機能42では、次のようにして、現存領域特定機能40で特定されたロボット10の現存領域と、干渉角度算出機能41で算出された各干渉角度θR、θHの差と、認識度推定手段22で推定された人間の認知度とに基づき、前記譲合度PRが決定され、当該譲合度PRに対応したロボット10の行動内容が特定される。
図9(B)中左側に例示するように、各干渉角度θR、θHの差が大きい場合、同図で言えば、ロボット10が左側に避け、人間Hが右側に避けるべきであると判断される等、それぞれの回避方向が分かりやすい。その一方で、同図(B)中右側に例示するように、各干渉角度θR、θHの差が少ない場合には、それぞれ左右どちらの方向に避けるべきかの判断が難しい。そこで、各干渉角度θR、θHの差の閾値Δθtを設定し、当該閾値Δθt未満のときに、閾値Δθt以上のときよりもロボット10の譲合度PRが高く設定される。
本実施形態では、次の各種状況に応じて、ロボット10の譲合度PRと当該譲合度PRに対応した行動が決定されるが、条件となる数値等については、特に限定されるものではなく、各干渉角度θR、θHの差が小さい程、ロボット10の譲合度PRを高く設定する限りにおいて、種々の変更が可能である。
また、現時点において、ロボット10が前記主張行動領域A2内に存在する場合、認識度推定手段22で推定された認知度をも考慮し、ロボット10の譲合度PRが決定される。ここで、ロボット10の譲合度PRは、認知度が大きい程、人間Hの干渉回避行動をより期待できると思われることから、低く設定される。
この場合、認知度が「大」で、各干渉角度θR、θHの差が閾値Δθt以上の(以下、「干渉角度に差が有る」と称する)ときには、ロボット10を人間Hと同等に扱うように、ロボット10の譲合度PRが50%とされる。一方、認知度が「大」で、各干渉角度θR、θHの差が閾値Δθt未満の(以下、「干渉角度に差が無い」と称する)ときには、ロボット10が少し多めに動けるように、ロボット10の譲合度PRが75%とされる。従って、これらの場合、すなわち、現時点でロボット10が主張行動領域A2内に存在し、且つ、認知度が「大」の場合には、人間Hに対し、ロボット10の進路示唆により物理的、心理的な変化を促す働きかけ行動であるロボット10の主張行動が計画される。
更に、認知度が「小」若しくは「無」で、干渉角度に差が有るときには、ロボット10の譲合度PRが100%とされる。一方、認知度が「小」若しくは「無」で、干渉角度に差が無いときには、更に安全性を考慮して、ロボット10の譲合度PRが125%とされる。従って、これらの場合、すなわち、ロボット10が主張行動領域A2内に存在し、且つ、認知度が「小」若しくは「無」の場合には、人間Hの回避行動をあまり期待せずに、人間Hの動きに合せてロボット10のみが干渉回避する同調行動が計画される。但し、この場合には、最初の働きかけ行動として、ロボット10の意図伝達率を上げるために人間Hの心理に働きかける声掛けの主張行動も計画される。
一方、現時点において、ロボット10が前記同調行動領域A1内に存在する場合、認知度に関わらず、干渉角度の差に応じ、前記同調行動のみが計画される。すなわち、この場合においては、ロボット10が主張行動領域A2内に存在し、且つ、認知度が「小」若しくは「無」のときと同一となる同調行動のみが計画される。つまり、干渉角度に差が有るときには、ロボット10の譲合度PRが100%とされる一方、干渉角度に差が無いときには、ロボット10の譲合度PRが125%とされる。
なお、ロボット10が前記干渉回避行動外領域A3内に存在する場合には、譲合度PRを未だ決定せずに、干渉回避のためのロボット10の行動は未だ計画されない。
以上の構成の行動内容特定部37での処理手順について、図10のフローチャートを用いながら以下に説明する。
先ず、現存領域特定機能40により、現時点において、ロボット10が、同調行動領域A1、主張行動領域A2、干渉回避行動外領域A3のどこの領域に存在するのかが特定される(ステップS101)。
ロボット10が、同調行動領域A1に現存する場合には、干渉角度算出機能41により、ロボット10と人間Hの各干渉角度θR、θHが算出される(ステップS102)。そして、譲合度決定機能42により、各干渉角度θR、θHの差が判定され(ステップS103)、干渉角度に差が有るときには、ロボット10の譲合度PRが100%となる同調行動が計画される一方(ステップS104)、干渉角度に差が無いときには、ロボット10の譲合度PRが125%となる同調行動が計画される(ステップS105)。
ロボット10が、主張行動領域A2に存在する場合にも、先ず、干渉角度算出機能41により、前記各干渉角度θR、θHが算出される(ステップS102)。そして、既に認識度推定手段22で推定された現時点の人間Hの認知度が「大」か、それ以外の「小」、「無」かが確認される(ステップS106)。
次に、譲合度決定機能42により、認知度が「大」の場合とそれ以外の「小」、「無」の場合で、それぞれ干渉角度の差の有無について判定される(ステップS107、ステップS108)。
ここで、認知度が「大」で、干渉角度に差が有るときには、ロボット10の譲合度PRが50%となる主張行動が計画される(ステップS109)。一方、認知度が「大」で、干渉角度に差が無いときには、ロボット10の譲合度PRが75%となる主張行動が計画される(ステップS110)。
また、認知度が「小」、「無」の場合で、干渉角度に差が有るときには、ロボット10の譲合度PRが100%となる同調行動が計画される(ステップS111)。一方、認知度が「小」、「無」の場合で、干渉角度に差が無いときには、ロボット10の譲合度PRが125%となる同調行動が計画される(ステップS112)。更に、既に声掛けの主張行動が行われたかが確認され(ステップS113)、未だ声掛けが行われていない場合のみに、声掛けを行う主張行動が計画される(ステップS114)。つまり、現時点で主張行動領域A2にロボット10が存在する場合で、認知度が「小」、「無」のときには、前記同調行動とともに、主張行動の一つである声掛けが1回計画される。
なお、主張行動領域A2において、人間の干渉回避行動の状況をセンシングし、その結果に応じて主張行動と同調行動を選択することもできる。例えば、人間が干渉回避行動を主張したと判断された場合には、ロボット10が同調行動し、人間が行動を変えないと判断された場合には、ロボット10が主張行動するように設定しても良い。
現時点で、ロボット10が干渉回避行動外領域A3に存在する場合には、人間に対するロボット10の干渉回避行動は、未だ計画されない。
1−B 実行軌道特定部38の説明
前記実行軌道特定部38では、行動内容特定部37で特定された譲合度PRに応じて、現在位置から目的地までのロボット10の移動経路に係る実行軌道を導出するようになっている。この実行軌道特定部38は、人間に干渉せずにロボット10を目的地に向って移動させることのできる複数パターンの軌道候補を探索する軌道候補探索部44と、探索された各軌道候補の中から最適となる軌道を抽出し、実際にロボット10を移動させる実行軌道とする最適軌道抽出部45とを備えている。
先ず、前記軌道候補探索部44について、以下に説明する。
この軌道候補探索部44は、相互に移動するロボット10と人間とが将来干渉する時刻である干渉時刻を算出する干渉時刻算出機能47と、ロボット10を所定の定常速度(一定速度)で移動させた場合における定速軌道を生成する定速軌道生成機能48と、ロボット10を一部区間で前記定常速度に対して加速や減速を行って移動させた場合における加減速軌道を生成する加減速軌道生成機能49と、これら定速軌道及び加減速軌道の中から、所定の要件に基づいて一部の軌道を除外し、最終的に軌道候補を決定する軌道候補決定機能50とを有する。なお、定速軌道生成機能48及び加減速軌道生成機能49は、干渉時刻での干渉を回避する経路に係る種々の軌道を生成する軌道生成機能を構成する。
前記干渉時刻算出機能47では、検出装置12での検出結果に基づき、将来的にロボット10と人間Hの各パーソナルスペースPSR,PSHが接する干渉時(図11(A)破線部分参照)の予想時刻である干渉時刻が演算により求められる。すなわち、ここでは、ロボット10と人間Hのそれぞれの速度ベクトルVR、VHと、現在時刻t0におけるロボット10と人間Hの位置ベクトルPR(t0),PH(t0)とから、演算により、ロボット10と人間Hの相対距離が、それぞれのパーソナルスペースPSR,PSHの半径の和であるRLR+RLHとなる時刻が求められ、当該時刻が干渉時刻tiとされる。
次に、前記定速軌道生成機能48について説明する。
この定速軌道生成機能48では、次のようにして、定常速度によるロボット10の移動により、ロボット10と人間の各パーソナルスペースが重ならないように干渉を回避する経路が特定され、当該経路における定速軌道が求められる。すなわち、先ず、人間との干渉を回避するための軌道の通過点(Way Point)が3箇所特定される。そして、ロボット10の現在位置、各通過点、及び目的地を移動時間順にそれぞれ直線で結ぶことで、定速軌道が特定される。なお、以下の説明において、前記各通過点を、ロボット10の移動時刻順に、「WP1」、「WP2」、「WP3」と称する。
この定速軌道生成機能48は、図6に示されるように、暫定的な定速軌道である仮軌道を求める仮軌道生成機能52と、仮軌道を調整しながら、所望となる干渉回避のための最終的な定速軌道を特定する軌道調整機能53とにより構成される。
前記仮軌道生成機能52では、次の手順で仮軌道が導出される。
すなわち、最初に、軌道中で人間Hに最接近するロボット10の横並び位置となる最接近地点であるWP2(図11(B)参照)の位置が仮決めされる。ここで、仮決めされるWP2の位置ベクトルP
R(wp2)は、次のように求められる。すなわち、干渉時刻算出機能47で求めた干渉時刻t
iにおける人間Hの位置(図11(B)中破線部分)から、ロボット10の現在位置SP(座標:(x
Rs,y
Rs))から目的地GP(座標:(x
Rg,y
Rg))に直線的に向う目標進行方向Tに対して法線方向Nに、人間Hから離れるように所定距離分シフトした位置に設定され、次式(15)〜(17)により算出される。次式において、ΔP
SGは、現在位置SPと目的地GPの間の相対位置ベクトルである。
なお、ここでの前記所定距離は、ロボット10と人間Hの各パーソナルスペースPS
R,PS
Hの半径の和であるR
LR+R
LHを基準距離L
bとし、当該基準距離L
bに、先に求めた譲合度P
R(%)を乗じた値となる。
また、次式において、P
H(ti)は、干渉時刻t
iにおける人間Hの位置ベクトルを表す。
その後、仮決めされたWP2の位置から、WP1及びWP3の位置がそれぞれ仮決めされる。ここで、WP1、WP2、及びWP3は、図11(B)に示されるように、目標進行方向Tに平行となる同一直線上に並ぶように配置される。また、WP1及びWP2間の直線距離と、WP2及びWP3の直線距離は、それぞれ一定値を採るように設定される。例えば、WP1及びWP2間の直線距離は、1m程度とされ、WP2及びWP3の直線距離は、ロボット10のパーソナルスペースPSRの半径RLRと同一の長さとされる。以上により、WP2が仮決めされると、その前後のWP1及びWP3の各位置がそれぞれ1箇所ずつ特定されることになる。そして、ロボット10の現在位置SP、WP1、WP2、WP3、及び目的地GPを順に直線で結んだ経路が計画され、当該経路から暫定的な仮軌道TRtが特定される。つまり、ロボット10が人間Hの最近傍を通過する際には、前記目標進行方向Tに沿う直線となるように軌道生成される。従って、通過点の中で、WP1は、最接近地点であるWP2よりも先にロボット10が通過する前側近傍地点となり、WP3は、WP2よりも後にロボット10が通過する後側近傍地点となる。
なお、本実施形態では、仮軌道TRtの通過点をWP1、WP2、WP3の3箇所に設けているが、WP3の先に更に1箇所(WP4)設定することもできる。このWP4は、図12に例示されるように、ロボット10の現在位置SPと目的地GPとを連結した直線TL上に配置されるとともに、WP3から当該直線TL上に垂下した地点から一定距離DTL分、目的地GP側に離れた地点に配置される。これによれば、ロボット10が人間Hとのすれ違い完了後、当初の目標進行方向Tに沿う軌道に戻ることになり、すれ違いの際における軌道の膨らみを最小限に抑制することができ、狭小地等のロボット10の移動において有用となる。
前記軌道調整機能53では、人間Hの移動と先の仮軌道でのロボット10の移動との時差による干渉を回避するための軌道調整が行われるが、当該軌道調整について、図11を用いながら以下に説明する。
ロボット10を所定の定常速度で仮軌道TRtに沿って移動させたときに、仮決めされたWP2の位置に達する時刻が、先に求めた干渉時刻tiと異なる事態が生じ得ることから、このときに人間Hとの干渉が生じてしまう可能性がある。すなわち、例えば、図11(C)のときのように、仮軌道TRtが、目標進行方向Tに沿う当初軌道に対して横に膨らむため、この当初軌道を移動しているときの干渉時刻tiのロボット10の位置(同図(A)の位置)よりも、長い距離の移動が必要となる位置にWP2が設定される可能性がある。この場合、ロボット10がWP2に達する時刻は、干渉時刻tiよりも遅い時刻となる。このため、人間Hは、図11(C)中の実線で示されるように、干渉時刻tiでの位置(同図中破線)よりも更に進行し、ロボット10との干渉が生じ得ることになる。
そこで、この軌道調整機能53では、干渉時刻tiとロボット10がWP2に達する時刻tw2とが相違している場合、これら各時刻が、完全に若しくはほぼ一致するまで、次のように調整が行われる。
前段階で特定された仮軌道TRtに沿ってロボット10が移動するとして、前述した干渉時刻算出機能47により、新たな干渉時刻tiが求められる。そして、仮軌道生成機能52での前述の手順により、新たな干渉時刻tiに基づき、図11(D)に示されるように、新たな仮軌道TRtが求められ、前記定常速度でのロボット10の移動により、新たな仮軌道TRt上のWP2に到達する時刻tw2が求められる。以上の処理を繰り返し行い、時刻tw2が、新たな干渉時刻tiに完全に若しくはほぼ一致したときに、その仮軌道が、正式な定速軌道として特定される。
なお、図11での説明においては、ロボット10が人間Hの前方を右側から横切る右避けの定速軌道を生成する例を図示しているが、定速軌道生成機能48では、同様の手順により、ロボット10が人間Hの後方を左側から横切る左避けの定速軌道の生成も行われる。従って、この定速軌道生成機能48では、右避け、左避けの2種類の定速軌道がそれぞれ1種類ずつ生成される。
次に、前記加速度軌道生成機能49について説明する。
この加減速軌道生成機能49では、前記所定の定常速度で移動するロボット10について、一部区間で加速や減速をすることにより、ロボット10と人間Hとの干渉を回避する加減速軌道が生成される。この加減速軌道としては、先ず、ロボット10を一時的に加速若しくは減速しながら、人間Hを右側若しくは左側から避けるように移動させる右避け及び左避けの横回避変速軌道がそれぞれ複数パターン生成される。
最初に、図13(A)に示されるように、定速軌道生成機能48で特定された定速軌道中のWP2にロボット10が達する時刻tw2での人間Hの位置を基準位置PHBとし、当該基準位置PHBから、人間Hの移動方向に沿って所定の間隔で前後にシフトした人間Hの位置となるシフト位置PHsが複数箇所設定される。なお、ここでの基準位置PHBからのシフトは、予め設定された範囲内で行われる。そして、前述した仮軌道生成機能52での仮軌道の導出手順と同様の手順により、各シフト位置PHsに達した人間Hにロボット10が干渉するとしたときの仮軌道がそれぞれ生成される。すなわち、仮軌道の導出において、上式(17)から、ロボット10のWP2の位置が求められ、当該各WP2の位置から、WP1及びWP3の位置がそれぞれ特定され、図13(B)に例示されるように、これら通過点を通る経路が定まる。なお、本実施形態において、各シフト位置PHsの間隔は0.1m程度とされるが、特に限定されるものではなく、例えば、人間Hの体幹幅に相当する0.5m程度まで広げることもでき、このようにすると、軌道候補決定のための演算処理の負荷を軽減させることが可能となる。
次に、各経路について、次の条件により、ロボット10の速度を部分的に変化させる横回避変速軌道が求められる。ロボット10が人間Hの近傍位置を移動して人間Hとすれ違いを行う際には、ロボット10が定常速度で移動することが望ましい。このことから、特に限定されるものではないが、本実施形態では、WP1以降は、予め設定された前記定常速度でロボット10を移動させ、現在位置SPからWP1までは、ロボット10の速度を変更可能にする。具体的には、各経路について、人間Hの速度ベクトルから、人間Hが前記シフト位置PHsに達する干渉時刻tiが求められ、干渉時刻tiが、当該シフト位置PHsを通過する経路上のWP2にロボット10が達する時刻となるように、現在位置SPからWP1までのロボット10の速度が求められる。つまり、各経路におけるWP1からWP2までの区間の直線距離と、当該区間におけるロボット10の速度とが、共に予め設定された一定値となるため、WP1からWP2までの通過時間が各軌道で一定となる。そこで、当該通過時間が、ロボット10の現在位置SPにおける時刻からWP2に達する前記干渉時刻tiまでの時間から減算され、当該減算された時間と、対応する経路におけるWP1の位置ベクトルとから、ロボット10の現在位置SPからWP1までの速度が算出され、各シフト位置PHsに対応する横回避変速軌道が特定されることになる。
これら横回避変速軌道のうち、図13(B)中実線で表される定速軌道に対し、現時点における人間Hの位置に近づく軌道(同図中破線)は、ロボット10の現在位置SPからWP1まで、前記定常速度に対して加速する軌道となっている。逆に、定速軌道に対し、現在の人間Hの位置から離れる軌道(同図中一点鎖線)は、ロボット10の現在位置SPからWP1まで、前記定常速度に対して減速する軌道となっている。
以上により、横回避変速軌道が、前記右避け及び前記左避けそれぞれ複数パターン生成される。
更に、加減速軌道生成機能49では、ロボット10の加減速軌道として、当初計画されたロボット10の現在位置SPから目的地GPまでの直線経路のままで、所定区間のみロボット10を加速若しくは減速させることにより、次のようにして、人間Hとの干渉を回避する直進変速軌道が生成される。すなわち、前記直線経路を人間Hが横切る際に、ロボット10と人間Hの各パーソナルスペースが重ならない所定距離分離れてロボット10が位置するように、ロボット10の進行方向の前後両側にWP2が設定される。そして、前記直線経路上のWP2の手前側に、横回避変速軌道の生成時と同様の条件で、WP1が設定されてロボット10の現在位置SPからWP1までの速度が求められ、直進変速軌道が特定される。この直進変速軌道は、ロボット10の現在位置SPからWP1までの変速区間を前記定常速度よりも速い速度で加速移動する軌道と、当該変速区間を前記定常速度よりも遅い速度で減速移動する軌道とがあり、これら軌道がそれぞれ1種類ずつ生成される。加速の直進変速軌道では、前記変速区間の加速により、前記直線経路を人間Hが横切る前に、その地点をロボット10が先に通過するように、人間Hよりも先にロボット10が進む干渉回避行動がなされる。一方、減速の直進変速軌道では、前記変速区間の減速により、前記直線経路を人間Hが横切った後に、その地点をロボット10が後で通過するように、ロボット10が人間Hを先に通す干渉回避行動がなされる。
以上のように、定速軌道生成機能48では、右避け、左避けそれぞれ1種類ずつ定速軌道が生成される。また、加減速軌道生成機能49では、加速度軌道として、右避け、左避けそれぞれで加速、減速を行う複数パターンの横回避変速軌道と、加速、減速それぞれ1種類ずつの直進変速軌道とが生成される。
前記軌道候補特定機能50では、定速軌道生成機能48や加減速軌道生成機能49で生成された軌道から、所定の軌道を除外し、残った軌道が軌道候補とされる。
ここで除外される軌道としては、ロボット10が物理的に実行不可能となる軌道が挙げられる。例えば、壁等の固定障害物の存在により、ロボット10が経路上を物理的に移動できない軌道や、加減速軌道生成機能49で生成される加減速軌道の中で特定された速度が、ロボット10の仕様上、予め設定された範囲外の速度になり、事実上実行不可能となる軌道等が挙げられる。
この軌道候補特定機能50によれば、後述する最適軌道抽出部45での演算前に、現実的に実行不可能となる軌道が候補から除外されることで、最適軌道抽出部45での演算処理負荷を極力軽減することができる。
以上の構成の軌道候補探索部44での処理手順を図14のフローチャートを用いながら説明する。
先ず、干渉時刻算出機能47で、相互に移動するロボット10と人間Hとが将来干渉する干渉時刻tiが求められる(ステップS201)。
その後、定速軌道生成機能48における仮軌道生成機能52において、求めた干渉時刻tiから、予め設定された定常速度でロボット10が進行する仮軌道が生成される(ステップS202)。更に、定速軌道生成機能48に含まれる軌道調整機能53において、先に求めた仮軌道におけるWP2にロボット10が達する時刻tw2と、先に特定された干渉時刻tiとが一致しているか否かが判定される(ステップS203)。そして、これら時刻が一致していない場合は、一致するまで前記ステップS201及びS202の処理が繰り返し行われる。その結果、WP2にロボット10が達する時刻tw2と、先に特定された干渉時刻tiとが一致したときに、その仮軌道が、定速軌道として決定される(ステップS204)。
次に、加減速軌道生成機能49において、前記定常速度でのロボット10の進行途中の一部区間で加速や減速を行う加減速軌道が生成される(ステップS205)。ここでは、定速軌道での干渉時刻tiを中心にして、干渉時刻を任意の間隔で変化させ、各干渉時刻における人間Hの位置をその進行方向に沿ってシフトさせることにより、加減速を利用した横回避変速軌道が生成される。更に、ここでは、ロボット10の現在位置から目的地までの直線経路上の一部区間で加速、減速する直進変速軌道も生成される。
最後に、軌道候補特定機能50において、定速軌道生成機能48及び加減速軌道生成機能49で生成された各軌道の中から、所定の要件に合致しない軌道を除外し、定速軌道や加減速軌道からなる複数の軌道候補が決定される(ステップS206)。
次に、前記最適軌道抽出部45について、以下に説明する。
この最適軌道抽出部45では、軌道候補探索部44で探索された複数の軌道候補それぞれについて、干渉回避のための運動エネルギーに対応する干渉回避エネルギーErを軌道コストとして算出し、当該軌道コストの最も少ない軌道候補が実行軌道として抽出される。
すなわち、干渉回避エネルギーErは、各軌道候補について、時系列で第1,2,・・・・,n−1,n,n+1,・・・・の各地点に区分し、当該軌道中の各地点において、後述する第1及び第2のエネルギー要素EGとEDを合計し、当該合計値を現在位置から目的地まで積分することにより算出される。つまり、干渉回避エネルギーErは、軌道中の各地点におけるロボットの運動エネルギーの変化状態を総合して求めた値となる。
具体的には、第1及び第2のエネルギー要素E
GとE
Dをパラメータとした次式(18)のコスト式により、干渉回避エネルギーE
rが求められる。
ここでは、図15に示されるように、ロボット10の現在位置SPと目的地GPを結ぶ直線経路の方向を目標進行方向(G方向)とし、当該直線経路に直交する法線方向を進行法線方向(D方向)とし、軌道中の各地点において、その際のロボット10の速度ベクトルをG方向とD方向に分解し、これら方向の速度ベクトルが用いられる。そこで、上式(18)でのVGNは、軌道中のn地点におけるG方向の速度ベクトルを表し、同VG0は、現在位置でのロボット10におけるG方向の速度ベクトルを表す。また、同VDNは、前記n地におけるD方向の速度ベクトルを表し、同VDn−1は、前記n地点の一つ前の地点n−1におけるD方向の速度ベクトルを表す。更に、A,Bは、予め所定値に設定される各エネルギー要素の重み付け定数であるが、例えば、それぞれの値を1にする等、相互に同数としても良い。
上式(18)において、第1のエネルギー要素EGは、ロボット10が一定の速度で進行し続けることが最も軌道コストの低い行動となることを踏まえ、現在位置SPでのロボット10のG方向の初期速度に対し、ロボット10の同方向の加速減速動作による速度変化に対応する要素となっている。
第2のエネルギー要素EDは、ロボット10が干渉回避を行う際に回避幅を大きくする程、移動効率が悪くなることを踏まえ、直前の地点からのD方向の速度変化に対応する要素となっている。
以上の最適軌道抽出部45によれば、回避行動前の定常状態からより一定の速度状態で目的地GPに向い、且つ、目的地GPに対して横方向に大きく逸れないような最良の実行軌道を各軌道候補の中から定量的に抽出することができる。
なお、軌道候補探索部44の軌道候補特定機能50において、次のように、軌道候補の絞り込みを更に行うことにより、最適軌道抽出部45でのコスト計算時における演算処理の負荷を更に軽減することができる。
ここでの絞り込みとしては、次のような軌道が、最適軌道抽出部45での演算対象となる軌道候補から除外される。例えば、図16(A)の破線で示されるように、ロボット10が、移動する人間Hの前方を減速しながら回避するような軌道は、回避幅が大きくなることから、軌道コストが増大することが明らかであり、軌道候補から除外される。また、同図中の一点鎖線で示されるように、ロボット10が、移動する人間Hの後方を加速しながら回避するような軌道も、同様に回避幅が大きくなるため、軌道候補から除外される。
また、図16(B)、(C)に例示されるように、ロボット10が人間Hに対して吸い付くように移動する軌道は、軌道コストが増大してしまうため、軌道候補から除外される。すなわち、WP1〜WP3の位置が、ロボット10の現在位置SP及び目的地GPを結んだ目標進行方向の直線TL(各図中一点鎖線参照)を境に、人間Hに対するロボット10の回避方向Dと逆側となる領域に存在する軌道は、軌道候補から除外される。つまり、図16(B)の軌道は、人間Hをその左側から回避する一方で、WP1〜WP3の位置が、前記直線TLよりも右側の領域に位置する軌道であり、軌道候補から除外される。また、同図(C)の軌道は、人間Hをその右側から回避する一方で、WP1〜WP3の位置が前記直線TLよりも左側の領域に位置する軌道であり、軌道候補から除外される。
1−C 実行軌道特定部38での反復的行動探索システムについて
なお、実行軌道特定部38には、ロボット10が将来的に複数の人間Hに干渉する可能性がある場合に、前述の手順を反復して軌道生成する反復的行動探索システムを設けることもできる。この反復的行動探索システムでは、前記軌道候補探索部44での前述した軌道生成手順により、ロボット10に対する最先の干渉が予測される人間(以下、「最短干渉予定者」と称する)に対する回避軌道が探索された後に、次に干渉が予測される他の人間についての回避軌道が探索される。そして、当該軌道探索が、現在位置でのロボット10により将来的な干渉が予測される全ての人間について干渉時刻順に反復して行われ、これら全ての人間を回避し得る軌道候補が求められる。
具体的に、先ず、検出装置12(図1参照)により、現在位置のロボット10の周囲の状況が検出され、前記干渉度判定手段21(同図参照)で、将来的に干渉が予測される複数の人間が特定される。その中から、ロボット10との相対距離が最も短く、最速でロボット10との干渉が予測される最短干渉予定者が特定される。
その後、軌道候補探索部44において、図17に示される手順で軌道候補が生成される。
すなわち、最初に、同図(A)に示されるように、最短干渉予定者H1について、ロボット10の現在位置SPから、前述と同様の手順により、前記通過点が決定され、当該通過点のうち前記後側近傍地点となるWP3(H1)までの各種軌道(定速軌道及び加速度軌道)Knが生成される。なお、図17の各図においては、図面の錯綜を回避するため、全ての軌道候補の表示を省略している。
次に、前記WP3(H1)までの各種軌道において、ロボット10がWP3(H1)に到達したと仮定し、それ以降、そのWP3(H1)をロボット10の初期位置として、同様に、目的地GPまでの干渉判定が行われる。その結果、干渉可能性のある他の人間H2が検出されると、同図(B)に示されるように、WP3(H1)を初期位置としたときに、次の時刻で干渉が予測される最短干渉予定者H2の特定がなされる。その上で、同図(C)に示されるように、先のWP3(H1)を初期位置として、同様に、最短干渉予定者H2を回避し得る軌道としてWP3(H2)までの各種軌道Knが生成される。
ここで、同図(C)に示されるように、先の手順で奥の人間H2に対する回避軌道を計画すると、ロボット10がWP3(H1)に到達した時に奥の人間H2もWP3(H1)の近傍に到達しており、同図(D)に示されるように干渉回避が不能になる場合等がある。そこで、このような事態を避けるために、同図(E)の一点鎖線部分で例示されるような軌道補正が行われる。この軌道補正は、1人目の人間H1と、その後の時刻で干渉が予測される次の2人目の人間H2について、回避する方向が同一の場合(同図中それぞれ右避けとなる軌道部分)に行われる。すなわち、ここでは、1人目の人間H1を回避するための軌道の通過点のうち前記前側近傍地点となるWP1(H1)から、2人目の人間H2を回避するための軌道における前側近傍地点となるWP1(H2)に直線移動する部分KAnを含む一部補正軌道が生成される。
以降、目的地GPまでに干渉する可能性のある人間Hnが更に存在する場合、同様の処理が繰り返し行われ、最後の人間HnでのWP3(Hn)と目的地GPを結ぶことで、ロボット10の現在位置SPから目的地GPまでの経路が決まり、当該経路に係る軌道候補が生成される。このようにして生成された複数パターンの軌道候補の中から、前記最適軌道抽出部45により、軌道コストの最も低い軌道候補が選択され、最終的な実行軌道とされる。
このような反復的行動探索システムによれば、現在のロボット10が将来的に干渉し得る複数の人間を回避する際に、チャタリングやスタックの発生しないスムーズな軌道計画を行うことができる。
なお、以上の反復的行動探索システムでの処理において、相互に近接する複数の人間をクラスタ化し、当該クラスタを1人の人間としてまとめて回避するように、前述の手順で軌道候補を生成することもできる。このようにすることで、軌道探索時における演算処理の負荷を低減することができる。
つまり、ここでは、所定時刻で干渉が予測される最短干渉予定者と、検出装置12で検出された他の人間との相対距離が所定範囲内、すなわち、当該相対距離が所定の閾値以下に存在するこれら複数の人間をクラスタとして推定し、当該クラスタを1人の人間としてまとめて回避する軌道が生成される。ここでの閾値としては、近接する人間同士の間をロボット10が干渉せずに通過可能となる空間幅(例えば、その最低値)が適用される。
2.狭小スペース用行動決定部の構成及び処理内容
以下、前記狭小スペース用行動決定部35の構成及び処理内容について、主に図1及び図6を用いながら説明する。
前記狭小スペース用行動決定部35では、現状においてロボット10が人間の横を通過できない状態であると予測され、後述するように、種々に条件に応じ、各種の働きかけ行動を含むロボット10の行動が選択される。
ここでの働きかけ行動としては、人間との干渉を回避するためにロボット10の移動方向の変更による人間への進路示唆の行動と、人間に対して回避を促す音声をスピーカー16から発生する声掛けの行動と、アーム15を人間に接触させる行動とがあり、これら働きかけ行動の何れか、若しくは任意の組み合わせが選択される。
また、ここでの働きかけ行動としては、前記干渉度判定手段21で干渉度有りと判定された場合に、認識度推定手段22で推定された認知度に応じ、先ず干渉を回避するための働きかけ行動を行う最初の働きかけ行動と、当該最初の働きかけ行動後の人間の行動から、後述する達成度推定手段24で推定された働きかけ行動の達成度に基づき、必要に応じて行われる再度の働きかけ行動とに大別される。以上の働きかけ行動の詳細については後述する。
この狭小スペース用行動決定部35は、人間へのアーム15の接触可能性の有無を判定するアーム接触判定部55と、当該接触可能性が「無」の場合の働きかけ行動を決定する遠方対応部56と、前記接触可能性が「有」の場合の働きかけ行動を決定する近接対応部57とからなる。
前記アーム接触判定部55では、アーム15の動作範囲内に人間が存在しているか否かにより、人間へのアーム15の接触可能性の有無が判定される。この接触可能性の有無は、検出装置12での検出結果によるロボット10と人間の相対位置情報と、予め設定されたアーム15の動作範囲とに基づいて判定される。
前記遠方対応部56では、次の3種の働きかけ行動により構成される遠方働きかけグループの中から、認知度に応じた働きかけ行動が決定される。
この遠方働きかけグループの中で、認知度「大」の場合には、次のように、ロボット10の移動方向の変更による人間への進路示唆を行う働きかけ行動が選択される。具体的に、図18に示されるように、このままではロボット10が通り抜けるための通り抜けスペースSの幅が十分に確保されないため、通り抜けスペースSの中央地点Pに向かってロボット10を移動させる。これにより、ロボット10が通過する通り抜けスペースSを確保できるように、人間Hに対して視覚的な進路示唆がなされる。
また、前記遠方働きかけグループの中で、認知度「小」の場合には、認知度「大」の場合と同様の進路示唆を行う働きかけ行動と、前述した介入度の弱い声掛け「小」の働きかけ行動とが選択される。
更に、前記遠方働きかけグループの中で、認知度「無」の場合には、認知度「大」の場合と同様の進路示唆を行う働きかけ行動と、前述した介入度の強い声掛け「大」の働きかけ行動とが選択される。
前記近接対応部57では、次の手順で各種の働きかけ行動がなされる。先ず、事前に、人間Hに対して聴覚面からロボット10の接近を知らせるために、所定の音声をスピーカー16から発する声掛けの働きかけ行動が選択される。
その上で、次の3種の働きかけ行動により構成される近接働きかけグループの中から、認知度に応じた働きかけ行動を含む行動が決定される。
この近接働きかけグループの中で、認知度「大」の場合には、次のように、ロボット10の移動方向が変更される。この場合には、事前に声掛けも行われていることから、人間Hはロボット10を十分に認知していると考えられ、人間Hの方では、ロボット10の通り抜けスペースSの幅を確保するための移動が不可能な事態であると推定される。従って、この場合には、例えば、図18中の一点鎖線矢印に示されるように、ロボット10が人間Hに対して後退しながら全く別の方向にロボット10を進行させるように、ロボット10の移動方向を変更する方向変換行動が選択される。なお、この際、ロボット10は、目的地への新たな移動経路が探索される。
また、前記近接働きかけグループの中で、認知度「小」の場合には、ロボット10のアーム15を人間Hに接触させることで、前記通り抜けスペースSを確保するように人間Hの移動を促す接触「小」の働きかけ行動が選択される。
更に、前記近接働きかけグループの中で、認知度「無」の場合には、認知度「小」の場合よりも介入度の強い接触「大」の働きかけ行動が選択される。
これら接触による働きかけ行動としては、例えば、人間Hの肩や背中をアーム15で押す動作があり、接触「小」の働きかけでは、人間Hに軽く接触し、接触「大」の働きかけでは、人間Hに繰り返し何度も接触し、或いは、人間Hとの接近時に不意な衝突を受け止めるように接触する等の動作を例示できる。
(達成度推定手段)
前記達成度推定手段24では、検出装置12の検出結果から、ロボット10の働きかけ行動による人間の行動状態を検証することにより、人間への働きかけの意図伝達の達成度が推定される。
具体的に、達成度推定手段24では、既になされた働きかけ行動によるロボット10の動作が、将来的なロボット10と人間との干渉回避の意図として、人間に伝達されたか否かが推定される。すなわち、ここでは、ロボット10の働きかけ動作後の人間の移動情報が、検出装置12の検出結果に基づいて取得され、当該移動情報から達成度が特定される。
この達成度推定手段24は、前記広大スペース用行動決定部34で計画されたロボット10の行動による達成度を決定する第1の達成度決定部59と、前記狭小スペース用行動決定部35で計画されたロボット10の行動による達成度を決定する第2の達成度決定部60とからなる。
前記第1の達成度決定部59では、将来的に人間がロボット10に最接近する時刻においての相互の体幹中心間の直線距離である相対距離が演算により求められ、当該相対距離に基づいて、次のように、達成度が「有」、「無」、「極有」の3種類で特定される。ここで、第1の評価である達成度「有」は、前記意図が人間に効果的に伝達されたと推定される場合であり、第2の評価である達成度「無」は、前記意図が人間にあまり伝達されなかったと推定される場合であり、第3の評価である達成度「極有」は、前記意図が達成度「有」よりも更に強力に人間に伝達されたと推定される場合である。
すなわち、ここでは、最接近時における前記相対距離が、ロボット10と人間の各パーソナルスペースの各半径を加算したパーソナルスペースの和よりも小さくなる場合には、将来の干渉可能性の状態が解消されていないとして、達成度「無」とされ、ロボット10の動作計画を再構築する必要性が有ると判断される。一方、前記相対距離が、前記パーソナルスペースの和を超える所定値以上の場合は、想定よりも人間が大きく避けていると推定され、達成度「極有」とされる。ここで、前記所定値は、予め設定されており、前記パーソナルスペースの和の1.5倍を例示できる。更に、前記相対距離が、前記パーソナルスペースの和以上で、前記所定値未満となる場合には、想定通りに人間が避けていると推定され、達成度「有」として、ロボット10の動作計画を再構成する必要性が無いと判断される。
前記第2の達成度決定部60では、ロボット10の通り抜けスペースSの有無に基づいて、次のように、達成度が「有」、「無」の2種類で判定される。ここでの達成度「有」、「無」についても、第1の達成度決定部59での達成度「有」、「無」の概念と同様である。
すなわち、ここでは、検出装置12で取得された働きかけ行動後の人間の移動情報から、前記通り抜けスペース判定部31での処理により、ロボット10の通り抜けスペースSの有無が判定される。そして、ロボット10の通り抜けスペースSが確保された場合には、達成度「有」とされ、更なる働きか行動の必要性が無いと判断される。一方、ロボット10の通り抜けスペースSが依然確保されていない場合には、達成度「無」とされ、別の行動の必要性が有ると判断される。
(計画調整手段)
前記計画調整手段25は、前記第1の達成度決定部59で決定された達成度に応じてロボット10の行動を決定する第1の調整部62と、前記第2の達成度決定部60で決定された達成度に応じてロボット10の行動を決定する第2の調整部63とからなる。
前記第1の調整部62では、次のようにして、達成度特定後におけるロボット10の行動が決定される。
すなわち、達成度「無」の場合は、ロボット10の行動計画を再構築する必要性が有るとされることから、再度、前述と同様に、行動計画手段23において、認知度に基づいて、通り抜けスペースの有無に応じた各種の行動計画がなされる。
また、達成度「極有」の場合には、想定よりも人間が大きく避けていると推定されることから、軌道コストを考慮して目的地に向ってより真っ直ぐとなる新たな軌道が生成される。
更に、達成度「有」の場合には、想定通りに人間が避けていると推定されることから、既に決定された行動計画を変更せずに、先に特定した実行軌道での移動がそのまま継続される。
前記第2の調整部63では、次のようにして、達成度特定後におけるロボット10の行動が決定される。
ここで、達成度「無」の場合には、先になされた働きかけグループに応じて、次の働きかけ行動が選択される。
前段階で遠方働きかけグループでの働きかけ行動がなされたときには、アーム接触判定部55で人間へのアーム15の接触可能性の有無が判断され、以降、前述した狭小スペース用行動決定部35での働きかけ行動がなされる。
また、前段階で近接働きかけグループでの働きかけ行動がなされたときには、このままでは、ロボット10が通り抜けるための通り抜けスペースをどうしても確保できないと判断され、全く別の方向にロボット10を移動させる軌道が生成され、当該軌道に沿った方向変換移動がなされる。なお、前段階の近接働きかけグループの中で、方向転換移動が既になされている場合は、当該移動がそのまま継続される。
一方、達成度「有」の場合には、ロボット10の通り抜けスペースが確保されることになったため、既に決定された軌道での移動がそのまま継続される。
(干渉回避の行動計画手順)
前記制御装置13によるロボット10の行動計画手順について、主として、図1、図6及び図19のフローチャートを用いながら、以下に説明する。
先ず、ロボット10が所定の出発地から移動を開始した後、干渉度判定手段21により、ロボット10とその周囲を移動する人間との間での将来的な干渉可能性の有無、すなわち、干渉度が判定される(ステップS301)。そして、行動計画手段23により、干渉度の有無に応じたロボット10の行動計画がなされる。
ここで、干渉度「無」の場合には、無干渉時計画部28で、人間との干渉回避を考慮することなく、壁等の固定移動物を避けながら、予め設定された目的地までロボット10を最短距離で移動させる経路計画がなされ、当該計画に対応する実行軌道が生成される(ステップS302)。そして、目的地に達するまで前記干渉度の判定以降の処理が一定時間毎に行われる(ステップS303)。
一方、干渉度「有」の場合には、干渉時計画部29で、各種状況に応じ、ロボット10と人間の将来的な干渉を回避するための行動計画がなされる。
干渉回避の行動計画を行うに際し、先ず、認識度推定手段22により、人間におけるロボット10に対する認知の程度を表す認知度「大」、「小」、「無」の何れかの判定が行われる(ステップS304)。
その後、通り抜けスペース判定部31において、ロボット10が人間の横を通り抜ける際の通り抜けスペースの有無が判定される(ステップS305)。そして、当該通り抜けスペースの有無に応じて異なる行動計画がなされる。
A.「通り抜けスペース有」と判定されたケース
このケースでは、広大スペース用行動決定部34にて次の各種処理が行われる。
先ず、行動内容特定部37にて譲合度が求められる(ステップS306)。
次に、軌道候補探索部44により、人間との干渉を回避するための軌道候補が、先に求めた譲合度に応じて複数パターン生成される(ステップS307)。
そして、最適軌道抽出部45により、軌道候補探索部44で生成された複数パターンの軌道候補の中から、軌道コストの最も低い効率的な移動が可能となる軌道が実行軌道として決定される(ステップS308)。
その後、実行軌道を生成するための動作時間や、実行軌道でのロボット10の移動開始から人間が行動するまでの応答時間等を考慮し、予め設定された一定の待機時間が設けられ、待機時間内が経過するまで次の処理が行われない(ステップS309)。
前記待機時間が経過した後、達成度推定手段24で達成度が推定される(ステップS310)。その結果に応じて、計画調整手段25により、行動内容の調整の要否が決定される。すなわち、達成度「無」の場合は、前述したステップS304に戻って認知度の判定がなされ、ステップS305以降の処理が再度なされる。
また、達成度「極有」の場合には、目的地に向ってより真っ直ぐとなるように、実行軌道の修正が行われる(ステップS311)。
更に、達成度「有」の場合には、既に決定された実行軌道がそのままの状態とされ、目的地に達するまで前記干渉度の判定以降の処理が一定時間毎に行われる(ステップS303)。
B.「通り抜けスペース無」と判定されたケース
このケースでは、狭小スペース用行動決定部35にて次の各種処理が行われる。
先ず、アーム接触判定部55により、現時点において、アーム15を人間に接触させる接触可能性の有無が判定される(ステップS312)。
B−1.接触可能性「無」の場合
アーム15に対して接触不能となる接触可能性「無」の場合には、認知度に応じて、前記遠方働きかけグループの中から働きかけ行動が選択される(ステップS313)。
すなわち、前記遠方働きかけグループの中で、認知度「大」の場合には、ロボット10の移動方向を変更させる軌道が生成され、ロボット10が通過する通り抜けスペースを確保できるように、人間に対して視覚的な進路示唆を行う働きかけ行動が選択される。
また、前記遠方働きかけグループの中で、認知度「小」の場合には、認知度「大」の場合と同様の進路示唆を行う働きかけ行動が選択されるとともに、介入度の弱い声掛け「小」の働きかけ行動が選択される。
更に、前記遠方働きかけグループの中で、認知度「無」の場合には、認知度「大」の場合と同様の進路示唆を行う働きかけ行動が選択されるとともに、介入度の強い声掛け「大」の働きかけ行動が選択される。
その後、達成度推定手段24で達成度が判定される(ステップS314)。ここでは、検出装置12で取得された働きかけ行動後の人間の位置情報から、前述のステップS305と同様の処理により、ロボット10の通り抜けスペースの有無が判定される。そこで、ロボット10の通り抜けスペースが確保された場合には、達成度「有」とされ、更なる干渉回避行動の必要性が無いと判断される。一方、ロボット10の通り抜けスペースが依然確保されていない場合には、達成度「無」とされ、更なる干渉回避行動の必要性が有ると判断される。
そして、計画調整手段25により次の対応がなされる。すなわち、達成度「有」の場合には、既に決定された実行軌道がそのままの状態とされ、目的地に達するまで前記干渉度の判定以降の処理が一定時間毎に行われる(ステップS303)。一方、達成度「無」の場合には、前述のステップS312に戻り、それ以降の処理が繰り返し行われる。
B−2.接触可能性「有」の場合
ステップS312で、アーム15に対して接触可能となる接触可能性「有」と判定された場合には、先ず、人間に対して聴覚面からロボット10の接近を知らせるために、所定の音声をスピーカー16から発する声掛けの働きかけ行動が行われる(ステップS315)。その上で、認知度に応じて、前記近接働きかけグループの中から各種行動が選択される(ステップS316)。
前記近接働きかけグループの中で、認知度「大」の場合には、ロボット10が人間に対して後退しながら全く別の方向にロボット10を方向転換させる行動が選択され、ロボット10の移動方向を変更しながら目的地を目指す新たな軌道が生成される。
また、前記近接働きかけグループの中で、認知度「小」の場合には、ロボット10のアーム15を人間に接触させることで、前記通り抜けスペースを確保するように人間の移動を促す接触「小」の働きかけ行動が選択される。
更に、前記近接働きかけグループの中で、認知度「無」の場合、認知度「小」の場合よりも介入度の強い接触「大」の働きかけ行動が選択される。
その後、達成度推定手段24で達成度が判定される(ステップS317)。ここでは、検出装置12で取得された人間の位置情報から、前述のステップS305と同様の処理により、ロボット10の通り抜けスペースの有無が判定される。そこで、ロボット10の通り抜けスペースが確保された場合には、達成度「有」とされ、更なる働きかけ行動の必要性が無いと判断される。一方、ロボット10の通り抜けスペースが依然確保されていない場合には、達成度「無」とされ、更なる働きかけ行動の必要性が有ると判断される。
そして、計画調整手段25により次の対応がなされる。すなわち、達成度「有」の場合には、既に決定された実行軌道がそのままの状態とされ、目的地に達するまで前記干渉度の判定以降の処理が一定時間毎に行われる(ステップS303)。一方、達成度「無」の場合には、このままでは、ロボット10が通り抜けるための通り抜けスペースをどうしても確保できないと判断され、全く別の方向にロボット10を移動させる方向変換軌道が生成される(ステップS318)。その後、干渉度判定手段21での干渉度の判定に戻り(ステップS301)、前述と同様の手順を経ながらロボット10の行動計画がなされる。
以上、本実施形態によれば、あたかも人間同士のすれ違い時のような人間の振る舞いに近いロボット10の動作が可能となり、ロボット10の移動に際して、その周囲の人間に過剰な不快感を与えることなく、しかも、ロボット10を目的地に向って効率良く移動させることができる。
なお、制御装置13では、前述した声掛けの働きかけ行動において、ロボット10の周囲の環境における音圧等の音情報をマイク(図示省略)で収集し、当該音情報に応じ、周囲の環境に配慮した音の大きさや種類等の音声をスピーカー16から発する音制御を行っても良い。
また、制御装置13では、アーム15が人間に接触する働きかけ行動において、人間の周囲が密集している等の場合にソフトな接触を行う等、ロボット10の周辺の環境情報を取得するセンサの検出結果に応じて、人間への接触の程度を可変にする動作制御を行っても良い。
その他、前述した働きかけの種類は、例示的なものであって、ロボット10の動作による他の働きかけ行動を種々設定することもできる。
また、前記実施形態では、「干渉度」が2段階、「認知度」が3段階、「達成度」が2段階にそれぞれ設定されているが、本発明はこれに限らず、それぞれ更に多段階で評価し、段階毎に更に細かい働きかけを設定することも可能である。
次に、本発明の他の実施形態について説明する。なお、以下の説明において、前記第1実施形態と同一若しくは同等の構成部分については同一符号を用いるものとし、説明を省略若しくは簡略にする。
(第2実施形態)
第2実施形態に係るロボット100は、移動障害物である人間のパーソナルスペースの内側領域の通過も想定し、人間に与える不快感の緩和を考慮しつつ、ロボット100と人間の不意の衝突をも回避可能な軌道計画を行えるようにしたところに特徴を有する。
すなわち、本実施形態のロボット100は、図20に示されるように、第1の実施形態の干渉時計画部29と構成の異なる干渉時計画部103を備えており、その他の構成は、前記第1実施形態と実質的に同一となっている。
この干渉時計画部103では、前記干渉度判定手段21で将来的な干渉度が「有」と判定されたときに、当該干渉が予測される人間を避けるためのロボット100の行動が計画される。
この干渉時計画部103は、図20に示されるように、現在位置でのロボット100が人間のパーソナルスペース内に存在するか否かを判定する接近状態判定部105と、ロボット100と人間が将来的に体幹接触する可能性の有無を判定する接触可能性判定部106と、ロボット100の人間への働きかけ行動に要する働きかけ準備時間の有無を判定する準備時間判定部107と、緊急時の衝突回避対応を行うロボット100の行動である緊急回避行動を選択する緊急回避行動選択部108と、緊急でない時の衝突回避対応を行うロボット100の行動である通常回避行動を選択する通常回避行動選択部109とを備えている。
(接近状態判定部の構成)
前記接近状態判定部105では、検出装置12での検出結果により、現時点において、第1実施形態と同様にして人間のパーソナルスペースの範囲が特定され、当該範囲内にロボット100のパーソナルスペースの少なくとも一部が侵入しているか否かが判定される。すなわち、現時点でロボット100のパーソナルスペースが、人間のパーソナルスペースの範囲内に位置する場合には、接近状態とされ、そうでない場合には、非接近状態とされる。
(接触可能性判定部の構成)
前記接触可能性判定部106では、検出装置12での検出結果に基づき、第1実施形態と同様にロボット100が人間に最接近した際の相対距離が求められ、当該相対距離の閾値を基準に、ロボット100の体幹部分が人間に接触する体幹接触の可能性の有無が判定される。すなわち、ここでは、前記相対距離が、予め特定されたロボット100及び人間の各体幹幅の合計よりも短い場合に、前記体幹接触の可能性が「有」とされ、そうでない場合に、前記体幹接触の可能性が「無」とされる。
(準備時間判定部の構成)
前記準備時間判定部107では、次のようにして働きかけ準備時間の有無が判定される。ここでは、検出装置12での検出結果から、第1実施形態と同様に、現時点において、ロボット100と人間との相対距離とそれらの速度ベクトルが特定されることにより、それらが相互に干渉するまでにかかる時間が算出される。そして、当該時間が、働きかけ行動等に必要となる各種動作(アーム15や声掛けの動作等)のために予め設定された働きかけ準備時間と対比され、当該働きかけ準備時間よりも短い場合に準備時間「無」とされ、当該働きかけ準備時間よりも長い場合に準備時間「有」とされる。
(緊急回避行動選択部の構成)
前記緊急回避行動選択部108では、現時点でロボット100のパーソナルスペースが人間のパーソナルスペース内に存在する接近状態の場合と、そうでない非接近状態であり、且つ、働きかけ行動のための準備時間「無」の場合とにおいて、後述する各種の緊急回避行動の中から所定の行動が選択される。
この緊急回避行動選択部108は、人間に接触させずに、人間のパーソナルスペース内に侵入して目的地までロボット100を進行させる接近経路における軌道である接近軌道を生成する接近軌道生成機能111と、ロボット100をこれまでと逆方向に進行させる逆移動経路における軌道である逆方向軌道を生成する逆方向軌道生成機能112と、ロボット100を一旦停止させる一旦停止機能113とを有する。
前記接近軌道生成機能111においては、接近状態判定部105、接触可能性判定部106、準備時間判定部107での判定結果に基づく次の各場合に、前記接近軌道が生成される。すなわち、第1の場合として、現時点でロボット100が人間のパーソナルスペース内に存在する接近状態のときに、それらの体幹接触の可能性が「無」であり、且つ、これまでロボット100から何らかの働きかけ行動をしている場合が挙げられる。また、第2の場合として、前記非接近状態のときに、働きかけ行動のための準備時間が「無」で、且つ、これまでロボット100から何らの働きかけ行動をしている場合が挙げられる。
この接近軌道生成機能111では、前記第1実施形態における定速軌道生成機能48(図6参照)と同様の演算処理により、ロボット100が定常速度で移動する接近軌道が生成される。具体的に、ここでは、前述の定速軌道生成機能48における上式(17)での譲合度PRを100%としている。また、上式(17)に代入される基準距離Lbを、ロボット10と人間の各パーソナルスペースの半径の和であるRLR+RLHとしていたところ、RLR+RLHよりも短く、且つ、人間の体幹幅よりも長い所定距離としている。これにより、ロボット100が人間のパーソナルスペースに侵入しながら、人間に接触することなくロボット100と人間とのすれ違いが可能な接近軌道が生成される。図21に例示されるように、この接近軌道は、人間Hの左右両側でのすれ違いを可能とするように、2種類の接近経路PT1の中から、移動距離の少ない方の軌道が選択される。また、固定障害物等の存在により、何れか一方の接近経路PT1でロボット100の通過が不可能となる場合には、他方の接近経路PT1における接近軌道が選択される。なお、何れの接近軌道でもロボット100の通過が不可能となる場合は、逆方向軌道生成機能112による逆方向軌道による移動か、一旦停止機能113での一旦停止の何れかの行動が選択される。
前記逆方向軌道生成機能112では、接近状態判定部105、接触可能性判定部106、準備時間判定部107での判定結果に基づく次の各場合に、人間から遠ざかるようにロボットを逆方向に方向変換移動させる逆方向軌道が1種類生成される。すなわち、ここでの第1の場合として、現時点でロボット100が人間のパーソナルスペース内に存在する接近状態のときに、それらの体幹接触の可能性が「無」であり、且つ、これまでロボット100から働きかけ行動が何らされていない場合が挙げられる。また、第2の場合として、前記接近状態のときに、それらの体幹接触の可能性が「有」の場合が挙げられる。更に、第3の場合として、前記非接近状態のときに、働きかけ行動のための準備時間が「無」で、且つ、これまでロボット100から働きかけ行動が何らされていない場合が挙げられる。
前記一旦停止機能113では、逆方向軌道生成機能112で生成される逆方向軌道が、経路上の固定障害物の存在等により、ロボット100の移動が不可能となる場合や、時間的に人間への体幹接触の回避が不可能であるような場合に、ロボット100を一旦停止させる行動が選択される。なお、その後、ロボット100を移動させながら、無干渉時経過部28(図1参照)若しくは干渉時経過部103での各種処理が再び行われる。
(通常回避行動選択部の構成)
前記通常回避行動選択部109では、現時点でロボット100のパーソナルスペースが人間のパーソナルスペースの外側に存在する非接近状態で、働きかけ行動のための準備時間「有」のときに、後述する各種の通常回避行動の中から所定の行動が選択される。この通常回避行動としては、ロボット100による働きかけ行動を伴いながら、接近軌道、迂回軌道、接触軌道の何れかの種類の軌道に沿ってロボット100を移動させる行動が挙げられる。
ここで、図21に示されるように、前述したように、接近経路PT1に基づく前記接近軌道は、ロボット100が人間のパーソナルスペースPSH内に侵入することを意図し、ロボット100が人間Hに接触しない最低距離を隔てて人間Hの横をすれ違いながら、目的地GPまでロボット100を進行させる軌道である。この接近軌道でのロボット10の移動に際しては、人間Hとの不意の衝突時に備えてアーム15を構える動作を行うとともに、ロボット100の接近を人間Hに知らせる声掛けを行う働きかけ行動がなされる。
また、前記迂回軌道は、図21の迂回経路PT2に基づく軌道である。すなわち、この迂回経路PT2は、人間Hへの働きかけにより、ロボット100と人間Hの各パーソナルスペースPSR,PSHが重ならないように、ロボット100のパーソナルスペースPSRが人間HのパーソナルスペースPSHに侵入しないことを意図し、ロボット100が人間HのパーソナルスペースPSHの外側を迂回する経路である。この迂回軌道での移動に際しては、前記第1実施形態で説明した譲合度PRを50%とする主張行動による働きかけ行動がなされる。
更に、前記接触軌道は、図21の接触経路PT3に基づく軌道である。この接触経路PT3は、アーム15を人間Hに接触させながらロボット100から離れる方向に人間Hを押すことにより人間Hを移動させ、移動後の人間Hの横をロボット100が通過しながら、目的地までロボット100を進行させる経路である。この接触軌道でのロボット100の移動に際しては、人間Hに対するロボット100の接近と接触予定等を人間Hに知らせる声掛けを行う働きかけ行動がなされるとともに、アーム15を使って人間Hに所定の押圧力を付加して人間Hの移動を促す動作がなされる。なお、ここでの接触軌道は、人間Hをロボット100と反対方向に所定量移動させることを意図しているが、図示しないセンサによって計測されるアーム15の反力が所定値以上になった場合は、人間Hが移動不能であると判断し、ロボット100の移動を一旦停止する等の行動を採ることもできる。
具体的に、前記通常回避行動選択部109は、ロボット100と人間Hの相対位置関係から、前記迂回経路PT2の設定可否を判定する迂回可否判定部115と、ロボット100が人間Hの横を通り抜ける通り抜け幅が有るか否かを判定する通り抜けスペース判定部116と、迂回軌道、接近軌道及び/又は接触軌道の中から、ロボット100を目的地に向って進行させる軌道候補を探索する軌道候補探索部117と、複数生成された軌道候補の中から最適となる軌道を選択し、実際にロボット10を移動させる実行軌道とする最適軌道抽出部118と、当該実行軌道の種別に対応した前述の働きかけ行動を特定する行動内容特定部119と、を備えている。
前記迂回可否判定部115では、現在のロボット100と人間との相対位置関係から、迂回経路PT2を物理的に計画できる相対距離があるか否かが判定される。つまり、先ず、前記第1実施形態における軌道候補探索部44と同様の演算処理により、各通過点(WP1〜WP3)が求められる。そして、固定障害物の存否に関わらず、当該各通過点に到達するために必要となるロボットの方向転換や速度調整が、予め特定されているロボット100の性能上、物理的に可能であるか否かが判定される。その結果、同可能の場合に迂回「可」され、同不可能の場合に迂回「不可」とされる。
前記通り抜けスペース判定部116は、前記第1実施形態の通り抜けスペース判定部31と同様にして、ロボット100が人間の横を実際に通り抜けるための幅である通り抜け幅があるか否かが判定される。すなわち、ロボット100が人間に最接近する際に、人間の横のスペースが前記通り抜け幅以上の幅を有する場合には、通り抜けスペース「有」とされ、そうでない場合には、通り抜けスペース「無」とされる。ここで、前記通り抜け幅としては、ロボット100の体幹幅に、その予備代を加算して得られた一定値が予め設定される。
前記軌道候補探索部117は、前記迂回軌道を生成する迂回軌道生成機能121と、前記接近軌道を生成する接近軌道生成機能122と、前記接触軌道を生成する接触軌道生成機能123と、これら各機能121〜123で生成された各軌道について、人間に対するロボット100の接近時のリスクである接近リスクに関する指標となる近接エネルギーを求める近接エネルギー算出機能124と、近接エネルギーによる接近リスク判定に基づき軌道候補を特定する軌道候補特定機能125とを有している。
前記迂回軌道生成機能121では、第1実施形態で説明した譲合度PRを50%とし、第1実施形態における軌道候補探索部44及び最適軌道抽出部45での処理と同様にして決定された1つの軌道が前記迂回軌道の軌道候補とされる。また、ここでは、後述する軌道候補特定機能125での接近リスク判定に利用するための基準となる迂回軌道である対比用の迂回軌道も求められる。この対比用の迂回軌道は、迂回経路PT2上においてロボット100の通過を阻害する壁等の固定障害物が無いと仮定し、同一の手順より1つに特定される。なお、ロボット100の周囲の環境によっては、対比用の迂回軌道が、固定障害物の存在を考慮して生成された迂回軌道と同一になる場合もある。
前記接近軌道生成機能122では、前記緊急回避行動選択部108の接近軌道生成機能111と同様にして接近軌道が生成されるが、ここでは、ロボット100の移動速度がそれぞれ異なる複数パターンの接近軌道が生成される。
前記接触軌道生成機能123では、前記第1実施形態における定速軌道生成機能48と同様の演算処理により、ロボット100が定常速度で移動する接触軌道が生成される。具体的に、ここでは、前述の定速軌道生成機能48における上式(17)での譲合度PRを100%にするとともに、上式(17)に代入される基準距離Lbとして、ロボット100の接触により人間を移動させる際の一定の目標移動量分を人間が移動したと仮定した位置において、ロボットが人間に接触しないように最低限必要となる距離が設定される。また、ここでは、ロボット100の移動速度が異なる複数パターンの接触軌道が生成される。
前記近接エネルギー算出機能124では、各軌道生成機能121〜123で求めた各軌道について、次式により前記近接エネルギーE
lが求められる。ここで、近接エネルギーE
lが大きいほど、前記接近リスクが高いと推定される。
上式において、Acは、第1実施形態と同様にして認識度推定手段22で求めた認知度に応じて特定される認知度をパラメータとした変数であり、認知度が「大」の場合と認知度が「小」、「無」の場合とで異なる値を採る。これら値は、予め設定されており、認知度が「小」、「無」の場合は、認知度が「大」の場合よりも、前記接近リスクが高いと推定され、大きな値が設定される。
また、上式のPnは、将来的な干渉が予測される人間に対する近寄り難さをパラメータとした変数である。すなわち、当該人間の年齢等の属性や、人間が何か物を持っている等の状況に応じて所定値が割り当てられるようになっている。この変数Pnは、図示省略したセンサ等による検出に基づいて、予め決められたルールにより所定値が割り当てられる他、遠隔地からの任意の値の入力等によって特定するようにしてもよい。
更に、上式のDrは、ロボット100が人間に最接近したときに、ロボット100に対する人間の向きをパラメータとした変数である。ここでは、検出装置12で検出された人間の顔の向きから、前記最接近時におけるロボット100に対する人間の向きが特定され、変数Drが算出される。この変数Drは、ロボット100が、人間の前方の正面位置に存在する状態から同後方の背面位置に存在する状態まで、ロボット100に対する人間の顔の向きを表す相対角度に応じて段階的に増大する値が特定される。つまり、人間の顔の真正面にロボット100が存在する状態から、ロボット100が人間の顔の側方から後方に存在する状態になるに従い、変数Drが徐々に増大するように設定される。このため、変数Drが大きくなる程、前記接近リスクが高くなる。
また、dは、ロボット100と人間が最接近した際のそれらの相対距離であり、ロボット100の各軌道に基づいて特定される。更に、vRは、ロボット100と人間が最接近した際のロボット100における人間方向の成分の速度であり、vHは、同最接近の際の人間におけるロボット100方向の成分の速度である。なお、k1とk2は、予め設定された一定の係数である。
以上の近接エネルギー算出機能124では、仮想的な迂回軌道である対比用の迂回軌道と、各パターンの接近軌道及び接触軌道のそれぞれについて、リスク判定の際の近接エネルギーElが求められる。
前記軌道候補特定機能125では、接近軌道、接触軌道についてそれぞれ求めた近接エネルギーElについて、基準となる対比用の迂回軌道における近接エネルギーElと同一若しくは同等の値の接近軌道及び接触軌道が抽出され、接近軌道と接触軌道の軌道候補とされる。つまり、ここでは、対比用の迂回軌道と同一若しくは同等の接近リスクを有する接近軌道と接触軌道が、軌道候補として特定される。なお、迂回軌道については、迂回軌道生成機能121で、固定障害物の存在をも考慮して第1実施形態での手順と同様に1つに特定された軌道が、軌道候補とされる。
以上の構成の軌道候補探索部117では、次のように、条件に応じて種類の異なる軌道候補が生成される。すなわち、迂回可否判定部115で迂回「可」と判定され、且つ、通り抜けスペース判定部116で通り抜けスペース「有」と判定された場合には、迂回軌道、接近軌道、及び接触軌道それぞれの軌道候補が生成される。また、迂回可否判定部115で迂回「不可」と判定され、通り抜けスペース判定部116で通り抜けスペース「有」と判定された場合には、接近軌道及び接触軌道それぞれの軌道候補が生成される。更に、迂回可否判定部115での迂回の可否に関わらず、通り抜けスペース判定部116で通り抜けスペース「無」と判定された場合には、接触軌道の軌道候補のみ生成される。
前記最適軌道抽出部118では、複数の軌道候補それぞれについて、第1実施形態の最適軌道抽出部45(図6参照)と同様にして、第1実施形態の干渉回避エネルギーErに相当する移動エネルギーErを算出し、移動エネルギーErの最も少ない軌道候補が、軌道コストの最も低い最適な実行軌道として抽出される。
すなわち、ここでは、迂回軌道及び接近軌道についての移動エネルギーErが、第1実施形態で用いられる上式(18)と同一のコスト式により求められる。
一方、接触軌道についての移動エネルギーE
rは、次式(20)の通り、ロボット100の接触による人間の移動量に対応するパラメータであるコストC・rを第3のエネルギー要素として、上式(18)の第1及び第2のエネルギー要素E
G,E
Dに更に加算されることで求められる。
なお、上式において、rは、ロボット100の接触により、人間をロボット100の反対側に移動させる所望量であり、予め一定値に設定される。Cは、当該エネルギー要素における重み付け定数であり、予め一定値に設定され、例えば、上式(18)における他のエネルギー要素の重み付定数A,Bよりも大きい値が設定される。
(干渉時計画部103での行動計画手順)
次に、前記干渉時計画部103でのロボット100の行動計画手順について、図22のフローチャートをも用いながら、以下に説明する。
先ず、接近状態判定部105で、現時点において、ロボット100のパーソナルスペースが人間のパーソナルスペース内に位置する接近状態か、そうでない非接近状態かが判定される(ステップS401)。
その結果、接近状態の場合には、接触可能性判定部106で、ロボット100の体幹部分が将来的に人間に体幹接触する可能性の有無について判定される(ステップS402)。ここで、体幹接触の可能性が「有」の場合には、緊急回避行動選択部108の逆方向軌道生成機能112で、人間から遠ざかるようにロボット100を逆方向に移動させる逆方向軌道が生成され、当該逆方向軌道に沿ってロボットを移動させる行動が計画される(ステップS403)。ここで、当該移動が不可能な場合には、一旦停止機能113で、ロボット100を一旦停止させる行動が計画される。
一方、接近状態で、前記体幹接触の可能性が「無」の場合には、これまでの何らかの働きかけ行動の有無に応じて異なる計画がなされる(ステップS404)。先ず、これまで働きかけ行動が行われていないときには、ロボット100の接近に対する人間Hの準備態勢が整っていないことが想定されることから、体幹接触の可能性「有」の場合と同一の行動が計画される(ステップS403)。すなわち、このときには、逆方向軌道が生成され、逆方向にロボット100が移動できないときにロボット100を一旦停止させる行動が計画される。逆に、既に働きかけ行動がなされているときには、人間の準備態勢がある程度整っていると想定される。そこで、緊急回避行動選択部108の接近軌道生成機能111で、人間の回避行動を期待せずに人間のパーソナルスペース内に侵入した状態で、ロボット100及び人間の体幹接触を回避する接近軌道が生成され、当該接近軌道に沿ってロボット100を移動させる行動が計画される(ステップS405)。
一方、現時点でロボット100が人間のパーソナルスペースの外側に位置する非接近状態の場合には、次の手順でロボット100の行動計画が決定される。
先ず、準備時間判定部107で、ロボット100の人間への働きかけ行動に要する働きかけ準備時間の有無が判定される(ステップS406)。
そして、準備時間「無」の場合には、前述した接近状態の体幹接触の可能性が「無」の場合と同様に、これまでの働きかけ行動の有無に応じて、緊急回避行動選択部108により、ロボット100の行動が計画される(ステップS403〜ステップS405)。
一方、準備時間「有」の場合には、通常回避行動選択部109で以下の手順の処理がなされる。
先ず、迂回可否判定部115により、現時点におけるロボット100と人間との相対位置関係から、迂回経路を物理的に計画できる相対距離があるか否かが判定される(ステップS407)。加えて、通り抜けスペース判定部116で、通り抜けスペースの有無が判定される(ステップS408、ステップS409)。
そこで、迂回「可」の場合には、軌道候補探索部117により、通り抜けスペースの有無に応じ、異なる種別の組み合わせの軌道候補が生成される。すなわち、通り抜けスペースが「有」のときには、迂回軌道、接近軌道及び接触軌道からなる軌道候補が生成される(ステップS410)。一方、壁等の固定障害物の存在等により、通り抜けスペースが「無」のときには、接触軌道からなる軌道候補が生成される(ステップS411)。
また、迂回「不可」の場合にも、軌道候補探索部117により、通り抜けスペースの有無に応じ、異なる種別の組み合わせの軌道候補が生成される。すなわち、通り抜けスペースが「有」のときには、接近軌道及び接触軌道からなる軌道候補が生成される(ステップS412)。一方、通り抜けスペースが「無」のときには、接触軌道からなる軌道候補が生成される(ステップS411)。
そして、得られた各軌道候補について、最適軌道抽出部118でのコスト計算により、最も低い軌道コストの軌道候補が実行軌道とされ、行動内容特定部119にて、実行軌道の種別に応じた働きかけ行動が特定される(ステップS413)。
以上の行動計画処理は、ロボット100が目的地に到達するまで、一定時間毎に繰り返し行われる(ステップS414)。
以上の第2実施形態によれば、ロボット100の進路における死角等の存在により、突然、人間のパーソナルスペースにロボット100が侵入してしまう等の不慮の事態が生じても、相互の不意の体幹接触を回避しながら、人間の心理面を考慮して目的地への効率の良いロボット100の移動が可能になる。また、人間に不快感を極力与えずに、ロボット100が人間のパーソナルスペースに侵入した状態で、人間とすれ違いさせる経路を選択することができ、相互の衝突回避のためにより効率的なロボット100の行動計画を策定することができる。
なお、本発明に係るロボットとしては、前記各実施形態で説明した自律移動型のロボット10,100に限定されるものではなく、自動車両、船舶、飛行体等、所定の空間内を自律的に移動可能な移動体の他に、所定範囲の空間内で動作するロボットアーム等のマニピュレータであってもよく、これらの動作計画を前述の通りに行うことができる。
また、本発明の移動障害物としては、前記各実施形態で説明した人間の他に、自転車、自動車、他の自律移動ロボット等の移動体や動物等との干渉回避の制御にも適用できる。この場合、移動障害物の認識度を推定する際において、推定対象が人間の操縦による移動体の場合には、例えば、前述と同様に、操縦者の認知状態に基づいて行うことが可能である。また、推定対象が他のロボットのように人間でない場合には、認識状態検出センサ19を含む検出装置12として、当該他のロボット等が周囲環境を認識するためのセンサ、カメラ等が設けられた部位における相対方向を認識状態として検出し、その検出結果に基づいて、前述と同様の手順で認識度を推定することを例示できる。
その他、本発明における装置各部の構成は図示構成例に限定されるものではなく、実質的に同様の作用を奏する限りにおいて、種々の変更が可能である。