JP2524389B2 - サ―ボ制御装置 - Google Patents

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、回転むらを有する位置伝達機構を持つサー
ボ系、特に多関節型産業用ロボットの円滑制御を行なう
サーボ制御装置に関する。

従来の技術 一般に位置伝達機構は、何等かの非線形を有する。こ
れを補償するため、従来は前記非線形特性をマイナスと
したものをそのまま位置指令に加えていた。比較的剛性
の高い機械かまたは比較的遅い動作を対象とするとき使
われる方法である。例えば精密旋盤がボールねじの送り
ピッチ誤差を事前に測定して補正している例などは公知
である。

剛性の小さな機械で、比較的速い速度の場合に起こる
共振に着目して、振動を抑圧することを検討したもの
に、「ロボットの防振制御に関する研究：向井他：日本
機械学会第14回技術講演会、NO.860−１、P21、1986」
がある。

又、モータ以降に比較的柔らかな弾性要素を有する機
械としては、代表的なものが多関節型産業用ロボットが
ある。

発明が解決しようとする課題 上記文献に示されたものは共振モデルを考えて、速度
またはトルクに補正を加えるので、比較的速度が遅く伝
達誤差が現れる場合には有効でない。またこれは、真の
起振源を伝達機構の位置伝達誤差に求め、その２次的表
現としての速度むらまたはトルクむらとする時、従来の
方法の補正そのものであって微分形で表現したに過ぎな
いことが示される。

また、上記多関節型産業用ロボットで移動経路精度を
向上させようとするとき、従来手法の位置伝達特性の静
的逆印加では弾性要素の補償が困難であり、また別の従
来方法では共振状態からはずれた低速域での位置伝達特
性の補償が困難である。

このように低速から共振状態まで両者を統一的に取り
扱う方法は従来なく、従って、共振状態でもないが低速
状態でもない場合の、変動抑圧はできていなかった。

本発明の目的は、比較的柔らかな弾性を有する共振系
に位置伝達機構が有する位置伝達誤差が加えられたとき
発生する変動を理論的に明らかにして、その統一的な抑
圧を実現できるサーボ制御装置を提供しようとするもの
である。

課題を解決するための手段 本発明は上記目的を達成するために、サーボモータ
と、前記サーボモータの位置と速度の制御を行なう制御
装置と、前記サーボモータに結合され、歯車のピッチ誤
差に原因する入力と出力の角度の位置検出値の計測値に
対応して算出される位置誤差が非線形特性を有する減速
機と、前記減速機に結合されたロボットアームとからな
るサーボ系に対し、前記減速機の入力と出力の角度の位
置検出値の計測値に対応して算出される位置誤差の絶対
値と、この絶対値の１回時間微分したものと、前記絶対
値の２回時間微分したものとを重み付け加算した和を位
置補正入力として、前記制御装置に与えられる位置指令
に加算するサーボ制御装置とした。

第２発明として、サーボ系の位置指令に対する減速機
出力の応答を定める伝達関数を２次で近似した逆関数
を、減速機の入力と出力の角度の位置検出値の計測値に
対応して算出される位置誤差の絶対値に乗じたものを位
置補正入力とするサーボ制御装置とした。

第３発明として、サーボ系の位置指令に対する減速機
出力の応答を定める伝達関数を必要な次数で近似した逆
関数を、減速機の入力と出力の角度の位置検出値の計測
値に対応して算出される位置誤差の絶対値に乗じたもの
を位置補正入力とするサーボ制御装置とした。

作 用 本発明ではサーボ系全体の伝達特性を考慮して、減速
機の伝達特性の非線形部分のみでなく、それの１次時間
微分を考慮することで速度に現れる非線形影響分を、ま
た２次時間微分を考えることで加速度に現われる非線形
影響分をも考慮できるので、遅い速度のほぼ静的な場合
から、比較的速く動いてシステムの動的特性を無視でき
ない場合まで包括的に対処が可能となり、従来手法では
対処できなかった中間的な速度域または柔らかな弾性要
素を有するシステムに対し減速機の非線形要素の影響を
抑制できる。

補正部分が０次の位置成分、１次の速度成分、２次の
加速度成分を有するので、動的システムの振舞に対し、
十分正確な補償を与えることができる。

実施例 第１図はサーボモータの特性を表わすブロック図であ
る。Ｖは指示速度、θ_Ｍはモータ位置、_Ｍはモータ速
度、T_Vはモータ軸に加わるトルク外乱を表わす。Ｌ、
Ｒ、K_T、K_eは、それぞれモータのインダクタンス、抵
抗、トルク定数、誘起電圧定数である。J_M、D_Mはそれぞ
れモータのイナーシャ、粘性抵抗を表わす。このような
モデルでの、T_Vの、速度θ_Ｍへの影響を抑制する試み
が、マイコンの急速な性能向上によって多くなされてい
る。

外乱としては色々なものが考えられる。コギングトル
クと呼ばれるモータ自身の回転トルクむら、ロボットア
ームであれば先に接続されている腕の姿勢変化による負
荷変動、グリースを使用しているのであれば粘性抵抗変
化、垂直多関節ロボットならば重力の影響、速度が速い
場合はコリオリ力などがあるが、以下では最も影響の大
きい減速機の回転むらにつき説明する。

ここで、減速機の回転むらについて説明する。減速機
の入力回転位置をθ１、出力回転位置をθ２、減速比を
Ａとしたとき一般に以下のような関係になる。

θ２＝Ｇ×（θ１＋φ） ここで、 φ＝０ であれば理想的減速機であり、減速機の入出力関係は線
形である。φがゼロでなければ減速機の入出力特性は非
線形となる。

φとしては様々なものがあるが、ここでは歯車のピッ
チ誤差が起因する回転むらを考える。回転むらはφで表
現すれば、正弦関数のように＋と−に連続的に変化する
関数である。

このような回転むらを含むサーボ系を以下に考える。

減速機のバネ定数をK_F、アームのイナーシャをJl、粘
性抵抗をD_Lとするとき、減速機とアームは１次の振動系
を構成する。この１次振動系に減速機の回転むらが加わ
るとする。

第２図がそのモデルを適用したサーボ系のブロック図
である。Ｎは減速比、ηは伝達効率である。K_E、K_Mは K_E＝K_T/（Ls＋Ｒ） K_M＝1/（J_Ls＋D_L） である。K_Vは位置偏差の電圧指令への変換である。

第２図を式で表わせば、 J_{M Ｍ}＋D_{M Ｍ}＝T_M＋T_G （１） J_{L Ｌ}＋D_{L Ｌ}＋K_F（θ_Ｌ−θ_Ｆ）＝０ （２） K_F（θ_Ｆ−θ_Ｌ）＝−（N/η）T_G （３） θ_Ｆ＝（θ_Ｍ＋T_V）/N （４） Ｌ＋RI＝Ｖ−K_eθ_Ｍ （５） K_TI＝T_M （６） T_V＝Fsin（２θ_Ｍ＋φ） （７） である。T_Gは減速機を介して結合される負荷のトルクの
フィードバックである。θ_Ｌは負荷の回転角度位置、θ
_Ｆは減速機の出力軸の回転角度位置で、バネ成分の手前
である。T_Vは減速機が発生する回転むらを表わしたもの
である。T_Vの形は減速機によって異なるが、ここでは代
表的な例としてハーモニックドライブ（以下、HDCと呼
ぶ）を考える。HDCではモータ回転数θ_Ｍの２倍の周期
の位置伝達誤差を持つ。もちろん２倍でなくとも良く、
周期的であれば良いことは以下の検討内容に注意すれば
簡単に理解できることであるので、以下代表的な２倍の
場合で考える。

式（４）を式（７）を使って書き直せば、 θ_Ｆ＝θ_M/N＋（1/N）Fsin（２θ_Ｍ＋φ） （８） であり、明らかに減速機出力に表わせる非線形の回転む
ら、すなわち位置伝達誤差表現である、Ｆ、φは機械で
定まる固定値である。

トルクむらでなく、位置伝達誤差でモデルを考えるの
は従来例で述べた２つの方法が、変動要因を位置伝達誤
差とするとき、全く同じことを別表現したに過ぎないこ
とを後で明らかにするためである。

さて、従来手法として、先ず静的な位置の補正である
が、速度が遅くK_Fの比較的大きな場合であるから、_Ｍ ＝_Ｍ＝_Ｌ＝_Ｌ＝＝０ （９） とする。（２）、（３）、（１）、（６）、（５）と順
次結果を代入して、結局のところ Ｖ＝０ となる。よって、第２図より θ_ｒ＝θ_Ｍ となる。（４）より θ_Ｌ＝θ_Ｆ＝（θ_Ｍ−T_V）/N＝（θ_ｒ−T_V）/N であるから、希望する位置出力をθ_Ｌとするとき Ｎθ_Ｌ＋T_V （10） を位置指令θ_ｒとしてやれば良いことが判かる。すなわ
ち、位置誤差の分だけ位置の指令を前もって補正すると
いう考えである。

次に従来例として引用した文献について説明する。θ
_Ｌが振動しないことを θ_Ｌ＝ω_Lt （11） で表わす。負荷の粘性抵抗と、モータのインダクタンス
は十分小さいとして無視する。（２）より θ_Ｆ＝θ_Ｌ である。よって（４）より θ_Ｍ＋Fsin（２θ_Ｍ＋φ）＝Ｎω_Lt （12） である。モータの回転位置を θ_Ｍ＝ω_Mt＋θ_MV （13） と表わす。θ_MVはモータ軸での振動を表わす項である。
この時、負荷は振動していない、即ちシステムは広義の
安定であるとして良いから ｜θ_MV|《ω_Ｍ （14） である。よって（12）は近似して ω_Mt＋θ_MV＋Fsin（２ω_Mt＋φ）≒Ｎω_Lt （15） となるので ω_Ｍ＝Ｎω_Ｌ （16） θ_MV＝−Fsin（２ω_Mt＋φ） （17） が得られる。

（５）、（６）より K_{F Ｍ}＝Ｖ−RT_M/K_T （18） であるから（１）よりT_Mを消去すれば J_{M Ｍ}＋（D_M＋K_eK_T/R）_Ｍ＝VK_T/R （19） が得られる。（13）、（17）より_Ｍ ＝ω_Ｍ−２ω_MFcos（２ω_Mt＋φ）_Ｍ ＝（２ω_Ｍ）²Fsin（２ω_Mt＋φ） であるから、指令電圧で表わせば Ｖ＝（RD_M/K_T＋K_e）ω_Ｍ −（RD_M/K_T＋K_e）（２ω_Ｍ）Fcos（２ω_Mt＋φ） ＋RJ_M/K_T（２ω_Ｍ）²Fsin（２ω_Mt＋φ） （20） が得られる。

ところで第１図の外乱の無いシステムが安定であるこ
と、すなわち、振動が十分に良く減衰するためには、良
く知られているように （RD_M＋K_TK_e）》RJ_M （21） でなければならないから、速度指令電圧としては Ｖ＝（RD_M/K_T＋K_e）ω_Ｍ −（RD_M/K_T＋K_e）（２ω_Ｍ）Fcos（２ω_Mt＋φ）（22） を与えれば良いことが判かる。（22）の右辺の第２項が
振動抑制のために加える量である。振幅の （RD_M/K_T＋K_e） （23） は機械的に固定な値である。

以上をまとめれば、減速機の位置伝達誤差を仮定し
て、物理的解釈に無理なく次の結論を得たことになる。
すなわち「機械的に固定な位相を持ち、機械的に固定さ
れた値と角速度の２倍を乗じた振幅を持つ正弦波を補正
信号とすれば良い。」この結果は上記文献の結論と同一
であるが、文献では、トルクむらまたは回転速度むらを
仮定し、「共振状態」における振動の特解を求めている
が、一方で共振をずらしていて、物理的解釈に曖昧な点
がある。意味の正しさと、共振を起こすモータ回転数で
ない場合に上記結論が有効であることは本発明で初めて
明らかにされた。

次に、クロージッドループによる補正方法と、オープ
ンループによる補正方法を検討し、本発明の必要性を示
す。

第２図を等価変換すれば第３図が得られる。

但し S_M＝K₁/（１＋K₁K_e） （24） S_A＝KS/［sN（K₂−１）（１＋K₁K_e）］ （25） K₁＝K_EK_M （26） K₂＝1/［（J_LS＋D_L）ｓ］ （27） K₃＝−K_MK_Fη/N （28） である。ｓはラプラス変数である。我々が直接知ること
ができるのは、入力Ｖと出力_Ｍ（θ_Ｍでも同一であ
る）であるから第４図の構成で_Ｍ ＝S_RV−sT_V （29） を与えるG₁、G₂が求まるか調べる。ここにS_Rは第３図で T_V＝０ としたときの、Ｖから_Ｍへの伝達関数であり S_R＝S_M/（１＋S_A） （30） で与えられる第４図より_Ｍ ＝［S_M（１−G₁）／（１＋S_MG₂＋S_A）］Ｖ ＋［S_A/（１＋S_MG₂＋S_A）］sT_V （31） となる。従って G₁＝1/（１＋S_A） （32） G₂＝−1/S_M （33） であれば良い。アームの系S_Aへの入力は_Ｍ ＋sT_V＝S_RV （34） となって、振動を含まない。

以上まとめれば、「G₁、G₂が（32）、（33）で与えら
れるとき、第４図のクロージッドループで位置伝達誤差
に起因する振動を抑圧できる」である。

しかし、モータ軸へ加わる外乱が同時に存在するとき
は問題は単純ではない。モータ軸へ加わる外乱をT_Cとし
て第５図を考える。α、βをパラメータとして_Ｍ ＝S_RV−α（1/S_A）T_C−（１＋β）sT_V （35） なる形に_Ｍを与えるG₁、G₂を求めてみる。α、βが十
分に小さいとすれば、アームの系S_AにはS_RVのみ与えら
れて振動は抑圧される。

第４図での検討と同じようにして_Ｍ ＝［S_M（１−G₁）／（１＋S_MG₂＋S_A）］Ｖ ＋［1/（１＋S_MG₂＋S_A）］T_C −［S_A/（１＋S_MG₂＋S_A）］sT_V （36） が得られる。（35）と（36）を比較して S_A/（１＋S_MG₂＋S_A）＝（１＋β） （37） 1/（１＋S_MG₂＋S_A）＝α/S_A （38） でなければならず、従って １＋α＋β＝０ （39） とならねばならない。よってαとβを同時に小さくはで
きず、第５図において外乱T_CとT_Vを同時に抑圧可能なG₁
とG₂は存在しない。即ち、「クロージッドループで外乱
T_CとT_Vの両者を抑圧することはできない」と結論され
る。

T_Vとして減速機回転むらを考えてきたが、T_Cの代表例
としてはコギングトルクむらがある。モータへ加わる外
乱はサーボアンプのゲインを十分に大きくすることで十
分に抑圧可能である。従って、減速機の回転むらの補正
はオープンループで行なう必要があることが判った。

さて文献で示された注入方法であるが２つの大きな問
題がある。１つは文献がそもそも速度むらまたはトルク
むらが原因する共振状態を考えていることから自明では
あるが、速度むらとかトルクむらの主たる原因である減
速機の位置伝達誤差自体の補正には有効でないことであ
る。トルクまたは速度の形で補正しようとするとき、補
正信号振幅は（22）で示すように角速度が乗じられてい
るので速度が小さくなるとゼロへ近づきノイズに埋もれ
て注入の効果はなくなる。これは速度に注入するからで
あるが、位置への注入については述べられていない。

第２に変動の真の原因が位置の伝達誤差であるから、
位置へ注入するとした場合は（22）の補正信号は、従来
例の静的な位置の補正そのものであって新規性に欠け
る。これを確認する。

（22）の補正信号のみ取り出して V_C＝−［（RD_M＋K_TK_e）/K_T］（２ω_Ｍ） ×Fcos（２ω_Mt＋φ） （40） とするとき、これは速度指示であるから積分して第６図
に示すように位置に加えるとすれば （1/▲Ｋ^′ _Ｖ▼（V_C/s）＝−［（RD_M＋K_TK_e）/K_T▲Ｋ^′
_Ｖ▼］ ×Fsin（２ω_Mt＋φ） （41） である。静的位置誤差を補正するため位置の指令への注
入を検討しているのであるから、対象としている周波数
領域では過度応答を無視してモータは十分速く応答する
としてよい。従って、モータの位置指令は位置伝達誤差
が無いとすれば、モータの回転位置と一致するから ▲Ｋ^′ _Ｖ▼＝1/S_M として良く、また 1/（J_Ms＋D_M）≒1/D_M として良い。よって S_M＝K₁/（１＋K₁K_e）≒K_T/（RD_M＋K_TK_M） となる。これを使って（41）を書き換えれば （V_C/▲Ｋ^′ _Ｖ▼ｓ）≒−（1/S_M▲Ｋ^′ _Ｖ▼）Fsin（２
ω_Mt＋φ） ＝−T_V （42） となる。即ち、文献に示した例は変動要因を位置伝達誤
差とするとき、従来手法の位置伝達誤差の逆を加えて補
正するのを微分形で表わしたに過ぎない。

以上述べてきたように、従来例はある特殊な場合を想
定して、上記想定外では成立しない近似のもとでのみ成
立する。

以下、比較的柔らかな弾性要素からなる振動系を比較
的ゆっくりと動かせた場合の、静的位置誤差と、振動系
による振動の増大が同時に存在する場合を検討する。位
置のフィードバックを考えたブロック図の第７図は、等
価変換されて第８図となる。

（K_VS_R/s）／（１＋K_VS_R/s） （43） が位置指令から位置出力まで伝達関数である。従って、
明らかに減速機の位置伝達誤差の模擬生成値を とする時 が位置補正信号である。これを第９図に示す。

（１＋K_VS_R/s）／（K_VS_R/s）＝（J_M/K_VK_E）s² ＋［（D_M/K_EK_e）/K_VK_E）］ｓ＋１ （45） であるから、結論として制御対象のシステムが第２図で
ある時、印加すべき信号は位置伝達誤差の模擬生成値の
０次の項と１次の項と２次の項の線形加算和である。
（21）で用いたシステムが安定であるための条件を書き
直せば （D_M/K_VK_e）》J_M であるから、システムが十分安定であれば、より簡易な を位置への補正信号として良い。

以上説明した方法は、システムのモデルを第２図とし
た場合であるが、システムに時間遅れ要素が追加された
場合でも、伝達関数でブロック図に表わされる限り全く
同様の議論が成立することは明らかである。実際、式
（44）ではK_V、S_Rの具体的な構造は要求していない。

上記の説明で明らかとなった重要点をまとめる。まず
第１に、先行例は補正信号を速度に注入している。しか
し、位置に注入することが重要であることが明らかとな
った。第２に共振状態またはその近傍の状態のみでな
く、減速機の回転むらを共振状態から外れたところで補
正しようとすれば、減速機の回転むらである減速機の入
力と出力の角度の位置検出値の計測値に対応して算出さ
れる位置誤差の絶対値と、この絶対値の１回時間微分と
前記絶対値の２回時間微分を重み付け加算すればよいこ
とが明らかとなった。式（43）、（44）、（45）または
第７図、第８図、第９図より、補正入力は系全体の伝達
関数と、変動成分である減速機の回転むら、即ち、非線
形特性とで与えられることが明らかである。サーボ系を
第７図で考えれば近似された系であるが、第８図と第９
図より系全体の伝達関数が補正に用いられるのが明らか
であるから、近似の精度を向上すればより正確な補正量
が得られることは明白である。

これらの補正信号の生成は、最近盛んに検討が行なわ
れている。信号処理要の高速マイコンを用いたディジタ
ルアンプで容易に実現される。

発明の効果 本発明は上記ような構成をとったので、モータ回転が
低速な場合から、高速な場合まで同様に回転むらを抑圧
することのできるサーボ制御装置を提供できた。

【図面の簡単な説明】

第１図はモータ軸にトルク外乱が加わっている場合のブ
ロック図、第２図は位置伝達誤差を有し、弾性要素が無
視できない減速機がモータ軸に結合され、減速機には更
に負荷が結合されている場合のブロック図、第３図は第
２図を等価変換して、位置伝達誤差T_Vに直目して整理し
たブロック図、第４図は位置伝達誤差に起因する振動を
クロージッドループで抑圧するブロック図、第５図は第
４図に加えて、モータ軸にも外乱が加わった場合のブロ
ック図、第６図は静的な位置伝達誤差を補正する場合の
ブロック図、第７図は位置フィードバックが加えられた
ブロック図、第８図は第７図の入出力関係で等価変換し
てまとめたブロック図、第９図は位置伝達誤差に起因す
る振動を抑圧するために位置指令に加えるべき信号を示
すブロック図である。

Claims (3)

(57)【特許請求の範囲】
1. 【請求項１】サーボモータと、前記サーボモータの位置
   と速度の制御を行なう制御装置と、前記サーボモータに
   結合され、歯車のピッチ誤差に原因する入力と出力の角
   度の位置検出値の計測値に対応して算出される位置誤差
   が非線形特性を有する減速機と、前記減速機に結合され
   たロボットアームとからなるサーボ系に対し、前記減速
   機の入力と出力の角度の位置検出値の計測値に対応して
   算出される位置誤差の絶対値と、この絶対値の１回時間
   微分したものと、前記絶対値の２回時間微分したものと
   を重み付け加算した和を位置補正入力として、前記制御
   装置に与えられる位置指令に加算することを特徴とする
   サーボ制御装置。
2. 【請求項２】サーボ系の位置指令に対する減速機出力の
   応答を定める伝達関数を２次で近似した逆関数を、減速
   機の入力と出力の角度の位置検出値の計測値に対応して
   算出される位置誤差の絶対値に乗じたものを位置補正入
   力とする請求項１記載のサーボ制御装置。
3. 【請求項３】サーボ系の位置指令に対する減速機出力の
   応答を定める伝達関数を必要な次数で近似した逆関数
   を、減速機の入力と出力の角度の位置検出値の計測値に
   対応して算出される位置誤差の絶対値に乗じたものを位
   置補正入力とする請求項１記載のサーボ制御装置。