JP2733743B2

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

Info

Publication number: JP2733743B2
Application number: JP21803594A
Authority: JP
Inventors: 亨鈴木
Original assignee: Dejitaru Sutoroomu Kk
Current assignee: Dejitaru Sutoroomu Kk
Priority date: 1994-08-19
Filing date: 1994-08-19
Publication date: 1998-03-30
Anticipated expiration: 2013-03-30
Also published as: JPH0861916A

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、光学式位置および姿勢
検出装置に関し、特に、３次元位置および姿勢を検出す
る装置に関する。

【０００２】

【従来の技術】光学式位置および姿勢検出装置が先願の
特願平５−２２５２５２号に提案されている。この先願
の出願の光学式位置および姿勢検出装置においては、３
次元の位置および姿勢を検出するために、固定側に３つ
の光源（例えば、ＬＥＤ）を同一線上にないように配置
し、１つの受光器を携帯側（可動側）に配置している。

【０００３】

【発明が解決しようとする課題】しかし、前述の特許出
願の発明においては、固定側に３つの光源を同一線上に
ならないように配置せざるをえないために、設置のしに
くさなどの使用上で不便な面があった。また、光の到達
強度比を基に位置を算出することから、計算精度を上げ
るには、「光を検出範囲内のあらゆる方向に均一な強度
で放射させなければならない」といった制約と、各光源
からの光を被検出ユニット側で受光するにあたり、「光
の入射角度による受光器面の光線密度の変化分を計算上
で補正する必要がある」といった制約とが生じていた。

【０００４】したがって、本発明の目的は、固定側ユニ
ットを直線的に形成できるため設置が容易に行うことが
でき、かつ固定側に角度検出器を設け、位置の被検出物
である携帯側に発光器を設けることによって入射光全体
としての光強度情報を必要としない光学式位置および姿
勢検出装置を提供することにある。

【０００５】

【課題を解決するための手段】前述の目的を達成するた
めに、本発明は、位置および姿勢が入力すべき機器に対
して配置された固定側ユニットと該固体側ユニットに対
して可動な携帯側ユニットから成る光学式位置および姿
勢検出装置において、前記固体側ユニットは、所定の距
離に配置されて交互に切り換えて発光される２つの光源
と、前記携帯側ユニットの光源からの光の入射角を検出
する角度検出器とを有し、前記携帯側ユニットは、１つ
の光源と、前記固体側ユニットの２つの光源からの光の
入射角を検出する角度検出器とを有し、前記２つの角度
検出器から得られた情報に基づいて位置および姿勢を算
出することを特徴とする光学式位置および姿勢検出装置
を採用するものである。

【０００６】

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

【０００７】（機械的構成）図１は、本発明の光学式位
置および姿勢検出装置を示す斜視図である。図１に示す
ように、本発明の光学式位置および姿勢検出装置は、固
定側受発光（受光および発光）ユニット（以下、固定側
ユニットという）１０と、携帯側受発光（受光および発
光）ユニット（以下、携帯側ユニットという）１２とか
ら成る。固定側ユニット１０は、携帯側ユニットの位置
および姿勢に関する情報が入力されるコンピュータ等の
装置の本体や周辺機器（例えば、キーボード、ＣＲＴな
ど）の任意の適当な位置に固定されるかまたは配置され
る。

【０００８】固定側ユニット１０は、例えば、細長い直
方体形の固定側ユニット本体１４と、この本体１４を水
平方向に支持している支持部材１６から成る。本体１４
の一方の垂直面には、所定の距離だけ離した位置に２つ
の光源１８、２０が配置されている。光源１８、２０の
各々は、例えば、複数のＬＥＤ（light emitting diode
)から成るＬＥＤアレイまたは単独のＬＥＤから成る。
なお、ここで、複数のＬＥＤを用いるのは、単独のＬＥ
Ｄでは、十分に光の強度を得ることがきない場合に、強
度を高めるためである。また、本体１４の垂直面の中央
部分には、光の角度検出器として作用する受光器２２
（以下、角度検出器という場合もある）が配置されてお
り、この受光器２２は、図３に関連して後述するが、好
ましくは、光の入射範囲を制限する開口が設けられた４
分割フォトディテクタから成るか、または任意の適当な
２次元入射角度を検出できる２次元入射角度検出器（例
えば、図５に関連して後述する５面張り付け型角度検出
器）から成ってもよい。なお、本明細書で「２次元入射
角度検出器」とは、水平角および垂直角のような２つ入
射角を１つの素子で検出できる素子をいう。なお、本体
１４内部には、図１４に関連して後述するが、情報を処
理して携帯側ユニットの位置、姿勢を算出するための電
気回路部品が配置されている。

【０００９】携帯側ユニット１２は、例えば、断面ほぼ
正方形状の直方体形の携帯側ユニット本体２４から成
り、このユニット本体２４の１つの垂直側面には、光の
角度検出器として作用する受光器２８（以下、角度検出
器という場合もある）が配置されており、この受光器２
８は、図５に関連して後述するが、好ましくは、５面張
り付け型角度検出器から成る。また、ユニット本体の上
面には、光源２６がその光の発光面を受光器２８と同一
側に向けて配置されている。この受光器２８は、固定側
ユニット１０の光源１８、２０と同様に、複数のＬＥＤ
から成るＬＥＤアレイまたは単独のＬＥＤから成るもの
である。

【００１０】（位置および姿勢の検出項目）図２は、固
定側ユニット１０に対する携帯側ユニット１２の位置お
よび姿勢の検出項目を説明するための斜視図である。図
２に示すように、本発明では、固定側ユニット１０に対
する携帯側ユニット１２の位置として、空間位置座標
（３次元位置座標）が検出でき、姿勢として、垂直方位
角、水平方位角および回転角が検出できる。

【００１１】（動作概要）固定側ユニット１０に対する
携帯側ユニット１２の位置および姿勢の検出動作は、概
略すると、以下のように行われる。（ａ）最初に、固定側ユニット１０の一方の光源１８を
点灯させ、携帯側ユニット１２の角度検出器２８（二次
元角度検出器）で光源１８からの光の入射方向を水平、
垂直方向について検出する。（ｂ）次に、固定側ユニット１０の光源１８の消灯後、
固定側ユニット１０の他方の光源２０を点灯させ、同様
に、携帯側ユニット１２の角度検出器２８で光源２０か
らの光の入射方向を水平、垂直方向について検出する。（ｃ）次に、固定側ユニット１０の光源２０の消灯後、
今度は、携帯側ユニット１２の光源２６を点灯し、固定
側ユニット１２の角度検出器２２（二次元角度検出器）
で光源２６からの光の入射方向を水平、垂直方向につい
て検出する。（ｄ）以上により、位置、姿勢を計算するのに必要なデ
ータは全て揃うことになる。その後、これらのデータを
用いて計算を行い、位置、姿勢を求める。（ｅ）その後、前述の（ａ）〜（ｄ）の動作を繰り返す
ことによって、順次、その時点での位置と姿勢が求めら
れる。

【００１２】（光線入射角算出方法）（１）４分割フォトディテクタ図３は、角度検出器（二次元角度検出器）として用いる
４分割フォトディテクタを説明するための図である。４
分割フォトディテクタ２２ｂは、例えば、４分割ピン
（ＰＩＮ）フォトダイオードのような上下左右に４分割
された部分（図３ｃのａ〜ｄで示すような部分）を有
し、各部分が入射した光の光量を個々に（独立して）検
出するものである。この４分割フォトディテクタ２２ｂ
は、ケース２２ａ内に配置され、ケース２２ａの前方に
は、絞り機能を持つ、例えば、図示のような円形の開口
２２ｃ（または、正方形の開口でもよい）が形成されて
いる。外部光源、例えば、光源２６からの光はその開口
２２ｃを通過した後、フォトディテクタの各部に当たる
ことになる。

【００１３】次に、角度検出の原理を説明すると、図３
ａに示すように、１つの光源２６と受光部２２の位置関
係によって、受光部の向いている方向と光源の方向が異
なるときには、光が受光部に対して傾いて入射してく
る。このときの光が受光部にあたる様子の一例を図３ｃ
に示す。

【００１４】開口の形状で通過してきた光の、４分割フ
ォトディテクタ上に結ぶ像の中心が開口の中心よりずれ
ることより、各フォトディテクタの各部分の間に出力差
が生じる。例えば、各部分ａ、ｂ、ｃ、ｄで得られる出
力値をａ、ｂ、ｃ、ｄとすると、図３ｃの場合には、
（ａ＋ｂ）＜（ｃ＋ｄ）となり、出力差を差動回路等で
取り出すこととすると、光量差に比例した電流もしくは
電圧を得ることができる。

【００１５】図４は、前述のような場合における差動出
力と光線入射角度の関係を示すグラフである。この関係
に基づいて、差動出力から光線入射角度を決定すること
ができる。

【００１６】今、前述のように、光の入射方向に関し
て、（ａ＋ｂ）と（ｃ＋ｄ）における光量比較で、例え
ば水平方向の入射角度が分かるとすれば、（ａ＋ｃ）と
（ｂ＋ｄ）における光量比較で、垂直方向の光の入射角
度が分かることになる。

【００１７】（２）５面張り付け型角度検出器図５は、５面張り付け型角度検出器（二次元角度検出
器）を説明するための図であり、図５ａは、５面張り付
け型角度検出器の平面図であり、図５ｂは、５面張り付
け型角度検出器の断面図である。図５ａに示すように、
この形式の角度検出器２８は、例えば、４角錐台形のベ
ース２８ｆの５つの各表面に同一面積のフォトディテク
タである例えばフォトダイオードチップ２８ａ〜２８ｅ
を張り付けたものである。図５ｂを参照して、この角度
検出器を用いたときの角度検出の原理を説明すると、原
理は、一言でいえば、「光源からの光の当たるところに
ある頂角を挟んだ両側のフォトディテクタに入射する光
の光量差を基に光の入射角を求める」ことである。

【００１８】今、図５ｂに示すように、頂角の２等分線
に垂直な線とフォトディテクタの取付け面とのなす角度
をβ、光源と頂角の２等分線とのなす角度をθとおく
と、各フォトディテクタからの出力は光の入射方向に対
するフォトディテクタの投影面積に比例することから、
以下の式が得られる。フォトディテクタ２８ａの出力∝ｃｏｓ（θ−β）フォトディテクタ２８ｂの出力∝ｃｏｓ（θ＋β）全出力で規格化したフォトディテクタ２８ａ、２８ｂ間
の差動出力を「Ｄ（ａ−ｂ）／（ａ＋ｂ）」とすると、Ｄ＝｛ｃｏｓ（θ−β）−ｃｏｓ（θ＋β）｝／｛ｃｏｓ（θ−β）＋ｃｏｓ（ θ＋β）｝＝ｔａｎθ・ｔａｎβ よって、θ＝ａｒｃｔａｎ（Ｄ／ｔａｎβ）・・・・・・（１）

【００１９】以上のようにして、２つのフォトディテク
タ２８ａ、２８ｂの全出力で規格化した差動出力から光
源２２に対して角度検出器２８がなす角度が求められる
ことになる。図５ａから分かるように、例えば、水平方
向に関しては、フォトディテクタ２８ａ、２８ｂ、２８
ｃに対して光が入射する方向によって、フォトディテク
タ２８ａ、２８ｂの組み合わせ、またはフォトディテク
タ２８ｂ、２８ｃの組み合わせのいずれかを用いて角度
を検出でき、したがって、広い範囲の入射角度を検出で
きる。また、同様に、垂直方向に関しては、フォトディ
テクタ２８ｄ、２８ｂ、２８ｅに対して光が入射する方
向によって、フォトディテクタ２８ｄ、２８ｂの組み合
わせ、またはフォトディテクタ２８ｂ、２８ｅの組み合
わせのいずれかを用いて角度を検出でき、したがって、
同様に広い範囲の入射角度を検出できる。

【００２０】本発明において、光の入射角度検出に対し
て上記の２つの構成のいずれを使用するかは、この光学
式位置および姿勢検出装置をどのような用途に用いるか
によって決まるものであり、この実施例で示すように、
固定側ユニットには４分割フォトディテクタを用い、携
帯側ユニットには５面張り付け型角度検出器を用いるこ
とに限定されるものではない。

【００２１】４分割フォトディテクタは、部品が製作し
易く、開口とフォトディテクタの間の距離を変えること
で角度検出感度の設定が容易にでき、検出範囲は狭くな
るが、感度を高めやすいといった特徴がある。一方、５
面張り付け型角度検出器では、感度を一定に保ったまま
角度の検出範囲を広くとれるといった特徴がある。結局
のところ、別の角度検出装置を含め、用途に適した角度
検出装置を選べばよく、それぞれの特徴を生かすこと
で、種々の機器への対応が可能である。

【００２２】（空間座標計算方法）図６は、空間座標を
説明するための図面である。図６ａに示すように、最初
に、固定側ユニットの２つの光源を結ぶ線と携帯側ユニ
ットとを含む平面を考え、その平面をＸ’Ｚ’平面とす
る。固定側ユニットをＸ’軸上に置き、角度検出器２２
の位置を原点とし、２つの光源１８、２０がそれぞれ
（＋ｄ，０）座標位置と（−ｄ，０）座標位置にあると
する。携帯側ユニットは（Ｘ’ｐ，Ｚ’ｐ）座標位置に
あるとする。なお、ここで、”Ｘ’ｐ”、”Ｚ’ｐ”
は、実際の座標（Ｘ，Ｙ，Ｚ）上の値ではないことに留
意すべきである。

【００２３】ここで、既知の値と角度検出によって求め
られている値は、図６ｂに示す図において、ｄ、θ、φ
（φ：光源２０と角度検出器２８を結ぶ直線と光源１８
と角度検出器２８を結ぶ直線がなす角度、即ち、光源１
８から光の入射角と光源２０らの光の入射角との差）で
あり、求めようする値は、原点より”Ｘ’ｐ”、”Ｚ’
ｐ”までの距離”Ｌ”である。

【００２４】図６ｃに示すように、原点から（Ｘ’ｐ，
Ｚ’ｐ）までの距離をＬとし、Ｌによって分割されるφ
の一方をφ１、他方をφ２とし、θの補角をαとする
と、以下の関係式が成り立つ。ｄ・ｃｏｓα＋ｄ・ｓｉｎα／ｔａｎφ１＝Ｌ・・・・（２） −ｄ・ｃｏｓα＋ｄ・ｓｉｎα／ｔａｎφ２＝Ｌ・・・・（３）（２）式より、 φ１＝ａｒｃｔａｎ（ｄ・ｓｉｎα／（Ｌ−ｄ・ｃｏｓα））同様に、（３）式より、 φ２＝ａｒｃｔａｎ（ｄ・ｓｉｎα／（Ｌ＋ｄ・ｃｏｓα））よって、 φ１＋φ２＝φ＝ａｒｃｔａｎ（ｄ・ｓｉｎα／（Ｌ−ｄ・ｃｏｓα））＋ａｒｃｔａｎ（ｄ・ｓｉｎα／（Ｌ＋ｄ・ｃｏｓα））・・（５）定義から明らかなように、α＝π／２−θであるから、
（５）式により、Ｌとφの関係が求まったことになる。

【００２５】前述のように、Ｌが決まれば、Ｘ’Ｚ’平
面上の座標値（Ｘ’ｐ，Ｚ’ｐ）は、Ｘ’ｐ＝Ｌ・ｃｏ
ｓθ、Ｚ’ｐ＝Ｌ・ｓｉｎθの関係によって計算され、
携帯側ユニットのＸ’Ｚ’平面上の位置が得られること
になるが、（５）式の形を見る限り、θとφからＬを直
接計算するのは困難である。そこで、実際の計算では、
マイクロコンピュータで行うことを念頭におき、精度が
必要な場合には収束計算でＬを求め、精度が必要でない
場合には近似計算でＬを求める。

【００２６】（１）収束計算法（５）式を直接利用せず、Ｚ’軸とθの角度をなす直線
を考え（Ｌはこの中に含まれる）、最初に直線上の適当
な位置に任意の点（Ｘ’ｔ，Ｚ’ｔ）をとる。この点と
（＋ｄ，０）、（−ｄ，０）がつくる三角形において、
（Ｘ’ｔ，Ｚ’ｔ）位置の頂角を求め、φとの比較を行
う。なお、この頂角は、以下の式で与えられる。ａｒｃｔａｎ｛（ｄ−Ｘ’ｔ）／Ｚ’ｔ｝＋ａｒｃｔａ
ｎ（ｄ＋Ｘ’ｔ）／Ｚ’ｔ｝

【００２７】比較の結果、φの方が小さければ、（Ｘ’
ｔ，Ｚ’ｔ）を原点からより遠くに設定し、大きい場合
にはより近くに設定し直して同様な計算を行う。この作
業を繰り返し、予め決めた誤差範囲に入れば計算を終了
する。この時点での（Ｘ’ｔ，Ｚ’ｔ）が（Ｘ’ｐ，
Ｚ’ｐ）となる。なお、原点から求めた位置までの距離
としてのＬは、以下の式で与えられる。Ｌ＝√（Ｘ’ｐ²＋Ｚ’ｐ²）・・・・・・・（６）

【００２８】（２）近似計算法 φ１およびφ２の絶対値が小さいと仮定して、ｔａｎφ
１≒φ１、ｔａｎφ２≒φ２で近似できるとする。
（５）式から、 φ１＋φ２＝φ≒ｄ・ｓｉｎα／（Ｌ−ｄ・ｃｏｓα）
＋ｄ・ｓｉｎα／（Ｌ＋ｄ・ｃｏｓα）＝２・Ｌ・ｄ・
ｓｉｎα／｛Ｌ²−（ｄ・ｃｏｓα）²｝したがって、 φ・Ｌ²−２・ｄ・ｓｉｎα・Ｌ−φ・（ｄ・ｃｏｓ
α）²＝０したがって、Ｌ＝｛ｄ・ｓｉｎα±√［（ｄ・ｓｉｎα）²＋（φ・
ｄ・ｃｏｓα）²］｝／φ ここで、φの値はある程度小さいとし、平方根内の第２
項を無視すると、Ｌ≒（ｄ・ｓｉｎα±ｄ・ｓｉｎα）／φ −符号は無意味であるから、以下の（７）式が得られ
る。Ｌ≒２・ｄ・ｓｉｎα／φ＝２・ｄ・ｃｏｓθ／φ ・・・・（７）加えて，座標値としては、以下のような結果になる。Ｘ’ｐ＝２・ｄ・ｓｉｎθ・ｃｏｓθ／φ ・・・・・（８）Ｚ’ｐ＝２・ｄ・（ｃｏｓθ）²／φ ・・・・・（９）

【００２９】これで準備が整ったので、最終的に求めよ
うとしている、実際の座標軸上での携帯側ユニットの座
標（Ｘｐ，Ｙｐ，Ｚｐ）を導出する。ここまでに、
（６）式または（７）式から、原点から携帯側までの距
離Ｌが求まっていることに留意すべきである。

【００３０】図７は、本発明の光学式位置および姿勢検
出装置をＸ軸方向に見た側面図である。図７に示す位置
関係から容易に理解できるように、携帯側ユニット１２
の座標（Ｘｐ，Ｙｐ，Ｚｐ）値は、距離Ｌをベクトルと
考えたときのＹ方向（垂直方向）の角度ｖによって、以
下の（１０）式に示すＹ座標値が最初に求まる。Ｙｐ＝Ｌ・ｓｉｎｖ・・・・・・・・（１０）Ｘ、Ｚ座標値Ｘｐ、ＺｐはベクトルＬをＸＺ平面に投影
した場合の先端座標であることから、以下の（１１）
式、（１２）式で与えられる。Ｘｐ＝Ｌ・ｃｏｓｖ・ｓｉｎθ ・・・・・（１１）Ｚｐ＝Ｌ・ｃｏｓｖ・ｃｏｓθ ・・・・・（１２）以上のようにして、（１０）〜（１２）式によって、携
帯側ユニットの３次元空間座標を得ることができる。

【００３１】（姿勢計算方法）前述の（光線入射角算出
方法）において、入射してくる光の水平、垂直方向が検
出できることを明らかにした。携帯側ユニットの角度検
出器より求めたこれらの角度は、携帯側ユニットの回転
角が０であれば、そのまま携帯側ユニットの水平、垂直
方向を表す。ただし、固定側ユニットの光源は２つある
ので、携帯側ユニットを水平方向に配置し、その中間点
を原点とすれば、各々の光源に対する水平角（Ｈ１、Ｈ
２とする）の平均値（Ｈ１＋Ｈ２）／２が原点に対して
の水平角となる。垂直角に関しては、原理的に各々の光
源から求めた値は同一（Ｖ１＝Ｖ２）になるはずであ
る。逆に同一でない場合には、携帯側ユニットが回転し
ていることを意味し、その差を利用して回転角を求める
ことになる。

【００３２】（１）回転角図８は、回転角を計算する方法を説明するための図面で
ある。図８において、２つの固定側ユニットの光源１
８、２０と携帯側ユニットを含む平面をＸＺ平面とし、
固定側ユニットの２つの光源を結ぶ線を水平方向（Ｘ軸
方向）、Ｙ方向を垂直方向とする。また、図８上に示し
ているように携帯側ユニットに依存した局部的な座標系
（Ｘ’，Ｙ’，Ｚ’）をとっておく。この座標系のＺ’
軸は基準の座標系のＺ軸と同一とし、Ｘ’、Ｙ’軸は携
帯側ユニットの回転に伴い、Ｚ軸を中心に回転するもの
とする。また、携帯側ユニット内の角度検出器として、
図１に示す実施例では、５面張り付け型角度検出器を用
いているが、４分割フォトディテクタを用いていること
もできるので、以下に示す図９において、５面張り付け
型角度検出器を用いる場合について説明し、図１０にお
いて、４分割フォトディテクタを用いる場合について説
明する。

【００３３】さて、座標系に対して携帯側ユニットが回
転角を持たない場合は、角度検出器の受光パターンは図
９ａ、図１０ａに示すようであり、固定側ユニットの各
々の光源に対する入射角を求めても垂直方向に関しては
角度差は生じない。即ち、図９の５面張り付け型角度検
出器の例では、例えば２箇所の光源からの５面張り付け
型角度検出器の中央部分の受光量をそれぞれａ、ａ’と
し、図示上その上の部分の受光量をｂ、ｂ’とすると、
ｂ／ａ＝ｂ’／ａ’となる。また図１０の４分割フォト
ディテクタの例では、例えば、２つの４分割フォトディ
テクタの各部分の受光量をそれぞれ、ａ、ｂ、ｃ、ｄお
よびａ’、ｂ’、ｃ’、ｄ’とすると、（ａ＋ｃ）−
（ｂ＋ｄ）＝（ａ’＋ｃ’）−（ｂ’＋ｄ’）である。

【００３４】しかし、回転角を持つと、図９ｂ、図１０
ｂのような受光パターンとなることから、図９ｂ、図１
０にそれぞれ示した通り、光源１８に対する垂直方向の
角度と光源２０に対する垂直方向の角度との間には差が
生じてくる。即ち、図９ｂの場合には、ｂ／ａ＜ｂ’／
ａ’となり、図１０ｂの場合には、（ａ＋ｃ）−（ｂ＋
ｄ）＞（ａ’＋ｃ’）−（ｂ’＋ｄ’）となる。

【００３５】なお、以下の説明は４分割フォトディテク
タに関しのみを行うが、５面張り付け型角度検出器の場
合にも同様に成り立つものである。携帯側ユニットを基
準に見方を変えると、図１１に示すように、あたかも光
源間に高低差（ｈ）が生じたものとみなすこともでき
る。この考え方を進めることで、図１２に示した、１回
転３６０°内を４つに区切った領域において、携帯側ユ
ニットが今どの領域に入る回転角となっているのかを判
断することが可能となる。即ち、２つの光源に対してそ
れぞれ水平角、垂直角を求めた場合、検出上の水平角間
の差よりも垂直角間の差の方が大きい状況であれば、携
帯側ユニットの回転状態は図１２の領域ＩＩまたはＩＶ
中にあることになる。さらに、領域ＩＩまたはＩＶのい
ずれであるかを特定するには、光源１８と光源２０との
位置関係を基に判断する。即ち、固定側ユニットの光源
１８、２０を順に点灯し、携帯側ユニットでそれぞれの
入射角を求めるのであるが、光源１８に対する垂直角Ｖ
１が光源２０に対しての垂直角Ｖ２より大きいと、携帯
側ユニットを基準として見た光源の位置関係は、図１１
のように、光源１８の方が上にきていることを意味す
る。従って、この場合は、携帯側ユニットは領域ＩＩ内
の回転状態に置かれていると判断できる。このように各
角度間の大小関係等の判定を行うことで、携帯側ユニッ
トの回転状況がどの領域に属しているかを特定すること
が可能である。

【００３６】次に、段階として、特定された各領域の中
において、回転角度は一体何度なのかを求めることにな
る。それにはまず、領域が分かったことで、それに合わ
せて水平、垂直角を定義しなおす必要がある。例えば、
領域ＩＩ内に携帯側ユニットの回転状態があるとする
と、基準の座標系と携帯側ユニットに属した座標系とで
は、９０°前後の回転ずれが存在することになり、４分
割フォトディテクタの差分でとる水平、垂直の意味は、
基準の座標系においては、垂直、水平方向と、逆の意味
合いとなることが起きる。このように領域によって、水
平、垂直方向のつじつまを合わせておかなければならな
い。

【００３７】さて、領域により定義し直した各角度（Ｈ
１、Ｖ１、Ｈ２、Ｖ２）をもとに以下の式で±４５°内
の回転角度”ｒｏｔ”が求まる。ｒｏｔ＝ａｒｃｔａｎ｛（Ｖ１−Ｖ２）／（Ｈ１−Ｈ２）｝・・（１３）この式の意味は、「（Ｈ１−Ｈ２）が回転の方向を表す
ベクトルの水平成分、（Ｖ１−Ｖ２）が垂直成分となる
ことで、そのベクトルの水平方向からの傾きを計算し、
回転位置を得る」といったことである。

【００３８】以上のように、特定された領域中の回転角
度が決まったことで、結局、携帯側ユニットの回転が３
６０°内のどの角度にあるのかが計算できたことにな
る。

【００３９】図１３は、各領域の決定と領域決定後の回
転角算出を行う動作のフローチャートである。各領域の
決定と領域決定後の回転角算出は以下のように行われ
る。最初、回転角を０とし、２つの光源からの水平入射
角、垂直入射角をそれぞれ求める。そして、求めた水平
入射角および垂直入射角をそれぞれＨ１、Ｈ２、Ｖ１、
Ｖ２とするとき、水平入射角の差ＣＨ（＝Ｈ１−Ｈ２）
と垂直入射角の差ＣＶ（＝Ｖ１−Ｖ２）を求める（ステ
ップＳ１）。

【００４０】次に、ＣＶがＣＨの絶対値より大きいか否
かを判定し（ステップＳ２）、大きい場合には、回転角
は領域ＩＩにあり、水平、垂直角の再計算を行う（ステ
ップＳ３）。ステップＳ２で、ＣＶがＣＨの絶対値より
小さいと判断されると、−ＣＶがＣＨの絶対値より大き
いか否かを判定し（ステップＳ４）、大きい場合には、
回転角は領域ＩＶにあり、水平、垂直角の再計算を行う
（ステップＳ５）。ステップＳ４で、−ＣＶがＣＨの絶
対値より小さいと判断されると、ＣＨが零より小さいか
否かが判定され（ステップＳ６）、小さい場合、回転角
は領域ＩＩＩにあり水平、垂直角の再計算を行う（ステ
ップＳ７）。ステップＳ６で、ＣＨが零より大きいと判
断されると、回転角は領域１にある。ステップＳ３、
５、７の後、およびステップＳ６で否と判定された後
は、回転角の算出を行う（ステップＳ８）。

【００４１】（２）水平方位角、垂直方位角（１）の回転角の計算で扱ってきた「水平角」、「垂直
角」は、基本的には、携帯側ユニット上の座標系におけ
る角度であったため、本来の姿勢を表現する目的を実現
するために、基準座標系から見た「水平角」、「垂直
角」の数字に変換しなければならない。携帯側ユニット
の基準系に対する回転角「ｒｏｔ」が（１３）式より求
まっているので、図１４ａから図１４ｂへのように、Ｚ
軸を中心とした座標の回転変換を行う。具体的には、図
１４ｃに示すように、携帯側ユニットの座標系における
「水平角」、「垂直角」を表す単位方向ベクトル成分を
基準座標系から見た成分で表現し直す。

【００４２】携帯側ユニットの座標系から見た、自分自
身の水平方向角度、垂直方向角度をそれぞれ「Ｈ’」、
「Ｖ’」とすると、携帯側ユニットの座標系単位ベクト
ルは［ｓｉｎ（Ｈ’），ｓｉｎ（Ｖ’），√｛１─ｓｉ
ｎ（Ｈ’）²−ｓｉｎ（Ｖ’）²｝で表される。Ｘｈ’
＝ｓｉｎ（Ｈ’）、Ｙｈ’＝ｓｉｎ（Ｖ’）と置き、
「Ｘｈ」、「Ｙｈ」を基準座標系においての対応する単
位ベクトル成分とすれば、角度「ｒｏｔ］の回転変換に
より、Ｘｈ＝Ｘｈ’・ｃｏｓ（ｒｏｔ）＋Ｙｈ’・ｓｉｎ（ｒ
ｏｔ）Ｙｈ＝−Ｘｈ’・ｓｉｎ（ｒｏｔ）＋Ｙｈ’・ｃｏｓ
（ｒｏｔ）となる。

【００４３】よって、基準系からみた水平方向角度、垂
直方向角度をＨ、Ｖで表せば、Ｘｈ＝ｓｉｎ（Ｈ）、Ｙ
ｈ＝ｓｉｎ（Ｖ）の関係から、以下の式が得られる。Ｈ＝ａｒｃｓｉｎ（Ｘｈ）・・・・（１３）Ｖ＝ａｒｃｓｉｎ（Ｙｈ）・・・・（１４）以上により、携帯側ユニットの水平、垂直方向の向きが
求められる。

【００４４】（３次元位置座標および姿勢の算出手順）
本発明の光学式位置および姿勢検出装置の動作全体を容
易に理解し易いように、今まで逐次説明してきた３次元
の位置座標および姿勢の算出方法を要約すると以下の通
りである。〔Ｉ〕各光源の点灯と光の入射角の検出（ａ）動作：固定側ユニットの一方の光源１８の点灯と角度検
出内容：携帯側ユニットの角度検出器２８で固定側ユニッ
トの光源１８からの光の入射角を水平、垂直方向につい
て検出する。検出値：Ｈ’１、Ｖ’１（ｂ）動作：固定側ユニットの他方の光源２０の点灯と角度検
出内容：携帯側ユニットの角度検出器２８で固定側ユニッ
トの光源２０からの光の入射角を水平、垂直方向につい
て検出する。検出値：Ｈ’２、Ｖ’２（ｃ）動作：携帯側ユニットの光源２６の点灯と角度検出内容：固定側ユニットの角度検出器２２で携帯側ユニッ
トの光源２６からの光の入射角を水平、垂直方向につい
て検出検出値：Ｈ３、Ｖ３〔ＩＩ〕座標、姿勢の計算（ａ）携帯ユニットの回転角の計算内容：Ｈ’１、Ｖ’１、Ｈ’２、Ｖ’２より、基準座標
系（固定側ユニットの座標系）に対して携帯側ユニット
の回転角を算出する。算出値：ｒｏｔ（ｂ）携帯側ユニットの固定側ユニットの各光源に対す
る水平角、垂直角の補正計算内容：回転角（ｒｏｔ）分の補正をＨ’１、Ｖ’１、
Ｈ’２、Ｖ’２に対して行い、基準座標系における水平
角、垂直角にそれぞれを変換する。算出値：Ｈ１、Ｖ１、Ｈ２、Ｖ２（ｃ）携帯側ユニットの基準座標系においての水平角、
垂直角の計算内容：Ｈ１、Ｖ１、Ｈ２、Ｖ２の平均化によって、携帯
側ユニットの基準系における水平角、垂直角を最終決定
する。算出値：Ｈ、Ｖ（ｄ）携帯側ユニットの座標計算内容：携帯側ユニットの角度検出器と固定側ユニットの
それぞれの光源の３点を頂点とする三角形に関し、補正
後のＨ１、Ｈ２から携帯側ユニットの位置の頂角の角度
φを算出し、その値と以前に検出ずみのＨ３、Ｖ３か
ら、携帯側ユニットの空間位置座標を算出する。算出値：Ｘ、Ｙ、Ｚ以上の手順により、携帯側ユニットの３次元の位置座標
（Ｘ、Ｙ、Ｚ）および姿勢（水平方位角Ｈ、垂直方位角
Ｖ、回転角ｒｏｔ）を求めることができる。

【００４５】（回路構成）図１５は、本発明の光学式位
置および姿勢検出装置の回路ブロック図である。図１５
に示すように、携帯側ユニットは、その内部に、光源２
６を点灯させる際の中央演算処理ユニット５６からの信
号をバッファー増幅するバッファアンプ３４と、フォト
ディテクタ（角度検出器）２８からの信号を電流−電圧
変換する電流−電圧変換アンプ３０と、変換された信号
を演算する演算器３２を有する。

【００４６】固定側ユニットは、その内部に、光源１
８、２０を点灯させる際の中央演算処理ユニット５６か
らの信号をバッファー増幅するバッファアンプ５４と、
フォトディテクタ（角度検出器）２２からの信号を電流
−電圧変換する電流−電圧変換アンプ５０と、変換され
た信号を演算する演算器５２を有する。固定側ユニット
は、さらに、演算器３２、５２からの信号を切り換える
データ切り替器４０と、切り替えた信号を処理する一連
の素子、バンドパスフィルタ４２、ゲイン調整器４４、
検波回路４６、ローパスフィルタ４８を有し、これらの
信号をＡ／Ｄ変換するＡ／Ｄ変換器を備えた中央演算処
理ユニット５６を有する。中央演算処理ユニット５６
は、今まで説明してきた位置および姿勢算出のための演
算を行う。

【００４７】

【発明の効果】以上説明したように、本発明によれば、
固定側ユニットを直線的に形成できるため設置が容易に
行うことができ、かつ固定側に角度検出器を設け、位置
の被検出物である携帯側に発光器を設けることによって
入射光全体としての光強度情報を必要としない光学式位
置および姿勢検出装置が得られる。

【図面の簡単な説明】

【図１】図１は、本発明の光学式位置および姿勢検出装
置を示す斜視図である。

【図２】図２は、固定側ユニットに対する携帯側ユニッ
トの位置および姿勢の検出項目を説明するための斜視図
である。

【図３】図３は、角度検出器として用いる４分割フォト
ディテクタを説明するための図である。

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

【図５】図５は、５面張り付け型角度検出器（二次元角
度検出器）を説明するための図である。

【図６】図６は、空間座標を説明するための図面であ
る。

【図７】図７は、本発明の光学式位置および姿勢検出装
置をＸ軸方向に見た側面図である。

【図８】図８は、回転角を計算する方法を説明するため
の図面である。

【図９】図９は、５面張り付け型角度検出器における回
転角を計算する方法を説明するための図面である。

【図１０】図１０は、４分割フォトディテクタにおける
回転角を計算する方法を説明するための図面である。

【図１１】図１１は、回転角を計算する方法を説明する
ための図面である。

【図１２】図１２は、回転角を計算する方法を説明する
ための図面である。

【図１３】図１３は、回転角を計算における動作のフロ
ーチャートである。

【図１４】図１４は、座標軸変換を説明するための図で
ある。

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

【符号の説明】

１０固定側ユニット１２携帯側ユニット１８光源２０光源２２フォトディテクタ（角度検出器）２６光源２８フォトディテクタ（角度検出器）

Claims

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

【請求項１】位置および姿勢が入力すべき機器に対し
て配置された固定側ユニットと該固体側ユニットに対し
て可動な携帯側ユニットから成る光学式位置および姿勢
検出装置において、前記固体側ユニットは、所定の距離
に配置されて交互に切り換えて発光される２つの光源
と、前記携帯側ユニットの光源からの光の入射角を検出
する角度検出器とを有し、前記携帯側ユニットは、１つ
の光源と、前記固体側ユニットの２つの光源からの光の
入射角を検出する角度検出器とを有し、前記２つの角度
検出器から得られた情報に基づいて位置および姿勢を算
出することを特徴とする光学式位置および姿勢検出装
置。
【請求項２】請求項１記載の光学式位置および姿勢検
出装置において、前記光源の各々は、１つの単独の発光
素子または複数の発光素子のアレイから成ることを特徴
とする光学式位置および姿勢検出装置。
【請求項３】請求項１記載の光学式位置および姿勢検
出装置において、前記角度検出器は２次元角度検出器か
ら成ることを特徴とする光学式位置および姿勢検出装
置。
【請求項４】請求項３記載の光学式位置および姿勢検
出装置において、前記２次元角度検出器は、光源からの
光の水平入射角および垂直入射角を検出することを特徴
とする光学式位置および姿勢検出装置。
【請求項５】請求項４記載の光学式位置および姿勢検
出装置において、前記固定側ユニットの角度検出器は、
上下左右に４つの単独に光を検出する部分を持ちかつ光
の入射を制限する開口が前方に配置された４分割フォト
ディテクタであり、前記携帯側ユニットの角度検出器
は、４角錐台形の５つの面にフォトディテクタを配置し
た５面張り付け型角度検出器であることを特徴とする光
学式位置および姿勢検出装置。
【請求項６】請求項１乃至４のいずれか１つに記載の
光学式位置および姿勢検出装置において、位置および姿
勢として３次元の位置（空間座標）および姿勢（水平方
位角、垂直方位角、回転）が検出されることを特徴とす
る光学式位置および姿勢検出装置。
【請求項７】位置および姿勢が入力すべき機器に対し
て配置された固定側ユニットと該固体側ユニットに対し
て可動な携帯側ユニットから成る光学式位置および姿勢
検出装置において、前記固体側ユニットは、所定の距離
に配置されて交互に切り換えて発光される２つの光源
と、前記携帯側ユニットの光源からの光の入射角を検出
する角度検出器とを有し、前記携帯側ユニットは、１つ
の光源と、前記固体側ユニットの２つの光源からの光の
入射角を検出する角度検出器とを有し、前記固定側ユニ
ットの角度検出器は、前記携帯側ユニットの光源からの
光の水平角および垂直角を検出し、前記携帯側ユニット
の角度検出器は、前記固定側ユニットの２つの光源の各
々からの光の水平角および垂直角を検出し、前記角度検
出器で検出した水平角および垂直角に基づいて３次元の
位置および姿勢を算出することを特徴とする光学式位置
および姿勢検出装置。
【請求項８】請求項７記載の光学式位置および姿勢検
出装置において、携帯側ユニットの座標系を固定側ユニ
ットの座標系に変換する手段をさらに有することを特徴
とする光学式位置および姿勢検出装置。
【請求項９】請求項７記載の光学式位置および姿勢検
出装置において、前記固定側ユニットに対する前記携帯
側ユニットの回転を領域に分けて判断する手段をさらに
有することを特徴とする光学式位置および姿勢検出装
置。