JPH0755419A - 光学式位置および姿勢検出装置 - Google Patents

Abstract

(57)【要約】 【目的】 比較的簡単な構成で物体の位置および姿勢を
検出できる光学式位置および姿勢検出装置を提供する。 【構成】 光学式位置および姿勢検出装置の構成とし
て、所定間隔で定位置に配置された少なくとも２つの光
源と、該光源を交互に切り換える切り換え手段と、前記
光源からの光を受光して各光源に対して少なくとも２次
元の角度を検出して信号を出力する１つの受光素子と、
前記所定間隔と前記信号から姿勢および位置を演算する
演算手段を設ける。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、光学式位置および姿勢
検出装置に関し、特に、３次元位置および姿勢を検出す
る装置に関する。本発明の装置の受光部を任意の物体に
取付けることによって、その物体の位置および姿勢を非
接触で求めることができ、物体の立体的な動きを得るこ
とができる。

【０００２】

【従来の技術】従来の物体の姿勢を検出する手段は、ジ
ャイロスコープを用いるのが一般的であり、その構成の
精密さに基づいて非常に高価なものとなっていた。ま
た、光を使用した方式では、被検出部には複数の光源を
配置し、受光部には受光素子としてＣＣＤ（Ｃｈａｒｇ
ｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）ラインセンサなど
を複数個配置し、複数の光源と複数の受光素子の位置関
係から姿勢を求めていた。

【０００３】また、物体の２次元または３次元の位置座
標検出は、光学的なものでは、物体に光源を設置し、そ
の光源からの光を複数の受光素子で受光し、三角測量の
原理に基づいて座標を算出するものが多い。

【０００４】

【発明が解決しようとする課題】基本的に、従来の姿勢
検出手段は、装置構成が大掛かりなものや複雑なものと
なっており、その結果高価であったり、容易に移動でき
なく、実用的に使い易いものでなかった。一方、位置座
標の検出においては、装置構成自体はある程度簡単に成
り得るが、光源を物体側に持った光学式三角測量では、
姿勢も同時に検出することが困難であった。

【０００５】したがって、本発明の目的は、比較的簡単
な構成で物体の位置および姿勢を検出できる光学式位置
および姿勢検出装置を提供することにある。

【０００６】

【課題を解決するための手段】前述の目的を達成するた
めに、本発明は、所定間隔で定位置に配置された少なく
とも２つの光源と、該光源を交互に切り換える切り換え
手段と、前記光源からの光を受光して各光源に対して少
なくとも２次元の角度を検出して信号を出力する１つの
受光素子と、前記所定間隔と前記信号から姿勢および位
置を演算する演算手段とを有することを特徴とする光学
式位置および姿勢検出装置を採用するものである。

【０００７】

【実施例】次に、本発明の好ましい実施例を図面を参照
して説明する。

【０００８】本発明では、市販品として入手し易い比較
的安価な部品を使用して、物品（本発明では受光ユニッ
ト）の姿勢および位置を同時に検出するために、光源、
受光ユニット（受光素子を持つ）として、以下のような
部品構成および配置を採用する。

【０００９】本発明の光源としては、光の放射角度の広
いＬＥＤを用い、姿勢および２次元位置座標を求めるた
めには、２つのＬＥＤ１０ａ、１０ｂを想定している平
面上に適当な距離を隔てて配置する（図１ａ）。また位
置座標を３次元で求めるためには、さらに１つのＬＥＤ
１０ｃを追加し、最初に配置した２つのＬＥＤとは同一
直線上にないように配置する（図１ｂ）。このときの座
標系を図１ｃに示す。この座標系において、２つのＬＥ
Ｄ１０ａ、１０ｂはＸ軸上に距離を隔てて配置され、追
加のＬＥＤ１０ｃはＺ軸上に配置されることになる。ま
た、検出すべき物品（図示せず）は、Ｘ−Ｚ平面より手
前の任意の位置にある。

【００１０】姿勢および位置を検出すべき対象となる物
品は、光源からの光を受光する受光素子を持つ移動可能
な受光ユニット１１（図３参照）から構成されている。
受光素子としては、図２ａに示すような４分割フォトデ
ィテクタ１４や図２ｂに示すような位置検出素子（ＰＳ
Ｄ：Ｐｏｓｉｔｉｏｎ Ｓｅｎｓｉｔｉｖｅ Ｄｅｖｉ
ｃｅ）２０が用いられる。４分割フォトディテクタ１４
は、例えば、４分割ピン（ＰＩＮ）フォトダイオードの
ような上下左右に４分割された部分を有し、各部分が入
射した光の光量を個々に（独立して）検出するものであ
る。この４分割フォトディテクタ１４は、ケース１２内
に配置され、ケース１２の前方には、絞りの機能を持つ
例えば図示のような円形の開口１２ａ（または正方形の
開口でもよい）が形成されている。位置検出素子２０と
しては、詳細には後述するような２次元位置検出素子が
用いられ、この位置検出素子２０は、ケース１６内に収
容され、ケース１６の前方には、入射する光を位置検出
素子２０の表面上で焦点をほぼ結ぶように集光用レンズ
１８が設けられている。

【００１１】次に、図３を参照して、検出すべき受光ユ
ニットの姿勢を説明する。今、図３ａに示すようにＸＹ
Ｚ直角座標系の原点に１つの発光素子１０ａが配置され
ており、Ｘ軸上に他の発光素子が配置されているとする
と、姿勢としては、図３ｂに示すように、原点に対して
受光ユニット１１が向いている角度、即ち水平方向角度
（ＸＹ平面上）と、図３ｃに示すように、垂直方向角度
（ＹＺ平面上）と、図３ｄに示すように、発光素子１０
ａ、１０ｂが含まれるＸ軸に対する受光ユニットの回転
角度が検出できる。

【００１２】次に、２つの光源が図１に示すように配置
されている場合の姿勢および２次元位置座標の算出方法
を説明する。光源は、交互に発光を繰り返し、受光素子
はそのタイミングに合わせて入射光を受光し、入射光に
対応する情報を出力するように構成されている。

【００１３】今、図４ａに示すように、１つの光源１０
ａに対する受光ユニット１１の姿勢により、光源１０ａ
と受光ユニット１１を結ぶ方向と、受光ユニット１１の
受光素子が向いている方向とが異なるときには、光源か
らの光は受光素子に傾いて入射することになる。このと
きの光が受光素子に入射する様子を図４ｂ（４分割フォ
トディテクタの場合）および図４ｃ（２次元位置検出素
子の場合）に概略的に示す。図４ｂに示すように、４分
割フォトディテクタ１４の４つに分割された部分ａ、
ｂ、ｃおよびｄに入射された光は、例えば、ｃおよびｄ
部分に多く入射され、中心よりずれることになる。また
図４ｃに示すように、２次元位置検出素子２０上に集光
された光は中心よりずれることになる。

【００１４】図５を参照してさらに具体的に説明する
と、図５は受光素子の受光面上の光の分布を示すもので
ある。図５ａ（４分割フォトディテクタの場合）の左の
部分は、受光ユニットの受光素子が光源に向いている場
合を示しており、このときには、光は４分割フォトディ
テクタの中心に入射し、各部分ａ、ｂ、ｃおよびｄで受
光する光の量は均等になるが、前述のように光源からの
光が受光素子に傾いて入射すると、図５ａの右の部分に
示すように、光は中心よりずれて入射され、各部分ａ、
ｂ、ｃおよびｄで受光した光量は異なったものとなる。
また、図５ｂ（２次元位置検出素子の場合）の左の部分
は、受光ユニットの受光素子が光源に向いている場合を
示しており、このときには、光は２次元位置検出素子の
中心で集光するが、前述のように光源からの光が受光素
子に傾いて入射すると、図５ｂの右の部分に示すように
中心からずれた位置に集光することになる。

【００１５】前述のように、光源からの光が４分割フォ
トディテクタに傾いて入射すると、各部分に光量差が生
じる。この結果、４分割フォトディテクタ間に出力差が
生じる。図６は、この場合の出力差を取り出す回路を示
すものである。今、説明の便宜上、４分割フォトディテ
クタの各部分ａ、ｂ、ｃおよびｄの光量（具体的には、
検出した光量に基づく電流値を電圧値に変換した値）を
ａ、ｂ、ｃおよびｄとすると、差動回路２２の反転入力
端子２２ａに（ａ＋ｂ）を入力し、非反転入力端子２２
ｂに（ｃ＋ｄ）を入力すると、出力として｛（ｃ＋ｄ）
−（ａ＋ｂ）｝が得られる。この出力を規格化した値で
ある規格化差動出力｛（ｃ＋ｄ）−（ａ＋ｂ）｝／
｛（ｃ＋ｄ）＋（ａ＋ｂ）｝と光線入射角度の間には、
以下に説明するように、入射角度が小さい範囲、例え
ば、２５°以下ではほぼ比例関係が成り立つ。

【００１６】図７は、Ｘ方向（水平方向）に関する規格
化差動出力と光線入射角度の関係を示すグラフである。
図７は、具体的には、例として、光源間距離を３００ｍ
ｍ、開口径を３ｍｍ、開口−ディテクタ間距離を２．９
ｍｍとしたときのグラフである。当然Ｚ方向（垂直方
向）に関しても同様な結果が得られる。したがって、こ
のグラフを利用することにより、差動出力値から逆に光
線の入射角度を求めることができる。図７のグラフに示
すように、入射角度と差動出力との関係は、入射角度が
２５°付近までほぼ比例関係である。したがって、簡易
的には、差動出力値をＤ、角度をθ、比例定数をｋとす
ると、以下の式が成り立つ。 Ｄ＝ｋ×θ、従って、θ＝Ｄ／ｋ （１） 即ち、（１）式に基づいて、角度が計算できる。

【００１７】次に、図８を参照して、２次元位置検出素
子（ＰＳＤ）について説明する。図８は、２次元位置検
出素子の概略平面図である。２次元位置検出素子は、そ
の４隅に出力端子を有し、光スポットが検出表面に入射
する位置によって、それらの出力端子から得られる電流
値が異なり、電流値から光スポットが入射した位置が、
以下に説明するように、計算から求められるものであ
る。

【００１８】今、各出力端子から得られる電流値の各々
をＩ₁ 、Ｉ₂ 、Ｉ₃ 、Ｉ₄ とし、全電流をＩ₀ （Ｉ₁ ＋
Ｉ₂ ＋Ｉ₃ ＋Ｉ₄ ）とし、Ｘ軸上の受光面長さをＬ_X と
し、Ｙ軸上の受光面の長さをＬ_Y とすると、受光面上の
光スポットＸ座標ｘｐと、受光面上の光スポットＹ座標
ｙｐは以下の式から求められる。 ｘｐ＝（Ｌ_X ／２）×（−Ｉ₁ −Ｉ₂ ＋Ｉ₃ ＋Ｉ₄ ）／Ｉ₀ （２） ｙｐ＝（Ｌ_Y ／２）×（Ｉ₁ −Ｉ₂ −Ｉ₃ ＋Ｉ₄ ）／Ｉ₀ （３）

【００１９】前述のように、受光面上の光スポットのＸ
座標およびＹ座標が求められることから、受光面上での
光スポットの移動距離も位置間の差によって求めること
ができる。これら光線の入射角度も次のようにして求め
ることができる。

【００２０】今、レンズの焦点距離をｆとし、光の入射
角度をθとし、光スポットの移動距離をｓとすると、θ
は以下のように表すことができる。 θ＝ａｒｃｔａｎ（ｓ／ｆ） （４）

【００２１】このようにして、光スポットの位置をＸ、
Ｚについて、検出すれば、前述の関係式より、４分割フ
ォトディテクタの場合と同様に光線の入射角度を決定で
きる。

【００２２】以上の説明は、１つの光源に対して１つの
受光素子を含む受光ユニットを用いた場合に、どのよう
にしてＸ方向（水平方向）およびＺ方向（垂直方向）の
入射角度を検出できるかに向けられたものである。本発
明においては、２つの光源を用いているが、当然同様に
して、それぞれの光源に対するＸ方向（水平方向）およ
びＺ方向（垂直方向）の入射角度を検出できる。

【００２３】しかし、以上の説明は、図９ａに示すよう
に、光源側の座標系と受光ユニット側の座標系が平行関
係にある場合に成り立つものであり、図９ｂのように、
光源側の座標系に対して受光ユニット側の座標系が回転
している（捩じれている）場合には成り立たない。即
ち、受光ユニット１１は携帯性のあることを前提として
いるために、図９ｂに示すように座標系の間に角度差が
常に生じ得ることから、この角度差、即ち回転角（光源
に対する捩じれ角）を検出し、先に求めた入射角を補正
しなければ、真の入射角は得られない。

【００２４】したがって、次に、補正方法について説明
する。補正としては、座標の回転変換を行うだけでよ
い。例えば、ηの角度差が座標間にあるとすると、光源
の中心座標がＸ’Ｙ’座標系で（ｘｃ’，ｙｃ’）にあ
る場合には、変換値（ｘｃ，ｙｃ）は以下の式で表され
る。 ｘｃ＝ｘｃ’ｃｏｓη＋ｙｃ’ｓｉｎη （５） ｙｃ＝ｘｃ’ｓｉｎη＋ｙｃ’ｃｏｓη （６）

【００２５】回転角について別の見方をすると、回転角
が検出できれば、光源から受光素子に対する入射角度も
求められ、結局、光源側を基準とした座標系においての
受光ユニットの向き、回転角度が明らかになることか
ら、受光ユニットの姿勢を検出できる。

【００２６】そこで、次に回転角の検出方法を説明す
る。これまでと同様に、２つの光源と受光ユニットを含
む平面をＸＹ平面とし、光源間をＸ座標（水平方向）と
し、Ｚ方向を垂直方向とする（図９）。この座標系にお
いて、図９ａに示すように、受光ユニットが回転角を持
たない場合には、受光パターンは図１０ａのようにな
り、各々の光源１０ａ、１０ｂに対する入射角を求める
と、垂直方向に関しては角度差は生じない。即ち、光源
１０ａの受光素子の各受光部分ａ、ｂ、ｃ、ｄの受光量
をａ、ｂ、ｃ、ｄとし、光源１０ｂの受光素子の各受光
部分ａ’、ｂ’、ｃ’、ｄ’の受光量をａ’、ｂ’、
ｃ’、ｄ’とすると、（ａ＋ｂ）−（ｃ＋ｄ）＝（ａ’
＋ｂ’）−（ｃ’＋ｄ’）となる。一方、図９ａに示す
ように、座標系間に角度差が生じた場合には、受光パタ
ーンは、図１０ｂに示すようになり、光源１０ａに対す
る垂直方向の角度と光源１０ｂに対する垂直方向の角度
との間には差が生じてくる。即ち、例えば、（ａ＋ｂ）
−（ｃ＋ｄ）＞（ａ’＋ｂ’）−（ｃ’＋ｄ’）とな
る。しかし、この回転角度と垂直方向の差動出力差の間
には、図１１に示すようなほぼ比例関係が成り立ち、受
光素子の垂直方向差動出力の差から回転角度を求めるこ
とができる。ただし、そのときの比例定数は光源と受光
素子間の距離に反比例するので、これらの距離を求める
ことが必要である。なお、距離は受光ユニットの座標位
置を算出することにより求まるが、ここでは、図１４を
参照してこれらの距離を求めてみることにする。

【００２７】図１４において、光源１０ａ、１０ｂ間の
距離をＤとし、光源１０ａに対する受光ユニットの水平
方向角度をθ１とし、光源１０ｂに対する受光ユニット
の水平方向角度を−θ２とすると、２つの光源と受光素
子が作る三角形の受光ユニットの頂角θは、θ＝θ１＋
θ２となる。受光ユニットから光源間の直線（Ｘ軸）ま
での距離をＬとすると、Ｌは、光源１０ａから受光ユニ
ット１１までの距離Ｌ１と光源１０ｂから受光ユニット
１１までの距離Ｌ２の差が少ない範囲において、下記の
式で表される。 Ｌ≒（ｄ／２）／ｔａｎ（θ／２） （７） したがって、回転角をηとし、比例定数をｋａとし、光
源１０ａからの垂直方向をｖ１とし、光源１０ｂからの
垂直方向をｖ２とすると、以下の式が成り立つ。 η＝ｋａ×（ｖ１−ｖ２）／Ｌ （８）

【００２８】これで、受光ユニットの姿勢を検出するこ
とが可能になったが、回転角が３０°を越えると、受光
素子の傾きの影響が垂直方向のみならず、水平方向にも
現れ、上記θ１およびθ２の角度が実際の水平角よりず
れて計算されてしまうことになる。その影響でＬの誤差
も大きくなることから、算出された回転角の誤差が増大
してくる。これらの対策としては、まず最初のデータよ
り一度回転角を算出し、その回転角で（５）、（６）式
を用いてデータの補正を行い、水平方向の光の入射角を
再計算する。再計算後のθ１、θ２より再び回転角を求
めるといった手順をとる。図１２は、この手順をとらな
かった場合ととった場合について、算出される角度と実
際に回転させた角度との関係を比較するためのグラフで
ある。

【００２９】次に、受光ユニットの座標の算出方法を説
明する。まずＸＹ平面上で考え、図１４において、前述
と同様に各値を定める。光源１０ａ、１０ｂ間の距離は
既知であり、θは前述の計算から算出される。受光素子
が４分割フォトディテクタの場合は、（ａ＋ｂ＋ｃ＋
ｄ）で、受光素子がＰＳＤの場合は、４端子からの電流
の合計値Ｉ₀ から光強度が測定できる。受光ユニットの
受光素子に到達する光量は測定されており、光源１０
ａ、１０ｂからの光の強度をそれぞれＰ１、Ｐ２と置く
と、図１４から以下の式が成り立つ。 Ｌ１＝ｄ×（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ） （９） Ｌ２＝ｄ×ｓｉｎφ／ｓｉｎθ （１０） また、光強度と距離の関係により、 Ｌ１／Ｌ２＝（Ｐ１／Ｐ２）^1/2 が成り立つ。従って、 （Ｐ１／Ｐ２）^1/2 ＝ｓｉｎθ（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ）÷ｓｉｎφ ＝ｃｏｓθ＋ｓｉｎθ／ｔａｎφ 従って、φ＝ａｒｃｔａｎ｛ｓｉｎθ／（Ｐ１／Ｐ２）^1/2 −ｃｏｓθ）｝ θの値を（１）式に代入すれば、Ｌ１が求まる。 よって、ｘｐ＝Ｌ１×ｃｏｓφ （１１） ｙｐ＝Ｌ１×ｓｉｎφ （１２） 即ち、受光ユニットの座標位置が得られる。

【００３０】座標が求まることによって、例えば、受光
ユニットの先端の延長がＸＺ平面と交差する位置（ｘ
ｓ、ｚｓ）も次の式から特定することもできる（図２参
照）。 ｘｓ×ｃｏｓφ＋ｘｓ×ｓｉｎφ／ｔａｎθ₁ ＝Ｌ１ 従って、ｘｓ＝Ｌ１／（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ₁ ） （１３）

【００３１】また、図には表現されていないが、入射角
の算出方法で説明したように、水平方向（Ｘ方向）と同
様に、垂直方向（Ｚ方向）の角度も測定できるので、例
えば、光源１０ａに対してθｖが得られるとすると、交
差位置のＺ座標ｚｓは、次式で計算できる。 ｚｓ＝Ｌ１×ｔａｎ（θｖ） （１４） よって、（１０）、（１１）、（１２）、（１３）式よ
り、受光ユニットのＸＹ平面位置およびその他の情報と
して先端の延長方向のＸＺ平面の交差位置がそれぞれ算
出できる。

【００３２】以上をまとめると、「最初に受光素子の差
動出力（４分割フォトディテクタの場合）やスポット到
着位置（ＰＳＤの場合）から、換算グラフまたは計算に
よって、光の入射方向を垂直、水平方向に対して求め、
次に、２つの光源に対する垂直方向の角度差および光源
と受光ユニットとの距離などから受光ユニットの回転角
（Ｘ軸に対する捩じれ角）が求められる。再びその回転
角から、座標変換を行って受光ユニットの光源に対する
水平、垂直方向の向きを表す角度を補正する。さらに、
これらの角度と到達光強度の比によって受光ユニットの
２次元座標が計算される。」といった手順になる。

【００３３】同様な考えを適用して、もう１つの光源を
さらにＺ軸上に配置して、それによる角度情報も加える
と、光源のＺ座標値も計算でき、３次元的な位置を求め
ることができる。これは、ＹＺ軸平面について先のＸＹ
平面と同様な計算を行い、座標値を算出すればよい。

【００３４】以上により、受光ユニットの姿勢および２
次元および３次元の位置座標を算出できたことになる。

【００３５】最後に、図１５を参照して、本発明の回路
動作を概略する。図１５は、本発明の回路のブロックダ
イアグラムである。受光ユニット１１は、受光素子とし
て４分割フォトディテクタ１４またはＰＳＤ２０を有す
る。受光ユニット１１は、電流−電圧変換器３２を有
し、この電流−電圧変換器３２は、光量に対応して得ら
れた電流（４分割フォトディテクタの場合）を電圧に変
換する。受光ユニット１１はさらに演算器３４を有し、
この演算器３４は電流−電圧変換器３２から得られた電
圧に基づいて例えば差分や和などの位置算出や姿勢算出
に必要な演算を行う。得られたデータは本体に送られ
る。本体において、データはデータ切り換器３６によっ
て光源１０ａ、１０ｂの切り換えに応じて切り換えられ
る。各々のデータはバンドパスフィルタ３８を通されて
ノイズが除去され、ゲイン調整器４０、検波回路４２、
ローパスフィルタ４４を通されて中央演算処理ユニット
（ＣＰＵ）４６にＡ／Ｄ変換器４６ａを介して送られ
る。ＣＰＵ４６において、前述したような姿勢や座標位
置を演算して出力する。なお、ＣＰＵ４６は、光源１０
ａ、１０ｂの切り換えを行うが、この信号はバッファー
アンプ４８を介して各光源１０ａ、１０ｂに与えられ
る。

【００３６】

【発明の効果】以上説明したように、本発明では、簡単
な部品構成で、物体（受光ユニット）の姿勢および位置
座標を同時に検出することが可能となり、したがって、
コストも従来の同様な情報を得るためのシステムに比
べ、低く抑えることができることから種々の新たな用途
への採用が考えられる。例えば、仮想現実感を実現する
ための手や頭の動きを検出するセンサや、ＴＶゲーム機
向けの操作状態を伝えるコントローラなどに利用でき
る。

【図面の簡単な説明】

【図１】図１は、本発明で用いる光源の配置例および座
標系を示す斜視図である。

【図２】図２は、本発明で用いる受光素子の例を示す斜
視図である。

【図３】図３は、検出すべき受光ユニットの姿勢を説明
するための図面である。

【図４】図４は、光源に対して受光ユニットが角度をな
す場合の受光素子に入射する光量または入射位置を説明
するための図である。

【図５】図５は、図４に対応する平面図である。

【図６】図６は、本発明で用いられる差動回路を示す図
である。

【図７】図７は、基準化差動出力と光線入射角度の関係
を示すグラフである。

【図８】図８は、２次元ＰＳＤの動作原理を説明するた
めの図である。

【図９】図９は、受光ユニットが光源に対して回転した
ときの受光状態を説明するための図である。

【図１０】図１０は、受光ユニットが光源に対して回転
したときの受光状態をさらに詳しく説明するための図で
ある。

【図１１】図１１は、差動出力の差と回転角度との関係
を説明するためのグラフである。

【図１２】図１２は、補正前、補正後の算出回転角と回
転角の関係を説明するための図である。

【図１３】図１３は、位置座標の算出を説明するための
図である。

【図１４】図１４は、位置座標の算出を説明するための
図である。

【図１５】図１５は、本発明の光学式位置および姿勢検
出装置の回路ブロック図である。

【符号の説明】

１０ａ、１０ｂ、１０ｃ 光源 １１ 受光ユニット １４ ４分割フォトディテクタ ２０ ＰＳＤ

─────────────────────────────────────────────────────

【手続補正書】

【提出日】平成５年９月２７日

【手続補正１】

【補正対象書類名】明細書

【補正対象項目名】００１８

【補正方法】変更

【補正内容】

【００１８】今、各出力端子から得られる電流値の各々
をＩ_１、Ｉ_２、Ｉ_３、Ｉ_４とし、全電流をＩ_０（Ｉ_１＋
Ｉ_２＋Ｉ_３＋Ｉ_４）とし、Ｘ軸上の受光面長さをＬ_ｘと
し、Ｚ軸上の受光面の長さをＬ_ｚとすると、受光面上の
光スポットＸ座標ｘｐと、受光面上の光スポットＺ座標
ｚｐは以下の式から求められる。 ｘｐ＝（Ｌ_ｘ／２）×（−Ｉ_１−Ｉ_２＋Ｉ_３＋Ｉ_４）／Ｉ_０ （２） ｚｐ＝（Ｌ_ｚ／２）×（Ｉ_１−Ｉ_２−Ｉ_３＋Ｉ_４）／Ｉ_０ （３）

【手続補正２】

【補正対象書類名】明細書

【補正対象項目名】００１９

【補正方法】変更

【補正内容】

【００１９】前述のように、受光面上の光スポットのＸ
座標およびＺ座標が求められることから、受光面上での
光スポットの移動距離も位置間の差によって求めること
ができる。これら光線の入射角度も次のようにして求め
ることができる。

【手続補正３】

【補正対象書類名】明細書

【補正対象項目名】００２４

【補正方法】変更

【補正内容】

【００２４】したがって、次に、補正方法について説明
する。補正としては、座標の回転変換を行うだけでよ
い。例えば、ηの角度差が座標間にあるとすると、光源
の中心座標がＸ’Ｙ’座標系で（ｘｃ’，ｙｃ’）にあ
る場合には、変換値（ｘｃ，ｙｃ）は以下の式で表され
る。 ｘｃ＝ｘｃ’ｃｏｓη＋ｙｃ’ｓｉｎη （５） ｙｃ＝−ｘｃ’ｓｉｎη＋ｙｃ’ｃｏｓη （６）

【手続補正４】

【補正対象書類名】明細書

【補正対象項目名】００２９

【補正方法】変更

【補正内容】

【００２９】次に、受光ユニットの座標の算出方法を説
明する。まずＸＹ平面上で考え、図１４において、前述
と同様に各値を定める。光源１０ａ、１０ｂ間の距離は
既知であり、θは前述の計算から算出される。受光素子
が４分割フォトディテクタの場合は、（ａ＋ｂ＋ｃ＋
ｄ）で、受光素子がＰＳＤの場合は、４端子からの電流
の合計値Ｉ_０から光強度が測定できる。受光ユニットの
受光素子に到達する光量は測定されており、光源１０
ａ、１０ｂからの光の強度をそれぞれＰ１、Ｐ２と置く
と、図１４から以下の式が成り立つ。 Ｌ１＝ｄ×（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ） （９） Ｌ２＝ｄ×ｓｉｎφ／ｓｉｎθ （１０） また、光強度と距離の関係により、 Ｌ１／Ｌ２＝（Ｐ１／Ｐ２）^１／２ が成り立つ。従って、 （Ｐ１／Ｐ２）^１／２＝ｓｉｎθ（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ）÷ｓｉｎ φ＝ｃｏｓθ＋ｓｉｎθ／ｔａｎφ 従って、φ＝ａｒｃｔａｎ｛ｓｉｎθ／（（Ｐ１／Ｐ２）^１／２−ｃｏｓθ）｝ θの値を（１）式に代入すれば、Ｌ１が求まる。 よって、ｘｐ＝Ｌ１×ｃｏｓφ （１１） ｙｐ＝Ｌ１×ｓｉｎφ （１２） 即ち、受光ユニットの座標位置が得られる。
─────────────────────────────────────────────────────

【手続補正書】

【提出日】平成５年１０月７日

【手続補正１】

【補正対象書類名】明細書

【補正対象項目名】００２９

【補正方法】変更

【補正内容】

【００２９】次に、受光ユニットの座標の算出方法を説
明する。まずＸＹ平面上で考え、図１４において、前述
と同様に各値を定める。光源１０ａ、１０ｂ間の距離は
既知であり、θは前述の計算から算出される。受光素子
が４分割フォトディテクタの場合は、（ａ＋ｂ＋ｃ＋
ｄ）で、受光素子がＰＳＤの場合は、４端子からの電流
の合計値Ｉ_ｏから光強度が測定できる。受光ユニットの
受光素子に到達する光量は測定されており、光源１０
ａ、１０ｂからの光の強度をそれぞれＰ１、Ｐ２と置く
と、図１４から以下の式が成り立つ。 Ｌ１＝ｄ×（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ） （９） Ｌ２＝ｄ×ｓｉｎφ／ｓｉｎθ （１０） また、光強度と距離の関係により、 Ｌ１／Ｌ２＝（Ｐ１／Ｐ２）^１／２ が成り立つ。従って、 （Ｐ１／Ｐ２）^１／２＝ｓｉｎθ（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ）÷ｓｉｎ φ ＝ｃｏｓθ＋ｓｉｎθ／ｔａｎφ 従って、φ＝ａｒｃｔａｎ｛ｓｉｎθ／（Ｐ１／Ｐ２）^１／２−ｃｏｓθ）｝φ の値を（４）式に代入すれば、Ｌ１が求まる。 よって、ｘｐ＝Ｌ１×ｃｏｓφ （１１） ｙｐ＝Ｌ１×ｓｉｎφ （１２） 即ち、受光ユニットの座標位置が得られる。
─────────────────────────────────────────────────────

【手続補正書】

【提出日】平成６年３月１１日

【手続補正１】

【補正対象書類名】明細書

【補正対象項目名】００２６

【補正方法】変更

【補正内容】

【００２６】そこで、次に回転角の検出方法を説明す
る。これまでと同様に、２つの光源と受光ユニットを含
む平面をＸＹ平面とし、光源間をＸ座標（水平方向）と
し、Ｚ方向を垂直方向とする（図９）。この座標系にお
いて、図９ａに示すように、受光ユニットが回転角を持
たない場合には、受光パターンは図１０ａのようにな
り、各々の光源１０ａ、１０ｂに対する入射各を求める
と、垂直方向に関しては角度差は商事ない。即ち、光源
１０ａの受光素子の各受光部分ａ、ｂ、ｃ、ｄの受光量
をａ、ｂ、ｃ、ｄとし、光源１０ｂの受光素子の各受光
部分ａ’、ｂ’、ｃ’、ｄ’の受光量をａ’、ｂ’、
ｃ’、ｄ’とすると、（ａ＋ｃ）＝（ｂ＋ｄ）＝（ａ’
＋ｃ’）−（ｂ’＋ｄ’）となる。一方、図９ｂに示す
ように、座標系間に角度差が生じた場合には、受光パタ
ーンは、図１０ｂに示すようになり、光源１０ａに対す
る垂直方向の角度と光源１０ｂに対する垂直方向の角度
との間には差が生じてくる。即ち、例えば、（ａ＋ｃ）
−（ｂ＋ｄ）＞（ａ’＋ｃ’）−（ｂ’＋ｄ’）とな
る。しかし、この回転角度と垂直方向の差動出力差の間
には、図１１に示すようなほぼ比例関係が成り立ち、受
光素子の垂直方向差動出力の差から回転角度を求めるこ
とができる。ただし、そのときの比例定数は光源と受光
素子間の距離に反比例するので、これらの距離を求める
ことが必要である。なお、距離は受光ユニットの座標位
置を算出することにより求まるが、ここでは、図１４を
参照してこれらの距離を求めてみることにする。

【手続補正２】

【補正対象書類名】明細書

【補正対象項目名】００２７

【補正方法】変更

【補正内容】

【００２７】図１４において、光源１０ａ、１０ｂ間の
距離をＤとし、光源１０ａに対する受光ユニットの水平
方向角度をθ１とし、光源１０ｂに対する受光ユニット
の水平方向角度を−θ２とすると、２つの光源と受光素
子が作る三角形の受光ユニットの頂角θは、θ＝θ１＋
θ２となる。受光ユニットから光源間の直線（Ｘ軸）ま
での距離をＬとすると、Ｌは、光源１０ａから受光ユニ
ット１１までの距離Ｌ１と光源１０ｂから受光ユニット
１１までの距離Ｌ２の差が少ない範囲において、下記の
式で表される。 Ｌ≒（Ｄ／２）／ｔａｎ（θ／２） （７） したがって、回転角をηとし、比例定数をｋａとし、光
源１０ａからの垂直方向をｖ１とし、光源１０ｂからの
垂直方向角度をｖ２とすると、以下の式が成り立つ。 η＝ｋａ×（ｖ１−ｖ２）／Ｌ （８）

【手続補正３】

【補正対象書類名】明細書

【補正対象項目名】００２９

【補正方法】変更

【補正内容】

【００２９】次に、受光ユニットの座標の算出方法を説
明する。まずＸＹ平面上で考え、図１４において、前述
と同様に各値を定める。光源１０ａ、１０ｄ間の距離は
既知であり、θは前述の計算から算出される。受光素子
が４分割フォトディテクタの場合は、（ａ＋ｂ＋ｃ＋
ｄ）で、受光素子がＰＳＤの場合は、４端子からの電流
の合計値Ｉ_ｏから光強度が測定できる。受光ユニットの
受光素子に到達する光量は測定されており、光源１０
ａ、１０ｂからの光の強度をそれぞれＰ１、Ｐ２と置く
と、図１４から以下の式が成り立つ、 Ｌ１：Ｄ×（ｃｏｓφ＋ｓｉｎφ／ｔａｎｎθ （９） Ｌ２＝Ｄ×ｓｉｎφ／ｓｉｎθ （１０） また、光強度と距離の関係により、 Ｌ１／Ｌ２＝（Ｐ２／Ｐ１）^１／２ が成り立つ。従って、 （Ｐ２／Ｐ１）^１／２＝ｓｉｎθ（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ）÷ｓｉ ｎθ＝ｃｏｓθ＋ｓｉｎθ／ｔａｎφ 従って、φ＝ａｒｃｔａｎ｛ｓｉｎθ／（Ｐ２／Ｐ１）^１／２−ｃｏｓθ）｝ θの値を（９）式に代入すれば、Ｌ１が求まる。 よって、ｘｐ＝Ｌ１×ｃｏｓφ （１１） ｙｐ＝Ｌ１×ｓｉｎφ （１２） 即ち、受光ユニットの座標位置が得られる。

【手続補正４】

【補正対象書類名】明細書

【補正対象項目名】００３０

【補正方法】変更

【補正内容】

【００３０】座標が求まることによって、例えば、受光
ユニットの先端の延長がＸＺ平面と交差する位置（ｘ
ｓ、ｚｓ）も次の式から特定することもできる（図１４
参照）。 ｘｓ×ｃｏｓφ＋ｚｓ×ｓｉｎφ／ｔａｎθ_１＝Ｌ１ 従って、ｘｓ＝Ｌ１／（ｃｏｓφ＋ｓｉｎφ／ｔａｎθ_１） （１３）

【手続補正５】

【補正対象書類名】図面

【補正対象項目名】図１４

【補正方法】変更

【補正内容】

【図１４】 ─────────────────────────────────────────────────────

【手続補正書】

【提出日】平成６年８月１日

【手続補正１】

【補正対象書類名】明細書

【補正対象項目名】００３３

【補正方法】変更

【補正内容】

【００３３】同様な考えを適用して、もう１つの光源を
さらにＺ軸上に配置して、それによる角度情報も加える
と、受光ユニットのＺ座標値も計算でき、３次元的な位
置を求めることができる。これは、ＹＺ軸平面について
先のＸＹ平面と同様な計算を行い、座標値を算出すれば
よい。

Claims (7)

【特許請求の範囲】
1. 【請求項１】 所定間隔で定位置に配置された少なくと
   も２つの光源と、該光源を交互に切り換える切り換え手
   段と、前記光源からの光を受光して各光源に対して少な
   くとも２次元の角度を検出して信号を出力する１つの受
   光素子と、前記所定間隔と前記信号から姿勢および位置
   を演算する演算手段とを有することを特徴とする光学式
   位置および姿勢検出装置。
2. 【請求項２】 請求項１記載の光学式位置および姿勢検
   出装置において、前記受光素子が４分割フォトディテク
   タであることを特徴とする光学式位置および姿勢検出装
   置。
3. 【請求項３】 請求項１記載の光学式位置および姿勢検
   出装置において、前記受光素子がＰＳＤであることを特
   徴とする光学式位置および姿勢検出装置。
4. 【請求項４】 請求項１記載の光学式位置および姿勢検
   出装置において、前記演算手段が２次元位置座標を算出
   する手段を有することを特徴とする光学式位置および姿
   勢検出装置。
5. 【請求項５】 請求項１記載の光学式位置および姿勢検
   出装置において、前記２つの光源を結ぶ線とは異なる位
   置にさらに他の１つの光源を有し、前記演算手段が前記
   他の光源に対する角度情報を含めて３次元位置座標を算
   出する手段を有することを特徴とする光学式位置および
   姿勢検出装置。
6. 【請求項６】 請求項１記載の光学式位置および姿勢検
   出装置において、前記演算手段は前記受光ユニットの２
   つの光源に対する回転を検出する回転検出手段をさらに
   有することを特徴とする光学式位置および姿勢検出装
   置。
7. 【請求項７】 請求項６記載の光学式位置および姿勢検
   出装置において、前記回転検出手段は２つの光源に対す
   るある方向に対する受光素子の各受光部分の差動出力差
   と回転角度との関係から回転角度を算出することを特徴
   とする光学式位置および姿勢検出装置。