JP4352148B2

JP4352148B2 - 移動ロボット用地図作成システム

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のもつ相対姿勢値を解とする。
【００４１】
以上のように、新規地図に既存地図を併合することで、地図の精度を改善することができる。たとえば、人間が家具の向きを設定した既存地図を作って、新規地図に併合すれば、新規地図でも家具の向きは定義される。また、ロボットに搭載したセンサで検出しにくい物体を既存地図に登録しておけば、新規地図にもその物体を登録することができる。
なお、既存地図は、ロボットが以前に作った地図でも人間が作った地図でもよいが、人間が既存地図を作る場合は、相対姿勢を定性的に表現すると、人間の負担が小さくてすむ。たとえば、２つの机が同じ向きで平行に並んでいる場合は、その間の相対姿勢の角度成分θの期待値を０度にし、分散を小さくする。また、向きが垂直ならば、θの期待値を９０度にし、分散を小さくする。このような定性的な幾何学的関係を相対姿勢の定量値に変換するユーザインタフェースを用意することで、人間の負担を大きく減らすことができる。とくに、屋内地図の場合は、物体の配置が平行、垂直、隣接などの規則正しい幾何学的関係に従うことが多いので、この方法は効果が大きい。
【００４２】
上述のように、本実施形態は、物体の姿勢を相対姿勢で表した上で、地図の併合を行う点を特徴とする。仮に物体姿勢を絶対姿勢で表すと、一般に絶対座標系の原点から遠くなるほど誤差分散が大きくなり、確率分布の推定精度は悪くなる。また、人間が既存地図を作成する場合は、任意に基準とする物体を決めて、そこからの相対姿勢で他の物体の姿勢を定義する方が容易である。さらに、相対姿勢を使えば、全体地図ではなく、局所領域を示す部分地図を既存地図として利用することも容易になる。前述の従来技術の方法（非特許文献３）では、地図の併合は行われているが、物体姿勢として絶対姿勢を用いているので、上記の問題には対処していない。
【００４３】
なお、本発明の地図作成システムは、上記の実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々変更を加え得ることができる。例えば、上述では移動ロボット内に存在するコンピュータ・システム上に構築されるとして説明したが、他の場所にあり、移動ロボットとのデータをやり取りできるように構成したコンピュータ・システム上に構築してもよい。
【００４４】
【発明の効果】
上述したように、本発明の移動ロボットによる地図作成方法によれば、ロボットの移動量とセンサの測定データに基づいて、物体の相対姿勢により地図を確率的に作成する。これにより、誤差の少ない（精度の高い）地図情報を作成することができる。
そして、作成した地図を、既存地図と併合することで修正できるため、地図の精度をさらに上げることができる。特に、既存地図として、平行、垂直、隣接といった物体間の定性的な姿勢関係で表した大まかな地図を利用できるため、既存地図を人間が作る場合でも、その負担は小さくて済む。
また、本発明では、既存地図の相対姿勢が新規地図に存在しない場合は、その相対姿勢に付随する物体も新規地図に追加される。これは、センサで計測しにくい物体を地図に登録できるという効果をもたらす。
【図面の簡単な説明】
【図１】本実施形態の地図情報のデータ表現の一例を示す図である。
【図２】本実施形態の地図作成方法の処理手順を示すフローチャートである。
【図３】物体間の相対姿勢、および、センサによる物体の姿勢測定データの一例を示す図である。
【図４】物体間の相対姿勢の推定方法の一例を示す図である。
【図５】物体間の相対姿勢の融合方法の一例を示す図である。
【図６】相対姿勢のループの一例を示す図である。

Claims

移動量センサと計測対象の物体の姿勢を計測する姿勢センサを搭載した移動ロボットによる移動ロボット用地図作成システムであって、
物体をノード，物体間の相対姿勢をアークとして作成し、該相対姿勢の不確実性を表す確率分布を該アークに付加したグラフにより表される、該移動ロボットが移動する環境の地図情報を格納する地図記憶手段と、
前記移動ロボットの移動を測定する移動量センサと、
前記移動ロボットの近傍の物体の姿勢を計測し、ロボットの座標系による物体の姿勢情報を得る姿勢センサと、
前記移動ロボットが環境中を移動して、前記移動量センサと前記姿勢センサにより得られたデータ列から、物体間の相対姿勢で表される地図情報とロボットの姿勢との同時推定により、物体間の相対姿勢の確率分布を推定する新規地図情報作成手段とを備え、
前記新規地図情報作成手段は、前記移動量センサから得た該移動ロボットの移動量と、前記姿勢センサで得られた該移動ロボットの座標系によるある物体の姿勢情報と、以前に基準とした他の物体の座標系によるロボットの姿勢とから、前記ある物体を基準とした座標系によるロボットの姿勢と、前記他の物体と前記ある物体との間の相対姿勢とを確率分布とともに推定することで、同時推定を行う
ことを特徴とする移動ロボット用地図作成システム。
請求項１に記載の移動ロボット用地図作成システムにおいて、
前記地図記憶手段は、既存地図情報も記憶しており、
記憶している既存地図情報を読み出して新規地図情報と比較し、同じ物体間の相対姿勢を表すアークがあれば、該アークに対する既存地図情報での相対姿勢と、新規地図情報での相対姿勢とを確率的に融合して、前記地図情報手段に新らたな相対姿勢として記憶する既存地図情報融合手段
を備えることを特徴とする移動ロボット用地図作成システム。
請求項２に記載の移動ロボット用地図作成システムにおいて、
前記既存地図情報融合手段は、新規地図情報を作成後に起動され、かつ新規地図情報がなければ既存地図情報を新規地図情報として記憶することを特徴とする移動ロボット用地図作成システム。
請求項１〜３のいずれかに記載の移動ロボット用地図作成システムにおいて、
新規地図情報において相対姿勢に関するループを検出した場合、ループの始点物体からループに沿って、各物体間の相対姿勢を全部足すと始点物体に戻るように、該ループを構成する相対姿勢の変位を計算するループ解決手段をさらに備えていることを特徴とする移動ロボット用地図作成システム。