JP2009298345A

JP2009298345A - 姿勢制御装置および位置制御装置

Info

Publication number: JP2009298345A
Application number: JP2008156659A
Authority: JP
Inventors: Yasuyuki Watanabe; 泰之渡邉; Katsuhiko Yamada; 克彦山田; Takeya Shima; 岳也島; Sanae Ishikawa; 早苗石川
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2008-06-16
Filing date: 2008-06-16
Publication date: 2009-12-24
Anticipated expiration: 2028-06-16
Also published as: JP5228641B2

Abstract

【課題】複数のコントロールモーメントジャイロを用いて人工衛星の三軸姿勢制御を行う姿勢制御装置では、姿勢制御トルクの目標値からジンバル角速度の目標値を計算する際に、特異点における不安定な挙動を避けるためにヤコビ行列の擬似逆行列の近似式を用いることが多いが、ヤコビ行列の擬似逆行列の近似式を用いると近似誤差が生じ、特異点の近傍では姿勢制御誤差が大きくなるという課題があった。
【解決手段】姿勢角誤差が小さくなるような姿勢制御トルクを算出するレギュレータと、姿勢制御トルクを出力するための各ＣＭＧのジンバル角速度を算出するＣＭＧ分配則部と、ＣＭＧ分配則部で算出されるジンバル角速度に、所望の姿勢運動に力学的に整合するフィードフォワードジンバル角速度を加える加算器を備えることで、特異点とその近傍において閉ループ制御と開ループ制御が連続的に切り替わり、特異点での姿勢制御誤差を低減することができる。
【選択図】図３

Description

この発明は、人工衛星に搭載されたコントロールモーメントジャイロ（以下、ＣＭＧ；Control Moment Gyro）を用いて姿勢制御を行う人工衛星の姿勢制御装置、および複数の関節を持つマニピュレータの手先位置を制御する位置制御装置に関するものである。

複数のＣＭＧを用いて、人工衛星の三軸姿勢制御を行う姿勢制御装置が知られている（例えば、特許文献１参照）。このようなシステムでは、各ＣＭＧのジンバル角度の組み合わせによりある軸まわりのトルクを出力できない特異点の状態が存在する。このため、特異点における不安定な挙動を避けるためにヤコビ行列の擬似逆行列の近似式を用いて、姿勢制御トルクの目標値からジンバル角速度の目標値を計算して制御している。

また、類似したシステムの例として、複数の関節を持つマニピュレータにおいて、各関節を駆動することによりマニピュレータ先端の位置・速度を制御する制御装置が知られている（例えば、特許文献２参照）。このようなシステムでは、フィードバック制御とフィードフォワード制御を組み合わせてマニピュレータ先端速度の目標値を計算する。この際、特異点における不安定な挙動を避けるためにヤコビ行列の近似逆行列を用いて、マニピュレータ先端速度の目標値から関節角速度の目標値を計算して制御している。

特表２００２−５０６７７３号公報特開平７−３１４３６３号公報

しかしながら、ヤコビ行列の擬似逆行列の近似式を用いると近似誤差が生じる。このため、このようにして計算されたジンバル角速度からは所望の姿勢運動を実現することができず、特異点の近傍では姿勢制御誤差が大きくなるという課題があった。

この発明は係る課題を解決するためになされたものであり、特異点とその近傍においても所望の姿勢運動を実現することのできる姿勢制御装置を得ることを目的とする。

また、この発明は係る課題を解決するためになされたものであり、特異点とその近傍においても所望の手先運動を実現することのできる位置制御装置を得ることを目的とする。

この発明による姿勢制御装置は、人工衛星の所望の姿勢運動に応じて求められるフィードフォワードジンバル角速度および当該人工衛星の目標姿勢角を計算するとともに、当該人工衛星の姿勢および複数のコントロールモーメントジャイロの回転を制御する姿勢コントローラを備え、上記姿勢コントローラは、上記目標姿勢角と観測される人工衛星の姿勢角との姿勢角誤差に基づいて姿勢制御トルクを計算するフィードバック制御系を有し、上記コントロールモーメントジャイロにおける特異点近傍で上記フィードフォワードジンバル角速度の成分に比して上記コントロールモーメントジャイロのジンバル角速度の成分が小さくなるように、上記フィードバック制御系で計算された姿勢制御トルクから上記コントロールモーメントジャイロのジンバル角速度の成分を求め、求めたジンバル角速度の成分と上記フィードフォワードジンバル角速度の組合せに基づいて、上記コントロールモーメントジャイロに上記人工衛星の姿勢制御トルクを発生させる、ものである。

また、この発明による位置制御装置は、複数の関節を持つマニピュレータの手先位置を制御する位置制御装置であって、上記マニピュレータの手先位置誤差に基づくフィードバック制御速度を出力するための関節角速度を計算する関節分配則部と、上記マニピュレータの所望の手先運動に基づいてフィードフォワード関節角速度を計算するマニュピレータ運動計画部と、上記関節分配則部で計算される関節角速度と上記マニュピレータ運動計画部で計算されるフィードフォワード関節角速度とを組み合わせ、上記マニュピレータの特異点において上記フィードバック制御速度の大きさが０になるように、当該特異点に近づくに従ってフィードバック制御速度の大きさを小さくし、フィードバック制御とフィードフォワード制御を連続的に切り替えることを特徴としたものである。

この発明に係る姿勢制御装置によれば、人工衛星の姿勢のフィードバック制御と、ＣＭＧのジンバル軸のフィードフォワード制御を組み合わせて、人工衛星の三軸姿勢制御を行うことにより、特異点とその近傍においても、人工衛星の姿勢制御誤差を低減することができる。

また、この発明に係る位置制御装置によれば、マニピュレータの手先位置のフィードバック制御と、マニピュレータの関節角速度のフィードフォワード制御を組み合わせて、マニピュレータの手先位置の位置制御を行うことにより、マニピュレータの特異点とその近傍においても、マニピュレータの手先位置制御誤差を低減することができる。

実施の形態１．
以下、図を用いてこの発明に係る実施の形態１について説明する。
図１は、実施の形態１による人工衛星用姿勢制御装置の構成を示す図である。人工衛星の三軸姿勢制御を行うには３台以上のＣＭＧが必要であり、図１では冗長系を持つ４台のＣＭＧを配置した例を示している。

各ＣＭＧはＣＭＧメカニズム（ＣＭＧ機構部）１とＣＭＧコントローラ（CMG Controller）２から構成される。ＣＭＧメカニズム１はＣＭＧコントローラ２により制御される。ＣＭＧコントローラ２はそれぞれ姿勢コントローラ（Attitude Controller）３に接続されて、姿勢コントローラ３からの制御信号に従ってＣＭＧメカニズム１を制御するとともに、姿勢コントローラ３にＣＭＧメカニズム１のロータ角速度やジンバル角度等の情報を出力する。姿勢コントローラ３では、人工衛星の姿勢を制御するために各ＣＭＧメカニズム１が出力すべきトルクを計算し、それに基づいて各ＣＭＧコントローラ２に制御信号を出力する。なお、ＣＭＧメカニズム１とＣＭＧコントローラ２と姿勢コントローラ３と人工衛星１５とで、人工衛星用姿勢制御装置の姿勢制御系が構成される。

図２は、実施の形態１の人工衛星用姿勢制御装置におけるＣＭＧメカニズム１の構成を示す図である。ロータ４は、スピン軸５のまわりでスピンモータ６により回転し、スピンモータ６を介してジンバル７に接続される。ジンバル７は、スピン軸５に概ね直交するジンバル軸８のまわりでジンバルモータ９により回転し、ジンバルモータ９を介してフレーム１０に接続される。フレーム１０は人工衛星に固定される。ＣＭＧメカニズム１は、スピン軸５のまわりで概ね一定角速度で回転するロータ４をジンバル軸８のまわりで回転させることにより、スピン軸５とジンバル軸８に直交する方向にトルクを出力する。スピンモータ６は、ＣＭＧコントローラ２により回転速度が一定の大きさになるように制御される。また、ジンバルモータ９は、ＣＭＧコントローラ２により回転が制御され、入力されるジンバル角速度目標値に応じてモータのコイルに印加される電流値が調整され、調整される電流値に応じて出力トルクが所望の大きさに調整される。

図３は、実施の形態１の人工衛星用姿勢制御装置における姿勢制御系の構成を示している。姿勢運動計画部１１とレギュレータ（フィードバック制御系）１２とＣＭＧ分配則部１３と姿勢決定系１６は、ＣＰＵやメモリやロジック回路などを用いて姿勢コントローラ３に実装される。姿勢運動計画部１１は姿勢角目標値（α_ｄ）２２を計算する。姿勢決定系１６は、スターセンサや地球センサおよび姿勢検出ジャイロ等を用いて人工衛星１５の姿勢角（α）を観測し、姿勢角検出値（α_ｓ）２９を得る。姿勢運動計画部１１により計算される姿勢角目標値２２と、姿勢決定系１６により観測される姿勢角検出値２９との差が、姿勢角誤差（ε_α）２３となる。レギュレータ１２では、姿勢角誤差２３の値に基づいて、姿勢角誤差２３が小さくなるような姿勢制御トルク（ベクトルτ_ｃ）２４を算出する。ＣＭＧ分配則部１３では、姿勢制御トルク２４を出力するための各ＣＭＧのジンバル角速度（ベクトルγ_ｃ’）２５を算出する。加算器１００は、ＣＭＧ分配則部１３で算出されるジンバル角速度２５に、姿勢運動計画部１１で計算されるフィードフォワードジンバル角速度２１を加えて、ジンバル角速度目標値（γ_ｇ’）２６を算出する。ＣＭＧ１４では、ジンバル角速度目標値２６に従ってジンバル７の回転を制御し、出力トルク（τ_ａ）２７を人工衛星１５に作用させる。人工衛星１５では、ＣＭＧ１４の出力トルク２７が作用することにより力学法則に従って姿勢角（α）２８が変化する。レギュレータ１２、ＣＭＧ分配則部１３、ＣＭＧ１４、人工衛星１５、および姿勢決定系１６は、姿勢角目標値２２と姿勢角検出値２９との差から得られる姿勢角誤差（ε_α）２３をレギュレータ１２に入力することで、フィードバック制御系の閉ループを構成する。また、姿勢運動計画部１１で計算されるフィードフォワードジンバル角速度２１を、加算器１００を介してＣＭＧ１４に入力することでフィードフォワード制御系の開ループが構成される。

以下、ＣＭＧ分配則部１３の動作について説明する。ＣＭＧの台数がＮ台であるとき、姿勢制御トルク２４とジンバル角速度２５との間には、次式の関係がある。

式（１）において、τ_ｃは姿勢制御トルクベクトル（３次元）、γ _c'はジンバル角速度ベクトル（Ｎ次元）、Jはヤコビ行列（３×Ｎ次元）である。ヤコビ行列Jはジンバル角度の関数であり、ジンバル角度によって値が変化する。

なお、姿勢制御トルク２４からジンバル角速度２５を算出するには、次式のように、姿勢制御トルクベクトルにヤコビ行列の擬似逆行列J^＃を掛けてジンバル角速度ベクトルγ _c'を求める従来の方法がある。

式（３）において、行列の右肩の添え字Ｔは転置を意味する。この従来の方法を用いると、ヤコビ行列のランクが２以下となる各ＣＭＧのジンバル角度の組み合わせ（特異点）では、行列式det(JJ^T)の値が０となるため、式（３）の計算ができない。また、特異点の近傍においてはdet(JJ^T)の値が０に近くなるため、式（２）により算出されるジンバル角速度が非常に大きくなり、ＣＭＧの性能を超えたり不安定な動作をしたりする可能性がある。その場合には、人工衛星の姿勢運動が異常となるため、特異点の近傍ではヤコビ行列の擬似逆行列を用いることは望ましくなく、様々な近似式が考案されているところである（例えば、特許文献１、特許文献２参照）。

一方、この実施の形態１では、ＣＭＧ分配則部１３において、姿勢制御トルク２４からジンバル角速度２５を算出するために、次式（４）、（５）を用いる。

ここで、分配行列J^＋を求める計算式（５）において、max()は最大値、adj( )は余因子行列を示す。特異点ではdet(JJ^T)＝０となることから、αは特異点近傍においてジンバル角速度２５が過大にならないように設定する閾値である。

各ＣＭＧのジンバル角度の組み合わせが特異点に近くない場合には、det(JJ^T)は十分大きいためαより小さくならず、式（５）は次式のようになる。

行列式の逆数と余因子行列の積は逆行列に等しいため、式（６）は式（３）と同一であり、分配行列J⁺はヤコビ行列の擬似逆行列に一致する。このとき、ＣＭＧ分配則部１３では、姿勢制御トルク２４を出力するジンバル角速度２５が算出される。

各ＣＭＧのジンバル角度の組み合わせが特異点に近づいて、det(JJ^T)がαより小さくなると、式（５）は次式のようになる。

このとき、分配行列J⁺はヤコビ行列の擬似逆行列のdet(JJ^T)/α倍となるため、ＣＭＧ分配則部１３では、姿勢制御トルク２４のdet(JJ^T)/α倍のトルクを出力するジンバル角速度２５が算出される。従って、ジンバル角速度２５によってＣＭＧ１４から出力されるトルクの大きさは、特異点に近づくにしたがって小さくなり、特異点では０となる。このようにして、ＣＭＧ分配則部１３は、特異点近傍においてジンバル角速度２５が過大となることを防ぐ仕組みになっている。

ＣＭＧの台数が冗長系を持たない場合には、ヤコビ行列が正方行列となるため、分配行列J⁺として式（５）を用いる代わりに、他の態様として、次式を用いることもできる。

式（８）においてsgn( )は符号関数である。式（８）を用いると、特異点においてジンバル角速度２５が必ずしも０になるとは限らないため、冗長系の有無に関わらず式（５）を用いることが望ましい。また、式（５）を用いることにより、ＣＭＧの台数が変化した場合でも同じ計算方法を適用できるという利点もある。

図３に示す通り、ＣＭＧ１４に入力されるジンバル角速度目標値２６は、ジンバル角速度２５にフィードフォワードジンバル角速度２１を加算した値である。各ＣＭＧのジンバル角の組み合わせが特異点に近づくに従って、ＣＭＧ１４の出力トルク２７は、ジンバル角速度２５によって出力される成分（フィードバック制御トルク）が小さくなることにより、フィードフォワードジンバル角速度２１によって出力される成分（フィードフォワード制御トルク）が支配的となる。特異点においては、フィードフォワードジンバル角速度２１による成分のみが出力される。そのため、姿勢運動計画部１１において、所望の姿勢運動に力学的に整合するフィードフォワードジンバル角速度２１を計算することにより、特異点とその近傍において閉ループ制御と開ループ制御が連続的に自動的に切り替わり、所望の姿勢運動が実現される。

ここで、姿勢運動計画部１１のフィードフォワードジンバル角速度の動作について説明しておく。図４は、１台のＣＭＧにおけるジンバル角度の軌跡を示すものであり、その時間履歴を示している。

はじめに、姿勢運動計画部１１は、ＣＭＧにおけるジンバル角度の初期値１１５と最終値１１６を定めてその間を複数の区間に分割し、初期値１１５と最終値１１６を除く各区間の端点におけるジンバル角度１１７、１１８をパラメータとして設定する。図４では３区間に分割した場合を示している。

次に、姿勢運動計画部１１は、各区間でのジンバル角度の時間履歴１１９、１２０、１２１をそれぞれ時間関数（例えば、時間を変数とする多項式）で表現し、時間関数を定めるパラメータ（例えば、多項式の係数）を設定する。ここで、時間関数の変数は時間に限らず、時間と実質的に等価な変数（例えば、時間をある数値で割った変数）であっても良い。
各区間の時間関数は、区間端点において設定されたジンバル角度１１５、１１６、１１７、１１８を満足する関数である。

このようにして、使用する全てのＣＭＧに対してパラメータを設定し、それにより定まるジンバル角度の時間履歴に沿って、ＣＭＧ１４のジンバル角度の微分値をフィードフォワードジンバル角速度として求め、求めたフィードフォワードジンバル角速度に基づいてＣＭＧ１４のＣＭＧコントローラ２が各モータ９を駆動し、各ジンバルを回転動作させると、人工衛星１５の姿勢角はそれと力学的に整合した時間履歴に沿って運動する。

従って、人工衛星の姿勢角の最終値が目標値と同一か微小な差で目標値に一致するようにパラメータを定めることにより、姿勢角を目標値に収束させるように制御することができる。パラメータを定める方法としては、例えば、非線形計画法があるが、ここではその詳細な説明は割愛する。

以上説明したとおり、この実施の形態１によれば、ＣＭＧ分配則部１３とフィードフォワードジンバル角速度２１を組み合わせることにより、特異点とその近傍においても所望の姿勢運動が実現され、姿勢制御誤差が大きくなることなく特異点近傍を通過することが可能である。

さらに、ＣＭＧ分配則部１３において、分配行列Ｊ^＋として式（５）を用いる代わりに、別の態様として、ヤコビ行列の擬似逆行列の異なる近似式を用いても良い。例えば、次式のような近似式がある。

式（９）において、I は単位行列であり、k は特異点において（JJ^T）^−１が算出できなくなることを防ぐために設定する微小な値である。

分配行列Ｊ^＋として式（５）とは異なる近似式を用いる場合には、特異点近傍において不要な姿勢駆動トルクが出力されて姿勢制御誤差が大きくなることがあるが、その場合でも、フィードフォワードジンバル角速度２１の効果により、特異点近傍を通過することが可能である。

以上説明したとおり、この実施の形態１による姿勢制御装置は、人工衛星の所望の姿勢運動に応じて求められるフィードフォワードジンバル角速度および当該人工衛星の目標姿勢角を計算するとともに、当該人工衛星の姿勢および複数のコントロールモーメントジャイロの回転を制御する姿勢コントローラを備え、上記姿勢コントローラは、上記目標姿勢角と観測される人工衛星の姿勢角との姿勢角誤差に基づいて姿勢制御トルクを計算するフィードバック制御系を有し、上記コントロールモーメントジャイロにおける特異点近傍で上記フィードフォワードジンバル角速度の成分に比して上記コントロールモーメントジャイロのジンバル角速度の成分が小さくなるように、上記フィードバック制御系で計算された姿勢制御トルクから上記コントロールモーメントジャイロのジンバル角速度の成分を求め、求めたジンバル角速度の成分と上記フィードフォワードジンバル角速度の組合せに基づいて、上記コントロールモーメントジャイロに上記人工衛星の姿勢制御トルクを発生させることを特徴とする。

また、複数のコントロールモーメントジャイロと、人工衛星の所望の姿勢運動に応じて求められるフィードフォワードジンバル角速度、および人工衛星の目標姿勢角を計算する姿勢運動計画部を有し、人工衛星の姿勢および上記コントロールモーメントジャイロの回転を制御する姿勢コントローラとを備え、上記姿勢コントローラは、上記目標姿勢角と観測される人工衛星の姿勢角との姿勢角誤差に基づいて姿勢制御トルクを計算するフィードバック制御系と、上記コントロールモーメントジャイロのジンバル角度をパラメータとしたヤコビ行列から構成される行列式と、上記コントロールモーメントジャイロの特異点で上記コントロールモーメントジャイロのジンバル角速度を０とする閾値との比較に基づいて分配行列を演算し、演算された分配行列と上記フィードバック制御系で計算された姿勢制御トルクに基づいて、上記コントロールモーメントジャイロのジンバル角速度を計算する分配則部と、上記分配則部で計算されたジンバル角速度とフィードフォワードジンバル角速度を加算する加算器とを具備し、上記コントロールモーメントジャイロは、上記加算器の加算結果に基づいて人工衛星の姿勢制御トルクを発生する、ことを特徴としても良い。

この際、分配則部は、出力ベクトルを入力ベクトルに変換するヤコビ行列と、ヤコビ行列の転置行列との積により行列を算出し、入力ベクトルに、その行列の逆行列と閾値との最大値の逆数と、その行列の余因子行列と、ヤコビ行列の転置行列とを掛けることにより、出力ベクトルを算出する。

これにより、複数のコントロールモーメントジャイロを用いた人工衛星の姿勢制御装置において、上記人工衛星の姿勢角誤差に基づくフィードバック制御トルクを出力するための上記コントロールモーメントジャイロのジンバル角速度を計算する分配則部と、上記人工衛星の所望の姿勢運動に基づいて上記コントロールモーメントジャイロのフィードフォワードジンバル角速度を計算する姿勢運動計画部と、上記分配則部で計算されるジンバル角速度と上記姿勢運動計画部で計算されるフィードフォワードジンバル角速度とを組み合わせ、上記コントロールモーメントジャイロの特異点において上記フィードバック制御トルクの大きさが０になるように、当該特異点に近づくに従ってフィードバック制御トルクの大きさを小さくし、フィードバック制御とフィードフォワード制御を連続的に切り替えることのできる姿勢制御装置が構成される。

このように、実施の形態１によれば、人工衛星の姿勢のフィードバック制御と、ＣＭＧのジンバル軸のフィードフォワード制御を組み合わせて、人工衛星の三軸姿勢制御を行うことによって、特異点とその近傍においても、より高い精度で所望の姿勢運動を得ることができる。

実施の形態２．
図５は、実施の形態２による姿勢制御系の構成を示している。図中、図３と同一符号のものは同一相当のものを示す。姿勢運動計画部１１において計算される姿勢角目標値２２と、姿勢決定系１６において観測される姿勢角検出値２９との差が、姿勢角誤差２３となる。レギュレータ（フィードバック制御系）１２では、姿勢角誤差２３の値に基づいて、姿勢角誤差２３が小さくなるような姿勢制御トルク２４を算出する。ＣＭＧ分配則部１３では、姿勢制御トルク２４を出力するための各ＣＭＧのジンバル角速度２５を算出する。加算器１０１は、積分器１７によりジンバル角速度２５が時間積分されたジンバル角度３２に、姿勢運動計画部１１で計算されるフィードフォワードジンバル角度３１を加えて、ジンバル角度目標値３３を算出する。ＣＭＧ１４では、ジンバル角度目標値３３に従ってジンバルの回転を制御し、出力トルク２７を人工衛星１５に作用させる。人工衛星１５では、ＣＭＧ１４の出力トルク２７が作用することにより力学法則に従って姿勢角２８が変化する。なお、フィードフォワードジンバル角度３１は、図４で説明したフィードフォワードジンバル角速度２１と同様にして、ジンバル角度についてフィードフォワード動作を行う。

この実施の形態２によれば、ＣＭＧ分配則部１３とフィードフォワードジンバル角度３１を組み合わせることにより、特異点とその近傍においても所望の姿勢運動が実現され、姿勢制御誤差が大きくなることなく特異点近傍を通過することが可能である。

実施の形態３．
図６は、実施の形態３による姿勢制御系の構成を示している。図中、図３と同一符号のものは同一相当のものを示す。姿勢運動計画部１１において計算される姿勢角目標値２２と、姿勢決定系１６において観測される姿勢角検出値２９との差が、姿勢角誤差２３となる。レギュレータ（フィードバック制御系）１２では、姿勢角誤差２３の値に基づいて、姿勢角誤差２３が小さくなるような姿勢制御トルク２４を算出する。ＣＭＧ分配則部１３では、姿勢制御トルク２４を出力するための各ＣＭＧのジンバル角速度２５を算出する。加算器１０１は、積分器１７によりジンバル角速度２５が時間積分されたジンバル角度３２に、姿勢運動計画部１１で計算されるフィードフォワードジンバル角度３１を加えて、ジンバル角度目標値３３を算出する。加算器１０２は、ＣＭＧ分配則部１３で算出されるジンバル角速度２５に、姿勢運動計画部１１で計算されるジンバルフィードフォワードジンバル角速度２１を加えて、ジンバル角速度目標値２６を算出する。フィードフォワードジンバル角加速度３４は、姿勢運動計画部１１で計算される。ＣＭＧ１４では、ジンバル角度目標値３３と、ジンバル角速度目標値２６と、フィードフォワードジンバル角加速度３４に従ってジンバルの回転を制御し、出力トルク２７を人工衛星１５に作用させる。人工衛星１５では、ＣＭＧ１４の出力トルク２７が作用することにより力学法則に従って姿勢角２８が変化する。なお、フィードフォワードジンバル角加速度３４は、図４で説明したフィードフォワードジンバル角速度２１と同様にして、ジンバル角加速度についてフィードフォワード動作を行う。

この実施の形態３によれば、ＣＭＧ分配則部１３と、フィードフォワードジンバル角度３１、フィードフォワードジンバル角速度２１、フィードフォワードジンバル角加速度３４を組み合わせることにより、特異点とその近傍においても所望の姿勢運動が実現され、姿勢制御誤差が大きくなることなく特異点近傍を通過することが可能である。

この実施の形態３によれば、ジンバルの回転が大きく加減速を行う場合においても、フィードフォワードジンバル角加速度３４をＣＭＧ１４に入力することにより、制御精度を向上させることが可能である。

図５において、フィードフォワードジンバル角加速度３４の代わりに、フィードフォワードジンバルトルクを用いても同様の効果が得られる。

実施の形態４．
図７は、実施の形態４による複数の関節を持つマニピュレータの手先位置制御系の構成を示している。
図において、マニピュレータ運動計画部４１において計算されるマニピュレータ手先位置の目標値５２と、順キネマティクス４６において関節角度５８を観測して計算されるマニピュレータ手先位置５９との差が、手先位置誤差５３となる。レギュレータ（フィードバック制御系）４２では、手先位置誤差５３の値に基づいて、手先位置誤差５３が小さくなるような手先速度５４を算出する。関節分配則部４３では、手先速度５４を出力するための各関節の関節角速度５５を算出する。

加算器１０３は、関節分配則部４３で算出される関節角速度５５に、マニピュレータ運動計画部４１で計算されるフィードフォワード関節角速度５１を加えて、関節角速度目標値５６を算出する。関節コントローラ４４では、関節角速度目標値５６に従って関節の回転を制御し、関節駆動トルク５７をマニピュレータ４５に作用させる。マニピュレータ４５では、関節駆動トルク５７が作用することにより力学法則に従って運動を行い、関節角度５８が変化する。

レギュレータ４２、関節分配則部４３、関節コントローラ４４、マニピュレータ４５、および順キネマティクス４６は、マニピュレータ手先位置の目標値５２とマニピュレータ手先位置５９との差から得られる手先位置誤差５３をレギュレータ４２に入力することで、フィードバック制御系の閉ループを構成する。また、マニピュレータ運動計画部４１で計算されるフィードフォワード関節角速度５１を、加算器１０３を介して関節コントローラ４４に入力することでフィードフォワード制御系の開ループが構成される。

以下、関節分配則部４３の機能について説明する。マニピュレータの関節数がＮm個であるとき、手先速度５４と関節角速度５５との間には、次式の関係がある。

式（１０）において、η_c'は手先速度ベクトル（３次元）、θ_c'は関節角速度ベクトル（Ｎm次元）、J_mはヤコビ行列（３×Ｎm次元）である。なお、手先の位置と姿勢を制御する場合には、速度ベクトルは６次元、ヤコビ行列は６×Ｎm次元となる。ヤコビ行列は関節角度の関数であり、関節角度によって値が変化する。

関節分配則部４３においては、手先速度５４から関節角速度５５を算出するために、次式（１１）、（１２）を用いる。

特異点ではdet(J_mJ_m ^T)=0となることから、α_mは特異点近傍において関節角速度５５が過大にならないように設定する閾値である。

式（１１）を用いて算出される関節角速度５５によって出力される手先速度の大きさは、特異点に近づくにしたがって小さくなり、特異点では0となる。図７に示す通り、関節コントローラ４４に入力される関節角速度目標値５６は、関節角速度５５にフィードフォワード関節角速度５１を加算した値である。各関節角度の組み合わせが特異点に近づくに従って、手先速度は、関節角速度５５によって出力される成分（フィードバック制御速度）が小さくなることにより、フィードフォワード関節角速度５１によって出力される成分（フィードフォワード制御速度）が支配的となる。特異点においては、フィードフォワード関節角速度５１による成分のみが出力される。そのため、マニピュレータ運動計画部４１において、所望の手先運動に整合するフィードフォワード関節角速度５１を計算することにより、特異点とその近傍において閉ループ制御と開ループ制御が連続的に切り替わり、所望の運動が実現される。

この実施の形態４によれば、関節分配則部４３とフィードフォワード関節角速度５１を組み合わせることにより、特異点とその近傍においてもマニピュレータの所望の運動が実現され、手先位置の制御誤差が大きくなることなく特異点近傍を通過することが可能である。

関節分配則部４３において、分配行列J_m ⁺として式（１２）を用いる代わりに、ヤコビ行列の擬似逆行列の異なる近似式を用いても良い。その場合には、特異点近傍において不要な手先速度が出力されて手先位置の制御誤差が大きくなることがあるが、その場合でも、フィードフォワード関節角速度５１の効果により、特異点近傍を通過することが可能である。

また、この実施の形態４によれば、マニピュレータの手先位置のフィードバック制御と、マニピュレータの関節角速度のフィードフォワード制御を組み合わせて、マニピュレータの手先位置の位置制御を行うことにより、マニピュレータの特異点とその近傍においても、より高い精度で所望の姿勢運動を得ることができる。

この発明の実施の形態１による人工衛星用姿勢制御装置の構成を説明するための図である。この発明の実施の形態１による人工衛星用姿勢制御装置のＣＭＧメカニズムの構成を示す図である。この発明の実施の形態１による人工衛星用姿勢制御装置の姿勢制御系の構成を示す図である。この発明の実施の形態１による人工衛星用姿勢制御装置のジンバル角度の軌跡を示す図である。この発明の実施の形態２による人工衛星用姿勢制御装置の姿勢制御系の構成を示す図である。この発明の実施の形態３による人工衛星用姿勢制御装置の姿勢制御系の構成を示している。この発明の実施の形態４によるマニピュレータ制御装置の先端位置制御系の構成を示す図である。

符号の説明

１ＣＭＧメカニズム、２ＣＭＧコントローラ、３姿勢コントローラ、４ロータ、５スピン軸、６スピンモータ、７ジンバル、８ジンバル軸、９ジンバルモータ、１０フレーム、１１姿勢運動計画部、１２レギュレータ、１３ＣＭＧ分配則部、１４ＣＭＧ、１５人工衛星、１６姿勢決定系、１７積分器、２１フィードフォワードジンバル角速度、２２姿勢角目標値、２３姿勢角誤差、２４姿勢制御トルク、２５ジンバル角速度、２６ジンバル角速度目標値、２７出力トルク、２８姿勢角、２９姿勢角検出値、３１フィードフォワードジンバル角度、３２ジンバル角度、３３ジンバル角度目標値、３４フィードフォワードジンバル角加速度、４１マニピュレータ運動計画部、４２レギュレータ、４３関節分配則部、４４関節コントローラ、４５マニピュレータ、４６順キネマティクス、５１フィードフォワード関節角速度、５２手先位置目標値、５３手先位置誤差、５４手先速度、５５関節角速度、５６関節角速度目標値、５７関節駆動トルク、５８関節角度、５９手先位置、１００，１０１，１０２，１０３加算器。

Claims

人工衛星の所望の姿勢運動に応じて求められるフィードフォワードジンバル角速度および当該人工衛星の目標姿勢角を計算するとともに、当該人工衛星の姿勢および複数のコントロールモーメントジャイロの回転を制御する姿勢コントローラを備え、
上記姿勢コントローラは、
上記目標姿勢角と観測される人工衛星の姿勢角との姿勢角誤差に基づいて姿勢制御トルクを計算するフィードバック制御系を有し、
上記コントロールモーメントジャイロにおける特異点近傍で上記フィードフォワードジンバル角速度の成分に比して上記コントロールモーメントジャイロのジンバル角速度の成分が小さくなるように、上記フィードバック制御系で計算された姿勢制御トルクから上記コントロールモーメントジャイロのジンバル角速度の成分を求め、求めたジンバル角速度の成分と上記フィードフォワードジンバル角速度の組合せに基づいて、上記コントロールモーメントジャイロに上記人工衛星の姿勢制御トルクを発生させる、
ことを特徴とする姿勢制御装置。
複数のコントロールモーメントジャイロと、
人工衛星の所望の姿勢運動に応じて求められるフィードフォワードジンバル角速度、および人工衛星の目標姿勢角を計算する姿勢運動計画部を有し、人工衛星の姿勢および上記コントロールモーメントジャイロの回転を制御する姿勢コントローラとを備え、
上記姿勢コントローラは、
上記目標姿勢角と観測される人工衛星の姿勢角との姿勢角誤差に基づいて姿勢制御トルクを計算するフィードバック制御系と、
上記コントロールモーメントジャイロのジンバル角度をパラメータとしたヤコビ行列から構成される行列式と、上記コントロールモーメントジャイロの特異点で上記コントロールモーメントジャイロのジンバル角速度を０とする閾値との比較に基づいて分配行列を演算し、演算された分配行列と上記フィードバック制御系で計算された姿勢制御トルクに基づいて、上記コントロールモーメントジャイロのジンバル角速度を計算する分配則部と、
上記分配則部で計算されたジンバル角速度とフィードフォワードジンバル角速度を加算する加算器とを具備し、
上記コントロールモーメントジャイロは、上記加算器の加算結果に基づいて人工衛星の姿勢制御トルクを発生する、
ことを特徴とする姿勢制御装置。
上記分配則部は、出力ベクトルを入力ベクトルに変換するヤコビ行列と、ヤコビ行列の転置行列との積により行列を算出し、入力ベクトルに、その行列の逆行列と閾値との最大値の逆数と、その行列の余因子行列と、ヤコビ行列の転置行列とを掛けることにより、出力ベクトルを算出することを特徴とする請求項２に記載の姿勢制御装置。
複数のコントロールモーメントジャイロを用いた人工衛星の姿勢制御装置であって、
上記人工衛星の姿勢角誤差に基づくフィードバック制御トルクを出力するための上記コントロールモーメントジャイロのジンバル角速度を計算する分配則部と、
上記人工衛星の所望の姿勢運動に基づいて上記コントロールモーメントジャイロのフィードフォワードジンバル角速度を計算する姿勢運動計画部と、
上記分配則部で計算されるジンバル角速度と上記姿勢運動計画部で計算されるフィードフォワードジンバル角速度とを組み合わせ、上記コントロールモーメントジャイロの特異点において上記フィードバック制御トルクの大きさが０になるように、当該特異点に近づくに従ってフィードバック制御トルクの大きさを小さくし、フィードバック制御とフィードフォワード制御を連続的に切り替えることを特徴とした姿勢制御装置。
複数の関節を持つマニピュレータの手先位置を制御する位置制御装置であって、
上記マニピュレータの手先位置誤差に基づくフィードバック制御速度を出力するための関節角速度を計算する関節分配則部と、
上記マニピュレータの所望の手先運動に基づいてフィードフォワード関節角速度を計算するマニュピレータ運動計画部と、
上記関節分配則部で計算される関節角速度と上記マニュピレータ運動計画部で計算されるフィードフォワード関節角速度とを組み合わせ、上記マニュピレータの特異点において上記フィードバック制御速度の大きさが０になるように、当該特異点に近づくに従ってフィードバック制御速度の大きさを小さくし、フィードバック制御とフィードフォワード制御を連続的に切り替えることを特徴とした位置制御装置。