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

Abstract

【課題】 力の方向に対して補正値が非対称なインピーダンス制御を行うロボット制御装置、ロボットシステム及びロボット制御方法等の提供。
【解決手段】 ロボット制御装置では、力覚センサー１０から取得される検出センサー値に基づいて、ロボット１００の目標軌道の補正値を出力する力制御部２０と、目標軌道に対して補正値に基づく補正処理を行って目標値を求め、求めた目標値を出力する目標値出力部６０と、目標値に基づいて、ロボット１００のフィードバック制御を行うロボット制御部８０と、を含む。そして、力制御部２０は、検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、外力の方向が第１の方向と反対方向である第２の方向である場合には、第１の力制御とは異なる第２の力制御を行う。
【選択図】 図１３

Description

本発明は、ロボット制御装置、ロボットシステム及びロボット制御方法等に関係する。

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

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

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

このようなインピーダンス制御や力制御に関する従来技術としては、特許文献１に開示される技術が知られている。

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

上述したインピーダンス制御における常微分方程式は線形であるため、このようなインピーダンス制御は力（外力）の方向（向き）に対して対称な特性を持つことになる。

しかし、人間が作業を行う際には、外力の方向に対して変位が非対称となるように力を制御する場合がままある。例えば、何らかの引き込み機構を持った装置に物体を挿入する場合には、装置からの反発力に対しては、それに抵抗するように力を加えるが、一旦、引き込み機構が作動し、引き込み力に切り替われば、それに倣って動く、つまり「柔らかい」特性を持つようになる。したがって、ロボットが人間と同じような作業を行う際には、このような非対称なインピーダンス制御が必要となる。

ここで、特許文献１では、力制御における運動方程式のコンプライアンス項（弾性項）を、仮想変位に対して非線形な関数とすることにより、非線形性を持たせる手法が開示されている。特許文献１の明細書中では明示的に記載されていないが、上記の非線形なコンプライアンス項には非対称なコンプライアンス項も含まれるものと考えることができ、これにより非対称なインピーダンス制御が実現できるものと考えることもできる。

しかし、この手法は、位置（変位）に対する非対称性であり、上記に説明したような力の方向に対する変位（補正値）の非対称性ではない。つまり、特許文献１において開示される手法では、力の方向に対して変位が非対称なインピーダンス制御を実現することができない。

本発明の幾つかの態様によれば、力の方向に対して補正値が非対称なインピーダンス制御を行うロボット制御装置、ロボットシステム及びロボット制御方法等を提供することができる。

本発明の一態様は、力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正値を出力する力制御部と、前記目標軌道に対して前記補正値に基づく補正処理を行って目標値を求め、求めた前記目標値を出力する目標値出力部と、前記目標値に基づいて、前記ロボットのフィードバック制御を行うロボット制御部と、を含み、前記力制御部は、前記検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、前記外力の方向が前記第１の方向と反対方向である第２の方向である場合には、前記第１の力制御とは異なる第２の力制御を行うロボット制御装置に関係する。

これにより、外力の方向に対して変位の変化量が非対称となるインピーダンス制御を行うことが可能となる。

また、本発明の一態様では、前記力制御部は、前記外力の方向が前記第１の方向である場合には、外力に対する変位変化量が第１の変位変化量となる力制御を、前記第１の力制御として行い、前記外力の方向が前記第２の方向である場合には、外力に対する前記変位変化量が前記第１の変位変化量とは異なる第２の変位変化量となる力制御を、前記第２の力制御として行ってもよい。

これにより、例えば第１の方向への外力を検出した場合には、変位変化量が大きくなるような力制御を行い、第２の方向への外力を検出した場合には、変位変化量が小さくなるような力制御を行うこと等が可能になる。

また、本発明の一態様では、前記力制御部は、前記外力の方向が前記第１の方向である場合には、前記第１の変位変化量に対応する第１の補正値を出力する前記第１の力制御を行い、前記外力の方向が前記第２の方向である場合には、前記第２の変位変化量に対応する第２の補正値を出力する前記第２の力制御を行ってもよい。

これにより、外力の方向に応じて異なる補正値を出力すること等が可能になる。

また、本発明の一態様では、前記力制御部は、前記検出センサー値により表される前記外力の方向を判定する外力方向判定部を含んでもよい。

これにより、外力の方向を判定すること等が可能になる。

また、本発明の一態様では、前記力制御部は、力制御における制御パラメーターセットを複数記憶する制御パラメーター記憶部と、前記制御パラメーター記憶部に記憶された前記制御パラメーターセットの中から、使用する前記制御パラメーターセットの選択処理を行う制御パラメーター選択部と、を有し、前記力制御部は、前記検出センサー値により表される前記外力の方向が前記第１の方向である場合には、第１の制御パラメーターセットを選択する前記選択処理を行い、前記外力の方向が前記第２の方向である場合には、第２の制御パラメーターセットを選択する前記選択処理を行い、選択した前記制御パラメーターセットを用いて、力制御における常微分方程式の解を前記補正値として求め、前記補正値を出力してもよい。

これにより、インピーダンス制御に用いる制御パラメーターセットを切り替えることで、非線形インピーダンス制御を実現すること等が可能となる。

また、本発明の一態様では、前記力制御部は、前記第１の制御パラメーターセットを用いて力制御を行っている時に、所定の制御パラメーター変更指示があったと判断された場合に、使用する前記制御パラメーターセットを前記第１の制御パラメーターセットから前記第２の制御パラメーターセットへと変更する変更処理を行ってもよい。

これにより、所定の制御パラメーター変更指示があった場合に、使用する制御パラメーターセットを変更すること等が可能になる。

また、本発明の一態様では、前記力制御部は、前記第２の制御パラメーターセットを用いて力制御を行っている場合には、前記第１の方向への外力の大きさが第１の閾値を超えたと判断した時に、使用する前記制御パラメーターセットを前記第１の制御パラメーターセットに変更する前記変更処理を行い、前記第１の制御パラメーターセットを用いて力制御を行っている場合には、前記第２の方向への外力の大きさが第２の閾値を超えたと判断した時に、使用する前記制御パラメーターセットを前記第２の制御パラメーターセットに変更する前記変更処理を行ってもよい。

これにより、制御パラメーターセットの変更制御にヒステリシス特性を与えることができ、使用する制御パラメーターセットの変更が頻繁に起こることを回避すること等が可能になる。

また、本発明の一態様では、前記力制御部は、前記検出センサー値により表される外力の方向が前記第２の方向である場合には、前記第１の力制御を行って求めた第１の補正値と、前記第２の力制御を行って求めた第２の補正値とに対して重み付け処理を行って、第３の補正値を求め、求めた前記第３の補正値を出力してもよい。

これにより、複数のデジタルフィルターのパラメーターを用いて、デジタルフィルター処理を行った時の出力値を求め、各出力値の重み付け加算を行うことにより補正値を求めて、非対称インピーダンス制御を実現すること等が可能になる。

また、本発明の一態様では、前記力制御部は、力制御における常微分方程式の解を前記補正値として求めるデジタルフィルター部を有してもよい。

これにより、常微分方程式の解を求めるという力制御において必要な処理を、デジタルフィルターを用いて行うことが可能になるため、ハードウェアー化を容易にすること等が可能となる。

また、本発明の一態様では、前記力制御部は、前記補正値を求める前記デジタルフィルター部の動作の安定度を判定し、前記デジタルフィルター部の前記動作が安定であると判定された場合に、力制御における前記常微分方程式の解を前記補正値として求めてもよい。

これにより、デジタルフィルターの安定性を判定すること等が可能になる。

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

これにより、運動方程式の解を求めること等が可能になる。

また、本発明の他の態様は、前記ロボット制御装置と、前記目標値出力部から取得される前記目標値に基づいて、各部を動作させる前記ロボットと、を含むロボットシステムに関係する。

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

また、本発明の他の態様は、力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正値として、力制御における常微分方程式の解を求め、前記検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、前記外力の方向が前記第１の方向と反対方向である第２の方向である場合には、前記第１の力制御と異なる第２の力制御を行い、前記目標軌道に対して前記補正値に基づく補正処理を行って目標値を求め、求めた前記目標値を出力し、前記目標値に基づいて、前記ロボットのフィードバック制御を行うことを特徴とするロボット制御方法に関係する。

また、本発明の他の態様では、前記外力の方向が前記第１の方向である場合には、外力に対する変位変化量が第１の変位変化量となる力制御を、前記第１の力制御として行い、前記外力の方向が前記第２の方向である場合には、外力に対する前記変位変化量が前記第１の変位変化量とは異なる第２の変位変化量となる力制御を、前記第２の力制御として行うことを特徴とするロボット制御方法に関係する。

また、本発明の他の態様は、力覚センサーを有するロボットの制御を行うロボット制御方法であって、前記力覚センサーに加えられた外力の方向に応じて、前記ロボットの変位の大きさが異なる力制御を行うことを特徴とするロボット制御方法に関係する。

また、本発明の他の態様は、力覚センサーを有するロボットの制御を行うロボット制御方法であって、前記力覚センサーに第１の外力が加えられた場合の前記ロボットの変位の大きさと、前記力覚センサーに前記第１の外力と同じ大きさであって反対の方向の力である第２の外力が加えられた場合の前記ロボットの変位の大きさとが異なる力制御を行うことを特徴とするロボット制御方法に関係する。

ロボット制御装置及びロボットシステムの基本構成例。 図２（Ａ）、図２（Ｂ）は、ロボットシステムの一例。 図３（Ａ）〜図３（Ｃ）は、力制御についての説明図。 図４（Ａ）、図４（Ｂ）は、コンプライアンス制御についての説明図。 図５（Ａ）、図５（Ｂ）は、インピーダンス制御についての説明図。 図６（Ａ）〜図６（Ｃ）は、非対称インピーダンス制御が必要となる事例の説明図。 図７（Ａ）〜図７（Ｄ）は、各力制御における外力と変位の関係の説明図。 力覚フィードバックを含まない制御系の基本構成例。 力覚フィードバックを含む制御系の基本構成例。 運動方程式の解を求める際のデジタルフィルターの基本形。 図１１（Ａ）〜図１１（Ｃ）は、系の安定性判定手法の説明図。 デジタルフィルターを用いたロボット制御装置及びロボットシステムの基本構成例。 第１の実施形態の詳細なシステム構成例。 第１の実施形態のインピーダンスデジタルフィルター処理を説明するフローチャート。 図１５（Ａ）〜図１５（Ｅ）は、非対称インピーダンス制御の各動作ステップを説明する具体例。 正弦波外力に対する対称インピーダンス制御の応答例。 正弦波外力に対する非対称インピーダンス制御の応答例。 ２つのインピーダンス処理出力の重み付け加算の例。 第２の実施形態の詳細なシステム構成例。 第２の実施形態のインピーダンスデジタルフィルター処理を説明するフローチャート。 制御パラメーターの変更処理の説明図。 図２２（Ａ）、図２２（Ｂ）は、目標軌道、補正値、目標値を求める具体的なシステム構成例。

以下、本実施形態について説明する。まず、本実施形態の概要を説明する。次に、第１の実施形態と第２の実施形態について、それぞれシステム構成例と詳細な処理について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また、本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

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

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

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

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

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

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

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

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

また、図２（Ａ）では、ロボット本体３１０（ロボット）と制御装置３００（ロボット制御装置）とが別体に構成されているが、本実施形態のロボットは図２（Ａ）の構成に限定されず、図２（Ｂ）のようにロボット本体３１０と制御装置３００とが一体に構成されていてもよい。具体的には、図２（Ｂ）に示したように、ロボットはロボット本体３１０（アーム３２０及びハンド３３０を有する）及びロボット本体３１０を支えるベースユニット部を含み、当該ベースユニット部に制御装置３００が格納される。図２（Ｂ）のロボットでは、ベースユニット部に車輪等が設けられ、ロボット全体が移動可能な構成となっている。なお、図２（Ａ）は単腕型の例であるが、ロボットは図２（Ｂ）に示すように双腕型等の多腕型のロボットであってもよい。なお、ロボットの移動は人手で行われてもよいし、車輪を駆動するモーターを設け、当該モーターを制御装置３００により制御することで行ってもよい。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

これらの例は線形かつ対称なインピーダンス制御を行っている様子を示している。なお、ここで、線形インピーダンス制御とは、外力に対して変位が線形に変化するインピーダンス制御のことを指し、非線形インピーダンス制御とは、外力に対して変位が非線形に変化するインピーダンス制御の指す。さらに、対称インピーダンス制御とは、外力の方向に対して変位が対称であるインピーダンス制御のことを指し、非対称インピーダンス制御とは、外力の方向に対して変位が非対称であるインピーダンス制御のことを指す。

１．３ 非対称インピーダンス制御
しかし、線形かつ対称なインピーダンス制御では、図６（Ａ）〜図６（Ｃ）に示すような外力の向きに応じて異なる力制御が要求される事例には、十分に対応することができない。

ここで、具体的に図６（Ａ）〜図６（Ｃ）において求められるインピーダンス制御について説明する。

まず、図６（Ａ）には、引き込み機構としてローラー部ＲＬを持ち、加えてガイドローラー部ＧＲＬを有する装置に、ロボットのアームＡＭがワークＷＫを挿入する様子を示している。なお、ガイドローラー部ＧＲＬは、引き込み機構を持たないローラーであり、アームＡＭの先端には力覚センサーＳＥが設けられている。

本例では、ワークＷＫの先端がガイドローラー部ＧＲＬに接触している際には、力覚センサーＳＥが右向きの力を検出する。この時点では、右向きの外力に抵抗するような堅い力制御を行うことが望ましい。すなわち、外力に対する変位の変化量を小さくすべきである。

そして、ワークＷＫがアームＡＭに押されて右に移動し、ワークＷＫの先端がローラー部ＲＬに接触した際には、力覚センサーが左向きの力を検出する。ローラー部ＲＬがワークＷＫを引き込むためである。本例では、ワークＷＫを装置内に押し込むことが目的であるため、これ以降は左向きの外力に倣って動くような柔らかい力制御を行うべきである。言い換えれば、外力に対する変位の変化量を大きくすべきである。

図６（Ｂ）の例も図６（Ａ）の例と同様に、ガイドローラー部ＧＲＬ上でワークＷＫを移動させている場合には、右向きの外力に抵抗するような力制御を行い、ベルトコンベアー部ＢＣにワークＷＫが接触した時点では、左向きの外力に対して従順な力制御を行うべきである。

さらに、図６（Ｃ）の場合も図６（Ａ）及び図６（Ｂ）の例と同様であり、坂を上っている時には外力に対する変位変化量が小さくなる制御を行い、坂を下っている時には外力に対する変位変化量が大きくなる制御を行うことが望ましい。

次に、各力制御における外力と変位の関係という観点から、対称インピーダンス制御と非対称インピーダンス制御の違いについて、図７（Ａ）〜図７（Ｄ）を用いて説明する。なお、例えば、図７（Ａ）〜図７（Ｄ）を、図６（Ａ）の例に当てはめて考える場合には、ワークＷＫを押し込む時に受ける右方向の力を負の外力として表し、ワークＷＫがローラー部ＲＬにより引っ張られる時に受ける左方向の力を正の外力として表すものとする。

まず、図７（Ａ）には、線形かつ対称なインピーダンス制御の例を示す。すなわち、図７（Ａ）のグラフは、外力が大きくなるほど、変位も比例して大きくなる様子を示している。前述した図５（Ａ）、図５（Ｂ）における外力と変位にはこのような関係が成り立っている。

次に、図７（Ｂ）には所定の閾値以上外力が大きくなった場合には、変位が大きくなりにくくする非線形インピーダンス制御の例を示す。

これらの図７（Ａ）と図７（Ｂ）に示す対称インピーダンス制御では、外力の方向が変わったとしても、外力と変位の関係は変化しない。つまり、図７（Ａ）と図７（Ｂ）では、外力の方向が第１の方向（右方向）であろうが、第１の方向と逆方向である第２の方向（左方向）であろうが、外力の大きさが同じならば、それに対する変位の大きさ（絶対値）も同じである。

次に、図６（Ａ）のような事例において求められる外力と変位の理想的な関係とそれを実現する力制御について説明する。なお、ここでは説明を分かりやすくするため、コンプライアンス制御についてのみ説明を行うが、コンプライアンス制御に限定されず、インピーダンス制御に対しても適用されるものである。

まず結論から先に述べると、後述する第１の実施形態及び第２の実施形態では、図６（Ａ）のような事例において求められる力制御として、力覚センサーに加えられた外力の方向と、外力が加えられた時のロボットの変位の大きさとの関係が非対称になる力制御を行う。すなわち、力覚センサーに加えられた外力の方向に応じて、ロボットの変位の大きさが異なる力制御を行う。

ここでは具体的に、式（１）におけるコンプライアンス項（弾性項）をｘの関数ｇ（ｘ）とした運動方程式である下式（２）を考える。

例えば、式（２）におけるｇ（ｘ）が図７（Ｃ）の曲線のような形をしていれば、図６（Ａ）のワークＷＫを押し込む時に受ける右方向の外力には硬く、ワークＷＫが引っ張られる時に受ける左方向の外力には柔らかい構造を実現しているように見える。このようなインピーダンス制御を行えば、外力の方向に対する非対称性を獲得できる。

しかし正確に言えば、このｇ（ｘ）は変位ｘの関数であり、外力の方向に依存して、制御パラメーターが変化するわけではない。つまり、外力の方向が変化していない範囲においても、応答特性が変化してしまっており、図６（Ａ）で求められる応答特性とは微妙に異なる。実際に欲しいのは、例えば、以下に示す図７（Ｄ）のような特性である。

図７（Ｄ）に示すような非対称インピーダンス制御では、第１の方向に対する外力と変位の関係はｇ１の直線のようになり、第１の方向と反対方向である第２の方向に対する外力と変位の関係はｇ２の直線のようになる。すなわち、図７（Ａ）や図７（Ｂ）に示す対称インピーダンス制御と違い、外力の方向が第１の方向である時と、外力の方向が第２の方向である時とで、外力に対する変位変化量（つまり、ｇ１とｇ２の直線の傾き）が異なる。言い換えれば、外力の方向に応じて、外力と変位の関係が異なる。

具体的には、図６（Ａ）のワークＷＫを押し込む時に受ける右方向の外力に対してはｇ１の曲線に従い、外力が大きくなっても変位が変化しにくい硬い力制御を行い、ワークＷＫが引っ張られる時に受ける左方向の外力には対してはｇ２の曲線に従い、小さい外力であっても変位が変化しやすい柔らかい力制御を実現する。

このように、本出願人は、例えば図６（Ａ）〜図６（Ｃ）のような事例に対して、図７（Ｄ）に示すような非対称インピーダンス制御を行うロボット制御装置等を提案する。

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

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

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

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

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

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

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

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

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

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

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

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

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

以上の結果、常微分方程式の解はデジタルフィルターの出力として求めることが可能になる。運動方程式であれば、図１０に示したように２極のデジタルフィルターとなる。図１０において、ｄは１サンプル分の遅延であり、Ｃ_０、Ｃ_１、Ｃ_２はデジタルフィルターの係数（デジタルフィルターパラメーター）である。なお、図１０のデジタルフィルターの入力値Ｆと出力値Ｙ_ｎの関係は式（４）のように表すことができる。

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

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

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

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

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

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

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

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

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

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

以下、図１２の構成を基本とし、図７（Ｄ）のような非対称インピーダンス制御を実現する第１の実施形態と第２の実施形態について説明する。第１の実施形態は、本発明の基本形であり、外力の方向に応じて使用する制御フィルターのパラメーターを切り替えて、非対称インピーダンス制御を行う例である。そして、第２の実施形態は、所定の制御パラメーター変更指示等があった場合に、制御パラメーターを変更して、非対称インピーダンス制御を行う例である。

２．第１の実施形態
２．１ 構成
図１３に第１の実施形態に係るロボット制御装置の構成例を示す。

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

また、ロボット制御装置の力制御部２０は、インピーダンス処理部２１と、第１の制御パラメーター記憶部２４−１と、第２の制御パラメーター記憶部２４−２と、制御パラメーター選択部２６と、外力方向判定部２７と、を含む。なお、本実施形態の力制御部２０は図１３の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。

また、図１３のインピーダンス処理部２１は、図１２のデジタルフィルター演算部２２１に対応し、動作も同様のため、説明を省略する。さらに、図１３の第１の制御パラメーター記憶部２４−１と第２の制御パラメーター記憶部２４−２は、図１２のデジタルフィルター係数記憶部２２４に対応し、図１３の制御パラメーター選択部２６は、図１２のデジタルフィルター係数変換部２２５に対応する。

第１の制御パラメーター記憶部２４−１と第２の制御パラメーター記憶部２４−２は、それぞれ異なる制御パラメーターを記憶する。ここで、制御パラメーターとは、後述する運動方程式の係数パラメーターであっても良いし、デジタルフィルターのパラメーターであってもよい。さらに、制御パラメーターとして、オフセットパラメーターを記憶してもよい。ここで、オフセットパラメーターとは、外力が０である場合の仮想変位のことをいう。制御パラメーター記憶部の機能は、ＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現でき、実際には一つのメモリー等により構成されていても良いし、複数のメモリー等により構成されていても良い。

次に、外力方向判定部２７は、入力補正部３０から取得される姿勢補正後の検出センサー値（外力）に基づいて、外力の方向を判定し、制御パラメーター選択部２６に外力の方向を通知する。

そして、制御パラメーター選択部２６は、外力方向判定部２７により判定された外力の方向に基づいて、第１の制御パラメーター記憶部２４−１と第２の制御パラメーター記憶部２４−２とに記憶された制御パラメーターの中から、使用する制御パラメーターを選択し、インピーダンス処理部２１に出力する。また、第１の制御パラメーター記憶部２４−１と第２の制御パラメーター記憶部２４−２とに記憶された制御パラメーターが、運動方程式の係数パラメーターである場合には、係数パラメーターをデジタルフィルターのパラメーターへ変換する処理等を行う。

また、本実施形態においては、図１０に示したデジタルフィルターと同様の構成からなるデジタルフィルターを用いることができる。

図１０に示したデジタルフィルターによる処理であれば、ハードウェアー化は容易であるし、安定性の判定も容易である。

２．２ 詳細な処理
次に図１４のフローチャートを用いて、本実施形態における力制御部が行うデジタルフィルター処理の流れを説明する。また、図６（Ａ）の事例における非対称インピーダンス制御の各動作ステップを説明する具体例を図１５（Ａ）〜図１５（Ｅ）に示す。以下では、具体例を交えて、フローチャートの説明を行う。

デジタルフィルター処理の流れとしては、まず、出力タイミングを待ち（Ｓ１０１）、出力タイミングとなった時に、入力補正部３０から姿勢補正後の外力（外力値）Ｆを取得する（Ｓ１０２）。

図１５（Ａ）〜図１５（Ｅ）に示す具体例では、図１５（Ａ）の時点では、ロボットのアームＡＭが把持するワークＷＫの先端がガイドローラー部ＧＲＬに触れておらず、外力は検出されないが（検出センサー値が０となる）、図１５（Ｂ）の時点でガイドローラー部ＧＲＬから受ける右向きの外力が検出される。なお、検出センサー値が０である場合でも、以下の処理を行っても良い。

次に、一つ前の外力値と現在の外力値の符号が同じか否かを判定する（Ｓ１０３）。ここで、外力値の符号とは、外力の方向のことを指す。例えば、外力値の符号が「−」である場合には、外力の方向が図の右方向（第１の方向）であり、外力値の符号が「＋」である場合には、外力の方向が図の左方向（第２の方向）であるとする。すなわち、ステップＳ１０３では、外力の方向に変化があったか否かを判定する。

一つ前の外力値と現在の外力値の符号が異なると判定した場合には、外力の方向が変化したと判断し、現在の符号（外力の方向）に対応する制御パラメーターを設定し（Ｓ１０４）、ステップＳ１０５の処理を行う。

例えば、図１５（Ｂ）における外力の方向は右方向であり、外力値の符号は「−」である。一方、図１５（Ｃ）の状態では、ローラー部ＲＬがワークＷＫを引き込むため、外力の方向は左方向であり、外力値の符号が「＋」となる。よって、図１５（Ｂ）の状態と図１５（Ｃ）の状態では外力値の符号が異なる。そのため、制御パラメーター選択部２６は、外力の方向が変化したと判断し、制御パラメーターを変更する。なお、本例では、初期状態では第１の制御パラメーター記憶部２４−１に記憶される第１の制御パラメーターが設定されているとし、外力の方向が変化した時点で、第２の制御パラメーター記憶部２４−２に記憶される第２の制御パラメーターへと使用する制御パラメーターを変更する。

一方、一つ前の外力値と現在の外力値の符号が同じであると判定した場合には、外力の方向に変化がみられないと判断し、現在設定されている制御パラメーターを変更せずに、ステップＳ１０５の処理を行う。

例えば、図１５（Ｃ）における外力の方向は左方向であり、外力値の符号は「＋」である。さらに、図１５（Ｄ）や図１５（Ｅ）の状態でも、外力の方向は左方向であり、外力値の符号は「＋」のままであるため、図１５（Ｃ）の状態から図１５（Ｅ）の状態まで外力値の符号に変化はない。そのため、ここでは使用する制御パラメーターを変更しない。

そして、設定された制御パラメーターに基づいて、式（４）を計算し、Ｙ_ｎを求め（Ｓ１０５）、ステップＳ１０１に戻る。以上が本実施形態のデジタルフィルター処理の流れである。

次に、図１６と図１７を用いて、対称インピーダンス制御の応答と非対称インピーダンス制御の応答を比較する。

まず、図１６には正弦波外力Ｆに対する対称インピーダンス制御の応答（Ｙ_ｓ、Ｙ_ｈ）を示す。図１６のグラフでは、運動方程式の係数パラメーター（仮想質量、仮想粘性、仮想剛性）の設定を同図に示すＰＲ１とした時の外力Ｆに対する応答をＹ_ｓとし、係数パラメーターの設定をＰＲ２とした時の応答をＹ_ｈとして表す。なお、図１６及び図１７では、縦軸を任意目盛、横軸を時間（秒）とし、駆動周期をマニピュレーターの電流制御ループの周期と同じ１２５マイクロ秒（８ｋサンプル／秒）とし、外力を１秒周期の正弦波であるとする。

図１６のＰＲ１とＰＲ２では、仮想質量ｍと仮想粘性μは同じであるが、仮想剛性ｋについてはＰＲ２がＰＲ１の２倍大きい。そのため、ロボットは、ＰＲ１設定時にはいわゆる軟らかいバネのような応答をし、ＰＲ２設定時にはＰＲ１設定時の２倍硬いバネのような応答をする。なお、粘性項、質量項があるため、ＰＲ１又はＰＲ２設定時の応答の位相は、外力の位相とずれる。

図１６を見てわかる通り、振幅や周期が違うものの、応答Ｙ_ｓ及び応答Ｙ_ｈは、どちらも外力Ｆの方向（符号）に対して対称な正弦波となる。

次に、正弦波外力Ｆに対する非対称インピーダンス制御の応答Ｙ_ｎを図１７に示す。同図に示すＹ_ｎは、外力の方向に応じて運動方程式の係数パラメーターを切り替える非対称インピーダンス制御の応答（変位、補正値）である。

係数パラメーターとしては、同図に示すＰＲ３とＰＲ４を用いる。外力の方向が左方向（第２の方向）である場合に設定する係数パラメーターをＰＲ３とし、外力の方向が右方向（第１の方向）である場合に設定する係数パラメーターをＰＲ４とする。図１７のＰＲ３とＰＲ４は、仮想質量ｍは同じであるが、仮想粘性μについてはＰＲ４の方がＰＲ３の３２倍大きく、仮想剛性ｋについてはＰＲ４の方がＰＲ３の４倍大きい。そのため、ロボットは、ＰＲ３設定時にはＰＲ４設定時と比べて軟らかいバネのような応答をし、ＰＲ４設定時にはＰＲ３設定時と比べて硬いバネのような応答をする。

例えば、図１７のグラフではＴ１の時点で外力が正（左方向）から負（右方向）になるため、ＰＲ４の係数パラメーターが設定される。そのため、応答Ｙ_ｎが負の方向へ移動しにくくなり、外力の変化に対する変位の変化量が小さくなる。一方、例えばＴ２の時点で外力が負（右方向）から正（左方向）になるため、ＰＲ３の係数パラメーターが設定される。そのため、応答Ｙ_ｎが正の方向へ移動しやすくなり、外力の変化に対する変位の変化量が大きくなる。

図１７のようなインピーダンス制御を行えば、外力の方向に対して変位の変化量が非対称となるインピーダンス制御を行うことができる。このように、図１６に示す応答Ｙ_ｓ及びＹ_ｈと図１７の応答Ｙ_ｎの曲線を比べれば、対称インピーダンス制御の応答と非対称インピーダンス制御の応答の違いが分かる。

以上の本実施形態のロボット制御装置では、力覚センサー１０から取得される検出センサー値に基づいて、ロボット１００の目標軌道の補正値を出力する力制御部２０と、目標軌道に対して補正値に基づく補正処理を行って目標値を求め、求めた目標値を出力する目標値出力部６０と、目標値に基づいて、ロボット１００のフィードバック制御を行うロボット制御部８０と、を含む。そして、力制御部２０は、検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、外力の方向が第１の方向と反対方向である第２の方向である場合には、第１の力制御とは異なる第２の力制御を行う。

まず、力制御は、力覚センサー１０から取得される検出センサー値に基づいて、ロボット１００の目標軌道の補正値を求め、求めた補正値を出力する。

ここで、検出センサー値（センサー情報）とは、力覚センサー１０からの出力値そのものであってもよいし、当該出力値に対して入力補正部３０による補正処理が施されたものであってもよい。また、帯域制限部２５（図１２に記載）による帯域制限処理が施されたものであってもよい。さらに、これらと数学的に等価な情報であってもよい。

また、補正値とは、力制御部２０により求められ、目標値出力部６０により目標軌道を補正するために用いられる値のことである。例えば、補正値は、図７（Ｄ）のグラフに表される変位のことである。図７（Ｄ）のグラフ等に表された変位は、外力に対するインピーダンス制御の応答（出力）であり、実際にロボット１００のマニピュレーター等が動いた距離等を表す値ではない。そのため、実際にロボット１００のマニピュレーター等が動いた時の変位と区別するために、この変位のことを仮想変位（仮想的な変位）とも呼ぶ。なお、変換処理前の補正値は、中間値、中間補正値とも言い換えることができる。

そして、目標値出力部は、目標軌道に対して補正値に基づく補正処理を行って目標値を求める。

ここで、目標値とは、ロボット１００のフィードバック制御における目標値のことであり、ロボット制御部８０における制御はこの目標値に基づいて行われる。目標値は目標軌道に対して補正値による補正処理を行うことで取得できる。

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

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

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

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

そして、力制御部２０は、検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、外力の方向が第１の方向と反対方向である第２の方向である場合には、第１の力制御と異なる第２の力制御を行う。なお、第１の力制御と第２の力制御の詳細については、後述する。

これにより、外力の方向に対して変位の変化量が非対称となるインピーダンス制御を行うことが可能となる。このことは、ロボットの実用上非常に有効であり、より作業適応度の高い力制御を実現することができる。

また、力制御部２０は、外力の方向が第１の方向である場合には、外力に対する変位変化量が第１の変位変化量となる力制御を、第１の力制御として行い、外力の方向が第２の方向である場合には、外力に対する変位変化量が第１の変位変化量とは異なる第２の変位変化量となる力制御を、第２の力制御として行ってもよい。

ここで、変位変化量とは、外力に対する変位の変化量のことをいう。例えば、図７（Ｄ）のグラフにおいては、変位変化量は変位を表す直線の傾きのことをいう。言い換えれば、ロボットのマニピュレーターに一定方向から力を加え、それを徐々に変化させてマニピュレーターの瞬間的な変位を測定し、図７（Ａ）〜図７（Ｄ）のようにグラフ化した時、ある大きさの外力に対するグラフの傾きが変位変化量となる。

そのため、このような変位変化量を確認するためには、例えば、互いに大きさの異なる第１の外力、第２の外力、第３の外力をロボットのマニピュレーターに与え、第１の外力を与えた時のマニピュレーターの変位と第２の外力を与えた時の変位、第３の外力を与えた時の変位をそれぞれ求める。そして、第１の外力を与えた時のマニピュレーターの変位と第２の外力を与えた時の変位との差や、第２の外力を与えた時の変位と第３の外力を与えた時の変位との差を、変位変化量として確認できる。なお、第１の外力と第２の外力（第２の外力と第３の外力）の差を微小にすれば、グラフの傾きと同様の値を求めることができる。すなわち、変位変化量は、第１の外力を加えた時の変位と、第１の外力と異なる第２の外力を加えた時の変位との差ということもできる。そしてこの時、第１の外力と第２の外力との差が微小であっても良い。ここで、微小とは０に限りなく近いことをいう。

また、上記のように変位変化量を確認した際に、外力に比例して変位が大きくなるようであれば、変位と外力とに図７（Ａ）のような関係が成り立っていることが分かり、外力を大きくしているにも関わらず変位が大きくならないようであれば、変位と外力とに図７（Ｂ）のような関係が成り立っていることが分かる。

このように第１の変位変化量と第２の変位変化量とが異なる力制御を行うことにより、非対称インピーダンス制御を実現することができる。例えば第１の方向への外力を検出した場合には、変位変化量が大きくなるような力制御を行い、第２の方向への外力を検出した場合には、変位変化量が小さくなるような力制御を行うこと等が可能になる。すなわち、図７（Ｄ）のグラフのような力制御が可能になる。

また、力制御部２０は、外力の方向が第１の方向である場合には、前記第１の変位変化量に対応する第１の補正値を出力する第１の力制御を行い、外力の方向が第２の方向である場合には、前記第２の変位変化量に対応する第２の補正値を出力する第２の力制御を行ってもよい。

すなわち、上述した第１の力制御とは、第１の補正値を出力する力制御のことでもあり、上述した第２の力制御とは、第２の補正値を出力する力制御のことでもある。

これにより、外力の方向に応じて異なる補正値を出力すること等が可能になる。

また、力制御部２０は、検出センサー値により表される外力の方向を判定する外力方向判定部２７を含んでもよい。

これにより、外力の方向を判定すること等が可能になる。

また、力制御部２０は、力制御における制御パラメーターセットを複数記憶する制御パラメーター記憶部２４と、制御パラメーター記憶部２４に記憶された制御パラメーターセットの中から、使用する制御パラメーターセットの選択処理を行う制御パラメーター選択部２６と、を有してもよい。そして、力制御部２０は、検出センサー値により表される外力の方向が第１の方向である場合には、第１の制御パラメーターセットを選択する選択処理を行い、外力の方向が第２の方向である場合には、第２の制御パラメーターセットを選択する選択処理を行い、選択した制御パラメーターセットを用いて、力制御における常微分方程式の解を補正値として求め、補正値を出力してもよい。

ここで、制御パラメーターセットとは、力制御における常微分方程式の各項の係数パラメーターのセットであってもよいし、デジタルフィルターのパラメーターセットであってもよい。

また、常微分方程式の各項の係数パラメーターとは、常微分方程式における定数項、１階微分項の係数、２階微分項の係数、・・・ｎ階微分項の係数を指すものとする。上述した式（１）の例であれば、ｍ、μ、ｋが係数パラメーターとなる。

一方、デジタルフィルターのパラメーターとは、式（４）における各項の係数を指す。具体的には、Ｃ_０、Ｃ_１、Ｃ_２のことである。なお、第１のパラメーターセットと第２のパラメーターセットは、いずれかのパラメーターが異なっているものとする。

なお、本実施形態では、外力の方向に基づいて、インピーダンス制御に用いる制御パラメーターセットの選択処理を行っているが、外力の方向ではなく仮想変位に基づいて、選択処理を行っても良い。

これにより、インピーダンス制御に用いる制御パラメーターセットを切り替えることで、非線形インピーダンス制御を実現すること等が可能となる。

また、力制御部２０は、検出センサー値により表される外力の方向が第２の方向である場合には、第１の力制御を行って求めた第１の補正値と、第２の力制御を行って求めた第２の補正値とに対して重み付け処理を行って、第３の補正値を求め、求めた第３の補正値を出力してもよい。

例えば、力制御部２０は、重み付け処理後の第１の補正値と第２の補正値との和を第３の補正値として求めてもよい。

具体例として、２つのインピーダンス処理出力の重み付け加算の例を図１８に示す。これは図１６と同じ２組の制御パラメーター（ＰＲ１とＰＲ２）を用いた場合であるが、本例では、パラメーターを切り替えるのではなく、２組のインピーダンス処理を並列動作させ、その出力を重み付け加算している。

図１８においては、外力がＦ、外力Ｆに対して、図１６に示す運動方程式の係数パラメーターセットＰＲ１を用いてインピーダンス制御を行った時の応答がＹ_ｓ、係数パラメーターセットＰＲ２を用いてインピーダンス制御を行った時の応答がＹ_ｈ、応答Ｙ_ｓとＹ_ｈとの重み付け加算値がＣＰＲとなる。なお、本例では、外力の方向に関わらず、常に重み付け和（ＣＰＲ）を求めている。

また、重みＷは式（５）のようになり、重み付け加算値（出力値、補正値）Ｏｕｔｐｕｔは式（６）のようになる。本実施形態では、このようにして求めた重み付け加算値を補正値として求めても良い。

なお、重み付け関数は、正負両方向に飽和特性を持ち、単調増加関数であれば他の関数でも良いが、ここではｔａｎｈあるいはシグモイド関数（実質的にはｔａｎｈと同一のもの）を用いる。シグモイド関数は２値の系の最大エントロピー状態を与える関数であり、さまざまな場面で現れる。ただし、ここで最大エントロピーに特別の意味があるわけではない。

これにより、制御パラメーターの切り替えではなく、複数のデジタルフィルターのパラメーターを用いて、デジタルフィルター処理を行った時の出力値を求め、各出力値の重み付け加算を行うことにより補正値を求めて、非対称インピーダンス制御を実現すること等が可能になる。

また、力制御部２０は、力制御における常微分方程式の解を補正値として求めるデジタルフィルター部２２を有してもよい。

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

また、力制御部２０は、補正値を求めるデジタルフィルター部２２の動作の安定度を判定し、デジタルフィルター部２２の動作が安定であると判定された場合に、力制御における常微分方程式の解を補正値として求めてもよい。

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

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

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

また、以上の本実施形態は、上述してきたロボット制御装置（力制御部２０、目標値出力部６０、ロボット制御部８０を含む）と、目標値出力部６０から取得される目標値に基づいて、各部を動作させるロボット１００を含むロボットシステムに関係する。

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

なお、上記ではデジタルフィルターを用いて力制御を行う場合について述べたが、デジタルフィルターを他の構成要素により代替することも可能である。デジタルフィルターを他の構成要素により代替した場合も、本発明の範囲に含まれるものとする。

３．第２の実施形態
第２の実施形態では、所定の制御パラメーター変更指示があった場合等に、制御パラメーターを変更して、非対称インピーダンス制御を行う。

３．１ 構成
図１９に第２の実施形態に係るロボット制御装置の構成例を示す。

ロボット制御装置の力制御部２０は、インピーダンス処理部２１と、第１の制御パラメーター記憶部２４−１と、第２の制御パラメーター記憶部２４−２と、閾値記憶部（基準外力方向記憶部）２４−３と、制御パラメーター選択部２６と、外力方向判定部（外力方向・外力値判定部）２７と、状態設定部（リセット部）２９と、を含む。なお、本実施形態の力制御部２０は、図１９の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。

ここでは、力覚センサー１０、入力補正部３０、フォワードキネマティクス処理部４０、目標値出力部６０（軌道生成部６２、インバースキネマティクス処理部６４）、ロボット制御部８０（モーター制御部８２−１〜モーター制御部８２−Ｎ）、インピーダンス処理部２１、第１の制御パラメーター記憶部２４−１、第２の制御パラメーター記憶部２４−２等については図１３と同様であるため説明は省略し、図１３と異なる構成又は動作をする機能部についてのみ説明を行う。

まず、閾値記憶部（基準外力方向記憶部）２４−３は、制御パラメーターを変更するか否かの判定処理に用いる所定の閾値や後述する基準外力方向を記憶する。閾値記憶部（基準外力方向記憶部）２４−３の機能は、ＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。実際には第１の制御パラメーター記憶部２４−１及び第２の制御パラメーター記憶部２４−２と共通の単一のメモリー等により構成されていても良いし、それぞれ別の複数のメモリー等により構成されていても良い。

次に、外力方向判定部（外力方向・外力値判定部）２７は、外力の方向を判定する他にも、検出センサー値に基づいて、閾値記憶部２４−３から取得される所定の閾値よりも外力値が大きいか否か等を判定しても良い。

さらに、制御パラメーター選択部２６は、外力方向判定部（外力方向・外力値判定部）２７から取得される外力の方向や外力と閾値との大小関係を表す情報に基づいて、使用する制御パラメーターを選択する。なお、外力方向判定部２７ではなく、制御パラメーター選択部２６が、所定の閾値よりも外力値が大きいか否か等を判定しても良い。

そして、状態設定部（リセット部）２９は、制御パラメーター変更指令やリセット信号を取得した場合に、制御パラメーター選択部にその旨を通知する。

なお、第２の実施形態においても、第１の実施形態と同様に図１０に示すデジタルフィルターを利用することができる。

３．２ 詳細な処理
次に図２０のフローチャートを用いて、本実施形態における力制御部が行うデジタルフィルター処理の流れを説明する。

ステップＳ３０１、Ｓ３０２までの流れは図１４と同様である。なお、本実施形態の初期状態では、デジタルフィルターのパラメーターセットとして、第１のパラメーターセットＰＲ１を用いる。

また、本実施形態では、外力値Ｆ（外力の方向と大きさ）に基づいて、使用するデジタルフィルターのパラメーターセットを変更する。その様子を、横軸を外力値Ｆとし、縦軸を制御パラメーターとするグラフを用いて図２１に表す。

前述した通り、初期状態では第１のパラメーターセットＰＲ１を選択しているため、図２１に示されるように、外力値Ｆが第２の閾値Ｔｈ２よりも大きいことが想定される。さらに、第２の閾値Ｔｈ２以下であるならば、使用するデジタルフィルターのパラメーターセットを第２のパラメーターセットＰＲ２に変更すべきである。

そのため、まず現在設定されている閾値が第２の閾値Ｔｈ２であるか否かを判定する（Ｓ３０３）。

そして、現在設定されている閾値が第２の閾値Ｔｈ２であると判定した場合には、外力値Ｆが第２の閾値以下か否かを判定する（Ｓ３０４）。

外力値Ｆが第２の閾値Ｔｈ２以下であると判定した場合には、デジタルフィルターのパラメーターセットを第２のパラメーターセットＰＲ２に設定する（Ｓ３０５）。

ここで、第２のパラメーターセットを使用する状態（以下、第２の状態と呼ぶ）から第１のパラメーターセットを使用する状態（以下、第１の状態と呼ぶ）へ遷移するか否かを判定する際の比較処理においても、第２の閾値Ｔｈ２を用いると、外力値Ｆが第２の閾値Ｔｈ２付近の値である場合に、使用するデジタルフィルターのパラメーターセットが頻繁に変化してしまうことがある。使用するデジタルフィルターのパラメーターセットが変化すると、応答特性も変化するため、このような頻繁な変更は通常好ましくない。

そこで、デジタルフィルターのパラメーターセットの変更制御にヒステリシス特性を与えるため、ステップＳ３０５の後に、閾値を第１の閾値Ｔｈ１に変更する（Ｓ３０６）。これにより、使用するデジタルフィルターのパラメーターセットの変更が頻繁に起こることを防ぐことができる。

一方で、ステップＳ３０４において外力値Ｆが第２の閾値よりも大きいと判定した場合には、使用するデジタルフィルターのパラメーターセットの変更処理は行わない。

そして、ステップＳ３０６の処理後又はステップＳ３０４において外力値Ｆが第２の閾値よりも大きいと判定した場合に、ステップＳ３１０の処理を行い、Ｙ_ｎを求め（Ｓ３１０）、ステップＳ３０１に戻る。

一方、第２の状態から第１の状態への遷移判定をする場合には、前述したように外力値Ｆと第１の閾値Ｔｈ１を比較する。なお、第２の状態である場合には、外力値Ｆと比較する閾値として第１の閾値Ｔｈ１が設定されているはずである（ステップＳ３０６参照）。

そこで、ステップＳ３０３において、現在設定されている閾値が第２の閾値Ｔｈ２ではないと判定した（すなわち、第１の閾値Ｔｈ１が設定されていると判定した）場合には、外力値Ｆが第１の閾値Ｔｈ１以上か否かを判定する（Ｓ３０７）。

外力値Ｆが第１の閾値Ｔｈ１以上であると判定した場合には、デジタルフィルターのパラメーターセットを第１のパラメーターセットＰＲ１に設定する（Ｓ３０８）。そして、ステップＳ３０６と同様に、使用するデジタルフィルターのパラメーターセットの変更制御にヒステリシス特性を持たせるために、閾値を第２の閾値Ｔｈ２に変更する（Ｓ３０９）。

一方で、ステップＳ３０７において外力値Ｆが第１の閾値Ｔｈ１よりも小さいと判定した場合には、使用するデジタルフィルターのパラメーターセットの変更処理は行わない。

そして、ステップＳ３０９の処理後又はステップＳ３０７において外力値Ｆが第１の閾値Ｔｈ１よりも小さいと判定した場合に、ステップＳ３１０の処理を行い、Ｙ_ｎを求め（Ｓ３１０）、ステップＳ３０１に戻る。以上が本実施形態のデジタルフィルター処理の流れである。

以上の本実施形態のロボット制御装置の力制御部２０は、第１の制御パラメーターセットを用いて力制御を行っている時に、所定の制御パラメーター変更指示があったと判断された場合に、使用する制御パラメーターセットを第１の制御パラメーターセットから第２の制御パラメーターセットへと変更する変更処理を行ってもよい。

ここで、所定の制御パラメーター変更指示とは、前述したように外力値と所定の閾値との比較結果のことであってもよいし、状態設定部２９から出力される制御パラメーターの変更命令のことであってもよい。

例えば、所定の制御パラメーター変更指示が前者である場合には、外力値が所定の閾値よりも大きいという比較結果を制御パラメーター選択部２６が取得した時に、使用する制御パラメーターセットの変更処理を行う。

一方で、例えば、所定の制御パラメーター変更指示が後者である場合には、以下のような処理を行う。

まず、本実施形態のロボット制御装置の力制御部２０は、基本外力方向記憶部２４−３を持つ。なお、基本外力方向記憶部２４−３は閾値記憶部と同じメモリー等であってもよい。また、初期状態では、基本外力方向記憶部２４−３に記憶された基本外力方向に対応する制御パラメーターセット（第１の制御パラメーター）が設定されているものとする。

そして、基本外力方向と異なる方向の外力が検出された時に、第１の制御パラメーターセットとは異なる第２の制御パラメーターセットを制御パラメーター選択部２６が選択する。ただし、状態設定部２９から制御パラメーターセットの変更命令が出力されるまでは、第１の制御パラメーターセットが設定されている状態を維持する。そして、状態設定部２９から制御パラメーターセットの変更命令が出力された時に初めて、選択された制御パラメーターセットを設定する。また、状態設定部２９が、初期状態への戻すことを指示するリセット信号を出力した場合には、制御パラメーター選択部２６は、第１の制御パラメーターセットを設定し直す。

これにより、所定の制御パラメーター変更指示があった場合に、使用する制御パラメーターセットを変更すること等が可能になる。

また、力制御部２０は、第２の制御パラメーターセットを用いて力制御を行っている場合には、第１の方向への外力の大きさが第１の閾値を超えたと判断した時に、使用する制御パラメーターセットを第１の制御パラメーターセットに変更する変更処理を行い、第１の制御パラメーターセットを用いて力制御を行っている場合には、第２の方向への外力の大きさが第２の閾値を超えたと判断した時に、使用する制御パラメーターセットを第２の制御パラメーターセットに変更する変更処理を行ってもよい。

ここで、第２の方向は、第１の方向と同一軸上での反対方向のことをいう。

また、ここでは外力の方向を区別し、外力の大きさを正の数、すなわち絶対値で表している。一方、上述した例では、図２１に示すように、第１の方向への外力の大きさと第１の閾値とを正の数で表し、第２の方向への外力の大きさと第２の閾値とを負の数で表している。しかし、これらは表現上での差異であるため、本質的には同一のものである。

これにより、上述したように、制御パラメーターセットの変更制御にヒステリシス特性を与えることができ、使用する制御パラメーターセットの変更が頻繁に起こることを回避すること等が可能になる。

また、力覚センサーに第１の外力が加えられた場合のロボットの変位の大きさと、力覚センサーに第１の外力と同じ大きさであって反対の方向の力である第２の外力が加えられた場合のロボットの変位の大きさとが異なる力制御を行ってもよい。

以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また、ロボット制御装置及びロボットシステムの構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。

１０ 力覚センサー、２０ 力制御部、２１ インピーダンス処理部、
２２ デジタルフィルター部、２３ インバースキネマティクス処理部、
２４−１ 第１の制御パラメーター記憶部、
２４−２ 第２の制御パラメーター記憶部、
２４−３ 閾値記憶部（基準外力方向記憶部）、
２５ 帯域制限部、２６ 制御パラメーター選択部、２７ 外力方向判定部、
２９ 状態設定部、３０ 入力補正部、４０ フォワードキネマティクス処理部、
６０ 目標値出力部、６２ 軌道生成部、６４ インバースキネマティクス処理部、
８０ ロボット制御部、８２−１〜８２−Ｎ 駆動制御部（モーター制御部）、
１００ ロボット、１０２−１〜１０２−Ｎ 駆動部（モーター）、
２２１ デジタルフィルター演算部、２２２ デジタルフィルター係数出力部、
２２３ デジタルフィルター安定度判定部、２２４ デジタルフィルター係数記憶部、
２２５ デジタルフィルター係数変換部、３００ 制御装置、
３１０ ロボット（本体）、３２０ アーム、３３０ ハンド、
５１０ 力覚センサー、５３２ 姿勢補正部、５３４ ハンド・ツール自重補正部、
５３６ 運動方程式処理部、５４０ フォワードキネマティクス処理部、
５６２ 軌道生成部、５６４ インバースキネマティクス処理部

Claims (16)

1. 力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正値を出力する力制御部と、
   前記目標軌道に対して前記補正値に基づく補正処理を行って目標値を求め、求めた前記目標値を出力する目標値出力部と、
   前記目標値に基づいて、前記ロボットのフィードバック制御を行うロボット制御部と、
   を含み、
   前記力制御部は、
   前記検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、
   前記外力の方向が前記第１の方向と反対方向である第２の方向である場合には、前記第１の力制御とは異なる第２の力制御を行うことを特徴とするロボット制御装置。
2. 請求項１において、
   前記力制御部は、
   前記外力の方向が前記第１の方向である場合には、外力に対する変位変化量が第１の変位変化量となる力制御を、前記第１の力制御として行い、
   前記外力の方向が前記第２の方向である場合には、外力に対する前記変位変化量が前記第１の変位変化量とは異なる第２の変位変化量となる力制御を、前記第２の力制御として行うことを特徴とするロボット制御装置。
3. 請求項２において、
   前記力制御部は、
   前記外力の方向が前記第１の方向である場合には、前記第１の変位変化量に対応する第１の補正値を出力する前記第１の力制御を行い、
   前記外力の方向が前記第２の方向である場合には、前記第２の変位変化量に対応する第２の補正値を出力する前記第２の力制御を行うことを特徴とするロボット制御装置。
4. 請求項１乃至３のいずれかにおいて、
   前記力制御部は、
   前記検出センサー値により表される前記外力の方向を判定する外力方向判定部を含むことを特徴とするロボット制御装置。
5. 請求項１乃至４のいずれかにおいて、
   前記力制御部は、
   力制御における制御パラメーターセットを複数記憶する制御パラメーター記憶部と、
   前記制御パラメーター記憶部に記憶された前記制御パラメーターセットの中から、使用する前記制御パラメーターセットの選択処理を行う制御パラメーター選択部と、
   を有し、
   前記力制御部は、
   前記検出センサー値により表される前記外力の方向が前記第１の方向である場合には、第１の制御パラメーターセットを選択する前記選択処理を行い、
   前記外力の方向が前記第２の方向である場合には、第２の制御パラメーターセットを選択する前記選択処理を行い、
   選択した前記制御パラメーターセットを用いて、力制御における常微分方程式の解を前記補正値として求め、前記補正値を出力することを特徴とするロボット制御装置。
6. 請求項５において、
   前記力制御部は、
   前記第１の制御パラメーターセットを用いて力制御を行っている時に、所定の制御パラメーター変更指示があったと判断された場合に、使用する前記制御パラメーターセットを前記第１の制御パラメーターセットから前記第２の制御パラメーターセットへと変更する変更処理を行うことを特徴とするロボット制御装置。
7. 請求項６において、
   前記力制御部は、
   前記第２の制御パラメーターセットを用いて力制御を行っている場合には、前記第１の方向への外力の大きさが第１の閾値を超えたと判断した時に、使用する前記制御パラメーターセットを前記第１の制御パラメーターセットに変更する前記変更処理を行い、
   前記第１の制御パラメーターセットを用いて力制御を行っている場合には、前記第２の方向への外力の大きさが第２の閾値を超えたと判断した時に、使用する前記制御パラメーターセットを前記第２の制御パラメーターセットに変更する前記変更処理を行うことを特徴とするロボット制御装置。
8. 請求項１乃至７のいずれかにおいて、
   前記力制御部は、
   前記検出センサー値により表される外力の方向が前記第２の方向である場合には、前記第１の力制御を行って求めた第１の補正値と、前記第２の力制御を行って求めた第２の補正値とに対して重み付け処理を行って、第３の補正値を求め、求めた前記第３の補正値を出力することを特徴とするロボット制御装置。
9. 請求項１乃至８のいずれかにおいて、
   前記力制御部は、
   力制御における常微分方程式の解を前記補正値として求めるデジタルフィルター部を有することを特徴とするロボット制御装置。
10. 請求項９において、
    前記力制御部は、
    前記補正値を求める前記デジタルフィルター部の動作の安定度を判定し、
    前記デジタルフィルター部の前記動作が安定であると判定された場合に、力制御における前記常微分方程式の解を前記補正値として求めることを特徴とするロボット制御装置。
11. 請求項９又は１０のいずれかにおいて、
    前記常微分方程式は、
    仮想質量項、仮想粘性項及び仮想弾性項を係数パラメーターとする運動方程式であることを特徴とするロボット制御装置。
12. 請求項１乃至１１のいずれかの前記ロボット制御装置と、
    前記目標値出力部から取得される前記目標値に基づいて、各部を動作させる前記ロボットと、
    を含むことを特徴とするロボットシステム。
13. 力覚センサーから取得される検出センサー値に基づいて、ロボットの目標軌道の補正値として、力制御における常微分方程式の解を求め、
    前記検出センサー値により表される外力の方向が第１の方向である場合には、第１の力制御を行い、
    前記外力の方向が前記第１の方向と反対方向である第２の方向である場合には、前記第１の力制御とは異なる第２の力制御を行い、
    前記目標軌道に対して前記補正値に基づく補正処理を行って目標値を求め、求めた前記目標値を出力し、
    前記目標値に基づいて、前記ロボットのフィードバック制御を行うことを特徴とするロボット制御方法。
14. 請求項１３において、
    前記外力の方向が前記第１の方向である場合には、外力に対する変位変化量が第１の変位変化量となる力制御を、前記第１の力制御として行い、
    前記外力の方向が前記第２の方向である場合には、外力に対する前記変位変化量が前記第１の変位変化量とは異なる第２の変位変化量となる力制御を、前記第２の力制御として行うことを特徴とするロボット制御方法。
15. 力覚センサーを有するロボットの制御を行うロボット制御方法であって、
    前記力覚センサーに加えられた外力の方向に応じて、前記ロボットの変位の大きさが異なる力制御を行うことを特徴とするロボット制御方法。
16. 力覚センサーを有するロボットの制御を行うロボット制御方法であって、
    前記力覚センサーに第１の外力が加えられた場合の前記ロボットの変位の大きさと、前記力覚センサーに前記第１の外力と同じ大きさであって反対の方向の力である第２の外力が加えられた場合の前記ロボットの変位の大きさとが異なる力制御を行うことを特徴とするロボット制御方法。

Images