JP2013134615A - 経路修正装置 - Google Patents
経路修正装置 Download PDFInfo
- Publication number
- JP2013134615A JP2013134615A JP2011284391A JP2011284391A JP2013134615A JP 2013134615 A JP2013134615 A JP 2013134615A JP 2011284391 A JP2011284391 A JP 2011284391A JP 2011284391 A JP2011284391 A JP 2011284391A JP 2013134615 A JP2013134615 A JP 2013134615A
- Authority
- JP
- Japan
- Prior art keywords
- route
- evaluation value
- description data
- moving body
- separation distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
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）（ここでｉ＝１，２，・・Ｎ）で定義される。移動体２６は、基準点２８が（ｘi，ｙi）の座標点を辿るように移動する。また、移動体２６は、基準線３０がθiの方向を向く車体姿勢をとる。本明細書では、角度をｘ軸の正方向から反時計回転方向に計測した角度で示す。
参照記号Ｏjは、移動体２６の走行を妨げる障害物である。本明細書では、出発点（ｘ1，ｙ1）から目標点（ｘN，ｙN）までの経路を決定する際に考慮する必要がある障害物がＭ個存在するものとする。
参照記号Ｏjは、移動体２６の走行を妨げる障害物である。本明細書では、出発点（ｘ1，ｙ1）から目標点（ｘN，ｙN）までの経路を決定する際に考慮する必要がある障害物がＭ個存在するものとする。
（初期経路）
図１の修正装置に入力される初期経路は、ＲＲＴのような確率的手法あるいはＡ＊探索のようなグリッド探索法で求めた大局的経路である。データＰｉ＝（ｘi，ｙi，θi）のＮ個の並びで記述されている。Ａ＊探索のようなグリッド探索法で経路を探索すると、移動体方向θiは計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。
初期経路のうち、出発点のデータＰ1＝（ｘ1，ｙ1，θ1）と目的点のデータＰN＝（ｘN，ｙN，θN）は、確定的なものであり、図１の修正装置で修正することはない。ただし、経由点のデータＰｉ（i＝２，・・，Ｎ−1）は最善なものでない可能性があり、図１の修正装置で修正する。初期経路には下記の問題が内在されていることがある。
（１）移動体と障害物が干渉する（重なりあう）経路である可能性がある。
（２）移動体と障害物が重なり合わないまでも非常に接近する経路である可能性がある。
（３）経路方向が急激に変化する経路である可能性がある。
（４）経路方向と移動体方向が不一致であって、移動体が追従できない経路情報である可能性がある。図１の修正装置は、上記の問題を解消する側に修正した経路を計算する。その修正計算のために、修正装置は下記の３種類の評価関数を利用する。
図１の修正装置に入力される初期経路は、ＲＲＴのような確率的手法あるいはＡ＊探索のようなグリッド探索法で求めた大局的経路である。データＰｉ＝（ｘi，ｙi，θi）のＮ個の並びで記述されている。Ａ＊探索のようなグリッド探索法で経路を探索すると、移動体方向θiは計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。
初期経路のうち、出発点のデータＰ1＝（ｘ1，ｙ1，θ1）と目的点のデータＰN＝（ｘN，ｙN，θN）は、確定的なものであり、図１の修正装置で修正することはない。ただし、経由点のデータＰｉ（i＝２，・・，Ｎ−1）は最善なものでない可能性があり、図１の修正装置で修正する。初期経路には下記の問題が内在されていることがある。
（１）移動体と障害物が干渉する（重なりあう）経路である可能性がある。
（２）移動体と障害物が重なり合わないまでも非常に接近する経路である可能性がある。
（３）経路方向が急激に変化する経路である可能性がある。
（４）経路方向と移動体方向が不一致であって、移動体が追従できない経路情報である可能性がある。図１の修正装置は、上記の問題を解消する側に修正した経路を計算する。その修正計算のために、修正装置は下記の３種類の評価関数を利用する。
（経路長評価関数）
出発点から目標点に至る経路の全長が短いことが好ましい。全長が短い経路に修正されるように、修正装置は経路長評価関数を利用する。経路長評価関数を、図２の（２）式に示す。係数Ｋａは、トータル評価値に占める経路長評価値の重みを示し、距離ｌiは、座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離である。ｉ＝１からＮ−１までの総和を取れば、経路全長に関する評価値が求められる。全長が短いほど小さな評価値が計算される。修正装置は、評価値を最小化する経路に修正する。全長が短いほど小さな評価値が計算される経路長評価関数を導入するために、修正装置によって全長が短い経路に修正される。
図２の（２）式に示す経路長評価関数を最小化すると、
（１）全長が短くなる経路に修正され、
（２）Ｐ2，・・，ＰN-1というＮ−２個の経由点を、等間隔に配置した経路記述データに修正される（全長が等しければ、経由点が等間隔である場合の評価値の方が、経由点が等間隔でない場合の評価値よりも小さくなる）。
もっとも他の評価関数をも利用するために、全長のみを考慮して修正されるものでないが、全長をも考慮した修正計算が実行されるということはできる。
出発点から目標点に至る経路の全長が短いことが好ましい。全長が短い経路に修正されるように、修正装置は経路長評価関数を利用する。経路長評価関数を、図２の（２）式に示す。係数Ｋａは、トータル評価値に占める経路長評価値の重みを示し、距離ｌiは、座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離である。ｉ＝１からＮ−１までの総和を取れば、経路全長に関する評価値が求められる。全長が短いほど小さな評価値が計算される。修正装置は、評価値を最小化する経路に修正する。全長が短いほど小さな評価値が計算される経路長評価関数を導入するために、修正装置によって全長が短い経路に修正される。
図２の（２）式に示す経路長評価関数を最小化すると、
（１）全長が短くなる経路に修正され、
（２）Ｐ2，・・，ＰN-1というＮ−２個の経由点を、等間隔に配置した経路記述データに修正される（全長が等しければ、経由点が等間隔である場合の評価値の方が、経由点が等間隔でない場合の評価値よりも小さくなる）。
もっとも他の評価関数をも利用するために、全長のみを考慮して修正されるものでないが、全長をも考慮した修正計算が実行されるということはできる。
（偏差評価関数）
移動体２６は、移動方向（経路方向）と独立して車体方向（姿勢方向、移動体方向）を変える機構を備えていないから、経由点Ｐi＝（ｘi，ｙi）を辿って移動する際の移動体方向θiが経路方向δiに一致することが保証されていない。その問題を解消するために、修正装置には経路方向δiと移動体方向θiを一致させるように修正する評価関数が導入されており、移動体がＰi＝（ｘi，ｙi）を辿って移動すると、移動体方向θiが経路方向δiによく一致する関係が得られるようにしている。移動方向から独立して車体方向を変える機構を備えていない移動体２６の経路を修正する際に、経路方向δ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を自乗した値を計算する。係数Ｋｂは、トータル評価値に占める第１偏差評価値の重みを示している。図３の（４）に示すように、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,jと、凸包３２の輪郭上にあって凸包２４に最も近い点Ｏi,jを計算し、その距離ｌｅi,jを計算することができる。図示の場合は、最近接点が頂点である場合を例示しているが、辺上に最近接点がある場合にも対応することができる。図４の（７）から（９）は、そうして求められる近接点Ｏi,jの座標値と、近接点Ｒi,jの座標値と、最短距離（離反距離）ｌｅi,jの値を示している。
図４は、移動体が経由点Ｐiに移動した状態を示し、近傍にｊ番目の障害物が位置している状態を示している。移動体２６の凸包２４の存在範囲は、座標値（ｘi，ｙi）と移動体方向θiから計算することができる。障害物は、動かない凸包３２の存在範囲を記述するデータで記述されている。凸包２４と凸包３２の位置と姿勢が与えられれば、ＧＪＫアルゴリズムなどの既知の数学手法によって、凸包２４の輪郭上にあって凸包３２に最も近い点Ｒi,jと、凸包３２の輪郭上にあって凸包２４に最も近い点Ｏ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は、移動体と障害物が重なりあっている距離であり、両者を離反させるのに必要な移動距離に等しい。本実施例では、移動体と障害物が接近して重なりあう場合には、ｌｅi,jが負の値を持つ距離だと扱う。
図５の（ｂ）は、離反距離評価関数を示している。上記の離反距離評価関数を用いると、移動体が障害物に接近し、接触し、深く重なりあうのに伴って、離反距離評価値が一様に増加する関係が得られる。係数Ｋｅは、離反距離評価関数の重み係数である。
実際には、移動体と障害物が重なりあう現象は生じない。しかしながら、修正計算の途上では、移動体と障害物が重なりあう経路に関する計算が必要とされることがある。例えば、移動体と障害物が重なりあう初期経路を、重なり合わない経路に修正する場合には、移動体と障害物が重なりあう経路に関する計算が必要とされる。本実施例では、移動体と障害物が重なりあう場合も有限の離反距離評価値が計算される離反距離評価関数を用いるので、修正計算の途上で計算不能に陥ることがない。
障害物はＭ個存在する。そこで、移動体が経由点Ｐiにあるときの離反距離評価値は、図５の（１０）式で求められる。また、経路の全長に亘って累積した離反距離評価値は、図５の（１１）式で求められる。出発点と目標点は、修正作業の対象でないので、i＝２，・・Ｎ−１まで加算する。
図５（ａ）は、離反距離評価関数Ｕｅi,jを示している。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以上であれば、離反距離評価値はゼロである。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以下になるほど接近すると、離反距離評価値は増加し始める。移動体と障害物が接近して離反距離ｌｅi,jがゼロに近づくほど、離反距離評価値は大きくなる。移動体と障害物が接近して離反距離ｌｅi,jがゼロとなっても、離反距離評価値は無限大に発散しない。有限の値が計算される関数形を採用している。
移動体と障害物が接近して重なりあう場合、ＧＪＫアルゴリズムでは正の値を持つｌｅi,jが計算される。この場合のｌｅi,jは、移動体と障害物が重なりあっている距離であり、両者を離反させるのに必要な移動距離に等しい。本実施例では、移動体と障害物が接近して重なりあう場合には、ｌｅi,jが負の値を持つ距離だと扱う。
図５の（ｂ）は、離反距離評価関数を示している。上記の離反距離評価関数を用いると、移動体が障害物に接近し、接触し、深く重なりあうのに伴って、離反距離評価値が一様に増加する関係が得られる。係数Ｋｅは、離反距離評価関数の重み係数である。
実際には、移動体と障害物が重なりあう現象は生じない。しかしながら、修正計算の途上では、移動体と障害物が重なりあう経路に関する計算が必要とされることがある。例えば、移動体と障害物が重なりあう初期経路を、重なり合わない経路に修正する場合には、移動体と障害物が重なりあう経路に関する計算が必要とされる。本実施例では、移動体と障害物が重なりあう場合も有限の離反距離評価値が計算される離反距離評価関数を用いるので、修正計算の途上で計算不能に陥ることがない。
障害物はＭ個存在する。そこで、移動体が経由点Ｐiにあるときの離反距離評価値は、図５の（１０）式で求められる。また、経路の全長に亘って累積した離反距離評価値は、図５の（１１）式で求められる。出発点と目標点は、修正作業の対象でないので、i＝２，・・Ｎ−１まで加算する。
（トータル評価関数）
本実施例では、下記式で示されるトータル評価関数を利用する。
トータル評価関数
＝経路長評価関数＋偏差評価関数＋ 離反距離評価関数
＝経路長評価関数＋第１偏差評価関数＋第２偏差評価関数＋離反距離評価関数
すなわち、下記の式３を用いて、経路の修正処理を実行する。
本実施例では、下記式で示されるトータル評価関数を利用する。
トータル評価関数
＝経路長評価関数＋偏差評価関数＋ 離反距離評価関数
＝経路長評価関数＋第１偏差評価関数＋第２偏差評価関数＋離反距離評価関数
すなわち、下記の式３を用いて、経路の修正処理を実行する。
本実施例では、トータル評価関数を経路記述パラメータｘi，ｙi，θiの各々で偏微分した偏微分関数を利用して、トータル評価値を最小化する経路記述パラメータｘi，ｙi，θiを探索する。
トータル評価関数＝経路長評価関数＋偏差評価関数＋離反距離評価関数であるために、
トータル評価関数の偏微分関数＝経路長評価関数の偏微分関数＋偏差評価関数の偏微分関数＋離反距離評価関数の偏微分関数の関係が成立する。
トータル評価関数＝経路長評価関数＋偏差評価関数＋離反距離評価関数であるために、
トータル評価関数の偏微分関数＝経路長評価関数の偏微分関数＋偏差評価関数の偏微分関数＋離反距離評価関数の偏微分関数の関係が成立する。
（経路長評価関数の偏微分関数）
最初に図２の（２）に示す経路長評価関数Ｕａの偏微分関数を求めると下記となる。
（偏差評価関数の偏微分関数）
図３の（４）（６）に示す第１偏差評価関数Ｕｂと第２偏差評価関数Ｕｃの偏微分関数から偏差評価関数Ｕｂ＋Ｕｃの偏微分関数を求めると下記となる。
最初に図２の（２）に示す経路長評価関数Ｕａの偏微分関数を求めると下記となる。
図３の（４）（６）に示す第１偏差評価関数Ｕｂと第２偏差評価関数Ｕｃの偏微分関数から偏差評価関数Ｕｂ＋Ｕｃの偏微分関数を求めると下記となる。
（離反距離評価関数の偏微分関数）
離反距離評価関数の偏微分関数の導出過程を、図６に示す。その結果、図５の（ｂ）に示す離反距離評価関数Ｕｅi,j、すなわち移動体が経由点Ｐiにあるときのｊ番目の障害物までの離反距離に対する評価関数の偏微分関数を求めると、下記となる。
上記において、ρxとρyは、移動体２４に固定された座標系での、移動体側の最近接点Ｒi,j（図４参照）の座標値である。
離反距離評価関数の偏微分関数の導出過程を、図６に示す。その結果、図５の（ｂ）に示す離反距離評価関数Ｕｅi,j、すなわち移動体が経由点Ｐiにあるときのｊ番目の障害物までの離反距離に対する評価関数の偏微分関数を求めると、下記となる。
（評価関数の最小化過程）
関数とその関数の偏微分関数が判明していれば、既知の数学的手法で、関数を最小化する変数の値を決定することができる。その問題は、最急降下法、ニュートン法、準ニュートン法、共役勾配法などの勾配法で解くことができる。本実施例では、計算の収束速度と安定性のバランスが優れている準ニュートン法を用いる。
準ニュートン法などの勾配法では、計算途上の変数値（複数個の変数を扱うからベクトルとなる）における関数値から、関数値を減少させる変数の修正方向を特定する。偏微分関数から変数の変化に起因して関数値が変化する量を計算することができるから、関数値を減少させる変数の修正方向を特定することができる。次に、修正量を仮定し、変数群を修正方向へ修正量だけ修正した場合の関数値を計算する。関数値を減少させる修正方向を用いることから、関数値は減少するはずである。しかしながら、修正量が大きすぎれば、関数が最小値を取る位置を越えて修正することになり、関数値が増大してしまう。そこで、関数値が減少するという制約を満たす修正量の範囲内で修正する。勾配法では、
（１）現在の変数値の並び（ベクトル）よりも関数値が減少する修正方向を計算し、
（２）関数値が減少するという制約を満たす修正量で変数値の並びを修正方向に修正し、
（２）で修正された変数値の並びを（１）の変数値の並びとして(１)と（２）を繰り返す。
この結果、変数値の並びは、関数値を最小化するものに近づいていく。
関数値を最小化する変数値に近づくと、偏微分係数がゼロなって、変数値を修正方向に修正量だけ修正しても関数値は変化しなくなる。このことから、関数値を最小化する変数の並びに修正されたことがわかる。
関数とその関数の偏微分関数が判明していれば、既知の数学的手法で、関数を最小化する変数の値を決定することができる。その問題は、最急降下法、ニュートン法、準ニュートン法、共役勾配法などの勾配法で解くことができる。本実施例では、計算の収束速度と安定性のバランスが優れている準ニュートン法を用いる。
準ニュートン法などの勾配法では、計算途上の変数値（複数個の変数を扱うからベクトルとなる）における関数値から、関数値を減少させる変数の修正方向を特定する。偏微分関数から変数の変化に起因して関数値が変化する量を計算することができるから、関数値を減少させる変数の修正方向を特定することができる。次に、修正量を仮定し、変数群を修正方向へ修正量だけ修正した場合の関数値を計算する。関数値を減少させる修正方向を用いることから、関数値は減少するはずである。しかしながら、修正量が大きすぎれば、関数が最小値を取る位置を越えて修正することになり、関数値が増大してしまう。そこで、関数値が減少するという制約を満たす修正量の範囲内で修正する。勾配法では、
（１）現在の変数値の並び（ベクトル）よりも関数値が減少する修正方向を計算し、
（２）関数値が減少するという制約を満たす修正量で変数値の並びを修正方向に修正し、
（２）で修正された変数値の並びを（１）の変数値の並びとして(１)と（２）を繰り返す。
この結果、変数値の並びは、関数値を最小化するものに近づいていく。
関数値を最小化する変数値に近づくと、偏微分係数がゼロなって、変数値を修正方向に修正量だけ修正しても関数値は変化しなくなる。このことから、関数値を最小化する変数の並びに修正されたことがわかる。
準ニュートン法では、修正方向ｄkが下記の式で計算される。なお添え字ｋは、前記（１）（２）の修正処理をすでにｋ-１回繰り返しており、これからｋ回目の修正処理を実施することを示している。
上記式の各項目は下記に示されるものである。
本実施例では、修正量を下記の表に従って変えていく。
本実施例では、数式７で決定された修正方向に、数式７で決定された修正量に「1.0」を乗じた修正量だけ修正し、関数値を計算する。関数値が増加すれば、「1.0」の乗数では過大であるとし、乗数を「0.1」に変更して再度関数値を計算する。それでも関数値が増加すれば、乗数を「0.01」に変更する。それでも関数値が増加すれば、乗数を「0.001」に変更する。
最小値に近づくにつれ、修正前のトータル評価関数の値と修正後のトータル評価関数の値の差の絶対値は減少していく。本実施例では、その差の絶対値が、閾値（本実施例では１０−６）を下回ったときに、トータル評価値を最小化する経路記述パラメータに並びに修正されたとし、修正処理を終了する。そうして修正された経路記述パラメータの並びを出力装置２２から出力する。
上記の修正計算を実行した結果、障害物に近い範囲で経由点が疎に分布し、障害物に遠い範囲で経由点が密に分布する経路記述データに修正されることがある。経由点が疎に分布すると、移動体と障害物の干渉の有無を判定する処理の精度が低下するので好ましくない。経由点が密に分布すると、曲率半径が小さな急カーブに修正される可能性があり、それも好ましくない。
それを避けるためには、全部の経由点が等間隔で配置されているという条件を課して修正処理を進めることが好ましい。下記式を加えて修正処理を進めれば、全部の経由点が等間隔で配置されている経路記述データに修正される
それを避けるためには、全部の経由点が等間隔で配置されているという条件を課して修正処理を進めることが好ましい。下記式を加えて修正処理を進めれば、全部の経由点が等間隔で配置されている経路記述データに修正される
また、計算された移動体方向θと実際の移動体方向が一致するようにするためには、経路方向δと移動体方向θの偏差に関して許容偏差角を設定しておいてもよい。下記式を加えて修正処理を進めれば、経路方向δと移動体方向θの偏差が許容偏差角η0，φ0以下であることが保証されている経路記述データに修正される。経路方向δと移動体方向θの偏差が小さい経路が計算されていれば、計算された移動体方向θと実際の移動体方向はよく一致する。
移動体と障害物の離反距離に関して最短値を設定して修正作業を進めることもできる。下記式を加えて修正処理を進めれば、移動体と障害物の最短離反距離が許容距離minlen以上であることが保証されている経路記述データに修正される。許容距離minlenは、図５に示した増加開始距離le0に等しくてもよいし、異なっていてもよい。
また、数式１０から１２のうちの２種または３種を用いて経路修正処理を実施してもよい。
図７は、移動体が移動可能な範囲と、出発点Ｐ1と、移動体が出発点Ｐ1にあるときの凸包２４と、目標点ＰNと、移動体が目標点ＰNにあるときの凸包２４を例示している。
図８は、初期経路を例示している。初期経路によると、移動体と障害物が重なりあってしまう。図９は、トータル評価値を低下させる経路記述データの探索処理を３０回実施した時点での経路を示している。図１０は、探索処理を６０回実施した時点での経路を示している。図１１は、探索処理を１３８回実施した時点での経路を示している。１３８回の探索処理の結果、経路記述データはそれ以上に修正されなくなった。移動体は、障害物を避けながら、目標点に目標姿勢で到達する。
図８は、初期経路を例示している。初期経路によると、移動体と障害物が重なりあってしまう。図９は、トータル評価値を低下させる経路記述データの探索処理を３０回実施した時点での経路を示している。図１０は、探索処理を６０回実施した時点での経路を示している。図１１は、探索処理を１３８回実施した時点での経路を示している。１３８回の探索処理の結果、経路記述データはそれ以上に修正されなくなった。移動体は、障害物を避けながら、目標点に目標姿勢で到達する。
図１２は、修正計算の進行によって評価値（ポテンシャル）が変化していく様子を示している。１３８回修正計算を繰り返した時点で、トータル評価値（全ポテンシャル）が最小化されることが確認される。
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
２：初期経路入力装置
４：修正途上の経路記述パラメータの記憶装置
６：経路長に関する評価関数の記憶装置
８：経路方向と移動体方向の偏差に関する評価関数の記憶装置
１０：障害物までの離反距離に関する評価関数の記憶装置
１２：トータル評価関数の記憶装置
１４：経路記述パラメータについてのトータル評価関数の微分関数の記憶装置
１６：探索方向ベクトルの計算装置
１８：ステップ幅の設定装置
２０：トータル評価値の変化量の判定装置
２２：修正後経路出力装置
４：修正途上の経路記述パラメータの記憶装置
６：経路長に関する評価関数の記憶装置
８：経路方向と移動体方向の偏差に関する評価関数の記憶装置
１０：障害物までの離反距離に関する評価関数の記憶装置
１２：トータル評価関数の記憶装置
１４：経路記述パラメータについてのトータル評価関数の微分関数の記憶装置
１６：探索方向ベクトルの計算装置
１８：ステップ幅の設定装置
２０：トータル評価値の変化量の判定装置
２２：修正後経路出力装置
また、計算された移動体方向θと実際の移動体方向が一致するようにするためには、経路方向δと移動体方向θの偏差に関して許容偏差角を設定しておいてもよい。下記式を加えて修正処理を進めれば、経路方向δと移動体方向θの偏差が許容偏差角η0，φ0以下であることが保証されている経路記述データに修正される。経路方向δと移動体方向θの偏差が小さい経路が計算されていれば、計算された移動体方向θと実際の移動体方向はよく一致する。
移動体と障害物の離反距離に関して最短値を設定して修正作業を進めることもできる。下記式を加えて修正処理を進めれば、移動体と障害物の最短離反距離が許容距離minlen以上であることが保証されている経路記述データに修正される。許容距離minlenは、図５に示した増加開始距離le0に等しくてもよいし、異なっていてもよい。
また、数式１０から１２のうちの２種または３種を用いて経路修正処理を実施してもよい。
Claims (10)
- 経路記述データを入力して記憶する装置と、
経路記述データから経路記述データで記述される経路を評価したトータル評価値を計算するトータル評価関数が、極値をとるときの経路記述データを探索する探索装置を備えており、
その探索装置が、経路記述データを用いて、
・経路長に関する経路長評価値を計算する処理と、
・移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
・少なくとも経路長評価値と離反距離評価値からトータル評価値を計算する処理を実行し、
移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算することを特徴とする経路記述データの修正装置。 - 経路記述データを入力して記憶する装置と、
経路記述データから経路記述データで記述される経路を評価したトータル評価値を計算するトータル評価関数が、極値をとるときの経路記述データを探索する探索装置を備えており、
経路記述データが移動体方向を記述するデータを含んでおり、
前記探索装置が、経路記述データを用いて、
・経路長に関する経路長評価値を計算する処理と、
・経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理と、
・移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
・経路長評価値と偏差評価値と離反距離評価値からトータル評価値を計算する処理を実行することを特徴とする経路記述データの修正装置。 - 経路記述データを入力して記憶する装置と、
経路記述データから経路記述データで記述される経路を評価したトータル評価値を計算するトータル評価関数が、極値をとるときの経路記述データを探索する探索装置を備えており、
経路記述データが移動体方向を記述するデータを含んでおり、
前記探索装置が、経路記述データを用いて、
・経路長に関する経路長評価値を計算する処理と、
・経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理と、
・移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
・経路長評価値と偏差評価値と離反距離評価値からトータル評価値を計算する処理を実行し、
移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算することを特徴とする経路記述データの修正装置。 - 前記探索装置が、移動体を包含する凸包と障害物を包含する凸包の最短距離を計算し、その最短距離から離反距離評価値を計算することを特徴とする請求項１から３のいずれかの１項に記載の経路記述データの修正装置。
- 前記探索装置が、トータル評価関数を経路記述データで偏微分した偏微分関数を用いて、トータル評価関数が極値をとるときの経路記述データを探索する極値探索処理を実行することを特徴とする請求項１から４のいずれかの１項に記載の経路記述データの修正装置。
- 前記探索装置が、トータル評価値を一挙に計算することを特徴とする請求項１から５のいずれかの１項に記載の経路記述データの修正装置。
- 前記探索装置が、直前の経路方向と移動体方向の偏差に関する評価値と、直後の経路方向と移動体方向の偏差に関する評価値を加算して、偏差評価値を計算することを特徴とする請求項２から６のいずれかの１項に記載の経路記述データの修正装置。
- 経路記述データが経由点の順列を含んでおり、
前記探索装置が、経由点間の距離が等しいという制約下で、トータル評価関数が極値をとるときの経路記述データを探索することを特徴とする請求項１から７のいずれかの１項に記載の経路記述データの修正装置。 - 前記探索装置が、経路方向と移動体方向の間の偏差が許容偏差角以下という制約下で、トータル評価関数が極値をとるときの経路記述データを探索することを特徴とする請求項２から８のいずれかの１項に記載の経路記述データの修正装置。
- 前記探索装置が、移動体と障害物の離反距離が許容距離以上という制約下で、トータル評価関数が極値をとるときの経路記述データを探索することを特徴とする請求項１から９のいずれかの１項に記載の経路記述データの修正装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011284391A JP5923973B2 (ja) | 2011-12-26 | 2011-12-26 | 経路修正装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011284391A JP5923973B2 (ja) | 2011-12-26 | 2011-12-26 | 経路修正装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2013134615A true JP2013134615A (ja) | 2013-07-08 |
| JP5923973B2 JP5923973B2 (ja) | 2016-05-25 |
Family
ID=48911265
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011284391A Expired - Fee Related JP5923973B2 (ja) | 2011-12-26 | 2011-12-26 | 経路修正装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5923973B2 (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014115924A (ja) * | 2012-12-12 | 2014-06-26 | Toyota Central R&D Labs Inc | 経路修正装置 |
| JP2015095255A (ja) * | 2013-11-08 | 2015-05-18 | 三星テクウィン株式会社Ｓａｍｓｕｎｇ Ｔｅｃｈｗｉｎ Ｃｏ．， Ｌｔｄ | 無人走行車両の最適駐車経路を生成する方法、及びその方法を採用した無人走行車両 |
| CN105203120A (zh) * | 2014-06-30 | 2015-12-30 | 腾讯科技（深圳）有限公司 | 导航路线评测方法及装置 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10230862A (ja) * | 1997-02-21 | 1998-09-02 | Nissan Motor Co Ltd | 車両用駐車補助装置 |
| JP2003054341A (ja) * | 2001-08-10 | 2003-02-26 | Nissan Motor Co Ltd | 車両用駐車支援装置 |
| JP2007253745A (ja) * | 2006-03-22 | 2007-10-04 | Nissan Motor Co Ltd | 回避操作算出装置、回避制御装置、各装置を備える車両、回避操作算出方法および回避制御方法 |
| JP2007326548A (ja) * | 2006-02-13 | 2007-12-20 | Denso Corp | 車両制御装置 |
| JP2008242859A (ja) * | 2007-03-27 | 2008-10-09 | Sony Corp | 物体の運動制御装置及び運動制御方法、並びにコンピュータ・プログラム |
| JP2008257002A (ja) * | 2007-04-06 | 2008-10-23 | National Institute Of Advanced Industrial & Technology | 駐車運転操作に対する駐車評価方法及びシステム |
-
2011
- 2011-12-26 JP JP2011284391A patent/JP5923973B2/ja not_active Expired - Fee Related
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10230862A (ja) * | 1997-02-21 | 1998-09-02 | Nissan Motor Co Ltd | 車両用駐車補助装置 |
| JP2003054341A (ja) * | 2001-08-10 | 2003-02-26 | Nissan Motor Co Ltd | 車両用駐車支援装置 |
| JP2007326548A (ja) * | 2006-02-13 | 2007-12-20 | Denso Corp | 車両制御装置 |
| JP2007253745A (ja) * | 2006-03-22 | 2007-10-04 | Nissan Motor Co Ltd | 回避操作算出装置、回避制御装置、各装置を備える車両、回避操作算出方法および回避制御方法 |
| JP2008242859A (ja) * | 2007-03-27 | 2008-10-09 | Sony Corp | 物体の運動制御装置及び運動制御方法、並びにコンピュータ・プログラム |
| JP2008257002A (ja) * | 2007-04-06 | 2008-10-23 | National Institute Of Advanced Industrial & Technology | 駐車運転操作に対する駐車評価方法及びシステム |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014115924A (ja) * | 2012-12-12 | 2014-06-26 | Toyota Central R&D Labs Inc | 経路修正装置 |
| JP2015095255A (ja) * | 2013-11-08 | 2015-05-18 | 三星テクウィン株式会社Ｓａｍｓｕｎｇ Ｔｅｃｈｗｉｎ Ｃｏ．， Ｌｔｄ | 無人走行車両の最適駐車経路を生成する方法、及びその方法を採用した無人走行車両 |
| CN105203120A (zh) * | 2014-06-30 | 2015-12-30 | 腾讯科技（深圳）有限公司 | 导航路线评测方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP5923973B2 (ja) | 2016-05-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109764886B (zh) | 一种路径规划方法 | |
| CN107702716B (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
| Gu et al. | Tunable and stable real-time trajectory planning for urban autonomous driving | |
| WO2018176593A1 (zh) | 一种面向无人自行车的局部避障路径规划方法 | |
| KR101585504B1 (ko) | 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치 | |
| Takei et al. | A practical path-planning algorithm for a simple car: a Hamilton-Jacobi approach | |
| JP2017016645A (ja) | 半自律車両、及び、半自律車両を制御する方法 | |
| CN109227533A (zh) | 移动规划装置、移动机器人和移动规划程序 | |
| EP3486612B1 (en) | Method for generating a trajectory | |
| CN103324196A (zh) | 基于模糊逻辑的多机器人路径规划与协调避碰方法 | |
| CN109491389A (zh) | 一种具有速度约束的机器人轨迹跟踪方法 | |
| CN106843236A (zh) | 基于权重改进粒子群算法的无人自行车路径规划方法 | |
| CN110162029A (zh) | 一种基于规划路径的运动控制方法及装置、机器人 | |
| JP5993784B2 (ja) | 経路修正装置 | |
| CN113433936B (zh) | 移动设备绕障方法、装置、移动设备及存储介质 | |
| JP5923973B2 (ja) | 経路修正装置 | |
| JP5986533B2 (ja) | 経路修正装置 | |
| EP3316065A1 (en) | Method for determining a path | |
| JP6131587B2 (ja) | 経路修正装置 | |
| CN114355923A (zh) | 一种a*引导下基于mpc的轨迹规划及跟踪方法 | |
| JP5074153B2 (ja) | 経路生成装置と方法および経路生成装置を備える移動装置 | |
| CN113759892A (zh) | 无人车障碍物绕行方法、装置、无人车及存储介质 | |
| JP6941039B2 (ja) | 軌道計画装置、軌道計画方法、およびプログラム | |
| Amer et al. | Adaptive trajectory tracking controller for an armoured vehicle: Hardware-in-the-ioop simulation | |
| Guo et al. | An improvement of D* algorithm for mobile robot path planning in partial unknown environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20141121 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20150723 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20150804 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20151005 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20160322 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20160404 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5923973 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |