移動体
【課題】 検出した表面形状に対応する表面形状データをより正確に特定し、自己の位置情報をより正確に修正することができる移動体を提供する。
【解決手段】障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを複数個記憶する記憶部と、移動体の位置及び角度をグローバル座標系により算出する自己位置・角度算出部と、光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を相対座標系により検出する障害物検出部と、移動体の位置及び角度並びにこれらの誤差と、検出された表面形状及びその誤差に基づいて、検出された表面形状に対応する表面形状データを特定するマッチング部と、検出された表面形状とマッチング部で特定された表面形状データとのずれから、移動体の位置及び角度を修正する自己位置・角度修正部を有する移動体。
【解決手段】障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを複数個記憶する記憶部と、移動体の位置及び角度をグローバル座標系により算出する自己位置・角度算出部と、光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を相対座標系により検出する障害物検出部と、移動体の位置及び角度並びにこれらの誤差と、検出された表面形状及びその誤差に基づいて、検出された表面形状に対応する表面形状データを特定するマッチング部と、検出された表面形状とマッチング部で特定された表面形状データとのずれから、移動体の位置及び角度を修正する自己位置・角度修正部を有する移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、床面上を移動する移動体の位置を測定する技術に関する。
【背景技術】
【０００２】
特許文献１には、自己の位置と角度（向き）（以下では、位置と角度をまとめて位置情報という）を検出しながら床面上を移動する移動体が開示されている。この移動体は、床面と平行な方向に直線状に伸びる障害物（例えば、壁等）の表面形状を示すデータを複数個記憶している。表面形状データは、床面を基準とするグローバル座標系により表されている。この移動体は、床面上を移動しながら、自己の位置情報を算出するとともに、拡張カルマンフィルタ（以下、ＥＫＦという）によって自己の位置情報を修正する。具体的には、移動体は、移動しながら以下の処理を繰り返す。まず、オドメトリにより床面上における自己の位置情報（グローバル座標系における位置情報）を算出する。次に、レーザレンジファインダにより光線を走査して、走査面上に存在する障害物の床面と平行な方向における表面形状を検出する。レーザレンジファインダによれば、障害物の表面形状が移動体に対する相対座標系上の座標点群として検出される。次に、移動体は、座標点群を直線近似した障害物の表面形状を算出する。次に、記憶している表面形状データの中から、検出した表面形状（直線近似した表面形状）に対応する表面形状データを特定する。この特定は、表面形状の検出誤差（すなわち、レーザレンジファインダによる検出誤差）を考慮して行われる。次に、特定した表面形状データと検出した表面形状とのずれを算出し、そのずれから自己の位置情報を修正する。このように、障害物の表面形状データを移動体に予め記憶させておくとともに、移動しながらレーザレンジファインダ等により障害物の表面形状を検出し、検出した表面形状とそれに対応する表面形状データのずれに基づいて自己の位置情報を修正することで、より正確に自己の位置情報を特定することができる。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】A. Garulli, A. Giannitrapani, A. Rossi, A. Vicino, "Mobile robot SLAM for line-based environment representation," Tech. Rep. 2005-3, Dipartimento di Ingegneria dell'Informazone, Universita di Siena, 2005
【発明の概要】
【発明が解決しようとする課題】
【０００４】
障害物の表面形状を検出する際には、誤差が生じる。このため、検出した表面形状に対応する表面形状データを複数の表面形状データの中から正確に特定するためには、誤差を考慮する必要がある。上述した非特許文献１の技術では、検出された表面形状の誤差（すなわち、レーザレンジファインダによる検出誤差）を考慮して対応する表面形状データの特定を行っているが、この技術によっても対応する表面形状データを正確に特定できない場合がある。このため、自己の位置情報を正確に修正できない場合があった。
【０００５】
本明細書では、上述した実情を鑑み、検出した表面形状に対応する表面形状データをより正確に特定し、自己の位置情報をより正確に修正することができる移動体を提供する。
【課題を解決するための手段】
【０００６】
移動体により検出される障害物の表面形状は、移動体に対する相対座標系により表される。一方、移動体に記憶されている表面形状データは、グローバル座標系により表される。したがって、検出された表面形状に対応する表面形状データは、検出された表面形状と、記憶されている表面形状データと、移動体の位置情報に基づいて特定される。このため、この特定には、検出された表面形状の誤差だけでなく、移動体の位置情報の誤差も影響する。本発明者らは、この点に着目し、移動体の位置情報の誤差も考慮して、検出された表面形状に対応する表面形状データを特定することを提案する。
【０００７】
本願明細書が開示する移動体は、障害物が存在する床面上を移動する。この移動体は、記憶部と、自己位置・角度算出部と、障害物検出部と、マッチング部と、自己位置・角度修正部を有している。記憶部は、床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを複数個記憶する。自己位置・角度算出部は、移動体の位置及び角度をグローバル座標系により算出する。障害物検出部は、光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する。マッチング部は、自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された表面形状及びその誤差に基づいて、記憶部に記憶されている複数の表面形状データの中から障害物検出部で検出された表面形状に対応する表面形状データを特定する。自己位置・角度修正部は、障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する。
【０００８】
なお、上記の「グローバル座標系」及び「相対座標系」は、３次元座標系としてもよいし、２次元座標系としてもよい。「グローバル座標系」及び「相対座標系」を２次元座標系とすると、移動体で行う計算量を少なくすることができる。２次元座標系を採用する場合、床面と平行な２つの座標軸により規定される座標系（例えば、直交座標系）であってもよいし、床面と平行な平面内における距離と角度により規定される極座標系であってもよい。また、表面形状データの座標系と移動体の位置及び角度の座標系が異なっていてもよい。例えば、表面形状データが極座標系により表され、移動体の位置が直交座標系により表されてもよい。また、表面形状データ、移動体の位置及び角度、並びに、障害物検出部が検出する障害物の表面形状は、少なくとも床面と平行な方向における形状（座標成分）により規定されていればよく、床面と垂直な方向の座標成分を含んでいてもよい。例えば、表面形状データは、床面と垂直な方向の座標成分を含めた障害物の表面の３次元形状を示すデータであってもよい。
また、「障害物検出部で検出された表面形状とマッチング部で特定された表面形状データのずれ」は、相対座標系で示される「障害物検出部で検出された表面形状」とグローバル座標系で示される「マッチング部で特定された表面形状データ」とを同一座標系で比較したときのずれを意味する。したがって、「障害物検出部で検出された表面形状」をグローバル座標系に座標変換し、グローバル座標系でこれらのずれを算出してもよい。また、「マッチング部で特定された表面形状データ」を相対座標系に座標変換し、相対座標系でこれらのずれを算出してもよい。
また、上記の「誤差」とは、ある特性値を検出（または算出等）するときに、現実の特性値と検出値との間に生じ得るずれの大きさを示す値を意味する。
また、障害物検出部が検出する障害物の表面形状は、光線の走査面上に存在する障害物の表面形状を複数の座標点により表すものであってもよいし、２つの座標点により規定される直線（または線分）により表面形状を表すものであってもよい。直線（または線分）により障害物の表面形状を表す場合には、例えば、光線の走査面上に存在する障害物の表面形状を複数の座標点により検出するレーザレンジファインダと、レーザレンジファインダが検出した複数の座標点を直線近似した直線（または線分）を算出する演算部により障害物検出部を構成することができる。
また、自己位置・角度算出部は、オドメトリによって移動体の位置と角度を算出してもよいし、ＧＰＳによって移動体の位置と角度を算出してもよいし、その他の技術によって算出してもよい。
【０００９】
この移動体では、移動中に、自己位置・角度算出部が移動体の位置と角度を算出する。また、移動中に、障害物検出部が、障害物の表面形状を検出する。そして、マッチング部が、記憶部に記憶されている複数の表面形状データの中から障害物検出部で検出された表面形状に対応する表面形状データを特定する。このとき、マッチング部は、障害物検出部で検出された表面形状の誤差のみならず、移動体の位置及び角度の誤差も考慮して、検出された表面形状に対応する表面形状データを特定する。したがって、検出された表面形状に対応する表面形状データを正確に特定することができる。自己位置・角度修正部は、特定された表面形状データと検出された表面形状とのずれに基づいて、移動体の位置と角度を修正する。対応する表面形状データが正確に特定されているので、自己位置・角度修正部は正確に移動体の位置と角度を修正することができる。
【００１０】
上述した移動体は、障害物検出部が、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出することが好ましい。また、移動体は、検出された線分の両端のみの位置の誤差に基づいて、障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれの誤差を算出するずれ誤差算出部をさらに有していることが好ましい。そして、自己位置・角度修正部は、ずれ誤差算出部によって算出された誤差に基づいて、移動体の位置及び角度を修正することが好ましい。
【００１１】
このような構成によれば、検出された線分の両端の位置の誤差のみからずれの誤差が算出されるので、表面形状全体の座標点の誤差からずれの誤差を算出する場合に比べて、移動体で行う計算量を低減することができる。これにより、より高速に移動体の位置と角度を特定することが可能となる。
【００１２】
上述した移動体では、障害物検出部が、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出することが好ましい。また、マッチング部は、検出された線分の両端の位置の誤差に基づいて、その両端の両方が同一の表面形状データ上に存在する確率を複数の表面形状データについて算出し、算出された確率が最も高い表面形状データを特定することが好ましい。
【００１３】
このような構成によれば、検出された線分の両端の表面形状データ上への存在確率に基づいて対応する表面形状データが特定されるので、表面形状全体の座標点の存在確率を算出する場合に比べて、移動体で行う計算量を低減することができる。これにより、より高速に移動体の位置と角度を特定することが可能となる。
【００１４】
また、本明細書は、以下の移動体も提供する。この移動体は、障害物が存在する床面上を移動する。この移動体は、記憶部と、自己位置・角度算出部と、障害物検出部と、マッチング部と、自己位置・角度修正部を有する。記憶部は、床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを記憶する。自己位置・角度算出部は、移動体の位置及び角度をグローバル座標系により算出する。障害物検出部は、光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する。マッチング部は、障害物検出部で複数の表面形状が検出されたときに、自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された複数の表面形状及びそれらの誤差に基づいて、障害物検出部で検出された複数の表面形状の中から記憶部に記憶されている表面形状データに対応する表面形状を特定する。自己位置・角度修正部は、マッチング部で特定された表面形状と記憶部に記憶されている表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する自己位置・角度修正部を有する。
【００１５】
この移動体によれば、障害物検出部で複数の表面形状が検出されたときに、これらの表面形状の中から記憶部に記憶されている表面形状データに対応する表面形状を正確に特定することができる。したがって、正確に移動体の位置及び角度を修正することができる。
【図面の簡単な説明】
【００１６】
【図１】自律移動体１０の概略斜視図。
【図２】自律移動体１０のブロック図。
【図３】環境マップの説明図。
【図４】レーザレンジファインダ３０の測定方法の説明図。
【図５】自律移動体１０の角度の説明図。
【図６】制御装置５０が実行する処理を示すフローチャート。
【図７】対象線分１２０の算出方法を示す説明図。
【図８】ステップＳ１０の詳細な処理を示すフローチャート。
【図９】線分上の点Ｐ及び領域Ｑについての説明図。
【図１０】対象線分１２０に対応する線分に関する説明図。
【図１１】対象線分１２０と対応する線分１００ｆの位置情報の説明図。
【図１２】２つの線分が検出された場合を示す説明図。
【発明を実施するための形態】
【００１７】
以下に説明する実施例の好ましい特徴を整理する。
（特徴１）記憶部は、障害物の表面形状を表す複数の線分を示すデータを記憶している。
（特徴２）移動体は、障害物検出部で検出された表面形状を直線近似した線分を算出する線分算出部を有している。
（特徴３）マッチング部は、記憶部が記憶している複数の線分の中から、線分算出部で算出した線分（対象線分）に対応する線分を特定する。
（特徴４）自己位置・角度修正部は、対象線分とこれに対応する線分とのずれ、及び、当該ずれの誤差を算出し、当該ずれ及び当該ずれの誤差に基づいて、移動体の位置及び角度を修正する。
【実施例】
【００１８】
図１は、実施例に係る自律移動体１０の概略斜視図を示している。自律移動体１０は、２つの駆動輪１２ａ、１２ｂと従動輪１２ｃを有しており、駆動輪１２ａ、１２ｂを駆動することで水平な床面上を移動する。従動輪１２ｃは、自律移動体１０の移動に伴って回転する。駆動輪１２ａと駆動輪１２ｂを同じ回転数で駆動すると、自律移動体１０は図１の矢印Ｆ１の向きに並進する。また、駆動輪１２ａと駆動輪１２ｂを異なる回転数で駆動すると、自律移動体１０は床面上をカーブしながら移動する。自律移動体１０は、床面上における自己の位置を算出しながら、目的地まで自律的に移動する。
【００１９】
図２は、自律移動体１０の各部の機能ブロック図を示している。自律移動体１０は、記憶装置２０と、レーザレンジファインダ３０と、サーボモータ４０ａ、４０ｂと、制御装置５０を備えている。図１に示すように、レーザレンジファインダ３０は、自律移動体１０の上部に設置されている。記憶装置２０、サーボモータ４０ａ、４０ｂ、及び、制御装置５０は、自律移動体１０内に組み込まれている。
【００２０】
記憶装置２０は、自律移動体１０が移動する床面上に存在する障害物（壁等）の表面形状を示す環境マップを記憶している。環境マップは、水平面内における障害物の表面形状を線分により表したデータである。図３は、環境マップが表す線分群の一例を示している。図３では、線分１００ａ〜１００ｊが示されている。環境マップは、始点と終点の座標により各線分１００ａ〜１００ｈを規定している。始点と終点の座標は、床面を基準して水平方向に設定されたｘ軸とｙ軸による直交座標系（以下、グローバル座標系という）により定められている。例えば、図３の線分１００ａは、始点座標（ｘａ１，ｙａ１）と終点座標（ｘａ２，ｙａ２）により表される。線分１００ｂは、始点座標（ｘｂ１，ｙｂ１）と終点座標（ｘｂ２，ｙｂ２）により表される。図３の例では、線分１００ａの終点座標（ｘａ２，ｙａ２）と線分１００ｂの始点座標（ｘｂ１，ｙｂ１）は等しい。各線分の始点及び終点は、始点から終点に向かうベクトルの右側が障害物側（自律移動体１０が進入できない領域）であり、ベクトルの左側が自律移動体１０が移動可能な領域であることを示している。図３においては、線分１００ａ〜１００ｊに囲まれた領域が自律移動体１０が移動可能な領域であり、その外側の領域が障害物（自律移動体１０が進入できない領域）である。
【００２１】
レーザレンジファインダ３０は、水平面内に光線を走査して、その走査面内に存在する障害物の表面形状を検出する。図４は、レーザレンジファインダ３０を上側から示した平面図である。レーザレンジファインダ３０は、最初に、自律移動体１０の進行方向Ｆ１より角度ｗ１（水平面内で見た時における角度）だけ右側にレーザ光を照射する。そして、照射したレーザ光の反射光を検出して角度ｗ１の方向に存在する障害物までの距離ｒ１（水平面内における距離）を検出する。すなわち、レーザ光が照射された箇所の自律移動体１０に対する相対位置座標（ｒ１，ｗ１）を検出する。次に、開始角度ｗ１よりも左側に微小角度ずれた角度ｗ２の方向にレーザ光を照射し、レーザ光が照射された箇所までの距離ｒ２を検出する。すなわち、レーザ光が照射された箇所の自律移動体１０に対する相対位置座標（ｒ２，ｗ２）を検出する。レーザレンジファインダ３０は、開始角度ｗ１から終了角度ｗｎ（自律移動体１０の進行方向Ｆ１より左側の角度）まで一定の角度間隔で同様の処理を繰り返し、レーザが照射された箇所の自律移動体１０に対する相対位置座標（ｒ１，ｗ１），（ｒ２，ｗ２），（ｒ３，ｗ３）・・・（ｒｎ，ｗｎ）を検出する。レーザレンジファインダ３０により検出される座標点群（ｒ１，ｗ１）〜（ｒｎ，ｗｎ）は、障害物の表面形状を表す。レーザレンジファインダ３０が検出した障害物の表面形状は、制御装置５０に入力される。
【００２２】
サーボモータ４０ａは、サーボ制御回路４２ａ、モータ４４ａ、エンコーダ４６ａを有している。モータ４４ａは、駆動輪１２ａを回転させる。サーボ制御回路４２ａには、制御装置５０から制御指令値が入力される。サーボ制御回路４２ａは、入力された制御指令値にしたがってモータ４４ａを駆動させる。したがって、駆動輪１２ａが、制御指令値に従って回転する。エンコーダ４６ａは、モータ４４ａ（すなわち、駆動輪１２ａ）の回転数を検出する。エンコーダ４６ａが検出する回転数は、制御装置５０に入力される。
サーボモータ４０ｂは、サーボ制御回路４２ｂ、モータ４４ｂ、エンコーダ４６ｂを有している。モータ４４ｂは、駆動輪１２ｂを回転させる。サーボ制御回路４２ｂには、制御装置５０から制御指令値が入力される。サーボ制御回路４２ｂは、入力された制御指令値にしたがってモータ４４ｂを駆動させる。したがって、駆動輪１２ｂが、制御指令値に従って回転する。エンコーダ４６ｂは、モータ４４ｂ（すなわち、駆動輪１２ｂ）の回転数を検出する。エンコーダ４６ｂが検出する回転数は、制御装置５０に入力される。
【００２３】
制御装置５０は、記憶装置２０、レーザレンジファインダ３０、及び、サーボモータ４０ａ、４０ｂと電気的に接続されている。制御装置５０は、記憶装置２０からデータを読み出すことができ、また、記憶装置２０にデータを記憶させることができる。制御装置５０は、後述する位置と角度の算出処理によって、自律移動体１０の位置（水平面内における自律移動体１０の位置）と角度（水平面内における自律移動体１０の進行方向Ｆ１の向き）を算出する。なお、自律移動体１０の位置は、上述したグローバル座標系のｘｙ座標によって表され、自律移動体１０の角度は、図５に示すように、グローバル座標系のｘ軸に対する自律移動体１０の進行方向Ｆ１の角度φにより表される。以下では、自律移動体１０の位置と角度を合わせて、位置情報という場合がある。制御装置５０は、算出した自律移動体１０の位置情報に基づいて、サーボモータ４０ａ、４０ｂに制御指令値を入力する。制御装置５０が、自律移動体１０の位置情報を算出し、その位置情報に基づいてサーボモータ４０ａ、４０ｂに制御指令値を入力する処理を繰り返すことで、自律移動体１０が目標地点に向かって自律的に移動する。
【００２４】
次に、制御装置５０が実行する処理について詳細に説明する。図６のフローチャートは、制御装置５０が実行する処理を示している。図６の処理の開始時点において、制御装置５０に、自律移動体１０の初期位置情報（ｘ０，ｙ０，φ０）（処理の開示時点における自律移動体１０の位置と角度）と、目標地点の位置が入力される。制御装置５０は、図６に示すステップＳ２〜Ｓ１６を一定の周期で繰り返して、自律移動体１０の位置情報を算出しながら移動する。
【００２５】
ステップＳ２では、制御装置５０は、現在の自律移動体１０の位置情報と、目標地点の位置と、環境マップ（障害物の位置）に基づいて、自律移動体１０の今後の経路を算出する。なお、最初のステップＳ２では、制御装置５０は、開始時に入力された初期位置情報（ｘ０，ｙ０，φ０）を現在の位置情報として経路を算出する。２回目以降のステップＳ２では、制御装置５０は、直前の周期のステップＳ１６で修正された自律移動体１０の位置情報Ｘｋ−１｜ｋ−１（＝ｘｋ−１｜ｋ−１，ｙｋ−１｜ｋ−１，φｋ−１｜ｋ−１）を現在の位置情報として経路を算出する。経路を算出したら、制御装置５０は、算出した経路に沿って自律移動体１０が移動するように、サーボモータ４０ａ、４０ｂに制御指令値を入力する。サーボモータ４０ａ、４０ｂは、次のステップＳ２が実行されるまで、直前のステップＳ２で入力された制御指令値にしたがって駆動輪１２ａ、１２ｂを駆動する。これによって、自律移動体１０が移動する。また、駆動輪１２ａ、１２ｂが駆動すると、エンコーダ４６ａ、４６ｂによって駆動輪１２ａ、１２ｂの回転数が検出される。
【００２６】
ステップＳ２を実行してから所定時間経過後に、ステップＳ４〜Ｓ１６の処理が実行される。これによって、現在の自律移動体１０の位置情報が算出される。ステップＳ４〜Ｓ１６の処理は、拡張カルマンフィルタに基づいて行われる。
【００２７】
ステップＳ４では、制御装置５０は、オドメトリによって自律移動体１０の現在の位置情報Ｘｋ｜ｋ−１（＝（ｘｋ｜ｋ−１，ｙｋ｜ｋ−１，φｋ｜ｋ−１））を算出する。具体的には、制御装置５０は、エンコーダ４６ａ、４６ｂが検出する駆動輪１２ａ、１２ｂの回転数と、直前の周期のステップＳ１６で修正された自律移動体１０の位置情報Ｘｋ−１｜ｋ−１（過去の位置情報）に基づいて、自律移動体１０の現在の位置情報Ｘｋ｜ｋ−１を算出する。
また、ステップＳ４では、位置情報Ｘｋ｜ｋ−１の算出と同時に、位置情報Ｘｋ｜ｋ−１の誤差ΣＲを算出する。なお、誤差ΣＲは、位置（ｘｋ｜ｋ−１，ｙｋ｜ｋ−１）と角度φｋ｜ｋ−１に生じうる誤差を分散（ｖｘｋ｜ｋ−１，ｖｙｋ｜ｋ−１，ｖφｋ｜ｋ−１）により表したものである。誤差ΣＲは、文献「S. Thrun, W.burgard, D. Fox, "Probabilistic robotics", The MIT Press, 2005」に記載の技術に従って算出することができる。すなわち、誤差ΣＲは、過去に自律移動体１０が移動した経路に基づいて算出される。ｘ方向への移動距離（各周期におけるｘ方向の移動距離の絶対値を積算した値）が大きいほど誤差ｖｘｋ｜ｋ−１が大きく算出され、ｙ方向への移動距離が大きいほど誤差ｖｙｋ｜ｋ−１が大きく算出され、角度φの変化量（各周期における角度φの変化量の絶対値を積算した値）が大きいほど誤差ｖφｋ｜ｋ−１が大きく算出される。なお、誤差ΣＲは、上述した他の方法により算出してもよい。また、誤差ΣＲが経時的に変化しない場合には、誤差ΣＲが記憶装置２０に記憶されていてもよい。
【００２８】
ステップＳ６では、制御装置５０は、レーザレンジファインダ３０により空間内に光線を走査して、走査範囲内に存在する障害物の表面形状を検出する。上述したように、レーザレンジファインダ３０は、障害物の表面形状を、自律移動体１０に対する相対座標系による座標点群（ｒ１，ｗ１），（ｒ２，ｗ２），（ｒ３，ｗ３）・・・（ｒｎ，ｗｎ）として検出する。
【００２９】
ステップＳ８では、制御装置５０は、ステップＳ６でレーザレンジファインダ３０により検出された座標点群を直線近似することによって線分を算出する。ステップＳ８の線分の算出処理は、例えば、文献「J. Xavier, "Rast Line, Arc/Circle and Leg Detection from Laser Scan Data in Player Driver", ICRA, 2005」に従って実行することができる。具体的には、座標点群（ｒ１，ｗ１）〜（ｒｎ，ｗｎ）を直線近似することで、図７に示すように、線分１２０を算出する。この線分は、自律移動体１０を基準として水平方向に設定されたｓ軸とｔ軸による直交座標系（以下では、ローカル座標系という）により始点の座標（ｓｓ，ｔｓ）と終点の座標（ｓｅ，ｔｅ）を規定することにより表される。なお、始点（ｓｓ，ｔｓ）は、レーザレンジファインダ３０が光線の走査を開始する側（座標点（ｒ１，ｗ１）側）の線分の端部を示しており、終点（ｓｅ，ｔｅ）はその反対側（座標点（ｒｎ，ｗｎ）側）の線分の端部を示している。次に、制御装置５０は、相対座標系で表されている線分の始点の座標（ｓｓ，ｔｓ）と終点の座標（ｓｅ，ｔｅ）の座標を、グローバル座標系上の座標（ｘｓ，ｙｓ），（ｘｅ，ｙｅ）に座標変換する。この座標変換は、ステップＳ４で算出された自律移動体１０の位置情報Ｘｋ｜ｋ−１に基づいて行われる。以下では、ステップＳ８で算出した線分を対象線分という。
【００３０】
ステップＳ１０では、制御装置５０は、記憶装置２０が記憶する環境マップにより規定されている線分の中から、対象線分に対応する線分を特定する。対象線分に対応する線分の特定処理は、図８に示すフローチャートに従って実行される。
【００３１】
ステップＳ４０では、制御装置５０は、対象線分の始点の位置の誤差Σ１と終点の位置の誤差Σ２を算出する。誤差Σ１は、対象線分の始点の位置（ｘｓ，ｙｓ）に生じうる誤差を分散（ｖｘｓ，ｖｙｓ）で表したものであり、誤差Σ２は、対象線分の終点の位置（ｘｅ，ｙｅ）に生じうる誤差を分散（ｖｘｅ，ｖｙｅ）で表したものである。上述したように、対象線分は、レーザレンジファインダ３０により検出される座標点群と、自律移動体１０の位置情報Ｘｋ｜ｋ−１に基づいて算出される。このため、レーザレンジファインダ３０の測定誤差と、自律移動体１０の位置情報Ｘｋ｜ｋ−１の誤差ΣＲとの影響によって、対象線分の始点と終点の位置に誤差が生じる。このため、制御装置５０は、レーザレンジファインダ３０の測定誤差と、位置情報Ｘｋ｜ｋ−１の誤差ΣＲに基づいて、対象線分の始点の誤差Σ１と終点の誤差Σ２を算出する。具体的には、制御装置５０は、以下の数式によって、これらの誤差を算出する。
Σ１＝Ｊ１×ΣＲ×Ｊ１Ｔ＋ΣＯ
Σ２＝Ｊ２×ΣＲ×Ｊ２Ｔ＋ΣＯ
ここで、ΣＯは、レーザレンジファインダ３０の測定誤差に基づいて生じる始点及び終点の位置の誤差を、ｘ座標及びｙ座標の分散で表したものである。本実施例では、誤差ΣＯは固定値とみなせるため、誤差ΣＯが予め記憶装置２０に記憶されている。Ｊ１は、自律移動体１０の位置情報（ｘ，ｙ，φ）に関する線分の始点のヤコビ行列であり、Ｊ１Ｔはヤコビ行列Ｊ１の転置行列である。Ｊ２は、自律移動体１０の位置情報（ｘ，ｙ，φ）に関する線分の終点のヤコビ行列であり、Ｊ２Ｔはヤコビ行列Ｊ２の転置行列である。ヤコビ行列Ｊ１，Ｊ２は以下の数１により表される。
【数１】
【００３２】
ステップＳ４２では、始点の位置（ｘｓ，ｙｓ）とその誤差Σ１に基づいて、始点の位置に関する確率密度関数を算出する。また、終点の位置（ｘｅ，ｙｅ）とその誤差Σ２に基づいて、終点の位置に関する確率密度関数を算出する。始点の位置の確率密度関数は、始点が存在している確率をｘ座標とｙ座標の関数として表したものである。ｘｙ座標の範囲で確率密度関数を積分することで、その座標範囲内に始点が存在する確率を算出することができる。終点の確率密度関数も、始点の確率密度関数と同様に、終点が存在している確率をｘ座標とｙ座標の関数として表したものである。
【００３３】
ステップＳ４４〜Ｓ５０では、ステップＳ４２で算出した確率密度関数に基づいて、環境マップ中の複数の線分の中から、対象線分に対応する線分を特定する。
【００３４】
ステップＳ４４では、制御装置５０は、対象線分の向き（始点から終点に向かう向き）と同じ向きを有する線分を環境マップから抽出する。向きが等しいか否かは、対象線分を示すベクトル（始点から終点に向かうベクトル）と環境マップ中の各線分を示すベクトル（始点から終点に向かうベクトル）との内積がゼロより大きいか否かにより判定することで行う。すなわち、対象線分のベクトルに対する角度が９０度未満のベクトルを有する線分が、同じ向きを有する線分として特定される。上述したように、環境マップ中の線分の向きに対して右側の領域は障害物を示している。また、対象線分の向きはレーザレンジファインダ３０によるスキャン方向を示している。したがって、対象線分の向きとこれに対応する環境マップ中の線分の向きが逆となることは、レーザレンジファインダ３０が障害物の内部に存在することとなり、ありえない。したがって、ステップＳ４４で対象線分と向きが等しい線分を抽出し、ステップＳ４６の処理を抽出した線分に対してのみ行う。このように、向きが同じとなる線分のみを抽出してステップＳ４６を実行することで、制御装置５０の負荷が軽減される。
【００３５】
ステップＳ４６では、制御装置５０は、ステップＳ４４で抽出した線分のそれぞれに対して、その線分上に対象線分の始点と終点の両方が存在する確率を算出する。具体的には、抽出された線分の夫々に対して、以下の処理が実行される。
最初に、抽出された線分上の点を一定間隔で抽出する。なお、抽出された線分の始点と終点については、隣の点との間隔にかかわらず抽出する。次に、図９に示すように、抽出した各点（図９では点Ｐとして示されている）を中心としてその周囲に対角線の長さが前記間隔と等しい正方形の領域Ｑを設定する。次に、抽出した各点Ｐの座標をステップＳ４２で算出した始点についての確率密度関数に代入することで、点Ｐ毎に始点が点Ｐ上に存在する確立を算出する。次に、点Ｐ上に始点が存在する確率に、点Ｐに対応する正方形の領域Ｑの面積を乗算して、領域Ｑ毎に領域Ｑ内に始点が存在する確率を算出する。次に、線分に対して設定された全ての領域Ｑについての始点の存在確率の総和を算出する。算出された総和は、その線分上に始点が存在する確率として近似することができる。
次に、抽出した各点Ｐの座標をステップＳ４２で算出した終点についての確率密度関数に代入することで、抽出した点Ｐ毎に終点が点Ｐ上に存在する確立を算出する。次に、点Ｐ上に終点が存在する確率に、点Ｐに対応する正方形の領域Ｑの面積を乗算して、領域Ｑ内に終点が存在する確率を算出する。次に、線分に対して設定された全ての領域Ｑについての終点の存在確率の総和を算出する。算出された総和は、その線分上に終点が存在する確率として近似することができる。
最後に、始点の存在確率と終点の存在確率とを乗算する。これによって、その線分上（より具体的には、その線分から所定の範囲内）に対象線分の始点と終端の両方が存在する確率が算出される。
【００３６】
ステップＳ４８では、制御装置５０は、ステップＳ４６で算出した確率の中で最大値を特定する。そして、特定した最大値が、閾値を超えているか否かを判定する。特定した最大値が閾値を超えていない場合には、制御装置５０はＮＵＬＬデータを報告し、図８の処理を中止する。ＮＵＬＬデータを報告した場合には、図６のステップＳ１２、１４は実行されず、次のステップＳ２は、修正されていない位置情報Ｘｋ｜ｋ−１（ステップＳ４で算出された位置情報）に基づいて実行される。特定した最大値が閾値を超えている場合には、ステップＳ５０において、その最大値を有する線分を、対象線分に対応する線分として特定する。以上により、図８の処理（すなわち、図６のステップＳ１０）が終了する。
【００３７】
以上に説明したように、ステップＳ４２では、レーザレンジファインダ３０の測定誤差ΣＯだけでなく、自律移動体１０の位置情報Ｘｋ｜ｋ−１の誤差ΣＲも考慮して、確率分布関数を算出する。そして、その確率分布関数に基づいて、対象線分に対応する線分を環境マップ中から特定する。このように、時々刻々と変化する誤差ΣＲを考慮することで、より正確に対象線分に対応する線分を特定することができる。例えば、環境マップ中の線分１００ｄ〜１００ｆと対象線分１２０が図１０に示す位置関係になった場合を考えると、自律移動体１０の位置情報の誤差がｘ方向に大きければ、対応する線分として線分１００ｆが特定される。自律移動体１０の位置情報の誤差がｙ方向に大きければ、対応する線分として線分１００ｄが特定される。このように、自律移動体１０の位置情報の誤差を考慮することで、正確な対応付けを行うことが可能となる。
【００３８】
図６のステップＳ１２では、制御装置５０は、対象線分と、対応する線分の夫々に対して、自律移動体１０に対する相対位置情報を算出する。これらの線分の相対位置情報は、自律移動体１０までの距離と自律移動体１０の進行方向Ｆ１に対する角度により表される。例えば、図１１に示すように、対象線分１２０とこれに対応する線分１００ｆが特定されている場合には、対象線分１２０の相対位置情報は、自律移動体１０までの距離ｒａ（対象線分の垂線方向における距離）と、自律移動体１０の進行方向Ｆ１に対する角度ｗａ（対象線分の垂線と進行方向Ｆ１の間の角度）により算出される。また、対応する線分１００ｆの相対位置情報は、自律移動体１０までの距離ｒｂ（対応する線分の垂線方向における距離）と、自律移動体１０の進行方向Ｆ１に対する角度ｗｂ（対応する線分の垂線と進行方向Ｆ１の間の角度）により算出される。次に、制御装置５０は、対象線分と対応する線分との相対位置情報のずれ（ｄｒ，ｄｗ）を算出する。長さのずれｄｒは、距離ｒａから距離ｒｂを減算した値であり、角度のずれｄｗは、角度ｗａから角度ｗｂを減算した値である。
【００３９】
ステップＳ１４では、制御装置５０は、上述したずれ（ｄｒ，ｄｗ）の誤差Σ３（＝（ｖｄｒ，ｖｄｗ））を算出する。なお、誤差ｖｄｒは距離のずれｄｒを算出する際に生じうる誤差を表す分散であり、誤差ｖｄｗは角度のずれｄｗを算出する際に生じうる誤差を表す分散である。対応する線分の位置情報は環境マップで規定されたものであるので、上述したずれの誤差Σ３は、対象線分の相対位置情報（ｒａ，ｗａ）の検出誤差と等しい。
したがって、距離の誤差ｖｄｒは、対象線分の始点及び終点の誤差ΣＯを以下の計算式により座標変換することで算出される。
ｖｄｒ＝Ｊ３×ΣＯ×Ｊ３Ｔ
ここで、Ｊ３は、対象線分の始点と終点の位置に関する距離ｒａのヤコビ行列であり、Ｊ３Ｔはヤコビ行列Ｊ３の転置行列である。ヤコビ行列Ｊ３は以下の数２により表される。
【数２】
また、角度の誤差ｖｄｗは、対象線分の始点及び終点の誤差ΣＯを以下の計算式により座標変換することで得られる。
ｖｄｗ＝Ｊ４×ΣＯ×Ｊ４Ｔ
ここで、Ｊ４は、対象線分の始点と終点の位置に関する角度ｗａのヤコビ行列であり、Ｊ４Ｔはヤコビ行列Ｊ４の転置行列である。ヤコビ行列Ｊ４は以下の数３により表される。
【数３】
【００４０】
ステップＳ１６では、制御装置５０は、ステップＳ１２で算出した相対位置のずれ（ｄｒ，ｄｗ）と、ステップＳ１４で算出したずれの誤差Σ３（＝（ｖｄｒ，ｖｄｗ））に基づいて、ステップＳ４で算出した自律移動体１０の位置情報Ｘｋ｜ｋ−１（＝（ｘｋ｜ｋ−１，ｙｋ｜ｋ−１，φｋ｜ｋ−１）を修正し、修正後の位置情報Ｘｋ｜ｋ（＝（ｘｋ｜ｋ，ｙｋ｜ｋ，φｋ｜ｋ）を算出する。自律移動体１０の位置情報の修正は、一般的なＥＫＦの手法により行われる。例えば、文献「S. Thrun, W.burgard, D. Fox, "Probabilistic robotics", The MIT Press, 2005」の手法により自律移動体１０の位置情報を修正することができる。このように位置を修正することで、より正確に自律移動体１０の位置情報を特定することができる。
【００４１】
その後、制御装置５０は、再度ステップＳ２からの処理を実行する。ステップＳ２では、修正後の正確な位置情報に基づいて経路が算出される。このように、位置情報の算出（ステップＳ４）、レーザレンジファインダ３０の検出値に基づく位置情報の修正（ステップＳ６〜Ｓ１６）、修正後の位置情報に基づく制御（ステップＳ２）が繰り返し行われることで、自律移動体１０が目標値まで正確に移動することができる。
【００４２】
以上に説明したように、実施例の自律移動体１０では、レーザレンジファインダ３０の検出誤差だけでなく、自律移動体１０の位置情報の誤差をも考慮して、対象線分に対応する線分を環境マップ中から特定する。このため、対象線分に対して誤った線分が対応付けられることが防止される。これにより、より正確に位置情報を修正することができる。
【００４３】
また、上述した自律移動体１０では、ステップＳ１４において対象線分とこれに対応する線分との相対位置のずれの誤差を算出するときに、対象線分の始点と終点の位置の誤差のみを考慮した。このように、レーザレンジファインダ３０が検出する各座標点の全ての検出誤差を考慮するのではなく、対象線分の両端点の位置の検出誤差のみを考慮することで、相対位置のずれの誤差の算出に要する演算量を低減することができる。これにより、より短い周期で図６のフローチャートを実行可能となり、より正確に自律移動体１０を移動させることが可能となる。
【００４４】
また、上述した自律移動体１０では、ステップＳ１０で対象線分に対応する線分を特定するときに、対象線分の両端点の存在確率のみを考慮した。このように、レーザレンジファインダ３０が検出する各座標点の全ての存在確率を考慮するのではなく、対象線分の両端点の存在確率のみを考慮することで、対象線分に対応する線分の特定処理に要する演算量を低減することができる。これにより、より短い周期で図６のフローチャートを実行可能となり、より正確に自律移動体１０を移動させることが可能となる。
【００４５】
なお、上述した実施例では自律移動体１０について説明したが、本発明は自律的に移動しない移動体に適用してもよい。例えば、移動体を遠隔操縦するときに、本発明により移動体の位置を特定してもよい。
【００４６】
上述した実施例では、移動領域内に他の移動体が存在しなかったが、移動領域内に他の移動体が存在してもよい。この場合は、レーザレンジファインダによって壁の表面形状と他の移動体の表面形状が検出される。このため、レーザレンジファインダによって検出された座標点群から、壁の表面形状に相当する座標点群を特定し、その座標点群に対して上述した処理を実行すればよい。
【００４７】
また、上述した実施例では、自律移動体の位置と角度の誤差を文献「S. Thrun, W.burgard, D. Fox, "Probabilistic robotics", The MIT Press, 2005」に記載の技術によって算出したが、位置と角度の誤差の算出には種々の方法を用いることができる。例えば、自律移動体の位置と角度の誤差を、自律移動体の運動状態を考慮して算出することができる。例えば、自律移動体の位置をＧＰＳセンサで検出する場合において、自律移動体の速度がｘ方向に大きく、ｙ方向に小さい場合は、ＧＰＳセンサの検出誤差を、ｘ方向の誤差が大きく、ｙ方向の誤差が小さくなるように算出する。自律移動体の運動状態を考慮することで、自律移動体の位置と角度の誤差を適切に算出することができる。
【００４８】
また、上述した実施例では、レーザレンジファインダ３０の検出結果から得られる線分が１つであり、この線分に対応する線分を環境マップの複数の線分の中から特定した。しかしながら、レーザレンジファインダ３０の検出結果から得られる線分が複数となる場合もある。また、環境マップ中の線分のうちの一部のみが自己位置・角度の修正の基準（すなわち、ランドマーク）として使用される場合がある。例えば、図１２に示すようにレーザレンジファインダ３０の検出結果から２つの線分１３０、１３２が得られており、線分１００ｘだけが自己位置・角度の修正の基準とされている場合を考える。例えば、線分１００ｘが示す壁面の前に別の障害物が置かれていたり、線分１００ｘが示す壁面に設けられた扉が開放されている場合等に、図１２に示すように１つの線分が検出されるはずの位置で２つの線分が検出される場合がある。この場合にも、レーザレンジファインダ３０の検出誤差だけでなく、自律移動体１０の位置情報の誤差をも考慮して、検出された線分１３０、１３２の中から線分１００ｘに対応する線分を特定することができる。対応する線分の特定は、上述した実施例と同様の演算により行うことができる。このように、検出される線分が１つでありランドマークとなる線分が複数である場合だけでなく、検出される線分が複数でありランドマークとなる線分が１つである場合にも、誤差を考慮して線分の対応関係を特定することができる。また、検出される線分が複数でありランドマークとなる線分が複数である場合にも、同様にした誤差を考慮して線分の対応関係を特定することができる。
【００４９】
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【００５０】
１０：自律移動体
２０：記憶装置
３０：レーザレンジファインダ
４０ａ：サーボモータ
４０ｂ：サーボモータ
４２ａ：サーボ制御回路
４２ｂ：サーボ制御回路
４４ａ：モータ
４４ｂ：モータ
４６ａ：エンコーダ
４６ｂ：エンコーダ
５０：制御装置
【技術分野】
【０００１】
本発明は、床面上を移動する移動体の位置を測定する技術に関する。
【背景技術】
【０００２】
特許文献１には、自己の位置と角度（向き）（以下では、位置と角度をまとめて位置情報という）を検出しながら床面上を移動する移動体が開示されている。この移動体は、床面と平行な方向に直線状に伸びる障害物（例えば、壁等）の表面形状を示すデータを複数個記憶している。表面形状データは、床面を基準とするグローバル座標系により表されている。この移動体は、床面上を移動しながら、自己の位置情報を算出するとともに、拡張カルマンフィルタ（以下、ＥＫＦという）によって自己の位置情報を修正する。具体的には、移動体は、移動しながら以下の処理を繰り返す。まず、オドメトリにより床面上における自己の位置情報（グローバル座標系における位置情報）を算出する。次に、レーザレンジファインダにより光線を走査して、走査面上に存在する障害物の床面と平行な方向における表面形状を検出する。レーザレンジファインダによれば、障害物の表面形状が移動体に対する相対座標系上の座標点群として検出される。次に、移動体は、座標点群を直線近似した障害物の表面形状を算出する。次に、記憶している表面形状データの中から、検出した表面形状（直線近似した表面形状）に対応する表面形状データを特定する。この特定は、表面形状の検出誤差（すなわち、レーザレンジファインダによる検出誤差）を考慮して行われる。次に、特定した表面形状データと検出した表面形状とのずれを算出し、そのずれから自己の位置情報を修正する。このように、障害物の表面形状データを移動体に予め記憶させておくとともに、移動しながらレーザレンジファインダ等により障害物の表面形状を検出し、検出した表面形状とそれに対応する表面形状データのずれに基づいて自己の位置情報を修正することで、より正確に自己の位置情報を特定することができる。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】A. Garulli, A. Giannitrapani, A. Rossi, A. Vicino, "Mobile robot SLAM for line-based environment representation," Tech. Rep. 2005-3, Dipartimento di Ingegneria dell'Informazone, Universita di Siena, 2005
【発明の概要】
【発明が解決しようとする課題】
【０００４】
障害物の表面形状を検出する際には、誤差が生じる。このため、検出した表面形状に対応する表面形状データを複数の表面形状データの中から正確に特定するためには、誤差を考慮する必要がある。上述した非特許文献１の技術では、検出された表面形状の誤差（すなわち、レーザレンジファインダによる検出誤差）を考慮して対応する表面形状データの特定を行っているが、この技術によっても対応する表面形状データを正確に特定できない場合がある。このため、自己の位置情報を正確に修正できない場合があった。
【０００５】
本明細書では、上述した実情を鑑み、検出した表面形状に対応する表面形状データをより正確に特定し、自己の位置情報をより正確に修正することができる移動体を提供する。
【課題を解決するための手段】
【０００６】
移動体により検出される障害物の表面形状は、移動体に対する相対座標系により表される。一方、移動体に記憶されている表面形状データは、グローバル座標系により表される。したがって、検出された表面形状に対応する表面形状データは、検出された表面形状と、記憶されている表面形状データと、移動体の位置情報に基づいて特定される。このため、この特定には、検出された表面形状の誤差だけでなく、移動体の位置情報の誤差も影響する。本発明者らは、この点に着目し、移動体の位置情報の誤差も考慮して、検出された表面形状に対応する表面形状データを特定することを提案する。
【０００７】
本願明細書が開示する移動体は、障害物が存在する床面上を移動する。この移動体は、記憶部と、自己位置・角度算出部と、障害物検出部と、マッチング部と、自己位置・角度修正部を有している。記憶部は、床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを複数個記憶する。自己位置・角度算出部は、移動体の位置及び角度をグローバル座標系により算出する。障害物検出部は、光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する。マッチング部は、自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された表面形状及びその誤差に基づいて、記憶部に記憶されている複数の表面形状データの中から障害物検出部で検出された表面形状に対応する表面形状データを特定する。自己位置・角度修正部は、障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する。
【０００８】
なお、上記の「グローバル座標系」及び「相対座標系」は、３次元座標系としてもよいし、２次元座標系としてもよい。「グローバル座標系」及び「相対座標系」を２次元座標系とすると、移動体で行う計算量を少なくすることができる。２次元座標系を採用する場合、床面と平行な２つの座標軸により規定される座標系（例えば、直交座標系）であってもよいし、床面と平行な平面内における距離と角度により規定される極座標系であってもよい。また、表面形状データの座標系と移動体の位置及び角度の座標系が異なっていてもよい。例えば、表面形状データが極座標系により表され、移動体の位置が直交座標系により表されてもよい。また、表面形状データ、移動体の位置及び角度、並びに、障害物検出部が検出する障害物の表面形状は、少なくとも床面と平行な方向における形状（座標成分）により規定されていればよく、床面と垂直な方向の座標成分を含んでいてもよい。例えば、表面形状データは、床面と垂直な方向の座標成分を含めた障害物の表面の３次元形状を示すデータであってもよい。
また、「障害物検出部で検出された表面形状とマッチング部で特定された表面形状データのずれ」は、相対座標系で示される「障害物検出部で検出された表面形状」とグローバル座標系で示される「マッチング部で特定された表面形状データ」とを同一座標系で比較したときのずれを意味する。したがって、「障害物検出部で検出された表面形状」をグローバル座標系に座標変換し、グローバル座標系でこれらのずれを算出してもよい。また、「マッチング部で特定された表面形状データ」を相対座標系に座標変換し、相対座標系でこれらのずれを算出してもよい。
また、上記の「誤差」とは、ある特性値を検出（または算出等）するときに、現実の特性値と検出値との間に生じ得るずれの大きさを示す値を意味する。
また、障害物検出部が検出する障害物の表面形状は、光線の走査面上に存在する障害物の表面形状を複数の座標点により表すものであってもよいし、２つの座標点により規定される直線（または線分）により表面形状を表すものであってもよい。直線（または線分）により障害物の表面形状を表す場合には、例えば、光線の走査面上に存在する障害物の表面形状を複数の座標点により検出するレーザレンジファインダと、レーザレンジファインダが検出した複数の座標点を直線近似した直線（または線分）を算出する演算部により障害物検出部を構成することができる。
また、自己位置・角度算出部は、オドメトリによって移動体の位置と角度を算出してもよいし、ＧＰＳによって移動体の位置と角度を算出してもよいし、その他の技術によって算出してもよい。
【０００９】
この移動体では、移動中に、自己位置・角度算出部が移動体の位置と角度を算出する。また、移動中に、障害物検出部が、障害物の表面形状を検出する。そして、マッチング部が、記憶部に記憶されている複数の表面形状データの中から障害物検出部で検出された表面形状に対応する表面形状データを特定する。このとき、マッチング部は、障害物検出部で検出された表面形状の誤差のみならず、移動体の位置及び角度の誤差も考慮して、検出された表面形状に対応する表面形状データを特定する。したがって、検出された表面形状に対応する表面形状データを正確に特定することができる。自己位置・角度修正部は、特定された表面形状データと検出された表面形状とのずれに基づいて、移動体の位置と角度を修正する。対応する表面形状データが正確に特定されているので、自己位置・角度修正部は正確に移動体の位置と角度を修正することができる。
【００１０】
上述した移動体は、障害物検出部が、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出することが好ましい。また、移動体は、検出された線分の両端のみの位置の誤差に基づいて、障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれの誤差を算出するずれ誤差算出部をさらに有していることが好ましい。そして、自己位置・角度修正部は、ずれ誤差算出部によって算出された誤差に基づいて、移動体の位置及び角度を修正することが好ましい。
【００１１】
このような構成によれば、検出された線分の両端の位置の誤差のみからずれの誤差が算出されるので、表面形状全体の座標点の誤差からずれの誤差を算出する場合に比べて、移動体で行う計算量を低減することができる。これにより、より高速に移動体の位置と角度を特定することが可能となる。
【００１２】
上述した移動体では、障害物検出部が、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出することが好ましい。また、マッチング部は、検出された線分の両端の位置の誤差に基づいて、その両端の両方が同一の表面形状データ上に存在する確率を複数の表面形状データについて算出し、算出された確率が最も高い表面形状データを特定することが好ましい。
【００１３】
このような構成によれば、検出された線分の両端の表面形状データ上への存在確率に基づいて対応する表面形状データが特定されるので、表面形状全体の座標点の存在確率を算出する場合に比べて、移動体で行う計算量を低減することができる。これにより、より高速に移動体の位置と角度を特定することが可能となる。
【００１４】
また、本明細書は、以下の移動体も提供する。この移動体は、障害物が存在する床面上を移動する。この移動体は、記憶部と、自己位置・角度算出部と、障害物検出部と、マッチング部と、自己位置・角度修正部を有する。記憶部は、床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを記憶する。自己位置・角度算出部は、移動体の位置及び角度をグローバル座標系により算出する。障害物検出部は、光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する。マッチング部は、障害物検出部で複数の表面形状が検出されたときに、自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された複数の表面形状及びそれらの誤差に基づいて、障害物検出部で検出された複数の表面形状の中から記憶部に記憶されている表面形状データに対応する表面形状を特定する。自己位置・角度修正部は、マッチング部で特定された表面形状と記憶部に記憶されている表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する自己位置・角度修正部を有する。
【００１５】
この移動体によれば、障害物検出部で複数の表面形状が検出されたときに、これらの表面形状の中から記憶部に記憶されている表面形状データに対応する表面形状を正確に特定することができる。したがって、正確に移動体の位置及び角度を修正することができる。
【図面の簡単な説明】
【００１６】
【図１】自律移動体１０の概略斜視図。
【図２】自律移動体１０のブロック図。
【図３】環境マップの説明図。
【図４】レーザレンジファインダ３０の測定方法の説明図。
【図５】自律移動体１０の角度の説明図。
【図６】制御装置５０が実行する処理を示すフローチャート。
【図７】対象線分１２０の算出方法を示す説明図。
【図８】ステップＳ１０の詳細な処理を示すフローチャート。
【図９】線分上の点Ｐ及び領域Ｑについての説明図。
【図１０】対象線分１２０に対応する線分に関する説明図。
【図１１】対象線分１２０と対応する線分１００ｆの位置情報の説明図。
【図１２】２つの線分が検出された場合を示す説明図。
【発明を実施するための形態】
【００１７】
以下に説明する実施例の好ましい特徴を整理する。
（特徴１）記憶部は、障害物の表面形状を表す複数の線分を示すデータを記憶している。
（特徴２）移動体は、障害物検出部で検出された表面形状を直線近似した線分を算出する線分算出部を有している。
（特徴３）マッチング部は、記憶部が記憶している複数の線分の中から、線分算出部で算出した線分（対象線分）に対応する線分を特定する。
（特徴４）自己位置・角度修正部は、対象線分とこれに対応する線分とのずれ、及び、当該ずれの誤差を算出し、当該ずれ及び当該ずれの誤差に基づいて、移動体の位置及び角度を修正する。
【実施例】
【００１８】
図１は、実施例に係る自律移動体１０の概略斜視図を示している。自律移動体１０は、２つの駆動輪１２ａ、１２ｂと従動輪１２ｃを有しており、駆動輪１２ａ、１２ｂを駆動することで水平な床面上を移動する。従動輪１２ｃは、自律移動体１０の移動に伴って回転する。駆動輪１２ａと駆動輪１２ｂを同じ回転数で駆動すると、自律移動体１０は図１の矢印Ｆ１の向きに並進する。また、駆動輪１２ａと駆動輪１２ｂを異なる回転数で駆動すると、自律移動体１０は床面上をカーブしながら移動する。自律移動体１０は、床面上における自己の位置を算出しながら、目的地まで自律的に移動する。
【００１９】
図２は、自律移動体１０の各部の機能ブロック図を示している。自律移動体１０は、記憶装置２０と、レーザレンジファインダ３０と、サーボモータ４０ａ、４０ｂと、制御装置５０を備えている。図１に示すように、レーザレンジファインダ３０は、自律移動体１０の上部に設置されている。記憶装置２０、サーボモータ４０ａ、４０ｂ、及び、制御装置５０は、自律移動体１０内に組み込まれている。
【００２０】
記憶装置２０は、自律移動体１０が移動する床面上に存在する障害物（壁等）の表面形状を示す環境マップを記憶している。環境マップは、水平面内における障害物の表面形状を線分により表したデータである。図３は、環境マップが表す線分群の一例を示している。図３では、線分１００ａ〜１００ｊが示されている。環境マップは、始点と終点の座標により各線分１００ａ〜１００ｈを規定している。始点と終点の座標は、床面を基準して水平方向に設定されたｘ軸とｙ軸による直交座標系（以下、グローバル座標系という）により定められている。例えば、図３の線分１００ａは、始点座標（ｘａ１，ｙａ１）と終点座標（ｘａ２，ｙａ２）により表される。線分１００ｂは、始点座標（ｘｂ１，ｙｂ１）と終点座標（ｘｂ２，ｙｂ２）により表される。図３の例では、線分１００ａの終点座標（ｘａ２，ｙａ２）と線分１００ｂの始点座標（ｘｂ１，ｙｂ１）は等しい。各線分の始点及び終点は、始点から終点に向かうベクトルの右側が障害物側（自律移動体１０が進入できない領域）であり、ベクトルの左側が自律移動体１０が移動可能な領域であることを示している。図３においては、線分１００ａ〜１００ｊに囲まれた領域が自律移動体１０が移動可能な領域であり、その外側の領域が障害物（自律移動体１０が進入できない領域）である。
【００２１】
レーザレンジファインダ３０は、水平面内に光線を走査して、その走査面内に存在する障害物の表面形状を検出する。図４は、レーザレンジファインダ３０を上側から示した平面図である。レーザレンジファインダ３０は、最初に、自律移動体１０の進行方向Ｆ１より角度ｗ１（水平面内で見た時における角度）だけ右側にレーザ光を照射する。そして、照射したレーザ光の反射光を検出して角度ｗ１の方向に存在する障害物までの距離ｒ１（水平面内における距離）を検出する。すなわち、レーザ光が照射された箇所の自律移動体１０に対する相対位置座標（ｒ１，ｗ１）を検出する。次に、開始角度ｗ１よりも左側に微小角度ずれた角度ｗ２の方向にレーザ光を照射し、レーザ光が照射された箇所までの距離ｒ２を検出する。すなわち、レーザ光が照射された箇所の自律移動体１０に対する相対位置座標（ｒ２，ｗ２）を検出する。レーザレンジファインダ３０は、開始角度ｗ１から終了角度ｗｎ（自律移動体１０の進行方向Ｆ１より左側の角度）まで一定の角度間隔で同様の処理を繰り返し、レーザが照射された箇所の自律移動体１０に対する相対位置座標（ｒ１，ｗ１），（ｒ２，ｗ２），（ｒ３，ｗ３）・・・（ｒｎ，ｗｎ）を検出する。レーザレンジファインダ３０により検出される座標点群（ｒ１，ｗ１）〜（ｒｎ，ｗｎ）は、障害物の表面形状を表す。レーザレンジファインダ３０が検出した障害物の表面形状は、制御装置５０に入力される。
【００２２】
サーボモータ４０ａは、サーボ制御回路４２ａ、モータ４４ａ、エンコーダ４６ａを有している。モータ４４ａは、駆動輪１２ａを回転させる。サーボ制御回路４２ａには、制御装置５０から制御指令値が入力される。サーボ制御回路４２ａは、入力された制御指令値にしたがってモータ４４ａを駆動させる。したがって、駆動輪１２ａが、制御指令値に従って回転する。エンコーダ４６ａは、モータ４４ａ（すなわち、駆動輪１２ａ）の回転数を検出する。エンコーダ４６ａが検出する回転数は、制御装置５０に入力される。
サーボモータ４０ｂは、サーボ制御回路４２ｂ、モータ４４ｂ、エンコーダ４６ｂを有している。モータ４４ｂは、駆動輪１２ｂを回転させる。サーボ制御回路４２ｂには、制御装置５０から制御指令値が入力される。サーボ制御回路４２ｂは、入力された制御指令値にしたがってモータ４４ｂを駆動させる。したがって、駆動輪１２ｂが、制御指令値に従って回転する。エンコーダ４６ｂは、モータ４４ｂ（すなわち、駆動輪１２ｂ）の回転数を検出する。エンコーダ４６ｂが検出する回転数は、制御装置５０に入力される。
【００２３】
制御装置５０は、記憶装置２０、レーザレンジファインダ３０、及び、サーボモータ４０ａ、４０ｂと電気的に接続されている。制御装置５０は、記憶装置２０からデータを読み出すことができ、また、記憶装置２０にデータを記憶させることができる。制御装置５０は、後述する位置と角度の算出処理によって、自律移動体１０の位置（水平面内における自律移動体１０の位置）と角度（水平面内における自律移動体１０の進行方向Ｆ１の向き）を算出する。なお、自律移動体１０の位置は、上述したグローバル座標系のｘｙ座標によって表され、自律移動体１０の角度は、図５に示すように、グローバル座標系のｘ軸に対する自律移動体１０の進行方向Ｆ１の角度φにより表される。以下では、自律移動体１０の位置と角度を合わせて、位置情報という場合がある。制御装置５０は、算出した自律移動体１０の位置情報に基づいて、サーボモータ４０ａ、４０ｂに制御指令値を入力する。制御装置５０が、自律移動体１０の位置情報を算出し、その位置情報に基づいてサーボモータ４０ａ、４０ｂに制御指令値を入力する処理を繰り返すことで、自律移動体１０が目標地点に向かって自律的に移動する。
【００２４】
次に、制御装置５０が実行する処理について詳細に説明する。図６のフローチャートは、制御装置５０が実行する処理を示している。図６の処理の開始時点において、制御装置５０に、自律移動体１０の初期位置情報（ｘ０，ｙ０，φ０）（処理の開示時点における自律移動体１０の位置と角度）と、目標地点の位置が入力される。制御装置５０は、図６に示すステップＳ２〜Ｓ１６を一定の周期で繰り返して、自律移動体１０の位置情報を算出しながら移動する。
【００２５】
ステップＳ２では、制御装置５０は、現在の自律移動体１０の位置情報と、目標地点の位置と、環境マップ（障害物の位置）に基づいて、自律移動体１０の今後の経路を算出する。なお、最初のステップＳ２では、制御装置５０は、開始時に入力された初期位置情報（ｘ０，ｙ０，φ０）を現在の位置情報として経路を算出する。２回目以降のステップＳ２では、制御装置５０は、直前の周期のステップＳ１６で修正された自律移動体１０の位置情報Ｘｋ−１｜ｋ−１（＝ｘｋ−１｜ｋ−１，ｙｋ−１｜ｋ−１，φｋ−１｜ｋ−１）を現在の位置情報として経路を算出する。経路を算出したら、制御装置５０は、算出した経路に沿って自律移動体１０が移動するように、サーボモータ４０ａ、４０ｂに制御指令値を入力する。サーボモータ４０ａ、４０ｂは、次のステップＳ２が実行されるまで、直前のステップＳ２で入力された制御指令値にしたがって駆動輪１２ａ、１２ｂを駆動する。これによって、自律移動体１０が移動する。また、駆動輪１２ａ、１２ｂが駆動すると、エンコーダ４６ａ、４６ｂによって駆動輪１２ａ、１２ｂの回転数が検出される。
【００２６】
ステップＳ２を実行してから所定時間経過後に、ステップＳ４〜Ｓ１６の処理が実行される。これによって、現在の自律移動体１０の位置情報が算出される。ステップＳ４〜Ｓ１６の処理は、拡張カルマンフィルタに基づいて行われる。
【００２７】
ステップＳ４では、制御装置５０は、オドメトリによって自律移動体１０の現在の位置情報Ｘｋ｜ｋ−１（＝（ｘｋ｜ｋ−１，ｙｋ｜ｋ−１，φｋ｜ｋ−１））を算出する。具体的には、制御装置５０は、エンコーダ４６ａ、４６ｂが検出する駆動輪１２ａ、１２ｂの回転数と、直前の周期のステップＳ１６で修正された自律移動体１０の位置情報Ｘｋ−１｜ｋ−１（過去の位置情報）に基づいて、自律移動体１０の現在の位置情報Ｘｋ｜ｋ−１を算出する。
また、ステップＳ４では、位置情報Ｘｋ｜ｋ−１の算出と同時に、位置情報Ｘｋ｜ｋ−１の誤差ΣＲを算出する。なお、誤差ΣＲは、位置（ｘｋ｜ｋ−１，ｙｋ｜ｋ−１）と角度φｋ｜ｋ−１に生じうる誤差を分散（ｖｘｋ｜ｋ−１，ｖｙｋ｜ｋ−１，ｖφｋ｜ｋ−１）により表したものである。誤差ΣＲは、文献「S. Thrun, W.burgard, D. Fox, "Probabilistic robotics", The MIT Press, 2005」に記載の技術に従って算出することができる。すなわち、誤差ΣＲは、過去に自律移動体１０が移動した経路に基づいて算出される。ｘ方向への移動距離（各周期におけるｘ方向の移動距離の絶対値を積算した値）が大きいほど誤差ｖｘｋ｜ｋ−１が大きく算出され、ｙ方向への移動距離が大きいほど誤差ｖｙｋ｜ｋ−１が大きく算出され、角度φの変化量（各周期における角度φの変化量の絶対値を積算した値）が大きいほど誤差ｖφｋ｜ｋ−１が大きく算出される。なお、誤差ΣＲは、上述した他の方法により算出してもよい。また、誤差ΣＲが経時的に変化しない場合には、誤差ΣＲが記憶装置２０に記憶されていてもよい。
【００２８】
ステップＳ６では、制御装置５０は、レーザレンジファインダ３０により空間内に光線を走査して、走査範囲内に存在する障害物の表面形状を検出する。上述したように、レーザレンジファインダ３０は、障害物の表面形状を、自律移動体１０に対する相対座標系による座標点群（ｒ１，ｗ１），（ｒ２，ｗ２），（ｒ３，ｗ３）・・・（ｒｎ，ｗｎ）として検出する。
【００２９】
ステップＳ８では、制御装置５０は、ステップＳ６でレーザレンジファインダ３０により検出された座標点群を直線近似することによって線分を算出する。ステップＳ８の線分の算出処理は、例えば、文献「J. Xavier, "Rast Line, Arc/Circle and Leg Detection from Laser Scan Data in Player Driver", ICRA, 2005」に従って実行することができる。具体的には、座標点群（ｒ１，ｗ１）〜（ｒｎ，ｗｎ）を直線近似することで、図７に示すように、線分１２０を算出する。この線分は、自律移動体１０を基準として水平方向に設定されたｓ軸とｔ軸による直交座標系（以下では、ローカル座標系という）により始点の座標（ｓｓ，ｔｓ）と終点の座標（ｓｅ，ｔｅ）を規定することにより表される。なお、始点（ｓｓ，ｔｓ）は、レーザレンジファインダ３０が光線の走査を開始する側（座標点（ｒ１，ｗ１）側）の線分の端部を示しており、終点（ｓｅ，ｔｅ）はその反対側（座標点（ｒｎ，ｗｎ）側）の線分の端部を示している。次に、制御装置５０は、相対座標系で表されている線分の始点の座標（ｓｓ，ｔｓ）と終点の座標（ｓｅ，ｔｅ）の座標を、グローバル座標系上の座標（ｘｓ，ｙｓ），（ｘｅ，ｙｅ）に座標変換する。この座標変換は、ステップＳ４で算出された自律移動体１０の位置情報Ｘｋ｜ｋ−１に基づいて行われる。以下では、ステップＳ８で算出した線分を対象線分という。
【００３０】
ステップＳ１０では、制御装置５０は、記憶装置２０が記憶する環境マップにより規定されている線分の中から、対象線分に対応する線分を特定する。対象線分に対応する線分の特定処理は、図８に示すフローチャートに従って実行される。
【００３１】
ステップＳ４０では、制御装置５０は、対象線分の始点の位置の誤差Σ１と終点の位置の誤差Σ２を算出する。誤差Σ１は、対象線分の始点の位置（ｘｓ，ｙｓ）に生じうる誤差を分散（ｖｘｓ，ｖｙｓ）で表したものであり、誤差Σ２は、対象線分の終点の位置（ｘｅ，ｙｅ）に生じうる誤差を分散（ｖｘｅ，ｖｙｅ）で表したものである。上述したように、対象線分は、レーザレンジファインダ３０により検出される座標点群と、自律移動体１０の位置情報Ｘｋ｜ｋ−１に基づいて算出される。このため、レーザレンジファインダ３０の測定誤差と、自律移動体１０の位置情報Ｘｋ｜ｋ−１の誤差ΣＲとの影響によって、対象線分の始点と終点の位置に誤差が生じる。このため、制御装置５０は、レーザレンジファインダ３０の測定誤差と、位置情報Ｘｋ｜ｋ−１の誤差ΣＲに基づいて、対象線分の始点の誤差Σ１と終点の誤差Σ２を算出する。具体的には、制御装置５０は、以下の数式によって、これらの誤差を算出する。
Σ１＝Ｊ１×ΣＲ×Ｊ１Ｔ＋ΣＯ
Σ２＝Ｊ２×ΣＲ×Ｊ２Ｔ＋ΣＯ
ここで、ΣＯは、レーザレンジファインダ３０の測定誤差に基づいて生じる始点及び終点の位置の誤差を、ｘ座標及びｙ座標の分散で表したものである。本実施例では、誤差ΣＯは固定値とみなせるため、誤差ΣＯが予め記憶装置２０に記憶されている。Ｊ１は、自律移動体１０の位置情報（ｘ，ｙ，φ）に関する線分の始点のヤコビ行列であり、Ｊ１Ｔはヤコビ行列Ｊ１の転置行列である。Ｊ２は、自律移動体１０の位置情報（ｘ，ｙ，φ）に関する線分の終点のヤコビ行列であり、Ｊ２Ｔはヤコビ行列Ｊ２の転置行列である。ヤコビ行列Ｊ１，Ｊ２は以下の数１により表される。
【数１】
【００３２】
ステップＳ４２では、始点の位置（ｘｓ，ｙｓ）とその誤差Σ１に基づいて、始点の位置に関する確率密度関数を算出する。また、終点の位置（ｘｅ，ｙｅ）とその誤差Σ２に基づいて、終点の位置に関する確率密度関数を算出する。始点の位置の確率密度関数は、始点が存在している確率をｘ座標とｙ座標の関数として表したものである。ｘｙ座標の範囲で確率密度関数を積分することで、その座標範囲内に始点が存在する確率を算出することができる。終点の確率密度関数も、始点の確率密度関数と同様に、終点が存在している確率をｘ座標とｙ座標の関数として表したものである。
【００３３】
ステップＳ４４〜Ｓ５０では、ステップＳ４２で算出した確率密度関数に基づいて、環境マップ中の複数の線分の中から、対象線分に対応する線分を特定する。
【００３４】
ステップＳ４４では、制御装置５０は、対象線分の向き（始点から終点に向かう向き）と同じ向きを有する線分を環境マップから抽出する。向きが等しいか否かは、対象線分を示すベクトル（始点から終点に向かうベクトル）と環境マップ中の各線分を示すベクトル（始点から終点に向かうベクトル）との内積がゼロより大きいか否かにより判定することで行う。すなわち、対象線分のベクトルに対する角度が９０度未満のベクトルを有する線分が、同じ向きを有する線分として特定される。上述したように、環境マップ中の線分の向きに対して右側の領域は障害物を示している。また、対象線分の向きはレーザレンジファインダ３０によるスキャン方向を示している。したがって、対象線分の向きとこれに対応する環境マップ中の線分の向きが逆となることは、レーザレンジファインダ３０が障害物の内部に存在することとなり、ありえない。したがって、ステップＳ４４で対象線分と向きが等しい線分を抽出し、ステップＳ４６の処理を抽出した線分に対してのみ行う。このように、向きが同じとなる線分のみを抽出してステップＳ４６を実行することで、制御装置５０の負荷が軽減される。
【００３５】
ステップＳ４６では、制御装置５０は、ステップＳ４４で抽出した線分のそれぞれに対して、その線分上に対象線分の始点と終点の両方が存在する確率を算出する。具体的には、抽出された線分の夫々に対して、以下の処理が実行される。
最初に、抽出された線分上の点を一定間隔で抽出する。なお、抽出された線分の始点と終点については、隣の点との間隔にかかわらず抽出する。次に、図９に示すように、抽出した各点（図９では点Ｐとして示されている）を中心としてその周囲に対角線の長さが前記間隔と等しい正方形の領域Ｑを設定する。次に、抽出した各点Ｐの座標をステップＳ４２で算出した始点についての確率密度関数に代入することで、点Ｐ毎に始点が点Ｐ上に存在する確立を算出する。次に、点Ｐ上に始点が存在する確率に、点Ｐに対応する正方形の領域Ｑの面積を乗算して、領域Ｑ毎に領域Ｑ内に始点が存在する確率を算出する。次に、線分に対して設定された全ての領域Ｑについての始点の存在確率の総和を算出する。算出された総和は、その線分上に始点が存在する確率として近似することができる。
次に、抽出した各点Ｐの座標をステップＳ４２で算出した終点についての確率密度関数に代入することで、抽出した点Ｐ毎に終点が点Ｐ上に存在する確立を算出する。次に、点Ｐ上に終点が存在する確率に、点Ｐに対応する正方形の領域Ｑの面積を乗算して、領域Ｑ内に終点が存在する確率を算出する。次に、線分に対して設定された全ての領域Ｑについての終点の存在確率の総和を算出する。算出された総和は、その線分上に終点が存在する確率として近似することができる。
最後に、始点の存在確率と終点の存在確率とを乗算する。これによって、その線分上（より具体的には、その線分から所定の範囲内）に対象線分の始点と終端の両方が存在する確率が算出される。
【００３６】
ステップＳ４８では、制御装置５０は、ステップＳ４６で算出した確率の中で最大値を特定する。そして、特定した最大値が、閾値を超えているか否かを判定する。特定した最大値が閾値を超えていない場合には、制御装置５０はＮＵＬＬデータを報告し、図８の処理を中止する。ＮＵＬＬデータを報告した場合には、図６のステップＳ１２、１４は実行されず、次のステップＳ２は、修正されていない位置情報Ｘｋ｜ｋ−１（ステップＳ４で算出された位置情報）に基づいて実行される。特定した最大値が閾値を超えている場合には、ステップＳ５０において、その最大値を有する線分を、対象線分に対応する線分として特定する。以上により、図８の処理（すなわち、図６のステップＳ１０）が終了する。
【００３７】
以上に説明したように、ステップＳ４２では、レーザレンジファインダ３０の測定誤差ΣＯだけでなく、自律移動体１０の位置情報Ｘｋ｜ｋ−１の誤差ΣＲも考慮して、確率分布関数を算出する。そして、その確率分布関数に基づいて、対象線分に対応する線分を環境マップ中から特定する。このように、時々刻々と変化する誤差ΣＲを考慮することで、より正確に対象線分に対応する線分を特定することができる。例えば、環境マップ中の線分１００ｄ〜１００ｆと対象線分１２０が図１０に示す位置関係になった場合を考えると、自律移動体１０の位置情報の誤差がｘ方向に大きければ、対応する線分として線分１００ｆが特定される。自律移動体１０の位置情報の誤差がｙ方向に大きければ、対応する線分として線分１００ｄが特定される。このように、自律移動体１０の位置情報の誤差を考慮することで、正確な対応付けを行うことが可能となる。
【００３８】
図６のステップＳ１２では、制御装置５０は、対象線分と、対応する線分の夫々に対して、自律移動体１０に対する相対位置情報を算出する。これらの線分の相対位置情報は、自律移動体１０までの距離と自律移動体１０の進行方向Ｆ１に対する角度により表される。例えば、図１１に示すように、対象線分１２０とこれに対応する線分１００ｆが特定されている場合には、対象線分１２０の相対位置情報は、自律移動体１０までの距離ｒａ（対象線分の垂線方向における距離）と、自律移動体１０の進行方向Ｆ１に対する角度ｗａ（対象線分の垂線と進行方向Ｆ１の間の角度）により算出される。また、対応する線分１００ｆの相対位置情報は、自律移動体１０までの距離ｒｂ（対応する線分の垂線方向における距離）と、自律移動体１０の進行方向Ｆ１に対する角度ｗｂ（対応する線分の垂線と進行方向Ｆ１の間の角度）により算出される。次に、制御装置５０は、対象線分と対応する線分との相対位置情報のずれ（ｄｒ，ｄｗ）を算出する。長さのずれｄｒは、距離ｒａから距離ｒｂを減算した値であり、角度のずれｄｗは、角度ｗａから角度ｗｂを減算した値である。
【００３９】
ステップＳ１４では、制御装置５０は、上述したずれ（ｄｒ，ｄｗ）の誤差Σ３（＝（ｖｄｒ，ｖｄｗ））を算出する。なお、誤差ｖｄｒは距離のずれｄｒを算出する際に生じうる誤差を表す分散であり、誤差ｖｄｗは角度のずれｄｗを算出する際に生じうる誤差を表す分散である。対応する線分の位置情報は環境マップで規定されたものであるので、上述したずれの誤差Σ３は、対象線分の相対位置情報（ｒａ，ｗａ）の検出誤差と等しい。
したがって、距離の誤差ｖｄｒは、対象線分の始点及び終点の誤差ΣＯを以下の計算式により座標変換することで算出される。
ｖｄｒ＝Ｊ３×ΣＯ×Ｊ３Ｔ
ここで、Ｊ３は、対象線分の始点と終点の位置に関する距離ｒａのヤコビ行列であり、Ｊ３Ｔはヤコビ行列Ｊ３の転置行列である。ヤコビ行列Ｊ３は以下の数２により表される。
【数２】
また、角度の誤差ｖｄｗは、対象線分の始点及び終点の誤差ΣＯを以下の計算式により座標変換することで得られる。
ｖｄｗ＝Ｊ４×ΣＯ×Ｊ４Ｔ
ここで、Ｊ４は、対象線分の始点と終点の位置に関する角度ｗａのヤコビ行列であり、Ｊ４Ｔはヤコビ行列Ｊ４の転置行列である。ヤコビ行列Ｊ４は以下の数３により表される。
【数３】
【００４０】
ステップＳ１６では、制御装置５０は、ステップＳ１２で算出した相対位置のずれ（ｄｒ，ｄｗ）と、ステップＳ１４で算出したずれの誤差Σ３（＝（ｖｄｒ，ｖｄｗ））に基づいて、ステップＳ４で算出した自律移動体１０の位置情報Ｘｋ｜ｋ−１（＝（ｘｋ｜ｋ−１，ｙｋ｜ｋ−１，φｋ｜ｋ−１）を修正し、修正後の位置情報Ｘｋ｜ｋ（＝（ｘｋ｜ｋ，ｙｋ｜ｋ，φｋ｜ｋ）を算出する。自律移動体１０の位置情報の修正は、一般的なＥＫＦの手法により行われる。例えば、文献「S. Thrun, W.burgard, D. Fox, "Probabilistic robotics", The MIT Press, 2005」の手法により自律移動体１０の位置情報を修正することができる。このように位置を修正することで、より正確に自律移動体１０の位置情報を特定することができる。
【００４１】
その後、制御装置５０は、再度ステップＳ２からの処理を実行する。ステップＳ２では、修正後の正確な位置情報に基づいて経路が算出される。このように、位置情報の算出（ステップＳ４）、レーザレンジファインダ３０の検出値に基づく位置情報の修正（ステップＳ６〜Ｓ１６）、修正後の位置情報に基づく制御（ステップＳ２）が繰り返し行われることで、自律移動体１０が目標値まで正確に移動することができる。
【００４２】
以上に説明したように、実施例の自律移動体１０では、レーザレンジファインダ３０の検出誤差だけでなく、自律移動体１０の位置情報の誤差をも考慮して、対象線分に対応する線分を環境マップ中から特定する。このため、対象線分に対して誤った線分が対応付けられることが防止される。これにより、より正確に位置情報を修正することができる。
【００４３】
また、上述した自律移動体１０では、ステップＳ１４において対象線分とこれに対応する線分との相対位置のずれの誤差を算出するときに、対象線分の始点と終点の位置の誤差のみを考慮した。このように、レーザレンジファインダ３０が検出する各座標点の全ての検出誤差を考慮するのではなく、対象線分の両端点の位置の検出誤差のみを考慮することで、相対位置のずれの誤差の算出に要する演算量を低減することができる。これにより、より短い周期で図６のフローチャートを実行可能となり、より正確に自律移動体１０を移動させることが可能となる。
【００４４】
また、上述した自律移動体１０では、ステップＳ１０で対象線分に対応する線分を特定するときに、対象線分の両端点の存在確率のみを考慮した。このように、レーザレンジファインダ３０が検出する各座標点の全ての存在確率を考慮するのではなく、対象線分の両端点の存在確率のみを考慮することで、対象線分に対応する線分の特定処理に要する演算量を低減することができる。これにより、より短い周期で図６のフローチャートを実行可能となり、より正確に自律移動体１０を移動させることが可能となる。
【００４５】
なお、上述した実施例では自律移動体１０について説明したが、本発明は自律的に移動しない移動体に適用してもよい。例えば、移動体を遠隔操縦するときに、本発明により移動体の位置を特定してもよい。
【００４６】
上述した実施例では、移動領域内に他の移動体が存在しなかったが、移動領域内に他の移動体が存在してもよい。この場合は、レーザレンジファインダによって壁の表面形状と他の移動体の表面形状が検出される。このため、レーザレンジファインダによって検出された座標点群から、壁の表面形状に相当する座標点群を特定し、その座標点群に対して上述した処理を実行すればよい。
【００４７】
また、上述した実施例では、自律移動体の位置と角度の誤差を文献「S. Thrun, W.burgard, D. Fox, "Probabilistic robotics", The MIT Press, 2005」に記載の技術によって算出したが、位置と角度の誤差の算出には種々の方法を用いることができる。例えば、自律移動体の位置と角度の誤差を、自律移動体の運動状態を考慮して算出することができる。例えば、自律移動体の位置をＧＰＳセンサで検出する場合において、自律移動体の速度がｘ方向に大きく、ｙ方向に小さい場合は、ＧＰＳセンサの検出誤差を、ｘ方向の誤差が大きく、ｙ方向の誤差が小さくなるように算出する。自律移動体の運動状態を考慮することで、自律移動体の位置と角度の誤差を適切に算出することができる。
【００４８】
また、上述した実施例では、レーザレンジファインダ３０の検出結果から得られる線分が１つであり、この線分に対応する線分を環境マップの複数の線分の中から特定した。しかしながら、レーザレンジファインダ３０の検出結果から得られる線分が複数となる場合もある。また、環境マップ中の線分のうちの一部のみが自己位置・角度の修正の基準（すなわち、ランドマーク）として使用される場合がある。例えば、図１２に示すようにレーザレンジファインダ３０の検出結果から２つの線分１３０、１３２が得られており、線分１００ｘだけが自己位置・角度の修正の基準とされている場合を考える。例えば、線分１００ｘが示す壁面の前に別の障害物が置かれていたり、線分１００ｘが示す壁面に設けられた扉が開放されている場合等に、図１２に示すように１つの線分が検出されるはずの位置で２つの線分が検出される場合がある。この場合にも、レーザレンジファインダ３０の検出誤差だけでなく、自律移動体１０の位置情報の誤差をも考慮して、検出された線分１３０、１３２の中から線分１００ｘに対応する線分を特定することができる。対応する線分の特定は、上述した実施例と同様の演算により行うことができる。このように、検出される線分が１つでありランドマークとなる線分が複数である場合だけでなく、検出される線分が複数でありランドマークとなる線分が１つである場合にも、誤差を考慮して線分の対応関係を特定することができる。また、検出される線分が複数でありランドマークとなる線分が複数である場合にも、同様にした誤差を考慮して線分の対応関係を特定することができる。
【００４９】
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【００５０】
１０：自律移動体
２０：記憶装置
３０：レーザレンジファインダ
４０ａ：サーボモータ
４０ｂ：サーボモータ
４２ａ：サーボ制御回路
４２ｂ：サーボ制御回路
４４ａ：モータ
４４ｂ：モータ
４６ａ：エンコーダ
４６ｂ：エンコーダ
５０：制御装置
【特許請求の範囲】
【請求項１】
障害物が存在する床面上を移動する移動体であって、
床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを複数個記憶する記憶部と、
移動体の位置及び角度をグローバル座標系により算出する自己位置・角度算出部と、
光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する障害物検出部と、
自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された表面形状及びその誤差に基づいて、記憶部に記憶されている複数の表面形状データの中から障害物検出部で検出された表面形状に対応する表面形状データを特定するマッチング部と、
障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する自己位置・角度修正部、
を有することを特徴とする移動体。
【請求項２】
障害物検出部は、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出し、
検出された線分の両端の位置の誤差のみに基づいて、障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれの誤差を算出するずれ誤差算出部をさらに有しており、
自己位置・角度修正部は、ずれ誤差算出部によって算出された誤差に基づいて、移動体の位置及び角度を修正する、
ことを特徴とする請求項１に記載の移動体。
【請求項３】
障害物検出部は、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出し、
マッチング部は、障害物検出部で検出された線分の両端の位置の誤差に基づいて、その両端の両方が同一の表面形状データ上に存在する確率を複数の表面形状データについて算出し、算出された確率が最も高い表面形状データを特定することを特徴とする請求項１または２に記載の移動体。
【請求項４】
障害物が存在する床面上を移動する移動体であって、
床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを記憶する記憶部と、
移動体の位置及び角度をグローバル座標系により算出する自己位置・角度算出部と、
光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する障害物検出部と、
障害物検出部で複数の表面形状が検出されたときに、自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された複数の表面形状及びそれらの誤差に基づいて、障害物検出部で検出された複数の表面形状の中から記憶部に記憶されている表面形状データに対応する表面形状を特定するマッチング部と、
マッチング部で特定された表面形状と記憶部に記憶されている表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する自己位置・角度修正部、
を有することを特徴とする移動体。
【請求項１】
障害物が存在する床面上を移動する移動体であって、
床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを複数個記憶する記憶部と、
移動体の位置及び角度をグローバル座標系により算出する自己位置・角度算出部と、
光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する障害物検出部と、
自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された表面形状及びその誤差に基づいて、記憶部に記憶されている複数の表面形状データの中から障害物検出部で検出された表面形状に対応する表面形状データを特定するマッチング部と、
障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する自己位置・角度修正部、
を有することを特徴とする移動体。
【請求項２】
障害物検出部は、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出し、
検出された線分の両端の位置の誤差のみに基づいて、障害物検出部で検出された表面形状とマッチング部で特定された表面形状データとのずれの誤差を算出するずれ誤差算出部をさらに有しており、
自己位置・角度修正部は、ずれ誤差算出部によって算出された誤差に基づいて、移動体の位置及び角度を修正する、
ことを特徴とする請求項１に記載の移動体。
【請求項３】
障害物検出部は、障害物の床面と平行な方向における表面形状を相対座標系における線分として検出し、
マッチング部は、障害物検出部で検出された線分の両端の位置の誤差に基づいて、その両端の両方が同一の表面形状データ上に存在する確率を複数の表面形状データについて算出し、算出された確率が最も高い表面形状データを特定することを特徴とする請求項１または２に記載の移動体。
【請求項４】
障害物が存在する床面上を移動する移動体であって、
床面と平行な方向に直線状に伸びる障害物の床面と平行な方向における表面形状をグローバル座標系により表す表面形状データを記憶する記憶部と、
移動体の位置及び角度をグローバル座標系により算出する自己位置・角度算出部と、
光線を走査して、その走査面上に存在する障害物の床面と平行な方向における表面形状を移動体に対する相対座標系により検出する障害物検出部と、
障害物検出部で複数の表面形状が検出されたときに、自己位置・角度算出部で算出された移動体の位置及び角度並びにこれらの誤差と、障害物検出部で検出された複数の表面形状及びそれらの誤差に基づいて、障害物検出部で検出された複数の表面形状の中から記憶部に記憶されている表面形状データに対応する表面形状を特定するマッチング部と、
マッチング部で特定された表面形状と記憶部に記憶されている表面形状データとのずれから、自己位置・角度算出部によって算出された位置及び角度を修正する自己位置・角度修正部、
を有することを特徴とする移動体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１１−２４８６４８（Ｐ２０１１−２４８６４８Ａ）
【公開日】平成２３年１２月８日（２０１１．１２．８）
【国際特許分類】
【出願番号】特願２０１０−１２１４５２（Ｐ２０１０−１２１４５２）
【出願日】平成２２年５月２７日（２０１０．５．２７）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２３年１２月８日（２０１１．１２．８）
【国際特許分類】
【出願日】平成２２年５月２７日（２０１０．５．２７）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]