(57)【要約】【課題】安全性等を考慮しつつも、設計者が意図した通りのフィルタ特性を得ることができるロボットの制御装置を提供する。【解決手段】ロボットの制御装置は、移動体の位置を指令する位置指令を生成する位置指令生成部２０と、位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の目標値となる位置指令軌道を生成する位置指令フィルタ２１（位置指令軌道生成部）と、位置指令軌道に基づいて移動体を制御するモータ制御部２２（制御部）と、を備え、位置指令フィルタ２１のフィルタは、位置指令軌道により定められる出力速度の限度値を位置指令により定められる入力速度の限度値により制約するように設計されている。【選択図】図２
2016043417.tif 000002
【特許請求の範囲】
【請求項１】
移動体の移動目標位置を指令する位置指令を生成する位置指令生成部と、
前記位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の目標値となる位置指令軌道を生成する位置指令軌道生成部と、
前記位置指令軌道に基づいて移動体を制御する制御部と、を備え、
前記位置指令軌道生成部の前記フィルタは、前記位置指令軌道により定められる出力速度の限度値を、前記位置指令により定められる入力速度の限度値により制約するように設計されていることを特徴とするロボットの制御装置。
【請求項２】
移動体の移動目標位置を指令する位置指令を生成する位置指令生成部と、
前記位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の目標値となる位置指令軌道を生成する位置指令生成部と、
前記位置指令軌道に基づいて移動体を制御する制御部と、を備え、
前記位置指令軌道生成部の前記フィルタは、前記位置指令軌道により定められる出力加速度の限度値を、前記位置指令により定められる入力加速度の限度値により制約するように設計されていることを特徴とするロボットの制御装置。
【請求項３】
前記位置指令軌道生成部の前記フィルタは、前記位置指令軌道により定められる出力加速度の限度値を、前記位置指令により定められる入力加速度の限度値により制約するように設計されていることを特徴とする請求項１記載のロボットの制御装置。
【請求項４】
前記位置指令軌道生成部の前記フィルタは、特定の周波数成分を低減する有限インパルス応答フィルタで構成されており、当該有限インパルス応答フィルタの係数を全て０以上に設定することで、前記位置指令の特定の周波数成分を低減しつつ、前記限度値による制約を実現することを特徴とする請求項１から３のいずれか一項記載のロボットの制御装置。
【請求項５】
前記位置指令生成部は、特定の周波数成分を低減するｎ次の有限インパルス応答フィルタで構成されており、当該有限インパルス応答フィルタの係数をｍ＝ｎ／２を中心として対称に設定することで、前記位置指令の特定の周波数成分を低減しつつ、零位相差特性を実現することを特徴とする請求項１から４のいずれか一項記載のロボットの制御装置。
【請求項６】
前記有限インパルス応答フィルタは、そのフィルタ特性を決定する際の最適化パラメータの一つとしてフィルタ係数の対称性を設定可能であり、当該フィルタ係数の対称性を設定することにより擬似的な零位相差特性を実現することを特徴とする請求項１から４のいずれか一項記載のロボットの制御装置。
【請求項７】
前記位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の入力であって、前記位置指令軌道に追従させるフィードフォワード制御を行うための制御入力を生成する制御入力生成部を備えることを特徴とする請求項１から６のいずれか一項記載のロボットの制御装置。
【請求項８】
前記制御入力生成部の前記フィルタは、前記制御入力により定められる出力トルクの限度値を、前記位置指令により定められる入力トルクの限度値により制約するように設計されていることを特徴とする請求項７記載のロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの制御装置に関する。
【背景技術】
【０００２】
ロボットの制御装置は、移動体であるアームの移動目標位置を指令するための位置指令に対してフィルタ処理を行い、移動体を制御するための目標値となる位置指令軌道を生成して、ロボットを制御している。このフィルタ処理では、動作時の振動を抑制するために、位置指令に含まれる所定の周波数成分を低減することが行われている。このとき、振動を抑制するためには、振動の原因となるロボットの固有振動数を効果的に低減できることが望ましい。そのため、例えば特許文献１には、固有振動数を低減することに着目し、フィルタの特性を詳細に設計できるノッチフィルタが記載されている。
【０００３】
また、フィルタ処理を行うとどうしても処理遅れが生じるものの、ロボットの場合、応答性も重要である。そのため、例えば特許文献２には、目標位置へ到達するまでの時間に着目し、設定したステップ数で負荷を目標位置に到達させる制御方法が記載されている。
このように、従来のロボットの制御装置では、フィルタ特性や応答性に着目したフィルタ設計が行われている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１１−１４２７３９号公報
【特許文献２】特許第５２１１３０６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、上記したように固有振動数の低減や応答性等のフィルタ特性に着目してフィルタ処理を行う場合、つまり、フィルタ特性を重視したフィルタ設計が行われている場合、次のような問題が発生する可能性がある。すなわち、フィルタ処理された位置指令軌道により定められる出力速度や出力加速度が、ロボットに設定されている入力速度や入力加速度の限度値を超えてしまうという問題である。
【０００６】
具体例を挙げると、例えば図１２（Ａ）にグラフＧ１にて示すような位置指令が入力されたとする。なお、グラフＧ１は、入力された位置指令により定まる入力速度、より厳密には入力された角速度の変化を示している。この図１２（Ａ）の場合、ロボットの動作開始時の入力速度の変化を示しており、停止している状態から直線的に入力速度が増加していき、６００ｒｐｍに到達した後、その速度を維持して動作していることを示している。この図１２（Ａ）の場合、６００ｒｐｍが、速度の限度値、より厳密には、速度の上限値に相当する。
【０００７】
さて、図１２（Ａ）に示すような位置指令に対してフィルタ処理を行った結果、最適なフィルタ特性を示すものとして、例えば図１２（Ｂ）にグラフＧ２にて示すような位置指令軌道が生成されたとする。なお、グラフＧ２は、位置指令軌道により定められる出力速度、より厳密には出力される角速度の変化を示している。この図１２（Ｂ）の場合、出力速度は、基本的には入力速度に追従して変化しているものの、最適なフィルタ特性を確保するために、領域Ｒ１では入力速度の限度値を超え、領域Ｒ２では入力速度の限度値を下回るような変化している。
【０００８】
つまり、フィルタ特性を重視したフィルタ処理を行うと、入力された位置指令によっては、出力速度が入力速度の限度値を超えるような位置指令軌道が生成されることがある。なお、固有振動数の低減を重視したフィルタ処理の場合だけでなく、応答性を上げることを重視してフィルタ処理を行う場合であっても、応答時間を短縮するためにロボットの動作速度を上げることがあるため、グラフＧ２のように出力速度が限度値を超えるような位置指令軌道が生成されることがある。
【０００９】
さて、ロボットは、安全性を確保するために、動作する際の速度や加速度の上限が定められている。例えば、手動運転時にロボットが意図しない動作を行った場合でも作業者がアームを避けることができるように、ＵＬ規格であれば手動運転時におけるアーム構造体の移動速度が２５０ｍｍ／ｓｅｃを超えないことが規定されている。このＵＬ規格の規定を守るための速度（以下、便宜的に安全基準速度と称する）の一例として、図１２（Ａ）の場合には６００ｒｐｍが設定されている。つまり、図１２（Ａ）は、ロボットが安全基準速度ぎりぎりで動作する位置指令を示している。
なお、位置指令により定まる入力速度が同一値であっても、ロボットのアームが伸びている場合とアームが縮んでいる場合とでは、実際のアーム構造体の移動速度は異なることになる。そのため、安全基準速度は、ロボットの姿勢の変化に応じてその都度変更する必要がある。
【００１０】
そのため、図１２（Ｂ）に示すように出力速度が安全基準速度を超えるような場合には、ロボットの制御装置は、図１２（Ｃ）に示すように、出力速度を安全基準速度で制限し、６００ｒｐｍを超えていた範囲においては、出力速度が６００ｒｐｍで頭打ちされている。このように、出力速度が安全基準速度を超える位置指令軌道が生成された場合には、出力速度が安全基準速度を超えるような状態は排除され、安全性を確保すること自体はできるようになる。
【００１１】
しかしながら、図１２（Ｂ）に示すグラフＧ２の位置指令軌道は、設計者が意図したとおりのフィルタ特性を得るためのものである。より具体的に言えば、グラフＧ２に示す位置指令軌道は、領域Ｒ１において出力速度が安全基準速度を一旦超えるような変化をすることによって所望のフィルタ特性を得ることができるのである。そのような位置指令軌道に対して、図１２（Ｃ）に示すような安全基準速度による制限を加えると、出力速度が安全基準速度を超える変化をしなくなる。
【００１２】
つまり、出力速度を安全基準速度で制限した場合には、換言すると、フィルタ処理の外部からいわば強制的に位置指令軌道を制限した場合には、制限された位置指令軌道（グラフＧ３）は、設計者が意図したフィルタ特性を得るための最適な位置指令軌道（グラフＧ２）から乖離してしまう。つまり、安全性を考慮した結果、設計者が意図した通りのフィルタ特性を得ることができなくなることを意味している。この場合、振動の抑制が不十分となり、ロボットの振動を抑制できなくなったり、反って振動が増加してしまったりするおそれがある。また、この振動により、たとえ安全基準速度に出力速度を制限したとしても、実際の動作速度は安全基準速度を超えてしまうおそれもある。
【００１３】
また、安全基準速度を超えないように制限された結果、図１２（Ｃ）に示す領域Ｒ２のように、不足した速度を補うように安全基準速度まで出力速度を増大させるような制御が行われる。その結果、グラフＧ２にて示す位置指令軌道からさらに乖離してしまい、所望のフィルタ特性を得ることが一層困難となる。
【００１４】
本発明は上記事情に鑑みてなされたものであり、その目的は、安全性等を考慮しつつも、設計者が意図した通りのフィルタ特性を得ることができるロボットの制御装置を提供することにある。
【課題を解決するための手段】
【００１５】
請求項１記載の発明では、移動体の移動目標位置を指令する位置指令を生成する位置指令生成部と、位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の目標値となる位置指令軌道を生成する位置指令軌道生成部と、位置指令軌道に基づいて移動体を制御する制御部と、を備えている。そして、位置指令軌道生成部のフィルタは、位置指令軌道により定められる出力速度の限度値を、位置指令により定められる入力速度の限度値により制約するように設計されている。
【００１６】
作業者がロボットを手動運転する場合、前述のように安全性を確保するための安全基準速度が設定されており、操作を入力する時点で、例えば手先の移動速度が安全基準速度を超えることがないように制限される。つまり、位置指令により定められる入力速度は、そもそも安全基準速度を超えることがない範囲で設定されている。
そのため、位置指令軌道生成部のフィルタを、出力速度の限度値を入力速度の限度値により制約するように設計したことにより、つまり、出力速度を制限するための基準を入力速度に求めるという従来とは全く異なる新規な思想でフィルタを設計したことにより、出力速度の限度値が入力速度の限度値を超えてしまうことが防止される。換言すると、出力速度は、入力速度の限度値を超えないことを保証できる。したがって、手動運転時の安全性を確保することができる。
【００１７】
また、出力速度が入力速度の限度値を超えないことを保証できることから、意図したフィルタ特性を得るために生成された位置指令軌道に対して、上記した図１２（Ｃ）のような外部からの強制的な制限を加える必要がなくなる。その結果、フィルタ特性に影響を与えることがなくなり、設計者の意図の通りのフィルタ特性に沿った位置指令軌道が制御部に与えられ、そのフィルタ特性に沿った動作、すなわち、特定の周波数成分を低減した状態で移動体を制御することができ、所望の振動抑制効果を得ることができる。
このように、本発明によれば、安全性等を考慮しつつも、意図した通りのフィルタ特性を得ることができる。
【００１８】
このことは、ロボットの制御装置において非常に有意である。すなわち、従来では、上記したように限度値で規制する処理、また、限度値をアームの伸縮状態等によって変更する処理等が必要となっていたのに対し、本発明では、出力速度が入力速度の限度値を超えないことが保証されるため、そのような処理が不要となる。したがって、制御装置の処理の複雑化を招くことがない。
【００１９】
また、位置指令により定められる入力速度は、ロボットを構成するモータやハーモニックギア等による物理的な制約も存在し、その制約を遵守した範囲内で実際に動作可能な値が設定される。そして、ロボットを自動運転する場合には、その物理的な制約の範囲内で可能な限り高速に動作をさせて作業効率を向上させることが行われる場合がある。そのため、従来のようなフィルタ設計では、自動運転時に出力速度が物理的な制約を超えるような状況になるおそれがあり、その場合には外部からの強制的な制限が加えられることになる。しかし、本発明のフィルタ設計であれば、出力速度が入力速度の限度値により制約されることから、出力速度が入力速度の限度値を超えるような動作が起きることがない。つまり、本発明の効果は、手動運転時だけでなく自動運転時にも得ることができる。
【００２０】
請求項２記載の発明では、位置指令を生成する位置指令生成部と、位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の目標値となる位置指令軌道を生成する位置指令生成部と、位置指令軌道に基づいて移動体を制御する制御部と、を備える。そして、位置指令軌道生成部のフィルタは、位置指令軌道により定められる出力加速度の限度値を、位置指令により定められる入力加速度の限度値により制約するように設計されている。つまり、本発明では、上記した請求項１記載の発明とその思想が共通するものであり、出力加速度を制限するための基準を入力加速度に求めるという従来とは全く異なる新規な思想でフィルタを設計している。
【００２１】
上記したように、入力される位置指令は、もともとロボットが動作可能な範囲で設定されていることから、その位置指令により定まる入力加速度の限度値で出力加速度を制約することで、実際に動作可能な位置指令軌道を生成することができる。そして、生成された位置指令軌道は、フィルタ特性に沿ったものであることから、設計者が意図したフィルタ特性を得ることができる。すなわち、ロボットに設定されている限度値を遵守しつつも、所望の振動抑制効果を得ることができる。
【００２２】
また、上記したようにロボットには物理的な制約が存在する。このとき、フィルタ特性によっては、物理的に不可能な加速度で動作させるような位置指令軌道が生成されるおそれがある。その場合、動作することが物理的に不可能であることから、当然のことながら、フィルタ特性に沿った動作をすることができず、設計者が意図したような振動の抑制をすることができなくなる。そこで、出力加速度の限度値を入力加速度の限度値で制約することにより、出力加速度が実際に動作可能な範囲に収めることができる。
【００２３】
請求項３記載の発明では、位置指令軌道生成部のフィルタは、位置指令軌道により定められる出力加速度の限度値を、位置指令により定められる入力加速度の限度値により制約するように設計されている。これにより、上記した請求項２記載の発明と同様に、出力加速度が入力加速度の限度値を超えることがなくなり、実際に動作可能な位置指令軌道を生成することができる。また、出力速度が入力速度の限度値を超えることもないため、上記した請求項１記載の発明と同様に、設計者が意図したとおりのフィルタ特性を得ることができる。
【００２４】
請求項４記載の発明では、位置指令軌道生成部のフィルタは、特定の周波数成分を低減する有限インパルス応答フィルタで構成されており、その有限インパルス応答フィルタの係数を全て０以上に設定することで、位置指令の特定の周波数成分を低減しつつ、限度値による制約を実現する。
【００２５】
特定の周波数成分の低減と限度値による制約とを実現する場合、単純に考えれば、個別にフィルタを設ければよい。しかし、仮に個別にフィルタを設けると、いずれか一方のフィルタの処理結果を他方のフィルタで再処理することになるため、一方のフィルタで得られたフィルタ特性が他方のフィルタでのフィルタ処理によって変化する可能性もある。つまり、個別にフィルタを設ける場合には、設計者が意図したフィルタ特性を得ることができなくなったりするおそれがある。
かといって、特定の周波数成分の低減と限度値による制約とを実現するために複雑なフィルタを設計すると、フィルタ処理における遅延が大きくなり、ひいてはロボットの動作が遅くなる。
【００２６】
そこで、発明者らは、研究を重ねた結果、もともとロボットの制御装置に設けられている特定の周波数成分を低減するための有限インパルス応答フィルタに対し、その係数を全て０以上に設定することで、特定の周波数成分の低減と限度値による制約とを実現することができることを見いだした。このような全く新規なフィルタの設計思想を得たことにより、本発明は、フィルタ処理の複雑化を招くことなく、特定の周波数成分の低減と限度値による制約とを１つのフィルタで実現することができる。また、個別にフィルタを設ける場合とは異なり特定の周波数成分を低減するフィルタ特性が変化することがなく、さらには、フィルタ処理自体は１回で済むのでフィルタ処理による遅延の増加を招くことがない。
【００２７】
請求項５記載の発明では、位置指令生成部は、特定の周波数成分を低減するｎ次の有限インパルス応答フィルタで構成されており、その有限インパルス応答フィルタの係数をｍ＝ｎ／２を中心として対称に設定することで、位置指令の特定の周波数成分を低減しつつ、零位相差特性を実現する。
【００２８】
特定の周波数成分の低減と零位相差特性とを実現する場合、単純に考えれば、個別にフィルタを設ければよいが、その場合、上記した請求項４に記載の発明で説明したように、設計者が意図したフィルタ特性を得ることができなくなったり、フィルタ処理における遅延が大きくなったりする。
【００２９】
そこで、発明者らは、研究を重ねた結果、もともとロボットの制御装置に設けられている特定の周波数成分を低減するためのｎ次の有限インパルス応答フィルタに対し、その係数をｍ＝ｎ／２を中心として対称に設定することで、特定の周波数成分の低減と零位相差特性とを実現することができることを見いだした。このような全く新規なフィルタの設計思想を得たことにより、本発明は、フィルタ処理の複雑化を招くことなく、特定の周波数成分の低減と零位相差特性とを１つのフィルタで実現することができる。また、個別にフィルタを設ける場合とは異なり特定の周波数成分を低減するフィルタ特性が変化することがなく、さらには、フィルタ処理自体は１回で済むのでフィルタ処理による遅延の増加を招くことがない。
請求項６記載の発明では、有限インパルス応答フィルタは、そのフィルタ特性を決定する際の最適化パラメータの一つとしてフィルタ係数の対称性を設定可能であり、当該フィルタ係数の対称性を設定することにより擬似的な零位相差特性を実現する。フィルタ係数を完全に対称に設定した場合には、上記した請求項５記載の発明のように完全な零位相差特性を備える異になる。その一方で、零位相差特性よりも応答性や移動速度を重視したい場合には、完全な零位相差特性が必ずしも必要とされないことも想定される。そこで、フィルタ係数の対称性を設定可能とすることで、零位相差特性を重視するか応答性や移動速度を重視するかを選択することが可能となり、フィルタ設計の自由度を向上させることができる。
【００３０】
請求項７記載の発明では、位置指令の特定の周波数成分を低減するフィルタを有し、移動体を制御する際の入力であって、位置指令軌道に追従させるフィードフォワード制御を行うための制御入力を生成する制御入力生成部を備える。
ロボットを制御する際、フィードバック補償を行うと、どうしても遅延が生じる。そこで、制御入力生成部を設け、生成された位置指令軌道に追従させるフィードフォワード制御を行うための制御入力を生成することで、フィードバック補償による遅延を補填することができ、応答性を高めることができる。
【００３１】
請求項８記載の発明では、制御入力生成部のフィルタは、制御入力により定められる出力トルクの限度値を、位置指令により定められる入力トルクの限度値により制約するように設計されている。つまり、上記した請求項記載の発明等と共通する思想に基づいて、制御入力に対しても、出力トルクの限度値を入力トルクの限度値による制約を加える。これにより、出力トルクが入力トルクの限度値を超えることがなくなり、外部からの制限等を加える必要が無くなる。したがって、フィルタ特性に影響を与えることがなくなり、設計者の意図の通りのフィルタ特性を得ることができる。
【図面の簡単な説明】
【００３２】
【図１】第１実施形態における制御装置およびモデル化したロボットを示す図
【図２】ロボットの制御装置の機能ブロックを示す図
【図３】フィルタ特性を示す図
【図４】位置指令軌道を示す図
【図５】零位相差特性を有するフィルタ特性を示す図
【図６】シミュレーション結果を示す図
【図７】第２実施形態におけるロボットの制御装置の機能ブロックを示す図
【図８】制御系を模式的に示す図
【図９】フィルタ特性を示す図
【図１０】フィードフォワード制御における周波数応答を示す図
【図１１】シミュレーション結果を示す図
【図１２】従来技術によるフィルタ処理を行った場合の速度の変化を示す図
【発明を実施するための形態】
【００３３】
以下、本発明の複数の実施形態について図面を参照しながら説明する。
（第１実施形態）
以下、本発明の第１実施形態について、図１から図６を参照しながら説明する。
図１に示すように、本実施形態におけるロボットの制御装置（以下、単にコントローラ１と称する）は、制御対象となるロボット２に接続されており、ロボット２の各アームに設けられているモータを制御することで、ロボット２の姿勢を制御する。このコントローラ１は、ティーチングペンダントに接続されており、作業者は、ティーチングペンダントから、ロボット２の動作軌跡の教示や手動運転することができる。
【００３４】
ロボット２は、本実施形態ではいわゆる２軸ロボットと称される自由度が２の多関節型ロボットを想定している。ただし、本実施形態では説明の簡略化のために２軸ロボットを例示しているのであって、本発明は、４軸ロボットや６軸ロボットあるいは７軸ロボット等、自由度が異なる場合であっても適用することができる。
【００３５】
このロボット２は、ベース部４と、ベース部４に取り付けられた第１アーム５と、第１アーム５に取り付けられた第２アーム６とを備えている。これら第１アーム５および第２アーム６は、移動体に相当する。ベース部４には、第１アーム５を駆動するための第１モータ７、および第１モータ７の回転位置を検出するための第１エンコーダ８が設けられおり、第１モータ７に取り付けられている波動歯車装置（以下、第１ハーモニックギア９と称する）を介して第１アーム５が駆動される。
【００３６】
第１アーム５には、第２アーム６を駆動するための第２モータ１０、および第２モータ１０の回転位置を検出するための第２エンコーダ１１が設けられおり、第２モータ１０に取り付けられている波動歯車装置（以下、第２ハーモニックギア１２と称する）を介して第２アーム６が駆動される。各エンコーダで検出されたモータの回転位置つまり各アームの位置は、コントローラ１に伝達され、フィードバック制御に用いられる。
【００３７】
さて、各アームを駆動するハーモニックギアは、周知のように、楕円と真円との差動を利用してモータの回転出力を減速する減速機である。このハーモニックギアは、楕円と真円との差動を利用していることから、それによって駆動されるアームを剛体として捉えられない場合が多く、高速且つ高精度にアームの位置を制御するためには、アームが回転する際の振動、つまり移動体に生じる振動を抑制することが必要とされる。
そのため、コントローラ１は、一般的に、振動を抑制するためのフィルタ処理を行っている。具体的には、図２に機能ブロックとして示すように、コントローラ１は、位置指令生成部２０と、位置指令フィルタ２１と、モータ制御部２２とを備えている。
【００３８】
位置指令生成部２０は、要求された作業をロボット２に行わせるために、移動体であるアームの位置を指令するための位置指令を生成する。本実施形態の場合、位置指令生成部２０は、各アームの位置を指令するための位置指令として、モータの回転角度を示す角度指令θｉｎを生成する。この角度指令θｉｎは、モータやハーモニックギア等による物理的な制約のもとで、ロボット２が動作可能な範囲に設定されている。そのため、角度指令θｉｎにより定められる入力速度に相当する角速度、および入力加速度に相当する角加速度も、モータやハーモニックギア等により制約されている。なお、入力速度および入力加速度の詳細については後述する。
【００３９】
位置指令フィルタ２１は、位置指令生成部２０で生成された角度指令θｉｎに対して特定の周波数成分を低減するフィルタ処理を行い、移動体であるアームを制御する際の目標値となる位置指令軌道に相当する角度指令軌道θoutを生成する。この位置指令フィルタ２１は、振動を抑制するためのフィルタ処理を行うものである。位置指令フィルタ２１は、詳細は後述するが、有限インパルス応答フィルタ（以下、ＦＩＲ（Finite Impulse Response）フィルタと称する）で構成されている。この位置指令フィルタ２１は、位置指令軌道生成部に相当する。
モータ制御部２２は、位置指令フィルタ２１で生成された角度指令軌道θoutに基づいて、各モータの回転角度を制御する。このモータ制御部２２は、制御部に相当する。
【００４０】
次に、位置指令フィルタ２１で行われるフィルタ処理について詳細に説明する。
従来では、一般的に、次のａ）、ｂ）の制約を満足するようにＦＩＲフィルタを設計している。
ａ）定常状態で角度指令θｉｎと角度指令軌道θoutとが一致すること。
ｂ）振動の原因となる特定の周波数成分を低減すること。
これらを考慮してＦＩＲフィルタを設計することで、各アームは、角度指令θｉｎに追従して指示された位置まで移動することができるとともに、移動時に生じるおそれのある振動を抑制することができる。
【００４１】
そして、本実施形態では、ａ）、ｂ）の制約に加えて、次のｃ）の制約をも満足するＦＩＲフィルタを設計している。
ｃ）角度指令軌道θoutにより定められる出力速度、出力加速度の限度値を、角度指令θｉｎにより定められる入力速度、入力加速度の限度値により制約すること。ただし、本実施形態の限度値には、上限および下限が含まれる。なお、以降は、このｃ）の制約を便宜的に上下限制約とも称する。
【００４２】
つまり、本実施形態では、ＦＩＲフィルタは、出力速度の上限が入力速度の上限（以下、便宜的に速度上限とも称する）により制約され、出力速度の下限が入力速度の下限（以下、便宜的に速度下限とも称する）により制約され、出力加速度の上限が入力加速度の上限（以下、便宜的に加速度上限とも称する）により制約され、出力加速度の上限が入力加速度の下限（以下、便宜的に加速度下限とも称する）により制約されるように設計されている。
【００４３】
入力速度や入力加速度は、前述のようにモータやハーモニックギア等による物理的な制約のもとで設定されているとともに、安全性等を考慮した制約が加えられている。つまり、入力速度は、そもそも安全基準速度を超えることがない範囲で設定されている。そのため、ＦＩＲフィルタを設計する際、出力が入力の限度値である速度上限、速度下限、加速度上限、加速度下限を超えなければ、出力が安全基準速度を超えることもないと考えられるためである。なお、必ずしも速度および加速度の双方を制約する必要は無く、速度のみを制約する構成でもよいし、加速度のみを制約する構成であってもよい。
【００４４】
以下、上記したａ）〜ｃ）の制約を満足するＦＩＲフィルタの設計手法について説明する。なお、説明の簡略化のために、以下では、まず従来から行われているａ）の制約を満足するための設計条件について説明し、続いて、ｂ）の制約を満足するための設計条件について２通りの手法で説明し、その後、ｃ）の制約を満足するための設計条件について説明する。
【００４５】
＜ａ）の制約について＞
ＦＩＲフィルタの伝達関数は、次の（１）式のように表される。
【００４６】
【数１】
2016043417.tif 000003
【００４７】
ここで、ａ０、ａ１、ａ２、・・・ａｎ−１、ａｎは、ＦＩＲフィルタの係数である。
そして、（１）式に示したＦＩＲフィルタの応答は、次の（２）式のように記述することができる。
【００４８】
【数２】
2016043417.tif 000004
【００４９】
さて、いま次の（３）式に示すようにｎステップの間、同一の角度指令θｉｎが与えられたとする。
【００５０】
【数３】
2016043417.tif 000005
【００５１】
この（３）式を（２）式に代入すると、次の（４）式が得られる。
【００５２】
【数４】
2016043417.tif 000006
【００５３】
この（４）式から、定常状態で角度指令θｉｎと角度指令軌道θoutとを一致させるためには、次の（５）式の条件を満たせばよいことが分かる。
【００５４】
【数５】
2016043417.tif 000007
【００５５】
すなわち、ＦＩＲフィルタの係数を設定する際、その総和が１となる条件を加えることで、上記したａ）の制約を満足するＦＩＲフィルタを設計できる。そして、係数の総和が１となるＦＩＲフィルタを用いることで、出力と指令値とを一致させて制御するいわゆるデッドビート制御を行うことができる。
【００５６】
また、この（５）式から、ＦＩＲフィルタの次数（ｎ）によって、ＦＩＲフィルタの応答性を調整できることが分かる。すなわち、次数（ｎ）を小さくすれば応答性を高くすることができ、次数（ｎ）を大きくすれば応答性を低くすることができる。なお、この（５）式は、変数行列ａを用いても表すことができ、以降の説明のために次の（６）式を定義しておく。
【００５７】
【数６】
2016043417.tif 000008
【００５８】
＜ｂ）の制約について＞
次に、ｂ）の制約を満足するように特定の周波数成分を低減するための手法は、評価関数による手法と、等式制約による手法等が考えられる。以下、その２通りについて個別に説明する。
＜＜評価関数による手法＞＞
評価関数をＪとすると、評価関数は、次の（７）式のように表すことができる。
【００５９】
【数７】
2016043417.tif 000009
【００６０】
ここで、Ｑは、ｎ×ｎの重み行列を示しており、次の（８）式のように、高周波成分の低減を目的としたＱｊと、特定の周波数帯の低減を目的とするＱｆｋｓとによって与えられる。
【００６１】
【数８】
2016043417.tif 000010
【００６２】
ここで、ｌは、低減する周波数帯の数、ｑｊとｑｆｋｓは重み係数を示している。
Ｑｊは、次の（９）式および（１０）式のように設計できる。
【００６３】
【数９】
2016043417.tif 000011
【００６４】
また、Ｑｆｋｓは、次の（１１）式、（１２）式および（１３）式の通りに与えられる。
【００６５】
【数１０】
2016043417.tif 000012
【００６６】
これらの式から、本実施形態のＦＩＲフィルタでは、ＱｊとＱｆｋｓとのバランスを操作することができること、そして、Ｑｆｋｓの設計時に指定する周波数帯にロボット２の固有振動数が含まれるようにすれば、想定したノミナル状態だけでなく、振動周波数が多少変動した場合であっても効果的に振動を抑制することができることが分かる。なお、ＦＩＲフィルタの次数（ｎ）が極端に小さい場合や、重み係数のｑｆｋｓが極端に小さい場合には、十分な振動抑制効果を得ることができないおそれがあるため、実際の設計時には、幾らかの試行錯誤を必要とすることがある。
【００６７】
＜＜等式制約による手法＞＞
等式制約により特定の周波数成分を低減する手法は、従来から行われている。そのため、ここでは基本的な設計方法について簡単に説明する。
ノッチフィルタに代表されるように、ＦＩＲフィルタが減衰係数の小さい零点を備えていると、ある特定周波数のゲインを抑制することができる。ここで、抑制したい特定周波数に対応する２次系の零点がｚ＝λ１、λ２であるとすると、ＦＩＲフィルタがこれらの零点を含む条件は、次の（１４）式となり、この（１４）式は、次の（１５）式のように行列によって再表現できる。
【００６８】
【数１１】
2016043417.tif 000013
【００６９】
これらの式から明らかなように、等式制約により特定周波数を抑制したい場合には、（１５）式と上記した（６）式とを合わせた次の（１６）式に示す２つの関係式を満足するようにＦＩＲフィルタを設計すれば、設定した特定周波数を等式制約により抑制することができる。
【００７０】
【数１２】
2016043417.tif 000014
【００７１】
このように、評価関数あるいは等式制約による手法を用いることにより、上記したｂ）の制約を満足することができる。なお、等式制約による手法は、評価関数による手法に比べると、振動周波数の変動に追従できないことからロバスト性が保証できないことが懸念される。その場合、多数の零点を含めることも可能であるが、その場合には次数（ｎ）が大きくなって応答性が低下する。そのため、ロバスト性と応答性とを考慮して、いずれの手法を採用するかを適宜選択すればよい。
【００７２】
＜ｃ）の制約について＞
次に、ａ）、ｂ）の制約を満足したＦＩＲフィルタが上下限制約をも満足するようにするための設計条件について説明する。
まず、入力速度（ｖｉｎ）は、角度指令θｉｎによって、次の（１７）式のように定められ、出力速度（ｖｏｕｔ）は、角度指令軌道θｏｕｔによって、次の（１８）式のように定められる。
【００７３】
【数１３】
2016043417.tif 000015
【００７４】
ここで、Ａの最小値をｍｉｎ（Ａ）として表記し、Ａの最大値をｍａｘ（Ａ）として表記すると、入力速度は、その最小値をｖｍｉｎとし、その最大値をｖｍａｘとした場合、次の（１９）式および（２０）式のように表される。また、入力加速度は、その最小値をαｍｉｎとし、その最大値をαｍａｘとすると、次の（２１）式および（２２）式のように表される。
【００７５】
【数１４】
2016043417.tif 000016
【００７６】
ただし、ｊ＝０、１、２、・・・、∞である。なお、上記した（１７）式および（１８）式は、ｊ＝０の場合に相当する。
これら（１９）式から（２２）式を用いると、上下限制約の設計条件は、次の（２３）式および（２４）式のように表すことができる。
【００７７】
【数１５】
2016043417.tif 000017
【００７８】
換言すると、上下限制約を満足するためには、（２３）式および（２４）式を満足するようにＦＩＲフィルタを設計すればよいことになる。
さて、ＦＩＲフィルタによって実現される応答は、上記した（２）式の通りである。
いま、（２）式を上記した（２３）式に代入すると、次の（２５）式が得られる。
【００７９】
【数１６】
2016043417.tif 000018
【００８０】
ただし、ｖ＝ｖｍａｘ＝−ｖｍｉｎである。
ここで、次の（２６）式に示すように、ＦＩＲフィルタの係数（ａ）が全て０以上であると仮定する。
【００８１】
【数１７】
2016043417.tif 000019
【００８２】
この仮定においては、（２５）式の左辺が最大となる条件は、次の（２７）式となる。
【００８３】
【数１８】
2016043417.tif 000020
【００８４】
なぜならば、（２５）式の左辺において係数（ａ）に掛かる部分は、ｖ＝ｖｍａｘ＝−ｖｍｉｎであれば１以下になるためである。そのため、（２７）式の関係を（２５）式に代入すると、（２６）式の仮定に対する十分条件として、次の（２８）式を得ることができる。
【００８５】
【数１９】
2016043417.tif 000021
【００８６】
そして、係数（ａ）がこの（２８）式を既に満足していることは、上記した（５）式より明らかである。
そのため、速度に関する上下限制約の設計条件である（２３）式を満足するためには、（２６）式の仮定を満足すればよいことが証明された。すなわち、上記したａ）、ｂ）の制約を満足するＦＩＲフィルタに対して、その全ての係数を０以上に設定することで、出力速度の限度値を入力速度の限度値で制約することができること、つまり、ｃ）の制約を満足することが明らかになった。
【００８７】
ここで、上記した証明に対する検証を速度に対して行ってみる。
上記した（１７）式および（１８）式を用いると、（２３）式は、次の（２９）式のように表すことができ、また、上記した（１９）式および（２０式）より、上下限制約の設計条件は、次の（３０）式から（３２）式のように表すことができる。
る。
【００８８】
【数２０】
2016043417.tif 000022
【００８９】
ＦＩＲフィルタが備える定常特性から、入力速度がある速度になっている場合には、出力速度もその速度になっている。この定常特性は、ＦＩＲフィルタの係数に正ではない係数が存在する場合にも得ることができる。ただし、いずれかの係数が負である場合には、以下に説明するように、出力速度が入力速度の上限を超える可能性がある。
【００９０】
さて、入力速度が速度上限で定常状態になっている状態が、最も入力速度が高い状態であると考えられる。
いま、速度上限で定常状態となる直前の状態、より具体的に言えば、入力速度が僅かに速度上限よりも低い状態を想定する。出力速度（ｖｏｕｔ）と速度上限との関係は、（５）式および（１８）式を用いると、次の（３３）式のように表すことができる。
【００９１】
【数２１】
2016043417.tif 000023
【００９２】
ここで、ＦＩＲフィルタの係数のうちいずれかの係数が負である状態における係数および入力速度は、次の（３４）式および（３５）式のように表すことができる。（３４）式および（３５）式は、ＦＩＲフィルタの係数のうちａｐが負であり、ａｐに対応する入力速度（ｖｉｎ［ｐ］）が入力速度の最大値（ｖｍａｘ）よりもΔｖだけ低い状態を示している。
【００９３】
【数２２】
2016043417.tif 000024
【００９４】
これら（３４）式および（３５）式を（１８）式に代入すると、次の（３６）式が得られる。
【００９５】
【数２３】
2016043417.tif 000025
【００９６】
この（３６）式は、いずれかの係数が負である場合には、出力速度（ｖｏｕｔ［ｋ］）が、入力速度の最大値（ｖｍａｘ）よりも｜ｂｐ｜Δｖだけ高くなること、すなわち、出力速度が入力速度の上限を超えてしまうことを示している。この場合、速度上限が安全基準速度であったとすると、安全基準速度を超えてしまうことになる。
【００９７】
このように、いずれかの係数が負である場合には出力速度が入力速度の上限を超えてしまうことから、ＦＩＲフィルタの係数が０以上であることが、上下限制約を満足するための設計条件であることが検証できた。
さて、ここまでは出力速度について説明したが、出力加速度についても同様である。
出力加速度に対する上下限制約は、出力速度の場合と同様の考え方により、次の（３７）式のように表すことができる。
【００９８】
【数２４】
2016043417.tif 000026
【００９９】
ここで、α＝αｍａｘ＝−αｍｉｎである。
上記した（２６）式のようにＦＩＲフィルタの係数（ａ）が全て０以上であると仮定すると、出力加速度を示す（３７）式の左辺が最大となる条件は、次の（３８）式となる。
【０１００】
【数２５】
2016043417.tif 000027
【０１０１】
そして、出力速度の場合と同じ展開により、（３８）式の十分条件として（２２）式を得ることができる。つまり、出力加速度に関しても、（２２）式の仮定、すなわち、ＦＩＲフィルタの係数が全て０以上であることが、上下限制約を満足するための条件であることが証明された。
この証明結果を変数行列ａを用いて表すと、次の（３９）式となる。つまり、この（３９）式を遵守するようにＦＩＲフィルタを設計することで、上下限制約を満足したＦＩＲフィルタを設計することができる。
【０１０２】
【数２６】
2016043417.tif 000028
【０１０３】
このように、（６）式および（１６）式の等式制約と（３９）式の不等式制約下で、（７）式の評価関数を最小化する変数行列ａを求めれば、所望のフィルタ特性を備えるＦＩＲフィルタを導出することができる。なお、（７）式の評価関数は２次形式であるため、２次計画問題を解くことで、変数行列ａを導出することができる。
【０１０４】
次に、ａ）〜ｃ）の制約を満足するように導出されたＦＩＲフィルタの具体例について、図３および図４を参照しながら説明する。
図３に示すフィルタ特性は、図１に示したモデルにおいて２軸目を０°から１４５°に回転させる場合の一例である。この具体例では、サンプリング周期を８ｍｓｅｃ、ＦＩＲフィルタの次数を８とし、振動の抑制は上記した評価関数を用いており、抑制する周波数帯として９．５Ｈｚから１０．５Ｈｚを指定している。
【０１０５】
図３は、ａ）〜ｃ）の制約を満足したＦＩＲフィルタのフィルタ特性をグラフＧ１０として示し、従来の手法つまり上下限制約を考慮せずに設計されたフィルタ特性をグラフＧ２０として示している。このグラフＧ１０に示すように、ａ）〜ｃ）の制約を満足するように設計されたＦＩＲフィルタは、ゲイン（Ｇａｉｎ）が９．５Ｈｚから１０．５Ｈｚの周波数帯のゲインが大きく低減されており、９．５Ｈｚから１０．５Ｈｚの周波数成分を効果的に低減することができていることを示している。つまり、本実施形態のように設計したＦＩＲフィルタにおいても、グラフＧ２０にて示す従来のＦＩＲフィルタと同様に、ａ）、ｂ）の制約を満足するものであることが示された。
なお、位相（Ｐｈａｓｅ）については、グラフＧ１１にて示す本実施形態によるＦＩＲフィルタも、グラフＧ２１にて示す従来技術のＦＩＲフィルタも位相ずれが生じているが、これについては後述する。
【０１０６】
図４は、従来技術によるＦＩＲフィルタを参考例として示し、本実施形態によるＦＩＲフィルタを実施例として示したシミュレーション結果である。この図４では、角度指令により指定された角度（以下、便宜的に入力角度と称する）とそれに追従して移動する角度（以下、便宜的に移動角度と称する）との対応関係を角度（ａｎｇｌｅ）で示し、入力速度と出力速度との対応関係を角速度（ａｎｇｌｅ ｖｅｌ．）で示し、入力加速度と出力加速度との対応関係を角加速度（ａｎｇｌｅ ａｃｃ．）で示している。
上記したように、ａ）およびｂ）の制約は、従来から行われている。そのため、参考例および実施例の何れにおいても、グラフＧ３０にて示す入力角度に追従して、グラフＧ４０にて示す参考例の移動角度およびグラフＧ５０にて示す実施例の移動角度が変化している。
【０１０７】
一方、角速度の場合、参考例では、グラフＧ４１にて示す出力速度が、領域Ｒ３に示すようにグラフＧ３１にて示す入力速度の上限を超えていたり、領域Ｒ４に示すように入力速度の下限を超えていたりする状態が見受けられる。このとき、入力速度の上限が安全基準速度であったとすると、安全性を確保するために、前述の図１２（Ｃ）に示すように、領域Ｒ３の安全基準速度を超えた部分が規制され、所望のフィルタ特性を得るための出力速度の変化が起きなくなり、フィルタ特性が悪化するおそれがある。これは、角加速度についても同様であり、グラフＧ４２にて示す参考例の場合、出力加速度は、領域Ｒ５に示すようにグラフＧ３２にて示す入力加速度の上限を超えていたり、領域Ｒ６に示すように入力加速度の下限を超えていたりする状態が見受けられる。
【０１０８】
これに対して、グラフＧ５１にて示す実施例の場合、出力速度は、グラフＧ３１にて示す入力速度の上限および下限を超えない範囲に抑えられている。また、グラフＧ５２にて示す出力加速度は、グラフＧ３２にて示す入力加速度の上限および下限を超えない範囲に抑えられている。つまり、実施例の場合、速度および加速度に対する上下限制約を満足したシミュレーション結果となっている。つまり、本実施形態のように設計したＦＩＲフィルタであれば、フィルタ特性に影響を与えるような領域の規制は行われることがないことから、所望のフィルタ特性を得ることができる。そして、出力速度が入力速度の上限を超えることがなく、また、安全基準速度を超えるような入力速度が入力されることがそもそもないため、安全性を確保することができる。
【０１０９】
このように、本実施形態のＦＩＲフィルタは、従来のものと同様にａ）、ｂ）の制約を満足することができるとともに、従来のものでは実現できない、あるいは、従来のものでは考慮されていなかった上下限制約をも満足することができる。
【０１１０】
ところで、フィルタ処理において複数の周波数帯を抑制したい場合や周波数帯を大きく低減させたい場合には、ＦＩＲフィルタの次数（ｎ）を位相差が無視できるほど十分に小さくすることができないことがある。そのため、図３に示したように、どうしても位相遅れが生じることになる。このような位相遅れが生じると、ＰＴＰ（Ｐｏｉｎｔ Ｔｏ Ｐｏｉｎｔ）動作のように単純な移動目的の動作であればそれほど大きな問題にはならないものの、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐａｔｈ）動作のように連続的に軌跡に追従させる動作の場合には軌跡がずれる等の問題が生じるおそれがある。
【０１１１】
そこで、本実施形態では、上記したａ）〜ｃ）の制約を満足し、且つ、以下に説明するように零位相差特性を備えたＦＩＲフィルタを実現している。
一般的に、零位相差のＦＩＲフィルタ（Ｇｚ（ｚ））は、次の（４０）式のように表すことができ、この（４０）式に次の（４１）式を代入すると、次の（４２）式が得られる。
【０１１２】
【数２７】
2016043417.tif 000029
【０１１３】
この（４２）式は、次の（４３）式の展開式に示すように、虚数ｊが掛かる項が互いに相殺されることから、右辺は必ず実数となる。
【０１１４】
【数２８】
2016043417.tif 000030
【０１１５】
つまり、（４３）式は、右辺が必ず実数となることから、位相遅れを生じさせないことを示している。また、（４３）式は、位相遅れを生じさせないためには、インパルス応答がｍ（＝ｎ／２）を中心として対称であること、および、ｍステップ分の未来値が必要となることを示している。なお、インパルス応答がｍ（＝ｎ／２）を中心として対称であるということは、ＦＩＲフィルタの係数が対称であることと同義である。
そのため、上記したａ）〜ｃ）の制約を満足するＦＩＲフィルタに位相遅れを生じさせない特性を与えることで、零位相差特性を備えたＦＩＲフィルタを実現することができる。
具体的には、インパルス応答を対称としたい場合には、次の（４４）式の等式制約を与えることで実現することができる。
【０１１６】
【数２９】
2016043417.tif 000031
【０１１７】
この（４４）式の場合、等式制約はｍ個であるため、ＦＩＲフィルタのｎ個の設計自由度の半分を占めてしまう。その場合、振動抑制効果が減少したり、ＦＩＲフィルタの応答性を高くできなくなったりする弊害を誘発するおそれがある。そのため、次の（４５）式のＱｚｐを定義する。
【０１１８】
【数３０】
2016043417.tif 000032
【０１１９】
いま、次の（４６）式に示すように、ＱにＱｚｐを加えた新たなＱｐを定義する。
【０１２０】
【数３１】
2016043417.tif 000033
【０１２１】
ここで、ｑｚｐは重み係数である。そして、上記した（７）式の評価関数の代わりに、次の（４７）式を新たな評価関数Ｊｐとする。
【０１２２】
【数３２】
2016043417.tif 000034
【０１２３】
そして、（４７）式の評価関数を、（６）式および（１６）式の等式制約のもとで最小化することにより、擬似的に零位相差特性を実現することができる。
この零位相差特性を実現したＦＩＲフィルタの具体例について、図５および図６を参照しながら説明する。なお、このＦＩＲフィルタは、図３に示したものと同様の条件で設計されたものである。
【０１２４】
図５にグラフＧ６０にて示すように、零位相差特性を実現したＦＩＲフィルタは、９．５Ｈｚ〜１０．５Ｈｚの周波数帯を大きく低減することができる。つまり、このＦＩＲフィルタは、ｂ）の制約を満足している。また、このＦＩＲフィルタは、グラフＧ６１に示すように、概ね１０Ｈｚ以下の周波数帯において、位相遅れがほぼ零になっている。すなわち、このＦＩＲフィルタが完全な零位相差特性を有していることが確認できる。
【０１２５】
そして、図６に示すように、このＦＩＲフィルタのシミュレーション結果では、グラフＧ７０にて示す入力速度に、グラフＧ８０にて示す出力速度が追従している。つまり、このＦＩＲフィルタは、ａ）の制約を満足している。
【０１２６】
さらに、このＦＩＲフィルタの場合、グラフＧ８１にて示す出力速度は、グラフＧ７１にて示す入力速度の上限および下限を超えないように変化しているとともに、グラフＧ８２にて示す出力加速度は、グラフＧ７２にて示す入力加速度の上限および下限を超えないように変化している。つまり、このＦＩＲフィルタは、ｃ）の制約を満足するものとなっていることを確認できる。
【０１２７】
このように、本実施形態のコントローラ１は、ａ）、ｂ）の制約を満足しつつ、ＦＩＲフィルタの係数を０以上に設定することでｃ）の制約を満足し、且つ、ＦＩＲフィルタの係数を対称に設定することで擬似的に零位相差特性を実現しＦＩＲフィルタを用いて、フィルタ処理を行っている。
【０１２８】
以上説明した実施形態によれば、次のような効果を得ることができる。
本実施形態では、位置指令軌道θｏｕｔにより定められる出力速度の限度値を、位置指令θｉｎにより定められる入力速度の限度値により制約するように設計されている。この場合、位置指令θｉｎにより定められる入力速度は、手動運転時の安全性を確保するための安全基準速度を超えることがない範囲で設定されている。そのため、出力速度の限度値を入力速度の限度値で制約することで、すなわち、出力速度の上限値は入力速度の上限値を超えることがなく、出力速度の下限値は入力速度の下限値を下回ることがないように設計されることで、出力速度が入力速度の限度値を超えないことが保証される。したがって、ロボットの動作速度を高めるために入力速度の上限が安全基準速度であったとしても、出力速度が安全基準速度を超えることがない。したがって、手動運転時の安全性を確保することができる。
【０１２９】
また、出力速度が入力速度の限度値を超えないことを保証できることから、意図したフィルタ特性を得るために生成された位置指令軌道に対して、安全基準速度等のいわば外部からの強制的な制限を加える必要がなくなる。これにより、フィルタ特性に影響を与えることがなくなり、設計者の意図の通りのフィルタ特性を得ることができる。すなわち、特定の周波数成分を低減する場合、所望の振動抑制効果を得ることができる。
【０１３０】
また、従来では、上記したように限度値で規制する処理、また、限度値をアームの伸縮状態等によって変更する処理等が必要となっていたのに対し、出力速度が入力速度の限度値を超えないことが保証されるため、そのような処理が不要となる。したがって、コントローラ１における処理の複雑化を招くことがない。
【０１３１】
また、位置指令θｉｎにより定められる入力速度は、ロボット２を構成するモータやハーモニックギア等による物理的な制約が存在しており、その制約を遵守した範囲内で実際に動作可能な値が設定されている。そして、ロボット２を自動運転する場合には、その物理的な制約の範囲内で可能な限り高速に動作をさせて作業効率を向上させることが行われる場合がある。そのため、自動運転時に出力速度が物理的な制約を超えるような状況になるおそれがあるものの、本実施形態のフィルタ設計であれば、出力速度が入力速度の限度値により制約されることから、出力速度が入力速度の限度値を超えるような動作が起きることがない。つまり、本実施形態のフィルタ設計は、手動運転だけでなく自動運転にも適用することができる。
【０１３２】
また、位置指令軌道θｏｕｔにより定められる出力加速度の限度値を、位置指令θｉｎにより定められる入力加速度の限度値により制約するように設計されている。上記したように、入力される位置指令は、そもそも安全性やロボット２の物理的な制約等に適合する範囲で設定されていることから、その位置指令により定まる入力加速度の限度値で出力加速度を制約することで、安全性を確保しつつ、実際に動作可能な位置指令軌道を生成することができる。そして、生成された位置指令軌道は、フィルタ特性に沿ったものであることから、設計者が意図した通りのフィルタ特性を得ることができる。
【０１３３】
また、位置指令フィルタ２１を特定の周波数成分を低減する有限インパルス応答フィルタで構成し、その有限インパルス応答フィルタの係数を全て０以上に設定することで、位置指令の特定の周波数成分を低減しつつ、限度値による制約を実現している。これにより、複数のフィルタを設けたりフィルタ処理の複雑化を招いたりすることなく、１つのＦＩＲフィルタで、特定の周波数成分の低減と限度値による制約とを実現することができる。このとき、個別にフィルタ処理を行うわけではないので、特定の周波数成分を低減するフィルタ特性が変化することがなく、また、フィルタ処理自体は１回で済むのでフィルタ処理による遅延の増加を招くこともない。
【０１３４】
また、位置指令フィルタ２１をｎ次の有限インパルス応答フィルタで構成し、その有限インパルス応答フィルタの係数をｍ＝ｎ／２を中心として対称に設定することで、位置指令の特定の周波数成分を低減しつつ、零位相差特性を実現している。これにより、フィルタ処理の複雑化を招くことなく、特定の周波数成分の低減と零位相差特性とを１つのフィルタで実現することができる。
【０１３５】
また、実施形態の（７）式のように、高周波成分の低減を目的としたＱｊと特定の周波数帯の低減を目的とするＱｆｋｓとによって与えられるＱを用いた評価関数による手法を採用すれば、高周波成分の低減と特定の周波数帯の低減とのバランスを操作することができる。そして、Ｑｆｋｓの設計時に指定する周波数帯にロボット２の固有振動数が含まれるようにすれば、ノミナル状態だけでなく、振動周波数が多少変動した場合であっても効果的に振動を抑制することができる。
【０１３６】
（第２実施形態）
以下、本発明の第２実施形態について、図７から図１１を参照しながら説明する。第２実施形態では、制御入力を生成する制御入力生成部を備え、上下限制約を与えつつ、制御入力に零位相差特性を与えている点において第１実施形態と異なっている。なお、制御対象となるロボット２は第１実施形態と共通するので、図１も参照しながら説明する。また、本実施形態の各式におけるａ、ｂ等の符号は、基本的に第１実施形態とは別のものを示している。
【０１３７】
本実施形態のコントローラ１は、図７に示すように、機能ブロックとして位置指令生成部３０、位置指令フィルタ３１、モータ制御部３２を備えており、フィードフォワード制御によりロボット２を制御している。
位置指令生成部３０は、第１実施形態と同様に、アームの位置を指令するための位置指令としてモータの回転角度を示す角度指令θｉｎを生成する。
【０１３８】
位置指令フィルタ３１は、詳細は後述するが、本実施形態では２つのＦＩＲフィルタで構成されており、角度指令θｉｎに基づいて、フィードフォワード位置指令θｒｅｆおよびフィードフォワード制御入力τｆｆを生成する。つまり、本実施形態の位置指令フィルタ２１は、位置指令生成部２０および制御入力生成部に相当する。なお、フィードフォワード制御入力τｆｆの具体例としては、トルク指令値が挙げられる。
モータ制御部３２は、フィードフォワード位置指令θｒｅｆおよびフィードフォワード制御入力τｆｆに基づいて、モータの回転角度、つまり、アームの関節接角度を制御する。
【０１３９】
図８に示す制御系において、ｎ（ｚ）／ｄ（ｚ）は制御対象の伝達関数を示し、Ｃ（ｚ）はフィードバック補償（以下、ＦＢ補償と称す）を示し、ＦＦａ（ｚ）／ＺｎおよびＦＦｂ（ｚ）／ＺｎはＦＩＲフィルタで構成されるフィードフォワード補償を示し、τは制御入力（トルク指令値）、θは制御対象となるアームの関節角度を示している。
【０１４０】
この制御系では、実際の制御対象とフィードフォワード補償のノミナルモデルとが一致する場合、角度指令θｉｎが目標位置に到達したｎステップ後に、関節角度θが角度指令θｉｎに到達することを補償するデッドビート制御が行われる。さらに、制御対象の振動周波数が変動する場合であっても、それを励起しないフィードフォワード制御入力τｆｆとフィードフォワード位置指令θｒｅｆとを生成するものである。なお、フィードフォワード制御入力τｆｆを生成する処理は従来のフィードフォワード補償の設計でも用いられている手法であるため、以下では概略を説明する。
【０１４１】
具体的には、フィードフォワード制御入力τｆｆを生成するＦＩＲフィルタでは、ＦＦａ（ｚ）は、次の（４８）式にて表すことができ、その場合、デッドビート制御を満足するための等式制約は、次の（４９）式によって与えられる。
【０１４２】
【数３３】
2016043417.tif 000035
【０１４３】
ここで、ｂ＝［ｂ０、ｂ１、・・・、ｂｎ−１、ｂｎ］Ｔ、Σ、Γは、ＦＦａ（ｚ）がｄ（ｚ）の根を全て含み、且つ、θｉｎからθまでの定数ゲインが１となることを保証するための係数行列である。なお、（４９）式の詳細については後述する。
さて、振動周波数の変動および高周波成分の低減を考慮するために、まず次の（５０）式のように評価関数Ｊを定める。
【０１４４】
【数３４】
2016043417.tif 000036
【０１４５】
ここで、Ｑは、第１実施形態で説明したものと同様の重み行列である。
これらの各式に基づいて、評価関数Ｊを（４９）式の制約のもとで最小化するｂを導出することで、ＦＦａ（ｚ）／Ｚｎのフィードフォワード補償を定めることができる。
一方、位置指令軌道に相当するフィードフォワード位置指令θｒｅｆを生成するＦＦｂ（ｚ）／Ｚｎは、制御対象とノミナルモデルとに誤差があるときだけフィードバック補償Ｃ（ｚ）が機能するように設計する必要がある。つまり、ＦＦａ（ｚ）／Ｚｎは、次の（５１）式によって定められることになる。
【０１４６】
【数３５】
2016043417.tif 000037
【０１４７】
ここで、ＦＦａ（ｚ）＝ｄ（ｚ）ＦＦｂ（ｚ）である。
さて、従来のフィードフォワード補償の設計では、ＦＦａ（ｚ）／Ｚｎを設計した後に、その結果にしたがって一意にＦＦｂ（ｚ）／Ｚｎが導出されていた。そのため、ＦＦａ（ｚ）／Ｚｎを設計する段階ではＦＦｂ（ｚ）／Ｚｎのフィルタ特性は全く考慮されておらず、このことは、角度指令θｉｎにより定められる入力速度および入力加速度についての上下限制約が、フィードフォワード位置指令θｒｅｆおよび関節角度θでは全く保証されていないことを意味している。さらに、従来では過渡状態における角度指令θｉｎとフィードフォワード位置指令θｒｅｆとの追従性についても、全く考慮されていなかった。
【０１４８】
そこで、本実施形態では、図８に示す２つのＦＩＲフィルタＦＦａ（ｚ）／ｚｎ、ＦＦｂ（ｚ）／ｚｎについて、従来とは異なる設計手法を用いることで、前述の上下限制約を満足すること、さらには、零位相差特性を有することを実現している。なお、説明の簡略化のために、次の（３６）式に示す多項式を定義しておく。
【０１４９】
【数３６】
2016043417.tif 000038
【０１５０】
本実施形態では、入力速度および入力加速度の上下限制約を満足するために、従来の手法の最適化問題である（４９）式および（５０）式に新たに不等式制約を加えている。具体的には、係数行列Ｍｂを用いると、（５２）式に示した多項式の係数行列ｃは、次の（５３）式に示すように、変数行列ｂの一次式で表される。
【０１５１】
【数３７】
2016043417.tif 000039
【０１５２】
ここで、ｃ＝［ｃ０、ｃ１、・・・、ｃｎ−１、ｃｎ］Ｔである。
さて、第１実施形態で説明したように、ＦＦｂ（ｚ）の係数が全て０以上であれば、フィードフォワード位置指令θｒｅｆにより定められる出力速度および出力加速度は、角度指令θｉｎにより定められる入力速度および入力加速度の上下限制約を満足する。すなわち、上下限制約を満足するためには、次の（５４）式を満足すればよい。
【０１５３】
【数３８】
2016043417.tif 000040
【０１５４】
換言すると、この（５４）式を満足する係数行列Ｍｂを導出すれば、上下限制約を満足するＦＩＲフィルタを設計することができる。以下、係数行列Ｍｂの導出について説明する。
いま、ｌ個のｄ（ｚ）の根が、それぞれλｄ１、λｄ２、・・・、λｄｌであるとする。このとき、ＦＦａｎ（ｚ）の係数行列ｄは、次の（５５）式から（５７）式を用いて表すことができる。
【０１５５】
【数３９】
2016043417.tif 000041
【０１５６】
これらにより、係数行列ｃは、次の（５８）式および（５９）式のように表される。
【０１５７】
【数４０】
2016043417.tif 000042
【０１５８】
このように、（５４）式に示す不等式制約を（４９）式および（５０）式の最適化問題に加えることで、上下限制約を満足することができる。
【０１５９】
さて、第１実施形態で説明したように、連続的に軌跡に追従させるＣＰ動作の場合には、位相遅れが問題となる。そのため、本実施形態では、上下限制約を満たしつつ、零位相差特性を与えている。なお、零位相差特性を与えるための条件は、第１実施形態にて説明したようにインパルス応答がｍ（＝ｎ／２）ステップ目を中心として対象であること、および、ｍステップ分の未来値が必要となることであり、これらをＦＦｂ（ｚ）／Ｚｎに与えることで、零位相差特性を与えることができる。
具体的には、例えば次の（６０）式の等式制約を与えることで、インパルス応答を対象とすることができる。
【０１６０】
【数４１】
2016043417.tif 000043
【０１６１】
なお、（６０）式の場合、ＦＩＲフィルタのｎ個の設計自由度の半分を占めてしまうため、第１実施形態で説明したように、次の（６１）式のＱｚｐを定義し、（５０）式のＱにＱｚｐを加えたＱｐを（６２）式のように新たに定義し、（５０）式の評価関数Ｊの代わりに（６３）式を新たな評価関数とする。
【０１６２】
【数４２】
2016043417.tif 000044
【０１６３】
この（６３）式の評価関数を（４９）式の等式制約のもとで最小化することにより、第１実施形態と同様に、零位相差特性を備えたフィードフォワード補償を実現できる。また、（５４）式の不等式制約を与えることで、上下限制約を満足することができる。つまり、零位相差特性を有することにより位相遅れが少ない状態で、且つ、上下限制約を満足することで安全性を確保した状態で、フィードフォワード制御を行うことができる。
【０１６４】
さて、上記したように、フィードフォワード制御をする場合、デッドビート制御を行うためには（４９）式に示す等式制約が必要となる。
図８に示す制御対象への制御入力τから制御量θ（アームの実際の関節角度）までの伝達関数は、次の（６４）式のように表され、そのとき、フィードフォワード制御入力τｆｆを導出するＦＩＲフィルタは、位置指令（角度指令θｉｎ）入力としており、次の（６５）式のように表すことができる。
【０１６５】
【数４３】
2016043417.tif 000045
【０１６６】
いま、（６５）式のフィードフォワード制御入力τｆｆが制御入力τとした与えられる場合、それに応答する位置応答θ（ｚ）は、次の（６６）式にて表される。
【０１６７】
【数４４】
2016043417.tif 000046
【０１６８】
さて、前述の通りＦＦａ（ｚ）はｄ（ｚ）の根を全て含み、且つ、角度指令θｉｎから関節角度θまでの定常ゲインが１となることを補償するとする。
まず、ＦＦａ（ｚ）がｄ（ｚ）の根（λｄ１、λｄ２、・・・、λｄｌ）を全て含むとすると、次の（６７）式が成立し、ＦＦａ（ｚ）がｄ（ｚ）の根を全て含む条件は、次の（６８）式の等式条件により定められる。ただし、Σａ、ｂ、Γａは、次の（６９）式の通りである。
【０１６９】
【数４５】
2016043417.tif 000047
【０１７０】
このとき、角度指令θｉｎから関節角度θまでの定常ゲインが１となる条件は、次の（７０）式で定められる。ただし、Σｂ、Γｂは、次の（７１）式に示す関係の通りである。
【０１７１】
【数４６】
2016043417.tif 000048
【０１７２】
これらの式により、（６８）式と（７０）式とを合わせた次の（７２）式および（７３）式に示す等式条件が成立すれば、デッドビート制御を行えることが分かる。つまり、フィードバック補償における遅延を補填することができ、応答性を高めることができる。
【０１７３】
【数４７】
2016043417.tif 000049
【０１７４】
次に、このように設計したＦＩＲフィルタの具体例について、図９から図１１を参照しながら説明する。
図９は、図１に示したモデルにおいて２軸目を０°から１４５°に回転させる場合を想定して、制御入力τから２軸目の関節角度θまでの周波数応答を示している。この図９からは、グラフＧ８０にて示すように、約３０Ｈｚに振動モードを有することが確認できる。この具体例では、この周波数応答をサンプリング周期を２５０μｓｅｃで離散化し、離散系制御対象ｎ（ｚ）／ｄ（ｚ）に対してｎ＝１２４のフィードフォワード補償を設計している。
【０１７５】
図１０は、本実施形態の手法で設計したフィードフォワード補償の周波数応答を示しており、このうち図１０（Ａ）は、フィードフォワード制御入力τｆｆを生成するＦＦａ（ｚ）／ｎｚを示し、図１０（Ｂ）は、フィードフォワード位置指令θｒｅｆを生成するＦＦｂ（ｚ）／ｎｚを示している。
この図１０（Ａ）にグラフＧ９にて示すように、振動モードの中心周波数である約３０Ｈｚにおいて、ゲインが低減されており、振動を抑制する効果を得ていることが示されている。また、図１０（Ｂ）にグラフＧ９３にて示すように、フィードフォワード位置指令θｒｅｆは、低周波数帯における位相遅れがほぼ零となっており、零位相差特性を有するフィードフォワード補償であることを示している。
【０１７６】
そして、図１１に示すシミュレーション結果では、グラフＧ１１０にて示す実施形態の場合、角度はグラフＧ１００にて示す入力にほぼ一致した状態で追従しており、零位相差特性を確認することができる。また、グラフＧ１１１にて示す実施形態の出力速度は、グラフＧ１０１にて示す入力速度の上限および下限を超えないように変化しているとともに、グラフＧ１１２にて示す実施形態の出力加速度も、グラフＧ１０２にて示す入力加速度の上限および下限を超えないように変化している。つまり、上下限制約を満足していることが確認できる。
【０１７７】
このように、本実施形態では、ＦＩＲフィルタの係数を０以上に設定することで上下限制約を満足し、ＦＩＲフィルタの係数を対称に設定することで擬似的に零位相差特性を実現しつつ、デッドビート制御を行うことができる。
【０１７８】
以上説明した本実施形態によれば、次のような効果を得ることができる。
本実施形態では、位置指令軌道θｏｕｔにより定められる出力速度の限度値を位置指令θｉｎにより定められる入力速度の限度値により制約するように設計され、また、位置指令軌道θｏｕｔにより定められる出力加速度の限度値を、位置指令θｉｎにより定められる入力加速度の限度値により制約するように設計されていることから、第１実施形態と同様の効果を得ることができる。
すなわち、本実施形態においても、出力速度が安全基準速度を超えることがないことから手動運転時の安全性を確保することができ、外部からの強制的な制限を加える必要がなくなることからフィルタ特性に影響を与えることがなく、設計者の意図の通りのフィルタ特性を得ることができ、所望の振動抑制効果を得ることができる。また、処理の複雑化を招くことがなく、手動運転だけでなく自動運転にも効果を得ることができる。
【０１７９】
また、位置指令フィルタ２１を特定の周波数成分を低減する有限インパルス応答フィルタで構成し、その有限インパルス応答フィルタの係数を全て０以上に設定することで、位置指令の特定の周波数成分を低減しつつ限度値による制約を実現し、有限インパルス応答フィルタの係数をｍ＝ｎ／２を中心として対称に設定することで、零位相差特性を実現している。これにより、フィルタ処理の複雑化を招くことなく、特定の周波数成分の低減と零位相差特性とを１つのフィルタで実現することができる。
【０１８０】
また、位置指令フィルタ３１において、位置指令軌道相当するフィードフォワード位置指令θｒｅｆに追従させるフィードフォワード制御を行うためのフィードフォワード制御入力τｆｆを生成している。フィードバック補償を行うとどうしても遅延が生じるが、フィードフォワード位置指令θｒｅｆに追従させるためのフィードフォワード制御入力τｆｆを生成することで、フィードバック補償による遅延を補填することができ、応答性を高めることができる。
【０１８１】
（その他の実施形態）
本発明は、各実施形態にて例示したものに限定されることなく、例えば次のように変形あるいは拡張することができる。
各実施形態で示した数値は一例であり、それらに限定されるものではない。
各実施形態では位置指令軌道により定められる出力速度と出力加速度との双方に上下限制約を設けたが、出力速度と出力加速度のいずれか一方に上下限制約を設ける構成としてもよい。
第１実施形態の構成に、移動体を制御する際の入力であって位置指令軌道に追従させるための制御入力つまりトルク指令を生成する制御入力生成部を設けてもよい。これにより、応答性を高めることができる。
【０１８２】
また、制御入力生成部を構成するＦＩＲフィルタにおいて、制御入力により定められる出力トルクの限度値を、位置指令により定められる入力トルクの限度値により制約してもよい。あるいは、第２実施形態のフィードフォワード制御入力τｆｆに対して、出力トルクの限度値を位置指令により定められる入力トルクの限度値により制約するように設計してもよい。これらにより、出力トルクは、入力トルクに追従する範囲内に収まり、上下限制約を満足することができる。すなわち、過度の出力トルクに対して外部からの制限等を加えること等が不要となり、設計者の意図の通りのフィルタ特性を得ることができる。
【０１８３】
第２実施形態では有限インパルス応答フィルタのフィルタ係数を対称に設定することで完全な零位相差特性を実現する構成を例示したが、フィルタ特性を決定する際の最適化パラメータの一つとしてフィルタ係数の対称性を設定可能とし、フィルタ係数の対称性を設定することにより擬似的な零位相差特性を実現する構成としてもよい。例えば、ＰＴＰ動作であれば、零位相差特性よりも応答性や移動速度が重視されることが想定される。そのため、フィルタ係数の対称性を設定可能とすることにより、零位相差特性を重視するか応答性や移動速度を重視するかを選択することが可能となる。これにより、フィルタ設計の自由度を向上させることができる。
【符号の説明】
【０１８４】
図面中、１はコントローラ（ロボットの制御装置）、２はロボット、５は第１アーム（移動体）、６は第２アーム（移動体）、２０は位置指令生成部、２１は位置指令フィルタ（位置指令軌道生成部）、２２はモータ制御部（制御部）、３０は位置指令生成部、３１は位置指令フィルタ（位置指令軌道生成部）、３２はモータ制御部（制御部）を示す。
【図１】
2016043417.tif 000050
【図２】
2016043417.tif 000051
【図３】
2016043417.tif 000052
【図４】
2016043417.tif 000053
【図５】
2016043417.tif 000054
【図６】
2016043417.tif 000055
【図７】
2016043417.tif 000056
【図８】
2016043417.tif 000057
【図９】
2016043417.tif 000058
【図１０】
2016043417.tif 000059
【図１１】
2016043417.tif 000060
【図１２】
2016043417.tif 000061
【手続補正書】
【提出日】平成26年9月3日(2014.9.3)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】００２９
【補正方法】変更
【補正の内容】
【００２９】
そこで、発明者らは、研究を重ねた結果、もともとロボットの制御装置に設けられている特定の周波数成分を低減するためのｎ次の有限インパルス応答フィルタに対し、その係数をｍ＝ｎ／２を中心として対称に設定することで、特定の周波数成分の低減と零位相差特性とを実現することができることを見いだした。このような全く新規なフィルタの設計思想を得たことにより、本発明は、フィルタ処理の複雑化を招くことなく、特定の周波数成分の低減と零位相差特性とを１つのフィルタで実現することができる。また、個別にフィルタを設ける場合とは異なり特定の周波数成分を低減するフィルタ特性が変化することがなく、さらには、フィルタ処理自体は１回で済むのでフィルタ処理による遅延の増加を招くことがない。
請求項６記載の発明では、有限インパルス応答フィルタは、そのフィルタ特性を決定する際の最適化パラメータの一つとしてフィルタ係数の対称性を設定可能であり、当該フィルタ係数の対称性を設定することにより擬似的な零位相差特性を実現する。フィルタ係数を完全に対称に設定した場合には、上記した請求項５記載の発明のように完全な零位相差特性を備えることになる。その一方で、零位相差特性よりも応答性や移動速度を重視したい場合には、完全な零位相差特性が必ずしも必要とされないことも想定される。そこで、フィルタ係数の対称性を設定可能とすることで、零位相差特性を重視するか応答性や移動速度を重視するかを選択することが可能となり、フィルタ設計の自由度を向上させることができる。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】０１４８
【補正方法】変更
【補正の内容】
【０１４８】
そこで、本実施形態では、図８に示す２つのＦＩＲフィルタＦＦａ（ｚ）／ｚｎ、ＦＦｂ（ｚ）／ｚｎについて、従来とは異なる設計手法を用いることで、前述の上下限制約を満足すること、さらには、零位相差特性を有することを実現している。なお、説明の簡略化のために、次の（５２）式に示す多項式を定義しておく。
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】０１６２
【補正方法】変更
【補正の内容】
【０１６２】
【数４２】
2016043417.tif 000062
【手続補正４】
【補正対象書類名】図面
【補正対象項目名】図３
【補正方法】変更
【補正の内容】
【図３】
2016043417.tif 000063