JP6390669B2

JP6390669B2 - ロボット制御システム、ロボットシステム

Info

Publication number: JP6390669B2
Application number: JP2016127317A
Authority: JP
Inventors: 稲積　満広; 満広稲積
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2016-06-28
Filing date: 2016-06-28
Publication date: 2018-09-19
Anticipated expiration: 2031-11-07
Also published as: JP2016168671A

Description

本発明は、ロボット制御システム、ロボットシステム及びセンサー情報処理装置等に関する。

マニピュレーター等のロボットを用いて行う作業において、種々の拘束条件、例えば物体との接触を伴う作業がある。このような場合においては、位置の制御に加えて、力の制御が要求される場合が多い。例えば、物体の表面をなぞる場合、１つの物体を他の物体に嵌め合わせる場合、柔軟物を破壊しないように把持する場合などにおいては、単なる位置の制御に加えて、物体からの反力に対応した動きが必要となる。

ロボットにおいて力制御を行う代表的な手法としては、インピーダンス制御と呼ばれる手法がある。インピーダンス制御とは、ロボットを、その実際の質量や粘性特性や弾性特性に関わらず、あたかも作業に適したそれらの値を持つかのように動作させる制御手法である。これは、ロボットに取り付けられた力覚センサーなどから得られる力情報に基づいて運動方程式を解き、ロボットをその解に従って動作させる制御手法である。この運動方程式を適切に設定することにより、マニピュレーター等のロボットを、あたかも所定の質量、粘性、弾性を持っているかのごとく動作させることが可能になる。このようなインピーダンス制御や力制御に関する従来技術としては、特許文献１、２、３に開示される技術が知られている。

特開平６−３２０４５１号公報特開平２−２０５４８９号公報特開平４−３６９００４号公報

特許文献１には、制御対象が柔軟である場合のような、非常に複雑な系においても、精度良くインピーダンス制御を行う手法が開示されている。

特許文献２には、力覚センサーを用いずに、制御系内部に運動モデルを持つことにより力を推定し、制御系は複雑になるもののセンサーは不要としつつ、かつ、より単純な機械的な構造により、インピーダンス制御を行う手法が開示されている。

一方、特許文献３には、演算量を削減するための１つの手法が開示されている。これは、上記の特許文献１、２に記載される逆ヤコビアン行列を求める計算を行わずに、転置ヤコビアン行列を用いることにより、計算量を削減するものである。

インピーダンス制御においては、ロボット等に所望の特性（質量・粘性特性・弾性特性）を持つかのように振る舞わせるために、当該特性に対応する係数パラメーターを用いた常微分方程式（２階の線形常微分方程式である運動方程式）を解く必要がある。常微分方程式を解く手法は種々知られているが、Ｒｕｎｇｅ−Ｋｕｔｔａ法やＮｅｗｔｏｎ法等が用いられる。

しかし、これらの手法はハードウェアー化に適さず、安定性の判定も難しい。さらに応答性の切り替えに対応することが困難であるという問題もある。ここで、応答性とはロボットに与える所望の特性（質量・粘性特性・弾性特性）により定まる動的な特性のことであり、応答性が低いとは、例えば大きな質量係数、大きな粘性係数、小さな弾性係数によって実現されるような特性を指し、応答性が高いとは、例えば小さな質量係数、小さな粘性係数、大きな弾性係数によって実現されるような特性を指す。これらは実現した作業内容、あるいは作業の場面に依存し、最適な特性が異なる。従って、容易に応答特性を切り替えることができることは非常に重要な機能である。

本発明の幾つかの態様によれば、ハードウェアー化や安定性の検証や解の応答性の切り替えを容易にするロボット制御システム、ロボットシステム及びセンサー情報処理装置等を提供することができる。

本発明の一態様は、力覚センサーからのセンサー情報に基づいて、ロボットの目標軌道の補正値を出力する力制御部と、前記目標軌道に対して前記補正値に基づく補正処理を行なって目標値を求め、求めた前記目標値を出力する目標値出力部と、前記目標値に基づいて、前記ロボットのフィードバック制御を行うロボット制御部と、を含み、前記力制御部は、デジタルフィルター部を有し、前記力制御部は、前記センサー情報に対して前記デジタルフィルター部によるデジタルフィルター処理を行うことで、力制御における常微分方程式の解を求めて、求めた前記解に基づいて前記補正値を出力するロボット制御システムに関係する。

本発明の一態様では、ロボット制御システムは、力覚センサーからのセンサー情報に基づいて力制御を行って補正値を求め、補正値に基づいて得られた目標値を用いてロボットのフィードバック制御を行う。力制御において、常微分方程式の解を求める処理をデジタルフィルター処理により行って補正値を出力することにより、当該処理のハードウェアー化や安定性の判定や応答性の切り替えを容易にすること等が可能になる。

また、本発明の一態様では、前記デジタルフィルター部は、前記常微分方程式の各項の係数パラメーターに基づいてデジタルフィルター係数を出力するデジタルフィルター係数出力部と、前記デジタルフィルター係数及び前記センサー情報に基づいて前記デジタルフィルター処理を行うデジタルフィルター演算部と、を含んでもよい。

これにより、常微分方程式の各項の係数パラメーターをデジタルフィルター係数に対応させて、デジタルフィルター処理を行うこと等が可能になる。

また、本発明の一態様では、前記デジタルフィルター係数出力部は、前記係数パラメーターを前記デジタルフィルター係数に変換する処理を行うデジタルフィルター係数変換部と、前記デジタルフィルター係数変換部により変換された前記デジタルフィルター係数を記憶するデジタルフィルター係数記憶部と、を含み、前記デジタルフィルター係数出力部は、前記デジタルフィルター係数記憶部に記憶された前記デジタルフィルター係数を出力してもよい。

これにより、常微分方程式の各項の係数パラメーターをデジタルフィルター係数に変換することが可能になる。

また、本発明の一態様では、前記デジタルフィルター部は、前記デジタルフィルター処理に用いられるデジタルフィルターの動作の安定度を判定するデジタルフィルター安定度判定部を含み、前記デジタルフィルター係数出力部は、前記デジタルフィルター安定度判定部において、前記デジタルフィルターの前記動作が安定であると判定された場合に、前記デジタルフィルター係数を出力してもよい。

これにより、デジタルフィルター処理の安定性の判定を行うことで、常微分方程式に関する系の安定性を判定すること等が可能になる。

また、本発明の一態様では、第１の駆動周波数及び前記第１の駆動周波数に比べて高い周波数である第２の駆動周波数のいずれかの駆動周波数で、前記デジタルフィルター処理が行われる場合に、前記デジタルフィルター係数出力部は、第１の駆動周波数に対応する第１のデジタルフィルター係数及び第２の駆動周波数に対応する第２のデジタルフィルター係数の少なくとも一方を出力し、前記デジタルフィルター演算部は、前記デジタルフィルター係数出力部から出力された前記デジタルフィルター係数に応じて、前記第１のデジタルフィルター係数を用いた前記第１の駆動周波数による前記デジタルフィルター処理及び前記第２のデジタルフィルター係数を用いた前記第２の駆動周波数による前記デジタルフィルター処理の少なくとも一方の処理を行ってもよい。

これにより、デジタルフィルター処理の駆動周波数を切り替えること等が可能になる。駆動周波数を切り替えることで、解の応答性を変更することができる他、ノイズ低減用の帯域制限処理等による応答特性の変化に対応すること等が可能になる。

また、本発明の一態様では、前記デジタルフィルター部は、前記センサー情報の信号周波数帯域情報に基づいて、前記第１の駆動周波数及び前記第２の駆動周波数のいずれか一方の前記駆動周波数を選択する選択処理部を含み、前記デジタルフィルター部は、前記選択処理部において選択された前記駆動周波数で前記デジタルフィルター処理を行って、前記補正値を出力してもよい。

これにより、センサー情報の信号周波数帯域情報に基づいて、自動的に駆動周波数を選択すること等が可能になる。

また、本発明の一態様では、前記選択処理部は、前記センサー情報に対して低域通過フィルター処理を施した低域センサー情報と、前記センサー情報に対して高域通過フィルター処理を施した高域センサー情報とを前記信号周波数帯域情報として、前記駆動周波数を選択してもよい。

これにより、信号周波数帯域情報として、低域センサー情報と高域センサー情報を用いることが可能になる。

また、本発明の一態様では、前記デジタルフィルター処理の前記駆動周波数を、高周波数の前記第２の駆動周波数から、低周波数の前記第１の駆動周波数に切り替える場合に、前記デジタルフィルター演算部は、第１の期間では、前記第２のフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理を行い、前記第１の期間の次の切り替え期間では、前記第１のフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理及び前記第２のフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理の両方を行い、前記切り替え期間の次の第２の期間では、前記第１のフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理を行ってもよい。

これにより、駆動周波数の高周波から低周波への切り替えをスムーズに行うこと等が可能になる。

また、本発明の一態様では、前記デジタルフィルター処理の前記駆動周波数を、低周波数の前記第１の駆動周波数から、高周波数の前記第２の駆動周波数に切り替える場合に、前記デジタルフィルター演算部は、切り替えタイミングにおける前記第１のフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理の出力値である第１の出力値、及び前記切り替えタイミングよりも前のタイミングである過去タイミングにおける前記第１のフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理の出力値である第２の出力値に基づいて、前記切り替えタイミングと前記過去タイミングとの間のタイミングにおける補間値を求める補間処理を行い、前記補間値に基づいて、前記第２のフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理を行ってもよい。

これにより、駆動周波数の低周波から高周波への切り替えをスムーズに行うこと等が可能になる。

また、本発明の一態様では、前記デジタルフィルター部は、前記デジタルフィルター演算部として、第１のデジタルフィルター演算部と、第２のデジタルフィルター演算部とを含み、前記デジタルフィルター係数出力部は、前記第１のデジタルフィルター演算部に第１のデジタルフィルター係数を出力するとともに、前記第２のデジタルフィルター演算部に前記第１のデジタルフィルター係数とは異なる第２のデジタルフィルター係数を出力し、前記第１のデジタルフィルター演算部は、前記第１のデジタルフィルター係数及び前記センサー情報に基づく前記デジタルフィルター処理を行い、前記第２のデジタルフィルター演算部は、前記第２のデジタルフィルター係数及び前記センサー情報に基づく前記デジタルフィルター処理を行ってもよい。

これにより、複数のデジタルフィルター演算部を並行動作させることが可能になり、応答特性の切り替えをスムーズにすること等が可能になる。

また、本発明の一態様では、前記デジタルフィルター部は、前記第１のデジタルフィルター演算部における処理結果と、前記第２のデジタルフィルター演算部における処理結果のうち、いずれか１つを選択し、前記補正値として出力するデジタルフィルター出力選択部を含んでもよい。

これにより、複数のデジタルフィルター処理を並行動作させた場合にも、適切な出力値を選択することが可能になる。

また、本発明の一態様では、第１の駆動周波数及び前記第１の駆動周波数に比べて高い周波数である第２の駆動周波数のいずれかの駆動周波数で、前記デジタルフィルター処理が行われる場合に、前記デジタルフィルター係数出力部は、前記第１の駆動周波数に対応するデジタルフィルター係数を前記第１のデジタルフィルター係数として出力するとともに、前記第２の駆動周波数に対応するデジタルフィルター係数を前記第２のデジタルフィルター係数として出力し、前記第１のデジタルフィルター演算部は、前記第１のデジタルフィルター係数に基づいて、前記第１の駆動周波数で前記デジタルフィルター処理を行い、前記第２のデジタルフィルター演算部は、前記第２のデジタルフィルター係数に基づいて、前記第２の駆動周波数で前記デジタルフィルター処理を行ってもよい。

これにより、デジタルフィルター処理の駆動周波数を切り替えること等が可能になる。

また、本発明の一態様では、前記力制御部は、前記センサー情報に対して所与の周波数帯域を通過させる帯域制限処理を行い、前記帯域制限処理を行った前記センサー情報を前記第１のデジタルフィルター演算部、前記第２のデジタルフィルター演算部に出力する帯域制限部を含み、前記帯域制限部は、前記第１のデジタルフィルター演算部に対しては、第１の周波数帯域を通過させる帯域制限処理を行った前記センサー情報を出力するとともに、前記第２のデジタルフィルター演算部に対しては、第１の周波数帯域よりも広い周波数帯域である第２の周波数帯域を通過させる帯域制限処理を行った前記センサー情報を出力してもよい。

これにより、センサー情報に対して、駆動周波数に対応させて帯域制限処理を行うことが可能になる。

また、本発明の一態様では、前記デジタルフィルター係数出力部は、第１のデジタルフィルター係数と、前記第１のデジタルフィルター係数とは異なる第２のフィルター係数のうち、いずれか１つを選択する係数選択部を含み、前記デジタルフィルター係数出力部は、前記係数選択部において選択された前記デジタルフィルター係数を前記デジタルフィルター演算部に出力してもよい。

これにより、複数のデジタルフィルター係数から１つを選択することができ、デジタルフィルター処理をスムーズに切り替えること等が可能になる。

また、本発明の一態様では、第１の駆動周波数及び前記第１の駆動周波数に比べて高い周波数である第２の駆動周波数のいずれかの駆動周波数で、前記デジタルフィルター処理が行われる場合に、前記係数選択部は、前記第１の駆動周波数に対応する前記第１のデジタルフィルター係数と、前記第２の駆動周波数に対応する前記第２のデジタルフィルター係数のうち、いずれか１つを選択し、前記デジタルフィルター係数出力部は、前記係数選択部において選択された前記デジタルフィルター係数を前記デジタルフィルター演算部に出力し、前記デジタルフィルター演算部は、前記デジタルフィルター係数出力部から前記第１のデジタルフィルター係数が出力された場合には、前記第１のデジタルフィルター係数に基づいて、前記第１の駆動周波数で前記デジタルフィルター処理を行い、前記デジタルフィルター係数出力部から前記第２のデジタルフィルター係数が出力された場合には、前記第２のデジタルフィルター係数に基づいて、前記第２の駆動周波数で前記デジタルフィルター処理を行ってもよい。

また、本発明の一態様では、前記力制御部は、前記センサー情報に対して所与の周波数帯域を通過させる帯域制限処理を行い、前記帯域制限処理を行った前記センサー情報を前記デジタルフィルター演算部に出力する帯域制限部を含み、前記帯域制限部は、前記第１の駆動周波数による前記デジタルフィルター処理が行われる場合には、第１の周波数帯域を通過させる帯域制限処理を行った前記センサー情報を前記デジタルフィルター演算部に出力し、前記第２の駆動周波数による前記デジタルフィルター処理が行われる場合には、前記第１の周波数帯域よりも広い周波数帯域である第２の周波数帯域を通過させる帯域制限処理を行った前記センサー情報を前記デジタルフィルター演算部に出力してもよい。

また、本発明の一態様では、前記常微分方程式は、仮想質量項、仮想粘性項及び仮想弾性項を係数パラメーターとする運動方程式であってもよい。

これにより、常微分方程式として運動方程式を解いて補正値を出力することが可能になる。

また、本発明の一態様では、前記目標値出力部は、前記ロボットの目標位置情報を出力する軌道生成部と、前記軌道生成部からの前記目標位置情報に基づいてインバースキネマティクス処理を行い、前記ロボットの関節角情報を出力するインバースキネマティクス処理部と、を含んでもよい。

これにより、目標位置情報を求めた上で、関節角情報に変換して、変換した関節角情報に基づいてロボットのフィードバック制御の目標値を求めることが可能になる。

また、本発明の一態様では、前記力制御部は、前記センサー情報に対して前記デジタルフィルター部によるデジタルフィルター処理を行うことで、前記力制御における前記常微分方程式の解を求めて、求めた前記解を前記補正値として出力してもよい。

これにより、常微分方程式の解をそのまま補正値として出力することが可能になる。

また、本発明の一態様では、前記力制御部は、前記センサー情報に対して前記デジタルフィルター部によるデジタルフィルター処理を行うことで、前記力制御における前記常微分方程式の解を求めて、求めた前記解に対してインバースキネマティクス処理を施して前記補正値を出力してもよい。

これにより、常微分方程式の解に対してインバースキネマティクス処理を施すことで補正値を求め、求めた補正値を出力することが可能になる。

また、本発明の他の態様は、上記のいずれかに記載のロボット制御システムと、前記ロボットと、を含むロボットシステムに関係する。

また、本発明の他の態様は、センサーからのセンサー情報を取得するセンサー情報取得部と、取得された前記センサー情報に対して処理を行うセンサー情報処理部と、を含み、前記センサー情報処理部は、デジタルフィルター部を有し、前記センサー情報処理部は、前記センサー情報に対して前記デジタルフィルター部によるデジタルフィルター処理を行うことで、センサー情報処理における常微分方程式の解を求め、前記センサー情報処理の出力値として出力するセンサー情報処理装置に関係する。

ロボット制御システム及びロボットシステムの基本構成例。ロボットシステムの一例。図３（Ａ）〜図３（Ｃ）は力制御についての説明図。図４（Ａ）、図４（Ｂ）はコンプライアンス制御についての説明図。図５（Ａ）、図５（Ｂ）はインピーダンス制御についての説明図。力覚フィードバックを含まない場合の制御系の基本構成例。力覚フィードバックを含む場合の制御系の基本構成例。運動方程式の解を求める際のデジタルフィルターの基本形。図９（Ａ）〜図９（Ｃ）は系の安定性判定手法の説明図。デジタルフィルターを用いたロボット制御システム及びロボットシステムの基本構成例。第１の実施形態のロボット制御システム及びロボットシステムの構成例。第１の実施形態の処理を説明するためのフローチャート。第２の実施形態のロボット制御システム及びロボットシステムの構成例。第２の実施形態の処理を説明するためのフローチャート。第３の実施形態のロボット制御システム及びロボットシステムの構成例。高周波から低周波への駆動周波数切り替え処理の説明図。高周波から低周波への駆動周波数切り替え処理を説明するためのフローチャート。低周波から高周波への駆動周波数切り替え処理の説明図。低周波から高周波への駆動周波数切り替え処理を説明するためのフローチャート。第４の実施形態のロボット制御システム及びロボットシステムの構成例。図２１（Ａ）、図２１（Ｂ）は目標軌道、補正値、目標値を求める具体的なシステム構成例。

以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．概要
１．１基本構成
本実施形態のロボット制御システム（マニピュレーター制御システム）及びこれを含むロボットシステムの構成例を図１に示す。なお本実施形態のロボット制御システム、ロボットシステムは図１の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

本実施形態のロボット制御システムは、力制御部２０、目標値出力部６０、ロボット制御部８０を含む。また本実施形態のロボットシステムは、このロボット制御システムと、ロボット１００（力覚センサー１０）により構成される。

目標値出力部６０は、ロボット（狭義にはマニピュレーター）のフィードバック制御の目標値を出力する。この目標値に基づいてロボット１００のフィードバック制御が実現される。多関節ロボット等を例に取れば、この目標値は、ロボットの関節角情報などである。ロボットの関節角情報は、例えばロボットのアームのリンク機構における各関節の角度（ジョイント軸とジョイント軸のなす角度）を表す情報である。

目標値出力部６０は、軌道生成部６２とインバースキネマティクス処理部６４を含むことができる。軌道生成部６２は、ロボットの軌道情報を出力する。軌道情報は、ロボットのエンドエフェクター部（エンドポイント）の位置情報（ｘ，ｙ，ｚ）と、各座標軸回りでの回転角度情報（ｕ，ｖ，ｗ）を含むことができる。インバースキネマティクス処理部６４は、軌道生成部６２からの軌道情報に基づいてインバースキネマティクス処理を行い、例えばロボットの関節角情報を目標値として出力する。インバースキネマティクス処理は、関節を有するロボットの動きを計算する処理であり、ロボットのエンドエフェクター部の位置姿勢などから関節角情報等を逆運動学により計算する処理である。

力制御部２０（狭義にはインピーダンス制御部）は、力覚センサー１０からのセンサー情報に基づいて力制御（力覚制御）を行って、目標値の補正値を出力する。更に具体的には、力制御部２０（インピーダンス制御部）は、力覚センサー１０からのセンサー情報（力情報、モーメント情報）に基づいてインピーダンス制御（或いはコンプライアンス制御）を行う。力制御は、例えば、従来の位置制御に対して、力のフィードバックが加わった制御である。インピーダンス制御は、外力に対するエンドエフェクター部（手先）の変位の生じやすさ（機械インピーダンス）を、制御により望ましい状態にする手法である。具体的には、ロボットのエンドエフェクター部に質量と粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した質量と粘性係数と弾性係数で物体に接触するようにする制御である。また力覚センサー１０は、ロボット１００が出している力の反力として受けている力やモーメントを検出するセンサーである。この力覚センサー１０は、通常、ロボット１００のアームの手首部分に取り付けられ、検出された力やモーメントは、センサー情報として、各種の力制御（インピーダンス制御）に用いられる。

ロボット制御部８０は、目標値出力部６０からの目標値に基づいて、ロボットのフィードバック制御を行う。具体的には、力制御部２０からの補正値に基づく補正処理の結果出力された目標値に基づいて、ロボットのフィードバック制御を行う。例えば目標値と、ロボット１００からのフィードバック信号に基づいて、ロボット１００のフィードバック制御を行う。例えばロボット制御部８０は、複数の駆動制御部８２-1〜８２-N（狭義にはモーター制御部）を含み、ロボット１００が有する駆動部１０２-1〜１０２-Nに対して、その制御信号を出力する。ここで駆動部１０２-1〜１０２-Nは、ロボット１００の各関節を動かすための駆動機構であり、例えばモーター等により実現される。

図２に本実施形態のロボット制御システムを含むロボットシステムの例を示す。このロボットシステムは、制御装置３００（情報処理装置）とロボット３１０（図１のロボット１００）とを含む。制御装置３００はロボット３１０の制御処理を行う。具体的には、動作シーケンス情報（シナリオ情報）に基づいてロボット３１０を動作させる制御を行う。ロボット３１０は、アーム３２０及びハンド（把持部）３３０を有する。そして制御装置３００からの動作指示にしたがって動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動するなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボットの姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。

本実施形態のロボット制御システムは例えば図２の制御装置３００に設けられ、例えば制御装置３００のハードウェアーやプログラムによりロボット制御システムが実現される。そして本実施形態のロボット制御システムによれば、制御装置３００などの制御ハードウェアーに対する性能要求を低減できると共に、ロボット３１０を高い応答性で動作させることが可能になる。なお図２は単腕型の例であるが、双腕型等の多腕型のロボットであってもよい。

１．２力制御・インピーダンス制御
次に、力制御、インピーダンス制御（コンプライアンス制御）の概要について説明する。

図３（Ａ）は、ロボットの左のアームＡＬ、右のアームＡＲで物体ＯＢを挟んで移動している様子を示している。例えば、位置制御だけでは、物体を落としたり、破壊してしまうおそれがある、力制御によれば、柔軟な物体や脆い物体を、図３（Ａ）のように両側から適切な力で挟んで移動させることが可能になる。

また、力制御によれば、図３（Ｂ）に示すように、不確実性のある物体の表面ＳＦをアームＡＭ等でなぞることが可能になる。このような制御は位置制御だけでは実現不能である。また、力制御によれば、図３（Ｃ）に示すように、粗い位置決めの後に、探って位置合わせをして、物体ＯＢを穴部ＨＬにはめ込むことも可能になる。

しかしながら、バネなどの実際の機械部品による力制御では、用途が限られるという問題がある。また、このような機械部品による力制御では、特性の動的な切り替えが困難である。

一方、モーターのトルクを制御するトルク制御は簡単であるが、位置精度が悪くなるという問題がある。また、異常時に衝突などの問題が生じる。例えば図３（Ａ）において、異常事態が起きて、物体ＯＢを落としてしまった場合に、トルク制御では、釣合うべき反力が無くなるため、左右のアームＡＬ、ＡＲが衝突してしまうなどの問題が生じる。

これに対して、インピーダンス制御（コンプライアンス制御）は、制御が複雑であるという不利点があるものの、汎用性や安全性が高いという利点がある。

図４（Ａ）、図４（Ｂ）は、インピーダンス制御の１つであるコンプライアンス制御を説明する図である。コンプライアンスはバネ定数の逆数を意味し、バネ定数が硬さを表すのに対して、コンプライアンスは柔らかさを意味する。ロボットと環境との間に相互作用が働くときに、機械的柔軟性であるコンプライアンスを与える制御をコンプライアンス制御と呼ぶ。

例えば図４（Ａ）では、ロボットのアームＡＭには力覚センサーＳＥが取り付けられている。このロボットのアームＡＭは、力覚センサーＳＥで得られたセンサー情報（力・トルク情報）に応じて姿勢が変わるようにプログラムされている。具体的には、図４（Ａ）のＡ１に示す仮想的なバネが、あたかもアームＡＭの先端に取り付けられているかのようにロボットを制御する。

例えばＡ１に示すバネのバネ定数が１００Ｋｇ／ｍであったとする。これを図４（Ｂ）のＡ２に示すように５Ｋｇの力で押せば、Ａ３に示すようにバネは５ｃｍだけ縮む。逆に言えば、５ｃｍだけ縮んでいれば、５Ｋｇの力で押されているといえる。つまり、力情報と位置情報とが対応づけられている。

コンプライアンス制御では、このＡ１に示す仮想的なバネがアームＡＭの先端に取り付けられているかのような制御が行われる。具体的には、ロボットは、力覚センサーＳＥの入力に応答して動作し、Ａ２に示す５Ｋｇの加重に対して、Ａ３に示すように５ｃｍだけ後退するように制御され、力情報に対応して位置情報が変化するように制御される。

このような単純なコンプライアンス制御では時間項を含まないが、時間項を含み、その２次の項までを考慮した制御が、インピーダンス制御である。具体的には、２次の項は質量項であり、１次の項は粘性項であり、インピーダンス制御のモデルは下式（１）に示すような運動方程式で表すことができる。

上式（１）において、ｍは質量、μは粘性係数、ｋは弾性係数、ｆは力、ｘは目標位置からの変位である。またｘの１次微分、２次微分は、各々、速度、加速度に対応する。インピーダンス制御では、上式（１）の特性をアームの先端であるエンドエフェクター部に持たせるための制御系を構成する。即ち上式（１）で表される仮想質量、仮想粘性係数、仮想弾性係数を、あたかもアームの先端が持っているかのように制御を行う。

このように、インピーダンス制御は、アームの先端の質量に粘性要素と弾性要素が各方向に接続されるモデルにおいて、目的として設定された粘性係数と弾性係数で物体に接触するようにする制御である。

例えば図５（Ａ）に示すように、ロボットのアームＡＬ、ＡＲで物体ＯＢをつかんで、軌道ＴＲに沿って移動させる制御を考える。この場合に、軌道ＴＲＬは、物体ＯＢの左側の内側に設定された点ＰＬが通る軌道であり、インピーダンス制御を想定して決定された仮想的な左手の軌道である。また軌道ＴＲＲは、物体ＯＢの右側の内側に設定された点ＰＲが通る軌道であり、インピーダンス制御を想定して決定された仮想的な右手の軌道である。この場合に、アームＡＬは、アームＡＬの先端と点ＰＬの距離差に応じた力が発生するように制御される。またアームＡＲは、アームＡＲの先端と点ＰＲの距離差に応じた力が発生するように制御される。このようにすれば、物体ＯＢを柔らかくつかみながら移動させるインピーダンス制御を実現できる。そして、インピーダンス制御では、図５（Ａ）のＢ１に示すように物体ＯＢが落下する事態が生じたとしても、アームＡＬ、ＡＲは、Ｂ２、Ｂ３に示すように、その先端が点ＰＬ、ＰＲの位置で止まるように制御される。即ち、仮想的な軌道が衝突軌道でなければ、アームＡＬ、ＡＲが衝突するのを防止できる。

また図５（Ｂ）のように、物体の表面ＳＦをなぞるように制御する場合にも、インピーダンス制御では、アームＡＭの先端に対して、仮想的な軌道ＴＲＶＡと先端との距離差ＤＦに応じた力が働くように制御される。従って、アームＡＭを、力を加えながら表面ＳＦをなぞるような制御が可能になる。

１．３制御系の構成
図６に、力覚フィードバックを含まない場合の制御系の基本構成例を示す。

軌道生成部５６２は、軌道情報ｐ（xyzuvw）を生成してインバースキネマティクス処理部５６４に出力する。ここで軌道情報ｐは、例えばアームの先端（エンドエフェクター部）の位置情報(xyz)と各軸回りの回転情報（uvw）を含む。そしてインバースキネマティクス処理部５６４は、この軌道情報ｐに基づいてインバースキネマティクス処理を行って、目標値である各関節の関節角θを生成して出力する。そして、この関節角θに基づいて、モーター制御を行うことで、ロボットのアームの動作制御が行われる。この場合に図６のモーター（Ｍ）の制御は、公知のＰＩＤ制御により実現されている。このＰＩＤ制御は公知の技術であるため、ここでは詳細な説明を省略する。

図６において、軌道生成部５６２とインバースキネマティクス処理部５６４とにより目標値出力部が構成される。この目標値出力部の処理は、ロボットの全体的な処理になる。一方、後段のモーター制御は、関節毎の制御になる。

図７に、力覚フィードバックを含む場合の制御系の基本構成例を示す。図７では、図６に対して、更に、力覚センサー５１０と、姿勢補正部５３２と、ハンド・ツール自重補正部５３４と、運動方程式処理部５３６と、フォワードキネマティクス処理部５４０が設けられている。

図７では、力覚センサー５１０からのセンサー情報を受けて、姿勢補正部５３２がセンサーの姿勢補正を行い、ハンド・ツール自重補正部５３４がハンド・ツール自重補正を行う。そして、運動方程式処理部５３６が、前述の式（１）に示すような運動方程式の解を求める処理を行い、補正値Δｐを出力する。この補正値Δｐにより軌道情報ｐが補正されることで、目標値である関節角θの補正処理が行われる。またフォワードキネマティクス処理部５４０は、フォワードキネマティクス処理を行って、ロボットの軌道情報ｐ’を求めて軌道生成部５６２にフィードバックする。また姿勢補正部５３２、ハンド・ツール自重補正部５３４に対して姿勢を特定するための情報を出力する。なお、ロボットの軌道情報ｐ’の軌道生成部５６２へのフィードバックは、ｐ’に基づいた軌道の修正処理等を行うためのものであり、当該修正処理等を行わないのであれば、フィードバックは必ずしも必要ない。

ハンド・ツール自重補正部５３４では、ハンド・ツール自重補正が行われ、姿勢補正部５３２では姿勢補正が行われる。ここでハンド・ツール自重補正は、ロボットのハンドの自重や、ハンドがつかむツールの自重による影響を、力覚センサー１０からのセンサー情報（力情報）から相殺するための補正処理である。また姿勢補正は、力覚センサー１０の姿勢による影響を、センサー情報（力情報）から相殺するための補正処理である。これらのハンド・ツール自重補正及び姿勢補正は、例えば下式（２）のように表すことができる。

上式（２）において、Ｆｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗは力覚センサー１０からのセンサー情報である力情報、トルク情報である。またＢｘ，Ｂｙ，Ｂｚ、Ｂｕ，Ｂｖ，Ｂｗはバイアス項である。そして、補正後のセンサー情報（力情報、トルク情報）であるｆｘ，ｆｙ，ｆｚ、ｆｕ，ｆｖ，ｆｗが運動方程式処理部５３６に入力される。なお、データには固定値があるため、実質的な補正係数は６×７＝４２個となる。これらのハンド・ツール自重補正及び姿勢補正は公知の補正処理であるため、詳しい説明は省略する。

１．４デジタルフィルター処理
図７の運動方程式処理部５３６では運動方程式（広義には常微分方程式）の解を求める必要がある。従来、運動方程式の解を求めるにはＮｅｗｔｏｎ法やＲｕｎｇｅ−Ｋｕｔｔａ法等が用いられていた。しかし、これらの手法はハードウェアー化に適さず、安定性の判定も難しい。さらに応答性の切り替えに対応することが困難であるという問題もある。

そこで本出願人は、上述の３つの問題に対応するために、常微分方程式を解く手法としてデジタルフィルターを用いる。

１．４．１運動方程式のデジタルフィルターを用いた解法
運動方程式は上述した式（１）の形で表される。運動方程式は線形常微分方程式であるため、インパルス入力に対する解であるインパルスレスポンスが求められれば、インパルスレスポンスと外力項との畳み込みにより、任意の外力項に対する解を得ることができる。

ここで、運動方程式の解を求めるというステップを、力覚センサーのセンサー情報の入力に対して解（例えば位置情報）を出力するフィルターであると捉えるならば、上式（１）の形から、２極のアナログフィルターとして考えることができる。

つまり、運動方程式の解はアナログフィルターの出力として求めることができるのであるから、当該アナログフィルターをデジタルフィルター化することで、運動方程式をデジタルフィルターを用いて解くことが可能になる。

アナログフィルターのデジタルフィルター化の手法は種々知られているが、例えばImpulse Invariance法を用いればよい。これは、アナログフィルターのインパルスレスポンスを離散時間Ｔでサンプルした値と同じインパルスレスポンスを与えるデジタルフィルターを考える手法である。Impulse Invariance法については公知の手法であるため詳細な説明は省略する。

以上の結果、常微分方程式の解はデジタルフィルターの出力として求めることが可能になる。運動方程式であれば、図８に示したように２極のデジタルフィルターとなる。図８において、ｄは１サンプル分の遅延であり、Ｃ０、Ｃ１、Ｃ２はフィルターの係数である。

デジタルフィルターによる処理であれば、ハードウェアー化は容易であるし、後述するように安定性の判定も容易である。また、デジタルフィルターの係数を切り替えれば、特性（柔らかく動かすか固く動かすか等）を切り替えることもできるし、フィルター駆動周波数を切り替えて解の応答性を切り替えることもできる。

１．４．２デジタルフィルターの安定度判定
インピーダンス制御においては、運動方程式の質量項（ｍ）、粘性項（μ）及び弾性項（ｋ）の設定次第では、安定でない系ができてしまう可能性がある。極端な例で言えば、ロボットに一度力を加えたら、その後一切触れていないにもかかわらずロボットの振動が継続するような、発振する系にもなりうるということである。そのような安定性（安定度）の低い系は実用上好ましくないため、運動方程式に関する系の安定性を判定し、安定でない場合には何らかの対処をする必要がある。

しかし、上述のＮｅｗｔｏｎ法やＲｕｎｇｅ−Ｋｕｔｔａ法等では、運動方程式の解を求めることはできても、安定性の判定は行えない。そのため、解を求める処理とは別に安定性を判定する処理が必要となるが、一般的に安定性の判定処理は容易でないことが知られている。

本実施形態の手法では、運動方程式を、デジタルフィルターを用いて処理するため、運動方程式に関する系の安定性の判定とは、対応するデジタルフィルターの安定性の判定に他ならない。デジタルフィルターの安定性の判定は、容易に行うことができ、極が単位円の中にあるか否かを判定すればよい。

具体例を図９（Ａ）〜図９（Ｃ）に示す。これらはどれも極が単位円の中に収まっている例であるが、極が単位円の外にある場合には安定でないと判定する。また、図９（Ｃ）のようにある程度、単位円の円周上から内側に離れた位置に極がある場合には、特に問題はない。しかし、図９（Ａ）のように単位円にかなり近い位置に極（なお、図９（Ａ）は重根ではなく限りなく近い位置に２つの極がある例である）がある場合には、注意が必要である。なぜなら、デジタルフィルターの実装手法によっては、設計値に対して誤差が生ずるおそれがあるからである。当該誤差が極の位置を単位円の外側方向に移動させる要因となった場合、図９（Ａ）のような安定性に余裕のないデジタルフィルターは、設計上安定であるにもかかわらず、実装時には安定でない動作をする可能性があるため、何らかの対処が必要となる。

１．４．３デジタルフィルターを用いた場合の構成例
図１０にデジタルフィルターを用いて運動方程式の解を求める場合の、ロボット制御システム及びそれを含むロボットシステムの構成例を示す。なお本実施形態のロボット制御システム、ロボットシステムは図１０の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

力覚センサー１０、目標値出力部６０、ロボット制御部８０及びロボット１００については図１と同様であるため、詳細な説明は省略する。

力制御部２０は、デジタルフィルター部２２を含む。デジタルフィルター部２２は、力覚センサーからのセンサー情報（センサー情報に対して補正処理や帯域制限処理が施された情報も含む）に対してデジタルフィルター処理を行って、出力値を補正値として目標値出力部６０に出力する。また、力制御部２０はセンサー情報に対して帯域制限処理を行う帯域制限部２４を含んでもよい。

デジタルフィルター部２２は、デジタルフィルター演算部２２１と、デジタルフィルター係数出力部２２２と、デジタルフィルター安定度判定部２２３を含む。デジタルフィルター演算部２２１は、センサー情報とデジタルフィルター係数に基づいて、デジタルフィルター処理を行って運動方程式の解を求める。デジタルフィルター係数出力部２２２は、運動方程式の係数パラメーター（質量項ｍ、粘性項μ、弾性項ｋ及び駆動周期Ｔ）に基づいて、デジタルフィルター係数を求め、デジタルフィルター演算部２２１とデジタルフィルター安定度判定部２２３に出力する。デジタルフィルター安定度判定部２２３は、デジタルフィルター係数に基づいて、当該デジタルフィルターの安定性の判定を行う。

デジタルフィルター係数出力部２２２は、デジタルフィルター係数記憶部２２４と、デジタルフィルター係数変換部２２５を含んでもよい。デジタルフィルター係数変換部２２５は、運動方程式の係数パラメーターをデジタルフィルター係数に変換する。デジタルフィルター係数記憶部２２４は、変換されたデジタルフィルター係数を記憶する。デジタルフィルター係数記憶部２２４に複数のデジタルフィルター係数を記憶しておけば、出力するデジタルフィルター係数を切り替えることで、ロボットの動作特性や、解の応答性を切り替えることが可能になる。

以下、図１０の構成を基本とし、第１〜第４の実施形態について説明する。第１の実施形態は、本発明の基本形である。第２の実施形態は、複数のフィルターを切り替える場合において、複数フィルターを並行動作させる例である。具体的にはデジタルフィルター演算部２２１が複数ある構成となる。第３の実施形態は、複数のフィルターを切り替える場合において、１つのデジタルフィルター演算部２２１を用いる例である。この場合、駆動周波数を切り替える際に、切り替え用の処理を行う必要がある。第４の実施形態は、複数のフィルターを切り替える際のフィルター選択処理を、センサー情報に基づいて行う例である。

２．第１の実施形態
２．１構成
図１１に第１の実施形態にかかるロボット制御システムを含むロボットシステムの構成例を示す。

力覚センサー１０、軌道生成部６２、インバースキネマティクス処理部６４、モーター制御部８２−１等については図１と同様であるため詳細な説明は省略する。また、入力補正部３０はセンサー情報に対して補正処理を行うもので、例えば図７の姿勢補正部５３２やハンド・ツール自重補正部５３４等を含んでもよい。フォワードキネマティクス処理部４０は図７のフォワードキネマティクス処理部５４０に対応し、フォワードキネマティクス処理の結果を入力補正部３０に出力するとともに、必要に応じて軌道生成部６２にも出力する。

また、ロボット制御システムは、デジタルフィルター演算部２２１と、デジタルフィルター安定度判定部２２３と、デジタルフィルター係数記憶部２２４と、デジタルフィルター係数変換部２２５と、応答特性入力部２２６と、フィルター状態記憶部２２７とを含み、具体的にはこれらは力制御部２０のデジタルフィルター部２２に含まれる。

デジタルフィルター演算部２２１、デジタルフィルター安定度判定部２２３、デジタルフィルター係数記憶部２２４、デジタルフィルター係数変換部２２５については図１０を用いて説明したため詳細については省略する。

応答特性入力部２２６は、デジタルフィルター係数変換部２２５に対して運動方程式の係数パラメーター（ｍ、μ、ｋ）及び駆動周期Ｔ（駆動周波数）を出力する。

フィルター状態記憶部２２７は、フィルター状態を記憶する。ここで、フィルター状態とは、デジタルフィルター演算部２２１の出力（つまり運動方程式の解）である。上述したように、運動方程式は２極のデジタルフィルターに対応し、具体的には図８のようになる。図８から明らかなように、このデジタルフィルターは２回遅延がかかるため、あるタイミングにおける出力値を求めるためには、１タイミング前の出力値と２タイミング前の出力値が必要となる。フィルター状態記憶部２２７はそれらを記憶するものであり、デジタルフィルター演算部２２１の出力を記憶するとともに、デジタルフィルター演算部２２１に対して過去の出力値を送る。

２．２詳細な処理
次に本実施形態の詳細な処理例について図１２のフローチャートを用いて説明する。図１２は、デジタルフィルター部２２における処理についてのフローチャートである。

この処理が開始されると、まず出力のタイミング待ちを行ってから（Ｓ１０１）、外力値Ｆを取得する（Ｓ１０２）。ここでの外力値Ｆは力覚センサー１０からのセンサー情報に対して入力補正部３０による補正処理が施されたものとする。

次に取得したＦを用いて運動方程式の解を求める（Ｓ１０３）。具体的には、フィルター状態記憶部２２７において、１ステップ前のデジタルフィルター演算部２２１の出力Ｙ_n―1をＹ_n―2の値とするとともに、Ｙ_nの値をＹ_n―1に上書きする。そして、デジタルフィルター係数記憶部２２４から出力されたデジタルフィルター係数（Ｃ０，Ｃ１，Ｃ２）と、フィルター状態記憶部２２７から出力された過去の出力値（Ｙ_n-1、Ｙ_n―2）から、出力値Ｙ_nを求める処理をデジタルフィルター演算部２２１で行う。

以上がある１タイミングでの運動方程式の解を求める処理となる。１タイミングの処理後に、次のタイミングにおける応答特性を変更するかの判定を行い（Ｓ１０４）、変更しない場合にはＳ１０１に戻る。変更する場合には、まず応答特性入力部２２６から応答特性のパラメーター（ｍ、μ、ｋ、Ｔ）を取得し（Ｓ１０５）、デジタルフィルター係数変換部２２５においてデジタルフィルター係数を計算する（Ｓ１０６）。そして、計算したデジタルフィルター係数に基づいて、デジタルフィルター安定度判定部２２３においてデジタルフィルターの安定性を判定する（Ｓ１０７）。フィルターが安定でない場合には、デジタルフィルター係数記憶部２２４に記憶することなくＳ１０１に戻る。つまり、次ステップにおいても応答特性は変更されない。Ｓ１０７でフィルターが安定であると判定された場合には、デジタルフィルター係数記憶部２２４にデジタルフィルター係数を記憶し（Ｓ１０８）、Ｓ１０１に戻って更新したデジタルフィルター係数で次のステップの処理を行う。

以上の本実施形態では、ロボット制御システムは、図１０に示したように力覚センサー１０からのセンサー情報に基づいて、ロボットの目標軌道についての補正値を出力する力制御部２０と、目標軌道を補正値に基づいて補正することで目標値を出力する目標値出力部６０と、目標値に基づいてロボット１００のフィードバック制御を行うロボット制御部８０と、を含む。そして力制御部２０はデジタルフィルター部２２を有し、センサー情報に対してデジタルフィルター部２２によるデジタルフィルター処理を行うことで、力制御における常微分方程式の解を求めて、求めた解に基づいて補正値を出力する。

ここで、目標値とはロボット１００のフィードバック制御における目標値のことであり、ロボット制御部８０における制御はこの目標値に基づいて行われる。目標値は目標軌道に対して補正値による補正処理を行うことで取得できる。なお目標軌道とは、狭義にはロボットのエンドエフェクター部（エンドポイント）の空間的な目標位置の変化を表すものであってもよい。１つの目標位置は、例えば３次元空間座標ｘｙｚ（姿勢も考慮すれば各軸周りの回転角ｕｖｗを追加してもよい）で表されることになり、目標軌道とは当該目標位置の集合となる。ただし、目標軌道はこれに限定されるものではなく、ロボットの目標関節角の集合であってもよい。関節を持つロボットでは、各関節の角度を決定すると、フォワードキネマティクス処理によりエンドエフェクター部の位置は一意に決定される。つまり、Ｎ関節ロボットではＮ個の関節角（θ１〜θＮ）により１つの目標位置を表現できることになるから、当該Ｎ個の関節角の組を１つの目標関節角とすれば、目標軌道とは目標関節角の集合と考えることができる。よって、力制御部２０から出力される補正値も、位置に関する値であってもよいし、関節角に関する値であってもよい。

具体例を図２１（Ａ）、図２１（Ｂ）に示す。デジタルフィルター処理により解を求める対象となる常微分方程式として、上式（１）の運動方程式を用いるのであれば、当該運動方程式の解は位置に関する値となる。よって、目標軌道が目標位置である場合には、解をそのまま補正値とすればよく、システム構成例は図２１（Ａ）のようになる。なお、目標値は位置に関する値でも関節角に関する値でもよいが、一般的にはロボットのフィードバック制御は関節角を用いることが想定される。

それに対して、目標値出力部６０のインバースキネマティクス処理部６４とは別に、力制御部２０がインバースキネマティクス処理部２９を含む図２１（Ｂ）のようなケースも考えられる。例えば、目標値出力部６０での目標軌道生成処理と、力制御部２０での補正値出力処理とで、処理のタイミングや処理レートが異なる場合等である。その場合には、目標軌道は目標関節角であり、力制御部２０では運動方程式の解に対して変換処理（例えばインバースキネマティクス処理）を行って補正値とすることになる。

また、力制御における常微分方程式とは、力制御において解を求める必要がある常微分方程式のことである。狭義には線形常微分方程式であってもよい。さらに狭義には、ロボット１００にあたかも所望の特性（質量・粘性・弾性等）を持つようにふるまわせるために解を求める必要がある常微分方程式のことであり、式（１）に示したような運動方程式であってもよい。

これにより、力制御において必要な、常微分方程式の解を求めるという処理を、デジタルフィルターを用いて行うことが可能になる。Ｎｅｗｔｏｎ法、Ｒｕｎｇｅ−Ｋｕｔｔａ法等の手法を用いる場合に比べて、ハードウェアー化が容易であり、上述したように安定性の判定も容易である。さらに、デジタルフィルター処理に用いるデジタルフィルターを切り替える（例えばフィルター係数を切り替える）ことで、簡単に応答特性を切り替えることも可能となる。

また、デジタルフィルター部２２は、図１０に示したように常微分方程式の各項の係数パラメーターに基づいてデジタルフィルター係数を出力するデジタルフィルター係数出力部２２２と、デジタルフィルター係数及びセンサー情報に基づいてデジタルフィルター処理を行うデジタルフィルター演算部２２１とを含んでもよい。

ここで、常微分方程式の各項の係数パラメーターとは、常微分方程式における定数項、１階微分項の係数、２階微分項の係数、・・・ｎ階微分項の係数を指すものとする。上述した式（１）の例であれば、ｍ、μ、ｋが係数パラメーターとなる。また、デジタルフィルター処理に用いられるセンサー情報とは、力覚センサー１０からの出力値そのものであってもよいし、当該出力値に対して入力補正部３０（図１１に記載）による補正処理が施されたものであってもよい。また、帯域制限部２４（図１０に記載）による帯域制限処理が施されたものであってもよい。

これにより、デジタルフィルター処理に必要なデジタルフィルター係数を出力した上で、当該デジタルフィルター係数及びセンサー情報に基づくデジタルフィルター処理が可能となる。

また、デジタルフィルター係数出力部２２２は、図１０、図１１に示したように、常微分方程式の係数パラメーターをデジタルフィルター係数に変換するデジタルフィルター係数変換部２２５と、デジタルフィルター係数を記憶するデジタルフィルター係数記憶部２２４を含んでもよい。そして、デジタルフィルター係数出力部２２２は、デジタルフィルター係数記憶部２２４に記憶されたデジタルフィルター係数を出力する。

これにより、常微分方程式の係数パラメーターから、デジタルフィルター係数を求めることが可能になる。力制御を行う際には、まず当該力制御に必要な常微分方程式が決定される。例えば、ロボット１００にとらせたい物理的な特性が決まれば、上式（１）のｍ、μ、ｋに具体的な数値が入れられた運動方程式が決定されることになる。しかし、常微分方程式の係数パラメーターとデジタルフィルター係数とは異なるものであるため、常微分方程式が決定されたら、当該常微分方程式の解を求めるデジタルフィルター（具体的にはデジタルフィルター係数）を求める処理が必要となる。本実施形態では、この処理をデジタルフィルター係数変換部２２５で行う。

また、デジタルフィルター部２２は、図１０に示したように、デジタルフィルター処理に用いられるデジタルフィルターの安定度（安定性）を判定するデジタルフィルター安定度判定部２２３を含んでもよい。そして、デジタルフィルター係数出力部２２２は、デジタルフィルターが安定であると判定された場合に、デジタルフィルター係数を出力する。

これにより、デジタルフィルターの安定性を判定することが可能になる。力制御における常微分方程式の係数パラメーターは、設定次第では現実にはあり得ない系（例えば発振するロボット等）を作ってしまうことがある。そのため、常微分方程式の安定性を判定する必要があるが、デジタルフィルターを用いれば、その判定が容易となる。詳細については上述したとおりである。

また、本実施形態ではデジタルフィルターの駆動周波数の切り替えについては述べていない（第２、第３の実施形態で後述している）が、図１０に示した基本構成を用いて駆動周波数の切り替えを行うことを妨げるものではない。具体的には、第１の駆動周波数と、第１の駆動周波数よりも高い周波数である第２の駆動周波数の間で駆動周波数を切り替えながらデジタルフィルター処理を行ってもよい。その場合、デジタルフィルター係数出力部２２２は、第１の駆動周波数に対応する第１のデジタルフィルター係数及び第２の駆動周波数に対応する第２のデジタルフィルター係数の少なくとも一方を出力する。デジタルフィルター演算部２２１は、出力されたデジタルフィルター係数に応じて、第１のデジタルフィルター係数を用いた第１の駆動周波数によるデジタルフィルター処理、及び第２のデジタルフィルター係数を用いた第２の駆動周波数によるデジタルフィルター処理の少なくとも一方を行う。

これにより、デジタルフィルター処理の駆動周波数の切り替えが可能になる。駆動周波数を切り替えることで、常微分方程式の解の応答性を切り替えることができる。

また、本実施形態での常微分方程式は、仮想質量項、仮想粘性項、仮想弾性項を係数パラメーターとする運動方程式であってもよい。

これにより、運動方程式の解を求めることが可能になる。よって、ロボット１００に対して、あたかも仮想質量項に対応する質量、仮想粘性項に対応する粘性、仮想弾性項に対応する弾性を持つかのようにふるまわせることが可能になる。

また、目標値出力部６０は、図１０に示したように、ロボット１００の目標位置情報を出力する軌道生成部６２と、軌道生成部６２からの目標位置情報に基づいてインバースキネマティクス処理を行い、ロボット１００の関節角情報を出力するインバースキネマティクス処理部６４を含んでもよい。

ここで、目標位置情報とは、ロボット１００（狭義にはロボット１００のエンドエフェクター部）の目標位置に関する情報であり、目標位置は例えばｘｙｚ（姿勢に関するｕｖｗを含んでもよい）により表される。目標位置情報は、１つの目標位置の情報であってもよいが、ロボット１００の時間的な位置変化（移動経路）を表す複数の目標位置の情報であってもよい。

これにより、ロボット１００の目標位置情報を出力した上で、インバースキネマティクス処理を行って関節角情報に変換することが可能になる。軌道生成部６２によりロボット１００の目標位置情報を生成することで、ロボット１００を計画的に制御することができる。しかし、ロボット１００を柔軟に移動させるためには関節を持たせることは自然であり、その場合一般的に関節の角度を制御することでロボット制御を行う。よって、目標位置情報を直接目標値とすることは難しく、関節角情報に変換するインバースキネマティクス処理部６４が必要となる。

また、以上の本実施形態は、上述してきたロボット制御システム（力制御部２０、目標値出力部６０、ロボット制御部８０を含む）と、ロボット１００を含むロボットシステムに関係する。

これにより、ロボット制御装置にとどまらず、本実施形態の処理を実行するロボットシステムを実現することが可能になる。

また、以上の本実施形態は、センサーからのセンサー情報を取得するセンサー情報取得部と、取得されたセンサー情報に対して処理を行うセンサー情報処理部とを含むセンサー情報処理装置に適用できる。センサー情報処理部は、デジタルフィルター部を含み、センサー情報に対してデジタルフィルター部によるデジタルフィルター処理を行うことで、センサー情報処理における常微分方程式の解を求め、センサー情報処理の出力値として出力する。

以上の説明では、センサー情報処理装置として、ロボット制御システムに適用した場合について説明した。ロボット制御システムでは、センサー情報処理装置は図１０等における力制御部２０に相当し、センサーは力覚センサー１０に相当する。センサー情報取得部は特に図示されてはいないが、力覚センサー１０と接続されているブロック（図１０の場合は帯域制限部２４）が兼ねてもよい。また、センサー情報処理部は、図１０のデジタルフィルター部２２に対応し、さらに帯域制限部２４や図１１の入力補正部３０等を含んでもよい。ただし、センサー情報処理装置は、ロボット制御システムへの適用に限定されるものではなく、他の制御システムに適用されてもよい。その場合、センサー情報に対するセンサー情報処理（デジタルフィルター処理を含む）に基づいて、制御で使用される情報を出力することになる。さらに言えば、センサー情報処理装置は制御システムへの適用に限定されず、他のシステム（装置）に用いられてもよい。例えば、センサーを用いて種々の情報を検出する検出システム等に用いられてもよい。

ここで、センサー情報処理とは、センサーからの出力値に対して何らかの処理を施して、有用な情報を求める処理のことであり、少なくとも常微分方程式の解を求める処理が含まれる。例えば、ロボット制御システムでは、力覚センサーにかかる力情報から補正値（例えばロボットの目標位置の補正値）を求めるような、物理的に意味の異なる情報への変換処理となる。また、検出システムであれば、検出精度を高める処理（例えば、センサー情報に含まれるノイズを低減する処理）等が考えられる。

なお、センサー情報処理装置の適用がロボット制御システムに限定されないように、センサー情報を出力するセンサーも力覚センサーに限定されるものではない。例えば、加速度センサーや角速度センサー、方位センサー、生体センサー（脈拍センサー等）、ＧＰＳ等であってもよい。

これにより、センサー情報に対するデジタルフィルター処理により、常微分方程式の解を求め、出力値を出力するセンサー情報処理装置を実現することが可能になる。このセンサー情報処理装置は例えばセンサーを含むＩＣチップ等として実現されてもよい。センサー情報そのものではなく、センサー情報を用いた常微分方程式の解等を出力できるため、他のシステム等と組み合わせた際に、装置の出力値の有用性が増すことになる。

３．第２の実施形態
本実施形態においては、デジタルフィルターの駆動周波数を切り替えることを想定している。例えば、帯域制限に応じて駆動周波数を切り替えることが考えられる。一般的に力覚センサーのセンサー情報にはノイズが多い。そのため帯域制限処理を行うことが望ましいが、通過帯域の広さは状況によって異なる（ノイズに対して大きい信号値が得られるのであれば、帯域制限処理は軽めでよく、通過帯域を広くできる）。通過帯域を狭くするほどノイズは減るが、応答特性も悪化するため、通過帯域に対応させてデジタルフィルターの駆動周波数も切り替える必要がある。具体的には、通過帯域を広くした場合には高い駆動周波数で動作させ、通過帯域を狭くした場合には低い駆動周波数で動作させることになる。

本実施形態では、デジタルフィルター演算部２２１を複数設け、それぞれに通過帯域の異なる帯域制限処理を施したセンサー情報を入力し、異なる駆動周波数で動作させる。つまり、駆動周波数の異なる２つのフィルターを並行動作させておき、状況に応じて適切なフィルターを選択する。なお、以下の説明では２つのフィルターを並行動作させる例について述べるが、並行動作させるデジタルフィルターは３つ以上であってもよい。

３．１構成
図１３に第２の実施形態にかかるロボット制御システムを含むロボットシステムの構成例を示す。

第１の実施形態（図１１）と同様の箇所については詳細な説明は省略する。第１の実施形態と比較した場合、デジタルフィルター演算部２２１が２つ（２２１−１、２２１−２）、デジタルフィルター係数記憶部２２４が２つ（２２４−１、２２４−２）、フィルター状態記憶部２２７が２つ（２２７−１、２２７−２）ある点が異なる。また、デジタルフィルター部２２は、帯域制限部２４−１、２４−２及びデジタルフィルター出力選択部２２０を含む。

帯域制限部２４−１は、センサー情報に対して第１の周波数帯域を通過帯域とする帯域制限処理を行いデジタルフィルター演算部２２１―１に出力する。帯域制限部２４−２は、センサー情報に対して第１の周波数帯域とは異なる第２の周波数帯域を通過帯域とする帯域制限処理を行いデジタルフィルター演算部２２１―２に出力する。

デジタルフィルター出力選択部２２０は、デジタルフィルター演算部２２１―１とデジタルフィルター演算部２２１―２の出力を受け付け、どちらか一方を選択し、選択した値をデジタルフィルター部２２の出力値として出力する。

デジタルフィルター係数記憶部２２４−１は、図１２のＳ１０５〜Ｓ１０７の処理を行った結果、安定と判定された第１のデジタルフィルター係数を記憶する。同様にデジタルフィルター係数記憶部２２４−２は、第１のデジタルフィルター係数とは異なる第２のデジタルフィルター係数を記憶する。

デジタルフィルター演算部２２１−１では、デジタルフィルター係数記憶部２２４−１に記憶された第１のデジタルフィルター係数と、帯域制限部２４−１から出力された帯域制限処理後のセンサー情報と、フィルター状態記憶部２２７―１から出力された過去の出力値とに基づいて、デジタルフィルター処理を行う。

同様に、デジタルフィルター演算部２２１−２では、デジタルフィルター係数記憶部２２４−２に記憶された第２のデジタルフィルター係数と、帯域制限部２４−２から出力された帯域制限処理後のセンサー情報と、フィルター状態記憶部２２７―２から出力された過去の出力値とに基づいて、デジタルフィルター処理を行う。つまり、２つのデジタルフィルターが並列動作することになる。

３．２詳細な処理
次に本実施形態の詳細な処理例について図１４のフローチャートを用いて説明する。図１４は、デジタルフィルター部２２における処理についてのフローチャートである。

この処理が開始されると、まず出力のタイミング待ちを行ってから（Ｓ２０１）、外力値Ｆを取得する（Ｓ２０２）。ここでの外力値Ｆは力覚センサー１０からのセンサー情報に対して入力補正部３０による補正処理が施されたものとする。

次に、並列動作しているフィルターの全てについて処理を行ったかを判定する（Ｓ２０３）。未処理のフィルターが残っている場合には、当該フィルターを選択し（Ｓ２０４）、帯域制限部（２４−１等のうち対応するもの）において選択したフィルターに対応する帯域制限処理を行う（Ｓ２０５）。そして選択したフィルターに対応するデジタルフィルター演算部において、帯域制限処理が施されたセンサー情報を入力値としてデジタルフィルター処理が行われる（Ｓ２０６）。Ｓ２０６の詳細についてはＳ１０３と同様である。処理後Ｓ２０３に戻る。

Ｓ２０３において、全てのフィルターが処理済みであると判定された場合には、デジタルフィルター出力選択部２２０において、複数のフィルターの中から出力フィルターを選択し（Ｓ２０７）、選択されたフィルターの出力値を出力する（Ｓ２０８）。Ｓ２０８の処理後、Ｓ２０１に戻る。

以上の本実施形態では、デジタルフィルター部２２は、図１３に示したようにデジタルフィルター演算部２２１として、第１のデジタルフィルター演算部（２２１−１）と、第２のデジタルフィルター演算部（２２１−２）を含む。デジタルフィルター係数出力部２２２は、第１のデジタルフィルター演算部に第１のデジタルフィルター係数を出力し、第２のデジタルフィルター演算部に第２のデジタルフィルター係数を出力する。そして、第１のデジタルフィルター演算部では第１のデジタルフィルター係数及びセンサー情報に基づくデジタルフィルター処理を行い、第２のデジタルフィルター演算部では第２のデジタルフィルター係数及びセンサー情報に基づくデジタルフィルター処理を行う。

これにより、複数のデジタルフィルター処理を並行して行うことが可能になる。そのため、ロボットの特性を切り替えるためのデジタルフィルター係数の切り替えを行う際にも、切り替え前後のデジタルフィルター処理をあらかじめ並行動作させておくことができるため、切り替えがスムーズに行える。また、駆動周波数の切り替えの際も、後述する第３の実施形態のような切り替え処理をする必要がない。

また、デジタルフィルター部２２は、図１３に示したように、第１のデジタルフィルター演算部における処理結果と、第２のデジタルフィルター演算部における処理結果のうちいずれか１つを選択して補正値として出力するデジタルフィルター出力選択部２２０を含んでもよい。

これにより、複数のデジタルフィルターが並行動作している場合にも、適切なデジタルフィルターを選択し、選択したデジタルフィルターの処理結果を補正値として出力することが可能になる。デジタルフィルターの選択は、力制御の内容（例えばロボット１００の振る舞い）に直結するため、ユーザーの所望する制御になるように、適切なデジタルフィルターを選択する必要がある。

また、第１の駆動周波数と第２の駆動周波数（第１の駆動周波数よりも高周波であるものとする）のいずれかの駆動周波数でデジタルフィルター処理を行ってもよい。その場合、デジタルフィルター係数出力部２２２は、第１のデジタルフィルター係数として第１の駆動周波数に対応する値を出力し、第２のデジタルフィルター係数として第２の駆動周波数に対応する値を出力する。そして、第１のデジタルフィルター演算部は、第１のデジタルフィルター係数を用いて第１の駆動周波数によるデジタルフィルター処理を行い、第２のデジタルフィルター演算部は、第２のデジタルフィルター係数を用いて第２の駆動周波数によるデジタルフィルター処理を行う。

これにより、デジタルフィルター処理の駆動周波数を切り替えることが可能になる。駆動周波数を切り替えることで、常微分方程式の解の応答性を切り替えることができる。本実施形態においては、切り替え前も切り替え後も、第１の駆動周波数によるデジタルフィルター処理と第２の駆動周波数によるデジタルフィルター処理を並行して行っているため、切り替えをスムーズに行うことができる。

また、力制御部２０は、図１０に示したように、センサー情報に対して所与の周波数帯域を通過させる帯域制限処理を行い、帯域制限処理を行ったセンサー情報をデジタルフィルター演算部に出力する帯域制限部２４を含んでもよい。そして、帯域制限部２４は、第１のデジタルフィルター演算部に対しては、第１の周波数帯域を通過させる帯域制限処理を行ったセンサー情報を出力し、第２のデジタルフィルター演算部に対しては、第２の周波数帯域（第１の周波数帯域よりも広い周波数帯域）を通過させる帯域制限処理を行ったセンサー情報を出力する。

なお、帯域制限部２４は、図１３に示したように帯域制限部２４―１と、帯域制限部２４―２から構成されてもよい。

これにより、駆動周波数にあわせて、センサー情報に対して帯域制限処理を施すことが可能になる。具体的には、駆動周波数が高いほど通過帯域幅は広くなり、駆動周波数が低いほど通過帯域幅は狭くなる。これは上述したように、帯域制限処理を強くするほど、応答特性が悪化することによる。なお、このように帯域制限処理の強度と駆動周波数は対応関係を持つことになるが、どちらが先に決定されてもよい。センサー情報にノイズが多くのっている場合には、ノイズ除去のため帯域制限処理を強くかけるとよい。つまり、ノイズ量の観点から、まず帯域制限処理の強度が決定され、それに応じて駆動周波数が決定されるケースがある。それに対して、常微分方程式の解の応答性から、駆動周波数が先に決定され（例えばすぐに解がほしいため駆動周波数を高くする等）、それに応じて帯域制限処理の強度が決定されるケースもある。

４．第３の実施形態
本実施形態においては、第２の実施形態と同様にデジタルフィルターの駆動周波数を切り替えることを想定している。本実施形態では、デジタルフィルター処理を行うデジタルフィルター演算部２２１が１つである構成について説明する。デジタルフィルター演算部２２１が１つの場合には、駆動周波数を切り替える際に切り替え処理を行う必要がある。具体的には、高周波から低周波への切り替えと、低周波から高周波への切り替えに分けて後述する。

４．１構成
図１５に第３の実施形態にかかるロボット制御システムを含むロボットシステムの構成例を示す。第２の実施形態（図１３）と同様の箇所については詳細な説明は省略する。第２の実施形態と比較した場合、デジタルフィルター演算部２２１が１つである点が異なる。また、入力選択部２５、係数選択部２２８及び状態選択・処理部２２９を含む。

係数選択部２２８は、デジタルフィルター係数記憶部２２４−１から出力された第１のデジタルフィルター係数と、デジタルフィルター係数記憶部２２４−２から出力された第２のデジタルフィルター係数のうち、デジタルフィルター処理に用いる係数を１つ選択し、選択したデジタルフィルター係数をデジタルフィルター演算部２２１に出力する。これは、複数のデジタルフィルターの中から、出力に用いられる出力フィルターを選択していると言うこともできる。

状態選択・処理部２２９は、係数選択部２２８で選択されたデジタルフィルター係数に対応するデジタルフィルター（以下、出力フィルター）のフィルター状態に関する処理を行う。具体的には、出力フィルターに対応するフィルター状態記憶部から、出力フィルターを用いたデジタルフィルター処理の過去の出力値を取得し、デジタルフィルター演算部２２１に出力する。さらに、デジタルフィルター演算部２２１の出力値を、出力フィルターに対応するフィルター状態記憶部に出力する。

上述したように、本実施形態のデジタルフィルター処理では過去のタイミングにおける出力値を用いる必要がある。その際、あるデジタルフィルターが出力フィルターとして選択された場合には、当然のことながら選択された当該デジタルフィルターの過去の出力値を用いなければならない。つまり、係数選択部２２８において第ｉのデジタルフィルターに対応するデジタルフィルター係数を選択しながら、状態選択・処理部２２９において第ｉとは異なる第ｋのデジタルフィルターに対応するフィルター状態を選択することはできない。よって、係数選択部２２８及び状態選択・処理部２２９はともに複数の候補の中から１つを選択する処理を行うものであるが、選択処理は独立に行われるのではなく、同一のデジタルフィルターに対応する候補を選択するような拘束条件を考慮する必要がある。

また、入力選択部２５は、帯域制限部２４―１及び帯域制限部２４−２からの出力の中から１つを選択しデジタルフィルター演算部２２１に出力する。ここでも、出力フィルターに対応したものを選択することになる。デジタルフィルターの駆動周波数と帯域制限処理の通過帯域幅との関係は、第２の実施形態において上述したとおりである。つまり、出力フィルターが選択されたら、帯域制限処理の通過帯域も決定されることになり、入力選択部２５は対応する帯域制限部からの出力を選択する。以上のことから、係数選択部２２８及び状態選択・処理部２２９だけでなく、入力選択部２５についても出力フィルターの選択に基づいた選択処理が行われることになる。

４．２詳細な処理
４．２．１高周波から低周波への切り替え
本実施形態においては、駆動周波数の切り替えの際に切り替え処理を行う必要がある。まず、高周波（短周期）から低周波（長周期）への切り替えについて説明する。

図１６に示したようにＡ１のタイミングで駆動周波数を切り替えることを考える。その際、切り替え後の低周波フィルターでの出力値を求めるには、Ａ２及びＡ３で示した過去２タイミングでの、低周波フィルターの出力値が必要となる。

駆動周波数が異なっても、サンプリングのレートが異なるだけであり、サンプリング点を滑らかにつないだ波形は同一になるはずである。つまり、図１６の例のように高周波が低周波の１０倍のサンプリングレートであるとすれば、高周波フィルターにおける１０タイミング前及び２０タイミング前の出力値を保持しておけば、低周波フィルターにおける１タイミング前及び２タイミング前の出力値を取得することができる。

しかし、フィルター状態記憶部２２７が過去２タイミング分の出力値しか記憶しないことを前提とすると問題が生じる。つまり、切り替え前は高周波フィルターが動作していたのであるから、Ａ４（高周波フィルターでの１タイミング過去のタイミング）及びＡ５（高周波フィルターでの２タイミング過去のタイミング）に示したように高周波フィルターにおける過去２タイミングの情報しか保持しておらず、Ａ２・Ａ３で示したタイミングの出力値は失われてしまっている。

そこで本実施形態では、切り替え時に切り替え期間を設け、当該切り替え期間内では高周波フィルターと低周波フィルターを並行動作させる。図１６で言えば、Ａ３のタイミングにおいて切り替え指示を出し、Ａ３からＡ１までの期間では２つのフィルターを並行動作させる。そのようにした上で、Ａ１のタイミングでフィルターの切り替えを行う。このようにすれば、Ａ１までの間は高周波フィルターを用いた出力を継続しつつ、低周波フィルターのＡ３、Ａ２での出力値を取得できるため、Ａ１において低周波フィルターの出力に必要な値がそろうことになり、問題なく切り替え処理を行うことができる。

なお、上述したように高周波フィルターと低周波フィルターはサンプリングレートが異なるだけであり、同一タイミングにおける出力は同一のはずである。よって、並行動作といっても、低周波フィルターのデジタルフィルター係数を用いた演算は必要なく、演算は高周波フィルターに関して行えばよい。そして、高周波フィルターの出力値のうち、Ａ３タイミング及びＡ２タイミングにおける値を、低周波フィルターに対応するフィルター状態記憶部に記憶する。

次に本実施形態の詳細な処理例について図１７のフローチャートを用いて説明する。図１７は、デジタルフィルター部２２における処理についてのフローチャートである。

この処理が開始されると、まず出力のタイミング待ちを行ってから（Ｓ３０１）、外力値Ｆを取得する（Ｓ３０２）。ここでの外力値Ｆは力覚センサー１０からのセンサー情報に対して入力補正部３０による補正処理が施されたものとする。

次に図１２のＳ１０３と同様のデジタルフィルター処理を行い（Ｓ３０３）、駆動周期を変更するかの判定を行う（Ｓ３０４）。Ｓ３０４でＮｏの場合は、Ｓ３０１に戻って処理を続ける。

Ｓ３０４でＹｅｓの場合には、変更後のデジタルフィルター係数を取得し、安定性の判定を行う（Ｓ３０５〜Ｓ３０７）。この処理については図１２のＳ１０５〜Ｓ１０７と同様である。

Ｓ３０７でＹｅｓの場合には、まずそのタイミングでの出力値を、低周波フィルターの２タイミング前の出力値として記憶する。これは図１６で言えば、Ａ３タイミングに対応する。実際の切り替えはＡ１タイミングになるため、Ａ３タイミングでの出力値は、切り替え時から見て２タイミング過去になる。

Ｓ３０９〜Ｓ３１１については、Ｓ３０１〜Ｓ３０３と同様に高周波フィルターでのデジタルフィルター処理である。そして、現タイミングが低周波フィルターにおける１周期目の出力タイミングであるかの判定を行う（Ｓ３１２）。これは図１６で言えば、Ａ２のタイミングであるか否かの判定である。Ａ３とＡ２の間のタイミングでの高周波フィルターの出力値は、低周波フィルターの動作には寄与しない。よって、Ａ２のタイミングであったときのみ（Ｓ３１２でＹｅｓの場合）、その際の出力値を低周波フィルターの１タイミング前の出力値として記憶する（Ｓ３１３）。Ｓ３１３の処理後はＳ３０９に戻る。

Ｓ３１２でＮｏの場合には、現タイミングが低周波フィルターにおける２周期目の出力タイミングであるかの判定を行う（Ｓ３１４）。これは図１６で言えば、Ａ１のタイミングであるか否かの判定である。Ｓ３１４でＮｏの場合にはＳ３０９に戻り、Ｙｅｓの場合には駆動周波数を低周波に切り替える（Ｓ３１５）。

つまり、Ｓ３０８〜Ｓ３１５においては、Ａ３のタイミングでの出力値を記憶し（Ｓ３０８）、Ａ３からＡ２の間では何もしない（Ｓ３１２及びＳ３１４でともにＮｏ）。そしてＡ２のタイミングで出力値を記憶し（Ｓ３１３）、Ａ２からＡ１の間では何もしない（Ｓ３１２及びＳ３１４でともにＮｏ）。そのようにしてＡ２、Ａ３での出力値を保持した上で、Ａ１のタイミングを待って駆動周波数の切り替えを行う（Ｓ３１５）。

４．２．２低周波から高周波への切り替え
次に低周波（長周期）から高周波（短周期）への切り替えについて説明する。図１８に示したようにＢ１のタイミングで駆動周波数を切り替えることを考える。その際、切り替え後の高周波フィルターでの出力値を求めるには、Ｂ４及びＢ５で示した過去２タイミングでの、高周波フィルターの出力値が必要となる。それに対して、Ｂ１のタイミングで保持されている過去タイミングの出力は、Ｂ２及びＢ３で示したタイミングのものである。

よって、高周波フィルターの動作に必要な過去２タイミングの出力値が無いため、そのままでは駆動周波数の切り替えができない。本実施形態では、Ｂ１のタイミング及びＢ２のタイミングにおける低周波フィルターの出力値に基づいて、Ｂ４・Ｂ５のタイミングにおける高周波フィルターの出力値を推定する補間処理（内挿処理）を行う。例えば、下式（３）を用いて処理を行えばよい。

上式（３）において、左辺のＹは高周波フィルターに関する値であり、右辺のＹは低周波フィルターに関する値である。また、Ｐ_Sは高周波フィルターの駆動周期、Ｐ_Lは低周波フィルターの駆動周期を表す。つまり、一般的な線形補間を用いるものとする。ただし補間処理の手法はこれに限定されるものではなく、例えばＢ３のタイミングにおける出力値も用いて、２次関数による近似等を行ってもよい。

次に本実施形態の詳細な処理例について図１９のフローチャートを用いて説明する。図１９は、デジタルフィルター部２２における処理についてのフローチャートである。

Ｓ４０１〜Ｓ４０７については図１７のＳ３０１〜Ｓ３０７の処理と同様であるため詳細な説明は省略する。Ｓ４０７でＹｅｓの場合、つまり駆動周波数の切り替えが指示され、かつ、切り替え後のデジタルフィルターが安定であると判定された場合には、上式（３）を用いて、高周波フィルターでの過去２タイミングの出力値を求め（Ｓ４０８）、Ｓ４０１に戻り高周波フィルターによるデジタルフィルター処理を行う。

以上の本実施形態では、デジタルフィルター係数出力部２２２は、図１５に示したように、第１のデジタルフィルター係数と、第１のデジタルフィルター係数とは異なる第２のデジタルフィルター係数のうち、いずれか１つを選択する係数選択部２２８を含む。そして、デジタルフィルター係数出力部２２２は、係数選択部２２８で選択されたデジタルフィルター係数をデジタルフィルター演算部２２１に出力する。

なお、係数選択部２２８にデジタルフィルター係数を出力するデジタルフィルター係数記憶部は、図１５に示したように２つのデジタルフィルター係数記憶部（２２４−１，２２４−２）から構成されてもよい。

これにより、複数のデジタルフィルター係数を記憶しておき、その中から１つを選択して用いることで、デジタルフィルター演算部２２１が１つの場合にも、複数のデジタルフィルターをスムーズに切り替えることが可能になる。なお、切り替えをスムーズにするためには、フィルター状態記憶部２２７は複数のデジタルフィルターに対応するフィルター状態を記憶しておくことが望ましい（例えば図１５に示したように、複数のフィルター状態記憶部２２７−１，２２７−２から構成する等により実現できる）。

また、第１の駆動周波数と第２の駆動周波数（第１の駆動周波数よりも高周波であるものとする）のいずれかの駆動周波数でデジタルフィルター処理を行ってもよい。その場合、係数選択部２２８は、第１の駆動周波数に対応する第１のデジタルフィルター係数と、第２の駆動周波数に対応する第２のデジタルフィルター係数のうち、いずれか１つを選択する。そして、デジタルフィルター演算部２２１は、選択されたデジタルフィルター係数を用いて、当該係数に対応する駆動周波数でデジタルフィルター処理を行う。

これにより、デジタルフィルター処理の駆動周波数を切り替えることが可能になる。駆動周波数を切り替える利点については上述したとおりである。

また、力制御部２０は、図１０に示したように、センサー情報に対して所与の周波数帯域を通過させる帯域制限処理を行い、帯域制限処理を行ったセンサー情報をデジタルフィルター演算部に出力する帯域制限部２４を含んでもよい。そして、帯域制限部２４は、第１の駆動周波数が用いられる場合には、第１の周波数帯域を通過させる帯域制限処理を行ったセンサー情報を出力し、第２の駆動周波数が用いられる場合には、第２の周波数帯域（第１の周波数帯域よりも広い周波数帯域）を通過させる帯域制限処理を行ったセンサー情報を出力する。

なお、帯域制限部２４は、図１５に示したように帯域制限部２４―１、帯域制限部２４―２及び入力選択部２５から構成されてもよい。複数の駆動周波数に対応して複数の帯域制限処理が施されるが、デジタルフィルター演算部２２１は１つであるから、複数の帯域制限処理後のセンサー情報の中から適切なものを選択する必要があるためである。或いは、複数の帯域制限処理のなかから、１つの処理のみを実行し、他の処理を実行しない構成にしてもよい。

これにより、駆動周波数にあわせて、センサー情報に対して帯域制限処理を施すことが可能になる。帯域制限処理の利点については第２の実施形態で上述した通りである。

また、デジタルフィルター処理の駆動周波数を、高周波の第２の駆動周波数から、低周波の第１の駆動周波数に切り替える場合に、デジタルフィルター演算部２２１は、第１の期間、切り替え期間、第２の期間でデジタルフィルター処理を切り替えてもよい。ここで、第１の期間の次の期間が切り替え期間であり、切り替え期間の次の期間が第２の期間である。第１の期間では、第２のデジタルフィルター係数を用いた第２の駆動周波数によるデジタルフィルター処理を行う。切り替え期間では、第１のデジタルフィルター係数を用いた第１の駆動周波数によるデジタルフィルター処理と第２のデジタルフィルター係数を用いた第２の駆動周波数によるデジタルフィルター処理の両方を行う。第２の期間では、第１のデジタルフィルター係数を用いた第１の駆動周波数によるデジタルフィルター処理を行う。

これにより、図１６及び図１７を用いて詳述したような、高周波から低周波へのスムーズな切り替え処理が可能になる。なお、図１６において、第１の期間とはＡ３のタイミングの前の期間、切り替え期間とはＡ３からＡ１の間の期間、第２の期間とはＡ１以降の期間に対応する。

また、デジタルフィルター処理の駆動周波数を、低周波の第１の駆動周波数から、高周波の第２の駆動周波数に切り替える場合に、デジタルフィルター演算部２２１は、切り替えタイミングにおける第１のデジタルフィルター処理の出力値である第１の出力値と、過去タイミングにおける第１のデジタルフィルター処理の出力値である第２の出力値に基づいて、切り替えタイミングと過去タイミングの間のタイミングにおける補間値を求める補間処理を行ってもよい。そして、補間処理によって求めた補間値に基づいて、切り替えタイミング以後の第２のデジタルフィルター処理を行う。

ここで、第ｉ（ｉ＝１ｏｒ２）のデジタルフィルター処理とは、第ｉのデジタルフィルター係数を用いた第ｉの駆動周波数によるデジタルフィルター処理のことである。また、切り替えタイミングとは、駆動周波数を切り替えるタイミングのことであり、図１８におけるＢ１に相当する。過去タイミングとは切り替えタイミングより過去のタイミングであり、Ｂ２やＢ３に対応する（狭義には第１のデジタルフィルター処理のサンプリングタイミングである）。補間値とは、第１の出力値及び第２の出力値から求められる値であり、狭義には、第２のデジタルフィルター処理のサンプリングタイミングであり、かつ、切り替えタイミングの１タイミング前及び２タイミング前のタイミング（Ｂ４及びＢ５に相当）におけるデジタルフィルター処理の出力値に対応する値のことである。

これにより、図１８及び図１９を用いて詳述したような、高周波から低周波へのスムーズな切り替え処理が可能になる。

５．第４の実施形態
第２の実施形態及び第３の実施形態では、駆動周波数の切り替えは、例えば外部Ｉ／Ｆ等を介したユーザーからの入力情報に基づいて行われる。それに対して本実施形態では、力覚センサーからのセンサー情報に基づいて出力フィルターの選択処理（狭義には駆動周波数の切り替え処理）を行う。

具体的には、センサー情報に対して低域通過フィルター処理を施した低域センサー情報と、高域通過フィルター処理を施した高域センサー情報を求める。そして、低域センサー情報と高域センサー情報から、信号全体に対する高周波成分の比率を算出する。高周波成分の比率が高い場合には、駆動周波数の高いデジタルフィルターを出力フィルターとして選択し、高周波成分の比率が低い場合には、駆動周波数の低いデジタルフィルターを選択する。

５．１構成
図２０に第４の実施形態にかかるロボット制御システムを含むロボットシステムの構成例を示す。第３の実施形態（図１５）と同様の箇所については詳細な説明は省略する。第３の実施形態と比較した場合、選択処理部５０、低域通過フィルター５１及び高域通過フィルター５２が追加されている点が異なる。なお、説明を簡略化するための帯域制限部２４−１、帯域制限部２４−２及び入力選択部２５が省略されているが、本実施形態においてこれらを用いる構成にしてもよい。その場合、選択処理部５０の処理結果は、入力選択部２５にも出力されることになる。

低域通過フィルター５１は力覚センサーからのセンサー情報に対して低域通過フィルター処理を施し低域センサー情報を出力する。同様に高域通過フィルター５２は力覚センサーからのセンサー情報に対して高域通過フィルター処理を施し高域センサー情報を出力する。

選択処理部５０は、低域センサー情報及び高域センサー情報に基づいて、信号全体に対する高周波成分の比率を求める。さらに求めた比率に基づいて、複数のデジタルフィルターの中から、出力フィルターとして用いるデジタルフィルターの選択処理を行い、選択処理の結果を係数選択部２２８と状態選択・処理部２２９に対して出力する。

係数選択部２２８は選択された出力フィルターに対応するデジタルフィルター係数を選択し、状態選択・処理部２２９は選択された出力フィルターに対応するフィルター状態に関する処理を行う。

５．２詳細な処理
本実施形態における処理の詳細は、図１７或いは図１９と同様である。Ｓ３０４或いはＳ４０４における駆動周波数の切り替え判定が、選択処理部５０で選択された出力フィルターが変化したか否かに基づいて行われることになる。

なお、以上では第３の実施形態と同様にデジタルフィルター演算部が１つの構成を用いて説明したが、これに限定されるものではない。例えば、第２の実施形態（図１３）のようにデジタルフィルター演算部が２つの構成をとってもよい。その場合、図１３に選択処理部５０、低域通過フィルター５１及び高域通過フィルター５２が追加されることになり、選択処理部５０の処理結果はデジタルフィルター出力選択部２２０に対して出力される。そして、デジタルフィルター出力選択部２２０では、選択処理部５０で選択されたデジタルフィルターに対応したデジタルフィルター演算部からの出力を選択し、デジタルフィルター部２２の出力とする。

以上の本実施形態では、デジタルフィルター部２２は、図２０に示したように、センサー情報の信号周波数帯域情報に基づいて、デジタルフィルターの駆動周波数を選択する選択処理部５０を含む。そして、デジタルフィルター部２２は、選択処理部５０において選択された駆動周波数でデジタルフィルター処理を行い、補正値を出力する。

ここで、駆動周波数の選択は、狭義には第１の駆動周波数と第２の駆動周波数のうちのいずれかの選択を行うものとするが、候補の駆動周波数が３つ以上であってもよい。

これにより、ユーザーが入力等を行うことなく、駆動周波数を選択することができるため、ユーザーにとって使いやすいシステムを提供することが可能になる。

また、選択処理部５０は、センサー情報に対して低域通過フィルター５１を施した低域センサー情報と、センサー情報に対して高域通過フィルター５２を施した高域センサー情報を、上述の信号周波数帯域情報として用いてもよい。つまり、低域センサー情報と高域センサー情報に基づいて駆動周波数を選択する。

これにより、フィルター処理という容易な処理により、駆動周波数の選択を行うことが可能になる。処理の具体例としては、低域センサー情報と高域センサー情報から、信号全体に対する高周波成分の比率を求めることが考えられる。高周波成分が多い場合には、駆動周波数も高くして信号の高周波成分に対応する。高周波成分が少なければ、駆動周波数も低くてよい。

以上、本発明を適用した４つの実施の形態１〜４およびその変形例について説明したが、本発明は、各実施の形態１〜４やその変形例そのままに限定されるものではなく、実施段階では、発明の要旨を逸脱しない範囲内で構成要素を変形して具体化することができる。また、上記した各実施の形態１〜４や変形例に開示されている複数の構成要素を適宜組み合わせることによって、種々の発明を形成することができる。例えば、各実施の形態１〜４や変形例に記載した全構成要素からいくつかの構成要素を削除してもよい。さらに、異なる実施の形態や変形例で説明した構成要素を適宜組み合わせてもよい。また、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。このように、発明の主旨を逸脱しない範囲内において種々の変形や応用が可能である。

ＡＬ、ＡＭ、ＡＲアーム、ＨＬ穴部、ＯＢ物体、ＳＦ表面、２０力制御部、２２デジタルフィルター部、２４、２４−１、２４−２帯域制限部、２５入力選択部、３０入力補正部、４０フォワードキネマティクス処理部、５０選択処理部、５１低域通過フィルター、５２高域通過フィルター、６０目標値出力部、６２軌道生成部、６４インバースキネマティクス処理部、８０ロボット制御部、８２―１〜８２―Ｎ駆動制御部（モーター制御部）、１００ロボット、１０２―１〜１０２―Ｎ駆動部、２２０デジタルフィルター出力選択部、２２１、２２１−１、２２１−２デジタルフィルター演算部、２２２デジタルフィルター係数出力部、２２３デジタルフィルター安定度判定部、２２４、２２４−１、２２４−２デジタルフィルター係数記憶部、２２５デジタルフィルター係数変換部、２２６応答特性入力部、２２７、２２７−１、２２７−２フィルター状態記憶部、２２８係数選択部、２２９状態選択・処理部、３００制御装置、３１０ロボット、３２０アーム、５１０力覚センサー、５３２姿勢補正部、５３４ハンド・ツール自重補正部、５３６運動方程式処理部

Claims

力覚センサーからのセンサー情報に基づいて、ロボットの目標軌道の補正値を出力する力制御部と、
前記目標軌道に対して前記補正値に基づく補正処理を行なって目標値を求め、求めた前記目標値を出力する目標値出力部と、
前記目標値に基づいて、前記ロボットのフィードバック制御を行うロボット制御部と、を備え、
前記力制御部は、
デジタルフィルター部を有し、
前記デジタルフィルター部は、
力制御における常微分方程式の各項の係数パラメーターに基づいてデジタルフィルター係数を出力するデジタルフィルター係数出力部、
前記デジタルフィルター係数及び前記センサー情報に基づいてデジタルフィルター処理を行い、前記常微分方程式の解を求めるデジタルフィルター演算部、及び
前記センサー情報に対して低域通過フィルター処理を施した低域センサー情報と、前記センサー情報に対して高域通過フィルター処理を施した高域センサー情報とに基づいて、第１の駆動周波数及び前記第１の駆動周波数に比べて高い周波数である第２の駆動周波数のいずれか一方の駆動周波数を、前記解の応答性を切り替えるために選択する選択処理部、を含み、
前記デジタルフィルター部は、前記解に基づいて前記補正値を出力することを特徴とするロボット制御システム。
請求項１において、
前記デジタルフィルター係数出力部は、
前記係数パラメーターを前記デジタルフィルター係数に変換する処理を行うデジタルフィルター係数変換部と、
前記デジタルフィルター係数変換部により変換された前記デジタルフィルター係数を記憶するデジタルフィルター係数記憶部と、を含み、
前記デジタルフィルター係数出力部は、前記デジタルフィルター係数記憶部に記憶された前記デジタルフィルター係数を出力することを特徴とするロボット制御システム。
請求項１乃至２のいずれかにおいて、
前記デジタルフィルター部は、
前記デジタルフィルター処理に用いられるデジタルフィルターの動作の安定度を判定するデジタルフィルター安定度判定部を含み、
前記デジタルフィルター係数出力部は、前記デジタルフィルター安定度判定部において、前記デジタルフィルターの前記動作が安定であると判定された場合に、前記デジタルフィルター係数を出力することを特徴とするロボット制御システム。
請求項１乃至３のいずれかにおいて、
前記第１の駆動周波数及び前記第２の駆動周波数のいずれか一方で、前記デジタルフィルター処理が行われる場合に、
前記デジタルフィルター係数出力部は、
前記第１の駆動周波数に対応する第１のデジタルフィルター係数及び前記第２の駆動周波数に対応する第２のデジタルフィルター係数の少なくとも一方を出力し、
前記デジタルフィルター演算部は、
前記第１のデジタルフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理及び前記第２のデジタルフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理の少なくとも一方の処理を行うことを特徴とするロボット制御システム。
請求項４において、
前記デジタルフィルター処理の前記駆動周波数を、前記第２の駆動周波数から前記第１の駆動周波数に切り替える場合に、
前記デジタルフィルター演算部は、
第１の期間では、前記第２のデジタルフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理を行い、
前記第１の期間の次の切り替え期間では、前記第１のデジタルフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理及び前記第２のデジタルフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理の両方を行い、
前記切り替え期間の次の第２の期間では、前記第１のデジタルフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理を行うことを特徴とするロボット制御システム。
請求項４において、
前記デジタルフィルター処理の前記駆動周波数を、前記第１の駆動周波数から前記第２の駆動周波数に切り替える場合に、
前記デジタルフィルター演算部は、
切り替えタイミングにおける前記第１のデジタルフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理の出力値である第１の出力値、及び前記切り替えタイミングよりも前のタイミングである過去タイミングにおける前記第１のデジタルフィルター係数を用いた第１の駆動周波数による前記デジタルフィルター処理の出力値である第２の出力値に基づいて、前記切り替えタイミングと前記過去タイミングとの間のタイミングにおける補間値を求める補間処理を行い、前記補間値に基づいて、前記第２のデジタルフィルター係数を用いた第２の駆動周波数による前記デジタルフィルター処理を行うことを特徴とするロボット制御システム。
請求項１乃至６のいずれかに記載のロボット制御システムと、
前記ロボットと、
を含むことを特徴とするロボットシステム。