特許 5986533 経路修正装置

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

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

(11)【特許番号】5986533

(24)【登録日】2016年8月12日

(45)【発行日】2016年9月6日

(54)【発明の名称】経路修正装置

(51)【国際特許分類】

   G05D 1/02 20060101AFI20160823BHJP

   G08G 1/16 20060101ALI20160823BHJP

   G01C 21/34 20060101ALN20160823BHJP

【ＦＩ】

   G05D1/02 H

   G08G1/16 C

   !G01C21/34

【請求項の数】3

【全頁数】19

(21)【出願番号】特願2013-87749(P2013-87749)

(22)【出願日】2013年4月18日

(65)【公開番号】特開2014-211760(P2014-211760A)

(43)【公開日】2014年11月13日

【審査請求日】2015年6月11日

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(73)【特許権者】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(74)【代理人】

【識別番号】110000110

【氏名又は名称】特許業務法人快友国際特許事務所

(72)【発明者】

【氏名】藤井 亮暢

(72)【発明者】

【氏名】高岡 豊

【審査官】 藤島 孝太郎

(56)【参考文献】

【文献】 特開２０１３−０１９６８３（ＪＰ，Ａ）

【文献】 特開２０１０−１２７６４６（ＪＰ，Ａ）

【文献】 特開２００４−３２５０８３（ＪＰ，Ａ）

【文献】 特開２００６−１９５９６９（ＪＰ，Ａ）

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

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

Ｇ０５Ｄ １／００− １／０２

Ｇ０１Ｃ ２１／００−２１／３６

２３／００−２５／００

Ｇ０８Ｇ １／００−９９／００

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

【請求項1】

経路データを入力して記憶する装置と、
入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えており、
経路データは、経路を複数の離散した経路点で記述するデータを含んでおり、
探索装置は、
隣接する経路点の経路点間隔に関する経路点間隔評価値を計算する処理と、
移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
少なくとも経路点間隔評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行し、
探索装置は、トータル評価値が小さくなるほど良い評価を与えるものであり、
探索装置は、経路点間隔が予め設定された設定範囲内から外れると、無限となる経路点間隔評価値を算出することを特徴とする経路データの修正装置。

【請求項2】

経路データを入力して記憶する装置と、
入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えており、
経路データは、経路を複数の離散した経路点で記述するデータを含んでおり、
探索装置は、
隣接する経路点の経路点間隔に関する経路点間隔評価値を計算する処理と、
移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
少なくとも経路点間隔評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行し、
探索装置は、トータル評価値が小さくなるほど良い評価を与えるものであり、
探索装置は、さらに計算途上の経路に対して、移動体と障害物との距離を算出し、
その算出した距離が予め設定された設定長さを越えるときは、経路点間隔が短くなるほど小さな評価値が計算される第１経路点間隔評価関数により経路点間隔評価値を算出し、
その算出した距離が予め設定された設定長さ以下となるときは、経路点間隔が予め設定された設定範囲内から外れると、無限となる評価値を算出する第２経路点間隔評価関数により経路点間隔評価値を算出することを特徴とする経路データの修正装置。

【請求項3】

経路データは、移動体の方向を記述するデータを含んでおり、
探索装置が、さらに、経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理を実行し、
トータル評価値を計算する処理では、経路点間隔評価値と離反距離評価値と偏差評価値とからトータル評価値を計算することを特徴とする請求項１又は２に記載の経路データの修正装置。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書では、移動体が移動するべき経路の初期値が与えられたときに、より好ましい経路に修正する技術を開示する。

【背景技術】

【0002】

移動体が移動するべき初期経路が与えられたときに、その初期経路をより好ましい経路に修正する技術が開発されている（例えば、非特許文献１）。非特許文献１には、経路を記述する経路データ（各種パラメータ）から、その経路の好ましさの程度を示す評価値を計算する評価関数を求め、その評価値を極小化する経路データ（各種パラメータ）を求める。これによって、初期経路がより好ましい経路に修正される。

【先行技術文献】

【非特許文献】

【0003】

【非特許文献1】車輪移動ロボットの形状と運動拘束を考慮した経路平滑化法，長坂直樹，津坂祐司，第１７回ロボティクスシンポジア予稿集，ｐｐ．１６９−１７４，２０１２

【発明の概要】

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

【0004】

非特許文献１では、経路を複数の離散した経路点で記述し、各経路点を修正してより好ましい経路データを計算する。この際、移動体と障害物との間の離反距離が大きくなるように経路を修正するため、障害物の近傍の経路点では、その経路点が障害物から離れるように修正される。このため、障害物の近傍では経路点が疎に分布し、それ以外の範囲では経路点が密に分布する経路データに修正されることがある。その結果、移動体と障害物との干渉を判定する精度が低下するという問題が生じる。

【0005】

本明細書では、経路を複数の離散した経路点で記述したときに、経路点間の距離が適切な距離とされた経路データに修正することができる技術を開示する。

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

【0006】

本明細書に開示する第１の経路修正装置は、経路データを入力して記憶する装置と、入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えている。経路データは、経路を複数の離散した経路点で記述するデータを含んでいる。探索装置は、隣接する経路点の経路点間隔に関する経路点間隔評価値を計算する処理と、移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、少なくとも経路点間隔評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行する。探索装置は、トータル評価値が小さくなるほど良い評価を与えるものである。探索装置は、経路点間隔が予め設定された設定範囲内から外れると、無限となる経路点間隔評価値を算出する。

【0007】

この経路修正装置では、隣接する経路点間の距離を評価する経路点間隔評価値を計算する。そして、経路点間隔が予め設定された設定範囲内から外れると、無限となる経路点間隔評価値を算出する。このため、経路点間隔が予め設定された設定範囲内となるように、経路データが修正される。その結果、経路点間の距離を適切な距離としながら、初期経路をより好適な経路データに修正することができる。

【0008】

本明細書が開示する第２の経路修正装置は、経路データを入力して記憶する装置と、入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えている。経路データは、経路を複数の離散した経路点で記述するデータを含んでいる。探索装置は、隣接する経路点の経路点間隔に関する経路点間隔評価値を計算する処理と、移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、少なくとも経路点間隔評価値と離反距離評価値とからトータル評価値を計算する処理を実行する。探索装置は、トータル評価値が小さくなるほど良い評価を与えるものである。探索装置は、さらに計算途上の経路に対して、移動体と障害物との距離を算出し、その算出した距離が予め設定された設定長さを越えるときは、経路点間隔が短くなるほど小さな評価値が計算される第１経路点間隔評価関数により経路点間隔評価値を算出し、その算出した距離が予め設定された設定長さ以下となるときは、経路点間隔が予め設定された設定範囲内から外れると、無限となる評価値を算出する第２経路点間隔評価関数により経路点間隔評価値を算出する。

【0009】

この経路修正装置でも、移動体と障害物との距離が近いときには、経路点間隔が予め設定された設定範囲内となるように経路データが修正される。したがって、移動体と障害物との距離が近いときに、経路点間の距離が短くなり過ぎることが防止できる。このため、経路点間の距離を適切な距離としながら、初期経路をより好適な経路データに修正することができる。

【図面の簡単な説明】

【0010】

【図1】実施例の経路修正装置のシステム構成を示す図である。

【図2】経路記述パラメータ、経路方向、移動体方向、障害物、経路点間隔に関する評価関数等の意味内容を示す図である。

【図3】経路方向と移動体方向の偏差に関する評価関数の意味内容を示す図である。

【図4】障害物までの離反距離に関する意味内容を示す図である。

【図5】移動体と障害物との離反距離に関する離反距離評価関数を示す図である。

【図6】移動体と障害物とが閾値以上離れているときの第１経路間隔評価関数を示す図である。

【図7】移動体と障害物とが閾値より接近しているときの第２経路間隔評価関数を示す図である。

【図8】経路修正装置によって初期経路を修正するときの処理手順を示すフローチャートである。

【図9】初期経路を例示する図である。

【図10】修正後の経路を例示する図である。

【図11】修正後の経路における経路点の間隔をグラフ化した図である。

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

【0011】

以下に説明する実施例の主要な特徴を列記しておく。なお、以下に記載する技術要素は、それぞれ独立した技術要素であって、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。

【0012】

（特徴１） 上記の経路修正装置では、経路データは、移動体の方向を記述するデータを含んでいてもよい。この場合に、探索装置は、さらに、経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理を実行してもよい。そして、トータル評価値を計算する処理では、経路点間隔評価値と離反距離評価値と偏差評価値とからトータル評価値を計算してもよい。このような構成によると、移動体の方向（向き）まで考慮して、より好適な経路に修正することができる。

【実施例】

【0013】

経路データの修正装置の実施例を説明する。最初に用語の意味を明らかにしておく。

【0014】

経路データとは、経路点（移動体の基準点が通過する点）のｘ座標と、経路点のｙ座標と、経路点における移動体方向（ｘ軸の正方向から移動体に固定されている基準線が指向している方向までの反時計回転方向の角θ）が、経路点の順序に従って並んでいるデータをいう。経路データは、式（１）で示され、Ｐｉは式（２）で示される。ｘi，ｙi，θiを、本明細書では、経路記述パラメータという。ｉ＝１は出発点であり、ｉ＝Ｎは目標点である。ｘ１、ｙ１，θ１，ｘＮ、ｙＮ，θＮは与えられている。

【数1】

【数2】

【0015】

初期経路とは、修正装置に入力される、修正処理前の経路データで記述される経路をいう。初期経路データは、ＲＲＴのような確率的手法で求めることもできるし、Ａ^＊探索のようなグリッド探索法で求めることができる。本実施例の修正装置は、初期経路データの生成プロセスに限定されないで適用することができる。初期経路は、粗い精度で探索された経路であり、移動体がその初期経路に沿って移動すると、移動体が障害物に接触することがある。あるいは、経路方向が急激に変化することがある。

【0016】

ここで、移動体は有限の大きさを備えている。障害物は有限の大きさを備えている。凸包とは、物体を包含する凸多角形であり、最短距離計算法とは、離れて存在する２個の凸包のそれぞれにおける最近接点を求め、その距離（凸包間の最短距離）を計算する手法である。最短距離計算法には、ＧＪＫアルゴリズムなどの既知の数学手法を利用することができる。

【0017】

また、ポテンシャル増大開始距離とは、離反距離がそれ以下となると、離反距離評価値を計算するポテンシャル関数がゼロでなくなる距離をいう。許容距離とは、移動体と障害物の間に確保する最小離反距離をいう。許容距離は、ポテンシャル増大開始距離に等しくしてもよいし、異なっていてもよい。また、許容偏差とは、先行経路点から後続経路点を指す方向と、ｘ軸の正方向がなす角をδとしたときに、先行経路点における移動体方向とδの偏差、ならびに、後続経路点における移動体方向とδの偏差に関して許容される最大偏差をいう。

【0018】

（修正装置の全体構成）
図１は、入力装置２から初期経路を入力し、その初期経路を好ましい経路に修正し、修正後経路を出力装置２２から出力する経路修正装置のシステム構成を示している。入力装置２から入力した初期経路は、修正途上の経路記述パラメータ記憶装置４に記憶される。後記するように、経路記述パラメータ記憶装置４の記憶内容は、計算の進行に伴って更新されていく。経路記述パラメータ記憶装置４は、修正途上の経路記述パラメータを記憶している装置ということができる。

【0019】

経路修正装置は、移動体と障害物との最短距離（離反距離）を判定する装置５と、経路点間隔に関する評価関数を複数記憶している装置６と、移動体と障害物との最短距離判定に基づいて経路点間隔評価関数記憶装置６から経路点間隔に関する評価関数を選択する装置７と、移動体から障害物までの離反距離に関する評価関数を記憶している装置８と、経路方向と移動体方向の偏差に関する評価関数を記憶している装置１０と、経路点間隔評価値と偏差評価値と離反距離評価値を合計したトータル評価値を計算するトータル評価関数記憶装置１２を備えている。経路記述パラメータ記憶装置４に記憶されている経路記述パラメータをトータル評価関数に代入することで、トータル評価値を計算することができる。図１は、システム構成を理解しやすくするために、トータル評価関数記憶装置１２とは別に、経路点間隔評価関数記憶装置６と経路点間隔評価関数選択装置７と離反距離評価関数記憶装置８と偏差評価関数記憶装置１０が図示されているが、これらの装置６，７，８，１０は、トータル評価関数記憶装置１２に内蔵されていてもよい。

【0020】

経路修正装置は、トータル評価関数を経路記述パラメータで偏微分した偏微分関数記憶装置１４を備えている。すなわち、トータル評価関数をｘiで偏微分した偏微分関数と、トータル評価関数をｙiで偏微分した偏微分関数と、トータル評価関数をθiで偏微分した偏微分関数を記憶している。ここで、i＝２，３，・・Ｎ―１である。ｉ＝１は出発点であり、ｉ＝Ｎは目標点であるから、ｘ１、ｙ１，θ１，ｘＮ、ｙＮ，θＮは固定されている。偏微分関数は、中間の経路点に関して偏微分する。経路記述パラメータ記憶装置４に記憶されている経路記述パラメータを、トータル評価関数をｘiで偏微分した偏微分関数と、トータル評価関数をｙiで偏微分した偏微分関数と、トータル評価関数をθiで偏微分した偏微分関数に代入することで、３種類の偏微分係数を計算することができる。図１では、微分関数が一つのブロック１４に図示されているが、ｘiの偏微分関数と、ｙiの偏微分関数と、θiの偏微分関数に分けて図示してもよい。

【0021】

経路記述パラメータを変数とするトータル評価関数が与えられ、経路記述パラメータを変数とする偏微分関数（トータル評価関数を経路記述パラメータで偏微分した関数）が与えられれば、既知の数学的手法で、トータル評価関数が極値をとる際の経路記述パラメータの値を決定することができる。本実施例では、好ましい経路ほど小さな評価値をとるトータル評価関数を採用する。経路修正装置は、トータル評価関数が最小値をとる経路記述パラメータの値を決定することによって、初期経路を最も好ましい経路に修正する。本実施例では、好ましい経路に修正する速度と安定性にすぐれている準ニュートン法を採用する。ただし、準ニュートン法に限られず、最急降下法、ニュートン法、共役勾配法などの勾配法を用いることもできる。

【0022】

多変数関数が最小値をとる条件を探査する勾配法では、偏微分関数を利用して、関数値を最小化させる変数の修正方向を決定する。多変数関数の場合には、各変数の修正量の比率を計算する。ここでは、それを探索方向ベクトルという。経路修正装置は、探索方向ベクトルを計算する装置１６を備えている。

【0023】

多変数関数が最小値をとる条件を探査する勾配法では、修正量の大きさが問題となる。修正量が過大であれば、修正方向は正しくても、最小化点を超えた範囲で関数値（トータル評価値）を計算することになり、トータル評価値が増大するという計算結果になってしまう。修正量が過小であれば、修正によって得られる関数値の減少量が微小となり、探索速度が遅くなる。本実施例の経路修正装置では、複数種類のステップ幅（修正量を決めるパラメータ）を順に用意しておき、関数値が減少する制約下で最大の修正量を採用するステップ幅設定装置１８を備えている。

【0024】

経路修正装置では、探索が進行して最小化位置に接近すると、探索方向ベクトル計算装置１６で計算された方向に、ステップ幅設定装置１８で設定された修正量だけ修正しても、評価値が変化しなくなる。最小化点では、偏微分係数がゼロであることに対応する。本実施例では、探索方向ベクトル計算装置１６で計算された方向にステップ幅設定装置１８で設定された修正量だけ修正することで関数値が変化（減少）するうちは、修正後の経路記述パラメータによって、経路記述パラメータ記憶装置４の記憶内容を更新する。更新した経路記述パラメータを用いて最小化条件探索処理を繰り返す。経路記述パラメータ記憶装置４の記憶内容は、計算途上の経路記述パラメータに更新されていく。探索方向ベクトル計算装置１６で計算された方向にステップ幅設定装置１８で設定された修正量だけ修正しても評価値が変化しなくなると、最小化点が検索されたとして処理を終了する。その段階の経路データ（経路記述パラメータ）が、出力装置２２から出力される。

【0025】

（経路データ）
図２において、参照番号２６は移動体を示し、出力装置２２から出力された修正後経路に沿って移動する。参照番号２４は移動体２６を包含する凸包であり、図１の修正装置では、凸包２４と障害物間の最短距離を求めて離反距離を計算する。参照番号２８は移動体２６に固定されている基準点であり、参照番号３０は移動体２６に固定されている基準線である。

【0026】

経路は、データＰｉ＝（ｘi，ｙi，θi）（ここでｉ＝１，２，・・Ｎ）で定義される。移動体２６は、基準点２８が（ｘi，ｙi）の座標点を辿るように移動する。また、移動体２６は、基準線３０がθiの方向を向く車体姿勢をとる。本明細書では、角度をｘ軸の正方向から反時計回転方向に計測した角度で示す。参照記号Ｏjは、移動体２６の走行を妨げる障害物である。本明細書では、出発点（ｘ1，ｙ1）から目標点（ｘN，ｙN）までの経路を決定する際に考慮する必要がある障害物がＭ個存在するものとする。

【0027】

（初期経路）
図１の経路修正装置に入力される初期経路は、ＲＲＴのような確率的手法あるいはＡ^＊探索のようなグリッド探索法で求めた大局的経路である。データＰｉ＝（ｘi，ｙi，θi）のＮ個の並びで記述されている。Ａ^＊探索のようなグリッド探索法で経路を探索すると、移動体方向θiは計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。

【0028】

初期経路のうち、出発点のデータＰ1＝（ｘ1，ｙ1，θ1）と目的点のデータＰN＝（ｘN，ｙN，θN）は、確定的なものであり、図１の修正装置で修正することはない。ただし、経路点のデータＰｉ（i＝２，・・，Ｎ−1）は最善なものでない可能性があり、図１の修正装置で修正する。初期経路には下記の問題が内在されていることがある。
（１）移動体と障害物が干渉する（重なりあう）経路である可能性がある。
（２）移動体と障害物が重なり合わないまでも非常に接近する経路である可能性がある。
（３）経路方向が急激に変化する経路である可能性がある。
（４）経路方向と移動体方向が不一致であって、移動体が追従できない経路である可能性がある。図１の修正装置は、上記の問題を解消する側に修正した経路を計算する。その修正計算のために、修正装置は下記の３種類の評価関数を利用する。

【0029】

（経路点間隔評価関数）
出発点から目標点に至る経路の全長（各経路点の間隔（先行経路点と後続経路点の距離）の総和）は短いことが好ましい。ただし、出発点から目標点に至る経路に、経路点の間隔が長い領域が含まれると、その領域において障害物との干渉判定の精度が低下する。その結果、障害物との干渉（衝突）が生じる経路が最終的な経路として出力される可能性が生じる。したがって、障害物との干渉が問題となる領域においては、経路点の間隔が長くなり過ぎることを抑制する必要がある。本実施例では、移動体と障害物との最短距離（離反距離）に応じて、２種類の経路点間隔評価関数を利用する。すなわち、修正途上の経路において、移動体と障害物との離反距離が設定値（正の値）以上であると判定される場合は、経路点間隔が短い経路に修正される第１経路点間隔評価関数を利用する。一方、修正途上の経路において、移動体と障害物との離反距離が設定値未満であると判定される場合は、経路点間隔が予め設定された範囲内となるように修正される第２経路点間隔評価関数を利用する。なお、移動体と障害物との距離判定については後述する。

【0030】

第１経路点間隔評価関数を図６に示す。係数Ｋａは、トータル評価値に占める経路点間隔評価値の重みを示し、距離ｌiは、座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離である。第１経路点間隔評価関数は全長（座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離）が短いほど小さな評価値が計算される。したがって、第1経路点評価関数は、経路点間隔を短くする方向に経路を修正する。

【0031】

第２経路点間隔評価関数を図７に示す。係数Ｋａと距離ｌiとは、第１経路点間隔評価関数と同一である。係数Ｋ_ｌは、第２経路点間隔評価関数の第２項の重みを示す係数であり、係数Ｋ_ｈは、第２経路点間隔評価関数の第３項の重みを示す係数である。距離ｌ_０，ｌ_ｈｉｇｈ（＞ｌ_０）は、予め設定された距離である。図７から明らかなように、第２経路点間隔評価関数は、３つの項（評価関数）の和となる。具体的には、第１項（１／２・Ｋａ（ｌi−ｌ_０）^２）は、経路点間隔（距離ｌi）が距離ｌ_０となるときに最小値（ゼロ）となる評価値を算出し、経路点間隔（距離ｌi）が距離ｌ_０より離れるほど大きな評価値を算出する。第２項（Ｋ_ｌ・ｌｏｇ_０．５ｌi）は、経路点間隔（距離ｌi）が距離ゼロとなると無限大となる評価値を算出し、経路点間隔（距離ｌi）が大きくなるほど小さな評価値を算出する。第３項（Ｋ_ｈ・ｌｏｇ_０．５｛−（ｌi−ｌ_ｈｉｇｈ）｝は、経路点間隔（距離ｌi）が距離ｌ_ｈｉｇｈとなると無限大となる評価値を算出し、経路点間隔（距離ｌi）が小さくなるほど小さな評価値を算出する。したがって、第２経路点間隔評価関数は、経路点間隔（距離ｌi（座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離））が距離ｌ_０となるときに最小値となり、経路点間隔がゼロ又は距離ｌ_ｈｉｇｈに近づくほど大きくな値となり、経路点間隔がゼロ又は距離ｌ_ｈｉｇｈのときに無限大となる。このため、第２経路点間隔評価関数は、経路点間隔が予め設定された範囲（０＜ｌ＜ｌ_ｈｉｇｈ）となるように経路を修正する。より詳細には、経路点間隔が予め設定された距離ｌ_０となるように経路を修正する。

【0032】

なお、距離ｌ_０は、移動体と障害物との干渉判定の精度が低下しないような距離に設定される。例えば、距離ｌ_０は、初期経路長を（Ｎ−１）で割った値とすることができる。この場合、第２経路点間隔評価関数は、初期経路長に近づくように経路を修正することとなる。また、第１経路点間隔評価関数と第２経路点間隔評価関数とを切換える設定値は、適宜設定することができる。例えば、設定値としては、上記の距離ｌ_０を用いてもよい。この場合、移動体と障害物との最短距離（離反距離）が距離ｌ_０以上となると、経路点間隔が短くなるように経路を修正し、一方、移動体と障害物との最短距離（離反距離）が距離ｌ_０未満となると、経路点間隔が距離ｌ_０となるように経路を修正する。これによって、移動体と障害物との最短距離（離反距離）が距離ｌ_０未満となる領域においては、経路点間隔が距離ｌ_０となるように経路が修正される。

【0033】

経路修正装置は、ｉ＝１からＮ−１までの各距離ｌiについて上記の経路点間隔評価関数を用いて評価値を算出し、それら評価値の総和を取れば、経路全長に関する評価値が求められる。具体的には、各距離ｌiについて移動体と障害物との最短距離が設定値（例えば、距離ｌ_０）以上となるか否かを判定し、移動体と障害物との最短距離が設定値以上となる場合は第１経路点間隔評価関数によって評価値を算出し、移動体と障害物との最短距離が設定値未満となる場合は第２経路点間隔評価関数によって評価値を算出する。そして、各距離ｌiについて算出した評価値の総和を取ることで、経路全長に関する評価値が得られる。これによって、経路点間隔が適切な範囲としながら、全長が短い経路に修正される。もっとも他の評価関数をも利用するために、経路点間隔のみを考慮して修正されるものでないが、経路点間隔をも考慮した修正計算が実行されるということはできる。

【0034】

（最短距離（離反距離））
ここで、移動体と障害物との最短距離（離反距離）について図４を参照して説明する。図４は、移動体が経路点Ｐiに移動した状態を示し、近傍にｊ番目の障害物が位置している状態を示している。移動体２６の凸包２４の存在範囲は、座標値（ｘi，ｙi）と移動体方向θiから計算することができる。障害物は、動かない凸包３２の存在範囲を記述するデータで記述されている。凸包２４と凸包３２の位置と姿勢が与えられれば、ＧＪＫアルゴリズムなどの既知の数学手法によって、凸包２４の輪郭上にあって凸包３２に最も近い点Ｒi,jと、凸包３２の輪郭上にあって凸包２４に最も近い点Ｏi,jを計算し、その距離ｌｅi,jを計算することができる。図示の場合は、最近接点が頂点である場合を例示しているが、辺上に最近接点がある場合にも対応することができる。図４に示す式には、近接点Ｏi,jの座標値と、近接点Ｒi,jの座標値と、最短距離（離反距離）ｌｅi,jの値を示している。したがって、最短距離（離反距離）ｌｅi,jが設定値（例えば、距離ｌ_０）以上となるか否かによって、選択する経路点間隔評価関数が変化することとなる。

【0035】

なお、移動体と障害物が接近して重なりあう場合（すなわち、干渉する場合）、ＧＪＫアルゴリズムでは正の値を持つｌｅi,jが計算される。この場合のｌｅi,jは、移動体と障害物が重なりあっている距離であり、両者を離反させるのに必要な移動距離に等しい。本実施例では、移動体と障害物が接近して重なりあう場合には、ｌｅi,jが負の値を持つ距離だとして扱う。したがって、最短距離（離反距離）ｌｅi,jが０以下となると、移動体と障害物とが干渉していると判断することができる。したがって、最短距離（離反距離）ｌｅi,jが０以下の場合、移動体と障害物との最短距離が設定値（例えば、距離ｌ_０）以下となり、第２経路間隔評価関数が選択される。その結果、経路が長くなるように修正され、移動体と障害物との干渉が解消される。

【0036】

なお、移動体と障害物との距離判定は、経路上の各経路点Ｐｉ（i＝２，・・，Ｎ−1）において行われる。また、経路上の各経路点Ｐｉ（i＝２，・・，Ｎ−1）において、Ｍ個の障害物のそれぞれについて最短距離（離反距離）ｌｅi,jが算出される。

【0037】

（離反距離評価関数）
上述した最短距離（離反距離）ｌｅi,jは、移動体と障害物とが干渉する確率を評価するための離反距離評価関数としても利用される。すなわち、離反距離ｌｅi,jが大きいほど移動体と障害物とが干渉する確率は低くなり、離反距離ｌｅi,jが小さいほど移動体と障害物とが干渉する確率は高くなる。したがって、離反距離を評価する離反距離評価関数によって、移動体と障害物とが干渉する確率を評価することができる。

【0038】

図５は、離反距離評価関数Ｕｅi,jを示している。係数Ｋｅは、離反距離評価関数の重み係数である。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以上であれば、離反距離評価値はゼロである。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以下となると、離反距離評価値は増加し始める。移動体と障害物が接近して離反距離ｌｅi,jがゼロに近づくほど、離反距離評価値は大きくなる。ただし、移動体と障害物が接近して離反距離ｌｅi,jがゼロとなっても、離反距離評価値は無限大に発散せず、有限の値が計算される関数形を採用している。

【0039】

なお、実際には、移動体と障害物が重なりあう現象は生じない。しかしながら、経路を修正する計算の途上では、移動体と障害物とが重なりあう経路に関する計算が必要とされることがある。例えば、移動体と障害物が重なりあう初期経路を、重なり合わない経路に修正する場合には、移動体と障害物が重なりあう経路に関する計算が必要とされる。本実施例では、移動体と障害物が重なりあう場合も有限の離反距離評価値が計算される離反距離評価関数を用いるので、修正計算の途上で計算不能に陥ることがない。また、障害物はＭ個存在するため、移動体が経路点Ｐiにあるときの離反距離評価値は、各障害物に対する離反距離評価値の和として求められる。

【0040】

（偏差評価関数）
本実施例では、移動体２６は、移動方向（経路方向）と独立して車体方向（姿勢方向、移動体方向）を変える機構を備えていない。このため、経路点Ｐi＝（ｘi，ｙi）を辿って移動する際の移動体方向θiが経路方向δiに一致することは保証されていない。その問題を解消するために、経路修正装置には経路方向δiと移動体方向θiを一致させるように修正する評価関数が導入されており、移動体がＰi（ｘi，ｙi）を辿って移動すると、移動体方向θiが経路方向δiによく一致する関係が得られるようにしている。移動方向から独立して車体方向を変える機構を備えていない移動体２６の経路を修正する際に、経路方向δiと独立して移動体方向θiを定義する経路記述データを採用しても、問題が生じないようにしている。

【0041】

図３の（ｂ）は、経路点Ｐiから経路点Ｐi+1に至る経路方向（δi）と、移動体方向の偏差を示している。実際には、経路点Ｐiにおける移動体方向θiと、経路点Ｐi+1における移動体方向θi+1は相違している。そこで、２種類の偏差評価関数を導入している。

【0042】

第１偏差評価関数は、図３の（３）に示すように、経路方向δiと移動体方向θiの偏差φiを自乗した値を計算する。係数Ｋｂは、トータル評価値に占める第１偏差評価値の重みを示している。図３の（４）に示すように、i＝１からＮ−１までの総和を取れば、第１偏差評価値を経路全長に亘って累積した第１偏差評価値Ｕｂを計算することができる。偏差φiが小さいほど評価値が小さくなる。評価値を最小化する方向に修正すると、偏差φiが小さくなる経路に修正される。

【0043】

第２偏差評価関数は、図３の（５）に示すように、経路方向δiと移動体方向θi+1の偏差ηiを自乗した値を計算する。係数Ｋｃは、トータル評価値に占める第２偏差評価値の重みを示している。図３の（６）に示すように、i＝１からＮ−１までの総和を取れば、第２偏差評価値を経路全長に亘って累積した第２偏差評価値Ｕｃを計算することができる。偏差ηiが小さいほど評価値が小さくなる。評価値を最小化する方向に修正すると、偏差ηiが小さくなる経路に修正される。

【0044】

移動体方向θを考慮すると、障害物までの離反距離を増大させられることがある。移動体方向θを加味して経路を修正すると、移動体方向θを振りながら移動することで移動体が障害物の間を縫って移動する経路に修正することができる。この場合には、移動体方向θによく一致する経路方向δとなる経路点位置に修正されることになる。

【0045】

図３の(ａ)と（ｂ）とを比較すると明らかなように、経路点Ｐｉで第１評価関数と第２評価関数を計算することになる。その際には、共通して移動体方向θｉを利用する。第１評価関数は、移動体方向θｉと経路方向δiの偏差φiが小さくなる方向に修正する。第２評価関数は、移動体方向θｉと経路方向δi-1の偏差ηi-1が小さくなる方向に修正する。共通の移動体方向θｉに対する偏差が小さくなる側に修正すると、経路方向δi-1と経路方向δiの差も小さくなっていく。経路方向δi-1と経路方向δiの差が大きいことは、経路が急激に方向を変えることを意味する。経路方向δi-1と経路方向δiの差を小さくすることは、緩やかに方向を変える経路に修正することに相当する。１つの経路点Ｐiに対して、その経路点Ｐiにおける移動体方向θiと直前の経路方向δi-1の偏差ηi-1に対応する第２偏差評価関数と、その経路点Ｐiにおける移動体方向θiと直後の経路方向δiの偏差φiに対応する第１偏差評価関数を用いて評価値を計算するようにすると、急激に方向を変える経路から緩やかに方向を変える経路に修正される。経路の平滑化処理が実施される。

【0046】

（トータル評価関数）
本実施例では、下記式で示されるトータル評価関数を利用する。
トータル評価関数
＝経路点間隔評価関数＋偏差評価関数＋離反距離評価関数
＝経路点間隔評価関数＋第１偏差評価関数＋第２偏差評価関数＋離反距離評価関数
すなわち、下記の式３を用いて、経路の修正処理を実行する。

【数3】

【0047】

本実施例では、トータル評価関数を経路記述パラメータｘi，ｙi，θiの各々で偏微分した偏微分関数を利用して、トータル評価値を最小化する経路記述パラメータｘi，ｙi，θiを探索する。トータル評価関数＝経路点間隔評価関数＋偏差評価関数＋離反距離評価関数であるため、トータル評価関数の偏微分関数＝経路点間隔評価関数の偏微分関数＋偏差評価関数の偏微分関数＋離反距離評価関数の偏微分関数の関係が成立する。

【0048】

（経路点間隔評価関数の偏微分関数）
経路点間隔評価関数Ｕａの偏微分関数を求めると下記となる。上述したように、経路点間隔評価関数は、移動体と障害物との最短距離が設定値以上となるか否かで２種類の経路点間隔評価関数を用いる。具体的には、移動体と障害物との最短距離が設定値以上となるときの第１経路点間隔評価関数の偏微分関数は式４となり、移動体と障害物との最短距離が設定値未満となるときの第２経路点間隔評価関数の偏微分関数は式５となる。

【数4】

【数5】

【0049】

（離反距離評価関数の偏微分関数）
離反距離評価関数Ｕｅの偏微分関数を求めると下記となる。

【数6】

【0050】

（偏差評価関数の偏微分関数）
図３の（４）（６）に示す第１偏差評価関数Ｕｂと第２偏差評価関数Ｕｃの偏微分関数から偏差評価関数Ｕｂ＋Ｕｃの偏微分関数を求めると下記となる。

【数7】

【0051】

（評価関数の最小化過程）
関数とその関数の偏微分関数が判明していれば、既知の数学的手法で、関数を最小化する変数の値を決定することができる。その問題は、最急降下法、ニュートン法、準ニュートン法、共役勾配法などの勾配法で解くことができる。本実施例では、計算の収束速度と安定性のバランスが優れている準ニュートン法を用いる。

【0052】

準ニュートン法などの勾配法では、計算途上の変数値（複数個の変数を扱うからベクトルとなる）における関数値から、関数値を減少させる変数の修正方向を特定する。偏微分関数から変数の変化に起因して関数値が変化する量を計算することができるため、関数値を減少させる変数の修正方向を特定することができる。次に、修正量を仮定し、変数群を修正方向へ修正量だけ修正した場合の関数値を計算する。関数値を減少させる修正方向を用いることから、関数値は減少するはずである。しかしながら、修正量が大きすぎれば、関数が最小値を取る位置を越えて修正することになり、関数値が増大してしまう。そこで、関数値が減少するという制約を満たす修正量の範囲内で修正する。勾配法では、
（１）現在の変数値の並び（ベクトル）よりも関数値が減少する修正方向を計算し、
（２）関数値が減少するという制約を満たす修正量で変数値の並びを修正方向に修正し、
（２）で修正された変数値の並びを（１）の変数値の並びとして(１)と（２）を繰り返す。
この結果、変数値の並びは、関数値を最小化するものに近づいていく。
関数値を最小化する変数値に近づくと、偏微分係数がゼロなって、変数値を修正方向に修正量だけ修正しても関数値は変化しなくなる。このことから、関数値を最小化する変数の並びに修正されたことがわかる。

【0053】

準ニュートン法では、修正方向ｄkが下記の式で計算される。なお添え字ｋは、前記（１）（２）の修正処理をすでにｋ-１回繰り返しており、これからｋ回目の修正処理を実施することを示している。

【数8】

上記式の各項目は下記に示されるものである。

【数9】

【数10】

【0054】

本実施例では、修正量を下記の表に従って変えていく。

【表1】

【0055】

本実施では、式８で決定された修正方向に、式８で決定された修正量に「1.0」を乗じた修正量だけ修正し、関数値を計算する。関数値が増加すれば、「1.0」の乗数では過大であるとし、乗数を「0.1」に変更して再度関数値を計算する。それでも関数値が増加すれば、乗数を「0.01」に変更する。それでも関数値が増加すれば、乗数を「0.001」に変更する。

【0056】

最小値に近づくにつれ、修正前のトータル評価関数の値と修正後のトータル評価関数の値の差の絶対値は減少していく。本実施例では、その差の絶対値が、閾値（例えば、１０^−６）を下回ったときに、トータル評価値を最小化する経路記述パラメータの並びに修正されたとし、修正処理を終了する。そうして修正された経路記述パラメータの並びを出力装置２２から出力する。

【0057】

なお、計算された移動体方向θと実際の移動体方向が一致するようにするためには、経路方向δと移動体方向θの偏差に関して許容偏差角を設定しておいてもよい。下記式を加えて修正処理を進めれば、経路方向δと移動体方向θの偏差が許容偏差角η0，φ0以下であることが保証されている経路記述データに修正される。経路方向δと移動体方向θの偏差が小さい経路が計算されていれば、計算された移動体方向θと実際の移動体方向はよく一致する。

【数11】

【0058】

移動体と障害物の離反距離に関して最短値を設定して修正作業を進めることもできる。下記式を加えて修正処理を進めれば、移動体と障害物の最短離反距離が許容距離minlen以上であることが保証されている経路記述データに修正される。許容距離minlenは、上述したポテンシャル増大開始距離ｌe0に等しくてもよいし、異なっていてもよい。

【数12】

【0059】

また、式１１，１２の両方を用いて経路修正処理を実施してもよい。

【0060】

次に、上述した経路修正装置で初期経路を最適な経路に修正する際に実行される処理について、図８のフローチャートに基づいて説明する。経路修正装置は、まず、初期経路（修正途上においては修正途上の経路）を入力する（Ｓ１０）。初期経路又は修正途上経路が入力されると、次に、経路点Ｐiに移動体を配置したときに、移動体と障害物との最短距離が設定値以上となるか否かを判定する（Ｓ１２）。具体的には、最短距離（離反距離）ｌｅi,jが設定値（距離ｌ_０）以上となるか否かで判定する。移動体と障害物との最短距離が設定値以上となる場合（Ｓ１２でＹＥＳ）は、第１経路点間隔評価関数を用いて評価値を算出する（Ｓ１６）。一方、移動体と障害物との最短距離が設定値未満の場合（Ｓ１２でＮＯ）は、第２経路点間隔評価関数を用いて評価値を算出する（Ｓ１４）。そして、移動体と障害物の離反距離に関する評価値を離反距離評価関数を用いて算出し（Ｓ１７）、経路方向と移動体の方向（車両方向）の偏差を評価するための評価値を偏差評価関数を用いて算出する（Ｓ１８）。

【0061】

Ｓ１４，１６，１８によって各評価値が算出されると、それらの評価値からトータル評価値を算出する（Ｓ２０）。次に、経路修正装置は、トータル評価値を最小化するために、経路点間隔評価関数と偏差評価関数と離反距離評価関数を偏微分する（Ｓ２２）。次に、上述した方法で探索方向ベクトルを計算し（Ｓ２４）、乗数（ステップ幅）を設定し（Ｓ２６）、計算した探索方向ベクトルと設定した乗数に基づいてトータル評価関数を最小化する（Ｓ２８）。次に、最小化されたトータル評価関数で算出されるトータル評価値の変化量（前回の処理で算出されたトータル評価値との差分）が閾値以下となるか否かを判定する（Ｓ３０）。トータル評価値の変化量が閾値以下でない場合（Ｓ３０でＮＯ）は、Ｓ１０に戻って、Ｓ１０からの処理を繰り返す。これによって、修正された経路に基づいて、Ｓ１０からの処理が繰り返される。一方、トータル評価値の変化量が閾値以下となる場合（Ｓ３０でＹＥＳ）は、初期経路が最適な経路に修正されたとして、その修正後の経路を出力して、処理を終了する（Ｓ３２）。

【0062】

上述した経路修正装置によって初期経路を修正した例を説明する。図９は、初期経路の一例を示している。図９に示す初期経路では、移動体は２つの障害物の間を通過し、その後、直角に進行方向を変えている。図１０は、経路修正装置から出力された経路を示している。出力された経路では、移動体は、障害物を通過した後、穏やかに進行方向を変え、目標点に目標姿勢で到達している。図１１は、修正後の経路（図１０）における、隣接する経路点間の距離（経路点Ｐ_ｉと経路点Ｐ_ｉ＋１の距離）を示すグラフである。グラフの縦軸は経路点間の距離（経路点間隔）であり、横軸が経路点間隔の番号ｉ（すなわち、経路点Ｐ_ｉと経路点Ｐ_ｉ＋１の距離）である。なお、第１経路点間隔評価関数のみを用いて経路を修正したときの比較例を併せて示す。また、計算条件は、ｌ_０＝０．０５，ｌ_ｈｉｇｈ＝０．１５とした。図１１から明らかなように、経路点間隔は、障害物近傍では設定値ｌ_０（０．０５）よりずれ、障害物から離れると設定値ｌ_０となった。ただし、本実施例では、最小の経路点間隔が０．０３５ｍとなり、最大経路点間隔が０．０６８ｍとなったのに対して、比較例では、最小の経路点間隔が０．０００３ｍとなり、最大経路点間隔が０．０８９ｍとなった。本実施例によって、経路点間隔が小さくなり過ぎることを防止できると共に、経路点間隔が大きくなり過ぎることを防止することができる。

【0063】

上述したように、本実施例の経路修正装置では、移動体と障害物との最短距離が設定値以上となるときは第１経路点間隔評価関数を用いて評価値を算出し、移動体と障害物との最短距離が設定値未満となるときは第２経路点間隔評価関数を用いて評価値を算出する。このため、経路点間隔が大きくなり過ぎることが防止され、移動体と障害物の干渉の有無を判定する処理の精度が低下することを抑制することができる。また、経路点間隔が小さくなり過ぎることが防止され、曲率半径の小さな急カーブに経路が修正されることを防止することができる。

【0064】

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

【0065】

例えば、上述した実施例では、移動体と障害物との最短距離に応じて２種類の経路点間隔評価関数を用いたが、移動体と障害物との最短距離に関係なく、第２経路点間隔評価関数のみを用いて評価値を算出してもよい。すなわち、移動体と障害物との最短距離が設定値以上となる場合にも、第２経路点間隔評価関数を用いて評価値を算出してもよい。このような構成としても、経路点間隔を適切な範囲とすることができる。

【0066】

また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

【符号の説明】

【0067】

２：初期経路入力装置
４：修正途上の経路記述パラメータの記憶装置
５：移動体と障害物との距離を判定する判定装置
６：経路点間隔に関する評価関数の記憶装置
８：障害物までの離反距離に関する評価関数の記憶装置
１０：経路方向と移動体方向の偏差に関する評価関数の記憶装置
１２：トータル評価関数の記憶装置
１４：経路記述パラメータについてのトータル評価関数の微分関数の記憶装置
１６：探索方向ベクトルの計算装置
１８：ステップ幅の設定装置
２０：トータル評価値の変化量の判定装置
２２：修正後経路出力装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】