JP2010066934A - ３次元モデルを用いた自己位置認識手法及び自己位置認識システム - Google Patents

Abstract

【課題】移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さく、さらに、高速且つ正確に自己位置認識する。
【解決手段】移動空間の３次元モデルを作成する手段２１と、所定時間における移動体の位置及び方向からカメラ画像を取得する手段１０と、所定時間における目標軌道上の目標位置及び方向を算出する手段１７と、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段１５と、この複数のモデル画像とカメラ画像との色情報の一致度を算出する手段１６と、この各一致度に基づいて、移動体の位置及び方向を認識する手段１８とを備えた。
【選択図】図３

Description

本発明は、通路を形成する対象物体が設置された移動空間を移動する移動体の自己位置認識手法及びシステムに関する。

物流倉庫等では、効率化及び省力化を向上するために、無人搬送車が荷物を運搬する。一般に、無人搬送車の自動走行として、電磁や磁気を検知して走行する方式（電磁誘導式・磁気誘導式）がある。この方式では、誘導手段となる磁気テープを床面に敷設したり（特許文献１等参照）、磁性体を床面に埋設する（特許文献２等参照）。そして、無人搬送車は、この誘導手段からの電磁や磁気等を検知して自動走行する。従って、磁性体等の誘導手段の位置変更に多大な手間や費用を要するから、倉庫内のレイアウト変更が非常に困難となる。また、この方式では磁性体を埋設等するので、導入コストが大きい。
特開２００２−２７８６２２号公報 特開２００５−１２２５６８号公報

そこで、本発明が解決しようとする課題は、移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さく、さらに、高速且つ正確に自己位置認識できる移動体の自己位置認識手法及びシステムを提供することである。

上記の課題を解決するために、本発明に係る自己位置認識手法は、
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識手法であって、移動空間の３次元モデルを作成するステップと、所定時間における移動体の位置及び方向からカメラ画像を取得するステップと、所定時間における目標軌道上の目標位置及び方向を算出するステップと、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成するステップと、この複数のモデル画像とカメラ画像との色情報の一致度を算出するステップと、この各一致度に基づいて、移動体の位置及び方向を認識するステップとを含む。

好ましくは、移動空間に対象物体を設置するステップと、カメラ画像における対象物体の境界垂直線を基準線に規定するステップと、カメラ画像における基準線の位置を特定するステップと、３次元モデルにおける基準線の位置を特定するステップと、この特定した各基準線の位置に基づいて、３次元モデルから任意の位置及び方向における複数のモデル画像を形成するステップとを含む。

好ましくは、対象物体が通路を形成する通路形成体である。

好ましくは、カメラ画像及びモデル画像を二値化処理するステップを含む。

好ましくは、各一致度から２次又は３次曲線に近似して、曲線の極大値に基づいて移動体の位置及び方向の双方又は一方を認識するステップを含む。

好ましくは、移動空間の入出庫管理システムからの情報に基づいて、３次元モデルにおける対象物体の形状を変更するステップを含む。

好ましくは、カメラ画像及びモデル画像における対象物体以外からなる検出領域を抽出するステップと、この各検出領域を比較するステップと、この比較値と所定の閾値とに基づいて移動体の方向における障害物を認識するステップとを含む。

また、本発明に係る自己位置認識システムは、
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識システムであって、移動空間の３次元モデルを作成する手段と、所定時間における移動体の位置及び方向からカメラ画像を取得する手段と、所定時間における目標軌道上の目標位置及び方向を算出する手段と、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段と、この複数のモデル画像とカメラ画像との色情報の一致度を算出する手段と、この各一致度に基づいて、移動体の位置及び方向を認識する手段とを備えた。

好ましくは、移動空間に対象物体を設置する手段と、カメラ画像における対象物体の境界垂直線を基準線に規定する手段と、カメラ画像における基準線の位置を特定する手段と、３次元モデルにおける基準線の位置を特定する手段と、この特定した各基準線の位置に基づいて、３次元モデルから任意の位置及び方向における複数のモデル画像を形成する手段とを備えた。

好ましくは、対象物体が通路を形成する通路形成体である。

好ましくは、カメラ画像及びモデル画像を二値化処理する手段を備えた。

好ましくは、各一致度から２次又は３次曲線に近似して、曲線の極大値に基づいて移動体の位置及び方向の双方又は一方を認識する手段を備えた。

好ましくは、移動空間の入出庫管理システムからの情報に基づいて、３次元モデルにおける対象物体の形状を変更する手段を備えた。

好ましくは、カメラ画像及びモデル画像における対象物体以外からなる検出領域を抽出する手段と、この各検出領域を比較する手段と、この比較値と所定の閾値とに基づいて移動体の方向における障害物を認識する手段とを備えた。

本発明に係る３次元モデルを用いた自己位置認識手法及びシステムは、移動体のカメラで取得したカメラ画像と予め作成した移動空間の３次元モデルとに基づいて、移動体の自己位置及び方向の認識を可能とする。そのため、移動空間を変更しても、３次元モデルを移動空間に合わせて変更するだけで対応可能なので、移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さい。さらに、所定時間における目標軌道上の目標位置及び方向を算出して、３次元モデルから目標位置及び方向の所定範囲内におけるモデル画像を形成してカメラ画像との比較を行うので、比較対象とするモデル画像の枚数を限定でき、高速に処理できる。また、モデル画像とカメラ画像とを、輪郭形状に基づく画素位置情報でなく色情報の一致度に基づいて識別するので、推定される位置及び方向の信頼性が非常に高い。

また、本発明では、カメラ画像における対象物体の境界垂直線を基準線に規定することができる。そして、カメラ画像及び３次元モデルにおける基準線の位置に基づいて、移動体の自己位置を認識する。従って、自己位置を認識するための特徴量が境界垂直線である基準線の位置となり、位置認識のための基準線が少なくて明確なので、高速且つ正確に自己位置を認識できる。

以下、添付図面に基づいて、本発明に係る３次元モデルを用いた自己位置認識手法及びシステムについて説明する。

［全体構成］
図１は、自己位置認識システムを搭載した移動体を示す図である。本実施形態では、移動体１は無人搬送車からなる。図１の通り、移動体１は、前方に向けてカメラ２を備える。カメラ２は、床面Ｆから一定のカメラ高さＨに設置される。移動体１は、コントローラ３を備える。

移動体１は、操舵輪４及び走行輪５を備える。操舵輪４は、操舵モータ４０の駆動に基づいて操舵角を変更する。走行輪５は、走行モータ５０の駆動に基づいて前後進及び停止する。コントローラ３は、後で詳説するように、カメラ２からの画像に基づいて自己位置及び方向を認識し、予め記憶された目標軌道ＳＴ（走行路）に沿って移動体１が走行するように、操舵モータ４０及び走行モータ５０の駆動を制御する。

［目標軌道］
図２は、目標軌道を説明するための図である。図２の通り、移動体１は、コントローラ３が操舵モータ４０及び走行モータ５０の駆動を制御して、予め記憶された２次元座標（ＸＹ座標）及び方向からなる目標軌道ＳＴ（図２の二点鎖線）に沿って操舵、減速、加速、停止及び進行等する。従って、コントローラ３は、所定時間間隔Δｔごとの所定時間・・・ｔ−１、ｔ、ｔ＋１・・・において、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘ_ｔ，Ｙ_ｔ）及び方向θ_ｔを算出可能とする。なお、本実施形態では、所定時間間隔Δｔは、例えば約０．３秒である。

［第１実施形態］
第１実施形態について説明する。
図３は、第１実施形態の自己位置認識システムを示すブロック図である。図４は、第１実施形態の自己位置認識手法を説明するフローチャート図である。図５は、第１実施形態の比較するモデル画像を説明するための図である。図６は、移動空間内の３次元モデルを示す概念図である。

自己位置認識システムは、カメラ２及びコントローラ３等からなる（図３）。コントローラ３は、ＣＰＵ等を搭載したコンピュータからなっており、カメラ２から取得したカメラ画像等を処理して、移動体１の位置及び方向を認識する。

本実施形態では、移動体１は、物流倉庫を移動空間６として移動する（図６）。この移動空間６では、荷物等を配列するための複数のラック（棚）６０が配置されている。移動体１は、それぞれのラック６０の間を通路６１として目標軌道ＳＴに沿って走行する。

先ず、移動体１が実際に走行する移動空間６（実空間）と同一の３次元モデル（モデル空間）（図６）を３次元モデル作成手段２１で作成する（図４のステップＳ０１）。そして、３次元モデル記憶手段１１が、この３次元モデルを記憶する。なお、３次元モデルは、実空間と略同一色で作成される。３次元モデル作成手段２１は、例えばＣＡＤシステムからなる。

３次元モデル作成手段２１は、対象物体（ラック）６０に配列される荷物を管理する入出庫・在庫管理システム（ＷＭＳ）２２に基づいて、３次元モデルにおける対象物体６０に荷物を配置した形状に随時変更する。これにより、常に、実際（実空間）と３次元モデル（モデル空間）との移動空間６における形状や色情報が一致する。色情報は、色相（色度）Ｈ、彩度Ｓ、明度（輝度）Ｖからなる。

そして、移動体１が走行する目標軌道ＳＴを作成して、目標軌道記憶手段１７に目標軌道ＳＴを記憶する（図４のステップＳ０２）。

図７は、カメラ画像を示す図である。そして、カメラ画像取得手段１０は、所定時間ｔにおいて、カメラ２でカメラ画像Ｒｉ_ｔを取得する（図４のステップＳ１１）。カメラ画像Ｒｉ_ｔは、所定時間ｔにおける、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）及び方向θ_Ｍに対する画像である。

図８は、二値化したカメラ画像を示す図である。カメラ画像Ｒｉ_ｔの全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０を抽出したカメラ画像Ｒｉ_ｔを作成する（図３のステップＳ３）。なお、床面及び壁面の色相Ｈが予め設定されているので、対象物体６０を抽出した二値化が可能である。

そして、所定時間ｔにおいて、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘ_ｔ，Ｙ_ｔ）及び方向θ_ｔを算出する（図４のステップＳ１２）。

そして、所定時間ｔの目標位置（Ｘ_ｔ，Ｙ_ｔ）及び方向θ_ｔの所定範囲内における複数の位置（Ｘ_ｔｎ，Ｙ_ｔｎ）及び方向θ_ｔｎのモデル画像Ｖｉ_ｔを作成する（図４のステップＳ１３）。図５の通り、本実施形態では、所定範囲（Ｗ１−Ｗ２）は、目標位置（Ｘ_ｔ，Ｙ_ｔ）を中心とする矩形範囲（円範囲等でもよい）であって、所定間隔Δｗごとに、複数の位置（Ｘ_ｔｎ，Ｙ_ｔｎ）が設定される（図５の△印）。さらに、それぞれの位置（Ｘ_ｔｎ，Ｙ_ｔｎ）ごとに、目標方向θ_ｔの左右所定角度範囲θｗに所定角度間隔Δθごとに、複数のθ_ｔｎが設定される（図５の×印）。なお、本実施形態では、例えば、Ｗ１＝Ｗ２＝２００〜６００ｍｍ、Δｗ＝１０〜５０ｍｍ、θｗ＝１〜１０°、Δθ＝０．１〜１．０°である。

図９は、モデル画像を示す図である。モデル画像作成手段１５は、予め記憶した３次元モデルに基づいて、移動体１の任意の位置及び方向におけるカメラ高さＨから見た２次元のモデル画像を作成する。従って、モデル画像作成手段１５が、上記の複数の位置（Ｘ_ｔｎ，Ｙ_ｔｎ）及び方向θ_ｔｎのモデル画像Ｖｉ_ｔを作成する。

モデル画像Ｖｉ_ｔにおいても、カメラ画像Ｒｉ_ｔと同様に、全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０を抽出したモデル画像Ｖｉ_θｉを作成する（図１０）。図１０は、二値化したモデル画像を示す図である。

そして、一致度算出手段１６が、カメラ画像Ｒｉ_ｔと各モデル画像Ｖｉ_ｔとの比較を行う（図４のステップＳ１４）。図１１は、カメラ画像とモデル画像とを比較する状態を示す図である。そして、カメラ画像Ｒｉ_ｔと各モデル画像Ｖｉ_ｔとの一致度ＣＶｉを求める（図４のステップＳ１５）。一致度ＣＶｉは、カメラ画像Ｒｉ_ｔと各モデル画像Ｖｉ_ｔとにおける特徴量（輝度値Ｖ「０」）の画素に基づいて、全画素に対する一致する画素の割合を採用する。

そして、最も一致度ＣＶｉの高い一つのモデル画像Ｖｉ_ｔを採用して、そのモデル画像Ｖｉ_ｔにおける（Ｘ_ｔｎ，Ｙ_ｔｎ）及び方向θ_ｔｎを、所定時間ｔにおける移動体１の自己位置（Ｘ_Ｒ，Ｙ_Ｒ）及び方向θ_Ｒとして決定する（図４のステップＳ１６）。この自己位置（Ｘ_Ｒ，Ｙ_Ｒ）及び方向θ_Ｒに基づいて、予め記憶された目標軌道ＳＴに沿って移動体１が追従するように、操舵モータ４０及び走行モータ５０を制御する（図４のステップＳ１７）。即ち、移動体１が、所定時間ｔ＋１における、目標軌道上の目標位置（Ｘ_ｔ＋１，Ｙ_ｔ＋１）及び方向θ_ｔ＋１に向かって走行するように制御する。

なお、上記では、完全に一致する色相Ｈに基づいて識別したが、ある程度の幅をもった色相Ｈ±Δｈに一致する画素を対象にして算出してもよい。また、上記では、カメラ画像Ｒｉ_ｔとモデル画像Ｖｉ_ｔとを色相Ｈに基づいて二値化処理して、一致度ＣＶｉを算出したが、二値化処理せずに、それぞれの画素における色情報の一致度ＣＶｉで算出してもよい。その場合、色相Ｈのみで識別するのではなく、例えば、グレー系に関しては明度（輝度）Ｖで識別する等、色の種類によって、色相Ｈ、彩度Ｓ、明度Ｖのいずれか又はそれらを組合わせて識別することもできる。

［第２実施形態］
第２実施形態について説明する。なお、第１実施形態と同じ内容については、詳細な説明を省略する。
図１２は、第２実施形態の自己位置認識システムを示すブロック図である。図１３及び図１４は、第２実施形態の自己位置認識手法を説明するフローチャート図である。

先ず、移動体１が走行する移動空間６と同一の３次元モデル（図６）を３次元モデル作成手段２１で作成する（図１３のステップＳ０１）。そして、移動体１が走行する目標軌道ＳＴを目標軌道記憶手段１７に記憶する（図１３のステップＳ０２）。

カメラ画像取得手段１０は、所定時間ｔにおいて、カメラ２でカメラ画像Ｒｉ_ｔを取得する（図１３のステップＳ２１）。カメラ画像Ｒｉ_ｔは、所定時間ｔにおける、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）及び方向θ_Ｍの画像である（図７）。

［基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの位置］
図１５は、基準線を規定したカメラ画像を示す図である。基準線規定手段１２は、二値化したカメラ画像Ｒｉ_ｔ（図８）における、対象物体（ラック）６０と正面壁との境界によって形成される境界垂直線を基準線ｒＲＬ_ｔ、ｌＲＬ_ｔと規定する（図１３のステップＳ２３）。本実施形態では、対象物体６０は、垂直方向及び水平方向に延設する支柱等を組み立てて形成されるラック（棚）なので、境界垂直線が現れる。図１５の通り、カメラ画像Ｒｉ_ｔ内での座標系をＩＸ−ＩＹ座標とし、基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの位置はＩＸ軸方向の座標値とする。

なお、境界垂直線は、移動体の位置等によっては１箇所しか現れない場合もある。また、境界垂直線は、対象物体６０の形状によって設定が異なり、例えば、対象物体６０と正面壁との境界が円弧の場合、その先端部分に接する垂直線としてもよい。

カメラ画像Ｒｉ_ｔにおいて、通路中心線Ｃｐ_ｔー１を設定する。通路中心線Ｃｐ_ｔー１は、所定時間ｔ−１において決定した移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）及び方向θ_Ｍに基づいて、時間ｔ−１における通路６１の中心位置（ＩＸ軸方向における各基準線ｒＲＬ_ｔ−１、ｌＲＬ_ｔ−１の間の中央位置）を採用する。

カメラ画像における基準線位置特定手段１３は、次の手順により基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの位置を特定する。カメラ画像Ｒｉ_ｔにおいて、通路中心線Ｃｐ_ｔー１の左右に対象物体６０が存在する探索高さ範囲Ｒａを設定する。探索高さ範囲Ｒａ内において、カメラ画像Ｒｉ_ｔにおける通路中心線Ｃｐ_ｔー１から、ＩＹ軸方向における所定画素間隔ごとに、ＩＸ軸方向に向けて探索を行い、対象物体６０と識別される（輝度値Ｖ「１」と識別される）左右の位置を特定する。

そして、左右それぞれに対して特定された各位置をソートし、信頼性の低い上位量１／４の位置と下位量１／４の位置とを除き、中位量１／２の位置について平均値を求め、その値を左右の基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの位置と特定する。

上記した通路中心線Ｃｐ_ｔー１からの位置に基づいて、カメラ画像Ｒｉ_ｔにおける画像中心垂直線から左右の基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの距離Ｗｐａ、Ｗｐｂを特定する（図１３のステップＳ２４）。

そして、所定時間ｔにおいて、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘ_ｔ，Ｙ_ｔ）及び方向θ_ｔを算出する（図１３のステップＳ２２）。

図１６は、基準線と移動体との位置関係を示す平面図である。３次元モデルにおける基準線位置特定手段１４は、モデル空間内における基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの位置座標（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）を特定する（図１３のステップＳ２５）。この位置座標（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）は、所定時間ｔにおける目標位置（Ｘ_ｔ，Ｙ_ｔ）及び方向θ_ｔを算出することにより（図１３のステップＳ２２）、対象物体６０の位置を推定して特定できる。

モデル空間内における基準線ｒＲＬ_ｔ、ｌＲＬ_ｔと移動体１との位置関係（図１６）に基づいて、次の（１）式〜（３）式が導かれる。

・・・（１）

・・・（２）

・・・（３）

ここで、（Ｘ_Ｍ，Ｙ_Ｍ）及びθ_Ｍはモデル空間内での移動体１の位置及び方向である。Ｆはカメラ２の焦点距離である。

上記の通り、Ｗｐａ、Ｗｐｂは、カメラ画像Ｒｉ_ｔに基づいて特定される（図１３のステップＳ２４）。また、Ｆはカメラ２のスペックで特定される。これらを（２）式及び（３）式に入力することによりα、βが特定される。

さらに、上記の通り、（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）は、３次元モデルに基づいて特定される（図１３のステップＳ２５）。これらより、（１）式に基づいて、移動体１の方向θ_Ｍを任意に決めると、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）が一義的に決定される。

モデル画像作成手段１５は、３次元モデルに基づいて、移動体１の任意の位置及び方向におけるカメラ高さＨから見たモデル画像を作成する（図９）。

［モデル空間における移動体１の方向θ_Ｍ］
次に、基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの拘束を行って、移動体１の方向θ_Ｍを認識する手順について説明する。先ず、所定時間ｔ−１において認識した移動体１の方向θ_Ｌを基準方向とする。基準方向θ_Ｌを中心とする任意の角度Ψを探索角度範囲とする。この探索角度範囲Ψを所定数（本例では７つ）に等分割し、φｉ（ｉ＝１，２，・・・，７）からなる集合Ｓ_φを決定する。Ｓ_φを（４）式に示す。

・・・（４）

このφｉをモデル空間内での移動体１の方向として、（１）式からモデル空間内での移動体１の位置（Ｘ_φｉ，Ｙ_φｉ）を決定する。そして、この決定したモデル空間内での移動体１の位置（Ｘ_φｉ，Ｙ_φｉ）及び方向φｉに基づいて、モデル画像作成手段１５でモデル画像Ｖｉ_φｉを作成する（図１３のステップＳ２６）。

全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０のみを抽出した、特徴量のみのモデル画像Ｖｉ_φｉを作成する（図１０）。

そして、一致度算出手段１６が、カメラ画像Ｒｉ_ｔとモデル画像Ｖｉ_θｉとの比較を行って（図１１）、一致度ＣＶｉを求める（図１３のステップＳ２７）。なお、一致度ＣＶｉは、カメラ画像Ｒｉ_ｔとモデル画像Ｖｉ_φｉとにおける特徴量（輝度値Ｖ「０」）の画素に基づいて、全画素に対する一致する画素の割合を採用する。

これまでの手順（図１３のステップＳ２６及びＳ２７）をそれぞれの方向φ_１〜φ_７で繰り返し行い、これによって得られるカメラ画像Ｒｉ_ｔとモデル画像Ｖｉ_φ１〜７と比較して、それぞれの一致度ＣＶ_１〜ＣＶ_７を求める。

図１７は、任意の方向と一致度との関係を示すグラフ図である。一致度算出手段１６は、ｉ＝１〜７における方向φｉの一致度ＣＶｉに基づいて、最小二乗法によって３次元曲線ｆ（φ）による近似を行う（図１３のステップＳ２８）。そして、３次元曲線ｆ（φ）において極大となる方向φｍを求める（図１３のステップＳ２９）。

方向φｍを新たな基準方向θ_Ｌとして、探索角度範囲をΨ／２として、これまでの手順（図１３のステップＳ２６〜Ｓ２９）を行い、これをｎ_１回繰り返して、自己位置認識手段１８で、モデル空間における移動体１の方向θ_Ｍを認識する（図１３のステップＳ３０）。

上記より、基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの拘束によって移動体１の方向θ_Ｍを認識して、これを（１）式に入力して移動体１の位置（Ｘ_θＭ，Ｙ_θＭ）を求める。そして、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）の認識精度を向上するために、次の手順によって、基準線ｒＲＬ_ｔ、ｌＲＬ_ｔの拘束を行わずに、カメラ画像Ｒｉ_ｔと各モデル画像Ｖｉ_θ１〜７とを比較する。

［モデル空間における移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）］
次に、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）を認識する手順について説明する。任意の変化量δとして、上記で求めた位置（Ｘ_θＭ，Ｙ_θＭ）を中心とする位置集合（Ｓｘ，Ｓｙ）を作成する。集合Ｓｘを（５）式とし、集合Ｓｙを（６）式とする。従って、位置集合（Ｓｘ，Ｓｙ）は、集合Ｓｘと集合Ｓｙとの全ての組合わせである９（＝３×３）箇所の位置となり、位置（Ｘ_θＭ，Ｙ_θＭ）の周囲位置となる。なお、変化量δは、実空間における約５ｃｍとする。

・・・（５）

・・・（６）

９箇所の位置集合（Ｓｘ，Ｓｙ）のそれぞれについて、モデル画像作成手段１５でモデル画像Ｖｉ_θＭを作成する（図１４のステップＳ３１）。そして、それぞれの９つのモデル画像Ｖｉ_θＭ とカメラ画像Ｒｉ_ｔとの比較を行い（図１１）、一致度算出手段１６で一致度ＣＶｉを求める（図１４のステップＳ３２）。そして、最大値となる一致度ＣＶｉを採用して、位置（Ｘ_θＭ，Ｙ_θＭ）を決定する（図１４のステップＳ３３）。

さらに、位置（Ｘ_θＭ，Ｙ_θＭ）を中心とし、任意の変化量をδ／２として、これまでの手順（図１４のステップＳ３１〜Ｓ３３）を行い、これをｎ_２回繰り返して、自己位置認識手段１８が、モデル空間における移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）を認識する（図１４のステップＳ３４）。

上記では、移動体１の方向θ_Ｍを決定する際に、一致度ＣＶｉから３次曲線ｆ（φ）による近似を行ったが、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）を決定する際にも、一致度ＣＶｉから３次曲線による近似を行うことができる。

［実空間における移動体１の位置（Ｘ_Ｒ，Ｙ_Ｒ）及び方向θ_Ｒ］
上記より認識されたモデル空間内での移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）及び方向θ_Ｍから、実空間内での移動体１の位置（Ｘ_Ｒ，Ｙ_Ｒ）及び方向θ_Ｒ をそれぞれ（７）式及び（８）式により決定する（図１４のステップＳ３５）。

・・・（７）

・・・（８）

ここで、Ｋは実空間とモデル空間との縮尺、（Ｘ_Ｓ，Ｙ_Ｓ）は実空間とモデル空間とのＸＹ軸方向のオフセット量、λは実空間とモデル空間との回転角、を表す。

［障害物認識］
障害物認識手段１９は、自己位置（Ｘ_Ｒ，Ｙ_Ｒ）及び方向θ_Ｒ を認識した後、カメラ画像Ｒｉ_ｔとモデル画像Ｖｉ_Ｍに基づいて障害物認識を行う（図１４のステップＳ３６）。モデル画像Ｖｉ_Ｍは、移動体１の位置（Ｘ_Ｍ，Ｙ_Ｍ）及び方向θ_Ｍに基づいて作成する。

通路６１と壁面からなる領域（対象物体６０以外の領域）を、障害物を検出するための領域（検出領域）とする。モデル画像Ｖｉ_Ｍにおいて検出領域を抽出する。カメラ画像Ｒｉ_ｔについても検出領域を抽出する。

そして、カメラ画像Ｒｉ_ｔとモデル画像Ｖｉ_Ｍとのそれぞれに対して検出領域内の画素をＨＳＶ変換し、それぞれの色相（Ｈ）、明度（Ｖ）、彩度（Ｓ）の差が閾値未満となる画素を除去して、カメラ画像Ｒｉ_ｔの検出領域において、モデル画像Ｖｉ_Ｍと異なる色の画素のみを抽出した画像を作成する。そして、この画像において、一定値以上の面積を有する領域がある場合には、その領域を障害物として認識する。

［目標軌道追従］
目標軌道追従手段２０は、上記によって認定した移動体１の位置（Ｘ_Ｒ，Ｙ_Ｒ）及び方向θ_Ｒに基づいて、予め記憶された目標軌道ＳＴ（走行路）に沿って移動体１が追従するように、操舵モータ４０及び走行モータ５０を制御する（図１４のステップＳ３７）。

そして、所定時間間隔Δｔごとに自己位置認識を行って、移動体１が目標軌道ＳＴを追従するように制御する。なお、上記手法において、所定時間ｔ−１における種々の値を採用するステップがあるので、スタート時における値（初期値）は予め設定する。

そして、上記の障害物認識手法によって障害物を認識した際には、移動体１が停止する等の所定の処置を取るように、目標軌道追従手段２０が、操舵モータ４０及び走行モータ５０を制御する。

［その他］
上記実施形態では、移動体１に前方に向けたカメラ２からのカメラ画像に基づいて自己位置を認識しているが、後方に向けたカメラ２からのカメラ画像を用いたり、前方からのカメラ画像と組合わせて認識してもよい。

また、上記では、ラックが配置された物流倉庫内を移動する無人搬送車について説明したが、座席（対象物体６０）が配置されたコンサート会場（移動空間６）内を走行又は歩行する警備ロボット（移動体１）等でも適用でき、本発明の趣旨を逸脱しない範囲内であれば様々な移動体及び移動空間等に採用できる。

自己位置認識システムを搭載した移動体を示す図である。 目標軌道を説明するための図である。 第１実施形態の自己位置認識システムを示すブロック図である。 第１実施形態の自己位置認識手法を説明するフローチャート図である。 第１実施形態の比較するモデル画像を説明するための図である。 移動空間内の３次元モデルを示す概念図である。 カメラ画像を示す図である。 二値化したカメラ画像を示す図である。 モデル画像を示す図である。 二値化したモデル画像を示す図である。 カメラ画像とモデル画像とを比較する状態を示す図である。 第２実施形態の自己位置認識システムを示すブロック図である。 第２実施形態の自己位置認識手法を説明するフローチャート図である。 図１３に続くフローチャート図である。 基準線を規定したカメラ画像を示す図である。 基準線と移動体との位置関係を示す平面図である。 任意の方向と一致度との関係を示すグラフ図である。

符号の説明

１ 移動体
２ カメラ
３ コントローラ
６ 移動空間
６０ 対象物体
６１ 通路
Ｒｉ_ｔ カメラ画像
Ｖｉ_ｔ モデル画像
ｒＲＬ_ｔ、ｌＲＬ_ｔ 基準線
θ_Ｍ モデル空間における移動体の方向
Ｘ_Ｍ，Ｙ_Ｍ モデル空間における移動体の位置
θ_Ｒ 実空間における移動体の方向
Ｘ_Ｒ，Ｙ_Ｒ 実空間における移動体の位置

Claims (14)

1. 予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識手法であって、前記移動空間の３次元モデルを作成するステップと、所定時間における前記移動体の位置及び方向からカメラ画像を取得するステップと、前記所定時間における前記目標軌道上の目標位置及び方向を算出するステップと、前記３次元モデルから前記目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成するステップと、この複数のモデル画像と前記カメラ画像との色情報の一致度を算出するステップと、この各一致度に基づいて、前記移動体の位置及び方向を認識するステップとを含むことを特徴とする自己位置認識手法。
2. 前記移動空間に対象物体を設置するステップと、前記カメラ画像における前記対象物体の境界垂直線を基準線に規定するステップと、前記カメラ画像における前記基準線の位置を特定するステップと、前記３次元モデルにおける前記基準線の位置を特定するステップと、この特定した各基準線の位置に基づいて、前記３次元モデルから任意の位置及び方向における複数のモデル画像を形成するステップとを含むことを特徴とする請求項１に記載の自己位置認識手法。
3. 前記対象物体が通路を形成する通路形成体であることを特徴とする請求項１又は２に記載の自己位置認識手法。
4. 前記カメラ画像及び前記モデル画像を二値化処理するステップを含むことを特徴とする請求項１〜３のいずれかに記載の自己位置認識手法。
5. 前記各一致度から２次又は３次曲線に近似して、前記曲線の極大値に基づいて前記移動体の位置及び方向の双方又は一方を認識するステップを含むことを特徴とする請求項１〜４のいずれかに記載の自己位置認識手法。
6. 前記移動空間の入出庫管理システムからの情報に基づいて、前記３次元モデルにおける前記対象物体の形状を変更するステップを含むことを特徴とする請求項１〜５のいずれかに記載の自己位置認識手法。
7. 前記カメラ画像及び前記モデル画像における前記対象物体以外からなる検出領域を抽出するステップと、この各検出領域を比較するステップと、この比較値と所定の閾値とに基づいて前記移動体の方向における障害物を認識するステップとを含むことを特徴とする請求項１〜６のいずれかに記載の自己位置認識手法。
8. 予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識システムであって、前記移動空間の３次元モデルを作成する手段と、所定時間における前記移動体の位置及び方向からカメラ画像を取得する手段と、前記所定時間における前記目標軌道上の目標位置及び方向を算出する手段と、前記３次元モデルから前記目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段と、この複数のモデル画像と前記カメラ画像との色情報の一致度を算出する手段と、この各一致度に基づいて、前記移動体の位置及び方向を認識する手段とを備えたことを特徴とする自己位置認識システム。
9. 前記移動空間に対象物体を設置する手段と、前記カメラ画像における前記対象物体の境界垂直線を基準線に規定する手段と、前記カメラ画像における前記基準線の位置を特定する手段と、前記３次元モデルにおける前記基準線の位置を特定する手段と、この特定した各基準線の位置に基づいて、前記３次元モデルから任意の位置及び方向における複数のモデル画像を形成する手段とを備えたことを特徴とする請求項８に記載の自己位置認識システム。
10. 前記対象物体が通路を形成する通路形成体であることを特徴とする請求項８又は９に記載の自己位置認識システム。
11. 前記カメラ画像及び前記モデル画像を二値化処理する手段を備えたことを特徴とする請求項８〜１０のいずれかに記載の自己位置認識システム。
12. 前記各一致度から２次又は３次曲線に近似して、前記曲線の極大値に基づいて前記移動体の位置及び方向の双方又は一方を認識する手段を備えたことを特徴とする請求項８〜１１のいずれかに記載の自己位置認識システム。
13. 前記移動空間の入出庫管理システムからの情報に基づいて、前記３次元モデルにおける前記対象物体の形状を変更する手段を備えたことを特徴とする請求項８〜１２のいずれかに記載の自己位置認識システム。
14. 前記カメラ画像及び前記モデル画像における前記対象物体以外からなる検出領域を抽出する手段と、この各検出領域を比較する手段と、この比較値と所定の閾値とに基づいて前記移動体の方向における障害物を認識する手段とを備えたことを特徴とする請求項８〜１３のいずれかに記載の自己位置認識システム。