JPH0375906A - 多軸機構の制御方法及び装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、直接駆動多関節形ロボット等のごとき多軸機
構の制御方法及び装置に係り、特に、多軸機構を速度制
御もしくは位置制御して駆動する際に、高い動作軌跡精
度をうるに好適な速度指令もしくは位置指令及び駆動力
指令を生成する技術に関する。

〔従来の技術〕

多軸機構は、直列軸連結形と並列軸連結形とに分類でき
る。かかる多軸機構の制御においては。

軸間干渉により動作軌跡の精度が影響されることから、
軸間干渉の除去が問題となっている。

直列軸連結形の多軸機構としては、従来、例えば■電気
学会論文誌り編１０７巻第１号（１９８７年）第１頁〜
第８頁に記載されたものが知られている。同文献によれ
ば、遠心力等の非線形作用力が小さい複数の駆動軸から
なる機構において、各軸の駆動力指令を各々定数倍し、
これを加算して新たな駆動力指令をソフトウェア的に生
成することにより軸間干渉除去を行う方法が論じられて
いる。

また、■特開昭６１−１６９９０５号公報に多関節形ロ
ボットの動作軌跡精度を高めるため、位置制御系の伝達
特性の逆特性を位置指令に乗算して修正位置指令を生成
することにより、軸間干渉除去と応答遅れの打ち消しを
はかる方法が述べられている。

また、■特開昭６４−２０９８８号公報に、並列軸連結
形直接駆動多関節形ロボットの動作軌跡精度を高めるた
め、機構的又は、制御的に非線形作用力の除去と軸間干
渉の除去を行う方法が述べられている、。

〔発明が解決しようとする課題〕

しかしながら、直列軸連結形多軸機構に関する上記の従
来技術のは、非線形作用力に対する補償がなされていな
い。したがって、高速動作時に作用する遠心力等の非線
形作用力が大きい条件下では、軸間干渉を除去すること
ができない。このため、高速動作時の動作軌跡精度を高
められないという問題がある。

また、位置指令の逆伝達特性補償を行う上記従来技術■
は、軸間干渉が存在する駆動系では逆伝達関数が複雑な
式になる。このため、逆伝達特性補償演算に多くの演算
時間を要し、制御系のサンプル周期を短くできず、制御
が粗くなって動作軌跡精度を高められないという問題が
ある。

他方、並列軸連結形の従来技術■によれば、リンクやベ
ルト等の動力伝達部材を介して動力を伝達する構成を有
することから、駆動装置の負荷イナーシャは低減できる
。しかし、動力伝達誤差が大きいため、上記従来技術■
のような動作指令生８ 威力法では、動作軌跡精度向上に一定の限界かある。

本発明の目的は、軸間干渉と非線形作用を除去し、また
低速域から高速域までの広い範囲にわたってサンプリン
グ周期を短縮化し、動作軌跡精度を向上させることがで
きる多軸機構の制御方法及び装置を提供することにある
。

〔課題を解決するための手段〕

上記目的を達成するため、本発明の制御方法は、複数の
駆動軸が連関してなる多軸機構の制御方法において、各
軸の駆動力指令に制御対象軸との軸間干渉除去ゲインを
乗じてその総和を当該対象軸の修正駆動指令とする一方
、各対象軸の速度又は位置指令に速度又は位置制御系の
逆伝達関数を乗じて対象軸の修正速度又は修正位置指令
とすることを特徴とする。

さらに、具体的には、駆動軸数もしくは主要干渉軸数を
ｎとしたとき、各軸の速度指令θｉ１（但し、ｉ＝１．
・・・・・・ｎ、以下同じ）と速度出カθ量の偏差を基
に各軸駆動力指令τｌを生成し、この各軸駆動力指令で
１と軸間干渉除去ゲインαＪ。

の積和としてｊ軸修正邦動力指令ττ（＝３．αＪ。

τｉ）を生威し、速度制御モードのときは速度制御系の
周波数（Ｓ）領域又は離散時間（Ｚ）領域における駆動
軸間閉ループ伝達関数［ＧＪ＋　（ｓ）］又は［ＧＪＩ
（Ｚ）］と、速度指令交流考慮した直流成分Ｆｊｎｉｒ
から、下式％式％）］） ］） ） に基づき修正速度指令を生威し、位置制御モードのとき
は位置制御系の周波数（ｓ）領域又は離散時間（ｚ）領
域における原動軸開閉ループ伝達関数［ＰＪＩ（Ｓ）］
又は［ＰＪｌ（Ｚ）］と、位置指令交流成分Ｃ）Ｊｒ（
ｓ）又は（ＥＩＪ、（Ｚ）と直流成分０゜ｊ、から、下
式 ％式％）］） ）］） ） に基づき修正位置指令を生成することを特徴とする。

なお、前記速度の直流成分１９ｎｉｒと位置の直流成分
０ｎ１ｒを前回サンプル時の検出量とし、この検出量と
今回指令値との偏差をそれぞれ交流成分とし、交流成分
ラプラス又はＺ変換量を偏差のステップ関数として表現
することが望ましい。

また、各軸の開力指令が一定のしきい値を越えたとき、
前記軸間干渉除去ゲインをαＪＪ”１＋α、（、≠、）
＝０に設定するとともに、速度又は位置指令を［θ、＋
ｒ］　−１：′ｏ−＋、］又は［θＪ、］−［θａｒｌ
　１こ設定することが望ましい。

また、前記軸間干渉除去ゲインの軸間角と負荷条件に対
する最適値をテーブル化して記憶し、軸間角と負荷の検
出値に基づき最適軸間干渉除去ゲインを設定することが
望ましい。

また、各軸間角と負荷条件に対する最適軸間干渉除去ゲ
インを設定するに当り、軸間角と負荷検１ 出値から理論的に求められる軸間干渉除去ゲイン値近傍
で複数段階に変化させ、速度制御系又は位置制御系の制
御対象軸ａに対する他の軸］伝達特性［Ｇ、］を求め、
応答周波数帯域において下式（但しεはしきい値） が成立するように軸間干渉除去ゲインを設定することが
望ましい。

上記制御方法を実現する本発明の多軸機構の制御装置は
、次の構成とすることができる。

すなわち、軸間干渉除去部と位置又は速度の修正指令生
成部と中央処理部とを含んでなり、軸間干渉除去部は廓
動力指令生成部の後段に配置され、干渉除去に当り必要
な軸間干渉除去ゲインα４１（後述の（６）式参照）の
演算に必要な状態量（軸間角、負荷）は検出器により検
出され、中央処理部でα、１が実時間設定される。軸間
干渉除去部は軸間干渉除去ゲインα６．と駆動力指令τ
、の乗算及び軸部の加算（埜、τＬ）をディジタル演算
も１．１°′ ２ しくはアナログ演算により行い、修正駆動力指令変換部
又は増幅部を介して各軸の駆動装置に出力する構成とす
る。

また、速度又は位置の修正指令生成部は、中央処理部内
で速度指令値又は位置指令値を基に後述の（１３）式又
は（１８）式に基づきディジタル演算を行い、修正速度
指令又は修正位置指令を生成する。

〔作用〕

このように構成されることから、本発明の制御方法によ
れば、次の作用により上記目的が達成される。

すなわち、基本的に、駆動力指令生成に係る軸間干渉除
去において、各軸の駆動力指令に制御対象軸との軸間干
渉除去ゲインを乗じてその総和（積和）を求め、これを
対象軸の修正駆動力指令とする。これにより、まず、駆
動制御部の伝達関数を簡素化できるとともに、非線形作
用力に因る軸間干渉成分（非線形干渉成分）は除去され
ないが、線形干渉成分は除去できる。次に、このように
して簡素化された速度又は位置制御系の伝達関数の逆特
性（逆伝達関数）を求め、この逆伝達関数を速度又は位
置指令に乗じて修正処理を施すことにより、上記非線形
干渉成分を打ち消した修正指令が生成されることになる
。この結果、軸間干渉及び非線形作用が有効に除去され
るとともに、逆伝達関数が簡略化されるので演算処理時
間が短縮され、これによりサンプリング周期を高密化で
きることから、低速から高速まで高精度の動作軌跡を得
ることが可能になる。

ここで、本発明の制御方法及び装置についての原理を説
明する。

多軸機構においては、前述したように、駆動軸間に干渉
作用や非線形作用があり、これらを除去することにより
動作軌跡精度を向上できる。ここでは、多軸機構の運動
を、駆動軸の回転運動に代表させて説明する。

一般に、対象とする駆動軸ｊ　（ｊ＝１．２．・・・・
・＋ｎ）の駆動力指令τ４、駆動力変換定数Ｋ　Ｔ　Ｊ
と駆動軸位置θＪ、速度θＪ、加速度θＪの間には下式
の関係が戒りたつ。

ここで、右辺第１項は慣性項であり、第２項は遠心・コ
リオリカ項であり、第３項は重力等バイアス作用力を補
償するバイアス駆動力項である。また、ｉ、に、Ｑはｌ
、２．・・・・・・、ｎをとり、関連軸の番号を表わし
、Ａ　Ｊ　Ｉｇ　Ｂ　Ｊ　ｈ　ｔ　ｇ　Ｃ４はそれぞれ
定数である。（１）式は下式のように書き換えられる。

ここで、（２）式が（３）式のように書き換えることが
できれば慣性項の軸間干渉が除去されることになる。な
お、Ａ、は定数である。

（３）式への書換えができるためにはｊ＠ｌｌＨ動力指
令τＪ＋　Ｊ軸修正駆動力指令τ４とすると、τ。

とτＪの間に下式の関係が成立すればよい。

１５− ・・・・・・（４） Ｃ＋）　＋ＫＴａ詰ＡｐＪｋｌｌ　ｅｋＯｔ＋ＫＴＪ−
’ＣＪ：勤Ｊｉ（τｌ’ＫＴ１−”、黙Ｐａｋｔｅｈろ
處−ＫＴＬ−１Ｃ１）＊−ま ただし、αａＩ＝Ｋｒａ−”ＡａｓＡｌｌＫＴｔ　　・
・・・・・（６）ここで、α。は軸間干渉除去ゲインで
あり、非線形作用力、バイアス作用力が無視できる程に
微小な場合には、（５）式は次の（７）式となり、軸間
干渉除去が実現できる。無視しえない場合は軸間干渉除
去は実現できない。

このように演算処理すれば駆動力伝達特性、すなわち駆
動力τ４と軸加速度θ、の関係は（２）式から（３）式
のように簡略化される。

６− 一方、多軸機構の駆動方法として、速度制御系としての
駆動方法と位置制御系としての駆動方法がある。まず速
度指令を直接与えて速度制御系として駆動する場合の速
度制御系の逆伝達特性を用いた非線形作用力除去方法に
ついて説明する。速度制御系は比例（Ｐ）動作を行い、
速度指令θ１ｒと速度出力θ−の偏差に比例定数に、を
乗算することにより駆動力指令が生成されるものとする
。

前記の駆動力伝達特性式（３）において、τｊは（８）
式で示されるため（３）式は（９）式のように書き換え
られる。

τＪ＝ＫＪ（θ１．−〇、）　　　　　　　・・・・・
・　（８）・・・・・・（９） ここで、（５）式より、速度伝達特性を明らかにするた
め、速度指令、速度出力を（１０）式のように直流成分
（添字に○を付して表わす）と交流成分（〜を付して表
わす）に分離すると（５）式は（１１）。

（１２）式のように書き直される。

（直流成分） ・　・・・（１］） （交流成分） 周或数領域で扱い。

０４□と記す。

ラプラス変換ｒ＝（ｂａ、） ＡＪＪＳ＋ＫＴＪＫＪ 〜Ｇ　Ｊ　Ｊ○、ｒ□＋δ　　　　　　　　　　・・・
　　（１２）これから、速度指令と速度出力の関係にお
いて（１１）、（１２）式右辺第２項以下が非線形作用
力により発生している項であり、修正速度指令を（１３
）式のように定めることにより、直流成分、交流成分共
に指令値と出力が一致し、非線形作用力の影響を除去で
き、（１４）式の速度伝達特性となる。

＝ＯＪｒｏ＋０Ｊｒ１ （１３） ０、二ａＪｒ。十〇４，１ ・・・・　（１４） ここで、（１２）式で示されているＧＪＪの逆特性ＧＪ
Ｊ−’を交流速度指令成分しこ乗算し、（１３）式で交
流速度指令成分を生成することにより、（１２）式では
速度制御系の応答周波数（ω＝ＫＴＪＫＪ／ＡＪＪ）以
上では低下していた応答特性を補償して応答周波数を理
論的には無限大に拡げている。

ここで、例えば速度指令直流成分１９ａｒｏとして前回
サンプル時速度検出量、同交流成分０３４．として今回
速度指令値と前回サンプル時速度検出量の偏差をとり、
交流成分のラプラス変換を偏差のステップ関数で表示す
ることにより修正速度指令を求めることができる。

次に多軸機構を位置制御系として原動する場合において
、位置制御系の逆転伝達特性を用いた非９− 線形作用力除去方法について説明する。位置制御系は位
置指令に対して比例微分先行形ＰＩＤ（Ｉ−ＰＤ）動作
を行い速度指令を生威し、速度制御系は比例動作を行う
ものとする。ａ軸の■−ＰＤ動作の積分、比例、微分各
制御ゲインをＫ　Ｉｔ　。

Ｋ　ｐ　４．　Ｋ　ｎ　ａと記すと位置指令Ｂｊｒ、位
置出力０３には次の関係が戒りたつ。

０ この場合も同様に（１８）式に基づく修正位置指令を与
えることにより、非線形作用力の影響を除去できる。

なお、直流成分は（１６）式の積分効果でＦｅｊｒ。→
ＯＪｏとなる。

０Ｊｒ＝ＯＪｒｏ＋Ｌ−１ ・・・・・（１５） （３）　、　（８）　、　（１５）式より位置伝達特性
を直流成分と交流成分に分離すると下式が得られる。

（直流成分） ＫＴｊＫＪ（ＫｒＪ　（Ｌｒ。−θＪｌｌ）　ｄ　ｔ　
　ＫＰＪＯＪｏ）　＝ＣＪ・（１６） （交流成分） 周波数領域で扱い、Ｌ（ＩＩＩＪ□）＝０１１と記す。

んτ８３十に丁ＪＫＪ（１＋ＫＤＪ）　　Ｓ２＋ＫＰＪ
ＫＪＫＤＪＳ＋ＫＴΔに、＋Ｋ　ＩａＡ：Ｐ　Ｊ　、１
０Ｊｒ１＋γ　　　　　　　　　　　・（１７）ここで
例えば位置指令直流成分として前回サンプル時位置検出
量、同交流成分として今回指令値と前回サンプル時位置
検出量の偏差をとり、交流成分のラプラス変換を偏差の
ステップ関数で表示することにより、修正位置指令を求
めることができる。

以上、制御系が連続時間系で扱える場合について述へた
が、離散時間系で扱う必要のあるサンプル周期が系の応
答周期と比して十分小さくない場合は、離散時間系とし
てＺ領域での扱いが必要であり、同様に実現可能である
。

〔実施例〕

以下、本発明を実施例に基づいて説明する。

葉よ失蒼狙 第１図に本発明の一実施例のブロック図を示す。

本実施例は、多軸機構（ｎ軸）において、軸間干渉除去
と修正位置指令の生成により、位置制御特性の改善をは
かったものである。

通常、多軸機構では、動作開始点から動作終了点まで、
加速、等速、減速の各動作からなる制御が含まれる。多
軸機構の作業点の位置決め動作には、（１）ＦＴＰ動作
と（２）連続軌跡動作の２種類がある。前者は動作開始
点から動作終了点まであらかじめ速度パターンを定めて
、速度制御で動作させ、動作終了点近傍のみ位置制御で
動作させて位置決めする動作である。後者は動作開始点
から動作終了点まで定められた軌跡に沿う動作であり、
補間位置目標値をサンプル周期毎に発生して位置制御で
動作させ、これにより高精度な軌跡を描く動作である。

したがって、高軌跡精度が要求されるのは後者であり、
位置制御系の軸間干渉除去と高応答化が要求されること
になる。

第１図に示すように、速度制御系は比例（Ｐ）動作、位
置制御系はＩ−ＰＤ動作により各々駆動力指令、速度指
令を生成する例である。Ｉ−ＰＤ動作は各軸位置偏差積
分量と位置の比例・微分量との偏差をとり速度指令を生
成するため、ステップ状の位置指令入力時にも通常のＰ
ＩＤ動作でみられる駆動力指令の突変を防ぐことができ
る利点がある。また、本実施例では速度制御系の応答を
高めるため更に速度フィードバックを行い、比例動作で
駆動力指令を生成する構成としている。

図示のように、ｎ軸分の位置指令θＪ、がサンプル周期
毎に入力されると、修正位置指令生成部１０１で（１８
）式に基づく修正位置指令θ、ｒが生成され、出力され
る。次の速度指令生成部１０２では、修正位置指令θ、
ｒと位置検出量θ１からＩ−ＰＤ動作で速度指令θ４．
が生成される。そして１次の駆動力指令生成部１０３に
おいて、速度指令θ４゜と速度検出量０．の偏差から、
比例動作で駆動力３ 指令で、が生成される。このτ、から非線形作用力とバ
イアス作用力の総和である補償用駆動力指令τＪＬを減
じた量が、軸間干渉除去部１０４に入力される。τＪＬ
は（５）式の（Ｋ；：凡１八、Ｂｉｈ究ＯｋＯにＫ　−
１Ｃｔ　）に相当する。軸間干渉除去部１０４ｓ では、軸間角と負荷条件により定まる軸間干渉除去ゲイ
ンαＪｉとの乗算と、各軸ａについての積和へｆｔＪ　
Ｉ　（ｉ　’−τＩＬ）を求め、修正ｉ区動カ指令τＪ
として出力する。軸間干渉除去ゲインα、ｉは、別途（
６）式により求めて入力されるようになっている。次に
てＪにτＪＬを加算して元に戻し、増幅部１０５で増幅
して対象多軸機構の各軸の駆動装置上０６に駆動力指令
ＴＪとして入力される。これにより、多軸機構の各軸Ｊ
に駆動力が発生する。

このように軸間干渉除去有りで修正位置指令を生成する
方法を採用すると、軸間干渉除去無しで修正位置指令を
生成する場合と比して下式のように修正位置指令演算式
が簡素化される。なお、（１８）式において、０ｊｒｏ
＝Ｏ１速度積微小とした。

２４ （軸間干渉除去無し） これにより演算時間の短縮、サンプル周期の短縮をはか
ることが可能になる。

なお、第工図実施例においては、修正位置指令生成部１
０１をデジタル制御系とし、速度制御以降をアナログ制
御系としたものであるが、このデジタル・アナログの区
分は、第２図〜第４図のように変形することができる。

この区分の選択は、設計や経済性等の他の要素を考慮し
て決定する。

例えば、デジタル化すれば、センサ等の部品が少なくな
るが、演算処理の負担が増し応答性に制約が生ずる。一
方、アナログ化すれば、制御精度向上に寄与し得るが、
部品点数が増大して経済性が悪化する。

第１図は修正位置指令θ４ｒがＤ／Ａ変換され、アナロ
グの位置θ４がフィードバックされる構成、第２図は速
度指令ＯＪｒがＤ／Ａ変換され、アナログの速度ＯＡが
フィードバックされる構成、第３図は駆動力指令τｉ−
τＪＬがＤ／Ａ変換され、軸間干渉除去部１０４に入力
される構成、第４図は駆動力指令τ３＋τ４ＬがＤ／Ａ
変換され増幅部１０５に入力される構成である。

第５図〜第８図に、第５図〜第８図実施例を実現してな
る具体的なハードウェア構成図をそれぞれ示す。

第５図は第１図に対応し、位置・速度制御系をアナログ
制御系で構成した場合の動作指令生成に係る制御装置の
ハードウェア構成を示している。

中央処理部１は中央処理装置（ＣＰＵ）２、リードオン
リーメモリ（ＲＯＭ）３、ランダムアクセスメモリ（Ｒ
ＡＭ）４より構成される。中央処理部ｌは位置検出、修
正位置指令生成、軸間干渉除去ゲイン設定、Ａ／Ｄ、Ｄ
／Ａ変換管理等を行っている。中央処理部１からデータ
バス６を介して出力される修正位置指令ＯＪｒは、Ｄ／
Ａ変換部２３によりアナログ変換され、位置制御回路２
４においてアナログ位置検出器２↓（例えばポテンショ
メータ）から構成される装置フィードバック信号ＯＪを
基に、Ｉ−ＰＤ動作に基づきアナログ速度指令θ、ｒを
生成する。次に速度制御回路上２においてアナログ速度
検出器２０（例えばタコジェネレータ）から入力される
速度フィードバック信号θ、を基に、比例動作によって
駆動力指令τ４を生成する。そして、このτ４からＤ／
Ａ変換部１５より出力される補償用駆動力指令τＪＬを
減算する。つづいて軸間干渉除去部１４０乗算部１６に
おいて、中央処理部１より送信された軸間干渉除去ゲイ
ン指令αＪ、を乗算して、アナログ乗算量α１．（τｊ
−τ、Ｉ７）を生威し、同加算部１７にて各軸（ｊ軸）
毎に乗算量の総和ΣαＪｌ（τ、−で１＋、）、＝１ ＝τ４を求める。これにＤ／Ａ変換部１５から出力され
る補償用駆動力指令τＪ１．を加算して修正駆動力指令
で４千で４１、を生成し、これを増幅部工８で増幅して
、駆動装置に出力される。これにより多軸機構］−９が
駆動される。

多軸機構１９の各粁動軸にはアナログ速度検出７ 器２０、アナログ位置検出器２１が設けられており、各
出力は前述のように速度制御回路上２、位置制御回路２
４にフィードバックされる。また、位置検出量はＡ／Ｄ
変換部２５にてディジタル量に変換され、−時記憶回路
７を介して中央処理部１に取り込まれ、多軸機構の各軸
動作が中央処理部↓にて監視可能な構成となっている。

なお、暉動装置廓動時には、速度に依存する逆起電力カ
釦区動装置の入力電圧を減するように作用するが、本装
置は定電流制御がなされていて増幅部１８出力電圧と発
生暉動力の間には比例関係が成立するものとする。また
、第５図中の増幅部変換定数ＫＡＪＫＴイ’／Ｋを第１
図ではＫＴＪと記した。

第６図は、第２図に対応し、位置制御部をディジタル制
御により行う場合の動作指令生成に係る制御装置のハー
ドウェア構成である。位置検出量Ｄ　ａはアナログ量と
してフィードバックする必要は無いのでエンコーダ等の
ディジタル検出器を用いる。速度Ｏｊはアナログ量でフ
ィードバックを行うため、タコジェネレータ等の速度検
出器を用８− いてもよい。しかし、アナログ位置検出器出力の微分信
号もしくはディジタル位置検出器出力の周波数・電圧（
Ｆ／Ｖ）変換信号を用いてもよい。

位置検出器２１信号は位置検出回路１１、−時記憶回路
７を介して中央処理部↓に取り込まれる。

中央処理部上では修正位置指令、位置検出量からＩ−Ｐ
Ｄ動作アルゴリズムに基づき、サンプル周期毎に速度指
令が更新される。そして、Ｄ／Ａ変換部」○でアナログ
変換され、速度制御回路上２において、速度検出器２０
より得られる速度フィードバック信号との偏差に基づき
原動力指令で。

が生成される。この駆動力指令τ、から補償用駆動力指
令τＪＬを滅した成分が軸間干渉除去部］４に入力され
、ここにおいて修正駆動力指令τ、が生成される。これ
にＤ／Ａ変換部１５から出力される補償用駆動力指令で
４Ｌが加算され、増幅部１８に送られ、駆動装置に出力
される。

第７図は第３図に対応し、位置・速度制御部をディジタ
ル制御により行う場合の動作指令生成に係る制御装置の
ハードウェア構Ｉ戊である。位置、速度検出量共アナロ
グ量としてフィードバックする必要が無いので速度検出
器を省くことができ簡易な構成とできる。位置検出器２
１としてはエンコーダ等ディジタル検出器を用い、中央
処理部ｌで修正位置指令の生成、位置制御のＩ−ＰＤ動
作、速度制御の比例動作等を行い駆動力指令τ−τＪＬ
を生成する。これがサンプル周期毎にＤ／Ａ変換部２２
でアナログ変換され、軸間干渉除去部１４において修正
駆動力指令が生成される。そしてＤ／Ａ変換部１５より
出力される補償用駆動力指令τＪＬと加算され、増幅部
１８を介して、駆動装置に出力される。

第８図は第４図に対応し、位置・速度制御部、軸間干渉
除去部をディジタル制御により行う場合の動作指令生成
装置のハードウェア構成である。

位置検出器２１としてはエンコーダ等ディジタル検出器
を用い、中央処理部上で修正位置指令の生成、位置制御
のＩ−ＰＤ動作、速度制御の比例動作、補償用駆動力指
令の減加算、軸間干渉除去部＋τＪＬを生威し、サンプ
ル周期毎にＤ／Ａ変換部２２でアナログ変換して増幅部
１８に送られ駆動装置に出力される。

ここで、第５図〜第７図に示したアナログ軸間干渉除去
部１４の具体的構成について第９図〜第１１図に示す。

軸間干渉除去部１４は乗算部１６と加算部１７の２段構
成となっている。乗算方法として、次の２方式が考えら
れる。なお、第９図〜第１１図において、駆動力指令を
τｉ′と記すがこれは前述のτｉ−τＩＬ相当量を意味
するものとする。

（Ｉ　）　ｌｌｉ動力指令を演算増幅する方式（ｎ）駆
動力指令を非線形増幅する方式通常軸間干渉除去ゲイン
αＪ、は各軸駆動力指令の極性が同一の場合Ｏ≦−αＪ
、≦−１で与えられる。

〈方式（Ｉ）について〉 増幅率は演算増幅回路の抵抗比により定まることから、
中央処理部１から送信される指令に基づき抵抗値可変と
できる方式としては、下記２方式が考えられる。

１− （１）指令値をＤ／Ａ変換することにより得られるアナ
ログ電圧を印加することにより抵抗値可変となる電圧可
変抵抗を演算増幅回路に組み込む方式 （２）複数段階の増幅率を得られるよう複数種類の抵抗
を並列配置し、その両端にマルチプレクサを接続し、抵
抗選択指令を両端のマルチプレクサに与えることにより
単一の抵抗が選択され増幅率を設定可能とする方式 各方式を各々第９図、第１０図を用いて説明する。方式
（Ｉ−１）は第５図〜第７図の一時記憶回路１３より送
信される指令（電圧可変抵抗Ｒ２Ｊ　ｔデータ）を次の
ように定める。本方式は第９図において、演算増幅器２
７を用いて反転増幅回路を構成するため、各抵抗値と駆
動力指令増幅率α、。

の間には下式が成りたつ。

したがって、αＪ１が与えられ、その可変域でＲ２□可
変となるよう固定抵抗Ｒを選択し、Ｒ２ＪＩ抵抗データ
を中央処理部１で作成することにより＝３２ 反転乗算駆動力指令−αｄｉτｉをうることが可能とな
る。また、演算増幅器２７の仮想接地抵抗３０はＲｉＲ
２、ｉ　ｔ　／　（Ｒ□＋　Ｒ２Ｊｔ　）に選ぶのが適
当であることが知られているため、抵抗３０にも電圧可
変抵抗を設けた。なお電圧可変抵抗の具体的構成につい
ては後述する。また、干渉除去部加算部エフは乗算部１
６で得られた反転乗算出力−α０τｉ′を並列設置され
た抵抗３２を介してｊ軸分につき並列入力することによ
り演算増幅器３１で増幅率１で反転増幅することにより
構成した。加算部１７の□構成は他方式でも同一とした
。

次に、方式（Ｉ−２）を第１０図を用いて説明する。２
段の反転増幅回路を用いて構成されている点は方式（Ｉ
−１）と同一であり、可変抵抗Ｒ２Ｊｌは、抵抗選択指
令がマルチプレクサ３４゜３５に入力されることにより
選択設定される。マルチプレクサ３４．３５により設定
しうる増＠率α４．は方式（Ｉ−１）と比して一般に少
段階にしか設定できない。したがって、選択されるべき
抵抗値は、（２０）式から算出される最適値との差が最
小さなるよう選択しなければならない。

〈方式（１’Ｉ）について〉 次に、方式（ＩＴ）について、対数増幅器を用いた例を
第１１図に示して説明する。駆動力指令τｊは演算増幅
器４１のＡ部でバイアス電圧ＶＡが印加され、対Ｉ・ラ
ンジスタ４２のトランジスタＱ１に電流ｊ、が流れる。

１ヘランジスタＱ２には定電流ｉ２が流れるように調整
されているため、Ｂ点ではトランジスタの非線形特性（
対数特性）による下式で示される電圧が発生する。

演算増幅器４３では抵抗Ｒ４，Ｒ５により非反転増幅が
行われ０点出力電圧は下式で示される値となる。

ここでは、Ｖ　ｃ　＝−αＪ１τｉ′となるように設定
することが望まれるので、抵抗Ｒ、、１１を電圧可変抵
抗により構成し、中央処理部１よりＤ／Ａ変換部２６を
介して抵抗値が下式の値となるように指令することによ
り所望の乗算を行うことが可能となる。

次に、第９図、第１土図で示した電圧可変抵抗２９．４
０の具体的構成とその特性を、第１２図〜第１９図を用
いて説明する。電圧可変抵抗として光導電素子を用いて
構成した例が第１２図〜第１７図である。第Ｉ２図は光
導電素子を用いたユニットセルの構造を示している。本
ユニットセルは光導電素子４５、発光体４４、遮光カバ
ー４６により構成されている。発光体４４端子ＣＤ間に
電圧が印加されると発光体４４が発光し、先導素子４５
が照射される。このときの照射光量により端子ＥＦ間の
導電度が変化する（抵抗値が変化する）光導電効果を利
用して抵抗を電圧制御するものである。遮光カバー４６
は光の洩れを防ぐ役割をはたしている。

ユニットセルの特性例（モリリカ製ＭＣＬ５ ７２３Ｃ１発光体：タングステンランプ、光導電素子：
　Ｃｄ　Ｓ）を第工３図に示した。ＣＤ間印加電圧５〜
ＩＯＶ付近ではほぼ双曲線で近似可能である。

そこで、バイアス電圧を印加する回路を第１４図に示す
ようにユニットセルの前段に設けると、ユニットセルの
特性は第１５図のように変化し、所望のＣＤ間印加電圧
に対して双曲線抵抗特性をうろことができる。更に第１
４図に示すように並列抵抗Ｒｐ（＝ｂ）を設けることに
より、その合成抵抗Ｒ′は下式のように印加電圧Ｖの１
次関数になる。

この結果を第１６図に示す。これから、所定の干渉除去
ゲインを設定するためには、αａ　＋　”　ＲＪ　Ｉ　
／Ｒｋとおくと、印加電圧Ｖを下式のように設定すれば
よい。（但し、Ｒｋ：固定抵抗） この場合Ｂ点印加電圧と増幅率（α４１）の関係は６ 第１７図に示す１次関数関係となり設定が容易な特性と
なっている。

ところで光導電素子は光入力時の立上り特性は良好だが
、断光時の立ち下り時間が１００ｍ５ｅｃ以」二かかる
ため、高速で指令値を変化させる用途には不向きである
。そこで、応答性を改善するために磁気抵抗素子を用い
た例を第１．８図、第工９図を用いて説明する。第１８
図は磁気抵抗素子を用いた電圧可変抵抗の構造を示して
いる。工法側入力電圧が磁気コア４８に巻回されたコイ
ル４７に印加される。磁気コア４８は、隙間を介して磁
気抵抗素子４９と近接配置されており、コイルに通電す
ることにより、磁気コア４８、磁気抵抗素子４９を貫通
する閉磁路が形成される。磁気抵抗素子４９としてはイ
ンジウム・アンチモナイド（Ｉ　ｎ　Ｓ　ｂ）等が用い
られる。磁気抵抗素子４９のＥＦ間抵抗Ｒと、ＣＤ間電
圧Ｖの間には、第１９図に示す関係がある。したがって
その双曲線近似領域を利用できるようバイアス電圧を印
加し、並列に抵抗を接続することにより、第１７図と同
様の特性が得られ、（２４）式に基づき、電圧指令によ
る軸間干渉除去ゲインα４．の設定が高速で可能となる
。

以上述べた本実施例の多軸機構の動作指令生成方法を用
いることにより、軸間干渉が除去され、簡素となった位
置伝達特性をもとに修正位置指令を生成するため、修正
位置指令演算が簡単になりサンプル周期を従来の修正位
置指令生成方法採用時と比して短縮でき、動作軌跡精度
を高めることができる利点がある。また、軸間干渉除去
部は簡易なアナログ回路で構成可能である。

髪主失置狙 次に、本発明の第２実施例を第２０図〜第２２図を用い
て説明する。本実施例は連続軌跡動作時にサンプル周期
毎に発生する位置指令値ＯＪｒをその微分量の速度指令
値θ４．に変換し、停止位置近えて速度制御系を動作さ
せ、停止位置近傍では位置制御系を動作させる方式とし
たものである。なお、軸間干渉除去部については第９〜
１１図で説明したものを適用する。第１実施例に比して
修正指令（速度）の演算を簡易化し、高い動作軌跡精度
をうるものである。

例えば、第２７図に示す２ア一ム直列配置ロボットの軸
間干渉除去時の速度０での位置、速度主伝達関数ＰＪＪ
、ＧＪＪは次の（２６）式で示される。速度主伝達関数
は位置主伝達関数と比して次数が低く、逆伝達関数の演
算も容易で、第１実施例の方法と比し、演算時間の短縮
、サンプル周期の短縮をはかることができる利点がある
。

次に、本実施例の動作指令生成方法について説明する。

第２０図〜第２２図は同一ブロック構成であるが、第２
０図から順にディジタル制御系で構成する割合を増した
例を示している。これらの実施例では、速度制御モード
と位置制御モードに９ 分け、これに応じて修正速度指令の生成方法を位置／速
度指令切換部２０１で切換えるようになっている。速度
制御時にはサンプル周期毎に生成される速度指令θ４ｒ
から、修正速度指令生成部２０２において、（１３）式
に基づいき修正速度指令−＊ θ−１が生成される。次に駆動力指令生成部２０３でＯ
Ｊｒと速度検出値θ、との偏差から比例動作に基づき駆
動力指令τＪが生成される。その後は第１実施例と同様
補償用駆動力指令τＪＬの減算、軸間干渉除去、補償用
駆動力指令τＪＬの加算が行われた後、増幅され駆動装
置に出力される構成となっている。一方、位置制御時に
は位置指令θＪｒよりＩ−ＰＤ動作により生成される速
度指令θ、ｒより修正速度指令θＪ、が生成され、以下
速度制御時と同様の処理が行われる構成となっている。

第２０図は修正速度指令ＯＪｒをＤ／Ａ変換し、アナロ
グ速度検出値θ、との偏差から駆動力指令τ」を生成す
る方式である。第２１図は、修正速度指令ＯＪｒと速度
のディジタル検出結果から生成される駆動力指令（τｉ
−τＩＬ）をＤ／Ａ変換し４〇− て軸間干渉除去部へ入力する方式である。第２２図は、
修正位置指令τ、の生成、軸間干渉除去演算補償用駆動
力指令域・加算をディジタル演算で行い、得られた駆動
力指令τＪ＋τＪＬをＤ／Ａ変換し増幅部へ入力する方
式である。

第２０図〜第２２図に示した動作指令生成は、それぞれ
前記第６図〜第８図に示したハード装置により実現可能
である。ただし、中央処理部１での演算内容が第２０図
〜第２２図で説明した動作指令生成方法による点が異な
る。

以上述べた第２実施例の多軸機構の動作指令生成方法及
び装置を用いることにより、第１実施例と比して簡素な
演算により連続軌跡動作を行わせることが可能となり、
サンプル周期の短縮と動作軌跡精度の一層の向上をはか
ることが可能になる。

第立失見狙 次に本実施例の第３実施例を第２３図を用いて説明する
。第１、第２実施例で述べた修正指令、軸間干渉除去を
行い多軸機構を駆動すると所要駆動力が大きくなり、加
減速時など所要駆動力が大きい際に能動力不足を起こし
オーバーシュート動作を起こすことがある。これを防ぐ
ために第２３図のステップ３０１〜３０５に示す手順で
、あるしきい値τ。以上の能動力が要求される際、指令
修正、軸間干渉除去を行わないことにより能動力不足に
よるオーバーシュートを防止する。

量±失凰斑 次に本発明の第４の実施例につき第２４図を用いて説明
する。第１、第２実施例の軸間干渉除去方法では、軸間
干渉除去ゲインαｊ１、速度と位置の閉ループ伝達関数
ＧｉＩとＰ　Ｉ　Ｌを演算するために、（１）式におけ
るパラメータＡ　Ｊｌｌ　ＢＪＩＩ　ＣＪＩ　を知る必
要がある。また特に、Ａ　Ｉ　Ｊを求めるためにＡＪｌ
を知る必要がある。各パラメータとも軸間角０□、軸杵
用外力等の状態量変化により変化する。

一般に直列軸連結形の多軸機構は、軸杵用外カー定時に
、各パラメータは軸間角積算値の正弦・余弦和で表示で
き、下式で示される。

ＡＪｌ＝ＡＪ１０＋Σ（ＡＪｌ、１ＩＣＯ８（Σθｓ）
＋Ａ、ｕ２ｍｓｊｎ（ΣＯ８））ｍ＝１　　　　　　　
　ｓ＝ｍ　　　　　　　　　　　ｓ：ｍＣＪ＝ＣＪＯ＋
Σ（Ｃ４□１ｌｃｏｓ（ΣＯｓ）　＋Ｃａ２＋５Ｓｌｎ
（Σ０５））ｍ＝１　　　　　　　　ｓ刊　　　　　　
　　　　５勺・・・（２７） したがって、３　ｎ　（ｎ２＋　ｎ　＋　１）（２ｎ十
王）個の未知パラメータを求めるために、３ｎ　（ｎ”
＋ｎ＋１）（２ｎ＋↓）個の軸間角条件で、ある軸に正
弦波能動力を与えた時の各軸速度、能動力を検出し、（
１）式に（２７）式を代入して得られる方程式に検出量
を代入し解くことにより、Ａ、ＪＩ、　ＢＪｈ＋＋ＣＪ
と軸間角０．の関係が明らかになり、Ｇ　ＪＩｒＰ　Ｊ
　Ｉ　Ｊ　α、１を実機条件で正確に求めることができ
る利点がある。第２４図ステップ３１］、〜３↑７にそ
のフローを示した。また、実際の多軸機構で運転方程式
が既知の場合は未知計数の数を大幅に低減でき実験条件
をも少なくできる。

髪旦失凰爽 次に、本発明の、第５実施例を第２５図のフローチャー
トを用いて説明する。本実施例は第４実施例に比して、
負荷イナーシャ検出に要する時間を３ 短縮する方法である。すなわち、ステップ３２５と３２
６において、多軸機構の軸杵用外カと負荷イナーシャの
関係及び軸杵用外カと暉動装置鄭動電流の関係をあらか
じめ明らかにしておく。これにより、ステップ３２３で
各軸駆動装置の原動電流を検出することによりステップ
３２４で簡易に負荷イナーシャを求めることができる。

第６実施例 次に本発明の第６実施例第２６図フローチャー１へを用
いて説明する。本実施例は第１、第２実施例の軸間干渉
除去ゲインα５．の最適化設定をはかるための方法であ
る。

第４．第５実施例で求めたイナーシャ値を基に演算した
軸間干渉除去ゲインの周辺で、α４１を複数段階変化さ
せ、低周波数域における速度制御系もしくは位置制御系
の干渉伝達ゲインＧＪｌ（ｉ≠ｊ）もしくはＰＪｌ（ｉ
≠ｊ）を求め、その主伝達ゲインＧＪＪ、Ｐａｄとのゲ
イン差があるしきい値以上となるように選ぶ方法である
。

具体的には、ステップ３３４で正弦波状の速度４− 指令もしくは位置指令を与え、ステップ３３５で速度も
しくは位置の応答特性から各伝達ゲインを求め、ステッ
プ３３６でその比較をする。この比較は、例えば、しき
い値εを２０ｄＢ　（０，１）とすると下式に基づき判
定する。

軸間角、作用外カ一定： （すべての１＋Ｊについて） 第７実施例 本発明の応用例として、多軸機構として水平多関節形直
接廓動ロボットに適用した実施例を、第２７図と〜第３
７図を用いて説明する。第２７図と第２８図本ロボット
の構造を示す。第２７図は本ロボットの簡略構造図であ
り、第２８図はその縦断面図である。それらの図に示す
ように、本ロボットは、第１アーム５工と、第２７−ム
５３をそれぞれ能動モータ５０と５２により直接駆動す
る構成で、手首軸（スプライン軸）５９はモータ５４の
回転動力をベルト７２を介してポールネジ軸５７に伝達
しそれと螺合するブラケット５８と共に上下方向に駆動
し、更にモータ５５により減速機５６、ベルト７３を介
して回転駆動する構成となっている。スプライン軸５９
はブラケット５８に対して回転自由に支持されており、
その下端に工具６０が取り付けられている。各軸の駆動
モータには位置・速度検出器８０，６９，７０゜７１が
設けられている。また、動作範囲のオーバラン、原点位
置を検出するための特定位置検出器、被検出体と、オー
バランして更に行き過ぎた動作を行うことを止めるスト
ッパ及び衝突部材が設けられている。なお、手首回転軸
は３６００回転する構造となっており、オーバラン検出
器、ストッパ、衝突部材は設けられてない。また、手首
上下軸には滑落防止用のブレーキ８工が設けられている
。

本ロボットの４軸すべての駆動モータが通電され、トル
クτ−を発生すると、第２７図かられかるように手首上
下軸と回転軸の駆動モータ５４と５５間には干渉は無い
。一方、手首上下軸と回転軸の駆動モータ５４及び５５
と、２軸用駆動モータ５２と、１軸用廓動モータ５０と
の各駆動モータの相互間には軸間干渉がある。ところで
、手首上下軸と回転軸の駆動モータ５４と５５のモータ
トルクは、１・２軸モータのトルクと比して小さく、同
時に加減速することも少ないと考えられる。

そこで、１・２軸間の干渉に着目し、手首部負荷はすべ
て第２アームに含まれていると考えた２ア一ム剛体モデ
ルを第２７図（Ｂ）に示す。１・２軸モータ５０と５２
により駆動される第１アーム５１と第２７−ム５３は、
それぞれアーム長ａｌｌａ２、重心位置党１．氾２、集
中質量ｍ１．ｍ２、集中イナーシャＩ、Ｚ、Ｉ。Ｚの剛
体モデルで扱える削正を有するものとする。この場合、
第１アーム角０□、１・２軸軸間角θ２．１・２軸のモ
ータトルクＴ□、Ｔ２とすると運動方程式は次式で示さ
れる。

・・・・・・（２９） ここで、Ｊ、、＝Ｊ工□。−Ｊ１３ＣＯＳθ２７ Ｊ工２＝Ｊ２□＝Ｊ２□−Ｊ１４ｃＯ８ｏ２Ｊ　１３　
”　２　Ｊ　１４　＝　　２　Ｊ　２３　＝　２　ｍ　
２　ａ　Ｉ　Ｑ　２Ｊ　１１゜＝　Ｌ、ｚ＋　Ｉｚｚ＋
ｍ１ｆｌ、”＋ｍ２（ａｌ”＋　Ｑ２”）Ｊ２２＝Ｌｚ
＋ｍＪ２”　　　　　　　　　である。

第１、第２の実施例で述べた動作指令生成方法を本ロボ
ットに適用する場合の位置速度制御用動作指令生成装置
のブロック図を各々第２９図と第３０図に示す。本ロボ
ットは１、上軸、２軸モータに重力によるバイアス作用
力は作用しないが、遠心トルクに関する補償用駆動カ指
令τＪＬが要求される。

そこで、第１、第２の実施例の軸間干渉除去方法を本ロ
ボットに適用する場合について説明する。

まず、第２９．３０図における速度制御系の閉ループ特
性を説明する。軸間干渉除去ゲインα１１＝α２□＝１
としてα□２．α２□を求める問題を扱う。

速度θとトルクτの化には次の関係が成立する。

４８− を各々（３１）式のように直流・交流成分に分離し、（
２９）　、　（３０）式に代入すると（３２）〜（３４
）の関係が得られる。

直流成分：Ｆ”ｋ。

：０２０＝θ２ｒＯ ・・・・・・（３２） 交流成分：周波数領域で扱い、Ｌ　（ｅ　Ｊ　）　＝　
Ｏａと記す。

ここで、Ｄ、、＝Ｊ、１Ｓ＋Ｊｘ３θ、ａｓｉｎθｚ　
ｏ　＋　Ｋ、　ＫＴ□Ｄｘｚ＝Ｊｘｚｓ＋ｃＪｘ３ｏ−
８＋２Ｊ１４み２゜）ｓｉｎθ２ｏ＋ａ１□に、に７□
Ｄ２１＝Ｊ２１Ｓ＋２Ｊ２３　ｆＪｘｏｓＷθ２ｏ＋α
２１に１ＫＴ２Ｄ２□＝Ｊ２□Ｓ　＋　Ｋ２　ＫＴ２ これから各軸応答速度は次式で示される。

右辺第１項で示される直流成分は、（３２）式で示され
るが、（３２）式右辺第２項の干渉成分はα１２゜α２
□を下式のように設定することにより除去できる。

ここで△は下式で示される。

△＝（Ｊｌ、Ｊ２□−Ｊｌ、”）Ｓ２＋［Ｊ　１１に２
に７２　＋　Ｊ２２（１丁、３ｆｊ　□。ｓｉｎ　Ｏ１
９十に、　ＫＴＩ）Ｊｌｚ（（２Ｊｚ３Ｌｏ　＋　Ｊｔ
３ｂ１ｏ　＋２Ｊ□４Ｑ２ｏ）ｓｊ、ｎｏ２゜＋α２□
に１ＫＴ２＋α２□に２ＫＴ、−））］　Ｓ　＋に２Ｋ
Ｔ２ＣＪ１３　ｆ１７０ｓｉｎｌ：ｊ２ｏ＋ＫＩＫＴ１
）−（２Ｊ２３（３□０５ｉｎＱＺ。十α２１に、−Ｋ
ｒ２）（（ＪＨ，３８１ｏ＋２．７Ｈａ２ｏ）ｓｘｎＯ
２ｏ＋　α１２に２ＫＴＪ・・・・（３４） また、（３５）式右辺第２項で示される交流成分は、（
３７）式のように軸間干渉除去ゲインを設定しＧ１□＝
Ｇ２□＝０とすることにより干渉成分を低減できる。

しかるに有限軸速度がある場合は、（３６）式により直
流成分の除去はできるが交流成分Ｇ、２．Ｇ２□の分子
定数項（Ｓの掛っていない項）がＯとならな１− いため完全な軸間干渉除去は実現できない。そこで、軸
間干渉除去ゲインを（３７）式より設定し、非線形項を
修正指令生成により除去する方法を次に述べる。

まず、第３０図で示す動作指令生成では、その非線形項
を除去するため、（３８）式で示す修正速度指令を生威
し、指令として与える。これにより高速動作時にも軸間
干渉除去が可能となる。

５ま ただし、α１□、α２１は（３７）式で示されＧ　Ｊ　
ｌは（３３）式で示されている。

また、第２９図で示す動作指令生成では、次式の修正位
置指令を与えることにより、高速域まで軸間干渉除去と
応答特性の改善をはかることができる。

ここで、例えば速度と位置指令の直流成分θＪｒＯとｆ
ＪＪｒｏとして、前回サンプル時の速度と位置検出量を
、速度と位置指令の交流成分ＯＪｒｔ＋　ＢＪｒ□とし
て今回指令値と前回サンプル時検出量の偏差をとり、そ
のステップ関数をラプラス変換する際に用いることによ
り、修正指令生成を行うことができる。この軸間干渉除
去の無い場合と有る場合の速度伝達ゲインの周波数特性
例（０□＝１８０゜θ１ｏ−０２ｏ＝Ｏ）を第３工図（
Ａ）−（Ｄ）　、第３２図（Ａ）、（Ｂ）　　に示す。

これから軸間干渉除去により速度伝達関数の極零相殺が
行われ、伝達関数が簡素化されたことがわかる。更に修
正速度指令生成部を付加すると速度伝達ゲインの周波数
特性は、第３３図（Ａ）、（Ｂ）に示すように、周波数
によらずゲイン低下の無い理想的な特性にできる。なお
、第３２図、第３３図で、Ｇ１□＝０２１＝Ｏである。

また、（３７）式の軸間干渉除去ゲインは、モータの駆
動力指令・駆動力変換定数Ｋ　Ｔ　ａと負荷イナーシャ
ＪｚＪの積の比で示されており、それらの値を知る必要
がある。Ｋ　Ｔ　Ｊはモータ駆動装置固有の値であるた
めロボットの動作条件によらず一定であるが、負荷イナ
ーシャはロボットの負荷条件、動作姿勢により大きく変
化する。そこで例えば第４実施例の負荷イナーシャ測定
法を適用する。

ここでは（１）式第１、第２項の係数Ａ、　Ｂ　（Ｃ＝
Ｏ）を求める方法を述べる。本ロボットの剛体モデルの
運動方程式（２９）は、同−成分項をまとめると、次式
で表現される。

Ｔ１＝　（Ｊｌｌ、−２Ｊ、ｃｏｓθ２）θ１＋　（Ｊ
２□−Ｊ、ｃｏｓＯ２）θ２十Ｊ１４　（２θ、＋０２
）０□Ｓ和０□Ｔ２”　（Ｊ２□Ｊ１４ｃｏｓθ２）θ
ｘ＋Ｊｚｚ　０２−Ｊ１４　ｆｌ□”５ｉｎＯ２”’（
４０）この方程式において、未知イナーシャパラメータ
はＪ　ｘｘｏ　＋　Ｊ　２□、Ｊ１４の３個に集約され
る。手先負荷一定時には特定のロボット姿勢角（θ２＝
９０°、１８０６）で（４０）式が（４１）式のように
簡素化される。

θ２＝１８０°Ｔ□＝（Ｊ１１ｏ＋２Ｊ１４）θ１＋　
（Ｊ２２　＋　Ｊ　ｔ４）θ２Ｔ２＝　（Ｊ２□＋Ｊ１
４）’１３□＋Ｊ２□°６□一定負荷条件の各ロボット
姿勢角で微小駆動トルクで正弦波状揺動運動をさせたと
きの速度、トルクを検出すると（４１）式で、Ｊ１１０
１Ｊ２□、Ｊ□。を未知数とした４式が得られるが、未
知数より方程式数が１つ多いため、２式を足し合わせて
１式とすることにより未知数を求めることができる。そ
５− の結果から（３７）式中の各イナーシャパラメータは次
式により求められる。

Ｊ□１＝Ｊ□□。−２Ｊ□４ＣＯ５θ２．Ｊ２□＝Ｊ２
□Ｊ工２”Ｊ２１”Ｊ２□−Ｊ１４ｃｏｓθ２　　　・
・・・・・（４２）本ロボットの（３７）式に基づく軸
間干渉除去ゲインの計算値を第３４図に示した。本計算
値は剛体モデルに基づく運動方程式から求められたもの
であるが、実機の最適軸間干渉ゲインはこの値と若干異
なる可能性がある。そこで第６実施例で説明したように
負荷条件、ロボット姿勢角一定とした時の軸間位置干渉
ゲインＰＪｉを測定し、最適軸間干渉除去ゲインαＪｉ
を求める。軸間干渉除去無し又は有りの場合の軸間位置
干渉ゲインＰ。（ｊ≠ｊ）の周波数変化をそれぞれ第３
５図と第３６図に示した。低周波域における応答ゲイン
が大幅に低減されていることから、最適軸間干渉除去ゲ
イン採集時には、低周波数の正弦波位置指令を入力し、
その応答を求めることにより簡易に最適値を求めること
が可能となる。一定負荷条件で各ロボット姿勢角につき
、軸間干渉除去ゲイン（α１□。

６− α２工）を第３４図に示した計算値近傍で数段階に変化
させ、Ｐ工２／Ｐ工□≦０．１．Ｐ２□／Ｐ２□≦０．
１となる（α１□、α２１）の組み合わせを第３７図に
示した。この結果は第３４図と差異がみられ、この軸間
干渉除去ゲインの組み合わせを、各負荷条件につき中央
処理部４のＲＯＭ２にテーブル化して記憶し、軸間干渉
除去ゲイン設定時に参照することにより、最適な軸間干
渉除去を実現することが可能になる。

ところで、手先負荷が頻繁に変化する場合は、第４実施
例で述べた方法（（４１）式を用いる方法）で負荷イナ
ーシャを測定するのは時間がかかる。

そこで第５実施例を適用し、上下軸駆動モータ所要トル
ク検出電流値と手先負荷の関係から、あらかじめ用意さ
れている手先負荷と負荷イナーシャＪ□、。ｌＪ２□、
Ｊ□。の関係を用いて、Ｊ工、。ｌＪ２□。

Ｊ、ｔ４修正値を求め、軸間干渉除去ゲイン計算値を求
めるのに利用する。これにより、第４実施例の方法を用
いるよりも時間短縮をはかることが可能になる。

また、本発明の第３実施例で説明した低速・低トルク域
では、軸間干渉除去を有効とし、一定のしきいトルク以
上では軸間干渉除去ゲインα、Ｉを零とすれば、トルク
不足を起こさないロボットの駆動が可能となる。一般に
高トルクが要求される急加減速時には、高い動作軌跡精
度が要求されることは少ないと考えられるから、本方法
を用いることにより干渉除去、指令修正有無切換時に振
動が生ずることがあっても、大きく精度を損なうことは
無い。

ここで、水平多関節形ロボッＩ・の機構による分類とそ
の特徴を第１表に示し、説明する。第１表に示した分類
に対応する機構の概念図を第３８図〜第４２図に示す。

各機構の運動方程式は次のとおりである。なお、第２７
図（Ｂ）に示したと同様の諸元に基づき、２軸の重心位
Ｍ　Ｑ２≠Ｏとし、リンク質量バランス有りの条件とす
る。

■直列方式、準直並列方式 ■直並列方式（特開昭６４−２０９８８号公報）Ｔ１＝
Ｊ□、Ｏ□十Ｊ１□０２ Ｔ２＝Ｊ、２θ□十Ｊ２２θ２ ■準並列方式（特開昭６１−１７３８７５号公報）■並
列方式（特開昭６１ １２］−８８０号公報） Ｔ□＝Ｊ、□０□ Ｔ２＝Ｊ２．θ２ ９ ６０ 〔発明の効果〕 以」二説明したように、本発明によれば、次の効果が得
られる。

（１）各軸の駆動力指令に制御対象軸との軸間干渉ゲイ
ンを乗じてその総和（積和）を求め、これを対象軸の修
正原動力指令としていることから、制御系の伝達関数を
簡略化でき、また軸間干渉の線形成分を除去でき、各軸
の独立動作が可能になる。

これに加え、上記簡略化された伝達関数に基づいて、速
度（又は位置）制御系の逆伝達関数を求め、この逆伝達
関数を速度（又は位置）指令に乗算し、これを対象軸の
修正速度（又は位置）指令としていることから、軸間干
渉の非線形成分が除去されるとともに、動作の追従性が
向上され、動作軌跡精を高くすることができる。しかも
、修正速度（又は位置）指令の演算処理が簡単化される
ので、演算時間の短縮およびサンプリング周期の高密化
を図ることができ、動作軌跡精度を更に向上できる。

（２）　　また、軸間干渉除去ゲイン設定にあたり、負
荷イナーシャの測定を多軸機構の揺動試験又は静的駆動
電流検出により行ってゲイン計算値を求め、その計算値
付近で数段階にゲインを変えて多軸機構の干渉伝達ゲイ
ン測定を行い、最適ゲインを見出すようにしたものによ
れば、同ゲインの最適化を容易に図ることができる。

【図面の簡単な説明】

第１図〜第４図はそれぞれ本発明の第１実施例の多軸機
構制御装置のブロック図、第５図〜第８図はそれぞれ第
１図〜第４図実施例に対応するハードウェア構成図、第
９図〜第１１図は本発明の第１実施例の軸間干渉除去部
の構成図、第１２図〜第１９図は第９図又は第１１図の
電圧可変抵抗の具体例の説明図であり、第工２図は発光
体・光導電素子を用いたユニットセルの構造図、第１３
図は第１２図の抵抗と電圧の関係図、第１４図は電圧可
変抵抗の駆動回路図、第１５図はバイアス電圧印加時の
抵抗、電圧の関係図、第１６図は並列抵抗設置時の抵抗
と電圧の関係図、第１７図は増幅率と電圧の関係図、第
１８図は磁気抵抗素子を用いた電圧可変抵抗の構造図、
第１９図は磁気抵抗素子の抵抗と電圧の関係図、第２０
図〜第２２図は本発明の第２実施例の多軸機構制御装置
のブロック図、第２３図は本発明の第３実施例の多軸機
構の動作指令生成における軸間干渉除去有無切換方法を
示す流れ図、第２４図は本発明の第４実施例の多軸機構
の動作指令生成における多軸機構の負荷イナーシャ測定
方法を示す流れ図、第２５図は本発明の第５実施例の多
軸機構の動作指令生成における多軸機構の負荷イナーシ
ャ測定方法を示す流れ図、第２６図は本発明の第６実施
例の多軸機構の動作指令生成における最適軸間干渉除去
ゲイン探索方法を示す流れ図、第２７図〜第３７図は本
発明を適用してなる具体的なロボット装置の一実施例に
関する図であり、第２７図は多軸機構の１例としての水
平多関節形直接駆動ロボットの外観図、第２８図は第３
工図の縦断面図、第２９図と第３０図は第２７図のロボ
ットの２軸動作指令生戒に係る制御ブロック図、第３１
図は軸６３− 間干渉除去無し時の速度制御系特性図、第３２図は軸間
干渉除去有り時の速度制御系特性図、第３３図は第３０
図の速度制御系特性図、第３４図は軸間干渉除去ゲイン
計算値とロボット姿勢角との関係を示す図、第３５図と
第３６図は軸間干渉除去部・有時の干渉位置伝達特性図
、第３７図は探索された最適軸間干渉除去ゲインとロボ
ッ１へ姿勢角との関係を示す図、第３８図〜第４２図は
水平多関節形直接駆動ロボットの機構を分類して示すも
のであり、それぞれ第３８図は直列方式、第３９図は準
直列方式、第４０図は直並列方式、第４工図は準並列方
式、第４２図は並列方式の概念構成図である。 １・・・中央処理部、２・・・ＣＰＵ、３・・・ＲＯＭ
、４・・ＲＡＭ、５・・・アドレスバス、６・・・デー
タバス、７，１３・・・−時記憶回路、８・・・サンプ
リングパルスタイマ、９・・デコーダ、１０，１５゜２
２．２３・・・Ｄ／Ａ変換部、１１・・・位置検出回路
、１２・・・速度制御回路、１４・・・軸間干渉除去部
、工６・乗算部、エフ・・加算部、１８・・・増幅４ 部、１９・・・駆動装置及びそれと結合された多軸機構
、２０・・・速度検出器、２１・・・位置検出器、２３
・・・位置検出回路、２７，３１，４．１．４３・・・
演算増幅器、２８，２９，３０，３２，３３゜３６．３
９，４０・・抵抗、３４，３５，３７゜３８、・・・マ
ルチプレクサ、４２・・・対トランジスタ、４４・・・
発光体、４５・・・光導電素子、４６・・・遮へい体、
４７・・・コイル、４８・・・磁気コア、４９・磁気抵
抗素子、５ｏ・・・１軸用直接廓動モータ、５１・・・
第１アーム、５２・・・２軸用直接廓動モータ、５３・
・・第２アーム、５４・・・手首上下軸駆動モータ、５
５・・手首回転軸駆動モータ、５６・・減速機、５７・
・ボールネジ軸、５８・・・ブラケット、５９・・・ス
プライン軸、６ｏ・・工具、６１．６５．７４・・・衝
突部材、６２．６６・・・ストッパ、６３，６７．７９
・・・特定位置検出器、６４．６８，７５．７６・・・
特定位置被検出体、６９．７０，７１．８０・・位置・
速度検出器、７２．７３・・・動力伝達ベルト、７７　
・スプライン軸受、７８・・・スプライン軸受取付部材
、８１・・ブレーキ。 ６７ （”　ｆ）、？７１ＦＩＩ＆１７４／ｌｌ、ノ２彊式ど
ｌノ５．／／ １′８ 第旧図 ＶｄＶＯＯＶ１３

Claims (1)

1. 【特許請求の範囲】 １、複数の駆動軸が連関してなる多軸機構の制御方法に
   おいて、各軸の駆動力指令に制御対象軸との軸間干渉除
   去ゲインを乗じてその総和を当該対象軸の修正駆動指令
   とする一方、各対象軸の速度又は位置指令に速度又は位
   置制御系の逆伝達関数を乗じて対象軸の修正速度又は修
   正位置指令とすることを特徴とする多軸機構の制御方法
   。 ２、複数の駆動軸が連関してなる多軸機構の制御方法に
   おいて、駆動軸数もしくは主要干渉軸数をｎとしたとき
   、各軸の速度指令■＿ｉ＿ｒ（但し、ｉ＝１、……ｎ、
   以下同じ）と速度出力■＿ｉの偏差を基に各軸駆動力指
   令τ＿ｉを生成し、この各軸駆動力指令τ＿ｉと軸間干
   渉除去ゲインα＿ｊ＿ｉの積和としてｊ軸修正駆動力指
   令▲数式、化学式、表等があります▼ を生成し、速度制御モードのときは速度 制御系の周波数（ｓ）領域又は離散時間（ｚ）領域にお
   ける駆動軸間閉ループ伝達関数［Ｇ＿ｊ＿ｉ（ｓ）］又
   は［Ｇ＿ｊ＿ｉ（ｚ）］と、速度指令交流成分■＿ｊ＿
   ｉ（ｓ）又は■＿ｊ＿ｒ（ｚ）と非線形力項を考慮した
   直流成分■＿ｎ＿ｊ＿ｒから、下式▲数式、化学式、表
   等があります▼ ▲数式、化学式、表等があります▼ （１≦ｉ、ｊ≦ｎ） に基づき修正速度指令を生成し、位置制御モードのとき
   は位置制御系の周波数（ｓ）領域又は離散時間（ｚ）領
   域における駆動軸間閉ループ伝達関数［Ｐ＿ｊ＿ｉ（ｓ
   ）］又は［Ｐ＿ｊ＿ｉ（ｚ）］と、位置指令交流成分■
   ＿ｊ＿ｒ（ｓ）又は■＿ｊ＿ｒ（ｚ）と直流成分θ＿ｎ
   ＿ｊ＿ｒから、下式 ▲数式、化学式、表等があります▼ ▲数式、化学式、表等があります▼ （１≦ｉ、ｊ≦ｎ） に基づき修正位置指令を生成することを特徴ととする多
   軸機構の制御方法。 ３、前記速度の直流成分■＿ｎ＿ｊ＿ｒと位置の直流成
   分θ＿ｎ＿ｊ＿ｒを前回サンプル時の検出量とし、この
   検出量と今回指令値との偏差をそれぞれ交流成分とし、
   交流成分ラプラス又はＺ変換量を偏差のステップ関数と
   して表現したことを特徴とする請求項２記載の多軸機構
   の制御方法。 ４、各軸の駆力指令が一定のしきい値を越えたとき、前
   記軸間干渉除去ゲインをα＿ｊ＿ｉ＝１、α＿ｊ＿ｉ（
   ｉ≠ｊ）＝０に設定するとともに、速度又は位置指令を
   ▲数式、化学式、表等があります▼又は▲数式、化学式
   、表等があります▼ に設定することを特徴とする請求項２ 又は３記載の多軸機構の制御方法。 ５、前記軸間干渉除去ゲインの軸間角と負荷条件に対す
   る最適値をテーブル化して記憶し、軸間角と負荷の検出
   値に基づき最適軸間干渉除去ゲインを設定することを特
   徴とする請求項２、３、４いずれかに記載の多軸機構の
   制御方法。 ６、各軸間角と負荷条件に対する最適軸間干渉除去ゲイ
   ンを設定するに当り、軸間角と負荷検出値から理論的に
   求められる軸間干渉除去ゲイン値近傍で複数段階に変化
   させ、速度制御系又は位置制御系の制御対象軸ｊに対す
   る他の軸ｉの伝達特性［Ｇ＿ｊ＿ｉ］を求め、応答周波
   数帯域において下式（但しεはしきい値） Ｇ＿ｊ＿ｉ（ｉ≠ｊ）／Ｇ＿ｊ＿ｊ≦ε（１≦ｉ、ｊ≦
   ｎ）が成立するように軸間干渉除去ゲインを設定するこ
   とを特徴とする請求項２、３、４、５いずれかに記載の
   多軸機構の制御方法。 ７、複数の駆動軸が連関してなる多軸機構の制御装置に
   おいて、各制御対象軸についての位置指令と位置検出量
   の偏差又は速度指令と速度検出量の偏差に基づいて各軸
   の駆動力指令を生成する駆動力生成手段と、該生成され
   た駆動力指令に与えられる軸間干渉除去ゲインを乗算す
   るとともに、その総和を求めて前記各制御対象軸の修正
   駆動力指令を生成する軸間干渉除去手段とを備え、該修
   正駆動力指令により前記制御対象軸の駆動装置を駆動す
   ることを特徴とする多軸機構の制御装置。 ８、前記軸間干渉除去手段がアナログ構成の乗算部と加
   算部を有してなり、該乗算部は与えられる軸間干渉除去
   ゲインに応じた電圧指令により抵抗値が変化する電圧可
   変抵抗と、固定抵抗と、演算増幅器を有して形成され、
   前記加算部は演算増幅器と固定抵抗を有して形成された
   ことを特徴とする請求項７記載の多軸機構の制御装置。 ９、前記軸間干渉除去手段がアナログ構成の乗算部と加
   算部を有してなり、該乗算部は与えられる軸間干渉除去
   ゲインに応じた電圧指令により抵抗値が変化する選択抵
   抗と、固定抵抗と、演算増幅器を有して形成され、前記
   加算部は演算増幅器と固定抵抗を有して形成されたこと
   を特徴とする請求項７記載の多軸機構の制御装置。 １０、前記軸間干渉除去手段がアナログ構成の乗算部と
   加算部を有してなり、該乗算部は与えられる軸間干渉除
   去ゲインに応じた電圧指令により抵抗値が変化する可変
   抵抗により増幅率が可変の対数増幅器を有して形成され
   、前記加算部は演算増幅器と固定抵抗を有して形成され
   たことを特徴とする請求項７記載の多軸機構の制御装置
   。 １１、前記乗算部を形成する電圧可変抵抗が、前記電圧
   指令に応じて発光する発光体と、該光が照射される光導
   電素子と、光の照射効率を高める遮光カバーにより構成
   され、光導電素子の両端子を出力としたことを特徴とす
   る請求項８、１０いずれかに記載の多軸機構の制御装置
   。 １２、前記乗算部を形成する電圧可変抵抗が、前記電圧
   指令に応じた磁力を発生する電磁石と、該電磁石の磁路
   の一部に形成した空隙に挿入された磁気抵抗素子とによ
   り形成され、該磁気抵抗素子の両端子を出力としたこと
   を特徴とする請求項８、１０いずれかに記載の多軸機構
   の制御装置。 １３、前記乗算部が、前記電圧可変抵抗の前段にバイア
   ス電圧印加手段を有し、かつ電圧可変抵抗と並列に抵抗
   が設けられ、該電圧可変抵抗の入力電圧と出力抵抗値と
   の間に１次関数関係が成立するように構成したことを特
   徴とする請求項８、１０、１１、１２いずれかに記載の
   多軸機構の制御装置。 １４、請求項１、２、３、４、５いずれかに記載の多軸
   機構の制御方法により駆動することを特徴とするロボッ
   トの駆動方法。 １５、請求項６、７、８、９、１０、１１、１２、１３
   いずれかに記載の多軸機構の制御装置を具備してなるこ
   とを特徴とするロボット。