JP2023055151A - 制御装置、ロボットシステム、ロボット制御方法及びロボット制御プログラム - Google Patents

Abstract

【課題】操作者等の変化があっても力制御の挙動を安定させる。【解決手段】制御装置３は、操作者Ｕから操作装置２へ加えられる操作力に応じてロボット１を動作させて対象物Ｗに作用を加えると共に、ロボット１が対象物Ｗから受ける反力に応じて操作装置２を動作させる力制御を実行する力制御部３６と、操作者Ｕの動力学特性を推定する推定部３７と、力制御の周波数特性が目標周波数特性となるように、推定された操作者の動力学特性に応じて力制御の制御パラメータを調整する調整部３１０とを備える。【選択図】図７

Description

ここに開示された技術は、制御装置、ロボットシステム、ロボット制御方法及びロボット制御プログラムに関する。

従来より、マスタ装置とスレーブ装置とを備えたロボットシステムを制御する制御装置が知られている。例えば、特許文献１に開示された制御装置は、マスタ装置を介して操作に応じて、スレーブ装置としてのマニピュレータを動作させる。マニピュレータは、対象物に接触した状態で作用を加える。操作者によるマスタ装置の操作に応じて、マニピュレータから対象物へ加える力が制御される。

特開平０３－１６２２８９号公報

ところで、前述のような力制御における安定性及び応答性等の周波数特性は、様々な要因により変化し得る。例えば、操作者が変わると、力制御の安定性等が変化し得る。操作者が同じであっても、力制御の安定性等が変化する場合もある。そのため、操作者等の変化に伴って、力制御の挙動が変わり得る。

ここに開示された技術は、かかる点に鑑みてなされたものであり、その目的とするところは、操作者等の変化があっても力制御の挙動を安定させることにある。

ここに開示された制御装置は、操作者からマスタ装置へ加えられる操作力に応じてスレーブ装置を動作させて対象物に作用を加えると共に、前記スレーブ装置が前記対象物から受ける反力に応じて前記マスタ装置を動作させる力制御を実行する力制御部と、前記操作者の動力学特性を推定する推定部と、前記力制御の周波数特性が目標周波数特性となるように、推定された前記操作者の動力学特性に応じて前記力制御の制御パラメータを調整する調整部とを備える。

ここに開示されたロボットシステムは、前記操作者に操作される前記マスタ装置と、前記対象物に作用を加える前記スレーブ装置と、前記制御装置とを備える。

ここに開示されたロボット制御方法は、操作者からマスタ装置へ加えられる操作力に応じてスレーブ装置を動作させて対象物に作用を加えると共に、前記スレーブ装置が前記対象物から受ける反力に応じて前記マスタ装置を動作させる力制御を実行することと、前記操作者の動力学特性を推定することと、前記力制御の周波数特性が目標周波数特性となるように、推定された前記操作者の動力学特性に応じて前記力制御の制御パラメータを調整することとを含む。

ここに開示されたロボット制御プログラムは、操作者から操作力が加えられるマスタ装置と対象物に作用を加えるスレーブ装置とを備えるロボットシステムを制御する機能をコンピュータに実現させるためのロボット制御プログラムであって、前記マスタ装置へ加えられる操作力に応じて前記スレーブ装置を動作させて対象物に作用を加えると共に、前記スレーブ装置が前記対象物から受ける反力に応じて前記マスタ装置を動作させる力制御を実行する機能と、前記操作者の動力学特性を推定する機能と、前記力制御の周波数特性が目標周波数特性となるように、推定された前記操作者の動力学特性に応じて前記力制御の制御パラメータを調整する機能とをコンピュータに実現させる。

前記制御装置は、操作者等の変化があっても力制御の挙動を安定させることができる。

前記ロボットシステムは、操作者等の変化があっても力制御の挙動を安定させることができる。

前記ロボット制御方法は、操作者等の変化があっても力制御の挙動を安定させることができる。

前記ロボット制御プログラムは、操作者等の変化があっても力制御の挙動を安定させることができる。

図１は、ロボットシステムの構成を示す模式図である。 図２は、エンドエフェクタの拡大図である。 図３は、ロボット制御装置の概略的なハードウェア構成を示す図である。 図４は、操作装置の斜視図である。 図５は、操作制御装置の概略的なハードウェア構成を示す図である。 図６は、制御装置の概略的なハードウェア構成を示す図である。 図７は、制御部の機能ブロック図である。 図８は、力制御部を詳細な機能に分解したブロック図である。 図９は、ロボットシステムの全体のブロック線図である。 図１０は、操作装置を含むマスタ制御系のブロック線図である。 図１１は、ロボットを含むスレーブ制御系のブロック線図である。 図１２は、推定部による推定処理のフローチャートである。 図１３は、制御パラメータ調整のフローチャートである。

以下、例示的な実施形態を図面に基づいて詳細に説明する。

本開示において、ロボットが行う作業は、ティーチング作業並びに教示の確認及び修正作業を含まない。したがって、以下の説明における操作装置２は、ティーチペンダントを含まない。

図１は、ロボットシステム１００の構成を示す模式図である。

ロボットシステム１００は、ロボット１と、操作者Ｕに操作される操作装置２と、ロボット１を制御する制御装置３とを備える。ロボットシステム１００は、マスタスレーブシステムを構成する。操作装置２は、マスタ装置として機能し、ロボット１は、スレーブ装置として機能する。制御装置３は、ロボットシステム１００の全体を制御し、ロボット１と操作装置２との間でバイラテラル制御を行う。

〈ロボット１の構成〉
ロボット１は、例えば、産業用ロボットである。ロボット１は、対象物Ｗに作用を加えるエンドエフェクタ１１と、エンドエフェクタ１１を動作させるロボットアーム１２とを有している。エンドエフェクタ１１は、ロボットアーム１２の先端に連結されている。ロボット１は、ロボットアーム１２によってエンドエフェクタ１１を動作、即ち、移動させて、エンドエフェクタ１１によって対象物Ｗに作用を加える。エンドエフェクタ１１は、対象物Ｗに接触した状態で対象物Ｗに作用を加え得る。例えば、作用は、加工である。例えば、対象物Ｗは、大型タンクの湾曲した壁等である。

ロボット１は、ロボットアーム１２を支持するベース１０と、ロボット１の全体を制御するロボット制御装置１４とをさらに有していてもよい。

ロボットアーム１２は、エンドエフェクタ１１の位置及び姿勢を変更する。ロボットアーム１２は、垂直多関節型のロボットアームである。ロボットアーム１２は、複数のリンク１２ａと、複数のリンク１２ａを接続する関節１２ｂと、複数の関節１２ｂを回転駆動するサーボモータ１５（図３参照）とを有している。例えば、ロボットアーム１２の一端部（エンドエフェクタ１１とは反対側の端部）に位置するリンク１２ａは、関節１２ｂを介して、鉛直方向に延びる回転軸Ｒ１回りに回転可能にベース１０に連結されている。

尚、ロボットアーム１２は、水平多関節型、パラレルリンク型、直角座標型、又は極座標型のロボットアーム等であってもよい。

図２は、エンドエフェクタ１１の拡大図である。エンドエフェクタ１１は、研削装置１１ａを有し、対象物Ｗに作用としての研削を加える。尚、エンドエフェクタ１１が対象物Ｗに加える作用は、研削ではなく、切削又は研磨等であってもよい。

例えば、研削装置１１ａは、グラインダ、オービタルサンダ、ランダムオービットサンダ、デルタサンダ又はベルトサンダ等であってもよい。グラインダは、円盤状の研削砥石を回転させるタイプ、円錐状又は円柱状の研削砥石を回転させるタイプ等であってもよい。ここでは、研削装置１１ａは、円盤状の研削砥石を回転させるタイプのグラインダである。

ロボット１には、直交３軸のスレーブ座標系が規定されている。スレーブ座標系は、ロボット１を基準に設定されている。スレーブ座標系は、互いに直交するＸｒ軸、Ｙｒ軸、及びＺｒ軸を有している。Ｘｒ軸、Ｙｒ軸、及びＺｒ軸は、原点Ｏｒにおいて互いに交差している。原点Ｏｒは、図１に示すように、ベース１０の上面に位置している。Ｘｒ軸及びＹｒ軸は、水平方向に、即ち、ベース１０の上面と平行に延びている。Ｚｒ軸は、鉛直方向に延びている。Ｚｒ軸は、ロボットアーム１２とベース１０とを連結する関節１２ｂの回転軸Ｒ１と一致している。Ｙｒ軸は、図１における紙面と垂直に延びている。

また、エンドエフェクタ１１には、直交３軸のツール座標系が規定されている。ツール座標系は、エンドエフェクタ１１に固定された座標系である。ツール座標系は、図２に示すように、互いに直交するＸｔ軸、Ｙｔ軸、及びＺｔ軸を有している。Ｘｔ軸、Ｙｔ軸、及びＺｔ軸は、原点Ｏｔにおいて互いに交差している。例えば、原点Ｏｔは、研削装置１１ａのうち対象物Ｗとの接点に位置している。詳しくは、研削装置１１ａの研削砥石の回転軸Ｂは、エンドエフェクタ１１が取り付けられたリンク１２ａの回転軸Ｒ２に対して傾斜している。研削砥石の外周縁のうち回転軸Ｒ２の方向においてリンク１２ａから最も離れた部分が、対象物Ｗとの接点として想定されている。Ｚｔ軸は、回転軸Ｒ２と平行に延びている。Ｘｔ軸は、研削砥石の回転軸ＢがＸｔ－Ｚｔ平面内で延びるように設定されている。Ｙｔ軸は、図２では紙面と垂直に延びている。ツール座標系は、スレーブ座標系から見ると、エンドエフェクタ１１の位置及び姿勢に応じて変位する。つまり、ツール座標系は、ロボットアーム１２の移動に応じて、エンドエフェクタ１１と共に移動する。

ロボット１は、エンドエフェクタ１１が対象物から受ける反力（以下、「接触力」という）を検出する接触力センサ１３をさらに有していてもよい。

接触力センサ１３は、この例では、ロボットアーム１２とエンドエフェクタ１１との間（具体的には、ロボットアーム１２とエンドエフェクタ１１との連結部）に設けられている。接触力センサ１３は、直交する３軸方向の力と該３軸回りのモーメントを検出する。接触力センサ１３は、接触力検出部の一例である。

尚、接触力検出部は、接触力センサ１３に限定されない。例えば、接触力センサ１３は、１軸、２軸又は３軸方向の力のみを検出してもよい。あるいは、接触力検出部は、ロボットアーム１２のサーボモータ１５の電流を検出する電流センサ又はサーボモータ１５のトルクを検出するトルクセンサ等であってもよい。

図３は、ロボット制御装置１４の概略的なハードウェア構成を示す図である。ロボット制御装置１４は、ロボットアーム１２のサーボモータ１５及び研削装置１１ａを制御する。ロボット制御装置１４は、接触力センサ１３の検出信号を受け付ける。ロボット制御装置１４は、制御装置３と情報、指令及びデータ等の送受信を行う。ロボット制御装置１４は、制御部１６と、記憶部１７と、メモリ１８とを有している。

制御部１６は、ロボット制御装置１４の全体を制御する。制御部１６は、各種の演算処理を行う。例えば、制御部１６は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御部１６は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）等で形成されていてもよい。

記憶部１７は、制御部１６で実行されるプログラム及び各種データを格納している。記憶部１７は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。

メモリ１８は、データ等を一時的に格納する。例えば、メモリ１８は、揮発性メモリで形成される。

〈操作装置２の構成〉
操作装置２は、図１に示すように、操作者Ｕが操作する操作部２１と、操作部２１に操作者Ｕから加えられる操作力を検出する操作力センサ２３とを有している。操作装置２は、ロボット１を手動運転で操作するための入力を受け付け、入力された情報である操作情報を制御装置３へ出力する。具体的には、操作者Ｕは、操作部２１を把持して操作装置２を操作する。その際に操作部２１に加えられる力を操作力センサ２３が検出する。操作力センサ２３によって検出される操作力は、操作情報として制御装置３へ出力される。

操作装置２は、ベース２０と、ベース２０に設けられ、操作部２１を支持する支持機構２２と、操作装置２の全体を制御する操作制御装置２４とをさらに有していてもよい。操作装置２は、制御装置３からの制御によって、操作力に対する反力を操作者Ｕに与える。具体的には、操作制御装置２４は、制御装置３からの指令を受けて、支持機構２２を制御することによって、反力を操作者Ｕに感知させる。

図４は、操作装置２の斜視図である。支持機構２２は、６つのアーム２２ａを有している。２つのアーム２２ａが１つの組を形成している。つまり、支持機構２２は、３組のアーム２２ａを有している。３組のアーム２２ａは、操作部２１から放射状に延びている。各アーム２２ａは、関節２２ｂを有している。各関節２２ｂは、アーム２２ａを形成する２つのリンクを直交する３軸回りに回転可能にボールジョイント等の自在継手を介して連結している。各アーム２２ａは、関節２２ｂで屈曲可能である。各アーム２２ａの一端は、直交する３軸回りに回転可能に操作部２１にボールジョイント等の自在継手を介して連結されている。各アーム２２ａの他端は、サーボモータ２５に減速機等（図示省略）を介して連結されている。サーボモータ２５は、ベース２０上に配置されている。

ベース２０の上面には、６つのサーボモータ２５が配置されている。同組の２つのアーム２２ａに連結された２つのサーボモータ２５が１つの組を形成している。各組の２つのサーボモータ２５の回転軸は、一直線上に、即ち、同軸上に延びている。３組のサーボモータ２５の回転軸が三角形を形成するように、６つのサーボモータ２５は配置されている。

このように構成された支持機構２２は、操作部２１が３次元空間内で任意の位置及び姿勢をとることができるように、操作部２１を支持する。操作部２１の位置及び姿勢に対応して、サーボモータ２５が回転する。サーボモータ２５の回転量、即ち、回転角は、一義的に決まる。

操作装置２には、直交３軸のマスタ座標系が規定されている。マスタ座標系は、操作装置２を基準に設定されている。マスタ座標系は、互いに直交するＸｍ軸、Ｙｍ軸、及びＺｍ軸を有している。Ｘｍ軸、Ｙｍ軸、及びＺｍ軸は、原点Ｏｍにおいて互いに交差している。原点Ｏｍは、ベース２０の上面に位置している。Ｘｍ軸及びＹｍ軸は、水平方向に、即ち、ベース２０の上面と平行に延びている。Ｚｍ軸は、鉛直方向に延びている。Ｚｍ軸は、３組のサーボモータ２５の回転軸によって形成される三角形の重心を通る。マスタ座標系は、操作装置２のベース２０に固定された座標系である。

また、操作部２１には、直交３軸の操作座標系が規定されている。操作座標系は、操作部２１に固定された座標系である。操作座標系は、互いに直交するＸｎ軸、Ｙｎ軸及びＺｎ軸を有している。Ｘｎ軸、Ｙｎ軸及びＺｎ軸は、原点Ｏｎにおいて互いに直交している。例えば、原点Ｏｎは、操作部２１の中央に位置している。操作座標系は、マスタ座標系から見ると、操作部２１の位置及び姿勢に応じて変位する。つまり、操作座標系は、操作部２１の移動に応じて、操作部２１と共に移動する。この例では、操作座標系は、ツール座標系と対応している。

操作力センサ２３は、この例では、図１に示すように操作部２１と支持機構２２との間（具体的には、操作部２１と支持機構２２との連結部）に設けられている。操作力センサ２３は、直交する３軸方向の力と該３軸回りのモーメントを検出する。操作力センサ２３は、操作力検出部の一例である。

尚、操作力検出部は、操作力センサ２３に限定されない。例えば、操作力センサ２３は、１軸、２軸又は３軸方向の力のみを検出してもよい。あるいは、操作力検出部は、支持機構２２のサーボモータ２５の電流を検出する電流センサ又はサーボモータ２５のトルクを検出するトルクセンサ等であってもよい。

図５は、操作制御装置２４の概略的なハードウェア構成を示す図である。操作制御装置２４は、サーボモータ２５を制御することによって支持機構２２を動作させる。操作制御装置２４は、操作力センサ２３の検出信号を受け付ける。操作制御装置２４は、制御装置３と情報、指令及びデータ等の送受信を行う。操作制御装置２４は、制御部２６と、記憶部２７と、メモリ２８とを有している。

制御部２６は、操作制御装置２４の全体を制御する。制御部２６は、各種の演算処理を行う。例えば、制御部２６は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御部２６は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）等で形成されていてもよい。

記憶部２７は、制御部２６で実行されるプログラム及び各種データを格納している。記憶部２７は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。

メモリ２８は、データ等を一時的に格納する。例えば、メモリ２８は、揮発性メモリで形成される。

〈制御装置３の構成〉
制御装置３は、ロボット１及び操作装置２を制御する。制御装置３は、操作装置２を介して入力される操作情報に応じてエンドエフェクタ１１を動作させるように、ロボット１への指令であるスレーブ指令をロボット１へ出力する。制御装置３は、操作装置２を介した操作に応じてロボットアーム１２を制御することによってエンドエフェクタ１１に対象物Ｗへ作用を加えさせる。さらに、制御装置３は、対象物Ｗからロボット１が受ける反力に応じて操作部２１を動作させるように、操作装置２への指令であるマスタ指令を操作装置２へ出力する。制御装置３は、支持機構２２を制御することによって、エンドエフェクタ１１が対象物Ｗから受ける反力を操作者Ｕに提示する。

図６は、制御装置３の概略的なハードウェア構成を示す図である。制御装置３は、ロボット制御装置１４及び操作制御装置２４と情報、指令及びデータ等の送受信を行う。制御装置３は、制御部３１と、記憶部３２と、メモリ３３とを有している。

制御部３１は、制御装置３の全体を制御する。制御部３１は、各種の演算処理を行う。例えば、制御部３１は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御部３１は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）等で形成されていてもよい。

記憶部３２は、制御部３１で実行されるプログラム及び各種データを格納している。記憶部３２は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。例えば、記憶部３２は、ロボット制御プログラム３２１を格納している。

ロボット制御プログラム３２１は、ロボットシステム１００を制御する機能をコンピュータとしての制御部３１に実現させるためのプログラムである。

メモリ３３は、データ等を一時的に格納する。例えば、メモリ３３は、揮発性メモリで形成される。

また、制御装置３は、操作者Ｕが入力操作を行うことができる入力装置３４と、操作者Ｕへの情報提示を可能とする表示装置３５とを備えている。

図７は、制御部３１の機能ブロック図である。制御部３１は、記憶部３２からロボット制御プログラム３２１をメモリ３３に読み出して展開することによって、各種機能を実現する。具体的には、制御部３１は、力制御部３６、推定部３７、提示部３８、設定部３９及び調整部３１０として機能する。

力制御部３６は、力制御を実行する。力制御とは、操作者から操作装置２へ加えられる操作力に応じてロボット１を動作させて対象物Ｗに作用を加えると共に、ロボット１が対象物Ｗから受ける反力に応じて操作装置２を動作させる制御である。詳しくは、力制御部３６は、ロボット１と操作装置２との間でバイラテラル制御を行う。力制御部３６は、操作装置２へ入力される操作力及び対象物Ｗからロボット１へ作用する反力に基づいて、ロボット１へのスレーブ指令及び操作装置２へのマスタ指令を生成する。力制御部３６は、スレーブ指令をロボット１へ出力すると共にマスタ指令を操作装置２へ出力することによってロボット１及び操作装置２を制御する。

推定部３７は、操作者Ｕの動力学特性を推定する。詳しくは、推定部３７は、操作者Ｕの動力学モデルである操作者モデルを用いて、操作者Ｕの動力学特性を推定する。さらに、推定部３７は、対象物Ｗの動力学特性、ロボット１の動力学特性及び操作装置２の動力学特性を推定する。詳しくは、推定部３７は、対象物Ｗの動力学モデルである対象物モデルを用いて、対象物Ｗの動力学特性を推定する。推定部３７は、ロボット１の動力学モデルであるスレーブモデルを用いて、ロボット１の動力学特性を推定する。推定部３７は、操作装置２の動力学モデルであるマスタモデルを用いて、操作装置２の動力学特性を推定する。推定部３７は、力制御部３６がロボット１及び操作装置２の力制御を実行している際に、操作者Ｕ、対象物Ｗ、ロボット１及び操作装置２の動力学特性を推定する。

提示部３８は、力制御の周波数特性を求め、求められた周波数特性を提示する。具体的には、提示部３８は、力制御部３６がロボット１及び操作装置２の力制御を実行している際に、力制御の周波数特性を求める。提示部３８は、推定部３７によって推定された操作者Ｕ、対象物Ｗ、ロボット１及び操作装置２の動力学特性を用いて周波数特性を求める。提示部３８は、周波数特性を所定の評価値の形式で表現する。提示部３８は、評価値を表示装置３５に表示させる。

設定部３９は、力制御の目標周波数特性の入力を受け付けて、目標周波数特性を設定する。具体的には、設定部３９は、操作者Ｕからの入力装置３４を介した、目標周波数特性の入力を受け付ける。例えば、設定部３９は、目標周波数特性の入力においても、周波数特性の評価値を利用する。つまり、設定部３９は、目標となる評価値の入力を受け付けることによって、実質的に目標周波数特性の入力を受け付ける。

調整部３１０は、力制御の周波数特性が設定部３９によって設定された目標周波数特性となるように力制御の制御パラメータを調整する。この例では、調整部３１０は、力制御の周波数特性が目標周波数特性となるように、操作者Ｕ、対象物Ｗ、ロボット１及び操作装置２の動力学特性の少なくとも１つに応じて制御パラメータを調整する。

これらの機能ブロックについてさらに詳細に説明する。

〈ロボットシステムにおける力制御〉
図８は、力制御部３６を詳細な機能に分解したブロック図である。図８では、ロボット制御装置１４の制御部１６及び操作制御装置２４の制御部２６の機能ブロックも図示されている。

ロボット制御装置１４の制御部１６は、記憶部１７からプログラムをメモリ１８に読み出して展開することによって、各種機能を実現する。具体的には、制御部１６は、入力処理部４１と動作制御部４２として機能する。

入力処理部４１は、接触力センサ１３及びサーボモータ１５から受け取る情報、データ及び指令等を制御装置３に出力する。具体的には、入力処理部４１は、接触力センサ１３から６軸の力の検出信号を受け取り、該検出信号を操作情報として制御装置３へ出力する。また、入力処理部４１は、サーボモータ１５から回転センサ（例えば、エンコーダ）及び電流センサの検出信号を受け取る。入力処理部４１は、動作制御部４２によるロボットアーム１２のフィードバック制御のために該検出信号を動作制御部４２へ出力する。また、入力処理部４１は、ロボットアーム１２の位置情報として該検出信号を制御装置３へ出力する。

動作制御部４２は、制御装置３からスレーブ指令（具体的には、指令位置ｘｄｓ）を受け取り、スレーブ指令に従ってロボットアーム１２を動作させるための制御指令を生成する。動作制御部４２は、サーボモータ１５へ制御指令を出力し、ロボットアーム１２を動作させ、研削装置１１ａを指令位置に対応する位置へ移動させる。このとき、動作制御部４２は、入力処理部４１からのサーボモータ１５の回転センサ及び／又は電流センサの検出信号に基づいて、ロボットアーム１２の動作をフィードバック制御する。また、動作制御部４２は、研削装置１１ａへ制御指令を出力し、研削装置１１ａを動作させる。これにより、研削装置１１ａが対象物Ｗを研削する。

操作制御装置２４の制御部２６は、記憶部２７からプログラムをメモリ２８に読み出して展開することによって、各種機能を実現する。具体的には、制御部２６は、入力処理部５１と動作制御部５２として機能する。

入力処理部５１は、操作力センサ２３から受け取る情報、データ及び指令等を制御装置３に出力する。具体的には、入力処理部５１は、操作力センサ２３から６軸の力の検出信号を受け取り、該検出信号を反力情報として制御装置３へ出力する。また、入力処理部５１は、サーボモータ２５から回転センサ（例えば、エンコーダ）及び電流センサの検出信号を受け取る。入力処理部５１は、動作制御部５２による支持機構２２のフィードバック制御のために該検出信号を動作制御部５２へ出力する。

動作制御部５２は、制御装置３からマスタ指令（具体的には、指令位置ｘｄｍ）を受け取り、マスタ指令に従って支持機構２２を動作させるための制御指令を生成する。動作制御部５２は、サーボモータ２５へ制御指令を出力し、支持機構２２を動作させ、操作部２１を指令位置に対応する位置へ移動させる。このとき、動作制御部５２は、入力処理部５１からのサーボモータ２５の回転センサ及び／又は電流センサの検出信号に基づいて、支持機構２２の動作をフィードバック制御する。これにより、操作者Ｕが操作部２１に与える操作力に対して反力が与えられる。その結果、操作者Ｕは、対象物Ｗから反力を操作部２１から疑似的に感じつつ、操作部２１を操作することができる。

制御装置３の制御部３１は、記憶部３２からロボット制御プログラム３２１をメモリ３３に読み出して展開することによって、各種機能を実現する。具体的には、制御部３１は、操作力取得部６１と接触力取得部６２と加算部６３と力／速度換算部６４とスレーブ出力部６５とマスタ出力部６６として機能する。操作力取得部６１、接触力取得部６２、加算部６３、力／速度換算部６４、スレーブ出力部６５及びマスタ出力部６６は、力制御部３６の一部である。これらの機能によって、制御装置３は、操作情報及び反力情報に応じてスレーブ指令及びマスタ指令を生成する。

操作力取得部６１は、入力処理部５１を介して、操作力センサ２３の検出信号を受け取り、検出信号に基づいて操作力ｆｍを取得する。例えば、操作力取得部６１は、操作力センサ２３の検出信号から、操作部２１に作用する力であって操作座標系で表される力を操作力ｆｍとして求める。操作力取得部６１は、操作力ｆｍを加算部６３へ入力する。

接触力取得部６２は、入力処理部４１を介して、接触力センサ１３の検出信号を受け取り、検出信号に基づいて接触力ｆｓを取得する。例えば、接触力取得部６２は、接触力センサ１３の検出信号から、エンドエフェクタ１１のうち対象物Ｗとの接点に作用する力であってツール座標系で表される力を接触力ｆｓとして求める。接触力取得部６２は、接触力ｆｓを加算部６３へ入力する。

加算部６３は、操作力取得部６１から入力された操作力ｆｍと接触力取得部６２から入力された接触力ｆｓとの和を算出する。ここで、操作力ｆｍと接触力ｆｓとは、反対向きの力なので、操作力ｆｍと接触力ｆｓとは正負の符号が異なる。つまり、操作力ｆｍと接触力ｆｓとの和である合成力ｆｍ＋ｆｓの絶対値は、操作力ｆｍの絶対値よりも小さくなる。

力／速度換算部６４は、合成力ｆｍ＋ｆｓを指令速度ｘｄ’に換算する。力／速度換算部６４は、慣性係数及び粘性係数（ダンパ係数）を含む運動方程式に基づく運動モデルを用いて、合成力ｆｍ＋ｆｓが作用した場合の物体の指令速度ｘｄ’を算出する。具体的には、力／速度換算部６４は、以下の運動方程式に基づいて指令速度ｘｄ’を算出する。

ここで、ｘｄは、物体の位置である。ｍｄは、慣性係数である。ｃｄは、粘性係数である。ｆｍは、操作力である。ｆｓは、接触力である。尚、「’」は１回微分を表し、「”」は２回微分を表す。

式（１）は線形微分方程式であり、式（１）を指令速度ｘｄ’について解くと、ｘｄ’＝Ｖ（ｆｍ，ｆｓ）となる。Ｖ（ｆｍ，ｆｓ）は、ｆｍ，ｆｓを変数とし，ｍｄ，ｃｄ等を定数とする関数である。ｍｄ，ｃｄは、力制御における制御パラメータである。

関数Ｖ（ｆｍ，ｆｓ）は、記憶部３２に格納されている。力／速度換算部６４は、記憶部３２から関数Ｖ（ｆｍ，ｆｓ）を読み出して指令速度ｘｄ’を求める。力／速度換算部６４は、求められた指令速度ｘｄ’をスレーブ出力部６５とマスタ出力部６６とへ出力する。

スレーブ出力部６５は、指令速度ｘｄ’に基づいてスレーブ指令を生成する。詳しくは、スレーブ出力部６５は、指令速度ｘｄ’をエンドエフェクタ１１の指令位置ｘｄｓに換算する。指令位置ｘｄｓは、ツール座標系における位置である。指令位置ｘｄｓがスレーブ指令である。例えば、操作装置２の移動量に対するロボット１の移動量の比が設定されている場合、スレーブ出力部６５は、指令速度ｘｄ’から求めた指令位置を移動比に応じて逓倍して指令位置ｘｄｓを求める。スレーブ出力部６５は、指令位置ｘｄｓをロボット制御装置１４、具体的には、動作制御部４２へ出力する。

動作制御部４２は、指令位置ｘｄｓへのエンドエフェクタ１１の移動を実現させるための、サーボモータ１５への制御指令を生成する。動作制御部４２は、生成された制御指令をサーボモータ１５へ出力することによって、ロボットアーム１２を動作させ、エンドエフェクタ１１を指令位置ｘｄｓに対応する位置へ移動させる。

マスタ出力部６６は、指令速度ｘｄ’に基づいてマスタ指令を生成する。詳しくは、マスタ出力部６６は、指令速度ｘｄ’を操作部２１の指令位置ｘｄｍに換算する。指令位置ｘｄｍは、操作座標系における位置である。指令位置ｘｄｍがマスタ指令である。マスタ出力部６６は、指令位置ｘｄｍを操作制御装置２４、具体的には、動作制御部５２へ出力する。

動作制御部５２は、指令位置ｘｄｍへの操作部２１の移動を実現させるための、サーボモータ２５への制御指令を生成する。動作制御部５２は、生成された制御指令をサーボモータ２５へ出力することによって、支持機構２２を動作させ、操作部２１を指令位置ｘｄｍに対応する位置へ移動させる。

次に、力制御におけるロボットシステム１００の動作について説明する。例えば、操作者Ｕは、操作装置２を操作して、ロボット１によって対象物Ｗに研削加工を行う。

まず、制御装置３は、操作力及び接触力を取得する。操作者Ｕが操作装置２を操作すると、操作者Ｕが操作部２１を介して加えた操作力を操作力センサ２３が検出する。操作力センサ２３に検出された操作力は、入力処理部５１によって検出信号として制御装置３へ入力される。このとき、ロボット１の接触力センサ１３によって検出される接触力が、入力処理部４１によって検出信号として制御装置３へ入力される。

制御装置３では、操作力取得部６１が、検出信号に基づく操作力ｆｍを加算部６３へ入力する。接触力取得部６２は、検出信号に基づく接触力ｆｓを加算部６３へ入力する。

続いて、制御装置３は、マスタ指令及びスレーブ指令の指令速度ｘｄ’を生成する。具体的には、加算部６３は、合成力ｆｍ＋ｆｓを力／速度換算部６４へ入力する。力／速度換算部６４は、関数Ｖ（ｆｍ，ｆｓ）を用いて、合成力ｆｍ＋ｆｓから指令速度ｘｄ’を求める。

その後、スレーブ出力部６５は、指令速度ｘｄ’からエンドエフェクタ１１の指令位置ｘｄｓ（即ち、スレーブ指令）を生成する。それと並行して、マスタ出力部６６は、操作部２１の指令位置ｘｄｍ（即ち、マスタ指令）を生成する。スレーブ出力部６５は、指令位置ｘｄｓをロボット１へ出力すると共に、マスタ出力部６６は、指令位置ｘｄｍを操作装置２へ出力する。これにより、ロボット１は、指令位置ｘｄｓに応じて動作して研削を実行する。それと並行して、操作装置２は、指令位置ｘｄｍに応じて動作して、操作者Ｕに反力を提示する。

〈周波数特性の提示〉
提示部３８は、このような力制御の周波数特性を求め、求められた周波数特性を提示する。力制御の周波数特性は、ロボットシステム１００における様々な要素の影響を受ける。図９は、ロボットシステム１００の全体のブロック線図である。ロボットシステム１００では、前述の如く、操作者Ｕが操作装置２を把持して操作装置２を操作する。操作者からの操作力ｆｍは、操作力センサ２３を介して制御装置３へ入力される。一方、エンドエフェクタ１１は、対象物Ｗに接触している。そのときの接触力ｆｓが接触力センサ１３を介して制御装置３へ入力される。制御装置３へは、合成力ｆｍ＋ｆｓが入力される。制御装置３の力制御部３６は、合成力ｆｍ＋ｆｓから、スレーブ指令及びマスタ指令を生成する。ロボット制御装置１４は、ロボット１の位置をスレーブ指令に応じて制御し、エンドエフェクタ１１が対象物Ｗを加工する。これと並行して、操作制御装置２４は、操作装置２の位置をマスタ指令に応じて制御し、操作部２１が操作者Ｕへ反力を提示する。

ここで、操作部２１は、操作者に把持されているので、操作部２１の動作は、操作者Ｕの動力学特性、例えば、機械インピーダンスの影響を受ける。操作者Ｕの剛性及び機械抵抗等には個人差がある。また、同じ操作者Ｕであっても、操作者Ｕの姿勢（腕が曲がり具合等の腕の状態も含む）に応じて操作者Ｕの剛性及び機械抵抗等は変化し得る。そのため、操作部２１の動作は、操作者Ｕの動力学特性の影響を受ける。

同様に、ロボット１のエンドエフェクタ１１は、対象物Ｗに接触しているので、エンドエフェクタ１１の動作は、対象物Ｗの動力学特性、例えば、機械インピーダンスの影響を受ける。対象物Ｗの剛性及び機械抵抗等には、個体差がある。また、同じ対象物Ｗであっても、加工の状況に応じて対象物Ｗの剛性及び機械抵抗等は変化し得る。そのため、エンドエフェクタ１１の動作は、対象物Ｗの動力学特性の影響を受ける。

また、制御装置３による制御及び操作者Ｕによる操作部２１の操作によって支持機構２２の形状及び姿勢等が変化する。支持機構２２の形状及び姿勢等は、操作装置２の周波数応答に影響を与え得る。操作装置２の動力学特性、例えば、周波数応答は、力制御における操作装置２の動作に影響を与える。つまり、操作装置２の動作は、操作装置２の動力学特性の影響を受ける。

同様に、制御装置３による制御によってロボットアーム１２の形状及び姿勢等が変化する。ロボットアーム１２の形状及び姿勢等は、ロボット１の周波数応答に影響を与え得る。ロボット１の動力学特性、例えば、周波数応答は、力制御におけるロボット１の動作に影響を与える。つまり、ロボット１の動作は、ロボット１の動力学特性の影響を受ける。

提示部３８は、これらの操作者Ｕの動力学特性等を表す動力学モデルを含む制御系の伝達関数に基づいて、力制御の周波数特性を求める。図１０は、操作装置２を含むマスタ制御系７Ａのブロック線図である。図１１は、ロボット１を含むスレーブ制御系７Ｂのブロック線図である。

マスタ制御系７Ａは、図１０に示すように、合成力演算要素７１と、力制御要素７２と、操作装置要素７３と、操作者要素７４と、操作力センサ要素７５とを有している。

合成力演算要素７１は、操作力ｆｍと接触力ｆｓとの合成力ｆｍ＋ｆｓを算出する。尚、図１０では、合成力演算要素７１に入力される接触力ｆｓの符号が＋となっているが、操作力ｆｍと接触力ｆｓとは反対向きの力なので、合成力演算要素７１においては、実質的には操作力ｆｍから接触力ｆｓが減算される。

力制御要素７２は、合成力ｆｍ＋ｆｓから、エンドエフェクタ１１の指令位置ｘｄｓ及び操作部２１の指令位置ｘｄｍを算出する。力制御要素７２は、制御装置３の力／速度換算部６４、スレーブ出力部６５及びマスタ出力部６６に相当する。力制御要素７２の伝達関数Ｇｃは、下記の式（２）で表される。

ここで、ｍｄは、運動方程式（１）における慣性係数であり、ｃｄは、粘性係数である。つまり、ｍｄ及びｃｄは、力制御の制御パラメータである。

操作装置要素７３は、指令位置ｘｄｍを実際の操作部２１の現在位置ｘｃｍに変換する。つまり、操作装置要素７３は、操作装置２に相当し、操作装置２の動力学モデルであるマスタモデルで表される。マスタモデルは、操作装置２の動力学特性、具体的には、操作装置２の周波数応答を表している。例えば、マスタモデルは、一次遅れ系のモデルである。操作装置要素７３の伝達関数Ｇｐ１は、下記の式（３）で表される。

ここで、Ｔｃは、マスタモデルの時定数である。

操作者要素７４は、現在位置ｘｃｍを操作力ｆｍに変換する。つまり、操作者要素７４は、操作者Ｕに相当し、操作者Ｕの動力学モデルである操作者モデルで表される。操作者モデルは、操作者Ｕの動力学特性を表す。具体的には、操作者モデルは、操作者Ｕの機械インピーダンス、即ち、剛性係数ｋｕ及び粘性係数ｃｕを含んでいる。例えば、操作者モデルをバネ・ダンパモデルで近似すると、操作者モデルは、下記の式（４）で表される。その場合、操作者要素７４の伝達関数Ｇｋ１は、下記の式（５）で表される。

操作力センサ要素７５は、操作力センサ２３によって検出された操作力ｆｍのノイズを除去する。操作力センサ要素７５は、ノイズ除去フィルタ、例えば、ローパスフィルタに相当する。操作力センサ要素７５は、一次遅れ系のモデルで表される。操作力センサ要素７５の伝達関数Ｇｆ１は、下記の式（６）で表される。

ここで、Ｔｆ１は、操作力センサ２３のモデルの時定数である。

このようなマスタ制御系７Ａの一巡伝達関数Ｇｍ（ｊω）は、下記の式（７）のようになる。

一巡伝達関数Ｇｍ（ｊω）から、マスタ制御系７Ａの周波数特性を求めることができる。例えば、一巡伝達関数Ｇｍ（ｊω）から、ボード線図におけるゲイン交差周波数ωｃｇ１及びゲイン余裕ｇｍ１を求めることができる。ゲイン交差周波数ωｃｇ１及びゲイン余裕ｇｍ１は、マスタ制御系７Ａの周波数特性を表している。ゲイン交差周波数ωｃｇ１は、応答性に関連している。ゲイン余裕ｇｍ１は、安定性に関連している。

一方、スレーブ制御系７Ｂは、図１１に示すように、合成力演算要素７１と、力制御要素７２と、ロボット要素７６と、対象物要素７７と、接触力センサ要素７８とを有している。

合成力演算要素７１及び力制御要素７２は、マスタ制御系７Ａと同様である。

ロボット要素７６は、指令位置ｘｄｓを実際のエンドエフェクタ１１の現在位置ｘｃｓに変換する。つまり、ロボット要素７６は、ロボット１に相当し、ロボット１の動力学モデルであるスレーブモデルで表される。スレーブモデルは、ロボット１の動力学特性、具体的には、ロボット１の周波数応答を表している。例えば、スレーブモデルは、一次遅れ系のモデルである。ロボット要素７６の伝達関数Ｇｐ２は、下記の式（８）で表される。

ここで、Ｔｒは、スレーブモデルの時定数である。

対象物要素７７は、現在位置ｘｃｓを接触力ｆｓに変換する。つまり、対象物要素７７は、対象物Ｗに相当し、対象物Ｗの動力学モデルである対象物モデルで表される。対象物モデルは、対象物Ｗの動力学特性を表す。具体的には、対象物モデルは、対象物Ｗの機械インピーダンス、即ち、剛性係数ｋｗ及び粘性係数ｃｗを含んでいる。例えば、対象物モデルをバネ・ダンパモデルで近似すると、対象物モデルは、下記の式（９）で表される。その場合、対象物要素７７の伝達関数Ｇｋ２は、下記の式（１０）で表される。

接触力センサ要素７８は、接触力センサ１３によって検出された接触力ｆｓのノイズを除去する。接触力センサ要素７８は、ノイズ除去フィルタ、例えば、ローパスフィルタに相当する。接触力センサ要素７８は、一次遅れ系のモデルで表される。接触力センサ要素７８の伝達関数Ｇｆ２は、下記の式（１１）で表される。

ここで、Ｔｆ２は、接触力センサ１３のモデルの時定数である。

このようなスレーブ制御系７Ｂの一巡伝達関数Ｇｓ（ｊω）は、下記の式（１２）のようになる。

一巡伝達関数Ｇｓ（ｊω）から、スレーブ制御系７Ｂの周波数特性を求めることができる。例えば、一巡伝達関数Ｇｓ（ｊω）から、ボード線図におけるゲイン交差周波数ωｃｇ２及びゲイン余裕ｇｍ２を求めることができる。ゲイン交差周波数ωｃｇ２及びゲイン余裕ｇｍ２は、スレーブ制御系７Ｂの周波数特性を表している。ゲイン交差周波数ωｃｇ２は、応答性に関連している。ゲイン余裕ｇｍ２は、安定性に関連している。

提示部３８は、マスタ制御系７Ａのゲイン交差周波数ωｃｇ１及びゲイン余裕ｇｍ１、並びに、スレーブ制御系７Ｂのゲイン交差周波数ωｃｇ２及びゲイン余裕ｇｍ２（以下、「ゲイン交差周波数ωｃｇ１等」とも称する）を求め、ゲイン交差周波数ωｃｇ１等を用いて力制御の周波数特性を表す。

提示部３８は、応答性に関連する第１評価値と安定性に関連する第２評価値とで力制御の周波数特性を表現する。例えば、提示部３８は、第１評価値を「速さ」と称し、０～１００％の値で表す。提示部３８は、第２評価値を「揺れにくさ」と称し、０～１００％の値で表す。第１評価値と第２評価値とは相対的な関係を有する。つまり、第１評価値が高い場合には、第２評価値は低くなる。一方、第１評価値が低い場合には、第１評価値は高くなる。

速さの０％は、応答性が低いことを意味し、速さの１００％は、応答性が高いことを意味する。例えば、ゲイン交差周波数が所定の第１周波数以下のときに、速さは０％となる。ゲイン交差周波数が第２周波数（＞第１周波数）以上のときに速さは１００％となる。ゲイン交差周波数が第１周波数よりも大きく第２周波数よりも小さいときには、速さは、０％よりも大きく１００％よりも小さい値となる。より詳しくは、ゲイン交差周波数が第１周波数に近いときには速さは０％に近い値となる。ゲイン交差周波数が第２周波数に近いときには速さは１００％に近い値となる。

揺れにくさの０％は、安定性が低いことを意味し、揺れにくさの１００％は、安定性が高いことを意味する。例えば、ゲイン余裕が所定の第１値以下のときには、揺れにくさは０％となる。ゲイン余裕が第２値（＞第１値）以上のときには、揺れにくさは１００％となる。ゲイン余裕が第１値よりも大きく第２値よりも小さいときには、揺れにくさは０％よりも大きく１００％よりも小さい値となる。より詳しくは、ゲイン余裕が第１値に近いときには揺れにくさは０％に近い値となる。ゲイン余裕が第２値に近いときには揺れにくさは１００％に近い値となる。

提示部３８は、マスタ制御系７Ａに関し、ゲイン交差周波数ωｃｇ１から速さを求め、ゲイン余裕ｇｍ１から揺れにくさを求める。提示部３８は、スレーブ制御系７Ｂに関し、ゲイン交差周波数ωｃｇ２から速さを求め、ゲイン余裕ｇｍ２から揺れにくさを求める。つまり、提示部３８は、マスタ制御系７Ａの速さ及び揺れにくさとスレーブ制御系７Ｂの速さ及び揺れにくさとを求める。

提示部３８は、こうして求められたマスタ制御系７Ａに関する速さ及び揺れにくさ、並びに、スレーブ制御系７Ｂに関する速さ及び揺れにくさを表示装置３５に表示させる。こうして、操作者Ｕは、操作者Ｕ及び操作装置２の動力学特性を考慮したマスタ制御系７Ａの周波数特性、並びに、対象物Ｗ及びロボット１の動力学特性を考慮したスレーブ制御系７Ｂの周波数特性を知ることができる。

〈動力学特性の推定〉
ただし、対象物Ｗ、操作者Ｕ、ロボット１及び操作装置２のそれぞれの動力学特性は、一定ではない。例えば、対象物Ｗの剛性係数ｋｗ及び粘性係数ｃｗは、対象物Ｗごとの個体差及び対象物Ｗにおける加工場所等によって異なり得る。操作者Ｕの剛性係数ｋｕ及び粘性係数ｃｕは、操作者Ｕごとの個人差及び操作者Ｕの姿勢等によって異なり得る。また、ロボット１の時定数Ｔｒは、ロボットアーム１２の機差及び形状等によって異なり得る。操作装置２の時定数Ｔｃは、支持機構２２の機差及び形状等によって異なり得る。

そのため、推定部３７は、対象物Ｗ、操作者Ｕ、ロボット１及び操作装置２のそれぞれの動力学特性を推定する。提示部３８は、推定された対象物Ｗ、操作者Ｕ、ロボット１及び操作装置２のそれぞれの動力学特性を用いてゲイン交差周波数ωｃｇ１等、ひいては、力制御の周波数特性を導出する。

尚、この例では、操作力センサ２３の時定数Ｔｆ１及び接触力センサ１３の時定数Ｔｆ２は、一定であると想定している。ただし、推定部３７は、操作力センサ２３及び接触力センサ１３の動力学特性として時定数Ｔｆ１及び時定数Ｔｆ２をさらに推定してもよい。その場合、提示部３８は、ゲイン交差周波数ωｃｇ１等を導出する際に、推定された操作力センサ２３及び接触力センサ１３の動力学特性も考慮する。

具体的には、推定部３７は、力制御の実行中に、対象物Ｗの剛性係数ｋｗ及び粘性係数ｃｗ、操作者Ｕの剛性係数ｋｕ及び粘性係数ｃｕ、ロボット１の時定数Ｔｒ、並びに、操作装置２の時定数Ｔｃを推定する。

推定部３７は、操作者Ｕの機械インピーダンス、即ち、剛性係数ｋｕ及び粘性係数ｃｕを用いて下記の式（１３）のように表される操作者モデルを想定する。

ここで、ｘｍは、操作部２１の変位量である。

推定部３７は、この操作者モデルから、θ及びξを式（１４）のように設定し、式（１５），（１６）で示す忘却係数付き逐次最小二乗法によってθを力制御の実行中に時々刻々と推定する。尚、式（１５）におけるｙ(ｋ)には、式（１３）のｆｍが代入される。

ここで、θ(k)は、時刻kにおけるθである。Γ(k)は、時刻kにおけるΓである。λは、忘却係数であり、０＜λ＜１である。

こうして、推定部３７は、操作者モデルを用いて操作者Ｕの剛性係数ｋｕ及び粘性係数ｃｕを推定する。

推定部３７は、対象物Ｗの機械インピーダンス、即ち、剛性係数ｋｗ及び粘性係数ｃｗを用いて下記の式（１７）のように表される対象物モデルを想定する。

ここで、ｘｓは、エンドエフェクタ１１の変位量である。

推定部３７は、この対象物モデルから、θ及びξを式（１８）のように設定し、前述の式（１５），（１６）で示す忘却係数付き逐次最小二乗法によってθを力制御の実行中に時々刻々と推定する。尚、式（１５）におけるｙ(ｋ)には、式（１７）のｆｓが代入される。

こうして、推定部３７は、対象物モデルを用いて対象物Ｗの剛性係数ｋｗ及び粘性係数ｃｗを推定する。

推定部３７は、下記の式（１９）のように、時定数Ｔｒを用いた一次遅れ系のモデルでスレーブモデルを想定する。

ここで、Ｘｃｕｒは、現在位置であり、Ｘｃｍｄは、指令位置である。

推定部３７は、このスレーブモデルから、θ及びξを式（２０）のように設定し、前述の式（１５），（１６）で示す忘却係数付き逐次最小二乗法によってθを力制御の実行中に時々刻々と推定する。尚、式（１５）にｙ(ｋ)には、式（１７）のＸｃｕｒが代入される。

こうして、推定部３７は、スレーブモデルを用いてロボット１の時定数Ｔｒを推定する。

推定部３７は、下記の式（２１）のように、時定数Ｔｃを用いた一次遅れ系のモデルでマスタモデルを想定する。

推定部３７は、このマスタモデルから、θ及びξを式（２２）のように設定し、前述の式（１５），（１６）で示す忘却係数付き逐次最小二乗法によってθを力制御の実行中に時々刻々と推定する。尚、式（１５）におけるｙ(ｋ)には、式（２１）のＸｃｕｒが代入される。

こうして、推定部３７は、マスタモデルを用いて操作装置２の時定数Ｔｃを推定する。

ただし、このようにして推定される操作者Ｕ等の動力学特性も、ロボット１等の状態によっては推定できない場合がある。例えば、操作はＵが操作部２１を把持していない場合によっては、操作力ｆｍが検出されないため、推定部３７は、剛性係数ｋｕ等を推定することができない。エンドエフェクタ１１が対象物Ｗに接触していない場合には、接触力ｆｓが検出されないため、推定部３７は、剛性係数ｋｗ等を推定することができない。あるいは、ロボット１及び操作装置２が停止している場合には、エンドエフェクタ１１及び操作装置２の変位が無いため、推定部３７は、時定数Ｔｒ等を推定することができない。

そのため、推定部３７は、操作者Ｕ等の動力学特性を推定可能な状態か否かを判定し、推定不能な状態の場合には推定値を保持する機能を有する。図１２は、推定部３７による推定処理のフローチャートである。このフローチャートに従った処理は、対象物Ｗ、操作者Ｕ、ロボット１及び操作装置２のそれぞれの動力学特性の推定について実行される。つまり、推定部３７は、図１２のフローチャートに従った対象物Ｗの動力学特性の推定、操作者Ｕの動力学特性の推定、ロボット１の動力学特性の推定、及び、操作装置２の動力学特性の推定を並行して別々に実行する。

推定の開始時は、剛性係数ｋｗ、粘性係数ｃｗ、剛性係数ｋｕ、粘性係数ｃｕ、時定数Ｔｒ、及び、時定数Ｔｃのそれぞれには初期値が設定されている。また、対象物Ｗ、操作者Ｕ、ロボット１及び操作装置２のそれぞれの動力学特性の推定における忘却係数付き逐次最小二乗法の式（１５）、（１６）のθ、Γ、λにも初期値が設定されている。

まず、推定部３７は、ステップＳ１０１において、動力学特性の推定が可能な状態か否かを判定する。

例えば、操作者Ｕ又は対象物Ｗの動力学特性の推定には、前述の如く、操作力ｆｍ又は接触力ｆｓが必要である。しかし、操作者Ｕが操作部２１に力を加えていない場合には操作力ｆｍが検出されない。また、エンドエフェクタ１１が対象物Ｗに接触していないときには接触力ｆｓが検出されない。そのため、操作者Ｕ又は対象物Ｗの動力学特性を推定する場合には、推定部３７は、操作力ｆｍ又は接触力ｆｓが所定期間以上検出されているか否かを判定する。推定部３７は、操作力ｆｍ又は接触力ｆｓの検出が瞬間的ではなく、所定期間継続していることを判定することによって、操作力ｆｍ又は接触力ｆｓの検出が加工作業によるものか偶発的なものかを判定することができる。

操作力ｆｍ又は接触力ｆｓが所定期間以上検出されている場合には、推定部３７は、操作者Ｕ又は対象物Ｗの動力学特性の推定可能状態であると判定する。一方、操作力ｆｍ又は接触力ｆｓが所定期間以上検出されていない場合には、推定部３７は、操作者Ｕ又は対象物Ｗの動力学特性の推定不能状態であると判定する。

一方、ロボット１又は操作装置２の動力学特性の推定には、前述の如く、エンドエフェクタ１１又は操作部２１の変位が必要である。そのため、ロボット１又は操作装置２の動力学特性を推定する場合には、推定部３７は、エンドエフェクタ１１又は操作部２１が変位しているかを判定する。具体的には、推定部３７は、指令速度ｘｄ’が所定閾値以上であることをもって、エンドエフェクタ１１又は操作部２１が変位していると判定する。

エンドエフェクタ１１又は操作部２１が変位している場合には、推定部３７は、ロボット１又は操作装置２の動力学特性の推定可能状態であると判定する。一方、エンドエフェクタ１１又は操作部２１が変位していない場合には、推定部３７は、ロボット１又は操作装置２の動力学特性の推定不能状態であると判定する。

ステップＳ１０１において推定可能状態と判定された場合には、推定部３７は、ステップＳ１０２において、忘却係数付き逐次最小二乗法に動力学特性の推定を行う。尚、操作者Ｕ又は対象物Ｗの動力学特性を推定する場合には、式（１４）、（１８）等に示すように、操作部２１又はエンドエフェクタ１１の変位量が必要である。そのため、ステップＳ１０１において推定可能状態と判定されたときの操作部２１又はエンドエフェクタ１１の位置を初期位置に設定して、操作部２１又はエンドエフェクタ１１の変位量を求める。このステップＳ１０２で求められた推定値は、暫定的であり、まだ確定的な推定値ではない。以下、ステップＳ１０２で推定されてから確定されるまでの推定値を「新推定値」と称し、確定して保持されている推定値を「現推定値」と称する。

次に、推定部３７は、ステップＳ１０３において、新推定値と現推定値との偏差の絶対値が所定値α以下か否かを判定する。例えば、推定不能状態から推定可能状態に遷移したときには、一時的に推定値の変動が大きい場合がある。ステップＳ１０３では、このような推定値の大きな変動があるか否かが判定される。

偏差の絶対値が所定値αよりも大きい場合には、推定部３７は、現推定値を更新することなく、ステップＳ１０１へ戻り、ステップＳ１０１の処理を再び実行する。

一方、偏差の絶対値が所定値α以下の場合には、推定部３７は、ステップＳ１０４において、推定値の平均を求める。詳しくは、推定部３７は、新推定値及び現推定値を含む、直近の所定個数の推定値の平均値を求める。続いて、推定部３７は、ステップＳ１０５において、推定値の平均値で現推定値を更新する。推定値の平均値で現推定値が更新されることによって、現推定値の急激な変動が防止される。

その後、推定部３７は、ステップＳ１０１へ戻り、ステップＳ１０１からの処理を繰り返す。

一方、ステップＳ１０１において推定不能状態と判定された場合には、推定部３７は、ステップＳ１０６において、忘却係数付き逐次最小二乗法における推定式の変数（具体的には、θ、Γ、λ）を初期化する。その後、推定部３７は、ステップＳ１０１へ戻り、ステップＳ１０１の処理を再び実行する。推定不能状態の間は、推定部３７は、ステップＳ１０１及びＳ１０６の処理を繰り返す。そして、推定不能状態から推定可能状態に遷移すると、推定部３７は、ステップＳ１０２以降の処理を実行し、新推定値の導出及び現推定値の更新等を実行する。つまり、推定部３７は、推定不能状態の間は、現推定値を更新せず保持する。

このようにして、推定部３７は、力制御の実行中に操作者Ｕ等の動力学特性を推定する。推定部３７は、動力学特性を推定可能か否かを判定することによって、動力学特性を適切に推定することができる。その結果、提示部３８は、力制御の周波数特性を適切に求めて、提示することができる。

〈制御パラメータの自動調整〉
ロボットシステム１００では、力制御の周波数特性が目標周波数特性となるように、力制御の制御パラメータが自動的に調整される。具体的には、設定部３９が目標周波数特性を設定し、調整部３１０が目標周波数特性に応じて制御パラメータを調整する。その際、推定部３７によって推定された操作者Ｕ等の動力学特性が用いられる。

詳しくは、設定部３９は、力制御の目標周波数特性の入力を入力装置３４を介して操作者Ｕから受け付け、目標周波数特性を設定する。設定部３９は、スレーブ制御系７Ｂの目標周波数特性とマスタ制御系７Ａの目標周波数特性とを個別に設定する。つまり、操作者Ｕは、スレーブ制御系７Ｂ及びマスタ制御系７Ａの一方又は両方の目標周波数特性を入力装置３４を介して入力する。

このとき、目標周波数特性の入力前に、提示部３８による周波数特性の提示があってもよい。この場合、操作者は、提示された現状の周波数特性を参考にして目標周波数特性を入力することができる。あるいは、予め目標周波数特性が決まっている場合等には、提示部３８による周波数特性の提示無しに目標周波数特性が入力されてもよい。

入力される目標周波数特性は、周波数特性に関連する任意のパラメータであり得る。例えば、目標周波数としてのパラメータは、提示部３８によって提示される周波数特性と同じ第1評価値（即ち、速さ）及び第２評価値（即ち、揺れにくさ）であってもよい。提示部３８によって現状の周波数特性が提示されてから目標周波数特性が入力される場合には、提示される周波数特性と入力される目標周波数特性とは同じパラメータであることが好ましい。ただし、目標周波数特性としてのパラメータは、提示部３８によって提示される周波数特性と異なるパラメータであってもよい。例えば、目標周波数特性のパラメータは、ゲイン交差周波数及びゲイン余裕であってもよい。

調整部３１０は、マスタ制御系７Ａとスレーブ制御系７Ｂとで別々に制御パラメータを調整する。具体的には、調整部３１０は、マスタ制御系７Ａの周波数特性が目標周波数特性となるように調整された制御パラメータである第１制御パラメータと、スレーブ制御系７Ｂの周波数特性が目標周波数特性となるように調整された制御パラメータである第２制御パラメータとを生成する。すなわち、第１制御パラメータは、操作者Ｕ等の動力学特性に応じて調整されている。第２制御パラメータは、対象物Ｗ等の動力学特性に応じて調整されている。

尚、マスタ制御系７Ａ及びスレーブ制御系７Ｂの一方だけの目標周波数特性が設定されている場合には、調整部３１０は、第１制御パラメータ及び第２制御パラメータのうち目標周波数特性が設定された制御系に対応する方の制御パラメータを生成する。

詳しくは、マスタ制御系７Ａの目標周波数特性が設定されている場合には、調整部３１０は、図１０に示すマスタ制御系７Ａのブロック線図及び式（７）のマスタ制御系７Ａの一巡伝達関数Ｇｍ（ｊω）に基づいて制御パラメータを調整する。調整部３１０は、操作者Ｕの動力学特性及び操作装置２の動力学特性を用いて制御パラメータを調整する。例えば、調整部３１０は、マスタ制御系７Ａのゲイン交差周波数ωｃｇ１及びゲイン余裕ｇｍ１が目標周波数特性に対応する値になるように、推定部３７によって推定された操作者Ｕの機械インピーダンス、即ち、剛性係数ｋｕ及び粘性係数ｃｕ、並びに、操作装置２の周波数応答、即ち、時定数Ｔｃを用いて、慣性係数ｍｄ及び粘性係数ｃｄを調整する。調整された慣性係数ｍｄ及び粘性係数ｃｄが、第１制御パラメータである。

スレーブ制御系７Ｂの目標周波数特性が設定されている場合には、調整部３１０は、図１１に示すスレーブ制御系７Ｂのブロック線図及び式（１２）のスレーブ制御系７Ｂの一巡伝達関数Ｇｓ（ｊω）に基づいて制御パラメータを調整する。調整部３１０は、対象物Ｗの動力学特性及びロボット１の動力学特性を用いて制御パラメータを調整する。例えば、調整部３１０は、スレーブ制御系７Ｂのゲイン交差周波数ωｃｇ２及びゲイン余裕ｇｍ２が目標周波数特性に対応する値になるように、推定部３７によって推定された対象物Ｗの機械インピーダンス、即ち、剛性係数ｋｗ及び粘性係数ｃｗ、並びに、ロボット１の周波数応答、即ち、時定数Ｔｒを用いて、慣性係数ｍｄ及び粘性係数ｃｄを調整する。調整された慣性係数ｍｄ及び粘性係数ｃｄが、第２制御パラメータである。

剛性係数ｋｕ、粘性係数ｃｕ、剛性係数ｋｗ、粘性係数ｃｗ、時定数Ｔｃ及び時定数Ｔｒは、前述の如く、力制御中に変化し得る。そのため、調整部３１０は、力制御中に推定部３７によって継続的に推定される剛性係数ｋｕ、粘性係数ｃｕ、剛性係数ｋｗ、粘性係数ｃｗ、時定数Ｔｃ及び時定数Ｔｒを用いて、慣性係数ｍｄ及び粘性係数ｃｄを継続的に調整する。

周波数応答が目標周波数応答となるような慣性係数ｍｄ及び粘性係数ｃｄを導出する方法は、種々の方法がある。例えば、調整部３１０は、ＦＲＩＴ（Fictitious Reference Iterative Tuning）を用いて、慣性係数ｍｄ及び粘性係数ｃｄを調整する。ＦＲＩＴは、データ駆動型調整手法の１つである。

あるいは、調整部３１０は、目標周波数特性に対応するゲイン交差周波数及びゲイン余裕を求め、求められたゲイン交差周波数及びゲイン余裕及び推定部３７によって推定された時定数Ｔｃ、剛性係数ｋｕ及び粘性係数ｃｕ、又は、時定数Ｔｒ、剛性係数ｋｗ及び粘性係数ｃｗを用いて、一巡伝達関数Ｇｍ又は一巡伝達関数Ｇｓに基づいて慣性係数ｍｄ及び粘性係数ｃｄを求めてもよい。

調整部３１０は、このような制御パラメータの調整を力制御中に実行する。力制御部３６は、調整部３１０によって調整された制御パラメータを用いて力制御を実行する。具体的には、力／速度換算部６４は、調整された制御パラメータを用いて指令速度ｘｄ’を求める。このとき、力制御部３６は、第１制御パラメータ及び第２制御パラメータの何れかを用いて力制御を実行する。

以下に、一例として、力制御部３６が第１制御パラメータと第２第２制御パラメータとを使い分けて力制御を実行する処理について説明する。図１３は、制御パラメータ調整のフローチャートである。

まず、ステップＳ２０１において、提示部３８は、現状の力制御の周波数特性を提示する。詳しくは、現状の操作者Ｕ、対象物Ｗ、ロボット１及び操作装置２によって既に実行された力制御において周波数特性が求められている場合等には、提示部３８は、その求められた周波数特性を表示装置３５を介して操作者Ｕに提示する。

それに対して、操作者Ｕが入力装置３４を介して目標周波数特性を入力すると、設定部３９は、力制御の目標周波数特性の入力を受け付け、目標周波数特性を設定する。この例では、マスタ制御系７Ａとスレーブ制御系７ｂとの両方の目標周波数特性が設定される。

その後、ステップＳ２０３において、力制御部３６は、力制御を開始する。力制御の開始後、推定部３７は、ステップＳ２０４において、操作者Ｕ等の動力学特性を推定する。ステップＳ２０４は、操作者の動力学特性を推定することに相当する。

そして、調整部３１０は、ステップＳ２０５において、力制御の周波数特性が目標周波数特性となるように、操作者Ｕ等の動力学特性等に応じて制御パラメータを調整する。このとき、調整部３１０は、マスタ制御系７Ａの周波数特性が目標周波数特性となるように調整された第１制御パラメータと、スレーブ制御系７Ｂの周波数特性が目標周波数特性となるように調整された第２制御パラメータとを生成する。

力制御部３６は、ステップＳ２０６において、エンドエフェクタ１１が対象物Ｗに接触しているか否かを判定する。力制御部３６は、接触力ｆｓが所定期間以上検出されているか否かを判定する。力制御部３６は、接触力ｆｓが所定期間以上検出されている場合には、エンドエフェクタ１１が対象物Ｗに接触していると判定する。力制御部３６は、接触力ｆｓの検出が瞬間的ではなく、所定期間継続していることを判定することによって、接触力ｆｓの検出が加工作業によるものか偶発的なものかを判定している。

尚、ここでの所定期間は、動力学特性の推定可能状態か否かを判定する際（ステップＳ１０１参照）の接触力ｆｓが検出されている所定期間と同じであっても、異なっていてもよい。

エンドエフェクタ１１が対象物Ｗへ接触している場合には、力制御部３６は、ステップＳ２０７において、第２制御パラメータを使用して力制御を実行する。つまり、スレーブ制御系７Ｂの周波数特性が目標周波数特性となるように制御パラメータが調整され、調整された制御パラメータによって力制御が実行される。

このように、力制御部３６は、エンドエフェクタ１１による加工中は第２制御パラメータを用いることによって、エンドエフェクタ１１及び操作部２１の両方の振動を防止して力制御を実行することができる。

詳しくは、ステップＳ２０２では、当然ながらマスタ制御系７Ａ及びスレーブ制御系７Ｂの何れにおいても振動が生じ難いような目標周波数特性が設定されると想定される。そして、対象物Ｗの機械インピーダンスは、通常、操作者Ｕの機械インピーダンスに比べて大きい。そのため、エンドエフェクタ１１を含むスレーブ制御系７Ｂの周波数特性が目標周波数特性となるように、即ち、スレーブ制御系７Ｂでの安定性が確保されるように制御パラメータが調整されることによって、マスタ制御系７Ａにおける操作部２１の振動も必然的に防止される。

一方、エンドエフェクタ１１が対象物Ｗへ接触していない場合には、力制御部３６は、ステップＳ２０８において、第１制御パラメータを使用して力制御を実行する。つまり、マスタ制御系７Ａの周波数特性が目標周波数特性となるように制御パラメータが調整され、調整された制御パラメータによって力制御が実行される。

エンドエフェクタ１１が対象物Ｗへ非接触の場合には、操作部２１が振動しなければ、エンドエフェクタ１１も振動しない。そのため、操作部２１を含むマスタ制御系７Ａの周波数特性が目標周波数特性となるように制御パラメータが調整されることによって、スレーブ制御系７Ｂにおけるエンドエフェクタ１１の振動も必然的に防止される。

尚、ステップＳ２０５，Ｓ２０６，Ｓ２０７，Ｓ２０８は、力制御の周波数特性が目標周波数特性となるように、推定された操作者の動力学特性に応じて力制御の制御パラメータを調整することに相当する。

ステップＳ２０７又はＳ２０８の後は、力制御部３６は、ステップＳ２０９において、力制御が終了されたか否かを判定する。力制御が終了していない場合には、力制御部３６は、ステップＳ２０６からの処理を繰り返す。

このように、力制御部３６は、エンドエフェクタ１１が対象物Ｗに接触しているか否かに応じて第１制御パラメータと第２制御パラメータとを使い分ける。具体的には、エンドエフェクタ１１が対象物Ｗに接触している場合には第２制御パラメータが使用され、エンドエフェクタ１１が対象物Ｗに接触していない場合には第１制御パラメータが使用される。その結果、エンドエフェクタ１１が対象物Ｗに接触している場合でも接触していない場合でも、エンドエフェクタ１１及び操作部２１の振動が効果的に防止される。その結果、力制御におけるロボット１及び操作装置２の挙動が安定する。

このように構成されたロボットシステム１００においては、操作者Ｕ又は対象物Ｗの動力学特性（例えば、機械インピーダンス）の変化を考慮した周波数特性が目標周波数特性になるように力制御の制御パラメータが調整される。それに加えて、操作装置２又はロボット１の動力学特性（例えば、周波数応答）の変化も考慮して、制御パラメータが調整される。その結果、力制御におけるロボット１及び操作装置２の挙動が安定する。

詳しくは、操作部２１は操作者Ｕに把持されるので、操作部２１は、支持機構２２だけでなく、操作者Ｕによっても支持されているとみなすことができる。つまり、操作部２１の安定性及び応答性は、操作者Ｕの機械インピーダンスの影響を受ける。操作者Ｕの機械インピーダンスは、操作者Ｕの個人差がある。さらに、同じ操作者Ｕであっても、操作者Ｕの姿勢、例えば、操作者Ｕの腕が伸びているか及び曲がっているか等の腕の状態に応じて操作者Ｕの機械インピーダンスが変化し得る。さらに、操作装置２の安定性及び応答性は、操作者Ｕの腕と同様に、支持機構２２の状態に応じて変化し得る。また、操作者Ｕの個人差ほどではないにしても、操作装置２にも多少の機差があり、この機差も操作装置２の安定性及び応答性に影響を与え得る。

一方、エンドエフェクタ１１は、対象物Ｗに接触しているので、エンドエフェクタ１１は、ロボットアーム１２だけでなく、対象物Ｗによっても支持されているとみなすことができる。つまり、エンドエフェクタ１１の安定性及び応答性は、対象物Ｗの機械インピーダンスの影響を受ける。対象物Ｗの機械インピーダンスは、対象物Ｗの個体差がある。さらに、同じ対象物Ｗであっても、対象物Ｗのうち加工部分又は加工の進行状況に応じて対象物Ｗの機械インピーダンスが変化し得る。さらに、ロボット１の安定性及び応答性は、ロボットアーム１２の形状に応じて変化し得る。また、対象物Ｗの個体差ほどではないにしても、ロボット１にも多少の機差があり、この機差もロボット１の安定性及び応答性に影響を与え得る。

これらの対象物Ｗ、操作者Ｕ、ロボット１及び操作装置２の動力学特性が考慮されない場合には、周波数特性が目標周波数特性となるように制御パラメータが調整されても、周波数特性が安定しにくい。

例えば、操作者Ｕが別の操作者Ｕに変わったり、同じ操作者Ｕであっても姿勢が変わったりすると、力制御の周波数特性が変化し得る。場合によっては、操作者Ｕが保持する操作部２１が振動する虞がある。あるいは、対象物Ｗが変わったり、対象物Ｗの加工部分が変わったりすると、力制御の周波数特性が変化し得る。場合によっては、エンドエフェクタ１１が振動する虞がある。

また、操作装置２の支持機構２２又はロボット１のロボットアーム１２の姿勢が変わると、力制御の周波数特性が変化し得る。例えば、力制御の応答性が変化し得る。

ロボットシステム１００では、実際の操作者Ｕの機械インピーダンス、操作装置２の周波数応答、対象物Ｗの機械インピーダンス、及びロボット１の周波数応答を推定し、これらの動力学特性の変化を考慮した力制御の周波数特性が目標周波数特性となるように、力制御の制御パラメータが調整される。そのため、操作者Ｕ等の動力学特性の変化に順応するように制御パラメータが調整される。その結果、操作者Ｕ等の変化があっても、力制御の挙動が安定する。

以上のように、制御装置３は、操作者Ｕから操作装置２（マスタ装置）へ加えられる操作力に応じてロボット１（スレーブ装置）を動作させて対象物Ｗに作用を加えると共に、ロボット１が対象物Ｗから受ける反力に応じて操作装置２を動作させる力制御を実行する力制御部３６と、操作者Ｕの動力学特性を推定する推定部３７と、力制御の周波数特性が目標周波数特性となるように、推定された操作者Ｕの動力学特性に応じて力制御の制御パラメータを調整する調整部３１０とを備える。

換言すると、制御装置３によるロボット制御方法は、操作者Ｕから操作装置２へ加えられる操作力に応じてロボット１を動作させて対象物Ｗに作用を加えると共に、ロボット１が対象物Ｗから受ける反力に応じて操作装置２を動作させる力制御を実行することと、操作者Ｕの動力学特性を推定することと、力制御の周波数特性が目標周波数特性となるように、推定された操作者Ｕの動力学特性に応じて力制御の制御パラメータを調整することとを含む。

また、ロボット制御プログラム３２１は、操作者Ｕから操作力が加えられる操作装置２と対象物Ｗに作用を加えるロボット１とを備えるロボットシステム１００を制御する機能をコンピュータに実現させるためのロボット制御プログラムであって、操作装置２へ加えられる操作力に応じてロボット１を動作させて対象物Ｗに作用を加えると共に、ロボット１が対象物Ｗから受ける反力に応じて操作装置２を動作させる力制御を実行する機能と、操作者Ｕの動力学特性を推定する機能と、力制御の周波数特性が目標周波数特性となるように、推定された操作者Ｕの動力学特性に応じて力制御の制御パラメータを調整する機能とをコンピュータに実現させる。

さらに、ロボットシステム１００は、操作者Ｕに操作される操作装置２と、対象物Ｗに作用を加えるロボット１と、制御装置３とを備える。

これらの構成によれば、制御パラメータが調整されることによって、力制御の周波数特性が目標周波数特性に近づく。この調整の際に、操作者Ｕの動力学特性の変化が考慮される。つまり、操作装置２には操作者Ｕから操作力が加えられるので、力制御における操作装置２の安定性及び応答性は、操作者Ｕの動力学特性の影響を受ける。操作者Ｕの動力学特性は、操作者Ｕの個人差及び姿勢等によって変化し得る。制御パラメータの調整に用いられる操作者Ｕの動力学特性は、一定ではなく、推定部３７によって推定される。推定された操作者Ｕの動力学特性は、操作者Ｕの個人差等の差異又は変化に対応して変化する。そのため、操作者Ｕの個人差及び姿勢等が考慮された周波数特性が目標周波数特性となるように、力制御の制御パラメータが調整される。その結果、操作者Ｕの変化等があっても、力制御の挙動を安定させることができる。

また、推定部３７は、操作装置２の動力学特性を推定し、調整部３１０は、周波数特性が目標周波数特性となるように、推定された操作者Ｕの動力学特性及び操作装置２の動力学特性に応じて制御パラメータを調整する。

この構成によれば、制御パラメータの調整の際に、操作者Ｕの動力学特性に加えて操作装置２の動力学特性が考慮される。つまり、力制御における操作装置２の安定性及び応答性は、操作装置２の動力学特性の影響を受ける。操作装置２の動力学特性は、操作装置２の機差及び状況等によって変化し得る。制御パラメータの調整に用いられる操作装置２の動力学特性は、一定ではなく、推定部３７によって推定される。推定された操作装置２の動力学特性は、操作装置２の差異又は変化等に対応して変化する。そのため、操作装置２の機差及び状況等が考慮された周波数特性が目標周波数特性となるように、力制御の制御パラメータが調整される。その結果、操作者Ｕ及び操作装置２の変化等があっても、力制御の挙動を安定させることができる。

さらに、推定部３７は、対象物Ｗの動力学特性を推定し、調整部３１０は、周波数特性が目標周波数特性となるように、推定された操作者Ｕの動力学特性及び対象物Ｗの動力学特性に応じて制御パラメータを調整する。

この構成によれば、制御パラメータの調整の際に、対象物Ｗの動力学特性の変化が考慮される。つまり、エンドエフェクタ１１は対象物Ｗに接触しているので、力制御におけるロボット１の安定性及び応答性は、対象物Ｗの動力学特性の影響を受ける。対象物Ｗの動力学特性は、対象物Ｗの個体差等によって変化し得る。制御パラメータの調整に用いられる対象物Ｗの動力学特性は、一定ではなく、推定部３７によって推定される。推定された対象物Ｗの動力学特性は、対象物Ｗの個体差等の差異又は変化に対応して変化する。そのため、対象物Ｗの個体差等が考慮された周波数特性が目標周波数特性となるように、力制御の制御パラメータが調整される。その結果、対象物Ｗ等の変化があっても、力制御の挙動を安定させることができる。

さらに、調整部３１０は、操作者Ｕの動力学特性に応じて調整された制御パラメータである第１制御パラメータと、対象物Ｗの動力学特性に応じて調整された制御パラメータである第２制御パラメータとを生成し、力制御部３６は、第１制御パラメータと第２制御パラメータとを使い分けて力制御を実行する。

この構成によれば、マスタ制御系７Ａの周波数特性を目標周波数特性とするための第１制御パラメータと、スレーブ制御系７Ｂの周波数特性を目標周波数特性とするための第２制御パラメータとが生成される。力制御部３６は、力制御を実行する際に第１制御パラメータと第２制御パラメータとを使い分けることによって、周波数特性を目標周波数特性に近づける制御系をマスタ制御系７Ａとスレーブ制御系７Ｂとで選択することができる。

さらにまた、力制御部３６は、ロボット１が対象物Ｗに接触している状態においては第２制御パラメータを用いて力制御を実行する一方、ロボット１が対象物Ｗに接触していない状態においては第１制御パラメータを用いて力制御を実行する。

この構成によれば、対象物Ｗへのロボット１の接触時には、スレーブ制御系７Ｂの周波数特性が目標周波数特性に調整される。操作者Ｕの機械インピーダンスに比べて対象物Ｗの機械インピーダンスの方が大きい。そのため、対象物Ｗの動力学特性の変化を考慮して力制御の周波数特性を調整することによって、操作者Ｕを含むマスタ制御系７Ａの周波数特性も安定化させることができる。一方、対象物Ｗへのロボット１の非接触時には、マスタ制御系７Ａの周波数特性が目標周波数特性に調整される。ロボット１の非接触状態においては、エンドエフェクタ１１が振動することはなく、振動の可能性があるのは操作装置２である。第１制御パラメータを用いることによって、操作装置２の動作を安定化させることができる。

また、推定部３７は、ロボット１の動力学特性を推定し、調整部３１０は、周波数特性が目標周波数特性となるように、推定された対象物Ｗの動力学特性及びロボット１の動力学特性に応じて第２制御パラメータを生成する。

この構成によれば、第２制御パラメータは、対象物Ｗの動力学特性だけでなく、ロボット１の動力学特性の変化も考慮されている。ロボット１は、ロボット１姿勢等の動作状況に応じて周波数応答が変化し得る。そのため、ロボット１の動力学特性の変化を考慮することによって、力制御における安定性及び応答性を調整することができる。

また、制御装置３は、目標周波数特性の入力を受け付けて、目標周波数特性を設定する設定部３９をさらに備える。

この構成によれば、目標周波数特性は、一定ではなく、変更され得る。目標周波数特性が設定部３９によって適宜設定されることによって、力制御の周波数特性を柔軟に調整することができる。

さらに、制御装置３は、力制御の周波数特性を求め、求められた周波数特性を提示する提示部３８をさらに備える。

この構成によれば、操作者Ｕは、現状の力制御の周波数特性を知ることができる。例えば、設定部３９が設けられている構成においては、操作者Ｕは、提示部３８によって提示された周波数特性を確認してから、目標周波数特性を入力することができる。

《その他の実施形態》
以上のように、本出願において開示する技術の例示として、前記実施形態を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、前記実施形態で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。また、添付図面および詳細な説明に記載された構成要素の中には、課題解決のために必須な構成要素だけでなく、前記技術を例示するために、課題解決のためには必須でない構成要素も含まれ得る。そのため、それらの必須ではない構成要素が添付図面や詳細な説明に記載されていることをもって、直ちに、それらの必須ではない構成要素が必須であるとの認定をするべきではない。

例えば、マスタ装置は、操作装置２に限定されず、操作者Ｕが操作力を加えることができる限り、任意の構成を採用することができる。スレーブ装置は、ロボット１に限定されず、対象物に接触した状態で作用を加える限り、任意の構成を採用することができる。

力制御部３６による力制御は、一例である。合成力ｆｍ＋ｆｓから指令速度ｘｄ’を導出する運動方程式は、式（１）に限定されない。運動方程式には、弾性係数が含まれていてもよい。

操作者モデル、対象物モデル、マスタモデル及びスレーブモデルは、一例である。各モデルは、対象の動力学特性を表す限り、任意のモデルとすることができる。例えば、マスタモデル又はスレーブモデルは、二次遅れ系のモデルであってもよい。動力学特性は、機械インピーダンス又は周波数応答（例えば、時定数）に限定されない。

推定部３７、提示部３８、設定部３９及び調整部３１０によって扱われる周波数特性は、マスタ制御系７Ａ及びスレーブ制御系７Ｂの両方の周波数特性に限定されない。例えば、マスタ制御系７Ａの周波数特性だけが対象となってもよい。

力制御部３６は、エンドエフェクタ１１の接触以外の要件で、第１制御パラメータと第２制御パラメータとを使い分けてもよい。例えば、力制御部３６は、第１制御パラメータ及び第２制御パラメータのうち、操作者Ｕ等からの入力によって決められた制御パラメータを用いて力制御を実行してもよい。

マスタ制御系７Ａの周波数特性に関し、操作装置２の動力学特性の変化を考慮しなくてもよい。つまり、操作装置２の周波数応答（即ち、時定数Ｔｃ）は一定とみなされ、操作者Ｕの動力学特性（即ち、剛性係数ｋｕ及び粘性係数ｃｕ）の変化だけが考慮されてもよい。

同様に、スレーブ制御系７Ｂの周波数特性に関し、ロボット１の動力学特性の変化を考慮しなくてもよい。つまり、ロボット１の周波数応答（即ち、時定数Ｔｒ）は一定とみなされ、対象物Ｗの動力学特性（即ち、剛性係数ｋｗ及び粘性係数ｃｗ）の変化だけが考慮されてもよい。

制御装置３において、力制御の周波数特性を提示する機能、即ち、提示部３８が省略されてもよい。また、周波数特性を提示する形式は、第１評価値及び第２評価値に限定されない。周波数特性として、ゲイン交差周波数又はゲイン余裕が提示されてもよい。あるいは、周波数特性として、速さ及び揺れにくさとは異なる、応答性又は安定性を表す評価値が提示されてもよい。

設定部３９が設定する目標周波数特性は、入力装置３４から入力されるものに限定されない。目標周波数特性は、外部装置から制御装置３へ入力されてもよい。また、制御装置３において、力制御の目標周波数特性を設定する機能、設定部３９が省略されてもよい。その場合、目標周波数特性は、予め設定されていてもよい。

調整部３１０が制御パラメータを調整する方法は、ＦＲＩＴを用いた方法に限定されない。目標周波数特性を達成するための制御パラメータを求めることができる限り、任意の方法が採用され得る。

前述のブロック図及びブロック線図は一例である。複数のブロックを１つのブロックとして実現したり、１つのブロックを複数のブロックに分割したり、一部の機能を別のブロックに移したりしてもよい。

前述のフローチャートは一例であり、ステップを省略又は変更してもよい。または、ステップの順番を変更したり、直列的な複数のステップを並列に処理したり、又は、並列的な複数のステップを直列的に処理したりしてもよい。

本開示の技術は、前記プログラムが記録された非一時的なコンピュータ読み取り可能な記録媒体であってもよい。また、前記プログラムは、インターネット等の伝送媒体を介して流通させることができるものであってもよい。

本明細書で開示する要素の機能は、開示された機能を実行するよう構成またはプログラムされた汎用プロセッサ、専用プロセッサ、集積回路、ＡＳＩＣ（Application Specific Integrated Circuits）、従来の回路、および/または、それらの組み合わせ、を含む回路または処理回路を使用して実行できる。プロセッサは、トランジスタやその他の回路を含むため、処理回路または回路と見なされる。本開示において、回路、ユニット、または手段は、列挙された機能を実行するハードウェアであるか、または、列挙された機能を実行するようにプログラムされたハードウェアである。ハードウェアは、本明細書に開示されているハードウェアであってもよいし、あるいは、列挙された機能を実行するようにプログラムまたは構成されているその他の既知のハードウェアであってもよい。ハードウェアが回路の一種と考えられるプロセッサである場合、回路、手段、またはユニットはハードウェアとソフトウェアの組み合わせであり、ソフトウェアはハードウェアおよび/またはプロセッサの構成に使用される。

１００ ロボットシステム
１ ロボット（スレーブ装置）
２ 操作装置（マスタ装置）
３ 制御装置
３６ 力制御部
３７ 推定部
３８ 提示部
３９ 設定部
３１０ 調整部
３２１ ロボット制御プログラム

Claims (11)

1. 操作者からマスタ装置へ加えられる操作力に応じてスレーブ装置を動作させて対象物に作用を加えると共に、前記スレーブ装置が前記対象物から受ける反力に応じて前記マスタ装置を動作させる力制御を実行する力制御部と、
   前記操作者の動力学特性を推定する推定部と、
   前記力制御の周波数特性が目標周波数特性となるように、推定された前記操作者の動力学特性に応じて前記力制御の制御パラメータを調整する調整部とを備える制御装置。
2. 請求項１に記載の制御装置において、
   前記推定部は、前記マスタ装置の動力学特性を推定し、
   前記調整部は、前記周波数特性が前記目標周波数特性となるように、推定された前記操作者の動力学特性及び前記マスタ装置の動力学特性に応じて前記制御パラメータを調整する制御装置。
3. 請求項１又は２に記載の制御装置において、
   前記推定部は、前記対象物の動力学特性を推定し、
   前記調整部は、前記周波数特性が前記目標周波数特性となるように、推定された前記操作者の動力学特性及び前記対象物の動力学特性に応じて前記制御パラメータを調整する制御装置。
4. 請求項３に記載の制御装置において、
   前記調整部は、前記操作者の動力学特性に応じて調整された前記制御パラメータである第１制御パラメータと、前記対象物の動力学特性に応じて調整された前記制御パラメータである第２制御パラメータとを生成し、
   前記力制御部は、前記第１制御パラメータと前記第２制御パラメータとを使い分けて前記力制御を実行する制御装置。
5. 請求項４に記載の制御装置において、
   前記力制御部は、前記スレーブ装置が前記対象物に接触している状態においては前記第２制御パラメータを用いて前記力制御を実行する一方、前記スレーブ装置が前記対象物に接触していない状態においては前記第１制御パラメータを用いて前記力制御を実行する制御装置。
6. 請求項４又は５に記載の制御装置において、
   前記推定部は、前記スレーブ装置の動力学特性を推定し、
   前記調整部は、前記周波数特性が前記目標周波数特性となるように、推定された前記対象物の動力学特性及び前記スレーブ装置の動力学特性に応じて前記第２制御パラメータを生成する制御装置。
7. 請求項１乃至６の何れか１つに記載の制御装置において、
   前記目標周波数特性の入力を受け付けて、前記目標周波数特性を設定する設定部をさらに備える制御装置。
8. 請求項１乃至７の何れか１つに記載の制御装置において、
   前記力制御の周波数特性を求め、求められた前記周波数特性を提示する提示部をさらに備える制御装置。
9. 前記操作者に操作される前記マスタ装置と、
   前記対象物に作用を加える前記スレーブ装置と、
   請求項１乃至８の何れか１つに記載の制御装置とを備えるロボットシステム。
10. 操作者からマスタ装置へ加えられる操作力に応じてスレーブ装置を動作させて対象物に作用を加えると共に、前記スレーブ装置が前記対象物から受ける反力に応じて前記マスタ装置を動作させる力制御を実行することと、
    前記操作者の動力学特性を推定することと、
    前記力制御の周波数特性が目標周波数特性となるように、推定された前記操作者の動力学特性に応じて前記力制御の制御パラメータを調整することとを含むロボット制御方法。
11. 操作者から操作力が加えられるマスタ装置と対象物に作用を加えるスレーブ装置とを備えるロボットシステムを制御する機能をコンピュータに実現させるためのロボット制御プログラムであって、
    前記マスタ装置へ加えられる操作力に応じて前記スレーブ装置を動作させて対象物に作用を加えると共に、前記スレーブ装置が前記対象物から受ける反力に応じて前記マスタ装置を動作させる力制御を実行する機能と、
    前記操作者の動力学特性を推定する機能と、
    前記力制御の周波数特性が目標周波数特性となるように、推定された前記操作者の動力学特性に応じて前記力制御の制御パラメータを調整する機能とをコンピュータに実現させるためのロボット制御プログラム。

Images