(57)【要約】【課題】基準点に対して自由運動する測定物体の移動中の角度と位置を、簡単な構成で小型化した角度・位置検出装置で検出する。【解決手段】基準点Ａに対して自由運動する測定物体１に、個別に判別可能であり、一直線状にない少なくとも３個のマーカ２ａ，２ｂ，２ｃを設け、基準点Ａに対するマーカ２ａ，２ｂ，２ｃからの光を撮像装置５に入力してマーカ２ａ，２ｂ，２ｃの像を撮像する。演算処理装置７は撮像したマーカ２ａ，２ｂ，２ｃの基準点Ａに対する角度を測定し、測定した各マーカ２ａ，２ｂ，２ｃの角度から基準点Ａに対する測定物体１の角度と位置を算出する。【選択図】    図２
2004233173.tif 000002
【特許請求の範囲】
【請求項１】
基準点に対して自由運動する測定物体と連動して動き、個別に判別可能であり、一直線状にない少なくとも３個のマーカと、基準点に対する各マーカの角度を光学的に測定する測定装置と、測定した各マーカの角度から基準点に対する測定物体の角度と位置を算出する演算処理装置を有することを特徴とする角度・位置検出装置。
【請求項２】
前記演算処理装置は、１つ前に測定された各マーカの角度又は角度と位置を初期値として測定物体の現在の角度と位置を算出する請求項１記載の角度・位置検出装置。
【請求項３】
前記演算処理装置は、各マーカの位置をマーカ部分の光量で重みをつけた重心として決定する請求項１又は２記載の角度・位置検出装置。
【請求項４】
前記マーカは光を散乱させる表面状態に形成され、前記測定装置はマーカを照明する照明手段と、マーカで散乱された散乱光を撮像する撮像装置からなる請求項１乃至３のいずれかに記載の角度・位置検出装置。
【請求項５】
前記マーカは発光光源からなり、前記測定装置はマーカの発光を撮像する撮像装置からなる請求項１乃至３のいずれかに記載の角度・位置検出装置。
【請求項６】
請求項１乃至６のいずれかに記載された角度・位置検出装置を有し、該角度・位置検出装置の少なくとも３個のマーカを、回転自在に連結された軸継手の一方の軸継手の内部に設け、前記角度・位置検出装置の測定装置を他方の軸継手に設けたことを特徴とする自在継手。
【請求項７】
前記回転自在に連結された軸継手の軸継手に伸縮機構を有する請求項６記載の自在継手。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、物体の角度と位置を検出する角度・位置検出装置及びそれを内蔵した自在継手に関するものである。
【０００２】
【従来の技術】
【特許文献１】特開２０００−２５６５号公報
【特許文献２】特開平１０−２５０９５号公報
【特許文献３】特開平８−１５９７５７号公報
２軸を、位置関係が連結によって拘束されずに自由自在に変り得るように連結する自在継手の回転角度や、自在継手と組み合わされた伸縮機構の伸縮検知は、一般に、自在継手や伸縮機構の外部に角度センサや変位センサなど複数のセンサを設けて行っている。
【０００３】
例えば特許文献１に示された変位検出装置は、第１の軸と第２の軸を連結した自在継手に、直交した２つの角度センサを設け、第２の軸に、ストロークセンサを有する伸縮機構を介して第３の軸を伸縮自在に取り付け、第１の軸に対する第２の軸と第３の軸の回転角度を２つの角度センサで検出し、第２の軸に対する第３の軸の伸縮量をストロークセンサで検出して第３の軸の端部の変位を検出するようにしている。また、特許文献２に示された操舵入力装置は、３軸のジョイスティックの回転と捩れを複数の傾斜センサで検出するようにしている。
【０００４】
特許文献３に示された地中を掘進したトンネル穴の円弧形状を計測する計測装置は、２本の計測管を自在継手で回転自在に連結し、この連結部にプリズムを配置し、一方の計測管の端部にはレーザ発振器を設け、他方の計測管の端部には受光位置検出器を設け、このレーザ発振器とプリズム及び受光位置検出木で角度検知センサを構成し、レーザ発振装置から出射したレーザビームを連結部に設けたプリズムで適宜照射角度を変えて他方の計測管に入射させ、受光位置検出器で検知したレーザビームの受光位置から２本の計測管の曲がり角度を検知するようにしている。また、特許文献３に示された操舵入力装置は、３軸のジョイスティックの回転と捩れを複数の傾斜センサで検出するようにしている。
【０００５】
【発明が解決しようとする課題】
特許文献１，２に示すように、移動する物体の角度や位置を検出するために複数のセンサを設けると、コストが上昇するとともにセンサの数に応じてセンサ部のサイズが大きくなってしまう。また、２軸を回転自在に連結した自在継手にセンサ部を組み込もうとしても、センサ部のサイズが大きくなるため、組み込むことは困難である。また、特許文献３に示すように、１軸の角度を検出するために専用のセンサを設けると、汎用性がなくコスト高になってしまう。
【０００６】
この発明は係る短所を改善し、簡単な構成で小型化した多自由度の角度・位置検出装置及びそれを内蔵した自在継手を提供することを目的とするものである。
【０００７】
【課題を解決するための手段】
この発明の角度・位置検出装置は、基準点に対して自由運動する測定物体と連動して動き、個別に判別可能であり、一直線状にない少なくとも３個のマーカと、基準点に対する各マーカの角度を光学的に測定する測定装置と、測定した各マーカの角度から基準点に対する測定物体の角度と位置を算出する演算処理装置を有することを特徴とする。
【０００８】
基準点に対して自由運動する測定物体は、角度に関して３つの自由度を持ち、並進に関して３つの自由度を持つ。この測定物体に個別に判別可能な一直線状にない３個のマーカを設け、測定物体とマーカが連動して動くようにしておくと、基準点に対してマーカの空間位置が分かれば、３個のマーカの張る平面に対して測定物体の位置が一意に決まるので、測定物体の基準点に対する角度と位置を決定でき、６軸センサとして作用することができる。
【０００９】
また、演算処理装置は、１つ前に測定された各マーカの角度又は角度と位置を初期値として測定物体の現在の角度と位置を算出し、算出する測定物体の現在の角度と位置の安定度を高める。
【００１０】
また、演算処理装置は、各マーカの位置をマーカ部分の光量で重みをつけた重心として決定し、測定物体の移動によりマーカの観察角度が変わっても安定して各マーカの位置を特定する。
【００１１】
また、マーカは光を散乱させる表面状態に形成し、測定装置はマーカを照明する照明手段と、マーカで散乱された散乱光を撮像する撮像装置とで構成したり、マーカを発光光源で構成し、測定装置はマーカの発光を撮像する撮像装置で構成して、移動している各マーカを安定して検出する。
【００１２】
この発明の自在継手は、前記角度・位置検出装置を有し、角度・位置検出装置の少なくとも３個のマーカを、回転自在に連結された軸継手の一方の軸継手の内部に設け、角度・位置検出装置の測定装置を他方の軸継手に設け、３軸の回転角度を検出することを特徴とする。
【００１３】
この自在継手の回転自在に連結された軸継手の一方の軸継手に伸縮機構を有し、３軸の回転と並進を検出する。
【００１４】
【発明の実施の形態】
まず、この発明の動作原理について説明する。例えば図１に示すように、基準点Ａに対して自由運動する測定物体１は、角度に関して３つの自由度と並進に関して３つの自由度を持つ。この測定物体１に個別に判別可能な一直線上にない３個のマーカ２ａ，２ｂ，２ｃを設け、測定物体１とマーカ２ａ，２ｂ，２ｃが連動して動くようにしておく。この測定物体１の位置は、基準点Ａに対するマーカ２ａ，２ｂ，２ｃの空間位置が分かれば、３個のマーカ２ａ，２ｂ，２ｃで特定される平面により測定物体１の位置が一意に決まり、測定物体１の基準点Ａに対する角度と位置が決定できる。
【００１５】
ここでマーカ２ａ，２ｂ，２ｃの座標をＰ１，Ｐ２，Ｐ３とし、基準点Ａに対するマーカ２ａ，２ｂ，２ｃの方向ベクトルをｔ１，ｔ２，ｔ３とし、基準点Ａからマーカ２ａ，２ｂ，２ｃまでの距離をｄ１，ｄ２，ｄ３とすると、下記（１）式が成り立つ。
Ｐ１＝ｄ１・ｔ１
Ｐ２＝ｄ２・ｔ２                    （１）
Ｐ３＝ｄ３・ｔ３
また、マーカ２ａとマーカ２ｂの距離をＬ１２、マーカ２ｂとマーカ２ｃの距離をＬ２３、マーカ２ｃとマーカ２ａの距離をＬ３１とすると、下記（２）式が成り立つ。
Ｌ１２＝｜Ｐ１−Ｐ２｜
Ｌ２３＝｜Ｐ２−Ｐ３｜              （２）
Ｌ３１＝｜Ｐ３−Ｐ１｜
この（１）式と（２）式から、未知の基準点Ａからマーカ２ａ，２ｂ，２ｃまでの距離ｄ１，ｄ２，ｄ３を決めることができる。
【００１６】
一般に上記（１），（２）式を満たすマーカ２の位置として取り得る解は複数あるが、マーカ数を３以上にすることにより全マーカ２の位置に関する拘束条件を増やしてマーカ位置を一意に求めることができる。すなわちマーカ点数がｎ個の場合、ｉ番目のマーカ２ｉの座標をＰｉ、基準点Ａに対するマーカ２ｉの方向ベクトルをｔｉ、基準点Ａからマーカ２ｉまでの距離をｄｉ、ｉ番目のマーカ２ｉとｊ番目のマーカ２ｊの距離をＬｉｊとすると、下記（３），（４）式が成り立つ。
Ｐｉ＝ｄｉ・ｔｉ                    （３）
Ｌｉｊ＝｜Ｐｉ−Ｐｊ｜              （４）
ｎ個の未知数ｄｉ（ｉ＝１〜ｎ）に対して、（３）式はｎ個成り立ち、（４）式はｎ（ｎ−１）／２個成り立つので、全マーカの位置に対する拘束条件となる方程式数は、｛ｎ・ｎ（ｎ−１）／２｝個となり、マーカ点数を増やすと指数関数的に拘束条件を増やすことができる。
【００１７】
この原理を使用して測定物体１の基準点Ａに対する角度と位置を検出する角度・位置検出装置の構成を図２に示す。角度・位置検出装置３は、測定物体１に、一直線上にないように設けられ、反射率がそれぞれ異なる３個のマーカ２ａ，２ｂ，２ｃと、例えばＬＥＤからなり、測定物体１に光を照射してマーカ２ａ，２ｂ，２ｃを照明する光源４と、撮像装置５と、光源４を駆動する駆動手段６と、例えばパソコン等からなり、撮像装置５からの信号により測定物体１の角度と位置を演算するとともに駆動手段６の動作を制御する演算処理装置７を有する。撮像装置５は、基準点Ａに結像レンズ８が設けられ、例えばＣＣＤからなる撮像面９が主点に設けられている。
【００１８】
この角度・位置検出装置３で、自由運動をしている測定物体１の基準点Ａに対する変化する角度と位置を検出するときの動作を図３のフローチャートを参照して説明する。
【００１９】
まず、基準点Ａに対する測定物体１の角度と位置の初期値を演算処理装置７に設定する（ステップＳ１）。その後、光源４から測定物体１に光を照射し、マーカ２ａ，２ｂ，２ｃで反射した光を撮像装置５の結像レンズ８で集光して撮像面９に入射し、マーカ２ａ，２ｂ，２ｃの像を撮像する。演算処理装置７は撮像装置５で撮像したマーカ２ａ，２ｂ，２ｃの像の出力信号と撮像装置５の焦点距離ｆによりマーカ２ａ，２ｂ，２ｃの基準点Ａに対する角度を演算し（ステップＳ２）、演算した角度から基準点Ａに対するマーカ２ａ，２ｂ，２ｃの方向ベクトルｔ１，ｔ２，ｔ３を算出する（ステップＳ３）。
【００２０】
このマーカ２ａ，２ｂ，２ｃの角度を演算するとき、図４のフローチャートに示すように、撮像装置５で撮像した画像の明暗によりマーカ２ａ，２ｂ，２ｃの像を切り出し（ステップＳ１１）、マーカ２ａ，２ｂ，２ｃの反射率により異なる像の濃度からマーカ２ａ，２ｂ，２ｃをそれぞれ判別する（ステップＳ１２）。このマーカ２ａ，２ｂ，２ｃを個別に判別するとき、マーカ２ａ，２ｂ，２ｃの反射率を異ならせる代わりに、マーカ２ａ，２ｂ，２ｃの色や形状を外径を異ならせても良い。また、マーカ２ａ，２ｂ，２ｃが基準点Ａを中心にする球面に沿って動く場合は、各マーカ２ａ，２ｂ，２ｃの距離が常に一定なので、各マーカ２ａ，２ｂ，２ｃの大きさやマーカ間の距離を個別に変えておいて、マーカ２ａ，２ｂ，２ｃを個別に判別するようにしても良い。
【００２１】
この判別したマーカ２ａ，２ｂ，２ｃの各像の光量で重みを付けて重心をそれぞれ計算する（ステップＳ１３）。このようにマーカ２ａ，２ｂ，２ｃの重心をマーカ部分の光量で重みを付けて算出することにより、マーカ２ａ，２ｂ，２ｃの観察角度が変わっても重心位置は変わらず、マーカ２ａ，２ｂ，２ｃの位置を特定することができる。
【００２２】
この計算した重心の位置と撮像装置５の焦点距離ｆによりマーカ２ａ，２ｂ，２ｃの角度を演算する。例えば撮像装置５をピンホールカメラで近似してピンホール位置を基準点Ａとすると、撮像面９とピンホールとの距離が焦点距離ｆとなる。ピンホールから撮像面９におろした垂線との交点において撮像面９内で直交するｘ方向とｙ方向をとり、ピンホールから撮像面９におろした垂線との交点からマーカ２の結像位置との距離のｘ方向の成分をｘとし、ｙ方向の成分をｙとすると、撮像面の垂線方向に対するマーカ方向の角度φ，θは下記式で得られる（ステップＳ１４）。
φ＝ａｒｃｔａｎ（ｘ／ｆ）                    （５）
θ＝ａｒｃｔａｎ（ｙ／ｆ）                    （６）
この角度φ，θによりマーカ２ａ，２ｂ，２ｃの方向ベクトルｔ１，ｔ２，ｔ３を求めることができる。
【００２３】
演算処理装置７はマーカ２ａ，２ｂ，２ｃの方向ベクトルｔ１，ｔ２，ｔ３を演算した後、演算した方向ベクトルｔ１，ｔ２，ｔ３と（１）式と（２）式によりマーカ２ａ，２ｂ，２ｃの基準点Ａからの距離ｄ１，ｄ２，ｄ３を算出し（ステップＳ４）、マーカ２ａ，２ｂ，２ｃの角度と距離から測定物体１の角度と位置を算出して出力する（ステップＳ５）。その後所定時間経過して終了信号がない場合は（ステップＳ６，Ｓ７）、測定物体１の角度と位置の初期値に、今回の測定結果を設定し（ステップＳ８）、演算処理装置７でマーカ２ａ，２ｂ，２ｃの基準点Ａに対する角度を演算して（ステップＳ２）、前記測定物体１のそのときの角度と位置の算出処理を行う（ステップＳ３〜Ｓ８）。この処理を繰り返して終了信号があったら測定物体１の角度と位置の算出処理を終了する（ステップＳ７）。
【００２４】
この測定物体１の角度と位置の算出処理において、（１）式と（２）式によりマーカ２ａ，２ｂ，２ｃの基準点Ａからの距離ｄ１，ｄ２，ｄ３を算出するとき、（２）式は変数ｄ１，ｄ２，ｄ３に対する非線形方程式なので、例えばニュートン法などで解を求める場合、初期値が解が真の値に近ければ解が得られるが、初期値が真の値から遠いと別の解に収束したり、発散したりする場合がある。これに対して、十分真の値に近い１つ前に測定された角度と位置を初期値に設定することにより真の値を安定して得ることができる。
【００２５】
このようにして基準点Ａに対して自由運動している測定物体１の角度と位置を、簡単な構成で精度良く測定することができる。
【００２６】
また、測定物体１の角度と位置を測定するため、マーカ２ａ，２ｂ，２ｃの角度と位置を検出するときの分解能は、撮像面９を構成するＣＣＤの撮像する視野と画素数で決まり、この精度を向上させるにはＣＣＤの画素数を増やせば良い。ＣＣＤは１００万画素でも対角１ｃｍ以下の製品が容易に入手可能になっており、ＣＣＤと結像レンズ８を含めた撮像装置５の容積は１ｃｍ３を十分下回るようにすることができる。また、結像レンズ８とマーカ２ａ，２ｂ，２ｃの距離は結像レンズ８の視野とマーカ２ａ，２ｂ，２ｃの移動範囲に依存するが、最短で数ｍｍ程度となり、マーカ２ａ，２ｂ，２ｃを含めても数ｃｍ３程度の小容量で角度・位置検出装置を構成することができる。また、マーカ２ａ，２ｂ，２ｃには電源を必要としないので測定物体１の選択の範囲が広がる利点がある。
【００２７】
前記説明では測定物体１に光源４から散乱光を照射し、マーカ２ａ，２ｂ，２ｃからの反射光を利用してマーカ２ａ，２ｂ，２ｃの角度と位置を検出する場合について説明したが、図５の構成図に示すように、マーカ２ａ，２ｂ，２ｃとしてＬＥＤやＥＬ素子等の発光素子を使用しても良い。このようにマーカ２ａ，２ｂ，２ｃとして発光素子を使用することにより、各マーカ２ａ，２ｂ，２ｃを判別するとき、各マーカ２ａ，２ｂ，２ｃを逐次点灯させたり、各マーカ２ａ，２ｂ，２ｃ毎に光量や発光色を変えることにより、各マーカ２ａ，２ｂ，２ｃを容易に判別することができる。
【００２８】
この場合、撮像装置５の撮像面９として２次元ＰＳＤ（Ｐｏｓｉｔｉｏｎ  Ｓｅｎｓｉｔｉｖｅ  Ｄｅｖｉｃｅ）を用いることにより、ＣＣＤ等の２次元撮像素子を使用する場合と比べて応答速度を早くすることができる。
【００２９】
この角度・位置検出装置を自在継手に適用して自在継手の回転角度を検出する場合について説明する。
【００３０】
図６は自在継手の構成図である。自在継手１０は、例えば球面部１１と、球面部１１が嵌合するソケット部１２を有するボールジョイントからなり、球面部１１を基準にしてソケット部１２が任意に回転する。球面部１１は少なくとも上部の半球部１３は透明又は半透明の材料で形成され、下部の半球部１４と上部の半球部１３の境界面に一直線上にない複数例えば３個のＬＥＤからなるマーカ２ａ，２ｂ，２ｃを有する。マーカ２ａ，２ｂ，２ｃのＬＥＤは駆動手段６により発光が制御される。ソケット部１２には結像レンズ８と例えばＣＣＤからなる撮像面９を有する撮像装置５が設けられている。
【００３１】
この自在継手１０の球面部１１を基準にしてソケット部１２を回転するとき、マーカ２ａ，２ｂ，２ｃを発光させて撮像装置５でマーカ２ａ，２ｂ，２ｃからの光を受光し、撮像面９でマーカ２ａ，２ｂ，２ｃの動きを検知し、球面部１１とソケット部１２の回転角度や捩れ角度を演算処理装置７で算出する。このようにして自在継手１０の外部に角度センサを設けずに自在継手１０の回転角度や捩れ角度を検知することができ、自在継手１０を配置するための空間さえあれば良くなり、他の角度センサを３個設ける場合に比べてコンパクトにすることができる。
【００３２】
前記説明では球面部１１にＬＥＤからなるマーカ２ａ，２ｂ，２ｃを設けた場合について説明したが、ＬＥＤの代わりに反射部材でマーカ２ａ，２ｂ，２ｃを構成し、ソケット部１２に光源を設けても良い。また、図７の構成図に示すように、球面部１１に撮像装置５を設け、ソケット部１２を不透明部材で形成し、３個の直径が異なる孔によりマーカ２ａ，２ｂ，２ｃを構成し、外部からマーカ２ａ，２ｂ，２ｃを通る光を撮像装置５で撮像して自在継手１０の回転角度や捩れ角度を検出するようにしても良い。
【００３３】
さらに、図８の構成図に示すように、自在継手１０と例えばベローズ等を使用した伸縮機構１５を組み合わせて回転と並進を行う場合も、マーカ２ａ，２ｂ，２ｃと撮像装置５を内部に設け、自在継手１０の動きと伸縮機構１５の動きと共に移動するマーカ２ａ，２ｂ，２ｃを撮像装置５で撮像し、移動するマーカ２ａ，２ｂ，２ｃの空間座標から自在継手１０と伸縮機構１５の動きを決定して３軸の回転と１軸の並進を検出することができる。また、伸縮機構１５の代わりに３軸の並進機構を設けても、同様にして３軸の回転と３軸の並進を検出することもできる。
【００３４】
【発明の効果】
この発明は以上説明したように、基準点に対して自由運動する測定物体に、個別に判別可能であり、一直線状にない少なくとも３個のマーカを設け、基準点に対する各マーカの角度を光学的に測定し、測定した各マーカの角度から基準点に対する測定物体の角度と位置を算出することにより、簡単な構成で、かつ小型で６軸センサとして機能することができる。
【００３５】
また、１つ前に測定された各マーカの角度又は角度と位置を初期値として測定物体の現在の角度と位置を算出することにより、算出する測定物体の現在の角度と位置の安定度を高め、測定精度を向上することができる。
【００３６】
さらに、各マーカの位置をマーカ部分の光量で重みをつけた重心として決定することにより、測定物体の移動によりマーカの観察角度が変わっても安定して各マーカの位置を特定することができ、角度や位置の測定精度を向上することができる。
【００３７】
また、マーカは光を散乱させる表面状態に形成し、測定装置はマーカを照明する照明手段と、マーカで散乱された散乱光を撮像する撮像装置とで構成したり、マーカを発光光源で構成し、測定装置はマーカの発光を撮像する撮像装置で構成知ることにより、移動している各マーカを安定して検出することができる。
【００３８】
また、角度・位置検出装置の少なくとも３個のマーカを、回転自在に連結された軸継手の一方の軸継手の内部に設け、角度・位置検出装置の測定装置を他方の軸継手に設けることにより、自在継手の外部に角度センサを設けずに、３軸の回転角度を容易に検出することができる。
【００３９】
この自在継手の回転自在に連結された軸継手の一方の軸継手に伸縮機構を設け、自在継手の動きと伸縮機構の動きと共に移動するマーカの空間座標から自在継手と伸縮機構の動きを決定することにより、簡単な構成で３軸の回転と並進を検出することができる。
【図面の簡単な説明】
【図１】この発明の動作原理を示す配置図である。
【図２】この発明の角度・位置検出装置の構成を示す配置図である。
【図３】角度・位置検出装置の動作を示すフローチャートである。
【図４】マーカの基準点に対する角度の算出処理を示すフローチャートである。
【図５】第２の角度・位置検出装置の構成を示す配置図である。
【図６】角度・位置検出装置を有する自在継手の構成図である。
【図７】角度・位置検出装置を有する第２の自在継手の構成図である。
【図８】角度・位置検出装置を有する第３の自在継手の構成図である。
【符号の説明】
１；測定物体、２；マーカ、３；角度・位置検出装置、４；光源、
５；撮像装置、６；駆動手段、７；演算処理装置、８；結像レンズ、
９；撮像面、１０；自在継手、１１；球面部、１２；ソケット部、
１５；伸縮機構。
【図１】
2004233173.tif 000003
【図２】
2004233173.tif 000004
【図３】
2004233173.tif 000005
【図４】
2004233173.tif 000006
【図５】
2004233173.tif 000007
【図６】
2004233173.tif 000008
【図７】
2004233173.tif 000009
【図８】
2004233173.tif 000010