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