JP3086550B2

JP3086550B2 - 運動体の運動同定方法及び運動体の運動同定装置

Info

Publication number: JP3086550B2
Application number: JP04291627A
Authority: JP
Inventors: 正芳橋間
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1992-10-29
Filing date: 1992-10-29
Publication date: 2000-09-11
Anticipated expiration: 2015-09-11
Also published as: JPH06185984A

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は空間上を運動している物
体の運動を同定する運動体の運動同定方法及び運動体の
運動同定装置に関し、特に宇宙空間を浮遊している人工
衛星の捕獲に使用する運動体の運動同定方法及び運動体
の運動同定装置に関する。

【０００２】人工衛星はその修理等のために、一時的に
回収する必要がある。このために、宇宙空間を浮遊して
いる人工衛星を捕獲する必要がある。

【０００３】

【従来の技術】現在、宇宙空間における人工衛星等の浮
遊物の回収作業は、人間の船外活動によって行われてい
る。しかし、これは非常に危険である。このために、宇
宙ロボットを使用し、地上からの遠隔操作による作業が
考えられている。

【０００４】

【発明が解決しようとする課題】しかし、地上と人工衛
星の間では通信の遅れ時間が大きいので、運動している
人工衛星を対象とした捕獲作業は極めて困難である。こ
のために、宇宙ロボットによる自律的な捕獲作業が望ま
れている。自律的な捕獲作業を行うためには、人工衛星
に接近するために、その運動を同定する必要がある。と
ころが、宇宙空間での計測では、リソースが制限されて
おり、少ないリソースで人工衛星の運動を同定する必要
がある。

【０００５】本発明はこのような点に鑑みてなされたも
のであり、限定されたリソースから確実に運動体の運動
を同定できる運動体の運動同定方法を提供することを目
的とする。

【０００６】また、本発明の他の目的は運動体の運動を
短時間で同定できる運動体の運動同定装置を提供するこ
とである。

【０００７】

【課題を解決するための手段】本発明の運動体の運動同
定方法では、図１に示すように、空間上を等速で直線運
動し、かつ等角速度運動している運動体の３つの時刻Ｔ
_i，Ｔ_i+1，Ｔ_i+2での運動体のターゲットマーク１１
を撮像して、このターゲットマーク１１の位置及び姿勢
を認識する。そして、これらの位置及び姿勢から、運動
体の速度ｄ、運動体の回転軸１２の方向ｋ及び運動体の
回転位置θを座標変換行列によって演算して求め、運動
体の位置、姿勢及び速度を同定する。

【０００８】また、本発明の運動体の運動同定装置は、
図２に示すように、運動体１０に設けられたターゲット
マーク１１と、このターゲットマーク１１を撮像するカ
メラ２０と、カメラ２０からの撮像情報を画像処理し
て、ターゲットマーク１１の位置情報を出力する画像処
理装置４０と、この位置情報から、複数の時刻での前記
ターゲットマーク１１の位置、姿勢及び速度を認識し、
位置及び姿勢から前記運動体の速度ｄ、前記運動体の回
転軸１２の方向ｋ及び運動体の回転位置θを座標変換行
列によって演算して求める演算処理装置５０と、から構
成される。

【０００９】

【作用】運動体の３つの時刻Ｔ_i，Ｔ_i+1，Ｔ_i+2での
運動体のターゲットマーク１１を撮像して、この３個の
撮像データから、３つの位置でのターゲットマーク１１
の位置及び姿勢を認識する。そして、これらの３つの位
置及び姿勢のデータから、座標変換行列によって、運動
体の速度ｄ、運動体の回転軸１２の方向ｋ及び運動体の
回転位置θを座標変換行列によって演算して求めること
により、運動体の位置、姿勢及び速度を同定する。

【００１０】また、カメラ２０は運動体１０に固定され
たターゲットマーク１１を撮像し、撮像データを画像処
理装置４０に送る。画像処理装置４０はこの撮像データ
を画像処理して、位置データにして、演算処理装置５０
に送る。演算処理装置５０ではこの位置データを演算処
理して、運動体の速度ｄ、回転軸１２の方向ｋ、回転位
置θを求める。これによって、運動体の位置、姿勢及び
速度を同定する。

【００１１】

【実施例】以下、本発明の一実施例を図面に基づいて説
明する。図２は本発明の運動体の運動同定装置の全体の
構成を示す図である。これらは、人工衛星１０と、人工
衛星１０を捕獲するロボット３０、人工衛星１０を認識
するためのカメラ２０、画像処理装置４０、演算処理装
置５０、ロボットを制御するロボット制御装置６０から
構成されている。

【００１２】人工衛星１０には、人工衛星１０の位置、
姿勢を認識するためのターゲットマーク１１がその正面
に固定されている。また、ターゲットマーク１１の下に
は、人工衛星を捕獲するための被把持棒１３が設けられ
ている。

【００１３】そして、人工衛星１０はその中心軸１２の
回りに一定の角速度で回転しながら、矢印１４で示す方
向に一定速度で運動している。一方、人工衛星１０を捕
獲するロボット３０のアーム３１の先端には、把持棒１
３を把持する把持装置３２、が固定されている。また、
アーム３１の上にはカメラ２０が固定されている。カメ
ラ２０は一定時刻毎に、ターゲットマーク１１を撮像
し、その撮像データを画像処理装置４０に送る。画像処
理装置４０はこの撮像データを画像処理して、ターゲッ
トマーク１１の位置、姿勢を認識する。さらに、複数の
ターゲットマーク１１の位置、姿勢を演算して、人工衛
星１０の位置、姿勢及び移動速度、回転速度等の運動情
報をロボット制御装置６０に送る。ロボット制御装置６
０は、これらの運動情報からアーム３１の方向、すなわ
ちカメラ２０がターゲットマーク１１を追跡できるよう
に、ロボット３０を制御する。また、把持装置３１によ
って、把持棒１３を把持できるようにロボットを制御す
る。

【００１４】次にターゲットマーク１１の位置及び姿勢
から人工衛星１０の運動を同定するアルゴリズムについ
て説明する。図３に人工衛星１０の運動を同定するため
の座標系を示す。基準座標系をＣＯ（Ｏ，Ｘ，Ｙ，Ｚ）
とするが、一般に基準座標系Ｔｃはカメラ２０を基準と
する。また、基準座標系Ｔｃに対するターゲットマーク
１１の座標系をＣＲ（Ｏ^R，Ｘ^R，Ｙ^R，Ｚ^R）とし、
座標系ＣＲは同次座標で、（１）式で表す。

【００１５】

【数１】

【００１６】また、この行列中の要素からなる（２）式
で示す行列Ｔ^Nは座標系Ｔの姿勢を表している。

【００１７】

【数２】

【００１８】座標系ＣＲの座標軸Ｘ^R，Ｙ^R，Ｚ^Rの単
位方向ベクトルは、基準座標系ＣＯに対して、それぞれ
〔ｎ_x，ｎ_y，ｎ_z〕、〔ｏ_x，ｏ_y，ｏ_z〕、
〔ａ_x，ａ _y，ａ_z〕で示される。このように、行列Ｔ
^Nは、基準座標系ＣＯの単位方向ベクトル〔１，０，
０〕、〔０，１，０〕、〔０，０，１〕をそれぞれ〔ｎ
_x，ｎ _y，ｎ_z〕、〔ｏ_x，ｏ_y，ｏ_z〕、〔ａ_x，ａ
_y，ａ_z〕に回転変換した姿勢を示している。また、同
時にこの回転変換も表している。この回転変換は同次座
標で、以下の（３）式で表すことができる。

【００１９】

【数３】

【００２０】また、行列Ｔの第４列のｐ＝〔ｐｘ，ｐｙ，ｐｚ〕^T は基準座標系ＣＯにおける座標系ＣＲの原点の位置Ｏ^R
の位置を示す。（なお、同次座標では３次元の位置ベク
トルは４つの要素で表し、最後の要素はスケール因子で
ある。）また、このｐは基準座標系ＣＯの原点Ｏからタ
ーゲットマーク１１の座標系ＣＲの原点Ｏ^Rまでの並進
変換の変位ベクトルを表しており、この並列変換は同次
座標で以下の（４）式で表される。

【００２１】

【数４】

【００２２】また、以上の座標変換行列から、Ｔ＝Ｕ_TＲ_T となり、Ｔは座標変換も表していることがわかる。この
ような同次座標では、位置及び姿勢と座標系と座標変換
は同じ形で表現される。

【００２３】次に人工衛星１０の回転軸１２の座標系Ｃ
Ｑを設定する。この座標系ＣＱは回転軸１２上の任意の
位置に設定できるが、本実施例では、ターゲットマーク
の座標系ＣＲの座標原点Ｏ^Rから回転軸１２に下ろした
垂線と回転軸１２が交わる点を原点Ｑとし、回転軸１２
の座標系ＣＱの姿勢がターゲットマーク１１の姿勢と同
じになるようにする。つまり、ターゲットマーク１１の
座標系ＣＲから見た原点Ｑの位置ベクトルを、ｍ＝〔ｍ_x，ｍ_y，ｍ_z〕^T とすると、ターゲットマーク１１の座標系ＣＲから見た
回転軸１２の座標系ＣＱは、次の（５）式で表すことが
できる。

【００２４】

【数５】

【００２５】なお、回転軸１２はターゲットマーク１１
の座標系ＣＲからみて一定であるので、ｍ_x，ｍ_y，ｍ
_zは定数である。また、基準座標系ＣＯからみた原点Ｑ
の位置ベクトルを、ｇ＝〔ｇ_x，ｇ_y，ｇ_z〕^T とすると、基準座標系ＣＯからみた回転軸の座標系ＣＱ
は次の（６）式で表すことができる。

【００２６】

【数６】

【００２７】ここで、Ｍはターゲットマーク１１の座標
系ＣＲに対する回転軸１２の座標系ＣＱへの座標変換で
もあるので、３つの座標系ＣＯ，ＣＲ，ＣＱには、

【００２８】

【数７】Ｇ＝ＴＭ ───（７）という関係が成り立つ。また、式（１）の位置成分のみ
みると、ｇとｍには、

【００２９】

【数８】ｇ＝Ｔｍ ───（８）という関係式が成り立つ。

【００３０】次に人工衛星１０の運動の記述を行う。こ
れを図１を使用して説明する。人工衛星１０のターゲッ
トマーク１１の位置及び姿勢は、一定時間間隔のシーン
から計測するものとする。そして、シーンの番号をｉと
し、ｉ番目のシーンのターゲットマーク１１及び回転軸
１２の座標系をそれぞれＴｉ，Ｇｉとする。同様に、原
点Ｑ及び座標ｇもそれぞれのシーンでＱｉ，ｇｉとす
る。したがって、式（７），（８）はそれぞれ、

【００３１】

【数９】Ｇｉ＝ＴｉＭ ───（９）

【００３２】

【数１０】ｇｉ＝Ｔｉｍ ───（１０）となる。

【００３３】ここで、人工衛星１０の運動は、等速直線
運動、等角速度運動と仮定しているので、人工衛星１０
は一定時間の間に、回転軸１２の回りにある角度θだけ
回転し、一定量ｄだけ並進移動する。よって、対象物の
運動は回転変換と並進変換で記述できる。原点を通り、
単位方向ベクトルｋ＝〔ｋ_x，ｋ_y，ｋ_z〕の直線回り
に角度θだけ回転する回転変換Ｒは、以下の（１１）式
BR>で表すことができる。

【００３４】

【数１１】

【００３５】また、変位ベクトルｄ＝〔ｄ_x，ｄ_y，ｄ
_z〕の並進変換Ｕは、次の（１２）式で表すことができ
る。

【００３６】

【数１２】

【００３７】今、基準座標系ＣＯに対する人工衛星１０
の運動パラメータを求めたいので、人工衛星１０の回転
軸１２の単位方向ベクトルｋと、変位ベクトルｄは基準
座標系ＣＯに対して設定し、回転変換、並進変換も基準
座標系ＣＯに対して行う。そして、人工衛星１０の運動
として回転軸１２を考えると、回転軸１２の座標系Ｇ _i
と一定時間後のＧ_i+1との関係は、

【００３８】

【数１３】Ｇ_i+1＝ＵＤ_iＲＤ_i ^-1Ｇ_i ───（１３）となる。ここでＤ_iは、基準座標系ＣＯの原点Ｏから回
転軸１２の座標系ＣＱの座標原点Ｑへの並進変換を表し
ており、以下の（１４）式で表される。

【００３９】

【数１４】

【００４０】つまり、回転軸１２の座標系ＣＱを回転変
換するのに、回転軸１２を基準座標系ＣＯの原点Ｏを通
るように並進変換している。それから回転変換し、元の
位置に戻し、並進変換をしている。なお、ある座標系を
基準座標系ＣＯに対して変換するときは、変換行列を座
標行列の左からかければよい。また、（９）、（１３）
式より、Ｔ_iとＴ_i+1の関係は、

【００４１】

【数１５】Ｔ_i+1＝ＵＤ_iＲＤ_i ^-1Ｔ_i ───（１５）となる。

【００４２】次に運動の同定について述べる。連続する
３つのシーンにおけるターゲットマーク１１の位置及び
姿勢をＴ₁，Ｔ₂，Ｔ₃とし、以下の（１６），（１
７），（１８）式で与えられるものとする。

【００４３】

【数１６】

【００４４】

【数１７】

【００４５】

【数１８】

【００４６】まず、回転成分を求める。回転成分Ｒ^Rは
２シーンの姿勢成分から求められる。

【００４７】

【数１９】Ｔ^R _i+1＝Ｒ^RＴ_i ───（１９）これに、Ｔ₁，Ｔ₂を代入して、Ｒ^Rを求める。

【００４８】

【数２０】Ｒ^R＝Ｔ^R ₂（Ｔ^R ₁）^-1 ───（２０）このＲ^Rを、次の式（２１）で表すと、

【００４９】

【数２１】

【００５０】回転軸１２の単位方向ベクトルｋの各成分
ｋ_x，ｋ_y，ｋ_zと回転角θは、式（１１）右辺と比較
して、次のように求められる。

【００５１】

【数２２】

【００５２】

【数２３】

【００５３】

【数２４】

【００５４】

【数２５】

【００５５】すなわち、式（２４）により回転軸１２の
回転角θが、式（２５）によって回転軸１２の方向ベク
トルｋの各成分ｋ_x，ｋ_y，ｋ_zが与えられる。これに
よって、運動の回転成分が求められことになる。

【００５６】次に並進成分について述べる。人工衛星１
０の並進成分は、回転軸１２の座標原点Ｑの位置の変位
を求めればよい。原点Ｑは等速直線運動するので、

【００５７】

【数２６】ｇ_i−ｇ_i+1＝ｇ_i+1−ｇ_i+2 ───（２６）という関係が成り立つ。これに式（１０）を代入して、

【００５８】

【数２７】（Ｔ_i−２Ｔ_i+1＋Ｔ_i+2）ｍ＝０───（２７）となる。これにＴ₁，Ｔ₂，Ｔ₃を代入すると、次の
（２８）式が得られる。

【００５９】

【数２８】

【００６０】これから、ｍ_x，ｍ_y，ｍ_zに関する３つ
の方程式ができるが、１つは冗長である。これは式（２
６）が回転軸１２上の全ての点に対して成り立つためで
ある。ｍ_x，ｍ_y，ｍ_zを決めるため、ターゲットマー
ク１１の座標原点Ｏ^Rから回転軸１２に下ろした垂線の
足がＱであるという条件を付けた。この条件により、

【００６１】

【数２９】ｐ_i−ｇ_i⊥ｋ ───（２９）が成り立ち、これに式（１０）を代入すると、

【００６２】

【数３０】（ｐ_i−Ｔ_iｍ）ｋ＝０ ───（３０）となる。これにＴ₁を代入すれば、次の（３１）式が得
られる。

【００６３】

【数３１】

【００６４】これから、式（２８），（３１）よりｍが
求められる。このｍと式（１０）より、ｇが求められる
ので、並進成分ｄは、次の式（３２）から求められる。

【００６５】

【数３２】ｄ＝ｇ_i+1−ｇ_i ───（３２）以上によって、回転成分及び並進成分が求まり、人工衛
星１０の運動が同定できる。

【００６６】次にあらたな、人工衛星１０のシーンを推
定する方法について述べる。図１における４番目のシー
ンにおけるターゲットマーク１１の位置及び姿勢は式
（１５）より求められる。式（１５）をまとめると次の
式（３３）のようになる。

【００６７】

【数３３】

【００６８】すなわち、ｉ＝３とすれば、４番目のシー
ンのターゲットマーク１１の位置Ｔ _i+1が推定でき、さ
らに人工衛星１０の位置及び姿勢が推定できるのであ
る。次に図２の演算処理装置５０の処理の流れについて
述べる。図４は演算処理装置５０の演算処理の流れを示
すフローチャートである。なお、図において、Ｓに続く
数値はステップ番号を示す。〔Ｓ１〕ｎ＝１に設定する。〔Ｓ２〕ターゲットマーク１１の位置及び姿勢を測定す
る。〔Ｓ３〕ｎをｎ＋１に置き換える。〔Ｓ４〕ｎが３より大か調べ、大であればＳ５へ進み、
大でなければＳ２に戻る。

【００６９】すなわちＳ１〜Ｓ４のステップで３シーン
分のターゲットマーク１１の位置及び姿勢を測定する。〔Ｓ５〕３シーン分のターゲットマーク１１の位置及姿
勢のデータから上記に述べたように、回転軸１２の回転
位置θ、方向ベクトルｋの各成分、速度ベクトルｄを求
める。これによって、人工衛星１０の位置、姿勢、速度
すなわち運動が同定される。〔Ｓ６〕あらたに、次のシーンのターゲットマーク１１
の位置及び姿勢を測定し、過去の２シーンと新しい１シ
ーン分のデータから、人工衛星１０の運動を同定する。

【００７０】なお、上記の説明ではターゲットマークを
有する人工衛星を例に説明したが、人工衛星以外の浮遊
物でも、その位置及び姿勢が直接あるいは間接的に測定
できれば、本発明を適用することができる。

【００７１】

【発明の効果】以上説明したように本発明では、ターゲ
ットマークの位置及び姿勢から、運動体の位置、姿勢及
び速度を同定するようにしたので、ロボット等を人工衛
星に正確に接近することが可能になり、人工衛星等の捕
獲作業が可能になる。

【００７２】また、カメラでターゲットマークを撮像し
て、これを画像処理して、位置情報を求め、位置情報か
ら運動体の運動を同定するうよに構成したので、宇宙空
間でロボット等を正確に接近させ、捕獲作業を行うこと
が可能になる。

【図面の簡単な説明】

【図１】本発明の原理図である。

【図２】本発明の運動体の運動同定装置の全体の構成を
示す図である。

【図３】人工衛星の運動を同定するための座標系を示す
図である。

【図４】演算処理装置の演算処理の流れを示すフローチ
ャートである。

【符号の説明】

１０人工衛星１１ターゲットマーク１２回転軸２０カメラ３０ロボット４０画像処理装置５０演算処理装置６０ロボット制御装置ＣＲターゲットマークの座標系ＣＯ基準座標系ＣＱ回転軸の座標系

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) G01B 11/00 - 11/30 B25J 19/00 - 19/04 G01S 7/48 G06T 7/20

Claims

(57)【特許請求の範囲】

【請求項１】空間上を等速で直線運動し、かつ等角速
度運動している運動体の位置、姿勢及び速度を同定する
運動体の運動同定方法において、３つの時刻における前記運動体のターゲットマーク（１
１）を撮像して、前記ターゲットマーク（１１）の位置及び姿勢を認識
し、前記位置及び姿勢から、前記運動体の速度（ｄ）、前記
運動体の回転軸（１２）の方向（ｋ）及び前記運動体の
回転位置（θ）を座標変換行列によって演算して求め、前記運動体の位置、姿勢及び速度を同定することを特徴
とする運動体の運動同定方法。
【請求項２】さらに新たな時刻の前記ターゲットマー
ク（１１）の位置及び姿勢を追加して、連続的に前記運
動体の位置、姿勢及び速度を同定することを特徴とする
請求項１記載の運動体の運動同定方法。
【請求項３】前記３つの時刻における前記ターゲット
マークの位置及び姿勢から、一定時刻後の前記運動体の
位置、姿勢及び速度を推定することを特徴とする請求項
１記載の運動体の運動同定方法。
【請求項４】空間上を等速で直線運動し、かつ等角速
度運動している運動体の位置、姿勢及び速度を同定する
運動体の運動同定装置において、前記運動体（１０）に設けられたターゲットマーク（１
１）と、前記ターゲットマーク（１１）を撮像するカメラ（２
０）と、前記カメラからの撮像情報を画像処理して、前記ターゲ
ットマーク（１１）の位置情報を出力する画像処理装置
（４０）と、前記位置情報から、複数の時刻での前記ターゲットマー
クの位置、姿勢及び速度を認識し、前記位置及び姿勢か
ら前記運動体の速度（ｄ）、前記運動体の回転軸（１
２）の方向（ｋ）及び前記運動体の回転位置（θ）を座
標変換行列によって演算して求める演算処理装置（５
０）と、を有することを特徴とする運動体の運動同定装置。
【請求項５】前記運動体（１０）の前記位置、姿勢及
び速度情報を受けて、ロボット（３０）を制御するロボ
ット制御装置と、を有することを特徴とする請求項４記
載の運動体の運動同定装置。
【請求項６】前記運動体（１０）には被把持装置（１
３）を有し、前記ロボット（１０）はアーム（３１）の
先端に前記運動体を把持するための把持装置（３２）を
有することを特徴とする請求項１記載の運動体の運動同
定装置。