移動ロボット、自己位置補正方法および自己位置補正プログラム
【課題】演算量の増加を抑えつつ、正確に自己位置を推定することのできる移動ロボットを提供する。
【解決手段】自律移動可能な移動ロボット１であって、自己位置を推定する自己位置推定部１６と、移動ロボット１が移動可能な領域を点集合データで示す環境地図を保持する環境地図保持部１８１と、移動ロボット１の周囲の移動可能な領域を検知する経路センサ１７と、経路センサ１７による検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成部１８４と、環境地図中の対応点データの候補を、周囲環境情報中の所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定部１８６と、限定された点を所定の点データに対する対応点データとして、環境地図中の各点データと、周囲環境情報中の対応点データとの間の位置ずれ量分自己位置を補正する位置補正部１８８とを備えた。
【解決手段】自律移動可能な移動ロボット１であって、自己位置を推定する自己位置推定部１６と、移動ロボット１が移動可能な領域を点集合データで示す環境地図を保持する環境地図保持部１８１と、移動ロボット１の周囲の移動可能な領域を検知する経路センサ１７と、経路センサ１７による検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成部１８４と、環境地図中の対応点データの候補を、周囲環境情報中の所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定部１８６と、限定された点を所定の点データに対する対応点データとして、環境地図中の各点データと、周囲環境情報中の対応点データとの間の位置ずれ量分自己位置を補正する位置補正部１８８とを備えた。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律移動可能な移動ロボット、自己位置補正方法および自己位置補正プログラムに関するものである。
【背景技術】
【０００２】
従来、ロボットが自律走行を行うためには、自己位置の認識が必要である。車輪型ロボットの場合は、車輪の回転数を計測することによる自己位置推定が一般的に行われている。また、車輪の回転数の計測だけでは走行距離に応じて誤差が蓄積していくため、走行経路上にロボットが自己位置を確認するためのロボット専用ランドマークを設置するなどして、このような蓄積誤差を解消する手法が多く提案されていた。
【０００３】
例えば、車輪の回転数と光ファイバジャイロを用いてロボットの自己位置（ｘ、ｙ、θ）を推定する方法が知られている。タイヤスリップなどの蓄積誤差を減少させるため、ロボット専用ランドマークをロボット走行経路上に一定間隔で設置し、ロボットがこれを読み取ることでロボット自己位置推定の精度を保つものである。
【０００４】
しかしながら、上記方法においては、ロボット専用ランドマークの設置や調整に労力がかかる、美観や建物構造の制約により、ロボット専用ランドマークが設置できないなどの問題があった。
【０００５】
そのため、近年、走行経路の周囲の壁などを環境地図として事前に記憶しておき、ロボットが走行中にレーザレンジセンサで計測した周囲の距離情報（壁形状）とのマッチング処理を行うことで、ロボットの自己位置を推定する手法が登場した。このマッチング処理は、ＩＣＰアルゴリズムを適用することによって、ロボット自律走行への適用が可能となる（例えば、「非特許文献１」参照）。
【０００６】
また、特許文献１には、ロボット周囲の距離情報を計測し、予め記憶していた環境地図との比較を行うことで自己位置同定を行う方法が開示されている。環境地図はバイナリトリー形式で構成され、計測距離情報から線分情報を抽出し、最尤位置候補を選定する。
【０００７】
【特許文献１】特開平４−２１６４８７号公報
【非特許文献１】"レーザレンジファインダ搭載移動ロボットによる動的環境の３次元地図生成", 映像情報メディア学会技術報告, Vol.30, No.36, pp.25-30, 浜松, July 2006（中本 琢実, 山下 淳, 金子 透）http://robotics.caltech.edu/~atsushi/paper/E/E078Final.pdf
【発明の開示】
【発明が解決しようとする課題】
【０００８】
ロボットが記憶している環境地図は、グローバル座標と呼ばれる二次元の絶対座標上に表され、ロボットが走行する周囲の壁や物体の外形を表す点集合である。ＩＣＰアルゴリズムなどでマッチング処理を行う際、同一の特徴を持った点集合同士であれば対応点が一致することによりマッチング精度が向上する。
【０００９】
また、ＩＣＰアルゴリズムの中で行っている対応点検索処理は、点集合Ａ，Ｂにおいてユークリッド距離が最小となる組合せを求めるものである。全検索を行った場合、演算量が(点集合Ａの要素数)×(点集合Ｂの要素数)となり、点集合が大きくなるにつれて計算時間が増大する。ＩＣＰアルゴリズムの計算時間が増大すると、ロボットの走行スピードや加減速度が大きくなとともに自己位置推定誤差が増大するという問題があった。
【００１０】
本発明は、上記に鑑みてなされたものであって、演算量の増加を抑えつつ、正確に自己位置を推定することのできる移動ロボット、自己位置補正方法および自己位置補正プログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上述した課題を解決し、目的を達成するために、請求項１にかかる発明は、自律移動可能な移動ロボットであって、当該移動ロボットの移動量および移動方向を検知する移動センサと、前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定手段と、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段と、当該移動ロボットの周囲の移動可能な領域を検知する経路センサと、前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成手段と、前記周囲環境情報中の所定の点データに対応する点データであって、前記点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定手段と、前記検索対象点限定手段により限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正する位置補正手段とを備えたことを特徴とする。
【００１２】
また、請求項２にかかる発明は、請求項１に記載の移動ロボットであって、前記移動ロボットが移動可能な領域を線データで示す線環境地図を保持する線環境地図保持手段と、前記線環境地図保持手段が保持する前記線環境地図から所定の密度の点で示された前記点環境地図を生成する点環境地図生成手段とをさらに備え、前記点環境地図保持手段は、前記点環境地図生成手段により生成された前記点データを保持することを特徴とする。
【００１３】
また、請求項３にかかる発明は、請求項１または２に記載の移動ロボットであって、前記周囲環境情報生成手段により生成された前記点集合データの点データの間隔が前記点環境地図保持手段が保持する前記点環境地図中の点集合データの点データの間隔に比べて狭い場合に、前記周囲環境情報中の点データの間隔が前記点環境地図中の点データの間隔になるよう前記点データを間引く周囲環境情報調整手段をさらに備えたことを特徴とする。
【００１４】
また、請求項４にかかる発明は、請求項１から３のいずれか一項に記載の移動ロボットであって、ＩＣＰアルゴリズムを用いて前記検索対象点限定手段により限定された後の前記点環境地図中の各点データと、前記対応点データとの間の位置ずれ量を算出する位置ずれ量算出手段をさらに備え、前記自己位置補正手段は、前記位置ずれ量算出手段により算出された前記位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正することを特徴とする。
【００１５】
また、請求項５にかかる発明は、自律移動可能な移動ロボットの自己位置補正方法であって、移動センサが当該移動ロボットの移動量および移動方向を検知する第１検知ステップと、前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、経路センサが当該移動ロボットの周囲の移動可能な領域を検知する第２検知ステップと、前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成ステップと、前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップとを有することを特徴とする。
【００１６】
また、請求項６にかかる発明は、自律移動可能な移動ロボットの自己位置補正処理をコンピュータに実行させる自己位置補正プログラムであって、当該移動ロボットの移動量および移動方向を検知する移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、当該移動ロボットの周囲の移動可能な領域を検知する経路センサによる検出結果を点集合データで示す周囲環境情報を取得する周囲環境情報取得ステップと、前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップとを有することを特徴とする。
【発明の効果】
【００１７】
本発明によれば、移動センサが、当該移動ロボットの移動量および移動方向を検知し、自己位置推定手段が、移動センサによる検出結果に基づいて、自己位置を推定し、点環境地図保持手段が、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持し、経路センサが、当該移動ロボットの周囲の移動可能な領域を検知し、周囲環境情報生成手段が、経路センサによる検出結果を点集合データで示す周囲環境情報を生成し、検索対象点限定手段が、周囲環境情報中の所定の点データに対応する点データであって、点環境地図中の点データである対応点データの候補を、点環境地図中の点データのうち所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定し、位置補正手段が、検索対象点限定手段により限定された点を所定の点データに対する対応点データとして、点環境地図中の各点データと、周囲環境情報中の対応点データとの間の位置ずれ量分自己位置推定手段により推定された自己位置を補正するので、演算量の増加を抑えつつ、正確に自己位置を推定することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１８】
以下に添付図面を参照して、この発明にかかる移動ロボット、自己位置補正方法および自己位置補正プログラムの最良な実施の形態を詳細に説明する。
【００１９】
図１は、実施の形態にかかる移動ロボット１の全体構成を示す図である。移動ロボット１は、走行制御部１０と、モーター１１と、車輪１２と、位置推定センサ１３と、自己位置推定部１６と、ＬＲＳ（Ｌａｓｅｒ Ｒａｎｇｅ Ｓｅｎｓｏｒ）１７と、自己位置補正部１８とを備えている。
【００２０】
走行制御部１０は、移動ロボット１の走行を制御する。具体的には、移動の開始および停止の制御、速度制御、車輪１２の方向の制御などを行う。モーター１１は、走行制御部１０の制御にしたがい車輪１２を駆動する。位置推定センサ１３は、光ファイバジャイロ１４と、エンコーダ１５とを有している。光ファイバジャイロ１４は、移動ロボット１の傾きを検出する。エンコーダ１５は、車輪１２の回転に応じたパルスを検出する。自己位置推定部１６は、光ファイバジャイロ１４およびエンコーダ１５の検出結果にしたがい移動ロボット１の自己位置（ｘ，ｙ，θ）を推定する。
【００２１】
ＬＲＳ１７は、一定の角度間隔でレーザー光を照射し、反射の位相差や時間差などに基づいて壁や障害物の位置を特定する。ＬＲＳ１７は、例えば０．５度間隔でレーザー光を照射する。ＬＲＳ１７は、移動ロボット１の前方を中心とした１８０度の間を走査し、この間で、０．５度間隔でレーザー光を照射する。また、他の例としては、走査範囲は３６０度であってもよい。
【００２２】
自己位置補正部１８は、点環境地図を有している。点環境地図とは、移動ロボット１が移動可能な領域を示した地図である。すなわち、点環境地図には、移動ロボット１が移動可能な領域における移動経路の境界線となる壁や障害物が示されている。自己位置補正部１８は、この点環境地図とＬＲＳ１７による測定結果とを比較し、両者の間の位置ずれを算出する。そして、この位置ずれの程度に応じて自己位置を補正する。具体的には、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）アルゴリズムにより点環境地図と測定結果の間のずれを算出する。ＩＣＰアルゴリズムについては後述する。
【００２３】
移動ロボット１が移動する際には、車輪１２のスリップなどにより自身が移動していると判断している距離や方向と実際の移動距離や方向との間にずれが生じ、移動し続けることによりこの誤差が蓄積されていく。自己位置補正部１８は、上記処理により適当なタイミングでこの誤差を補正する。具体的には、予め定められた時間が経過する毎に自己位置の補正を行う。また、他の例としては、自己位置補正部１８は、一定の距離を移動する毎に自己位置の補正を行ってもよい。
【００２４】
図２は、自己位置補正部１８の詳細な機能構成を示すブロック図である。自己位置補正部１８は、位置情報取得部１８０と、点環境地図ＤＢ１８１と、点環境地図読出部１８２と、不要点除去部１８３と、周囲環境情報取得部１８４と、周囲環境情報調整部１８５と、対象点特定部１８６と、誤差評価関数（Ｅ）算出部１８７と、位置補正部１８８とを有している。
【００２５】
位置情報取得部１８０は、自己位置推定部１６により推定された自己位置を示す位置情報を自己位置推定部１６から取得する。点環境地図ＤＢ１８１は、点環境地図を格納しており、この点環境地図ＤＢ１８１は、ハードディスクドライブ装置（ＨＤＤ）やメモリ等の記憶媒体に記憶されている。なお、他の例としては、点環境地図は外部から取得してもよい。図３は、点環境地図を説明するための図である。図３に示すように、点環境地図は、移動ロボットが移動可能な領域を示すデータであり、壁や障害物など上記移動可能な領域の移動通路の境界線を示すデータである。境界線は、例えば１０ｃｍ間隔など一定密度の点で示された点集合データで示されている。
【００２６】
なお、本実施の形態では、環境地図として点集合データで示される点環境地図を用いているがこれに限定されるものではない。例えば、環境地図として線データ集合で示される線環境地図を用いることもでき、この線データを点データに変換するように構成することができる。この場合には、ＬＲＳ１７のレーザビームと、線データの交点を点データとする。
【００２７】
点環境地図読出部１８２は、点環境地図ＤＢ１８１から位置情報に示される自己位置周辺の点環境地図を読み出す。図４は、点環境地図読出部１８２の処理を説明するための図である。点環境地図読出部１８２は、図４に示すようにＬＲＳ１７の位置と姿勢（ｘL，ｙL，θL）においてＬＲＳ１７により計測可能な範囲の点環境地図を読み出す。
【００２８】
図５は、移動ロボット１の位置および姿勢（ｘR，ｙR，θR）とＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）の関係を示す図である。このように、移動ロボット１の位置および姿勢（ｘR，ｙR，θR）は、移動ロボット１の中心の座標である。これに対し、ＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）は、ＬＲＳ１７の中心の座標である。そこで、まず位置情報に示される移動ロボット１の位置および姿勢（ｘR，ｙR，θR）をＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）に変換する。
【００２９】
なお、ここで、（ｘL，ｙL，θL）および（ｘR，ｙR，θR）は、いずれもグローバル座標系における座標である。ここで、グローバル座標とは、点環境地図の座標系である。すなわち、地図上の所定の位置を原点とする座標系である。
【００３０】
具体的には、式（１）により、（ｘR，ｙR，θR）を（ｘL，ｙL，θL）に変換する。ここで、ｏｆｆｓｅｔLRSは、（ｘR，ｙR，θR）と（ｘL，ｙL，θL）の間の距離である。
【数１】
【００３１】
さらに、点環境地図読出部１８２は、（式２）によりＬＲＳ１７の中心座標（ｘL，ｙL）と点環境地図の座標（ｘm，ｙm）との間の距離ｒmおよび角度θmを算出する。
【数２】
【００３２】
点環境地図読出部１８２は、こうして得られた距離ｒmおよび角度θmと予め定められた閾値とを比較する。距離ｒmおよび角度θmがそれぞれ閾値よりも小さい場合には、この点（ｘm，ｙm）を点環境地図として読み出す。距離ｒmおよび角度θmの閾値は、ＬＲＳ１７の計測可能範囲に応じて定めておく。これにより、点環境地図読出部１８２は、ＬＲＳ１７により計測可能な範囲の点環境地図のみを読み出すことができる。
【００３３】
図２の不要点除去部１８３は、点環境地図読出部１８２により読み出された点環境地図に含まれる点集合データから不要点を除去する。図６は、不要点除去部１８３の処理を説明するための図である。移動ロボット１が、図６に示す位置に存在する場合には、移動可能な領域の境界線となる壁領域２００がＬＲＳ１７により検出される。しかし、壁２００によりレーザー光が遮断されるので、ＬＲＳ１７は、その奥の壁領域２０２，２０４を検出することはできない。このため、壁領域２０２，２０４のようないわゆる陰面領域の点環境地図については、ＬＲＳ１７による測定結果との比較を行うことができない。すなわち、後のＩＣＰアルゴリズムにおいて不要な点データである。そこで、不要点除去部１８３は、陰面領域中の点データを点環境地図読出部１８２が読み出した点環境地図から削除する。
【００３４】
不要点除去部１８３は、陰面領域の点データを削除する。具体的には、以下１．から６．の処理を行う。
１．点環境地図上の一定間隔の点データを検索する。
２．図７は、グローバル座標系からｆｓ座標系に変換する処理を説明するための図である。（式３）により、点環境地図の点データの座標をグローバル座標系の座標（ｘm，ｙm）からｆｓ座標系の座標（ｘfs，ｙfs）に変換する。ここで、グローバル座標系とは、点環境地図における所定の位置を原点とする座標系である。ｆｓ座標系とは、ＬＲＳ１７の中心を原点とし、移動ロボット１の進行方向、すなわち正面をｘ軸のプラス方向とする座標系である。
【数３】
３．点環境地図の隣接する点データの直線式を導出する。隣接する点Ｐ1（ｘ1，ｙ1）と、点Ｐ2（ｘ2，ｙ2）の間の直線Ｐ1Ｐ2は（式４）のように示される。
【数４】
ここで、係数ａ1，ｂ1，ｃ1は、（式５）で示される。
【数５】
４．ｆｓ座標系におけるＬＲＳ１７の各レーザー光の照射方向を示す直線式を算出する。図８は、レーザー光の照射方向を示す直線式を算出する処理を説明するための図である。図８において、点Ｐ3は、ＬＲＳ１７の中心位置であり、ｆｓ座標系における原点である。点Ｐ4は、レーザー光の照射方向上の任意の点である。θpは、ｆｓ座標系における原点（ｘ３，ｙ３）とレーザー光の照射方向のなす角である。点Ｐ4の座標は、（式６）のように示される。
【数６】
ここで、ｒpは、点Ｐ3および点Ｐ4を結ぶ線分Ｐ3Ｐ4の長さである。
【００３５】
直線Ｐ3Ｐ4は、（式７）で表される。ここで、各係数は、（式８）の通りである。
【数７】
【数８】
５．図９に示すように、直線Ｐ1Ｐ2と直線Ｐ3Ｐ4の交点をＰ0とする。交点Ｐ0の座標（ｘ0，ｙ0）を（式９）により算出する。
【数９】
６．ＬＲＳ１７のすべての走査角度θpについて交点Ｐ0と原点Ｐ3の距離、すなわち深さ（ｒp）を（式１０）により算出する。
【数１０】
そして、各θpにおいて最小のｒpとなるような点Ｐを出力する。すなわち、各θpにおいて最小のｒpとなるような点Ｐ以外の点については、すべて陰面の点とみなし除去する。
【００３６】
すなわち、本実施の形態では、点環境地図の各点の間隔を一例として１０ｃｍとしているが、不要点を除去する場合には、上述のように点と点の間は直線（直線Ｐ1Ｐ2）で接続されていると仮定し、直線Ｐ1Ｐ2とＬＲＳセンサ１７のレーザ光（直線Ｐ3Ｐ4）の交点Ｐ0を計算し、交点Ｐ0と原点Ｐ3の距離、すなわち深さ（ｒp）によって隠面の点であるか否かを判別している。言い換えれば、不要点除去処理では、一度、１０ｃｍ間隔の点の間を直線で接続し関連づけてから不要点除去処理をしている。
【００３７】
図２の周囲環境情報取得部１８４は、ＬＲＳ１７の測定結果、すなわち周囲環境情報を取得する。周囲環境情報は、点集合データである。ＬＲＳ１７から取得した周囲環境情報は、グローバル座標系の座標データである。そこで、周囲環境情報取得部１８４は、周辺環境情報に含まれる各点データをＬＲＳ回転中心座標系からグローバル座標系に変換する。ＬＲＳ回転中心座標系とは、ＬＲＳ１７の中心を原点とし、移動ロボット１の正面をｙ軸のプラス方向とする座標系である。
【００３８】
具体的には、図１０に示すように、まず計測座標系の座標データ（ｘl，ｙl）をＬＲＳ１７の中心を原点とするｆｓ座標系の座標データ（ｘlfs，ｙlfs）に変換する。このとき、（式１１）を用いる。
【数１１】
さらに、グローバル座標系の座標データ（ｘgl，ｙgl）に変換する。このとき、（式１２）を用いる。
【数１２】
【００３９】
図１１は、周囲環境情報を説明するための図である。このように、周囲環境情報は、ＬＲＳ１７による一定角度間隔おきに走査されたレーザー光２３０による検出結果に対応した点集合データである。例えば、図１１における移動ロボット１の右側の壁に照射されたレーザー光２３０ａ〜２３０ｅにより点２４０ａ〜２４０ｄが得られる。これらの点が点集合データに含まれている。
【００４０】
周囲環境情報調整部１８５は、周囲環境情報取得部１８４が取得した周囲環境情報の点集合データのうち所定の点データを間引く。ＬＲＳ１７は、放射線状にレーザー光を照射する。このため、ＬＲＳ１７との距離が近い領域において得られた点においては、各点の間隔が狭く、遠くなるにしたがい、点の間隔が広くなっていく。そこで、点の間隔が狭い部分については、所定間隔で点の数を間引く。例えば、点環境地図が１０ｃｍ間隔の点データである場合には、周囲環境情報においても、点の間隔が１０ｃｍよりも密な領域においては、１０ｃｍに近くなるように点を間引く。逆にレーザ光の間隔が広くなる場合、例えば点の間隔が１０ｃｍ以上で粗な領域においては、ＬＲＳ１７との距離が遠くにある点と判断し、かかる領域の点を不要情報として除去する。
【００４１】
図２の誤差評価関数算出部１８７は、ＩＣＰアルゴリズムにおいて、点環境地図と周囲環境情報の対応点の検索を行う。対応点の検索においては、点環境地図中のすべての点と周囲環境情報中のすべての点を対象とするため、点環境地図中の点の個数に周囲環境情報中の点の個数を乗じた計算量が必要となり、演算負荷が大きい。この検索にかかる演算負荷を低減すべく、対象点特定部１８６は、周囲環境情報中の所定の点の対応点を検索する際に、点環境地図中のすべての点を対象とするのではなく、対象をより少ない点に限定する。移動ロボット１の移動においては、推定された自己位置と実際の自己位置が大きくずれることはないことがわかっている。そこで、対象点特定部１８６は、推定された自己位置と実際の自己位置のずれの程度に応じた範囲内の点のみを、対応点を検索する際の対象点とする。
【００４２】
図１２は、対象点特定部１８６の処理を具体的に説明するための図である。図１２において、白丸は点環境地図上の点、黒丸は周囲環境情報中の点を示している。例えば、周囲環境情報中の点３００の対応点を点環境地図上の点から検索する場合、図１２に示す点環境地図上のすべての点データを検索対象とするのではなく、例えば、点環境地図中の点データのうち、点３００を中心としたＬＲＳの照射角度±１０度の領域３１０内に含まれる点データのみを検索対象とする。
【００４３】
このように、検索対象点を制限することにより、処理の高速化を図ることができる。なお、領域のサイズは、ロボットの位置ずれの精度や、補正タイミング、路面の状態などの条件に応じて設定すればよく、実施の形態に限定されるものではない。
【００４４】
図２の誤差評価関数算出部１８７は、ＩＣＰアルゴリズムを用いて周囲環境情報と点環境地図とを比較する。このとき、対象点特定部１８６により特定された検索対象点のみを検索対象とする。図１３−１から図１３−４は、ＩＣＰアルゴリズムを説明するための図である。ＩＣＰアルゴリズムは、２つの点集合間の距離誤差を最小化するような剛体変換（回転または平行移動）パラメータＲ，ｔを求める手法である。ＩＣＰアルゴリズムにおいては、図１３−１に示す初期状態において、図１３−２に示すように第１グループ中の点と第２グループ中の点のうち距離が最小となるような対応点（ペア）を探索する。これをすべての点に対して行う。次に、各対応点における２点間の距離が小さくなるように、図１３−３に示すように剛体変換を行う。対応点の探索と剛体変換とを繰り返し、図１３−４に示すように２つの点集合が重なり合えば、ＩＣＰアルゴリズム終了とする。
【００４５】
２つの点集合が重なるということは、（式１３）に示す誤差評価関数（Ｅ）を最小化することに等しい。誤差評価関数算出部１８７は、誤差評価関数（Ｅ）を最小化するようなパラメータＲ，ｔをニュートン法などを用いて算出する。
【数１３】
【００４６】
誤差評価関数算出部１８７は、点環境地図の点集合と周囲環境情報の点集合に対してこのＩＣＰアルゴリズムを施すことにより、各対応点間の誤差を最小化するようなパラメータＲ，ｔを算出する。位置補正部１８８は、誤差評価関数算出部１８７により算出されたパラメータにより、位置情報取得部１８０が取得した自己位置を補正する。なお、実施の形態にかかる誤差評価関数算出部１８７は、位置ずれ量算出手段に相当する。
【００４７】
図１４は、自己位置補正部１８による自己位置補正処理を示すフローチャートである。まず、位置情報取得部１８０は、自己位置推定部１６から位置情報を取得する（ステップＳ１００）。次に、点環境地図読出部１８２は、点環境地図ＤＢ１８１から自己位置周辺の点環境地図を読み出す（ステップＳ１０２）。なお、このとき、ＬＲＳ１７の中心位置との距離および角度が予め定められた閾値以下の点環境地図のみを読み出す。次に、不要点除去部１８３は、点環境地図読出部１８２のうち不要点を除去する（ステップＳ１０４）。具体的には、点環境地図ＤＢ１８１読み出した点環境地図から陰面領域中の点データを除去する。
【００４８】
このように、後のＩＣＰアルゴリズムにおける点環境地図と周囲環境情報の比較において比較対象とならない点を予め除去しておくことにより、２つの点集合データにおける対応点を検索する処理の演算量を削減することができる。
【００４９】
次に、周囲環境情報取得部１８４は、ＬＲＳ１７から周囲環境情報を取得する（ステップＳ１０６）。次に、周囲環境情報調整部１８５は、周囲環境情報に含まれる点データを調整する（ステップＳ１０８）。具体的には、点環境地図の点密度に比べて点データが密な領域については点データを適宜間引く。
【００５０】
点環境地図の点密度に比べて密に点データが存在していた場合、ＩＣＰアルゴリズムにおいては、点データが多いことに起因した演算精度の向上よりも、演算量の増加の方が問題となる。そこで、ＩＣＰアルゴリズムにおける処理結果に影響の小さい点データを間引く。これにより、ＩＣＰアルゴリズムにおける２つの点集合データにおける対応点を検索する処理の演算量を削減することができる。
【００５１】
次に、対象点特定部１８６は、ＩＣＰアルゴリズムにおいて周囲環境情報中の所定の点に対応する点環境地図中の点、すなわち対応点の検索における検索対象となる点を、点環境地図中の一部の点に限定する（ステップＳ１１０）。このように、検索対象となる点を点環境地図中のすべての点ではなく、近傍の点のみに限定することにより、対応点を検索する処理の演算量を削減することができる。
【００５２】
次に、誤差評価関数算出部１８７は、対象点特定部１８６により限定された点のみを検索対象点として、対応点となる最近傍点を検索する対応点検索処理を行う（ステップＳ１１２）。次に、誤差評価関数算出部１８７は、誤差評価関数（Ｅ）がゼロに最も近付くようなパラメータＲ，ｔを算出する（ステップＳ１１４）。次に、周囲環境情報をパラメータＲ，ｔを用いたアフィン変換する（ステップＳ１１６）。さらに、対応点の検索と、剛体変換（アフィン変換）を繰り返す。
【００５３】
誤差評価関数（Ｅn）と、その直前に得られた誤差評価関数（Ｅn-1）の差分が予め定められた閾値以下である場合には、収束したと判断し（ステップＳ１１８，Ｙｅｓ）、このとき得られたパラメータＲ，ｔにより、自己位置推定部１６により推定された自己位置を補正する（ステップＳ１２０）。以上で処理を終了する。一方、差分や誤差関数Ｅの値が閾値よりも大きい場合には（ステップＳ１１８，Ｎｏ）、再びステップＳ１１２へ戻る。以上の処理により、自己位置推定部１６により推定された自己位置を実際の自己位置に推定することができる。
【００５４】
図１５は、対応点検索処理（ステップＳ１１２）における誤差評価関数算出部１８７の詳細な処理を示すフローチャートである。まず、点環境地図の点集合Ｍを読み込む（ステップＳ２００）。次に、周辺点環境地図の点集合、すなわちＬＲＳの点集合Ｄを読み込む（ステップＳ２０２）。次に、ＬＲＳの点集合の変数ｊを１に初期化する(ステップＳ２０４)。さらに、観測点ｄjと点環境地図点ｍiの間の全ての最小距離Ｍｉｎｄｉｓｔａｎｃｅ[ｊ]の初期値をＭａｘｄｉｓｔａｎｃｅとする（ステップＳ２０５）。ここで、Ｍａｘｄｉｓｔａｎｃｅは変数の最大値などである。
【００５５】
さらに、ＬＲＳの観測点ｊに対する点環境地図の点集合の検索範囲をｉminからｉmaxとし、この検索範囲を計算する（ステップＳ２０６）。次に、点環境地図の点集合の変数ｉをｉminに初期化する（ステップＳ２０８）。次に、ＬＲＳの観測点ｄjと、点環境地図点ｍiの間の距離を計算する（ステップＳ２１０）。そして、Ｄｉｓｔａｎｃｅ（ｄj，ｍi）をｄｉｓｔａｎｃｅに設定する。
【００５６】
次に、観測点ｄjと点環境地図点ｍiの間の最小距離Ｍｉｎｄｉｓｔａｎｃｅ[ｊ]と、ｄｉｓｔａｎｃｅとを比較し、最小判別を行う。ｄｉｓｔａｎｃｅがＭｉｎｄｉｓｔａｎｃｅ［ｊ］に比べて小さい場合には（ステップＳ２１２，Ｙｅｓ）、ｄｉｓｔａｎｃｅをＭｉｎｄｉｓｔａｎｃｅ［ｊ］に設定し、ｉを最小距離点Ｍｉｎｐｏｉｎｔ［ｊ］に設定する（ステップＳ２１４）。ｄｉｓｔａｎｃｅがＭｉｎｄｉｓｔａｎｃｅ［ｊ］以上である場合には（ステップＳ２１２，Ｎｏ）、ステップＳ２１６に進む。
【００５７】
次に、変数ｉに「ｉ＋１」を設定する（ステップＳ２１６）。すなわち、次の点環境地図点を指定する。以上ステップＳ２１０からステップＳ２１６の処理を点環境地図点のすべてに対して行うまで繰り返す（ステップＳ２１８）。
【００５８】
ｉminからｉmaxまですべての点環境地図点に対してステップＳ２１０からステップＳ２１６の処理を行うと（ステップＳ２１８，Ｙｅｓ）、次に、ＬＲＳの観測の変数をｊに「ｊ＋１」を設定する（ステップＳ２２０）。すなわち、次のＬＲＳの観測点を指定する。すべての観測点ｊに対して、ステップＳ２０８からステップＳ２２０の処理を行うまで繰り返し（ステップＳ２２２）、すべての観測点に対する処理が終了すると（ステップＳ２２２，Ｙｅｓ）、対応点検索処理（ステップＳ１１２）が完了する。
【００５９】
図１６は、自己位置補正部１７および自己位置補正部１８のハードウェア構成を示す図である。自己位置補正部１７および自己位置補正部１８は、ハードウェア構成として、自己位置補正部１７および自己位置補正部１８における自己位置推定処理および自己位置補正処理を実行するプログラムなどが格納されているＲＯＭ５２と、ＲＯＭ５２内のプログラムに従って自己位置補正部１７および自己位置補正部１８の各部を制御するＣＰＵ５１と、自己位置補正部１７および自己位置補正部１８の制御に必要な種々のデータを記憶するＲＡＭ５３と、ネットワークに接続して通信を行う通信Ｉ／Ｆ５７と、各部を接続するバス６２とを備えている。
【００６０】
先に述べた自己位置補正部１７および自己位置補正部１８におけるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、フロッピー（登録商標）ディスク（ＦＤ）、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録されて提供されてもよい。
【００６１】
この場合には、プログラムは、自己位置補正部１７および自己位置補正部１８において上記記録媒体から読み出して実行することにより主記憶装置上にロードされ、上記ソフトウェア構成で説明した各部が主記憶装置上に生成されるようになっている。
【００６２】
また、本実施の形態のプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成してもよい。
【００６３】
以上、本発明を実施の形態を用いて説明したが、上記実施の形態に多様な変更または改良を加えることができる。
【００６４】
図１７は、そうした変更例にかかる自己位置補正部１９の詳細な機能構成を示すブロック図である。自己位置補正部１９の線環境地図ＤＢ１９０は、移動ロボットが移動可能な領域における壁や障害物などの境界線が点集合データで示された点環境地図にかえて移動ロボットが移動可能な領域における壁や障害物などの境界線が線データで示された線環境地図を保持している。この線環境地図ＤＢ１９０は、ＨＤＤやメモリ等の記憶媒体に記憶されている。自己位置補正部１９は、さらに点環境地図生成部１９２を有している。点環境地図生成部１９２は、線環境地図ＤＢ１９０に保持されている線環境地図から予め定められた密度の点で示された点環境地図を生成する。点環境地図読出部１８２は、この点環境地図から位置情報に示される自己位置周辺の点環境地図を読み出す。
【００６５】
なお、読み出した点環境地図からの不要点除去から位置情報修正までの処理（図１４のステップＳ１０４〜Ｓ１２０）および対応点検索処理（図１５の処理）については実施の形態１と同様に行われる。
【００６６】
また、線環境地図ＤＢ１９０に保存されている線環境地図から不要点除去処理を行うように構成してもよい。点環境地図から不要点を除去する場合には、上述のように、一度１０ｃｍ間隔の点の間を直線で接続し関連づけてから不要点除去処理をしているが、線環境地図の場合も同様に不要点除去を行うことができる。
図１８は、線環境地図のを説明するための図である。図１８に示すように、線環境地図は、線の始点および終点（点の間隔は任意とする）とこれを関連づけることによって線データが構成されている。このため、不要点除去部１８３は、図８、９を用いて説明した点環境地図から不要点除去を行う手法と同様に、ＬＲＳ１７のレーザ光（直線Ｐ3Ｐ4）と、線データ（直線Ｐ1Ｐ2）の交点Ｐ0を計算し、交点Ｐ0と原点Ｐ3の距離、すなわち深さ（ｒp）によって隠面の線であるか否かを判別している。図１９は、不要点除去後の線環境地図の一例を示す説明図である。従って、線環境地図からの不要点除去も可能となる。
【図面の簡単な説明】
【００６７】
【図１】移動ロボット１の全体構成を示す図である。
【図２】自己位置補正部１８の詳細な機能構成を示すブロック図である。
【図３】点環境地図を説明するための図である。
【図４】点環境地図読出部１８２の処理を説明するための図である。
【図５】移動ロボット１の位置および姿勢（ｘR，ｙR，θR）とＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）の関係を示す図である。
【図６】不要点除去部１８３の処理を説明するための図である。
【図７】グローバル座標系からｆｓ座標系に変換する処理を説明するための図である。
【図８】レーザー光の照射方向を示す直線式を算出する処理を説明するための図である。
【図９】直線Ｐ1Ｐ2と直線Ｐ3Ｐ4の交点Ｐ0を示す図である。
【図１０】座標変換を説明するための図である。
【図１１】周囲環境情報を説明するための図である。
【図１２】対象点特定部１８６の処理を具体的に説明するための図である。
【図１３−１】ＩＣＰアルゴリズムを説明するための図である。
【図１３−２】ＩＣＰアルゴリズムを説明するための図である。
【図１３−３】ＩＣＰアルゴリズムを説明するための図である。
【図１３−４】ＩＣＰアルゴリズムを説明するための図である。
【図１４】自己位置補正部１８による自己位置補正処理を示すフローチャートである。
【図１５】対応点検索処理（ステップＳ１１２）における誤差評価関数算出部１８７の詳細な処理を示すフローチャートである。
【図１６】自己位置補正部１８のハードウェア構成を示す図である。
【図１７】自己位置補正部１９の詳細な機能構成を示すブロック図である。
【図１８】線環境地図を説明するための図である。
【図１９】不要点除去後の線環境地図の一例を示す説明図である。
【符号の説明】
【００６８】
１ 移動ロボット
１０ 走行制御部
１１ モーター
１２ 車輪
１３ 位置推定センサ
１４ 光ファイバジャイロ
１５ エンコーダ
１６ 自己位置推定部
１７ ＬＲＳ
１８ 自己位置補正部
５１ ＣＰＵ
５２ ＲＯＭ
５３ ＲＡＭ
５７ 通信I／Ｆ
６２ バス
１８０ 位置情報取得部
１８１ 点環境地図ＤＢ
１８２ 点環境地図読出部
１８３ 不要点除去部
１８４ 周囲環境情報取得部
１８５ 周囲環境情報調整部
１８６ 対象点特定部
１８７ 誤差評価関数算出部
１８８ 位置補正部
１９０ 線環境地図ＤＢ
１９２ 点環境地図生成部
【技術分野】
【０００１】
本発明は、自律移動可能な移動ロボット、自己位置補正方法および自己位置補正プログラムに関するものである。
【背景技術】
【０００２】
従来、ロボットが自律走行を行うためには、自己位置の認識が必要である。車輪型ロボットの場合は、車輪の回転数を計測することによる自己位置推定が一般的に行われている。また、車輪の回転数の計測だけでは走行距離に応じて誤差が蓄積していくため、走行経路上にロボットが自己位置を確認するためのロボット専用ランドマークを設置するなどして、このような蓄積誤差を解消する手法が多く提案されていた。
【０００３】
例えば、車輪の回転数と光ファイバジャイロを用いてロボットの自己位置（ｘ、ｙ、θ）を推定する方法が知られている。タイヤスリップなどの蓄積誤差を減少させるため、ロボット専用ランドマークをロボット走行経路上に一定間隔で設置し、ロボットがこれを読み取ることでロボット自己位置推定の精度を保つものである。
【０００４】
しかしながら、上記方法においては、ロボット専用ランドマークの設置や調整に労力がかかる、美観や建物構造の制約により、ロボット専用ランドマークが設置できないなどの問題があった。
【０００５】
そのため、近年、走行経路の周囲の壁などを環境地図として事前に記憶しておき、ロボットが走行中にレーザレンジセンサで計測した周囲の距離情報（壁形状）とのマッチング処理を行うことで、ロボットの自己位置を推定する手法が登場した。このマッチング処理は、ＩＣＰアルゴリズムを適用することによって、ロボット自律走行への適用が可能となる（例えば、「非特許文献１」参照）。
【０００６】
また、特許文献１には、ロボット周囲の距離情報を計測し、予め記憶していた環境地図との比較を行うことで自己位置同定を行う方法が開示されている。環境地図はバイナリトリー形式で構成され、計測距離情報から線分情報を抽出し、最尤位置候補を選定する。
【０００７】
【特許文献１】特開平４−２１６４８７号公報
【非特許文献１】"レーザレンジファインダ搭載移動ロボットによる動的環境の３次元地図生成", 映像情報メディア学会技術報告, Vol.30, No.36, pp.25-30, 浜松, July 2006（中本 琢実, 山下 淳, 金子 透）http://robotics.caltech.edu/~atsushi/paper/E/E078Final.pdf
【発明の開示】
【発明が解決しようとする課題】
【０００８】
ロボットが記憶している環境地図は、グローバル座標と呼ばれる二次元の絶対座標上に表され、ロボットが走行する周囲の壁や物体の外形を表す点集合である。ＩＣＰアルゴリズムなどでマッチング処理を行う際、同一の特徴を持った点集合同士であれば対応点が一致することによりマッチング精度が向上する。
【０００９】
また、ＩＣＰアルゴリズムの中で行っている対応点検索処理は、点集合Ａ，Ｂにおいてユークリッド距離が最小となる組合せを求めるものである。全検索を行った場合、演算量が(点集合Ａの要素数)×(点集合Ｂの要素数)となり、点集合が大きくなるにつれて計算時間が増大する。ＩＣＰアルゴリズムの計算時間が増大すると、ロボットの走行スピードや加減速度が大きくなとともに自己位置推定誤差が増大するという問題があった。
【００１０】
本発明は、上記に鑑みてなされたものであって、演算量の増加を抑えつつ、正確に自己位置を推定することのできる移動ロボット、自己位置補正方法および自己位置補正プログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上述した課題を解決し、目的を達成するために、請求項１にかかる発明は、自律移動可能な移動ロボットであって、当該移動ロボットの移動量および移動方向を検知する移動センサと、前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定手段と、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段と、当該移動ロボットの周囲の移動可能な領域を検知する経路センサと、前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成手段と、前記周囲環境情報中の所定の点データに対応する点データであって、前記点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定手段と、前記検索対象点限定手段により限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正する位置補正手段とを備えたことを特徴とする。
【００１２】
また、請求項２にかかる発明は、請求項１に記載の移動ロボットであって、前記移動ロボットが移動可能な領域を線データで示す線環境地図を保持する線環境地図保持手段と、前記線環境地図保持手段が保持する前記線環境地図から所定の密度の点で示された前記点環境地図を生成する点環境地図生成手段とをさらに備え、前記点環境地図保持手段は、前記点環境地図生成手段により生成された前記点データを保持することを特徴とする。
【００１３】
また、請求項３にかかる発明は、請求項１または２に記載の移動ロボットであって、前記周囲環境情報生成手段により生成された前記点集合データの点データの間隔が前記点環境地図保持手段が保持する前記点環境地図中の点集合データの点データの間隔に比べて狭い場合に、前記周囲環境情報中の点データの間隔が前記点環境地図中の点データの間隔になるよう前記点データを間引く周囲環境情報調整手段をさらに備えたことを特徴とする。
【００１４】
また、請求項４にかかる発明は、請求項１から３のいずれか一項に記載の移動ロボットであって、ＩＣＰアルゴリズムを用いて前記検索対象点限定手段により限定された後の前記点環境地図中の各点データと、前記対応点データとの間の位置ずれ量を算出する位置ずれ量算出手段をさらに備え、前記自己位置補正手段は、前記位置ずれ量算出手段により算出された前記位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正することを特徴とする。
【００１５】
また、請求項５にかかる発明は、自律移動可能な移動ロボットの自己位置補正方法であって、移動センサが当該移動ロボットの移動量および移動方向を検知する第１検知ステップと、前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、経路センサが当該移動ロボットの周囲の移動可能な領域を検知する第２検知ステップと、前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成ステップと、前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップとを有することを特徴とする。
【００１６】
また、請求項６にかかる発明は、自律移動可能な移動ロボットの自己位置補正処理をコンピュータに実行させる自己位置補正プログラムであって、当該移動ロボットの移動量および移動方向を検知する移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、当該移動ロボットの周囲の移動可能な領域を検知する経路センサによる検出結果を点集合データで示す周囲環境情報を取得する周囲環境情報取得ステップと、前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップとを有することを特徴とする。
【発明の効果】
【００１７】
本発明によれば、移動センサが、当該移動ロボットの移動量および移動方向を検知し、自己位置推定手段が、移動センサによる検出結果に基づいて、自己位置を推定し、点環境地図保持手段が、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持し、経路センサが、当該移動ロボットの周囲の移動可能な領域を検知し、周囲環境情報生成手段が、経路センサによる検出結果を点集合データで示す周囲環境情報を生成し、検索対象点限定手段が、周囲環境情報中の所定の点データに対応する点データであって、点環境地図中の点データである対応点データの候補を、点環境地図中の点データのうち所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定し、位置補正手段が、検索対象点限定手段により限定された点を所定の点データに対する対応点データとして、点環境地図中の各点データと、周囲環境情報中の対応点データとの間の位置ずれ量分自己位置推定手段により推定された自己位置を補正するので、演算量の増加を抑えつつ、正確に自己位置を推定することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１８】
以下に添付図面を参照して、この発明にかかる移動ロボット、自己位置補正方法および自己位置補正プログラムの最良な実施の形態を詳細に説明する。
【００１９】
図１は、実施の形態にかかる移動ロボット１の全体構成を示す図である。移動ロボット１は、走行制御部１０と、モーター１１と、車輪１２と、位置推定センサ１３と、自己位置推定部１６と、ＬＲＳ（Ｌａｓｅｒ Ｒａｎｇｅ Ｓｅｎｓｏｒ）１７と、自己位置補正部１８とを備えている。
【００２０】
走行制御部１０は、移動ロボット１の走行を制御する。具体的には、移動の開始および停止の制御、速度制御、車輪１２の方向の制御などを行う。モーター１１は、走行制御部１０の制御にしたがい車輪１２を駆動する。位置推定センサ１３は、光ファイバジャイロ１４と、エンコーダ１５とを有している。光ファイバジャイロ１４は、移動ロボット１の傾きを検出する。エンコーダ１５は、車輪１２の回転に応じたパルスを検出する。自己位置推定部１６は、光ファイバジャイロ１４およびエンコーダ１５の検出結果にしたがい移動ロボット１の自己位置（ｘ，ｙ，θ）を推定する。
【００２１】
ＬＲＳ１７は、一定の角度間隔でレーザー光を照射し、反射の位相差や時間差などに基づいて壁や障害物の位置を特定する。ＬＲＳ１７は、例えば０．５度間隔でレーザー光を照射する。ＬＲＳ１７は、移動ロボット１の前方を中心とした１８０度の間を走査し、この間で、０．５度間隔でレーザー光を照射する。また、他の例としては、走査範囲は３６０度であってもよい。
【００２２】
自己位置補正部１８は、点環境地図を有している。点環境地図とは、移動ロボット１が移動可能な領域を示した地図である。すなわち、点環境地図には、移動ロボット１が移動可能な領域における移動経路の境界線となる壁や障害物が示されている。自己位置補正部１８は、この点環境地図とＬＲＳ１７による測定結果とを比較し、両者の間の位置ずれを算出する。そして、この位置ずれの程度に応じて自己位置を補正する。具体的には、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）アルゴリズムにより点環境地図と測定結果の間のずれを算出する。ＩＣＰアルゴリズムについては後述する。
【００２３】
移動ロボット１が移動する際には、車輪１２のスリップなどにより自身が移動していると判断している距離や方向と実際の移動距離や方向との間にずれが生じ、移動し続けることによりこの誤差が蓄積されていく。自己位置補正部１８は、上記処理により適当なタイミングでこの誤差を補正する。具体的には、予め定められた時間が経過する毎に自己位置の補正を行う。また、他の例としては、自己位置補正部１８は、一定の距離を移動する毎に自己位置の補正を行ってもよい。
【００２４】
図２は、自己位置補正部１８の詳細な機能構成を示すブロック図である。自己位置補正部１８は、位置情報取得部１８０と、点環境地図ＤＢ１８１と、点環境地図読出部１８２と、不要点除去部１８３と、周囲環境情報取得部１８４と、周囲環境情報調整部１８５と、対象点特定部１８６と、誤差評価関数（Ｅ）算出部１８７と、位置補正部１８８とを有している。
【００２５】
位置情報取得部１８０は、自己位置推定部１６により推定された自己位置を示す位置情報を自己位置推定部１６から取得する。点環境地図ＤＢ１８１は、点環境地図を格納しており、この点環境地図ＤＢ１８１は、ハードディスクドライブ装置（ＨＤＤ）やメモリ等の記憶媒体に記憶されている。なお、他の例としては、点環境地図は外部から取得してもよい。図３は、点環境地図を説明するための図である。図３に示すように、点環境地図は、移動ロボットが移動可能な領域を示すデータであり、壁や障害物など上記移動可能な領域の移動通路の境界線を示すデータである。境界線は、例えば１０ｃｍ間隔など一定密度の点で示された点集合データで示されている。
【００２６】
なお、本実施の形態では、環境地図として点集合データで示される点環境地図を用いているがこれに限定されるものではない。例えば、環境地図として線データ集合で示される線環境地図を用いることもでき、この線データを点データに変換するように構成することができる。この場合には、ＬＲＳ１７のレーザビームと、線データの交点を点データとする。
【００２７】
点環境地図読出部１８２は、点環境地図ＤＢ１８１から位置情報に示される自己位置周辺の点環境地図を読み出す。図４は、点環境地図読出部１８２の処理を説明するための図である。点環境地図読出部１８２は、図４に示すようにＬＲＳ１７の位置と姿勢（ｘL，ｙL，θL）においてＬＲＳ１７により計測可能な範囲の点環境地図を読み出す。
【００２８】
図５は、移動ロボット１の位置および姿勢（ｘR，ｙR，θR）とＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）の関係を示す図である。このように、移動ロボット１の位置および姿勢（ｘR，ｙR，θR）は、移動ロボット１の中心の座標である。これに対し、ＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）は、ＬＲＳ１７の中心の座標である。そこで、まず位置情報に示される移動ロボット１の位置および姿勢（ｘR，ｙR，θR）をＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）に変換する。
【００２９】
なお、ここで、（ｘL，ｙL，θL）および（ｘR，ｙR，θR）は、いずれもグローバル座標系における座標である。ここで、グローバル座標とは、点環境地図の座標系である。すなわち、地図上の所定の位置を原点とする座標系である。
【００３０】
具体的には、式（１）により、（ｘR，ｙR，θR）を（ｘL，ｙL，θL）に変換する。ここで、ｏｆｆｓｅｔLRSは、（ｘR，ｙR，θR）と（ｘL，ｙL，θL）の間の距離である。
【数１】
【００３１】
さらに、点環境地図読出部１８２は、（式２）によりＬＲＳ１７の中心座標（ｘL，ｙL）と点環境地図の座標（ｘm，ｙm）との間の距離ｒmおよび角度θmを算出する。
【数２】
【００３２】
点環境地図読出部１８２は、こうして得られた距離ｒmおよび角度θmと予め定められた閾値とを比較する。距離ｒmおよび角度θmがそれぞれ閾値よりも小さい場合には、この点（ｘm，ｙm）を点環境地図として読み出す。距離ｒmおよび角度θmの閾値は、ＬＲＳ１７の計測可能範囲に応じて定めておく。これにより、点環境地図読出部１８２は、ＬＲＳ１７により計測可能な範囲の点環境地図のみを読み出すことができる。
【００３３】
図２の不要点除去部１８３は、点環境地図読出部１８２により読み出された点環境地図に含まれる点集合データから不要点を除去する。図６は、不要点除去部１８３の処理を説明するための図である。移動ロボット１が、図６に示す位置に存在する場合には、移動可能な領域の境界線となる壁領域２００がＬＲＳ１７により検出される。しかし、壁２００によりレーザー光が遮断されるので、ＬＲＳ１７は、その奥の壁領域２０２，２０４を検出することはできない。このため、壁領域２０２，２０４のようないわゆる陰面領域の点環境地図については、ＬＲＳ１７による測定結果との比較を行うことができない。すなわち、後のＩＣＰアルゴリズムにおいて不要な点データである。そこで、不要点除去部１８３は、陰面領域中の点データを点環境地図読出部１８２が読み出した点環境地図から削除する。
【００３４】
不要点除去部１８３は、陰面領域の点データを削除する。具体的には、以下１．から６．の処理を行う。
１．点環境地図上の一定間隔の点データを検索する。
２．図７は、グローバル座標系からｆｓ座標系に変換する処理を説明するための図である。（式３）により、点環境地図の点データの座標をグローバル座標系の座標（ｘm，ｙm）からｆｓ座標系の座標（ｘfs，ｙfs）に変換する。ここで、グローバル座標系とは、点環境地図における所定の位置を原点とする座標系である。ｆｓ座標系とは、ＬＲＳ１７の中心を原点とし、移動ロボット１の進行方向、すなわち正面をｘ軸のプラス方向とする座標系である。
【数３】
３．点環境地図の隣接する点データの直線式を導出する。隣接する点Ｐ1（ｘ1，ｙ1）と、点Ｐ2（ｘ2，ｙ2）の間の直線Ｐ1Ｐ2は（式４）のように示される。
【数４】
ここで、係数ａ1，ｂ1，ｃ1は、（式５）で示される。
【数５】
４．ｆｓ座標系におけるＬＲＳ１７の各レーザー光の照射方向を示す直線式を算出する。図８は、レーザー光の照射方向を示す直線式を算出する処理を説明するための図である。図８において、点Ｐ3は、ＬＲＳ１７の中心位置であり、ｆｓ座標系における原点である。点Ｐ4は、レーザー光の照射方向上の任意の点である。θpは、ｆｓ座標系における原点（ｘ３，ｙ３）とレーザー光の照射方向のなす角である。点Ｐ4の座標は、（式６）のように示される。
【数６】
ここで、ｒpは、点Ｐ3および点Ｐ4を結ぶ線分Ｐ3Ｐ4の長さである。
【００３５】
直線Ｐ3Ｐ4は、（式７）で表される。ここで、各係数は、（式８）の通りである。
【数７】
【数８】
５．図９に示すように、直線Ｐ1Ｐ2と直線Ｐ3Ｐ4の交点をＰ0とする。交点Ｐ0の座標（ｘ0，ｙ0）を（式９）により算出する。
【数９】
６．ＬＲＳ１７のすべての走査角度θpについて交点Ｐ0と原点Ｐ3の距離、すなわち深さ（ｒp）を（式１０）により算出する。
【数１０】
そして、各θpにおいて最小のｒpとなるような点Ｐを出力する。すなわち、各θpにおいて最小のｒpとなるような点Ｐ以外の点については、すべて陰面の点とみなし除去する。
【００３６】
すなわち、本実施の形態では、点環境地図の各点の間隔を一例として１０ｃｍとしているが、不要点を除去する場合には、上述のように点と点の間は直線（直線Ｐ1Ｐ2）で接続されていると仮定し、直線Ｐ1Ｐ2とＬＲＳセンサ１７のレーザ光（直線Ｐ3Ｐ4）の交点Ｐ0を計算し、交点Ｐ0と原点Ｐ3の距離、すなわち深さ（ｒp）によって隠面の点であるか否かを判別している。言い換えれば、不要点除去処理では、一度、１０ｃｍ間隔の点の間を直線で接続し関連づけてから不要点除去処理をしている。
【００３７】
図２の周囲環境情報取得部１８４は、ＬＲＳ１７の測定結果、すなわち周囲環境情報を取得する。周囲環境情報は、点集合データである。ＬＲＳ１７から取得した周囲環境情報は、グローバル座標系の座標データである。そこで、周囲環境情報取得部１８４は、周辺環境情報に含まれる各点データをＬＲＳ回転中心座標系からグローバル座標系に変換する。ＬＲＳ回転中心座標系とは、ＬＲＳ１７の中心を原点とし、移動ロボット１の正面をｙ軸のプラス方向とする座標系である。
【００３８】
具体的には、図１０に示すように、まず計測座標系の座標データ（ｘl，ｙl）をＬＲＳ１７の中心を原点とするｆｓ座標系の座標データ（ｘlfs，ｙlfs）に変換する。このとき、（式１１）を用いる。
【数１１】
さらに、グローバル座標系の座標データ（ｘgl，ｙgl）に変換する。このとき、（式１２）を用いる。
【数１２】
【００３９】
図１１は、周囲環境情報を説明するための図である。このように、周囲環境情報は、ＬＲＳ１７による一定角度間隔おきに走査されたレーザー光２３０による検出結果に対応した点集合データである。例えば、図１１における移動ロボット１の右側の壁に照射されたレーザー光２３０ａ〜２３０ｅにより点２４０ａ〜２４０ｄが得られる。これらの点が点集合データに含まれている。
【００４０】
周囲環境情報調整部１８５は、周囲環境情報取得部１８４が取得した周囲環境情報の点集合データのうち所定の点データを間引く。ＬＲＳ１７は、放射線状にレーザー光を照射する。このため、ＬＲＳ１７との距離が近い領域において得られた点においては、各点の間隔が狭く、遠くなるにしたがい、点の間隔が広くなっていく。そこで、点の間隔が狭い部分については、所定間隔で点の数を間引く。例えば、点環境地図が１０ｃｍ間隔の点データである場合には、周囲環境情報においても、点の間隔が１０ｃｍよりも密な領域においては、１０ｃｍに近くなるように点を間引く。逆にレーザ光の間隔が広くなる場合、例えば点の間隔が１０ｃｍ以上で粗な領域においては、ＬＲＳ１７との距離が遠くにある点と判断し、かかる領域の点を不要情報として除去する。
【００４１】
図２の誤差評価関数算出部１８７は、ＩＣＰアルゴリズムにおいて、点環境地図と周囲環境情報の対応点の検索を行う。対応点の検索においては、点環境地図中のすべての点と周囲環境情報中のすべての点を対象とするため、点環境地図中の点の個数に周囲環境情報中の点の個数を乗じた計算量が必要となり、演算負荷が大きい。この検索にかかる演算負荷を低減すべく、対象点特定部１８６は、周囲環境情報中の所定の点の対応点を検索する際に、点環境地図中のすべての点を対象とするのではなく、対象をより少ない点に限定する。移動ロボット１の移動においては、推定された自己位置と実際の自己位置が大きくずれることはないことがわかっている。そこで、対象点特定部１８６は、推定された自己位置と実際の自己位置のずれの程度に応じた範囲内の点のみを、対応点を検索する際の対象点とする。
【００４２】
図１２は、対象点特定部１８６の処理を具体的に説明するための図である。図１２において、白丸は点環境地図上の点、黒丸は周囲環境情報中の点を示している。例えば、周囲環境情報中の点３００の対応点を点環境地図上の点から検索する場合、図１２に示す点環境地図上のすべての点データを検索対象とするのではなく、例えば、点環境地図中の点データのうち、点３００を中心としたＬＲＳの照射角度±１０度の領域３１０内に含まれる点データのみを検索対象とする。
【００４３】
このように、検索対象点を制限することにより、処理の高速化を図ることができる。なお、領域のサイズは、ロボットの位置ずれの精度や、補正タイミング、路面の状態などの条件に応じて設定すればよく、実施の形態に限定されるものではない。
【００４４】
図２の誤差評価関数算出部１８７は、ＩＣＰアルゴリズムを用いて周囲環境情報と点環境地図とを比較する。このとき、対象点特定部１８６により特定された検索対象点のみを検索対象とする。図１３−１から図１３−４は、ＩＣＰアルゴリズムを説明するための図である。ＩＣＰアルゴリズムは、２つの点集合間の距離誤差を最小化するような剛体変換（回転または平行移動）パラメータＲ，ｔを求める手法である。ＩＣＰアルゴリズムにおいては、図１３−１に示す初期状態において、図１３−２に示すように第１グループ中の点と第２グループ中の点のうち距離が最小となるような対応点（ペア）を探索する。これをすべての点に対して行う。次に、各対応点における２点間の距離が小さくなるように、図１３−３に示すように剛体変換を行う。対応点の探索と剛体変換とを繰り返し、図１３−４に示すように２つの点集合が重なり合えば、ＩＣＰアルゴリズム終了とする。
【００４５】
２つの点集合が重なるということは、（式１３）に示す誤差評価関数（Ｅ）を最小化することに等しい。誤差評価関数算出部１８７は、誤差評価関数（Ｅ）を最小化するようなパラメータＲ，ｔをニュートン法などを用いて算出する。
【数１３】
【００４６】
誤差評価関数算出部１８７は、点環境地図の点集合と周囲環境情報の点集合に対してこのＩＣＰアルゴリズムを施すことにより、各対応点間の誤差を最小化するようなパラメータＲ，ｔを算出する。位置補正部１８８は、誤差評価関数算出部１８７により算出されたパラメータにより、位置情報取得部１８０が取得した自己位置を補正する。なお、実施の形態にかかる誤差評価関数算出部１８７は、位置ずれ量算出手段に相当する。
【００４７】
図１４は、自己位置補正部１８による自己位置補正処理を示すフローチャートである。まず、位置情報取得部１８０は、自己位置推定部１６から位置情報を取得する（ステップＳ１００）。次に、点環境地図読出部１８２は、点環境地図ＤＢ１８１から自己位置周辺の点環境地図を読み出す（ステップＳ１０２）。なお、このとき、ＬＲＳ１７の中心位置との距離および角度が予め定められた閾値以下の点環境地図のみを読み出す。次に、不要点除去部１８３は、点環境地図読出部１８２のうち不要点を除去する（ステップＳ１０４）。具体的には、点環境地図ＤＢ１８１読み出した点環境地図から陰面領域中の点データを除去する。
【００４８】
このように、後のＩＣＰアルゴリズムにおける点環境地図と周囲環境情報の比較において比較対象とならない点を予め除去しておくことにより、２つの点集合データにおける対応点を検索する処理の演算量を削減することができる。
【００４９】
次に、周囲環境情報取得部１８４は、ＬＲＳ１７から周囲環境情報を取得する（ステップＳ１０６）。次に、周囲環境情報調整部１８５は、周囲環境情報に含まれる点データを調整する（ステップＳ１０８）。具体的には、点環境地図の点密度に比べて点データが密な領域については点データを適宜間引く。
【００５０】
点環境地図の点密度に比べて密に点データが存在していた場合、ＩＣＰアルゴリズムにおいては、点データが多いことに起因した演算精度の向上よりも、演算量の増加の方が問題となる。そこで、ＩＣＰアルゴリズムにおける処理結果に影響の小さい点データを間引く。これにより、ＩＣＰアルゴリズムにおける２つの点集合データにおける対応点を検索する処理の演算量を削減することができる。
【００５１】
次に、対象点特定部１８６は、ＩＣＰアルゴリズムにおいて周囲環境情報中の所定の点に対応する点環境地図中の点、すなわち対応点の検索における検索対象となる点を、点環境地図中の一部の点に限定する（ステップＳ１１０）。このように、検索対象となる点を点環境地図中のすべての点ではなく、近傍の点のみに限定することにより、対応点を検索する処理の演算量を削減することができる。
【００５２】
次に、誤差評価関数算出部１８７は、対象点特定部１８６により限定された点のみを検索対象点として、対応点となる最近傍点を検索する対応点検索処理を行う（ステップＳ１１２）。次に、誤差評価関数算出部１８７は、誤差評価関数（Ｅ）がゼロに最も近付くようなパラメータＲ，ｔを算出する（ステップＳ１１４）。次に、周囲環境情報をパラメータＲ，ｔを用いたアフィン変換する（ステップＳ１１６）。さらに、対応点の検索と、剛体変換（アフィン変換）を繰り返す。
【００５３】
誤差評価関数（Ｅn）と、その直前に得られた誤差評価関数（Ｅn-1）の差分が予め定められた閾値以下である場合には、収束したと判断し（ステップＳ１１８，Ｙｅｓ）、このとき得られたパラメータＲ，ｔにより、自己位置推定部１６により推定された自己位置を補正する（ステップＳ１２０）。以上で処理を終了する。一方、差分や誤差関数Ｅの値が閾値よりも大きい場合には（ステップＳ１１８，Ｎｏ）、再びステップＳ１１２へ戻る。以上の処理により、自己位置推定部１６により推定された自己位置を実際の自己位置に推定することができる。
【００５４】
図１５は、対応点検索処理（ステップＳ１１２）における誤差評価関数算出部１８７の詳細な処理を示すフローチャートである。まず、点環境地図の点集合Ｍを読み込む（ステップＳ２００）。次に、周辺点環境地図の点集合、すなわちＬＲＳの点集合Ｄを読み込む（ステップＳ２０２）。次に、ＬＲＳの点集合の変数ｊを１に初期化する(ステップＳ２０４)。さらに、観測点ｄjと点環境地図点ｍiの間の全ての最小距離Ｍｉｎｄｉｓｔａｎｃｅ[ｊ]の初期値をＭａｘｄｉｓｔａｎｃｅとする（ステップＳ２０５）。ここで、Ｍａｘｄｉｓｔａｎｃｅは変数の最大値などである。
【００５５】
さらに、ＬＲＳの観測点ｊに対する点環境地図の点集合の検索範囲をｉminからｉmaxとし、この検索範囲を計算する（ステップＳ２０６）。次に、点環境地図の点集合の変数ｉをｉminに初期化する（ステップＳ２０８）。次に、ＬＲＳの観測点ｄjと、点環境地図点ｍiの間の距離を計算する（ステップＳ２１０）。そして、Ｄｉｓｔａｎｃｅ（ｄj，ｍi）をｄｉｓｔａｎｃｅに設定する。
【００５６】
次に、観測点ｄjと点環境地図点ｍiの間の最小距離Ｍｉｎｄｉｓｔａｎｃｅ[ｊ]と、ｄｉｓｔａｎｃｅとを比較し、最小判別を行う。ｄｉｓｔａｎｃｅがＭｉｎｄｉｓｔａｎｃｅ［ｊ］に比べて小さい場合には（ステップＳ２１２，Ｙｅｓ）、ｄｉｓｔａｎｃｅをＭｉｎｄｉｓｔａｎｃｅ［ｊ］に設定し、ｉを最小距離点Ｍｉｎｐｏｉｎｔ［ｊ］に設定する（ステップＳ２１４）。ｄｉｓｔａｎｃｅがＭｉｎｄｉｓｔａｎｃｅ［ｊ］以上である場合には（ステップＳ２１２，Ｎｏ）、ステップＳ２１６に進む。
【００５７】
次に、変数ｉに「ｉ＋１」を設定する（ステップＳ２１６）。すなわち、次の点環境地図点を指定する。以上ステップＳ２１０からステップＳ２１６の処理を点環境地図点のすべてに対して行うまで繰り返す（ステップＳ２１８）。
【００５８】
ｉminからｉmaxまですべての点環境地図点に対してステップＳ２１０からステップＳ２１６の処理を行うと（ステップＳ２１８，Ｙｅｓ）、次に、ＬＲＳの観測の変数をｊに「ｊ＋１」を設定する（ステップＳ２２０）。すなわち、次のＬＲＳの観測点を指定する。すべての観測点ｊに対して、ステップＳ２０８からステップＳ２２０の処理を行うまで繰り返し（ステップＳ２２２）、すべての観測点に対する処理が終了すると（ステップＳ２２２，Ｙｅｓ）、対応点検索処理（ステップＳ１１２）が完了する。
【００５９】
図１６は、自己位置補正部１７および自己位置補正部１８のハードウェア構成を示す図である。自己位置補正部１７および自己位置補正部１８は、ハードウェア構成として、自己位置補正部１７および自己位置補正部１８における自己位置推定処理および自己位置補正処理を実行するプログラムなどが格納されているＲＯＭ５２と、ＲＯＭ５２内のプログラムに従って自己位置補正部１７および自己位置補正部１８の各部を制御するＣＰＵ５１と、自己位置補正部１７および自己位置補正部１８の制御に必要な種々のデータを記憶するＲＡＭ５３と、ネットワークに接続して通信を行う通信Ｉ／Ｆ５７と、各部を接続するバス６２とを備えている。
【００６０】
先に述べた自己位置補正部１７および自己位置補正部１８におけるプログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、フロッピー（登録商標）ディスク（ＦＤ）、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録されて提供されてもよい。
【００６１】
この場合には、プログラムは、自己位置補正部１７および自己位置補正部１８において上記記録媒体から読み出して実行することにより主記憶装置上にロードされ、上記ソフトウェア構成で説明した各部が主記憶装置上に生成されるようになっている。
【００６２】
また、本実施の形態のプログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成してもよい。
【００６３】
以上、本発明を実施の形態を用いて説明したが、上記実施の形態に多様な変更または改良を加えることができる。
【００６４】
図１７は、そうした変更例にかかる自己位置補正部１９の詳細な機能構成を示すブロック図である。自己位置補正部１９の線環境地図ＤＢ１９０は、移動ロボットが移動可能な領域における壁や障害物などの境界線が点集合データで示された点環境地図にかえて移動ロボットが移動可能な領域における壁や障害物などの境界線が線データで示された線環境地図を保持している。この線環境地図ＤＢ１９０は、ＨＤＤやメモリ等の記憶媒体に記憶されている。自己位置補正部１９は、さらに点環境地図生成部１９２を有している。点環境地図生成部１９２は、線環境地図ＤＢ１９０に保持されている線環境地図から予め定められた密度の点で示された点環境地図を生成する。点環境地図読出部１８２は、この点環境地図から位置情報に示される自己位置周辺の点環境地図を読み出す。
【００６５】
なお、読み出した点環境地図からの不要点除去から位置情報修正までの処理（図１４のステップＳ１０４〜Ｓ１２０）および対応点検索処理（図１５の処理）については実施の形態１と同様に行われる。
【００６６】
また、線環境地図ＤＢ１９０に保存されている線環境地図から不要点除去処理を行うように構成してもよい。点環境地図から不要点を除去する場合には、上述のように、一度１０ｃｍ間隔の点の間を直線で接続し関連づけてから不要点除去処理をしているが、線環境地図の場合も同様に不要点除去を行うことができる。
図１８は、線環境地図のを説明するための図である。図１８に示すように、線環境地図は、線の始点および終点（点の間隔は任意とする）とこれを関連づけることによって線データが構成されている。このため、不要点除去部１８３は、図８、９を用いて説明した点環境地図から不要点除去を行う手法と同様に、ＬＲＳ１７のレーザ光（直線Ｐ3Ｐ4）と、線データ（直線Ｐ1Ｐ2）の交点Ｐ0を計算し、交点Ｐ0と原点Ｐ3の距離、すなわち深さ（ｒp）によって隠面の線であるか否かを判別している。図１９は、不要点除去後の線環境地図の一例を示す説明図である。従って、線環境地図からの不要点除去も可能となる。
【図面の簡単な説明】
【００６７】
【図１】移動ロボット１の全体構成を示す図である。
【図２】自己位置補正部１８の詳細な機能構成を示すブロック図である。
【図３】点環境地図を説明するための図である。
【図４】点環境地図読出部１８２の処理を説明するための図である。
【図５】移動ロボット１の位置および姿勢（ｘR，ｙR，θR）とＬＲＳ１７の位置および姿勢（ｘL，ｙL，θL）の関係を示す図である。
【図６】不要点除去部１８３の処理を説明するための図である。
【図７】グローバル座標系からｆｓ座標系に変換する処理を説明するための図である。
【図８】レーザー光の照射方向を示す直線式を算出する処理を説明するための図である。
【図９】直線Ｐ1Ｐ2と直線Ｐ3Ｐ4の交点Ｐ0を示す図である。
【図１０】座標変換を説明するための図である。
【図１１】周囲環境情報を説明するための図である。
【図１２】対象点特定部１８６の処理を具体的に説明するための図である。
【図１３−１】ＩＣＰアルゴリズムを説明するための図である。
【図１３−２】ＩＣＰアルゴリズムを説明するための図である。
【図１３−３】ＩＣＰアルゴリズムを説明するための図である。
【図１３−４】ＩＣＰアルゴリズムを説明するための図である。
【図１４】自己位置補正部１８による自己位置補正処理を示すフローチャートである。
【図１５】対応点検索処理（ステップＳ１１２）における誤差評価関数算出部１８７の詳細な処理を示すフローチャートである。
【図１６】自己位置補正部１８のハードウェア構成を示す図である。
【図１７】自己位置補正部１９の詳細な機能構成を示すブロック図である。
【図１８】線環境地図を説明するための図である。
【図１９】不要点除去後の線環境地図の一例を示す説明図である。
【符号の説明】
【００６８】
１ 移動ロボット
１０ 走行制御部
１１ モーター
１２ 車輪
１３ 位置推定センサ
１４ 光ファイバジャイロ
１５ エンコーダ
１６ 自己位置推定部
１７ ＬＲＳ
１８ 自己位置補正部
５１ ＣＰＵ
５２ ＲＯＭ
５３ ＲＡＭ
５７ 通信I／Ｆ
６２ バス
１８０ 位置情報取得部
１８１ 点環境地図ＤＢ
１８２ 点環境地図読出部
１８３ 不要点除去部
１８４ 周囲環境情報取得部
１８５ 周囲環境情報調整部
１８６ 対象点特定部
１８７ 誤差評価関数算出部
１８８ 位置補正部
１９０ 線環境地図ＤＢ
１９２ 点環境地図生成部
【特許請求の範囲】
【請求項１】
自律移動可能な移動ロボットであって、
当該移動ロボットの移動量および移動方向を検知する移動センサと、
前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定手段と、
当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段と、
当該移動ロボットの周囲の移動可能な領域を検知する経路センサと、
前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成手段と、
前記周囲環境情報中の所定の点データに対応する点データであって、前記点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定手段と、
前記検索対象点限定手段により限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正する位置補正手段と、
を備えたことを特徴とする移動ロボット。
【請求項２】
前記移動ロボットが移動可能な領域を線データで示す線環境地図を保持する線環境地図保持手段と、
前記線環境地図保持手段が保持する前記線環境地図から所定の密度の点で示された前記点環境地図を生成する点環境地図生成手段と
をさらに備え、
前記点環境地図保持手段は、前記点環境地図生成手段により生成された前記点データを保持することを特徴とする請求項１に記載の移動ロボット。
【請求項３】
前記周囲環境情報生成手段により生成された前記点集合データの点データの間隔が、前記点環境地図保持手段が保持する前記点環境地図中の点集合データの点データの間隔に比べて狭い場合に、前記周囲環境情報中の点データの間隔が前記点環境地図中の点データの間隔になるよう前記点データを間引く周囲環境情報調整手段をさらに備えたことを特徴とする請求項１または２に記載の移動ロボット。
【請求項４】
ＩＣＰアルゴリズム（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）を用いて前記検索対象点限定手段により限定された後の前記点環境地図中の各点データと、前記対応点データとの間の位置ずれ量を算出する位置ずれ量算出手段をさらに備え、
前記自己位置補正手段は、前記位置ずれ量算出手段により算出された前記位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正することを特徴とする請求項１から３のいずれか一項に記載の移動ロボット。
【請求項５】
自律移動可能な移動ロボットの自己位置補正方法であって、
移動センサが当該移動ロボットの移動量および移動方向を検知する第１検知ステップと、
前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、
経路センサが当該移動ロボットの周囲の移動可能な領域を検知する第２検知ステップと、
前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成ステップと、
前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、
前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップと、
を有することを特徴とする自己位置補正方法。
【請求項６】
自律移動可能な移動ロボットの自己位置補正処理をコンピュータに実行させる自己位置補正プログラムであって、
当該移動ロボットの移動量および移動方向を検知する移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、
当該移動ロボットの周囲の移動可能な領域を検知する経路センサによる検出結果を点集合データで示す周囲環境情報を取得する周囲環境情報取得ステップと、
前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、
前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップと、
を有することを特徴とする自己位置補正プログラム。
【請求項１】
自律移動可能な移動ロボットであって、
当該移動ロボットの移動量および移動方向を検知する移動センサと、
前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定手段と、
当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段と、
当該移動ロボットの周囲の移動可能な領域を検知する経路センサと、
前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成手段と、
前記周囲環境情報中の所定の点データに対応する点データであって、前記点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定手段と、
前記検索対象点限定手段により限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正する位置補正手段と、
を備えたことを特徴とする移動ロボット。
【請求項２】
前記移動ロボットが移動可能な領域を線データで示す線環境地図を保持する線環境地図保持手段と、
前記線環境地図保持手段が保持する前記線環境地図から所定の密度の点で示された前記点環境地図を生成する点環境地図生成手段と
をさらに備え、
前記点環境地図保持手段は、前記点環境地図生成手段により生成された前記点データを保持することを特徴とする請求項１に記載の移動ロボット。
【請求項３】
前記周囲環境情報生成手段により生成された前記点集合データの点データの間隔が、前記点環境地図保持手段が保持する前記点環境地図中の点集合データの点データの間隔に比べて狭い場合に、前記周囲環境情報中の点データの間隔が前記点環境地図中の点データの間隔になるよう前記点データを間引く周囲環境情報調整手段をさらに備えたことを特徴とする請求項１または２に記載の移動ロボット。
【請求項４】
ＩＣＰアルゴリズム（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）を用いて前記検索対象点限定手段により限定された後の前記点環境地図中の各点データと、前記対応点データとの間の位置ずれ量を算出する位置ずれ量算出手段をさらに備え、
前記自己位置補正手段は、前記位置ずれ量算出手段により算出された前記位置ずれ量分前記自己位置推定手段により推定された前記自己位置を補正することを特徴とする請求項１から３のいずれか一項に記載の移動ロボット。
【請求項５】
自律移動可能な移動ロボットの自己位置補正方法であって、
移動センサが当該移動ロボットの移動量および移動方向を検知する第１検知ステップと、
前記移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、
経路センサが当該移動ロボットの周囲の移動可能な領域を検知する第２検知ステップと、
前記経路センサによる検出結果を点集合データで示す周囲環境情報を生成する周囲環境情報生成ステップと、
前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、
前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップと、
を有することを特徴とする自己位置補正方法。
【請求項６】
自律移動可能な移動ロボットの自己位置補正処理をコンピュータに実行させる自己位置補正プログラムであって、
当該移動ロボットの移動量および移動方向を検知する移動センサによる検出結果に基づいて、自己位置を推定する自己位置推定ステップと、
当該移動ロボットの周囲の移動可能な領域を検知する経路センサによる検出結果を点集合データで示す周囲環境情報を取得する周囲環境情報取得ステップと、
前記周囲環境情報中の所定の点データに対応する点データであって、当該移動ロボットが移動可能な領域を点集合データで示す点環境地図を保持する点環境地図保持手段が保持する点環境地図中の点データである対応点データの候補を、前記点環境地図中の点データのうち前記所定の点データを基準として予め定められた閾値以下の距離の領域に存在する点データに限定する検索対象点限定ステップと、
前記検索対象点限定ステップにおいて限定された点を前記所定の点データに対する対応点データとして、前記点環境地図中の各点データと、前記周囲環境情報中の対応点データとの間の位置ずれ量分前記自己位置推定ステップにおいて推定された前記自己位置を補正する位置補正ステップと、
を有することを特徴とする自己位置補正プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３−１】
【図１３−２】
【図１３−３】
【図１３−４】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３−１】
【図１３−２】
【図１３−３】
【図１３−４】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【公開番号】特開２００８−２５０９０６（Ｐ２００８−２５０９０６Ａ）
【公開日】平成２０年１０月１６日（２００８．１０．１６）
【国際特許分類】
【出願番号】特願２００７−９４６２３（Ｐ２００７−９４６２３）
【出願日】平成１９年３月３０日（２００７．３．３０）
【出願人】（０００２０２３６１）綜合警備保障株式会社 (266)
【Ｆターム（参考）】
【公開日】平成２０年１０月１６日（２００８．１０．１６）
【国際特許分類】
【出願日】平成１９年３月３０日（２００７．３．３０）
【出願人】（０００２０２３６１）綜合警備保障株式会社 (266)
【Ｆターム（参考）】
[ Back to top ]