JP2020186981A - 位置計測システム及び位置計測方法 - Google Patents

Abstract

【課題】トラッカーの位置トラッキングを同一座標系で確実に得て、トラッカーに取り付けられた計測用プローブの先端３次元位置を同一座標系で計測することができる位置計測システムを提供すること。【解決手段】光照射部１０と、光照射部１０から出力されたレーザ光を受光して自身の位置及び姿勢を検出するトラッカー３０と、トラッカー３０に取り付けられた計測用プローブ３と、光照射部１０から出力されたレーザ光を受光して自身の位置を検出する補助トラッカー２０と、光照射部１０の基準座標系ＧＷに対する補助トラッカー２０の初期の位置Ｂとトラッカー３０の位置Ａとを取得し、トラッカー３０の位置Ａを、補助トラッカー２０の初期の位置Ｂを原点とする相対基準座標系ＧＢに対する位置に座標変換するとともに、トラッカー３０の姿勢をもとに相対基準座標系ＧＢにおける計測用プローブ３の先端３次元位置ＰＡを演算する制御部とを備える。【選択図】図１

Description

本発明は、トラッカーの位置トラッキングを同一座標系で確実に得て、前記トラッカーに取り付けられた計測用プローブの先端３次元位置を同一座標系で計測することができる位置計測システム及び位置計測方法に関する。

ＶＲ（Ｖｉｒｔｕａｌ Ｒｅａｌｉｔｙ）の位置トラッキングは、ヘッドマウントディスプレイなどの機器にカメラやセンサを取り付け、周囲の測定物を検出して相対的な動きをとるインサイドアウト（ｉｎｓｉｄｅ−ｏｕｔ）方式と、外部にカメラを置いて上記機器の位置を測定するアウトサイドイン（ｏｕｔｓｉｄｅ−ｉｎ）方式とがある。アウトサイドイン方式では、例えば赤外線カメラを用いたセンサが移動する機器のマーカに実装されたＬＥＤ等が発する赤外線を撮像し、画像処理を行うことによってマーカの位置を検出している。

特許文献１には、カメラによってマーカを検出してマーカの位置を観測するＡＲ（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ）装置であって、連続したフレーム間での追跡が遮蔽やカメラアングルの急な変更などで中断した場合、照合処理を行うことで、離れたフレーム間で対応付けが行われるものが開示されている。

アウトサイドイン方式には、赤外レーザ光を用い、カメラに替えて赤外線レーザビームを照射する光照射部を備え、移動するマーカに対応するトラッカーに複数の光検出部を設け、光検出部が検出したレーザ光の受光時間や各光検出部の受光角度、各検出部の受光時間差をもとにトラッカーの位置及び姿勢を演算するものがある。また、トラッカーに複数の再帰反射部を設け、光照射部側に設けた光受光部がトラッカーからの反射光を受光し、反射光の時間差、角度、あるいは変調位相差をもとにトラッカーの位置及び姿勢を演算するものも考えられている。

特開２０１３−１４１０４９号公報

ところで、例えば、トラッカーの位置トラッキングでは、トラッカーが遮蔽された位置に移動するとトラッカーの位置を検出することができない。また、トラッカーが光照射部の照射領域外に移動した場合もトラッカーの位置を検出できない。このため、光照射部を移動させ、移動したトラッカーが光照射部の照射領域に含まれるようにする。

しかしながら、移動前のトラッカーの位置と移動後のトラッカーの位置とは、異なる光照射部の座標系に対するものとなるため、移動後のトラッカーの位置を移動前のトラッカーの位置に座標変換する必要がある。特に、トラッカーを用いて計測対象物の多点位置を計測する場合、同一の座標系である必要がある。ところが、光照射部はレーザ光を受信する機能がなく、さらには、光照射部の照射領域が、移動したトラッカーの特定方向に向けられるため、光照射部間の移動ベクトルの計測は困難なものとなり、統一した同一座標系上におけるトラッカーの位置トラッキングができない場合があるという課題があった。

本発明は、上記の課題を解決するためになされたものであり、トラッカーの位置トラッキングを同一座標系で確実に得て、前記トラッカーに取り付けられた計測用プローブの先端３次元位置を同一座標系で計測することができる位置計測システム及び位置計測方法を提供することを目的とする。

上述した課題を解決し、目的を達成するため、本発明は、レーザ光を照射領域に走査出力する光照射部と、前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置及び姿勢を検出するトラッカーと、前記トラッカーに取り付けられた計測用プローブと、前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置を検出する補助トラッカーと、前記光照射部の初期位置を原点とする基準座標系に対する前記補助トラッカーの初期位置と前記トラッカーの位置とを取得し、前記トラッカーの位置を、前記補助トラッカーの初期位置を原点とする相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算する制御部とを備えたことを特徴とする。

また、本発明は、レーザ光を再帰反射するトラッカーと、前記トラッカーに取り付けられた計測用プローブと、レーザ光を再帰反射する補助トラッカーと、前記レーザ光を照射領域に走査出力し、前記トラッカーからの反射光をもとに前記トラッカーの位置及び姿勢を検出するとともに、前記補助トラッカーからの反射光をもとに前記補助トラッカーの位置を検出する光照射部と、前記光照射部の初期位置を原点とする基準座標系に対する前記補助トラッカーの初期位置と前記トラッカーの位置とを取得し、前記トラッカーの位置を、前記補助トラッカーの初期位置を原点とする相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算する制御部とを備えたことを特徴とする。

また、本発明は、上記の発明において、前記制御部は、前記トラッカーが前記光照射部の照射領域内の遮蔽領域に移動し、かつ、前記光照射部が移動して前記光照射部の次の照射領域内に前記トラッカー及び前記補助トラッカーが存在する場合、前記次の照射領域内において現在の前記光照射部の基準座標系の位置に対する前記補助トラッカーの位置と前記トラッカーの位置とを取得し、前記トラッカーの位置を、前記相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算することを繰り返すことを特徴とする。

また、本発明は、上記の発明において、前記制御部は、前記トラッカーが前記光照射部の照射領域外に移動し、かつ、前記光照射部及び前記補助トラッカーが移動して前記光照射部の次の照射領域内に前記トラッカー及び前記補助トラッカーが存在し、かつ、前記補助トラッカーが前回の照射領域内及び前記次の照射領域内に存在する場合、前記前回の照射領域内において移動前の前記光照射部の基準座標系の位置に対する移動後の前記補助トラッカーの位置を取得し、移動後の前記補助トラッカーの位置を、前記相対基準座標系に対する位置に座標変換し、前記次の照射領域内において移動後の前記光照射部の基準座標系の位置に対する移動後の前記補助トラッカーの位置と移動後の前記トラッカーの位置とを求め、移動後の前記トラッカーの位置を、前記移動後の補助トラッカーの位置を原点とする仮相対基準座標系の位置に座標変換するとともに、移動後の前記トラッカーの姿勢をもとに前記仮相対基準座標系における前記計測用プローブの先端３次元位置を演算し、前記相対基準座標上における移動後の前記補助トラッカーの位置をもとに、前記仮相対基準座標系における前記計測用プローブの先端３次元位置を前記相対基準座標系における前記計測用プローブの先端３次元位置に座標変換することを繰り返すことを特徴とする。

また、本発明は、上記の発明において、前記補助トラッカーは複数配置され、隣接する補助トラッカーとの間の座標変換行列は、予め求められており、前記座標変換行列を順次用いて、順次生成される前記仮相対基準座標系における前記計測用プローブの先端３次元位置を前記相対基準座標系における前記計測用プローブの先端３次元位置に座標変換することを特徴とする。

また、本発明は、上記の発明において、前記光照射部、前記補助トラッカー、又は前記トラッカーの少なくとも１つは前記制御部に無線接続されることを特徴とする。

また、本発明は、レーザ光を照射領域に走査出力する光照射部と、前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置及び姿勢を検出するトラッカーと、前記トラッカーに取り付けられた計測用プローブと、前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置を検出する補助トラッカーとを有する位置計測システムにおける位置計測方法であって、前記光照射部の初期位置を原点とする基準座標系に対する前記補助トラッカーの初期位置と前記トラッカーの位置とを取得する取得工程と、前記取得工程により取得された前記トラッカーの位置を、前記補助トラッカーの初期位置を原点とする相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算する演算工程とを含んだことを特徴とする。

本発明によれば、トラッカーの位置トラッキングを同一座標系で確実に得て、前記トラッカーに取り付けられた計測用プローブの先端３次元位置を同一座標系で計測することができる。

図１は、本実施の形態に係る位置計測システムの概要構成及び概要処理を説明する説明図である。 図２は、補助トラッカーを移動させる場合における位置計測システムの概要構成及び概要処理を説明する説明図である。 図３は、位置計測システムの詳細構成を示すブロック図である。 図４は、計測制御ユニットによる位置計測処理手順を示すフローチャートである。 図５は、変形例である位置計測システムの詳細構成を示すブロック図である。

以下、添付図面を参照して、本実施の形態に係る位置計測システム及び位置計測方法について説明する。

＜概要構成及び概要処理＞
図１は、本実施の形態に係る位置計測システム１の概要構成及び概要処理を説明する説明図である。なお、位置計測システム１は、被計測対象物１００の複数の位置を測定するものである。位置計測システム１は、例えば、ロボットアームを用いた溶接打点位置の教示データ作成、溶接打点位置のチェック、搬送容器の形状測定、車のバンパーなどの変位量測定、ケガキ位置の指示あるいは確認など各種位置計測作業に用いられる。

図１に示すように、位置計測システム１は、光照射部１０、補助トラッカー２０、計測用プローブが取り付けられたトラッカー３０を有する。光照射部１０は、レーザ光を照射領域Ｅ１に走査出力する。トラッカー３０は、光照射部１０から出力されたレーザ光を照射領域Ｅ１内において受光して自身の位置及び姿勢を検出する。補助トラッカー２０は、光照射部１０から出力されたレーザ光を照射距離内において受光して自身の位置を検出する。また、光照射部１０、補助トラッカー２０及びトラッカー３０は、相互に無線接続されている。なお、補助トラッカー２０及びトラッカー３０は、それぞれ複数の光検出部を有し、各光検出部の検出結果から位置及び姿勢を検出することができる。

ここで、トラッカー３０の計測用プローブ３の先端が被計測対象物１００の位置ＰＡを指している状態において、トラッカー３０は、自身の位置Ａを、光照射部１０からのレーザ光を受光して光照射部１０の初期の位置Ｗを原点とする基準座標系ＧＷ上の位置として計測される。なお、トラッカー３０は、自身の位置及び姿勢を検出する。また、補助トラッカー２０は、自身の位置Ｂを、光照射部１０からのレーザ光を受光して基準座標系ＧＷ上の位置として計測される。

その後、トラッカー３０は、位置Ａを、補助トラッカー２０の初期の位置Ｂを原点とする相対基準座標系ＧＢに対する位置に座標変換する。同一座標系での座標変換は、平行移動ベクトルと回転行列とを組み合わせた同時変換行列が用いられる。基準座標系ＧＷに対する位置Ａ，Ｂを示す行列をそれぞれ［Ａ_W］，［Ｂ_W］とし、相対基準座標系ＧＢに対する位置Ａを示す行列を［Ａ_B］とすると、
［Ａ_B］＝［Ｂ_W］^-1×［Ａ_W］
として求められる。なお、［Ｘ］^-1は、Ｘの逆行列である。

すなわち、相対基準座標系ＧＢに対する位置Ａ（行列［Ａ_B］）は、行列［Ａ_W］に逆行列［Ｂ_W］^-1を乗算することによって得られる。ここで、トラッカー３０に取り付けられた計測用プローブ３の先端の位置ＰＡと、トラッカー３０の位置Ａとの相対位置は、既知であり、位置ＰＡは、トラッカー３０の姿勢に応じた方向に位置を移動させることによって簡易に求められる。この位置ＰＡは、相対基準座標系ＧＢ上の位置となる。

ここで、トラッカー３０を移動して被計測対象物１００の他の位置ＰＡ´を計測する場合、トラッカー３０は遮蔽物１０１によって光照射部１０からのレーザ光が届かないため、位置ＰＡ´を計測することができない。このため、光照射部１０を位置Ｗ´に移動し、照射領域Ｅ２を形成して、トラッカー３０が光照射部１０からのレーザ光を受けることができるようにする。なお、この場合、補助トラッカー２０は、照射領域Ｅ１，Ｅ２の重複領域に存在するため、移動しなくてもよい。

この場合も、位置Ａの計測と同様に、基準座標系ＧＷ´に対する位置Ａ´，Ｂを示す行列をそれぞれ［Ａ´_W´］，［Ｂ_W´］とし、相対基準座標系ＧＢに対する位置Ａ´を示す行列を［Ａ´_B］とすると、
［Ａ´_B］＝［Ｂ_W´］^-1×［Ａ´_W´］
として求められる。さらに、位置ＰＡ´は、位置ＰＡと同様に、トラッカー３０の姿勢に応じた方向に位置を移動させることによって求められる。

この結果、光照射部１０を移動する場合であっても、位置ＰＡ，ＰＡ´は、相対基準座標系ＧＢ上の位置となり、被計測対象物１００上の複数の計測点を同一座標系で得ることができる。

＜補助トラッカーを移動させる場合＞
図２は、補助トラッカー２０を移動させる場合における位置計測システム１の概要構成及び概要処理を説明する説明図である。図１では、トラッカー３０が照射領域Ｅ１のレーザ光を受けることができない位置に移動した場合に、補助トラッカー２０を移動させる必要がなかった。これに対し、図２では、トラッカー３０が照射領域Ｅ１１の外に移動した場合であって、トラッカー３０の移動に伴って移動した光照射部１０の次の照射領域Ｅ１１内に補助トラッカー２０が存在しない場合、補助トラッカー２０が前回の照射領域Ｅ１１と次の照射領域Ｅ１２との重複領域に位置するように移動し、相対基準座標系ＧＢでの位置を得るようにしている。

図２に示すように、照射領域Ｅ１１内においてトラッカー３０は、上記のように、相対基準座標系ＧＢ上のトラッカー３０の位置Ａを求め、さらに計測用プローブ３の先端の位置を求める（図２（ａ））。

その後、トラッカー３０が照射領域Ｅ１１外の位置Ａ´に移動すると、光照射部１０がトラッカー３０と、移動した補助トラッカー２０とを含めた照射領域が形成できないため、補助トラッカー２０を、次の照射領域Ｅ１２と前回の照射領域Ｅ１１との重複領域に移動する。そして、照射領域Ｅ１１内において、移動した位置Ｂ´に対する基準座標系ＧＷの行列［Ｂ´_W］を取得する。その後、位置Ａの演算時に求めた、照射領域Ｅ１１内における位置Ｂに対する基準座標系ＧＷの行列［Ｂ_W］の逆行列を行列［Ｂ´_W］に乗算した行列［Ｃ］を求め、基準座標系ＧＷ上の位置Ｂ´を、相対基準座標系ＧＢ上の位置Ｂ´に座標変換する（図２（ｂ））。

その後、照射領域Ｅ１２内において、移動した補助トラッカー２０の位置Ｂ´を原点とする仮相対基準座標系ＧＢ´に対するトラッカー３０の位置Ａ´を求める。この仮相対基準座標系ＧＢ´上の位置Ａ´は、行列［Ｃ］の逆行列を乗算することによって、相対基準座標系ＧＢ上の位置Ａ´に座標変換される。この位置Ａ´のトラッカー３０に取り付けられる計測用プローブ３の先端の位置は、トラッカー３０の姿勢に応じた方向に移動させることによって求められる（図２（ｃ））。

同様にして、さらにトラッカー３０が照射領域Ｅ１２外の位置Ａ´´に移動した場合、図２（ｂ），（ｃ）に対応する処理を繰り返す。すなわち、照射領域Ｅ１３内において、移動した補助トラッカー２０の位置Ｂ´´を原点とする仮相対基準座標系ＧＢ´´に対するトラッカー３０の位置Ａ´´を求める。この仮相対基準座標系ＧＢ´´上の位置Ａ´´は、行列［Ｃ´］の逆行列を乗算し、さらに行列［Ｃ´］の逆行列を乗算することによって、相対基準座標系ＧＢ上の位置Ａ´´に座標変換される。この位置Ａ´´のトラッカー３０に取り付けられる計測用プローブ３の先端の位置は、トラッカー３０の姿勢に応じた方向に移動させることによって求められる（図２（ｄ））。そして、トラッカー３０の取捨領域外への移動に伴って、上記の処理を繰り返すことになる。

これにより、トラッカー３０の位置Ａ，Ａ´，Ａ´´は、同一の相対基準座標系ＧＢ上の位置となり、各位置Ａ，Ａ´，Ａ´´における計測プローブの先端の位置も同一の相対基準座標系ＧＢ上の位置となる。

＜位置計測システムの詳細構成＞
図３は、位置計測システム１の詳細構成を示すブロック図である。図３に示すように、位置計測システム１は、光照射部１０、補助トラッカー２０、計測用プローブ３が取り付けられたトラッカー３０、及び計測制御ユニット４０を有し、各部が無線接続される。なお、図１，２では、計測制御ユニット４０がトラッカー３０に内蔵あるいは外付けされるものとして説明している。計測制御ユニット４０のトラッカー３０への外付けは、直接接続、有線接続、無線接続のいずれであってもよい。無線接続の場合、計測操作性を得るために、計測制御ユニット４０は、操作者が携帯あるいは装着できる可搬性をもつことが好ましい。

光照射部１０は、レーザ光源１１、スキャンミラー１２、光照射制御部１３、通信部１４及びバッテリ１５を有する。レーザ光源１１は、赤外線ビーム光をスキャンミラー１２に照射する。スキャンミラー１２は、例えばＭＥＭＳミラーであり、高速走査が可能であり、レーザ光源１１からの赤外線ビーム光を照射領域Ｅにスキャンして出力する。光照射制御部１３は、計測制御ユニット４０からの指示を受けてレーザ光のスキャン開始及び停止を行う。通信部１４は、計測制御ユニット４０と通信するための通信インタフェースである。バッテリ１５は、光照射部１０内の各部に電源を供給する。

補助トラッカー２０は、複数の光検出部２１、演算制御部２２、通信部２３、バッテリ２４及び計測ボタン２５を有する。複数の光検出部２１は、光照射部１０から照射されたレーザ光を受光する。この際、各光検出部２１は、レンズを介して入光するレーザ光を２次元の光検出領域上で受光する。

演算制御部２２は、光検出部２１の光検出領域上のずれ位置をもとに各光検出部２１におけるレーザ光の受光角度を求める。また、演算制御部２２は、各光検出部２１の受光時間及び各光検出部２１間の受光時間差を求める。演算制御部２２は、各光検出部２１の受光角度をもとに、交会法によって光照射部１０に対する位置を求め、さらに、受光時間や受光時間差をもとに、補助トラッカー２０の姿勢を求める。演算制御部２２は、求めた補助トラッカー２０の位置及び姿勢を計測制御ユニット４０に送信する。

通信部２３は、計測制御ユニット４０と通信するための通信インタフェースである。バッテリ２４は、補助トラッカー２０内の各部に電源を供給する。計測ボタン２５は、補助トラッカー２０の位置及び姿勢の検出を指示するボタンである。

トラッカー３０は、補助トラッカー２０と同じ構成であるが、計測用プローブ３が取り付けられている。トラッカー３０は、複数の光検出部３１、演算制御部３２、通信部３３、バッテリ３４及び計測ボタン３５を有する。複数の光検出部３１は、光照射部１０から照射されたレーザ光を受光する。この際、各光検出部３１は、レンズを介して入光するレーザ光を２次元の光検出領域上で受光する。

演算制御部３２は、光検出部２１の光検出領域上のずれ位置をもとに各光検出部２１におけるレーザ光の受光角度を求める。また、演算制御部３２は、各光検出部３１の受光時間及び各光検出部３１間の受光時間差を求める。演算制御部３２は、各光検出部３１の受光角度をもとに、交会法によって光照射部１０に対する位置を求め、さらに、受光時間や受光時間差をもとに、トラッカー３０の姿勢を求める。演算制御部３２は、求めたトラッカー３０の位置及び姿勢を計測制御ユニット４０に送信する。

通信部３３は、計測制御ユニット４０と通信するための通信インタフェースである。バッテリ３４は、トラッカー３０内の各部に電源を供給する。計測ボタン３５は、トラッカー３０の位置及び姿勢の検出を指示するボタンである。

計測制御ユニット４０は、制御部４１、通信部４４、表示操作部４５及び記憶部４６を有する。制御部４１は、計測制御ユニット４０の全体制御を行う制御部であり、計測制御部４２及び座標変換部４３を有する。計測制御部４２は、光照射部１０、補助トラッカー２０及びトラッカー３０との間における計測処理を制御する。座標変換部４３は、基準座標系の位置を相対基準座標系に座標変換する処理及び計測用プローブ３の先端の位置Ｐ３を演算する処理を行う。

通信部４４は、光照射部１０、補助トラッカー２０及びトラッカー３０との間の通信を行う通信インタフェースである。表示操作部４５は、各種情報の表示及び操作入力を行う入出力デバイスである。表示操作部４５は、補助トラッカー２０及びトラッカー３０が受光状態であるか否かを示すインジケータを有する。なお、このインジケータは、補助トラッカー２０及びトラッカー３０側に設けるようにしてもよい。記憶部４６は、不揮発性メモリなどの記憶デバイスであり、トラッカー３０と計測用プローブ３の先端の位置Ｐ３との位置関係、計測した位置計測データなどを記憶する。

＜計測制御ユニットによる位置計測処理＞
図４は、計測制御ユニット４０による位置計測処理手順を示すフローチャートである。なお、光照射部１０は、レーザ光をスキャン出力している状態であることを前提として説明する。図４に示すように、計測制御ユニット４０の計測制御部４２は、まず、計測ボタン３５が押下されたか否かを判定する（ステップＳ１０１）。計測ボタン３５が押下されていないならば（ステップＳ１０１；Ｎｏ）、本判定処理を繰り返す。

一方、計測ボタン３５が押下されたならば（ステップＳ１０１；Ｙｅｓ）、トラッカー３０及び補助トラッカー２０に計測指示を行い、光照射部１０の位置を原点とする基準座標系におけるトラッカー３０の位置及び姿勢と補助トラッカー２０の位置とを取得する（ステップＳ１０２）。その後、座標変換部４３は、トラッカー３０の位置を、補助トラッカー２０の位置を原点とする相対基準座標系に座標変換する（ステップＳ１０３）。さらに、座標変換部４３は、相対基準座標系上におけるトラッカー３０の姿勢をもとに、計測用プローブ３の先端３次元位置を演算して、表示操作部４５及び記憶部４６に出力する（ステップＳ１０４）。

その後、計測制御部４２は、移動したトラッカー３０は照射領域外か否かを判定する（ステップＳ１０５）。この照射領域外か否かの判定は、トラッカー３０の光検出部３１がレーザ光を受光しているか否かによって行われる。移動したトラッカー３０が照射領域内であるならば（ステップＳ１０５；Ｎｏ）、ステップＳ１０１に移行して、移動したトラッカー３０に取り付けられた計測用プローブ３の先端３次元位置の計測を行う。

一方、移動したトラッカー３０が照射領域外であるならば（ステップＳ１０５；Ｙｅｓ）、光照射部１０の移動指示を表示操作部４５に出力する（ステップＳ１０６）。その後、計測制御部４２は、補助トラッカー２０の移動指示を行ったか否かを判定する（ステップＳ１０７）。なお、計測制御部４２は、光照射部１０が移動した場合であって、現在の補助トラッカー２０の位置が次の照射領域から外れる場合には補助トラッカー２０が次の照射領域内に含まれる位置に移動させる補助トラッカー２０の移動指示を表示操作部４５に出力する。計測制御部４２は、補助トラッカー２０の移動指示を行っていないならば（ステップＳ１０７；Ｎｏ）、ステップＳ１０１に移行して、移動したトラッカー３０に取り付けられた計測用プローブ３の先端３次元位置の計測を行う。

計測制御部４２は、補助トラッカー２０の移動指示を行っているならば（ステップＳ１０７；Ｙｅｓ）、光照射部１０の移動前に、前回の照射領域において、移動後の補助トラッカー２０の位置（基準座標系）を取得する（ステップＳ１０８）。さらに、座標変換部４３は、移動後の補助トラッカー２０の位置を、相対基準座標系に座標変換する（ステップＳ１０９）。

その後、計測制御部４２は、計測ボタン３５が押下されたか否かを判定する（ステップＳ１１０）。計測ボタン３５が押下されないならば（ステップＳ１１０；Ｎｏ）、本判定処理を繰り返す。一方、計測ボタン３５が押下されたならば（ステップＳ１１０；Ｙｅｓ）、光照射部１０の移動後における標準座標上のトラッカー３０の位置及び姿勢と補助トラッカーの位置及び姿勢と補助トラッカー２０の位置とを取得する（ステップＳ１１１）。

その後、座標変換部４３は、移動後のトラッカー３０の位置（標準座標系）を、移動後の補助トラッカーの位置を原点とする座標系（仮相対基準座標系）に座標変換し、さらに、移動前の補助トラッカー２０の位置を原点とする相対基準座標系に座標変換する（ステップＳ１１２）。その後、座標変換部４３は、相対基準座標系上におけるトラッカー３０の姿勢をもとに、計測用プローブ３の先端３次元位置を演算して、表示操作部４５及び記憶部４６に出力する（ステップＳ１１３）。

その後、計測終了指示があったか否を判定し（ステップＳ１１４）、計測終了指示がないならば（ステップＳ１１４；Ｎｏ）、ステップＳ１０５に移行して上記の処理を繰り返す。一方、計測終了指示があったならば（ステップＳ１１４，Ｙｅｓ）、本処理を終了する。

＜変形例＞
図５は、変形例である位置計測システム２の詳細構成を示すブロック図である。上記の実施の形態では、レーザ光を照射領域に走査出力する光照射部１０と、光照射部１０から出力されたレーザ光を照射領域内において受光して自身の位置及び姿勢を検出するトラッカー３０及び補助トラッカー２０とを有し、トラッカー３０及び補助トラッカー２０が自身の位置及び姿勢を検出していた。本変形例では、トラッカー３０に対応するトラッカー１３０及び補助トラッカー２０に対応する補助トラッカー１２０がそれぞれ複数の再帰反射部１２１，１３１を設け、光照射部１０に対応する光照射部１１０がトラッカー１３０及び補助トラッカー１２０からの反射光を検出して、トラッカー１３０及び補助トラッカー１２０の位置及び姿勢を検出するようにしている。その他の座標変換処理等は実施の形態と同じである。

すなわち、光照射部１１０は、再帰反射部１２１，１３１からの反射光を受光する受光部１１６を有する。光照射制御部１３に対応する光照射演算制御部１１３は、さらに受光した再帰レーザ光をもとにトラッカー１３０及び補助トラッカー１２０の位置及び姿勢を検出する演算処理を行う。光照射部１１０は、演算したトラッカー１３０及び補助トラッカー１２０の位置及び姿勢を計測制御ユニット１４０に送信する。

一方、補助トラッカー１２０及びトラッカー１３０は、自身の位置及び姿勢を検出せず、複数の再帰反射部１２１，１３１を設けるのみでよい。なお、計測ボタン２５，３５による指示を計測制御ユニット１４０に送信するため、演算制御部２２，３２に替えて通信制御部１２２，１３２を有する。なお、計測制御ユニット１４０は、トラッカー１３０に直接接続、有線接続、無線接続のいずれかで接続される。

なお、上記の実施の形態及び変形例では、レーザ光を用いてトラッカー及び補助トラッカーの位置及び姿勢を検出するようにしていたが、トラッカー及び補助トラッカーに設けられたＬＥＤなどの複数のマーカを含めてトラッカー及び補助トラッカーを撮像し、撮像した画像に対する画像処理を施してトラッカー及び補助トラッカーの位置及び姿勢を検出するようにしてもよい。なお、レーザ光を用いたトラッカー及び補助トラッカーの位置検出では演算負荷が小さい行列演算を行うが、撮像画像を用いたトラッカー及び補助トラッカーの位置検出では処理負荷の大きい画像処理演算を行う必要がある。

また、上記の実施の形態及び変形例では、１つの補助トラッカーを用いていたが、補助トラッカーを予め所定位置に複数配置し、隣接する補助トラッカーとの間の座標変換行列を予め求めておき、この座標変換行列を順次用いて、順次生成される仮相対基準座標系における計測用プローブの先端３次元位置を相対基準座標系における計測用プローブの先端３次元位置に座標変換するようにしてもよい。

なお、上記の実施の形態で図示した各構成は機能概略的なものであり、必ずしも物理的に図示の構成をされていることを要しない。すなわち、各装置の分散・統合の形態は図示のものに限られず、その全部又は一部を各種の負荷や使用状況などに応じて、任意の単位で機能的又は物理的に分散・統合して構成することができる。

本発明の位置計測システム及び位置計測方法は、トラッカーの位置トラッキングを同一座標系で確実に得て、前記トラッカーに取り付けられた計測用プローブの先端３次元位置を同一座標系で計測したい場合に有用である。

１，２ 位置計測システム
３ 計測用プローブ
１０，１１０ 光照射部
１１ レーザ光源
１２ スキャンミラー
１３ 光照射制御部
１４，２３，３３，４４ 通信部
１５，２４，３４ バッテリ
２０，１２０ 補助トラッカー
２１，３１ 光検出部
２２，３２ 演算制御部
２５，３５ 計測ボタン
３０，１３０ トラッカー
４０，１４０ 計測制御ユニット
４１ 制御部
４２ 計測制御部
４３ 座標変換部
４５ 表示操作部
４６ 記憶部
１００ 被計測対象物
１０１ 遮蔽物
１１３ 光照射演算制御部
１１６ 受光部
１２１，１３１ 再帰反射部
１２２，１３２ 通信制御部
Ａ，Ａ´，Ａ´´，Ｂ，Ｂ´，Ｂ´´，Ｐ３，ＰＡ，ＰＡ´，Ｗ，Ｗ´ 位置
Ｅ１，Ｅ２，Ｅ１１〜Ｅ１３ 照射領域
ＧＢ 相対基準座標系
ＧＢ´，ＧＢ´´ 仮相対基準座標系
ＧＷ，ＧＷ´ 基準座標系

Claims (7)

1. レーザ光を照射領域に走査出力する光照射部と、
   前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置及び姿勢を検出するトラッカーと、
   前記トラッカーに取り付けられた計測用プローブと、
   前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置を検出する補助トラッカーと、
   前記光照射部の初期位置を原点とする基準座標系に対する前記補助トラッカーの初期位置と前記トラッカーの位置とを取得し、前記トラッカーの位置を、前記補助トラッカーの初期位置を原点とする相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算する制御部と
   を備えたことを特徴とする位置計測システム。
2. レーザ光を再帰反射するトラッカーと、
   前記トラッカーに取り付けられた計測用プローブと、
   レーザ光を再帰反射する補助トラッカーと、
   前記レーザ光を照射領域に走査出力し、前記トラッカーからの反射光をもとに前記トラッカーの位置及び姿勢を検出するとともに、前記補助トラッカーからの反射光をもとに前記補助トラッカーの位置を検出する光照射部と、
   前記光照射部の初期位置を原点とする基準座標系に対する前記補助トラッカーの初期位置と前記トラッカーの位置とを取得し、前記トラッカーの位置を、前記補助トラッカーの初期位置を原点とする相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算する制御部と
   を備えたことを特徴とする位置計測システム。
3. 前記制御部は、前記トラッカーが前記光照射部の照射領域内の遮蔽領域に移動し、かつ、前記光照射部が移動して前記光照射部の次の照射領域内に前記トラッカー及び前記補助トラッカーが存在する場合、前記次の照射領域内において現在の前記光照射部の基準座標系の位置に対する前記補助トラッカーの位置と前記トラッカーの位置とを取得し、前記トラッカーの位置を、前記相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算することを繰り返すことを特徴とする請求項１又は２に記載の位置計測システム。
4. 前記制御部は、前記トラッカーが前記光照射部の照射領域外に移動し、かつ、前記光照射部及び前記補助トラッカーが移動して前記光照射部の次の照射領域内に前記トラッカー及び前記補助トラッカーが存在し、かつ、前記補助トラッカーが前回の照射領域内及び前記次の照射領域内に存在する場合、前記前回の照射領域内において移動前の前記光照射部の基準座標系の位置に対する移動後の前記補助トラッカーの位置を取得し、移動後の前記補助トラッカーの位置を、前記相対基準座標系に対する位置に座標変換し、前記次の照射領域内において移動後の前記光照射部の基準座標系の位置に対する移動後の前記補助トラッカーの位置と移動後の前記トラッカーの位置とを求め、移動後の前記トラッカーの位置を、前記移動後の補助トラッカーの位置を原点とする仮相対基準座標系の位置に座標変換するとともに、移動後の前記トラッカーの姿勢をもとに前記仮相対基準座標系における前記計測用プローブの先端３次元位置を演算し、前記相対基準座標上における移動後の前記補助トラッカーの位置をもとに、前記仮相対基準座標系における前記計測用プローブの先端３次元位置を前記相対基準座標系における前記計測用プローブの先端３次元位置に座標変換することを繰り返すことを特徴とする請求項１又は２に記載の位置計測システム。
5. 前記補助トラッカーは複数配置され、隣接する補助トラッカーとの間の座標変換行列は、予め求められており、前記座標変換行列を順次用いて、順次生成される前記仮相対基準座標系における前記計測用プローブの先端３次元位置を前記相対基準座標系における前記計測用プローブの先端３次元位置に座標変換することを特徴とする請求項４に記載の位置計測システム。
6. 前記光照射部、前記補助トラッカー、又は前記トラッカーの少なくとも１つは前記制御部に無線接続されることを特徴とする請求項１〜５のいずれか一つに記載の位置計測システム。
7. レーザ光を照射領域に走査出力する光照射部と、前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置及び姿勢を検出するトラッカーと、前記トラッカーに取り付けられた計測用プローブと、前記光照射部から出力されたレーザ光を前記照射領域内において受光して自身の位置を検出する補助トラッカーとを有する位置計測システムにおける位置計測方法であって、
   前記光照射部の初期位置を原点とする基準座標系に対する前記補助トラッカーの初期位置と前記トラッカーの位置とを取得する取得工程と、
   前記取得工程により取得された前記トラッカーの位置を、前記補助トラッカーの初期位置を原点とする相対基準座標系に対する位置に座標変換するとともに、前記トラッカーの姿勢をもとに前記相対基準座標系における前記計測用プローブの先端３次元位置を演算する演算工程と
   を含んだことを特徴とする位置計測方法。

Images