【０００１】
【発明の属する技術分野】
この発明は、物体の角度と位置を検出する角度・位置検出装置及びそれを内蔵した自在継手に関するものである。
【０００２】
【従来の技術】
【特許文献１】特開２０００−２５６５号公報
【特許文献２】特開平１０−２５０９５号公報
【特許文献３】特開平８−１５９７５７号公報
２軸を、位置関係が連結によって拘束されずに自由自在に変り得るように連結する自在継手の回転角度や、自在継手と組み合わされた伸縮機構の伸縮検知は、一般に、自在継手や伸縮機構の外部に角度センサや変位センサなど複数のセンサを設けて行っている。
【０００３】
例えば特許文献１に示された変位検出装置は、第１の軸と第２の軸を連結した自在継手に、直交した２つの角度センサを設け、第２の軸に、ストロークセンサを有する伸縮機構を介して第３の軸を伸縮自在に取り付け、第１の軸に対する第２の軸と第３の軸の回転角度を２つの角度センサで検出し、第２の軸に対する第３の軸の伸縮量をストロークセンサで検出して第３の軸の端部の変位を検出するようにしている。また、特許文献２に示された操舵入力装置は、３軸のジョイスティックの回転と捩れを複数の傾斜センサで検出するようにしている。
【０００４】
特許文献３に示された地中を掘進したトンネル穴の円弧形状を計測する計測装置は、２本の計測管を自在継手で回転自在に連結し、この連結部にプリズムを配置し、一方の計測管の端部にはレーザ発振器を設け、他方の計測管の端部には受光位置検出器を設け、このレーザ発振器とプリズム及び受光位置検出木で角度検知センサを構成し、レーザ発振装置から出射したレーザビームを連結部に設けたプリズムで適宜照射角度を変えて他方の計測管に入射させ、受光位置検出器で検知したレーザビームの受光位置から２本の計測管の曲がり角度を検知するようにしている。また、特許文献３に示された操舵入力装置は、３軸のジョイスティックの回転と捩れを複数の傾斜センサで検出するようにしている。
【０００５】
【発明が解決しようとする課題】
特許文献１，２に示すように、移動する物体の角度や位置を検出するために複数のセンサを設けると、コストが上昇するとともにセンサの数に応じてセンサ部のサイズが大きくなってしまう。また、２軸を回転自在に連結した自在継手にセンサ部を組み込もうとしても、センサ部のサイズが大きくなるため、組み込むことは困難である。また、特許文献３に示すように、１軸の角度を検出するために専用のセンサを設けると、汎用性がなくコスト高になってしまう。
【０００６】
この発明は係る短所を改善し、簡単な構成で小型化した多自由度の角度・位置検出装置及びそれを内蔵した自在継手を提供することを目的とするものである。
【０００７】
【課題を解決するための手段】
この発明の角度・位置検出装置は、基準点に対して自由運動する測定物体と連動して動き、個別に判別可能であり、一直線状にない少なくとも３個のマーカと、基準点に対する各マーカの角度を光学的に測定する測定装置と、測定した各マーカの角度から基準点に対する測定物体の角度と位置を算出する演算処理装置を有することを特徴とする。
【０００８】
基準点に対して自由運動する測定物体は、角度に関して３つの自由度を持ち、並進に関して３つの自由度を持つ。この測定物体に個別に判別可能な一直線状にない３個のマーカを設け、測定物体とマーカが連動して動くようにしておくと、基準点に対してマーカの空間位置が分かれば、３個のマーカの張る平面に対して測定物体の位置が一意に決まるので、測定物体の基準点に対する角度と位置を決定でき、６軸センサとして作用することができる。
【０００９】
また、演算処理装置は、１つ前に測定された各マーカの角度又は角度と位置を初期値として測定物体の現在の角度と位置を算出し、算出する測定物体の現在の角度と位置の安定度を高める。
【００１０】
また、演算処理装置は、各マーカの位置をマーカ部分の光量で重みをつけた重心として決定し、測定物体の移動によりマーカの観察角度が変わっても安定して各マーカの位置を特定する。
【００１１】
また、マーカは光を散乱させる表面状態に形成し、測定装置はマーカを照明する照明手段と、マーカで散乱された散乱光を撮像する撮像装置とで構成したり、マーカを発光光源で構成し、測定装置はマーカの発光を撮像する撮像装置で構成して、移動している各マーカを安定して検出する。
【００１２】
この発明の自在継手は、前記角度・位置検出装置を有し、角度・位置検出装置の少なくとも３個のマーカを、回転自在に連結された軸継手の一方の軸継手の内部に設け、角度・位置検出装置の測定装置を他方の軸継手に設け、３軸の回転角度を検出することを特徴とする。
【００１３】
この自在継手の回転自在に連結された軸継手の一方の軸継手に伸縮機構を有し、３軸の回転と並進を検出する。
【００１４】
【発明の実施の形態】
まず、この発明の動作原理について説明する。例えば図１に示すように、基準点Ａに対して自由運動する測定物体１は、角度に関して３つの自由度と並進に関して３つの自由度を持つ。この測定物体１に個別に判別可能な一直線上にない３個のマーカ２ａ，２ｂ，２ｃを設け、測定物体１とマーカ２ａ，２ｂ，２ｃが連動して動くようにしておく。この測定物体１の位置は、基準点Ａに対するマーカ２ａ，２ｂ，２ｃの空間位置が分かれば、３個のマーカ２ａ，２ｂ，２ｃで特定される平面により測定物体１の位置が一意に決まり、測定物体１の基準点Ａに対する角度と位置が決定できる。
【００１５】
ここでマーカ２ａ，２ｂ，２ｃの座標をＰ１，Ｐ２，Ｐ３とし、基準点Ａに対するマーカ２ａ，２ｂ，２ｃの方向ベクトルをｔ１，ｔ２，ｔ３とし、基準点Ａからマーカ２ａ，２ｂ，２ｃまでの距離をｄ１，ｄ２，ｄ３とすると、下記（１）式が成り立つ。
Ｐ１＝ｄ１・ｔ１
Ｐ２＝ｄ２・ｔ２ （１）
Ｐ３＝ｄ３・ｔ３
また、マーカ２ａとマーカ２ｂの距離をＬ１２、マーカ２ｂとマーカ２ｃの距離をＬ２３、マーカ２ｃとマーカ２ａの距離をＬ３１とすると、下記（２）式が成り立つ。
Ｌ１２＝｜Ｐ１−Ｐ２｜
Ｌ２３＝｜Ｐ２−Ｐ３｜ （２）
Ｌ３１＝｜Ｐ３−Ｐ１｜
この（１）式と（２）式から、未知の基準点Ａからマーカ２ａ，２ｂ，２ｃまでの距離ｄ１，ｄ２，ｄ３を決めることができる。
【００１６】
一般に上記（１），（２）式を満たすマーカ２の位置として取り得る解は複数あるが、マーカ数を３以上にすることにより全マーカ２の位置に関する拘束条件を増やしてマーカ位置を一意に求めることができる。すなわちマーカ点数がｎ個の場合、ｉ番目のマーカ２ｉの座標をＰｉ、基準点Ａに対するマーカ２ｉの方向ベクトルをｔｉ、基準点Ａからマーカ２ｉまでの距離をｄｉ、ｉ番目のマーカ２ｉとｊ番目のマーカ２ｊの距離をＬｉｊとすると、下記（３），（４）式が成り立つ。
Ｐｉ＝ｄｉ・ｔｉ （３）
Ｌｉｊ＝｜Ｐｉ−Ｐｊ｜ （４）
ｎ個の未知数ｄｉ（ｉ＝１〜ｎ）に対して、（３）式はｎ個成り立ち、（４）式はｎ（ｎ−１）／２個成り立つので、全マーカの位置に対する拘束条件となる方程式数は、｛ｎ・ｎ（ｎ−１）／２｝個となり、マーカ点数を増やすと指数関数的に拘束条件を増やすことができる。
【００１７】
この原理を使用して測定物体１の基準点Ａに対する角度と位置を検出する角度・位置検出装置の構成を図２に示す。角度・位置検出装置３は、測定物体１に、一直線上にないように設けられ、反射率がそれぞれ異なる３個のマーカ２ａ，２ｂ，２ｃと、例えばＬＥＤからなり、測定物体１に光を照射してマーカ２ａ，２ｂ，２ｃを照明する光源４と、撮像装置５と、光源４を駆動する駆動手段６と、例えばパソコン等からなり、撮像装置５からの信号により測定物体１の角度と位置を演算するとともに駆動手段６の動作を制御する演算処理装置７を有する。撮像装置５は、基準点Ａに結像レンズ８が設けられ、例えばＣＣＤからなる撮像面９が主点に設けられている。
【００１８】
この角度・位置検出装置３で、自由運動をしている測定物体１の基準点Ａに対する変化する角度と位置を検出するときの動作を図３のフローチャートを参照して説明する。
【００１９】
まず、基準点Ａに対する測定物体１の角度と位置の初期値を演算処理装置７に設定する（ステップＳ１）。その後、光源４から測定物体１に光を照射し、マーカ２ａ，２ｂ，２ｃで反射した光を撮像装置５の結像レンズ８で集光して撮像面９に入射し、マーカ２ａ，２ｂ，２ｃの像を撮像する。演算処理装置７は撮像装置５で撮像したマーカ２ａ，２ｂ，２ｃの像の出力信号と撮像装置５の焦点距離ｆによりマーカ２ａ，２ｂ，２ｃの基準点Ａに対する角度を演算し（ステップＳ２）、演算した角度から基準点Ａに対するマーカ２ａ，２ｂ，２ｃの方向ベクトルｔ１，ｔ２，ｔ３を算出する（ステップＳ３）。
【００２０】
このマーカ２ａ，２ｂ，２ｃの角度を演算するとき、図４のフローチャートに示すように、撮像装置５で撮像した画像の明暗によりマーカ２ａ，２ｂ，２ｃの像を切り出し（ステップＳ１１）、マーカ２ａ，２ｂ，２ｃの反射率により異なる像の濃度からマーカ２ａ，２ｂ，２ｃをそれぞれ判別する（ステップＳ１２）。このマーカ２ａ，２ｂ，２ｃを個別に判別するとき、マーカ２ａ，２ｂ，２ｃの反射率を異ならせる代わりに、マーカ２ａ，２ｂ，２ｃの色や形状を外径を異ならせても良い。また、マーカ２ａ，２ｂ，２ｃが基準点Ａを中心にする球面に沿って動く場合は、各マーカ２ａ，２ｂ，２ｃの距離が常に一定なので、各マーカ２ａ，２ｂ，２ｃの大きさやマーカ間の距離を個別に変えておいて、マーカ２ａ，２ｂ，２ｃを個別に判別するようにしても良い。
【００２１】
この判別したマーカ２ａ，２ｂ，２ｃの各像の光量で重みを付けて重心をそれぞれ計算する（ステップＳ１３）。このようにマーカ２ａ，２ｂ，２ｃの重心をマーカ部分の光量で重みを付けて算出することにより、マーカ２ａ，２ｂ，２ｃの観察角度が変わっても重心位置は変わらず、マーカ２ａ，２ｂ，２ｃの位置を特定することができる。
【００２２】
この計算した重心の位置と撮像装置５の焦点距離ｆによりマーカ２ａ，２ｂ，２ｃの角度を演算する。例えば撮像装置５をピンホールカメラで近似してピンホール位置を基準点Ａとすると、撮像面９とピンホールとの距離が焦点距離ｆとなる。ピンホールから撮像面９におろした垂線との交点において撮像面９内で直交するｘ方向とｙ方向をとり、ピンホールから撮像面９におろした垂線との交点からマーカ２の結像位置との距離のｘ方向の成分をｘとし、ｙ方向の成分をｙとすると、撮像面の垂線方向に対するマーカ方向の角度φ，θは下記式で得られる（ステップＳ１４）。
φ＝ａｒｃｔａｎ（ｘ／ｆ） （５）
θ＝ａｒｃｔａｎ（ｙ／ｆ） （６）
この角度φ，θによりマーカ２ａ，２ｂ，２ｃの方向ベクトルｔ１，ｔ２，ｔ３を求めることができる。
【００２３】
演算処理装置７はマーカ２ａ，２ｂ，２ｃの方向ベクトルｔ１，ｔ２，ｔ３を演算した後、演算した方向ベクトルｔ１，ｔ２，ｔ３と（１）式と（２）式によりマーカ２ａ，２ｂ，２ｃの基準点Ａからの距離ｄ１，ｄ２，ｄ３を算出し（ステップＳ４）、マーカ２ａ，２ｂ，２ｃの角度と距離から測定物体１の角度と位置を算出して出力する（ステップＳ５）。その後所定時間経過して終了信号がない場合は（ステップＳ６，Ｓ７）、測定物体１の角度と位置の初期値に、今回の測定結果を設定し（ステップＳ８）、演算処理装置７でマーカ２ａ，２ｂ，２ｃの基準点Ａに対する角度を演算して（ステップＳ２）、前記測定物体１のそのときの角度と位置の算出処理を行う（ステップＳ３〜Ｓ８）。この処理を繰り返して終了信号があったら測定物体１の角度と位置の算出処理を終了する（ステップＳ７）。
【００２４】
この測定物体１の角度と位置の算出処理において、（１）式と（２）式によりマーカ２ａ，２ｂ，２ｃの基準点Ａからの距離ｄ１，ｄ２，ｄ３を算出するとき、（２）式は変数ｄ１，ｄ２，ｄ３に対する非線形方程式なので、例えばニュートン法などで解を求める場合、初期値が解が真の値に近ければ解が得られるが、初期値が真の値から遠いと別の解に収束したり、発散したりする場合がある。これに対して、十分真の値に近い１つ前に測定された角度と位置を初期値に設定することにより真の値を安定して得ることができる。
【００２５】
このようにして基準点Ａに対して自由運動している測定物体１の角度と位置を、簡単な構成で精度良く測定することができる。
【００２６】
また、測定物体１の角度と位置を測定するため、マーカ２ａ，２ｂ，２ｃの角度と位置を検出するときの分解能は、撮像面９を構成するＣＣＤの撮像する視野と画素数で決まり、この精度を向上させるにはＣＣＤの画素数を増やせば良い。ＣＣＤは１００万画素でも対角１ｃｍ以下の製品が容易に入手可能になっており、ＣＣＤと結像レンズ８を含めた撮像装置５の容積は１ｃｍ３を十分下回るようにすることができる。また、結像レンズ８とマーカ２ａ，２ｂ，２ｃの距離は結像レンズ８の視野とマーカ２ａ，２ｂ，２ｃの移動範囲に依存するが、最短で数ｍｍ程度となり、マーカ２ａ，２ｂ，２ｃを含めても数ｃｍ３程度の小容量で角度・位置検出装置を構成することができる。また、マーカ２ａ，２ｂ，２ｃには電源を必要としないので測定物体１の選択の範囲が広がる利点がある。
【００２７】
前記説明では測定物体１に光源４から散乱光を照射し、マーカ２ａ，２ｂ，２ｃからの反射光を利用してマーカ２ａ，２ｂ，２ｃの角度と位置を検出する場合について説明したが、図５の構成図に示すように、マーカ２ａ，２ｂ，２ｃとしてＬＥＤやＥＬ素子等の発光素子を使用しても良い。このようにマーカ２ａ，２ｂ，２ｃとして発光素子を使用することにより、各マーカ２ａ，２ｂ，２ｃを判別するとき、各マーカ２ａ，２ｂ，２ｃを逐次点灯させたり、各マーカ２ａ，２ｂ，２ｃ毎に光量や発光色を変えることにより、各マーカ２ａ，２ｂ，２ｃを容易に判別することができる。
【００２８】
この場合、撮像装置５の撮像面９として２次元ＰＳＤ（Ｐｏｓｉｔｉｏｎ Ｓｅｎｓｉｔｉｖｅ Ｄｅｖｉｃｅ）を用いることにより、ＣＣＤ等の２次元撮像素子を使用する場合と比べて応答速度を早くすることができる。
【００２９】
この角度・位置検出装置を自在継手に適用して自在継手の回転角度を検出する場合について説明する。
【００３０】
図６は自在継手の構成図である。自在継手１０は、例えば球面部１１と、球面部１１が嵌合するソケット部１２を有するボールジョイントからなり、球面部１１を基準にしてソケット部１２が任意に回転する。球面部１１は少なくとも上部の半球部１３は透明又は半透明の材料で形成され、下部の半球部１４と上部の半球部１３の境界面に一直線上にない複数例えば３個のＬＥＤからなるマーカ２ａ，２ｂ，２ｃを有する。マーカ２ａ，２ｂ，２ｃのＬＥＤは駆動手段６により発光が制御される。ソケット部１２には結像レンズ８と例えばＣＣＤからなる撮像面９を有する撮像装置５が設けられている。
【００３１】
この自在継手１０の球面部１１を基準にしてソケット部１２を回転するとき、マーカ２ａ，２ｂ，２ｃを発光させて撮像装置５でマーカ２ａ，２ｂ，２ｃからの光を受光し、撮像面９でマーカ２ａ，２ｂ，２ｃの動きを検知し、球面部１１とソケット部１２の回転角度や捩れ角度を演算処理装置７で算出する。このようにして自在継手１０の外部に角度センサを設けずに自在継手１０の回転角度や捩れ角度を検知することができ、自在継手１０を配置するための空間さえあれば良くなり、他の角度センサを３個設ける場合に比べてコンパクトにすることができる。
【００３２】
前記説明では球面部１１にＬＥＤからなるマーカ２ａ，２ｂ，２ｃを設けた場合について説明したが、ＬＥＤの代わりに反射部材でマーカ２ａ，２ｂ，２ｃを構成し、ソケット部１２に光源を設けても良い。また、図７の構成図に示すように、球面部１１に撮像装置５を設け、ソケット部１２を不透明部材で形成し、３個の直径が異なる孔によりマーカ２ａ，２ｂ，２ｃを構成し、外部からマーカ２ａ，２ｂ，２ｃを通る光を撮像装置５で撮像して自在継手１０の回転角度や捩れ角度を検出するようにしても良い。
【００３３】
さらに、図８の構成図に示すように、自在継手１０と例えばベローズ等を使用した伸縮機構１５を組み合わせて回転と並進を行う場合も、マーカ２ａ，２ｂ，２ｃと撮像装置５を内部に設け、自在継手１０の動きと伸縮機構１５の動きと共に移動するマーカ２ａ，２ｂ，２ｃを撮像装置５で撮像し、移動するマーカ２ａ，２ｂ，２ｃの空間座標から自在継手１０と伸縮機構１５の動きを決定して３軸の回転と１軸の並進を検出することができる。また、伸縮機構１５の代わりに３軸の並進機構を設けても、同様にして３軸の回転と３軸の並進を検出することもできる。
【００３４】
【発明の効果】
この発明は以上説明したように、基準点に対して自由運動する測定物体に、個別に判別可能であり、一直線状にない少なくとも３個のマーカを設け、基準点に対する各マーカの角度を光学的に測定し、測定した各マーカの角度から基準点に対する測定物体の角度と位置を算出することにより、簡単な構成で、かつ小型で６軸センサとして機能することができる。
【００３５】
また、１つ前に測定された各マーカの角度又は角度と位置を初期値として測定物体の現在の角度と位置を算出することにより、算出する測定物体の現在の角度と位置の安定度を高め、測定精度を向上することができる。
【００３６】
さらに、各マーカの位置をマーカ部分の光量で重みをつけた重心として決定することにより、測定物体の移動によりマーカの観察角度が変わっても安定して各マーカの位置を特定することができ、角度や位置の測定精度を向上することができる。
【００３７】
また、マーカは光を散乱させる表面状態に形成し、測定装置はマーカを照明する照明手段と、マーカで散乱された散乱光を撮像する撮像装置とで構成したり、マーカを発光光源で構成し、測定装置はマーカの発光を撮像する撮像装置で構成知ることにより、移動している各マーカを安定して検出することができる。
【００３８】
また、角度・位置検出装置の少なくとも３個のマーカを、回転自在に連結された軸継手の一方の軸継手の内部に設け、角度・位置検出装置の測定装置を他方の軸継手に設けることにより、自在継手の外部に角度センサを設けずに、３軸の回転角度を容易に検出することができる。
【００３９】
この自在継手の回転自在に連結された軸継手の一方の軸継手に伸縮機構を設け、自在継手の動きと伸縮機構の動きと共に移動するマーカの空間座標から自在継手と伸縮機構の動きを決定することにより、簡単な構成で３軸の回転と並進を検出することができる。
【図面の簡単な説明】
【図１】この発明の動作原理を示す配置図である。
【図２】この発明の角度・位置検出装置の構成を示す配置図である。
【図３】角度・位置検出装置の動作を示すフローチャートである。
【図４】マーカの基準点に対する角度の算出処理を示すフローチャートである。
【図５】第２の角度・位置検出装置の構成を示す配置図である。
【図６】角度・位置検出装置を有する自在継手の構成図である。
【図７】角度・位置検出装置を有する第２の自在継手の構成図である。
【図８】角度・位置検出装置を有する第３の自在継手の構成図である。
【符号の説明】
１；測定物体、２；マーカ、３；角度・位置検出装置、４；光源、
５；撮像装置、６；駆動手段、７；演算処理装置、８；結像レンズ、
９；撮像面、１０；自在継手、１１；球面部、１２；ソケット部、
１５；伸縮機構。[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to an angle / position detecting device for detecting an angle and a position of an object, and a universal joint incorporating the same.
[0002]
[Prior art]
[Patent Document 1] Japanese Patent Application Laid-Open No. 2000-2565 [Patent Document 2] Japanese Patent Application Laid-Open No. 10-25095 [Patent Document 3] Japanese Patent Application Laid-Open No. 8-159957 The two axes can be freely moved without being restricted by the connection. The rotation angle of the universal joint that can be freely changed and the expansion and contraction detection of the expansion and contraction mechanism combined with the universal joint are generally performed by providing a plurality of sensors such as an angle sensor and a displacement sensor outside the universal joint and the expansion and contraction mechanism. Is going.
[0003]
For example, in a displacement detection device disclosed in Patent Document 1, a universal joint that connects a first shaft and a second shaft is provided with two orthogonal angle sensors, and a telescopic mechanism having a stroke sensor on the second shaft. The third axis is attached to the first axis so as to be extendable and contractible, the rotation angles of the second axis and the third axis with respect to the first axis are detected by two angle sensors, and the third axis is extended and contracted with respect to the second axis. The displacement is detected by the stroke sensor to detect the displacement of the end of the third shaft. Further, the steering input device disclosed in Patent Document 2 detects rotation and twist of a three-axis joystick with a plurality of tilt sensors.
[0004]
The measuring device disclosed in Patent Document 3 for measuring the arc shape of a tunnel hole dug in the ground is rotatably connected to two measuring tubes by a universal joint, and a prism is arranged at the connecting portion. A laser oscillator is provided at the end of the measuring tube, and a light receiving position detector is provided at the end of the other measuring tube.The laser oscillator, a prism and a light receiving position detecting tree constitute an angle detection sensor. The emitted laser beam is incident on the other measuring tube at an appropriately changed irradiation angle by a prism provided at the connecting portion, and the bending angles of the two measuring tubes are detected from the light receiving position of the laser beam detected by the light receiving position detector. Like that. Further, the steering input device disclosed in Patent Document 3 detects rotation and twist of a three-axis joystick with a plurality of inclination sensors.
[0005]
[Problems to be solved by the invention]
As described in Patent Documents 1 and 2, when a plurality of sensors are provided to detect the angle and position of a moving object, the cost increases and the size of the sensor unit increases according to the number of sensors. Further, even if an attempt is made to incorporate the sensor unit into a universal joint in which two axes are rotatably connected, it is difficult to incorporate the sensor unit because the size of the sensor unit increases. Further, as shown in Patent Literature 3, if a dedicated sensor is provided to detect the angle of one axis, there is no versatility and the cost increases.
[0006]
SUMMARY OF THE INVENTION It is an object of the present invention to provide a multi-degree-of-freedom angle / position detecting device which is improved in such disadvantages and has a simple structure and is miniaturized, and a universal joint incorporating the same.
[0007]
[Means for Solving the Problems]
The angle / position detection device according to the present invention moves in conjunction with a measurement object that freely moves with respect to a reference point, can be individually distinguished, and has at least three markers that are not linear and each marker with respect to the reference point. It is characterized by having a measuring device for optically measuring the angle and an arithmetic processing device for calculating the angle and the position of the measured object with respect to the reference point from the measured angle of each marker.
[0008]
A measurement object that moves freely with respect to a reference point has three degrees of freedom in terms of angles and three degrees of freedom in terms of translation. If three markers that are not linear and can be individually distinguished are provided on this measurement object and the measurement object and the marker move in conjunction with each other, if the spatial position of the marker with respect to the reference point is known, three markers can be obtained. Since the position of the measurement object is uniquely determined with respect to the plane on which the marker is placed, the angle and position of the measurement object with respect to the reference point can be determined, and can function as a six-axis sensor.
[0009]
Further, the arithmetic processing device calculates the current angle and position of the measurement object using the angle or the angle and position of each marker measured immediately before as an initial value, and stabilizes the current angle and position of the calculated measurement object. Increase the degree.
[0010]
Further, the arithmetic processing device determines the position of each marker as a center of gravity weighted by the light amount of the marker portion, and stably specifies the position of each marker even when the observation angle of the marker changes due to the movement of the measurement object.
[0011]
Further, the marker is formed in a surface state that scatters light, and the measuring device is configured by an illuminating unit that illuminates the marker and an imaging device that captures the scattered light scattered by the marker, or the marker is configured by a light source. The measuring device is constituted by an image pickup device for picking up the light emission of the marker, and stably detects each moving marker.
[0012]
A universal joint according to the present invention includes the angle / position detecting device, wherein at least three markers of the angle / position detecting device are provided inside one of the rotatable shaft couplings, The measuring device of the position detecting device is provided on the other shaft joint, and the rotation angle of three axes is detected.
[0013]
One of the shaft joints rotatably connected to the universal joint has a telescopic mechanism, and detects rotation and translation of three axes.
[0014]
BEST MODE FOR CARRYING OUT THE INVENTION
First, the operation principle of the present invention will be described. For example, as shown in FIG. 1, a measurement object 1 that freely moves with respect to a reference point A has three degrees of freedom in terms of angles and three degrees of freedom in terms of translation. The measurement object 1 is provided with three markers 2a, 2b, 2c that are not on a straight line that can be individually distinguished, and the measurement object 1 and the markers 2a, 2b, 2c are moved in conjunction with each other. If the spatial position of the markers 2a, 2b, 2c with respect to the reference point A is known, the position of the measuring object 1 is uniquely determined by the plane specified by the three markers 2a, 2b, 2c, The angle and the position of the measurement object 1 with respect to the reference point A can be determined.
[0015]
Here, the coordinates of the markers 2a, 2b, and 2c are P1, P2, and P3, and the direction vectors of the markers 2a, 2b, and 2c with respect to the reference point A are t1, t2, and t3, from the reference point A to the markers 2a, 2b, and 2c. Is defined as d1, d2, and d3, the following equation (1) holds.
P1 = d1 · t1
P2 = d2 · t2 (1)
P3 = d3 · t3
When the distance between the marker 2a and the marker 2b is L12, the distance between the marker 2b and the marker 2c is L23, and the distance between the marker 2c and the marker 2a is L31, the following equation (2) is established.
L12 = | P1-P2 |
L23 = | P2-P3 | (2)
L31 = | P3-P1 |
From the equations (1) and (2), the distances d1, d2, and d3 from the unknown reference point A to the markers 2a, 2b, and 2c can be determined.
[0016]
In general, there are a plurality of solutions that can be taken as the positions of the markers 2 satisfying the above equations (1) and (2). However, by setting the number of markers to 3 or more, the constraint condition on the positions of all the markers 2 is increased to uniquely determine the marker positions. You can ask. That is, when the number of marker points is n, the coordinates of the i-th marker 2i are Pi, the direction vector of the marker 2i with respect to the reference point A is ti, the distance from the reference point A to the marker 2i is di, and the i-th markers 2i and j Assuming that the distance of the second marker 2j is Lij, the following equations (3) and (4) hold.
Pi = di · ti (3)
Lij = | Pi-Pj | (4)
For n unknowns di (i = 1 to n), equation (3) holds n, and equation (4) holds n (n-1) / 2. The number of equations becomes {n · n (n−1) / 2}. When the number of marker points is increased, the constraint condition can be exponentially increased.
[0017]
FIG. 2 shows the configuration of an angle / position detecting device that detects the angle and the position of the measurement object 1 with respect to the reference point A using this principle. The angle / position detecting device 3 is provided on the measurement object 1 so as not to be on a straight line, and includes three markers 2a, 2b, and 2c having different reflectances, for example, LEDs, and irradiates the measurement object 1 with light. A light source 4 for illuminating the markers 2a, 2b, and 2c, an imaging device 5, a driving unit 6 for driving the light source 4, and a personal computer, for example, and the angle and the position of the measurement object 1 based on a signal from the imaging device 5. And an arithmetic processing unit 7 for controlling the operation of the driving means 6. The imaging device 5 is provided with an imaging lens 8 at a reference point A, and an imaging surface 9 formed of, for example, a CCD at a principal point.
[0018]
The operation of the angle / position detection device 3 when detecting the changing angle and position of the measurement object 1 in a free motion with respect to the reference point A will be described with reference to the flowchart of FIG.
[0019]
First, initial values of the angle and the position of the measurement object 1 with respect to the reference point A are set in the arithmetic processing device 7 (step S1). Thereafter, light is emitted from the light source 4 to the measurement object 1, and the light reflected by the markers 2a, 2b, 2c is condensed by the imaging lens 8 of the imaging device 5 and is incident on the imaging surface 9, where the markers 2a, 2b, The image of 2c is captured. The arithmetic processing device 7 calculates the angles of the markers 2a, 2b, 2c with respect to the reference point A based on the output signals of the images of the markers 2a, 2b, 2c captured by the imaging device 5 and the focal length f of the imaging device 5 (step S2). Then, the direction vectors t1, t2, t3 of the markers 2a, 2b, 2c with respect to the reference point A are calculated from the calculated angles (step S3).
[0020]
When calculating the angles of the markers 2a, 2b, 2c, as shown in the flowchart of FIG. 4, the images of the markers 2a, 2b, 2c are cut out based on the brightness of the image captured by the imaging device 5 (step S11), and the marker 2a , 2b, and 2c, the markers 2a, 2b, and 2c are determined from the densities of the images that differ depending on the reflectances (step S12). When the markers 2a, 2b, 2c are individually determined, the colors and shapes of the markers 2a, 2b, 2c may have different outer diameters instead of different reflectances of the markers 2a, 2b, 2c. Further, when the markers 2a, 2b, 2c move along a spherical surface centered on the reference point A, the distance between the markers 2a, 2b, 2c is always constant. May be individually changed, and the markers 2a, 2b, and 2c may be individually determined.
[0021]
Weights are assigned based on the amounts of light of the images of the determined markers 2a, 2b, and 2c, and the centers of gravity are calculated (step S13). In this way, by calculating the center of gravity of the markers 2a, 2b, 2c by weighting them with the light quantity of the marker portion, the position of the center of gravity does not change even if the observation angle of the markers 2a, 2b, 2c changes, and the markers 2a, 2b, 2c The position of 2c can be specified.
[0022]
The angles of the markers 2a, 2b, 2c are calculated based on the calculated position of the center of gravity and the focal length f of the imaging device 5. For example, when the imaging device 5 is approximated by a pinhole camera and the pinhole position is set as the reference point A, the distance between the imaging surface 9 and the pinhole becomes the focal length f. At the intersection with the perpendicular drawn from the pinhole to the imaging surface 9, the x direction and the y direction orthogonal to each other within the imaging surface 9 are taken. From the intersection with the perpendicular drawn from the pinhole to the imaging surface 9, the imaging position of the marker 2 is determined. Assuming that the component of the distance in the x direction is x and the component in the y direction is y, the angles φ and θ in the marker direction with respect to the direction perpendicular to the imaging surface are obtained by the following equations (step S14).
φ = arctan (x / f) (5)
θ = arctan (y / f) (6)
The direction vectors t1, t2, and t3 of the markers 2a, 2b, and 2c can be obtained from the angles φ and θ.
[0023]
After calculating the direction vectors t1, t2, and t3 of the markers 2a, 2b, and 2c, the arithmetic processing unit 7 calculates the markers 2a, 2b, and 2c according to the calculated direction vectors t1, t2, and t3, and the expressions (1) and (2). The distances d1, d2, and d3 from the reference point A are calculated (step S4), and the angle and position of the measurement object 1 are calculated and output from the angles and distances of the markers 2a, 2b, and 2c (step S5). If the end signal does not exist after a lapse of a predetermined time (steps S6 and S7), the current measurement result is set to the initial value of the angle and position of the measurement object 1 (step S8), and the arithmetic processing unit 7 sets the marker 2a. , 2b, 2c with respect to the reference point A are calculated (step S2), and the angle and position of the measuring object 1 at that time are calculated (steps S3 to S8). This process is repeated, and when there is an end signal, the calculation process of the angle and the position of the measurement object 1 is ended (step S7).
[0024]
In the calculation processing of the angle and the position of the measurement object 1, when calculating the distances d1, d2, and d3 of the markers 2a, 2b, and 2c from the reference point A by the equations (1) and (2), the equation (2) is used. Is a nonlinear equation for variables d1, d2, and d3. For example, when a solution is obtained by the Newton method or the like, a solution can be obtained if the initial value is close to the true value, but if the initial value is far from the true value, another solution is obtained. The solution may converge or diverge. On the other hand, a true value can be stably obtained by setting an angle and a position measured immediately before the value sufficiently close to the true value to the initial values.
[0025]
In this way, the angle and position of the measurement object 1 that is freely moving with respect to the reference point A can be accurately measured with a simple configuration.
[0026]
In addition, since the angle and position of the measurement object 1 are measured, the resolution for detecting the angles and positions of the markers 2a, 2b, and 2c is determined by the field of view and the number of pixels of the CCD constituting the imaging surface 9. In order to improve the accuracy, the number of pixels of the CCD may be increased. A CCD having a diagonal of 1 cm or less even with 1 million pixels is easily available, and the volume of the imaging device 5 including the CCD and the imaging lens 8 can be made sufficiently smaller than 1 cm 3 . The distance between the imaging lens 8 and the markers 2a, 2b, 2c depends on the visual field of the imaging lens 8 and the moving range of the markers 2a, 2b, 2c. , The angle / position detecting device can be configured with a small capacity of about several cm 3 . Further, since no power supply is required for the markers 2a, 2b and 2c, there is an advantage that the range of selection of the measurement object 1 is widened.
[0027]
In the above description, a case where the measurement object 1 is irradiated with scattered light from the light source 4 and the angles and positions of the markers 2a, 2b, 2c are detected using the reflected light from the markers 2a, 2b, 2c has been described. As shown in FIG. 5, a light emitting element such as an LED or an EL element may be used as the markers 2a, 2b, and 2c. By using the light emitting elements as the markers 2a, 2b, 2c in this manner, when the markers 2a, 2b, 2c are determined, the markers 2a, 2b, 2c are sequentially turned on, or the markers 2a, 2b, 2c are turned on. The markers 2a, 2b, and 2c can be easily determined by changing the light amount and the emission color for each.
[0028]
In this case, by using a two-dimensional PSD (Position Sensitive Device) as the imaging surface 9 of the imaging device 5, the response speed can be increased as compared with the case of using a two-dimensional imaging device such as a CCD.
[0029]
A case where the angle / position detecting device is applied to a universal joint to detect the rotation angle of the universal joint will be described.
[0030]
FIG. 6 is a configuration diagram of the universal joint. The universal joint 10 includes, for example, a ball joint having a spherical portion 11 and a socket portion 12 in which the spherical portion 11 is fitted. The socket portion 12 rotates arbitrarily with respect to the spherical portion 11. The spherical portion 11 has at least an upper hemispherical portion 13 formed of a transparent or translucent material, and a marker 2a composed of a plurality of, for example, three LEDs that are not in a straight line on a boundary surface between the lower hemispherical portion 14 and the upper hemispherical portion 13 , 2b, 2c. Light emission of the LEDs of the markers 2a, 2b, 2c is controlled by the driving means 6. The socket unit 12 is provided with an imaging device 5 having an imaging lens 8 and an imaging surface 9 formed of, for example, a CCD.
[0031]
When the socket portion 12 is rotated with reference to the spherical portion 11 of the universal joint 10, the markers 2 a, 2 b, and 2 c are emitted, and the imaging device 5 receives light from the markers 2 a, 2 b, and 2 c, and Then, the movements of the markers 2a, 2b, 2c are detected, and the rotation angle and the twist angle of the spherical portion 11 and the socket portion 12 are calculated by the arithmetic processing device 7. In this way, the rotation angle and the torsion angle of the universal joint 10 can be detected without providing an angle sensor outside the universal joint 10, and only a space for arranging the universal joint 10 is sufficient. It can be more compact than when three sensors are provided.
[0032]
In the above description, the case where the markers 2a, 2b, 2c made of LEDs are provided on the spherical portion 11 has been described. However, instead of the LEDs, the markers 2a, 2b, 2c are configured by reflecting members, and the light source is provided on the socket portion 12. Is also good. As shown in the configuration diagram of FIG. 7, the imaging device 5 is provided on the spherical portion 11, the socket portion 12 is formed of an opaque member, and the markers 2a, 2b, and 2c are formed by three holes having different diameters. Light passing through the markers 2a, 2b, 2c from outside may be imaged by the imaging device 5 to detect the rotation angle and the twist angle of the universal joint 10.
[0033]
Further, as shown in the configuration diagram of FIG. 8, also in the case where rotation and translation are performed by combining the universal joint 10 and the expansion and contraction mechanism 15 using, for example, a bellows, the markers 2a, 2b, 2c and the imaging device 5 are provided inside. The marker 2a, 2b, 2c that moves with the movement of the universal joint 10 and the movement of the telescopic mechanism 15 is imaged by the imaging device 5, and the movement of the universal joint 10 and the telescopic mechanism 15 is determined based on the spatial coordinates of the moving markers 2a, 2b, 2c. And rotation of three axes and translation of one axis can be detected. Further, even if a three-axis translation mechanism is provided instead of the expansion and contraction mechanism 15, the rotation of the three axes and the translation of the three axes can be similarly detected.
[0034]
【The invention's effect】
As described above, according to the present invention, at least three markers that are individually identifiable and that are not linear are provided on a measurement object that freely moves with respect to a reference point, and the angle of each marker with respect to the reference point is optically determined. By calculating the angle and the position of the measurement object with respect to the reference point from the measured angles of the markers, it is possible to function as a simple, compact, and six-axis sensor.
[0035]
Also, by calculating the current angle and position of the measurement object using the angle or angle and position of each marker measured immediately before as an initial value, the stability of the current angle and position of the measurement object to be calculated is increased. The measurement accuracy can be improved.
[0036]
Furthermore, by determining the position of each marker as the center of gravity weighted by the light amount of the marker portion, the position of each marker can be specified stably even if the observation angle of the marker changes due to movement of the measurement object, Measurement accuracy of angles and positions can be improved.
[0037]
In addition, the marker is formed in a surface state that scatters light, and the measuring device is configured by an illuminating unit that illuminates the marker and an imaging device that captures the scattered light scattered by the marker, or the marker is configured by a light source. By knowing that the measuring device is constituted by an image pickup device for picking up the light emission of the marker, each moving marker can be detected stably.
[0038]
Also, by providing at least three markers of the angle / position detecting device inside one of the rotatably connected shaft couplings and providing the measuring device of the angle / position detecting device on the other shaft coupling, In addition, the rotation angle of three axes can be easily detected without providing an angle sensor outside the universal joint.
[0039]
An expansion mechanism is provided on one of the shaft joints rotatably connected to the universal joint, and the movement of the universal joint and the expansion mechanism is determined from the spatial coordinates of the marker that moves with the movement of the universal joint and the movement of the expansion mechanism. Thus, rotation and translation of three axes can be detected with a simple configuration.
[Brief description of the drawings]
FIG. 1 is a layout diagram showing the operation principle of the present invention.
FIG. 2 is a layout diagram showing a configuration of an angle / position detecting device of the present invention.
FIG. 3 is a flowchart showing the operation of the angle / position detection device.
FIG. 4 is a flowchart illustrating a process of calculating an angle of a marker with respect to a reference point.
FIG. 5 is a layout diagram showing a configuration of a second angle / position detecting device.
FIG. 6 is a configuration diagram of a universal joint having an angle / position detecting device.
FIG. 7 is a configuration diagram of a second universal joint having an angle / position detecting device.
FIG. 8 is a configuration diagram of a third universal joint having an angle / position detecting device.
[Explanation of symbols]
1; measuring object; 2; marker; 3; angle / position detecting device; 4; light source;
5; imaging device; 6; driving means; 7; arithmetic processing device; 8; imaging lens;
9: imaging surface, 10: universal joint, 11: spherical part, 12: socket part,
15; telescopic mechanism.