JP4352148B2

JP4352148B2 - Mobile robot mapping system

Info

Publication number: JP4352148B2
Application number: JP2003070782A
Authority: JP
Inventors: 正裕友納
Original assignee: Japan Science and Technology Agency; National Institute of Japan Science and Technology Agency
Current assignee: Japan Science and Technology Agency; National Institute of Japan Science and Technology Agency
Priority date: 2003-03-14
Filing date: 2003-03-14
Publication date: 2009-10-28
Anticipated expiration: 2023-03-14
Also published as: JP2004276168A

Description

【０００１】
【発明の属する技術分野】
本発明は、移動ロボットが自らの移動に利用する地図情報を作成するための地図作成システムに関する。
【０００２】
【技術的背景】
移動ロボットが移動する環境の地図の構築は、移動ロボットにおいて重要な課題である。この地図は、移動ロボットが走行するために使用し、ロボットの走行可能領域を示すとともに、ロボットの自己位置推定に必要なランドマークの配置を知らせる。移動ロボットにより地図を構築するには、ロボットの自己位置がわかっていなければならず、また、自己位置を知るためには地図がなければならない。そのため、両者を同時に推定するＳＬＡＭ（Simultaneous Localization and Mapping）が研究されている（例えば非特許文献１）。
従来の方法として、移動ロボットが移動するための地図を環境の測定データに基づいて作成するには、移動ロボットにセンサを搭載して環境を計測し、ロボットの移動量とセンサの測定データを組み合わせて物体の位置と方位を計算して地図を作成する方法がある。
この方法の概要を以下に記す。なお、以下では、位置と方位をまとめて姿勢と呼ぶ。
【０００３】
まず、ロボットの移動量は、移動ロボットが車輪型であれば、左右の車輪の回転数を測定すれば、ロボットの進行方向の速度と回転角速度が計算でき、これを時間で積分することでロボットの姿勢を求めることができる（例えば非特許文献２）。なお、本発明では移動ロボットという表現を用いているが、車輪の回転数から移動量を測定できれば、人間が押す車両でもかまわない。また、車輪型でなく脚型の移動ロボットの場合も、関節角の変位量からロボットの姿勢を計算することができる。ロボットから見たセンサの相対姿勢はあらかじめ測定できるので、ロボットの姿勢がわかれば、センサの姿勢も計算できる。このことから、以下では、センサの姿勢とロボットの姿勢は同義とする。
【０００４】
センサによる測定データは、センサの種類により様々である。たとえば、レーザ距離センサでは、センサから対象物体までの距離と方向を測ることができる。また、超音波センサもセンサから対象物体までの距離とおおまかな方向を測ることができる。カメラ画像を用いる場合は、ステレオ立体視の原理により、カメラから対象物までの距離と方向を求めることができる。以上の３つの場合は、対象物体に対するセンサデータは、一般に、点の集合となる。また、カメラ画像の場合は、物体のモデルとの照合によって、画像中の物体を認識し、カメラから見た物体の相対姿勢を求めることもできる。この場合は、どのセンサでも、センサから見た対象物体の相対姿勢を求めることができるが、得られるデータの形態や性質は一般に異なる。
【０００５】
ロボットの移動量とセンサの測定データから地図を作るには、まず、ロボットの走行中に、ロボットの移動量の計測とセンサの計測を同期させてデータを記録する。次に、ロボットの移動量を累積してセンサの絶対座標系での姿勢を計算し、求めたセンサの絶対姿勢にセンサの測定データを加えて対象物体の絶対座標系での姿勢を計算する。これをセンサの全測定データについて繰り返すことで、計測した物体の配置が決まり、地図を作ることができる。
このような方法で作った地図は、ロボットの移動量やセンサの誤差のため、正しい形状が得られないことが多い。そのため、ロボットの移動量やセンサデータの誤差を確率分布で表し、センサデータをもとにロボット姿勢や物体姿勢の確率分布を推定しながら、地図全体の誤差が最小となるような配置を求めることが行われている。例えば、ロボットと物体の姿勢を絶対座標系のうえで定義し、ロボットの移動量とセンサ測定データから物体姿勢の確率分布を推定し、ロボットが同じ物体を計測した場合は、ロボットの経路のループを閉じる処理を行うことで地図の誤差を縮小する（例えば非特許文献３参照）。
【０００６】
【非特許文献１】
J. J. Leonard and H. F. Durrant-Whyte: "Simultaneous Map Building and Localization for an Autonomous Mobile Robot," Proc. of IROS'91, pp.1442-1447, 1991.
【非特許文献２】
米田、坪内、大隅著「はじめてのロボット創造設計」、講談社、22〜25頁
【非特許文献３】
S.Thrun, W.Burgard, and D.Fox: "A Real-Time Algorithm for Mobile Robot Mapping with Applications to Multi-Robot and 3D Mapping," Proc. of ICRA2000, 2000.
【０００７】
【発明が解決しようとする課題】
しかしながら、上述の方法では、ロボットの移動量やセンサデータの計測誤差が大きい場合には、十分な精度の地図は得られないという問題がある。確率分布の推定に基づく方法では、ロボットの移動量やセンサデータの誤差分布を仮定しているので、その誤差分布がもともと大きければ、得られる地図の誤差も当然大きくなる。たとえば、ロボットが移動する部屋の地図を作る場合に、机や棚などの家具の向きは重要な情報であるが、センサでその向きを計測するのは、一般に難しい。超音波センサやレーザ距離センサでは、このような家具を認識することは困難であり、たとえば、長方形という形状は知ることができても、どの方向が正面かを知ることはできない。また、カメラ画像から物体を認識する方法ならば、どの方向が正面かを知ることはできるが、その角度の計測値は高い精度が得られないことがある。
また、センサによって、計測できない物体が存在するという問題もある。たとえば、超音波センサやレーザ距離センサは通常、センサが置かれた高さの物体しか見えない。また、細い棒状の物体は検出しにくい。カメラ画像を用いる場合は、透明な物体、複雑な物体などは検出しにくい。
【０００８】
【課題を解決するための手段】
上記の課題を解決するために、本発明は、移動量センサと計測対象の物体の姿勢を計測する姿勢センサを搭載した移動ロボットによる移動ロボット用地図作成システムであって、物体をノード，物体間の相対姿勢をアークとして作成し、該相対姿勢の不確実性を表す確率分布を該アークに付加したグラフにより表される、該移動ロボットが移動する環境の地図情報を格納する地図記憶手段と、前記移動ロボットの移動を測定する移動量センサと、前記移動ロボットの近傍の物体の姿勢を計測し、ロボットの座標系による物体の姿勢情報を得る姿勢センサと、前記移動ロボットが環境中を移動して、前記移動量センサと前記姿勢センサにより得られたデータ列から、物体間の相対姿勢で表される地図情報とロボットの姿勢との同時推定により、物体間の相対姿勢の確率分布を推定する新規地図情報作成手段とを備え、前記新規地図情報作成手段は、前記移動量センサから得た該移動ロボットの移動量と、前記姿勢センサで得られた該移動ロボットの座標系による、今回基準とするある物体の姿勢情報と、以前に基準とした他の物体の座標系によるロボットの姿勢とから、前記ある物体を基準とした座標系によるロボットの姿勢と、前記他の物体と前記ある物体との間の相対姿勢とを確率分布とともに推定することで、同時推定を行うことを特徴とする。
また、前記地図記憶手段は、既存地図情報も記憶しており、記憶している既存地図情報を読み出して新規地図情報と比較し、同じ物体間の相対姿勢を表すアークがあれば、該アークに対する既存地図情報での相対姿勢と、新規地図情報での相対姿勢とを確率的に融合して、前記地図情報手段に新らたな相対姿勢として記憶する既存地図情報融合手段を備えていてもよい。
前記既存地図情報融合手段は、新規地図情報を作成後に起動され、かつ新規地図情報がなければ既存地図情報を新規地図情報として記憶するとよい。
さらに、新規地図情報において相対姿勢に関するループを検出した場合、ループの始点物体からループに沿って、各物体間の相対姿勢を全部足すと始点物体に戻るように、該ループを構成する相対姿勢の変位を計算するループ解決手段を備えていてもよい。
【０００９】
【発明の実施の形態】
本発明の移動ロボットによる移動ロボット用地図作成システムでは、物体を構成単位とした地図を上述のＳＬＡＭの枠組みで構築する。具体的には、フロア平面上に物体モデルを配置した形式で地図を構成し、ロボットは環境内を移動しながら、センサによって物体を認識して、地図を構築していく。従来の地図構築研究の多くはロボットの走行可能領域を記述した地図を対象としており、環境を物体単位に構造化して地図を記述したものは少ない。しかし、例えば、物体の把持や運搬等複雑な作業を行うには、個々の物体を識別してその位置姿勢を知ることが不可欠であり、物体単位に構成された地図が必要になる。
本発明で提案する方式では、物体をノード、物体間の相対姿勢をアークとしたグラフで地図を表現する。相対姿勢アークは、誤差を表すための確率分布を持つ。ロボットは、移動量とセンサデータ（測定データ）により自己位置推定を行ないながら、物体間の相対姿勢を推定して地図を構築していく。
【００１０】
また、本発明の移動ロボットによる地図作成システムは、ロボットが作成した地図（以下、新規地図という）にあらかじめ人間が作成した地図、あるいは、他のセンサで計測した地図（以下、既存地図という）を併合することで新規地図の精度を改善し、また、ロボットに搭載したセンサで検出できなかった物体を登録する。たとえば、人間が家具の向きを設定した既存地図を作って、新規地図に併合すれば、新規地図でも家具の向きは定義される。また、ロボットに搭載したセンサでは検出しにくい物体を既存地図に登録しておけば、新規地図にもその物体を登録することができる。
以下、図面を参照しながら、本発明の移動ロボット用地図作成システムの実施形態の一例を詳細に説明する。
まず、本実施形態のシステム構成を説明する。本システムは、移動量や姿勢を検出するセンサを有し、移動するロボット内等に存在するコンピュータ・システム上に構成されている。
【００１１】
＜地図情報の構成＞
最初に、本実施形態の地図情報の構成を説明する。上述したように、本発明では、作成した地図情報を地図記憶手段に格納している。
図１（ａ）〜（ｃ）の各表は、記憶する地図の情報の構成例（データ表現の例）を示す図である。
図１（ａ）は、地図情報の構成例を示す図である。同図に示すように、地図は物体情報の集合と相対姿勢情報の集合で構成される。
図１（ｂ）は、相対姿勢情報の構成の一例を示す図である。図１（ｂ）に示すように、相対姿勢情報は、その始点となる物体（始点ノード）と終点となる物体（終点ノード）、および、始点物体から見た終点物体の相対姿勢の期待値と分散で構成される。相対姿勢情報から物体を指示するには、次に述べる物体の一意名を用いる。
【００１２】
図１（ｃ）は、物体情報の構成の一例を示す図である。図１（ｃ）に示すように、物体は、その一意名、形状情報、絶対姿勢をもつ。一意名は、各々の物体に一意に付けられた名前である。形状情報および絶対姿勢は、ディスプレイやプリンタなどに地図を出力するために用いる。形状情報は、２次元あるいは３次元の図形として定義する。これは、計算機上での汎用的な図形表示プログラムの形式にしたがったものでよい。絶対姿勢は、地図上の絶対座標系での物体の姿勢を表す。これも、地図をユーザに見やすく表示するために用いる。なお、地図を表示する必要がない場合は、形状情報および絶対姿勢は不要である。
【００１３】
＜本実施形態の処理の概要＞
次に、本実施形態の処理の流れを、図２のフローチャートを参照しながら説明する。本発明は、ロボットが移動して行くにしたがって、計測対象の物体の相対姿勢を計測しながら、地図を作成して行く。
図２において、まず、ステップＳ２１０で、ロボットセンサの測定データから、ロボットの移動量と環境に存在する物体間の相対姿勢を推定して新規地図情報ｍ_１を作成する。なお、新規地図の作成手法については後で詳しく説明する。
次に、ステップＳ２２０で、既存地図情報ｍ_２を読み込んで、既存地図のもつ相対姿勢ｑ_ｉｊを１つずつ取り出す。そして、ステップＳ２３０で、新規地図情報ｍ_１と比較し、ｑ_ｉｊと同じ相対姿勢が新規地図情報ｍ_１にもあるかを調べる。ここで、同じ相対姿勢とは、測定対象の物体が同じであるものを指す。
２つの地図の間で物体が同じかどうかは、物体の一意名が同じかどうかで判定する。ただし、ステップＳ２３０では一意名による同一性判定だけを行い、物体が本当に同じかどうかは判定しない。このため、２つの地図の間で同じ物体は同じ一意名をもつようにしておく必要がある。本実施形態では、あらかじめ他の既存の方法で与えるか、人間が与えることとする。
【００１４】
ステップＳ２３０で、もし新規地図情報ｍ_１に同じ相対姿勢があれば、ステップＳ２４０に進む。新規地図情報ｍ_１に同じ相対姿勢がなければ、ステップＳ２５０に進む。
ステップＳ２４０では、新規地図情報ｍ_１と既存地図情報ｍ_２が共通に持つ相対姿勢を確率的に融合する。なお、相対姿勢の確率的な融合については後で詳しく説明する。
ステップＳ２５０では、既存地図情報ｍ_２にあって新規地図情報ｍ_１にはない相対姿勢を新規地図情報ｍ_１に追加する。このため、まず、該相対姿勢を新規地図情報ｍ_１の相対姿勢集合に加える。次に、該相対姿勢の始点物体および終点物体が新規地図情報ｍ_１にあるかどうか調べ、なければ、その物体を新規地図情報ｍ_１の物体集合に加える。
【００１５】
次に、ステップＳ２６０で、既存地図情報ｍ_２のすべての相対姿勢を取り出したかを調べ、すべて取り出してあれば、ステップＳ２７０に移る。まだ残っていれば、ステップＳ２２０に戻る。ステップＳ２７０では、新規地図情報ｍ_１の相対姿勢集合の中から、ループを構成するものを見つけて、そのずれを解消する処理を行う。なお、ループ解決は、すべての相対姿勢でなく、地図上での現在注目している部分領域に含まれる相対姿勢だけで行ってもよい。たとえば、１つの部屋の内部に限定してループ解決をする場合などはその例である。これにより、計算量を減らすことができる。
【００１６】
なお、本実施形態においては、Ｓ２１０の処理で新規地図情報ｍ_１を作成し終えた後に、Ｓ２２０以降の既存地図情報ｍ_２との比較・融合処理を行なっているが、新規地図情報ｍ_１を作成しながら、既存地図情報ｍ_２との融合処理を行なってもよい。例えば、新規地図情報ｍ_１の作成において物体間の相対姿勢を計測するごとに、既存地図情報ｍ_２から対応する相対姿勢ｑ_ｉｊを選択して融合を行ってもよい。
【００１７】
以降、図２の実施形態を示すフローチャートの各処理を詳しく説明する。
＜新規地図の作成＞
まず、新規地図の作成処理（Ｓ２１０）について、図３，図４を用いて説明する。
（１．相対姿勢による地図表現）
本実施形態では、地図ｍを、物体をノード、計測対象の物体間の相対姿勢をアークとしたグラフｍ＝<Ｅ，Ｑ>で表す。Ｅ＝{ｅ_ｉ}は計測対象の物体の集合，Ｑ＝{ｑ_ｉｊ}は物体の相対姿勢の集合である。各物体にはローカル座標系が付与されているとする。ｑ_ｉｊ＝（ｘ，ｙ，θ）^Ｔとして、ｑ_ｉｊは物体ｅ_ｉのローカル座標系から見た物体ｅ_ｊの相対姿勢を表す。ここで、ｘ，ｙはローカル座標系であるｅ_ｉ座標系から見たｅ_ｊ座標系の原点の位置、θはｅ_ｉ座標系から見たｅ_ｊ座標系のなす角である。また、相対姿勢アークは、人間やロボットにより明示的に測定されたものだけを扱う。相対姿勢には不確実性を持つことを許し、その不確実性を確率分布で表現する。
【００１８】
ロボット姿勢とセンサデータは次のように表す。
（１）ロボット姿勢の表現
ロボット姿勢は、近傍に存在する物体を基準に表現し、ロボットが移動するにしたがって、基準とする物体を切り替える。この物体を基準実体という。基準実体ｅ_ｉの座標系でのロボット姿勢をｒ_ｉ＝（ｘ，ｙ，θ）^Ｔと表す。
ここで、ｘ，ｙはローカル座標系であるｅ_ｉ座標系から見たロボットの位置、θはｅ_ｉ座標系から見たロボットのなす角である。
（２）センサデータの表現
ロボットは、センサにより物体を認識し、その姿勢を推定する。物体ｕがロボット座標系でｑ＝（ｘ，ｙ，θ）^Ｔの姿勢にあることを、ｚ＝<ｕ，ｑ>と表す。
物体認識では物体のクラスしかわからないため、ｕは未同定の物体である。環境中には同じクラスの物体は複数個存在することがありうるため、ｕの同定が必要である。本実施形態では、この個体同定は、物体の一意名を与える際に、他の既存の方法または人間が行う。
なお、物体認識の方式は従来技術である（例えば、「友納，油田：不正確さを許すマップと物体認識による屋内ナビゲーション，第７回ロボティクスシンポジア予稿集，２００２」を参照。）。
【００１９】
図３（ａ）に物体ｅ_１，ｅ_２間の相対姿勢ｑ_１２の一例を示す。上述したように、相対姿勢は不確実性をもつことを許し、その不確実性を確率分布で表現する。この確率分布は任意でよいが、たとえば、正規分布と仮定すれば、平均値と分散で表現できる。本実施形態は、この物体間の相対姿勢を、移動ロボットの物体に対する姿勢センサと移動量センサの測定データで推定していく。
上述したように、物体の姿勢は、ロボットの座標系でｑ＝（ｘ，ｙ，θ）^Ｔが姿勢センサにより得られる。図３（ｂ）にロボット３００に搭載したセンサによる、物体ｅ_１の測定データの一例を示す。ここで、センサから得られる物体ｅ_１の姿勢はｑ＝（ｘ，ｙ，θ）で表されている。センサによってはθが不定の場合もありうる。このセンサデータには誤差が含まれるので、たとえば、正規分布で誤差を表すことができる。この誤差分散は、あらかじめ、センサの特性を調べて決めておく。また、ロボットの移動量は、従来技術と同様に、ロボットの搭載したセンサで求める。移動量も位置と方位を示す（ｘ，ｙ，θ）で表すことができる。移動量には誤差が含まれるので、たとえば、正規分布で誤差を表す。この誤差分散は、あらかじめ、ロボットの特性を調べて決めておく。
【００２０】
（２．地図とロボット姿勢の同時推定）
（１）問題設定
次に、センサから得た物体の姿勢とロボットの移動量とから、２つの物体間の相対姿勢を推定する方法を説明する。
本実施形態では、地図情報の構築を、時点ｔまでにロボットが得たデータ列ＺＺ^（ｔ）（後述で詳しく説明する）のもとで、地図ｍの確率分布Ｐ（ｍ｜ＺＺ^（ｔ））を推定することととらえる。しかし、確率分布Ｐ（ｍ｜ＺＺ^（ｔ））は、全ての相対姿勢アークの同時確率なので、次元が非常に大きく、そのまま扱うのは難しい。そこで、本実施形態では、地図情報全体の確率を管理するのではなく、個々の相対姿勢アークの確率分布を求める問題として扱う。すなわち、Ｐ（ｑ_ｉｊ｜ＺＺ^（ｔ））を求めることにより地図情報を構築する。
【００２１】
１つの相対姿勢アークに対応する地図情報とロボット姿勢の同時推定の模式図を、図４（ａ）および（ｂ）に示す。いま、ロボット３００は点Ｓ_１から点Ｓ_２に移動するものとする。ロボット３００は、点Ｓ_１にいる時点（ｔ_１）から点Ｓ_２にいる時点（ｔ_２）までは物体ｅ_１を基準実体とし、時点ｔ_２で物体ｅ_２を観測して基準実体をｅ_２に切り替える。点Ｓ_１でのロボットの物体ｅ_１からの相対姿勢をｒ^１ _１，点Ｓ_２でのロボットの物体ｅ_１からの相対姿勢をｒ^２ _１とする。また、物体ｅ_１から見た物体ｅ_２の相対姿勢をｑ_１２とする。また、点Ｓ_２でロボットが物体ｅ_２を観測したときのロボットから見た物体ｅ_２の相対姿勢の測定データをｑとする。なお、環境は静的であり、相対姿勢ｑ_１２は時間とともに変化しないものとする。
このような状況で、ロボットは環境を認識しながら走行し、自己位置と地図情報の確率分布を更新する。すなわち、相対姿勢ｒ^１ _１の確率分布はそれまでの計算で得られているとすると、点Ｓ_１から点Ｓ_２までの移動による移動量と物体ｅ_２の測定データとから、点Ｓ_２でのロボットの物体ｅ_２からの相対姿勢（ｒ^１ _２とする）と相対姿勢ｑ_１２の確率分布を求める。
【００２２】
また、点Ｓ_２におけるロボットのｅ_１からの相対姿勢ｒ^２ _１の確率分布は、前述のようにロボットの移動量から計算できる。
このとき、移動量の推定誤差により、図４（ｂ）に示すように、相対姿勢ｒ^２ _１の確率分布４２０は相対姿勢ｒ^１ _１の確率分布４１０に比べて広がる。いま、点Ｓ_２において、ロボット３００が物体ｅ_２を観測し測定データｑを得たとする。この測定データｑは、独自の確率分布を有している。そこで、相対姿勢ｒ^２ _１とｑの和に対する確率分布を計算することにより、物体ｅ_１からの物体ｅ_２の相対姿勢ｑ^２ _１２を推定することができる。実際、ｑ^２ _１２＝ｒ^２ _１＋ｑというベクトル和の関係があるので、ｑ^２ _１２の確率分布は、２つの確率変数ｒ^２ _１とｑの和の確率分布として、畳み込み積分で計算できる。図４（ｂ）においては、相対姿勢ｑ^２ _１２の確率分布を４３０で示している。
以降、上述の確率分布を求める処理を定式化して説明する。
【００２３】
（２）地図情報とロボット姿勢の同時確率
データ列をＺＺ^（ｔ）＝<ｚ^（０），ａ^（０），…，ｚ^{（ｔ−１）}，ａ^{（ｔ−１）}，ｚ^（ｔ）>，ＺＭ^{（ｔ−１）}＝<ｚ^（０），ａ^（０），…，ｚ^{（ｔ−１）}，ａ^{（ｔ−１）}>と表す。ｚ^（ｔ）は時点ｔでのセンサデータ<ｕ^（ｔ），ｑ^（ｔ）>，ａ^{（ｔ−１）}は時点ｔ−１からｔまでのロボットの走行事象を表す。なお、以下では、ｑ^（ｔ）を単にｑと表す。ロボットは、基準実体をその時点で認識された物体に設定し、１つの走行事象中は変えないとする。
なお、前述したように、本実施形態において個体は一意名により同定できることを前提とする。以下では、センサデータ<ｕ^（ｔ），ｑ^（ｔ）>のｕ^（ｔ）は物体ｅ_１や物体ｅ_２に同定されているとして、式からは暗黙的に除いている。
【００２４】
まず、相対姿勢アークｑ_１２とロボット姿勢ｒ^１ _２の同時確率Ｐ（ｑ_１２，ｒ^１ _２｜ＺＺ^（ｔ））は、ベイズの定理を用いて以下の式（１）で計算される。なお、ｒ^１ _２は点Ｓ_２でのロボットの物体ｅ_２からの相対姿勢である。
【数１】

…式（１）
ここで、αは正規化係数である。また、右辺第一項で、Ｐ（ｑ｜ｒ^１ _２，ｑ_１２，ＺＭ^{（ｔ−１）}）＝Ｐ（ｑ｜ｒ^１ _２）としている。ＺＭ^{（ｔ−１）}の省略は、マルコフ性の仮定による。ｑ_１２の省略は、ｑがロボットと物体ｅ_２との相対姿勢ｒ^１ _２だけで決まるため、ｑ_１２に依存しないことによる。
【００２５】
また、式（１）のＰ（ｑ_１２，ｒ^１ _２｜ＺＭ^{（ｔ−１）}）は、ロボットがｒ^１ _１からｒ^２ _１にまで走行して、その後基準実体を物体ｅ_２に変換したと考えると、以下の式（２）のように変形できる。
【数２】

…式（２）
ここで、Ｐ（ｒ^１ _２｜ｒ^２ _１，ｑ_１２，ＺＭ^{（ｔ−１）}）はｒ^１ _２，ｒ^２ _１，ｑ_１２の関係だけで決まるため（δ関数になる）、ＺＭ^{（ｔ−１）}は省略できる。
【００２６】
式（２）のＰ（ｑ_１２，ｒ^２ _１｜ＺＭ^{（ｔ−１）}）は以下の式（３）で表される。
【数３】

…式（３）
式（３）のＰ（ｒ^２ _１｜ｒ^１ _１，ａ^{（ｔ−１）}）は、ｒ^１ _１から走行ａ^{（ｔ−１）}を行なったときに、ｒ^２ _１に到達する確率である。各物体の近傍の底面の状態が同じで障害物もないと仮定すれば、この確率は地図情報にもそれまでのデータにも依存せず、走行ａ^{（ｔ−１）}に伴う移動量の測定結果だけから計算できる。一方、Ｐ（ｑ_１２，ｒ^１ _１｜ａ^{（ｔ−１）}，ＺＺ^{（ｔ−１）}）のａ^{（ｔ−１）}はｒ^１ _１からｒ^２ _１までの走行であるから、ｑ_１２，ｒ^１ _１に影響しないので省略した。また、ｑ_１２とｒ^１ _１は独立であると仮定し、Ｐ（ｑ_１２，ｒ^１ _１｜ＺＺ^{（ｔ−１）}）を分解した。
【００２７】
上記の式（１）〜（３）をまとめると、ロボットが１つの移動と物体の認識を行なう１ラウンドにおいて、以下のように分布が更新される。
【数４】

…式（４）
これより、１つ前のロボット姿勢の分布と相対姿勢アークの分布を保存しておけば、現時点でのロボット姿勢と相対姿勢アークの同時分布を計算できる。
【００２８】
（３）ロボット姿勢の推定
上記の式（４）から、ロボット姿勢ｒ^１ _２の確率分布は次のように計算できる。
【数５】

…式（５）
【００２９】
（４）相対姿勢アークの推定
また、確率分布Ｐ（ｑ_１２｜ＺＺ^（ｔ））は、以下のように計算できる。
【数６】

…式（６）
式（５）および式（６）において、Ｐ（ｑ｜ｒ^１ _２）はロボットの物体姿勢センサの誤差特性で決まり、また、Ｐ（ｒ^２ _１｜ｒ^１ _１，ａ^{（ｔ−１）}）はロボットの移動量センサの誤差特性で決まる。Ｐ（ｒ^１ _２｜ｒ^２ _１，ｑ_１２）は、ベクトル和（ｒ^１ _２＋ｑ_１２）がｒ^２ _１に等しくなるとき１となり、それ以外は０である。
Ｐ（ｑ_１２｜ＺＺ^{（ｔ−１）}）は、相対姿勢ｑ_１２が既存地図に存在する場合は、その確率分布を使う。既存地図に存在しない場合は、一様分布とする。Ｐ（ｒ^１ _１｜ＺＺ^{（ｔ−１）}）は、前述のように、それ以前の処理で値が求められているとする。
以上の確率分布を用いて、式（５）および式（６）を計算する。
しかし、式（５）および式（６）は積分を含むため、一般に計算は困難である。そのため、従来技術であるparticle filterを用いた自己位置推定アルゴリズムを拡張して実装した。なお、この従来技術については例えば「友納，油田：”不正確さを許すマップにおける移動ロボットの自己位置推定”，日本ロボット学会誌，VOL.20, No.4, pp.75-86, 2002」を参照されたい。
【００３０】
＜新規地図と既存地図の併合＞
次に、新規地図と既存地図の併合について、図５を用いて説明する。これは、上述の図２のＳ２２０〜Ｓ２６０で示した処理であり、上述で移動ロボットが作成した新規地図情報に、例えば人間が作成した既存地図情報を併合して、より精度の高い地図情報を作成することを目的としている。
【００３１】
（１．相対姿勢の融合）
既存地図に物体ｅ_１とｅ_２間の相対姿勢が定義されていれば、これを用いて、上述で推定される相対姿勢ｑ_１２の精度を上げることができる。
人間が作成した既存地図と移動ロボットが作成した新規地図の両地図が共通の相対姿勢アークをもつ場合、その相対姿勢を確率的に融合する。これは、上述の式（６）において、Ｐ（ｑ_１２｜ＺＺ^{（ｔ−１）}）を既存地図で与え、それに測定データを加えてＰ（ｑ_１２｜ＺＺ^（ｔ））を計算することで実現できる。
【００３２】
図５にその様子を示す。図５（ａ）は、既存地図で定義された物体ｅ_１から見た物体ｅ_２の相対姿勢ｑ^１ _１２の確率分布５１０を示す図である。この例では、物体ｅ_１と物体ｅ_２は直線上に同じ向きに並んでいるが、距離は不確実である。このような定義は、壁に沿って並んでいる物体をおおまかに測定して手作業で記述した場合などに現われる。
そして、図５（ｂ）に示すように、測定データを相対姿勢ｑ^１ _１２に融合することで、より正確な相対姿勢ｑ^３ _１２（図５（ｂ）では５２０で示している）を得ることができる。具体的には、図５（ａ）のｑ^１ _１２の確率分布５１０をＰ（ｑ_１２｜ＺＺ^{（ｔ−１）}）の値として式（６）を計算すればよい。
【００３３】
さらに、図５（ｃ）に示すように、ロボット姿勢も更新する（図では、更新値を確率分布５３０で表している）。具体的には、やはり図５（ａ）のｑ^１ _１２の確率分布５１０をＰ（ｑ_１２｜ＺＺ^{（ｔ−１）}）の値として、式（５）を計算すればよい。
なお、このようにして更新されたロボット姿勢ｒ^１ _２は、基準実体がｅ_１からｅ_２に変わっている。ロボットがｅ_２から次の物体に向かうときは、その物体に対して上記と同様の処理を繰り返す。
以上により、計測した物体間の相対姿勢を求めることができる。
一方、既存地図の相対姿勢のうち、新規地図にはないものがあった場合、相対姿勢の融合を行うことはできないので、該相対姿勢を新規地図に追加する。
【００３４】
（２．ループとしての融合）
既存地図が新規地図に存在しない相対姿勢アークを持つ場合は、上述した姿勢分布の融合を直接行なうことはできない。しかし、両者のアークを合わせたことにより、ループが生じることがある。この場合、ループを通じて既存地図の相対姿勢アークが新規地図への制約条件として働き、地図情報の精度を向上させることができる。
平行あるいは垂直な位置関係にある物体や、近接した物体の間の相対姿勢は、人間が容易に定義できる上、有効な幾何制約となる。このような制約を表す相対姿勢アークを既存地図に定義しておき、新規地図と併合してループの解決を行なうことで、地図情報の精度を向上させることができる。
なお、ループの解決については、以下で説明する。
【００３５】
＜ループの解決＞
次に、ループの解決について詳しく説明する。これは、上述の図２のＳ２７０で示した処理である。
作成された地図情報を移動ロボットが使用したり、ユーザに表示したりする際には、上述で求めた各相対姿勢アークの確率分布の期待値や最大確率点を代表値として用いて、具体的な地図を作ることになる。しかし、個々の相対姿勢アークの代表値をつなげて作った地図には、データ誤差による歪みが生じる。そこで、相対姿勢アーク間に制約を導入してこの歪みを解消することを考える。本実施形態では、このためにループ制約を用いる方法を使用する。
【００３６】
（１．定式化）
物体間の相対姿勢の連鎖は一般にループを構成するが、種々の誤差のため、ループがうまく閉じないという現象が生じる。
図６に、ループの例を示す。図６（ａ）は相対姿勢に整合性がとれている場合の地図である。この場合、相対姿勢ｑ_１〜ｑ_４をたどると物体ｅ_１が元の姿勢に戻り、ループが閉じている。
一方、図６（ｂ）は相対姿勢に整合性がとれていない歪んだ場合の例である。相対姿勢ｑ_１〜ｑ_４をたどると同じ物体ｅ_１が最初と異なる姿勢（６１０で示している）になり、ループが閉じない。この現象は、上述したように、相対姿勢の値に誤差があるため、その期待値を代表値として採用すると、たどる相対姿勢の列によってｅ_１の絶対座標系での姿勢が食い違うために起こる。
この相対姿勢間の不整合を解消するために、本実施形態ではループを閉じる処理を行う。
【００３７】
いま、地図にｎ個の相対姿勢アークが含まれるとし、その各々をｑ_ｉ＝（ｘ_ｉ，ｙ_ｉ，θ_ｉ）^Ｔ，１≦ｉ≦ｎと表す。マップにあらかじめ定義されたｑ_ｉや、ロボットが求めたｑ_ｉの推定値をｑ'_ｉ、その共分散行列をＲ_ｉとする。
本実施形態で提案する方法では、ループ解決を、ループ制約を満たしながら推定値ｑ'_ｉとの誤差の二乗和を最小にする相対姿勢ｑを求める最適化問題として考える。ただし、ｑ＝｛ｑ_１，…，ｑ_ｎ｝である。この定式化を式（７）〜式（１０）に示す。
【００３８】
【数７】

…式（７）
【数８】

…式（８）
【数９】

…式（９）
【数１０】

…式（１０）
【００３９】
式（８）のｇ_ｊ（ｑ）はｊ番目のループに対する制約式である。ループｊはｎ_ｊ個のアークからなり、ｑ_ｊ１，ｑ_ｊ２，…，ｑ_ｊｎｊの順にアークが並んでいるものとする。ｑ_ｊ１をループの始点とする。ｇ_ｊ（ｑ）＝０は、ループの始点物体からループに沿って各物体間の相対姿勢を全部足すと始点物体に戻らなければならないという制約を表している。Ｌは地図に含まれる独立なループの個数である。また、Ｍ（φ_ｊｋ）は相対姿勢アークｑ_ｊｋをループの始点物体の座標系に変換する行列である。
【００４０】
（２．実装）
上記の問題は非線形最小化問題となる。実装にはparticle filterを用いたが非線形最小化問題を解くための他の方法もある。ここでのparticle filterは、上述で用いたようなマルコフ過程の各時点の分布を求めるものとは違い、最小値を求めるために用いる。このときのparticle filterは、Genetic Algorithmによく似た作用をする。
なお、ここでのparticle filterについては、例えば「J.Deutshcher, A.Blake, and I.Reid: "Articulated Body Motion Capture by Annealed Particle Filtering" Proc. of CVPR2000, 2000」に詳しい。
以下に、手順を示す。各particleは、ｑに含まれる全変数を持つ。評価値として、Ｅ（ｑ）＝ｆ（ｑ）＋Σ_ｊ‖ｇ_ｊ（ｑ）‖を用いる。
（１）式（６）で得られた各相対姿勢を初期値として、particleを発生させる。
（２）particleの各変数値を所定の正規分布に従ってランダムに変動させ、評価値Ｅ（ｑ）を計算する。最小の評価値をもつparticleを保存しておく。各particleの重みをｅ^{−Ｅ（ｑ）}に比例した値にする。
（３）重みに比例した確率で、particleをリサンプルし、（２）に戻る。
（４）所定の回数だけ（２）（３）を繰り返した後、最小の評価値を得たparticleのもつ相対姿勢値を解とする。
【００４１】
以上のように、新規地図に既存地図を併合することで、地図の精度を改善することができる。たとえば、人間が家具の向きを設定した既存地図を作って、新規地図に併合すれば、新規地図でも家具の向きは定義される。また、ロボットに搭載したセンサで検出しにくい物体を既存地図に登録しておけば、新規地図にもその物体を登録することができる。
なお、既存地図は、ロボットが以前に作った地図でも人間が作った地図でもよいが、人間が既存地図を作る場合は、相対姿勢を定性的に表現すると、人間の負担が小さくてすむ。たとえば、２つの机が同じ向きで平行に並んでいる場合は、その間の相対姿勢の角度成分θの期待値を０度にし、分散を小さくする。また、向きが垂直ならば、θの期待値を９０度にし、分散を小さくする。このような定性的な幾何学的関係を相対姿勢の定量値に変換するユーザインタフェースを用意することで、人間の負担を大きく減らすことができる。とくに、屋内地図の場合は、物体の配置が平行、垂直、隣接などの規則正しい幾何学的関係に従うことが多いので、この方法は効果が大きい。
【００４２】
上述のように、本実施形態は、物体の姿勢を相対姿勢で表した上で、地図の併合を行う点を特徴とする。仮に物体姿勢を絶対姿勢で表すと、一般に絶対座標系の原点から遠くなるほど誤差分散が大きくなり、確率分布の推定精度は悪くなる。また、人間が既存地図を作成する場合は、任意に基準とする物体を決めて、そこからの相対姿勢で他の物体の姿勢を定義する方が容易である。さらに、相対姿勢を使えば、全体地図ではなく、局所領域を示す部分地図を既存地図として利用することも容易になる。前述の従来技術の方法（非特許文献３）では、地図の併合は行われているが、物体姿勢として絶対姿勢を用いているので、上記の問題には対処していない。
【００４３】
なお、本発明の地図作成システムは、上記の実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々変更を加え得ることができる。例えば、上述では移動ロボット内に存在するコンピュータ・システム上に構築されるとして説明したが、他の場所にあり、移動ロボットとのデータをやり取りできるように構成したコンピュータ・システム上に構築してもよい。
【００４４】
【発明の効果】
上述したように、本発明の移動ロボットによる地図作成方法によれば、ロボットの移動量とセンサの測定データに基づいて、物体の相対姿勢により地図を確率的に作成する。これにより、誤差の少ない（精度の高い）地図情報を作成することができる。
そして、作成した地図を、既存地図と併合することで修正できるため、地図の精度をさらに上げることができる。特に、既存地図として、平行、垂直、隣接といった物体間の定性的な姿勢関係で表した大まかな地図を利用できるため、既存地図を人間が作る場合でも、その負担は小さくて済む。
また、本発明では、既存地図の相対姿勢が新規地図に存在しない場合は、その相対姿勢に付随する物体も新規地図に追加される。これは、センサで計測しにくい物体を地図に登録できるという効果をもたらす。
【図面の簡単な説明】
【図１】本実施形態の地図情報のデータ表現の一例を示す図である。
【図２】本実施形態の地図作成方法の処理手順を示すフローチャートである。
【図３】物体間の相対姿勢、および、センサによる物体の姿勢測定データの一例を示す図である。
【図４】物体間の相対姿勢の推定方法の一例を示す図である。
【図５】物体間の相対姿勢の融合方法の一例を示す図である。
【図６】相対姿勢のループの一例を示す図である。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a map creation system for creating map information used by a mobile robot for its own movement.
[0002]
[Technical background]
Construction of an environment map in which a mobile robot moves is an important issue for mobile robots. This map is used by the mobile robot to travel, shows the travelable area of the robot, and informs the arrangement of landmarks necessary for the self-position estimation of the robot. In order to construct a map by a mobile robot, the robot's self-location must be known, and a map must be present in order to know the self-location. Therefore, SLAM (Simultaneous Localization and Mapping) that estimates both at the same time has been studied (for example, Non-Patent Document 1).
As a conventional method, to create a map for moving a mobile robot based on environmental measurement data, a sensor is mounted on the mobile robot, the environment is measured, and the movement amount of the robot is combined with the measurement data of the sensor. There is a method of creating a map by calculating the position and orientation of the object.
The outline of this method is described below. In the following, the position and orientation are collectively referred to as a posture.
[0003]
First, if the mobile robot is a wheel type, the robot's moving direction speed and rotational angular velocity can be calculated by measuring the number of rotations of the left and right wheels. Can be obtained (for example, Non-Patent Document 2). Although the expression “mobile robot” is used in the present invention, a vehicle pushed by a human may be used as long as the amount of movement can be measured from the number of rotations of the wheels. Also, in the case of a legged mobile robot instead of a wheel type, the posture of the robot can be calculated from the displacement amount of the joint angle. Since the relative orientation of the sensor as seen from the robot can be measured in advance, if the orientation of the robot is known, the orientation of the sensor can also be calculated. Therefore, hereinafter, the sensor posture and the robot posture are synonymous.
[0004]
The data measured by the sensor varies depending on the type of sensor. For example, a laser distance sensor can measure the distance and direction from the sensor to the target object. The ultrasonic sensor can also measure the distance from the sensor to the target object and a rough direction. When a camera image is used, the distance and direction from the camera to the object can be obtained based on the principle of stereo stereoscopic vision. In the above three cases, the sensor data for the target object is generally a set of points. In the case of a camera image, an object in the image can be recognized by collating with the object model, and the relative posture of the object viewed from the camera can be obtained. In this case, any sensor can determine the relative posture of the target object viewed from the sensor, but the form and nature of the data obtained are generally different.
[0005]
In order to create a map from the movement amount of the robot and the measurement data of the sensor, first, while the robot is traveling, the measurement of the movement amount of the robot and the measurement of the sensor are synchronized to record the data. Next, the movement amount of the robot is accumulated to calculate the attitude of the sensor in the absolute coordinate system, and the measurement data of the sensor is added to the obtained absolute attitude of the sensor to calculate the attitude of the target object in the absolute coordinate system. By repeating this for all measurement data of the sensor, the arrangement of the measured object is determined, and a map can be created.
A map created by such a method often cannot obtain the correct shape due to the amount of movement of the robot and sensor errors. Therefore, the robot movement amount and sensor data error are represented by probability distribution, and the probability distribution of the robot posture and object posture is estimated based on the sensor data, and the layout that minimizes the error of the entire map is obtained. Has been done. For example, if the robot and object poses are defined on an absolute coordinate system, the probability distribution of the object pose is estimated from the robot movement and sensor measurement data, and the robot measures the same object, the robot path loop The map error is reduced by performing the process of closing (see, for example, Non-Patent Document 3).
[0006]
[Non-Patent Document 1]
J. J. Leonard and H. F. Durrant-Whyte: "Simultaneous Map Building and Localization for an Autonomous Mobile Robot," Proc. Of IROS'91, pp.1442-1447, 1991.
[Non-Patent Document 2]
Yoneda, Tsubouchi, Osumi, “First Robot Creation Design”, Kodansha, pp. 22-25
[Non-Patent Document 3]
S. Thrun, W. Burgard, and D. Fox: "A Real-Time Algorithm for Mobile Robot Mapping with Applications to Multi-Robot and 3D Mapping," Proc. Of ICRA2000, 2000.
[0007]
[Problems to be solved by the invention]
However, the above-described method has a problem that a sufficiently accurate map cannot be obtained when the movement amount of the robot or the measurement error of the sensor data is large. In the method based on the estimation of the probability distribution, an error distribution of the robot movement amount and sensor data is assumed. Therefore, if the error distribution is originally large, the obtained map error naturally increases. For example, when making a map of a room where a robot moves, the orientation of furniture such as a desk or shelf is important information, but it is generally difficult to measure the orientation with a sensor. It is difficult to recognize such furniture with an ultrasonic sensor or a laser distance sensor. For example, even if the shape of a rectangle can be known, it is not possible to know which direction is the front. Further, if the method recognizes an object from a camera image, it is possible to know which direction is the front, but the measured value of the angle may not be highly accurate.
There is also a problem that there are objects that cannot be measured by the sensor. For example, an ultrasonic sensor or a laser distance sensor usually only shows an object at a height where the sensor is placed. Further, it is difficult to detect a thin rod-like object. When using a camera image, it is difficult to detect a transparent object, a complex object, and the like.
[0008]
[Means for Solving the Problems]
  In order to solve the above-described problems, the present invention measures the attitude of a movement amount sensor and an object to be measured.postureA mobile robot mapping system using a mobile robot equipped with sensors,Object as node and relative posture between objects as arcmake, Represented by a graph in which a probability distribution representing the uncertainty of the relative posture is added to the arc, of the environment in which the mobile robot movesMap storage means for storing map information, a movement amount sensor for measuring movement of the mobile robot, and an object in the vicinity of the mobile robotPosture sensor that measures the posture of the robot and obtains the posture information of the object in the robot coordinate systemWhen,The mobile robot moves through the environment,The movement amount sensor and theAttitude sensorByFrom the obtained data columnBy simultaneous estimation of the map information represented by the relative posture between objects and the posture of the robot,Estimate probability distribution of relative posture between objectsNew map information creation meansThe new map information creating means includes a movement amount of the mobile robot obtained from the movement amount sensor, and posture information of an object as a reference this time based on the coordinate system of the mobile robot obtained by the posture sensor. Probability of the robot posture in the coordinate system based on the certain object and the relative posture between the other object and the certain object from the posture of the robot in the coordinate system of the other object based on the previous Simultaneous estimation is performed by estimating together with the distribution.It is characterized by that.
  The map storage means also stores existing map information, reads the stored existing map information, compares it with new map information, and the same objectArc representing relative posture betweenIf there is a,Against the arcExisting map informationRelative posture atWhen,New map informationRelative posture atAnd stochastic fusion,In the map information meansNew relative postureThe existing map information fusion means for storing as may be provided.
  The existing map information fusion means is activated after creating new map information, and if there is no new map information, the existing map information may be stored as new map information.
  furtherWhen the loop related to the relative posture is detected in the new map information, the displacement of the relative posture constituting the loop is such that, when the relative posture between each object is fully added along the loop from the starting point object of the loop, the loop returns to the starting point object. There may be provided a loop solving means for calculating.
[0009]
DETAILED DESCRIPTION OF THE INVENTION
In the mobile robot map creation system using the mobile robot of the present invention, a map having an object as a structural unit is constructed in the above-described SLAM framework. Specifically, the map is constructed in a form in which an object model is arranged on the floor plane, and the robot recognizes the object by the sensor while moving in the environment, and constructs the map. Most of the conventional map construction studies are targeted at maps that describe the area where the robot can travel, and few have described the map by structuring the environment in units of objects. However, for example, in order to perform complicated operations such as gripping and transporting an object, it is indispensable to identify each object and know its position and orientation, and a map configured in units of objects is required.
In the method proposed in the present invention, a map is represented by a graph in which an object is a node and a relative posture between objects is an arc. The relative attitude arc has a probability distribution for representing an error. The robot constructs a map by estimating the relative posture between objects while performing self-position estimation based on the movement amount and sensor data (measurement data).
[0010]
In addition, the map creation system using a mobile robot according to the present invention uses a map created by a human on a map created by a robot (hereinafter referred to as a new map) or a map measured by another sensor (hereinafter referred to as an existing map). The accuracy of the new map is improved by merging, and the object that could not be detected by the sensor mounted on the robot is registered. For example, if a person creates an existing map in which the direction of furniture is set and merges it with a new map, the direction of furniture is defined even in the new map. If an object that is difficult to detect with a sensor mounted on the robot is registered in an existing map, the object can be registered in a new map.
Hereinafter, an example of an embodiment of a mobile robot map creation system of the present invention will be described in detail with reference to the drawings.
First, the system configuration of this embodiment will be described. This system has a sensor for detecting a movement amount and a posture, and is configured on a computer system existing in a moving robot or the like.
[0011]
<Composition of map information>
Initially, the structure of the map information of this embodiment is demonstrated. As described above, in the present invention, the created map information is stored in the map storage means.
Each table in FIGS. 1A to 1C is a diagram illustrating a configuration example (an example of data expression) of map information to be stored.
FIG. 1A is a diagram illustrating a configuration example of map information. As shown in the figure, the map is composed of a set of object information and a set of relative posture information.
FIG. 1B is a diagram illustrating an example of the configuration of relative posture information. As shown in FIG. 1B, the relative posture information includes the object (start point node) as the start point, the object (end point node) as the end point, and the expected value of the relative posture of the end point object viewed from the start point object. Composed of distributed. In order to indicate an object from the relative posture information, the following unique object name is used.
[0012]
FIG. 1C is a diagram illustrating an example of the configuration of object information. As shown in FIG. 1C, the object has its unique name, shape information, and absolute posture. The unique name is a name uniquely given to each object. The shape information and the absolute posture are used for outputting a map to a display or a printer. The shape information is defined as a two-dimensional or three-dimensional figure. This may be in accordance with a general-purpose graphic display program format on a computer. The absolute posture represents the posture of the object in the absolute coordinate system on the map. This is also used to display the map in an easy-to-view manner for the user. In addition, when there is no need to display a map, shape information and an absolute posture are unnecessary.
[0013]
<Outline of processing of this embodiment>
Next, the processing flow of this embodiment will be described with reference to the flowchart of FIG. The present invention creates a map while measuring the relative posture of the object to be measured as the robot moves.
In FIG. 2, first, in step S210, the amount of movement of the robot and the relative posture between objects existing in the environment are estimated from the measurement data of the robot sensor, and new map information m₁Create The method for creating a new map will be described in detail later.
Next, in step S220, the existing map information m₂And the relative orientation q of the existing map_ijAre taken out one by one. In step S230, new map information m₁And q_ijThe same relative posture as the new map information m₁Find out if there is also. Here, the same relative posture indicates that the objects to be measured are the same.
Whether the objects are the same between the two maps is determined by whether the unique names of the objects are the same. However, in step S230, only identity determination based on the unique name is performed, and it is not determined whether the objects are really the same. For this reason, the same object needs to have the same unique name between the two maps. In the present embodiment, it is given in advance by another existing method or given by a human.
[0014]
In step S230, if new map information m₁If the same relative posture exists, the process proceeds to step S240. New map information m₁If the same relative posture does not exist, the process proceeds to step S250.
In step S240, new map information m₁And existing map information m₂Stochastic fusion of the relative postures that have in common. The stochastic fusion of relative postures will be described in detail later.
In step S250, the existing map information m₂New map information m₁New map information₁Add to For this reason, first, the relative posture is represented by the new map information m.₁Add to the relative posture set. Next, the starting point object and the ending point object of the relative posture are new map information m.₁If not, if the object is new map information m₁To the set of objects.
[0015]
Next, in step S260, the existing map information m₂It is checked whether all the relative postures are extracted. If all the relative postures are extracted, the process proceeds to step S270. If it still remains, the process returns to step S220. In step S270, new map information m₁From the set of relative postures, a component that constitutes a loop is found, and processing for eliminating the deviation is performed. Note that the loop solution may be performed not only for all the relative postures but only for the relative postures included in the partial region of interest on the map. For example, the case where the loop solution is limited to the inside of one room is an example. Thereby, the amount of calculation can be reduced.
[0016]
In the present embodiment, the new map information m is obtained in S210.₁After the creation of the existing map information m after S220₂New map information m₁While creating the existing map information m₂Fusion processing may be performed. For example, new map information m₁Each time the relative posture between objects is measured in the creation of the existing map information m₂To the corresponding relative posture q_ijFusion may be performed by selecting.
[0017]
Hereinafter, each process of the flowchart showing the embodiment of FIG. 2 will be described in detail.
<Create a new map>
First, the new map creation process (S210) will be described with reference to FIGS.
(1. Map representation by relative posture)
In the present embodiment, the map m is represented by a graph m = <E, Q> in which an object is a node and a relative posture between objects to be measured is an arc. E = {e_i} Is a set of objects to be measured, Q = {q_ij} Is a set of relative poses of the object. Assume that a local coordinate system is assigned to each object. q_ij= (X, y, θ)^TAs q_ijIs the object e_iObject e seen from the local coordinate system_jRepresents the relative posture. Here, x and y are local coordinate systems e_iE seen from the coordinate system_jThe position of the origin of the coordinate system, θ is e_iE seen from the coordinate system_jAn angle formed by the coordinate system. In addition, the relative attitude arc deals only with those explicitly measured by a human or a robot. The relative posture is allowed to have uncertainty, and the uncertainty is expressed by a probability distribution.
[0018]
The robot posture and sensor data are expressed as follows.
(1) Representation of robot posture
The robot posture is expressed based on an object existing in the vicinity, and the reference object is switched as the robot moves. This object is called a reference entity. Reference entity e_iThe robot posture in the coordinate system of r_i= (X, y, θ)^TIt expresses.
Here, x and y are local coordinate systems e_iThe robot position as seen from the coordinate system, θ is e_iThe angle formed by the robot as seen from the coordinate system.
(2) Representation of sensor data
The robot recognizes an object with a sensor and estimates its posture. The object u is q = (x, y, θ) in the robot coordinate system^TIs represented as z = <u, q>.
Since object recognition only knows the class of an object, u is an unidentified object. Since there may be multiple objects of the same class in the environment, it is necessary to identify u. In this embodiment, this individual identification is performed by another existing method or a human being when giving a unique name of an object.
Note that the object recognition method is a conventional technique (see, for example, “Tomona, Oil Field: Indoor Navigation with Inaccuracy Map and Object Recognition, 7th Robotics Symposia Proceedings, 2002”).
[0019]
The object e is shown in FIG.₁, E₂Relative posture q₁₂An example is shown. As described above, the relative posture is allowed to have uncertainty, and the uncertainty is expressed by a probability distribution. Although this probability distribution may be arbitrary, for example, assuming a normal distribution, it can be expressed by an average value and a variance. In the present embodiment, the relative posture between the objects is estimated using the measurement data of the posture sensor and the movement amount sensor with respect to the object of the mobile robot.
As described above, the posture of the object is q = (x, y, θ) in the robot coordinate system.^TIs obtained by the attitude sensor. FIG. 3B shows an object e by a sensor mounted on the robot 300.₁An example of the measurement data is shown. Here, the object e obtained from the sensor₁Is represented by q = (x, y, θ). Depending on the sensor, θ may be indefinite. Since this sensor data includes an error, for example, the error can be expressed by a normal distribution. This error variance is determined in advance by examining the characteristics of the sensor. Further, the movement amount of the robot is obtained by a sensor mounted on the robot, as in the prior art. The amount of movement can also be expressed by (x, y, θ) indicating the position and orientation. Since the movement amount includes an error, for example, the error is represented by a normal distribution. This error variance is determined in advance by examining the characteristics of the robot.
[0020]
(2. Simultaneous estimation of map and robot posture)
(1) Problem setting
Next, a method for estimating the relative posture between two objects from the posture of the object obtained from the sensor and the movement amount of the robot will be described.
In the present embodiment, the construction of the map information is a data string ZZ obtained by the robot up to time t.^(T)Under the probability distribution P (m | ZZ) of the map m (described in detail later)^(T)) Is estimated. However, the probability distribution P (m | ZZ^(T)) Is the simultaneous probability of all relative attitude arcs, so the dimension is very large and difficult to handle as it is. Therefore, in this embodiment, instead of managing the probability of the entire map information, it is handled as a problem of obtaining the probability distribution of each relative attitude arc. That is, P (q_ij｜ ZZ^(T)) To build map information.
[0021]
4A and 4B are schematic diagrams of the map information corresponding to one relative posture arc and the simultaneous estimation of the robot posture. Now, robot 300 is point S.₁To point S₂Shall be moved to. Robot 300 has a point S₁At the time (t₁) To point S₂At the time (t₂Until e)₁Is the reference entity, and time t₂In object e₂And the reference entity is₂Switch to. Point S₁Robot object e₁The relative attitude from r¹ ₁, Point S₂Robot object e₁The relative attitude from r² ₁And The object e₁Object e seen from₂The relative posture of q₁₂And Also, point S₂The robot is the object e₂Object seen from robot when observing₂Let q be the measurement data of the relative posture of The environment is static and the relative posture q₁₂Shall not change over time.
In such a situation, the robot travels while recognizing the environment, and updates the probability distribution of the self-location and the map information. That is, the relative posture r¹ ₁If the probability distribution of is obtained by the previous calculations, the point S₁To point S₂Amount of movement and object e₂From the measured data, point S₂Robot object e₂Relative posture from (r¹ ₂And relative posture q₁₂Find the probability distribution of.
[0022]
Also, point S₂Robot e₁Relative posture from² ₁Can be calculated from the amount of movement of the robot as described above.
At this time, due to the estimation error of the movement amount, as shown in FIG.² ₁Probability distribution 420 of relative posture r¹ ₁The probability distribution 410 is wider. Now, point S₂The robot 300 moves the object e₂And measurement data q is obtained. The measurement data q has a unique probability distribution. Therefore, the relative posture r² ₁By calculating the probability distribution for the sum of q and q₁Object e from₂Relative posture q² ₁₂Can be estimated. In fact, q² ₁₂= R² ₁Since there is a vector sum relationship of + q, q² ₁₂The probability distribution of is two random variables r² ₁The probability distribution of the sum of q and q can be calculated by convolution integration. In FIG. 4B, the relative posture q² ₁₂The probability distribution of 430 is indicated by 430.
Hereinafter, the process for obtaining the above probability distribution will be formulated and described.
[0023]
(2) Simultaneous probability of map information and robot posture
The data string is ZZ^(T)= <Z⁽⁰⁾, A⁽⁰⁾, ..., z^(T-1), A^(T-1), Z^(T)>, ZM^(T-1)= <Z⁽⁰⁾, A⁽⁰⁾, ..., z^(T-1), A^(T-1)> z^(T)Is the sensor data <u at time t^(T), Q^(T)>, A^(T-1)Represents a running event of the robot from time t-1 to t. In the following, q^(T)Is simply represented as q. It is assumed that the robot sets the reference entity to the object recognized at that time and does not change during one traveling event.
As described above, in the present embodiment, it is assumed that an individual can be identified by a unique name. In the following, sensor data <u^(T), Q^(T)> U^(T)Is the object e₁And objects e₂Is implicitly excluded from the expression.
[0024]
First, the relative attitude arc q₁₂And robot posture r¹ ₂Simultaneous probability P (q₁₂, R¹ ₂｜ ZZ^(T)) Is calculated by the following equation (1) using Bayes' theorem. R¹ ₂Is point S₂Robot object e₂The relative posture from
[Expression 1]

... Formula (1)
Here, α is a normalization coefficient. In the first term on the right side, P (q | r¹ ₂, Q₁₂, ZM^(T-1)) = P (q | r¹ ₂). ZM^(T-1)The omission is based on Markovian assumption. q₁₂Is omitted when q is robot and object e₂Relative posture r¹ ₂Q₁₂By not depending on.
[0025]
Further, P (q) in the formula (1)₁₂, R¹ ₂｜ ZM^(T-1))¹ ₁To r² ₁And then the reference entity is moved to the object e₂It can be transformed as the following equation (2).
[Expression 2]

... Formula (2)
Where P (r¹ ₂| R² ₁, Q₁₂, ZM^(T-1)) R¹ ₂, R² ₁, Q₁₂Because it is determined only by the relationship (becomes a δ function), ZM^(T-1)Can be omitted.
[0026]
P (q) in formula (2)₁₂, R² ₁｜ ZM^(T-1)) Is represented by the following formula (3).
[Equation 3]

... Formula (3)
P (r in formula (3)² ₁| R¹ ₁, A^(T-1)) Is r¹ ₁Travel from a^(T-1)R² ₁Is the probability of reaching Assuming that the state of the bottom surface in the vicinity of each object is the same and there is no obstacle, this probability does not depend on the map information or the data so far,^(T-1)It can be calculated only from the measurement result of the movement amount. On the other hand, P (q₁₂, R¹ ₁| A^(T-1), ZZ^(T-1)A)^(T-1)Is r¹ ₁To r² ₁Q₁₂, R¹ ₁Omitted because it does not affect. Q₁₂And r¹ ₁Are independent and P (q₁₂, R¹ ₁｜ ZZ^(T-1)) Was disassembled.
[0027]
Summarizing the above equations (1) to (3), the distribution is updated as follows in one round in which the robot recognizes one movement and recognizes an object.
[Expression 4]

... Formula (4)
Thus, if the previous robot posture distribution and the relative posture arc distribution are stored, the simultaneous distribution of the robot posture and the relative posture arc at the present time can be calculated.
[0028]
(3) Estimation of robot posture
From the above equation (4), the robot posture r¹ ₂The probability distribution of can be calculated as follows.
[Equation 5]

... Formula (5)
[0029]
(4) Relative attitude arc estimation
The probability distribution P (q₁₂｜ ZZ^(T)) Can be calculated as follows:
[Formula 6]

... Formula (6)
In the equations (5) and (6), P (q | r¹ ₂) Is determined by the error characteristic of the object posture sensor of the robot, and P (r² ₁| R¹ ₁, A^(T-1)) Is determined by the error characteristics of the robot movement amount sensor. P (r¹ ₂| R² ₁, Q₁₂) Is the vector sum (r¹ ₂+ Q₁₂) Is r² ₁1 when equal to 0, 0 otherwise.
P (q₁₂｜ ZZ^(T-1)) Is the relative posture q₁₂If is present on an existing map, its probability distribution is used. If it does not exist on the existing map, the distribution is uniform. P (r¹ ₁｜ ZZ^(T-1)), As described above, it is assumed that the value has been obtained in the previous processing.
Equations (5) and (6) are calculated using the above probability distribution.
However, since equations (5) and (6) include integration, they are generally difficult to calculate. For this reason, the self-position estimation algorithm using the conventional particle filter was expanded and implemented. As for this prior art, for example, “Tomona, Oilfield:“ Self-position estimation of mobile robot in a map that allows inaccuracy ”, Journal of the Robotics Society of Japan, VOL.20, No.4, pp.75-86, 2002” Please refer to.
[0030]
<Consolidation of new map and existing map>
Next, the merge of a new map and an existing map will be described with reference to FIG. This is the process shown in S220 to S260 of FIG. 2 described above. For example, the new map information created by the mobile robot is merged with the existing map information created by a human to obtain more accurate map information. The purpose is to create.
[0031]
(1. Fusion of relative posture)
Object on existing map₁And e₂If the relative posture between the two is defined, this is used to determine the relative posture q estimated above.₁₂Can improve the accuracy.
When both the existing map created by a human and the new map created by the mobile robot have a common relative attitude arc, the relative attitude is stochastically fused. This is equivalent to P (q in the above equation (6).₁₂｜ ZZ^(T-1)) With an existing map, add measurement data to it and add P (q₁₂｜ ZZ^(T)) Can be calculated.
[0032]
This is shown in FIG. FIG. 5A shows an object e defined on an existing map.₁Object e seen from₂Relative posture q¹ ₁₂It is a figure which shows the probability distribution 510 of no. In this example, the object e₁And object e₂Are aligned in the same direction on a straight line, but the distance is uncertain. Such a definition appears when an object lined up along a wall is roughly measured and described manually.
Then, as shown in FIG.¹ ₁₂Is more accurate relative posture q³ ₁₂(Indicated by 520 in FIG. 5B) can be obtained. Specifically, q in FIG.¹ ₁₂The probability distribution 510 of P (q₁₂｜ ZZ^(T-1)(6) may be calculated as the value of).
[0033]
Further, as shown in FIG. 5C, the robot posture is also updated (in the figure, the updated value is represented by a probability distribution 530). Specifically, q in FIG.¹ ₁₂The probability distribution 510 of P (q₁₂｜ ZZ^(T-1)(5) may be calculated as the value of).
The robot posture r updated in this way¹ ₂Is the reference entity e₁To e₂It has changed to. Robot is e₂When going from one to the next object, the same processing as described above is repeated for the object.
As described above, the relative posture between the measured objects can be obtained.
On the other hand, if there is a relative posture of the existing map that is not in the new map, the relative posture cannot be merged, and the relative posture is added to the new map.
[0034]
(2. Fusion as a loop)
When the existing map has a relative posture arc that does not exist in the new map, the above-described posture distribution cannot be directly merged. However, a loop may occur due to the combination of both arcs. In this case, the relative attitude arc of the existing map works as a constraint condition for the new map through the loop, and the accuracy of the map information can be improved.
Humans can easily define relative postures between objects in parallel or perpendicular positional relations and adjacent objects, and are effective geometric constraints. The accuracy of map information can be improved by defining a relative attitude arc representing such a constraint in an existing map and solving a loop by merging with a new map.
The loop solution will be described below.
[0035]
<Resolution of loop>
Next, the loop solution will be described in detail. This is the process shown in S270 of FIG.
When the created map information is used by the mobile robot or displayed to the user, the expected value or maximum probability point of the probability distribution of each relative attitude arc obtained above is used as a representative value. Will make a simple map. However, a map created by connecting representative values of individual relative attitude arcs is distorted by data errors. Therefore, consider introducing a constraint between the relative attitude arcs to eliminate this distortion. In this embodiment, a method using a loop constraint is used for this purpose.
[0036]
(1. Formulation)
A chain of relative postures between objects generally forms a loop, but due to various errors, a phenomenon occurs in which the loop does not close well.
FIG. 6 shows an example of a loop. FIG. 6A is a map when the relative posture is consistent. In this case, the relative posture q₁~ Q₄Follow the object e₁Returns to its original position and the loop is closed.
On the other hand, FIG. 6B shows an example in the case where the relative posture is not consistent and is distorted. Relative posture q₁~ Q₄Follow the same object e₁Becomes a different posture from the beginning (indicated by 610), and the loop does not close. Since this phenomenon has an error in the value of the relative posture as described above, if the expected value is adopted as the representative value, e₁This happens because the postures in the absolute coordinate system are different.
In order to eliminate this inconsistency between the relative postures, in the present embodiment, processing for closing the loop is performed.
[0037]
Now assume that the map contains n relative attitude arcs, each of which is represented by q_i= (X_i, Y_i, Θ_i)^T, 1 ≦ i ≦ n. Q predefined in the map_iOr q that the robot asked for_iThe estimated value of q ′_i, The covariance matrix is R_iAnd
In the method proposed in this embodiment, the loop solution is performed by estimating the value q ′ while satisfying the loop constraint._iThis is considered as an optimization problem for obtaining a relative posture q that minimizes the sum of squares of errors. Where q = {q₁, ..., q_n}. This formulation is shown in Formula (7)-Formula (10).
[0038]
[Expression 7]

... Formula (7)
[Equation 8]

... Formula (8)
[Equation 9]

... Formula (9)
[Expression 10]

... Formula (10)
[0039]
G in formula (8)_j(Q) is a constraint equation for the j-th loop. Loop j is n_jQ consisting of arcs_j1, Q_j2, ..., q_jnjAssume that arcs are arranged in the order of. q_j1Is the starting point of the loop. g_j(Q) = 0 represents a constraint that if the relative postures between the objects are all added along the loop from the starting object of the loop, the object must return to the starting object. L is the number of independent loops included in the map. M (φ_jk) Is the relative attitude arc q_jkIs a matrix that converts to the coordinate system of the starting object of the loop.
[0040]
(2. Implementation)
The above problem becomes a nonlinear minimization problem. The implementation used particle filters, but there are other ways to solve the nonlinear minimization problem. The particle filter here is used for obtaining the minimum value, unlike the case of obtaining the distribution at each point of the Markov process as described above. The particle filter at this time works very similar to the Genetic Algorithm.
The particle filter here is detailed in, for example, “J. Deutshcher, A. Blake, and I. Reid:“ Articulated Body Motion Capture by Annealed Particle Filtering ”Proc. Of CVPR2000, 2000”.
The procedure is shown below. Each particle has all the variables included in q. As an evaluation value, E (q) = f (q) + Σ_j‖G_j(Q) Use cocoons.
(1) Particles are generated using the relative postures obtained by equation (6) as initial values.
(2) Each variable value of the particle is randomly changed according to a predetermined normal distribution, and the evaluation value E (q) is calculated. Save the particle with the smallest evaluation value. The weight of each particle is e^{-E (q)}Set to a value proportional to.
(3) Resample the particles with a probability proportional to the weight, and return to (2).
(4) After repeating (2) and (3) a predetermined number of times, the relative orientation value of the particle that has obtained the minimum evaluation value is taken as the solution.
[0041]
As described above, the accuracy of the map can be improved by merging the existing map with the new map. For example, if a person creates an existing map in which the direction of furniture is set and merges it with a new map, the direction of furniture is defined even in the new map. If an object that is difficult to detect by a sensor mounted on the robot is registered in an existing map, the object can be registered in a new map.
The existing map may be a map previously made by a robot or a map made by a human. However, when a human makes an existing map, if the relative posture is qualitatively expressed, the human burden can be reduced. For example, when two desks are arranged in parallel in the same direction, the expected value of the angle component θ of the relative posture between them is set to 0 degree to reduce the variance. If the orientation is vertical, the expected value of θ is set to 90 degrees to reduce the variance. By preparing a user interface for converting such a qualitative geometric relationship into a quantitative value of a relative posture, the burden on humans can be greatly reduced. Particularly in the case of indoor maps, this method is very effective because the arrangement of objects often follows a regular geometric relationship such as parallel, vertical, or adjacent.
[0042]
As described above, the present embodiment is characterized in that the map is merged after the posture of the object is expressed as a relative posture. If the object posture is represented by an absolute posture, generally, the error variance increases as the distance from the origin of the absolute coordinate system increases, and the estimation accuracy of the probability distribution deteriorates. In addition, when a human creates an existing map, it is easier to determine an object as a reference arbitrarily and define the posture of another object based on the relative posture therefrom. Furthermore, if the relative posture is used, it becomes easy to use a partial map indicating a local area as an existing map instead of the entire map. In the above-described prior art method (Non-Patent Document 3), the map is merged, but since the absolute posture is used as the object posture, the above problem is not dealt with.
[0043]
In addition, the map creation system of this invention is not limited to said embodiment, A various change can be added in the range which does not deviate from the summary of this invention. For example, in the above description, it is assumed that the computer system is built on the mobile robot. However, the computer system may be built on a computer system that is in another location and configured to exchange data with the mobile robot. Good.
[0044]
【The invention's effect】
As described above, according to the map creation method using the mobile robot of the present invention, the map is created probabilistically based on the relative posture of the object based on the movement amount of the robot and the sensor measurement data. Thereby, map information with little error (high accuracy) can be created.
Since the created map can be corrected by merging with the existing map, the accuracy of the map can be further increased. In particular, as an existing map, a rough map expressed by a qualitative posture relation between objects such as parallel, vertical, and adjacent objects can be used.
Further, in the present invention, when the relative posture of the existing map does not exist in the new map, an object accompanying the relative posture is also added to the new map. This brings about an effect that an object that is difficult to measure with a sensor can be registered in a map.
[Brief description of the drawings]
FIG. 1 is a diagram illustrating an example of a data representation of map information according to the present embodiment.
FIG. 2 is a flowchart showing a processing procedure of a map creation method of the present embodiment.
FIG. 3 is a diagram illustrating an example of relative posture between objects and data for measuring the posture of an object by a sensor.
FIG. 4 is a diagram illustrating an example of a method for estimating a relative posture between objects.
FIG. 5 is a diagram illustrating an example of a method for merging relative postures between objects.
FIG. 6 is a diagram illustrating an example of a relative posture loop;

Claims

A mobile robot mapping system using a mobile robot equipped with a movement sensor and a posture sensor that measures the posture of an object to be measured,
Creates an object as a node and a relative pose between objects as an arc, and stores map information of the environment in which the mobile robot moves, represented by a graph with a probability distribution representing the uncertainty of the relative pose added to the arc Map storage means to
A movement amount sensor for measuring movement of the mobile robot;
A posture sensor that measures the posture of an object in the vicinity of the mobile robot and obtains posture information of the object by a coordinate system of the robot ;
Said mobile robot moves in the environment, from said data string obtained by the moving amount sensor and the orientation sensor, the simultaneous estimation of the orientation of the map information and a robot represented by a relative posture between the object, between the object a new map information creating means for estimating the probability distribution of the relative attitude,
The new map information creation means includes the movement amount of the mobile robot obtained from the movement amount sensor, the posture information of an object based on the coordinate system of the mobile robot obtained by the posture sensor, and other previously used references. From the posture of the robot based on the coordinate system of the object, the posture of the robot based on the coordinate system based on the certain object and the relative posture between the other object and the certain object are estimated together with the probability distribution. A mobile robot mapping system characterized by simultaneous estimation .

The mobile robot map creation system according to claim 1,
The map storage means also stores existing map information,
The stored existing map information is read out and compared with new map information. If there is an arc representing the relative posture between the same objects , the relative posture in the existing map information and the relative posture in the new map information with respect to the arc A map creation system for a mobile robot , comprising: existing map information fusion means for stochastically fusing and storing the map information means as a new relative posture .

The mobile robot map creation system according to claim 2,
The existing map information fusion means is activated after creating new map information, and stores the existing map information as new map information if there is no new map information.

In the mobile robot map creation system according to any one of claims 1 to 3,
When a loop related to the relative posture is detected in the new map information, the relative posture displacement that constitutes the loop is changed so that returning to the starting point object when all the relative postures between the objects are added along the loop from the starting point object of the loop. A map creation system for a mobile robot, further comprising a loop solving means for calculating.