(57)【要約】【課題】運動対象物体との係合後に回転機構に作用するトルクを検出し、過大な力が回転部に作用するのを防止しつつ、回転機構の回転部を所定の回転角度で制御する回転機構用制御装置を提供する。【解決手段】回転機構についてコンプライアンス制御フィルタ１１ｂとして設定された仮想コンプライアンス機構に基づいて、検出関節トルクτｓ  に対応する仮想コンプライアンス角度変位θｄ  を求め、この角度変位θｄ  を、制動力と制動トルクとについて予め設定されている各プロファイル生成部１５，１５からの指令関節角度θｃｏｍ  と目標制動トルクτｃｏｍ  とに基づいて修正し、目標関節角度θｒｅｆ  で回転部の回転角度を制御しているので、回転機構の各機構部に過大な荷重を負荷物や機構に加えることなく、回転部に係合する負荷物の運動の減速・停止を行うことができる。【選択図】      図１
2005014192.tif 000002
【特許請求の範囲】
【請求項１】
回転機構の回転部を回転させるモータに制御信号として目標回転角度θｒｅｆ  を出力することで前記回転部の回転角度を制御する回転機構用制御装置において、
制動トルクプロファイル生成部からの目標制動トルクτｃｏｍ  に基づいて目標コンプライアンス角度変位θｃｃを出力するトルク追従制御部、
制動動作プロファイル生成部からの目標指令角度θｃｏｍ  を回転角度検出部が検出した前記回転部の検出回転角度θｅｓに基づいて修正した修正目標指令角度θ’ｃｏｍ  を出力するトルクリミット制御部、及び
トルク検出部が検出した前記回転部に作用する検出トルクτｓ  の入力を受けて仮想コンプライアンス角度変位θｄ  を出力するコンプライアンス制御フィルタを備え、前記仮想コンプライアンス角度変位θｄ  を前記目標コンプライアンス角度変位θｃｃと前記修正目標指令角度θ’ｃｏｍ  とで修正して前記目標回転角度θｒｅｆ  を出力するコンプライアンス制御部
を備えることから成る回転機構用制御装置。
【請求項２】
前記コンプライアンス制御フィルタは、仮想慣性、仮想粘性、仮想剛性に基づいて記述される伝達関数で記述されることから成る請求項１に記載の回転機構用制御装置。
【請求項３】
前記トルクリミット制御部は、前記目標指令角度θｃｏｍ  に対して、前記目標指令角度θｃｏｍ  と前記検出回転角度θｅｓとの差の入力に対して不感帯処理を施した出力で補正することにより、前記修正目標指令角度θ’ｃｏｍ  を出力することから成る請求項１に記載の回転機構用制御装置。
【請求項４】
前記不感帯処理は、前記目標指令角度θｃｏｍ  と前記検出回転角度θｅｓとの差が所定偏差以内のときには前記出力をせずに、前記目標指令角度θｃｏｍ  がそのまま前記修正目標指令角度θ’ｃｏｍ  となり、前記差が前記所定偏差を超えるときには前記目標指令角度θｃｏｍ  の値に応じた値を前記出力とし、前記目標指令角度θｃｏｍ  が前記出力で減算されることから成る請求項３に記載の回転機構用制御装置。
【請求項５】
前記トルク追従制御部は、前記目標制動トルクτｃｏｍ  を前記仮想剛性で除した値を前記目標コンプライアンス角度変位θｃｃとして出力することから成る請求項２に記載の回転機構用制御装置。
【請求項６】
前記制動トルクプロファイル生成部は、前記目標制動トルクτｃｏｍ  を、所定時刻までは一定値とし、前記所定時刻から前記回転部の停止時刻までは次第に減少する関数として設定していることから成る請求項１に記載の回転機構用制御装置。
【請求項７】
制動動作プロファイル生成部は、前記回転部に設けられたアームの先端部の速度に対する制動力の関数として設定しており、前記関数は、前記アームの先端部の速度が所定速度までは前記先端部の速度に従って増加し、前記所定速度以上の速度に対して一定の制動力に飽和させていることから成る請求項１に記載の回転機構用制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、ロボットアーム関節等の回転機構に生じるトルクを考慮して、回転機構の回転部における回転角度の制御を行う回転機構用制御装置に関する。
【０００２】
【従来の技術】
地球周回軌道などの軌道上においては、ランデブー対象や衛星残滓などの運動する対象物体を他の衛星に設けられているロボットアームで捕獲する必要が生じる。衛星上のロボットアームと、その衛星に対して相対運動する対象物体とを互いに接近させるために、例えばスラスタによって衛星に力を作用させると当該衛星の軌道を変更することになるとともに、捕獲のためにアームを作動させるとその反作用で衛星自体の位置や姿勢が変化する。したがって、両者の間の距離や速度の制御は、両者地上の場合と比較して格段に複雑になる。また、捕獲に際してロボットアームが対象物体と接触又は衝突するとき、両者の間に作用する力及びトルク等の制御量が過大にならないように、対象物体の捕獲前からも両者間の相対距離や相対速度を精密に制御する必要がある。互いの負荷慣性が大きい場合に相対速度が大きい状態で対象物体を捕獲するときには回転機械の摩擦により対象物体に減速・停止動作をさせることになるが、対象物体には急激な減速が生じ、過大な荷重が衛星や対象物体の各部、或いはロボットアームに加わり、不測の事態を生じることもあり得る。
【０００３】
そこで、衛星に備わるアームやマニピュレータ等のロボットによって運動対象物体を捕獲するに際しては、捕獲前に両者間の相対的な位置や速度を可能な限り少なくしてターゲットの運動に追従した上で捕捉することにより捕獲時の衝撃を少なくすること（特許文献２参照）、或いは捕獲時における多少の衝撃にも耐えられるように捕獲機構を強化すること（特許文献３参照）等が提案されている。
【０００４】
一方、スペースシャトルや人工衛星等の宇宙航行体に搭載され、宇宙ステーションの建設や他の人工衛星の捕獲のように、自他の宇宙航行体を作業対象にしてピン挿入、ボルト締め／弛め、ハンドル作業等を各種の作業を実行する宇宙作業用多関節ロボットが提案されている。そうした多関節ロボットにおいては、各関節について設定された角度目標値を達成するように各関節角度を制御するが、対象物を損傷させないためにエンドエフェクタに加わる力を制御する必要がある。力制御を行うロボットの制御装置として、間接的に仮想のインピーダンスを実現してロボットアームの先端に加わる力を緩和するコンプライアンス制御が知られている。コンプライアンス制御は、ロボットのアームに柔軟性を与えるハードウェアを装着してコンプライアンス動作を直接的に実現して安定で高速な動作を可能とする方法もあるが、ハードウェアの特性については、一度設定するとその変更をすることができず、制御に柔軟性がないという問題がある。そこで、例えば、ロボットアームの先端に設けられるエンドエフェクタを、ソフトウェアによって仮想質量、仮想粘性係数及び仮想ばね剛性を持つ仮想コンプライアンス機構に想定することが行われている。仮想コンプライアンス機構の特性は、実際をよく反映し且つ簡単化して表現するため、位置の二次微分を含む慣性力項と、位置の一次微分を含む粘性力項及び位置の一次項としてのばね力項を含む二階の線形微分方程式で表されるものとされる。仮想コンプライアンス制御においては、仮想コンプライアンス機構の運動を所定のアルゴリズムによって実時間で計算し、仮想コンプライアンス機構の動作にロボットの動作を追従させる。仮想コンプライアンス機構の特性は、作業の内容に応じてソフトウェアによって変更可能であるので、状況に応じて最適な柔軟性を与えることができる（特許文献４参照）。
【０００５】
宇宙空間に浮遊しているターゲットの捕捉を容易にするため、捕捉時の衝撃力をセンサで検出して捕獲機構を修正して衝撃を少なくする宇宙ロボットの制御装置が提案されている（特許文献１参照）。この宇宙ロボットの制御装置によれば、動的モデルを用いることで、人工衛星に搭載されたセンサで人工衛星の位置・速度を検出し、この検出情報に基づいて、マニピュレータの動作に伴う人工衛星の位置・姿勢の変化を考慮したマニピュレータの先端速度を生成してマニピュレータのターゲットへの追従動作を実行し、近接センサで得られたターゲットの被把持部分とグリッパとの相対的な位置・姿勢情報を用いてターゲットへのアプローチ軌道を生成し、更に、マニピュレータの手首部に設けられた力覚センサによって、グリッパがターゲットを把持する時にマニピュレータに作用する力を検出して、外力に応じてマミピュレータの各軸のモータを駆動しながら把持動作を実行している。
【０００６】
この宇宙ロボットの制御装置について詳述すると、マニピュレータ先端速度生成器において、マニピュレータの先端位置に不感帯を備えたダンパ（粘性減衰係数Ｃ）を想定し、グリッパがターゲットの被把持部分を把持するときには、力覚センサで検出された力が作用した場合の運動と同一になるようにマニピュレータの先端速度を生成し、ターゲットが把持されたときのターゲットの揺動を低減させている。即ち、力覚センサで検出された力が予め定められた力の設定値を超える場合にはマニピュレータの先端速度を生成し、当該設定値以下の場合にはマニピュレータの先端速度をゼロに設定している。また、ターゲットを把持する際に、マニピュレータ先端位置補正器において、マニピュレータの逐次目標位置回りにバネ定数Ｋのバネ、粘性減衰係数Ｃのダンパで接続された質量Ｍの慣性体を想定し、これに力覚センサで検出された力が作用した場合の運動と同一になるように、マニピュレータの修正された逐次目標位置を生成している。更に、マニピュレータ先端速度生成器において、補正された逐次目標位置と現在位置とからＰＩＤ制御によりマニピュレータの先端速度を生成して、マニピュレータ関節速度生成器においてマニピュレータの各関節の速度指令値を求めて、マニピュレータの各軸モータを駆動している。
【０００７】
しかしながら、運動対象物体の負荷慣性が大きい場合、ロボットアームや回転機構が運動対象物体を捕捉後に減速・停止などの制御動作をするときに、回転機構の摩擦により急激な減速を生じ、モータ軸が急速に停止することによって、過大な荷重が各機構部に加わる。こうした過大な荷重は、ロボットアーム（関節部、把持部）、衛星上のアーム取付け部、対象物体の被把持部等の各機構部への大きな負担・制約になっている。
【０００８】
【特許文献１】
特開平５−１０４４６９号公報（段落［０００９］〜［００２２］、図２）
【特許文献２】
特開平６−１２７４９５号公報（段落［０００８］〜［００２２］、図１〜図３）
【特許文献３】
特開２００１−２６０９９７号公報（段落［００１６］、図１）
【特許文献４】
特開平１０−１２８６８５号公報（段落［０００１］〜［０００９］、図３〜図５）
【０００９】
【発明が解決しようとする課題】
そこで、基端側に回転部を備えるロボットアームやマニピュレータ等に設けられる係合部が運動対象物体と衝突又は運動対象物体を捕獲又は把持するときのように、係合部が運動対象物体と係合することに起因して回転部にトルクが作用する場合、そうした回転部を所定の回転角度で制御する必要があるときには、回転部に過大な力が作用するのを防止しつつ、回転機構の回転部の回転角度を制御する点で解決すべき課題がある。
【００１０】
この発明の目的は、回転部に備わるロボットアームやマニピュレータ等の係合部が運動対象物体と衝突、捕獲又は把持を行うときに、運動対象物体との係合後に回転機構に作用するトルクを考慮して、過大な力が回転部に作用するのを防止しつつ、回転機構の回転部の回転角度を制御することで、トルクが作用する回転部を所定の回転角度で制御することが可能な回転機構用制御装置を提供することである。
【００１１】
【課題を解決するための手段】
上記の課題を解決するため、この発明による回転機構用制御装置は、回転機構の回転部を回転させるモータに制御信号として目標回転角度θｒｅｆ  を出力することで前記回転部の回転角度を制御する回転機構用制御装置において、制動トルクプロファイル生成部からの目標制動トルクτｃｏｍ  に基づいて目標コンプライアンス角度変位θｃｃを出力するトルク追従制御部、制動動作プロファイル生成部からの目標指令角度θｃｏｍ  を回転角度検出部が検出した前記回転部の検出回転角度θｅｓに基づいて修正した修正目標指令角度θ’ｃｏｍ  を出力するトルクリミット制御部、及びトルク検出部が検出した前記回転部に作用する検出トルクτｓ  の入力を受けて仮想コンプライアンス角度変位θｄ  を出力するコンプライアンス制御フィルタを備え、前記仮想コンプライアンス角度変位θｄ  を前記目標コンプライアンス角度変位θｃｃと前記修正目標指令角度θ’ｃｏｍ  とで修正して前記目標回転角度θｒｅｆ  を出力するコンプライアンス制御部を備えることから成っている。
【００１２】
この回転機構用制御装置によれば、コンプライアンス制御部は、トルク検出部が検出した回転部に作用する検出トルクτｓ  の入力を受けてコンプライアンス制御フィルタが仮想コンプライアンス角度変位θｄ  を出力する。トルク追従制御部は、制動トルクプロファイル生成部からの目標制動トルクτｃｏｍ  に基づいて目標コンプライアンス角度変位θｃｃを出力する。また、トルクリミット制御部は、制動動作プロファイル生成部からの目標指令角度θｃｏｍ  を回転角度検出部が検出した回転部の検出回転角度θｅｓに基づいて修正した修正目標指令角度θ’ｃｏｍ  を出力する。コンプライアンス制御部は、修正目標指令角度を仮想コンプライアンス角度変位θｄ  と、目標コンプライアンス角度変位θｃｃとで修正して、回転機構の回転部を回転させるモータに制御信号としての目標回転角度θｒｅｆ  を出力する。上記のように、回転機構についての仮想上の動的モデルを設定しておき、検出トルクτｓ  を受けて当該動的モデルに基づいて、仮想コンプライアンス角度変位θｄ  を求め、当該仮想コンプライアンス角度変位θｄ  を、制動力と制動トルクとについて各プロファイル生成部からの目標指令角度θｃｏｍ  と目標制動トルクτｃｏｍ  とに基づいて修正し、その修正で得られた目標回転角度θｒｅｆ  を出力することで前記回転部の回転角度を制御しているので、回転機構の回転部の回転角度が制動トルクや制動力を考慮して制御されることになり、過大な力が各機構部に作用することがない。
【００１３】
この回転機構用制御装置において、前記コンプライアンス制御フィルタは、仮想慣性、仮想粘性、仮想剛性に基づいて記述される伝達関数で記述される。角度とトルクとの間に成立する運動方程式は、一般的には角度の二次微分項として表される慣性項、角度の一次微分項として表される速度（粘性）項及び角度の比例項として表される力（トルク）項から成る２次微分方程式で表される。伝達関数を仮想慣性、仮想粘性及び仮想剛性に基づいて記述した回転機構のモデルが、仮想の動的なモデルである。
【００１４】
この回転機構用制御装置において、前記トルクリミット制御部は、前記目標指令角度θｃｏｍ  に対して、前記目標指令角度θｃｏｍ  と前記検出回転角度θｅｓとの差の入力に対して不感帯処理を施した出力で補正することにより、前記修正目標指令角度θ’ｃｏｍ  を出力することができる。不感帯処理を施すことにより、検出回転角度θｅｓが目標指令角度θｃｏｍ  に十分近ければ、目標指令角度θｃｏｍ  は、修正されることなく、そのままトルクリミット制御部からの出力とされる。
【００１５】
この回転機構用制御装置において、前記不感帯処理は、前記目標指令角度θｃｏｍ  と前記検出回転角度θｅｓとの差が所定偏差以内のときには前記出力を出さないので、前記目標指令角度θｃｏｍ  が補正されることなくそのまま前記修正目標指令角度θ’ｃｏｍ  としてトルクリミット制御部の出力とされる。前記差が前記所定偏差を超えるときには、目標指令角度θｃｏｍ  が、例えば、前記目標指令角度θｃｏｍ  の値に応じて、当該差に基づいて修正されて出力とされ、前記目標指令角度θｃｏｍ  が修正された前記出力で減算され、その結果がトルクリミット制御部の出力とされる。
【００１６】
この回転機構用制御装置において、前記トルク追従制御部は、前記目標制動トルクτｃｏｍ  を前記仮想剛性で除した値を前記目標コンプライアンス角度変位θｃｃとして出力することから成っている。コンプライアンス制御では、アームの先端を対象物である壁に力を以て押し付ける場合を例に取ってみると、目標値が壁の中に食い込む状態（即ち、目標値の仮想食込み）でないとアームの先端を実際に壁に対して押付け力を発生させることができない。目標が仮想食込み状態にあるのに対して現実にはアームの先端が壁に規制されているとき、駆動系は目標と現実との差を解消しようとしてアームの先端を壁に対して押し付ける。このとき、上記の差は仮想コンプライアンス機構の変位によって吸収されるとし、押付け力をその変位に応じたばね項と粘性項とによって計算上制御することができる。そこで、ロボットによって運動物体に制動をかけるには、コンプライアンス制御部において、目標となるコンプライアンス角度変位θｃｃを与える必要がある。目標コンプライアンス角度変位θｃｃとしては、目標制動トルクτｃｏｍ  の大きさを反映する必要があるが、制御が複雑化するのを回避するため簡素なものを採用するのが好ましい。そこで、与えられた目標制動トルクτｃｏｍ  （それ自体は変化する）に対して動的な変動分を考慮することなく、仮想コンプライアンス機構のうち仮想捩じり剛性のみに応じて生じると想定される角度変位（単に目標制動トルクτｃｏｍ  に比例する）を目標コンプライアンス角度変位θｃｃとして採用する。
【００１７】
この回転機構用制御装置において、前記制動トルクプロファイル生成部は、前記目標制動トルクτｃｏｍ  を、所定時刻までは一定値とし、前記所定時刻から前記回転部の停止時刻までは次第に減少する関数として設定していることから成っている。目標制動トルクτｃｏｍ  を所定時刻までは一定値とすることで、各機構部へのダメージの大きい過大な制動トルクが生じないように制御することができる。回転部の制動が一定の程度進めば、その後の目標制動トルクを小さく設定しても、回転部を確実に停止させることができる。
【００１８】
この回転機構用制御装置において、制動動作プロファイル生成部は、前記回転部に設けられたアームの先端部の速度に対する制動力の関数として設定しており、前記関数は、前記アームの先端部の速度が所定速度までは前記先端部の速度に従って増加し、前記所定速度以上の速度に対して一定の制動力に飽和させていることから成っている。アームの先端部の速度が所定速度以上の場合に、制動力を飽和させて大きくしないことにより、回転部に設けられた例えばアームが運動対象物体と係合して後、アームを大きな速度で移動させても、制動力を制限することで、過大な制動トルクが生じるのを回避するとともに、アームの先端部の速度が所定速度以下の場合には制動力を小さくして、アームが運動対象物体と係合した初期の段階及び制動終期の段階での大きな制動力の発生を防止している。
【００１９】
【発明の実施の形態】
以下、添付した図面に基づいて、この発明による回転機構用制御装置の実施例を説明する。図１はこの発明による回転機構用制御装置の実施例としての関節動作制御装置の一例を示すブロック図、図２はこの回転機構用制御装置が適用される関節システムの模式図、図３は図１に示す関節動作制御装置におけるトルクリミット制御部の不感帯特性の一例を示す線図、図４は図１に示す関節動作制御装置における減速動作プロファイルの一例を示す線図、図５は図１に示す関節動作制御装置における制動トルクプロファイルの一例を示す線図である。
【００２０】
図２に示す関節システムによれば、本発明における回転機構としての人工衛星等の機体１に取り付けられる関節機構２は、内側の減速機構３と、減速機構３を介してその周りで相対回転可能に装着された回転部４と、回転部４に取り付けられているアーム５とを有している。アーム５には、その先端部において、運動対象物体Ａに係合可能なハンド６が備わっている。減速機構３は機体１に固定されたモータ７の出力軸に接続されており、モータ７の回転出力が減速機構３によって減速されて回転部４に与えられる。回転部４の回転角度を検出するため、関節機構２には関節角度検出器（本発明における「回転角度検出部」に相当する）８が設けられている。更に、関節機構２の関節出力軸部（アーム５とも一体化されている）に、関節トルク検出器（本発明における「トルク検出部」に相当する）９が設けられている。関節角度検出器８が検出した検出関節角度θｅｓと関節トルク検出器９が検出した検出関節トルクτｓ  とが関節動作制御装置（以下、簡単のため「制御装置」と略す）１０に入力される。
【００２１】
今、動きのある運動対象物体Ａをアーム５で捕捉しようとする場合には、運動対象物体Ａをアーム５のハンド６に衝突させ、その後に回転しようとする回転部４の回転を減速・停止を制御することで、衝突を緩和させることが考えられる。運動対象物体Ａの質量が大きい場合、停止までの負荷慣性が大きいので、短時間で停止させるには回転機構の摩擦によって急激な減速・停止となる制動動作となるが、そうした制動動作を行うにはモータ軸等の各機構部に過大な荷重が作用し、関節システムの運用に対して大きな負担・制約になる。制御装置１０は、このような場合でも、過大なトルクが関節に生じることなく運動対象物体Ａを停止させるように関節機構２の動きを制御しようとするものであって、回転部４の出力軸部に配置された関節トルク検出器９からのセンサ出力情報である検出関節トルクτｓ  に基づき、目標とする制動トルクプロファイルに従って運動対象物体Ａに制動トルクを作用させようとするモータ７駆動用の制御装置である。
【００２２】
図１に示すように、制御装置１０は、コンプライアンス制御部１１、トルクリミット制御部１２、トルク追従制御部１３、制動動作プロファイル生成部１４、制動トルクプロファイル生成部１５等から構成されている。即ち、制御装置１０は、関節角度検出部８から時々刻々検出され入力される検出関節角度θｅｓの検出情報と、関節トルク検出器９から時々刻々入力される検出関節トルクτｓ  の検出情報とに基づいて、演算処理部での演算を行って、関節角度制御部であるモータ７への指令として目標関節角度θｒｅｆ  を時々刻々出力する機能を有する。
【００２３】
コンプライアンス制御部１１は、関節トルク検出器９が検出した検出関節トルクτｓ  の信号の入力を受けて、検出関節トルクτｓ  の低周波数成分のみを通過させる低域濾波フィルタ１１ａと、低域濾波フィルタ１１ａによってフィルタ処理されたトルク信号τｆ  にコンプライアンスフィルタ処理をするコンプライアンス制御フィルタ１１ｂとを備え、関節角度制御器であるモータ７に目標関節角度θｒｅｆ  を出力する。低域濾波フィルタ１１ａは、入力信号である検出関節トルクτｓ  の高周波成分をカットすることで信号ノイズを除去すると共に、高周波数域でゲインを下げてゲイン余裕を確保する。コンプライアンス制御フィルタ１１ｂは、トルク信号τｆ  に対して、Ｓ（ラプラス変換における複素パラメータ）領域において仮想の関節インピーダンスパラメータを用いた次の式で表される関節コンプライアンスフィルタ処理を行い、コンプライアンス角度変位θｄ  を算出する。なお、コンプライアンス制御フィルタ１１ｂによるフィルタ処理は、デジタルフィルタ演算処理によって実現される。
θｄ  ＝（１／（ｍｃ  ・Ｓ２  ＋ｃｃ  ・Ｓ＋ｋｃ  ））・τｆ        （１）
ここで、ｍｃ  ：関節仮想負荷慣性
ｃｃ  ：関節仮想粘性係数
ｋｃ  ：関節仮想捩れ剛性
τｆ  ：検出関節トルクτＳ  に低域濾波フィルタ処理を行った関節トルク
【００２４】
トルクリミット制御部１２は、関節角度検出器８が検出した検出関節角度θｅｓの入力を受けて、後述する制動動作プロファイル生成部１４からの指令関節角度θｃｏｍ  を修正し、且つ飽和特性を以てコンプライアンス制御部１１に出力する制御部である。トルクリミット制御部１２の制御特性は、図３（ａ）に示すように、入力のゼロを含む領域（−ε０  ≦εｃｏｍ  （＝θｃｏｍ  −θｅｓ）≦ε０  ）に対して出力がゼロの不感帯を持つ要素を備えることにより、図３（ｂ）に示すように目標関節角度を修正する関節角度修正量（θ’ｃｏｍ  −θｅｓ）に飽和特性を持たせることができる。指令関節角度θｃｏｍ  から検出関節角度θｅｓを差し引いた偏差εｃｏｍ  が所定値ε０  よりも小さければ、トルクリミット制御部１２は指令関節角度θｃｏｍ  を修正することなくそのまま出力する。指令関節角度θｃｏｍ  と検出関節角度θｅｓとの偏差εｃｏｍ  が所定値ε０  を超える場合は、トルクリミット制御部１２は、指令関節角度θｃｏｍ  を偏差εｃｏｍ  に応じて修正して修正指令関節角度θ’ｃｏｍ  を出力する。図３（ｂ）において、検出関節角度θｅｓを基準とした指令関節角度θｃｏｍ  と修正指令関節角度θ’ｃｏｍ  との関係を示す。偏差εｃｏｍ  の大きさが所定値ε０  を超える範囲では、修正指令関節角度θ’ｃｏｍ  と検出関節角度θｅｓとの差が飽和していることが解る。トルクリミット制御部１２は、この飽和特性によって、コンプライアンス制御において生じがちな目標値（指令値）の対象物への過剰な食込みを防止する働きをする。
【００２５】
制動動作プロファイル生成部１４が生成する制動力プロファイルの一例が、図４に示されている。制動力プロファイルは、アーム先端速度の関数や読出し数値テーブルの形態で予め設定されている。図４に示す制動力プロファイルは、アーム先端速度Ｖａに対する制動力Ｆｂのパターンとして構成されており、具体的には、アーム先端速度Ｖａが一定の大きさＶａ０まではアーム先端速度Ｖａに比例して制動力ＦをＦ０まで大きくするが、当該一定のアーム先端速度Ｖａ０以上の速度に対しては、制動力Ｆを大きくすることなくＦ０の大きさに保つパターンとされる。制動動作プロファイル生成部１４では、図４に示す制動力プロファイルに則って、下記の式（２）及び式（３）によって、アーム５の時々刻々の目標先端位置Ｘｃｏｍ  が算出される。
Ｖｃｏｍ  ＝∫（−Ｆ／Ｍ）ｄｔ＋ｖ０                            （２）
Ｘｃｏｍ  ＝∫Ｖｃｏｍ  ｄｔ＋ｘ０                                  （３）
ここで、ｖ０  は本制御開始時のアーム先端速度、ｘ０  は本制御開始時のアーム先端位置である。
式（２）は、本制御開始時のアーム先端速度ｖ０  に、アームの想定負荷慣性Ｍと制動力プロファイルから読み出される制動力Ｆとから求まる加速度（−Ｆ／Ｍ）を時間ｔで積分して得られる速度を加算する式であり、制御開始からの時間ｔ経過後にアーム５の先端部が有すべき目標先端速度（指令値）Ｖｃｏｍ  を表している。また、式（３）は、制御開始時のアーム先端位置ｘ０  に、式（２）に示す目標先端速度（指令値）Ｖｃｏｍ  を積分して得られる位置を加算する式であり、制御開始からの時間ｔ経過後にアーム５の先端部が有すべき目標先端位置（指令値）Ｘｃｏｍ  を表している。
【００２６】
目標先端位置Ｘｃｏｍ  から、アームの逆キネマティクス変換演算（運動学方程式の解の算出）によって時々刻々の各関節の指令関節角度θｃｏｍ  が算出される。トルクリミット制御部１２では、指令関節角度θｃｏｍ  について、その時点の検出関節角度θｅｓに対する偏差εｃｏｍ  ＝（θｃｏｍ  −θｅｓ）が一定以上にならないように抑える処理を行っている。即ち、不感帯処理は、図３に示す特性を有しており、検出関節角度θｅｓが指令関節角度θｃｏｍ  と大きく相違しない場合には、指令関節角度θｃｏｍ  を修正するための出力を出さず、指令関節角度θｃｏｍ  をそのままトルクリミット制御部１２の出力とする。検出関節角度θｅｓが指令関節角度θｃｏｍ  と大きく異なる場合には、不感帯処理は指令関節角度θｃｏｍ  を減算修正するための出力をし、トルクリミット制御部１２は修正指令関節角度θ’ｃｏｍ  が検出関節角度θｅｓから大きく離間しない値として出力する。
【００２７】
トルク追従制御部１３は、後述する制動トルクプロファイル生成部１５から出力される目標関節出力トルクτｃｏｍ  の入力を受けて、目標関節出力トルクτｃｏｍ  を関節仮想捩れ剛性ｋｃ  で除することにより、目標コンプライアンス角度変位θｃｃを算出し、目標コンプライアンス角度変位θｃｃをコンプライアンス制御部１１に出力する。目標関節出力トルクτｃｏｍ  としては、例えば、図５に示すような、プロファイルが生成される。即ち、図５に例示する目標の制動トルクプロファイルは、運動対象物体Ａを捕獲してからの時間Ｔ（本制御動作を開始した後の時間）を横軸に取り、縦軸を制動トルクの大きさとしたグラフで示されており、時間Ｔ１までは目標関節出力トルクτｃｏｍ  を一定とし、時間Ｔ１から一定の目標関節出力トルクから次第に（この例では直線的に）減少して時間Ｔ２においてゼロとなるパターンとして設定される。設定の態様は、例えば、時間Ｔの関数又は読出し数値テーブルとすることができる。制動トルクプロファイル生成部１５は、図４に示す設定パターンを持つ目標制動力プロファイルＦ（ｖ）によって時々刻々のアーム５が与えるべき制動力Ｆｃｏｍ  を算出し、これよりアーム１５の関節回転速度から先端速度Ｖａへの変換行列であるヤコビアンＪを用いて、次式（４）に示すように目標関節出力トルクτｃｏｍ  を時々刻々算出する。
τｃｏｍ  ＝Ｊｔ  Ｆｃｏｍ                                  （４）
【００２８】
コンプライアンス制御部１１においては、トルク追従制御部１３が算出した目標コンプライアンス角度変位θｃｃと、コンプライアンス制御フィルタ１１ｂが出力したコンプライアンス角度変位θｄ  との差（θｃｃ−θｄ  ）としてコンプライアンス偏差εｃ  を算出する。コンプライアンス偏差εｃ  と、トルクリミット制御部１２の出力である修正指令関節角度θ’ｃｏｍ  との和が目標関節角度θｒｅｆ  として関節角度制御部（モータ７）へ出力される。即ち、関節トルク検出器９が検出した関節トルクτｓ  、トルク目標値、及び目標制動力と検出関節角度が、それぞれ、仮想コンプライアンス機構から求めたコンプライアンス角度変位θｄ  、トルク追従制御部１３からの目標コンプライアンス角度変位θｃｃ、又は修正指令関節角度θ’ｃｏｍ  として目標関節角度θｒｅｆ  に反映している。検出関節角度については、関節角度制御器７において常に制御に反映されている。なお、各値に付加的に設けられている素子ＲＬは増分リミッタであり、運動物体の捕獲に際してアームの動きの変化が速すぎるために制御系の動作が追従できない場合に回転機構に過大な力が働くのを防止している。
【００２９】
以下、関節動作制御装置１０の作動について説明する。例えば、図６に示すように、ロボットアーム５により運動対象物体Ａを追従・捕獲して、これを制動して停止させる作業を考える。図１に示す関節動作制御装置１０によれば、基本的には、仮想質量、仮想捩れ剛性及び仮想粘性係数を持つ仮想コンプライアンス機構に基づいて仮想コンプライアンス制御が行われ、この制御によれば、関節トルク検出器９が検出しコンプライアンス制御部１１  に入力される検出関節トルクτｓ  のうち、信号ノイズとなる高周波成分が取り除かれた低周波成分のみが、仮想の各パラメータから構成されるコンプライアンス制御フィルタ１１ｂに入力される。コンプライアンス制御フィルタ１１ｂによって、検出関節トルクτｓ  に応じた仮想コンプライアンス角度変位θｄ  がデジタル処理によって算出される。即ち、仮想コンプライアンス機構を用いたデジタル演算にて、検出関節トルクτｓ  に対応した関節角度を推定し、仮想コンプライアンス角度変位θｄ  が得られる。
【００３０】
一方、関節動作制御装置１０はトルク追従制御も行う。トルク追従制御においては、制御開始からの時間に依存し制動トルクプロファイル生成部１５から得られる目標関節出力トルクτｃｏｍ  を仮想捩れ剛性ｋｃ  で除する（ここでは、先端部６の仮想質量ｍ１  及び仮想粘性係数については考慮されない）ことによって、目標関節出力トルクτｃｏｍ  を反映した角度変位としての目標コンプライアンス角度変位θｃｃが得られ、目標コンプライアンス角度変位θｃｃから推定値としての仮想コンプライアンス角度変位θｄ  を差し引いたコンプライアンス角度偏差εｃ  （θｃｃ−θｄ  ）が、関節角度制御器（モータ７）に出力される。コンプライアンス角度偏差εｃ  （θｃｃ−θｄ  ）を出力することより、制御装置１０はコンプライアンス角度偏差εｃ  （θｃｃ−θｄ  ）をゼロとするように動作し、その結果、関節機構２は、制動時に摩擦等によって過大なトルクを発生することなく、目標の制動トルクのプロファイルに沿って制動トルクを作用させて制動動作を行うことができる。制動トルクプロファイル生成部１５においては、実際の制動動作の状況を検出関節角度θｅｓにより常時モニタすることにより、想定よりも減速の早遅を生じた場合には、自動的に制動トルクプロファイルの時間Ｔ１を調整する。詳細には、時間Ｔ１経過後に予め想定している所定の速度よりも大きかった（十分に制動されていない）場合には、時間Ｔ１，Ｔ２を延長する処理が行われる。
【００３１】
制動動作プロファイル生成部１４で生成された指令関節角度θｃｏｍ  は、トルクリミット制御部１２において、検出関節角度θｅｓとの偏差に不感帯処理を施し、当該偏差が小さければ不感帯処理の出力をゼロとし、当該偏差が一定以上の場合にその偏差に基づく信号が出力される。トルクリミット制御部１２の出力として、指令関節角度θｃｏｍ  は、検出関節角度θｅｓとの偏差が小さい場合にはそのままの指令関節角度θｃｏｍ  が、当該偏差が一定以上の場合には指令関節角度θｃｏｍ  との差を取ることにより飽和された信号が、コンプライアンス制御部１１に出力される。制御装置１０は、関節角度が指令関節角度θｃｏｍ  に一致するように関節角度制御部（モータ７）を制御する。トルクリミット制御部１２の出力とコンプライアンス角度偏差（θｃｃ−θｄ  ）との和の信号が目標関節角度θｒｅｆ  としてコンプライアンス制御部１１から関節角度制御器（モータ７）に出力される。この制御によって、関節角度制御部は、関節角度を指令関節角度θｃｏｍ  に、同時に関節トルクを目標関節トルクτｃｏｍ  に一致させるように関節角度制御を行う。その結果、運動対象物体Ａの追従によりロボットアーム５の先端部６は対象物体Ａの運動速度にほぼ同期した状態で対象の捕獲を行うことができる。なお、ロボットアーム５によって運動対象物体Ａを捕獲した後は、対象物体Ａとアーム５の先端部６の共通重心に仮想質量ｍ２  を置く場合もある。
【００３２】
【発明の効果】
この発明は、上記のように、回転機構について仮想コンプライアンス機構を想定し、検出トルクを受けて当該仮想コンプライアンス機構に基づいて、仮想コンプライアンス角度変位を求め、当該仮想コンプライアンス角度変位を、制動力と制動トルクとについて各プロファイル生成部からの目標指令角度と目標制動トルクとに基づいて修正し、その修正で得られた目標回転角度を出力することで前記回転部の回転角度を制御しているので、回転機構の回転部の回転角度が制動トルクや制動力を考慮して制御されることになり、回転機構の各機構部に過大な荷重を負荷物や機構に加えることなく、負荷物の運動の減速・停止などの制動を行うことができる。即ち、この発明によれば、回転部に備わるロボットアームやマニピュレータ等の係合部が運動対象物体と衝突、捕獲又は把持を行うときに、運動対象物体との係合後に回転機構に作用する制動力と制動トルクを考慮して、過大な力が回転部に作用するのを防止しつつ、回転機構の回転部の回転角度を制御することで、トルクが作用する回転部を所定の回転角度で制御することが可能な回転機構用制御装置を提供することができる。
【図面の簡単な説明】
【図１】この発明による回転機構用制御装置の実施例としての関節動作制御装置の一例を示すブロック図である。
【図２】この発明による回転機構用制御装置が適用される関節システムの模式図である。
【図３】図１に示す関節動作制御装置におけるトルクリミット制御部の不感帯特性の一例を示す線図である。
【図４】図１に示す関節動作制御装置における減速動作プロファイルの一例を示す線図である。
【図５】図１に示す関節動作制御装置における制動トルクプロファイルの一例を示す線図である。
【図６】本発明によるコンプライアンス制御の概要を示す図である。
【符号の説明】
１  機体                            ２  関節機構
３  減速機構                        ４  回転部
５  アーム                          ６  ハンド
７  モータ                          ８  関節角度検出器
９  トルク検出器                  １０  制御装置
１１  コンプライアンス制御部
１１ａ  低域濾波フィルタ            １１ｂ  コンプライアンス制御フィルタ
１２  トルクリミット制御部          １３  トルク追従制御部
１４  制動動作プロファイル生成部    １５  制動トルクプロファイル生成部
θｅｓ  検出関節角度                  τｓ    検出関節トルク
θｒｅｆ  目標関節角度                  τｃｏｍ  目標制動トルク
θｃｃ  目標コンプライアンス角度変位指令
εｃｏｍ  関節角度θｃｏｍ  と検出関節角度θｅｓとの偏差
θｃｏｍ  指令関節角度                  θ’ｃｏｍ    修正指令関節角度
θｄ    仮想コンプライアンス角度変位
εｃ    コンプライアンス角度偏差εｃ  （θｃｃ−θｄ  ）
εｃｏｍ  偏差（θｃｏｍ  −θｅｓ）
ｍｃ    関節仮想負荷慣性              ｃｃ    関節仮想粘性係数
ｋｃ    関節仮想捩れ剛性
τｆ    検出関節トルクτＳ  に低域濾波フィルタ処理を行った関節トルク
【図１】
2005014192.tif 000003
【図２】
2005014192.tif 000004
【図３】
2005014192.tif 000005
【図４】
2005014192.tif 000006
【図５】
2005014192.tif 000007
【図６】
2005014192.tif 000008