経路探索機能付き移動体
【課題】地図データ上で自己位置と最終目標点が特定されたときに、自己位置から移動可能範囲内を伸びて最終目標点に至る大域経路を計算する技術が知られている。障害物を検出したときに、障害物を回避しながら最終目標点に至る経路を計算する技術も知られている。しかしながら両者を融合する技術が未解決であり、障害物を回避してから大域経路に復帰する経路がうまく計算できない。
【解決手段】障害物回避経路計算手段で用いる最終目標点に代えて中間目標点を用いる。障害物よりも遠方にある大域経路上の点を中間目標点とすると、障害物を回避してから大域経路に復帰する経路を計算できる。
【解決手段】障害物回避経路計算手段で用いる最終目標点に代えて中間目標点を用いる。障害物よりも遠方にある大域経路上の点を中間目標点とすると、障害物を回避してから大域経路に復帰する経路を計算できる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、経路を探索しながら移動開始時の自己位置から最終目標点に移動する移動体に関する。
【背景技術】
【０００２】
地図データ上で自己位置と最終目標点が特定されたときに、自己位置から移動可能範囲内を伸びて最終目標点に至る経路を計算し、計算された経路に沿って移動する技術が開発されている。その種の技術が特許文献１に開示されている。上記技術によって、地図データから判明する移動可能な範囲を辿りながら自己位置から最終目標点まで移動することができる。ただし上記技術は、未知の障害物が出現する事態を想定していない。
障害物を回避する経路を計算する技術も開発されており、非特許文献１に開示されている。この技術では、障害物を回避できる経路の候補を複数生成し、生成された経路の候補毎に、最終目標点となす角度と移動可能速度と障害物までの距離に基づいて評価値を計算し、最大評価値をもたらす候補を採択する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特願２０１０−１８７０４４号の明細書と図面
【非特許文献】
【０００４】
【非特許文献１】歩行者の行動予測に基づく移動ロボットのオンライン回避行動計画、久保田文子、田中稔、津坂祐司、第15回ロボティクス・シンポジア予稿集、2010、1-6頁
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１に開示されている技術によって、地図データと自己位置と最終目標点から、自己位置から移動可能範囲内を伸びて最終目標点に至る経路を計算することができ、計算した経路に沿って移動することができる。ただし計算した経路に沿って移動している間に地図データに記憶されていない障害物が出現すると、その障害物を回避できないことになる。
非特許文献１に開示されている技術によれば、障害物を回避する経路が計算できる。
そこで、特許文献１の技術と非特許文献１の技術を併用すれば、移動開始時の自己位置から移動可能範囲内を伸びて最終目標点に至る経路に沿って移動している間に障害物が出現したときにはその障害物を回避することができるように思われる。
【０００６】
しかしながら、実際には特許文献１の技術と非特許文献１の技術を組み合わせて用いることが難しい。非特許文献１の技術では、障害物を回避する経路に沿った方向と最終目標点を向く方向とがなす角度に基づいて評価値を計算する。すなわち、回避経路の計算時に最終目標点の位置を利用することによって、障害物を回避しながら最終目標点に至る経路を計算する。特許文献１の技術と非特許文献１の技術を併用すると、障害物を回避した後に、移動可能範囲内を伸びている経路情報を無視し、最終目標点に直行する経路が計算されてしまい、移動可能範囲から逸脱してしまうことがある。すなわち移動可能範囲の境界線となっている壁等に接触してしまうことがある。あるいは壁等との接触を避けるために移動体が停止してしまうことがある。
【０００７】
非特許文献１の技術で用いる最終目標点に代えて、移動可能範囲内を伸びている経路上にある中間点を一時的目標点とすれば、障害物を回避することと、移動可能範囲内を伸びている経路に沿って移動することの両者を実現できるように想像できる。
【０００８】
しかしながら実際には難しい。実際に試行してみると、障害物を回避する要素と経路に復帰する要素がともに作用してしまい、障害物を回避する移動方向と、経路に復帰しようとして障害物に接近する移動方向が交互に計算されるといったことが生じる。特許文献１の技術と非特許文献１の技術を併用するためには、両者を融合する技術が必要とされる。
【課題を解決するための手段】
【０００９】
本発明では、地図データを用いて自己位置から移動可能範囲内を伸びて最終目標点に至る経路を計算して移動する技術と、障害物が出現したときには障害物を回避する経路を計算して移動する技術を融合し、障害物が出現するまでは経路に沿って移動し、障害物が出現すれば障害物を回避する経路に沿って移動し、障害物を回避した後には経路に復帰する経路に沿って移動する移動体を提供する。なお、本明細書では、地図データから計算した経路であって、移動可能範囲内を自己位置から最終目標点まで伸びている経路を大域経路といい、障害物を回避する経路や、回避後に大域経路に復帰する経路と区別する。
【００１０】
本発明は、地図データに含まれていない障害物を検出したときに、その障害物を回避しながら最終目標点に至る経路に沿って移動する移動体に関する。その移動体は、地図データベースと、大域経路計算手段と、障害物検出装置と、局地経路計算手段と、制御装置を備えている。地図データベースは、移動可能範囲を画定するデータを記憶している。大域経路計算手段は、地図データ上で特定された自己位置から伸びて最終目標点に至る経路であって、移動可能範囲内を伸びる大域経路を計算する。障害物検出装置は、少なくとも移動体の移動方向の前方に障害物が出現したときに、出現した障害物の位置を特定する。局地経路計算手段は、障害物を回避するともに中間目標点に復帰する経路を計算する。制御装置は、移動体が備えている移動装置を制御して局地経路計算手段が計算した経路に沿って移動体を実際に移動させる。
本発明では、局地経路計算手段で用いる中間目標点を、下記条件、すなわち、
１）大域経路計算手段が計算した大域経路上にあり、
２）大域経路に沿って測定した中間目標点までの距離＞大域経路に沿って測定した障害物までの距離を満たす位置に設定する。
【００１１】
大域経路計算手段には既知の技術が活用できる。例えば特許文献１に記載の技術を活用できる。自動車に搭載されているナビゲーション装置で用いる経路探索技術等を活用してもよい。局地経路計算手段にも既知の技術が活用できる。例えば非特許文献１に記載の技術を活用できる。ただし、本発明では、最終目標点に代えて中間目標点を用いて局地経路を計算する。ここで用いる中間目標点は、大域経路上の点とする。この結果、大域経路に復帰する局地経路が計算される。また、大域経路に沿って計測した中間目標点までの距離＞大域経路に沿って計測した障害物までの距離の条件を満たす中間目標点を採用する。上記の距離関係にあると、移動体が障害物を回避して障害物の側方を通過するまでの間は障害物を回避することが高く評価されて局地経路が計算され、側方を通過した後は中間目標点を指向することが高く評価されて局地経路が計算される。障害物を回避する必要がある期間は回避経路が計算され、障害物を回避する必要が薄れた後は大域経路へ復帰する経路が計算される。大域経路計算技術と、障害物を回避する局地経路を計算する技術がうまく融合する。
【００１２】
局地経路計算手段で用いる中間目標点設定する際に、さらに下記条件、すなわち、
３）大域経路に沿って測定した中間目標点までの距離が、大域経路に沿って測定した移動体と障害物の相対速度に比例する距離を含む関係とすることが好ましい。
この場合、移動体と障害物が高速度で接近する場合には、離れた点を中間目標点にして局地経路が計算され、移動体と障害物が低速度で接近する場合には、近接した点を中間目標点にして局地経路が計算される。この結果、自然な経路、すなわち歩行者等に不安を感じさせることなく回避するとともに回避後に無理なく大域経路に復帰する局地経路が計算される。
大域経路に沿った中間目標点までの距離を６メートル以上とすることが好ましい。その場合、移動体が歩行者を避けて移動する際に、歩行者のパーソナルスペースに入り込まない局地経路を計算することができる。
【発明の効果】
【００１３】
本発明によると、移動可能範囲内を伸びて自己位置から最終目標点に至る大域経路を計算する技術と、障害物を回避する局地経路を計算する技術がうまく融合し、大域経路に沿って移動できる間は大域経路を辿り、障害物が出現して障害物を回避する必要が生じれば障害物を回避する局地経路に移行し、障害物を回避した後には大域経路に復帰する経路が計算される。
【図面の簡単な説明】
【００１４】
【図１】移動体が備えている各種装置と、各種装置の機能を説明する図。
【図２】移動体が備えている各種装置の詳細を示す図。
【図３】大域経路を計算する処理手順図。
【図４】移動時に繰り返し実行する処理手順図。
【図５】図４の処理によって得られる経路を説明する図。
【発明を実施するための最良の形態】
【００１５】
以下に説明する実施例の主要な特徴を列記しておく。
（特徴１）移動体は自走式である。
（特徴２）移動体には人を検出する外界センサが搭載されている。
（特徴３）移動体には搭乗者が最終目標点を入力する入力装置が搭載されている。
（特徴４）移動体はコンピュータを搭載しており、経路を計算して移動する。
（特徴５）移動体は、人の周りにあるスペースであって、他者が侵入するとその人が不安を覚えるパーソナルスペースを回避する経路を計算する。
【実施例】
【００１６】
図１に本発明の移動体を具現化した自走式の移動体２が図示されている。移動体２は、移動装置１４と制御装置１２を備えている。移動装置１４は、左輪と左輪用モータと右輪と右輪用モータを備えている。制御装置１２は、制御装置１２に指示される移動体２の移動速度と移動体２の角速度の値から、左輪用モータの回転速度と右輪用モータの回転速度を計算し、計算した回転速度に調整する。制御装置１２は、左輪用モータと右輪用モータの回転速度を独立に制御する。移動体２は、制御装置１２に指示された移動速度で移動し、制御装置１２に指示された角速度で移動方向を変化させる。移動体２は、制御装置１２に指示された経路に沿って移動する。なお、本実施例では、左右輪の回転速度を独立に制御することで移動方向を変化させるが、操舵装置を備えている場合には操舵輪の方向を調整することによって移動方向を変化させてもよい。移動体２の機械的構成は、非特許文献１に開示されている。
【００１７】
移動体２は、壁等の境界線２０で輪郭が定められている移動可能範囲２８内を移動して最終目標点３８に到達する大域経路１８を計算して移動する。移動可能範囲２８内には、例えば駐車中の自動車２４といった静止障害物もあれば、歩行者３２、３６等の移動障害物もあり、移動体２は、それらを避ける局地経路２６，３０、３４を計算して移動する。図１から明らかに、移動体２は、障害物を避け終わると、最終目標点３８に到達する大域経路１８に復帰する。
【００１８】
移動体２は、境界線２０の幾何的パターンを記憶している地図データベース４を備えている。移動体２は、現在位置検出装置（図２の自己位置推定部）を備えており、地図上のどこにいるかを決定することができる。移動体２には、最終目標点を教示することが可能であり、教示された最終目標点が地図上のどこにあるのかを決定することができる。例えば移動体２の現在位置が１６であり、最終目標点が３８であるといったことを特定することができる。自己位置と最終目標点が特定されると、移動体２は、境界線２０に衝突しない条件で（換言すれば移動可能範囲２８内を伸びるという条件で）、自己位置１６から最終目標点３８まで伸びる経路１８を計算する計算手段を備えている。そうして計算される経路は、検出された障害物を避けるために計算される局地経路よりも大域的な経路である。移動体２は、地図データベース４と自己位置と最終目標点から、大域経路１８を計算する大域経路計算手段６を備えている。
大域経路計算手段６は、移動開始時の自己位置と最終目標点から大域経路１８を計算してしまい、その後は再計算しないものであってもよい。あるいは、移動に伴って自己位置が更新されるのに対応して、大域経路１８を再計算するものであってもよい。図３は、前者の方式の大域経路計算手段６によって実行される処理を示している。
【００１９】
移動体２は、障害物検出装置８を備えている。障害物検出装置８は、レーザを発光して反射光を受光するものであり、反射体までの距離と方位を特定することができる。レーザに代えて電波を利用することもできる。参照番号２２は、障害部検出装置８によって障害物の存在を検出可能な範囲を示している。なお、壁等の影の部分までは検出するができない。図１の場合、駐車中の自動車２４が検出され、歩行者３２、３６等は検出されていない様子を示している。
【００２０】
移動体２は、局地経路計算手段１０を備えている。局地経路計算手段１０は、障害物が検出されたときに障害物を回避する局地経路を計算する。図１の場合、障害物２４が検出されたので局地経路２６が計算され、障害物３２が検出されたら局地経路３０が計算され、障害物３６が検出されたら局地経路３４が計算されることを例示している。局地経路２６，３０，３４に図示されているように、本実施例で計算される局地経路２６，３０，３４は、移動体２が障害物を回避した直後に大域経路１８に復帰することを特徴とする。本実施例では、大域経路１８に沿って移動することで最終目標点３８に移動することと、障害物を避ける局地経路２６，３０，３４を採用することで障害物を避けて移動することを両立させている。
【００２１】
図２は、移動体２のシステム構成を示しており、操作者が移動体２に最終目標点３８を教示するために使用する最終目標点指定部４０と、自己位置推定部４２と、地図データベース４を備えている。最終目標点指定部４０は、地図を表示し、表示された地図上において最終目標点の位置を指定するものであってよい。自己位置推定部４２は、ＧＰＳ等であってもよいし、車輪の回転数から自己位置を推定するものであってもよいし、障害物検出装置８で検出される壁等の境界線２０の形状から、地図上における自己位置を推定するものであってもよい。デッドレコニング、スキャンマッチングなどの手法が利用できる。大域経路計算手段６は、自己位置から最終目標点に至る経路であって境界線と干渉しない経路を計算する。図１の場合、大域経路計算手段６が、自己位置１６から最終目標点３８に至る経路であって境界線２０と干渉しない大域経路１８を計算したことを例示している。
【００２２】
大域経路計算手段６は、既知の手法で大域経路を計算する。例えば、ダイクストラ探索技術、Ａ＊探索技術等を利用することができる。自己位置１６から最終目標点３８に至るルートが複数存在する場合には、市販のナビゲーション装置が採用している検索条件を流用することができる。例えば、距離優先を指定したり、道幅が狭いルートを避けるといったように検索条件を設定することができる。大域経路１８は、ルートよりも詳細レベルであり、境界線２０と干渉を避けるために、道幅のどこをたどっていくのかまで指定する。
【００２３】
移動体２は、外界センサ８ａを備えている。外界センサ８ａはレーザ光の発光・受光素子を備えており、図１の検出範囲２２内にある障害物を検出するように、レーザ光の発光方向を水平面内で走査する。外界センサ８ａは、図１に示す検出範囲２２内に、障害物が存在する場合に、その障害物からの反射光を受光する。レーザ光の発光タイミングと受光タイミングから障害物までの距離が計算でき、レーザ光の発光方向から障害物の存在する方位が計算できる。
外界センサ８ａが検出する物体のなかには、壁等の固定物であって地図データベース４に含まれている物体と、駐車車両や歩行者といった一時的な存在物であって地図データベース４に含まれていない物体が含まれる。本明細書では、後者を障害物という。障害物の中には、駐車車両のように静止している障害物と、歩行者のように移動している障害物が存在する。
【００２４】
障害物抽出部８ｂは、外界センサ８ａが検出する物体のなかから地図データベース４に含まれている物体を除外することで障害物を抽出する。
障害物軌道計算部８ｃは、外界センサ８ａで検出された反射光の空間的分布パターンから、障害物毎に特徴的なパターン（フューチャー）を抽出する。外界センサ８ａによる検出と、障害物抽出部８ｂによる抽出処理は、所定時間間隔で繰り返し実施される。所定時間毎に得られるフューチャーの位置から、障害物毎の位置と移動速度と移動方向が計算される。障害物軌道計算部８ｃは、障害物毎に移動軌道を計算する。駐車車両２４であれば静止していることがわかり、歩行者３２，３６であれば、どの位置をどの方向にどの速度で移動しているかがわかる。障害物を抽出して障害物毎の位置と移動速度と移動方向を計算する技術の詳細は、非特許文献１に開示されている。
本実施例では、外界センサ８ａにレーザ光の発光・受光素子を利用しているが、電波を利用するレーダでもよいし、可視光を受光するステレオカメラでもよい。また移動体２に搭載されているとは限らず、移動可能範囲２８を監視している定置式カメラであってもよい。
【００２５】
大域経路計算手段６が計算した大域経路は、中間目標点設定部１０ａに入力される。中間目標点設定部１０ａは、下記のロジックで、大域経路上に中間目標点を次々に設定していく。
１)大域経路がまっすぐに伸びている場合には、中間目標点と隣接する中間目標点の距離を所定距離とする。本実施例では１０ｃｍ毎に中間目標点を設定する。
２）大域経路が曲がっている場合には、中間目標点と隣接する中間目標点の距離を前記の所定距離よりも短くする。このとき、経路が急カーブであるほど、距離を短くする。
中間目標点を上記のように設定し、所定時間毎に次の中間目標点に移動するようにすれば、移動体２は、大域経路１８上を、直進路では高速に移動し、カーブでは減速しながら移動することができる。
【００２６】
設定された中間目標点の情報と、計算された障害物の軌道は、局地経路計算部１０ｂに入力される。局地経路計算部１０ｂでは、非特許文献１に記載の技術を用いて障害物を避ける局地経路を計算する。この際に、局地経路計算部１０ｂは、障害物を避けながら最終目標点３８に進む経路を求めるのではなく、障害物を避けたら大域経路１８に復帰する局地経路を計算する。そのために、局地経路計算部１０ｂは、最終目標点３８の位置に代えて復帰用の中間目標点の位置を計算に用いる。
【００２７】
図５は、移動体２が時刻ｔ０において、位置Ｐ０上にあり（位置Ｐ０は大域経路１８上にある）、障害物５０が位置Ｐ２にあって、ベクトルＶ２に沿って移動していることが判明した状態を例示している。障害物５０は、時刻の経過ともに、ｔ１、ｔ２、ｔ３、ｔ４に示す位置に移動すると推定される。図５は、移動体２が大域経路１８に沿って移動し続けると、障害物５０と干渉することが予想され、障害物５０を回避する経路に切り換える必要が生じた場合を例示している。
【００２８】
局地経路計算部１０ｂは、移動体２と障害物５０が干渉することになるか否かを計算し、干渉することになることが判明したら、移動体２と障害物５０の相対速度を計算する。
ここでいう相対速度は、障害物５０の速度ベクトルＶ２の内の大域経路１８に沿った速度成分Ｖ２ｃと、移動体２の大域経路１８に沿った速度成分Ｖ１ｃから計算されるものであり、速度成分Ｖ２ｃが移動体２に接近する場合には、両者を加算した値であり、速度成分Ｖ２ｃが速度成分Ｖ１ｃと同一方向の場合には、Ｖ１ｃからＶ２ｃを減算した値である。
【００２９】
局地経路計算部１０ｂは、相対速度Ｖから、中間目標点までの距離Ｌ２を下記の式で計算する。
Ｌ２＝α＋β×相対速度Ｖ・・・・（１）
上記のαとβは、干渉する可能性のある障害物５０までの距離Ｌ１に対してＬ２＞Ｌ１の関係にある距離Ｌ２が計算される値である。αとβは定数としてもよい。またαについては、移動体の速度によって増減する値としてもよい。あるいは、経路の局率半径が大きい（直進に近い経路を高速に移動している）ほど大きなαを採用し、経路の局率半径が小さい（急カーブを低速で移動している）ほど小さなαを採用してもよい。距離Ｌ２が計算されたら、計算された距離Ｌ２に最も近い中間目標点を抽出し、抽出された中間目標点を復帰用の中間目標点とする。局地経路計算部１０ｂは、復帰用の中間目標点において大域経路に復帰する局地経路を計算する。
相対速度Ｖは、移動体２と障害物５０が接近する場合に正の値とする。最終目標点に近づいた結果、Ｌ２＞最終目標点までの距離となったら、最終目標点を局地計算に用いる復帰用の中間目標点とする。移動体２と障害物５０が離反するために負の値をとる場合には、移動体２と障害物５０が干渉する可能性がない。
【００３０】
移動体２と障害物５０が干渉しない場合には、局地経路計算部１０ｂが、中間目標点設定部１０ａで設定された中間目標点を所定時間毎に辿る局地経路を計算する。移動体２と障害物５０が干渉する場合には、局地経路計算部１０ｂが、非特許文献１に記載されているDynamic Window Approachの技術を活用して障害物を回避する経路を計算する。その際に、最終目標点に代えて、上記の距離Ｌ２の近傍にある復帰用中間目標点を用いる。その結果、障害物の側方を通過するまでは障害物を回避することを重視して経路が計算され、障害物の側方を通過すると大域経路に復帰することを重視して経路が計算される。復帰用に選択した中間目標点において大域経路に復帰する局地経路が計算される。
【００３１】
非特許文献１の技術では、進行方向の候補と進行スピードの候補を複数作成し、候補毎に好ましさの指標を求め、最も高い指標をもたらす進行方向の候補と進行スピードの候補を採用するロジックを用いる。例えば移動体２がＰ０の示す位置にある場合、Ａ〜Ｇに例示する進行方向（ＡからＧは所定角度ずつ相違している）の候補と、複数段に設定されている進行スピードのレベルで指定される複数の候補（例えば低速・中速・高速の３候補）の組み合わせで得られる進行ベクトルの候補を設定する。次に、各進行ベクトルの候補毎に、障害物までの距離と、中間目標点を指向するベクトルとなす角度と、現在の進行方向からの変化角度等を計算し、障害物までの距離については長いほど高得点を与え、中間目標点を指向するベクトルとなす角度についてはその角度が小さいほど高得点を与え、変化角度についてはその変化角度が小さいほど高得点を与える。最も高得点を与える候補を採用する。障害物までの距離が長く、できるだけ復帰用の中間目標点を指向しており、進行方向の変化が少ない経路が採用される。図５の場合、時刻ｔ０ではベクトルＶ１１が最も高得点となる場合を例示している。移動体２は、上記の計算を所定時間毎に繰り返しながら進行する。図５では、時刻ｔ１ではベクトルＶ１２が最も高得点となる場合を例示し、時刻ｔ２ではベクトルＶ１３が最も高得点となる場合を例示している。
【００３２】
本実施例の場合、移動体２の側で障害物５０の軌道を計算しており、障害物５０の将来位置が予測可能となっている。そこで、時刻ｔ０において、時刻ｔ１、ｔ２等における位置関係を予想することができ、時刻ｔ０において、時刻ｔ１、ｔ２等のおける進行ベクトルを決定することも可能である。例えば、時刻ｔ０で局地経路を計算する際に、障害物５０が時刻ｔ１において存在するであろう位置、障害物５０が時刻ｔ２において存在するであろう位置等を推定することができる。そこで、時刻ｔ０で局地経路を計算する際に、移動体２は、時刻ｔ０において可能な進行方向の候補と進行速度の候補、時刻ｔ１において可能な進行方向の候補と進行速度の候補、時刻ｔ２において可能な進行方向の候補と進行速度の候補等を計算し、時刻ｔ０における候補とその得点、時刻ｔ１における候補とその得点、時刻ｔ２における候補とその得点等を計算し、それらの得点を合計した合計得点が最も高くなる経路を計算する。図の場合、時刻ｔ０では進行ベクトルＶ１１を採用し、時刻ｔ１では進行ベクトルＶ１２を採用し、時刻ｔ３では進行ベクトルＶ１３を採用し、時刻ｔ４では進行ベクトルＶ１４を採用し、時刻ｔ５では進行ベクトルＶ１５を採用する経路によるときに、合計得点が最も高くなる場合を例示している。この場合、局地経路計算手段１０は、進行ベクトルＶ１１、Ｖ１２、Ｖ１３、Ｖ１４、Ｖ１５を接続した局地経路を計算する。本実施例では、時刻ｔ０において、その時点で設定した復帰用中間目標点Ｍ５にまで移動体２が移動するのに要する時間にわたって合計した得点が最も高くなる局地経路を計算する。復帰用中間目標点Ｍ５までのあいだの障害物の移動軌跡に基づいて局地経路を計算する。
【００３３】
上記の計算を所定時間毎に繰り返してもよい。例えば、時刻ｔ０において、図５のＶ１１、Ｖ１２、Ｖ１３、Ｖ１４、Ｖ１５を計算し、時刻ｔ１において同種の計算を再び実行する。この結果、図５に示す速度ベクトルＶ１２、Ｖ１３、Ｖ１４、Ｖ１５が再計算され、Ｖ１６が新に計算される。
後者の方法によると、障害物が移動速度または移動方向を変化させることにも追従することができる。
【００３４】
図４は、移動体２と障害物５０が干渉する場合に、局地経路計算部１０ｂによって実行される処理手順を示している。移動体２の移動中は、図４の処理が所定時間間隔で繰り返し実行される。
ステップＳ４０では、移動体２の自己位置が検出される。ステップＳ４２では、所定時間前に検出された自己位置からの変化量から、自己速度（正確には方向も）が計算される。ステップＳ４４では、障害物の位置が検出される。ステップＳ４４で検出される障害物位置は、移動体２に対する相対的位置であるが、ステップＳ４０で移動体１の自己位置が検出されていることから、障害物の地図上の位置が判る。ステップＳ４６では、所定時間前に検出された障害物位置からの変化量から、障害物の移動速度（正確には移動方向も）が計算される。
【００３５】
ステップＳ４８では、図５を参照して説明した大域経路１８に沿った方向での相対速度を計算する。ステップＳ５０では、式１を用いて、局地経路計算手段１０で用いる中間目標点までの距離を計算する。ステップＳ５２では、最終目標点までの距離と、ステップＳ５０で計算した距離との大小を比較する。最終目標点に近づくと、Ｌ２＞最終目標点までの距離となる。その条件が成立したら（Ｓ５２でＹＥＳとなったら）、最終目標点を局地計算に用いる復帰用の中間目標点とする（Ｓ５４）。Ｌ２＜最終目標点までの距離の間は（Ｓ５２でＮＯとなる間は）、距離Ｌ２にある中間目標点を局地計算に用いる復帰用の中間目標点とする（Ｓ５６）。
ステップＳ５８では、非特許文献１の技術で障害物を回避する局地経路を計算する。ステップＳ５４またはＳ５６で設定した復帰用中間目標点を用い、移動体が復帰用中間目標点に到達するまでの間の障害物の移動軌跡を用いて、局地経路を計算する。局地経路が計算されれば、その経路となる左モータの回転速度と右モータの回転速度を計算してその回転速度に調整する。
【００３６】
局地経路計算手段１０が最終目標点に代えて復帰用中間目標点を利用する場合、復帰用中間目標点において大域経路１８に復帰する局地経路が高く評価される。
障害物を回避する必要がある間は障害物を回避する局地経路が計算され、障害物を回避し終えたら大域経路に復帰する局地経路が計算されるようにするためには、局地経路計算手段１０で用いる復帰用中間目標点の選定が重要である。復帰用中間目標点が近すぎる場合、すなわち移動体と障害物の間に復帰用中間目標点を設定した場合、障害物を回避する必要がある間に、障害物を回避する局地経路を計算する要素と、大域経路に復帰する局地経路を計算する要素が同時に作用する。後者は、障害物に向かう局地経路を計算することになる。移動体と障害物の間に中間目標点を設定した場合、障害物を回避する必要がある間に、障害物を回避する局地経路を計算する要素と、障害物に向かう局地経路を計算する要素が同時に作用する。障害物を回避する経路が高く評価されたり、障害物に向かう経路が高く評価されたりしながら局地経路が計算されるために、実際に得られる局地経路は複雑でギクシャクしたものとなる。
【００３７】
本実施例では、障害物までの距離Ｌ１よりも大きな距離Ｌ２が計算される値に設定されているαとβを用い、Ｌ２＝α＋β×相対速度Ｖの式で計算される距離にある中間目標点を用いて局地経路を計算するために、障害物を回避する必要がある間は障害物を回避する経路が高く評価されて局地経路が計算され、障害物を回避し終えたら大域経路に復帰する経路が高く評価されて局地経路が計算される。
【００３８】
上記の結果、移動体は、
１）大域経路に沿って最終目標点に向かって移動する。
２）移動中に障害物を検出すると、障害物との干渉を避ける経路を高く評価しながら局地経路を探索する。この際にも大域経路上に置かれている復帰用中間目標点に復帰する要素が加味されるが、復帰用中間目標点は障害物の背後に設定されることから、障害物の側方を通過するまでの間は、中間目標点に復帰する要素が、移動体を障害物の前方に侵入させる局地経路を探索することがない。
３）障害物の側方を通過すると、その後は速やかに復帰用中間目標点に復帰する経路が探索される。障害物を避けているうちに目標点を見失うことがない。
４）大域経路に復帰すると、その後は大域経路に沿って最終目標点に向けて移動する。
以上の結果、移動体は、大域経路に沿って最終目標点に移動することと、障害物を回避することをうまく両立させることができる。
【００３９】
局地経路に沿って移動する間に局地経路を計算し直す技術による場合、図１の検出範囲２２が示すように、移動体２の背面では障害物を検出しないために、移動体２が障害物の側方にまですすんだ段階で、障害物が検出されなくなる。この場合、Ｌ２＝α＋β×相対速度Ｖの式における相対速度Ｖがゼロとなる。障害物が検出されなくなくなった時に、距離Ｌ２の値が不連続に変化する。これを避けるためには、移動体２の後方まで検出するセンサを用いることが有効である。あるいは、障害物が検出されなくなった時点で相対速度Ｖをゼロにするのに代えて、一定の速度でゼロに変化する人為的相対速度を用いてもよい。
【００４０】
障害物が歩行者であることがある。歩行者にはそれ以上に他者に接近されると不安ないし不快と感じるパーソナルスペースが存在する。図5の５２は、歩行者５０のパーソナルスペースを示す。本実施例の場合、大域経路に沿って６メートル以上はなれた位置にある中間目標点を復帰用の中間目標点に選定すると、移動体２が歩行者５０に干渉しないだけでなく、歩行者５０のパーソナルスペース５２に入り込まない局地経路を計算することができる。
【００４１】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず特許請求の範囲を限定するものではない。また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書又は図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【００４２】
２：移動体
４：地図データベース
６：大域経路計算手段
８：障害物検出装置
１０:局地経路計算手段
１２：制御装置
１４：移動装置
１６：現在位置
１８：大域経路
２０：境界線
２２：検出範囲
２４：駐車車両（静止障害物）
２６，３０，３４：局地経路
２８：移動可能範囲
３２，３６：歩行者（移動障害物）
３８：最終目標点
【技術分野】
【０００１】
本発明は、経路を探索しながら移動開始時の自己位置から最終目標点に移動する移動体に関する。
【背景技術】
【０００２】
地図データ上で自己位置と最終目標点が特定されたときに、自己位置から移動可能範囲内を伸びて最終目標点に至る経路を計算し、計算された経路に沿って移動する技術が開発されている。その種の技術が特許文献１に開示されている。上記技術によって、地図データから判明する移動可能な範囲を辿りながら自己位置から最終目標点まで移動することができる。ただし上記技術は、未知の障害物が出現する事態を想定していない。
障害物を回避する経路を計算する技術も開発されており、非特許文献１に開示されている。この技術では、障害物を回避できる経路の候補を複数生成し、生成された経路の候補毎に、最終目標点となす角度と移動可能速度と障害物までの距離に基づいて評価値を計算し、最大評価値をもたらす候補を採択する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特願２０１０−１８７０４４号の明細書と図面
【非特許文献】
【０００４】
【非特許文献１】歩行者の行動予測に基づく移動ロボットのオンライン回避行動計画、久保田文子、田中稔、津坂祐司、第15回ロボティクス・シンポジア予稿集、2010、1-6頁
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１に開示されている技術によって、地図データと自己位置と最終目標点から、自己位置から移動可能範囲内を伸びて最終目標点に至る経路を計算することができ、計算した経路に沿って移動することができる。ただし計算した経路に沿って移動している間に地図データに記憶されていない障害物が出現すると、その障害物を回避できないことになる。
非特許文献１に開示されている技術によれば、障害物を回避する経路が計算できる。
そこで、特許文献１の技術と非特許文献１の技術を併用すれば、移動開始時の自己位置から移動可能範囲内を伸びて最終目標点に至る経路に沿って移動している間に障害物が出現したときにはその障害物を回避することができるように思われる。
【０００６】
しかしながら、実際には特許文献１の技術と非特許文献１の技術を組み合わせて用いることが難しい。非特許文献１の技術では、障害物を回避する経路に沿った方向と最終目標点を向く方向とがなす角度に基づいて評価値を計算する。すなわち、回避経路の計算時に最終目標点の位置を利用することによって、障害物を回避しながら最終目標点に至る経路を計算する。特許文献１の技術と非特許文献１の技術を併用すると、障害物を回避した後に、移動可能範囲内を伸びている経路情報を無視し、最終目標点に直行する経路が計算されてしまい、移動可能範囲から逸脱してしまうことがある。すなわち移動可能範囲の境界線となっている壁等に接触してしまうことがある。あるいは壁等との接触を避けるために移動体が停止してしまうことがある。
【０００７】
非特許文献１の技術で用いる最終目標点に代えて、移動可能範囲内を伸びている経路上にある中間点を一時的目標点とすれば、障害物を回避することと、移動可能範囲内を伸びている経路に沿って移動することの両者を実現できるように想像できる。
【０００８】
しかしながら実際には難しい。実際に試行してみると、障害物を回避する要素と経路に復帰する要素がともに作用してしまい、障害物を回避する移動方向と、経路に復帰しようとして障害物に接近する移動方向が交互に計算されるといったことが生じる。特許文献１の技術と非特許文献１の技術を併用するためには、両者を融合する技術が必要とされる。
【課題を解決するための手段】
【０００９】
本発明では、地図データを用いて自己位置から移動可能範囲内を伸びて最終目標点に至る経路を計算して移動する技術と、障害物が出現したときには障害物を回避する経路を計算して移動する技術を融合し、障害物が出現するまでは経路に沿って移動し、障害物が出現すれば障害物を回避する経路に沿って移動し、障害物を回避した後には経路に復帰する経路に沿って移動する移動体を提供する。なお、本明細書では、地図データから計算した経路であって、移動可能範囲内を自己位置から最終目標点まで伸びている経路を大域経路といい、障害物を回避する経路や、回避後に大域経路に復帰する経路と区別する。
【００１０】
本発明は、地図データに含まれていない障害物を検出したときに、その障害物を回避しながら最終目標点に至る経路に沿って移動する移動体に関する。その移動体は、地図データベースと、大域経路計算手段と、障害物検出装置と、局地経路計算手段と、制御装置を備えている。地図データベースは、移動可能範囲を画定するデータを記憶している。大域経路計算手段は、地図データ上で特定された自己位置から伸びて最終目標点に至る経路であって、移動可能範囲内を伸びる大域経路を計算する。障害物検出装置は、少なくとも移動体の移動方向の前方に障害物が出現したときに、出現した障害物の位置を特定する。局地経路計算手段は、障害物を回避するともに中間目標点に復帰する経路を計算する。制御装置は、移動体が備えている移動装置を制御して局地経路計算手段が計算した経路に沿って移動体を実際に移動させる。
本発明では、局地経路計算手段で用いる中間目標点を、下記条件、すなわち、
１）大域経路計算手段が計算した大域経路上にあり、
２）大域経路に沿って測定した中間目標点までの距離＞大域経路に沿って測定した障害物までの距離を満たす位置に設定する。
【００１１】
大域経路計算手段には既知の技術が活用できる。例えば特許文献１に記載の技術を活用できる。自動車に搭載されているナビゲーション装置で用いる経路探索技術等を活用してもよい。局地経路計算手段にも既知の技術が活用できる。例えば非特許文献１に記載の技術を活用できる。ただし、本発明では、最終目標点に代えて中間目標点を用いて局地経路を計算する。ここで用いる中間目標点は、大域経路上の点とする。この結果、大域経路に復帰する局地経路が計算される。また、大域経路に沿って計測した中間目標点までの距離＞大域経路に沿って計測した障害物までの距離の条件を満たす中間目標点を採用する。上記の距離関係にあると、移動体が障害物を回避して障害物の側方を通過するまでの間は障害物を回避することが高く評価されて局地経路が計算され、側方を通過した後は中間目標点を指向することが高く評価されて局地経路が計算される。障害物を回避する必要がある期間は回避経路が計算され、障害物を回避する必要が薄れた後は大域経路へ復帰する経路が計算される。大域経路計算技術と、障害物を回避する局地経路を計算する技術がうまく融合する。
【００１２】
局地経路計算手段で用いる中間目標点設定する際に、さらに下記条件、すなわち、
３）大域経路に沿って測定した中間目標点までの距離が、大域経路に沿って測定した移動体と障害物の相対速度に比例する距離を含む関係とすることが好ましい。
この場合、移動体と障害物が高速度で接近する場合には、離れた点を中間目標点にして局地経路が計算され、移動体と障害物が低速度で接近する場合には、近接した点を中間目標点にして局地経路が計算される。この結果、自然な経路、すなわち歩行者等に不安を感じさせることなく回避するとともに回避後に無理なく大域経路に復帰する局地経路が計算される。
大域経路に沿った中間目標点までの距離を６メートル以上とすることが好ましい。その場合、移動体が歩行者を避けて移動する際に、歩行者のパーソナルスペースに入り込まない局地経路を計算することができる。
【発明の効果】
【００１３】
本発明によると、移動可能範囲内を伸びて自己位置から最終目標点に至る大域経路を計算する技術と、障害物を回避する局地経路を計算する技術がうまく融合し、大域経路に沿って移動できる間は大域経路を辿り、障害物が出現して障害物を回避する必要が生じれば障害物を回避する局地経路に移行し、障害物を回避した後には大域経路に復帰する経路が計算される。
【図面の簡単な説明】
【００１４】
【図１】移動体が備えている各種装置と、各種装置の機能を説明する図。
【図２】移動体が備えている各種装置の詳細を示す図。
【図３】大域経路を計算する処理手順図。
【図４】移動時に繰り返し実行する処理手順図。
【図５】図４の処理によって得られる経路を説明する図。
【発明を実施するための最良の形態】
【００１５】
以下に説明する実施例の主要な特徴を列記しておく。
（特徴１）移動体は自走式である。
（特徴２）移動体には人を検出する外界センサが搭載されている。
（特徴３）移動体には搭乗者が最終目標点を入力する入力装置が搭載されている。
（特徴４）移動体はコンピュータを搭載しており、経路を計算して移動する。
（特徴５）移動体は、人の周りにあるスペースであって、他者が侵入するとその人が不安を覚えるパーソナルスペースを回避する経路を計算する。
【実施例】
【００１６】
図１に本発明の移動体を具現化した自走式の移動体２が図示されている。移動体２は、移動装置１４と制御装置１２を備えている。移動装置１４は、左輪と左輪用モータと右輪と右輪用モータを備えている。制御装置１２は、制御装置１２に指示される移動体２の移動速度と移動体２の角速度の値から、左輪用モータの回転速度と右輪用モータの回転速度を計算し、計算した回転速度に調整する。制御装置１２は、左輪用モータと右輪用モータの回転速度を独立に制御する。移動体２は、制御装置１２に指示された移動速度で移動し、制御装置１２に指示された角速度で移動方向を変化させる。移動体２は、制御装置１２に指示された経路に沿って移動する。なお、本実施例では、左右輪の回転速度を独立に制御することで移動方向を変化させるが、操舵装置を備えている場合には操舵輪の方向を調整することによって移動方向を変化させてもよい。移動体２の機械的構成は、非特許文献１に開示されている。
【００１７】
移動体２は、壁等の境界線２０で輪郭が定められている移動可能範囲２８内を移動して最終目標点３８に到達する大域経路１８を計算して移動する。移動可能範囲２８内には、例えば駐車中の自動車２４といった静止障害物もあれば、歩行者３２、３６等の移動障害物もあり、移動体２は、それらを避ける局地経路２６，３０、３４を計算して移動する。図１から明らかに、移動体２は、障害物を避け終わると、最終目標点３８に到達する大域経路１８に復帰する。
【００１８】
移動体２は、境界線２０の幾何的パターンを記憶している地図データベース４を備えている。移動体２は、現在位置検出装置（図２の自己位置推定部）を備えており、地図上のどこにいるかを決定することができる。移動体２には、最終目標点を教示することが可能であり、教示された最終目標点が地図上のどこにあるのかを決定することができる。例えば移動体２の現在位置が１６であり、最終目標点が３８であるといったことを特定することができる。自己位置と最終目標点が特定されると、移動体２は、境界線２０に衝突しない条件で（換言すれば移動可能範囲２８内を伸びるという条件で）、自己位置１６から最終目標点３８まで伸びる経路１８を計算する計算手段を備えている。そうして計算される経路は、検出された障害物を避けるために計算される局地経路よりも大域的な経路である。移動体２は、地図データベース４と自己位置と最終目標点から、大域経路１８を計算する大域経路計算手段６を備えている。
大域経路計算手段６は、移動開始時の自己位置と最終目標点から大域経路１８を計算してしまい、その後は再計算しないものであってもよい。あるいは、移動に伴って自己位置が更新されるのに対応して、大域経路１８を再計算するものであってもよい。図３は、前者の方式の大域経路計算手段６によって実行される処理を示している。
【００１９】
移動体２は、障害物検出装置８を備えている。障害物検出装置８は、レーザを発光して反射光を受光するものであり、反射体までの距離と方位を特定することができる。レーザに代えて電波を利用することもできる。参照番号２２は、障害部検出装置８によって障害物の存在を検出可能な範囲を示している。なお、壁等の影の部分までは検出するができない。図１の場合、駐車中の自動車２４が検出され、歩行者３２、３６等は検出されていない様子を示している。
【００２０】
移動体２は、局地経路計算手段１０を備えている。局地経路計算手段１０は、障害物が検出されたときに障害物を回避する局地経路を計算する。図１の場合、障害物２４が検出されたので局地経路２６が計算され、障害物３２が検出されたら局地経路３０が計算され、障害物３６が検出されたら局地経路３４が計算されることを例示している。局地経路２６，３０，３４に図示されているように、本実施例で計算される局地経路２６，３０，３４は、移動体２が障害物を回避した直後に大域経路１８に復帰することを特徴とする。本実施例では、大域経路１８に沿って移動することで最終目標点３８に移動することと、障害物を避ける局地経路２６，３０，３４を採用することで障害物を避けて移動することを両立させている。
【００２１】
図２は、移動体２のシステム構成を示しており、操作者が移動体２に最終目標点３８を教示するために使用する最終目標点指定部４０と、自己位置推定部４２と、地図データベース４を備えている。最終目標点指定部４０は、地図を表示し、表示された地図上において最終目標点の位置を指定するものであってよい。自己位置推定部４２は、ＧＰＳ等であってもよいし、車輪の回転数から自己位置を推定するものであってもよいし、障害物検出装置８で検出される壁等の境界線２０の形状から、地図上における自己位置を推定するものであってもよい。デッドレコニング、スキャンマッチングなどの手法が利用できる。大域経路計算手段６は、自己位置から最終目標点に至る経路であって境界線と干渉しない経路を計算する。図１の場合、大域経路計算手段６が、自己位置１６から最終目標点３８に至る経路であって境界線２０と干渉しない大域経路１８を計算したことを例示している。
【００２２】
大域経路計算手段６は、既知の手法で大域経路を計算する。例えば、ダイクストラ探索技術、Ａ＊探索技術等を利用することができる。自己位置１６から最終目標点３８に至るルートが複数存在する場合には、市販のナビゲーション装置が採用している検索条件を流用することができる。例えば、距離優先を指定したり、道幅が狭いルートを避けるといったように検索条件を設定することができる。大域経路１８は、ルートよりも詳細レベルであり、境界線２０と干渉を避けるために、道幅のどこをたどっていくのかまで指定する。
【００２３】
移動体２は、外界センサ８ａを備えている。外界センサ８ａはレーザ光の発光・受光素子を備えており、図１の検出範囲２２内にある障害物を検出するように、レーザ光の発光方向を水平面内で走査する。外界センサ８ａは、図１に示す検出範囲２２内に、障害物が存在する場合に、その障害物からの反射光を受光する。レーザ光の発光タイミングと受光タイミングから障害物までの距離が計算でき、レーザ光の発光方向から障害物の存在する方位が計算できる。
外界センサ８ａが検出する物体のなかには、壁等の固定物であって地図データベース４に含まれている物体と、駐車車両や歩行者といった一時的な存在物であって地図データベース４に含まれていない物体が含まれる。本明細書では、後者を障害物という。障害物の中には、駐車車両のように静止している障害物と、歩行者のように移動している障害物が存在する。
【００２４】
障害物抽出部８ｂは、外界センサ８ａが検出する物体のなかから地図データベース４に含まれている物体を除外することで障害物を抽出する。
障害物軌道計算部８ｃは、外界センサ８ａで検出された反射光の空間的分布パターンから、障害物毎に特徴的なパターン（フューチャー）を抽出する。外界センサ８ａによる検出と、障害物抽出部８ｂによる抽出処理は、所定時間間隔で繰り返し実施される。所定時間毎に得られるフューチャーの位置から、障害物毎の位置と移動速度と移動方向が計算される。障害物軌道計算部８ｃは、障害物毎に移動軌道を計算する。駐車車両２４であれば静止していることがわかり、歩行者３２，３６であれば、どの位置をどの方向にどの速度で移動しているかがわかる。障害物を抽出して障害物毎の位置と移動速度と移動方向を計算する技術の詳細は、非特許文献１に開示されている。
本実施例では、外界センサ８ａにレーザ光の発光・受光素子を利用しているが、電波を利用するレーダでもよいし、可視光を受光するステレオカメラでもよい。また移動体２に搭載されているとは限らず、移動可能範囲２８を監視している定置式カメラであってもよい。
【００２５】
大域経路計算手段６が計算した大域経路は、中間目標点設定部１０ａに入力される。中間目標点設定部１０ａは、下記のロジックで、大域経路上に中間目標点を次々に設定していく。
１)大域経路がまっすぐに伸びている場合には、中間目標点と隣接する中間目標点の距離を所定距離とする。本実施例では１０ｃｍ毎に中間目標点を設定する。
２）大域経路が曲がっている場合には、中間目標点と隣接する中間目標点の距離を前記の所定距離よりも短くする。このとき、経路が急カーブであるほど、距離を短くする。
中間目標点を上記のように設定し、所定時間毎に次の中間目標点に移動するようにすれば、移動体２は、大域経路１８上を、直進路では高速に移動し、カーブでは減速しながら移動することができる。
【００２６】
設定された中間目標点の情報と、計算された障害物の軌道は、局地経路計算部１０ｂに入力される。局地経路計算部１０ｂでは、非特許文献１に記載の技術を用いて障害物を避ける局地経路を計算する。この際に、局地経路計算部１０ｂは、障害物を避けながら最終目標点３８に進む経路を求めるのではなく、障害物を避けたら大域経路１８に復帰する局地経路を計算する。そのために、局地経路計算部１０ｂは、最終目標点３８の位置に代えて復帰用の中間目標点の位置を計算に用いる。
【００２７】
図５は、移動体２が時刻ｔ０において、位置Ｐ０上にあり（位置Ｐ０は大域経路１８上にある）、障害物５０が位置Ｐ２にあって、ベクトルＶ２に沿って移動していることが判明した状態を例示している。障害物５０は、時刻の経過ともに、ｔ１、ｔ２、ｔ３、ｔ４に示す位置に移動すると推定される。図５は、移動体２が大域経路１８に沿って移動し続けると、障害物５０と干渉することが予想され、障害物５０を回避する経路に切り換える必要が生じた場合を例示している。
【００２８】
局地経路計算部１０ｂは、移動体２と障害物５０が干渉することになるか否かを計算し、干渉することになることが判明したら、移動体２と障害物５０の相対速度を計算する。
ここでいう相対速度は、障害物５０の速度ベクトルＶ２の内の大域経路１８に沿った速度成分Ｖ２ｃと、移動体２の大域経路１８に沿った速度成分Ｖ１ｃから計算されるものであり、速度成分Ｖ２ｃが移動体２に接近する場合には、両者を加算した値であり、速度成分Ｖ２ｃが速度成分Ｖ１ｃと同一方向の場合には、Ｖ１ｃからＶ２ｃを減算した値である。
【００２９】
局地経路計算部１０ｂは、相対速度Ｖから、中間目標点までの距離Ｌ２を下記の式で計算する。
Ｌ２＝α＋β×相対速度Ｖ・・・・（１）
上記のαとβは、干渉する可能性のある障害物５０までの距離Ｌ１に対してＬ２＞Ｌ１の関係にある距離Ｌ２が計算される値である。αとβは定数としてもよい。またαについては、移動体の速度によって増減する値としてもよい。あるいは、経路の局率半径が大きい（直進に近い経路を高速に移動している）ほど大きなαを採用し、経路の局率半径が小さい（急カーブを低速で移動している）ほど小さなαを採用してもよい。距離Ｌ２が計算されたら、計算された距離Ｌ２に最も近い中間目標点を抽出し、抽出された中間目標点を復帰用の中間目標点とする。局地経路計算部１０ｂは、復帰用の中間目標点において大域経路に復帰する局地経路を計算する。
相対速度Ｖは、移動体２と障害物５０が接近する場合に正の値とする。最終目標点に近づいた結果、Ｌ２＞最終目標点までの距離となったら、最終目標点を局地計算に用いる復帰用の中間目標点とする。移動体２と障害物５０が離反するために負の値をとる場合には、移動体２と障害物５０が干渉する可能性がない。
【００３０】
移動体２と障害物５０が干渉しない場合には、局地経路計算部１０ｂが、中間目標点設定部１０ａで設定された中間目標点を所定時間毎に辿る局地経路を計算する。移動体２と障害物５０が干渉する場合には、局地経路計算部１０ｂが、非特許文献１に記載されているDynamic Window Approachの技術を活用して障害物を回避する経路を計算する。その際に、最終目標点に代えて、上記の距離Ｌ２の近傍にある復帰用中間目標点を用いる。その結果、障害物の側方を通過するまでは障害物を回避することを重視して経路が計算され、障害物の側方を通過すると大域経路に復帰することを重視して経路が計算される。復帰用に選択した中間目標点において大域経路に復帰する局地経路が計算される。
【００３１】
非特許文献１の技術では、進行方向の候補と進行スピードの候補を複数作成し、候補毎に好ましさの指標を求め、最も高い指標をもたらす進行方向の候補と進行スピードの候補を採用するロジックを用いる。例えば移動体２がＰ０の示す位置にある場合、Ａ〜Ｇに例示する進行方向（ＡからＧは所定角度ずつ相違している）の候補と、複数段に設定されている進行スピードのレベルで指定される複数の候補（例えば低速・中速・高速の３候補）の組み合わせで得られる進行ベクトルの候補を設定する。次に、各進行ベクトルの候補毎に、障害物までの距離と、中間目標点を指向するベクトルとなす角度と、現在の進行方向からの変化角度等を計算し、障害物までの距離については長いほど高得点を与え、中間目標点を指向するベクトルとなす角度についてはその角度が小さいほど高得点を与え、変化角度についてはその変化角度が小さいほど高得点を与える。最も高得点を与える候補を採用する。障害物までの距離が長く、できるだけ復帰用の中間目標点を指向しており、進行方向の変化が少ない経路が採用される。図５の場合、時刻ｔ０ではベクトルＶ１１が最も高得点となる場合を例示している。移動体２は、上記の計算を所定時間毎に繰り返しながら進行する。図５では、時刻ｔ１ではベクトルＶ１２が最も高得点となる場合を例示し、時刻ｔ２ではベクトルＶ１３が最も高得点となる場合を例示している。
【００３２】
本実施例の場合、移動体２の側で障害物５０の軌道を計算しており、障害物５０の将来位置が予測可能となっている。そこで、時刻ｔ０において、時刻ｔ１、ｔ２等における位置関係を予想することができ、時刻ｔ０において、時刻ｔ１、ｔ２等のおける進行ベクトルを決定することも可能である。例えば、時刻ｔ０で局地経路を計算する際に、障害物５０が時刻ｔ１において存在するであろう位置、障害物５０が時刻ｔ２において存在するであろう位置等を推定することができる。そこで、時刻ｔ０で局地経路を計算する際に、移動体２は、時刻ｔ０において可能な進行方向の候補と進行速度の候補、時刻ｔ１において可能な進行方向の候補と進行速度の候補、時刻ｔ２において可能な進行方向の候補と進行速度の候補等を計算し、時刻ｔ０における候補とその得点、時刻ｔ１における候補とその得点、時刻ｔ２における候補とその得点等を計算し、それらの得点を合計した合計得点が最も高くなる経路を計算する。図の場合、時刻ｔ０では進行ベクトルＶ１１を採用し、時刻ｔ１では進行ベクトルＶ１２を採用し、時刻ｔ３では進行ベクトルＶ１３を採用し、時刻ｔ４では進行ベクトルＶ１４を採用し、時刻ｔ５では進行ベクトルＶ１５を採用する経路によるときに、合計得点が最も高くなる場合を例示している。この場合、局地経路計算手段１０は、進行ベクトルＶ１１、Ｖ１２、Ｖ１３、Ｖ１４、Ｖ１５を接続した局地経路を計算する。本実施例では、時刻ｔ０において、その時点で設定した復帰用中間目標点Ｍ５にまで移動体２が移動するのに要する時間にわたって合計した得点が最も高くなる局地経路を計算する。復帰用中間目標点Ｍ５までのあいだの障害物の移動軌跡に基づいて局地経路を計算する。
【００３３】
上記の計算を所定時間毎に繰り返してもよい。例えば、時刻ｔ０において、図５のＶ１１、Ｖ１２、Ｖ１３、Ｖ１４、Ｖ１５を計算し、時刻ｔ１において同種の計算を再び実行する。この結果、図５に示す速度ベクトルＶ１２、Ｖ１３、Ｖ１４、Ｖ１５が再計算され、Ｖ１６が新に計算される。
後者の方法によると、障害物が移動速度または移動方向を変化させることにも追従することができる。
【００３４】
図４は、移動体２と障害物５０が干渉する場合に、局地経路計算部１０ｂによって実行される処理手順を示している。移動体２の移動中は、図４の処理が所定時間間隔で繰り返し実行される。
ステップＳ４０では、移動体２の自己位置が検出される。ステップＳ４２では、所定時間前に検出された自己位置からの変化量から、自己速度（正確には方向も）が計算される。ステップＳ４４では、障害物の位置が検出される。ステップＳ４４で検出される障害物位置は、移動体２に対する相対的位置であるが、ステップＳ４０で移動体１の自己位置が検出されていることから、障害物の地図上の位置が判る。ステップＳ４６では、所定時間前に検出された障害物位置からの変化量から、障害物の移動速度（正確には移動方向も）が計算される。
【００３５】
ステップＳ４８では、図５を参照して説明した大域経路１８に沿った方向での相対速度を計算する。ステップＳ５０では、式１を用いて、局地経路計算手段１０で用いる中間目標点までの距離を計算する。ステップＳ５２では、最終目標点までの距離と、ステップＳ５０で計算した距離との大小を比較する。最終目標点に近づくと、Ｌ２＞最終目標点までの距離となる。その条件が成立したら（Ｓ５２でＹＥＳとなったら）、最終目標点を局地計算に用いる復帰用の中間目標点とする（Ｓ５４）。Ｌ２＜最終目標点までの距離の間は（Ｓ５２でＮＯとなる間は）、距離Ｌ２にある中間目標点を局地計算に用いる復帰用の中間目標点とする（Ｓ５６）。
ステップＳ５８では、非特許文献１の技術で障害物を回避する局地経路を計算する。ステップＳ５４またはＳ５６で設定した復帰用中間目標点を用い、移動体が復帰用中間目標点に到達するまでの間の障害物の移動軌跡を用いて、局地経路を計算する。局地経路が計算されれば、その経路となる左モータの回転速度と右モータの回転速度を計算してその回転速度に調整する。
【００３６】
局地経路計算手段１０が最終目標点に代えて復帰用中間目標点を利用する場合、復帰用中間目標点において大域経路１８に復帰する局地経路が高く評価される。
障害物を回避する必要がある間は障害物を回避する局地経路が計算され、障害物を回避し終えたら大域経路に復帰する局地経路が計算されるようにするためには、局地経路計算手段１０で用いる復帰用中間目標点の選定が重要である。復帰用中間目標点が近すぎる場合、すなわち移動体と障害物の間に復帰用中間目標点を設定した場合、障害物を回避する必要がある間に、障害物を回避する局地経路を計算する要素と、大域経路に復帰する局地経路を計算する要素が同時に作用する。後者は、障害物に向かう局地経路を計算することになる。移動体と障害物の間に中間目標点を設定した場合、障害物を回避する必要がある間に、障害物を回避する局地経路を計算する要素と、障害物に向かう局地経路を計算する要素が同時に作用する。障害物を回避する経路が高く評価されたり、障害物に向かう経路が高く評価されたりしながら局地経路が計算されるために、実際に得られる局地経路は複雑でギクシャクしたものとなる。
【００３７】
本実施例では、障害物までの距離Ｌ１よりも大きな距離Ｌ２が計算される値に設定されているαとβを用い、Ｌ２＝α＋β×相対速度Ｖの式で計算される距離にある中間目標点を用いて局地経路を計算するために、障害物を回避する必要がある間は障害物を回避する経路が高く評価されて局地経路が計算され、障害物を回避し終えたら大域経路に復帰する経路が高く評価されて局地経路が計算される。
【００３８】
上記の結果、移動体は、
１）大域経路に沿って最終目標点に向かって移動する。
２）移動中に障害物を検出すると、障害物との干渉を避ける経路を高く評価しながら局地経路を探索する。この際にも大域経路上に置かれている復帰用中間目標点に復帰する要素が加味されるが、復帰用中間目標点は障害物の背後に設定されることから、障害物の側方を通過するまでの間は、中間目標点に復帰する要素が、移動体を障害物の前方に侵入させる局地経路を探索することがない。
３）障害物の側方を通過すると、その後は速やかに復帰用中間目標点に復帰する経路が探索される。障害物を避けているうちに目標点を見失うことがない。
４）大域経路に復帰すると、その後は大域経路に沿って最終目標点に向けて移動する。
以上の結果、移動体は、大域経路に沿って最終目標点に移動することと、障害物を回避することをうまく両立させることができる。
【００３９】
局地経路に沿って移動する間に局地経路を計算し直す技術による場合、図１の検出範囲２２が示すように、移動体２の背面では障害物を検出しないために、移動体２が障害物の側方にまですすんだ段階で、障害物が検出されなくなる。この場合、Ｌ２＝α＋β×相対速度Ｖの式における相対速度Ｖがゼロとなる。障害物が検出されなくなくなった時に、距離Ｌ２の値が不連続に変化する。これを避けるためには、移動体２の後方まで検出するセンサを用いることが有効である。あるいは、障害物が検出されなくなった時点で相対速度Ｖをゼロにするのに代えて、一定の速度でゼロに変化する人為的相対速度を用いてもよい。
【００４０】
障害物が歩行者であることがある。歩行者にはそれ以上に他者に接近されると不安ないし不快と感じるパーソナルスペースが存在する。図5の５２は、歩行者５０のパーソナルスペースを示す。本実施例の場合、大域経路に沿って６メートル以上はなれた位置にある中間目標点を復帰用の中間目標点に選定すると、移動体２が歩行者５０に干渉しないだけでなく、歩行者５０のパーソナルスペース５２に入り込まない局地経路を計算することができる。
【００４１】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず特許請求の範囲を限定するものではない。また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書又は図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【００４２】
２：移動体
４：地図データベース
６：大域経路計算手段
８：障害物検出装置
１０:局地経路計算手段
１２：制御装置
１４：移動装置
１６：現在位置
１８：大域経路
２０：境界線
２２：検出範囲
２４：駐車車両（静止障害物）
２６，３０，３４：局地経路
２８：移動可能範囲
３２，３６：歩行者（移動障害物）
３８：最終目標点
【特許請求の範囲】
【請求項１】
障害物を検出したときにその障害物を回避しながら最終目標点に至る経路に沿って移動する移動体であり、
移動可能範囲を画定するデータを記憶している地図データベースと、
地図データ上で特定された自己位置から伸びて最終目標点に至る経路であって、移動可能範囲内を伸びる大域経路を計算する大域経路計算手段と、
少なくとも移動体の移動方向の前方に障害物が出現したときに出現した障害物の位置を特定する障害物検出装置と、
障害物を回避するともに中間目標点に復帰する経路を計算する局地経路計算手段と、
移動体が備えている移動装置を制御して局地経路計算手段が計算した経路に沿って移動体を移動させる制御装置を備えており、
前記局地経路計算手段で用いる前記中間目標点を、下記条件、すなわち、
１）前記大域経路計算手段が計算した前記大域経路上にあり、
２）大域経路に沿った中間目標点までの距離＞大域経路に沿った障害物までの距離
を満たす位置に設定することを特徴とする移動体。
【請求項２】
大域経路に沿った中間目標点までの距離が、大域経路に沿った移動体と障害物の相対速度に比例する距離を含むことを特徴とする請求項1に記載の移動体。
【請求項３】
大域経路に沿った中間目標点までの距離が６メートル以上であることを特徴とする請求項１または２に記載の移動体。
【請求項１】
障害物を検出したときにその障害物を回避しながら最終目標点に至る経路に沿って移動する移動体であり、
移動可能範囲を画定するデータを記憶している地図データベースと、
地図データ上で特定された自己位置から伸びて最終目標点に至る経路であって、移動可能範囲内を伸びる大域経路を計算する大域経路計算手段と、
少なくとも移動体の移動方向の前方に障害物が出現したときに出現した障害物の位置を特定する障害物検出装置と、
障害物を回避するともに中間目標点に復帰する経路を計算する局地経路計算手段と、
移動体が備えている移動装置を制御して局地経路計算手段が計算した経路に沿って移動体を移動させる制御装置を備えており、
前記局地経路計算手段で用いる前記中間目標点を、下記条件、すなわち、
１）前記大域経路計算手段が計算した前記大域経路上にあり、
２）大域経路に沿った中間目標点までの距離＞大域経路に沿った障害物までの距離
を満たす位置に設定することを特徴とする移動体。
【請求項２】
大域経路に沿った中間目標点までの距離が、大域経路に沿った移動体と障害物の相対速度に比例する距離を含むことを特徴とする請求項1に記載の移動体。
【請求項３】
大域経路に沿った中間目標点までの距離が６メートル以上であることを特徴とする請求項１または２に記載の移動体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１２−２４３０２９（Ｐ２０１２−２４３０２９Ａ）
【公開日】平成２４年１２月１０日（２０１２．１２．１０）
【国際特許分類】
【出願番号】特願２０１１−１１１６０９（Ｐ２０１１−１１１６０９）
【出願日】平成２３年５月１８日（２０１１．５．１８）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２４年１２月１０日（２０１２．１２．１０）
【国際特許分類】
【出願日】平成２３年５月１８日（２０１１．５．１８）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]