JPH01145516A - マニピュレータ用距離及び姿勢測定装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、マニピュレータの先端に装着したＴＶカメ
ラで撮像した画像を用いて、マニピュレータと測定対象
物との距離及び姿勢を測定するようにしたマニピュレー
タ用距離及び姿勢測定装置に関する。

〔従来の技術〕

近年、スペースシャトル等で行われている宇宙実験等の
規模・能力拡大のために、大型宇宙構造物の建設の計画
がなされている。かかる大型宇宙構造物においては、各
種器材の取付・組立等にリモートマニピュレータの利用
が考えられている。

〔発明が解決しようとする問題点〕

このように大型宇宙構造物にリモートマニピュレータを
用いて安全且つ正確に各種器材の取付・組立操作を行う
には、マニピュレータの先端と、組立・取付対象物間の
距離や対象物の姿勢を確認して操作する必要がある。

マニピュレータの先端と対象物との距離や姿勢を測定す
る手段としては、レーザ光を発射させて対象物からの反
射光を、ＣＣＤ等のセンサで検出する測距センサを用い
て行う方法が考えられたが、対象物に反射鏡を取り付け
ない場合は充分且つ安定した輝度の反射光が得られない
ために、正確な測距ができないという問題点があった。

本発明は、ＣＣＤ測距センサ等を用いたマニピュレータ
用距離測定装置における上記問題点を解決するためにな
されたもので、マニピュレータと対象物間の距離及び姿
勢測定を簡便な手段で確実に行えるようにしたマニピュ
レータ用距離及び姿勢測定装置を提供することを目的と
する。

〔問題点を解決するための手段及び作用〕上記問題点を
解決するため、本発明は、マニピュレータの先端に取り
付けられた２軸姿勢可変雲台と、位置関係が既知で一直
線上に並んで位置しない少なくとも３つの標的を設定し
た測定対象物の該標的を撮像するための前記雲台上に装
着されたＴＶ左カメラ置と、前記雲台を前記ＴＶカメラ
が測定対象物の標的を指向するように駆動し標的指向時
の該雲台の姿勢角より測定対象物の距離及び姿勢角を算
出するための雲台駆動・処理装置と、前記ＴＶ左カメラ
置により撮像された画像を表示するＴＶモニタとを備え
、前記雲台を駆動して前記ＴＶ左カメラ置を測定対象物
の標的に指向させることにより得られる雲台姿勢角に基
づいて、測定対象物との距離及びその姿勢を測定するよ
うに構成するものである。

このように構成することにより、操作者の簡単な雲台の
標的指向のための駆動操作だけで、対象物との距離を３
次元で決定でき、同時に対象物のＴＶカメラに対する姿
勢も決定することができる。

そしてこれによりマニピュレータアームの最終位置及び
姿勢制御を容易に行うことができ、マニピュレータの安
全、確実な操作を行うことが可能となる。

〔実施例〕

以下実施例について説明する。第１図は、本発明に係る
マニピュレータ用距離及び姿勢測定装置の一実施例を示
す概略ブロック構成図である。図において、１はＴＶ左
カメラ置で、マニピュレータアーム２の先端に取り付け
られた２軸姿勢可変雲台３に装着され、２自由度の回転
によって視線ｌＯの方向を任意に変更しうるようになっ
ている。

４はＴＶ左カメラ置の取得した画像を表示するＴＶモニ
タであり、５は雲台駆動・処理装置で、雲台３に角度指
令を与えて駆動し、そして測定対象物６に取着された後
述の標的プレート７の特定標的をＴＶモニタ４の特定基
準点（例えば中央）４ａに一敗させたときの雲台３の姿
勢角を読み取り、その姿勢角に基づいて測定対象物の距
離及び姿勢角を算出するものである。測定対象物６には
少なくとも３個の全てが一直線上に並んで位置しないよ
うに配置された標的７−１．７−２．７−３を有する標
的プレート７が設けられており（第２図参照）、またこ
れらの各標的？−１，７−２，７−３間の対象物座標原
点７ａ（第４図参照）に対する位置関係は予め所定値に
設定されている。

本発明は、このような構成の距離及び姿勢測定装置を用
い、ＴＶモニタ４を観察しながら雲台３を駆動操作して
、対象物６に取着した標的プレート７の各標的７−１．
７−２．７−３　ニＴＶカメラ装置１を指向させ、その
時点で得られる雲台３の姿勢角から測定対象物６の距離
並びに姿勢を決定するものであるが、次にその原理につ
いて説明する。

ＴＶモニタ４の画面の基準点４ａに各標的７−１゜７−
２．７−３のイメージ中央部を重ね合わせた時の雲台３
の姿勢角は、ＴＶカメラ装置１のカメラ基準座標軸と各
標的７−１．７−２．７−３との関係を示しており、こ
れにより各標的７−Ｌ　７−２．７−３の方向ベクトル
が決定できる。すなわち、第２図において、カメラ基準
点１ａを原点とする座標軸Ｘ、（、Ｙ、。

Ｚ１４からなるカメラ基準座標に対する対象物６の各標
的？−１，７−２，７−３の方向の単位ベクトルｅｌ。

ｅ　ｔ、（１２は、カメラ雲台３より得られる上下及び
左右の回転角によって求められる。なおこの回転角は本
装置における唯一の測定データであり、第２図において
１．、Ｎ、、Ｎ、はカメラ基準点１ａから各標的７４．
７−２．７−３までのそれぞれの距離を示している。

これらの測定値ｅ、と、第３図に示す各標的７−１゜７
−２．７−３の位置ベクトルｒＩ＋　　”２＝　　’３
の大きさと、各標的７−１．７−２．７−３の相互姿勢
関係が、ｅ、＝既知（ｉ＝１．２．３） ｒ、・ｒ、＝既知（ｉ、ｊ＝１．２．３）但し・印はス
カラ積を示す という条件の下で、カメラ基準点１ａから各標的７−１
．７−２．７−３までの距離Ａ、　　Ｉ！、、１．と、
各標的７−１．７−２．７−３の位置ベクトルｒ　＋、
　　ｒ　ｚ、　　ｒ　３のカメラ基準座標軸に対するベ
クトルの方向を求めることにより、標的座標系の原点７
ａの位置、すなわちカメラ基準点１ａから標的座標系の
原点７ａまでの距離と対象物の姿勢角が決定できる。

なお第４図は、座標軸Ｘｔ、Ｙｔ、Ｚｔからなる標的座
標系を示しており、υ＋　＋　　Ｌ’　ｘ　＋　ＩＪ　
＊は座標軸Ｘｔ、ＹＴ、ＺＴに沿った単位ベクトルを示
している。

次に標的座標系の原点７ａまでの距離及び対象物６の姿
勢の導出過程について説明する。まず測定対象物に第２
図及び第３図に示す標的プレートが取着されているもの
とすると、次の関係式が得られる。

ここで、ｅ　＋　ｌ　　ｅ　ｚ　＋　　６３は先に述べ
たようにカメラ基準点１ａと各標的７−１．７−２．７
−３を結ぶ方向の単位ベクトルであり、カメラ雲台３の
方位角型３、及び上下角θ、より次の（２）式により求
められる。

・・・・・・（２） また標的の相互関係を規定するベクトルｒＩ＋”Ｚ＋”
３は、 ｒ（・ｒＪ＝ｃｏｎｓＬ　　　（ｉ、ｊ＝１．２．３＞
・・・・・・（３） であり、第２図及び第３図に示した図示例では、であり
、これらの条件が付加されて、上記＋１１式は次のよう
に変形される。

Ａ１”＋ｆｆｚ”　　２ｊ！＋１ｚ＋８＋・ｅｚ　　４
ｒ＋”＝０１２”＋１２２２１１ｚＲａｅｔ・ｅｘ（ｒ
ｌ”ｒｓ”）＝０ ７！３”＋１．”−２←（１１ｅ３・ｅｌ−（ｒｌ”＋
ｒ３”）＝０ ・・・・・・（５） ここで、ｒｌ、ｒｓ及び、ｅ、・ｅ、（スカラ積）は既
知の値であり、１．、　１２．　７！、は未知の決定す
べき値であり、またｅ、・ｅＪはベクトルｅ、とｅ、の
なす角度φ□、の余弦と等しい。すなわちｅ、・ｅｊ”
Ｃｏｓφ６Ｊ　（ｉ、ｊ＝１．２．３）・・・・・・（
６） と表現することもできる。

さて上記（５）弐の解ｌ９．β２．！３を求めるために
、 ｆ（ｘ＋、　　ｘｉ、　　Ｘ３） ”ＸＩ”＋Ｘ２”　　２　Ｘ３Ｘ＋ＣＯ３φ１３　４ｒ
＋”ｇ（ｘ　＋、　　ｘ　２１　　　ｘ　コ）＝　ｘ　
２２＋　ｘ、！　　　２　　ＸｚＸ　３ＣＯ９φｆｆ１
ｆｆ−（ｒｌ”　　ｒ　３”）ｈ　（ｘ　Ｉ＋　　ｘ　
ｔ＋　　ｘ３）＝　Ｘ３”＋　ＸＩ”　　２　Ｘ３Ｘ＋
ＣＯ３φ１３−（ｒｌ”＋　ｒ　：＋２）・・・・・・
（７） なる関数を導入して、 ｘ、＝ＩＨ＋６１．　　　　　（ｉ＝１．２．３）・・
・・・・（８） とすれば、（７１式の１．まわりのテーラ−展開よりｆ
（ｘ＋、　　ｘ、、　　ｘ３） ＝ｆ、δＮ、＋ｆ、δ１２ ”’　２　（Ｘ　＋　−Ｘ　ｚｃＯ３φ目）δ１！１＋
　２　（ＸＺ　−ｘ　１ｃｏｓφ、２）δ１２ｇ（ｘ＋
、ＸＩ、Ｘ３） ＝ｇｚδ１ｚ＋ｇｓδｌ。

＝２（ｘ、−ｘコｃｏｓφ２３）δｊ２２＋　２　（ｘ
　３−　ｘ　ｆｉｃＯ５φ２３）δβ３ｈ（ｘ、、ＸＺ
、Ｘ５） ＝ｈ、δＩｔ、＋ｈ１６Ｎ。

＝　２　（ｘ、　−ｘ　、ｃｏｓφ１３）δｌ−コ＋　
２　（ｘ＋　−Ｘ３Ｃ，Ｏ５φ１３）δｌ！・・・・・
・（９） が得られる。ここでｆ、、ｇｌ、ｈ、は、それぞれＸ、
に関する導関数を意味する。上記（９）式を行列形式に
書き直すと次のようになる。

このように（５）式の解！＋、’２＊　　！、の近傍の
値を設定して（７）弐及び（９）式の係数を求めると、
００式より（８）式によって表現される偏位量δ２１が
求まることになる。すなわち、 Ｃｆ＋ｇｚｈｓ＋　ｆｚｇｚｈＩ＞ ・・・・・・（１２） （１１）式により導出したδ１．を用いて、Ｘｉ＝δ１
１→　Ｘ。

として再びδ１直を求めることにより、Ｘ、はＩｌ。

に漸近し、ｌ、が求まることになる。

次に［１１式に求められた１、の値を代入すると、ｒ＋
＝（Ｌｅ＋　　Ａｚｅｚ）／２ ｒｓ＝１３ｅｘ　　（ｊ！＋８＋”　ｌ１ｚｅｚ）／２
・・・・・・（１３） となり、ｒｌ＋　　ｒ３はカメラ座標軸に対して、それ
らの成分が求められたことになる。これにより第４図に
示した標的座標軸Ｘｔ、Ｙｔ、Ｚｔに沿った単位ベクト
ルＬ１＋＋　　Ｌ１ｚ＋　　ｕ２は、となり、標的座標
軸への変換マトリックスＣＭ７は、によって求まる。

（１５）式の行列の成分は、オイラ角に関係付けられる
ことによって、これより通常の計算によって、回転順序
の定義にあったオイラ角を導出すればよい。

一例としてオイラ角をカメラ基準座標軸に対し、方位角
ｖ（７Ｍ軸）、ピッチ角θ（ＸＭ軸）、ロール角φ（Ｚ
ｌｌ軸）の回転順序で定義すれば、Ｃ，Ｔのｉｊ酸成分
Ｃｉ　ｊとすれば、オイラ角は次式より求まる。

ここで座標変換マトリックスＣ，４７とオイラ角の関係
は、 但しＳθ：ｓｉｎθ、ｃθ：　ｃｏｓθＳ甲：　ｓｉｎ
平、ｃ’Ｐ：ｃｏｓ甲 Ｓ　φ　：ｓｉｎφ、ｃ　φ　：　ｃｏｓφより求まる
。

またカメラ基準点から標的原点までの距離ベクトルＬは
、 Ｌ　＝　１　、ｅ　３−　ｒ　、　　　　　　　　　−
、、−０，（１８）で与えられる。

以上のように、カメラ雲台３より得られる雲台姿勢角と
測定対象物の各標的の位置ベクトルの大きさ及びそれら
の相互姿勢関係に基づいて上記の如き演算が雲台操作・
処理装置５において行われ、対象物の距離及び姿勢角が
得られる。

〔発明の効果〕

以上実施例に基づいて説明したように、本発明によれば
、ＴＶモニタを観察しながら雲台を駆動操作して対象物
の標的にＴＶカメラ装置を指向させ、その時点で得られ
る雲台の姿勢角から測定対象物の距離及びその姿勢を測
定するようにしたので、操作者の簡単な雲台の標的指向
のための駆動操作のみで、対象物の距離を３次元で決定
でき、同時に対象物のＴＶカメラ装置に対する姿勢も決
定することができ、１次元の測距センサでは得られない
汎用性と確実性をもたせることができる。

【図面の簡単な説明】

第１図は、本発明に係るマニピュレータ用距離及び姿勢
測定装置の一実施例を示す概略ブロック構成図、第２図
は、測定対象物に取着した標的プレートとＴＶカメラ装
置の基準座標軸との関連を示す図、第３図は、測定対象
物の標的位置関係を示す図、第４図は、標的座標軸を示
す図である。 図において、１はＴＶカメラ装置、２はリモートマニピ
ュレータアーム、３は２軸姿勢可変雲台、４はＴＶモニ
タ、５は雲台駆動・処理装置、６は測定対象物、７は標
的プレートを示す。

Claims (1)

【特許請求の範囲】
1. 　マニピュレータの先端に取り付けられた２軸姿勢可変
   雲台と、位置関係が既知で一直線上に並んで位置しない
   少なくとも３つの標的を設定した測定対象物の該標的を
   撮像するための前記雲台上に装着されたＴＶカメラ装置
   と、前記雲台を前記ＴＶカメラが測定対象物の標的を指
   向するように駆動し標的指向時の該雲台の姿勢角より測
   定対象物の距離及び姿勢角を算出するための雲台駆動・
   処理装置と、前記ＴＶカメラ装置により撮像された画像
   を表示するＴＶモニタとを備え、前記雲台を駆動して前
   記ＴＶカメラ装置を測定対象物の標的に指向させること
   により得られる雲台姿勢角に基づいて、測定対象物との
   距離及びその姿勢を測定するようにしたマニピュレータ
   用距離及び姿勢測定装置。