JP2960232B2 - ロボットの制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットのアームを制
御するための装置に関する。

【０００２】

【従来の技術】近年の制御や計算機技術の発達に伴い、
ロボットに対してもその適用範囲の拡大が期待されてい
るが、その中でも特に重要視されているのが、ロボット
と対象物であるワークとの接触を伴う作業である。この
作業の中には、ピンの嵌め合い作業、バリ取り、研磨作
業などがあるが、これらの一連の接触作業を実現するた
めには、ロボットアームのマニピュレータである作業端
の制御に力覚情報を取込み、これを制御することが不可
欠である。

【０００３】一方、力制御の応用として、マニピュレー
タ作業端で望ましいインピーダンスを設定して柔軟な動
作を実現するインピーダンス制御と、位置制御系と力制
御系を複合した位置／力ハイブリッド制御とがあるが、
これらは対象とする作業に応じてそれぞれ使い分けるこ
とが重要である。

【０００４】たとえば産業用ロボットなどにおいて従来
から実施されている位置／力ハイブリッド制御の方式で
は、作業端の力／トルクの方向と値を指令して、目標の
力および目標のトルクを制御することができるけれど
も、このような先行技術では、目標の力／トルクの値お
よび方向が正確に判っている場合しか、実施することが
できない。

【０００５】他の先行技術として、作業端の抵抗を制御
する方式として、いわゆるインピーダンス制御および仮
想コンプライアンス制御の各方式があり、これは、作業
端の目標剛性および目標粘性を指定することによって、
対象物であるワークとの抵抗を制御する構成となってい
る。このような先行技術では、力／トルクの精度を必要
とせずにアーム手先に取付けられた作業端とワークとの
接触を行う作業には有効であるけれども、精度を必要と
する作業には不向きである。

【０００６】

【発明が解決しようとする課題】本発明の目的は、位置
／力ハイブリッド制御方式とインピーダンス／仮想コン
プライアンス制御方式との各長所を生かして、作業に応
じて柔軟な対応が可能であるロボットの制御装置を提供
することである。

【０００７】

【課題を解決するための手段】本発明は、アーム手先の
作業端３によって作業を行う複数軸ＪＴ１〜ＪＴ６のロ
ボット２と、関節角速度θ１を検出する関節角速度検出
手段１５と、関節角速度検出手段１５の出力に応答し、
作業端３の関心点のヤコビ行列Ｊの演算を行い、現在の
速度ｘ１の出力を導出する第１演算器１７と、目標速度
ｘ_d１と、目標の位置ｘ_dと、目標の力／トルクｆ_dと、
関節角速度の目標値θ_d１とを表す出力を導出する目標
値設定回路１９と、第１演算器１７の出力ｘ１と、目標
値設定回路１９からの目標速度ｘ_d１とに応答し、出力
（ｘ_d１−ｘ１）を導出する第１減算器１８と、第１減
算器１８の出力（ｘ_d１−ｘ１）に応答し、目標粘性行
列Ｄ_dの演算を行い、出力Ｄ_d（ｘ_d１−ｘ１）を導出す
る第２演算器２２と、作業端３の現在位置ｘを検出する
現在位置検出手段２５と、目標値設定回路１９からの目
標の位置ｘ_dと、現在位置検出手段２５からの現在位置
ｘとに応答し、出力（ｘ_d−ｘ）を導出する第２減算器
２７と、第２減算器２７の出力（ｘ_d−ｘ）に応答し、
目標剛性行列Ｋ_dの演算を行い、出力Ｋ_d（ｘ_d−ｘ）を
導出する第３演算器２９と、作業端３の力／トルクｆを
検出する力／トルク検出手段３１と、目標値設定回路１
９からの目標の力／トルクｆ_dと、力／トルク検出手段
３１からの力／トルクｆとに応答し、出力（ｆ−ｆ_d）
を導出する第３減算器３３と、第３減算器３３の出力
に、力フィードバックゲインＫ_fを掛け算して出力Ｋ
_f（ｆ−ｆ_d）を導出する力制御補償器３７と、力／トル
ク検出手段３１の出力ｆと、力制御補償器３７の出力Ｋ
_f（ｆ−ｆ_d）とに応答し、出力｛Ｋ_f（ｆ−ｆ_d）−ｆ｝
を導出する第４減算器３４と、第４減算器３４の出力
｛Ｋ_f（ｆ−ｆ_d）−ｆ｝に応答し、Ｓを対角スイッチ行
列とし、Ｅを単位行列とするとき、出力（Ｅ−Ｓ）｛Ｋ
_f（ｆ−ｆ_d）−ｆ｝を導出する第１行列演算器７７と、
第２演算器２２の出力Ｄ_d（ｘ_d１−ｘ１）と、第３演算
器２９の出力Ｋ_d（ｘ_d−ｘ）と、第１行列演算器７７の
出力（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝とを加算する第
１加算器２４と、第１加算器２４からの出力［Ｄ_d（ｘ_d
１−ｘ１）＋Ｋ_d（ｘ_d−ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−
ｆ_d）−ｆ｝］に応答し、作業端３の関心点の転置ヤコ
ビ行列^tＪを求めて出力^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d
（ｘ_d−ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］を
導出する第２行列演算器３８と、第２行列演算器３８の
出力を、各関節のモータのトルクに換算する係数Ｋｑを
掛け算して出力する第４演算器３９と、関節角速度検出
手段１５の出力θ１と、目標値設定回路１９からの関節
角速度の目標値θ_d１とに応答し、出力（θ_d１−θ１）
を導出する第５減算器４２と、第５減算器４２の出力
に、関節速度フィードバックゲインＫｖを掛け算して出
力Ｋｖ（θ_d１−θ１）を導出する第５演算器４３と、
重力補償項Ｇ（θ）の信号を導出する信号発生器４４
と、第４演算器３９の出力と、第５演算器４３の出力
と、信号発生器４４の出力とを加算し、関節トルクτ、 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ） ＋(Ｅ−Ｓ)｛Ｋ_f（ｆ−ｆ_d）−ｆ｝]＋Ｇ(θ)＋Ｋ_v（θ_d１−θ１） を表す信号を導出して各軸を駆動するモータ４９に与え
る第２加算器４０とを含むことを特徴とするロボットの
制御装置である。

【０００８】

【作用】本発明に従えば、たとえば産業用ロボットなど
のアームの各関節に関節速度サーボ系を有し、所望の直
交座標系において、ロボットアームの手先である作業端
３の関心点の方向剛性と方向粘性を設定することがで
き、さらに、所望の方向の力・モーメントをも制御する
ことが可能になる。このように本発明に従えば、ロボッ
トのアーム手先の作業端で純粋に力を制御する方向と、
抵抗を制御する方向とに分離して作業を行わせることが
可能になる。これによってロボットアームの取扱うこと
ができる接触作業範囲が大きく拡がる。

【０００９】

【実施例】図１は、本発明の一実施例の全体のブロック
図である。演算回路１は、図２に示される産業用ロボッ
ト２の各軸を制御し、そのロボットアーム手先の作業端
３によって、たとえば図３に示されるようにワークであ
るボルト４を把んで、物体５のねじ孔６に嵌め込む作業
を行う。このボルト４をねじ孔６に螺合する際には、ね
じ孔６の方向７には、低剛性の並進剛性を設定し、ボル
ト４を矢符８のように回転させる方向には、必要な目標
締付トルクを指令し、これによってボルト４をねじ孔７
に嵌め込む作業を達成する。ねじ孔６の方向７は、ｚ軸
に平行であり、力制御が行われ、回転方向８およびｚ軸
方向は、コンプライアンス制御が行われ、後述の目標剛
性Ｋ_dｘを低く設定し、これによって反力を小さくし、
ねじ孔６へのボルト４の挿入を容易にする。このように
力制御とコンプライアンス制御とを複合し、これによっ
て作業に応じた柔軟な対応を可能とする。

【００１０】ロボット２は、図２に示されるように床に
固定されたベース９と、このベース９に第１軸ＪＴ１ま
わりに角変位されるアーム１０、さらに第２および第３
の各軸ＪＴ２，ＪＴ３まわりに角変位されるアーム１
１，１２が設けられ、このアーム１２は第４軸ＪＴ４の
まわりに角変位可能であり、さらにアーム１３は第５軸
ＪＴ５まわりに角変位し、またこのアーム１３に固定さ
れている作業端３は第６軸ＪＴ６まわりに角変位可能で
ある。

【００１１】このようなロボットの応答制御は、数１を
実現することによって達成される。

【００１２】

【数１】 Ｍ_x・ｘ２＋Ｄ_d（ｘ１−ｘ_d１）＋Ｓ・Ｋ_d（ｘ−ｘ_d） ＝Ｓ・ｆ＋（Ｅ−Ｓ）Ｋ_f（ｆ−ｆ_d） ここでｘはロボットアーム手先作業端３である関心点の
位置、ｘ１は手先関心点の速度、ｘ２は手先関心点の加
速度である。

【００１３】Ｍ_x は作業座標系での実慣性行列、ｘ_d は
目標の位置、ｘ_d１は目標の速度、ｆ_d は目標の力／ト
ルクである。

【００１４】さらにＤ_d は目標粘性行列、Ｋ_d は目標剛
性行列、Ｋ_f は力フィードバックゲインである。

【００１５】ｆは外力である。

【００１６】Ｓは対角スイッチ行列、Ｅは単位行列であ
る。

【００１７】Ｓ＝Ｅのときには全方向にコンプライアン
ス制御が行われる。Ｓ＝１が設定される成分方向にコン
プライアンス制御が行われ、Ｓ＝０が設定される成分方
向には力制御が行われる。

【００１８】マニピュレータ、すなわち作業端３の作業
座標系でのダイナミクスは数２で示される。

【００１９】

【数２】Ｍ_x・ｘ２＋Ｈ_x（θ１，θ）＝ ^tＪ^-1τ＋ｆ ここでθは現在の関節角、θ１は現在の関節角速度、Ｈ
_x（θ１，θ）は遠心力、コリオリ力、重力および摩擦
力などの外乱を含む非線形項、Ｊはヤコビ行列であり、
^tＪ^-1はその転置逆行列であり、τは関節トルク入力で
ある。

【００２０】数１および数２から、Ｍ_x・ｘ２を消去し
て整理すると、数３が成立する。

【００２１】

【数３】 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋ ＳＫ_d（ｘ_d −ｘ） ＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝＋Ｈ_x（θ１，θ）］ この数３は、ハイブリッド・コンプライアンス／力制御
の基礎式である。

【００２２】前述の外乱を含む非線形項の中で、本発明
の一実施例では、重力項Ｇ（θ）だけを補償する。また
応答改善のために各軸の速度サーボ系を追加し、Ｋ_vを
各軸速度ゲインとすると、数４が成立する。

【００２３】

【数４】 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d （ｘ_d −ｘ） ＋(Ｅ−Ｓ)｛Ｋ_f（ｆ−ｆ_d）−ｆ｝]＋Ｇ(θ)＋Ｋ_v（θ_d１−θ１） 数４は、前述の図１に示される実施例によって実現され
る。各軸のロボットアーム固有のベース座標系での関節
角速度は検出手段１５によって検出され、その出力はラ
イン１６を介して演算器１７に与えられる。この演算器
１７は、作業座標系におけるアーム手先の関心点のヤコ
ビ行列Ｊの演算を行い、これによってライン１７ａに
は、現在の速度ｘ１が導出されて減算器１８に与えられ
る。目標値設定回路１９からライン２０には、目標速度
ｘ_d１が導出されて減算器１８に与えられる。減算器１
８の出力（ｘ_d１−ｘ１）はライン２１から演算器２２
に与えられ、目標粘性行列Ｄ_d の演算が行われ、その出
力Ｄ_d（ｘ_d１−ｘ１）はライン２３から加算回路２４に
与えられる。

【００２４】検出手段２５はライン２６にアーム手先で
ある作業端３の現在位置ｘを表す信号を導出して減算器
２７に与える。目標値設定回路１９からライン２８に
は、目標の位置ｘ_dを表す信号が導出され、これによっ
て減算器２７の出力（ｘ_d −ｘ）はライン５２から演算
器２９に与えられる。演算器２９は、減算器２７の出力
と目標剛性行列Ｋｄの積を求め、ライン３０から加算器
２４に、Ｋ_d（ｘ_d −ｘ）を導出する。

【００２５】検出手段３１は、ライン３２にアーム手先
である作業端３の検出された力／トルクｆを表す信号を
導出して減算器３３，３４にそれぞれ与える。目標値設
定回路１９は、ライン３５から目標の力／トルクｆ_dを
表す信号を導出して減算器３３に与える。減算器３３は
ライン３６に、（ｆ−ｆ_d）を表す信号を導出し、力制
御補償器３７は、力フィードバックゲインＫｆを掛け算
して減算器３４に与える。減算器３４は、Ｋ_f（ｆ−
ｆ_d）−ｆを演算し、行列演算器７７に与える。行列演
算器７７は、（Ｅ−Ｓ）を減算器３４の出力に掛け算
し、その出力を加算器２４に与える。加算器２４の出力
はライン５３を介して行列演算器３８に与えられる。こ
の行列演算器３８は、加算器２４の出力に応答し、作業
座標系におけるアーム手先である作業端３の関心点の転
置ヤコビ行列^tＪを求め、その出力を演算器３９に与え
る。演算器３９は、行列演算器３８の出力を、各関節の
モータの電流すなわちトルクに換算する係数Ｋｑを掛け
算し、その出力を加算器４０に与える。

【００２６】目標値設定回路１９からライン４１には、
関節角速度の目標値θ_d１を表す信号を導出して減算器
４２に与える。減算器４２は、（θ_d１−θ１）を演算
し、演算器４３に与える。演算器４３は、関節速度フィ
ードバックゲインＫ_vを掛け算し、その出力Ｋ_v（θ_d１
−θ１）を導出して加算器４０に与える。この加算器４
０には、重力補償項Ｇ（θ）の信号を導出する信号発生
器４４からの信号がライン４５を介して与えられる。こ
うして加算器４０からライン４６には、前述の数４で示
される関節トルク入力τを表す信号が与えられる。

【００２７】演算回路１において、ライン４６の関節ト
ルク入力τを表す信号は減算器４７に与えられ、増幅回
路４８から各軸を駆動するモータ４９に与えられる。モ
ータ４９によって駆動される各軸の位置／速度はエンコ
ーダ５０によって検出され、検出手段１５と検出手段２
５とに与えられる。このようにしてモータ４９によって
各軸のトルクが前記値τに設定される。

【００２８】このような実施例によれば、たとえば組立
て作業としてガイドピンを有する物体を、そのピンを台
盤の孔にならわせることによって着脱する作業を行い、
この場合の設定は、ｘｙ平面へは力制御、ｚ方向および
回転方向はコンプライアンス制御を行い、この挿入直前
にミスアライメントがあるとき、そのたとえばｘ方向に
約１５ｍｍのミスアライメント偏差があっても、本件発
明者の実験結果によれば、エンドエフェクタ、すなわち
作業端３に柔軟なコンプライアンスを設定することによ
って、安定な装着および取外し作業を容易に実現するこ
とができることが確認された。したがって組立て作業に
先立って適当な位置決めさえできていれば、多少の誤差
があっても、その作業が可能であることが判る。これに
よって作業用ロボットのティーチング作業を簡素化する
ことができ、また作業端位置絶対精度を得にくい長尺ア
ームや浮遊ロボットアームの制御などに本発明が有効に
実施される。

【００２９】本発明は産業用ロボットだけでなく、予め
定める特定の用途にのみ実施されるロボットなどに関連
して実施することもまた可能である。

【００３０】

【発明の効果】以上のように本発明によれば、産業用ロ
ボットなどのアーム手先作業端で、純粋に力を制御する
方向と、抵抗を制御する方向とに分離して作業を行わせ
ることが可能になり、これによってロボットアームの取
扱うことができる接触作業範囲が大きく拡がることにな
る。特に本発明では、現在の各軸の角速度θ１を検出し
て目標角速度θ_d１となるように負帰還し、これによっ
て各軸の速度サーボ系の追加による応答改善を行うこと
ができ、また各軸を目標速度θ_dに保つための重力補償
項Ｇ（θ）を加算するようにし、重力による誤差をなく
す。こうして本発明によれば、位置／力ハイブリッド制
御とインピーダンス／コンプライアンス制御とを組合せ
て柔軟な作業を可能とするロボットの制御装置を実現す
ることができる。こうして作業対象物体であるワークに
対してアーム手先の作業端３から加える力／トルクを、
アーム手先の作業端３の変位または速度に応じて可変と
してインピーダンス／コンプライアンス制御を行い、し
かも手先座標軸方向別にその特性を任意に選べる構成と
してハイブリッド制御を行うことを可能にし、前述の数
４で示してある重力補償項Ｇ（θ）を含む特性が改良さ
れた動作が実現される。インピーダンス／コンプライア
ンス制御は、単に位置、速度、加速度、さらには力を単
独に制御するのではなく、それらの制御を組合わせてさ
らに発展させたものであり、作業端３に発生される力／
トルクを作業端３の位置ずれまたは速度の関数として指
定するものであり、柔軟な作業のために必要である。

【図面の簡単な説明】

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

【図２】産業用ロボット２の簡略化した断面図である。

【図３】図１および図２に示される実施例によって物体
５のねじ孔６にボルト４を嵌め込む作業を示す断面図で
ある。

【符号の説明】

１ 演算回路 ２ 産業用ロボット ３ アーム手先作業端 ４ ボルト ５ 物体 １９ 目標値設定回路

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 昭60−75904（ＪＰ，Ａ) 特開 昭64−66715（ＪＰ，Ａ) 特開 平１−234183（ＪＰ，Ａ) 特開 平４−343690（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁶，ＤＢ名) G05D 3/00 - 3/20 B25J 9/10 B25J 13/08 G05B 19/18 - 19/40 G05B 19/403 - 19/415 G05B 19/417 - 19/46

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 アーム手先の作業端３によって作業を行
   う複数軸ＪＴ１〜ＪＴ６のロボット２と、 関節角速度θ１を検出する関節角速度検出手段１５と、 関節角速度検出手段１５の出力に応答し、作業端３の関
   心点のヤコビ行列Ｊの演算を行い、現在の速度ｘ１の出
   力を導出する第１演算器１７と、 目標速度ｘ_d１と、目標の位置ｘ_dと、目標の力／トルク
   ｆ_dと、関節角速度の目標値θ_d１とを表す出力を導出す
   る目標値設定回路１９と、 第１演算器１７の出力ｘ１と、目標値設定回路１９から
   の目標速度ｘ_d１とに応答し、出力（ｘ_d１−ｘ１）を導
   出する第１減算器１８と、 第１減算器１８の出力（ｘ_d１−ｘ１）に応答し、目標
   粘性行列Ｄ_dの演算を行い、出力Ｄ_d（ｘ_d１−ｘ１）を
   導出する第２演算器２２と、 作業端３の現在位置ｘを検出する現在位置検出手段２５
   と、 目標値設定回路１９からの目標の位置ｘ_dと、現在位置
   検出手段２５からの現在位置ｘとに応答し、出力（ｘ_d
   −ｘ）を導出する第２減算器２７と、 第２減算器２７の出力（ｘ_d−ｘ）に応答し、目標剛性
   行列Ｋ_dの演算を行い、出力Ｋ_d（ｘ_d−ｘ）を導出する
   第３演算器２９と、 作業端３の力／トルクｆを検出する力／トルク検出手段
   ３１と、 目標値設定回路１９からの目標の力／トルクｆ_dと、力
   ／トルク検出手段３１からの力／トルクｆとに応答し、
   出力（ｆ−ｆ_d）を導出する第３減算器３３と、 第３減算器３３の出力に、力フィードバックゲインＫ_f
   を掛け算して出力Ｋ_f（ｆ−ｆ_d）を導出する力制御補償
   器３７と、 力／トルク検出手段３１の出力ｆと、力制御補償器３７
   の出力Ｋ_f（ｆ−ｆ_d）とに応答し、出力｛Ｋ_f（ｆ−
   ｆ_d）−ｆ｝を導出する第４減算器３４と、 第４減算器３４の出力｛Ｋ_f（ｆ−ｆ_d）−ｆ｝に応答
   し、Ｓを対角スイッチ行列とし、Ｅを単位行列とすると
   き、出力（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝を導出する
   第１行列演算器７７と、 第２演算器２２の出力Ｄ_d（ｘ_d１−ｘ１）と、第３演算
   器２９の出力Ｋ_d（ｘ_d−ｘ）と、第１行列演算器７７の
   出力（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝とを加算する第
   １加算器２４と、 第１加算器２４からの出力［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d
   （ｘ_d−ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］に
   応答し、作業端３の関心点の転置ヤコビ行列^tＪを求め
   て出力^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ）＋
   （Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］を導出する第２行
   列演算器３８と、 第２行列演算器３８の出力を、各関節のモータのトルク
   に換算する係数Ｋｑを掛け算して出力する第４演算器３
   ９と、 関節角速度検出手段１５の出力θ１と、目標値設定回路
   １９からの関節角速度の目標値θ_d１とに応答し、出力
   （θ_d１−θ１）を導出する第５減算器４２と、第５減算
   器４２の出力に、関節速度フィードバックゲインＫｖを
   掛け算して出力Ｋｖ（θ_d１−θ１）を導出する第５演
   算器４３と、 重力補償項Ｇ（θ）の信号を導出する信号発生器４４
   と、 第４演算器３９の出力と、第５演算器４３の出力と、信
   号発生器４４の出力とを加算し、関節トルクτ、 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ） ＋(Ｅ−Ｓ)｛Ｋ_f（ｆ−ｆ_d）−ｆ｝]＋Ｇ(θ)＋Ｋ_v（θ_d１−θ１） を表す信号を導出して各軸を駆動するモータ４９に与え
   る第２加算器４０とを含むことを特徴とするロボットの
   制御装置。