JPH09193062A - 宇宙機の制御装置 - Google Patents

Abstract

(57)【要約】 【課題】マニピュレータ動作中にもマニピュレータの動
作に影響を及ぼさずに、宇宙機本体の位置、姿勢の制御
をすることができる制御装置を提供する。 【解決手段】マニピュレータが搭載された宇宙機の制御
装置において、宇宙機本体の位置姿勢及び速度を検出す
るために宇宙機本体に取り付けられた位置／姿勢センサ
２１０と、マニピュレータの関節の発生トルクを検出す
るためにマニピュレータに搭載された力覚センサ３２０
と、マニピュレータの運動を監視する視覚装置と、位置
／姿勢センサ２１０と力覚センサ３２０から得られた情
報を基にして宇宙機本体の位置姿勢をマニピュレータへ
与える外乱分を考慮して宇宙機本体を制御する第一の制
御装置と、視覚装置の情報を基にマニピュレータを駆動
させる第二の制御装置を有している。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、マニピュレータが
搭載された宇宙機の制御装置に関するものである。

【０００２】

【従来の技術】従来の制御装置としては、例えば、特開
平５−１０４４６９や、日本ロボット学会誌「Ｖｏｌ．
７、Ｎｏ．４、ｐｐ６３−７３、１９８９、一般化ヤコ
ビ行列を用いた宇宙用ロボットマニピュレータの分解速
度制御」に記載されているものがある。前者の制御装置
では、マニピュレータの動作に伴う宇宙機本体の位置、
姿勢の変化を考慮して、マニピュレータの先端速度指令
を生成するというものである。また、後者の制御装置で
は、マニピュレータの動作に伴う宇宙機本体の位置、姿
勢の変化を宇宙機に搭載の姿勢制御装置で制御するとい
うものである。

【０００３】

【発明が解決しようとする課題】上記の従来の制御装置
では、マニピュレータの動作による宇宙機本体の位置、
姿勢の変動の考慮はされているが、マニピュレータの動
作中に宇宙機の位置、姿勢の制御を行った場合における
マニピュレータの動作への影響については考慮されてい
なかった。

【０００４】通常、宇宙機本体の質量及び運動加速度の
方が、マニピュレータの質量及び運動加速度よりも大き
く、宇宙機本体の運動がマニピュレータの運動に与える
影響の方が、マニピュレータの運動が宇宙機本体の運動
に与える影響よりも大きい。

【０００５】このため、マニピュレータの動作中に宇宙
機本体の位置、姿勢の制御を行うと、マニピュレータの
動作に対する外乱となり、マニピュレータを正確に動作
させることができなくなる。

【０００６】したがって、マニピュレータの動作中に宇
宙機本体の位置、姿勢を制御する場合には、マニピュレ
ータと宇宙機本体の同時動作を考慮した制御演算を行う
必要があり、マニピュレータが通常多リンク機構である
ことも合わさって演算処理が膨大となっていた。特に、
宇宙機に搭載されたマニピュレータが複数台になると、
この傾向は顕著であった。

【０００７】上記の事情に鑑み、本発明の目的は、マニ
ピュレータ動作中にもマニピュレータの動作に影響を及
ぼさずに、宇宙機本体の位置、姿勢の制御をすることが
できる制御装置を提供することである。

【０００８】

【課題を解決するための手段】前記の目的を達成するた
め、本発明においては、マニピュレータを設けた宇宙機
の制御装置において、前記宇宙機本体に、前記マニピュ
レータの動作を監視する視覚装置と、宇宙機本体の位
置、姿勢および速度を検出する位置／姿勢センサを設
け、前記マニピュレータに、マニピュレータの関節で発
生するトルクを検出する力覚センサを設け、前記位置／
姿勢センサの出力に基づいて、前記宇宙機の位置、姿勢
の制御指令を生成する姿勢指令生成器と、前記位置／姿
勢センサの出力と前記力覚センサの出力に基づいて前記
宇宙機本体の位置、姿勢の変化が前記マニピュレータに
与える変化量を評価する外乱影響評価器と、この外乱影
響評価器の出力に基づいて前記姿勢指令生成器の出力を
補正する姿勢制御指令補正器とから成り、宇宙機の位
置、姿勢を制御する第１の制御装置を設け、前記視覚装
置の出力に基づいて前記マニピュレータを制御する第２
の制御装置を設け、前記位置／姿勢センサの出力に基づ
いて、前記宇宙機の位置、姿勢の制御指令を生成し、前
記位置／姿勢センサの出力と前記力覚センサの出力に基
づいて前記宇宙機本体の位置、姿勢の変化が前記マニピ
ュレータに与える変化量を評価し、この評価結果に基づ
いて前記宇宙機本体に与える姿勢制御指令を補正するこ
とにより、宇宙機本体の位置、姿勢の変動を考慮せずに
マニピュレータを動作させることを可能にした。

【０００９】

【実施の形態】以下、図面を参照しながら本発明の実施
の形態を説明する。図１は、本発明の実施の形態に係る
制御装置によって制御される宇宙機１００の概念図であ
る。同図において、宇宙機１００は、宇宙機本体となる
人工衛星２００と、この人工衛星２００に搭載されたマ
ニピュレータ３００によって構成される。

【００１０】人工衛星２００には、人工衛星２００の位
置、姿勢、速度を検出するための位置／姿勢センサ２１
０と、マニピュレータ３００の動作を監視する視覚装置
２２０と、人工衛星２００の飛行、姿勢制御を行うため
の推進装置２３０が取り付けられている。一方、マニピ
ュレータ３００は、複数のリンクと、各リンクをつなぐ
複数の関節から成り、その先端にはエンドエフェクタ３
１０が、また、各関節には、関節に発生しているトルク
を検出するための力覚センサ３２０が設けられている。

【００１１】図２は、本発明による宇宙機の制御装置の
第１の実施の形態を示すブロック線図である。この実施
の形態においては、人工衛星の質量・イナーシャがマニ
ピュレータの質量・イナーシャに比べて大きく、マニピ
ュレータの動作が人工衛星の位置、姿勢にほとんど影響
しない場合、あるいは、マニピュレータが、人工衛星上
にあるターゲット（図示せず）を把持する場合を想定し
ている。

【００１２】制御装置は、人工衛星のダイナミクス２０
１に接続された系と、マニピュレータのダイナミクス３
０１に接続された系で構成されている。人工衛星のダイ
ナミクス２０１に接続された制御装置は、ターゲットに
対する人工衛星の位置、姿勢、速度を検出する位置／姿
勢センサ２１０と、この位置／姿勢センサ２１０の出力
に基づいて人工衛星のターゲットに対する位置、姿勢の
目標値を生成する姿勢指令生成器２４０と、この姿勢指
令生成器２４０の出力と、マニピュレータに設けられた
力覚センサ３２０およびマニピュレータの各関節の回転
角を検出するサーボセンサ３３０の出力に基づいて、人
工衛星の姿勢制御がマニピュレータに与える影響を評価
する外乱影響評価器２５０と、前記姿勢指令生成器２４
０の出力を前記外乱影響評価器２５０の出力に基づいて
補正する姿勢制御指令補正器２６０とにより構成されて
いる。

【００１３】マニピュレータのダイナミクス３０１に接
続された制御装置は、マニピュレータに設けられた力覚
センサ３２０およびマニピュレータの各関節の回転角を
検出するサーボセンサ３３０と、マニピュレータの目標
先端位置、姿勢指令を生成する軌道生成器３６０と、こ
の軌道生成器３６０の出力に基づいてマニピュレータの
先端の速度指令を生成する先端速度生成器３５０と、こ
の先端速度生成器３５０の出力に基づいて、マニピュレ
ータの各関節の速度を生成する関節速度生成器３４０
と、この関節速度生成器３４０の出力に基づいて、マニ
ピュレータの各関節に設けられたモータを駆動するモー
タドライバ３８０とにより構成され、前記軌道生成器３
６０と、先端速度生成器３５０および関節速度生成器３
４０でマニピュレータ制御系を構成している。

【００１４】軌道生成器３６０から目標位置指令が入力
されると、先端速度生成器３５０によりマニピュレータ
の先端速度指令が生成される。この先端速度指令が入力
されると、関節速度生成器３４０が先端速度指令と、サ
ーボセンサにより検出された関節角に基づき、マニピュ
レータの各関節に与える関節速度指令を生成し、モータ
ドライバ３８０を介して、マニピュレータの各軸モータ
を駆動する。

【００１５】関節速度の生成方法は例えば、（１）式に
示すようなヤコビ行列を用いた方法がある。 ｖ ＝Ｊ・ｄθ …（１） ｖ ：マニピュレータの先端速度ベクトル指令 Ｊ ：ヤコビ行列（マニピュレータのダイナミクス
３０１からサーボセンサ３３０が検出する関節角情報に
より算出） ｄθ ：マニピュレータ関節速度ベクトル指令 一方、外乱影響評価器２５０は、マニピュレータの各関
節に搭載された力覚センサ３２０により検出された各関
節の発生トルクτと、人工衛星のダイナミクス２０１か
らセンサ２１０により検出された情報に基づいて、姿勢
指令生成器２４０にて生成される人工衛星本体の並進加
速度（指令）ｄｖ０と、回転加速度（指令）ｄω０とに
より、以下の（２）〜（４）式を用いて算出されたｎｉ
を関節回転軸方向へ正射影し、各関節に加わるトルクｄ
τｉを算出する。

【００１６】 ｉ ＝１〜ｎ ωｉ ＝ω（ｉ−１） ｄωｉ ＝ｄω（ｉ−１） ｄｖｊｉ＝ｄｖｊ（ｉ−１）＋ｄω（ｉ−１）×ｌ（ｉ−１）＋ω（ｉ−１） ×（ω（ｉ−１）×ｌ（ｉ−１）） ｄｖｉ ＝ｄｖ（ｉ−１）＋ｄωｉ×ａｉ＋ωｉ×（ωｉ×ａｉ） …（２） ｎ ：マニピュレータ関節数 ｄωｉ ：マニピュレータリンクｉの回転加速度 ｄｖｊｉ：マニピュレータ関節ｉの並進加速度 ｄｖｉ ：マニピュレータリンクｉの並進加速度 ωｉ ：マニピュレータリンクｉの回転速度 ｄω０ ：人工衛星本体の回転加速度（指令） ｄｖ０ ：人工衛星本体の並進加速度（指令） ω０ ：人工衛星本体の回転速度 ｋｉ ：マニピュレータ関節ｉの回転軸ベクトル ｌｉ ：マニピュレータ関節ｉから関節ｉ＋１に至る
ベクトル ａｉ ：マニピュレータ関節ｉからリンクｉ重心へ至
るベクトル ｉ ＝１〜ｎ Ｆｉ ＝ｍｉ・ｄｖｉ Ｎｉ ＝Ｉｉ・ｄωｉ＋ωｉ×（Ｉｉωｉ） …（３） ｍｉ ：マニピュレータリンクｉの質量 Ｉｉ ：マニピュレータリンクｉのイナーシャ Ｆｉ ：マニピュレータリンクｉ重心で発生している
力 Ｎｉ ：マニピュレータリンクｉ重心で発生している
モーメント ｉ ＝ｎ〜１ ｆｉ ＝ｆｉ＋１＋Ｆｉ ｎｉ ＝ｎｉ＋１＋ｌｉ×ｆｉ＋１＋ａｉ×Ｆｉ＋Ｎｉ …（４） ｆｉ ：マニピュレータ関節ｉにおいて発生している
力 ｎｉ ：マニピュレータ関節ｉにおいて発生している
モーメント ｆｎ＋１：マニピュレータ先端に加えられた力 （力覚センサ３２０で検出） ｎｎ＋１：マニピュレータ先端に加えられたモーメント （力覚センサ３２０で検出） また、関節の性能により補償されている出力トルクτｍ
ｉより、（５）式によりトルクｄτｉを算出する。 ｄτｉ ＝τｍｉ−τｄｉ−τｉ …（５） τｉ ：マニピュレータ関節ｉにおいて発生している
トルク （力覚センサ３２０により検出） この処理において、力覚センサ３２０を使用していない
ときには、τｍ＝０として計算を行う。

【００１７】前記（５）式による計算の結果、トルクｄ
τ＜０の場合には、姿勢制御指令補正器２６０におい
て、人工衛星の並進加速度（指令）ｄｖ０、回転加速度
（指令）ｄω０の値を小さい値へ変更する。またトルク
ｄτの絶対値があるしきい値以上の時には、姿勢制御指
令補正器２６０からブレーキ指令をマニピュレータ制御
系３７０に送り、マニピュレータ制御系の指令により、
マニピュレータの各関節に設けられたブレーキを作動さ
せる。

【００１８】図３は、本発明による宇宙機の制御装置の
第２の実施の形態を示すブロック線図である。この実施
の形態においては、人工衛星の質量・イナーシャがマニ
ピュレータの質量・イナーシャに比べてあまり大きくな
く、マニピュレータの動作が人工衛星の位置、姿勢へ無
視できない影響を与える場合、あるいはマニピュレータ
が、宇宙空間を浮遊しているターゲットを把持する場合
を想定している。この実施の形態においては、マニピュ
レータの動作に伴う人工衛星の位置姿勢の変化を、人工
衛星に搭載されたセンサで検出し、人工衛星の位置姿勢
の変動を考慮しながらマニピュレータを動かす。なお、
この図３において、図２と同一機能部分は同一符号で示
し、重複する部分の詳しい説明は省略する。

【００１９】先端速度生成器３５１では、マニピュレー
タの先端速度を（６）〜（８）式により算出する。

【００２０】 Ｅｔ ＝Σ｛ｍｉ（ｖｉ・ｖｉ）｝／２ Ｅｒ ＝Σ｛ωｉ・Ｉｉ・ωｉ｝／２ …（６） Ｅｔ ：マニピュレータの並進エネルギーＥｒ ：
マニピュレータの回転エネルギー ｍｅ ＝２Ｅｔ／（ｖｒ・ｖｒ） Ｉｅ ＝２Ｅｒ（ωｒ・ωｒ）／｛（ωｒ・ωｒ）・（ωｒ・ωｒ）｝ …（７） ｍｅ ：マニピュレータ先端の等価質量 Ｉｅ ：マニピュレータ先端の等価イナーシャ ｖｒ ：マニピュレータ先端の並進速度ベクトル （サーボセンサ３３０検出情報より算出） ωｒ ：マニピュレータ先端回転速度ベクトル （サーボセンサ３３０検出情報より算出） ｍｅ・ｖｒ＋ｍ０・ｖ０＝ｍｅ・ｖｔ＋ｍ０・ｖｄ０ Ｉｅ・ωｒ＋ｍｅ・（ｒｇ×ｖｒ）＋Ｉ０・ω０ ＝Ｉｅωｔ＋ｍｅ・（ｒｇ×ｖｔ）＋Ｉ０ωｄ０ …（８） ｖｔ ：マニピュレータの先端速度指令の並進成分 ωｔ ：マニピュレータの先端速度指令の回転成分 ｍ０ ：人工衛星本体の質量 Ｉ０ ：人工衛星本体のイナーシャ ｖ０ ：人工衛星本体の並進速度 ω０ ：人工衛星本体の回転速度 ｒ０ ：人工衛星本体の重心位置ベクトル ｒｇ ：マニピュレータの重心ベクトル ｖｄ０ ：人工衛星本体の並進速度予測値 ωｄ０ ：人工衛星本体の回転速度予測値 この人工衛星本体の並進速度予測値ｖｄ０および回転速
度予測値ωｄ０を姿勢指令生成器２４１Ｂへ入力し、姿
勢指令生成器２４１Ｂでは、この人工衛星本体の並進速
度予測値ｖｄ０および回転速度予測値ωｄ０と位置／姿
勢センサ２１０からの位置、姿勢情報より位置、姿勢目
標を生成する。

【００２１】また、人工衛星本体の並進速度予測値ｖｄ
０および回転速度予測値ωｄ０があるしきい値以上の時
には、上記マニピュレータ先端速度生成器３５１Ｂでの
演算の他に、軌道生成器３６０においては、マニピュレ
ータの動作が人工衛星に与える影響を少なくするため
に、マニピュレータの先端が人工衛星にできるだけ近い
位置を通る軌道を生成する。

【００２２】図４は、本発明による宇宙機の制御装置の
第３の実施の形態を示すブロック線図である。この実施
の形態においては、前記第１の実施の形態と同様に、人
工衛星の質量・イナーシャがマニピュレータの質量・イ
ナーシャに比べて大きく、マニピュレータの動作が人工
衛星の位置、姿勢にほとんど影響しない場合、あるい
は、マニピュレータが、人工衛星上にあるターゲット
（図示せず）を把持する場合を想定している。

【００２３】この図４では、図２と同一機能部分が同一
符号で示されている。従って、重複する部分の詳しい説
明は省略する。

【００２４】この実施の形態においては、マニピュレー
タの各関節に装備されている力覚センサ３２０の代わり
に、マニピュレータのエンドエフェクタの近傍に力覚セ
ンサ３２１を備えて、力覚計算の中で、エンドエフェク
タ近傍の力覚情報を各関節のトルクに変換する。この変
換の方法には、例えば以下の（９）式の様な方法があ
る。この変換はヤコビ行列を生成する部分であるマニピ
ュレータ関節速度生成器３４１にて行われる。

【００２５】 τ ＝ ｔＪ・ｆ …（９） τ ：マニピュレータ関節に加わるトルクのベクト
ル ｔＪ ：ヤコビ行列の転置行列 ｆ ：マニピュレータ先端に加わる力、モーメント
のベクトル（力覚センサ３２１にて検出） 前記の実施の形態のいずれにおいても、マニピュレータ
の制御に関する部分と人工衛星の制御に関する部分をそ
れぞれ別々の処理装置により制御することが可能であ
る。また、マニピュレータと宇宙機本体の制御演算を簡
素化し、演算処理時間を短縮することができる。さら
に、宇宙機本体に搭載されるマニピュレータは、一台で
はなく、複数台であっても同一の制御アルゴリズムで制
御することができる。

【００２６】なお、上述の実施の形態では、速度制御を
行うマニピュレータについて説明したが、加速度制御や
トルク制御を行うマニピュレータについても、同様の制
御を行うことが可能である。

【００２７】また、前記各実施の形態においては、マニ
ピュレータを設けた人工衛星の場合について説明した
が、宇宙機が宇宙往還機、ＯＴＶ(宇宙輸送機)、ＯＳＶ
（軌道上作業機）、宇宙ステーションなどであってもよ
い。

【００２８】

【発明の効果】以上説明した通り、本発明によれば、宇
宙機本体の姿勢変化による宇宙機搭載マニピュレータへ
の外乱の影響を除去することができる。

【００２９】また、マニピュレータ制御を行う処理と宇
宙機姿勢制御を行う処理を別々の処理装置の中に設ける
ことが可能であり、効率的な処理が可能で、また、各処
理装置は、他の処理装置とは独立に設けることが可能で
ある。このため、宇宙機本体への搭載マニピュレータを
複数台に拡張することも容易となる。

【図面の簡単な説明】

【図１】本発明にかかる制御装置によって制御される宇
宙機の概観図である。

【図２】本発明にかかる第一の実施例の制御装置のブロ
ック線図である。

【図３】本発明にかかる第二の実施例の制御装置のブロ
ック線図である。

【図４】本発明にかかる第三の実施例の制御装置のブロ
ック線図である。

【符号及び記号の説明】

１００・・・宇宙機 ２００・・・人工衛星 ２０１・・・人工衛星のダイナミクス ２１０・・・センサ ２２０・・・視覚装置 ２３０・・・推進装置 ２４０・・・人工衛星の姿勢指令生成器、２４１・・・人工衛
星の姿勢指令生成器 ２５０・・・マニピュレータの外乱影響評価器、 ２６０・・・人工衛星の姿勢制御補正器 ３００・・・マニピュレータ ３０１・・・マニピュレータのダイナミクス ３１０・・・マニピュレータのエンドエフェクタ ３２０・・・力覚センサ、３２１・・・力覚センサ ３３０・・・サーボセンサ ３４０・・・マニピュレータの関節速度生成器 ３４１・・・マニピュレータの関節速度生成器 ３５０・・・マニピュレータの先端速度生成器 ３５１・・・マニピュレータの先端速度生成器 ３６０・・・マニピュレータの軌道生成器 ３７０・・・マニピュレータの制御系 ３８０・・・モータドライバ

Claims (2)

【特許請求の範囲】
1. 【請求項１】マニピュレータを搭載した宇宙機の制御装
   置において、前記宇宙機本体に、前記マニピュレータの
   動作を監視する視覚装置と、宇宙機本体の位置、姿勢お
   よび速度を検出する位置／姿勢センサとを設け、前記マ
   ニピュレータに、マニピュレータの関節で発生するトル
   クを検出する力覚センサを設け、前記位置／姿勢センサ
   の出力に基づいて、前記宇宙機の位置、姿勢の制御指令
   を生成する姿勢指令生成器と、前記位置／姿勢センサの
   出力と前記力覚センサの出力に基づいて前記宇宙機本体
   の位置、姿勢の変化が前記マニピュレータに与える変化
   量を評価する外乱影響評価器と、この外乱影響評価器に
   出力に基づいて前記姿勢指令生成器の出力を補正する姿
   勢制御指令補正器とから成り、宇宙機の位置、姿勢を制
   御する第１の制御装置を設け、前記視覚装置の出力に基
   づいて前記マニピュレータを制御する第２の制御装置を
   設けたことを特徴とする宇宙機の制御装置。
2. 【請求項２】前記宇宙機本体に、前記マニピュレータと
   第２の制御装置を複数台搭載して成る請求項１に記載の
   宇宙機の制御装置。