特許 7652389 ロボット、最適ゴール探索装置及びそのプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2025-03-18

(45)【発行日】2025-03-27

(54)【発明の名称】ロボット、最適ゴール探索装置及びそのプログラム

(51)【国際特許分類】

   G05D 1/43 20240101AFI20250319BHJP

【ＦＩ】

G05D1/43

【請求項の数】 5

(21)【出願番号】P 2021138489

(22)【出願日】2021-08-27

(65)【公開番号】P2023032381

(43)【公開日】2023-03-09

【審査請求日】2024-04-04

【国等の委託研究の成果に係る記載事項】（出願人による申告）令和２年度、国立研究開発法人科学技術振興機構、戦略的創造研究推進事業「同調と主張に基づく接近・接触状態での人共存型モビリティの協調移動技術」委託研究、産業技術力強化法第１７条の適用を受ける特許出願

(73)【特許権者】

【識別番号】899000068

【氏名又は名称】学校法人早稲田大学

(74)【代理人】

【識別番号】100114524

【弁理士】

【氏名又は名称】榎本 英俊

(72)【発明者】

【氏名】亀▲崎▼ 允啓

(72)【発明者】

【氏名】平山 三千昭

(72)【発明者】

【氏名】櫻井 絵梨子

(72)【発明者】

【氏名】斎藤 喬介

(72)【発明者】

【氏名】濱田 太郎

(72)【発明者】

【氏名】菅野 重樹

【審査官】岩▲崎▼ 優

(56)【参考文献】

【文献】特開２０２０－００４１８２（ＪＰ，Ａ）

【文献】特開２０１４－１１９９０１（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００－ １／８７

(57)【特許請求の範囲】

【請求項1】

予め設定された環境内での移動を伴う所定のタスクを実行可能に動作する動作部と、周囲に存在する動作体や静止体の位置情報及び速度情報を検出する検出装置と、当該検出装置の検出結果に基づいて前記動作部を動作制御する制御装置とを備えたロボットにおいて、
前記制御装置は、前記環境内における複数の地点から、前記タスクを実行する際のロボットのゴール位置の候補となるゴール候補を複数抽出し、当該ゴール候補毎に前記タスクの実行に際して考慮すべき環境情報に基づく要素を定量化した状況コストを算出するゴール候補抽出手段と、前記各ゴール候補を目的地としたときのロボットの経路計画を行った上で、前記タスクの実行に最適となる前記ゴール位置である最適ゴールを決定する最適ゴール決定手段とを備え、
前記ゴール候補抽出手段は、予め記憶された数式により、前記各地点を前記ゴール位置としたときの前記各地点のコスト分布が記録されたコストマップを生成するコストマップ生成部と、前記コストマップに基づいて、複数の前記地点を第１のゴール候補として選択する第１のゴール候補選択部と、同一のタスクを遂行するために移動する前記動作体とロボットの相対移動情報に基づきそれらの譲り合いを考慮した第２のゴール候補を求める譲り合い検討部とを備え、
前記譲り合い検討部は、ロボットと前記動作体の間での譲り合いにおける優先度を求める優先度算出部と、譲り合いのパターンを複数導出する譲り合いパターン導出部と、前記優先度及び前記コストマップ中のマップ内コストに基づき、前記各パターンの中から最適パターンを決定する最適パターン決定部と、前記最適パターンにおける譲り合い後のロボットの位置を前記第２のゴール候補とする第２のゴール候補選択部とを備え、
前記最適ゴール決定手段では、前記各ゴール候補の中から、前記状況コストに、前記各ゴール候補までの移動の労力に関する移動コストを加味した総コストの最も低い前記ゴール候補を前記最適ゴールとして決定することを特徴とするロボット。

【請求項2】

前記第２のゴール候補選択部では、前記第２のゴール候補の地点における前記マップ内コストに、前記動作体が譲り合う行動を行わないリスクに対応するリスクコストを加算した値を前記状況コストとすることを特徴とする請求項１記載のロボット。

【請求項3】

前記最適ゴール決定手段は、前記各ゴール候補それぞれについて、ロボットの経路候補の生成を行う経路生成部と、当該経路生成部で生成された各経路候補について、前記移動コストを算出する移動コスト算出部と、前記各ゴール候補における前記総コストに基づき、前記最適ゴールを決定する最終決定部とを備えたことを特徴とする請求項２記載のロボット。

【請求項4】

予め設定された環境内での移動を伴う所定のタスクを実行しながら動作するロボットの経路計画に用いられ、周囲に存在する動作体や静止体の位置情報及び速度情報に基づき、前記環境内における複数の地点から前記タスクの実行に最適となるロボットの目的地となる最適ゴールを探索する最適ゴール探索機能を有する装置において、
前記環境内における複数の地点から、前記タスクを実行する際のロボットのゴール位置の候補となるゴール候補を複数抽出し、当該ゴール候補毎に前記タスクの実行に際して考慮すべき環境情報に基づく要素を定量化した状況コストを算出するゴール候補抽出手段と、前記各ゴール候補を目的地としたときのロボットの経路計画を行った上で、前記タスクの実行に最適となる前記ゴール位置である最適ゴールを決定する最適ゴール決定手段とを備え、
前記ゴール候補抽出手段は、予め記憶された数式により、前記各地点を前記ゴール位置としたときの前記各地点のコスト分布が記録されたコストマップを生成するコストマップ生成部と、前記コストマップに基づいて、複数の前記地点を第１のゴール候補として選択する第１のゴール候補選択部と、同一のタスクを遂行するために移動する前記動作体とロボットの相対移動情報に基づきそれらの譲り合いを考慮した第２のゴール候補を求める譲り合い検討部とを備え、
前記譲り合い検討部は、ロボットと前記動作体の間での譲り合いにおける優先度を求める優先度算出部と、譲り合いのパターンを複数導出する譲り合いパターン導出部と、前記優先度及び前記コストマップ中のマップ内コストに基づき、前記各パターンの中から最適パターンを決定する最適パターン決定部と、前記最適パターンにおける譲り合い後のロボットの位置を前記第２のゴール候補とする第２のゴール候補選択部とを備え、
前記最適ゴール決定手段では、前記各ゴール候補の中から、前記状況コストに、前記各ゴール候補までの移動の労力に関する移動コストを加味した総コストの最も低い前記ゴール候補を前記最適ゴールとして決定することを特徴とする最適ゴール探索装置。

【請求項5】

予め設定された環境内での移動を伴う所定のタスクを実行しながら動作するロボットの経路計画に用いられ、周囲に存在する動作体や静止体の位置情報及び速度情報に基づき、前記環境内における複数の地点から前記タスクの実行に最適となるロボットの目的地となる最適ゴールを探索する最適ゴール探索装置のプログラムにおいて、
前記環境内における複数の地点から、前記タスクを実行する際のロボットのゴール位置の候補となるゴール候補を複数抽出し、当該ゴール候補毎に前記タスクの実行に際して考慮すべき環境情報に基づく要素を定量化した状況コストを算出するゴール候補抽出手段と、前記各ゴール候補を目的地としたときのロボットの経路計画を行った上で、前記タスクの実行に最適となる前記ゴール位置である最適ゴールを決定する最適ゴール決定手段としてコンピュータを機能させ、
前記ゴール候補抽出手段は、予め記憶された数式により、前記各地点を前記ゴール位置としたときの前記各地点のコスト分布が記録されたコストマップを生成するコストマップ生成部と、前記コストマップに基づいて、複数の前記地点を第１のゴール候補として選択する第１のゴール候補選択部と、同一のタスクを遂行するために移動する前記動作体とロボットの相対移動情報に基づきそれらの譲り合いを考慮した第２のゴール候補を求める譲り合い検討部とを備え、
前記譲り合い検討部は、ロボットと前記動作体の間での譲り合いにおける優先度を求める優先度算出部と、譲り合いのパターンを複数導出する譲り合いパターン導出部と、前記優先度及び前記コストマップ中のマップ内コストに基づき、前記各パターンの中から最適パターンを決定する最適パターン決定部と、前記最適パターンにおける譲り合い後のロボットの位置を前記第２のゴール候補とする第２のゴール候補選択部とを備え、
前記最適ゴール決定手段では、前記各ゴール候補の中から、前記状況コストに、前記各ゴール候補までの移動の労力に関する移動コストを加味した総コストの最も低い前記ゴール候補を前記最適ゴールとして決定することを特徴とする最適ゴール探索装置のプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、周囲環境の状況に応じた自然なタスク遂行のために適切なゴール位置を探索するロボット、最適ゴール探索装置及びそのプログラムに関する。

【背景技術】

【0002】

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

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０２０－４６７７３号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、前述のような経路計画は、基本的に人間が事前に設定した１点をゴール位置として行われることを前提としている。しかしながら、所定のタスクを行うためのロボットにおいては、当該１点以外の位置でも所定のタスクを遂行できる場合があり、また、このような１点のゴール位置に拘るが故に、環境の経時的変化により、ロボットに不自然な動作を生じさせるような場合がある。例えば、ロボットのタスクとして、人混みの中で、ホール内に掲示したポスターをカメラで撮影し、その動画を遠隔地に提示することを想定した場合に、次のような不都合がある。すなわち、例えば、ポスターの正面から所定距離にある１地点をロボットのゴール位置とした場合、ある時点でそのゴール位置に人間がいる場合に、ロボットは当該人間がいなくなるまで待機する必要がある。更に、人間に移動を促すことで、当該ゴール位置にロボットが移動して到達することも可能であるが、当該人間に不快感を与えかねない。この点、ポスターを鑑賞（撮影）するというタスクの遂行を目的とするのであれば、ポスターを撮影可能にする限り、ロボットのゴール位置を変更することも有効である。そこで、所定のタスクを実行する自律移動ロボットにおいては、環境中の人間の存在位置、移動状況、タスクの内容に応じて適切となるゴール位置を自動的に特定する新たな手法が必要となる。

【0005】

本発明は、このような課題に着目して案出されたものであり、その目的は、ロボットが移動を伴う所定のタスクを実行する際に、経時的に変化する周辺の環境状況やタスクの内容を考慮した適切なゴール位置を自動的に探索できるロボット、最適ゴール探索装置及びそのプログラムを提供することにある。

【課題を解決するための手段】

【0006】

前記目的を達成するため、本発明は、主として、予め設定された環境内での移動を伴う所定のタスクを実行可能に動作する動作部と、周囲に存在する動作体や静止体の位置情報及び速度情報を検出する検出装置と、当該検出装置の検出結果に基づいて前記動作部を動作制御する制御装置とを備えたロボットにおいて、前記制御装置は、前記環境内における複数の地点から前記タスクの実行に最適となるロボットの目的地となる最適ゴールを探索する最適ゴール探索機能を有し、前記最適ゴール探索機能では、前記環境内の複数の地点をゴール候補とし、当該ゴール候補毎に前記タスクの実行に際して考慮すべき複数の要素を定量化して総合した総コストを算出し、当該総コストの最も低い前記ゴール候補を前記最適ゴールとして決定する、という構成を採っている。

【発明の効果】

【0007】

本発明によれば、ロボットが移動を伴う所定のタスクを実行する際に、周囲の環境の状況変化に応じて、当該状況を考慮しながら環境中の人間等に配慮した柔軟かつ自然な目的地を自動的に探索することができ、周囲の人間等と調和したロボット動作が可能になると期待できる。

【図面の簡単な説明】

【0008】

【図1】本実施形態に係るロボットの動作制御に関連する全体構成のみを概略的に表したブロック図である。

【図2】（Ａ）は、コストマップの概念図であり、（Ｂ）は、環境中におけるタスクコストの分布を表す（Ａ）と同様の概念図であり、（Ｃ）は、環境中における社会規範コストの分布を表す同概念図であり、（Ｄ）は、環境中における障害物コストの分布を表す同概念図である。

【図3】（Ａ）～（Ｅ）は、譲り合いパターン導出部での人間とロボットとの譲り合いパターンを説明するための概念図である。

【発明を実施するための形態】

【0009】

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

【0010】

図１には、本実施形態に係るロボット１０の動作制御に関連する全体構成のみを概略的に表したブロック図が示されている。本実施形態のロボット１０は、予め設定された環境内での移動を伴う所定のタスクを実行可能に動作するように、予め設定されたスタート位置からゴール位置まで自律的に移動する自律ロボットとして機能する。

【0011】

なお、図示省略するが、本実施形態においては、前記タスクとして、複数の人間が存在するホール内の壁面にポスターが掲示されている状況下で、ホール内を移動しながら、ポスターの前で停止してポスターをカメラで撮像する作業を例として、以下に説明する。なお、本発明は、当該タスクに限定されるものではなく、他のタスクにも適用可能である。

【0012】

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

【0013】

前記動作部１１は、所定範囲内を自律移動させるための動力源となる機器や機構等を含み、制御装置１３からの動作指令により、スタート位置からゴール位置までの経路に沿ってロボット１０を所定速度で自律移動させる公知の構造をなす。この動作部１１の構成については、本発明の本質部分でないため、詳細な図示説明を省略する。

【0014】

前記検出装置１２は、ロボット１０の周囲に存在する人間や物体等の位置情報を検出する位置検出部１５と、位置検出部１５での検出結果に基づき、前記人間や物体等の速度情報を検出するコンピュータからなる速度検出部１６とを備えている。ここでの検出対象としては、環境内で静止した状態となっている静止体である壁等の障害物や人間等と、環境内で移動している動作体である人間、他のロボット、動物等が挙げられる。

【0015】

本実施形態では、環境中に、タスクの対象物となるポスターと壁等の障害物が存在する他、同一のタスクを行おうとする複数の人間が存在していることを前提とする。これら人間は、それぞれ移動したり、ポスターの前で立ち止まったり等、経時的に動態が変化することを前提とする。従って、前記静止体としては、ポスターや壁等の障害物の他、所定時間、所定地点に留まっている人間が想定される。一方、前記動作体としては、タスクを実行するために目的地に移動する人間等が想定される。

【0016】

前記位置検出部１５は、特に限定されるものではないが、公知のレーザレンジファインダ等の測域センサの他に、カメラ情報を利用する深度センサを含む公知のセンサ類により構成される。これらセンサ類により、検出時刻における人間や物体等の位置座標が取得されるとともに、時系列データに基づき、同一の動作体か否かが判定される。そして、同一の動作体については、取得した時系列の位置情報の変化から、速度検出部１６で速度情報が求められることになる。

【0017】

前記速度検出部１６では、本発明者らが既に提案した手法（特開２０２０－９１２３５号公報）等により、対象者とともにロボット１０周囲の動作体の位置情報を取得した時刻毎に、当該位置情報から動作体の速度情報が算出される。なお、当該速度情報を導出するアルゴリズムについては、本発明の本質要素ではないため、詳細な説明を省略する。

【0018】

なお、前記検出装置１２としては、前述の態様に限定されるものではなく、ロボット１０の周囲の動作体及び静止体の位置情報や速度方法を検出できる限りにおいて、様々なセンサ類、装置類、或いはシステム類等を適用することもできる。

【0019】

以上のように、ロボット１０周囲の所定範囲内に存在する人間や障害物等について、位置情報及び速度情報が、所定時間毎に検出装置１２で逐次検出され、その検出タイミングで制御装置１３に伝送されるとともに、当該検出タイミング毎に制御装置１３での以下の各処理が行われる。

【0020】

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

【0021】

前記制御装置１３では、ロボット１０があたかも人間のような行動判断を伴ったタスクを実行できるように、環境内の複数地点から、ロボット１０のゴール位置の候補となるゴール候補が複数抽出され、これら各ゴール候補におけるロボット１０の経路計画を行った上で、当該経路の内容を踏まえて最適となるゴール位置である最適ゴールが決定され、当該最適ゴールへの経路に沿ってロボット１０が移動するように、動作部１１に対して動作指令が行われる。

【0022】

この制御装置１３は、環境内における複数の地点からタスクの実行に最適となるロボット１０の目的地となる最適ゴールを探索する最適ゴール探索機能を有する最適ゴール探索装置となる。ここでの最適ゴール探索機能は、以下に詳述するように、環境内の複数の地点をゴール候補とし、ゴール候補毎にタスクの実行に際して考慮すべき複数の要素を定量化して総合した総コストを算出し、総コストの最も低いゴール候補を最適ゴールとして決定するようになっている。

【0023】

ここでの要素としては、タスクの実行し易さに関する第１の要素と、人間との距離感やマナーに関する社会規範に関する第２の要素と、壁等の障害物との距離に関する第３の要素と、同一のタスクを行う人間との譲り合いに関する第４の要素と、ゴール位置までの移動の労力に関する第５の要素とがある。

【0024】

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

【0025】

前記制御装置１３は、前記第１～第４の要素を考慮し、環境内における複数の地点から、タスクを実行する際のロボット１０のゴール候補を複数抽出するゴール候補抽出手段１８と、前記第５の要素を考慮し、各ゴール候補を目的地としたときのロボット１０の経路計画を行った上で最適ゴールを決定する最適ゴール決定手段１９と、決定された最適ゴールまでの経路に沿ってロボット１０が動作するように、動作部１１に動作指令する動作指令手段２０とを備えている。

【0026】

前記ゴール候補抽出手段１８は、環境中の各地点について、各地点をゴール位置としたときにタスクに応じて考慮すべき第１～第３の要素を総合した各地点のコスト分布が記録されたコストマップを生成するコストマップ生成部２２と、コストマップに基づいて、ゴール候補となる複数地点を第１のゴール候補として選択する第１のゴール候補選択部２３と、同一のタスクを遂行するために移動する人間とロボット１０との相対移動情報に基づき前記第４の要素を考慮したゴール候補としての第２のゴール候補を求める譲り合い検討部２４とを備えている。

【0027】

前記コストマップ生成部２２は、前記第１の要素に対応し、タスクの実行し易さを表すタスクコストを各地点で求めるタスクコスト算出部２６と、前記第２の要素に対応し、人間との距離やタスクの対象物（ポスター）からの距離に応じた社会規範コストを各地点で求める社会規範コスト算出部２７と、前記第３の要素に対応し、壁等の障害物との距離に応じた障害物コストを各地点で求める障害物コスト算出部２８と、各地点おいて、これらタスクコスト、社会規範コスト、及び障害物コストを加算したコストを各地点の座標毎に記憶させるコスト合計部２９と、移動している人間の向かう地点について、コスト合計部２９でのコストに加算する追加コストを検討するコスト加算検討部３０と、コスト合計部２９及び加算コスト検討部３０での処理結果に基づく各地点のコストを反映した最終的なコストマップを求めるマップ決定部３１とを備えている。

【0028】

なお、以下の各処理においては、ロボット１０が移動し得る環境中における平面視の範囲内を正方形状のグリッドに分割した上で、当該グリッドを構成する各セルの中心座標を代表値としたセル単位で行われる。なお、得られるコストマップとしては、図２（Ａ）に概念的に表される。図２の各マップにおいては、濃色である程、コストが高いことを表しており、各図中の○印は、環境中の人間Ｈの存在位置を表し、同□印は、環境中のロボットＲの存在位置を表し、同×印は、ポスターの存在位置を表す。

【0029】

前記タスクコスト算出部２６では、環境中の各地点について、タスクがどれだけ実行し易いかという観点に基づいて求められ、目標となる対象物との距離や角度が要素となる。すなわち、ポスターを見るというタスクの場合では、ポスターに近い正面位置がポスターを見易く、ゴールとしての価値が高いことから、このような観点に基づきタスクコストが求められる。なお、このタスクコストは、タスクを達成できる範囲でコストが０以上１以下となり、タスクを達成できない範囲でコストが１より大きくなるように設定される。なお、このタスクコストは、タスクの内容に合わせて変更される。

【0030】

本実施形態におけるポスター観賞タスクでは、ロボット１０により撮影されたポスターの画像がどれだけ鮮明かが重要となるため、ロボット１０のカメラ（図示省略）による撮影時のポスターの解像度を基準にタスクコストが決定される。ここで、撮影されたポスターの縦方向の解像度は撮影距離のみに左右されるのに対し、横方向の解像度は撮影角度にも影響されるため、次式に従い、横方向の解像度を基準にタスクコストＣ_ｔが、環境中の各地点におけるセルそれぞれについて算出される。なお、タスクコストＣ_ｔの分布は、図２（Ｂ）に例示される。

【数1】

上式において、Ｗ_ｍｉｎは、タスク達成のための横幅の最低値（ｐｉｘｅｌ）であり、ｋは、ポスター正面からポスターを撮影した際の距離とポスター幅の変換係数であり、ｄＰは、ポスターと、計算対象のセル（以下、「対象セル」と称する）中心との距離であり、θは、ポスター正面からの対象セルの角度である。

【0031】

前記社会規範コスト算出部２７では、人間との距離感やマナーに関連した社会規範コストが算出される。一般的に、他の人間に過度に近い場所や他の人間が立っている場所を目的地とすると、その人間に不快感を与えてしまう。また、ポスターのすぐ近くに立つと他の人間がポスターを見づらく、マナー違反となることから、これらを考慮した指標として、社会規範コストが算出される。

【0032】

具体的に、ここでは、人間との距離に応じて生じる心理的斥力に基づく人間との距離コストと、ポスターを中心としたガウス分布となる対象物との距離コストとが、環境中の各地点におけるセルそれぞれについて求められ、これら距離コストの和が社会規範コストとなる。

【0033】

先ず、人間との距離コストＣ_ｈは、次式より算出される。この際のコスト算出で対象となる人間として、所定速度（例えば、１．５ｍ／ｓｅｃ）以上の人間に関しては、その場に留まらないと考えられるため、ここでのコスト計算の対象外とし、それ以外の静止状態の人間の位置に基づいて、人間との距離コストＣ_ｈが求められる。

【数2】

上式において、Ｌ_ｃは、ロボット１０と人間との接触時の体幹同士の距離（体幹接触距離）であり、Ｌ_ｐｓは、ロボット１０と人間とのパーソナルスペースの半径の和であり、ｄＨは、対象セルに最も近い人間と当該セル中心との距離であり、Ｓ_Ａは、予め設定された定数である。ここで、ロボット１０及び人間のパーソナルスペースは、それぞれの横幅に対して定数倍した直径を有するように予め設定された円形のスペースとされる。

【0034】

次に、前記対象物との距離コストＣ_ｐは、次式により算出される。

【数3】

上式において、ｄＰは、ポスターと対象セルとの距離であり、Ｇ_Ｒは、コスト分布の距離係数であり、予め設定された値を採る。

【0035】

そして、人間との距離コストＣ_ｈと対象物との距離コストＣ_ｐとの合計が、社会規範コストＣ_ｓとなる。当該社会規範コストＣ_ｓの分布は、図２（Ｃ）に例示される。

【0036】

前記障害物コスト算出部２８では、障害物との距離感に関連した障害物コストが算出される。一般的に、障害物との衝突を避けるため、障害物とは一定の距離を取ることが自然であることから、壁等の移動しない障害物や、接近すると危険となる障害物に近づき過ぎないことを考慮した指標として、障害物コストが算出される。障害物コストＣ_ｗは、障害物との距離による心理的な斥力に基づく次式により、環境中の各地点におけるセルそれぞれについて算出され、０から１の範囲に標準化される。

【数4】

上式において、Ｌ_ｃは、ロボット１０と人間との体幹接触距離であり、Ｌ_ｐｓは、ロボット１０と人間とのパーソナルスペースの半径の和であり、ｄＷは、対象セルに最も近い障害物と、当該対象セル中心との距離であり、Ｓ_Ａは、予め設定された定数である。当該障害物コストＣ_ｗの分布は、図２（Ｄ）に例示される。

【0037】

前記コスト合計部２９では、セル毎に、タスクコストＣ_ｔ、社会規範コストＣ_ｓ、及び障害物コストＣ_ｗに対して、それぞれに設定された重み付け定数を乗じた値を加算した合計のコストが求められ、この合計コストを各セルに対応して記憶されてなる基本コストマップが生成される。この基本コストマップは、図２（Ｂ）～（Ｄ）を重畳した同図（Ａ）として表される。なお、以下の説明において、基本コストマップ中の各セルのコストを「マップ内コスト」と称する。

【0038】

前記コスト加算検討部３０では、移動している人間がロボット１０と同一のタスクを実行すると仮定し、人間の移動に関係するセルについて、コスト合計部２９で求めたマップ内コストに加算する追加コストが求められる。

【0039】

すなわち、このコスト加算検討部３０は、移動していると認識された人間の予測ゴールを推定する予測ゴール推定部３３と、人間の予測ゴールが位置するセルのマップ内コストに追加コストを加算するコスト加算部３４とを備えている。

【0040】

前記予測ゴール推定部３３では、移動していると認識された人間の速度方向に存在する各セルを対象とし、当該各セルについて、コスト合計部２９で求めたマップ内コストのうち最も低いコストのセルが当該人間の予測ゴールとされる。

【0041】

なお、この予測ゴール推定部３３では、例えば、現時点において、ロボット１０がポスターの前等の所定位置で静止している場合に、コスト合計部２９で求めたマップ内コストが調整される。すなわち、この場合においては、人間がロボット１０の存在位置を目指さないものとして、当該存在位置のセルに対し、その周囲の一定範囲内に所定値のコストを更に追加したものがマップ内コストとされる。

【0042】

前記コスト加算部３４では、人間の予測ゴールを中心とした人間との距離コストが追加コストとして求められ、予測ゴールを中心とした所定範囲の各セルに対し、ここでの追加コストが、前記マップ内コストに加算される。ここでの距離コストは、現時点で予測ゴールに人間が静止しているものとして、社会規範コスト算出部２７で求められる人間との距離コストと同一の数式を使って算出された上で、予め設定された所定係数が乗じられる。

【0043】

前記マップ決定部３１では、コスト加算部３４で対象となるセルについて、コスト合計部２９で求めた追加コストの加算後のコストが採用され、それ以外のセルにおいてはマップ内コストが採用されることにより、セル毎にコスト値を対応させてなる最終的なコストマップが生成される。

【0044】

前記第１のゴール候補選択部２３では、計算処理速度等を考慮し、マップ決定部３１で決定された最終的なコストマップから第１のゴール候補となる複数のセルが抽出される。具体的に、ここでは、所定のセルのコストがその周囲８個のセルのコストよりも低い場合に、当該所定のセルが極小値セルとして抽出され、コストの低い順に所定個（例えば、最大５個）の極小値セルが第１のゴール候補として選択される。

【0045】

前記譲り合い検討部２４では、同一のタスクを行う周囲の人間に配慮した最適ゴールを検討する上で、移動する人間の予測ゴールをロボット１０が目指すと想定したときに、当該予測ゴールで譲り合う場合のロボット１０の目的地として、人間との到着時間差とコスト分布に基づき、譲り合いを考慮した第２のゴール候補として新たに抽出される。なお、譲り合い検討部２４での以下の説明において、移動する人間の予測ゴールの位置（座標）を「譲り合い点」と称する。

【0046】

この譲り合い検討部２４は、ロボット１０と人間の間での譲り合いにおける優先度を求める優先度算出部３６と、高優先度側が許容するコストの上限を算出するコスト上限算出部３７と、譲り合いのパターンを複数導出する譲り合いパターン導出部３８と、優先度及び基本コストマップでのマップ内コストに基づいて、各パターンの中から最適パターンを決定する最適パターン決定部３９と、最適パターン決定部３９での決定結果から、最適パターンにおける譲り合い後のロボット１９の位置を第２のゴール候補とする第２のゴール候補選択部４０とを備えている。

【0047】

前記優先度算出部３６では、移動する人間について、前記予測ゴール推定部３３で推定された予測ゴールへの人間とロボット１０との到着時間差に基づいて、次式により、人間の優先度Ｐ_Ｈとロボットの優先度Ｐ_Ｒとが算出される。ここでは、予測ゴールに早く到着する方の行動が優先されるように設定される。

【数5】

上式において、ｖ_Ｒは、ロボット１０の速度であり、ｄ_Ｒは、現時点のロボット１０の位置と譲り合い点との距離であり、ｖ_Ｈは、人間の速度であり、ｄ_Ｈは、現時点での人間の位置と譲り合い点との距離であり、ｋ_Ｐは、予め設定された定数である。但し、一方の優先度が１を超える場合は、その優先度を１とし、他方の優先度を０にすることにより、各優先度Ｐ_Ｈ、Ｐ_Ｒは、０から１までの間の値を採ることになる。

【0048】

前記コスト上限算出部３７では、優先度の差が大きい程、高優先度側で許容するコスト上昇量が少なくなるように、次式により、高優先度側の優先度Ｐと、譲り合い点のセルの前記マップ内コストＣ_ｔｇとを用いて、高優先度側のコスト上限Ｃ_Ｌが算出される。

【数6】

なお、上式において、ｋは、予め設定された定数である。

【0049】

前記譲り合いパターン導出部３８では、図３（Ａ）に示されるように、人間Ｈとロボット１０が譲り合い点Ｇを目指す状況において、譲り合い点Ｇからポスターに向かう方向に対して垂直方向に譲り合いを行うものとし、人間Ｈとロボット１０の距離がパーソナルスペースＰＳの分確保できるように双方若しくは一方が移動するパターンを導出する。セル単位での計算のため、例えば、図３（Ａ）の状況においては、同図（Ｂ）～（Ｅ）の４パターンが導出される。

【0050】

前記最適パターン決定部３９では、各譲り合いパターンについて、高優先度側の譲り合い後のセルでの前記マップ内コストが、コスト上限算出部３７で算出されたコスト上限Ｃ_Ｌを下回り、且つ、次式で算出される双方のコストの重み付け和Ｃ_ｃｐが最小になるパターンが最適パターンとされる。

【数7】

なお、上式において、Ｃ_Ｈは、譲り合い後の人間Ｈが位置するセルのマップ内コストであり、Ｃ_Ｒは、譲り合い後のロボット１０が位置するセルのマップ内コストであり、Ｐ_Ｈは、人間Ｈの優先度であり、Ｐ_Ｒはロボット１０の優先度である。

【0051】

前記第２のゴール候補選択部４０では、最適パターンにおけるロボット１０の譲り合い後の移動位置が譲り合いを考慮した第２のゴール候補として追加される。この際、第２のゴール候補のコストとしては、最適パターンにおけるロボット１０の譲り合い後の移動位置のセルのマップ内コストに、以下のリスクコストが加算される。つまり、ロボット１０が適切だと判断した譲り合いであっても、人間がそれに従ってくれるとは限らない。そこで、人間が従ってくれない譲り合いを強行することが無いよう、譲り合い後のロボット１０の移動位置を第２のゴール候補とする際に、基本コストマップにおけるマップ内コストに、譲り合いのリスクの分のコストが加算される。

【0052】

ここでのリスクコストＣ_ｒは、次式により算出される。

【数8】

なお、上式において、Ｃ_Ｈは、譲り合い後の人間Ｈが位置するセルのマップ内コストであり、Ｃ_Ｌは、コスト上限算出部３７で求めた高優先度側のコスト上限値であり、ｄＨは、現時点での人間Ｈの位置と譲り合い点Ｇとの距離であり、ｖＨ_ｇは、人間Ｈの速度ベクトルの譲り合い点Ｇ方向の成分であり、ｋ_１は、予め設定されたコスト増加リスクの係数であり、ｋ_２は、予め設定された速度リスクの係数である。

【0053】

以上のゴール候補抽出手段１８においては、第１及び第２のゴール候補毎に、タスクの実行に際して考慮すべき環境情報に基づく要素を定量化した状況コストが特定されることになる。第１のゴール候補では、当該地点のセルに対応する最終的なコストマップにおけるコスト値が状況コストとされ、第２のゴール候補では、第２のゴール候補選択部４０でリスクコストを加算した後のコスト値が状況コストとされる。

【0054】

前記最適ゴール決定手段１９は、前記ゴール候補抽出手段１８で抽出された第１及び第２のゴール候補について、それぞれ目的地とした経路候補の生成を行う経路生成部４２と、経路生成部４２で生成された各経路候補への移動し易さに関する前記第５の要素を定量化した移動コストを算出する移動コスト算出部４３と、各ゴール候補における状況コストに、移動コストを加味して総合した総コストに基づき、最適ゴールを決定する最終決定部４４とを備えている。

【0055】

前記経路生成部４２では、本発明者らが既に提案した特開２０２０－４６７７３号公報等に開示された手法等を用い、ロボット１０の経路候補を複数生成するようになっている。すなわち、検出装置１２の検出結果から人間や物体等との間での干渉が将来的に生じ得ると判定された場合には、人間や物体等の左右両側となる横に、それらとの干渉を回避する経路の通過点（Ｗａｙ Ｐｏｉｎｔ：以下、「ＷＰ」と称する）が設定される。更に、当該ＷＰと目的地とを直線で結んだ別の直線経路上において、他の人間や物体等との間での干渉が将来的に生じ得る場合に、当該他の人間や物体の左右両側となる横に同様にして次のＷＰが設定され、この処理が繰り返し行われる。最後に、このような手順で設定された各ＷＰを順に通過する経路候補が複数生成される。

【0056】

前記移動コスト算出部４３では、経路生成部４２で導出された経路候補それぞれについて、前記特開２０２０－４６７７３号公報等に開示された手法に類した次式により、移動コストＣ_ｍが算出される。

【数9】

上式において、ｄ_ｐは、経路の移動距離であり、Δｖｔは、時刻ｔでのロボット１０の標準速度からの速度変化であり、ｎ_ｔは、ロボット１０の接触により、人間をロボット１０の反対側に移動させる所望量であって予め設定される。ａ、ｂ、ｃは、各要素における重み付け定数であり、予め所定値に設定される。

【0057】

前記最終決定部４４では、各ゴール候補における状況コストに、移動コストを加算した総コストの値が最小となるゴール候補が最適ゴールとして決定され、その際の移動コストに対応する経路候補が、最適ゴールへの決定経路とされる。なお、総コストの算出に際しては、状況コストと移動コストに対する重み付けを行った上で加算しても良い。

【0058】

前記動作指令手段２０では、最終決定部４４で決定された最適ゴールへの決定経路に沿ってロボット１０が移動するように、動作部１１の動作制御指令が行われる。

【0059】

以上の制御装置１３では、所定時間（例えば、０．５秒）毎に、ロボット１０の最適ゴールの決定及び決定経路の導出に関する前述の処理が繰り返し行われる。

【0060】

なお、一度の目的地への移動で完了しないタスクの場合には、複数のサブゴールを想定し、サブゴール毎に前述の各処理を行い、各サブゴールの総コストを加算し、当該加算後のコストが最も少ない各サブゴールの組み合わせと、当該組み合わせにおけるサブゴールを結ぶ決定経路を求めるようにすると良い。

【0061】

また、本発明は、前記実施形態で説明したアルゴリズムに限定されるものでなく、ゴール候補毎にタスクの実行に際して考慮すべき要素をコストとして定量化し、当該コストの最も低いゴール候補を前記最適ゴールとして決定するものであれば種々のアルゴリズムを採ることができる。

【0062】

更に、本発明の対象のロボットとしては、前記各実施形態で説明した自律移動型のロボット１０に限定されるものではなく、自動車両、船舶、飛行体等、所定の空間内を自律的に移動可能な移動体の他に、所定範囲の空間内で動作するロボットアーム等のマニピュレータとしても良い。

【0063】

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

【符号の説明】

【0064】

１０ ロボット
１１ 動作部
１２ 検出装置
１３ 制御装置（最適ゴール探索装置）
１８ ゴール候補抽出手段
１９ 最適ゴール決定手段
２２ コストマップ生成部
２３ 第１のゴール候補選択部
２４ 譲り合い検討部
３６ 優先度算出部
２６ 譲り合いパターン導出部
３９ 最適パターン決定部
４０ 第２のゴール候補選択部
４２ 経路生成部
４３ 移動コスト算出部
４４ 最終決定部

【図1】

【図2】

【図3】