(57)【要約】【課題】  走行中の移動体の揺動にかかわらず、移動体の方向角及び座標位置を迅速且つ高精度に算出する。【解決手段】  移動体に２基のＣＣＤカメラからなる組１４A、１４B、１４C、距離算出手段１８、方向角算出手段２０、座標算出手段２２を搭載し、各組において２基のＣＣＤカメラの光軸間の距離ＤCを、横列７E、７W、７S、７Nの隣接するＬＥＤ間の距離ＤL以下とし、各組１４A、１４B、１４CのＣＣＤカメラによって横列７E、７W、７S、７NのＬＥＤを撮像し、組をなす２基ＣＣＤカメラが撮像した２枚の画像中に共通して存在するＬＥＤとＣＣＤカメラの光軸との間の水平距離を距離算出手段１８が算出し、既知のＬＥＤの座標と幾何学的位置関係から、移動体１の基準線ＣＬが向く方向角θを方向角算出手段２０が算出し、移動体１上の一点Ｒの座標を座標算出手段２２が算出する。【選択図】  図１
2005351836.tif 000002
【特許請求の範囲】
【請求項１】
移動体の座標位置を測定する移動体位置測定装置であって、
互いに異なる第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物の横列を撮像可能であり、互いに平行な光軸を有し、光軸間の距離が横列内で隣接する目標物同士の間隔以上である２基の撮像装置からなる組を備え、
組をなす２基の撮像装置が撮像した同じ方向の２つの撮像データ中に共通して存在する目標物とこの組をなす２基の撮像装置の各光軸との間の水平距離を算出する距離算出手段と、
第１方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第１方向の目標物の座標、第２方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第２方向の目標物の座標、移動体の基準軸と第１方向を撮像する撮像装置の光軸とがなす角度、及び、移動体の基準軸と第２方向を撮像する撮像装置の光軸とがなす角度を用いて、移動体の基準軸が向く方向角を算出する方向角算出手段と、
第３方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第３方向の目標物の座標、移動体の基準軸と第３方向を撮像する撮像装置の光軸とがなす角度、及び、方向角算出手段が算出した方向角を用いて、移動体の座標を算出する座標算出手段と、を有することを特徴とする移動体位置測定装置。
【請求項２】
移動体の座標位置を測定する移動体位置測定装置であって、
互いに異なる第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物の横列のいずれかを撮像可能であり、互いに平行な光軸を有し、光軸間の距離が横列内で隣接する目標物同士の間隔以上である２基の撮像装置からなる組を３組備え、
１組の撮像装置が他の１組の撮像装置に対して１８０°反転した向きを向き、残りの１組の撮像装置が他の撮像装置に対して９０°回転した向きを向くとともに移動体の基準軸と同じ方向を向き、
組をなす２基の撮像装置が撮像した同じ方向の２つの撮像データ中に共通して存在する目標物とこの組をなす２基の撮像装置の各光軸との間の水平距離を算出する距離算出手段と、
第１方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第１方向の目標物の座標、第２方向の目標物について距離算出手段が算出した水平距離、及び、水平距離を算出する対象となった第２方向の目標物の座標を用いて、移動体の基準軸が向く方向角を算出する方向角算出手段と、
第３方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第３方向の目標物の座標、及び、方向角算出手段が算出した方向角を用いて、移動体の座標を算出する座標算出手段と、を有することを特徴とする移動体位置測定装置。
【請求項３】
撮像装置がＣＣＤカメラであり、第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物が発光体であることを特徴とする請求項１又は請求項２に記載の移動体位置測定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己の座標位置を測定する移動体位置測定装置に関する。
【背景技術】
【０００２】
床面上を自律走行するロボット等の移動体が目的位置に正確に到達するためには、移動体が自己の位置を正確に認識する必要がある。移動体が自己の位置を正確に認識する技術として、以下に示す移動体の位置認識装置がある（特許文献１を参照）。
この位置認識装置は、絶対空間において、基準面から略同一の高さに設けられる、位置を識別するための光を発生する光点と、移動体に設けられ、光点からの光により光点の位置を検出する検出手段と、移動体の現在位置を仮想原点とする仮想座標系において、その仮想原点及び検出手段により検出された２つの光点の位置を結んで形成される三角形の外接円の中心と該２つの光点の位置を結ぶ直線との間の距離を求める距離算出手段と、距離算出手段での算出結果を用いて、絶対空間の所定位置を原点とした絶対座標系における外接円の中心座標を求める外接円中心座標算出手段と、外接円中心座標算出手段での算出結果を用いて、絶対座標系における移動体の絶対座標を求める移動体絶対座標算出手段とを備えている。
【０００３】
そして、この移動体が位置を認識する場合、次に述べる位置認識方法を用いる。この位置認識方法は、移動体に設けられた検出手段により、基準面から略同一の高さに設けられた光点からの光により光点の位置を検出するステップと、移動体の現在位置を仮想原点とする仮想座標系において、その仮想原点及び検出手段により検出された２つの光点の位置を結んで形成される三角形の外接円の中心とこれらの２つの光点の位置を結ぶ直線との間の距離を求めるステップ、求めた距離を用いて、絶対空間の所定位置を原点とした絶対座標系における外接円の中心座標を求めるステップ、２つの光点の位置を結ぶ直線と外接円の中心点との間の距離を求めるステップ、求めた距離を用いて、絶対空間の所定位置を原点とした絶対座標系における外接円の中心座標を求めるステップ、求めた中心座標を用いて、絶対座標系における移動体の絶対座標を求めるステップとからなる。これらの各ステップにしたがって、移動体の座標位置が計算される。
【特許文献１】特開２００２−２１３９２０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、移動体の走行面上に凹凸が存在したり、移動体が加減速を行うと、移動体が揺動し、前述の位置認識装置において以下の問題が生じる。
すなわち、移動体の検出手段が、基準面から略同一の高さに設けられた光点の位置を検出し、この検出結果に基づいて一連の計算を行っている。検出手段が光点の位置を検出する際、移動体が揺動していると、検出手段の光軸も揺動する。検出手段の光軸が揺動すると、検出対象の２つの光点を結ぶ直線と検出手段の光軸との交角が変動する。両者の交角が変動すると、検出手段の検出結果（例えば画像）中において、２つの光点間の距離が交角とともに変動し、検出される光点の位置に誤差が含まれることとなる。誤差を含む光点の位置に基づいて移動体の座標位置を計算しても、移動体の座標位置を正しく計算できない。
かかる問題は、移動体が走行中である場合のみならず、移動体が傾いた姿勢等をとって停止している場合にも生じる。
【０００５】
また、検出対象の２つの光点を結ぶ直線と検出手段の光軸との交角を測定する交角測定装置を位置認識装置に組み込み、交角測定装置が測定した交角に基づいて、光点の位置を補正することが考えられる。しかし、位置認識装置とともに補正を行う補正演算手段を位置認識装置に組み込まなければならず、位置認識装置が複雑化し、そのコストも高くなってしまう。加えて、補正を行うための演算時間が必要となり、走行中の移動体の座標位置を迅速に計算することが困難となる。したがって、移動体が正確に走行するためには、緩慢な速度で走行せざるを得ない。
本発明は、上記問題を解決するものであり、その目的とするところは、移動体の揺動や傾斜に影響されることなく、移動体の方向角及び座標位置を迅速且つ高精度に算出可能な移動体位置測定装置を提供することである。
【課題を解決するための手段】
【０００６】
本発明は、その課題を解決するために以下のような構成をとる。請求項１の発明に係る移動体位置測定装置は、移動体の座標位置を測定する移動体位置測定装置であって、互いに異なる第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物の横列を撮像可能であり、互いに平行な光軸を有し、光軸間の距離が横列内で隣接する目標物同士の間隔以上である２基の撮像装置からなる組を備え、組をなす２基の撮像装置が撮像した同じ方向の２つの撮像データ中に共通して存在する目標物とこの組をなす２基の撮像装置の各光軸との間の水平距離を算出する距離算出手段と、第１方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第１方向の目標物の座標、第２方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第２方向の目標物の座標、移動体の基準軸と第１方向を撮像する撮像装置の光軸とがなす角度、及び、移動体の基準軸と第２方向を撮像する撮像装置の光軸とがなす角度を用いて、移動体の基準軸が向く方向角を算出する方向角算出手段と、第３方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第３方向の目標物の座標、移動体の基準軸と第３方向を撮像する撮像装置の光軸とがなす角度、及び、方向角算出手段が算出した方向角を用いて、移動体の座標を算出する座標算出手段と、を有する。
【０００７】
請求項１の発明によれば、組をなす２基の撮像装置の光軸間の距離、及び、２基の撮像装置が撮像した画像等の撮像データ中に共通して存在する目標物の像を用いて、目標物と各光軸との間の水平距離を距離算出手段が幾何学的に高精度に算出する。この距離算出手段の演算において、撮像する目標物の横列の方向と撮像装置の光軸との交角を考慮する必要がなく、両者の交角に基づいて補正を行う必要もない。
【０００８】
目標物の既知の座標、距離算出手段の算出結果、及び、移動体の基準軸と撮像装置の光軸との角度を用いて、移動体の基準軸が向く方向角を方向角算出手段が幾何学的に高精度に算出する。方向角算出手段の演算において、撮像装置の光軸と撮像する目標物の横列の方向との交角を考慮する必要がなく、両者の交角に基づいて補正を行う必要もない。
目標物の既知の座標、距離算出手段の算出結果、移動体の基準軸と撮像装置の光軸との角度、及び、方向角算出手段の算出結果を用いて、移動体の座標を座標算出手段が幾何学的に高精度に算出する。座標算出手段の演算において、撮像装置の光軸と撮像する目標物の横列の方向との交角を考慮する必要がなく、両者の交角に基づいて補正を行う必要もない。
【０００９】
撮像装置が撮像する目標物の横列の方向と撮像装置の光軸とが直交していない場合であっても、演算して誤差を補正する必要がなく、移動体の座標を迅速に算出でき、走行中の移動体の座標を算出するために発生するタイムラグが非常に小さくなる。
なお、撮像装置の組が、第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物の横列を撮像するにあたっては、１組の撮像装置によって、３つの方向を順次撮像してもよいし、複数組の撮像装置によって、３つの方向をそれぞれ撮像してもよい。
【００１０】
請求項２の発明に係る移動体位置測定装置は、移動体の座標位置を測定する移動体位置測定装置であって、互いに異なる第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物の横列のいずれかを撮像可能であり、互いに平行な光軸を有し、光軸間の距離が横列内で隣接する目標物同士の間隔以上である２基の撮像装置からなる組を３組備え、１組の撮像装置が他の１組の撮像装置に対して１８０°反転した向きを向き、残りの１組の撮像装置が他の撮像装置に対して９０°回転した向きを向くとともに移動体の基準軸と同じ方向を向き、組をなす２基の撮像装置が撮像した同じ方向の２つの撮像データ中に共通して存在する目標物とこの組をなす２基の撮像装置の各光軸との間の水平距離を算出する距離算出手段と、第１方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第１方向の目標物の座標、第２方向の目標物について距離算出手段が算出した水平距離、及び、水平距離を算出する対象となった第２方向の目標物の座標を用いて、移動体の基準軸が向く方向角を算出する方向角算出手段と、第３方向の目標物について距離算出手段が算出した水平距離、水平距離を算出する対象となった第３方向の目標物の座標、及び、方向角算出手段が算出した方向角を用いて、移動体の座標を算出する座標算出手段と、を有する。
【００１１】
請求項２の発明によれば、３組の撮像装置が、第１方向、第２方向及び第３方向をそれぞれ同時に撮像できるので、走行中の移動体の座標を算出するために必要なデータを瞬時に取得でき、走行中の移動体の座標の算出に要する演算時間がより短くなり、タイムラグを殆ど生じることなく走行中の移動体の座標が算出される。
各撮像装置の光軸と移動体の基準軸とがなす角度が固定され、３組の撮像装置の各光軸間の角度もそれぞれ固定されており、既知となっている。したがって、これらの角度を測定する必要がなく、これらの角度の測定に起因する誤差が、距離算出手段、方向角算出手段及び座標算出手段の演算に入り込む余地がなく、移動体の座標が算出結果の精度が向上する。
【００１２】
請求項３の発明に係る移動体位置測定装置は、請求項１又は請求項２に記載の移動体位置測定装置であって、撮像装置がＣＣＤカメラであり、第１方向、第２方向及び第３方向にそれぞれ存在する既知の目標物が発光体である。
請求項３の発明によれば、ＣＣＤカメラが発光体を撮像するので、撮像データ中において発光体の位置を輝度や明度の変化から正確に特定でき、距離算出手段、方向角算出手段及び座標算出手段の演算において、誤差が入り込む余地が少なくなり、移動体の座標が算出結果の精度が向上する。
また、複数の発光体において、それぞれ、発光色、明度、輝度等を変えることによって、ＣＣＤカメラに撮像された発光体を容易に特定可能である。
【発明の効果】
【００１３】
本発明は、上記のような移動体位置測定装置であるので、移動体の揺動や傾斜に影響されることなく、移動体の方向角及び座標位置を迅速且つ高精度に算出可能である。
【発明を実施するための最良の形態】
【００１４】
本発明を実施するための最良の形態を図１から図５を参照しつつ説明する。図１は床面上を走行する移動体の上面図、図２は距離算出手段における演算の説明図、図３は組をなすＣＣＤカメラが撮像した画像の説明図、図４は方向角算出手段における演算の説明図、図５は座標算出手段における演算の説明図である。
屋内に図１に示す四角形の床面１があり、床面１は４面の壁３E、３W、３S、３Nによって囲まれている。壁３Eが東側（図１の右側）に位置し、壁３Wが西側（図１の左側）に位置し、壁３Sが南側（図１の下側）に位置し、壁３Nが北側（図１の上側）に位置している。
【００１５】
壁３E上の床面１から所定の高さ位置に、複数個のＬＥＤ５Em（ｍ＝１、２、３、・・・）が一列に南方から北方に並んで横列７Eを形成している。壁３W上の横列７Eと同じ高さ位置に、複数個のＬＥＤ５Wn（ｎ＝１、２、３、・・・）が一列に南方から北方に並んで横列７Wを形成している。壁３Wと同様、壁３S上に複数個のＬＥＤ５Sp（ｐ＝１、２、３、・・・）が一列に西方から東方に並んで横列７Sを形成し、壁３N上に複数個のＬＥＤ５Nq（ｑ＝１、２、３、・・・）が一列に西方から東方に並んで横列７Nを形成している。
【００１６】
横列７E、７W、７S、７Nにおいて、隣接するＬＥＤ同士の距離はＤLとなっている。地表に固定された絶対座標系において、各ＬＥＤ５Em、５Wn、５Sp、５Nqの座標は既知である。なお、この絶対座標系において、Ｘ軸が東西方向を向き、Ｙ軸が南北方向を向いている。また、各ＬＥＤ５Em、５Wn、５Sp、５Nqは、それぞれ、色、輝度、明度が異なり、識別可能となっている。
図１に示す移動体１０は立方体をなし、２対の駆動輪１１を有して床面１上を自律走行可能に構成されている。移動体１０は基準軸ＣＬを有し、移動体１０の前方への直進方向が基準軸ＣＬに一致している。
【００１７】
移動体１０の上面には６基のＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2が搭載されている。ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2はそれぞれ同じ構成のＣＣＤカメラであり、レンズ中心から像を結ぶＣＣＤ素子までの焦点距離がｆとなっている。床面１から各ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2までの高さは、床面１から横列７N、７E、７S、７Wの各ＬＥＤまでの高さとほぼ一致しており、ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2の各光軸１６A1、１６A2、１６B1、１６B2、１６C1、１６C2は同一面内に存在している。ＣＣＤカメラ１２A1、１２A2が組１４Aを形成し、ＣＣＤカメラ１２B1、１２B2が組１４Bを形成し、ＣＣＤカメラ１２C1、１２C2が組１４Cを形成している。
【００１８】
移動体１０を上方から見ると、組１４AのＣＣＤカメラ１２A1、１２A2が基準軸ＣＬから時計回りに９０°回転した向きを向いており、ＣＣＤカメラ１２A1、１２A2の各光軸１６A1、１６A2は互い平行であり、光軸１６A1と光軸１６A2との距離はＤCとなっている。なお、距離ＤCと距離ＤLとの間にはＤC≧ＤLの関係が成立している。
同様に、移動体１０を上方から見ると、組１４BのＣＣＤカメラ１２B1、１２B2が基準軸ＣＬから反時計回りに９０°回転した向きを向いており、組１４CのＣＣＤカメラ１２C1、１２C2が基準軸ＣＬと同じ方向を向いている。ＣＣＤカメラ１２B1、１２B2の光軸１６B1、１６B2は互い平行であり、ＣＣＤカメラ１２C1、１２C2の光軸１６C1、１６C2も互い平行である。光軸１６B1、１６B2間の距離と、光軸１６C1、１６C2間の距離は、それぞれＤCである。また、ＣＣＤカメラ１２A1の光軸１６A1とＣＣＤカメラ１２B2の光軸１６B2とが互いに同一直線上にあり、ＣＣＤカメラ１２A2の光軸１６A2とＣＣＤカメラ１２B1の光軸１６B1とが互いに同一直線上にある。
【００１９】
移動体１０には、距離算出手段１８、方向角算出手段２０、座標算出手段２２及びメモリ２４が搭載されており、距離算出手段１８はプログラムＰ１を有し、方向角算出手段２０はプログラムＰ２を有し、座標算出手段２２はプログラムＰ３を有し、メモリ２４には各ＬＥＤ５Em、５Wn、５Sp、５Nqの座標データ及び色、輝度、明度のデータが記憶されている。
【００２０】
次に、距離算出手段１８のプログラムＰ１、方向角算出手段２０のプログラムＰ２、座標算出手段２２のプログラムＰ３が行う演算について説明する。以下の説明において、組１４AのＣＣＤカメラ１２A1、１２A2が、壁３Eの方向を向いて横列７EのうちからＬＥＤ５Emを撮像し、組１４BのＣＣＤカメラ１２B1、１２B2が、壁３Wの方向を向いて横列７WのうちからＬＥＤ５Wnを撮像し、組１４CのＣＣＤカメラ１２C1、１２C2が、壁３Nの方向を向いて横列７NのうちからＬＥＤ５Nqを撮像するものとする。
距離算出手段１８のプログラムＰ１は、ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2が撮像した画像を用いて、以下に述べる手順で演算を行うプログラムである（図２及び図３を参照）。
【００２１】
ＣＣＤカメラ１２A1、１２A2の光軸１６A1、１６A2間の距離がＤCであり、横列７Eの隣接するＬＥＤ間の距離がＤLであり、ＤC≧ＤLの関係が成立している。したがって、ＣＣＤカメラ１２A1、１２A2が、壁３Eの方向を向いて横列７EのＬＥＤを撮像すると、光軸１６A1、１６A2間に少なくとも１個のＬＥＤ（今の場合、ＬＥＤ５Em）が存在し、ＣＣＤカメラ１２A1、１２A2の各視野に入る。ＣＣＤカメラ１２A1がＬＥＤ５Emを画像２６A1中に撮像し、ＣＣＤカメラ１２A2がＬＥＤ５Emを画像２６A2中に撮像する。
【００２２】
プログラムＰ１が、画像２６A1、２６A2に撮像されている各ＬＥＤの色、輝度、明度を測定し、測定結果をメモリ２４のデータと照合し、撮像されている各ＬＥＤをそれぞれ識別し、画像２６A1、２６A2に共通して撮像されているＬＥＤとしてＬＥＤ５Emを特定する。
ＬＥＤ５Emが特定されたら、プログラムＰ１が、画像２６A1中で、画像２６A1の中心位置ＣA1とＬＥＤ５Emとの間の水平距離αEm1を測定する。同様に、プログラムＰ１が、画像２６A2中で、画像２６A2の中心位置ＣA2とＬＥＤ５Emとの間の水平距離αEm2を測定する。
【００２３】
ＬＥＤ５EmとＣＣＤカメラ１２A1の光軸１６A1との距離をβEm1、ＬＥＤ５EmとＣＣＤカメラ１２A2の光軸１６A2との距離をβEm2とすると、幾何学的にβEm1、βEm2が次式（１）、（２）、（３）により表される。
βEm1＝αEm1×（ｆEm／ｆ）・・・（１）
βEm2＝αEm2×（ｆEm／ｆ）・・・（２）
ＤC＝βEm1＋βEm2・・・（３）
なお、ｆEmは、ＬＥＤ５Emを通り光軸１６A1、１６A2と直交する直線Ｌと、ＣＣＤカメラ１２A1のレンズ中心との距離であり、直線ＬとＣＣＤカメラ１２A2のレンズ中心との距離でもある。
【００２４】
式（１）、（２）、（３）を変形すると、次式（４）、（５）が得られる。
βEm1＝ＤC×αEm1／（αEm1＋αEm2）・・・（４）
βEm2＝ＤC×αEm2／（αEm1＋αEm2）・・・（５）
プログラムＰ１が、測定されたαEm1、αEm2、既知のＤCをそれぞれ式（４）、（５）に代入し、βEm1、βEm2を算出する。
【００２５】
同様に、プログラムＰ１が、組１４BのＣＣＤカメラ１２B1、１２B2が撮像した画像を用いて、ＬＥＤ５WnとＣＣＤカメラ１２B1の光軸１６B1との距離βWn1、ＬＥＤ５WnとＣＣＤカメラ１２B2の光軸１６B2との距離βWn2を算出する。プログラムＰ１が、組１４CのＣＣＤカメラ１２C1、１２C2が撮像した画像を用いて、ＬＥＤ５NqとＣＣＤカメラ１２C1の光軸１６C1との距離βNq1、ＬＥＤ５NqとＣＣＤカメラ１２C2の光軸１６C2との距離βNq2を算出する。
【００２６】
プログラムＰ１においては、ＣＣＤカメラ１２A1の光軸１６A1とＣＣＤカメラ１２A2の光軸１６A2が壁３Eに対して垂直であるか否か、ＣＣＤカメラ１２B1の光軸１６B1とＣＣＤカメラ１２B2の光軸１６B2が壁３Wに対して垂直であるか否か、ＣＣＤカメラ１２C1の光軸１６C1とＣＣＤカメラ１２C2の光軸１６C2が壁３Nに対して垂直であるか否かが、演算の誤差の要因とはならない。したがって、プログラムＰ１において、ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2の各画像についての補正計算を行う必要がなく、プログラムＰ１がβEm1、βEm2、βWn1、βWn2、βNq1、βNq2を高精度に算出する。
【００２７】
方向角算出手段２０のプログラムＰ２は、距離算出手段１８のプログラムＰ１の演算結果を用いて、以下に述べる手順で演算を行うプログラムである（図４を参照）。
まず、プログラムＰ２が、プログラムＰ１によって特定されたＬＥＤ５Emの座標（ＸEm、ＹEm）と、ＬＥＤ５Wnの座標（ＸWn、ＹWn）をメモリ２４から読み出す。
【００２８】
ＣＣＤカメラ１２A1の光軸１６A1とＣＣＤカメラ１２B2の光軸１６B2とが互いに同一直線上にあり、光軸１６A1及び光軸１６B2がなす直線は、ＬＥＤ５Emを中心とする半径βEm1の円とＬＥＤ５Wnを中心とする半径βWn2の円の共通接線である。絶対座標系において、光軸１６A1及び光軸１６B2がなす直線の傾きθは、幾何学的に次式（６）によって表される。なお、θは反時計回り方向が正である。
θ＝ｓｉｎ-1〔（βEm1−βWn2）／｛（ＸEm−ＸWn）2＋（ＹEm−ＹWn）2｝1/2〕＋ｔａｎ-1｛（ＹEm−ＹWn）／（ＸEm−ＸWn）｝・・・（６）
【００２９】
プログラムＰ２が、プログラムＰ１によって算出されたβEm1、βWn2、メモリ２４から読み出したＸEm、ＹEm、ＸWn、ＹWnを式（６）に代入してθを算出する。幾何学的に、光軸１６A1及び光軸１６B2がなす直線の傾きθは、基準軸ＣＬが絶対座標系のＹ軸に対して傾斜する方向角と等しいので、絶対座標系における移動体１０の進行方向がわかる。
【００３０】
プログラムＰ２の演算においては、ＣＣＤカメラ１２A1の光軸１６A1とＣＣＤカメラ１２A2の光軸１６A2が壁３Eに対して垂直であるか否か、ＣＣＤカメラ１２B1の光軸１６B1とＣＣＤカメラ１２B2の光軸１６B2が壁３Wに対して垂直であるか否か、ＣＣＤカメラ１２C1の光軸１６C1とＣＣＤカメラ１２C2の光軸１６C2が壁３Nに対して垂直であるか否かが、演算の誤差の要因とはならいので、プログラムＰ２において、ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2の各画像についての補正計算を行う必要がない。プログラムＰ２が、プログラムＰ１による高精度の演算結果を用いて、θを高精度に算出する。
【００３１】
座標算出手段２２のプログラムＰ３は、距離算出手段１８のプログラムＰ１の演算結果と方向角算出手段２０のプログラムＰ２の演算結果を用いて、以下に述べる手順で演算を行うプログラムである（図５を参照）。
ＣＣＤカメラ１２B2の光軸１６B2と壁３Wとの交差点２８WCの座標（ＸWC、ＹWC）は、次式（７）、（８）によって表される。
ＸWC＝ＸWn・・・（７）
ＹWC＝ＹWn＋βWn2×ｓｅｃθ・・・（８）
【００３２】
同様に、ＣＣＤカメラ１２C2の光軸１６C2と壁３Nとの交差点２８NCの座標（ＸNC、ＹNC）は、次式（９）、（１０）によって表される。
ＸNC＝ＸNq＋βNq2×ｓｅｃθ・・・（９）
ＹNC＝ＹNq・・・（１０）
したがって、ＣＣＤカメラ１２B2の光軸１６B2は次式（１１）によって表され、ＣＣＤカメラ１２C2の光軸１６C2は、次式（１２）によって表される。
Ｙ＝（Ｘ−ＸWC）×ｔａｎθ＋ＹWC・・・（１１）
Ｙ＝−（Ｘ−ＸNC）×ｃｏｔθ＋ＹNC・・・（１２）
【００３３】
式（１１）と式（１２）を連立して解くと、ＣＣＤカメラ１２B2の光軸１６B2の延長線とＣＣＤカメラ１２C2の光軸１６C2の延長線とが交わる点Ｒ（ＸR、ＹR）の座標が得られる。点Ｒ（ＸR、ＹR）は次式（１３）、（１４）によって表される。
ＸR＝ＸWC×ｓｉｎ2θ＋ＸNC×ｃｏｓ2θ−（ＹWC−ＹNC）×ｓｉｎθ×ｃｏｓθ・・・（１３）
ＹR＝ＹWC×ｃｏｓ2θ＋ＹNC×ｓｉｎ2θ−（ＸWC−ＸNC）×ｓｉｎθ×ｃｏｓθ・・・（１４）
【００３４】
式（１３）、（１４）に式（７）、（８）、（９）、（１０）を代入すれば、次式（１５）、（１６）が得られる。
ＸR＝ＸWn×ｓｉｎ2θ＋ＸNq×ｃｏｓ2θ−βWn2×ｓｉｎθ＋βNq2×ｃｏｓθ＋（ＹNq−ＹWn）×ｓｉｎθ×ｃｏｓθ・・・（１５）
ＹR＝ＹWn×ｃｏｓ2θ＋ＹNq×ｓｉｎ2θ＋βWn2×ｃｏｓθ＋βNq2×ｓｉｎθ＋（ＸNq−ＸWn）×ｓｉｎθ×ｃｏｓθ・・・（１６）
式（１５）、（１６）において、ＸWn、ＸNq、ＹWn、ＹNqはメモリ２４から読み取った既知の値であり、θの値はプログラムＰ２で算出されており、βWn2、βNq2の値はプログラムＰ１で算出されているので、点Ｒ（ＸR、ＹR）が既知の値によって表される。
【００３５】
点Ｒ（ＸR、ＹR）は、ＣＣＤカメラ１２B2の光軸１６B2の延長線とＣＣＤカメラ１２C2の光軸１６C2の延長線の交点であり、移動体１０上の一点の位置を表すので、点Ｒ（ＸR、ＹR）によって絶対座標系における移動体１０の位置を表すことができる。
プログラムＰ３が、プログラムＰ１によって算出されたβWn2、βNq2、プログラムＰ２によって算出されたθ、メモリ２４から読み出したＸWn、ＸNq、ＹWn、ＹNqを式（１５）、（１６）に代入してＸR、ＹRを算出する。
【００３６】
プログラムＰ３の演算においては、ＣＣＤカメラ１２A1の光軸１６A1とＣＣＤカメラ１２A2の光軸１６A2が壁３Eに対して垂直であるか否か、ＣＣＤカメラ１２B1の光軸１６B1とＣＣＤカメラ１２B2の光軸１６B2が壁３Wに対して垂直であるか否か、ＣＣＤカメラ１２C1の光軸１６C1とＣＣＤカメラ１２C2の光軸１６C2が壁３Nに対して垂直であるか否かが、演算の誤差の要因とはならないので、プログラムＰ３において、ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2の各画像についての補正計算を行う必要がない。プログラムＰ３が、プログラムＰ１、Ｐ２による高精度の演算結果を用いて、移動体１０の位置を表す点Ｒの座標（ＸR、ＹR）を高精度に算出する。
【００３７】
したがって、床面１の凹凸や移動体１０の加減速によって、走行中の移動体１０が揺動しても、組１４AのＣＣＤカメラ１２A1、１２A2、組１４BのＣＣＤカメラ１２B1、１２B2、組１４CのＣＣＤカメラ１２C1、１２C2を用いて、横列７N、７E、７S、７Wのうち３つの横列のＬＥＤを撮像すれば、絶対座標系における移動体１０の位置の座標とその進行方向を高精度に算出できる。
【００３８】
また、走行中の移動体１０が揺動しても、ＣＣＤカメラ１２A1、１２A2、１２B1、１２B2、１２C1、１２C2によって撮像された画像に補正を加える必要がないので、移動体１０の位置を即座に算出でき、移動体１０は走行速度や走行方向を迅速且つ正確に制御可能となる。さらに、移動体１０が床面１の凹凸上で停止している場合、移動体１０が傾斜しているか否かにかかわらず、移動体１０の位置を即座に算出できる。
【００３９】
なお、本実施の形態において、移動体１０が組１４AのＣＣＤカメラ１２A1、１２A2、組１４BのＣＣＤカメラ１２B1、組１４Cの１２B2、ＣＣＤカメラ１２C1、１２C2をそれぞれ搭載しているが、代わりに、移動体１０が、旋回可能に構成された組１４AのＣＣＤカメラ１２A1、１２A2と角度検出装置とを搭載し、角度検出装置がＣＣＤカメラ１２A1、１２A2の光軸１６A1、１６A2と移動体１０の基準軸ＣＬとの交角を検出する構成としてもよい。
【００４０】
この場合、組１４AのＣＣＤカメラ１２A1、１２A2が旋回して、例えば、横列７E、７N、７Wを順次撮像するとともに、角度検出装置が各撮像時に光軸１６A1、１６A2と移動体１０の基準軸ＣＬとの角度を検出する。そして、方向角算出手段２０が、横列７E及び７Wの各撮像時に角度検出装置が検出した角度、ＬＥＤ５Emの座標（ＸEm、ＹEm）、ＬＥＤ５Wnの座標（ＸWn、ＹWn）、βEm1、βEm2、βWn1、βWn2を用いて、基準軸ＣＬが絶対座標系のＹ軸に対して傾斜する方向角θを幾何学的に算出する。さらに、座標算出手段２２が、横列７Nの撮像時に角度検出装置が検出した角度、ＬＥＤ５Nqの座標（ＸNq、ＹNq）、βNq1、βNq2、基準軸ＣＬの方向角を用いて、移動体１０の位置を表す点Ｒの座標（ＸR、ＹR）を幾何学的に算出する。
【図面の簡単な説明】
【００４１】
【図１】床面上を走行する移動体の上面図である。
【図２】距離算出手段における演算の説明図である。
【図３】組をなすＣＣＤカメラが撮像した画像の説明図である。
【図４】方向角算出手段における演算の説明図である。
【図５】座標算出手段における演算の説明図である。
【符号の説明】
【００４２】
１    床面
３E、３W、３S、３N    壁
５Em、５Wn、５Sp、５Nq（ｍ、ｎ、ｐ、ｑは自然数）    ＬＥＤ
７E、７W、７S、７N    横列
１０    移動体
１１    駆動輪
１２A1、１２A2、１２B1、１２B2、１２C1、１２C2    ＣＣＤカメラ
１４A、１４B、１４C    組
１６A1、１６A2、１６B1、１６B2、１６C1、１６C2    光軸
１８    距離算出手段
２０    方向角算出手段
２２    座標算出手段
２４    メモリ
２６A1、２６A2    画像
【図１】
2005351836.tif 000003
【図２】
2005351836.tif 000004
【図３】
2005351836.tif 000005
【図４】
2005351836.tif 000006
【図５】
2005351836.tif 000007