JP2010152787A - 環境地図生成プログラム、環境地図生成方法及び移動ロボット - Google Patents

Abstract

【課題】移動体の位置と環境の形状を安定的に推定する。
【解決手段】速度分散モデル予測生成手段１２は、記憶手段１１から対象の計測点の過去の推定位置と推定誤差の分散とを読み出す。そして、速度分散モデルを適用し、計測点の推定位置及び推定誤差の分散の速度分散付予測値を算出する。第１の現在位置推定手段１３は、この計測点の推定位置及び推定誤差の分散の速度分散付予測値と、計測点の３次元計測値と、を用いて移動体の現在位置・姿勢及び推定誤差の分散の予測値を算出する。そして、算出された移動体の現在位置・姿勢及び推定誤差の分散に応じて、計測点の推定位置及び推定誤差の分散が更新される。
【選択図】図１

Description

本発明は環境地図生成プログラム、環境地図生成方法及び移動ロボットに関し、特に所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と計測点の位置を推定し、環境地図を生成するための環境地図生成プログラム、その環境地図生成方法及び環境地図を生成する移動ロボットに関する。

従来、自律型の移動ロボットは、予め入力された経路地図を利用し、経路上に存在する誘導ガイド、ランドマーク、反射板などによって自身の位置を同定しながら目的地へ移動する。自身の位置の同定に用いるため、経路上のランドマークの設置地点や、経路周辺の物体の３次元形状など、移動ロボットが走行する周辺の環境を示した環境地図の生成が必要となる。従来、環境地図の生成は人手によって行われていた。このため、商業施設のように広大で変化の激しい環境では、ランドマークや環境地図の頻繁な更新に膨大な人的コストが要求されるという問題があった。

この問題に対し、近年、移動ロボットの走行時、環境地図生成と自己位置決めとを同時に行うＳＬＡＭ（Simultaneous Localization and Mapping）技術が提案されている。
図９は、従来のＳＬＡＭ技術を用いた移動ロボットの構成例を示した図である。なお、図９は、ＳＬＡＭ処理を実行する移動ロボットの処理部のみの構成を示した図である。

記憶部９１には、これまでに検出された環境内に存在する所定の計測点の推定位置が位置情報として格納される。３次元位置計測部９２は、図示しないセンサによって検出されたセンサ情報に基づいて環境の３次元計測値を生成する。ロボット現在位置推定部９３は、３次元位置計測部９２の生成した３次元位置情報と、記憶部９１から読み出した過去の同じ計測点の推定位置とを用いて、移動ロボットの現在位置・姿勢を推定する。続いて、静止対象の位置情報更新部９４は、現在の移動ロボットの位置・姿勢の推定値を利用して、環境の現在の計測点のデータを、過去の同じ計測点のデータに統合して、環境の計測点の推定位置を更新する。

しかし、このようなＳＬＡＭ処理では、移動ロボット以外に運動する物体のない静的環境を前提としている場合が多い。このため、環境内に運動体が存在して、その位置・姿勢が変化すると、環境内の計測点の現在の３次元計測値と過去の推定位置との間に矛盾が発生し、移動ロボットの自己位置の推定に誤りが生じる場合がある。そして最悪の場合、位置推定が破綻する恐れがある。このため、検出された物体が静止体であるか、運動体であるかを判別する必要がある。

例えば、他のセンサ情報などにより環境内で運動する対象を検出する手段を別途設ける方法がある。図１０は、従来の運動体検出機能を備えた移動ロボットの構成例を示した図である。図９と同じものには同じ番号を付し、説明は省略する。

運動体検出機能を備えた移動ロボットは、図９の構成に加え、運動対象検出部９５と、静止対象選択部９６とを有する。また、図示はしていないが、従来の環境の３次元位置を計測するためのセンサ情報（図１０ではセンサ情報２）を生成するセンサ（以下、位置検出用センサとする）とは別に、運動体を検出するためのセンサ情報（図１０ではセンサ情報１）を生成するセンサ（以下、運動体検出用センサとする）を設ける必要がある。

運動対象検出部９５は、運動体検出用センサからセンサ情報１を取得し、検出された環境内の対象が運動しているか静止しているかを判定する。静止対象選択部９６は、運動対象検出部９５の判定結果を取得し、検出された物体が静止体であるときのみ、３次元位置計測部９２が生成した３次元計測値を用いたＳＬＡＭ処理を行う。ＳＬＡＭ処理の内容は、図９に示した構成の場合と同様である。

また、静止体と運動体とを識別する手法として、レーダの周波数を正の方向と負の方向にそれぞれシフトして取得されたレーダ画像の相互相関に基づいて運動体と静止体とを識別し、さらに自車両の運動を推定する手法がある（例えば、特許文献１参照）。

同様に、基準カメラと参照カメラとによって撮影された画像から車両が走行する平面領域を抽出し、その時間的変化から車両の移動量を推定し、静止体と運動体を識別する手法もある（例えば、特許文献２参照）。
特開２００７−２８５９１２号公報 特開２００６−５３７５５号公報

しかし、従来の環境地図の自動作成では、運動体検出用センサを新たに設けなければ、動環境において移動ロボットの位置と環境の形状を安定的に推定することが難しいという問題点があった。なお、動環境とは、環境内に移動ロボット以外に運動する物体が存在する環境を言う。

図１０に示したように、周辺環境の３次元計測値を得るための位置検出用センサとは別に、運動体検出用センサを設ければ、静止体と運動体との識別は可能となる。しかしながら、新たにセンサを設けるためには、そのためにコスト増になるという問題がある。また、そのスペースを移動ロボットに確保しなければならない。

このため、別途センサを用いることなく、本来移動ロボットに搭載されている周辺環境の３次元計測値を得るための位置検出用センサが検出するセンサ情報から、環境内の運動する対象を的確に検出することが求められている。

なお、上記の周波数をシフトさせて静止体と運動体を識別する手法は、検出が２次元平面内に限定され、３次元での識別が必要な移動ロボットには適さない。また、車両が走行する平面領域を抽出する手法は、基準カメラに加え参照カメラが必要になるほか、平面領域を抽出しなければならず、処理が煩雑になるという問題がある。

本発明はこのような点に鑑みてなされたものであり、動環境においても位置検出用センサのセンサ情報から移動体の位置と環境の形状を安定的に推定することが可能な環境地図生成プログラム、環境地図生成方法及び移動ロボットを提供することを目的とする。

上記課題を解決するために、所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と計測点の位置を推定し、環境地図を生成する処理を実行させる環境地図生成プログラムが提供される。この環境地図生成プログラムは、コンピュータを、速度分散モデル予測手段及び現在位置推定手段として機能させる。速度分散モデル予測手段は、計測点の計測値が取得されると、環境内で過去に算出された計測点の推定位置を含む位置情報が格納される記憶手段から計測点に対応する位置情報を抽出する。そして、位置情報に含まれる計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する。現在位置推定手段は、環境内を自律移動しながら計測点の計測を行っている移動体の現在位置を、速度分散モデル予測手段によって予測された計測点の過去の速度分散付推定位置と、計測点の計測値と、を用いて推定する。

このような環境地図生成プログラムを実行するコンピュータによれば、環境内の計測点が計測され、計測値が取得されると、記憶手段から計測点に対応する位置情報が抽出される。そして、速度分散を考慮した運動モデルが適用され、計測点の過去の推定位置に速度分散を付加した速度分散付推定位置が予測される。次に、速度分散付推定位置と、計測値とに基づいて移動体の現在位置が推定される。

また、上記課題を解説するために、上記環境地図生成プログラムと同様の処理を実行する環境地図生成方法、及び上記環境地図生成プログラムの機能を実現する移動ロボットが提供される。

開示の環境地図生成プログラム、環境地図生成方法及び移動ロボットによれば、環境内に運動する物体があっても、計測点の位置を検出するためのセンサ情報に基づいて、移動体の位置と環境の形状を安定的に推定し、環境地図を生成することができる。

以下、本発明の実施の形態を図面を参照して説明する。まず、発明の概要について説明し、その後、具体的な内容を説明する。
図１は、発明の概要を示した図である。

環境地図生成処理は、記憶手段１１、速度分散モデル予測生成手段１２、第１の現在位置推定手段１３、静止モデル予測生成手段１４、運動モデル予測生成手段１５、計測点静止／運動判定手段１６、第２の現在位置推定手段１７及び静止計測点位置更新手段１８により実行される。なお、各処理手段は、コンピュータが環境地図生成プログラムを実行することにより、その処理機能を実現する。

記憶手段１１には、これまでの処理によって算出された静止計測点の位置の推定値と、推定誤差の分散とを含む位置情報が格納される。なお、静止計測点とは、経路上またはその周辺を含む環境内で検知された物体の特徴点を計測した計測点のうち、静止していると判定された計測点を言う。位置情報には、環境内の静止計測点を識別する識別情報に関連付けて、この静止計測点の位置の推定値と、推定誤差の分散とが登録される。

速度分散モデル予測生成手段１２は、計測点が所定の速度分散で移動しているという運動モデル（以下、速度分散モデルとする）に基づいて、過去の静止計測点の位置の推定値（以下、推定位置とする）及びその推定誤差の分散に、速度分散を付加する。そして、速度分散が付加された速度分散付推定位置及び推定誤差の分散を予測する。

速度分散モデルについて説明する。移動体が移動中、位置検出センサによって物体が検知され、計測が行われる。ＳＬＡＭでは計測が行われるごとに、計測が行われた任意の計測点ｉ（ｉは計測点に割り当てられた任意の整数）の位置の推定が行われる。計測点ｉについて算出された推定位置は、推定誤差の分散を持つ。

ここで、時刻ｔにおける計測点ｉの推定位置と推定誤差の分散行列とを、
推定位置：ｐ_i（ｔ）＝［ｘ_i（ｔ） ｙ_i（ｔ） ｚ_i（ｔ）］^T，
推定誤差の分散行列：Ｐ_i（ｔ） ・・・（１）
と表す。なお、［ ］^Tは、転置行列を表す。

従来のＳＬＡＭ処理では、計測点ｉが静止しているという前提で処理が行われた。この場合、時刻（ｔ＋Ｔ）における推定位置ｐ_i（ｔ＋Ｔ）と、推定誤差の分散行列Ｐ_iとは、
ｐ_i（ｔ＋Ｔ）＝ｐ_i（ｔ），
Ｐ_i（ｔ＋Ｔ）＝Ｐ_i（ｔ） ・・・（２）
と表された。すなわち、時刻（ｔ＋Ｔ）における計測点ｉの推定位置と推定誤差の分散行列は、時刻ｔの時点と同じになる。

これに対し、速度分散モデルでは、計測点は、速度０、かつ所定の速度誤差の分散で移動しているとみなし、計測点ｉの推定位置と推定誤差とを算出する。この運動モデルによれば、時刻（ｔ＋Ｔ）における推定位置ｐ_i（ｔ＋Ｔ）と、推定誤差の分散行列Ｐ_i（ｔ＋Ｔ）とは、

と表すことができる。なお、Ｐ_i（ｔ）及びＶ_iは、それぞれ３×３の行列になる。このように、時刻（ｔ＋Ｔ）における推定位置ｐ_i（ｔ＋Ｔ）は、速度０により時刻ｔの推定位置ｐ_i（ｔ）と同じになる。しかし、推定誤差の分散行列Ｐ_i（ｔ＋Ｔ）には、速度誤差の分散Ｖ_iが含まれる。これにより、計測点ｉは、ある確率を持って動いているとみなすことができる。

速度分散モデル予測生成手段１２では、記憶手段１１に格納される過去、例えば時刻ｔにおける計測点ｉの推定位置ｐ_i（ｔ）と、推定誤差の分散行列Ｐ_i（ｔ）とを読み出す。そして、式（３）に示した速度分散モデルを適用し、推定位置と推定誤差の分散行列に速度分散を付加する。算出された速度分散付推定位置及び推定誤差の分散の予測値は、第１の現在位置推定手段１３へ出力される。

第１の現在位置推定手段１３は、図示しない位置検出用センサが検知した計測点ｉの３次元計測値を取得する。また、速度分散モデル予測生成手段１２から計測点ｉの速度分散付推定位置及び推定誤差の分散の予測値を取得する。そして、計測データと、速度分散が付加された過去の推定位置及び推定誤差の分散と、を利用して、移動体の現在位置・姿勢及び推定誤差の分散を算出する。これらの算出には、例えばカルマンフィルタを用いて算出する方法が知られている。ここでは、一般的な算出方法を用いるとし、詳細な説明は後述する。算出された移動体の現在位置・姿勢及び推定誤差の分散の予測値は、未知の運動体が計測点に含まれている可能性のある第１段階の予測値である。したがって、以下の説明では概略推定値とする。

静止モデル予測生成手段１４では、計測点が静止していると仮定したときの静止モデルを用いて、現時の計測点ｉの予測値を算出する。このため、記憶手段１１に格納される位置情報から、対象の計測点ｉの位置情報（推定位置と推定誤差の分散）を読み出す。例えば、時刻ｔにおける推定位置ｐ_i（ｔ）と、推定誤差の分散行列Ｐ_i（ｔ）とを読み出し、時刻（ｔ＋Ｔ）における予測値を、

によって算出する。算出された静止モデルを適用した計測点ｉの予測値は、計測点静止／運動判定手段１６へ出力される。
運動モデル予測生成手段１５は、計測点が運動していると仮定したときの運動モデルを用いて、現時の計測点ｉの予測値を算出する。

運動モデルについて説明する。図２は、計測点の運動モデルを示した図である。図では、計測点ｉは定速度で運動していると仮定している。
計測点ｉの時刻ｔにおける推定位置をｐ_i（ｔ）、時刻（ｔ−Ｔ）における推定位置をｐ_i（ｔ−Ｔ）、時刻（ｔ＋Ｔ）における推定位置をｐ_i（ｔ＋Ｔ）とすると、ｐ_i（ｔ−Ｔ）からｐ_i（ｔ）までの移動速度ｖ_i（ｔ）は、図に示したように、
ｖ_i（ｔ）＝｛ｐ_i（ｔ）−ｐ_i（ｔ−Ｔ）｝／Ｔ ・・・（５）
と表すことができる。ｐ_i（ｔ＋Ｔ）まで同じ速度で移動することから、運動モデルを適用した場合の時刻（ｔ＋Ｔ）における予測値は、

によって算出することができる。算出された運動モデルを適用した計測点ｉの予測値は、計測点静止／運動判定手段１６へ出力される。
計測点静止／運動判定手段１６は、静止モデル予測生成手段１４が静止モデルを適用した計測点ｉの予測値と、運動モデル予測生成手段１５が運動モデルを適用した計測点ｉの予測値とを取得する。これらの予測値を、移動体の現在位置・姿勢の概略推定値を利用して位置検出用センサの座標系に変換し、それぞれのモデルについて計測点ｉの現在の計測値及び計測誤差の分散の予測値を生成する。これにより、計測点ｉが静止していると仮定したときに得られる計測値の予測値及び計測誤差の分散の予測値が生成される。同様に、計測点ｉが運動していると仮定したときに得られる計測値の予測値及び計測誤差の分散の予測値が生成される。そして、現在の実計測値と計測誤差の分散と、それぞれのモデルについて算出された計測値及び計測誤差の分散の予測値との統計的一致度を算出する。そして、統計的一致度が高い予測値を生成したモデルを選択することにより、計測点の静止／運動を判別する。さらに、計測点が運動している運動計測点と判別されたときは、その運動計測点を位置情報の登録から削除し、位置情報を更新する。

第２の現在位置推定手段１７は、運動計測点が削除された位置情報が格納される記憶手段１１から対象の計測点の推定位置と推定誤差の分散とを読み出す。そして、計測点の３次元計測値を用いて、移動体の現在位置・姿勢及び推定誤差の分散を算出する。算出方法は、第１の現在位置推定手段１３と同様である。運動体と判定された計測点が削除された位置情報に基づいて算出された移動体の現在位置・姿勢及び推定誤差の分散の予測値は、詳細推定値とする。詳細推定値は、静止計測点位置更新手段１８に出力される。なお、図１では、第１の現在位置推定手段１３と別の構成にしているが、共通の現在位置推定手段で処理を行うとしてもよい。第１の現在位置推定手段１３と、第２の現在位置推定手段１７は、入力が異なることを除いて同じである。

静止計測点位置更新手段１８は、計測点の３次元計測値、第２の現在位置推定手段１７が算出した移動体の現在位置・姿勢及び推定誤差の分散の詳細推定値、及び過去の計測点の推定位置と推定誤差の分散とを入力する。そして、これらの情報を用いて、静止計測点の推定位置と、推定誤差の分散とを再度算出し、記憶手段１１に格納される位置情報を更新する。

このような各処理手段によって実行される環境地図の生成方法について説明する。
移動体に搭載される図示しない位置検出用センサが検知した計測点の３次元計測値が取得され、処理が開始される。

速度分散モデル予測生成手段１２は、記憶手段１１に格納される位置情報から、これまでに算出された対象の計測点（静止していると判定された静止計測点）の推定位置及びその推定誤差の分散を抽出する。そして、計測点が速度０、かつ所定の速度分散を持って移動しているとみなす速度分散モデルによって、過去の計測点の推定位置及び推定誤差の分散に速度分散を付加した予測値を算出する。第１の現在位置推定手段１３は、速度分散が付加された過去の計測点の推定位置及び推定誤差の分散の予測値と、３次元計測値とに基づいて移動体の現在位置・姿勢及び推定誤差の分散の予測値（概略推定値）を算出する。こうして算出される移動体の現在位置・姿勢及び推定誤差の分散の予測値には、過去の計測点の推定位置に速度の分散が加味されている。したがって、計測点が運動していた場合であっても、移動体の位置・姿勢の安定的な推定が可能となる。

次に、第１の現在位置推定手段１３が算出した移動体の現在位置・姿勢及び推定誤差の分散の概略推定値に基づいて、計測点の種別を判定する。上述のように、計測点には静止計測点と、運動計測点とがある。このため、静止モデル予測生成手段１４及び運動モデル予測生成手段１５が、それぞれのモデルに基づいて、計測点の現在の推定位置と推定誤差の分散の予測値を算出する。計測点静止／運動判定手段１６は、それぞれのモデルについて算出された予測値を対象の計測点の計測値と計測誤差の分散に変換し、実計測値とその計測誤差の分散との統計的一致度を算出する。そして、より適合する予測値を生成したモデルを選択し、計測点の静止／運動状態を判定する。静止モデルの予測値の方が実計測値との統計的一致度が高いときは、この計測点は静止計測点と判定される。一方、運動モデルの予測値の方が実計測値との統計的一致度が高いときは、この運動計測点と判定される。運動計測点と判定された計測点は、位置情報から削除する。このように、別途運動検出用センサを設けることなく、計測点が静止しているのか運動しているのかを判定することができる。

続いて、第２の現在位置推定手段１７は、運動計測点が削除され、静止計測点のみが登録される位置情報を用いて再度移動体の現在位置・姿勢及び推定誤差の分散を推定する。位置情報に登録される静止計測点の推定位置と推定誤差の分散と、３次元計測値とを用いて移動体の現在位置・姿勢と推定誤差分散の予測値を算出する。そして、静止計測点位置更新手段１８は、第２の現在位置推定手段１７によって算出された移動体の現在位置・姿勢と推定誤差の分散の予測値、３次元計測値、及び過去の対象計測点の静止計測点推定位置と推定誤差の分散とを用いて、静止計測点の推定位置と推定誤差の分散とを更新する。更新された情報は記憶手段１１に格納される。このように、運動計測点が削除された位置情報を用いて再度移動体の現在位置を推定することにより、より高い精度で現在位置・姿勢及び推定誤差の分散を予測することができる。また、この予測値を用いて再度計測点の位置情報を算出すれば、計測点の推定位置及び推定誤差の分散も高い精度で得られる。

以上のように、環境内に運動する物体があっても、計測点の位置を検出するためのセンサ情報に基づいて、移動体の位置と環境の形状を安定的に推定し、環境地図を生成することが可能となる。

以下、上記の環境地図生成方法を移動ロボットが実行する場合について、詳細に説明する。図３は、移動ロボットの制御部のハードウェア構成を示した図である。
移動ロボット（制御部）１００は、ＣＰＵ（Central Processing Unit）１０１によって全体が制御されている。ＣＰＵ１０１には、バス１０７を介してＲＡＭ（Random Access Memory）１０２、ハードディスクドライブ（ＨＤＤ：Hard Disk Drive）１０３、入力インタフェース１０４、通信インタフェース１０５及び走行部インタフェース１０６が接続されている。

ＲＡＭ１０２には、ＣＰＵ１０１に実行させるＯＳ（Operating System）のプログラムやアプリケーションプログラムの少なくとも一部が一時的に格納される。また、ＲＡＭ１０２には、ＣＰＵ１０１による処理に必要な各種データが格納される。ＨＤＤ１０３には、ＯＳやアプリケーションのプログラムが格納される。なお、ＨＤＤ１０３の代わりに、ＲＯＭ（Read Only Memory）などでその機能を実現してもよい。入力インタフェース１０４は、位置検出用センサ１０８が接続されており、ＣＰＵ１０１からの命令に従って位置検出用センサ１０８を操作し、計測データを取得する。通信インタフェース１０５は、無線通信によって接続する遠隔サーバ１０９との間でデータの送受信を行う。走行部インタフェース１０６は、ＣＰＵ１０１からの命令に従って、走行指示を走行部へ出力する。また、障害物検出などの走行情報が走行部から入力されたときは、バス１０７を介してＣＰＵ１０１に通知する。

このようなハードウェア構成によって、本実施の形態の処理機能を実現することができる。
次に、このような構成の移動ロボットの環境地図生成処理部のソフトウェア構成について説明する。図４は、移動ロボットの環境地図生成処理部のソフトウェア構成を示した図である。

移動ロボットの環境地図生成処理部は、記憶部１１０、速度分散モデル予測生成部１２０、現在位置推定部１３０、静止モデル予測生成部１４０、運動モデル予測生成部１５０、計測点静止／運動判定部１６０、静止計測点位置更新部１８０及び位置計測部１９０を有する。

記憶部１１０は、静止計測点位置情報を格納する。速度分散モデル予測生成部１２０は、
速度分散モデルを用いて、対象の計測点の過去の推定位置及び推定誤差の分散に速度分散を付加する。

現在位置推定部１３０は、移動ロボットの現在位置・姿勢及び推定誤差の分散を推定する。図４の現在位置推定部１３０は、第１の現在位置推定手段１３及び第２の現在位置推定手段１７として機能し、入力を適宜切り替え、概略推定値または詳細推定値を算出する。まず、速度分散モデル予測生成部１２０が算出した対象の計測点の過去の推定位置及び推定誤差の分散に速度分散を付加した予測値（ａ）を入力し、３次元計測値を用いて概略推定値を算出する。概略推定値は、計測点静止／運動判定部１６０へ出力する。静止計測点位置情報から運動する計測点が削除された後は、静止計測点位置情報に設定される静止計測点の過去の推定位置及び推定誤差の分散（ｂ）を入力し、詳細推定値を算出する。詳細推定値は、静止計測点位置更新部１８０へ出力する。

静止モデル予測生成部１４０は、静止モデルを用いて、対象の計測点を静止計測点と仮定した場合の現時の推定位置及び推定誤差の分散を予測する。運動モデル予測生成部１５０は、運動モデルを用いて、対象の計測点を運動計測点と仮定した場合の現時の推定位置及び推定誤差の分散を予測する。計測点静止／運動判定部１６０は、それぞれのモデルについて算出された予測値から得られる計測値の予測と、実際の３次元計測値との統計的一致度を求める。そして、より一致しているモデルを選択し、計測点が静止計測点、運動計測点のいずれであるかを判定する。運動計測点と判定されたときは、この計測点に対応する静止計測点位置情報を削除する。

静止計測点位置更新部１８０は、移動ロボットの現在位置の詳細推定値と、静止計測点と判定された対象計測点の３次元計測値及び過去の推定位置と推定誤差の分散と、を用いて静止計測点の推定位置及び推定誤差の分散を算出し、この静止計測点に対応する静止計測点位置情報を更新する。

記憶部１１０に格納される静止計測点位置情報について説明する。図５は、静止計測点位置情報の一例を示した図である。
静止計測点位置情報１１００には、静止計測点１１０１、推定位置１１０２及び推定誤差１１０３の情報項目が設定される。

静止計測点１１０１には、静止計測点として検知された計測点（静止物体の特徴点）の識別情報が設定される。図５の例では、識別情報として静止計測点に一意の番号が割り振られている。推定位置１１０２には、静止計測点１１０１で指示される静止計測点の推定位置ｐ_iが設定される。推定誤差１１０３には、対応する静止計測点の推定誤差の分散Ｐｉが設定される。例えば、静止計測点１１０１の位置座標は、ｐ₁＝［ｘ₁，ｙ₁，ｚ₁］^T、推定誤差はＰ₁になる。

なお、静止計測点位置情報１１００には、未知の計測点が静止計測点と判定されたときは、この計測点が新たに登録される。一方、登録されている静止計測点が運動計測点と判定されたときは、削除される。こうして、計測点について計測が行われるごとに登録計測点の静止／運動が判定され、静止計測点位置情報１１００が更新される。また、静止計測点の推定位置及び推定誤差の分散についても、計測が行われるごとに更新される。これにより、静止計測点位置情報１１００は、常に最新の状態に保たれる。また、計測回数が増えるに従って、推定位置もより精度の高い値になっていく。

このように、過去の静止計測点位置情報が記憶部１１０に格納された状態で、経路地図に沿って移動ロボットの走行が開始される。なお、第１回目の走行時の静止計測点位置情報は、静止計測点なしの状態、あるいは、人手により初期値が設定された状態で開始される。

位置計測部１９０は、所定のタイミングで移動ロボットの位置検出用センサ１０８からセンサ情報を取得する。この位置検出用センサ１０８は、例えば、進行方向や周囲にレーザ光線や可視光線、赤外線、超音波信号などの探査信号を放射し、対象の物体の反射信号を受信して対象の物体を検知する。

図６は、位置検出用センサの動作を示した図である。図の例では、階段３００上の各地点を計測点１，２，３，・・・，ｉ−１，ｉとして検出する。
図６では、位置検出用センサ１０８は、時刻ｔ１でａ地点で各計測点の計測を行い、時刻ｔ２に移動ロボットがｂ地点まで移動したとき、再度各計測点の計測を行うとする。

時刻ｔ１にａ地点に到達した位置検出用センサ１０８ａは、鎖線で示された領域を探査範囲として、探査範囲に含まれる計測点の計測を行う。各計測点について検知されたセンサ情報Ｓ₁（ｔ１），Ｓ₂（ｔ１），・・・，Ｓ_i（ｔ１）は、位置計測部１９０へ引き渡される。位置計測部１９０は、各計測点のセンサ情報を用いて、各計測点の３次元計測値を生成する。その後、経路を移動し、時刻ｔ２にｂ地点まで移動した位置検出用センサ１０８ｂは、一点鎖線で示された領域を探査範囲として、探査範囲に含まれる計測点の計測を行う。各計測点について検知されたセンサ情報Ｓ₁（ｔ２），Ｓ₂（ｔ２），・・・，Ｓ_i（ｔ２）は、位置計測部１９０に引き渡される。そして、上記と同様に、各計測点の３次元計測値が得られる。

現在位置推定部１３０は、得られた３次元計測値と、速度分散モデル予測生成部１２０が算出する計測点の過去の推定位置及び推定誤差の分散に速度分散が付加された予測値とを用いて、このときの移動ロボットの位置（ａ地点またはｂ地点に相当する）と姿勢（探査範囲に相当する）の推定を行う。

まず、速度分散モデル予測生成部１２０は、対象の計測点１，２，３，・・・，ｉ−１，ｉに対応する過去の推定位置及び推定誤差の分散を静止計測点位置情報から読み出す。
そして、各計測点について速度分散モデルに基づく式（３）を用いて、速度分散が付加された計測点の過去の推定位置及び推定誤差の分散の予測値を算出する。これを予測値ａとする。現在位置推定部１３０は、３次元計測値と、速度分散モデル予測生成部１２０が算出した予測値ａとを用いて、以下のカルマンフィルタにより、移動ロボットの現在位置・姿勢及び推定誤差の分散を算出する。まず、状態変数を以下のように定義する。

時刻ｔにおける計測点の推定位置と推定誤差の共分散行列を、式（１）と同様に、次のように定義する。
推定位置： ｐ_i（ｔ）＝［ｘ_i（ｔ） ｙ_i（ｔ） ｚ_i（ｔ）］^T，
推定誤差の共分散行列： Ｐ_i（ｔ）、
なお、ｘ，ｙ，ｚは、３次元座標の水平、垂直及び奥行き方向の座標を表す。

移動ロボットの位置・姿勢と、推定誤差の共分散行列を次のように定義する。

そして、移動ロボットによる計測点位置の計測値と計測誤差の共分散行列を次のように定義する。

ここで、ｔは時刻、ｉは計測点のインデックスである。また、０≦ｉ≦（Ｎ_f−１）とする。Ｎ_fは計測点の個数である。
そして、以下に示すカルマンフィルタを適用し、移動ロボットの現在位置・姿勢及び推定誤差の分散の予測値を得る。

移動ロボットの位置などに関する概略推定値が算出された後、計測点が静止しているか運動しているかが判定される。静止モデル予測生成部１４０が、式（４）に示した静止モデルを用いて、計測点が静止しているときの現在の計測点の推定位置と、推定誤差の分散とを予測する。また、運動モデル予測生成部１５０は、式（６）に示した運動モデルを用いて、計測点が運動しているときの現在の計測点の推定位置と、推定誤差の分散とを予測する。

計測点静止／運動判定部１６０では、静止モデル予測生成部１４０が生成した予測値を移動ロボットの位置検出用センサの座標系に変換して、現在の計測値及び計測誤差の分散の予測値を算出する。そして、静止モデルに基づく計測値及び計測誤差の分散の予測値と、実計測データ及び計測誤差の分散との統計的一致度を算出する。同様にして、運動モデル予測生成部１５０が生成した予測値から現在の計測値及び計測誤差の分散の予測値を算出する。そして、運動モデルに基づく計測値及び計測誤差の分散の予測値と、実計測データ及び計測誤差の分散との統計的一致度を算出する。統計的一致度としては、例えば、以下のＢｈａｔｔａｃｈａｒｒｙａの評価値を利用する。

ここで、ｆ_A（ｘ），ｆ_B（ｘ）は、確率密度関数である。式（１４）の解析解は、１次元の標準関数（ガウス分布）の場合については、それぞれの平均と分散μ_A，μ_B，σ_A，σ_Bを用いて、以下の式で得られる。

こうして算出されたそれぞれのモデルの実計測データとの統計的一致度を比較し、一致度の高い方のモデルを選択する。静止モデルが選択された計測点は、静止計測点と判定される。また、運動モデルが選択された計測点は、運動計測点と判定される。そして、運動計測点と判定されたときは、この計測点を静止計測点位置情報から削除する。

続いて、現在位置推定部１３０が、運動計測点が削除された静止計測点位置情報から対象の計測点の静止計測点推定位置と推定誤差の分散（ｂ）を読み出し、３次元計測値とを用いて移動ロボットの現在位置・姿勢と、その推定誤差の分散を算出する。算出は、記憶部１１０に格納される静止計測点推定位置・推定誤差の分散を用いることを除き、概略推定値の場合と同様に行われる。

こうして、移動ロボットの現在位置・姿勢と推定誤差の分散の詳細推定値が算出されると、静止計測点位置更新部１８０がこれを用いて計測点の位置を算出する。
静止計測点位置更新部１８０では、各計測点について、３次元計測値と、静止計測位置情報に登録される過去の推定位置と推定誤差の分散を用いて、状態変数を、

とし、カルマンフィルタ、

により、各計測点の推定位置と推定誤差の分散を更新する。
このように、運動計測点が削除された静止計測点位置情報を用いて再度推定された移動ロボットの現在位置・姿勢及び推定誤差の分散を用いて各計測点の推定位置及び推定誤差の分散を算出する。これにより、推定時に誤差の要因となる運動計測点が含まれる可能性を抑えることができるため、安定的かつ高い精度で計測点の位置を推定することが可能となる。

次に、環境地図生成処理の手順についてフローチャートを用いて説明する。図７は、移動ロボットによる環境地図生成処理の手順を示したフローチャートである。
［ステップＳ０１］ 位置計測部１９０が、位置検出用センサが検知した進行方向あるいは周囲の物体の特徴点などの計測点のセンサ情報を取得し、３次元計測値を生成する。

［ステップＳ０２］ 速度分散モデル予測生成部１２０が、ステップＳ０１で計測された各計測点について、計測点が速度０、速度誤差の分散を持って移動しているとみなす速度分散モデルに基づく予測値を生成する。すなわち、各計測点について、記憶部１１０に格納される静止計測点位置情報から対応する過去の同じ計測点の推定位置と、推定誤差の分散とを読み出す。そして、速度分散モデルに基づく式（３）を用いて、読み出した過去のデータから、速度分散が付加された推定位置と推定誤差の分散の予測値を算出する。

［ステップＳ０３］ 現在位置推定部１３０が、各計測点の３次元計測値と、過去の推定値に速度分散を付加した推定位置及び推定誤差の分散とから、移動ロボットの現在位置・姿勢を推定する。ステップＳ０２で算出された各計測点の推定位置と推定誤差の分散の予測値と、ステップＳ０１で計測された各計測点の３次元計測値とから、式（１）、式（７）〜（１３）に示したカルマンフィルタを用いて算出する。これにより、移動ロボットの現在位置・姿勢の推定値と、推定誤差の分散が算出される。なお、算出に用いた計測点には、運動計測点が含まれている可能性があるため、ここで算出される推定値は、概略推定値とする。

［ステップＳ０４］ 静止モデル予測生成部１４０及び運動モデル予測生成部１５０が、各計測点について、過去の推定位置及び推定誤差の分散から、静止または運動を仮定したモデルに基づいて現在の予測値を算出する。静止モデル予測生成部１４０は、各計測点について、記憶部１１０に格納される静止計測点位置情報から対応する過去の同じ計測点の推定位置と、推定誤差の分散とを読み出す。そして、静止モデルに基づく式（４）を用いて、読み出した過去のデータから、現在の計測点が静止していたときの推定位置と推定誤差の分散の予測値を算出する。また、運動モデル予測生成部１５０は、同様に、過去の同じ計測点の推定位置と推定誤差の分散とを読み出し、運動モデルに基づく式（６）を用いて、現在の計測点が運動していたときの推定位置と推定誤差の分散の予測値を算出する。

［ステップＳ０５］ 計測点静止／運動判定部１６０が、ロボットの現在位置・姿勢の推定値に基づいて、ステップＳ０４で算出された計測点の静止モデル及び運動モデルに応じた予測値から、計測値の予測値を生成する。そして、実際の計測データと、モデルごとに算出された計測値の予測値との統計的一致度を評価し、計測点の静止／運動を判別する。このため、ステップＳ０４で算出された静止モデルに基づく計測点の推定位置を計測データの座標系に変換する。そして、ステップＳ０１で生成された実計測データである３次元計測値との統計的一致度を算出する。例えば、式（１５）に示したような評価式を用いる。運動モデルに基づく計測点の推定位値についても同様の処理を行い、実計測データとの統計的一致度を算出する。そして、一致度の高いモデルを選択し、計測点の静止／運動を判定する。このとき、運動計測点と判定された計測点は、静止計測点位置情報から削除する。

［ステップＳ０６］ 現在位置推定部１３０は、静止計測点について、計測データと過去の推定位置及び推定誤差の分散から、移動ロボットの現在位置・姿勢を推定する。すなわち、ステップＳ０５の処理により運動していると判定された運動計測点が削除され、静止計測点のみが登録される静止計測点位置情報から、各計測点の推定位置及び推定誤差の分散を読み出す。そして、読み出された各計測点の過去の推定位置及び推定誤差の分散と、ステップＳ０１で計測された各計測点の３次元計測値とから、式（１）、式（７）〜（１３）に示したカルマンフィルタを用いて算出する。これにより、移動ロボットの現在位置・姿勢の推定値と、推定誤差の分散が算出される。なお、算出に用いた計測点は、運動計測点が削除されているため、ここで算出される推定値は、精度の高い詳細推定値とすることができる。

［ステップＳ０７］ 静止計測点位置更新部１８０が、各静止計測点について、３次元計測値と、ステップＳ０６で算出された移動ロボットの現在位置・姿勢の詳細推定値と、を用いて推定位置及び推定誤差の分散を算出する。具体的には、静止計測点の過去の推定位置及び推定誤差の分散を静止計測点位置情報から読み出す。そして、読み出した静止計測点の過去の推定位置及び推定誤差の分散と、ステップＳ０１で計測された対応する計測点の３次元計測値とを用いて、式（１６）〜（２３）に示したカルマンフィルタによって算出する。算出された静止計測点の推定位置及び推定誤差の分散を、該当する静止計測点位置情報に書き込み、静止計測点位置情報を更新する。

以上の処理手順が実行されることにより、新たに計測点の運動検出用センサを用いることなく、移動ロボットの現在位置・姿勢及び各計測点の形状を安定的に推定することが可能となる。なお、最初の現在位置推定処理では、計測点が速度分散を持っているというモデルに基づいて移動ロボットの現在位置・姿勢が推定される。したがって、環境内に未知の運動する物体が存在する場合でも、移動ロボットの位置の安定的な推定を行うことができる。また、計測データが得られる度に、静止モデル及び運動モデルを用いて計測点の静止／運動が判定され、運動している計測点は除外されていく。したがって、移動ロボットの現在位置・姿勢の推定及び静止計測点の推定位置の精度は、計測が行われるごとに上がってゆく。この結果、精度の高い環境地図を生成することができる。

なお、経路を走行する移動ロボットが計測を行う度に上記の処理手順を行うことにより、静止計測点の推定位置の精度を上げることができるが、さらに、１回の処理における計測点の静止／運動判定の精度を上げることもできる。図７に示した処理手順では、ステップＳ０５の静止／運動判定は、移動ロボットの現在位置・姿勢の概略推定値によって行われている。これを、より精度の高い詳細推定値を用いることによって、静止／運動判定の精度を上げることができる。以下、その処理手順について説明する。

図８は、移動ロボットによる他の環境地図生成処理の手順を示したフローチャートである。以下のステップＳ１１からステップＳ１６の処理は、図７のステップＳ０１からステップＳ０６と同様であるので、詳細な説明は省略する。

［ステップＳ１１］ 位置計測部１９０が、位置検出用センサが検知した進行方向あるいは周囲の物体の特徴点などの計測点のセンサ情報を取得し、３次元計測値を生成する。
［ステップＳ１２］ 速度分散モデル予測生成部１２０が、ステップＳ１１で計測された各計測点について、計測点が速度０、速度誤差の分散を持って移動しているとみなす速度分散モデルに基づく予測値を生成する。

［ステップＳ１３］ 現在位置推定部１３０が、各計測点の３次元計測値と、過去の推定値に速度分散を付加した推定位置及び推定誤差の分散とから、移動ロボットの現在位置・姿勢を推定する。得られた移動ロボットの現在位置・姿勢及び推定誤差の分散の予測値は、概略推定値になる。

［ステップＳ１４］ 静止モデル予測生成部１４０及び運動モデル予測生成部１５０が、各計測点について、過去の推定位置及び推定誤差の分散から、静止または運動を仮定したモデルに基づいて現在の予測値を算出する。

［ステップＳ１５］ 計測点静止／運動判定部１６０が、ロボットの現在位置・姿勢の推定値に基づいて、ステップＳ１４で算出された計測点の静止モデル及び運動モデルに応じた予測値から、計測値の予測値を生成する。そして、実際の計測データと、モデルごとに算出された計測値の予測値との統計的一致度を評価し、計測点の静止／運動を判別する。このとき、運動計測点と判定された計測点は、静止計測点位置情報から削除する。

［ステップＳ１６］ 現在位置推定部１３０は、静止計測点について、計測データと過去の推定位置及び推定誤差の分散から、移動ロボットの現在位置・姿勢を推定する。得られた移動ロボットの現在位置・姿勢及び推定誤差の分散の予測値は、詳細推定値になる。

［ステップＳ１７］ ステップＳ１５の計測点の静止／運動判定処理で、運動していると判定された計測点（運動計測点）が検出されたかどうかを判定する。検出されたときは、処理をステップＳ１５に戻し、移動ロボットの現在位置・姿勢の詳細推定値を用いて計測点の静止／運動判定処理を行わせる。検出されないときは、処理をステップＳ１８に進める。

［ステップＳ１８］ 静止計測点位置更新部１８０が、各静止計測点について、３次元計測値と、ステップＳ１６で算出された移動ロボットの現在位置・姿勢の詳細推定値と、を用いて推定位置及び推定誤差の分散を算出する。算出された静止計測点の推定位置及び推定誤差の分散を、該当する静止計測点位置情報に書き込み、静止計測点位置情報を更新する。

以上の処理手順が実行されることにより、運動計測点がすべて除外されるまで、運動計測点が除外されたより確度の高い移動ロボットの現在位置・姿勢の推定値で、繰り返し計測点の静止／運動が判定される。これにより、より安定的に移動ロボットの現在位置・姿勢及び各計測点の形状を推定できるようになる。

なお、上記の処理機能は、コンピュータによって実現することができる。その場合、環境地図生成装置が有すべき機能の処理内容を記述したプログラムが提供される。そのプログラムをコンピュータで実行することにより、上記処理機能がコンピュータ上で実現される。処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。

プログラムを流通させる場合には、例えば、そのプログラムが記録されたＤＶＤ（Digital Versatile Disc）、ＣＤ−ＲＯＭ（Compact Disc）などの可搬型記録媒体が販売される。また、プログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することもできる。

プログラムを実行するコンピュータは、例えば、可搬型記録媒体に記録されたプログラムもしくはサーバコンピュータから転送されたプログラムを、自己の記憶装置に格納する。そして、コンピュータは、自己の記憶装置からプログラムを読み取り、プログラムに従った処理を実行する。なお、コンピュータは、可搬型記録媒体から直接プログラムを読み取り、そのプログラムに従った処理を実行することもできる。また、コンピュータは、サーバコンピュータからプログラムが転送されるごとに、逐次、受け取ったプログラムに従った処理を実行することもできる。

以上の実施の形態に関し、さらに以下の付記を開示する。
（付記１） 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する処理を実行させる環境地図生成プログラムにおいて、
コンピュータを、
前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段、
前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する現在位置推定手段、
として機能させることを特徴とする環境地図生成プログラム。

（付記２） 前記速度分散を考慮した運動モデルは、前記計測点が速度０、かつ所定の速度誤差の分散を持って動いているとするモデルであることを特徴とする付記１記載の環境地図生成プログラム。

（付記３） 前記コンピュータを、さらに、
前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が静止していたとする静止モデルを適用し、前記計測点が静止計測点である場合の現在の推定位置を予測する静止モデル予測生成手段、
前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が運動していたとする運動モデルを適用し、前記計測点が運動計測点である場合の現在の推定位置を予測する運動モデル予測生成手段、
前記計測点が、前記静止計測点の場合の現在の推定位置と、前記運動計測点の場合の現在の推定位置と、を取得してそれぞれの場合の計測値の予測値を算出し、前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、評価結果に基づいて前記計測点が前記静止計測点または前記運動計測点のいずれであるかを判定する計測点静止／運動判定手段、
として機能させることを特徴とする付記１記載の環境地図生成プログラム。

（付記４） 前記静止モデルは、前記静止計測点の現在の推定位置及び推定誤差の分散が、前記静止計測点の過去の推定位置及び前記推定誤差の分散から変化がないとするモデルであることを特徴とする付記３記載の環境地図生成プログラム。

（付記５） 前記運動モデルは、前記運動計測点が定速度で動いているとするモデルであり、前記運動計測点の現在の推定位置は前記運動計測点の過去の推定位置から前記定速度で移動しており、前記運動計測点の現在の推定誤差の分散には前記運動計測点の過去の推定誤差の分散が付加されていることを特徴とする付記３記載の環境地図生成プログラム。

（付記６） 前記計測点静止／運動判定手段は、前記静止計測点または前記運動計測点の現在の推定位置及び推定誤差の分散と、計測された前記計測点の計測値及び測定誤差分散とを、Ｂｈａｔｔａｃｈａｒｒｙａの評価値を用いて前記統計的一致度を評価する、ことを特徴とする付記３記載の環境地図生成プログラム。

（付記７） 前記計測点静止／運動判定手段は、前記統計的一致度の評価に応じて前記計測点が前記運動計測点であると判定されたときは、前記運動計測点であると判定された前記計測点に対応する前記位置情報を前記記憶手段から削除し、
前記現在位置推定手段は、前記計測点静止／運動判定手段によって前記運動計測点が削除され前記静止計測点のみが残された前記位置情報を参照し、前記位置情報に登録される前記静止計測点の過去の推定位置と、前記計測点の計測値と、を用いて前記移動体の現在位置を再度推定し、前記移動体の現在位置の詳細推定値を算出し、
静止計測点位置更新手段は、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値と、前記計測点の計測値と、前記位置情報に登録される前記静止計測点の過去の推定位置と、に基づいて前記静止計測点の推定位置を更新する、
ことを特徴とする付記３記載の環境地図生成プログラム。

（付記８） 前記計測点静止／運動判定手段は、前記運動計測点が検出されたときは、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値を用いて前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、前記運動計測点に対応する前記位置情報を前記記憶手段から削除する、
ことを特徴とする付記７記載の環境地図生成プログラム。

（付記９） 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する環境地図生成方法において、
速度分散モデル予測手段が、前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する手順と、
現在位置推定手段が、前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する手順と、
を有することを特徴とする環境地図生成方法。

（付記１０） 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて自身の現在位置と前記計測点の位置を推定し、環境地図を生成する移動ロボットにおいて、
前記環境内で過去に推定された前記計測点の推定位置を含む位置情報が格納される記憶手段と、
前記計測点の計測値が取得されると、前記記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段と、
前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて自身の現在位置を推定する現在位置推定手段と、
を有することを特徴とする移動ロボット。

発明の概要を示した図である。 計測点の運動モデルを示した図である。 移動ロボットの制御部のハードウェア構成を示した図である。 移動ロボットの環境地図生成処理部のソフトウェア構成を示した図である。 静止計測点位置情報の一例を示した図である。 位置検出用センサの動作を示した図である。 移動ロボットによる環境地図生成処理の手順を示したフローチャートである。 移動ロボットによる他の環境地図生成処理の手順を示したフローチャートである。 従来のＳＬＡＭ技術を用いた移動ロボットの構成例を示した図である。 従来の運動体検出機能を備えた移動ロボットの構成例を示した図である。

符号の説明

１１ 記憶手段
１２ 速度分散モデル予測生成手段
１３ 第１の現在位置推定手段
１４ 静止モデル予測生成手段
１５ 運動モデル予測生成手段
１６ 計測点静止／運動判定手段
１７ 第２の現在位置推定手段
１８ 静止計測点位置更新手段

Claims (6)

1. 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する処理を実行させる環境地図生成プログラムにおいて、
   コンピュータを、
   前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段、
   前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する現在位置推定手段、
   として機能させることを特徴とする環境地図生成プログラム。
2. 前記コンピュータを、さらに、
   前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が静止していたとする静止モデルを適用し、前記計測点が静止計測点である場合の現在の推定位置を予測する静止モデル予測生成手段、
   前記記憶手段から前記計測点に対応する前記位置情報を抽出し、抽出された前記位置情報に含まれる前記計測点の過去の推定位置に対して前記計測点が運動していたとする運動モデルを適用し、前記計測点が運動計測点である場合の現在の推定位置を予測する運動モデル予測生成手段、
   前記計測点が、前記静止計測点の場合の現在の推定位置と、前記運動計測点の場合の現在の推定位置と、を取得してそれぞれの場合の計測値の予測値を算出し、前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、評価結果に基づいて前記計測点が前記静止計測点または前記運動計測点のいずれであるかを判定する計測点静止／運動判定手段、
   として機能させることを特徴とする請求項１記載の環境地図生成プログラム。
3. 前記計測点静止／運動判定手段は、前記統計的一致度の評価に応じて前記計測点が前記運動計測点であると判定されたときは、前記運動計測点であると判定された前記計測点に対応する前記位置情報を前記記憶手段から削除し、
   前記現在位置推定手段は、前記計測点静止／運動判定手段によって前記運動計測点が削除され前記静止計測点のみが残された前記位置情報を参照し、前記位置情報に登録される前記静止計測点の過去の推定位置と、前記計測点の計測値と、を用いて前記移動体の現在位置を再度推定し、前記移動体の現在位置の詳細推定値を算出し、
   静止計測点位置更新手段は、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値と、前記計測点の計測値と、前記位置情報に登録される前記静止計測点の過去の推定位置と、に基づいて前記静止計測点の推定位置を更新する、
   ことを特徴とする請求項２記載の環境地図生成プログラム。
4. 前記計測点静止／運動判定手段は、前記運動計測点が検出されたときは、前記現在位置推定手段によって再度推定された前記移動体の現在位置の詳細推定値を用いて前記静止計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度、及び前記運動計測点の場合の前記計測値の予測値と、計測された前記計測点の計測値との統計的一致度を評価し、前記運動計測点に対応する前記位置情報を前記記憶手段から削除する、
   ことを特徴とする請求項３記載の環境地図生成プログラム。
5. 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて現在位置と前記計測点の位置を推定し、環境地図を生成する環境地図生成方法において、
   速度分散モデル予測手段が、前記計測点の計測値が取得されると、前記環境内で過去に算出された前記計測点の推定位置を含む位置情報が格納される記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する手順と、
   現在位置推定手段が、前記環境内を自律移動しながら前記計測点の計測を行っている移動体の現在位置を、前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて推定する手順と、
   を有することを特徴とする環境地図生成方法。
6. 所定の環境内を自律移動しながら計測した計測点の計測値に基づいて自身の現在位置と前記計測点の位置を推定し、環境地図を生成する移動ロボットにおいて、
   前記環境内で過去に推定された前記計測点の推定位置を含む位置情報が格納される記憶手段と、
   前記計測点の計測値が取得されると、前記記憶手段から前記計測点に対応する位置情報を抽出し、前記位置情報に含まれる前記計測点の過去の推定位置に対して速度分散を考慮した運動モデルを適用し、前記計測点の過去の推定位置に速度分散を付加した速度分散付推定位置を予測する速度分散モデル予測手段と、
   前記速度分散モデル予測手段によって予測された前記計測点の過去の前記速度分散付推定位置と、前記計測点の計測値と、を用いて自身の現在位置を推定する現在位置推定手段と、
   を有することを特徴とする移動ロボット。

Images