JP6606442B2

JP6606442B2 - 移動体の経路計画生成装置

Info

Publication number: JP6606442B2
Application number: JP2016033029A
Authority: JP
Inventors: 泰三吉川
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2016-02-24
Filing date: 2016-02-24
Publication date: 2019-11-13
Anticipated expiration: 2036-02-24
Also published as: US9995589B2; US20170241790A1; JP2017151687A

Description

本発明は、移動体の経路計画を生成する装置に関する。

移動体の経路計画を生成する手法としては、従来より、ダイクストラ法、確率的ロードマップ法、ＲＲＴ（Rapidly-exploring Random Tree）法、ポテンシャル法等の手法が一般的に知られている。

しかるに、これらの個々の手法のそれぞれは、一長一短があり、種々様々な状況下では、移動体の移動を適切に行い得る経路計画を生成することが困難となる場合も多々ある。

例えば、ＲＲＴ法では、移動体の動作環境に移動障害物が存在するような場合には、該移動障害物の移動状態の変化に対して動的に安定性の高い経路計画を生成することが困難となりやすい。

また、例えば、ポテンシャル法では、ポテンシャル場のポテンシャル値が、局所的に極小となるような状況が生じると、経路計画を適切に修正することができなくなることが発生しやすい。

一方、上記の如き、従来の手法の課題を解決するための手法として、例えば特許文献１に見られる如き手法も提案されている。この手法では、まず、ＲＲＴ法により基本の経路計画が生成される。さらに、この基本の経路計画の各ノードを質点とみなした多質点系に対して、各質点に作用する力に関する複数の拘束条件が設定される。そして、上記多質点系における動力学的なシミュレーションを実行することで、複数の拘束条件を満たし得る経路計画を生成する。

特開２００９−２１１５７１号公報

しかしながら、前記特許文献１に見られる手法では、複数の拘束条件を満たし得る経路計画は、一般に、動力学的なシミュレーションを繰り返すことで生成されることとなるため、該経路計画の生成に時間がかかりやすい。このため、例えば、移動体の動作環境で複数の移動障害物が移動するような環境下、あるいは、移動経路の予測を精度よく行うことが難しい移動障害物（例えば人）が存在するような環境下では、移動障害物の移動に伴う環境変化に対して、経路計画を素早く適切に更新することが困難である。さらに、経路計画の更新毎に、比較的大きく経路計画が変化してしまう虞もある。

本発明はかかる背景に鑑みてなされたものであり、移動体の動作環境の環境変化に対して経路計画を素早く動的に変化させることを可能としつつ、安定性の高い経路計画を生成することを可能とする経路計画生成装置を提供することを目的とする。

本発明の移動体の経路計画を生成する装置であって、
前記移動体の動作環境に存在する各障害物の存在位置を示す情報を少なくとも含む障害物情報を取得する障害物情報取得手段と、
前記移動体の移動を開始する初期位置から到達目標地点までの経路計画である初期経路計画を生成する初期経路計画生成手段と、
前記初期経路計画と前記障害物情報とに基づいて、該初期経路計画上の前記移動体といずれかの障害物との干渉が将来に発生することが予測される場合に、前記移動体の現在位置から到達目標地点までの経路計画を、ＲＲＴ（Rapidly-exploring Random Tree）法に基づく処理により前記移動体と各障害物との将来の干渉を回避し得るように生成することを実行するＲＲＴ経路計画生成手段と、
各障害物の周囲の領域に仮想的な斥力を発生するポテンシャル場を、前記障害物情報を用いて設定し、生成済の経路計画が、１つ以上の障害物のポテンシャル場が作用することが予測される区間を含む場合に、ポテンシャル法に基づく処理により、該区間に前記ポテンシャル場から作用する斥力を低減するように前記生成済の経路計画を修正することを、前記初期経路計画と、前記ＲＲＴ経路計画生成手段が生成した経路計画であるＲＲＴ経路計画とのうちの少なくともＲＲＴ経路計画に対して逐次実行する経路計画修正手段とを備えることを特徴とする（第１発明）。

なお、本発明において、「経路計画」は、移動体の移動時の位置（２次元空間又は３次元空間での位置）と時刻との関係を規定するものを意味する。

また、任意の経路計画に対して、「経路計画上の移動体」というのは、該経路計画に従って移動する移動体を意味する。

上記第１発明によれば、前記初期経路計画上の前記移動体といずれかの障害物との干渉が将来に発生することが予測される場合に、ＲＲＴ法に基づく処理によって、ＲＲＴ経路計画が生成される。

従って、前記初期経路計画生成手段は、移動体と障害物との干渉が将来に発生するか否かをさほど考慮することなく、簡易な手法で初期経路計画を生成し得る。

一方、前記ＲＲＴ経路計画は、該ＲＲＴ経路計画上での移動体と各障害物との将来の干渉を回避し得るように生成されるものの、該ＲＲＴ経路計画の生成後に、前記動作環境に存在する障害物のうちの移動障害物の移動状態の変化等の環境変化が生じた場合には、該ＲＲＴ経路計画上の移動体といずれかの障害物との干渉が発生する可能性がある。

しかるに、第１発明では、前記ＲＲＴ経路計画が生成された場合には、その後は、前記経路計画修正部が、ポテンシャル法に基づく処理により、該ＲＲＴ経路計画を修正する処理を逐次実行する。

このため、ＲＲＴ経路計画の生成後に、前記動作環境に存在する障害物のうちの移動障害物の移動状態の変化等の環境変化が生じても、移動体と各障害物との干渉が発生することを極力回避し得るように、ＲＲＴ経路計画が動的に修正されていく。

この場合、ＲＲＴ経路計画の修正は、ポテンシャル法に基づく処理により行われるので、該修正を素早く実行できると共に、修正後の経路計画が、元のＲＲＴ経路計画から乖離した経路計画となってしまうようなことを防止できる。

また、前記初期経路計画上の前記移動体といずれかの障害物との干渉が将来に発生することが予測されない場合には、該初期経路計画を、移動体の移動用の経路計画として利用することが可能である。

よって、第１発明によれば、移動体の動作環境の環境変化に対して経路計画を素早く動的に変化させることを可能としつつ、安定性の高い経路計画を生成することが可能となる。

上記第１発明では、前記経路計画修正手段は、前記初期経路計画生成手段による前記初期経路計画の生成後、前記ＲＲＴ経路計画生成手段により前記ＲＲＴ経路計画が生成されるまでは、前記初期経路計画を、ポテンシャル法に基づく処理により修正することを逐次実行するように構成されていることが好ましい（第２発明）。

これによれば、前記初期経路計画上の前記移動体といずれかの障害物との干渉が将来に発生することが予測されずに、前記ＲＲＴ経路計画が生成されない場合であっても、移動体と各障害物との干渉が発生することを極力回避し得るように、初期経路計画がポテンシャル法に基づく処理により動的に修正されていく。

従って、前記ＲＲＴ経路計画の生成前であっても、移動体の動作環境の環境変化に対して経路計画を素早く動的に変化させることを可能としつつ、安定性の高い経路計画を生成することが可能となる。

上記第１発明又は第２発明では、前記ＲＲＴ経路計画生成手段は、前記経路計画修正手段による修正後の経路計画と前記障害物情報とに基づいて、該修正後の経路計画上の前記移動体と障害物との干渉を回避することができないことが予測される場合に、前記ＲＲＴ経路計画を新たに生成する機能をさらに有するように構成され、前記経路計画修正手段は、前記ＲＲＴ経路計画生成手段により前記ＲＲＴ経路計画が新たに生成された場合には、当該新たなＲＲＴ経路計画を、ポテンシャル法に基づく処理により修正することを逐次実行するように構成されていることが好ましい（第３発明）。

ここで、前記経路計画修正手段による、生成済の経路計画（前記初期経路計画又はＲＲＴ経路計画）の修正処理（ポテンシャル法に基づく修正処理）では、移動体の動作環境における障害物の配置状態、あるいは、移動障害物の移動状態に応じてポテンシャル場の分布状態等によっては、修正後の経路計画が、移動体と障害物との干渉を回避することができない状態（例えば、経路計画が２つの障害物の間に挟まれてしまう状態等）に拘束されてしまう場合がある。

しかるに、第３発明によれば、このような場合には、前記ＲＲＴ経路計画生成手段によりＲＲＴ経路計画が新たに生成される。そして、その後は、新たなＲＲＴ経路計画が、経路計画修正手段の処理（ポテンシャル法の基づく処理）によって修正されていくこととなる。

従って、移動体と障害物との干渉が発生しないように移動体を移動させ得る適切な経路計画を生成することができなくなるのを極力防止して、移動体の移動を継続し得る経路計画を生成することができる。

また、第３発明によれば、前記ＲＲＴ経路計画生成手段によりＲＲＴ経路計画を生成することを、前記初期経路計画上の前記移動体といずれかの障害物との干渉が将来に発生することが予測される場合と、前記経路計画修正手段による修正後の経路計画上の前記移動体と障害物との干渉を回避することができないことが予測される場合とに限定することができる。このため、経路計画が、比較的大きく変化する可能性を極力低くして、経路計画の安定性を高めることができる。

上記第３発明では、前記経路計画修正手段は、前記生成済の経路計画をポテンシャル法に基づく処理により修正することを所定の制御処理周期で逐次実行するように構成されており、前記修正後の経路計画上の前記移動体と障害物との干渉を回避することができないことが予測される場合というのは、前記移動体といずれかの障害物との干渉が将来に発生することが予測される前記修正後の経路計画が、前記経路計画修正手段の所定の複数回の制御処理周期で繰り返して生成された場合であることが好ましい（第４発明）。

ここで、例えば、ある制御処理周期の時点で、前記修正後の経路計画上の移動体とある移動障害物との干渉が将来に発生することが予測される場合であっても、その後の移動障害物の移動状態の変化等によって、前記修正後の経路計画上の移動体とある移動障害物との干渉が将来に発生することが予測されなくなる場合がある。

そこで、第４発明では、前記移動体といずれかの障害物との干渉が将来に発生することが予測される前記修正後の経路計画が、前記経路計画修正手段の所定の複数回の制御処理周期で繰り返して生成された場合に、前記ＲＲＴ経路計画生成手段が、ＲＲＴ経路計画を新たに生成する。

これにより、前記ＲＲＴ経路計画生成手段によるＲＲＴ経路計画の生成が頻繁に行われるのを防止することができる。ひいては、経路計画の安定性をより一層高めることができる。

上記第１〜第４発明では、前記経路計画修正手段は、前記生成済の経路計画の前記区間に設定した１つ以上のノードを、少なくとも前記障害物のポテンシャル場から作用する斥力に応じて決定した変位量だけ変位させることにより、前記区間における経路計画を修正するように構成され得る（第５発明）。

これによれば、前記生成済の経路計画の前記区間に設定した１つ以上のノードを、ばねの変位と同様の形態で弾性的に変位させることができる。

上記第５発明では、前記経路計画修正手段は、前記区間における経路計画の修正後に、該区間に前記ポテンシャル場が作用しなくなることが予測される場合に、該区間を修正前の状態に徐々に戻すように構成されていることが好ましい（第６発明）。

これによれば、移動障害物の移動状態の変化等に起因して、前記区間に前記ポテンシャル場が作用しなくなることが予測される場合には、前記修正後の経路計画を、元の経路計画（初期経路計画又はＲＲＴ経路計画）に徐々に近づけることができる。ひいては前記修正後の経路計画が、元の経路計画（初期経路計画又はＲＲＴ経路計画）から過剰に変化するのを防止することができる。

上記第１〜第６発明では、前記初期経路計画、前記ＲＲＴ経路計画、及び前記経路計画修正手段による修正後の経路計画のうちの任意の１つの経路計画である対象経路計画に対して、該対象経路計画上の前記移動体と、前記障害物のうちの移動障害物との干渉が生じるか否かを、前記対象経路計画上の前記移動体の存在領域と、前記障害物情報に基づき設定した各障害物の存在領域とが重なり合う状態があるか否かによって判断する干渉判断手段を備えており、該干渉判断手段は、前記移動体と前記移動障害物との干渉が生じるか否かを判断するために用いる前記移動障害物の将来の存在領域を、該存在領域の大きさが時間の経過に伴いゼロまで減衰する領域となるように設定するように構成されていることが好ましい（第７発明）。

ここで、前記移動障害物、特に人等の移動障害物は、移動速度等が変動しやすいため、該移動障害物と移動体とが比較的離れている状態では、前記対象経路計画上の移動体と、前記移動障害物との干渉が生じるか否かの判断を、高い信頼性で判断することは難しい。

そこで、第７発明では、前記干渉判断手段は、前記移動体と前記移動障害物との干渉が生じるか否かを判断するために用いる前記移動障害物の将来の存在領域を、該存在領域の大きさが時間の経過に伴いゼロまで減衰する領域となるように設定する。

このため、移動障害物と移動体とが比較的離れている状態では、該移動障害物が移動体に近づいてくることが予測される場合であっても、該移動障害物の存在領域の大きさは、移動体に近づくに伴い、減衰して、最終的にゼロになる（該存在領域が消滅する）。

これにより、移動障害物と移動体とが比較的離れている状態で、該移動障害物と移動体との干渉が将来に発生すると前記干渉判断手段が判断するの極力抑制することができる。

その結果、前記ＲＲＴ経路計画生成手段が、不必要に移動障害物と移動体との干渉を回避するＲＲＴ経路計画を生成したり、あるいは、ＲＲＴ経路計画を新たに生成することを極力抑制できる。ひいては、生成する経路計画の安定性をより一層高めることができる。

上記第７発明では、前記経路計画修正手段は、前記移動障害物の将来のポテンシャル場の領域を、該移動障害物の存在領域の大きさの減衰に伴い縮小させるように設定するように構成されていることが好ましい（第８発明）。

これによれば、移動障害物と移動体とが比較的離れている状態では、該移動障害物が移動体に近づいてくることが予測される場合であっても、該移動障害物のポテンシャル場は、移動体に近づくに伴い、減衰して、最終的に消滅する。

その結果、前記経路計画修正手段が、不必要に経路計画の修正を行うのを防止することができる。ひいては、前記経路計画修正手段による修正後の経路計画の安定性を高めることができる。

本発明の実施形態における移動体の動作環境を例示する図。実施形態の移動体を示す図。動作環境に備えた障害物検知センサの一例を示す図。実施形態の経路計画生成装置の機能的構成を示すブロック図。図５Ａ及び図５Ｂは、それぞれ、固定障害物又は静止障害物と移動体との干渉が生じないと判断される状況と、該干渉が生じると判断される状況とを例示する図。移動障害物の存在領域の経時変化を示す図。図７Ａ及び図７Ｂは、それぞれ、移動障害物と移動体との干渉が生じないと判断される状況と、該干渉が生じると判断される状況とを例示する図。図８Ａ及び図８Ｂは、移動障害物が移動状態から停止した場合における移動障害物の存在領域の経時変化を示す図。図４に示すＲＲＴ経路計画生成部の処理を示すフローチャート。図１０Ａ、図１０Ｂ及び図１０Ｃは、図４に示すＲＲＴ経路計画生成部の処理を説明するための図。図４に示すＲＲＴ経路計画生成部の処理により生成されるＲＲＴ経路計画を例示する図。障害物のポテンシャル場を例示する図。図４に示す経路計画修正部の処理を示すフローチャート。図４に示す経路計画修正部の処理を説明するための図。図４に示す経路計画修正部の処理による経路計画の修正例を示す図。図４に示す経路計画生成処理部が実行する処理を示すフローチャート。時刻ｔ0で最初に生成される経路計画の一例を示す図。時刻ｔ0の後の時刻ｔ1で修正された経路計画の一例を示す図。時刻ｔ1の後の時刻ｔ2で修正された経路計画の一例を示す図。時刻ｔ2の後の時刻ｔ3で修正された経路計画の一例を示す図。時刻ｔ3の後の時刻4で修正された経路計画の一例を示す図。図２２Ａは固定障害物又は静止障害物と移動障害物との間に経路計画が挟まれた状態を示す図、図２２Ｂは２つの移動障害物の間に経路計画が挟まれた状態を示す図。図４に示す移動指令部の処理を説明するための図。

本発明の一実施形態を図１〜図２３を参照して以下に説明する。図１は、本実施形態における移動体１を移動させる動作環境を概念的に例示している。移動体１は、本実施形態の一例では、例えば、図２に示す如き人型の脚式移動ロボットである。この移動体１を移動させる動作環境には、図１に概念的に例示する如く、複数の障害物Ａａ，Ａｂ，Ａｃが存在する。

障害物Ａａ，Ａｂ，Ａｃは、移動体１の移動時に該移動体１との接触を避けるべき物体である。障害物Ａａ，Ａｂ，Ａｃには、例えば、建物の壁、柱等の固定障害物Ａａ、動作環境に設置されたテーブル、パーティション、棚、椅子、鉢植え、表示板等の静止障害物Ａｂ、あるいは、人、他の移動ロボット等の移動障害物Ａｃが含まれる。なお、静止障害物Ａｂは、より詳しくは、固定障害物Ａａのように定常的に不動に固定されたものではないが、通常時は、静止状態に保たれる障害物である。

以降の説明では、障害物Ａａ，Ａｂ，Ａｃを区別する必要がないときは、それらを総称的に障害物Ａと称する。

本実施形態は、上記の如き障害物Ａが存在する動作環境において、移動体１（脚式移動ロボット）を、任意のスタート地点（移動開始地点）から到達目標地点まで、各障害物Ａとの干渉（接触）を生じることなく、移動させ得る経路計画を生成する装置の実施形態である。該経路計画は、移動体１の移動経路（詳しくは、移動体１の存在領域の代表点の移動経路）の空間的な配置（位置及び方向）と、該移動経路上の各位置に対応する時刻とを規定する情報、あるいは、移動体１の移動期間の各時刻での該移動体１の位置（詳しくは、移動体１の存在領域の代表点の位置）を規定する情報により構成される。

動作環境には、図３に例示する如く、そこに存在する障害物Ａを検知するための障害物検知センサ３が配置されている。該障害物検知センサ３としては、例えば、レーザ・レンジ・ファイダ等のレーザ式の多点測距センサ、カメラ、レーダ等を使用し得る。

また、移動体１には、動作環境における自己位置を認識し、あるいは、移動体１の周囲環境を認識するためのセンサ（図示省略）が搭載されている。該センサとしては、例えば、レーザ・レンジ・ファイダ等のレーザ式の多点測距センサ、カメラ、レーダ等を使用し得る。なお、移動体１の自己位置を認識するためのセンサとしては、上記のセンサの他、例えば、慣性航法方式に基づく自己位置認識用のセンサ（加速度センサ、ジャイロセンサ等）、移動体１の各関節の変位量を検出する関節変位センサ、ＧＰＳ受信機等も使用し得る。

図４は、本実施形態の経路計画生成装置１０の基本構成を示すブロック図である。この経路計画生成装置１０は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニット、あるいは、コンピュータ、あるいは、これらの組み合わせにより構成される。該経路計画生成装置１０の構成要素は、移動体１に搭載されたもの、あるいは、移動体１の外部に設置されたもののいずれであってもよい。また、経路計画生成装置１０は、移動体１に搭載された構成要素と移動体１の外部に設置された構成要素との両方を含んでいてもよい。

かかる経路計画生成装置１０には、上記した障害物検知センサ３と移動体１のセンサ（以降、移動体搭載センサという）のそれぞれの検出データ（センシングデータ）と、移動体１の操縦装置もしくはスケジュールサーバ等から出力される移動体１の移動要求（移動体１の移動開始タイミング、移動時の到達目標地点、経由点、移動速度等に関する要求）とが入力される。

そして、経路計画生成装置１０は、実装されるプログラム又はハードウェア構成により実現される機能部として、移動体１及び各障害物Ａのそれぞれの位置情報を取得する位置情報取得部１１と、該位置情報等を用いて移動体１の経路計画を生成する処理を実行する経路計画生成処理部１２と、該経路計画に基づいて、移動体１に対して移動指令を出力する移動指令部１３とを備える。

以下、経路計画生成装置１０の各機能部の処理の詳細を説明する。なお、以下の本実施形態の説明は、理解の便宜上、移動体１が平面状の床面を移動する場合を主要例として行う。

（位置情報取得部１１の処理）
位置情報取得部１１は、前記障害物検知センサ３の検出データを用いて、各障害物Ａの位置情報を逐次取得すると共に、前記移動体搭載センサ（図示省略）の検出データを用いて、移動体１の位置情報を逐次取得する。なお、該位置情報取得部１１は、各障害物Ａの位置情報を取得する機能を有するので、本発明における障害物情報取得手段としての機能を有する。この場合、各障害物Ａの位置情報は、本発明における障害物情報に相当する。

ここで、各障害物Ａ及び移動体１のそれぞれの位置情報は、より詳しくは、各障害物Ａ及び移動体１のそれぞれの存在領域の位置を表す情報である。

移動体１の存在領域は、その内側に実際の移動体１の全体が存在し、もしくは、存在する可能性が高いとみなし得る領域である。このことは、各障害物Ａの存在領域についても同様である。この場合、移動体１及び各障害物Ａのそれぞれの存在領域は、移動体１及び各障害物Ａのそれぞれの外面形状の凹凸、もしくは、上記検出データの誤差等を考慮して、該検出データから特定される実際の存在領域よりも大きめの領域として定義される。

例えば、移動体１の存在領域は、本実施形態では、図２に例示する如く、例えば円形状の領域として定義される。

この場合、本実施形態では、移動体１が脚式移動ロボットであるため、該移動体１の移動時（歩行時）に、該移動体１の上体（基体）が左右に揺れ動く。このため、移動体１の存在領域は、図２に例示する如く、移動体１の実際の幅Ｗ１に、移動体１の上体の揺れ幅の設定値α１と、所定値のマージンα２とを加算してなる幅Ｗ（直径）を有する領域として定義される。

障害物Ａが、人もしくは脚式移動ロボット（移動体１と同様の構成の移動ロボット）である場合（すなわち、脚式の移動障害物Ａｃである場合）には、該障害物Ａｃの存在領域は、図３に例示するように、移動体１の存在領域と同様の領域として定義される。

また、障害物Ａが、固定障害物Ａａ又は静止障害物Ａｂである場合には、該障害物Ａａ又はＡｂの存在領域は、例えば、図３に例示するように、該障害物Ａａ又はＡｂの実際の表面位置から所定値のマージンだけ張り出した領域として定義される。

なお、図２に示した移動体１の存在領域、あるいは、図３に示した各障害物Ａの存在領域は、より詳しくは、水平面（又は床面）に投影して見た移動体１の経路計画を生成する場合に使用する２次元領域である。

移動体１を３次元的に移動させる経路計画を生成する場合には、移動体１及び各障害物Ａのそれぞれの存在領域として、３次元領域を使用し得る。例えば移動体１もしくは移動障害物Ａｃの存在領域として、移動体１もしくは移動障害物Ａｃの実際の高さよりも若干大きい高さの円柱状領域等の柱状領域を使用し得る。この場合、当該存在領域の横断面の面積を高さ方向に変化させることも可能である。

補足すると、移動体１及び各障害物Ａのそれぞれの存在領域は、該移動体１及び各障害物Ａのそれぞれの外形状等に応じて種々様々な形状を採用し得る。例えば、移動体１もしくは移動障害物Ａｃの存在領域として、楕円形状又は多角形状の２次元領域、又は横断面が楕円形状又は多角形状となる柱状の３次元領域を採用し得る。

また、移動体１もしくは移動障害物Ａｃが、移動時に横揺れが生じないか、もしくは、該横揺れが十分に微小なもの（例えば車輪型の移動体もしくは移動障害物）である場合には、移動体１もしくは移動障害物Ａｃの存在領域の幅は、揺れ幅分を含む必要はない。

位置情報取得部１１は、上記の如く移動体１及び各障害物Ａ毎に定義される存在領域の位置を表す位置情報を、移動体搭載センサ及び障害物検知センサ３の検出データを用いて、所定の制御処理周期で逐次取得する。

ここで、位置情報取得部１１が取得する移動体１及び各障害物Ａのそれぞれの存在領域の位置情報は、動作環境に対して任意に設計的に設定されるグローバル座標系（例えば図３に示すＸＹＺ直交座標系）で見た該存在領域の位置を表す情報である。

この場合、移動体１及び各障害物Ａの位置情報は、例えば次のような手順で取得することができる。

すなわち、前記移動体搭載センサの検出データから、移動体１用の座標系で見た移動体１の位置の推定値が逐次得られる。さらに、この位置推定値を座標変換することで、グローバル座標系で見た移動体１の位置の推定値が逐次得られる。そして、この座標変換により得られた位置推定値から、移動体１の存在領域の定義に基づいて、グローバル座標系で見た移動体１の存在領域の位置を表す位置情報が逐次取得される。

また、前記障害物検知センサ３の検出データから、障害物検知センサ３用の座標系（センサ座標系）で見た各障害物Ａの表面部の位置の推定値が逐次得られる。さらに、この位置推定値を座標変換することで、グローバル座標系で見た各障害物Ａの表面部の位置の推定値が逐次得られる。そして、この座標変換により得られた位置推定値から、各障害物Ａの存在領域の定義に基づいて、グローバル座標系で見た移動体１の存在領域の位置を表す位置情報が逐次取得される。

さらに、障害物Ａに関しては、位置情報取得部１１は、例えば、該障害物Ａの位置の推定値の時系列に基づいて、該障害物Ａが移動障害物Ａｃであるか否かを特定する。そして、位置情報取得部１１は、移動障害物Ａｃに対しては、その位置推定値（グローバル座標系で見た位置推定値）の時系列に基づいて、該移動障害物Ａｃの移動速度（移動方向を含む）を逐次推定する処理も逐次実行する。

補足すると、前記障害物検知センサ３の検出データには、障害物Ａだけでなく、移動体１に関する検出データも含まれる。このため、位置情報取得部１１は、移動体１の自己位置情報を用い、障害物検知センサ３の検出データから移動体１に関する検出データを除く処理を行う。

なお、固定障害物Ａａの位置は定常的な一定値である。従って、固定障害物Ａａの存在領域の位置を表す位置情報は、あらかじめ経路計画生成装置１０のメモリに記憶保持してもよい。この場合には、固定障害物Ａａに関する検出データは不要である。

また、静止障害物Ａｂの位置は、一般に、一定に維持される期間が長い。従って、静止障害物Ａｂの位置情報は、例えば、移動障害物Ａｃよりも十分に長い時間間隔で定期的に、検出データに基づいて取得するようにしてもよい。

（経路計画生成処理部１２の処理）
次に、経路計画生成処理部１２は、移動体１の移動要求に応じて、移動体１の経路計画を生成する処理を所定の制御処理周期で逐次実行する。

本実施形態では、経路計画生成処理部１２は、図４に示すように、移動体１の初期位置（スタート地点）から到達目標地点までの初期経路計画を生成する処理を実行する初期経路計画生成部２１と、移動体１の現在位置から到達目標地点までのＲＲＴ経路計画をＲＲＴ（Rapidly-exploring Random Tree）法により生成する処理を実行するＲＲＴ経路計画生成部２２と、初期経路計画又はＲＲＴ経路計画をポテンシャル法により適宜修正する処理を実行する経路計画修正部２３と、生成もしくは修正された各経路計画上で、移動体１と障害物Ａとの干渉（接触）が発生するか否か（もしくは発生する可能性が高いか否か）の判断処理（以降、干渉判断処理という）を適宜実行する干渉判断処理部２４とを主たる要素処理部として含む。

なお、初期経路計画生成部２１、ＲＲＴ経路計画生成部２２、経路計画修正部２３、及び干渉判断処理部２４は、それぞれ、本発明における初期経路計画生成手段、ＲＲＴ経路計画生成手段、経路計画修正手段、干渉判断手段に相当する。

以下に、これらの要素処理部のそれぞれの処理を説明しておく。なお、以降の説明では、Ｘ軸、Ｙ軸は、特にことわらない限り、グローバル座標系の座標軸を意味し、Ｘ軸及びＹ軸により形成されるＸＹ平面は水平面（又は動作環境の平面状の床面）を意味する。

（干渉判断処理部２４の処理）
まず、干渉判断処理部２４の処理を説明する。干渉判断処理部２４は、基本的には、経路計画上の現在時刻以後の各時刻の地点での移動体１の存在領域が、各障害物Ａの存在領域と重なる部分を有するか否かによって、移動体１と該障害物Ａとの干渉が将来に発生するか否かの判断処理を適宜実行する。ただし、本実施形態では、障害物Ａが固定障害物Ａａ又は静止障害物Ａｂである場合と、移動障害物Ａｃである場合とで、判断処理の手法が若干相違する。

障害物Ａが固定障害物Ａａ又は静止障害物Ａｂである場合においては、干渉判断処理部２４は、図５Ａに例示する如く、経路計画上の任意の時刻での移動体１の存在領域が、固定障害物Ａａ又は静止障害物Ａｂの存在領域と重なり合うことが無い場合に、移動体１と該固定障害物Ａａ又は静止障害物Ａｂとの干渉が発生しないと判断する。なお、図５Ａでは、移動体１の存在領域（ＸＹ平面上の存在領域）の代表例として、３つの時刻ｔ1，ｔ2，ｔ3での移動体１の存在領域１(t1)，１(t2)，１(t3)を例示している。

また、干渉判断処理部２４は、図５Ｂに例示する如く、経路計画上のある時刻での移動体１の存在領域が、固定障害物Ａａ又は静止障害物Ａｂの存在領域と重なり合う場合に、移動体１と該固定障害物Ａａ又は静止障害物Ａｂとの干渉が発生すると判断する。なお、図５Ｂでは、移動体１の存在領域（ＸＹ平面上の存在領域）の代表例として、３つの時刻ｔ1，ｔ2，ｔ3での移動体１の存在領域１(t1)，１(t2)，１(t3)を例示している。そして、この図示例では、時刻ｔ2での移動体１の存在領域１(t2)が、固定障害物Ａａ又は静止障害物Ａｂの存在領域と重なり合っている。

なお、かかる干渉判断処理部２４の処理において、移動体１と固定障害物Ａａ又は静止障害物Ａｂとの干渉の発生の有無を判断するために使用する該障害物Ａａ又はＡｂの存在領域は、前記位置情報取得部１１が、障害物検知センサ３の検出データの基づいて決定した最新の存在領域である。ただし、固定障害物Ａａの存在領域は、あらかじめメモリに記憶保持されたものでもよい。

一方、障害物Ａが移動障害物Ａｃである場合においては、干渉判断処理部２４は、図６に示すように、移動障害物Ａｃが、現在時刻ｔ0での移動障害物Ａｃの移動速度Ｖacの推定値（位置情報取得部１１により算出された推定値）と同じ一定速度で直線的に移動していくものとみなして、現在時刻ｔ0以後の移動障害物Ａｃの移動経路を予測する。そして、干渉判断処理部２４は、移動障害物Ａｃの予測経路上の各位置（将来位置）での移動障害物Ａｃの存在領域が、現在時刻ｔ0以後の経路計画上での将来の移動体１の存在領域と重なり合う部分を有するか否かによって、移動体１と該移動障害物Ａｃとの干渉が発生するか否かを判断する。

ただし、この場合においては、干渉判断処理部２４は、移動障害物Ａｃの現在時刻ｔ0以後の将来の存在領域の大きさ、例えば幅Ｗacが、図６に示す如く、本来の幅（前記した如く移動障害物Ａｃが内側に収まるように定義した存在領域の幅）から、移動障害物Ａｃの移動速度Ｖacの大きさに応じた減衰速度（単位時間当たりの減少量又は減少割合）で、時間の経過に伴いゼロまで減衰するように、移動障害物Ａｃの将来の存在領域を設定する。

ここで、図６の中の三角形が、移動障害物Ａｃの存在領域（図示例では、ＸＹ平面上での２次元領域）の幅Ｗacが一定の減衰速度で減少していく態様を表している。

この場合、本実施形態では、干渉判断処理部２４は、移動速度Ｖacが大きいほど、上記減衰速度が小さくなるように、Ｖacの推定値から、あらかじめ作成された演算式又はデータテーブル等を用いて上記減衰速度を決定する。

例えば、図６を参照して、将来の時刻ｔ1における移動障害物Ａｃの存在領域Ａc(t1)は、図示の如く、現在時刻ｔ0での移動障害物Ａｃの移動速度Ｖacの推定値（図示例では、Ｖac：大）と現在時刻ｔ0からの経過時間（＝ｔ1−ｔ0）とに応じて規定される量だけ、現在時刻ｔ0での移動障害物Ａｃの存在領域Ａc(t0)よりも小さい幅の存在領域として設定される。

補足すると、現在時刻ｔ0以後の移動障害物Ａｃの存在領域の大きさの減衰の態様は、上記以外の態様を採用し得る。例えば、移動障害物Ａｃの存在領域の面積（ＸＹ平面上での面積）を、移動速度Ｖacに応じた減衰速度で減衰させる態様、あるいは、移動障害物Ａｃの存在領域の幅Ｗac又は面積を、移動速度Ｖacによらずに所定の減衰速度で減衰させる態様、あるいは、移動障害物Ａｃの存在領域の幅Ｗac又は面積の減衰速度を、時間の経過に伴い変化させる態様等を適宜採用し得る。

また、移動障害物Ａｃの存在領域を３次元領域として設定する場合には、該存在領域をＸＹ平面に投影した領域の幅もしくは面積と、該存在領域の高さとを減衰させる態様、あるいは、該存在領域の体積を減衰させる態様等を採用し得る。

さらに、移動障害物Ａｃの種別に応じて、その存在領域の大きさの減衰速度を変化させる態様等も採用し得る。

ここで、上記のように、移動体１と移動障害物Ａｃとの将来の干渉の有無を判断するにあたって、現在時刻ｔ0以後の移動障害物Ａｃの存在領域の大きさを、時間の経過に伴い減衰させることの理由を説明しておく。

すなわち、移動障害物Ａｃと移動体１との間の距離が比較的大きい状態では、移動障害物Ａｃの移動方向や移動速度は、移動障害物Ａｃが移動体１に十分に近づいてくるまでに変化する場合が多々ある。特に、移動障害物Ａｃが、人等の脚式の移動障害物Ａｃである場合には、その上体の横揺れの影響によって、前記位置情報取得部１１が障害物検知センサ３の検出データに基づいて推定する移動障害物Ａｃの移動速度の向きが変動しやすい。

従って、移動障害物Ａｃと移動体１との間の距離が比較的大きい状態での該移動障害物Ａｃの予測経路は、一般に、該予測経路上の位置が移動体１に近づくに伴い、信頼性が乏しいものとなる。

さらに、移動障害物Ａｃの移動速度が遅いほど、該移動障害物Ａｃが移動体１に十分に近づくまでに要する時間が長くなると共に、移動障害物Ａｃが移動体１に十分に近づくまでに、該移動障害物のＡｃの移動方向が比較的大きく変化する状況も発生し得る。

このため、移動障害物Ａｃと移動体１との間の距離が比較的大きい状態では、移動障害物Ａｃの予測経路は、一般に、移動障害物Ａｃの移動速度（現在時刻での推定値）が遅いほど、移動体１に近い側での信頼性がより乏しいものとなる。

従って、仮に、移動障害物Ａｃと移動体１との間の距離が比較的大きい状態において、移動障害物Ａｃの存在領域を、実際の移動障害物Ａｃを包含する一定サイズに保たれる領域とみなして、現在時刻以後の予測経路上の移動障害物Ａｃと、移動体１との将来の干渉の有無を判断するようにした場合には、特に、移動体１に近い側において、当該判断を高い信頼性で行うことは一般には困難である。

そして、移動障害物Ａｃと移動体１との将来の干渉の有無の判断結果の信頼性が乏しい場合には、その判断結果に応じて、移動体１の経路計画の生成あるいは修正を行うと、経路計画が不必要に過剰に変化したり、あるいは、干渉を回避し得る経路計画を生成できずに、移動体１を不必要に早期に停止させることとなる等の不都合を生じる。

また、移動障害物Ａｃと移動体１との将来の干渉を回避するように経路計画を生成もしくは修正することは、多くの場合、移動障害物Ａｃと移動体１とがある程度接近した時点でも可能である。

そこで、本実施形態では、干渉判断処理部２４は、移動体１と移動障害物Ａｃとの干渉を判断する干渉判断処理では、移動障害物Ａｃの現在時刻以後の存在領域の大きさ（幅Ｗac）を、前記した如く、移動障害物Ａｃの移動速度Ｖacの推定値に応じた減衰速度で、時間の経過に伴いゼロまで減衰させるように、予測経路上の各位置での移動障害物Ａｃの存在領域を設定する。

そして、干渉判断処理部２４は、このように設定した移動障害物Ａｃの存在領域と、移動体１の存在領域とが重なり合う部分を有するか否かによって、移動障害物Ａｃと移動体１との干渉の有無を判断する。

このようにすることにより、移動障害物Ａｃと移動体１との間の距離が比較的大きい状態では、移動障害物Ａｃと移動体１との将来の干渉が生じると判断される状況が発生しにくくなる。

この結果、移動障害物Ａｃと移動体１との間の距離が比較的大きい状態では、干渉判断処理部２４が、移動障害物Ａｃの信頼性の低い予測経路に基づいて、移動障害物Ａｃと移動体１との将来の干渉が生じると判断することとなるのを極力防止することができる。

例えば、図７Ａに例示する状況は、現在時刻ｔ0において、移動障害物Ａｃが経路計画上の移動体１に近づいていくように移動している状況であるものの、現在時刻ｔ0での移動体１の存在領域１(t0)と、移動障害物Ａｃの存在領域Ａc(t0)は、比較的大きく離れている。

この状況では、移動障害物Ａｃが移動体１の経路計画上の位置に達する時刻ｔ1の前までに、移動障害物Ａｃの存在領域の幅又は面積がゼロまで減衰される。このため、経路計画上での移動体１の存在領域と、移動障害物Ａｃとの存在領域が重なり合うことはない。従って、干渉判断処理部２４は、現在時刻ｔ0の時点では、移動体１と移動障害物Ａｃとの干渉は生じないと判断する。

なお、図７Ａでは、移動体１及び移動障害物Ａｃの存在領域を示す下段側の図は、ＸＹ平面上（あるいは動作環境の床面上）での移動体１及び移動障害物Ａｃの存在領域の移動位置を例示しており、上段側の図は時間軸方向の変位を含めた、移動体１及び移動障害物Ａｃの存在領域の移動位置を例示している。このことは、後述する図７Ｂ等、図７Ａと同様の形態の各図において同様である。

一方、移動障害物Ａｃと移動体１との間の距離が比較的小さい状態では、移動障害物Ａｃが移動体１に近づくように移動している状況での予測経路上の移動障害物Ａｃの存在領域は、さほど小さくならずに、移動体１の存在領域に接近することとなる。そして、この場合、移動障害物Ａｃが移動体１に比較的近い位置に存在するため、移動障害物Ａｃの予測経路の信頼性も比較的高い。

従って、移動障害物Ａｃと移動体１との干渉が実際に生じる可能性が高い状況では、干渉判断処理部２４は、適切に、移動障害物Ａｃと移動体１との将来の干渉が生じると判断することができることとなる。

例えば、図７Ｂに例示する状況は、現在時刻ｔ0において、移動障害物Ａｃが移動体１に比較的近い位置に存在している状態で、該移動障害物Ａｃが、経路計画上の移動体１に近づいていくように移動している状況である。この状況では、移動障害物Ａｃが、移動体１の経路計画上の位置に達する時刻近辺の時刻ｔ1では、該移動障害物Ａｃの存在領域Ａc(t1)の幅（又は面積）が、現在時刻ｔ0での存在領域Ａc(t0)の幅（又は面積）に比してさほど減衰していない。

このため、時刻ｔ1において、移動体１の存在領域１(t1)と、移動障害物Ａｃの存在領域Ａc(t1)とが重なり合う。従って、干渉判断処理部２４は、現在時刻ｔ0の時点にて、将来の時刻ｔ1に、移動体１と移動障害物Ａｃとの干渉が生じると判断する。

また、本実施形態では、移動障害物Ａｃが停止していると推定される場合（移動速度Ｖacの推定値が所定値Ｖac_min以下の大きさ（ゼロを含む）である場合）においては、干渉判断処理部２４は、図６に例示するように、移動障害物Ａｃの存在領域の大きさ（図示例では、幅Ｗac又は面積）を一定に維持した状態で、移動障害物Ａｃと移動体１との干渉の発生の有無を判断する。

ただし、移動障害物Ａｃの移動速度Ｖacの推定値が上記所定値Ｖac_minよりも大きい速度から、所定値Ｖac_minよりも小さい速度に変化した場合（移動障害物Ａｃが移動状態から停止状態に変化したと推定される場合）には、その変化の直後においては、干渉判断処理部２４は、図８Ａに示すように、現在時刻ｔ0（停止状態の開始時刻）から所定時間ｄｔ0が経過した後（時刻ｔ0＋ｄｔ0の後）は、移動障害物Ａｃの存在領域の大きさ（幅Ｗac又は面積）がゼロになるものとみなして、該移動障害物Ａｃと移動体１の干渉の発生の有無を判断する。

この場合、移動体１の停止直前において、移動障害物Ａｃの存在領域の大きさが時間の経過に伴いゼロまで減衰することから、本実施形態では、干渉判断処理部２４は、移動体１の停止状態の開始時刻ｔ0では、移動障害物Ａｃの存在領域の大きさが一定に維持される時間幅としての上記所定時間ｄｔ0として、図８Ａに示すように、移動体１の停止直前の移動速度に応じた減衰速度で、移動障害物Ａｃの存在領域の大きさがゼロまで減衰するのに要する時間ｄｔ0と同じ時間幅を設定する。

そして、その後は、干渉判断処理部２４は、移動障害物Ａｃの存在領域の大きさが一定に維持される時間幅を、徐々に延長する。例えば、図８Ｂに例示する如く、移動体１の停止状態の開始時刻ｔ0の後の時刻ｔ1では、移動障害物Ａｃの存在領域の大きさが一定に維持される時間幅ｄｔ1は、移動体１の停止状態の開始時刻ｔ0での時間幅ｄｔ0よりも長い時間幅に設定される。

以上が、干渉判断処理部２４の処理の詳細である。

（初期経路計画生成部２１の処理）
次に、初期経路計画生成部２１の処理を説明する。初期経路計画生成部２１が生成する初期経路計画は、移動体１の移動要求に応じた移動を新たに開始するタイミング又はその直前のタイミングで簡易な手法により生成される基本の経路計画である。

この初期経路計画は暫定的なものでよい。該初期経路計画は、いずれかの障害物Ａと移動体１との干渉が生じ得る経路計画、あるいは、いずれかの障害物Ａと移動体１との干渉を回避することの確実性が低い経路計画であってもよい。

一例として、初期経路計画生成部２１は、例えば、移動体１のスタート地点（移動開始地点）から到達目標地点まで、移動要求により指定された移動速度で（又はあらかじめ定められた所定の移動速度で）、直線的に移動させる経路計画を初期経路計画として生成し得る。

あるいは、初期経路計画生成部２１は、例えば、移動体１のスタート地点から到達目標地点まで、動作環境に存在する障害物Ａのうちの固定障害物Ａａ（又は固定障害物Ａａ及び静止障害物Ａｂ）と移動体１との干渉だけを回避し得るように、移動要求により指定された移動速度で（又はあらかじめ定められた所定の移動速度で）、移動体１を直線状もしくは折れ線状に移動させる経路計画を初期経路計画として生成し得る。

なお、初期経路計画は、上記以外の手法により生成してもよい。例えば、簡易的に、移動障害物Ａｃを静止障害物Ａｂと見なした上で、ＲＲＴ法等の手法により、各障害物Ａと移動体１との干渉を回避し得るように初期経路計画を生成してもよい。

（ＲＲＴ経路計画生成部２２の処理）
次に、ＲＲＴ経路計画生成部２２の処理を説明する。ＲＲＴ経路計画生成部２２が生成するＲＲＴ経路計画は、ＲＲＴ法により、移動体１と各障害物Ａとの干渉を回避し得るように探索的に生成される経路計画である。

本実施形態では、ＲＲＴ経路計画生成部２２は、移動体１の現在位置から到達目標地点までの複数のＲＲＴ経路計画をＲＲＴ法を用いて暫定的に生成する。そして、ＲＲＴ経路計画生成部２２は、生成した複数のＲＲＴ経路計画から、移動体１と障害物Ａとの干渉を極力高い信頼性で回避する上で好適な１つのＲＲＴ経路計画を選定して出力する。

この場合、最終的に選定されるＲＲＴ経路計画の候補となる複数の暫定的なＲＲＴ経路計画のそれぞれは、ＲＲＴ法の手法に基づいて、例えば、図９のフローチャートに示す処理によって生成される。

ＳＴＥＰ１において、ＲＲＴ経路計画生成部２２は、ＲＲＴ法に基づく探索木の起点ノードＮｄ(0)の位置を設定する。例えば、位置情報取得部１１により取得された移動体１の現在位置（移動体１の存在領域の中心点の現在位置）が、起点ノードＮｄ(0)の位置として設定される。ただし、起点ノードＮｄ(0)の位置は、移動体１の現在位置から将来側にずらした位置に設定されてもよい。例えば、移動体１の移動のために使用している現在の経路計画上で、移動体１の現在位置から進行方向前方側にずらした位置であって、且つ、現在位置との間の経路区間で移動体１と障害物Ａとの干渉が生じることがない位置を、探索木の起点ノードＮｄ(0)の位置として設定してもよい。

次いで、ＳＴＥＰ２において、ＲＲＴ経路計画生成部２２は、新規ノードＮｄ_newをランダムに生成する。

そして、ＳＴＥＰ３において、ＲＲＴ経路計画生成部２２は、生成済の探索木の全てのノードから、新規ノードＮｄ_newに対する親ノードＮｄ_pを抽出する。親ノードＮｄ_pは、生成済の探索木の全てのノードのうち、新規ノードＮｄ_newとの距離が最小となるノードである。

例えば、図１０Ａに示す如く探索木が生成されている状態で、新規ノードＮd_newが生成された場合、同図中のノードＮd_pが親ノードとして抽出される。

なお、ＳＴＥＰ２で最初に生成される新規ノードＮd_newに対する親ノードＮd_pは、前記起点ノードＮd(0)である。

次いで、ＳＴＥＰ４において、ＲＲＴ経路計画生成部２２は、新規ノードＮｄ_newに対応する時刻ｔ_newを決定する。この時刻ｔ_newは、図１０Ｂに示すように、新規ノードＮｄ_newと親ノードＮｄ_pとの間の距離ｄＤを、移動体１の移動速度Ｖの指令値（移動要求に基づく指令値又はあらかじめ設定された所定値）で除算してなる時間ｄＴ（すなわち、親ノードＮｄ_pから新規ノードＮｄ_newまでの移動所要時間ｄＴ）を、親ノードＮｄ_pの時刻ｔ_pに加えた時刻とされる。

次いで、ＳＴＥＰ５において、ＲＲＴ経路計画生成部２２は、親ノードＮｄ_pの位置から新規ノードＮd_newの位置まで移動体１を直線経路で移動させたと仮定した場合に、親ノードＮｄ_pから新規ノードＮｄ_newまでの区間での移動体１と障害物Ａとの干渉が発生するか否かの判断処理を前記干渉判断処理部２４に実行させる。

このＳＴＥＰ５の判断結果が肯定的である場合には、ＲＲＴ経路計画生成部２２は、前記ＳＴＥＰ２からの処理を改めて実行する。この場合、先に生成した新規ノードＮｄ_newは消去される。

このＳＴＥＰ５の判断結果が否定的である場合には、ＲＲＴ経路計画生成部２２は、ＳＴＥＰ６において、新規ノードＮｄ_newを探索木の構成要素のノードとして登録する。

さらに、ＲＲＴ経路計画生成部２２は、ＳＴＥＰ７において、新規ノードＮｄ_newから到達目標地点まで移動体１を直線経路で移動させたと仮定した場合に、移動体１と障害物Ａとの干渉が発生するか否かの判断処理を前記干渉判断処理部２４に実行させる。

このＳＴＥＰ７の判断結果が肯定的である場合には、ＲＲＴ経路計画生成部２２は、ＳＴＥＰ２からの処理を繰り返す。

このようにして、図１０Ｃに示す如く、探索木（時刻情報を含む探索木）が作成されていくこととなる。そして、ＳＴＥＰ７の判断結果が否定的である場合には、ＲＲＴ経路計画生成部２２は、ＳＴＥＰ８において、図１０Ｃに示す如く、移動体１の現在位置（図示例では、起点ノードＮｄ(0)の位置）から到達目標地点まで連続的に結ぶ経路計画を、ＳＴＥＰ１〜ＳＴＥＰ７までの探索処理より生成してなる探索結果の経路計画ｔｒとして決定する。この探索結果の経路計画ｔｒは、現在位置から到達目標地点まで、探索木の枝を連接した形態の経路計画であり、最後に生成された新規ノードＮｄ_newと起点ノードＮｄ(0)とを含む。

これにより、移動体１の現在位置から到達目標地点まで、障害物Ａとの干渉を生じることなく該移動体１を移動させ得る１つの経路計画ｔｒがＲＲＴ法に基づき得られることとなる。なお、この場合、探索木の起点ノードＮｄ(0)の位置が、移動体１の現在位置と異なる場合には、現在位置から起点ノードＮｄ(0)の位置までの現在の経路計画が、上記経路計画ｔｒの一部として採用される。

次いで、ＳＴＥＰ９において、ＲＲＴ経路計画生成部２２は、ＳＴＥＰ８で決定した経路計画ｔｒをスムージングする。これにより、ＲＲＴ法の基づく１つのＲＲＴ経路計画Ｔｒ（最終的に選定するＲＲＴ経路計画の１つの候補）が生成されることとなる。

ここで、図１１には、ＳＴＥＰ８で決定される経路計画ｔrの一例としてのｔｒ(1)と、この経路計画ｔｒ(1)をＳＴＥＰ９でスムージングしてなるＲＲＴ経路計画Ｔｒ(1)とが例示されている。なお、同図において、参照符号Ａｂを付した斜線領域は、静止障害物Ａｂの存在領域を示し、参照符号Ａｃ(tx)（ｘ＝１又は２又は３）を付した網掛け領域は、時刻ｔxでの移動障害物Ａｃの存在領域を示している。

同図を参照して、ＳＴＥＰ９のスムージングでは、経路計画ｔｒ(1)の全てのノード（折れ点）のうちの一部のノードが抽出され、当該一部のノードを経由点として、現在位置から到達目標地点まで結ぶ経路計画がスムージング後のＲＲＴ経路計画Ｔｒ(1)として決定される。

具体的には、ＲＲＴ経路計画Ｔｒ(1)の隣り合うノードを結ぶ線分（直線経路）上で移動体１と障害物Ａとの干渉が発生しないという条件を満たしつつ、現在位置から到達目標地点までの移動体１の移動距離が最小になるように、ＲＲＴ経路計画Ｔｒ(1)の各ノードが、移動体１の現在位置側から順番に、元の経路計画ｔｒ(1)から抽出される。そして、この抽出したノードを現在位置側から順番に結ぶことで、スムージング後のＲＲＴ経路計画Ｔｒ(1)が決定される。

本実施形態では、ＲＲＴ経路計画生成部２２は、以上の如くスムージング後のＲＲＴ経路計画Ｔｒを生成する処理を、所定回数、繰り返すことで、最終的に選定するＲＲＴ経路計画の候補となる複数の（所定数の）ＲＲＴ経路計画Ｔｒを生成する。例えば、図１１に例示する如く、複数（図示例では３つ）のＲＲＴ経路計画Ｔｒ(1)，Ｔｒ(2)，Ｔｒ(3)が生成される。

そして、ＲＲＴ経路計画生成部２２は、各障害物Ａの周囲に仮想的に設定するポテンシャル場を用いて、上記複数のＲＲＴ経路計画から、移動体１と障害物Ａとの干渉を回避する上で好適な１つのＲＲＴ経路計画を選定して出力する（最終的に１つのＲＲＴ経路計画を確定する）。

ここで、各障害物Ａに対応するポテンシャル場は、該障害物Ａに対する接近度合を表すポテンシャル値の空間的な分布を示す場であり、該障害物Ａに近いほど、ポテンシャル値が大きくなるように定義される。かかるポテンシャル場は、例えば、図１２に示す如く定義される。

すなわち、各障害物Ａに対応するポテンシャル場は、該障害物Ａの存在領域において、ポテンシャル値が最大となり、且つ、該障害物Ａの存在領域の境界から所定距離までの範囲内にて、障害物Ａの存在領域から遠ざかるに伴って、ポテンシャル値が最大値からゼロまで減衰していくように設定される。そして、障害物Ａの存在領域の境界から所定距離までの範囲の外側の領域では、該障害物Ａに対応するポテンシャル値はゼロとされ、換言すれば、該障害物Ａのポテンシャル場が存在しないものとされる。

この場合、固定障害物Ａａ又は静止障害物Ａｂに対応するポテンシャル場は一定位置及び一定領域の場として設定されることとなる。一方、移動障害物Ａｃに対応するポテンシャル場は、移動障害物Ａｃの前記した予測経路上を、該移動障害物Ａｃの存在領域と一体に移動する場として設定される。

この場合、ＲＲＴ経路計画生成部２２は、前記干渉判断処理部２４の処理と同様に、移動障害物Ａｃの存在領域の大きさを、現在時刻での移動障害物Ａｃの移動速度の推定値に応じた減衰速度で、時間の経過に伴って減衰していくように決定する。このため、ＲＲＴ経路計画生成部２２は、移動障害物Ａｃのポテンシャル場の領域を、時間の経過に伴い縮小していくように設定する。そして、移動障害物Ａｃの存在領域の大きさがゼロまで減衰した後は、該移動障害物Ａｃのポテンシャル場は、存在しないものとされる。

さらに、本実施形態では、ＲＲＴ経路計画生成部２２は、移動体１が固定障害物Ａａ又は静止障害物Ａｂに近づき過ぎるのを極力防止するために、移動障害物Ａｃのポテンシャル場のポテンシャル値の最大値を、固定障害物Ａａ及び静止障害物Ａｂのそれぞれのポテンシャル場のポテンシャル値の最大値よりも、小さい値に設定する。

なお、図１２に示す例では、各障害物Ａの存在領域の外側では、ポテンシャル値は、該存在領域の境界からの距離に応じて、例えば二次関数の形態（放物線形態）で減衰していくように設定されている。ただし、その減衰形態としては、他の形態（例えば直線状の減衰形態、あるいは、指数関数状の減衰形態等）も採用し得る。

また、図１２では、各障害物Ａの存在領域が２次元領域である場合についてのポテンシャル場を示しているが、各障害物Ａの存在領域を、３次元領域として設定した場合についても、上記と同様に各障害物Ａのポテンシャル場を定義し得る。

ＲＲＴ経路計画生成部２２は、上記の如く定義される各障害物Ａのポテンシャル場を用いて、各ＲＲＴ経路計画Ｔｒ毎に、該ＲＲＴ経路計画Ｔｒ上でのポテンシャル評価値を算出する。そして、ＲＲＴ経路計画生成部２２は、各ＲＲＴ経路計画Ｔｒ毎のポテンシャル評価値を相互に比較することで、好適なＲＲＴ経路計画Ｔｒを選定する。

具体的には、上記ポテンシャル評価値としては、例えば、各ＲＲＴ経路計画Ｔｒの全区間でのポテンシャル値の総和（各ＲＲＴ経路計画Ｔｒの各位置でのポテンシャル値を全区間において積分してなる値）、あるいは、各ＲＲＴ経路計画Ｔｒのうち、いずれかの障害物Ａに最も接近する位置でのポテンシャル値、あるいは、各ＲＲＴ経路計画Ｔｒのうち、障害物Ａの存在領域からの距離が所定距離以下となる区間でのポテンシャル値の総和（該区間の各位置でのポテンシャル値を、該区間の全体において積分してなる値）、あるいは、これらのポテンシャル評価値のうちの２つ以上の評価値を線形結合してなる値等を採用し得る。

なお、各ＲＲＴ経路計画Ｔｒのうち、複数の障害物Ａのポテンシャル場に属する位置でのポテンシャル値は、例えば、各障害物Ａのポテンシャル場に基づくポテンシャル値を加え合わせた値として算出される。

この場合、ＲＲＴ経路計画生成部２２は、複数のＲＲＴ経路計画Ｔｒのうち、上記ポテンシャル評価値が最も小さいものとなるＲＲＴ経路計画Ｔｒを、好適なＲＲＴ経路計画として選定する。

例えば、図１１に示した状況で、３つのＲＲＴ経路計画Ｔｒ(1)，Ｔｒ(2)，Ｔｒ(3)のうち、Ｔｒ(1)が好適なＲＲＴ経路計画として選定される。

以上が、ＲＲＴ経路計画生成部２２の処理の詳細である。

（経路計画修正部２３の処理）
次に、経路計画修正部２３の処理を説明する。経路計画修正部２３は、前記初期経路計画又はＲＲＴ経路計画のうち、移動体１の移動のために実際に使用している経路計画（以降、修正対象経路計画ということがある）を、ポテンシャル法を用いて修正する。

ここで、移動障害物Ａｃの移動速度もしくは移動方向は、修正対象経路計画の生成後に変化し得る。このため、修正対象経路計画に従って移動する移動体１と、移動障害物Ａｃとの将来の干渉が新たに生じる場合がある。このような場合に、経路計画修正部２３は、ポテンシャル法を用いて修正対象経路計画の部分的な修正を行う。

具体的には、経路計画修正部２３は、ＲＲＴ経路計画生成部２２の処理に関して前記した各障害物Ａのポテンシャル場を、そこに存在する移動体１に対して、障害物Ａから遠ざける方向（ポテンシャル値が低減する方向）に斥力を作用させる場として使用する。この場合、該ポテンシャル場は、ポテンシャル値が高いほど、大きな斥力を発生する。

従って、各障害物Ａに対応して前記した如く定義されるポテンシャル場が発生する斥力は（図１２を参照）、該障害物Ａの存在領域で最大の斥力となり、該存在領域の境界から所定距離までの範囲にて、該存在領域から遠ざかるに伴い、斥力が最大値からゼロまで減衰する。

なお、移動障害物Ａｃのポテンシャル場は、前記した如く、該移動障害物Ａｃの存在領域の大きさが、予測経路上での時間の経過に伴いゼロまで減衰した後は消滅する（移動障害物Ａｃの周囲のポテンシャル値が定常的にゼロに維持される）。従って、移動障害物Ａｃの存在領域の大きさがゼロまで減衰した後は、該移動障害物Ａｃのポテンシャル場は斥力を発生しないものとなる。

そして、経路計画修正部２３は、上記ポテンシャル場に基づく斥力によって、修正対象経路計画の一部の区間のノード（折れ点）を弾性的に変位させるように、該修正対象経路計画を部分的に修正する。

さらに、経路計画修正部２３は、移動障害物Ａｃの移動速度もしくは移動方向の変化等に起因して、ポテンシャル場に基づく斥力が作用しなくなった場合には、修正対象経路計画の修正区間の隣り合うノード間に仮想的に発生させる内力によって、修正対象経路計画の修正区間を元の状態に徐々に戻すようにする。

かかる経路計画修正部２３の処理は、より詳しくは、所定の制御処理周期で、図１３のフローチャートに示す如く実行される。

ＳＴＥＰ１１において、経路計画修正部２３は、現在時刻以後の各障害物Ａのポテンシャル場を設定する。この場合、固定障害物Ａａ又は静止障害物Ａｂのポテンシャル場は、一定位置及び一定領域の場として設定される。また、移動障害物Ａｃのポテンシャル場は、予測経路上での移動障害物Ａｃの移動に伴って移動し、且つ、移動障害物Ａｃの存在領域の大きさの減衰に伴って、該ポテンシャル場の領域が縮小する（最終的に消滅する）ように設定される。

次いで、ＳＴＥＰ１２において、経路計画修正部２３は、移動体１の現在位置から到達目標地点までの修正対象経路計画上の各点（所定の刻み幅間隔で設定した点）と、障害物Ａとの距離を計算する。この場合、移動障害物Ａｃについては、修正対象経路計画上の各点と、該点に対応する時刻での前記予測経路上の点との距離が算出される。

次いで、ＳＴＥＰ１３において、経路計画修正部２３は、現在位置から到達目標地点までの修正対象経路計画の各ノードに対して、障害物Ａのポテンシャル場により作用する斥力を算出する。

より詳しくは、ＳＴＥＰ１１で各障害物Ａ毎に設定したポテンシャル場と、ＳＴＥＰ１２で算出した距離とに基づいて、現在位置から到達目標地点までの修正対象経路計画のうち、１つ又は複数の障害物Ａのポテンシャル場が作用する区間（以降、ポテンシャル場作用区間という）が特定される。そして、該ポテンシャル場作用区間に含まれる各ノード（修正対象経路計画の折れ点）と障害物Ａとの距離から、該障害物Ａのポテンシャル場によって該ノードに作用する斥力（ベクトル）が算出される。

この場合、ポテンシャル場作用区間のノードのうち、複数の障害物Ａのポテンシャル場が作用するノードについては、該複数の障害物Ａのそれぞれに対応する斥力が算出される。

例えば、図１４は、修正対象経路計画のノードＮｄ(1)に対して、固定障害物Ａａ（又は静止障害物Ａｂ）のポテンシャル場と、ノードＮｄ(1)に対応する時刻で該ノードＮｄ(1)に近接する移動障害物Ａｃ(1)のポテンシャル場とが作用している状態を例示している。この場合、固定障害物Ａａ（又は静止障害物Ａｂ）のポテンシャル場によってノードＮｄ(1)の位置に作用する斥力Ｆｅ(1)（ベクトル）と、移動障害物Ａｃ(1)のポテンシャル場によってノードＮｄ(1)の位置に作用する斥力Ｆｃ(1)（ベクトル）とが算出される。

なお、修正対象経路計画のうち、ポテンシャル場作用区間に属さない各ノードについては、該ノードに作用する斥力はゼロとされる。

補足すると、修正対象経路計画のポテンシャル場作用区間内に、ノードが設定されていない場合もある。この場合には、経路計画修正部２３は、修正対象経路計画を、ポテンシャル場作用区間内で分断して、その分断位置に補助的なノードを新たに追加する。そして、経路計画修正部２３は、当該補助的なノードに対して、ポテンシャル場による斥力を算出する。

次いで、ＳＴＥＰ１４において、経路計画修正部２３は、現在位置から到達目標地点までの修正対象経路計画の各ノード（ＳＴＥＰ１３で上記補助的なノードが追加されている場合には、該補助的なノードを含む）に、これと隣り合うノードとの間で作用する内力を設定する。

この内力は、当該ノードの位置が、当初の修正対象経路計画（初期経路計画生成部２１が生成した初期経路計画又はＲＲＴ経路計画生成部２２が生成したＲＲＴ経路計画）での位置から修正された状態で、当該ノードの位置を当初の位置に戻すように機能する力、あるいは、当該ノードと、その隣りのノードとの間との距離を当初の距離に戻すように機能する力となるように設定される。

例えば、図１４におけるノードＮｄ(1)の位置が、移動障害物Ａｃのポテンシャル場による斥力によって、当初の位置から固定障害物Ａａ（又は静止障害物Ａｂ）に近づく側に修正された位置である場合、ノードＮｄ(1)に、その両隣のノードとの間で作用する内力Ｆｉ(1)（ベクトル）が図示の如く設定される。

なお、当初の修正対象経路計画の位置と同じ位置のノードについては、それに作用する内力はゼロとされる。

また、各ノードに作用させる内力は、基本的には、各障害物Ａのポテンシャル場による斥力の最大値よりも十分に小さい大きさの力に設定される。

次いで、ＳＴＥＰ１５において、経路計画修正部２３は、現在位置から到達目標地点までの修正対象経路計画の各ノードの変位量（位置の修正量）を算出する。

この場合、経路計画修正部２３は、各ノードが、それに作用する斥力及び内力に対して、弾性的に変位するものとみなして、該ノードの変位量（ベクトル）を算出する。

具体的には、本実施形態では、固定障害物Ａａ又は静止障害物Ａｂのポテンシャル場による斥力Ｆｅと該斥力Ｆｅが作用するノードの変位量ｄＸｅとの間の関係、移動障害物Ａｃのポテンシャル場による斥力Ｆｃと該斥力Ｆｃが作用するノードの変位量ｄＸｃとの間の関係、並びに、ノードに作用する内力Ｆｉと該ノードの変位量ｄＸｉと間の関係が、それぞれ、次式（１ａ），（１ｂ），（１ｃ）により表される弾性モデルによりモデル化されている。

ｄＸｅ＝Ｆｅ／Ｋｅ ……（１ａ）
ｄＸｃ＝Ｆｃ／Ｋｃ ……（１ｂ）
ｄＸｉ＝Ｆｉ／Ｋｉ ……（１ｃ）

上記Ｋｅ、Ｋｃ、Ｋｉは、剛性度合の指標値としての所謂ばね定数に相当するものとしてあらかじめ定められた定数値である。なお、Ｋｅ、Ｋｃ、Ｋｉは、互いに同じ値でもよい。

そして、経路計画修正部２３は、各ノードに対して、ＳＴＥＰ１３，１４でそれぞれ算出した斥力及び内力から、次式（２）により該ノードの変位量ｄＸｎを算出する。

ｄＸｎ＝ΣＦｅ／Ｋｅ＋ΣＦｃ／Ｋｃ＋ΣＦｉ／Ｋｉ ……（２）

なお、ΣＦｅは、固定障害物Ａａ又は静止障害物Ａｂのポテンシャル場によってノードに作用する全ての斥力Ｆｅの合力（ベクトル）、ΣＦｃは、移動障害物Ａｃのポテンシャル場によってノードに作用する全ての斥力Ｆｃの合力（ベクトル）、ΣＦｉは、ノードに作用する全ての内力Ｆｉの合力（ベクトル）を意味する。

上記式（２）により、ポテンシャル場作用区間の各ノードが、斥力Ｆｅ，Ｆｃ及び内力Ｆｉに対して弾性的に変位するように、該ノードの変位量ｄＸｎ（ベクトル）が算出される。

なお、斥力Ｆｅ，Ｆｃ及び内力Ｆｉが作用しないノードの変位量ｄＸｎはゼロである。

次いで、ＳＴＥＰ１６において、ポテンシャル場作用区間の各ノードの位置を、現在の位置からＳＴＥＰ１５で算出した変位量ｄＸｎだけずらした位置に修正する。

経路計画修正部２３は、以上の処理を所定の制御処理周期で逐次実行する。

以上の処理により、現在位置から到達目標地点までの修正対象経路計画のうち、障害物Ａのポテンシャル場が作用するポテンシャル場作用区間の各ノードの位置、あるいは、内力が作用する各ノード（当初の位置からずれたノード）の位置が、所定の制御処理周期で逐次、修正対象区間の各ノードの位置が修正されていくこととなる。

この場合、移動障害物Ａｃのポテンシャル場による斥力Ｆｃによって位置が修正された各ノードについては、該移動障害物Ａｃの移動速度又は移動方向の変化等に起因して、該移動障害物Ａｃのポテンシャル場が作用しなくなっても（斥力Ｆｃがゼロになっても）、内力Ｆｉによって、当初の位置に徐々に戻されることとなる。従って、修正後の経路計画が当初の修正対象経路計画から大きく乖離しないように、該修正対象経路計画を修正することができる。

図１５は、かかる経路計画修正部２３の処理による修正対象経路計画の修正例を示している。同図において、斜線領域は、静止障害物Ａｂの存在領域を示し、Ａｃ(t1)、Ａｃ(t2)はそれぞれ、時刻ｔ1，ｔ2での移動障害物Ａｃの存在領域を示している。

この例では、静止障害物Ａｂのポテンシャル場は、修正対象経路計画に作用しないものの、時刻ｔ1の近辺で、ノードＮｄ(1)，Ｎｄ(3)の間の区間で移動障害物Ａｃのポテンシャル場が作用し、時刻ｔ2の近辺で、ノードＮｄ(4)，Ｎｄ(6)の間の区間で移動障害物Ａｃのポテンシャル場が作用する。

この場合、図１５に実線で示す当初の修正対象経路計画は、ノードＮｄ(1)，Ｎｄ(3)の間の区間と、ノードＮｄ(4)，Ｎｄ(6)の間の区間とで、例えば破線で示す如く修正される。なお、この例では、ノードＮｄ(1)，Ｎｄ(3)の間の区間のうちのノードＮｄ(2)だけを変位させても、ノードＮｄ(2)，Ｎｄ(3)の間で、時刻ｔ1の近辺の移動障害物Ａｃのポテンシャル場が作用することから、ノードＮｄ(2)，Ｎｄ(3)との間に補助的なノードＮｄ(23)が追加されている。同様に、ノードＮｄ(4)，Ｎｄ(6)の間の区間のうちのノードＮｄ(5)，Ｎｄ(6)の間に補助的なノードＮｄ(56)が追加されている。

補足すると、移動障害物Ａｃの移動速度又は移動方向の変化等に起因して、ノードＮｄ(1)，Ｎｄ(3)の間の区間、または、ノードＮｄ(4)，Ｎｄ(6)の間の区間で移動障害物Ａｃの該ポテンシャル場が作用しなくなった場合には、該区間の経路計画は、前記した内力によって、当初の修正対象経路計画の状態に戻される。

ところで、動作環境に移動体１の自己位置の推定用のマーカＭｋが設置されている場合には、移動体１と障害物Ａとの干渉を生じない限り、該移動体１をマーカＭｋに近づけるように移動体１の移動を行うことが好ましい。

そこで、本実施形態では、経路計画修正部２３は、図１４に例示する如く、マーカＭｋの周囲（マーカＭｋから所定距離の範囲内）に、マーカＭｋに近づける向きの引力Ｆｍ（ベクトル）を発生するポテンシャル場を設定する。この場合、該ポテンシャル場における引力Ｆｍの大きさは一定でもよいが、マーカＭｋからの距離に応じて変化してもよい。

そして、経路計画修正部２３は、修正対象経路計画のノードのうち、障害物Ａのポテンシャル場による斥力Ｆｅ，Ｆｃが作用せず、且つ、マーカＭｋのポテンシャル場による引力Ｆｍが作用するノード（例えば図１４に示すノードＮｄ(2)）に対して、マーカＭｋの引力Ｆｍ（図１４では、Ｆｍ(2)）を作用させる。

そして、経路計画修正部２３は、この引力Ｆｍに応じて決定した変位量ｄＸｍだけ、ノードの位置を修正する。この場合の変位量ｄＸｍは、例えば、前記式（１ａ）〜（１ｃ）と同様に、引力Ｆｍを所定値のばね定数Ｋｍにより除算してなる値として算出し得る。

なお、この場合、該ノードに引力Ｆｍに加えて、前記内力Ｆｉを作用させ、これらの引力Ｆｍ及び内力Ｆｉのそれぞれに応じた変位量を合成することで、該ノードの変位量ｄＸｍを決定するようにしてもよい。

このようにすることにより、修正対象経路計画のノードのうち、障害物Ａのポテンシャル場による斥力が作用しないノードのうち、マーカＭｋに比較的近いノードをマーカＭｋにさらに近づけることができる。そのため、該ノードの近辺に移動体１を移動させたときに、マーカＭｋを利用して移動体１の自己位置を推定することを適切に行うことができる。ひいては、移動体１の自己位置の推定精度を高めることができる。

以上が経路計画修正部２３の処理の詳細である。

（経路計画生成処理部１２の全体処理）
次に、経路計画生成処理部１２の全体処理を説明する。経路計画生成処理部１２は、前記初期経路計画生成部２１、ＲＲＴ経路計画生成部２２及び経路計画修正部２３のいずれの処理を実行するかを選択する処理を、所定の制御処理周期で逐次実行する。そして、選択した処理を実行することで、経路計画の生成又は修正を実行する。

この場合、初期経路計画生成部２１、ＲＲＴ経路計画生成部２２及び経路計画修正部２３のいずれの処理を実行するかの選択処理は、図１６のフローチャートに示す如く実行される。なお、以降の説明では、初期経路計画生成部２１の処理を実行するモードをＧＰＰモード（ＧＰＰ：Global Path Planning）、ＲＲＴ経路計画生成部２２の処理を実行するモードをＲＲＴモード、経路計画修正部２３の処理を実行するモードをＥＰＭモード（ＥＰＭ：Elastic Potential Method）という。

まず、ＳＴＥＰ２１において、経路計画生成処理部１２は、経路計画生成処理部１２は、移動体１の経路計画の生成の初期タイミングであるか否かを判断する。該初期タイミングは、例えば、移動体１の移動要求に応じた新たな移動を開始するタイミング又はその直前のタイミングである。

ＳＴＥＰ２１の判断結果が肯定的である場合には、経路計画生成処理部１２は、ＳＴＥＰ２２において、処理モードとして、ＧＰＰモードを選択する。このとき、経路計画生成処理部１２は、初期経路計画生成部２１の処理を前記した如く実行させる。これにより、移動体１の初期位置（スタート地点）から到達目標地点までの前記初期経路計画が生成される。

ＳＴＥＰ２１の判断結果が否定的である場合には、経路計画生成処理部１２は、現在の処理モード（前回の制御処理周期で選択された処理モード）がＧＰＰモードであるか否かをＳＴＥＰ２３で判断する。

このＳＴＥＰ２３の判断結果が肯定的である場合には、経路計画生成処理部１２は、さらに、前回の制御処理周期で初期経路計画生成部２１により既に生成された初期経路計画が、現在時刻以後の将来に、移動体１といずれかの障害物Ａとの干渉が生じる経路計画であるか否かをＳＴＥＰ２４で判断する。

この判断処理は、生成済の初期経路計画に対して、干渉判断処理部２４により前記した如く実行される。

ＳＴＥＰ２４の判断結果が肯定的である場合（初期経路計画上で、移動体１と障害物Ａとの干渉が生じることが予測された場合）には、経路計画生成処理部１２は、ＳＴＥＰ２５において、処理モードとして、ＲＲＴモードを選択する。このとき、経路計画生成処理部１２は、ＲＲＴ経路計画生成部２２の処理を前記した如く実行させる。これにより、移動体１の現在位置から到達目標位置まで前記ＲＲＴ経路計画が新たに生成される。

一方、ＳＴＥＰ２４の判断結果が否定的である場合（初期経路計画上で、移動体１と障害物Ａとの干渉が発生しないことが予測される場合）には、経路計画生成処理部１２は、ＳＴＥＰ２６において、処理モードとして、ＥＰＭモードを選択する。このとき、経路計画生成処理部１２は、初期経路計画を修正対象経路計画として、経路計画修正部２３の処理を前記した如く実行させる。これにより、移動体１と障害物Ａとの干渉を回避するように、初期経路計画が修正される。以降、処理モードがＥＰＭモードに維持される限り、経路計画修正部２３による初期経路計画の修正処理が継続的に実行される。

また、ＳＴＥＰ２３の判断結果が否定的である場合（現在の処理モードがＧＰＰモードでない場合）には、経路計画生成処理部１２は、さらに現在の処理モードがＲＲＴモードであるか否かをＳＴＥＰ２７で判断する。

このＳＴＥＰ２７の判断結果が肯定的である場合には、、経路計画生成処理部１２は、さらに、ＲＲＴ経路計画の生成が完了しているか否かをＳＴＥＰ２８で判断する。

ここで、ＲＲＴ経路計画生成部２２の処理では、探索木の新規ノードＮｄ_newをランダムに生成するため、ＲＲＴ経路計画の生成が完了する（最終的に複数のＲＲＴ経路計画から好適なＲＲＴ経路計画が選定される）までに時間を要する場合がある。

このような場合に、ＳＴＥＰ２８の判断結果が否定的になる。この場合には、経路計画生成処理部１２は、前記ＳＴＥＰ２５において、処理モードとして、引き続き、ＲＲＴモードを選択する。このとき、ＲＲＴ経路計画生成部２２の処理が継続し、あるいは、改めて実行される。

ＳＴＥＰ２８の判断結果が肯定的である場合（前回の制御処理周期でＲＲＴ経路計画の生成が完了している場合）には、経路計画生成処理部１２は、ＳＴＥＰ２９において、処理モードとして、ＥＰＭモードを選択する。このとき、経路計画生成処理部１２は、生成済みのＲＲＴ経路計画を修正対象経路計画として、経路計画修正部２３の処理を前記した如く実行させる。これにより、移動体１と障害物Ａとの干渉を回避するように、ＲＲＴ経路計画が修正される。以降、処理モードがＥＰＭモードに維持される限り、経路計画修正部２３によるＲＲＴ経路計画の修正処理が継続的に実行される。

前記ＳＴＥＰ２７の判断結果が否定的である場合は、現在の処理モードがＥＰＭモードとなっている場合である。

この場合には、経路計画生成処理部１２は、経路計画修正部２３の処理による修正後の経路計画が、現在時刻以後の将来に、移動体１といずれかの障害物Ａとの干渉が生じる経路計画であるか否かをＳＴＥＰ３０で判断する。

この判断処理は、修正後の経路計画に対して、干渉判断処理部２４により前記した如く実行される。

ここで、前回の制御処理周期での経路計画修正部２３の処理（ＥＰＭモードの処理）では、基本的には、移動体１と障害物Ａとの干渉が生じるのを回避するように経路計画の修正が行われるものの、移動障害物Ａｃの移動速度又は移動方向の変化によって、移動体１と移動障害物Ａｃとの干渉が生じることが予測されるようになったり、あるいは、移動障害物Ａｃの存在領域と他の障害物Ａ（Ａａ又はＡｂ又はＡｃ）の存在領域との間に修正後の経路計画が挟まれた状態となることによって、該移動障害物Ａｃ及び他の障害物Ａの両方と、移動体１との干渉が生じることが予測されるようになる場合がある。

また、修正対象経路計画が初期経路計画である場合、経路計画修正部２３の処理による該初期経路計画の修正量（ノードの位置の変位量）が不足して、修正後の経路計画が移動体１と障害物Ａとの干渉を生じるものとなっている場合もある。

このような場合に、ＳＴＥＰ３０の判断結果が肯定的になる。この場合には、経路計画生成処理部１２は、ＳＴＥＰ３１において、移動体１と障害物Ａとの干渉が生じるという判断結果の連続的な繰り返し回数を示す干渉判断カウンタＣｃの値を１だけ増加させる。

なお、干渉判断カウンタＣｃの値は、ＧＰＰモード又はＲＲＴモードからＥＰＭモードへの移行時にゼロに初期化される。

さらに、経路計画生成処理部１２は、干渉判断カウンタＣｃの値があらかじめ定められた所定値Ｎｃ以下であるか否かをＳＴＥＰ３２で判断する。

このＳＴＥＰ３２の判断結果が肯定的である場合（Ｃｃ≦Ｎｃである場合）には、経路計画生成処理部１２は、ＳＴＥＰ３３において、処理モードとして、引き続き、ＥＰＭモードを選択する。このとき、経路計画修正部２３による経路計画の修正処理が継続的に実行される。

一方、ＳＴＥＰ３０の判断結果が連続的に肯定的となる回数（＝Ｃｃ）が上記所定値Ｎｃを超えることによって、ＳＴＥＰ３２の判断結果が否定的になった場合（Ｃｃ＞Ｎｃである場合）には、経路計画修正部２３による処理では、移動体１と障害物Ａとの干渉を回避し得るように経路計画を修正することができない状況であるとみなし得る。

そこで、この場合には、経路計画生成処理部１２は、ＳＴＥＰ３４において、処理モードとして、ＲＲＴモードを選択する。このとき、ＲＲＴ経路計画生成部２２により、現在位置から到達目標地点までのＲＲＴ経路計画が新たに生成させる。この場合、新たなＲＲＴ経路計画の生成が完了すると、ＥＰＭモードでの修正後の経路計画は破棄される。

また、ＳＴＥＰ３０の判断結果が否定的である場合（経路計画修正部２３による修正後の経路計画が、移動体１と障害物Ａとの干渉を生じないと予測される経路計画である場合）には、経路計画生成処理部１２は、ＳＴＥＰ３５において、干渉判断カウンタＣｃの値をゼロにリセットした後、前記ＳＴＥＰ３３において、処理モードとして、引き続き、ＥＰＭモードを選択する。このとき、経路計画修正部２３による経路計画の修正処理が継続的に実行される。

以上の如く、経路計画生成処理部１２は、処理モードを逐次選択しつつ、選択した処理モードに対応する処理（初期経路計画生成部２１の処理、又はＲＲＴ経路計画生成部２２の処理、又は経路計画修正部２３の処理）を実行する。これにより、移動障害物Ａｃの移動状態を反映させながら、移動体１と障害物Ａとの干渉を極力回避し得るように、経路計画の生成もしくは修正が行われる。

この場合、経路計画生成処理部１２は、初期タイミングにて初期経路計画生成部２１によ生成した初期経路計画、あるいは、その後に（移動体１の移動途中で）、ＲＲＴ経路計画生成部２２により生成したＲＲＴ経路計画にて、移動体１と障害物Ａとの将来の干渉が生じないことが予測される状況（ＳＴＥＰ２４もしくは３０の判断結果が否定的となる状況、又は、ＳＴＥＰ２８の判断結果が肯定的となる状況）、あるいは、該干渉が生じると予測される状況から生じないと予測される状況に変化する可能性がある状況（ＳＴＥＰ３２の判断結果が肯定的となる状況）では、経路計画修正部２３により、初期経路計画又はＲＲＴ経路計画を修正する処理が逐次実行される。

これにより、障害物Ａのポテンシャル場による斥力を発生させるポテンシャル法の手法により、初期経路計画又はＲＲＴ経路計画が、移動体１と障害物Ａとの将来の干渉を生じることを極力回避し得るように修正される。この場合、特に、移動障害物Ａｃの移動速度の推定値に応じて該移動障害物Ａｃの予測経路が逐次更新されながら、最新の予測経路に対応して設定される移動障害物Ａｃのポテンシャル場を使用して、経路計画の修正が行われる。

従って、移動障害物Ａｃの実際の移動状態をリアルタイムに反映させながら、経路計画を動的に修正することができる。

また、経路計画生成処理部１２は、移動障害物Ａｃの移動状態の変化等に起因して、経路計画修正部２３の処理により、移動体１と障害物Ａとの干渉を回避し得る経路計画を生成することができなくなった場合（ＳＴＥＰ３２の判断結果が否定的となった場合）には、ＲＲＴ経路計画生成部２２により、ＲＲＴ経路計画が新たに生成される。

これにより、ポテンシャル法の手法では、移動体１と障害物Ａとの干渉を回避し得る経路計画を生成することができなくなっても、ＲＲＴ法の手法によって、移動体１と障害物Ａとの干渉を回避し得る経路計画を改めて生成することができる。

かかる経路計画生成処理部１２の処理によりる経路計画の生成及び修正の一例の形態を図１７〜図２１に例示する。これらの図１７〜図２１は、動作環境ＡＲの平面状の床面上（ＸＹ平面上）で、移動体１をスタート地点Ｐｓ（初期位置）から到達目標地点Ｐｇまで移動させる経路計画を例示している。なお、図１７〜図２１において、Ｂ１は移動体１の存在領域の通過領域、Ｂacは移動障害物Ａｃの存在領域（時間の経過に伴い大きさが減衰する存在領域）の通過領域を表している。また、参照符号１(ti)（「ti」は時刻を表す任意の変数）を付した領域は、時刻ｔiでの移動体１の存在領域を示し、参照符号Ａｃ(ti)を付した領域は、時刻ｔiでの移動障害物Ａｃの存在領域を表している。また、図１７〜図２１では、便宜上、移動体１と固定障害物Ａａ又は静止障害物Ａｂとの干渉が生じることがない経路計画を例示している。

図１７は、経路計画の生成を開始する初期タイミングにて、スタート地点Ｐｓから到達目標地点Ｐｇまで、初期経路計画生成部２１により生成された初期経路計画を例示している。この例では、初期経路計画は、スタート地点Ｐｓから到達目標地点Ｐｇまで、直線的に移動させるように生成されている。なお、同図において、時刻ｔ0は、移動体１の移動開始時刻、ｔeは、移動体１が到達目標地点Ｐｇに達する時刻である。

図１７に示す時点では、初期経路計画上の各時刻において、移動体１の存在領域と、移動障害物Ａｃの存在領域とが重ならない（該移動体１と移動障害物Ａｃとの干渉が発生しない）ように、初期経路計画が生成されている。

次に、図１８は、図１７よりも後の時刻ｔ1にて、経路計画修正部２３の処理により修正された経路計画を例示している。この場合、例えば、将来の時刻ｔ1＋ｄｔ1において、移動体１の存在領域１(ｔ1＋ｄｔ1)と移動障害物Ａｃの存在領域Ａｃ(ｔ1＋ｄｔ1)とが接近する（移動障害物Ａｃのポテンシャル場が移動体１に作用する）ことが予測されることから、時刻ｔ1＋ｄｔ1の近辺の区間Ｄ１において、図示の如く、経路計画が時刻ｔ1の直前の制御処理周期で決定された元の経路計画から修正されている。

なお、図示例では、元の経路計画は、図１７に示した初期経路計画が想定されているが、時刻ｔ1よりも前の時刻で、ＲＲＴ経路計画生成部２２が生成したＲＲＴ経路計画、あるいは、経路計画修正部２３による修正済の経路計画であってもよい。

次に、図１９は、図１８よりも後の時刻ｔ2にて、経路計画修正部２３の処理により修正された経路計画を例示している。この場合、例えば、将来の時刻ｔ2＋ｄｔ2において、移動体１の存在領域１(ｔ2＋ｄｔ2)と移動障害物Ａｃの存在領域Ａｃ(ｔ2＋ｄｔ2)とが接近する（移動障害物Ａｃのポテンシャル場が移動体１に作用する）ことが予測されることから、時刻ｔ2＋ｄｔ2の近辺の区間Ｄ２において、図示の如く、経路計画が時刻ｔ2の直前の制御処理周期で決定された元の経路計画から修正されている。

次に、図２０は、図１９よりも後の時刻ｔ3にて、経路計画修正部２３の処理により修正された経路計画を例示している。この場合、例えば、現在時刻ｔ3の直後に、移動体１と移動障害物Ａｃとが干渉する（それぞれの存在領域が重なり合う）ことが予測されることから、時刻ｔ3の直後の区間Ｄ３において、図示の如く、経路計画が時刻ｔ3の直前の制御処理周期で決定された元の経路計画から修正されている。

次に、図２１は、図２０よりも後の時刻ｔ4にて、経路計画修正部２３の処理により修正された経路計画を例示している。この場合、現在時刻ｔ4以後に、移動体１と移動障害物Ａｃとが十分に離れる（移動障害物Ａｃのポテンシャル場が移動体１に作用しない）ことが予測されることから、時刻ｔ4以後の経路計画が、時刻ｔ4の直前の制御処理周期で決定された元の経路計画に維持されている。

なお、例えば図１９の時刻ｔ2又は図２０の時刻ｔ3において、横方向（Ｙ軸方向）に張り出すように修正された経路計画上の位置が、固定障害物Ａａ又は静止障害物Ａｂに接近することよって、例えば、図２２Ａに例示する如く、当該修正された経路計画が、移動障害物Ａｃと固定障害物Ａａ又は静止障害物Ａｂとの両方の障害物の存在領域を通るように、該移動障害物Ａｃと固定障害物Ａａ又は静止障害物Ａｂとの間に挟まれてしまう場合があり得る。

このような場合には、経路計画修正部２３の処理では、移動体１と移動障害物Ａｃとの干渉と、移動体１と固定障害物Ａａ又は静止障害物Ａｂとの干渉とを回避し得るに経路計画を修正することができなくなる。

このため、図１６のフローチャートのＳＴＥＰ３２の判断結果が否定的となって、処理モードがＲＲＴモードになる。ひいては、ＲＲＴ経路計画生成部２２により、ＲＲＴ法により、現在位置から到達目標地点までのＲＲＴ経路計画が新たに生成されることとなる。このことは、経路計画が、例えば図２２Ｂに例示する如く、２つの移動障害物Ａｃ，Ａｃの間に挟まれたような場合でも同様である。

（移動指令部１３の処理）
次に、移動指令部１３の処理を説明する。移動指令部１３は、前記した経路計画生成処理部１２の処理により決定された（生成又は修正された）最新の経路計画に従って、移動体１を移動させるように、移動体１の動作制御装置（図示省略）に対して移動速度等の移動指令を出力する。

この場合、本実施形態では、最新の経路計画が、移動体１といずれかの障害物Ａとの干渉を生じることが干渉判断処理部２４により予測される経路計画であっても、移動体１の存在領域と障害物Ａの存在領域とが干渉する（重なり合う）ことが予測される時刻の直前の時刻までは、移動体１を経路計画に従って移動させるように、移動体１の移動指令を出力する。

さらに詳細には、本実施形態では、移動指令部１３は、移動体１の移動速度の指令値に応じて、移動体１を該指令値の移動速度から停止させるまでに必要な制動距離を、あらかじめ作成された演算式又はデータテーブルを用いて決定する。該制動距離は、移動体１の移動速度の指令値が大きいほど、大きくなるように決定される。なお、上記制動距離は、経路計画生成装置１０と移動体１の動作制御装置との通信遅れ等の影響分を含んでいてもよい。

そして、移動指令部１３は、図２３に例示する如く、経路計画上で、移動体１の現在位置から進行方向前方側に制動距離だけずらした点である停止予測点が、障害物Ａの存在領域（図示例では、移動障害物Ａｃの存在領域Ａ(t1)（網掛け領域））に進入することとなる時刻（図示例では、時刻ｔ1）にて、移動体１を停止させる指令を出力する。なお、参照符号１(t1)を付した領域は、時刻ｔ1での移動体１の存在領域を示している。

これにより、移動指令部１３は、移動体１の存在領域が、障害物Ａの存在領域に干渉する直前に移動体１が停止できるように、移動体１の動作制御装置に対して移動体１を停止させる指令を出力することとなる。

なお、その後、障害物Ａの存在領域から移動体１の存在領域と停止予測点とが逸脱した場合、例えば、図２３の二点鎖線の領域Ａ(t2)で示すように、移動障害物Ａｃの存在領域が移動した場合には、移動体１は、移動を再開することが可能となる。

以上説明した実施形態では、ＲＲＴ経路計画の生成処理は、初期経路計画上の移動体１といずれかの障害物Ａとの干渉が発生することが予測される場合（ＳＴＥＰ２４の判断結果が肯定的となる場合）と、経路計画修正部２３による処理では、移動体１と障害物Ａとの干渉を回避し得る経路計画を生成することができなくなった場合（ＳＴＥＰ３２の判断結果が否定的となった場合）との２つの場合に限定されている。

そして、上記の２つの場合以外では、初期経路計画が最初に生成される場合を除いて、初期経路計画又はＲＲＴ経路計画が、経路計画修正部２３によりポテンシャル法に基づく手法によって、移動障害物Ａｃの移動状態の変化等を反映させるように動的に修正される。

従って、ＲＲＴ経路計画を生成することを、必要最小限に留めて、経路計画が過剰に変更されるのを極力抑制することができる。

また、移動障害物Ａｃの存在領域が前記したように減衰するように設定されるので、移動体１と移動障害物Ａｃとが離れている状態で、移動体１と移動障害物Ａｃとの干渉が生じると干渉判断処理部２４が判断するのを極力防止できる。

さらに、移動障害物Ａｃの大きさの減衰に伴い、ポテンシャル場の領域も縮小されるので、経路計画修正部２３が必要以上に、経路計画の修正を行ってしまうことも防止できる。

この結果、移動体１の移動用に用いる経路計画が、比較的大きく変化してしまうようなことを極力抑制して、該経路計画の安定性を高めることができる。

（変形態様について）
本発明は、以上説明した実施形態に限定されるものではない。以下に、いくつかの変形位態様を説明する。

前記実施形態では、２次元領域での経路計画を生成する場合を主要例として説明した。ただし、移動体１及び各障害物Ａの存在領域として、３次元領域を使用することで、２次元領域での経路計画を生成することができることはもちろんである。

また、前記実施形態では、移動体１として、脚式移動ロボットを例示した。ただし、移動体１は、経路計画に従って移動させ得るものであれば、他の移動体であってもよい。例えば、移動体１は、車両、飛行物体、船舶、搬送台車、あるいは、産業用ロボット等の作業機械の可動アームの先端部等であってもよい。

また、ＲＲＴ経路計画生成部２２の処理では、ＲＲＴ経路計画の複数の候補を生成せずに、単一のＲＲＴ経路計画だけを生成して出力するようにしてもよい。

１…移動体、１０…経路計画生成装置、１１…位置情報取得部（障害物情報取得手段）、２１…初期経路計画生成部（初期経路計画生成手段）、２２…ＲＲＴ経路計画生成部（ＲＲＴ経路計画生成手段）、２３…経路計画修正部（経路計画修正手段）、２４…干渉判断処理部（干渉判断手段）、Ａ（Ａａ，Ａｂ，Ａｃ）…障害物。

Claims

移動体の経路計画を生成する装置であって、
前記移動体の動作環境に存在する各障害物の存在位置を示す情報を少なくとも含む障害物情報を取得する障害物情報取得手段と、
前記移動体の移動を開始する初期位置から到達目標地点までの経路計画である初期経路計画を生成する初期経路計画生成手段と、
前記初期経路計画と前記障害物情報とに基づいて、該初期経路計画上の前記移動体といずれかの障害物との干渉が将来に発生することが予測される場合に、前記移動体の現在位置から到達目標地点までの経路計画を、ＲＲＴ（Rapidly-exploring Random Tree）法に基づく処理により前記移動体と各障害物との将来の干渉を回避し得るように生成することを実行するＲＲＴ経路計画生成手段と、
各障害物の周囲の領域に仮想的な斥力を発生するポテンシャル場を、前記障害物情報を用いて設定し、生成済の経路計画が、１つ以上の障害物のポテンシャル場が作用することが予測される区間を含む場合に、ポテンシャル法に基づく処理により、該区間に前記ポテンシャル場から作用する斥力を低減するように前記生成済の経路計画を修正することを、前記初期経路計画と、前記ＲＲＴ経路計画生成手段が生成した経路計画であるＲＲＴ経路計画とのうちの少なくともＲＲＴ経路計画に対して逐次実行する経路計画修正手段とを備えることを特徴とする移動体の経路計画生成装置。
請求項１記載の移動体の経路計画生成装置において、
前記経路計画修正手段は、前記初期経路計画生成手段による前記初期経路計画の生成後、前記ＲＲＴ経路計画生成手段により前記ＲＲＴ経路計画が生成されるまでは、前記初期経路計画を、ポテンシャル法に基づく処理により修正することを逐次実行するように構成されていることを特徴とする移動体の経路計画生成装置。
請求項１又は２記載の移動体の経路計画生成装置において、
前記ＲＲＴ経路計画生成手段は、前記経路計画修正手段による修正後の経路計画と前記障害物情報とに基づいて、該修正後の経路計画上の前記移動体と障害物との干渉を回避することができないことが予測される場合に、前記ＲＲＴ経路計画を新たに生成する機能をさらに有するように構成され、
前記経路計画修正手段は、前記ＲＲＴ経路計画生成手段により前記ＲＲＴ経路計画が新たに生成された場合には、当該新たなＲＲＴ経路計画を、ポテンシャル法に基づく処理により修正することを逐次実行するように構成されていることを特徴とする移動体の経路計画生成装置。
請求項３記載の移動体の経路計画生成装置において、
前記経路計画修正手段は、前記生成済の経路計画をポテンシャル法に基づく処理により修正することを所定の制御処理周期で逐次実行するように構成されており、
前記修正後の経路計画上の前記移動体と障害物との干渉を回避することができないことが予測される場合というのは、前記移動体といずれかの障害物との干渉が将来に発生することが予測される前記修正後の経路計画が、前記経路計画修正手段の所定の複数回の制御処理周期で繰り返して生成された場合であることを特徴とする移動体の経路計画生成装置。
請求項１〜４のいずれか１項に記載の移動体の経路計画生成装置において、
前記経路計画修正手段は、前記生成済の経路計画の前記区間に設定した１つ以上のノードを、少なくとも前記障害物のポテンシャル場から作用する斥力に応じて決定した変位量だけ変位させることにより、前記区間における経路計画を修正するように構成されていることを特徴とする移動体の経路計画生成装置。
請求項５記載の移動体の経路計画生成装置において、
前記経路計画修正手段は、前記区間における経路計画の修正後に、該区間に前記ポテンシャル場が作用しなくなることが予測される場合に、該区間を修正前の状態に徐々に戻すように構成されていることを特徴とする移動体の経路計画生成装置。
請求項１〜６のいずれか１項に記載の移動体の経路計画生成装置において、
前記初期経路計画、前記ＲＲＴ経路計画、及び前記経路計画修正手段による修正後の経路計画のうちの任意の１つの経路計画である対象経路計画に対して、該対象経路計画上の前記移動体と、前記障害物のうちの移動障害物との干渉が生じるか否かを、前記対象経路計画上の前記移動体の存在領域と、前記障害物情報に基づき設定した各障害物の存在領域とが重なり合う状態があるか否かによって判断する干渉判断手段を備えており、
該干渉判断手段は、前記移動体と前記移動障害物との干渉が生じるか否かを判断するために用いる前記移動障害物の将来の存在領域を、該存在領域の大きさが時間の経過に伴いゼロまで減衰する領域となるように設定するように構成されていることを特徴とする移動体の経路計画生成装置。
請求項７記載の移動体の経路計画生成装置において、
前記経路計画修正手段は、前記移動障害物の将来のポテンシャル場の領域を、該移動障害物の存在領域の大きさの減衰に伴い縮小させるように設定するように構成されていることを特徴とする移動体の経路計画生成装置。