JP2014018931A - 制御システム、プログラム及び機械装置の制御方法 - Google Patents

Abstract

【課題】 既知の物理モデルを機械機構に適用することが困難な場合に、当該機械機構による力覚センサーへの影響を適切に補正する制御システム、プログラム及び機械装置の制御方法等を提供すること。
【解決手段】 制御システムは、機械機構（狭義にはエンドエフェクター１２）と、Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有し、Ｎ個の３軸力覚センサーユニットの各３軸力覚センサーユニットから、機械機構に起因する値が付加されたユニット出力値を取得し、ユニット出力値に基づく力覚値を出力する力覚センサー２０と、力覚センサー２０が出力した力覚値に基づいて、力覚値を補正する力覚値補正部１２０と、力覚値補正部１２０において補正された力覚値に基づいて、機械機構を含む機械装置（狭義にはロボット１０）の制御を行う制御部１１０を含む。
【選択図】 図１２

Description

本発明は、制御システム、プログラム及び機械装置の制御方法等に関する。

ロボットなどを用いた作業において、位置のみの制御ではなく、力を制御しながら特定の動作をさせたいということがある。力制御は、例えば柔らかい物、脆い物を破壊せずに取り扱う場合や、複雑な形状を持つ物体表面を一定の力でなぞる場合等に用いることができる。これらの力制御を行うためには、力覚センサーを用いて力を検出し、その力の大きさや方向を制御ループの中に組み込んだ処理が必要となる。

しかし力覚センサーは、検出したい外力に加え、それに取り付けられた手先構造物の影響による力、及び手先が掴んだツールやワークによる力なども検出してしまう。これらの力は一定値ではなく、種々の条件により変化する。そのため、検出したい外力と手先構造物の影響による力、及び手先が掴んだツールやワークによる力を分離することは困難である。

これに対して、特許文献１では上記手先構造物の重量と、その回転によるジャイロ効果を補正する技術が開示されている。特許文献１は、ロボットを用いて、穴開け、バリ取り、研削、研磨作業のような回転体を取り扱う作業を想定したものである。具体的には、センサー座標系におけるツール重心、回転体重心、接触点の位置を既知情報とし、アーム姿勢により変化するモーメント、および回転体により発生するジャイロ効果を補正し、実質的な外力を検知するものである。

また、特許文献２では、特許文献１に加えて遠心力などの効果を補正する技術が開示されている。特許文献２は、アームの運動による遠心力を補正するものであり、補正処理はツールの質量、重心位置、アームの位置、速度、加速度検出部、及び動力学項、重力算出部を用いて実現される。

また、特許文献３では、特許文献１、２のような細かな外力補正ではなく、単に鉛直方向の外力を推定することにより、物体を把持、解放する技術が開示されている。特許文献３は、手先で検知される力を、ハンド座標系からロボットのベース座標系へ変換し、それにより把持制御を行うものである。

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

特許文献１〜３の手法は、機械機構（手先構造物、場合によっては当該手先構造物により把持されるツール等を含む）の物理モデル（例えば、重量分布等）が既知であることを前提としている。そのため、機械機構の物理モデルが未知である場合や、状況に応じて物理モデルが変化する場合等には、当該機械機構による力覚値（力覚センサーの検出値）への影響を適切に補正することが困難である。

本発明の幾つかの態様によれば、既知の物理モデルを機械機構に適用することが困難な場合に、当該機械機構による力覚センサーへの影響を適切に補正する制御システム、プログラム及び機械装置の制御方法等を提供することができる。

本発明の一態様は、機械機構と、Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有し、Ｎ個の前記３軸力覚センサーユニットの各３軸力覚センサーユニットから、前記機械機構に起因する値が付加されたユニット出力値を取得し、前記ユニット出力値に基づく力覚値を出力する力覚センサーと、前記力覚センサーが出力した前記力覚値に基づいて、前記力覚値を補正する力覚値補正部と、前記力覚値補正部において補正された前記力覚値に基づいて、前記機械機構を含む機械装置の制御を行う制御部と、を含む制御システムに関係する。

本発明の一態様では、力覚センサーはＮ個の３軸力覚センサーユニットを有し、各ユニットから出力されるユニット出力値に基づく力覚値に基づいて補正処理を行う。ここではＮを２以上としているため、力覚センサーからは通常の６軸力覚センサーと同等或いはそれ以上の量の情報が出力されることになり、６軸力覚センサーの力覚値を用いた補正処理と比べた場合に精度の向上等が可能になる。

また、本発明の一態様では、前記力覚センサーは、Ｎ個の前記３軸力覚センサーユニットの各３軸力覚センサーユニットから出力された前記ユニット出力値を含む情報を、前記力覚値として出力し、前記力覚値補正部は、前記力覚値に含まれる前記ユニット出力値に基づいて、前記機械機構の質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行ってもよい。

これにより、機械機構の質量中心位置を用いた補正処理等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記力覚値に含まれるユニット出力値から、Ｎ個の前記３軸力覚センサーユニットのそれぞれが検出した力に対応するＮ個の力ベクトルを求め、求めたＮ個の前記力ベクトルの合成ベクトルの大きさに基づいて、前記機械機構の質量を推定し、推定した前記質量に基づいて、前記補正処理を行ってもよい。

これにより、機械機構の質量を用いた補正処理等が可能になる。

また、本発明の一態様では、前記力覚センサーは、３個以上の前記３軸力覚センサーユニットを有してもよい。

これにより、力覚値に基づく質量中心位置の推定等を適切に行うこと等が可能になる。

また、本発明の一態様では、前記力覚センサーは、４個以上の前記３軸力覚センサーユニットを有してもよい。

これにより、質量中心位置等の推定精度を向上させること等が可能になる。

また、本発明の一態様では、前記力覚センサーは、少なくとも２個の前記３軸力覚センサーユニットを有し、前記力覚値補正部は、前記機械装置の第１の姿勢において、２個の前記３軸力覚センサーユニットからの前記ユニット出力値に基づき前記力覚センサーから出力された第１の力覚値に基づいて、前記機械機構の前記質量中心位置を推定する処理を行い、前記制御部は、前記力覚値補正部により前記質量中心位置の推定が不可と判定された場合には、前記機械装置の姿勢を前記第１の姿勢とは異なる第２の姿勢に変更する制御を行い、前記力覚値補正部は、前記制御部により前記機械装置が前記第２の姿勢となる制御が行われた後に、２個の前記３軸力覚センサーユニットからの前記ユニット出力値に基づき前記力覚センサーから出力された第２の力覚値に基づいて、前記機械機構の前記質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行ってもよい。

これにより、力覚センサーの低コスト化、小型化等が可能になるとともに、用いる３軸力覚センサーユニットが２個であっても、適切な質量中心位置の推定等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記機械機構の前記質量中心位置の最小自乗解を正規化方程式により求めることで、前記質量中心位置を推定してもよい。

これにより、質量中心位置を正規化方程式の最小自乗解として求めることが可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記正規化方程式に対して特異値分解を行い、前記特異値分解により得られる特異値の数に基づいて、前記質量中心位置の推定が可能か否かの判定を行ってもよい。

これにより、特異値の数により、質量中心位置の推定結果の妥当性判定を行うこと等が可能になる。

また、本発明の一態様では、前記力覚センサーは、Ｎ個の前記３軸力覚センサーユニットの各３軸力覚センサーユニットから、前記ユニット出力値として３つの値を取得することで、３×Ｎ個の値を取得し、取得した３×Ｎ個の値に基づいて、Ｘ軸、Ｙ軸及びＺ軸の並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚを算出し、算出したＦｘ，Ｆｙ，Ｆｚ及びＭｘ，Ｍｙ，Ｍｚを含む情報を前記力覚値として出力する６軸力覚センサーであってもよい。

これにより、Ｎ個の３軸力覚センサーユニットを有する力覚センサーを、必要に応じて６軸力覚センサーとして用いること等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記機械装置の複数の姿勢において得られた複数の前記力覚値に基づいて、前記補正処理を行ってもよい。

これにより、質量中心位置の推定精度を向上させること等が可能になる。

また、本発明の一態様では、前記機械機構が有する可変部の姿勢情報を取得する姿勢情報取得部を含み、前記力覚値補正部は、前記可変部の前記姿勢情報に基づいて、前記可変部の前記質量中心位置を推定して前記補正処理を行ってもよい。

これにより、機械機構が可変部を有する場合にも、適切な質量中心位置の推定等が可能になる。

また、本発明の一態様では、前記機械装置は、ロボットであり、前記機械機構は、前記ロボットのエンドエフェクターであり、前記力覚値補正部は、Ｎ個の前記３軸力覚センサーユニットを有し、前記ロボットの前記エンドエフェクターに対応して設けられる前記力覚センサーから、前記力覚値を取得して、取得した前記力覚値の補正処理を行い、前記制御部は、前記補正処理後の前記力覚値に基づいて、前記ロボット及び前記エンドエフェクターの制御を行ってもよい。

これにより、機械装置としてロボット、機械装置としてエンドエフェクターを用いた場合に、当該ロボット等を制御する制御システムを実現すること等が可能になる。

また、本発明の他の態様は、機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、前記補正処理後の前記力覚値に基づいて、前記機械装置及び前記機械機構の制御を行う制御部として、コンピューターを機能させ、前記力覚センサーは、Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有し、前記力覚値補正部は、Ｎ個の前記３軸力覚センサーユニットからのユニット出力値に基づき前記力覚センサーから出力された前記力覚値に基づいて、前記補正処理を行うプログラムに関係する。

本発明の他の態様では、力覚センサーがＮ個の３軸力覚センサーユニットを有する場合に、当該力覚センサーからの力覚値に対する補正処理を行い、補正処理後の力覚値に基づく適切な機械装置及び機械機構の制御を行うプログラムを実現すること等が可能になる。

また、本発明の他の態様は、機械装置の機械機構に対応して設けられ、Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有する力覚センサーから力覚値を取得し、取得した前記力覚値に対して補正処理を行い、前記補正処理後の前記力覚値に基づいて、前記機械装置及び前記機械機構の制御を行い、前記補正処理として、Ｎ個の前記３軸力覚センサーユニットからのユニット出力値に基づき前記力覚センサーから出力された前記力覚値に基づいて、前記力覚値を補正する処理を行う機械装置の制御方法に関係する。

本発明の他の態様では、力覚センサーがＮ個の３軸力覚センサーユニットを有する場合に、当該力覚センサーからの力覚値に対する補正処理を行い、補正処理後の力覚値に基づく適切な機械装置及び機械機構の制御を行う制御方法を実現すること等が可能になる。

機械装置としてロボットを用いる場合の構成例。 機械機構と力覚センサーの位置関係を説明する図。 図３（Ａ）〜図３（Ｃ）はリセット動作を説明する図。 図４（Ａ）、図４（Ｂ）は機械機構の物理モデルが未知の場合の例。 図５（Ａ）、図５（Ｂ）は機械機構の物理モデル化が不可能な場合の例。 図６（Ａ）〜図６（Ｃ）は機械機構が可変部を持つ場合の例。 ３軸力覚センサーユニットの構成例。 図８（Ａ）、図８（Ｂ）は力覚センサーにおける３軸力覚センサーユニットの配置例。 ６軸力覚センサーにより検出される力覚値の説明図。 センサー座標系とユニット座標系の関係を表す具体例。 図１１（Ａ）、図１１（Ｂ）は可変部を持たず、且つ機械機構の物理モデルが未知の場合の例。 第１の実施形態のシステム構成例。 並進力及びモーメントの座標変換の説明図。 物理モデルに基づく補正処理を説明する図。 物理モデルに基づく補正処理を説明する他の図。 物理モデルに基づく補正処理を説明する他の図。 ＸＹ平面における質量中心位置の推定処理を説明する図。 ＸＹ平面上の任意の位置に配置されたユニットに基づく推定処理を説明する図。 ３つの３軸力覚センサーユニットに基づく推定処理を説明する図。 図２０（Ａ）、図２０（Ｂ）は姿勢変更を説明する図。 図２１（Ａ）、図２１（Ｂ）は３軸力覚センサーユニット数を変化させた場合の推定精度の変化を説明する図。 第１の実施形態における処理を説明するフローチャート。 固定部に関する補正処理を説明するフローチャート。 第２の実施形態における処理を説明するフローチャート。 リセット処理を説明するフローチャート。 第３の実施形態のシステム構成例。 機械機構の固定部と可変部への分割を説明する図。 図２８（Ａ）、図２８（Ｂ）は可変部に対する処理を説明する図。 可変部の姿勢情報の説明図。 図３０（Ａ）〜図３０（Ｃ）は物理モデルやパラメーター等から力覚値に対する補正量を求める処理の説明図。 第３の実施形態における処理を説明するフローチャート。 第４の実施形態のシステム構成例。 第４の実施形態における処理を説明するフローチャート。

以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．本実施形態の手法
まず本実施形態の手法について説明する。従来、多くの機械装置において力覚センサーを用いた制御等が行われている。機械装置としては例えばロボットが考えられ、当該ロボットに設けられた力覚センサーからの力覚値に基づいて、ロボット制御が行われる。具体的には、ロボットの機械機構（狭義にはハンド等の手先構造物）にはたらく外力を力覚値として検出することで、インピーダンス制御等の力制御を行うことが可能になる。

また、ロボット以外の機械装置としては、パワーアシストの入力装置を考えることもできるし、医療分野におけるリハビリテーション機器、或いはそれに付随するデータ記録装置も考えられる。リハビリテーション用途においては、患者がどのような部位においてどのような大きさの力を加えているのかという情報は、治療・診断上有用であるし、またパワーアシスト等のサポート機器から使用者に加える力も適切に制御する必要性が高く、力覚センサーを用いる利点が大きい。さらには、機械装置は遠隔操作機器（医療機器を含む）であってもよく、その場合、力覚センサーからの力覚値は、力覚フィードバック等に用いることが想定される。その他にも、熟練の技術を要する職人技を科学的に解析する場合等に用いられる力覚レコーダー等も本実施形態の機械装置となり得るし、玩具やＰＣ等の入力装置が本実施形態の機械装置であってもよい。つまり、本実施形態の機械装置としては様々な装置を想定できる。

その際、所望の制御を行うためには、力覚センサーの力覚値に対して種々の補正処理が必要となる場合がある。上述の力制御を行うロボットの例であれば、制御システムが必要とするのは、実際に作業対象と接触する部分にはたらく外力である。例えば、ハンドにより所与のツールを把持し、当該ツールを作業対象の表面を滑らせる（過度の力を加えたり、浮き上がったりしないように移動させる）制御を行う場合には、ツールが作業対象を押す力に対応して当該ツールにはたらく反力を外力として検出する必要がある。

しかし、力覚センサーが作業対象と接触する位置に設けられずに、図１及び図２に示したようにロボット１０の手首に相当する位置に設けられることも大いに考えられる。その場合、力覚センサー２０は当該センサーの先に設けられるエンドエフェクター１２（広義には機械機構であり、具体的にはハンド、或いは当該ハンドが把持するツールであってもよい）による力も検出してしまう。具体的には、エンドエフェクター１２にはたらく重力等が力覚センサー２０に伝わることになるため、力覚値は上述した外力以外の力も含めた値となり、ロボット１０の適切な制御が困難となる。

これに対しては、従来リセット処理により補正が行われていた。一例として図３（Ａ）に示したように力覚センサー２０の先に指構造を持つハンドが取り付けられた例を考える。ここでは、図３（Ｂ）に示したようにハンドがツール（ドライバー）を把持するものであり、エンドエフェクター１２は当該ハンド及びツールに相当する。リセット処理では、図３（Ｃ）に示したようにロボット１０に対して作業時と同じ姿勢をとらせる。その際、作業を実際には行わず、外力がはたらかない状態にすれば、その際の力覚センサー２０からの力覚値はエンドエフェクター１２に起因した値となる。このときの値をオフセット値とすれば、その後、ロボット１０の姿勢を変えない限り、エンドエフェクター１２に起因する力覚値はオフセット値に一致する。よって、オフセット値の記憶後、作業終了までの間、力覚値からオフセット値を除去する処理を行うことで、エンドエフェクター１２による力覚値への影響を抑止することが可能になる。

しかしリセット処理では、オフセット値の記憶後は作業終了まで、オフセット値に影響を与えるようなロボット１０の姿勢変更ができないことが、制御において大きな制限となっていた。そこで、特許文献１〜３では力覚センサーの先に設けられるエンドエフェクター１２を物理モデル化することで、当該エンドエフェクター１２による力覚センサー２０への影響を補正する手法等を用いている。物理モデルは例えば重量分布等を表すモデルであってもよく、さらに具体的には質量中心位置（ｘ，ｙ，ｚ）及び質量ｍをパラメーターとするモデルであってもよい。エンドエフェクター１２を物理モデル化した情報を保持しておけば、当該エンドエフェクター１２にはたらく重力や遠心力は物理的な解析により求めることができ、さらにそれらの力が力覚センサー２０にどのように伝わるかも解析できる。この場合、リセット処理とは異なり、姿勢が変化しても変化後の姿勢に基づく物理的な解析を行えば姿勢変化に対応可能なため、制御上の制約を小さくできる。特許文献１〜３ではこのような手法により、力覚センサー２０からの力覚値を補正し、所望の制御を実現している。

しかし、従来手法ではエンドエフェクター１２の物理モデルが既知であることを前提としている。そのため、従来手法では対応が困難となる状況が考えられる。以下、３つのケースについて具体的に説明する。

なお、以下の説明においても機械装置はロボット１０であり、機械機構はロボット１０のエンドエフェクター１２であるものとするが、機械装置及び機械機構がそれらに限定されないことは上述したとおりである。

第１にエンドエフェクター１２の物理モデルが未知の場合である。特定用途のためのロボット１０であれば、力覚センサー２０の先に設けられるエンドエフェクター１２の種類は限定されるため、従来手法のように候補となるエンドエフェクター１２の物理モデルを全て保持しておき、適切なモデルを用いて解析を行うことは可能である。しかし、汎用性の高いロボット１０では取り付け可能なエンドエフェクター１２の種類が豊富なことも考えられ、物理モデルを事前に入手しておくことが困難な可能性がある。また多くの物理モデルを事前に記憶しておくことができたとしても、図４（Ａ）、図４（Ｂ）に示したようにエンドエフェクター１２がハンド及び当該ハンドにより把持されるツールである場合には問題が残る。図４（Ａ）、図４（Ｂ）の例では、ハンドによるツールの把持位置にある程度の自由度があるため、ハンドの物理モデル及びツールの物理モデルが既知であったとしても、その両方を含むエンドエフェクター１２の物理モデルはハンドによるツールの把持位置が決定されない限り不明となるためである。つまり、図４（Ａ）、図４（Ｂ）の例では実際にツールが把持されるまでエンドエフェクター１２の物理モデルは決定不可であり、所与の既知物理モデルを用いて解析する従来手法の適用は困難となる。

第２にエンドエフェクター１２の物理モデル化がそもそも困難な場合が考えられる。具体的には、図５（Ａ）に示したようにエンドエフェクター１２が電源ケーブルを有するツールを含む場合が考えられる。この場合、ケーブルが伸びきっているのか、たるみがあるのかという点でも物理モデルは変化する。或いは、ケーブルが床等に接触している場合にはケーブルと床との摩擦等も考慮する必要があるし、ケーブルの他端が電源に接続されていることでツールの移動に要する力が変化すれのであれば、そのことでも物理モデルは変化しうる。また、図５（Ｂ）に示したコネクター付きケーブル等も同様であり、ツールの図５（Ｂ）のＡ１で示した把持部分と、Ａ２で示したケーブル部分との相対的な位置関係が変化しうるため、Ａ１及びＡ２を含んだエンドエフェクター１２全体を単一の物理モデルで表現することは困難である。この場合、そもそも物理モデル化ができないのであるから、力学的な解析は不可能であるが、従来手法においてこの問題に対応する手法は示されていない。

第３にエンドエフェクター１２が可変部を有する場合が考えられる。具体的には、図６（Ａ）〜図６（Ｃ）に示したように指構造を有するハンドにおいては、指の開き具合等が変化すれば物理モデルが変化する。例えば、図６（Ａ）のように各指構造がＤ１及びＤ２に示した箇所に関節を有する場合、それぞれの関節角を設定することで指の姿勢を変更することができる。その場合、図６（Ａ）に示したように各指構造の先端部が内側へ向くような関節角を設定した場合と、図６（Ｂ）に示したように各指構造の先端部が外側へ向くような関節角を設定した場合とでは、指構造１２−３と手首部分１２−４とを含めたエンドエフェクター１２全体の物理モデルは異なるものになる。また、手首部分１２−４が図６（Ｃ）のＤ３に示したように回転可能（ここでは力覚センサー２０に対する相対的な回転）である場合には、Ｄ３の回転角によってもエンドエフェクター１２の物理モデルは変化しうる。しかし、特許文献１、２等ではこの変化を考慮した処理は行っていない。また、特許文献３には指構造を持つハンドの図面が添付されてはいるが、処理としては重量に基づく単純なものしか行っておらず、可変部の状態に応じた変化等は考慮されていない。

そこで本出願人は、既知の物理モデルをそのまま解析に用いることができない場合にも、適切な力覚値補正を行う手法を提案する。具体的には、物理モデルが未知のケースにおいては、力覚センサー２０から取得した力覚値に基づいて、エンドエフェクター１２の物理モデルを推定する。ここで、力覚センサー２０としては、３軸力覚センサーユニットをＮ個（Ｎは２以上の整数）有するセンサーを用いるものとし、力覚センサー２０の詳細については後述する。この処理については第１の実施形態において詳述する。

また、エンドエフェクター１２の物理モデル化が困難なケースでは、処理自体は従来と同様にリセット処理を行う。ただし、ここでは第１の実施形態で説明する推定処理を試行し、試行結果に基づいてエンドエフェクター１２の物理モデル化の可否を判定する。物理モデル化が可能であれば第１の実施形態と同様に推定結果を用いればよいし、不可の場合にはリセット処理に移行すればよい。この処理については第２の実施形態において詳述する。

また、可変部を含むことで物理モデルが変化するケースでは、エンドエフェクター１２を固定部と可変部とに分離し、それぞれについて姿勢情報に基づく処理を行うことで、エンドエフェクター１２全体を表す物理モデルを求める処理を行う。この処理については第３の実施形態において詳述する。なお、第３の実施形態においては、図２７〜図２９等を用いて後述するように、説明の簡略化のため、エンドエフェクターの関節部分は図６（Ａ）のＤ２に示した箇所を考え、Ｄ１に示した関節や、図６（Ｃ）のＤ３で示した回転等は考慮していない。しかし、エンドエフェクター１２がＤ１やＤ３、或いは他の部分に関節等の構造を有することを妨げるものではない。

なお、第１〜第３の実施形態で説明する手法は独立で用いられるものに限定されず、複数を組み合わせてもよい。例えば、図４（Ａ）、図４（Ｂ）の例では可動部を含むハンドによりツールを把持し、且つ把持位置が変化しうるため、第１，第３の実施形態を組み合わせるとよい。また、把持対象となるツールに図５（Ａ）、図５（Ｂ）のような物理モデル化が不可となるものが想定されるのであれば、第２の実施形態と組み合わせる必要も出てくる。これら組み合わせについては第４の実施形態で詳述する。

以下では、まず本実施形態で用いる、３軸力覚センサーユニットをＮ個有する力覚センサー２０の詳細について説明する。その後、第１〜第４の実施形態について順に説明する。

２．複数の３軸力覚センサーユニットを有する力覚センサー
２．１ 力覚センサーの構成例
まず、３軸力覚センサーユニットをＮ個有する力覚センサー２０について説明する。Ｎ個の３軸力覚センサーユニットの各ユニットは、Ｘ軸、Ｙ軸及びＺ軸の各軸の並進力Ｆｘ，Ｆｙ，Ｆｚをユニット出力値として取得する。各ユニットは種々の構成により実現可能であるが、例えば図７に示したように、センサー素子４６，５２，５８に均等に与圧を印加させることで、高精度での力検出を可能とする構成を用いればよい。図７に示したセンサーユニット２１では、第１センサー素子４６の水晶板４８Ａ及び４８Ｂ、第２センサー素子５２の水晶板５４Ａ及び５４Ｂ、第３センサー素子５８の水晶板６０Ａ及び６０Ｂは、それぞれ結晶方位が異なるように構成されている。その結果、センサーユニット２１は、第１センサー素子４６によりユニット座標系におけるＸ軸方向の並進力を検出し、第２センサー素子５２によりＹ軸方向の並進力を検出し、第３センサー素子５８によりＺ軸方向の並進力を検出することになる。

また、力覚センサー２０において、各３軸力覚センサーユニットがどのように配置されるかについても、種々の手法が考えられるが、一例としては図８（Ａ）に示したように配置すればよい。図８（Ａ）の例では、力覚センサー２０に設定されるセンサー座標系のＸＹ平面上に、４つの３軸力覚センサーユニット２１−１〜２１−４が配置される。各ユニットに対して設定されるユニット座標系のＸ軸Ｙ軸Ｚ軸の方向は、全てセンサー座標系のＸ軸Ｙ軸Ｚ軸に一致するものとしている。図８（Ａ）の配置を上方から（センサー座標系のＺ軸正方向から）みたのが図８（Ｂ）となる。なお、各ユニットの出力であるユニット出力値は当該ユニットに設定されたユニット座標系のＸ軸Ｙ軸Ｚ軸における並進力となる。

ただし、ユニットの配置は図８（Ａ）に限定されるものではない。例えば、３軸力覚センサーユニットの個数は４個に限定されるものではなく、２個や３個、或いは５個以上であってもよい。また、全ての力覚センサーユニットが同一平面上に配置される必要はなく、力覚センサー２０内において立体的に配置されてもよい。さらに、センサー座標系の座標軸とユニット座標系の座標軸とが一致する必要もなく、所与のユニット座標系の座標軸と他のユニット座標系の座標軸とが一致する必要もない。その他、種々の配置による変形実施が可能である。ただし、第１の実施形態において後述するように、全ての３軸力覚センサーユニットが同一直線上に並ぶ配置は避けることが望ましい。

２．２ ６軸力覚センサーとしての利用
上述した力覚センサー２０は、各３軸力覚センサーユニットからＦｘ，Ｆｙ，Ｆｚの３つの値がユニット出力値として出力されるため、図８（Ａ）の例であれば３×４個の値を取得できる。第１の実施形態で後述する推定処理では３×４個の力覚値をそのまま処理に用いることを想定しているが、上記力覚センサー２０を一般的な６軸力覚センサーとして使用することも可能である。ここで６軸力覚センサーとは、図９に示したように、センサー座標系におけるＸ軸、Ｙ軸、Ｚ軸についての並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚの６つの値を力覚値として出力するセンサーである。

図８（Ａ）のようにセンサー座標系のＸＹ平面上に４つの３軸力覚センサーユニットが配置され、センサー座標系の座標軸とユニット座標系の座標軸とが一致する場合で、特に図１０に示したように、それぞれのユニット座標系の原点が、センサー座標系において（ｒ，０，０）、（０，ｒ，０）、（−ｒ，０，０）、（０，−ｒ，０）にある例を考える。

ユニット２１−１の出力を（Ｆ_１ｘ，Ｆ_１ｙ，Ｆ_１ｚ）、ユニット２１−２の出力を（Ｆ_２ｘ，Ｆ_２ｙ，Ｆ_２ｚ）、ユニット２１−３の出力を（Ｆ_３ｘ，Ｆ_３ｙ，Ｆ_３ｚ）、ユニット２１−４の出力を（Ｆ_４ｘ，Ｆ_４ｙ，Ｆ_４ｚ）とする。この場合、力覚センサー２０全体として検出する６軸の力覚値である、センサー座標系のＸＹＺ軸での並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚは下式（１）で与えられる。

Ｆｘ＝Ｆ_１ｘ＋Ｆ_２ｘ＋Ｆ_３ｘ＋Ｆ_４ｘ
Ｆｙ＝Ｆ_１ｙ＋Ｆ_２ｙ＋Ｆ_３ｙ＋Ｆ_４ｙ
Ｆｚ＝Ｆ_１ｚ＋Ｆ_２ｚ＋Ｆ_３ｚ＋Ｆ_４ｚ
Ｍｘ＝ｒ（Ｆ_２ｚ−Ｆ_４ｚ）
Ｍｙ＝ｒ（Ｆ_３ｚ−Ｆ_１ｚ）
Ｍｚ＝ｒ（Ｆ_１ｙ−Ｆ_３ｙ＋Ｆ_４ｘ−Ｆ_２ｘ） ・・・・・（１）
つまり、各ユニットからの出力値に基づいて算出処理を行うことで、図８（Ａ）に示した力覚センサー２０を６軸力覚センサーとして用いることが可能になる。また、力覚センサー２０における３軸力覚センサーユニットの数、或いは配置等が異なった場合にも、算出式が上式（１）とは異なるものになるが、同様に６軸の力覚値を算出することが可能である。言い換えれば、本実施形態の力覚センサー２０は、Ｎを２以上の任意の整数とした場合にも、各３軸力覚センサーユニットのユニット出力値の集合である３×Ｎ個の値に基づいて、６軸の力覚値を算出できる。

よって、機械装置としてロボットを用いて、当該ロボットの力制御（インピーダンス制御等）を行う場合等には、通常の６軸力覚センサーとして上式（１）等の結果を用いればよいし、後述する推定処理等のように情報量が多いことが望ましい状況においては、各ユニットからのユニット出力値の集合である３×Ｎ個の値を用いて処理を行えばよい。つまり、本明細書や特許請求の範囲における「力覚値」という用語は、力覚センサー２０の出力する情報を表すものであり、ユニット出力値の集合である３×Ｎ個の値と、そこから求められた（Ｆｘ，Ｆｙ，Ｆｚ，Ｍｘ，Ｍｙ，Ｍｚ）の両方を含みうる概念である。

後述する第１〜第４の実施形態では「力覚値」という用語で表現されているが、典型的には、力覚値のうちのユニット出力値の集合である３×Ｎ個の値に基づいて物理モデル（質量中心位置等）を求め、当該物理モデルに基づいて、力覚値のうちの６軸の値（Ｆｘ，Ｆｙ，Ｆｚ，Ｍｘ，Ｍｙ，Ｍｚ）の補正処理を行って、補正処理後の６軸の値に基づいて機械装置の制御を行うことを想定している。ただし、物理モデルの推定処理に用いられる力覚値が、ユニット出力値の集合である３×Ｎ個の値である点は本実施形態における前提となる可能性が高いものの、補正処理の対象については６軸の値ではなく、ユニット出力値の集合である３×Ｎ個の値であることを妨げるものではない。

３．第１の実施形態
第１の実施形態では、力覚センサーから取得した力覚値に基づいてエンドエフェクターの物理モデルを推定し、推定した物理モデルを用いた補正処理を行う手法について説明する。以下、システム構成例、物理モデルの推定処理、及び処理の詳細について説明する。

なお、図４（Ａ）、図４（Ｂ）の例では、エンドエフェクターは可変の指構造を含むため、第３の実施形態で後述する手法を併用する必要が生じる。よってここでは、図１１（Ａ）、図１１（Ｂ）に示した吸着ハンド１２−１等により、エンドエフェクター１２が可変部を含まずに構成される例について考えるものとする。なお、図１１（Ａ）、図１１（Ｂ）の吸着ハンド１２−１は、その先端部分においてツール１２−２を固定（把持）するものである。この場合、図１１（Ａ）に示したようにツール１２−２の中央付近を吸着した場合と、図１１（Ｂ）に示したようにツール１２−２の端部に近い位置を吸着した場合とでは、吸着ハンド１２−１とツール１２−２を含むエンドエフェクター１２全体の物理モデルは異なるものになり、吸着位置が事前にわからない限り、物理モデルは未知となる。

３．１ システム構成例
図１２に本実施形態のロボット１０、エンドエフェクター（ハンド）１２、力覚センサー２０、及びロボットの制御装置１００を含む制御システム（ここではロボットシステム）の構成例を示す。

エンドエフェクター１２は、ロボット１０に設けられる機構であり、ここでは特にロボット１０において力覚センサー２０よりも先に設けられるものを表す。エンドエフェクター１２はハンドや、当該ハンドにより把持されるツール（例えばドライバー等）を含む。

力覚センサー２０は、センシング部にはたらく力を検出するセンサーであり、上述した３軸力覚センサーユニットを複数有するものを用いる。

ロボットの制御装置１００は、制御部１１０と、力覚値補正部１２０と、姿勢情報取得部１３０とを含む。ただし、制御装置１００は図１２の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

制御部１１０は、ロボット１０及びエンドエフェクター１２の制御を行う。本実施形態では、力覚値補正部１２０による補正処理後の力覚値に基づいて、ロボット１０及びエンドエフェクター１２の制御を行う。ここで、ロボット１０等の制御とは、例えば関節部分の角度変化量、及び変化速度等の制御であってもよい。

力覚値補正部１２０は、力覚センサー２０からのセンサー情報（力覚値）を取得し、取得した力覚値に対する補正処理を行う。力覚値補正部１２０は、第１の補正処理部１２１と、第２の補正処理部１２３と、固定部パラメーター記憶部１２４と、実行時パラメーター算出部１２６と、実行時パラメーター記憶部１２７とを含む。

本実施形態では、図１１（Ａ）に示したように物理モデルが既知の部分（以下固定部と記載。具体的には図１１（Ａ）吸着ハンド１２−１）を用いて、所与の部材（図１１（Ａ）のツール１２−２）の任意の位置を把持することにより、エンドエフェクター１２全体の物理モデルが未知のケースを想定している。第１の補正処理部１２１は、固定部について、力学的な解析を行って当該固定部による力覚値への影響を抑止する補正処理を行う。具体的には、固定部の物理モデルを固定部パラメーターとして記憶している固定部パラメーター記憶部１２４から当該固定部パラメーターを読み出し、読み出した固定部パラメーターと、姿勢情報取得部１３０から出力されたロボット１０の姿勢情報とに基づいて補正処理を行う。第１の補正処理部１２１からの出力では、固定部に起因する力覚値が除去（広義には低減）されている。固定部に対応する補正処理の詳細は後述する。

実行時パラメーター算出部１２６は、第１の補正処理部１２１から出力された力覚値に基づいて、固定部に取り付けられた（把持された）部材の物理モデルを推定する。詳細については後述する。実行時パラメーター記憶部１２７は、実行時パラメーター算出部１２６で推定された物理モデルを、実行時パラメーターとして記憶する。

第２の補正処理部１２３は、実行時パラメーター記憶部１２７に記憶された実行時パラメーターと、姿勢情報取得部１３０から出力されたロボット１０の姿勢情報とに基づいて補正処理を行う。第２の補正処理部１２３からの出力では、物理モデルが未知となる部材に起因する力覚値が除去（広義には低減）されている。つまり、第２の補正処理部１２３から制御部１１０に対して出力される力覚値は、エンドエフェクター１２に起因する値が除去されたものとなる。

姿勢情報取得部１３０は、ロボット１０及びエンドエフェクター１２の姿勢情報を取得する。ここで姿勢情報とは、ロボット１０やエンドエフェクター１２の姿勢を表すものであり、例えば関節角をパラメーターとして表現することができる。全ての関節角について値が決定されれば、フォワードキネマティクスによりロボット１０の姿勢を一意に決定可能である。具体的には、姿勢情報取得部１３０はロボット１０等の各関節に対応するエンコーダー等から値を取得すればよい。

３．２ 第１の補正処理部における補正処理の詳細
第１の補正処理部において行われる固定部についての力覚値補正処理について説明する。図１３に示したように、ある座標系１において、並進力がＦ、モーメントがＭで与えられている場合を考える（Ｆ，Ｍは例えば３次元ベクトル）。この際、座標系２として、座標系２から座標系１への原点の並進ベクトルがｔ、座標系２から座標系１への回転行列がＲとなる座標系を考えると、上記の力を座標系２において観察したばあい、その並進力Ｆ’及びモーメントＭ’は下式（２）を満たす。なお２式目の右辺第２項はベクトルの外積を表す。

Ｆ’＝ＲＦ
Ｍ’＝ＲＭ＋ｔ×Ｆ ・・・・・（２）
ここで、図１４に示したように、センサー座標系を基準とした場合に、位置（ｘ，ｙ，ｚ）に質量ｍを有するものとモデル化できるエンドエフェクター１２により、力覚センサー２０にはたらく力を計算する。ここでは、図１５に示したように、図１３の座標系２としてセンサー座標系を考え、図１３の座標系１として質量中心位置（ｘ，ｙ，ｚ）を原点、Ｚ軸方向が鉛直上向きとなる座標系を考える。

この場合、座標系１における上記のＦ，Ｍは質量ｍの物体にはたらく重力を考えればよいため、Ｆ＝（０，０，−ｍ）、Ｍ＝（０，０，０）となる。また、図１５のように座標系１，２で回転がない場合には、Ｒを考慮しなくてよく、上記のｔはｔ＝（ｘ，ｙ，ｚ）となる。これにより、上式（２）に基づきＦ’＝（０，０，−ｍ）、Ｍ’＝（−ｍｙ，ｍｘ，０）となる。

つまり、図１５の例では、センサー座標系において（ｘ，ｙ，ｚ）の位置に質量ｍを有するエンドエフェクター１２による力は、センサー座標系ではＦ’＝（０，０，−ｍ）、Ｍ’＝（−ｍｙ，ｍｘ，０）として観測される。これは、力覚センサー２０にＦ’＝（０，０，−ｍ）という並進力、及びＭ’＝（−ｍｙ，ｍｘ，０）というモーメントがはたらくことに他ならない。よって、求められたＦ’及びＭ’の値を力覚値から除去することで、エンドエフェクター１２による力覚値への影響を抑止することが可能になる。

また、座標系１，２で回転がある場合にはＲまで考慮すればよい。図１６でも座標系１のＺ軸が鉛直上向きである点に変わりはなく、Ｆ＝（０，０，−ｍ）、Ｍ＝（０，０，０）となる。ただし図１６では、力覚センサー２０が鉛直方向に対して傾いている他、Ｘ軸Ｙ軸も座標系１とは異なっており、回転行列Ｒは座標系１に対するセンサー座標系のロール角、ピッチ角、及びヨー角の値に基づきその要素が決定されることになる。この場合も上式（２）により求めたＦ’及びＭ’に基づいて補正処理を行えばよい。

３．３ 物理モデルの推定処理
次に本実施形態における物理モデルの推定処理について説明する。以下の説明では、物理モデルとは質量中心位置（力の作用点に相当）を表す（ｘ，ｙ，ｚ）と、質量ｍにより表されるモデルとする。上述したように、Ｎ個の３軸力覚センサーユニットを有する力覚センサー２０は６軸力覚センサーとして用いることができるため、６軸力覚センサーとしての６つの力覚値に基づいて推定処理を行ってもよい。しかしＮ≧３の場合であれば、上記力覚センサー２０は、６軸力覚センサーの６つの力覚値よりも多い数の情報を出力できるため、推定精度の向上等が期待できる。よって本実施形態では、３×Ｎ個の出力値をそのまま用いる推定手法について説明する。

３．３．１ 基本的な手法
エンドエフェクター１２に外力がはたらくような作業をしていない場合、誤差等が生じる可能性はあるが、基本的には力覚センサー２０には当該エンドエフェクター１２による力のみがはたらくことが想定される。そして各３軸力覚センサーユニットは、エンドエフェクター１２による当該力の各ユニットに対応した射影成分を検出することになる。本実施形態ではこの状態においてエンドエフェクター１２の物理モデルを推定するものとする。

説明を簡略化するために、まずＸＹ平面について考える。エンドエフェクター１２により、所与の三次元座標を力の作用点（ｘ，ｙ，ｚ）とするベクトルＦで表される大きさ及び方向の力が力覚センサー２０にはたらいている場合、図１７に示したようにＸＹ平面では（ｘ，ｙ）を力の作用点とし、ベクトルＦをＸＹ平面に射影したベクトルＦ_ｘｙで表される力がはたらく。

ここで、図１７に示したようにセンサーユニットの数を２個とする。この場合、ベクトルＦ_ｘｙを、力の作用点からユニット２１−１の座標系の原点への方向である第１の方向と、力の作用点からユニット２１−２の座標系の原点への方向である第２の方向とに分離した際の、第１の方向成分Ｆ１がユニット２１−１にはたらき、第２の方向成分Ｆ２がユニット２１−２にはたらくことになる。つまり、ユニット２１−１は、ベクトルＦ１に対応するユニット出力値Ｆ_１ｘとＦ_１ｙを検出し、ユニット２１−２は、ベクトルＦ２に対応するＦ_２ｘとＦ_２ｙを検出する。

以上の流れを逆に考えれば、ユニット２１−１がユニット出力値Ｆ_１ｘとＦ_１ｙを検出し、ユニット２１−２がＦ_２ｘとＦ_２ｙを検出した場合、Ｆ_１ｘとＦ_１ｙにより表されるベクトルＦ１と、Ｆ_２ｘとＦ_２ｙにより表されるベクトルＦ２を考えれば、力覚センサー２０に対して力を及ぼしているエンドエフェクター１２の物理モデルを推定することができる。

具体的には、ベクトルＦ１を含む第１の直線と、ベクトルＦ２を含む第２の直線はともに力の作用点を含むという特性を有することから、第１の直線と第２の直線の交点を力の作用点（つまり質量中心位置）として求めることができる。さらに、当該作用点に作用する力の大きさ及び方向は、ベクトルＦ１とベクトルＦ２の合成ベクトルの大きさ及び方向として求めればよい。

以上の処理を３次元に拡張すれば、エンドエフェクター１２の物理モデルの推定が可能になる。少し条件を一般化し、図１８に示したように、ユニット２１−１のユニット座標系の原点が、センサー座標系における（Ｘ１，Ｙ１）であるとする。この場合、力の作用点（具体的には作用点をＸＹ平面へ射影した点）を含む直線は、下式（３）及びそれを変形した下式（４）で与えられる。

Ｆ_１ｙＸ_１−Ｆ_１ｘＹ_１＝Ｆ_１ｙＸ−Ｆ_１ｘＹ ・・・・・（４）
なお、より条件を一般化するのであれば、ユニット座標系とセンサー座標系との間の回転Ｒも考慮する必要がある。その場合、Ｒ^−１を用いた補正を行って上式（４）と同等の式を求めればよく、ここでは詳細な説明は省略する。上式（４）をベクトル形式で表記すれば下式（５）となる。

これをＹＺ平面及びＺＸ平面についても同様に考えることで、下式（６）が成り立つ。

上式（６）の右辺の行列の逆行列を求めることができれば、当該逆行列を左からかけることで質量中心位置（ｘ，ｙ，ｚ）を算出可能である。しかし、行列式を求めれば値が０になることからわかるように、右辺の行列は正則ではなく逆行列は求められない。これは図１７や図１８に示したように、１つの３軸力覚センサーユニットからは、質量中心位置を含む１本の直線（３次元に拡張すれば３次元直線となる）が求められるに過ぎず、質量中心位置の特定には少なくとも２本の直線が必要なことからも明らかである。

つまり、質量中心位置の推定にはセンサーユニット２１−１とは異なるセンサーユニット２１−２のユニット出力値も用いればよいことになる。つまり、上式（６）に対応する式をセンサーユニット２１−２についても求め、求めた式と上式（６）を連立方程式として解を求めればよい。これは下式（７）のｘ，ｙ，ｚを求めることに相当する。

上式（７）では、一部の場合を除いて右辺の行列Ｍは正則となるため、逆行列Ｍ^−１を用いることで質量中心位置を推定することができる。なお、行列Ｍが正則でない場合については後述する。

３軸力覚センサーユニットの検出するユニット出力値に全く誤差がないという理想的な状況であれば、センサーユニット２１−１から求められる直線と、センサーユニット２１−２から求められる直線は交点を持つはずであり、上式（７）から解を定めることができる。しかし、現実には誤差は生じることが当然であり、図形的には２本の３次元直線は交点を持つとは限らないことを考えれば、上式（７）からは単純には解が定まらないことになる。

そこで本実施形態では、誤差が最小となるように質量中心位置ｐ＝（ｘ，ｙ，ｚ）を求める。具体的には、上式（７）の自乗誤差ｅを下式（８）のように定義する。

ここで、自乗誤差ｅを最小にする解は下式（９）の正規化方程式の解として得られることが知られており、下式（９）の正規化方程式の解である最小自乗解は、形式的に下式（１０）により与えられることも知られている。

Ｍ^Ｔｑ＝Ｍ^ＴＭｐ ・・・・・（９）
ｐ＝（Ｍ^ＴＭ）^−１Ｍ^Ｔｑ ・・・・・（１０）
なお、上式（１０）の（Ｍ^ＴＭ）^−１Ｍ^Ｔは一般化逆行列（或いは疑似逆行列）と呼ばれるものであり、Ｍが下式（１１）のように特異値分解される場合には、一般化逆行列は下式（１２）によっても与えられることになる。

Ｍ＝ＵΣＶ^Ｔ ・・・・・（１１）
（Ｍ^ＴＭ）^−１Ｍ^Ｔ＝ＶΣ^−１Ｕ^Ｔ ・・・・・（１２）
ｎ≧ｍとし、Ｍ∈Ｒ^ｎ×ｍ、Ｕ∈Ｒ^ｎ×ｎ、Ｖ∈Ｒ^ｍ×ｍ、Σ∈Ｒ^ｎ×ｍ、Ｕ，Ｖが適切に定められたｎ次元とｍ次元の正規直交基底である場合に、行列Ｍは下式（１３）のように分解される。

ここでΣの対角要素であるσは特異値と呼ばれるものであり、非ゼロであるσの個数は行列Ｍのランクと等しい。つまり、上述した式により適切に質量中心位置を求めることができるか否かは、非ゼロであるσの個数から判定可能ということになる。

３．３．２ 変形例
上記処理（例えば上式（１０）を解く処理）により、質量中心位置を求めることができる。しかし、３軸力覚センサーユニットが２つであり、当該２つのユニットにそれぞれ設定されたユニット座標系の原点と、外力の作用点が同一直線上に乗る場合には、質量中心位置を求めることができない。

具体的には、図１７において、外力２として示した位置に質量中心位置がある状況であり、この場合にはユニット２１−１もユニット２１−２も外力に対して同一の情報を検出することしかできない。具体的にはユニット２１−１から求められる直線と、ユニット２１−２から求められる直線とが一致してしまう。よって有意な情報はユニット１つ分しか得られないことになり、質量中心位置を推定することができなくなる。これは上式（７）でいえば、逆行列Ｍ^−１を求めることができない（Ｍが正則でない）状況に該当することになる。そして、この状況にあることは上式（１３）における非ゼロのσの個数が少ないことから検出可能である。

この問題に対応するための第１の手法としては、３個（広義には３個以上）の３軸力覚センサーユニットを用い、各ユニットにそれぞれ設定されたユニット座標系の原点が、全て同一直線上に並ぶことがないように、力覚センサー２０においてユニットの配置を行えばよい。例えば、図１９に示したように３つのユニットを配置すれば、外力の作用点がどのような位置にあったとしても、少なくとも２つのユニットは有意な情報を検出するため、質量中心位置を推定することが可能になる。

この場合には、３個目のユニットについても上式（６）に相当する式を考え、全てのセンサーに対応する式を連立方程式として解けばよい。具体的には、用いるユニットの個数を３個とした場合には、上式（７）のベクトルｑが３×３＝９次元のベクトルに拡張されるとともに、行列Ｍが９行、３列の行列に拡張されることになる。上式（７）に対応する式が求められた後の処理については、ユニットが２個の場合と同様である。

なお、誤差が生じる場合には図１９に示したように、平面で考えても３本以上の直線が１点で交わらない可能性がある。つまり３次元空間で考えた場合、交点が複数ある、或いは交点は１つであるが当該交点を通らない直線がある、さらには全ての直線が交点を持たない等、上式（７）からは単純には解が定まらない可能性が高い。よって、上式（７）をそのまま解くのではなく、上式（８）以降を用いて最小自乗解を求めることが望ましいのは、３個以上の３軸力覚センサーユニットを用いる場合でも同様となる。

また、上記問題に対応するための第２の手法としては、３軸力覚センサーユニットの数は２個のままにした上で、一回の推定処理により質量中心位置を決定するのではなく、ロボット１０の姿勢を変えて（エンドエフェクター１２によりはたらく力の、センサー座標系における方向が変化するように姿勢を変えて）複数回の推定処理を行ってもよい。姿勢変更は例えば、図２０（Ａ）の状態から図２０（Ｂ）の状態への変更により行われる。ロボット１０の姿勢が変われば、センサー座標系におけるエンドエフェクター１２の質量中心位置が変わることが想定されるため、全てのユニット座標系の原点と質量中心位置が同一直線上にある状況を脱することが可能なためである。

３．３．３ 推定精度の向上
以上の推定処理では力覚センサー２０の有する３軸力覚センサーユニットの個数は２個又は３個であるものとして説明した。しかし、ユニット数はこれに限定されず４個以上のユニットを用いてもよい。Ｎ個のユニットを用いた場合、各ユニットからのユニット出力値の集合である３×Ｎ個の値を用いて推定処理を行うことができるため、Ｎが大きいほど推定精度の向上が期待できる。

Ｎを２≦Ｎ≦１２とし、各ユニットからのユニット出力値に最大３％強のランダムノイズを重畳して推定処理をシミュレーションした結果を図２１（Ａ）、図２１（Ｂ）に示す。図２１（Ｂ）がＸＹＺの各軸について正解データとの誤差の最大値をＮごとに示した図であり、図２１（Ａ）は推定誤差の偏差をＮごとに示した図である。

図２１（Ｂ）を見ればわかるように、誤差の最大値は、多少のばらつきはあるもののＮが大きいほど小さくなる傾向にある。また、図２１（Ａ）から明らかなように推定誤差の偏差についてはＮが大きくなることで単調減少する。つまり、力覚センサー２０が含むユニット数が多くなるほど、推定精度の向上が期待できるといえる。

ただし、ユニット数を多くすることで、コストの増大や力覚センサー２０の大型化等の可能性も考慮されるため、ユニット数については複合的に適切な値を設定するとよい。

また、センサーユニット数とは異なる精度向上のアプローチとして、複数の姿勢による推定処理を行って精度向上を図る手法も考えられる。例えば、センサー等のノイズが非常に大きい場合等では、推定精度が低くなるが、複数の姿勢での推定値を複合的に用いることで対応してもよい。

複数の姿勢を用いる場合には、図２０（Ａ）、図２０（Ｂ）を用いて上述したように、姿勢を変更し、変更前及び変更後の両方で推定処理を行えばよい。よって、推定された交点（質量中心位置）が不適切と判定された場合に姿勢を変えてやり直すだけでなく、適切に交点を推定できた場合にも、姿勢変更及び再度の推定処理を行うとよい。例えば、少なくともｋ（ｋは２以上の整数）個の適切な推定値を取得して、それらの平均等を最終的な質量中心位置とすればよい。このようにすれば、推定精度の向上が期待できる。

３．４ 処理の詳細
図２２のフローチャートを用いて本実施形態の補正処理の流れを説明する。この処理が開始されると、まず力覚センサー２０からの出力値（補正処理前の力覚値）を取得する（ステップＳ１０１）。そして取得した力覚値に対して固定部に対応する補正処理を行う（ステップＳ１０２）。

ステップＳ１０２での固定部の補正処理の流れを図２３のフローチャートに示す。固定部の補正処理では、まずロボット１０の姿勢情報を取得する（ステップＳ２０１）。ここでロボット１０の姿勢情報とは、当該ロボット１０の設置状態を表す設置情報（例えば設置対象が床であるのか、壁や天井であるのかを表す情報であり、具体的には重力方向に対する角度情報等）と、ロボット１０の関節部分の関節角の値等である。そして、姿勢情報と固定部パラメーター（上述したように固定部の物理モデルｘ，ｙ，ｚ，ｍに対応）とに基づいて、力覚値に対する補正量である固定部補正量を算出する（ステップＳ２０２）。固定部補正量とは具体的には上式（１）で示したＦ’やＭ’の要素に相当する。そして、ステップＳ１０１で取得した力覚値と、ステップＳ２０２で算出した固定部補正量に基づいて、固定部補正処理後の力覚値を出力する（ステップＳ２０３）。ステップＳ２０３の出力は図１２の第１の補正処理部１２１の出力に対応する。

固定部に関する補正処理後の力覚値（ステップＳ１０２の処理の出力値）が得られたら、３軸力覚センサーユニットごとに、その値に基づいて図１７等に示した直線を求め、複数の（具体的にはセンサーユニット数だけの）直線の交点として質量中心位置を推定する（ステップＳ１０３）。具体的には、上式（７）（或いは上式（７）を３つ以上のユニットに拡張したもの）を解けばよいが、誤差を考慮すれば正規化方程式の最小自乗解を上式（１０）等から求めることになる。

ステップＳ１０３の処理により推定値を取得することができるが、それは適切なものであるとは限らない。具体的には、図１７の外力２に示したように全ての３軸力覚センサーユニットの座標系の原点と、質量中心位置が同一直線上の場合には、推定値は実際の値とは大きく異なる値となり、補正処理に用いることは適切ではない。そこで、推定した交点（質量中心位置）が適切なものか否かの判定を行う（ステップＳ１０４）。具体的な判定は上式（１３）における非ゼロのσの個数を判定すればよい。上述したように、非ゼロのσの個数は行列Ｍのランクと一致するため、非ゼロのσの個数から上式（７）が解けるのか否か、つまり上式（１０）等から推定される交点が適切か否かの判定が可能である。

求められた交点が不適切な場合（ステップＳ１０４でＮｏの場合）には、制御部１１０に対して姿勢変更を指示して（ステップＳ１０５）、ステップＳ１０１に戻る。つまり、推定した交点は質量中心位置として採用せずに、姿勢を変更した上で力覚値の取得からやり直すことになる。

一方、推定した交点が適切である場合（ステップＳ１０４でＹｅｓの場合）には、推定した交点を質量中心位置として採用するとともに、図１７に示したように各ユニットにおいて得られた力ベクトル（Ｆ１やＦ２等）の合成ベクトルの大きさとして質量ｍを求め、（ｘ，ｙ，ｚ，ｍ）を実行時パラメーターとする（ステップＳ１０６）。

次に、再度処理を行うかの判定を行う（ステップＳ１０７）。これは上述した精度向上等に関する判定である。例えば、少なくともｋ個（ｋは２以上の整数）の実行時パラメーターを求め、それらに基づいて最終的な実行時パラメーターを決定するという手法を用いるのであれば、ステップＳ１０７の判定はその時点で取得された実行時パラメーターの数がｋ以上であればＮｏ、ｋより小さい場合にはＹｅｓとなる。ステップＳ１０７でＹｅｓの場合には、ステップＳ１０４でＮｏであった場合と同様にステップＳ１０５に移行し、姿勢を変更した上でステップＳ１０１に戻り、力覚値を取得して上述した処理を行う。

ステップＳ１０７でＮｏの場合には、複数回の処理を行ったかの判定を行う（ステップＳ１０８）。ステップＳ１０８は実際には、複数の実行時パラメーターを求めたか否かの判定であり、ステップＳ１０４においてＮｏと判定されて姿勢変更が行われた場合であっても、取得された実行時パラメーターが１つのみであればＮｏと判定される。ステップＳ１０８でＮｏの場合には、求めておいた単一の実行時パラメーターを、そのままその後の補正処理に用いられる実行時パラメーターとして記憶する（ステップＳ１０９）。

ステップＳ１０８でＹｅｓの場合には、複数求められた実行時パラメーターが所定誤差以内にあるかの判定を行う（ステップＳ１１０）。本実施形態で用いている実行時パラメーターのうち位置を表す（ｘ，ｙ，ｚ）はセンサー座標系における値であることを想定している。センサー座標系における質量中心位置（ｘ，ｙ，ｚ）は、エンドエフェクター１２自体の構成が変更されない限り、ロボット１０の姿勢が変わったとしても一定のはずである。仮に、実行時パラメーターがセンサー座標系以外の座標系を用いて表現されていたとしても、その値をセンサー座標系に変換すれば、その値がロボット１０の姿勢に依存しない特性に変わりはない。また、質量ｍについては、ロボット１０がどのような姿勢であっても、またどのような座標系を用いたとしても、エンドエフェクター１２自体の構成が変更されなければ一定値となるのは当然である。つまり、誤差が全く生じない理想的な状況であれば、複数回求められた実行時パラメーターは全て同じ値をとるはずである。現実には、力覚値や姿勢情報等に誤差が生じるため、全ての実行時パラメーターが一致するとは考えにくいが、それでもある程度近い値をとることが期待される。逆に言えば、所定誤差以上に値がばらついているのであれば、何らかの要因により推定した質量中心位置等は信頼性が低いと言うことになる。

以上のことから、実行時パラメーターが所定誤差以内の場合（ステップＳ１１０でＹｅｓの場合）には、求めた実行時パラメーターは信頼できるものとして、ステップＳ１０９に移行してそのときの実行時パラメーターを記憶する。ステップＳ１０９で記憶されるのは、複数の実行時パラメーターのうちの代表値であってもよいし、平均値等から求められたものであってもよい。

一方、ステップＳ１１０でＮｏの場合には、求めた実行時パラメーターは信頼できないということであるため、その際の実行時パラメーターは記憶せずに、エラーを出力して（ステップＳ１１１）処理を終了する。

以上の本実施形態では、図１２に示したように制御システムは、エンドエフェクター（機械機構）１２と、Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有し、Ｎ個の３軸力覚センサーユニットの各３軸力覚センサーユニットから、エンドエフェクター１２に起因する値が付加されたユニット出力値を取得し、ユニット出力値に基づいて力覚値を出力する力覚センサー２０と、力覚センサー２０が出力した力覚値に基づいて、力覚値を補正する力覚値補正部１２０と、力覚値補正部１２０において補正された力覚値に基づいて、エンドエフェクター１２を含むロボット（機械装置）１０の制御を行う制御部１１０とを含む。

ここで、力覚値はユニット出力値に基づく値であるとともに力覚センサー２０の出力となる値であり、具体的には、各３軸力覚センサーユニットからのユニット出力値の集合が含まれる。また、力覚値とはユニット出力値から所与の演算により求められる値（具体的には上式（１）により求められるＸＹＺ軸での並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚ）であってもよい。

これにより、Ｎ個の３軸力覚センサーユニットを有する力覚センサー２０を用いた制御を行う場合に、当該Ｎ個の３軸力覚センサーユニットから出力されるユニット出力値に応じた力覚値に基づいて、補正処理を行うことが可能になる。上述したように、ロボット１０の制御においては、力覚センサー２０の出力である力覚値をそのまま用いるのではなく、何らかの補正処理が必要になる可能性が高く、これは機械装置としてロボット１０以外の装置を用いる場合も同様である。その場合、通常の６軸力覚センサーであればその出力である力覚値は６つであるところ、Ｎ個の３軸力覚センサーユニットのユニット出力値をそのまま用いることができれば、本実施形態の力覚センサー２０からは３×Ｎ個の値を取得することができる。つまり、Ｎ＝２の場合であっても、通常の６軸力覚センサーと同程度の精度での補正処理が期待でき、Ｎが３以上であれば情報量が６軸力覚センサーよりも多くなるため、より精度の高い補正処理を行うこと等が可能になる。

なお、本実施形態では力覚センサー２０からの力覚値に基づいて、力覚値の補正処理を行うことになるが、補正処理に用いられる力覚値と、補正処理の対象となる力覚値とは同一の値に限定されない。例えば、力覚値に含まれる所与の値を用いて補正処理に必要なパラメーターを求め、求めたパラメーターを用いて力覚値に含まれる他の値の補正処理を行ってもよい。具体的には、ユニット出力値の集合からエンドエフェクター１２の物理モデルを求め、当該物理モデルを用いて、３軸並進力及び各軸まわりのモーメントである６軸の値に対する補正処理を行ってもよい。

また、力覚センサー２０は、Ｎ個の３軸力覚センサーユニットの各３軸力覚センサーユニットから出力されたユニット出力値を含む情報を、力覚値として出力してもよい。そして、力覚値補正部１２０は、力覚値に含まれるユニット出力値に基づいて、エンドエフェクター１２の質量中心位置を推定し、推定した質量中心位置に基づいて、補正処理を行ってもよい。また、力覚値補正部１２０は、力覚値に含まれるユニット出力値から、Ｎ個の前記３軸力覚センサーユニットのそれぞれが検出した力に対応するＮ個の力ベクトルを求め、求めたＮ個の力ベクトルの合成ベクトルの大きさに基づいて、エンドエフェクター１２の質量を推定し、推定した質量に基づいて、補正処理を行ってもよい。

これにより、エンドエフェクター１２を表すパラメーター（物理モデル）として、質量中心位置（ｘ，ｙ，ｚ）と質量ｍとを用いることが可能になる。質量中心位置と質量が求められれば、図１３〜図１６に示したように、それらと回転行列Ｒ（ロボット１０の姿勢情報から算出される）から、力覚値の補正処理が可能である。

また、力覚センサー２０は、３個以上の３軸力覚センサーユニットを有してもよい。さらに望ましくは、力覚センサー２０は、４個以上の３軸力覚センサーユニットを有してもよい。

これにより、適切な交点（質量中心位置）推定、及び推定処理の精度向上等が可能になる。図１７において、外力２として示したように、全てのユニット座標系の原点と外力の作用点（推定対象となる質量中心位置）とが同一直線上にある場合には、各ユニットから得られる情報が同じものとなってしまい、適切な推定ができない。具体的には、上式（１０）等から一応の解を求めることはできるが、その値は正解値とはかけ離れたものとなる。その点、３軸力覚センサーユニットを３個以上とし、それらのユニット座標系の原点が同一直線上に乗らないように配置すれば、質量中心位置がセンサー座標系のどの位置にあったとしても、上記のような状況にはならず適切に質量中心位置を推定することができる。さらに、図２１（Ａ）、図２１（Ｂ）に示したように、力覚センサー２０に含まれる３軸力覚センサーユニットの個数を増やすことで、推定精度を向上させることが可能であるため、コストやセンサーサイズ等が許容するのであれば、３軸力覚センサーユニットの個数は多いほど望ましいということになる。

また、力覚センサー２０は、２個（少なくとも２個）の３軸力覚センサーユニットを有してもよい。その場合、力覚値補正部１２０は、ロボット１０の第１の姿勢において、Ｎ個の３軸力覚センサーユニットからのユニット出力値に基づき力覚センサー２０から出力された第１の力覚値に基づいて、エンドエフェクター１２の質量中心位置を推定する処理を行い、制御部１１０は、力覚値補正部１２０により質量中心位置の推定が不可と判定された場合には、ロボット１０の姿勢を第１の姿勢とは異なる第２の姿勢に変更する制御を行ってもよい。そして、力覚値補正部１２０は、制御部１１０によりロボット１０が第２の姿勢となる制御が行われた後に、Ｎ個の３軸力覚センサーユニットからのユニット出力値に基づき力覚センサー２０から出力された第２の力覚値に基づいて、エンドエフェクター１２の質量中心位置を推定し、推定した質量中心位置に基づいて、補正処理を行う。

これにより、２個の３軸力覚センサーユニットを有する力覚センサー２０を用いることが可能になる。用いる力覚センサーユニットが少なくてすむため、力覚センサー２０のコスト低減や、小型化等が可能になる。なお、図１７に示したように、３軸力覚センサーユニットが２個の場合には、全てのユニット座標系の原点と質量中心位置が同一直線上となる可能性があるため、単純に質量中心位置を推定する（２直線の交点を求める）だけではなく、その妥当性を検証して、その上で交点の推定が適切に行われていない場合には、姿勢変更して推定をやり直すとよい。姿勢変更は例えば図２０（Ａ）から図２０（Ｂ）への変更等であり、センサー座標系におけるエンドエフェクター１２の質量中心位置が変化するような姿勢変更である。

また、力覚値補正部１２０は、ロボット１０の質量中心位置の最小自乗解を正規化方程式により求めることで、エンドエフェクター１２の質量中心位置を推定してもよい。

ここで、正規化方程式とは、その解が、上式（８）で示した自乗誤差ｅを最小にする解となるような方程式であり、この場合上式（９）で与えられる。そして、最小自乗解とは正規化方程式の解であり、上式（９）の正規化方程式の解である最小自乗解は、上式（１０）により与えられる。

これにより、上式（８）〜（１０）を用いて、直線の交点として質量中心位置を推定することが可能になる。各３軸力覚センサーユニットから出力される力覚値等に全く誤差が含まれないことは考えにくいため、上述したようにユニット毎に決定される直線は、複数の交点を持つ、或いは全く交点を持たない等の状況が考えられ、上式（７）から交点が適切に定まるとは言えない。その点、正規化方程式から最小自乗解を求めることで、そのような状況においても誤差の少ない推定が可能になる。

また、力覚値補正部１２０は、正規化方程式に対して特異値分解を行い、特異値分解により得られる特異値の数に基づいて、質量中心位置の推定が可能か否かの判定を行ってもよい。

ここで、特異値分解とは上式（１３）に示したような分解のことであり、特異値の数とは、上式（１３）の行列Σの対角成分のうち、非ゼロとなるものの数を表す。

これにより、質量中心位置の推定（交点の推定）が適切に行われたか否かを容易に判定することが可能になる。上述したように、本実施形態では上式（７）をそのまま解くのではなく、上式（８）以降の式により最小自乗解を求める。そのため、上式（７）の行列Ｍが正則でないような、そもそも解を求めることができない状況であっても一応の解が推定できてしまう。よって、得られた解をそのまま採用してしまうのは、その後の補正処理等を考慮すれば望ましいことではない。そのため、何らかの手法により推定した交点の妥当性はチェックするべきであり、一例としては特異値の数による判定を行えばよい。

また、力覚センサー２０は、Ｎ個の３軸力覚センサーユニットの各３軸力覚センサーユニットから、ユニット出力値として３つの値を取得することで、３×Ｎ個の値を取得し、取得した３×Ｎ個の値に基づいて、Ｘ軸、Ｙ軸及びＺ軸の並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚを算出し、算出したＦｘ，Ｆｙ，Ｆｚ及びＭｘ，Ｍｙ，Ｍｚを含む情報を力覚値として出力する６軸力覚センサーであってもよい。

これにより、本実施形態の力覚センサー２０を通常の６軸力覚センサーとして用いることが可能になる。ロボット１０の力制御等では、６軸力覚センサーの６つの値が力覚値として出力されれば処理が可能なことが想定される。その場合、Ｎ個のユニットから出力される３×Ｎ個の値（ユニット出力値の集合である力覚値）を全て制御に用いようとしても処理が複雑化し、かえって好ましくない可能性もある。よって、通常の６軸力覚センサーとして機能すればよい状況においては、Ｆｘ，Ｆｙ，Ｆｚ及びＭｘ，Ｍｙ，Ｍｚを出力するものとして処理の簡略化を図るとよい。

また、力覚値補正部１２０は、ロボット１０の複数の姿勢において得られた複数の力覚値に基づいて補正処理を行ってもよい。

これにより、ロボット１０の姿勢を変化させ、それぞれの姿勢で得られた力覚値に基づいて、力覚値の補正処理を行うことが可能になる。エンドエフェクター１２による力覚センサー２０への影響はロボット１０の姿勢により変動することが想定されるため、複数の姿勢を用いて補正処理を行うことで、異なる状況を複合的に用いた補正処理が可能になる。つまり、複数の姿勢を用いればより精度の高い補正処理を行うことが可能になる。これは、図２２のフローチャートではステップＳ１０７やステップＳ１０５の処理に相当する。

また、本実施形態において上述してきたように、機械装置は、ロボット１０であってもよく、機械機構は、ロボット１０のエンドエフェクター１２であってもよい。その場合、力覚値補正部１２０は、ロボット１０のエンドエフェクター１２に対応して設けられ、Ｎ個の３軸力覚センサーユニットを有する力覚センサー２０から力覚値を取得して、取得した力覚値の補正処理を行い、制御部１１０は、補正処理後の力覚値に基づいて、ロボット１０及びエンドエフェクター１２の制御を行う。

ここで、ロボット１０のエンドエフェクター１２とは、ロボット１０のエンドポイントに取り付けられる機器のことであり、ハンドやツール等を含む概念である。なお、機械装置が本実施形態で説明したロボット１０以外の装置であってもよいことを考えれば、機械機構とはエンドエフェクター１２に限定されず、他の機構も含む概念となる。

これにより、機械装置としてロボット１０を用いることが可能になる。この場合、エンドエフェクター１２自身による力覚センサー２０への力を補正することができ、例えばロボット１０のエンドエフェクター１２にはたらく外力を用いた力制御（例えばインピーダンス制御）等が可能になる。

なお、本実施形態の制御システム等は、その処理の一部または大部分をプログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態の制御システム等が実現される。具体的には、情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そして、ＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち、情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

４．第２の実施形態
第２の実施形態では、図５（Ａ）、図５（Ｂ）に示したように、物理モデル化が困難なエンドエフェクター１２を用いるケースについて説明する。なお、本実施形態の手法は、複雑な解析処理により物理モデル化を試みるものではなく、物理モデル化が可能か否かをシステムにより自動的に検出し、不可と判定された場合に従来のリセット処理に切り替えるというものである。

システム構成例については第１の実施形態と同様に図１２で示されるため詳細な説明は省略する。本実施形態の補正処理を図２４のフローチャートを用いて説明する。

図２４から明らかなように、ステップＳ３０１〜ステップＳ３１０については図２２のステップＳ１０１〜ステップＳ１１０と同様であるため詳細な説明は省略する。つまり、本実施形態においても、エンドエフェクター１２の物理モデル化が可能であるか否かはまだわからないため、推定処理を試行することになる。推定処理の詳細については第１の実施形態と同様であるため詳細な説明は省略する。この際、推定処理が一回だけ、つまり取得される実行時パラメーターが１つだけでは、その実行時パラメーターにより表される物理モデルが適切か（当該物理モデルがエンドエフェクター１２を表すものとして広範な条件で用いることができるか）否かの判定をすることは困難である。よって実際には、複数回の推定処理を行い、そこから得られる複数の実行時パラメーターが所定誤差以内か否かを判定する（ステップＳ３１０）ことが想定される。このことから、本実施形態の処理では図２４のステップＳ３０８を省略し、ステップＳ３０７の処理後は必ずステップＳ３１０に移行するものとしてもよい。

ステップＳ３１０において、実行時パラメーターが所定誤差以内の場合には、推定した物理モデルによりエンドエフェクター１２が適切に表現されているということであるため、第１の実施形態と同様にその際の実行時パラメーターを記憶して（ステップＳ３０９）、物理モデルに基づく補正処理を行えばよい。

一方、ステップＳ３１０において実行時パラメーターが所定誤差よりも大きい場合には、図５（Ａ）、図５（Ｂ）のようにエンドエフェクター１２は物理モデル化が困難なものであるという判定をする。なぜなら、上述したように図５（Ａ）ではケーブルの伸び具合やケーブルと床等の摩擦、図５（Ｂ）ではＡ１とＡ２の相対的位置関係の変化等により、エンドエフェクター１２を表す物理モデルが頻繁に変化しうる。つまり、エンドエフェクター１２が物理モデル化が可能なものであれば、第１の実施形態で上述したように求められた実行時パラメーターはある程度の誤差内に収まることが想定されるのに対して、物理モデル化が困難なエンドエフェクター１２の場合には、仮にセンサー等に全く誤差がなかったとしても、状況次第で求められる実行時パラメーターが変化してしまう。このことから、求めた実行時パラメーターのばらつきが大きいのであれば、それはエンドエフェクター１２自体が物理モデル化に適していないものであると判定することになる。

つまり、ステップＳ３１０でＮｏの場合には、エンドエフェクター１２の物理モデル化は断念し、従来のリセット処理を行えばよい（ステップＳ３１１）。リセット処理の詳細を図２５のフローチャートに示す。リセット処理が開始されると、まず作業時と同様の姿勢となるように制御部１１０に指示する（ステップＳ４０１）。作業時と同じ姿勢をとり、且つ実際の作業時とは異なり力を加えないようにすれば、その際に力覚値として検出される値（オフセット値）は、エンドエフェクター１２に起因して力覚センサーにはたらく力を表すことになり、その値は作業時においても変化しないはずである。つまり、ロボット１０全体の姿勢が作業時と同一になっている必要はなく、オフセット値が作業時と十分に近い値となる姿勢となっていればよい。

そして、指示に従って制御部１１０により姿勢変更が行われたら、姿勢変更後の力覚値をオフセット値として記憶する（ステップＳ４０２）。そして、ステップＳ４０２の後、作業の終了時まで、力覚センサー２０から取得される力覚値に対して、オフセット値を除去するオフセット除去処理を行う（ステップＳ４０３）。

５．第３の実施形態
第３の実施形態では、図６（Ａ）〜図６（Ｃ）に示したようにエンドエフェクター１２が可変部を含む場合について説明する。本実施形態では、エンドエフェクター１２としてハンドにより把持されるツール（図１１（Ａ）の１２−２等）を考慮しなくてもよい場合、或いはツールを把持するがその把持位置が固定の場合等を想定しており、物理モデルが未知であったり、そもそも推定が不可であることはないものとする。つまり本実施形態では、エンドエフェクター１２の形状、質量等は既知であるものの、例えば図６（Ｂ）に示したような指関節角の変化等により、当該エンドエフェクター１２の物理モデルが変化する点を問題とする。

５．１ システム構成例
本実施形態のシステム構成例を図２６に示す。第１の実施形態に比べて、力覚値補正部１２０の第２の補正処理部１２３、実行時パラメーター算出部１２６及び実行時パラメーター記憶部１２７が除かれ、第３の補正処理部１２２及び可変部パラメーター記憶部１２５が追加された構成となっている。

固定部パラメーター記憶部１２４は、エンドエフェクター１２のうち固定部（図２７における手のひら部分１２−３等）の物理モデルに対応する固定部パラメーターを記憶する。そして、第１の補正処理部１２１は、固定部パラメーターと、姿勢情報取得部１３０からのロボット１０の姿勢情報とに基づいて、固定部についての力覚値補正処理を行う。

可変部パラメーター記憶部１２５は、エンドエフェクター１２のうち可変部（図２７における指部分１２−４等）の物理モデルに対応する可変部パラメーターを記憶する。そして、第３の補正処理部１２１は、可変部パラメーターと、姿勢情報取得部１３０からのロボット１０の姿勢情報、及びエンドエフェクター１２の姿勢情報とに基づいて、可変部についての力覚値補正処理を行う。

５．２ 可変部に関する補正処理の詳細
固定部の補正処理として第１の実施形態において上述したように、可変部についても質量中心位置を原点とする座標系１を設定した場合に、センサー座標系（座標系２）から座標系１への並進ベクトルｔ及び回転行列Ｒを求めることができれば、力覚値の補正処理が可能になる。

ここで、説明を簡略化するために、図２８（Ｂ）に示したように、図２８（Ａ）のうち力覚センサー２０と、１本の指構造のみを考える。さらに、図２９に示したように指構造を、指回転座標系の原点（図２８（Ｂ）のＣ１）において、Ｘ軸まわりに回転可能な構造とし、その回転角をθとする。つまりここでは、指構造の他の部分には関節はないものとする。

この場合、指構造自体の物理モデルは既知であるため、指回転座標系の原点から質量中心位置（図２８（Ｂ）のＣ２）までの並進ベクトルｔ２と、質量ｍはあらかじめ可変部パラメーターとして記憶されている。また、指回転座標系の原点Ｃ１は関節部分（つまり回転角θを規定する際の回転軸）であるため、センサー座標系の原点との相対位置は不変であり、センサー座標系の原点からＣ１への並進ベクトルｔ３も既知の可変部パラメーターとして記憶されている。また、センサー座標系と指回転座標系との間の回転は、指回転座標系がθの値に応じて回転することを考慮すれば未知であるが、θが所与の基準値である場合の座標系間のロール角・ピッチ角・ヨー角（ｕ，ｖ，ｗ）は既知であり、同様に可変部パラメーターとして記憶されている。

つまり、可変部の動きを表すパラメーターθさえ決まれば、θ、ｔ２、ｔ３，ｕ，ｖ，ｗにより、センサー座標系における質量中心位置を表す変位ベクトル（センサー座標系の原点から質量中心位置へのベクトル）を求めることができ、当該変位ベクトルは図１３等で示した並進ベクトルｔに他ならない。一例としては、並進ベクトルｔ２に対して、θ及びｕ，ｖ，ｗにより表される回転行列に基づいて座標変換を行いセンサー座標系におけるベクトルに変換し、変換後のベクトルとｔ３とのベクトルの和を求めることが考えられる。また、一般的には４×４の斉次行列の要素として求めてもよく、この場合並進及び回転の両方を行列により処理できるというメリットがある。

以上に示したように、可変部パラメーター（ｔ２、ｔ３，ｕ，ｖ，ｗ）とエンドエフェクター１２の姿勢情報（θ）から、センサー座標系を基準とした並進ベクトルｔ（質量中心位置ｘ，ｙ，ｚに相当）が求められ、また質量ｍについては既知の値をそのまま使うことができる。つまり、可変部についても固定部パラメーターに相当する値を算出することができるため、その後の処理は第１の実施形態において上述したように、上式（２）に基づいてＦ’及びＭ’を求め、求められたＦ’及びＭ’の値を力覚値から除去すればよい。このようにすることで、可変部による力覚値への影響を抑止することが可能になる。

以上の処理をまとめたものが図３０（Ａ）〜図３０（Ｃ）である。第１の実施形態で上述した固定部については、図３０（Ａ）に示したように、（ｘ，ｙ，ｚ，ｍ）が固定部パラメーターとして記憶されており、ロボット１０の姿勢情報に基づき回転行列Ｒが求められれば、固定部補正量を求めることができ、それにより補正処理が可能になる。それに対して可変部については、図３０（Ｂ）に示したように、可変部パラメーターとして一例としては上述したように（ｔ２、ｔ３，ｕ，ｖ，ｗ）が記憶されており、当該可変部パラメーターと、エンドエフェクター１２（狭義には可変部）の姿勢情報θとに基づいて（ｘ，ｙ，ｚ，ｍ）を求める。そして、求めた（ｘ，ｙ，ｚ，ｍ）と回転行列Ｒとから可変部補正量を求めて補正処理を行う。

また、第１の実施形態に示した推定処理を行う場合には、事前にパラメーターを記憶するのではなく、図３０（Ｃ）に示したように力覚値に基づいて（ｘ，ｙ，ｚ，ｍ）を推定することになる。推定結果が得られた後は、固定部と同様に回転行列Ｒから補正量を求めて補正処理を行うことになる。

５．３ 処理の詳細
本実施形態の補正処理を図３１のフローチャートを用いて説明する。この処理が開始されると、力覚センサー２０からの出力値（補正処理前の力覚値）を取得する（ステップＳ５０１）。そして取得した力覚値に対して固定部に対応する補正処理を行う（ステップＳ５０２）。

ステップＳ５０２での固定部の補正処理については第１の実施形態で説明した図２３と同様であるため、詳細な説明は省略する。次に可変部パラメーター（上記の例ではｔ２、ｔ３，ｕ，ｖ，ｗ）と指関節パラメーター（上記の例ではθ）に基づいて、補正用パラメーター（上記の例ではｔ＝（ｘ，ｙ，ｚ）とｍ）を求める（ステップＳ５０３）。そして求めた補正用パラメーターとロボット１０の姿勢に基づいて可変部補正量を算出する（ステップＳ５０４）。ステップＳ５０４の処理は上記の例では、ロボット１０の姿勢情報に基づいてＲを求め、上式（２）によりＦ’とＭ’を求める処理に相当する。

そして、ステップＳ５０２の出力である固定部補正処理後の力覚値に対して、ステップＳ５０４で求めた可変部補正量による補正処理を行い（ステップＳ５０５）、その結果を補正処理後の力覚値として出力する（ステップＳ５０６）。ステップＳ５０６での出力は固定部及び可変部の両方の補正処理が行われたものであるため、エンドエフェクター１２全体に関する補正処理が行われた力覚値となる。

以上の本実施形態では、制御システムは図２６に示したように、ロボット１０が有する可変部（例えば図２７の１２−４に示した指構造）の姿勢情報を取得する姿勢情報取得部１３０を含んでもよい。そして、力覚値補正部１２０は、可変部の姿勢情報に基づいて、可変部の質量中心位置を推定して補正処理を行う。

これにより、エンドエフェクター１２が可変部を含むことで、エンドエフェクター１２全体の物理モデルが変化するような場合においても、適切な補正処理を行うことが可能になる。具体的には、（ｔ２、ｔ３，ｕ，ｖ，ｗ）のような可変部パラメーターを保持しておき、当該可変部パラメーターと、可変部の姿勢情報（θ）とから、質量中心位置（ｘ，ｙ，ｚ）を推定する。質量中心位置及び質量が求められれば、その後は図３０（Ａ）、図３０（Ｂ）に示したように、固定部と同様の処理により補正が可能である。

６．第４の実施形態
第１〜第３の実施形態で説明した手法は、それぞれ独立に用いられるものに限定されず、複数を組み合わせてもよい。ここでは図４（Ａ）に示したように、可変の指構造を有するハンドによりツールが把持され、且つ当該ツールの把持位置に自由度がある場合、さらに把持対象ツールの候補として図５（Ａ）等に示した物理モデル化が困難なものも含まれる場合について考える。この場合には、エンドエフェクター１２が可変部を有するため、第３の実施形態で説明した手法を用い、さらに物理モデルが未知であるため第１の実施形態で説明した推定手法を用いる必要がある。さらに物理モデル化そのものが不可能な場合もあり得るため、推定の可否を判定する第２の実施形態の手法も用いるとよい。

６．１ システム構成例
本実施形態のシステム構成例を図３２に示す。図３２に示したように、制御システムの力覚値補正部１２０は、図１２及び図２６に示した各部を含む。なお、図２６には不図示であるが、力覚値補正部１２０は、実行時パラメーター算出部１２６での算出結果に基づいて、制御部１１０に対して姿勢の変更を指示する姿勢変更指示部を含んでもよい。姿勢変更指示部は、第１の実施形態において上述した図１２の力覚値補正部１２０に含まれてもよいものである。

第１の補正処理部１２１は、第３の補正処理部１２２に接続される。第３の補正処理部１２２は、第２の補正処理部１２３と、実行時パラメーター算出部１２６に接続される。実行時パラメーター算出部１２６は、実行時パラメーター記憶部１２７に接続される。

力覚値補正部１２０が姿勢変更指示部を含む場合、当該姿勢変更指示部は、実行時パラメーター算出部１２６での算出処理の結果に基づいて、制御部１１０に対してロボット１０の姿勢変更を指示する。具体的には、推定された交点（質量中心位置）が不適切と判定された場合、或いは算出した実行時パラメーター（推定された交点を含む）とは異なる姿勢での実行時パラメーターを求めたい場合等に、姿勢変更指示部は制御部１１０に対して姿勢変更を指示することになる。

６．２ 処理の詳細
本実施形態の補正処理を図３３のフローチャートを用いて説明する。この処理が開始されると、力覚センサー２０からの出力値（補正処理前の力覚値）を取得する（ステップＳ６０１）。そして取得した力覚値に対して固定部及び可変部に対応する補正処理を行う（Ｓ６０２）。ステップＳ６０２での固定部の補正処理については第１の実施形態で説明した図２３と同様であるため、詳細な説明は省略する。また、可変部の補正処理については、第３の実施形態で説明した図３１のステップＳ５０３〜ステップＳ５０６と同様である。

その後のステップＳ６０３〜ステップＳ６１１については、第２の実施形態で説明した図２４のステップＳ３０３〜ステップＳ３１１と同様である。つまり、第１〜第３の実施形態を組み合わせた本実施形態の処理手順の一例としては、まず固定部・可変部に対する補正処理を行い、その補正処理後の力覚値に基づいて推定処理を行う。そして推定処理の結果からエンドエフェクター１２の物理モデル化の可否を判定し、物理モデル化が可能であれば推定結果を用いて補正処理を行う。一方、物理モデル化が不可であれば物理モデルを用いた補正処理をあきらめ、リセット処理により補正処理を行うことになる。

以上、本発明を適用した４つの実施の形態１〜４およびその変形例について説明したが、本発明は、各実施の形態１〜４やその変形例そのままに限定されるものではなく、実施段階では、発明の要旨を逸脱しない範囲内で構成要素を変形して具体化することができる。また、上記した各実施の形態１〜４や変形例に開示されている複数の構成要素を適宜組み合わせることによって、種々の発明を形成することができる。例えば、各実施の形態１〜４や変形例に記載した全構成要素からいくつかの構成要素を削除してもよい。さらに、異なる実施の形態や変形例で説明した構成要素を適宜組み合わせてもよい。また、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。このように、発明の主旨を逸脱しない範囲内において種々の変形や応用が可能である。

Ｆｘ，Ｆｙ，Ｆｚ 並進力、Ｍｘ，Ｍｙ，Ｍｚ モーメント、Ｒ 回転行列、
ｔ 並進ベクトル、１０ ロボット、１２ エンドエフェクター、
２０ 力覚センサー、２１−１〜２１−４ ３軸力覚センサーユニット、
１００ 制御装置、１１０ 制御部、１２０ 力覚値補正部、
１２１ 第１の補正処理部、１２２ 第３の補正処理部、１２３ 第２の補正処理部、
１２４ 固定部パラメーター記憶部、１２５ 可変部パラメーター記憶部、
１２６ 実行時パラメーター算出部、１２７ 実行時パラメーター記憶部、
１３０ 姿勢情報取得部

Claims (14)

1. 機械機構と、
   Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有し、Ｎ個の前記３軸力覚センサーユニットの各３軸力覚センサーユニットから、前記機械機構に起因する値が付加されたユニット出力値を取得し、前記ユニット出力値に基づく力覚値を出力する力覚センサーと、
   前記力覚センサーが出力した前記力覚値に基づいて、前記力覚値を補正する力覚値補正部と、
   前記力覚値補正部において補正された前記力覚値に基づいて、前記機械機構を含む機械装置の制御を行う制御部と、
   を含むことを特徴とする制御システム。
2. 請求項１において、
   前記力覚センサーは、
   Ｎ個の前記３軸力覚センサーユニットの各３軸力覚センサーユニットから出力された前記ユニット出力値を含む情報を、前記力覚値として出力し、
   前記力覚値補正部は、
   前記力覚値に含まれる前記ユニット出力値に基づいて、前記機械機構の質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行うことを特徴とする制御システム。
3. 請求項２において、
   前記力覚値補正部は、
   前記力覚値に含まれるユニット出力値から、Ｎ個の前記３軸力覚センサーユニットのそれぞれが検出した力に対応するＮ個の力ベクトルを求め、求めたＮ個の前記力ベクトルの合成ベクトルの大きさに基づいて、前記機械機構の質量を推定し、推定した前記質量に基づいて、前記補正処理を行うことを特徴とする制御システム。
4. 請求項１乃至３のいずれかにおいて、
   前記力覚センサーは、
   ３個以上の前記３軸力覚センサーユニットを有することを特徴とする制御システム。
5. 請求項１乃至３のいずれかにおいて、
   前記力覚センサーは、
   ４個以上の前記３軸力覚センサーユニットを有することを特徴とする制御システム。
6. 請求項１乃至３のいずれかにおいて、
   前記力覚センサーは、
   少なくとも２個の前記３軸力覚センサーユニットを有し、
   前記力覚値補正部は、
   前記機械装置の第１の姿勢において、２個の前記３軸力覚センサーユニットからの前記ユニット出力値に基づき前記力覚センサーから出力された第１の力覚値に基づいて、前記機械機構の前記質量中心位置を推定する処理を行い、
   前記制御部は、
   前記力覚値補正部により前記質量中心位置の推定が不可と判定された場合には、前記機械装置の姿勢を前記第１の姿勢とは異なる第２の姿勢に変更する制御を行い、
   前記力覚値補正部は、
   前記制御部により前記機械装置が前記第２の姿勢となる制御が行われた後に、２個の前記３軸力覚センサーユニットからの前記ユニット出力値に基づき前記力覚センサーから出力された第２の力覚値に基づいて、前記機械機構の前記質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行うことを特徴とする制御システム。
7. 請求項２乃至６のいずれかにおいて、
   前記力覚値補正部は、
   前記機械機構の前記質量中心位置の最小自乗解を正規化方程式により求めることで、前記質量中心位置を推定することを特徴とする制御システム。
8. 請求項７において、
   前記力覚値補正部は、
   前記正規化方程式に対して特異値分解を行い、前記特異値分解により得られる特異値の数に基づいて、前記質量中心位置の推定が可能か否かの判定を行うことを特徴とする制御システム。
9. 請求項１乃至８のいずれかにおいて、
   前記力覚センサーは、
   Ｎ個の前記３軸力覚センサーユニットの各３軸力覚センサーユニットから、前記ユニット出力値として３つの値を取得することで、３×Ｎ個の値を取得し、
   取得した３×Ｎ個の値に基づいて、Ｘ軸、Ｙ軸及びＺ軸の並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚを算出し、
   算出したＦｘ，Ｆｙ，Ｆｚ及びＭｘ，Ｍｙ，Ｍｚを含む情報を前記力覚値として出力する６軸力覚センサーであることを特徴とする制御システム。
10. 請求項１乃至９のいずれかにおいて、
    前記力覚値補正部は、
    前記機械装置の複数の姿勢において得られた複数の前記力覚値に基づいて、前記補正処理を行うことを特徴とする制御システム。
11. 請求項１乃至１０のいずれかにおいて、
    前記機械機構が有する可変部の姿勢情報を取得する姿勢情報取得部を含み、
    前記力覚値補正部は、
    前記可変部の前記姿勢情報に基づいて、前記可変部の前記質量中心位置を推定して前記補正処理を行うことを特徴とする制御システム。
12. 請求項１乃至１１のいずれかにおいて、
    前記機械装置は、ロボットであり、
    前記機械機構は、前記ロボットのエンドエフェクターであり、
    前記力覚値補正部は、
    Ｎ個の前記３軸力覚センサーユニットを有し、前記ロボットの前記エンドエフェクターに対応して設けられる前記力覚センサーから、前記力覚値を取得して、取得した前記力覚値の補正処理を行い、
    前記制御部は、
    前記補正処理後の前記力覚値に基づいて、前記ロボットおよび前記エンドエフェクターの制御を行うことを特徴とする制御システム。
13. 機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、
    前記補正処理後の前記力覚値に基づいて、前記機械装置および前記機械機構の制御を行う制御部として、
    コンピューターを機能させ、
    前記力覚センサーは、
    Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有し、
    前記力覚値補正部は、
    Ｎ個の前記３軸力覚センサーユニットのユニット出力値に基づき前記力覚センサーからから出力された前記力覚値に基づいて、前記補正処理を行うことを特徴とするプログラム。
14. 機械装置の機械機構に対応して設けられ、Ｎ（Ｎは２以上の整数）個の３軸力覚センサーユニットを有する力覚センサーから力覚値を取得し、
    取得した前記力覚値に対して補正処理を行い、
    前記補正処理後の前記力覚値に基づいて、前記機械装置および前記機械機構の制御を行い、
    前記補正処理として、Ｎ個の前記３軸力覚センサーユニットのユニット出力値に基づき前記力覚センサーからから出力された前記力覚値に基づいて、前記力覚値を補正する処理を行うことを特徴とする機械装置の制御方法。