３次元モデルを用いた自己位置認識手法及び自己位置認識システム
【課題】移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さく、さらに、高速且つ正確に自己位置認識する。
【解決手段】移動空間の３次元モデルを作成する手段２１と、所定時間における移動体の位置及び方向からカメラ画像を取得する手段１０と、所定時間における目標軌道上の目標位置及び方向を算出する手段１７と、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段１５と、この複数のモデル画像とカメラ画像との色情報の一致度を算出する手段１６と、この各一致度に基づいて、移動体の位置及び方向を認識する手段１８とを備えた。
【解決手段】移動空間の３次元モデルを作成する手段２１と、所定時間における移動体の位置及び方向からカメラ画像を取得する手段１０と、所定時間における目標軌道上の目標位置及び方向を算出する手段１７と、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段１５と、この複数のモデル画像とカメラ画像との色情報の一致度を算出する手段１６と、この各一致度に基づいて、移動体の位置及び方向を認識する手段１８とを備えた。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、通路を形成する対象物体が設置された移動空間を移動する移動体の自己位置認識手法及びシステムに関する。
【背景技術】
【０００２】
物流倉庫等では、効率化及び省力化を向上するために、無人搬送車が荷物を運搬する。一般に、無人搬送車の自動走行として、電磁や磁気を検知して走行する方式（電磁誘導式・磁気誘導式）がある。この方式では、誘導手段となる磁気テープを床面に敷設したり（特許文献１等参照）、磁性体を床面に埋設する（特許文献２等参照）。そして、無人搬送車は、この誘導手段からの電磁や磁気等を検知して自動走行する。従って、磁性体等の誘導手段の位置変更に多大な手間や費用を要するから、倉庫内のレイアウト変更が非常に困難となる。また、この方式では磁性体を埋設等するので、導入コストが大きい。
【特許文献１】特開２００２−２７８６２２号公報
【特許文献２】特開２００５−１２２５６８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
そこで、本発明が解決しようとする課題は、移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さく、さらに、高速且つ正確に自己位置認識できる移動体の自己位置認識手法及びシステムを提供することである。
【課題を解決するための手段】
【０００４】
上記の課題を解決するために、本発明に係る自己位置認識手法は、
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識手法であって、移動空間の３次元モデルを作成するステップと、所定時間における移動体の位置及び方向からカメラ画像を取得するステップと、所定時間における目標軌道上の目標位置及び方向を算出するステップと、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成するステップと、この複数のモデル画像とカメラ画像との色情報の一致度を算出するステップと、この各一致度に基づいて、移動体の位置及び方向を認識するステップとを含む。
【０００５】
好ましくは、移動空間に対象物体を設置するステップと、カメラ画像における対象物体の境界垂直線を基準線に規定するステップと、カメラ画像における基準線の位置を特定するステップと、３次元モデルにおける基準線の位置を特定するステップと、この特定した各基準線の位置に基づいて、３次元モデルから任意の位置及び方向における複数のモデル画像を形成するステップとを含む。
【０００６】
好ましくは、対象物体が通路を形成する通路形成体である。
【０００７】
好ましくは、カメラ画像及びモデル画像を二値化処理するステップを含む。
【０００８】
好ましくは、各一致度から２次又は３次曲線に近似して、曲線の極大値に基づいて移動体の位置及び方向の双方又は一方を認識するステップを含む。
【０００９】
好ましくは、移動空間の入出庫管理システムからの情報に基づいて、３次元モデルにおける対象物体の形状を変更するステップを含む。
【００１０】
好ましくは、カメラ画像及びモデル画像における対象物体以外からなる検出領域を抽出するステップと、この各検出領域を比較するステップと、この比較値と所定の閾値とに基づいて移動体の方向における障害物を認識するステップとを含む。
【００１１】
また、本発明に係る自己位置認識システムは、
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識システムであって、移動空間の３次元モデルを作成する手段と、所定時間における移動体の位置及び方向からカメラ画像を取得する手段と、所定時間における目標軌道上の目標位置及び方向を算出する手段と、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段と、この複数のモデル画像とカメラ画像との色情報の一致度を算出する手段と、この各一致度に基づいて、移動体の位置及び方向を認識する手段とを備えた。
【００１２】
好ましくは、移動空間に対象物体を設置する手段と、カメラ画像における対象物体の境界垂直線を基準線に規定する手段と、カメラ画像における基準線の位置を特定する手段と、３次元モデルにおける基準線の位置を特定する手段と、この特定した各基準線の位置に基づいて、３次元モデルから任意の位置及び方向における複数のモデル画像を形成する手段とを備えた。
【００１３】
好ましくは、対象物体が通路を形成する通路形成体である。
【００１４】
好ましくは、カメラ画像及びモデル画像を二値化処理する手段を備えた。
【００１５】
好ましくは、各一致度から２次又は３次曲線に近似して、曲線の極大値に基づいて移動体の位置及び方向の双方又は一方を認識する手段を備えた。
【００１６】
好ましくは、移動空間の入出庫管理システムからの情報に基づいて、３次元モデルにおける対象物体の形状を変更する手段を備えた。
【００１７】
好ましくは、カメラ画像及びモデル画像における対象物体以外からなる検出領域を抽出する手段と、この各検出領域を比較する手段と、この比較値と所定の閾値とに基づいて移動体の方向における障害物を認識する手段とを備えた。
【発明の効果】
【００１８】
本発明に係る３次元モデルを用いた自己位置認識手法及びシステムは、移動体のカメラで取得したカメラ画像と予め作成した移動空間の３次元モデルとに基づいて、移動体の自己位置及び方向の認識を可能とする。そのため、移動空間を変更しても、３次元モデルを移動空間に合わせて変更するだけで対応可能なので、移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さい。さらに、所定時間における目標軌道上の目標位置及び方向を算出して、３次元モデルから目標位置及び方向の所定範囲内におけるモデル画像を形成してカメラ画像との比較を行うので、比較対象とするモデル画像の枚数を限定でき、高速に処理できる。また、モデル画像とカメラ画像とを、輪郭形状に基づく画素位置情報でなく色情報の一致度に基づいて識別するので、推定される位置及び方向の信頼性が非常に高い。
【００１９】
また、本発明では、カメラ画像における対象物体の境界垂直線を基準線に規定することができる。そして、カメラ画像及び３次元モデルにおける基準線の位置に基づいて、移動体の自己位置を認識する。従って、自己位置を認識するための特徴量が境界垂直線である基準線の位置となり、位置認識のための基準線が少なくて明確なので、高速且つ正確に自己位置を認識できる。
【発明を実施するための最良の形態】
【００２０】
以下、添付図面に基づいて、本発明に係る３次元モデルを用いた自己位置認識手法及びシステムについて説明する。
【００２１】
［全体構成］
図１は、自己位置認識システムを搭載した移動体を示す図である。本実施形態では、移動体１は無人搬送車からなる。図１の通り、移動体１は、前方に向けてカメラ２を備える。カメラ２は、床面Ｆから一定のカメラ高さＨに設置される。移動体１は、コントローラ３を備える。
【００２２】
移動体１は、操舵輪４及び走行輪５を備える。操舵輪４は、操舵モータ４０の駆動に基づいて操舵角を変更する。走行輪５は、走行モータ５０の駆動に基づいて前後進及び停止する。コントローラ３は、後で詳説するように、カメラ２からの画像に基づいて自己位置及び方向を認識し、予め記憶された目標軌道ＳＴ（走行路）に沿って移動体１が走行するように、操舵モータ４０及び走行モータ５０の駆動を制御する。
【００２３】
［目標軌道］
図２は、目標軌道を説明するための図である。図２の通り、移動体１は、コントローラ３が操舵モータ４０及び走行モータ５０の駆動を制御して、予め記憶された２次元座標（ＸＹ座標）及び方向からなる目標軌道ＳＴ（図２の二点鎖線）に沿って操舵、減速、加速、停止及び進行等する。従って、コントローラ３は、所定時間間隔Δｔごとの所定時間・・・ｔ−１、ｔ、ｔ＋１・・・において、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出可能とする。なお、本実施形態では、所定時間間隔Δｔは、例えば約０．３秒である。
【００２４】
［第１実施形態］
第１実施形態について説明する。
図３は、第１実施形態の自己位置認識システムを示すブロック図である。図４は、第１実施形態の自己位置認識手法を説明するフローチャート図である。図５は、第１実施形態の比較するモデル画像を説明するための図である。図６は、移動空間内の３次元モデルを示す概念図である。
【００２５】
自己位置認識システムは、カメラ２及びコントローラ３等からなる（図３）。コントローラ３は、ＣＰＵ等を搭載したコンピュータからなっており、カメラ２から取得したカメラ画像等を処理して、移動体１の位置及び方向を認識する。
【００２６】
本実施形態では、移動体１は、物流倉庫を移動空間６として移動する（図６）。この移動空間６では、荷物等を配列するための複数のラック（棚）６０が配置されている。移動体１は、それぞれのラック６０の間を通路６１として目標軌道ＳＴに沿って走行する。
【００２７】
先ず、移動体１が実際に走行する移動空間６（実空間）と同一の３次元モデル（モデル空間）（図６）を３次元モデル作成手段２１で作成する（図４のステップＳ０１）。そして、３次元モデル記憶手段１１が、この３次元モデルを記憶する。なお、３次元モデルは、実空間と略同一色で作成される。３次元モデル作成手段２１は、例えばＣＡＤシステムからなる。
【００２８】
３次元モデル作成手段２１は、対象物体（ラック）６０に配列される荷物を管理する入出庫・在庫管理システム（ＷＭＳ）２２に基づいて、３次元モデルにおける対象物体６０に荷物を配置した形状に随時変更する。これにより、常に、実際（実空間）と３次元モデル（モデル空間）との移動空間６における形状や色情報が一致する。色情報は、色相（色度）Ｈ、彩度Ｓ、明度（輝度）Ｖからなる。
【００２９】
そして、移動体１が走行する目標軌道ＳＴを作成して、目標軌道記憶手段１７に目標軌道ＳＴを記憶する（図４のステップＳ０２）。
【００３０】
図７は、カメラ画像を示す図である。そして、カメラ画像取得手段１０は、所定時間ｔにおいて、カメラ２でカメラ画像Ｒｉｔを取得する（図４のステップＳ１１）。カメラ画像Ｒｉｔは、所定時間ｔにおける、移動体１の位置（ＸＭ，ＹＭ）及び方向θＭに対する画像である。
【００３１】
図８は、二値化したカメラ画像を示す図である。カメラ画像Ｒｉｔの全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０を抽出したカメラ画像Ｒｉｔを作成する（図３のステップＳ３）。なお、床面及び壁面の色相Ｈが予め設定されているので、対象物体６０を抽出した二値化が可能である。
【００３２】
そして、所定時間ｔにおいて、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出する（図４のステップＳ１２）。
【００３３】
そして、所定時間ｔの目標位置（Ｘｔ，Ｙｔ）及び方向θｔの所定範囲内における複数の位置（Ｘｔｎ，Ｙｔｎ）及び方向θｔｎのモデル画像Ｖｉｔを作成する（図４のステップＳ１３）。図５の通り、本実施形態では、所定範囲（Ｗ１−Ｗ２）は、目標位置（Ｘｔ，Ｙｔ）を中心とする矩形範囲（円範囲等でもよい）であって、所定間隔Δｗごとに、複数の位置（Ｘｔｎ，Ｙｔｎ）が設定される（図５の△印）。さらに、それぞれの位置（Ｘｔｎ，Ｙｔｎ）ごとに、目標方向θｔの左右所定角度範囲θｗに所定角度間隔Δθごとに、複数のθｔｎが設定される（図５の×印）。なお、本実施形態では、例えば、Ｗ１＝Ｗ２＝２００〜６００ｍｍ、Δｗ＝１０〜５０ｍｍ、θｗ＝１〜１０°、Δθ＝０．１〜１．０°である。
【００３４】
図９は、モデル画像を示す図である。モデル画像作成手段１５は、予め記憶した３次元モデルに基づいて、移動体１の任意の位置及び方向におけるカメラ高さＨから見た２次元のモデル画像を作成する。従って、モデル画像作成手段１５が、上記の複数の位置（Ｘｔｎ，Ｙｔｎ）及び方向θｔｎのモデル画像Ｖｉｔを作成する。
【００３５】
モデル画像Ｖｉｔにおいても、カメラ画像Ｒｉｔと同様に、全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０を抽出したモデル画像Ｖｉθｉを作成する（図１０）。図１０は、二値化したモデル画像を示す図である。
【００３６】
そして、一致度算出手段１６が、カメラ画像Ｒｉｔと各モデル画像Ｖｉｔとの比較を行う（図４のステップＳ１４）。図１１は、カメラ画像とモデル画像とを比較する状態を示す図である。そして、カメラ画像Ｒｉｔと各モデル画像Ｖｉｔとの一致度ＣＶｉを求める（図４のステップＳ１５）。一致度ＣＶｉは、カメラ画像Ｒｉｔと各モデル画像Ｖｉｔとにおける特徴量（輝度値Ｖ「０」）の画素に基づいて、全画素に対する一致する画素の割合を採用する。
【００３７】
そして、最も一致度ＣＶｉの高い一つのモデル画像Ｖｉｔを採用して、そのモデル画像Ｖｉｔにおける（Ｘｔｎ，Ｙｔｎ）及び方向θｔｎを、所定時間ｔにおける移動体１の自己位置（ＸＲ，ＹＲ）及び方向θＲとして決定する（図４のステップＳ１６）。この自己位置（ＸＲ，ＹＲ）及び方向θＲに基づいて、予め記憶された目標軌道ＳＴに沿って移動体１が追従するように、操舵モータ４０及び走行モータ５０を制御する（図４のステップＳ１７）。即ち、移動体１が、所定時間ｔ＋１における、目標軌道上の目標位置（Ｘｔ＋１，Ｙｔ＋１）及び方向θｔ＋１に向かって走行するように制御する。
【００３８】
なお、上記では、完全に一致する色相Ｈに基づいて識別したが、ある程度の幅をもった色相Ｈ±Δｈに一致する画素を対象にして算出してもよい。また、上記では、カメラ画像Ｒｉｔとモデル画像Ｖｉｔとを色相Ｈに基づいて二値化処理して、一致度ＣＶｉを算出したが、二値化処理せずに、それぞれの画素における色情報の一致度ＣＶｉで算出してもよい。その場合、色相Ｈのみで識別するのではなく、例えば、グレー系に関しては明度（輝度）Ｖで識別する等、色の種類によって、色相Ｈ、彩度Ｓ、明度Ｖのいずれか又はそれらを組合わせて識別することもできる。
【００３９】
［第２実施形態］
第２実施形態について説明する。なお、第１実施形態と同じ内容については、詳細な説明を省略する。
図１２は、第２実施形態の自己位置認識システムを示すブロック図である。図１３及び図１４は、第２実施形態の自己位置認識手法を説明するフローチャート図である。
【００４０】
先ず、移動体１が走行する移動空間６と同一の３次元モデル（図６）を３次元モデル作成手段２１で作成する（図１３のステップＳ０１）。そして、移動体１が走行する目標軌道ＳＴを目標軌道記憶手段１７に記憶する（図１３のステップＳ０２）。
【００４１】
カメラ画像取得手段１０は、所定時間ｔにおいて、カメラ２でカメラ画像Ｒｉｔを取得する（図１３のステップＳ２１）。カメラ画像Ｒｉｔは、所定時間ｔにおける、移動体１の位置（ＸＭ，ＹＭ）及び方向θＭの画像である（図７）。
【００４２】
［基準線ｒＲＬｔ、ｌＲＬｔの位置］
図１５は、基準線を規定したカメラ画像を示す図である。基準線規定手段１２は、二値化したカメラ画像Ｒｉｔ（図８）における、対象物体（ラック）６０と正面壁との境界によって形成される境界垂直線を基準線ｒＲＬｔ、ｌＲＬｔと規定する（図１３のステップＳ２３）。本実施形態では、対象物体６０は、垂直方向及び水平方向に延設する支柱等を組み立てて形成されるラック（棚）なので、境界垂直線が現れる。図１５の通り、カメラ画像Ｒｉｔ内での座標系をＩＸ−ＩＹ座標とし、基準線ｒＲＬｔ、ｌＲＬｔの位置はＩＸ軸方向の座標値とする。
【００４３】
なお、境界垂直線は、移動体の位置等によっては１箇所しか現れない場合もある。また、境界垂直線は、対象物体６０の形状によって設定が異なり、例えば、対象物体６０と正面壁との境界が円弧の場合、その先端部分に接する垂直線としてもよい。
【００４４】
カメラ画像Ｒｉｔにおいて、通路中心線Ｃｐｔー１を設定する。通路中心線Ｃｐｔー１は、所定時間ｔ−１において決定した移動体１の位置（ＸＭ，ＹＭ）及び方向θＭに基づいて、時間ｔ−１における通路６１の中心位置（ＩＸ軸方向における各基準線ｒＲＬｔ−１、ｌＲＬｔ−１の間の中央位置）を採用する。
【００４５】
カメラ画像における基準線位置特定手段１３は、次の手順により基準線ｒＲＬｔ、ｌＲＬｔの位置を特定する。カメラ画像Ｒｉｔにおいて、通路中心線Ｃｐｔー１の左右に対象物体６０が存在する探索高さ範囲Ｒａを設定する。探索高さ範囲Ｒａ内において、カメラ画像Ｒｉｔにおける通路中心線Ｃｐｔー１から、ＩＹ軸方向における所定画素間隔ごとに、ＩＸ軸方向に向けて探索を行い、対象物体６０と識別される（輝度値Ｖ「１」と識別される）左右の位置を特定する。
【００４６】
そして、左右それぞれに対して特定された各位置をソートし、信頼性の低い上位量１／４の位置と下位量１／４の位置とを除き、中位量１／２の位置について平均値を求め、その値を左右の基準線ｒＲＬｔ、ｌＲＬｔの位置と特定する。
【００４７】
上記した通路中心線Ｃｐｔー１からの位置に基づいて、カメラ画像Ｒｉｔにおける画像中心垂直線から左右の基準線ｒＲＬｔ、ｌＲＬｔの距離Ｗｐａ、Ｗｐｂを特定する（図１３のステップＳ２４）。
【００４８】
そして、所定時間ｔにおいて、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出する（図１３のステップＳ２２）。
【００４９】
図１６は、基準線と移動体との位置関係を示す平面図である。３次元モデルにおける基準線位置特定手段１４は、モデル空間内における基準線ｒＲＬｔ、ｌＲＬｔの位置座標（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）を特定する（図１３のステップＳ２５）。この位置座標（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）は、所定時間ｔにおける目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出することにより（図１３のステップＳ２２）、対象物体６０の位置を推定して特定できる。
【００５０】
モデル空間内における基準線ｒＲＬｔ、ｌＲＬｔと移動体１との位置関係（図１６）に基づいて、次の（１）式〜（３）式が導かれる。
【００５１】
・・・（１）
【００５２】
・・・（２）
【００５３】
・・・（３）
【００５４】
ここで、（ＸＭ，ＹＭ）及びθＭはモデル空間内での移動体１の位置及び方向である。Ｆはカメラ２の焦点距離である。
【００５５】
上記の通り、Ｗｐａ、Ｗｐｂは、カメラ画像Ｒｉｔに基づいて特定される（図１３のステップＳ２４）。また、Ｆはカメラ２のスペックで特定される。これらを（２）式及び（３）式に入力することによりα、βが特定される。
【００５６】
さらに、上記の通り、（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）は、３次元モデルに基づいて特定される（図１３のステップＳ２５）。これらより、（１）式に基づいて、移動体１の方向θＭを任意に決めると、移動体１の位置（ＸＭ，ＹＭ）が一義的に決定される。
【００５７】
モデル画像作成手段１５は、３次元モデルに基づいて、移動体１の任意の位置及び方向におけるカメラ高さＨから見たモデル画像を作成する（図９）。
【００５８】
［モデル空間における移動体１の方向θＭ］
次に、基準線ｒＲＬｔ、ｌＲＬｔの拘束を行って、移動体１の方向θＭを認識する手順について説明する。先ず、所定時間ｔ−１において認識した移動体１の方向θＬを基準方向とする。基準方向θＬを中心とする任意の角度Ψを探索角度範囲とする。この探索角度範囲Ψを所定数（本例では７つ）に等分割し、φｉ（ｉ＝１，２，・・・，７）からなる集合Ｓφを決定する。Ｓφを（４）式に示す。
【００５９】
・・・（４）
【００６０】
このφｉをモデル空間内での移動体１の方向として、（１）式からモデル空間内での移動体１の位置（Ｘφｉ，Ｙφｉ）を決定する。そして、この決定したモデル空間内での移動体１の位置（Ｘφｉ，Ｙφｉ）及び方向φｉに基づいて、モデル画像作成手段１５でモデル画像Ｖｉφｉを作成する（図１３のステップＳ２６）。
【００６１】
全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０のみを抽出した、特徴量のみのモデル画像Ｖｉφｉを作成する（図１０）。
【００６２】
そして、一致度算出手段１６が、カメラ画像Ｒｉｔとモデル画像Ｖｉθｉとの比較を行って（図１１）、一致度ＣＶｉを求める（図１３のステップＳ２７）。なお、一致度ＣＶｉは、カメラ画像Ｒｉｔとモデル画像Ｖｉφｉとにおける特徴量（輝度値Ｖ「０」）の画素に基づいて、全画素に対する一致する画素の割合を採用する。
【００６３】
これまでの手順（図１３のステップＳ２６及びＳ２７）をそれぞれの方向φ１〜φ７で繰り返し行い、これによって得られるカメラ画像Ｒｉｔとモデル画像Ｖｉφ１〜７と比較して、それぞれの一致度ＣＶ１〜ＣＶ７を求める。
【００６４】
図１７は、任意の方向と一致度との関係を示すグラフ図である。一致度算出手段１６は、ｉ＝１〜７における方向φｉの一致度ＣＶｉに基づいて、最小二乗法によって３次元曲線ｆ（φ）による近似を行う（図１３のステップＳ２８）。そして、３次元曲線ｆ（φ）において極大となる方向φｍを求める（図１３のステップＳ２９）。
【００６５】
方向φｍを新たな基準方向θＬとして、探索角度範囲をΨ／２として、これまでの手順（図１３のステップＳ２６〜Ｓ２９）を行い、これをｎ１回繰り返して、自己位置認識手段１８で、モデル空間における移動体１の方向θＭを認識する（図１３のステップＳ３０）。
【００６６】
上記より、基準線ｒＲＬｔ、ｌＲＬｔの拘束によって移動体１の方向θＭを認識して、これを（１）式に入力して移動体１の位置（ＸθＭ，ＹθＭ）を求める。そして、移動体１の位置（ＸＭ，ＹＭ）の認識精度を向上するために、次の手順によって、基準線ｒＲＬｔ、ｌＲＬｔの拘束を行わずに、カメラ画像Ｒｉｔと各モデル画像Ｖｉθ１〜７とを比較する。
【００６７】
［モデル空間における移動体１の位置（ＸＭ，ＹＭ）］
次に、移動体１の位置（ＸＭ，ＹＭ）を認識する手順について説明する。任意の変化量δとして、上記で求めた位置（ＸθＭ，ＹθＭ）を中心とする位置集合（Ｓｘ，Ｓｙ）を作成する。集合Ｓｘを（５）式とし、集合Ｓｙを（６）式とする。従って、位置集合（Ｓｘ，Ｓｙ）は、集合Ｓｘと集合Ｓｙとの全ての組合わせである９（＝３×３）箇所の位置となり、位置（ＸθＭ，ＹθＭ）の周囲位置となる。なお、変化量δは、実空間における約５ｃｍとする。
【００６８】
・・・（５）
【００６９】
・・・（６）
【００７０】
９箇所の位置集合（Ｓｘ，Ｓｙ）のそれぞれについて、モデル画像作成手段１５でモデル画像ＶｉθＭを作成する（図１４のステップＳ３１）。そして、それぞれの９つのモデル画像ＶｉθＭ とカメラ画像Ｒｉｔとの比較を行い（図１１）、一致度算出手段１６で一致度ＣＶｉを求める（図１４のステップＳ３２）。そして、最大値となる一致度ＣＶｉを採用して、位置（ＸθＭ，ＹθＭ）を決定する（図１４のステップＳ３３）。
【００７１】
さらに、位置（ＸθＭ，ＹθＭ）を中心とし、任意の変化量をδ／２として、これまでの手順（図１４のステップＳ３１〜Ｓ３３）を行い、これをｎ２回繰り返して、自己位置認識手段１８が、モデル空間における移動体１の位置（ＸＭ，ＹＭ）を認識する（図１４のステップＳ３４）。
【００７２】
上記では、移動体１の方向θＭを決定する際に、一致度ＣＶｉから３次曲線ｆ（φ）による近似を行ったが、移動体１の位置（ＸＭ，ＹＭ）を決定する際にも、一致度ＣＶｉから３次曲線による近似を行うことができる。
【００７３】
［実空間における移動体１の位置（ＸＲ，ＹＲ）及び方向θＲ］
上記より認識されたモデル空間内での移動体１の位置（ＸＭ，ＹＭ）及び方向θＭから、実空間内での移動体１の位置（ＸＲ，ＹＲ）及び方向θＲ をそれぞれ（７）式及び（８）式により決定する（図１４のステップＳ３５）。
【００７４】
・・・（７）
【００７５】
・・・（８）
【００７６】
ここで、Ｋは実空間とモデル空間との縮尺、（ＸＳ，ＹＳ）は実空間とモデル空間とのＸＹ軸方向のオフセット量、λは実空間とモデル空間との回転角、を表す。
【００７７】
［障害物認識］
障害物認識手段１９は、自己位置（ＸＲ，ＹＲ）及び方向θＲ を認識した後、カメラ画像Ｒｉｔとモデル画像ＶｉＭに基づいて障害物認識を行う（図１４のステップＳ３６）。モデル画像ＶｉＭは、移動体１の位置（ＸＭ，ＹＭ）及び方向θＭに基づいて作成する。
【００７８】
通路６１と壁面からなる領域（対象物体６０以外の領域）を、障害物を検出するための領域（検出領域）とする。モデル画像ＶｉＭにおいて検出領域を抽出する。カメラ画像Ｒｉｔについても検出領域を抽出する。
【００７９】
そして、カメラ画像Ｒｉｔとモデル画像ＶｉＭとのそれぞれに対して検出領域内の画素をＨＳＶ変換し、それぞれの色相（Ｈ）、明度（Ｖ）、彩度（Ｓ）の差が閾値未満となる画素を除去して、カメラ画像Ｒｉｔの検出領域において、モデル画像ＶｉＭと異なる色の画素のみを抽出した画像を作成する。そして、この画像において、一定値以上の面積を有する領域がある場合には、その領域を障害物として認識する。
【００８０】
［目標軌道追従］
目標軌道追従手段２０は、上記によって認定した移動体１の位置（ＸＲ，ＹＲ）及び方向θＲに基づいて、予め記憶された目標軌道ＳＴ（走行路）に沿って移動体１が追従するように、操舵モータ４０及び走行モータ５０を制御する（図１４のステップＳ３７）。
【００８１】
そして、所定時間間隔Δｔごとに自己位置認識を行って、移動体１が目標軌道ＳＴを追従するように制御する。なお、上記手法において、所定時間ｔ−１における種々の値を採用するステップがあるので、スタート時における値（初期値）は予め設定する。
【００８２】
そして、上記の障害物認識手法によって障害物を認識した際には、移動体１が停止する等の所定の処置を取るように、目標軌道追従手段２０が、操舵モータ４０及び走行モータ５０を制御する。
【００８３】
［その他］
上記実施形態では、移動体１に前方に向けたカメラ２からのカメラ画像に基づいて自己位置を認識しているが、後方に向けたカメラ２からのカメラ画像を用いたり、前方からのカメラ画像と組合わせて認識してもよい。
【００８４】
また、上記では、ラックが配置された物流倉庫内を移動する無人搬送車について説明したが、座席（対象物体６０）が配置されたコンサート会場（移動空間６）内を走行又は歩行する警備ロボット（移動体１）等でも適用でき、本発明の趣旨を逸脱しない範囲内であれば様々な移動体及び移動空間等に採用できる。
【図面の簡単な説明】
【００８５】
【図１】自己位置認識システムを搭載した移動体を示す図である。
【図２】目標軌道を説明するための図である。
【図３】第１実施形態の自己位置認識システムを示すブロック図である。
【図４】第１実施形態の自己位置認識手法を説明するフローチャート図である。
【図５】第１実施形態の比較するモデル画像を説明するための図である。
【図６】移動空間内の３次元モデルを示す概念図である。
【図７】カメラ画像を示す図である。
【図８】二値化したカメラ画像を示す図である。
【図９】モデル画像を示す図である。
【図１０】二値化したモデル画像を示す図である。
【図１１】カメラ画像とモデル画像とを比較する状態を示す図である。
【図１２】第２実施形態の自己位置認識システムを示すブロック図である。
【図１３】第２実施形態の自己位置認識手法を説明するフローチャート図である。
【図１４】図１３に続くフローチャート図である。
【図１５】基準線を規定したカメラ画像を示す図である。
【図１６】基準線と移動体との位置関係を示す平面図である。
【図１７】任意の方向と一致度との関係を示すグラフ図である。
【符号の説明】
【００８６】
１ 移動体
２ カメラ
３ コントローラ
６ 移動空間
６０ 対象物体
６１ 通路
Ｒｉｔ カメラ画像
Ｖｉｔ モデル画像
ｒＲＬｔ、ｌＲＬｔ 基準線
θＭ モデル空間における移動体の方向
ＸＭ，ＹＭ モデル空間における移動体の位置
θＲ 実空間における移動体の方向
ＸＲ，ＹＲ 実空間における移動体の位置
【技術分野】
【０００１】
本発明は、通路を形成する対象物体が設置された移動空間を移動する移動体の自己位置認識手法及びシステムに関する。
【背景技術】
【０００２】
物流倉庫等では、効率化及び省力化を向上するために、無人搬送車が荷物を運搬する。一般に、無人搬送車の自動走行として、電磁や磁気を検知して走行する方式（電磁誘導式・磁気誘導式）がある。この方式では、誘導手段となる磁気テープを床面に敷設したり（特許文献１等参照）、磁性体を床面に埋設する（特許文献２等参照）。そして、無人搬送車は、この誘導手段からの電磁や磁気等を検知して自動走行する。従って、磁性体等の誘導手段の位置変更に多大な手間や費用を要するから、倉庫内のレイアウト変更が非常に困難となる。また、この方式では磁性体を埋設等するので、導入コストが大きい。
【特許文献１】特開２００２−２７８６２２号公報
【特許文献２】特開２００５−１２２５６８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
そこで、本発明が解決しようとする課題は、移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さく、さらに、高速且つ正確に自己位置認識できる移動体の自己位置認識手法及びシステムを提供することである。
【課題を解決するための手段】
【０００４】
上記の課題を解決するために、本発明に係る自己位置認識手法は、
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識手法であって、移動空間の３次元モデルを作成するステップと、所定時間における移動体の位置及び方向からカメラ画像を取得するステップと、所定時間における目標軌道上の目標位置及び方向を算出するステップと、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成するステップと、この複数のモデル画像とカメラ画像との色情報の一致度を算出するステップと、この各一致度に基づいて、移動体の位置及び方向を認識するステップとを含む。
【０００５】
好ましくは、移動空間に対象物体を設置するステップと、カメラ画像における対象物体の境界垂直線を基準線に規定するステップと、カメラ画像における基準線の位置を特定するステップと、３次元モデルにおける基準線の位置を特定するステップと、この特定した各基準線の位置に基づいて、３次元モデルから任意の位置及び方向における複数のモデル画像を形成するステップとを含む。
【０００６】
好ましくは、対象物体が通路を形成する通路形成体である。
【０００７】
好ましくは、カメラ画像及びモデル画像を二値化処理するステップを含む。
【０００８】
好ましくは、各一致度から２次又は３次曲線に近似して、曲線の極大値に基づいて移動体の位置及び方向の双方又は一方を認識するステップを含む。
【０００９】
好ましくは、移動空間の入出庫管理システムからの情報に基づいて、３次元モデルにおける対象物体の形状を変更するステップを含む。
【００１０】
好ましくは、カメラ画像及びモデル画像における対象物体以外からなる検出領域を抽出するステップと、この各検出領域を比較するステップと、この比較値と所定の閾値とに基づいて移動体の方向における障害物を認識するステップとを含む。
【００１１】
また、本発明に係る自己位置認識システムは、
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識システムであって、移動空間の３次元モデルを作成する手段と、所定時間における移動体の位置及び方向からカメラ画像を取得する手段と、所定時間における目標軌道上の目標位置及び方向を算出する手段と、３次元モデルから目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段と、この複数のモデル画像とカメラ画像との色情報の一致度を算出する手段と、この各一致度に基づいて、移動体の位置及び方向を認識する手段とを備えた。
【００１２】
好ましくは、移動空間に対象物体を設置する手段と、カメラ画像における対象物体の境界垂直線を基準線に規定する手段と、カメラ画像における基準線の位置を特定する手段と、３次元モデルにおける基準線の位置を特定する手段と、この特定した各基準線の位置に基づいて、３次元モデルから任意の位置及び方向における複数のモデル画像を形成する手段とを備えた。
【００１３】
好ましくは、対象物体が通路を形成する通路形成体である。
【００１４】
好ましくは、カメラ画像及びモデル画像を二値化処理する手段を備えた。
【００１５】
好ましくは、各一致度から２次又は３次曲線に近似して、曲線の極大値に基づいて移動体の位置及び方向の双方又は一方を認識する手段を備えた。
【００１６】
好ましくは、移動空間の入出庫管理システムからの情報に基づいて、３次元モデルにおける対象物体の形状を変更する手段を備えた。
【００１７】
好ましくは、カメラ画像及びモデル画像における対象物体以外からなる検出領域を抽出する手段と、この各検出領域を比較する手段と、この比較値と所定の閾値とに基づいて移動体の方向における障害物を認識する手段とを備えた。
【発明の効果】
【００１８】
本発明に係る３次元モデルを用いた自己位置認識手法及びシステムは、移動体のカメラで取得したカメラ画像と予め作成した移動空間の３次元モデルとに基づいて、移動体の自己位置及び方向の認識を可能とする。そのため、移動空間を変更しても、３次元モデルを移動空間に合わせて変更するだけで対応可能なので、移動空間内のレイアウトを変更しても容易に適用でき、導入コストが小さい。さらに、所定時間における目標軌道上の目標位置及び方向を算出して、３次元モデルから目標位置及び方向の所定範囲内におけるモデル画像を形成してカメラ画像との比較を行うので、比較対象とするモデル画像の枚数を限定でき、高速に処理できる。また、モデル画像とカメラ画像とを、輪郭形状に基づく画素位置情報でなく色情報の一致度に基づいて識別するので、推定される位置及び方向の信頼性が非常に高い。
【００１９】
また、本発明では、カメラ画像における対象物体の境界垂直線を基準線に規定することができる。そして、カメラ画像及び３次元モデルにおける基準線の位置に基づいて、移動体の自己位置を認識する。従って、自己位置を認識するための特徴量が境界垂直線である基準線の位置となり、位置認識のための基準線が少なくて明確なので、高速且つ正確に自己位置を認識できる。
【発明を実施するための最良の形態】
【００２０】
以下、添付図面に基づいて、本発明に係る３次元モデルを用いた自己位置認識手法及びシステムについて説明する。
【００２１】
［全体構成］
図１は、自己位置認識システムを搭載した移動体を示す図である。本実施形態では、移動体１は無人搬送車からなる。図１の通り、移動体１は、前方に向けてカメラ２を備える。カメラ２は、床面Ｆから一定のカメラ高さＨに設置される。移動体１は、コントローラ３を備える。
【００２２】
移動体１は、操舵輪４及び走行輪５を備える。操舵輪４は、操舵モータ４０の駆動に基づいて操舵角を変更する。走行輪５は、走行モータ５０の駆動に基づいて前後進及び停止する。コントローラ３は、後で詳説するように、カメラ２からの画像に基づいて自己位置及び方向を認識し、予め記憶された目標軌道ＳＴ（走行路）に沿って移動体１が走行するように、操舵モータ４０及び走行モータ５０の駆動を制御する。
【００２３】
［目標軌道］
図２は、目標軌道を説明するための図である。図２の通り、移動体１は、コントローラ３が操舵モータ４０及び走行モータ５０の駆動を制御して、予め記憶された２次元座標（ＸＹ座標）及び方向からなる目標軌道ＳＴ（図２の二点鎖線）に沿って操舵、減速、加速、停止及び進行等する。従って、コントローラ３は、所定時間間隔Δｔごとの所定時間・・・ｔ−１、ｔ、ｔ＋１・・・において、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出可能とする。なお、本実施形態では、所定時間間隔Δｔは、例えば約０．３秒である。
【００２４】
［第１実施形態］
第１実施形態について説明する。
図３は、第１実施形態の自己位置認識システムを示すブロック図である。図４は、第１実施形態の自己位置認識手法を説明するフローチャート図である。図５は、第１実施形態の比較するモデル画像を説明するための図である。図６は、移動空間内の３次元モデルを示す概念図である。
【００２５】
自己位置認識システムは、カメラ２及びコントローラ３等からなる（図３）。コントローラ３は、ＣＰＵ等を搭載したコンピュータからなっており、カメラ２から取得したカメラ画像等を処理して、移動体１の位置及び方向を認識する。
【００２６】
本実施形態では、移動体１は、物流倉庫を移動空間６として移動する（図６）。この移動空間６では、荷物等を配列するための複数のラック（棚）６０が配置されている。移動体１は、それぞれのラック６０の間を通路６１として目標軌道ＳＴに沿って走行する。
【００２７】
先ず、移動体１が実際に走行する移動空間６（実空間）と同一の３次元モデル（モデル空間）（図６）を３次元モデル作成手段２１で作成する（図４のステップＳ０１）。そして、３次元モデル記憶手段１１が、この３次元モデルを記憶する。なお、３次元モデルは、実空間と略同一色で作成される。３次元モデル作成手段２１は、例えばＣＡＤシステムからなる。
【００２８】
３次元モデル作成手段２１は、対象物体（ラック）６０に配列される荷物を管理する入出庫・在庫管理システム（ＷＭＳ）２２に基づいて、３次元モデルにおける対象物体６０に荷物を配置した形状に随時変更する。これにより、常に、実際（実空間）と３次元モデル（モデル空間）との移動空間６における形状や色情報が一致する。色情報は、色相（色度）Ｈ、彩度Ｓ、明度（輝度）Ｖからなる。
【００２９】
そして、移動体１が走行する目標軌道ＳＴを作成して、目標軌道記憶手段１７に目標軌道ＳＴを記憶する（図４のステップＳ０２）。
【００３０】
図７は、カメラ画像を示す図である。そして、カメラ画像取得手段１０は、所定時間ｔにおいて、カメラ２でカメラ画像Ｒｉｔを取得する（図４のステップＳ１１）。カメラ画像Ｒｉｔは、所定時間ｔにおける、移動体１の位置（ＸＭ，ＹＭ）及び方向θＭに対する画像である。
【００３１】
図８は、二値化したカメラ画像を示す図である。カメラ画像Ｒｉｔの全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０を抽出したカメラ画像Ｒｉｔを作成する（図３のステップＳ３）。なお、床面及び壁面の色相Ｈが予め設定されているので、対象物体６０を抽出した二値化が可能である。
【００３２】
そして、所定時間ｔにおいて、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出する（図４のステップＳ１２）。
【００３３】
そして、所定時間ｔの目標位置（Ｘｔ，Ｙｔ）及び方向θｔの所定範囲内における複数の位置（Ｘｔｎ，Ｙｔｎ）及び方向θｔｎのモデル画像Ｖｉｔを作成する（図４のステップＳ１３）。図５の通り、本実施形態では、所定範囲（Ｗ１−Ｗ２）は、目標位置（Ｘｔ，Ｙｔ）を中心とする矩形範囲（円範囲等でもよい）であって、所定間隔Δｗごとに、複数の位置（Ｘｔｎ，Ｙｔｎ）が設定される（図５の△印）。さらに、それぞれの位置（Ｘｔｎ，Ｙｔｎ）ごとに、目標方向θｔの左右所定角度範囲θｗに所定角度間隔Δθごとに、複数のθｔｎが設定される（図５の×印）。なお、本実施形態では、例えば、Ｗ１＝Ｗ２＝２００〜６００ｍｍ、Δｗ＝１０〜５０ｍｍ、θｗ＝１〜１０°、Δθ＝０．１〜１．０°である。
【００３４】
図９は、モデル画像を示す図である。モデル画像作成手段１５は、予め記憶した３次元モデルに基づいて、移動体１の任意の位置及び方向におけるカメラ高さＨから見た２次元のモデル画像を作成する。従って、モデル画像作成手段１５が、上記の複数の位置（Ｘｔｎ，Ｙｔｎ）及び方向θｔｎのモデル画像Ｖｉｔを作成する。
【００３５】
モデル画像Ｖｉｔにおいても、カメラ画像Ｒｉｔと同様に、全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０を抽出したモデル画像Ｖｉθｉを作成する（図１０）。図１０は、二値化したモデル画像を示す図である。
【００３６】
そして、一致度算出手段１６が、カメラ画像Ｒｉｔと各モデル画像Ｖｉｔとの比較を行う（図４のステップＳ１４）。図１１は、カメラ画像とモデル画像とを比較する状態を示す図である。そして、カメラ画像Ｒｉｔと各モデル画像Ｖｉｔとの一致度ＣＶｉを求める（図４のステップＳ１５）。一致度ＣＶｉは、カメラ画像Ｒｉｔと各モデル画像Ｖｉｔとにおける特徴量（輝度値Ｖ「０」）の画素に基づいて、全画素に対する一致する画素の割合を採用する。
【００３７】
そして、最も一致度ＣＶｉの高い一つのモデル画像Ｖｉｔを採用して、そのモデル画像Ｖｉｔにおける（Ｘｔｎ，Ｙｔｎ）及び方向θｔｎを、所定時間ｔにおける移動体１の自己位置（ＸＲ，ＹＲ）及び方向θＲとして決定する（図４のステップＳ１６）。この自己位置（ＸＲ，ＹＲ）及び方向θＲに基づいて、予め記憶された目標軌道ＳＴに沿って移動体１が追従するように、操舵モータ４０及び走行モータ５０を制御する（図４のステップＳ１７）。即ち、移動体１が、所定時間ｔ＋１における、目標軌道上の目標位置（Ｘｔ＋１，Ｙｔ＋１）及び方向θｔ＋１に向かって走行するように制御する。
【００３８】
なお、上記では、完全に一致する色相Ｈに基づいて識別したが、ある程度の幅をもった色相Ｈ±Δｈに一致する画素を対象にして算出してもよい。また、上記では、カメラ画像Ｒｉｔとモデル画像Ｖｉｔとを色相Ｈに基づいて二値化処理して、一致度ＣＶｉを算出したが、二値化処理せずに、それぞれの画素における色情報の一致度ＣＶｉで算出してもよい。その場合、色相Ｈのみで識別するのではなく、例えば、グレー系に関しては明度（輝度）Ｖで識別する等、色の種類によって、色相Ｈ、彩度Ｓ、明度Ｖのいずれか又はそれらを組合わせて識別することもできる。
【００３９】
［第２実施形態］
第２実施形態について説明する。なお、第１実施形態と同じ内容については、詳細な説明を省略する。
図１２は、第２実施形態の自己位置認識システムを示すブロック図である。図１３及び図１４は、第２実施形態の自己位置認識手法を説明するフローチャート図である。
【００４０】
先ず、移動体１が走行する移動空間６と同一の３次元モデル（図６）を３次元モデル作成手段２１で作成する（図１３のステップＳ０１）。そして、移動体１が走行する目標軌道ＳＴを目標軌道記憶手段１７に記憶する（図１３のステップＳ０２）。
【００４１】
カメラ画像取得手段１０は、所定時間ｔにおいて、カメラ２でカメラ画像Ｒｉｔを取得する（図１３のステップＳ２１）。カメラ画像Ｒｉｔは、所定時間ｔにおける、移動体１の位置（ＸＭ，ＹＭ）及び方向θＭの画像である（図７）。
【００４２】
［基準線ｒＲＬｔ、ｌＲＬｔの位置］
図１５は、基準線を規定したカメラ画像を示す図である。基準線規定手段１２は、二値化したカメラ画像Ｒｉｔ（図８）における、対象物体（ラック）６０と正面壁との境界によって形成される境界垂直線を基準線ｒＲＬｔ、ｌＲＬｔと規定する（図１３のステップＳ２３）。本実施形態では、対象物体６０は、垂直方向及び水平方向に延設する支柱等を組み立てて形成されるラック（棚）なので、境界垂直線が現れる。図１５の通り、カメラ画像Ｒｉｔ内での座標系をＩＸ−ＩＹ座標とし、基準線ｒＲＬｔ、ｌＲＬｔの位置はＩＸ軸方向の座標値とする。
【００４３】
なお、境界垂直線は、移動体の位置等によっては１箇所しか現れない場合もある。また、境界垂直線は、対象物体６０の形状によって設定が異なり、例えば、対象物体６０と正面壁との境界が円弧の場合、その先端部分に接する垂直線としてもよい。
【００４４】
カメラ画像Ｒｉｔにおいて、通路中心線Ｃｐｔー１を設定する。通路中心線Ｃｐｔー１は、所定時間ｔ−１において決定した移動体１の位置（ＸＭ，ＹＭ）及び方向θＭに基づいて、時間ｔ−１における通路６１の中心位置（ＩＸ軸方向における各基準線ｒＲＬｔ−１、ｌＲＬｔ−１の間の中央位置）を採用する。
【００４５】
カメラ画像における基準線位置特定手段１３は、次の手順により基準線ｒＲＬｔ、ｌＲＬｔの位置を特定する。カメラ画像Ｒｉｔにおいて、通路中心線Ｃｐｔー１の左右に対象物体６０が存在する探索高さ範囲Ｒａを設定する。探索高さ範囲Ｒａ内において、カメラ画像Ｒｉｔにおける通路中心線Ｃｐｔー１から、ＩＹ軸方向における所定画素間隔ごとに、ＩＸ軸方向に向けて探索を行い、対象物体６０と識別される（輝度値Ｖ「１」と識別される）左右の位置を特定する。
【００４６】
そして、左右それぞれに対して特定された各位置をソートし、信頼性の低い上位量１／４の位置と下位量１／４の位置とを除き、中位量１／２の位置について平均値を求め、その値を左右の基準線ｒＲＬｔ、ｌＲＬｔの位置と特定する。
【００４７】
上記した通路中心線Ｃｐｔー１からの位置に基づいて、カメラ画像Ｒｉｔにおける画像中心垂直線から左右の基準線ｒＲＬｔ、ｌＲＬｔの距離Ｗｐａ、Ｗｐｂを特定する（図１３のステップＳ２４）。
【００４８】
そして、所定時間ｔにおいて、移動体１が目標軌道ＳＴに沿って移動するための目標軌道上の目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出する（図１３のステップＳ２２）。
【００４９】
図１６は、基準線と移動体との位置関係を示す平面図である。３次元モデルにおける基準線位置特定手段１４は、モデル空間内における基準線ｒＲＬｔ、ｌＲＬｔの位置座標（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）を特定する（図１３のステップＳ２５）。この位置座標（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）は、所定時間ｔにおける目標位置（Ｘｔ，Ｙｔ）及び方向θｔを算出することにより（図１３のステップＳ２２）、対象物体６０の位置を推定して特定できる。
【００５０】
モデル空間内における基準線ｒＲＬｔ、ｌＲＬｔと移動体１との位置関係（図１６）に基づいて、次の（１）式〜（３）式が導かれる。
【００５１】
・・・（１）
【００５２】
・・・（２）
【００５３】
・・・（３）
【００５４】
ここで、（ＸＭ，ＹＭ）及びθＭはモデル空間内での移動体１の位置及び方向である。Ｆはカメラ２の焦点距離である。
【００５５】
上記の通り、Ｗｐａ、Ｗｐｂは、カメラ画像Ｒｉｔに基づいて特定される（図１３のステップＳ２４）。また、Ｆはカメラ２のスペックで特定される。これらを（２）式及び（３）式に入力することによりα、βが特定される。
【００５６】
さらに、上記の通り、（Ｘｑ，Ｙｑ）、（Ｘｐ，Ｙｐ）は、３次元モデルに基づいて特定される（図１３のステップＳ２５）。これらより、（１）式に基づいて、移動体１の方向θＭを任意に決めると、移動体１の位置（ＸＭ，ＹＭ）が一義的に決定される。
【００５７】
モデル画像作成手段１５は、３次元モデルに基づいて、移動体１の任意の位置及び方向におけるカメラ高さＨから見たモデル画像を作成する（図９）。
【００５８】
［モデル空間における移動体１の方向θＭ］
次に、基準線ｒＲＬｔ、ｌＲＬｔの拘束を行って、移動体１の方向θＭを認識する手順について説明する。先ず、所定時間ｔ−１において認識した移動体１の方向θＬを基準方向とする。基準方向θＬを中心とする任意の角度Ψを探索角度範囲とする。この探索角度範囲Ψを所定数（本例では７つ）に等分割し、φｉ（ｉ＝１，２，・・・，７）からなる集合Ｓφを決定する。Ｓφを（４）式に示す。
【００５９】
・・・（４）
【００６０】
このφｉをモデル空間内での移動体１の方向として、（１）式からモデル空間内での移動体１の位置（Ｘφｉ，Ｙφｉ）を決定する。そして、この決定したモデル空間内での移動体１の位置（Ｘφｉ，Ｙφｉ）及び方向φｉに基づいて、モデル画像作成手段１５でモデル画像Ｖｉφｉを作成する（図１３のステップＳ２６）。
【００６１】
全画素（全ピクセル）をＨＳＶ変換して、床面及び壁面の色相Ｈからなる画素の輝度値Ｖを「０」に、それ以外の画素の輝度値Ｖを「１」に変換して二値化処理し、対象物体６０のみを抽出した、特徴量のみのモデル画像Ｖｉφｉを作成する（図１０）。
【００６２】
そして、一致度算出手段１６が、カメラ画像Ｒｉｔとモデル画像Ｖｉθｉとの比較を行って（図１１）、一致度ＣＶｉを求める（図１３のステップＳ２７）。なお、一致度ＣＶｉは、カメラ画像Ｒｉｔとモデル画像Ｖｉφｉとにおける特徴量（輝度値Ｖ「０」）の画素に基づいて、全画素に対する一致する画素の割合を採用する。
【００６３】
これまでの手順（図１３のステップＳ２６及びＳ２７）をそれぞれの方向φ１〜φ７で繰り返し行い、これによって得られるカメラ画像Ｒｉｔとモデル画像Ｖｉφ１〜７と比較して、それぞれの一致度ＣＶ１〜ＣＶ７を求める。
【００６４】
図１７は、任意の方向と一致度との関係を示すグラフ図である。一致度算出手段１６は、ｉ＝１〜７における方向φｉの一致度ＣＶｉに基づいて、最小二乗法によって３次元曲線ｆ（φ）による近似を行う（図１３のステップＳ２８）。そして、３次元曲線ｆ（φ）において極大となる方向φｍを求める（図１３のステップＳ２９）。
【００６５】
方向φｍを新たな基準方向θＬとして、探索角度範囲をΨ／２として、これまでの手順（図１３のステップＳ２６〜Ｓ２９）を行い、これをｎ１回繰り返して、自己位置認識手段１８で、モデル空間における移動体１の方向θＭを認識する（図１３のステップＳ３０）。
【００６６】
上記より、基準線ｒＲＬｔ、ｌＲＬｔの拘束によって移動体１の方向θＭを認識して、これを（１）式に入力して移動体１の位置（ＸθＭ，ＹθＭ）を求める。そして、移動体１の位置（ＸＭ，ＹＭ）の認識精度を向上するために、次の手順によって、基準線ｒＲＬｔ、ｌＲＬｔの拘束を行わずに、カメラ画像Ｒｉｔと各モデル画像Ｖｉθ１〜７とを比較する。
【００６７】
［モデル空間における移動体１の位置（ＸＭ，ＹＭ）］
次に、移動体１の位置（ＸＭ，ＹＭ）を認識する手順について説明する。任意の変化量δとして、上記で求めた位置（ＸθＭ，ＹθＭ）を中心とする位置集合（Ｓｘ，Ｓｙ）を作成する。集合Ｓｘを（５）式とし、集合Ｓｙを（６）式とする。従って、位置集合（Ｓｘ，Ｓｙ）は、集合Ｓｘと集合Ｓｙとの全ての組合わせである９（＝３×３）箇所の位置となり、位置（ＸθＭ，ＹθＭ）の周囲位置となる。なお、変化量δは、実空間における約５ｃｍとする。
【００６８】
・・・（５）
【００６９】
・・・（６）
【００７０】
９箇所の位置集合（Ｓｘ，Ｓｙ）のそれぞれについて、モデル画像作成手段１５でモデル画像ＶｉθＭを作成する（図１４のステップＳ３１）。そして、それぞれの９つのモデル画像ＶｉθＭ とカメラ画像Ｒｉｔとの比較を行い（図１１）、一致度算出手段１６で一致度ＣＶｉを求める（図１４のステップＳ３２）。そして、最大値となる一致度ＣＶｉを採用して、位置（ＸθＭ，ＹθＭ）を決定する（図１４のステップＳ３３）。
【００７１】
さらに、位置（ＸθＭ，ＹθＭ）を中心とし、任意の変化量をδ／２として、これまでの手順（図１４のステップＳ３１〜Ｓ３３）を行い、これをｎ２回繰り返して、自己位置認識手段１８が、モデル空間における移動体１の位置（ＸＭ，ＹＭ）を認識する（図１４のステップＳ３４）。
【００７２】
上記では、移動体１の方向θＭを決定する際に、一致度ＣＶｉから３次曲線ｆ（φ）による近似を行ったが、移動体１の位置（ＸＭ，ＹＭ）を決定する際にも、一致度ＣＶｉから３次曲線による近似を行うことができる。
【００７３】
［実空間における移動体１の位置（ＸＲ，ＹＲ）及び方向θＲ］
上記より認識されたモデル空間内での移動体１の位置（ＸＭ，ＹＭ）及び方向θＭから、実空間内での移動体１の位置（ＸＲ，ＹＲ）及び方向θＲ をそれぞれ（７）式及び（８）式により決定する（図１４のステップＳ３５）。
【００７４】
・・・（７）
【００７５】
・・・（８）
【００７６】
ここで、Ｋは実空間とモデル空間との縮尺、（ＸＳ，ＹＳ）は実空間とモデル空間とのＸＹ軸方向のオフセット量、λは実空間とモデル空間との回転角、を表す。
【００７７】
［障害物認識］
障害物認識手段１９は、自己位置（ＸＲ，ＹＲ）及び方向θＲ を認識した後、カメラ画像Ｒｉｔとモデル画像ＶｉＭに基づいて障害物認識を行う（図１４のステップＳ３６）。モデル画像ＶｉＭは、移動体１の位置（ＸＭ，ＹＭ）及び方向θＭに基づいて作成する。
【００７８】
通路６１と壁面からなる領域（対象物体６０以外の領域）を、障害物を検出するための領域（検出領域）とする。モデル画像ＶｉＭにおいて検出領域を抽出する。カメラ画像Ｒｉｔについても検出領域を抽出する。
【００７９】
そして、カメラ画像Ｒｉｔとモデル画像ＶｉＭとのそれぞれに対して検出領域内の画素をＨＳＶ変換し、それぞれの色相（Ｈ）、明度（Ｖ）、彩度（Ｓ）の差が閾値未満となる画素を除去して、カメラ画像Ｒｉｔの検出領域において、モデル画像ＶｉＭと異なる色の画素のみを抽出した画像を作成する。そして、この画像において、一定値以上の面積を有する領域がある場合には、その領域を障害物として認識する。
【００８０】
［目標軌道追従］
目標軌道追従手段２０は、上記によって認定した移動体１の位置（ＸＲ，ＹＲ）及び方向θＲに基づいて、予め記憶された目標軌道ＳＴ（走行路）に沿って移動体１が追従するように、操舵モータ４０及び走行モータ５０を制御する（図１４のステップＳ３７）。
【００８１】
そして、所定時間間隔Δｔごとに自己位置認識を行って、移動体１が目標軌道ＳＴを追従するように制御する。なお、上記手法において、所定時間ｔ−１における種々の値を採用するステップがあるので、スタート時における値（初期値）は予め設定する。
【００８２】
そして、上記の障害物認識手法によって障害物を認識した際には、移動体１が停止する等の所定の処置を取るように、目標軌道追従手段２０が、操舵モータ４０及び走行モータ５０を制御する。
【００８３】
［その他］
上記実施形態では、移動体１に前方に向けたカメラ２からのカメラ画像に基づいて自己位置を認識しているが、後方に向けたカメラ２からのカメラ画像を用いたり、前方からのカメラ画像と組合わせて認識してもよい。
【００８４】
また、上記では、ラックが配置された物流倉庫内を移動する無人搬送車について説明したが、座席（対象物体６０）が配置されたコンサート会場（移動空間６）内を走行又は歩行する警備ロボット（移動体１）等でも適用でき、本発明の趣旨を逸脱しない範囲内であれば様々な移動体及び移動空間等に採用できる。
【図面の簡単な説明】
【００８５】
【図１】自己位置認識システムを搭載した移動体を示す図である。
【図２】目標軌道を説明するための図である。
【図３】第１実施形態の自己位置認識システムを示すブロック図である。
【図４】第１実施形態の自己位置認識手法を説明するフローチャート図である。
【図５】第１実施形態の比較するモデル画像を説明するための図である。
【図６】移動空間内の３次元モデルを示す概念図である。
【図７】カメラ画像を示す図である。
【図８】二値化したカメラ画像を示す図である。
【図９】モデル画像を示す図である。
【図１０】二値化したモデル画像を示す図である。
【図１１】カメラ画像とモデル画像とを比較する状態を示す図である。
【図１２】第２実施形態の自己位置認識システムを示すブロック図である。
【図１３】第２実施形態の自己位置認識手法を説明するフローチャート図である。
【図１４】図１３に続くフローチャート図である。
【図１５】基準線を規定したカメラ画像を示す図である。
【図１６】基準線と移動体との位置関係を示す平面図である。
【図１７】任意の方向と一致度との関係を示すグラフ図である。
【符号の説明】
【００８６】
１ 移動体
２ カメラ
３ コントローラ
６ 移動空間
６０ 対象物体
６１ 通路
Ｒｉｔ カメラ画像
Ｖｉｔ モデル画像
ｒＲＬｔ、ｌＲＬｔ 基準線
θＭ モデル空間における移動体の方向
ＸＭ，ＹＭ モデル空間における移動体の位置
θＲ 実空間における移動体の方向
ＸＲ，ＹＲ 実空間における移動体の位置
【特許請求の範囲】
【請求項１】
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識手法であって、前記移動空間の３次元モデルを作成するステップと、所定時間における前記移動体の位置及び方向からカメラ画像を取得するステップと、前記所定時間における前記目標軌道上の目標位置及び方向を算出するステップと、前記３次元モデルから前記目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成するステップと、この複数のモデル画像と前記カメラ画像との色情報の一致度を算出するステップと、この各一致度に基づいて、前記移動体の位置及び方向を認識するステップとを含むことを特徴とする自己位置認識手法。
【請求項２】
前記移動空間に対象物体を設置するステップと、前記カメラ画像における前記対象物体の境界垂直線を基準線に規定するステップと、前記カメラ画像における前記基準線の位置を特定するステップと、前記３次元モデルにおける前記基準線の位置を特定するステップと、この特定した各基準線の位置に基づいて、前記３次元モデルから任意の位置及び方向における複数のモデル画像を形成するステップとを含むことを特徴とする請求項１に記載の自己位置認識手法。
【請求項３】
前記対象物体が通路を形成する通路形成体であることを特徴とする請求項１又は２に記載の自己位置認識手法。
【請求項４】
前記カメラ画像及び前記モデル画像を二値化処理するステップを含むことを特徴とする請求項１〜３のいずれかに記載の自己位置認識手法。
【請求項５】
前記各一致度から２次又は３次曲線に近似して、前記曲線の極大値に基づいて前記移動体の位置及び方向の双方又は一方を認識するステップを含むことを特徴とする請求項１〜４のいずれかに記載の自己位置認識手法。
【請求項６】
前記移動空間の入出庫管理システムからの情報に基づいて、前記３次元モデルにおける前記対象物体の形状を変更するステップを含むことを特徴とする請求項１〜５のいずれかに記載の自己位置認識手法。
【請求項７】
前記カメラ画像及び前記モデル画像における前記対象物体以外からなる検出領域を抽出するステップと、この各検出領域を比較するステップと、この比較値と所定の閾値とに基づいて前記移動体の方向における障害物を認識するステップとを含むことを特徴とする請求項１〜６のいずれかに記載の自己位置認識手法。
【請求項８】
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識システムであって、前記移動空間の３次元モデルを作成する手段と、所定時間における前記移動体の位置及び方向からカメラ画像を取得する手段と、前記所定時間における前記目標軌道上の目標位置及び方向を算出する手段と、前記３次元モデルから前記目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段と、この複数のモデル画像と前記カメラ画像との色情報の一致度を算出する手段と、この各一致度に基づいて、前記移動体の位置及び方向を認識する手段とを備えたことを特徴とする自己位置認識システム。
【請求項９】
前記移動空間に対象物体を設置する手段と、前記カメラ画像における前記対象物体の境界垂直線を基準線に規定する手段と、前記カメラ画像における前記基準線の位置を特定する手段と、前記３次元モデルにおける前記基準線の位置を特定する手段と、この特定した各基準線の位置に基づいて、前記３次元モデルから任意の位置及び方向における複数のモデル画像を形成する手段とを備えたことを特徴とする請求項８に記載の自己位置認識システム。
【請求項１０】
前記対象物体が通路を形成する通路形成体であることを特徴とする請求項８又は９に記載の自己位置認識システム。
【請求項１１】
前記カメラ画像及び前記モデル画像を二値化処理する手段を備えたことを特徴とする請求項８〜１０のいずれかに記載の自己位置認識システム。
【請求項１２】
前記各一致度から２次又は３次曲線に近似して、前記曲線の極大値に基づいて前記移動体の位置及び方向の双方又は一方を認識する手段を備えたことを特徴とする請求項８〜１１のいずれかに記載の自己位置認識システム。
【請求項１３】
前記移動空間の入出庫管理システムからの情報に基づいて、前記３次元モデルにおける前記対象物体の形状を変更する手段を備えたことを特徴とする請求項８〜１２のいずれかに記載の自己位置認識システム。
【請求項１４】
前記カメラ画像及び前記モデル画像における前記対象物体以外からなる検出領域を抽出する手段と、この各検出領域を比較する手段と、この比較値と所定の閾値とに基づいて前記移動体の方向における障害物を認識する手段とを備えたことを特徴とする請求項８〜１３のいずれかに記載の自己位置認識システム。
【請求項１】
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識手法であって、前記移動空間の３次元モデルを作成するステップと、所定時間における前記移動体の位置及び方向からカメラ画像を取得するステップと、前記所定時間における前記目標軌道上の目標位置及び方向を算出するステップと、前記３次元モデルから前記目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成するステップと、この複数のモデル画像と前記カメラ画像との色情報の一致度を算出するステップと、この各一致度に基づいて、前記移動体の位置及び方向を認識するステップとを含むことを特徴とする自己位置認識手法。
【請求項２】
前記移動空間に対象物体を設置するステップと、前記カメラ画像における前記対象物体の境界垂直線を基準線に規定するステップと、前記カメラ画像における前記基準線の位置を特定するステップと、前記３次元モデルにおける前記基準線の位置を特定するステップと、この特定した各基準線の位置に基づいて、前記３次元モデルから任意の位置及び方向における複数のモデル画像を形成するステップとを含むことを特徴とする請求項１に記載の自己位置認識手法。
【請求項３】
前記対象物体が通路を形成する通路形成体であることを特徴とする請求項１又は２に記載の自己位置認識手法。
【請求項４】
前記カメラ画像及び前記モデル画像を二値化処理するステップを含むことを特徴とする請求項１〜３のいずれかに記載の自己位置認識手法。
【請求項５】
前記各一致度から２次又は３次曲線に近似して、前記曲線の極大値に基づいて前記移動体の位置及び方向の双方又は一方を認識するステップを含むことを特徴とする請求項１〜４のいずれかに記載の自己位置認識手法。
【請求項６】
前記移動空間の入出庫管理システムからの情報に基づいて、前記３次元モデルにおける前記対象物体の形状を変更するステップを含むことを特徴とする請求項１〜５のいずれかに記載の自己位置認識手法。
【請求項７】
前記カメラ画像及び前記モデル画像における前記対象物体以外からなる検出領域を抽出するステップと、この各検出領域を比較するステップと、この比較値と所定の閾値とに基づいて前記移動体の方向における障害物を認識するステップとを含むことを特徴とする請求項１〜６のいずれかに記載の自己位置認識手法。
【請求項８】
予め記憶された目標軌道に沿って移動空間を移動する移動体の自己位置認識システムであって、前記移動空間の３次元モデルを作成する手段と、所定時間における前記移動体の位置及び方向からカメラ画像を取得する手段と、前記所定時間における前記目標軌道上の目標位置及び方向を算出する手段と、前記３次元モデルから前記目標位置及び方向の所定範囲内における複数の位置及び方向のモデル画像を形成する手段と、この複数のモデル画像と前記カメラ画像との色情報の一致度を算出する手段と、この各一致度に基づいて、前記移動体の位置及び方向を認識する手段とを備えたことを特徴とする自己位置認識システム。
【請求項９】
前記移動空間に対象物体を設置する手段と、前記カメラ画像における前記対象物体の境界垂直線を基準線に規定する手段と、前記カメラ画像における前記基準線の位置を特定する手段と、前記３次元モデルにおける前記基準線の位置を特定する手段と、この特定した各基準線の位置に基づいて、前記３次元モデルから任意の位置及び方向における複数のモデル画像を形成する手段とを備えたことを特徴とする請求項８に記載の自己位置認識システム。
【請求項１０】
前記対象物体が通路を形成する通路形成体であることを特徴とする請求項８又は９に記載の自己位置認識システム。
【請求項１１】
前記カメラ画像及び前記モデル画像を二値化処理する手段を備えたことを特徴とする請求項８〜１０のいずれかに記載の自己位置認識システム。
【請求項１２】
前記各一致度から２次又は３次曲線に近似して、前記曲線の極大値に基づいて前記移動体の位置及び方向の双方又は一方を認識する手段を備えたことを特徴とする請求項８〜１１のいずれかに記載の自己位置認識システム。
【請求項１３】
前記移動空間の入出庫管理システムからの情報に基づいて、前記３次元モデルにおける前記対象物体の形状を変更する手段を備えたことを特徴とする請求項８〜１２のいずれかに記載の自己位置認識システム。
【請求項１４】
前記カメラ画像及び前記モデル画像における前記対象物体以外からなる検出領域を抽出する手段と、この各検出領域を比較する手段と、この比較値と所定の閾値とに基づいて前記移動体の方向における障害物を認識する手段とを備えたことを特徴とする請求項８〜１３のいずれかに記載の自己位置認識システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図１２】
【図１３】
【図１４】
【図１６】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１５】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図１２】
【図１３】
【図１４】
【図１６】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１５】
【図１７】
【公開番号】特開２０１０−６６９３４（Ｐ２０１０−６６９３４Ａ）
【公開日】平成２２年３月２５日（２０１０．３．２５）
【国際特許分類】
【出願番号】特願２００８−２３１５９９（Ｐ２００８−２３１５９９）
【出願日】平成２０年９月１０日（２００８．９．１０）
【出願人】（０００２３２８０７）日本輸送機株式会社 (320)
【出願人】（３０７０２０５４５）公立大学法人岡山県立大学 (8)
【Ｆターム（参考）】
【公開日】平成２２年３月２５日（２０１０．３．２５）
【国際特許分類】
【出願日】平成２０年９月１０日（２００８．９．１０）
【出願人】（０００２３２８０７）日本輸送機株式会社 (320)
【出願人】（３０７０２０５４５）公立大学法人岡山県立大学 (8)
【Ｆターム（参考）】
[ Back to top ]