地図処理方法及びプログラム、並びにロボットシステム
【課題】地図処理方法及びプログラム、並びにロボットシステムにおいて、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することを目的とする。
【解決手段】ロボットの第1のセンサの計測情報に基づき予め生成された第1の地図と、ロボットの各位置での第1のセンサの計測情報とに基づいて第1の地図上の自己位置を推定し、第2のセンサの計測情報に基づき予め生成された第2の地図と、各位置での第2のセンサの計測情報とに基づいて第2の地図上の自己位置を推定し、ロボットの第1の地図上の第1の移動軌跡と第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、推定されたロボットの第1の地図上の自己位置を第2の地図上の自己位置に変換するように構成する。
【解決手段】ロボットの第1のセンサの計測情報に基づき予め生成された第1の地図と、ロボットの各位置での第1のセンサの計測情報とに基づいて第1の地図上の自己位置を推定し、第2のセンサの計測情報に基づき予め生成された第2の地図と、各位置での第2のセンサの計測情報とに基づいて第2の地図上の自己位置を推定し、ロボットの第1の地図上の第1の移動軌跡と第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、推定されたロボットの第1の地図上の自己位置を第2の地図上の自己位置に変換するように構成する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、地図処理方法及びプログラム、並びにロボットシステムに関する。本発明は、地図処理プログラムを格納したコンピュータ読み取り可能な記憶媒体にも関する。
【背景技術】
【0002】
自律移動型ロボットは、実環境内で障害物、壁などと衝突すること無く自由に移動できるように、例えばロボットに搭載されたセンサの計測情報と環境地図との照合に基づいて自己位置の推定を行う。ロボットが実環境下で自己位置を確実、且つ、正確に推定するために、複数の異なる地図を併用する方式がある。また、環境地図を自動的に作成する技術としては、例えばSLAM(Simultaneous Localization And Mapping)が知られている。
【0003】
複数の地図を自動的に作成する方式は、一般的には以下の2つの方式に分類できる。第1の方式では、ある種類のセンサの計測情報に基づいて最初の地図を作成する。その後、他の種類のセンサの計測情報を最初の地図に足し加えて2番目の地図を作成し、以下同様にして3番目以降の地図を作成する。一方、第2の方式では、複数の異なる種類のセンサの計測情報を推定器に同時観測情報として入力して、複数の地図を同時に、且つ、自動的に作成する。
【0004】
第1の方式の場合、2番目以降に作成される地図が最初に作成された地図に依存する。このため、2番目以降に作成された地図の精度は、最初に作成された地図の精度に依存してしまう。一方、第2の方式の場合、センサの種類が多くなると、推定器の観測空間が大きくなるため、推定器の計算量が多くなると共に計算時間がかかる。また、異なる種類のセンサは異なる計測レートを用いるため、使用するセンサの計測レートを最も遅い計測レートに合わせる必要がある。
【0005】
しかし、複数の地図を同時に生成する場合、異なる種類のセンサの異なる誤差モデルを反映しながら各センサ独自の計測レートを用いることが望ましい。このように、独立して作成された複数の地図では、各地図の座標系が異なると共に、推定誤差によって夫々の地図が異なる歪みを含んでいる。このため、ロボットが自己位置の推定を行う段階では、各センサの計測情報と対応する地図とのマッチングにより推定された自己位置が、異なる座標系で表現されたものであるため、例えばロボットの移動経路を決定する経路計画(または、経路プランニング)などを行うためには推定した自己位置の座標系を統合(または、統一)する必要がある。
【0006】
従来、例えばグリッドマップで表現可能な環境地図については、地図間で相互変換を行う座標補正方法が提案されている(例えば、特許文献1)。しかし、この提案された座標補正方法のように地図間で線形変換を行っても、各地図には歪み、すなわち、非線形成分が含まれている。このため、複数の地図が同一の座標系を共有するように地図間で線形変換を行っても、高精度にこれら複数の地図の座標系を統合することは難しい。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開2009−157430号公報
【特許文献2】特開平11−282343号公報
【非特許文献】
【0008】
【非特許文献1】C. E. Rasmussen, C. K. I. Williams, "Gaussian Process for Machine Learning", The MIT Press, 2006
【発明の概要】
【発明が解決しようとする課題】
【0009】
従来、独立して作成された複数の地図の座標系が異なる場合、高精度にこれら複数の地図の座標系を統合することは難しいという問題があった。
【0010】
そこで、本発明は、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することが可能な地図処理方法及びプログラム、並びにロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【0011】
本発明の一観点によれば、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定処理と、前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定処理と、前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定処理により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換処理をコンピュータに実行させることを特徴とする地図処理方法が提供される。
【0012】
本発明の一観点によれば、コンピュータに地図を処理させるプログラムであって、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定手順と、前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定手順と、前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定手順により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換手順を前記コンピュータに実行させることを特徴とするプログラムが提供される。
【0013】
本発明の一観点によれば、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定器と、前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定器と、前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定器により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する座標変換部を備えたことを特徴とするロボットシステムが提供される。
【発明の効果】
【0014】
開示の地図処理方法及びプログラム、並びにロボットシステムによれば、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することができる。
【図面の簡単な説明】
【0015】
【図1】本発明の一実施例におけるロボットシステムの一例を示すブロック図である。
【図2】ロボットの位置の共分散の変換を説明する図である。
【図3】ロボットの姿勢の変換を説明する図である。
【図4】地図生成部及び座標変換パラメータ算出部の処理の一例を説明するフローチャートである。
【図5】本発明の一実施例におけるロボットシステムの他の例を示すブロック図である。
【図6】統合部の統合処理の一例を説明する図である。
【図7】自己位置推定処理の一例を説明するフローチャートである。
【図8】非線形マッピングを説明する図である。
【図9】ロボットの一例を示す側面図である。
【図10】センサの計測範囲を説明する図である。
【図11】センサの計測情報に基づき作成した地図の一例を示す図である。
【図12】作成した地図上のロボットの移動軌跡と、基準地図へ変換後の移動軌跡を説明する図である。
【図13】推定経路の誤差を説明する図である。
【発明を実施するための形態】
【0016】
開示の地図処理方法及びプログラム、並びにロボットシステムでは、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成された第1の地図と、ロボットの各位置での第1のセンサの計測情報とに基づいてロボットの第1の地図上の自己位置を推定するし、ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成された第2の地図と、ロボットの各位置での第2のセンサの計測情報とに基づいてロボットの第2の地図上の自己位置を推定する。第1のセンサの計測情報に基づき予め推定されたロボットの第1の地図上の第1の移動軌跡と第2のセンサの計測情報に基づき予め推定されたロボットの第2の地図上の第2の移動軌跡とに基づいて予め算出された、第1の移動軌跡の第1の座標系を第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、第1のセンサの計測情報に基づいて推定されたロボットの第1の地図上の自己位置を第2の地図上の自己位置に変換する。
【0017】
以下に、開示の地図処理方法及びプログラム、並びにロボットシステムの各実施例を図面と共に説明する。
【実施例】
【0018】
図1は、本発明の一実施例におけるロボットシステムの一例を示すブロック図である。この例では、本発明がロボットを含むロボットシステムに適用されている。ロボットは、例えばモータなどにより駆動される台車、各種内的センサ及び外的センサ、及びプロセッサなどにより形成された制御部を有する周知の基本構成を有する自律走行型ロボットで形成可能である。ロボットシステム1は、センサ部2、SLAM部3、及び地図間マッピング部4を有する。
【0019】
センサ部2は、オドメトリ(Odometry)部21及び異なる種類のセンサ22A,22B,22Cを有する。SLAM部3は、地図生成部31A,31B,31C及び地図A,B,Cを記憶する記憶部32A,32B,32Cを有する。地図間マッピング部4は、移動軌跡保持部41A,41B,41C、座標変換パラメータ算出部42,43、及び変換パラメータを記憶する記憶部44,45を有する。
【0020】
ロボットシステムがロボットで形成されている場合、センサ部2、SLAM部3、及び地図間マッピング部4はロボットに設けられる。一方、ロボットシステムがロボット及び当該ロボットを制御する当該ロボットとは別体の情報処理装置で形成されている場合、SLAM部3を情報処理装置に設けても、SLAM部3及び地図間マッピング部4を情報処理装置に設けても良い。情報処理装置は、ロボットと通信可能な通信部(または、インタフェース)、CPUなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。プログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
【0021】
先ず、地図作成処理について説明する。図1において、環境5内でロボット6を例えば手動で移動して、環境情報を収集して地図を作成する。この例では、異なる複数の地図を作成すると共に、移動軌跡も利用して地図座標系間の変換も行う。
【0022】
オドメトリ部21は、ロボット6の移動量を、台車6−1の車輪6−2の回転を検出するエンコーダ(図示せず)、または、ロボット6に設けられたジャイロセンサ(または、ジャイロスコープ、図示せず)などの内的センサからの検出信号に基づいて計算する周知の構成を有する。センサ22Aは、例えばレーザレンジファインダ(LRF:Laser Range Finder)、カメラなどで形成されて環境計測を行う。センサ22Bは、例えばセンサ22Aとは異なる位置、及び/または、向きで取り付けられたLRF、カメラ等で形成されて環境計測を行う。センサ22Cは、例えばセンサ22A,22Bとは異なる位置、及び/または、向きで取り付けられたLRF、カメラ等で形成されて環境計測を行う。この例では、センサ22A,22Bは互いに異なる位置、及び/または、向きで取り付けられたLRFで形成され、センサ22Cはカメラで形成されており、センサ22A,22B,22Cはいずれも外的センサを形成する。
【0023】
地図生成部31Aは、ロボット6が移動している時に、センサ22Aが収集した時系列の環境データを、オドメトリ部21が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップAを生成し、記憶部32Aに記憶する。同様にして、地図生成部31Bは、ロボット6が移動している時に、センサ22Bが収集した時系列の環境データを、オドメトリ部21が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップBを生成し、記憶部32Bに記憶する。また、地図生成部31Cは、ロボット6が移動している時に、センサ22Cが収集した時系列の環境データを、オドメトリ部21が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップCを生成し、記憶部32Cに記憶する。
【0024】
また、地図生成部31Aは、生成した地図Aに基づいて、オドメトリ部21が検出したオドメトリ情報のみに基づいて推定したロボット6の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差(または、オドメトリ誤差)をキャンセルして、ロボット6の正しい移動軌跡MLAを推定して移動軌跡保持部41Aに保持する。同様にして、地図生成部31Bは、生成した地図Bに基づいて、オドメトリ部21が検出したオドメトリ情報のみに基づいて推定したロボット6の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差をキャンセルして、ロボット6の正しい移動軌跡MLBを推定して移動軌跡保持部41Bにする。さらに、地図生成部31Cは、生成した地図Cに基づいて、オドメトリ部21が検出したオドメトリ情報のみに基づいて推定したロボット6の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差をキャンセルして、ロボット6の正しい移動軌跡MLCを推定して移動軌跡生成部41Cに保持する。地図生成部31A,31B,31Cがロボット6の正しい移動軌跡を推定する際には、周知の移動軌跡推定方法を利用可能である。この例では、移動軌跡推定方法にファスト(Fast)SLAMという方法を利用するものとする。移動軌跡保持部41A,41B,41Cは、メモリなどの記憶部で形成可能である。
【0025】
座標変換パラメータ算出部42は、移動軌跡保持部41Aに保持されたロボット6の移動軌跡の座標系CAを移動軌跡保持部41Bに保持されたロボット6の移動軌跡の座標系CBへマッピングするパラメータ(以下、座標変換パラメータとも言う)を算出し、記憶部44に記憶する。また、座標変換パラメータ算出部43は、移動軌跡保持部41Cに保持されたロボット6の移動軌跡の座標系CCを移動軌跡保持部41Bに保持されたロボット6の移動軌跡の座標系CBへマッピングする座標変換パラメータを算出し、記憶部45に記憶する。この例では、位置座標の変換式にGPR(Gaussian Process Regression)を利用してマッピングを行う。
【0026】
地図生成時に推定する移動軌跡は、次のように表すことができる。Xは、地図Aの作成時に地図生成部31Aが推定により生成する移動軌跡を示し、Yは地図Bの作成時に地図生成部31Bが推定により生成する移動軌跡を示す。
【0027】
【数1】
【0028】
なお、GPRに必要なパラメータσ1, ..., 4は、例えば非特許文献1にて提案されている方法で算出可能である。
【0029】
図2は、ロボット6の位置の共分散の変換、すなわち、アンセンテッド変換(Unscented Transformation)を説明する図である。図2において、Oは地図Aで推定したロボット6の位置、Σは地図Aで推定したロボット6の位置の共分散行列、E,F,G,H,Oはアンセンテッド変換により算出されるシグマポイント、E',F',G',H',O'は地図Bの座標系CBに変換した後の座標、Σ'は地図Bの座標系CBに変換した後のロボット6の位置の共分散行列を表す。アンセンテッド変換によるシグマポイントの算出、及び、地図Bの座標系CBに変換した後のロボット6の位置の共分散行列については、例えば非特許文献1に記載されている。
【0030】
このようにして、地図Aで推定した位置共分散行列からシグマポイントE,F,G,H,Oを求める。次に、各シグマポイントE,F,G,H,Oを地図Bの座標系CBに変換した後の座標E',F',G',H',O'を求める。そして、変換後のシグマポイントE',F',G',H',O'から地図Bの座標系CBに変換した後のロボット6の位置の共分散行列を求める。
【0031】
図3は、ロボット6の姿勢の変換を説明する図である。図3中、(a)は地図A上で推定したロボット6の位置P(xp,yp)を示す。θは、地図A上で推定したロボット6の向きを示す。この場合、微小移動先Q(xq,yq)を次式に基づいて算出する。
【0032】
xq=xp+dcos(θ)
yq=yp+dsin(θ)
次に、地図A上のP,Qを、地図Bの座標系CB上のP'(x'p,y'p),Q'(x'q,y'q)を求める。そして、地図B上でのロボット6の姿勢θ'= arctan(y'q−y'p,x'q−x'p)を求める。
【0033】
姿勢分散の変換は、次のように求めることができる。先ず、姿勢推定の分散Σθからシグマポイントθ0,θ1,θ2を計算する。次に、シグマポイントθ0,θ1,θ2を地図Bの座標系CB上のシグマポイントθ'0,θ'1,θ'2に変換する。そして、変換されたシグマポイントθ'0,θ'1,θ'2から地図B上のロボット6の姿勢の分散Σθ'を求める。
【0034】
図4は、地図生成部31A,31B及び座標変換パラメータ算出部42の処理の一例を説明するフローチャートである。図4において、地図生成部31Aの処理ST1、地図生成部31Bの処理ST2、及び座標変換パラメータ算出部42の処理ST3は、互いに独立したタスクで形成可能である。
【0035】
図4において、地図生成部31Aの処理ST1が開始されると、ステップS1は、地図生成部31Aの推定器が持つロボット6の現在位置の座標と姿勢のデータを初期化すると共に、ロボット6の目的地点(または、ゴール地点)の座標を初期化する。この例では、説明の便宜上、初期位置の座標と姿勢を[xA1, yA1, θA1]、ゴール地点を[xG, yG]で表すものとする。ステップS2は、ロボット6の現在位置がゴール地点であるか否かを判断する。ステップ2の判断結果がNOであれば処理はステップS3へ進み、判断結果がYESであれば地図生成部31Aの処理ST1の完了フラグを座標変換パラメータ算出部42へ通知し、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS31へ進む。ステップS2の判断結果に応じて、ロボット6の現在位置がゴール地点であるか否かを示すフラグを記憶しても良い。
【0036】
ステップS3は、オドメトリ部21からロボット6の現在位置を示すオドメトリ情報を入力する。オドメトリ部21が出力するオドメトリ情報には、エンコーダやジャイロセンサなどの内的なセンサのみで推定したロボットの位置と姿勢のデータが含まれる。ロボット6は、車輪の機械的的誤差、床上での車輪のスリップなどによって推定誤差が徐々に拡大するが、この例では位置の差分のみに着目する。ステップS3が入力するオドメトリ情報に含まれる位置と姿勢のデータ[xOt, yOt]のうち、tはステップS2〜S6のループの実行回数を示す。
【0037】
ステップS4は、ステップS3で入力したオドメトリ情報(位置と姿勢のデータ)と、前回入力したオドメトリ情報(位置と姿勢のデータ)との差分を算出し、推定器が前回推定したオドメトリ情報(位置と姿勢のデータ)にこの差分を足してオドメトリ情報(位置と姿勢のデータ)を更新することで推定器の内部状態を予測する。ステップS5は、センサ22Aからの計測データ(すなわち、環境データ)を入力する。この例ではセンサ22AはLRFであるため、センサ22Aからの計測データはロボット6(すなわち、LRF)から壁までの計測距離を示す。ステップS6は、センサ22Aからの計測データに基づいて生成され記憶部32Aに記憶された地図Aと、センサ22Aで計測された計測データとに基づきロボット6の現在位置と姿勢のデータ[xAt, yAt]を推定し、ロボット6の位置と姿勢のデータをこのデータ[xAt, yAt]に更新する。地図Aには、壁やランドマークなどの位置のデータも含まれる。
【0038】
ステップS7は、ステップS6で推定された現在位置と姿勢のデータ[xAt, yAt]のうち、現在位置の推定値を抽出する。ステップS7の後、処理はステップS8及びステップS9へ並行して進む。ステップS8は、ステップS7で抽出した現在位置の推定値に基づいて、ステップS5で取得した環境データを地図Aに反映させることで地図Aの追加更新を行い、追加更新された地図Aを記憶部32Aに記憶する。一方、ステップS9は、ステップS7で抽出した現在位置の推定値を例えば地図生成部31A内の記憶部に記憶することで保存する。この記憶部は、移動軌跡保持部41Aを形成する記憶部であっても良い。ステップS10は、ステップS6で推定されたロボット6の現在位置と姿勢のデータ[xAt, yAt]を時系列的に移動軌跡保持部41Aに保持する。これにより、データ[xA1, yA1], [xA2, yA2],…, [xAt, yAt],…, [xAT, yAT]が移動軌跡MLAとして保持され、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS33へ進む。
【0039】
一方、地図生成部31Bの処理ST2が開始されると、ステップS11〜S20がステップS1〜S10と同様の処理をセンサ22Bに対して行う。ステップS12の判断結果がYESであれば地図生成部31Bの処理ST2の完了フラグを座標変換パラメータ算出部42へ通知し、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS31へ進む。ステップS20の後、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS34へ進む。
【0040】
座標変換パラメータ算出部42の処理ST3が開始されると、ステップS31は、両地図作成完了待ち状態となる。ステップS32は、地図生成部31Aによる地図Aの生成と、地図生成部31Bによる地図Bの生成が完了しているか否かを判断する。具体的には、地図生成部31Aの処理ST1から完了フラグが通知され、且つ、地図生成部31Bの処理ST2から完了フラグが通知されたか否かを判断する。ステップS32の判断結果がNOであると処理はステップS31へ戻り、判断結果がYESであると処理はステップS33へ進む。
【0041】
ステップS33は、記憶部(移動軌跡保持部41A)から推定されたロボット6の現在位置と姿勢のデータ [xA1, yA1], [xA2, yA2],…, [xAt, yAt],…, [xAT, yAT]を読み込み、ロボット6の移動軌跡MLAを取得する。ステップS34は、記憶部(移動軌跡保持部41B)から推定されたロボット6の現在位置と姿勢のデータ [xB1, yB1], [xB2, yB2],…, [xBt, yBt],…, [xBT, yBT]を読み込み、ロボット6の移動軌跡MLBを取得する。
【0042】
ステップS35は、移動軌跡MLB上の各ポイントが対応する移動軌跡MLA上のポイントを計算する。具体的には、同じ時刻に合わせて、移動軌跡MLA上の点座標と移動軌跡MLB上の点座標のペア、例えば(t1:[xA1, yA1]→ [xB1, yB1], t2:[xA2, yA2] →[xB2, yB2],…, tT:[xAT, yAT] →[xBT, yBT])を計算する。ステップS36は、ステップS35で計算した移動軌跡MLA,MLB上の点座標のペアに基づいて、GPRパラメータを最適化する。具体的には、パラメータσ1〜σ4と座標点のペア列t1:[xA1, yA1] →[xB1, yB1], t2:[xA2, yA2] →[xB2, yB2],…, tT:[xAT, yAT] →[xBT, yBT]が得られ、これらは地図Aから地図Bへの変換パラメータとして記憶部44に記憶される。
【0043】
なお、地図生成部31C,31B及び座標変換パラメータ算出部43の処理は、図4の処理と同様に行えるので、その図示及び説明は省略する。
【0044】
センサ22A,22B,22Cで計測した環境データと、上記の如く作成した地図A,B,Cとを照合して、各地図A,B,Cにおけるロボット6の位置座標と向き角度を計算することができる。しかし、各センサ22A,22B,22Cで推定したロボット6の位置と姿勢を統合する処理を行うためには、異なる地図座標系で表した位置座標と姿勢をある基準座標系に変換する必要がある。本実施例では、以下のような処理により必要な座標変換を行う。なお、以下の説明では、地図Bを基準地図とするので、地図Bの座標系が基準座標系となる。
【0045】
図5は、本発明の一実施例におけるロボットシステムの他の例を示すブロック図である。図5中、図1と同一部分には同一符号を付し、その説明は省略する。この例においても、本発明がロボットを含むロボットシステムに適用されている。ロボットは、例えばモータなどにより駆動される台車、環境計測を行う各種センサ、及びプロセッサなどにより形成された制御部を有する周知の基本構成を有する自律走行型ロボットで形成可能である。ロボットシステム11は、センサ部2、自己位置推定部7、及び統合処理部8を有する。
【0046】
ロボットシステムがロボットで形成されている場合、センサ部2、自己位置推定部7、及び統合処理部8はロボットに設けられる。一方、ロボットシステムがロボット及び当該ロボットを制御する当該ロボットとは別体の情報処理装置で形成されている場合、自己位置推定部7を情報処理装置に設けても、自己位置推定部7及び統合処理部8を情報処理装置に設けても良い。情報処理装置は、ロボットと通信可能な通信部、CPUなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。プログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
【0047】
自己位置推定部7は、自己位置推定器71A,71B,71Cと、記憶部32A,32B,32Cを有する。統合処理部8は、座標変換部81,82、記憶部44,45、及び統合部83を有する。
【0048】
自己位置推定部7において、自己位置推定器71Aは、センサ22Aからの計測情報と、オドメトリ部21からのオドメトリ情報と、記憶部32Aから読み出した地図Aに基づいて周知の自己位置推定を行う。自己位置推定器71Aの自己位置推定により推定された自己位置PAは、記憶部32Aから読み出した地図Aと、センサ22Aからの計測情報に基づいて推定した地図Aとを照合することで、地図Aの座標系におけるロボット6の尤もらしい現在位置と姿勢を表す。自己位置推定器71Bは、センサ22Bからの計測情報と、オドメトリ部21からのオドメトリ情報と、記憶部32Bから読み出した地図Bに基づいて周知の自己位置推定を行う。自己位置推定器71Bの自己位置推定により推定された自己位置PBは、記憶部32Bから読み出した地図Bと、センサ22Bからの計測情報に基づいて推定した地図Bとを照合することで、地図Bの座標系におけるロボット6の尤もらしい現在位置と姿勢を表す。自己位置推定器71Cは、センサ22Cからの計測情報と、オドメトリ部21からのオドメトリ情報と、記憶部32Cから読み出した地図Cに基づいて周知の自己位置推定を行う。自己位置推定器71Cの自己位置推定により推定された自己位置PCは、記憶部32Cから読み出した地図Cと、センサ22Cからの計測情報に基づいて推定した地図Cとを照合することで、地図Cの座標系におけるロボット6の尤もらしい現在位置と姿勢を表す。記憶部32A〜32Cに記憶された地図A〜Cは、図1の地図生成部31A〜31Cで予め生成されたものである。
【0049】
統合処理部8において、座標変換部81は、自己位置推定器71Aで推定した地図Aの座標系でのロボット6の自己位置PAを、記憶部44から読み出した地図Aから地図Bへの座標変換パラメータを用いて地図Bの座標系(すなわち、基準座標系)の自己位置PA'へ変換する。座標変換部82は、自己位置推定器71Cで推定した地図Cの座標系でのロボット6の自己位置PCを、記憶部45から読み出した地図Cから地図Bへの座標変換パラメータを用いて地図Bの座標系(すなわち、基準座標系)の自己位置PC'へ変換する。記憶部44,45に記憶された変換パラメータは、図1の座標変換パラメータ算出部42,43により予め算出されたものである。
【0050】
統合処理部83は、座標変換部81,82で得られた地図Bの座標系の自己位置PA',PC'及び自己位置推定器71Bで得られた地図Bの座標系の自己位置PBを統合する統合処理を行い、統合結果を例えばロボット6の移動経路を決定する経路計画部84などの処理部に供給する。
【0051】
図6は、統合部83の統合処理の一例を説明する図である。図6中、図5と同一部分には同一符号を付し、その説明は省略する。この例では、説明の簡略化のため基準座標系の自己位置PA'と自己位置PBの統合を例にとるが、自己位置PC'と自己位置PBの統合も同様に行えば良い。
【0052】
変換後の自己位置PA'は位置姿勢xAと地図Aで推定したロボット6の位置の共分散行列ΣAで表され、自己位置PBは位置姿勢xBと地図Bで推定したロボット6の位置の共分散行列ΣBで表されるものとすると、統合部83での統合処理により統合される共分散行列は例えば
【0053】
【数2】
【0054】
図7は、自己位置推定処理の一例を説明するフローチャートである。図7において、自己位置推定器71Aの処理ST11、自己位置推定器71Bの処理ST12、座標変換部81の処理ST13、及び統合部83の処理ST14は、互いに独立したタスクで形成可能である。地図Bを基準地図とするので、地図Aの座標系で推定したロボット6の位置を地図Bの座標系に変換して、地図Bの座標系で推定した位置と統合する。同様にして、地図Cの座標系で推定したロボット6の位置は、地図Bの座標系に変換して、地図Bの座標系で推定した位置と統合すれば良い。
【0055】
図7において、自己位置推定器71Aの処理ST11が開始されると、ステップS50は、地図Aによる自己位置推定器71Aの持つロボット6の初期位置姿勢[xA1, yA1, θA1]を初期化する。ステップS51は、タスクを終了するか否かを判定する。例えば、ロボット6が目的地点(または、ゴール地点)着いて自己推定位置と設定された目的地点が一致した場合、或いは、ユーザからの終了指示があれば、ステップS51の判定結果はYESとなり、処理は終了する。一方、ステップS51の判定結果がNOであると、処理はステップS52へ進む。ステップS52は、オドメトリ部21からオドメトリ情報を入力することでロボット6の現在位置を得る。ステップS52が入力するオドメトリ情報に含まれる位置と姿勢のデータ[xOt, yOt]のうち、tはステップS51〜S55のループの実行回数を示す。
【0056】
ステップS53は、ステップS52で入力したオドメトリ情報と、前回入力したオドメトリ情報との差分を算出し、自己位置推定器71Aが前回推定した位置と姿勢のデータにこの差分を足して位置と姿勢のデータを更新することで自己位置推定器71Aの内部状態を予測する。ステップS54は、センサ22Aからの計測データを入力する。ステップS55は、センサ22Aからの計測データに基づいて生成され記憶部32Aに記憶された地図Aと、センサ22Aで計測された計測データとに基づきロボット6の現在位置と姿勢のデータ[xAt, yAt]を推定し、ロボット6の位置と姿勢のデータをこのデータ[xAt, yAt]に更新する。ステップS56は、ステップS55で推定された現在位置と姿勢のデータ[xAt, yAt]のうち、現在位置の推定値、すなわち、推定された自己位置PAを抽出し、処理は座標変換部81の処理ST13へ進む。
【0057】
一方、自己位置推定器71Bの処理ST12が開始されると、ステップS70〜S76がステップS50〜S56と同様の処理をセンサ22Bに対して行う。ステップS76の後、ステップS77は、推定結果である、自己位置推定器71Bで推定した地図Bの座標系でのロボット6の自己位置PBを統合部83へ通知し、処理は統合部83の処理ST14へ進む。
【0058】
座標変換部81の処理ST13において、ステップS61は、自己位置推定器71Aで推定した地図Aの座標系でのロボット6の自己位置PAを、記憶部44から読み出した地図Aから地図Bへの座標変換パラメータを用いて地図Bの座標系(すなわち、基準座標系)の自己位置PA'へ変換する。変換後の自己位置PA'は、位置姿勢xAと地図Aで推定したロボット6の位置の共分散行列ΣAで表される。ステップS62は、ステップS61の座標変換により求めた自己位置PA'、すなわち、位置姿勢xAと共分散行列ΣAを例えば座標変換部81内の記憶部に保存する。ステップS63は、推定結果である変換後の自己位置PA'を統合部83へ通知する。
【0059】
統合部83の処理ST14において、ステップS81は、推定結果の通知を待ち、ステップS63またはステップS77から推定結果の通知があれば例えば統合部83内の記憶部(例えば、共有メモリ)に記憶する。また、ステップS81は、推定結果を共有メモリに記憶すると、共有メモリの更新があることを示す更新フラグを設定する。ステップS82は、更新フラグが設定されているか否かを判定することで、推定結果が新たに通知されたか否かを判定する。ステップS82の判定結果がNOであると、処理はステップS81へ戻る。一方、ステップS82の判定結果がYESであると、ステップS83は、新たに通知された推定結果について上記の如き統合処理を行い、統合処理の結果を例えば統合部83内の記憶部に記憶する。また、必要に応じてステップS84を行っても良い。ステップS84は、統合処理の結果を経路計画部84などの処理部に供給する。
【0060】
このように、本実施例では、ロボットを実環境下で移動させ、ロボットに取り付けられている複数の異なるセンサで計測した環境情報に基づいて複数の地図を独立して作成した後、各地図に対応してロボットの移動軌跡を推定する。推定したロボットの複数の移動軌跡を使って、複数の地図座標系間の非線形変換パラメータ、非線形座標変換式を求める。つまり、地図座標系間の変換は非線形変換方式によって行い、変換に必要なパラメータは独立して推定した移動軌跡から求めて例えばデータベースとして保存できる。独立して作成した環境地図の座標系は独立しており、座標系間の変換は非線形マッピングによって行う。
【0061】
図8は、上記の非線形マッピングを説明する図である。図8(a)は、ロボット6が実環境下で位置Sから位置Gへ移動する場合を示す。ロボット6が移動中に、センサ22Aが環境情報SAを収集し、センサ22B環境情報SBを収集するものとする。この場合、環境情報SAに基づいて図8(b)に示す環境地図Aを作成し、環境情報SBに基づいて環境地図Bを作成する。同時に、ロボット6の環境地図A上の移動軌跡MLAと、環境地図B上の移動軌跡MLBを推定する。そして、移動軌跡MLA,MLBを用いて、座標系XaOaYaから座標系XbObYbへの座標変換パラメータΘを求める。つまり、座標変換パラメータに、推定した移動軌跡MLA,MLBを用いる。
【0062】
【数3】
【0063】
非線形変換方式は特に限定されないが、ガウスプロセスを利用することもできる。ガウスプロセスパラメータの最適化を行う方法自体は周知であり、特に限定されない。
【0064】
座標変換式には、例えばGPRを使用する。GPRで必要とするカーネル関数は、以下のようにサブカーネル関数の組み合わせからなるものでも良い。
【0065】
カーネル関数=RBFカーネル関数+線形カーネル関数+バイアスカーネル関数
また、ロボットの位置姿勢を以下の(1)〜(4)のように変換する。
(1)上記GPRを使用してロボット位置の基準座標系への座標変換を行う。
(2)上記アンセンテッド変換を利用してロボット位置の共分散行列を変換する。具体的には、ロボット位置の元の座標系でシグマポイントを求めて、基準座標系への座標変換を行い、変換されたシグマポイントの座標を用いて変換後の共分散行列を計算する。
(3)ロボットの現在位置を変換先の基準座標系へ変換することで、ロボット姿勢を変換する。具体的には、ロボットを微小距離移動させた場所での位置座標を基準座標系に変換し、基準座標系に変換された後の移動前と移動後の2つの位置座標によってarctanを計算して基準座標系におけるロボットの姿勢を求める。
(4)上記アンセンテッド変換を利用してロボット姿勢の分散を変換する。具体的には、ロボットの姿勢のシグマポイントを求めて、各シグマポイントについて基準座標系への座標変換を行い、各シグマポイントの対応する変換先の基準座標系での姿勢からロボット姿勢の分散を計算する。
【0066】
このように、ロボットの自律移動時の自己位置推定は、各地図で推定したロボットの位置を上記非線形変換方式の変換とパラメータに従って行う。ロボットを実環境下で自律移動をさせるために、ロボットに取り付けられている複数のセンサが独立して環境データを収集し、作成済みの各地図とマッチングすることによりロボットの現在位置の座標を推定する。異なる座標系を持つ複数の地図を用いることにより、異なる座標系でのロボット位置座標が推定できる。異なる位置姿勢の座標間変換は、上記非線形変換方式の変換とパラメータに従って行う。
【0067】
また、各地図で推定した夫々の姿勢値の変換は、座標系の異なる複数の地図により推定されたロボット姿勢(角度)の間の相互変換を、変換元の座標系でのロボット位置とロボット向きの夫々任意の一点を変換先の基準座標系に変換し、変換後の2点のベクトルから求めることで行う。さらに、各地図で推定した夫々の位置姿勢共分散行列の変換は、座標系の異なる複数の地図により推定されたロボット位置姿勢に関しての共分散行列の間の座標系変換をアンセンデット変換により行う。
【0068】
次に、上記実施例で求められるロボット6の推定経路を、図9乃至図13と共に説明する。
【0069】
図9は、ロボットの一例を示す側面図である。図9に示すロボット6には、センサ22A,22Bが設けられている。センサ22Aの地面(または、床)からの高さ位置は例えば50cmであり、センサ22Bの地面からの高さ位置は例えば20cmである。
【0070】
図10は、センサの計測範囲を説明する図である。図10中、(a)はロボット6とセンサ22Aの図9中の矢印で示す線に沿った断面の平面図を示し、破線で囲まれた範囲はセンサ22Aの計測範囲を示す。同様に、図10中、(b)はロボット6とセンサ22Bの図9中の矢印で示す線に沿った断面の平面図を示し、破線で囲まれた範囲はセンサ22Bの計測範囲を示す。
【0071】
図11は、センサの計測情報に基づき作成した地図の一例を示す図である。図11中、(a)はセンサ22Aの計測情報に基づいて図1の地図生成部31Aが作成した地図Aの一例を示し、MLAは地図生成部31Aがセンサ22Aの計測情報を収集することで推定されるロボット6の移動軌跡の一例を示す。同様に、図11中、(b)はセンサ22Bの計測情報に基づいて図1の地図生成部31Bが作成した地図Bの一例を示し、MLBは地図生成部31Bがセンサ22Bの計測情報を収集することで推定されるロボット6の移動軌跡の一例を示す。移動軌跡MLA,MLBは、物理空間内では同じであるが、夫々の地図A,B上では座標系が異なるため異なる。
【0072】
ロボット6の1回の移動では、物理的な移動軌跡は1つしかない。異なる地図A,Bの場合、推定した移動軌跡MLA,MLBの座標値の系列が異なる。推定した移動軌跡MLA,MLBが直接的に地図A,Bから由来したものであり、移動軌跡MLA,MLB間の座標変換関係を求めれば、地図A、B間の座標変換を近似することができる。そこで、上記実施例では移動軌跡MLA,MLBの情報に着眼した座標変換を行う。
【0073】
なお、地図A,B全体の回転成分があるだけではなく、局所的に、500A,500Bで示すような歪みがある。そこで、座標変換を行う際、地図A,B全体の回転成分に注目するだけではなく、局所的な歪みを補正する。
【0074】
図12は、作成した地図上のロボットの移動軌跡と、基準地図へ変換後の移動軌跡を説明する図である。図12中、(a)は図5の自己位置推定器71Aにより推定されたロボット6の地図A上の自己位置PAから推定された移動軌跡MLAを示し、グリッドは地図Aの座標系上のグリッドである。これに対し、図12中、(b)は自己位置推定器71Aにより推定されたロボット6の地図A上の自己位置PAを図5の座標変換部81により地図Bの座標系の自己位置PA'に変換し、変換後の自己位置PA'から推定されたロボット6の移動軌跡MLA'(すなわち、移動軌跡MLAを地図Bの座標系に変換した移動軌跡MLA')を示し、グリッドは地図Bの座標系上のグリッドである。
【0075】
図13は、推定経路の誤差を説明する図である。図13中、MLAは地図Aを用いて図5の自己位置推定器71Aが推定した自己位置PAから推定されたロボット6の移動軌跡、MLBは自己位置推定器71Bが推定した自己位置PBから推定されたロボット6の移動軌跡、MLA'は自己位置推定器71Aにより推定されたロボット6の地図A上の自己位置PAを座標変換部81により地図Bの座標系の自己位置PA'に変換し、変換後の自己位置PA'から推定されたロボット6の移動軌跡を示す。
【0076】
ロボット6が自律移動する際、例えば各地図A,Bに基づいて自己位置推定を行う。そこで、地図Aに基づき推定した移動軌跡MLAを基準地図Bに基づき推定した移動軌跡MLBと直接比較した場合、移動軌跡上の各ポイントの位置誤差の平均は0.52m、標準偏差は0.42mであった。これに対し、地図Aに基づき推定した移動軌跡MLAを基準地図Bの座標系に変換してから変換後の移動軌跡MLA'を基準地図Bに基づき推定した移動軌跡MLBと比較した場合、移動軌跡上の各ポイントの位置誤差の平均は0.21m、標準偏差は0.14mであった。これにより、図13に示す移動軌跡MLA,MLA',MLBの比較から、上記実施例の如き座標変換処理を行うことにより、独立して作成された複数の地図(この例では地図A,B)の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することが可能になることが確認された。
【0077】
以上の実施例を含む実施形態に関し、更に以下の付記を開示する。
(付記1)
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定処理と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定処理と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定処理により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換処理
をコンピュータに実行させることを特徴とする、地図処理方法。
(付記2)
前記第1の推定処理により推定され前記変換処理により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定処理により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合処理
を更に前記コンピュータに実行させることを特徴とする、付記1記載の地図処理方法。
(付記3)
前記変換処理は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、付記1または2記載の地図処理方法。
(付記4)
前記変換処理は、前記第1の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記1乃至3のいずれか1項記載の地図処理方法。
(付記5)
前記変換処理は、前記ロボットを微小距離移動させた場所での前記第1の座標系の位置座標を前記第2の座標系に変換し、前記第2の座標系に変換後の移動前と移動後の2つの位置座標に基づいて変換後の前記第2の座標系における前記ロボットの姿勢を求めることを特徴とする、付記1乃至4のいずれか1項記載の地図処理方法。
(付記6)
前記変換処理は、前記第1の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記5記載の地図処理方法。
(付記7)
コンピュータに地図を処理させるプログラムであって、
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定手順と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定手順と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定手順により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換手順
を前記コンピュータに実行させることを特徴とする、プログラム。
(付記8)
前記第1の推定手順により推定され前記変換手順により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定手順により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合手順
を更に前記コンピュータに実行させることを特徴とする、付記7記載のプログラム。
(付記9)
前記変換手順は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、付記7または8記載のプログラム。
(付記10)
前記変換手順は、前記第1の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記7乃至9のいずれか1項記載のプログラム。
(付記11)
前記変換手順は、前記ロボットを微小距離移動させた場所での前記第1の座標系の位置座標を前記第2の座標系に変換し、前記第2の座標系に変換後の移動前と移動後の2つの位置座標に基づいて変換後の前記第2の座標系における前記ロボットの姿勢を求めることを特徴とする、付記7乃至10のいずれか1項記載のプログラム。
(付記12)
前記変換手順は、前記第1の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記11記載のプログラム。
(付記13)
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定器と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定器と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定器により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する座標変換部
を備えたことを特徴とする、ロボットシステム。
(付記14)
前記第1の推定器により推定され前記座標変換部により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定器により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合部
を更に備えたことを特徴とする、付記13記載のロボットシステム。
(付記15)
前記座標変換部は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、付記13または14記載のロボットシステム。
(付記16)
前記座標変換部は、前記第1の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記13乃至15のいずれか1項記載のロボットシステム。
(付記17)
前記座標変換部は、前記ロボットを微小距離移動させた場所での前記第1の座標系の位置座標を前記第2の座標系に変換し、前記第2の座標系に変換後の移動前と移動後の2つの位置座標に基づいて変換後の前記第2の座標系における前記ロボットの姿勢を求めることを特徴とする、付記13乃至16のいずれか1項記載のロボットシステム。
(付記18)
前記座標変換部は、前記第1の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記17記載のロボットシステム。
(付記19)
前記第1の地図を予め生成して前記記憶部に記憶する第1の地図生成部と、
前記第2の地図を予め生成して前記記憶部に記憶する第2の地図生成部と、
前記座標変換パラメータを予め算出して前記記憶部に記憶するパラメータ算出部
を更に備えたことを特徴とする、付記13乃至18のいずれか1項記載のロボットシステム。
(付記20)
少なくとも前記統合部は、前記ロボットとは別体で前記ロボットと通信可能な情報処理装置に設けらていることを特徴とする、付記14乃至18のいずれか1項記載のロボットシステム。
【0078】
以上、開示の地図処理方法及びプログラム、並びにロボットシステムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。
【符号の説明】
【0079】
1 ロボットシステム
2 センサ部
3 SLAM部
4 地図間マッピング部
7 自己位置推定部
8 統合処理部
21 オドメトリ部
22A〜22C センサ
31A〜31C 地図生成部
32A〜32C 記憶部
71A〜71C 自己位置推定器
81,82 座標変換部
83 統合部
【技術分野】
【0001】
本発明は、地図処理方法及びプログラム、並びにロボットシステムに関する。本発明は、地図処理プログラムを格納したコンピュータ読み取り可能な記憶媒体にも関する。
【背景技術】
【0002】
自律移動型ロボットは、実環境内で障害物、壁などと衝突すること無く自由に移動できるように、例えばロボットに搭載されたセンサの計測情報と環境地図との照合に基づいて自己位置の推定を行う。ロボットが実環境下で自己位置を確実、且つ、正確に推定するために、複数の異なる地図を併用する方式がある。また、環境地図を自動的に作成する技術としては、例えばSLAM(Simultaneous Localization And Mapping)が知られている。
【0003】
複数の地図を自動的に作成する方式は、一般的には以下の2つの方式に分類できる。第1の方式では、ある種類のセンサの計測情報に基づいて最初の地図を作成する。その後、他の種類のセンサの計測情報を最初の地図に足し加えて2番目の地図を作成し、以下同様にして3番目以降の地図を作成する。一方、第2の方式では、複数の異なる種類のセンサの計測情報を推定器に同時観測情報として入力して、複数の地図を同時に、且つ、自動的に作成する。
【0004】
第1の方式の場合、2番目以降に作成される地図が最初に作成された地図に依存する。このため、2番目以降に作成された地図の精度は、最初に作成された地図の精度に依存してしまう。一方、第2の方式の場合、センサの種類が多くなると、推定器の観測空間が大きくなるため、推定器の計算量が多くなると共に計算時間がかかる。また、異なる種類のセンサは異なる計測レートを用いるため、使用するセンサの計測レートを最も遅い計測レートに合わせる必要がある。
【0005】
しかし、複数の地図を同時に生成する場合、異なる種類のセンサの異なる誤差モデルを反映しながら各センサ独自の計測レートを用いることが望ましい。このように、独立して作成された複数の地図では、各地図の座標系が異なると共に、推定誤差によって夫々の地図が異なる歪みを含んでいる。このため、ロボットが自己位置の推定を行う段階では、各センサの計測情報と対応する地図とのマッチングにより推定された自己位置が、異なる座標系で表現されたものであるため、例えばロボットの移動経路を決定する経路計画(または、経路プランニング)などを行うためには推定した自己位置の座標系を統合(または、統一)する必要がある。
【0006】
従来、例えばグリッドマップで表現可能な環境地図については、地図間で相互変換を行う座標補正方法が提案されている(例えば、特許文献1)。しかし、この提案された座標補正方法のように地図間で線形変換を行っても、各地図には歪み、すなわち、非線形成分が含まれている。このため、複数の地図が同一の座標系を共有するように地図間で線形変換を行っても、高精度にこれら複数の地図の座標系を統合することは難しい。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開2009−157430号公報
【特許文献2】特開平11−282343号公報
【非特許文献】
【0008】
【非特許文献1】C. E. Rasmussen, C. K. I. Williams, "Gaussian Process for Machine Learning", The MIT Press, 2006
【発明の概要】
【発明が解決しようとする課題】
【0009】
従来、独立して作成された複数の地図の座標系が異なる場合、高精度にこれら複数の地図の座標系を統合することは難しいという問題があった。
【0010】
そこで、本発明は、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することが可能な地図処理方法及びプログラム、並びにロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【0011】
本発明の一観点によれば、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定処理と、前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定処理と、前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定処理により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換処理をコンピュータに実行させることを特徴とする地図処理方法が提供される。
【0012】
本発明の一観点によれば、コンピュータに地図を処理させるプログラムであって、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定手順と、前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定手順と、前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定手順により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換手順を前記コンピュータに実行させることを特徴とするプログラムが提供される。
【0013】
本発明の一観点によれば、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定器と、前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定器と、前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定器により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する座標変換部を備えたことを特徴とするロボットシステムが提供される。
【発明の効果】
【0014】
開示の地図処理方法及びプログラム、並びにロボットシステムによれば、独立して作成された複数の地図の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することができる。
【図面の簡単な説明】
【0015】
【図1】本発明の一実施例におけるロボットシステムの一例を示すブロック図である。
【図2】ロボットの位置の共分散の変換を説明する図である。
【図3】ロボットの姿勢の変換を説明する図である。
【図4】地図生成部及び座標変換パラメータ算出部の処理の一例を説明するフローチャートである。
【図5】本発明の一実施例におけるロボットシステムの他の例を示すブロック図である。
【図6】統合部の統合処理の一例を説明する図である。
【図7】自己位置推定処理の一例を説明するフローチャートである。
【図8】非線形マッピングを説明する図である。
【図9】ロボットの一例を示す側面図である。
【図10】センサの計測範囲を説明する図である。
【図11】センサの計測情報に基づき作成した地図の一例を示す図である。
【図12】作成した地図上のロボットの移動軌跡と、基準地図へ変換後の移動軌跡を説明する図である。
【図13】推定経路の誤差を説明する図である。
【発明を実施するための形態】
【0016】
開示の地図処理方法及びプログラム、並びにロボットシステムでは、自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成された第1の地図と、ロボットの各位置での第1のセンサの計測情報とに基づいてロボットの第1の地図上の自己位置を推定するし、ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成された第2の地図と、ロボットの各位置での第2のセンサの計測情報とに基づいてロボットの第2の地図上の自己位置を推定する。第1のセンサの計測情報に基づき予め推定されたロボットの第1の地図上の第1の移動軌跡と第2のセンサの計測情報に基づき予め推定されたロボットの第2の地図上の第2の移動軌跡とに基づいて予め算出された、第1の移動軌跡の第1の座標系を第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、第1のセンサの計測情報に基づいて推定されたロボットの第1の地図上の自己位置を第2の地図上の自己位置に変換する。
【0017】
以下に、開示の地図処理方法及びプログラム、並びにロボットシステムの各実施例を図面と共に説明する。
【実施例】
【0018】
図1は、本発明の一実施例におけるロボットシステムの一例を示すブロック図である。この例では、本発明がロボットを含むロボットシステムに適用されている。ロボットは、例えばモータなどにより駆動される台車、各種内的センサ及び外的センサ、及びプロセッサなどにより形成された制御部を有する周知の基本構成を有する自律走行型ロボットで形成可能である。ロボットシステム1は、センサ部2、SLAM部3、及び地図間マッピング部4を有する。
【0019】
センサ部2は、オドメトリ(Odometry)部21及び異なる種類のセンサ22A,22B,22Cを有する。SLAM部3は、地図生成部31A,31B,31C及び地図A,B,Cを記憶する記憶部32A,32B,32Cを有する。地図間マッピング部4は、移動軌跡保持部41A,41B,41C、座標変換パラメータ算出部42,43、及び変換パラメータを記憶する記憶部44,45を有する。
【0020】
ロボットシステムがロボットで形成されている場合、センサ部2、SLAM部3、及び地図間マッピング部4はロボットに設けられる。一方、ロボットシステムがロボット及び当該ロボットを制御する当該ロボットとは別体の情報処理装置で形成されている場合、SLAM部3を情報処理装置に設けても、SLAM部3及び地図間マッピング部4を情報処理装置に設けても良い。情報処理装置は、ロボットと通信可能な通信部(または、インタフェース)、CPUなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。プログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
【0021】
先ず、地図作成処理について説明する。図1において、環境5内でロボット6を例えば手動で移動して、環境情報を収集して地図を作成する。この例では、異なる複数の地図を作成すると共に、移動軌跡も利用して地図座標系間の変換も行う。
【0022】
オドメトリ部21は、ロボット6の移動量を、台車6−1の車輪6−2の回転を検出するエンコーダ(図示せず)、または、ロボット6に設けられたジャイロセンサ(または、ジャイロスコープ、図示せず)などの内的センサからの検出信号に基づいて計算する周知の構成を有する。センサ22Aは、例えばレーザレンジファインダ(LRF:Laser Range Finder)、カメラなどで形成されて環境計測を行う。センサ22Bは、例えばセンサ22Aとは異なる位置、及び/または、向きで取り付けられたLRF、カメラ等で形成されて環境計測を行う。センサ22Cは、例えばセンサ22A,22Bとは異なる位置、及び/または、向きで取り付けられたLRF、カメラ等で形成されて環境計測を行う。この例では、センサ22A,22Bは互いに異なる位置、及び/または、向きで取り付けられたLRFで形成され、センサ22Cはカメラで形成されており、センサ22A,22B,22Cはいずれも外的センサを形成する。
【0023】
地図生成部31Aは、ロボット6が移動している時に、センサ22Aが収集した時系列の環境データを、オドメトリ部21が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップAを生成し、記憶部32Aに記憶する。同様にして、地図生成部31Bは、ロボット6が移動している時に、センサ22Bが収集した時系列の環境データを、オドメトリ部21が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップBを生成し、記憶部32Bに記憶する。また、地図生成部31Cは、ロボット6が移動している時に、センサ22Cが収集した時系列の環境データを、オドメトリ部21が検出したオドメトリ情報に基づいてつなぎ合わせて環境マップCを生成し、記憶部32Cに記憶する。
【0024】
また、地図生成部31Aは、生成した地図Aに基づいて、オドメトリ部21が検出したオドメトリ情報のみに基づいて推定したロボット6の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差(または、オドメトリ誤差)をキャンセルして、ロボット6の正しい移動軌跡MLAを推定して移動軌跡保持部41Aに保持する。同様にして、地図生成部31Bは、生成した地図Bに基づいて、オドメトリ部21が検出したオドメトリ情報のみに基づいて推定したロボット6の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差をキャンセルして、ロボット6の正しい移動軌跡MLBを推定して移動軌跡保持部41Bにする。さらに、地図生成部31Cは、生成した地図Cに基づいて、オドメトリ部21が検出したオドメトリ情報のみに基づいて推定したロボット6の移動軌跡に含まれる車輪の機械的誤差やスリップなどで生じた位置推定誤差をキャンセルして、ロボット6の正しい移動軌跡MLCを推定して移動軌跡生成部41Cに保持する。地図生成部31A,31B,31Cがロボット6の正しい移動軌跡を推定する際には、周知の移動軌跡推定方法を利用可能である。この例では、移動軌跡推定方法にファスト(Fast)SLAMという方法を利用するものとする。移動軌跡保持部41A,41B,41Cは、メモリなどの記憶部で形成可能である。
【0025】
座標変換パラメータ算出部42は、移動軌跡保持部41Aに保持されたロボット6の移動軌跡の座標系CAを移動軌跡保持部41Bに保持されたロボット6の移動軌跡の座標系CBへマッピングするパラメータ(以下、座標変換パラメータとも言う)を算出し、記憶部44に記憶する。また、座標変換パラメータ算出部43は、移動軌跡保持部41Cに保持されたロボット6の移動軌跡の座標系CCを移動軌跡保持部41Bに保持されたロボット6の移動軌跡の座標系CBへマッピングする座標変換パラメータを算出し、記憶部45に記憶する。この例では、位置座標の変換式にGPR(Gaussian Process Regression)を利用してマッピングを行う。
【0026】
地図生成時に推定する移動軌跡は、次のように表すことができる。Xは、地図Aの作成時に地図生成部31Aが推定により生成する移動軌跡を示し、Yは地図Bの作成時に地図生成部31Bが推定により生成する移動軌跡を示す。
【0027】
【数1】
【0028】
なお、GPRに必要なパラメータσ1, ..., 4は、例えば非特許文献1にて提案されている方法で算出可能である。
【0029】
図2は、ロボット6の位置の共分散の変換、すなわち、アンセンテッド変換(Unscented Transformation)を説明する図である。図2において、Oは地図Aで推定したロボット6の位置、Σは地図Aで推定したロボット6の位置の共分散行列、E,F,G,H,Oはアンセンテッド変換により算出されるシグマポイント、E',F',G',H',O'は地図Bの座標系CBに変換した後の座標、Σ'は地図Bの座標系CBに変換した後のロボット6の位置の共分散行列を表す。アンセンテッド変換によるシグマポイントの算出、及び、地図Bの座標系CBに変換した後のロボット6の位置の共分散行列については、例えば非特許文献1に記載されている。
【0030】
このようにして、地図Aで推定した位置共分散行列からシグマポイントE,F,G,H,Oを求める。次に、各シグマポイントE,F,G,H,Oを地図Bの座標系CBに変換した後の座標E',F',G',H',O'を求める。そして、変換後のシグマポイントE',F',G',H',O'から地図Bの座標系CBに変換した後のロボット6の位置の共分散行列を求める。
【0031】
図3は、ロボット6の姿勢の変換を説明する図である。図3中、(a)は地図A上で推定したロボット6の位置P(xp,yp)を示す。θは、地図A上で推定したロボット6の向きを示す。この場合、微小移動先Q(xq,yq)を次式に基づいて算出する。
【0032】
xq=xp+dcos(θ)
yq=yp+dsin(θ)
次に、地図A上のP,Qを、地図Bの座標系CB上のP'(x'p,y'p),Q'(x'q,y'q)を求める。そして、地図B上でのロボット6の姿勢θ'= arctan(y'q−y'p,x'q−x'p)を求める。
【0033】
姿勢分散の変換は、次のように求めることができる。先ず、姿勢推定の分散Σθからシグマポイントθ0,θ1,θ2を計算する。次に、シグマポイントθ0,θ1,θ2を地図Bの座標系CB上のシグマポイントθ'0,θ'1,θ'2に変換する。そして、変換されたシグマポイントθ'0,θ'1,θ'2から地図B上のロボット6の姿勢の分散Σθ'を求める。
【0034】
図4は、地図生成部31A,31B及び座標変換パラメータ算出部42の処理の一例を説明するフローチャートである。図4において、地図生成部31Aの処理ST1、地図生成部31Bの処理ST2、及び座標変換パラメータ算出部42の処理ST3は、互いに独立したタスクで形成可能である。
【0035】
図4において、地図生成部31Aの処理ST1が開始されると、ステップS1は、地図生成部31Aの推定器が持つロボット6の現在位置の座標と姿勢のデータを初期化すると共に、ロボット6の目的地点(または、ゴール地点)の座標を初期化する。この例では、説明の便宜上、初期位置の座標と姿勢を[xA1, yA1, θA1]、ゴール地点を[xG, yG]で表すものとする。ステップS2は、ロボット6の現在位置がゴール地点であるか否かを判断する。ステップ2の判断結果がNOであれば処理はステップS3へ進み、判断結果がYESであれば地図生成部31Aの処理ST1の完了フラグを座標変換パラメータ算出部42へ通知し、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS31へ進む。ステップS2の判断結果に応じて、ロボット6の現在位置がゴール地点であるか否かを示すフラグを記憶しても良い。
【0036】
ステップS3は、オドメトリ部21からロボット6の現在位置を示すオドメトリ情報を入力する。オドメトリ部21が出力するオドメトリ情報には、エンコーダやジャイロセンサなどの内的なセンサのみで推定したロボットの位置と姿勢のデータが含まれる。ロボット6は、車輪の機械的的誤差、床上での車輪のスリップなどによって推定誤差が徐々に拡大するが、この例では位置の差分のみに着目する。ステップS3が入力するオドメトリ情報に含まれる位置と姿勢のデータ[xOt, yOt]のうち、tはステップS2〜S6のループの実行回数を示す。
【0037】
ステップS4は、ステップS3で入力したオドメトリ情報(位置と姿勢のデータ)と、前回入力したオドメトリ情報(位置と姿勢のデータ)との差分を算出し、推定器が前回推定したオドメトリ情報(位置と姿勢のデータ)にこの差分を足してオドメトリ情報(位置と姿勢のデータ)を更新することで推定器の内部状態を予測する。ステップS5は、センサ22Aからの計測データ(すなわち、環境データ)を入力する。この例ではセンサ22AはLRFであるため、センサ22Aからの計測データはロボット6(すなわち、LRF)から壁までの計測距離を示す。ステップS6は、センサ22Aからの計測データに基づいて生成され記憶部32Aに記憶された地図Aと、センサ22Aで計測された計測データとに基づきロボット6の現在位置と姿勢のデータ[xAt, yAt]を推定し、ロボット6の位置と姿勢のデータをこのデータ[xAt, yAt]に更新する。地図Aには、壁やランドマークなどの位置のデータも含まれる。
【0038】
ステップS7は、ステップS6で推定された現在位置と姿勢のデータ[xAt, yAt]のうち、現在位置の推定値を抽出する。ステップS7の後、処理はステップS8及びステップS9へ並行して進む。ステップS8は、ステップS7で抽出した現在位置の推定値に基づいて、ステップS5で取得した環境データを地図Aに反映させることで地図Aの追加更新を行い、追加更新された地図Aを記憶部32Aに記憶する。一方、ステップS9は、ステップS7で抽出した現在位置の推定値を例えば地図生成部31A内の記憶部に記憶することで保存する。この記憶部は、移動軌跡保持部41Aを形成する記憶部であっても良い。ステップS10は、ステップS6で推定されたロボット6の現在位置と姿勢のデータ[xAt, yAt]を時系列的に移動軌跡保持部41Aに保持する。これにより、データ[xA1, yA1], [xA2, yA2],…, [xAt, yAt],…, [xAT, yAT]が移動軌跡MLAとして保持され、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS33へ進む。
【0039】
一方、地図生成部31Bの処理ST2が開始されると、ステップS11〜S20がステップS1〜S10と同様の処理をセンサ22Bに対して行う。ステップS12の判断結果がYESであれば地図生成部31Bの処理ST2の完了フラグを座標変換パラメータ算出部42へ通知し、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS31へ進む。ステップS20の後、処理は後述する座標変換パラメータ算出部42の処理ST3のステップS34へ進む。
【0040】
座標変換パラメータ算出部42の処理ST3が開始されると、ステップS31は、両地図作成完了待ち状態となる。ステップS32は、地図生成部31Aによる地図Aの生成と、地図生成部31Bによる地図Bの生成が完了しているか否かを判断する。具体的には、地図生成部31Aの処理ST1から完了フラグが通知され、且つ、地図生成部31Bの処理ST2から完了フラグが通知されたか否かを判断する。ステップS32の判断結果がNOであると処理はステップS31へ戻り、判断結果がYESであると処理はステップS33へ進む。
【0041】
ステップS33は、記憶部(移動軌跡保持部41A)から推定されたロボット6の現在位置と姿勢のデータ [xA1, yA1], [xA2, yA2],…, [xAt, yAt],…, [xAT, yAT]を読み込み、ロボット6の移動軌跡MLAを取得する。ステップS34は、記憶部(移動軌跡保持部41B)から推定されたロボット6の現在位置と姿勢のデータ [xB1, yB1], [xB2, yB2],…, [xBt, yBt],…, [xBT, yBT]を読み込み、ロボット6の移動軌跡MLBを取得する。
【0042】
ステップS35は、移動軌跡MLB上の各ポイントが対応する移動軌跡MLA上のポイントを計算する。具体的には、同じ時刻に合わせて、移動軌跡MLA上の点座標と移動軌跡MLB上の点座標のペア、例えば(t1:[xA1, yA1]→ [xB1, yB1], t2:[xA2, yA2] →[xB2, yB2],…, tT:[xAT, yAT] →[xBT, yBT])を計算する。ステップS36は、ステップS35で計算した移動軌跡MLA,MLB上の点座標のペアに基づいて、GPRパラメータを最適化する。具体的には、パラメータσ1〜σ4と座標点のペア列t1:[xA1, yA1] →[xB1, yB1], t2:[xA2, yA2] →[xB2, yB2],…, tT:[xAT, yAT] →[xBT, yBT]が得られ、これらは地図Aから地図Bへの変換パラメータとして記憶部44に記憶される。
【0043】
なお、地図生成部31C,31B及び座標変換パラメータ算出部43の処理は、図4の処理と同様に行えるので、その図示及び説明は省略する。
【0044】
センサ22A,22B,22Cで計測した環境データと、上記の如く作成した地図A,B,Cとを照合して、各地図A,B,Cにおけるロボット6の位置座標と向き角度を計算することができる。しかし、各センサ22A,22B,22Cで推定したロボット6の位置と姿勢を統合する処理を行うためには、異なる地図座標系で表した位置座標と姿勢をある基準座標系に変換する必要がある。本実施例では、以下のような処理により必要な座標変換を行う。なお、以下の説明では、地図Bを基準地図とするので、地図Bの座標系が基準座標系となる。
【0045】
図5は、本発明の一実施例におけるロボットシステムの他の例を示すブロック図である。図5中、図1と同一部分には同一符号を付し、その説明は省略する。この例においても、本発明がロボットを含むロボットシステムに適用されている。ロボットは、例えばモータなどにより駆動される台車、環境計測を行う各種センサ、及びプロセッサなどにより形成された制御部を有する周知の基本構成を有する自律走行型ロボットで形成可能である。ロボットシステム11は、センサ部2、自己位置推定部7、及び統合処理部8を有する。
【0046】
ロボットシステムがロボットで形成されている場合、センサ部2、自己位置推定部7、及び統合処理部8はロボットに設けられる。一方、ロボットシステムがロボット及び当該ロボットを制御する当該ロボットとは別体の情報処理装置で形成されている場合、自己位置推定部7を情報処理装置に設けても、自己位置推定部7及び統合処理部8を情報処理装置に設けても良い。情報処理装置は、ロボットと通信可能な通信部、CPUなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。プログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
【0047】
自己位置推定部7は、自己位置推定器71A,71B,71Cと、記憶部32A,32B,32Cを有する。統合処理部8は、座標変換部81,82、記憶部44,45、及び統合部83を有する。
【0048】
自己位置推定部7において、自己位置推定器71Aは、センサ22Aからの計測情報と、オドメトリ部21からのオドメトリ情報と、記憶部32Aから読み出した地図Aに基づいて周知の自己位置推定を行う。自己位置推定器71Aの自己位置推定により推定された自己位置PAは、記憶部32Aから読み出した地図Aと、センサ22Aからの計測情報に基づいて推定した地図Aとを照合することで、地図Aの座標系におけるロボット6の尤もらしい現在位置と姿勢を表す。自己位置推定器71Bは、センサ22Bからの計測情報と、オドメトリ部21からのオドメトリ情報と、記憶部32Bから読み出した地図Bに基づいて周知の自己位置推定を行う。自己位置推定器71Bの自己位置推定により推定された自己位置PBは、記憶部32Bから読み出した地図Bと、センサ22Bからの計測情報に基づいて推定した地図Bとを照合することで、地図Bの座標系におけるロボット6の尤もらしい現在位置と姿勢を表す。自己位置推定器71Cは、センサ22Cからの計測情報と、オドメトリ部21からのオドメトリ情報と、記憶部32Cから読み出した地図Cに基づいて周知の自己位置推定を行う。自己位置推定器71Cの自己位置推定により推定された自己位置PCは、記憶部32Cから読み出した地図Cと、センサ22Cからの計測情報に基づいて推定した地図Cとを照合することで、地図Cの座標系におけるロボット6の尤もらしい現在位置と姿勢を表す。記憶部32A〜32Cに記憶された地図A〜Cは、図1の地図生成部31A〜31Cで予め生成されたものである。
【0049】
統合処理部8において、座標変換部81は、自己位置推定器71Aで推定した地図Aの座標系でのロボット6の自己位置PAを、記憶部44から読み出した地図Aから地図Bへの座標変換パラメータを用いて地図Bの座標系(すなわち、基準座標系)の自己位置PA'へ変換する。座標変換部82は、自己位置推定器71Cで推定した地図Cの座標系でのロボット6の自己位置PCを、記憶部45から読み出した地図Cから地図Bへの座標変換パラメータを用いて地図Bの座標系(すなわち、基準座標系)の自己位置PC'へ変換する。記憶部44,45に記憶された変換パラメータは、図1の座標変換パラメータ算出部42,43により予め算出されたものである。
【0050】
統合処理部83は、座標変換部81,82で得られた地図Bの座標系の自己位置PA',PC'及び自己位置推定器71Bで得られた地図Bの座標系の自己位置PBを統合する統合処理を行い、統合結果を例えばロボット6の移動経路を決定する経路計画部84などの処理部に供給する。
【0051】
図6は、統合部83の統合処理の一例を説明する図である。図6中、図5と同一部分には同一符号を付し、その説明は省略する。この例では、説明の簡略化のため基準座標系の自己位置PA'と自己位置PBの統合を例にとるが、自己位置PC'と自己位置PBの統合も同様に行えば良い。
【0052】
変換後の自己位置PA'は位置姿勢xAと地図Aで推定したロボット6の位置の共分散行列ΣAで表され、自己位置PBは位置姿勢xBと地図Bで推定したロボット6の位置の共分散行列ΣBで表されるものとすると、統合部83での統合処理により統合される共分散行列は例えば
【0053】
【数2】
【0054】
図7は、自己位置推定処理の一例を説明するフローチャートである。図7において、自己位置推定器71Aの処理ST11、自己位置推定器71Bの処理ST12、座標変換部81の処理ST13、及び統合部83の処理ST14は、互いに独立したタスクで形成可能である。地図Bを基準地図とするので、地図Aの座標系で推定したロボット6の位置を地図Bの座標系に変換して、地図Bの座標系で推定した位置と統合する。同様にして、地図Cの座標系で推定したロボット6の位置は、地図Bの座標系に変換して、地図Bの座標系で推定した位置と統合すれば良い。
【0055】
図7において、自己位置推定器71Aの処理ST11が開始されると、ステップS50は、地図Aによる自己位置推定器71Aの持つロボット6の初期位置姿勢[xA1, yA1, θA1]を初期化する。ステップS51は、タスクを終了するか否かを判定する。例えば、ロボット6が目的地点(または、ゴール地点)着いて自己推定位置と設定された目的地点が一致した場合、或いは、ユーザからの終了指示があれば、ステップS51の判定結果はYESとなり、処理は終了する。一方、ステップS51の判定結果がNOであると、処理はステップS52へ進む。ステップS52は、オドメトリ部21からオドメトリ情報を入力することでロボット6の現在位置を得る。ステップS52が入力するオドメトリ情報に含まれる位置と姿勢のデータ[xOt, yOt]のうち、tはステップS51〜S55のループの実行回数を示す。
【0056】
ステップS53は、ステップS52で入力したオドメトリ情報と、前回入力したオドメトリ情報との差分を算出し、自己位置推定器71Aが前回推定した位置と姿勢のデータにこの差分を足して位置と姿勢のデータを更新することで自己位置推定器71Aの内部状態を予測する。ステップS54は、センサ22Aからの計測データを入力する。ステップS55は、センサ22Aからの計測データに基づいて生成され記憶部32Aに記憶された地図Aと、センサ22Aで計測された計測データとに基づきロボット6の現在位置と姿勢のデータ[xAt, yAt]を推定し、ロボット6の位置と姿勢のデータをこのデータ[xAt, yAt]に更新する。ステップS56は、ステップS55で推定された現在位置と姿勢のデータ[xAt, yAt]のうち、現在位置の推定値、すなわち、推定された自己位置PAを抽出し、処理は座標変換部81の処理ST13へ進む。
【0057】
一方、自己位置推定器71Bの処理ST12が開始されると、ステップS70〜S76がステップS50〜S56と同様の処理をセンサ22Bに対して行う。ステップS76の後、ステップS77は、推定結果である、自己位置推定器71Bで推定した地図Bの座標系でのロボット6の自己位置PBを統合部83へ通知し、処理は統合部83の処理ST14へ進む。
【0058】
座標変換部81の処理ST13において、ステップS61は、自己位置推定器71Aで推定した地図Aの座標系でのロボット6の自己位置PAを、記憶部44から読み出した地図Aから地図Bへの座標変換パラメータを用いて地図Bの座標系(すなわち、基準座標系)の自己位置PA'へ変換する。変換後の自己位置PA'は、位置姿勢xAと地図Aで推定したロボット6の位置の共分散行列ΣAで表される。ステップS62は、ステップS61の座標変換により求めた自己位置PA'、すなわち、位置姿勢xAと共分散行列ΣAを例えば座標変換部81内の記憶部に保存する。ステップS63は、推定結果である変換後の自己位置PA'を統合部83へ通知する。
【0059】
統合部83の処理ST14において、ステップS81は、推定結果の通知を待ち、ステップS63またはステップS77から推定結果の通知があれば例えば統合部83内の記憶部(例えば、共有メモリ)に記憶する。また、ステップS81は、推定結果を共有メモリに記憶すると、共有メモリの更新があることを示す更新フラグを設定する。ステップS82は、更新フラグが設定されているか否かを判定することで、推定結果が新たに通知されたか否かを判定する。ステップS82の判定結果がNOであると、処理はステップS81へ戻る。一方、ステップS82の判定結果がYESであると、ステップS83は、新たに通知された推定結果について上記の如き統合処理を行い、統合処理の結果を例えば統合部83内の記憶部に記憶する。また、必要に応じてステップS84を行っても良い。ステップS84は、統合処理の結果を経路計画部84などの処理部に供給する。
【0060】
このように、本実施例では、ロボットを実環境下で移動させ、ロボットに取り付けられている複数の異なるセンサで計測した環境情報に基づいて複数の地図を独立して作成した後、各地図に対応してロボットの移動軌跡を推定する。推定したロボットの複数の移動軌跡を使って、複数の地図座標系間の非線形変換パラメータ、非線形座標変換式を求める。つまり、地図座標系間の変換は非線形変換方式によって行い、変換に必要なパラメータは独立して推定した移動軌跡から求めて例えばデータベースとして保存できる。独立して作成した環境地図の座標系は独立しており、座標系間の変換は非線形マッピングによって行う。
【0061】
図8は、上記の非線形マッピングを説明する図である。図8(a)は、ロボット6が実環境下で位置Sから位置Gへ移動する場合を示す。ロボット6が移動中に、センサ22Aが環境情報SAを収集し、センサ22B環境情報SBを収集するものとする。この場合、環境情報SAに基づいて図8(b)に示す環境地図Aを作成し、環境情報SBに基づいて環境地図Bを作成する。同時に、ロボット6の環境地図A上の移動軌跡MLAと、環境地図B上の移動軌跡MLBを推定する。そして、移動軌跡MLA,MLBを用いて、座標系XaOaYaから座標系XbObYbへの座標変換パラメータΘを求める。つまり、座標変換パラメータに、推定した移動軌跡MLA,MLBを用いる。
【0062】
【数3】
【0063】
非線形変換方式は特に限定されないが、ガウスプロセスを利用することもできる。ガウスプロセスパラメータの最適化を行う方法自体は周知であり、特に限定されない。
【0064】
座標変換式には、例えばGPRを使用する。GPRで必要とするカーネル関数は、以下のようにサブカーネル関数の組み合わせからなるものでも良い。
【0065】
カーネル関数=RBFカーネル関数+線形カーネル関数+バイアスカーネル関数
また、ロボットの位置姿勢を以下の(1)〜(4)のように変換する。
(1)上記GPRを使用してロボット位置の基準座標系への座標変換を行う。
(2)上記アンセンテッド変換を利用してロボット位置の共分散行列を変換する。具体的には、ロボット位置の元の座標系でシグマポイントを求めて、基準座標系への座標変換を行い、変換されたシグマポイントの座標を用いて変換後の共分散行列を計算する。
(3)ロボットの現在位置を変換先の基準座標系へ変換することで、ロボット姿勢を変換する。具体的には、ロボットを微小距離移動させた場所での位置座標を基準座標系に変換し、基準座標系に変換された後の移動前と移動後の2つの位置座標によってarctanを計算して基準座標系におけるロボットの姿勢を求める。
(4)上記アンセンテッド変換を利用してロボット姿勢の分散を変換する。具体的には、ロボットの姿勢のシグマポイントを求めて、各シグマポイントについて基準座標系への座標変換を行い、各シグマポイントの対応する変換先の基準座標系での姿勢からロボット姿勢の分散を計算する。
【0066】
このように、ロボットの自律移動時の自己位置推定は、各地図で推定したロボットの位置を上記非線形変換方式の変換とパラメータに従って行う。ロボットを実環境下で自律移動をさせるために、ロボットに取り付けられている複数のセンサが独立して環境データを収集し、作成済みの各地図とマッチングすることによりロボットの現在位置の座標を推定する。異なる座標系を持つ複数の地図を用いることにより、異なる座標系でのロボット位置座標が推定できる。異なる位置姿勢の座標間変換は、上記非線形変換方式の変換とパラメータに従って行う。
【0067】
また、各地図で推定した夫々の姿勢値の変換は、座標系の異なる複数の地図により推定されたロボット姿勢(角度)の間の相互変換を、変換元の座標系でのロボット位置とロボット向きの夫々任意の一点を変換先の基準座標系に変換し、変換後の2点のベクトルから求めることで行う。さらに、各地図で推定した夫々の位置姿勢共分散行列の変換は、座標系の異なる複数の地図により推定されたロボット位置姿勢に関しての共分散行列の間の座標系変換をアンセンデット変換により行う。
【0068】
次に、上記実施例で求められるロボット6の推定経路を、図9乃至図13と共に説明する。
【0069】
図9は、ロボットの一例を示す側面図である。図9に示すロボット6には、センサ22A,22Bが設けられている。センサ22Aの地面(または、床)からの高さ位置は例えば50cmであり、センサ22Bの地面からの高さ位置は例えば20cmである。
【0070】
図10は、センサの計測範囲を説明する図である。図10中、(a)はロボット6とセンサ22Aの図9中の矢印で示す線に沿った断面の平面図を示し、破線で囲まれた範囲はセンサ22Aの計測範囲を示す。同様に、図10中、(b)はロボット6とセンサ22Bの図9中の矢印で示す線に沿った断面の平面図を示し、破線で囲まれた範囲はセンサ22Bの計測範囲を示す。
【0071】
図11は、センサの計測情報に基づき作成した地図の一例を示す図である。図11中、(a)はセンサ22Aの計測情報に基づいて図1の地図生成部31Aが作成した地図Aの一例を示し、MLAは地図生成部31Aがセンサ22Aの計測情報を収集することで推定されるロボット6の移動軌跡の一例を示す。同様に、図11中、(b)はセンサ22Bの計測情報に基づいて図1の地図生成部31Bが作成した地図Bの一例を示し、MLBは地図生成部31Bがセンサ22Bの計測情報を収集することで推定されるロボット6の移動軌跡の一例を示す。移動軌跡MLA,MLBは、物理空間内では同じであるが、夫々の地図A,B上では座標系が異なるため異なる。
【0072】
ロボット6の1回の移動では、物理的な移動軌跡は1つしかない。異なる地図A,Bの場合、推定した移動軌跡MLA,MLBの座標値の系列が異なる。推定した移動軌跡MLA,MLBが直接的に地図A,Bから由来したものであり、移動軌跡MLA,MLB間の座標変換関係を求めれば、地図A、B間の座標変換を近似することができる。そこで、上記実施例では移動軌跡MLA,MLBの情報に着眼した座標変換を行う。
【0073】
なお、地図A,B全体の回転成分があるだけではなく、局所的に、500A,500Bで示すような歪みがある。そこで、座標変換を行う際、地図A,B全体の回転成分に注目するだけではなく、局所的な歪みを補正する。
【0074】
図12は、作成した地図上のロボットの移動軌跡と、基準地図へ変換後の移動軌跡を説明する図である。図12中、(a)は図5の自己位置推定器71Aにより推定されたロボット6の地図A上の自己位置PAから推定された移動軌跡MLAを示し、グリッドは地図Aの座標系上のグリッドである。これに対し、図12中、(b)は自己位置推定器71Aにより推定されたロボット6の地図A上の自己位置PAを図5の座標変換部81により地図Bの座標系の自己位置PA'に変換し、変換後の自己位置PA'から推定されたロボット6の移動軌跡MLA'(すなわち、移動軌跡MLAを地図Bの座標系に変換した移動軌跡MLA')を示し、グリッドは地図Bの座標系上のグリッドである。
【0075】
図13は、推定経路の誤差を説明する図である。図13中、MLAは地図Aを用いて図5の自己位置推定器71Aが推定した自己位置PAから推定されたロボット6の移動軌跡、MLBは自己位置推定器71Bが推定した自己位置PBから推定されたロボット6の移動軌跡、MLA'は自己位置推定器71Aにより推定されたロボット6の地図A上の自己位置PAを座標変換部81により地図Bの座標系の自己位置PA'に変換し、変換後の自己位置PA'から推定されたロボット6の移動軌跡を示す。
【0076】
ロボット6が自律移動する際、例えば各地図A,Bに基づいて自己位置推定を行う。そこで、地図Aに基づき推定した移動軌跡MLAを基準地図Bに基づき推定した移動軌跡MLBと直接比較した場合、移動軌跡上の各ポイントの位置誤差の平均は0.52m、標準偏差は0.42mであった。これに対し、地図Aに基づき推定した移動軌跡MLAを基準地図Bの座標系に変換してから変換後の移動軌跡MLA'を基準地図Bに基づき推定した移動軌跡MLBと比較した場合、移動軌跡上の各ポイントの位置誤差の平均は0.21m、標準偏差は0.14mであった。これにより、図13に示す移動軌跡MLA,MLA',MLBの比較から、上記実施例の如き座標変換処理を行うことにより、独立して作成された複数の地図(この例では地図A,B)の座標系が異なる場合であっても、高精度にこれら複数の地図の座標系を統合することが可能になることが確認された。
【0077】
以上の実施例を含む実施形態に関し、更に以下の付記を開示する。
(付記1)
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定処理と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定処理と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定処理により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換処理
をコンピュータに実行させることを特徴とする、地図処理方法。
(付記2)
前記第1の推定処理により推定され前記変換処理により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定処理により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合処理
を更に前記コンピュータに実行させることを特徴とする、付記1記載の地図処理方法。
(付記3)
前記変換処理は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、付記1または2記載の地図処理方法。
(付記4)
前記変換処理は、前記第1の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記1乃至3のいずれか1項記載の地図処理方法。
(付記5)
前記変換処理は、前記ロボットを微小距離移動させた場所での前記第1の座標系の位置座標を前記第2の座標系に変換し、前記第2の座標系に変換後の移動前と移動後の2つの位置座標に基づいて変換後の前記第2の座標系における前記ロボットの姿勢を求めることを特徴とする、付記1乃至4のいずれか1項記載の地図処理方法。
(付記6)
前記変換処理は、前記第1の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記5記載の地図処理方法。
(付記7)
コンピュータに地図を処理させるプログラムであって、
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定手順と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定手順と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定手順により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換手順
を前記コンピュータに実行させることを特徴とする、プログラム。
(付記8)
前記第1の推定手順により推定され前記変換手順により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定手順により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合手順
を更に前記コンピュータに実行させることを特徴とする、付記7記載のプログラム。
(付記9)
前記変換手順は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、付記7または8記載のプログラム。
(付記10)
前記変換手順は、前記第1の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記7乃至9のいずれか1項記載のプログラム。
(付記11)
前記変換手順は、前記ロボットを微小距離移動させた場所での前記第1の座標系の位置座標を前記第2の座標系に変換し、前記第2の座標系に変換後の移動前と移動後の2つの位置座標に基づいて変換後の前記第2の座標系における前記ロボットの姿勢を求めることを特徴とする、付記7乃至10のいずれか1項記載のプログラム。
(付記12)
前記変換手順は、前記第1の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記11記載のプログラム。
(付記13)
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定器と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定器と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定器により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する座標変換部
を備えたことを特徴とする、ロボットシステム。
(付記14)
前記第1の推定器により推定され前記座標変換部により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定器により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合部
を更に備えたことを特徴とする、付記13記載のロボットシステム。
(付記15)
前記座標変換部は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、付記13または14記載のロボットシステム。
(付記16)
前記座標変換部は、前記第1の座標系で推定された前記ロボットの位置の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記13乃至15のいずれか1項記載のロボットシステム。
(付記17)
前記座標変換部は、前記ロボットを微小距離移動させた場所での前記第1の座標系の位置座標を前記第2の座標系に変換し、前記第2の座標系に変換後の移動前と移動後の2つの位置座標に基づいて変換後の前記第2の座標系における前記ロボットの姿勢を求めることを特徴とする、付記13乃至16のいずれか1項記載のロボットシステム。
(付記18)
前記座標変換部は、前記第1の座標系で推定された前記ロボットの姿勢の共分散列の変換をアンセンテッド変換(Unscented Transformation)を利用して行うことを特徴とする、付記17記載のロボットシステム。
(付記19)
前記第1の地図を予め生成して前記記憶部に記憶する第1の地図生成部と、
前記第2の地図を予め生成して前記記憶部に記憶する第2の地図生成部と、
前記座標変換パラメータを予め算出して前記記憶部に記憶するパラメータ算出部
を更に備えたことを特徴とする、付記13乃至18のいずれか1項記載のロボットシステム。
(付記20)
少なくとも前記統合部は、前記ロボットとは別体で前記ロボットと通信可能な情報処理装置に設けらていることを特徴とする、付記14乃至18のいずれか1項記載のロボットシステム。
【0078】
以上、開示の地図処理方法及びプログラム、並びにロボットシステムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。
【符号の説明】
【0079】
1 ロボットシステム
2 センサ部
3 SLAM部
4 地図間マッピング部
7 自己位置推定部
8 統合処理部
21 オドメトリ部
22A〜22C センサ
31A〜31C 地図生成部
32A〜32C 記憶部
71A〜71C 自己位置推定器
81,82 座標変換部
83 統合部
【特許請求の範囲】
【請求項1】
コンピュータに地図を処理させるプログラムであって、
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定手順と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定手順と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定手順により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換手順
を前記コンピュータに実行させることを特徴とする、プログラム。
【請求項2】
前記第1の推定手順により推定され前記変換手順により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定手順により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合手順
を更に前記コンピュータに実行させることを特徴とする、請求項1記載のプログラム。
【請求項3】
前記変換手順は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、請求項1または2記載のプログラム。
【請求項4】
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定器と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定器と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定器により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する座標変換部
を備えたことを特徴とする、ロボットシステム。
【請求項5】
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定処理と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定処理と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定処理により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換処理
をコンピュータに実行させることを特徴とする、地図処理方法。
【請求項1】
コンピュータに地図を処理させるプログラムであって、
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定手順と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定手順と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定手順により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換手順
を前記コンピュータに実行させることを特徴とする、プログラム。
【請求項2】
前記第1の推定手順により推定され前記変換手順により変換された前記ロボットの前記第2の地図上の推定自己位置と、前記第2の推定手順により推定された前記ロボットの前記第2の地図上の推定自己位置とを統合する統合手順
を更に前記コンピュータに実行させることを特徴とする、請求項1記載のプログラム。
【請求項3】
前記変換手順は、推定された前記ロボットの位置の前記第1の座標系をGPR(Gaussian Process Regression)を利用した変換式を用いた前記非線形マッピングにより前記第2の座標系に変換することを特徴とする、請求項1または2記載のプログラム。
【請求項4】
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定器と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定器と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定器により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する座標変換部
を備えたことを特徴とする、ロボットシステム。
【請求項5】
自律移動型ロボットに取り付けられた第1のセンサの計測情報に基づき予め生成され記憶部に記憶された第1の地図と、前記ロボットの各位置での前記第1のセンサの計測情報とに基づいて前記ロボットの前記第1の地図上の自己位置を推定する第1の推定処理と、
前記ロボットに取り付けられた第2のセンサの計測情報に基づき予め生成され前記記憶部に記憶された第2の地図と、前記ロボットの各位置での前記第2のセンサの計測情報とに基づいて前記ロボットの前記第2の地図上の自己位置を推定する第2の推定処理と、
前記第1のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第1の地図上の第1の移動軌跡と前記第2のセンサの計測情報に基づき予め推定され前記記憶部に記憶された前記ロボットの前記第2の地図上の第2の移動軌跡とに基づいて予め算出された、前記第1の移動軌跡の第1の座標系を前記第2の移動軌跡の第2の座標系へ非線形マッピングする座標変換パラメータに基づいて、前記第1のセンサの計測情報に基づいて前記第1の推定処理により推定された前記ロボットの前記第1の地図上の自己位置を前記第2の地図上の自己位置に変換する変換処理
をコンピュータに実行させることを特徴とする、地図処理方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【公開番号】特開2012−242967(P2012−242967A)
【公開日】平成24年12月10日(2012.12.10)
【国際特許分類】
【出願番号】特願2011−110740(P2011−110740)
【出願日】平成23年5月17日(2011.5.17)
【国等の委託研究の成果に係る記載事項】(出願人による申告)平成23年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能(サービス産業分野)の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第19条の適用を受ける特許出願
【出願人】(000005223)富士通株式会社 (25,993)
【Fターム(参考)】
【公開日】平成24年12月10日(2012.12.10)
【国際特許分類】
【出願日】平成23年5月17日(2011.5.17)
【国等の委託研究の成果に係る記載事項】(出願人による申告)平成23年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能(サービス産業分野)の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第19条の適用を受ける特許出願
【出願人】(000005223)富士通株式会社 (25,993)
【Fターム(参考)】
[ Back to top ]