制御装置及び方法
【課題】アドミッタンス制御の不安定化を緩和できる制御装置を提供する。
【解決手段】ロボット制御装置１は、推力を発生させるアクチュエータ１２とエンコーダ１５と力覚センサ１１とを有したアーム１０を制御する力制御手段３の内部に、アームの質量よりも小さな０でない質量を有した仮想物体について予め定められた、仮想質量ｍd、仮想粘性ｃdおよび仮想物体に加えられる仮想的な力としての目標力ｆdの各パラメータを含む運動方程式を力覚センサ１１で検出された接触力ｆを入力として用いて解くことで目標位置ｐdを算出する仮想物体運動算出手段７と、目標位置ｐdとエンコーダ１５で検出されたアームの位置ｐとを入力としてプロクシベーストスライディングモード制御を用いてアームに加える推力ｇを算出し、これをアクチュエータ１２への指令値とする位置制御手段８とを備える。
【解決手段】ロボット制御装置１は、推力を発生させるアクチュエータ１２とエンコーダ１５と力覚センサ１１とを有したアーム１０を制御する力制御手段３の内部に、アームの質量よりも小さな０でない質量を有した仮想物体について予め定められた、仮想質量ｍd、仮想粘性ｃdおよび仮想物体に加えられる仮想的な力としての目標力ｆdの各パラメータを含む運動方程式を力覚センサ１１で検出された接触力ｆを入力として用いて解くことで目標位置ｐdを算出する仮想物体運動算出手段７と、目標位置ｐdとエンコーダ１５で検出されたアームの位置ｐとを入力としてプロクシベーストスライディングモード制御を用いてアームに加える推力ｇを算出し、これをアクチュエータ１２への指令値とする位置制御手段８とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、制御装置及び方法の技術に関し、より詳しくは、位置制御ベースの力制御を行う制御装置及び方法に関する。
【背景技術】
【０００２】
従来、マニピュレータの手先の位置を、マニピュレータを保持するアクチュエータによって力制限を付しながら目標位置に速やかに追従させる位置制御装置が知られている（特許文献１参照）。特許文献１に開示された位置制御装置に組み込まれている制御則は、プロクシベーストスライディングモード制御（ＰＳＭＣ：Proxy-based sliding mode control）と呼ばれている。ＰＳＭＣの物理的解釈について図９を参照して説明する。ＰＳＭＣの制御器は、設定された目標位置９０１をｐdとしたときに、マニピュレータの手先等の制御対象９０２に対して直接的に力を加えるのではなく、仮想空間内に想定されたプロクシ（proxy）９０３と呼ばれる質量のない点状の仮想的な物体を介して力を加えるものである。
【０００３】
プロクシ９０３は、制御対象９０２と仮想連結要素（virtual coupling）９０４によって結合されており、制御対象９０２とともに仮想空間内を移動する。図示した仮想連結要素９０４において、Ｋは比例ゲイン、Ｂは微分ゲインを表しており、制御器内（計算器内）では、制御対象９０２の代理点であるプロクシ９０３は、ＰＤ制御で制御対象９０２に接続される。プロクシ９０３は、スライディングモード制御則に基づくＳＭＣ（スライディングモード制御器）９０５にも接続される。プロクシ９０３にはＰＤ制御による力とＳＭＣ（スライディングモード制御）９０５による力が働く。これらの力は同じ力であって、常に平衡状態にある。このことは、プロクシ９０３が質量を持っていないことから、物理的に矛盾が生じない。ＰＳＭＣでは、検出された制御対象９０２の位置ｐによってプロクシ９０３の位置ｐsが推定され、プロクシ９０３に力を加えることで、プロクシ９０３を目標位置ｐdに移動させるように位置制御が行われる。ＰＳＭＣの詳細は、「R. Kikuuwe and H. Fujimoto, “Proxy-based sliding mode control for accurate and safe position control” , Proc. of the 2006 IEEE Int. Conf. on Robotics and Automation, 2006, p.26-31」に記載されている。
【０００４】
また、ロボットでは、マニピュレータの手先等を示す制御対象の位置を制御する位置制御装置のほかに、制御対象の力を制御する力制御装置も知られている。位置情報を入力として力の指令値を出力する制御（位置入力・力出力）はインピーダンス型の制御（インピーダンス制御）を行う制御装置と言われている。また、力情報を入力として位置の指令値を出力する制御（力入力・位置出力）はアドミッタンス型の制御（アドミッタンス制御）を行う制御装置と言われている。
【０００５】
従来からアドミッタンス制御の制御装置において、力制御の内部の位置制御にＰ制御、ＰＤ制御又はＰＩＤ制御等を用いた制御装置が一般に用いられている。つまり、位置制御ベースの力制御において、内部の位置制御にＰ制御やＰＤ制御やＰＩＤ制御が用いられている。位置制御ベースにすることで、実際のアクチュエータの摩擦等の影響を抑制することができるという利点がある。
【０００６】
一例として、このようなアドミッタンス制御の制御装置を、人と協働して作業するロボットアームに適用する場合には、例えば、ロボットアームが人に衝突したときに人への衝撃を緩和するために即座にロボットアームが人から離間する向きの推力を発生するような制御が要求されることとなる。このようにロボットアームが人に衝突したときの応答性を向上させるために、制御装置の内部コントロールとして仮想空間を想定して、仮想空間上において制御対象物であるロボットアームを制御する方法が考えられる。具体的には、ロボットアームの力制御において、ロボットアームに仮想的に付与する仮想質量や仮想粘性を、０ではない小さな値に設定することで、応答性を向上させることが考えられる。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００７−１０２７４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、内部の位置制御にＰ制御、ＰＤ制御又はＰＩＤ制御等を用いた従来のアドミッタンス制御の制御装置において、ロボットアームに付与する仮想質量等を小さく設定する制御には、限界がある。その理由は、力制御は、運動方程式ベースなので、力制御の内部においては、ロボットアームに設定する仮想質量の低減に伴って、位置制御器への指令値を生成するもととなる加速度が増大し、実質的に高ゲインとなって不安定化するからである。言い換えると、質量を軽く設定すると、ロボットアームが実際には重いにも関わらずあたかも軽いように振舞おうとするので、不安定化する。つまり、ロボットアームに設定する仮想質量の低減には限界がある。仮想質量の低減が限界に達すると、ロボットアームが人に接触したときの接触力が時間経過に伴って一定値にはならずに振動してしまう。つまり、このような制御方法では、アドミッタンス制御が不安定になってしまうという問題がある。
【０００９】
そこで、本発明では、前記した問題を解決し、従来のアドミッタンス制御の不安定化を緩和できる制御装置及び方法を提供することを課題とする。
【課題を解決するための手段】
【００１０】
前記課題を解決するために、本願発明者らは、アドミッタンス制御において種々検討を行った。その結果、力制御のベースとなる位置制御にＰ制御、ＰＤ制御又はＰＩＤ制御等を用いた従来の制御にて設定する仮想質量の低減が限界となる場合であっても、位置制御にＰＳＭＣ（プロクシベーストスライディングモード制御）を用いることで、アドミッタンス制御の不安定化を緩和できることを見出した。
【００１１】
そこで、本発明に係る制御装置は、位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置であって、前記位置制御手段が、プロクシベーストスライディングモード制御を用いることを特徴とする。
【００１２】
また、本発明に係る制御装置は、前記位置制御手段が、比例動作を少なくとも含むフィードバック制御を行う比例制御を含むフィードバック制御器と、スライディングモード制御則に基づくスライディングモード制御器とを備え、前記比例制御を含むフィードバック制御器と前記スライディングモード制御器とを協働させて前記プロクシベーストスライディングモード制御を行うものであって、前記比例制御を含むフィードバック制御器が、仮想空間における質量のない点状の仮想的な物体であるプロクシと前記外部の物体との間を仮想的に連結する仮想連結要素から前記プロクシに対して加えられる仮想的な粘弾性力を算出し、前記スライディングモード制御器が、前記粘弾性力と釣り合う力であって前記プロクシの位置を当該スライディングモード制御器に設定された目標位置に収束させる制御を行う際に前記プロクシに加えられる仮想的な力を前記物体に加える推力として算出することが好ましい。
【００１３】
また、前記物体が、当該物体に加える推力を発生させる駆動機構と位置検出センサとを備えている態様において、本発明に係る制御装置は、前記力制御手段が、前記物体に対応して前記物体の実質量よりも小さな０でない所定の質量を有した仮想物体について予め定められた、仮想的な質量、仮想的な粘弾性および前記仮想物体に加えられる仮想的な力としての目標力の各パラメータを含む前記仮想物体についての運動方程式を前記接触力を入力として用いて解くことで前記仮想物体の目標位置を算出する仮想物体運動算出手段を備え、前記位置制御手段が、前記算出された目標位置と前記位置検出センサで検出された前記物体の位置とを入力として前記プロクシベーストスライディングモード制御を用いて前記物体を駆動する駆動機構にトルク（推力）指令値を出力することが好ましい。
【００１４】
かかる構成によれば、制御装置は、物体に対応して実質量よりも小さな０でない質量を有した仮想物体の運動方程式に基づいて力制御における目標位置を算出すると共に、仮想的な力を物体に加える推力として算出し、指令値として出力することができる。
【００１５】
また、前記課題を解決するために、本発明に係る制御方法は、位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置による制御方法であって、前記位置制御手段が、プロクシベーストスライディングモード制御を用いることを特徴とする。
【発明の効果】
【００１６】
本発明によれば、従来のアドミッタンス制御の不安定化を緩和することができる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態に係るロボット制御装置を含む人ロボット協働システムを模式的に示す概念図である。
【図２】本発明の実施形態に係るロボット制御装置の構成を模式的に示すブロック図である。
【図３】本発明の実施形態に係るロボット制御装置による力制御の流れの概略を示すフローチャートである。
【図４】本発明の実施形態に係るロボット制御装置の性能を検証する検証装置の構成を模式的に示すブロック図である。
【図５】検証装置の仮想物体運動算出器における制御モデルを模式的に示す概念図である。
【図６】検証装置の位置制御器に比較例として用いた出力制限型ＰＤ制御器の制御ブロック図である。
【図７】検証装置の位置制御器に実施例として用いたＰＳＭＣの制御ブロック図である。
【図８】検証装置による検証結果の一例を示すグラフである。
【図９】従来のＰＳＭＣ制御器の物理的解釈を示す模式図である。
【発明を実施するための形態】
【００１８】
以下、本発明を実施するための形態（実施形態という）について図面を参照して詳細に説明する。
【００１９】
［１．人ロボット協働システムの構成］
図１に示す人ロボット協働システム１００は、ロボットＲと、ロボット制御装置１とを備えている。ロボット制御装置１は、ケーブル２０を介してロボットＲに接続されており、ロボットＲのアーム１０が人Ｐと協働して作業できるようにアーム１０を制御するものである。本実施形態では、ロボット制御装置１の外部の物体をアーム１０として説明するが、本発明はこれに限られるものではない。また、図１に示す例では、ロボットＲは２本のアーム１０を備えているが、アームの台数はこれに限定されるものではない。アーム１０としては、作業用途に合わせた形態のものを用いることができる。アーム１０は、例えば多関節で旋回軸が多軸の産業用ロボットアームで構成される。
【００２０】
ロボットＲのアーム１０は、図２に示すように、力覚センサ１１と、アクチュエータ１２とを主に備えている。なお、図２では１つのアーム１０を例示した。
力覚センサ１１は、アーム１０の例えば先端等に１つ又は複数設けられている。力覚センサ１１には、作業に必要な押圧力を検出するセンサ、人Ｐや障害物に接触したことを検出するセンサを含む。なお、図２では１つの力覚センサ１１を例示した。
【００２１】
アクチュエータ１２は、アーム１０の推力を発生させる駆動機構であって、アーム１０の関節等を形成している。アクチュエータ１２は、例えば、関節の屈曲や伸縮の駆動力を発生するサーボモータ１４や位置検出センサとしてのエンコーダ１５を備えている。なお、図２では１つのアクチュエータ１２を例示した。
【００２２】
［２．制御装置の構成］
ロボット制御装置１は、例えば、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disk Drive）、入出力インタフェース等を備え、図２に示すように、目標力設定手段２と、力制御手段３と、サーボアンプ４とを主に備えている。
【００２３】
目標力設定手段２は、アーム１０の推力の目標となる目標力を力制御手段３に設定するものである。目標力の設定値は、例えば、ロボット制御装置１の外部から入力するようにしてもよいし、ロボット制御装置１の内部に記憶しておいてもよい。例えば内部に記憶する場合、例えばアーム１０の行う作業に応じた固定値の目標力を記憶したり、時間経過に応じて変化する目標力等を予めプログラミングして記憶したりすることができる。
例えばアーム１０が人に衝突した場合の目標力ｆdとしては、アーム１０が他の物体に接触した際にダメージを与えず、かつ、応答性を確保できる大きさ、例えば、
（仮想物体の加速度）＝（目標力ｆd−仮想粘性×仮想物体の速度）／（仮想物体の質量）≧４．９ｍ／ｓ2の条件を満たすように設定されている。
【００２４】
力制御手段３は、位置制御手段８を内部に有し、外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体（アーム）と他の物体との間で発生する接触力を制御するものであり、算出したトルク（推力）指令値をサーボアンプ４に出力する。
サーボアンプ４は、トルク（推力）指令値を電流指令値に変換し、電流をサーボモータ１４に出力することで、サーボモータ１４を制御する。
【００２５】
本実施形態では、力制御手段３は、図示するように、目標力取得手段５と、接触力取得手段６と、仮想物体運動算出手段７と、位置制御手段８とを備えることとした。
目標力取得手段５は、目標力設定手段２から目標力を取得するものであり、所定のインタフェースから構成されている。
接触力取得手段６は、アーム１０の力覚センサ１１で検出された接触力を取得するものであり、所定のインタフェースから構成されている。
【００２６】
仮想物体運動算出手段７は、力覚センサ１１で検出された接触力を入力として用いて、予め定められたパラメータを含む仮想物体についての運動方程式を解くことで仮想物体の目標位置を算出するものである。算出される目標位置は、目標位置指令値として位置制御手段８に出力される。ここで、仮想物体とは、アーム１０に対応してアームの実質量よりも小さな０でない所定の質量を有して所定の加速度運動を行うと仮定したときのアームに対応した仮想空間上（計算機内）の物体である。以下では仮想物体とはプロクシを意味するものではないこととする。なお、仮想物体は、仮想物体運動算出手段７による力制御にのみ導入されており、位置制御手段８の処理においては導入されていないが、見かけ上はアーム１０と同じ位置にあると想定してもよい。また、接触力については、力覚センサ１１の検出値を用いずに、推定（シミュレーション）により得ることもできる。
【００２７】
仮想物体運動算出手段７は、仮想物体の動特性を示す以下の式（１）に接触力ｆを入力することで、式（１）左辺の目標加速度指令値（位置指令値ｐdの時間２階微分値）を所定の動作周期で算出する。この仮想物体運動算出器４１０は、式（１）左辺の時間２階微分値に対して積分を２回実行して位置指令値ｐdを目標位置として求め、位置制御手段８に出力する。位置指令値ｐdを目標位置ｐdとも表記する。
【００２８】
【数１】
【００２９】
ここで、パラメータｆdは仮想物体に加えられる仮想の力（目標力）、ｆは仮想物体が人等の被接触体に加える力（接触力）、ｖは仮想物体の速度、ｍdは仮想物体の質量、ｃdは仮想物体の粘性を示す。
例えば、ｆd、ｍd、ｃdとしては予め定められた所定値を付与しておく。
ｆとしては力覚センサ１１の検出値を用いることができる。
ｖとしては、例えば式（１）を積分する計算を行うことにより求めることができる。また、アーム１０の速度の実測値を用いることもできる。
【００３０】
本実施形態では、仮想物体運動算出手段７には、目標力取得手段５から目標力ｆdが入力し、接触力取得手段６から接触力ｆが入力する。また、算出された位置指令値ｐdは位置制御手段８に出力される。
【００３１】
ここで、位置指令値ｐdにおける位置とは、仮想物体の目標位置であり、アクチュエータ１２のエンコーダ１５にて検出することのできる位置（これを単にアクチュエータ１２の位置ともいう）の目標値を示している。エンコーダ１５にて検出することのできる位置とは、エンコーダの出力値、エンコーダ出力から換算されるサーボモータ１４の回転位置、アーム１０の移動した位置の位置座標であってもよい。以下では、エンコーダ１５にて検出された位置を、一例としてアーム１０の位置（これを位置ｐと呼ぶ）とする。よって、アクチュエータ１２の位置とは、一例としてアーム１０の位置と同じ意味で用いる。この意味で、アーム１０の位置は仮想物体の位置と解釈することができる。
【００３２】
位置制御手段８は、公知のプロクシベーストスライディングモード制御（以下、ＰＳＭＣとも表記する）を用いてアーム１０に備わるアクチュエータ１２を駆動するトルク（推力）指令値を出力するものである。
【００３３】
位置制御手段８は、例えば、図９に示す仮想連結要素９０４としてのＰＤ制御器と、図９に示すＳＭＣ（スライディングモード制御器）９０５とを備え、ＰＤ制御器とＳＭＣとを協働させてプロクシベーストスライディングモード制御を行う。この場合、ＰＤ制御器は、仮想空間における質量のない点状の仮想的な物体であるプロクシとアーム１０との間を仮想的に連結する仮想連結要素からプロクシに対して加えられる仮想的な粘弾性力を比例動作および微分動作により算出する。また、ＳＭＣは、仮想的な粘弾性力と釣り合う力であってプロクシの位置をＳＭＣに設定された目標位置に収束させる制御を行う際にプロクシに加えられる仮想的な力をアーム１０に加える推力として算出する。
【００３４】
本実施形態では、位置制御手段８には、仮想物体運動算出手段７から位置指令値ｐdが目標位置として入力すると共に、アクチュエータ１２のエンコーダ１５で検出されたアームの位置ｐが入力する。位置制御手段８は、これら入力に基づいてＰＳＭＣを用いて、アクチュエータ１２を駆動するためのトルク（推力）指令を算出する。
【００３５】
［３．ロボット制御装置による力制御の流れ］
ロボット制御装置１による力制御の流れについて図３を参照（適宜、図２参照）して説明する。図３は、本発明の実施形態に係るロボット制御装置による力制御の流れの概略を示すフローチャートである。ここでは、一例として目標力ｆdが固定値に設定されているものとする。
【００３６】
ロボット制御装置１は、接触力取得手段６によって、力覚センサ１１の出力を取得することで接触力ｆを検出する（ステップＳ１）。接触力ｆが目標力ｆd−ｃdｖよりも小さい場合（ステップＳ２：Ｙｅｓ）、ロボット制御装置１は、仮想物体運動算出手段７によって、目標位置ｐdを現在よりも深くする向きの加速度を設定し（ステップＳ３）、目標位置ｐdを設定（更新）する（ステップＳ７）。目標位置ｐdを現在よりも深い位置に設定することでアーム１０の接触力を高めることができる。
【００３７】
また、接触力ｆの判定において、接触力ｆが目標力ｆd−ｃdｖよりも大きい場合（ステップＳ４：Ｙｅｓ）、ロボット制御装置１は、仮想物体運動算出手段７によって、目標位置ｐdを現在よりも浅くする向きの加速度を設定し（ステップＳ５）、目標位置ｐdを更新する（ステップＳ７）。目標位置ｐdを現在よりも浅い位置に設定することでアーム１０の接触力を低くすることができる。
【００３８】
また、接触力ｆの判定において、接触力ｆと目標力ｆd−ｃdｖとが等しい場合（ステップＳ４：Ｎｏ）、ロボット制御装置１は、加速度をゼロに設定し（ステップＳ６）、目標位置ｐdを更新する（ステップＳ７）。
【００３９】
そして、目標位置ｐdの更新（ステップＳ７）に続いて、ロボット制御装置１は、位置制御手段８によって、トルク（推力）指令値を算出する（ステップＳ８）。そして、ロボット制御装置１は、サーボアンプ４によって、トルク（推力）の指令値を電流指令値に変換し、電流をロボットＲのアーム１０のアクチュエータ１２に供給し（ステップＳ９）、ステップＳ１に戻る。よって、衝突の瞬間に大きな衝突力が発生したとしても、ロボット制御装置１は、これらの処理を繰り返すことで、やがて接触力ｆと目標力ｆd−ｃdｖとが等しくなるような制御、すなわち、アーム１０が停まって人Ｐから離間する向きの推力を出力するように制御をかけることができる。
【００４０】
［４．ロボット制御装置の性能］
ここでは、アーム１０が人に衝突する場合におけるロボット制御装置１の性能について説明する。アーム１０が人に衝突する場合を想定した衝突試験を行って性能を検証した。以下に、４−１．検証装置の構成、４−２．制御モデル、４−３．位置制御器の比較例の構成、４−４．位置制御器の実施例の構成、４−５．衝突試験条件、４−６．試験結果に分けて詳細に説明する。
【００４１】
＜４−１．検証装置の構成＞
衝突試験に用いた検証装置の構成について図４を参照して説明する。図４は、本発明の実施形態に係るロボット制御装置の性能を検証する検証装置の構成を模式的に示すブロック図である。検証装置４００は、図４に示すように、アドミッタンス制御器４０１と、サーボアンプ４０２と、直動アクチュエータ４０３と、衝突体４０４と、力覚センサ４０５と、緩衝材４０７を含む被衝突体４０６と、を備える。
【００４２】
アドミッタンス制御器４０１は、直動アクチュエータ４０３を制御対象として衝突体４０４の位置を移動させることで衝突体４０４による衝突力を制御するためのトルク（推力）指令を出力するものであり、仮想物体運動算出器４１０と、位置制御器４２０とを備える。
【００４３】
仮想物体運動算出器４１０は、図２に示した目標力取得手段５、接触力取得手段６および仮想物体運動算出手段７の機能を備えている。
仮想物体運動算出器４１０には、力制御において仮想物体に設定される目標力ｆdが入力すると共に、力覚センサ４０５で検出される衝突体４０４と被衝突体４０６との間の接触力ｆが入力する。ここで、仮想物体とは、衝突体４０４が実際の質量よりも小さな０でない所定の質量を有すると仮定したときの衝突体に対応した仮想空間上（計算機内）の物体である。仮想物体運動算出器４１０は、後記する制御モデルにしたがって直動アクチュエータ４０３の目標位置である位置指令値ｐdを算出し、位置制御器４２０に出力する。
【００４４】
位置制御器４２０は、仮想物体運動算出器４１０から入力する位置指令値ｐdに基づいて、移動機構である直動アクチュエータ４０３を駆動するためのトルク（推力）指令を算出するものである。位置制御器４２０には、直動アクチュエータ４０３の目標位置として設定された位置指令値ｐdが入力すると共に、直動アクチュエータ４０３の図示しないエンコーダにて検出された位置ｐが入力する。
【００４５】
性能を比較するため、位置制御器４２０として、図６に示す出力制限型ＰＤ制御器６０１を採用した場合の検証装置を比較例とする。出力制限型ＰＤ制御器６０１の詳細については後記する。
また、位置制御器４２０として、図７に示すＰＳＭＣ７０１を採用した場合の検証装置を実施例とする。ＰＳＭＣ７０１は、図２に示す位置制御手段８の機能を有している。詳細については後記する。
【００４６】
サーボアンプ４０２は、図２に示すサーボアンプ４と同様なので説明を省略する。
直動アクチュエータ４０３は、図２に示すアクチュエータ１２と同様の機能を有し、アドミッタンス制御器４０１の制御対象である。直動アクチュエータ４０３は、衝突体４０４を移動させる移動機構であって、サーボモータ、ボールねじ、直動ガイド、エンコーダを備えている。
【００４７】
衝突体４０４は、図２に示すアーム１０の手先に相当し、力覚センサ４０５を備え、直動アクチュエータ４０３の駆動によって直線上にスライド運動をして被衝突体４０６に衝突する。なお、図４では、衝突前の状態を図示した。
力覚センサ４０５は、図２に示す力覚センサ１１と同様の機能を有し、被衝突体４０６に接触したときの接触力を検知し、アドミッタンス制御器４０１にフィードバックする。
被衝突体４０６の衝突体４０４と接触する面には、スポンジからなる緩衝材４０７が設けられている。緩衝材は衝突体に設けられていてもよいし、両方に設けられていてもよい。
【００４８】
＜４−２．制御モデル＞
仮想物体運動算出器４１０において目標位置を算出するための制御モデルについて図５を参照して説明する。図５は、検証装置の仮想物体運動算出器における制御モデルを模式的に示す概念図である。図５に示す構成はアドミッタンス制御の一例である。
仮想物体５０１は、力制御において図４に示す衝突体４０４が実際の質量よりも小さな０でない質量ｍdを有したものと仮定したときの衝突体に対応した計算機内の物体である。なお、図５では、衝突したときの状態を図示した。
【００４９】
被接触体５０２は、図４に示す緩衝材４０７を含む被衝突体４０６に対応した仮想空間上（計算機内）の物体である。仮想物体５０１が被接触体５０２に加える力（接触力）をｆとする。ここで、力ｆは、図４に示す衝突体４０４がシミュレーション通りの運動を実際にした場合、衝突体４０４の接触力と同じものとなる。そこで、接触力ｆを、図４に示す力覚センサ４０５を使って測定した。なお、力覚センサによる直接的な測定をすることなく推定（シミュレーション）により接触力ｆを求めてもよい。
【００５０】
力発生要素５０３は、図４に示す目標力ｆdに対応し、仮想物体５０１に加えられる仮想の力（仮想物体５０１に対して設定された力）を表している。
仮想ダンパ５０４は、仮想物体５０１の粘性ｃdを発生させる仮想空間上（計算機内）のダンパを表している。粘性抵抗（ｃdｖ）を算出するにあたり、仮想物体５０１の速度ｖとしては、前記式（１）を積分した値を用いた。
仮想物体５０１の目標位置ｐdは、衝突体４０４の目標位置である位置指令値ｐdを示している。
【００５１】
なお、仮想物体５０１の速度ｖとしては、衝突体４０４の実際の速度を用いてもよい。また、ある速度からの衝突（接触）に限定する必要はない。また、最初から接触状態にあってもよい。
【００５２】
仮想物体運動算出器４１０は、仮想物体５０１の動特性を示す前記した式（１）に接触力ｆを入力することで、式（１）左辺の目標加速度（位置指令値ｐdの時間２階微分値）を所定の動作周期で算出し、さらに積分を２回実行して求めた、仮想物体の目標位置ｐdを算出し、位置制御器４２０に出力する。
【００５３】
＜４−３．位置制御器の比較例の構成＞
力の指令値を算出する位置制御器の比較例について図６を参照（適宜図４参照）して説明する。図６は、検証装置の位置制御器に比較例として用いた出力制限型ＰＤ制御器の制御ブロック図である。
出力制限型ＰＤ制御器６０１は、位置指令値ｐdと制御対象６０２の位置ｐに基づいて制御対象６０２（図４の衝突体４０４）に加える力ｇを算出し、これをアクチュエータへの力指令値とするものである。ここで、制御対象６０２の位置ｐは衝突体４０４の位置すなわちエンコーダの検出値である。図６においてｈは、このシステムに加わる外力であり、ＳＡＴ６０３は以下の式（２）に示す単位飽和関数sat(x)を表している。
【００５４】
【数２】
【００５５】
出力制限型ＰＤ制御器６０１は、図６にて破線で示す枠内の制御ブロックで表される制御として式（３）の右辺を演算することで制御対象６０２（図４の衝突体４０４）に加える力ｇを算出する。
【００５６】
【数３】
【００５７】
ここで、Ｆは、移動機構である直動アクチュエータ４０３が発生する力の上限値を表す。また、Ｋは比例ゲイン、Ｂは微分ゲイン、ｐdは位置指令値、ｐは衝突体４０４の位置すなわち直動アクチュエータ４０３のエンコーダ検出値、sat(x)は式（２）の単位飽和関数を示す。
【００５８】
＜４−４．位置制御器の実施例の構成＞
位置制御器の実施例について図７を参照（適宜図４および図９参照）して説明する。図７は、検証装置の位置制御器に実施例として用いたＰＳＭＣの制御ブロック図である。このＰＳＭＣの制御ブロックは、一例として、ＰＤ制御器（図９の仮想連結要素９０４）と、スライディングモード制御器（図９のＳＭＣ９０５）との組み合わせにより実現されるものである。なお、ＰＳＭＣの制御ブロックは、Ｐ制御器あるいはＰＩＤ制御器と、スライディングモード制御器との組み合わせによっても実現可能である。
【００５９】
図７に示すＰＳＭＣ７０１は、位置指令値ｐd（図９においてはＳＭＣ９０５に設定された目標位置９０１）と制御対象７０２（図９の制御対象９０２）の位置ｐに基づいてアクチュエータへの力指令値ｇを算出するものである。ここで、制御対象７０２の位置ｐはエンコーダの検出値である。また、制御対象７０２（図９の制御対象９０２）には、プロクシ（図９のプロクシ９０３）が接続されているものとする。プロクシは質量のない点状の仮想物体である。プロクシの位置をｐsとする（図９参照）。
【００６０】
図７においてｈは、このシステムに加わる外力であり、ＳＧＮ７０３は以下の式（４）に示す符号関数sgn(x)を表している。
【００６１】
【数４】
【００６２】
式（４）の定義により、ｘ＞０の場合にはsgn(x)＝＋１となり、ｘ＜０の場合にはsgn(x)＝−１となる。また、式（４）はｘ＝０の場合にはsgn(x)が閉区間［−１，１］の任意の値を取りうることを定義している。つまり、図７に示すＳＧＮ７０３は、位置情報を入力とし、−１〜１のいずれかの無次元の値を出力する。なお、式（４）と前記した式（２）との間には、所定の条件を満たすときにy=sgn(x-y)とy=sat(x)とが等価であるという関係があることが知られている（例えば特開２００８−９７１５２号公報参照）。
【００６３】
図７に示すＰＳＭＣ７０１は、図７にて破線で示す枠内の主として下側の制御ブロックで表される制御として式（５）の上側の式と、同枠内の主として上側で表される制御ブロックの制御として式（５）の下側の式と、を解いて制御対象に加える力ｇを算出し、これをアクチュエータへの指令値とする。
【００６４】
【数５】
【００６５】
ここで、Ｈはプロクシが目標位置に収束するときの時定数を表す。また、Ｆは移動機構である直動アクチュエータ４０３が発生する力の上限値、Ｋは比例ゲイン、Ｂは微分ゲイン、ｐsはプロクシの位置、ｐdは位置指令値、ｐは衝突体４０４の位置すなわちエンコーダ検出値、sgn(x)は式（４）の符号関数を示す。
【００６６】
ＰＳＭＣ７０１が発生する発生力ｇは、プロクシに対してスライディングモード制御器（ＳＭＣ９０５）から加えられる力であって、式（５）の上側のｇで表される。また、プロクシに対して仮想連結要素９０４（図９参照）から加えられる力（仮想的な粘弾性力）は、式（５）の下側のｇで表され、式（５）の上側のｇと同じ力である。なお、式（５）の導出過程の詳細については例えば特許文献１に記載されている。
【００６７】
＜４−５．衝突試験条件＞
衝突試験における各種の条件を以下に説明する。
≪４−５−１．アドミッタンス制御パラメータ≫
アドミッタンス制御パラメータとは、アドミッタンス制御器４０１の仮想物体運動算出器４１０にて前記した式（１）を演算するための以下のパラメータのことである。すなわち、アドミッタンス制御パラメータは、仮想物体に加えられる仮想の力（目標力）ｆd、仮想物体の質量ｍd、仮想物体の粘性ｃdである。これらｆd、ｍd、ｃdとして次の値を予め設定した。
ｆd：５［Ｎ］
ｍd：０．７［ｋｇ］
ｃd：０．００８［Ｎｓ／ｍｍ］
【００６８】
≪４−５−２．位置制御器パラメータ≫
位置制御器パラメータとは、アドミッタンス制御器４０１の位置制御器４２０にて前記した式（２）〜（５）を演算するための以下のパラメータのことである。すなわち、位置制御器パラメータは、比較例および実施例に関して比例ゲインＫ、微分ゲインＢ、アクチュエータが発生する力の上限値Ｆと、実施例に関しての時定数Ｈである。これらＫ、Ｂ、Ｆ、Ｈとして次の値を予め設定した。
Ｋ：１０００［Ｎ／ｍｍ］
Ｂ：３［Ｎｓ／ｍｍ］
Ｆ：４５［Ｎ］
Ｈ：０．１［ｓ］（ＰＳＭＣ７０１に対してのみ用いた）
【００６９】
≪４−５−３．その他の検証条件≫
その他の検証条件として、次の値を実測した。
衝突体の実質量ｍ：４．５［ｋｇ］
緩衝材：ＥＰＤＭスポンジ厚ｔ：１０［ｍｍ］
衝突直前の速度：ｖ＝１５０［ｍｍ／ｓ］
なお、ＥＰＤＭは、エチレン−プロピレン−ジエンゴムである。
【００７０】
＜４−６．試験結果＞
図６に示す出力制限型ＰＤ制御器６０１を位置制御器４２０として採用した場合の検証装置（比較例）と、図７に示すＰＳＭＣ７０１を位置制御器４２０として採用した場合の検証装置（実施例）とを用いて、上記衝突試験条件のもとで、衝突試験をそれぞれ行った。衝突試験は、一例として実質量４．５ｋｇの衝突体を直線上で移動させて、衝突直前に時速０．５４ｋｍで被衝突体に衝突させるものである。この衝突試験は、例えば、上から下へと直線上に移動するロボットアームの下に、人が誤って手を出して衝突した（はさまれた）場合を想定している。衝突試験の結果によって、比較例や実施例の検証装置が、人への衝撃を緩和するために即座にロボットアームが人から離間する向きの推力を発生させるような制御が安定にできるか検証することが可能となる。
【００７１】
各検証装置による検証結果として、衝突体４０４が被衝突体４０６に衝突したときに測定された接触力ｆの時間変動を図８に示す。図８に示すグラフの横軸は時刻ｔ［秒］を示し、縦軸は接触力ｆ［Ｎ］を示す。接触力が５［Ｎ］のライン（細い実線）は、予め設定されたアドミッタンス制御パラメータの目標力ｆdを示す。破線はベースの位置制御を出力制限型ＰＤ制御とした場合の力制御（比較例）による衝突体の接触力ｆを示す。太い実線はベースの位置制御をＰＳＭＣとした場合の力制御（実施例）による衝突体の接触力ｆを示す。
【００７２】
比較例は、力制御の内部の位置制御にＰＤ制御を用いた従来の制御装置の場合の効果を検証したことに相当する。具体的には、衝突試験では、ロボットアームに対応した衝突体（実質量ｍ＝４．５ｋｇ）に対して実質量の約１５％にあたる質量（仮想物体の質量ｍd＝０．７ｋｇ）を仮想質量として設定した。
【００７３】
この例では、図示するようにｔ＝０．２５秒のときに衝突した。比較例の場合、図８に破線で示すように、ｔ＝０．３秒（衝突後０．０５秒）のときには、目標力ｆdを僅かに超える程度（約１０Ｎ）の接触力ｆを示す。その後、接触力ｆは０Ｎまたはマイナスの接触力となるが、ｔ＝０．４秒（衝突後０．１５秒）のときには、かろうじてアクチュエータが発生する力の上限値Ｆ（＝４５Ｎ）以内の約４０Ｎの接触力を示した。その後、接触力ｆは０Ｎまたはマイナスの接触力となるが、ｔ＝０．６秒（衝突後０．３５秒）のときには、設定された力の上限値Ｆ（＝４５Ｎ）をはるかに超え、目標力ｆdの２２倍となる約１１０Ｎの接触力を示した。以下同様に、図８に破線で示すように、大きな接触力と０あるいはマイナスの接触力とが時間的に交互に表れる波形が衝突から１秒間に６回も表れ、衝突後１秒以上経過しても接触力が振動する現象が観測された。
なお、図示を省略するが、安定化のため（振動をなくすため）仮想質量をより大きく設定した場合は接触力（衝突力）が大きくなった。また、接触力を下げるため仮想質量をより小さく設定すると振動が観測された。
【００７４】
実施例の場合、図８に太い実線で示すように、ｔ＝０．３秒（衝突後０．０５秒）のときには、比較例と同様に、目標力ｆdを僅かに超える程度（約１０Ｎ）の接触力ｆを示す。その後、接触力はこれ以上大きくなることはなく、目標力ｆdに向かって減衰振動し、衝突後０．２〜０．３秒で収束しており安定であった。実施例は、比較例と同じ衝突試験条件において、力制御の内部の位置制御にＰＳＭＣを用いた制御装置とすることで、接触力が小さくなり、かつ、振動も短時間に収束したので、従来のアドミッタンス制御の不安定化を緩和できる効果を奏することを検証することができた。なお、衝撃低減効果は一例であり、衝撃力検証のために実施した実験は一例である。
【００７５】
力制御の内部の位置制御において従来のＰＤ制御をＰＳＭＣに置き換えることでどのような現象が生じるかを事前に想定することは困難であった。ゆえに、前記した試験結果のように、力制御の内部の位置制御にＰＳＭＣを用いることで接触力の振動が緩和されることは、容易に想到できない顕著な効果である。
【００７６】
以上説明したように、本発明の実施形態に係るロボット制御装置１は、力制御のベースとなる位置制御にＰＤ制御等を用いた従来の制御にて設定する仮想質量の低減が限界となる場合であっても、アドミッタンス制御の不安定化を緩和することができる。
【００７７】
以上、本発明の制御装置の好ましい実施形態について説明したが、本発明は、前記した実施形態に限定されるものではない。例えば、ＰＳＭＣはＰＤ型に限らず、Ｐ型，ＰＩＤ型でもよい。
【００７８】
また、本実施形態では、制御装置がロボットアームの力制御を行うものとして説明したが、本発明は、ロボットアームの力制御に限らず、アドミッタンス制御全般に幅広く応用することが可能である。この場合、アドミッタンス制御全般の安定化に寄与する効果を奏することができる。
【符号の説明】
【００７９】
１００ 人ロボット協働システム
１ ロボット制御装置
２ 目標力設定手段
３ 力制御手段
４ サーボアンプ
５ 目標力取得手段
６ 接触力取得手段
７ 仮想物体運動算出手段
８ 位置制御手段
１０ アーム
１１ 力覚センサ
１２ アクチュエータ
１４ サーボモータ
１５ エンコーダ
２０ ケーブル
４０１ アドミッタンス制御器
４０２ サーボアンプ
４０３ 直動アクチュエータ
４０４ 衝突体
４０５ 力覚センサ
４０６ 被衝突体
４０７ 緩衝材
４１０ 仮想物体運動算出器
４２０ 位置制御器
５０１ 仮想物体
５０２ 被接触体
５０３ 力発生要素
５０４ 仮想ダンパ
６０１ 出力制限型ＰＤ制御器
６０２ 制御対象
６０３ 単位飽和関数
７０１ ＰＳＭＣ
７０２ 制御対象
７０３ 符号関数
９０１ 目標位置
９０２ 制御対象
９０３ プロクシ
９０４ 仮想的な保持要素
９０５ スライディングモード制御
Ｐ 人
Ｒ ロボット
【技術分野】
【０００１】
本発明は、制御装置及び方法の技術に関し、より詳しくは、位置制御ベースの力制御を行う制御装置及び方法に関する。
【背景技術】
【０００２】
従来、マニピュレータの手先の位置を、マニピュレータを保持するアクチュエータによって力制限を付しながら目標位置に速やかに追従させる位置制御装置が知られている（特許文献１参照）。特許文献１に開示された位置制御装置に組み込まれている制御則は、プロクシベーストスライディングモード制御（ＰＳＭＣ：Proxy-based sliding mode control）と呼ばれている。ＰＳＭＣの物理的解釈について図９を参照して説明する。ＰＳＭＣの制御器は、設定された目標位置９０１をｐdとしたときに、マニピュレータの手先等の制御対象９０２に対して直接的に力を加えるのではなく、仮想空間内に想定されたプロクシ（proxy）９０３と呼ばれる質量のない点状の仮想的な物体を介して力を加えるものである。
【０００３】
プロクシ９０３は、制御対象９０２と仮想連結要素（virtual coupling）９０４によって結合されており、制御対象９０２とともに仮想空間内を移動する。図示した仮想連結要素９０４において、Ｋは比例ゲイン、Ｂは微分ゲインを表しており、制御器内（計算器内）では、制御対象９０２の代理点であるプロクシ９０３は、ＰＤ制御で制御対象９０２に接続される。プロクシ９０３は、スライディングモード制御則に基づくＳＭＣ（スライディングモード制御器）９０５にも接続される。プロクシ９０３にはＰＤ制御による力とＳＭＣ（スライディングモード制御）９０５による力が働く。これらの力は同じ力であって、常に平衡状態にある。このことは、プロクシ９０３が質量を持っていないことから、物理的に矛盾が生じない。ＰＳＭＣでは、検出された制御対象９０２の位置ｐによってプロクシ９０３の位置ｐsが推定され、プロクシ９０３に力を加えることで、プロクシ９０３を目標位置ｐdに移動させるように位置制御が行われる。ＰＳＭＣの詳細は、「R. Kikuuwe and H. Fujimoto, “Proxy-based sliding mode control for accurate and safe position control” , Proc. of the 2006 IEEE Int. Conf. on Robotics and Automation, 2006, p.26-31」に記載されている。
【０００４】
また、ロボットでは、マニピュレータの手先等を示す制御対象の位置を制御する位置制御装置のほかに、制御対象の力を制御する力制御装置も知られている。位置情報を入力として力の指令値を出力する制御（位置入力・力出力）はインピーダンス型の制御（インピーダンス制御）を行う制御装置と言われている。また、力情報を入力として位置の指令値を出力する制御（力入力・位置出力）はアドミッタンス型の制御（アドミッタンス制御）を行う制御装置と言われている。
【０００５】
従来からアドミッタンス制御の制御装置において、力制御の内部の位置制御にＰ制御、ＰＤ制御又はＰＩＤ制御等を用いた制御装置が一般に用いられている。つまり、位置制御ベースの力制御において、内部の位置制御にＰ制御やＰＤ制御やＰＩＤ制御が用いられている。位置制御ベースにすることで、実際のアクチュエータの摩擦等の影響を抑制することができるという利点がある。
【０００６】
一例として、このようなアドミッタンス制御の制御装置を、人と協働して作業するロボットアームに適用する場合には、例えば、ロボットアームが人に衝突したときに人への衝撃を緩和するために即座にロボットアームが人から離間する向きの推力を発生するような制御が要求されることとなる。このようにロボットアームが人に衝突したときの応答性を向上させるために、制御装置の内部コントロールとして仮想空間を想定して、仮想空間上において制御対象物であるロボットアームを制御する方法が考えられる。具体的には、ロボットアームの力制御において、ロボットアームに仮想的に付与する仮想質量や仮想粘性を、０ではない小さな値に設定することで、応答性を向上させることが考えられる。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００７−１０２７４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、内部の位置制御にＰ制御、ＰＤ制御又はＰＩＤ制御等を用いた従来のアドミッタンス制御の制御装置において、ロボットアームに付与する仮想質量等を小さく設定する制御には、限界がある。その理由は、力制御は、運動方程式ベースなので、力制御の内部においては、ロボットアームに設定する仮想質量の低減に伴って、位置制御器への指令値を生成するもととなる加速度が増大し、実質的に高ゲインとなって不安定化するからである。言い換えると、質量を軽く設定すると、ロボットアームが実際には重いにも関わらずあたかも軽いように振舞おうとするので、不安定化する。つまり、ロボットアームに設定する仮想質量の低減には限界がある。仮想質量の低減が限界に達すると、ロボットアームが人に接触したときの接触力が時間経過に伴って一定値にはならずに振動してしまう。つまり、このような制御方法では、アドミッタンス制御が不安定になってしまうという問題がある。
【０００９】
そこで、本発明では、前記した問題を解決し、従来のアドミッタンス制御の不安定化を緩和できる制御装置及び方法を提供することを課題とする。
【課題を解決するための手段】
【００１０】
前記課題を解決するために、本願発明者らは、アドミッタンス制御において種々検討を行った。その結果、力制御のベースとなる位置制御にＰ制御、ＰＤ制御又はＰＩＤ制御等を用いた従来の制御にて設定する仮想質量の低減が限界となる場合であっても、位置制御にＰＳＭＣ（プロクシベーストスライディングモード制御）を用いることで、アドミッタンス制御の不安定化を緩和できることを見出した。
【００１１】
そこで、本発明に係る制御装置は、位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置であって、前記位置制御手段が、プロクシベーストスライディングモード制御を用いることを特徴とする。
【００１２】
また、本発明に係る制御装置は、前記位置制御手段が、比例動作を少なくとも含むフィードバック制御を行う比例制御を含むフィードバック制御器と、スライディングモード制御則に基づくスライディングモード制御器とを備え、前記比例制御を含むフィードバック制御器と前記スライディングモード制御器とを協働させて前記プロクシベーストスライディングモード制御を行うものであって、前記比例制御を含むフィードバック制御器が、仮想空間における質量のない点状の仮想的な物体であるプロクシと前記外部の物体との間を仮想的に連結する仮想連結要素から前記プロクシに対して加えられる仮想的な粘弾性力を算出し、前記スライディングモード制御器が、前記粘弾性力と釣り合う力であって前記プロクシの位置を当該スライディングモード制御器に設定された目標位置に収束させる制御を行う際に前記プロクシに加えられる仮想的な力を前記物体に加える推力として算出することが好ましい。
【００１３】
また、前記物体が、当該物体に加える推力を発生させる駆動機構と位置検出センサとを備えている態様において、本発明に係る制御装置は、前記力制御手段が、前記物体に対応して前記物体の実質量よりも小さな０でない所定の質量を有した仮想物体について予め定められた、仮想的な質量、仮想的な粘弾性および前記仮想物体に加えられる仮想的な力としての目標力の各パラメータを含む前記仮想物体についての運動方程式を前記接触力を入力として用いて解くことで前記仮想物体の目標位置を算出する仮想物体運動算出手段を備え、前記位置制御手段が、前記算出された目標位置と前記位置検出センサで検出された前記物体の位置とを入力として前記プロクシベーストスライディングモード制御を用いて前記物体を駆動する駆動機構にトルク（推力）指令値を出力することが好ましい。
【００１４】
かかる構成によれば、制御装置は、物体に対応して実質量よりも小さな０でない質量を有した仮想物体の運動方程式に基づいて力制御における目標位置を算出すると共に、仮想的な力を物体に加える推力として算出し、指令値として出力することができる。
【００１５】
また、前記課題を解決するために、本発明に係る制御方法は、位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置による制御方法であって、前記位置制御手段が、プロクシベーストスライディングモード制御を用いることを特徴とする。
【発明の効果】
【００１６】
本発明によれば、従来のアドミッタンス制御の不安定化を緩和することができる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態に係るロボット制御装置を含む人ロボット協働システムを模式的に示す概念図である。
【図２】本発明の実施形態に係るロボット制御装置の構成を模式的に示すブロック図である。
【図３】本発明の実施形態に係るロボット制御装置による力制御の流れの概略を示すフローチャートである。
【図４】本発明の実施形態に係るロボット制御装置の性能を検証する検証装置の構成を模式的に示すブロック図である。
【図５】検証装置の仮想物体運動算出器における制御モデルを模式的に示す概念図である。
【図６】検証装置の位置制御器に比較例として用いた出力制限型ＰＤ制御器の制御ブロック図である。
【図７】検証装置の位置制御器に実施例として用いたＰＳＭＣの制御ブロック図である。
【図８】検証装置による検証結果の一例を示すグラフである。
【図９】従来のＰＳＭＣ制御器の物理的解釈を示す模式図である。
【発明を実施するための形態】
【００１８】
以下、本発明を実施するための形態（実施形態という）について図面を参照して詳細に説明する。
【００１９】
［１．人ロボット協働システムの構成］
図１に示す人ロボット協働システム１００は、ロボットＲと、ロボット制御装置１とを備えている。ロボット制御装置１は、ケーブル２０を介してロボットＲに接続されており、ロボットＲのアーム１０が人Ｐと協働して作業できるようにアーム１０を制御するものである。本実施形態では、ロボット制御装置１の外部の物体をアーム１０として説明するが、本発明はこれに限られるものではない。また、図１に示す例では、ロボットＲは２本のアーム１０を備えているが、アームの台数はこれに限定されるものではない。アーム１０としては、作業用途に合わせた形態のものを用いることができる。アーム１０は、例えば多関節で旋回軸が多軸の産業用ロボットアームで構成される。
【００２０】
ロボットＲのアーム１０は、図２に示すように、力覚センサ１１と、アクチュエータ１２とを主に備えている。なお、図２では１つのアーム１０を例示した。
力覚センサ１１は、アーム１０の例えば先端等に１つ又は複数設けられている。力覚センサ１１には、作業に必要な押圧力を検出するセンサ、人Ｐや障害物に接触したことを検出するセンサを含む。なお、図２では１つの力覚センサ１１を例示した。
【００２１】
アクチュエータ１２は、アーム１０の推力を発生させる駆動機構であって、アーム１０の関節等を形成している。アクチュエータ１２は、例えば、関節の屈曲や伸縮の駆動力を発生するサーボモータ１４や位置検出センサとしてのエンコーダ１５を備えている。なお、図２では１つのアクチュエータ１２を例示した。
【００２２】
［２．制御装置の構成］
ロボット制御装置１は、例えば、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disk Drive）、入出力インタフェース等を備え、図２に示すように、目標力設定手段２と、力制御手段３と、サーボアンプ４とを主に備えている。
【００２３】
目標力設定手段２は、アーム１０の推力の目標となる目標力を力制御手段３に設定するものである。目標力の設定値は、例えば、ロボット制御装置１の外部から入力するようにしてもよいし、ロボット制御装置１の内部に記憶しておいてもよい。例えば内部に記憶する場合、例えばアーム１０の行う作業に応じた固定値の目標力を記憶したり、時間経過に応じて変化する目標力等を予めプログラミングして記憶したりすることができる。
例えばアーム１０が人に衝突した場合の目標力ｆdとしては、アーム１０が他の物体に接触した際にダメージを与えず、かつ、応答性を確保できる大きさ、例えば、
（仮想物体の加速度）＝（目標力ｆd−仮想粘性×仮想物体の速度）／（仮想物体の質量）≧４．９ｍ／ｓ2の条件を満たすように設定されている。
【００２４】
力制御手段３は、位置制御手段８を内部に有し、外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体（アーム）と他の物体との間で発生する接触力を制御するものであり、算出したトルク（推力）指令値をサーボアンプ４に出力する。
サーボアンプ４は、トルク（推力）指令値を電流指令値に変換し、電流をサーボモータ１４に出力することで、サーボモータ１４を制御する。
【００２５】
本実施形態では、力制御手段３は、図示するように、目標力取得手段５と、接触力取得手段６と、仮想物体運動算出手段７と、位置制御手段８とを備えることとした。
目標力取得手段５は、目標力設定手段２から目標力を取得するものであり、所定のインタフェースから構成されている。
接触力取得手段６は、アーム１０の力覚センサ１１で検出された接触力を取得するものであり、所定のインタフェースから構成されている。
【００２６】
仮想物体運動算出手段７は、力覚センサ１１で検出された接触力を入力として用いて、予め定められたパラメータを含む仮想物体についての運動方程式を解くことで仮想物体の目標位置を算出するものである。算出される目標位置は、目標位置指令値として位置制御手段８に出力される。ここで、仮想物体とは、アーム１０に対応してアームの実質量よりも小さな０でない所定の質量を有して所定の加速度運動を行うと仮定したときのアームに対応した仮想空間上（計算機内）の物体である。以下では仮想物体とはプロクシを意味するものではないこととする。なお、仮想物体は、仮想物体運動算出手段７による力制御にのみ導入されており、位置制御手段８の処理においては導入されていないが、見かけ上はアーム１０と同じ位置にあると想定してもよい。また、接触力については、力覚センサ１１の検出値を用いずに、推定（シミュレーション）により得ることもできる。
【００２７】
仮想物体運動算出手段７は、仮想物体の動特性を示す以下の式（１）に接触力ｆを入力することで、式（１）左辺の目標加速度指令値（位置指令値ｐdの時間２階微分値）を所定の動作周期で算出する。この仮想物体運動算出器４１０は、式（１）左辺の時間２階微分値に対して積分を２回実行して位置指令値ｐdを目標位置として求め、位置制御手段８に出力する。位置指令値ｐdを目標位置ｐdとも表記する。
【００２８】
【数１】
【００２９】
ここで、パラメータｆdは仮想物体に加えられる仮想の力（目標力）、ｆは仮想物体が人等の被接触体に加える力（接触力）、ｖは仮想物体の速度、ｍdは仮想物体の質量、ｃdは仮想物体の粘性を示す。
例えば、ｆd、ｍd、ｃdとしては予め定められた所定値を付与しておく。
ｆとしては力覚センサ１１の検出値を用いることができる。
ｖとしては、例えば式（１）を積分する計算を行うことにより求めることができる。また、アーム１０の速度の実測値を用いることもできる。
【００３０】
本実施形態では、仮想物体運動算出手段７には、目標力取得手段５から目標力ｆdが入力し、接触力取得手段６から接触力ｆが入力する。また、算出された位置指令値ｐdは位置制御手段８に出力される。
【００３１】
ここで、位置指令値ｐdにおける位置とは、仮想物体の目標位置であり、アクチュエータ１２のエンコーダ１５にて検出することのできる位置（これを単にアクチュエータ１２の位置ともいう）の目標値を示している。エンコーダ１５にて検出することのできる位置とは、エンコーダの出力値、エンコーダ出力から換算されるサーボモータ１４の回転位置、アーム１０の移動した位置の位置座標であってもよい。以下では、エンコーダ１５にて検出された位置を、一例としてアーム１０の位置（これを位置ｐと呼ぶ）とする。よって、アクチュエータ１２の位置とは、一例としてアーム１０の位置と同じ意味で用いる。この意味で、アーム１０の位置は仮想物体の位置と解釈することができる。
【００３２】
位置制御手段８は、公知のプロクシベーストスライディングモード制御（以下、ＰＳＭＣとも表記する）を用いてアーム１０に備わるアクチュエータ１２を駆動するトルク（推力）指令値を出力するものである。
【００３３】
位置制御手段８は、例えば、図９に示す仮想連結要素９０４としてのＰＤ制御器と、図９に示すＳＭＣ（スライディングモード制御器）９０５とを備え、ＰＤ制御器とＳＭＣとを協働させてプロクシベーストスライディングモード制御を行う。この場合、ＰＤ制御器は、仮想空間における質量のない点状の仮想的な物体であるプロクシとアーム１０との間を仮想的に連結する仮想連結要素からプロクシに対して加えられる仮想的な粘弾性力を比例動作および微分動作により算出する。また、ＳＭＣは、仮想的な粘弾性力と釣り合う力であってプロクシの位置をＳＭＣに設定された目標位置に収束させる制御を行う際にプロクシに加えられる仮想的な力をアーム１０に加える推力として算出する。
【００３４】
本実施形態では、位置制御手段８には、仮想物体運動算出手段７から位置指令値ｐdが目標位置として入力すると共に、アクチュエータ１２のエンコーダ１５で検出されたアームの位置ｐが入力する。位置制御手段８は、これら入力に基づいてＰＳＭＣを用いて、アクチュエータ１２を駆動するためのトルク（推力）指令を算出する。
【００３５】
［３．ロボット制御装置による力制御の流れ］
ロボット制御装置１による力制御の流れについて図３を参照（適宜、図２参照）して説明する。図３は、本発明の実施形態に係るロボット制御装置による力制御の流れの概略を示すフローチャートである。ここでは、一例として目標力ｆdが固定値に設定されているものとする。
【００３６】
ロボット制御装置１は、接触力取得手段６によって、力覚センサ１１の出力を取得することで接触力ｆを検出する（ステップＳ１）。接触力ｆが目標力ｆd−ｃdｖよりも小さい場合（ステップＳ２：Ｙｅｓ）、ロボット制御装置１は、仮想物体運動算出手段７によって、目標位置ｐdを現在よりも深くする向きの加速度を設定し（ステップＳ３）、目標位置ｐdを設定（更新）する（ステップＳ７）。目標位置ｐdを現在よりも深い位置に設定することでアーム１０の接触力を高めることができる。
【００３７】
また、接触力ｆの判定において、接触力ｆが目標力ｆd−ｃdｖよりも大きい場合（ステップＳ４：Ｙｅｓ）、ロボット制御装置１は、仮想物体運動算出手段７によって、目標位置ｐdを現在よりも浅くする向きの加速度を設定し（ステップＳ５）、目標位置ｐdを更新する（ステップＳ７）。目標位置ｐdを現在よりも浅い位置に設定することでアーム１０の接触力を低くすることができる。
【００３８】
また、接触力ｆの判定において、接触力ｆと目標力ｆd−ｃdｖとが等しい場合（ステップＳ４：Ｎｏ）、ロボット制御装置１は、加速度をゼロに設定し（ステップＳ６）、目標位置ｐdを更新する（ステップＳ７）。
【００３９】
そして、目標位置ｐdの更新（ステップＳ７）に続いて、ロボット制御装置１は、位置制御手段８によって、トルク（推力）指令値を算出する（ステップＳ８）。そして、ロボット制御装置１は、サーボアンプ４によって、トルク（推力）の指令値を電流指令値に変換し、電流をロボットＲのアーム１０のアクチュエータ１２に供給し（ステップＳ９）、ステップＳ１に戻る。よって、衝突の瞬間に大きな衝突力が発生したとしても、ロボット制御装置１は、これらの処理を繰り返すことで、やがて接触力ｆと目標力ｆd−ｃdｖとが等しくなるような制御、すなわち、アーム１０が停まって人Ｐから離間する向きの推力を出力するように制御をかけることができる。
【００４０】
［４．ロボット制御装置の性能］
ここでは、アーム１０が人に衝突する場合におけるロボット制御装置１の性能について説明する。アーム１０が人に衝突する場合を想定した衝突試験を行って性能を検証した。以下に、４−１．検証装置の構成、４−２．制御モデル、４−３．位置制御器の比較例の構成、４−４．位置制御器の実施例の構成、４−５．衝突試験条件、４−６．試験結果に分けて詳細に説明する。
【００４１】
＜４−１．検証装置の構成＞
衝突試験に用いた検証装置の構成について図４を参照して説明する。図４は、本発明の実施形態に係るロボット制御装置の性能を検証する検証装置の構成を模式的に示すブロック図である。検証装置４００は、図４に示すように、アドミッタンス制御器４０１と、サーボアンプ４０２と、直動アクチュエータ４０３と、衝突体４０４と、力覚センサ４０５と、緩衝材４０７を含む被衝突体４０６と、を備える。
【００４２】
アドミッタンス制御器４０１は、直動アクチュエータ４０３を制御対象として衝突体４０４の位置を移動させることで衝突体４０４による衝突力を制御するためのトルク（推力）指令を出力するものであり、仮想物体運動算出器４１０と、位置制御器４２０とを備える。
【００４３】
仮想物体運動算出器４１０は、図２に示した目標力取得手段５、接触力取得手段６および仮想物体運動算出手段７の機能を備えている。
仮想物体運動算出器４１０には、力制御において仮想物体に設定される目標力ｆdが入力すると共に、力覚センサ４０５で検出される衝突体４０４と被衝突体４０６との間の接触力ｆが入力する。ここで、仮想物体とは、衝突体４０４が実際の質量よりも小さな０でない所定の質量を有すると仮定したときの衝突体に対応した仮想空間上（計算機内）の物体である。仮想物体運動算出器４１０は、後記する制御モデルにしたがって直動アクチュエータ４０３の目標位置である位置指令値ｐdを算出し、位置制御器４２０に出力する。
【００４４】
位置制御器４２０は、仮想物体運動算出器４１０から入力する位置指令値ｐdに基づいて、移動機構である直動アクチュエータ４０３を駆動するためのトルク（推力）指令を算出するものである。位置制御器４２０には、直動アクチュエータ４０３の目標位置として設定された位置指令値ｐdが入力すると共に、直動アクチュエータ４０３の図示しないエンコーダにて検出された位置ｐが入力する。
【００４５】
性能を比較するため、位置制御器４２０として、図６に示す出力制限型ＰＤ制御器６０１を採用した場合の検証装置を比較例とする。出力制限型ＰＤ制御器６０１の詳細については後記する。
また、位置制御器４２０として、図７に示すＰＳＭＣ７０１を採用した場合の検証装置を実施例とする。ＰＳＭＣ７０１は、図２に示す位置制御手段８の機能を有している。詳細については後記する。
【００４６】
サーボアンプ４０２は、図２に示すサーボアンプ４と同様なので説明を省略する。
直動アクチュエータ４０３は、図２に示すアクチュエータ１２と同様の機能を有し、アドミッタンス制御器４０１の制御対象である。直動アクチュエータ４０３は、衝突体４０４を移動させる移動機構であって、サーボモータ、ボールねじ、直動ガイド、エンコーダを備えている。
【００４７】
衝突体４０４は、図２に示すアーム１０の手先に相当し、力覚センサ４０５を備え、直動アクチュエータ４０３の駆動によって直線上にスライド運動をして被衝突体４０６に衝突する。なお、図４では、衝突前の状態を図示した。
力覚センサ４０５は、図２に示す力覚センサ１１と同様の機能を有し、被衝突体４０６に接触したときの接触力を検知し、アドミッタンス制御器４０１にフィードバックする。
被衝突体４０６の衝突体４０４と接触する面には、スポンジからなる緩衝材４０７が設けられている。緩衝材は衝突体に設けられていてもよいし、両方に設けられていてもよい。
【００４８】
＜４−２．制御モデル＞
仮想物体運動算出器４１０において目標位置を算出するための制御モデルについて図５を参照して説明する。図５は、検証装置の仮想物体運動算出器における制御モデルを模式的に示す概念図である。図５に示す構成はアドミッタンス制御の一例である。
仮想物体５０１は、力制御において図４に示す衝突体４０４が実際の質量よりも小さな０でない質量ｍdを有したものと仮定したときの衝突体に対応した計算機内の物体である。なお、図５では、衝突したときの状態を図示した。
【００４９】
被接触体５０２は、図４に示す緩衝材４０７を含む被衝突体４０６に対応した仮想空間上（計算機内）の物体である。仮想物体５０１が被接触体５０２に加える力（接触力）をｆとする。ここで、力ｆは、図４に示す衝突体４０４がシミュレーション通りの運動を実際にした場合、衝突体４０４の接触力と同じものとなる。そこで、接触力ｆを、図４に示す力覚センサ４０５を使って測定した。なお、力覚センサによる直接的な測定をすることなく推定（シミュレーション）により接触力ｆを求めてもよい。
【００５０】
力発生要素５０３は、図４に示す目標力ｆdに対応し、仮想物体５０１に加えられる仮想の力（仮想物体５０１に対して設定された力）を表している。
仮想ダンパ５０４は、仮想物体５０１の粘性ｃdを発生させる仮想空間上（計算機内）のダンパを表している。粘性抵抗（ｃdｖ）を算出するにあたり、仮想物体５０１の速度ｖとしては、前記式（１）を積分した値を用いた。
仮想物体５０１の目標位置ｐdは、衝突体４０４の目標位置である位置指令値ｐdを示している。
【００５１】
なお、仮想物体５０１の速度ｖとしては、衝突体４０４の実際の速度を用いてもよい。また、ある速度からの衝突（接触）に限定する必要はない。また、最初から接触状態にあってもよい。
【００５２】
仮想物体運動算出器４１０は、仮想物体５０１の動特性を示す前記した式（１）に接触力ｆを入力することで、式（１）左辺の目標加速度（位置指令値ｐdの時間２階微分値）を所定の動作周期で算出し、さらに積分を２回実行して求めた、仮想物体の目標位置ｐdを算出し、位置制御器４２０に出力する。
【００５３】
＜４−３．位置制御器の比較例の構成＞
力の指令値を算出する位置制御器の比較例について図６を参照（適宜図４参照）して説明する。図６は、検証装置の位置制御器に比較例として用いた出力制限型ＰＤ制御器の制御ブロック図である。
出力制限型ＰＤ制御器６０１は、位置指令値ｐdと制御対象６０２の位置ｐに基づいて制御対象６０２（図４の衝突体４０４）に加える力ｇを算出し、これをアクチュエータへの力指令値とするものである。ここで、制御対象６０２の位置ｐは衝突体４０４の位置すなわちエンコーダの検出値である。図６においてｈは、このシステムに加わる外力であり、ＳＡＴ６０３は以下の式（２）に示す単位飽和関数sat(x)を表している。
【００５４】
【数２】
【００５５】
出力制限型ＰＤ制御器６０１は、図６にて破線で示す枠内の制御ブロックで表される制御として式（３）の右辺を演算することで制御対象６０２（図４の衝突体４０４）に加える力ｇを算出する。
【００５６】
【数３】
【００５７】
ここで、Ｆは、移動機構である直動アクチュエータ４０３が発生する力の上限値を表す。また、Ｋは比例ゲイン、Ｂは微分ゲイン、ｐdは位置指令値、ｐは衝突体４０４の位置すなわち直動アクチュエータ４０３のエンコーダ検出値、sat(x)は式（２）の単位飽和関数を示す。
【００５８】
＜４−４．位置制御器の実施例の構成＞
位置制御器の実施例について図７を参照（適宜図４および図９参照）して説明する。図７は、検証装置の位置制御器に実施例として用いたＰＳＭＣの制御ブロック図である。このＰＳＭＣの制御ブロックは、一例として、ＰＤ制御器（図９の仮想連結要素９０４）と、スライディングモード制御器（図９のＳＭＣ９０５）との組み合わせにより実現されるものである。なお、ＰＳＭＣの制御ブロックは、Ｐ制御器あるいはＰＩＤ制御器と、スライディングモード制御器との組み合わせによっても実現可能である。
【００５９】
図７に示すＰＳＭＣ７０１は、位置指令値ｐd（図９においてはＳＭＣ９０５に設定された目標位置９０１）と制御対象７０２（図９の制御対象９０２）の位置ｐに基づいてアクチュエータへの力指令値ｇを算出するものである。ここで、制御対象７０２の位置ｐはエンコーダの検出値である。また、制御対象７０２（図９の制御対象９０２）には、プロクシ（図９のプロクシ９０３）が接続されているものとする。プロクシは質量のない点状の仮想物体である。プロクシの位置をｐsとする（図９参照）。
【００６０】
図７においてｈは、このシステムに加わる外力であり、ＳＧＮ７０３は以下の式（４）に示す符号関数sgn(x)を表している。
【００６１】
【数４】
【００６２】
式（４）の定義により、ｘ＞０の場合にはsgn(x)＝＋１となり、ｘ＜０の場合にはsgn(x)＝−１となる。また、式（４）はｘ＝０の場合にはsgn(x)が閉区間［−１，１］の任意の値を取りうることを定義している。つまり、図７に示すＳＧＮ７０３は、位置情報を入力とし、−１〜１のいずれかの無次元の値を出力する。なお、式（４）と前記した式（２）との間には、所定の条件を満たすときにy=sgn(x-y)とy=sat(x)とが等価であるという関係があることが知られている（例えば特開２００８−９７１５２号公報参照）。
【００６３】
図７に示すＰＳＭＣ７０１は、図７にて破線で示す枠内の主として下側の制御ブロックで表される制御として式（５）の上側の式と、同枠内の主として上側で表される制御ブロックの制御として式（５）の下側の式と、を解いて制御対象に加える力ｇを算出し、これをアクチュエータへの指令値とする。
【００６４】
【数５】
【００６５】
ここで、Ｈはプロクシが目標位置に収束するときの時定数を表す。また、Ｆは移動機構である直動アクチュエータ４０３が発生する力の上限値、Ｋは比例ゲイン、Ｂは微分ゲイン、ｐsはプロクシの位置、ｐdは位置指令値、ｐは衝突体４０４の位置すなわちエンコーダ検出値、sgn(x)は式（４）の符号関数を示す。
【００６６】
ＰＳＭＣ７０１が発生する発生力ｇは、プロクシに対してスライディングモード制御器（ＳＭＣ９０５）から加えられる力であって、式（５）の上側のｇで表される。また、プロクシに対して仮想連結要素９０４（図９参照）から加えられる力（仮想的な粘弾性力）は、式（５）の下側のｇで表され、式（５）の上側のｇと同じ力である。なお、式（５）の導出過程の詳細については例えば特許文献１に記載されている。
【００６７】
＜４−５．衝突試験条件＞
衝突試験における各種の条件を以下に説明する。
≪４−５−１．アドミッタンス制御パラメータ≫
アドミッタンス制御パラメータとは、アドミッタンス制御器４０１の仮想物体運動算出器４１０にて前記した式（１）を演算するための以下のパラメータのことである。すなわち、アドミッタンス制御パラメータは、仮想物体に加えられる仮想の力（目標力）ｆd、仮想物体の質量ｍd、仮想物体の粘性ｃdである。これらｆd、ｍd、ｃdとして次の値を予め設定した。
ｆd：５［Ｎ］
ｍd：０．７［ｋｇ］
ｃd：０．００８［Ｎｓ／ｍｍ］
【００６８】
≪４−５−２．位置制御器パラメータ≫
位置制御器パラメータとは、アドミッタンス制御器４０１の位置制御器４２０にて前記した式（２）〜（５）を演算するための以下のパラメータのことである。すなわち、位置制御器パラメータは、比較例および実施例に関して比例ゲインＫ、微分ゲインＢ、アクチュエータが発生する力の上限値Ｆと、実施例に関しての時定数Ｈである。これらＫ、Ｂ、Ｆ、Ｈとして次の値を予め設定した。
Ｋ：１０００［Ｎ／ｍｍ］
Ｂ：３［Ｎｓ／ｍｍ］
Ｆ：４５［Ｎ］
Ｈ：０．１［ｓ］（ＰＳＭＣ７０１に対してのみ用いた）
【００６９】
≪４−５−３．その他の検証条件≫
その他の検証条件として、次の値を実測した。
衝突体の実質量ｍ：４．５［ｋｇ］
緩衝材：ＥＰＤＭスポンジ厚ｔ：１０［ｍｍ］
衝突直前の速度：ｖ＝１５０［ｍｍ／ｓ］
なお、ＥＰＤＭは、エチレン−プロピレン−ジエンゴムである。
【００７０】
＜４−６．試験結果＞
図６に示す出力制限型ＰＤ制御器６０１を位置制御器４２０として採用した場合の検証装置（比較例）と、図７に示すＰＳＭＣ７０１を位置制御器４２０として採用した場合の検証装置（実施例）とを用いて、上記衝突試験条件のもとで、衝突試験をそれぞれ行った。衝突試験は、一例として実質量４．５ｋｇの衝突体を直線上で移動させて、衝突直前に時速０．５４ｋｍで被衝突体に衝突させるものである。この衝突試験は、例えば、上から下へと直線上に移動するロボットアームの下に、人が誤って手を出して衝突した（はさまれた）場合を想定している。衝突試験の結果によって、比較例や実施例の検証装置が、人への衝撃を緩和するために即座にロボットアームが人から離間する向きの推力を発生させるような制御が安定にできるか検証することが可能となる。
【００７１】
各検証装置による検証結果として、衝突体４０４が被衝突体４０６に衝突したときに測定された接触力ｆの時間変動を図８に示す。図８に示すグラフの横軸は時刻ｔ［秒］を示し、縦軸は接触力ｆ［Ｎ］を示す。接触力が５［Ｎ］のライン（細い実線）は、予め設定されたアドミッタンス制御パラメータの目標力ｆdを示す。破線はベースの位置制御を出力制限型ＰＤ制御とした場合の力制御（比較例）による衝突体の接触力ｆを示す。太い実線はベースの位置制御をＰＳＭＣとした場合の力制御（実施例）による衝突体の接触力ｆを示す。
【００７２】
比較例は、力制御の内部の位置制御にＰＤ制御を用いた従来の制御装置の場合の効果を検証したことに相当する。具体的には、衝突試験では、ロボットアームに対応した衝突体（実質量ｍ＝４．５ｋｇ）に対して実質量の約１５％にあたる質量（仮想物体の質量ｍd＝０．７ｋｇ）を仮想質量として設定した。
【００７３】
この例では、図示するようにｔ＝０．２５秒のときに衝突した。比較例の場合、図８に破線で示すように、ｔ＝０．３秒（衝突後０．０５秒）のときには、目標力ｆdを僅かに超える程度（約１０Ｎ）の接触力ｆを示す。その後、接触力ｆは０Ｎまたはマイナスの接触力となるが、ｔ＝０．４秒（衝突後０．１５秒）のときには、かろうじてアクチュエータが発生する力の上限値Ｆ（＝４５Ｎ）以内の約４０Ｎの接触力を示した。その後、接触力ｆは０Ｎまたはマイナスの接触力となるが、ｔ＝０．６秒（衝突後０．３５秒）のときには、設定された力の上限値Ｆ（＝４５Ｎ）をはるかに超え、目標力ｆdの２２倍となる約１１０Ｎの接触力を示した。以下同様に、図８に破線で示すように、大きな接触力と０あるいはマイナスの接触力とが時間的に交互に表れる波形が衝突から１秒間に６回も表れ、衝突後１秒以上経過しても接触力が振動する現象が観測された。
なお、図示を省略するが、安定化のため（振動をなくすため）仮想質量をより大きく設定した場合は接触力（衝突力）が大きくなった。また、接触力を下げるため仮想質量をより小さく設定すると振動が観測された。
【００７４】
実施例の場合、図８に太い実線で示すように、ｔ＝０．３秒（衝突後０．０５秒）のときには、比較例と同様に、目標力ｆdを僅かに超える程度（約１０Ｎ）の接触力ｆを示す。その後、接触力はこれ以上大きくなることはなく、目標力ｆdに向かって減衰振動し、衝突後０．２〜０．３秒で収束しており安定であった。実施例は、比較例と同じ衝突試験条件において、力制御の内部の位置制御にＰＳＭＣを用いた制御装置とすることで、接触力が小さくなり、かつ、振動も短時間に収束したので、従来のアドミッタンス制御の不安定化を緩和できる効果を奏することを検証することができた。なお、衝撃低減効果は一例であり、衝撃力検証のために実施した実験は一例である。
【００７５】
力制御の内部の位置制御において従来のＰＤ制御をＰＳＭＣに置き換えることでどのような現象が生じるかを事前に想定することは困難であった。ゆえに、前記した試験結果のように、力制御の内部の位置制御にＰＳＭＣを用いることで接触力の振動が緩和されることは、容易に想到できない顕著な効果である。
【００７６】
以上説明したように、本発明の実施形態に係るロボット制御装置１は、力制御のベースとなる位置制御にＰＤ制御等を用いた従来の制御にて設定する仮想質量の低減が限界となる場合であっても、アドミッタンス制御の不安定化を緩和することができる。
【００７７】
以上、本発明の制御装置の好ましい実施形態について説明したが、本発明は、前記した実施形態に限定されるものではない。例えば、ＰＳＭＣはＰＤ型に限らず、Ｐ型，ＰＩＤ型でもよい。
【００７８】
また、本実施形態では、制御装置がロボットアームの力制御を行うものとして説明したが、本発明は、ロボットアームの力制御に限らず、アドミッタンス制御全般に幅広く応用することが可能である。この場合、アドミッタンス制御全般の安定化に寄与する効果を奏することができる。
【符号の説明】
【００７９】
１００ 人ロボット協働システム
１ ロボット制御装置
２ 目標力設定手段
３ 力制御手段
４ サーボアンプ
５ 目標力取得手段
６ 接触力取得手段
７ 仮想物体運動算出手段
８ 位置制御手段
１０ アーム
１１ 力覚センサ
１２ アクチュエータ
１４ サーボモータ
１５ エンコーダ
２０ ケーブル
４０１ アドミッタンス制御器
４０２ サーボアンプ
４０３ 直動アクチュエータ
４０４ 衝突体
４０５ 力覚センサ
４０６ 被衝突体
４０７ 緩衝材
４１０ 仮想物体運動算出器
４２０ 位置制御器
５０１ 仮想物体
５０２ 被接触体
５０３ 力発生要素
５０４ 仮想ダンパ
６０１ 出力制限型ＰＤ制御器
６０２ 制御対象
６０３ 単位飽和関数
７０１ ＰＳＭＣ
７０２ 制御対象
７０３ 符号関数
９０１ 目標位置
９０２ 制御対象
９０３ プロクシ
９０４ 仮想的な保持要素
９０５ スライディングモード制御
Ｐ 人
Ｒ ロボット
【特許請求の範囲】
【請求項１】
位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置であって、
前記位置制御手段は、プロクシベーストスライディングモード制御を用いることを特徴とする制御装置。
【請求項２】
前記位置制御手段は、
比例動作を少なくとも含むフィードバック制御を行う比例制御を含むフィードバック制御器と、
スライディングモード制御則に基づくスライディングモード制御器とを備え、
前記比例制御を含むフィードバック制御器と前記スライディングモード制御器とを協働させて前記プロクシベーストスライディングモード制御を行うものであって、
前記比例制御を含むフィードバック制御器は、仮想空間における質量のない点状の仮想的な物体であるプロクシと前記外部の物体との間を仮想的に連結する仮想連結要素から前記プロクシに対して加えられる仮想的な粘弾性力を算出し、
前記スライディングモード制御器は、前記粘弾性力と釣り合う力であって前記プロクシの位置を当該スライディングモード制御器に設定された目標位置に収束させる制御を行う際に前記プロクシに加えられる仮想的な力を前記物体に加える推力として算出する、
ことを特徴とする請求項１に記載の制御装置。
【請求項３】
前記物体は、当該物体に加える推力を発生させる駆動機構と位置検出センサとを備えており、
前記力制御手段は、
前記物体に対応して前記物体の実質量よりも小さな０でない所定の質量を有した仮想物体について予め定められた、仮想的な質量、仮想的な粘弾性および前記仮想物体に加えられる仮想的な力としての目標力の各パラメータを含む前記仮想物体についての運動方程式を前記接触力を入力として用いて解くことで前記仮想物体の目標位置を算出する仮想物体運動算出手段を備え、
前記位置制御手段は、前記算出された目標位置と前記位置検出センサで検出された前記物体の位置とを入力として前記プロクシベーストスライディングモード制御を用いて前記物体を駆動する駆動機構にトルク（推力）指令値を出力することを特徴とする請求項１または請求項２に記載の制御装置。
【請求項４】
位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置による制御方法であって、
前記位置制御手段は、プロクシベーストスライディングモード制御を用いることを特徴とする制御方法。
【請求項１】
位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置であって、
前記位置制御手段は、プロクシベーストスライディングモード制御を用いることを特徴とする制御装置。
【請求項２】
前記位置制御手段は、
比例動作を少なくとも含むフィードバック制御を行う比例制御を含むフィードバック制御器と、
スライディングモード制御則に基づくスライディングモード制御器とを備え、
前記比例制御を含むフィードバック制御器と前記スライディングモード制御器とを協働させて前記プロクシベーストスライディングモード制御を行うものであって、
前記比例制御を含むフィードバック制御器は、仮想空間における質量のない点状の仮想的な物体であるプロクシと前記外部の物体との間を仮想的に連結する仮想連結要素から前記プロクシに対して加えられる仮想的な粘弾性力を算出し、
前記スライディングモード制御器は、前記粘弾性力と釣り合う力であって前記プロクシの位置を当該スライディングモード制御器に設定された目標位置に収束させる制御を行う際に前記プロクシに加えられる仮想的な力を前記物体に加える推力として算出する、
ことを特徴とする請求項１に記載の制御装置。
【請求項３】
前記物体は、当該物体に加える推力を発生させる駆動機構と位置検出センサとを備えており、
前記力制御手段は、
前記物体に対応して前記物体の実質量よりも小さな０でない所定の質量を有した仮想物体について予め定められた、仮想的な質量、仮想的な粘弾性および前記仮想物体に加えられる仮想的な力としての目標力の各パラメータを含む前記仮想物体についての運動方程式を前記接触力を入力として用いて解くことで前記仮想物体の目標位置を算出する仮想物体運動算出手段を備え、
前記位置制御手段は、前記算出された目標位置と前記位置検出センサで検出された前記物体の位置とを入力として前記プロクシベーストスライディングモード制御を用いて前記物体を駆動する駆動機構にトルク（推力）指令値を出力することを特徴とする請求項１または請求項２に記載の制御装置。
【請求項４】
位置制御手段を内部に有した力制御手段を備え、前記力制御手段が外部から入力された力情報に基づいてアドミッタンス制御を行うことで、外部の物体と他の物体との間で発生する接触力を制御する制御装置による制御方法であって、
前記位置制御手段は、プロクシベーストスライディングモード制御を用いることを特徴とする制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２０１３−５４６２９（Ｐ２０１３−５４６２９Ａ）
【公開日】平成２５年３月２１日（２０１３．３．２１）
【国際特許分類】
【出願番号】特願２０１１−１９３６６１（Ｐ２０１１−１９３６６１）
【出願日】平成２３年９月６日（２０１１．９．６）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
【公開日】平成２５年３月２１日（２０１３．３．２１）
【国際特許分類】
【出願日】平成２３年９月６日（２０１１．９．６）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
[ Back to top ]