JP2929328B2 - 浮遊ロボットの制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、宇宙空間または水中で
作業を行うマニピュレータが備えられた浮遊ロボットに
おけるロボット本体の位置および姿勢を制御するための
装置に関する。

【０００２】

【従来の技術】典型的な先行技術は図９に示されてい
る。人工衛星の本体１の位置および姿勢を制御するため
に、その本体１にはセンサ２が設けられ、これによって
本体１の位置および姿勢が検出され、減算回路３に与え
られる。減算回路３にはまた、目標となる位置および姿
勢を表す信号が与えられる。減算回路３の出力は、本体
１に設けられた推進力を発生するための推進機であるス
ラスタ４を制御するためのスラスタ制御回路５に与えら
れ、こうして本体１の位置および姿勢が制御される。

【０００３】この図９に示される先行技術のブロック図
は、図１０である。制御回路５の出力であるスラスタ噴
射指令信号はスラスタ４に与えられ、これによって本体
１に推力およびトルクが発生され、参照符６で示される
ように、本体１のダイナミックスすなわち動力学の入出
力特性が得られ、その本体１の位置および姿勢が、セン
サ２によって前述のように検出されて、減算回路３に与
えられる。

【０００４】このような先行技術において、さらに図１
１に示されるように、本体に複数のリンクを備えるマニ
ピュレータが設けられているとき、図１１に示されるブ
ロック図が構成されることになる。参照符９で示される
ようにマニピュレータによる反力およびトルクは、加算
回路１０で示されるように、スラスタ４による推力およ
びトルクに加算される。したがってマニピュレータから
の反力とトルクによって、衛星本体１の動き、すなわち
位置および姿勢が乱されることになり、このマニピュレ
ータからの反力とトルクによって発生する本体１の位置
および姿勢の誤差を修正するようにスラスタ４が作動さ
れる。すなわち、マニピュレータからの反力とトルクに
より、衛星の動き（位置、姿勢）が乱される。従来の姿
勢制御では、その結果、発生する位置や姿勢の誤差を修
正するようにスラスタ４を作動させる。

【０００５】このような図９〜図１１に示される先行技
術では、本体１がマニピュレータの反力とトルクによっ
て動いた後に、スラスタ４が作動されることになり、応
答速度が低いという問題がある。

【０００６】さらに他の先行技術のブロック図は、図１
２に示されている。この方法は、マニピュレータの運動
と衛星本体の運動の相互干渉を考慮して、それぞれの動
きが目的に合致するように、一度にスラスタ推力とマニ
ピュレータ関節トルクを決定する方法である。この先行
技術では、浮遊ロボットを多体リンク系として、その運
動方程式から各マニピュレータの各関節トルクが本体の
運動に及ぼす影響を数式的に表現し、その数式を解くこ
とによって、本体の推進力を決定する。この図１２の先
行技術では、制御回路である補償器１１からのスラスタ
噴射指令信号はスラスタ４に与えられ、またマニピュレ
ータのトルク指令信号は関節アクチュエータ１２に与え
られ、こうしてスラスタ４によって発生される本体の推
進力およびトルク、ならびに関節アクチュエータ１２に
よる関節トルクによって本体１とマニピュレータの複合
系ダイナミクス１３が形成され、センサ２は本体１の位
置および姿勢を検出して減算器３に与え、この減算器３
には本体１の目標となる位置および姿勢を表す信号が与
えられ、また複合系ダイナミクス１３からのマニピュレ
ータ制御変数、すなわち位置、姿勢、関節角、速度、手
先反力などを表す信号は減算回路１４に与えられ、この
減算回路１４には、マニピュレータの制御変数の目標値
が与えられ、これらの減算器３，１４の出力は制御回路
１１に与えられる。すなわちマニピュレータの運動と、
本体の運動の相互干渉を考慮して、それぞれの動きが目
的に合致するように、一度にスラスタ推進力とマニピュ
レータ関節トルクを決定する。これによって正確に本体
の動きを制御することができるという利点があるけれど
も、次のような欠点（１）および（２）がある。

【０００７】（１）対象のダイナミクスが一般的に複雑
であり、マニピュレータが６軸であれば、本体１と併せ
て７個の剛体がつながったリンクのダイナミクスにな
る。マニピュレータが複数になると、さらに飛躍的に複
雑なダイナミクスになる。したがって現存の計算機で実
現するのは困難であり、特に宇宙空間に設けられること
ができるような小形の計算機で数式を解くのは、不可能
に近い。

【０００８】（２）またこの先行技術ではマニピュレー
タの各リンクおよび本体の質量、慣性モーメント、重心
位置などの多くの力学パラメータが必要となり、これら
の力学パラメータは、すべて正しい値を用いなければ、
正しい制御を実現することはできない。したがってマニ
ピュレータで物をつかんだりした場合、つかんだ物の質
量や慣性モーメント、重心位置などを正しく与えなけれ
ばならない。したがってつかむ可能性のある物のデータ
ベースを持つか、対象の力学特性を計測する必要があ
り、現実的には、用途が限られてしまうという問題があ
る。

【０００９】本発明の目的は、比較的簡単な構成で、浮
遊ロボットの本体の位置および姿勢などを正確に制御す
ることができるようにした浮遊ロボットの制御装置を提
供することである。

【００１０】

【課題を解決するための手段】本発明は、マニピュレー
タが設けられ、宇宙空間または水中で作業を行う浮遊ロ
ボットの制御装置において、ロボット本体に設けられ、
ガスジェットを噴射して推進力を発生して位置の３自由
度と姿勢の３自由度とを制御する手段と、ロボット本体
の位置および姿勢の目標値を設定する目標値設定手段
と、ロボット本体の位置および姿勢を検出する第１検出
手段と、ロボット本体と、そのロボット本体に設けられ
るマニピュレータとの間に介在され、マニピュレータに
よってロボット本体に作用する反力とトルクとを検出す
る第２検出手段と、目標値設定手段と第１検出手段と第
２検出手段との各出力に応答して、ロボット本体が目標
値設定手段で設定した目標値からずれるのを防ぐため
に、マニピュレータからの反力とトルクとを打ち消すよ
うに推進力発生手段を駆動する手段とを含むことを特徴
とする浮遊ロボットの制御装置である。

【００１１】

【作用】本発明に従えば、本体の位置および姿勢を第１
検出手段によって検出し、マニピュレータとそれが取付
けられている浮遊ロボット本体（以下、本体と言うこと
がある）との間に、第２検出手段を介在して可動物体の
運動による本体に及ぼす影響を検出し、こうして得られ
る第１および第２検出手段からの出力と本体の位置およ
び姿勢の予め設定されている目標値とに応答して、その
目標値からずれるのを防ぐようにスラスタである推進力
発生手段を制御する。こうしてマニピュレータをどう動
かそうと、それらが本体に及ぼす影響は、マニピュレー
タの付け根の力とトルクに集約される。したがってその
マニピュレータの付け根における本体に作用する反力と
トルクを第２検出手段によって検出すれば、力学パラメ
ータおよびハンドリングされるべき物体がどうなってい
ようと、直接的に本体への影響が判り、容易に、その影
響を補正した本体の制御が可能となる。

【００１２】第１検出手段によって検出される本体の位
置および姿勢だけでなく、さらにその第１検出手段によ
って、速度および加速度が検出されてもよく、目標値設
定手段では、本体の位置および姿勢だけでなく、その他
さらに速度および加速度が設定され、その目標値となる
ように、推進力発生手段が制御されるようにしてもよ
い。

【００１３】

【実施例】図１は本発明の一実施例の全体のブロック図
であり、図２はスラスタ２４を駆動する構成を示すブロ
ック図であり、図３はその具体的な構成を示す図であ
る。この実施例では、本体１７とマニピュレータ２０と
を切り放して考え、マニピュレータ２０から本体１７へ
の影響は、マニピュレータ２０の根元に発生する力とト
ルクに集約されるのであるから、それらを打ち消すよう
な信号をスラスタ２４の噴射指令に加えてやれば、マニ
ピュレータ２０の影響を受けずに衛星本体の姿勢制御が
実現できる。

【００１４】浮遊するロボット本体１７は、位置の３自
由度と姿勢（方向）の３自由度の計６個の自由度を持
ち、ガスジェットの推進機であるスラスタ２４は、それ
らの６個の自由度を制御するのに必要な個数が設置され
ている。スラスタ２４としてガスジェットを用いた場
合、ガスボンベ２４ｃとノズル２４ａの間に弁２４ｂを
置き、その弁２４ｂの開閉のタイミングによって制御を
行う。すなわち、噴射されるジエットの強さは一定で、
ジエットを噴射する時間の長さと頻度でその効き目を調
整する。

【００１５】弁開閉指令は、正の定められた値とゼロお
よび負の定められた値の３つの値しかとらず、弁開閉指
令が正の値になったときは正方向に力を発生するスラス
タ２４の弁２４ｂのみを開き、弁開閉指令が負の値にな
ったときは負方向に力を発生するスラスタ２４の弁２４
ｂのみを開き、弁開閉指令がゼロのときはいずれの弁２
４ｂも閉じる。また、力指令から弁開閉指令に到るブロ
ックは、連続の値をとる力指令に対応してパルス列を作
り出す回路の一例を示している。力指令がゼロであれば
弁開閉指令のパルスは全く出ず、力指令がプラスになる
と、弁開閉指令として正のパルスが出力され、力指令の
値が大きくなるに従って出力パルス幅が増大する。力指
令がある値を越すと、出力パルスはオフにならずに出続
ける。また、マイナスの力指令に対しては、マイナスの
パルスが出力され、力指令がマイナス方向に増大するに
従って、そのパルス幅は増大し、ついには負のパルスが
出続ける。この図２における力指令から弁開閉指令を生
成する方式によるとスラスタ２４が発生できる力の範囲
内では、スラスタ２４の推力の時間平均が力指令に正比
例することは容易に示せる。これらのことから、この図
２で示された系においては位置指令からの実際の位置の
ずれに応じて力指令が生成され、スラスタ２４からは断
続的であるが平均すればその力指令に比例する力が宇宙
機に加えられる構成になっており、これによって宇宙機
である本体１７の位置が所定位置に保たれるよう制御さ
れる。

【００１６】前述のように、この図２に示した制御装置
は、宇宙機である本体１７の位置制御システムの１例で
あって、各部分の構成や制御手法について、種々の方法
があるのは明らかである。たとえば、位置だけでなく、
速度や加速度も検出して制御に用いれば、より精密な制
御が可能となる。以下の説明では本発明の作用、効果を
示すための例として、この図２の制御系に減衰をよくす
るために速度をフィードバックを付加したものをとりあ
げるが、異なった制御システムにおいても、以下で説明
する考え方でもって、本発明を実施することは容易であ
る。

【００１７】浮遊ロボットは、図２の宇宙機である本体
１７にマニピュレータ２０を取付けた図４のものとみな
すことができる。マニピュレータ２０が作業を行うため
に動くと、ロボット本体１７に対してその反動、すなわ
ち本体１７とマニピュレータ２０との結合状態を保つた
めの拘束力が作用し、その結果、ロボット本体１７の運
動が影響を受ける。また、その反動は、マニピュレータ
２０がどう動こうとマニピュレータ２０と本体１７との
接続部２０ａにおける並進力とモーメントにすべて集約
されることは明らかである。いま、本体１７とマニピュ
レータ２０の接続部２０ａにおいて本体１７に作用する
並進力をＦｄ、モーメントをＮｄ、本体重心Ｇから見た
接続位置の位置ベクトルをｒｄとおく。また、本体１７
が持っているスラスタ２４などの推進装置による並進力
およびモーメントをそれぞれＦｔおよびＮｔとおく。本
体１７には、これらの他の並進力やモーメントは働かな
いとする。本体１７の質量をＭ、慣性テンソルをＩとお
くと、本体１７の運動は、数１で示される運動方程式に
よって決定される。

【００１８】

【数１】

【００１９】ただし、ｖは本体速度ベクトル、ωは本体
角速度ベクトル、×はベクトル積記号である。

【００２０】いま、ロボット本体１７に加えたい並進力
をＦｔ０、モーメントをＮｔ０とすると、マニピュレー
タ２０からの反動による並進力とモーメントを打ち消す
ためには、スラスタ２４などの推力発生手段に

【００２１】

【数２】Ｆｔ＝Ｆｔ０−Ｆｄ なる並進力と、 Ｎｔ＝Ｎｔ０−（Ｎｄ＋ｒｄ×Ｆｄ） なるモーメントを発生させればよいことになる。たとえ
ば、ｎ個のスラスタ２４を用いてこれを実現するには、
各スラスタ２４の推力Ｆｔ１が作用する点の本体重心に
対する位置ベクトルをｒｔ１とすると、

【００２２】

【数４】

【００２３】

【数５】

【００２４】を満たすようにＦｔ１を決めればよい。

【００２５】本発明で重要なことは、 （１）浮遊ロボット本体の推進力およびモーメントを数
２および数３によってそれぞれ決定すること、および （２）数２、数３に含まれるＮｄ，Ｆｄを検知するため
に、マニピュレータ２０とロボット本体１７の接続部分
に力／トルクセンサを設置し、その信号を用いてロボッ
ト本体１７の推進力とモーメントを制御すること、の２
点を組合せた浮遊ロボットの制御装置である。

【００２６】前述の図１と図２の構成に、本発明による
制御を組込んだ場合の例を図４に示す。

【００２７】このような構成によれば、マニピュレータ
２０からの反力とモーメントが正確に検出でき、推進力
とモーメントを直接厳密に制御できる場合は、本発明が
理想的な結果をもたらすことは明らかである。ここで
は、マニピュレータ２０からの力とモーメントの検出は
理想的で、ロボット本体１７の推進力と推進モーメント
が図１あるいは図２のスラスタ２４を使った場合で示す
ように、一定値のオン／オフでしか発生できない場合に
ついて示す。本発明の効果を示すことが目的であるから
モデルは簡単化して、１自由度の質点とし推進力は正お
よび負の定まった値とゼロの３つの値のみをとれること
とし、力指令からスラスタのオン／オフへの変換は図１
あるいは図２で示した方法をとることとする。この場合
の制御ブロック図を、図５に示す。この外乱補償付き位
置制御系ブロック図の一次元モデルにおいて、 Ｍ：ロボット本体質量 Ｋｖ，Ｇｃ，Ｋｉ：制御ゲイン である。∫は、積分を示す。

【００２８】この図５のブロック図の下でシミュレーシ
ョンを行った結果を、以下に示す。

【００２９】図６は外乱力は働かず、本体１７が目標位
置からずれている状態でこの制御を働かせたときの応答
で、スラスタ２４の数回のオン／オフの後に本体は目標
位置に収束している。

【００３０】図７、図８は、外乱が加わった場合の応答
で、図７は外乱補償をしない場合、すなわち図５の外乱
補償オン／オフスイッチがオフの場合、図８は本発明に
基づく補償を加えた場合、すなわち図５の外乱補償オン
／オフスイッチがオンの場合の応答である。図７におい
ては、本体１７の位置は目標からずれたところへ収束す
るのに対し、図８においては外乱が加わった当初にわず
かに偏位が生じるが、すぐに目標位置に収束している。
この例より、本発明の効果は明らかである。図６
（１）、図７（１）および図８（１）は推力は示し、図
６（２）、図７（２）および図８（２）は速度を示し、
図６（３）、図７（３）および図８（３）は変位を示
す。これらの図面を参照符して、宇宙空間および水中な
どに設けられる浮遊ロボット１６の本体１７には、複数
のリンク１８と複数の関節１９とを有するマニピュレー
タ２０が、設けられる。本体１７の位置および姿勢、さ
らには速度および加速度を検出するために、第１検出手
段２１がその本体１７に設けられる。本体１７とマニピ
ュレータ２０の付け根との間には、第２検出手段２２が
設けられる。この第２検出手段２２は、マニピュレータ
２０による本体１７に作用する力とトルクを検出する。
マニピュレータ２０の手先２３は、ハンドリングすべき
物体をつかむことなどができる。本体１７には、希望す
る方向に本体１７に推進力を発生するための手段である
スラスタ２４が取付けられる。

【００３１】本体１７の位置および姿勢、さらには速度
および加速度の目標値は、目標値設定手段２５によって
設定される。その目標値は減算回路２６に与えられる。
減算回路２６には、第１検出手段２１によって検出され
た本体１７の位置および姿勢を表す信号が与えられる。
減算回路２６からの出力は補償器である制御回路２７に
与えられる。制御回路２７からライン２８を介するスラ
スタ２４のための信号は、減算回路２９に与えられる。
減算回路２９から出力は減算回路３２に与えられ、その
減算回路３２の出力は積分回路３３に与えられて積分さ
れ、ヒステリシス回路３４から弁開閉指令信号が導出さ
れ、弁２４ｂに対応する駆動回路３５，３６にそれぞれ
与えられ、この弁開閉指令信号はまた、減算回路３２に
与えられる。

【００３２】マニピュレータ２０の動きによる本体１７
に作用する反力およびトルクは、第２検出手段２２によ
って検出され、制御回路２７からの出力に対応するよう
に換算回路３０によって換算され、こうして得られた信
号は、加算回路２９に与えられ、こうして加算回路２９
からは前述のように、スラスタ２４のための信号が得ら
れる。このようにして、マニピュレータ２０による本体
１７に作用する反力およびトルクを打ち消すための信号
が換算回路３０から発生されて得られ、したがってマニ
ピュレータ２０による本体１７の目標位置および姿勢か
らのずれをなくすのではなく、マニピュレータ２０によ
る外乱によって、本体１７の位置および姿勢の制御系が
乱れるのを防ぐ。こうして本体１７がマニピュレータ２
０による外乱によって動いてしまうよりも先に、換算回
路３０から得られる信号によってスラスタ２４を動作さ
せて、本体１７への影響を打ち消し、本体１７がマニピ
ュレータ２０による外乱で動くことを防ぐ。

【００３３】上述の実施例では本体１７の外部にマニピ
ュレータ２０が備えられているけれども、本発明の他の
実施例として、本体１７内に形成された空間に、マニピ
ュレータ２０の作業空間が設けられるようにしてもよ
く、このようにマニピュレータ２０が本体１７の内部空
間に設けられるときにおいても、本発明が実施される。

【００３４】

【発明の効果】以上のように本発明によれば、第１検出
手段によってロボット本体の位置および姿勢を検出し、
また本体とマニピュレータとの間に第２検出手段を介在
して、ロボット本体に作用する反力とトルクを検出し、
こうして本体に設けられている推進力発生手段を、目標
値設定手段によって設定されている本体の位置および姿
勢となるように制御することによって、簡単な構成で、
しかも本体の位置および姿勢がマニピュレータの影響に
よって動いてしまうよりも先に、推進力発生手段によっ
てそのような外乱による位置および姿勢の変動を打ち消
して、本体が、マニピュレータの動きである外乱によっ
て変動することを防ぐことができる。

【図面の簡単な説明】

【図１】本発明の一実施例の全体のブロック図である。

【図２】図１に示される実施例の具体的なブロック図で
ある。

【図３】本発明の一実施例の具体的な構成を示す図であ
る。

【図４】図３に示される実施例の電気的な構成を示すブ
ロック図である。

【図５】本発明の一実施例の本件発明者による一次元モ
デルを示すブロック図である。

【図６】本件発明者の実験結果を示すグラフである。

【図７】本件発明者の実験結果を示すグラフである。

【図８】本件発明者の実験結果を示すさらに他のグラフ
である。

【図９】先行技術の衛生姿勢制御系を示す構成の図であ
る。

【図１０】図９に示される先行技術のブロック図であ
る。

【図１１】図９および図１０に示される先行技術におい
てマニピュレータを備えたときの先行技術の構成を示す
ブロック図である。

【図１２】他の先行技術を示すブロック図である。

【符号の説明】

１６ 浮遊ロボット １７ ロボット本体 ２０ マニピュレータ ２１ 第１検出手段 ２２ 第２検出手段 ２４ スラスタ

フロントページの続き (56)参考文献 特開 平３−62205（ＪＰ，Ａ) 特開 昭62−18398（ＪＰ，Ａ) 特開 昭58−67600（ＪＰ，Ａ) 特開 平４−85199（ＪＰ，Ａ) 特開 平１−289683（ＪＰ，Ａ) 特開 平５−178295（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁶，ＤＢ名) B25J 3/04 B64G 1/24 G05D 1/10

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 マニピュレータが設けられ、宇宙空間ま
   たは水中で作業を行う浮遊ロボットの制御装置におい
   て、 ロボット本体に設けられ、ガスジェットを噴射して推進
   力を発生して位置の３自由度と姿勢の３自由度とを制御
   する手段と、 ロボット本体の位置および姿勢の目標値を設定する目標
   値設定手段と、 ロボット本体の位置および姿勢を検出する第１検出手段
   と、 ロボット本体と、そのロボット本体に設けられるマニピ
   ュレータとの間に介在され、マニピュレータによってロ
   ボット本体に作用する反力とトルクとを検出する第２検
   出手段と、 目標値設定手段と第１検出手段と第２検出手段との各出
   力に応答して、ロボット本体が目標値設定手段で設定し
   た目標値からずれるのを防ぐために、マニピュレータか
   らの反力とトルクとを打ち消すように推進力発生手段を
   駆動する手段とを含むことを特徴とする浮遊ロボットの
   制御装置。