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

Description

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

従来、ロボットアームにかかる力を制御して、部品を組み付けることが行われている。
その一例として、特許文献１には、インピーダンス制御によりロボットアーム（マニピュレーター）が発生する力を制御して組み付け作業を行わせる際の制御変数を、実際の作業を反復させながら探索する技術が開示されている。しかしながら、実際の作業を反復するため、最適な制御変数を求めるまでに長時間を要してしまう問題があった。

また、特許文献２には、力センサーを双腕に搭載し、それらの値が予め設定された閾値以上になってからの時間に基づいて組み付け作業を行う技術が開示されている。
一方、組み付け部品と被組み付け部品が接触する際の挙動をモデル化し、コンピューターシミュレーションを行うことで、実際の作業を反復せずに最適な制御変数を求めることも行われている。

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

組み付け部品と被組み付け部品の剛性が高い場合、上述の挙動をモデル化するためには、マニピュレーターの剛性も考慮しないとモデル化の精度が低下してしまう。しかし、マニピュレーターの剛性は姿勢に応じて変化するため、モデル化が困難である。ゆえに、組み付け部品と被組み付け部品の剛性が高い場合、モデル化の精度が低下し、最適な制御変数が得られない。そのため、組み付けの精度が低下する場合があるという問題があった。

そこで本発明は、上記問題に鑑みてなされたものであり、組み付け物体と被組み付け物体の剛性が高い場合でも、組み付けの精度を向上させることを可能とする制御装置、制御方法、制御プログラム、ロボット及びロボットシステムを提供することを課題とする。

本発明は前記事情に鑑みなされたもので、本発明の一態様は、第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置であって、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくする制御部を備えることを特徴とする制御装置である。
また、上記に記載の制御装置において、前記制御部は、前記第２の物体が前記第１の物体からうける力と前記第２のマニピュレーターのばね定数を表す制御変数とに基づいて、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくすることを特徴とする。
また、本発明の他の態様は、第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置の制御部が、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくする手順を有することを特徴とする制御方法である。
また、本発明の他の態様は、第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置に、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくするステップを実行させるための制御プログラムである。
また、本発明の他の態様は、第１の物体を移動させる第１のマニピュレーターと、第２の物体を移動させる第２のマニピュレーターと、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくする制御部と、を備えることを特徴とするロボットである。
（１）本発明は前記事情に鑑みなされたもので、本発明の一態様は、第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置であって、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に組み付ける際に、前記第１の物体に対する前記第２の物体の見かけの剛性を下げるように前記第２のマニピュレーターを制御する制御部を備えることを特徴とする制御装置である。
この構成によれば、第２の物体の見かけの剛性を下げることにより、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。
そのため、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（２）上記に記載の制御装置において、本発明の一態様は、前記第１の物体を前記第２の物体に押し付ける力と前記第２のマニピュレーターの仮想のばね定数を表す制御変数とに基づいて、前記第２のマニピュレーターを制御することを特徴とする。
この構成によれば、制御装置は、第２の物体の見かけの剛性を第１のマニピュレーターの剛性より小さくすることができるので、第１の物体と第２の物体とが接触する際に生じる力が大きくなっても、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。その結果、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（３）上記に記載の制御装置において、本発明の一態様は、前記制御変数は、前記第１のマニピュレーターの先端に働く並進力を当該並進力方向の変位で割った値である第１のマニピュレーターのばね定数より小さいことを特徴とする。
この構成によれば、制御装置は、第２の物体の見かけの剛性を第１のマニピュレーターのばね定数より小さくすることができるので、第１の物体と第２の物体とが接触する際に生じる力が大きくなっても、第１の物体の押し付け力の抗力に与える第１のマニピュレーターのばね定数の影響を小さくすることができる。その結果、制御装置は、第１のマニピュレーターのばね定数を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（４）上記に記載の制御装置において、本発明の一態様は、前記制御変数は、前記第１のマニピュレーターの先端に働く並進力を当該並進力方向の変位で割った値である第１のマニピュレーターのばね定数と前記第２制御変数とを合成した合成ばね定数と、前記第２制御変数との差が、予め決められた差に収まるように決められていることを特徴とする。
この構成によれば、制御装置は、第１の物体を第２の物体に押し付ける力を所望の範囲に収まるようにすることができるので、組み付けの精度を向上させることができる。

（５）上記に記載の制御装置において、本発明の一態様は、前記制御部は、前記第２の物体が前記第１の物体から受ける力を取得する力覚取得部と、前記第２のマニピュレーターの仮想のばね定数を表す制御変数を取得する制御変数取得部と、前記力覚取得部が取得した押し付け力と、前記制御変数取得部が取得した制御変数とに基づいて、前記第２のマニピュレーターの運動を表わす値を算出する運動算出部と、前記運動算出部が算出した第２のマニピュレーターの運動を表わす値に基づいて、第２のマニピュレーターを駆動させるマニピュレーター駆動部と、を備えることを特徴とする。
この構成によれば、第２の物体の見かけの剛性を下げることができるので、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。そのため、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（６）本発明の一態様は、第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置の制御部が、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に組み付ける際に、前記第１の物体に対する前記第２の物体の見かけの剛性を下げるように前記第２のマニピュレーターを制御する手順を有することを特徴とする制御方法である。
この方法によれば、第２の物体の見かけの剛性を下げることにより、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。そのため、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（７）上記に記載の制御方法において、本発明の一態様は、前記手順において、力覚取得部が、前記第２の物体が前記第１の物体から受ける力を取得する手順と、制御変数取得部が前記第１のマニピュレーターのばね定数より小さい制御変数を取得する手順と、位置算出部が、前記力覚取得部が取得した押し付け力と、前記制御変数取得部が取得した制御変数とに基づいて、前記第２のマニピュレーターの位置を算出する手順と、マニピュレーター駆動部が、前記位置算出部が算出した第２のマニピュレーターの位置に基づいて、第２のマニピュレーターを駆動させる手順と、を有することを特徴とする。
この方法によれば、第２の物体の見かけの剛性を下げることができるので、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。そのため、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（８）本発明の一態様は、第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置に、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に組み付ける際に、前記第１の物体に対する前記第２の物体の見かけの剛性を下げるように前記第２のマニピュレーターを制御するステップを実行させるための制御プログラムである。
この制御プログラムによれば、第２の物体の見かけの剛性を下げることにより、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。そのため、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（９）第１の物体を移動させる第１のマニピュレーターと、第２の物体を移動させる第２のマニピュレーターと、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に組み付ける際に、前記第１の物体に対する前記第２の物体の見かけの剛性を下げるように前記第２のマニピュレーターを制御する制御部と、を備えることを特徴とするロボットである。
この構成によれば、第２の物体の見かけの剛性を下げることにより、第１の物体の押し付け力の抗力に与える第１のマニピュレーターの剛性の影響を小さくすることができる。そのため、制御装置は、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

（１０）本発明の一態様は、第１の物体を移動させるマニピュレーターを備えるロボットと、前記マニピュレーターを制御する制御装置と、前記第１の物体が組みつけられる第２の物体を支持する治具と、を備え、前記治具のばね定数が、前記マニピュレーターのばね定数より小さいことを特徴とするロボットシステムである。
この構成によれば、第１の物体と第２の物体とが接触する際に生じる力が大きくなっても、治具のばね定数が、マニピュレーターのばね定数より小さいことで、第１の物体の押し付け力の抗力に与えるマニピュレーターのばね定数の影響を小さくすることができる。その結果、ロボットシステムは、第１のマニピュレーターのばね定数を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

本発明によれば、組み付け物体と被組み付け物体の剛性が高い場合でも、組み付けの精度を向上させることができる。

第１の実施形態におけるロボットシステムの構成を示す概略ブロック図である。 第１の実施形態におけるロボットシステムの概略図である。 第１の物体と第２の物体の拡大図である。 第１の実施形態における制御装置のハードウェア構成を示す概略ブロック図である。 第１のマニピュレーターの模式図の一例である。 合成ばね定数を説明するための図である。 第１の実施形態における制御部の論理的な構成を示す概略ブロック図である。 式（１）が表す系を説明する図である。 第１の実施形態における制御装置の処理の流れの一例を示すフローチャートである。 図９のステップＳ１０１における第１制御変数Ｍ、Ｄ及びＫの算出処理の一例を示すフローチャートである。 図８の第２の仮想ばねとしてＫ_ｅ＝９８６[Ｎ／ｍ]のばねを用いた場合のシミュレーション結果と従来のロボットの実測結果である。 図８の第２の仮想ばねとしてＫ_ｅ＝６３４４[Ｎ／ｍ]のばねを用いた場合のシミュレーション結果と従来のロボットの実測結果である。 第２の実施形態におけるロボットシステムの構成を示す概略ブロック図である。 第２の実施形態における治具の構造の一例を示す図である。

＜第１の実施形態＞
以下、本発明の実施形態について、図面を参照して詳細に説明する。図１は、第１の実施形態におけるロボットシステム１の構成を示す概略ブロック図である。ロボットシステム１は、ロボット２と撮像装置１０とを備える。ここで、ロボット２は、ロボット本体２０と制御装置３０とを備える。

撮像装置１０は、制御装置３０による制御に従って、ロボット本体２０が把持する第１の物体と第２の物体とを撮像する。ここで、第１の物体は、一例として、組み付け部品である。また、第２の物体は、一例として、被組み付け部品である。そして、撮像装置１０は、撮像することにより得られた撮像画像を示す撮像画像データを制御装置３０へ出力する。

制御装置３０は、撮像装置１０に第１の物体を撮像させ、撮像することにより得られた撮像画像データを取得する。制御装置３０は、取得した撮像画像データに基づいて、ロボット本体２０を制御し、第１の物体２５を第２の物体２６に組み付ける。

図２は、第１の実施形態におけるロボットシステム１の概略図である。同図において、ロボットシステム１は、撮像装置１０と、ロボット２とを備えている。また、ロボット２は、ロボット本体２０と、制御装置３０とを備えている。同図は、ロボット本体２０が第１の物体２５をこれから第２の物体２６に組み付ける動作を開始する直前の図である。

第１の実施形態におけるロボット本体２０は、２系統のマニピュレーターを備えた垂直多関節ロボット（双腕ロボット）である。ロボット本体２０は、第１のアーム部２０ａと第２のアーム部２０ｂとを備える。第１のアーム部２０ａは、第１のマニピュレーター２１ａと、第１の力覚センサー２３ａと、第１の把持部２４ａとを備える。第２のアーム部２０ｂは、第２のマニピュレーター２１ｂと、第２の力覚センサー２３ｂと、第２の把持部２４ｂとを備える。

第１のマニピュレーター２１ａの開放端であるハンドには、第１の力覚センサー２３ａの一端が接続されている。第１の力覚センサー２３ａの他端には、第１の把持部２４ａが取り付けられている。同様に、第２のマニピュレーター２１ｂの開放端であるハンドには、第２の力覚センサー２３ｂの一端が接続されている。第２の力覚センサー２３ｂの他端には、第２の把持部２４ｂが取り付けられている。

第１の力覚センサー２３ａは、例えば、第１の力覚センサー２３ａのある面に対して垂直な軸の力を計測する。例えば、第１の物体２５を第２の物体２６の組み付け部位２６２（図３参照）に組み付けるときに、第１の力覚センサー２３ａが計測する力の軸と、第１の物体２５が第２の物体２６の組み付け部位の底面２６３（図３参照）から受ける力の軸が一致するように、第１の把持部２４ａは第１の物体２５を把持する。これにより、第１の力覚センサー２３ａは、第１の物体２５の組み付け側の面２５１（図３参照）に垂直な方向の押し付け力の抗力を計測する。

第２の力覚センサー２３ｂは、第２の物体２６が第１の物体２５から受ける力を取得する。第２の力覚センサー２３ｂは、例えば、第２の力覚センサー２３ｂのある面に対して垂直な軸の力を計測する。本実施形態では、一例として、第２の力覚センサー２３ｂが計測する力の軸と、第２の物体２６の組み付け部位２６２（図３参照）の底面２６３（図３参照）が第１の物体２５から受ける力の軸が一致するように、第２の把持部２４ｂが第２の物体２６を把持する。これによって、第２の力覚センサー２３ｂは、第２の物体２６の組み付け部位２６２の底面２６３に垂直な方向の押し付け力を計測する。
なお、第１の力覚センサー２３ａ及び第２の力覚センサー２３ｂは、撮像装置１０を用いることで、各センサーが力を計測する軸と、面２５１と底面２６３に垂直な軸との相対的な関係を求めて、それぞれが計測した力をそれぞれ面２５１と底面２６３に垂直な軸方向の成分に変換してもよい。

第１の把持部２４ａは、第１の物体２５を把持する。また、第２の把持部２４ｂは、第２の物体２６を把持する。同図において、原点Ｏが示され、第１の物体２５と第２の物体２６とを結ぶｚ軸が示されている。ｚ軸は、原点Ｏから第２の物体２６への向きが正である。原点Ｏの位置は、第２の物体２６の組み付け部位２６２の底面２６３（図３参照）とｚ軸が交差する位置である。

ロボット本体２０は、制御装置３０から供給されるロボット制御命令を取り込み、このロボット制御命令による駆動制御によって、第１の把持部２４ａおよび第２の把持部２４ｂそれぞれの位置および姿勢を三次元空間内で所望に変更する。また、ロボット本体２０は、第１の把持部２４ａおよび第２の把持部２４ｂの爪部を開閉させる。

第１の物体２５を第２の物体２６に組み付ける組み付け作業は、一例として、以下の通りである。制御装置３０は、第１の把持部２４ａに第１の物体２５を把持させ、第２の把持部２４ｂに第２の物体２６を把持させる。そして、制御装置３０は、第１のマニピュレーター２１ａ及び第２のマニピュレーター２１ｂを制御して第１の物体２５を第２の物体２６に組み付ける。その際、制御装置３０は、第１の力覚センサー２３ａ及び第２の力覚センサー２３ｂを利用して、第１の物体２５の第２の物体２６に対する押し付け力を所定の値に近づけることで組み付ける。

図３は、第１の物体２５と第２の物体２６の拡大図である。同図に示すように、第２の物体２６の一面２６１には、溝が組み付け部位２６２として設けられており、その組み付け部位２６２の底面２６３が示されている。また、第１の物体２５の組み付け側の面２５１が示されている。本実施形態における制御装置３０は、第１の物体２５の一端が、その組み付け部位２６２の底面２６３に接触するまでロボット本体２０を制御する。本実施形態では、底面２６３のｚ軸の値が０であるので、制御装置３０は、一例として、第１の物体２５の一端のｚの値が０になった場合に、第１の物体２５が第２の物体２６に組みつけられたと判定する。

図４は、第１の実施形態における制御装置３０のハードウェア構成を示す概略ブロック図である。制御装置３０は、記憶部３１と、制御部４０とを備える。
本実施形態では、一例として、第１のマニピュレーター２１ａの姿勢によって変化する剛性は、第１のマニピュレーター２１ａの先端に働く並進力を当該並進力方向の変位で割ったものであって、それを「第１のマニピュレーター２１ａのばね定数」と表現する。
記憶部３１には、第１のマニピュレーター２１ａの各関節の角度を示す情報と、第１のマニピュレーター２１ａのばね定数を示す情報とが関連付けられて記憶されている。これにより、制御部４０は、第１のマニピュレーター２１ａの各関節の角度から第１のマニピュレーター２１ａのばね定数を取得することができる。

制御部４０は、撮像装置１０から撮像画像データを取得する。そして、制御部４０は、撮像画像データに基づいて、第１のマニピュレーター２１ａを制御して、第１の物体２５を第２の物体２６の組み付け部位２６２に組み付ける。その際、例えば、制御部４０は、インピーダンス制御により、第１の物体２５が第２の物体２６に加える押し付け力を制御する。

また、制御部４０は、第１の物体２５を第２の物体２６に組み付ける際に、第１の物体２５に対する第２の物体２６の見かけの剛性を下げるように第２のマニピュレーター２１ｂを制御する。その際、例えば、制御部４０は、第１の物体２５の移動方向についての第２の物体２６の見かけの剛性を下げるよう制御してもよい。
また、制御部４０は、例えば、スティフネス制御に従って、第１の物体２５に対する第２の物体２６の見かけの剛性を下げるよう制御してもよい。ここで、スティフネス制御は、外部環境との接触によって生じた反力に比例した変位指令を力の方向に与える制御の一つで、対象物（本実施形態では、第２のマニピュレーター２１ｂ）の剛性のみを考慮した制御である。

具体的には、例えば、制御部４０は、第２の物体２６が第１の物体２５から受ける力と第２のマニピュレーター２１ｂの仮想のばね定数を表す第２制御変数Ｋ_ｅとに基づいて、第２のマニピュレーター２１ｂを制御してもよい。

この構成によれば、第２の物体２６の見かけの剛性を下げることにより、第１のマニピュレーター２１ａの姿勢によって変化する剛性の影響を小さくすることができる。そのため、制御装置３０は、第１のマニピュレーター２１ａの姿勢によって変化する剛性を考慮しなくても、第１のマニピュレーター２１ａの制御に用いる制御変数を最適なものに近づけることができる。その結果、適切な力で第１の物体２５を第２の物体２６に押し付けることができるので、組み付けの精度を向上させることができる。また、制御部４０は、第１のマニピュレーター２１ａの姿勢によって変化する剛性を考慮してインピーダンス制御で用いる制御変数を算出しなくていいので、第１のマニピュレーター２１ａの制御に用いる制御変数の最適化を短時間で行うことができる。

第２制御変数Ｋ_ｅは、一例として、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒより小さい。これにより、制御装置３０は、第２の物体２６の見かけの剛性を第１のマニピュレーター２１ａのばね定数より小さくすることができる。

続いて、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを求める方法について、図５を用いて説明する。図５は、第１のマニピュレーター２１ａの模式図の一例である。同図の例における第１のマニピュレーター２１ａは、第１の軸Ｋ５１〜第６の軸Ｋ５６を備える。第１の軸Ｋ５１がロボット本体の胴体Ｒ５０に最も近い軸で、第６の軸Ｋ５６が第１の把持部２４ａに最も近く軸である。ここで、一般的にマニピュレーターを伸ばした状態で、その先端を矢印Ａ５７の方向に押したときの剛性が最も小さい。矢印Ａ５７の方向へ押した力（並進力）Ｆをその力Ｆに応じた並進力方向の変位Δｘで割った値Ｆ／Δｘを第１のマニピュレーター２１ａのばね定数Ｋ_Ｒとする。ここで、その変位は矢印Ａ５８の方向である。

続いて、第２制御変数Ｋ_ｅの決定方式の一例について、図６を用いて説明する。図６は、合成ばね定数を説明するための図である。合成ばね定数Ｋ_ａは、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒと第２制御変数Ｋ_ｅとが直列に接続された場合において、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒと第２制御変数Ｋ_ｅとを合成したばね定数である。合成ばね定数Ｋ_ａは、Ｋ_ｅＫ_Ｒ／（Ｋ_ｅ＋Ｋ_Ｒ）で表わされる。

ここで、例えば、第２制御変数Ｋ_ｅを第１のマニピュレーター２１ａのばね定数Ｋ_Ｒのα分の１（αは正の数）に設定すると、第２制御変数Ｋ_ｅは（１／α）Ｋ_Ｒと表わされる。よって、式変形すると、Ｋ_Ｒ＝αＫ_ｅとなる。このＫ_ＲをＫ_ａ＝Ｋ_ｅＫ_Ｒ／（Ｋ_ｅ＋Ｋ_Ｒ）に代入すると、合成ばね定数Ｋ_ａはＫ_ａ＝（α／（１＋α））Ｋ_ｅで表わされる。
ここで、仮に第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを無視してＫ_ａ＝Ｋ_ｅとみなした場合と実際の合成ばね定数Ｋ_ａの差は、Ｋ_ｅ−Ｋ_ａ＝Ｋ_ｅ−（α／（１＋α））Ｋ_ｅ＝（１／（１＋α））Ｋ_ｅである。よって、この差の割合（１／（１＋α））×１００[％]が、許容誤差よりも小さくなるように係数αが予め決められているものとする。例えば、許容誤差が５％である場合、係数αは１９より大きい数である。すなわち、係数αは許容誤差に応じて決められているので、第２制御変数Ｋ_ｅは、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒと許容誤差に応じて決められている。換言すれば、第２制御変数と合成ばね定数Ｋ_ａとの差が予め決められた差に収まるように第２制御変数が決められている。

これによれば、制御装置３０は、第１の物体２５を第２の物体２６に押し付ける力を所望の範囲に収まるようにすることができるので、第１の物体２５を第２の物体２６に押し付ける力の経時変化を所望の範囲にすることができる。その結果、制御装置３０は、第１の物体２５の剛性によらず、第１の物体２５を第２の物体２６に組み付けることができる。

図７は、第１の実施形態における制御部４０の論理的な構成を示す概略ブロック図である。制御部４０は、第１制御変数算出部４１と、剛性取得部４２と、第２制御変数取得部（制御変数取得部）４３と、第１制御部４４と、第２制御部４５と、終了判定部４６とを備える。

第１制御変数算出部４１は、インピーダンス制御で用いる制御変数を算出する。具体的には、例えば、次の式（１）で表される第１制御変数Ｍ、Ｄ及びＫを算出する。

ここで、Ｆ_ｓ１は、第１の力覚センサー２３ａで測定される力であって組み付け部位２６２の底面２６３に垂直な方向の第１の物体２５の押し付け力の抗力（以下、第１力覚ともいう）である。Ｆ_ｄは、組み付け作業を達成させるために必要とされる、予め決められた押し付け力（以下、必要押付力という）である。必要押付力Ｆ_ｄを示す情報は、予め記憶部３１に記憶されている。また、ｚ（・・）はｚの上に符号・（ドット）が二つ記された記号であるとすると、ｚ（・・）は第２の物体２６の組み付け部位２６２の底面２６３と垂直方向の第１の把持部２４ａの加速度である。また、ｚ（・）はｚの上に符号・（ドット）が記された記号であるとすると、ｚ（・）は第２の物体２６の組み付け部位２６２の底面２６３と垂直方向の第１の把持部２４ａの速度である。また、ｚは第２の物体２６の組み付け部位２６２の底面２６３と垂直方向の第１の把持部２４ａの位置である。

続いて、式（１）が表す系を、図８を用いて説明する。図８は、式（１）が表す系を説明する図である。同図に示されたバネ−マス−ダンパー系において、仮想質量６１、第１の仮想ばね６２、仮想ダンパ６３及び第２の仮想ばね６４が示されている。第２の仮想ばねネ６４は、第１の物体２５と第２の物体２６とが接触する際に生じる弾性力を発生させる要素をモデル化したばねである。同図における第１制御変数Ｍ、Ｄ及びＫは、それぞれ第１の物体２５がバネ−マス−ダンパー系として振る舞う時の、慣性質量、粘性係数及びばね定数の大きさである。また、Ｋ_ｅは、第２の仮想ばね６４のばね定数で、第１の物体２５と第２の物体２６とが接触する際に生じる弾性力を表す。

同図では、図２と同様に、第１の物体２５が第２の物体２６の組み付け部位２６２の底底面２６３に接触する高さにおいてｚを０として、下方にｚ軸が定義されている。従って、第１の力覚センサー２３ａで測定される抗力である第１力覚Ｆ_ｓ１は、次の式（２）で表される。

式（２）を式（１）に代入すると、式（１）は次の式（３）で表される。

第１制御変数算出部４１は、第１制御変数Ｍ、Ｄ及びＫを、第１の物体２５と第２の物体２６が接触する際の挙動を考慮して適切に設定する。第１制御変数Ｍ、Ｄ及びＫの設定処理の詳細は後述する。

図７に戻って、剛性取得部４２は、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを取得する。具体的には、例えば、第１のマニピュレーター２１ａの各関節の角度を示す情報に対応する第１のマニピュレーター２１ａのばね定数を示す情報記憶部３１から読み出すことで、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを取得する。そして、剛性取得部４２は、取得した第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを示す情報を第２制御変数取得部４３へ出力する。

第２制御変数取得部４３は、一例として、剛性取得部４２が取得した第１のマニピュレーター２１ａのばね定数Ｋ_Ｒに基づいて、第２制御変数Ｋ_ｅを算出する。具体的には、例えば、第２制御変数取得部４３は、上述した許容誤差に応じて決められた係数αを保持する。そして、第２制御変数取得部４３は、例えば、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを係数αで割った値を第２制御変数Ｋ_ｅとして算出する。第２制御変数取得部４３は、算出した第２制御変数Ｋ_ｅを示す情報を第２制御部４５の後述する第２運動算出部４５１へ出力する。
なお、本実施形態では、一例として、第２制御変数取得部４３は、第２制御変数Ｋ_ｅを算出することにより取得したが、これに限ったものではない。記憶部３１に、第２制御変数Ｋ_ｅが予め記憶されており、第２制御変数取得部４３は、記憶部３１から第２制御変数Ｋ_ｅを読み出すことで取得してもよい。すなわち、第２制御変数取得部４３は、第２制御変数Ｋ_ｅを取得すればよい。

第１制御部４４は、例えば、インピーダンス制御により、第１のマニピュレーター２１ａを制御する。具体的には、例えば、第１制御部４４は、式（１）に従って第１の把持部２４ａの位置を移動するよう第１のマニピュレーター２１ａを制御する。ここで、第１制御部４４は、減算部４４１と、第１運動算出部４４２と、第１マニピュレーター駆動部４４３と、第１力覚取得部４４４とを備える。

減算部４４１は、記憶部３１に記憶されている必要押付力Ｆ_ｄを示す情報を読み出す。また、減算部４４１は、第１力覚取得部４４４から第１力覚Ｆ_ｓ１を示す情報を取得する。そして、減算部４４１は、必要押付力Ｆ_ｄから第１力覚Ｆ_ｓ１を減算する。そして、減算後の値（Ｆ_ｄ−Ｆ_ｓ１）を示す減算後情報を第１運動算出部４４２へ出力する。

第１運動算出部４４２は、減算部４４１から入力された減算後情報に基づいて、インピーダンス制御によって、第１の把持部２４ａの運動を表わす値（例えば、予め決められた周期毎の第１の把持部２４ａの変位）を算出する。具体的には、例えば、第１運動算出部４４２は、次の式（４）に従って、第１の把持部２４ａの位置ｚ_１を算出する。位置ｚ_１は、位置ｚ_０を基準とする位置である。

ここで、ｓはラプラス変換で用いられるラプラス演算子（ラプラス変数）である。第１運動算出部４４２は、算出した第１の把持部２４ａの位置ｚ_１を示す第１位置情報を第１マニピュレーター駆動部４４３へ出力する。

第１マニピュレーター駆動部４４３は、第１運動算出部４４２から入力された第１位置情報に基づいて、第１のマニピュレーター２１ａが有する関節の各角度を算出する。そして、第１マニピュレーター駆動部４４３は、関節の各角度が算出した値になるように、第１のマニピュレーター２１ａを駆動する。また、第１マニピュレーター駆動部４４３は、第１のマニピュレーター２１ａを駆動した後に、駆動が終了した旨を第１力覚取得部４４４へ出力する。

第１力覚取得部４４４は、第１マニピュレーター駆動部４４３から駆動が終了した旨を受け取ると、第１の力覚センサー２３ａから第１力覚Ｆ_ｓ１を示す第１力覚情報を取得する。そして、第１力覚取得部４４４は、取得した第１力覚情報を減算部４４１へ出力する。

第２制御部４５は、例えば、スティフネス制御により、第２制御変数Ｋ_ｅに基づいて第２の物体２６の位置を制御する。具体的には、例えば、第２制御部４５は、式（５）に従って第２の把持部２４ｂの位置を移動するよう第２のマニピュレーター２１ｂを制御する。

ここで、Ｆ_ｓ２は第２の力覚センサー２３ｂで測定される第２の物体２６の組み付け部位２６２の底面２６３に垂直な方向の押し付け力（以下、第２力覚という）である。ここで、第２制御部４５は、第２運動算出部（位置算出部）４５１と、第２マニピュレーター駆動部（マニピュレーター駆動部）４５２と、第２力覚取得部（力覚取得部）４５３とを備える。

第２運動算出部４５１は、第２力覚取得部４５３から第２力覚を示す第２力覚情報を取得する。第２運動算出部４５１は、取得した第２力覚情報と、第２制御変数取得部４３から入力された第２制御変数Ｋ_ｅを示す情報とに基づいて、第２の把持部２４ｂの運動を表わす値（例えば、予め決められた周期毎の第２の把持部２４ｂの変位）を算出する。具体的には、例えば、第２運動算出部４５１は、次の式（６）に従って、第２の把持部２４ｂの位置ｚ_２を算出する。位置ｚ_２は、位置ｚ_０を基準とする位置である。

第２運動算出部４５１は、算出した、第２の把持部２４ｂの位置ｚ_２を示す第２位置情報を第２マニピュレーター駆動部４５２へ出力する。

第２マニピュレーター駆動部４５２は、第２運動算出部４５１が算出した第２のマニピュレーター２１ｂの位置に基づいて、第２のマニピュレーター２１ｂを駆動させる。具体的には、例えば、第２マニピュレーター駆動部４５２は、第２運動算出部４５１から入力された第２位置情報に基づいて、第２のマニピュレーター２１ｂが有する関節の各角度を算出する。そして、例えば、第２マニピュレーター駆動部４５２は、算出した関節の各角度になるように、第２のマニピュレーター２１ｂを駆動する。また、第２マニピュレーター駆動部４５２は、第２のマニピュレーター２１ｂを駆動した後に、駆動が終了した旨を第２力覚取得部４５３へ出力する。

第２力覚取得部４５３は、第２マニピュレーター駆動部４５２から駆動が終了した旨を受け取ると、第２の力覚センサー２３ｂから第２力覚Ｆ_ｓ２を示す第２力覚情報を取得する。すなわち、第２力覚取得部４５３は、第２の物体２６の組み付け部位の底面に垂直な方向の押し付け力を取得する。そして、第２力覚取得部４５３は、取得した第２力覚情報を第２運動算出部４５１へ出力する。

終了判定部４６は、予め決められた周期で、第１の物体２５が第２の物体２６に組み付けられたか否か判定する。具体的には、例えば、終了判定部４６は、第１の物体２５が目標の位置に到達したか否か判定することで、第１の物体２５が第２の物体２６に組み付けられたか否か判定する。一例として、終了判定部４６は、以下のようにして、第１の物体２５が目標の位置に到達したか否か判定する。
まず、終了判定部４６は、例えば、第１運動算出部４４２から第１位置情報を取得し、取得した第１位置情報から第１の物体２５のｚ軸上の位置を算出する。また、終了判定部４６は、例えば、第２運動算出部４５１から第２位置情報を取得し、取得した第２位置情報から、組み付け部位２６２の底面２６３のｚ軸上の位置ｚ_０（本実施形態の例では、ｚ_０は０）を算出する。そして、終了判定部４６は、例えば、予め決められた周期で、第１の物体２５のｚ軸上の位置が、組み付け部位２６２の底面２６３のｚ軸上の位置ｚ_０に到達したか否か判定する。

終了判定部４６は、第１の物体２５が目標の位置に到達した場合、第１の物体２５が第２の物体２６に組み付けられたと判定し、第１制御部４４と第２制御部４５の処理を終了させる。一方、第１の物体２５が目標の位置に到達していない場合、終了判定部４６は、第１の物体２５が第２の物体２６に組み付けられていないと判定し、第１制御部４４と第２制御部４５の処理を続けさせる。

なお、本実施形態では、終了判定部４６は、第１の物体２５が第２の物体２６に組み付けられたか判定する際に、一例として、第１の物体２５のｚ軸上の位置が、組み付け部位２６２の底面２６３のｚ軸上の位置ｚ_０に到達したか否か判定したが、これに限ったものではない。終了判定部４６は、第１力覚Ｆ_ｓ１が予め決められた力覚Ｆ_０よりも大きいか否か判定し、第１力覚Ｆ_ｓ１が予め決められた力覚Ｆ_０よりも大きくなった場合に、第１の物体２５が第２の物体２６に組み付けられたと判定してもよい。また、終了判定部４６は、予め決められた周期で、必要押付力Ｆ_ｄと第１力覚Ｆ_ｓ１が一致するか否か判定し、必要押付力Ｆ_ｄと第１力覚Ｆ_ｓ１が一致した場合、第１の物体２５が第２の物体２６に組み付けられたと判定してもよい。

図９は、第１の実施形態における制御装置３０の処理の流れの一例を示すフローチャートである。まず、第１制御変数算出部４１は、第１制御変数Ｍ、Ｄ及びＫの最適化の処理により、第１制御変数Ｍ、Ｄ及びＫを算出する（ステップＳ１０１）。この処理の詳細は、後述する。次に、剛性取得部４２は、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒを取得する（ステップＳ１０２）。次に、第２制御変数取得部４３は、第２制御変数Ｋ_ｅを算出する（ステップＳ１０３）。

次に、制御装置３０は、ステップＳ１０４〜Ｓ１０７の処理と、ステップＳ１０８〜Ｓ１１０の処理とを並行して行う。まず、ステップＳ１０４〜Ｓ１０７の処理について説明する。ステップＳ１０４において、減算部４４１は、必要押付力Ｆ_ｄから第１力覚Ｆ_ｓ１を減算する。次に、第１運動算出部４４２は、減算後の値（Ｆ_ｄ−Ｆ_ｓ１）を示す減算後情報に基づいて、第１の把持部２４ａの運動を表わす値を算出する（ステップＳ１０５）。次に、第１マニピュレーター駆動部４４３は、算出した第１の把持部２４ａの運動を表わす値に基づいて、第１のマニピュレーター２１ａを駆動する（ステップＳ１０６）。次に、第１力覚取得部４４４は、第１の力覚センサー２３ａから第１力覚Ｆ_ｓ１を示す第１力覚情報を取得する（ステップＳ１０７）。

続いて、ステップＳ１０８〜Ｓ１１０の処理について説明する。ステップＳ１０８において、第２運動算出部４５１は、第２力覚情報と第２制御変数Ｋ_ｅを示す情報とに基づいて、第２の把持部２４ｂの運動を表わす値を算出する。次に、第２マニピュレーター駆動部４５２は、算出した第２の把持部２４ｂの運動を表わす値に基づいて、第２のマニピュレーター２１ｂを駆動する（ステップＳ１０９）。次に、第２力覚取得部４５３は、第２の力覚センサー２３ｂから第２力覚Ｆ_ｓ２を示す第２力覚情報を取得する（ステップＳ１１０）。

ステップＳ１１１において、終了判定部４６は、第１の物体２５が目標の位置に到達したか判定する。第１の物体２５が目標の位置に到達していない場合（ステップＳ１１１ ＮＯ）、制御装置３０は、ステップＳ１０４及びステップＳ１０８の処理に戻る。第１の物体２５が目標の位置に到達した場合（ステップＳ１１１ ＹＥＳ）、制御装置３０は、その処理を終了する。以上で、本フローチャートの処理を終了する。

図１０は、図９のステップＳ１０１における第１制御変数Ｍ、Ｄ及びＫの算出処理の一例を示すフローチャートである。まず、第１制御変数算出部４１は、第１制御変数の候補である候補変数Ｍ’、Ｄ’及びＫ’の組の集合から、一の候補変数Ｍ’、Ｄ’及びＫ’の組を取得する（ステップＳ２０１）。次に、第１制御変数算出部４１は、取得した一の候補変数Ｍ’、Ｄ’及びＫ’の組を用いて式（３）の過渡的な振る舞いを解析し、評価関数Ｅの値を算出する（ステップＳ２０２）。式（３）の過渡的な振る舞いを解析する際には、第１制御変数算出部４１は、例えば、ルンゲ・クッタ方式で、式（３）の微分方程式を解く。そして、例えば、第１制御変数算出部４１は、以下の式（７）に従って、評価関数Ｅの値を算出する。

ここで、ｐは押付力が必要押付力Ｆ_ｄを基準に予め定められた範囲の力に落ち着く（以下、このことを整定という）までの最大の押付力から必要押付力Ｆ_ｄを減算した量（以下、オーバーシュートの量という）である。ｑは、整定までの振動回数（以下、単に振動回数という）である。ｒは整定までの所要時間（以下、整定時間という）である。ｕは、定常偏差である。係数α、β、γはそれぞれオーバーシュートの量ｐの重み係数、振動回数ｑの重み係数、整定時間ｒの重み係数であり、予め決められている。

次に、第１制御変数算出部４１は、算出した評価関数Ｅがこれまで算出した評価関数Ｅのうち最小であれば、当該候補変数Ｍ’、Ｄ’及びＫ’の値を記憶部３１に格納する（ステップＳ２０３）。次に、第１制御変数算出部４１は、全ての候補変数Ｍ’、Ｄ’及びＫ’の組について評価関数Ｅを算出したか否か判定する（ステップＳ２０４）。

全ての候補変数Ｍ’、Ｄ’及びＫ’の組について評価関数Ｅを算出していない場合（ステップＳ２０４ ＮＯ）、第１制御変数算出部４１は、ステップＳ２０１の処理に戻る。全ての候補変数Ｍ’、Ｄ’及びＫ’の組について評価関数Ｅを算出した場合（ステップＳ２０４ ＹＥＳ）、第１制御変数算出部４１は、記憶部３１に記憶されている候補変数Ｍ’、Ｄ’及びＫ’の値をそれぞれ第１制御変数Ｍ、Ｄ及びＫの値として採用する（ステップＳ２０５）。以上で、本フローチャートの処理を終了する。

続いて、図１１と図１２を参照して、従来の問題点を説明し、それに対する本実施形態の構成および効果を説明する。図１１は、第２の仮想ばね６４のばね定数Ｋ_ｅが９８６[Ｎ／ｍ]の場合の、式（１）のインピーダンス制御による押付力のシミュレーション結果と従来のロボット（以下、実機ともいう）の押付力の実測結果である。ここで、従来のロボットは、マニピュレーターを一つのみ備える。ここで、マニピュレーターが把持する部品と予め決められた台の間には、ばね定数が９８６[Ｎ／ｍ]のばねが部品の底面と略垂直に挿入されているものとする。そして、従来のロボットは、マニピュレーターを制御して部品を当該ばねに押し付ける。

同図のグラフＧ７０において、押付力Ｆ_ｓ１と経過時間の関係が示されている。同図の結果は、一例として、最適な第１制御変数Ｍ、Ｄ及びＫは、それぞれ１０[ｋｇ]、２００[ｋｇ・ｍ／ｓ]、１[ｋｇ・ｍ／ｓ]として得られた結果である。ここで、図１１と後述する図１２では、必要押付力Ｆ_ｄは、−１０[Ｎ／ｍ]が用いられたものとする。
また、テーブルＴ７３において、シミュレーション結果は、オーバーシュートの量ｐが０．００[Ｎ]で、振動回数が０回で、整定時間ｒが０．４９秒で、定常偏差ｕが０．０１[Ｎ]であることが示されている。

同図のグラフＧ７０において曲線Ｗ７１がシミュレーションにより算出された押付力の時間変化である。同図のグラフＧ７０において曲線Ｗ７２が実機で計測された押付力の時間変化である。同図において、曲線Ｗ７１と曲線Ｗ７２とは、ほぼ同じような軌跡を示す。このことから、部品と被組み付け部品とが接触する際に生じる弾性力Ｋ_ｅが比較的低い場合には、式（１）のインピーダンス制御のみによる押付力のシミュレーション結果と従来のロボットの押付力の実測結果とは整合する。

図１２は、第２の仮想ばね６４のばね定数Ｋ_ｅが６３４４[Ｎ／ｍ]の場合の、式（１）のインピーダンス制御のみによる押付力のシミュレーション結果と従来のロボットの押付力の実測結果である。ここで、従来のロボットは、図１１の場合と同様のロボットである。但し、図１２では、マニピュレーターが把持する部品と予め決められた台の間には、ばね定数が６３４４[Ｎ／ｍ]のばねが部品の底面と略垂直に挿入されているものとする。

同図のグラフＧ８０において、押付力Ｆ_ｓ１と経過時間の関係が示されている。同図の結果は、一例として、最適な第１制御変数Ｍ、Ｄ及びＫは、それぞれ３０[ｋｇ]、８００[ｋｇ・ｍ／ｓ]、２[ｋｇ・ｍ／ｓ]として得られた結果である。また、テーブルＴ８３において、シミュレーション結果は、オーバーシュートの量ｐが０．００[Ｎ]で、振動回数が０回で、整定時間ｒが０．３２秒で、定常偏差ｕが０．００[Ｎ]であることが示されている。

同図のグラフＧ８０において曲線Ｗ８１がシミュレーションにより算出された押付力の時間変化である。同図のグラフＧ８０において曲線Ｗ８２が実機で計測された押付力の時間変化である。同図において、曲線Ｗ８１と曲線Ｗ８２とを比較すると、曲線Ｗ８１の方が整定時間が短く、曲線Ｗ８１と曲線Ｗ８２とは、その軌跡にずれが生じている。このことから、部品と被組み付け部品とが接触する際に生じる弾性力Ｋ_ｅが比較的高い場合には、式（１）のインピーダンス制御のみによる押付力のシミュレーション結果と従来のロボットの押付力の実測結果とは整合しない。

図１１と図１２を比較すると、部品と被組み付け部品とが接触する際に生じる弾性力Ｋ_ｅが９８６[Ｎ／ｍ]の場合の方が、６３４４[Ｎ／ｍ]の場合よりも、シミュレーションと実機の挙動の差が小さく、シミュレーション結果の精度が高い。すなわち、部品と被組み付け部品とが接触する際に生じる弾性力Ｋ_ｅが比較的高い場合には、式（１）のインピーダンス制御のみによる押付力のシミュレーション結果と従来のロボットの押付力の実測結果の差が大きい。これは、部品と被組み付け部品とが接触する際に生じる弾性力Ｋ_ｅが大きくなり、ロボットが有するマニピュレーターの剛性と近い値になると、そのマニピュレーターの剛性を無視することが出来なくなるためである。

この問題を解決するために、制御装置３０は、例えば、第２制御変数Ｋ_ｅを９８６[Ｎ／ｍ]として、式（５）に従って第２の把持部２４ｂの位置を算出する。そして、制御装置３０は、実際の第２の把持部２４ｂが算出した第２の把持部２４ｂの位置になるよう第２のマニピュレーター２１ｂを制御する。制御装置３０は、そのように制御することで、第１の物体２５と第２の物体２６とが接触する際に生じる弾性力を小さくすることができる。その結果、制御装置３０は、図１１のように、第１力覚のシミュレーション結果と第１の力覚センサー２３ａが実際に測定する第１力覚の差を小さくすることができる。

以上、本実施形態における制御装置３０は、第１の物体２５に対する第２の物体２６の見かけの剛性を小さくするように第２のマニピュレーターを制御する。
これにより、第２の物体２６の見かけの剛性を下げることで、第１力覚に与える第１のマニピュレーター２１ａの剛性の影響を小さくすることができる。それにより、制御装置３０は、第１の物体２５と第２の物体２６の剛性によらず、第１のマニピュレーターの剛性を考慮しなくても、第１のマニピュレーターの制御変数を最適なものに近づけることができる。その結果、制御装置３０は、第１の物体２５と第２の物体２６の剛性によらず、適切な力の経時変化で第１の物体を第２の物体に押し付けることができるので、組み付けの精度を向上させることができる。

従来、実際のマニピュレーターの挙動とモデルにおけるマニピュレーターの挙動との間の差を軽減するために、マニピュレーターの剛性もモデル化する必要がある。しかし、マニピュレーターの剛性が姿勢に応じて変化するため、マニピュレーターの剛性をモデル化することが困難であるという問題もあった。
それに対し、本実施形態における制御装置３０は、被組み付け部品である第２の物体２６を把持する第２のマニピュレーター２１ｂも、第２の力覚センサー２３ｂにより取得した第２の力覚を用いた制御をして、第２の物体２６の見かけの剛性を下げる。これにより、制御装置３０は第１のマニピュレーター２１ａのばね定数Ｋ_Ｒの影響を小さくできる状況を作ることにより、第１のマニピュレーター２１ａのばね定数Ｋ_Ｒのモデル化を回避することができる。それにより、制御装置３０は、第１のマニピュレーター２１ａばね定数Ｋ_Ｒを考慮しないで、式（１）に従って第１のマニピュレーター２１ａの制御変数を決定することができるので、第１のマニピュレーター２１ａの制御変数の最適化を短時間で行うことができる。

また、多くの組み付け作業では部品同士を圧着する必要があり、押し付ける力を一定に制御することが求められるが、特許文献２に記載の技術では、部品に加える力を一定にするという制御がなかった。それゆえ、特許文献２に記載の技術では、組み付けが可能な部品が限られていた。

それに対し、本実施形態における制御装置３０は、第１の物体に対する第２の物体２６の見かけの剛性を下げることで、第２の物体２６に対する第１の物体２５の押し付け力の経時変化を所望のものにする。これにより、制御装置３０は、第１の物体２５と第２の物体２６が接触しても、第１の物体２５を適切な力の経時変化で第２の物体２６に押し付けられるので、第１の物体２５の剛性によらず、第１の物体２５を第２の物体に組み付けるようにすることができる。その結果、制御装置３０は、幅広い剛性の物体を組み付けるようにすることができる。

＜第２の実施形態＞
続いて、第２の実施形態について説明する。図１３は、第２の実施形態におけるロボットシステム１ｂの構成を示す概略ブロック図である。同図において、ロボットシステム１ｂは、ロボット本体２０ｂと、制御装置３０ｂと、治具１１０とを備える。同図は、ロボット本体２０ｂが第１の物体１０５をこれから第２の物体１０６に組み付ける動作を開始する直前の図である。

ロボット本体２０ｂは、例えば６軸の垂直多関節ロボットである。ロボット本体２０ｂは、ロボット制御装置３０ｂによる制御によって、マニピュレーター１０１と把持部１０４とのうちいずれか一つまたは組み合わせを動かす。なお、ロボット本体２０ｂの自由度は６軸によるものに限ったものではなく、任意の数の軸を有してもよい。例えば、ロボット本体２０ｂの自由度は７軸でもよい。

ここで、ロボット本体２０ｂは、マニピュレーター１０１と、力覚センサー１０３と、把持部１０４とを備える。マニピュレーター１０１の開放端であるハンドには、力覚センサー１０３の一端が接続されている。力覚センサー１０３の他端には、把持部１０４が取り付けられている。
把持部１０４は、第１の物体１０５を把持する。そして、マニピュレーター１０１は、第１の物体１０５を移動させる。ここで、第１の物体は、一例として、組み付け部品である。第２の物体１０６は、一例として、被組み付け部品である。

制御装置３０ｂは、第１の物体１０５の第２の物体１０６に対する押し付け力が、所定の値に近づけることにより、第１の物体１０５を第２の物体１０６に組み付ける。組み付け作業において、制御装置３０ｂは、把持部１０４に第１の物体１０５を把持させ、マニピュレーター１０１を制御して、第１の物体１０５を第２の物体１０６の組み付け部位の底面に接触させる。その後に、制御装置３０ｂは、力覚センサー１０３を利用して、第１の物体１０５の第２の物体１０６に対する押し付け力を所定の値に近づける。これにより、ロボット本体２０ｂは、第１の物体１０５を第２の物体１０６に組み付ける。このとき、制御装置３０ｂは、インピーダンス制御によりマニピュレーター１０１を制御することで把持部１０４の位置を変更し、第１の物体１０５の第２の物体１０６に対する押し付け力を所望の値にする。

治具１１０は、支持部１１１と、弾性部１１２と、土台部１１３とを備える。支持部１１１は、第２の物体１０６を支持する。弾性部１１２は、支持部１１１と土台部１１３とを連結する。弾性部１１２は、弾性をもつ部材であり、一例として、金属製の弦巻ばねである。なお、弾性部１１２の材料は、金属に限定されず、空気ばね、ゴムなどをもちいてもよい。また、金属製のばねとして、渦巻ばね、板ばね、捩りばねなどを用いることができる。土台部１１３は、弾性部１１２を支持する。

図１４は、第２の実施形態における治具１１０の構造の一例を示す図である。図１４（Ａ）は、治具１１０を真上からみた場合の模式図である。図１４（Ａ）において、支持部１１１の外縁Ｒ１２１と、第２の物体１０６が置かれる部位の底面Ｓ１２２が示されている。
図１４（Ｂ）は、治具１１０を図１４（Ａ）のＡからＡ´までの直線で示された断面でみた場合の模式図である。図１４（Ｂ）において、支持部１１１の底面は、弾性部１１２の一端Ｐ１２３に接続されている。弾性部１１２は、一端Ｐ１２３が支持部１１１に接続され、他端Ｐ１２４が土台部１１３に接続されている。土台部１１３の上面は、弾性部１１２の他端Ｐ１２４に接続されている。

本実施形態では、マニピュレーター１０１の姿勢によって変化する剛性は、マニピュレーター１０１の先端に働く並進力を当該並進力方向の変位で割った値であって、それを「マニピュレーター１０１のばね定数」と表現する。治具１１０のばね定数が、マニピュレーター１０１のばね定数より小さい。例えば、弾性部１１２のばね定数Ｋ_Ｄがマニピュレーター１０１のばね定数より小さい。弾性部１１２のばね定数Ｋ_Ｄは、例えば、マニピュレーター１０１のばね定数と予め決められた差とに基づいて決められている。例えば、予め決められた差が０．１であるとすると、弾性部１１２のばね定数Ｋ_Ｄは、マニピュレーター１０１のばね定数の１０分の１である。

これにより、第１の物体１０５の剛性が大きいことで、第１の物体１０５と第２の物体１０６とが接触する際に生じる弾性力が大きくなっても、治具１１０のばね定数が、マニピュレーター１０１のばね定数より小さいことで、マニピュレーター１０１のばね定数の影響を小さくすることができる。それにより、ロボットシステム１ｂは、第１の物体１０５が第２の物体１０６を押し付ける押付力のシミュレーション結果と力覚センサー１０３が測定する上記押付力の差を小さくすることができる。このことから、ロボットシステム１ｂは、マニピュレーター１０１が移動させる第１の物体１０５の剛性によらず、第１の物体１０５が第２の物体１０６を押し付ける押付力の経時変化を適切に設定できるので、第１の物体１０５を第２の物体１０６に組み付けることができる。

なお、複数の装置を備えるシステムが、本実施形態の制御装置３０または制御装置３０ｂの各処理を、それらの複数の装置で分散して処理してもよい。
また、本実施形態の制御装置３０または制御装置３０ｂの各処理を実行するためのプログラムをコンピューター読み取り可能な記録媒体に記録して、当該記録媒体に記録されたプログラムをコンピューターシステムに読み込ませ、実行することにより、制御装置３０または制御装置３０ｂに係る上述した種々の処理を行ってもよい。

なお、ここでいう「コンピューターシステム」とは、ＯＳや周辺機器等のハードウェアを含むものであってもよい。また、「コンピューターシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、フラッシュメモリ等の書き込み可能な不揮発性メモリ、ＣＤ−ＲＯＭ等の可搬媒体、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。

さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピューターシステム内部の揮発性メモリ（例えばＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ））のように、一定時間プログラムを保持しているものも含むものとする。また、上記プログラムは、このプログラムを記憶装置等に格納したコンピューターシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピューターシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。

以上、本発明の実施形態について図面を参照して詳述したが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。

１、１ｂ ロボットシステム
２ ロボット
１０ 撮像装置
２０ ロボット本体
２１ａ 第１のマニピュレーター
２１ｂ 第２のマニピュレーター
２３ａ 第１の力覚センサー
２３ｂ 第２の力覚センサー
２４ａ 第１の把持部
２４ｂ 第２の把持部
２５、１０５ 第１の物体
２６、１０６ 第２の物体
３０ 制御装置
３１ 記憶部
４０ 制御部
４１ 第１制御変数算出部
４２ 剛性取得部
４３ 第２制御変数取得部（制御変数取得部）
４４ 第１制御部
４５ 第２制御部
４６ 終了判定部
１０１ マニピュレーター
１０３ 力覚センサー
１０４ 把持部
１１１ 支持部
１１２ 弾性部
１１３ 土台部
２６１ 一面
２６２ 組み付け部位
２６３ 底面
４４１ 減算部
４４２ 第１運動算出部
４４３ 第１マニピュレーター駆動部
４４４ 第１力覚取得部
４５１ 第２運動算出部（位置算出部）
４５２ 第２マニピュレーター駆動部（マニピュレーター駆動部）
４５３ 第２力覚取得部（力覚取得部）

Claims (10)

1. 第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置であって、
   前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくする制御部を備えることを特徴とする制御装置。
2. 前記制御部は、前記第２の物体が前記第１の物体からうける力と前記第２のマニピュレーターのばね定数を表す制御変数とに基づいて、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくすることを特徴とする請求項１に記載の制御装置。
3. 前記制御変数は、前記第１のマニピュレーターの先端に働く並進力を当該並進力方向の変位で割った値である前記第１のマニピュレーターのばね定数より小さいことを特徴とする請求項２に記載の制御装置。
4. 前記制御変数は、前記第１のマニピュレーターの先端に働く並進力を当該並進力方向の変位で割った値である前記第１のマニピュレーターのばね定数と前記制御変数とを合成した合成ばね定数と、前記制御変数との差が、予め決められた差に収まるように決められていることを特徴とする請求項２または請求項３に記載の制御装置。
5. 前記制御部は、
   前記第２の物体が前記第１の物体から受ける力を取得する力覚取得部と、
   前記第２のマニピュレーターの仮想のばね定数を表す制御変数を取得する制御変数取得部と、
   前記力覚取得部が取得した押し付け力と、前記制御変数取得部が取得した制御変数とに基づいて、前記第２のマニピュレーターの運動を表わす値を算出する運動算出部と、
   前記運動算出部が算出した前記第２のマニピュレーターの運動を表わす値に基づいて、前記第２のマニピュレーターを駆動させるマニピュレーター駆動部と、
   を備えることを特徴とする請求項１から請求項４のいずれか一項に記載の制御装置。
6. 第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置の制御部が、前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくする手順を有することを特徴とする制御方法。
7. 前記手順において、
   力覚取得部が、前記第２の物体が前記第１の物体から受ける力を取得する手順と、
   制御変数取得部が前記第１のマニピュレーターのばね定数より小さい制御変数を取得する手順と、
   位置算出部が、前記力覚取得部が取得した押し付け力と、前記制御変数取得部が取得した制御変数とに基づいて、前記第２のマニピュレーターの位置を算出する手順と、
   マニピュレーター駆動部が、前記位置算出部が算出した前記第２のマニピュレーターの位置に基づいて、前記第２のマニピュレーターを駆動させる手順と、
   を有することを特徴とする請求項６に記載の制御方法。
8. 第１のマニピュレーターと第２のマニピュレーターとを備えるロボットを制御する制御装置に、
   前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくするステップを実行させるための制御プログラム。
9. 第１の物体を移動させる第１のマニピュレーターと、
   第２の物体を移動させる第２のマニピュレーターと、
   前記第１のマニピュレーターが移動させる第１の物体を前記第２のマニピュレーターが移動させる第２の物体に接触させた際に、前記第２のマニピュレーターのばね定数を前記第１のマニピュレーターのばね定数よりも小さくする制御部と、
   を備えることを特徴とするロボット。
10. 第１の物体を移動させるマニピュレーターを備えるロボットと、
    前記マニピュレーターを制御する制御装置と、
    前記第１の物体が組みつけられる第２の物体を支持する治具と、
    を備え、
    前記治具のばね定数が、前記マニピュレーターのばね定数より小さいことを特徴とするロボットシステム。

Images