(57)【要約】      （修正有）【課題】複数のサーボ制御系の各々について、その可動範囲を有効に利用する。【解決手段】コントローラ１０は、第一逆運動学軌道ＳＰから位相遅延が発生しないようにして高周波成分を除去した補正後軌道ＳＰｆを、第一サーボ制御系２０についての指令軌道とする。第二サーボ制御系３０についての指令軌道として、前記高周波成分に対応する軌道を含む第二指令軌道を生成する第二軌道生成部と、を備える。【選択図】図１
2019021086.tif 000002
【特許請求の範囲】
【請求項１】
基準軌道から位相遅延が発生しないようにして高周波成分を除去した第一指令軌道を、第一サーボ制御系についての指令軌道とする第一軌道生成部と、
第二サーボ制御系についての指令軌道として、前記高周波成分に対応する軌道を含む第二指令軌道を生成する第二軌道生成部と、を備えることを特徴とする制御装置。
【請求項２】
前記第一軌道生成部は、前記基準軌道に対し、時間軸の順方向と逆方向との両方向にローパスフィルタ処理を行なって前記第一指令軌道を生成することを特徴とする請求項１に記載の制御装置。
【請求項３】
前記第一軌道生成部は、前記基準軌道に対し、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって前記第一指令軌道を生成することを特徴とする請求項２に記載の制御装置。
【請求項４】
前記第一軌道生成部は、前記基準軌道を示す曲線の、上下の包絡線の平均から、前記第一指令軌道を生成することを特徴とする請求項１に記載の制御装置。
【請求項５】
前記第一サーボ制御系の動特性モデルを用いて、前記第一指令軌道に対応する前記第一サーボ制御系の応答を予測する予測部をさらに備えることを特徴とする請求項１から４のいずれか１項に記載の制御装置。
【請求項６】
前記予測部は、前記第一サーボ制御系の動特性モデルを用いて、前記第一指令軌道から生成する第一指令値に対する前記第一サーボ制御系の出力である制御量を予測し、
前記予測部によって予測された制御量と、前記第一サーボ制御系からフィードバック情報として取得する前記第一サーボ制御系の制御量の実測値と、を用いたモデル予測制御によって補正した前記第一指令値を、前記第一サーボ制御系に出力する指示部をさらに備えることを特徴とする請求項５に記載の制御装置。
【請求項７】
前記第二軌道生成部は、前記予測部によって予測された前記第一サーボ制御系の応答を用いて生成する中間軌道と、前記基準軌道と、の誤差に対応する軌道を含む前記第二指令軌道を生成することを特徴とする請求項５に記載の制御装置。
【請求項８】
基準軌道から位相遅延が発生しないようにして高周波成分を除去した第一指令軌道を、第一サーボ制御系についての指令軌道とする第一軌道生成ステップと、
第二サーボ制御系についての指令軌道として、前記高周波成分に対応する軌道を含む第二指令軌道を生成する第二軌道生成ステップと、を含むことを特徴とする制御装置の制御方法。
【請求項９】
請求項１から７のいずれか１項に記載の制御装置としてコンピュータを機能させるための情報処理プログラムであって、前記各部としてコンピュータを機能させるための情報処理プログラム。
【請求項１０】
請求項９に記載の情報処理プログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サーボドライバ等のフィードバック制御系に指令値を出力する制御装置等に関する。
【背景技術】
【０００２】
複数のサーボ制御系の各々について、目標軌道から前記サーボ制御系ごとの指令軌道を生成し、前記指令軌道から生成した指令値を前記複数のサーボ制御系の各々に対する制御周期ごとに出力して、前記複数のサーボ制御系を協調制御する制装置等が知られている。
【０００３】
例えば、下掲の特許文献１には、（１）目標軌道に対する逆運動学演算の結果にローパスフィルタ処理を行なって生成した第一軌道を第一サーボ制御系についての指令軌道とし、（２）前記第一軌道に対する順運動学演算の結果と目標軌道との誤差から、第二サーボ制御系についての指令軌道を生成する制御装置が開示されている。なお、以下の説明においては逆運動学を「インバースキネマティクス」と称し、順運動学を「フォワードキネマティクス」と称することがある。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１２／００９５５９９号（２０１２年４月１９日公開）
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上述のような従来技術は、前記第一サーボ制御系の追従性向上のために高周波成分を除去するローパスフィルタ処理によって前記第一軌道に位相遅れ（位相遅延）が発生し、発生した位相遅延を前記第二サーボ制御系に補償させることになる。つまり、上述のような従来技術は、前記第一サーボ制御系の追従性を向上させる代わりに、本来は前記第一サーボ制御系が実現すべき軌道の一部を前記第二サーボ制御系に実現させることになる。したがって、上述のような従来技術は、前記第二サーボ制御系の可動範囲を有効に利用することができないという問題がある。
【０００６】
本発明の一態様は、複数のサーボ制御系を協調させて制御する制御装置等において、前記第一サーボ制御系の追従性を維持しつつ、複数のサーボ制御系の各々について、その可動範囲を有効に利用することを目的とする。
【課題を解決するための手段】
【０００７】
上記の課題を解決するために、本発明の一態様に係る制御装置は、基準軌道から位相遅延が発生しないようにして高周波成分を除去した第一指令軌道を、第一サーボ制御系についての指令軌道とする第一軌道生成部と、第二サーボ制御系についての指令軌道として、前記高周波成分に対応する軌道を含む第二指令軌道を生成する第二軌道生成部と、を備えることを特徴としている。
【０００８】
前記の構成によれば、前記制御装置は、前記基準軌道からの位相遅延が発生しないようにして前記基準軌道から高周波成分を除去して、前記第一サーボ制御系についての指令軌道である前記第一指令軌道を生成する。
【０００９】
ここで、前記基準軌道からローパスフィルタによって高周波成分を除去した場合、高周波成分除去後の前記基準軌道は、高周波成分除去前の前記基準軌道に比べて位相遅れ（位相遅延）が発生することが知られている。そして、発生した位相遅延は前記第二サーボ制御系に補償させることになり、つまり、本来は前記第一サーボ制御系が実現すべき軌道の一部を前記第二サーボ制御系に実現させることになり、前記第二サーボ制御系の可動範囲を有効に利用することができない。
【００１０】
これに対して、前記制御装置は、前記基準軌道に対する位相遅延が発生しないようにして前記基準軌道から高周波成分を除去して前記第一指令軌道を生成するので、前記第一サーボ制御系は、高周波成分を除去した後の前記第一指令軌道に十分追従することができる。つまり、前記制御装置は、前記第一サーボ制御系の追従性を維持することができる。
【００１１】
また、前記制御装置は、前記高周波成分の除去に伴って従来発生していた位相遅延の発生を防ぐので、前記第一サーボ制御系が本来実現すべき軌道の一部を前記第二サーボ制御系に補償させる必要がない。つまり、前記制御装置は、前記第二サーボ制御系の可動範囲を有効に利用することができる。
【００１２】
以上に整理した通り、前記制御装置は、高周波成分の除去によって前記第一サーボ制御系の追従性を維持しつつ、位相遅延の発生を防ぐことで前記第二サーボ制御系の可動範囲を有効に利用することができる。したがって、前記制御装置は、前記第一サーボ制御系の追従性を維持しつつ、前記第一サーボ制御系および前記第二サーボ制御系の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【００１３】
本発明に係る制御装置において、前記第一軌道生成部は、前記基準軌道に対し、時間軸の順方向と逆方向との両方向にローパスフィルタ処理を行なって前記第一指令軌道を生成してもよい。
【００１４】
前記の構成によれば、前記制御装置は、前記基準軌道に対し、時間軸の順方向と逆方向との両方向にローパスフィルタ処理を行なうことで、前記基準軌道からの位相遅延が発生しないようにして前記基準軌道から高周波成分を除去して前記第一指令軌道を生成する。つまり、前記制御装置は、前記基準軌道にローパスフィルタ処理を行なう場合と異なり、位相遅延を発生させないので、前記第一サーボ制御系が本来実現すべき軌道の一部を前記第二サーボ制御系に補償させる必要がない。
【００１５】
したがって、前記制御装置は、高周波成分の除去によって前記第一サーボ制御系の追従性を維持しつつ、位相遅延の発生を防ぐことで前記第二サーボ制御系の可動範囲を有効に利用することができる。つまり、前記制御装置は、前記第一サーボ制御系の追従性を維持しつつ、前記第一サーボ制御系および前記第二サーボ制御系の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【００１６】
本発明に係る制御装置において、前記第一軌道生成部は、前記基準軌道に対し、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって前記第一指令軌道を生成してもよい。
【００１７】
前記の構成によれば、前記制御装置は、前記基準軌道に対し、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって前記第一指令軌道を生成する。
【００１８】
ここで、前記基準軌道に、時間軸の順方向から逆方向の順に前記ローパスフィルタ処理を行なった場合、前記第一指令軌道について、スタート時点（ｔ＝０の時点）において前記基準軌道に対するデータ飛びが発生する。
【００１９】
これに対して、前記制御装置は、前記基準軌道に、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって前記第一指令軌道を生成する。したがって、前記制御装置は、前記第一指令軌道について、スタート時点（ｔ＝０の時点）において前記基準軌道に対するデータ飛びが発生することを防ぐことができるとの効果を奏する。
【００２０】
本発明に係る制御装置において、前記第一軌道生成部は、前記基準軌道を示す曲線の、上下の包絡線の平均から、前記第一指令軌道を生成してもよい。
【００２１】
前記の構成によれば、前記制御装置は、前記基準軌道を示す曲線の、上下の包絡線の平均から、前記第一指令軌道を生成することにより、前記基準軌道からの位相遅延が発生しないようにして高周波成分を除去して前記第一指令軌道を生成する。つまり、前記制御装置は、高周波成分の除去によって前記第一サーボ制御系の追従性を維持しつつ、位相遅延の発生を防ぐことで前記第二サーボ制御系の可動範囲を有効に利用することができる。
【００２２】
したがって、前記制御装置は、前記第一サーボ制御系の追従性を維持しつつ、前記第一サーボ制御系および前記第二サーボ制御系の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【００２３】
本発明に係る制御装置は、前記第一サーボ制御系の動特性モデルを用いて、前記第一指令軌道に対応する前記第一サーボ制御系の応答を予測する予測部をさらに備えてもよい。
【００２４】
前記の構成によれば、前記制御装置は、前記第一サーボ制御系の動特性モデルを用いて、前記第一指令軌道に対応する前記第一サーボ制御系の応答を予測する。
【００２５】
したがって、前記制御装置は、前記第一サーボ制御系の動特性モデルを用いて、前記第一指令軌道に対応する前記第一サーボ制御系の応答を予測することができるとの効果を奏する。
【００２６】
本発明に係る制御装置において、前記予測部は、前記第一サーボ制御系の動特性モデルを用いて、前記第一指令軌道から生成する第一指令値に対する前記第一サーボ制御系の出力である制御量を予測し、本発明に係る制御装置は、前記予測部によって予測された制御量と、前記第一サーボ制御系からフィードバック情報として取得する前記第一サーボ制御系の制御量の実測値と、を用いたモデル予測制御によって補正した前記第一指令値を、前記第一サーボ制御系に出力する指示部をさらに備えてもよい。
【００２７】
前記の構成によれば、前記制御装置は、前記第一サーボ制御系の動特性モデルを用いて予測した制御量と、前記第一サーボ制御系の制御量の実測値と、を用いたモデル予測制御によって補正した前記第一指令値を、前記第一サーボ制御系に出力する。
【００２８】
したがって、前記制御装置は、前記第一サーボ制御系の前記第一指令軌道への追従性能を向上させることができ、さらに、前記第一サーボ制御系および前記第二サーボ制御系の全体の追従性能を向上させることができるとの効果を奏する。
【００２９】
本発明に係る制御装置において、前記第二軌道生成部は、前記予測部によって予測された前記第一サーボ制御系の応答を用いて生成する中間軌道と、前記基準軌道と、の誤差に対応する軌道を含む前記第二指令軌道を生成してもよい。
【００３０】
前記の構成によれば、前記制御装置は、前記予測部によって予測された前記第一サーボ制御系の応答を用いて生成する中間軌道と、前記基準軌道と、の誤差に対応する軌道を含む前記第二指令軌道を生成する。
【００３１】
したがって、前記制御装置は、前記第一サーボ制御系の応答遅れを前記第二サーボ制御系によって補償することができ、前記第一サーボ制御系および前記第二サーボ制御系の全体の追従性能を向上させることができるとの効果を奏する。
【００３２】
上記の課題を解決するために、本発明に係る制御方法は、基準軌道から位相遅延が発生しないようにして高周波成分を除去した第一指令軌道を、第一サーボ制御系についての指令軌道とする第一軌道生成ステップと、第二サーボ制御系についての指令軌道として、前記高周波成分に対応する軌道を含む第二指令軌道を生成する第二軌道生成ステップと、を含むことを特徴としている。
【００３３】
前記の方法によれば、前記制御方法は、前記基準軌道からの位相遅延が発生しないようにして前記基準軌道から高周波成分を除去して、前記第一サーボ制御系についての指令軌道である前記第一指令軌道を生成する。
【００３４】
ここで、前記基準軌道からローパスフィルタによって高周波成分を除去した場合、高周波成分除去後の前記基準軌道は、高周波成分除去前の前記基準軌道に比べて位相遅れが発生することが知られている。そして、発生した位相遅延は前記第二サーボ制御系に補償させることになり、つまり、本来は前記第一サーボ制御系が実現すべき軌道の一部を前記第二サーボ制御系に実現させることになり、前記第二サーボ制御系の可動範囲を有効に利用することができない。
【００３５】
これに対して、前記制御方法は、前記基準軌道に対する位相遅延が発生しないようにして前記基準軌道から高周波成分を除去して前記第一指令軌道を生成するので、前記第一サーボ制御系は、高周波成分を除去した後の前記第一指令軌道に十分追従することができる。つまり、前記制御方法は、前記第一サーボ制御系の追従性を維持することができる。
【００３６】
また、前記制御方法は、前記高周波成分の除去に伴って従来発生していた位相遅延の発生を防ぐので、前記第一サーボ制御系が本来実現すべき軌道の一部を前記第二サーボ制御系に補償させる必要がない。つまり、前記制御方法は、前記第二サーボ制御系の可動範囲を有効に利用することができる。
【００３７】
以上に整理した通り、前記制御方法は、高周波成分の除去によって前記第一サーボ制御系の追従性を維持しつつ、位相遅延の発生を防ぐことで前記第二サーボ制御系の可動範囲を有効に利用することができる。したがって、前記制御方法は、前記第一サーボ制御系の追従性を維持しつつ、前記第一サーボ制御系および前記第二サーボ制御系の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【発明の効果】
【００３８】
本発明の一態様によれば、複数のサーボ制御系を協調させて制御する制御装置等において、前記第一サーボ制御系の追従性を維持しつつ、複数のサーボ制御系の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【図面の簡単な説明】
【００３９】
【図１】本発明の実施形態１に係るコントローラ等の要部構成等を示すブロック図である。
【図２】図１のコントローラを含む制御システムの全体概要を示す図である。
【図３】図１のコントローラが実行する処理の概要を示すフロー図である。
【図４】高周波成分の除去方法に応じて発生する位相遅延およびデータ飛びについて説明する図である。
【図５】図２の制御システム等を用いて実施する制御テストの内容を示す図である。
【図６】図５に示す制御テストについて、図１のコントローラによって制御された第一アクチュエータおよび第二アクチュエータの各々の位置、位置偏差、およびトルクの変化を示す図である。
【図７】図５に示す制御テストについて、図１のコントローラによって制御された第一アクチュエータおよび第二アクチュエータの合計の位置および位置偏差の変化を示す図である。
【図８】本発明の実施形態２に係るコントローラ等の要部構成等を示すブロック図である。
【図９】図８のコントローラが、目標軌道に逆運動学演算を行なって生成した第一演算軌道を示す曲線の上下包絡線の平均から指令軌道を生成する方法を説明する図である。
【図１０】本発明の実施形態３に係るコントローラ等の要部構成等を示すブロック図である。
【図１１】図１０のコントローラを含む制御システムの全体概要を示す図である。
【図１２】図１０のコントローラが実行する処理の概要を示すフロー図である。
【図１３】図５に示す制御テストについて、図１０のコントローラによって制御された第一アクチュエータおよび第二アクチュエータの各々の位置、位置偏差、モデル予測誤差、およびトルクの変化を示す図である。
【図１４】図５に示す制御テストについて、図１０のコントローラによって制御された第一アクチュエータおよび第二アクチュエータの合計の位置および位置偏差の変化を示す図である。
【図１５】本発明の実施形態４に係るコントローラ等の要部構成等を示すブロック図である。
【図１６】図１５のコントローラを含む制御システムの全体概要を示す図である。
【図１７】図１５のコントローラが実行する処理の概要を示すフロー図である。
【図１８】図５に示す制御テストについて、従来の協調制御コントローラによって制御された第一アクチュエータおよび第二アクチュエータの各々の位置、位置偏差、およびトルクの変化を示す図である。
【図１９】図５に示す制御テストについて、従来の協調制御コントローラによって制御された第一アクチュエータおよび第二アクチュエータの合計の位置および位置偏差の変化を示す図である。
【発明を実施するための形態】
【００４０】
〔実施形態１〕
以下、本発明の実施形態１について、図１から図７、図１８、および図１９に基づいて詳細に説明する。図中同一または相当部分には同一符号を付してその説明は繰返さない。本発明の一態様に係るコントローラ１０（制御装置）についての理解を容易にするため、先ず、コントローラ１０を含む制御システム１の概要を、図２を用いて説明する。
【００４１】
（制御システムの概要）
図２は、コントローラ１０を含む制御システム１の概要を示す図である。図２に例示する制御システム１は、上位コントローラであるコントローラ１０と、コントローラ１０により協調制御される第一サーボ制御系２０および第二サーボ制御系３０と、を含む。第一サーボ制御系２０は、第一サーボドライバ２１と、第一サーボドライバ２１によって駆動を制御される第一アクチュエータ２２と、を含むフィードバック制御系である。同様に、第二サーボ制御系３０は、第二サーボドライバ３１と、第二サーボドライバ３１によって駆動を制御される第二アクチュエータ３２と、を含むフィードバック制御系である。
【００４２】
コントローラ１０と、第一サーボドライバ２１および第二サーボドライバ３１の各々とは通信可能に接続されており、その接続方式は、任意の有線接続方式または無線接続方式である。
【００４３】
第一サーボドライバ２１は、軸ごとの指令値Ｐｃ（ｉ）をコントローラ１０から受信し、制御対象である第一アクチュエータ２２の軸ごとの出力（つまり、軸ごとの制御量）が軸ごとの指令値Ｐｃ（ｉ）に追従するように、フィードバック制御を行う。第一サーボドライバ２１の制御周期は、例えば１／１２ｍｓである。
【００４４】
第一アクチュエータ２２は、第二アクチュエータ３２よりも可動範囲は広いが、第二アクチュエータ３２の動作速度よりも動作速度が低速なアクチュエータであり、例えば、サーボモータまたはステッピングモータである。第一サーボドライバ２１は、コントローラ１０からの指令値Ｐｃに従って第一アクチュエータ２２を駆動する。第一サーボドライバ２１は、コントローラ１０からの指令値Ｐｃを目標値に設定し、実測値をフィードバック値として、第一アクチュエータ２２に対しフィードバック制御を行う。すなわち、第一サーボドライバ２１は、実測値が目標値に近づくように第一アクチュエータ２２を駆動するための電流を調整する。なお、第一サーボドライバ２１は、サーボモータアンプと称されることもある。
【００４５】
第二サーボドライバ３１は、軸ごとの指令値Ｓｃ（ｉ）をコントローラ１０から受信し、制御対象である第二アクチュエータ３２の軸ごとの出力（つまり、軸ごとの制御量）が軸ごとの指令値Ｓｃ（ｉ）に追従するように、フィードバック制御を行う。第二サーボドライバ３１の制御周期は、例えば１／１２ｍｓである。ただし、１／１２ｍｓという制御周期は単なる例示に過ぎず、第二サーボドライバ３１の制御周期はより短く（速く）てもよく、例えば、１０μｓであってもよい。
【００４６】
第二アクチュエータ３２は、第一アクチュエータ２２よりも高速な動作が可能だが、第一アクチュエータ２２よりも可動範囲が狭いアクチュエータであり、例えば、ピエゾアクチュエータまたはガルバノスキャナである。第二サーボドライバ３１は、コントローラ１０からの指令値Ｓｃに従って第二アクチュエータ３２を駆動する。第二サーボドライバ３１は、コントローラ１０からの指令値Ｓｃを目標値に設定し、実測値をフィードバック値として、第二アクチュエータ３２に対しフィードバック制御を行う。すなわち、第二サーボドライバ３１は、実測値が目標値に近づくように第二アクチュエータ３２を駆動するための電流を調整する。なお、第二サーボドライバ３１は、サーボモータアンプと称されることもある。
【００４７】
なお、上述の通り、第二アクチュエータ３２は、第一アクチュエータ２２よりも高速な動作が可能であり、以下の説明においては、第二アクチュエータ３２を「高速アクチュエータ」と称し、第一アクチュエータ２２を「低速アクチュエータ」と称することがある。
【００４８】
コントローラ１０は、第一サーボ制御系２０および第二サーボ制御系３０を含む制御システム１の全体を制御し、例えば、プログラマブルコントローラ（ＰＬＣ、Programmable Logic Controller）である。コントローラ１０は、（１）コントローラ１０は、低速アクチュエータ（第一アクチュエータ２２）についての指令軌道から高周波成分を除去することにより、低速アクチュエータの追従性を向上させる。また、（２）コントローラ１０は、低速アクチュエータについての指令軌道に位相遅延が発生するのを防ぐことにより、高速アクチュエータ（第二アクチュエータ３２）の動作範囲を節約する。具体的には、コントローラ１０は、以下の（処理１）および（処理２）を実行することにより、第一サーボ制御系２０と第二サーボ制御系３０とを協調させて制御する。
【００４９】
（処理１）コントローラ１０は、第一サーボ制御系２０および第二サーボ制御系３０の各々について、目標軌道Ｔｔを用いて指令軌道を生成する。具体的には、コントローラ１０は、逆運動学演算および順運動学演算を用いて、目標軌道Ｔｔから「第一サーボ制御系２０についての指令軌道」と「第二サーボ制御系３０についての指令軌道」とを生成する。すなわち、コントローラ１０は、目標軌道Ｔｔに逆運動学演算を行なって第一サーボ制御系２０の軸ごとに生成した第一逆運動学軌道ＳＰ（ｉ）（不図示）から高周波成分を除去して、「第一サーボ制御系２０についての指令軌道」として補正後軌道ＳＰｆ（ｉ）（不図示）を生成する。なお、図２においては図示していない第一逆運動学軌道ＳＰ（ｉ）および補正後軌道ＳＰｆ（ｉ）について、詳細は図１等を用いて後述する。
【００５０】
また、コントローラ１０は、「第一サーボ制御系２０についての指令軌道」に対する順運動学演算の演算結果と目標軌道Ｔｔとの誤差に逆運動学演算を行なって生成した第二逆運動学軌道ＩＫｔ（ｉ）（不図示）を、「第二サーボ制御系３０についての指令軌道」とする。なお、図２においては図示していない第二逆運動学軌道ＩＫｔ（ｉ）について、第一逆運動学軌道ＳＰ（ｉ）および補正後軌道ＳＰｆ（ｉ）と同様に、詳細は図１等を用いて後述する。
【００５１】
ここで、コントローラ１０は、第一逆運動学軌道ＳＰ（ｉ）から、位相遅延が発生しないようにして高周波成分を除去して、補正後軌道ＳＰｆ（ｉ）を生成する。具体的には、コントローラ１０は、第一逆運動学軌道ＳＰ（ｉ）に対し、時間軸の順方向と逆方向との両方向にローパスフィルタ処理を行なって、つまり、ゼロ位相フィルタ処理を行なって、補正後軌道ＳＰｆ（ｉ）を生成する。コントローラ１０は、生成した補正後軌道ＳＰｆ（ｉ）をメモリ（具体的には、後述する記憶部１６０の第一指示軌道テーブル１６１）に格納する。
【００５２】
（処理２）コントローラ１０は、第一サーボ制御系２０および第二サーボ制御系３０の各々についての指令軌道から、第一サーボ制御系２０および第二サーボ制御系３０の各々に出力する指令値（第一指令値Ｐｃ（ｉ）および第二指令値Ｓｃ（ｉ））を生成する。具体的には、コントローラ１０は、補正後軌道ＳＰｆ（ｉ）から第一指令値Ｐｃ（ｉ）を生成し、第二逆運動学軌道ＩＫｔ（ｉ）から第二指令値Ｓｃ（ｉ）を生成する。コントローラ１０は、第一指令値Ｐｃ（ｉ）および第二指令値Ｓｃ（ｉ）の各々を、第一サーボ制御系２０および第二サーボ制御系３０の各々に対する制御周期ごとに、第一サーボ制御系２０および第二サーボ制御系３０の各々に出力する。
【００５３】
ここで、コントローラ１０の第二サーボ制御系３０に対する制御周期は、コントローラ１０の第一サーボ制御系２０に対する制御周期よりも短い（速い）。具体的には、コントローラ１０の第一サーボ制御系２０に対する制御周期、すなわち、コントローラ１０が第一サーボドライバ２１に出力する第一指令値Ｐｃ（ｉ）の更新周期は、例えば１ｍｓである。また、コントローラ１０の第二サーボ制御系３０に対する制御周期、すなわち、コントローラ１０が第二サーボドライバ３１に出力する第二指令値Ｓｃ（ｉ）の更新周期は、例えば１／１２ｍｓである。
【００５４】
例えば、コントローラ１０は、補正後軌道ＳＰｆ（ｉ）から１ｍｓごとに生成した第一指令値Ｐｃ（ｉ）を第一サーボドライバ２１に出力する。また、例えば、コントローラ１０は、第二逆運動学軌道ＩＫｔ（ｉ）から１／１２ｍｓごとに生成した第二指令値Ｓｃ（ｉ）を第二サーボドライバ３１に出力する。ここで、前述の通り、第一サーボドライバ２１と第二サーボドライバ３１との制御周期は、例えば、共に１／１２ｍｓである。したがって、第一サーボドライバ２１は、コントローラ１０によって１ｍｓごとに更新される第一指令値Ｐｃ（ｉ）を用いて、１／１２ｍｓの制御周期で、第一アクチュエータ２２をフィードバック制御する。第二サーボドライバ３１は、コントローラ１０によって１／１２ｍｓごとに更新される第二指令値Ｓｃ（ｉ）を用いて、１／１２ｍｓの制御周期で、第二アクチュエータ３２をフィードバック制御する。
【００５５】
なお、上述の通り、コントローラ１０の第二サーボ制御系３０に対する制御周期は、コントローラ１０の第一サーボ制御系２０に対する制御周期よりも短い（速い）。そこで、以下の説明においては、第一サーボ制御系２０を「低速サーボ制御系（低速サーボシステム）」と称し、第二サーボ制御系３０を「高速サーボ制御系（高速サーボシステム）」と称することがある。
【００５６】
また、以下の説明においては、「逆運動学演算」は「インバースキネマティクス処理」と称することがあり、「順運動学演算」は「フォワードキネマティクス処理」と称することがある。さらに、第一逆運動学軌道ＳＰ、補正後軌道ＳＰｆ、および指令値Ｐｃの各々について、第一アクチュエータ２２の「軸」ごとの値である場合、第一逆運動学軌道ＳＰ（ｉ）、補正後軌道ＳＰｆ（ｉ）、および指令値Ｐｃ（ｉ）と記すことがある。第一アクチュエータ２２の軸数は「１〜ｎ」であり、つまり、指令軌道ＳＰ（ｉ）および指令値Ｐｃ（ｉ）について、「ｉ＝１〜ｎ」である。同様に、第二逆運動学軌道ＩＫｔおよび指令値Ｓｃの各々について、第二アクチュエータ３２の「軸」ごとの値である場合、第二逆運動学軌道ＩＫｔ（ｉ）および指令値Ｓｃ（ｉ）と記すことがある。第二アクチュエータ３２の軸数は「１〜ｍ」であり、つまり、第二逆運動学軌道ＩＫｔ（ｉ）および指令値Ｓｃ（ｉ）について、「ｉ＝１〜ｍ」である。第一逆運動学軌道ＳＰ（ｉ）、補正後軌道ＳＰｆ（ｉ）、指令値Ｐｃ（ｉ）、第二逆運動学軌道ＩＫｔ（ｉ）、および指令値Ｓｃ（ｉ）について、軸ごとの値であることを特に説明する必要がない場合、「（ｉ）」を略記することがある。
【００５７】
（制御装置の概要）
これまで図２を用いて概要を説明してきた制御システム１に含まれるコントローラ１０について、次に、その構成および処理の内容等を、図１等を用いて説明していく。図１を参照して詳細を説明する前に、コントローラ１０についての理解を容易にするため、その概要について以下のように整理しておく。
【００５８】
コントローラ１０は、第一逆運動学軌道ＳＰ（基準軌道）から位相遅延が発生しないようにして高周波成分を除去した補正後軌道ＳＰｆ（第一指令軌道）を、第一サーボ制御系２０についての指令軌道とするゼロ位相フィルタ部１３０（第一軌道生成部）と、第二サーボ制御系３０についての指令軌道として、前記高周波成分に対応する軌道を含む第二逆運動学軌道ＩＫｔ（第二指令軌道）を生成する第二逆運動学演算部１５０（第二軌道生成部）と、を備えている。
【００５９】
前記の構成によれば、コントローラ１０は、第一逆運動学軌道ＳＰからの位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して、「第一サーボ制御系２０についての指令軌道」である補正後軌道ＳＰｆを生成する。
【００６０】
ここで、第一逆運動学軌道ＳＰからローパスフィルタによって高周波成分を除去した場合、高周波成分除去後の第一逆運動学軌道ＳＰは、高周波成分除去前の第一逆運動学軌道ＳＰに比べて位相遅れ（位相遅延）が発生することが知られている。そして、発生した位相遅延は第二サーボ制御系３０に補償させることになり、つまり、本来は第一サーボ制御系２０が実現すべき軌道の一部を第二サーボ制御系３０に実現させることになり、第二サーボ制御系３０の可動範囲を有効に利用することができない。
【００６１】
これに対して、コントローラ１０は、第一逆運動学軌道ＳＰに対する位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して補正後軌道ＳＰｆを生成する。そのため、第一サーボ制御系２０は、高周波成分を除去した後の補正後軌道ＳＰｆに十分追従することができる。つまり、コントローラ１０は、第一サーボ制御系２０の追従性を維持することができる。
【００６２】
また、コントローラ１０は、前記高周波成分の除去に伴って従来発生していた位相遅延の発生を防ぐので、第一サーボ制御系２０が本来実現すべき軌道の一部を第二サーボ制御系３０に補償させる必要がない。つまり、コントローラ１０は、第二サーボ制御系３０の可動範囲を有効に利用することができる。
【００６３】
以上に整理した通り、コントローラ１０は、高周波成分の除去によって第一サーボ制御系２０の追従性を維持しつつ、位相遅延の発生を防ぐことで第二サーボ制御系３０の可動範囲を有効に利用することができる。したがって、コントローラ１０は、第一サーボ制御系２０の追従性を維持しつつ、第一サーボ制御系２０および第二サーボ制御系３０の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【００６４】
コントローラ１０において、ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰに対し、時間軸の順方向と逆方向との両方向にローパスフィルタ処理を行なって補正後軌道ＳＰｆを生成する。
【００６５】
前記の構成によれば、コントローラ１０は、第一逆運動学軌道ＳＰに対し、時間軸の順方向と逆方向との両方向にローパスフィルタ処理を行なうことで、第一逆運動学軌道ＳＰからの位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して補正後軌道ＳＰｆを生成する。つまり、コントローラ１０は、第一逆運動学軌道ＳＰにローパスフィルタ処理を行なう場合と異なり、第一サーボ制御系２０の軌道の位相遅延を発生させないので、前記第一サーボ制御系が本来実現すべき軌道の一部を前記第二サーボ制御系に補償させる必要がない。
【００６６】
したがって、コントローラ１０は、高周波成分の除去によって第一サーボ制御系２０の追従性を維持しつつ、位相遅延の発生を防ぐことで第二サーボ制御系３０の可動範囲を有効に利用することができる。つまり、コントローラ１０は、第一サーボ制御系２０の追従性を維持しつつ、第一サーボ制御系２０および第二サーボ制御系３０の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【００６７】
コントローラ１０において、ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰに対し、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって補正後軌道ＳＰｆを生成する。
【００６８】
前記の構成によれば、コントローラ１０は、第一逆運動学軌道ＳＰに対し、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって補正後軌道ＳＰｆを生成する。
【００６９】
ここで、詳細は図４を用いて後述するが、第一逆運動学軌道ＳＰに、時間軸の順方向から逆方向の順に前記ローパスフィルタ処理を行なった場合、補正後軌道ＳＰｆについて、スタート時点（ｔ＝０の時点）において第一逆運動学軌道ＳＰに対するデータ飛びが発生する。
【００７０】
これに対して、コントローラ１０は、第一逆運動学軌道ＳＰに、時間軸の逆方向から順方向の順に前記ローパスフィルタ処理を行なって補正後軌道ＳＰｆを生成する。したがって、コントローラ１０は、補正後軌道ＳＰｆについて、スタート時点（ｔ＝０の時点）において第一逆運動学軌道ＳＰに対するデータ飛びが発生することを防ぐことができるとの効果を奏する。
【００７１】
以上を整理すれば、コントローラ１０は、目標軌道Ｔｔに逆運動学演算を行なって生成した軌道に遅延補償付きのローパスフィルタ処理を実施したものを、第一アクチュエータ２２についての指令軌道とする。具体的には、コントローラ１０は、ゼロ位相フィルタを使用する。ゼロ位相フィルタは、フィルタ処理を時間軸の順方向と逆方向との２回（１往復）実行することで、位相遅れ（位相遅延）を相殺する。さらに、コントローラ１０は、スタート時のデータ飛びをなくすために、逆方向から順方向の順に、ゼロ位相フィルタ処理を実行する。
【００７２】
コントローラ１０は、低速アクチュエータ（第一アクチュエータ２２）についての軌道（指令軌道）の位相遅れ（位相遅延）を低減させることにより、高速アクチュエータ（第二アクチュエータ３２）の動作範囲を節約することができる。したがって、コントローラ１０は、より高速の軌道に対応できる。また、コントローラ１０は、ローパスフィルタの効果を強めて、低速アクチュエータの追従性を向上させることができ、トータル（第一サーボ制御系２０および第二サーボ制御系３０の全体）の制御精度を向上することができる。さらに、コントローラ１０は、制御システム１において採用する高速アクチュエータについて選択肢を広がることができ、採用する高速アクチュエータについての選定を容易にすることができる。これまでに概要を整理してきたコントローラ１０について、次に、その構成等の詳細について、図１等を用いて説明していく。
【００７３】
（制御装置の詳細）
図１は、本発明の実施形態１に係るコントローラ１０等の要部構成を示すブロック図である。図１に示すように、コントローラ１０は、機能ブロックとして、目標軌道取得部１１０、第一逆運動学演算部１２０、ゼロ位相フィルタ部１３０、順運動学演算部１４０、第二逆運動学演算部１５０、記憶部１６０、および指示部１７０を備えている。なお、記載の簡潔性を担保するため、本実施の形態に直接関係のない構成は、説明およびブロック図から省略している。ただし、実施の実情に則して、コントローラ１０は、当該省略された構成を備えてもよい。図１に例示した各機能ブロックは、例えば、ＣＰＵ（central processing unit）などが、ＲＯＭ（read only memory）、ＮＶＲＡＭ（non-Volatile random access memory）等で実現された記憶装置（記憶部１６０）に記憶されているプログラムを不図示のＲＡＭ（random access memory）等に読み出して実行することで実現できる。以下、コントローラ１０における各機能ブロックについて説明する。
【００７４】
（記憶部以外の機能ブロックの詳細）
目標軌道取得部１１０は、外部（例えばユーザ）から目標軌道データ（目標軌道Ｔｔ）を受け付け、受け付けた目標軌道Ｔｔを、第一逆運動学演算部１２０および第二逆運動学演算部１５０に出力する。
【００７５】
第一逆運動学演算部１２０は、目標軌道取得部１１０から取得した目標軌道Ｔｔに逆運動学演算を行なって、第一サーボ制御系２０の軸ごとの第一逆運動学軌道ＳＰ（ｉ）を生成する。第一逆運動学演算部１２０は、生成した第一逆運動学軌道ＳＰ（ｉ）をゼロ位相フィルタ部１３０に出力する。
【００７６】
ゼロ位相フィルタ部１３０は、第一逆運動学演算部１２０から取得した第一逆運動学軌道ＳＰ（ｉ）から、位相遅延が発生しないようにして高周波成分を除去して、第一サーボ制御系２０の軸ごとの補正後軌道ＳＰｆ（ｉ）を生成する。具体的には、ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰ（ｉ）にゼロ位相フィルタ処理を行なって補正後軌道ＳＰｆ（ｉ）を生成する。
【００７７】
ここで、ゼロ位相フィルタ処理とは、フィルタリング処理（本実施形態では、ローパスフィルタ処理）を、順方向と逆方向との両方向で（例えば、時間軸の順方向と逆方向との各々について１回ずつ、計２回）、実行することをいう。具体的には、第一逆運動学軌道ＳＰ（ｉ）に対して行う、時間軸の順方向と逆方向との両方向でのローパスフィルタ処理を「ゼロ位相フィルタ処理」と称する。なお、ローパスフィルタ処理はローパスフィルタリングと称することがあり、ゼロ位相フィルタ処理はゼロ位相フィルタリングと称することがある。
【００７８】
特に、ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰ（ｉ）に対し、時間軸の逆方向から順方向の順にローパスフィルタ処理を行なって、つまり、時間軸について逆方向（逆順）にゼロ位相フィルタ処理を行なって、補正後軌道ＳＰｆ（ｉ）を生成する。時間軸の逆方向から順方向の順にローパスフィルタ処理を行なうことにより、ゼロ位相フィルタ部１３０は、スタート時点（ｔ＝０の時点）において第一逆運動学軌道ＳＰ（ｉ）に対するデータ飛びの無い補正後軌道ＳＰｆ（ｉ）を生成することができる。ゼロ位相フィルタ部１３０の用いるローパスフィルタ（ゼロ位相フィルタ）のフィルタ特性は、例えば、カットオフ周波数１０Ｈｚの２次バターワース（Butterworth）型である。ゼロ位相フィルタ部１３０が実行するゼロ位相フィルタ処理の詳細について、図４を用いて後述する。
【００７９】
ゼロ位相フィルタ部１３０は、生成した補正後軌道ＳＰｆ（ｉ）を、「第一サーボ制御系２０についての指令軌道」として、記憶部１６０の第一指示軌道テーブル１６１に格納する。また、ゼロ位相フィルタ部１３０は、生成した補正後軌道ＳＰｆ（ｉ）を、順運動学演算部１４０に出力する。
【００８０】
ゼロ位相フィルタ部１３０は、オフラインで事前に軌道計算を実行し、実行した軌道計算の結果をメモリに格納する。つまり、ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰ（ｉ）に対してゼロ位相フィルタ処理を行なって生成した補正後軌道ＳＰｆ（ｉ）を、第一指示軌道テーブル１６１に格納する。
【００８１】
順運動学演算部１４０は、ゼロ位相フィルタ部１３０から取得する「ｉ＝１〜ｎ」である補正後軌道ＳＰｆ（ｉ）の全てに対する順運動学演算から、順運動学軌道ＦＫｔ（中間軌道）を生成する。順運動学演算部１４０は、生成した順運動学軌道ＦＫｔを第二逆運動学演算部１５０に出力する。
【００８２】
第二逆運動学演算部１５０は、「第二サーボ制御系３０についての指令軌道」として、ゼロ位相フィルタ部１３０により第一逆運動学軌道ＳＰ（ｉ）から除去された高周波成分に対応する軌道を含む第二逆運動学軌道ＩＫｔ（第二指令軌道）を生成する。
【００８３】
具体的には、第二逆運動学演算部１５０は、「補正後軌道ＳＰｆと、第二逆運動学軌道ＩＫｔとの合成軌道が、目標軌道Ｔｔに一致する」ようにして、第一逆運動学軌道ＳＰと補正後軌道ＳＰｆとの誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。第二逆運動学演算部１５０は、「第二サーボ制御系３０についての指令軌道」として、ゼロ位相フィルタ部１３０により第一逆運動学軌道ＳＰ（ｉ）から除去された高周波成分に対応する軌道を含む第二逆運動学軌道ＩＫｔ（第二指令軌道）を生成する。例えば、第二逆運動学演算部１５０は、「順運動学演算部１４０から取得する順運動学軌道ＦＫｔと、目標軌道Ｔｔと、の誤差」に逆運動学演算を行なって、第二サーボ制御系３０の軸ごとの指令軌道である第二逆運動学軌道ＩＫｔ（ｉ）を生成してもよい。第二逆運動学演算部１５０は、生成した第二逆運動学軌道ＩＫｔ（ｉ）を、「第二サーボ制御系３０についての指令軌道」として、記憶部１６０の第二指示軌道テーブル１６２に格納する。
【００８４】
第二サーボ制御系３０の指令軌道である第二逆運動学軌道ＩＫｔ（第二指令軌道）は、ゼロ位相フィルタ部１３０により第一逆運動学軌道ＳＰ（ｉ）から除去された高周波成分を含んでいる。また、第二逆運動学軌道ＩＫｔ（第二指令軌道）は、「第一サーボ制御系２０の指令軌道（補正後軌道ＳＰｆ）と、第二サーボ制御系３０の指令軌道（第二逆運動学軌道ＩＫｔ）との合成軌道が、目標軌道Ｔｔに一致する」との条件を満たしている。すなわち、第二逆運動学演算部１５０は、「補正後軌道ＳＰｆと、第二逆運動学軌道ＩＫｔとの合成軌道が、目標軌道Ｔｔに一致する」ようにして、第一逆運動学軌道ＳＰと補正後軌道ＳＰｆとの誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成すればよい。
【００８５】
指示部１７０は、第一サーボ制御系２０に第一指令値Ｐｃ（ｉ）を出力する第一指示部１７１と、第二サーボ制御系３０に第二指令値Ｓｃ（ｉ）を出力する第二指示部１７２と、を含んでいる。
【００８６】
第一指示部１７１は、第一サーボ制御系２０に対する制御周期ごとに、「第一サーボ制御系２０についての指令軌道」から第一サーボ制御系２０の軸ごとの第一指令値Ｐｃ（ｉ）を生成し、生成した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。具体的には、第一指示部１７１は、記憶部１６０の第一指示軌道テーブル１６１を参照して、「第一サーボ制御系２０についての指令軌道」として、補正後軌道ＳＰｆ（ｉ）を取得する。そして、第一指示部１７１は、例えば１ｍｓごとに、補正後軌道ＳＰｆ（ｉ）から第一サーボ制御系２０の軸ごとの第一指令値Ｐｃ（ｉ）を生成し、生成した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。
【００８７】
第二指示部１７２は、第二サーボ制御系３０に対する制御周期ごとに、「第二サーボ制御系３０についての指令軌道」から、第二サーボ制御系３０の軸ごとの第二指令値Ｓｃ（ｉ）を生成し、生成した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。具体的には、第二指示部１７２は、記憶部１６０の第二指示軌道テーブル１６２を参照して、「第二サーボ制御系３０についての指令軌道」として、第二逆運動学軌道ＩＫｔ（ｉ）を取得する。そして、第二指示部１７２は、例えば１／１２ｍｓごとに、第二逆運動学軌道ＩＫｔ（ｉ）から第二サーボ制御系３０の軸ごとの第二指令値Ｓｃ（ｉ）を生成し、生成した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。
【００８８】
（記憶部の詳細）
記憶部１６０は、コントローラ１０が使用する各種データを格納する記憶装置である。なお、記憶部１６０は、コントローラ１０が実行する（１）制御プログラム、（２）ＯＳプログラム、（３）コントローラ１０が有する各種機能を実行するためのアプリケーションプログラム、および、（４）該アプリケーションプログラムを実行するときに読み出す各種データを非一時的に記憶してもよい。上記の（１）〜（４）のデータは、例えば、ＲＯＭ（read only memory）、フラッシュメモリ、ＥＰＲＯＭ（Erasable Programmable ROM）、ＥＥＰＲＯＭ（登録商標）（Electrically EPROM）、ＨＤＤ（Hard Disc Drive）等の不揮発性記憶装置に記憶される。コントローラ１０は、図示しない一時記憶部を備えていてもよい。一時記憶部は、コントローラ１０が実行する各種処理の過程で、演算に使用するデータおよび演算結果等を一時的に記憶するいわゆるワーキングメモリであり、ＲＡＭ（Random Access Memory）などの揮発性記憶装置で構成される。どのデータをどの記憶装置に記憶するのかについては、コントローラ１０の使用目的、利便性、コスト、または、物理的な制約などから適宜決定される。記憶部１６０はさらに、第一指示軌道テーブル１６１および第二指示軌道テーブル１６２を格納している。第一指示軌道テーブル１６１には、「第一サーボ制御系２０についての指令軌道」が格納され、具体的には、ゼロ位相フィルタ部１３０によって、補正後軌道ＳＰｆ（ｉ）が格納される。第二指示軌道テーブル１６２には、「第二サーボ制御系３０についての指令軌道」が格納され、具体的には、第二逆運動学演算部１５０によって、第二逆運動学軌道ＩＫｔ（ｉ）が格納される。
【００８９】
コントローラ１０は、第一指示軌道テーブル１６１をオフライン処理で予め作成しておき、つまり、「第一サーボ制御系２０についての指令軌道」を予め算出しておく。コントローラ１０は、全ての処理をオフラインで実行しておき、第二指示軌道テーブル１６２についても予め作成しておいてもよく、つまり、「第二サーボ制御系３０についての指令軌道」を予め算出しておいてもよい。なお、図１には、コントローラ１０が、全ての処理をオフラインで予め実行しておき、第二指示軌道テーブル１６２を作成しておく例が示されている。ただし、コントローラ１０が、「第二サーボ制御系３０についての指令軌道」を予め算出しておくことは必須ではなく、つまり、第二指示軌道テーブル１６２を予め作成しておくことは必須ではない。
【００９０】
（コントローラが実行する処理について）
図３は、コントローラ１０が実行する処理の概要を示すフロー図である。図３に示すように、第一逆運動学演算部１２０は、目標軌道Ｔｔから低速アクチュエータ（第一アクチュエータ２２）の軸毎の軌道を生成し、つまり、目標軌道Ｔｔに逆運動学演算を行なって第一逆運動学軌道ＳＰ（ｉ）を生成する（Ｓ１１０）。
【００９１】
ゼロ位相フィルタ部１３０は、軸毎の軌道（つまり、第一逆運動学軌道ＳＰ（ｉ））に対して、ゼロ位相フィルタリングを実行し、生成した補正後軌道ＳＰｆ（ｉ）を、記憶部１６０の第一指示軌道テーブル１６１に格納する（Ｓ１２０）。
【００９２】
コントローラ１０は、ゼロ位相フィルタリング後の軌道と目標軌道Ｔｔとの差分を計算する。具体的には、順運動学演算部１４０が、順運動学演算により、ゼロ位相フィルタリング後の軌道（つまり、補正後軌道ＳＰｆ（ｉ））から中間軌道（つまり、順運動学軌道ＦＫｔ）を生成する（Ｓ１３０）。そして、第二逆運動学演算部１５０は、中間軌道と目標軌道Ｔｔとの差分（誤差）を計算する（Ｓ１４０）。
【００９３】
第二逆運動学演算部１５０は、計算した誤差（差分）を用いて、高速アクチュエータ（第二アクチュエータ３２）の軸毎の軌道を計算し、記憶部１６０の第二指示軌道テーブル１６２に格納する（Ｓ１５０）。具体的には、第二逆運動学演算部１５０は、「中間軌道（つまり、順運動学軌道ＦＫｔ）と、目標軌道Ｔｔと、の誤差」に逆運動学演算を行なって、第二サーボ制御系３０の軸ごとの指令軌道である第二逆運動学軌道ＩＫｔ（ｉ）を生成する。そして、第二逆運動学演算部１５０は、生成した第二逆運動学軌道ＩＫｔ（ｉ）を、記憶部１６０の第二指示軌道テーブル１６２に格納する。
【００９４】
第二逆運動学演算部１５０は、第一逆運動学軌道ＳＰと補正後軌道ＳＰｆとの誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。具体的には、第二逆運動学演算部１５０は、「補正後軌道ＳＰｆと、第二逆運動学軌道ＩＫｔとの合成軌道が、目標軌道Ｔｔに一致する」ようにして、第一逆運動学軌道ＳＰと補正後軌道ＳＰｆとの誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。
【００９５】
指示部１７０は、格納している軸毎の軌道から、現時刻の指令値を、低速サーボシステム（つまり、第一サーボ制御系２０）および高速サーボシステム（つまり、第二サーボ制御系３０）へ出力する（Ｓ１６０）。具体的には、第一指示部１７１は、第一指示軌道テーブル１６１に格納されている補正後軌道ＳＰｆ（ｉ）から、第一サーボ制御系２０に対する制御周期ごとに、第一指令値Ｐｃ（ｉ）を生成し、生成した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。また、第二指示部１７２は、第二指示軌道テーブル１６２に格納されている第二逆運動学軌道ＩＫｔ（ｉ）から、第二サーボ制御系３０に対する制御周期ごとに、第二指令値Ｓｃ（ｉ）を生成し、生成した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。コントローラ１０は、軌道終点に到達したかを判定しながら（Ｓ１７０）、制御周期ごとに、Ｓ１４０〜Ｓ１６０の処理を、または、Ｓ１６０の処理を、繰り返す。具体的には、コントローラ１０は、「Ｓ１４０〜Ｓ１６０の処理、または、Ｓ１６０の処理」のうち、「第一サーボ制御系２０のみに係る処理」を、軌道終点に到達したかを判定しながら（Ｓ１７０）、第一サーボ制御系２０に対する制御周期で繰り返し実行する。また、コントローラ１０は、「Ｓ１４０〜Ｓ１６０の処理、または、Ｓ１６０の処理」のうち、「第一サーボ制御系２０のみに係る処理」以外の処理（例えば、第二サーボ制御系３０に係る処理）を、軌道終点に到達したかを判定しながら（Ｓ１７０）、第二サーボ制御系３０に対する制御周期で繰り返し実行する。
【００９６】
これまで説明してきた、コントローラ１０の実行する制御方法は、以下のように整理することができる。すなわち、コントローラ１０の実行する制御方法は、第一逆運動学軌道ＳＰから位相遅延が発生しないようにして高周波成分を除去した補正後軌道ＳＰｆを、「第一サーボ制御系２０についての指令軌道」とする第一軌道生成ステップ（Ｓ１２０）と、「第二サーボ制御系３０についての指令軌道」として、前記高周波成分に対応する軌道を含む第二逆運動学軌道ＩＫｔ（第二指令軌道）を生成する第二軌道生成ステップ（Ｓ１５０）と、を含んでいる。
【００９７】
前記の方法によれば、前記制御方法は、第一逆運動学軌道ＳＰからの位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して、第一サーボ制御系２０についての指令軌道である補正後軌道ＳＰｆを生成する。
【００９８】
ここで、第一逆運動学軌道ＳＰからローパスフィルタによって高周波成分を除去した場合、高周波成分除去後の第一逆運動学軌道ＳＰは、高周波成分除去前の第一逆運動学軌道ＳＰに比べて位相遅れ（位相遅延）が発生することが知られている。そして、発生した位相遅延は第二サーボ制御系３０に補償させることになり、つまり、本来は第一サーボ制御系２０が実現すべき軌道の一部を第二サーボ制御系３０に実現させることになり、第二サーボ制御系３０の可動範囲を有効に利用することができない。
【００９９】
これに対して、前記制御方法は、第一逆運動学軌道ＳＰに対する位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して補正後軌道ＳＰｆを生成する。そのため、第一サーボ制御系２０は、高周波成分を除去した後の補正後軌道ＳＰｆに十分追従することができる。つまり、前記制御方法は、第一サーボ制御系２０の追従性を維持することができる。
【０１００】
また、前記制御方法は、前記高周波成分の除去に伴って従来発生していた位相遅延の発生を防ぐので、第一サーボ制御系２０が本来実現すべき軌道の一部を第二サーボ制御系３０に補償させる必要がない。つまり、前記制御方法は、第二サーボ制御系３０の可動範囲を有効に利用することができる。
【０１０１】
以上に整理した通り、前記制御方法は、高周波成分の除去によって第一サーボ制御系２０の追従性を維持しつつ、位相遅延の発生を防ぐことで第二サーボ制御系３０の可動範囲を有効に利用することができる。したがって、前記制御方法は、第一サーボ制御系２０の追従性を維持しつつ、第一サーボ制御系２０および第二サーボ制御系３０の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【０１０２】
（ゼロ位相フィルタ処理について）
ここで、ゼロ位相フィルタ部１３０が行うゼロ位相フィルタ処理（ゼロ位相フィルタリング）について、詳細を説明しておく。前述の通り、ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰ（ｉ）に対し、時間軸の逆方向から順方向の順に（つまり時間軸について逆方向（逆順）に）、ローパスフィルタ処理を行なって、補正後軌道ＳＰｆ（ｉ）を生成する。
【０１０３】
ゼロ位相フィルタ部１３０は、予め、１次遅れフィルタ計算の係数を求めておく。具体的には、ゼロ位相フィルタ部１３０は、予め、「ａ＝Ｔｓ／（Ｔ＋Ｔｓ）」および「ｂ＝Ｔ／（Ｔ＋Ｔｓ）」を求めておく。ここで、Ｔｓはサンプリング周期であり、Ｔはフィルタ時定数である。
【０１０４】
ゼロ位相フィルタ部１３０は、第一逆運動学軌道ＳＰ（ｉ）について時間軸を逆転させた、仮の軌道データＳＰr（ｉ）を作成し、「ＳＰr（ｉ）＝ＳＰ（ｎ−ｉ）」を求める。ゼロ位相フィルタ部１３０は、フィルタリング計算（ローパスフィルタ処理）の１回目（時間軸の逆方向）を実行し、すなわち、「ＳＰｆｒ（ｉ）＝ａ×ＳＰr（ｉ）＋ｂ×ＳＰr（ｉ−１）」を求める。ただし、「ＳＰr（−１）＝ＳＰr（０）」とする。
【０１０５】
ゼロ位相フィルタ部１３０は、時間軸を再度逆転させて元に戻し、すなわち、「ＳＰｆｎ（ｉ）＝ＳＰｆｒ（ｎ−ｉ）」を求める。ゼロ位相フィルタ部１３０は、フィルタリング計算２回目（順方向）を実行して、ゼロ位相フィルリング後の補正後軌道ＳＰｆ（ｉ）を求める。すなわち、ゼロ位相フィルタ部１３０は、「ＳＰｆ（ｉ）＝ａ×ＳＰｆｎ（ｉ）＋ｂ×ＳＰｆｎ（ｉ−１）」を求める。ただし、「ＳＰｆｎ（−１）＝ＳＰ（０）」とする。ここで、「ＳＰｆｎ（−１）＝ＳＰｆｎ（０）」ではなく、「ＳＰｆｎ（−１）＝ＳＰ（０）」とすることにより、ゼロ位相フィルタ部１３０は、スタート時のＳＰに飛びが生じないようにすることができる。
【０１０６】
ゼロ位相フィルタ部１３０が、時間軸の逆順にゼロ位相フィルタ処理を行なう（つまり、ローパスフィルタ処理を時間軸の逆方向から順方向への１往復行なう）ことにより、スタート時のデータ飛びの発生を防ぐ点について、図４を用いて説明する。
【０１０７】
（高周波成分の除去方法について）
図４は、第一逆運動学軌道ＳＰ（ｉ）から高周波成分の除去する方法に応じて補正後軌道ＳＰｆ（ｉ）に発生する位相遅延およびデータ飛びについて説明する図である。図４において、縦軸は位置、横軸は時間（ｔ）であり、図４の（Ａ）、（Ｂ）、（Ｃ）の各々について、上段のスタート時点（ｔ＝０）付近を拡大して示すのが下段の図である。図４の（Ａ）は、第一逆運動学軌道ＳＰ（ｉ）に通常のローパスフィルタ処理（ローパスフィルタリング）を行なった時の、第一逆運動学軌道ＳＰ（ｉ）および補正後軌道ＳＰｆ（ｉ）を示している。図４の（Ａ）において、ローパスフィルタ特性は、カットオフ周波数１０Ｈｚの４次バターワース型である。図４の（Ｂ）は、第一逆運動学軌道ＳＰ（ｉ）にローパスフィルタ処理を時間軸の順方向から逆方向への１往復実行した時の、第一逆運動学軌道ＳＰ（ｉ）および補正後軌道ＳＰｆ（ｉ）を示している。図４の（Ｃ）は、第一逆運動学軌道ＳＰ（ｉ）にローパスフィルタ処理を時間軸の逆方向から順方向への１往復実行した時の、第一逆運動学軌道ＳＰ（ｉ）および補正後軌道ＳＰｆ（ｉ）を示している。図４の（Ｂ）および（Ｃ）において、ゼロ位相フィルタのフィルタ特性は、カットオフ周波数１０Ｈｚの２次バターワース型である。
【０１０８】
図４の（Ａ）に示すように、通常のローパスフィルタ処理（フィルタリング処理）を行った場合、このフィルタリング処理により、補正後軌道ＳＰｆ（ｉ）について、第一逆運動学軌道ＳＰ（ｉ）からの位相遅れ（位相遅延）が発生している。
【０１０９】
これに対し、図４の（Ｂ）および（Ｃ）に示すように、ゼロ位相フィルタ処理を実行することで、言い換えれば、「フィルタリング処理を時間軸の順方向と逆方向との両方向に１回ずつ計２回実行する（１往復実行する）」ことで、位相遅れを除去できる。
【０１１０】
ただし、「フィルタリング処理を時間軸の順方向から逆方向への１往復実行した（つまり、フィルタリング処理を時間軸の順方向から逆方向の順に実行した）」場合、図４の（Ｂ）に示されているように、スタート時点（ｔ＝０）でデータ飛びが発生する。
【０１１１】
そこで、ゼロ位相フィルタ処理を「時間軸の逆方向から順方向へ」の順で実行することにより、言い換えれば「フィルタリング処理を、時間軸の逆方向から順方向への１往復実行する」ことにより、スタート時点でのデータ飛びを回避することができる。図４の（Ｃ）において、ゼロ位相フィルタ処理を「時間軸の逆方向から順方向へ」の順で実行することにより、スタート時点（ｔ＝０）でのデータ飛びを回避することができている。
【０１１２】
（コントローラが奏する効果について）
コントローラ１０が、第一サーボドライバ２１および第二サーボドライバ３１を介して、第一アクチュエータ２２および第二アクチュエータ３２を実際にどのように制御するかを、図５等を用いて説明していく。
【０１１３】
図５は、制御システム１においてコントローラ１０を用いて実施する制御テスト（制御シミュレーション）の内容を示す図である。コントローラ１０と従来までのコントローラとについて、複雑な２次元軌跡のＸ軸成分について、制御性能（位置偏差）と第二アクチュエータ３２の動作範囲とを比較する。なお、「従来までのコントローラ」とは、第一逆運動学演算部１２０から取得した第一逆運動学軌道ＳＰ（ｉ）に対して、ローパスフィルタ処理を行なって、「第一サーボ制御系２０についての指令軌道」を生成する協調制御コントローラである。すなわち、コントローラ１０は、第一逆運動学軌道ＳＰ（ｉ）に「ゼロ位相フィルタ処理」を行なって、「第一サーボ制御系２０についての指令軌道」を生成する。これに対し、「従来までのコントローラ」は、第一逆運動学軌道ＳＰ（ｉ）に「ローパスフィルタ処理」を行なって、「第一サーボ制御系２０についての指令軌道」を生成する。その他の点については、コントローラ１０と従来までのコントローラとに違いはないものとする。
【０１１４】
制御テストに用いた２次元軌跡は以下の２つの曲線１および曲線２を合成したものであり、すなわち、「曲線１：円、１秒で１周、半径１０ｍｍ」であり、「曲線２：正葉線、０．１秒で１周、葉数９、葉の長さ２ｍｍ」とする。図５の（Ａ）には、曲線１＋曲線２のＸＹ軌跡が示されている。図５の（Ａ）において、縦軸はＹ軸位置、横軸はＸ軸位置である。図５の（Ｂ）には、図５の（Ａ）のＸＹ軌跡のＸ軸指令値が示されており、図５の（Ｂ）において、縦軸はＸ軸指令値（位置）、横軸は時間（ｔ）である。なお、開始直後の０．０２ｓ間は一定加速度で滑らかに加速するものとする。
【０１１５】
第一アクチュエータ２２および第二アクチュエータ３２は共にＸＹ２軸の構成とする。第一サーボドライバ２１および第二サーボドライバ３１の各々の制御周期は共に１／１２ｍｓとする。つまり、第一サーボドライバ２１の第一アクチュエータ２２に対する制御周期は１／１２ｍｓであり、第二サーボドライバ３１の第二アクチュエータ３２に対する制御周期は１／１２ｍｓである。また、コントローラ１０の制御周期、つまり、コントローラ１０の第一サーボドライバ２１（低速サーボ制御系）および第二サーボドライバ３１（高速サーボ制御系）の各々に対する指令値の更新周期は、低速側：１ｍｓ、高速側：１／１２ｍｓとする。
【０１１６】
（従来のコントローラによる位置等の制御）
図１８は、図５に示す制御テストについて、従来のコントローラによって制御された第一アクチュエータ２２および第二アクチュエータ３２の各々の位置、位置偏差、およびトルクの変化を示す図である。図１８の（Ａ）には、上から順に、従来のコントローラによって制御された第一アクチュエータ２２の位置、位置偏差、およびトルクの変化が示されている。図１８の（Ｂ）には、上から順に、従来のコントローラによって制御された第二アクチュエータ３２の位置、位置偏差、およびトルクの変化が示されている。図１８に示す各図において、横軸はいずれも時間（ｔ）である。
【０１１７】
図１９は、図５に示す制御テストについて、従来のコントローラによって制御された第一アクチュエータ２２および第二アクチュエータ３２の合計の位置（図１９の（Ａ））および位置偏差（図１９の（Ｂ））の変化を示す図である。図１９に示す各図において、横軸はいずれも時間（ｔ）である。なお、図１９の（Ａ）において、指令値と低速高速合成制御量（第一アクチュエータ２２の制御量の実測値と第二アクチュエータ３２の制御量の実測値とを合計（合成）した値）とは、ほぼ重なっている。
【０１１８】
図１８および図１９に制御テスト（制御シミュレーション）の結果を示した「従来のコントローラ」について、ローパスフィルタ特性は、カットオフ周波数１０Ｈｚの４次バターワース型である。前述の通り、「従来までのコントローラ」は、第一逆運動学軌道ＳＰ（ｉ）に「ゼロ位相フィルタ処理」の代わりに「ローパスフィルタ処理」を行なって「第一サーボ制御系２０についての指令軌道」を生成する点以外は、コントローラ１０と同様である。図１８および図１９に示されているように、「従来までのコントローラ」を用いた場合、高速アクチュエータ（第二アクチュエータ３２）の動作範囲は、約プラスマイナス４．５ｍｍである（図１８の（Ｂ）の最上段に示す軌跡）。「従来までのコントローラ」を用いた場合、「ローパスフィルタ処理」による位相遅れ（位相遅延）分が、高速アクチュエータ側の動作範囲を広げている。
【０１１９】
（コントローラ１０による位置等の制御）
図６は、図５に示す制御テストについて、コントローラ１０によって制御された第一アクチュエータ２２および第二アクチュエータ３２の各々の位置、位置偏差、およびトルクの変化を示す図である。図６の（Ａ）には、上から順に、コントローラ１０によって制御された第一アクチュエータ２２の位置、位置偏差、およびトルクの変化が示されている。図６の（Ｂ）には、上から順に、コントローラ１０によって制御された第二アクチュエータ３２の位置、位置偏差、およびトルクの変化が示されている。図６に示す各図において、横軸はいずれも時間（ｔ）である。
【０１２０】
図７は、図５に示す制御テストについて、コントローラ１０によって制御された第一アクチュエータ２２および第二アクチュエータ３２の合計（総合）の位置（図７の（Ａ））および位置偏差（図７の（Ｂ））の変化を示す図である。図７に示す各図において、横軸はいずれも時間（ｔ）である。なお、図７の（Ａ）において、指令値と低速高速合成制御量（第一アクチュエータ２２の制御量の実測値と第二アクチュエータ３２の制御量の実測値とを合計（合成）した値）とは、ほぼ重なっている。
【０１２１】
コントローラ１０は、第一逆運動学軌道ＳＰ（ｉ）に、「時間軸の逆方向から順方向へ」の順にローパスフィルタを一往復実行して、「第一サーボ制御系２０についての指令軌道」を生成する。つまり、コントローラ１０は、第一逆運動学軌道ＳＰ（ｉ）に、「時間軸の逆方向から順方向へ」の順にゼロ位相フィルタ処理を実行して、「第一サーボ制御系２０についての指令軌道」を生成する。コントローラ１０の用いたローパスフィルタのローパスフィルタ特性は、カットオフ周波数１０Ｈｚの２次バターワース型である。
【０１２２】
低速アクチュエータ（第一アクチュエータ２２）、高速アクチュエータ（第二アクチュエータ３２）、および総合（第一アクチュエータ２２と第二アクチュエータ３２との合計）のいずれの偏差も、「従来までのコントローラ」を用いた場合と同等である。
【０１２３】
ただし、コントローラ１０を用いた場合、図６および図７に示されているように、高速アクチュエータの動作範囲は約プラスマイナス２．０ｍｍとなっている（図６の（Ｂ）の最上段に示す軌跡）。したがって、コントローラ１０を用いた場合、高速アクチュエータの動作範囲を、「従来までのコントローラ」を用いた場合の「約プラスマイナス４．５ｍｍ（図１８の（Ｂ）の最上段に示す軌跡）」と比べて、１／２以下に低減することができている。
【０１２４】
以上に説明した通り、コントローラ１０は、低速アクチュエータ（第一アクチュエータ２２）についての軌道（指令軌道）の位相遅れを低減させることにより、高速アクチュエータ（第二アクチュエータ３２）の動作範囲を節約することができている。
【０１２５】
〔実施形態２〕
本発明の実施形態２について、図８および図９に基づいて説明すれば、以下のとおりである。なお、説明の便宜上、前記実施形態にて説明した部材と同じ機能を有する部材については、同じ符号を付記し、その説明を省略する。本実施形態における制御システム２が、上述の実施形態１における制御システム１と異なるのは、本実施形態に係る制御装置であるコントローラ１１が、コントローラ１０のゼロ位相フィルタ部１３０に代えて、包絡線平均取得部１３１を備えている点である。「ゼロ位相フィルタ部１３０に代えて包絡線平均取得部１３１を備える」点以外の点においては、コントローラ１１の構成は、コントローラ１０の構成と同様である。
【０１２６】
（制御装置の概要）
コントローラ１１（制御装置）は、第一逆運動学軌道ＳＰ（基準軌道）から位相遅延が発生しないようにして高周波成分を除去した補正後軌道ＳＰｆ（第一指令軌道）を、第一サーボ制御系２０についての指令軌道とする包絡線平均取得部１３１と、第二サーボ制御系３０についての指令軌道として、前記高周波成分に対応する軌道を含む第二逆運動学軌道ＩＫｔ（第二指令軌道）を生成する第二逆運動学演算部１５０（第二軌道生成部）と、を備えている。
【０１２７】
前記の構成によれば、コントローラ１１は、第一逆運動学軌道ＳＰからの位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して、第一サーボ制御系２０についての指令軌道である補正後軌道ＳＰｆを生成する。
【０１２８】
ここで、第一逆運動学軌道ＳＰからローパスフィルタによって高周波成分を除去した場合、高周波成分除去後の第一逆運動学軌道ＳＰは、高周波成分除去前の第一逆運動学軌道ＳＰに比べて位相遅れ（位相遅延）が発生することが知られている。そして、発生した位相遅延は第二サーボ制御系３０に補償させることになり、つまり、本来は第一サーボ制御系２０が実現すべき軌道の一部を第二サーボ制御系３０に実現させることになり、第二サーボ制御系３０の可動範囲を有効に利用することができない。
【０１２９】
これに対して、コントローラ１１は、第一逆運動学軌道ＳＰに対する位相遅延が発生しないようにして第一逆運動学軌道ＳＰから高周波成分を除去して補正後軌道ＳＰｆを生成する。そのため、第一サーボ制御系２０は、高周波成分を除去した後の補正後軌道ＳＰｆに十分追従することができる。つまり、コントローラ１１は、第一サーボ制御系２０の追従性を維持することができる。
【０１３０】
また、コントローラ１１は、前記高周波成分の除去に伴って従来発生していた位相遅延の発生を防ぐので、第一サーボ制御系２０が本来実現すべき軌道の一部を第二サーボ制御系３０に補償させる必要がない。つまり、コントローラ１１は、第二サーボ制御系３０の可動範囲を有効に利用することができる。
【０１３１】
以上に整理した通り、コントローラ１１は、高周波成分の除去によって第一サーボ制御系２０の追従性を維持しつつ、位相遅延の発生を防ぐことで第二サーボ制御系３０の可動範囲を有効に利用することができる。したがって、コントローラ１１は、第一サーボ制御系２０の追従性を維持しつつ、第一サーボ制御系２０および第二サーボ制御系３０の各々について、その可動範囲を有効に利用することができるという効果を奏する。
【０１３２】
コントローラ１１において、包絡線平均取得部１３１は、第一逆運動学軌道ＳＰを示す曲線の、上下の包絡線の平均から、補正後軌道ＳＰｆを生成する。
【０１３３】
前記の構成によれば、コントローラ１１は、第一逆運動学軌道ＳＰを示す曲線の、上下の包絡線の平均から、補正後軌道ＳＰｆを生成することにより、第一逆運動学軌道ＳＰからの位相遅延が発生しないようにして高周波成分を除去して補正後軌道ＳＰｆを生成する。つまり、コントローラ１１は、高周波成分の除去によって第一サーボ制御系２０の追従性を維持しつつ、位相遅延の発生を防ぐことで第二サーボ制御系３０の可動範囲を有効に利用することができる。
【０１３４】
したがって、コントローラ１１は、第一サーボ制御系２０の追従性を維持しつつ、第一サーボ制御系２０および第二サーボ制御系３０の各々について、その可動範囲を有効に利用することができるとの効果を奏する。
【０１３５】
これまでに概要を整理してきたコントローラ１１について、次に、その構成等の詳細について、図８等を用いて説明していく。
【０１３６】
（制御装置の詳細）
図８は、本発明の実施形態２に係るコントローラ等の要部構成等を示すブロック図である。上述の通り、「ゼロ位相フィルタ部１３０に代えて包絡線平均取得部１３１を備える」点以外は、コントローラ１１の構成は、コントローラ１０の構成と同様であるため、包絡線平均取得部１３１についてのみ、説明する。
【０１３７】
包絡線平均取得部１３１は、第一逆運動学演算部１２０から取得した第一逆運動学軌道ＳＰ（ｉ）を示す曲線の、上下の包絡線の平均から、補正後軌道ＳＰｆ（ｉ）を生成する。包絡線平均取得部１３１が第一逆運動学軌道ＳＰ（ｉ）からどのように補正後軌道ＳＰｆ（ｉ）を生成するのかを図９を用いて説明する。
【０１３８】
図９は、コントローラ１１が、目標軌道Ｔｔに逆運動学演算を行なって生成した第一逆運動学軌道ＳＰ（ｉ）を示す曲線の上下包絡線の平均から補正後軌道ＳＰｆ（ｉ）を生成する方法を説明する図である。図９において、縦軸は位置、横軸は時間（ｔ）である。なお、図９において、第一逆運動学軌道ＳＰ（ｉ）は「演算軌道」と略記し、第一逆運動学軌道ＳＰ（ｉ）を示す曲線の上包絡線を「上包絡線」と、第一逆運動学軌道ＳＰ（ｉ）を示す曲線の下包絡線を「下包絡線」と、略記している。さらに、「上包絡線」と「下包絡線」との平均を示す曲線を「平均線」と略記している。
【０１３９】
包絡線平均取得部１３１は、「第一逆運動学軌道ＳＰ（ｉ）を示す、時間軸を横軸とする曲線」について上下の包絡線を求め、その上下の包絡線の平均を示す線（図９における「平均線」）を、補正後軌道ＳＰｆ（ｉ）を示す曲線とする。包絡線平均取得部１３１は、「第一逆運動学軌道ＳＰ（ｉ）を示す、時間軸を横軸とする曲線」の上下包絡線の平均から補正後軌道ＳＰｆ（ｉ）を生成することにより、第一逆運動学軌道ＳＰ（ｉ）から、位相遅延が発生しないようにして高周波成分を除去する。
【０１４０】
以上を整理すれば、コントローラ１１は、目標軌道Ｔｔに逆運動学演算を行なって生成した軌道から位相遅延が発生しないようにして高周波成分を除去した軌道、第一アクチュエータ２２についての指令軌道とする。具体的には、コントローラ１０は、第一逆運動学軌道ＳＰ（ｉ）を示す曲線の上下包絡線の平均を使用する。
【０１４１】
コントローラ１１は、低速アクチュエータ（第一アクチュエータ２２）についての軌道（指令軌道）の位相遅れ（位相遅延）を低減させることにより、高速アクチュエータ（第二アクチュエータ３２）の動作範囲を節約することができる。したがって、コントローラ１１は、より高速の軌道に対応できる。また、コントローラ１１は、ローパスフィルタの効果を強めて、低速アクチュエータの追従性を向上させることができ、トータル（第一サーボ制御系２０および第二サーボ制御系３０の全体）の制御精度を向上することができる。さらに、コントローラ１１は、制御システム２において採用する高速アクチュエータについて選択肢を広がることができ、採用する高速アクチュエータについての選定を容易にすることができる。
【０１４２】
〔実施形態３〕
本発明の実施形態３について、図１０〜図１４に基づいて説明すれば、以下のとおりである。なお、説明の便宜上、前記実施形態にて説明した部材と同じ機能を有する部材については、同じ符号を付記し、その説明を省略する。本実施形態における制御システム３が、上述の実施形態１における制御システム１と異なるのは、本実施形態に係る制御装置であるコントローラ１２が、コントローラ１０の指示部１７０に代えて、ＭＰＣ指令部１８０を備えている点である。「指示部１７０に代えてＭＰＣ指令部１８０を備える」点以外の点においては、コントローラ１１の構成は、コントローラ１０の構成と同様である。なお、ＭＰＣは、「モデル予測制御（Model Predictive Control）」を意味しており、以下の説明においては、「モデル予測制御」を適宜、「ＭＰＣ」と略記することがある。
【０１４３】
（制御システムの概要）
図１１は、コントローラ１２を含む制御システム３の全体概要を示す図である。制御システム３のコントローラ１２には、予め作成されていた低速サーボシステム（第一サーボ制御系２０）の動特性モデルが設定される。コントローラ１２は、第一サーボ制御系２０の動特性モデルを用いて、第一サーボ制御系２０についてモデル予測制御を行なう。具体的には、コントローラ１２は、補正後軌道ＳＰｆ（ｉ）から第一サーボ制御系２０に対する制御周期ごとに生成する第一指令値Ｐｃ（ｉ）を、第一サーボ制御系２０の動特性モデルを用いたＭＰＣによって補正する。そして、コントローラ１２は、補正した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。コントローラ１２は、第二サーボ制御系３０の動特性モデルを用いて、第二サーボ制御系３０についてＭＰＣを行なってもよい。
【０１４４】
コントローラ１２は、ＭＰＣによる位置補正制御によって、低速アクチュエータ（第一アクチュエータ２２）側の応答遅れを低減させることにより、追従性能を向上させる。
【０１４５】
（制御装置の概要）
コントローラ１２（制御装置）は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆに対応する第一サーボ制御系２０の応答を予測する第一応答予測部１８３（予測部）を備えている。
【０１４６】
前記の構成によれば、コントローラ１２は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆに対応する第一サーボ制御系２０の応答を予測する。したがって、コントローラ１２は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆに対応する第一サーボ制御系２０の応答を予測することができるとの効果を奏する。
【０１４７】
コントローラ１２において、第一応答予測部１８３は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆから生成する第一指令値Ｐｃに対する第一サーボ制御系２０の出力である制御量を予測する。コントローラ１２は、第一応答予測部１８３によって予測された制御量と、第一サーボ制御系２０からフィードバック情報として取得する「第一サーボ制御系２０の制御量の実測値」と、を用いたモデル予測制御によって補正した第一指令値Ｐｃを、第一サーボ制御系２０に出力する第一ＭＰＣ位置指令部１８１（指示部）を備えている。
【０１４８】
前記の構成によれば、コントローラ１２は、「第一サーボ制御系２０の動特性モデルを用いて予測した制御量」と、「第一サーボ制御系２０の制御量の実測値」と、を用いたモデル予測制御によって補正した第一指令値Ｐｃを、第一サーボ制御系２０に出力する。
【０１４９】
したがって、コントローラ１２は、第一サーボ制御系２０の補正後軌道ＳＰｆへの追従性能を向上させることができ、さらに、第一サーボ制御系２０および第二サーボ制御系３０の全体の追従性能を向上させることができるとの効果を奏する。
【０１５０】
コントローラ１２は、第一サーボ制御系２０の動特性モデルを使用して応答遅れを予測し、予測した応答遅れを第一サーボ制御系２０において取り戻す協調制御コントローラである。すなわち、コントローラ１２は、第一サーボ制御系２０の動特性モデルを用いたモデル予測制御による位置補正制御によって、低速アクチュエータ（第一アクチュエータ２２）の軌道追従性を向上させる。
【０１５１】
また、コントローラ１２は、第二サーボ制御系３０の動特性モデルを用いたモデル予測制御による位置補正制御によって、高速アクチュエータ（第二アクチュエータ３２）の軌道追従性を向上させる。
【０１５２】
これまでに概要を整理してきたコントローラ１２について、次に、その構成等の詳細について、図１０等を用いて説明していく。
【０１５３】
（制御装置の詳細）
図１０は、コントローラ１２等の要部構成等を示すブロック図である。上述の通り、「指示部１７０に代えてＭＰＣ指令部１８０を備える」点以外は、コントローラ１２の構成は、コントローラ１０の構成と同様であるため、ＭＰＣ指令部１８０についてのみ、説明する。
【０１５４】
ＭＰＣ指令部１８０は、第一サーボ制御系２０に第一指令値Ｐｃ（ｉ）を出力する第一ＭＰＣ位置指令部１８１と、第二サーボ制御系３０に第二指令値Ｓｃ（ｉ）を出力する第二ＭＰＣ位置指令部１８２と、を含んでいる。
【０１５５】
第一ＭＰＣ位置指令部１８１は第一応答予測部１８３を含み、第一応答予測部１８３には、予め作成された第一サーボ制御系２０の動特性モデルが設定されている。第一応答予測部１８３は、第一サーボ制御系２０の動特性モデルを予め作成しておき、作成した第一サーボ制御系２０の動特性モデルを自身に設定しておいてもよい。
【０１５６】
第一ＭＰＣ位置指令部１８１は、以下の２つの処理を実行する。第一に、第一ＭＰＣ位置指令部１８１は、第一サーボ制御系２０に対する制御周期ごとに、「第一サーボ制御系２０についての指令軌道」から第一サーボ制御系２０の軸ごとの第一指令値Ｐｃ（ｉ）を生成する。例えば、第一ＭＰＣ位置指令部１８１は、記憶部１６０の第一指示軌道テーブル１６１を参照して、「第一サーボ制御系２０についての指令軌道」として、補正後軌道ＳＰｆ（ｉ）を取得する。そして、第一ＭＰＣ位置指令部１８１は、例えば１ｍｓごとに、補正後軌道ＳＰｆ（ｉ）から第一サーボ制御系２０の軸ごとの第一指令値Ｐｃ（ｉ）を生成する。
【０１５７】
第二に、第一ＭＰＣ位置指令部１８１は、補正後軌道ＳＰｆ（ｉ）から第一サーボ制御系２０に対する制御周期ごとに生成する第一指令値Ｐｃ（ｉ）を、モデル予測制御によって補正し、補正した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。
【０１５８】
具体的には、第一応答予測部１８３は、設定されている第一サーボ制御系２０の動特性モデルを用いて、第一指令値Ｐｃ（ｉ）に対する第一サーボ制御系２０の出力である制御量を予測する。第一ＭＰＣ位置指令部１８１は、第一応答予測部１８３によって予測された制御量と、第一サーボ制御系２０からフィードバック情報として取得する第一サーボ制御系２０の制御量の実測値と、を用いたモデル予測制御を実行する。すなわち、第一ＭＰＣ位置指令部１８１は、第一応答予測部１８３が第一サーボ制御系２０の動特性モデルを用いて予測した「第一サーボ制御系２０の出力である制御量」と、第一サーボ制御系２０の制御量の実測値と、を用いて第一指令値Ｐｃ（ｉ）を補正する。そして、第一ＭＰＣ位置指令部１８１は、モデル予測制御を用いて補正した第一指令値Ｐｃ（ｉ）を、第一サーボ制御系２０に対する制御周期ごとに、第一サーボ制御系２０に出力する。第一ＭＰＣ位置指令部１８１は、例えば１ｍｓごとに、補正した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。
【０１５９】
第二ＭＰＣ位置指令部１８２は第二応答予測部１８４を含み、第二応答予測部１８４には、予め作成された第二サーボ制御系３０の動特性モデルが設定されている。第二応答予測部１８４は、第二サーボ制御系３０の動特性モデルを予め作成しておき、作成した第二サーボ制御系３０の動特性モデルを自身に設定しておいてもよい。
【０１６０】
第二ＭＰＣ位置指令部１８２は、以下の２つの処理を実行する。第一に、第二ＭＰＣ位置指令部１８２は、第二サーボ制御系３０に対する制御周期ごとに、「第二サーボ制御系３０についての指令軌道」から第二サーボ制御系３０の軸ごとの第二指令値Ｓｃ（ｉ）を生成する。例えば、第二ＭＰＣ位置指令部１８２は、記憶部１６０の第二指示軌道テーブル１６２を参照して、「第二サーボ制御系３０についての指令軌道」として、第二逆運動学軌道ＩＫｔ（ｉ）を取得する。そして、第二ＭＰＣ位置指令部１８２は、例えば、１／１２ｍｓごとに、第二逆運動学軌道ＩＫｔ（ｉ）から第二サーボ制御系３０の軸ごとの第二指令値Ｓｃ（ｉ）を生成する。
【０１６１】
第二に、第二ＭＰＣ位置指令部１８２は、第二逆運動学軌道ＩＫｔ（ｉ）から第二サーボ制御系３０に対する制御周期ごとに生成する第二指令値Ｓｃ（ｉ）を、モデル予測制御によって補正し、補正した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。
【０１６２】
具体的には、第二応答予測部１８４は、設定されている第二サーボ制御系３０の動特性モデルを用いて、第二指令値Ｓｃ（ｉ）に対する第二サーボ制御系３０の出力である制御量を予測する。第二ＭＰＣ位置指令部１８２は、第二応答予測部１８４によって予測された制御量と、第二サーボ制御系３０からフィードバック情報として取得する第二サーボ制御系３０の制御量の実測値と、を用いたモデル予測制御を実行する。すなわち、第二ＭＰＣ位置指令部１８２は、第二応答予測部１８４が第二サーボ制御系３０の動特性モデルを用いて予測した「第二サーボ制御系３０の出力である制御量」と、第二サーボ制御系３０の制御量の実測値と、を用いて第二指令値Ｓｃ（ｉ）を補正する。そして、第二ＭＰＣ位置指令部１８２は、モデル予測制御を用いて補正した第二指令値Ｓｃ（ｉ）を、第二サーボ制御系３０に対する制御周期ごとに、第二サーボ制御系３０に出力する。第二ＭＰＣ位置指令部１８２は、例えば１／１２ｍｓごとに、補正した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。
【０１６３】
（動特性モデルについて）
第一サーボ制御系２０および第二サーボ制御系３０の各々の動特性モデルは、例えば、次の（式１）に示す離散時間伝達関数のように表される。（式１）において、ｕは入力を表し、ｙは出力（制御量の予測、つまり、制御量についての予測値）を表し、ｄ、ａ１〜ａｎおよびｂ１〜ｂｍは特性パラメータを表し、ｚ−１は遅延演算子を表している。
【０１６４】
【数１】
2019021086.tif 000003
【０１６５】
コントローラ１２において、第一サーボ制御系２０および第二サーボ制御系３０の各々について、（式１）に例示するような動特性モデルが予め作成され、作成された各々の動特性モデルは第一応答予測部１８３および第二応答予測部１８４の各々に設定される。
【０１６６】
なお、ＭＰＣ指令部１８０が第二ＭＰＣ位置指令部１８２および第二応答予測部１８４を備えることは必須ではなく、コントローラ１２は少なくとも第一ＭＰＣ位置指令部１８１および第一応答予測部１８３を備えればよい。すなわち、コントローラ１２は、第一サーボ制御系２０の動特性モデルを用いたモデル予測制御によって、低速アクチュエータ（第一アクチュエータ２２）側の応答遅れを低減させることができればよい。第二アクチュエータ３２にも追従性改善の余地がある場合、コントローラ１２は、第二サーボ制御系３０の動特性モデルを用いたモデル予測制御によって第二指令値Ｓｃ（ｉ）を補正して、第二アクチュエータ３２の追従性能を向上させてもよい。
【０１６７】
（コントローラが実行する処理について）
図１２は、コントローラ１２が実行する処理の概要を示すフロー図である。コントローラ１２は、第一サーボ制御系２０の動特性モデルを予め作成しておき、作成しておいた第一サーボ制御系２０の動特性モデルを用いて、低速アクチュエータ（第一アクチュエータ２２）側のＭＰＣ位置補正制御を実行する。具体的には、コントローラ１２は、第一サーボ制御系２０の動特性モデルを用いたＭＰＣによって第一指令値Ｐｃ（ｉ）を補正し、補正した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。高速アクチュエータ（第二アクチュエータ３２）にも追従性改善の余地がある場合、コントローラ１２は、第二アクチュエータ３２に対する制御についても、ＭＰＣ位置補正制御を適用してもよい。言い換えれば、コントローラ１２は、第二アクチュエータ３２に対する制御についてもＭＰＣ位置補正制御を適用することにより、第二アクチュエータ３２の応答遅れに起因する追従誤差を低減することができる。以下、コントローラ１２が第一アクチュエータ２２および第二アクチュエータ３２についてＭＰＣ位置補正制御を実行する場合の、コントローラ１２が実行する処理の詳細について、図１２を用いて説明していく。
【０１６８】
コントローラ１２は、低速サーボシステム（第一サーボ制御系２０）および高速サーボシステム（第二サーボ制御系３０）の各々について、動特性モデルを予め作成しておき、第一応答予測部１８３および第二応答予測部１８４に設定しておく（Ｓ２１０）。
【０１６９】
第一逆運動学演算部１２０は、目標軌道Ｔｔから第一アクチュエータ２２の軸毎の指令値を生成し、つまり、目標軌道Ｔｔに逆運動学演算を行なって第一逆運動学軌道ＳＰ（ｉ）を生成する（Ｓ２２０）。
【０１７０】
ゼロ位相フィルタ部１３０は、軸毎の軌道（つまり、第一逆運動学軌道ＳＰ（ｉ））に対して、ゼロ位相フィルタリングを実行し、生成した補正後軌道ＳＰｆ（ｉ）を、記憶部１６０の第一指示軌道テーブル１６１に格納する（Ｓ２３０）。
【０１７１】
コントローラ１２は、「ゼロ位相フィルタリング後の指令軌道から生成される指令値と、目標軌道の指令値と、の差分」を計算し、計算した差分から高速アクチュエータ（第二アクチュエータ３２）の軸毎の指令値を生成する。
【０１７２】
例えば、順運動学演算部１４０は、順運動学演算により、ゼロ位相フィルタリング後の軌道（つまり、補正後軌道ＳＰｆ（ｉ））から中間軌道（つまり、順運動学軌道ＦＫｔ）を生成する（Ｓ２４０）。そして、第二逆運動学演算部１５０は、中間軌道と目標軌道Ｔｔとの差分（誤差）を計算する（Ｓ２５０）。そして、第二逆運動学演算部１５０は、Ｓ２５０で計算した誤差（差分）を用いて、第二アクチュエータ３２の軸毎の軌道を計算し、記憶部１６０の第二指示軌道テーブル１６２に格納する（Ｓ２６０）。具体的には、第二逆運動学演算部１５０は、「中間軌道（つまり、順運動学軌道ＦＫｔ）と、目標軌道Ｔｔと、の誤差」に逆運動学演算を行なって、第二サーボ制御系３０の軸ごとの指令軌道である第二逆運動学軌道ＩＫｔ（ｉ）を生成する。そして、第二逆運動学演算部１５０は、生成した第二逆運動学軌道ＩＫｔ（ｉ）を、記憶部１６０の第二指示軌道テーブル１６２に格納する。
【０１７３】
第二逆運動学演算部１５０は、第一逆運動学軌道ＳＰと補正後軌道ＳＰｆとの誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。具体的には、第二逆運動学演算部１５０は、「補正後軌道ＳＰｆと、第二逆運動学軌道ＩＫｔとの合成軌道が、目標軌道Ｔｔに一致する」ようにして、第一逆運動学軌道ＳＰと補正後軌道ＳＰｆとの誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。
【０１７４】
コントローラ１２は、ＭＰＣ位置補正制御により、低速サーボシステムおよび高速サーボシステムの各々への補正後指令値を計算し、計算した補正後指令値を低速サーボシステムおよび高速サーボシステムの各々に出力する（Ｓ２７０）。具体的には、ＭＰＣ指令部１８０は、第一指令値Ｐｃ（ｉ）と第二指令値Ｓｃ（ｉ）との各々を、第一サーボ制御系２０および第二サーボ制御系３０の動特性モデルを用いたＭＰＣにより補正し、補正した第一指令値Ｐｃ（ｉ）と第二指令値Ｓｃ（ｉ）とを出力する。
【０１７５】
第一ＭＰＣ位置指令部１８１は、第一指示軌道テーブル１６１に格納されている補正後軌道ＳＰｆ（ｉ）から第一サーボ制御系２０に対する制御周期ごとに生成される第一指令値Ｐｃ（ｉ）を、第一サーボ制御系２０の動特性モデルを用いたＭＰＣにより補正する。そして、第一ＭＰＣ位置指令部１８１は、補正した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。
【０１７６】
第二ＭＰＣ位置指令部１８２は、第二指示軌道テーブル１６２に格納されている第二逆運動学軌道ＩＫｔ（ｉ）から第二サーボ制御系３０に対する制御周期ごとに生成される第二指令値Ｓｃ（ｉ）を、第二サーボ制御系３０の動特性モデルを用いたＭＰＣにより補正する。そして、第二ＭＰＣ位置指令部１８２は、補正した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。そして、コントローラ１２は、軌道終点に到達したかを判定しながら（Ｓ２８０）、制御周期ごとに、Ｓ２５０〜Ｓ２７０の処理を、または、Ｓ２７０の処理を、繰り返す。具体的には、コントローラ１２は、「Ｓ２５０〜Ｓ２７０の処理、または、Ｓ２７０の処理」のうち、「第一サーボ制御系２０のみに係る処理」を、軌道終点に到達したかを判定しながら（Ｓ２８０）、第一サーボ制御系２０に対する制御周期で繰り返し実行する。また、コントローラ１２は、「Ｓ２５０〜Ｓ２７０の処理、または、Ｓ２７０の処理」のうち、「第一サーボ制御系２０のみに係る処理」以外の処理（例えば、第二サーボ制御系３０に係る処理）を、軌道終点に到達したかを判定しながら（Ｓ２８０）、第二サーボ制御系３０に対する制御周期で繰り返し実行する。
【０１７７】
（コントローラが奏する効果について）
図１３は、図５に示す制御テストについて、コントローラ１２によって制御された第一アクチュエータ２２および第二アクチュエータ３２の各々の位置、位置偏差、モデル予測誤差、およびトルクの変化を示す図である。図１３の（Ａ）には、上から順に、コントローラ１２によって制御された第一アクチュエータ２２の位置、位置偏差、モデル予測誤差、およびトルクの変化が示されている。図１３の（Ｂ）には、上から順に、コントローラ１２によって制御された第二アクチュエータ３２の位置、位置偏差、モデル予測誤差、およびトルクの変化が示されている。図１３に示す各図において、横軸はいずれも時間（ｔ）である。
【０１７８】
図１４は、図５に示す制御テストについて、コントローラ１２によって制御された第一アクチュエータ２２および第二アクチュエータ３２の合計の位置（図１４の（Ａ））および位置偏差（図１４の（Ｂ））の変化を示す図である。図１４に示す各図において、横軸はいずれも時間（ｔ）である。なお、図１４の（Ａ）において、指令値と低速高速合成制御量（第一アクチュエータ２２の制御量の実測値と第二アクチュエータ３２の制御量の実測値とを合計（合成）した値）とは、ほぼ重なっている。
【０１７９】
コントローラ１２は、第一逆運動学軌道ＳＰ（ｉ）に、「時間軸の逆方向から順方向へ」の順にローパスフィルタを一往復実行して、「第一サーボ制御系２０についての指令軌道」を生成する。つまり、コントローラ１２は、第一逆運動学軌道ＳＰ（ｉ）に、「時間軸の逆方向から順方向へ」の順にゼロ位相フィルタ処理を実行して、「第一サーボ制御系２０についての指令軌道」を生成する。コントローラ１２の用いたローパスフィルタのローパスフィルタ特性は、カットオフ周波数１０Ｈｚの２次バターワース型である。コントローラ１２は、さらに、第一サーボ制御系２０および第二サーボ制御系３０の動特性モデルを用いたＭＰＣにより第一指令値Ｐｃ（ｉ）と第二指令値Ｓｃ（ｉ）との各々を補正し、追従性能を向上させている。
【０１８０】
コントローラ１２は、コントローラ１０と同様に、図１８および図１９に制御結果を示した従来のコントローラと比べて、低速アクチュエータについての指令軌道の位相遅れを低減させることにより、高速アクチュエータの動作範囲を節約することができている。また、図１４の（Ｂ）に例示する位置偏差は、図７の（Ｂ）に例示する位置偏差よりも格段に小さくなっている。
【０１８１】
〔実施形態４〕
本発明の実施形態４について、図１５〜図１７に基づいて説明すれば、以下のとおりである。なお、説明の便宜上、前記実施形態にて説明した部材と同じ機能を有する部材については、同じ符号を付記し、その説明を省略する。本実施形態における制御システム４が、上述の実施形態１における制御システム１と異なるのは、本実施形態に係る制御装置であるコントローラ１３が、コントローラ１０の構成に加えて、応答予測部１９０を備えている点である。「応答予測部１９０をさらに備える」点以外の点においては、コントローラ１３の構成は、コントローラ１０の構成と同様である。
【０１８２】
（制御システムの概要）
図１６は、コントローラ１３を含む制御システム４の全体概要を示す図である。制御システム４のコントローラ１３には、予め作成されていた低速サーボシステム（第一サーボ制御系２０）の動特性モデルが設定される。コントローラ１３は、ゼロ位相フィルタ処理後の補正後軌道ＳＰｆ（ｉ）に対応する低速サーボ（つまり、第一サーボ制御系２０）の応答を、設定されている低速サーボシステムの動特性モデルを用いて、予測する。
【０１８３】
ここで、前述のコントローラ１２とコントローラ１３とは、第一サーボ制御系２０の動特性モデルを使用して応答遅れを予測する点において共通している。コントローラ１２とコントローラ１３とは、予測した応答遅れをどこで取り戻すかが異なっている。コントローラ１２が予測した応答遅れを第一サーボ制御系２０において取り戻すのに対して、コントローラ１３は、予測した応答遅れを第二サーボ制御系３０（第二アクチュエータ３２）において取り戻す。
【０１８４】
コントローラ１３は、第一アクチュエータ２２の応答を予測し、応答遅れ分を第二アクチュエータ３２についての指令軌道に反映することによって、応答遅れ分を第二サーボ制御系３０（第二アクチュエータ３２）において取り戻す。具体的には、コントローラ１３は、「予測した第一サーボ制御系２０の応答に順運動学演算を行なって生成した中間軌道と、目標軌道Ｔｔと、の誤差」に逆運動学演算を行なって、第二サーボ制御系３０についての指令軌道を生成する。
【０１８５】
上述の方法により、コントローラ１３は、「予測した第一サーボ制御系２０（第一アクチュエータ２２）の応答」を反映させた「第二サーボ制御系３０についての指令軌道」を生成する。そして、コントローラ１３は、「予測した第一サーボ制御系２０の応答」を反映させた「第二サーボ制御系３０についての指令軌道」から、「第二サーボ制御系３０に対する指令値」である第二指令値Ｓｃ（ｉ）を生成する。コントローラ１３は、生成した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力することにより、第一サーボ制御系２０および第二サーボ制御系３０の全体の追従性能を向上することができる。
【０１８６】
つまり、コントローラ１３は、第一アクチュエータ２２側の応答遅れ分を第二アクチュエータ３２側でカバーさせることにより、第一アクチュエータ２２と第二アクチュエータ３２との合計（総合）の追従性能を向上させる。ただし、コントローラ１３による制御は、コントローラ１２による制御に比べて、第二アクチュエータ３２の動作範囲を広くする傾向がある。
【０１８７】
（制御装置の概要）
コントローラ１３（制御装置）は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆに対応する第一サーボ制御系２０の応答を予測する応答予測部１９０（予測部）を備えている。
【０１８８】
前記の構成によれば、コントローラ１３は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆに対応する第一サーボ制御系２０の応答を予測する。したがって、コントローラ１３は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆに対応する第一サーボ制御系２０の応答を予測することができるとの効果を奏する。
【０１８９】
コントローラ１３において、第二逆運動学演算部１５０（第二軌道生成部）は、応答予測部１９０によって予測された第一サーボ制御系２０の応答を用いて生成する順運動学軌道ＦＫｔ（中間軌道）と、第一逆運動学軌道ＳＰ（または、目標軌道Ｔｔ）と、の誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔ（第二指令軌道）を生成する。
【０１９０】
すなわち、第二逆運動学演算部１５０は、「第二サーボ制御系３０についての指令軌道」として、「ゼロ位相フィルタ部１３０により第一逆運動学軌道ＳＰ（ｉ）から除去された高周波成分」に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。具体的には、第二逆運動学演算部１５０は、第一逆運動学軌道ＳＰと、応答予測部１９０によって予測された第一サーボ制御系２０の応答から生成される中間軌道と、の誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。第二逆運動学演算部１５０は、「中間軌道と、第二逆運動学軌道ＩＫｔとの合成軌道が、目標軌道Ｔｔに一致する」ようにして、第一逆運動学軌道ＳＰと中間軌道との誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。
【０１９１】
また第二逆運動学演算部１５０は、例えば、目標軌道Ｔｔと、中間軌道と、の誤差を用いて、第二逆運動学軌道ＩＫｔを生成してもよい。すなわち、第二逆運動学演算部１５０は、「中間軌道と、第二逆運動学軌道ＩＫｔとの合成軌道が、目標軌道Ｔｔに一致する」ようにして、目標軌道Ｔｔと中間軌道との誤差を用いて、第二逆運動学軌道ＩＫｔを生成してもよい。
【０１９２】
前記の構成によれば、コントローラ１３は、応答予測部１９０によって予測された第一サーボ制御系２０の応答を用いて生成する順運動学軌道ＦＫｔと、第一逆運動学軌道ＳＰ（または、目標軌道Ｔｔ）と、の誤差に対応する軌道を含む第二逆運動学軌道ＩＫｔを生成する。
【０１９３】
したがって、コントローラ１３は、第一サーボ制御系２０の応答遅れを第二サーボ制御系３０によって補償することができ、第一サーボ制御系２０および第二サーボ制御系３０の全体の追従性能を向上させることができるとの効果を奏する。
【０１９４】
これまでに概要を整理してきたコントローラ１３について、次に、その構成等の詳細について、図１５等を用いて説明していく。
【０１９５】
（制御装置の詳細）
図１５は、本発明の実施形態４に係るコントローラ１３等の要部構成等を示すブロック図である。上述の通り、「応答予測部１９０をさらに備える」点以外は、コントローラ１３の構成は、コントローラ１０の構成と同様であるため、応答予測部１９０についてのみ、説明する。
【０１９６】
応答予測部１９０は、低速サーボシステム（つまり、第一サーボ制御系２０）の動特性モデルを予め作成し、作成しておいた動特性モデルを用いて、ゼロ位相フィルタ部１３０が生成した補正後軌道ＳＰｆ（ｉ）に対応する第一サーボ制御系２０の応答を予測する。応答予測部１９０は、予測した第一サーボ制御系２０の応答を、順運動学演算部１４０に出力し、順運動学演算部１４０は、応答予測部１９０により予測された第一サーボ制御系２０の応答に対する順運動学演算から、順運動学軌道ＦＫｔ（中間軌道）を生成する。応答予測部１９０が用いる「第一サーボ制御系２０の動特性モデル」は、例えば、前述の（式１）に示す離散時間伝達関数のように表されてもよい。
【０１９７】
（コントローラが実行する処理について）
図１７は、コントローラ１３が実行する処理の概要を示すフロー図である。先ず、低速サーボシステム（第一サーボ制御系２０）の動特性モデルを作成して、低速サーボ応答予測部（つまり、応答予測部１９０）に設定しておく（Ｓ３１０）。
【０１９８】
第一逆運動学演算部１２０は、目標軌道Ｔｔから第一アクチュエータ２２の軸毎の指令値を生成し、つまり、目標軌道Ｔｔに逆運動学演算を行なって第一逆運動学軌道ＳＰ（ｉ）を生成する（Ｓ３２０）。
【０１９９】
ゼロ位相フィルタ部１３０は、軸毎の軌道（つまり、第一逆運動学軌道ＳＰ（ｉ））に対して、ゼロ位相フィルタリングを実行し、生成した補正後軌道ＳＰｆ（ｉ）を、記憶部１６０の第一指示軌道テーブル１６１に格納する（Ｓ３３０）。
【０２００】
応答予測部１９０は、第一サーボ制御系２０の動特性モデルを用いて、補正後軌道ＳＰｆ（ｉ）に対する低速サーボシステムの軸毎の応答を予測計算し、つまり、補正後軌道ＳＰｆ（ｉ）に対応する第一サーボ制御系２０の応答を予測する（Ｓ３４０）。
【０２０１】
コントローラ１３は、「予測応答（第一サーボ制御系２０の制御量についての予測値）と、目標軌道の指令値と、の差分」を計算し、計算した差分から高速アクチュエータ（第二アクチュエータ３２）の軸毎の指令値を生成する。
【０２０２】
例えば、順運動学演算部１４０は、応答予測部１９０によって予測された第一サーボ制御系２０の応答（第一サーボ制御系２０の制御量についての予測値）に、順運動学演算を行なって、中間軌道（つまり、順運動学軌道ＦＫｔ）を生成する（Ｓ３５０）。そして、第二逆運動学演算部１５０は、中間軌道と目標軌道Ｔｔとの差分（誤差）を計算する（Ｓ３６０）。そして、第二逆運動学演算部１５０は、Ｓ３６０で計算した誤差（差分）を用いて、第二アクチュエータ３２の軸毎の軌道を計算し、記憶部１６０の第二指示軌道テーブル１６２に格納する（Ｓ３７０）。具体的には、第二逆運動学演算部１５０は、「中間軌道（つまり、順運動学軌道ＦＫｔ）と、目標軌道Ｔｔと、の誤差」に逆運動学演算を行なって、第二サーボ制御系３０の軸ごとの指令軌道である第二逆運動学軌道ＩＫｔ（ｉ）を生成する。そして、第二逆運動学演算部１５０は、生成した第二逆運動学軌道ＩＫｔ（ｉ）を、記憶部１６０の第二指示軌道テーブル１６２に格納する。第二逆運動学演算部１５０は、第一逆運動学軌道ＳＰと「応答予測部１９０によって予測された第一サーボ制御系２０の応答から生成した中間軌道」との誤差に対応する軌道を含む、第二逆運動学軌道ＩＫｔを生成する。
【０２０３】
指示部１７０は、格納している軸毎の軌道から、現時刻の指令値を、低速サーボシステム（つまり、第一サーボ制御系２０）および高速サーボシステム（つまり、第二サーボ制御系３０）へ出力する（Ｓ３８０）。具体的には、第一指示部１７１は、第一指示軌道テーブル１６１に格納されている補正後軌道ＳＰｆ（ｉ）から、第一サーボ制御系２０に対する制御周期ごとに、第一指令値Ｐｃ（ｉ）を生成し、生成した第一指令値Ｐｃ（ｉ）を第一サーボ制御系２０に出力する。また、第二指示部１７２は、第二指示軌道テーブル１６２に格納されている第二逆運動学軌道ＩＫｔ（ｉ）から、第二サーボ制御系３０に対する制御周期ごとに、第二指令値Ｓｃ（ｉ）を生成し、生成した第二指令値Ｓｃ（ｉ）を第二サーボ制御系３０に出力する。コントローラ１３は、軌道終点に到達したかを判定しながら（Ｓ３９０）、制御周期ごとに、Ｓ３６０〜Ｓ３８０の処理を、または、Ｓ３８０の処理を、繰り返す。具体的には、コントローラ１３は、「Ｓ３６０〜Ｓ３８０の処理、または、Ｓ３８０の処理」のうち、「第一サーボ制御系２０のみに係る処理」を、軌道終点に到達したかを判定しながら（Ｓ３９０）、第一サーボ制御系２０に対する制御周期で繰り返し実行する。また、コントローラ１３は、「Ｓ３６０〜Ｓ３８０の処理、または、Ｓ３８０の処理」のうち、「第一サーボ制御系２０のみに係る処理」以外の処理（例えば、第二サーボ制御系３０に係る処理）を、軌道終点に到達したかを判定しながら（Ｓ３９０）、第二サーボ制御系３０に対する制御周期で繰り返し実行する。
【０２０４】
〔変形例〕
こでまで、コントローラ１０、１１、１２、および１３が第一サーボ制御系２０および第二サーボ制御系３０という２つのサーボ制御系を協調制御する例について説明してきた。しかしながら、コントローラ１０、１１、１２、および１３が協調制御するサーボ制御系が２つであることは必須ではない。コントローラ１０、１１、１２、および１３は、複数のサーボ制御系の各々に、目標軌道Ｔｔを用いて生成した指令値を出力して、前記複数のサーボ制御系を協調制御できればよい。
【０２０５】
〔ソフトウェアによる実現例〕
コントローラ１０、１１、１２、および１３の制御ブロック（特に、目標軌道取得部１１０、第一逆運動学演算部１２０、ゼロ位相フィルタ部１３０、包絡線平均取得部１３１、順運動学演算部１４０、第二逆運動学演算部１５０、指示部１７０、第一指示部１７１、第二指示部１７２、ＭＰＣ指令部１８０、第一ＭＰＣ位置指令部１８１、第二ＭＰＣ位置指令部１８２、第一応答予測部１８３、第二応答予測部１８４、および、応答予測部１９０）は、集積回路（ＩＣチップ）等に形成された論理回路（ハードウェア）によって実現してもよいし、ソフトウェアによって実現してもよい。
【０２０６】
後者の場合、コントローラ１０、１１、１２、および１３は、各機能を実現するソフトウェアであるプログラムの命令を実行するコンピュータを備えている。このコンピュータは、例えば１つ以上のプロセッサを備えていると共に、上記プログラムを記憶したコンピュータ読み取り可能な記録媒体を備えている。そして、上記コンピュータにおいて、上記プロセッサが上記プログラムを上記記録媒体から読み取って実行することにより、本発明の目的が達成される。上記プロセッサとしては、例えばＣＰＵ（Central Processing Unit）を用いることができる。上記記録媒体としては、「一時的でない有形の媒体」、例えば、ＲＯＭ（Read Only Memory）等の他、テープ、ディスク、カード、半導体メモリ、プログラマブルな論理回路などを用いることができる。また、上記プログラムを展開するＲＡＭ（Random Access Memory）などをさらに備えていてもよい。また、上記プログラムは、該プログラムを伝送可能な任意の伝送媒体（通信ネットワークや放送波等）を介して上記コンピュータに供給されてもよい。なお、本発明の一態様は、上記プログラムが電子的な伝送によって具現化された、搬送波に埋め込まれたデータ信号の形態でも実現され得る。
【０２０７】
本発明は上述した各実施形態に限定されるものではなく、請求項に示した範囲で種々の変更が可能であり、異なる実施形態にそれぞれ開示された技術的手段を適宜組み合わせて得られる実施形態についても本発明の技術的範囲に含まれる。
【符号の説明】
【０２０８】
１０      コントローラ（制御装置）
１１      コントローラ（制御装置）
１２      コントローラ（制御装置）
１３      コントローラ（制御装置）
２０      第一サーボ制御系
３０      第二サーボ制御系
１３０    ゼロ位相フィルタ部（第一軌道生成部）
１３１    包絡線平均取得部（第一軌道生成部）
１５０    第二逆運動学演算部（第二軌道生成部）
１８１    第一ＭＰＣ位置指令部（指示部）
１８３    第一応答予測部（予測部）
１９０    応答予測部（予測部）
ＦＫｔ    順運動学軌道（中間軌道）
ＩＫｔ    第二逆運動学軌道（第二指令軌道）
Ｐｃ      第一指令値
ＳＰ      第一逆運動学軌道（基準軌道）
ＳＰｆ    補正後軌道（第一指令軌道）
Ｓ１２０  第一軌道生成ステップ
Ｓ１５０  第二軌道生成ステップ
【図１】
2019021086.tif 000004
【図２】
2019021086.tif 000005
【図３】
2019021086.tif 000006
【図４】
2019021086.tif 000007
【図５】
2019021086.tif 000008
【図６】
2019021086.tif 000009
【図７】
2019021086.tif 000010
【図８】
2019021086.tif 000011
【図９】
2019021086.tif 000012
【図１０】
2019021086.tif 000013
【図１１】
2019021086.tif 000014
【図１２】
2019021086.tif 000015
【図１３】
2019021086.tif 000016
【図１４】
2019021086.tif 000017
【図１５】
2019021086.tif 000018
【図１６】
2019021086.tif 000019
【図１７】
2019021086.tif 000020
【図１８】
2019021086.tif 000021
【図１９】
2019021086.tif 000022