JP5776324B2 - 地図処理方法及びプログラム、並びにロボットシステム - Google Patents

Description

本発明は、地図処理方法及びプログラム、並びにロボットシステムに関する。本発明は、地図処理プログラムを格納したコンピュータ読み取り可能な記憶媒体にも関する。

自律移動型ロボットは、実環境内で障害物、壁などと衝突すること無く自由に移動できるように、例えばロボットに搭載されたセンサの計測情報と環境地図との照合に基づいて自己位置の推定を行う。ロボットが実環境下で自己位置を確実、且つ、正確に推定するために、複数の異なる地図を併用する方式がある。また、環境地図を自動的に作成する技術としては、例えばＳＬＡＭ（Simultaneous Localization And Mapping）が知られている。

複数の地図を自動的に作成する方式は、一般的には以下の２つの方式に分類できる。第１の方式では、ある種類のセンサの計測情報に基づいて最初の地図を作成する。その後、他の種類のセンサの計測情報を最初の地図に足し加えて２番目の地図を作成し、以下同様にして３番目以降の地図を作成する。一方、第２の方式では、複数の異なる種類のセンサの計測情報を推定器に同時観測情報として入力して、複数の地図を同時に、且つ、自動的に作成する。

第１の方式の場合、２番目以降に作成される地図が最初に作成された地図に依存する。このため、２番目以降に作成された地図の精度は、最初に作成された地図の精度に依存してしまう。一方、第２の方式の場合、センサの種類が多くなると、推定器の観測空間が大きくなるため、推定器の計算量が多くなると共に計算時間がかかる。また、異なる種類のセンサは異なる計測レートを用いるため、使用するセンサの計測レートを最も遅い計測レートに合わせる必要がある。

しかし、複数の地図を同時に生成する場合、異なる種類のセンサの異なる誤差モデルを反映しながら各センサ独自の計測レートを用いることが望ましい。このように、独立して作成された複数の地図では、各地図の座標系が異なると共に、推定誤差によって夫々の地図が異なる歪みを含んでいる。このため、ロボットが自己位置の推定を行う段階では、各センサの計測情報と対応する地図とのマッチングにより推定された自己位置が、異なる座標系で表現されたものであるため、例えばロボットの移動経路を決定する経路計画（または、経路プランニング）などを行うためには推定した自己位置の座標系を統合（または、統一）する必要がある。

従来、例えばグリッドマップで表現可能な環境地図については、地図間で相互変換を行う座標補正方法が提案されている（例えば、特許文献１）。しかし、この提案された座標補正方法のように地図間で線形変換を行っても、各地図には歪み、すなわち、非線形成分が含まれている。このため、複数の地図が同一の座標系を共有するように地図間で線形変換を行っても、高精度にこれら複数の地図の座標系を統合することは難しい。

特開２００９−１５７４３０号公報 特開平１１−２８２３４３号公報

C. E. Rasmussen, C. K. I. Williams, "Gaussian Process for Machine Learning", The MIT Press, 2006

従来、独立して作成された複数の地図の座標系が異なる場合、高精度にこれら複数の地図の座標系を統合することは難しいという問題があった。

そこで、本発明は、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することが可能な地図処理方法及びプログラム、並びにロボットシステムを提供することを目的とする。

本発明の一観点によれば、自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定処理と、前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定処理と、同じ時刻に合わせて計算された、前記第１の地図上の前記自己位置から推定された前記ロボットの前記第１の地図上の第１の移動軌跡上の座標点と、前記第２の地図上の前記自己位置から推定された前記ロボットの前記第２の地図上の第２の移動軌跡上の点座標とのペアから予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１の推定処理により推定された前記ロボットの前記第１の地図上の前記自己位置を前記第２の地図上の前記自己位置に変換する変換処理をコンピュータに実行させる地図処理方法が提供される。

本発明の一観点によれば、コンピュータに地図を処理させるプログラムであって、自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定手順と、前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定手順と、同じ時刻に合わせて計算された、前記第１の地図上の前記自己位置から推定された前記ロボットの前記第１の地図上の第１の移動軌跡上の点座標と、前記第２の地図上の前記自己位置から推定された前記ロボットの前記第２の地図上の第２の移動軌跡上の点座標とのペアから予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１の推定手順により推定された前記ロボットの前記第１の地図上の前記自己位置を前記第２の地図上の前記自己位置に変換する変換手順を前記コンピュータに実行させるプログラムが提供される。

本発明の一観点によれば、自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定器と、前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定器と、同じ時刻に合わせて計算された、前記第１の地図上の前記自己位置から推定された前記ロボットの前記第１の地図上の第１の移動軌跡上の点座標と、前記第２の地図上の前記自己位置から推定された前記ロボットの前記第２の地図上の第２の移動軌跡上の点座標とのペアから予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１の推定器により推定された前記ロボットの前記第１の地図上の前記自己位置を前記第２の地図上の前記自己位置に変換する座標変換部を備えたロボットシステムが提供される。

開示の地図処理方法及びプログラム、並びにロボットシステムによれば、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することができる。

本発明の一実施例におけるロボットシステムの一例を示すブロック図である。 ロボットの位置の共分散の変換を説明する図である。 ロボットの姿勢の変換を説明する図である。 地図生成部及び座標変換パラメータ算出部の処理の一例を説明するフローチャートである。 本発明の一実施例におけるロボットシステムの他の例を示すブロック図である。 統合部の統合処理の一例を説明する図である。 自己位置推定処理の一例を説明するフローチャートである。 非線形マッピングを説明する図である。 ロボットの一例を示す側面図である。 センサの計測範囲を説明する図である。 センサの計測情報に基づき作成した地図の一例を示す図である。 作成した地図上のロボットの移動軌跡と、基準地図へ変換後の移動軌跡を説明する図である。 推定経路の誤差を説明する図である。

開示の地図処理方法及びプログラム、並びにロボットシステムでは、自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成された第１の地図と、ロボットの各位置での第１のセンサの計測情報とに基づいてロボットの第１の地図上の自己位置を推定するし、ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成された第２の地図と、ロボットの各位置での第２のセンサの計測情報とに基づいてロボットの第２の地図上の自己位置を推定する。第１のセンサの計測情報に基づき予め推定されたロボットの第１の地図上の第１の移動軌跡と第２のセンサの計測情報に基づき予め推定されたロボットの第２の地図上の第２の移動軌跡とに基づいて予め算出された、第１の移動軌跡の第１の座標系を第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、第１のセンサの計測情報に基づいて推定されたロボットの第１の地図上の自己位置を第２の地図上の自己位置に変換する。

以下に、開示の地図処理方法及びプログラム、並びにロボットシステムの各実施例を図面と共に説明する。

図１は、本発明の一実施例におけるロボットシステムの一例を示すブロック図である。この例では、本発明がロボットを含むロボットシステムに適用されている。ロボットは、例えばモータなどにより駆動される台車、各種内的センサ及び外的センサ、及びプロセッサなどにより形成された制御部を有する周知の基本構成を有する自律走行型ロボットで形成可能である。ロボットシステム１は、センサ部２、ＳＬＡＭ部３、及び地図間マッピング部４を有する。

センサ部２は、オドメトリ（Odometry）部２１及び異なる種類のセンサ２２Ａ，２２Ｂ，２２Ｃを有する。ＳＬＡＭ部３は、地図生成部３１Ａ，３１Ｂ，３１Ｃ及び地図Ａ，Ｂ，Ｃを記憶する記憶部３２Ａ，３２Ｂ，３２Ｃを有する。地図間マッピング部４は、移動軌跡保持部４１Ａ，４１Ｂ，４１Ｃ、座標変換パラメータ算出部４２，４３、及び変換パラメータを記憶する記憶部４４，４５を有する。

ロボットシステムがロボットで形成されている場合、センサ部２、ＳＬＡＭ部３、及び地図間マッピング部４はロボットに設けられる。一方、ロボットシステムがロボット及び当該ロボットを制御する当該ロボットとは別体の情報処理装置で形成されている場合、ＳＬＡＭ部３を情報処理装置に設けても、ＳＬＡＭ部３及び地図間マッピング部４を情報処理装置に設けても良い。情報処理装置は、ロボットと通信可能な通信部（または、インタフェース）、ＣＰＵなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。プログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。

先ず、地図作成処理について説明する。図１において、環境５内でロボット６を例えば手動で移動して、環境情報を収集して地図を作成する。この例では、異なる複数の地図を作成すると共に、移動軌跡も利用して地図座標系間の変換も行う。

オドメトリ部２１は、ロボット６の移動量を、台車６−１の車輪６−２の回転を検出するエンコーダ（図示せず）、または、ロボット６に設けられたジャイロセンサ（または、ジャイロスコープ、図示せず）などの内的センサからの検出信号に基づいて計算する周知の構成を有する。センサ２２Ａは、例えばレーザレンジファインダ（ＬＲＦ：Laser Range Finder）、カメラなどで形成されて環境計測を行う。センサ２２Ｂは、例えばセンサ２２Ａとは異なる位置、及び／または、向きで取り付けられたＬＲＦ、カメラ等で形成されて環境計測を行う。センサ２２Ｃは、例えばセンサ２２Ａ，２２Ｂとは異なる位置、及び／または、向きで取り付けられたＬＲＦ、カメラ等で形成されて環境計測を行う。この例では、センサ２２Ａ，２２Ｂは互いに異なる位置、及び／または、向きで取り付けられたＬＲＦで形成され、センサ２２Ｃはカメラで形成されており、センサ２２Ａ，２２Ｂ，２２Ｃはいずれも外的センサを形成する。

地図生成部３１Ａは、ロボット６が移動している時に、センサ２２Ａが収集した時系列の環境データを、オドメトリ部２１が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップＡを生成し、記憶部３２Ａに記憶する。同様にして、地図生成部３１Ｂは、ロボット６が移動している時に、センサ２２Ｂが収集した時系列の環境データを、オドメトリ部２１が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップＢを生成し、記憶部３２Ｂに記憶する。また、地図生成部３１Ｃは、ロボット６が移動している時に、センサ２２Ｃが収集した時系列の環境データを、オドメトリ部２１が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップＣを生成し、記憶部３２Ｃに記憶する。

また、地図生成部３１Ａは、生成した地図Ａに基づいて、オドメトリ部２１が検出したオドメトリ情報のみに基づいて推定したロボット６の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差（または、オドメトリ誤差）をキャンセルして、ロボット６の正しい移動軌跡ＭＬ_Ａを推定して移動軌跡保持部４１Ａに保持する。同様にして、地図生成部３１Ｂは、生成した地図Ｂに基づいて、オドメトリ部２１が検出したオドメトリ情報のみに基づいて推定したロボット６の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差をキャンセルして、ロボット６の正しい移動軌跡ＭＬ_Ｂを推定して移動軌跡保持部４１Ｂにする。さらに、地図生成部３１Ｃは、生成した地図Ｃに基づいて、オドメトリ部２１が検出したオドメトリ情報のみに基づいて推定したロボット６の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差をキャンセルして、ロボット６の正しい移動軌跡ＭＬ_Ｃを推定して移動軌跡生成部４１Ｃに保持する。地図生成部３１Ａ，３１Ｂ，３１Ｃがロボット６の正しい移動軌跡を推定する際には、周知の移動軌跡推定方法を利用可能である。この例では、移動軌跡推定方法にファスト（Fast）ＳＬＡＭという方法を利用するものとする。移動軌跡保持部４１Ａ，４１Ｂ，４１Ｃは、メモリなどの記憶部で形成可能である。

座標変換パラメータ算出部４２は、移動軌跡保持部４１Ａに保持されたロボット６の移動軌跡の座標系Ｃ_Ａを移動軌跡保持部４１Ｂに保持されたロボット６の移動軌跡の座標系Ｃ_Ｂへマッピングするパラメータ（以下、座標変換パラメータとも言う）を算出し、記憶部４４に記憶する。また、座標変換パラメータ算出部４３は、移動軌跡保持部４１Ｃに保持されたロボット６の移動軌跡の座標系Ｃ_Ｃを移動軌跡保持部４１Ｂに保持されたロボット６の移動軌跡の座標系Ｃ_Ｂへマッピングする座標変換パラメータを算出し、記憶部４５に記憶する。この例では、位置座標の変換式にＧＰＲ（Gaussian Process Regression）を利用してマッピングを行う。

地図生成時に推定する移動軌跡は、次のように表すことができる。Ｘは、地図Ａの作成時に地図生成部３１Ａが推定により生成する移動軌跡を示し、Ｙは地図Ｂの作成時に地図生成部３１Ｂが推定により生成する移動軌跡を示す。

なお、ＧＰＲに必要なパラメータσ_{1, ..., 4}は、例えば非特許文献１にて提案されている方法で算出可能である。

図２は、ロボット６の位置の共分散の変換、すなわち、アンセンテッド変換（Unscented Transformation）を説明する図である。図２において、Ｏは地図Ａで推定したロボット６の位置、Σは地図Ａで推定したロボット６の位置の共分散行列、Ｅ，Ｆ，Ｇ，Ｈ，Ｏはアンセンテッド変換により算出されるシグマポイント、Ｅ'，Ｆ'，Ｇ'，Ｈ'，Ｏ'は地図Ｂの座標系Ｃ_Ｂに変換した後の座標、Σ'は地図Ｂの座標系Ｃ_Ｂに変換した後のロボット６の位置の共分散行列を表す。アンセンテッド変換によるシグマポイントの算出、及び、地図Ｂの座標系Ｃ_Ｂに変換した後のロボット６の位置の共分散行列については、例えば非特許文献１に記載されている。

このようにして、地図Ａで推定した位置共分散行列からシグマポイントＥ，Ｆ，Ｇ，Ｈ，Ｏを求める。次に、各シグマポイントＥ，Ｆ，Ｇ，Ｈ，Ｏを地図Ｂの座標系Ｃ_Ｂに変換した後の座標Ｅ'，Ｆ'，Ｇ'，Ｈ'，Ｏ'を求める。そして、変換後のシグマポイントＥ'，Ｆ'，Ｇ'，Ｈ'，Ｏ'から地図Ｂの座標系Ｃ_Ｂに変換した後のロボット６の位置の共分散行列を求める。

図３は、ロボット６の姿勢の変換を説明する図である。図３中、（ａ）は地図Ａ上で推定したロボット６の位置Ｐ（ｘ_ｐ，ｙ_ｐ）を示す。θは、地図Ａ上で推定したロボット６の向きを示す。この場合、微小移動先Ｑ（ｘ_ｑ，ｙ_ｑ）を次式に基づいて算出する。

ｘ_ｑ＝ｘ_ｐ＋ｄｃｏｓ（θ）
ｙ_ｑ＝ｙ_ｐ＋ｄｓｉｎ（θ）
次に、地図Ａ上のＰ，Ｑを、地図Ｂの座標系Ｃ_Ｂ上のＰ'（ｘ'_ｐ，ｙ'_ｐ），Ｑ'（ｘ'_ｑ，ｙ'_ｑ）を求める。そして、地図Ｂ上でのロボット６の姿勢θ'＝ arctan（ｙ'_ｑ−ｙ'_ｐ，ｘ'_ｑ−ｘ'_ｐ）を求める。

姿勢分散の変換は、次のように求めることができる。先ず、姿勢推定の分散Σ_θからシグマポイントθ_０，θ_１，θ_２を計算する。次に、シグマポイントθ_０，θ_１，θ_２を地図Ｂの座標系Ｃ_Ｂ上のシグマポイントθ'_０，θ'_１，θ'_２に変換する。そして、変換されたシグマポイントθ'_０，θ'_１，θ'_２から地図Ｂ上のロボット６の姿勢の分散Σ_θ'を求める。

図４は、地図生成部３１Ａ，３１Ｂ及び座標変換パラメータ算出部４２の処理の一例を説明するフローチャートである。図４において、地図生成部３１Ａの処理ＳＴ１、地図生成部３１Ｂの処理ＳＴ２、及び座標変換パラメータ算出部４２の処理ＳＴ３は、互いに独立したタスクで形成可能である。

図４において、地図生成部３１Ａの処理ＳＴ１が開始されると、ステップＳ１は、地図生成部３１Ａの推定器が持つロボット６の現在位置の座標と姿勢のデータを初期化すると共に、ロボット６の目的地点（または、ゴール地点）の座標を初期化する。この例では、説明の便宜上、初期位置の座標と姿勢を[xA1, yA1, θA1]、ゴール地点を[xG, yG]で表すものとする。ステップＳ２は、ロボット６の現在位置がゴール地点であるか否かを判断する。ステップ２の判断結果がＮＯであれば処理はステップＳ３へ進み、判断結果がＹＥＳであれば地図生成部３１Ａの処理ＳＴ１の完了フラグを座標変換パラメータ算出部４２へ通知し、処理は後述する座標変換パラメータ算出部４２の処理ＳＴ３のステップＳ３１へ進む。ステップＳ２の判断結果に応じて、ロボット６の現在位置がゴール地点であるか否かを示すフラグを記憶しても良い。

ステップＳ３は、オドメトリ部２１からロボット６の現在位置を示すオドメトリ情報を入力する。オドメトリ部２１が出力するオドメトリ情報には、エンコーダやジャイロセンサなどの内的なセンサのみで推定したロボットの位置と姿勢のデータが含まれる。ロボット６は、車輪の機械的的誤差、床上での車輪のスリップなどによって推定誤差が徐々に拡大するが、この例では位置の差分のみに着目する。ステップＳ３が入力するオドメトリ情報に含まれる位置と姿勢のデータ[xOt, yOt]のうち、ｔはステップＳ２〜Ｓ６のループの実行回数を示す。

ステップＳ４は、ステップＳ３で入力したオドメトリ情報（位置と姿勢のデータ）と、前回入力したオドメトリ情報（位置と姿勢のデータ）との差分を算出し、推定器が前回推定したオドメトリ情報（位置と姿勢のデータ）にこの差分を足してオドメトリ情報（位置と姿勢のデータ）を更新することで推定器の内部状態を予測する。ステップＳ５は、センサ２２Ａからの計測データ（すなわち、環境データ）を入力する。この例ではセンサ２２ＡはＬＲＦであるため、センサ２２Ａからの計測データはロボット６（すなわち、ＬＲＦ）から壁までの計測距離を示す。ステップＳ６は、センサ２２Ａからの計測データに基づいて生成され記憶部３２Ａに記憶された地図Ａと、センサ２２Ａで計測された計測データとに基づきロボット６の現在位置と姿勢のデータ[xAt, yAt]を推定し、ロボット６の位置と姿勢のデータをこのデータ[xAt, yAt]に更新する。地図Ａには、壁やランドマークなどの位置のデータも含まれる。

ステップＳ７は、ステップＳ６で推定された現在位置と姿勢のデータ[xAt, yAt]のうち、現在位置の推定値を抽出する。ステップＳ７の後、処理はステップＳ８及びステップＳ９へ並行して進む。ステップＳ８は、ステップＳ７で抽出した現在位置の推定値に基づいて、ステップＳ５で取得した環境データを地図Ａに反映させることで地図Ａの追加更新を行い、追加更新された地図Ａを記憶部３２Ａに記憶する。一方、ステップＳ９は、ステップＳ７で抽出した現在位置の推定値を例えば地図生成部３１Ａ内の記憶部に記憶することで保存する。この記憶部は、移動軌跡保持部４１Ａを形成する記憶部であっても良い。ステップＳ１０は、ステップＳ６で推定されたロボット６の現在位置と姿勢のデータ[xAt, yAt]を時系列的に移動軌跡保持部４１Ａに保持する。これにより、データ[xA1, yA1], [xA2, yA2],…, [xAt, yAt],…, [xAT, yAT]が移動軌跡ＭＬ_Ａとして保持され、処理は後述する座標変換パラメータ算出部４２の処理ＳＴ３のステップＳ３３へ進む。

一方、地図生成部３１Ｂの処理ＳＴ２が開始されると、ステップＳ１１〜Ｓ２０がステップＳ１〜Ｓ１０と同様の処理をセンサ２２Ｂに対して行う。ステップＳ１２の判断結果がＹＥＳであれば地図生成部３１Ｂの処理ＳＴ２の完了フラグを座標変換パラメータ算出部４２へ通知し、処理は後述する座標変換パラメータ算出部４２の処理ＳＴ３のステップＳ３１へ進む。ステップＳ２０の後、処理は後述する座標変換パラメータ算出部４２の処理ＳＴ３のステップＳ３４へ進む。

座標変換パラメータ算出部４２の処理ＳＴ３が開始されると、ステップＳ３１は、両地図作成完了待ち状態となる。ステップＳ３２は、地図生成部３１Ａによる地図Ａの生成と、地図生成部３１Ｂによる地図Ｂの生成が完了しているか否かを判断する。具体的には、地図生成部３１Ａの処理ＳＴ１から完了フラグが通知され、且つ、地図生成部３１Ｂの処理ＳＴ２から完了フラグが通知されたか否かを判断する。ステップＳ３２の判断結果がＮＯであると処理はステップＳ３１へ戻り、判断結果がＹＥＳであると処理はステップＳ３３へ進む。

ステップＳ３３は、記憶部（移動軌跡保持部４１Ａ）から推定されたロボット６の現在位置と姿勢のデータ [xA1, yA1], [xA2, yA2],…, [xAt, yAt],…, [xAT, yAT]を読み込み、ロボット６の移動軌跡ＭＬ_Ａを取得する。ステップＳ３４は、記憶部（移動軌跡保持部４１Ｂ）から推定されたロボット６の現在位置と姿勢のデータ [xB1, yB1], [xB2, yB2],…, [xBt, yBt],…, [xBT, yBT]を読み込み、ロボット６の移動軌跡ＭＬ_Ｂを取得する。

ステップＳ３５は、移動軌跡ＭＬ_Ｂ上の各ポイントが対応する移動軌跡ＭＬ_Ａ上のポイントを計算する。具体的には、同じ時刻に合わせて、移動軌跡ＭＬ_Ａ上の点座標と移動軌跡ＭＬ_Ｂ上の点座標のペア、例えば(t1:[xA1, yA1]→ [xB1, yB1], t2:[xA2, yA2] →[xB2, yB2],…, tT:[xAT, yAT] →[xBT, yBT])を計算する。ステップＳ３６は、ステップＳ３５で計算した移動軌跡ＭＬ_Ａ，ＭＬ_Ｂ上の点座標のペアに基づいて、ＧＰＲパラメータを最適化する。具体的には、パラメータσ1〜σ４と座標点のペア列t1:[xA1, yA1] →[xB1, yB1], t2:[xA2, yA2] →[xB2, yB2],…, tT:[xAT, yAT] →[xBT, yBT]が得られ、これらは地図Ａから地図Ｂへの変換パラメータとして記憶部４４に記憶される。

なお、地図生成部３１Ｃ，３１Ｂ及び座標変換パラメータ算出部４３の処理は、図４の処理と同様に行えるので、その図示及び説明は省略する。

センサ２２Ａ，２２Ｂ，２２Ｃで計測した環境データと、上記の如く作成した地図Ａ，Ｂ，Ｃとを照合して、各地図Ａ，Ｂ，Ｃにおけるロボット６の位置座標と向き角度を計算することができる。しかし、各センサ２２Ａ，２２Ｂ，２２Ｃで推定したロボット６の位置と姿勢を統合する処理を行うためには、異なる地図座標系で表した位置座標と姿勢をある基準座標系に変換する必要がある。本実施例では、以下のような処理により必要な座標変換を行う。なお、以下の説明では、地図Ｂを基準地図とするので、地図Ｂの座標系が基準座標系となる。

図５は、本発明の一実施例におけるロボットシステムの他の例を示すブロック図である。図５中、図１と同一部分には同一符号を付し、その説明は省略する。この例においても、本発明がロボットを含むロボットシステムに適用されている。ロボットは、例えばモータなどにより駆動される台車、環境計測を行う各種センサ、及びプロセッサなどにより形成された制御部を有する周知の基本構成を有する自律走行型ロボットで形成可能である。ロボットシステム１１は、センサ部２、自己位置推定部７、及び統合処理部８を有する。

ロボットシステムがロボットで形成されている場合、センサ部２、自己位置推定部７、及び統合処理部８はロボットに設けられる。一方、ロボットシステムがロボット及び当該ロボットを制御する当該ロボットとは別体の情報処理装置で形成されている場合、自己位置推定部７を情報処理装置に設けても、自己位置推定部７及び統合処理部８を情報処理装置に設けても良い。情報処理装置は、ロボットと通信可能な通信部、ＣＰＵなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。プログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。

自己位置推定部７は、自己位置推定器７１Ａ，７１Ｂ，７１Ｃと、記憶部３２Ａ，３２Ｂ，３２Ｃを有する。統合処理部８は、座標変換部８１，８２、記憶部４４，４５、及び統合部８３を有する。

自己位置推定部７において、自己位置推定器７１Ａは、センサ２２Ａからの計測情報と、オドメトリ部２１からのオドメトリ情報と、記憶部３２Ａから読み出した地図Ａに基づいて周知の自己位置推定を行う。自己位置推定器７１Ａの自己位置推定により推定された自己位置Ｐ_Ａは、記憶部３２Ａから読み出した地図Ａと、センサ２２Ａからの計測情報に基づいて推定した地図Ａとを照合することで、地図Ａの座標系におけるロボット６の尤もらしい現在位置と姿勢を表す。自己位置推定器７１Ｂは、センサ２２Ｂからの計測情報と、オドメトリ部２１からのオドメトリ情報と、記憶部３２Ｂから読み出した地図Ｂに基づいて周知の自己位置推定を行う。自己位置推定器７１Ｂの自己位置推定により推定された自己位置Ｐ_Ｂは、記憶部３２Ｂから読み出した地図Ｂと、センサ２２Ｂからの計測情報に基づいて推定した地図Ｂとを照合することで、地図Ｂの座標系におけるロボット６の尤もらしい現在位置と姿勢を表す。自己位置推定器７１Ｃは、センサ２２Ｃからの計測情報と、オドメトリ部２１からのオドメトリ情報と、記憶部３２Ｃから読み出した地図Ｃに基づいて周知の自己位置推定を行う。自己位置推定器７１Ｃの自己位置推定により推定された自己位置Ｐ_Ｃは、記憶部３２Ｃから読み出した地図Ｃと、センサ２２Ｃからの計測情報に基づいて推定した地図Ｃとを照合することで、地図Ｃの座標系におけるロボット６の尤もらしい現在位置と姿勢を表す。記憶部３２Ａ〜３２Ｃに記憶された地図Ａ〜Ｃは、図１の地図生成部３１Ａ〜３１Ｃで予め生成されたものである。

統合処理部８において、座標変換部８１は、自己位置推定器７１Ａで推定した地図Ａの座標系でのロボット６の自己位置Ｐ_Ａを、記憶部４４から読み出した地図Ａから地図Ｂへの座標変換パラメータを用いて地図Ｂの座標系（すなわち、基準座標系）の自己位置Ｐ_Ａ'へ変換する。座標変換部８２は、自己位置推定器７１Ｃで推定した地図Ｃの座標系でのロボット６の自己位置Ｐ_Ｃを、記憶部４５から読み出した地図Ｃから地図Ｂへの座標変換パラメータを用いて地図Ｂの座標系（すなわち、基準座標系）の自己位置Ｐ_Ｃ'へ変換する。記憶部４４，４５に記憶された変換パラメータは、図１の座標変換パラメータ算出部４２，４３により予め算出されたものである。

統合処理部８３は、座標変換部８１，８２で得られた地図Ｂの座標系の自己位置Ｐ_Ａ'，Ｐ_Ｃ'及び自己位置推定器７１Ｂで得られた地図Ｂの座標系の自己位置Ｐ_Ｂを統合する統合処理を行い、統合結果を例えばロボット６の移動経路を決定する経路計画部８４などの処理部に供給する。

図６は、統合部８３の統合処理の一例を説明する図である。図６中、図５と同一部分には同一符号を付し、その説明は省略する。この例では、説明の簡略化のため基準座標系の自己位置Ｐ_Ａ'と自己位置Ｐ_Ｂの統合を例にとるが、自己位置Ｐ_Ｃ'と自己位置Ｐ_Ｂの統合も同様に行えば良い。

変換後の自己位置Ｐ_Ａ'は位置姿勢ｘ_Ａと地図Ａで推定したロボット６の位置の共分散行列Σ_Ａで表され、自己位置Ｐ_Ｂは位置姿勢ｘ_Ｂと地図Ｂで推定したロボット６の位置の共分散行列Σ_Ｂで表されるものとすると、統合部８３での統合処理により統合される共分散行列は例えば

図７は、自己位置推定処理の一例を説明するフローチャートである。図７において、自己位置推定器７１Ａの処理ＳＴ１１、自己位置推定器７１Ｂの処理ＳＴ１２、座標変換部８１の処理ＳＴ１３、及び統合部８３の処理ＳＴ１４は、互いに独立したタスクで形成可能である。地図Ｂを基準地図とするので、地図Ａの座標系で推定したロボット６の位置を地図Ｂの座標系に変換して、地図Ｂの座標系で推定した位置と統合する。同様にして、地図Ｃの座標系で推定したロボット６の位置は、地図Ｂの座標系に変換して、地図Ｂの座標系で推定した位置と統合すれば良い。

図７において、自己位置推定器７１Ａの処理ＳＴ１１が開始されると、ステップＳ５０は、地図Ａによる自己位置推定器７１Ａの持つロボット６の初期位置姿勢[xA1, yA1, θA1]を初期化する。ステップＳ５１は、タスクを終了するか否かを判定する。例えば、ロボット６が目的地点（または、ゴール地点）着いて自己推定位置と設定された目的地点が一致した場合、或いは、ユーザからの終了指示があれば、ステップＳ５１の判定結果はＹＥＳとなり、処理は終了する。一方、ステップＳ５１の判定結果がＮＯであると、処理はステップＳ５２へ進む。ステップＳ５２は、オドメトリ部２１からオドメトリ情報を入力することでロボット６の現在位置を得る。ステップＳ５２が入力するオドメトリ情報に含まれる位置と姿勢のデータ[xOt, yOt]のうち、ｔはステップＳ５１〜Ｓ５５のループの実行回数を示す。

ステップＳ５３は、ステップＳ５２で入力したオドメトリ情報と、前回入力したオドメトリ情報との差分を算出し、自己位置推定器７１Ａが前回推定した位置と姿勢のデータにこの差分を足して位置と姿勢のデータを更新することで自己位置推定器７１Ａの内部状態を予測する。ステップＳ５４は、センサ２２Ａからの計測データを入力する。ステップＳ５５は、センサ２２Ａからの計測データに基づいて生成され記憶部３２Ａに記憶された地図Ａと、センサ２２Ａで計測された計測データとに基づきロボット６の現在位置と姿勢のデータ[xAt, yAt]を推定し、ロボット６の位置と姿勢のデータをこのデータ[xAt, yAt]に更新する。ステップＳ５６は、ステップＳ５５で推定された現在位置と姿勢のデータ[xAt, yAt]のうち、現在位置の推定値、すなわち、推定された自己位置Ｐ_Ａを抽出し、処理は座標変換部８１の処理ＳＴ１３へ進む。

一方、自己位置推定器７１Ｂの処理ＳＴ１２が開始されると、ステップＳ７０〜Ｓ７６がステップＳ５０〜Ｓ５６と同様の処理をセンサ２２Ｂに対して行う。ステップＳ７６の後、ステップＳ７７は、推定結果である、自己位置推定器７１Ｂで推定した地図Ｂの座標系でのロボット６の自己位置Ｐ_Ｂを統合部８３へ通知し、処理は統合部８３の処理ＳＴ１４へ進む。

座標変換部８１の処理ＳＴ１３において、ステップＳ６１は、自己位置推定器７１Ａで推定した地図Ａの座標系でのロボット６の自己位置Ｐ_Ａを、記憶部４４から読み出した地図Ａから地図Ｂへの座標変換パラメータを用いて地図Ｂの座標系（すなわち、基準座標系）の自己位置Ｐ_Ａ'へ変換する。変換後の自己位置Ｐ_Ａ'は、位置姿勢ｘ_Ａと地図Ａで推定したロボット６の位置の共分散行列Σ_Ａで表される。ステップＳ６２は、ステップＳ６１の座標変換により求めた自己位置Ｐ_Ａ'、すなわち、位置姿勢ｘ_Ａと共分散行列Σ_Ａを例えば座標変換部８１内の記憶部に保存する。ステップＳ６３は、推定結果である変換後の自己位置Ｐ_Ａ'を統合部８３へ通知する。

統合部８３の処理ＳＴ１４において、ステップＳ８１は、推定結果の通知を待ち、ステップＳ６３またはステップＳ７７から推定結果の通知があれば例えば統合部８３内の記憶部（例えば、共有メモリ）に記憶する。また、ステップＳ８１は、推定結果を共有メモリに記憶すると、共有メモリの更新があることを示す更新フラグを設定する。ステップＳ８２は、更新フラグが設定されているか否かを判定することで、推定結果が新たに通知されたか否かを判定する。ステップＳ８２の判定結果がＮＯであると、処理はステップＳ８１へ戻る。一方、ステップＳ８２の判定結果がＹＥＳであると、ステップＳ８３は、新たに通知された推定結果について上記の如き統合処理を行い、統合処理の結果を例えば統合部８３内の記憶部に記憶する。また、必要に応じてステップＳ８４を行っても良い。ステップＳ８４は、統合処理の結果を経路計画部８４などの処理部に供給する。

このように、本実施例では、ロボットを実環境下で移動させ、ロボットに取り付けられている複数の異なるセンサで計測した環境情報に基づいて複数の地図を独立して作成した後、各地図に対応してロボットの移動軌跡を推定する。推定したロボットの複数の移動軌跡を使って、複数の地図座標系間の非線形変換パラメータ、非線形座標変換式を求める。つまり、地図座標系間の変換は非線形変換方式によって行い、変換に必要なパラメータは独立して推定した移動軌跡から求めて例えばデータベースとして保存できる。独立して作成した環境地図の座標系は独立しており、座標系間の変換は非線形マッピングによって行う。

図８は、上記の非線形マッピングを説明する図である。図８（ａ）は、ロボット６が実環境下で位置Ｓから位置Ｇへ移動する場合を示す。ロボット６が移動中に、センサ２２Ａが環境情報ＳＡを収集し、センサ２２Ｂ環境情報ＳＢを収集するものとする。この場合、環境情報ＳＡに基づいて図８（ｂ）に示す環境地図Ａを作成し、環境情報ＳＢに基づいて環境地図Ｂを作成する。同時に、ロボット６の環境地図Ａ上の移動軌跡ＭＬ_Ａと、環境地図Ｂ上の移動軌跡ＭＬ_Ｂを推定する。そして、移動軌跡ＭＬ_Ａ，ＭＬ_Ｂを用いて、座標系XaOaYaから座標系XbObYbへの座標変換パラメータΘを求める。つまり、座標変換パラメータに、推定した移動軌跡ＭＬ_Ａ，ＭＬ_Ｂを用いる。

非線形変換方式は特に限定されないが、ガウスプロセスを利用することもできる。ガウスプロセスパラメータの最適化を行う方法自体は周知であり、特に限定されない。

座標変換式には、例えばＧＰＲを使用する。ＧＰＲで必要とするカーネル関数は、以下のようにサブカーネル関数の組み合わせからなるものでも良い。

カーネル関数＝ＲＢＦカーネル関数＋線形カーネル関数＋バイアスカーネル関数
また、ロボットの位置姿勢を以下の（１）〜（４）のように変換する。
（１）上記ＧＰＲを使用してロボット位置の基準座標系への座標変換を行う。
（２）上記アンセンテッド変換を利用してロボット位置の共分散行列を変換する。具体的には、ロボット位置の元の座標系でシグマポイントを求めて、基準座標系への座標変換を行い、変換されたシグマポイントの座標を用いて変換後の共分散行列を計算する。
（３）ロボットの現在位置を変換先の基準座標系へ変換することで、ロボット姿勢を変換する。具体的には、ロボットを微小距離移動させた場所での位置座標を基準座標系に変換し、基準座標系に変換された後の移動前と移動後の２つの位置座標によってarctanを計算して基準座標系におけるロボットの姿勢を求める。
（４）上記アンセンテッド変換を利用してロボット姿勢の分散を変換する。具体的には、ロボットの姿勢のシグマポイントを求めて、各シグマポイントについて基準座標系への座標変換を行い、各シグマポイントの対応する変換先の基準座標系での姿勢からロボット姿勢の分散を計算する。

このように、ロボットの自律移動時の自己位置推定は、各地図で推定したロボットの位置を上記非線形変換方式の変換とパラメータに従って行う。ロボットを実環境下で自律移動をさせるために、ロボットに取り付けられている複数のセンサが独立して環境データを収集し、作成済みの各地図とマッチングすることによりロボットの現在位置の座標を推定する。異なる座標系を持つ複数の地図を用いることにより、異なる座標系でのロボット位置座標が推定できる。異なる位置姿勢の座標間変換は、上記非線形変換方式の変換とパラメータに従って行う。

また、各地図で推定した夫々の姿勢値の変換は、座標系の異なる複数の地図により推定されたロボット姿勢（角度）の間の相互変換を、変換元の座標系でのロボット位置とロボット向きの夫々任意の一点を変換先の基準座標系に変換し、変換後の２点のベクトルから求めることで行う。さらに、各地図で推定した夫々の位置姿勢共分散行列の変換は、座標系の異なる複数の地図により推定されたロボット位置姿勢に関しての共分散行列の間の座標系変換をアンセンデット変換により行う。

次に、上記実施例で求められるロボット６の推定経路を、図９乃至図１３と共に説明する。

図９は、ロボットの一例を示す側面図である。図９に示すロボット６には、センサ２２Ａ，２２Ｂが設けられている。センサ２２Ａの地面（または、床）からの高さ位置は例えば５０ｃｍであり、センサ２２Ｂの地面からの高さ位置は例えば２０ｃｍである。

図１０は、センサの計測範囲を説明する図である。図１０中、（ａ）はロボット６とセンサ２２Ａの図９中の矢印で示す線に沿った断面の平面図を示し、破線で囲まれた範囲はセンサ２２Ａの計測範囲を示す。同様に、図１０中、（ｂ）はロボット６とセンサ２２Ｂの図９中の矢印で示す線に沿った断面の平面図を示し、破線で囲まれた範囲はセンサ２２Ｂの計測範囲を示す。

図１１は、センサの計測情報に基づき作成した地図の一例を示す図である。図１１中、（ａ）はセンサ２２Ａの計測情報に基づいて図１の地図生成部３１Ａが作成した地図Ａの一例を示し、ＭＬ_Ａは地図生成部３１Ａがセンサ２２Ａの計測情報を収集することで推定されるロボット６の移動軌跡の一例を示す。同様に、図１１中、（ｂ）はセンサ２２Ｂの計測情報に基づいて図１の地図生成部３１Ｂが作成した地図Ｂの一例を示し、ＭＬ_Ｂは地図生成部３１Ｂがセンサ２２Ｂの計測情報を収集することで推定されるロボット６の移動軌跡の一例を示す。移動軌跡ＭＬ_Ａ，ＭＬ_Ｂは、物理空間内では同じであるが、夫々の地図Ａ，Ｂ上では座標系が異なるため異なる。

ロボット６の１回の移動では、物理的な移動軌跡は１つしかない。異なる地図Ａ，Ｂの場合、推定した移動軌跡ＭＬ_Ａ，ＭＬ_Ｂの座標値の系列が異なる。推定した移動軌跡ＭＬ_Ａ，ＭＬ_Ｂが直接的に地図Ａ，Ｂから由来したものであり、移動軌跡ＭＬ_Ａ，ＭＬ_Ｂ間の座標変換関係を求めれば、地図Ａ、Ｂ間の座標変換を近似することができる。そこで、上記実施例では移動軌跡ＭＬ_Ａ，ＭＬ_Ｂの情報に着眼した座標変換を行う。

なお、地図Ａ，Ｂ全体の回転成分があるだけではなく、局所的に、５００Ａ，５００Ｂで示すような歪みがある。そこで、座標変換を行う際、地図Ａ，Ｂ全体の回転成分に注目するだけではなく、局所的な歪みを補正する。

図１２は、作成した地図上のロボットの移動軌跡と、基準地図へ変換後の移動軌跡を説明する図である。図１２中、（ａ）は図５の自己位置推定器７１Ａにより推定されたロボット６の地図Ａ上の自己位置Ｐ_Ａから推定された移動軌跡ＭＬ_Ａを示し、グリッドは地図Ａの座標系上のグリッドである。これに対し、図１２中、（ｂ）は自己位置推定器７１Ａにより推定されたロボット６の地図Ａ上の自己位置Ｐ_Ａを図５の座標変換部８１により地図Ｂの座標系の自己位置Ｐ_Ａ'に変換し、変換後の自己位置Ｐ_Ａ'から推定されたロボット６の移動軌跡ＭＬ_Ａ'（すなわち、移動軌跡ＭＬ_Ａを地図Ｂの座標系に変換した移動軌跡ＭＬ_Ａ'）を示し、グリッドは地図Ｂの座標系上のグリッドである。

図１３は、推定経路の誤差を説明する図である。図１３中、ＭＬ_Ａは地図Ａを用いて図５の自己位置推定器７１Ａが推定した自己位置Ｐ_Ａから推定されたロボット６の移動軌跡、ＭＬ_Ｂは自己位置推定器７１Ｂが推定した自己位置Ｐ_Ｂから推定されたロボット６の移動軌跡、ＭＬＡ'は自己位置推定器７１Ａにより推定されたロボット６の地図Ａ上の自己位置Ｐ_Ａを座標変換部８１により地図Ｂの座標系の自己位置Ｐ_Ａ'に変換し、変換後の自己位置Ｐ_Ａ'から推定されたロボット６の移動軌跡を示す。

ロボット６が自律移動する際、例えば各地図Ａ，Ｂに基づいて自己位置推定を行う。そこで、地図Ａに基づき推定した移動軌跡ＭＬ_Ａを基準地図Ｂに基づき推定した移動軌跡ＭＬ_Ｂと直接比較した場合、移動軌跡上の各ポイントの位置誤差の平均は０．５２ｍ、標準偏差は０．４２ｍであった。これに対し、地図Ａに基づき推定した移動軌跡ＭＬ_Ａを基準地図Ｂの座標系に変換してから変換後の移動軌跡ＭＬ_Ａ'を基準地図Ｂに基づき推定した移動軌跡ＭＬ_Ｂと比較した場合、移動軌跡上の各ポイントの位置誤差の平均は０．２１ｍ、標準偏差は０．１４ｍであった。これにより、図１３に示す移動軌跡ＭＬ_Ａ，ＭＬ_Ａ'，ＭＬ_Ｂの比較から、上記実施例の如き座標変換処理を行うことにより、独立して作成された複数の地図（この例では地図Ａ，Ｂ）の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することが可能になることが確認された。

以上の実施例を含む実施形態に関し、更に以下の付記を開示する。
（付記１）
自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定処理と、
前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定処理と、
前記第１のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第１の地図上の第１の移動軌跡と前記第２のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第２の地図上の第２の移動軌跡とに基づいて予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１のセンサの計測情報に基づいて前記第１の推定処理により推定された前記ロボットの前記第１の地図上の自己位置を前記第２の地図上の自己位置に変換する変換処理
をコンピュータに実行させることを特徴とする、地図処理方法。
（付記２）
前記第１の推定処理により推定され前記変換処理により変換された前記ロボットの前記第２の地図上の推定自己位置と、前記第２の推定処理により推定された前記ロボットの前記第２の地図上の推定自己位置とを統合する統合処理
を更に前記コンピュータに実行させることを特徴とする、付記１記載の地図処理方法。
（付記３）
前記変換処理は、推定された前記ロボットの位置の前記第１の座標系をＧＰＲ（Gaussian Process Regression）を利用した変換式を用いた前記非線形マッピングにより前記第２の座標系に変換することを特徴とする、付記１または２記載の地図処理方法。
（付記４）
前記変換処理は、前記第１の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換（Unscented Transformation）を利用して行うことを特徴とする、付記１乃至３のいずれか１項記載の地図処理方法。
（付記５）
前記変換処理は、前記ロボットを微小距離移動させた場所での前記第１の座標系の位置座標を前記第２の座標系に変換し、前記第２の座標系に変換後の移動前と移動後の２つの位置座標に基づいて変換後の前記第２の座標系における前記ロボットの姿勢を求めることを特徴とする、付記１乃至４のいずれか１項記載の地図処理方法。
（付記６）
前記変換処理は、前記第１の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換（Unscented Transformation）を利用して行うことを特徴とする、付記５記載の地図処理方法。
（付記７）
コンピュータに地図を処理させるプログラムであって、
自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定手順と、
前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定手順と、
前記第１のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第１の地図上の第１の移動軌跡と前記第２のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第２の地図上の第２の移動軌跡とに基づいて予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１のセンサの計測情報に基づいて前記第１の推定手順により推定された前記ロボットの前記第１の地図上の自己位置を前記第２の地図上の自己位置に変換する変換手順
を前記コンピュータに実行させることを特徴とする、プログラム。
（付記８）
前記第１の推定手順により推定され前記変換手順により変換された前記ロボットの前記第２の地図上の推定自己位置と、前記第２の推定手順により推定された前記ロボットの前記第２の地図上の推定自己位置とを統合する統合手順
を更に前記コンピュータに実行させることを特徴とする、付記７記載のプログラム。
（付記９）
前記変換手順は、推定された前記ロボットの位置の前記第１の座標系をＧＰＲ（Gaussian Process Regression）を利用した変換式を用いた前記非線形マッピングにより前記第２の座標系に変換することを特徴とする、付記７または８記載のプログラム。
（付記１０）
前記変換手順は、前記第１の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換（Unscented Transformation）を利用して行うことを特徴とする、付記７乃至９のいずれか１項記載のプログラム。
（付記１１）
前記変換手順は、前記ロボットを微小距離移動させた場所での前記第１の座標系の位置座標を前記第２の座標系に変換し、前記第２の座標系に変換後の移動前と移動後の２つの位置座標に基づいて変換後の前記第２の座標系における前記ロボットの姿勢を求めることを特徴とする、付記７乃至１０のいずれか１項記載のプログラム。
（付記１２）
前記変換手順は、前記第１の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換（Unscented Transformation）を利用して行うことを特徴とする、付記１１記載のプログラム。
（付記１３）
自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定器と、
前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定器と、
前記第１のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第１の地図上の第１の移動軌跡と前記第２のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第２の地図上の第２の移動軌跡とに基づいて予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１のセンサの計測情報に基づいて前記第１の推定器により推定された前記ロボットの前記第１の地図上の自己位置を前記第２の地図上の自己位置に変換する座標変換部
を備えたことを特徴とする、ロボットシステム。
（付記１４）
前記第１の推定器により推定され前記座標変換部により変換された前記ロボットの前記第２の地図上の推定自己位置と、前記第２の推定器により推定された前記ロボットの前記第２の地図上の推定自己位置とを統合する統合部
を更に備えたことを特徴とする、付記１３記載のロボットシステム。
（付記１５）
前記座標変換部は、推定された前記ロボットの位置の前記第１の座標系をＧＰＲ（Gaussian Process Regression）を利用した変換式を用いた前記非線形マッピングにより前記第２の座標系に変換することを特徴とする、付記１３または１４記載のロボットシステム。
（付記１６）
前記座標変換部は、前記第１の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換（Unscented Transformation）を利用して行うことを特徴とする、付記１３乃至１５のいずれか１項記載のロボットシステム。
（付記１７）
前記座標変換部は、前記ロボットを微小距離移動させた場所での前記第１の座標系の位置座標を前記第２の座標系に変換し、前記第２の座標系に変換後の移動前と移動後の２つの位置座標に基づいて変換後の前記第２の座標系における前記ロボットの姿勢を求めることを特徴とする、付記１３乃至１６のいずれか１項記載のロボットシステム。
（付記１８）
前記座標変換部は、前記第１の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換（Unscented Transformation）を利用して行うことを特徴とする、付記１７記載のロボットシステム。
（付記１９）
前記第１の地図を予め生成して前記記憶部に記憶する第１の地図生成部と、
前記第２の地図を予め生成して前記記憶部に記憶する第２の地図生成部と、
前記座標変換パラメータを予め算出して前記記憶部に記憶するパラメータ算出部
を更に備えたことを特徴とする、付記１３乃至１８のいずれか１項記載のロボットシステム。
（付記２０）
少なくとも前記統合部は、前記ロボットとは別体で前記ロボットと通信可能な情報処理装置に設けらていることを特徴とする、付記１４乃至１８のいずれか１項記載のロボットシステム。

以上、開示の地図処理方法及びプログラム、並びにロボットシステムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。

１ ロボットシステム
２ センサ部
３ ＳＬＡＭ部
４ 地図間マッピング部
７ 自己位置推定部
８ 統合処理部
２１ オドメトリ部
２２Ａ〜２２Ｃ センサ
３１Ａ〜３１Ｃ 地図生成部
３２Ａ〜３２Ｃ 記憶部
７１Ａ〜７１Ｃ 自己位置推定器
８１，８２ 座標変換部
８３ 統合部

Claims (5)

1. コンピュータに地図を処理させるプログラムであって、
   自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定手順と、
   前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定手順と、
   同じ時刻に合わせて計算された、前記第１の地図上の前記自己位置から推定された前記ロボットの前記第１の地図上の第１の移動軌跡上の点座標と、前記第２の地図上の前記自己位置から推定された前記ロボットの前記第２の地図上の第２の移動軌跡上の点座標とのペアから予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１の推定手順により推定された前記ロボットの前記第１の地図上の前記自己位置を前記第２の地図上の前記自己位置に変換する変換手順
   を前記コンピュータに実行させることを特徴とする、プログラム。
2. 前記第１の推定手順により推定され前記変換手順による変換により得られた前記ロボットの前記第２の地図上の前記自己位置と、前記第２の推定手順により推定された前記ロボットの前記第２の地図上の前記自己位置とを統合する統合手順
   を更に前記コンピュータに実行させることを特徴とする、請求項１記載のプログラム。
3. 前記変換手順は、前記第１の推定手順により推定された前記ロボットの前記自己位置の前記第１の座標系を、ＧＰＲ（Gaussian Process Regression）を利用した変換式を用いた前記非線形マッピングにより前記第２の座標系に変換することを特徴とする、請求項１または２記載のプログラム。
4. 自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定器と、
   前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定器と、
   同じ時刻に合わせて計算された、前記第１の地図上の前記自己位置から推定された前記ロボットの前記第１の地図上の第１の移動軌跡上の点座標と、前記第２の地図上の前記自己位置から推定された前記ロボットの前記第２の地図上の第２の移動軌跡上の点座標とのペアから予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１の推定器により推定された前記ロボットの前記第１の地図上の前記自己位置を前記第２の地図上の前記自己位置に変換する座標変換部
   を備えたことを特徴とする、ロボットシステム。
5. 自律移動型ロボットに取り付けられた第１のセンサの計測情報に基づき予め生成され記憶部に記憶された第１の地図と、前記ロボットの各位置での前記第１のセンサの計測情報とに基づいて前記ロボットの前記第１の地図上の自己位置を推定する第１の推定処理と、
   前記ロボットに取り付けられた第２のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第２の地図と、前記ロボットの各位置での前記第２のセンサの計測情報とに基づいて前記ロボットの前記第２の地図上の自己位置を推定する第２の推定処理と、
   同じ時刻に合わせて計算された、前記第１の地図上の前記自己位置から推定された前記ロボットの前記第１の地図上の第１の移動軌跡上の座標点と、前記第２の地図上の前記自己位置から推定された前記ロボットの前記第２の地図上の第２の移動軌跡上の点座標とのペアから予め算出された、前記第１の移動軌跡の第１の座標系を前記第２の移動軌跡の第２の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第１の推定処理により推定された前記ロボットの前記第１の地図上の前記自己位置を前記第２の地図上の前記自己位置に変換する変換処理
   をコンピュータに実行させることを特徴とする、地図処理方法。

Images