JP7525854B2 - ロボット、制御装置及びそのプログラム - Google Patents

Description

本発明は、ロボット及びその制御装置、並びに、移動範囲推定装置及びそのプログラムに係り、更に詳しくは、人間等の移動障害物との将来的な干渉を効率良く回避するロボット及びその制御装置と、それらに適用され、移動障害物の将来的な移動範囲を予測する移動範囲推定装置及びそのプログラムとに関する。

近時において、人間との共存環境下で自律的に移動可能となるロボットが出現している。当該ロボットとしては、所定のスタート地点からゴール地点まで自律移動する際に、周囲の人間を含む障害物の状況をセンシングし、当該障害物との将来的な干渉を回避しながら移動するものがある。このようなロボットにおいては、単に一方的に人間を避けるだけではなく、人間の動きや他の障害物等に配慮し、干渉を効果的に避けるための人間との協調的な動作が要求される。すなわち、当該環境下での移動時には、干渉回避動作や停止動作等、人間の動作に対して受動的な動作に加え、当該動作を通じてロボットの行動意図を人間に伝達し、人間自身の干渉回避行動を能動的に働きかけることが必要となる。そこで、本発明者らは、自身の研究成果として、前述の働きかけを踏まえた経路計画に基づいて自律移動可能なロボットを種々提案している（特許文献１、２等参照）。

前述の働きかけを行う際のロボットの移動経路を生成するためには、ロボットの周囲を移動する歩行者等の存在やその軌跡、移動速度等を検出する必要がある。当該移動速度の検出は、ロボットに、周囲環境の位置情報を取得するレーザレンジファインダ（ＬＲＦ）等のセンサを設け、当該位置情報に基づく演算により推定される。そこで、本発明者らは、歩行者の周期振動による推定誤差を抑制することで、歩行者の位置推定精度を向上するとともに、負担の少ない演算処理により、位相遅れを抑制して応答性を高めることができる歩行者の速度推定手法を既に提案している（特許文献３参照）。

特開２０１９－８４６４１号公報 特開２０２０－４６７５９号公報 特開２０２０－９１２３５号公報

前記特許文献１、２でのロボットの経路生成の際には、現時刻における歩行者の位置及び速度ベクトルにより、歩行者とロボットの将来的な干渉の可能性を判断し、当該干渉が予測される場合に、歩行者への働きかけを行うようにロボットの経路計画を行うようになっている。ここでは、歩行者の現在の速度ベクトルが将来も維持されるという前提の下、歩行者の速度ベクトルの予測モデルを等速直線運動として、干渉回避のためのロボットの移動経路の生成がなされる。この際、ロボットでは、歩行者の将来の予測位置が時系列で一義的に特定され、前記干渉の可能性ある場合に、それらの将来的な干渉地点が一箇所に推定される。

しかしながら、実際の人間の速度ベクトルは、このような単純モデルでは表されない外乱が多々存在する。第１の外乱として、歩行者と離れた位置のロボットに搭載されたセンサを利用するため、その検出値には測定誤差が含まれる。この測定誤差は、現時刻からの時間が経過する程、誤差範囲が拡大し、歩行者の将来的な位置が曖昧となる。また、第２の外乱としては、将来の速度ベクトルが現在の値を維持する確率に対応する保持可能性がある。つまり、例えば、歩行者が一定の速度でこれまで長い距離を直進しているようなケースでは、将来的に予測される速度ベクトルは、現在の数値から大きく変動しないと考えられる。一方で、歩行者がジグザグ歩行し、或いは、曲がり角を曲がる等、これまで一定の方向に進行していないようなケースでは、将来的に予測される速度ベクトルは、現在の数値から大きく変動すると考えられる。従って、歩行者の将来的な速度ベクトルは、前述の測定誤差やこれまでの遷移状態により、将来時刻にて予測される歩行者の移動範囲の大きさが変化することになる。

以上のように、歩行者の現時点での位置情報に基づいて算出した現時点でのその速度ベクトルでは、前述した測定誤差と保持可能性の要素を考慮すると、歩行者の将来的な予測位置を一点に決定することは出来ず、当該予測位置が所定範囲に亘ることになる。前記特許文献１、２でのアルゴリズムでは、人間とロボットの将来的な干渉の可能性が想定されると、干渉回避のために移動経路を生成している。ところが、歩行者の速度ベクトルの予測モデルを等速直線運動としているため、当該予測モデルでロボットの経路計画をしてしまうと、歩行者の将来的な移動範囲が大きくなる程、その後の所定時間毎の同様の処理において、当該移動経路のリプランニングが頻発してしまう。つまり、不確定な情報である先の速度ベクトルに基づき、干渉回避のための経路計画をしても、歩行者の速度ベクトルが進路変更等により変化した際に、再度、歩行者とロボットの干渉の可能性が出現し、再度、経路計画をし直す必要がある。従って、将来的な歩行者の速度ベクトルが不確実である程、従前の経路計画をリセットして再設定する作業が何度も繰り返し行われるため、経路計画に際する処理が非効率となる。加えて、前述した従来のアルゴリズムでは、ロボットが必要最低限の回避しか行わないため、想定に対して人間の速度ベクトルが少しでも変化すると、更に歩行者に接近した時点で再度干渉する可能性が生じ、ロボットをより大きく回避させる必要が生じ得る場合がある。

本発明は、このような課題に着目して案出されたものであり、その目的は、移動障害物の将来的な予想位置のブレを想定し、移動障害物との干渉を効率良く回避できるロボット及びその制御装置と、それらに適用可能な移動範囲推定装置及びそのプログラムを提供することにある。

前記目的を達成するため、本発明は、主として、周囲に存在する移動障害物の位置情報及び速度情報を検出する検出装置と、当該検出装置の検出結果から、前記移動障害物との将来的な干渉を回避するための働きかけを行う制御装置とを備え、前記干渉を回避しながら自律移動可能なロボットにおいて、前記制御装置は、前記働きかけの要否を判定する働きかけ要否判定手段と、当該働きかけ要否判定手段で前記働きかけが必要と判定されたときに、前記働きかけを実行するようにロボットを動作制御する働きかけ実行手段とを備え、前記働きかけ要否判定手段は、将来の所定時刻にて前記移動障害物が存在する位置の確率分布を表す予測分布領域を求める予測分布領域導出部と、前記移動障害物とロボットの将来的な相対位置関係から、前記予測分布領域を用いて、前記干渉が発生する確率である干渉確率を特定する干渉確率特定部と、当該干渉確率を所定の閾値と対比して前記働きかけの要否を決定する要否決定部とを備える、という構成を採っている。

本発明では、予測分布領域により、所定の将来時刻において、移動障害物の将来的な移動範囲が推定され、その移動範囲内における各地点での存在確率が特定される。この存在確率により、移動障害物の将来的な移動における外乱に応じた干渉確率が導出され、当該干渉確率の大きさに応じて働きかけの要否が決定される。これにより、移動障害物が等速直線運動する予測モデル上で、将来的にロボットとの干渉可能性が無いと推定されるような場合でも、干渉確率が閾値よりも高ければ、効率的な移動を行うための早めの働きかけが可能となる。一方で、前記予測モデル上で干渉可能性が有ると推定されるような場合でも、干渉確率が閾値よりも低ければ、今後、移動障害物に大きなブレが想定されるとして、現時点での働きかけを見送って一旦様子見することが可能となる。以上により、本発明によれば、移動障害物の将来的な存在範囲のブレに対応し、ロボットの移動経路生成時のリプランニングを最小限にして移動障害物との干渉を効率良く回避でき、移動障害物の移動における外乱の影響を最小限に抑えた移動制御システムのロバスト化を図ることができる。

また、回避量調整部により、ロボットと移動障害物との干渉を回避するために必要な回避量として、移動障害物の将来的な移動範囲を考慮した余裕分を加算することができ、この点からも、ロボットの移動経路生成時のリプランニングを最小限にすることができる。

本実施形態に係るロボットの移動制御に関連する構成のみを概略的に表したブロック図である。 予測分布領域を説明するための概念図である。 相対座標系及び相対座標系における予測分布領域を説明するための図である。 パーソナルスペースを説明するための概念図である。 （Ａ）は、第１のケースにおける条件値の算出手順を説明するための概念図であり、（Ｂ）は、第２のケースにおける条件値の算出手順を説明するための概念図である。 （Ａ）～（Ｃ）は、ズレ幅を求める際の手順を説明するための概念図である。

以下、本発明の実施形態について図面を参照しながら説明する。

図１には、本実施形態に係るロボット１０の移動制御に関連する構成のみを概略的に表したブロック図が示されている。この図において、前記ロボット１０は、予め設定された移動開始地点（スタート地点）から所定の目標地点（ゴール地点）まで自律的に移動する移動体として機能する。このロボット１０では、移動途中で壁等の物体や人間等からなる障害物との将来的な干渉が予想される場合に、これら障害物を避けながら進行する移動経路として、周囲の人間等への影響を軽減して効率良く移動可能な移動経路を探索し、当該移動経路に沿った動作制御がなされる。

ここでの障害物としては、ロボット１０の移動経路の周辺に存在する壁等、環境中で常時静止した状態の固定障害物と、人間、動物、ロボット等、環境中で移動可能となる各種の移動体からなる移動障害物とがある。本実施形態においては、歩行状態の人間（歩行者）を移動障害物とし、当該歩行者とロボット１０との将来的な干渉を回避するためのロボット１０の動作制御について説明するが、他の移動体に対しても、後述と同様の処理によるロボット１０の動作制御が可能となる。このロボット１０では、歩行者が接近する方向に移動する際に、歩行者との干渉を回避するための働きかけを行うように移動経路を生成し、各種部位の動作制御がなされる。なお、特に限定されるものではないが、この働きかけは、前記干渉回避のための歩行者の協調移動を促すことを意図して行われるが、当該協調移動を期待しないロボット１０の単なる干渉回避動作も含まれる。

前記ロボット１０には、図１に示されるように、各種動作を可能に構成された機構や機器からなる動作部１１と、ロボット１０の周囲の環境情報を検出する検出装置１２と、検出装置１２の検出結果に基づき、前記障害物の状況を考慮しながらロボット１０の自律移動制御を行う制御装置１３とが設けられている。

前記動作部１１は、所定範囲内でロボット１０を自律移動させるための機構やその動力源からなる移動装置１４を含む。当該移動装置１４を含む動作部１１については、全て公知の部材、機構、装置類等から構成されており、本発明の本質部分でないため、各構成についての詳細な図示説明を省略する。

前記検出装置１２は、ロボット１０の周囲に存在する歩行者その他の障害物の位置情報を検出する位置検出部１６と、位置検出部１６での検出結果に基づき、歩行者の速度情報を検出するコンピュータからなる速度検出部１７とを備えており、所定時間毎に前記位置情報及び速度情報を取得可能となっている。

前記位置検出部１６は、特に限定されるものではないが、ロボット１０からその周囲へのレーザ光の照射による人間を含む物体の反射状態に基づいて、ロボット１０の周囲の物体の各表面部分の位置情報を検出する公知のレーザレンジファインダ等の測域センサにより構成される。この測域センサでは、人間等の表面部分を構成する点群における各点の平面視での２次元位置座標が所定時間毎に検出され、各時刻における検出結果から、公知の手法を利用して人間の仮想体幹の当該時刻における位置が特定され、当該仮想体幹における位置情報を観測値とするようになっている。ここで、時系列データでの仮想体幹の位置情報の変位が所定の閾値未満である場合には、同一の歩行者であるとし、同一のＩＤが割り当てられる。そして、当該各歩行者について、取得した時系列の位置情報の変化から、速度検出部１７で速度情報が求められることになる。

なお、前記位置検出部１６としては、前述のレーザレンジファインダやその他の測域センサに限定されるものではなく、歩行者の位置情報を検出できる限りにおいて、様々なセンサや装置類によって構成することもできる。

前記速度検出部１７では、本発明者らが既に提案した手法（特開２０２０－９１２３５号公報）により、歩行者の位置情報を取得した時刻毎に、当該位置情報から、対応する歩行者のその位置での速度情報が算出される。なお、当該速度情報を導出するアルゴリズムについては、本発明の本質要素ではないため、詳細な説明を省略する。また、本発明においては、位置検出部１６で検出した歩行者の位置情報からその速度情報を求めることができれば、前述の手法に限定されるものではなく、他の公知手法等により前記速度情報を導出可能な他のセンサ類、装置類、システム類等を適用することができる。

検出装置１２で検出される本実施形態における位置情報としては、平面視における直交２軸方向の座標成分からなる位置ベクトルが用いられ、同速度情報としては、進行方向を表す回転角の成分と、速さの成分とからなる速度ベクトルが用いられる。

以上のように、歩行者の位置情報及び速度情報を含む移動情報は、検出装置１２で逐次検出され、その検出タイミングで制御装置１３に伝送されるとともに、取得した時刻に対応させて制御装置１３にログデータとして記録される。

前記制御装置１３では、予め設定されたスタート地点から所定のゴール地点まで、歩行者との干渉を回避しながらロボット１０が自身の存在位置と速度ベクトルに基づいて自律移動可能となるように、検出装置１２での移動情報の取得タイミング毎に働きかけの要否が判定される。そして、働きかけが必要と判断された場合に最適となる移動経路を探索し、動作部１１への動作指令が行われる。

この制御装置１３は、ロボット１０に一体的に或いは別体として設けられており、ＣＰＵ等の演算処理装置及びメモリやハードディスク等の記憶装置等からなるコンピュータによって構成されている。当該コンピュータには、以下の各手段として機能させるためのプログラムがインストールされている。

次に、前記制御装置１３の具体的構成について説明する。

前記制御装置１３は、ロボット１０が、その周囲を移動する歩行者に将来的に干渉する可能性が有る場合に、当該干渉を回避するための歩行者への働きかけの要否を判定する働きかけ要否判定手段１９と、働きかけ要否判定手段１９で働きかけが必要と判定されたときに、干渉回避に好適となる働きかけを実行するようにロボット１０の動作制御を行う働きかけ実行手段２０とを備えている。

前記働きかけ要否判定手段１９は、将来の所定時刻にて、干渉判断の対象となる歩行者（以下、「対象歩行者」と称する）が存在する位置の確率分布を表す予測分布領域を求める予測分布領域導出部２２と、対象歩行者がロボット１０に接近する将来時刻での予測分布領域を用いて、それらが相互に干渉する確率である干渉確率を特定する干渉確率特定部２３と、当該干渉確率を所定の閾値と対比して前述の働きかけの要否を決定する要否決定部２４とを備えている。

ここで、前記予測分布領域について説明する。前提として、将来の所定時刻における対象歩行者の位置予測は、現在（現時刻）に検出された速度ベクトルに基づき、等速直線運動モデルで算出された座標が最も確率が高く、その地点から離れる程、確率が低くなる。従って、図２に概念的に示されるように、予測分布領域Ｇは、所定の将来時刻における対象歩行者Ｈの中心座標を等速直線運動モデルで算出し、領域の範囲を進行方向速度Ｖと回転角速度θを２軸とした二変量ガウス分布として、対象歩行者Ｈの存在確率が表される。

前記働きかけ要否判定手段１９での以下の各種処理においては、図３に示されるように、対象歩行者Ｈを基準としてロボット１０の相対位置を表す相対座標系が採用される。この相対座標系は、所定時刻において、等速直線運動モデルで算出された対象歩行者Ｈの位置を原点とし、当該モデルにおける進行（直進）方向（同図中矢印方向）をｙ軸（ｍ）とし、当該進行方向に直交する方向をｘ軸（ｍ）として定義される。

前記予測分布領域導出部２２では、現時刻からｔ_ｎ時間（秒）における将来時刻での二変量ガウス分布として、次式による確率密度関数ｆ_ｔｎ（Ｐ，Σ）が用いられ、図３に概念的に示すように、現時点からｔ_ｎ時間（秒）後における対象歩行者Ｈの予測分布領域Ｇが特定される。なお、この予測分布領域Ｇにおける外縁部分の外側は、対象歩行者Ｈの存在確率が限りなくゼロに近い部分とされる。

上式において、Ｐは、前記相対座標系における対象歩行者Ｈの座標内位置である任意のｘ，ｙ座標であり、Σは共分散行列を意味する。共分散行列Σは、確率変数の散らばり具合を表すものであり、一次元のガウス分布の分散を多次元に拡張したものである。ｘ軸、ｙ軸に対して分散をとると、現時刻からｔ_ｎ時間後の共分散行列Σは、次式のように表される。

上式において、σ_ｘは、ｘ軸方向における標準偏差（ｍ／ｓ）であり、σ_ｙは、ｙ軸方向における標準偏差（ｍ／ｓ）である。つまり、σ_ｘ ^２は、進行時の方向の変化量に対応して変化し、σ_ｙ ^２は、速さの変化量に対応して変化する。

ここで、予測分布領域Ｇの範囲は、位置検出部１６の機器上の測定誤差に関する要素と、対象歩行者Ｈの将来の速度ベクトルが現在の値を維持する確率に対応する保持可能性に関する要素とに応じて変化する。つまり、予測分布領域Ｇの範囲を決定する標準偏差（σ_ｘ，σ_ｙ）は、次式に示すように、速度ベクトルの測定誤差に関する標準偏差と、対象歩行者Ｈの保持可能性に関する標準偏差を総合した和により決定される。

上式において、σ_ｓθは、測定誤差のｘ軸方向における標準偏差（ｍ／ｓ）であり、σ_ｓｖは、測定誤差のｙ軸方向における標準偏差（ｍ／ｓ）である。また、σ_Δθは、保持可能性のｘ軸方向における標準偏差（ｍ／ｓ）であり、σ_ΔＶは、保持可能性のｙ軸方向における標準偏差（ｍ／ｓ）である。

ここで、前記測定誤差の標準偏差（σ_ｓθ，σ_ｓｖ）は、次式により求められる。

上式において、Ｅ_θは、予め行った実験によって特定された特定値（例えば、５．６４）であり、Ｅ_Ｖは、予め行った実験によって特定された特定値（例えば、０．１４）であり、これらＥ_θ、Ｅ_Ｖは、ロボット１０の周囲の環境中の人間等の移動状況に応じて定まる値となっている。また、Ｖ_Ｈｔ０は、現時刻ｔ_０における対象歩行者Ｈの速さである。

また、前記保持可能性の標準誤差（σ_Δθ，σ_ΔＶ）は、現時刻から過去の所定タイミング毎に取得された対象歩行者Ｈの移動情報の遷移状態に基づき、当該移動情報のログデータを利用して次式により求められる。

上式において、Ｎは、ログデータの窓長であり、この窓長は、検出装置１２での移動情報の検出タイミングとなる各ステップのうち、現在での演算処理がなされる現時刻ｔ_０でのステップから過去に遡るステップ数を意味する。以下、現時刻ｔ_０でのステップｋからその１つ前の直前ステップを、ステップｋ－１と表し、その更に１つ前のステップをｋ－２と表し、現時刻ｔ_０でのステップｋからｉ個前のステップをステップｋ－ｉと表す。なお、特に限定されるものではないが、本実施形態の窓長Ｎは、人間の１歩行周期の１／２の時間に相当する２０ステップに設定される。また、θ_{Ｈ（ｋ－ｉ）}は、現時刻ｔ_０でのステップｋからｉステップ前の対象歩行者Ｈの回転角速度（ｒａｄ／ｓ）であり、Ｖ_{Ｈ（ｋ－ｉ）}は、現時刻ｔ_０でのステップｋからｉステップ前の対象歩行者Ｈの速さ（ｍ／ｓ）である。また、θ_ａｖは、Ｎステップ分のログデータにおける平均回転角速度（ｒａｄ／ｓ）を表し、Ｖ_ａｖは、Ｎステップ分のログデータにおける平均速さ（ｍ／ｓ）を表す。

前記干渉確率特定部２３は、図１に示されるように、干渉確率を求める際の各種の条件値を算出する条件値算出部２６と、当該条件値から、予測分布領域Ｇを特定する前述の数式を用いて干渉確率を算出する確率算出部２７とを備えている。

前記条件値算出部２６では、後述するように、相対的に移動するロボット１０及び対象歩行者Ｈの将来的な接近状態を推定し、当該接近状態における所定の接近地点の位置情報と、その接近状態における将来の接近時刻までの現時刻からの経過時間である時間情報とが、前記条件値として数学的に算出される。

ここで、接近状態の特定に際しては、図４に示されるように、ロボット１０の位置を中心にして仮想的に設定した総合パーソナルスペースＰＳが用いられる。この総合パーソナルスペースＰＳは、ロボット１０の横幅と人間の体幹幅に対して、それぞれ定数Ｋ（Ｋ＞１）倍した半径Ｌ_ＲＰ、Ｌ_ＨＰ有する仮想的な円形のパーソナルスペースＳ_Ｒ，Ｓ_Ｈ（同図中破線部分）を合計した円形領域である。すなわち、総合パーソナルスペースＰＳの半径Ｌ_ＰＳは、各パーソナルスペースＳ_Ｒ，Ｓ_Ｈの半径Ｌ_ＲＰ、Ｌ_ＨＰを合計した値となり、特に限定されるものではないが、本実施形態の半径Ｌ_ＰＳは、前記半径Ｌ_ＲＰ、Ｌ_ＨＰをそれぞれ０．５ｍとし、それらの合計１ｍに設定される。この総合パーソナルスペースＰＳは、所定時刻において、その内側領域に対象歩行者Ｈが存在する場合には、各パーソナルスペースＳ_Ｒ，Ｓ_Ｈの干渉・非干渉を判定する境界となるロボット１０と対象歩行者Ｈの相対距離Ｌ_ＰＳ以下となり、それらが相互に干渉すると判断されることになる。

前記条件値算出部２６では、次の手順で前記条件値が求められる。すなわち、先ず、相対座標系における現時刻でのロボット１０の相対位置ベクトル及び相対速度ベクトルを用いて、総合パーソナルスペースＰＳの将来の移動状態が推定される。そして、当該将来の移動状態に応じて、総合パーソナルスペースＰＳが対象歩行者Ｈに接近する所定のタイミングにおける接近地点の位置情報と、当該タイミングにおける接近時刻である時間情報とが条件値として求められる。

ここで、前記条件値の算出は、対象歩行者Ｈが原点に存在する相対座標系において、将来のどのタイミングでも、原点の対象歩行者Ｈが総合パーソナルスペースＰＳの範囲内に存在しない常時非干渉の第１のケースと、将来における所定のタイミングで、原点の対象歩行者Ｈが総合パーソナルスペースＰＳの範囲内に存在し得る干渉可能性有の第２のケースとに分けて行われる。つまり、第１のケースは、対象歩行者Ｈとロボット１０の将来的な相対移動予測によるそれらの相対距離が、予め設定された総合パーソナルスペースＰＳの半径Ｌ_ＰＳを常時下回らないと推定される場合である。一方、第２のケースは、同相対距離が、総合パーソナルスペースＰＳの半径Ｌ_ＰＳを下回る時があると推定される場合である。

前記第１のケースでは、前述した予測分布領域の概念から、ある将来時刻において、相対座標系の原点から離れた位置程、対象歩行者Ｈの存在確率が低下するが、当該存在確率を干渉確率としたときに、当該干渉確率が最大となるのは、図５（Ａ）の破線で示されるように、総合パーソナルスペースＰＳが原点に最も近づいたときである。そこで、この最接近状態のときの相対位置情報及び時間情報が、次のように求められる。

つまり、図５中実線の総合パーソナルスペースＰＳを有する現時刻のロボット１０の相対位置から、同図中破線の総合パーソナルスペースＰＳの位置にロボット１０が相対移動した最接近状態において、その際の総合パーソナルスペースＰＳの中心点Ｃ（ロボット１０の相対位置）が特定される。ここで、当該最接近状態では、直線で表されるロボット１０の相対移動軌跡Ｍに対し、相対座標系の原点と中心点Ｃとを結ぶ直線ｌが直交することになり、当該条件から、直線ｌの長さ、すなわち、最接近状態での中心点Ｃと原点との直線距離ｄ_ＲＨが求められる。この直線距離ｄ_ＲＨは、相対座標系における現時刻のロボット１０の相対位置ベクトルと、現時刻における対象歩行者Ｈとロボット１０の相対回転角とから数学的に算出される。

そして、相対座標系において、最接近状態での総合パーソナルスペースＰＳの領域内で、原点に最も近い地点が前記接近地点ＣＰとされ、その位置ベクトルＰ_ＣＰＡが前記位置情報として求められる。この位置ベクトルＰ_ＣＰＡは、前記直線距離ｄ_ＲＨと総合パーソナルスペースＰＳの半径Ｌ_ＰＳとの差分と、現時刻における対象歩行者Ｈ及びロボット１０の相対回転角とにより数学的に算出される。

更に、前記時間情報として、現時刻から、最接近状態での総合パーソナルスペースＰＳの位置にロボット１０が到達する接近時刻までの経過時間Ｔ_ＣＰＡが、現時刻及び接近時刻におけるロボット１０の相対位置ベクトルと、現時刻における対象歩行者Ｈとのロボット１０の相対速さとから数学的に算出される。

前記第２のケースでは、図５（Ｂ）に示されるように、総合パーソナルスペースＰＳの周縁が相対座標系の原点に接したときが最も早い時刻での接近となる最も早い接近状態であり、この状態におけるロボット１０の予測位置Ｃが接近地点ＣＰとされる。従って、位置情報である接近地点ＣＰの位置ベクトルＰ_ＣＰＡは、相対座標系における最も早い接近状態でのロボット１０の位置ベクトルとなる。また、時間情報である前記経過時間Ｔ_ＣＰＡは、現時刻から最も早い接近状態に達する将来時刻までの時間とされる。これら位置ベクトルＰ_ＣＰＡ及び経過時間Ｔ_ＣＰＡは、相対座標系における現時刻におけるロボット１０の位置ベクトルと、速度ベクトル（相対回転角及び相対速さ）とから数学的に算出される。

前記確率算出部２７では、予測分布領域を特定する前述の確率密度関数に、接近地点ＣＰの位置ベクトルＰ_ＣＰＡ及び経過時間Ｔ_ＣＰＡを代入することで得られた数値が干渉確率として求められる。すなわち、干渉確率は、ロボット１０の総合パーソナルスペースＰＳが対象歩行者Ｈに対して前記第１のケースの最接近状態となるとき、若しくは、前記第２のケースの最も早い接近状態となるときのそれぞれの接近地点ＣＰにおける対象歩行者Ｈの予測分布領域での確率となる。このため、前述の式（１）～（５）を使い、これらの式には、位置ベクトルＰとして、接近地点ＣＰの位置ベクトルＰ_ＣＰＡが代入され、現時刻からの時間ｔ_ｎとして、前記経過時間Ｔ_ＣＰＡが代入され、干渉確率が求められる。

前記要否決定部２４は、図１に示されるように、現時刻のロボット１０と対象歩行者Ｈの相対関係に応じて、働きかけのタイミングの基準となる閾値を決定する閾値決定部２９と、干渉確率特定部２３で特定された干渉確率が閾値決定部２９で求めた閾値よりも大きいときに、干渉回避のためのロボット１０の働きかけが必要として、当該働きかけを行うように働きかけ実行手段２０に指令する働きかけ指令部３０とを備えている。

前記閾値は、ロボット１０による働きかけを必要と判断する最小の干渉確率とし、各種の状況に応じて可変になるように、干渉確率特定部２３で干渉確率が求められるタイミング毎に、次のように決定される。

つまり、例えば、対象歩行者Ｈがロボット１０に正面側から衝突すると想定される場合には、対象歩行者Ｈの予測位置に多少のズレが生じたとしても、それらの各パーソナルスペースが相互に干渉する可能性が高い。従って、このような場合には、対象歩行者Ｈの予測位置のズレ幅が極端に大きくない限り、各パーソナルスペースの干渉を回避する可能性が低く、働きかけが必要となる可能性が高くなることから、閾値を低くしてより早く干渉回避する必要がある。一方、対象歩行者Ｈとロボット１０の相対関係から、それらが衝突するかどうか微妙な場合には、対象歩行者Ｈの予測位置に少しでもズレが生じると、それらのパーソナルスペースが相互に干渉しない可能性が高い。従って、このような場合には、現時刻で働きかけを必要とする可能性が低く、働きかけがなされると対象歩行者Ｈの予測位置のズレに対して敏感に干渉状態が変動してしまうため、働きかけのタイミングを遅らせるように閾値を高く設定して、現状のロボット１０の移動経路で一旦様子見をすることが有用である。

そこで、前記閾値決定部２９では、現時刻における対象歩行者Ｈ及びロボット１０の相対関係から、それらの各パーソナルスペースが将来的に干渉すると予測される前述の第２のケースにおいて、当該干渉を回避するために必要な最小のズレ幅ΔＤが算出され、当該ズレ幅ΔＤに応じて、以下のように閾値が算出される。当該閾値は、前述の考え方により、ズレ幅ΔＤが大きい程、働きかけを行い易くするように、低く算出されることになる。一方で、対象歩行者Ｈ及びロボット１０の各パーソナルスペースが将来的に干渉しないと予測される前述の第１のケースの場合には、前記ズレ幅ΔＤがゼロであることから、後述の演算は行わずに、要望に応じて予め設定された所定値が閾値として採用される。

前記ズレ幅ΔＤは、将来における対象歩行者Ｈとロボット１０の相対位置関係が、それらの各パーソナルスペースが重なり合う干渉状態から、それらの周縁が相互接する状態になるまで、各パーソナルスペースを離す方向にシフトさせるときの移動ベクトルとされる。換言すると、当該ズレ幅ΔＤ以上に各パーソナルスペースをシフトさせると、それらの干渉状態が回避されることになり、当該回避のための最小移動ベクトルがズレ幅ΔＤとなる。

具体的に、ズレ幅ΔＤは、次の各ステップでの手順を経て、現時刻の相対座標系におけるロボット１０の相対位置情報及び相対速度情報から数学的演算によって求められる。

先ず、第１のステップとして、図６（Ａ）に示されるように、相対座標系において、原点の対象歩行者Ｈにロボット１０が最も接近する状態のロボット１０の中心点Ｃと、それらの相対直線距離ｄ_ＲＨとが算出される。

次に、第２のステップとして、図６（Ｂ）に示されるように、相対座標系において、対象歩行者Ｈとロボット１０の各パーソナルスペースの干渉非干渉の境界となるロボット１０の境界地点ＡＰの位置ベクトルが算出される。すなわち、境界地点ＡＰは、相対座標系において、原点と現時刻のロボット１０の位置を結んだ直線ｌに直交し、且つ、原点を通る直線上に存在するとして、境界地点ＡＰを中心とした半径Ｌ_ＰＳの総合パーソナルスペースＰＳの外縁が原点を通る地点に設定される。この境界地点ＡＰは、同図に示されるように、相対座標系の２箇所に位置することになる。

更に、第３のステップとして、図６（Ｃ）に示されるように、対象歩行者Ｈに最も接近した際のロボット１０の中心点Ｃについて、その位置ベクトルと各境界地点ＡＰの位置ベクトルとのそれぞれとの差分が求められ、小さい方の差分がズレ幅ΔＤとされる。

そして、前記第２のケースにおける閾値については、ズレ幅ΔＤを用い、前述の式（１）、（２）の確率密度関数を応用して算出される。すなわち、当該確率密度関数において、対象歩行者Ｈの位置ベクトルＰを原点（０，０）とし、上式（２）の共分散行列Σの対角成分であるｘ軸、ｙ軸それぞれに対する標準偏差は、ベクトルとして表されるズレ幅ΔＤとなり、閾値Ｔは、前述の式（１）にこれらを代入した次式により算出される。

上式において、Δｘは、ズレ幅ΔＤのｘ軸成分であり、Δｙは、ズレ幅ΔＤのｙ軸成分である。

前記働きかけ実行手段２０は、干渉確率特定部２３で特定された干渉確率が閾値決定部２９で求めた閾値よりも大きいときに、働きかけ指令部３０からの指令によって、働きかけに関するロボット１０の動作を実行するように機能する。

この働きかけ実行手段２０は、図１に示されるように、働きかけに際して、ロボット１０と対象歩行者Ｈとの干渉を回避するための回避量を調整する回避量調整部３２と、当該回避量を基準としてロボット１０の移動制御を行う移動制御部３３とを備えている。

前記回避量調整部３２では、ロボット１０と対象歩行者Ｈとの干渉回避に最低限必要となるそれらの相対距離である総合パーソナルスペースＰＳの半径Ｌ_ＰＳとなる一定の干渉回避幅に、予測分布領域の大きさに応じて変化する安全距離を加算することで、対象歩行者Ｈの将来的な予測位置のズレを考慮した回避量が求められる。

前記安全距離Ｌ_Ａは、予測分布領域の範囲を決定するパラメータである各軸に対する分散に対応して、前述の式（２）の対角成分に基づいて次式により算出される。

上式において、σ_ｘ、σ_ｙは、前述の式（３）～（５）により求められる値であり、ｔ_ｎは、現時刻からロボット１０と対象歩行者Ｈの各パーソナルスペースが干渉するまでにかかる時間であり、前述と同様にして算出される。

前記移動制御部３３では、本発明者らが既に提案した特開２０２０－４６７５９号公報等に開示された手法等を用い、様々な働きかけに対応するロボット１０の動作が選択される。ここで、干渉回避に好適となる働きかけに対応するロボット１０の移動経路を生成する際には、回避量調整部３２で調整された回避量、すなわち、総合パーソナルスペースＰＳの幅である半径Ｌ_ＰＳに、対象歩行者Ｈの移動誤差を考慮した安全距離Ｌ_Ａを付加して拡張された回避量が用いられる。なお、働きかけに応じたロボット１０の動作選択は、本発明の本質部分ではないため、詳細な説明を省略する。

以上で説明したように、前記予測分布領域導出部２２により、所定の将来時刻において、対象歩行者Ｈの将来的な移動範囲内における各地点での存在確率を特定する予測分布領域Ｇが求められる。従って、予測分布領域導出部２２を含む制御装置１３は、対象歩行者Ｈの将来の所定時刻における移動範囲を推定する移動範囲推定装置として機能する。

なお、本発明に係るロボットとしては、前記実施形態で説明した自律移動型のロボット１０に限定されるものではなく、自動車両、船舶、飛行体等、所定の空間内を自律的に移動可能な移動体の他に、所定範囲の空間内で移動するロボットアーム等のマニピュレータであってもよく、これら移動時における対象歩行者Ｈとの干渉を回避する際の手法としても適用可能である。

その他、本発明における装置各部の構成は図示構成例に限定されるものではなく、実質的に同様の作用を奏する限りにおいて、種々の変更が可能である。

１０ ロボット
１２ 検出装置
１３ 制御装置（移動範囲推定装置）
１９ 働きかけ要否判定手段
２０ 働きかけ実行手段
２２ 予測分布領域導出部
２３ 干渉確率特定部
２４ 要否決定部
２７ 確率算出部
２９ 閾値決定部
３０ 働きかけ指令部
３２ 回避量調整部
３３ 移動制御部
ＣＰ 接近地点
Ｇ 予測分布領域
Ｈ 対象歩行者（移動障害物）

Claims (6)

1. 周囲に存在する移動障害物の位置情報及び速度情報を検出する検出装置と、当該検出装置の検出結果から、前記移動障害物との将来的な干渉を回避するための働きかけを行う制御装置とを備え、前記干渉を回避しながら自律移動可能なロボットにおいて、
   前記制御装置は、前記働きかけの要否を判定する働きかけ要否判定手段と、当該働きかけ要否判定手段で前記働きかけが必要と判定されたときに、前記働きかけを実行するようにロボットを動作制御する働きかけ実行手段とを備え、
   前記働きかけ要否判定手段は、将来の所定時刻にて前記移動障害物が存在する位置の確率分布を表す予測分布領域を求める予測分布領域導出部と、前記移動障害物とロボットの将来的な相対位置関係から、前記予測分布領域を用いて、前記干渉が発生する確率である干渉確率を特定する干渉確率特定部と、当該干渉確率を所定の閾値と対比して前記働きかけの要否を決定する要否決定部とを備え、
   前記予測分布領域導出部では、前記検出装置の検出結果から、前記移動障害物の等速直線運動モデルで中心座標を算出し、前記移動障害物の速度と回転角に対応する２軸とした二変量ガウス分布からなる確率密度関数を領域範囲とする前記予測分布領域を求め、所定時刻において所定位置に前記移動障害物が存在する確率が推定され、
   前記要否決定部は、現時刻における前記移動障害物とロボットの相対関係に応じて、前記閾値を決定する閾値決定部と、前記干渉確率が前記閾値よりも大きいときに、前記働きかけを行うように前記働きかけ実行手段に指令する働きかけ指令部とを備え、
   前記閾値決定部では、前記移動障害物とロボットの相対距離が、予め設定された所定値未満になる将来時刻が存在すると予測されるときに、当該将来時刻における前記相対距離が前記所定値以上になるように、ロボットを更に相対移動させる際の移動ベクトルに相当する最小のズレ幅が算出され、前記ズレ幅を標準偏差として前記確率密度関数に代入して前記閾値を算出することを特徴とするロボット。
2. 前記予測分布領域の標準偏差は、前記検出装置の測定誤差に関する要素と、将来の速度情報が現在の状態を維持する確率に対応する保持可能性に関する要素とを総合して決定され、
   前記保持可能性は、前記検出装置で検出された過去のログデータの遷移状態に応じて特定されることを特徴とする請求項１記載のロボット。
3. 前記干渉確率特定部は、前記干渉確率を求める際の各種条件値を算出する条件値算出部と、前記条件値から、前記予測分布領域を表す数式を用いて前記干渉確率を算出する確率算出部とを備え、
   前記条件値算出部では、現時刻における前記検出装置の検出結果から、前記移動障害物がロボットに接近する接近状態になると予測される将来の接近時刻までの現時刻からの経過時間である時間情報と、前記接近状態における所定の接近地点の位置情報とが前記条件値として求められ、
   前記移動障害物とロボットの将来的な相対移動予測によるそれらの相対距離が、予め設定された所定値を常時下回らないと推定される場合に、前記移動障害物とロボットが最接近する時刻が前記接近時刻とされ、当該接近時刻における前記移動障害物とロボットとの間を結ぶ直線上で、ロボットから前記所定値分離れた地点が前記接近地点とされる一方、前記相対距離が将来的に前記所定値を下回る時があると推定される場合に、前記相対距離が前記所定値に一致する最も早い時刻が前記接近時刻とされ、当該時刻におけるロボットの予測位置が前記接近地点とされ、
   前記確率算出部では、前記数式に前記条件値を代入することにより、前記接近地点での前記移動障害物の存在確率を前記干渉確率として算出することを特徴とする請求項１記載のロボット。
4. 前記働きかけ実行手段は、前記働きかけ時に前記干渉を回避するための回避量を調整する回避量調整部と、当該回避量を基準としてロボットの移動制御を行う移動制御部とを備え、
   前記回避量調整部では、干渉回避に最低限必要となる一定の干渉回避幅に、前記予測分布領域の大きさに応じて変化する安全距離を加算することで前記回避量が求められることを特徴とする請求項１記載のロボット。
5. 周囲に存在する移動障害物の位置情報及び速度情報を検出する検出装置の検出結果に基づき、前記移動障害物との将来的な干渉を回避するための働きかけを行うように、ロボットの自律移動を制御する制御装置において、
   前記働きかけの要否を判定する働きかけ要否判定手段と、当該働きかけ要否判定手段で前記働きかけが必要と判定されたときに、前記働きかけを実行するようにロボットを動作制御する働きかけ実行手段とを備え、
   前記働きかけ要否判定手段は、将来の所定時刻にて前記移動障害物が存在する位置の確率分布を表す予測分布領域を求める予測分布領域導出部と、前記移動障害物とロボットの将来的な相対位置関係から、前記予測分布領域を用いて、前記干渉が発生する確率である干渉確率を特定する干渉確率特定部と、当該干渉確率を所定の閾値と対比して前記働きかけの要否を決定する要否決定部とを備え、
   前記予測分布領域導出部では、前記検出装置の検出結果から、前記移動障害物の等速直線運動モデルで中心座標を算出し、前記移動障害物の速度と回転角に対応する２軸とした二変量ガウス分布からなる確率密度関数を領域範囲とする前記予測分布領域を求め、所定時刻において所定位置に前記移動障害物が存在する確率が推定され、
   前記要否決定部は、現時刻における前記移動障害物とロボットの相対関係に応じて、前記閾値を決定する閾値決定部と、前記干渉確率が前記閾値よりも大きいときに、前記働きかけを行うように前記働きかけ実行手段に指令する働きかけ指令部とを備え、
   前記閾値決定部では、前記移動障害物とロボットの相対距離が、予め設定された所定値未満になる将来時刻が存在すると予測されるときに、当該将来時刻における前記相対距離が前記所定値以上になるように、ロボットを更に相対移動させる際の移動ベクトルに相当する最小のズレ幅が算出され、前記ズレ幅を標準偏差として前記確率密度関数に代入して前記閾値を算出することを特徴とする制御装置。
6. 周囲に存在する移動障害物の位置情報及び速度情報を検出する検出装置の検出結果に基づき、前記移動障害物との将来的な干渉を回避するための働きかけを行うように、ロボットの自律移動を制御する制御装置のプログラムにおいて、
   前記制御装置は、前記働きかけの要否を判定する働きかけ要否判定手段と、当該働きかけ要否判定手段で前記働きかけが必要と判定されたときに、前記働きかけを実行するようにロボットを動作制御する働きかけ実行手段としてコンピュータを機能させ、
   前記働きかけ要否判定手段は、将来の所定時刻にて前記移動障害物が存在する位置の確率分布を表す予測分布領域を求める予測分布領域導出部と、前記移動障害物とロボットの将来的な相対位置関係から、前記予測分布領域を用いて、前記干渉が発生する確率である干渉確率を特定する干渉確率特定部と、当該干渉確率を所定の閾値と対比して前記働きかけの要否を決定する要否決定部とを備え、
   前記予測分布領域導出部では、前記検出装置の検出結果から、前記移動障害物の等速直線運動モデルで中心座標を算出し、前記移動障害物の速度と回転角に対応する２軸とした二変量ガウス分布からなる確率密度関数を領域範囲とする前記予測分布領域を求め、所定時刻において所定位置に前記移動障害物が存在する確率が推定され、
   前記要否決定部は、現時刻における前記移動障害物とロボットの相対関係に応じて、前記閾値を決定する閾値決定部と、前記干渉確率が前記閾値よりも大きいときに、前記働きかけを行うように前記働きかけ実行手段に指令する働きかけ指令部とを備え、
   前記閾値決定部では、前記移動障害物とロボットの相対距離が、予め設定された所定値未満になる将来時刻が存在すると予測されるときに、当該将来時刻における前記相対距離が前記所定値以上になるように、ロボットを更に相対移動させる際の移動ベクトルに相当する最小のズレ幅が算出され、前記ズレ幅を標準偏差として前記確率密度関数に代入して前記閾値を算出することを特徴とする制御装置のプログラム。

Images