JP2014211759A - 経路修正装置 - Google Patents

Abstract

【課題】計算不能となることを防止しながら、移動体と障害物とが接触しない経路に修正する。【解決手段】 経路修正装置は、入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する。経路修正装置は、経路長に関する経路長評価値と、移動体と障害物の離反距離に関する離反距離評価値と、少なくとも経路長評価値と離反距離評価値とからトータル評価値を計算する。移動体と障害物とが干渉する計算途上の経路に対しては、第１経路長評価関数を用いて経路長評価値を算出すると共に、第１離反距離評価関数を用いて離反距離評価値を計算する。第１経路長評価関数は、経路長が予め設定された第１長さとなるように経路長評価値を算出し、第１離反距離評価関数は、移動体と障害物とが干渉するときに有限の離反距離評価値を算出する。【選択図】図１

Description

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

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

Practical Search Techniques in Path Planning for Autonomous Driving, Dmitri Dolgov, Sebastian Thrun, Ｍichael Montemerlo, James Diebel, Proceedings of the First International Symposium on Search Techniques in Artificial Intelligence and Robotics, 2008 車輪移動ロボットの形状と運動拘束を考慮した経路平滑化法，長坂直樹，津坂祐司，第１７回ロボティクスシンポジア予稿集，ｐｐ．１６９−１７４，２０１２

非特許文献１では、移動体と障害物との間の離反距離が短くなるほど、大きな評価値となる離反距離評価関数を採用している。具体的には、離反距離がゼロになると（すなわち、移動体と障害物が接触する場合）、評価値が無限大となる離反距離評価関数を用いる。このような離反距離評価関数を用いて評価値を最小化する経路を探索すれば、移動体と障害物が接触しない経路が探索される。しかしながら、このような離反距離評価関数を用いると、初期経路によっては移動体と障害物とが重なり合い、計算不能となる場合がある。

そこで、非特許文献２では、移動体と障害物との間の離反距離が短くなるほど、大きな評価値となるが、離反距離がゼロとなるときも評価値が有限値となる離反距離評価関数を用いる。このような離反距離評価関数を用いると、移動体と障害物とが重なり合っても評価値を算出することができ、計算不能となることはない。しかしながら、非特許文献２では、移動体の経路長が短くなるほど、小さな評価値となる経路長評価関数も採用している。このため、離反距離評価関数の評価値と経路長評価関数の評価値とを総合的に評価した時に、移動体と障害物とが接触する経路が好適な経路として算出されることがある。

本明細書では、与えられた初期経路を好適な経路に修正する際に、計算不能となることがなく、かつ、移動体と障害物とが接触しない経路に修正する技術を開示する。

本明細書に開示する経路修正装置は、経路データを入力して記憶する装置と、入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えている。探索装置は、経路長に関する経路長評価値を計算する処理と、移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、少なくとも経路長評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行する。探索装置は、移動体と障害物とが干渉する計算途上の経路に対して、第１経路長評価関数を用いて経路長評価値を算出すると共に、第１離反距離評価関数を用いて離反距離評価値を計算する。ここで、第１経路長評価関数は、経路長が予め設定された第１長さとなるように経路長評価値を算出する。第１離反距離評価関数は、移動体と障害物とが干渉するときに有限の離反距離評価値を算出する。

この経路修正装置では、入力して記憶した経路データで記述される経路よりも好ましい経路を記述する経路データに修正する。具体的には、少なくとも経路長に関する経路長評価値と、離反距離に関する離反距離評価値とを用いてトータル評価値を計算し、トータル評価値が極値を取る時の経路データを探索する。この際、移動体と障害物とが干渉する計算途上の経路に対しては、有限の離反距離評価値を算出すると共に、経路長が予め設定された第１長さとなるように経路長評価値を算出する。移動体と障害物とが干渉しても、離反距離評価値が有限であるため、計算不能となることが防止される。また、経路長が予め設定された第１長さとなるように経路長評価値を算出するため、経路長が短くなりすぎることが防止され、移動体と障害物とが干渉してしまう経路が算出されることを抑制することができる。

実施例の経路修正装置のシステム構成を示す図である。 経路記述パラメータ、経路方向、移動体方向、障害物、経路長に関する評価関数等の意味内容を示す図である。 経路方向と移動体方向の偏差に関する評価関数の意味内容を示す図である。 障害物までの離反距離に関する意味内容を示す図である。 移動体と障害物とが干渉しない場合の第２離反距離評価関数を示す図である。 移動体と障害物とが干渉する場合の第１離反距離評価関数を示す図である。 移動体と障害物とが干渉しない場合の第２経路長評価関数を示す図である。 移動体と障害物とが干渉する場合の第１経路長評価関数を示す図である。 経路修正装置によって初期経路を修正するときの処理手順を示すフローチャートである。 初期経路を例示する図である。 修正後の経路を例示する図である。

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

（特徴１） 上記の経路修正装置では、探索装置が、移動体と障害物とが干渉しない計算途上の経路に対して、第１経路長評価関数又は第２経路長評価関数を用いて経路長評価値を算出すると共に、第２離反距離評価関数を用いて離反距離評価値を計算してもよい。この場合に、第２経路長評価関数は、経路長が最短となるように経路長評価値を算出してもよく、第２離反距離評価関数は、移動体と障害物とが干渉するときに無限の離反距離評価値を算出してもよい。このような構成によると、計算途上の経路（初期経路や最終経路となるまでに算出される経路）が移動体と障害物との干渉の有無によって、評価値を算出するための評価関数を変更する。これによって、より好適な経路に修正することができる。

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

（特徴３） 上記の経路修正装置では、経路データは、経路を複数の離散した経由点で記述するデータを含んでいてもよい。この場合に、第１経路長評価関数は、隣接する経由点の間隔が予め設定された第２長さとなるように経路長評価値を算出してもよい。このような構成によると、移動体と障害物が干渉しないようにしながら、経路を短くすることができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

（経路長評価関数）
出発点から目標点に至る経路の全長は短いことが好ましい。その一方、出発点から目標点に至る経路の全長を短くしすぎると、移動体と障害物とが干渉する経路となる場合がある。本実施例では、移動体と障害物との干渉判定に応じて、２種類の経路長評価関数を利用する。すなわち、修正途上の経路において、移動体と障害物とが干渉すると判定される場合は、全長が長い経路に修正される第１経路長評価関数を利用する。一方、修正途上の経路において、移動体と障害物が干渉しないと判定される場合は、全長が短い経路に修正される第２経路長評価関数を利用する。なお、移動体と障害物との干渉判定については後述する。

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

第１経路長評価関数を図８に示す。係数Ｋａと距離ｌiとは、第２経路長評価関数と同一である。距離ｌ_０は予め設定された距離である。図８から明らかなように、第１経路長評価関数は、距離ｌi（座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離）が距離ｌ_０となるように、経路を修正する。すなわち、距離ｌiが長くなるように経路が修正される。なお、距離ｌ_０は、移動体と障害物とが干渉しない程度に短い距離に設定される。例えば、距離ｌ_０は、初期経路長を（Ｎ−１）で割った値とすることができる。この場合、第１経路長評価関数は、初期経路長に近づくように経路を修正することとなる。

経路修正装置は、ｉ＝１からＮ−１までの各距離ｌiについて評価値を算出し、それら評価値の総和を取れば、経路全長に関する評価値が求められる。具体的には、各距離ｌiについて移動体と障害物との干渉を判定し、移動体と障害物とが干渉しない場合は第２経路長評価関数によって評価値を算出し、移動体と障害物とが干渉する場合は第１経路長評価関数によって評価値を算出する。そして、各距離ｌiについて算出した評価値の総和を取ることで、経路全長に関する評価値が得られる。これによって、移動体と障害物とが干渉しない程度に、全長が短い経路に修正される。もっとも他の評価関数をも利用するために、全長のみを考慮して修正されるものでないが、全長をも考慮した修正計算が実行されるということはできる。

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

移動体と障害物が干渉するか否かは、最短距離（離反距離）ｌｅi,jが０以下となるか否かで判定する。すなわち、移動体と障害物が接近して重なりあう場合（すなわち、干渉する場合）、ＧＪＫアルゴリズムでは正の値を持つｌｅi,jが計算される。この場合のｌｅi,jは、移動体と障害物が重なりあっている距離であり、両者を離反させるのに必要な移動距離に等しい。本実施例では、移動体と障害物が接近して重なりあう場合には、ｌｅi,jが負の値を持つ距離だとして扱う。したがって、最短距離（離反距離）ｌｅi,jが０以下となると、移動体と障害物とが干渉していると判断する。なお、移動体と障害物とが干渉するか否かは、経路上の各経由点Ｐｉ（i＝２，・・，Ｎ−1）において行われる。なお、上述した各距離ｌi（座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの間）について、移動体と障害物とが干渉するか否かは、経由点（ｘi，ｙi）と経由点（ｘi+1，ｙi+1）において最短距離（離反距離）ｌｅi,jが０以下となるか否かで判定することができる。なお、最短距離（離反距離）ｌｅi,jはＭ個の障害物のそれぞれについて算出される。

（離反距離評価関数）
上述した離反距離ｌｅi,jは、移動体と障害物とが干渉する確率を評価するための離反距離評価関数としても利用される。すなわち、離反距離ｌｅi,jが大きいほど移動体と障害物とが干渉する確率は低くなり、離反距離ｌｅi,jが小さいほど移動体と障害物とが干渉する確率は高くなる。したがって、離反距離を評価する離反距離評価関数によって、移動体と障害物とが干渉する確率を評価することができる。本実施例では、移動体と障害物との干渉判定に応じて、２種類の離反距離評価関数を利用する。すなわち、修正途上の経路において、移動体と障害物とが干渉すると判定される場合は、移動体と障害物が干渉するときに評価値が有限となる第１離反距離評価関数を利用する。一方、修正途上の経路において、移動体と障害物が干渉しないと判定される場合は、移動体と障害物が干渉するときに評価値が無限大となる第２離反距離評価関数を利用する。

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

図５は、第２離反距離評価関数Ｕｅi,jを示している。第１離反距離評価関数と同様に、離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以上であれば、離反距離評価値はゼロである。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以下となると、離反距離評価値は増加し始める。移動体と障害物が接近して離反距離ｌｅi,jがゼロに近づくほど、離反距離評価値は大きくなる。そして、移動体と障害物が接近して離反距離ｌｅi,jがゼロとなると、離反距離評価値は無限大に発散する。

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

また、障害物はＭ個存在するため、移動体が経由点Ｐiにあるときの離反距離評価値は、各障害物に対する離反距離評価値の和として求められる。なお、一部の障害物に対して干渉し、他の障害物に対しては干渉しない場合は、一部の障害物については第１離反距離評価関数によって評価値を算出し、他の障害物に対しては第２離反距離評価関数によって評価値を算出する。そして、各経由点Ｐiにおける離反距離評価値の和を取ることで、経路全長に関する評価値が得られる。

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

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

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

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

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

図３の(ａ)と（ｂ）とを比較すると明らかなように、経由点Ｐｉで第１評価関数と第２評価関数を計算することになる。その際には、共通して移動体方向θｉを利用する。第１評価関数は、移動体方向θｉと経路方向δ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に対応する第１偏差評価関数を用いて評価値を計算するようにすると、急激に方向を変える経路から緩やかに方向を変える経路に修正される。経路の平滑化処理が実施される。

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

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

（経路長評価関数の偏微分関数）
経路長評価関数Ｕａの偏微分関数を求めると下記となる。上述したように、経路長評価関数は、移動体が障害物に干渉するか否かで２種類の経路長評価関数を用いる。具体的には、移動体が障害物に干渉するときの第１経路長評価関数の偏微分関数は式４となり、移動体が障害物に干渉しないときの第２経路長評価関数の偏微分関数は式５となる。

（離反距離評価関数の偏微分関数）
離反距離評価関数Ｕｅの偏微分関数を求めると下記となる。離反距離評価関数は、移動体が障害物に干渉するか否かで２種類の離反距離評価関数を用いる。このため、移動体が障害物に干渉するときの第１離反距離評価関数の偏微分関数は式６となり、移動体が障害物に干渉しないときの第２離反距離評価関数の偏微分関数は式７となる。

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

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

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

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

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

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

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

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

なお、上記の修正計算を実行した結果、障害物に近い範囲で経由点が疎に分布し、障害物に遠い範囲で経由点が密に分布する経路記述データに修正されることがある。経由点が疎に分布すると、移動体と障害物の干渉の有無を判定する処理の精度が低下するので好ましくない。経由点が密に分布すると、曲率半径が小さな急カーブに修正される可能性があり、それも好ましくない。

それらを避けるためには、全部の経由点が等間隔で配置されているという条件を課して修正処理を進めてもよい。下記式を加えて修正処理を進めれば、全部の経由点が等間隔で配置されている経路記述データに修正される

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

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

また、式１２〜１４のうちの２種または３種を用いて経路修正処理を実施してもよい。

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

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

上述した経路修正装置によって初期経路を修正した例を説明する。図１０は、初期経路の一例を示している。初期経路によると、移動体と２つの障害物とが重なりあっている。図１１は、経路修正装置から出力された経路を示している。出力された経路では、移動体は、障害物を避けながら、目標点に目標姿勢で到達することができる。

上述したように、本実施例の経路修正装置では、移動体と障害物が干渉するときは第１経路長評価関数及び第１離反距離評価関数を用いて評価値を算出し、移動体と障害物が干渉しないときは第２経路長評価関数及び第２離反距離評価関数を用いて評価値を算出する。このため、計算不能となることを防止しながら、移動体と障害物とが干渉してしまう経路が算出されることを抑制することができる。

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

例えば、上述した実施例では、移動体と障害物とが干渉するか否かによって経路長評価関数を変えたが、移動体と障害物とが干渉するか否かに係らず同一の経路長評価関数を用いて評価値を算出してもよい。例えば、上述した実施例で用いた第１経路長評価関数のみを用いて評価値を算出してもよい。すなわち、移動体と障害物が干渉しない場合にも、第１経路長評価関数を用いて評価値を算出してもよい。このような構成としても、距離ｌ_０を適切に設定することで、移動体と障害物とが干渉しない程度に経路長を短くすることができる。

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

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

Claims (4)

1. 経路データを入力して記憶する装置と、
   入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えており、
   探索装置は、
   経路長に関する経路長評価値を計算する処理と、
   移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
   少なくとも経路長評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行し、
   移動体と障害物とが干渉する計算途上の経路に対して、第１経路長評価関数を用いて経路長評価値を算出すると共に、第１離反距離評価関数を用いて離反距離評価値を計算し、
   第１経路長評価関数は、経路長が予め設定された第１長さとなるように経路長評価値を算出し、
   第１離反距離評価関数は、移動体と障害物とが干渉するときに有限の離反距離評価値を算出することを特徴とする経路データの修正装置。
2. 探索装置は、移動体と障害物とが干渉しない計算途上の経路に対して、第１経路長評価関数又は第２経路長評価関数を用いて経路長評価値を算出すると共に、第２離反距離評価関数を用いて離反距離評価値を計算し、
   第２経路長評価関数は、経路長が最短となるように経路長評価値を算出し、
   第２離反距離評価関数は、移動体と障害物とが干渉するときに無限の離反距離評価値を算出することを特徴とする請求項１に記載の経路データの修正装置。
3. 経路データは、移動体の方向を記述するデータを含んでおり、
   探索装置が、さらに、経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理を実行し、
   トータル評価値を計算する処理では、経路長評価値と離反距離評価値と偏差評価値とからトータル評価値を計算する、請求項１又は２に記載の経路データの修正装置。
4. 経路データは、経路を複数の離散した経由点で記述するデータを含んでおり、
   第１経路長評価関数は、隣接する経由点の間隔が予め設定された第２長さとなるように経路長評価値を算出する、請求項１〜３のいずれか一項に記載の経路データの修正装置。

Images