JP5349804B2

JP5349804B2 - 移動ロボットシステム及びその制御方法

Info

Publication number: JP5349804B2
Application number: JP2008002716A
Authority: JP
Inventors: 俊夫守屋; 高斉松本; 良三正木; 修一槙
Original assignee: Hitachi Industrial Equipment Systems Co Ltd
Current assignee: Hitachi Industrial Equipment Systems Co Ltd
Priority date: 2008-01-10
Filing date: 2008-01-10
Publication date: 2013-11-20
Anticipated expiration: 2028-01-10
Also published as: JP2009163156A

Description

本発明は、目的地まで人の操作なしに自動的に到達することができる自律移動型移動ロボットに関する。より詳しくは、移動ロボットに搭載された距離センサで測定されたセンシングデータと環境の幾何状況を示した二次元の環境地図とを用いて自己の位置姿勢を推定し、その結果に基づいて自律移動制御を行うロボットシステムに関する。

まず、距離センサと二次元の環境地図を用いて移動ロボットの位置姿勢を推定する方法について説明する。

ここで距離センサとは、距離センサの周辺の複数の方向について、障害物までの距離を計測できるものである。具体的な仕様としてさまざまなものが考えられるが、ここでは図２に示すように、ある高さの水平面上を距離センサ２１０の正面に対して-90°から+90°の方向に、１°毎の合計181通りの異なる方向について、障害物２３０までの距離データが瞬時に計測できるものを想定する。また、この距離センサ２１０は、移動ロボット２２０に搭載されているものとする。

図３に、この距離センサ２１０を用いて測定されたセンシングデータの例を示す。各センシングデータは、角度φと距離dの組み合わせとして(d₀,φ₀),(d₁,φ₁),...,(d₁₈₀,φ₁₈₀)としてあらわされる。ここで各変数の数字の添え字iはデータ番号で、φ_iはi番目の測定方向、またd_iはφ_i方向における障害物までの距離をを示す。測定方向は時計回りを正方向とし、距離センサの主方向となす角度を示すものとし、主方向とのなす角度が小さい順にデータ番号が割り当てられる。すなわちφ₀=-90°,φ₁=-89°,...,φ₁₈₀=+90.0°を示す。
なお、ここでは距離センサの測定能力を考慮し、測定可能距離を0.0mより大きく16.0m以下と想定し、d_iの値の範囲を、0.0m＜d_i≦16.0mに制限することとする。また、この181個の距離データを１まとまりとして、１フレーム分のセンシングデータ３００と呼ぶこととする。

一方、二次元の環境地図とは、距離センサ２１０による測定高さ断面での障害物の存在を、図４に示すようなディジタル画像としてあらわしたものである。ここでは図４に示すように、障害物が存在しない場所４１０を白を示す画素値、障害物が存在する場所４２０を黒を示す画素値としてあらわすことにする。

移動ロボット２２０は、平らな地面上を移動するものと仮定する。よってその移動によって変化する位置姿勢に関する自由度は、位置について(x,y)の２パラメータ、姿勢については移動ロボットが向く方向角度に関するθの１パラメータの、合計３パラメータである。なお、これらのパラメータをあらわす座標系は、図４に示すような環境地図５１０をあらわす座標系と一致させるものとする。

移動ロボットの位置姿勢を推定することとは、任意の位置姿勢にある移動ロボットのセンシングデータから、上記(x,y,θ)の３パラメータを求めることを意味する。よって、本発明が対象とする機能は、任意の位置姿勢にある移動ロボットの位置姿勢パラメータ(x,y,θ)を、移動ロボットに搭載された距離センサによるセンシングデータと、あらかじめ用意された環境地図を用いて求めることとなる。

本機能を実現する手段として、いくつかの方法が考えられるが、ここではその最も基本的な方法となるマッチング処理による方法を説明する。

マッチング処理による方法とは、あるフレームにおいて計測されたセンシングデータ全体の位置および姿勢（方向）を変えながら、当該センシングデータを環境地図に重ね合わせ、センシングデータと環境地図とが最もマッチする位置および姿勢を探し出し、これをもって移動ロボットの位置姿勢パラメータの推定値とするというものである。その原理は、移動ロボットがその位置および姿勢にあるときに、計測されたデータと同じデータが最も高い確率で得られるであろうという考え方に基づいている。

このようなマッチング処理による方法は、具体的には以下に述べるような処理によって実施される。

(1)あるフレームにおけるすべてのセンシングデータ（前述した距離センサの例では１８１個）に対して、これを地図上でθだけ回転させ、(x,y)だけ平行移動させた位置を求める。

(2)それらの位置の環境地図上の画素値を参照し、障害物があるものの数を数え、これをそのパラメータ (x,y,θ)における評価値とする。

(3)上記(1)および(2)の処理を、考えられうるすべての値をもつ(x,y,θ)の組み合わせについて、評価値が最も大きい(x,y,θ)を解とする。

図５に、この考え方に基づくマッチング処理の概要を示す。環境地図５１０と、センシングデータ３００とが与えられたときに、この方法によって最もマッチすると判断された状況が図５右に示した状況であり、このときの平行移動成分(x,y)と回転成分θが、移動ロボットの位置姿勢推定値となる。

移動ロボット２２０は、このようにして逐次自分の位置姿勢を求め、その値と自分がすすむべき経路とを比較することで移動方向や移動速度などを決定し、これに基づき移動ロボット２２０の移動機構部を制御することで目的の移動が実現される。なおここで移動を正確に行うためには移動制御をなるべく短い周期で行う必要があるため、位置姿勢推定処理も高速に行うことが要求される。具体的には数10msecから100msec程度、すなわち1秒間に数10回程度の処理が逐次行えるような高速性が望まれる。

上記位置姿勢推定処理は、ＣＰＵに代表される中央演算装置と、メモリに代表される主記憶装置、ならびにハードディスクに代表される補助記憶装置の組み合わせで実現されることが一般に考えられる。ここで位置姿勢推定処理を高速に行うためには、そこで頻繁にアクセスするデータ、すなわち環境地図５１０を、メモリ等の主記憶上に記憶しておく必要がある。しかしながらここで用いる環境地図について、これが実環境の長さ1cmの距離を1pixelであらわしたものと仮定すると、例えば300m×300mの領域で 900MBのデータサイズとなるなど、実環境の対象とする領域が大きくなると現在一般的に用いられているメモリには記憶しきれない大きなサイズとなってしまう可能性がある。またこれをハードディスク等の補助記憶装置に記憶する場合も、その読み書きの処理時間やこれを複数管理する場合などを考慮すると、できるだけサイズを削減して記憶することが望まれる。
そこで、環境地図を圧縮して補助記憶に記憶しておき、位置姿勢推定時にはそこで必要な領域のみ主記憶に伸長、展開して利用する方法を適用することが考えられる。なおここで伸長、展開のための処理時間が新たに必要となるが、上に述べたように位置姿勢推定の全体処理を数10msecから100msec程度で行おうとするとこの処理は数msecから長くても20msec程度で行う必要がある。

一方、主記憶に読み込むべきデータの量は、例えばセンサのセンシング可能範囲が16mであることから、位置推定に必要な地図領域は(16m×2)×(16m×2)程度の大きさとなり、これは非圧縮のデータとして10MB程度のサイズとなる。現在の一般的なＣＰＵ等の性能を考慮すると、伸長後にこの程度の大きさになる圧縮データに対し、外部記憶装置からの読み込み、伸長処理、主記憶装置への書き込み等のすべての処理を上記時間内で行うことは極めて困難であり、仮に可能だとしても他の処理に計算資源を割り当てる目的からこの処理は少なけれ少ないほど望ましい。

そこで本発明では、補助記憶装置と主記憶装置を併用して地図の管理を行う際に、位置姿勢推定時に必要となる処理量を少なくする方法を提供する。

上記課題に関連する公知技術として、「特開2006-39486 地図情報処理装置」では、ユーザの経路に沿って形成される地図画像を小地図として簡易に抜き出し、ユーザ経路に沿い且つ途切れることがなくユーザが携帯する携帯機器に表示させることができる地図情報処理装置を提示している。しかしながら、本方法は地図データを画面上に表示することを目的としており、単一のメモリ上に効率的に展開する機能を提示しているわけではない。

「特開平9-160482 ナビゲーション装置」では、地面を鳥瞰撮影した画像を分割圧縮し、車両の現在位置に基づき適切な領域の画像を伸長して表示する。この方法において、画像を分割圧縮し、車両の位置移動にともなって、この分割圧縮された画像から適切なものを選び、バッファメモリに効率的に蓄え、表示する方法を提示している。しかしながらこの方法では、バッファメモリが連続したものではないため、これを地図として連続としたものとして表示するためには、バッファメモリから表示メモリに適切な順番になるように転送する別の処理が新たに必要となり、ここで処理時間が余計にかかってしまう問題が発生する。すなわち、このようなキャッシュメモリやバッファメモリを用いる方法では、地図として空間上連続する領域が、メモリ空間上では必ずしも連続したものとはならないため、本発明が述べるような位置姿勢推定を目的とした場合、推定時に大量に行われるメモリアクセスのためのアドレス計算が複雑になり、結果として処理時間が長くなる問題がある。
すなわち本発明では、記憶装置が記憶すべき容量を削減し、さらにマッチングに基づく位置姿勢推定処理を行う際の地図データへの高速なアクセスを可能とし、さらに補助記憶装置と主記憶装置を併用した地図データ処理を高速に行うことを同時に実現しなければならないという課題を解決する。

特開2006-39486号特開平9-160482号

周囲の状況を距離センサで計測したセンシングデータと、地図との照合により、自己の位置姿勢を認識し、その認識結果に基づく移動制御によって移動する移動ロボットシステムにおいて、上記課題を解決するため本発明では、全体地図を管理する全体地図記憶手段と、位置姿勢推定に必要な地図領域を部分地図として連続的に管理する部分地図記憶手段と、全体地図管理手段から地図データを選択的に読み込み、これを用いて部分地図記憶手段の内容を更新する部分地図更新手段を備える。

地図を主記憶と補助記憶に分けて管理することで、地図が拡大しても、限られた容量の主記憶だけを用いて高速な位置姿勢推定処理を行うことができる。補助記憶には全体地図を圧縮して保存することで、記憶すべきデータ量ならびにその読み込み処理時間を削減することができる。また補助記憶には全体地図を複数のブロックに分割して記憶し、必要となるブロックのみを選択的に伸長して主記憶上に書き込むことで、主記憶上のデータを高速に準備することができる。

以下、本発明の実施の形態について説明する。

図１は、本発明の一実施形態に係る移動ロボットシステムの全体構成を示す機能ブロック構成図である。
移動ロボットシステムは、車輪などによって平らな床を持つ実環境を任意の方向に移動可能な移動機構部１３０、経路やロボットの位置姿勢パラメータから移動すべき方向や距離、速度などを算出し、その結果に基づき移動機構１３０の制御を行う移動制御部１２０、ロボット周辺の環境幾何状況を計測する距離センサ２１０、ロボットが現在いる位置と方向を、背景技術の項で説明した地図とセンサデータのマッチングによって求める位置姿勢推定部１１０、その位置姿勢推定を行う際に利用する部分地図１９０を記憶する部分地図記憶部１７０、環境の全体地図１８０を複数のブロックに分割し、なおかつそれぞれを圧縮して記憶する全体地図記憶部１６０、全体地図１８０の中から、部分地図１９０を構成するのに必要なブロック領域を選択して読み込み、部分地図１９０の内容を更新する部分地図更新部１５０から構成される。なおこの部分地図更新部１５０は、読み込むべきブロックを選択指示する読み込みブロック選択指示部１５２と、選択されて読み込まれた圧縮ブロック地図１８３を伸長しこれを更新用ブロック地図１８６として部分地図記憶部１７０に書き込む部分地図データ伸長部１５４から構成される。

また、上記の全体地図記憶部１６０、膨大な量のデータを管理するという観点からハードディスク等の外部記憶装置によって実装されることを想定し、また上記の部分地図記憶部１７０は、位置姿勢推定の演算時にデータへのアクセスを高速化させる目的からメモリ等の主記憶装置によって実装されることを想定する。また、上述の選択的な読み込み処理や伸長処理、位置姿勢推定処理等の各処理は、ＣＰＵ等の中央演算装置を動作させるプログラムによって行われることを想定する。

以上のシステム構成において、全体地図ならびに部分地図を管理する方法について説明する。ここでは説明をわかりやすくするため、以下１例となる具体的な数値を用いて説明をすすめることにする。ただしこの数値は本発明の適用をこの値に限定するものではない。
まず全体地図１８０に関して説明する。ここではこれを400cm×400cmの矩形領域を１ブロック画像とし、このブロック画像の集合として全体をあらわすものとする。なお実環境における1cmを画像上では1pixelであらわすものとする。これらのブロック画像ごとに圧縮がかけられ、ハードディスク上に記憶されるものとする。ここでの圧縮方法は、圧縮・伸長を繰り返しても内容がかわらない可逆圧縮方法を用いる。その具体的方法としてはランレングス法をはじめとして様々な方法が考えられるが、その方法の内容に関しては本発明の本質とは無関係なのでここでの詳細説明は省き、現在一般的に利用可能な圧縮プログラムを利用するものと仮定する。またこの圧縮方法に対する伸長プログラムも本システム内で利用可能であるものと仮定する。これらのブロック画像は、それぞれ異なるファイルとして保存するなどの方法によって、プログラムが任意に指定するブロック画像に対するランダムなアクセスを可能にしておく。

次に部分地図１９０に関して説明する。位置姿勢推定におけるマッチング処理では、メモリ上にある非圧縮の地図データにアクセスする。このメモリ上に展開されている地図データが、本発明における部分地図１９０である。この部分地図１９０はロボットが位置姿勢の推定を行うのに必要な範囲分だけあればよく上記の全体地図１８０をすべて含んでいる必要は無い。すなわち上記全体地図の一部分であることから、部分地図という名称をつけている。

この部分地図１９０が、全体地図１８０のどの部分をあらわしたものであるかを図６を用いて説明するのにあたって、必要部分地図６２０という考え方を導入することにする。移動ロボット２２０に搭載された距離センサ２１０が計測できる領域は、移動ロボットが現在存在しているであろう候補位置６１０を中心として、ある一定の領域内に限られる。すなわち、この領域の地図があれば移動ロボットの位置姿勢推定には十分であり、逆にいうとそれ以上の範囲の地図データがあっても無駄になってしまうことになる。

この領域を決定するにあたって、まず移動ロボット２２０の存在可能範囲を考える。移動ロボットの移動速度を例えば7.2km/hとし、前述の位置姿勢推定のフレーム周期を100msごととすると、周期ごとに移動する距離は 720000cm/60/60/1000*100 = 20cm である。よって、前回のフレームの移動ロボット２２０の位置に対して、現在のフレームの移動ロボット２２０の位置は縦横ともに±20cmの範囲におさまっている。そこで、前回のフレームの位置としてそのフレームの位置推定値を用い、またその推定値の誤差を最大で5cm程度あるものと仮定し、現在のフレームの移動ロボット２２０の位置はその±25cmの範囲に収まっているものと仮定する。これから、移動ロボット２２０の存在可能範囲は、前回のフレームの位置推定値に対し、±25cmの矩形の範囲にあるものと定義する。なお、前回のフレームの位置推定値を、図６における移動ロボット２２０の候補位置６１０と呼ぶことにする。なおこの値を決める方法としては、この前フレームの位置推定値を用いることに限られるものではなく、ロボットの移動速度からこれを計算・予測する方法、車輪の回転情報からこれを推定する方法なども考えられる。
必要部分地図６２０の領域を決定するにあたって、次に距離センサ２１０の測定可能距離を考える。距離センサ２１０が測定可能な距離の最大値が1600cmなので、位置姿勢推定に用いることができる地図の領域は、移動ロボット２２０の位置に対して±1600cmの範囲に限られる。

以上述べたように、移動ロボット２２０の存在可能範囲、ならびに距離センサ２１０の測定可能距離によって、位置姿勢推定に用いる地図の領域が限定されるが、この２つの要素を合わせると、移動ロボットの候補位置６１０を中心に、(1600cm×2 + 25cm × 2) × (1600cm×2 + 25cm × 2) = 3300cm×3300cmの範囲の地図があれば、位置姿勢推定に十分であることがわかる。

以上を考慮し、図６に示すように、本実施の形態では移動ロボットの候補位置６１０を含むブロックを中心とし、その左右上下にそれぞれ4ブロックの幅を持つ領域を必要部分地図６２０と定義することにする。具体的には9×9のブロックとなり、ここで1ブロックの大きさが400cm×400cmなので全体として3600cm×3600cmの領域となるものである。上で説明した 3300cm×3300cmの大きさにたいして若干余裕をもたせた値になっているが、これについては適宜調整可能である。この必要部分地図６２０が準備されていれば、移動ロボット２２０がどの方向を向こうとも、位置推定に必要な地図の領域は必ず含んでいることになる。

以上述べた、必要部分地図６２０の考え方を用い、まず図１における部分地図１９０が、上記必要部分地図６２０と同じものとして構成される場合において、部分地図１９０を準備する方法を図６、図７、図８を用いて説明する。このときの基本的な考え方は、部分地図１９０に必要部分地図６２０が過不足なく常に書き込まれるようにするということである。すなわち図６における必要部分地図６２０の領域の地図のデータが、非圧縮データとして図１あるいは図７における部分地図１９０としてメモリ上に記憶されている状態にする。

この処理は、図１における部分地図更新部１５０によって行われるが、その処理内容を図８を用いて説明する。なおここで点線の枠で囲まれた部分８６０の処理が、部分地図更新部１５０で行う処理である。

まず、移動ロボットの候補位置６１０を、前フレームの位置推定値として設定する（８２０）。最初の動き始めで前フレームの推定値が無いときには、これを人が指定したり、あらかじめ決められた場所から動き始めるといった条件を設けることにより、これが一意に決められるものとする。

次に、この候補位置６１０から必要部分地図６２０の領域を算出する（８２５）。その算出方法は、上に述べた必要部分地図の定義で説明したとおりである。
次に、部分地図記憶部１７０すなわちメモリ上の部分地図１９０に書き込まれている内容と、必要部分地図６２０の示す領域が等しいものとなるように、全体地図記憶部１６０すなわちハードディスクに記憶されている全体地図１８０から足りない領域のブロックを選択する（８３０）。

以上の処理を行うのが、部分地図更新部１５０における読み込みブロック選択指示部１５２である。

次に、選択されたブロックのデータを読み込み（８３５）、その伸長処理を行ない（８４０）、部分地図１９０にこれを書き込む（８４５）。この際に、部分地図を記憶する実体となるメモリ上のどの部分に書き込むかが問題となるが、これについては後述することにする。

以上の処理を行うのが、部分地図更新部１５０における読み込みデータ伸長部１５４である。

この書き込みが終わると、図１において位置姿勢推定部１１０が位置姿勢推定を行うのに必要となる部分地図１９０がメモリ上に準備されたことになるので、これを用いた位置姿勢処理や移動ロボット２２０の移動制御などが行われる（８５０）。
これら一連の処理を１周期の処理（１フレームの処理）とし、これを繰り返すことで移動ロボット２２０の移動制御が行われる。

すなわち、部分地図更新手段は、ロボットの位置姿勢推定初期値に基づき、ロボットの位置姿勢推定に必要となる地図領域の中で、まだ部分地図記憶手段上に書き込まれていないブロックのみを選択して伸長し、空間上連続する領域がメモリ上でも連続する領域となるように記憶されている内容を更新する。これによって位置姿勢推定処理を行う際の必要な地図データへのアクセスが高速に行われる。次に、移動ロボット２２０の移動にともない、全体地図１８０のどの領域が、部分地図１９０としてメモリ上に展開されていくかを、図９を用いて説明する。

まず移動ロボット２２０の候補位置６１０が図９に示した位置のとき、全体地図１８０の中で必要部分地図６２０としてメモリ上に読み込まれる領域は、図９上図に示したとおりである。この領域が上述の必要部分地図の定義に従ったものとなっていることが確認される。ロボットの移動にともない、このロボットの候補位置６１０が移動したとしても、それが図におけるブロック９００の内部にある限りにおいては、この必要部分地図の領域は変わらず、よってメモリ上に展開されている部分地図も変えなくてよいことになる。しかし図９に示すように、ロボットの移動にともない、このロボットの候補位置６１０がブロックをまたいでブロック９０３の内部の点６１１に移動したときは、必要部分地図６２０の領域が図９下図に示すものに変化する。このときには、図９下図で斜線で示した領域９８０がメモリ上から不要になり、斜線で示した領域９７０が、新たにメモリ上に必要となるので、領域９８０が領域９７０に置き換わるようにメモリの内容が更新されることになる。

以上述べてきたように必要部分地図６２０の内容と同じになるように部分地図１９０の内容を更新していくことにより、限られた大きさのメモリだけを用いて位置姿勢推定処理が行えるようになる。

しかしながらこの方法だと、ロボットがブロックの境い目付近を動く場合、次のような問題が発生する。それは、ロボットがブロックをまたいで移動する頻度が多くなったとき、そのたびことに必要部分地図６２０の領域が変わり、部分地図１９０の内容を更新してメモリの書き換え処理が頻繁に発生し、全体地図の読み込み、データの圧縮展開、およびメモリへの書き込みの処理量が多くなってしまう、という問題である。

そこで本発明では、上記部分地図１９０は、必要部分地図６２０の示す領域のみから構成されるのではなく、これに拡張領域と呼ばれる付属的な領域を含んだ形で構成されるようにする。これを拡張領域を含む手法と呼ぶことにし、これについて図１０を用いて詳述する。

拡張領域を含む手法では、図１０に示すように、全体地図１８０における点線で囲んだ部分７７０を、図１１に示すような部分地図１９０としてメモリ上に展開する。このときの内容は、図６で説明した必要部分地図６２０に加えて、縦１列、横１列のブロック分の領域７６７を加えたものであり、メモリはこの領域分のデータが保存されるサイズを準備しておく。

このときに、図１１に示す部分地図１９０には、必要部分地図６２０の領域が不足なく常に書き込まれるようにする。これは、図１における部分地図更新部１５０によって行われるが、その処理内容は、基本的に図８で説明したものと同等である。
しかしながら、拡張領域を含む手法ではこの拡張領域７６７が付加されたことによって、ロボットがブロックをまたいで移動する頻度が多くなった場合でも、部分地図１９０の内容を更新する頻度は増えず、結果として全体地図の読み込みやデータの圧縮展開およびメモリへの書き込みの処理量が多くなってしまう問題を解決することができる。その原理を図１２を用いて説明する。

まず移動ロボット２２０の候補位置６１０が図１２に示した位置のとき、全体地図１８０の中で必要部分地図６２０に拡張領域７６７を加えた部分地図メモリ上に読み込まれる領域は、図１２上図に点線で示した領域７７０のとおりである。移動ロボット２２０の移動にともない、図１２下図に示すように、このロボットの候補位置６１０がブロックをまたいで点６１１に移動したときは、必要部分地図６２０の領域が図１２下図に示すものに変化する。このときには、図１２下図で斜線で示した領域９１２がメモリ上から不要になり、斜線で示した領域９１５が、新たにメモリ上に必要となるので、領域９１５が領域９１２に置き換わるようにメモリの内容が更新されることになる。ここでの処理は、図６、図７で説明した方法とほぼ同等である。

しかしその後に図１３で示すように、移動ロボット２２０の移動にともない、ロボットの候補位置６１１が、ブロックをまたいだ位置６１２に変化したときに、この拡張領域を加えた手法が効果をあらわす。この移動により、必要部分地図６２０の領域は、図１３の上図に示したものから、図１３の下図に示したものに変化する。しかしながら、拡張領域７６７が存在していることにより、変化した後の必要部分地図６２０の領域は、すべて点線部分７７０すなわち部分地図１９０としてメモリ上にすでに書き込まれている。すなわち、部分地図を更新する必要は一切ない、ということになる。これが、体地図の読み込みやデータの圧縮展開およびメモリへの書き込みの処理量が多くなってしまう問題を解決することになっている。この効果は、図１３の数に示した位置６１１を含むブロックと、位置６１２を含むブロックの間を繰り返し移動ロボット２２０が行き来してもあらわれることは明白である。なお以上述べた効果は、図１３で説明した２つのブロック間を上下にまたいだときだけでなく、左右にまたいだときにも全くおなじようにあらわれることも容易に理解される。

次に、上で説明した部分地図１９０を、メモリ上でどのように管理するかについて詳しく述べることする。メモリはそのアドレス番号と、そのアドレスに書き込まれるデータの組み合わせで管理されるものとする。

メモリは、10×10ブロック分のピクセル数、すなわち(40×10)×(40×10) = 4000×4000ピクセル分の記憶容量を持つ連続メモリとして準備する。メモリ番号は、左上を始点とし右にむかって番号が増えていき、右端に到達したら次にその1つ下の行の左端からまた右にむかって番号が増えていき、これを繰り返すことで右下を終点とする画像データの一般的なもち方と同様である。

すなわち、部分地図記憶手段は、メモリ等の主記憶装置によって実現され、このメモリは(M×W)×(N×H)ピクセルあるいは((M+1)×W)×((N+1)×H)ピクセルの画素データを格納できるサイズをもち、部分地図はそのメモリ上において、矩形領域の右端が左端に連続するように、また下端が上端に連続するように割り当てられるようにする。これによって移動ロボットが必要となるブロックを変更する境目付近を移動し、この境目を連続して行き来する場合にも、主記憶の内容の更新は繰り返し行う必要が無くなり、推定に利用する地図準備のための処理量を大幅に削減することができる。

図１４は上で述べた拡張領域を含む手法における部分地図１９０である。図１５は、この部分地図が示す各ブロックのデータが、上で述べたメモリ７７６上のどの位置に書き込まれるかを対応づけて示したものである。その原則は、もとの部分地図１９０の左右上下方向のブロックの並びの連続性が、メモリ７７６上の並びにおいても、右端が左端に連続し、下端が上端に連続するとの条件のもと、保たれるようにすることである。
また移動ロボット２２０の移動にともない、部分地図１９０の内容を図１４に示すものから図１６に示すものに変更する必要が生じた場合は、なくなった行（あるいは列）に、あらたに加わった行（あるいは列）の内容が上書きされるものとする。この様子を示したものが図１７である。

このようなルールにもとづき部分地図１９０の内容をメモリ上に書き込むことで、部分地図１９０の左上のブロックの内容が書き込まれたメモリ番号から、右方向に走査していき、右端に到達したら、左端から再び走査すること、またさらに下方向に走査していき、下端に到達したら、上端から再び走査することで、部分地図１９０の任意の点にアクセスできることになる。

次に、以上述べた方法により部分地図１９０をメモリ上で管理することの効果を説明する。

まず第１に、地図上の位置とメモリ番地を対応づける対応表を準備する必要が無くなるということがあげられる。一般的なバッファメモリの考え方に基づき内容の更新を行うと、同じメモリ番地上にさまざまな位置の地図の内容が書き込まれるため、これをすべて管理しておく必要があるが、本手法によれば上に述べたルールに基づき地図上の位置とメモリ番地が対応付けられるのでその必要が無くなる。これによって、位置姿勢推定時にメモリ上の地図データにアクセスする際に、対応表を参照する処理が必要なくなることを意味し、結果として位置姿勢推定処理の高速化を図ることができる。

第２の効果として、地図上で連続する位置の地図データが、メモリ上でも連続して記録されるということが上げられる。背景技術の項で説明した位置姿勢推定のためのマッチング処理では、移動ロボットが候補位置にいたときにそのセンサデータが示す位置を中心として、その周辺の地図の値を参照する。すなわち、アクセスする地図上のデータは空間上連続していることが多いため、メモリ上でもこれが連続する番地に書き込まれていれば、ランダムアクセスに比べてメモリアクセスのための時間を短縮することが可能となる。

しかしながら一方、上に述べたようなメモリ配列によると、右端に到達したときに左端に戻るための、右端に到達したかどうかを確認する処理、ならびに、下端に到達したときに上端に戻るための、下端に到達したかどうかを確認する処理が、地図上の１点にアクセスするたびに必要となる。これを説明するのが図１８である。図の１５００の領域にアクセスする場合はすべてを連続しているメモリとして扱うことができるが、図の１６００の領域にアクセスする場合は、全体領域の右端を超えるかどうかをその都度確認し、そのときには左端の領域１６３０をアクセスする必要が生じている。これは具体的に言うと、プログラムにおいて、メモリ番地が図の右端を越えるかどうかを確認するためのif文に相当する確認分岐処理が必要となる。一般にこの処理は単体としては無視できるほど極めて短時間におこなわれるものであるが、背景技術の項で説明した位置姿勢推定のためのマッチング処理では、評価関数を算出するために最も多く繰り返し行われ、なおかつここで同様に最も多く繰り返し行われるその他の処理はもともと非常に単純なものであるため、この確認分岐処理を行うか行わないかが全体処理時間に与える影響は結果として無視できない大きなものになる。例えば、マッチング処理で最も多く行われる処理ループの最内部分の演算がもともと3の処理量であり、この確認分岐の処理量が単体として1の処理量であったときに、全体の処理量は理論上は4/3程度に増加されることになる。

この問題を解決するため、上で述べた部分地図１９０のメモリ上での管理方法を、以下のように改良することもできる。これを部分地図のメモリ管理に関する改良方法と呼ぶことにし、これについて詳しく述べる。

部分地図のメモリ管理に関する改良方法では、図１９に示すように、メモリは、11×11ブロック分のピクセル数、すなわち(40×11)×(40×11) = 4400×4400ピクセル分の記憶容量を持つ連続メモリとして準備する。メモリ番号のつけかたは、図１５を用いて述べた方法と同じである。この改良方法では、メモリ領域の最上１列および最左１列に新たな領域１３００を設けている。ここで最上１列には、メモリ領域の最下１列の内容と同等の内容が書き込まれ、最左１列には、メモリ領域の最右１列同等の内容が書き込まれるようにする。このような領域を設け、図２０に示すようにエリア１６００およびエリア１６２０の内容を参照するかわりにエリア１６７０を参照することで、同じ内容を連続するメモリ領域として参照することが可能となる。これによりマッチング処理を行う際に、ある限定された領域の連続メモリの参照時に、メモリの端に到達したかどうかの確認処理を行う必要が無くなり、マッチング処理で最も多く行われる処理ループの最内部分の演算量を削減するができ、結果としてマッチング全体処理量を削減することができる。例えば上で述べた理論上の値によれば、処理量を3/4に削減することが可能となる。

なお、上の説明では、新たな領域１４３０の幅が地図のブロックのサイズの幅と等しいものとしたが、この領域の幅はマッチング時にアクセスする領域の幅と等しいように決定するのが合理的でありこのように設定することもできる。

また、以上の説明では移動ロボット２２０の回転方向の制約は考慮しなかったが、実際には上で述べた１周期の間に回転できる角度は限定されるので、この角度に含まれない部分は必要部分地図６２０の領域外とすることで、部分地図１９０の記憶に必要なメモリの大きさを削減したり、その更新に必要となるデータの読み書きの量を削減することもできる。

図２１は、いままで説明した本発明の機能を実現するハードウェア構成の１例を示すハードウェア構成図である。ＣＰＵ２０、メモリ３０、ハードディスク４０、距離センサ用インタフェース（ＩＦ）５０、移動機構用インタフェース（ＩＦ）６０、ネットワークインタフェース（ＩＦ）７０からなるコンピュータ９５、および距離センサ８０、移動機構９０を備える。

ＣＰＵ２０は、ハードディスク４０あるいはメモリ３０に記憶されたプログラムに基づいて動作し、各部の制御を行う。ハードディスク４０は、ＣＰＵ２０によって実行されるプログラムを記憶し、また全体地図を記憶する。メモリ３０は、ＣＰＵ２０によって実行されるプログラムを記憶し、また部分地図を格納する。距離センサ用ＩＦ５０は、ＣＰＵ２０による制御に基づき距離センサ２１０を動作させ、センシングデータを入力する。移動機構用ＩＦ６０は、ＣＰＵ２０による制御に基づき移動機構９０を動作させる。ネットワークＩＦ７０は無線あるいは有線の通信回線とのインタフェースを行い、プログラムの入出力、データの入出力、外部コマンドの入出力などを行う。

これらの機器と図１の各ブロックとの対応を説明する。全体地図記憶部１６０を実現すものがハードディスク４０である。なおこのハードディスクは、コンパクトフラッシュ（登録商標）のようなメモリ型外部記憶媒体や、ネットワークを介して接続される外部のコンピュータやサーバシステムなどに置き換えることも可能である。部分地図更新部１５０を実現するものはＣＰＵ２０で動作するプログラムである。部分地図記憶部１７０を実現するものはメモリ３０である。位置姿勢推定部１１０を実現するものはＣＰＵ２０で動作するプログラムと距離センサ用ＩＦ５０である。移動制御部１２０を実現するものはＣＰＵ２０で動作するプログラムと移動機構用ＩＦ６０である。移動機構部１３０は移動機構９０によって実現される。

本発明の一実施形態に係る移動ロボットシステムの全体構成を示す機能ブロック構成図である。本発明の一実施形態に係る移動ロボットシステムにおけるセンシング方法を説明する図である。距離センサで計測したセンシングデータの一例を説明する図である。位置姿勢推定に用いる地図の一例である。地図とセンシングデータのマッチングにより位置姿勢を推定するしくみを説明する図である。全体地図と移動ロボットの候補位置ならびに必要部分地図の関係を説明する図である。部分地図と移動ロボットの候補位置との関係を説明する図である。部分地図更新部によって行われる処理の流れを説明するフローチャートである。移動ロボットの候補位置がブロックをまたがって移動したときに、必要部分地図の領域が変化する様子を説明する図である。拡張領域を含む手法において、部分地図の領域を説明する図である。拡張領域を含む手法において、部分地図と移動ロボットの候補位置との関係を説明する図である。拡張領域を含む手法において、移動ロボットの候補位置がブロックをまたがって移動したときに、部分地図の領域が変化する様子を説明する図である。拡張領域を含む手法において、移動ロボットの候補位置がブロックをまたがって移動したときに、部分地図の内容を更新する必要がないことを説明する図である。部分地図のブロックの並びを説明する図である。図１４の部分地図のブロックとメモリ上の位置との関係を説明する図である。部分地図の領域が変化したときに、部分地図のブロックの並びを説明する図である。図１６の部分地図のブロックとメモリ上の位置との関係を説明する図である。マッチング処理時に２つのセンサデータの値に基づいてそれぞれがアクセスするメモリの領域を説明する図である。部分地図のメモリ管理に関する改良方法における、メモリ上のブロックの並びかたを説明する図である。部分地図のメモリ管理に関する改良方法における、マッチング処理時にあるセンサデータの値に基づいてアクセスするメモリの領域を説明する図である。本発明の一実施の形態におけるハードウェア構成を説明する図である。

符号の説明

１８１読み込みブロック選択指示部によって選択された圧縮ブロック地図、１８６部分地図記憶部の更新用ブロック地図

Claims

周囲の状況を距離センサで計測したセンシングデータと、地図との照合により、自己の位置姿勢を認識し、その認識結果に基づく移動制御によって移動する、移動ロボットシステムにおいて、
該移動ロボットシステムは、
全体地図を、複数に分割したブロック地図として管理する全体地図記憶部と、
位置姿勢推定に必要な必要部分地図とその周辺部分の地図からなる拡張部分地図とから構成される部分地図を、連続する領域で管理する部分地図記憶部と、
該部分地図と該センシングデータから該位置姿勢を求める位置姿勢推定部と、
該全体地図管理手段から該ブロック地図を選択して読み込み、該読み込んだブロック地図を用いて該部分地図記憶部の内容を更新する部分地図更新部とを備えることを特徴とする移動ロボットシステム。
請求項１記載の移動ロボットシステムにおいて、
該部分地図として管理すべき領域を、ロボットの位置姿勢推定初期値に基づき決定することを特徴とする移動ロボットシステム。
請求項１または２記載の移動ロボットシステムにおいて、
該全体地図記憶部は該ブロック地図を圧縮データとして記憶し、
該部分地図更新部は読み込んだ圧縮データを伸長する手段を備えることを特徴とする移動ロボットシステム。
請求項１から３記載のいずれかの移動ロボットシステムにおいて、
該全体地図は、W×Hピクセルの矩形領域となる複数のブロック領域から構成され、
該必要部分地図は、M×N個の該ブロック領域から構成される矩形領域であり、
該必要部分地図と該拡張部分地図をあわせた該部分地図は、(M+1)×(N+1)個の該ブロック領域から構成される矩形領域であり、
ロボットが移動しロボットの位置姿勢推定初期値が変化したときは、該部分地図に必要部分地図が必ず含まれるようにするために足りないブロック領域を該全体地図から読み込み、該部分地図を更新することを特徴とする移動ロボットシステム。
請求項１から３記載のいずれかの移動ロボットシステムにおいて、
該部分地図記憶領域は連続する物理メモリから構成され、
該物理メモリは、該全体地図を構成するW×Hピクセルの矩形領域と、該必要部分地図を構成するM×N個のブロック領域を示す、(M×W)×(N×H)ピクセルの画素データを格納できるサイズをもち、
該部分地図記憶領域は、該物理メモリ上において、矩形領域の右端が左端に連続するように、また下端が上端に連続するように割り当てられることを特徴とする移動ロボットシステム。
請求項４記載の移動ロボットシステムにおいて、
該部分地図記憶領域は連続する物理メモリから構成され、
該物理メモリは、該全体地図を構成するW×Hピクセルの矩形領域と、該必要部分地図を構成するM×N個のブロック領域を示す、((M+1)×W)×((N+1)×H)ピクセルの画素データを格納できるサイズをもち、
該部分地図記憶領域は、該物理メモリ上において、矩形領域の右端が左端に連続するように、また下端が上端に連続するように割り当てられることを特徴とする移動ロボットシステム。
請求項１から３記載のいずれかの移動ロボットシステムにおいて、
該部分地図記憶領域は連続する物理メモリから構成され、
該物理メモリは、あらかじめ定められた幅パラメータUを用いて、該全体地図を構成するW×Hピクセルの矩形領域と、該必要部分地図を構成するM×N個のブロック領域を示す ((U+M)×W)×((U+N)×H)ピクセルの画素データを格納できるサイズをもち、
該部分地図記憶領域は、該物理メモリ上において、矩形領域の右端が左端に連続するように、また下端が上端に連続するように割り当てられ、
該物理メモリの左端の幅Uの部分には右端の幅Uの内容が複写され、また上端の幅Uの部分には下端の幅Uの内容が複写されることを特徴とする移動ロボットシステム。
請求項４記載の移動ロボットシステムにおいて、
該部分地図記憶領域は連続する物理メモリから構成され、
該物理メモリは、あらかじめ定められた幅パラメータUを用いて、該全体地図を構成するW×Hピクセルの矩形領域と、該必要部分地図を構成するM×N個のブロック領域を示す、(U+(M+1)×W)×(U+(N+1)×H)ピクセルの画素データを格納できるサイズをもち、
該部分地図記憶領域は、該物理メモリ上において、矩形領域の右端が左端に連続するように、また下端が上端に連続するように割り当てられ、
該物理メモリの左端の幅Uの部分には右端の幅Uの内容が複写され、また上端の幅Uの部分には下端の幅Uの内容が複写されることを特徴とする移動ロボットシステム。
移動ロボットシステムの周囲の状況を計測し、センシングデータを生成する距離センサと、複数のブロック地図を記憶する全体地図記憶部と、
前記全体地図記憶部に記憶された複数のブロック地図のうちいくつかを伸張した部分地図を管理する部分地図管理部と、
前記部分地図管理部が管理する部分地図に含まれる前記ブロック地図を全体地図記憶部が記憶する前記ブロック地図から選択するとともに、該選択したブロック地図を伸張して前記部分地図管理部の管理する部分地図を更新する部分地図更新部と、
前記センシングデータと前記部分地図とを照合して前記移動ロボットシステムの位置姿勢を求める位置姿勢推定部と、
求めた前記位置姿勢に応じて、前記移動ロボットシステムを移動させる移動機構部を制御する移動制御部とを備え、
該部分地図は、位置姿勢推定に必要な必要部分地図と、その周辺部分の地図からなる拡張部分地図から構成され、
前記部分地図更新部は、前記位置姿勢に基づいて必要部分地図の範囲を算出し、当該必要部分地図の全体を含むように前記部分地図管理部が管理する部分地図が有するブロック地図を選択することを特徴とする移動ロボットシステム。
請求項９に記載の移動ロボットシステムにおいて、
前記全体地図記憶部は、前記ブロック地図を圧縮して記憶していることを特徴とする移動ロボットシステム。
請求項９に記載の移動ロボットシステムにおいて、
前記必要部分地図は、前記位置姿勢から求めた前記移動ロボットシステムの位置に対応するブロック地図が中心となるように選択させることを特徴とする移動ロボットシステム。
請求項９に記載の移動ロボットシステムにおいて、
前記部分地図は、前記必要部分地図よりも多くの前記ブロック地図を有し、
前記必要部分地図の範囲は、前記位置姿勢から求めた前記移動ロボットシステムの位置の変化に応じて変更され、
当該必要部分地図の範囲が前記部分地図に含まれないブロック地図を含むように変更されたときに、前記部分地図更新部は前記全体記憶地図記憶部から当該ブロック地図を伸張して前記部分地図管理部の管理する部分地図に加えることを特徴とする移動ロボットシステム。
請求項１２に記載の移動ロボットシステムにおいて、
前記ブロック地図を前記部分地図に加えたときに、前記必要部分地図に含まれないブロック地図を前記部分地図管理部から削除することを特徴とする移動ロボットシステム。
請求項１３に記載の移動ロボットシステムにおいて、
前記部分地図に加えるブロック地図は、前記削除されるブロック地図を記憶している前記部分地図管理部の記憶領域に記憶されることを特徴とする移動ロボットシステム。
周囲の状況を距離センサで計測したセンシングデータと、地図との照合により、自己の位置姿勢を認識し、その認識結果に基づく移動制御によって移動する、移動ロボットシステムの制御方法において、
前記移動ロボットシステムは、
全体地図を、複数に分割したブロック地図として管理する全体地図記憶部と、
位置姿勢推定に必要な地図領域を、部分地図として連続する領域で管理する部分地図記憶部とを備え、
該部分地図は、位置姿勢推定に必要な必要部分地図と、その周辺部分の地図からなる拡張部分地図から構成され、
移動ロボットシステムの位置姿勢推定部が、該部分地図と該センシングデータから該位置姿勢を求める位置姿勢推定ステップと、
移動ロボットシステムの部分地図更新部が、該全体地図管理手段から該ブロック地図を選択して読み込み、該読み込んだブロック地図を用いて該部分地図記憶部の内容を更新するステップとを備えることを特徴とする移動ロボットシステムの制御方法。
距離センサが、移動ロボットシステムの周囲の状況を計測し、センシングデータを生成する距離計測ステップと、
部分地図更新部が、複数のブロック地図を記憶する全体地図記憶部から前記部分地図管理部が管理する部分地図に含まれる前記ブロック地図から選択するブロック地図選択ステップと、
前記部分地図更新部が、前記選択したブロック地図を伸張して前記部分地図管理部の管理する部分地図を更新する部分地図更新ステップと、
位置姿勢推定部が、前記センシングデータと前記部分地図とを照合して前記移動ロボットシステムの位置姿勢を求める位置姿勢推定ステップと、
移動制御部が、前記求めた位置姿勢に応じて、前記移動ロボットシステムを移動させる移動機構部を制御する移動ステップとを備え、
該部分地図は、位置姿勢推定に必要な必要部分地図と、その周辺部分の地図からなる拡張部分地図から構成され、
前記地図選択ステップでは、前記部分地図更新部が、前記位置姿勢に基づいて必要部分地図の範囲を算出し、当該必要部分地図の全体を含むように前記部分地図管理部が管理する部分地図が有するブロック地図を選択することを特徴とする移動ロボットシステムの制御方法。