JP2613843B2

JP2613843B2 - 光学式位置および姿勢検出装置

Info

Publication number: JP2613843B2
Application number: JP22525293A
Authority: JP
Inventors: 亨鈴木
Original assignee: 株式会社デジタルストリーム
Priority date: 1993-08-18
Filing date: 1993-08-18
Publication date: 1997-05-28
Anticipated expiration: 2012-05-28
Also published as: JPH0755419A

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

【０００４】

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

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

【０００６】

【課題を解決するための手段】前述の目的を達成するた
めに、本発明は、所定間隔で定位置に配置された少なく
とも２つの光源と、該光源を交互に切り換える切り換え
手段と、前記光源からの光を４つの部分で受光して光量
を検出することによって各光源に対して少なくとも２次
元の角度を指示する信号を出力するように絞りとして機
能する開口を前方に配置された１つの４分割フォトディ
テクタと、該４分割フォトディテクタを支持する被測定
物である受光ユニットと、前記所定間隔と前記信号から
前記受光ユニットの姿勢および位置を演算する演算手段
とを有することを特徴とする光学式位置および姿勢検出
装置を採用するものである。

【０００７】

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

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

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

【００１０】姿勢および位置を検出すべき対象となる物
品は、光源からの光を受光する受光素子を持つ移動可能
な受光ユニット１１（図３参照）から構成されている。
受光素子としては、図２に示すような４分割フォトディ
テクタ１４が用いられる。４分割フォトディテクタ１４
は、例えば、４分割ピン（ＰＩＮ）フォトダイオードの
ような上下左右に４分割された部分を有し、各部分が入
射した光の光量を個々に（独立して）検出するものであ
る。この４分割フォトディテクタ１４は、ケース１２内
に配置され、ケース１２の前方には、絞りの機能を持つ
例えば図示のような円形の開口１２ａ（または正方形の
開口でもよい）が形成されている。

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

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

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

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

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

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

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

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

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

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

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

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

【００２３】図１３において、光源１０ａ、１０ｂ間の
距離をＤとし、光源１０ａに対する受光ユニットの水平
方向角度をθ１とし、光源１０ｂに対する受光ユニット
の水平方向角度を−θ２とすると、２つの光源と受光素
子が作る三角形の受光ユニットの頂角θは、θ＝θ１＋
θ２となる。受光ユニットから光源間の直線（Ｘ軸）ま
での距離をＬとすると、Ｌは、光源１０ａから受光ユニ
ット１１までの距離Ｌ１と光源１０ｂから受光ユニット
１１までの距離Ｌ２の差が少ない範囲において、下記の
式で表される。Ｌ≒（Ｄ／２）／ｔａｎ（θ／２）（５）したがって、回転角をηとし、比例定数をｋａとし、光
源１０ａからの垂直方向をｖ１とし、光源１０ｂからの
垂直方向をｖ２とすると、以下の式が成り立つ。 η＝ｋａ×（ｖ１−ｖ２）／Ｌ（６）なお，前述の垂直方向とは、図３ｃで垂直角と表示して
示す方向であり、受光ユニット１１の向いている方向ベ
クトルのＹＺ平面成分と、Ｙ軸とのなす角である。即
ち、光源１０ａからの光によって検出できる受光ユニッ
ト１１のＹ軸に対して向いている角度をｖ１とし、光源
１０ｂからの光によって検出できる受光ユニット１１の
Ｙ軸に対して向いている角度をｖ２としている（受光ユ
ニットが回転していなけらば、ｖ１とｖ２はほぼ等しい
値になる）。

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

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

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

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

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

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

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

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

【００３２】

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

【図面の簡単な説明】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【符号の説明】

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

Claims

(57)【特許請求の範囲】

【請求項１】所定間隔で定位置に配置された少なくと
も２つの光源と、該光源を交互に切り換える切り換え手
段と、前記光源からの光を４つの部分で受光して光量を
検出することによって各光源に対して少なくとも２次元
の角度を指示する信号を出力するように絞りとして機能
する開口を前方に配置された１つの４分割フォトディテ
クタと、該４分割フォトディテクタを支持する被測定物
である受光ユニットと、前記所定間隔と前記信号から前
記受光ユニットの姿勢および位置を演算する演算手段と
を有することを特徴とする光学式位置および姿勢検出装
置。
【請求項２】請求項１記載の光学式位置および姿勢検
出装置において、前記演算手段が前記受光ユニットの２
次元位置座標を算出する手段を有することを特徴とする
光学式位置および姿勢検出装置。
【請求項３】請求項１記載の光学式位置および姿勢検
出装置において、前記２つの光源を結ぶ線とは異なる位
置にさらに他の１つの光線を有し、前記演算手段が前記
他の光源に対する角度情報を含めて前記受光ユニットの
３次元位置座標を算出する手段を有することを特徴とす
る光学式位置および姿勢検出装置。
【請求項４】請求項１記載の光学式位置および姿勢検
出装置において、前記演算手段は前記受光ユニットの２
つの光源に対する回転を検出する回転検出手段をさらに
有することを特徴とする光学式位置および姿勢検出装
置。
【請求項５】請求項４記載の光学式位置および姿勢検
出装置において、前記回転検出手段は２つの光源に対す
るある方向に対する４分割フォトディテクタの各受光部
分の差動出力差と回転角度との関係から回転角度を算出
することを特徴とする光学式位置および姿勢検出装置。