(57)【特許請求の範囲】
【請求項１】
駆動部と、動力伝達部と、負荷部と、駆動側位置検出部と、負荷側位置検出部と、制御部とを備えており、
前記動力伝達部は、前記駆動部から前記負荷部に動力を伝達する構成となっており、
前記駆動側位置検出部は、前記駆動部における駆動側位置情報を取得する構成となっており、
前記負荷側位置検出部は、前記負荷部における負荷側位置情報を取得する構成となっており、
前記制御部は、前記負荷側位置情報（ωＬ又はθＬ）と、前記駆動側位置情報（ωＭ又はθＭ）とを用いて、前記駆動部への指令値（ω＊Ｍ又はθ＊Ｍ）を算出することによって、前記動力伝達部で伝達される動力（Ｔｓ）を制御する構成となっており、
前記制御部は、非線形要素補償ＦＦ部を備えており、
前記非線形要素補償ＦＦ部は、前記動力伝達部における非線形要素をフィードフォワード補償するための、位置についての出力値（Δω＊又はΔθ＊）を、前記非線形要素の逆関数を用いて生成する構成となっており、
さらに、前記非線形補償ＦＦ部への入力値は、前記動力伝達部における動力の指令値（Ｔ＊ｓ）となっており、
前記制御部は、前記非線形補償ＦＦ部からの、位置についての出力値（Δω＊又はΔθ＊）と、前記負荷側位置情報（ωＬ又はθＬ）との加算の結果を用いて、前記駆動部への指令値（ω＊Ｍ又はθ＊Ｍ）を生成する構成となっている
制御装置。
【請求項２】
前記非線形要素の逆関数としては、前記動力伝達部における不感帯の逆関数モデルが用いられている
請求項１に記載の制御装置。
【請求項３】
前記制御部は、駆動側位置ＦＢ部を備えており、
前記駆動側位置ＦＢ部は、前記駆動側位置情報を用いて、前記駆動側位置をフィードバック制御する構成となっている
請求項１又は２に記載の制御装置。
【請求項４】
前記制御部は、動力ＦＢ部を備えており、
前記動力ＦＢ部は、前記駆動側位置情報を用いて推定した推定動力を用いて、前記動力伝達部における動力の推定値をフィードバック制御する構成となっている
請求項１〜３のいずれか１項に記載の制御装置。
【請求項５】
前記制御部は、駆動側位置ＦＦ部を備えており、
前記駆動側位置ＦＦ部は、前記駆動部に対する前記指令値に対して、実際の位置が位置指令値に一致するようにフィードフォワード補償を行うことによって、駆動側位置における制御帯域を上昇させる構成となっている
請求項１〜４のいずれか１項に記載の制御装置。
【請求項６】
前記駆動部は、回転型モータであり、
前記動力伝達部は、回転軸を用いて前記動力を伝達する構成となっており、
前記動力は、前記回転軸における軸トルクである
請求項１〜５のいずれか１項に記載の制御装置。
【請求項７】
前記位置は、角度又は角速度である
請求項１〜６のいずれか１項に記載の制御装置。
【請求項８】
前記駆動部は、リニアモータであり、
前記動力伝達部は、前記駆動部から前記負荷側に並進力を伝達する構成となっており、
前記動力は、前記動力伝達部で伝達される並進力である
請求項１〜５のいずれか１項に記載の制御装置。
【請求項９】
駆動部と、動力伝達部と、負荷部と、駆動側位置検出部と、負荷側位置検出部と、制御部とを備えた制御装置を用いており、
前記動力伝達部は、前記駆動部から前記負荷部に動力を伝達する構成となっており、
前記駆動側位置検出部は、前記駆動部における駆動側位置情報を取得する構成となっており、
前記負荷側位置検出部は、前記負荷部における負荷側位置情報を取得する構成となっており、
前記制御部が、前記負荷側位置情報と、前記駆動側位置情報とを用いて、前記駆動部への指令値を算出することによって、前記動力伝達部で伝達される動力を制御するステップを備えており、
ここで、前記制御部は、非線形要素補償ＦＦ部を備えており、
前記制御部による前記ステップは、
前記非線形要素補償ＦＦ部が、前記動力伝達部における非線形要素をフィードフォワード補償するための、位置についての出力値（Δω＊又はΔθ＊）を、前記非線形要素の逆関数を用いて生成するステップと、
前記制御部が、前記非線形補償ＦＦ部からの、位置についての出力値（Δω＊又はΔθ＊）と、前記負荷側位置情報（ωＬ又はθＬ）との加算の結果を用いて、前記駆動部への指令値（ω＊Ｍ又はθ＊Ｍ）を生成するステップとを備えており、
ここで、前記非線形補償ＦＦ部への入力値は、前記動力伝達部における動力の指令値（Ｔ＊ｓ）となっている
制御方法。
【請求項１０】
請求項９に記載のステップをコンピュータに実行させるためのコンピュータプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、いわゆる２慣性系としてモデル化可能な制御対象を制御するための技術に関するものである。
【背景技術】
【０００２】
下記において、右肩の（ｎ）は、明細書末尾の参考文献番号を示す。
【０００３】
近年、エンコーダの高分解能化、低コスト化が進んでおり、安価になった高分解能エンコーダが様々な産業分野で使われるようになってきた。従来、工作機械や産業用ロボット、福祉ロボット等の分野では、駆動側エンコーダを用いて駆動側情報をフィードバックするセミクローズド制御が一般に行なわれていた(1)(2)。しかしセミクローズド制御では、モータと負荷との間の軸ねじれにより、最終位置決めをする負荷側における位置決め精度が劣化してしまう。駆動側だけでなく負荷側にもエンコーダを用いてフィードバックするフルクローズド制御をすることで、最終位置決め精度を上げることができるため、工作機械等の分野では負荷側にもエンコーダを用いるようになってきている。産業用ロボットでは負荷側にエンコーダを付けることが困難とされてきたが、負荷側にエンコーダを備える産業用ロボットモジュールも近年提案されてきている。このように、今後最終位置決め精度の向上を狙い、負荷側にエンコーダを備える装置の利用は拡大していくと考えられる。
【０００４】
このような産業界の流れがあるにも関わらず、負荷側エンコーダを用いた研究が十分にされているとは言い難い。そこで本発明者らは、負荷側エンコーダを用いた制御法をこれまで提案してきた(3)〜(5)。今後エンコーダの低コスト化により、負荷側におけるエンコーダの利用はますます広がっていくと思われるため、駆動側と負荷側の双方のエンコーダ情報が得られる系における新たな制御法が求められている。
【発明の概要】
【発明が解決しようとする課題】
【０００５】
そこで、本発明者らは、負荷側エンコーダを用いた軸トルク制御法の研究を行っている。２慣性系における軸トルクを制御することによって、産業用ロボットにおいては組立作業などの高度な作業が可能となると考えられる。また、福祉用ロボットあるいはEV（Electric Vehicle）においては、利便性や安全性の向上を図ることができ、さらにはバックドライバビリティを実現できると考えられる(6)(7)。
【０００６】
バックドライバビリティが求められるウェアラブルロボット等の分野においては、Serial Elastic Actuator (SEA)がよく用いられている(8)(9)。一般に制御対象が剛性の低い要素を持つ場合、制御性能は劣化するため、制御対象の剛性はできるだけ高くすることが望ましい(10)。SEAは、柔軟なばね要素を持つことで、制御性能を犠牲にしてバックドライバビリティを確保したアクチュエータと言える。
【０００７】
また従来の軸トルク制御法として，トルクセンサを用いて軸トルクを制御する方式がある。しかしながら、トルクセンサを用いると、センサ部分が変形するため、系全体の剛性を下げることになる。
【０００８】
したがって、剛性を下げる要因となるトルクセンサを用いず、またSEAのようにハードウェアを変更することなく軸トルクを制御する方法が望まれている。本発明により、減速機を有するサーボモータのバックドライバビリティを実現することも可能になると考えられる。
【０００９】
本発明は、前記の状況に鑑みてなされたものである。本発明の主な目的は、負荷側の位置情報を用いて、動力伝達部で伝達される動力を制御する技術を提供することである。
【課題を解決するための手段】
【００１０】
前記した課題を解決する手段は、以下の項目のように記載できる。
【００１１】
（項目１）
駆動部と、動力伝達部と、負荷部と、駆動側位置検出部と、負荷側位置検出部と、制御部とを備えており、
前記動力伝達部は、前記駆動部から前記負荷部に動力を伝達する構成となっており、
前記駆動側位置検出部は、前記駆動部における駆動側位置情報を取得する構成となっており、
前記負荷側位置検出部は、前記負荷部における負荷側位置情報を取得する構成となっており、
前記制御部は、前記負荷側位置情報と、前記駆動側位置情報とを用いて、前記駆動部への指令値を算出することによって、前記動力伝達部で伝達される動力を制御する構成となっている
制御装置。
【００１２】
（項目２）
前記制御部は、非線形要素補償ＦＦ部を備えており、
前記非線形要素補償ＦＦ部は、前記動力伝達部における非線形要素を陽にフィードフォワード補償する構成となっている
項目１に記載の制御装置。
【００１３】
（項目３）
前記制御部は、駆動側位置ＦＢ部を備えており、
前記駆動側位置ＦＢ部は、前記駆動側位置情報を用いて、前記駆動側位置をフィードバック制御する構成となっている
項目１又は２に記載の制御装置。
【００１４】
（項目４）
前記制御部は、動力ＦＢ部を備えており、
前記動力ＦＢ部は、前記駆動側位置情報を用いて推定した推定動力を用いて、前記動力伝達部における動力の推定値をフィードバック制御する構成となっている
項目１〜３のいずれか１項に記載の制御装置。
【００１５】
（項目５）
前記制御部は、駆動側位置ＦＦ部を備えており、
前記駆動側位置ＦＦ部は、前記駆動部に対する前記指令値に対して、実際の位置が位置指令値に一致するようにフィードフォワード補償を行うことによって、駆動側位置における制御帯域を上昇させる構成となっている
項目１〜４のいずれか１項に記載の制御装置。
【００１６】
（項目６）
前記駆動部は、回転型モータであり、
前記動力伝達部は、回転軸を用いて前記動力を伝達する構成となっており、
前記動力は、前記回転軸における軸トルクである
項目１〜５のいずれか１項に記載の制御装置。
【００１７】
（項目７）
前記位置は、角度又は角速度である
項目１〜６のいずれか１項に記載の制御装置。
【００１８】
（項目８）
前記駆動部は、リニアモータであり、
前記動力伝達部は、前記駆動部から前記負荷側に並進力を伝達する構成となっており、
前記動力は、前記動力伝達部で伝達される並進力である
項目１〜５のいずれか１項に記載の制御装置。
【００１９】
（項目９）
駆動部と、動力伝達部と、負荷部と、駆動側位置検出部と、負荷側位置検出部と、制御部とを備えた制御装置を用いており、
前記動力伝達部は、前記駆動部から前記負荷部に動力を伝達する構成となっており、
前記駆動側位置検出部は、前記駆動部における駆動側位置情報を取得する構成となっており、
前記負荷側位置検出部は、前記負荷部における負荷側位置情報を取得する構成となっており、
前記制御部が、前記負荷側位置情報と、前記駆動側位置情報とを用いて、前記駆動部への指令値を算出することによって、前記動力伝達部で伝達される動力を制御するステップを備える
制御方法。
【００２０】
（項目１０）
項目９に記載のステップをコンピュータに実行させるためのコンピュータプログラム。
【００２１】
このコンピュータプログラムは、適宜な記録媒体（例えばＣＤ−ＲＯＭやＤＶＤディスクのような光学的な記録媒体、ハードディスクやフレキシブルディスクのような磁気的記録媒体、あるいはＭＯディスクのような光磁気記録媒体）に格納することができる。このコンピュータプログラムは、インターネットなどの通信回線を介して伝送されることができる。コンピュータプログラムを用いて制御部の構成を実現することが可能である。
【発明の効果】
【００２２】
本発明によれば、負荷側の位置情報を用いて、動力伝達部で伝達される動力を制御する技術を提供することが可能になる。
【図面の簡単な説明】
【００２３】
【図１】本発明の一実施形態に係る制御装置を概念的に表したブロック図である。
【図２】２慣性系を概念的に表した説明図である。
【図３】２慣性系をモデリングした場合のブロック線図である。
【図４】実施例１のブロック線図である。
【図５】実施例１における軸トルクの目標値応答周波数特性を示すグラフである。
【図６】実施例１において、軸トルクのステップ応答の角速度FF無しとしたときの、軸トルクFF制御の有無での比較を示すグラフである。
【図７】実施例１において、軸トルクのステップ応答の軸トルクFF有りとしたときの、角速度FF制御の有無での比較を示すグラフである。
【図８】実施例１において、軸トルクのステップ応答を示すグラフである。このグラフは、軸トルクFF中の非線形補償の有無による比較を表している。
【図９】実施例１において、軸トルクの正弦波応答を示すグラフである。このグラフは、軸トルクFF中の非線形補償の有無による比較を表している。
【図１０】実施例１におけるバックドライバビリティの実現性を示すためのグラフである。図１０(a)は、負荷側外乱を入れたときにおいて、提案制御の有無による軸トルク応答の比較を示す。図１０(b)は、負荷側外乱を入れたときにおいて、提案制御有無による負荷側角度の比較を示す。
【図１１】実験例における、軸トルクの目標値応答周波数特性を示すグラフである。
【図１２】実験例において、軸トルクのステップ応答についての、軸トルクFF有無での比較を示すグラフである。
【図１３】実験例において、軸トルクのステップ応答についての、角速度FF有無での比較を示すグラフである。
【図１４】実施例２のブロック線図である。
【図１５】実施例３のブロック線図である。
【図１６】実施例１〜３相互の関係を説明するための表である。
【発明を実施するための形態】
【００２４】
（実施形態）
図１を参照して、本発明の一実施形態に係る制御装置を説明する。この制御装置は、駆動部１と、動力伝達部２と、負荷部３と、駆動側位置検出部４と、負荷側位置検出部５と、制御部６とを備えている。
【００２５】
動力伝達部２は、駆動部１から負荷部３に動力を伝達する構成となっている。動力伝達部２としては、例えば、ギア、ベルト、ボールねじなど、各種の機構を用いることができる。また、本実施形態では、柔軟性を持つ動力伝達部２を想定するが、物理的に完全な剛体は想定しがたいので、現実的に存在するすべての動力伝達機構は柔軟性を持つということができる。
【００２６】
駆動側位置検出部４は、駆動部１における駆動側位置情報を取得する構成となっている。駆動側位置検出部４は、例えばエンコーダを用いて実現することができる。
【００２７】
負荷側位置検出部５は、負荷部３における負荷側位置情報を取得する構成となっている。負荷側位置検出部５も、例えばエンコーダを用いて実現することができる。
【００２８】
ここで、この実施形態において位置とは、並進方向における位置や速度に限らず、回転部材における角度又は角速度を含む意味で用いる。
【００２９】
駆動部１は、例えば回転型モータである。この場合、動力伝達部２は、回転軸を用いて動力を伝達する構成とすることができる。また、この場合、動力を、回転軸における軸トルクとして把握することができる。
【００３０】
駆動部１は、例えばリニアモータであってもよい。この場合、動力伝達部２は、駆動部１から負荷側に並進力を伝達する構成とすることができる。また、この場合、動力を、動力伝達部２で伝達される並進力として把握することができる。
【００３１】
また、駆動部１を回転型モータとする一方で、動力伝達部２を、回転軸を用いて並進力を発生させるボールねじ機構を用いて構成することができる。この場合、動力を、回転軸における軸トルク、あるいは、ボールねじ機構で発生する並進力として把握することができる。
【００３２】
（制御部）
制御部６は、負荷側位置情報と、駆動側位置情報とを用いて、駆動部への指令値を算出することによって、動力伝達部で伝達される動力を制御する構成となっている。
【００３３】
制御部６は、非線形要素補償ＦＦ部６１と、駆動側位置ＦＢ部６２と、動力ＦＢ部６３と、駆動側位置ＦＦ部６４とを備えている。
【００３４】
非線形要素補償ＦＦ部６１は、動力伝達部２における非線形要素を陽にフィードフォワード補償する構成となっている。また、本実施形態の非線形要素補償ＦＦ部６１は、非線形要素だけでなく、線形要素についても陽にフィードフォワード補償を行うものとなっている。
【００３５】
駆動側位置ＦＢ部６２は、駆動側位置情報を用いて、駆動側位置をフィードバック制御する構成となっている。
【００３６】
動力ＦＢ部６３は、駆動側位置情報を用いて推定した推定動力を用いて、動力伝達部における動力の推定値をフィードバック制御する構成となっている。
【００３７】
駆動側位置ＦＦ部６４は、駆動部に対する指令値に対して、実際の位置が位置指令値に一致するようにフィードフォワード補償を行うことによって、駆動側位置における制御帯域を上昇させる構成となっている。
【００３８】
制御部６の具体的な構成と動作については、後述の実施例においてより詳しく説明する。
【００３９】
（実施例１）
以下、前記した実施形態の制御装置を具体化した実施例を説明する。以下の実施例の説明においては、前記した実施形態で用いた符号と基本的に共通する要素には、同じ符号を用いる。
【００４０】
（２慣性系について）
まず、本実施例が適用されるプラントは、いわゆる２慣性系である。ここで、２慣性系の概念を、図２を参照しながら説明する。このプラントでは、駆動部１として回転型モータが用いられている。駆動部１は、剛性（回転軸の場合は軸ねじれ剛性）Ｋを有する動力伝達部２を介して負荷部３を回転駆動するようになっている。
【００４１】
図２のプラントをモデリングして得たブロック線図を図３に示す。この図３では、駆動モータ（駆動部）の慣性モーメント、粘性摩擦係数、入力トルクをJM,DM,TMとし、負荷側（負荷部）の慣性モーメント、粘性摩擦係数、外乱をJL,DL,dL、回転軸の軸ねじれ剛性をK、動力伝達部２での減速比をRとした。また、軸トルクはTs、駆動側角度はθM、駆動側角速度はωM、負荷側角度はθL、負荷側角速度はωL、軸ねじれ角度はΔθである。
【００４２】
プラントは各種非線形要素を含む(11)。図３においては、プラントの動力伝達部２に含まれる非線形要素の内の1つであるバックラッシを不感帯（Dead zone）としてモデリングし表現している。
【００４３】
なお、図３のプラントがとりうるパラメータ値の一例を下記表１に示す。
【００４４】
【表１】
0006502634.tif 000002
【００４５】
（軸トルク制御の先行研究）
軸トルク制御は、トルクセンサを用いる制御法(12)(13)と、トルクセンサを用いずに反力オブザーバを用いる制御法(14)(15)という２種類に大別できる。トルクセンサを用いる手法の場合、コストが高い、剛性の低下を招きプラント特性に影響を与えてしまう、センサの帯域が低い、センサノイズの影響を受ける等の問題点がある(16)。
【００４６】
一方、反力オブザーバを用いる場合、機構の剛性を低下させる等のトルクセンサのようなデメリットを回避できる。
【００４７】
しかし、摩擦やモデル化誤差が含まれた力が推定されてしまうため、これらの成分を上手く取り除いて軸トルクを推定することが重要であり、多くの研究が行なわれている(17)。本実施例は、駆動側エンコーダ（駆動側位置検出部）で検出された駆動側位置情報を用いた反力オブザーバ（制御部の一例）により軸トルクを推定する。さらに本実施例では、負荷側エンコーダ情報（負荷側位置情報）を組み合わせることにより、より精密な軸トルク制御を可能にしようとするものである。
【００４８】
（本実施例での制御法）
近年様々な分野で最終位置決め精度への要求の向上から負荷側でのエンコーダの利用が広がっていることを踏まえ、本実施例の制御法は、負荷側エンコーダ（負荷側位置検出部）を利用した２慣性系における軸トルク制御法となっている。駆動側と負荷側双方の情報が得られることから、この制御法においては、動力伝達部２における回転軸の軸ねじれ角速度を制御することができ、精密に軸トルクを制御することができる。また、軸ねじれ角速度を制御することができるため、バックラッシ等の軸ねじれ部の非線形要素補償を陽に考慮したFF（フィードフォワード）の制御器設計が可能となっている。
【００４９】
本制御法で用いる制御部６を含むブロック線図を図４に示す。なお、この図において、CPは駆動側角速度のP制御器、CPIは軸トルクFB（フィードバック）制御のPI制御器、QFFは角速度制御のインナーループにおけるFF制御を実現するための１次のローパスフィルタ、Tsハットは外乱オブザーバで推定した軸トルク、Qは外乱オブザーバのローパスフィルタ、τpは擬似微分の時定数を示す。また、添え字のnはノミナル値、*は制御部への指令値を示す。図４のブロック線図の一部には、図３に示すプラント（制御対象）と実質的に等価なブロック線図が含まれている。
【００５０】
図４に示す制御系は、大きく３つの部分に分けて考えることができる。１つ目は、軸ねじれ角速度を制御するための駆動側角速度制御部、２つ目は軸トルク指令値から軸ねじれ角速度指令値を生成する軸トルクFF制御部、３つ目は駆動側反力オブザーバで推定した軸トルクのFB制御部である。
【００５１】
（駆動側角速度制御）
まず、１つ目の駆動側角速度制御部（駆動側位置FB部及び駆動側位置FF部に対応）に関して述べる。本制御法は、軸ねじれ角速度を制御することにより、軸トルク制御をする。コロケート系である駆動側角速度を制御したうえで，負荷側角速度を組み合わせて，軸ねじれ角速度を制御する。駆動側角速度制御は，2自由度制御となっており，フィードフォワード制御とフィードバック制御をする。ここで、図４より、軸ねじれ角速度Δωは式（１）により与えられる。
【００５２】
0006502634.tif 000003
【００５３】
したがって、駆動側角速度の指令値は軸ねじれ角速度指令値と負荷側角速度を用いて式（２）のように生成できる。
【００５４】
0006502634.tif 000004
【００５５】
駆動側角速度は、逆モデルとして(JMns+DMn)を用いた外乱オブザーバを用いることと、Cpを用いたP制御をすることとにより制御される（駆動側位置ＦＢ部）。つまり、本例の駆動側位置ＦＢ部６２は、駆動側位置情報ωMを用いて、駆動側位置をフィードバック制御する構成となっている。また、駆動側角速度制御は２自由度制御になっており、制御帯域の向上を図っている。インナーループの帯域の向上により、アウターループである軸トルク制御（後述）の応答性の向上が期待できる。インナーループにおける角速度制御のFF制御（駆動側位置ＦＦ部）は、(JMns+DMn)にプロパーにするための1次のローパスフィルタQFFを用いて実現される。つまり、本例の駆動側位置ＦＦ部６４は、駆動部１に対する指令値に対して、駆動部１の実際の位置（例えば実際の駆動側角速度又は角度）が位置指令値ω*Mに一致するようにフィードフォワード補償を行うことによって、駆動側位置における制御帯域を上昇させる構成となっている。
【００５６】
（軸トルクFF制御）
次に、２つ目の軸トルクFF制御部（非線形要素補償FF部に相当）では、軸トルク指令値T*sを用いて、前記の式（２）で表わされる駆動側角速度指令値ω*Mを作る。図４に示されるΔωからTsへの逆モデルを考え、軸ねじれ剛性（線形要素）の逆数、非線形要素の逆関数、微分を用いてω*Mを生成する。微分は時定数τpの擬似微分により実現する。本実施例では、非線形要素としてバックラッシを想定し不感帯でモデリングするため、非線形要素補償として、不感帯の逆関数モデルを用いる。つまり、本例における非線形要素補償ＦＦ部６１は、前記要素を用いてΔω*を生成し、これを用いて、動力伝達部２における非線形要素及び線形要素を陽にフィードフォワード補償する構成となっている。
【００５７】
（軸トルクFB制御）
３つ目の軸トルクFB制御部（動力ＦＢ部）では、駆動側反力オブザーバで推定した軸トルクTsハットをFBし、PI制御により、Ts=(k/s)Δωに対して極配置をする。PI制御をすることで、定常偏差なく指令値に追従させることができる。駆動側反力オブザーバのローパスフィルタQの遅れを考慮し、軸トルク指令値T*sにもQを入れる。つまり、本例の動力ＦＢ部６３は、駆動側位置情報ωMを用いて推定した推定動力Tsハットを用いて、動力伝達部２における動力の推定値をフィードバック制御する構成となっている。
【００５８】
以上の構成を備えることにより、本例の制御部６は、全体として、負荷側位置情報ωLと、駆動側位置情報ωMとを用いて、駆動部１への入力値TM（つまり制御部からプラントへの指令値）を算出することによって、動力伝達部２で伝達される動力Ts（軸トルク）を制御する構成となっている。
【００５９】
（シミュレーション）
前記で提案した制御系の有効性をシミュレーションによって検証をした。シミュレーションに使ったモデルは、図３に示す２慣性系モデルであり、パラメータは前記した表１の値を用いる。また簡単のため、特に明記の無い場合にはシミュレーションには非線形要素及び非線形補償要素は含まれていない。
【００６０】
（1）駆動側角速度制御器の設計
まず、駆動側角速度についてのP制御器は、帯域が180Hzとなるように設計をした。外乱オブザーバのカットオフ周波数は30Hzとした。外乱オブザーバにより軸トルクの反力の影響は非干渉化されていると考え、FF制御器のモデルとして(JMns+DMn)を用い、実現のために1次のカットオフ周波数1kHzのローパスフィルタを用いる。
【００６１】
（2）軸トルク制御器の設計と応答
軸トルクについてのPI制御器は、Ts=(k/s)Δωに対して極配置を行い、30Hzに重根極配置を行った。擬似微分のカットオフ周波数は1kHz、駆動側反力オブザーバのカットオフ周波数は30Hzとしている。トルクセンサを使うことを想定していないので、フィードバックされる軸トルク及び実験で検出できる軸トルクは駆動側反力オブザーバで推定した軸トルクとなるため、推定した軸トルクの特性をみる。なお、オブザーバやFF制御におけるモデル化誤差は与えていない。
【００６２】
このとき、軸トルクの目標値応答の周波数特性は図５のようになった。実線が角速度のFF制御ありの場合で、一点鎖線が角速度のFF制御なしの場合である。インナーループにFF制御を入れることで、アウターループである軸トルクの目標値応答の帯域が20Hzから27Hzに向上している。
【００６３】
また、図６は、軸トルクのステップ応答を表しており、角速度FF制御無しとした状態での、軸トルクFF制御が有る場合と無い場合の比較が示されている。0.050sの時に8.0Nmのステップ指令にカットオフ周波数30Hzの1次のローパスフィルタを通した指令値を入力し、その後、0.15sに負荷側の外乱dLとして-10Nmのステップ外乱を入力している。点線がローパスフィルタを通したステップ指令値、実線が軸トルクFF制御を入れた場合、一点鎖線が軸トルクFF制御を入れない場合を示している。FF制御を入れることで、応答の立ち上がりが早くなっていることが分かる。軸トルクのFF制御無しの場合は、指令値に到達するのは早いが若干振動的な応答になっている。外乱応答に関しては、軸トルクFF制御の有無で差は出ない。
【００６４】
次に、図７は、軸トルクのステップ応答を表しており、軸トルクFF制御有りとした状態での、角速度FF制御の有り無しでの比較が示されている。点線がローパスフィルタを通したステップ指令値、実線が角速度FF制御を入れた場合、一点鎖線が角速度FF制御を入れない場合をそれぞれ示す。FF制御を入れることでオーバーシュート気味になっているものの、速応性及び負荷側外乱抑圧特性が向上している。
【００６５】
また、図８は軸トルクのステップ応答を示しており、実線がFF制御の部分で非線形要素を補償した場合、一点鎖線が非線形要素を補償しなかった場合である。本シミュレーションでは、非線形要素は不感帯としてモデリングし、不感帯の幅は想定した非線形要素であるハーモニックギヤのカタログ値の最大バックラッシ幅（5.3e-5rad）を用いた。初期位置はバックラッシの中央である。非線形要素をFF補償することによって、応答の振動を抑えられ制御性能を改善できることが分かる。
【００６６】
また、速度反転時の検討のため、図９に正弦波応答を示す。指令値は、振幅1Nmでかつ角周波数50rad/sとした。点線が正弦波指令値、実線が非線形要素補償を入れた場合、一点鎖線が非線形要素補償を入れない場合をそれぞれ示す。非線形要素補償を入れない場合において、初めの応答のみ振幅が小さくなっているのは、初めの応答は初期位置がバックラッシの中央に位置し、非線形部の影響が速度反転時の半分になっているからである。非線形要素補償をすることにより、制御性能が向上していることがわかる。
【００６７】
なお、今回のシミュレーションでは、非線形要素補償のモデルはプラントに含まれる非線形要素の完全な逆モデルであったが、実際にはプラントの非線形要素を正確にモデリングするのは困難である。したがって、提案法の制御性能の向上のためには、非線形要素をよく再現し、逆関数を持つようなモデリングが重要である。
【００６８】
最後に、バックドライバビリティの実現に関するシミュレーションを図１０(a),１０(b)に示す。T*sを0Nmにした状態で、負荷側外乱dLとして0.050sのときに-10Nmのステップ外乱を入力する。実線が本例の制御の場合、一点鎖線が何も制御をしない場合である。本例の制御では約0.050sで軸トルクを0Nmにすることができる。また、図１０(b)より、何も制御しない場合と比べて、同じ負荷側外乱に対して、負荷側角度の移動量が大きいことから、駆動側の摩擦の影響を受けないバックドライバビリティの実現ができていると考えられる。
【００６９】
（実験例）
以下、実験例を示す。実験例における制御器のパラメータは、シミュレーションと同じである。この実験例では、制御器を、Tustin変換により5kHzで離散化することで実装した。また、角速度は、各エンコーダより得られる角度情報についての5kHz後進差分に、1kHzの１次のローパスフィルタを適用することにより得た。さらに、軸トルクは、カットオフ周波数30Hzの駆動側反力オブザーバで推定した。
【００７０】
実験による軸トルクの目標値応答の周波数特性は図１１のようになった。角速度FF制御なしの場合であるため、シミュレーションでは図５の一点鎖線に対応する。図１１に示されるように、帯域は18Hzとなった。
【００７１】
本制御法の軸トルクステップ応答の実験結果を図１２及び図１３に示す。図１２では、点線がステップ指令を示し、実線が軸トルクFF制御ありの場合、一点鎖線が軸トルクFF制御なしの場合を比較して示している。これは、シミュレーションでは図６に対応する。図１２より、軸トルクFF制御ありの場合の方が、シミュレーションと同様に応答の立ち上がりが良くなっていることが分かる。
【００７２】
図１３では、点線がステップ指令を示し、実線が角速度FF制御ありの場合、一点鎖線が角速度FF制御なしの場合の比較を示す。これは、シミュレーションでは図７に対応する。実験では、軸トルクの応答性の改善があまり見られなかったが、モデル化誤差やセンサノイズ等が理由として考えられる。また、角速度FF制御を入れた場合には、推定軸トルクに、より多くのノイズが乗ってしまっている。これは、インナーループにおいてFF制御を追加することで、モデル化誤差やセンサノイズに起因する軸トルク推定値のノイズの影響が大きくなってしまっているためと考えられる。
【００７３】
（本実施例の利点）
本実施例では、負荷側にエンコーダを用いることが近年増えてきている現状を考慮し、負荷側エンコーダを用いた軸トルク制御法を提案した。２慣性系における軸トルクを制御し、負荷へ伝わるトルクを制御することでロボットや工作機械においてより高度な作業が可能になる。そして提案法の制御器を設計し、シミュレーション及び実験にて制御性能を評価し、またバックドライバビリティが実現できることを示した。
【００７４】
本実施例は、非線形要素補償としてバックラッシの逆モデルを用いるという単純なものであったが、駆動側と負荷側双方のエンコーダにより得られる初期位置を考慮した非線形要素補償も可能であると考えられる。また、本実施例では、軸トルク制御の指令値としてステップ指令や0を与えたが、インピーダンス制御や衝突検知等という、より高度な制御も考えられる(14)〜(16)。本実施例では、提案制御法をロボットへ適用したが、２慣性系にモデリングできる工作機械等の他分野への応用も可能である。
【００７５】
（実施例２）
次に、前記した実施例１を変形した例を実施例２として図１４に示す。なお、実施例１と同様又は実質的に等価な部分については、詳しい説明を省略する。また、実施例１と実施例２とでは、プラントのブロック線図の表記（積分器の位置等）が若干異なるが、理論的には等価である。それぞれの制御法において見やすいように適宜修正して記載している。
【００７６】
実施例２では、インナーループを角度制御としている。この場合には、インナーループが角速度制御の場合と異なり、軸ねじれ角速度ではなく軸ねじれ角度を制御する。そのため、FF制御の部分に擬似微分が不要である。また、Ts =(k/s)Δωに代えて、Ts =kΔθに対して極配置をすることとしたので、Ｉ制御を用いている。しかし、インナーループを角速度制御とした実施例１と比べて、インナーループの帯域を高くできないことが実施例２の欠点である。駆動側角度制御は、PD制御である。また、実施例２のインナーループにおけるFFは、実施例１のような駆動側角速度ではなく、駆動側角度制御のFF制御になっている。
【００７７】
本例の駆動側位置ＦＢ部６２は、駆動側位置情報θMを用いて、駆動側位置をフィードバック制御する構成となっている。
【００７８】
また、本例の駆動側位置ＦＦ部６４は、駆動部１に対する指令値に対して、駆動部１の実際の位置（例えば実際の駆動側角速度又は角度）が位置指令値θ*Mに一致するようにフィードフォワード補償を行うことによって、駆動側位置における制御帯域を上昇させる構成となっている。
【００７９】
また、本例における非線形要素補償ＦＦ部６１は、Δθ*を生成し、これを用いて、動力伝達部２における非線形要素及び線形要素を陽にフィードフォワード補償する構成となっている。
【００８０】
さらに、本例の動力ＦＢ部６は、駆動側位置情報θMを用いて推定した推定動力Tsハットを用いて、動力伝達部２における動力の推定値をフィードバック制御する構成となっている。
【００８１】
さらに、本例の制御部６は、全体として、負荷側位置情報θLと、駆動側位置情報θMとを用いて、駆動部１への入力値TM（つまり制御部からプラントへの指令値）を算出することによって、動力伝達部２で伝達される動力Ts（軸トルク）を制御する構成となっている。
【００８２】
（実施例３）
次に、前記した実施例１を変形した別の例を実施例３として図１５に示す。図１５においても、プラントは実施例１と等価である。
【００８３】
実施例３では、軸トルクFF制御をΔθに対して行うことで、実施例１の欠点であった軸トルクFF制御部の擬似微分が不要になる。また、インナーループを角速度制御とすることで、実施例２のように角度制御とした場合に比べ、帯域を高くすることができると考えられる。
【００８４】
これは、駆動側角速度制御のインナーループに、駆動側角度制御及び軸トルク制御という２つのアウターループを並列に組むことで可能になっている。しかし、実施例３では、軸トルクFB制御と駆動側角度制御のFBとが干渉する可能性があるので、これを避けるような設計が望ましい。
【００８５】
実施例３での駆動側角度制御はP制御である。
【００８６】
本例の駆動側位置ＦＢ部６２は、駆動側位置情報θMを用いて、駆動側位置をフィードバック制御する構成となっている。
【００８７】
また、本例の駆動側位置ＦＦ部６４は、駆動部１に対する指令値に対して、駆動部１の実際の位置（例えば実際の駆動側角速度又は角度）が位置指令値ω*Mに一致するようにフィードフォワード補償を行うことによって、駆動側位置における制御帯域を上昇させる構成となっている。
【００８８】
また、本例における非線形要素補償ＦＦ部６１は、Δθ*を生成し、これを用いて、動力伝達部２における非線形要素及び線形要素を陽にフィードフォワード補償する構成となっている。
【００８９】
さらに、本例の動力ＦＢ部６３は、駆動側位置情報ωMを用いて推定した推定動力Tsハットを用いて、動力伝達部２における動力の推定値をフィードバック制御する構成となっている。
【００９０】
さらに、本例の制御部６は、全体として、負荷側位置情報θLと、駆動側位置情報θMとを用いて、駆動部１への入力値TM（つまり制御部からプラントへの指令値）を算出することによって、動力伝達部２で伝達される動力Ts（軸トルク）を制御する構成となっている。
【００９１】
実施例２及び３における前記以外の構成及び動作は実施例１と同様なので、これ以上詳しい説明は省略する。
【００９２】
実施例１，２，３の比較を、図１６に示す。分類としては、軸ねじれ角度を制御するのか、軸ねじれ角速度を制御するのかで分類できる。さらに、インナーループの制御を駆動側角度制御にするのか駆動側角度制御にするのかでも分類できる。
【００９３】
なお、本発明の内容は、前記各実施形態に限定されるものではない。本発明は、特許請求の範囲に記載された範囲内において、具体的な構成に対して種々の変更を加えうるものである。
【００９４】
（本明細書中で引用した参照文献一覧）
（1） J. Hu and F. Hu: "Analysis and Design to the Asymptotic Behavior of the Semi/Full/Dual-closed Loop Feedback Position Control System", Proc. of the SICE Annual Conference 2012, pp. 1096-1101, (2012).
（2） F. Sakai, M. Hikizu, Y. Kamiya and H. Seki: "Precision Positioning Which Contains Flexible Mechanical System: Fusion of Semi-closed and Full-closed Loop Control", J. JSPE, vol. 66, no. 9, (2000) (in Japanese).
（3） M. Aoki, H. Fujimoto, Y. Hori and T. Takahashi: "Robust Resonance Suppression Control  based on Self Resonance Cancellation Control and Self Resonance Cancellation Disturbance Observer for Application to Humanoid Robot", IEEJ Trans. on Industry Applications, vol. 134, no. 4, pp. 376-383, (2014).
（4） K. Sakata, H. Asaumi, K. Hirachi, K. Saiki and H. Fujimoto: "Frequency Separation Self Resonance Cancellation for Vibration Suppression Control of a Large-Scale Stage Using Multiple Position Sensors", Proc. of the 39th Annual Conference of IEEE Ind. Electron Society (IECON-2013)pp. 6478-6483, (2013).
（5） S. Yamada, H. Fujimoto and Y. Hori: "Vibration Suppression Control of Two-Inertia System using Load-Side State Variables with High-Resolution Encoder", Proc. of IEE of Japan Technical Meeting Record, IIC- 14-140, MEC-14-128, pp. 107-112, (2014).
（6） B. Na, J. Bae and K. Kong: "Back-drivability recovery of a full lower extremity assistive robot", Proc. of the 12th International Conference of IEEE on Control, Automation and Systems (ICCAS), pp. 1030- 1034,   (2012).
（7） P. Weiss, P. Zenker and E. Maehle: "Feed-forward Friction and Inertia Compensation for Improving Back-drivability of Motors", Proc. of the 12th International Conference of IEEE on Control, Automation, Robotics and Vision (ICARCV-2012), pp. 288-293, (2013).
（8） N. Paine, S. Oh and L. Sentis: "Design and Control Considerations for High-Performance Series Elastic Actuator", IEEE Trans. on Mechatronics, vol. 19, no. 3, (2014).
（9） K. Kong, J. Bae and M. Tomizuka: "A Compact Rotary Series Elastic Actator for Human Assistive Systems", IEEE Trans. on Mechatronics, vol. 17, no. 2, (2012).
（10） Y. Kuroki, Y. Kosaka, T. Takahashi, E. Niwa, H. Kaminaga and Y. Nakamura: "Cr-N Alloy Thin-film Based Torque Sensors and Joint Torque Servo Systems for Compliant Robot Control", IEEE International Conference on Robotics and Automation (ICRA-2013), pp. 4954-4959, (2013).
（11） M. Iwasaki, M. Kainuma, M. Yamamoto and Y. Okitsu: "Compensation by Exact Linearization Method for Nonlinear Components in Positioning Device with Harmonic Drive Gearings", Journal of JSPE, vol. 78, no. 7,  pp.  624-630,  (2012).
（12） T. Kawakami, K. Ayusawa, H. Kaminaga and Y. Nakamura: "High-Fidelity Joint Drive System by Torque Feedback Control Using High Precision Linear Encoder", IEEE International Conference on Robotics and Automation (ICRA-2010), pp. 3904-3909, (2010).
（13） T. Kawakami, K. Ayusawa, H. Kaminaga, C. Ott and Y. Nakamura: "Development of Joint Driving Mechanism with Rigid Torque Sensor and its Torque Control Systems", Proc. of the JSME Conference on Robotics and Mechatronics, no. 08-4, pp. 1-4, 2008, (in Japanese).
（14） S. Oh and Y. Hori: "Experimental Study on Impedance and Impact Force Control by Position Feedback Control", Proc. of IEE of Japan Technical Meeting Record, IIC-09-117, pp. 31-36, (2009), (in Japanese).
（15） N. Shibata, T. Yoshioka, K. Ohishi and T. Miyazaki: "High Reliable Force Sensorless Contact Detection of Contact Motion", Proc. of IEE of Japan Technical Meeting Record, IIC-12-151, pp. 107-112, (2012), (in Japanese).
（16） S. Katsura, Y. Matsumoto and K. Ohnishi: "Modeling of Force Sensing and Validation of Disturbance Observer for Force Control", IEEE Trans. on Industrial Electronics, vol. 54, no. 1, (2007).
（17） C. Mitsantisuk, M. Nandapaya, K. Ohishi and S. Katsura: "Design for Sensorless Force Control of Flexible Robot by Using Resonance Ration Control Based on Coefficient Diagram Method", Automatika, vol. 54, no. 1, pp. 62-73, (2013).
【符号の説明】
【００９５】
１  駆動部
２  動力伝達部
３  負荷部
４  駆動側位置検出部
５  負荷側位置検出部
６  制御部
６１  非線形要素補償ＦＦ部
６２  駆動側位置ＦＢ部
６３  動力ＦＢ部
６４  駆動側位置ＦＦ部
【図１】
0006502634.tif 000005
【図２】
0006502634.tif 000006
【図３】
0006502634.tif 000007
【図４】
0006502634.tif 000008
【図５】
0006502634.tif 000009
【図６】
0006502634.tif 000010
【図７】
0006502634.tif 000011
【図８】
0006502634.tif 000012
【図９】
0006502634.tif 000013
【図１０】
0006502634.tif 000014
【図１１】
0006502634.tif 000015
【図１２】
0006502634.tif 000016
【図１３】
0006502634.tif 000017
【図１４】
0006502634.tif 000018
【図１５】
0006502634.tif 000019
【図１６】
0006502634.tif 000020