特開 2024-121906 移動体制御システム

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024121906

(43)【公開日】2024-09-09

(54)【発明の名称】移動体制御システム

(51)【国際特許分類】

   G05D 1/43 20240101AFI20240902BHJP

【ＦＩ】

G05D1/02 H

【審査請求】未請求

【請求項の数】13

【出願形態】ＯＬ

(21)【出願番号】P 2023029137

(22)【出願日】2023-02-28

(71)【出願人】

【識別番号】000005108

【氏名又は名称】株式会社日立製作所

(71)【出願人】

【識別番号】504176911

【氏名又は名称】国立大学法人大阪大学

(74)【代理人】

【識別番号】110002572

【氏名又は名称】弁理士法人平木国際特許事務所

(72)【発明者】

【氏名】溝口 将史

(72)【発明者】

【氏名】潮 俊光

【テーマコード（参考）】

5H301

【Ｆターム（参考）】

5H301AA01

5H301BB14

5H301CC03

5H301CC06

5H301CC10

5H301LL03

5H301LL06

5H301LL11

(57)【要約】      （修正有）

【課題】フィールドを共有する各移動体をそれぞれの目的地まで移動させる制御を安全性保証と低コストを両立して実現する。
【解決手段】移動体制御システムは、目的地までの移動を制御する目的地移動用制御入力決定部と目的地移動用制御入力決定部に入力値を修正する制御入力修正部とデッドロックを検知するデッドロック検知部と自己駆動させる駆動部と、を有する移動体を複数備えた移動体制御システムであって、移動体の制御入力修正部はデッドロック検知部がデッドロックを検知した場合に回避移動が必要と判断して、制御入力修正部から回避移動の指示を受けた場合に自己位置情報と障害物位置情報と他者ロボット位置情報と他者ロボットロック位置情報とに基づき回避軌道を生成し、出力部は自己の現在位置を示す自己ロボット位置情報と生成した回避軌道上にあって移動体が未通過の位置を自己ロボットロック位置情報として他の移動体に出力する。
【選択図】図４

【特許請求の範囲】

【請求項1】

目的地までの移動を制御する目的地移動用制御入力決定部と前記目的地移動用制御入力決定部に入力された値を修正する制御入力修正部とデッドロックを検知するデッドロック検知部と自己を駆動させる駆動部と、を有する移動体を複数備えた移動体制御システムであって、
前記移動体の各々は回避移動用有限状態遷移モデル動的生成部と出力部とを有し、
前記移動体の前記制御入力修正部は前記デッドロック検知部がデッドロックを検知した場合に回避移動が必要と判断して前記回避移動用有限状態遷移モデル動的生成部に指示し、
前記回避移動用有限状態遷移モデル動的生成部は前記制御入力修正部から回避移動の指示を受けた場合に自己位置情報と障害物位置情報と他者ロボット位置情報と他者ロボットロック位置情報とに基づき回避移動用有限状態遷移モデルを動的に生成して回避軌道を生成し、
前記出力部は自己の現在位置を示す自己ロボット位置情報と前記生成した回避軌道上にあって前記移動体が未通過の位置を自己ロボットロック位置情報として他の移動体に出力する、
ことを特徴とする移動体制御システム。

【請求項2】

請求項１に記載の移動体制御システムであって、
前記制御入力修正部は、他者ロボットと衝突する可能性を検知した場合において、前記目的地移動用制御入力決定部が決定した制御入力に関わらず、該制御入力修正部が搭載された移動体を停止させるよう前記駆動部に指示する、
ことを特徴とする移動体制御システム。

【請求項3】

請求項２に記載の移動体制御システムであって、
前記制御入力修正部は、他者ロボット位置情報および他者ロボットロック位置情報に含まれる位置に自己移動体が進入しようとした場合に該移動体を停止させ、
前記デッドロック検知部は前記移動体の停止が一定回数以上連続して発生した場合にデッドロックを検知する、
ことを特徴とする移動体制御システム。

【請求項4】

請求項３に記載の移動体制御システムであって、
前記制御入力修正部は、他者ロボット位置情報または他者ロボットロック位置情報の少なくとも１つの受信に失敗した場合において、前記目的地移動用制御入力決定部が決定した制御入力に関わらず、当該制御入力修正部が搭載された移動体を停止させるよう前記駆動部に指示する、
ことを特徴とする移動体制御システム。

【請求項5】

請求項４に記載の移動体制御システムであって、
前記他者ロボット位置情報または他者ロボットロック位置情報の受信を失敗したことに起因する前記制御入力修正部による停止指示の上書きは、前記デッドロック検知部によるデッドロック検知判定のための停止の連続回数カウントに含まない、
ことを特徴とする移動体制御システム。

【請求項6】

請求項５に記載の移動体制御システムであって、
前記制御入力修正部は、受信した他者ロボット位置情報および他者ロボットロック位置情報と、当該制御入力修正部が搭載された移動体に含まれる前記回避移動用有限状態遷移モデル動的生成部が生成し他者ロボットに送信した自己ロック位置と、を比較し、前記受信した位置情報と前記自己ロック位置が重複していなければ前記回避移動用有限状態遷移モデル動的生成部が生成した前記回避移動用有限状態遷移モデルに従い制御入力を決定して前記駆動部に当該制御入力を印加し、前記受信した位置情報と前記自己ロック位置が重複していれば前記回避移動用有限状態遷移モデルを破棄して前記駆動部に停止命令を印加するとともに前記回避移動用有限状態遷移モデル動的生成部に対して回避移動用有限状態遷移モデルを再生成するよう指示する、
ことを特徴とする移動体制御システム。

【請求項7】

請求項６に記載の移動体制御システムであって、
前記目的地移動用制御入力決定部および前記制御入力修正部はそれぞれハイレベル制御器を有し、
前記移動体の各々はローレベル制御器を有し、
前記ハイレベル制御器は前記移動体の状態空間を有限の状態数に分割した抽象化状態空間を構成し前記抽象化状態空間における各状態において各制御入力を印加した場合にどの状態に遷移するかに基づき有限状態遷移モデルを生成し当該生成した有限状態遷移モデル上で目的地までの移動経路を計画し当該計画に基づき制御入力を並進または回転のいずれかにするかを決定し、
前記ローレベル制御器は前記ハイレベル制御器の指示に従い前記各移動体が並進または回転を行うように前記駆動部に含まれるモータまたはエンジンまたはその他移動体を移動させる駆動力を発生する装置を制御する、
ことを特徴とする移動体制御システム。

【請求項8】

請求項７に記載の移動体制御システムであって、
前記回避移動用有限状態遷移モデル動的生成部が生成する回避移動用有限状態遷移モデルは前記ハイレベル制御器における前記有限状態遷移モデルの構成において他者の移動体から受信したロック位置への状態遷移を禁止することによって得られる、
ことを特徴とする移動体制御システム。

【請求項9】

請求項８に記載の移動体制御システムであって、
前記移動体の各々に含まれる前記ハイレベル制御器は同一の方法により抽象化状態空間を構成し、
前記各移動体が送信および受信する前記他者ロボット位置情報、前記自己ロボット位置情報、前記他者ロボットロック位置情報及び前記自己ロボットロック位置情報は前記抽象化状態空間上における位置の情報として記述される、
ことを特徴とする移動体制御システム。

【請求項10】

請求項９に記載の移動体制御システムであって、
前記移動体の各々は制御マイコンを有し、
前記ハイレベル制御器は該制御マイコン上に実装され、
前記制御マイコンは、前記有限状態遷移モデルの実装に必要となるサイズ以上のメモリ領域を有する、
ことを特徴とする移動体制御システム。

【請求項11】

請求項１０に記載の移動体制御システムであって、
前記ハイレベル制御器による目的地までの移動経路の計画は、ハイレベル制御器が生成する有限状態遷移モデルを重み付きの有向グラフと見立てて動的計画法を適用することにより得られる、
ことを特徴とする移動体制御システム。

【請求項12】

請求項１１に記載の移動体制御システムであって、
前記回避移動用有限状態遷移モデル動的生成部による回避軌道の生成は、前記動的計画法を適用する際に設定する目的地を乱数生成により確率的に選択する、
ことを特徴とする移動体制御システム。

【請求項13】

請求項１２に記載の移動体制御システムであって、
前記有限状態遷移モデルは構造体配列によって表現され、前記構造体配列の要素である構造体の各々には少なくとも１つの移動体の位置と角度と、各制御入力を印加した際に遷移可能な移動体の状態に対応する構造体へのポインタを格納する、
ことを特徴とする移動体制御システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、複数の移動体を制御する移動体制御システムに関する。

【背景技術】

【0002】

人手不足等を背景に、物流倉庫や建設現場や空港や道路等において地上または空中を自律的に移動するロボットや車両やＵＡＶ（Ｕｎｍａｎｎｅｄ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅ）の開発が期待されている。これらにおいては、同一のフィールドに複数の移動体が存在するため、障害物や他の移動体に衝突させることなく安全に各移動体を所定の目的地まで移動させることが求められる。

【0003】

この分野に関連する技術として、特許文献１に記載の技術がある。この公報においては、複数のロボットの存在を考慮しつつも、計画計算に必要な計算時間や計算機の記憶容量を低減可能な、ロボット協調搬送計画技術を提供することを課題としている。そしてその解決手段として、マルコフ状態空間を階層的に構成し（物品軌道計算処理を第１階層、切り替え位置決定処理を第２階層、移動経路計画処理を第３階層とする）、動作計画の探索を変化頻度の低い階層（第１階層）から行い、そして、変化頻度の低い階層での探索計算の結果を使用して、変化頻度の高い階層における探索の範囲を限定し、限られた状態空間において変化頻度の高い下位の階層（第２階層、第３階層）における探索を行うことが記載されている。

【0004】

また、特許文献２によれば、対象物とロボットの一方または両方の内部状態を推定し、この内部状態に基づいて、ロボットの制御に必要な予測値（制御指令値）を生成する場合に、内部状態の更新処理にかかる時間の影響を受けることなく、決められた制御周期で制御指令値を生成できる運動予測制御装置と方法を提供することを目的としている。そして当該目的の達成のため、対象物とロボットの一方または両方を計測することによりセンサ情報を取得する計測装置と、センサ情報に基づいて、対象物とロボットの一方または両方の内部状態を予測して更新する状態推定装置と、前記内部状態を記憶するデータ記憶装置と、前記ロボットを制御するロボット制御装置と、を備え、状態推定装置は、ロボットの制御周期に依らない任意のタイミングで、前記内部状態を更新し、ロボット制御装置は、前記制御周期で、データ記憶装置に記憶された最新の内部状態に基づいて、ロボットの制御に必要な予測値を算出する、ことを特徴とする運動予測制御装置を提供している。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２０１２－２２７３４９号公報

【特許文献2】国際公開２０１２／１５３６２９号公報

【発明の概要】

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

【0006】

特許文献１によれば、複数のロボットのとりうる状態を直接考慮すると状態数が膨大となるところ、状態空間を有限に分割したマルコフ遷移モデルを作成し当該遷移モデル上で軌道または行動計画を探索することにより、計画計算に必要な計算時間や計算機の記憶容量を低減することを可能としている。しかしながら、本公知例はロボットの協調動作を想定してなされたものであり、各ロボットの行動計画をあらかじめ立案する必要があることから、例えば動作中に一部の通信パケットの遅延または途絶等の行動計画と相違する状況が発生した場合、いったん全てのロボットを停止させ、再度行動計画を立てる必要がある。これは、全体システムの稼働率を低下させる。特に、物流倉庫や建設現場や空港や道路等において自律的に移動するロボットや車両に関しては、必ずしもすべてのロボットまたは車両が互いに協調して作業を行っているわけではなく、また、構成するロボットまたは車両の数が増大し通信量が増加するほど１つ以上のパケットの遅延や途絶は高い頻度で容易に発生しうる。したがって、全体システムの稼働率が深刻に低下することから適用できない。

【0007】

一方、特許文献２によれば、ロボットの外部からのセンシング情報や他のロボットからの情報を受信する前にカルマンフィルタ等の技術を用いてそれらの情報を予測し、受信した情報を観測結果として予測に反映させる手法が提示されている。これにより、通信途絶などのような当初の計画と相違するような状況が発生しても、予測を修正することにより制御を継続させることが可能である、しかしながら、このような予測制御の計算は複雑であり、一定の制御周期の間に常に正しい計算結果が得られているかを確認するためにはあらゆる条件で検証テストを実施する必要がありコストが高い。また、当初の計画と相違する状況の発生に伴う予測の修正が各ロボットまたは全体システムに対してどのような影響を与えるかは自明ではない。特に、複数のロボットを同時に考慮する必要がある場合においては、検証テストすべき状態の組み合わせは膨大となりそのすべてを検証することは不可能である。

【0008】

上記を踏まえ、本発明は、フィールドを共有する各移動体にそれぞれ与えられたミッション（例えば目的地移動やサーベイランス）を安全性保証と低コストを両立して実現することを目的とし、通信パケットの予期せぬ途絶が発生する場合や、フィールドの障害物位置や移動体数が当初の想定と異なる（変化した）場合においても、安全性保証と低コストの両立を保つことが課題である。

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

【0009】

上記課題を解決するために、本発明の一実施例に係る移動体制御システムは、目的地までの移動を制御する目的地移動用制御入力決定部と目的地移動用制御入力決定部に入力された値を修正する制御入力修正部とデッドロックを検知するデッドロック検知部と自己を駆動させる駆動部と、を有する移動体を複数備えた移動体制御システムであって、移動体の各々は回避移動用有限状態遷移モデル動的生成部と出力部とを有し、移動体の制御入力修正部はデッドロック検知部がデッドロックを検知した場合に回避移動が必要と判断して回避移動用有限状態遷移モデル動的生成部に指示し、回避移動用有限状態遷移モデル動的生成部は制御入力修正部から回避移動の指示を受けた場合に自己位置情報と障害物位置情報と他者ロボット位置情報と他者ロボットロック位置情報とに基づき回避移動用有限状態遷移モデルを動的に生成して回避軌道を生成し、出力部は自己の現在位置を示す自己ロボット位置情報と生成した回避軌道上にあって移動体が未通過の位置を自己ロボットロック位置情報として他の移動体に出力する。

【発明の効果】

【0010】

本発明によれば、通信途絶を有限状態遷移モデル上の非決定的遷移として取り扱うことにより、フィールドを共有する各移動体をそれぞれの目的地まで移動させる制御を安全性保証と低コストを両立して実現することが可能になる。
本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。また、上記した以外の課題、構成及び効果は、以下の実施例の説明により明らかにされる。

【図面の簡単な説明】

【0011】

【図1】第一の実施例における制御システム０の全体構成を示した図。

【図2】本実施例における座標系を示した図。

【図3】本実施例における抽象化制御システム６を示した図。

【図4】本実施例における移動体制御システムの機能構成を示すブロック図。

【図5】本実施例における移動ロボットの構造の一例を示した図。

【図6】本実施例における第一のハイレベル制御７から見た第一の有限状態遷移モデル１３の一部を示した図。

【図7A】第一の有限状態遷移モデル１３を生成するためのソースコード例を示した図。

【図7B】第一の有限状態遷移モデル１３を生成するためのソースコード例を示した図。

【図7C】第一の有限状態遷移モデル１３を生成するためのソースコード例を示した図。

【図8】本実施例における第一のハイレベル制御７を生成するための第一の有限状態遷移モデル１３から生成される第一の有向グラフ１６の一部を示した図。

【図9】本実施例における第一のハイレベル制御７が決定する最短路と入力列を示した図。

【図10】本実施例における各移動ロボット間で通信される通信パケット１９を示した図。

【図11】各移動ロボットが各タイムステップ時間幅内に行う処理を示した図。

【図12】本実施例における第一の移動ロボット３に関して、制御入力の安全性確認における伝達および非伝達の判定を示した擬似コード。

【図13】制御入力の安全性確認によるデッドロックが発生する直前の状態を示した図。

【図14】制御入力の安全性確認によるデッドロックが発生する時点の状態を示した図。

【図15】回避軌道を生成する手順を示した図。

【図16】ロック位置パケット２０を示した図。

【図17】図１４において第三の移動ロボット５が回避軌道を生成した後で第三の移動ロボット５が回避している動作およびその間のその他の移動ロボットの動作を示した図。

【図18】図１７の状態において第三の移動ロボット５が送信するロック位置パケット２０を示した図。

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

【0012】

本実施例は、複数の移動体を制御する制御システムに関するものである。
以下、本発明に好適な実施形態の例（実施例）を説明する。本実施例においては、３つの移動ロボットを目的地まで安全に移動させる例で説明するが、移動ロボットの数は３以外でもよく、移動ロボットは車両としてもよく、移動ロボットが移動する領域（フィールド）の大きさおよび障害物の位置および各移動ロボットの初期位置および目的地はあくまで一例であってこれに限らない。

【0013】

＜制御システム０の全体構成＞
図１は、本実施例における制御システム０の全体構成を示した図である。
図１に示すように、制御システム０は、フィールド１と障害物２と第一の移動ロボット３と第二の移動ロボット４と第三の移動ロボット５とを有している。本実施例においてフィールド１の大きさは２メートル四方であり、障害物２の位置は図示の通りであってその幅は１０センチメートルである。

【0014】

＜座標系＞
図２は、本実施例における座標系を示した図である。
２メートル四方のフィールド１の中心を原点とし、図示の通りＸ軸とＹ軸を設定する。また、各移動ロボットの方向を示す角度をＸ軸から反時計回りを正として－πラジアン超から＋πラジアン以下までの値で表記する。また、第一の移動ロボット３の状態を（Ｘ＿１、Ｙ＿１、θ＿１）、第二の移動ロボット４の状態を（Ｘ＿２、Ｙ＿２、θ＿２）、第三の移動ロボット５の状態を（Ｘ＿３、Ｙ＿３、θ＿３）とそれぞれ表記する。これにより、制御システム０の状態は（（Ｘ＿１、Ｙ＿１、θ＿１）、（Ｘ＿２、Ｙ＿２、θ＿２）、（Ｘ＿３、Ｙ＿３、θ＿３））で表される。よって、制御システム０の状態空間は－１から１までの実数６つ（Ｘ＿１、Ｘ＿２、Ｘ＿３、Ｙ＿１、Ｙ＿２、Ｙ＿３）と－πからπまでの実数３つ（θ＿１、θ＿２、θ＿３）の直積集合となる。

【0015】

＜抽象化制御システム６＞
図３は、本実施例における抽象化制御システム６を示した図である。
抽象化制御システム６は制御システム０の状態空間を削減したモデルである。抽象化制御システム６を用いることにより、制御をローレベルとハイレベルに分割して考えることができる。すなわち、ハイレベル制御において移動ロボットの移動計画を立案し、ローレベル制御に指示を行い、ローレベル制御はハイレベル制御の指示を受けてアクチュエータを駆動する構成とする。

【0016】

本実施例においては、Ｘ軸、Ｙ軸方向に幅０．２メートルのグリッドを構成し、各移動ロボットの一回の並進量をプラスマイナス０．２メートルずつ行うこととする。また、各移動ロボットの方向を０、π／２ラジアン、πラジアン、－π／２ラジアンのいずれかとし、各移動ロボットの回転をプラスマイナスπ／２ラジアンとする。各タイムステップ（制御周期が開始となる時刻）において、ハイレベル制御は、ローレベル制御に対し、プラスマイナス０．２メートルの並進かプラスマイナスπ／２ラジアンの回転か何もせず前回の状態を維持する（以後、これを停止と呼称する）かのいずれか１つの指示を送信する。ローレベル制御は、前記指示に従いアクチュエータを駆動する。なお、ハイレベル制御とローレベル制御はいずれも各移動ロボットにそれぞれ搭載される。よって、第一の移動ロボット３には第一のハイレベル制御７と第一のローレベル制御８が搭載され、第二の移動ロボット４には第二のハイレベル制御９と第二のローレベル制御１０が搭載され、第三の移動ロボット５には第三のハイレベル制御１１と第三のローレベル制御１２が搭載される。

【0017】

＜システムの機能構成＞
図４は、本実施例における移動体制御システム全体の機能構成を示すブロック図である。
図４に示すように、移動体制御システムは複数の移動体１００を含む。移動体１００には各々他者ロボット位置情報２０１及び他者ロボットロック位置情報２０２が入力され、自己ロボット位置情報２０６及び自己ロボットロック位置情報２０７を出力する。なお本発明においてロック位置とは、あるロボットが排他的に進入可能となるフィールド上の位置である。各ロボットは、後述するロック位置パケット２０を用いることにより、フィールドの一部をロック位置に指定することができる。

【0018】

移動体１００は、自己位置取得部１０１、目的地移動用制御入力決定部１０２、制御入力修正部１０３、デッドロック検知部１０４、回避移動用有限状態遷移システム動的生成部１０５、駆動部１０６、自己ロック位置出力部１０７、及び自己位置出力部１０８を有する。

【0019】

なお、移動体１００の各々には不図示のＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）及びメモリが搭載されており、メモリに格納された各種のプログラムをＣＰＵが実行することによって上記の各機能部による機能が発揮される。

【0020】

自己位置取得部１０１は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）により自己の所在する座標位置を取得する。目的地移動用制御入力決定部１０２は、取得した自己位置と目的地との位置関係及び障害物位置情報２０３から、現在位置から目的地まで移動するための移動経路を算出し、目的地移動制御入力２０４として必要な制御を決定する。制御入力修正部１０３は、目的地移動制御入力２０４、他者ロボット位置情報２０１及び他者ロボットロック位置情報２０２から、回避すべき経路を算出し、最終制御入力２０５として出力する。

【0021】

デッドロック検知部１０４は、制御入力修正部１０３からの出力を基にデッドロックを検知する。回避移動用有限状態遷移システム動的生成部１０５は、デッドロック検知部１０４の検知結果、自己位置、他者ロボット位置情報２０１、他者ロボットロック位置情報２０２、及び障害物位置情報２０３から、自己ロボットが目的地まで障害物及び他者ロボットを回避して移動できる回避経路を算出し、出力する。駆動部１０６は、制御入力修正部１０３及び回避移動用有限状態遷移システム動的生成部１０５の出力和である最終制御入力２０５に基づいて自己ロボットを駆動させる。自己ロック位置出力部は回避移動用有限状態遷移システム動的生成部１０５の出力結果に基づいて自己のロック位置を算出し、自己ロボットロック位置情報２０７として出力する。自己位置出力部１０８は自己位置を自己ロボット位置情報２０６として出力する。

【0022】

＜ローレベル制御＞
各移動ロボットに搭載されるローレベル制御について説明する。
図５は、本実施例におけるロボット（本図においては第一の移動ロボット３を示す）の構造を示した図である。
各移動ロボットの構造はいずれも共通である。移動ロボットは一般的な二輪モデルで表現され、各車輪の回転量をサーボモータによって独立して制御可能である。すなわち、左側車輪３０１に対して左側サーボモータ３０３が接続され、右側車輪３０２に対して右側サーボモータ３０４が接続される。左側サーボモータ３０３および右側サーボモータ３０４は内部にロータリエンコーダを有し、サーボモータの外部から目標位置を与えることにより一定の角度分モータを回転させることができる。また、ローレベル制御および後述するハイレベル制御はソフトウェアにより計算処理され、当該ソフトウェアは移動ロボットに搭載の制御マイコン３０５に実装される。

【0023】

第一のハイレベル制御７より＋０．２メートルの並進を指示された場合、第一のローレベル制御８は、左側サーボモータ３０３については目標位置を（現在左側サーボモータ位置）＋０．２／（車輪半径：メートル）、右側サーボモータ３０４については目標位置を（現在右側サーボモータ位置）＋０．２／（車輪半径：メートル）に設定する。これにより左側サーボモータ３０３と右側サーボモータ３０４が当該目標位置まで同一方向に回転し、第一の移動ロボット３は０．２メートル前進する。第一のハイレベル制御７より＋π／２ラジアンの回転を指示された場合、第一のローレベル制御８は、左側サーボモータ３０３については目標位置を（現在左側サーボモータ位置）－（π／２）＊（トレッド長：メートル）／（車輪直径：メートル）、右側サーボモータについては目標位置を（現在右側サーボモータ位置）＋（π／２）＊（トレッド長：メートル）／（車輪直径：メートル）に設定する。これにより左側サーボモータ３０３と右側サーボモータ３０４が当該目標位置まで逆方向に回転し、第一の移動ロボット３は反時計回りにπ／２ラジアン回転する。マイナス方向の並進、回転も同様に制御できる。第一のハイレベル制御７から前回の状態を維持する指示を受けた場合、第一のローレベル制御８は各サーボモータの目標位置を変更しない。これにより第一の移動ロボット３は並進および回転を実施せず前回と同じ状態を保持する。第二の移動ロボット４と第三の移動ロボット５における第二のローレベル制御１０と第三のローレベル制御１２に関しても、第二のハイレベル制御９または第三のハイレベル制御１１からの指示に従い、第一のローレベル制御８と同様に動作する。

【0024】

＜タイムステップ＞
本実施例においては、第一の移動ロボット３と第二の移動ロボット４と第三の移動ロボット５は互いに時刻同期される。本実施例においては各移動ロボットは互いに通信可能であるので、公知の時刻同期手法例えばＮＴＰ（Ｎｅｔｗｏｒｋ Ｔｉｍｅ Ｐｒｏｔｏｃｏｌ）を用いることで時刻同期することができる。また、移動体制御システム外部から時刻同期信号を与えてもよい。上記の通りローレベル制御では各サーボモータが指示位置まで回転するが、サーボ位置決め制御はＰＩＤ制御などの公知の制御手法で行うものであり、サーボモータに対して目標位置を指定してから該サーボモータが実際に該目標位置に到達するまでの間にはいくらかの時間を要する。そこで、タイムステップを、サーボモータに対して目標位置を指定してからサーボモータが該目標位置に到達するまでに要する時間の最大値以上の長さの時間として定める。

【0025】

サーボモータに対して目標位置を指示してから該目標位置に到達するまでの時間の長さは、一般に現在サーボモータ位置と目標位置までの差の長さに依存するので、タイムステップは、ローレベル制御で選択する制御入力５パターン（＋０．２メートル並進、－０．２メートル並進、＋π／２ラジアン回転、－π／ラジアン回転、停止）のなかで最も位置決めに時間を要する制御入力に対しての目標位置を指示してから該目標位置に到達するまでの時間以上の値として定めればよい。タイムステップは、各移動ロボットが並進・回転・停止のいずれかの動作を行う時間間隔すなわち制御周期となる。本実施例においては、第一の移動ロボット３と第二の移動ロボット４と第三の移動ロボット５はすべて同一のハードウェア構成とし、タイムステップはすべての移動ロボットに対して同一とする。

【0026】

＜受信処理＞
第一の移動ロボット３（または第二の移動ロボット４または第三の移動ロボット５）はタイムステップの冒頭に、他の移動ロボットが送信した通信パケット１９とロック位置パケット２０を受信する。通信パケット１９とロック位置パケット２０については後述する。

【0027】

＜有限状態遷移モデル＞
ローレベル制御を実装した各移動ロボットは、ハイレベル制御から見ると有限状態遷移モデルとみなすことができる。
図６は、本実施例における第一のハイレベル制御７から見た第一の有限状態遷移モデル１３の一部を示した図である。

【0028】

第一の有限状態遷移モデル１３は、第一のハイレベル制御７から見て、第一のローレベル制御８が搭載された第一の移動ロボット３のダイナミクスを示したモデルである。例えば、図６において、第一の移動ロボット３の状態が（０、０、０）である場合を考える。ここで第一のハイレベル制御７がＵ１（＋０．２メートル並進）を指示した場合、第一の移動ロボット３はＸ軸方向に＋０．２メートル進むことから、次のタイムステップにおいて状態は（０．２、０、０）に遷移する。第一のハイレベル制御７がＵ２（－０．２メートル並進）を指示した場合、第一の移動ロボット３はＸ軸方向に－０．２メートル進むことから、次のタイムステップにおいて状態は（－０．２、０、０）に遷移するが、座標（－０．２、０）には障害物が存在するため、この遷移は生成しない。第一のハイレベル制御７がＵ３（＋π／２ラジアン回転）を指示した場合、第一の移動ロボット３は＋π／２ラジアン回転するから、次のタイムステップにおいて状態は（０、０、π／２）となり、Ｕ４（－π／２ラジアン回転）を指示した場合は次のタイムステップにおいて状態は（０、０、－π／２）となる。第一のハイレベル制御７がＵ５（移動なし）を指示した場合は次のタイムステップにおいても状態は（０、０、０）となる。この操作を（０、０、π／２）、（０．２、０、０）などの各状態に対して繰り返し適用することにより、第一の移動ロボット３に関する第一の有限状態遷移モデル１３を得る。同様にして第二の移動ロボット４と第三の移動ロボット５それぞれに関する第二の有限状態遷移モデル１４および第三の有限状態遷移モデル１５を得る。

【0029】

図７Ａ～Ｃは、上述した第一の有限状態遷移モデル１３を生成するためのソースコード例を示した図である。なお本実施例においてはＣ＋＋に従って作成されたソースコードを説明するが、ソースコードの種類はこれに限られない。

【0030】

＜有限状態遷移モデルの生成＞
第一の有限状態遷移モデル１３を生成するソースコードを説明する。なお、第二の有限状態遷移モデル１４および第三の有限状態遷移モデル１５を生成する場合も全く同様である。

【0031】

まず、図７Ａに示すように、構造体と配列を定義する。構造体ｐｌａｎｔｓｔａｔｅはロボットの座標ｘ、ｙ、θと、各入力を印加した場合にどの状態（すなわち別のｐｌａｎｔｓｔａｔｅ構造体）に遷移するかを格納したポインタ配列を有する。構造体ｉｎｐｕｔは入力する値ｖａｌｕｅとその入力が回転を示すものか並進を示すものかを区別するためのブール変数と、ダイクストラ法（詳しくは後述する。ハイレベル制御のために使用する）のための重み値を有する（後述するが、実際にはすべて１である）。また、ｐｏｓｉｔｉｏｎ構造体にはｘ座標とｙ座標を格納するための変数がそれぞれ用意される。なお、本実施例における有限状態遷移モデルにおいては、Ｘ座標およびＹ座標は０．２刻みの１０点でありθはπ／２刻みの４点であるが、これを０～１０または０～４の整数値にそれぞれ対応付けして管理することとする。例えば、座標（－０．６、０．４、π／２）は（２、７、１）に対応付ける。この対応付けの操作をエンコードと呼び、逆に（２、７、１）を座標（－０．６、０．４、π／２）に対応付ける操作をデコードと呼ぶこととする。これらの定義した構造体およびエンコード・デコードの操作を用いて、配列データを定義する。配列ｐｌａｎｔ［１１］［１１］［４］は第一の有限状態遷移モデル１３をｐｌａｎｔｓｔａｔｅ構造体の配列として表現するためのものである。図６－１の段階では配列を宣言しただけであり、第一の有限状態遷移モデル１３はまだ表現されていない。次に配列ｉｎｐｕｔ［］は入力としてグリッドサイズ分すなわち０．２メートルの前後方向の並進およびθ軸のプラスマイナスπ／２ラジアン方向の回転が格納される。最後に配列ｏｂｓｔａｃｌｅ［］は障害物位置をエンコードしてｐｏｓｉｔｉｏｎ構造体として記述したものを保持する。なお、図７Ａにおけるｏｂｓｔａｃｌｅ配列の中身は図３における障害物位置と対応している。

【0032】

次に、図７Ｂに示すように、第一の有限状態遷移モデル１３において、ある状態である入力を印加した場合にどの状態に遷移するかを計算するための関数ｔｒａｎｓｉｔｉｏｎ（）を定義する。遷移先の計算結果は整数変数ｉ、ｊ、ｋにそれぞれ格納され、最終的には＆ｐｌａｎｔ［ｉ］［ｊ］［ｋ］としてアドレスが返される。まず、入力が回転指示だった場合、ｉ、ｊは変わらず、ｋのみが変化する。計算はデコードした生のラジアン角度を用いて行い、最後に整数値にエンコードする。なお、図７Ｂでは記載を省略したが、実際には、正しく角度を管理するため、デコードした値が－π／２以下の場合は２πを加え、デコードした値がπ／２より大きい場合は２πを引く操作を行う。一方、入力が並進指示だった場合、ｋは変わらずｉとｊが変化する。変数ｔｒａｎｓ＿ｓｔｅｐに、並進する距離をグリッド数に置き換えた値を格納する。今回の場合、プラスマイナス０．２メートルの並進であってグリッドサイズは０．２メートルであるから、＋１もしくは－１のいずれかが格納される。そしてｉまたはｊに対してｔｒａｎｓ＿ｓｔｅｐの値が加算もしくは減算されるが、ｉとｊどちらが変化し加算と減算どちらとなるのかは、ロボットがどの方向を向いているかによって異なる。したがって、４つ（Ｘ軸・Ｙ軸の正負方向）に場合分けして計算する。

【0033】

図７ＢではロボットがＸ軸の正方向に向いている場合を特に記載しており、本文においてもこれについて説明する。まず、境界チェックを行う。もし現在のＸ座標が９でありｔｒａｎｓ＿ｓｔｅｐが＋１であった場合、Ｘ座標は１０になるはずであるが、これは境界（障害物）に衝突するので不適である。同様に現在のＸ座標が１でありｔｒａｎｓ＿ｓｔｅｐが―１であった場合、Ｘ座標は０になるはずであるが、これも境界（障害物）に衝突するので不適である。そのような不適な遷移に対してはｔｒａｎｓｉｔｉｏｎ（）はＮＵＬＬをリターンする。境界に当たらないことを確認した後、遷移を実行する。遷移中に障害物位置を通過しようとしないかを確認するため、ｉをインクリメントまたはデクリメントするたびに障害物判定のための関数ｏｂｓｔａｃｌｅ＿ｃｈｅｃｋ（）を実行する。当該関数の定義は図６－２には記載していないが、引数となっている座標（ｉ、ｊ、ｋ）特に（ｉ、ｊ）の組が配列ｏｂｓｔａｃｌｅ［］に含まれていなければｔｒｕｅを返し、そうでなければｆａｌｓｅを返すものである。障害物位置を通過しないことを確認の後、インクリメントまたはデクリメントされたｉを用いて＆ｐｌａｎｔ［ｉ］［ｊ］［ｋ］をリターンする。なお、もし障害物位置を通過すると判定された場合は不適なのでＮＵＬＬをリターンする。

【0034】

最後に、図７Ｃにおいて第一の有限状態遷移モデル１３を生成する。これは、ｐｌａｎｔ［１１］［１１］［４］の各要素（状態）に対して関数ｔｒａｎｓｉｔｉｏｎ（）を用いて次の遷移先を計算しポインタ配列に追加することによって実現される。すなわち、３重ループを用いる。要素ｐｌａｎｔ［ｉ］［ｊ］［ｋ］のｘ、ｙ、θにはそれぞれ対応するエンコードした値であるｉ、ｊ、ｋがそのまま格納される。状態（ｉ、ｊ、ｋ）が境界上もしくは境界外あるいは障害物上ではないことを確認したのち、各入力に対して状態遷移を計算する。入力はＮＵＭＢＥＲ＿ＯＦ＿ＩＮＰＵＴＳ個（今回は５）あるので、５回Ｆｏｒループを実行する。なお、ｓｔｄ：：ｖｅｃｔｏｒ＜ｓｔｒｕｃｔ ｐｌａｎｔｓｔａｔｅ＊＞ｐｌａｎｔ［ｉ］［ｊ］［ｋ］．ｎｅｘｔ［ｕ］は、入力ｉｎｐｕｔ［ｕ］が印加された時の遷移先を表している。これは、図７Ｂで示した関数ｔｒａｎｓｉｔｉｏｎ（）により計算され、ポインタｎｅｘｔに格納される。ポインタｎｅｘｔがＮＵＬＬでなければ、遷移可能であることを示しているので、そのポインタを遷移先としてｓｔｄ：：ｖｅｃｔｏｒ＜ｓｔｒｕｃｔ ｐｌａｎｔｓｔａｔｅ＊＞ｐｌａｎｔ［ｉ］［ｊ］［ｋ］．ｎｅｘｔ［ｕ］に登録する。なお、後述するが本実施例においては、通信途絶時（すなわち他ロボットからの通信パケット１９を受信しなかったとき）にはロボット同士の衝突を回避するためローレベル制御はハイレベル制御をキャンセルしてその場に停止して状態を保持する。よって、ｓｔｄ：：ｖｅｃｔｏｒ＜ｓｔｒｕｃｔ ｐｌａｎｔｓｔａｔｅ＊＞ｐｌａｎｔ［ｉ］［ｊ］［ｋ］．ｎｅｘｔ［ｕ］にはｎｅｘｔだけでなく＆ｐｌａｎｔ［ｉ］［ｊ］［ｋ］自身も登録しておく。以上の操作により、第一の有限状態遷移モデル１３は、遷移先をポインタとして保持した構造体配列として生成される。

【0035】

＜制御マイコン３０５＞
ロボットに搭載された制御マイコン３０５はＣＰＵとメモリを含む装置であって、ローレベル制御およびハイレベル制御の計算処理を行う。本実施例においては、有限状態遷移モデルの実装である配列ｐｌａｎｔ［１１］［１１］［４］は、各要素において、自身の状態を表すための３つの変数ｘ、ｙ、θを含み、遷移を表すためのポインタを最大でＮＵＭＢＥＲ＿ＯＦ＿ＩＮＰＵＴＳ＊２個含みうる。必要なメモリ量を確保できないと有限状態遷移モデルが正しく実装されず、制御が不定となり安全性が保証されないため、必要メモリ量の見積もりが重要である。３つの変数ｘ、ｙ、θをそれぞれ１バイトの変数で表現し、１つのポインタを４バイトで表現すると、１つの有限状態遷移モデルを表現するための必要メモリ量は（１＊３＋４＊５＊２）＊１１＊１１＊４＝２０８１２バイトすなわち約２０キロバイトと計算される。後述する回避移動用有限状態遷移モデルのために同じく約２０キロバイトを消費すると見積もると、制御マイコン３０５に１００キロバイト程度のメモリ量があれば十分である。

【0036】

ただし、本実施例においては、２０センチメートルより小さな精度で目的地を指定することはできない。なぜなら、２０センチ四方のグリッド内の各位置は抽象化制御システム６においてはすべて同一とみなされるからである。一方、図３における抽象化制御システム６のグリッドサイズを１０分の１にして２センチメートル刻みにした場合、目的地指定の精度は２センチメートルに改善するものの、有限状態遷移モデルの実装配列はｐｌａｎｔ［１０１］［１０１］［４］となり、１つの有限状態遷移モデルを表現するための必要メモリ量は（１＊３＋４＊５＊２）＊１０１＊１０１＊４＝１７５４５７２バイトすなわち約１．７メガバイトと計算される。後述する回避移動用有限状態遷移モデルのために同じく約１．７メガバイトを消費すると見積もると、制御マイコン３０５に４メガバイト程度のメモリ量が必要となる。

【0037】

一般に、メモリ容量の大きなマイコンはメモリ容量の小さなマイコンよりも高価である。したがって、有限状態遷移モデルとしてモデル化することにより、要求される制御性能（目的地指定の精度）に対して必要なメモリ量を推定し、当該推定に必要十分なマイコンを制御マイコン３０５として選択することが可能となる。逆に、与えられた制御マイコン３０５のメモリ容量に収まるうちで最も小さなグリッドサイズを求めることにより、与えられたコスト制約の下で最大の制御性能を発揮させることができる。

【0038】

図８は、本実施例における第一のハイレベル制御７を生成するための第一の有限状態遷移モデル１３から生成される第一の有向グラフ１６の一部を示した図である。
図９は、本実施例における第一のハイレベル制御７が決定する最短路と入力列を示した図である。

【0039】

＜ハイレベル制御＞
以下では、ハイレベル制御について説明する。第一のハイレベル制御７は、第一の移動ロボット３を目的地まで移動させるための経路を計画し、第一のローレベル制御８に対して並進・回転・停止のいずれかの指示を行う役割を持つ。これは、第一の有限状態遷移モデル１３上で最短経路探索を適用することにより得ることができる。すなわち、第一の有限状態遷移モデル１３に対し、制御入力Ｕ１～Ｕ５を重み１と置き換えることにより得られる第一の有向グラフ１６を構成し、第一の有向グラフ１６上でダイクストラ法を適用し最短路を得る。第一のハイレベル制御７は、この最短路に第一の有限状態遷移モデル１３を対応付け重み１に置き換える前の制御入力を対応付けすることにより、第一のローレベル制御８に対して並進・回転・停止のいずれかの指示を行う。

【0040】

図９は、初期位置が（０、０、０）で目的地が（０、０．２、π／２）である場合に、ダイクストラ法を適用した後に第一のハイレベル制御７が得る最短路および入力列である。まず初期状態（０、０、０）において第一のハイレベル制御７は制御入力Ｕ３（＋π／２ラジアン回転）を選択する。この制御入力が第一のローレベル制御８により実行されサーボモータを駆動するので、次のタイムステップにおいて、第一の移動ロボット３の状態は（０、０、π／２）に遷移する。第一のハイレベル制御７は制御入力Ｕ１（＋０．２メートル並進）を選択し、次のタイムステップにおいて第一の移動ロボット３の状態は（０、０．２、π／２）に遷移し、目的地に到着する。第二の有向グラフ１７と第三の有向グラフ１８も同様に生成し、第二のハイレベル制御９と第三のハイレベル制御１１を生成する。

【0041】

図１０は、本実施例における各移動ロボット間で通信される通信パケット１９を示した図である。

【0042】

＜通信パケット＞
本実施例においては、第一の移動ロボット３と第二の移動ロボット４と第三の移動ロボット５は互いに通信可能である。各タイムステップの冒頭において受信データを確認し、ハイレベル制御およびローレベル制御が動作し、最後にデータを送信する。各移動ロボットは各タイムステップにおいて通信パケット１９をそれぞれ１つ送信する。また、各通信パケット１９は全ての他の移動ロボットに対してブロードキャストで送信される。例えば、第一の移動ロボット３の送信する通信パケット１９は、該タイムステップにおける第一の移動ロボット３の状態と、第一のハイレベル制御７が選択する制御入力の結果により遷移することとなる次のタイムステップにおける第一の移動ロボット３の位置と、第一の移動ロボット３の目標状態とを含む。なお、本実施例においては、各移動体ロボット間では通信パケット１９の他にロック位置パケット２０（後述）も通信する。

【0043】

＜各タイムステップ時間幅に行う処理＞
図１１は、各移動ロボットが各タイムステップ時間幅内に行う処理を示した図である。
前述の通り、各移動ロボットは時刻同期されており、各移動ロボットに設定するタイムステップは同一である。各移動ロボットが各タイムステップ時間幅において行う処理を図１０に示す。まず、タイムステップの開始時刻に置いて、他の移動ロボットから受信した通信パケット１９を取得する。次に、第一の移動ロボット３においては第一のハイレベル制御７（第二の移動ロボット４と第三の移動ロボット５はそれぞれ第二のハイレベル制御９と第三のハイレベル制御１１）が、目標状態に到達するための経路を基に制御入力を１つ決定する。次に、該制御入力を第一のローレベル制御８（第二の移動ロボット４と第三の移動ロボット５はそれぞれ第二のローレベル制御１０と第三のローレベル制御１２）に伝達するか否かを確認する「制御入力の安全性確認」を行う。制御入力の安全性確認については後述する。その後、第一のローレベル制御８（第二の移動ロボット４と第三の移動ロボット５はそれぞれ第二のローレベル制御１０と第三のローレベル制御１２）は左側サーボモータ３０３および右側サーボモータ３０４を制御し、第一の移動ロボット３（または第二の移動ロボットまたはと第三の移動ロボット５）が並進または回転または停止する。ローレベル制御が終了すると、ハイレベル制御は現在の移動ロボットの状態を取得し、取得した結果に応じて状態遷移を行う。この状態遷移については後述する。タイムステップの最後に、第一の移動ロボット３（または第二の移動ロボットまたはと第三の移動ロボット５）は通信パケット１９を他の移動ロボットに対して送信する。

【0044】

＜制御入力の安全性確認＞
図１２は、本実施例における第一の移動ロボット３に関して、制御入力の安全性確認における伝達および非伝達の判定を示した擬似コードである。
制御入力の安全性確認は、各移動ロボットにおいて、ハイレベル制御による制御入力をローレベル制御に伝達するか否かを決定する役割を持つ。伝達すると決定した場合はハイレベル制御による制御入力をローレベル制御にそのまま渡し、伝達しないと決定した場合は、ハイレベル制御による制御入力を無効化して制御入力を停止で置換しローレベル制御に伝達する。図１２は制御入力の安全性確認の具体的な判定手法を示した擬似コードである。他の移動体からのパケットが一部でも不足していた場合、安全性なしと判断して即座にローレベル制御器に伝達しない（停止で置換）と決定する。パケット途絶が発生していない場合は、以下の処理に進む。まず、第一のハイレベル制御７が決定した制御入力を仮にそのまま入力した場合の第一の移動ロボット３が次のタイムステップにおける状態を計算し、ｎｅｘｔ＿ｔｍｐ＿ｓｔａｔｅに格納する。

【0045】

次に、第一の移動ロボット３に関する安全状態を計算する。安全状態とは、全状態空間に対する部分集合であって、安全が確保された状態の集合である。本実施例の場合、安全が確保された状態とは、他の移動ロボットすなわち第二の移動ロボット４および第三の移動ロボット５が存在しえない位置をいう。一方、非安全状態とは、他の移動ロボットすなわち第二の移動ロボット４もしくは第三の移動ロボット５の少なくとも１つが存在する可能性がある位置をいう。安全状態と非安全状態は補完関係にあるので、今回は非安全状態を考える。すなわち、配列ｕｎｓａｆｅ＿ｌｉｓｔを作成し、受信した通信パケット１９とロック位置パケット２０（後述）に含まれる状態を登録する。

【0046】

本実施例においては、第二の移動ロボット４と第三の移動ロボット５それぞれの現在状態および次状態およびロック位置を登録する。最後に、ｕｎｓａｆｅ＿ｌｉｓｔに登録した各状態をチェックし、もしｎｅｘｔ＿ｔｍｐ＿ｓｔａｔｅのＸ座標およびＹ座標が一致する状態が含まれていた場合は、第一のハイレベル制御７が決定した制御入力は安全でないすなわちＦＡＬＳＥを返し、第一のハイレベル制御７が決定した制御入力ではなく停止を第一のローレベル制御８に対して指示する。もしｕｎｓａｆｅ＿ｌｉｓｔに登録したすべての状態に対して、ｎｅｘｔ＿ｔｍｐ＿ｓｔａｔｅのＸ座標またはＹ座標の少なくともいずれかが異なる場合は、第一のハイレベル制御７が決定した制御入力は安全と判定し、第一のハイレベル制御７が決定した制御入力をそのまま第一のローレベル制御８に伝達する。第二の移動ロボット４と第三の移動ロボット５に関してもそれぞれ同様に安全性確認を行う。

【0047】

図１３は、制御入力の安全性確認によるデッドロックが発生する直前の状態を示した図である。
図１４は、制御入力の安全性確認によるデッドロックが発生する時点の状態を示した図である。

【0048】

＜停止連続時の回避軌道生成＞
各ロボットの位置および目標状態によっては、制御入力の安全性確認において複数のロボットが互いに衝突防止のための停止を実施し、結果としてデッドロックが発生する可能性がある。図１３においては、第二の移動ロボット４は第三の移動ロボット５の先にある目標状態に移動しようとし、第三の移動ロボット５は第二の移動ロボット４の先にある目標状態に移動しようとする。その結果、図１４のように第二の移動ロボット４と第三の移動ロボット５の互いの自己位置が次に進みたい位置と重複してデッドロックとなる。なお、第一の移動ロボット３は通常通り移動が継続する。

【0049】

デッドロックを解消するためには、第二の移動ロボット４または第三の移動ロボット５の少なくともいずれかが一時的に回避軌道を生成する必要がある。したがって、制御入力の安全確認における停止（不安全）が連続してある閾値以上発生した場合は回避軌道生成を実施する。この閾値は例えば３回としてもよいし、その他の正の整数であってもよい。また、パケットロスにより停止した場合は当該連続停止回数のカウントに含まないようにしてもよう。これにより、パケットロスのタイミングが重複しただけで移動体間のデッドロックは発生していない場合に不要な回避軌道の生成を行うことを防止することができる。また、閾値を超えたからといって必ずしも回避軌道を生成せずとも、閾値を超えた際に回避軌道を生成するか否かを確率的に決定する仕組みを入れてもよい。これにより、どちらかのロボットだけが回避軌道を生成し他方のロボットは本来の目的地から逸脱せずに済むようになる。

【0050】

図１５は、回避軌道を生成する手順を示した図である。
まずステップＳ１において、配列ｏｂｓｔａｃｌｅ２［］に対し、ｏｂｓｔａｃｌｅ［］に含まれるすべての位置と、移動体が受信した他者の自己位置とロック位置の総和を格納する。

【0051】

次にステップＳ２において、回避移動用有限状態遷移モデルを生成する。回避移動用有限状態遷移モデルは、第一の有限状態遷移モデル１３と類似するモデルである。このモデルが第一の有限状態遷移モデル１３と異なる点は、第一の有限状態遷移モデル１３においては障害物位置への遷移を禁止したのに対し、回避移動用有限状態遷移モデルは障害物位置への遷移に加えて、現在他の移動ロボットが存在している位置と、現在他の移動ロボットがロック（占有）している位置への遷移を禁止する点である。これは、第一の有限状態遷移モデル１３を生成するときに使用したｔｒａｎｓｉｔｉｏｎ（）およびｐｌａｎｔ＿ｉｎｉｔ（）関数において、ｏｂｓｔａｃｌｅ［］を参照していたところをステップＳ１において生成したｏｂｓｔａｃｌｅ２［］を参照するように置換することにより生成できる。当該置換は、ｔｒａｎｓｉｔｉｏｎ（）およびｐｌａｎｔ＿ｉｎｉｔ（）をコピーして別名の関数ｔｒａｎｓｉｔｉｏｎ２（）およびｐｌａｎｔ＿ｉｎｉｔ２（）を定義し、ｔｒａｎｓｉｔｉｏｎ２（）およびｐｌａｎｔ＿ｉｎｉｔ２（）においてｏｂｓｔａｃｌｅ［］を参照していた部分をｏｂｓｔａｃｌｅ２（）を参照するように書き換えることにより実装してもよい。

【0052】

ステップＳ３において、回避軌道を生成するための一時的な仮の目的地を設定する。回避軌道を生成すると本来の目的地から遠ざかる可能性が高いため、なるべく現在位置に近い場所を設定するようにしてもよい。また、この目的地の設定には乱数生成など確率的なふるまいを入れてもよい。確率的なふるまいを入れることにより、回避軌道が他の移動ロボットと重複してしまうなどして一度回避軌道の生成に失敗したとしても、次の生成においては異なる回避軌道を生成することで回避に成功することが期待される。

【0053】

ステップＳ４において、ステップＳ２で生成した回避移動用有限状態遷移モデル上においてステップＳ３で決定した目的地まで移動する経路を生成する。これは図８と同様にダイクストラ法による最短経路探索を実施すればよい。

【0054】

最後に、ステップＳ５において、ステップＳ４で生成した経路によって実際に移動体の現在位置からステップＳ３で決定した目的地まで到達できるか否かを回避移動用有限状態遷移モデル上で確認する。確認の結果到達できた場合、回避軌道の生成は終了する。到達できなかった場合、ステップＳ３で決定した目的地が不適であることが分かるため、ステップＳ３に戻り再度ステップＳ４、Ｓ５を実施する。

【0055】

なお、回避軌道を生成したタイムステップにおいては移動体は回避軌道に進入せず、停止を保持する。これは、デッドロックを回避する必要のある相手側で生成した回避軌道が自身の生成した回避軌道と重複している可能性があるためである。したがって、回避軌道を生成した次のタイムステップにおける制御入力の安全性確認において、他の移動体から受信した通信パケット１９およびロック位置パケット２０（後述）を解析し、自身の生成した回避軌道がこれらと重複していないことを確認した後で、当該回避軌道に進入する。重複が発生していた場合は、再度回避軌道を生成する。このようにして、回避軌道が互いに重複しないようにし、回避軌道を移動中においては移動ロボットは必ず安全であることが保証される。このような回避軌道の生成および再生成は、行き違いのために移動ロボット間で自ら回避軌道を生成し、また生成した回避軌道が採用可能であるか否かを他の移動ロボットの情報を見て判断するという、いわば移動ロボット間の交渉を実現するものである。これにより、フィールドに存在するすべての移動ロボットに対して全体計画を立案することなく、各移動ロボットが移動ロボット自身で衝突およびデッドロック回避を実現することができる。

【0056】

＜ロック位置パケット２０＞
図１６は、ロック位置パケット２０を示した図である。
ロック位置パケット２０は、ロック位置パケット２０を送信する移動ロボットが生成した回避軌道上の位置であって未通過の位置を示したパケットであって、他のすべての移動ロボットに対して送信される。ロック位置パケット２０の説明のため、一例として、図１４において、第三の移動ロボット５が（０．８、―０．２、―π／２）の状態にあって、図１５に示す回避軌道生成手順の結果として回避のための一時的な目的地（ステップＳ３）を（０．６、０、－π／２）と指定し、図１５のステップＳ４において、（０．８、―０．２、―π／２）において制御入力Ｕ４（－π／２ラジアン回転）を入力して（０．８、―０．２、π）に遷移し、（０．８、―０．２、π）において制御入力Ｕ１（＋０．２メートル並進）を入力して（０．６、―０．２、π）に遷移し、（０．６、―０．２、π）において制御入力Ｕ３（＋π／２ラジアン回転）を入力して（０．６、―０．２、－π／２）に遷移し、０．６、―０．２、－π／２）において制御入力Ｕ２（―０．２メートル並進）を入力して（０．６、０、－π／２）に遷移する、という回避軌道を生成し、また、第二の移動ロボット４は回避軌道を生成しなかった場合を考える。

【0057】

図１７は、図１４において第三の移動ロボット５が回避軌道を生成した後で第三の移動ロボット５が回避している動作およびその間のその他の移動ロボットの動作を示した図である。
図１８は、図１７の状態において第三の移動ロボット５が送信するロック位置パケット２０を示した図である。

【0058】

図１７においては、第三の移動ロボット５は回避軌道に従い（０．６、－０．２、π）の状態にあり、次は制御入力Ｕ３を入力して（０．６、－０．２、－π／２）に遷移する。移動ロボットが回避軌道を移動している間は、その移動ロボットが送信する通信パケット１９の情報は回避軌道に従った情報となる。すなわち、図１７の状態における第三の移動ロボット５が送信する通信パケット１９に含まれる情報は、自ロボット状態（０．６、－０．２、π）、自ロボット次状態（０．６、－０．２、－π／２）、自ロボット目標状態（０．６、０、－π／２）となる。

【0059】

また、この状態において、第三の移動ロボット５はすでに位置（０．６、―０．２）に到達しており通信パケット１９にその情報が記載されていることから、第一の移動ロボット３および第二の移動ロボット４は、制御入力の安全性確認により、次の時刻において（０．６、－０．２）に到達するような制御入力はとらない。したがって、（０．６、－０．２）をロック位置として宣言する必要がない。よって、この時刻において第三の移動ロボット５が送信するロック位置パケット２０は、図１８の通り、回避軌道上にあって未通過の位置である（０．６、０）のみを含む。このように、回避軌道上で通過済みの位置をロック位置から順次解除することにより、第三の移動ロボット５が回避軌道の終端である（０．６、０、－π／２）に到達した時にはすべてのロック位置が解除される。

【0060】

＜ハイレベル制御の状態遷移＞
ハイレベル制御の状態遷移とは、図６に示す第一の有限状態遷移モデル１３（または第二の有限状態遷移モデル１４または第三の有限状態遷移モデル１５）における第一の移動ロボット３（または第二の移動ロボット４または第三の移動ロボット５）の現在の状態を更新する処理である。本実施例においては、ある状態である制御入力を印加した場合に、遷移先は事前には１つには定まらず、制御入力の安全性確認によって安全と判断された場合には次の状態に遷移し、そうでない場合には同じ状態にとどまる（停止）となる。

【0061】

例えば図６において第一の移動ロボット３の状態が（０、０、０）であるときのタイムステップを考える。このとき、図８に示すように、第一のハイレベル制御７が制御入力Ｕ３（＋π／２ラジアン回転）を指示したとする。ここで、このタイムステップにおける制御入力の安全性確認が安全と判断していれば、制御入力Ｕ３が第一の移動ロボット３に印加されているので、第一の有限状態遷移モデル１３の状態は（０、０、π／２）に遷移し、そうでない場合は（０、０、０）のままとなる。なお、第一の有限状態遷移モデル１３の状態遷移は、上述した制御入力の安全性確認の安全または不安全の結果により決定する方法以外に、第一の移動ロボット３に対して位置情報を取得するＧＰＳや姿勢情報を取得するＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を用いて取得した現在位置・姿勢の情報により決定することとしてもよい。すなわち、先の例においては第一の有限状態遷移モデル１３は（０、０、π／２）と（０、０、０）のいずれかに遷移する可能性があるが、ＧＰＳにより位置情報（０．００１２３、０．００３１１）、ＩＭＵによりＺ軸角度０．０００１ラジアンのように検出されていれば第一の有限状態遷移モデル１３においては（０、０、０）に遷移すればよいと判定できる。

【0062】

＜送信処理＞
第一の移動ロボット３（または第二の移動ロボット４または第三の移動ロボット５）はタイムステップの最後に、通信パケット１９とロック位置パケット２０を送信する。

【0063】

本実施例によれば、各移動ロボットの状態空間を同じ方法で有限の状態数に分割して有限状態遷移モデルとしてモデル化するとともに制御入力の安全性確認処理を実施して不安全の場合には制御入力を停止信号で上書きさせることから、通信途絶に対しては有限状態遷移モデル上の非決定的遷移として取り扱うことが可能であり通信途絶が発生しても制御を安全に継続でき、また、障害物位置／移動体数変更を行った場合においては、障害物位置変更に関しては図７Ａ～Ｃにおける障害物位置情報を示す配列ｏｂｓｔａｃｌｅ［］を新たな障害物位置に更新することにより有限状態遷移モデルを自動で再生成可能であり、移動体数変更に関しては送受信する通信パケット１９とロック位置パケット２０の数が変わるだけで制御に全く影響を与えないことから、安全が理論的に保証された制御器を工数をかけずに構築することを可能とする効果を有する。

【0064】

以上で説明した本発明の実施例によれば、以下の作用効果を奏する。
（１）本発明の一実施例に係る移動体制御システムは、目的地までの移動を制御する目的地移動用制御入力決定部と目的地移動用制御入力決定部に入力された値を修正する制御入力修正部とデッドロックを検知するデッドロック検知部と自己を駆動させる駆動部と、を有する移動体を複数備えた移動体制御システムであって、移動体の各々は回避移動用有限状態遷移モデル動的生成部と出力部とを有し、移動体の制御入力修正部はデッドロック検知部がデッドロックを検知した場合に回避移動が必要と判断して回避移動用有限状態遷移モデル動的生成部に指示し、回避移動用有限状態遷移モデル動的生成部は制御入力修正部から回避移動の指示を受けた場合に自己位置情報と障害物位置情報と他者ロボット位置情報と他者ロボットロック位置情報とに基づき回避移動用有限状態遷移モデルを動的に生成して回避軌道を生成し、出力部は自己の現在位置を示す自己ロボット位置情報と生成した回避軌道上にあって移動体が未通過の位置を自己ロボットロック位置情報として他の移動体に出力する。

【0065】

上記構成により、通信途絶を有限状態遷移モデル上の非決定的遷移として取り扱うことにより、フィールドを共有する各移動体をそれぞれの目的地まで移動させる制御を安全性保証と低コストを両立して実現することが可能になる。

【0066】

（２）制御入力修正部は、他者ロボットと衝突する可能性を検知した場合において、目的地移動用制御入力決定部が決定した制御入力に関わらず、該制御入力修正部が搭載された移動体を停止させるよう駆動部に指示する。これにより、他者ロボットと衝突する可能性がある場合には経路移動より停止を優先することになるため、他者ロボットとの衝突を確実に回避することが可能になる。

【0067】

（３）制御入力修正部は、他者ロボット位置情報および他者ロボットロック位置情報に含まれる位置に自己移動体が進入しようとした場合に該移動体を停止させ、デッドロック検知部は移動体の停止が一定回数以上連続して発生した場合にデッドロックを検知する。一度のみの停止ではデッドロックであるとは断定できず、このように一定回数以上停止した場合にデッドロックであると検知することが好ましい。

【0068】

（４）制御入力修正部は、他者ロボット位置情報または他者ロボットロック位置情報の少なくとも１つの受信に失敗した場合において、目的地移動用制御入力決定部が決定した制御入力に関わらず、当該制御入力修正部が搭載された移動体を停止させるよう駆動部に指示する。このように情報の受信に失敗した場合は、自己移動体と他者ロボットとの間の位置関係を正確に把握できない可能性があり、衝突を回避するためにこのような停止措置を講ずる。

【0069】

（５）他者ロボット位置情報または他者ロボットロック位置情報の受信を失敗したことに起因する制御入力修正部による停止指示の上書きは、デッドロック検知部によるデッドロック検知判定のための停止の連続回数カウントに含まない。このようにすることでデッドロックの誤検知を防止することが可能になる。

【0070】

（６）制御入力修正部は、受信した他者ロボット位置情報および他者ロボットロック位置情報と、当該制御入力修正部が搭載された移動体に含まれる回避移動用有限状態遷移モデル動的生成部が生成し他者ロボットに送信した自己ロック位置と、を比較し、受信した位置情報と自己ロック位置が重複していなければ回避移動用有限状態遷移モデル動的生成部が生成した回避移動用有限状態遷移モデルに従い制御入力を決定して駆動部に当該制御入力を印加し、受信した位置情報と自己ロック位置が重複していれば回避移動用有限状態遷移モデルを破棄して駆動部に停止命令を印加するとともに回避移動用有限状態遷移モデル動的生成部に対して回避移動用有限状態遷移モデルを再生成するよう指示する。これにより、生成された回避移動用有限状態遷移モデルが他者ロボットとの衝突を回避するものであるか否か常に監視しながら移動体を制御することが可能になる。

【0071】

（７）目的地移動用制御入力決定部および制御入力修正部はそれぞれハイレベル制御器を有し、移動体の各々はローレベル制御器を有し、ハイレベル制御器は移動体の状態空間を有限の状態数に分割した抽象化状態空間を構成し抽象化状態空間における各状態において各制御入力を印加した場合にどの状態に遷移するかに基づき有限状態遷移モデルを生成し当該生成した有限状態遷移モデル上で目的地までの移動経路を計画し当該計画に基づき制御入力を並進または回転のいずれかにするかを決定し、ローレベル制御器はハイレベル制御器の指示に従い各移動体が並進または回転を行うように駆動部に含まれるモータまたはエンジンまたはその他移動体を移動させる駆動力を発生する装置を制御する。本発明の移動体制御は具体的にはこのように実行される。

【0072】

（８）回避移動用有限状態遷移モデル動的生成部が生成する回避移動用有限状態遷移モデルはハイレベル制御器における有限状態遷移モデルの構成において他者の移動体から受信したロック位置への状態遷移を禁止することによって得られる。本発明における回避移動用有限状態遷移モデルは具体的にこのように生成される。

【0073】

（９）移動体の各々に含まれるハイレベル制御器は同一の方法により抽象化状態空間を構成し、各移動体が送信および受信する他者ロボット位置情報、自己ロボット位置情報、他者ロボットロック位置情報及び自己ロボットロック位置情報は抽象化状態空間上における位置の情報として記述される。これにより位置情報を座標情報として簡便に取り扱うことが可能になる。

【0074】

（１０）移動体の各々は制御マイコンを有し、ハイレベル制御器は該制御マイコン上に実装され、制御マイコンは、有限状態遷移モデルの実装に必要となるサイズ以上のメモリ領域を有する。回避移動用有限状態遷移モデルの生成にはこのようなスペックのマイコンが要求される。

【0075】

（１１）ハイレベル制御器による目的地までの移動経路の計画は、ハイレベル制御器が生成する有限状態遷移モデルを重み付きの有向グラフと見立てて動的計画法を適用することにより得られる。これにより、各経路に重み付けを行うため、経路間の優先順位を考慮しながら移動経路を計画することが可能になる。

【0076】

（１２）回避移動用有限状態遷移モデル動的生成部による回避軌道の生成は、動的計画法を適用する際に設定する目的地を乱数生成により確率的に選択する。このような方法を採用することにより容易に回避軌道を生成することが可能になる。

【0077】

（１３）有限状態遷移モデルは構造体配列によって表現され、構造体配列の要素である構造体の各々には少なくとも１つの移動体の位置と角度と、各制御入力を印加した際に遷移可能な移動体の状態に対応する構造体へのポインタを格納する。これにより、回避移動用有限状態遷移モデルを視覚的に把握することが可能になり、簡便に処理することが可能になる。

【0078】

なお、本発明は、上記の実施例に限定されるものではなく、様々な変形が可能である。例えば、上記の実施例は、本発明を分かりやすく説明するために詳細に説明したものであり、本発明は、必ずしも説明した全ての構成を備える態様に限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能である。また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、削除したり、他の構成を追加・置換したりすることが可能である。

【符号の説明】

【0079】

０ 制御システム、
３、４、５ 移動ロボット、１００ 移動体、１０１ 自己位置取得部、１０２ 目的地移動用制御入力決定部、１０３ 制御入力修正部、１０４ デッドロック検知部、１０５ 回避移動用有限状態遷移システム動的生成部、１０６ 駆動部、１０７ 自己ロック位置出力部、１０８ 自己位置出力部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7A】

【図7B】

【図7C】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】