経路生成装置と方法および経路生成装置を備える移動装置
【課題】移動経路の探索において、より高速で移動可能な移動経路を見つけ出すことで、移動装置の高速移動を可能にすることにある。
【解決手段】自律移動する移動装置の移動経路を生成する経路生成装置10であって、移動開始位置から目標到達位置までの間に設定され移動経路の生成の指標となる複数の経由位置を記憶した記憶装置3と、移動装置の現在位置を検出する位置検出装置5と、移動装置の移動中において、現在位置に基づいて、複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を生成する経路探索装置9とを備える。経路探索装置9は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、選択した移動経路候補を実際に使用する移動経路として設定する。
【解決手段】自律移動する移動装置の移動経路を生成する経路生成装置10であって、移動開始位置から目標到達位置までの間に設定され移動経路の生成の指標となる複数の経由位置を記憶した記憶装置3と、移動装置の現在位置を検出する位置検出装置5と、移動装置の移動中において、現在位置に基づいて、複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を生成する経路探索装置9とを備える。経路探索装置9は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、選択した移動経路候補を実際に使用する移動経路として設定する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、自律移動する移動装置の移動経路を生成する経路生成装置と方法および経路生成装置を備える移動装置に関する。
【背景技術】
【0002】
自律移動する移動装置は、舗装路、未舗装路(オフロード)または屋内において自律的に移動するロボットや車両である。
移動装置が自律移動・走行するために、移動装置は、既知の地図上に設定された移動経路に沿って移動するように制御される(例えば、下記特許文献1)。
【0003】
この制御を行うために移動装置は、例えば記憶装置、内界情報検出装置、障害物検出装置、経路探索装置、制御装置を有する。
【0004】
記憶装置は、移動装置の移動範囲を含む領域の地図(地図情報)と、移動開始位置から目標到達位置までの設定経路(経路情報)と、を記憶する。地図情報は、例えば2次元座標による座標系である。経路情報は、移動開始位置から目標到達位置までの間に設定される複数の経由位置である。これら経由位置は後述する上記移動経路の生成・設定の指標となる。なお、これら経由位置は、上記座標系における座標であってよい。
【0005】
内界情報検出装置は、移動装置の現在位置、移動装置の移動方向、移動速度などの移動装置自体に関する情報を検出する。
【0006】
障害物検出装置は、移動装置の周囲(例えば、移動装置の進行側の所定領域内)において停止車両、凹凸地、壁などの障害物の有無およびその位置・範囲を検出する。
【0007】
経路探索装置は、例えば所定の周期で、移動装置の現在位置、障害物の位置・範囲、上記経路情報に基づいて、次の経由位置に向かうための移動経路を探索して生成する。即ち、経路探索装置は、検出された障害物を回避して現在位置から次の経由位置へ移動するための移動経路を探索して生成する。生成された移動経路は、移動装置の走行制御に使用するために記憶装置に設定・記憶される。
【0008】
制御装置は、移動装置の現在位置に基づいて、上述のように設定された移動経路に沿って移動装置が走行するように、移動装置の移動方向や速度などを制御する。
【先行技術文献】
【特許文献】
【0009】
【特許文献1】特開平7−248820号公報
【特許文献2】特開2009−110154号公報
【非特許文献】
【0010】
【非特許文献1】The International Journal Robotics Research Vol. 22, No. 7−8, July−August 2003, pp.583−601
【非特許文献2】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
【発明の開示】
【発明が解決しようとする課題】
【0011】
移動経路の探索において、より高速で移動可能な移動経路を見つけ出すことができれば、移動装置は、より高速で移動することができる。
【0012】
そこで、本発明の目的は、移動経路の探索において、より高速で移動可能な移動経路を見つけ出すことで、移動装置の高速移動を可能にすることにある。
【課題を解決するための手段】
【0013】
上記目的を達成するため、本発明によると、自律移動する移動装置の移動経路を生成する経路生成装置であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
前記移動装置の現在位置を検出する位置検出装置と、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する経路探索装置と、を備え、
前記経路探索装置は、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成装置が提供される。
【0014】
本発明の好ましい実施形態によると、前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、移動経路候補に沿った路面領域の高低差に基づいた路面良好度、および、現在使用している移動経路と移動経路候補との近接度の少なくともいずれかを含み、
前記経路探索装置は、前記各要素の値に応じて前記高速走行可能度を算出する。
【0015】
好ましくは、前記高速走行可能度は、前記各要素の値が大きいほど高い。
【0016】
本発明によると、上述の経路生成装置と、該経路生成装置により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備えることを特徴とする移動装置が提供される。
【0017】
本発明の別の実施形態によると、前記経路探索装置は、前記移動装置の現在位置と、前記移動装置に設けた障害物検出装置により検出した障害物とに基づいて、前記現在位置近傍の局所地図を生成し、
前記経路探索装置は、局所地図において、前記現在位置から多数の直線状の探索線を、障害物の有無が検出されていない未検出領域または障害物に至るまで放射状に延ばすことにより、両側が障害物に挟まれた移動装置の走行可能箇所を特定し、該走行可能箇所に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する。
【0018】
この場合、好ましくは、前記経路探索装置は、局所地図において、
(A)次に通過する経由位置に向かう方向に対する左右両側に障害物が存在する場合に、左右の障害物の間に存在する前記移動装置の走行可能領域を抽出し、
(B)走行可能領域内に位置して走行可能領域の境界に接する複数の内接円を生成し、
(C)各内接円の中心を通過するラインに基づいて道なりラインを生成し、
(D)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく前記未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(E)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(F)前記対象衝突点から延びて、前記道なりラインと直交し、対象衝突点と反対側にある前記障害物まで延びる線分を前記走行可能箇所として生成し、
(G)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する。
【0019】
代わりに、前記経路探索装置は、局所地図において、
(A)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(B)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(C)対象衝突点と反対側の前記障害物において、前記対象衝突点からの距離が最も短くなる点を設定し、当該点から前記対象衝突点まで延びる線分を前記走行可能箇所として生成し、
(D)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出してもよい。
【0020】
本発明のさらに別の実施形態によると、移動装置は、回転駆動される走行車輪により路面上を走行する車両であり、
前記車両には、その走行時に前記車両の振動を計測する振動センサが搭載されており、
前記振動センサにより計測された振動に基づいて、前記車両が現在走行している路面が、未舗装路面と舗装路面のいずれであるかを判定する路面判定装置が設けられ、
前記高速走行可能度は、その要素として、前記路面良好度を含むだけでなく、前記経路長さと最小曲率半径の一方または両方を含み、
前記経路探索装置は、前記各要素の値を規格値に換算して算出し、各要素の規格値を反映させた高速走行可能度を算出し、
前記経路探索装置は、同じ移動経路候補について、前記路面が未舗装路面であると判定された場合には、前記路面が舗装路であると判定された場合よりも、前記経路長さと前記最小曲率半径の規格値を小さく算出する。
【0021】
また、本発明によると、自律移動する移動装置の移動経路を生成する経路生成方法であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置とを設け、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する場合に、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成方法が提供される。
【発明の効果】
【0022】
上述した本発明によると、前記経路探索装置は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定するので、高速走行可能度がより高い移動経路が設定される。これにより、移動装置の高速移動が可能になる。
【図面の簡単な説明】
【0023】
【図1】本発明の第1実施形態による経路生成装置の構成図である。
【図2】高速走行可能度の各要素の規格化を示すグラフである。
【図3】移動経路候補の各評価領域ERを示す。
【図4】評価領域の評価値を算出する方法を示すフローチャートである。
【図5】図3における評価領域を示す部分拡大図である。
【図6】評価領域の路面状態を説明するための図である。
【図7】路面良好度を算出するために使用するグラフである。
【図8】現在の移動経路と移動経路候補との距離の算出を説明するための図である。
【図9】本発明の第1実施形態による経路生成方法を示すフローチャートである。
【図10】経路探索装置が生成する局所地図である。
【図11】本発明の第3実施形態による経路生成方法の説明図である。
【図12】本発明の第3実施形態による経路生成方法を示すフローチャートである。
【図13】本発明の第3実施形態による経路生成方法の他の説明図である。
【図14】本発明の第4実施形態による経路生成方法の説明図である。
【図15】本発明の第5実施形態による経路生成方法の説明図である。
【図16】本発明の第6実施形態による経路生成装置の構成図である。
【図17】走行速度と振動の振幅の平均値との関係を示す。
【図18】第6実施形態による高速走行可能度の各要素の規格化を示すグラフである。
【発明を実施するための形態】
【0024】
本発明の実施形態を図面に基づいて説明する。
【0025】
[第1実施形態]
本実施形態による経路生成装置10は、図1に示すように、記憶装置3、位置検出装置5、経路探索装置9、障害物検出装置7を備える。
【0026】
記憶装置3は、移動装置の移動範囲を含む地図に関する地図情報と、該地図における位置を示す経由位置情報(即ち、経由位置)と、を記憶する。地図情報は、2次元の地図に関する情報であってよく、この地図情報は例えばx−y座標系であってよく、与えられたx座標とy座標により地図上の位置が特定されるものであってよい。経由位置情報は、移動装置の移動開始位置から目標到達位置までの間に設定された複数の経由位置の位置情報(例えば、x座標とy座標)である。これら複数の経由位置は、移動経路の生成の指標として、経路探索装置9に使用される。なお、複数の経由位置は、移動開始位置と目標到達位置を含んでよい。
【0027】
位置検出装置5は、移動装置の現在位置を検出する。位置検出装置5は移動装置に設けられる。位置検出装置5は、例えば、GPSシステムを利用してGPSシステム(GPS衛星)から位置に関する情報を受信し、当該情報に基づいて移動装置の現在位置を検出するGPS装置であってよい。
代わりに、位置検出装置5は、移動装置の移動速度、移動方向を随時検出し、検出した移動速度と移動方向と移動装置の初期位置情報に基づいて、移動装置の現在位置を演算して検出するものであってもよい。
【0028】
障害物検出装置7は、移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する。移動装置は、移動装置の周囲(例えば、移動装置が進行する側における所定の領域内)において停止車両、凹凸地、壁などの障害物およびその位置・範囲を検出する。障害物検出装置7は、例えば、レーザ光を射出し、このレーザ光の反射光を検出することで障害物を検出するものであってよい。この場合、障害物検出装置7は、移動装置の進行方向に複数のレーザビームを射出するレーザ光源と、これら射出されたレーザビームの反射光を検出して障害物の有無およびその位置・範囲を検出する反射光検出装置と、を有する。レーザ光源は、移動装置の進行方向と垂直な水平方向に関して互いに傾いた角度で、かつ、鉛直下方側に傾けて、複数のレーザビームを射出する。反射光検出装置は、射出されたレーザビームの反射光を受光し、受光した反射光に基づいて障害物の有無およびその位置・範囲を検出する。
なお、障害物検出装置7は、上記のものに限定されず他の公知の装置であってもよい。例えば、レーザ光を用いた障害物検出装置以外に、CCDカメラなどを用いた装置を障害物検出装置7としてもよい。
【0029】
経路探索装置9は、移動装置の移動中において、位置検出装置5が検出した上記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する。経路探索装置9は、コンピュータにより構成されてよい。
【0030】
経路探索装置9は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定する。
【0031】
経路探索装置9は、複数の移動経路候補を、次の式(Eq1)により生成してよい。
κ(S)=a1+a2S+a3S2+・・・+an+1Sn ・・・(Eq1)
この式(Eq1)で表わされるκ(S)は、移動経路候補の曲率である(例えば、非特許文献1)。
この式(Eq1)において、
nは、1または2以上の整数であり、
Sは、移動装置の現在位置から前記移動経路候補上の任意位置までの前記移動経路候補に沿った経路長さを示し、該Sの上限値は、該現在位置から前記移動経路候補の終了位置までの移動経路候補に沿った移動経路長さであり、
a1〜an+1は、係数である。
この式(Eq1)において、Sの上限値、a1、a2、a1・・・、およびan+1の少なくともいずれかが互いに異なる複数のκ(S)により、それぞれ、複数の移動経路候補が特定・生成されてよい。
【0032】
各移動経路候補を表す上記κ(S)は、式(Eq1)中のSをパラメータとして、移動経路候補の方位角(即ち、移動経路候補の接線方向)と移動経路候補上の位置とを次の[数1]の式(Eq2)と(Eq3)により求めることができる。
【0033】
【数1】
【0034】
高速走行可能度について説明する。前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、路面良好度、および、現在使用している移動経路と移動経路候補との近接度の少なくともいずれか(この例では、すべて)を含む。
【0035】
前記経路探索装置は、前記各要素の値に応じて前記高速走行可能度を算出する。本実施形態では、前記高速走行可能度は、これら各要素の値が大きいほど高くなる。好ましくは、前記高速走行可能度を、各要素の規格値の総和として算出する。規格値は、例えば、0〜1の範囲の値となり、各要素の取り得る値が、0〜1の範囲とならない場合には、この範囲の値になるように変換を行う。その上で、移動経路候補の経路長さの規格値と、移動経路候補の最小曲率半径の規格値と、現在使用している移動経路と移動経路候補との近接度の規格値との総和を高速走行可能度とする。
なお、移動経路候補の経路長さが長いほど、次の移動経路候補が見つからないことにより移動装置の移動を一時的に停止させる必要性が低くなる。その結果、目標到達位置に到達するのに要する時間が短縮される。また、前記近接度が高いほど、現在使用している移動経路から移動経路候補から逸れている程度が小さくなるので、移動経路候補は、それだけ少ないカーブを有する可能性が高くなる。カーブが少ないと、高速で移動できる部分が多くなる。
【0036】
(経路長さ)
移動経路候補の経路長さは、移動経路候補の始点から終点(終了位置)までの、移動経路候補に沿った長さである。すなわち、κ(S)におけるSの上限値である。
【0037】
移動経路候補の経路長さは、図2(A)の関係に従って上述の規格値に変換される。図2(A)において、経路長さとその規格値は、比例関係にある。図2(A)の例では、生成される移動経路候補の長さが設定値を超える場合には、その規格値を1にする。
【0038】
(最小曲率半径)
移動経路候補の最小曲率半径は、移動経路候補の全範囲(移動経路候補の始点から終点まで)の各位置における曲率半径(旋回半径)のうちの最小値である。具体的には、移動経路候補の各位置の曲率半径は、上述のκ(S)の逆数1/κ(S)であるので、当該各位置における1/κ(S)のうちの最小値である。
【0039】
移動経路候補の最小曲率半径は、図2(B)の関係に従って上述の規格値に変換される。図2(B)の例では、最小曲率半径をRとして、最小曲率半径の規格値Zは、次の式(Eq4)で表わされる。
Z=(1/n)×logR ・・・(Eq4)
すなわち、規格値は、最小曲率半径Rに対して10を底とする対数のn分の1であり、nは、移動経路候補を直線と見なす曲率半径(設定値)に対して10を底とする対数値である。なお、移動経路候補の最小曲率半径が、移動経路候補を直線と見なす曲率半径(設定値)を超える場合には、その規格値を1にする。
【0040】
(路面良好度)
前記路面良好度は、移動経路候補に沿った路面領域の高低差に基づいて算出される。前記路面良好度は、次のように、移動経路候補の各評価領域について算出した評価値の最低値である。まず、対象とする移動経路候補に沿って該移動経路候補の始点から終点まで移動装置が移動した場合に、移動装置が通過することになる、移動経路候補に沿って間隔をおいた各評価領域毎に、該評価領域における各計測点の高さに基づいて、該評価領域が、走行不能であるか、段差面であるか、平面であるか、または、凹凸面であるかを判断する。その上で、移動経路候補の各評価領域の中に、走行不能であると判断されたものがある場合には、移動経路候補の路面良好度を0にし、そうでない場合には、次のようにする。すなわち、各評価領域毎に、段差面であると判断した場合には、その段差がどれだけ小さいかを示す路面良好度の評価値を算出し、平面であると判断した場合には、その路面良好度の評価値を算出し、凹凸面であると判断した場合には、その凹凸がどれだけ小さいかを示す路面良好度の評価値を算出し、移動経路候補のすべての評価領域の各評価値のうち最も小さい値を、移動経路候補の路面良好度とする。なお、移動経路候補の路面良好度は、0〜1までの値をとるので、そのまま上述の規格値となる。
【0041】
以下において、路面良好度について詳しく説明する。
【0042】
路面良好度は、移動装置の現在位置を含む2次元の局所地図(後で詳しく説明する)に組み込まれた各計測点の高さに基づいて経路探索装置9により算出される。これら各計測点の高さ(即ち、鉛直方向に関する位置座標値)は、それぞれ、実際の路面上の対応する各位置の高さであり、障害物検出装置7により、レーザビームを射出し、その反射レーザビームを検出することで得た計測データに基づいて算出されてよい。例えば、当該計測データは、前記レーザビームを反射した路面上の点と前記レーザ光源との距離であり、レーザビームの移出方向や、移動装置の姿勢などに基づいて、当該点の高さに変換されて前記局所地図上の前記計測点と対応付けられて前記局所地図に組み込まれる。
【0043】
各移動経路候補の路面良好度は、当該移動経路候補における各評価領域(図3において符号ERで示す)の評価値のうち最も低い値である。移動経路候補の各評価領域毎に、当該評価領域の各計測点の高さに基づいて評価値が算出され、算出された各評価領域の評価値のうち最も低い値を、当該移動経路候補の路面良好度とする。図3に示すように、移動経路候補の各評価領域ERは、2次元の局所地図において、移動経路候補(図3において破線で示す)上の所定間隔をおいた各位置(図3において黒丸で示す)にあり、当該位置において移動経路候補と交差(例えば、直交)する両側(水平方向両側に対応)へ、当該位置が中心となるように広がっている線状領域である。この線状領域ERの長さは、2次元の局所地図において、移動装置(車体)の幅に相当する長さ程度であるのがよい。
【0044】
各評価領域の評価値は、図4のフローチャートに従って算出される。各移動経路候補毎に、当該移動経路候補の各評価領域の評価値を図4のフローチャートに従って算出し、当該移動経路候補の各評価領域の評価値のうち最も値が低いものを、当該移動経路候補の路面良好度とする。
【0045】
図4のフローチャートに従って評価領域の評価値を算出する方法について図5などを参照して説明する。図5は、図3における複数の線状の評価領域ERのうちの任意の1つを示した部分拡大図である。
【0046】
ステップST1において、図5に示す評価領域ER上に存在し互いに間隔が置かれている複数の前記計測点(図5において白丸で示す)のうち左右の各走行タイヤ毎に、第1〜第4の計測点Pm1〜Pm4について、隣接する2つの計測点毎に、当該2つの計測点の高低差を算出し、当該2つの計測点間の勾配を算出し、これら高低差の分散を算出する。
第1〜第4の計測点Pm1〜Pm4を図5に基づいて説明する。図5において、破線は、移動装置(車両)の左右の走行タイヤを示す。図5のように、このステップST1で使用する計測点が各タイヤの幅(図における左右方向の幅)内に2〜3つ以上存在する。より詳しく述べると、図5の破線で囲まれた2つの部分は、前記局所地図上に、対象の移動経路候補が仮に後述のように設定された場合に、移動装置の左右の2つの走行タイヤが評価領域ERを通過する時のこれら走行タイヤを示す。第1および第2の計測点Pm1、Pm2は、図5に示すように、路面が平面である場合に走行タイヤが当該路面と接触する接触領域における当該走行タイヤの左右一方側端を挟む。第3および第4の計測点Pm3、Pm4は、路面が平面である場合に走行タイヤが当該路面と接触する接触領域における当該走行タイヤの左右他方側端を挟む。第1および第4の計測点Pm1、Pm4は、前記接触領域外にあり、第2および第3の計測点Pm2、Pm3は、前記接触領域内にある。
上述した2つの計測点の高低差は、当該2つの計測点の前記高さの差であり、上述した2つの計測点間の勾配は、水平面からの傾き角度であり、前記局所座標における、水平方向に関する当該2つの計測点の位置座標値の差をXとし、当該2つの計測点の前記高さの差をYとして、Atan(Y/X)である。Atanは、正接の逆関数(アークタンジェント)である。
なお、2つの計測点の高低差は、当該2つの計測点の前記高さの差であり、2つの計測点間の勾配は、水平面からの傾き角度であり、前記局所地図上の当該2つの計測点の距離に相当する実際の距離をXとし、当該2つの計測点の前記高さの差をYとして、Atan(Y/X)である。Atanは、正接の逆関数(アークタンジェント)である。
【0047】
ステップST2において、各走行タイヤに対応する第1〜第4の計測点Pm1〜Pm4について、隣接する2つの計測点(Pm1とPm2など)の高低差が、所定のしきい値以上であるかを判断する。すなわち、ステップST1で算出した高低差の中に、所定の高低差しきい値以上となるものがある場合には、ステップST3へ進む。一方、ステップST1で算出した高低差の中に、前記高低差しきい値以上となるものがない場合には、ステップST4へ進む。
【0048】
ステップST3において、対象の評価領域は通過不可能(走行不能)であるとして、評価値を0にする。
【0049】
ステップST4において、対象とする評価領域が段差面であるかどうかを判断する。すなわち、各走行タイヤに対応する第1〜第4の計測点Pm1〜Pm4について、ステップST1で算出した第1および第2の計測点Pm1、Pm2間の勾配、および、ステップST1で算出した第3および第4の計測点Pm3、Pm4間の勾配のいずれもが、平面とみなせる上限値以下である場合には、対象とする評価領域が段差面であるとして、ステップST5に進む(なお、この上限値は、前記勾配しきい値よりも小さい)。一方、各走行タイヤの少なくともいずれかに対応する第1〜第4の計測点Pm1〜Pm4について、ステップST1で算出した第1および第2の計測点Pm1、Pm2間の勾配、および、ステップST1で算出した第1および第2の計測点Pm3、Pm4間の勾配のいずれもが、平面とみなせる前記上限値以下でない場合には、ステップST6へ進む。
例えば、図6(A)の場合には、対象とする評価領域が段差面であるとして、ステップST5に進む。図6(A)〜(C)のグラフにおいて、横軸は、走行タイヤに対応する各計測点Pm1〜Pm4の水平方向位置座標を示し、縦軸は、これら計測点Pm1〜Pm4の高さを示し、白丸は、各計測点Pm1〜Pm4を示す。
【0050】
ステップST5において、ステップST1で算出した、各走行タイヤに対応する第2および第3の計測点Pm2、Pm3の高低差の大きさのうち大きいほうに応じて、評価値を算出する。例えば、図7(A)に示す関係に従って評価値を算出する。図7(A)のグラフは、計測点Pm2、Pm3の高低差の大きさが大きい方の走行タイヤの当該計測点Pm2、Pm3の高低差の大きさと評価値との関係を示す。
【0051】
ステップST6において、対象とする評価領域が平面とみなせるかどうかを判断する。なわち、ステップST1で算出した高低差の分散が、平面とみなせる上限値以下である場合には、対象とする評価領域は平面であるとして、ステップST7へ進む。ここで、分散は、ステップST1で算出した高低差に基づいて計算される。すなわち、ステップST1で算出した各高低差について、当該高低差と、ステップST1で算出した高低差の平均値との差を自乗し、これら自乗して得た値の平均値が、上述の分散である。一方、ステップST1で算出した高低差の分散が、平面とみなせる上限値以下でない場合には、ステップST8へ進む。
例えば、図6(B)の場合には、対象とする評価領域が平面であるとして、ステップST7に進む。
【0052】
ステップST7において、ステップST6で算出した分散に応じて、評価値を算出する。例えば、図7(B)に示す関係に従って評価値を算出する。図7(B)のグラフは、分散と評価値との関係を示す。
【0053】
ステップST8において、対象とする評価領域が凹凸面であるとする。例えば、図6(C)の場合には、対象とする評価領域が凹凸面であるとする。
また、ステップST8において、ステップST1で算出した高低差の大きさのうち最大となるもの(最大高低差という)に応じて、評価値を算出する。例えば、図7(C)に示す関係に従って評価値を算出する。図7(C)のグラフは、最大高低差と評価値との関係を示す。
【0054】
(現在の移動経路と移動経路候補との近接度)
この近接度は、現在、使用・設定されている移動経路(以下、現在の移動経路という)と移動経路候補との距離を用いて算出される。
【0055】
この距離を図8に基づいて説明する。図8は、前記局所地図上に移動経路候補を示した図である。現在の移動経路と移動経路候補との距離は、経路探索装置9により、図8の各対応点間距離L1〜LPの総和として算出される。まず、当該移動経路候補上(図8の破線上)における複数のサンプル点P1〜Pnを取り、当該各サンプル点に最も近い現在の移動経路上(図8の実線上)の点を対応点として求める。次に、各サンプル点毎に、当該サンプル点とこれの対応点との距離を対応点間距離として算出する。その後、各サンプル点の対応点距離L1〜LPの総和を、現在の移動経路と移動経路候補との距離として算出する。
【0056】
なお、各移動経路候補のサンプル点の取り方は、図8に示す方法であってよい。すなわち、移動距離の現在位置から移動経路候補に沿って一定距離だけ仮想的に進行した点を最初のサンプル点P1とし、このサンプル点P1から移動経路候補に沿って一定距離だけさらに仮想的に進行した点を次のサンプル点P2とし、このサンプル点P2から移動経路候補に沿って一定距離だけ仮想的にさらに進行した点を次のサンプル点P3とするといった具合に複数のサンプル点P1〜Pnを取ってよい。移動距離の現在位置から移動経路候補に沿って最も離れた最後のサンプル点Pnは、現在の移動経路の終点から最も近い移動経路候補上の点であってよい。
【0057】
上記のように算出された、現在の移動経路と移動経路候補との距離は、図2(C)の関係に従って、前記近接度(すなわち、前記規格値)に変換される。図2(C)の例では、算出された距離が設定値を超える場合には、その規格値を0にする。
【0058】
本実施形態によると、経路探索装置9は、所定の経路探索条件の下で、または、後述のように変更した経路探索条件の下で、生成されて見出された複数の移動経路候補について、上述のように、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定する。
【0059】
経路探索装置9は、前記経路探索条件の下で前記移動経路候補を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路候補を再び探索する。
前記経路探索条件(即ち、初期の経路探索条件)は、障害物検出装置7が検出した障害物と移動経路候補とが干渉しないという干渉条件と、移動装置が指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという障害物検出条件と、を含む。この場合、前記経路探索装置9は、前記経路探索条件の下で前記移動経路候補を見出せなかった場合に、当該経路探索条件の障害物検出条件において前記速度制御に使用された前記指令速度値(前回の指令速度値ともいう)よりも低速度で移動装置が移動するように、速度指令装置から出力された指令速度値を調整し(即ち、現時点の指令速度値が前回の指令速度値より大きい場合には、減少させ)、これにより、移動装置が該調整された指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うことで、経路探索条件を変更し、該変更した経路探索条件の下で移動経路候補を再び探索する。
【0060】
また、前記経路探索条件(即ち、初期の経路探索条件)は、前記移動経路候補の曲率が該移動経路候補の全範囲にわたって所定の曲率基準値以下になるという曲率条件を含んでよい。
【0061】
前記移動経路候補の曲率に関する速度制御について説明する。上記速度指令装置は、後述のように設定された移動経路における、移動装置が通過する直前の箇所の曲率に基づいて指令速度値を出力する。具体的には、上記速度指令装置は、横滑りおよび横転することなく当該箇所を移動装置が進行できるように、当該曲率に応じた指令速度値(例えば、当該曲率が大きいほど小さい指令速度値)を出力する。出力された指令速度値は、速度制御装置に入力され、この速度制御装置は、入力された指令速度値(即ち、この値が示す速度)で移動装置が移動するように移動装置の速度を制御する。これにより、移動装置は、設定された移動経路において、曲率の小さい箇所を通過する時ほど、より高速で移動する。
【0062】
なお、上述では、指令速度値を調整することで経路探索条件を変更したが、この変更を行わないようにすることもできる。例えば、前記経路探索条件は、障害物検出条件を省いて、干渉条件と曲率条件を含み、経路探索装置9は、前記経路探索条件の下で前記移動経路候補を見出せなかった場合に、前記所定の曲率基準値を増加することで経路探索条件を変更してもよい。
【0063】
また、前記経路探索装置9は、前記経路探索条件を満足する移動経路候補を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路候補を再び探索する処理を繰り返す。
【0064】
次に、経路生成装置10による移動経路の探索・生成について詳しく説明する。なお、以下の説明は、経路探索条件が干渉条件、障害物検出条件および曲率条件を含む場合と、障害物検出条件を省き経路探索条件が干渉条件および曲率条件を含む場合の両方に対応している。
【0065】
図9は、経路生成装置10による経路生成方法を示すフローチャートである。
【0066】
ステップS1では、経路探索装置9は、2次元の前記局所地図を生成する。即ち、経路探索装置9は、位置検出装置5により検出された移動装置の現在位置と障害物検出装置7による障害物探索領域とを含む範囲の地図情報を記憶装置3から抽出し、当該範囲の局所地図を生成する。この局所地図には、次の経由位置への方向情報が組み込まれていてよい。方向情報は、例えば、直前に通過した経由位置と次の経由位置とを結ぶ直線の方向であってよく、または、移動装置の現在位置から見た次の経由位置の方向であってもよい。この局所地図には、次の経由位置自体が組み込まれていてもよい。
なお、ステップS1で使用される現在位置と、障害物検出装置7による障害物検出結果情報(障害物の位置と範囲)とは、ステップS1を開始する毎に、それぞれ位置検出装置5および障害物検出装置7により獲得され、局所地図に組み込まれる。
【0067】
ステップS2では、移動経路候補を探索する。具体的には、前記経路探索装置9は、上記κ(S)の式を用いて、次の処理(A)または(B)を行う。
(A)前記複数の係数a1〜an+1および前記Sの上限値にそれぞれ所定の値を与えることで得られるκ(S)について、該κ(S)により定まる移動経路候補が前記経路探索条件(干渉条件と曲率条件)を満たすかを判断する。
(B)前記(A)の処理を複数回行う場合に、前記(A)の処理を行うごとにa1〜an+1のうち少なくとも1つの値を変更して前記(A)の処理を行う。なお、前記Sの上限値は一定にしたまま前記(A)の処理を複数回行ってよいし、複数の前記(A)の処理うち少なくとも1回の処理で前記Sの上限値を変更してもよい。
経路探索装置9が、前記(A)または(B)の処理により、前記経路探索条件を満たす移動経路候補を見出せなかった場合には、ステップS3に進み、経路探索条件を満たす移動経路候補を見出せた場合、ステップS4へ進む。
【0068】
ステップS3では、経路探索条件を変更してステップS1に戻る。具体的には、ステップS3で、経路探索装置9は、前記指令速度値を上述のように調整し、ステップS1で、移動装置が該調整された指令速度値に基づいて速度制御されている状態で障害物検出装置7が障害物検出を再び行い。この障害物検出結果に基づいて上記局所地図を生成する。この時、移動装置の現在位置も位置検出装置5により再び検出され、この現在位置と再度行われた障害物検出結果とに基づいて局所地図が生成されてよい。
また、ステップS3では、経路探索装置9は、上述のように前記指令速度値を調整するとともに、前記曲率基準値を増加させてもよい。
なお、ステップS3において、指令速度値を調整することを行わないようにしてもよい。即ち、単に前記曲率基準値を増加することで経路探索条件を変更してもよい。この場合には、図9と異なり、ステップS3からステップS1を経由せずにステップS2へ戻ってもよいし、図9のように、ステップS3からステップS1へ戻って新たな現在位置と障害物検出結果とに基づいて再び局所地図を生成してもよい。
【0069】
ステップS4では、ステップS2で見出した複数の移動経路候補について、上述のように、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定する。
なお、ステップS2で見出した移動経路候補が1つのみである場合には、ステップS4を省略して、ステップS2からステップS5へ進む。
【0070】
ステップS5では、ステップS4で選択した移動経路候補を実際に使用する移動経路として設定して移動経路を更新する。即ち、移動装置の進行制御にこれまで使用していた設定移動経路を、ステップS4で選択した移動経路候補に置き換えることで、設定移動経路を更新する。なお、設定移動経路は記憶装置3に記憶される。更新したらステップS6へ進む。また、ステップS4を省略した場合には、ステップS2で見出した1つのみの移動経路候補を移動経路として設定して移動経路を更新する。
【0071】
ステップS6では、経路探索装置9は、ステップS1で、局所地図の生成を開始してから所定時間(例えば、100ms)経過したかを、経路探索装置9に内蔵されたタイマーの時間計測により判断する。所定時間経過したと判断した場合には、ステップS1に戻り、所定時間経過していないと判断した場合にはステップS6の判断を繰り返す。
これにより、所定時間経過毎に、局所地図の生成および経路探索を行う。これに対応して、位置検出装置5は、上記の所定時間経過毎に移動装置の現在位置を検出し、障害物検出装置7は、上記の所定時間経過毎に障害物の検出を行う。
ステップS6からステップS1に戻りステップS2へ戻った時、このステップS2での経路探索は、変更した経路探索条件ではなく初期の経路探索条件(例えば、初期の干渉条件と障害物検出条件と曲率条件)で行う。
【0072】
なお、ステップS1〜S4、S6の処理の最中に、移動装置の制御装置は、記憶装置3に記憶された上記の設定移動経路に沿って移動装置が移動するように移動装置を制御する。
【0073】
[第1実施形態の作用効果]
上述した第1実施形態によると、経路探索装置9は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定するので、高速走行可能度がより高い移動経路が設定される。これにより、移動装置の高速移動が可能になる。
【0074】
なお、移動経路候補が見出せない原因として、移動装置が高速で移動することにより移動装置が揺動・振動することで、障害物を誤って検出してしまう場合がある。例えば、揺動・振動により障害物検出装置の検出方向が下方側に変更されてしまい路面自体を障害物として検出してしまう場合がある。
これに対し、第1実施形態では、移動経路候補を見出せなかった場合に、上述のように指令速度値を調整し、移動装置がより低速で移動している状態で障害物検出装置7が障害物検出を行うので、障害物の誤検出により移動経路候補が見出せない可能性を効果的に小さくできる。
【0075】
また、障害物検出装置7が検出した障害物(例えば、停止車両、穴、岩、壁など)と移動経路候補とが干渉しないという干渉条件を含む経路探索条件の下では、該障害物により移動経路候補を見出せない場合がある。この場合、上記移動経路候補の曲率の曲率基準値を増加することで、障害物と干渉しない移動経路候補、即ち、障害物を回避するように比較的大きい曲率・カーブを有する移動経路候補を見出せる可能が効果的に高まる。これにより、移動経路候補を見出し移動装置の移動を継続させられる可能性を効果的に高めることができる。
【0076】
[第1実施形態の実施例]
次に、第1実施形態の実施例について説明する。この実施例では、移動装置は車両であってよい。
【0077】
経路探索装置9は、障害物検出装置7が実際に検出した障害物の範囲を所定の大きさだけ拡大して上記局所地図上で認識する。即ち、障害物検出装置7が実際に検出した障害物の範囲が、所定の大きさ(例えば、車両幅の半分の大きさ+局所地図上の1ピクセル)だけすべての方向において拡大されて、局所地図上で認識・設定される。
【0078】
本実施例によると、移動経路候補の曲率を上述のようにκ(S)とするが、κ(S)をSの3次多項式とする。即ち、κ(S)を次の式(Eq5)で表す。
κ(S)=a1+a2S+a3S2+a4S3 ・・・(Eq5)
この式において、
Sは、移動装置の現在位置から前記移動経路候補上の任意位置までの前記移動経路候補に沿った経路長さを示し、該Sの上限値は、該現在位置から前記移動経路候補の終了位置までの移動経路候補に沿った移動経路長さであり、
a1〜a4は、パラメータ(係数)である。
【0079】
上記κ(S)によって、移動経路候補を表すことができる。即ち、a1〜a4に所定の値を与えることにより、言い換えると、a1〜a4の値が定まると移動経路候補が定まる。本実施例では、次の3つの境界条件を与えることで、例えば、a1、a3、a4をa2の関数で表現できる。
【0080】
境界条件:
(1)移動経路候補の開始点、即ち、移動装置の現在位置における曲率についての次の境界条件(Eq6)
κ(0)=κ0=a1 ・・・(Eq6)
(2)探索・生成対象の移動経路候補の終了点Sf(即ち、Sの上限値)における曲率についての次の境界条件(Eq7)
κ(Sf)=κf=a1+a2Sf+a3Sf2+a4Sf3 ・・・(Eq7)
(3)探索・生成対象の移動経路候補の終了点Sfにおける移動経路方向(移動経路の接線方向)についての次の境界条件(Eq8)
θ(Sf)=θf=a1Sf+a2Sf2/2+a3Sf3/3+a4Sf4/4
・・・(Eq8)
なお、κ0,κf,θfは、既知であるとする。また、好ましくは、κfは、ゼロであり、θfは、移動経路候補の終了点と次の経由地点を結ぶ直線と同じ方向である。
【0081】
上記境界条件から,次の式(Eq9)、(Eq10)
a3Sf2+a4Sf3=κf−κ0−a2Sf ・・・(Eq9)
a3Sf3/3+a4Sf4/4=θf−κ0Sf−a2Sf2/2 ・・・(Eq10)
が得られ、従って、上記式(Eq9)、(Eq10)などから次の関係式1が得られる。
[関係式1]
a1=κ0
a3=(−3κf−9κ0)/Sf2−3a2/Sf+12θf/Sf3
a4=4(κf+2κ0)/Sf3+2a2/Sf2−12θf/Sf4
【0082】
よって、a2をパラメータとして移動経路候補(軌道)を一意に求めることができる。これら、a1、a3およびa4の各々とa2、κ0、κf、Sfとの上記関係式1は記憶装置3に記憶され、経路探索時に使用される。なお、上記のような境界条件を設定することで、a2の値を与えると、a1、a1、a4を短時間の演算で求めることができる。その結果、κ(S)により移動経路候補を短時間で特定できる。
【0083】
この実施例では、経路探索条件は、障害物検出装置7が検出した障害物と移動経路候補とが干渉しないという干渉条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという障害物検出条件と、前記移動経路候補の曲率が該移動経路候補の全範囲にわたって曲率基準値以下になるという曲率条件と、設定長さを有する1つ以上の移動経路候補から干渉条件を満たすものを生成用の移動経路候補として選択するという生成経路選択条件と、を含む。
【0084】
(曲率条件について)
移動装置が走行する装置(例えば、車両)である場合、曲率κ(S)の上限値は、例えば、次の、κturn、κslip、κrolloverのうちの最も小さいものである。
(1)車両の最小旋回可能半径を示す移動経路候補の曲率κturn。
(2)車両が、所定速度で走行する時に、横滑りを起こさない限界の曲率κslip。即ち、車両が、所定速度で走行する場合に、移動経路候補上における曲率κslipより大きい曲率κ(S)の箇所で車両が横滑りする。
(3)車両が、前記所定速度で走行する時に、横転を起こさない限界の曲率κrollover。即ち、車両が、前記所定速度で走行する場合に、移動経路候補上における曲率κrolloverより大きい曲率κ(S)の箇所で車両が横転する。
なお、通常は、曲率κslipおよび曲率κrolloverの値は、曲率κturnの値よりも小さい。また、曲率κslipおよび曲率κrolloverの値は、車両の前記所定速度により変化し、一般的に、車両の前記所定速度が大きいほど減少する。また、曲率κslipの値は、車両とその走行面との動摩擦係数にも依存する。
従って、κ(S)が次の大小関係を満足する場合には、曲率条件が満たされることになる。
κ(S)≦κturn、かつ、κ(S)≦κslip、かつ、κ(S)≦κrollover、
このような曲率κslipおよび曲率κrolloverは移動装置(例えば、車両)の特性などに基づいて求めることができる。参考のために述べると、曲率κslipは、例えば非特許文献2に記載された式(2)であってもよく、曲率κrolloverは、非特許文献2に記載された式(4)であってもよい。
【0085】
(生成経路選択条件について)
経路探索装置9は、上記κ(S)により定められる移動経路候補を探索する時に、Sの上限値Sfを設定長さの値に固定して、上記パラメータa2の値を与えることで、上記関係式1からa1、a3、a4の値を定め、これにより、定まる移動経路候補が干渉条件と曲率条件を満たすかを判断する。
【0086】
(設定値について)
経路探索装置9は、経路探索に使用するパラメータ・係数などに所定の値を与えて経路探索を行う。そのために、記憶装置3は、予め設定されたこれらパラメータ・係数の値を記憶する。具体的には、記憶装置3は、経路長Sの上限値Sf、a2、κ0,κf,θfなどの各設定値を記憶する。
また、複数の移動経路候補を生成するために、a2の複数の値を設定しておく。例えば、a2の最小値a2minおよびa2の最大値a2maxと、この最小値からのa2の増加量da2とを設定しておき、経路探索装置9は、a2min、a2min+da2、a2min+2da2、a2+3da2・・・、a2maxのそれぞれついて移動経路候補が定まるようにしておく。これにより、複数(例えば、21本)の移動経路候補を生成できる。
記憶装置3は、曲率κ(S)の前記曲率基準値を記憶する。
また、κslipやκrolloverが移動装置の速度などの変数に依存する場合には、これら変数とκslipとの関係式と、これら変数とκrolloverとの関係式も記憶装置3に記憶させる。これにより、経路探索装置9は、任意の時に、これら変数の値に基づいて当該関係式を用いてκslipとκrolloverを演算し、κturn、κslip、κrolloverの大小を比較し、これらのうち最小のものを前記曲率基準値として特定する。なお、κturnは記憶装置3に記憶された固定値であってよい、
さらに、記憶装置3は、unknown領域に関する上述の所定の閾値、後述する指令速度値減少量・減少率、および経路長増加量・増加率などを記憶する。
【0087】
次に、本実施例による移動経路探索方法について説明する。この方法は、図9に示すフローチャートと同じ方法であるが、この実施例では、図9の方法の一例としてより詳しく説明する。以下では、上述しなかった点を詳しく述べるが、以下で述べない点は、上述と同じ内容であってよい。
【0088】
ステップS1では、上述のように局所地図を生成する。
【0089】
ステップS2では、移動経路候補を探索する。具体的には、上述のように設定した複数のa2のそれぞれをκ(S)の式に適用する。a1、a3、a4の値は、a2の各設定値ごとに関係式1のようにa2、κ0、κf、Sf、θfを用いて得られる。このようにして、複数の移動経路候補が生成される。
これら移動経路候補が、干渉条件および曲率条件を満足するかを判断する。曲率条件については、各移動経路候補について、κ(S)が、0≦S≦Sfの全範囲にわたって、上述したκ(S)の曲率基準値以下であるかを判断する。曲率基準値以下である場合には、曲率条件が満たされることになる。
このようにして、干渉条件および曲率条件を満たす移動経路候補が存在しない場合には、即ち、移動経路候補のいずれもが干渉条件および曲率条件の両方を満足しない場合には、移動経路候補が見出されなかったことになり、ステップS3へ進む。一方、干渉条件および曲率条件を満たす移動経路候補が存在する場合には、移動経路候補が見出されたことになり、ステップS4へ進む。
【0090】
ステップS3では、経路探索条件を変更する。この実施例では、上述の指令速度値の調整に加えて、前記曲率基準値を増加し、かつ、経路長さSの上限値Sfの値も増加させることで、経路探索条件を変更する。なお、曲率基準値の増加は、記憶装置3に記憶された曲率基準値拡大量・拡大率に基づいて行い、上限値Sfの値の増加は、記憶装置3に記憶されや経路長増加量・増加率に基づいて行う。経路探索条件を変更したら、ステップS2へ戻り、変更した経路探索条件の下で、経路探索を行う。この再度の経路探索処理は、経路探索条件を変更した以外は、前回、ステップS2で行った処理と同じである。
このようにすることで、前回の経路探索では、移動経路候補上に障害物が存在していた場合でも、Sfを増加させることで、上記関係式1によりa3、a4の値も変化し、κ(S)により定まる移動経路候補の経路も前回と異なったものとなる。しかも、生成される移動経路候補の全長も大きくなる。これにより、例えば図10に模式的に示すように、障害物を回避するように延びる移動経路候補が生成される可能性が効果的に高まる。なお、図10において、複数の細い曲線は、前回生成された移動経路候補を示し、太い曲線は今回生成された移動経路候補(図10では、簡単のため1本のみ)を示す。
【0091】
ステップS4の処理は、上述と同じである。
【0092】
ステップS5では、上述と同様に設定移動経路を更新する。
【0093】
ステップS6では、上述のように局所地図の生成を開始してから所定時間(例えば、100ms)経過したかを判断する。所定時間経過したと判断した場合には、ステップS1に戻り、所定時間経過していないと判断した場合にはステップS6の判断を繰り返す。
【0094】
[第2実施形態]
本発明の第2実施形態について説明する。第2実施形態による経路生成装置と方法は以下で述べる点で、第1実施形態と異なる。第2実施形態による経路生成装置と方法の他の内容は第1実施形態と同じであってよい。
【0095】
第2実施形態によると、前記経路探索装置9は、経路探索時に、次の(a),(b)の処理を行う。
(a)前記経路探索装置9は、前記1つ以上の移動経路候補が干渉条件を満足するかを判断する。
(b)該移動経路候補のいずれも干渉条件を満足しないと判断した場合に、前記1つ以上の移動経路候補を変更し、該変更された1つ以上の移動経路候補が前記干渉条件を満足するかを判断する。
【0096】
前記1つ以上の移動経路候補の変更は、前記設定長さの増加と、変更した前記1つ以上の移動経路候補の各々における前記現在位置からその任意の途中位置までの経路が、変更前の前記1つ以上の移動経路候補のいずれとも異なることと、によりなされている。
【0097】
第2実施形態によると、第1実施形態で述べたステップS2の処理を、第1実施形態と異なり次のように行う。
即ち、前記経路探索装置9は、上記κ(S)=a1+a2S+a3S2+・・・+an+1Snの式を用いて、次の処理(A)または(B)を行う。
(A)前記Sの上限値および前記複数の係数a1〜an+1にそれぞれ所定の値を与えることで得られるκ(S)について、該κ(S)により定まる前記移動経路候補が干渉条件を満たすかを判断する。
(B)前記(A)の処理を複数回行う場合に、前記(A)の処理を行うごとにa1〜an+1のうち少なくとも1つの値を変更して前記(A)の処理を行い、これにより、複数回の処理で得られる複数の前記移動経路候補について干渉条件を満たすかを判断する。
【0098】
また、第2実施形態によると、ステップS2において、前記(A)または(B)の処理において前記移動経路候補のいずれも干渉条件を満足しないと判断した場合に、ステップS3へ進む。
第2実施形態では、ステップS3において、前記Sの上限値を増加させ、かつ、前記係数a1〜an+1の少なくともいずれかの値を、対応する当該係数に前記(A)または(B)の処理で与えた値から変更することで、前記1つ以上の移動経路候補を変更し、前記(A)または(B)の処理を再び行う。
【0099】
[第3実施形態]
次に、本発明の第3実施形態による経路生成装置10と経路生成方法について説明する。第3実施形態による経路生成装置10と経路生成方法は以下で述べない点は、第1実施形態または第2実施形態と同じであってもよいし、第1実施形態または第2実施形態から適宜変更してもよい。
【0100】
上述のステップS1において、前記経路探索装置9は、前記移動装置の現在位置Xと、前記移動装置に設けた障害物検出装置7により検出した障害物とに基づいて、現在位置X近傍の局所地図を生成する。図11(A)は、この局所地図の一例である。図11(A)において上側が次に通過する経由位置(次の経由位置)の方向である。なお、経路探索装置9は、さらに、障害物が存在する領域と障害物が存在しない領域を示す既知の地形データにも基づいて、局所地図における後述の走行可能領域13と未計測領域15を特定してもよい。この地形データは、経路探索装置9のメモリに記憶されていてもよいし、無線通信で経路探索装置9に受けられてもよい。
【0101】
上述のステップS2は、第3実施形態では、次のように第1実施形態または第2実施形態の場合と異なる。
【0102】
ステップS2は、図12のフローチャートのように、ステップS21〜S28を有する。
【0103】
ステップS21では、経路探索装置9は、2次元の前記局所地図において、次に通過する経由位置に向かう方向に対する左右両側に障害物11a、11bが存在する場合に、左右の障害物11a、11bの間に存在する前記移動装置の走行可能領域13を抽出する。この走行可能領域13は、障害物が存在しない領域であり、左右の障害物11a、11bと接する境界を有する。図11(A)では、編目部分が障害物11a、11bである。
図11(A)の例では、障害物検出装置7により、または、障害物検出装置7と前記地形データにより、障害物11a、11bおよび走行可能領域13が検出される。図11(A)において、他の部分は、障害物検出装置7により障害物11a、11bの有無が検出されていない未検出領域15である。なお、図11(A)のように、移動装置の走行中の揺れにより、障害物検出装置7により検出された走行可能領域13が不連続となる場合もある。
【0104】
ステップS22では、経路探索装置9は、局所地図において、走行可能領域13内に位置して走行可能領域13の境界に接する複数の内接円を生成する。これらの内接円は、移動装置の現在位置Xから次に通過する経由位置の側へ間隔をおいて生成される。
図11(B)は、図11(A)の場合に対して生成された内接円を示す。現在位置Xの側から順に、左右の障害物11a,11bの両方に接する内接円が生成される。
次に生成する内接円(対象内接円という)の中心は、この対象内接円の直前に生成された内接円(1つ手前の内接円という)の中心と、この1つ手前の内接円の直前に生成された内接円(2つ手前の内接円という)の中心とを通る直線と直交する直線(直交直線という)上に位置する。当該次に生成する内接円は、当該直交直線上に中心が位置する最大の内接円である。
なお、当該直交直線は、次に生成する内接円が何番目のものであるかにかかわらず、次に生成する当該内接円に対して2つ手前の内接円の中心から一定の距離だけ離れた位置に生成されてよい。最初の内接円は、現在位置X近傍に生成され、2番目の内接円は、現在位置Xと最初の内接円の中心とを通る直線と直交する直線上に中心が位置するように生成されてよい。
【0105】
ステップS23では、経路探索装置9は、局所地図において、各内接円の中心を通る道なりライン17を生成する。図11(C)は、図11(B)の場合に対して生成された道なりライン17を示す。道なりライン17は、隣接する各対の内接円の中心を結ぶことにより生成した折れ線を、適宜の手法(例えば、最小二乗法やRANSAC法)で近似された曲線であってもよい。
【0106】
ステップS24では、前記現在位置Xから多数の直線状の探索線を放射状に延ばす。図11(D)は、図11(A)の場合に対して生成された多数の探索線を示す。
互いに連続して隣接し障害物11a,11bに衝突することなく未検出領域15に至る複数の探索線(図11(D)において符号Gで示す)のうち一方端の探索線Laと隣接する探索線Lbが、左側の障害物11aと衝突する衝突点を第1衝突点Paとして特定し、さらに、前記複数の探索線(図11(D)において符号Gで示す)のうち他方端の探索線Lcと隣接する探索線Ldが、右側の障害物11bと衝突する衝突点を第1衝突点Pbとして特定する。好ましくは、隣接する探索線のなす角度は、いずれの隣接する探索線についても同じであるのがよい。
【0107】
ステップS25では、第1衝突点Paと第2衝突点Pbを比較し、両衝突点のうち現在位置Xに近い方を、対象衝突点として特定する。図11(D)の例では、第2衝突点Pbが対象衝突点になる。
【0108】
ステップS26では、前記対象衝突点から延びて、道なりライン17と直交し、第1衝突点Paと第2衝突点Pbをそれぞれ含む左右の障害物11a、11bのうち対象衝突点と反対側の障害物まで延びる線分19を生成する。図13(A)は、図11の場合に対して生成された線分19を示す。
【0109】
ステップS27では、生成した線分19の中心位置を経由点Piに設定する。なお、この中心位置は、線分19の両端から等距離にある点である。図13(B)は、図13(A)の場合に対して設定された経由点Piを示す。
【0110】
ステップS28では、ステップS26で生成した前記線分19(すなわち、線分19上の点)に至る複数の移動経路候補を生成する。好ましくは、これらの移動経路候補は、経由点Piにおいて道なりライン17と同じ方向を向く経路である。すなわち、これらの各移動経路候補と線分19との交点における当該移動経路候補の接線と、道なりライン17と線分19との交点における道なりライン17の接線とが平行になる。ステップS28において、前記複数の移動経路候補を生成する方法は、このように線分19に至る移動経路候補を生成する点以外は、上述の第1実施形態または第2実施形態のステップS2において複数の移動経路候補を生成する方法と同じであってよい。例えば、上述の経路探索条件をさらに満足する複数の移動経路候補が生成されてよい。
【0111】
ステップS28において、線分19に至る複数の移動経路候補が生成されたら、上述のステップS4へ進み、経路探索装置9は、これらの移動経路候補の各々について高速走行可能度を算出する。
【0112】
ステップS4では、高速走行可能度の要素として、経由点Piからの距離をさらに含む。経由点Piからの距離とは、ステップS28で生成した移動経路候補と線分19との交点と、経由点Piとの距離である。経由点Piからの距離の規格値は、当該距離がゼロである場合には1であり、当該距離がゼロから増加するにつれて次第に減少し、線分19の長さの1/2から当該距離を引いた値が移動装置の幅の1/2以下になるとゼロになる。このように、経由点Piを通る場合に、最も高速走行が可能であるとして規格値も最大になる。
従って、経路探索装置9は、第1実施形態または第2実施形態の各要素の規格値だけでなく、経由点Piからの距離の規格値も考慮して、(例えば、すべての要素の規格値の総和として)高速走行可能度を算出する。
【0113】
第3実施形態によると、上述のように内接円を利用して道なりライン17を生成できるので、図11のように走行可能領域13の幅が急激に狭くなる部分に対して、障害物に接触する可能性を低くするように、より安全で高速に走行できる移動経路を生成できるようになる。
【0114】
[第4実施形態]
次に、本発明の第4実施形態による経路生成装置10と経路生成方法について説明する。第4実施形態による経路生成装置10と経路生成方法は、第3実施形態の変更例であり、以下で述べない点は、第3実施形態と同じである。
【0115】
第4実施形態では、上述のステップS22、S23が省略される。すなわち、ステップS21の次にステップS24を行う。
【0116】
また、第4実施形態では、ステップS26を次のように行う。
すなわち、ステップS26において、第1衝突点Paと第2衝突点Pbをそれぞれ含む障害物11a、11bのうち対象衝突点と反対側にある前記障害物の境界において、前記対象衝突点から距離が最も短くなる点を設定し、当該点から前記対象衝突点まで延びる線分21を生成する。図14(A)は、図11(D)の場合に対して生成された線分21を示す。
【0117】
ステップS27では、ステップS26で生成された線分21の中心位置を経由点Piに設定する。図14(B)は、図14(A)の場合に対して設定された経由点Piを示す。
【0118】
ステップS28では、線分21に至る(すなわち、線分21上の点に至る)複数の移動経路候補を生成する。好ましくは、これらの移動経路候補は、経由点Piにおいて、ステップS25で生成された線分21と直交する経路である。
【0119】
ステップS28の次に行うステップS4では、高速走行可能度の要素として、第3実施形態と同様に、経由点Piからの距離をさらに含む。経由点Piからの距離とは、ステップS28で生成した移動経路候補が線分21との交点と経由点Piとの距離である。経由点Piからの距離の規格値は、当該距離がゼロである場合には1であり、当該距離がゼロから増加するにつれて次第に減少し、線分23の長さの1/2から当該距離を引いた値が移動装置の幅の1/2以下になるとゼロになる。このように、経由点Piを通る場合に、最も高速走行が可能であるとして規格値も最大になる。
【0120】
第4実施形態によると、上述のように線分21と経由点Piに基づいて移動経路候補を生成して選択するので、図14のように走行可能領域13の幅が急激に狭くなる部分に対して、障害物に接触する可能性を低くするように、より安全で高速に走行できる移動経路を生成できるようになる。
【0121】
[第5実施形態]
次に、本発明の第5実施形態による経路生成装置10と経路生成方法について説明する。第5実施形態による経路生成装置10と経路生成方法は、第3実施形態または第4実施形態の変更例であり、以下で述べない点は、第3実施形態または第4実施形態と同じである。
【0122】
第5実施形態は、走行可能領域13が分岐している場合に関する。例えば、第5実施形態は、上述のステップS1で生成された局所地図が図15(A)のような場合に関する。
【0123】
まず、第5実施形態では、上述のステップS24において、第3実施形態と同様に、第1衝突点および第2衝突点を特定する。図15(B)は、図15(A)の場合に対して、このように特定した1対の第1衝突点Pa0および第2衝突点Pb0を示す。
【0124】
さらに、第5実施形態では、上述のステップS24において、次のような対となる第1衝突点および第2衝突点も特定する。
すなわち、互いに隣接して障害物11に至る2本の探索線の長さの差が、しきい値より大きい場合に、当該2本の探索線がそれぞれ障害物11に衝突する点となる第1衝突点および第2衝突点をも特定する。図15(B)は、図15(A)の場合に対して、このように特定した1対の第1衝突点Pa1および第2衝突点Pb1と、別の1対の第1衝突点Pa2および第2衝突点Pb2を示す。
【0125】
このステップS24で使用する前記しきい値は、移動装置の幅に相当する値であってよい。すなわち、このしきい値は、実際の長さに換算すると、移動装置の幅と同じになる。また、このステップS24で使用する前記探索線は、第3実施形態で説明した探索線と同じである。
【0126】
第5実施形態では、ステップS24において、複数対の第1衝突点および第2衝突点が特定された場合には、走行可能領域13が分岐しているとみなす。図15(B)の例では、3対の第1衝突点および第2衝突点が特定されている。
従って、分岐しているとみなされた複数本の走行可能領域13のうちいずれに進むかを決める。
【0127】
このような複数本の走行可能領域13が存在する位置において、いずれの走行可能領域13を通過するべきかの指令が移動装置の制御装置に保持されている場合には、当該指令が示す走行可能領域13へ進むようにする。すなわち、経路探索装置9は、当該指令が示す走行可能領域13に相当する対の第1衝突点および第2衝突点の間を進むとして、当該第1衝突点および第2衝突点に対して、第3実施形態または第4実施形態に上述のステップS25〜S28を行う。
【0128】
一方、このような複数本の走行可能領域13が存在する位置において、いずれの走行可能領域13を通過するべきかの指令が移動装置の制御装置に保持されていない場合には、第3実施形態による手法で特定された第1衝突点Pa0と第2衝突点Pb0の間を進むとして、当該第1衝突点Pa0と第2衝突点Pb0に対して、第3実施形態または第4実施形態による上述のステップS25〜S28を行う。
【0129】
なお、第3実施形態による手法で特定されず、第5実施形態による手法で特定された第1衝突点および第2衝突点に対しては、第4実施形態によるステップS25〜S28を行う。
【0130】
図15(C)は、図15(B)の場合に対して、第1衝突点Pa1および第2衝突点Pb1に第4実施形態による上述のステップS25、S26を行うことにより線分21を生成した場合を示す。図15(D)は、図15(C)の場合に対して、第4実施形態による上述のステップS27を行うことにより経由点Piを設定した場合を示す。
【0131】
[第6実施形態]
次に、本発明の第6実施形態による経路生成装置10と経路生成方法について説明する。第6実施形態において、以下で説明しない点は、上述の第1実施形態、第2実施形態、第3実施形態、第4実施形態または第5実施形態と同じである。
【0132】
第6実施形態では、移動装置は、回転駆動される走行車輪により路面上を走行する車両である。例えば、走行車輪は、路面に接触しながら回転駆動されるものであってよい。
【0133】
第6実施形態によると、走行時に車両に生じる振動に基づいて、路面が未舗装路面であるか舗装路面であるかを判定し、未舗装路面であると判定された場合には、路面の高低差に基づく前記路面良好度が高い移動経路候補が選択されやすくなる。
【0134】
第6実施形態では、前記車両には、その走行時に前記車両の振動を計測する振動センサ23(図16を参照)が搭載されている。振動センサ23は、本実施形態では、、鉛直方向における車両の加速度を前記振動として計測するバーチカルジャイロである。
【0135】
第6実施形態による経路生成装置10は、図16に示すように、振動センサ23により計測された振動に基づいて、前記車両が現在走行している路面が、未舗装路面と舗装路面のいずれであるかを判定する路面判定装置25を備える。
【0136】
路面判定装置25は、振動センサ23が計測した振動の大きさが所定のしきい値を越える場合には、路面が未舗装路面であると判定し、そうでない場合には、路面が舗装路面であると判定してよい。
【0137】
この判定方法の具体例を説明する。
【0138】
舗装路面においては、前記振動の大きさは、車両の走行速度に依存せず、通常は、所定値以下になる傾向がある。一方、未舗装路面においては、前記振動の大きさは、車両の走行速度が増加するに伴い、増大する傾向がある。
【0139】
そこで、路面判定装置25は、車両に搭載された速度センサにより計測された車両の走行速度が、速度しきい値を越えており、かつ、振動センサ23により計測された前記振動の大きさが、振動しきい値以下である場合には、路面が舗装路面であると判定し、そうでない場合には、路面が未舗装路面であると判定する。
速度しきい値は、当該値の走行速度で車両が走行すると、舗装路面の場合と未舗装路面の場合とで、車両の振動の大きさに差が現れる車両の走行速度の下限値である。振動しきい値は、走行速度で車両が舗装路面を走行する場合には現れにくい車両の振動の大きさであり、かつ、速度しきい値を超える走行速度で車両が未舗装路面を走行する場合に現れる車両の振動の大きさの下限値である。例えば、走行速度と振動の振幅の平均値(振動の大きさ)とは、図17のグラフに示す傾向があるので、図17のように速度しきい値と振動しきい値を設定してよい。
【0140】
車両が、車両に生じる振動を吸収するサスペンションバネを有する場合には、サスペンションバネにより、車両の加減速によりダンピング振動が車両に発生する。従って、この場合には、このようなダンピング振動の周波数より大きいカットオフ周波数(例えば3Hz)を持つハイパスフィルタが用いられる。すなわち、路面判定装置25は、振動センサ23が計測した振動のうち、前記カットオフ周波数以上の周波数の振動成分のみを抽出し、抽出した振動成分に基づいて上述の判定を行う。
【0141】
路面判定装置25は、速度センサにより計測された走行速度が前記速度しきい値を越えている時に、振動センサ23が、所定時間にわたって計測した振動の振幅に基づいて、上述の判定を行う。当該計測した振動の振幅は、前記所定時間内の各時点において計測されたものである。路面判定装置25は、当該各時点の振動の振幅(例えば、上述のハイパスフィルタにより抽出された振動成分の大きさ)の平均値(例えば2乗平均値)を前記振動の大きさとして算出し、当該平均値が、前記振動しきい値以下である場合には、路面が舗装路面であると判定し、そうでない場合には、路面が未舗装路面であると判定する。
【0142】
前記高速走行可能度は、前記路面良好度だけでなく、前記経路長さと最小曲率半径の一方または両方を含む。好ましくは、前記高速走行可能度は、その要素として、上述の経路長さ、最小曲率、路面良好度、および近接度を有する。
【0143】
経路探索装置9は、高速走行可能度の各要素の値を(例えば、上述のように0から1の範囲の値を持つ)規格値に換算して算出し、各要素の規格値の総和を高速走行可能度として算出する。
【0144】
前記経路探索装置9は、同じ移動経路候補について、前記路面が未舗装路面であると判定された場合には、前記路面が舗装路であると判定された場合よりも、前記経路長さと前記最小曲率半径の規格値を小さく算出する。
【0145】
第6実施形態による経路長さの規格値の算出方法の具体例を説明する。この具体例として算出方法1、2を説明する。
【0146】
(算出方法1)
路面判定装置25により路面が舗装路面であると判定された場合には、経路探索装置9は、経路長さの規格値を、上述の第1実施形態の場合と同じ方法で算出する。一方、路面判定装置25により路面が未舗装路であると判定された場合には、経路探索装置9は、上述の第1実施形態と同じ方法で算出された経路長さの規格値に、ゼロ以上であり1未満である値を乗算することにより得た値を経路長さの規格値として算出する。
【0147】
(算出方法2)
経路探索装置9は、経路長さの規格値を、車両の停止距離の規格値として算出する。車両の停止距離は、次の式で表される。
停止距離=V×t+V2/(2×μ×G)
ここで、Vは、車両の走行速度の上限値(車両が出せる最高速度)である。tは、後述する車両の空走距離である。μは、路面の動摩擦係数の単位を持ち、例えば、路面判定装置25により路面が舗装路面であると判定された場合には0.8であるとし、路面判定装置25により路面が未舗装路面であると判定された場合には、0.5であるとする。Gは、重力加速度である。
空走距離とは、車両の制御装置が、車両の走行時に、車両を停止させる旨の指令信号を出力してから、車両の制動装置(ブレーキ)が作動を開始するまでの時間である。
このように算出される規格値の一例を、図18(A)に示す。
【0148】
第6実施形態による最小曲率半径の規格値の算出方法の具体例を説明する。この具体例として算出方法1、2を説明する。
【0149】
(算出方法1)
路面判定装置25により路面が舗装路面であると判定された場合には、経路探索装置9は、最小曲率半径の規格値を、上述の第1実施形態の場合と同じ方法で算出する。一方、路面判定装置25により路面が未舗装路であると判定された場合には、経路探索装置9は、上述の第1実施形態と同じ方法で算出された最小曲率半径の規格値に、ゼロ以上であり1未満である値を乗算することにより得た値を最小曲率半径の規格値として算出する。
【0150】
(算出方法2)
走行速度Vで車両が走行している時に、車両が横滑りすることなく旋回できるその最小旋回半径は、次の式で表される。
最小旋回半径=V2/(μ×G)
ここで、Vは、車両の走行速度である。μは、路面の動摩擦係数である。Gは、重力加速度である。
μの値は、舗装路面の場合よりも未舗装路面の場合の方が小さい。従って、最小曲率半径の規格値は、図18(B)のように、路面が舗装路面の場合は、上述の規格値(1/n)×logRで表わされるが、路面が未舗装路面の場合は、この規格値に、ゼロより大きく1より小さい値を乗算した規格値として表されてよい。なお、図18(B)では、最小曲率半径が、走行速度がほぼゼロである時に車両が旋回できる最小旋回半径R0以下である場合には、規格値をゼロにしている。
【0151】
第6実施形態によると、未舗装路面であると判定された場合には、経路長さと最小曲率半径の規格値を小さくするので、路面良好度が高速走行可能度に寄与する度合いが大きくなる。従って、未舗装路面であると判定された場合には、路面良好度が高い移動経路候補が選択されやすくなり、未舗装路面においては路面良好度が高い移動経路を走行する制御を行うことができる。
【0152】
[経路生成装置を備える移動装置]
また、自律移動する移動装置は、第1実施形態または第2実施形態の経路生成装置10と、該経路生成装置10により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備える。
制御装置は、位置検出装置5により検出された現在位置、上記生成された移動経路(即ち、記憶装置3に設定されている移動経路)、上述の速度指令装置から出力される指令速度値、移動装置の現在の速度、および、進行方向などに基づいて移動装置の速度制御を行う上述の速度制御装置を有する。また、移動装置は、位置検出装置5により検出された現在位置、上記生成された移動経路、速度指令装置から出力される指令速度値(例えば、一定指定速度値であってもよい)、移動装置の現在の速度および進行方向などに基づいて移動装置の進行方向制御を行う方向制御装置を有する。速度制御装置は、経路探索装置9が上述のように指令速度値を調整する場合には、調整された指令速度値に基づいて移動装置の速度制御を行う。
なお、速度指令装置は、設定されている移動経路における、移動装置が通過する直前の箇所の曲率に基づいて、指令速度値を出力する。この場合、速度指令装置は、曲率が大きいほど小さい速度指令値を出力してよい。
この移動装置は、上述と同様に移動経路をより確実に見出せるので、移動装置の自律移動を継続して行える。
【0153】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。
【0154】
移動装置は、上述した実施例では車両であったが、本発明はこれに限定されない。例えば、移動装置は、自律走行または自律歩行するロボットであってもよい。この場合、上述の実施形態は適宜変更されてよい。
【0155】
前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、路面良好度、および、現在使用している移動経路と移動経路候補との近接度のうち、いずれか1つのみからなっていてもよいし、任意の2つ以上のものからなっていてもよい。
【0156】
また、本発明によると、上述した干渉条件、曲率条件、障害物検出条件、生成経路選択条件のうち1つ以上を省略したものを経路探索条件としてもよい。例えば、干渉条件は、予め障害物が存在しないことが分かっている場合には、省略されてよく、曲率条件は、必要に応じて省略されてよく、障害物検出条件は、高精度な障害物検出が要求されない場合などには、省略されてよく、生成経路選択条件は、必要に応じて省略されてよい。
【0157】
また、本発明によると、経路探索条件を用いずに複数の移動経路候補を生成してよい。すなわち、上述のステップS2では、経路探索条件を考慮することなく、複数の移動経路候補を生成し(例えば、式(Eq1)を用いた上記の方法で生成し)、これら生成した移動経路候補に対し上述のステップS4を行ってもよい。この場合、上述のステップS3は省略される。
【符号の説明】
【0158】
3 記憶装置、5 位置検出装置、7 障害物検出装置
9 経路探索装置、10 経路生成装置、11a,11b 障害物
13 走行可能領域、15 未検出領域、17 道なりライン、19,21 線分、23 振動センサ、25 路面判定装置
【技術分野】
【0001】
本発明は、自律移動する移動装置の移動経路を生成する経路生成装置と方法および経路生成装置を備える移動装置に関する。
【背景技術】
【0002】
自律移動する移動装置は、舗装路、未舗装路(オフロード)または屋内において自律的に移動するロボットや車両である。
移動装置が自律移動・走行するために、移動装置は、既知の地図上に設定された移動経路に沿って移動するように制御される(例えば、下記特許文献1)。
【0003】
この制御を行うために移動装置は、例えば記憶装置、内界情報検出装置、障害物検出装置、経路探索装置、制御装置を有する。
【0004】
記憶装置は、移動装置の移動範囲を含む領域の地図(地図情報)と、移動開始位置から目標到達位置までの設定経路(経路情報)と、を記憶する。地図情報は、例えば2次元座標による座標系である。経路情報は、移動開始位置から目標到達位置までの間に設定される複数の経由位置である。これら経由位置は後述する上記移動経路の生成・設定の指標となる。なお、これら経由位置は、上記座標系における座標であってよい。
【0005】
内界情報検出装置は、移動装置の現在位置、移動装置の移動方向、移動速度などの移動装置自体に関する情報を検出する。
【0006】
障害物検出装置は、移動装置の周囲(例えば、移動装置の進行側の所定領域内)において停止車両、凹凸地、壁などの障害物の有無およびその位置・範囲を検出する。
【0007】
経路探索装置は、例えば所定の周期で、移動装置の現在位置、障害物の位置・範囲、上記経路情報に基づいて、次の経由位置に向かうための移動経路を探索して生成する。即ち、経路探索装置は、検出された障害物を回避して現在位置から次の経由位置へ移動するための移動経路を探索して生成する。生成された移動経路は、移動装置の走行制御に使用するために記憶装置に設定・記憶される。
【0008】
制御装置は、移動装置の現在位置に基づいて、上述のように設定された移動経路に沿って移動装置が走行するように、移動装置の移動方向や速度などを制御する。
【先行技術文献】
【特許文献】
【0009】
【特許文献1】特開平7−248820号公報
【特許文献2】特開2009−110154号公報
【非特許文献】
【0010】
【非特許文献1】The International Journal Robotics Research Vol. 22, No. 7−8, July−August 2003, pp.583−601
【非特許文献2】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
【発明の開示】
【発明が解決しようとする課題】
【0011】
移動経路の探索において、より高速で移動可能な移動経路を見つけ出すことができれば、移動装置は、より高速で移動することができる。
【0012】
そこで、本発明の目的は、移動経路の探索において、より高速で移動可能な移動経路を見つけ出すことで、移動装置の高速移動を可能にすることにある。
【課題を解決するための手段】
【0013】
上記目的を達成するため、本発明によると、自律移動する移動装置の移動経路を生成する経路生成装置であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
前記移動装置の現在位置を検出する位置検出装置と、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する経路探索装置と、を備え、
前記経路探索装置は、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成装置が提供される。
【0014】
本発明の好ましい実施形態によると、前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、移動経路候補に沿った路面領域の高低差に基づいた路面良好度、および、現在使用している移動経路と移動経路候補との近接度の少なくともいずれかを含み、
前記経路探索装置は、前記各要素の値に応じて前記高速走行可能度を算出する。
【0015】
好ましくは、前記高速走行可能度は、前記各要素の値が大きいほど高い。
【0016】
本発明によると、上述の経路生成装置と、該経路生成装置により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備えることを特徴とする移動装置が提供される。
【0017】
本発明の別の実施形態によると、前記経路探索装置は、前記移動装置の現在位置と、前記移動装置に設けた障害物検出装置により検出した障害物とに基づいて、前記現在位置近傍の局所地図を生成し、
前記経路探索装置は、局所地図において、前記現在位置から多数の直線状の探索線を、障害物の有無が検出されていない未検出領域または障害物に至るまで放射状に延ばすことにより、両側が障害物に挟まれた移動装置の走行可能箇所を特定し、該走行可能箇所に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する。
【0018】
この場合、好ましくは、前記経路探索装置は、局所地図において、
(A)次に通過する経由位置に向かう方向に対する左右両側に障害物が存在する場合に、左右の障害物の間に存在する前記移動装置の走行可能領域を抽出し、
(B)走行可能領域内に位置して走行可能領域の境界に接する複数の内接円を生成し、
(C)各内接円の中心を通過するラインに基づいて道なりラインを生成し、
(D)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく前記未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(E)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(F)前記対象衝突点から延びて、前記道なりラインと直交し、対象衝突点と反対側にある前記障害物まで延びる線分を前記走行可能箇所として生成し、
(G)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する。
【0019】
代わりに、前記経路探索装置は、局所地図において、
(A)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(B)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(C)対象衝突点と反対側の前記障害物において、前記対象衝突点からの距離が最も短くなる点を設定し、当該点から前記対象衝突点まで延びる線分を前記走行可能箇所として生成し、
(D)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出してもよい。
【0020】
本発明のさらに別の実施形態によると、移動装置は、回転駆動される走行車輪により路面上を走行する車両であり、
前記車両には、その走行時に前記車両の振動を計測する振動センサが搭載されており、
前記振動センサにより計測された振動に基づいて、前記車両が現在走行している路面が、未舗装路面と舗装路面のいずれであるかを判定する路面判定装置が設けられ、
前記高速走行可能度は、その要素として、前記路面良好度を含むだけでなく、前記経路長さと最小曲率半径の一方または両方を含み、
前記経路探索装置は、前記各要素の値を規格値に換算して算出し、各要素の規格値を反映させた高速走行可能度を算出し、
前記経路探索装置は、同じ移動経路候補について、前記路面が未舗装路面であると判定された場合には、前記路面が舗装路であると判定された場合よりも、前記経路長さと前記最小曲率半径の規格値を小さく算出する。
【0021】
また、本発明によると、自律移動する移動装置の移動経路を生成する経路生成方法であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置とを設け、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する場合に、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成方法が提供される。
【発明の効果】
【0022】
上述した本発明によると、前記経路探索装置は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定するので、高速走行可能度がより高い移動経路が設定される。これにより、移動装置の高速移動が可能になる。
【図面の簡単な説明】
【0023】
【図1】本発明の第1実施形態による経路生成装置の構成図である。
【図2】高速走行可能度の各要素の規格化を示すグラフである。
【図3】移動経路候補の各評価領域ERを示す。
【図4】評価領域の評価値を算出する方法を示すフローチャートである。
【図5】図3における評価領域を示す部分拡大図である。
【図6】評価領域の路面状態を説明するための図である。
【図7】路面良好度を算出するために使用するグラフである。
【図8】現在の移動経路と移動経路候補との距離の算出を説明するための図である。
【図9】本発明の第1実施形態による経路生成方法を示すフローチャートである。
【図10】経路探索装置が生成する局所地図である。
【図11】本発明の第3実施形態による経路生成方法の説明図である。
【図12】本発明の第3実施形態による経路生成方法を示すフローチャートである。
【図13】本発明の第3実施形態による経路生成方法の他の説明図である。
【図14】本発明の第4実施形態による経路生成方法の説明図である。
【図15】本発明の第5実施形態による経路生成方法の説明図である。
【図16】本発明の第6実施形態による経路生成装置の構成図である。
【図17】走行速度と振動の振幅の平均値との関係を示す。
【図18】第6実施形態による高速走行可能度の各要素の規格化を示すグラフである。
【発明を実施するための形態】
【0024】
本発明の実施形態を図面に基づいて説明する。
【0025】
[第1実施形態]
本実施形態による経路生成装置10は、図1に示すように、記憶装置3、位置検出装置5、経路探索装置9、障害物検出装置7を備える。
【0026】
記憶装置3は、移動装置の移動範囲を含む地図に関する地図情報と、該地図における位置を示す経由位置情報(即ち、経由位置)と、を記憶する。地図情報は、2次元の地図に関する情報であってよく、この地図情報は例えばx−y座標系であってよく、与えられたx座標とy座標により地図上の位置が特定されるものであってよい。経由位置情報は、移動装置の移動開始位置から目標到達位置までの間に設定された複数の経由位置の位置情報(例えば、x座標とy座標)である。これら複数の経由位置は、移動経路の生成の指標として、経路探索装置9に使用される。なお、複数の経由位置は、移動開始位置と目標到達位置を含んでよい。
【0027】
位置検出装置5は、移動装置の現在位置を検出する。位置検出装置5は移動装置に設けられる。位置検出装置5は、例えば、GPSシステムを利用してGPSシステム(GPS衛星)から位置に関する情報を受信し、当該情報に基づいて移動装置の現在位置を検出するGPS装置であってよい。
代わりに、位置検出装置5は、移動装置の移動速度、移動方向を随時検出し、検出した移動速度と移動方向と移動装置の初期位置情報に基づいて、移動装置の現在位置を演算して検出するものであってもよい。
【0028】
障害物検出装置7は、移動装置に設けられ、移動装置の進行の妨げとなる障害物を検出する。移動装置は、移動装置の周囲(例えば、移動装置が進行する側における所定の領域内)において停止車両、凹凸地、壁などの障害物およびその位置・範囲を検出する。障害物検出装置7は、例えば、レーザ光を射出し、このレーザ光の反射光を検出することで障害物を検出するものであってよい。この場合、障害物検出装置7は、移動装置の進行方向に複数のレーザビームを射出するレーザ光源と、これら射出されたレーザビームの反射光を検出して障害物の有無およびその位置・範囲を検出する反射光検出装置と、を有する。レーザ光源は、移動装置の進行方向と垂直な水平方向に関して互いに傾いた角度で、かつ、鉛直下方側に傾けて、複数のレーザビームを射出する。反射光検出装置は、射出されたレーザビームの反射光を受光し、受光した反射光に基づいて障害物の有無およびその位置・範囲を検出する。
なお、障害物検出装置7は、上記のものに限定されず他の公知の装置であってもよい。例えば、レーザ光を用いた障害物検出装置以外に、CCDカメラなどを用いた装置を障害物検出装置7としてもよい。
【0029】
経路探索装置9は、移動装置の移動中において、位置検出装置5が検出した上記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する。経路探索装置9は、コンピュータにより構成されてよい。
【0030】
経路探索装置9は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定する。
【0031】
経路探索装置9は、複数の移動経路候補を、次の式(Eq1)により生成してよい。
κ(S)=a1+a2S+a3S2+・・・+an+1Sn ・・・(Eq1)
この式(Eq1)で表わされるκ(S)は、移動経路候補の曲率である(例えば、非特許文献1)。
この式(Eq1)において、
nは、1または2以上の整数であり、
Sは、移動装置の現在位置から前記移動経路候補上の任意位置までの前記移動経路候補に沿った経路長さを示し、該Sの上限値は、該現在位置から前記移動経路候補の終了位置までの移動経路候補に沿った移動経路長さであり、
a1〜an+1は、係数である。
この式(Eq1)において、Sの上限値、a1、a2、a1・・・、およびan+1の少なくともいずれかが互いに異なる複数のκ(S)により、それぞれ、複数の移動経路候補が特定・生成されてよい。
【0032】
各移動経路候補を表す上記κ(S)は、式(Eq1)中のSをパラメータとして、移動経路候補の方位角(即ち、移動経路候補の接線方向)と移動経路候補上の位置とを次の[数1]の式(Eq2)と(Eq3)により求めることができる。
【0033】
【数1】
【0034】
高速走行可能度について説明する。前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、路面良好度、および、現在使用している移動経路と移動経路候補との近接度の少なくともいずれか(この例では、すべて)を含む。
【0035】
前記経路探索装置は、前記各要素の値に応じて前記高速走行可能度を算出する。本実施形態では、前記高速走行可能度は、これら各要素の値が大きいほど高くなる。好ましくは、前記高速走行可能度を、各要素の規格値の総和として算出する。規格値は、例えば、0〜1の範囲の値となり、各要素の取り得る値が、0〜1の範囲とならない場合には、この範囲の値になるように変換を行う。その上で、移動経路候補の経路長さの規格値と、移動経路候補の最小曲率半径の規格値と、現在使用している移動経路と移動経路候補との近接度の規格値との総和を高速走行可能度とする。
なお、移動経路候補の経路長さが長いほど、次の移動経路候補が見つからないことにより移動装置の移動を一時的に停止させる必要性が低くなる。その結果、目標到達位置に到達するのに要する時間が短縮される。また、前記近接度が高いほど、現在使用している移動経路から移動経路候補から逸れている程度が小さくなるので、移動経路候補は、それだけ少ないカーブを有する可能性が高くなる。カーブが少ないと、高速で移動できる部分が多くなる。
【0036】
(経路長さ)
移動経路候補の経路長さは、移動経路候補の始点から終点(終了位置)までの、移動経路候補に沿った長さである。すなわち、κ(S)におけるSの上限値である。
【0037】
移動経路候補の経路長さは、図2(A)の関係に従って上述の規格値に変換される。図2(A)において、経路長さとその規格値は、比例関係にある。図2(A)の例では、生成される移動経路候補の長さが設定値を超える場合には、その規格値を1にする。
【0038】
(最小曲率半径)
移動経路候補の最小曲率半径は、移動経路候補の全範囲(移動経路候補の始点から終点まで)の各位置における曲率半径(旋回半径)のうちの最小値である。具体的には、移動経路候補の各位置の曲率半径は、上述のκ(S)の逆数1/κ(S)であるので、当該各位置における1/κ(S)のうちの最小値である。
【0039】
移動経路候補の最小曲率半径は、図2(B)の関係に従って上述の規格値に変換される。図2(B)の例では、最小曲率半径をRとして、最小曲率半径の規格値Zは、次の式(Eq4)で表わされる。
Z=(1/n)×logR ・・・(Eq4)
すなわち、規格値は、最小曲率半径Rに対して10を底とする対数のn分の1であり、nは、移動経路候補を直線と見なす曲率半径(設定値)に対して10を底とする対数値である。なお、移動経路候補の最小曲率半径が、移動経路候補を直線と見なす曲率半径(設定値)を超える場合には、その規格値を1にする。
【0040】
(路面良好度)
前記路面良好度は、移動経路候補に沿った路面領域の高低差に基づいて算出される。前記路面良好度は、次のように、移動経路候補の各評価領域について算出した評価値の最低値である。まず、対象とする移動経路候補に沿って該移動経路候補の始点から終点まで移動装置が移動した場合に、移動装置が通過することになる、移動経路候補に沿って間隔をおいた各評価領域毎に、該評価領域における各計測点の高さに基づいて、該評価領域が、走行不能であるか、段差面であるか、平面であるか、または、凹凸面であるかを判断する。その上で、移動経路候補の各評価領域の中に、走行不能であると判断されたものがある場合には、移動経路候補の路面良好度を0にし、そうでない場合には、次のようにする。すなわち、各評価領域毎に、段差面であると判断した場合には、その段差がどれだけ小さいかを示す路面良好度の評価値を算出し、平面であると判断した場合には、その路面良好度の評価値を算出し、凹凸面であると判断した場合には、その凹凸がどれだけ小さいかを示す路面良好度の評価値を算出し、移動経路候補のすべての評価領域の各評価値のうち最も小さい値を、移動経路候補の路面良好度とする。なお、移動経路候補の路面良好度は、0〜1までの値をとるので、そのまま上述の規格値となる。
【0041】
以下において、路面良好度について詳しく説明する。
【0042】
路面良好度は、移動装置の現在位置を含む2次元の局所地図(後で詳しく説明する)に組み込まれた各計測点の高さに基づいて経路探索装置9により算出される。これら各計測点の高さ(即ち、鉛直方向に関する位置座標値)は、それぞれ、実際の路面上の対応する各位置の高さであり、障害物検出装置7により、レーザビームを射出し、その反射レーザビームを検出することで得た計測データに基づいて算出されてよい。例えば、当該計測データは、前記レーザビームを反射した路面上の点と前記レーザ光源との距離であり、レーザビームの移出方向や、移動装置の姿勢などに基づいて、当該点の高さに変換されて前記局所地図上の前記計測点と対応付けられて前記局所地図に組み込まれる。
【0043】
各移動経路候補の路面良好度は、当該移動経路候補における各評価領域(図3において符号ERで示す)の評価値のうち最も低い値である。移動経路候補の各評価領域毎に、当該評価領域の各計測点の高さに基づいて評価値が算出され、算出された各評価領域の評価値のうち最も低い値を、当該移動経路候補の路面良好度とする。図3に示すように、移動経路候補の各評価領域ERは、2次元の局所地図において、移動経路候補(図3において破線で示す)上の所定間隔をおいた各位置(図3において黒丸で示す)にあり、当該位置において移動経路候補と交差(例えば、直交)する両側(水平方向両側に対応)へ、当該位置が中心となるように広がっている線状領域である。この線状領域ERの長さは、2次元の局所地図において、移動装置(車体)の幅に相当する長さ程度であるのがよい。
【0044】
各評価領域の評価値は、図4のフローチャートに従って算出される。各移動経路候補毎に、当該移動経路候補の各評価領域の評価値を図4のフローチャートに従って算出し、当該移動経路候補の各評価領域の評価値のうち最も値が低いものを、当該移動経路候補の路面良好度とする。
【0045】
図4のフローチャートに従って評価領域の評価値を算出する方法について図5などを参照して説明する。図5は、図3における複数の線状の評価領域ERのうちの任意の1つを示した部分拡大図である。
【0046】
ステップST1において、図5に示す評価領域ER上に存在し互いに間隔が置かれている複数の前記計測点(図5において白丸で示す)のうち左右の各走行タイヤ毎に、第1〜第4の計測点Pm1〜Pm4について、隣接する2つの計測点毎に、当該2つの計測点の高低差を算出し、当該2つの計測点間の勾配を算出し、これら高低差の分散を算出する。
第1〜第4の計測点Pm1〜Pm4を図5に基づいて説明する。図5において、破線は、移動装置(車両)の左右の走行タイヤを示す。図5のように、このステップST1で使用する計測点が各タイヤの幅(図における左右方向の幅)内に2〜3つ以上存在する。より詳しく述べると、図5の破線で囲まれた2つの部分は、前記局所地図上に、対象の移動経路候補が仮に後述のように設定された場合に、移動装置の左右の2つの走行タイヤが評価領域ERを通過する時のこれら走行タイヤを示す。第1および第2の計測点Pm1、Pm2は、図5に示すように、路面が平面である場合に走行タイヤが当該路面と接触する接触領域における当該走行タイヤの左右一方側端を挟む。第3および第4の計測点Pm3、Pm4は、路面が平面である場合に走行タイヤが当該路面と接触する接触領域における当該走行タイヤの左右他方側端を挟む。第1および第4の計測点Pm1、Pm4は、前記接触領域外にあり、第2および第3の計測点Pm2、Pm3は、前記接触領域内にある。
上述した2つの計測点の高低差は、当該2つの計測点の前記高さの差であり、上述した2つの計測点間の勾配は、水平面からの傾き角度であり、前記局所座標における、水平方向に関する当該2つの計測点の位置座標値の差をXとし、当該2つの計測点の前記高さの差をYとして、Atan(Y/X)である。Atanは、正接の逆関数(アークタンジェント)である。
なお、2つの計測点の高低差は、当該2つの計測点の前記高さの差であり、2つの計測点間の勾配は、水平面からの傾き角度であり、前記局所地図上の当該2つの計測点の距離に相当する実際の距離をXとし、当該2つの計測点の前記高さの差をYとして、Atan(Y/X)である。Atanは、正接の逆関数(アークタンジェント)である。
【0047】
ステップST2において、各走行タイヤに対応する第1〜第4の計測点Pm1〜Pm4について、隣接する2つの計測点(Pm1とPm2など)の高低差が、所定のしきい値以上であるかを判断する。すなわち、ステップST1で算出した高低差の中に、所定の高低差しきい値以上となるものがある場合には、ステップST3へ進む。一方、ステップST1で算出した高低差の中に、前記高低差しきい値以上となるものがない場合には、ステップST4へ進む。
【0048】
ステップST3において、対象の評価領域は通過不可能(走行不能)であるとして、評価値を0にする。
【0049】
ステップST4において、対象とする評価領域が段差面であるかどうかを判断する。すなわち、各走行タイヤに対応する第1〜第4の計測点Pm1〜Pm4について、ステップST1で算出した第1および第2の計測点Pm1、Pm2間の勾配、および、ステップST1で算出した第3および第4の計測点Pm3、Pm4間の勾配のいずれもが、平面とみなせる上限値以下である場合には、対象とする評価領域が段差面であるとして、ステップST5に進む(なお、この上限値は、前記勾配しきい値よりも小さい)。一方、各走行タイヤの少なくともいずれかに対応する第1〜第4の計測点Pm1〜Pm4について、ステップST1で算出した第1および第2の計測点Pm1、Pm2間の勾配、および、ステップST1で算出した第1および第2の計測点Pm3、Pm4間の勾配のいずれもが、平面とみなせる前記上限値以下でない場合には、ステップST6へ進む。
例えば、図6(A)の場合には、対象とする評価領域が段差面であるとして、ステップST5に進む。図6(A)〜(C)のグラフにおいて、横軸は、走行タイヤに対応する各計測点Pm1〜Pm4の水平方向位置座標を示し、縦軸は、これら計測点Pm1〜Pm4の高さを示し、白丸は、各計測点Pm1〜Pm4を示す。
【0050】
ステップST5において、ステップST1で算出した、各走行タイヤに対応する第2および第3の計測点Pm2、Pm3の高低差の大きさのうち大きいほうに応じて、評価値を算出する。例えば、図7(A)に示す関係に従って評価値を算出する。図7(A)のグラフは、計測点Pm2、Pm3の高低差の大きさが大きい方の走行タイヤの当該計測点Pm2、Pm3の高低差の大きさと評価値との関係を示す。
【0051】
ステップST6において、対象とする評価領域が平面とみなせるかどうかを判断する。なわち、ステップST1で算出した高低差の分散が、平面とみなせる上限値以下である場合には、対象とする評価領域は平面であるとして、ステップST7へ進む。ここで、分散は、ステップST1で算出した高低差に基づいて計算される。すなわち、ステップST1で算出した各高低差について、当該高低差と、ステップST1で算出した高低差の平均値との差を自乗し、これら自乗して得た値の平均値が、上述の分散である。一方、ステップST1で算出した高低差の分散が、平面とみなせる上限値以下でない場合には、ステップST8へ進む。
例えば、図6(B)の場合には、対象とする評価領域が平面であるとして、ステップST7に進む。
【0052】
ステップST7において、ステップST6で算出した分散に応じて、評価値を算出する。例えば、図7(B)に示す関係に従って評価値を算出する。図7(B)のグラフは、分散と評価値との関係を示す。
【0053】
ステップST8において、対象とする評価領域が凹凸面であるとする。例えば、図6(C)の場合には、対象とする評価領域が凹凸面であるとする。
また、ステップST8において、ステップST1で算出した高低差の大きさのうち最大となるもの(最大高低差という)に応じて、評価値を算出する。例えば、図7(C)に示す関係に従って評価値を算出する。図7(C)のグラフは、最大高低差と評価値との関係を示す。
【0054】
(現在の移動経路と移動経路候補との近接度)
この近接度は、現在、使用・設定されている移動経路(以下、現在の移動経路という)と移動経路候補との距離を用いて算出される。
【0055】
この距離を図8に基づいて説明する。図8は、前記局所地図上に移動経路候補を示した図である。現在の移動経路と移動経路候補との距離は、経路探索装置9により、図8の各対応点間距離L1〜LPの総和として算出される。まず、当該移動経路候補上(図8の破線上)における複数のサンプル点P1〜Pnを取り、当該各サンプル点に最も近い現在の移動経路上(図8の実線上)の点を対応点として求める。次に、各サンプル点毎に、当該サンプル点とこれの対応点との距離を対応点間距離として算出する。その後、各サンプル点の対応点距離L1〜LPの総和を、現在の移動経路と移動経路候補との距離として算出する。
【0056】
なお、各移動経路候補のサンプル点の取り方は、図8に示す方法であってよい。すなわち、移動距離の現在位置から移動経路候補に沿って一定距離だけ仮想的に進行した点を最初のサンプル点P1とし、このサンプル点P1から移動経路候補に沿って一定距離だけさらに仮想的に進行した点を次のサンプル点P2とし、このサンプル点P2から移動経路候補に沿って一定距離だけ仮想的にさらに進行した点を次のサンプル点P3とするといった具合に複数のサンプル点P1〜Pnを取ってよい。移動距離の現在位置から移動経路候補に沿って最も離れた最後のサンプル点Pnは、現在の移動経路の終点から最も近い移動経路候補上の点であってよい。
【0057】
上記のように算出された、現在の移動経路と移動経路候補との距離は、図2(C)の関係に従って、前記近接度(すなわち、前記規格値)に変換される。図2(C)の例では、算出された距離が設定値を超える場合には、その規格値を0にする。
【0058】
本実施形態によると、経路探索装置9は、所定の経路探索条件の下で、または、後述のように変更した経路探索条件の下で、生成されて見出された複数の移動経路候補について、上述のように、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定する。
【0059】
経路探索装置9は、前記経路探索条件の下で前記移動経路候補を見出せなかった場合に、前記経路探索条件を変更し、該変更した経路探索条件の下で前記移動経路候補を再び探索する。
前記経路探索条件(即ち、初期の経路探索条件)は、障害物検出装置7が検出した障害物と移動経路候補とが干渉しないという干渉条件と、移動装置が指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという障害物検出条件と、を含む。この場合、前記経路探索装置9は、前記経路探索条件の下で前記移動経路候補を見出せなかった場合に、当該経路探索条件の障害物検出条件において前記速度制御に使用された前記指令速度値(前回の指令速度値ともいう)よりも低速度で移動装置が移動するように、速度指令装置から出力された指令速度値を調整し(即ち、現時点の指令速度値が前回の指令速度値より大きい場合には、減少させ)、これにより、移動装置が該調整された指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うことで、経路探索条件を変更し、該変更した経路探索条件の下で移動経路候補を再び探索する。
【0060】
また、前記経路探索条件(即ち、初期の経路探索条件)は、前記移動経路候補の曲率が該移動経路候補の全範囲にわたって所定の曲率基準値以下になるという曲率条件を含んでよい。
【0061】
前記移動経路候補の曲率に関する速度制御について説明する。上記速度指令装置は、後述のように設定された移動経路における、移動装置が通過する直前の箇所の曲率に基づいて指令速度値を出力する。具体的には、上記速度指令装置は、横滑りおよび横転することなく当該箇所を移動装置が進行できるように、当該曲率に応じた指令速度値(例えば、当該曲率が大きいほど小さい指令速度値)を出力する。出力された指令速度値は、速度制御装置に入力され、この速度制御装置は、入力された指令速度値(即ち、この値が示す速度)で移動装置が移動するように移動装置の速度を制御する。これにより、移動装置は、設定された移動経路において、曲率の小さい箇所を通過する時ほど、より高速で移動する。
【0062】
なお、上述では、指令速度値を調整することで経路探索条件を変更したが、この変更を行わないようにすることもできる。例えば、前記経路探索条件は、障害物検出条件を省いて、干渉条件と曲率条件を含み、経路探索装置9は、前記経路探索条件の下で前記移動経路候補を見出せなかった場合に、前記所定の曲率基準値を増加することで経路探索条件を変更してもよい。
【0063】
また、前記経路探索装置9は、前記経路探索条件を満足する移動経路候補を見出せるまで、前記経路探索条件を変更し該変更した経路探索条件の下で移動経路候補を再び探索する処理を繰り返す。
【0064】
次に、経路生成装置10による移動経路の探索・生成について詳しく説明する。なお、以下の説明は、経路探索条件が干渉条件、障害物検出条件および曲率条件を含む場合と、障害物検出条件を省き経路探索条件が干渉条件および曲率条件を含む場合の両方に対応している。
【0065】
図9は、経路生成装置10による経路生成方法を示すフローチャートである。
【0066】
ステップS1では、経路探索装置9は、2次元の前記局所地図を生成する。即ち、経路探索装置9は、位置検出装置5により検出された移動装置の現在位置と障害物検出装置7による障害物探索領域とを含む範囲の地図情報を記憶装置3から抽出し、当該範囲の局所地図を生成する。この局所地図には、次の経由位置への方向情報が組み込まれていてよい。方向情報は、例えば、直前に通過した経由位置と次の経由位置とを結ぶ直線の方向であってよく、または、移動装置の現在位置から見た次の経由位置の方向であってもよい。この局所地図には、次の経由位置自体が組み込まれていてもよい。
なお、ステップS1で使用される現在位置と、障害物検出装置7による障害物検出結果情報(障害物の位置と範囲)とは、ステップS1を開始する毎に、それぞれ位置検出装置5および障害物検出装置7により獲得され、局所地図に組み込まれる。
【0067】
ステップS2では、移動経路候補を探索する。具体的には、前記経路探索装置9は、上記κ(S)の式を用いて、次の処理(A)または(B)を行う。
(A)前記複数の係数a1〜an+1および前記Sの上限値にそれぞれ所定の値を与えることで得られるκ(S)について、該κ(S)により定まる移動経路候補が前記経路探索条件(干渉条件と曲率条件)を満たすかを判断する。
(B)前記(A)の処理を複数回行う場合に、前記(A)の処理を行うごとにa1〜an+1のうち少なくとも1つの値を変更して前記(A)の処理を行う。なお、前記Sの上限値は一定にしたまま前記(A)の処理を複数回行ってよいし、複数の前記(A)の処理うち少なくとも1回の処理で前記Sの上限値を変更してもよい。
経路探索装置9が、前記(A)または(B)の処理により、前記経路探索条件を満たす移動経路候補を見出せなかった場合には、ステップS3に進み、経路探索条件を満たす移動経路候補を見出せた場合、ステップS4へ進む。
【0068】
ステップS3では、経路探索条件を変更してステップS1に戻る。具体的には、ステップS3で、経路探索装置9は、前記指令速度値を上述のように調整し、ステップS1で、移動装置が該調整された指令速度値に基づいて速度制御されている状態で障害物検出装置7が障害物検出を再び行い。この障害物検出結果に基づいて上記局所地図を生成する。この時、移動装置の現在位置も位置検出装置5により再び検出され、この現在位置と再度行われた障害物検出結果とに基づいて局所地図が生成されてよい。
また、ステップS3では、経路探索装置9は、上述のように前記指令速度値を調整するとともに、前記曲率基準値を増加させてもよい。
なお、ステップS3において、指令速度値を調整することを行わないようにしてもよい。即ち、単に前記曲率基準値を増加することで経路探索条件を変更してもよい。この場合には、図9と異なり、ステップS3からステップS1を経由せずにステップS2へ戻ってもよいし、図9のように、ステップS3からステップS1へ戻って新たな現在位置と障害物検出結果とに基づいて再び局所地図を生成してもよい。
【0069】
ステップS4では、ステップS2で見出した複数の移動経路候補について、上述のように、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定する。
なお、ステップS2で見出した移動経路候補が1つのみである場合には、ステップS4を省略して、ステップS2からステップS5へ進む。
【0070】
ステップS5では、ステップS4で選択した移動経路候補を実際に使用する移動経路として設定して移動経路を更新する。即ち、移動装置の進行制御にこれまで使用していた設定移動経路を、ステップS4で選択した移動経路候補に置き換えることで、設定移動経路を更新する。なお、設定移動経路は記憶装置3に記憶される。更新したらステップS6へ進む。また、ステップS4を省略した場合には、ステップS2で見出した1つのみの移動経路候補を移動経路として設定して移動経路を更新する。
【0071】
ステップS6では、経路探索装置9は、ステップS1で、局所地図の生成を開始してから所定時間(例えば、100ms)経過したかを、経路探索装置9に内蔵されたタイマーの時間計測により判断する。所定時間経過したと判断した場合には、ステップS1に戻り、所定時間経過していないと判断した場合にはステップS6の判断を繰り返す。
これにより、所定時間経過毎に、局所地図の生成および経路探索を行う。これに対応して、位置検出装置5は、上記の所定時間経過毎に移動装置の現在位置を検出し、障害物検出装置7は、上記の所定時間経過毎に障害物の検出を行う。
ステップS6からステップS1に戻りステップS2へ戻った時、このステップS2での経路探索は、変更した経路探索条件ではなく初期の経路探索条件(例えば、初期の干渉条件と障害物検出条件と曲率条件)で行う。
【0072】
なお、ステップS1〜S4、S6の処理の最中に、移動装置の制御装置は、記憶装置3に記憶された上記の設定移動経路に沿って移動装置が移動するように移動装置を制御する。
【0073】
[第1実施形態の作用効果]
上述した第1実施形態によると、経路探索装置9は、複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、該選択した移動経路候補を実際に使用する移動経路として設定するので、高速走行可能度がより高い移動経路が設定される。これにより、移動装置の高速移動が可能になる。
【0074】
なお、移動経路候補が見出せない原因として、移動装置が高速で移動することにより移動装置が揺動・振動することで、障害物を誤って検出してしまう場合がある。例えば、揺動・振動により障害物検出装置の検出方向が下方側に変更されてしまい路面自体を障害物として検出してしまう場合がある。
これに対し、第1実施形態では、移動経路候補を見出せなかった場合に、上述のように指令速度値を調整し、移動装置がより低速で移動している状態で障害物検出装置7が障害物検出を行うので、障害物の誤検出により移動経路候補が見出せない可能性を効果的に小さくできる。
【0075】
また、障害物検出装置7が検出した障害物(例えば、停止車両、穴、岩、壁など)と移動経路候補とが干渉しないという干渉条件を含む経路探索条件の下では、該障害物により移動経路候補を見出せない場合がある。この場合、上記移動経路候補の曲率の曲率基準値を増加することで、障害物と干渉しない移動経路候補、即ち、障害物を回避するように比較的大きい曲率・カーブを有する移動経路候補を見出せる可能が効果的に高まる。これにより、移動経路候補を見出し移動装置の移動を継続させられる可能性を効果的に高めることができる。
【0076】
[第1実施形態の実施例]
次に、第1実施形態の実施例について説明する。この実施例では、移動装置は車両であってよい。
【0077】
経路探索装置9は、障害物検出装置7が実際に検出した障害物の範囲を所定の大きさだけ拡大して上記局所地図上で認識する。即ち、障害物検出装置7が実際に検出した障害物の範囲が、所定の大きさ(例えば、車両幅の半分の大きさ+局所地図上の1ピクセル)だけすべての方向において拡大されて、局所地図上で認識・設定される。
【0078】
本実施例によると、移動経路候補の曲率を上述のようにκ(S)とするが、κ(S)をSの3次多項式とする。即ち、κ(S)を次の式(Eq5)で表す。
κ(S)=a1+a2S+a3S2+a4S3 ・・・(Eq5)
この式において、
Sは、移動装置の現在位置から前記移動経路候補上の任意位置までの前記移動経路候補に沿った経路長さを示し、該Sの上限値は、該現在位置から前記移動経路候補の終了位置までの移動経路候補に沿った移動経路長さであり、
a1〜a4は、パラメータ(係数)である。
【0079】
上記κ(S)によって、移動経路候補を表すことができる。即ち、a1〜a4に所定の値を与えることにより、言い換えると、a1〜a4の値が定まると移動経路候補が定まる。本実施例では、次の3つの境界条件を与えることで、例えば、a1、a3、a4をa2の関数で表現できる。
【0080】
境界条件:
(1)移動経路候補の開始点、即ち、移動装置の現在位置における曲率についての次の境界条件(Eq6)
κ(0)=κ0=a1 ・・・(Eq6)
(2)探索・生成対象の移動経路候補の終了点Sf(即ち、Sの上限値)における曲率についての次の境界条件(Eq7)
κ(Sf)=κf=a1+a2Sf+a3Sf2+a4Sf3 ・・・(Eq7)
(3)探索・生成対象の移動経路候補の終了点Sfにおける移動経路方向(移動経路の接線方向)についての次の境界条件(Eq8)
θ(Sf)=θf=a1Sf+a2Sf2/2+a3Sf3/3+a4Sf4/4
・・・(Eq8)
なお、κ0,κf,θfは、既知であるとする。また、好ましくは、κfは、ゼロであり、θfは、移動経路候補の終了点と次の経由地点を結ぶ直線と同じ方向である。
【0081】
上記境界条件から,次の式(Eq9)、(Eq10)
a3Sf2+a4Sf3=κf−κ0−a2Sf ・・・(Eq9)
a3Sf3/3+a4Sf4/4=θf−κ0Sf−a2Sf2/2 ・・・(Eq10)
が得られ、従って、上記式(Eq9)、(Eq10)などから次の関係式1が得られる。
[関係式1]
a1=κ0
a3=(−3κf−9κ0)/Sf2−3a2/Sf+12θf/Sf3
a4=4(κf+2κ0)/Sf3+2a2/Sf2−12θf/Sf4
【0082】
よって、a2をパラメータとして移動経路候補(軌道)を一意に求めることができる。これら、a1、a3およびa4の各々とa2、κ0、κf、Sfとの上記関係式1は記憶装置3に記憶され、経路探索時に使用される。なお、上記のような境界条件を設定することで、a2の値を与えると、a1、a1、a4を短時間の演算で求めることができる。その結果、κ(S)により移動経路候補を短時間で特定できる。
【0083】
この実施例では、経路探索条件は、障害物検出装置7が検出した障害物と移動経路候補とが干渉しないという干渉条件と、移動装置が前記指令速度値に基づいて速度制御されている状態で障害物検出装置が障害物検出を行うという障害物検出条件と、前記移動経路候補の曲率が該移動経路候補の全範囲にわたって曲率基準値以下になるという曲率条件と、設定長さを有する1つ以上の移動経路候補から干渉条件を満たすものを生成用の移動経路候補として選択するという生成経路選択条件と、を含む。
【0084】
(曲率条件について)
移動装置が走行する装置(例えば、車両)である場合、曲率κ(S)の上限値は、例えば、次の、κturn、κslip、κrolloverのうちの最も小さいものである。
(1)車両の最小旋回可能半径を示す移動経路候補の曲率κturn。
(2)車両が、所定速度で走行する時に、横滑りを起こさない限界の曲率κslip。即ち、車両が、所定速度で走行する場合に、移動経路候補上における曲率κslipより大きい曲率κ(S)の箇所で車両が横滑りする。
(3)車両が、前記所定速度で走行する時に、横転を起こさない限界の曲率κrollover。即ち、車両が、前記所定速度で走行する場合に、移動経路候補上における曲率κrolloverより大きい曲率κ(S)の箇所で車両が横転する。
なお、通常は、曲率κslipおよび曲率κrolloverの値は、曲率κturnの値よりも小さい。また、曲率κslipおよび曲率κrolloverの値は、車両の前記所定速度により変化し、一般的に、車両の前記所定速度が大きいほど減少する。また、曲率κslipの値は、車両とその走行面との動摩擦係数にも依存する。
従って、κ(S)が次の大小関係を満足する場合には、曲率条件が満たされることになる。
κ(S)≦κturn、かつ、κ(S)≦κslip、かつ、κ(S)≦κrollover、
このような曲率κslipおよび曲率κrolloverは移動装置(例えば、車両)の特性などに基づいて求めることができる。参考のために述べると、曲率κslipは、例えば非特許文献2に記載された式(2)であってもよく、曲率κrolloverは、非特許文献2に記載された式(4)であってもよい。
【0085】
(生成経路選択条件について)
経路探索装置9は、上記κ(S)により定められる移動経路候補を探索する時に、Sの上限値Sfを設定長さの値に固定して、上記パラメータa2の値を与えることで、上記関係式1からa1、a3、a4の値を定め、これにより、定まる移動経路候補が干渉条件と曲率条件を満たすかを判断する。
【0086】
(設定値について)
経路探索装置9は、経路探索に使用するパラメータ・係数などに所定の値を与えて経路探索を行う。そのために、記憶装置3は、予め設定されたこれらパラメータ・係数の値を記憶する。具体的には、記憶装置3は、経路長Sの上限値Sf、a2、κ0,κf,θfなどの各設定値を記憶する。
また、複数の移動経路候補を生成するために、a2の複数の値を設定しておく。例えば、a2の最小値a2minおよびa2の最大値a2maxと、この最小値からのa2の増加量da2とを設定しておき、経路探索装置9は、a2min、a2min+da2、a2min+2da2、a2+3da2・・・、a2maxのそれぞれついて移動経路候補が定まるようにしておく。これにより、複数(例えば、21本)の移動経路候補を生成できる。
記憶装置3は、曲率κ(S)の前記曲率基準値を記憶する。
また、κslipやκrolloverが移動装置の速度などの変数に依存する場合には、これら変数とκslipとの関係式と、これら変数とκrolloverとの関係式も記憶装置3に記憶させる。これにより、経路探索装置9は、任意の時に、これら変数の値に基づいて当該関係式を用いてκslipとκrolloverを演算し、κturn、κslip、κrolloverの大小を比較し、これらのうち最小のものを前記曲率基準値として特定する。なお、κturnは記憶装置3に記憶された固定値であってよい、
さらに、記憶装置3は、unknown領域に関する上述の所定の閾値、後述する指令速度値減少量・減少率、および経路長増加量・増加率などを記憶する。
【0087】
次に、本実施例による移動経路探索方法について説明する。この方法は、図9に示すフローチャートと同じ方法であるが、この実施例では、図9の方法の一例としてより詳しく説明する。以下では、上述しなかった点を詳しく述べるが、以下で述べない点は、上述と同じ内容であってよい。
【0088】
ステップS1では、上述のように局所地図を生成する。
【0089】
ステップS2では、移動経路候補を探索する。具体的には、上述のように設定した複数のa2のそれぞれをκ(S)の式に適用する。a1、a3、a4の値は、a2の各設定値ごとに関係式1のようにa2、κ0、κf、Sf、θfを用いて得られる。このようにして、複数の移動経路候補が生成される。
これら移動経路候補が、干渉条件および曲率条件を満足するかを判断する。曲率条件については、各移動経路候補について、κ(S)が、0≦S≦Sfの全範囲にわたって、上述したκ(S)の曲率基準値以下であるかを判断する。曲率基準値以下である場合には、曲率条件が満たされることになる。
このようにして、干渉条件および曲率条件を満たす移動経路候補が存在しない場合には、即ち、移動経路候補のいずれもが干渉条件および曲率条件の両方を満足しない場合には、移動経路候補が見出されなかったことになり、ステップS3へ進む。一方、干渉条件および曲率条件を満たす移動経路候補が存在する場合には、移動経路候補が見出されたことになり、ステップS4へ進む。
【0090】
ステップS3では、経路探索条件を変更する。この実施例では、上述の指令速度値の調整に加えて、前記曲率基準値を増加し、かつ、経路長さSの上限値Sfの値も増加させることで、経路探索条件を変更する。なお、曲率基準値の増加は、記憶装置3に記憶された曲率基準値拡大量・拡大率に基づいて行い、上限値Sfの値の増加は、記憶装置3に記憶されや経路長増加量・増加率に基づいて行う。経路探索条件を変更したら、ステップS2へ戻り、変更した経路探索条件の下で、経路探索を行う。この再度の経路探索処理は、経路探索条件を変更した以外は、前回、ステップS2で行った処理と同じである。
このようにすることで、前回の経路探索では、移動経路候補上に障害物が存在していた場合でも、Sfを増加させることで、上記関係式1によりa3、a4の値も変化し、κ(S)により定まる移動経路候補の経路も前回と異なったものとなる。しかも、生成される移動経路候補の全長も大きくなる。これにより、例えば図10に模式的に示すように、障害物を回避するように延びる移動経路候補が生成される可能性が効果的に高まる。なお、図10において、複数の細い曲線は、前回生成された移動経路候補を示し、太い曲線は今回生成された移動経路候補(図10では、簡単のため1本のみ)を示す。
【0091】
ステップS4の処理は、上述と同じである。
【0092】
ステップS5では、上述と同様に設定移動経路を更新する。
【0093】
ステップS6では、上述のように局所地図の生成を開始してから所定時間(例えば、100ms)経過したかを判断する。所定時間経過したと判断した場合には、ステップS1に戻り、所定時間経過していないと判断した場合にはステップS6の判断を繰り返す。
【0094】
[第2実施形態]
本発明の第2実施形態について説明する。第2実施形態による経路生成装置と方法は以下で述べる点で、第1実施形態と異なる。第2実施形態による経路生成装置と方法の他の内容は第1実施形態と同じであってよい。
【0095】
第2実施形態によると、前記経路探索装置9は、経路探索時に、次の(a),(b)の処理を行う。
(a)前記経路探索装置9は、前記1つ以上の移動経路候補が干渉条件を満足するかを判断する。
(b)該移動経路候補のいずれも干渉条件を満足しないと判断した場合に、前記1つ以上の移動経路候補を変更し、該変更された1つ以上の移動経路候補が前記干渉条件を満足するかを判断する。
【0096】
前記1つ以上の移動経路候補の変更は、前記設定長さの増加と、変更した前記1つ以上の移動経路候補の各々における前記現在位置からその任意の途中位置までの経路が、変更前の前記1つ以上の移動経路候補のいずれとも異なることと、によりなされている。
【0097】
第2実施形態によると、第1実施形態で述べたステップS2の処理を、第1実施形態と異なり次のように行う。
即ち、前記経路探索装置9は、上記κ(S)=a1+a2S+a3S2+・・・+an+1Snの式を用いて、次の処理(A)または(B)を行う。
(A)前記Sの上限値および前記複数の係数a1〜an+1にそれぞれ所定の値を与えることで得られるκ(S)について、該κ(S)により定まる前記移動経路候補が干渉条件を満たすかを判断する。
(B)前記(A)の処理を複数回行う場合に、前記(A)の処理を行うごとにa1〜an+1のうち少なくとも1つの値を変更して前記(A)の処理を行い、これにより、複数回の処理で得られる複数の前記移動経路候補について干渉条件を満たすかを判断する。
【0098】
また、第2実施形態によると、ステップS2において、前記(A)または(B)の処理において前記移動経路候補のいずれも干渉条件を満足しないと判断した場合に、ステップS3へ進む。
第2実施形態では、ステップS3において、前記Sの上限値を増加させ、かつ、前記係数a1〜an+1の少なくともいずれかの値を、対応する当該係数に前記(A)または(B)の処理で与えた値から変更することで、前記1つ以上の移動経路候補を変更し、前記(A)または(B)の処理を再び行う。
【0099】
[第3実施形態]
次に、本発明の第3実施形態による経路生成装置10と経路生成方法について説明する。第3実施形態による経路生成装置10と経路生成方法は以下で述べない点は、第1実施形態または第2実施形態と同じであってもよいし、第1実施形態または第2実施形態から適宜変更してもよい。
【0100】
上述のステップS1において、前記経路探索装置9は、前記移動装置の現在位置Xと、前記移動装置に設けた障害物検出装置7により検出した障害物とに基づいて、現在位置X近傍の局所地図を生成する。図11(A)は、この局所地図の一例である。図11(A)において上側が次に通過する経由位置(次の経由位置)の方向である。なお、経路探索装置9は、さらに、障害物が存在する領域と障害物が存在しない領域を示す既知の地形データにも基づいて、局所地図における後述の走行可能領域13と未計測領域15を特定してもよい。この地形データは、経路探索装置9のメモリに記憶されていてもよいし、無線通信で経路探索装置9に受けられてもよい。
【0101】
上述のステップS2は、第3実施形態では、次のように第1実施形態または第2実施形態の場合と異なる。
【0102】
ステップS2は、図12のフローチャートのように、ステップS21〜S28を有する。
【0103】
ステップS21では、経路探索装置9は、2次元の前記局所地図において、次に通過する経由位置に向かう方向に対する左右両側に障害物11a、11bが存在する場合に、左右の障害物11a、11bの間に存在する前記移動装置の走行可能領域13を抽出する。この走行可能領域13は、障害物が存在しない領域であり、左右の障害物11a、11bと接する境界を有する。図11(A)では、編目部分が障害物11a、11bである。
図11(A)の例では、障害物検出装置7により、または、障害物検出装置7と前記地形データにより、障害物11a、11bおよび走行可能領域13が検出される。図11(A)において、他の部分は、障害物検出装置7により障害物11a、11bの有無が検出されていない未検出領域15である。なお、図11(A)のように、移動装置の走行中の揺れにより、障害物検出装置7により検出された走行可能領域13が不連続となる場合もある。
【0104】
ステップS22では、経路探索装置9は、局所地図において、走行可能領域13内に位置して走行可能領域13の境界に接する複数の内接円を生成する。これらの内接円は、移動装置の現在位置Xから次に通過する経由位置の側へ間隔をおいて生成される。
図11(B)は、図11(A)の場合に対して生成された内接円を示す。現在位置Xの側から順に、左右の障害物11a,11bの両方に接する内接円が生成される。
次に生成する内接円(対象内接円という)の中心は、この対象内接円の直前に生成された内接円(1つ手前の内接円という)の中心と、この1つ手前の内接円の直前に生成された内接円(2つ手前の内接円という)の中心とを通る直線と直交する直線(直交直線という)上に位置する。当該次に生成する内接円は、当該直交直線上に中心が位置する最大の内接円である。
なお、当該直交直線は、次に生成する内接円が何番目のものであるかにかかわらず、次に生成する当該内接円に対して2つ手前の内接円の中心から一定の距離だけ離れた位置に生成されてよい。最初の内接円は、現在位置X近傍に生成され、2番目の内接円は、現在位置Xと最初の内接円の中心とを通る直線と直交する直線上に中心が位置するように生成されてよい。
【0105】
ステップS23では、経路探索装置9は、局所地図において、各内接円の中心を通る道なりライン17を生成する。図11(C)は、図11(B)の場合に対して生成された道なりライン17を示す。道なりライン17は、隣接する各対の内接円の中心を結ぶことにより生成した折れ線を、適宜の手法(例えば、最小二乗法やRANSAC法)で近似された曲線であってもよい。
【0106】
ステップS24では、前記現在位置Xから多数の直線状の探索線を放射状に延ばす。図11(D)は、図11(A)の場合に対して生成された多数の探索線を示す。
互いに連続して隣接し障害物11a,11bに衝突することなく未検出領域15に至る複数の探索線(図11(D)において符号Gで示す)のうち一方端の探索線Laと隣接する探索線Lbが、左側の障害物11aと衝突する衝突点を第1衝突点Paとして特定し、さらに、前記複数の探索線(図11(D)において符号Gで示す)のうち他方端の探索線Lcと隣接する探索線Ldが、右側の障害物11bと衝突する衝突点を第1衝突点Pbとして特定する。好ましくは、隣接する探索線のなす角度は、いずれの隣接する探索線についても同じであるのがよい。
【0107】
ステップS25では、第1衝突点Paと第2衝突点Pbを比較し、両衝突点のうち現在位置Xに近い方を、対象衝突点として特定する。図11(D)の例では、第2衝突点Pbが対象衝突点になる。
【0108】
ステップS26では、前記対象衝突点から延びて、道なりライン17と直交し、第1衝突点Paと第2衝突点Pbをそれぞれ含む左右の障害物11a、11bのうち対象衝突点と反対側の障害物まで延びる線分19を生成する。図13(A)は、図11の場合に対して生成された線分19を示す。
【0109】
ステップS27では、生成した線分19の中心位置を経由点Piに設定する。なお、この中心位置は、線分19の両端から等距離にある点である。図13(B)は、図13(A)の場合に対して設定された経由点Piを示す。
【0110】
ステップS28では、ステップS26で生成した前記線分19(すなわち、線分19上の点)に至る複数の移動経路候補を生成する。好ましくは、これらの移動経路候補は、経由点Piにおいて道なりライン17と同じ方向を向く経路である。すなわち、これらの各移動経路候補と線分19との交点における当該移動経路候補の接線と、道なりライン17と線分19との交点における道なりライン17の接線とが平行になる。ステップS28において、前記複数の移動経路候補を生成する方法は、このように線分19に至る移動経路候補を生成する点以外は、上述の第1実施形態または第2実施形態のステップS2において複数の移動経路候補を生成する方法と同じであってよい。例えば、上述の経路探索条件をさらに満足する複数の移動経路候補が生成されてよい。
【0111】
ステップS28において、線分19に至る複数の移動経路候補が生成されたら、上述のステップS4へ進み、経路探索装置9は、これらの移動経路候補の各々について高速走行可能度を算出する。
【0112】
ステップS4では、高速走行可能度の要素として、経由点Piからの距離をさらに含む。経由点Piからの距離とは、ステップS28で生成した移動経路候補と線分19との交点と、経由点Piとの距離である。経由点Piからの距離の規格値は、当該距離がゼロである場合には1であり、当該距離がゼロから増加するにつれて次第に減少し、線分19の長さの1/2から当該距離を引いた値が移動装置の幅の1/2以下になるとゼロになる。このように、経由点Piを通る場合に、最も高速走行が可能であるとして規格値も最大になる。
従って、経路探索装置9は、第1実施形態または第2実施形態の各要素の規格値だけでなく、経由点Piからの距離の規格値も考慮して、(例えば、すべての要素の規格値の総和として)高速走行可能度を算出する。
【0113】
第3実施形態によると、上述のように内接円を利用して道なりライン17を生成できるので、図11のように走行可能領域13の幅が急激に狭くなる部分に対して、障害物に接触する可能性を低くするように、より安全で高速に走行できる移動経路を生成できるようになる。
【0114】
[第4実施形態]
次に、本発明の第4実施形態による経路生成装置10と経路生成方法について説明する。第4実施形態による経路生成装置10と経路生成方法は、第3実施形態の変更例であり、以下で述べない点は、第3実施形態と同じである。
【0115】
第4実施形態では、上述のステップS22、S23が省略される。すなわち、ステップS21の次にステップS24を行う。
【0116】
また、第4実施形態では、ステップS26を次のように行う。
すなわち、ステップS26において、第1衝突点Paと第2衝突点Pbをそれぞれ含む障害物11a、11bのうち対象衝突点と反対側にある前記障害物の境界において、前記対象衝突点から距離が最も短くなる点を設定し、当該点から前記対象衝突点まで延びる線分21を生成する。図14(A)は、図11(D)の場合に対して生成された線分21を示す。
【0117】
ステップS27では、ステップS26で生成された線分21の中心位置を経由点Piに設定する。図14(B)は、図14(A)の場合に対して設定された経由点Piを示す。
【0118】
ステップS28では、線分21に至る(すなわち、線分21上の点に至る)複数の移動経路候補を生成する。好ましくは、これらの移動経路候補は、経由点Piにおいて、ステップS25で生成された線分21と直交する経路である。
【0119】
ステップS28の次に行うステップS4では、高速走行可能度の要素として、第3実施形態と同様に、経由点Piからの距離をさらに含む。経由点Piからの距離とは、ステップS28で生成した移動経路候補が線分21との交点と経由点Piとの距離である。経由点Piからの距離の規格値は、当該距離がゼロである場合には1であり、当該距離がゼロから増加するにつれて次第に減少し、線分23の長さの1/2から当該距離を引いた値が移動装置の幅の1/2以下になるとゼロになる。このように、経由点Piを通る場合に、最も高速走行が可能であるとして規格値も最大になる。
【0120】
第4実施形態によると、上述のように線分21と経由点Piに基づいて移動経路候補を生成して選択するので、図14のように走行可能領域13の幅が急激に狭くなる部分に対して、障害物に接触する可能性を低くするように、より安全で高速に走行できる移動経路を生成できるようになる。
【0121】
[第5実施形態]
次に、本発明の第5実施形態による経路生成装置10と経路生成方法について説明する。第5実施形態による経路生成装置10と経路生成方法は、第3実施形態または第4実施形態の変更例であり、以下で述べない点は、第3実施形態または第4実施形態と同じである。
【0122】
第5実施形態は、走行可能領域13が分岐している場合に関する。例えば、第5実施形態は、上述のステップS1で生成された局所地図が図15(A)のような場合に関する。
【0123】
まず、第5実施形態では、上述のステップS24において、第3実施形態と同様に、第1衝突点および第2衝突点を特定する。図15(B)は、図15(A)の場合に対して、このように特定した1対の第1衝突点Pa0および第2衝突点Pb0を示す。
【0124】
さらに、第5実施形態では、上述のステップS24において、次のような対となる第1衝突点および第2衝突点も特定する。
すなわち、互いに隣接して障害物11に至る2本の探索線の長さの差が、しきい値より大きい場合に、当該2本の探索線がそれぞれ障害物11に衝突する点となる第1衝突点および第2衝突点をも特定する。図15(B)は、図15(A)の場合に対して、このように特定した1対の第1衝突点Pa1および第2衝突点Pb1と、別の1対の第1衝突点Pa2および第2衝突点Pb2を示す。
【0125】
このステップS24で使用する前記しきい値は、移動装置の幅に相当する値であってよい。すなわち、このしきい値は、実際の長さに換算すると、移動装置の幅と同じになる。また、このステップS24で使用する前記探索線は、第3実施形態で説明した探索線と同じである。
【0126】
第5実施形態では、ステップS24において、複数対の第1衝突点および第2衝突点が特定された場合には、走行可能領域13が分岐しているとみなす。図15(B)の例では、3対の第1衝突点および第2衝突点が特定されている。
従って、分岐しているとみなされた複数本の走行可能領域13のうちいずれに進むかを決める。
【0127】
このような複数本の走行可能領域13が存在する位置において、いずれの走行可能領域13を通過するべきかの指令が移動装置の制御装置に保持されている場合には、当該指令が示す走行可能領域13へ進むようにする。すなわち、経路探索装置9は、当該指令が示す走行可能領域13に相当する対の第1衝突点および第2衝突点の間を進むとして、当該第1衝突点および第2衝突点に対して、第3実施形態または第4実施形態に上述のステップS25〜S28を行う。
【0128】
一方、このような複数本の走行可能領域13が存在する位置において、いずれの走行可能領域13を通過するべきかの指令が移動装置の制御装置に保持されていない場合には、第3実施形態による手法で特定された第1衝突点Pa0と第2衝突点Pb0の間を進むとして、当該第1衝突点Pa0と第2衝突点Pb0に対して、第3実施形態または第4実施形態による上述のステップS25〜S28を行う。
【0129】
なお、第3実施形態による手法で特定されず、第5実施形態による手法で特定された第1衝突点および第2衝突点に対しては、第4実施形態によるステップS25〜S28を行う。
【0130】
図15(C)は、図15(B)の場合に対して、第1衝突点Pa1および第2衝突点Pb1に第4実施形態による上述のステップS25、S26を行うことにより線分21を生成した場合を示す。図15(D)は、図15(C)の場合に対して、第4実施形態による上述のステップS27を行うことにより経由点Piを設定した場合を示す。
【0131】
[第6実施形態]
次に、本発明の第6実施形態による経路生成装置10と経路生成方法について説明する。第6実施形態において、以下で説明しない点は、上述の第1実施形態、第2実施形態、第3実施形態、第4実施形態または第5実施形態と同じである。
【0132】
第6実施形態では、移動装置は、回転駆動される走行車輪により路面上を走行する車両である。例えば、走行車輪は、路面に接触しながら回転駆動されるものであってよい。
【0133】
第6実施形態によると、走行時に車両に生じる振動に基づいて、路面が未舗装路面であるか舗装路面であるかを判定し、未舗装路面であると判定された場合には、路面の高低差に基づく前記路面良好度が高い移動経路候補が選択されやすくなる。
【0134】
第6実施形態では、前記車両には、その走行時に前記車両の振動を計測する振動センサ23(図16を参照)が搭載されている。振動センサ23は、本実施形態では、、鉛直方向における車両の加速度を前記振動として計測するバーチカルジャイロである。
【0135】
第6実施形態による経路生成装置10は、図16に示すように、振動センサ23により計測された振動に基づいて、前記車両が現在走行している路面が、未舗装路面と舗装路面のいずれであるかを判定する路面判定装置25を備える。
【0136】
路面判定装置25は、振動センサ23が計測した振動の大きさが所定のしきい値を越える場合には、路面が未舗装路面であると判定し、そうでない場合には、路面が舗装路面であると判定してよい。
【0137】
この判定方法の具体例を説明する。
【0138】
舗装路面においては、前記振動の大きさは、車両の走行速度に依存せず、通常は、所定値以下になる傾向がある。一方、未舗装路面においては、前記振動の大きさは、車両の走行速度が増加するに伴い、増大する傾向がある。
【0139】
そこで、路面判定装置25は、車両に搭載された速度センサにより計測された車両の走行速度が、速度しきい値を越えており、かつ、振動センサ23により計測された前記振動の大きさが、振動しきい値以下である場合には、路面が舗装路面であると判定し、そうでない場合には、路面が未舗装路面であると判定する。
速度しきい値は、当該値の走行速度で車両が走行すると、舗装路面の場合と未舗装路面の場合とで、車両の振動の大きさに差が現れる車両の走行速度の下限値である。振動しきい値は、走行速度で車両が舗装路面を走行する場合には現れにくい車両の振動の大きさであり、かつ、速度しきい値を超える走行速度で車両が未舗装路面を走行する場合に現れる車両の振動の大きさの下限値である。例えば、走行速度と振動の振幅の平均値(振動の大きさ)とは、図17のグラフに示す傾向があるので、図17のように速度しきい値と振動しきい値を設定してよい。
【0140】
車両が、車両に生じる振動を吸収するサスペンションバネを有する場合には、サスペンションバネにより、車両の加減速によりダンピング振動が車両に発生する。従って、この場合には、このようなダンピング振動の周波数より大きいカットオフ周波数(例えば3Hz)を持つハイパスフィルタが用いられる。すなわち、路面判定装置25は、振動センサ23が計測した振動のうち、前記カットオフ周波数以上の周波数の振動成分のみを抽出し、抽出した振動成分に基づいて上述の判定を行う。
【0141】
路面判定装置25は、速度センサにより計測された走行速度が前記速度しきい値を越えている時に、振動センサ23が、所定時間にわたって計測した振動の振幅に基づいて、上述の判定を行う。当該計測した振動の振幅は、前記所定時間内の各時点において計測されたものである。路面判定装置25は、当該各時点の振動の振幅(例えば、上述のハイパスフィルタにより抽出された振動成分の大きさ)の平均値(例えば2乗平均値)を前記振動の大きさとして算出し、当該平均値が、前記振動しきい値以下である場合には、路面が舗装路面であると判定し、そうでない場合には、路面が未舗装路面であると判定する。
【0142】
前記高速走行可能度は、前記路面良好度だけでなく、前記経路長さと最小曲率半径の一方または両方を含む。好ましくは、前記高速走行可能度は、その要素として、上述の経路長さ、最小曲率、路面良好度、および近接度を有する。
【0143】
経路探索装置9は、高速走行可能度の各要素の値を(例えば、上述のように0から1の範囲の値を持つ)規格値に換算して算出し、各要素の規格値の総和を高速走行可能度として算出する。
【0144】
前記経路探索装置9は、同じ移動経路候補について、前記路面が未舗装路面であると判定された場合には、前記路面が舗装路であると判定された場合よりも、前記経路長さと前記最小曲率半径の規格値を小さく算出する。
【0145】
第6実施形態による経路長さの規格値の算出方法の具体例を説明する。この具体例として算出方法1、2を説明する。
【0146】
(算出方法1)
路面判定装置25により路面が舗装路面であると判定された場合には、経路探索装置9は、経路長さの規格値を、上述の第1実施形態の場合と同じ方法で算出する。一方、路面判定装置25により路面が未舗装路であると判定された場合には、経路探索装置9は、上述の第1実施形態と同じ方法で算出された経路長さの規格値に、ゼロ以上であり1未満である値を乗算することにより得た値を経路長さの規格値として算出する。
【0147】
(算出方法2)
経路探索装置9は、経路長さの規格値を、車両の停止距離の規格値として算出する。車両の停止距離は、次の式で表される。
停止距離=V×t+V2/(2×μ×G)
ここで、Vは、車両の走行速度の上限値(車両が出せる最高速度)である。tは、後述する車両の空走距離である。μは、路面の動摩擦係数の単位を持ち、例えば、路面判定装置25により路面が舗装路面であると判定された場合には0.8であるとし、路面判定装置25により路面が未舗装路面であると判定された場合には、0.5であるとする。Gは、重力加速度である。
空走距離とは、車両の制御装置が、車両の走行時に、車両を停止させる旨の指令信号を出力してから、車両の制動装置(ブレーキ)が作動を開始するまでの時間である。
このように算出される規格値の一例を、図18(A)に示す。
【0148】
第6実施形態による最小曲率半径の規格値の算出方法の具体例を説明する。この具体例として算出方法1、2を説明する。
【0149】
(算出方法1)
路面判定装置25により路面が舗装路面であると判定された場合には、経路探索装置9は、最小曲率半径の規格値を、上述の第1実施形態の場合と同じ方法で算出する。一方、路面判定装置25により路面が未舗装路であると判定された場合には、経路探索装置9は、上述の第1実施形態と同じ方法で算出された最小曲率半径の規格値に、ゼロ以上であり1未満である値を乗算することにより得た値を最小曲率半径の規格値として算出する。
【0150】
(算出方法2)
走行速度Vで車両が走行している時に、車両が横滑りすることなく旋回できるその最小旋回半径は、次の式で表される。
最小旋回半径=V2/(μ×G)
ここで、Vは、車両の走行速度である。μは、路面の動摩擦係数である。Gは、重力加速度である。
μの値は、舗装路面の場合よりも未舗装路面の場合の方が小さい。従って、最小曲率半径の規格値は、図18(B)のように、路面が舗装路面の場合は、上述の規格値(1/n)×logRで表わされるが、路面が未舗装路面の場合は、この規格値に、ゼロより大きく1より小さい値を乗算した規格値として表されてよい。なお、図18(B)では、最小曲率半径が、走行速度がほぼゼロである時に車両が旋回できる最小旋回半径R0以下である場合には、規格値をゼロにしている。
【0151】
第6実施形態によると、未舗装路面であると判定された場合には、経路長さと最小曲率半径の規格値を小さくするので、路面良好度が高速走行可能度に寄与する度合いが大きくなる。従って、未舗装路面であると判定された場合には、路面良好度が高い移動経路候補が選択されやすくなり、未舗装路面においては路面良好度が高い移動経路を走行する制御を行うことができる。
【0152】
[経路生成装置を備える移動装置]
また、自律移動する移動装置は、第1実施形態または第2実施形態の経路生成装置10と、該経路生成装置10により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備える。
制御装置は、位置検出装置5により検出された現在位置、上記生成された移動経路(即ち、記憶装置3に設定されている移動経路)、上述の速度指令装置から出力される指令速度値、移動装置の現在の速度、および、進行方向などに基づいて移動装置の速度制御を行う上述の速度制御装置を有する。また、移動装置は、位置検出装置5により検出された現在位置、上記生成された移動経路、速度指令装置から出力される指令速度値(例えば、一定指定速度値であってもよい)、移動装置の現在の速度および進行方向などに基づいて移動装置の進行方向制御を行う方向制御装置を有する。速度制御装置は、経路探索装置9が上述のように指令速度値を調整する場合には、調整された指令速度値に基づいて移動装置の速度制御を行う。
なお、速度指令装置は、設定されている移動経路における、移動装置が通過する直前の箇所の曲率に基づいて、指令速度値を出力する。この場合、速度指令装置は、曲率が大きいほど小さい速度指令値を出力してよい。
この移動装置は、上述と同様に移動経路をより確実に見出せるので、移動装置の自律移動を継続して行える。
【0153】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。
【0154】
移動装置は、上述した実施例では車両であったが、本発明はこれに限定されない。例えば、移動装置は、自律走行または自律歩行するロボットであってもよい。この場合、上述の実施形態は適宜変更されてよい。
【0155】
前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、路面良好度、および、現在使用している移動経路と移動経路候補との近接度のうち、いずれか1つのみからなっていてもよいし、任意の2つ以上のものからなっていてもよい。
【0156】
また、本発明によると、上述した干渉条件、曲率条件、障害物検出条件、生成経路選択条件のうち1つ以上を省略したものを経路探索条件としてもよい。例えば、干渉条件は、予め障害物が存在しないことが分かっている場合には、省略されてよく、曲率条件は、必要に応じて省略されてよく、障害物検出条件は、高精度な障害物検出が要求されない場合などには、省略されてよく、生成経路選択条件は、必要に応じて省略されてよい。
【0157】
また、本発明によると、経路探索条件を用いずに複数の移動経路候補を生成してよい。すなわち、上述のステップS2では、経路探索条件を考慮することなく、複数の移動経路候補を生成し(例えば、式(Eq1)を用いた上記の方法で生成し)、これら生成した移動経路候補に対し上述のステップS4を行ってもよい。この場合、上述のステップS3は省略される。
【符号の説明】
【0158】
3 記憶装置、5 位置検出装置、7 障害物検出装置
9 経路探索装置、10 経路生成装置、11a,11b 障害物
13 走行可能領域、15 未検出領域、17 道なりライン、19,21 線分、23 振動センサ、25 路面判定装置
【特許請求の範囲】
【請求項1】
自律移動する移動装置の移動経路を生成する経路生成装置であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
前記移動装置の現在位置を検出する位置検出装置と、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する経路探索装置と、を備え、
前記経路探索装置は、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成装置。
【請求項2】
前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、移動経路候補に沿った路面領域の高低差に基づいた路面良好度、および、現在使用している移動経路と移動経路候補との近接度の少なくともいずれかを含み、
前記経路探索装置は、前記各要素の値に応じて前記高速走行可能度を算出する、ことを特徴とする請求項1に記載の経路生成装置。
【請求項3】
前記高速走行可能度は、前記各要素の値が大きいほど高い、ことを特徴とする請求項2に記載の経路生成装置。
【請求項4】
前記経路探索装置は、前記移動装置の現在位置と、前記移動装置に設けた障害物検出装置により検出した障害物とに基づいて、前記現在位置近傍の局所地図を生成し、
前記経路探索装置は、局所地図において、前記現在位置から多数の直線状の探索線を、障害物の有無が検出されていない未検出領域または障害物に至るまで放射状に延ばすことにより、両側が障害物に挟まれた移動装置の走行可能箇所を特定し、該走行可能箇所に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する、ことを特徴とする請求項1、2または3に記載の経路生成装置。
【請求項5】
前記経路探索装置は、局所地図において、
(A)次に通過する経由位置に向かう方向に対する左右両側に障害物が存在する場合に、左右の障害物の間に存在する前記移動装置の走行可能領域を抽出し、
(B)走行可能領域内に位置して走行可能領域の境界に接する複数の内接円を生成し、
(C)各内接円の中心を通過するラインに基づいて道なりラインを生成し、
(D)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく前記未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(E)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(F)前記対象衝突点から延びて、前記道なりラインと直交し、対象衝突点と反対側にある前記障害物まで延びる線分を前記走行可能箇所として生成し、
(G)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する、ことを特徴とする請求項4に記載の経路生成装置。
【請求項6】
前記経路探索装置は、局所地図において、
(A)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(B)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(C)対象衝突点と反対側の前記障害物において、前記対象衝突点からの距離が最も短くなる点を設定し、当該点から前記対象衝突点まで延びる線分を前記走行可能箇所として生成し、
(D)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する、ことを特徴とする請求項4に記載の経路生成装置。
【請求項7】
移動装置は、回転駆動される走行車輪により路面上を走行する車両であり、
前記車両には、その走行時に前記車両の振動を計測する振動センサが搭載されており、
前記振動センサにより計測された振動に基づいて、前記車両が現在走行している路面が、未舗装路面と舗装路面のいずれであるかを判定する路面判定装置を備え、
前記高速走行可能度は、その要素として、前記路面良好度を含むだけでなく、前記経路長さと最小曲率半径の一方または両方を含み、
前記経路探索装置は、前記各要素の値を規格値に換算して算出し、各要素の規格値を反映させた高速走行可能度を算出し、
前記経路探索装置は、同じ移動経路候補について、前記路面が未舗装路面であると判定された場合には、前記路面が舗装路であると判定された場合よりも、前記経路長さと前記最小曲率半径の規格値を小さく算出する、ことを特徴とする請求項1〜6のいずれか一項に記載の経路生成装置。
【請求項8】
請求項1〜7のいずれか一項に記載の経路生成装置と、該経路生成装置により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備えることを特徴とする移動装置。
【請求項9】
自律移動する移動装置の移動経路を生成する経路生成方法であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置とを設け、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する場合に、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成方法。
【請求項1】
自律移動する移動装置の移動経路を生成する経路生成装置であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、
前記移動装置の現在位置を検出する位置検出装置と、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する経路探索装置と、を備え、
前記経路探索装置は、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成装置。
【請求項2】
前記高速走行可能度は、その要素として、移動経路候補の経路長さ、移動経路候補の最小曲率半径、移動経路候補に沿った路面領域の高低差に基づいた路面良好度、および、現在使用している移動経路と移動経路候補との近接度の少なくともいずれかを含み、
前記経路探索装置は、前記各要素の値に応じて前記高速走行可能度を算出する、ことを特徴とする請求項1に記載の経路生成装置。
【請求項3】
前記高速走行可能度は、前記各要素の値が大きいほど高い、ことを特徴とする請求項2に記載の経路生成装置。
【請求項4】
前記経路探索装置は、前記移動装置の現在位置と、前記移動装置に設けた障害物検出装置により検出した障害物とに基づいて、前記現在位置近傍の局所地図を生成し、
前記経路探索装置は、局所地図において、前記現在位置から多数の直線状の探索線を、障害物の有無が検出されていない未検出領域または障害物に至るまで放射状に延ばすことにより、両側が障害物に挟まれた移動装置の走行可能箇所を特定し、該走行可能箇所に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する、ことを特徴とする請求項1、2または3に記載の経路生成装置。
【請求項5】
前記経路探索装置は、局所地図において、
(A)次に通過する経由位置に向かう方向に対する左右両側に障害物が存在する場合に、左右の障害物の間に存在する前記移動装置の走行可能領域を抽出し、
(B)走行可能領域内に位置して走行可能領域の境界に接する複数の内接円を生成し、
(C)各内接円の中心を通過するラインに基づいて道なりラインを生成し、
(D)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく前記未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(E)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(F)前記対象衝突点から延びて、前記道なりラインと直交し、対象衝突点と反対側にある前記障害物まで延びる線分を前記走行可能箇所として生成し、
(G)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する、ことを特徴とする請求項4に記載の経路生成装置。
【請求項6】
前記経路探索装置は、局所地図において、
(A)前記現在位置から多数の直線状の前記探索線を放射状に延ばし、互いに連続して隣接し障害物に衝突することなく未検出領域に至る複数の探索線のうち一方端の探索線と隣接する探索線が、左側の障害物に衝突する衝突点を第1衝突点として特定し、前記複数の探索線のうち他方端の探索線と隣接する探索線が、右側の障害物と衝突する衝突点を第2衝突点として特定し、
(B)第1衝突点と第2衝突点を比較し、両衝突点のうち前記現在位置に近い方を、対象衝突点とし、
(C)対象衝突点と反対側の前記障害物において、前記対象衝突点からの距離が最も短くなる点を設定し、当該点から前記対象衝突点まで延びる線分を前記走行可能箇所として生成し、
(D)前記線分に至る前記複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出する、ことを特徴とする請求項4に記載の経路生成装置。
【請求項7】
移動装置は、回転駆動される走行車輪により路面上を走行する車両であり、
前記車両には、その走行時に前記車両の振動を計測する振動センサが搭載されており、
前記振動センサにより計測された振動に基づいて、前記車両が現在走行している路面が、未舗装路面と舗装路面のいずれであるかを判定する路面判定装置を備え、
前記高速走行可能度は、その要素として、前記路面良好度を含むだけでなく、前記経路長さと最小曲率半径の一方または両方を含み、
前記経路探索装置は、前記各要素の値を規格値に換算して算出し、各要素の規格値を反映させた高速走行可能度を算出し、
前記経路探索装置は、同じ移動経路候補について、前記路面が未舗装路面であると判定された場合には、前記路面が舗装路であると判定された場合よりも、前記経路長さと前記最小曲率半径の規格値を小さく算出する、ことを特徴とする請求項1〜6のいずれか一項に記載の経路生成装置。
【請求項8】
請求項1〜7のいずれか一項に記載の経路生成装置と、該経路生成装置により生成された移動経路に沿って移動するように移動装置を制御する制御装置と、を備えることを特徴とする移動装置。
【請求項9】
自律移動する移動装置の移動経路を生成する経路生成方法であって、
移動開始位置から目標到達位置までの間に設定され前記移動経路の生成の指標となる複数の経由位置を記憶した記憶装置と、前記移動装置の現在位置を検出する位置検出装置とを設け、
移動装置の移動中において、前記現在位置に基づいて、前記複数の経由位置のうち移動装置が次に通過する経由位置に向かうための移動経路を探索し生成する場合に、
複数の移動経路候補を生成するとともに、これら移動経路候補の各々について高速走行可能度を算出し、
これら高速走行可能度を互いに比較することで、前記複数の移動経路候補の中から、最も高速走行可能度が高い移動経路候補を選択し、
該選択した移動経路候補を実際に使用する移動経路として設定する、ことを特徴とする経路生成方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【公開番号】特開2011−170843(P2011−170843A)
【公開日】平成23年9月1日(2011.9.1)
【国際特許分類】
【出願番号】特願2011−9350(P2011−9350)
【出願日】平成23年1月20日(2011.1.20)
【出願人】(500302552)株式会社IHIエアロスペース (298)
【出願人】(000000099)株式会社IHI (5,014)
【Fターム(参考)】
【公開日】平成23年9月1日(2011.9.1)
【国際特許分類】
【出願日】平成23年1月20日(2011.1.20)
【出願人】(500302552)株式会社IHIエアロスペース (298)
【出願人】(000000099)株式会社IHI (5,014)
【Fターム(参考)】
[ Back to top ]