JP7338960B2 - サーボ制御装置及びサーボ制御方法 - Google Patents

Description

本発明は、モータに対して波動歯車装置を介して接続された制御対象の位置制御や軸制御を行なうサーボ制御装置及びサーボ制御方法に関する。

内周側に内歯を有する環状の内歯歯車と、内歯歯車の径方向内方に配置されて外周に内歯歯車の内歯と噛み合う外歯を有する可撓性の環状の外歯歯車と、外歯歯車の径方向内方に配置され回転することにより外歯歯車を径方向変形させる楕円状のカムとを組み合わせて構成される波動歯車装置は、大きな減速比が得られる、入力と出力が同軸である、小型である、などの特徴を有し、産業用ロボットの駆動軸などにおいて減速機として広く使用されている。波動歯車装置を用いた減速機のことを波動歯車減速機構と呼ぶ。波動歯車装置には各スプラインでの歯の加工誤差などに起因して波動歯車装置に特有の角度伝達誤差（回転伝達誤差ともいう）が存在することが知られている（例えば非特許文献１）。この角度伝達誤差の顕著な１次周波数成分は、出力軸の回転角位置が、入力軸の回転角位置から減速比によって決まる基準位置を中心にして周期的に変動することによる誤差である。周期変動する誤差であるので、例えば波動歯車減速機構を備えるロボットの場合であれば、角度伝達誤差によってアームの振動が引き起こされる。特に、ロボットアームを駆動しているときに角度伝達誤差の１次周波数成分とアームの機械的な共振周波数とが一致した場合には、アームが持続的に振動するようになる。

ロボットのサーボ制御では、通常、各アームを駆動するモータの回転位置を検出することによるセミクローズドループ制御を採用していており、サーボ制御系の性能を高めることで、外乱によるモータ軸の振動などが起こらないように補償している。しかしながら、波動歯車減速機構によって生じる外乱のようにモータ軸の後段において加わる周期性を有する外乱については単純にサーボ制御系の性能を高めるだけではその影響を除去することは難しく、ロボットアームの振動が収束しないことがある。

波動歯車装置の角度伝達誤差を補償する方法として、非特許文献２は、２慣性系モデルとして扱ってオブザーバを用いて補償する方法を開示しているが、この方法では制御器がかなり複雑になる。非特許文献３は、閉ル－プ内にノッチフィルタを挿入し、外乱として閉ループに加わるトルクを補償することにより角度伝達誤差による振動を抑制する方法を開示している。角度伝達誤差の各成分の周波数は入力軸の回転数に比例するから、この方法ではノッチフィルタの周期を可変とする必要があるが、ノッチフィルタの周期を可変とすることにより装置が複雑化することに比べて効果は大きくない。

特許文献１は、モータとモータに接続する波動歯車減速機構とを備えるアクチュエータの出力軸の位置決め精度を高める方法として、モータの回転軸の位置データをフィードバックして制御を行なう際に、モータの回転軸の位置と角度伝達誤差との関係を予め実測して蓄積した補正テーブルを参照して位置のフィードバック量に対して角度伝達誤差に関する補償を行うことを開示している。特許文献２は、特許文献１に記載された方法において、速度のフィードバック量に対しても補正テーブルを参照して補償を行うことを開示している。

特開２００２－１７５１２０号公報 特開２００８－９０６９２号公報

石田 武，他，"波動歯車装置の回転伝達誤差に関する研究（実験による考察）"，日本機械学会論文誌（Ｃ編），1995年，第61巻，第592号，p. 202-210 宮崎 敏昌，他，"ギアの角度伝達誤差を考慮したロボットアームのロバスト速度制御系の一構成法"，電気学会論文誌Ｄ，1998年，第118巻，第12号，p. 1427-1434 岩崎 誠，他，"波動歯車の角度伝達誤差に起因する共振振動の可変ノッチフィルタによる制振制御"，精密工学会誌，2012年，第78巻，第10号，p. 887-893

波動歯車装置の角度伝達誤差を補償してサーボ制御を行なう方法として、非特許文献２，３に記載される方法は、装置規模が大きくなる、あるいは装置規模に比べて効果が限定的である、という課題を有する。位置フィードバック量に対してモータの回転位置に応じた補正を行う特許文献１，２による方法も、システムのダイナミクスの影響により効果は限定的なものとなる。

本発明の目的は、装置規模を大きくすることなく、セミクローズドループ制御により、波動歯車装置の角度伝達誤差を効果的に補償することができるサーボ制御装置及びサーボ制御方法を提供することにある。

本発明のサーボ制御装置は、モータに対して波動歯車減速機構を介して動作対象物が接続しているときに位置指令に基づいてモータを制御するサーボ制御装置において、モータの軸角位置を検出し、軸角位置に基づいて閉ループ制御を行う位置制御系と、位置制御系のダイナミクスの逆特性を有する逆制御要素と、位置指令に基づいて波動歯車減速機構の角度伝達誤差を補償する補償量を発生する補償量発生部と、を備え、位置制御系が離散時間系で構成されており、逆制御要素に遅延要素が追加されており、補償量によって位置指令に対する補償を行い、補償後の位置指令が逆制御要素を介して位置制御系に印加されることを特徴とする。

本発明のサーボ制御装置では、位置制御系のダイナミクスの逆特性を有する逆制御要素と、波動歯車減速機構の角度伝達誤差を補償する補償量を発生する補償量発生部と、を設け、角度伝達誤差の補償量が逆制御要素を介して位置制御系に印加されるようにすることにより、後述の説明からも明らかになるように、モータの軸角位置から波動歯車減速機構の出力角位置までの伝達関数は、波動歯車減速機構の減速比をＮとすれば１／Ｎとなり、すなわち減速比Ｎだけで記述されるものとなり、波動歯車減速機構の角度伝達誤差を正確に補償することができる。

本発明のサーボ制御装置においては、位置制御系を離散時間系で構成し、逆制御要素に遅延要素を追加している。位置制御系を離散時間系で構成することにより、マイクロプロセッサなどによるデジタル制御が可能になり、逆制御要素に遅延要素を追加することで、逆制御要素を実現可能（プロパー）な関数で確実に表されるものとすることができる。

本発明のサーボ制御装置においては、位置制御系においてモデルマッチング制御による閉ループ制御を実行することが好ましい。モデルマッチング制御を用いることにより、位置制御系とそれに対応する逆制御要素とを同時に安定して構築することが可能になり、またミスマッチや外乱に対するロバスト性を保持しながら指令応答性を希望特性に調整することが容易になり、かつ指令応答性とロバスト性とを独立に調整できるようになる。そのようなモデルマッチング制御の１つとして、閉ループ制御における２つの極を外乱ロバストに配置するロバスト極配置に基づくものがある。ロバスト極配置に基づくものでは、外乱ロバストに極を配置することにより、指令応答性と外乱ロバスト性とを別々に調整できるようになる。位置制御系において好ましく利用することができるモデルマッチング制御以外の閉ループ制御の例として、制御対象に対応するモデルを構築し、指令をモデルに入力して得られる結果に対して実際の制御対象の出力が追従するように制御を行うモデルフォロイング制御に基づくものがある。モデルフォロイング制御を用いることにより、希望する応答が得られる制御系を構築しながら、ロバスト性も確保することができるようになる。

本発明のサーボ制御装置においては、位置制御系においてモデルマッチング制御やモデルフォロイング制御による手法以外を用いて閉ループ制御を行うこともできる。例えば、位置制御系において、Ｐ－ＰＩ制御による閉ループ制御を実行してもよい。Ｐ－ＰＩ制御を用いる場合には、「全体の制御系＝閉ループ制御系」を把握した逆制御要素を用いることで波動歯車減速機構の角度伝達誤差を補償することができる。あるいは位置制御系において、外乱オブザーバ補償制御による閉ループ制御を実行してもよい。外乱オブザーバ補償制御を用いる場合も、「全体の制御系＝閉ループ制御系」を把握した逆制御要素を用いることで波動歯車減速機構の角度伝達誤差を補償することができる。

本発明のサーボ制御装置において、補償量発生部は、モータの軸角位置と角度伝達誤差との関係を予め実測して角度伝達誤差の１次周波数成分を正弦関数で近似する機能を備え、得られた正弦関数に基づいて補償量を発生する。角度伝達誤差を近似する正弦関数を用いることにより、正弦関数をひとたび求めた後は、正弦関数を特定する少数のパラメータを記憶するだけでよいので、補正テーブルを用いて補償量を発生する場合に比べてメモリの消費量を減らすことができる。

本発明のサーボ制御方法は、モータに対して波動歯車減速機構を介して動作対象物が接続しているときに位置指令に基づいてモータを制御するサーボ制御方法において、モータの軸角位置を検出し、位置制御系において軸角位置に基づいて閉ループ制御を実行する工程と、位置指令に基づいて波動歯車減速機構の角度伝達誤差を補償する補償量を発生する工程と、補償量によって位置指令に対する補償を行い、補償後の位置指令に対し位置制御系のダイナミクスの逆特性を作用させる工程と、逆特性を作用させた位置指令を位置制御系に印加する工程と、を有し、離散時間系において閉ループ制御を実行し、逆特性には遅延要素が追加されていることを特徴とする。

本発明のサーボ制御方法では、波動歯車減速機構の角度伝達誤差を補償する補償量を発生し、位置制御系のダイナミクスの逆特性を作用させてからこの補償量を位置制御系に印加することにより、モータの軸角位置から波動歯車減速機構の出力角位置までの伝達関数は波動歯車減速機構の減速比だけで記述されるものとなり、波動歯車減速機構の角度伝達誤差を正確に補償することができる。

本発明のサーボ制御方法においては、離散時間系において閉ループ制御を実行し、前記逆特性には遅延要素が追加されている。位置制御系を離散時間系で構成することにより、マイクロプロセッサなどによるデジタル制御が可能になり、逆特性に遅延要素を追加することで、逆特性を確実に実現可能（プロパー）な関数で表されるものとすることができる。

本発明のサーボ制御方法においては、モータの軸角位置と角度伝達誤差との関係を予め実測して得られた角度伝達誤差の１次周波数成分を近似する正弦関数により補償量を発生させる。角度伝達誤差を近似する正弦関数を用いることにより、正弦関数をひとたび求めた後は、正弦関数を特定する少数のパラメータを記憶するだけでよいので、補償量の発生に補正テーブルを用いる場合に比べてメモリの消費量を減らすことができる。

このように本発明によれば、装置規模を大きくすることなく、セミクローズドループ制御により、波動歯車装置の角度伝達誤差を効果的に補償することができるようになる。

本発明の実施の一形態のサーボ制御システムを示すブロック図である。 波動歯車減速機構を有するシステムの角度伝達誤差のモデルを示すブロック線図である。 フィードバック量を補正することによる補償方法を説明するブロック線図である。 原理的に角度伝達誤差を補償することができるシステムのブロック線図である。 本発明の実施の一形態のサーボ制御装置における角度伝達誤差の補償方法を説明するブロック線図である。 図５に示す補償方法を離散化した例を示すブロック線図である。 別の実施形態のサーボ制御装置における角度伝達誤差の補償方法を説明するブロック線図である。 図７に示す補償方法を離散化した例を示すブロック線図である。 １次制御器によるロバスト極配置手法によるロバストモデルマッチングを説明するブロック線図である。 ２次制御器によるロバスト極配置手法によるロバストモデルマッチングを説明するブロック線図である。 モデルフォロイング制御を説明するブロック線図である。

次に、本発明の好ましい実施の形態について、図面を参照して説明する。図１は、本発明の実施の一形態のサーボ制御システムの構成を示している。

本実施形態のサーボ制御システムは、動作対象物４を駆動するモータ２と、モータ２を制御するサーボ制御装置１とを備えている。動作対象物４は、波動歯車減速機構５を介してモータ２に接続している。モータ２は、一例としてＡＣサーボモータあるいはＤＣサーボモータであり、負荷である動作対象物４を駆動する。モータ２には、波動歯車減速機構５からは独立してモータ２の回転位置を検出する、エンコーダなどの位置検出機構３が取り付けられている。サーボ制御装置１は、外部から与えられる位置指令と位置検出機構３からの検出信号とに基づいて閉ループ系による制御を実行し、モータ２を駆動する。

図２は、図１に示すサーボ制御システムを示すブロック線図であり、波動歯車減速機構５の角度伝達誤差のモデルを表している。図においてＴ（ｓ）は、サーボ制御装置１、モータ２及び位置検出機構３からなり閉ループ制御を行う位置制御系１１の伝達関数を示しており、モータ２に対する位置指令を入力してモータ２の軸の角位置を出力する。モータ２の軸の角位置は、波動歯車減速機構５の入力θ_inとなるものである。波動歯車減速機構５は、その入力を減速比Ｎで減速する理想的な減速機要素１２に対して並列に、Ｆ［θ_in］で表される角度伝達誤差を発生する角度伝達誤差要素１３が設けられた構成であり、減速機要素１２の出力に角度伝達誤差を加算したものをその出力角位置として出力する。角度伝達誤差は、減速機要素１２の出力にする周期的な外乱として扱うことができるから、外乱としての角度伝達誤差をｄ_dで表すことにする。

図３は、位置検出機構３で検出したモータ２の回転位置ｘをフィードバックすることによってモータ２を制御する位置制御系１１が設けられている場合において、特許文献１に示すようにフィードバック量に対して角度伝達誤差に基づく補償を加えることによって、波動歯車減速機構５の出力ｙにおける角度伝達誤差の影響を相殺する構成のブロック線図を示している。位置制御系１１にはモータ２も含まれているものとする。モータ２に対する位置指令はｖで表されており、モータ２を含めたループゲイン要素９１の伝達関数がＧ（ｓ）で表され、モータ２の出力位置すなわち軸角位置ｘが波動歯車減速機構５に入力する。モータ２の軸角位置ｘが位置制御系１１内でフィードバックされて位置指令ｖから減算され、この減算結果がループゲイン要素９１に入力するが、軸角位置ｘのフィードバック経路に対し、軸角位置ｘに対応して角度伝達誤差を補償するための補償量を発生する補償量発生部９２が並列に設けられ、軸角位置ｘに補償量を加算したものがループゲイン要素９１にフィードバックされるようになっている。補償量発生部９２は、モータ２の軸角位置ｘと角度伝達誤差Ｆ［θ_in］との関係を予め実測して蓄積した補正テーブルによるものであってよいし、近似関数によるものであってよいが、軸角位置ｘに応じた角度伝達誤差を減速比倍した補償量、すなわち角度伝達誤差を（Ｎ／１）倍した補償量（Ｎ／１）×ｄ_dを発生する。

図３に示すシステムにおいて波動歯車減速機構５の出力角位置ｙは、
ｙ＝（１／Ｎ）×｛Ｔ（ｓ）ｖ－Ｔ（ｓ）×（Ｎ／１）×ｄ_d｝＋ｄ_d
＝（１／Ｎ）×ｘ－（１／Ｎ）×（Ｎ／１）×Ｔ（ｓ）×ｄ_d＋ｄ_d
＝（１／Ｎ）×ｘ－Ｔ（ｓ）×ｄ_d＋ｄ_d
と表される。（１－Ｔ（ｓ））が恒常的に０であればｙ＝（１／Ｎ）×ｘとなるが、システムのダイナミクスにより、１－Ｔ（ｓ）が恒常的に０になるとは言えない。したがって、図３に示すシステムは、波動歯車減速機構５の出力角位置に角度伝達誤差の影響が残ることになる。その原因は、伝達関数Ｔ（ｓ）で表される位置制御系１１の内部におけるフィードバック量に対して補償を行なったことにある。

図４は、波動歯車減速機構５の出力における角度伝達誤差の影響を原理的に除くことができるシステムのブロック線図である。波動歯車減速装置５の減速比がＮであるとして、波動歯車減速機構５への入力θ_inに基づいて補償量を発生する補償量発生要素９３を設け、位置制御系１１の出力すなわちモータ２の軸角位置ｘから補償量を減算したものが波動歯車減速機構５に入力するようにしている。補償量発生要素９３は、（Ｎ／１）×Ｆ［θ_in］、すなわち（Ｎ／１）×ｄ_dで表される補償量を発生する。このシステムでは、波動歯車減速機構５の出力角位置ｙは、
ｙ＝（１／Ｎ）×｛ｘ－（Ｎ／１）×ｄ_d｝＋ｄ_d
＝（１／Ｎ）×ｘ－（１／Ｎ）×（Ｎ／１）×ｄ_d＋ｄ_d
＝（１／Ｎ）×ｘ
となる。すなわち、波動歯車減速機構５の出力角位置ｙには、角度伝達誤差ｄ_dの影響は現れない。

しかしながら図４に示すシステムは、波動歯車減速機構５の入力となる軸角度に対して機械的に補償を行おうとするものであり、現実的には実現することができないものである。そこで本発明では、図４に示すシステムを近似するシステムを構築することによって、サーボ制御装置１において角度伝達誤差の補償を行う。図５は本発明の実施の一形態のサーボ制御装置における角度伝達誤差の補償方法を説明するブロック線図である。図５では、モータ２の軸角位置ｘを閉ループ制御により制御する位置制御系２１の伝達関数がＴ（ｓ）で表されている。位置制御系２１はモータ２の出力軸も含んでいるものとし、したがって、図１に示す位置検出機構３なども位置制御系２１に含まれている。そして図５に示すシステムでは、位置制御系２１の外部に角度伝達誤差補償用のフィードバックループすなわち補償ループを設け、この補償ループ内において補償量発生部２３を設けるとともに位置制御系２１の逆システム２２を生成し、この逆システム２２と補償量発生部２３とを用いて角度伝達誤差を補償する。モータ２の軸角位置ｘは波動歯車減速機構５の入力となるとともに、補償ループを介して補償量発生部２３に与えられる。補償量発生部２３は、角度伝達誤差のモデルであって、軸角位置ｘに応じた角度伝達誤差を減速比倍した補償量（Ｎ／１）×ｄ_dを発生する。補償量発生部２３は、モータ２の軸角位置ｘと角度伝達誤差Ｆ［θ_in］との関係を予め実測して蓄積した補正テーブルによるものであってもよいし、角度伝達誤差を近似する関数を発生する関数発生器によるものであってもよいが、波動歯車減速機構５ごとに角度伝達誤差の特性が異なることから、実測に基づく補正テーブルを用いるものの方が好ましい。角度伝達誤差を近似する関数の例として、三角関数、例えば正弦関数が挙げられる。近似関数によって補償量を発生する場合には、モータ２の軸角位置ｘと角度伝達誤差Ｆ［θ_in］との関係を予め実測して角度伝達誤差［θ_in］の１次周波数成分を求め、この１次周波数成分を近似する正弦関数を求める。そしてこの正弦関数に基づいて補償量を発生する。この場合、補償量発生部２３は、実測したデータを近似する正弦関数を求める近似機能を備えていることが好ましい。補償量発生部２３が発生した補償量（Ｎ／１）×ｄ_dは逆システム２２に入力される。モータ２に対する位置指令ｖは、減算器２４において、逆システム２２の出力が減算されて位置制御系２１に入力される。

図５に示すシステムにおいて波動歯車減速機構５の出力角位置ｙは、
ｙ＝（１／Ｎ）×｛Ｔ（ｓ）ｖ－Ｔ（ｓ）Ｔ^-1（ｓ）×（Ｎ／１）×ｄ_d｝＋ｄ_d
＝（１／Ｎ）×ｘ－（１／Ｎ）×（Ｎ／１）Ｔ（ｓ）Ｔ^-1（ｓ）×ｄ_d＋ｄ_d
＝（１／Ｎ）×ｘ－Ｔ（ｓ）Ｔ^-1（ｓ）×ｄ_d＋ｄ_d
＝（１／Ｎ）×ｘ
と表される。すなわち、波動歯車減速機構５の出力では、ダイナミクスの影響が除去されて、波動歯車減速機構の出力各位置ｙには角度伝達誤差の影響が現れないことになる。

図５に示すシステムでは、位置制御系２１からモータ２と位置検出機構３とを除いた部分と、逆システム２２、補償量発生部２３及び減算器２４とが図１に示す構成におけるサーボ制御装置１に構築されることになる。

多くの場合、逆システム２２は非プロパーとなってそのままでは実現できない。逆システム２２が非プロパーの場合には、適切な遅延を逆システム２２の伝達関数Ｔ^-1（ｓ）に加えてプロパーとし、その遅延を補償するために、補償量発生部２３の入力に対して進相処理を行う。進相処理あるいは進相補償は将来の値に関する推定を含むが、補償量発生部２３の入力はモータ２の軸角位置ｘであってその出力である補償信号（誤差信号）に比べて時間変化が小さいため、十分な精度で将来の値を推定可能である。あるいは、位置制御系２１の応答速度に比べてモータ２の軸角位置ｘの変化が十分に遅いというのであれば、進相処理を行わないこととして逆システム２２の伝達関数Ｔ^-1（ｓ）に対して遅延を加えるだけでも角度伝達誤差の影響を実質的に除去することができる。

図５に示したシステムに基づいてサーボ制御を行なう手法は、閉ループ制御を行う位置制御系２１が変動せず、ある程度は正しくモデル化できることを前提としている。より具体的にはこの方法は、システムＴ（ｓ）と逆システムＴ^-1（ｓ）とを同時に安定して構築できることが必要であり、そのため、明確にモデル化された、すなわちモデルマッチング制御系とされた位置制御系２１を構築することが好ましい。したがってこの方法は、制御系を構成する各要素の試行錯誤によるゲイン調整を主体とする古典的な制御手法によっては、逆システムを求めることが難しいので、安定して実現することが難しい。もっともこのことは、Ｐ（比例）－ＰＩ（比例積分）制御によって閉ループ制御を行う位置制御系２１の利用を否定するものではなく、閉ループ制御がＰ－ＰＩ制御に基づくものであってもよいが、そのＰ－ＰＩ制御はモデル化されたものであることが好ましいことを意味している。モデルマッチング制御系とすることにより位置制御系２１は、変動が少ない明確なモデルにマッチングされるよう制御されるため、この特長を用いて逆システムを構築してフィードバック補償することが可能になる。このように逆システムを構築する場合には遅延が生じるので、その遅延を補償するために、補償量発生部２３に対する入力信号（すなわち位置制御系２１の出力ｘ）を進相補償する。進相補償には、入力信号をその疑似微分（差分）を使って推定補償する方法などがある。

マイクロプロセッサなどによってサーボ制御装置１を構成しデジタル制御を行なう場合であれば、伝達関数Ｔ（ｓ）やＴ^-1（ｓ）の代わりに、進み演算子ｚを用いた離散化伝達関数Ｔ（ｚ）やＴ^-1（ｚ）を用いてシステムを記述することになる。特に、上述したようにモデルマッチング制御系を用いる場合には、実用上、デジタル制御での実装が不可欠となる。図６は、図５に示すシステムを離散化伝達関数によって記述したものの一例である。以下、図６に示すシステムについて説明する。

図５に示すシステムの位置制御系２１が時間に関して離散化されて下記のような２次モデルにモデルマッチング制御されるものとする。ａ₀及びａ₁はモデルパラメータである。

この離散化された位置制御系モデルの逆システムすなわち逆モデルは、

であるが、これは非プロパーであってこのままでは実現不可能な関数となっている。そこで２階の遅延ｚ^-2を追加して、

とすれば、実現可能な逆モデルとなる。しかしながら、２階の遅延ｚ^-2が追加されているので、より正確な制御を実現するために、補償ループに設けられる逆モデルへの入力信号の位相を２階進ませて２階先の入力信号を推定することを考える。位相を１階進めることは、対象とする信号の擬似微分（すなわち制御周期の１周期での信号値の差分）を信号値に加算することによって実現でき、２階進めることは２周期分加算することで実現できる。逆モデルの入力側には角度伝達誤差のモデルが配置しており、角度伝達誤差のモデルからの出力信号の位相を２階進めることと、角度伝達誤差のモデルへの入力信号の位相を２階進めることとは等価であるが、角度伝達誤差のモデルからの出力信号の時間変化に比べて角度伝達誤差のモデルへの入力信号の方が、この入力信号がモータ２の軸角位置ｘという位置信号であることから時間変化がゆっくりしているため、角度伝達誤差のモデルへの入力信号を擬似微分する方が誤差を小さくすることができる。逆モデル化で生じた遅延分を補償ループ内において進相処理により補償することにより、ダイナミクス等による遅延がない状態で角度伝達誤差に対する補償を行うことができるので、角度伝達誤差の理想的な補償を行うことができる。一般にモータのデジタル制御において、モータの軸角位置は、制御サンプリング周期内では十分にゆっくりと変化するから、図６に示すシステムにおいて、モータ２の軸角位置の差分から、２階先の位置を推定することは容易である。

したがって、図６に示すシステムは、伝達関数がＴ（ｚ）で表されるモデルマッチング制御系で構築されて閉ループ制御を行う位置制御系３１を備え、位置制御系３１の出力信号（モータ２の軸角位置）ｘが波動歯車減速機構５に入力する。さらにこのシステムは、位置制御系３１の逆モデル３２と、角度伝達誤差のモデルである補償量発生部３３と、モータ２に対する位置指令ｖから逆モデル３２の出力を減算して減算結果を位置制御系３１に出力する減算器３４と、２階の進相補償（ｚ²）を実行する進み補償器３５と、を備えている。上述したように逆モデルの離散化伝達関数は、ｚ^-2Ｔ^-1（ｚ）で表される。モータ２の軸角位置ｘが進み補償器３５に供給され、進み補償器３５の出力が補償量発生部３３に供給され、補償量発生部３３で発生した補償量が逆モデル３２に入力する。補償量発生部３３としては、補正テーブルあるいは関数発生器などによって構成され、デジタル値であるモータ２の出力軸角位置の値に対応して、角度伝達誤差の減速比倍の値を出力するものが用いられる。

次に、本発明の別の実施形態のサーボ制御方法として、図５を用いて説明したサーボ制御方法とは異なるものを説明する。図５に示したブロック線図において位置制御系２１を減算器２４の位置よりもさらに左に配置したとすると、位置制御系２１は、補償ループ内では逆モデルによる補償と進相補償とによって相殺される。そのことを踏まえると、図４に示す理想的ではあるが実現が難しいシステムから図７に示すシステムを得ることができる。図７に示すシステムによっても本発明に基づいて角度伝達誤差の補償を実現することができる。

図７に示すシステムでは、モータ２に対する位置指令ｖが上述したものと同様の補償量発生部４３に供給され、減算器４４において、位置指令ｖから補償量発生部４３の出力が減算される。そして減算器４４の減算結果が逆システム４２に供給され、逆システム４２の出力が、閉ループ制御による位置制御系４１に入力する。位置制御系４１の出力はモータ２の軸角位置ｘであって、波動歯車減速機構５に入力する。位置制御系４１の伝達関数はＴ（ｓ）で表され、逆システム４２の伝達関数はＴ^-1（ｓ）で表されている。逆システム４２と位置制御系４１とによって位置制御ブロック４０が構成されている。

図５及び図７から分かるように、本発明に基づくサーボ制御方法を実現するための最低限の構成は、モータ２の検出された軸角位置ｘに基づいて閉ループ制御を行う位置制御系と、位置制御系のダイナミクスの逆特性を有する逆制御要素（逆システムあるいは逆モデル）と、波動歯車減速機構５の角度伝達誤差を補償する補償量を発生する補償量発生部と、を備え、角度伝達誤差の補償量が逆制御要素を介して位置制御系に印加されるようにしたものである。

図８は、デジタル制御のために図７に示すシステムを離散時間系としたシステムを示している。図８に示すシステムは、図７に示すシステムでの位置制御ブロック４０、位置制御系４１、逆モデル４２、補償量発生部４３及び減算器４４をそれぞれ、位置制御ブロック５０、モデルマッチング制御系で構成された位置制御系５１、位置制御系５１に対応する逆モデル５２、補償量発生部５３及び減算器５４で置き換えたものである。位置制御系４１及び逆モデル４２の伝達関数は、それぞれ、Ｔ（ｚ）及びｚ^-2Ｔ^-1（ｚ）で表される。位置制御系５１、逆モデル５２及び補償量発生部５３は、それぞれ、図６に示すシステムにおける位置制御系３１、逆モデル３２及び補償量発生部３３と同様のものである。図８に示すシステムにおいても、位置制御ブロック５０に対し仮想的に群遅延のみのデッドビート制御を行なっている。

次に、位置制御系２１，３１，４１，５１のモデルマッチング制御について説明する。モータ２から波動歯車減速機構５を介して駆動される動作対象物４が例えばロボットの関節である場合、角度伝達誤差以外にも干渉力などの種々の外乱が関節に加わり、ロボットの姿勢変化によってイナーシャも変化する。そこで位置制御系２１，３１，４１，５１をモデルマッチング制御系とするときは、明確なモデル化とロバスト性とを兼ね備えたロバストモデルマッチング制御系により構築することが好ましい。ここでロバストモデルマッチング制御系とは、モータ２及び動作対象物４を含んだフィードバック位置サーボ制御系を、変動やミスマッチ、外乱に対してロバストなモデルマッチング制御系としたものである。ロバストモデルマッチング制御系とすることにより、ミスマッチや外乱に対するロバスト性が向上するため、角度伝達誤差の補償効果もよりよいものとなる。

ロバストモデルマッチング制御系の実現手法としていくつかの手法が知られており、その１つとして、なんらかのモデルを仮定としてそのモデルにおいて外乱ロバストとなるように、閉ループ制御における２つの極の配置を決定するロバスト極配置法がある（伊藤 彰啓、他、“メカトロ機器におけるロバスト制御の動向とその簡易設計例”、精密工学会誌、2000年、第66巻、第5号、p. 811-815）。ロバスト極配置法では、制御器の次数に１次制御器を用いる方法と２次制御器を用いる方法などがある。図９は、１次制御器を使用するロバスト極配置法の一例を説明するブロック線図である。ここでは離散時間系を仮定しており、以下の説明において、δ＝ｚ－１であるものとする。外乱ｄ（ｋ）は制御対象の入力に加わるものとする。制御対象の伝達関数Ｐ（ｚ）はＰ（ｚ）＝ｒ₀ｚ／（δ₂＋ｐ₁δ）でモデル化されている。ｒ₀，ｐ₁はモデルパラメータである。制御部６０は、ＰＩ制御をモデル化したＰＩ制御部６１と、制御対象の出力位置ｙ（ｋ）の差分を求めることにより擬似速度ｖ₁（ｋ）を生成する差分要素６２と、擬似速度ｖ₁（ｋ）が入力する第１のゲイン要素６３と、擬似速度ｖ₁（ｋ）を遅延させる遅延要素６４と、遅延要素６４の出力が入力する第２のゲイン要素６５と、ローパスフィルタ６６と、を備えている。ローパスフィルタ６６には、第１のゲイン要素６３の出力と第２のゲイン要素の出力６５とを加算したものが入力する。位置指令ｒ（ｋ）から位置ｙ（ｋ）が減算されて位置偏差ｅ（ｋ）が生成し、位置偏差ｅ（ｋ）からローパスフィルタ６６の出力を減算したものがＰＩ制御部６１に入力する。

ＰＩ制御部６１の伝達関数Ｆ_a（ｚ）、第１のゲイン要素６３の伝達関数Ｈ₁、第２のゲイン要素６５の伝達関数Ｈ₂及びローパスフィルタ６６の伝達関数Ｆ_b(ｚ)は、それぞれ、
Ｆ_a（ｚ）＝（δ＋ｑ₀）ｍ₀／（ｒ₀δ），
Ｈ₁＝－（ｐ₁－ｍ₁＋ｍ₀－ｑ₀）／（ｍ₀ｑ₀），
Ｈ₂＝｛（ｍ₁－ｍ₀）／ｍ₀｝－Ｈ₁，
Ｆ_b(ｚ)＝ｑ₀ｚ／（δ＋ｑ₀）で表される。ここでｍ₀，ｍ₁もモデルパラメータであり、上述したモデルパラメータａ₀，ａ₁との関係は、ｍ₀＝ａ₀＋ａ₁＋１，ｍ₁＝ａ₁＋２である。外乱ｄ（ｋ）がゼロであるとすると、図９に示すブロック線図から、
ｙ（ｋ）＝Ｐ（ｚ）ｘ（ｋ），
ｖ₁（ｋ）＝（δ／ｚ）ｙ（ｋ）
が得られ、さらに、
ｖ₁（ｋ）＝［ｍ₀／（δ＋ｐ₁）］ｕ（ｋ）
が得られる。ローパスフィルタ６６の入力をｗ(ｋ)とおけば、

が成り立つ。ＰＩ制御部６１の出力であるｕ(ｋ)に関して、ｕ(ｋ)＝Ｆ_a(ｚ)｛ｅ(ｋ)－Ｆ_b(ｚ)ｗ(ｋ)｝が成り立つ。Ｆ_a(ｚ)とＦ_b(ｚ)との間には

の関係があることに注意すると

が得られ、さらに式の変形を続けると最終的には以下の式を得る。

この式は、図９に示すシステムにおいて、位置指令ｒ（ｋ）から位置検出ｙ（ｋ）に至る位置指令応答特性が（ｍ₀ｚ）／（δ₂＋ｍ₁δ＋ｍ₀）で表わされること、積分機能（外乱応答）をｑ₀によって調節できるが、この調節によってはこの位置指令応答特性が影響を受けないこと、すなわち、ロバスト性と指令応答特性とを別個に調整できることを示している。言い換えれば、ＰＩ制御部６１やローパスフィルタ６６において積分を示す因子であるｑ₀の値によらず、位置制御特性を特性多項式δ₂＋ｍ₁δ＋ｍ₀に一致させることができる。ｑ₀を極として、システムがロバストであるようにこの極ｑ₀を配置できる。

２次制御器を用いるロバスト極配置法は、例えば、特許第３２００４９６号明細書、特許第３８５０６２８号明細書及び第３９６２２１５号明細書に記載されている。一例として特許第３８５０６２８号明細書に開示されるロバスト極配置法では、制御対象の伝達関数が上記と同じくＰ（ｚ）＝ｒ₀ｚ／（δ₂＋ｐ₁δ）であり、安定特性多項式がｍ（δ）＝δ₂＋ｍ₁δ＋ｍ₀であるときに、補償対象の想定される外乱次数をｄとして、安定なオブザーバ多項式ｑ（δ）として
ｑ（δ）＝δⁿ＋ｑ_n-1δ^n-1＋…＋ｑ⁰，ｎ＝２＋ｄ
を選択する。そして、
ｋ（δ）＝ｋ_n-1δ^n-1＋…＋ｋ_d+1δ^d+1＋ｑ_dδ^d＋…＋ｑ₀，
ｈ（δ）＝ｈ_nδⁿ＋…＋ｈ₀
としたとき、ｋ_j＝ｑ_j，ｊ＝０～ｄとし、さらに、
ｋ（δ）ｒ₀＋ｈ（δ）｛δ²＋ｐ₁δ｝＝ｑ（δ）｛ｐ（δ）－ｍ（δ）｝
を満たすように、ｋ_i，ｉ＝ｄ＋１～ｎ－１及びｈ_i，ｉ＝０～ｎを選んで、
ｘ（ｋ）＝｛ｋ（δ）／ｑ（δ）｝ｘ（ｋ）＋｛ｈ（δ）／ｑ（δ）｝ｙ（ｋ）＋（ｍ₀／ｒ₀）ｘ₁（ｋ）
で表される制御入力を与える閉ループ制御系を構成し、さらに、その閉ループ制御系の２つの極、すなわち特性多項式ｍ（δ）の根に対し、各々Ｆ₁及びＦ₂により補償する割合を調整するｘ₁（ｋ）で表されるフィードフォワード補償器を設けている。ここで
ｘ₁（ｋ）＝Ｆ₂［｛ｚ^-1ｒ（ｋ）＋Ｆ₁（ｒ（ｋ）－ｚ^-1ｒ（ｋ））｝－ｚ^-1｛ｚ^-1ｒ（ｋ）＋Ｆ₁（ｒ（ｋ）－ｚ^-1ｒ（ｋ））｝＋ｚ^-1｛ｒ（ｋ）－ｚ^-1ｒ（ｋ）｝］＋ｚ^-1［ｚ^-1ｒ（ｋ）＋Ｆ₁｛ｒ（ｋ）－ｚ^-1ｒ（ｋ）｝］
であり、ｘ（ｋ）は制御対象への入力信号、ｙ（ｋ）は制御対象からの位置出力信号、ｋ（δ）は入力信号ｘ（ｋ）を入力するフィルタ伝達関数ｋ（δ）／ｑ（δ）の分子多項式、ｈ（δ）は位置出力信号ｙ（ｋ）を入力するフィルタ伝達関数ｈ（δ）／ｑ（δ）の分子多項式、ｘ₁（ｋ）は閉ループ制御系への位置指令、ｒ（ｋ）は全体の制御系への規範位置指令入力である。Ｆ₁及びＦ₂は、それぞれ、閉ループ制御系の第１の極及び第２の極に対するフィードフォワード補償係数である。ｐ（δ）は、制御対象の特性を表す伝達関数の分母多項式、ｒ₀は制御対象の伝達関数の分子項（係数）、ｐ₁は制御対象の伝達関数の分母多項式の係数である。ｑ（δ）は制御器に組み込まれる安定なオブザーバ特性多項式であって、ｑ₀はその０次項である。

図１０は、上述した２次制御器を用いるロバスト極配置法の一例を説明するブロック線図である。制御部７０には、規範位置指令入力ｒ（ｋ）を入力してフィードフォワード補償信号ｘ₁（ｋ）を出力するフィードフォワード補償器７１と、規範位置指令入力ｒ（ｋ）を入力とする部分に設けられたゲイン要素７２と、２個の制御器７３，７５を備えている。制御器７３は、入力信号ｘ（ｋ）を入力するフィルタ伝達関数ｋ（δ）／ｑ（δ）に対応するものであって伝達関数がＫ（ｚ）で表される。制御器７５は、出力信号ｙ（ｋ）を入力するフィルタ伝達関数ｈ（δ）／ｑ（δ）に対応するものであって伝達関数がＨ（ｚ）で表される。ゲイン要素７２の出力に対して制御器７３の出力と制御器７５の出力の和を加算し、さらにフィードバック補償器７１の出力ｘ₁（ｋ）を加算したものが制御対象への入力信号ｘ（ｋ）である。制御器７３にはｘ（ｋ）が入力し、制御器７５にはｙ（ｋ）が入力している。

２次制御器を用いるロバスト極配置法によれば、フィードフォワード補償により、ｍ（δ）の第１の極に対する補償をＦ₁で、第２の極に対する補償をＦ₂で独立して行うことができる。したがって、この性質を利用することにより、ミスマッチや外乱に対してロバストなモデルマッチング制御を実現することができる。

以上、１次制御器あるいは２次制御器を用いるロバスト極配置法について説明したが、ロバスト極配置法の実装形態は、上述したものに限定されず、他の形態も存在する。例えば、上述した構成における各ブロックをまとめたり分離したりすることができ、それらのブロックの挿入位置も、図９あるいは図１０に示すブロック線図におけるものと等価になるのであれば任意である。ロバスト極配置法を適用してロバストとしたモデルマッチング制御系により位置制御系を構築することにより、所定の帯域までは設定モデルの帯域特性を保持するようになる。高域では帯域特性を保持できなくなることがあるが、本発明が対象としている角度伝達誤差や外乱は比較的低域のものであるから、ロバスト極配置法の利用によって十分な性能を得ることができる。

ロバストモデルマッチング制御以外の制御の手法として、モデルフォロイング制御がある。図１１は、モデルフォロイング制御の一例を説明するブロック線図である。モデルフォロイング制御は、制御対象Ｐ（ｓ）に対応するモデルＭ（ｓ）を構築し、指令ｒ（ｓ）をモデルＭ（ｓ）に入力して得られる結果Ｍ（ｓ）ｒ（ｓ）に対して実際の制御対象Ｐ（ｓ）の出力ｙ（ｓ）が追従するように制御を行うものである。モデルフォロイング制御の制御部８０は、モデルＭ（ｓ）と、伝達関数がＭ（ｓ）Ｐ^-1（ｓ）で表されるフィードフォワード制御器８１と、伝達関数がＣ（ｓ）で表されるフィードバック制御器８２とを備えている。指令ｒ（ｓ）はモデルＭ（ｓ）とフィードフォワード制御器８１とに入力し、フィードバック制御器８２は、モデルＭ（ｓ）の出力から制御対象Ｐ（ｓ）の出力ｙ（ｓ）を減算したものが入力する。フィードフォワード制御器８１とフィードバック制御器８２の出力をそれぞれｕ_ff（ｓ），ｕ_fb（ｓ）とすると、制御部８０からはｕ_ff（ｓ）とｕ_fb（ｓ）との和であるｕ（ｓ）が出力し、これが制御対象Ｐ（ｓ）に入力する。このとき、外乱ｄ（ｓ）も入力に加わることを想定する。

モデルフォロイング制御の目標は、ｙ（ｓ）＝Ｍ（ｓ）ｒ（ｓ）とすることである。ここで外乱ｄ（ｓ）がないものとすれば、
ｕ_ff（ｓ）＝Ｍ（ｓ）Ｐ^-1（ｓ）ｒ（ｓ），
ｕ_fb（ｓ）＝Ｃ（ｓ）｛Ｍ（ｓ）ｒ（ｓ）－ｙ（ｓ）｝，
ｕ（ｓ）＝ｕ_ff（ｓ）＋ｕ_fb（ｓ），
ｙ（ｓ）＝Ｐ（ｓ）ｕ（ｓ）
であることから、
ｙ（ｓ）＝Ｐ（ｓ）｛Ｍ（ｓ）Ｐ^-1（ｓ）ｒ（ｓ）＋Ｃ（ｓ）［Ｍ（ｓ）ｒ（ｓ）－ｙ（ｓ）］｝
が得られ、これからｙ（ｓ）＝Ｍ（ｓ）ｒ（ｓ）が得られるから、図１１に示すシステムにおいて制御対象Ｐ（ｓ）がモデルＭ（ｓ）に追従することが分かる。そしてｒ（ｓ）＝０として外乱ｄ（ｓ）が加わった場合を考えると、
ｙ（ｓ）＝Ｐ（ｓ）ｄ（ｓ）－Ｐ（ｓ）Ｃ（ｓ）ｙ（ｓ）
が成り立つ。これより、
ｙ（ｓ）＝［Ｐ（ｓ）／｛１＋Ｐ（ｓ）Ｃ（ｓ）｝］ｄ（ｓ）
が得られる。上記式より、Ｐ（ｓ）／｛１＋Ｐ（ｓ）Ｃ（ｓ）｝の特性多項式が安定であり、かつその分子成分に外乱ｄ（ｓ）の分母成分を持っていれば、外乱ｄ（ｓ）の影響は０に収束する。すなわち、図１１に示すモデルフォロイング制御において、フィードバック制御器８２の伝達関数Ｃ（ｓ）に外乱ｄ（ｓ）のモデルを含ませることで、外乱ｄ（ｓ）に対するロバスト性を付与することができる。

さらに本発明では、位置制御系を構築するためのロバストなモデルマッチング制御以外の手法として、例えば、ミスマッチや外乱にロバストであるように構築したＰ－ＰＩ制御を行い、システム全体を把握する手法や、ミスマッチや外乱にロバストであるように外乱オブザーバ補償制御（例えば、大西 公平、“外乱オブサーバによるロバスト・モーションコントロール、日本ロボット学会誌、1993年、第11巻、第4号、p. 486-493を参照）を行い、システム全体を把握する手法などを用いることができる。Ｐ－ＰＩ制御によるものでも外乱オブザーバ補償制御によるものでも位置制御系が正しくモデル化できれば、位置制御系の逆モデルを構築することは可能である。

上述した実施形態のサーボ制御装置では、モータの軸角位置を検出して軸角位置に基づいて閉ループ制御を行う位置制御系が設けられているときに、位置制御系のダイナミクスの逆特性を有する逆制御要素すなわち逆システムまたは逆モデルと、例えば補正テーブルを有し波動歯車減速機構の角度伝達誤差を補償する補償量を発生する補償量発生部とを設け、角度伝達誤差の補償量が逆制御要素を介して位置制御系に印加されるようにするという簡素な構成により、高い精度で波動歯車装置の角度伝達誤差を効果的に補償することができる。

１…サーボ制御装置、２…モータ、３…位置検出機構、４…動作対象物、５…波動歯車減速機構、１１，２１，３１，４１，５１…位置制御系、１３…角度伝達誤差要素、２２，４２…逆システム、２３，３３，４３，５３…補償量発生部、３２，５２…逆モデル。

Claims (7)

1. モータに対して波動歯車減速機構を介して動作対象物が接続しているときに位置指令に基づいて前記モータを制御するサーボ制御装置において、
   前記モータの軸角位置を検出し、前記軸角位置に基づいて閉ループ制御を行う位置制御系と、
   前記位置制御系のダイナミクスの逆特性を有する逆制御要素と、
   前記位置指令に基づいて前記波動歯車減速機構の角度伝達誤差を補償する補償量を発生する補償量発生部と、
   を備え、
   前記位置制御系が離散時間系で構成されており、前記逆制御要素に遅延要素が追加されており、
   前記補償量によって前記位置指令に対する補償を行い、補償後の前記位置指令が前記逆制御要素を介して前記位置制御系に印加され、
   前記補償量発生部は、前記モータの軸角位置と前記角度伝達誤差との関係を予め実測することにより前記角度伝達誤差の１次周波数成分を正弦関数で近似する機能を備え、前記正弦関数に基づいて前記補償量を発生することを特徴とする、サーボ制御装置。
2. 前記位置制御系はモデルマッチング制御による閉ループ制御を実行する、請求項１に記載のサーボ制御装置。
3. 前記モデルマッチング制御は、前記閉ループ制御における２つの極を外乱ロバストに配置するロバスト極配置に基づいたものである、請求項２に記載のサーボ制御装置。
4. 前記位置制御系は、制御対象に対応するモデルを構築し、指令をモデルに入力して得られる結果に対して実際の前記制御対象の出力が追従するように制御を行うモデルフォロイング制御に基づいた制御を実行する、請求項１に記載のサーボ制御装置。
5. 前記位置制御系は、Ｐ－ＰＩ制御による閉ループ制御を実行する、請求項１に記載のサーボ制御装置。
6. 前記位置制御系は、外乱オブザーバ補償制御による閉ループ制御を実行する、請求項１に記載のサーボ制御装置。
7. モータに対して波動歯車減速機構を介して動作対象物が接続しているときに位置指令に基づいて前記モータを制御するサーボ制御方法において、
   前記モータの軸角位置を検出し、位置制御系において前記軸角位置に基づいて閉ループ制御を実行する工程と、
   前記位置指令に基づいて前記波動歯車減速機構の角度伝達誤差を補償する補償量を発生する工程と、
   前記補償量によって前記位置指令に対する補償を行い、補償後の前記位置指令に対し前記位置制御系のダイナミクスの逆特性を作用させる工程と、
   前記逆特性を作用させた前記位置指令を前記位置制御系に印加する工程と、
   を有し、
   離散時間系において前記閉ループ制御を実行し、前記逆特性には遅延要素が追加されており、
   前記補償量を発生する工程は、前記モータの軸角位置と前記角度伝達誤差との関係を予め実測して得られた前記角度伝達誤差の１次周波数成分を近似する正弦関数により前記補償量を発生する工程であることを特徴とする、サーボ制御方法。

Images