位置姿勢推定システム、位置姿勢推定装置、および位置姿勢推定方法
【課題】センシングデータリストに基づくマッチング方法において、さらなる計算量の削減を実現すること位置姿勢推定システム１０を提供する。
【解決手段】本発明の位置姿勢推定システム１０は、参照データ群を作成する手段１５０において、第１の参照データ群１０３を環境地図１０１から作成する処理１０２と、第２の参照データ群１０５を第１の参照データ群１０３の差分処理により作成する処理１０４とを行い、位置姿勢を推定する手段１７０において、センシングデータ１０７に基づいて、第１の参照データ群１０３および第２の参照データ群１０５の中から必要なデータを選択する処理１２０と、選択された複数の参照データと前回作成した評価のためのデータ１２２とを加算することで新たな評価のためのデータ１２２を作成する処理１２１と、評価のためのデータ１２２から位置姿勢推定値１２４を求める処理１２３とを行う。
【解決手段】本発明の位置姿勢推定システム１０は、参照データ群を作成する手段１５０において、第１の参照データ群１０３を環境地図１０１から作成する処理１０２と、第２の参照データ群１０５を第１の参照データ群１０３の差分処理により作成する処理１０４とを行い、位置姿勢を推定する手段１７０において、センシングデータ１０７に基づいて、第１の参照データ群１０３および第２の参照データ群１０５の中から必要なデータを選択する処理１２０と、選択された複数の参照データと前回作成した評価のためのデータ１２２とを加算することで新たな評価のためのデータ１２２を作成する処理１２１と、評価のためのデータ１２２から位置姿勢推定値１２４を求める処理１２３とを行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動ロボットの自律移動制御等において利用される、実環境内における自己の位置姿勢を認識する技術に関する。特に、移動ロボットが目的地まで人の操作なしに自動的に到達することを目的とし、移動ロボットの位置姿勢を、移動ロボットに搭載された距離センサからのセンシングデータと、環境の幾何状況を示した二次元の環境地図とを用いて算出する技術に関する。
【背景技術】
【０００２】
まず、距離センサと二次元の環境地図を用いて移動ロボットの位置姿勢を推定する従来の方法について説明する。
【０００３】
ここで距離センサとは、距離センサの周辺の複数の方向について、障害物までの距離を計測できるものである。仕様としてさまざまなものが考えられるが、ここでは図１３および図１４に示すように、ある高さの水平面上を距離センサ４２１の正面に対して-90°から+90°の方向に、１°毎の合計１８１通りの異なる方向について、障害物４２２までの距離データが瞬時に計測できるものを想定する。よって各センシングデータ４３１は、角度φと距離dの組み合わせとして(d0,φ0),(d1,φ1),...,(d180,φ180)としてあらわされる。
【０００４】
ここで各変数の数字の添え字はデータ番号で、φiはi番目の測定方向を示す。測定方向とは、反時計回りを正方向とし、距離センサの主方向となす角度であり、主方向とのなす角度が小さい順にデータ番号が割り当てられる。すなわちφ0=-90°,φ1=-89°,...,φ180=+90.0°を示す。また、diは、それぞれの方向における障害物までの距離を示す。
【０００５】
なお、ここでは距離センサの測定能力を考慮し、測定可能距離を0.0mより大きく4.0m以下と想定し、diの値の範囲を、0.0m＜di≦4.0mに制限することとする。また、この１８１個の距離データを１まとまりとして、１フレーム分のセンシングデータ群４３０と呼ぶこととする。また、(di,φi)であらわされたパラメータは、センシングデータの位置、すなわち障害物の位置を、距離センサの原点位置を原点とした極座標で表現したものと等価になるが、これをxi=di・cos(φi)、yi=di・sin(φi)とすることで、(xi,yi)の形の直交座標表現に変換することもできる。
【０００６】
一方、二次元の環境地図とは、距離センサ４２１による測定高さ断面での障害物４２２の存在を、図１５に示すようなディジタル画像としてあらわしたものである。ここでは図１５に示すように、障害物が存在しない場所を白を示す画素値４４１、障害物が存在する場所を黒を示す画素値４４２としてあらわすことにする。
【０００７】
移動ロボット４２０は、図１３に示すように平らな地面上を移動するものと仮定する。よってその移動によって変化する位置姿勢に関する自由度は、位置について(x,y)の２パラメータ、姿勢については移動ロボットが向く方向角度に関するθの１パラメータの、合計３パラメータである。なお、これらのパラメータをあらわす座標系は、図１５に示すような環境地図があらわわれる座標系と一致させるものとする。
【０００８】
移動ロボットの位置姿勢を推定することとは、任意の位置姿勢にある移動ロボットのセンシングデータから、上記(x,y,θ)の３パラメータを求めることを意味する。よって、本発明が対象とする機能は、任意の位置姿勢にある移動ロボットの位置姿勢パラメータ(x,y,θ)を、移動ロボットに搭載された距離センサによるセンシングデータと、あらかじめ用意された環境地図を用いて求めることとなる。
【０００９】
本機能を実現する手段として、いくつかの方法が考えられるが、ここではその最も基本的な方法となるマッチング方法を説明する。
【００１０】
マッチング方法とは、あるフレームにおいて計測されたセンシングデータ全体の位置および姿勢（方向）を変えながら、当該センシングデータを環境地図に重ね合わせ、センシングデータと環境地図とが最もマッチする位置および姿勢を探し出し、これをもって移動ロボットの位置姿勢パラメータの推定値とするというものである。その原理は、移動ロボットがその位置および姿勢にあるときに、計測されたデータと同じデータが最も高い確率で得られるであろうという考え方に基づいている。
【００１１】
このようなマッチング方法は、具体的には以下に述べるような処理によって実施される。
(1) あるフレームにおけるすべてのセンシングデータ（前述した距離センサの例では１８１個）に対して、これを地図上でθだけ回転させ、(x,y)だけ平行移動させた位置を求める。
(2) それらの位置の環境地図上の画素値を参照し、１の値をもつもの（障害物があるもの）の数を数え、これをそのパラメータ (x,y,θ)における評価値とする。
(3) 上記(1)および(2)の処理を、考えられうるすべての値をもつ(x,y,θ)の組み合わせについて、評価値が最も大きい(x,y,θ)を解とする。
【００１２】
上記の手法を、基本手法によるマッチング方法と呼ぶことにする。図１６に基本手法によるマッチング方法の概要を示す。図１５に示した環境地図４４０と、図１６（ａ）に示したセンシングデータ群４３０とが与えられたときに、この方法によって最もマッチすると判断された状況が図１６（ｂ）の位置４５０および方向４５１となる。このときの平行移動成分(x,y)と、回転成分θが、移動ロボット位置姿勢パラメータの推定値となる。
【００１３】
さて、基本手法によるマッチング方法は、処理量が膨大になるという問題がある。具体的には、環境地図のサイズ、すなわち対象とする部屋等移動ロボットが移動可能なエリアのサイズを例えば10m×10mとし、マッチングの粒度、すなわちどの程度の細かさでマッチングの際のパラメータを変化させていくかについて、その最小単位を、位置すなわち(x,y)パラメータに関して例えば10cm、姿勢すなわちθパラメータに関して例えば１°とすると、上述した基本手法によるマッチング方法では、181×360×101×101 = 664,697,160回の画素値の参照演算が必要となる。
【００１４】
ここで、「181」はデータ数、「360」は回転角度θがとりうる値の数（0°,1°,2°,...,359°の３６０通り）、「101」はx方向の移動量がとりうる値の数（-5.0m,-4.9m,...,+5.0mの１０１通り、次の「101」は同じようにy方向の移動量がとりうる値の数（-5.0m,-4.9m,...,+5.0mの１０１通り）である。
【００１５】
マッチング方法は、一般にコンピュータ等の計算機で行うことが想定される。上述した基本手法によるマッチング方法で行われる画素値の参照演算は、１つ１つはそれほど大きな計算負荷にならないが、その量が膨大であれば、その量にほぼ比例して計算負荷が増大することになる。上述した参照演算回数の具体例は、一般的に流通している現在の計算機の能力を考慮すると、一般的に、実用上また実装上無視できないオーダの計算量となっている。
【００１６】
このような、処理量が膨大になるという基本手法によるマッチング方法の問題点を解決する方法として、例えば非特許文献１には、センシングデータリストセンシングデータリストに基づくマッチング方法が提案されている。この考えに基づく処理について説明する。なお以降本処理を、センシングデータリストに基づくマッチング方法と呼ぶことにする。
【００１７】
ここで説明を簡単にするため、移動ロボットの移動は並進運動だけで、回転運動は行われないものと仮定する。すなわち、推定するパラメータは(x,y)の２つで、θについてはここでは０に固定するものとする。
【００１８】
まず、上述した基本手法によるマッチング方法を考える。ここでは、図１７に示すようなセンシングデータ群４６０と環境地図４６３とが与えられることを想定する。なお以降用いる図では説明を簡単にするため、画素の詳細度を荒くし、センシングデータの数を少なくして解説するものとする。ここで、図１７において、４６１はセンサ原点、４６２はセンシングデータであり、図１７に示した(0)から(3)の数字の順に、第０番目のセンシングデータ、第１番目のセンシングデータ、第２番目のセンシングデータ、第３番目のセンシングデータと呼ぶこととする。
【００１９】
基本手法によるマッチング方法では、図１８に示すように、センシングデータ全体を平行移動させながら環境地図に重ね合わせ、各センシングデータが指し示す、すなわちセンシングデータが重なる画素の値が１であるものの数を、評価値として数えていくことになる。例えば、図１８の(x,y)=(-3,-3)では評価値が０、(x,y)=(-1,-3)では評価値が１である。(x,y)=(-1,-1)では評価値が４となり、評価値として最大値となる。この場合、推定値は、(x,y)=(-1,-1)となる。
【００２０】
次に、センシングデータリストに基づくマッチング方法を説明する。
【００２１】
いま、図１７のセンシングデータ群４６０における第０番目のセンシングデータに着目すると共に、この第０番目のセンシングデータが指し示す環境地図上の画素値が１になるのは、センシングデータ群全体をどこに移動させた場合であるかを考える。図１８からそのような移動は、図１９に示した１０通りであることがわかる。なお、図１９に示す４７０が、第０番目のセンシングデータである。本例では、環境地図上に画素値が１となる点が１０個あるので、移動方法も１０通りあることになる。
【００２２】
次に、このときのセンシングデータの原点位置４７１を、白紙の地図上にプロットすると、その結果は図２０に示すようになる。このように作成されたパターンを参照地図、より詳しくは、第０番目のセンシングデータに関する参照地図４８０と呼ぶこととする。
【００２３】
ところで、この第０番目のセンシングデータに関する参照地図４８０をよく見ると、色のついた画素パターンは、図１７（ｂ）に示した環境地図の画素パターンを図２１（ｄ）に示すベクトル成分４９５だけ平行移動させたものであることがわかる。また、このベクトル成分４９５は、センシングデータ群における第０番目のセンシングデータ４９４とセンサ原点４９３から決まるベクトルの逆方向であることがわかる。
【００２４】
よって、第０番目のセンシングデータを(d0,φ0)とすると、x0=d0・cos(φ0)、y0=d0・sin(φ0)の計算を行うことで、上記ベクトル成分を(x0,y0)の形の直交座標系で表現することができる。これを算出した後に、もとの環境地図をこのベクトル分だけ逆方向に平行移動させてやれば、この第０番目のセンシングデータに関する参照地図４８０が作成されることになる。
【００２５】
このようにして作成された参照地図４８０（第０番目のデータに関する参照地図）における、センサ原点４８１がプロットされた画素の意味は次のようになる。それは、「この位置に原点位置がくるようにセンシングデータを平行移動させてやれば、第０番目のセンシングデータが、環境地図データの１の値を持つ画素に一致する」ということである。
【００２６】
これは、第０番目のセンシングデータについて考えたときであるが、これを別のセンシングデータについても同様に考えることができる。すなわち、第ｎ番目のセンシングデータについて、それとセンサ原点とがなすベクトルを考え、そのベクトル成分だけ環境地図を反対方向に平行移動させることにより、第ｎ番目のセンシングデータに関する参照地図を作成することができる。
【００２７】
具体的には、第０番目のセンシングデータのときと同様に第ｎ番目のセンシングデータが(dn,φn)と得られたときに、まずこの直交座標表現の値(xn,yn)をxn=dn・cos(φn)、yn=dn・sin(φn)の計算を行うことにより求める。次に、この値(xn,yn)の分だけ地図画像を平行移動させる。このようにして作成される第ｎ番目のセンシングデータに関する参照地図は、この中の斜線のついた画素にセンシングデータの原点を平行移動させれば、第ｎ番目のセンシングデータが、環境地図の１の値を持つ画素に一致することを意味することになる。
【００２８】
上の例ではセンシングデータが４つあるので、それぞれについて参照地図、すなわち図２２に示すように、第０番目のセンシングデータに関する参照地図５００、第１番目のセンシングデータに関する参照地図５０１、第２番目のセンシングデータに関する参照地図５０２、第３番目のセンシングデータに関する参照地図５０３の合計４つを準備する。
【００２９】
次に、図２３に示すように、この４つの参照地図を重ね合わせ、それぞれの画素についていくつ色のついた画素が重なるかを足し合わせて数えることを考える。それぞれの参照地図の意味が上に述べたとおりなので、この数字は、それぞれの画素の位置にセンシングデータの原点が移動したときに、４つのセンシングデータの中で、環境地図の１の値を持つ画素と一致するものの数をあらわすことになる。
【００３０】
この足し合わせた値を画素値とした画像データは、例えば図２４に示すようになり、これを評価画像５１０と呼ぶことにする。すなわちこの評価画像５１０における(x,y)の位置の画素値が、上で述べた基本手法によるマッチング方法において、センサを(x,y)だけ移動させたときに算出される評価値をあらわすことになる。よって基本手法によるマッチング方法と同様に、この評価画像５１０の中で最も大きな画素値をもつものを見つけ、その点の画像上での座標位置を調べてやれば、これが求めるべき移動ロボット位置の推定値となることになる。
【００３１】
なお、ここで各参照地図を重ね合わせる処理は、各画像の画素値を調べていくのではなく、予め各参照地図の中で、１の値を持つ画素の位置を (x0,y0),(x1,y1), ... ,(xn,yn)の形で記録しておき、この記録された座標値が示す評価画像の画素の値を、１つずつ増やしていく、すなわちその画素に対して投票していく、という方法で行うものとする。これは後に述べる、参照地図の各画素に対する参照回数を少なくする、すなわち参照地図の中で０の値を持つ画素については参照する必要がなくなる、という効果をもたらすことになる。
【００３２】
この原理に基づき、センシングデータリストに基づくマッチング方法は、以下の手順で処理が行われる。
(1) 考えうるすべてのセンシングデータ位置について、それぞれ参照地図を作成しておく。これはすなわち上の説明で述べたとおり、考えうるすべてのセンシングデータ位置とセンサ原点がなすベクトル毎に、参照地図を作成することになる。なお、参照地図は、実際には画像データの形で記録するのではなく、上に述べたとおり、１の値を持つ画素の位置を順に(x0,y0),(x1,y1), ... ,(xn,yn)のように並べたものとして記録するものとする。
(2) １フレーム分のセンシングデータ群が与えられたときに、各センシングデータ（合計１８１個）それぞれの位置と、センサ原点がなすベクトルを算出し、そのベクトルに関する参照地図を選んでくる（合計１８１枚）。
(3) 選ばれたすべての参照地図を重ね合わせ、画素毎に画素値を足し合わせた画像を、評価画像として作成する。これは、それぞれの画素について色のついているものの数を求めることになる。
(4) 作成された評価画像の中で、最大の画素値をもつ画素の位置(x,y)を探し、このときの(x,y)をマッチングの解とする。
【００３３】
ここで、上記の説明で述べた、考えうるすべてのセンシングデータという部分を補足する。冒頭でも述べたように、距離センサによるセンシングデータは、(d0,φ0),(d1,φ1), ... ,(d180,φ180)の形であらわされる。ここで、角度φiについては、センシング方向が１°ずつに固定されているので、φ0=-90°,φ1=-89°, ... ,φ180=+90°の決まった値になる。
【００３４】
しかし、距離diについては、測定範囲内(0.0m＜di≦4.0m)においてさまざまな値をとりうるので、これは無限に考えられることになる。そこでこれを、予め定められた離散的な値に代表させることを考える。すなわち、代表値として例えば0.1m,0.2m, ... ,4.0mの４０通りを考え、di≦0.15mであれば、di=0.1m、0.15m＜di≦0.25mであれば、di=0.2mのような規則で値を割り当てていく。このようにすれば、センシングデータ(di,φi)のとりうる値の組み合わせは、diについて４０通り、φiについて１８０通りあることになるので、考えうるすべてのセンシングデータは、合計４０×１８０通りの有限個になる。
【００３５】
これは、いわゆるディジタル処理における量子化の操作を行ったことにほかならないが、このような10cm程度の単位で行う量子化に伴う精度の劣化は、例えば位置姿勢推定における初期値の算出といった用途においてはほとんど問題がない。なお、センシングデータの量子化と同様に、参照地図についても、以下の説明では同様に10cm単位での量子化を行うことにする。すなわち画像上の１ピクセルが、実環境における10cm×10cmの領域を示すことになる。
【００３６】
さて、以上の説明においては移動ロボットの移動が平行移動のみ行われることを仮定し、マッチングについては(x,y)の２パラメータのみで行われることとしたが、実際にはこれに加えて回転（姿勢）θに関するパラメータも求める必要がある。これについては、予め考えうるすべての回転θ、すなわちこれについても上に述べた量子化の考え方を導入し離散値としてθ=0°, 1°, 2°, ..., 359°の３６０通りの値を持つものとすると、これらの角度それぞれについて、予めセンシングデータ群全体を回転させ、これをもとに上述の評価画像をそれぞれ作成し（合計３６０枚）、この全ての評価画像の中で、最も値の大きな画素を見つけ、その画像が属するθ値およびその画素の位置(x,y)をもって求める解(x,y,θ)とすることになる。
【００３７】
以上の方法では、181×360×101×101×1/2=約5億回以下の画素値の参照演算が行われる。ここで、「181」、「360」、「101」、「101」の各数字の持つ意味は、基本手法によるマッチング方法のときと同様である。ここで新たに加わる「1/2」は、環境地図の画像内において全体の画素数に対する障害物が存在する画素数のおおよその割合である。すなわち、センシングデータリストに基づくマッチング方法は、基本手法によるマッチング方法に比べて、この数字の分、言い換えると環境地図画像中の障害物が存在しない画素の割合分、参照演算回数が削減されることになる。これがセンシングデータリストによる効果である。
【００３８】
ただし、一方で、予め参照地図を作成しておき、これを１の値を持つ画素の位置情報として(x0,y0),(x1,y1), ..., (xn,yn)のように記録しておかなければならないので、(1)参照地図を作る処理が必要になる、(2)メモリ使用量が膨大になる、という２つの問題が新たに発生する。(1)については、移動ロボットの駆動前に参照地図を予め作成しておけばよいので、実用上の問題は少ないものと考えられる。(2)については、おおよその見積もりで、上の例では必要なメモリ量は数ギガバイト程度と見込まれ、現在の主記憶あるいは補助記憶装置の性能から、実現可能なオーダとなっている。
【００３９】
【非特許文献１】L. M. Paz, P. Pinies, J. Neira, J. D. Tardos, "Global Localization in SLAM in Bilinear Time," In Proc. International Conference on Intelligent Robots and Systems, 2005, pp. 1445-1450.
【発明の開示】
【発明が解決しようとする課題】
【００４０】
距離センサによって測定されたセンシングデータと環境地図とを用いて移動ロボットの位置姿勢を推定する方法に関し、上述した基本手法によるマッチング方法では、処理量が膨大になり計算時間が長くなる問題がある。また、センシングデータリストに基づくマッチング方法では、これに比べ処理量がある程度削減されるものの、移動ロボットの軽量化・低コスト化等の観点から、さらなる計算処理の削減が望まれる。
【００４１】
本発明は上記事情を鑑みてなされたものであり、本発明の目的は、センシングデータリストに基づくマッチング方法において、さらなる計算量の削減を実現することにある。
【課題を解決するための手段】
【００４２】
上記課題を解決するために本発明では、例えば図１に示すような構成により、以下の特徴を有する位置姿勢推定方法を提供する。
【００４３】
すなわち、予め準備された環境地図１０１と、距離センサ１０６によって測定されたセンシングデータ１０７とに基づいて、マッチングにより距離センサ１０６の位置姿勢値１２４を推定する位置姿勢推定方法において、当該位置推定方法は、環境地図１０１からマッチングのための第１の参照データ群１０３および第２の参照データ群１０５を作成する手段１５０と、測定されたセンシングデータ１０７、第１の参照データ群１０３、および第２の参照データ群１０５に基づいて位置姿勢値１２４を推定する手段１７０とを有する位置姿勢推定システムにおいて、参照データ群を作成する手段１５０では、第１の参照データ群１０３を環境地図１０１から作成する処理１０２と、第２の参照データ群１０５を第１の参照データ群１０３の差分処理により作成する処理１０４とを行い、位置姿勢を推定する手段１７０では、センシングデータ１０７に基づいて、第１の参照データ群１０３および第２の参照データ群１０５の中から必要なデータを選択する処理１２０と、選択された複数の参照データと前回作成した評価のためのデータ１２２とを加算することで新たな評価のためのデータ１２２を作成する処理１２１と、評価のためのデータ１２２から位置姿勢推定値１２４を求める処理１２３とを行う。
【発明の効果】
【００４４】
本発明によれば、センシングデータリストに基づくマッチング方法に比べて、さらなる計算量の削減を実現することができる。
【発明を実施するための最良の形態】
【００４５】
以下、本発明の実施の形態について説明する。
【００４６】
図２は、本発明の一実施形態に係る位置姿勢推定システム１０の構成を示すシステム構成図である。位置姿勢推定システム１０は、参照データ作成サーバ２０および移動ロボット３０を備える。参照データ作成サーバ２０は、ＬＡＮ等の通信回線１１に接続されている。移動ロボット３０は、通信回線１１に接続された基地局１２と、無線ＬＡＮ等の通信方式により無線通信を行い、当該基地局１２を介して参照データ作成サーバ２０と通信する。
【００４７】
参照データ作成サーバ２０は、二次元の環境地図から参照地図および後述する差分画像を作成し、作成した参照地図および差分画像を、通信回線１１を介して移動ロボット３０へ送る。
【００４８】
移動ロボット３０は、無線通信装置３１、位置姿勢推定装置３２、距離センサ３３、および移動装置３４を有する。無線通信装置３１は、基地局１２と無線通信を行い、基地局１２を介して、参照データ作成サーバ２０から参照地図および差分画像を受信して位置姿勢推定装置３２に供給する。位置姿勢推定装置３２は、無線通信装置３１から供給された参照地図および差分画像と、距離センサ３３によって測定されたセンシングデータとを用いて、環境地図内における移動ロボット３０の位置および向き（姿勢）を推定する。移動装置３４は、位置姿勢推定装置３２によって算出された、環境地図内における移動ロボット３０の位置および向きに基づき、予め設定された移動プランに従って、移動ロボット３０を移動させる。
【００４９】
図３は、参照データ作成サーバ２０の機能構成の一例を示すブロック図である。参照データ作成サーバ２０は、環境地図格納部２１、参照地図作成部２２、参照地図格納部２３、差分画像作成部２４、差分画像格納部２５、およびデータ送信部２６を備える。
【００５０】
ここで、「背景技術」の欄で述べたセンシングデータリストに基づくマッチング方法において、平行移動だけでなく回転についての推定も同時に行う方法について述べる。すなわち移動ロボットは平行移動に関する自由度（x, y）に加え、回転（姿勢）に関する自由度θについてのパラメータ推定を同時に行うものとする。
【００５１】
本実施形態においても、「背景技術」の欄のセンシングデータリストに基づくマッチング方法の説明で述べた参照地図を用いる。すなわち、センシングデータとして考えられるすべての値(d,φ)毎に、参照地図を予め用意しておくものとする。なお以下の説明では、それぞれの値(d,φ)に関する参照地図を、クラス(d,φ)の参照地図と呼ぶこととする。
【００５２】
ここで、図４（ａ）に示すように、クラス(d,-φ)の参照地図４００が与えられたときに、これがどのような意味を持つかについて説明する。
【００５３】
移動ロボットの回転方向θが０°のときにクラス(d,-φ)の参照地図４００を用いるのは、距離d、方向−φのセンシングデータ４０１（図４（ｂ）参照）を対象にしたときであるが、移動ロボットの回転方向θを考えると、これを次のように解釈することもできる。それは、距離d、方向(-φ-θ)のセンシングデータ４０２（図４（ｃ）参照）が得られたときに、移動ロボットの方向と環境地図の主方向（本実施形態ではｘ軸方向）とのなす角度がθ、すなわち、センシングデータ全体が環境地図に対してθだけ回転した場合である（図４（ｄ）参照）。
【００５４】
このような場合、回転していない(すなわちθ＝０°)状態の距離センサにおいて、距離d、方向-φのセンシングデータが得られたときと全く等価なセンシングが行われることになるので、距離d、方向-φであり、且つ、距離センサ（移動ロボット）の回転方向がθ＝０°のセンシングデータと、距離d、方向(-φ-θ)であり、且つ、距離センサ（移動ロボット）の回転θのセンシングデータとは、計算上は同様に扱うことができる。
【００５５】
一方、「背景技術」の欄で説明したとおり、クラス(d,-φ)の参照地図４００上で色がついた部分（１の値を持つ部分）は、センシングデータとして(d,-φ)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になることを示している。これは、環境地図と移動ロボットとの方向の差θが０°の場合であった。しかし上で述べた通り、移動ロボットが回転することも考慮に入れると、この状況はセンシングデータとして(d,-φ-θ)の値が観測されたときに、移動ロボットが環境地図に対してθだけ回転したときと等価になる。
【００５６】
よって、上記参照地図４００上で１の値を持つ部分の意味は、下記のように３６０通りに解釈することができる。
(0) 移動ロボットの方向θがθ=0°で、なおかつセンシングデータとして(d,-φ)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
(1) 移動ロボットの方向θがθ=1°で、なおかつセンシングデータとして(d,-φ-1°)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
(2) 移動ロボットの方向θがθ=2°で、なおかつセンシングデータとして(d,-φ-2°)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
・
・
・
(359) 移動ロボットの方向θがθ=359°で、なおかつセンシングデータとして(d,-φ-359°)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
【００５７】
このような原理に基づき、以下の手順で評価値画像を作成すれば、位置(x,y)に加え回転方向θについても同時に推定できるマッチングが実施できることになる。
【００５８】
まず、参照地図を、図５に示すような(r,φ)の形の極座標表現であらわされる各クラス毎に予め作成しておく。ここでrおよびφはそれぞれr = 0.1m, 0.2m, ..., 4.0m、φ = 0°, 1°, ..., 359°の値をとりうるので、この全ての組み合わせについて並べている。これらの参照地図は、移動ロボットが移動する前で、なおかつ、移動ロボットが移動する部屋の環境地図画像が与えられた後に作成しておくものである。またその作成方法は、「背景技術」の項のセンシングデータリストに基づくマッチング方法の説明で述べた通りである。
【００５９】
次に移動ロボットが移動して、図１４に示したセンシングデータ群４３０が、(d0, -90°)，(d1, -89°)， ... ，(d180, +90°)の１８１個のセンシングデータとして得られたときに、このセンシングデータと図５に示した参照地図２３０から、移動ロボットの位置姿勢を推定する方法を説明する。
(1) まず、移動ロボットの方向θを、θ＝０°と仮定する。
(2) (d0, -90°)，(d1, -89°)，... ，(d180, +90°)の各クラスの参照地図（合計１８１枚）を図５における一連の参照地図群の中から選び出す。なお、選び出す際に、(di,ψ)におけるψの値が０より小さい場合は、これに３６０°を加えてψ+360°とすることで、0°≦ψ＜360°の範囲にψの値が収まるようにする。このようにしても指し示す角度の意味は変わらない。この考え方は、以下の説明における、全ての角度を扱う処理においても同様に用いるものとする。
(3) 選び出された１８１枚の参照地図全てについて、これを各画素毎に足し合わせた評価値画像を作成する。この評価値画像の（x,y）の位置にある画素値が、(x,y,θ)（ただしθ=0°）のパラメータにおける評価値となる。
(4) 次にθ=１°と仮定する。
(5) (d0, -90°-θ)，(d1, -89°-θ)， ... ，(d180, +90°-θ)の各クラスの参照地図（合計１８１枚）を(2)と同様に図５における一連の参照地図群の中から選び出す。
(6) (3)と同様にこれを各画素毎に足し合わせた評価値画像を作成する。この評価値画像の(x,y)の位置にある画素値が、(x,y,θ)のパラメータにおける評価値となる。
(7) θの値を１°増やし、(5)、(6)の処理を行う。これをθ=359°になるまで繰り返す。
(8) 以上の処理により、θ＝０°，１°，・・・，３５９°の合計３６０枚の評価値画像が作成されるが、この中で最も評価値が大きい画素を見つけ、この画素の環境地図上の位置(x,y)と、その評価値が属するθの値の組み合わせ(x,y,θ)が推定値となる。
【００６０】
以上述べた方法は、本発明において、「背景技術」の欄で述べたセンシングデータリストに基づく方法を用いて、移動ロボットの回転方向θを移動ロボットの平行移動量(x,y)と同時に求める方法である。本方法を用いるだけでは、「背景技術」の欄で述べたセンシングデータリストに基づく方法に比べて特に効果は見出せないが、以下に述べる差分処理を用いる方法の説明をわかりやすくするためにここで詳しく述べた。なおこの方法を、センシングデータリストに基づく回転を含めたマッチング方法、と呼ぶことにする。
【００６１】
次に、本発明において、差分処理を用いる方法について述べる。これは一言で言うと、上に述べたセンシングデータリストに基づく回転を含めたマッチング方法の考え方を基本にして、これに、参照地図に関する差分処理を加えるものである。
【００６２】
まず、図６に示すように、２つの参照地図４１０および参照地図４１１から、差分画像４１２を作成することを考える。
【００６３】
ここで、差分画像４１２は、距離パラメータｒが同じ値を持ち、角度パラメータφが隣り合う値を持つ組みを選んで作成するものとする。すなわち、クラス(r,ψ)の参照地図と、クラス(r,ψ+1°)の参照地図を選ぶことになる（ここでr = 0.1, 0.2, ..., 4.0m、ψ = 0°, 1°, ..., 359°である）。これによって作成される差分画像を、クラス (r,ψ+1°)の差分画像と呼ぶこととする。
【００６４】
クラス(r,ψ)の差分画像G(r,ψ)は、クラス(r,ψ)の参照地図F(r,ψ)を用いて、下記の数式１により算出することができる。
【００６５】
G(r,ψ+1°)(x, y) = F(r,ψ+1°)(x, y) - F(r,ψ)(x, y) ・・・数式１
なお、(x,y)は、それぞれの画像における座標を示している。
【００６６】
上に述べた通り、ｒの値としてはr = 0.1m, 0.2m, ..., 4.0mの４０通り、ψの値としては、ψ= 0°, 1°, ..., 359°の３６０通りあるので、この差分画像Ｇは全部で ４０×３５９枚作成されることになる。
【００６７】
ところで、差分画像Ｇを作るときに用いられる隣り合う角度、すなわちψとψ+1°のそれぞれの角度からなる２つのクラス(r,ψ+1°)と(r,ψ)の参照地図を考えたときに、前者の画像は、後者の画像を１°だけ微小に回転させたものであるので、画像の形状が非常に似たようなものになるという性質をもっている。このため上の式に基づいて差分画像Ｇを作成すると、２つの画像において各画素値が同一であるものが多いため、画像中の各画素の多くの部分の差分値は０になることになる。
【００６８】
本発明では、差分画像Ｇについて、全ての画素の画素値を差分画像Ｇとして保持するのではなく、０以外の画素値を持つ画素について、その画素の位置と画素値を差分画像として保持する。具体的には、対応する画素の画素値をvkとして、(x0, y0, v0), (x1, y1, v1), ..., (xn, yn, vn) の形で記録しておくものとする。これにより、差分画像Ｇの各画素に対する参照回数を少なくする、すなわち差分画像Ｇの中で０の値を持つ画素については参照する必要がなくなる、という効果をもたらすことになる。従って、データ量の削減および演算量の削減を実現することができる。
【００６９】
図３に戻り、説明を続ける。環境地図格納部２１には、例えば図１５を用いて説明した二次元の環境地図が格納される。参照地図作成部２２は、環境地図格納部２１に格納されている環境地図から、r（本実施形態では、r = 0.1m, 0.2m, ..., 4.0m）およびφ（本実施形態では、φ = 0°, 1°, ..., 359°）のそれぞれのクラスに対応する参照地図を、合計４０×３６０枚作成し、作成した参照地図を参照地図格納部２３に格納する。参照地図格納部２３には、図５で説明した参照地図２３０が格納される。
【００７０】
差分画像作成部２４は、上述した数式１に基づいて、参照地図格納部２３に格納されている参照地図を用いて、各クラスの差分画像を作成し、作成した差分画像を差分画像格納部２５に格納する。差分画像格納部２５には、例えば図７に示すように、クラス２５０毎に、当該クラスに対応する差分画像中で画素値が０でない画素の位置を示す画素位置２５１、および、当該画素位置２５１にある画素の画素値２５２が格納される。
【００７１】
データ送信部２６は、環境地図格納部２１に格納されている環境地図、参照地図格納部２３に格納されている参照地図、および差分画像格納部２５に格納されている差分画像を、通信回線１１を介して移動ロボット３０へ送信する。
【００７２】
図８は、位置姿勢推定装置３２の機能構成の一例を示すブロック図である。位置姿勢推定装置３２は、データ取得部３２０、データ格納部３２１、第一の評価画像作成部３２２、統合差分画像作成部３２３、第一の評価値算出部３２４、第二の評価画像作成部３２５、位置姿勢推定部３２６、評価値格納部３２７、および第二の評価値算出部３２８を備える。
【００７３】
ここで、本発明において差分画像を用いてマッチングを行う方法を述べる。ここでは、各クラス(rj,ψk)（rj = 0.1m, 0.2m, 0.3m, ..., 4.0m、ψk = 0°, 1°, ..., 359°）の参照地図、ならびにクラス(rj,ψk)（rj = 0.1m, 0.2m, 0.3m, ..., 4.0m、ψk = 1°, 2°, ..., 359°）の差分画像が予め作成されており、センシングデータとして(d0 ,-90°)，(d1 ,-89°), ..., (d180 ,+90°)の１８１個のデータが得られていることを想定する。
(1) まず、移動ロボットの方向θを、θ＝０°と仮定する。
(2) (d0 ,-90°), (d1 ,-89°), ..., (d180 ,+90°)の各クラスの参照地図（合計１８１枚）を一連の参照地図群の中から選び出す。なお、角度は適宜３６０°を加えることで、０°以上３６０°未満の範囲に収まるようにするのは、上で述べた通りである。
(3) 選び出された１８１枚全ての参照地図について、これを各画素毎に足し合わせた評価値画像を作成する。この評価値画像の(x,y)の位置にある画素値が、(x,y,θ)（ただしθ=0°）のパラメータにおける評価値となる。
(4) 次にθ＝１°と仮定する。
(5) (d0 ,-90°-θ)，(d1 ,-89°-θ)，...，(d180 ,+90°-θ)の各クラスの参照地図（合計１８１枚）を、今度は参照地図ではなく、差分画像群の中から選び出す。
(6) 選びだされた全ての差分画像について、これを各画素毎に足し合わせた画像データを作成する。このように作成された画像データを、統合差分画像と呼ぶ。
(7) 作成された統合差分画像と、前回、すなわち(3)で作成されたθ=0°のときの評価値画像を、各画素毎に足し合わせた画像データを作成する。これが移動ロボットの回転角度がθのときの評価値画像となる。すなわち、この評価値画像の(x,y)の位置にある画素値が、(x,y,θ)のパラメータにおける評価値となる。
(8) θの値を１° 増やし、(5), (6), (7) の処理を行う。これをθ=359°になるまで繰り返す。
(9) 以上の処理により、θ= 0°, 1°, ..., 359°の合計360枚の評価値画像が作成されるが、この中で最も値が大きい画素を見つけ、この画素の画像上の位置(x,y)と、その画像が属するθの値の組み合わせ(x,y,θ)が、パラメータの推定値となる。
【００７４】
本発明による差分画像を用いる方法では、センシングデータリストに基づく回転を含めたマッチング方法に比べ、計算時に用いる画像データの枚数は変わらない。具体的には、センシングデータリストに基づく回転を含めたマッチング方法では、θ= 0°, 1°, ..., 359°のときにそれぞれ(d0 ,-90°-θ), (d1 ,-89°-θ), ..., (d180 ,+90°-θ) の合計１８１の異なるクラスの参照地図を用いることとなり、扱う総画像枚数は１８１×３６０枚となる。
【００７５】
これに対して、本発明による差分画像を用いる方法では、θ=０°ときの(d0 ,-90°), (d1 ,-89°), ..., (d180 ,+90°)の合計１８１枚の異なるクラスの参照地図と、θ= 1°, 2°, ..., 359°のそれぞれのときに(d0 ,-90°-θ), (d1 ,-89°-θ), ..., (d180 ,+90°-θ) の合計１８１枚の異なる差分画像を用いるので、扱う総画像枚数は１８１×（１＋３５９）枚となる。
【００７６】
このように、計算に利用する画像の枚数は、センシングデータリストに基づく回転を含めたマッチング方法も、本発明による差分画像を用いる方法も変わらないことになる。しかし、それぞれの画像データに着目したとき、０以外の値を持つ画素の数、すなわち計算に用いられる画素の数という点において、参照地図に比べて、差分画像は大幅に削減されることになる。そのため、計算に用いられる画素の数はこの削減分と同じだけ削減されることになり、計算処理量もこれに比例して削減されることになる。
【００７７】
これは、差分画像を作成するときに用いられる隣り合う角度の２つのクラスの参照地図が非常に似ているために、差分画像における大部分の画素値が０になる、という性質によってもたらされるものである。もし、この差分画像における０以外の値を持つ画素の数が、もとの参照地図の０以外の値を持つ画素の数の例えば１／３になれば、マッチング方法における画素値の参照演算の回数は以下のとおりとなる。
181×360×101×101×1/2×1/3 （回）
ここで、「181」、「360」、「101」、「101」、「1/2」の各数値の持つ意味は、センシングデータリストに基づくマッチング方法のときと同様である。これに新たに加わる「1/3」の数値が、上に述べた０以外の値を持つ画素の数の削減割合である。すなわち、本発明における差分処理を用いた方法は、この数字の分、参照演算数が処理が削減されることになる。これが本発明における差分処理の効果である。
【００７８】
図８に戻って、位置姿勢推定装置３２の機能構成の説明を続ける。データ取得部３２０は、無線通信装置３１を介して参照データ作成サーバ２０から、環境地図、各クラスの参照地図、および各クラスの差分画像を取得し、取得した環境地図、参照地図、および差分画像をデータ格納部３２１に格納する。
【００７９】
第一の評価画像作成部３２２は、１８１個のセンシングデータ(di,φi)(i=0,1,・・・,180)を距離センサ３３から取得する。そして、第一の評価画像作成部３２２は、取得したそれぞれのセンシングデータ(di,φi)について、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)をデータ格納部３２１から抽出する。そして、第一の評価画像作成部３２２は、抽出した１８１個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される評価画像(θ0)を作成する。評価画像(θ0)は、移動ロボット３０の向きと環境地図において予め定められた方向（本実施形態では図１６におけるｘ軸方向）とのなす角がθ0（本実施形態ではθ0＝０°）であると仮定した場合の評価画像を意味する。
【００８０】
第一の評価値算出部３２４は、第一の評価画像作成部３２２によって作成された評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として評価値格納部３２７に格納する。評価値格納部３２７には、例えば図９に示すように、環境地図内の画素の位置を示すｘ座標３２７０およびｙ座標３２７１に対応付けて、当該位置にある画素の評価値３２７３、および、当該評価値３２７３が算出される際に仮定された、環境地図における所定の方向に対する移動ロボット３０の向きを示す方向３２７２が格納される。
【００８１】
統合差分画像作成部３２３は、距離センサ３３によって測定された１８１個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を、データ格納部３２１から抽出する。そして、統合差分画像作成部３２３は、抽出した１８１個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される統合差分画像(θj)を作成する。統合差分画像(θj)は、移動ロボット３０の向きと環境地図において予め定められた方向とのなす角がθjであると仮定した場合の統合差分画像を意味する。
【００８２】
第二の評価画像作成部３２５は、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する。第二の評価値算出部３２８は、第二の評価画像作成部３２５によって作成された評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として評価値格納部３２７に格納する。
【００８３】
なお、統合差分画像作成部３２３、第一の評価値算出部３２４、および第二の評価画像作成部３２５は、第一の評価画像作成部３２２によって評価画像(θ0)が作成された後に、j=1からj=m-1まで、統合差分画像(θj)を作成する処理、評価画像(θj)を作成する処理、および評価画像(θj)内の(x,y)の位置にある画素の評価値を格納する処理を、それぞれ実行する。
【００８４】
位置姿勢推定部３２６は、評価値格納部３２７を参照して、最大の評価値が対応付けられている(x,y,θ)を特定し、環境地図の所定方向とのなす角度がθの向きで、環境地図内の(x,y)の位置に、移動ロボット３０の向きおよび位置を推定し、推定した向きおよび位置を移動装置３４に通知する。
【００８５】
図１０は、参照データ作成サーバ２０の動作の一例を示すフローチャートである。例えば環境地図が環境地図格納部２１に格納される等の所定のタイミングで、参照データ作成サーバ２０は、本フローチャートに示す動作を開始する。
【００８６】
まず、参照地図作成部２２は、ｊの値を１に、ｋの値を０に、それぞれ初期化し（Ｓ１００）、環境地図格納部２１を参照して、環境地図からクラス(rj,ψk)の参照地図を作成し、作成した参照地図を参照地図格納部２３に格納する（Ｓ１０１）。そして、参照地図作成部２２は、ｊの値が４０か否かを判定し、ｊの値が４０未満の場合（Ｓ１０２：Ｎｏ）、ｊの値を１増やし（Ｓ１０３）、再びステップＳ１０１に示した処理を実行する。
【００８７】
ｊの値が４０の場合（Ｓ１０２：Ｙｅｓ）、参照地図作成部２２は、ｊの値を０に初期化すると共に、ｋの値を１増やし（Ｓ１０４）、ｋの値が１８１よりも大きいか否かを判定する（Ｓ１０５）。ｋの値が１８１以下の場合（Ｓ１０５：Ｎｏ）、参照地図作成部２２は、再びステップＳ１０１に示した処理を実行する。
【００８８】
ｋの値が１８１よりも大きい場合（Ｓ１０５：Ｙｅｓ）、差分画像作成部２４は、ｊおよびｋの値を１に初期化し（Ｓ１０６）、参照地図格納部２３を参照して、下記の数式２を用いて、クラス(rj,ψk)における差分画像を算出し、算出した差分画像(rj,ψk)を差分画像格納部２５に格納する（Ｓ１０７）。
【００８９】
差分画像(rj,ψk)＝参照地図(rj,ψk)−参照地図(rj,ψk-1) ・・・数式２
次に、差分画像作成部２４は、ｋの値が１８１であるか否かを判定し、ｋの値が１８１未満の場合（Ｓ１０８：Ｎｏ）、ｋの値を１増やし（Ｓ１０９）、再びステップＳ１０７に示した処理を実行する。ｋの値が１８１の場合（Ｓ１０８：Ｙｅｓ）、差分画像作成部２４は、ｊの値を１増やすと共に、ｋの値を１に初期化し（Ｓ１１０）、ｊの値が４０よりも大きいか否かを判定する（Ｓ１１１）。
【００９０】
ｊの値が４０以下の場合（Ｓ１１１：Ｎｏ）、差分画像作成部２４は、再びステップＳ１０７に示した処理を実行する。ｊの値が４０より大きい場合（Ｓ１１１：Ｙｅｓ）、データ送信部２６は、環境地図格納部２１に格納されている環境地図、参照地図格納部２３に格納されている参照地図、および差分画像格納部２５に格納されている差分画像を、通信回線１１を介して移動ロボット３０へ送信し（Ｓ１１２）、参照データ作成サーバ２０は、本フローチャートに示した動作を終了する。
【００９１】
図１１は、位置姿勢推定装置３２の動作の一例を示すフローチャートである。移動ロボット３０は、環境地図、参照地図、および差分画像を、参照データ作成サーバ２０から受信した後に、所定のタイミングで本フローチャートに示す動作を開始する。
【００９２】
まず、第一の評価画像作成部３２２は、ｉの値を０に初期化し（Ｓ２００）、距離センサ３３によって測定されたセンシングデータ(di,φi)に対応する参照地図(di,φi)をデータ格納部３２１から抽出する（Ｓ２０１）。そして、第一の評価画像作成部３２２は、ｉの値が１８０であるか否かを判定し、ｉの値が１８０未満である場合には（Ｓ２０２：Ｎｏ）、ｉの値を１増やし（Ｓ２０３）、再びステップＳ２０１に示した処理を実行する。
【００９３】
ｉの値が１８０である場合（Ｓ２０２：Ｙｅｓ）、第一の評価画像作成部３２２は、抽出した参照地図(di,φi)を画素毎に加算することにより、評価画像(θ0)を作成する（Ｓ２０４）。そして、第一の評価値算出部３２４は、第一の評価画像作成部３２２によって作成された評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として評価値格納部３２７に格納する（Ｓ２０５）。
【００９４】
次に、統合差分画像作成部３２３、第二の評価画像作成部３２５、および第二の評価値算出部３２８は、ｉの値を０に、ｊの値を１に、それぞれ初期化する（Ｓ２０６）。そして、統合差分画像作成部３２３は、距離センサ３３によって測定されたセンシングデータ(di,φi-θj)に対応する差分画像(di,φi-θj)をデータ格納部３２１から抽出し（Ｓ２０７）、ｉの値が１８０であるか否かを判定する（Ｓ２０８）。ｉの値が１８０未満である場合（Ｓ２０８：Ｎｏ）、統合差分画像作成部３２３は、ｉの値を１増やし（Ｓ２０３）、再びステップＳ２０７に示した処理を実行する。
【００９５】
ｉの値が１８０である場合（Ｓ２０８：Ｙｅｓ）、統合差分画像作成部３２３は、抽出した１８１個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される統合差分画像(θj)を作成する（Ｓ２１０）。そして、第二の評価画像作成部３２５は、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する（Ｓ２１１）。
【００９６】
次に、第二の評価値算出部３２８は、第二の評価画像作成部３２５によって作成された評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として評価値格納部３２７に格納し（Ｓ２１２）、ｊの値が３５９であるか否かを判定する（Ｓ２１３）。ｊの値が３５９未満である場合（Ｓ２１３：Ｎｏ）、統合差分画像作成部３２３は、ｊの値を１増やし（Ｓ２１４）、再びステップＳ２０７に示した処理を実行する。
【００９７】
ｊの値が３５９である場合（Ｓ２１３：Ｙｅｓ）、位置姿勢推定部３２６は、評価値格納部３２７を参照して、最大の評価値が対応付けられている(x,y,θ)を特定し、環境地図の所定方向とのなす角度がθの向きで、環境地図内の(x,y)の位置に、移動ロボット３０の向きおよび位置を推定し（Ｓ２１５）、位置姿勢推定装置３２は、本フローチャートに示した動作を終了する。
【００９８】
図１２は、参照データ作成サーバ２０または位置姿勢推定装置３２の機能を実現するコンピュータ４０のハードウェア構成の一例を示すハードウェア構成図である。コンピュータ４０は、ＣＰＵ（Central Processing Unit）４１、ＲＡＭ（Random Access Memory）４２、ＲＯＭ（Read Only Memory）４３、ＨＤＤ（Hard Disk Drive）４４、通信インターフェイス（Ｉ／Ｆ）４５、入出力インターフェイス（Ｉ／Ｆ）４６、およびメディアインターフェイス（Ｉ／Ｆ）４７を備える。
【００９９】
ＣＰＵ４１は、ＲＯＭ４３またはＨＤＤ４４に格納されたプログラムに基づいて動作し、各部の制御を行う。ＲＯＭ４３は、コンピュータ４０の起動時にＣＰＵ４１が実行するブートプログラムや、コンピュータ４０のハードウェアに依存するプログラム等を格納する。ＨＤＤ４４は、ＣＰＵ４１によって実行されるプログラムを格納する。通信インターフェイス４５は、通信回線を介して他の機器からデータを受信してＣＰＵ４１へ送ると共に、ＣＰＵ４１が生成したデータを、通信回線を介して他の機器へ送信する。
【０１００】
ＣＰＵ４１は、入出力インターフェイス４６を介して、モニタ等の出力装置、および、キーボードやマウス等の入力装置を制御する。ＣＰＵ４１は、入出力インターフェイス４６を介して、入力装置からデータを取得する。また、ＣＰＵ４１は、生成したデータを、入出力インターフェイス４６を介して出力装置へ出力する。なお、コンピュータ４０が位置姿勢推定装置３２として機能する場合、コンピュータ４０には通信インターフェイス４５が設けられず、コンピュータ４０は、入出力インターフェイス４６に接続された無線通信装置３１を介して通信を行う。また、コンピュータ４０が位置姿勢推定装置３２として機能する場合、距離センサ３３および移動装置３４とのデータやり取りは、入出力インターフェイス４６を介して行われる。
【０１０１】
メディアインターフェイス４７は、記録媒体４８に格納されたプログラムまたはデータを読み取り、ＲＡＭ４２に提供する。ＲＡＭ４２を介してＣＰＵ４１に提供されるプログラムは、記録媒体４８に格納されている。当該プログラムは、記録媒体４８から読み出されて、ＲＡＭ４２を介してコンピュータ４０にインストールされ、ＣＰＵ４１によって実行される。記録媒体４８は、例えばＤＶＤ（Digital Versatile Disk）、ＰＤ（Phase change rewritable Disk）等の光学記録媒体、ＭＯ（Magneto-Optical disk）等の光磁気記録媒体、テープ媒体、磁気記録媒体、または半導体メモリ等である。
【０１０２】
コンピュータ４０が参照データ作成サーバ２０として機能する場合、コンピュータ４０にインストールされて実行されるプログラムは、コンピュータ４０を、環境地図格納部２１、参照地図作成部２２、参照地図格納部２３、差分画像作成部２４、差分画像格納部２５、およびデータ送信部２６として機能させる。
【０１０３】
また、コンピュータ４０が位置姿勢推定装置３２として機能する場合、コンピュータ４０にインストールされて実行されるプログラムは、コンピュータ４０を、データ取得部３２０、データ格納部３２１、第一の評価画像作成部３２２、統合差分画像作成部３２３、第一の評価値算出部３２４、第二の評価画像作成部３２５、位置姿勢推定部３２６、評価値格納部３２７、および第二の評価値算出部３２８として機能させる。
【０１０４】
コンピュータ４０は、これらのプログラムを、記録媒体４８から読み取って実行するが、他の例として、他の装置から、通信媒体を介してこれらのプログラムを取得してもよい。通信媒体とは、有線もしくは無線の通信回線、または、当該通信回線を伝搬するディジタル信号もしくは搬送波を指す。
【０１０５】
以上、本発明の実施の形態について説明した。
【０１０６】
上記説明から明らかなように、本実施形態の位置姿勢推定システム１０によれば、センシングデータリストに基づくマッチング方法に比べて、さらなる計算量の削減を実現することができる。
【０１０７】
ここで、今まで述べた説明をもとに、「課題を解決するための手段」の欄で図１を参照して述べた本発明の構成と、上記した実施形態との対応関係を補足する。
【０１０８】
図１において、環境地図１０１は、「背景技術」の項で説明したように、距離センサ１０６による測定高さ断面での障害物の存在を、ディジタル画像としてあらわしたものである。ここでは、障害物が存在しない場所を画素値0（白色）、障害物が存在する場所を画素値1（黒色）としてあらわすことにする。
【０１０９】
距離センサ１０６は、「背景技術」の項で説明したように、自分の周辺の予め決められた複数の方向について、障害物までの距離を計測できるものである。ここでは、測定は正面に対して-90°から+90°に１°毎の合計１８１の異なる方向について行われるものとする。
【０１１０】
センシングデータ１０７は、「背景技術」の項で説明したように、上記距離センサ１０６によって障害物の位置が、角度と距離の組み合わせとして(d0 ,-90°)，(d1 ,-89°)， ...,，(d180 ,+90°)の合計１８１個の極座標表現によるデータとしてあらわされるものである。また、d0, d1, ..., d180 は、0.1m, 0.2m, 0.3m, ..., 4.0mのいずれかの値として量子化されているものである。
【０１１１】
位置姿勢推定値１２４とは、「背景技術」の欄で説明したように、移動ロボットが平面運動を行うことを想定するので、位置と方向に関する合計３パラメータ(x, y,θ)としてあらわされるものである。
【０１１２】
まず、図１における参照データ群を作成する手段１５０において、環境地図からマッチングのための第１の参照データ群１０３を作成する方法について説明する。ここで第１の参照データ群とは、「背景技術」の項で説明した一連の参照地図を示す。具体的には、(rj,ψk)（ただし、rj = 0.1m, 0.2m, 0.3m, ..., 4.0mの４０通り、ψk = 0°, 1°, ..., 359°の３６０通り）のクラスとして作成するので、合計４０×３６０枚になる。このデータは、環境地図を(x,y)の方向に移動させることで作成される。なお、x = rj・cos(ψk), y = rj・sin(ψk) である。
【０１１３】
また、各画素は実環境における１０ｃｍ×１０ｃｍの大きさの領域を示すように量子化されるものとする。さらに、この参照地図は、画像そのものではなく、画像上において、1の値を持つ画素の座標値を列挙したデータとして、(x0, y0), (x1, y1), ..., (xn, yn) のように記録されるものとする。
【０１１４】
次に、図１における参照データ群を作成する手段１５０において、第１の参照データ群から第２の参照データ群１０５を作成する方法について説明する。ここで、第２の参照データ群とは、第１の実施例で説明した一連の差分画像を示す。具体的には、(rj,ψk)（ただし、rj = 0.1m, 0.2m, 0.3m, ..., 4.0mの４０通り、ψk = 1°, 2°, ..., 359°の３５９通りのクラスとして作成するので、合計４０×３５９枚になる。このデータは、上に述べた参照地図の中で角度パラメータについて隣り合うクラスのものを２つ組み合わせ、この画像の画素毎の差分処理を行うことで作成される。
【０１１５】
すなわち、クラス(rj,ψ+1°)と、クラス(rj,ψ)の参照地図から、クラス(rj,ψ+1°)の差分画像が作成されることになる。その具体的な作成方法は、発明を実施するための最良の形態の前半部分で説明した通りである。この差分画像は、画像そのものではなく、画像上において、０以外の値をもつ画素の座標値と、その値を列挙したデータとして、(x0, y0 v0), (x1, y1, v1), ..., (xn, yn, vn) のように記録されるものとする。
【０１１６】
次に、図１における位置姿勢を推定する手段１７０における、センシングデータに基づいて、第１の参照データ群と第２の参照データ群の中から必要なデータを選択する処理について説明する。ここでは、移動ロボットの回転をθと仮定し、この値とセンシングデータとから算出される値に基づき、第１の参照データ群、すなわち各クラスの参照地図と、第２の参照データ群、すなわち各クラスの差分画像から、適切なクラスのものを選択する。選択されるものは複数枚になる。その具体的な選択方法は、発明を実施するための最良の形態の前半部分で説明したとおりである。
【０１１７】
次に、図１の位置姿勢を推定する手段１７０における、選択されたデータを加算することで評価のためのデータ１２２を作成する処理１２１について説明する。ここでは、仮定した移動ロボットの回転θの値毎に、上で選択した各クラスの参照地図と差分画像を画素毎に足し合わせ、さらにこれに、移動ロボットの回転(θ-1°)として作成した評価値画像を足し合わせ、これを移動ロボットの回転θの評価値画像とする。その具体的な方法は、発明を実施するための最良の形態の前半部分で説明したとおりである。なお、この評価値画像が、図１における評価のためのデータとなる。
【０１１８】
次に、図１の位置姿勢を推定する手段１７０における、評価のためのデータから位置姿勢推定値を求める処理１２３について説明する。これは、上で求められた各移動ロボットの回転θ毎に算出された評価値画像の中から、最大の値をもつ画素を見つけ出し、これが属する回転角度θと、その画素の画像上の位置(x,y)から、位置姿勢推定値を(x,y,θ)として求めるものである。その具体的な方法は、発明を実施するための最良の形態の前半部分で説明した通りである。
【０１１９】
また、本発明は、上記した実施形態に限定されるものではなく、その要旨の範囲内で数々の変形が可能である。
【０１２０】
例えば、上記した実施形態において、位置姿勢推定装置３２は、パラメータ推定時には、参照データ作成サーバ２０によって予め作成された参照地図および差分画像の両方を利用するが、本発明はこれに限られない。他の形態として、位置姿勢推定装置３２は、参照データ作成サーバ２０によって予め作成された差分画像と一部の参照地図とを用いて各パラメータを推定するようにしてもよい。
【０１２１】
例えば、参照データ作成サーバ２０は、参照地図としてψ＝０°のクラス、すなわちクラス（0.1m, 0°), クラス(0.2m, 0°), ..., クラス(4.0m, 0°)の参照地図を作成し、位置姿勢推定装置３２は、参照データ作成サーバ２０によって作成されたクラスの参照地図をデータ格納部３２１に保存する。これは、上記した実施形態においてψ= 0°, 1°, ..., 359°の全てのクラスの参照地図を記録しておくのに比べて１／３６０のメモリ量となる。なお、差分画像については、参照データ作成サーバ２０は、上記した実施形態と同様に全てのクラスについて差分画像を作成し、位置姿勢推定装置３２は、参照データ作成サーバ２０によって作成された全てのクラスの差分画像をデータ格納部３２１に保存する。
【０１２２】
このような準備のもと、第一の評価画像作成部３２２は、以下のステップに従い、方向θをθ＝０°と仮定したときの評価値画像を作成する。
(1) θ＝０°とする。
(2) センシングデータ(d0 ,-90°), (d1,-89°), ..., (d180 ,+90°)について、距離パラメータはそれぞれの値とし、角度パラメータは０°に置き換えたクラスの参照地図を選び出す。すなわち(d0 ,0°), (d1 ,0°), ..., (d180 ,0°)のクラスの参照地図を選び出すことになる。
(3) これらのデータについて、角度パラメータがセンシングデータのものになるまで、差分画像を足し合わせていく。この足し合わせた画像データが、そのクラスの参照地図となる。例えば、(d0 ,-90°)のセンシングデータについては、-90° → -90°+ 360° = +270°の変換処理を行ったのちに、以下の計算を行うことで、目的となる (d0 ,+270°)のクラスの参照地図を作成する。
【０１２３】
F(d0 ,270°) = F(d0 ,0°) + G(d0 ,1°) + G(d0 ,2°) + ... + G(d0 ,270°)
・・・数式３ ここで、F(d,ψ)およびG(d,ψ) は、それぞれ、クラス(d,ψ)の参照地図およびクラス(d,ψ)の差分画像である。
【０１２４】
以上の処理により、θ＝０°のときの各クラスの参照地図が求められるので、それ以降は、この参照地図に、それぞれの角度における統合差分画像を加算していくことにより、目的となる推定値が求められることになる。このようにすることにより、センシングデータに対応する各クラスの参照地図を作成する処理が新たに必要になるものの、データ格納部３２１内に参照地図の大部分を記憶する必要がないため、メモリ量を削減することができる。
【０１２５】
また、上記した実施形態において、移動ロボット３０は、有線または無線通信により、環境地図、参照地図、および差分画像を参照データ作成サーバ２０から取得するが、他の形態として、移動ロボット３０は、可搬性の記録媒体を介して、これらのデータを参照データ作成サーバ２０から取得するようにしてもよい。
【０１２６】
また、上記した実施形態において、位置姿勢推定システム１０は、参照データ作成サーバ２０と移動ロボット３０とを備えるが、他の形態として、参照データ作成サーバ２０の機能を移動ロボット３０内に設けることにより、移動ロボット３０単体で位置姿勢推定システム１０の機能を実現するように構成されていてもよい。
【０１２７】
また、上記した実施形態において用いられている様々な具体的な数値は、説明に対する理解を助けるために仮に設定したものであり、本発明の実施時にその値が使われることを限定するものではない。例えば、上記した実施形態では、角度の分解能が１°であったが、本発明を適用するシステムの仕様や要求される精度等に応じて、０．５°や０．１°等であってもよい。
【図面の簡単な説明】
【０１２８】
【図１】本発明の位置姿勢推定システム１０の全体構成の一例を説明するための概念図である。
【図２】本発明の一実施形態に係る位置姿勢推定システム１０の構成を示すシステム構成図である。
【図３】参照データ作成サーバ２０の機能構成の一例を示すブロック図である。
【図４】参照地図４００の意味を説明するための概念図である。
【図５】参照地図格納部２３に格納されるデータ構造の一例を説明するための概念図である。
【図６】差分画像４１２の作成過程を説明するための概念図である。
【図７】差分画像格納部２５に格納されるデータ構造の一例を示す図である。
【図８】位置姿勢推定装置３２の機能構成の一例を示すブロック図である。
【図９】評価値格納部３２７に格納されるデータ構造の一例を示す図である。
【図１０】参照データ作成サーバ２０の動作の一例を示すフローチャートである。
【図１１】位置姿勢推定装置３２の動作の一例を示すフローチャートである。
【図１２】参照データ作成サーバ２０の機能を実現するコンピュータ４０の一例を示すハードウェア構成図である。
【図１３】移動ロボット４２０の動作概要を説明するための概念図である。
【図１４】距離センサ４２１によって測定されたセンシングデータ群４３０の一例を示す図である。
【図１５】環境地図４４０の一例を示す図である。
【図１６】移動ロボットの位置を特定する過程を説明するための概念図である。
【図１７】センシングデータ群４６０および環境地図４６３の一例を示す概念図である。
【図１８】基本手法によるマッチング方法の過程を説明するための概念図である。
【図１９】第０番目のセンシングデータ４７０と環境地図内の黒い画素とが一致する場合を抜き出した図である。
【図２０】第０番目のセンシングデータ４７０と環境地図内の黒い画素とが一致する場合のセンサ原点４８１の位置をプロットした図である。
【図２１】図２０と環境地図との関係を説明するための概念図である。
【図２２】各センシングデータと参照地図との関係を説明するための概念図である。
【図２３】評価画像が作成される過程を説明するための概念図である。
【図２４】評価画像５１０の一例を示す図である。
【符号の説明】
【０１２９】
１０・・・位置姿勢推定システム、１０１・・・環境地図、１０２・・・第１の参照データ群の作成処理、１０３・・・第１の参照データ群、１０４・・・第２の参照データ群の作成処理、１０５・・・第２の参照データ群、１０６・・・距離センサ、１０７・・・センシングデータ、１２０・・・参照データ選択処理、１２１・・・加算処理、１２２・・・評価のためのデータ、１２３・・・位置姿勢推定値を求めるための処理、１２４・・・位置姿勢推定値、１５０・・・参照データ群を作成する手段、１７０・・・位置姿勢を推定する手段、１１・・・通信回線、１２・・・基地局、２０・・・参照データ作成サーバ、２１・・・環境地図格納部、２２・・・参照地図作成部、２３・・・参照地図格納部、２４・・・差分画像作成部、２５・・・差分画像格納部、２６・・・データ送信部、３０・・・移動ロボット、３１・・・無線通信装置、３２・・・位置姿勢推定装置、３２０・・・データ取得部、３２１・・・データ格納部、３２２・・・第一の評価画像作成部、３２３・・・統合差分画像作成部、３２４・・・第一の評価値算出部、３２５・・・第二の評価画像作成部、３２６・・・位置姿勢推定部、３２７・・・評価値格納部、３２８・・・第二の評価値算出部、３３・・・距離センサ、３４・・・移動装置
【技術分野】
【０００１】
本発明は、移動ロボットの自律移動制御等において利用される、実環境内における自己の位置姿勢を認識する技術に関する。特に、移動ロボットが目的地まで人の操作なしに自動的に到達することを目的とし、移動ロボットの位置姿勢を、移動ロボットに搭載された距離センサからのセンシングデータと、環境の幾何状況を示した二次元の環境地図とを用いて算出する技術に関する。
【背景技術】
【０００２】
まず、距離センサと二次元の環境地図を用いて移動ロボットの位置姿勢を推定する従来の方法について説明する。
【０００３】
ここで距離センサとは、距離センサの周辺の複数の方向について、障害物までの距離を計測できるものである。仕様としてさまざまなものが考えられるが、ここでは図１３および図１４に示すように、ある高さの水平面上を距離センサ４２１の正面に対して-90°から+90°の方向に、１°毎の合計１８１通りの異なる方向について、障害物４２２までの距離データが瞬時に計測できるものを想定する。よって各センシングデータ４３１は、角度φと距離dの組み合わせとして(d0,φ0),(d1,φ1),...,(d180,φ180)としてあらわされる。
【０００４】
ここで各変数の数字の添え字はデータ番号で、φiはi番目の測定方向を示す。測定方向とは、反時計回りを正方向とし、距離センサの主方向となす角度であり、主方向とのなす角度が小さい順にデータ番号が割り当てられる。すなわちφ0=-90°,φ1=-89°,...,φ180=+90.0°を示す。また、diは、それぞれの方向における障害物までの距離を示す。
【０００５】
なお、ここでは距離センサの測定能力を考慮し、測定可能距離を0.0mより大きく4.0m以下と想定し、diの値の範囲を、0.0m＜di≦4.0mに制限することとする。また、この１８１個の距離データを１まとまりとして、１フレーム分のセンシングデータ群４３０と呼ぶこととする。また、(di,φi)であらわされたパラメータは、センシングデータの位置、すなわち障害物の位置を、距離センサの原点位置を原点とした極座標で表現したものと等価になるが、これをxi=di・cos(φi)、yi=di・sin(φi)とすることで、(xi,yi)の形の直交座標表現に変換することもできる。
【０００６】
一方、二次元の環境地図とは、距離センサ４２１による測定高さ断面での障害物４２２の存在を、図１５に示すようなディジタル画像としてあらわしたものである。ここでは図１５に示すように、障害物が存在しない場所を白を示す画素値４４１、障害物が存在する場所を黒を示す画素値４４２としてあらわすことにする。
【０００７】
移動ロボット４２０は、図１３に示すように平らな地面上を移動するものと仮定する。よってその移動によって変化する位置姿勢に関する自由度は、位置について(x,y)の２パラメータ、姿勢については移動ロボットが向く方向角度に関するθの１パラメータの、合計３パラメータである。なお、これらのパラメータをあらわす座標系は、図１５に示すような環境地図があらわわれる座標系と一致させるものとする。
【０００８】
移動ロボットの位置姿勢を推定することとは、任意の位置姿勢にある移動ロボットのセンシングデータから、上記(x,y,θ)の３パラメータを求めることを意味する。よって、本発明が対象とする機能は、任意の位置姿勢にある移動ロボットの位置姿勢パラメータ(x,y,θ)を、移動ロボットに搭載された距離センサによるセンシングデータと、あらかじめ用意された環境地図を用いて求めることとなる。
【０００９】
本機能を実現する手段として、いくつかの方法が考えられるが、ここではその最も基本的な方法となるマッチング方法を説明する。
【００１０】
マッチング方法とは、あるフレームにおいて計測されたセンシングデータ全体の位置および姿勢（方向）を変えながら、当該センシングデータを環境地図に重ね合わせ、センシングデータと環境地図とが最もマッチする位置および姿勢を探し出し、これをもって移動ロボットの位置姿勢パラメータの推定値とするというものである。その原理は、移動ロボットがその位置および姿勢にあるときに、計測されたデータと同じデータが最も高い確率で得られるであろうという考え方に基づいている。
【００１１】
このようなマッチング方法は、具体的には以下に述べるような処理によって実施される。
(1) あるフレームにおけるすべてのセンシングデータ（前述した距離センサの例では１８１個）に対して、これを地図上でθだけ回転させ、(x,y)だけ平行移動させた位置を求める。
(2) それらの位置の環境地図上の画素値を参照し、１の値をもつもの（障害物があるもの）の数を数え、これをそのパラメータ (x,y,θ)における評価値とする。
(3) 上記(1)および(2)の処理を、考えられうるすべての値をもつ(x,y,θ)の組み合わせについて、評価値が最も大きい(x,y,θ)を解とする。
【００１２】
上記の手法を、基本手法によるマッチング方法と呼ぶことにする。図１６に基本手法によるマッチング方法の概要を示す。図１５に示した環境地図４４０と、図１６（ａ）に示したセンシングデータ群４３０とが与えられたときに、この方法によって最もマッチすると判断された状況が図１６（ｂ）の位置４５０および方向４５１となる。このときの平行移動成分(x,y)と、回転成分θが、移動ロボット位置姿勢パラメータの推定値となる。
【００１３】
さて、基本手法によるマッチング方法は、処理量が膨大になるという問題がある。具体的には、環境地図のサイズ、すなわち対象とする部屋等移動ロボットが移動可能なエリアのサイズを例えば10m×10mとし、マッチングの粒度、すなわちどの程度の細かさでマッチングの際のパラメータを変化させていくかについて、その最小単位を、位置すなわち(x,y)パラメータに関して例えば10cm、姿勢すなわちθパラメータに関して例えば１°とすると、上述した基本手法によるマッチング方法では、181×360×101×101 = 664,697,160回の画素値の参照演算が必要となる。
【００１４】
ここで、「181」はデータ数、「360」は回転角度θがとりうる値の数（0°,1°,2°,...,359°の３６０通り）、「101」はx方向の移動量がとりうる値の数（-5.0m,-4.9m,...,+5.0mの１０１通り、次の「101」は同じようにy方向の移動量がとりうる値の数（-5.0m,-4.9m,...,+5.0mの１０１通り）である。
【００１５】
マッチング方法は、一般にコンピュータ等の計算機で行うことが想定される。上述した基本手法によるマッチング方法で行われる画素値の参照演算は、１つ１つはそれほど大きな計算負荷にならないが、その量が膨大であれば、その量にほぼ比例して計算負荷が増大することになる。上述した参照演算回数の具体例は、一般的に流通している現在の計算機の能力を考慮すると、一般的に、実用上また実装上無視できないオーダの計算量となっている。
【００１６】
このような、処理量が膨大になるという基本手法によるマッチング方法の問題点を解決する方法として、例えば非特許文献１には、センシングデータリストセンシングデータリストに基づくマッチング方法が提案されている。この考えに基づく処理について説明する。なお以降本処理を、センシングデータリストに基づくマッチング方法と呼ぶことにする。
【００１７】
ここで説明を簡単にするため、移動ロボットの移動は並進運動だけで、回転運動は行われないものと仮定する。すなわち、推定するパラメータは(x,y)の２つで、θについてはここでは０に固定するものとする。
【００１８】
まず、上述した基本手法によるマッチング方法を考える。ここでは、図１７に示すようなセンシングデータ群４６０と環境地図４６３とが与えられることを想定する。なお以降用いる図では説明を簡単にするため、画素の詳細度を荒くし、センシングデータの数を少なくして解説するものとする。ここで、図１７において、４６１はセンサ原点、４６２はセンシングデータであり、図１７に示した(0)から(3)の数字の順に、第０番目のセンシングデータ、第１番目のセンシングデータ、第２番目のセンシングデータ、第３番目のセンシングデータと呼ぶこととする。
【００１９】
基本手法によるマッチング方法では、図１８に示すように、センシングデータ全体を平行移動させながら環境地図に重ね合わせ、各センシングデータが指し示す、すなわちセンシングデータが重なる画素の値が１であるものの数を、評価値として数えていくことになる。例えば、図１８の(x,y)=(-3,-3)では評価値が０、(x,y)=(-1,-3)では評価値が１である。(x,y)=(-1,-1)では評価値が４となり、評価値として最大値となる。この場合、推定値は、(x,y)=(-1,-1)となる。
【００２０】
次に、センシングデータリストに基づくマッチング方法を説明する。
【００２１】
いま、図１７のセンシングデータ群４６０における第０番目のセンシングデータに着目すると共に、この第０番目のセンシングデータが指し示す環境地図上の画素値が１になるのは、センシングデータ群全体をどこに移動させた場合であるかを考える。図１８からそのような移動は、図１９に示した１０通りであることがわかる。なお、図１９に示す４７０が、第０番目のセンシングデータである。本例では、環境地図上に画素値が１となる点が１０個あるので、移動方法も１０通りあることになる。
【００２２】
次に、このときのセンシングデータの原点位置４７１を、白紙の地図上にプロットすると、その結果は図２０に示すようになる。このように作成されたパターンを参照地図、より詳しくは、第０番目のセンシングデータに関する参照地図４８０と呼ぶこととする。
【００２３】
ところで、この第０番目のセンシングデータに関する参照地図４８０をよく見ると、色のついた画素パターンは、図１７（ｂ）に示した環境地図の画素パターンを図２１（ｄ）に示すベクトル成分４９５だけ平行移動させたものであることがわかる。また、このベクトル成分４９５は、センシングデータ群における第０番目のセンシングデータ４９４とセンサ原点４９３から決まるベクトルの逆方向であることがわかる。
【００２４】
よって、第０番目のセンシングデータを(d0,φ0)とすると、x0=d0・cos(φ0)、y0=d0・sin(φ0)の計算を行うことで、上記ベクトル成分を(x0,y0)の形の直交座標系で表現することができる。これを算出した後に、もとの環境地図をこのベクトル分だけ逆方向に平行移動させてやれば、この第０番目のセンシングデータに関する参照地図４８０が作成されることになる。
【００２５】
このようにして作成された参照地図４８０（第０番目のデータに関する参照地図）における、センサ原点４８１がプロットされた画素の意味は次のようになる。それは、「この位置に原点位置がくるようにセンシングデータを平行移動させてやれば、第０番目のセンシングデータが、環境地図データの１の値を持つ画素に一致する」ということである。
【００２６】
これは、第０番目のセンシングデータについて考えたときであるが、これを別のセンシングデータについても同様に考えることができる。すなわち、第ｎ番目のセンシングデータについて、それとセンサ原点とがなすベクトルを考え、そのベクトル成分だけ環境地図を反対方向に平行移動させることにより、第ｎ番目のセンシングデータに関する参照地図を作成することができる。
【００２７】
具体的には、第０番目のセンシングデータのときと同様に第ｎ番目のセンシングデータが(dn,φn)と得られたときに、まずこの直交座標表現の値(xn,yn)をxn=dn・cos(φn)、yn=dn・sin(φn)の計算を行うことにより求める。次に、この値(xn,yn)の分だけ地図画像を平行移動させる。このようにして作成される第ｎ番目のセンシングデータに関する参照地図は、この中の斜線のついた画素にセンシングデータの原点を平行移動させれば、第ｎ番目のセンシングデータが、環境地図の１の値を持つ画素に一致することを意味することになる。
【００２８】
上の例ではセンシングデータが４つあるので、それぞれについて参照地図、すなわち図２２に示すように、第０番目のセンシングデータに関する参照地図５００、第１番目のセンシングデータに関する参照地図５０１、第２番目のセンシングデータに関する参照地図５０２、第３番目のセンシングデータに関する参照地図５０３の合計４つを準備する。
【００２９】
次に、図２３に示すように、この４つの参照地図を重ね合わせ、それぞれの画素についていくつ色のついた画素が重なるかを足し合わせて数えることを考える。それぞれの参照地図の意味が上に述べたとおりなので、この数字は、それぞれの画素の位置にセンシングデータの原点が移動したときに、４つのセンシングデータの中で、環境地図の１の値を持つ画素と一致するものの数をあらわすことになる。
【００３０】
この足し合わせた値を画素値とした画像データは、例えば図２４に示すようになり、これを評価画像５１０と呼ぶことにする。すなわちこの評価画像５１０における(x,y)の位置の画素値が、上で述べた基本手法によるマッチング方法において、センサを(x,y)だけ移動させたときに算出される評価値をあらわすことになる。よって基本手法によるマッチング方法と同様に、この評価画像５１０の中で最も大きな画素値をもつものを見つけ、その点の画像上での座標位置を調べてやれば、これが求めるべき移動ロボット位置の推定値となることになる。
【００３１】
なお、ここで各参照地図を重ね合わせる処理は、各画像の画素値を調べていくのではなく、予め各参照地図の中で、１の値を持つ画素の位置を (x0,y0),(x1,y1), ... ,(xn,yn)の形で記録しておき、この記録された座標値が示す評価画像の画素の値を、１つずつ増やしていく、すなわちその画素に対して投票していく、という方法で行うものとする。これは後に述べる、参照地図の各画素に対する参照回数を少なくする、すなわち参照地図の中で０の値を持つ画素については参照する必要がなくなる、という効果をもたらすことになる。
【００３２】
この原理に基づき、センシングデータリストに基づくマッチング方法は、以下の手順で処理が行われる。
(1) 考えうるすべてのセンシングデータ位置について、それぞれ参照地図を作成しておく。これはすなわち上の説明で述べたとおり、考えうるすべてのセンシングデータ位置とセンサ原点がなすベクトル毎に、参照地図を作成することになる。なお、参照地図は、実際には画像データの形で記録するのではなく、上に述べたとおり、１の値を持つ画素の位置を順に(x0,y0),(x1,y1), ... ,(xn,yn)のように並べたものとして記録するものとする。
(2) １フレーム分のセンシングデータ群が与えられたときに、各センシングデータ（合計１８１個）それぞれの位置と、センサ原点がなすベクトルを算出し、そのベクトルに関する参照地図を選んでくる（合計１８１枚）。
(3) 選ばれたすべての参照地図を重ね合わせ、画素毎に画素値を足し合わせた画像を、評価画像として作成する。これは、それぞれの画素について色のついているものの数を求めることになる。
(4) 作成された評価画像の中で、最大の画素値をもつ画素の位置(x,y)を探し、このときの(x,y)をマッチングの解とする。
【００３３】
ここで、上記の説明で述べた、考えうるすべてのセンシングデータという部分を補足する。冒頭でも述べたように、距離センサによるセンシングデータは、(d0,φ0),(d1,φ1), ... ,(d180,φ180)の形であらわされる。ここで、角度φiについては、センシング方向が１°ずつに固定されているので、φ0=-90°,φ1=-89°, ... ,φ180=+90°の決まった値になる。
【００３４】
しかし、距離diについては、測定範囲内(0.0m＜di≦4.0m)においてさまざまな値をとりうるので、これは無限に考えられることになる。そこでこれを、予め定められた離散的な値に代表させることを考える。すなわち、代表値として例えば0.1m,0.2m, ... ,4.0mの４０通りを考え、di≦0.15mであれば、di=0.1m、0.15m＜di≦0.25mであれば、di=0.2mのような規則で値を割り当てていく。このようにすれば、センシングデータ(di,φi)のとりうる値の組み合わせは、diについて４０通り、φiについて１８０通りあることになるので、考えうるすべてのセンシングデータは、合計４０×１８０通りの有限個になる。
【００３５】
これは、いわゆるディジタル処理における量子化の操作を行ったことにほかならないが、このような10cm程度の単位で行う量子化に伴う精度の劣化は、例えば位置姿勢推定における初期値の算出といった用途においてはほとんど問題がない。なお、センシングデータの量子化と同様に、参照地図についても、以下の説明では同様に10cm単位での量子化を行うことにする。すなわち画像上の１ピクセルが、実環境における10cm×10cmの領域を示すことになる。
【００３６】
さて、以上の説明においては移動ロボットの移動が平行移動のみ行われることを仮定し、マッチングについては(x,y)の２パラメータのみで行われることとしたが、実際にはこれに加えて回転（姿勢）θに関するパラメータも求める必要がある。これについては、予め考えうるすべての回転θ、すなわちこれについても上に述べた量子化の考え方を導入し離散値としてθ=0°, 1°, 2°, ..., 359°の３６０通りの値を持つものとすると、これらの角度それぞれについて、予めセンシングデータ群全体を回転させ、これをもとに上述の評価画像をそれぞれ作成し（合計３６０枚）、この全ての評価画像の中で、最も値の大きな画素を見つけ、その画像が属するθ値およびその画素の位置(x,y)をもって求める解(x,y,θ)とすることになる。
【００３７】
以上の方法では、181×360×101×101×1/2=約5億回以下の画素値の参照演算が行われる。ここで、「181」、「360」、「101」、「101」の各数字の持つ意味は、基本手法によるマッチング方法のときと同様である。ここで新たに加わる「1/2」は、環境地図の画像内において全体の画素数に対する障害物が存在する画素数のおおよその割合である。すなわち、センシングデータリストに基づくマッチング方法は、基本手法によるマッチング方法に比べて、この数字の分、言い換えると環境地図画像中の障害物が存在しない画素の割合分、参照演算回数が削減されることになる。これがセンシングデータリストによる効果である。
【００３８】
ただし、一方で、予め参照地図を作成しておき、これを１の値を持つ画素の位置情報として(x0,y0),(x1,y1), ..., (xn,yn)のように記録しておかなければならないので、(1)参照地図を作る処理が必要になる、(2)メモリ使用量が膨大になる、という２つの問題が新たに発生する。(1)については、移動ロボットの駆動前に参照地図を予め作成しておけばよいので、実用上の問題は少ないものと考えられる。(2)については、おおよその見積もりで、上の例では必要なメモリ量は数ギガバイト程度と見込まれ、現在の主記憶あるいは補助記憶装置の性能から、実現可能なオーダとなっている。
【００３９】
【非特許文献１】L. M. Paz, P. Pinies, J. Neira, J. D. Tardos, "Global Localization in SLAM in Bilinear Time," In Proc. International Conference on Intelligent Robots and Systems, 2005, pp. 1445-1450.
【発明の開示】
【発明が解決しようとする課題】
【００４０】
距離センサによって測定されたセンシングデータと環境地図とを用いて移動ロボットの位置姿勢を推定する方法に関し、上述した基本手法によるマッチング方法では、処理量が膨大になり計算時間が長くなる問題がある。また、センシングデータリストに基づくマッチング方法では、これに比べ処理量がある程度削減されるものの、移動ロボットの軽量化・低コスト化等の観点から、さらなる計算処理の削減が望まれる。
【００４１】
本発明は上記事情を鑑みてなされたものであり、本発明の目的は、センシングデータリストに基づくマッチング方法において、さらなる計算量の削減を実現することにある。
【課題を解決するための手段】
【００４２】
上記課題を解決するために本発明では、例えば図１に示すような構成により、以下の特徴を有する位置姿勢推定方法を提供する。
【００４３】
すなわち、予め準備された環境地図１０１と、距離センサ１０６によって測定されたセンシングデータ１０７とに基づいて、マッチングにより距離センサ１０６の位置姿勢値１２４を推定する位置姿勢推定方法において、当該位置推定方法は、環境地図１０１からマッチングのための第１の参照データ群１０３および第２の参照データ群１０５を作成する手段１５０と、測定されたセンシングデータ１０７、第１の参照データ群１０３、および第２の参照データ群１０５に基づいて位置姿勢値１２４を推定する手段１７０とを有する位置姿勢推定システムにおいて、参照データ群を作成する手段１５０では、第１の参照データ群１０３を環境地図１０１から作成する処理１０２と、第２の参照データ群１０５を第１の参照データ群１０３の差分処理により作成する処理１０４とを行い、位置姿勢を推定する手段１７０では、センシングデータ１０７に基づいて、第１の参照データ群１０３および第２の参照データ群１０５の中から必要なデータを選択する処理１２０と、選択された複数の参照データと前回作成した評価のためのデータ１２２とを加算することで新たな評価のためのデータ１２２を作成する処理１２１と、評価のためのデータ１２２から位置姿勢推定値１２４を求める処理１２３とを行う。
【発明の効果】
【００４４】
本発明によれば、センシングデータリストに基づくマッチング方法に比べて、さらなる計算量の削減を実現することができる。
【発明を実施するための最良の形態】
【００４５】
以下、本発明の実施の形態について説明する。
【００４６】
図２は、本発明の一実施形態に係る位置姿勢推定システム１０の構成を示すシステム構成図である。位置姿勢推定システム１０は、参照データ作成サーバ２０および移動ロボット３０を備える。参照データ作成サーバ２０は、ＬＡＮ等の通信回線１１に接続されている。移動ロボット３０は、通信回線１１に接続された基地局１２と、無線ＬＡＮ等の通信方式により無線通信を行い、当該基地局１２を介して参照データ作成サーバ２０と通信する。
【００４７】
参照データ作成サーバ２０は、二次元の環境地図から参照地図および後述する差分画像を作成し、作成した参照地図および差分画像を、通信回線１１を介して移動ロボット３０へ送る。
【００４８】
移動ロボット３０は、無線通信装置３１、位置姿勢推定装置３２、距離センサ３３、および移動装置３４を有する。無線通信装置３１は、基地局１２と無線通信を行い、基地局１２を介して、参照データ作成サーバ２０から参照地図および差分画像を受信して位置姿勢推定装置３２に供給する。位置姿勢推定装置３２は、無線通信装置３１から供給された参照地図および差分画像と、距離センサ３３によって測定されたセンシングデータとを用いて、環境地図内における移動ロボット３０の位置および向き（姿勢）を推定する。移動装置３４は、位置姿勢推定装置３２によって算出された、環境地図内における移動ロボット３０の位置および向きに基づき、予め設定された移動プランに従って、移動ロボット３０を移動させる。
【００４９】
図３は、参照データ作成サーバ２０の機能構成の一例を示すブロック図である。参照データ作成サーバ２０は、環境地図格納部２１、参照地図作成部２２、参照地図格納部２３、差分画像作成部２４、差分画像格納部２５、およびデータ送信部２６を備える。
【００５０】
ここで、「背景技術」の欄で述べたセンシングデータリストに基づくマッチング方法において、平行移動だけでなく回転についての推定も同時に行う方法について述べる。すなわち移動ロボットは平行移動に関する自由度（x, y）に加え、回転（姿勢）に関する自由度θについてのパラメータ推定を同時に行うものとする。
【００５１】
本実施形態においても、「背景技術」の欄のセンシングデータリストに基づくマッチング方法の説明で述べた参照地図を用いる。すなわち、センシングデータとして考えられるすべての値(d,φ)毎に、参照地図を予め用意しておくものとする。なお以下の説明では、それぞれの値(d,φ)に関する参照地図を、クラス(d,φ)の参照地図と呼ぶこととする。
【００５２】
ここで、図４（ａ）に示すように、クラス(d,-φ)の参照地図４００が与えられたときに、これがどのような意味を持つかについて説明する。
【００５３】
移動ロボットの回転方向θが０°のときにクラス(d,-φ)の参照地図４００を用いるのは、距離d、方向−φのセンシングデータ４０１（図４（ｂ）参照）を対象にしたときであるが、移動ロボットの回転方向θを考えると、これを次のように解釈することもできる。それは、距離d、方向(-φ-θ)のセンシングデータ４０２（図４（ｃ）参照）が得られたときに、移動ロボットの方向と環境地図の主方向（本実施形態ではｘ軸方向）とのなす角度がθ、すなわち、センシングデータ全体が環境地図に対してθだけ回転した場合である（図４（ｄ）参照）。
【００５４】
このような場合、回転していない(すなわちθ＝０°)状態の距離センサにおいて、距離d、方向-φのセンシングデータが得られたときと全く等価なセンシングが行われることになるので、距離d、方向-φであり、且つ、距離センサ（移動ロボット）の回転方向がθ＝０°のセンシングデータと、距離d、方向(-φ-θ)であり、且つ、距離センサ（移動ロボット）の回転θのセンシングデータとは、計算上は同様に扱うことができる。
【００５５】
一方、「背景技術」の欄で説明したとおり、クラス(d,-φ)の参照地図４００上で色がついた部分（１の値を持つ部分）は、センシングデータとして(d,-φ)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になることを示している。これは、環境地図と移動ロボットとの方向の差θが０°の場合であった。しかし上で述べた通り、移動ロボットが回転することも考慮に入れると、この状況はセンシングデータとして(d,-φ-θ)の値が観測されたときに、移動ロボットが環境地図に対してθだけ回転したときと等価になる。
【００５６】
よって、上記参照地図４００上で１の値を持つ部分の意味は、下記のように３６０通りに解釈することができる。
(0) 移動ロボットの方向θがθ=0°で、なおかつセンシングデータとして(d,-φ)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
(1) 移動ロボットの方向θがθ=1°で、なおかつセンシングデータとして(d,-φ-1°)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
(2) 移動ロボットの方向θがθ=2°で、なおかつセンシングデータとして(d,-φ-2°)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
・
・
・
(359) 移動ロボットの方向θがθ=359°で、なおかつセンシングデータとして(d,-φ-359°)の値が観測されたときに、センサ原点がこの位置にあればそのセンシングデータに関する評価値が１になる。
【００５７】
このような原理に基づき、以下の手順で評価値画像を作成すれば、位置(x,y)に加え回転方向θについても同時に推定できるマッチングが実施できることになる。
【００５８】
まず、参照地図を、図５に示すような(r,φ)の形の極座標表現であらわされる各クラス毎に予め作成しておく。ここでrおよびφはそれぞれr = 0.1m, 0.2m, ..., 4.0m、φ = 0°, 1°, ..., 359°の値をとりうるので、この全ての組み合わせについて並べている。これらの参照地図は、移動ロボットが移動する前で、なおかつ、移動ロボットが移動する部屋の環境地図画像が与えられた後に作成しておくものである。またその作成方法は、「背景技術」の項のセンシングデータリストに基づくマッチング方法の説明で述べた通りである。
【００５９】
次に移動ロボットが移動して、図１４に示したセンシングデータ群４３０が、(d0, -90°)，(d1, -89°)， ... ，(d180, +90°)の１８１個のセンシングデータとして得られたときに、このセンシングデータと図５に示した参照地図２３０から、移動ロボットの位置姿勢を推定する方法を説明する。
(1) まず、移動ロボットの方向θを、θ＝０°と仮定する。
(2) (d0, -90°)，(d1, -89°)，... ，(d180, +90°)の各クラスの参照地図（合計１８１枚）を図５における一連の参照地図群の中から選び出す。なお、選び出す際に、(di,ψ)におけるψの値が０より小さい場合は、これに３６０°を加えてψ+360°とすることで、0°≦ψ＜360°の範囲にψの値が収まるようにする。このようにしても指し示す角度の意味は変わらない。この考え方は、以下の説明における、全ての角度を扱う処理においても同様に用いるものとする。
(3) 選び出された１８１枚の参照地図全てについて、これを各画素毎に足し合わせた評価値画像を作成する。この評価値画像の（x,y）の位置にある画素値が、(x,y,θ)（ただしθ=0°）のパラメータにおける評価値となる。
(4) 次にθ=１°と仮定する。
(5) (d0, -90°-θ)，(d1, -89°-θ)， ... ，(d180, +90°-θ)の各クラスの参照地図（合計１８１枚）を(2)と同様に図５における一連の参照地図群の中から選び出す。
(6) (3)と同様にこれを各画素毎に足し合わせた評価値画像を作成する。この評価値画像の(x,y)の位置にある画素値が、(x,y,θ)のパラメータにおける評価値となる。
(7) θの値を１°増やし、(5)、(6)の処理を行う。これをθ=359°になるまで繰り返す。
(8) 以上の処理により、θ＝０°，１°，・・・，３５９°の合計３６０枚の評価値画像が作成されるが、この中で最も評価値が大きい画素を見つけ、この画素の環境地図上の位置(x,y)と、その評価値が属するθの値の組み合わせ(x,y,θ)が推定値となる。
【００６０】
以上述べた方法は、本発明において、「背景技術」の欄で述べたセンシングデータリストに基づく方法を用いて、移動ロボットの回転方向θを移動ロボットの平行移動量(x,y)と同時に求める方法である。本方法を用いるだけでは、「背景技術」の欄で述べたセンシングデータリストに基づく方法に比べて特に効果は見出せないが、以下に述べる差分処理を用いる方法の説明をわかりやすくするためにここで詳しく述べた。なおこの方法を、センシングデータリストに基づく回転を含めたマッチング方法、と呼ぶことにする。
【００６１】
次に、本発明において、差分処理を用いる方法について述べる。これは一言で言うと、上に述べたセンシングデータリストに基づく回転を含めたマッチング方法の考え方を基本にして、これに、参照地図に関する差分処理を加えるものである。
【００６２】
まず、図６に示すように、２つの参照地図４１０および参照地図４１１から、差分画像４１２を作成することを考える。
【００６３】
ここで、差分画像４１２は、距離パラメータｒが同じ値を持ち、角度パラメータφが隣り合う値を持つ組みを選んで作成するものとする。すなわち、クラス(r,ψ)の参照地図と、クラス(r,ψ+1°)の参照地図を選ぶことになる（ここでr = 0.1, 0.2, ..., 4.0m、ψ = 0°, 1°, ..., 359°である）。これによって作成される差分画像を、クラス (r,ψ+1°)の差分画像と呼ぶこととする。
【００６４】
クラス(r,ψ)の差分画像G(r,ψ)は、クラス(r,ψ)の参照地図F(r,ψ)を用いて、下記の数式１により算出することができる。
【００６５】
G(r,ψ+1°)(x, y) = F(r,ψ+1°)(x, y) - F(r,ψ)(x, y) ・・・数式１
なお、(x,y)は、それぞれの画像における座標を示している。
【００６６】
上に述べた通り、ｒの値としてはr = 0.1m, 0.2m, ..., 4.0mの４０通り、ψの値としては、ψ= 0°, 1°, ..., 359°の３６０通りあるので、この差分画像Ｇは全部で ４０×３５９枚作成されることになる。
【００６７】
ところで、差分画像Ｇを作るときに用いられる隣り合う角度、すなわちψとψ+1°のそれぞれの角度からなる２つのクラス(r,ψ+1°)と(r,ψ)の参照地図を考えたときに、前者の画像は、後者の画像を１°だけ微小に回転させたものであるので、画像の形状が非常に似たようなものになるという性質をもっている。このため上の式に基づいて差分画像Ｇを作成すると、２つの画像において各画素値が同一であるものが多いため、画像中の各画素の多くの部分の差分値は０になることになる。
【００６８】
本発明では、差分画像Ｇについて、全ての画素の画素値を差分画像Ｇとして保持するのではなく、０以外の画素値を持つ画素について、その画素の位置と画素値を差分画像として保持する。具体的には、対応する画素の画素値をvkとして、(x0, y0, v0), (x1, y1, v1), ..., (xn, yn, vn) の形で記録しておくものとする。これにより、差分画像Ｇの各画素に対する参照回数を少なくする、すなわち差分画像Ｇの中で０の値を持つ画素については参照する必要がなくなる、という効果をもたらすことになる。従って、データ量の削減および演算量の削減を実現することができる。
【００６９】
図３に戻り、説明を続ける。環境地図格納部２１には、例えば図１５を用いて説明した二次元の環境地図が格納される。参照地図作成部２２は、環境地図格納部２１に格納されている環境地図から、r（本実施形態では、r = 0.1m, 0.2m, ..., 4.0m）およびφ（本実施形態では、φ = 0°, 1°, ..., 359°）のそれぞれのクラスに対応する参照地図を、合計４０×３６０枚作成し、作成した参照地図を参照地図格納部２３に格納する。参照地図格納部２３には、図５で説明した参照地図２３０が格納される。
【００７０】
差分画像作成部２４は、上述した数式１に基づいて、参照地図格納部２３に格納されている参照地図を用いて、各クラスの差分画像を作成し、作成した差分画像を差分画像格納部２５に格納する。差分画像格納部２５には、例えば図７に示すように、クラス２５０毎に、当該クラスに対応する差分画像中で画素値が０でない画素の位置を示す画素位置２５１、および、当該画素位置２５１にある画素の画素値２５２が格納される。
【００７１】
データ送信部２６は、環境地図格納部２１に格納されている環境地図、参照地図格納部２３に格納されている参照地図、および差分画像格納部２５に格納されている差分画像を、通信回線１１を介して移動ロボット３０へ送信する。
【００７２】
図８は、位置姿勢推定装置３２の機能構成の一例を示すブロック図である。位置姿勢推定装置３２は、データ取得部３２０、データ格納部３２１、第一の評価画像作成部３２２、統合差分画像作成部３２３、第一の評価値算出部３２４、第二の評価画像作成部３２５、位置姿勢推定部３２６、評価値格納部３２７、および第二の評価値算出部３２８を備える。
【００７３】
ここで、本発明において差分画像を用いてマッチングを行う方法を述べる。ここでは、各クラス(rj,ψk)（rj = 0.1m, 0.2m, 0.3m, ..., 4.0m、ψk = 0°, 1°, ..., 359°）の参照地図、ならびにクラス(rj,ψk)（rj = 0.1m, 0.2m, 0.3m, ..., 4.0m、ψk = 1°, 2°, ..., 359°）の差分画像が予め作成されており、センシングデータとして(d0 ,-90°)，(d1 ,-89°), ..., (d180 ,+90°)の１８１個のデータが得られていることを想定する。
(1) まず、移動ロボットの方向θを、θ＝０°と仮定する。
(2) (d0 ,-90°), (d1 ,-89°), ..., (d180 ,+90°)の各クラスの参照地図（合計１８１枚）を一連の参照地図群の中から選び出す。なお、角度は適宜３６０°を加えることで、０°以上３６０°未満の範囲に収まるようにするのは、上で述べた通りである。
(3) 選び出された１８１枚全ての参照地図について、これを各画素毎に足し合わせた評価値画像を作成する。この評価値画像の(x,y)の位置にある画素値が、(x,y,θ)（ただしθ=0°）のパラメータにおける評価値となる。
(4) 次にθ＝１°と仮定する。
(5) (d0 ,-90°-θ)，(d1 ,-89°-θ)，...，(d180 ,+90°-θ)の各クラスの参照地図（合計１８１枚）を、今度は参照地図ではなく、差分画像群の中から選び出す。
(6) 選びだされた全ての差分画像について、これを各画素毎に足し合わせた画像データを作成する。このように作成された画像データを、統合差分画像と呼ぶ。
(7) 作成された統合差分画像と、前回、すなわち(3)で作成されたθ=0°のときの評価値画像を、各画素毎に足し合わせた画像データを作成する。これが移動ロボットの回転角度がθのときの評価値画像となる。すなわち、この評価値画像の(x,y)の位置にある画素値が、(x,y,θ)のパラメータにおける評価値となる。
(8) θの値を１° 増やし、(5), (6), (7) の処理を行う。これをθ=359°になるまで繰り返す。
(9) 以上の処理により、θ= 0°, 1°, ..., 359°の合計360枚の評価値画像が作成されるが、この中で最も値が大きい画素を見つけ、この画素の画像上の位置(x,y)と、その画像が属するθの値の組み合わせ(x,y,θ)が、パラメータの推定値となる。
【００７４】
本発明による差分画像を用いる方法では、センシングデータリストに基づく回転を含めたマッチング方法に比べ、計算時に用いる画像データの枚数は変わらない。具体的には、センシングデータリストに基づく回転を含めたマッチング方法では、θ= 0°, 1°, ..., 359°のときにそれぞれ(d0 ,-90°-θ), (d1 ,-89°-θ), ..., (d180 ,+90°-θ) の合計１８１の異なるクラスの参照地図を用いることとなり、扱う総画像枚数は１８１×３６０枚となる。
【００７５】
これに対して、本発明による差分画像を用いる方法では、θ=０°ときの(d0 ,-90°), (d1 ,-89°), ..., (d180 ,+90°)の合計１８１枚の異なるクラスの参照地図と、θ= 1°, 2°, ..., 359°のそれぞれのときに(d0 ,-90°-θ), (d1 ,-89°-θ), ..., (d180 ,+90°-θ) の合計１８１枚の異なる差分画像を用いるので、扱う総画像枚数は１８１×（１＋３５９）枚となる。
【００７６】
このように、計算に利用する画像の枚数は、センシングデータリストに基づく回転を含めたマッチング方法も、本発明による差分画像を用いる方法も変わらないことになる。しかし、それぞれの画像データに着目したとき、０以外の値を持つ画素の数、すなわち計算に用いられる画素の数という点において、参照地図に比べて、差分画像は大幅に削減されることになる。そのため、計算に用いられる画素の数はこの削減分と同じだけ削減されることになり、計算処理量もこれに比例して削減されることになる。
【００７７】
これは、差分画像を作成するときに用いられる隣り合う角度の２つのクラスの参照地図が非常に似ているために、差分画像における大部分の画素値が０になる、という性質によってもたらされるものである。もし、この差分画像における０以外の値を持つ画素の数が、もとの参照地図の０以外の値を持つ画素の数の例えば１／３になれば、マッチング方法における画素値の参照演算の回数は以下のとおりとなる。
181×360×101×101×1/2×1/3 （回）
ここで、「181」、「360」、「101」、「101」、「1/2」の各数値の持つ意味は、センシングデータリストに基づくマッチング方法のときと同様である。これに新たに加わる「1/3」の数値が、上に述べた０以外の値を持つ画素の数の削減割合である。すなわち、本発明における差分処理を用いた方法は、この数字の分、参照演算数が処理が削減されることになる。これが本発明における差分処理の効果である。
【００７８】
図８に戻って、位置姿勢推定装置３２の機能構成の説明を続ける。データ取得部３２０は、無線通信装置３１を介して参照データ作成サーバ２０から、環境地図、各クラスの参照地図、および各クラスの差分画像を取得し、取得した環境地図、参照地図、および差分画像をデータ格納部３２１に格納する。
【００７９】
第一の評価画像作成部３２２は、１８１個のセンシングデータ(di,φi)(i=0,1,・・・,180)を距離センサ３３から取得する。そして、第一の評価画像作成部３２２は、取得したそれぞれのセンシングデータ(di,φi)について、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)をデータ格納部３２１から抽出する。そして、第一の評価画像作成部３２２は、抽出した１８１個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される評価画像(θ0)を作成する。評価画像(θ0)は、移動ロボット３０の向きと環境地図において予め定められた方向（本実施形態では図１６におけるｘ軸方向）とのなす角がθ0（本実施形態ではθ0＝０°）であると仮定した場合の評価画像を意味する。
【００８０】
第一の評価値算出部３２４は、第一の評価画像作成部３２２によって作成された評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として評価値格納部３２７に格納する。評価値格納部３２７には、例えば図９に示すように、環境地図内の画素の位置を示すｘ座標３２７０およびｙ座標３２７１に対応付けて、当該位置にある画素の評価値３２７３、および、当該評価値３２７３が算出される際に仮定された、環境地図における所定の方向に対する移動ロボット３０の向きを示す方向３２７２が格納される。
【００８１】
統合差分画像作成部３２３は、距離センサ３３によって測定された１８１個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を、データ格納部３２１から抽出する。そして、統合差分画像作成部３２３は、抽出した１８１個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される統合差分画像(θj)を作成する。統合差分画像(θj)は、移動ロボット３０の向きと環境地図において予め定められた方向とのなす角がθjであると仮定した場合の統合差分画像を意味する。
【００８２】
第二の評価画像作成部３２５は、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する。第二の評価値算出部３２８は、第二の評価画像作成部３２５によって作成された評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として評価値格納部３２７に格納する。
【００８３】
なお、統合差分画像作成部３２３、第一の評価値算出部３２４、および第二の評価画像作成部３２５は、第一の評価画像作成部３２２によって評価画像(θ0)が作成された後に、j=1からj=m-1まで、統合差分画像(θj)を作成する処理、評価画像(θj)を作成する処理、および評価画像(θj)内の(x,y)の位置にある画素の評価値を格納する処理を、それぞれ実行する。
【００８４】
位置姿勢推定部３２６は、評価値格納部３２７を参照して、最大の評価値が対応付けられている(x,y,θ)を特定し、環境地図の所定方向とのなす角度がθの向きで、環境地図内の(x,y)の位置に、移動ロボット３０の向きおよび位置を推定し、推定した向きおよび位置を移動装置３４に通知する。
【００８５】
図１０は、参照データ作成サーバ２０の動作の一例を示すフローチャートである。例えば環境地図が環境地図格納部２１に格納される等の所定のタイミングで、参照データ作成サーバ２０は、本フローチャートに示す動作を開始する。
【００８６】
まず、参照地図作成部２２は、ｊの値を１に、ｋの値を０に、それぞれ初期化し（Ｓ１００）、環境地図格納部２１を参照して、環境地図からクラス(rj,ψk)の参照地図を作成し、作成した参照地図を参照地図格納部２３に格納する（Ｓ１０１）。そして、参照地図作成部２２は、ｊの値が４０か否かを判定し、ｊの値が４０未満の場合（Ｓ１０２：Ｎｏ）、ｊの値を１増やし（Ｓ１０３）、再びステップＳ１０１に示した処理を実行する。
【００８７】
ｊの値が４０の場合（Ｓ１０２：Ｙｅｓ）、参照地図作成部２２は、ｊの値を０に初期化すると共に、ｋの値を１増やし（Ｓ１０４）、ｋの値が１８１よりも大きいか否かを判定する（Ｓ１０５）。ｋの値が１８１以下の場合（Ｓ１０５：Ｎｏ）、参照地図作成部２２は、再びステップＳ１０１に示した処理を実行する。
【００８８】
ｋの値が１８１よりも大きい場合（Ｓ１０５：Ｙｅｓ）、差分画像作成部２４は、ｊおよびｋの値を１に初期化し（Ｓ１０６）、参照地図格納部２３を参照して、下記の数式２を用いて、クラス(rj,ψk)における差分画像を算出し、算出した差分画像(rj,ψk)を差分画像格納部２５に格納する（Ｓ１０７）。
【００８９】
差分画像(rj,ψk)＝参照地図(rj,ψk)−参照地図(rj,ψk-1) ・・・数式２
次に、差分画像作成部２４は、ｋの値が１８１であるか否かを判定し、ｋの値が１８１未満の場合（Ｓ１０８：Ｎｏ）、ｋの値を１増やし（Ｓ１０９）、再びステップＳ１０７に示した処理を実行する。ｋの値が１８１の場合（Ｓ１０８：Ｙｅｓ）、差分画像作成部２４は、ｊの値を１増やすと共に、ｋの値を１に初期化し（Ｓ１１０）、ｊの値が４０よりも大きいか否かを判定する（Ｓ１１１）。
【００９０】
ｊの値が４０以下の場合（Ｓ１１１：Ｎｏ）、差分画像作成部２４は、再びステップＳ１０７に示した処理を実行する。ｊの値が４０より大きい場合（Ｓ１１１：Ｙｅｓ）、データ送信部２６は、環境地図格納部２１に格納されている環境地図、参照地図格納部２３に格納されている参照地図、および差分画像格納部２５に格納されている差分画像を、通信回線１１を介して移動ロボット３０へ送信し（Ｓ１１２）、参照データ作成サーバ２０は、本フローチャートに示した動作を終了する。
【００９１】
図１１は、位置姿勢推定装置３２の動作の一例を示すフローチャートである。移動ロボット３０は、環境地図、参照地図、および差分画像を、参照データ作成サーバ２０から受信した後に、所定のタイミングで本フローチャートに示す動作を開始する。
【００９２】
まず、第一の評価画像作成部３２２は、ｉの値を０に初期化し（Ｓ２００）、距離センサ３３によって測定されたセンシングデータ(di,φi)に対応する参照地図(di,φi)をデータ格納部３２１から抽出する（Ｓ２０１）。そして、第一の評価画像作成部３２２は、ｉの値が１８０であるか否かを判定し、ｉの値が１８０未満である場合には（Ｓ２０２：Ｎｏ）、ｉの値を１増やし（Ｓ２０３）、再びステップＳ２０１に示した処理を実行する。
【００９３】
ｉの値が１８０である場合（Ｓ２０２：Ｙｅｓ）、第一の評価画像作成部３２２は、抽出した参照地図(di,φi)を画素毎に加算することにより、評価画像(θ0)を作成する（Ｓ２０４）。そして、第一の評価値算出部３２４は、第一の評価画像作成部３２２によって作成された評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として評価値格納部３２７に格納する（Ｓ２０５）。
【００９４】
次に、統合差分画像作成部３２３、第二の評価画像作成部３２５、および第二の評価値算出部３２８は、ｉの値を０に、ｊの値を１に、それぞれ初期化する（Ｓ２０６）。そして、統合差分画像作成部３２３は、距離センサ３３によって測定されたセンシングデータ(di,φi-θj)に対応する差分画像(di,φi-θj)をデータ格納部３２１から抽出し（Ｓ２０７）、ｉの値が１８０であるか否かを判定する（Ｓ２０８）。ｉの値が１８０未満である場合（Ｓ２０８：Ｎｏ）、統合差分画像作成部３２３は、ｉの値を１増やし（Ｓ２０３）、再びステップＳ２０７に示した処理を実行する。
【００９５】
ｉの値が１８０である場合（Ｓ２０８：Ｙｅｓ）、統合差分画像作成部３２３は、抽出した１８１個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される統合差分画像(θj)を作成する（Ｓ２１０）。そして、第二の評価画像作成部３２５は、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する（Ｓ２１１）。
【００９６】
次に、第二の評価値算出部３２８は、第二の評価画像作成部３２５によって作成された評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として評価値格納部３２７に格納し（Ｓ２１２）、ｊの値が３５９であるか否かを判定する（Ｓ２１３）。ｊの値が３５９未満である場合（Ｓ２１３：Ｎｏ）、統合差分画像作成部３２３は、ｊの値を１増やし（Ｓ２１４）、再びステップＳ２０７に示した処理を実行する。
【００９７】
ｊの値が３５９である場合（Ｓ２１３：Ｙｅｓ）、位置姿勢推定部３２６は、評価値格納部３２７を参照して、最大の評価値が対応付けられている(x,y,θ)を特定し、環境地図の所定方向とのなす角度がθの向きで、環境地図内の(x,y)の位置に、移動ロボット３０の向きおよび位置を推定し（Ｓ２１５）、位置姿勢推定装置３２は、本フローチャートに示した動作を終了する。
【００９８】
図１２は、参照データ作成サーバ２０または位置姿勢推定装置３２の機能を実現するコンピュータ４０のハードウェア構成の一例を示すハードウェア構成図である。コンピュータ４０は、ＣＰＵ（Central Processing Unit）４１、ＲＡＭ（Random Access Memory）４２、ＲＯＭ（Read Only Memory）４３、ＨＤＤ（Hard Disk Drive）４４、通信インターフェイス（Ｉ／Ｆ）４５、入出力インターフェイス（Ｉ／Ｆ）４６、およびメディアインターフェイス（Ｉ／Ｆ）４７を備える。
【００９９】
ＣＰＵ４１は、ＲＯＭ４３またはＨＤＤ４４に格納されたプログラムに基づいて動作し、各部の制御を行う。ＲＯＭ４３は、コンピュータ４０の起動時にＣＰＵ４１が実行するブートプログラムや、コンピュータ４０のハードウェアに依存するプログラム等を格納する。ＨＤＤ４４は、ＣＰＵ４１によって実行されるプログラムを格納する。通信インターフェイス４５は、通信回線を介して他の機器からデータを受信してＣＰＵ４１へ送ると共に、ＣＰＵ４１が生成したデータを、通信回線を介して他の機器へ送信する。
【０１００】
ＣＰＵ４１は、入出力インターフェイス４６を介して、モニタ等の出力装置、および、キーボードやマウス等の入力装置を制御する。ＣＰＵ４１は、入出力インターフェイス４６を介して、入力装置からデータを取得する。また、ＣＰＵ４１は、生成したデータを、入出力インターフェイス４６を介して出力装置へ出力する。なお、コンピュータ４０が位置姿勢推定装置３２として機能する場合、コンピュータ４０には通信インターフェイス４５が設けられず、コンピュータ４０は、入出力インターフェイス４６に接続された無線通信装置３１を介して通信を行う。また、コンピュータ４０が位置姿勢推定装置３２として機能する場合、距離センサ３３および移動装置３４とのデータやり取りは、入出力インターフェイス４６を介して行われる。
【０１０１】
メディアインターフェイス４７は、記録媒体４８に格納されたプログラムまたはデータを読み取り、ＲＡＭ４２に提供する。ＲＡＭ４２を介してＣＰＵ４１に提供されるプログラムは、記録媒体４８に格納されている。当該プログラムは、記録媒体４８から読み出されて、ＲＡＭ４２を介してコンピュータ４０にインストールされ、ＣＰＵ４１によって実行される。記録媒体４８は、例えばＤＶＤ（Digital Versatile Disk）、ＰＤ（Phase change rewritable Disk）等の光学記録媒体、ＭＯ（Magneto-Optical disk）等の光磁気記録媒体、テープ媒体、磁気記録媒体、または半導体メモリ等である。
【０１０２】
コンピュータ４０が参照データ作成サーバ２０として機能する場合、コンピュータ４０にインストールされて実行されるプログラムは、コンピュータ４０を、環境地図格納部２１、参照地図作成部２２、参照地図格納部２３、差分画像作成部２４、差分画像格納部２５、およびデータ送信部２６として機能させる。
【０１０３】
また、コンピュータ４０が位置姿勢推定装置３２として機能する場合、コンピュータ４０にインストールされて実行されるプログラムは、コンピュータ４０を、データ取得部３２０、データ格納部３２１、第一の評価画像作成部３２２、統合差分画像作成部３２３、第一の評価値算出部３２４、第二の評価画像作成部３２５、位置姿勢推定部３２６、評価値格納部３２７、および第二の評価値算出部３２８として機能させる。
【０１０４】
コンピュータ４０は、これらのプログラムを、記録媒体４８から読み取って実行するが、他の例として、他の装置から、通信媒体を介してこれらのプログラムを取得してもよい。通信媒体とは、有線もしくは無線の通信回線、または、当該通信回線を伝搬するディジタル信号もしくは搬送波を指す。
【０１０５】
以上、本発明の実施の形態について説明した。
【０１０６】
上記説明から明らかなように、本実施形態の位置姿勢推定システム１０によれば、センシングデータリストに基づくマッチング方法に比べて、さらなる計算量の削減を実現することができる。
【０１０７】
ここで、今まで述べた説明をもとに、「課題を解決するための手段」の欄で図１を参照して述べた本発明の構成と、上記した実施形態との対応関係を補足する。
【０１０８】
図１において、環境地図１０１は、「背景技術」の項で説明したように、距離センサ１０６による測定高さ断面での障害物の存在を、ディジタル画像としてあらわしたものである。ここでは、障害物が存在しない場所を画素値0（白色）、障害物が存在する場所を画素値1（黒色）としてあらわすことにする。
【０１０９】
距離センサ１０６は、「背景技術」の項で説明したように、自分の周辺の予め決められた複数の方向について、障害物までの距離を計測できるものである。ここでは、測定は正面に対して-90°から+90°に１°毎の合計１８１の異なる方向について行われるものとする。
【０１１０】
センシングデータ１０７は、「背景技術」の項で説明したように、上記距離センサ１０６によって障害物の位置が、角度と距離の組み合わせとして(d0 ,-90°)，(d1 ,-89°)， ...,，(d180 ,+90°)の合計１８１個の極座標表現によるデータとしてあらわされるものである。また、d0, d1, ..., d180 は、0.1m, 0.2m, 0.3m, ..., 4.0mのいずれかの値として量子化されているものである。
【０１１１】
位置姿勢推定値１２４とは、「背景技術」の欄で説明したように、移動ロボットが平面運動を行うことを想定するので、位置と方向に関する合計３パラメータ(x, y,θ)としてあらわされるものである。
【０１１２】
まず、図１における参照データ群を作成する手段１５０において、環境地図からマッチングのための第１の参照データ群１０３を作成する方法について説明する。ここで第１の参照データ群とは、「背景技術」の項で説明した一連の参照地図を示す。具体的には、(rj,ψk)（ただし、rj = 0.1m, 0.2m, 0.3m, ..., 4.0mの４０通り、ψk = 0°, 1°, ..., 359°の３６０通り）のクラスとして作成するので、合計４０×３６０枚になる。このデータは、環境地図を(x,y)の方向に移動させることで作成される。なお、x = rj・cos(ψk), y = rj・sin(ψk) である。
【０１１３】
また、各画素は実環境における１０ｃｍ×１０ｃｍの大きさの領域を示すように量子化されるものとする。さらに、この参照地図は、画像そのものではなく、画像上において、1の値を持つ画素の座標値を列挙したデータとして、(x0, y0), (x1, y1), ..., (xn, yn) のように記録されるものとする。
【０１１４】
次に、図１における参照データ群を作成する手段１５０において、第１の参照データ群から第２の参照データ群１０５を作成する方法について説明する。ここで、第２の参照データ群とは、第１の実施例で説明した一連の差分画像を示す。具体的には、(rj,ψk)（ただし、rj = 0.1m, 0.2m, 0.3m, ..., 4.0mの４０通り、ψk = 1°, 2°, ..., 359°の３５９通りのクラスとして作成するので、合計４０×３５９枚になる。このデータは、上に述べた参照地図の中で角度パラメータについて隣り合うクラスのものを２つ組み合わせ、この画像の画素毎の差分処理を行うことで作成される。
【０１１５】
すなわち、クラス(rj,ψ+1°)と、クラス(rj,ψ)の参照地図から、クラス(rj,ψ+1°)の差分画像が作成されることになる。その具体的な作成方法は、発明を実施するための最良の形態の前半部分で説明した通りである。この差分画像は、画像そのものではなく、画像上において、０以外の値をもつ画素の座標値と、その値を列挙したデータとして、(x0, y0 v0), (x1, y1, v1), ..., (xn, yn, vn) のように記録されるものとする。
【０１１６】
次に、図１における位置姿勢を推定する手段１７０における、センシングデータに基づいて、第１の参照データ群と第２の参照データ群の中から必要なデータを選択する処理について説明する。ここでは、移動ロボットの回転をθと仮定し、この値とセンシングデータとから算出される値に基づき、第１の参照データ群、すなわち各クラスの参照地図と、第２の参照データ群、すなわち各クラスの差分画像から、適切なクラスのものを選択する。選択されるものは複数枚になる。その具体的な選択方法は、発明を実施するための最良の形態の前半部分で説明したとおりである。
【０１１７】
次に、図１の位置姿勢を推定する手段１７０における、選択されたデータを加算することで評価のためのデータ１２２を作成する処理１２１について説明する。ここでは、仮定した移動ロボットの回転θの値毎に、上で選択した各クラスの参照地図と差分画像を画素毎に足し合わせ、さらにこれに、移動ロボットの回転(θ-1°)として作成した評価値画像を足し合わせ、これを移動ロボットの回転θの評価値画像とする。その具体的な方法は、発明を実施するための最良の形態の前半部分で説明したとおりである。なお、この評価値画像が、図１における評価のためのデータとなる。
【０１１８】
次に、図１の位置姿勢を推定する手段１７０における、評価のためのデータから位置姿勢推定値を求める処理１２３について説明する。これは、上で求められた各移動ロボットの回転θ毎に算出された評価値画像の中から、最大の値をもつ画素を見つけ出し、これが属する回転角度θと、その画素の画像上の位置(x,y)から、位置姿勢推定値を(x,y,θ)として求めるものである。その具体的な方法は、発明を実施するための最良の形態の前半部分で説明した通りである。
【０１１９】
また、本発明は、上記した実施形態に限定されるものではなく、その要旨の範囲内で数々の変形が可能である。
【０１２０】
例えば、上記した実施形態において、位置姿勢推定装置３２は、パラメータ推定時には、参照データ作成サーバ２０によって予め作成された参照地図および差分画像の両方を利用するが、本発明はこれに限られない。他の形態として、位置姿勢推定装置３２は、参照データ作成サーバ２０によって予め作成された差分画像と一部の参照地図とを用いて各パラメータを推定するようにしてもよい。
【０１２１】
例えば、参照データ作成サーバ２０は、参照地図としてψ＝０°のクラス、すなわちクラス（0.1m, 0°), クラス(0.2m, 0°), ..., クラス(4.0m, 0°)の参照地図を作成し、位置姿勢推定装置３２は、参照データ作成サーバ２０によって作成されたクラスの参照地図をデータ格納部３２１に保存する。これは、上記した実施形態においてψ= 0°, 1°, ..., 359°の全てのクラスの参照地図を記録しておくのに比べて１／３６０のメモリ量となる。なお、差分画像については、参照データ作成サーバ２０は、上記した実施形態と同様に全てのクラスについて差分画像を作成し、位置姿勢推定装置３２は、参照データ作成サーバ２０によって作成された全てのクラスの差分画像をデータ格納部３２１に保存する。
【０１２２】
このような準備のもと、第一の評価画像作成部３２２は、以下のステップに従い、方向θをθ＝０°と仮定したときの評価値画像を作成する。
(1) θ＝０°とする。
(2) センシングデータ(d0 ,-90°), (d1,-89°), ..., (d180 ,+90°)について、距離パラメータはそれぞれの値とし、角度パラメータは０°に置き換えたクラスの参照地図を選び出す。すなわち(d0 ,0°), (d1 ,0°), ..., (d180 ,0°)のクラスの参照地図を選び出すことになる。
(3) これらのデータについて、角度パラメータがセンシングデータのものになるまで、差分画像を足し合わせていく。この足し合わせた画像データが、そのクラスの参照地図となる。例えば、(d0 ,-90°)のセンシングデータについては、-90° → -90°+ 360° = +270°の変換処理を行ったのちに、以下の計算を行うことで、目的となる (d0 ,+270°)のクラスの参照地図を作成する。
【０１２３】
F(d0 ,270°) = F(d0 ,0°) + G(d0 ,1°) + G(d0 ,2°) + ... + G(d0 ,270°)
・・・数式３ ここで、F(d,ψ)およびG(d,ψ) は、それぞれ、クラス(d,ψ)の参照地図およびクラス(d,ψ)の差分画像である。
【０１２４】
以上の処理により、θ＝０°のときの各クラスの参照地図が求められるので、それ以降は、この参照地図に、それぞれの角度における統合差分画像を加算していくことにより、目的となる推定値が求められることになる。このようにすることにより、センシングデータに対応する各クラスの参照地図を作成する処理が新たに必要になるものの、データ格納部３２１内に参照地図の大部分を記憶する必要がないため、メモリ量を削減することができる。
【０１２５】
また、上記した実施形態において、移動ロボット３０は、有線または無線通信により、環境地図、参照地図、および差分画像を参照データ作成サーバ２０から取得するが、他の形態として、移動ロボット３０は、可搬性の記録媒体を介して、これらのデータを参照データ作成サーバ２０から取得するようにしてもよい。
【０１２６】
また、上記した実施形態において、位置姿勢推定システム１０は、参照データ作成サーバ２０と移動ロボット３０とを備えるが、他の形態として、参照データ作成サーバ２０の機能を移動ロボット３０内に設けることにより、移動ロボット３０単体で位置姿勢推定システム１０の機能を実現するように構成されていてもよい。
【０１２７】
また、上記した実施形態において用いられている様々な具体的な数値は、説明に対する理解を助けるために仮に設定したものであり、本発明の実施時にその値が使われることを限定するものではない。例えば、上記した実施形態では、角度の分解能が１°であったが、本発明を適用するシステムの仕様や要求される精度等に応じて、０．５°や０．１°等であってもよい。
【図面の簡単な説明】
【０１２８】
【図１】本発明の位置姿勢推定システム１０の全体構成の一例を説明するための概念図である。
【図２】本発明の一実施形態に係る位置姿勢推定システム１０の構成を示すシステム構成図である。
【図３】参照データ作成サーバ２０の機能構成の一例を示すブロック図である。
【図４】参照地図４００の意味を説明するための概念図である。
【図５】参照地図格納部２３に格納されるデータ構造の一例を説明するための概念図である。
【図６】差分画像４１２の作成過程を説明するための概念図である。
【図７】差分画像格納部２５に格納されるデータ構造の一例を示す図である。
【図８】位置姿勢推定装置３２の機能構成の一例を示すブロック図である。
【図９】評価値格納部３２７に格納されるデータ構造の一例を示す図である。
【図１０】参照データ作成サーバ２０の動作の一例を示すフローチャートである。
【図１１】位置姿勢推定装置３２の動作の一例を示すフローチャートである。
【図１２】参照データ作成サーバ２０の機能を実現するコンピュータ４０の一例を示すハードウェア構成図である。
【図１３】移動ロボット４２０の動作概要を説明するための概念図である。
【図１４】距離センサ４２１によって測定されたセンシングデータ群４３０の一例を示す図である。
【図１５】環境地図４４０の一例を示す図である。
【図１６】移動ロボットの位置を特定する過程を説明するための概念図である。
【図１７】センシングデータ群４６０および環境地図４６３の一例を示す概念図である。
【図１８】基本手法によるマッチング方法の過程を説明するための概念図である。
【図１９】第０番目のセンシングデータ４７０と環境地図内の黒い画素とが一致する場合を抜き出した図である。
【図２０】第０番目のセンシングデータ４７０と環境地図内の黒い画素とが一致する場合のセンサ原点４８１の位置をプロットした図である。
【図２１】図２０と環境地図との関係を説明するための概念図である。
【図２２】各センシングデータと参照地図との関係を説明するための概念図である。
【図２３】評価画像が作成される過程を説明するための概念図である。
【図２４】評価画像５１０の一例を示す図である。
【符号の説明】
【０１２９】
１０・・・位置姿勢推定システム、１０１・・・環境地図、１０２・・・第１の参照データ群の作成処理、１０３・・・第１の参照データ群、１０４・・・第２の参照データ群の作成処理、１０５・・・第２の参照データ群、１０６・・・距離センサ、１０７・・・センシングデータ、１２０・・・参照データ選択処理、１２１・・・加算処理、１２２・・・評価のためのデータ、１２３・・・位置姿勢推定値を求めるための処理、１２４・・・位置姿勢推定値、１５０・・・参照データ群を作成する手段、１７０・・・位置姿勢を推定する手段、１１・・・通信回線、１２・・・基地局、２０・・・参照データ作成サーバ、２１・・・環境地図格納部、２２・・・参照地図作成部、２３・・・参照地図格納部、２４・・・差分画像作成部、２５・・・差分画像格納部、２６・・・データ送信部、３０・・・移動ロボット、３１・・・無線通信装置、３２・・・位置姿勢推定装置、３２０・・・データ取得部、３２１・・・データ格納部、３２２・・・第一の評価画像作成部、３２３・・・統合差分画像作成部、３２４・・・第一の評価値算出部、３２５・・・第二の評価画像作成部、３２６・・・位置姿勢推定部、３２７・・・評価値格納部、３２８・・・第二の評価値算出部、３３・・・距離センサ、３４・・・移動装置
【特許請求の範囲】
【請求項１】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定システムにおける位置姿勢推定方法であって、
前記位置姿勢推定システムは、
参照データ作成サーバと、
前記距離センサが搭載された装置に搭載される位置姿勢推定装置と
を備え、
前記参照データ作成サーバが、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成ステップと、
作成した参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成ステップと
を実行し、
前記位置姿勢推定装置が、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を、前記参照データ作成サーバによって算出された参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成ステップと、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出ステップと、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を、前記参照データ作成サーバによって作成された差分画像の中から抽出し、抽出したn個の差分画像G(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成し、評価画像(θj-1)と、統合差分画像(θψj)とを、画素毎に加算することにより、評価画像(θj)を作成し、作成した評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出ステップと、
前記第一の評価値算出ステップにおいて算出した評価値、および、前記第二の評価値算出ステップにおいて算出した評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定ステップと
を実行することを特徴とする位置姿勢推定方法。
【請求項２】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定システムであって、
参照データ作成サーバと、
前記距離センサが搭載された装置に搭載される位置姿勢推定装置と
を備え、
前記参照データ作成サーバは、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成手段と、
前記参照地図作成手段によって作成された参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成手段と
を有し、
前記位置姿勢推定装置は、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を、前記参照データ作成サーバによって算出された参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成手段と、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出手段と、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を、前記参照データ作成サーバによって作成された差分画像の中から抽出し、抽出したn個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成する統合差分画像作成手段と、
j=1からj=m-1まで、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する第二の評価画像作成手段と、
j=1からj=m-1まで、前記評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出手段と、
前記第一の評価値算出手段によって算出された評価値、および、前記第二の評価値算出手段によって算出された評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定手段と
を有することを特徴とする位置姿勢推定システム。
【請求項３】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定装置における位置姿勢推定方法であって、
前記位置姿勢推定装置は、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成ステップと、
前記参照地図作成ステップにおいて作成した参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成ステップと、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成ステップと、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出ステップと、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を差分画像の中から抽出し、抽出したn個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成し、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成し、前記評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出ステップと、
前記第一の評価値算出ステップにおいて算出した評価値、および、前記第二の評価値算出ステップにおいて算出した評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定ステップと
を実行することを特徴とする位置姿勢推定方法。
【請求項４】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定装置であって、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成手段と、
前記参照地図作成手段によって作成された参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成手段と、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成手段と、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出手段と、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を差分画像の中から抽出し、抽出したn個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成する統合差分画像作成手段と、
j=1からj=m-1まで、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する第二の評価画像作成手段と、
j=1からj=m-1まで、前記評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出手段と、
前記第一の評価値算出手段によって算出された評価値、および、前記第二の評価値算出手段によって算出された評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定手段と
を有することを特徴とする位置姿勢推定装置。
【請求項１】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定システムにおける位置姿勢推定方法であって、
前記位置姿勢推定システムは、
参照データ作成サーバと、
前記距離センサが搭載された装置に搭載される位置姿勢推定装置と
を備え、
前記参照データ作成サーバが、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成ステップと、
作成した参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成ステップと
を実行し、
前記位置姿勢推定装置が、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を、前記参照データ作成サーバによって算出された参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成ステップと、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出ステップと、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を、前記参照データ作成サーバによって作成された差分画像の中から抽出し、抽出したn個の差分画像G(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成し、評価画像(θj-1)と、統合差分画像(θψj)とを、画素毎に加算することにより、評価画像(θj)を作成し、作成した評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出ステップと、
前記第一の評価値算出ステップにおいて算出した評価値、および、前記第二の評価値算出ステップにおいて算出した評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定ステップと
を実行することを特徴とする位置姿勢推定方法。
【請求項２】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定システムであって、
参照データ作成サーバと、
前記距離センサが搭載された装置に搭載される位置姿勢推定装置と
を備え、
前記参照データ作成サーバは、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成手段と、
前記参照地図作成手段によって作成された参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成手段と
を有し、
前記位置姿勢推定装置は、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を、前記参照データ作成サーバによって算出された参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成手段と、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出手段と、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を、前記参照データ作成サーバによって作成された差分画像の中から抽出し、抽出したn個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成する統合差分画像作成手段と、
j=1からj=m-1まで、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する第二の評価画像作成手段と、
j=1からj=m-1まで、前記評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出手段と、
前記第一の評価値算出手段によって算出された評価値、および、前記第二の評価値算出手段によって算出された評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定手段と
を有することを特徴とする位置姿勢推定システム。
【請求項３】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定装置における位置姿勢推定方法であって、
前記位置姿勢推定装置は、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成ステップと、
前記参照地図作成ステップにおいて作成した参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成ステップと、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成ステップと、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出ステップと、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を差分画像の中から抽出し、抽出したn個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成し、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成し、前記評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出ステップと、
前記第一の評価値算出ステップにおいて算出した評価値、および、前記第二の評価値算出ステップにおいて算出した評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定ステップと
を実行することを特徴とする位置姿勢推定方法。
【請求項４】
距離センサの主方向とのなす角度が異なり、主方向とのなす角度が小さい順にψ0,ψ1,・・・ψm-1とするm個の方向ψk(k=0,1,・・・m-1)と、前記距離センサから異なる距離rとにおいて、方向ψkおよび距離rの組み合わせ毎にベクトルA(r,ψk)を定義し、
それぞれのベクトルA(r,ψk)毎に、複数の画素から構成される二次元の環境地図を、当該ベクトルA(r,ψk)で示される方向と逆方向へ、当該ベクトルA(r,ψk)で示される距離分平行移動することにより、参照地図(r,ψk)を作成し、
作成した参照地図(r,ψk)と、前記距離センサによって測定されたセンシングデータであって、前記距離センサの主方向とのなす角度が異なるn個の方向φi(i=0,1,・・・n-1)、および、それぞれの方向φiにおける前記距離センサから物体までの距離di(i=0,1,・・・n-1)を示すセンシングデータ(di,φi)とから、前記距離センサが搭載された装置の位置および向きを前記環境地図内で推定する位置姿勢推定装置であって、
前記環境地図から参照地図(r,ψk)(k=0,1,・・・m-1)を作成する参照地図作成手段と、
前記参照地図作成手段によって作成された参照地図(r,ψk)を参照して、同一の距離r毎に、参照地図(r,ψk)−参照地図(r,ψk-1)により、隣り合う方向における参照地図の差分を画素毎に算出し、算出した画素毎の差分において、０以外の画素値を有する画素を集めた差分画像(r,ψk)(k=1,2,・・・m-1)を作成する差分画像作成手段と、
前記距離センサによって測定されたn個のセンシングデータ(di,φi)(i=0,1,・・・n-1)のそれぞれについて、当該センシングデータ(di,φi)に示される距離diおよび方向φiと同一の距離および方向に対応する参照地図(di,φi)を参照地図の中から抽出し、抽出したn個の参照地図(di,φi)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθ0とする評価画像(θ0)を作成する第一の評価画像作成手段と、
前記評価画像(θ0)内の(x,y)の位置にある画素値を、(x,y,θ0)における評価値として算出する第一の評価値算出手段と、
j=1からj=m-1まで、前記距離センサによって測定されたn個のセンシングデータ(di,φi-θj)のそれぞれについて、当該センシングデータ(di,φi-θj)に示される距離diおよび方向φi-θjと同一の距離および方向に対応する差分画像(di,φi-θj)を差分画像の中から抽出し、抽出したn個の差分画像(di,φi-θj)の画素値を画素毎に加算することにより、加算された画素値から構成される、前記距離センサが搭載された装置の向きと前記環境地図の主方向とのなす角をθjとする統合差分画像(θj)を作成する統合差分画像作成手段と、
j=1からj=m-1まで、評価画像(θj-1)と、統合差分画像(θj)とを、画素毎に加算することにより、評価画像(θj)を作成する第二の評価画像作成手段と、
j=1からj=m-1まで、前記評価画像(θj)内の(x,y)の位置にある画素値を、(x,y,θj)における評価値として算出する第二の評価値算出手段と、
前記第一の評価値算出手段によって算出された評価値、および、前記第二の評価値算出手段によって算出された評価値の中で、最大の評価値に対応する(x,y,θ)を特定し、前記環境地図の主方向とのなす角度がθの向きで、前記環境地図内の(x,y)の位置に前記距離センサが搭載された装置の向きおよび位置を推定する位置姿勢推定手段と
を有することを特徴とする位置姿勢推定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【公開番号】特開２００９−１０９２００（Ｐ２００９−１０９２００Ａ）
【公開日】平成２１年５月２１日（２００９．５．２１）
【国際特許分類】
【出願番号】特願２００７−２７８４９６（Ｐ２００７−２７８４９６）
【出願日】平成１９年１０月２６日（２００７．１０．２６）
【出願人】（０００００５１０８）株式会社日立製作所 (27,607)
【Ｆターム（参考）】
【公開日】平成２１年５月２１日（２００９．５．２１）
【国際特許分類】
【出願日】平成１９年１０月２６日（２００７．１０．２６）
【出願人】（０００００５１０８）株式会社日立製作所 (27,607)
【Ｆターム（参考）】
[ Back to top ]