JP5074153B2 - 経路生成装置と方法および経路生成装置を備える移動装置 - Google Patents

Description

本発明は、自律移動する移動装置の移動経路を生成する経路生成装置と方法および経路生成装置を備える移動装置に関する。

自律移動する移動装置は、舗装路、未舗装路（オフロード）または屋内において自律的に移動するロボットや車両である。
移動装置が自律移動・走行するために、移動装置は、既知の地図上に設定された移動経路に沿って移動するように制御される（例えば、下記特許文献１）。

この制御を行うために移動装置は、例えば記憶装置、内界情報検出装置、障害物検出装置、経路探索装置、制御装置を有する。

記憶装置は、移動装置の移動範囲を含む領域の地図（地図情報）と、移動開始位置から目標到達位置までの設定経路（経路情報）と、を記憶する。地図情報は、例えば２次元座標による座標系である。経路情報は、移動開始位置から目標到達位置までの間に設定される複数の経由位置である。これら経由位置は後述する上記移動経路の生成・設定の指標となる。なお、これら経由位置は、上記座標系における座標であってよい。

内界情報検出装置は、移動装置の現在位置、移動装置の移動方向、移動速度などの移動装置自体に関する情報を検出する。

障害物検出装置は、移動装置の周囲（例えば、移動装置の進行側の所定領域内）において停止車両、凹凸地、壁などの障害物の有無およびその位置・範囲を検出する。

経路探索装置は、例えば所定の周期で、移動装置の現在位置、障害物の位置・範囲、上記経路情報に基づいて、次の経由位置に向かうための移動経路を探索して生成する。即ち、経路探索装置は、検出された障害物を回避して現在位置から次の経由位置へ移動するための移動経路を探索して生成する。生成された移動経路は、移動装置の走行制御に使用するために記憶装置に設定・記憶される。

制御装置は、移動装置の現在位置に基づいて、上述のように設定された移動経路に沿って移動装置が走行するように、移動装置の移動方向や速度などを制御する。
特開平７−２４８８２０号公報 「不整地移動ロボットの自律走行システム」 The International Journal Robotics Research Vol. 22, No. 7-8, July-August 2003, pp.583-601 Journal of Field Robotics 23(5), 311-331(2006), 2006 Wiley Periodicals, Inc. Published online in Wiley InterScience(www.interscience.wiley.com), DOI: 10.1002/rob.20118

しかし、障害物と次の経由位置との位置関係や障害物の大きさなどによっては、障害物を回避して次の経由位置へ移動するための経路を見出せない場合がある。このような場合には、移動装置の移動が継続できなくなってしまう。

そこで、本発明の目的は、このような場合を想定し、移動経路をより確実に見出し、移動装置の移動を継続させられる可能性を効果的に高めることができる経路生成装置と方法を提供し、このような経路生成装置を備える移動装置を提供することにある。

上記目的を達成するため、本発明によると、自律移動する移動装置の移動経路を生成する経路生成装置であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
前記移動装置の現在位置を検出する位置検出装置と、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索し生成する経路探索装置と、を備え、
前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索する、ことを特徴とする経路生成装置が提供される。

上記経路生成装置では、前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索するので、移動経路をより確実に見出し、移動装置の移動を継続させられる可能性を効果的に高めることができる。

本発明の好ましい実施形態によると、上記経路生成装置は、移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する障害物検出装置を備え、
移動装置は、移動装置に設けられた速度指令装置から出力された指令速度値に基づいて速度制御されるようになっており、
前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという第２条件と、を含み、
前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記指令速度値を減少させ、移動装置が該減少させられた指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うことで、経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する。

移動経路が見出せない原因として、移動装置が高速で移動することにより移動装置が揺動・振動することで、障害物を誤って検出してしまう場合がある。例えば、揺動・振動により障害物検出装置の検出方向が下方側に変更されてしまい路面自体を障害物として検出してしまう場合がある。
上記構成では、移動経路を見出せなかった場合に、指令速度値を減少させ、移動装置がより低速となっている状態で障害物検出装置が障害物検出を行うので、障害物の誤検出により移動経路が見出せない可能性を効果的に小さくできる。

好ましくは、前記経路探索条件は、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件を含み、
前記速度指令装置は、一定の速度指令値を出力するようになっており、
経路探索装置は、前記許容上限値と、移動装置の速度制御に使用される一定の指令速度値との関係を記憶する記憶装置を有し、
前記経路探索装置は、前記指令速度値を減少させる場合には、前記関係に基づいて許容上限値を増加させる。

許容上限値は、例えば、移動装置が許容上限値以下の曲率の箇所を進行する場合に、移動装置が横滑りおよび横転することなく、当該箇所を進行できるように設定される。
このような場合、移動装置が一定の指令速度値で移動するように速度制御装置に制御されているときには、指令速度値を減少させることで、許容上限値を増加させることができる。従って、上記構成では、前記指令速度を減少させる場合には、許容上限値と一定の指令速度値との前記関係に基づいて許容上限値を増加させることで、移動経路を見出せる可能性をさらに高めることができる。

本発明によると、経路探索条件が上記の第２条件を含んでいなくてもよい。この場合にも、次のように、経路生成装置は、上記構成と一部重複する構成を有してよく、経路探索装置は、上記動作と一部重複するように動作してよい。
即ち、上記経路生成装置は、移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する障害物検出装置を備え、
前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件と、を含み、
前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記許容上限値を増加することで経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する。

このように、障害物検出装置が検出した障害物（例えば、停止車両、穴、岩、壁など）と移動経路とが干渉しないという第１条件を含む経路探索条件の下では、該障害物により移動経路を見出せない場合がある。この場合、上記移動経路の曲率の許容上限値を増加することで、障害物と干渉しない移動経路、即ち、障害物を回避するように比較的大きい曲率・カーブを有する移動経路を見出せる可能性が効果的に高まる。これにより、移動経路を見出し移動装置の移動を継続させられる可能性を効果的に高めることができる。

好ましくは、前記移動経路の曲率を次のκ（Ｓ）とし、

κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋・・・＋ａ_ｎ＋１Ｓ^ｎ

この式において、
ｎは、１または２以上の整数であり、
Ｓは、移動装置の現在位置から前記移動経路上の任意位置までの前記移動経路に沿った経路長さを示し、該Ｓの上限値は、該現在位置から前記移動経路の終了位置までの移動経路に沿った移動経路長さであり、
ａ_１〜ａ_ｎ＋１は、係数であり、
前記経路探索装置は、移動経路探索時に、
（Ａ）前記複数の係数ａ_１〜ａ_ｎ＋１および前記Ｓの上限値にそれぞれ所定の値を与えることで得られるκ（Ｓ）について、該κ（Ｓ）により定まる移動経路が前記経路探索条件を満たすかを判断し、
（Ｂ）前記（Ａ）の処理を複数回行う場合に、前記（Ａ）の処理を行うごとにａ_１〜ａ_ｎ＋１のうち少なくとも１つの値を変更して前記（Ａ）の処理を行い、
（Ｃ）前記（Ａ）または（Ｂ）の処理において、前記経路探索条件を満たす移動経路を見出せなかった場合に、前記許容上限値を増加することで前記経路探索条件を変更し、該変更した経路探索条件の下で前記（Ａ）または（Ｂ）の処理を再び行う。

このように、経路探索条件を満足する移動経路を見出せなかった場合に、上記κ（Ｓ）の値の許容上限値を増加した経路探索条件の下で、前記（Ａ）または（Ｂ）の処理を再び行うことで、移動経路を見出し移動装置の移動を継続させられる可能性を効果的に高めることができる。

本発明の別の実施形態によると、上記経路生成装置は、移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する障害物検出装置を備え、
前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、設定長さを有する１つ以上の移動経路候補から第１条件を満たすものを生成用の移動経路として選択するという第４条件と、を含み、
前記経路探索装置は、経路探索時に、
（ａ）前記１つ以上の移動経路候補が第１条件を満足するかを判断し、
（ｂ）該移動経路候補のいずれも第１条件を満足しないと判断した場合に、前記１つ以上の移動経路候補を変更し、該変更された１つ以上の移動経路候補が前記第１条件を満足するかを判断する。なお、移動経路候補が複数ある場合に、前記設定長さは、複数の移動経路候補の間で同じであっても異なっていてもよい。

このように、障害物検出装置が検出した障害物（例えば、停止車両、穴、岩、壁など）と移動経路とが干渉しないという第１条件を含む経路探索条件の下では、該障害物により移動経路を見出せない場合がある。この場合、移動経路候補を変更することで、障害物と干渉しないように延びる移動経路候補を見出せる可能性が効果的に高まる。

移動経路候補の変更は、例えば、次のように行える。

前記１つ以上の移動経路候補の変更は、
前記設定長さの増加と、
変更した前記１つ以上の移動経路候補の各々における前記現在位置からその任意の途中位置までの経路が、変更前の前記１つ以上の移動経路候補のいずれとも異なることと、
によりなされている。なお、移動経路候補が複数ある場合に、複数の移動経路候補の設定長さの増加は、変更対象となる複数の移動経路候補の間でその増加量または増加率が同じになるようにまたは異なるように、行ってよい。
これにより、障害物を回避してより遠くまで延びる移動経路を見出すことができる。

または、上述した移動経路候補の変更は、より具体的には、例えば、次のように行える。

前記移動経路の曲率を次のκ（Ｓ）とし、

κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋・・・＋ａ_ｎ＋１Ｓ^ｎ

この式において、
ｎは、１または２以上の整数であり、
Ｓは、移動装置の現在位置から前記移動経路上の任意位置までの前記移動経路に沿った経路長さを示し、該Ｓの上限値は、該現在位置から前記移動経路の終了位置までの移動経路に沿った移動経路長さであり、
ａ_１〜ａ_ｎ＋１は、係数であり、
前記経路探索装置は、移動経路探索時に、
（Ａ）前記Ｓの上限値および前記複数の係数ａ_１〜ａ_ｎ＋１にそれぞれ所定の値を与えることで得られるκ（Ｓ）について、該κ（Ｓ）により定まる前記移動経路候補が第１条件を満たすかを判断し、
（Ｂ）前記（Ａ）の処理を複数回行う場合に、前記（Ａ）の処理を行うごとにａ_１〜ａ_ｎ＋１のうち少なくとも１つの値を変更して前記（Ａ）の処理を行い、これにより、複数回の処理で得られる複数の前記移動経路候補について第１条件を満たすかを判断し、
（Ｃ）前記（Ａ）または（Ｂ）の処理において前記移動経路候補のいずれも第１条件を満足しないと判断した場合に、前記Ｓの上限値を増加させ、かつ、前記係数ａ_１〜ａ_ｎ＋１の少なくともいずれかの値を、対応する当該係数に前記（Ａ）または（Ｂ）の処理で与えた値から変更することで、前記１つ以上の移動経路候補を変更し、前記（Ａ）または（Ｂ）の処理を再び行う。

好ましくは、前記経路探索装置は、前記経路探索条件を満足する移動経路を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路を再び探索する処理を繰り返す。

このように、前記経路探索装置は、移動経路を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路を再び探索する処理を繰り返すので、移動装置が進行可能な移動経路をより確実に見出せる。
即ち、移動経路を再び探索する時ごとに経路探索条件を変更するので、移動経路を見出せる可能性を大幅に高めることができ、その結果、より確実に移動装置を継続して自律移動させることが可能になる。

また、上記目的を達成するため、本発明によると、上述した経路生成装置と、該経路生成装置により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備えることを特徴とする移動装置が提供される。
この移動装置は、上述と同様に移動経路をより確実に見出せるので、移動装置の自律移動を継続して行える可能性が効果的に高まる。

また、上記目的を達成するため、本発明によると、自律移動する移動装置の移動経路を生成する経路生成方法であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置と、を設け、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索して生成し、
前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索する、ことを特徴とする経路生成方法が提供される。

上記経路生成方法では、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索するので、移動経路をより確実に見出し、移動装置の移動を継続させられる可能性を効果的に高めることができる。

上述した本発明によると、移動経路をより確実に見出し、移動装置の移動を継続させられる可能性を効果的に高めることができる。

本発明を実施するための最良の実施形態を図面に基づいて説明する。

［第１実施形態］
本実施形態による経路生成装置１０は、図１に示すように、記憶装置３、位置検出装置５、経路探索装置９、障害物検出装置７を備える。

記憶装置３は、移動装置の移動範囲を含む地図に関する地図情報と、該地図における位置を示す経由位置情報（即ち、経由位置）と、を記憶する。地図情報は、２次元の地図に関する情報であってよく、この地図情報は例えばｘ−ｙ座標系であってよく、与えられたｘ座標とｙ座標により地図上の位置が特定されるものであってよい。経由位置情報は、移動装置の移動開始位置から目標到達位置までの間に設定された複数の経由位置の位置情報（例えば、ｘ座標とｙ座標）である。これら複数の経由位置は、移動経路の生成の指標として、経路探索装置９に使用される。なお、複数の経由位置は、移動開始位置と目標到達位置を含んでよい。

位置検出装置５は、移動装置の現在位置を検出する。位置検出装置５は移動装置に設けられる。位置検出装置５は、例えば、ＧＰＳシステムを利用してＧＰＳシステム（ＧＰＳ衛星）から位置に関する情報を受信し、当該情報に基づいて移動装置の現在位置を検出するＧＰＳ装置であってよい。
代わりに、位置検出装置５は、移動装置の移動速度、移動方向を随時検出し、検出した移動速度と移動方向と移動装置の初期位置情報に基づいて、移動装置の現在位置を演算して検出するものであってもよい。

障害物検出装置７は、移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する。移動装置は、移動装置の周囲（例えば、移動装置が進行する側における所定の領域内）において停止車両、凹凸地、壁などの障害物およびその位置・範囲を検出する。障害物検出装置７は、例えば、レーザ光を射出し、このレーザ光の反射光を検出することで障害物を検出するものであってよい。この場合、障害物検出装置７は、移動装置の進行方向に複数のレーザビームを射出するレーザ光源と、これら射出されたレーザビームの反射光を検出して障害物の有無およびその位置・範囲を検出する反射光検出装置と、を有する。レーザ光源は、移動装置の進行方向と垂直な水平方向に関して互いに傾いた角度で、かつ、鉛直下方側に傾けて、複数のレーザビームを射出する。反射光検出装置は、射出されたレーザビームの反射光を受光し、受光した反射光に基づいて障害物の有無およびその位置・範囲を検出する。
なお、障害物検出装置７は、上記のものに限定されず他の公知の装置であってもよい。例えば、レーザ光を用いた障害物検出装置以外に、ＣＣＤカメラなどを用いた装置を障害物検出装置７としてもよい。

経路探索装置９は、移動装置の移動中において、位置検出装置５が検出した上記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索し生成する。経路探索装置９は、コンピュータにより構成されてよい。
本実施形態によると、経路探索装置９は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索する。
移動装置は、移動装置に設けられた速度指令装置から出力された指令速度値に基づいて速度制御されるようになっており、
前記経路探索条件（即ち、初期状態の経路探索条件）は、障害物検出装置７が検出した障害物と移動経路とが干渉しないという第１条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという第２条件と、を含む。この場合、前記経路探索装置９は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記指令速度値を減少させ、移動装置が該減少させられた指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うことで、経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する。
また、前記経路探索条件（即ち、初期状態の経路探索条件）は、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件と、を含んでよい。この場合、上記速度指令装置は、一定の速度指令値を出力するようになっており、経路探索装置９は、前記許容上限値と、移動装置の速度制御に使用される一定の指令速度値との関係を記憶する記憶装置を有し、経路探索装置は、前記指令速度を減少させる場合には、前記関係に基づいて許容上限値を増加させる。なお、許容上限値は、例えば、移動装置が許容上限値以下の曲率の箇所を進行する場合に、移動装置が横滑りおよび横転することなく、当該箇所を進行できるように設定される。

なお、上述では、指令速度値を減少させることで経路探索条件を変更したが、この変更を行わないようにすることもできる。例えば、前記経路探索条件は、第２条件を省いて、第１条件と第３条件を含み、経路探索装置９は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記許容上限値を増加することで経路探索条件を変更してもよい。

また、前記経路探索装置９は、前記経路探索条件を満足する移動経路を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路を再び探索する処理を繰り返す。

次に、経路生成装置１０による移動経路の探索・生成について詳しく説明する。なお、以下の説明は、経路探索条件が第１条件、第２条件および第３条件を含む場合と、第２条件を省き経路探索条件が第１条件および第３条件を含む場合の両方に対応している。

移動経路の曲率をκ（Ｓ）とする。このκ（Ｓ）は次の式で表すことができる（例えば、非特許文献１）。

κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋・・・＋ａ_ｎ＋１Ｓ^ｎ

この式において、
ｎは、１または２以上の整数であり、
Ｓは、移動装置の現在位置から前記移動経路上の任意位置までの前記移動経路に沿った経路長さを示し、該Ｓの上限値は、該現在位置から前記移動経路の終了位置までの移動経路に沿った移動経路長さであり、
ａ_１〜ａ_ｎ＋１は、係数である。

上記κ（Ｓ）の式により移動経路（移動経路候補）を特定できる。即ち、Ｓをパラメータとして、移動経路の方位角（即ち、移動経路の接線方向）と移動経路上の位置とを次の［数１］で求めることができる。

図２は、経路生成装置１０による経路生成方法を示すフローチャートである。

ステップＳ１では、経路探索装置９は、（例えば、２次元の）局所地図を生成する。即ち、経路探索装置９は、位置検出装置５により検出された移動装置の現在位置と障害物検出装置７による障害物探索領域とを含む範囲の地図情報を記憶装置３から抽出し、当該範囲の局所地図を生成する。この局所地図には、次の経由位置への方向情報が組み込まれていてよい。方向情報は、例えば、直前に通過した経由位置と次の経由位置とを結ぶ直線の方向であってよく、または、移動装置の現在位置から見た次の経由位置の方向であってもよい。この局所地図には、次の経由位置自体が組み込まれていてもよい。
なお、ステップＳ１で使用される現在位置と、障害物検出装置７による障害物検出結果情報（障害物の位置と範囲）とは、ステップＳ１を開始する毎に、それぞれ位置検出装置５および障害物検出装置７により獲得され、局所地図に組み込まれる。

ステップＳ２では、移動経路を探索する。具体的には、前記経路探索装置９は、上記κ（Ｓ）の式を用いて、次の処理（Ａ）または（Ｂ）を行う。
（Ａ）前記複数の係数ａ_１〜ａ_ｎ＋１および前記Ｓの上限値にそれぞれ所定の値を与えることで得られるκ（Ｓ）について、該κ（Ｓ）により定まる移動経路が前記経路探索条件（第１条件と第３条件）を満たすかを判断する。
（Ｂ）前記（Ａ）の処理を複数回行う場合に、前記（Ａ）の処理を行うごとにａ_１〜ａ_ｎ＋１のうち少なくとも１つの値を変更して前記（Ａ）の処理を行う。なお、前記Ｓの上限値は一定にしたまま前記（Ａ）の処理を複数回行ってよいし、複数の前記（Ａ）の処理うち少なくとも１回の処理で前記Ｓの上限値を変更してもよい。
経路探索装置９が、前記（Ａ）または（Ｂ）の処理により、前記経路探索条件を満たす移動経路を見出せなかった場合には、ステップＳ３に進み、経路探索条件を満たす移動経路を見出せた場合、ステップＳ４へ進む。

ステップＳ３では、経路探索条件を変更してステップＳ１に戻る。具体的には、ステップＳ３で、経路探索装置９は、前記指令速度値を減少させ、ステップＳ１で、移動装置が該減少させられた指令速度値に基づいて速度制御されている状態で障害物検出装置７が障害物検出を再び行い。この障害物検出結果に基づいて上記局所地図を生成する。この時、移動装置の現在位置も位置検出装置５により再び検出され、この現在位置と再度行われた障害物検出結果とに基づいて局所地図が生成されてよい。
また、ステップＳ３では、上記速度指令装置は一定の速度指令値を出力することで移動装置が一定速度で移動するように制御される場合には、経路探索装置９は、前記指令速度値を減少さることに伴い、前記許容上限値と、移動装置の速度制御に使用される一定の指令速度値との関係に基づいて許容上限値を増加させる。
なお、ステップＳ３において、指令速度値を減少させることを行わないようにしてもよい。即ち、上記関係を用いずに、単に前記許容上限値を増加することで経路探索条件を変更してもよい。この場合には、図１と異なり、ステップＳ３からステップＳ１を経由せずにステップＳ２へ戻ってもよいし、図１のように、ステップＳ３からステップＳ１へ戻って新たな現在位置と障害物検出結果とに基づいて再び局所地図を生成してもよい。

ステップＳ４では、ステップＳ２で見出した移動経路を設定して設定移動経路を更新する。即ち、移動装置の進行制御にこれまで使用していた設定移動経路を、ステップＳ２で新たに見出した移動経路に置き換えることで、設定移動経路を更新する。なお、設定移動経路は記憶装置３に記憶される。更新したらステップＳ５へ進む。

ステップＳ５では、経路探索装置９は、ステップＳ１で、局所地図の生成を開始してから所定時間（例えば、１００ｍｓ）経過したかを、経路探索装置９に内蔵されたタイマーの時間計測により判断する。所定時間経過したと判断した場合には、ステップＳ１に戻り、所定時間経過していないと判断した場合にはステップＳ５の判断を繰り返す。
これにより、所定時間経過毎に、局所地図の生成および経路探索を行う。これに対応して、位置検出装置５は、上記の所定時間経過毎に移動装置の現在位置を検出し、障害物検出装置７は、上記の所定時間経過毎に障害物の検出を行う。
ステップＳ５からステップＳ１に戻りステップＳ２へ戻った時、当該時点での設定移動経路が経路探索条件を変更して得られたものである場合には、このステップＳ２での経路探索は、第２条件以外は初期の経路探索条件（例えば、初期の第１条件と第３条件）に戻して行う。ステップＳ２へ戻った時点での設定移動経路が初期状態の第３条件で移動経路が見出されたものである場合には、当該ステップＳ２で、第２条件も初期状態に戻して経路探索を行う。
また、速度指令装置が一定の指令速度値を出力する場合、次の処理を行う。ステップＳ３で指令速度値を減少させたとき、当該時点から、経路探索装置９は、指令速度値を減少させる処理を継続させ、これにより、移動装置は速度指令装置からの一定指令速度よりも低速で移動させられる。なお、この間において、ステップＳ３で指令速度値を再び減少させる場合には、指令速度値の減少量を増加させることで経路探索条件（第２条件）をさらに変更する。この場合、経路探索装置９は、指令速度値を（増加させた）当該減少量だけ減少させる処理を継続させ、これにより、移動装置は速度指令装置からの一定指令速度よりもさらに低速で移動させられる。ただし、ステップＳ２にて、初期状態の第３条件の下で、移動経路が見出された場合には、この移動経路が設定された時点以降は、経路探索装置９は、指令速度値を減少させる処理を行わない。

なお、ステップＳ１〜Ｓ３、Ｓ５の処理の最中に、移動装置の制御装置は、記憶装置３に記憶された上記の設定移動経路に沿って移動装置が移動するように移動装置を制御する。また、

［第１実施形態の作用効果］
上述した第１実施形態の経路生成装置と方法によると、前記経路探索装置９は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索するので、移動経路をより確実に見出し、移動装置の移動を継続させられる可能性を効果的に高めることができる。

また、移動経路が見出せない原因として、移動装置が高速で移動することにより移動装置が揺動・振動することで、障害物を誤って検出してしまう場合がある。例えば、揺動・振動により障害物検出装置の検出方向が下方側に変更されてしまい路面自体を障害物として検出してしまう場合がある。
これに対し、第１実施形態では、移動経路を見出せなかった場合に、指令速度値を減少させ、移動装置がより低速で移動している状態で障害物検出装置７が障害物検出を行うので、障害物の誤検出により移動経路が見出せない可能性を効果的に小さくできる。

許容上限値は、例えば、移動装置が許容上限値以下の曲率の箇所を進行する場合に、移動装置が横滑りおよび横転することなく、当該箇所を進行できるように設定される。
このような場合、移動装置が一定の指令速度値で移動するように速度制御装置に制御されているときには、指令速度値を減少させることで、許容上限値を増加させることができる。従って、上記構成では、前記指令速度を減少させる場合には、許容上限値と一定の指令速度値との前記関係に基づいて許容上限値を増加させることで、移動経路を見出せる可能性をさらに高めることができる。

さらに、移動装置が一定の指令速度値で移動するように速度制御装置に制御されている場合には、指令速度値を減少させることで、許容上限値を増加させることができる。従って、前記指令速度を減少させる場合には、許容上限値と一定の指令速度値との前記関係に基づいて許容上限値を増加させることで、移動経路を見出せる可能性をさらに高めることができる。

また、障害物検出装置７が検出した障害物（例えば、停止車両、穴、岩、壁など）と移動経路とが干渉しないという第１条件を含む経路探索条件の下では、該障害物により移動経路を見出せない場合がある。この場合、上記移動経路の曲率の許容上限値を増加することで、障害物と干渉しない移動経路、即ち、障害物を回避するように比較的大きい曲率・カーブを有する移動経路を見出せる可能が効果的に高まる。これにより、移動経路を見出し移動装置の移動を継続させられる可能性を効果的に高めることができる。この場合、経路探索条件は第２条件を含んでいなくてもよく、経路探索条件の変更は、前記指令速度値の減少により行われなくてもよい。

また、前記経路探索装置９は、移動経路を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路を再び探索する処理を繰り返すので、移動装置が進行可能な移動経路をより確実に見出せる。
即ち、移動経路を再び探索する時ごとに経路探索条件を変更するので、移動経路を見出せる可能性を大幅に高めることができ、その結果、より確実に移動装置を継続して自律移動させることが可能になる。例えば、図２に示すように、ステップＳ２で移動経路が見出せるまで、ステップＳ３を通過する度に、上述のように指令速度を減少させることで、ステップＳ３を通過する度に、速度制御装置はより減少した指令速度値に基づいて移動装置の速度制御を行う。これにより、障害物の誤検出が解消されるまで移動装置が段階的に減速されることになる。

［第１実施形態の実施例］
次に、第１実施形態の実施例について説明する。この実施例では、移動装置は車両であってよい。

経路探索装置９は、障害物検出装置７が実際に検出した障害物の範囲を所定の大きさだけ拡大して上記局所地図上で認識する。即ち、障害物検出装置７が実際に検出した障害物の範囲が、所定の大きさ（例えば、車両幅の半分の大きさ＋局所地図上の１ピクセル）だけすべての方向において拡大されて、局所地図上で認識・設定される。

本実施例によると、移動経路の曲率を上述のようにκ（Ｓ）とするが、κ（Ｓ）をＳの３次多項式とする。即ち、κ（Ｓ）を次の式で表す。

κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋ａ_４Ｓ^３

この式において、
Ｓは、移動装置の現在位置から前記移動経路上の任意位置までの前記移動経路に沿った経路長さを示し、該Ｓの上限値は、該現在位置から前記移動経路の終了位置までの移動経路に沿った移動経路長さであり、
ａ_１〜ａ_４は、パラメータ（係数）である。

上記κ（Ｓ）によって、移動経路を表すことができる。即ち、ａ_１〜ａ_４に所定の値を与えることにより、言い換えると、ａ_１〜ａ_４の値が定まると移動経路が定まる。本実施例では、次の３つの境界条件を与えることで、例えば、ａ_１、ａ_３、ａ_４をａ_２の関数で表現できる。

境界条件：
（１）移動経路の開始点、即ち、移動装置の現在位置における曲率についての境界条件

κ（０）＝κ_０＝ａ_１

（２）探索・生成対象の移動経路（移動経路候補）の終了点Ｓ_ｆ（即ち、Ｓの上限値）における曲率についての境界条件

κ（Ｓ_ｆ）＝κ_ｆ＝ａ_１＋ａ_２Ｓ_ｆ＋ａ_３Ｓ_ｆ ^２＋ａ_４Ｓ_ｆ ^３

（３）探索・生成対象の移動経路（移動経路候補）の終了点Ｓ_ｆにおける移動経路方向（移動経路の接線方向）についての境界条件

θ（Ｓ_ｆ）＝θ_ｆ＝ａ_１Ｓ_ｆ＋ａ_２Ｓ_ｆ ^２／２＋ａ_３Ｓ_ｆ ^３／３＋ａ_４Ｓ_ｆ ^４／４

なお、κ_０，κ_ｆ，θ_ｆは、既知であるとする。また、好ましくは、κ_ｆは、ゼロであり、θ_ｆは、移動経路の終了点と次の経由地点を結ぶ直線と同じ方向である。

上記境界条件から，

ａ_３Ｓ_ｆ ^２＋ａ_４Ｓ_ｆ ^３＝κ_ｆ−κ_０−ａ_２Ｓ_ｆ

ａ_３Ｓ_ｆ ^３／３＋ａ_４Ｓ_ｆ ^４／４＝θ_ｆ−κ_０Ｓ_ｆ−ａ_２Ｓ_ｆ ^２／２

が得られ、従って、次の関係式１が得られる。
［関係式１］

ａ_１＝κ_０

ａ_３＝（−３κ_ｆ−９κ_０）／Ｓ_ｆ ^２−３ａ_２／Ｓ_ｆ＋１２θ_ｆ／Ｓ_ｆ ^３

ａ_４＝４（κ_ｆ＋２κ_０）／Ｓ_ｆ ^３＋２ａ_２／Ｓ_ｆ ^２−１２θ_ｆ／Ｓ_ｆ ^４

よって、ａ_２をパラメータとして移動経路（軌道）を一意に求めることができる。これら、ａ_１、ａ_３およびａ_４の各々とａ_２、κ_０、κ_ｆ、Ｓ_ｆとの上記関係式１は記憶装置３に記憶され、経路探索時に使用される。なお、上記のような境界条件を設定することで、ａ_２の値を与えると、ａ_１、ａ_１、ａ_４を短時間の演算で求めることができる。その結果、κ（Ｓ）により移動経路候補を短時間で特定できる。

この実施例では、経路探索条件は、障害物検出装置７が検出した障害物と移動経路とが干渉しないという第１条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという第２条件と、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件と、設定長さを有する１つ以上の移動経路候補から第１条件を満たすものを生成用の移動経路として選択するという第４条件と、を含む。

（第１条件に関連したUnknown領域ついて）
経由位置が予め車両が走行可能な領域（例えば、舗装路）に設定されている場合に、雨上がり時の小さな水溜りや、穴などの小さな窪みなどが存在する領域に対して障害物検出装置７が障害物検出を行うと、レーザ光の反射光が障害物の有無を認識できるように障害物検出装置７に帰ってこない場合がある。従って、水溜りや穴などの窪みが、本来、移動装置が走行できるものであっても、当該領域は、障害物の有無が認識できない領域（以下、Unknown領域という）となる。
本実施例では、Unknown領域により実際には進行可能な領域が排除されないようにするため、移動経路候補上において、Unknown領域が占める割合が、所定の閾値以上になった場合に、移動経路候補が障害物と干渉すると判断し、この移動経路候補を排除する。当該所定の閾値は、車両が走行する領域の環境と生成する移動経路の長さに基づいて定めてよく、例えば数十パーセントまたは数パーセントであってよい。これにより、実際には車両が進行可能である移動経路候補が排除されてしまう可能性を適切に下げることができる。

（第２条件について）
上記のステップＳ３において、指令速度値を減少させ、移動装置が減少させられた前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うようにすることで、第２条件の変更を行う。

（第３条件について）
移動装置が走行する装置（例えば、車両）である場合、曲率κ（Ｓ）の許容上限値は、例えば、次の、κturn、κslip、κrolloverのうちの最も小さいものである。
（１）車両の最小旋回可能半径を示す移動経路の曲率κturn。
（２）車両が横滑りを起こさない限界の曲率κslip。即ち、移動経路上における曲率κslipより大きい曲率κ（Ｓ）の箇所で車両が横滑りする。
（３）車両が横転を起こさない限界の曲率κrollover。即ち、移動経路上における曲率κrolloverより大きい曲率κ（Ｓ）の箇所で車両が横転する。
なお、通常は、曲率κslipおよび曲率κrolloverの値は、曲率κturnの値よりも小さい。また、曲率κslipおよび曲率κrolloverの値は、車両の速度により変化し、一般的に、車両の速度が大きいほど減少する。また、曲率κslipの値は、車両とその走行面との動摩擦係数にも依存する。
従って、κ（Ｓ）が次の大小関係を満足する場合には、第３条件が満たされることになる。

κ（Ｓ）≦κturn、かつ、κ（Ｓ）≦κslip、かつ、κ（Ｓ）≦κrollover、

このような曲率κslipおよび曲率κrolloverは移動装置（例えば、車両）の特性などに基づいて求めることができる。参考のために述べると、曲率κslipは、例えば非特許文献２に記載された式（２）であってもよく、曲率κrolloverは、非特許文献２に記載された式（４）であってもよい。

（第４条件について）
経路探索装置９は、上記κ（Ｓ）により定められる移動経路を探索する時に、Ｓの上限値Ｓ_ｆを設定長さの値に固定して、上記パラメータａ_２の値を与えることで、上記関係式１からａ_１、ａ_３、ａ_４の値を定め、これにより、定まる移動経路候補が第１条件と第３条件を満たすかを判断する。

（設定値について）
経路探索装置９は、経路探索に使用するパラメータ・係数などに所定の値を与えて経路探索を行う。そのために、記憶装置３は、予め設定されたこれらパラメータ・係数の値を記憶する。具体的には、記憶装置３は、経路長Ｓの上限値Ｓ_ｆ、ａ_２、κ_０，κ_ｆ，θ_ｆなどの各設定値を記憶する。
また、複数の移動経路候補を生成するために、ａ_２の複数の値を設定しておく。例えば、ａ_２の最小値ａ_２minおよびａ_２の最大値ａ_２maxと、この最小値からのａ_２の増加量ｄａ_２とを設定しておき、経路探索装置９は、ａ_２min、ａ_２min＋ｄａ_２、ａ_２min＋２ｄａ_２、ａ_２＋３ｄａ_２・・・、ａ_２maxのそれぞれついて移動経路候補が定まるようにしておく。これにより、複数（例えば、２１本）の移動経路候補を生成できる。
記憶装置３は、曲率κ（Ｓ）の許容上限値を記憶する。
また、κslipやκrolloverが移動装置の速度などの変数に依存する場合には、これら変数とκslipとの関係式と、これら変数とκrolloverとの関係式も記憶装置３に記憶させる。これにより、経路探索装置９は、任意の時に、これら変数の値に基づいて当該関係式を用いてκslipとκrolloverを演算し、κturn、κslip、κrolloverの大小を比較し、これらのうち最大のものを許容上限値として特定する。なお、κturnは記憶装置３に記憶された固定値であってよい、
さらに、記憶装置３は、unknown領域に関する上述の所定の閾値、後述する指令速度値減少量・減少率、および経路長増加量・増加率などを記憶する。

次に、本実施例による移動経路探索方法について説明する。この方法は、図２に示すフローチャートと同じ方法であるが、この実施例では、図２の方法の一例としてより詳しく説明する。以下では、上述しなかった点を詳しく述べるが、以下で述べない点は、上述と同じ内容であってよい。

ステップＳ１では、上述のように局所地図を生成する。

ステップＳ２では、移動経路を探索する。具体的には、上述のように設定した複数のａ_２のそれぞれをκ（Ｓ）の式に適用する。ａ_１、ａ_３、ａ_４の値は、ａ_２の各設定値ごとに関係式１のようにａ_２、κ_０、κ_ｆ、Ｓ_ｆ、θ_ｆを用いて得られる。このようにして、複数の移動経路候補が生成される。
これら移動経路候補が、第１条件および第３条件を満足するかを判断する。具体的には、第１条件に関して、移動経路候補上にUnknown領域が存在しない場合には、上述のように移動経路候補が第１条件を満たすかを判断する。移動経路候補上にUnknown領域が存在する場合、移動経路候補上においてUnknown領域が占める割合が所定の閾値より小さいかの判断をしてよい。この割合が所定の閾値より小さくない場合には、第１条件が満たされないと判断してよい。第３条件については、各移動経路候補について、κ（Ｓ）が、０≦Ｓ≦Ｓ_ｆの全範囲にわたって、上述したκ（Ｓ）の許容上限値以下であるかを判断する。許容上限値以下である場合には、第３条件が満たされることになる。
このようにして、第１条件および第３条件を満たす移動経路候補が存在しない場合には、即ち、移動経路候補のいずれもが第１条件および第３条件の両方を満足しない場合には、移動経路が見出されなかったことになり、ステップＳ３へ進む。一方、第１条件および第３条件を満たす移動経路候補が存在する場合には、移動経路が見出されたことになり、ステップＳ４へ進む。

ステップＳ３では、経路探索条件を変更する。この実施例では、上述の指令速度値の減少に加えて、上記許容上限値を増加し、かつ、経路長さＳの上限値Ｓ_ｆの値も増加させることで、経路探索条件を変更する。なお、指令速度値の減少は、記憶装置３に記憶された指令速度値減少量・減少率に基づいて行い、許容上限値の増加は、記憶装置３に記憶された許容上限値拡大量・拡大率に基づいて行い、上限値Ｓ_ｆの値の増加は、記憶装置３に記憶されや経路長増加量・増加率に基づいて行う。経路探索条件を変更したら、ステップＳ２へ戻り、変更した経路探索条件の下で、経路探索を行う。この再度の経路探索処理は、経路探索条件を変更した以外は、前回、ステップＳ２で行った処理と同じである。
このようにすることで、前回の経路探索では、移動経路候補上に障害物が存在していた場合でも、Ｓ_ｆを増加させることで、上記関係式１によりａ_３、ａ_４の値も変化し、κ（Ｓ）により定まる移動経路候補の経路も前回と異なったものとなる。しかも、生成される移動経路候補の全長も大きくなる。これにより、例えば図３に模式的に示すように、障害物を回避するように延びる移動経路候補が生成される可能性が効果的に高まる。なお、図３において、複数の細い曲線は、前回生成された移動経路候補を示し、太い曲線は今回生成された移動経路候補（図３では、簡単のため１本のみ）を示す。

ステップＳ４では、ステップＳ２で見出した第１条件および第３条件を満たす移動経路候補を新たな移動経路として、上述と同様に設定移動経路を更新する。
なお、ステップＳ４において、ステップＳ２で見出した第１条件および第３条件を満たす移動経路候補が複数存在する場合には、経路探索装置９は、直前に通過した前回の経由位置と次の経由位置とを結ぶ直線を基準直線とし、この基準直線からの距離の積算値を求める。経路探索装置９は、積算値が最も小さくなる移動経路候補を選択し、選択した移動経路候補を新たな移動経路として、上述と同様に設定移動経路を更新し、ステップＳ５へ進む。
上記積算値は、例えば図４に示すすべての長さｄＬの総和である。具体的には、図４において、基準直線と対象の移動経路候補上の位置（ｘ（Ｓ），ｙ（Ｓ））との距離ｄＬを、移動経路候補の開始点から終了点までにわたって、この移動経路候補に沿った距離ｄＳごとの各位置に関して求め、求めた値ｄＬの総和が上記積算値である。この時、基準直線の一方側にある移動経路候補上の位置についてはｄＬを正の値とし、基準直線の他方側にある移動経路候補上の位置についてはｄＬを負の値として、積算値を求める。これにより、次の経由位置に向かうための最短の移動経路を選択することが可能になる。

ステップＳ５では、上述のように局所地図の生成を開始してから所定時間（例えば、１００ｍｓ）経過したかを判断する。所定時間経過したと判断した場合には、ステップＳ１に戻り、所定時間経過していないと判断した場合にはステップＳ５の判断を繰り返す。

本実施例では、移動装置（この例では、車両）が一定指令速度値で移動するように移動装置の制御装置により制御されている場合には、次の構成・処理を採用してもよい。
即ち、移動装置の制御装置（速度制御装置）は、移動装置に設けられた速度指令装置から出力される一定指令速度値と、移動装置に設けられた速度検出装置に検出された移動装置の速度値とに基づいて、移動装置の速度を制御する。これにより、移動装置が一定指令速度値で走行するように制御される。

［第２実施形態］
本発明の第２実施形態について説明する。第２実施形態による経路生成装置と方法は以下で述べる点で、第１実施形態と異なる。第２実施形態による経路生成装置と方法の他の内容は第１実施形態と同じであってよい。

第２実施形態によると、前記経路探索装置９は、経路探索時に、次の（ａ），（ｂ）の処理を行う。
（ａ）前記経路探索装置９は、前記１つ以上の移動経路候補が第１条件を満足するかを判断する。
（ｂ）該移動経路候補のいずれも第１条件を満足しないと判断した場合に、前記１つ以上の移動経路候補を変更し、該変更された１つ以上の移動経路候補が前記第１条件を満足するかを判断する。

前記１つ以上の移動経路候補の変更は、前記設定長さの増加と、変更した前記１つ以上の移動経路候補の各々における前記現在位置からその任意の途中位置までの経路が、変更前の前記１つ以上の移動経路候補のいずれとも異なることと、によりなされている。

第２実施形態によると、第１実施形態で述べたステップＳ２の処理を、第１実施形態と異なり次のように行う。
即ち、前記経路探索装置９は、上記κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋・・・＋ａ_ｎ＋１Ｓ^ｎの式を用いて、次の処理（Ａ）または（Ｂ）を行う。
（Ａ）前記Ｓの上限値および前記複数の係数ａ_１〜ａ_ｎ＋１にそれぞれ所定の値を与えることで得られるκ（Ｓ）について、該κ（Ｓ）により定まる前記移動経路候補が第１条件を満たすかを判断する。
（Ｂ）前記（Ａ）の処理を複数回行う場合に、前記（Ａ）の処理を行うごとにａ_１〜ａ_ｎ＋１のうち少なくとも１つの値を変更して前記（Ａ）の処理を行い、これにより、複数回の処理で得られる複数の前記移動経路候補について第１条件を満たすかを判断する。

また、第２実施形態によると、ステップＳ２において、前記（Ａ）または（Ｂ）の処理において前記移動経路候補のいずれも第１条件を満足しないと判断した場合に、ステップＳ３へ進む。
第２実施形態では、ステップＳ３において、前記Ｓの上限値を増加させ、かつ、前記係数ａ_１〜ａ_ｎ＋１の少なくともいずれかの値を、対応する当該係数に前記（Ａ）または（Ｂ）の処理で与えた値から変更することで、前記１つ以上の移動経路候補を変更し、前記（Ａ）または（Ｂ）の処理を再び行う。

［第２実施形態の作用効果］
上述した第２実施形態の経路生成装置と方法によると、前記経路探索装置９は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索するので、移動経路をより確実に見出し、移動装置の移動を継続させられる可能性を効果的に高めることができる

また、障害物検出装置７が検出した障害物（例えば、停止車両、穴、岩、壁など）と移動経路とが干渉しないという第１条件を含む経路探索条件の下では、該障害物により移動経路を見出せない場合がある。この場合、移動経路候補を変更することで、障害物と干渉しないように延びる移動経路候補を見出せる可能性が効果的に高まる。

さらに、第１実施形態と同様に、前記経路探索装置９は、移動経路を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路を再び探索する処理を繰り返すので、移動装置が進行可能な移動経路をより確実に見出せる。

［経路生成装置を備える移動装置］
また、自律移動する移動装置は、第１実施形態または第２実施形態の経路生成装置１０と、該経路生成装置１０により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備える。
制御装置は、位置検出装置５により検出された現在位置、上記生成された移動経路（即ち、記憶装置３に設定されている移動経路）、上述の速度指令装置から出力される指令速度値（例えば、一定指定速度値であってもよい）、移動装置の現在の速度、および、進行方向などに基づいて移動装置の速度制御を行う上述の速度制御装置を有する。また、移動装置は、位置検出装置５により検出された現在位置、上記生成された移動経路、速度指令装置から出力される指令速度値（例えば、一定指定速度値であってもよい）、移動装置の現在の速度および進行方向などに基づいて移動装置の進行方向制御を行う方向制御装置を有する。速度制御装置は、経路探索装置９が指令速度値を減少させる場合には、減少させられや指令速度値に基づいて移動装置の速度制御を行う。
なお、速度指令装置は、移動装置の現在位置における設定移動経路の曲率に基づいて、指令速度値を出力してもよい。この場合、速度指令装置は、曲率が大きいほど小さい速度指令値（単純な例では、曲率の大きさに比例して小さくなる速度指令値）を出力しておい。
この移動装置は、上述と同様に移動経路をより確実に見出せるので、移動装置の自律移動を継続して行える。

本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。例えば、障害物を検出した時、所定の時間間隔で、各経由位置を通過する毎に移動経路を探索生成してよい。移動装置は、上述した実施例では車両であったが、本発明はこれに限定されない。例えば、移動装置は、自律走行または自律歩行するロボットであってもよい。

本発明の第１実施形態による経路生成装置の構成図である。 本発明の第１実施形態による経路生成方法を示すフローチャートである。 経路探索装置が生成する局所地図である。 積算長の計算方法を説明するための図である。

符号の説明

３ 記憶装置、５ 位置検出装置、７ 障害物検出装置
９ 経路探索装置、１０ 経路生成装置

Claims (11)

1. 自律移動する移動装置の移動経路を生成する経路生成装置であって、
   移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
   前記移動装置の現在位置を検出する位置検出装置と、
   移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索し生成する経路探索装置と、を備え、
   前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索し、
   移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する障害物検出装置を備え、
   移動装置は、速度指令装置から出力された指令速度値に基づいて速度制御されるようになっており、
   前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという第２条件と、を含み、
   前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記指令速度値を減少させ、移動装置が該減少させられた指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うことで、経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する、ことを特徴とする経路生成装置。
2. 自律移動する移動装置の移動経路を生成する経路生成装置であって、
   移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
   前記移動装置の現在位置を検出する位置検出装置と、
   移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索し生成する経路探索装置と、を備え、
   前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索し、
   移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する障害物検出装置を備え、
   前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件と、を含み、
   前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記許容上限値を増加することで経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する、ことを特徴とする経路生成装置。
3. 自律移動する移動装置の移動経路を生成する経路生成装置であって、
   移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
   前記移動装置の現在位置を検出する位置検出装置と、
   移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索し生成する経路探索装置と、を備え、
   前記経路探索装置は、前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索し、
   移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する障害物検出装置を備え、
   前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、設定長さを有する１つ以上の移動経路候補から第１条件を満たすものを生成用の移動経路として選択するという第４条件と、を含み、
   前記経路探索装置は、経路探索時に、
   （ａ）前記１つ以上の移動経路候補が第１条件を満足するかを判断し、
   （ｂ）該移動経路候補のいずれも第１条件を満足しないと判断した場合に、前記１つ以上の移動経路候補を変更し、該変更された１つ以上の移動経路候補が前記第１条件を満足するかを判断し、
   前記１つ以上の移動経路候補の変更は、
   前記設定長さの増加と、
   変更した前記１つ以上の移動経路候補の各々における前記現在位置からその途中位置までの経路が、変更前の前記１つ以上の移動経路候補のいずれとも異なることと、
   によりなされている、ことを特徴とする経路生成装置。
4. 前記経路探索条件は、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件を含み、
   前記速度指令装置は、一定の指令速度値を出力するようになっており、
   経路探索装置は、前記許容上限値と、移動装置の速度制御に使用される一定の指令速度値との関係を記憶する記憶装置を有し、
   前記経路探索装置は、前記指令速度値を減少させる場合には、前記関係に基づいて許容上限値を増加させる、ことを特徴とする請求項１に記載の経路生成装置。
5. 前記移動経路の曲率を次のκ（Ｓ）とし、
   
   κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋・・・＋ａ_ｎ＋１Ｓ^ｎ
   
   この式において、
   ｎは、１または２以上の整数であり、
   Ｓは、移動装置の現在位置から前記移動経路上の任意位置までの前記移動経路に沿った経路長さを示し、該Ｓの上限値は、該現在位置から前記移動経路の終了位置までの移動経路に沿った移動経路長さであり、
   ａ_１〜ａ_ｎ＋１は、係数であり、
   前記経路探索装置は、移動経路探索時に、
   （Ａ）前記複数の係数ａ_１〜ａ_ｎ＋１および前記Ｓの上限値にそれぞれ所定の値を与えることで得られるκ（Ｓ）について、該κ（Ｓ）により定まる移動経路が前記経路探索条件を満たすかを判断し、
   （Ｂ）前記（Ａ）の処理を複数回行う場合に、前記（Ａ）の処理を行うごとにａ_１〜ａ_ｎ＋１のうち少なくとも１つの値を変更して前記（Ａ）の処理を行い、
   （Ｃ）前記（Ａ）または（Ｂ）の処理において、前記経路探索条件を満たす移動経路を見出せなかった場合に、前記許容上限値を増加することで前記経路探索条件を変更し、該変更した経路探索条件の下で前記（Ａ）または（Ｂ）の処理を再び行う、ことを特徴とする請求項２または４に記載の経路生成装置。
6. 前記移動経路の曲率を次のκ（Ｓ）とし、
   
   κ（Ｓ）＝ａ_１＋ａ_２Ｓ＋ａ_３Ｓ^２＋・・・＋ａ_ｎ＋１Ｓ^ｎ
   
   この式において、
   ｎは、１または２以上の整数であり、
   Ｓは、移動装置の現在位置から前記移動経路上の任意位置までの前記移動経路に沿った経路長さを示し、該Ｓの上限値は、該現在位置から前記移動経路の終了位置までの移動経路に沿った移動経路長さであり、
   ａ_１〜ａ_ｎ＋１は、係数であり、
   前記経路探索装置は、移動経路探索時に、
   （Ａ）前記Ｓの上限値および前記複数の係数ａ_１〜ａ_ｎ＋１にそれぞれ所定の値を与えることで得られるκ（Ｓ）について、該κ（Ｓ）により定まる前記移動経路候補が第１条件を満たすかを判断し、
   （Ｂ）前記（Ａ）の処理を複数回行う場合に、前記（Ａ）の処理を行うごとにａ_１〜ａ_ｎ＋１のうち少なくとも１つの値を変更して前記（Ａ）の処理を行い、これにより、複数回の処理で得られる複数の前記移動経路候補について第１条件を満たすかを判断し、
   （Ｃ）前記（Ａ）または（Ｂ）の処理において前記移動経路候補のいずれも第１条件を満足しないと判断した場合に、前記Ｓの上限値を増加させ、かつ、前記係数ａ_１〜ａ_ｎ＋１の少なくともいずれかの値を、対応する当該係数に前記（Ａ）または（Ｂ）の処理で与えた値から変更することで、前記１つ以上の移動経路候補を変更し、前記（Ａ）または（Ｂ）の処理を再び行う、ことを特徴とする請求項３に記載の経路生成装置。
7. 前記経路探索装置は、前記経路探索条件を満足する移動経路を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路を再び探索する処理を繰り返す、ことを特徴とする請求項１〜６のいずれかに記載の経路生成装置。
8. 請求項１〜７のいずれかに記載の経路生成装置と、該経路生成装置により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備えることを特徴とする移動装置。
9. 自律移動する移動装置の移動経路を生成する経路生成方法であって、
   移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置と、を設け、
   移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索して生成し、
   前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索し、
   移動装置の進行の妨げとなる障害物を検出する障害物検出装置が、移動装置に設けられており、
   移動装置は、速度指令装置から出力された指令速度値に基づいて速度制御されるようになっており、
   前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという第２条件と、を含み、
   前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記指令速度値を減少させ、移動装置が該減少させられた指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うことで、経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する、ことを特徴とする経路生成方法。
10. 自律移動する移動装置の移動経路を生成する経路生成方法であって、
    移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置と、を設け、
    移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索して生成し、
    前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索し、
    移動装置の進行の妨げとなる障害物を検出する障害物検出装置が、移動装置に設けられており、
    前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、前記移動経路の曲率が該移動経路の全範囲にわたって許容上限値以下になるという第３条件と、を含み、
    前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記許容上限値を増加することで経路探索条件を変更し、該変更した経路探索条件の下で移動経路を再び探索する、ことを特徴とする経路生成方法。
11. 自律移動する移動装置の移動経路を生成する経路生成方法であって、
    移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置と、を設け、
    移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を所定の経路探索条件の下で探索して生成し、
    前記経路探索条件の下で前記移動経路を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路を再び探索し、
    移動装置の進行の妨げとなる障害物を検出する障害物検出装置が、移動装置に設けられており、
    前記経路探索条件は、障害物検出装置が検出した障害物と移動経路とが干渉しないという第１条件と、設定長さを有する１つ以上の移動経路候補から第１条件を満たすものを生成用の移動経路として選択するという第４条件と、を含み、
    経路探索時に、
    （ａ）前記１つ以上の移動経路候補が第１条件を満足するかを判断し、
    （ｂ）該移動経路候補のいずれも第１条件を満足しないと判断した場合に、前記１つ以上の移動経路候補を変更し、該変更された１つ以上の移動経路候補が前記第１条件を満足するかを判断し、
    前記１つ以上の移動経路候補の変更を、
    前記設定長さの増加と、
    変更した前記１つ以上の移動経路候補の各々における前記現在位置からその途中位置までの経路が、変更前の前記１つ以上の移動経路候補のいずれとも異なることと、
    により行う、ことを特徴とする経路生成方法。

Images