JPWO2018179649A1 - 地図作成システム及びロボットシステム - Google Patents

Abstract

本発明は、動的物体の情報を含まない地図データを効率的に作成可能なシステムに関する。本発明の一実施形態に係る地図作成システムは、周囲の物体上の複数の測定点までの距離及び角度を計測し、距離及び角度の集合である距離情報を出力する距離センサを備えた移動体を有している。地図作成システムは距離センサにより、移動体が第１の位置に存在する時に計測された第１の距離情報と、移動体が第２の位置に存在する時に計測された第２の距離情報とを収集して、両者を合成することで、物体が存在する位置を表す位置情報を記録した物体存在地図を作成する。また距離情報をもとに物体の存在しない領域である削除対象領域を特定し、この削除対象領域に含まれる位置に相当する位置情報を物体存在地図から削除することで、静止物体の位置情報のみが記録された静止物体地図を作成する。

Description

本発明は地図作成システム及びロボットシステムに関する。

従来、電動車椅子などの移動体が、周辺環境の地図を作成すると同時に自身の位置を推定する技術がある。具体的には、移動体は距離センサ等のセンサや、車輪の回転数データを検出する手段を備え、回転数データを用いて移動体の位置や向きを求めるとともに、センサによって、壁などの周辺の物体までの距離を得て、それらの情報をもとにして、移動体の位置の推定と周辺環境の地図の作成を行う。この技術はＳＬＡＭ（Simultaneous Localization and Mapping）と呼ばれる。

場合によっては、移動体の周囲に動く物体（動的物体と呼ぶ）が存在する環境下で地図の作成を行わなければならないこともある。この場合、動的物体を、壁などの静止物（静的物体）と誤認識すると、誤った地図が作成されてしまうため、地図作成時にはセンサなどで得られた動的物体の情報を除去することが必要である。

例えば特許文献１には、クラスタリング手段により距離が所定値TH未満の位置同士をまとめたクラスタを作成し、そのクラスタの固有値から、動的物体か否かを判断して、動的物体を削除する方法が示されている。例えば、長手方向のサイズとその垂直方向のサイズを固有値として判断すれば、人を表すクラスタを動的物体と断定することができる。このようにして、地図の中から動的物体を削除できる。

特開２０１４−１７４２７５号公報

特許文献１に開示の技術では、人などの形状はクラスタの固有値により動的物体と判断することができるが、大型ＡＧＶ（Automated Guided Vehicle）などが移動する環境下で特許文献１に開示の技術を用いた場合、大型ＡＧＶなどと、壁や塀などの静的物体とを区別することが困難になることがある。そのため、動的物体のクラスタをすべて自動的に地図から削除することが難しいという課題がある。本発明は、上記のような課題を解決するためになされたものである。

本発明の一実施形態に係る地図作成システムは、周囲の物体上の複数の測定点までの距離及び角度を計測し、距離及び角度の集合である距離情報を出力する距離センサを備えた移動体を有している。地図作成システムは距離センサにより、移動体が第１の位置に存在する時に計測された第１の距離情報と、移動体が第２の位置に存在する時に計測された第２の距離情報とを収集して、両者を合成することで、物体が存在する位置を表す位置情報を記録した物体存在地図を作成する。また距離情報をもとに物体の存在しない領域である削除対象領域を特定し、この削除対象領域に含まれる位置に相当する位置情報を物体存在地図から削除することで、静止物体の位置情報のみが記録された静止物体地図を作成する。

本発明によれば、動的物体の情報を含まない地図データを効率的に作成することができる。

実施例１に係る地図作成システムのブロック図である。 移動体が環境内を移動しながら地図を作成する処理の説明図である。 環境内に動的物体が存在する場合の問題点を説明する図である。 空き領域地図作成手段による、削除対象領域の求め方を説明する図である。 実施例２に係る地図作成システムが有する空き領域地図作成手段の構成を示した図である。 領域短縮手段による削除対象領域の求め方を説明する図である。 実施例２の空き領域地図作成手段によって、地図から動的物体の情報が削除される様子を示した説明図である。 実施例３に係る地図作成システムのブロック図である。 実施例３において、移動体が距離データを収集する過程を時系列順に示した図である。 実施例３に係る地図作成システムの、地図ノイズの削除方法を説明する図である。 実施例１と実施例２の違いを説明する図である。

以下、本発明の実施の形態について具体的に説明する。

図１は、実施例１に係る地図作成システムの一例である移動体１００が有する、地図作成処理を実現するための機能ブロック図である。移動体１００は、レーザ距離センサ１、距離収集手段２、物体存在地図作成手段３、空き領域地図作成手段８、地図修正手段９を備える。また本実施例における移動体１００は、自動車などの車両、または走行機能を備えたロボットである。ただし図１では移動体１００の外観や、走行に必要な車輪等の存在は略して、地図作成処理に必要な機能部を中心に図示している。また別の実施形態として、移動体１００が走行機能を持たない（モータなどの動力を持たない）装置であってもよく、たとえば手押し車のようなものであっても良い。

レーザ距離センサ１は、１回の測定で移動体１００の周囲の物体までの距離を所定の角度範囲（例えば左右１３５°、合計270°の範囲）で計測するものである。なお本実施例では、レーザ距離センサ１によって測定される物体上の点を「測定点」と呼ぶ。

レーザ距離センサ１が物体までの距離を計測すると、その距離及び角度のデータは距離収集手段２に入力される。以下では、レーザ距離センサ１が１回の測定で得た距離及び角度のデータのセットを「距離データ」と呼ぶ。たとえばレーザ距離センサ１が、左右１３５°（合計270°）の範囲について１度毎に距離測定を行う場合、距離データは２７１個の距離及び角度の組から構成される。なお、レーザ距離センサ１による１回の測定は極めて短時間で終了するため、移動体１００は移動しながらレーザ距離センサ１による計測を行える。

距離収集手段２は収集指令が与えられると、レーザ距離センサ１から入力されたデータを自身の有する記憶領域に格納（収集）する。収集指令は、移動体１００の外部（たとえば移動体１００に指示を出す人、あるいは移動体１００に接続されたコントローラ等）から与えられてもよいし、あるいは移動体１００内の非図示のコンポーネントから発行されてもよい。地図作成に必要な複数回分の距離データが収集されると、これらの距離データは距離収集手段２から物体存在地図作成手段３に転送される。あるいは別の例として、距離収集手段２は、レーザ距離センサ１から距離データが入力されるたびに、距離データを物体存在地図作成手段３に送信してもよい。

また、以下の説明では、レーザ距離センサ１は左右１３５°（合計270°）の範囲について１度毎に距離測定を行う前提で説明する。またレーザ距離センサ１の存在する位置を“Ｏ”と表記し、移動体１００の正面にある物体上の測定点を“Ｐ_０”と表記し、点Ｏから測定点Ｐ_０までの距離及び角度の組を（ｄ_０，θ_０）と表記する。また、移動体１００の左方ｎ度（ｎは１以上１３５以下の整数である）の方向にある物体上の測定点を“Ｐ_−ｎ”と表記し、点Ｏから測定点Ｐ_−ｎまでの距離及び角度の組を（ｄ_−ｎ，θ_−ｎ）と表記する。そして移動体１００の右方ｎ度（ｎは１以上１３５以下の整数である）の物体上の測定点を“Ｐ_ｎ”と表記し、点Ｏから測定点Ｐ_ｎまでの距離及び角度の組を（ｄ_ｎ，θ_ｎ）と表記する（なお、θ_ｎ＝ｎの関係にある）。なお、（ｄ_ｕ，θ_ｕ）（ｕは−１３５≦ｕ≦１３５の整数である）は、点Ｏ（レーザ距離センサ１）を原点とする極座標系上の座標とみなすこともできるので、（ｄ_ｕ，θ_ｕ）のことを「座標」と呼ぶこともある。

図２を用いて、物体存在地図作成手段３が行う地図作成処理を説明する。図２の（a-1）、（b-1）、（c-1）は、移動体１００が、地図作成対象の環境内を移動している様子を上から見た時の図である。図は、移動体１００が時間が経過するにつれて、（a-1）、（b-1）、（c-1）の順に移動したことを示している。

次に、図２（a-2）、（b-2）、（c-2）はそれぞれ、移動体１００が図２（a-1）、（b-1）、（c-1）に示された位置に存在するときに、レーザ距離センサ１から周囲（左右１３５°の範囲）にレーザを照射することで計測・収集された距離データを概念的に示したものである。図２（a-2）、（b-2）、（c-2）に示す複数の実線矢印はそれぞれ、移動体１００から矢印の終点（つまり測定点）までの距離及び角度を計測していることを表している。ここでは説明を分かりやすくするために、距離データを地図上の実線矢印で表現しているが、実際に収集される距離データは上で述べたとおり、距離及び角度（ｄ_ｕ，θ_ｕ）の集合である。

本実施例に係る地図作成システムは、図２（a-2）、（b-2）、（c-2）の距離データを組み合わせることにより、移動体１００の周囲にある物体の形状を把握することができる（なお、正確には、形状を把握可能な物体は、レーザ距離センサ１と同じ高さあるいはそれ以上の高さを有する物体に限られるが、ここでは地図作成処理で認識（計測）されるべき物体はいずれも、レーザ距離センサ１の高さ以上の高さを有するものとする）。図２（a-2）、（b-2）、（c-2）では、レーザ距離センサ１により計測された周囲の物体の形状を太い実線で示している。

物体存在地図作成手段３は、地図作成指令を受領すると、距離データを用いて地図を作成する。まず、図２（a-2）、（b-2）の距離データ同士を合成した（つなぎ合わせた）地図データを作成する。物体存在地図作成手段３による地図データの作成処理は、たとえばIterative Closest Point （ICP）アルゴリズムなどの公知の位置合わせ手法を行うことで実現できる。物体存在地図作成手段３は、図２（a-2）、（b-2）の距離データ同士を合成した地図データを作成した後、その地図データに対してさらに図２（c-2）の距離データを合成する。図２（a-2）、（b-2）、（c-2）の距離データを用いて作成（合成）された地図の例を、図２（d-2）に示す。なお、ここでは説明のために、図２（d-2）のように図形表現されたものを、地図の例として示しているが、物体存在地図作成手段３が作成する地図（地図データ）は実際には、物体の位置情報（座標）の集合で構成される。

図２を用いて説明した例では、移動体１００の移動する環境内に装置Ａから装置Ｄまでの静止物体しか存在しなかったため、物体存在地図作成手段３により、静止物体だけが存在する理想的な地図を作成することができる。

これに対して、移動体１００の周囲に動的物体が存在する時に、物体存在地図作成手段３が地図作成を行った時に起こり得る問題点を、図３を用いて以下に説明する。図３（a-3）、（b-3）、（c-3）は図２（a-1）、（b-1）、（c-1）と同様に、移動体１００が、地図作成対象の環境内を移動している様子を表しており、図３（a-4）、（b-4）、（c-4）の実線矢印は、移動体１００が図３（a-3）、（b-3）、（c-3）の位置に存在する時に測定した距離データを表している。図２と図３の違いは、図３の環境では、移動体１００の前方を、動的物体の１つである作業者２００が時間とともに移動している点が、図２で説明した環境と異なる。この場合、図３（a-4）、（b-4）、（c-4）に示されるように、装置Ａ〜Ｄなどの静止物体の位置の情報が距離収集手段２に収集されるだけでなく、作業者２００がいる位置の情報も収集される。また、作業者２００がいた影響により、作業者２００の背後にある装置Ｂや右側壁の一部が計測できていないことがわかる。

物体存在地図作成手段３がこれらの３つの場所で計測された距離データについて、図２で説明した方法と同様の演算を行うと、図３（d-4）に示す地図が作成される。なお、この図は図３（a-4）、（b-4）の間に３つの計測した距離データによる地図情報があったものとして、表示している。同様に、図３（b-4）、（c-4）の間にも３つの距離データの情報があったものとしている。つまり図３（d-4）は、計９組の距離データによる地図情報を重ねあわせた地図である。

図３（d-4）の地図には、作業者２００が歩行したときの形跡が地図に表れている。このように、物体存在地図作成手段３によって作成された地図上に記録されてしまう動的物体の情報を、本実施例では「地図ノイズ」と呼ぶ。移動体１００が地図ノイズを含んだ地図を用いて移動体１００の位置同定を行うと、位置精度が低下することがある。さらに、場合によっては、位置同定を失敗することもある。

次に、本実施例に係る地図作成システムにおける空き領域地図作成手段８の動作について説明する。例えば、図３（a-4）に示すように、移動体１００が所定の角度範囲（たとえば270°の範囲）の距離データを収集することは、移動体１００（正確にはレーザ距離センサ１）から各測定点（Ｐ_−ｎ〜Ｐ_ｎ）までの距離を得られるとともに、移動体１００と各測定点までの間には、他の物体が存在しないことを意味する。

図４（a-5）、（b-5）、（c-5）は、物体存在地図作成手段３によって作成された物体存在地図（図３（d-4））の上に、レーザ距離センサ１により検出され距離収集手段２に収集された距離データを重ねて図示したものである。また図４（a-5）、（b-5）、（c-5）に示した網掛けされた領域は、点Ｏと測定点Ｐ_−１３５，Ｐ_−１３４，．．．，Ｐ_０，Ｐ_１，．．．，Ｐ_１３５を頂点とする多角形で、この領域は物体が存在しない領域であることを意味している。本実施例では、点Ｏと各測定点Ｐ_−１３５，Ｐ_−１３４，．．．，Ｐ_０，Ｐ_１，．．．，Ｐ_１３５を頂点とする多角形（つまり図４（a-5）、（b-5）、（c-5）に示されるような、網掛けされた領域）のことを「空き領域」と呼ぶ。

図４（a-5）、（b-5）、（c-5）で重要なことは、物体存在地図（たとえば図３（d-4））上の太い実線のうち、空き領域の中に含まれる線は、レーザ距離センサ１が作業者２００等のような動く物体を計測したために得られたもの（つまり地図ノイズ）であるという事実である。そこで、空き領域内の太い実線を削除すれば、動的物体の情報（地図ノイズ）を取り除くことができる。

しかしながら、精度の低いレーザ距離センサ１を用いた場合には、照合のときに生じる誤差が大きいことがある。その結果、地図修正手段９が空き領域内に含まれる物体の位置情報を削除すると、装置や壁などの静止物体の情報も削除してしまう可能性がある。それを防止するために、空き領域地図作成手段８は、以下に説明する距離短縮手段１０と空き領域合成手段１１を有している。

空き領域地図作成手段８は距離収集手段２から収集した距離データ（つまり、各測定点の座標）を、まず距離短縮手段１０に渡す。距離短縮手段１０は渡された距離データの値から一律に一定の距離Loを減じた値を空き領域合成手段１１に出力する。具体的には距離短縮手段１０は、距離収集手段２が収集した各測定点Ｐ_ｕの座標（ｄ_ｕ，θ_ｕ）から距離Loを減算したもの、つまり（ｄ_ｕ−Lo，θ_ｕ）を求め、これを空き領域合成手段１１に出力する。この距離Loは０以上の任意の値でよいが、好ましくは、照合の際に生じる位置同定誤差Leより大きい値であるとよい。また、以下では、測定点Ｐ_ｕから距離Loを減算して得られた点、つまり座標値が（ｄ_ｕ−Lo，θ_ｕ）の点を、「Ｐ_ｕ’」と表記する。

空き領域合成手段１１は、距離短縮手段１０から取得したＰ_ｕ’を基に図４（a-6）、（b-6）、（c-6）に示すような領域（これは、点Ｏ及び点Ｐ_−１３５’，Ｐ_−１３４’，．．．，Ｐ_０’，Ｐ_１’，．．．，Ｐ_１３５’を頂点とする多角形である）を特定する。ここで特定された領域のことを「削除対象領域」と呼ぶ。空き領域合成手段１１はこの削除対象領域の情報（点Ｏ及び点Ｐ_−１３５’，Ｐ_−１３４’，．．．，Ｐ_０’，Ｐ_１’，．．．，Ｐ_１３５’を頂点とする多角形の情報）を地図修正手段９に出力する。削除対象領域の情報を受領した地図修正手段９は、物体存在地図作成手段３で作成された地図（たとえば図３（ｄ−４））から削除対象領域内に含まれる物体の位置情報（これは、図４（a-6）、（b-6）、（c-6）に示された網掛けされた領域内に存在する太い実線に相当する
）を削除する処理を行う。

距離短縮手段１０はこのようにして削除対象領域を求めるため、たとえば図４（a-6）に示されているように、静止物体と削除対象領域の間には一定の距離（空間）が設けられる。そのため、地図修正手段９が削除対象領域に含まれる物体の削除を行っても、静止物体を削除してしまう可能性を低くできる。

地図修正手段９が、物体存在地図作成手段３で作成された地図から、空き領域合成手段１１で作成された削除対象領域（図４（ａ−６））内に含まれる物体の位置情報を削除した結果を図４（a-7）に示す。図４（a-7）では、図４（ａ−６）内にあった太実線の一部が破線になっているが、これは削除対象領域内に含まれていた物体の位置情報が削除されたことを示す。この図では、まだ、作業者２００の動的物体の情報の一部が残った状態であるが、削除対象領域の作成及びそれを用いた物体の削除処理を繰り返し行うことで、動的物体の位置情報の多くを削除できる。その例を以下に説明する。

距離短縮手段１０及び空き領域合成手段１１は上で説明した方法により、移動体１００が図４（ｂ−５）に示された位置に移動した時の距離データを用いて、図４（b-6）に示されるような削除対象領域を作成する。図４（b-6）の削除対象領域が作成されると、地図修正手段９は図４（a-7）に示された地図（これは先の処理により、物体存在地図作成手段３で作成された地図から動的物体の一部が削除された地図である）から、さらに図４（b-6）の削除対象領域内に含まれる物体の位置情報の削除を実施する。その結果、図４（b-7）の地図に示すように、残っていた動的物体の情報がすべて削除された地図を作成することができる。この実施例の場合には、この時点で動的物体の情報を完全に削除できたが、まだ動的物体の情報が残っている場合には、同様の処理を図４（c-5）、図４（c-6）で示すように、すべての距離データについて実施し、最終的に、図４（c-7）の静止物体地図が完成する。

本実施例に係る地図作成システムは以上のような方法により、地図ノイズを削除することができるので、レーザ距離センサの精度が低い場合でも、静止物体だけの地図を作成することができる特徴がある。

続いて実施例２に係る地図作成システムの説明を行う。実施例２に係る地図作成システムは、空き領域地図作成手段８に代えて空き領域地図作成手段８’を有する。それ以外の点については、実施例２に係る地図作成システムの構成は実施例１に係る地図作成システムと同じであるため、図示を略す。

図５に、実施例２に係る地図作成システムが有する空き領域地図作成手段８’の構成を示す。空き領域地図作成手段８’は空き領域合成手段１１と領域短縮手段１２を有する。空き領域合成手段１１は実施例１におけるものと同じ機能を持つ。だが、空き領域地図作成手段８’は実施例１で説明した距離短縮手段１０を有さず、実施例２における空き領域合成手段１１は距離収集手段２から取得した各測定点Ｐ_ｕの座標（つまり距離をLoだけ短縮する処理が行われていない座標）を用いて空き領域を求める。そのため、距離収集手段２がたとえば図３（a-4）の距離データを収集した場合、空き領域合成手段１１は図４（a-5）に示された領域（つまり、点Ｏと測定点Ｐ_−１３５，Ｐ_−１３４，．．．，Ｐ_０，Ｐ_１，．．．，Ｐ_１３５を頂点とする多角形）を求め、その領域の情報を領域短縮手段１２に出力する。そして実施例２に係る地図作成システムでは、領域短縮手段１２が削除対象領域を作成する。

次に、領域短縮手段１２における処理について図６（a-8）（a-9）を用いて説明する。領域短縮手段１２では、図６（a-8）に示すように、各測定点の各座標点を中心にした半径Loの円を求める。以下では、測定点Ｐ_ｕを中心とする半径Loの円のことを“Ｒ_ｕ”と表記する（ｕは実施例１と同じく、−１３５以上１３５以下の整数とする）。

次に領域短縮手段１２は、図６（a-9）のように、隣り合う２つの円（たとえば円Ｒ_ｕとＲ_ｕ＋１）に接する接線を求める。理論上、２つの円に接する接線は２つ存在するが、領域短縮手段１２は２つの接線のうち、空き領域の内側に位置する接線を求める。以下では円Ｒ_ｕとＲ_ｕ＋１に接する接線を“Ｔ_ｕ”と表記する。

そして領域短縮手段１２は、各接線Ｔ_ｕ、各円Ｒ_ｕの弧、点Ｏと測定点Ｐ_−１３５を結ぶ線分、そして点Ｏと測定点Ｐ_１３５を結ぶ線分とで取り囲まれる領域を削除対象領域と決定し、この削除対象領域の情報を地図修正手段９に出力する。なお、説明ではわかりやすくするため、図を利用して削除対象領域を求める方法を説明したが、この削除対象領域は計算によっても求められることは言うまでもない。これによって得られた削除対象領域を用いて、地図修正手段９は図６（a-10）のように、物体存在地図から動的物体の情報を削除する。図６（a-11）は、削除対象領域を用いて、物体存在地図から動的物体の情報を削除した結果を示している。

図１１を用いて、実施例１における方法で作成された削除対象領域と、実施例２における方法で作成される削除対象領域の違いを説明する。ここでは説明が複雑化することを避けるため、２つの測定点Ｐ_ａ、Ｐ_ａ＋１（これらは装置Ｘ上の点である）の距離データから削除対象領域を求める例を説明することで、両者の比較を行う。

図１１（Ａ）は実施例１での削除対象領域を求める方法を概念的に示した図である。実施例１における空き領域地図作成手段８では先に述べたとおり、測定点Ｐ_ａの座標（ｄ_ａ，θ_ａ）から距離Loだけ点Ｏ（レーザ距離センサ１）に近い点（図１１（Ａ）における点Ｐ_ａ’）を求める。同様に測定点Ｐ_ａ＋１の座標（ｄ_ａ＋１，θ_ａ＋１）から距離Loだけ点Ｏに近い点Ｐ_ａ＋１’を求め、点Ｐ_ａ’、点Ｐ_ａ＋１’及び点Ｏによって囲まれる領域を削除対象領域とする（図１１（Ａ）における網掛けされた領域）。

そのため、削除対象領域と装置Ｘの間の距離はLoよりも小さくなることがある。特に図１１（Ａ）のように、レーザ距離センサ１による照射光（たとえば線分Ｏ−Ｐ_ａあるいは線分Ｏ−Ｐ_ａ＋１）と装置Ｘ上の面（線分Ｐ_ａ−Ｐ_ａ＋１）のなす角が小さければ小さいほど、削除対象領域の外周（たとえば線分Ｐ_ａ’−Ｐ_ａ＋１’）と装置Ｘの間の距離は短くなる。そのため、レーザ距離センサ１の測定誤差が大きい場合、地図修正手段９が地図ノイズを削除する時に、装置や壁などの静止物体の情報も削除してしまう可能性が高くなる。

一方図１１（Ｂ）は実施例２における空き領域地図作成手段８’による、削除対象領域を求める方法を概念的に示した図である。実施例２における空き領域地図作成手段８’は図１１（Ｂ）のとおり、各測定点Ｐ_ａ、Ｐ_ａ＋１を中心とする半径Loの円（Ｒ_ａ、Ｒ_ａ＋１）を求め、さらにこの２つの円（Ｒ_ａ、Ｒ_ａ＋１）に接する接線（図１１（Ｂ）における、点Ｐ_ａ’と点Ｐ_ａ＋１’を通過する線分Ｔ_ａ）を求める。最後に空き領域地図作成手段８’は、点Ｏからこの接線Ｔ_ａまたは円Ｒ_ａ（または円Ｒ_ａ＋１）の弧に至るまでの領域（図１１（Ｂ）における網掛けされた領域）を、削除対象領域と決定する。

図１１（Ｂ）から分かる通り、実施例２の方法によって作成される削除対象領域の外周上の各点（接線Ｔ_ａまたは円Ｒ_ａの弧の上の点）は、物体上の任意の点（たとえば図１１における装置Ｘ上の各点）から距離Lo以上離れている（言い換えると、削除対象領域の外周上の各点は、物体上の各点からの距離の最小値がLoである）。実施例２の方法によれば、レーザ距離センサ１による照射光（たとえば線分Ｏ−Ｐ_ａあるいは線分Ｏ−Ｐ_ａ＋１）と装置Ｘ上の面（線分Ｐ_ａ−Ｐ_ａ＋１）のなす角の大きさに依らず、常に物体上の各点からの距離の最小値がLoである点を外周とする削除対象領域を得ることができる。そのため、地図修正手段９が静止物体を削除してしまう可能性をさらに低減することができる。

なお、上で説明した、空き領域地図作成手段８’による削除対象領域の決定方法は一例であり、必ずしも上で説明した方法を用いて削除対象領域を決定しなければならないわけではない。物体上の各点からの距離の最小値がLoである点を外周とする削除対象領域が得られる方法であれば、空き領域地図作成手段８’は任意の方法を用いて削除対象領域を求めてよい。

図６では、移動体１００が環境内の左端にいる時に得られた距離データをもとに削除対象領域を得る方法を説明したが、空き領域地図作成手段８’は同様にして、各時点で得られた距離データ、たとえば移動体１００が図７（b-10）、（c-10）に示される位置にいた時に得られた距離データを用いて削除対象領域を作成して、物体存在地図作成手段３によって作成された地図の中から、削除対象領域に含まれる動的物体の情報を削除する処理を繰り返す。図７（b-11）は、移動体１００が図７（b-10）に示される位置にいた時に得られた距離データを用いて作成された削除対象領域に基づいて、図６（a-11）から動的物体の情報を削除した結果の例を表している。また図７（c-11）は、移動体１００が図７（c-10）に示される位置にいた時に得られた距離データを用いて作成された削除対象領域に基づいて、図７（b-11）から動的物体の情報を削除した結果の例を表している。なお、この実施例の場合には、一部の動的物体を取り除くことができていないが、取り除けなかった動的物体の情報は僅かであり、位置同定を行う際に特に問題になることはないものである。

実施例３に係る地図作成システムは、実施例１または２で説明した地図作成システムを自律移動ロボットに適用したものである。図８にその機能ブロック図を示す。なお図８でも実施例１と同様に、自律移動ロボットの外観や、走行に必要な車輪等の存在の記述は略している。実施例３に係る地図作成システムでは、移動体１００に実施例１等で説明したレーザ距離センサ１、距離収集手段２が搭載され、また移動体１００との間で通信を行う上位コントローラ１０１に物体存在地図作成手段３、空き領域地図作成手段８’、及び、地図修正手段９が設けられる。移動体１００と上位コントローラ１０１の間の通信手段は、無線通信でも有線通信でも良い。

物体存在地図作成手段３、及び地図修正手段９は実施例１で説明したものと同じものであり、また空き領域地図作成手段８’’は、実施例２で説明したものと同様のもので、空き領域合成手段１１’と領域短縮手段１２を内蔵している。領域短縮手段１２は、実施例２で説明したものと同じものである。空き領域合成手段１１’の処理内容については後述する。

さらに、上位コントローラ１０１には、上位側地図保存手段１３を備える。上位側地図保存手段１３は、地図修正手段９で作成された静止物体地図を保存する。また、移動体１００には、車載側地図保存手段１４、位置算出手段１５、車両制御手段１６、そして左車輪と右車輪（非図示）、左車輪を駆動するための左モータ１７、右車輪を駆動するための右モータ１８、地図更新指令手段１９を備えている。

実施例３において、移動体１００は作成した静止物体地図を用いて位置を同定し走行する自律移動ロボットである。まず、上位コントローラ１０１の上位側地図保存手段１３に保存された静止物体地図が、移動体１００の車載側地図保存手段１４にダウンロードされる。移動体１００が起動すると、位置算出手段１５において、車載側地図保存手段１４に格納された静止物体地図と、レーザ距離センサ１から得られた距離データを照合することにより、移動体１００の現在の姿勢を算出する。なお位置算出手段１５が現在の姿勢を算出する際、静止物体地図上の座標値を算出する。

レーザ距離センサ１から得られた距離データと静止物体地図とを照合することで移動体１００の姿勢を求める方法は、位置同定技術、あるいは、ＳＬＡＭ（Simultaneous Localization and Mapping）技術としてよく知られている。たとえば特開２００９−１０９２００号公報や特開２００９−９３３０８号公報などに記載されている技術であるため、ここでは詳細説明は略し、姿勢を求める方法の概要のみ記す。位置算出手段１５は車載側地図保存手段１４に格納されている静止物体地図を取得する。なお静止物体地図は、物体の存在する位置の座標が、環境内の所定の位置を原点とした直交座標系（ｘｙ座標系）上の座標として格納されているものとし、以下では静止物体地図を構成する座標のセットを「第一座標群」と呼ぶ。

続いて位置算出手段１５はレーザ距離センサ１から受領した距離データを、移動体１００（正確にはレーザ距離センサ１）の位置を原点としたときの、直交座標系（ｘｙ座標系）上の座標に変換する。これにより移動体１００の位置を原点としたときの、周囲の物体の座標が得られる。なお、距離データには複数（たとえば２７１個）の情報（距離と角度の組）が含まれているので、複数個（２７１個）の座標が得られる。この複数個の座標を「第二座標群」と呼ぶ。

次に位置算出手段１５は、第二座標群の各座標を、原点（移動体１００）を中心に角度θだけ回転させ、また（ｘ，ｙ）平行移動させた座標値を求める（ｘ，ｙ，θの値は任意で良いが、小さい値が好ましい）。ここで求められた座標を「移動後の第二座標群」と呼ぶ。

そして位置算出手段１５は、移動後の第二座標群の各点と第一座標群の各点とが一致するか判定する処理を行い、この判定で一致する点の数を計数する。位置算出手段１５は、ｘ，ｙ，θの値それぞれを少しずつ変化させながら、繰り返しこの判定処理を行うことにより、第二座標群と第一座標群とが一致する点が最も多くなる時のｘ，ｙ，θを求める。ここで求められた（ｘ，ｙ）が、移動体１００の位置する座標（静止物体地図上の座標）となり、θは移動体１００の向きを表す。位置算出手段１５はこのようにして照合結果から、レーザ距離センサ１を搭載した移動体１００の位置（静止物体地図上の座標）を算出する。

なお、移動後の第二座標群の各点と第一座標群の各点とは、必ずしもすべて一致しないこともある。たとえば、静止物体地図が作成された後、移動体１００の居る環境内の一部の物体の位置が変更されることもある。その場合、移動後の第二座標群の各点と第一座標群の各点とは、すべては一致しない。位置算出手段１５は、移動後の第二座標群の各点と第一座標群の各点の一致する割合が、所定値よりも少ない場合、車載側地図保存手段１４に格納されている静止物体地図に記録されている物体の位置と、実際の物体の位置の間にずれが発生しており、静止物体地図の情報を更新すべきと判断する。そして位置算出手段１５は距離収集手段２に収集指令を出力することで、上位コントローラ１０１に新たな地図を作成させる。この処理は後述する。

次に、車両制御手段１６に対して外部から移動すべき目標点の座標ｘo、ｙo、θoが与えられると、車両制御手段１６は移動体１００の走行制御を行う。車両制御手段１６は移動体を自律移動可能にするための制御部で、たとえば特開２００９−９３３０８号公報などに記載の公知のものでよい。車両制御手段１６は移動体１００の現在の位置（ｘ、ｙ、θ）と目標点の座標ｘo、ｙo、θoとの関係から、移動体１００の速度指令と曲率を算出する。車両制御手段１６はその結果を基に、左モータ１７と右モータ１８の速度指令を出力し、それにより各モータが速度制御される。その結果、移動体１００は目標点に向かって走行することになる。

続いて図９、図１０を用いて、本実施例に係る地図作成システムが地図を作成する処理の流れを説明する。なお、以下の説明では、初期状態では車載側地図保存手段１４に図４（c-7）に示された静止物体地図が格納されており、また移動体１００は図９（a-12）、（b-12）、（c-12）に示されるように、環境内を左から右に順に移動するケースについて説明する。

また、以下の例では、移動体１００の周囲に配置される物体の位置が、必ずしも車載側地図保存手段１４に格納された静止物体地図に記録されている位置とは一致しないケースを説明する。具体的には図９に示されているように、環境内の物体のうち、装置Ｂが図４（c-7）に示された配置状態から右方向に傾いた状態に変化している。

図９（a-12）、（b-12）、（c-12）は、移動体１００が車両制御手段１６により自律走行をして、左側から右側に移動している状態を時系列的に示している。また、それらの図には、対向する方向から動的物体である走行車３００が右側から中央方向に移動している状況であることが示されている。

図９（a-13）は、図４（c-7）に示された地図とレーザ距離センサ１で得られた距離データを照合して、位置算出手段１５で行う位置同定処理により、移動体１００の位置を算出している様子を概念的に表したものである。またここで位置算出手段１５によって特定された移動体１００の座標値は、（ｘa、ｙa、θa）とする。なお、図９の例では装置Ｂの位置が静止物体地図に記録されている位置情報と異なっており、また静止物体地図には記録されていない走行車３００が存在しているため、位置算出手段１５が静止物体地図と距離データを照合すると、静止物体地図の情報と距離データの一致する割合が低下していると判断される。そのため、このときには、距離収集手段２に対して、位置算出手段１５から収集指令が出力される。

収集指令を受けた距離収集手段２は、レーザ距離センサ１から出力される距離データを収集し、収集された距離データを上位コントローラ１０１へと送信（アップロード）する。図９（a-14）に収集された距離データの例を示す。図９（a-14）は、移動体が図９（a-12）に示された位置に存在した時に、距離収集手段２が収集した距離データを概念的に表した図である。

時間の経過とともに、移動体１００が図９（b-12）、（c-12）のように移動したときも、位置算出手段１５は同じように静止物体地図と距離データを照合することで、移動体１００の位置、姿勢を求める。ここでは、移動体１００が図９（b-12）、（c-12）の位置にいる時の座標をそれぞれ、（ｘb、ｙb、θb）と、（ｘc、ｙc、θc）とする。それと同時に、距離収集手段２では、図９（b-14）、（c-14）に示す距離データが収集され、収集された距離データは上位コントローラ１０１へとアップロードされる。図９（b-14）、（c-14）はそれぞれ、移動体が図９（b-12）、（c-12）に示された位置に存在した時に、距離収集手段２が収集した距離データを概念的に表した図である。

図９（a-14）、（b-14）、（c-14）の距離データが距離収集手段２から、上位コントローラ１０１にアップロードされると、物体存在地図作成手段３は図１０（d-14）に示す地図を作成する。地図作成の契機は、距離収集手段２がたとえば実施例１で説明した地図作成指令を物体存在地図作成手段３に発行したことを契機に行われてもよいし、あるいは物体存在地図作成手段３は所定個の距離データが上位コントローラ１０１にアップロードされたことを契機に地図作成を行ってもよい。

図１０を用いて、本実施例に係る地図作成システムにおける地図ノイズを削除する方法を説明する。図８の空き領域合成手段１１’は、まず、実施例２で説明した方法と同様の方法で、図９（a-14）、（b-14）、（c-14）の距離データから得られる空き領域地図を作成する。図１０（a-15）、（b-15）、（c-15）の網掛けされた領域がそれぞれ、図９（a-14）、（b-14）、（c-14）の距離データから得られる空き領域地図を表している。

次に空き領域合成手段１１’は、これらの空き領域地図の和集合を求めることで、すべての距離データを用いた空き領域地図（合成された空き領域地図）を求める。図１０（d-15）に示している網掛けされた領域が、合成された空き領域地図の例である。

続いて空き領域合成手段１１’はこの空き領域地図を領域短縮手段１２に入力する。領域短縮手段１２は実施例２で説明した方法を用いて、外周が、装置や壁などの静止物体から距離Loだけ離れた点で構成される削減対象領域を作成する。図１０（d-16）に示された網掛けされた領域が、作成された削減対象領域の例を示す。

削除対象領域の作成後、地図修正手段９は削減対象領域の情報を用いて、物体存在地図作成手段３で作成された地図から地図ノイズの削除を行う。これは実施例１または２で説明した方法と同様である。

実施例３で説明した空き領域地図の作成方法は、実施例１または実施例２で説明したものと比べると、一旦合成された空き領域地図を作成した後に、合成された空き領域地図を用いて削減対象領域を決定し地図ノイズの削除を行うため、地図ノイズの削除処理のコストを、実施例１または２で説明した方法よりも小さくできる利点がある。ここで作成された削減対象領域をもとにして、図１０（d-14）の物体存在地図から地図ノイズを削除すると、図１０（d-17）に示すような静止物体地図を得ることができる。地図修正手段９は、作成した静止物体地図を上位側地図保存手段１３に保存する。

なお、実施例３では地図作成システムが空き領域地図作成手段８’’によって削減対象領域を作成する例を説明したが、空き領域地図作成手段８’’に代えて、実施例１で説明した空き領域地図作成手段８、あるいは実施例２で説明した空き領域地図作成手段８’を用いる構成（つまり上位コントローラ１０１が空き領域地図作成手段８または空き領域地図作成手段８’を備える構成）であってもよい。またここでは、空き領域地図作成手段８’’は複数の空き領域地図から合成された空き領域地図（たとえば図１０(d-15)）を一旦作成し、合成された空き領域地図から、削除対象領域を求める例を説明したが、別の手順で削除対象領域を求めてもよい。一例として空き領域地図作成手段８’’は、まず距離収集手段２から距離データ（たとえば図９（a-14）、（b-14）、（c-14））を取得するたびに削除対象領域を作成する。削除対象領域の作成方法は実施例１の方法でも実施例２の方法でも良い。距離データを複数回（たとえばｎ回）取得した場合、ｎ個の削除対象領域が作成される。ｎ個の削除対象領域が作成された後、空き領域地図作成手段８’’はｎ個の削除対象領域の和集合を求めることで、最終的な削除対象領域を得る。

さらに、移動体１００の位置算出手段１５は、収集指令を距離収集手段２に出力する時、同時に地図更新指令手段１９にも地図と距離データの一致する割合が低下している状態を出力する。地図更新指令手段１９はこれにより、地図の更新が必要であると判断し、上位側地図保存手段１３に対して更新指令を出力する。上位側地図保存手段１３は更新指令を受領すると、（地図修正手段９によって）上位側地図保存手段１３に新たに保存された静止物体地図を移動体１００に送信する。移動体１００は新たに作成された静止物体地図を受領すると、それを車載側地図保存手段１４に格納することで、静止物体地図の更新を行う。

以上のように、本実施例に係る地図作成システムでは、自律走行状態の移動体の距離データを用いて、動的物体がある場合でも、静止物体だけの地図を得ることができ、自動的に地図更新を行うことができる利点がある。

本発明において、以上のように、性能が低い距離センサを用いても、誤って静止物体を削除して動領域を拡大してしまうことがなく、地図の精度を低下させないで、自動的に動的物体の情報を削除することができるので、これにより作成した地図を用いた位置同定の精度を確保し、優れた移動制御特性を有する自律移動ロボットを提供することができる。

１ レーザ距離センサ， ２ 距離収集手段， ３ 物体存在地図作成手段， ８ 空き領域地図作成手段， ９ 地図修正手段， １０ 距離短縮手段， １１ 空き領域合成手段， １２ 領域短縮手段， １３ 上位側地図保存手段， １４ 車載側地図保存手段， １５ 位置算出手段， １６ 車両制御手段， １７ 左モータ， １８ 右モータ， １９ 地図更新指令手段， ２０ 収集判定手段， １００ 移動体， １０１ 上位コントローラ， ２００ 作業者（動的物体１）， ３００ 走行車（動的物体２）

Claims (15)

1. 周囲の物体上の複数の測定点までの距離及び角度を計測し、前記距離及び角度の集合である距離情報を出力する距離センサを備えた移動体を有する地図作成システムであって、
   前記地図作成システムは、
   前記移動体が第１の位置に存在する時に計測された第１の距離情報と、前記移動体が第２の位置に存在する時に計測された第２の距離情報とを合成することで、前記物体が存在する位置を表す位置情報を記録した物体存在地図を作成する、物体存在地図作成手段と、
   前記距離情報をもとに、前記物体の存在しない領域である削除対象領域を特定する空き領域地図作成手段と、
   前記物体存在地図作成手段が作成した前記物体存在地図から、前記削除対象領域に含まれる位置に相当する位置情報を削除することで、前記物体のうち動的物体の位置情報を削除した静止物体地図を作成する地図修正手段と、
   を有することを特徴とする、地図作成システム。
2. 前記空き領域地図作成手段は、前記測定点から所定の距離だけ前記距離センサに近い点によって囲まれる領域を、前記削除対象領域と決定する、
   ことを特徴とする、請求項１に記載の地図作成システム。
3. 前記空き領域地図作成手段によって作成される前記削除対象領域の外周上の各点は、前記物体からの距離の最小値が所定の固定値である、
   ことを特徴とする、請求項１に記載の地図作成システム。
4. 前記空き領域地図作成手段は、前記第１の距離情報を用いて第１の削除対象領域を特定し、前記第２の距離情報を用いて第２の削除対象領域を特定し、
   前記地図修正手段は、前記物体存在地図に含まれている前記位置情報のうち、前記第１の削除対象領域に含まれる位置に相当する位置情報と、前記第２の削除対象領域に含まれる位置に相当する位置情報とを削除することで、前記静止物体地図を作成する、
   ことを特徴とする、請求項１に記載の地図作成システム。
5. 前記地図作成システムはさらに、前記物体存在地図作成手段、前記空き領域地図作成手段、前記地図修正手段を有する上位コントローラを備え、
   前記移動体は、
   前記静止物体地図を格納する地図保存手段と、
   前記距離センサが出力する前記距離情報を収集して前記上位コントローラに送信する距離収集手段と、
   前記地図保存手段に格納された前記静止物体地図と、前記距離センサによって計測された前記距離情報とを照合することで、前記移動体の位置を特定する位置算出手段と、
   を有することを特徴とする、請求項１に記載の地図作成システム。
6. 前記位置算出手段は、前記静止物体地図と前記距離情報の照合の結果、前記静止物体地図の情報を更新すべきと判断した場合、前記距離収集手段に対し収集指令を送信し、
   前記距離収集手段は前記収集指令の受領に応じて、前記距離情報を前記上位コントローラに送信し、前記上位コントローラに前記静止物体地図を作成させる、
   ことを特徴とする、請求項５に記載の地図作成システム。
7. 前記位置算出手段が前記距離収集手段に前記収集指令を送信する際、前記移動体は前記上位コントローラに前記静止物体地図の更新指令を送信し、
   前記上位コントローラは、前記更新指令を受信すると、新たに作成された前記静止物体地図を、前記移動体に送信し、
   前記移動体は、前記上位コントローラから受領した前記静止物体地図を前記地図保存手段に格納する
   ことを特徴とする、請求項６に記載の地図作成システム。
8. 前記移動体は、前記位置算出手段が算出した前記移動体の位置を用いて、前記移動体の走行制御を行う車両制御手段を備える、
   ことを特徴とする、請求項６に記載の地図作成システム。
9. 周囲の物体上の複数の測定点までの距離及び角度を計測し、前記距離及び角度の集合である距離情報を出力する距離センサを備えたロボットを有するロボットシステムであって、
   前記ロボットシステムは、
   前記ロボットが第１の位置に存在する時に計測された第１の距離情報と、前記ロボットが第２の位置に存在する時に計測された第２の距離情報とを合成することで、前記物体が存在する位置を表す位置情報を記録した物体存在地図を作成する、物体存在地図作成手段と、
   前記距離情報をもとに、前記物体の存在しない領域である削除対象領域を特定する空き領域地図作成手段と、
   前記物体存在地図作成手段が作成した前記物体存在地図から、前記削除対象領域に含まれる位置に相当する位置情報を削除することで、前記物体のうち動的物体の位置情報を削除した静止物体地図を作成する地図修正手段と、
   を有することを特徴とする、ロボットシステム。
10. 前記空き領域地図作成手段は、前記測定点から所定の距離だけ前記距離センサに近い点によって囲まれる領域を、前記削除対象領域と決定する、
    ことを特徴とする、請求項９に記載のロボットシステム。
11. 前記空き領域地図作成手段によって作成される前記削除対象領域の外周上の各点は、前記物体からの距離の最小値が所定の固定値である、
    ことを特徴とする、請求項９に記載のロボットシステム。
12. 前記空き領域地図作成手段は、前記第１の距離情報を用いて第１の削除対象領域を特定し、前記第２の距離情報を用いて第２の削除対象領域を特定し、
    前記地図修正手段は、前記物体存在地図に含まれている前記位置情報のうち、前記第１の削除対象領域に含まれる位置に相当する位置情報と、前記第２の削除対象領域に含まれる位置に相当する位置情報とを削除することで、前記静止物体地図を作成する、
    ことを特徴とする、請求項９に記載のロボットシステム。
13. 前記ロボットシステムはさらに、前記物体存在地図作成手段、前記空き領域地図作成手段、前記地図修正手段を有する上位コントローラを備え、
    前記ロボットは、
    前記静止物体地図を格納する地図保存手段と、
    前記距離センサが出力する前記距離情報を収集して前記上位コントローラに送信する距離収集手段と、
    前記地図保存手段に格納された前記静止物体地図と、前記距離センサによって計測された前記距離情報とを照合することで、前記ロボットの位置を特定する位置算出手段と、
    を有することを特徴とする、請求項９に記載のロボットシステム。
14. 前記位置算出手段は、前記静止物体地図と前記距離情報の照合の結果、前記静止物体地図の情報を更新すべきと判断した場合、前記距離収集手段に対し収集指令を送信するとともに前記上位コントローラに前記静止物体地図の更新指令を送信し、
    前記距離収集手段は前記収集指令の受領に応じて、前記距離情報を前記上位コントローラに送信し、
    前記上位コントローラは、前記距離収集手段から送信された前記距離情報をもとに前記静止物体地図を新たに作成して前記ロボットに送信し、
    前記ロボットは、前記上位コントローラから受領した前記静止物体地図を前記地図保存手段に格納する、
    ことを特徴とする、請求項１３に記載のロボットシステム。
15. 前記ロボットは、前記位置算出手段が算出した前記ロボットの位置を用いて、前記ロボットの走行制御を行う車両制御手段を備える、
    ことを特徴とする、請求項１４に記載のロボットシステム。

Images