特許 6131587 経路修正装置

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

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

(11)【特許番号】6131587

(24)【登録日】2017年4月28日

(45)【発行日】2017年5月24日

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

(51)【国際特許分類】

   G05D 1/02 20060101AFI20170515BHJP

   B25J 5/00 20060101ALN20170515BHJP

【ＦＩ】

   G05D1/02 H

   G05D1/02 S

   !B25J5/00 E

【請求項の数】2

【全頁数】22

(21)【出願番号】特願2012-270948(P2012-270948)

(22)【出願日】2012年12月12日

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

(43)【公開日】2014年6月26日

【審査請求日】2015年10月23日

(73)【特許権者】

【識別番号】000003609

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

(74)【代理人】

【識別番号】110000110

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

(72)【発明者】

【氏名】長坂 直樹

(72)【発明者】

【氏名】津坂 祐司

【審査官】 川東 孝至

(56)【参考文献】

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

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

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

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

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

【文献】 特開平０５−２９７９３７（ＪＰ，Ａ）

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

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

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

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

Ｂ２５Ｊ １／００−２１／０２

Ｂ６２Ｄ ６／００− ６／１０

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

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

【請求項1】

経路記述データを入力して記憶する装置と、
トータル評価関数が極値をとるときの経路記述データを探索する探索装置を備えており、
その探索装置が、経路記述データを用いて、
・目標位置と目標姿勢のうちの少なくとも一方の修正量に対する目標修正量評価値を計算する処理と、
・移動体と障害物の離反距離に対する離反距離評価値を計算する処理と、
・少なくとも目標修正量評価値と離反距離評価値を含むトータル評価値を計算する処理を実行し、
移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算することを特徴とする経路記述データの修正装置。

【請求項2】

その探索装置が、
・目標位置の第１座標成分の修正量に関する目標修正量第１評価値を計算する処理と、
・目標位置の第２座標成分の修正量に関する目標修正量第２評価値を計算する処理と、
・目標姿勢の修正量に関する目標修正量第３評価値を計算する処理と、
のうちの少なくとも１種を実施することを特徴とする請求項１に記載の経路記述データの修正装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

出発位置と目標位置が与えられたときに、出発位置から目標位置に至る経路を計算する技術が開発されている。例えば、出発位置と目標位置と障害物の存在範囲を示す地図情報に基づいて経路を計算する技術が開発されている。特許文献１には、移動体と障害物の間に斥力が作用するモデルを利用して、出発位置から目標位置に至る経路を計算する技術が開示されている。
移動体に障害物の存在範囲を検出するセンサを搭載し、センサで検出された障害物の存在範囲から、現在位置から目標位置に至る経路を計算する技術も開発されている。

【0003】

地図情報に基づいて計算した経路が、現場の情況に適していないことがある。例えば、経路計算に用いた地図情報が不正確であり、経路計算に用いた目標位置では移動体と障害物が干渉してしまうことが後になって判明することもある。あるいは、移動開始時に検出した障害物の存在範囲を示すデータが不正確であり、障害物に接近して障害物の存在範囲が正確に検出できるようになった時点で、移動開始時に設定した目標位置または目標姿勢が好ましいものでなかったことが判明することがある。移動体に搭載されているセンサには死角が存在することが多いことから、実際に移動している途中で、目標位置の周辺における障害物の存在範囲等が判明することもある。経路の初期値には、修正を要するデータが含まれていることがある。

【0004】

経路の初期値から現場の情況に適した経路に修正する技術も開発されている。非特許文献１では、経路を記述するパラメータからその経路の好ましさ（あるいは現場への適応度）の程度を示す評価値を計算するための評価関数を利用し、評価値を極値化させる経路記述パラメータに修正する。非特許文献１では、好ましい経路ほど小さな評価値を与える評価関数を用いるので、評価値を極小化させる経路記述パラメータに修正する。好ましい経路ほど大きな評価値を与える評価関数を用いる場合には、評価値を極大化させる経路記述パラメータに修正することになる。本明細書では、両者を包含するために、評価値を極値化させる経路記述パラメータを探索するという。好ましい経路ほど小さな値を与える評価関数を用いる場合には評価値を極小とする経路記述パラメータに修正することに相当し、好ましい経路ほど大きな値を与える評価関数を用いる場合には評価値を極大とする経路記述パラメータに修正することに相当する。

【先行技術文献】

【特許文献】

【0005】

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

【非特許文献】

【0006】

【非特許文献1】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

【発明の概要】

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

【0007】

特許文献１等の経路計算技術と、非特許文献１等の経路修正技術を併用すると、現場の情況に適した経路が得られるように思われる。しかしながら、実行可能な移動経路に修正できないために、移動体が移動不能となって立ち往生する事態が発生する。
本発明者らがその原因を追究したところ、目標位置および／または目標姿勢（目標位置と目標姿勢の一方または双方）が不適当であるために、実行可能な移動経路に修正できなくなっている事態が発生していることを発見した。非特許文献１では、目標位置に至る経由点について最適化する。しかしながら、目標位置自体は与えられたものであり、最適化の対象にしない。移動体でどこへ行くかは、移動目的によって決まる問題であり、数学的に最適化するものでないのに対応している。

【0008】

一般的にいうと、経路計算装置あるいは経路修正装置では、目標位置および／または目標姿勢を与えられたものと扱い、最適化の対象にしないとするのが正しいと思われる。どこへ行くか・いかなる姿勢で停止するかは、移動目的によって決まる問題であり、数学的に最適化にできるものでないはずだからである。実際に、経路計算に用いる地図情報等が正確であるか、あるいは目標位置周辺における障害物の存在範囲が正確には検出されていれば、正確なデータに基づいて設定された目標位置または目標姿勢をそれ以上に最適化する必要はないし、それ以上には最適化しようがないということもできる。

【0009】

しかしながら、経路計算装置あるいは経路修正装置に与えられた目標位置および／または目標姿勢が、正確なデータに基づいて最適に設定された目標位置および／または目標姿勢であるとは限られない。経路計算に用いた地図情報が不正確であり、実際に移動してみると、目標位置および／または目標姿勢が不適切に設定されていたことが判明することがある。あるいは、移動開始時に検出した障害物の存在範囲を示すデータが不正確であり、障害物に接近してみて始めて、目標位置および／または目標姿勢が不適切に設定されていたことが判明することもある。移動体に搭載されているセンサで障害物の存在位置を検出する場合、障害物の存在位置を検出した時のセンサ位置とセンサ姿勢に誤差が含まれており、それに起因して、障害物の存在範囲を示すデータが不正確であることもある。

【0010】

本来は最適化の対象に馴染まないにも係らず、目標位置および／または目標姿勢を最適化するという着想を導入すると、実行可能な移動経路に修正できないために移動体が立ち往生してしまうという事態の発生を抑制できることが確認されたことから、本明細書で開示する技術が創作された。

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

【0011】

本明細書で開示する経路記述データの修正装置は、経路記述データを入力して記憶する装置と、トータル評価関数が極値をとるときの経路記述データを探索する探索装置を備えている。
その探索装置は、経路記述データを用いて、
・目標位置と目標姿勢のうちの少なくとも一方の修正量に対する目標修正量評価値を計算する処理と、
・移動体と障害物の離反距離に対する離反距離評価値を計算する処理と、
・少なくとも目標修正量評価値と離反距離評価値を含むトータル評価値を計算する処理を実行する。
上記の評価値計算では、移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算する。

【0012】

上記の修正装置によると、移動体と障害物が重なりあう計算途上の経路に対して有限の離反距離評価値を計算することから、修正計算の途中で移動体と障害物が重なりあう経路を処理することになっても計算不能となることがなく、修正計算を続行することができる。何らかの理由（その理由は後で説明する）で、移動体と障害物が重なりあう経路が形成されている場合には、移動体と障害物が重なりあわない経路に修正される。
何らかの理由で目標位置および／または目標姿勢（以下では目標値という）が不適切である場合には、経由点の修正だけでは、移動体と障害物が重なりあわない経路に修正されない。本装置では、目標値までをも修正対象とする。何らかの理由で目標値が不適切であって移動体と障害物が重なりあってしまう場合には、目標値が修正され、移動体と障害物が重なりあわない経路探索が続行される。
目標値の修正に際に、目標修正量評価値が計算に組み込まれる。目標修正量から目標修正量評価値を計算する評価関数には、目標値の修正が許容される程度を反映した関数を用いることができる。例えば、目標値の修正量が小さいほど高評価値を与える評価関数が用いられる。
目標値が不適切な場合、目標値を修正することで障害物までの離反距離が確保される経路が採用可能となる。目標値を修正することで、離反距離評価値が高評価値側に変化するものと期待される。
目標値を修正すること自体で、修正に対する評価値が低評価値側に変化するが、その一方において、離反距離評価値は高評価値側に改善され、両者を含むトータル評価値を指標にして探索処理を続けると、移動体と障害物が重なりあわない経路が得られる目標値に修正され、移動体と障害物が重なりあわない経路に修正される。修正量に対する評価関数を、目標値の修正量が小さいほど高評価値を与える関数にしておくと、目標値を小さく修正することで移動体と障害物が重なりあわない経路が得られる目標値に修正される。

【0013】

修正処理を実際に実行すると、何らかの理由で目標値が不適切であって移動体と障害物が重なりあってしまう場合には、目標値を少しずつ修正し、修正された目標値に至る経路によると移動体と障害物が重なりあわない経路に修正される。経路計算に用いた地図情報が不正確であって実際に移動してみると障害物と重なりあってしまう目標値が設定されていたような場合、あるいは、移動開始時に検出した障害物の存在範囲を示すデータが不正確であって障害物に接近してみると障害物と重なりあってしまう目標値が設定されていたことが判明するような場合には、目標値が少しずつ修正され、修正された目標値に至る経路によると移動体と障害物が重なりあわない経路に修正される。修正された目標値に至る経路によると移動体と障害物が重なりあわない経路が得られるに至るまで、修正量を少しずつ増加させながら、経路の最適化処理が繰り返される。
本来は最適化の対象に馴染まない目標値を最適化することによって、実行可能な経路へ修正する計算が実行可能となることが多い。

【0014】

目標位置と目標姿勢は、目標位置の第１座標成分と、目標位置の第２座標成分と、目標姿勢（角度）で記述される。例えば、地図情報がｘ−ｙ直交座標系で記述されていれば、移動体の基準点のｘ座標値（第１座標成分）と、ｙ座標値（第２座標成分）と、移動体に固定されている基準線とｘ軸のなす角（目標姿勢）で、目標位置と目標姿勢が記述される。
目標値の修正に伴う評価値を計算する際には、
・目標位置の第１座標成分の修正量に対する目標修正量第１評価値を計算する処理と、
・目標位置の第２座標成分の修正量に対する目標修正量第２評価値を計算する処理と、
・目標姿勢の修正量に対する目標修正量第３評価値を計算する処理と、
のうちの少なくとも１種を実施することが好ましい。

【0015】

目標値の修正に関する拘束条件（あるいは許容条件）は必ずしも一様でない。例えば、ｘ軸に沿って進行して駐車するような場合、目標姿勢の修正に関しては強く拘束し、目標ｘ座標値の修正に関しては弱く拘束するのが適当なことがある。その場合、移動体はｘ軸に沿った姿勢を維持し、障害物までの実際距離が予定距離となる位置に停止させやすい。目標ｘ座標値の修正を強く拘束すると、障害物までの実際距離が予定距離から離れていても目標ｘ座標値の修正が拘束されるのに対し、目標ｘ座標値の修正に対する拘束を緩めれば、障害物までの実際距離を基準にして目標ｘ座標値が修正されていく。

【0016】

ｘ―ｙ座標系に代えて、移動体の進行方向を第１座標軸とし、それに直交する方向を第２座標軸とすることもできる。例えば、移動体の進行方向の目標位置については強く拘束し、それに直交する方向には弱く拘束するのが適当なことがある。その場合、例えば狭い通路内に侵入して停止させる経路に修正しやすい。

【0017】

目標位置の第１座標成分と、目標位置の第２座標成分と、目標姿勢の全部を修正対象に含めてもよい。あるいは、そのうちの１種または２種のみを修正対象としてもよい。修正量に対して敏感に増大する評価関数を設定すれば、その値の修正を強く拘束することができ、極端に敏感に変化する評価関数を設定することによって、修正対象から外すこともできる。
目標姿勢を拘束して目標位置のみを修正する形態、目標位置を拘束して目標姿勢のみを修正する形態、目標位置と目標姿勢の修正を同程度に許容する形態等に調整することができる。

【発明の効果】

【0018】

本明細書に記載の技術によると、目標値が不適切であって移動体と障害物が重なりあう経路となる場合には、移動体と障害物が重なりあわない経路に修正されるまで目標値が修正され、移動体と障害物が重なりあわない経路に修正される。本来的には最適化の対象でない目標値までをも修正対象に含めることで、実行可能な経路が探索されないために（実行可能な経路に修正できないことに対応する）、移動不能となる事態の発生を抑制することができる。

【0019】

特に、目標値の種類毎に修正量に対する評価値の変化量に大小関係を設定すると、大きく修正してもかまわない種類の目標値と大きく修正してはいけない種類の目標値があるのに対応して目標値を修正することができ、実情に適合した修正形態に調整することができる。

【図面の簡単な説明】

【0020】

【図1】（ａ）は実施例の経路修正装置のシステム構成を示す図であり、(ｂ)は第１座標成分の修正量と評価値の関係を示す図であり、(ｃ)は第２座標成分の修正量と評価値の関係を示す図であり、(ｄ)は目標姿勢の修正量と評価値の関係を示す図であり、(ｅ)は離反距離と評価値の関係を示す図である。

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

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

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

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

【図6】偏微分関数の導出過程を示す図である。

【図7】障害物と走行可能な範囲を示す図である。

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

【図9】修正途上の経路を例示する図である。

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

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

【図12】修正計算の進行に伴って評価値が変化する関係を例示する図である。

【図13】目標位置の修正過程を例示する図である。

【図14】進行方向に直交する方向に目標位置を修正する場合の、修正量αと、ｘ方向修正量ΔｘＮと、ｙ方向修正量ΔｙＮの関係を示す図である。

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

【0021】

下記に説明する実施例の主要な特徴を列記する。
特徴１：離反距離評価値は、離反距離が長ければ小さな値をとり、離反距離が短くなると増大する。移動体と障害物が重なりあう場合には、深く重なりあうほど増大する離反距離評価値を用いる。
特徴２：離反距離がポテンシャル増大開始距離以上であれば、離反距離評価値はゼロである。
特徴３：第１座標成分の修正量から計算される第１評価値と、第２座標成分の修正量から計算される第２評価値と、目標姿勢の修正量から計算される第３評価値と、離反距離から計算される離反距離評価値を加味してトータル評価値を計算する。
特徴４：第１座標成分の修正量から計算される第１評価値と、第２座標成分の修正量から計算される第２評価値と、目標姿勢の修正量から計算される第３評価値と、離反距離から計算される離反距離評価値と、経路長から計算される経路長評価値を加味してトータル評価値を計算する。
特徴５：第１座標成分の修正量から計算される第１評価値と、第２座標成分の修正量から計算される第２評価値と、目標姿勢の修正量から計算される第３評価値と、離反距離から計算される離反距離評価値と、経路長から計算される経路長評価値と、移動体の向きと移動体の進行方向の偏差から計算される偏差評価値を加味してトータル評価値を計算する。

【実施例】

【0022】

経路記述データの修正装置の実施例を説明する。最初に用語の意味を明らかにしておく。
経路記述データ：経由点（移動体の基準点が通過する点）のｘ座標と、経由点のｙ座標と、経由点における移動体方向（ｘ軸の正方向から移動体に固定されている基準線が指向している方向までの反時計回転方向の角θ）が、経由点の順序に従って並んでいるデータをいう。
経路記述データは、式（１）で示され、Ｐｉは式（２）で示される。ｘi，ｙi，θiを、本明細書では、経路記述パラメータという。ｉ＝１は出発点であり、ｉ＝Ｎは目標点である。ｘ１，ｙ１，θ１は与えられている。目標位置と目標姿勢を記述するｘＮ，ｙＮ，θＮは、経路記述データの初期値を計算する段階では与えられた値であるが、修正した方がよい場合がある。実施例の修正装置では、目標位置と目標姿勢を記述するｘＮ，ｙＮ，θＮを変数として扱い、修正処理の対象とする。

【数1】

【数2】

【0023】

初期経路：修正装置に入力される、修正処理前の経路記述データで記述される経路をいう。初期経路記述データは、ＲＲＴのような確率的手法で求めることもできるし、Ａ^＊探索のようなグリッド探索法で求めることができる。実施例の修正装置は、初期経路記述データの生成プロセスに限定されないで適用することができる。
初期経路は、出発点の出発前に、地図情報に基づいて計算したものであってもよい。あるいは、移動体が移動しながら計算したものであってもよい。地図情報に基づいて初期経路を計算した場合、地図情報が不正確であり、地図情報に基づいて設定した目標値が不適切な場合がある。あるいは、地図情報と移動体の現在位置の間にずれがあり、そのずれの分だけ、移動体から見た目標値が地図上の目標値からずれていることがある。移動体が障害物の存在範囲を検出するセンサを備えている場合、移動開始直後には目標位置までの距離が遠く、目標位置の近傍に位置している障害物の存在範囲が不正確に検出されていることがある。あるいは、センサの死角になっていたために目標位置の近傍に位置している障害物が検出されていなかったこともある。あるいは、障害物の存在範囲を検出したときのセンサの位置と姿勢の計算値に誤差が含まれており、それによって障害物の存在範囲が不正確に検出されていることがある。
初期経路に含まれている目標値は、必ずしも適切なものでなく、障害物の実際の存在範囲が判明した際に、修正したほうがよい場合が生じる。
初期経路には各種の不都合が含まれている可能性があり、例えば移動体がその初期経路に沿って移動すると移動体が障害物に接触することがある。あるいは、経路方向が急激に変化することがある。あるいは、移動体の経路長が不必要に長い場合もある。ＲＲＴのような確率的手法やＡ^＊探索のようなグリッド探索法で求めた経路が、移動体と障害物の干渉をもたらすにはいくつかの理由がある。例えば、初期経路の探索過程を簡単化するために、移動体の形状を小さめに設定することがある。あるいは移動体の形状を単純化することがある。移動体の形状を簡単化したために、実際には移動体と障害物が干渉することがある。またグリッド探索手法では移動体方向を無視することがある。移動体が、計算された経路に沿って移動体の方向を変えながら移動すると、移動体と障害物が干渉することがある。種々の理由によって、初期経路には各種の不都合が含まれている可能性がある。経路の修正装置には、障害物と干渉する経路を処理して障害物と干渉しない経路に修正する能力が必要とされる。

【0024】

移動体：有限の大きさを備えている。
障害物：有限の大きさを備えている。
凸包：物体を包含する最小の凸多角形をいう。
最短距離計算法：離れて存在する２個の凸包のそれぞれにおける最近接点を求め、その距離（凸包間の最短距離）を計算する手法。ＧＪＫアルゴリズムなどの既知の数学手法を利用することができる。
ポテンシャル増大開始距離：離反距離がそれ以下となると、離反距離評価値を計算するポテンシャル関数がゼロでなくなる距離。
許容距離：移動体と障害物の間に確保する最小離反距離。ポテンシャル増大開始距離に等しくしてもよいし、異なっていてもよい。
許容偏差：先行経由点から後続経由点を指す方向と、ｘ軸の正方向がなす角をδとしたときに、先行経由点における移動体の方向とδの偏差、ならびに、後続経由点における移動体の方向とδの偏差に関して許容される最大偏差をいう。

【0025】

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

【0026】

修正装置は、トータル評価値を計算するトータル評価関数記憶装置２０を備えている。経路記述パラメータ記憶装置６に記憶されている経路記述パラメータを、トータル評価関数に代入することで、トータル評価値を計算することができる。

【0027】

トータル評価値は、下記の評価値の総和である。
第１評価値：目標位置のｘ座標値（目標ｘ座標値）の修正量に依存する評価値である。本実施例では、高評価であるほど値が低い評価値を用いる。図１（ｂ）に例示されているように、目標ｘ座標値の修正量がゼロであれば評価値がゼロであり、目標ｘ座標値の修正量が増大すれば増大する評価値を用いる。目標ｘ座標値の修正量がゼロに近いほど、高く評価される。目標ｘ座標値を示すｘＮの修正量ΔｘＮから第１評価値が特定される。
第２評価値：目標位置のｙ座標値（目標ｙ座標値）の修正量に依存する評価値である。図１（ｃ）に例示されているように、目標ｙ座標値の修正量がゼロであれば評価値がゼロであり、目標ｙ座標値の修正量が増大すれば増大する評価値を用いる。目標ｙ座標値の修正量がゼロに近いほど、高く評価される。目標ｙ座標値を示すｙＮの修正量ΔｙＮから第２評価値が特定される。
第３評価値：目標姿勢の修正量に依存する評価値である。本実施例では、図２を参照して後記するように、ｘ軸の正方向と移動体２６に固定されている基準線３０がなす角度θiで目標姿勢を記述する。図１（ｄ）に例示されているように、目標姿勢の修正量がゼロであれば評価値がゼロであり、目標姿勢の修正量が増大すれば増大する評価値を用いる。目標姿勢の修正量がゼロに近いほど、高く評価される。目標姿勢を示すθＮの修正量ΔθＮから第３評価値が特定される。
離反距離評価値：障害物から移動体までの最短距離に依存する評価値である。経路を示すｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮから、障害物から移動体までの最短距離が計算でき、離反距離評価値が計算できる。離反距離評価値の一例が図１（ｅ）に例示されている。詳しくは図４、図５を参照して後記するが、移動体と障害物間の離反距離が接近するほど、低く評価される評価値（大きな評価値）を与える。

【0028】

技術的には、上記によって、目標値を修正して移動体と障害物が重なり合わない経路に修正することができるが、本実施例では、さらに下記の評価値を利用する。
経路長評価値：出発点から目標点に至る経路の全長が短いほど高評価を与える評価値である。経由点を示すｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮから、出発点から目標点に至る経路の全長に対応する量が計算でき、経路長評価値が計算できる。
偏差評価値：移動体の向き（車体方向、姿勢方向、移動体方向といってもよい）と移動方向（経路方向）の偏差が小さいほど高評価を与える評価値である。移動方向がスムースに変化する経路が高評価されるように作用する。経路を示すｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮから、偏差に対応する量が計算でき、偏差評価値が計算できる。

【0029】

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

【0030】

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

【0031】

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

【0032】

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

【0033】

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

【0034】

（経路記述データの詳細）
図２において、参照番号２６は移動体を示し、出力装置８から出力された修正後経路に沿って移動する。参照番号２４は移動体２６を包含する凸包であり、図１の修正装置では、凸包２４と障害物間の最短距離を求めて離反距離を計算する。参照番号２８は移動体２６に固定されている基準点であり、参照番号３０は移動体２６に固定されている基準線である。
経路は、データＰｉ＝（ｘi，ｙi，θi）（ここでｉ＝１，２，・・Ｎ）で定義される。移動体２６は、基準点２８が（ｘi，ｙi）の座標点を辿るように移動する。また、移動体２６は、基準線３０がθiの方向を向く車体姿勢をとる。本明細書では、角度をｘ軸の正方向から反時計回転方向に計測した角度で示す。
参照記号Ｏjは、移動体２６の走行を妨げる障害物である。本明細書では、出発点（ｘ１，ｙ１）から目標点（ｘＮ，ｙＮ）までの経路を決定する際に考慮する必要がある障害物がＭ個存在するものとする。

【0035】

（初期経路の詳細）
図１の修正装置に入力される初期経路は、ＲＲＴのような確率的手法あるいはＡ^＊探索のようなグリッド探索法で求めた大局的経路である。データＰｉ＝（ｘi，ｙi，θi）のＮ個の並びで記述されている。Ａ^＊探索のようなグリッド探索法で経路を探索すると、移動体方向θiは計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。
確率的手法あるいはグリッド探索法で狭い空間を通過する経路を求める場合、探索に失敗することがある。確率的手法による場合には、実用的な時間内に通過経路が計算できないことがある。グリッド探索による場合は、離散化されたグリッドが粗すぎて通過経路を発見できないことがある。探索失敗を避けるために、初期経路の探索過程で制約条件を緩和し、実用的な時間内に初期経路が探索されるようにしている。
上記の事情等によって、初期経路には下記の問題が内在されていることがある。
（１）目標位置および／または目標姿勢が適切でなく、目標位置および／または目標姿勢をわずかに修正すれば移動体と障害物と重なり合わないのに、初期データの目標位置と目標姿勢によると、移動体と障害物と重なりあって走行不能となってしまう。
（２）移動体と障害物が干渉する（重なりあう）経路である。
（３）移動体と障害物が重なり合わないまでも非常に接近する経路である。
（４）経路方向が急激に変化する経路である。
（５）経路方向と移動体方向が不一致であって、移動体が追従できない経路情報である。
図１の修正装置は、上記の問題を解消する側に修正した経路を計算する。初期経路のうち、出発点のデータＰ１＝（ｘ１，ｙ１，θ１）は、確定的なものであり、図１の修正装置で修正することはない。経由点のデータＰｉ（i＝２，・・，Ｎ−1）は最善なものでない可能性があり、図１の修正装置で修正する。目標点のデータＰＮ（i＝Ｎ）も最善なものでない可能性があり、図１の修正装置で修正する。その修正計算のために、修正装置は下記の６種類の評価関数を利用する。

【0036】

（第１評価値）目標ｘ座標値を示すｘＮの修正量ΔｘＮから第１評価値が特定される。目標ｘ座標値の初期値が好ましいものでなく、実際には障害物と移動体が重なり合ってしまう位置であることがある。目標ｘ座標値の初期値が好ましいものでない場合、それを与えられた値として固定的に扱うと、走行可能な経路に修正できず、移動体が立ち往生することがある。本実施例では、目標ｘ座標値を修正対象に含めるために、目標ｘ座標値ｘＮの修正量ΔｘＮに対して有限の評価値を与える。本実施例では、図２の（２）に示すように、目標ｘ座標値の修正量ΔｘＮの二乗に比例する第１評価値を与える。Ｋ１は重み係数である。
目標ｘ座標値の初期値によると移動体と障害物が重なりあう場合、目標ｘ座標値を修正することで第１評価値が増大する（低評価に変わる）が、反面、離反距離評価値が高評価値に変わる。トータル評価値を極小化するプロセスを実行することで、移動体と障害物が重なりあわない位置に目標ｘ座標値を修正する結果が得られる。目標値ｘ座標値を修正することで移動体が障害物と重なりあわないで移動できる経路に修正される。

【0037】

（第２評価値）目標ｙ座標値を示すｙＮの修正量ΔｙＮから第２評価値が特定される。本実施例では、図２の（２）に示すように、目標ｙ座標値の修正量ΔｙＮの二乗に比例する第２評価値を利用する。Ｋ２は重み係数である。第２評価値を導入すると、目標ｙ座標値を修正することによって移動体が障害物と重なりあわないで移動できる経路に修正される。

【0038】

（第３評価値）目標姿勢を示すθＮの修正量ΔθＮからから第３評価値が特定される。本実施例では、図２の（２）に示すように、目標姿勢の修正量ΔθＮの二乗に比例する第３評価値を利用する。Ｋ３は重み係数である。第３評価値を導入すると、目標姿勢を修正することによって移動体が障害物と重なりあわないで移動できる経路に修正される。

【0039】

本実施例では、第１〜第３評価関数に、目標値の修正量ΔｘＮ，ΔｙＮ，ΔθＮの二乗に比例する評価関数を用いる。その重み係数Ｋ１，Ｋ２，Ｋ３は、それぞれ固有に決めることができる。例えば、ｘ軸に沿って進行して駐車するような場合、目標姿勢の修正に関しては強く拘束し、目標ｘ座標値の修正に関しては弱く拘束するのが適当なことがある。そのためには、目標姿勢の修正量ΔθＮに対して敏感に変化する関数を第３評価関数とし（重み係数Ｋ３に大きな値を用いる）、目標ｘ座標値の修正量ΔｘＮに対して鈍感に変化する関数を第１評価関数とする（重み係数Ｋ１に小さな値を用いる）。その場合、姿勢に関しては初期値に近い値を維持するためにｘ軸に沿った姿勢を維持する一方において、ｘ方向の位置に関しては柔軟に修正して障害物までの実際距離を優先して停止させるという経路に修正することができる。
また第１〜第３評価関数の重み係数を変えるのに代えて、それぞれに固有の関数形を利用してもよい。

【0040】

（経路長評価値）
出発点から目標点に至る経路の全長が短いことが好ましい。全長が短い経路に修正されるように、修正装置は経路長評価関数を利用する。経路長評価関数を、図２の（３）式に示す。係数Ｋａは、トータル評価値に占める経路長評価値の重みを示している。距離ｌiは、座標点（ｘｉ，ｙｉ）から座標点（ｘｉ＋１，ｙｉ＋１）までの距離である。ｉ＝１からＮ−１までの総和を取れば、経路全長に関する評価値が求められる。全長が短いほど小さな評価値が計算される。修正装置は、トータル評価値を極小化する経路に修正する。全長が短いほど小さな評価値が計算される経路長評価関数を導入するために、修正装置によって全長が短い経路に修正される。
図２の（３）式に示す経路長評価関数を極小化すると、
（１）全長が短くなる経路に修正され、
（２）Ｐ２，・・，ＰＮ−１というＮ−２個の経由点を、等間隔に配置した経路記述データに修正される（全長が等しければ、経由点が等間隔である場合の評価値の方が、経由点が等間隔でない場合の評価値よりも小さくなる）。
経路長評価関数以外の評価関数をも利用するために、全長のみを考慮して修正されるものでない。目標位置を出発位置に近づける修正をすれば、経路長評価値は高評価に修正されるが、目標位置を大きく修正すれば第１〜第３評価値が低評価に修正されるので、目標位置が出発位置にむやみに近づけられることはない。

【0041】

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

【0042】

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

【0043】

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

【0044】

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

【0045】

（離反距離評価関数）
図５（ａ）は、離反距離評価関数Ｕｅi,jを示している。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以上であれば、離反距離評価値はゼロである。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以下になるほど接近すると、離反距離評価値は増加し始める。移動体と障害物が接近して離反距離ｌｅi,jがゼロに近づくほど、離反距離評価値は大きくなる。移動体と障害物が接近して離反距離ｌｅi,jがゼロとなっても、離反距離評価値は無限大に発散しない。有限の値が計算される関数形を採用している。
移動体と障害物が接近して重なりあう場合、ＧＪＫアルゴリズムでは正の値を持つｌｅi,jが計算される。この場合のｌｅi,jは、移動体と障害物が重なりあっている距離であり、両者を離反させるのに必要な移動距離に等しい。本実施例では、移動体と障害物が接近して重なりあう場合には、ｌｅi,jが負の値を持つ距離だと扱う。
図５の（ｂ）は、離反距離評価関数を示している。上記の離反距離評価関数を用いると、移動体が障害物に接近し、接触し、深く重なりあうのに伴って、離反距離評価値が一様に増加する関係が得られる。係数Ｋｅは、離反距離評価関数の重み係数である。
実際には、移動体と障害物が重なりあう現象は生じない。しかしながら、修正計算の途上では、移動体と障害物が重なりあう経路に関する計算が必要とされることがある。例えば、移動体と障害物が重なりあう初期経路を、重なり合わない経路に修正する場合には、移動体と障害物が重なりあう経路に関する計算が必要とされる。本実施例では、移動体と障害物が重なりあう場合も有限の離反距離評価値が計算される離反距離評価関数を用いるので、修正計算の途上で計算不能に陥ることがない。
障害物はＭ個存在する。そこで、移動体が経由点Ｐiにあるときの離反距離評価値は、図５の（１０）式で求められる。また、経路の全長に亘って累積した離反距離評価値は、図５の（１１）式で求められる。出発点は、修正作業の対象でないので、i＝２，・・Ｎまで加算する。

【0046】

（トータル評価関数）
本実施例では、下記の式３で示されるトータル評価関数を利用する。式３において、Ｕ１は第１評価関数であり、Ｕ２は第２評価関数であり、Ｕ３は第３評価関数であり、Ｕａは経路長評価関数であり、Ｕｂは第１偏差評価関数であり、Ｕｃは第２偏差評価関数であり、Ｕｅは離反距離評価関数である。

【数3】

【0047】

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

【0048】

（第１〜第３評価関数の偏微分関数）
図２の（２）に示す第１評価関数Ｕ１をΔｘＮで偏微分した偏微分関数を求めると下記の式（ａ）に示すものとなる。ここで、ｘＮは修正後の目標ｘ座標値であり、ｘＮＯは目標ｘ座標値の初期値である。第２評価関数の偏微分関数は下記の式（ｂ）に示すものとなる。ここで、ｙＮは修正後の目標ｙ座標値であり、ｙＮＯは目標ｙ座標値の初期値である。第３評価関数の偏微分関数下記の式（ｃ）に示すものとなる。θＮは修正後の目標姿勢であり、θＮＯは目標姿勢の初期値である。

【0049】

（経路長評価関数の偏微分関数）
図２の（３）に示した経路長評価関数Ｕａの偏微分関数を求めると下記の式（ｄ），（ｅ），（ｆ）で示すものとなる。

【数4】

【0050】

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

【数5】

【0051】

（離反距離評価関数の偏微分関数）
離反距離評価関数の偏微分関数の導出過程を、図６に示す。その結果、図５の（１０）に示した離反距離評価関数Ｕｅi,j、すなわち移動体が経由点Ｐiにあるときのｊ番目の障害物までの離反距離に対する評価関数の偏微分関数を求めると、下記となる。

【数6】

上記において、ρxとρyは、移動体２４に固定された座標系での、移動体側の最近接点Ｒi,j（図４参照）の座標値である。

【0052】

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

【0053】

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

【0054】

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

【数7】

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

【数8】

【数9】

【0055】

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

【表1】

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

【0056】

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

【0057】

上記の修正計算を実行した結果、障害物に近い範囲で経由点が疎に分布し、障害物に遠い範囲で経由点が密に分布する経路記述データに修正されることがある。経由点が疎に分布すると、移動体と障害物の干渉の有無を判定する処理の精度が低下するので好ましくない。経由点が密に分布すると、曲率半径が小さな急カーブに修正される可能性があり、それも好ましくない。
それを避けるためには、全部の経由点が等間隔で配置されているという条件を課して修正処理を進めることが好ましい。下記式を加えて修正処理を進めれば、全部の経由点が等間隔で配置されている経路記述データに修正される

【数10】

【0058】

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

【数11】

【0059】

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

【数12】

【0060】

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

【0061】

図７は、移動体が移動可能な範囲と、出発点Ｐ1と、移動体が出発点Ｐ1にあるときの凸包２４と、目標点ＰＮと、移動体が目標点ＰＮにあるときの凸包２４を例示している。
図８は、初期経路を例示している。初期経路によると、移動体と障害物が重なりあってしまう。図９は、トータル評価値を低下させる経路記述データの探索処理を３０回実施した時点での経路を示している。図１０は、探索処理を６０回実施した時点での経路を示している。図１１は、探索処理を１３８回実施した時点での経路を示している。１３８回の探索処理の結果、経路記述データはそれ以上に修正されなくなった。移動体は、障害物を避けながら、目標点に目標姿勢で到達する。

【0062】

図１２は、修正計算の進行によって評価値（ポテンシャル）が変化していく様子を示している。１３８回修正計算を繰り返した時点で、トータル評価値（全ポテンシャル）が極小化されることが確認される。

【0063】

（第２実施例）
図１３は、初期経路記述データ１３２における目標位置（ｘＮＯ、ｙＮＯ）１３２ｅが不適切に設定されており、実際には障害物と干渉する範囲１３６内に設定されていた場合を例示する。また、目標姿勢に沿った座標軸における目標座標値の修正量に対して第１評価関数が設定されており、目標姿勢に直交する座標軸における目標座標値の修正量に対して第２評価関数が設定されている場合を例示している。さらに、目標姿勢に沿った座標軸での目標位置の修正量に対して第１評価関数が敏感に変化し、目標姿勢に直交する座標軸での目標位置の修正量に対して第２評価関数が鈍感に変化し、目標姿勢の修正量に対して第３評価関数が敏感に変化する場合を例示している。この場合、目標姿勢に直交する座標軸上での目標座標が柔軟に修正される。その結果、干渉範囲１３６外に目標位置を修正する際に、主として目標姿勢に直交する方向に修正する探索が行われ、１３４ｅに示す修正後目標位置が探索され、目標値を修正することによってトータル評価値が極小化された場合を例示している。

【0064】

第１・第３評価値が敏感に変化し、第２評価値が鈍感に変化する場合、目標姿勢に直交する方向の修正量αに対して、目標ｘ座標の修正量ΔｘＮと目標ｙ座標の修正量ΔｙＮは図１４に示すものになる。この関係を利用して、目標姿勢に直交する方向の修正量αに関するトータル評価値の偏微分関数を得ることができ、それによって、トータル評価値を極小化するαの値を探索することができる。第２実施例によると、目標位置の初期値が不適切な場合に、進行方向と直交方向に目標位置をずらす経路に修正されることになる。

【0065】

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
例えば、実施例ではトータル評価値に経路長評価値と偏差評価値を組み込んでいるが、これらの一方または双方を外してもよい。また実施例では、トータル評価値に目標修正量第１評価値と目標修正量第２評価値と目標修正量第３評価値の全部を組み込んでいるが、これらのうちの1種または2種のみを採用してもよい。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

【符号の説明】

【0066】

２：修正前の経路記述パラメータ
４：入力装置
６：修正途上の経路記述パラメータの記憶装置
８：出力装置
１０：修正後の経路記述パラメータ
１２：経路記述パラメータについてのトータル評価関数の偏微分関数の記憶装置
１４：探索方向ベクトルの計算装置
１６：ステップ幅の設定装置
１８：トータル評価値の変化量の判定装置
２０：トータル評価関数の記憶装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】