JP2009157430A - 座標補正方法、座標補正プログラム、及び自律移動ロボット - Google Patents

Abstract

【課題】２つの環境地図の間の姿勢ずれを補正することで、一方の環境地図上の点を他方の環境地図の座標系に合わせて座標変換する処理を、自動的に、かつ少ない計算量で行なうことを可能とする。
【解決手段】（ａ）一方の環境地図の座標系を並進又は回転させることにより、一方の環境地図の姿勢を変動させ、（ｂ）一方の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の一方の環境地図の各障害物セルから、他方の環境地図における最寄りの障害物セルまでの距離を求める。（ｃ）複数の障害物セルについて算出した最寄りの障害物セルまでの距離の合計値を目的関数として最適化計算を行い、目的関数が最適化される一方の環境地図の姿勢を決定する。（ｄ）最適化計算により得られた一方の環境地図の姿勢に基づいて、２つの環境地図のうちの一方の地図上の座標を、他方の地図上の座標に座標変換する。
【選択図】図４

Description

本発明は、自律移動ロボットに関し、特に移動ロボットに参照される環境地図の更新時において、過去の環境地図上の点を新たな環境地図の座標系に合わせて座標変換する技術に関する。

自律移動ロボット（以下、単にロボットと呼ぶ）は、環境地図を参照することによって、壁などの障害物に衝突せずにゴール地点まで移動するための経路を計画し、当該経路に従って移動を行う。ロボット用の環境地図は、ロボットの移動空間内における移動可能領域を示す。

環境地図は、例えば、ロボットが移動を行う２次元の移動空間を格子状に分割し、格子に囲まれた各々のセルが移動可能なであるか否かを表したグリッド地図として作成される。環境地図の作成は、例えば、視覚センサを搭載したロボット自身によって行なわれる。具体的には、ロボットに視覚センサを搭載し、ロボット自身が移動空間内を移動しながら外部環境を計測し、ロボットの移動量及び視覚センサによる計測データを用いて環境地図が作成される。ロボットの自己位置推定と環境地図の構築を同時に行なう技術は、ＳＬＡＭ（Simultaneous Localization and Mapping）と呼ばれる。なお、環境地図は、予め人手により環境の計測を行って、コンピュータを用いて編集された後に、ロボットに供給される場合もある。

障害物の配置変更などによって環境地図の構成に変更が発生すると、これに合わせて新たな環境地図を作成する必要がある。新たな環境地図の作成は、上述したＳＬＡＭ等の地図作成技術を用いて行なわれる。しかしながら、一般的に、新たな環境地図を作成する際のロボットの初期位置を過去の環境地図の作成時の初期位置と完全に一致させることは困難である。したがって、新しい環境地図の座標系が古い環境地図の座標系に比べてずれてしまうことがしばしば発生する。例えば、展示場内を案内するロボットを考えると、環境地図を更新したにもかかわらず、古い環境地図に対して設定されたゴール地点の座標を目標に移動を行なった場合、ゴール地点のずれにより案内先がずれてしまう。

このため、環境地図の更新時には、古い環境地図に対して設定されていたゴール地点の座標をそのまま使用することができず、新たな環境地図の座標系に対応したゴール地点の座標を新たに生成する必要がある。このためには、ゴール地点の座標を、古い環境地図の座標系から新しい環境地図の座標系に座標変換する必要がある。一般的に、２つの地図（例えば古い環境地図と新しい環境地図）間の座標変換を行うためには、２つの地図の姿勢のずれを求める必要がある。ここで地図間の姿勢のずれとは、比較される２つの地図の間での地図原点の位置ずれ及び回転ずれである。

２つの地図の姿勢のずれを求めてこれを補正するように座標変換を行う技術が特許文献１及び２に開示されている。

特許文献１には、携帯電話端末等の利用者に対して地図画像データを提示し、地図画像データ上の点を現在地として指定するユーザの入力操作を受け付け、ユーザによって現在地に指定された地図画像データ上の点の座標と、ＧＰＳ（Global Positioning System）によって取得した実際の現在地の座標とを対応付ける技術が示されている。

特許文献２には、レーザ距離センサを搭載したロボットに移動空間内を走行させ、レーザ距離センサによる計測値からロボット周囲の障害物の有無を示すグリッド地図を順次生成し、異なるロボット位置で作成された複数のグリッド画像を結合することによって、移動空間全体の環境地図を生成する技術が開示されている。特許文献２には、異なるロボット位置で作成された複数のグリッド画像の結合手順の具体例として、以下の手順（１）〜（３）が開示されている。
（１）まず、結合する２枚のグリッド画像の各々から複数の特徴点を抽出する。
（２）次に、２枚のグリッド画像間で複数の特徴点を照合して一致する特徴点を識別する。特徴点の照合は、例えば、特徴点周囲の複数のグリッドにおける占有グリッド、非占有グリッド及び未知グリッドの数を表すヒストグラムを比較することにより行われる。ここで、占有グリッドは障害物に占有されているために走行不可能なグリッドであり、非占有グリッドは走行可能なグリッドであり、未知グリッドは未計測のグリッドである。
（３）続いて、一致した特徴点同士が重なるように、２枚のグリッド画像の重ね合わせ、すなわち座標変換を実行する。

さらに、上述した手順（２）における一致する特徴点を識別するための特徴点の照合手順の具体例として、以下の手順（２−１）〜（２−３）が開示されている。
（２−１）一方のグリッド画像の複数の特徴点の中から２点を選択し、これら２点を通る座標系を設定し、設定された座標系における他の特徴点の座標を算出する。もう一方のグリッド画像についても同様に、２つの特徴点を選択し、これら２点を通る座標系を設定し、設定された座標系における他の特徴点の座標を算出する。
（２−２）次に、２つのグリッド画像に含まれる全ての特徴点の間でヒストグラムを照合し、占有グリッドの数の差が所定の閾値以下である場合に特徴点間の相対距離を求め、得られた相対距離が所定の閾値以下である場合にこれらの特徴点が一致したものとしてカウントする。
（２−３）上記の（２−１）及び（２−２）の処理を２つの特徴点の全通りの組み合わせの座標系に対して行なう。
（２−４）特徴点の一致率の最も高かった座標系を２枚のグリッド画像の重ね合わせ用の座標系に決定する。
特開２００５−１０６９５３号公報 特開２００５−３２６９４４号公報

特許文献１に開示された技術を用いて、２つの地図（即ち地図画像データと緯度及び経度によって表される世界地図）の間の座標変換を求めるためには、ユーザによる手動操作が必要である。このため、作業が煩雑であるし、コンピュータによる自動化に適していない。例えば、特許文献１に開示された技術を用いて、上述した案内ロボット等に環境地図の更新に伴うゴール地点の座標補正を自動的に行わせることは困難である。

また、上述した特許文献２に開示された技術は、ロボットの周囲について作成された比較的小さなグリッド地図間の姿勢ずれを求めるには有効と考えられる。しかしながら、特許文献２に開示された技術は、特徴点数をｎ個とした場合に、計算量が特徴点数の４乗のオーダＯ（ｎ^４）で増大する。これは、座標系を設定するためにｎ個の特徴点の中から２点を選択する組合せ（_ｎＣ_２）のオーダがＯ（ｎ^２）であること、さらに２つの特徴点間のヒストグラムを照合して相対距離を算出するために、ｎ−２個の中から２点を選択する組合せ（_ｎ−２Ｃ_２）のオーダもまたＯ（ｎ^２）であるためである。このため、特許文献２に開示された技術では、環境地図のサイズが増大し、これに伴って環境地図から抽出される特徴点数が増大した場合に、計算量が膨大となるおそれがある。したがって、特許文献２に開示された技術を用いて、広大な移動空間に関する環境地図の姿勢ずれを補正することは実質的に困難であると考えられる。

本発明は上述した知見に基づいてなされたものであって、本発明の目的は、２つの環境地図の間の姿勢ずれを補正することで、一方の環境地図上の点を他方の環境地図の座標系に合わせて座標変換する処理を、自動的に、かつ少ない計算量で行なうことが可能な座標補正方法、座標補正プログラム、及び自律移動ロボットを提供することである。

本発明の第１の態様は、各々がグリッド地図として構成され、前記グリッド地図を構成する複数のセルの各々に関連付けて、ロボットが移動可能な領域であるか否かを示す情報が少なくとも保持される第１及び第２の環境地図の間での座標補正方法である。当該方法は、以下のステップ（ａ）〜（ｄ）を含む。
（ａ）前記第１の環境地図の座標系を並進又は回転させることにより、前記前記第１の環境地図の姿勢を変動させるステップ、
（ｂ）前記第１の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の前記第１の環境地図の各障害物セルから、前記第２の環境地図における最寄りの障害物セルまでの距離を求めるステップ、
（ｃ）前記複数の障害物セルについて算出した前記距離の合計値又は前記合計値に関連するパラメータを目的関数として最適化計算を行い、前記目的関数が最適化される前記第１の環境地図の姿勢を決定するステップ、及び、
（ｄ）前記最適化計算により得られた前記第１の環境地図の姿勢に基づいて、前記第１及び前記第２の環境地図のうちの一方の地図上の座標を、他方の地図上の座標に座標変換するステップ。

本発明の第２の態様は、各々がグリッド地図として構成され、前記グリッド地図を構成する複数のセルの各々に関連付けて、ロボットが移動可能な領域であるか否かを示す情報が少なくとも保持される第１及び第２の環境地図の間での座標補正処理をコンピュータに実行させるための座標補正プログラムである。ここで、前記座標補正処理は、以下の処理（ａ）〜（ｄ）を含む。
（ａ）前記第１の環境地図の座標系を並進又は回転させることにより、前記前記第１の環境地図の姿勢を変動させる処理、
（ｂ）前記第１の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の前記第１の環境地図の各障害物セルから、前記第２の環境地図における最寄りの障害物セルまでの距離を求める処理、
（ｃ）前記複数の障害物セルについて算出した前記距離の合計値又は前記合計値に関連するパラメータを目的関数として最適化計算を行い、前記目的関数が最適化される前記第１の環境地図の姿勢を決定する処理、及び、
（ｄ）前記最適化計算により得られた前記第１の環境地図の姿勢に基づいて、前記第１及び前記第２の環境地図のうちの一方の地図上の座標を、他方の地図上の座標に座標変換する処理。

本発明の第３の態様は、移動空間に関する環境地図を作成する機能を有する自律移動ロボットである。当該自律移動ロボットは、センサ、地図作成部、相対姿勢算出部、及び座標変換部を備える。前記センサは、前記自律移動ロボットの周囲環境を計測する。前記地図作成部は、前記センサの計測情報に基づいて、新たな環境地図を作成する。前記相対姿勢算出部は、過去に作成された環境地図と前記新たな環境地図との間の相対姿勢を算出する。前記座標変換部は、前記相対姿勢に基づいて、前記過去の環境地図上の座標を、前記新たな環境地図上の座標に変換する。ここで、前記新たな環境地図及び前記過去の環境地図は、各々がグリッド地図として構成され、前記グリッド地図を構成する複数のセルの各々に関連付けて、前記自律移動ロボットが移動可能な領域であるか否かを示す情報が少なくとも保持された地図である。さらに、前記相対姿勢算出部は、（ａ）前記新たな環境地図及び前記過去の環境地図のうちの一方の環境地図の座標系を並進又は回転させることにより、前記一方の環境地図の姿勢を変動させ、（ｂ）前記一方の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の前記一方の環境地図の各障害物セルから、他方の環境地図における最寄りの障害物セルまでの距離を求め、（ｃ）前記複数の障害物セルについて算出した前記距離の合計値又は前記合計値に関連するパラメータを目的関数として最適化計算を行い、前記目的関数が最適化される前記一方の環境地図の姿勢を決定する。

上述した本発明の第１乃至第３の態様によれば、環境地図間の相対姿勢を、一方の環境地図に含まれる障害物セルから見た他方の環境地図の最寄りの障害物セルまでの距離の合計値（又はこれに関連するパラメータ）を目的関数とする最適化計算によって求める。したがって、ユーザによる手動入力が不要であり、自律移動ロボット等が自動的に新ゴール座標の生成を行う場合に好適である。また、本発明の第１乃至第３の態様において、環境地図間の相対姿勢を求めるのに要する計算量のオーダは、２つの環境地図の各々のセル総数をｎとした場合にＯ（ｎ^２）である。このため、環境地図のサイズが大きくなった場合でも計算量の増大を抑制できる。

本発明により、２つの環境地図の間の姿勢ずれを補正することで、一方の環境地図上の点を他方の環境地図の座標系に合わせて座標変換する処理を、自動的に、かつ少ない計算量で行なうことが可能な座標補正方法、座標補正プログラム、及び自律移動ロボットを提供できる。

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。

＜発明の実施の形態１＞
本実施の形態にかかる自律移動ロボット１（以下、単にロボット１と呼ぶ）は、移動空間内の床面上を車輪１５３で走行する。なお、本実施の形態では、車輪走行型の自律移動ロボットを例にとって具体的に説明するが、脚歩行型など他の移動ロボットも本発明に含まれることは勿論である。

ロボット１の自律移動に関する制御系の構成を図１に示す。図１において、視覚センサ１１は、ロボット１の外界の距離画像データを取得する。具体的には、レーザレンジファインダ等のアクティブ距離センサによって距離画像データを取得すればよい。なお、ＣＣＤ（Charge Coupled Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサ等の撮像素子を備えた複数のカメラを備え、これら複数のカメラによって撮影した画像データを用いて距離画像データを生成してもよい。具体的には、複数のカメラによって撮影した画像データから対応点を検出し、ステレオ視によって対応点の３次元位置を復元すればよい。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。

環境地図生成部１２は、距離画像データを用いて、ロボット１が移動を行なう移動空間に関する環境地図を生成する。本実施の形態では、環境地図は、移動空間を２次元格子状に分割したグリッド地図として作成される。環境地図を構成する各セルは、少なくともロボット１が移動可能であるか否かを示す値を保持する。以下では、ロボット１が移動可能なセルを「移動可能セル」と呼び、移動不可能なセルを「壁セル」と呼ぶ。なお、移動不可能なセルをさらに分類してもよく、例えば、ロボット１が位置する場所から見て障害物を介して向こう側に位置し、未観測であるために移動不可能であるセルを「未知セル」として、観測済みの「壁セル」と分類してもよい。

ロボット１により作成される環境地図の一例を図２に示す。図２の環境地図２１は、６ピクセル×６ピクセルのグリッド地図である。図２において、セル２１０を含む白抜きのセルは、「移動可能セル」を示し、セル２１１を含む斜線でハッチングされたセルは、「壁セル」を示している。環境地図２１の各セルが、「移動可能セル」であるか「壁セル」であるかは、例えば、１ビットの値により識別すればよい。具体的には、セルの値が０のとき「移動可能セル」を示し、１のとき「壁セル」を示すこととすればよい。以下では、環境地図２１のような、各セルの値として移動可能であるか否かを示す値が保持されたグリッド地図を「壁マップ」と呼ぶ。

なお、環境地図生成部１２による環境地図の生成は、視覚センサ１１の計測から得られる距離画像データと、後述するエンコーダ１５４により取得されるオドメトリ情報を入力として、例えば、スキャンマッチング又は確率推定手法などの公知の手法を用いて行なえばよい。

相対姿勢算出部１３は、環境地図生成部１２によって新たに作成された環境地図（以下、新環境地図と呼ぶ）と、過去に作成済みの古い環境地図（以下、旧環境地図と呼ぶ）の間の相対姿勢を算出する。ここで、２次元の環境地図間の相対姿勢とは、環境地図面内での並進ずれ若しくは環境地図面に垂直な回転軸周りの回転ずれ、又はこれら並進ずれ及び回転ずれの組合せを意味する。したがって、相対姿勢は、３つのパラメータ（ｄＸ，ｄＹ，ｄΘ）で表現することができる。これは、環境地図面をｘｙ平面とした場合に、一方の環境地図（例えば旧環境地図）をｘ軸方向にｄＸだけ並進させ、ｙ軸方向にｄｙだけ並進させ、ｘｙ平面に垂直なｚ軸周りにｄΘだけ回転させることにより、一方の環境地図が他方の環境地図（例えば新環境地図）に最も類似する状態になることを表している。なお、新旧の環境地図間の相対姿勢の算出手順の具体例については後述する。

座標変換部１４は、相対姿勢算出部１３により算出された相対姿勢（ｄＸ，ｄＹ，ｄΘ）と、旧環境地図に対して設定された古いゴール座標（以下、旧ゴール座標と呼ぶ）を用いて、新環境地図の座標系に対応した新しいゴール座標を算出する。ここで、ゴール座標とは、ロボット１の到達目標地点の座標を意味する。具体的に述べると、座標変換部１４は、相対姿勢（ｄＸ，ｄＹ，ｄΘ）を用いて表される以下の座標変換式（１）によって、旧ゴール座標（Ｇ´_ｘ，Ｇ´_ｙ）を新ゴール座標（Ｇ_ｘ，Ｇ_ｙ）に座標変換すればよい。

自律移動部１５は、環境地図生成部１２及び座標変換部１４より供給される新環境地図及び新ゴール座標を入力し、新ゴール座標で表されるゴール地点まで自律的に移動するための制御を実行する。例えば、図１に示すように、自律移動部１５は、動作計画部１５０、動作制御部１５１、駆動部１５２、車輪１５３、及びエンコーダ１５４を含む。

動作計画部１５０は、新環境地図、新ゴール座標、視覚センサ１１によって取得した外界情報などに基づいて、ロボット１の動作内容を決定する。より具体的に延べると、動作計画部１５０は、ロボット１の移動経路、目標移動速度、及び目標加速度、ロボット１が備える関節（不図示）の目標角度軌道などを生成する。

動作制御部１５１は、動作計画部１５０により決定された目標移動速度又は目標加速度などの制御目標値と、エンコーダ１５４によって計測される車輪１５３の回転量を入力してフィードバック制御を実行し、車輪１５３を回転駆動するためのトルク制御値を算出する。動作制御部１５１によって算出されたトルク制御値に従って駆動部１５２が車輪１５３を駆動することにより、ロボット１の移動が行なわれる。

図３は、新環境地図の生成から座標変換により新ゴール座標を得るまでの全体的な処理手順を示すフローチャートである。ステップＳ１１では、環境地図生成部１２が新環境地図を生成する。ステップＳ１２では、相対姿勢算出部１３が、旧環境地図と新環境地図を比較することにより、これら２つの環境地図間の相対姿勢を算出する。ステップＳ１３では、座標変換部１４が、相対姿勢算出部１３により算出された相対姿勢を用いて、旧ゴール座標を新ゴール座標に変換する。最後に、ステップＳ１４では、環境地図生成部１２及び座標変換部１４が、新環境地図及び新ゴール座標を自律移動部１５に供給する。

続いて以下では、旧環境地図と新環境地図の間の相対姿勢の算出手順の具体例について、図４〜６を用いて説明する。図４は、２つの環境地図間の相対姿勢の算出手順の一例を示すフローチャートである。図４に示す具体例は、最適化計算により相対姿勢（ｄＸ，ｄＹ，ｄΘ）を求めるものである。具体的には、相対姿勢（ｄＸ，ｄＹ，ｄΘ）を変数とし、旧環境地図の壁セル座標ＣＡから新環境地図における最寄りの壁セルまでの距離（以下、壁距離と呼ぶ）を全ての壁セルに関して合計した合計値を目的関数として、当該目的関数を最小化する最適化計算を行なう。以下、図４のフローチャートに従って、処理内容を順に説明する。

ステップＳ１０１では、壁マップとして作成された旧環境地図に含まれる全ての壁セルの座標ＣＡを抽出する。なお、ここでは、全ての壁セルを対象としているが、例えば、旧環境地図の一部分（例えば、中央部分など）に含まれる壁セルのみを対象としてもよい。

ステップＳ１０２では、座標ＣＡの揺動パターンを６つの選択肢＋ｄｘ，−ｄｘ，＋ｄｙ，−ｄｙ，＋ｄθ，−ｄθの中から選択する。なお、これら６つの選択肢は、予め定められた順序で選択すればよく、後述するように、繰り返し処理によって全ての選択肢が網羅される。ｄｘは、旧環境地図の平面に含まれるｘ軸方向の微小変位を意味する。同様に、ｄｙは、旧環境地図の平面に含まれるｙ軸方向の微小変位を意味する。また、ｄθは、旧環境地図の平面に垂直なｚ軸周りの微小回転角度を意味する。なお、ｄｘ、ｄｙ及びｄθの大きさは予め設定しておけばよく、例えば、旧環境地図の１グリッドサイズ程度移動できる量とすればよい。

ステップＳ１０３では、ステップＳ１０２で選択された揺動パターンに従って、旧環境地図の壁セル座標ＣＡを移動させる。移動後の壁セル座標をＣＢとする。

ステップＳ１０４では、移動後の旧環境地図の壁セル座標ＣＢの全てについて、新環境地図の最寄りの壁セルからの距離を算出する。具体的には、後述する「壁距離マップ」に変換された新環境地図から、座標ＣＢに対応するセルに保持された壁距離の値を読み出せばよい。

ここで最寄りの壁セルまでの距離の算出に使用される「壁距離マップ」について、図５（ａ）及び（ｂ）を用いて説明する。図５（ａ）は、図２に示したのと同様に、「壁マップ」として作成された新環境地図５１を示している。図５（ａ）において、セル５１０を含む白抜きのセルは、「移動可能セル」を示し、セル５１１を含む斜線でハッチングされたセルは、「壁セル」を示している。

一方、図５（ｂ）は、図５（ａ）の「壁マップ」として作成された新環境地図５１を「壁距離マップ」に変換した例を示している。図５（ｂ）の「壁距離マップ」に変換された新環境地図５２は、各セルの値として最寄りの壁セルからの距離が保持されている。例えば、図５（ｂ）の斜線でハッチングされたセルは、セル５１１を含む壁セルに対応しているため、最寄りの壁セルからの距離はゼロである。したがって、壁セルの保持値はゼロである。また、これら壁セルの上下左右のいずれかに隣接するセルの保持値は１である。その他のセルも同様に、セルピッチ（セルの一辺の長さ）を単位距離として、各セルの中心から最寄りの壁セルの中心までの距離を表した数値を保持している。

なお、図５（ｂ）に示したような壁距離マップは、図４のフローチャートの実行前に予め作成しておくとよい。そして、図４のフローチャートに示す手順において、最寄りの壁セル案での距離を繰り返し求める際には、予め壁距離マップに変換された新環境地図を参照するとよい。これにより、新環境地図の保持値を読み出すことによって所望の距離値が得られるため、距離の解析的な計算を都度行う必要が無く計算量を削減することができる。

壁距離マップの作成は、例えば、以下に示す手順で行なうことができる。ここでは、最寄りの壁セルまでの距離及び最寄りの壁セル座標が各セルに保持された壁距離マップの作成について説明する。まず、始めに、壁距離マップの初期値を設定する。具体的には、壁マップとして作成された新環境地図の壁セルについて、自身の座標を最寄りの壁セル座標としてセットし、壁距離をゼロにセットする。壁セル以外のセル、例えば、「移動可能セル」及び「未知セル」等については、十分遠方の座標を壁セル座標の初期値にセットし、壁距離の値も予め定められた最大値にセットする。

次に、図６に示すように、壁マップとして作成された新環境地図の最外周のセルを除くセル６０１から右方向に１セルずつ走査する。順次走査される各セルでは、隣接する８セルに保持されている最寄りの壁セル座標（ｘ_ｗ、ｙ_ｗ）を用いて走査対象セルの座標から最寄りの壁セルまでの距離を計算する。例えば、セル６０１の場合、四角形６０３で囲まれたセルＣ１〜Ｃ８が対象となる。８つの隣接セルの保持値を用いて最寄りの壁セルまでの距離を計算した結果、最小となった距離と、最小値を与える壁セル座標を、走査対象セルに関連付けて保存する。このようなセルの走査を、左上から右下まで順次行い、次に、右下のセル６０２から左上のセル６０１まで逆順序で走査する。このような手順により、壁マップとして作成された新環境地図をもとに、壁距離マップとされた新環境地図を生成することができる。

図４に戻り説明を続ける。ステップＳ１０５では、移動後の旧環境地図の全ての壁セル座標ＣＢに関して、ステップＳ１０４で得られた壁距離を合計した合計値を記録する。

ステップＳ１０６では、全ての揺動パターン、つまり図４の例では、６つの揺動パターン＋ｄｘ，−ｄｘ，＋ｄｙ，−ｄｙ，＋ｄθ，及び−ｄθの全てについて、壁距離の合計値の計算が完了したか否かを判定する。全揺動パターンについての計算が完了していなければ、ステップＳ１０２に戻り、未完了の揺動パターンについて計算を継続する。一方、全揺動パターンについて壁距離の合計値の計算が完了した場合（ステップＳ１０６でＹＥＳ）、記録された壁距離の合計値が最小となる揺動パターンを決定する（ステップＳ１０７）。

ステップＳ１０８では、今回のステップＳ１０２〜Ｓ１０７の繰り返し処理によって得られた壁距離の合計値の最小値が、前回のステップＳ１０２〜Ｓ１０７の繰り返し処理で得られた最小値より小さい値であるか否かを判定する。今回の処理で得られた壁距離の合計値の最小値のほうが前回のものより小さければ、新環境地図と旧環境地図とのマッチングが終了していないものとして、言い換えると、最適解に到達していないものとして、ステップＳ１０９に進む。

ステップＳ１０９では、壁距離の合計値の最小値を与えるものとしてステップＳ１０７で決定された揺動パターンを移動履歴として保存する。さらに、決定された揺動パターンに対応する移動後の座標ＣＢによって移動前の座標ＣＡを上書き更新する。つまり、決定された揺動パターンに従って姿勢を変更した旧環境地図を、次のステップＳ１０２実行時の初期値とする。

一方、ステップＳ１０８において前回得られた最小値のほうが今回の揺動後の最小値より小さいと判定された場合、前回得られた壁距離の合計値の最小値が最適解であるとみなし、前回までの揺動パターンの積み重ねによる座標ＣＡの移動履歴を用いて、旧環境地図に対する新環境地図の相対姿勢（ｄＸ，ｄＹ，ｄΘ）を算出する（ステップＳ１１０）。

例えば、旧環境地図の座標系及び新環境地図の座標系の原点をともに地図の左下に置くとともに、揺動パターンｄθによる回転を地図中心を通る回転軸を基準として行った場合、相対姿勢（ｄＸ，ｄＹ，ｄΘ）は、移動履歴として保存されている揺動パターンの積算値ｄＸ´、ｄＹ´及びｄΘ´を用いて以下の（２）〜（４）式により表される。揺動パターンの積算値ｄＸ´、ｄＹ´及びｄΘ´は、（５）〜（７）式により表される。（２）式及び（３）式中のＬ_Ｗは、新環境地図及び旧環境地図の幅、つまりｘ軸方向の長さである。また、Ｌ_Ｈは、新環境地図及び旧環境地図の高さ、つまりｙ軸方向の長さである。

（２）〜（４）式により得られた相対姿勢（ｄＸ，ｄＹ，ｄΘ）を用いて、旧環境地図の座標系で設定された旧ゴール座標を（１）式に示した座標変換式に従って座標変換することにより、新環境地図の座標系で表された新ゴール座標を生成することができる。

なお、（２）〜（４）式及び（１）式の座標変換式を用いた解析的な計算により新ゴール座標を求める方法は一例である。例えば、座標ＣＡの移動履歴として記録された全揺動パターンに従って、旧ゴール座標を移動させることにより、新ゴール座標を生成してもよい。

上述したように、本実施の形態では、新環境地図と旧環境地図との相関計算を、環境地図間の相対姿勢を変数とし、最寄りの壁セルからの距離の合計値を目的関数とする最適化手法によって行なうこととした。

本実施の形態で述べた環境地図間の相対姿勢を求める手法は、ユーザによる手動入力が不要であり、自律移動ロボット等が自動的に新ゴール座標の生成を行う場合に好適である。

また、本実施の形態で述べた環境地図間の相対姿勢を求める手法の計算量のオーダは、セル総数（ピクセル数）をｎとした場合にＯ（ｎ^２）である。事前処理として壁距離マップの作成に要する計算量のオーダがＯ（ｎ^２）であり、図４のフローチャートを実行する際の計算量のオーダもＯ（ｎ^２）であるためである。このため、環境地図のサイズが大きくなった場合でも計算量の増大を抑制できる。

ところで、図１に示した環境地図生成部１２、相対姿勢算出部１３及び座標変換部１４が実行する処理は、ＣＰＵ（Central Processing Unit）と、新旧の環境地図データ及び新旧のゴール座標データを格納するためのメモリを有するコンピュータに、図３及び図４の処理手順が記述されたプログラムを実行させることによって実現可能である。

また、図４に示した相対姿勢（ｄＸ，ｄＹ，ｄΘ）を得るための手順は一例に過ぎない。例えば、粗い揺動パターンを用いてステップＳ１０８まで完了した後に、より微細な揺動パターンを用いて再度ステップＳ１０２〜Ｓ１０８までの処理を実行してもよい。このような手順によれば、より誤差の小さい相対姿勢（ｄＸ，ｄＹ，ｄΘ）を算出することができる。また、その他の公知の最適化手法を適用して、目的関数（最寄りの壁セルまでの距離の合計値）の最小解を求めてもよい。また、目的関数は、最寄りの壁セルまでの距離の合計値に限らず、これと関連する他のパラメータ、例えば、最寄りの壁セルまでの距離の二乗和としてもよい。

また、図４では、新環境地図を壁距離マップに変換し、旧環境地図の壁セル座標ＣＡを揺動させる例を示した。しかしながら、新環境地図と旧環境地図を入れ替えてもよい。つまり、旧環境地図を壁距離マップに変換し、新環境地図の壁セル座標ＣＡを揺動させてもよい。

また、壁距離マップの作成を行わずに、壁距離を都度、解析的に計算してもよい。具体的には、図７の概念図に示すように、新環境地図及び旧環境地図の壁セルの配置を共に座標化し、一方の環境地図（例えば、新環境地図）の壁座標を基準として、他方の環境地図（例えば旧環境地図）の最寄りの壁座標を検索してもよい。このとき、一方の環境地図の壁座標から他方の環境地図の最寄りの壁座標までの距離を、壁距離マップを用いずに解析的に計算してもよい。なお、図７のΣＤ_Ｗ ⁽ⁱ⁾は、一方の環境地図の壁座標から他方の環境地図の最寄りの壁座標までの距離を全ての壁セルに亘って合計した合計値を表している。

＜その他の実施の形態＞
上述した発明の実施の形態１は、車輪走行型の移動ロボットについて具体的に説明した。しかしながら、本発明は、車輪走行型に限らず様々な移動ロボットに適用可能である。例えば、本発明は、球形の回転体の回転駆動によって走行する移動ロボット、脚部によって歩行する二足歩行型又は多足歩行型の移動ロボット等にも適用可能である。

また、ロボット１は、自ら環境地図の生成を行うものとして説明したが、外部から供給される旧環境地図及び新環境地図を用いて相対姿勢の算出及びゴール座標の座標変換を行ってもよい。

さらに、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態にかかる自律移動ロボットの制御系を示すブロック図である。 壁マップとして生成された環境地図の一例を示す図である。 新環境地図の生成からゴール座標の座標変換までの全体的な処理手順を示すフローチャートである。 旧環境地図と新環境地図の間の相対姿勢を算出する手順の具体例を示すフローチャートである。 壁マップとして生成された環境地図の壁距離マップへの変換例を示す図である。 壁距離マップの生成手順を説明するための概念図である。 旧環境地図と新環境地図の間の相対姿勢を算出する手順の他の例を説明するための概念図である。

符号の説明

１ 自律移動ロボット
１１ 視覚センサ
１２ 環境地図生成部
１３ 相対姿勢算出部
１４ 座標変換部
１５ 自律移動部
１５０ 動作計画部
１５１ 動作制御部
１５２ 駆動部
１５３ 車輪
１５４ エンコーダ
２１、５１ 環境地図（壁マップ）
５２ 環境地図（壁距離マップ）

Claims (6)

1. 各々がグリッド地図として構成され、前記グリッド地図を構成する複数のセルの各々に関連付けて、ロボットが移動可能な領域であるか否かを示す情報が少なくとも保持される第１及び第２の環境地図の間での座標補正方法であって、
   前記第１の環境地図の座標系を並進又は回転させることにより、前記前記第１の環境地図の姿勢を変動させるステップ（ａ）と、
   前記第１の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の前記第１の環境地図の各障害物セルから、前記第２の環境地図における最寄りの障害物セルまでの距離を求めるステップ（ｂ）と、
   前記複数の障害物セルについて算出した前記距離の合計値又は前記合計値に関連するパラメータを目的関数として最適化計算を行い、前記目的関数が最適化される前記第１の環境地図の姿勢を決定するステップ（ｃ）と、
   前記最適化計算により得られた前記第１の環境地図の姿勢に基づいて、前記第１及び前記第２の環境地図のうちの一方の地図上の座標を、他方の地図上の座標に座標変換するステップ（ｄ）と、
   を含む座標補正方法。
2. 前記第２の環境地図に基づいて、前記第２の環境地図を構成する各セルに関連付けて、最寄りの前記障害物セルからの距離情報が保持されたグリッド地図としての障害物距離マップを作成するステップをさらに含み、
   前記ステップ（ｂ）では、姿勢変動後の前記第１の環境地図の各障害物セルの位置に対応する前記障害物距離マップ上のセルに関連付けられた前記距離情報を読み出すことにより、前記距離を求める、請求項１に記載の座標補正方法。
3. 各々がグリッド地図として構成され、前記グリッド地図を構成する複数のセルの各々に関連付けて、ロボットが移動可能な領域であるか否かを示す情報が少なくとも保持される第１及び第２の環境地図の間での座標補正処理をコンピュータに実行させるための座標補正プログラムであって、
   前記座標補正処理は、
   （ａ）前記第１の環境地図の座標系を並進又は回転させることにより、前記前記第１の環境地図の姿勢を変動させる処理と、
   （ｂ）前記第１の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の前記第１の環境地図の各障害物セルから、前記第２の環境地図における最寄りの障害物セルまでの距離を求める処理と、
   （ｃ）前記複数の障害物セルについて算出した前記距離の合計値又は前記合計値に関連するパラメータを目的関数として最適化計算を行い、前記目的関数が最適化される前記第１の環境地図の姿勢を決定する処理と、
   （ｄ）前記最適化計算により得られた前記第１の環境地図の姿勢に基づいて、前記第１及び前記第２の環境地図のうちの一方の地図上の座標を、他方の地図上の座標に座標変換する処理と、
   を含む座標補正プログラム。
4. 前記座標補正処理は、
   前記第２の環境地図に基づいて、前記第２の環境地図を構成する各セルに関連付けて、最寄りの前記障害物セルからの距離情報が保持されたグリッド地図としての障害物距離マップを作成する処理をさらに含み、
   前記処理（ｂ）では、姿勢変動後の前記第１の環境地図の各障害物セルの位置に対応する前記障害物距離マップ上のセルに関連付けられた前記距離情報を読み出すことにより、前記距離を求める、請求項３に記載の座標補正プログラム。
5. 移動空間に関する環境地図を作成する機能を有する自律移動ロボットであって、
   前記自律移動ロボットの周囲環境を計測するセンサと、
   前記センサの計測情報に基づいて、新たな環境地図を作成する地図作成部と、
   過去に作成された環境地図と前記新たな環境地図との間の相対姿勢を算出する相対姿勢算出部と、
   前記相対姿勢に基づいて、前記過去の環境地図上の座標を、前記新たな環境地図上の座標に変換する座標変換部と備え、
   前記新たな環境地図及び前記過去の環境地図は、各々がグリッド地図として構成され、前記グリッド地図を構成する複数のセルの各々に関連付けて、前記自律移動ロボットが移動可能な領域であるか否かを示す情報が少なくとも保持されており、
   前記相対姿勢算出部は、
   前記新たな環境地図及び前記過去の環境地図のうちの一方の環境地図の座標系を並進又は回転させることにより、前記一方の環境地図の姿勢を変動させ、
   前記一方の環境地図に含まれる移動不可能な領域である複数の障害物セルの各々について、姿勢変動後の前記一方の環境地図の各障害物セルから、他方の環境地図における最寄りの障害物セルまでの距離を求め、
   前記複数の障害物セルについて算出した前記距離の合計値又は前記合計値に関連するパラメータを目的関数として最適化計算を行い、前記目的関数が最適化される前記一方の環境地図の姿勢を決定する、
   自律移動ロボット。
6. 前記相対姿勢算出部は、
   前記他方の環境地図に基づいて、前記他方の環境地図を構成する各セルに関連付けて、最寄りの前記障害物セルからの距離情報が保持されたグリッド地図としての障害物距離マップを参照し、
   姿勢変動後の前記一方の環境地図の各障害物セルの位置に対応する前記障害物距離マップ上のセルに関連付けられた前記距離情報を読み出すことにより、前記距離を求める、請求項５に記載の自律移動ロボット。

Images