JPH04155204A - 光学式３次元位置検出装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〈産業上の利用分野〉 この発明は光学式３次元位置検出装置に間し、検出対象
である可動体に全方向光反射体を付設し、その間隔が既
知の２つの一１１足部で発光走査し、反射光の測定部へ
の入射角相当値およびこの間隔に基づいて可動体の３次
元位置を算出するに際し、最初の走査範囲を広く設定し
、可動体の検出後は、その検出位置を基準に走査範囲を
減少させることにより、可動体の３次元位置く座標）を
簡単な装置構成で、かつ追従性良く検出するものである
。

〈従来の技術〉 従来より３次元立方物の位置測定を可能にしたものとし
ては、例えば特開昭５９−２１８５３９゛号公報に示す
３次元スペースデジタイザが知られている。

このものは、ソース（直交コイル）に交流を加え、磁界
を発生させる。この磁界中にセンサ（直交コイル）を置
くとセンサに電流が誘起される。

この電流の大きさをコンピュータで処理して位置、角度
データを出力するものである。

また、従来より撮像デバイスを用いた画像処理方式位置
センサとしては特開平２−２３２６９２号公報に示すも
のが知られている。

このものは、撮像デバイスとしてＣＣＤ電荷結合デバイ
スによるカメラで光学的に撮像する。そして、この画像
を画像処理回路で処理することにより、測定対象物の運
動を非接触によって検出するものである。

〈発明が解決しようとする課題〉 しかしながら、このような従来の３次元スペースデジタ
イザにあっては、ソース側にもセンサ側にも回路が必要
であり、その構造や用途に制限があった。

また、後者に示す位置センサにあフては、装置が大型で
高価なものとなっていた。球状マーカの動きを２値化画
像処理により判定する複雑な装置構成であったからであ
る。

そこで、本発明は、その構造や用途について自由度が大
きく、小型軽量で安価なシステム構築が得られ、検出側
も比較的簡単な装置構成で、さらに可動体の運動に対す
る検出側の光走査に無駄がなく、この運動に迅速に追従
することができる光学式３次元位置検出装置を提供する
ことを、その目的としている。

〈課題を解決するための手段〉 本発明は、第１図にその構成を例示するように、３次元
空間を移動可能な可動体の３次元位置を検出する光学式
３次元位置検出装置であって、この可動体に付設された
全方向光反射体１００と、光を発する発光部、該発光部
による発光で２次元走査する走査部、および、その反射
光を受光する受光部を有し、所定位置ここ配設された第
１の測定手段２００と、光を発する発光部、この発光部
からの発光により２次元走査する走査部、および、その
反射光を受光する受光部を有し、この第１の測定手段２
００＋こ対して所定間隔離間して配設された第２の測定
手段３００と、これらの第１の測定手段２００および第
２の測定手段３００にそれぞれ入射する上記全方向光反
射体１００からの反射光の入射角相当量を検出する検出
手段４００と、これらの入射角相当量、および、上記第
１の測定手段２００と第２の測定手段３００との間の間
隔に基づいて上記全方向光反射体１００の３次元位置を
算出する算出手段５００と、上記第１の測定手段２００
および第２の測定手段３００が上記反射光を受光した後
、該受光時の反射光の入射角相当値を基準に上記２次元
走査の走査範囲を減少補正する走査範囲補正手段６００
と、を備えた光学式３次元位置検出装置である。

〈作用〉 本発明に係る光学式３次元位置検出装置は、第１および
第２の測定手段２００，３００のそれぞれは、各発光部
からの発光により可動体を２次元的に走査する。この走
査光が可動体に付設した全方向光反射体１００を捕捉す
ると、この全方向光反射体１００からの反射光がこれら
の測定手段２００．３００の各受光部に入射する。

検出手段４００は、これらの反射光の入射角相当量をそ
れぞれ検出する。

算出手段５００は、これらの入射角相当量および第１と
第２の測定手段２００．３０Ｑの間の間隔に基づいて当
該可動体の３次元位置を算出する。

そして、走査範囲補正手段６００は、上記第１の測定手
段２００および第２の測定手段３００が上記反射光を受
光した後、該受光時の反射光の入射角相当値を基準に上
記２次元走査の走査範囲を減少補正する。この結果、第
１および第２の測定手段２００，３００は発光による走
査範囲を減少させることができ、その発光走査に無駄が
なく、可動体の移動に対しても、迅速に追従することが
できる。

〈実施例〉 以下、本発明の実施例について図面を参照して説明する
。

第２図〜第９図は本発明に係る光学式３次元位置検出装
置の一実施例を示している。

第２図は本発明の一実施例に係る光学式３次元位置検出
装置の−＃Ｂ（第］の測定手段）を示している。

第２図において、１】は第１の測定手段の走査部を構成
する第１のミラーであり、１２はこの第１のミラー１２
に対向して配設された第２のミラーである。これらのミ
ラー１１．１２はその回転軸を中心にそれぞれ一定角度
範囲内で回動自在に設けられており、これらの回転軸は
互いに直交する平面にそれぞれ配設されている。また、
これらのミラー１１．１２は一定の距離だけ離れて配置
されている。

１３．１４はこれらのミラー１３．１２をそれぞれ回転
駆動するモータであり、それぞれがエンコーダ１５．１
８を付設している。エンコーダ１５．１６は、第１のミ
ラー１】および第２のミラー１２の各回転角Δθ！、△
θ２をそれぞれ検出し、デジタル信号として出力するも
のである。

１７．１８は上記各モータ１３，１４の駆動を電圧値に
よりそれぞれ制御するモータ制御回路である。

また、各エンコーダ１５，１６の出力はそれぞれデータ
ラッチ回路１９．２０に入力され、さらに制御装置２１
に入力されている。データラッチ回路１９．２０は各エ
ンコーダ１５．１６の出力である入射角相当値θ１．θ
２をラッチ可能であって、’Ｄ／Ａ変換回路２６．２７
にそれぞれ出力可能に接続されている。

Ｄ／Ａ変換回路２６．２７はスイッチＳＷ１゜ＳＷ２の
各切換側端子にそれぞれ接続されている。

これらのスイッチＳＷＩ、ＳＷ２は、制御装置２１から
の切換信号によりモータ制御回路１７，１８との接続を
、このＤ／Ａ変換回路２６．２７の出力または接地端子
と切り換えるものである。すなわち、スイッチＳＷＩ、
ＳＷ２の他方の切換側端子は接地されており、その固定
側端子はモータ制御回路１７．１８の入力側端子に接続
されているものである。

この制御装置２１は、例えば周知構成のマイクロコンピ
ュータ等により構成されるもので、演算処理、入出力制
御を行うＭＰＵ、プログラムを格納するＲＯＭ、データ
またはプログラムの一時記憶用のＲＡＭ、入出力端末と
してのＩｌｏ等を有している。

また、反射光を受光する受光素子２２の出力は、電流−
電圧変換回路＜ｒ−ｖ変換回路）２３、アンプ２４を介
して比較器２５の一方の入力端子に供給されている。ま
た、この比較器２５の他方の入力端子には反射光による
出力電圧のスレッショルドレベルを決定する基準電圧Ｖ
　ｒｅｆが印加されている。さらに、この比較器２５の
出力は、上記データラッチ回路１９．２０にそれぞれデ
ータラッチのタイミング信号として供給されている。

また、比較器２５の出力は制御装置２１にも供給されて
いる。この比較器２５の出力により制御　　゛装置２１
は、上記ＳＷ１、ＳＷ２を切り換えて、Ｄ／Ａ変換器２
６．２７の出力をモータ制御回路１７．１８に接続する
。

同時に制御装置２１は、モータ制御回路１７゜１８へ、
モータ１３，１４の回動範囲を縮小する制御信号を出力
して大振幅モードから小振幅モードに切り換える。

Ｄ／Ａ変換器２６．２７の各アナログ出力は、オフセッ
ト値としてモータ制御回路１７．１８にそれぞれ加えら
れるものである。

第３図は、３次元空間を矢印Ｘ方向に移動可能な検出対
象である可動体３１と、上記第１およ１第２の測定手段
としてのセンサ３２，３３との・。

置関係を示している。

この図に示すように、可動体３１の一端部に１全方向光
反射体３４が付設されている。

可動体３１は、空間内を上下左右に動き得る場ので、こ
の全方向光反射体３４はその動きが最冨大きな部分に取
り付けられている。この全方向シ反射体３４としては、
球形反射体、コーナキューブプリズム等入射光に対して
同じ光軸に反射す；ものを使用している。

また、上記センサ３２，３３は所定の間隔だ各層れて固
定本体３δに固定されている。各セン凭３２．３３は、
それぞれ第２図に示すように構互されており、発光、受
光機能を有している。

第４図はこれらのセンサ３２，３３をスタン）３５に固
定した場合を示している。この図は本多明の実際の適用
例を示すものであって、一対の畦ンサ３２，３３は、指
揮者の振る指揮棒（可動側３１の先端に付設した全方向
光反射体３４の位澤Ｊ　　を検出することとなる。

立 第５図に示すように、これらの測定手段（セン、、ｔ　
　サ）は、発光部として、半導体レーザ４１、コリメー
トレンズ４２を有している。また、走査部とし　　　し
ては上記ミラー１１，１２、モーター３，１４、ｂ　　
モータ制御回路１７．１８を有している。さらに、管　
　受光部としては、上記ミラー１１，１２、偏光ビー　
　−ムスブリッタ４３、受光素子２２を有して構成６　
　　されている。

したがって、半導体レーザ４１から出たレーザｔ　　光
は、コリメートレンズ４２により平行光となり、ト　　
偏光ビームスプリッタ（ＰＢ’５）４３に入射する。

に　　入射光に対してＰＢＳ４３はＰ波を通過させ、Ｓ
波を反射させるように設計されているので、Ｐ波′　　
成分のみミラー１１へ入射する。ミラー１１はこ邑　　
の入射光に対して所定の角度だけ傾斜して配置ざ′　　
れている。

；）　　　ミラー１１で反射した光は、そのミラー１１
の４　　傾斜角度に対応してミラー１２に入射する。ミ
ラー１２では入射角に対応して所定の方向に光を照射す
る。

ここで、ミラー１】とミラー１２とは、それぞれ定めら
れたスピードで、定められた角度だけ回動される構成で
ある。ミラー１１は図中矢印Ｙで示す方向に所定角度範
囲で回動するよう制御され、ミラー１２はこれに対して
軸Ｚの回りに矢印Ｗで示すように一定の角速度で回動す
るように制御されている。２次元平面での発光走査を可
能とするものである。なお、ミラー１２は例えばポリゴ
ンミラー（回転多面鏡）で構成する。

第６図（Ａ）、（Ｂ）はこのミラー１１，１２の回動に
より２次元平面上で所定の範囲で光を振る走査パターン
を光軸方向およびその平行方向から各々示している。

これらの図に示すように、この２次元走査は、例えばミ
ラー１１の回動により矢印Ｘ方向に走査するとともに、
ミラー１２の回動により矢印Ｘ方向（Ｘ方向とは直交方
向）にも走査するものであそして、この発明にあっては
、電源投入時または全方向光反射体３４位置を未だ検出
していない場合には、これらのミラー１１．１２の回動
角度範囲（振＠）を大きくする大振幅モードと、いつ、
　　たん検出した後はミラー１１，１２の回動角度範囲
をそれよりも小さくする小振幅モードと、をとり得る構
成である。図中実線りは大振幅モードの場合の走査パタ
ーンを、図中破＆！Ｍは小振幅モードのそれをそれぞれ
示すものである。

そして、この走査においてミラー１２により反射した光
が全方向光反射体３４に入射すると、その反射光は同じ
光軸（軌跡）を戻ってミラー１２、ミラー１１、そして
、ＰＢＳ４３へ入射する。

このとき、ＰＢＳ４３への入射光はＳ波になっているか
らＰＢＳ４３で反射されて角度を変えられて受光素子２
２へ入射する。

受光素子２２ではその反射光は光電変換された後、電流
−電圧変換回路２３にて電圧値として出力され、この出
力電圧は増幅器２４を経て比較器２５で基準設定値Ｖ　
ｒｅｆと比較される。

この比較の結果により受光素子２２からの出力がスレッ
シュホールドレベルを超えた場合には、比較器２５はデ
ータラッチ回路１９．２０に対してデータラッチのタイ
ミングを指定する出力をなす。すなわち、この比較器２
５からのデータラッチタイミング信号により、データラ
ッチ回路１９゜２０はエンコーダ１５，１６の各出力（
△θ１．△θ２）をラッチするものである。エンコーダ
１５゜１６はミラー１１．１２の回転角を検出し、その
値を２倍にして出力している。この２倍にした値が、出
射光と入射光との間の角度となる。

これによりデータラッチ回路１９．２０は、センサ３２
，３３が全方向光反射体３４すなわち可動体３１を検出
したときのミラー１１．１２の角度に基づいて入射角θ
１．θ２およびＣ３１ＯＡを得るものである。

これらの入射角θ１．θ２およびθ３．θ１は、制御装
置２１に入力され、この制御装置２１において当該可動
体３１の３次元位置（座標）が演算さ演算される。

また、比較器２５の出力は制御装置２１にも供給されて
いる。この比較器２５の出力により制御装置２１は、上
記スイッチＳＷ１、ＳＷ２を切り換えて、Ｄ／Ａ変換回
路２６．２７の出力をモータ制御回ｒａＩ７．１８に接
続する。

同時に制御装置２１は、モータ制御回路１７゜１８の駆
動電圧の振幅レベルを下げて大振幅モードから小振幅モ
ードに切り換える。

すなわち、受光素子２２により反射光を検出した場合に
は、ミラーＩＩ、１２による大振幅モードＬの走査を、
小振幅モードＭの走査に切り換えるものである。この場
合、Ｄ／Ａ変換回路２６゜２７の出力は、受光素子２２
の出力により逐次書き換えられてオフセット値としてモ
ータ制御回路１７．１８に出力されている。

例えば、第７図（Ａ）に示すように、可動体３１が運動
した場合、同図（Ｂ）に示すように、最初は大振幅モー
ドＬにより広い走査範囲で走査し、可動体３１を検出す
ると小振幅モードＭての走査に切り換えるものである。

そして、この小振幅モードＭにおいては、モータ制御回
路１７．１８への制御信号の指示する回動範囲の輻は大
振幅モードＬの場合に比較して小さい。ミラー１１，１
２０回動角度は小さいものとなっている。また、この小
ｆｆ１ＵモードＭにあっては、上記のようにオフセット
値が、可動体３１の運動に伴って逐次変化する。このオ
フセット値が変更される結果、可動体３１の運動にとも
なって走査範囲も変動し、可動体３１の位置を常に捕捉
しながら追従することができる。また、その場合レーザ
光の無駄な動きはなくなり、その追従の速度も向上して
いるものである。

第８図は３次元座標の演算処理の原理を説明するための
概念図である。

この図において、Ｏはセンサ３２の位置、Ｑはセンサ３
３の位置とする。これらのセンサ３２゜３３の間隔はａ
だけ離れて設置されている。そして、これらのセンサ３
２，３３の位置を結ぶ線をＸ軸、このＸ軸に垂直な線を
ｙ軸、さらにこのＸｙ平面に垂直な軸を２軸としている
。また、Ｐはこの３次元座標軸での可動体３】（正確に
は全方向光反射体３４）の位置である。

また、Ｐからｙ軸に下ろした垂線の交点をＡ、ＰからＸ
Ｚ平面に下ろした垂線の交点はＣ２このＣから２軸、Ｘ
軸への垂線の交点はＢ、　　Ｄとしている。

したがフて、可動体３１０３次元空間での座標は（ＯＤ
、ＯＡ、ＯＢ）　で表される。

ここで、上記入射角度θ１．θ２および角度θ３゜Ｃ４
は図示のようにｘｙ両軸との間の角度で表される。

以下、この実施例のＭＰｔＪ２１での可動体３１０３次
元位置算出の手ＩＩｍについて第９図のフローチャート
を参照して説明する。

まず、電源投入後、光Ｒ（センサ３２，３３の各半導体
レーザ４１）を点灯する（Ｓ９０１）。

そして、スイッチＳＷ１．ＳＷ２．ＳＷ３．ＳＷ４　（
これらのスイッチＳＷ３．ＳＷ４は第２に図示していな
いがスイッチＳＷＩ、ＳＷ２と−の構成である）を接地
側に切り換える指令をている。

次に、センサ３２のミラー１１，１２、およセンサ３３
のミラー１１．１２をともに大振幅−ドで定められた角
度範囲内で、所定の角速度回動させる指令をモータ制御
回路１７．１８に査を行うものである。

そして、受光票子２２から比較器２５を介し受光信号が
入力されるのを確認しく５９０４．９０５）、受光信号
の入力後、上記スイッチＳ゛１、ＳＷ２．ＳＷ３．ＳＷ
４をＤ／Ａ変換回路６．２７側の端子に切り換える指令
を出力する５９０６）。受光時のエンコーダ１５．１６
か・の入射角相当値をオフセット値とするものてあ。

これにより検出目標とする可動体３１の運動に５図　　
しての追従を開始するものである。

同　　　そして、各センサ３２，３３のミラー１１，１
出　　２を小振幅モードＭで定められた所定角度範囲内
で、所定角速度回動させる指令をモータ制御回路１７．
１８に出力する（５９０７）。

び、　　　その結果、レーザ光が可動体３１の全方向光
反モ　　射体３４を検出すると、センサ３２，３３の番
受で　　光素子２２はデータラッチタイミング信号を出
力圧　　し、データラッチ回路１９．２０からその検出
した位置におけるミラーの角度データ△θ１．△θ２゜
△θ３．Δθ１が人力される（５９０　Ｂ）。この場合
の可動体３１の位置は第８図のＰで示されるもて　　　
のとする。

Ｓ　　　　そして、これらの角度データの入力がすべて
完Ｗ　　了したか否かを判断する（５９０９）。

２　　　人力完了後、可動体３１のＸ座標ＯＤの算出外
（理を行う（Ｓ９１０）。これは、以下の計算式にろ　
　　　よる。

る。　　　　０Ｄ＝ａ　ｔ　ａｎθａ／（ｔａｎθ、＋
ｔａｎθ３）吋　　　次に、同じく可動体３１のＹ座標
ＯＡの算出処理を以下の式により行う（Ｓ９１１）。

０Ａ＝（ｃｏｓθ２／ＣＯ９θ＋）　−（ａｔａｒθｇ
／（ｔａｎθ＋＋ｔａｎθ３）） 更に、そのＺ座標ＯＢの算出処理を次式によ化行う（Ｓ
９１２）。

ＯＢ＝＜（ｓｉｎθ２／　ｃ　ｏ　ｓθ＋）　２−１　
）　”・ａｔａｎθ３／（ｔａｎθ１＋ｔ　ａ　ｎθ３
）そして、以上により、算出した座標値（ＯＤ。

ＯＡ、ＯＢ）を出力する（Ｓ９］　３）。出力は例えば
ＣＲ７表示、プリント出力とすることもできる。

次いて、算出処理が終了したか否かを判断する（５９１
４）。終了しない場合は、走査、検出を続行すべく、再
び上記ステップ（Ｓ９０８）に戻る。

一方、終了したら、光源を消灯し、センサ３２３３の各
ミラー１１．１２の回動停止の指令をモータ制御回路１
７．１８に出力する（！；　９１５）そして、このプロ
グラムを終了する。

以上のようにして可動体３１０３次元位置は算出される
こととなる。そして、可動体３１の運動ｌ　　　にした
がってその３次元座標も変化するがこの変化した座標に
ついても順次算出することができ、）　　　これらの座
標に基づいて可動体３１の運動パターン、運動速度等も
算出することが可能となる。

２　　　なお、各ミラー１１．１２の回転角速度、回転
角加速度も、例えば、積分等の演算処理を施すこｊ　　
　　きる。

〈発明の効果〉 以上説明してきたように、本発明に係る光学式３次元位
置検出装置によれば、検出の対象である物体側には反射
体以外のもの（回路等）を備える必要がなく、検出対象
の選択等の自由度が大きく、かつ、小型軽量で安価なシ
ステム構築が得られる。

また、検出側も比較的簡単な装置構成で、物体の３次元
位置を検出することができる。また、検出・　　対象で
ある可動体の運動に対する検出側の光走査できる。

【図面の簡単な説明】

第１図は本発明に係る光学式３次元位置検出装置の構成
を示すブロック図、 第２図は本発明の一実施例に係る光学式３次元位置検出
装置の一方の測定部の構成を示すそのブロック図、 第３図は一実施例に係る可動体とセンサとの空間での位
置間係を示す概念図、 第４図は同じく一実施例に係る光学式３次元位置検出装
置の具体的適用例を説明するための側面図、 第５図は一実施例に係るセンサの構成を示す概念図、 第６図（Ａ）、　　（Ｂ）は一実施例に係る走査パター
ンを説明するための概念図、 第７図（Ａ）は一実施例に係る可動体の運動を時間と１
次元位置との関係で示すグラフ、第７図（Ｂ）は一実施
例に係る走査部による発光走査の範囲を説明するための
モータ駆動電圧と時間との関係を示すグラフ、 第８図は一実施例に係る演算処理の原理を説明するため
の概念図、 第９図は一実施例に係る制御装置（ＭＰＵ）でのプログ
ラムを示すフローチャートである。 １１．１２・・・・・・・ミラー（走査部）、１３．１
４−・・◆・・・モータ（走査部）、１５．１６・争・
・・・争エンコーダ （検出手段）、 １７．１Ｂ・・・・・・・モータ制御回路（走査部）、 １９．２０・・・・・・・データラッチ回路（検出手段
）、 ２１・・・・・・・・・・制御装置 （算出手段、走査範囲補正手段）、 ２２・・・・・・・・・・受光素子（受光部）、３１・
・・・・・・・・・可動体、 ３２・・・・・・・・・・センサ （第１の測定手段）、 ３３・・・・・・・・・・センサ （第２の測定手段）、 ３４・・・・・・・・・・全方向光反射体、４】・・・
・・・・・・・半導体レーザ（発光部）、 ５Ｗ１−ＳＷ４・・・・・スイッチ （走査範囲補正手段）、 １００・・・・・・・・・全方向光反射体、２００・・
・・・・・・・第１の測定手段、３００・・・・・・・
・・第２の測定手段、４００・・・・・・・・・検出手
段、 ５００・・・・・・・・・算出手段、 ６００・・・・・・・・・走査範囲補正手段。

Claims (1)

1. 【特許請求の範囲】 ３次元空間を移動可能な可動体の３次元位置を検出する
   光学式３次元位置検出装置であって、この可動体に付設
   された全方向光反射体と、光を発する発光部、該発光部
   による発光で２次元走査する走査部、および、その反射
   光を受光する受光部を有し、所定位置に配設された第１
   の測定手段と、 光を発する発光部、この発光部からの発光により２次元
   走査する走査部、および、その反射光を受光する受光部
   を有し、この第１の測定手段に対して所定間隔離間して
   配設された第２の測定手段と、 これらの第１の測定手段および第２の測定手段にそれぞ
   れ入射する上記全方向光反射体からの反射光の入射角相
   当量を検出する検出手段と、これらの入射角相当量、お
   よび、上記第１の測定手段と第２の測定手段との間の間
   隔に基づいて上記全方向光反射体の３次元位置を算出す
   る算出手段と、 上記第１の測定手段および第２の測定手段が上記反射光
   を受光した後、該受光時の反射光の入射角相当値を基準
   に上記２次元走査の走査範囲を減少補正する走査範囲補
   正手段と、を備えたことを特徴とする光学式３次元位置
   検出装置。