JP4348276B2 - ロボット制御装置 - Google Patents

Description

本発明は、複数の移動ロボットを制御するロボット制御装置に関し、詳しくは、複数の移動ロボットを、移動ロボット同士が衝突することなく移動させるロボット制御装置に関する。

従来、自律移動可能な複数の移動ロボットにタスクを実行させるロボット制御装置がある。このロボット制御装置においては、一つの移動領域内を複数の移動ロボットが移動するので、移動ロボット同士の衝突を回避する必要がある。移動ロボット同士の衝突を回避するため、以下の技術が開示されている。

特許文献１に記載の移動ロボットシステムは、移動ロボットが走行路に設けられたノードを検出しつつ、検出したノードを目標として移動するシステムである。そして、移動ロボットが制御局に対して走行予約を要求し、制御局が走行予約を許可するか否かを判定する。そして、走行予約が許可された場合に移動ロボットが許可されたノードを目標として移動する。

特許文献２に記載の自律移動ロボットは、自律移動ロボット同士の相互通信によって、他の自律移動ロボットを回避するように構成されている。
特開平５−６６８３１号公報（段落０００８〜００１５、図１〜図７） 特開平８−６３２２９号公報（段落００１２〜００６４、図１〜図１２）

しかしながら、特許文献１に記載の移動ロボットシステムでは、先の走行予約が優先されるので、優先順位が低いタスクを実行している移動ロボットの走行予約が先に行われ、優先順位が高いタスクを実行している移動ロボットが、実際には衝突する可能性がないにも関わらず、走行予約を行えずに移動できないおそれがあるなど、複数の移動ロボットの移動の最適化が行われていない。
また、移動ロボットが移動するためには常に走行予約を行うことが必要となるため、走行予約に伴う移動ロボット−制御局間の通信処理が煩雑になる。

また、特許文献２に記載の自律移動ロボットにおいても、自律移動ロボットのタスクの優先順位の変化に対応できず、自律移動ロボットの回避動作の最適化が行われてない。
また、他の自律移動ロボットを回避する動作は、予め設定された定型動作によるため、自律移動ロボットの移動領域の地形などを反映した回避動作ができない。

本発明は、前記した問題を解決すべく創案されたものであり、通常時のロボットの移動に影響を与えることなく、必要時にのみロボットの移動経路を変更し、ロボット同士の衝突を避けることが可能なロボット制御装置を提供することを課題とする。

前記課題を解決するため、本発明の請求項１に記載の発明は、移動経路に基づいて移動する機能を備えた複数のロボットと通信を行い、前記ロボット同士の衝突を避けるために複数の前記ロボットの移動を制御するロボット制御装置であって、前記移動経路が形成される移動領域に関する地図データを取得する地図取得手段と、前記ロボットの現在位置に関する現在位置データを取得する現在位置取得手段と、前記移動経路に設定された有限個のサブゴールに関するサブゴールデータを取得するサブゴール取得手段と、取得した前記サブゴールデータに基づいて、２台の前記ロボットが衝突する可能性があるか否かを判定する衝突可能性判定手段と、衝突可能性がある２台の前記ロボットのうち、少なくとも１台の前記ロボットの前記移動経路を、前記地図データ、前記現在位置データ及び前記サブゴールデータに基づいて、衝突可能性がある他の前記ロボットとの衝突を避けるように変更させる移動経路変更命令信号を生成する移動経路変更命令手段と、前記移動経路変更命令信号を対応する前記ロボットに送信する送信手段と、衝突可能性がある２台の前記ロボットの優先順位を、優先順位が高い方を優先ロボット、優先順位が低い方を非優先ロボットと設定する優先順位設定手段と、を備え、前記移動経路変更命令手段は、少なくとも前記非優先ロボットの移動経路を、前記優先ロボットとの衝突を避けるように変更させる移動経路変更命令信号を生成し、前記移動経路変更命令手段は、前記移動経路変更命令信号として、前記非優先ロボットの移動経路を、退避位置に移動することによって前記優先ロボットの移動経路から退避するように変更させる退避命令信号を生成する退避命令手段と、前記移動経路変更命令信号として、前記優先ロボットの移動経路を、前記退避位置に退避した前記非優先ロボットを回避して移動するように変更させる回避命令信号を生成する回避命令手段と、を備えており、前記退避命令手段は、前記地図データ及び前記サブゴールデータに基づいて、前記退避位置に関する退避位置データを生成する退避位置生成部と、前記退避位置データに基づいて、前記非優先ロボットを前記退避位置に退避させる前記退避命令信号を生成する退避命令生成部と、を備えており、前記回避命令手段は、前記退避位置に関する退避位置データ、前記地図データ及び前記サブゴールデータに基づいて、前記優先ロボットが回避移動する回避位置に関する回避位置データを生成する回避位置生成部と、前記回避位置データに基づいて、前記優先ロボットを前記回避位置に回避させる回避命令信号を生成する回避命令生成部と、を備えており、前記回避位置生成部は、前記地図データ及び前記退避位置データに基づいて、前記退避位置に退避した前記非優先ロボットを回避しつつ通過する回避中継位置に関する回避中継位置データを生成する回避中継位置生成部と、前記優先ロボットが前記移動経路に復帰する回避終了位置に関する回避終了位置データを生成する回避終了位置生成部と、を備えており、前記回避命令生成部は、前記回避中継位置データ及び前記回避終了位置データに基づいて、前記優先ロボットを、前記回避中継位置を経由して前記回避終了位置に移動させる前記回避命令信号を生成し、前記退避位置生成部は、前記現在位置データ、前記サブゴールデータ及び前記地図データに基づいて、前記サブゴールごとに、退避候補位置に関する退避候補位置データを生成する退避候補位置生成部と、前記退避候補位置データ及び前記現在位置データに基づいて、前記退避候補位置から退避位置を決定する退避位置決定部と、を備えており、前記退避候補位置生成部は、前記サブゴールごとに、二以上の前記退避候補位置に関する前記退避候補位置データを生成し、前記回避中継位置生成部は、前記退避位置に対応する前記サブゴールの、残りの前記退避候補位置から、前記回避中継位置を決定することを特徴とする。

このように、衝突可能性を判定し、衝突可能性があるロボットの移動経路のみを変更させるので、衝突可能性がない場合のロボットの移動を妨げることがなく、必要なときのみに衝突を避ける制御を行うことが可能となる。また、ロボット制御装置が衝突可能性のあるロボットの移動経路を、地図データに基づいて変更させるので、より適切な衝突回避動作を設定することが可能となる。また、ロボットが独自に移動経路を変更するのではなく、ロボット制御装置が、衝突可能性がある２台のロボットの両方の移動経路と地図データとに基づいて移動経路変更命令信号を生成するので、より好適な衝突回避動作が可能となる。
また、ロボットの優先順位に基づいて移動経路の変更内容を決定するので、衝突回避以降のロボットの動作に対する影響を抑えることができる。
本発明における「退避」は、非優先ロボットが退避位置に移動し、この退避位置で優先ロボットの回避を待機することを意味している。
また、本発明における「回避」は、優先ロボットが、退避位置で待機している非優先ロボットを回避して移動することを意味している。
このように、衝突可能性のある２台のロボットのそれぞれに、退避、回避を行わせることにより、確実に衝突を避けることができる。

また、請求項２に記載の発明は、請求項１に記載のロボット制御装置であって、前記ロボットの移動速度に関する移動速度データを取得する移動速度取得手段と、取得した前記現在位置データ及び前記移動速度データに基づいて、２台の前記ロボットが接近しているか否かを判定する接近判定手段と、をさらに備え、前記接近判定手段が、２台の前記ロボットが接近していると判定した場合に、前記衝突可能性判定手段が、接近している２台の前記ロボットが衝突する可能性があるか否かを判定することを特徴とする。

さらに、２台のロボットが接近している場合にのみ衝突可能性を判定するので、衝突可能性の判定制御の頻度を抑えることができ、必要なときのみに衝突可能性を判定することが可能となる。

また、請求項３に記載の発明は、請求項２に記載のロボット制御装置であって、複数の前記ロボットから２台の前記ロボットを選択するロボット選択手段をさらに備え、前記現在位置取得手段は、選択した２台の前記ロボットの前記現在位置データを取得し、前記移動速度取得手段は、選択した２台の前記ロボットの前記移動速度データを取得し、前記接近判定手段は、取得した前記位置データ及び前記移動速度データに基づいて、選択した２台の前記ロボットが接近しているか否かを判定することを特徴とする。

また、請求項４に記載の発明は、請求項２または請求項３に記載のロボット制御装置であって、前記接近判定手段が、２台の前記ロボットが接近していると判定した場合に、前記サブゴール取得手段が、接近している２台の前記ロボットの前記サブゴールデータを取得することを特徴とする。

また、請求項５に記載の発明は、請求項２から請求項４のいずれか一項に記載のロボット制御装置であって、前記衝突可能性判定手段は、接近している２台の前記ロボットの前記サブゴールデータから一つずつ選択するサブゴール選択部と、選択した前記サブゴールデータに基づいてサブゴール間距離を算出するサブゴール間距離算出部と、算出した前記サブゴール間距離が所定距離以下であるか否かを判定するサブゴール間距離判定部と、選択した前記サブゴールデータ並びに取得した前記現在位置データ及び前記移動速度データに基づいて、接近している２台の前記ロボットが対応する前記サブゴールに到達するのに要する到達時間をそれぞれ算出する到達時間算出部と、算出した前記到達時間の差が一定時間内であるか否かを判定する到達時間判定部と、前記サブゴール間距離判定部が、前記サブゴール間距離が所定距離以下であると判定し、かつ、前記到達時間判定部が、前記到達時間の差が一定時間内であると判定した場合に、接近している２台の前記ロボットが衝突する可能性があると判定する衝突可能性判定部と、を備えていることを特徴とする。

このようにすることで、２台のロボットが、一定時間内に一定距離内に位置していることが判定でき、衝突可能性の判定を好適に行うことができる。

また、請求項６に記載の発明は、請求項５に記載のロボット制御装置であって、前記サブゴール間距離判定部が、前記サブゴール間距離が所定距離以下であると判定した場合に、前記到達時間算出部が、接近している２台の前記ロボットが対応する前記サブゴールに到達するのに要する到達時間を算出することを特徴とする。

このようにすることで、到達時間を算出する回数を減らすことができる。

また、請求項７に記載の発明は、請求項５または請求項６に記載のロボット制御装置であって、前記衝突可能性判定手段は、前記サブゴール取得手段が取得した前記サブゴールデータから、隣り合うサブゴールデータを補間するため新たなサブゴールデータを生成するサブゴール補間部をさらに備え、前記サブゴール補間部は、補間された前記サブゴールデータを前記サブゴール選択部に出力することを特徴とする。

ロボットが移動するのに使用するサブゴールは、ロボットの移動開始位置と移動終了位置との間に設けられた有限個のノードである。隣り合うサブゴール間の距離が、ロボットの大きさと比較して大きい場合には、かかるサブゴールに関するサブゴールデータが、衝突可能性を判定するのに不向きとなる。そこで、隣り合うサブゴール間を補間するようにサブゴールデータを生成し、補間したサブゴールデータを用いて衝突可能性を判定することで、衝突可能性の判定精度がさらに高まる。

また、請求項８に記載の発明は、請求項１から請求項７のいずれか一項に記載のロボット制御装置であって、前記優先順位設定手段は、衝突可能性がある２台の前記ロボットのタスクの種類に基づいて優先順位を設定することを特徴とする。

本発明によれば、通常時のロボットの移動に影響を与えることなく、必要時にのみロボットの移動経路を変更し、ロボット同士の衝突を避けることが可能なロボット制御装置を提供することができる。

以下、本発明の実施形態について、本発明のロボット制御装置を企業などの受付案内に適用した場合を例にとり、適宜図面を参照しながら説明する。同様の部分には同一符号を付し、重複する説明を省略する。

（ロボット制御システムＡの構成）
はじめに、本発明の実施形態に係るロボット制御システムついて説明する。図１は、本発明の実施形態に係るロボット制御システムを示すシステム構成図である。
このロボット制御システムＡは、ロボット制御装置３がロボットＲにタスクを実行させる実行命令信号を送信し、実行命令信号を受信したロボットＲがタスクを実行するものである。

図１に示すように、ロボット制御システムＡは、タスク実行エリアＥＡに配置された複数（本実施形態では３台）のロボットＲＡ，ＲＢ，ＲＣと、これらロボットＲＡ，ＲＢ，ＲＣと無線通信によって接続された基地局（例えば、無線ＬＡＮ）１と、基地局１にルーター２を介して接続されたロボット制御装置（例えば、サーバー）３と、ロボット制御装置３にネットワーク（ＬＡＮ）４を介して接続された端末５と、を備えている。そして、ロボットＲＡ，ＲＢ，ＲＣが、特許請求の範囲における「移動ロボット」に相当し、タスク実行エリアＥＡが、特許請求の範囲における「移動領域」に相当する。

ロボットＲＡ，ＲＢ，ＲＣは、タスク実行エリアＥＡに配置されており、タスク実行エリアＥＡ内において自律移動を行い、タスクを実行するものである。以下、ロボットＲＡ，ＲＢ，ＲＣを区別する必要がない場合には、ロボットＲと記載することもある。

基地局１は、タスク実行エリアＥＡに設けられており、ロボットＲと通信するためのものである。

以下、ロボットＲ及びロボット制御装置３の構成についてそれぞれ詳細に説明する。

［ロボットＲ］
本発明の実施形態に係るロボットＲは、自律移動型の２足移動ロボットである。
このロボットＲは、ロボット制御装置３から送信された実行命令信号に基づき、タスクを実行するものである。

図１に示すように、このロボットＲは、頭部Ｒ１、腕部Ｒ２、脚部Ｒ３を有しており、頭部Ｒ１、腕部Ｒ２、脚部Ｒ３は、それぞれアクチュエータにより駆動され、自律移動制御部５０（図２参照）により２足移動の制御がなされる。この２足移動についての詳細は、例えば特開２００１−６２７６０号公報に開示されている。

図２は、図１のロボットを示すブロック図である。図２に示すように、ロボットＲは、前記した頭部Ｒ１、腕部Ｒ２、脚部Ｒ３に加えて、カメラＣ，Ｃ、スピーカＳ、マイクＭＣ、画像処理部１０、音声処理部２０、制御部４０、自律移動制御部５０、無線通信部６０、記憶部７０を有している。
さらに、ロボットＲは、ロボットＲ自身の向きを検出するためのジャイロセンサＳＲ１と、ロボットＲ自身の現在位置を検出するためのＧＰＳ受信器ＳＲ２と、を有している。
ジャイロセンサＳＲ１が得た向きデータと、ＧＰＳ受信器ＳＲ２が得た現在位置データとは、それぞれ制御部４０に出力され、ロボットＲの行動を決定するのに利用されると共に、制御部４０から無線通信部６０を介してロボット制御装置３に送信される。

［カメラＣ，Ｃ］
カメラＣ，Ｃは、映像をデジタルデータとして取り込むことができるものであり、例えばカラーＣＣＤ（Charge-Coupled Device）カメラが使用される。カメラＣ，Ｃは、左右に平行に並んで配置され、撮影した画像は画像処理部１０に出力される。このカメラＣ，Ｃと、スピーカＳ及びマイクＭＣは、いずれも頭部Ｒ１の内部に配設されている。

［画像処理部１０］
画像処理部１０は、カメラＣ，Ｃが撮影した画像を処理して、撮影された画像からロボットＲの周囲の状況を把握するため、周囲の障害物や人物の認識を行う部分である。この画像処理部１０は、ステレオ処理部１１ａ、移動体抽出部１１ｂ、及び顔認識部１１ｃを含んで構成される。
ステレオ処理部１１ａは、左右のカメラＣ，Ｃが撮影した２枚の画像の一方を基準としてパターンマッチングを行い、左右の画像中の対応する各画素の視差を計算して視差画像を生成し、生成した視差画像及び元の画像を移動体抽出部１１ｂに出力する。なお、この視差は、ロボットＲから撮影された物体までの距離を表すものである。

移動体抽出部１１ｂは、ステレオ処理部１１ａから出力されたデータに基づき、撮影した画像中の移動体を抽出するものである。移動する物体（移動体）を抽出するのは、移動する物体は人物であると推定して、人物の認識をするためである。
移動体の抽出をするために、移動体抽出部１１ｂは、過去の数フレーム（コマ）の画像を記憶しており、最も新しいフレーム（画像）と、過去のフレーム（画像）を比較して、パターンマッチングを行い、各画素の移動量を計算し、移動量画像を生成する。そして、視差画像と、移動量画像とから、カメラＣ，Ｃから所定の距離範囲内で、移動量の多い画素がある場合に、その位置に人物がいると推定し、その所定距離範囲のみの視差画像として、移動体を抽出し、顔認識部１１ｃへ移動体の画像を出力する。

顔認識部１１ｃは、抽出した移動体から肌色の部分を抽出して、その大きさ、形状などから顔の位置を認識する。なお、同様にして、肌色の領域と、大きさ、形状などから手の位置も認識される。
認識された顔の位置は、ロボットＲが移動するときの情報として、また、その人とのコミュニケーションを取るため、制御部４０に出力されると共に、無線通信部６０に出力されて、基地局１を介して、ロボット制御装置３に送信される。

［音声処理部２０］
音声処理部２０は、音声合成部２１ａと、音声認識部２１ｂとを有している。
音声合成部２１ａは、制御部４０が決定し、出力してきた発話行動の指令に基づき、文字情報から音声データを生成し、スピーカＳに音声を出力する部分である。音声データの生成には、予め記憶している文字情報と音声データとの対応関係を利用する。
音声認識部２１ｂは、マイクＭＣから音声データが入力され、予め記憶している音声データと文字情報との対応関係に基づき、音声データから文字情報を生成し、制御部４０に出力するものである。

［制御部４０］
制御部４０は、後記するロボット制御装置３に送信する信号を生成すると共に、ロボット制御装置から出力された実行命令信号に基づいて、ロボットＲの各部（画像処理部１０、音声処理部２０、自律移動制御部５０及び無線通信部６０）を制御するものである。

そして、制御部４０は、ロボット状態データを定期的に生成し、出力する。ロボット状態データは、ロボットＩＤ、現在位置データ、向きデータ及び移動有無データを含んでおり、無線通信部６０を介してロボット制御装置３に送信される。
現在位置データは、ロボットＲの現在位置（座標）に関するデータであり、ＧＰＳ受信器ＳＲ２から得られる。
向きデータは、ロボットＲの向きに関するデータであり、ジャイロセンサＳＲ１から得られる。
移動有無データは、ロボットＲが移動しているか否かに関するデータであり、脚部制御部５１ｃが脚部Ｒ３を駆動している場合には「移動している」、脚部制御部５１ｃが脚部Ｒ３を駆動していない場合には「移動していない」とする。

また、制御部４０は、タスク開始位置（スタート）データ、タスク終了位置（ゴール）データ及び地図データに基づいて、サブゴールに関するサブゴールデータを生成する。
ここで、サブゴールについて説明する。図３は、ロボットのサブゴールを説明するための模式図である。
ロボットＲは、ロボット制御装置３からの実行命令信号を受信することによって、タスクを実行する。この実行命令信号は、タスク（自律移動）の目的地であるタスク終了位置に関するタスク終了位置データを含んでいる。
制御部４０は、ＧＰＳ受信器ＳＲ２から得たロボットＲの現在位置に関する現在位置データと、タスク終了位置データと、地図データと、に基づいて、タスク開始位置（スタート）とタスク終了位置（ゴール）との間に有限個のノードであるサブゴールを設定する。これらサブゴールの位置に関するデータが、サブゴールデータである。このサブゴールは、地図データを参照し、ロボットＲが自律移動可能な場所に、隣り合うサブゴール間の距離が略一定となるように生成される。

本実施形態において、地図データは、地図上にメッシュ状に配置された有限個のノード（点列）に関するノードデータを含んでいる。これらノードのうち、ロボットＲが移動可能な位置にあるノードが、移動可能ノード（以下、単にノードともいう）として設定されている。隣り合う移動可能ノード間には、リンクが設定されている。つまり、ある移動可能ノードに存在するロボットＲは、リンクが設定された隣の移動可能ノードへと移動可能である。制御部４０は、当該ロボットＲの現在位置データ及びタスク終了位置データと、地図データとに基づいて、いくつかの移動可能ノードを選択し、選択された移動可能ノードを当該ロボットＲの自律移動におけるサブゴールとして設定する。

本実施形態では、ロボットＲＡはＮ_A個のサブゴールデータＳＧ_A（ｎ_A）を有しており、Ｎ_A番目のサブゴールデータＳＧ_A（Ｎ_A）がタスク終了位置データとなっている。また、ロボットＲＢはＮ_B個のサブゴールデータＳＧ_B（ｎ_B）を有しており、Ｎ_B番目のサブゴールデータＳＧ_B（Ｎ_B）がタスク終了位置データとなっている。ロボットＲは、一つ先のサブゴールを目標として移動することを繰り返し、タスク終了位置まで移動する。

また、制御部４０は、ロボット制御装置３から出力されたサブゴール要求信号に基づいて、記憶部７０からサブゴールデータを取得する。取得されたサブゴールデータは、無線通信部６０を介して、ロボット制御装置３に向けて送信される。

また、制御部４０は、退避移動開始連絡信号を生成する。この退避移動開始連絡信号は、ロボットＲが受信した退避命令信号に基づき、退避を開始したことを連絡するための信号である。退避移動開始連絡信号は、無線通信部６０を介して、ロボット制御装置３に向けて送信される。

また、制御部４０は、回避移動終了連絡信号を生成する。回避移動終了連絡信号は、ロボットＲが受信した回避命令信号に基づき、回避を終了したことを連絡するための信号である。制御部４０は、ＧＰＳ受信器ＳＲ２から出力された現在位置データと、回避終了位置データとを比較する。そして、これらの位置が一致する場合に、ロボットＲの回避移動が終了したと判定する。そして、制御部４０は、回避移動が終了したと判定した場合に、回避移動終了連絡信号を生成する。回避移動終了連絡信号は、無線通信部６０を介して、ロボット制御装置３に向けて送信される。

［自律移動制御部５０］
図２に戻り、ロボットＲの構成に関する説明を続ける。自律移動制御部５０は、頭部制御部５１ａ、腕部制御部５１ｂ、脚部制御部５１ｃを有している。
頭部制御部５１ａは、制御部４０の指示に従い頭部Ｒ１を駆動し、腕部制御部５１ｂは、制御部４０の指示に従い腕部Ｒ２を駆動し、脚部制御部５１ｃは、制御部４０の指示に従い脚部Ｒ３を駆動する。

［無線通信部６０］
無線通信部６０は、ロボット制御装置３とデータの送受信を行う通信装置である。無線通信部６０は、公衆回線通信装置６１ａ及び無線通信装置６１ｂを有している。
公衆回線通信装置６１ａは、携帯電話回線やＰＨＳ（Personal Handyphone System）回線などの公衆回線を利用した無線通信手段である。一方、無線通信装置６１ｂは、IEEE802.11b規格に準拠する無線ＬＡＮなどの、近距離無線通信による無線通信手段である。
無線通信部６０は、ロボット制御装置３からの接続要求に従い、公衆回線通信装置６１ａ又は無線通信装置６１ｂを選択してロボット制御装置３とデータ通信を行う。

［記憶部７０］
記憶部７０は、ロボットＲの移動制御に必要なデータを記憶する。記憶部７０に記憶されたデータは、無線通信部６０を介して更新可能である。
記憶部７０は、ロボットＲがタスクを実行するタスク実行エリアＥＡの地図情報を格納する地図情報データベース７１を備えている。地図情報データベース７１に格納された地図情報は、ロボット制御装置３の地図情報データベース２２０に格納された地図情報と同一である。
また、記憶部７０は、制御部４０が生成したサブゴールデータを記憶する。

［ロボット制御装置３］
続いて、ロボット制御装置３の構成について説明する。図４は、図１のロボット制御装置を示すブロック図である。図４に示すように、ロボット制御装置３は、入出力手段１００、記憶手段２００及び制御手段３００を備えている。

［入出力手段１００］
入出力手段１００は、基地局１やネットワーク４を介して、ロボットＲや端末５との間でデータの送受信を行うためのインタフェースである。この入出力手段１００が、特許請求の範囲における「送信手段」の一例である。

［記憶手段２００］
記憶手段２００は、ロボットＲを制御するための各種データを記憶する。
記憶手段２００は、ロボット情報データベース２１０及び地図情報データベース２２０を備えている。

［ロボット情報データベース２１０］
ロボット情報データベース２１０は、ロボットＲの状態に関するデータであるロボットデータをロボットＲごとに関連付けて記憶するためのデータベースである。
ロボットデータは、少なくともロボットＩＤ、現在位置データ及び移動速度データを含み、その他、当該ロボットＲの大きさ（幅など）、バッテリ残量、駆動系異常の有無などに関する情報（データ）を含んでいる。
現在位置データは、ロボットＲから送信されたロボット状態データに含まれている。
移動速度データは、ロボットＲの移動速度に関するデータであり、移動速さに関する移動速さデータと、移動の向きに関する向きデータと、を含んでいる。
移動速さデータは、ロボットＲから送信されたロボット状態データに含まれる移動有無データと、予め記憶されたロボットＲの平均移動速さに関する平均移動速さデータに基づいて生成される。移動有無データが、「移動している」ことを示すデータである場合には、平均移動速さデータを移動速さデータとする。一方、移動有無データが、「移動していない」ことを示す場合には、移動速さデータを「０」とする。
向きデータは、ロボットＲから送信されたロボット状態データに含まれている。
また、ロボット情報データベース２１０は、ロボットＲに実行させるタスクに関するタスクデータも記憶している。

また、ロボット情報データベース２１０は、衝突回避リストを備えている。
衝突回避リストは、衝突する可能性があると判定された２台のロボットＲ，Ｒの組み合わせを記憶するためのものである。衝突する可能性があるロボットＲ，Ｒの組み合わせは、衝突可能性判定手段３６０によって追加登録され、また、シナリオ復帰命令手段４００によって削除される。
また、衝突回避リストには、２台のロボットＲ，Ｒの回避制御に関する状態が、「State０」、「State１」、「State２」、「State３」、「State４」の５段階に分類されて記憶されている。この回避制御に関する状態は、ロボット制御装置３の衝突回避制御動作に伴い、制御手段３００によって設定・更新される。

［地図情報データベース２２０］
地図情報データベース２２０は、ロボットＲが自律移動し、タスクを実行する位置に関する地図データ（グローバルマップ）を格納するためのデータベースである。地図データは、ロボットＲの自律移動範囲にある通路、階段、エレベータ、部屋などに関する情報を含むデータである。ロボットＲは、地図データによって移動可能な範囲で自律移動可能である。また、地図データは、オペレータによる端末５の操作によって更新可能である。

［制御手段３００］
制御手段３００は、実行命令生成手段３０１、ロボット選択手段３１０、現在位置取得手段３２０、移動速度取得手段３３０、接近判定手段３４０、サブゴール取得手段３５０、衝突可能性判定手段３６０、優先順位設定手段３７０、地図取得手段３８０、移動経路変更命令手段３９０及びシナリオ復帰命令手段４００を備えている。

［実行命令生成手段３０１］
実行命令生成手段３０１は、ロボット情報データベース２１０に記憶されたタスクデータに基づいて、ロボットＲにタスクを実行させるための実行命令信号を生成するものである。生成した実行命令信号は、入出力手段１００を介して、対応するロボットＲに向けて送信される。

［ロボット選択手段３１０］
ロボット選択手段３１０は、複数のロボットＲから２台のロボットＲ，Ｒを選択するためのものである。以下、ロボット選択手段３１０が２台のロボットＲＡ，ＲＢを選択した場合を例にとり説明する。ロボット選択手段３１０は、選択した２台のロボットＲＡ，ＲＢに関するロボットＩＤを、現在位置取得手段３２０及び移動速度取得手段３３０に出力する。

［現在位置取得手段３２０］
現在位置取得手段３２０は、ロボット選択手段３１０が選択したロボットＲＡ，ＲＢに関する現在位置データを、ロボット情報データベース２１０から取得する。取得された現在位置データは、接近判定手段３４０に出力され、また、必要に応じて移動経路変更命令手段３９０に出力される。

［移動速度取得手段３３０］
移動速度取得手段３３０は、ロボット選択手段３１０が選択したロボットＲＡ，ＲＢに関する移動速度データを、ロボット情報データベース２１０から取得する。取得された移動速度データは、接近判定手段３４０に出力される。

［接近判定手段３４０］
接近判定手段３４０は、入力された現在位置データ及び移動速度データに基づいて、複数のロボットＲのうち、任意の２台のロボットＲ，Ｒが接近しているか否かを判定する。
本実施形態では、ロボット選択手段３１０が選択したロボットＲＡ，ＲＢが接近しているか否かを判定する。

ここで、接近判定手段３４０による接近判定について説明する。図５は、接近判定手段による接近判定を説明するためのグラフである。
接近判定手段３４０は、ロボット接近速度Ｖとロボット間距離Ｄとが次の式（１）を満たしているか否かを判定する。なお、ａは比例定数である。
Ｖ≧ａＤ …（１）
ロボット接近速度Ｖは、ロボットＲＡ，ＲＢが互いに近づく速度であり、ロボットＲＡ，ＲＢが近づくように移動する場合には正の値をとり、ロボットＲＡ，ＲＢが離れるように移動する場合には負の値をとる。このロボット接近速度Ｖは、ロボット情報データベース２１０に予め記憶されたロボットＲＡ，ＲＢの平均移動速さデータと、ロボットＲＡ，ＲＢの向きデータと、に基づいて算出される。
ロボット間距離Ｄは、ロボットＲＡ，ＲＢの現在位置データに基づいて算出される。
比例定数ａは、正の値であり、ロボットＲの大きさなどに基づいて、予め設定されている。
ロボット接近速度Ｖ及びロボット間距離Ｄが前記式（１）を満たす場合（図５の斜線領域）に、接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していると判定する。
すなわち、接近判定手段３４０は、ロボット接近速度Ｖ及びロボット間距離Ｄに基づいて、ロボット接近速度Ｖが一定の場合には、ロボット間距離Ｄが所定値（Ｖ／ａ）以下である場合にロボットＲＡ，ＲＢが接近していると判定する。そして、ロボット間距離Ｄが一定の場合には、ロボット接近速度Ｖが所定値（ａＤ）以上である場合にロボットＲＡ，ＲＢが接近していると判定する。
また、ロボット間距離Ｄが小さい場合であっても、ロボット接近速度Ｖが負の値をとる場合、すなわち、ロボットＲＡ，ＲＢが離れるように移動している場合には、接近判定手段３４０は、ロボットＲＡ，ＲＢが接近していないと判定する。
接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していると判定した場合には、接近判定手段３４０は、ロボットＲＡ，ＲＢのロボットＩＤをサブゴール取得手段３５０に出力する。
接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していないと判定した場合には、その旨を伝える信号をロボット選択手段３１０に出力する。

［サブゴール取得手段３５０］
図４に戻り、ロボット制御装置３の構成の説明を続ける。サブゴール取得手段３５０は、接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していると判定した場合に、当該２台のロボットＲＡ，ＲＢのサブゴールデータを取得する。
以下、サブゴールデータの取得手順について簡単に説明する。
まず、サブゴール取得手段３５０は、ロボットＲＡ，ＲＢのロボットＩＤを含むサブゴール要求信号を生成する。サブゴール要求信号は、入出力手段１００を介して、ロボットＲに向けて送信される。サブゴール要求信号を受信したロボットＲは、ロボットＩＤを比較し、自身のロボットＩＤと一致するものが含まれている場合には、サブゴールデータをロボット制御装置３に向けて送信する。ここでは、ロボットＩＤが一致するロボットＲＡ，ＲＢがそれぞれサブゴールデータを送信する。送信されるサブゴールデータには、当該ロボットＲＡ（ＲＢ）のロボットＩＤが付与されている。サブゴール取得手段３５０は、ロボットＲＡ，ＲＢから送信されたサブゴールデータを取得する。取得されたサブゴールデータは、衝突可能性判定手段３６０に出力され、必要に応じて、優先順位設定手段３７０に出力される。

［衝突可能性判定手段３６０］
衝突可能性判定手段３６０は、接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していると判定した場合に、ロボットＲＡ，ＲＢが衝突する可能性があるか否かを判定する。
本実施形態において、衝突可能性判定手段３６０は、２台のロボットＲＡ，ＲＢのサブゴールデータ、現在位置データ及び移動速度データに基づいて、２台のロボットＲＡ，ＲＢが衝突する可能性があるか否かを判定する。

図６は、図４の衝突可能性判定手段を示すブロック図である。図６に示すように、衝突可能性判定手段３６０は、サブゴール補間部３６１、サブゴール選択部３６２、サブゴール間距離算出部３６３、サブゴール間距離判定部３６４、到達時間算出部３６５、到達時間判定部３６６及び衝突可能性判定部３６７を備えている。

（サブゴール補間部３６１）
サブゴール補間部３６１は、入力されたサブゴールデータに基づいて、隣り合うサブゴール間を補間するために新たにサブゴールを生成する。
ここで、サブゴールの補間について説明する。図７は、サブゴール補間部によるサブゴールの補間を説明するための模式図であり、（ａ）は補間前を示す図、（ｂ）は補間後を示す図である。サブゴール補間部３６１は、入力されたサブゴールデータから、ｂ倍のサブゴールデータを新たに生成し、隣り合うサブゴール間の距離を小さくする。つまり、図７に示すように、サブゴールＳＧ_A（ｎ_A）とサブゴールＳＧ_A（ｎ_A＋１）との間に、（ｂ−１）個のサブゴールを生成し、サブゴールデータを補間する（図７（ａ）→図７（ｂ））。

なお、本実施形態では、一つ過去に通過したサブゴール以降のサブゴールについて補間する。これは、既に通過したサブゴールに関しては、衝突可能性を判定する必要はないため、一つ過去に通過したサブゴール以前のサブゴールに関する演算を省略し、サブゴール補間部３６１にかかる負荷を減らすための処置である。また、一つ未来のサブゴールまでの間に衝突する可能性もあるため、一つ過去のサブゴールと一つ未来のサブゴールとの間については補間する。補間されたサブゴールデータは、サブゴール選択部３６２に出力される。なお、補間されることによって新たに生成されたサブゴールデータは、衝突可能性を判定するために使用されるものであり、ロボットＲの移動制御には使用されない。

サブゴールを補間するための所定の値ｂは、隣り合うサブゴール間の距離を、ロボットＲ同士の衝突可能性を判定するのに十分な距離とするためのものであり、ロボットＲの大きさと、元の隣り合うサブゴール間の距離とに基づいて予め設定されている。

（サブゴール選択部３６２）
図６に示すサブゴール選択部３６２は、サブゴール補間部３６１が補間したサブゴールデータを一時記憶すると共に、記憶したサブゴールデータから、ロボットＲＡの一つのサブゴールデータと、ロボットＲＢの一つのサブゴールデータを選択し、サブゴール間距離算出部３６３及び到達時間算出部３６５に出力する。

（サブゴール間距離算出部３６３）
サブゴール間距離算出部３６３は、入力されたサブゴールデータに基づいて、サブゴール間距離を算出する。算出されたサブゴール間距離は、サブゴール間距離判定部３６４に出力される。

（サブゴール間距離判定部３６４）
サブゴール間距離判定部３６４は、入力されたサブゴール間距離が、一定距離ｃ内であるか否かを判定する。ここで、一定距離ｃは、対応するサブゴールにロボットＲＡ，ＲＢが位置しているときに、ロボットＲＡ，ＲＢがぶつからない程度の距離に予め設定されている。サブゴール間距離判定部３６４の判定結果は、衝突可能性判定部３６７に出力される。
本実施形態では、サブゴール間距離判定部３６４が、一定距離ｃ内であると判定した場合には、その旨を伝える信号が到達時間算出部３６５に出力される。

（到達時間算出部３６５）
到達時間算出部３６５は、ロボットＲＡ，ＲＢのそれぞれが、選択されたサブゴールデータに対応するサブゴールに到達するのに要する時間、すなわち、ロボットＲＡ，ＲＢのそれぞれが、現在位置から対応するサブゴールに到達するのに要する到達時間を算出する。
ロボットＲＡが選択されたサブゴールまで到達するのに要する時間は、ロボットＲＡの現在位置データ、平均移動速さデータ及び対応するサブゴールデータに基づいて算出される。
ロボットＲＢが選択されたサブゴールまで到達するのに要する時間は、ロボットＲＢの現在位置データ、平均移動速さデータ及び対応するサブゴールデータに基づいて算出される。
本実施形態では、到達時間算出部３６５は、後記するサブゴール間距離判定部３６４が一定距離ｃ内であると判定した場合に、対応するサブゴールに関する演算を行う。算出された到達時間は、到達時間判定部３６６に出力される。

（到達時間判定部３６６）
到達時間判定部３６６は、２台のロボットＲＡ，ＲＢのそれぞれが、選択されたサブゴールに到達するのに要する時間の差が一定時間ｄ内であるか否かを判定する。ここで、一定時間ｄは、ロボットＲの平均移動速さを考慮して設定されている。到達時間判定部３６６の判定結果は、衝突可能性判定部３６７に出力される。

（衝突可能性判定部３６７）
衝突可能性判定部３６７は、サブゴール間距離判定部３６４及び到達時間判定部３６６の判定結果に基づいて、選択されたサブゴールにおいてロボットＲＡ，ＲＢが衝突する可能性があるか否かを判定する。
衝突可能性判定部３６７は、サブゴール間距離判定部３６４の判定結果が一定距離ｃ内であり、到達時間判定部３６６の判定結果が一定時間ｄ内である場合には、ロボットＲＡ，ＲＢが衝突する可能性があると判定し、それ以外の場合には、ロボットＲＡ，ＲＢが衝突する可能性がないと判定する。
衝突可能性判定部３６７が、衝突可能性があると判定した場合には、その旨を伝える信号を優先順位設定手段３７０に出力する。
衝突可能性判定部３６７が、衝突可能性がないと判定した場合には、その旨を伝える信号がロボット選択手段３１０に出力される。そして、ロボット選択手段３１０は、別のロボットＲ，Ｒの組み合わせを選択する。

［優先順位設定手段３７０］
優先順位設定手段３７０は、衝突可能性があると判定されたロボットＲＡ，ＲＢについて、ロボットＲＡ，ＲＢのタスクを比較することによって優先順位を設定する。

ここで、優先順位設定手段３７０による優先順位の設定について説明する。
本実施形態のロボット制御システムＡは、ロボットＲにタスク（受付案内）を実行させるためのものである。ロボットＲの状態は、タスクデータに基づいて決定され、以下の＜０＞，＜１＞，＜２＞，＜９＞などに分類される。
＜０＞：フリー
＜１＞：タスク実行中
＜２＞：タスク一時停止中
＜９＞：非常事態

状態＜１＞は、さらに以下のように細かく分類される。
＜１−１＞ ：案内タスク
＜１−２＞ ：引継ぎ案内タスク
＜１−５＞ ：受付業務
＜１−９９＞：バッテリ交換

これらの状態を優先順位の高いものから順に並べると、以下のようになる。
＜１−１＞ → ＜１−２＞ → ＜１−９９＞ → ＜１−５＞ → ＜２＞ → ＜０＞ → ＜９＞
ロボットＲの優先順位が同じであるなど、これだけでロボットＲの優先順位の判定ができない場合には、タスク終了位置までの未通過サブゴールの個数が多い、すなわち、現在位置からタスク終了位置までの距離が遠いロボットＲの優先順位が、タスク終了位置までの未通過サブゴールの個数が少ないロボットＲの優先順位よりも高くなるように優先順位を設定する。
なお、以下の説明において、ロボットＲＢの優先順位がロボットＲＡの優先順位よりも高いと判定されたものとし、ロボットＲＡを非優先ロボットＲＡ、ロボットＲＢを優先ロボットＲＢと記載することもある。

［地図取得手段３８０］
地図取得手段３８０は、地図情報データベース２２０に記憶された地図データを取得する。取得された地図データは、移動経路変更命令手段３９０に出力される。

［移動経路変更命令手段３９０］
移動経路変更命令手段３９０は、衝突可能性があると判定されたロボットＲＡ，ＲＢについて、少なくとも一方の移動経路を変更させる移動経路変更命令信号を生成する。本実施形態において、移動経路変更命令信号には、非優先ロボットＲＡを退避させる退避命令信号と、優先ロボットＲＢを回避させる回避命令信号の２種類がある。
図８は、図４の移動経路変更命令手段を示すブロック図である。図８に示すように、移動経路変更命令手段３９０は、退避命令信号を生成する退避命令手段３９１と、回避命令信号を生成する回避命令手段３９６を備えている。
退避命令信号は、非優先ロボットＲＡに対して退避移動を命令するための信号であり、非優先ロボットＲＡのロボットＩＤと、退避位置データとを含んでいる。
回避命令信号は、優先ロボットＲＢに対して回避移動を命令するための信号であり、優先ロボットＲＢのロボットＩＤと、回避位置データとを含んでいる。

（退避命令手段３９１）
退避命令手段３９１は、地図データ、非優先ロボットＲＡの現在位置データ及び非優先ロボットＲＡのサブゴールデータに基づいて、非優先ロボットＲＡを退避させる退避命令信号を生成する。
退避命令手段３９１は、退避候補位置生成部３９２ａ及び退避位置決定部３９２ｂを備えた退避位置生成部３９２と、退避命令生成部３９３と、を備えている。
退避候補位置生成部３９２ａは、非優先ロボットＲＡが退避移動して優先ロボットＲＢの回避移動を待つ退避候補位置に関する退避候補位置データを生成する。
退避位置決定部３９２ｂは、退避候補位置データから、退避位置データを選択し、決定する。
退避命令生成部３９３は、退避位置データに基づいて、非優先ロボットＲＡを退避位置に退避移動させる退避命令信号を生成する。
この退避命令手段３９１による退避位置決定手順については、後で詳細に説明する。

（回避命令手段３９６）
回避命令手段３９６は、優先ロボットＲＢを回避させる回避命令信号を生成する。
回避命令手段３９６は、回避中継位置生成部３９７ａ及び回避終了位置生成部３９７ｂを備えた回避位置生成部３９７と、回避命令生成部３９８と、を備えている。
回避中継位置生成部３９７ａは、優先ロボットＲＢが回避移動する際に通過する回避中継位置に関する回避中継位置データを生成する。
回避終了位置生成部３９７ｂは、退避位置データ及び優先ロボットＲＢのサブゴールデータに基づいて、優先ロボットＲＢが回避移動を終了し元の移動経路に復帰する回避終了位置に関する回避終了位置データを生成する。
回避命令生成部３９８は、回避中継位置データ及び回避終了位置データに基づいて、優先ロボットＲＢを、回避中継位置を経由して回避終了位置に回避移動させる回避命令信号を生成する。
この回避命令手段３９６による回避位置決定手順については、後で詳細に説明する。

［シナリオ復帰命令手段４００］
シナリオ復帰命令手段４００は、退避した非優先ロボットＲＡ及び回避した優先ロボットＲＢのそれぞれに対して、退避または回避を終了し、元のタスクを実行する動作に復帰させるためのシナリオ復帰命令信号を生成する。ここでいうシナリオは、退避または回避を行う前にロボットＲが実行していたタスクの内容に相当する。生成されたシナリオ復帰命令信号は、入出力手段１００を介して、対応するロボットＲに向けて送信される。
シナリオ復帰命令信号を生成するタイミングなどについては、後で詳細に説明する。

以下、本実施形態のロボット制御システムＡを用いて、タスク実行エリアＥＡにおける２台のロボットＲＡ，ＲＢの衝突を避ける制御について説明する。図９ないし図１２は、２台のロボットの退避及び回避を説明するための模式図であり、タスク実行エリアの一部と、そこに位置する２台のロボットとを示す図である。
まず、図９に示すように、２台のロボットＲＡ，ＲＢがタスク実行エリアＥＡにおける通路を移動している際に、ロボット制御装置３（図１参照）が、２台のロボットが衝突する可能性があると判定する。
続いて、図１０に示すように、ロボット制御装置３が、ロボットＲＡを退避位置Ｐ１に退避させる退避命令信号を生成し、ロボットＲＡに向けて送信する。退避命令信号を受信したロボットＲＡは、退避位置Ｐ１へ退避する。
続いて、図１１に示すように、ロボット制御装置３が、ロボットＲＢを、回避中継位置Ｐ２を経由して回避終了位置Ｐ３に回避させる回避命令信号を生成し、ロボットＲＢに向けて送信する。回避命令信号を受信したロボットＲＢは、回避中継位置Ｐ２を経由して回避終了位置Ｐ３へ回避する。
そして、図１２に示すように、ロボットＲＢが回避中継位置Ｐ２を通過すると、ロボットＲＡが退避位置Ｐ１から移動を再開する。

＜ロボット制御システムＡによる衝突回避制御＞
続いて、ロボット制御システムＡによるロボットＲ同士の衝突を避けるための制御について、２台のロボットＲＡ，ＲＢの衝突を避ける場合を例にとって説明する。図１３は、本発明の実施形態に係るロボット制御システムによる、２台のロボットの衝突を避けるためのロボット及びロボット制御装置の動作例を示すフローチャートである。なお、ロボットＲＡ，ＲＢ及びロボット制御装置３の構成要素の符号については、適宜図２、図４、図６及び図８を参照する。

ロボットＲ（ロボットＲＡ，ＲＢ）は、ロボット状態データを生成し、生成したロボット状態データをロボット制御装置３に向けて定期的に送信する（Ｓ１１，Ｓ３１）。
ロボット制御装置３は、受信したロボットＲ（ロボットＲＡ，ＲＢ）のロボット状態データをロボット情報データベース２１０に記憶する。なお、ロボットＲＡ，ＲＢ以外のロボットＲＣがタスク実行エリアＥＡに存在する場合には、ロボットＲＣもロボット状態データを生成し、生成したロボット状態データをロボット制御装置３に向けて送信する。
ロボット選択手段３１０は、複数のロボットＲから２台のロボットＲＡ，ＲＢを選択する。現在位置取得手段３２０が、ロボットＲＡ，ＲＢの現在位置データを取得すると共に、移動速度取得手段３３０が、ロボットＲＡ，ＲＢの移動速度データを取得する。取得した現在位置データ及び移動速度データは、接近判定手段３４０に出力される。
そして、接近判定手段３４０が、入力された現在位置データ及び移動速度データに基づいて、ロボットＲＡ，ＲＢが接近しているか否かを判定する（Ｓ２１）。以下、接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していると判定したものとして説明する。

続いて、サブゴール取得手段３５０が２台のロボットＲＡ，ＲＢに対するサブゴール要求信号を生成する。サブゴール要求信号は、入出力手段１００を介して、ロボットＲに向けて送信される（Ｓ２２）。

ロボットＲＡ，ＲＢがサブゴール要求信号を受信すると、制御部４０が、記憶部７０からサブゴールデータを呼び出す。呼び出されたサブゴールデータは、無線通信部６０を介して、ロボット制御装置３に向けて送信される（Ｓ１２，Ｓ３２）。

ロボット制御装置３が、サブゴールデータを受信すると、サブゴール取得手段３５０がサブゴールデータを取得する。そして、衝突可能性判定手段３６０が、入力されたサブゴールデータに基づいて、ロボットＲＡ，ＲＢが衝突する可能性があるか否かを判定する（Ｓ２３）。以下、衝突可能性判定手段３６０が、ロボットＲＡ，ＲＢが衝突する可能性があると判定したものとして説明する。

続いて、優先順位設定手段３７０が、ロボットＲＡ，ＲＢの優先順位を設定する（Ｓ２４）。そして、移動経路変更命令手段３９０の退避命令手段３９１が、非優先ロボットＲＡの退避位置を算出し、退避位置データを含む退避命令信号を生成する。退避命令信号は、入出力手段１００を介して、ロボットＲＡに向けて送信される（Ｓ２５）。

非優先ロボットＲＡが退避命令信号を受信すると、非優先ロボットＲＡの制御部４０が、退避位置までの退避経路を生成する（Ｓ１３）。そして、非優先ロボットＲＡが退避位置までの移動を開始すると共に、非優先ロボットＲＡの制御部４０が退避開始連絡信号を生成する。退避開始連絡信号は、無線通信部６０を介して、ロボット制御装置３に向けて送信される（Ｓ１４）。非優先ロボットＲＡは、退避位置まで移動すると、退避移動を終了し、退避位置で停止し、優先ロボットＲＢが回避移動するまで待機する（Ｓ１５）。

一方、ロボット制御装置３が退避開始連絡信号を受信すると、移動経路変更命令手段３９０の回避命令手段３９６が、優先ロボットＲＢの回避位置を算出し、回避位置を含む回避命令信号を生成する。回避命令信号は、入出力手段１００を介して、優先ロボットＲＢに向けて送信される（Ｓ２６）。

優先ロボットＲＢが回避命令信号を受信すると、優先ロボットＲＢの制御部４０が、回避位置までの回避経路を生成する（Ｓ３３）。そして、優先ロボットＲＢが回避位置までの移動を開始し（Ｓ３４）、回避移動が終了すると、優先ロボットＲＢの制御部４０が回避終了連絡信号を生成する。回避終了連絡信号は、無線通信部６０を介して、ロボット制御装置３に向けて送信される（Ｓ３５）。

ロボット制御装置３が回避終了連絡信号を受信すると、ロボット制御装置３は、ロボットＲＡ，ＲＢの衝突回避制御を終了する（Ｓ２７）。

＜ロボット制御装置３のメインフローチャート＞
続いて、ロボット制御装置３の動作についてさらに詳細に説明する。図１４は、本発明の実施形態に係るロボット制御装置の動作を示すフローチャートである。なお、ロボットＲＡ，ＲＢ及びロボット制御装置３の構成要素の符号については、適宜図２、図４、図６及び図８を参照する。

まず、ロボット選択手段３１０が、ロボット情報データベース２１０に記憶されたロボットＲから２台のロボットＲＡ，ＲＢを選択する（Ｓ１０１）。

続いて、ロボット選択手段３１０が、ロボットＲＡ，ＲＢが衝突回避リストにリストアップされているか否かを判定する（Ｓ１０２）。
ロボットＲＡ，ＲＢが衝突回避リストにリストアップされていない場合（Ｓ１０２でｎｏ）には、現在位置取得手段３２０がロボットＲＡ，ＲＢの現在位置データを取得すると共に、移動速度取得手段３３０がロボットＲＡ，ＲＢの移動速度データを取得し、これらのデータが接近判定手段３４０に出力される。
そして、接近判定手段３４０がロボットＲＡ，ＲＢが接近しているか否かを判定する。そして、接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していると判定した場合には、サブゴール取得手段３５０が、ロボットＲＡ，ＲＢに対するサブゴール要求信号を生成する。サブゴール要求信号はロボットＲＡ，ＲＢに向けて送信される。ロボットＲＡ（ロボットＲＢ）がサブゴール要求信号を受信すると、制御部４０がサブゴールデータを生成する。サブゴールデータはロボット制御装置３に向けて送信される。そして、サブゴール取得手段３５０がサブゴールデータを取得し、取得したサブゴールデータを衝突可能性判定手段３６０に出力する。そして、衝突可能性判定手段３６０が、ロボットＲＡ，ＲＢが衝突する可能性があるか否かを判定する（Ｓ１０３）。

衝突可能性判定手段３６０が、ロボットＲＡ，ＲＢが衝突する可能性があると判定した場合には、ロボットＲＡ，ＲＢの組み合わせを衝突回避リストに追加し、（Ｓ１０４）制御手段３００が、衝突回避リストにおけるロボットＲＡ，ＲＢの状態を「State０」に移行させる（Ｓ１０５）。
ステップＳ１０３において、接近判定手段３４０が、ロボットＲＡ，ＲＢが接近していないと判定した場合、及び衝突可能性判定手段３６０が、ロボットＲＡ，ＲＢが衝突する可能性がないと判定した場合には、ステップＳ１０１に移行し、別の２台のロボットＲ，Ｒの組み合わせを選択し、この２台のロボットＲ，Ｒについての処理を開始する。

続いて、制御手段３００が、選択されたロボットＲＡ，ＲＢの状態を判定する（Ｓ１０６）。そして、ロボットＲＡ，ＲＢの状態に応じて、各ステップに移行する。

「State０」
ステップＳ１０６において、ロボットＲＡ，ＲＢの状態が「State０」であると判定された場合には、サブゴール取得手段３５０が、ＲＡ，ＲＢに対するサブゴール要求信号を生成する。サブゴール要求信号は、入出力手段１００を介して、ロボットＲＡ，ＲＢに向けて送信される（Ｓ１０７）。そして、制御手段３００がロボットＲＡ，ＲＢの状態を「State１」に移行させ（Ｓ１０８）、ステップＳ１０１に戻る。

「State１」
ステップＳ１０６において、ロボットＲＡ，ＲＢの状態が「State１」であると判定された場合には、サブゴール取得手段３５０が、ロボットＲＡ，ＲＢからのサブゴールデータを受信する（Ｓ１０９）。
サブゴール取得手段３５０がサブゴールデータを受信した場合（Ｓ１０９でｙｅｓ）には、優先順位設定手段３７０がロボットＲＡ，ＲＢの優先順位を設定する（Ｓ１１０）。
続いて、移動経路変更命令手段３９０の退避命令手段３９１が、非優先ロボットＲＡに対する退避命令信号を生成する（Ｓ１１１）。退避命令信号は、入出力手段１００を介して、非優先ロボットＲＡに向けて送信される（Ｓ１１２）。そして、制御手段３００がロボットＲＡ，ＲＢの状態を「State２」に移行させ（Ｓ１１３）、ステップＳ１０１に戻る。
なお、サブゴール取得手段３５０がサブゴールデータを受信していない場合（Ｓ１０９でｎｏ）には、ステップＳ１０１に移行し、サブゴールデータを受信するまで待機する。

「State２」
ステップＳ１０６において、ロボットＲＡ，ＲＢの状態が「State２」であると判定された場合には、移動経路変更命令手段３９０の回避命令手段３９６が、非優先ロボットＲＡからの退避開始連絡信号を受信する（Ｓ１１４）。
回避命令手段３９６が退避開始連絡信号を受信した場合（ステップＳ１１４でｙｅｓ）には、回避命令手段３９６が優先ロボットＲＢに対する回避命令信号を生成する（Ｓ１１５）。回避命令信号は入出力手段１００を介して、優先ロボットＲＢに向けて送信される（Ｓ１１６）。そして、制御手段３００がロボットＲＡ，ＲＢの状態を「State３」に移行させ（Ｓ１１７）、ステップＳ１０１に戻る。
なお、回避命令手段３９６が退避開始連絡信号を受信していない場合（ステップＳ１１４でｎｏ）には、ステップＳ１０１に移行し、退避開始連絡信号を受信するまで待機する。

「State３」
ステップＳ１０６において、ロボットＲＡ，ＲＢの状態が「State３」であると判定された場合には、シナリオ復帰命令手段４００が、非優先ロボットＲＡの退避移動が終了し、かつ、ロボット間距離Ｄが増大しているか否かを判定する（Ｓ１１８）。非優先ロボットＲＡの退避移動が終了したか否かは、定期的に送信される非優先ロボットＲＡのロボットデータに含まれる現在位置データと退避位置データとを比較することによって判定可能である。また、非優先ロボットＲＡが退避位置まで移動した際に、非優先ロボットＲＡの制御部４０が退避終了連絡信号を生成し、シナリオ復帰命令手段４００がこの退避終了連絡信号を受信することによって判定する構成であってもよい。
シナリオ復帰命令手段４００が、非優先ロボットＲＡの退避移動が終了し、かつ、ロボット間距離Ｄが増大していると判定した場合（Ｓ１１８でｙｅｓ）には、シナリオ復帰命令手段４００が、非優先ロボットＲＡに対するシナリオ復帰命令信号を生成する。シナリオ復帰命令信号は、非優先ロボットＲＡに向けて送信される（Ｓ１１９）。
シナリオ復帰命令信号を受信した非優先ロボットＲＡは、退避して中止していたタスクを再開する。
そして、制御手段３００がロボットＲＡ，ＲＢの状態を「State４」に移行させ（Ｓ１２０）、ステップＳ１０１に戻る。
なお、ステップＳ１１８において、制御手段３００が、非優先ロボットＲＡの退避移動が終了していない、またはロボット間距離Ｄが増大していないと判定した場合（Ｓ１１８でｎｏ）には、ステップＳ１０１に戻る。

「State４」
ステップＳ１０６において、ロボットＲＡ，ＲＢの状態が「State４」であると判定された場合には、シナリオ復帰命令手段４００が、優先ロボットＲＢの回避移動が終了したか否かを判定する（Ｓ１２０）。
シナリオ復帰命令手段４００が、優先ロボットＲＢの回避移動が終了したと判定した場合（Ｓ１２０でｙｅｓ）には、シナリオ復帰命令手段４００が、優先ロボットＲＢに対するシナリオ復帰命令信号を生成する。シナリオ復帰命令信号は、優先ロボットＲＢに向けて送信される（Ｓ１２２）。
そして、シナリオ復帰命令手段４００は、このロボットＲＡ，ＲＢの組み合わせを、衝突回避リストから削除させ（Ｓ１２３）、ステップＳ１０１に戻る。

＜接近判定・衝突可能性判定＞
続いて、接近判定手段３４０による接近判定手順と、衝突可能性判定手段３６０による衝突可能性判定手順とについて、図１５を参照し、さらに詳しく説明する。
図１５は、接近判定手段及び衝突可能性判定手段の動作を示すフローチャートである。以下に説明するステップＳ２０１〜Ｓ２０５は、図１４のフローチャートにおけるステップＳ１０３のサブルーチンに相当する。

接近判定手段３４０は、ロボットＲＡ，ＲＢの現在位置データ及び移動速度データに基づいて、ロボットＲＡ，ＲＢが接近しているか否かを判定する（Ｓ２０１）。
ロボット接近速度Ｖとロボット間距離Ｄとが前記式（１）を満たしている場合（Ｓ２０１でｙｅｓ）には、接近判定手段３４０は、ロボットＲＡ，ＲＢが接近していると判定し、サブゴール取得手段３５０を作動させる。
ロボット接近速度Ｖとロボット間距離Ｄとが前記式（１）を満たしていない場合（Ｓ２０１でｎｏ）には、接近判定手段３４０は、ロボットＲＡ，ＲＢが接近しておらず、衝突する可能性がないと判定する。

そして、接近判定手段３４０がロボットＲＡ，ＲＢが接近していると判定した場合には、衝突可能性判定手段３６０のサブゴール補間部３６１が、ロボットＲＡ，ＲＢから受信したサブゴールデータを補間する（Ｓ２０２）。

続いて、サブゴール間距離算出部３６３がロボットＲＡのサブゴールとロボットＲＢのサブゴールとの距離（サブゴール間距離）を算出し、サブゴール間距離判定部３６４が、サブゴール間距離が一定距離ｃ以下であるか否かを判定する。このようにして、一定距離ｃ以下となる分割サブゴールの組み合わせが抽出される（Ｓ２０３）。

続いて、到達時間算出部３６５が、抽出されたサブゴールの組み合わせに対する、２台のロボットＲＡ，ＲＢの到達時間を算出する（Ｓ２０４）。

続いて、到達時間判定部３６６が、到達時間の差が一定時間ｄ内であるか否かを判定する（Ｓ２０５）。到達時間の差が一定時間ｄ内の場合（Ｓ２０５でｙｅｓ）には、ある時刻に２台のロボットＲＡ，ＲＢが非常に近づいて位置することになるので、衝突可能性判定部３６７が、ロボットＲＡ，ＲＢが衝突する可能性があると判定する。到達時間の差が一定時間ｄよりも大きい場合（Ｓ２０５でｎｏ）には、２台のロボットＲＡ，ＲＢの移動経路が接近しているものの、ロボットＲＡ，ＲＢは接近部分を、時間差を持って移動することになるので、衝突可能性判定部３６７が、ロボットＲＡ，ＲＢが衝突する可能性がないと判定する。

＜退避位置算出＞
続いて、非優先ロボットＲＡの退避位置の算出について、さらに詳しく説明する。
図１６は、退避位置生成部による非優先ロボットの退避位置生成を説明するための模式図である。図１７は、退避位置生成部による退避位置生成動作を示すフローチャートである。以下の説明におけるステップＳ３０１〜Ｓ３１２が、図１４のフローチャートにおけるステップＳ１１５のサブルーチンに相当する。

ここで、退避動作を行う非優先ロボットＲＡは、Ｎ_A個のサブゴールを有しているものとする。そして、ｎ_A個目のサブゴールに関するサブゴールデータを、２次元座標上の点として定義し、
ＳＧ_A（ｎ_A） （ｎ_A＝１，２，…，Ｎ_A）
と記載する。
そして、ロボットＲＡが、現在ｍ_A個目のサブゴール（サブゴールデータはＳＧ_A（ｍ_A））を目標として移動しているとし、退避候補位置生成部３９２ａが、現時点における過去、未来２個ずつのサブゴールに関するサブゴールデータを抽出する（Ｓ３０１）。これら４個のサブゴールデータをそれぞれ、
Ｃａｌ＿ＳＧ_A（ｉ_A） （ｉ_A＝１，２，３，４）
と記載する。つまり、抽出したサブゴールデータＣａｌ＿ＳＧ_A（ｉ_A）は、以下のようになる。

Ｃａｌ＿ＳＧ_A（１）＝ＳＧ（ｍ_A＋１） …二つ未来のサブゴール
Ｃａｌ＿ＳＧ_A（２）＝ＳＧ（ｍ_A） …現在目標とする（一つ未来の）サブゴール
Ｃａｌ＿ＳＧ_A（３）＝ＳＧ（ｍ_A−１） …一つ過去のサブゴール
Ｃａｌ＿ＳＧ_A（４）＝ＳＧ（ｍ_A−２） …二つ過去のサブゴール

続いて、退避候補位置生成部３９２ａが、抽出した４個のサブゴールデータに対応する退避候補位置に関する退避候補位置データを生成する（Ｓ３０２）。
４個のサブゴールに対応して、それぞれ退避位置の候補となる退避候補位置が２点または４点存在する。これら退避候補位置に関する退避候補位置データを
Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（ｊ_A） （ｉ_A＝１，２，３，４、ｊ_A＝２ｏｒ４）
と記載する。
これら退避候補位置の中から一つを選び、退避位置を決定する。

ここで、各サブゴールに対応する退避候補位置データの生成について、（１）２点設定の場合、（２）４点設定の場合の順に説明する。

（１）２点設定の場合
所定幅の通路など、サブゴール（移動可能ノード）が直線上に並んでおり、隣り合う移動可能ノード間にリンクが設定されている場合には、退避候補位置生成部３９２ａが、退避候補位置を２点設定する。以下、２点設定の場合について、図１８ないし図２０を参照して説明する。
まず、退避候補位置生成部３９２ａが、サブゴールデータＣａｌ＿ＳＧ_A（ｉ_A）に対応するリンクを探索する。ここでは、図１８に示すように、サブゴールデータＣａｌ＿ＳＧ_A（ｉ_A）に対応するリンクが、Ｌｉｎｋ（１），Ｌｉｎｋ（２）の二つ存在している。これらのリンクＬｉｎｋ（１），Ｌｉｎｋ（２）は、同一直線上に並んでいる。
そして、図１９に示すように、退避候補位置生成部３９２ａが、そのリンクが作るベクトルＬｉｎｋ＿Ｖｅｃｔｏｒに対して垂直であり、かつ、サブゴールＣａｌ＿ＳＧ_A（ｉ_A）を通る直線Ｌを引く。
続いて、退避候補位置生成部３９２ａが、この直線Ｌ上に、ロボット間最小マージンを満たし、かつ、壁とのマージンを満足する２点の平面座標に関するデータを、退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）として設定する。ここでは、図２０に示すように、２点の退避候補位置が設定される。ロボット間最小マージンは、ロボットＲＡ，ＲＢが衝突せずにすれ違うことができる距離であり、ロボットＲの大きさに基づいて予め設定されている。
ここで、退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）それぞれから壁面までの距離ｓ１，ｓ３が、壁とのマージンであり、所定の値以上に設定される。また、退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）間の距離ｓ２が、ロボット間最小マージンを満たしている。壁面同士が離れている場合などには、ロボット間最小マージンをｍとしたとき、サブゴールＣａｌ＿ＳＧ_A（ｉ_A）と退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１）との距離をｍ／２の値に設定する構成であってもよい（退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）についても同様）。

（２）４点設定の場合
サブゴール（移動可能ノード）が十字路内にある場合などには、退避候補位置生成部３９２ａが、退避候補位置を４点設定する。以下、４点設定の場合について、図２１ないし図２４を参照して説明する。
まず、退避候補位置生成部３９２ａが、サブゴールデータＣａｌ＿ＳＧ_A（ｉ_A）に対応するリンクを探索する。ここでは、図２１に示すように、サブゴールデータＣａｌ＿ＳＧ_A（ｉ_A）に対応するリンクが、Ｌｉｎｋ（１１），Ｌｉｎｋ（１２），Ｌｉｎｋ（１３），Ｌｉｎｋ（１４）の四つ存在している。これらのリンクＬｉｎｋ（１１），Ｌｉｎｋ（１２），Ｌｉｎｋ（１３），Ｌｉｎｋ（１４）は、十字状に配置されている。
そして、図２２に示すように、退避候補位置生成部３９２ａが、これらのリンクが作る二つのベクトルＬｉｎｋ＿Ｖｅｃｔｏｒ（１），Ｌｉｎｋ＿Ｖｅｃｔｏｒ（２）に対して垂直であり、かつ、サブゴールＣａｌ＿ＳＧ_A（ｉ_A）を通る２本の直線Ｌ（１），Ｌ（２）を引く。
続いて、退避候補位置生成部３９２ａが、この直線Ｌ（１），Ｌ（２）上それぞれに、ロボット間最小マージンを満たし、かつ、壁とのマージンを満足する２点の平面座標に関するデータを、計４点の退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（３），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（４）として設定する。ここでは、図２３に示すように、直線Ｌ（１）上に２点の退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（３）が設定され、直線Ｌ（２）上に２点の退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２），Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（４）が設定される。
設定した退避候補位置が他のリンクが作るベクトル上に存在する場合には、退避候補位置生成部３９２ａが、ベクトル同士のなす角度の半分を回転角とし、サブゴールを中心に全ての退避候補位置を回転する。ここでは、図２４に示すように、各退避候補位置を、サブゴールＣａｌ＿ＳＧ_A（ｉ_A）を中心として時計回りに４５度回転させる。

続いて、退避位置決定部３９２ｂが、ｉ_A＝１とし（Ｓ３０３）、退避候補位置に対して、当該退避候補位置が退避位置に適しているか否かを判定するための計算（ｉ）〜（ｉｉｉ）を実行する。

＜計算（ｉ）＞
まず、退避候補位置が４点存在する場合には、２点に絞り込む。
退避候補位置が４点存在する場合とは、図２５に示すように、サブゴールＣａｌ＿ＳＧ_A（ｉ_A）が交差点内に位置する場合などである。
まず、退避位置決定部３９２ｂは、優先ロボットＲＢの現在位置と移動終了位置とを結ぶベクトルを生成する。このベクトルを、
Ｄｅｓ＿Ｖｅｃｔｏｒ
と記載する。
また、退避位置決定部３９２ｂは、４点の退避候補位置のうち、対角に位置する２点を結ぶベクトルを生成する。これら２個のベクトルを、
Ｐａｔｈ＿Ｖｅｃｔｏｒ（１）
Ｐａｔｈ＿Ｖｅｃｔｏｒ（２）
と記載する。
そして、Ｄｅｓ＿ＶｅｃｔｏｒとＰａｔｈ＿Ｖｅｃｔｏｒ（１）とがなす角の小さい方を
ａｒｇ（１）
と記載し、Ｄｅｓ＿ＶｅｃｔｏｒとＰａｔｈ＿Ｖｅｃｔｏｒ（２）とがなす角の小さい方を
ａｒｇ（２）
と記載する。そして、退避位置決定部３９２ｂは、ａｒｇ（１）とａｒｇ（２）との大きさを比較し、大きい方の角を与える退避候補位置の対（本実施形態では、Ｏｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）及びＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（４））を、退避候補位置として採用する。

ここで、４点の退避候補位置を回転させた場合と、回転させない場合とにおけるロボットＲＡ，ＲＢの退避／回避動作について比較する。図２６は、退避候補位置が４点ある場合のロボットの退避／回避動作の例を説明する模式図であり、（ａ）は退避候補位置を回転させた場合を示す図、（ｂ）は退避候補位置を回転させない場合を示す図である。
図２６（ａ）に示す例では、退避候補位置を回転させた状態で、非優先ロボットＲＡがＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（４）に退避し、優先ロボットＲＢがＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）を回避中継位置として回避移動する。
一方、図２６（ｂ）に示す例では、退避候補位置を回転させない状態で、非優先ロボットＲＡがＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（１）またはＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（４）に退避し、優先ロボットＲＢがＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）及びＯｆｆｓｅｔ＿Ｐａｔｈ（ｉ_A）（２）を回避中継位置として回避移動する。
図２６（ａ），（ｂ）からもわかるように、４点の退避候補位置を回転させた状態で、対角上の２点の退避候補位置を採用することで、ロボットＲＡ，ＲＢの距離を確保することができ、より好適に衝突を避けることができる。

＜計算（ｉｉ）＞
２点の退避候補位置に対して、以下の計算を行う。
まず、退避位置決定部３９２ｂは、距離Ｄａ，Ｄｂを算出する。

Ｄａ＝（当該退避候補位置と近傍のロボットＲ全てとの距離の最小値）
Ｄｂ＝（当該退避候補位置と優先ロボットＲＢの移動終了位置（ゴール）との距離）

ここで、Ｄａを算出する際の近傍のロボットＲ全てとは、退避候補位置から所定距離内にある非優先ロボットＲＡ以外の全てのロボットＲのことであり、優先ロボットＲＢも含まれる。本実施形態では、優先ロボットＲＢと、その他のロボットＲＣが「近傍のロボットＲ全て」に該当する（図１６参照）。
図１６には、退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（２）（１）に対するＤａ，Ｄｂが示されている。本実施形態では、退避候補位置と優先ロボットＲＢとの距離Ｄｒ（１）が退避候補位置とその他のロボットＲＣとの距離Ｄｒ（２）よりも小さい。したがって、距離Ｄｒ（１）がＤａとなる。
そして、退避位置決定部３９２ｂは、ＤａとＤｂのうち、小さい方を当該退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（２）（１）に対するＤｍｉｎ（＝Ｄｍｉｎ（１））とする。本実施形態では、ＤａがＤｍｉｎ（＝Ｄｍｉｎ（１））となる。
また、退避位置決定部３９２ｂは、対となる退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（２）（２）についても同様の計算を行い、退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（２）（２）に対するＤｍｉｎ（＝Ｄｍｉｎ（２））を算出する。

＜計算（ｉｉｉ）＞
退避候補位置２点に対するＤｍｉｎをそれぞれＤｍｉｎ（１），Ｄｍｉｎ（２）とし、Ｄｍｉｎ（１），Ｄｍｉｎ（２）のうち、大きい方をＤｍｉｎ＿ｆ、小さい方をＤｍｉｎ＿ｎとする。
すなわち、Ｄｍｉｎ＿ｆ，Ｄｍｉｎ＿ｎは、以下の式によって求められる。

Ｄｍｉｎ＿ｆ＝Ｍａｘ（Ｄｍｉｎ（１），Ｄｍｉｎ（２））
Ｄｍｉｎ＿ｎ＝ｍｉｎ（Ｄｍｉｎ（１），Ｄｍｉｎ（２））

本実施形態では、 Ｄｍｉｎ＿ｆ＝Ｄｍｉｎ（１） ， Ｄｍｉｎ＿ｎ＝Ｄｍｉｎ（２） である。
そして、
Ｄｍｉｎ＿ｆ＞（ロボット間最小距離＋マージン） …式（２）
を満たす場合には、退避位置決定部３９２ｂは、Ｄｍｉｎ＿ｆを与える退避候補位置を退避位置とし、回避中継位置生成部３９７ａは、Ｄｍｉｎ＿ｎを与える退避候補位置を回避中継位置とする（Ｓ３０５）。
なお、式（２）を満たし、かつ、 Ｄｍｉｎ＿ｆ＝Ｄｍｉｎ＿ｎ である場合には、退避位置決定部３９２ｂは、Ｄｂが大きい方の退避候補位置を退避位置として選択する。これは、優先ロボットＲＢが、その移動終了位置に近い方を回避中継位置とするための措置である。
さらに、一対の退避候補位置の各Ｄｂの値が等しい場合には、退避位置決定部３９２ｂは、Ｄｍｉｎ（１）を与える退避候補位置を退避位置として選択する。

前記式（２）を満たさない場合には、退避位置決定部３９２ｂは、ｉ_A＝１における退避候補位置については解なしと判断する。そして、ｉ_Aを一つだけ大きくし（Ｓ３０７）、退避候補位置Ｏｆｆｓｅｔ＿Ｐａｔｈ（２）（ｊ_A）についてステップＳ３０４，Ｓ３０５を実行する。

本実施形態では、退避位置決定部３９２ｂは、ｉ_A＝１から順に退避候補位置を探索する。そして、解が見つかった時点で探索を終了する。これは、小さいｉ_Aに基づく退避候補位置の方が、非優先ロボットＲＡのゴールにより近いため、より好適な退避位置であると考えられるためである。
なお、本実施形態では、図１６に示すように、退避位置決定部３９２ｂは、退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（２）（１）を退避位置Ｐ１に関する退避位置データとして設定し、回避中継位置生成部３９７ａは、退避候補位置データＯｆｆｓｅｔ＿Ｐａｔｈ（２）（２）を回避中継位置Ｐ２に関する回避中継位置データとして設定する。そして、非優先ロボットＲＢがシナリオ復帰命令信号を受信すると、非優先ロボットＲＢはタスクを再開し、サブゴールＣａｌ＿ＳＧ_A（１）に向かって移動する。

解が見つからない場合（Ｓ３０５でｎｏ、かつ、Ｓ３０６でｙｅｓ）、または、Ｓ３０１においてサブゴールがない場合には、退避候補位置生成部３９２ａは、非優先ロボットＲＡの現在位置を始点とし、渦巻状に退避候補位置を探索し（Ｓ３０８）、探索された退避候補位置について、退避候補位置生成部３９２ａ及び退避位置決定部３９２ｂが、計算（ｉ）〜（ｉｉｉ）を実行する（Ｓ３０９）。かかる演算は、探索半径Ｒ’まで続行される（Ｓ３１０，Ｓ３１１）。

ここで、渦巻状の探索について説明する。退避候補位置生成部３９２ａが、適当な負でない実数ｔに対して、非優先ロボットＲＡの現在位置を原点とした平面座標（ｘ，ｙ）において、

ｘ＝（ｒ＋ｆｔ）ｓｉｎ（ｇｔ）
ｙ＝（ｒ＋ｆｔ）ｃｏｓ（ｇｔ）

を算出する（ｆ，ｇ，ｒは定数。）。退避候補位置生成部３９２ａ及び退避位置決定部３９２ｂは、算出されたｘ，ｙに最も近いノード（移動可能ノード）に対して前記計算を行い、解（退避位置）が存在するかを調べる。

これらを実行しても解が見つからなかった場合には（Ｓ３１０でｎｏ、かつ、Ｓ３１１でｙｅｓ）、優先ロボットと非優先ロボットとの設定を反転させ（Ｓ３１２）、退避候補位置生成部３９２ａ及び退避位置決定部３９２ｂが、新たに非優先ロボットとなったロボットＲＢについて退避位置算出処理を行う。

続いて、優先ロボットＲＢの回避位置の算出について説明する。
図２７は、回避位置生成部による優先ロボットの回避位置生成を説明するための模式図である。図２８は、回避位置生成部による回避位置生成動作を示すフローチャートである。以下に説明するステップＳ４０１〜Ｓ４０８は、図１４のフローチャートにおけるステップＳ１１５のサブルーチンに相当する。

優先ロボットＲＢの回避移動は、回避中継位置及び回避終了位置を算出することによって行われる。ここで、回避中継位置の生成は、前記した非優先ロボットＲＡの退避位置の生成の際に行われており、回避中継位置生成部３９７ａは、既に生成された回避中継位置データを受け取ることによって、回避中継位置を設定する。

まず、回避終了位置生成部３９７ｂは、優先ロボットＲＢのサブゴールから回避終了候補位置を抽出する（Ｓ４０１）。
回避移動を行う優先ロボットＲＢは、Ｎ_B個のサブゴールを有しているものとする。そして、ｎ_B個目のサブゴールに関するサブゴールデータを、２次元座標上の点として定義し、
ＳＧ_B（ｎ_B） （ｎ_B＝１，２，…，Ｎ_B）
と記載する。

優先ロボットＲＢが現在ｍ_B番目のサブゴールを目標として移動していると仮定する。この場合に、回避終了候補位置に関する回避終了候補位置データを、
Ｃａｌ＿ＳＧ_B（ｉ_B） （ｉ_B＝１，２，…ｋ）
と記載する。
Ｃａｌ＿ＳＧ_B（１）＝ＳＧ（ｍ_B）
…
Ｃａｌ＿ＳＧ_B（ｋ）＝ＳＧ（Ｎ_B） （ｋ＝Ｎ_B−ｍ_B＋１）
と、ｋ個、すなわち（Ｎ_B−ｍ_B＋１）個のサブゴールの回避終了候補位置Ｃａｌ＿ＳＧ_B（ｉ_B）が存在する。

そして、回避終了位置生成部３９７ｂは、ｉ_B＝１と設定し（Ｓ４０２）、Ｃａｌ＿ＳＧＢ（１）に対して距離Ｄｃ，Ｄｄ，Ｄｅを算出する（Ｓ４０３）。

Ｄｃ＝（当該回避終了候補位置と非優先ロボットの退避位置との距離）
Ｄｄ＝（当該回避終了候補位置と優先ロボットの現在位置との距離）
Ｄｅ＝（当該回避終了候補位置と他のロボットの現在位置との距離の最小値）

ここでいう他のロボットとは、非優先ロボットＲＡ及び優先ロボットＲＢ以外の全てのロボットＲのことである。
なお、図２７には、回避終了候補位置データＣａｌ＿ＳＧ_B（３）におけるＤｃ，Ｄｄ，Ｄｅを図示している。

そして、回避終了位置生成部３９７ｂは、回避終了候補位置Ｃａｌ＿ＳＧ_B（ｉ_B）に対して以下の条件を当てはめ、以下の条件（ｉｖ），（ｖ）を満たすか否かを判定する（Ｓ４０４）。

（ｉｖ） 当該回避終了候補位置が優先ロボットＲＢの移動終了位置（最終ゴール）（ｉ_B＝ｋ）でない場合
Ｄｃ＞（ロボット間最小距離＋マージン） …（３）
Ｄｄ＞（非優先ロボットの現在位置と優先ロボットの現在位置との距離＋マージン） …（４）
Ｄｅ＞（ロボット間最小距離＋マージン） …（５）

Ｄｃが式（３）を満たせば、「回避終了位置が、退避位置に存在する非優先ロボットＲＡから十分な距離を保持している」ことになる。
Ｄｄが式（４）を満たせば、「回避終了位置が、非優先ロボットよりも遠く（優先ロボットＲＢのゴール側）に設定されている」ことになる。
Ｄｅが式（５）を満たせば、「回避終了位置が、他のロボットＲ（非優先ロボットＲＡを含む）から十分な距離を保持している」ことになる。

（ｖ） 当該回避終了候補位置が最終ゴール（ｉ_B＝ｋ）である場合
Ｄｃ＞（ロボット間最小距離＋マージン） …（６）
Ｄｄ＞（ロボット間最小距離＋マージン） …（７）
Ｄｅ＞（ロボット間最小距離＋マージン） …（８）

Ｄｃ，Ｄｄ，Ｄｅがこれらの式（６）、（７）、（８）を満たせば、「回避終了位置が、優先ロボットＲＢ以外の全てのロボットＲに接近していない」ことになる。

これらの条件を満たさない場合には、回避終了位置生成部３９７ｂは、ｉ_Bの値を１ずつ増やし（Ｓ４０６）、条件を満たす回避終了候補位置が表れるまでステップＳ４０３，Ｓ４０４を繰り返す。

条件を満たす場合には、回避終了位置生成部３９７ｂは、当該回避終了候補位置が最終ゴールであるか否かを判定する（Ｓ４０７）。当該回避目終了候補位置が最終ゴールでないと判定された場合には、回避終了位置生成部３９７ｂは、この回避終了候補位置を回避終了位置とする。当該回避終了候補位置が最終ゴールであると判定された場合には、回避終了位置生成部３９７ｂは、回避中継位置を回避終了位置とする（Ｓ４０８）。

また、すべてのｉ_Bについて条件を満たさない場合には（Ｓ４０４でｎｏ、かつ、Ｓ４００５でｙｅｓ）、回避終了位置生成部３９７ｂは、回避中継位置を回避終了位置とする（Ｓ４０８）。

さらに、ステップＳ４０１において回避終了候補位置を抽出できない場合、例えば、優先ロボットＲＢがサブゴールを持っていない場合には、回避終了位置生成部３９７ｂは、回避中継位置を回避終了位置とする（Ｓ４０９）。
なお、本実施形態では、図２７に示すように、回避終了位置生成部３９７ｂは、回避終了候補位置データＣａｌ＿ＳＧ_Ｂ（３）を回避終了位置Ｐ３に関する回避終了位置データとして設定する。

本発明の実施形態に係るロボット制御装置３は、ロボットＲの接近を判定し、続いて接近した２台のロボットＲ，Ｒの衝突可能性を判定し、衝突可能性のある２台のロボットＲ，Ｒの衝突を避ける装置である。すなわち、ロボット制御装置３は、通信を用いて各ロボットＲの状態と行動プランニング（タスク）を監視し、必要なときにのみロボットＲに対してその行動プランニングに応じた衝突回避動作（退避または回避）を送信する。このように、衝突可能性がある場合にのみ、２台のロボットＲ，Ｒの衝突を避ける制御を行うので、通常時（衝突可能性のない場合）のロボットＲの移動に影響を与えることがない。また、２台のロボットＲ，Ｒの優先順位を判定し、優先順位に基づいて退避、回避を行わせるので、ロボット制御システムＡ全体におけるロボットタスク実行の遂行効率を向上させることができる。

以上、各実施形態について詳細に説明したが、本発明は、前記した各実施形態に限定されず、本発明の要旨を逸脱しない範囲で適宜設計変更可能である。
例えば、ロボットＲの自律移動経路（サブゴール）の生成手法は、前記したものに限定されない。また、ロボット制御装置３がサブゴールを生成し、ロボットＲに送信する構成であってもよい。

本発明の実施形態に係るロボット制御システムを示すシステム構成図である。 図１のロボットを示すブロック図である。 ロボットのサブゴールを説明するための模式図である。 図１のロボット制御装置を示すブロック図である。 接近判定手段による接近判定を説明するためのグラフである。 図４の衝突可能性判定手段を示すブロック図である。 サブゴール補間部によるサブゴールの補間を説明するための模式図であり、（ａ）は補間前を示す図、（ｂ）は補間後を示す図である。 図４の移動経路変更命令手段を示すブロック図である。 ２台のロボットの退避及び回避を説明するための模式図である。 ２台のロボットの退避及び回避を説明するための模式図である。 ２台のロボットの退避及び回避を説明するための模式図である。 ２台のロボットの退避及び回避を説明するための模式図である。 本発明の実施形態に係るロボット制御システムによる、２台のロボットの衝突を避けるためのロボット及びロボット制御装置の動作を示すフローチャートである。 本発明の実施形態に係るロボット制御装置の動作を示すフローチャートである。 接近判定手段及び衝突可能性判定手段の動作を示すフローチャートである。 退避位置生成部による非優先ロボットの退避位置生成を説明するための模式図である。 退避位置生成部による退避位置生成動作を示すフローチャートである。 退避候補位置を２点設定する場合を説明するための模式図である。 退避候補位置を２点設定する場合を説明するための模式図である。 退避候補位置を２点設定する場合を説明するための模式図である。 退避候補位置を４点設定する場合を説明するための模式図である。 退避候補位置を４点設定する場合を説明するための模式図である 退避候補位置を４点設定する場合を説明するための模式図である 退避候補位置を４点設定する場合を説明するための模式図である 退避候補位置が４点存在する場合を説明するための模式図である。 退避候補位置が４点ある場合のロボットの退避／回避動作の例を説明する模式図であり、（ａ）は退避候補位置を回転させた場合を示す図、（ｂ）は退避候補位置を回転させない場合を示す図である。 回避位置生成部による優先ロボットの回避位置生成を説明するための模式図である。 回避位置生成部による回避位置生成動作を示すフローチャートである。

符号の説明

３ ロボット制御装置
３１０ ロボット選択手段
３２０ 現在位置取得手段
３３０ 移動速度取得手段
３４０ 接近判定手段
３５０ サブゴール取得手段
３６０ 衝突可能性判定手段
３７０ 優先順位設定手段
３８０ 地図取得手段
３９０ 移動経路変更命令手段

Claims (8)

1. 移動経路に基づいて移動する機能を備えた複数のロボットと通信を行い、前記ロボット同士の衝突を避けるために複数の前記ロボットの移動を制御するロボット制御装置であって、
   前記移動経路が形成される移動領域に関する地図データを取得する地図取得手段と、
   前記ロボットの現在位置に関する現在位置データを取得する現在位置取得手段と、
   前記移動経路に設定された有限個のサブゴールに関するサブゴールデータを取得するサブゴール取得手段と、
   取得した前記サブゴールデータに基づいて、２台の前記ロボットが衝突する可能性があるか否かを判定する衝突可能性判定手段と、
   衝突可能性がある２台の前記ロボットのうち、少なくとも１台の前記ロボットの前記移動経路を、前記地図データ、前記現在位置データ及び前記サブゴールデータに基づいて、衝突可能性がある他の前記ロボットとの衝突を避けるように変更させる移動経路変更命令信号を生成する移動経路変更命令手段と、
   前記移動経路変更命令信号を対応する前記ロボットに送信する送信手段と、
   衝突可能性がある２台の前記ロボットの優先順位を、優先順位が高い方を優先ロボット、優先順位が低い方を非優先ロボットと設定する優先順位設定手段と、
   を備え、
   前記移動経路変更命令手段は、少なくとも前記非優先ロボットの移動経路を、前記優先ロボットとの衝突を避けるように変更させる移動経路変更命令信号を生成し、
   前記移動経路変更命令手段は、
   前記移動経路変更命令信号として、前記非優先ロボットの移動経路を、退避位置に移動することによって前記優先ロボットの移動経路から退避するように変更させる退避命令信号を生成する退避命令手段と、
   前記移動経路変更命令信号として、前記優先ロボットの移動経路を、前記退避位置に退避した前記非優先ロボットを回避して移動するように変更させる回避命令信号を生成する回避命令手段と、
   を備えており、
   前記退避命令手段は、
   前記地図データ及び前記サブゴールデータに基づいて、前記退避位置に関する退避位置データを生成する退避位置生成部と、
   前記退避位置データに基づいて、前記非優先ロボットを前記退避位置に退避させる前記退避命令信号を生成する退避命令生成部と、
   を備えており、
   前記回避命令手段は、
   前記退避位置に関する退避位置データ、前記地図データ及び前記サブゴールデータに基づいて、前記優先ロボットが回避移動する回避位置に関する回避位置データを生成する回避位置生成部と、
   前記回避位置データに基づいて、前記優先ロボットを前記回避位置に回避させる回避命令信号を生成する回避命令生成部と、
   を備えており、
   前記回避位置生成部は、
   前記地図データ及び前記退避位置データに基づいて、前記退避位置に退避した前記非優先ロボットを回避しつつ通過する回避中継位置に関する回避中継位置データを生成する回避中継位置生成部と、
   前記優先ロボットが前記移動経路に復帰する回避終了位置に関する回避終了位置データを生成する回避終了位置生成部と、
   を備えており、
   前記回避命令生成部は、前記回避中継位置データ及び前記回避終了位置データに基づいて、前記優先ロボットを、前記回避中継位置を経由して前記回避終了位置に移動させる前記回避命令信号を生成し、
   前記退避位置生成部は、
   前記現在位置データ、前記サブゴールデータ及び前記地図データに基づいて、前記サブゴールごとに、退避候補位置に関する退避候補位置データを生成する退避候補位置生成部と、
   前記退避候補位置データ及び前記現在位置データに基づいて、前記退避候補位置から退避位置を決定する退避位置決定部と、
   を備えており、
   前記退避候補位置生成部は、前記サブゴールごとに、二以上の前記退避候補位置に関する前記退避候補位置データを生成し、
   前記回避中継位置生成部は、前記退避位置に対応する前記サブゴールの、残りの前記退避候補位置から、前記回避中継位置を決定する
   ことを特徴とするロボット制御装置。
2. 前記ロボットの移動速度に関する移動速度データを取得する移動速度取得手段と、
   取得した前記現在位置データ及び前記移動速度データに基づいて、２台の前記ロボットが接近しているか否かを判定する接近判定手段と、
   をさらに備え、
   前記接近判定手段が、２台の前記ロボットが接近していると判定した場合に、前記衝突可能性判定手段が、接近している２台の前記ロボットが衝突する可能性があるか否かを判定する
   ことを特徴とする請求項１に記載のロボット制御装置。
3. 複数の前記ロボットから２台の前記ロボットを選択するロボット選択手段をさらに備え、
   前記現在位置取得手段は、選択した２台の前記ロボットの前記現在位置データを取得し、
   前記移動速度取得手段は、選択した２台の前記ロボットの前記移動速度データを取得し、
   前記接近判定手段は、取得した前記位置データ及び前記移動速度データに基づいて、選択した２台の前記ロボットが接近しているか否かを判定する
   ことを特徴とする請求項２に記載のロボット制御装置。
4. 前記接近判定手段が、２台の前記ロボットが接近していると判定した場合に、前記サブゴール取得手段が、接近している２台の前記ロボットの前記サブゴールデータを取得する
   ことを特徴とする請求項２または請求項３に記載のロボット制御装置。
5. 前記衝突可能性判定手段は、
   接近している２台の前記ロボットの前記サブゴールデータから一つずつ選択するサブゴール選択部と、
   選択した前記サブゴールデータに基づいてサブゴール間距離を算出するサブゴール間距離算出部と、
   算出した前記サブゴール間距離が所定距離以下であるか否かを判定するサブゴール間距離判定部と、
   選択した前記サブゴールデータ並びに取得した前記現在位置データ及び前記移動速度データに基づいて、接近している２台の前記ロボットが対応する前記サブゴールに到達するのに要する到達時間をそれぞれ算出する到達時間算出部と、
   算出した前記到達時間の差が一定時間内であるか否かを判定する到達時間判定部と、
   前記サブゴール間距離判定部が、前記サブゴール間距離が所定距離以下であると判定し、かつ、前記到達時間判定部が、前記到達時間の差が一定時間内であると判定した場合に、接近している２台の前記ロボットが衝突する可能性があると判定する衝突可能性判定部と、
   を備えていることを特徴とする請求項２から請求項４のいずれか一項に記載のロボット制御装置。
6. 前記サブゴール間距離判定部が、前記サブゴール間距離が所定距離以下であると判定した場合に、前記到達時間算出部が、接近している２台の前記ロボットが対応する前記サブゴールに到達するのに要する到達時間を算出する
   ことを特徴とする請求項５に記載のロボット制御装置。
7. 前記衝突可能性判定手段は、前記サブゴール取得手段が取得した前記サブゴールデータから、隣り合うサブゴールデータを補間するため新たなサブゴールデータを生成するサブゴール補間部をさらに備え、
   前記サブゴール補間部は、補間された前記サブゴールデータを前記サブゴール選択部に出力する
   ことを特徴とする請求項５または請求項６に記載のロボット制御装置。
8. 前記優先順位設定手段は、衝突可能性がある２台の前記ロボットのタスクの種類に基づいて優先順位を設定する
   ことを特徴とする請求項１から請求項７のいずれか一項に記載のロボット制御装置。

Images