JP4755886B2

JP4755886B2 - 位置検出システムおよび位置検出方法

Info

Publication number: JP4755886B2
Application number: JP2005327679A
Authority: JP
Inventors: 亜紀子沼田; 雅仁佐野
Original assignee: Toshiba TEC Corp
Current assignee: Toshiba TEC Corp
Priority date: 2005-11-11
Filing date: 2005-11-11
Publication date: 2011-08-24
Anticipated expiration: 2025-11-11
Also published as: JP2007132853A

Description

この発明は、移動体の位置を検出する位置検出システムおよび位置検出方法に関する。

従来、移動体の位置を検出する方法として、三角測量が知られている。
三角測量により移動体の位置を検出するシステムの例として、少なくとも３つの指標を設け、これら指標と移動体とを結ぶ各方向線の相互間の角度（指標が３つであれば３つの角度）を測定し、測定した各角度のうち２つを選択し、選択した２つの角度および各指標の位置を用いた演算により、移動体の位置を検出するものがある（例えば、特許文献１）。
特開２００１−３３２４４号公報

上記システムでは、各指標と移動体とを結ぶ各方向線の相互間の角度が測定されるが、この測定角度に誤差が含まれていれば、その誤差が位置の検出に悪影響を及ぼしてしまう。

この発明は、上記事情を考慮したもので、たとえ測定角度に誤差が含まれていても、その誤差の影響を最小限に抑えて、移動体の位置を的確に検出することが可能な信頼性にすぐれた位置検出システムおよび位置検出方法を提供することを目的とする。

請求項１に係る発明の位置検出システムは、移動体の位置検出用として３つ以上の指標を備えたものであって、各指標のうち３つの指標と移動体とを結ぶ各方向線の相互間の角度を測定する測定手段と、各指標の位置と上記測定手段で測定される角度から、移動体の位置のＸ座標ＰｘおよびＹ座標Ｐｙを演算する演算手段と、上記測定手段の測定角度のうち、それぞれ２つの測定角度α，βからなる複数の組合せを選定する選定手段と、この選定手段で選定される組合せごとに、それぞれの測定角度α，βの変化に対する、上記演算手段で演算されるＸ座標ＰｘおよびＹ座標Ｐｙの変化率Δｘ，Δｙを、推定する推定手段と、この推定手段で推定される変化率Δｘ，Δｙに対し、上記測定角度αの実際値がΘ、上記測定角度βの実際値がΨで、測定角度α，βが測定誤差ｋを含む場合の実際の変化率Δｘ _１，Δｙ _１を算出する第１算出手段と、上記推定手段で推定される変化率Δｘ，Δｙに対し、上記測定角度αの実際値がΘ、上記測定角度βの実際値がΨで、測定角度α，βが測定誤差を含まない場合の実際の変化率Δｘ _２，Δｙ _２を算出する第２算出手段と、を備えている。そして、上記演算手段は、上記選定手段で選定される各組合せの測定角度α，βのうち、上記第１算出手段で算出される実際の変化率Δｘ _１と上記第２算出手段で算出される実際の変化率Δｘ _２との差（＝｜Δｘ _１｜−｜Δｘ _２｜）が最小となる組合せの測定角度α，βを用いて、移動体の位置のＸ座標Ｐｘを演算するとともに、上記第１算出手段で算出される実際の変化率Δｙ _１と上記第２算出手段で算出される実際の変化率Δｙ _２との差（＝｜Δｙ _１｜−｜Δｙ _２｜）が最小となる組合せの測定角度α，βを用いて、移動体の位置のＹ座標Ｐｙを算出する。

この発明の位置検出システムおよび位置検出方法によれば、測定角度に誤差が含まれていても、その誤差の影響を最小限に抑えて、移動体の位置を的確に検出することができる。これにより、位置検出の信頼性が大幅に向上する。

以下、この発明の一実施形態について図面を参照して説明する。
図１に示すように、１は大型商店などの建物で、床、壁、天井で覆われている。この建物１の床面に移動体２が移動自在に載置されている。そして、建物１の内壁上部あるいは天井に、少なくとも３つ以上の指標として６個の光学ビーコン♯０〜♯６が分散して取付けられている。これら光学ビーコン♯０〜♯６は、発光素子として赤外線光を発する発光ダイオードを用いており、取付け位置（平面上のＸ，Ｙ座標）については施設時に既知となっている。

光学ビーコン♯０〜♯６から発せられる赤外線光は、壁面に取付けられている場合に平面図上で水平方向が最大１８０度（角部に取付けのものは９０度または２７０度）の範囲で上下左右方向に拡がり、天井に取付けられている場合に平面図上で最大３６０度の範囲で下方向に拡がる。

とくに、光学ビーコン♯０〜♯６は、自己以外の少なくとも１つの光学ビーコンから発せられる光の到達領域に存している。

これら光学ビーコン♯０〜♯６には、符号順にそのまま対応する発光順序が予め定められている。発光順序が１番目の光学ビーコン♯０は、基準光学ビーコンとして、定期的に発光動作する。この光学ビーコン♯０の制御回路は、図２に示すように、発光パターン生成部１０、タイマ１１、ＩＤ設定部１２、および発光素子（発光ダイオード）１３により構成されている。このうち、発光パターン生成部１０およびタイマ１１により、発光制御部が構成されている。
タイマ１１は、光学ビーコン♯０の周期的な発光動作を設定するための一定時間ｔ１をカウントする。ＩＤ設定部１２は、光学ビーコン♯０に固有の識別情報いわゆるＩＤを人為的な操作により可変設定するためのものである。

発光パターン生成部１０は、図３のタイムチャートに示すように、タイマ１１が一定時間ｔ１をカウントするごとに、所定周波数のキャリア信号を変調し、その変調信号（パルス信号）により、特定の発光パターンを有する開始コードを生成し、続いてＩＤ設定部１２の設定に応じたＩＤコードを生成し、生成した開始信号およびＩＤコードに応じて発光素子１３を発光させする。開始信号の立上がりからＩＤコードの終了までに一定時間ｔａが確保される。ＩＤコードは、一定時間ｔｂに収まる３ビットの２進数値であり、光学ビーコン♯０用として２進数値“０”が設定されている。一定時間ｔａ，ｔｂについては、発光パターン生成部１０内のクロック信号のカウントにより設定される。

残りの光学ビーコン♯１〜♯６は、従動光学ビーコンとして、自己以外の光学ビーコンから発せられる光のうち、予め定められている発光順序が１つ前の光学ビーコンから発せられる光をその光に含まれているＩＤコードから判別し、その判別した光を受けることにより順次に動作して発光する。

光学ビーコン♯１〜♯６の制御回路は互いに同一のもので、そのうちの光学ビーコン♯１の制御回路を代表して図４に示している。すなわち、自己以外の光学ビーコンから発せられる光を受ける受光素子（フォトダイオード）２０、この受光素子２０の受光信号に含まれている開始信号およびＩＤコードを判別する信号判別部２１、この信号判別部２１で開始信号およびＩＤコードが判別された場合に同判別されたＩＤコードとＩＤメモリ２３内の特定ＩＤコード（発光順序が１つ前の光学ビーコンのＩＤコード）とを比較する比較部２２が設けられている。この比較部２２の比較結果が、発光パターン生成部２４に供給される。そして、発光パターン生成部２４に、タイマ２５、ＩＤ設定部２６、発光素子（発光ダイオード）２７が接続されている。
タイマ２５は、比較部２２の比較結果が一致の場合に、発光動作開始までの一定時間ｔ２をカウントするために用意されている。ＩＤ設定部２６は、光学ビーコン♯１に固有の識別情報であるＩＤを人為的な操作により可変設定するためのものである。

発光パターン生成部２４は、図２に示しているように、比較部２２の比較結果が一致となった場合にタイマ２５を動作させ、そのタイマ２５による一定時間ｔ２のカウントが終了した後、所定周波数のキャリア信号を変調し、その変調信号（パルス信号）により、特定の発光パターンを有する開始信号を生成し、続いてＩＤ設定部２６の設定に応じたＩＤコードを生成し、生成した開始信号およびＩＤコードに応じて発光素子２７を発光させる。開始信号の立上がりからＩＤコードの終了までに一定時間ｔａが確保される。ＩＤコードは、一定時間ｔｂに収まる３ビットの２進数値であり、光学ビーコン♯１用として２進数値“１”が設定されている。一定時間ｔａ，ｔｂについては、発光パターン生成部２４内のクロック信号のカウントにより設定される。

上記信号判別部２１、比較部２２、ＩＤメモリ２３、発光パターン生成部２４、およびタイマ２５により、発光制御部が構成されている。

なお、他の光学ビーコン♯２〜♯６のＩＤコードとして、２進数値“２”“３”“４”“５”“６”がそれぞれ設定されている。光学ビーコン♯０〜♯６のＩＤコード、および光学ビーコン♯０〜♯６のＩＤメモリ２３に記憶される特定ＩＤコード（発光順序が１つ前の光学ビーコンのＩＤコード）の関係を、図５に示している。

こうして、光学ビーコン♯０が一定時間ｔ１ごとに発光動作して開始信号およびＩＤコードを発する。光学ビーコン♯１は、他の光学ビーコンから受ける光のＩＤコードを監視し、発光順序が１つ前の光学ビーコン♯０の特定ＩＤコードである場合に、それから一定時間ｔ２後に発光動作して開始信号およびＩＤコードを発する。光学ビーコン♯２は、他の光学ビーコンから受ける光のＩＤコードを監視し、発光順序が１つ前の光学ビーコン♯１の特定ＩＤコードである場合に、それから一定時間ｔ２後に発光動作して開始信号およびＩＤコードを発する。以後、同様に、光学ビーコン♯３〜♯６が発光動作する。全ての光学ビーコン♯０〜♯６の発光動作が終了するタイミングは、上記一定時間ｔ１のカウントが終了するタイミングの前である。これにより、一定時間ｔ１ごとに、光学ビーコン♯０〜♯６の順繰りの発光動作が繰返される。

一方、移動体２は、光学ビーコン♯０〜♯６から入射する光の入射角度を算出するとともに、その各入射光に含まれているＩＤコードに対応する光学ビーコン♯０〜♯６の位置を判別し、これら算出結果および判別結果に応じて自己の位置を検出する機能を有するもので、図６の制御回路に示すように、検出手段として、受光部３０、光点位置計測部３３、入射角度計算部３４、自己位置演算部３５、コード検知部３６、位置データメモリ３７を有している。

受光部３０は、光学ビーコン♯０〜♯６から到達して入射部３１に入射する光を、二次元光学センサ３２に集光する。入射部３１は、図７に示すように、絞り板３１ａを有し、その絞り板３１ａの開口（絞り）に入射する光をレンズ３１ｂにより二次元光学センサ３２に集光して、二次元光学センサ３２の上面に集光点を形成する。光点位置計測部３３は、二次元光学センサ３２における各集光点を計測する。入射角度計算部３４は、光点位置計測部３３で計測される各集光点と二次元光学センサ３２の上面の中心位置に立つ中心軸との間の距離に基づいて、上記入射部３１への各入射光の入射角度（各入射光の光源方向）を計算する。

二次元光学センサ３２としては、例えばＰＳＤ（Position Sensitive Detector）と称される位置センサが採用される。この位置センサは、フォトダイオードの表面抵抗を利用して集光点の受光強度の重心位置を検知するもので、その重心位置に応じた電圧レベルの信号を出力する。すなわち、集光点の受光強度の重心位置のＸ，Ｙ座標がＸｃ，Ｙｃであれば、電圧レベルＶＸｃ，ＶＹｃの信号が位置センサから出力される。そして、この重心位置Ｘｃ，Ｙｃを用いた下式で得られる角度の方向に、発光元の光学ビーコンが存在することが分かる。
tan^−１(Yc/Xc)±π
コード検知部３６は、上記入射部３１への各入射光に含まれている開始コードおよびＩＤコードを上記二次元光学センサ３２の出力により検知する。位置データメモリ３７は、光学ビーコン♯０〜♯６の位置データをその光学ビーコン♯０〜♯６のＩＤコードに対応付けて記憶している。

自己位置演算部３５は、主要な機能として次の（１）〜（４）を有している。
（１）入射角度計算部３４の計算結果である各入射光の入射角度のうち３つを適宜に選択し、選択した３つの入射角度に基づき、図８のように３つの光学ビーコンＡ，Ｂ，Ｃ（光学ビーコン♯０〜♯６のいずれか３つ）と移動体２とを結ぶ３本の方向線Ｌａ，Ｌｂ，Ｌｃを認識し、認識した方向線Ｌａ，Ｌｂ，Ｌｃの相互間の角度（＝θ_１，θ_２，θ_３）を測定する測定手段。

（２）上記光学ビーコンＡ，Ｂ，Ｃの位置と上記測定手段で測定される角度（＝θ_１，θ_２，θ_３）から、移動体２の位置のＸ座標ＰｘおよびＹ座標Ｐｙを演算する演算手段。

（３）上記測定手段で測定される測定角度のうち、それぞれ２つの測定角度α，β（＝［θ_１，θ_２］［θ_２，θ_３］［θ_１，θ_３］）からなる複数の組合せを選定する選定手段。

（３）コード検知部３６で検知される各ＩＤコードに基づいて位置データメモリ３７を参照することにより、上記入射部３１への各入射光の発光元である各光学ビーコンの位置（ＸＹ座標）を判別する判別手段。

（４）上記選定手段で選定される組合せごとに、その測定角度α，βの変化に対する、上記演算手段で演算されるＸ座標ＰｘおよびＹ座標Ｐｙの変化率Δｘ，Δｙを、推定する推定手段。

なお、（２）の演算手段は、上記選定手段で選定される各組合せの測定角度α，βのうち、上記推定手段で推定される変化率Δｘが最小となる組合せの測定角度α，βを用いて、移動体２の位置のＸ座標Ｐｘを演算するとともに、上記推定手段で推定される変化率Δｙが最小となる組合せの測定角度α，βを用いて、移動体２の位置のＹ座標Ｐｙを演算する。

また、移動体２は、自己位置演算部３５で検出される位置を蓄積して記憶するための蓄積メモリ３８を有している。この蓄積メモリ３８の記憶内容に基づき、移動体２の移動経路を解析することが可能である。

また、移動体２は、自律走行可能な例えば移動ロボットとしての使用を可能にするため、コントローラ４０、走行ユニット４１、マップデータメモリ４２、移動ルートプログラムメモリ４３を有している。マップデータメモリ４２は、建物１内の移動空間のマップデータを記憶している。移動ルートプログラムメモリ４３は、当該移動体２の移動ルートを指定するための移動ルートプログラムを記憶している。コントローラ４０は、移動ルートプログラムメモリ４３内の移動ルートプログラムに従い、かつ自己位置演算部３５で特定される移動体位置とマップデータメモリ４２内のマップデータとの照合により、自律走行ユニット４１を駆動制御する。

ここで、自己位置演算部３５の機能について、詳細に説明する。
３つの光学ビーコンＡ，Ｂ，Ｃの位置のＸＹ座標が、Ａ（ｘ_ａ，ｙ_ａ）、Ｂ（ｘ_ｂ，ｙ_ｂ）、Ｃ（ｘ_ｃ，ｙ_ｃ）であるとする。

移動体２の位置Ｐを求めるためには、方向線Ｌａ，Ｌｂ，Ｌｃの相互間の角度θ_１，θ_２，θ_３のうちいずれか２つが分かればよく、それぞれ２つの測定角度α，β（＝［θ_１，θ_２］［θ_２，θ_３］［θ_１，θ_３］）からなる複数の組合せが選定される。

これら組合せのうち［θ_１，θ_２］の組合せを用いて、移動体２のＸ方向位置であるＸ座標ＰｘおよびＹ方向位置であるＹ座標Ｐｙを求める方法について説明する。

光学ビーコンＡ，Ｂを通り、θ_１を円周角とする円の中心のＸＹ座標をＯ_１（ｘ_１，ｙ_１）とする。
光学ビーコンＡ，Ｃを通り、θ_２を円周角とする円の中心のＸＹ座標をＯ_２（ｘ_２，ｙ_２）とする。
光学ビーコンＡ，Ｂと中心のＸＹ座標Ｏ_１（ｘ_１，ｙ_１）とがなす角度を、∠ＢＡＯ_１＝φとする。
光学ビーコンＡ，Ｃと中心のＸＹ座標Ｏ_２（ｘ_２，ｙ_２）とがなす角度を、∠ＣＡＯ_２＝ηとする。

移動体２の位置は、光学ビーコンＡ，Ｂを通りθ_１を円周角とする円と、光学ビーコンＡ，Ｃを通りθ_２を円周角とする円との交点として、求められる。通常、これらの円は２点で交わるが、一方の交点は光学ビーコンＡの位置と一致するので、残りの交点Ｐが移動体２の位置として決定される。

光学ビーコンＡの位置が基点となるため、便宜上、光学ビーコンＢの位置のＸＹ座標（ｘＢ，ｙＢ）および光学ビーコンＣの位置のＸＹ座標（ｘＣ，ｙＣ）を、
（ｘＢ，ｙＢ）＝（ｘｂ−ｘａ，ｙｂ−ｙａ）
（ｘＣ，ｙＣ）＝（ｘｃ−ｘａ，ｙｃ−ｙａ）
とすると、円の中心のＸＹ座標Ｏ_１，Ｏ_２はそれぞれ下式（１）（２）のように求めることができる。この結果を用いると、移動体２の位置のＸ座標ＰｘおよびＹ座標Ｐｙは第１の演算式である下式（４）により求めることができる。

移動体２の位置のＸ座標ＰｘおよびＹ座標Ｐｙを決定するパラメータは、測定角度θ_１，θ_２である。この測定角度θ_１，θ_２に変化があれば、移動体２の位置のＸ座標ＰｘおよびＹ座標Ｐｙも変化する。

測定角度θ_１，θ_２の変化に対するＸ座標Ｐｘの変化率Δｘは、式（４）の偏微分により、第２の演算式である下式（６）により推定することができる。

測定角度θ_１，θ_２の変化に対するＹ座標Ｐｙの変化率Δｙは、式（４）の偏微分により、第２の演算式である下式（７）により推定することができる。

一方、測定角度θ_２，θ_３の組合せを用いて移動体２の位置を検出する場合も、その測定角度θ_２，θ_３に変化があれば、移動体２の位置のＸ座標ＰｘおよびＹ座標Ｐｙに変化が生じる。

測定角度θ_２，θ_３の変化に対するＸ座標Ｐｘの変化率Δｘは、式（４）の偏微分により、第２の演算式である下式（８）により推定することができる。

測定角度θ_２，θ_３の変化に対するＹ座標Ｐｙの変化率Δｙは、式（４）の偏微分により、第２の演算式である下式（９）により推定することができる。

また、測定角度θ_３，θ_１の組合せを用いて移動体２の位置を検出する場合も、その測定角度θ_３，θ_１に変化があれば、移動体２の位置のＸ座標ＰｘおよびＹ座標Ｐｙに変化が生じる。

測定角度θ_３，θ_１の変化に対するＸ座標Ｐｘの変化率Δｘは、式（４）の偏微分により、第２の演算式である下式（１０）により推定することができる。

測定角度θ_３，θ_１の変化に対するＹ座標Ｐｙの変化率Δｙは、式（４）の偏微分により、第２の演算式である下式（１１）により推定することができる。

測定角度θ_１，θ_２，θ_３に同一の測定誤差が含まれているとすれば、それぞれ２つの測定角度の組合せを用いて演算されるＸ座標Ｐｘのうち、上記推定される変化率Δｘが最小となる測定角度の組合せを用いて演算されるＸ座標Ｐｘを、測定誤差の影響が最も小さいＸ座標Ｐｘとして特定することができる。

同様に、測定角度θ_１，θ_２，θ_３に同一の測定誤差が含まれているとすれば、それぞれ２つの測定角度の組合せを用いて演算されるＹ座標Ｐｙのうち、上記推定される変化率Δｙが最小となる測定角度の組合せを用いて演算されるＹ座標Ｐｙを、測定誤差の影響が最も小さいＹ座標Ｐｙとして特定することができる。

具体的な条件として、図９に示すように、３つの光学ビーコンＡ，Ｂ，ＣのＸ，Ｙ座標（Ａｘ，Ａｙ）、（Ｂｘ，Ｂｙ）、（Ｃｘ，Ｃｙ）がそれぞれ（0，0）、（0，100）、（86.6，50）で、移動体２の位置であるＰ点のＸ，Ｙ座標（Ｐｘ，Ｐｙ）が（49.43，120.71）で、測定角度θ_１，θ_２，θ_３がθ_１＝90°、θ_２＝125°、θ_３＝145°である場合、上記各変化率は次のような値となる。
まず、測定角度θ_１，θ_２の変化に対するＸ座標Ｐｘの変化率Δｘは、上式（６）から、Δｘ＝0.81となる。

測定角度θ_２，θ_３の変化に対するＸ座標Ｐｘの変化率Δｘは、上式（８）から、Δｘ＝1.76となる。

測定角度θ_３，θ_１の変化に対するＸ座標Ｐｘの変化率Δｘは、上式（１０）から、Δｘ＝0.95となる。
このうち、変化率Δｘが最小となる測定角度の組合せは、測定角度θ_１，θ_２の組合せである。

続いて、測定角度θ_１，θ_２の変化に対するＸ座標Ｐｙの変化率Δｙは、上式（７）から、Δｙ＝0.64となる。

測定角度θ_２，θ_３の変化に対するＸ座標Ｐｙの変化率Δｙは、上式（９）から、Δｙ＝0.16となる。

測定角度θ_３，θ_１の変化に対するＸ座標Ｐｙの変化率Δｙは、上式（１１）から、Δｙ＝0.80となる。
このうち、変化率Δｙが最小となる測定角度の組合せは、測定角度θ_２，θ_３の組合せである。

したがって、移動体２のＰ点のＸ座標Ｐｘを検出するには測定角度θ_１，θ_２の組合せを選択し、Ｐ点のＹ座標Ｐｙを検出するには測定角度θ_２，θ_３の組合せを選択することにより、たとえ測定角度に誤差が含まれていても、その誤差の影響を最小限に抑えて、移動体２の位置を的確に検出することができる。これにより、位置検出の信頼性が大幅に向上する。

なお、測定角度に誤差＋１°が含まれている場合を想定すると、移動体２の位置ＰのＸ座標ＰｘおよびＹ座標Ｐｙとして、次の検出結果が得られる。
測定角度θ_１，θ_２の組合せを演算に使用した場合、Ｐｘ＝48.97（−0.81）、Ｐｙ＝54.07（−0.64）となる。括弧内の数値は、誤差である。

測定角度θ_２，θ_３の組合せを演算に使用した場合、Ｐｘ＝51.57（1.79）、Ｐｙ＝54.55（−0.16）となる。

測定角度θ_３，θ_１の組合せを演算に使用した場合、Ｐｘ＝48.83（−0.95）、Ｐｙ＝55.53（0.82）となる。

Ｘ座標Ｐｘに関しては測定角度θ_１，θ_２の組合せを選択したとき、Ｙ座標Ｐｙに関しては測定角度θ_２，θ_３の組合せを選択したときに、検出位置の誤差が最小になっている。

また、図１０に示すように、３つの光学ビーコンＡ，Ｂ，ＣのＸ，Ｙ座標（Ａｘ，Ａｙ）、（Ｂｘ，Ｂｙ）、（Ｃｘ，Ｃｙ）がそれぞれ（0，0）、（0，100）、（86.6，50）で、移動体２の位置であるＰ点のＸ，Ｙ座標（Ｐｘ，Ｐｙ）が（49.43，120.71）で、測定角度θ_１，θ_２，θ_３がθ_１＝45°、θ_２＝50°、θ_３＝95°である場合、上記各変化率は次のような値となる。
まず、測定角度θ_１，θ_２の変化に対するＸ座標Ｐｘの変化率Δｘは、上式（６）から、Δｘ＝1.67となる。

測定角度θ_２，θ_３の変化に対するＸ座標Ｐｘの変化率Δｘは、上式（８）から、Δｘ＝4.30となる。

測定角度θ_３，θ_１の変化に対するＸ座標Ｐｘの変化率Δｘは、上式（１０）から、Δｘ＝2.63となる。
このうち、変化率Δｘが最小となる測定角度の組合せは、測定角度θ_１，θ_２の組合せである。

続いて、測定角度θ_１，θ_２の変化に対するＸ座標Ｐｙの変化率Δｙは、上式（７）から、Δｙ＝1.23となる。

測定角度θ_２，θ_３の変化に対するＸ座標Ｐｙの変化率Δｙは、上式（９）から、Δｙ＝0.04となる。

測定角度θ_３，θ_１の変化に対するＸ座標Ｐｙの変化率Δｙは、上式（１１）から、Δｙ＝1.19となる。
このうち、変化率Δｙが最小となる測定角度の組合せは、測定角度θ_２，θ_３の組合せである。

なお、測定角度に誤差＋１°が含まれている場合を想定すると、移動体２の位置ＰのＸ座標ＰｘおよびＹ座標Ｐｙとして、次の検出結果が得られる。
測定角度θ_１，θ_２の組合せを演算に使用した場合、Ｐｘ＝47.67（−1.76）、Ｐｙ＝119.51（−1.20）となる。括弧内の数値は、誤差である。

測定角度θ_２，θ_３の組合せを演算に使用した場合、Ｐｘ＝45.02（−4.41）、Ｐｙ＝120.54（−0.17）となる。

測定角度θ_３，θ_１の組合せを演算に使用した場合、Ｐｘ＝52.12（2.69）、Ｐｙ＝119.4（−1.31）となる。

以下、変形例について説明する。
上記（６）式〜（１１）式のいわゆる偏微分式を用いて選択される測定角度の組合せと、位置座標から誤差を求めて選択される測定角度の組合せとの間には、多少のずれが生じる。たとえば、Ｐ点のＹ座標Ｙｐに関して測定誤差の影響が２番目に小さい組合せは、微分式から選択される場合に測定角度θ_３，θ_１の組合せとなるのに対し、位置座標から誤差を求めて選択される場合に測定角度θ_１，θ_２の組合せとなり、異なる。

そこで、より正確な位置検出を行うために、次の方法がある。
測定角度θ_１，θ_２，θ_３のうち、各組合せをなす２つの測定角度をα，βと見なし、その測定角度αの実際値がΘ（deg）、測定角度βの実際値がΨ（deg）で、測定角度α，βが測定誤差ｋ（deg；任意の定数）を含む場合に、上記推定される変化率Δｘ，Δｙの実際値として、変化率Δｘ_１，Δｙ_１を算出する（第１算出手段）。

さらに、測定角度αの実際値がΘ（deg）、測定角度βの実際値がΨ（deg）で、測定角度α，βが測定誤差ｋ（deg；任意の定数）を含まない場合に、上記推定される変化率Δｘ，Δｙの実際値として、変化率Δｘ_２，Δｙ_２を算出する（第２算出手段）。

そして、上記演算手段で演算される各座標Ｐｘのうち、上記算出される変化率Δｘ_１と、上記算出される変化率Δｘ_２との差（＝｜Δｘ_１｜−｜Δｘ_２｜）を求める。この差が最小となる組合せに対応するＸ座標Ｐｘを、移動体２の位置のＸ座標Ｐｘとして特定する。

さらに、上記演算手段で演算される各Ｙ座標Ｐｙのうち、上記算出される変化率Δｙ_１と、上記算出される変化率Δｙ_２との差（＝｜Δｙ_１｜−｜Δｙ_２｜）を求める。この差が最小となる組合せに対応するＹ座標Ｐｙを、移動体２の位置のＹ座標Ｐｙとして特定する。

具体的には、例えばｋ＝１（deg）の場合、測定角度θ_１，θ_２，θ_３の組合せごとに、次の算出結果が得られる。

すなわち、測定角度α＝Θ（deg），β＝Ψ（deg）で、それぞれの測定角度に計測誤差ｋ（deg）が含まれたとき、P点の座標Ｐｘ，Ｐｙの実際値と検出値との誤差は、ｋが微小であるほど、下式に近づくという性質がある。

したがって、図１０の例で考えた場合、移動体２の点Ｐの座標Ｐｘの変化率および座標Ｐｙの変化率は、次のように計算できる。

測定角度θ_１，θ_２の組合せを使用したとき、座標Ｐｘの変化率＝−1.76、座標Ｐｙの変化率＝−1.20となる。

測定角度θ_２，θ_３の組合せを選択したとき、座標Ｐｘの変化率＝−4.41、座標Ｐｙの変化率＝−0.18となる。

測定角度θ_３，θ_１の組合せを選択したとき、座標Ｐｘの変化率＝−2.70、座標Ｐｙの変化率＝−1.31となる。

この場合、すなわち偏微分式を用いて選択される測定角度の組合せは、位置座標から誤差を求めて選択される測定角度の組合せと、ほぼ一致する。

なお、上記実施形態では、複数の光学ビーコン♯０〜♯６から発せられる光を移動体２側で捕らえながら移動体２の位置を検出する場合について説明したが、それに限らず、移動体２から周囲にレーザ光を発し、周囲の複数の反射部材で反射されるレーザ光を移動体２で捕らえながら移動体２の位置を検出する場合についても、同様に実施できる。

その他、この発明は、上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。更に、異なる実施形態に亘る構成要素を適宜組み合わせてもよい。

一実施形態の全体的な構成を示す図。一実施形態の発光順序が第１位の光学ビーコンの制御回路のブロック図。一実施形態における各光学ビーコンの発光動作を示すタイムチャート。一実施形態の残りの各光学ビーコンの制御回路のブロック図。一実施形態における各光学ビーコンのＩＤコードおよび各光学ビーコンに記憶される特定ＩＤコードの関係を示す図。一実施形態における移動体の制御回路のブロック図。一実施形態における受光部の構成を示す図。一実施形態における３つの光学ビーコンと移動体の位置との関係を示す図。一実施形態における３つの光学ビーコンのＸＹ座標と移動体の位置のＸＹ座標との関係を示す図。一実施形態における３つの光学ビーコンのＸＹ座標と移動体の位置のＸＹ座標との関係の別の例を示す図。

符号の説明

１…建物、２…移動体、♯０〜♯６…光学ビーコン、１０…発光パターン生成部、１１…タイマ、１２…ＩＤ設定部、１３…発光素子、２０…受光素子、２１…信号判定部、２２…比較部、２３…ＩＤメモリ、２４…発光パターン生成部、２５…タイマ、２６…ＩＤ設定部、２７…発光素子、３０…受光部、３２…二次元光学センサ、３３…光点位置計測部、３４…入射角度計算部、３５…自己位置演算部、３６…コード検知部、

Claims

移動体の位置検出用として３つ以上の指標を備えた位置検出システムにおいて、
前記各指標のうち３つの指標と前記移動体とを結ぶ各方向線の相互間の角度を測定する測定手段と、
前記各指標の位置と前記測定手段で測定される角度から、前記移動体の位置のＸ座標ＰｘおよびＹ座標Ｐｙを演算する演算手段と、
前記測定手段の測定角度のうち、それぞれ２つの測定角度α，βからなる複数の組合せを選定する選定手段と、
前記選定手段で選定される組合せごとに、それぞれの測定角度α，βの変化に対する、前記演算手段で演算されるＸ座標ＰｘおよびＹ座標Ｐｙの変化率Δｘ，Δｙを、推定する推定手段と、
前記推定手段で推定される変化率Δｘ，Δｙに対し、前記測定角度αの実際値がΘ、前記測定角度βの実際値がΨで、測定角度α，βが測定誤差ｋを含む場合の実際の変化率Δｘ_１，Δｙ_１を算出する第１算出手段と、
前記推定手段で推定される前記変化率Δｘ，Δｙに対し、前記測定角度αの実際値がΘ、前記測定角度βの実際値がΨで、測定角度α，βが測定誤差を含まない場合の実際の変化率Δｘ_２，Δｙ_２を算出する第２算出手段と、
を備え、
前記演算手段は、前記選定手段で選定される各組合せの測定角度α，βのうち、前記第１算出手段で算出される実際の変化率Δｘ_１と前記第２算出手段で算出される実際の変化率Δｘ_２との差（＝｜Δｘ_１｜−｜Δｘ_２｜）が最小となる組合せの測定角度α，βを用いて、前記移動体の位置のＸ座標Ｐｘを演算するとともに、前記第１算出手段で算出される実際の変化率Δｙ_１と前記第２算出手段で算出される実際の変化率Δｙ_２との差（＝｜Δｙ_１｜−｜Δｙ_２｜）が最小となる組合せの測定角度α，βを用いて、前記移動体の位置のＹ座標Ｐｙを算出する、
ことを特徴とする位置検出システム。
移動体の位置検出用として３つ以上の指標を備えた位置検出システムの位置検出方法であって、
前記各指標のうち３つの指標と前記移動体とを結ぶ各方向線の相互間の角度を測定するステップと、
前記各指標の位置と前記測定される角度から、前記移動体の位置のＸ座標ＰｘおよびＹ座標Ｐｙを演算するステップと、
前記測定される角度のうち、それぞれ２つの測定角度α，βからなる複数の組合せを選定するステップと、
前記選定される組合せごとに、それぞれの測定角度α，βの変化に対する、前記演算されるＸ座標ＰｘおよびＹ座標Ｐｙの変化率Δｘ，Δｙを、推定するステップと、
前記推定される変化率Δｘ，Δｙに対し、前記測定角度αの実際値がΘ、前記測定角度βの実際値がΨで、測定角度α，βが測定誤差ｋを含む場合の実際の変化率Δｘ _１，Δｙ _１を算出するステップと、
前記推定される変化率Δｘ，Δｙに対し、前記測定角度αの実際値がΘ、前記測定角度βの実際値がΨで、測定角度α，βが測定誤差を含まない場合の実際の変化率Δｘ _２，Δｙ _２を算出するステップと、
を備え、
前記演算するステップは、前記選定される各組合せの測定角度α，βのうち、前記算出される実際の変化率Δｘ _１と前記算出される実際の変化率Δｘ _２との差（＝｜Δｘ _１｜−｜Δｘ _２｜）が最小となる組合せの測定角度α，βを用いて、前記移動体の位置のＸ座標Ｐｘを演算するとともに、前記算出される実際の変化率Δｙ _１と前記算出される実際の変化率Δｙ _２との差（＝｜Δｙ _１｜−｜Δｙ _２｜）が最小となる組合せの測定角度α，βを用いて、前記移動体の位置のＹ座標Ｐｙを算出する、
ことを特徴とする位置検出方法。