(57)【特許請求の範囲】
【請求項１】
力制御におけるインピーダンス制御を行うデジタルフィルター部により、力覚センサーから出力される出力値に基づきロボットの制御に用いる補正値を求め、
前記補正値に非線形変換処理を行い変換処理後の補正値を求め、
前記変換処理後の補正値に基づき前記力覚センサーを備える前記ロボットの制御を行うことを特徴とするロボット制御装置。
【請求項２】
力覚センサーから出力される出力値に非線形変換処理を行い変換処理後の出力値を求め、
力制御におけるインピーダンス制御を行うデジタルフィルター部により、前記変換処理後の出力値に基づき、ロボットの制御に用いる補正値を求め、
前記補正値に基づいて前記力覚センサーを備えるロボットの制御を行うことを特徴とするロボット制御装置。
【請求項３】
請求項１又は２において、
前記補正値に対応する前記ロボットの仮想変位が第１の変位である時の外力に対する変位変化量を第１の変位変化量とし、前記仮想変位が第２の変位である時の前記変位変化量を第２の変位変化量とする場合に、
前記仮想変位が前記第２の変位よりも大きい前記第１の変位である場合は、前記第１の変位変化量が前記第２の変位変化量よりも大きくなるように前記非線形変換処理を行い、前記補正値を出力することを特徴とするロボット制御装置。
【請求項４】
請求項１又は２において、
前記補正値に対応する前記ロボットの仮想変位が第１の変位である時の外力に対する変位変化量を第１の変位変化量とし、前記仮想変位が第２の変位である時の前記変位変化量を第２の変位変化量とする場合に、
前記仮想変位が前記第２の変位よりも大きい前記第１の変位である場合は、前記第１の変位変化量が前記第２の変位変化量よりも小さくなるように前記非線形変換処理を行い、前記補正値を出力することを特徴とするロボット制御装置。
【請求項５】
請求項３又は４において、
前記仮想変位の方向が第１の方向である場合は、第１の力制御を行い、
前記仮想変位の方向が前記第１の方向と反対方向である第２の方向である場合は、第１の力制御と異なる第２の力制御を行うことを特徴とするロボット制御装置。
【請求項６】
請求項１乃至５のいずれかにおいて、
シグモイド関数を用いて前記補正値又は前記力覚センサーから出力される前記出力値の前記非線形変換処理を行うことを特徴とするロボット制御装置。
【請求項７】
請求項１乃至６のいずれかにおいて、
前記補正値を求める前記デジタルフィルター部の動作の安定度を判定し、
前記デジタルフィルター部の前記動作が安定であると判定された場合に、力制御における前記インピーダンス制御を行って、前記補正値を求めることを特徴とするロボット制御装置。
【請求項８】
請求項１乃至７のいずれかにおいて、
前記インピーダンス制御は、
仮想質量項、仮想粘性項及び仮想弾性項を係数パラメーターとする運動方程式の解を求めることであることを特徴とするロボット制御装置。
【請求項９】
請求項１乃至８のいずれかの前記ロボット制御装置と、
前記ロボット制御装置に制御される前記ロボットと、
を含むことを特徴とするロボットシステム。
【請求項１０】
力制御におけるインピーダンス制御を行うデジタルフィルター部により、力覚センサーから出力される出力値に基づいて、ロボットの目標軌道の補正処理に用いる補正値を求め、
前記補正値に非線形変換処理を行い変換処理後の補正値を求め、
前記変換処理後の補正値に基づいて、前記ロボットの前記目標軌道に対する前記補正処理を行って目標値を求め、
前記目標値に基づいて、前記ロボットのフィードバック制御を行うことを特徴とするロボット制御方法。
【請求項１１】
力覚センサーから出力される出力値に非線形変換処理を行い変換処理後の出力値を求め、
力制御におけるインピーダンス制御を行うデジタルフィルター部により、前記変換処理後の出力値に基づいて、ロボットの制御に用いる補正値を求め、
前記補正値に基づいて、ロボットの目標軌道に対する補正処理を行って目標値を求め、
前記目標値に基づいて、前記ロボットのフィードバック制御を行うことを特徴とするロボット制御方法。
【請求項１２】
請求項１乃至８のいずれかのロボット制御装置によって制御されることを特徴とするロボット。
【請求項１３】
力覚センサーを備え、
力制御におけるインピーダンス制御を行うデジタルフィルター部により、前記力覚センサーから出力される出力値に基づきロボットの制御に用いる補正値を求め、
前記補正値に非線形変換処理を行い変換処理後の補正値を求め、
前記変換処理後の補正値に基づき制御を行うことを特徴とするロボット。
【請求項１４】
力覚センサーを備え、
前記力覚センサーから出力される出力値に非線形変換処理を行い変換処理後の出力値を求め、
力制御におけるインピーダンス制御を行うデジタルフィルター部により、前記変換処理後の出力値に基づき、ロボットの制御に用いる補正値を求め、
前記補正値に基づいて制御を行うことを特徴とするロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置、ロボットシステム、ロボット制御方法及びロボット等に関係する。
【背景技術】
【０００２】
マニピュレーター等のロボットを用いて行う作業において、種々の拘束条件、例えば物体との接触を伴う作業がある。このような場合においては、位置の制御に加えて、力の制御が要求される場合が多い。例えば、物体の表面をなぞる場合、１つの物体を他の物体に嵌め合わせる場合、柔軟物を破壊しないように把持する場合などにおいては、単なる位置の制御に加えて、物体からの反力に対応した動きが必要となる。
【０００３】
ロボットにおいて力制御を行う代表的な手法としては、インピーダンス制御と呼ばれる手法がある。インピーダンス制御とは、ロボットを、その実際の質量や粘性特性や弾性特性に関わらず、あたかも作業に適したそれらの値を持つかのように動作させる制御手法である。これは、ロボットに取り付けられた力覚センサーなどから得られる力情報に基づいて運動方程式を解き、ロボットをその解に従って動作させる制御手法である。この運動方程式を適切に設定することにより、マニピュレーター等のロボットを、あたかも所定の質量、粘性、弾性を持っているかのごとく動作させることが可能になる。
【０００４】
なお、インピーダンス制御においては、ロボット等に所望の特性（質量・粘性特性・弾性特性）を持つかのように振る舞わせるために、当該特性に対応する係数パラメーターを用いた常微分方程式（２階の線形常微分方程式である運動方程式）を解く必要がある。常微分方程式を解く手法は種々知られているが、Ｒｕｎｇｅ−Ｋｕｔｔａ法やＮｅｗｔｏｎ法等が用いられる。
【０００５】
このようなインピーダンス制御や力制御に関する従来技術としては、特許文献１又は特許文献２に開示される技術が知られている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平１０−１２８６８５号公報
【特許文献２】特開２０１１−８３６０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
インピーダンス制御においては、例えば仮想的なバネの伸び縮み（仮想変位）として力を指定する。
【０００８】
しかし、大きな力を必要とする場合、大きな仮想変位を指定すると、その力に対する外力が無くなった時に、マニピュレーターは指定された仮想変位に対応する非常に大きな動きをする。このような大きな動きは、実用上、安全上、非常に問題となる。
【０００９】
また、大きな仮想変位を指定することにより、原理的には非常に大きな力（外力）を発生させることが可能であるが、発生させた力によりロボットの作業対象物であるワークを破壊してしまう恐れがあるなどの問題もある。
【００１０】
これらは仮想変位に対して線形な出力をするインピーダンス制御を行うために生じる問題である。前述した特許文献１及び特許文献２では、仮想変位に対して非線形な出力をするインピーダンス制御を行う手法を開示している。
【００１１】
まず、特許文献１には、力制御における運動方程式の弾性項を仮想変位に対して非線形な関数とすることにより、非線形インピーダンス制御を行う手法が開示されている。
【００１２】
しかし、この技術においては、力制御に用いる運動方程式が安定であるかどうかの判断が非常に困難であるという問題がある。
【００１３】
特許文献２には、２つ（複数）の制御機構と２つの制御パラメーターを持ち、制御状態空間の中にスライディング面と呼ばれる超平面を想定し、制御状態がその超平面で分割される空間のどちらにあるかによって、２つ（複数)の制御機構と２つの制御パラメーターを切り替え、それによって、制御状態をスライディング面上に拘束し、制御系の安定性、収束性を保証する手法が開示されている。
【００１４】
しかし、この技術においては、スライディングモード制御系の設計が困難であり、実用性に欠けるという問題があった。
【００１５】
本発明の幾つかの態様によれば、非線形インピーダンス制御を行い、運動方程式の解の安定性の検証やハードウェアー化を容易にするロボット制御装置、ロボットシステム及びロボット制御方法等を提供することができる。
【課題を解決するための手段】
【００１６】
本発明の一態様は、力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正値を出力する力制御部と、前記目標軌道に対して前記補正値に基づく補正処理を行って目標値を求め、求めた前記目標値を出力する目標値出力部と、前記目標値に基づいて、前記ロボットのフィードバック制御を行うロボット制御部と、を含み、前記力制御部は、力制御における常微分方程式の解を、変換処理前の前記補正値として求めるインピーダンス処理部と、前記インピーダンス処理部から取得される前記変換処理前の前記補正値に対して非線形変換処理を行うことにより前記変換処理後の前記補正値を求め、求めた前記変換処理後の前記補正値を出力する非線形変換部と、を含むロボット制御装置に関係する。
【００１７】
本発明の一態様では、線形インピーダンス処理の出力値に対して、フィルター処理を行うことにより、非線形インピーダンス制御を実現することができる。また、線形なシステムの組み合わせを用いることにより、運動方程式の解の安定性の検証を行うことが可能となる。
【００１８】
また、本発明の他の態様では、力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正値を出力する力制御部と、前記目標軌道に対して前記補正値に基づく補正処理を行って目標値を求め、求めた前記目標値を出力する目標値出力部と、前記目標値に基づいて、前記ロボットのフィードバック制御を行うロボット制御部と、を含み、前記力制御部は、前記力覚センサーから取得される前記検出センサー値に対して非線形変換処理を行うことにより、変換処理後の前記検出センサー値を求める非線形変換部と、前記非線形変換部から取得される前記変換処理後の前記検出センサー値に基づいて、力制御における常微分方程式の解を前記補正値として求めるインピーダンス処理部と、を含むロボット制御装置に関係する。
【００１９】
本発明の他の態様では、線形インピーダンス処理の入力値に対して、フィルター処理を行うことにより、非線形インピーダンス制御を実現することができる。また、線形なシステムの組み合わせを用いることにより、運動方程式の解の安定性の検証を行うことが可能となる。
【００２０】
また、本発明の一態様では、前記補正値に対応する前記ロボットについての仮想的な変位が第１の変位である場合の外力に対する変位変化量を第１の変位変化量とし、前記仮想的な変位が第２の変位である場合の前記変位変化量を第２の変位変化量とする場合に、前記力制御部は、前記仮想的な変位が前記第２の変位よりも大きい前記第１の変位である場合に、前記第１の変位変化量が前記第２の変位変化量よりも大きくなるように前記非線形変換処理を行い、前記補正値を出力してもよい。
【００２１】
これにより、例えば仮想的な変位の絶対値が大きくなればなるほど、変位変化量が大きくなるような力制御を行うこと等が可能になる。
【００２２】
また、本発明の一態様では、前記補正値に対応する前記ロボットについての仮想的な変位が第１の変位である場合の外力に対する変位変化量を第１の変位変化量とし、前記仮想的な変位が第２の変位である場合の前記変位変化量を第２の変位変化量とする場合に、前記力制御部は、前記仮想的な変位が前記第２の変位よりも大きい前記第１の変位である場合に、前記第１の変位変化量が前記第２の変位変化量よりも小さくなるように前記非線形変換処理を行い、前記補正値を出力してもよい。
【００２３】
これにより、例えば仮想的な変位の絶対値が大きくなればなるほど、変位変化量が小さくなるような力制御を行うこと等が可能になる。
【００２４】
また、本発明の一態様では、前記力制御部は、前記補正値に対応する前記ロボットについての前記仮想的な変位の方向が第１の方向である場合には、第１の力制御を行い、前記仮想的な変位の方向が前記第１の方向と反対方向である第２の方向である場合には、第１の力制御と異なる第２の力制御を行ってもよい。
【００２５】
これにより、仮想的な変位の方向に基づいて、実行する力制御を切り替えること等が可能になる。
【００２６】
また、本発明の一態様では、前記非線形変換部は、前記インピーダンス処理部から取得される前記変換処理前の前記補正値又は前記力覚センサーから取得される前記検出センサー値に対して、シグモイド関数を用いて前記非線形変換処理を行ってもよい。
【００２７】
これにより、シグモイド関数を用いて非線形変換処理を行うこと等が可能になる。
【００２８】
また、本発明の一態様では、前記インピーダンス処理部は、力制御における前記常微分方程式の解を前記補正値として求めるデジタルフィルター部を有してもよい。
【００２９】
これにより、常微分方程式の解を求めるという力制御において必要な処理を、デジタルフィルターを用いて行うことが可能になるため、ハードウェアー化を容易にすること等が可能となる。
【００３０】
また、本発明の一態様では、前記力制御部は、前記補正値を求める前記デジタルフィルター部の動作の安定度を判定し、前記デジタルフィルター部の前記動作が安定であると判定された場合に、力制御における前記常微分方程式の解を前記補正値として求めてもよい。
【００３１】
これにより、デジタルフィルターの安定性を判定すること等が可能になる。
【００３２】
また、本発明の一態様では、前記常微分方程式は、仮想質量項、仮想粘性項及び仮想弾性項を係数パラメーターとする運動方程式であってもよい。
【００３３】
これにより、運動方程式の解を求めること等が可能になる。
【００３４】
また、本発明の他の態様では、前記ロボット制御装置と、前記目標値出力部から取得される前記目標値に基づいて、各部を動作させる前記ロボットと、を含むロボットシステムに関係する。
【００３５】
これにより、ロボット制御装置にとどまらず、本実施形態の処理を実行するロボットシステムを実現すること等が可能になる。
【００３６】
また、本発明の他の態様では、力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正処理に用いる変換処理前の前記補正値として、力制御における常微分方程式の解を求め、求めた前記変換処理前の補正値に対して非線形変換処理を行うことにより前記変換処理後の前記補正値を求め、求めた前記変換処理後の前記補正値に基づいて、前記ロボットの前記目標軌道に対する前記補正処理を行って目標値を求め、求めた前記目標値に基づいて、前記ロボットのフィードバック制御を行うことを特徴とするロボット制御方法に関係する。
【００３７】
また、本発明の他の態様では、力覚センサーから取得される検出センサー値に対して非線形変換処理を行うことにより、ロボットの目標軌道の補正処理に用いる変換処理後の前記検出センサー値を求め、求めた前記変換処理後の前記検出センサー値に基づいて、力制御における常微分方程式の解を前記補正値として求め、求めた前記補正値に基づいて、前記ロボットの前記目標軌道に対する前記補正処理を行って目標値を求め、求めた前記目標値に基づいて、前記ロボットのフィードバック制御を行うことを特徴とするロボット制御方法に関係する。
【００３８】
また、本発明の他の態様は、力覚センサーを有するロボットの制御を行うロボット制御方法であって、前記力覚センサーに加えられた外力の大きさと、前記外力が加えられた時の前記ロボットの変位の大きさとの関係が非線形になる力制御を行うことを特徴とするロボット制御方法に関係する。
【図面の簡単な説明】
【００３９】
【図１】ロボット制御装置及びロボットシステムの基本構成例。
【図２】図２（Ａ）、図２（Ｂ）は、ロボットシステムの一例。
【図３】図３（Ａ）〜図３（Ｃ）は、力制御についての説明図。
【図４】図４（Ａ）、図４（Ｂ）は、コンプライアンス制御についての説明図。
【図５】図５（Ａ）、図５（Ｂ）は、インピーダンス制御についての説明図。
【図６】図６（Ａ）〜図６（Ｃ）は、線形インピーダンス制御の問題点の説明図。
【図７】力覚フィードバックを含まない場合の制御系の基本構成例。
【図８】力覚フィードバックを含む場合の制御系の基本構成例。
【図９】運動方程式の解を求める際のデジタルフィルターの基本形。
【図１０】図１０（Ａ）〜図１０（Ｃ）は、系の安定性判定手法の説明図。
【図１１】デジタルフィルターを用いたロボット制御装置及びロボットシステムの基本構成例。
【図１２】図１２（Ａ）〜図１２（Ｃ）は、非線形インピーダンス制御の説明図。
【図１３】第１の実施形態の詳細なシステム構成例。
【図１４】第１の実施形態のデジタルフィルター。
【図１５】第１の実施形態のインピーダンスデジタルフィルター処理を説明するフローチャート。
【図１６】正弦波外力に対する線形インピーダンス制御応答例。
【図１７】正弦波外力に対する非線形インピーダンス制御応答例。
【図１８】正弦波外力に対する非線形・非対称インピーダンス制御応答例。
【図１９】第２の実施形態の詳細なシステム構成例。
【図２０】第２の実施形態のデジタルフィルター。
【図２１】第２の実施形態のインピーダンスデジタルフィルター処理を説明するフローチャート。
【図２２】図２２（Ａ）、図２２（Ｂ）は、目標軌道、補正値、目標値を求める具体的なシステム
【発明を実施するための形態】
【００４０】
以下、本実施形態について説明する。まず、本実施形態の概要を説明する。次に、第１の実施形態と第２の実施形態について、それぞれシステム構成例と詳細な処理について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また、本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。
【００４１】
１．概要
１．１  基本構成
本実施形態のロボット制御装置（マニピュレーター制御装置）及びこれを含むロボットシステムの構成例を図１に示す。なお、本実施形態のロボット制御装置、ロボットシステムは図１の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。
【００４２】
本実施形態のロボット制御装置は、力制御部２０、目標値出力部６０、ロボット制御部８０を含む。また本実施形態のロボットシステムは、このロボット制御装置と、ロボット１００（力覚センサー１０）により構成される。
【００４３】
目標値出力部６０は、ロボット（狭義にはマニピュレーター）のフィードバック制御の目標値を出力する。この目標値に基づいてロボット１００のフィードバック制御が実現される。多関節ロボット等を例に取れば、この目標値は、ロボットの関節角情報などである。ロボットの関節角情報は、例えばロボットのアームのリンク機構における各関節の角度（ジョイント軸とジョイント軸のなす角度）を表す情報である。
【００４４】
目標値出力部６０は、軌道生成部６２とインバースキネマティクス処理部６４を含むことができる。軌道生成部６２は、ロボットの軌道情報を出力する。軌道情報は、ロボットのエンドエフェクター部（エンドポイント）の位置情報（ｘ，ｙ，ｚ）と、各座標軸回りでの回転角度情報（ｕ，ｖ，ｗ）を含むことができる。インバースキネマティクス処理部６４は、軌道生成部６２からの軌道情報に基づいてインバースキネマティクス処理を行い、例えばロボットの関節角情報を目標値として出力する。インバースキネマティクス処理は、関節を有するロボットの動きを計算する処理であり、ロボットのエンドエフェクター部の位置姿勢などから関節角情報等を逆運動学により計算する処理である。
【００４５】
力制御部２０（狭義にはインピーダンス制御部）は、力覚センサー１０からのセンサー情報に基づいて力制御（力覚制御）を行って、目標値の補正値を出力する。更に具体的には、力制御部２０（インピーダンス制御部）は、力覚センサー１０からのセンサー情報（力情報、モーメント情報）に基づいてインピーダンス制御（或いはコンプライアンス制御）を行う。力制御は、例えば、従来の位置制御に対して、力のフィードバックが加わった制御である。インピーダンス制御は、外力に対するエンドエフェクター部（手先）の変位の生じやすさ（機械インピーダンス）を、制御により望ましい状態にする手法である。具体的には、ロボットのエンドエフェクター部に質量と粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した質量と粘性係数と弾性係数で物体に接触するようにする制御である。また力覚センサー１０は、ロボット１００が出している力の反力として受けている力やモーメントを検出するセンサーである。この力覚センサー１０は、通常、ロボット１００のアームの手首部分に取り付けられ、検出された力やモーメントは、センサー情報として、各種の力制御（インピーダンス制御）に用いられる。
【００４６】
ロボット制御部８０は、目標値出力部６０からの目標値に基づいて、ロボットのフィードバック制御を行う。具体的には、力制御部２０からの補正値に基づく補正処理の結果出力された目標値に基づいて、ロボットのフィードバック制御を行う。例えば目標値と、ロボット１００からのフィードバック信号に基づいて、ロボット１００のフィードバック制御を行う。例えばロボット制御部８０は、複数の駆動制御部８２―１〜８２―Ｎ（狭義にはモーター制御部）を含み、ロボット１００が有する駆動部１０２―１〜１０２―Ｎに対して、その制御信号を出力する。ここで駆動部１０２―１〜１０２―Ｎは、ロボット１００の各関節を動かすための駆動機構であり、例えばモーター等により実現される。
【００４７】
図２（Ａ）に本実施形態のロボット制御装置を含むロボットシステムの例を示す。このロボットシステムは、制御装置３００（情報処理装置）とロボット３１０（図１のロボット１００）とを含む。制御装置３００はロボット３１０の制御処理を行う。具体的には、動作シーケンス情報（シナリオ情報）に基づいてロボット３１０を動作させる制御を行う。ロボット３１０は、アーム３２０及びハンド（把持部）３３０を有する。そして制御装置３００からの動作指示にしたがって動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動したりするなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボットの姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。
【００４８】
本実施形態のロボット制御装置は、例えば図２（Ａ）の制御装置３００に設けられ、制御装置３００のハードウェアーやプログラムによりロボット制御装置が実現される。そして本実施形態のロボット制御装置によれば、制御装置３００などの制御ハードウェアーに対する性能要求を低減できると共に、ロボット３１０を高い応答性で動作させることが可能になる。
【００４９】
また、図２（Ａ）では、ロボット本体３１０（ロボット）と制御装置３００（ロボット制御装置）とが別体に構成されているが、本実施形態のロボットは図２（Ａ）の構成に限定されず、図２（Ｂ）のようにロボット本体３１０と制御装置３００とが一体に構成されていてもよい。具体的には図２（Ｂ）に示したように、ロボットはロボット本体３１０（アーム３２０及びハンド３３０を有する）、及びロボット本体３１０を支えるベースユニット部を含み、当該ベースユニット部に制御装置３００が格納される。図２（Ｂ）のロボットでは、ベースユニット部に車輪等が設けられ、ロボット全体が移動可能な構成となっている。なお、図２（Ａ）は単腕型の例であるが、ロボットは図２（Ｂ）に示すように双腕型等の多腕型のロボットであってもよい。なお、ロボットの移動は人手で行われてもよいし、車輪を駆動するモーターを設け、当該モーターを制御装置３００により制御することで行なってもよい。
【００５０】
１．２  力制御・インピーダンス制御
次に、力制御、インピーダンス制御（コンプライアンス制御）の概要について説明する。
【００５１】
図３（Ａ）は、ロボットの左のアームＡＬ、右のアームＡＲで物体ＯＢを挟んで移動している様子を示している。例えば、位置制御だけでは、物体を落としたり、破壊したりしてしまうおそれがある。力制御によれば、柔軟な物体や脆い物体を、図３（Ａ）のように両側から適切な力で挟んで移動させることが可能になる。
【００５２】
また、力制御によれば、図３（Ｂ）に示すように、不確実性のある物体の表面ＳＦをアームＡＭ等でなぞることが可能になる。このような制御は位置制御だけでは実現不能である。また、力制御によれば、図３（Ｃ）に示すように、粗い位置決めの後に、探って位置合わせをして、物体ＯＢを穴部ＨＬにはめ込むことも可能になる。
【００５３】
しかしながら、バネなどの実際の機械部品による力制御では、用途が限られるという問題がある。また、このような機械部品による力制御では、特性の動的な切り替えが困難である。
【００５４】
一方、モーターのトルクを制御するトルク制御は簡単であるが、位置精度が悪くなるという問題がある。また、異常時に衝突などの問題が生じる。例えば図３（Ａ）において、異常事態が起きて、物体ＯＢを落としてしまった場合に、トルク制御では、釣合うべき反力が無くなるため、左右のアームＡＬ、ＡＲが衝突してしまうなどの問題が生じる。
【００５５】
これに対して、インピーダンス制御（コンプライアンス制御）は、制御が複雑であるものの、汎用性や安全性が高いという利点がある。
【００５６】
図４（Ａ）、図４（Ｂ）は、インピーダンス制御の１つであるコンプライアンス制御を説明する図である。コンプライアンスはバネ定数の逆数を意味し、バネ定数が硬さを表すのに対して、コンプライアンスは柔らかさを意味する。ロボットと環境との間に相互作用が働くときに、機械的柔軟性であるコンプライアンスを与える制御をコンプライアンス制御と呼ぶ。
【００５７】
例えば図４（Ａ）では、ロボットのアームＡＭには力覚センサーＳＥが取り付けられている。このロボットのアームＡＭは、力覚センサーＳＥで得られたセンサー情報（力・トルク情報）に応じて姿勢が変わるようにプログラムされている。具体的には、図４（Ａ）のＡ１に示す仮想的なバネが、あたかもアームＡＭの先端に取り付けられているかのようにロボットを制御する。
【００５８】
例えばＡ１に示すバネのバネ定数が１００Ｋｇ／ｍであったとする。これを図４（Ｂ）のＡ２に示すように５Ｋｇの力で押せば、Ａ３に示すようにバネは５ｃｍだけ縮む。逆に言えば、５ｃｍだけ縮んでいれば、５Ｋｇの力で押されているといえる。つまり、力情報と位置情報とが線形に対応づけられている。
【００５９】
コンプライアンス制御では、このＡ１に示す仮想的なバネがアームＡＭの先端に取り付けられているかのような制御が行われる。具体的には、ロボットは、力覚センサーＳＥの入力に応答して動作し、Ａ２に示す５Ｋｇの加重に対して、Ａ３に示すように５ｃｍだけ後退するように制御され、力情報に対応して位置情報が変化するように制御される。
【００６０】
このような単純なコンプライアンス制御では時間項を含まないが、時間項を含み、その２次の項までを考慮した制御が、インピーダンス制御である。具体的には、２次の項は質量項であり、１次の項は粘性項であり、インピーダンス制御のモデルは下式（１）に示すような運動方程式で表すことができる。
【００６１】
【数１】
0005966372.tif 000002
【００６２】
上式（１）において、ｍは質量、μは粘性係数、ｋは弾性係数、ｆは力、ｘは目標位置からの変位である。またｘの１次微分、２次微分は、各々、速度、加速度に対応する。インピーダンス制御では、上式（１）の特性をアームの先端であるエンドエフェクター部に持たせるための制御系を構成する。即ち上式（１）で表される仮想質量、仮想粘性係数、仮想弾性係数を、あたかもアームの先端が持っているかのように制御を行う。
【００６３】
このように、インピーダンス制御は、アームの先端の質量に粘性要素と弾性要素が各方向に接続されるモデルにおいて、目的として設定された粘性係数と弾性係数で物体に接触するようにする制御である。
【００６４】
例えば図５（Ａ）に示すように、ロボットのアームＡＬ、ＡＲで物体ＯＢをつかんで、軌道ＴＲに沿って移動させる制御を考える。この場合に、軌道ＴＲＬは、物体ＯＢの左側の内側に設定された点ＰＬが通る軌道であり、インピーダンス制御を想定して決定された仮想的な左手の軌道である。また軌道ＴＲＲは、物体ＯＢの右側の内側に設定された点ＰＲが通る軌道であり、インピーダンス制御を想定して決定された仮想的な右手の軌道である。この場合に、アームＡＬは、アームＡＬの先端と点ＰＬの距離差に応じた力が発生するように制御される。またアームＡＲは、アームＡＲの先端と点ＰＲの距離差に応じた力が発生するように制御される。このようにすれば、物体ＯＢを柔らかくつかみながら移動させるインピーダンス制御を実現できる。そして、インピーダンス制御では、図５（Ａ）のＢ１に示すように物体ＯＢが落下する事態が生じたとしても、アームＡＬ、ＡＲは、Ｂ２、Ｂ３に示すように、その先端が点ＰＬ、ＰＲの位置で止まるように制御される。即ち、仮想的な軌道が衝突軌道でなければ、アームＡＬ、ＡＲが衝突するのを防止できる。
【００６５】
また図５（Ｂ）のように、物体の表面ＳＦをなぞるように制御する場合にも、インピーダンス制御では、アームＡＭの先端に対して、仮想的な軌道ＴＲＶＡと先端との距離差ＤＦに応じた力が働くように制御される。従って、アームＡＭを、力を加えながら表面ＳＦをなぞるような制御が可能になる。
【００６６】
これらの例は線形インピーダンス制御を行っている様子を示している。そのため、例えば、図５（Ａ）の例で、より強い力をかけなければ物体ＯＢを把持できないという場合には、ＰＬをより右手ＡＲ側に、ＰＲをより左手ＡＬ側に設定する必要がある。この場合に、物体ＯＢが落下してしまうと、ロボットの右手ＡＲは点ＰＲに、左手ＡＬは点ＰＬにそのまま移動してしまい、両手が衝突してしまう恐れがある。このように、線形インピーダンス制御では、仮想変位に対して、線形な解（力、外力）が定まるため、点ＰＬ及びＰＲを図５（Ａ）の位置に設定したまま、物体ＯＢにかける力を強くすることができない。
【００６７】
同様に図６（Ａ）〜図６（Ｃ）に、ロボットの指先が、凹凸のある物体の表面に沿って動く場合、あるいは、表面の凹凸を削りながら動く場合の例を示す。
【００６８】
図６（Ａ）の凸ＯＢＳ１を削る場合のようにあまり力を必要としない場合には、ロボットの指先の目標位置をＰＤ１とするような小さな仮想変位で、凸ＯＢＳ１を削るために必要な力を発生させることができる。
【００６９】
しかし、図６（Ａ）と同じインピーダンス制御パラメーターのみを用いて、図６（Ｂ）の凸ＯＢＳ２を削る場合のように大きな力を必要とする場合には、ロボットの指先の目標位置をＰＤ２とするような大きな仮想変位を発生させる必要がある。
【００７０】
この時、図６（Ｃ）のように反力がなくなると、ロボットのマニピュレーターは大きな動きを起こしてしまう。これは人間の作業でも起こりえることではあるが、実際には、人間は、対象物に与える力、仮想変位によって、力加減を変化させている。
【００７１】
そこで、後述する実施形態では、仮想変位に対して非線形な解（力）を出力して、図５（Ａ）の衝突や、図６（Ｃ）の大きな動きを回避する手法を用いる。
【００７２】
１．３  制御系の構成
ここで、力覚フィードバックを含まない場合の制御系の基本構成例を図７に示す。
【００７３】
軌道生成部５６２は、軌道情報ｐ（ｘｙｚｕｖｗ）を生成してインバースキネマティクス処理部５６４に出力する。ここで軌道情報ｐは、例えばアームの先端（エンドエフェクター部）の位置情報（ｘｙｚ）と各軸回りの回転情報（ｕｖｗ）を含む。そしてインバースキネマティクス処理部５６４は、この軌道情報ｐに基づいてインバースキネマティクス処理を行って、目標値である各関節の関節角θを生成して出力する。そして、この関節角θに基づいて、モーター制御を行うことで、ロボットのアームの動作制御が行われる。この場合に図７のモーター（Ｍ）の制御は、公知のＰＩＤ制御により実現されている。このＰＩＤ制御は公知の技術であるため、ここでは詳細な説明を省略する。
【００７４】
図７において、軌道生成部５６２とインバースキネマティクス処理部５６４とにより目標値出力部が構成される。この目標値出力部の処理は、ロボットの全体的な処理になる。一方、後段のモーター制御は、関節毎の制御になる。
【００７５】
図８に、力覚フィードバックを含む場合の制御系の基本構成例を示す。図８では、図７に対して、更に、力覚センサー５１０と、姿勢補正部５３２と、ハンド・ツール自重補正部５３４と、運動方程式処理部５３６と、フォワードキネマティクス処理部５４０が設けられている。
【００７６】
図８では、力覚センサー５１０からのセンサー情報を受けて、姿勢補正部５３２がセンサーの姿勢補正を行い、ハンド・ツール自重補正部５３４がハンド・ツール自重補正を行う。そして、運動方程式処理部５３６が、前述の式（１）に示すような運動方程式の解を求める処理を行い、補正値Δｐを出力する。この補正値Δｐにより軌道情報ｐが補正されることで、目標値である関節角θの補正処理が行われる。またフォワードキネマティクス処理部５４０は、フォワードキネマティクス処理を行って、ロボットの軌道情報ｐ’を求めて軌道生成部５６２にフィードバックする。また姿勢補正部５３２、ハンド・ツール自重補正部５３４に対して姿勢を特定するための情報を出力する。なお、ロボットの軌道情報ｐ’の軌道生成部５６２へのフィードバックは、ｐ’に基づいた軌道の修正処理等を行うためのものであり、当該修正処理等を行わないのであれば、フィードバックは必ずしも必要ない。
【００７７】
ハンド・ツール自重補正部５３４では、ハンド・ツール自重補正が行われ、姿勢補正部５３２では姿勢補正が行われる。ここでハンド・ツール自重補正は、ロボットのハンドの自重や、ハンドがつかむツールの自重による影響を、力覚センサー１０からのセンサー情報（力情報）から相殺するための補正処理である。また姿勢補正は、力覚センサー１０の姿勢による影響を、センサー情報（力情報）から相殺するための補正処理である。これらのハンド・ツール自重補正及び姿勢補正は、例えば下式（２）のように表すことができる。
【００７８】
【数２】
0005966372.tif 000003
【００７９】
上式（２）において、Ｆｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗは力覚センサー１０からのセンサー情報である力情報、トルク情報である。またＢｘ，Ｂｙ，Ｂｚ、Ｂｕ，Ｂｖ，Ｂｗはバイアス項である。そして、補正後のセンサー情報（力情報、トルク情報）であるｆｘ，ｆｙ，ｆｚ、ｆｕ，ｆｖ，ｆｗが運動方程式処理部５３６に入力される。なお、データには固定値があるため、実質的な補正係数は６×７＝４２個となる。これらのハンド・ツール自重補正及び姿勢補正は公知の補正処理であるため、詳しい説明は省略する。
【００８０】
１．４  デジタルフィルター処理
図８の運動方程式処理部５３６では運動方程式（広義には常微分方程式）の解を求める必要がある。従来、運動方程式の解を求めるにはＮｅｗｔｏｎ法やＲｕｎｇｅ−Ｋｕｔｔａ法等が用いられていた。しかし、これらの手法はハードウェアー化に適さず、安定性の判定も難しい。さらに応答性の切り替えに対応することが困難であるという問題もある。
【００８１】
そこで本出願人は、上述の３つの問題に対応するために、常微分方程式を解く手法としてデジタルフィルターを用いる。
【００８２】
１．４．１  運動方程式のデジタルフィルターを用いた解法
運動方程式は上述した式（１）の形で表される。運動方程式は線形常微分方程式であるため、インパルス入力に対する解であるインパルスレスポンスが求められれば、インパルスレスポンスと外力項との畳み込みにより、任意の外力項に対する解を得ることができる。
【００８３】
ここで、運動方程式の解を求めるというステップを、力覚センサーのセンサー情報の入力に対して解（例えば位置情報）を出力するフィルターであると捉えるならば、上式（１）の形から、２極のアナログフィルターとして考えることができる。
【００８４】
つまり、運動方程式の解はアナログフィルターの出力として求めることができるのであるから、当該アナログフィルターをデジタルフィルター化することで、デジタルフィルターを用いて運動方程式を解くことが可能になる。
【００８５】
アナログフィルターのデジタルフィルター化の手法は種々知られているが、例えばImpulse Invariance法を用いればよい。これは、アナログフィルターのインパルスレスポンスを離散時間Ｔでサンプルした値と同じインパルスレスポンスを与えるデジタルフィルターを考える手法である。Impulse Invariance法については公知の手法であるため詳細な説明は省略する。
【００８６】
以上の結果、常微分方程式の解はデジタルフィルターの出力として求めることが可能になる。運動方程式であれば、図９に示したように２極のデジタルフィルターとなる。図９において、ｄは１サンプル分の遅延であり、Ｃ０、Ｃ１、Ｃ２はデジタルフィルターの係数（デジタルフィルターパラメーター）である。なお、図９のデジタルフィルターの入力値Ｆと出力値Ｙｎの関係は式（３）のように表すことができる。
【００８７】
【数３】
0005966372.tif 000004
【００８８】
デジタルフィルターによる処理であれば、ハードウェアー化は容易であるし、後述するように安定性の判定も容易である。また、デジタルフィルターの係数を切り替えれば、特性（柔らかく動かすか固く動かすか等）を切り替えることもできるし、フィルター駆動周波数を切り替えて解の応答性を切り替えることもできる。
【００８９】
１．４．２  デジタルフィルターの安定度判定
インピーダンス制御においては、運動方程式の質量項（ｍ）、粘性項（μ）及び弾性項（ｋ）の設定次第では、安定でない系ができてしまう可能性がある。極端な例で言えば、ロボットに一度力を加えたら、その後一切触れていないにもかかわらずロボットの振動が継続するような、発振する系にもなりうるということである。そのような安定性（安定度）の低い系は実用上好ましくないため、運動方程式に関する系の安定性を判定し、安定でない場合には何らかの対処をする必要がある。
【００９０】
しかし、上述のＮｅｗｔｏｎ法やＲｕｎｇｅ−Ｋｕｔｔａ法等では、運動方程式の解を求めることはできても、安定性の判定は行えない。そのため、解を求める処理とは別に安定性を判定する処理が必要となるが、一般的に安定性の判定処理は容易でないことが知られている。
【００９１】
本実施形態の手法では、運動方程式を、デジタルフィルターを用いて処理するため、運動方程式に関する系の安定性の判定とは、対応するデジタルフィルターの安定性の判定に他ならない。デジタルフィルターの安定性の判定は、容易に行うことができ、極が単位円の中にあるか否かを判定すればよい。
【００９２】
具体例を図１０（Ａ）〜図１０（Ｃ）に示す。これらはどれも極が単位円の中に収まっている例であるが、極が単位円の外にある場合には安定でないと判定する。また、図１０（Ｃ）のようにある程度、単位円の円周上から内側に離れた位置に極がある場合には、特に問題はない。しかし、図１０（Ａ）のように単位円にかなり近い位置に極（なお、図１０（Ａ）は重根ではなく限りなく近い位置に２つの極がある例である）がある場合には、注意が必要である。なぜなら、デジタルフィルターの実装手法によっては、設計値に対して誤差が生ずるおそれがあるからである。当該誤差が極の位置を単位円の外側方向に移動させる要因となった場合、図１０（Ａ）のような安定性に余裕のないデジタルフィルターは、設計上安定であるにもかかわらず、実装時には安定でない動作をする可能性があるため、何らかの対処が必要となる。
【００９３】
１．４．３  デジタルフィルターを用いた場合の構成例
図１１にデジタルフィルターを用いて運動方程式の解を求める場合の、ロボット制御装置及びそれを含むロボットシステムの構成例を示す。なお本実施形態のロボット制御装置、ロボットシステムは図１１の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。
【００９４】
力覚センサー１０、目標値出力部６０、ロボット制御部８０及びロボット１００については図１と同様であるため、詳細な説明は省略する。
【００９５】
力制御部２０は、デジタルフィルター部２２を含む。デジタルフィルター部２２は、力覚センサーからのセンサー情報（センサー情報に対して補正処理や帯域制限処理が施された情報も含む）に対してデジタルフィルター処理を行って、出力値を補正値として目標値出力部６０に出力する。また、力制御部２０はセンサー情報に対して帯域制限処理を行う帯域制限部２５を含んでもよい。
【００９６】
デジタルフィルター部２２は、デジタルフィルター演算部２２１と、デジタルフィルター係数出力部２２２と、デジタルフィルター安定度判定部２２３を含む。デジタルフィルター演算部２２１は、センサー情報とデジタルフィルター係数に基づいて、デジタルフィルター処理を行って運動方程式の解を求める。デジタルフィルター係数出力部２２２は、運動方程式の係数パラメーター（質量項ｍ、粘性項μ、弾性項ｋ及び駆動周期Ｔ）に基づいて、デジタルフィルター係数を求め、デジタルフィルター演算部２２１とデジタルフィルター安定度判定部２２３に出力する。デジタルフィルター安定度判定部２２３は、デジタルフィルター係数に基づいて、当該デジタルフィルターの安定性の判定を行う。
【００９７】
デジタルフィルター係数出力部２２２は、デジタルフィルター係数記憶部２２４と、デジタルフィルター係数変換部２２５を含んでもよい。デジタルフィルター係数変換部２２５は、運動方程式の係数パラメーターをデジタルフィルター係数に変換する。デジタルフィルター係数記憶部２２４は、変換されたデジタルフィルター係数を記憶する。デジタルフィルター係数記憶部２２４に複数のデジタルフィルター係数を記憶しておけば、出力するデジタルフィルター係数を切り替えることで、ロボットの動作特性や、解の応答性を切り替えることが可能になる。
【００９８】
１．５  非線形インピーダンス制御
次に、図１２（Ａ）〜図１２（Ｃ）を用いて、以下で説明する第１の実施形態と第２の実施形態において実現する非線形インピーダンス制御の概要を説明する。
【００９９】
第１の実施形態及び第２の実施形態では、力覚センサーに加えられた外力の大きさと、外力が加えられた時のロボットの変位の大きさとの関係が非線形になる力制御を行う。
【０１００】
具体的に、まず、図１２（Ａ）には、線形インピーダンス制御の例を示す。図１２（Ａ）のグラフは、外力が大きくなるほど、変位も比例して大きくなる様子を示している。前述した図５（Ａ）、図５（Ｂ）や図６（Ａ）〜図６（Ｃ）における外力と変位にはこのような関係が成り立っている。
【０１０１】
次に、図１２（Ｂ）には所定の閾値以上外力が大きくなった場合には、変位が大きくなりにくくする非線形インピーダンス制御の例を示す。すなわち、変位が大きくなるにつれ、発生させる外力が大きくなりやすくする制御である。このような非線形インピーダンス制御を行えば、例えば図６（Ａ）〜図６（Ｃ）のように大きな力をかけなければ削れない凸ＯＢＳ２がある場合でも、変位を大きくすることなく（目標位置ＰＤ２を遠くに設定することなく）、必要な力を発生させることができる。その結果、凸ＯＢＳ２を削った後でもマニピュレーターが大きく動きすぎることを防ぐことができる。なお、図１２（Ｂ）のグラフは、外力と変位の関係を一意にするためには傾斜が必要であるため、外力をいくら大きくしても傾きを０にはしない。
【０１０２】
さらに、図１２（Ｃ）には所定の閾値以上変位が大きくなった場合には、外力（抗力）が大きくなりにくくする非線形インピーダンス制御の例を示す。言い換えれば、外力が大きい場合に変位を非常に大きく非線形インピーダンス制御である。このような非線形インピーダンス制御を行えば、例えば図５（Ａ）のように物体ＯＢを把持する場合に、物体ＯＢをつぶさずに変位を大きくすることが可能となる。
【０１０３】
以下、図１１の構成を基本とし、図１２（Ｂ）及び図１２（Ｃ）のような非線形インピーダンス制御を実現する第１の実施形態と第２の実施形態について説明する。第１の実施形態は、線形デジタルフィルター処理を行なった後に、出力値に対してフィルター処理を行い、非線形インピーダンス制御を実現する例である。第２の実施形態は、デジタルフィルターの入力値に対してフィルター処理を行った後、線形デジタルフィルター処理を行ない、非線形インピーダンス制御を実現する例である。
【０１０４】
２．第１の実施形態
２．１  構成
図１３に第１の実施形態に係るロボット制御装置の構成例を示す。
【０１０５】
力覚センサー１０、目標値出力部６０（軌道生成部６２、インバースキネマティクス処理部６４）、ロボット制御部８０（モーター制御部８２−１〜モーター制御部８２−Ｎ）等については図１と同様であるため詳細な説明は省略する。また、入力補正部３０は検出センサー値（センサー情報）に対して補正処理を行うもので、例えば図８の姿勢補正部５３２やハンド・ツール自重補正部５３４等を含んでもよい。フォワードキネマティクス処理部４０は、図８のフォワードキネマティクス処理部５４０に対応し、フォワードキネマティクス処理の結果を入力補正部３０に出力するとともに、必要に応じて軌道生成部６２にも出力してもよい。
【０１０６】
また、ロボット制御装置の力制御部２０は、インピーダンス処理部２１と、制御パラメーター記憶部２４と、非線形変換部２９と、を含む。なお、本実施形態の力制御部２０は図１３の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。
【０１０７】
また、図１３のインピーダンス処理部２１は、図１１のデジタルフィルター演算部２２１に対応し、動作も同様のため、説明を省略する。
【０１０８】
さらに、図１３の制御パラメーター記憶部２４は、図１１のデジタルフィルター係数記憶部２２４に対応し、制御パラメーターを記憶する。ここで、制御パラメーターとは、後述する運動方程式の係数パラメーターであっても良いし、デジタルフィルターのパラメーターであってもよい。さらに、制御パラメーターとして、オフセットパラメーターを記憶してもよい。制御パラメーター記憶部の機能は、ＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現でき、実際には一つのメモリー等により構成されていても良いし、複数のメモリー等により構成されていても良い。
【０１０９】
そして、非線形変換部２９は、インピーダンス処理部の出力値に対して、フィルター処理を行って、補正値を出力する。フィルター処理の前後の補正値（出力値、応答）については、図１７を用いて後述する。
【０１１０】
ここで、第１の実施形態におけるデジタルフィルターは図１４のようになる。図１４に示すデジタルフィルターでは、式（４）に表すように、図９のデジタルフィルターの出力値Ｙｎ（式（３）参照）に対して、フィルター処理関数Ｇによりフィルター処理を行なった値（Ｏｕｔｐｕｔ）を補正値（出力値）として求める。式（４）では、変位を制御する場合や力を制御する場合など目的に合わせた関数Ｇを用いる。
【０１１１】
【数４】
0005966372.tif 000005
【０１１２】
図１４に示したデジタルフィルターによる処理であれば、ハードウェアー化は容易であるし、安定性の判定も容易である。
【０１１３】
２．２  詳細な処理
次に図１５のフローチャートを用いて、本実施形態における力制御部が行うデジタルフィルター処理の流れを説明する。また、図１６に、正弦波外力に対する線形インピーダンス制御の応答例を示し、図１７に正弦波外力に対する非線形インピーダンス制御の応答例を示す。ここでは、先に図１６の説明を行い、次に図１５のフローチャートの説明を交えながら、図１７の説明を行う。
【０１１４】
まず図１６には、縦軸を任意目盛、横軸を時間（秒）として、外力Ｆの曲線が表されている。そして、図１６のグラフには、運動方程式の係数パラメーター（仮想質量、仮想粘性、仮想剛性）の設定を同図に示すＰＲ１とＰＲ２とした時の外力Ｆに対する線形インピーダンス制御の応答をそれぞれ示している。また、駆動周期は１２５マイクロ秒（８ｋサンプル／秒）であり、マニピュレーターの電流制御ループの周期と同じであるものとし、外力は１秒周期の正弦波であるとする。
【０１１５】
図１６のＰＲ１とＰＲ２では、仮想質量と仮想粘性は同じであるが、仮想剛性についてはＰＲ２がＰＲ１の２倍大きい。そのため、ロボットは、ＰＲ１設定時にはいわゆる軟らかいバネのような応答をし、ＰＲ２設定時にはＰＲ１設定時の２倍硬いバネのような応答をする。なお、粘性項、質量項があるため、ＰＲ１又はＰＲ２設定時の応答の位相は、外力の位相とずれる。
【０１１６】
次に、図１７の説明を行う。図１７に示す外力Ｆ、ＰＲ１設定時の応答及びその他の設定は図１６の場合と同じである。図１７では、ＰＲ１の曲線（すなわちＰＲ１設定時のインピーダンス処理部２１の出力値）に対して、フィルター処理を行い、非線形インピーダンス制御を実現した時の応答曲線（ＦＰＲ１）を新たに加えている。
【０１１７】
デジタルフィルター処理の流れとしては、まず、出力タイミングを待ち（Ｓ１０１）、出力タイミングになった場合に、入力補正部から姿勢補正後の外力（外力値）Ｆを取得する（Ｓ１０２）。
【０１１８】
次に、式（３）を計算し、Ｙｎを求める（Ｓ１０３）。このＹｎの値を図１７のグラフにおいて表すと、ＰＲ１の曲線となる。
【０１１９】
さらに、求めたＹｎにフィルター処理を行い、出力値（Ｏｕｔｐｕｔ）を求める（Ｓ１０４）。図１７では、ＦＰＲ１の曲線がこのＯｕｔｐｕｔの値を表している。なお、本実施形態では、フィルター処理関数として、例えばシグモイド関数を用いる。シグモイド関数は２値の系の最大エントロピー状態を与える関数であり、さまざまな場面で現れる。ただし、ここで最大エントロピーに特別の意味があるわけではない。
【０１２０】
以上の本実施形態のロボット制御装置は、力覚センサー１０から取得される検出センサー値に基づいて、ロボット１００の目標軌道の補正値を出力する力制御部２０と、目標軌道に対して補正値に基づく補正処理を行って目標値を求め、求めた目標値を出力する目標値出力部６０と、目標値に基づいて、ロボット１００のフィードバック制御を行うロボット制御部８０と、を含む。そして、力制御部２０は、力制御における常微分方程式の解を、変換処理前の補正値として求めるインピーダンス処理部２１と、インピーダンス処理部２１から取得される変換処理前の補正値に対して非線形変換処理を行うことにより変換処理後の補正値を求め、求めた変換処理後の補正値を出力する非線形変換部２９と、を含む。
【０１２１】
すなわち、力制御部２０は、補正値を求めるために用いる値である力制御における常微分方程式の解を求めるインピーダンス処理部２１と、インピーダンス処理部２１から取得される常微分方程式の解に対して非線形変換処理を行うことにより補正値を求め、求めた補正値を出力する非線形変換部２９と、を含む。
【０１２２】
まず、力制御は、力覚センサー１０から取得される検出センサー値に基づいて、ロボット１００の目標軌道の補正値を求め、求めた補正値を出力する。
【０１２３】
ここで、検出センサー値（センサー情報）とは、力覚センサー１０からの出力値そのものであってもよいし、当該出力値に対して入力補正部３０による補正処理が施されたものであってもよい。また、帯域制限部２５（図１１に記載）による帯域制限処理が施されたものであってもよい。さらに、これらと数学的に等価な情報であってもよい。
【０１２４】
また、補正値とは、力制御部２０により求められ、目標値出力部６０により目標軌道を補正するために用いられる値のことである。例えば、補正値は、図１２（Ｂ）や図１２（Ｃ）のグラフに表される変位のことである。図１２（Ｂ）や図１２（Ｃ）のグラフ等に表された変位は、外力に対するインピーダンス制御の応答（出力）であり、実際にロボット１００のマニピュレーター等が動いた距離等を表す値ではない。そのため、実際にロボット１００のマニピュレーター等が動いた時の変位と区別するために、この変位のことを仮想変位（仮想的な変位）とも呼ぶ。なお、変換処理前の補正値は、中間値、中間補正値とも言い換えることができる。
【０１２５】
そして、目標値出力部は、目標軌道に対して補正値に基づく補正処理を行って目標値を求める。
【０１２６】
ここで、目標値とは、ロボット１００のフィードバック制御における目標値のことであり、ロボット制御部８０における制御はこの目標値に基づいて行われる。目標値は目標軌道に対して補正値による補正処理を行うことで取得できる。
【０１２７】
また、目標軌道とは、狭義にはロボット１００のエンドエフェクター部（エンドポイント）の空間的な目標位置の変化を表すものであってもよい。１つの目標位置は、例えば３次元空間座標ｘｙｚ（姿勢も考慮すれば各軸周りの回転角ｕｖｗを追加してもよい）で表されることになり、目標軌道とは当該目標位置の集合となる。ただし、目標軌道はこれに限定されるものではなく、ロボット１００の目標関節角の集合であってもよい。関節を持つロボット１００では、各関節の角度を決定すると、フォワードキネマティクス処理によりエンドエフェクター部の位置は一意に決定される。つまり、Ｎ関節ロボットではＮ個の関節角（θ１〜θＮ）により１つの目標位置を表現できることになるから、当該Ｎ個の関節角の組を１つの目標関節角とすれば、目標軌道とは目標関節角の集合と考えることができる。よって、力制御部２０から出力される補正値も、位置に関する値であってもよいし、関節角に関する値であってもよい。
【０１２８】
具体例を図２２（Ａ）、図２２（Ｂ）に示す。力制御における常微分方程式として、上式（１）の運動方程式を用いるのであれば、当該運動方程式の解は位置に関する値となる。よって、目標軌道が目標位置である場合には、解をそのまま補正値とすればよく、システム構成例は図２２（Ａ）のようになる。なお、目標値は位置に関する値でも関節角に関する値でもよいが、一般的にはロボット１００のフィードバック制御は関節角を用いることが想定される。
【０１２９】
それに対して、目標値出力部６０のインバースキネマティクス処理部６４とは別に、力制御部２０がインバースキネマティクス処理部２３を含む図２２（Ｂ）のようなケースも考えられる。例えば、目標値出力部６０での目標軌道生成処理と、力制御部２０での補正値出力処理とで、処理のタイミングや処理レートが異なる場合等である。その場合には、目標軌道は目標関節角であり、力制御部２０では運動方程式の解に対して変換処理（例えばインバースキネマティクス処理）を行って補正値とすることになる。
【０１３０】
また、力制御における常微分方程式とは、力制御において解を求める必要がある常微分方程式のことである。狭義には線形常微分方程式であってもよい。さらに狭義には、ロボットにあたかも所望の特性（質量・粘性・弾性等）を持つようにふるまわせるために解を求める必要がある常微分方程式のことであり、式（１）に示したような運動方程式であってもよい。
【０１３１】
そして、非線形変換部２９は、インピーダンス処理部２１から取得される変換処理前の補正値に対して非線形変換処理を行うことにより変換処理後の補正値を求める。第１の実施形態では、インピーダンス処理部２１の出力値に対してフィルター処理を行う点が、後述する第２の実施形態と異なる。具体的には、上述したようにシグモイド関数等を用いて、フィルター処理を行うが、フィルター処理関数はこれに限定されず、目的に合わせた関数を用いることが可能である。例えば図１７では、外力が大きくなればなるほど、補正値（仮想変位）が大きくなりにくくなるようなフィルター処理を行っている。すなわち、図１７では図１２（Ｂ）のような非線形インピーダンス制御を実現している。
【０１３２】
このように線形インピーダンス処理の出力値に対して、フィルター処理を行うことにより、非線形インピーダンス制御を実現することができる。具体的には、例えば上述したように、外力の大小に係わらず、その外力が急に消失した場合に起こるマニピュレーターの動きを所定の範囲に制限することができる。あるいは、外力（抗力）を所定の範囲内に制限することができる。このことは、ロボットの実用上、安全上、非常に有効である。
【０１３３】
また、本実施形態のロボット制御装置は線形なシステムの組み合わせであるため、運動方程式の解の安定性の判定も容易である。
【０１３４】
また、仮想変位が第１の変位である時の外力に対する変位変化量を第１の変位変化量とし、仮想変位が第１の変位と異なる第２の変位である時の変位変化量を第２の変位変化量とする場合に、力制御部２０は、第１の変位変化量と第２の変位変化量とが異なる力制御を行ってもよい。
【０１３５】
例えば、力制御部２０は、仮想的な変位が第２の変位よりも大きい第１の変位である場合に、第１の変位変化量が第２の変位変化量よりも大きくなるように非線形変換処理を行い、補正値を出力してもよい。
【０１３６】
ここで、変位変化量とは、外力に対する変位の変化量のことをいう。例えば、図１２（Ｂ）のグラフにおいては、変位変化量は変位を表す曲線の傾きのことをいう。言い換えれば、ロボットのマニピュレーターに一定方向から力を加え、それを徐々に変化させてマニピュレーターの瞬間的な変位を測定し、図１２（Ｂ）のようにグラフ化した時、ある大きさの外力に対するグラフの傾きが変位変化量となる。
【０１３７】
そのため、このような変位変化量を確認するためには、例えば、互いに大きさの異なる第１の外力、第２の外力、第３の外力をロボットのマニピュレーターに与え、第１の外力を与えた時のマニピュレーターの変位と第２の外力を与えた時の変位、第３の外力を与えた時の変位をそれぞれ求める。そして、第１の外力を与えた時のマニピュレーターの変位と第２の外力を与えた時の変位との差や、第２の外力を与えた時の変位と第３の外力を与えた時の変位との差を、変位変化量として確認できる。なお、第１の外力と第２の外力（第２の外力と第３の外力）の差を微小にすれば、グラフの傾きと同様の値を求めることができる。すなわち、変異変化量は、第１の外力を加えた時の変位と、第１の外力と異なる第２の外力を加えた時の変位との差ということもできる。そしてこの時、第１の外力と第２の外力との差が微小であっても良い。ここで、微小とは０に限りなく近いことをいう。
【０１３８】
また、上記のように変位変化量を確認した際に、外力に比例して変位が大きくなるようであれば、変位と外力とに図１２（Ａ）のような関係が成り立っていることが分かり、外力を大きくしているにも関わらず変位が大きくならないようであれば、変位と外力とに図１２（Ｂ）のような関係が成り立っていることが分かる。
【０１３９】
このように第１の変位変化量と第２の変位変化量とが異なる力制御を行うことにより、非線形インピーダンス制御を実現することができる。例えば、仮想的な変位（または外力）の絶対値が大きくなればなるほど、変位変化量が大きくなるような力制御を行うこと等が可能になる。すなわち、図１２（Ｃ）のグラフのような力制御が可能になる。具体的には、ロボットのマニピュレーターに加える力がある程度小さいうちはマニピュレーターが反発したかのようにマニピュレーターが押し込みにくいが、マニピュレーターに加える力が大きくなるとマニピュレーターが抵抗せずに、加えた力に応じてマニピュレーターが動くようになる。したがって、ロボットのマニピュレーターが把持する物体が壊れやすいものであったとしても、外力が大きい場合に変位を非常に大きくする図１２（Ｃ）に示すような制御を行うことにより、外力（抗力）を所定の範囲内に制限することができる。このことは、ロボットの実用上、安全上、非常に有効である。
【０１４０】
また、力制御部２０は、仮想的な変位が第２の変位よりも大きい第１の変位である場合に、第１の変位変化量が第２の変位変化量よりも小さくなるように非線形変換処理を行い、補正値を出力してもよい。
【０１４１】
これにより、例えば仮想的な変位（または外力）の絶対値が大きくなればなるほど、変位変化量が小さくなるような力制御を行うこと等が可能になる。すなわち、図１２（Ｂ）のグラフのような力制御が可能になる。具体的には、ロボットのマニピュレーターに加える力がある程度小さいうちはマニピュレーターが押し込みやすく、加えた力に応じてマニピュレーターが動くが、マニピュレーターに加える力が大きくなるとマニピュレーターが反発したかのように押し込みにくくなる。したがって、外力が急に消失した場合でも、外力が大きい場合に変位を制限する図１２（Ｂ）のような制御を行うことにより、マニピュレーターの動きを所定の範囲に制限することができる。
【０１４２】
また、これらの力制御を組み合わせて、第１の変位が第１の範囲のある場合には、仮想的な変位の絶対値が大きくなればなるほど、変位変化量が大きくなるような力制御を行い、第１の変位が第２の範囲のある場合には、仮想的な変位の絶対値が大きくなればなるほど、変位変化量が小さくなるような力制御を行うなどの変形実施が可能である。
【０１４３】
また、力制御部２０は、補正値に対応するロボット１００についての仮想的な変位の方向が第１の方向である場合には、第１の力制御を行い、仮想的な変位の方向が第１の方向と反対方向である第２の方向である場合には、第１の力制御と異なる第２の力制御を行ってもよい。
【０１４４】
具体例を図１８に示す。図１８の諸設定は図１７と同じであるが、使用するフィルター処理関数が異なる。図１８の例では、同図のグラフにおける仮想変位の方向が正の方向である時（すなわちＰＲ１の傾きが正である時）には、補正値（出力値）が大きくなりやすい第１の力制御を行い、仮想変位の方向が負の方向である時（すなわちＰＲ１の傾きが負である時）には、補正値が大きくなりにくい第２の力制御を行う。
【０１４５】
例えば、鉛筆削りで鉛筆を削る場合を考える。この場合に、鉛筆削りの中に鉛筆を押し込む方向（第１の方向）には、仮想変位を大きくする力制御を行う一方で、鉛筆削りから鉛筆を引っ張り出す方向（第２の方向）には、仮想変位を小さくするような力制御を行うことが可能になる。この例では、鉛筆削りの中に鉛筆を押し込みやすくなる一方で、鉛筆削りの中から鉛筆を引っ張り出しにくくなる。
【０１４６】
これにより、仮想的な変位の方向に基づいて、実行する力制御を切り替えること等が可能になる。なお、仮想変位の方向ではなく、外力の方向に基づいて、第１の力制御を行うか、第２の力制御を行うかを決定してもよい。すなわち、力制御部２０は、検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、外力の方向が第１の方向と反対方向である第２の方向である場合には、第１の力制御と異なる第２の力制御を行ってもよい。
【０１４７】
また、非線形変換部２９は、インピーダンス処理部２１から取得される変換処理前の補正値又は力覚センサー１０から取得される検出センサー値に対して、シグモイド関数を用いて非線形変換処理を行ってもよい。
【０１４８】
これにより、シグモイド関数を用いて非線形変換処理を行うこと等が可能になる。
【０１４９】
また、インピーダンス処理部２１は、力制御における常微分方程式の解を補正値として求めるデジタルフィルター部２２を有してもよい。
【０１５０】
これにより、常微分方程式の解を求めるという力制御において必要な処理を、デジタルフィルターを用いて行うことが可能になるため、Ｎｅｗｔｏｎ法、Ｒｕｎｇｅ−Ｋｕｔｔａ法等の手法を用いる場合や、上述した特許文献２に示されているようなスライディングモード制御を行う場合に比べて、ハードウェアー化が容易である。そして、デジタルフィルター処理に用いるデジタルフィルターを切り替える（例えばフィルター係数を切り替える）ことで、簡単に応答特性を切り替えることも可能となる。
【０１５１】
また、力制御部２０は、補正値を求めるデジタルフィルター部２２の動作の安定度を判定し、デジタルフィルター部２２の動作が安定であると判定された場合に、力制御における常微分方程式の解を補正値として求めてもよい。
【０１５２】
これにより、デジタルフィルターの安定性を判定すること等が可能になる。力制御における常微分方程式の係数パラメーターは、設定次第では現実にはあり得ない系（例えば発振するロボット等）を作ってしまうことがある。そのため、常微分方程式の安定性を判定する必要があるが、デジタルフィルターを用いれば、その判定が容易となる。
【０１５３】
また、常微分方程式は、仮想質量項、仮想粘性項及び仮想弾性項を係数パラメーターとする運動方程式であってもよい。
【０１５４】
これにより、運動方程式の解を求めること等が可能になる。よって、ロボット１００に対して、あたかも仮想質量項に対応する質量、仮想粘性項に対応する粘性、仮想弾性項に対応する弾性を持つかのようにふるまわせることが可能になる。
【０１５５】
また、本発明の他の態様では、ロボット制御装置と、目標値出力部６０から取得される目標値に基づいて、各部を動作させるロボット１００と、を含むロボットシステムに関係する。
【０１５６】
これにより、ロボット制御装置にとどまらず、本実施形態の処理を実行するロボットシステムを実現すること等が可能になる。
【０１５７】
３．第２の実施形態
３．１  構成
図１９に第２の実施形態に係るロボット制御装置の構成例を示す。
【０１５８】
力覚センサー１０、入力補正部３０、フォワードキネマティクス処理部４０、目標値出力部６０（軌道生成部６２、インバースキネマティクス処理部６４）、ロボット制御部８０（モーター制御部８２−１〜モーター制御部８２−Ｎ）等については図１３に表す第１の実施形態と同様であるため詳細な説明は省略する。
【０１５９】
また、図１３と同様に、ロボット制御装置の力制御部２０は、インピーダンス処理部２１と、制御パラメーター記憶部２４と、非線形変換部２９と、を含む。ただし、インピーダンス処理部２１と、非線形変換部２９との配置が逆になっている点が第１の実施形態と異なる。
【０１６０】
そのため、非線形変換部２９は、入力補正部３０から取得した外力値Ｉに対して、フィルター処理を行って、インピーダンス処理部２１に非線形変換処理後の外力Ｆを出力する。なお、本実施形態の力制御部２０は、図１９の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。また、その他の構成要素の動作等は、第１の実施形態と同様である。
【０１６１】
ここで、第２の実施形態におけるデジタルフィルターは図２０のようになる。図２０に示すデジタルフィルターでは、式（５）に表すように、図９のデジタルフィルターへの入力値に対してフィルター処理関数Ｈによりフィルター処理を行なった値（Ｆ）を、非線形変換処理後の外力（入力値）として求める。式（５）では、変位を制御する場合や力を制御する場合など目的に合わせた関数Ｈを用いる。
【０１６２】
【数５】
0005966372.tif 000006
【０１６３】
図２０に示したデジタルフィルターによる処理であれば、第１の実施形態と同様に、ハードウェアー化は容易であるし、安定性の判定も容易である。
【０１６４】
３．２  詳細な処理
次に図２１のフローチャートを用いて、本実施形態における力制御部が行うデジタルフィルター処理の流れを説明する。
【０１６５】
まず、出力タイミングを待ち（Ｓ２０１）、出力タイミングになった場合に、入力補正部から姿勢補正後の外力（外力値）Ｉを取得する（Ｓ２０２）。
【０１６６】
次に、外力（外力値）Ｉに基づいて、式（５）を計算し、変換処理後の外力Ｆを求める（Ｓ２０３）。さらに、求めたＦを入力値として式（３）に従って、出力値（Ｙｎ）を求める（Ｓ２０４）。
【０１６７】
なお、本実施形態では、第１の実施形態と同様に、フィルター処理関数として、例えばシグモイド関数を用いる。ただし、これに限定されない。
【０１６８】
以上の本実施形態のロボット制御装置は、力覚センサー１０から取得される検出センサー値に基づいて、ロボットの目標軌道の補正値を出力する力制御部２０と、目標軌道に対して補正値に基づく補正処理を行って目標値を求め、求めた目標値を出力する目標値出力部６０と、目標値に基づいて、ロボットのフィードバック制御を行うロボット制御部８０と、を含む。そして、力制御部２０は、力覚センサー１０から取得される検出センサー値に対して非線形変換処理を行うことにより、変換処理後の検出センサー値を求める非線形変換部２９と、非線形変換部２９から取得される変換処理後の検出センサー値に基づいて、力制御における常微分方程式の解を補正値として求めるインピーダンス処理部２１と、を含む。
【０１６９】
これにより、線形インピーダンス処理の入力値に対して、フィルター処理を行うことにより、非線形インピーダンス制御を実現することができる。また、線形なシステムの組み合わせを用いることにより、運動方程式の解の安定性の検証を行うことが可能となる。
【０１７０】
以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また、ロボット制御装置及びロボットシステムの構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１７１】
１０  力覚センサー、２０  力制御部、２１  インピーダンス処理部、
２２  デジタルフィルター部、２３  インバースキネマティクス処理部、
２４  制御パラメーター記憶部、２５  帯域制限部、２９  非線形変換部、
３０  入力補正部、４０  フォワードキネマティクス処理部、６０  目標値出力部、
６２  軌道生成部、６４  インバースキネマティクス処理部、８０  ロボット制御部、
８２  駆動制御部（モーター制御部）、１００  ロボット、１０２  駆動部、
２２１  デジタルフィルター演算部、２２２  デジタルフィルター係数出力部、
２２３  デジタルフィルター安定度判定部、２２４  デジタルフィルター係数記憶部、
２２５  デジタルフィルター係数変換部、３００  ロボット制御装置、
３１０  ロボット、３２０  アーム、３３０  ハンド、５１０  力覚センサー、
５３２  姿勢補正部、５３４  ハンド・ツール自重補正部、
５３６  運動方程式処理部、５４０  フォワードキネマティクス処理部、
５６２  軌道生成部、５６４  インバースキネマティクス処理部
【図１】
0005966372.tif 000007
【図２】
0005966372.tif 000008
【図３】
0005966372.tif 000009
【図４】
0005966372.tif 000010
【図５】
0005966372.tif 000011
【図６】
0005966372.tif 000012
【図７】
0005966372.tif 000013
【図８】
0005966372.tif 000014
【図９】
0005966372.tif 000015
【図１０】
0005966372.tif 000016
【図１１】
0005966372.tif 000017
【図１２】
0005966372.tif 000018
【図１３】
0005966372.tif 000019
【図１４】
0005966372.tif 000020
【図１５】
0005966372.tif 000021
【図１６】
0005966372.tif 000022
【図１７】
0005966372.tif 000023
【図１８】
0005966372.tif 000024
【図１９】
0005966372.tif 000025
【図２０】
0005966372.tif 000026
【図２１】
0005966372.tif 000027
【図２２】
0005966372.tif 000028