JP6003312B2 - ロボットシステム - Google Patents

Description

本発明は、ロボットシステム等に関係する。

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

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

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

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

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

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

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

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

本発明の一態様は、機械装置の制御システムであって、前記機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、前記補正処理後の前記力覚値に基づいて、前記機械装置の制御を行う制御部と、を含み、前記力覚値補正部は、前記機械装置の複数の姿勢において得られた複数の前記力覚値に基づいて、前記補正処理を行う制御システムに関係する。

本発明の一態様では、機械機構に対応して設けられた力覚センサーから力覚値を取得し、当該力覚値に補正処理を行った結果を用いて機械装置を制御する。この際、機械装置に複数の姿勢をとらせ、各姿勢において得られた力覚値に基づいて補正処理を行うため、望ましい補正処理（例えば高精度の補正処理）等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記機械装置の第１〜第Ｎ（Ｎは２以上の整数）の姿勢において得られた第１〜第Ｎの力覚値に基づいて、前記機械機構の質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行ってもよい。

これにより、複数の姿勢で取得された力覚値に基づいて、機械機構の質量中心位置を推定することが可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記機械機構の質量、前記質量中心位置、及び前記機械装置の前記姿勢に基づいて、前記機械機構により前記力覚センサーに作用する前記力覚値を推定することで、前記補正処理を行ってもよい。

これにより、推定された質量中心位置（広義には機械機構の物理モデル）に基づいて、機械機構による力覚センサーへの影響を解析、補正すること等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記機械装置の第１の姿勢において得られた第１の力覚値に基づいて、前記機械機構の前記質量中心位置を推定する処理を行い、前記制御部は、前記力覚値補正部により前記質量中心位置の推定が不可と判定された場合には、前記機械装置の姿勢を前記第１の姿勢とは異なる第２の姿勢に変更する制御を行い、前記力覚値補正部は、前記制御部により前記機械装置が第２の姿勢となる制御が行われた後に取得された第２の力覚値に基づいて、前記機械機構の前記質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行ってもよい。

これにより、第１の姿勢での力覚値では質量中心位置の推定処理が不可能と判定された場合に、姿勢を変更した上で推定処理を再試行すること等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、推定した前記質量中心位置が所定誤差以内ではないと判定された場合には、エラー処理を実行してもよい。

これにより、質量中心位置の推定精度を判定し、精度が低いと判定された場合にエラー処理により対応すること等が可能になる。

また、本発明の一態様では、前記力覚値補正部により推定された前記質量中心位置が所定誤差以内ではないと判定された場合には、前記制御部は、前記機械装置の姿勢を所与の姿勢とする制御を行い、前記力覚値補正部は、前記制御部により前記機械装置が前記所与の姿勢となる制御が行われた後の前記力覚値を取得し、取得した前記力覚値に対応する値をオフセット値として設定し、設定した前記オフセット値によるオフセット除去処理を、前記エラー処理として実行してもよい。

これにより、質量中心位置の推定精度が低いと判定された場合に、オフセット除去処理に移行することで、適切に補正処理を行うこと等が可能になる。

また、本発明の一態様では、前記力覚センサーが、Ｘ軸、Ｙ軸及びＺ軸の並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚを取得する６軸力覚センサーである場合に、前記力覚値補正部は、Ｆｘ，Ｆｙ及びＭｚにより決定される第１の平面と、Ｆｙ，Ｆｚ及びＭｘにより決定される第２の平面と、Ｆｚ，Ｆｘ及びＭｙにより決定される第３の平面を求め、前記第１〜第３の平面の交点を前記質量中心位置として推定してもよい。

これにより、力覚センサーとして６軸力覚センサーを用いる場合に、各力覚値から特定される平面の交点として質量中心位置を推定すること等が可能になる。

また、本発明の一態様では、前記第１の平面のｘ切片をＡ１，ｙ切片をＢ１、ｚ切片をＣ１とし、前記第２の平面のｘ切片をＡ２，ｙ切片をＢ２、ｚ切片をＣ２とし、前記第３の平面のｘ切片をＡ３，ｙ切片をＢ３、ｚ切片をＣ３として、行列Ｍ及びベクトルｑを

とした場合に、前記力覚値補正部は、前記質量中心位置を表す３次元ベクトルｐをＭｐ＝ｑに従って求めることで、前記質量中心位置を推定してもよい。

これにより、質量中心位置を行列及びベクトルからなる数式の解として求めることが可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記行列Ｍが正則でない場合に、前記質量中心位置の推定が不可であると判定してもよい。

これにより、質量中心位置の推定の可否を、行列Ｍが正則か否かに基づいて判定することが可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記質量中心位置及び、前記機械機構の質量を含む情報を実行時パラメーターとして算出する実行時パラメーター算出部と、算出された前記実行時パラメーターを記憶する実行時パラメーター記憶部と、を含んでもよい。

これにより、質量中心位置等を実行時パラメーターとして算出し、算出した実行時パラメーターを記憶すること等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記実行時パラメーター記憶部に記憶された前記実行時パラメーターを読み出し、前記実行時パラメーターと前記機械装置の姿勢とに基づいて、前記機械機構が前記力覚センサーに及ぼす力をオフセット値として算出し、算出した前記オフセット値により前記力覚値の前記補正処理を行ってもよい。

これにより、記憶した実行時パラメーターと姿勢情報とに基づいて、オフセット値（補正量）を求め、求めたオフセット値による補正処理を行うこと等が可能になる。

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

これにより、機械機構が可変部を有する場合にも、適切な補正処理を行うこと等が可能になる。

また、本発明の他の態様は、機械装置の制御システムであって、前記機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、前記補正処理後の前記力覚値に基づいて、前記機械装置の制御を行う制御部と、前記機械機構が有する可変部の姿勢情報を取得する姿勢情報取得部と、を含み、前記力覚値補正部は、前記可変部の前記姿勢情報に基づいて、前記可変部の質量中心位置を推定して前記補正処理を行う制御システムに関係する。

本発明の他の態様では、機械機構が可変部を有する場合にも、適切な補正処理を行うこと等が可能になる。

また、本発明の一態様では、前記姿勢情報取得部は、前記機械装置の前記姿勢情報を取得し、前記力覚値補正部は、前記機械機構に含まれる固定部の前記質量中心位置と、前記機械装置の前記姿勢情報に基づく第１の補正処理と、前記可変部の前記姿勢情報に基づいて推定された前記可変部の前記質量中心位置と、前記機械装置の前記姿勢情報に基づく第２の補正処理を、前記補正処理として実行してもよい。

これにより、機械機構を固定部と可変部に分け、それぞれについて補正処理を行うことで、機械機構全体の補正処理を行うこと等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記固定部の前記質量中心位置と、前記固定部の質量を含む情報を固定部パラメーターとして記憶する固定部パラメーター記憶部と、前記可変部に対応して設定された座標系における前記可変部の前記質量中心位置と、前記可変部の前記質量を含む情報を可変部パラメーターとして記憶する可変部パラメーター記憶部と、を含んでもよい。

これにより、固定部に関する固定部パラメーターと、可変部に関する可変部パラメーターをそれぞれ記憶し、記憶したパラメーターに基づく補正処理を行うこと等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記固定部パラメーター記憶部に記憶された前記固定部パラメーターを読み出し、前記固定部パラメーターと前記機械装置の前記姿勢情報とに基づいて、前記固定部が前記力覚センサーに及ぼす力をオフセット値として算出し、算出した前記オフセット値により前記第１の補正処理を行ってもよい。

これにより、固定部パラメーターと、機械装置の姿勢情報に基づいて、固定部に関する補正処理を行うこと等が可能になる。

また、本発明の一態様では、前記力覚値補正部は、前記可変部パラメーター記憶部に記憶された前記可変部パラメーターを読み出し、前記可変部パラメーター、前記可変部の前記姿勢情報、及び前記機械装置の前記姿勢情報に基づいて、前記可変部が前記力覚センサーに及ぼす力をオフセット値として算出し、算出した前記オフセット値により前記第２の補正処理を行ってもよい。

これにより、可変部パラメーター、機械機構（狭義には可変部）の姿勢情報、及び機械装置の姿勢情報に基づいて、可変部に関する補正処理を行うこと等が可能になる。

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

これにより、機械装置としてロボットを用いること等が可能になる。

また、本発明の一態様では、前記機械装置は、ロボットであり、前記機械機構は、前記ロボットのエンドエフェクターであり、前記可変部は、前記エンドエフェクターを構成するパーツであり、前記姿勢情報取得部は、前記エンドエフェクターの前記パーツである前記可変部の前記姿勢情報を取得し、前記力覚値補正部は、前記ロボットの前記エンドエフェクターに対応して設けられる前記力覚センサーから力覚値を取得し、前記可変部の前記姿勢情報に基づいて、前記可変部の前記質量中心位置を推定して前記力覚値の補正処理を行い、前記制御部は、前記補正処理後の前記力覚値に基づいて、前記ロボットの制御を行ってもよい。

これにより、機械装置としてロボットを用いること等が可能になる。

また、本発明の他の態様は、機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、前記補正処理後の前記力覚値に基づいて、前記機械装置の制御を行う制御部として、コンピューターを機能させ、前記力覚値補正部は、前記機械機構の複数の姿勢において得られた複数の前記力覚値に基づいて、前記補正処理を行うプログラムに関係する。

本発明の他の態様では、機械装置の複数の姿勢において得られた力覚値に基づいて補正処理を行い、補正処理後の力覚値に基づく機械装置の制御を行うことで、より望ましい（例えば高精度の）制御を行うプログラムを実現すること等が可能になる。

また、本発明の他の態様は、機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、前記補正処理後の前記力覚値に基づいて、前記機械装置の制御を行う制御部と、前記機械機構が有する可変部の姿勢情報を取得する姿勢情報取得部として、コンピューターを機能させ、前記力覚値補正部は、前記可変部の前記姿勢情報に基づいて、前記可変部の質量中心位置を推定して前記補正処理を行うプログラムに関係する。

本発明の他の態様では、機械機構が可変部を有する場合にも、適切な補正処理を行い、補正処理の結果に基づいて機械装置の制御を行うプログラムを実現すること等が可能になる。

また、本発明の他の態様は、機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行い、前記補正処理後の前記力覚値に基づいて、前記機械装置の制御を行い、前記補正処理として、前記機械機構の複数の姿勢において得られた複数の前記力覚値に基づいて、前記力覚値を補正する処理を行う機械装置の制御方法に関係する。

本発明の他の態様では、機械装置の複数の姿勢において得られた力覚値に基づいて補正処理を行い、補正処理後の力覚値に基づく機械装置の制御を行うことで、より望ましい（例えば高精度の）制御を行う制御方法を実現すること等が可能になる。

また、本発明の他の態様は、機械装置の機械機構に対応して設けられる力覚センサーから力覚値を取得し、前記機械機構が有する可変部の姿勢情報を取得し、前記力覚値と前記可変部の前記姿勢情報に基づいて、前記力覚値の補正処理を行い、前記補正処理後の前記力覚値に基づいて、前記機械装置の制御を行い、前記補正処理として、前記可変部の前記姿勢情報に基づいて、前記可変部の質量中心位置を推定して、前記力覚値を補正する処理を行う機械装置の制御方法に関係する。

本発明の他の態様では、機械機構が可変部を有する場合にも、適切な補正処理を行い、補正処理の結果に基づいて機械装置の制御を行う制御方法を実現すること等が可能になる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

力覚センサー２０は、センシング部にはたらく力を検出するセンサーである。力覚センサー２０には種々の構成が考えられるが、基本的には外力によるセンシング部の微小な変形を、何らかの手法により測定することで外力を検知する。変形の測定手法には、歪みゲージを用いる手法、静電センサーを用いる手法、光学センサーを用いる手法、水晶素子等の圧電素子を用いる手法等が考えられる。本実施形態においては、力覚センサー２０として、図９に示したように３軸の並進力及び各軸まわりのモーメントを検出する６軸力覚センサーを用いる。

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

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

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

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

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

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

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

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

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

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

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

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

２．３ 物理モデルの推定処理
次に本実施形態における物理モデルの推定処理について説明する。以下の説明では、物理モデルとは質量中心位置（力の作用点に相当）を表す（ｘ，ｙ，ｚ）と、質量ｍにより表されるモデルとする。

２．３．１ 基本的な手法
本実施形態の力覚センサー２０は一般的に用いられる６軸力覚センサーであり、Ｘ軸Ｙ軸Ｚ軸のそれぞれの並進力であるＦｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントであるＭｘ，Ｍｙ，Ｍｚを力覚値として取得する。エンドエフェクター１２により力を加えるような作業をしていなければ、力覚値はエンドエフェクター１２に起因する力（狭義にはエンドエフェクター１２にはたらく重力に対応する力）となり、本実施形態ではこの状態においてエンドエフェクター１２の物理モデルを推定するものとする。

具体的には、力覚センサー２０からの力覚値に基づいて、力の作用点を推定することになり、ここでは各平面への射影を用いて考えるものとする。まず、図１４に示したようにＸＹ平面における力を考える。力覚センサー２０からは、Ｘ軸方向の並進力Ｆｘと、Ｙ軸方向の並進力Ｆｙと、ＸＹ平面を垂直に貫くＺ軸まわりのモーメントＭｚの値が取得されている。この場合、図１４から明らかなように下式（２）が成り立つ。ここでＦはエンドエフェクター１２にはたらく力をＸＹ平面へ射影した力の大きさに対応し、ｒは原点から力の作用線までの距離に対応する。θは図１４に示した角度であり、力の作用線とＹ軸とのなす角度に対応する。

Ｆｘ＝−Ｆｓｉｎθ
Ｆｙ＝−Ｆｃｏｓθ
Ｍｚ＝−ｒ×Ｆ ・・・・・（２）
上式（２）を変形することで下式（３）が得られる。

上述したようにＦｘ，Ｆｙ，Ｍｚは力覚値として力覚センサー２０から取得されるため、Ｆ，ｒ，θは上式（３）に基づき求めることができる。しかし、力の大きさ（Ｆ）とその方向（ｒとθにより決定される）は定まる、つまり力の作用線を決定することはできるが、力の作用点までは決定できない。

これは３次元空間に拡張して考えれば、Ｆｘ，Ｆｙ，Ｍｚに基づいて、図１５に示すように力の作用点（質量中心位置）を含む１つの平面を決定することができるが、当該平面のどこに作用点があるかまでは決定できないということになる。

しかし、Ｆｙ，Ｆｚ，Ｍｘからも図１６（Ａ）に示したように１つの平面が決定され、同様にＦｚ，Ｆｘ，Ｍｙからも図１６（Ｂ）に示したように１つの平面が決定される。各平面は質量中心位置を含むという条件で求められているものであるから、３つの平面の共通部分、つまり３平面の交点を質量中心位置として求めることが可能である。図１６（Ｂ）の例であれば、Ｂ１に示した点が力の作用点、つまり質量中心位置となる。また、その際の力の大きさ（質量に相当）は、各平面での力ベクトルを合成したベクトルの大きさとすればよい。ここで力ベクトルとは、図１５等に示した平面を決定する際に用いられた力の大きさ（上式（３）のＦ）、及び力の方向（上式（３）のｒ及びθ）により決定されるベクトルのことである。

つまり、Ｆｘ，Ｆｙ，Ｍｚから決定されたＸＹ平面での力ベクトル（図１６（Ｂ）のＢ２）と、Ｆｙ，Ｆｚ，Ｍｘから決定されたＹＺ平面での力ベクトル（図１６（Ｂ）のＢ３）と、Ｆｚ，Ｆｘ，Ｍｙから決定されたＺＸ平面での力ベクトル（図１６（Ｂ）のＢ４）を合成したベクトルの大きさが求める質量ｍに相当する。

以上の処理により、未知であった物理モデル（ｘ，ｙ，ｚ，ｍ）を推定することができる。物理モデルが推定された後は、上述した固定部についての補正手法と同様に当該物理モデルとロボット１０の姿勢情報を用いて上式（１）からＦ’とＭ’を求め、力覚値の補正処理を行えばよい。

なお、平面を表す一般的な式は、Ｘ軸，Ｙ軸，Ｚ軸の各切片をＡ，Ｂ，Ｃとすれば下式（４）、及びそれを変形した下式（５）となる。よって、第１の平面の切片をＡ１，Ｂ１，Ｃ１とし、第２の平面の切片をＡ２，Ｂ２，Ｃ２とし、第３の平面の切片をＡ３，Ｂ３，Ｃ３とすれば、質量中心位置（ｘ，ｙ，ｚ）は下式（６）の連立方程式の解となり、下式（６）を行列を用いた式で表現した下式（７）が成り立つことになる。

つまり、質量中心位置は上式（７）を変形した下式（８）から求めることができる。

２．３．２ 変形例
上記処理（例えば上式（８）を解く処理）により、質量中心位置を求めることができる。しかし、外力の方向が座標系（具体的には力覚センサーに設定されたセンサー座標系）の原点方向を向いているときには、質量中心位置を求めることができない。

具体的には、図１７（Ａ）に示した状況であり、モーメントＭｚの値が０となる。つまり力覚センサーから得られる情報量が少なくなり、適切な処理ができなくなる。これは図１７（Ｂ）に示したように３軸のうちのいずれかが力の方向と一致するように、座標系を変換すると理解しやすい。図１７（Ｂ）に示したように、座標系を構成するＸ’軸、Ｙ’軸、Ｚ’軸のうち、Ｙ’軸と力の方向が一致する場合、Ｘ’Ｚ’平面では、並進力Ｆｘ’、Ｆｚ’は両方０になり、Ｙ’軸まわりのモーメントＭｙ’も０となる。つまり、この平面において力覚値はなんら情報を有することはない。よって、実質的に情報を含む面は２面しかなく、図１６（Ｂ）に示したように３つの平面の交点として求められる質量中心位置の推定はできない。

これは、上式（７）を考えれば明らかであり、力の方向が座標系の原点方向である場合には、行列Ｍのランクが下がることになり、逆行列Ｍ^−１を求めることができない（Ｍが正則でない）。つまり、上式（８）から質量中心位置を求めることができないということになる。

以上のことから、本実施形態では一回の推定処理により質量中心位置を決定するのではなく、ロボット１０の姿勢を変えて（エンドエフェクター１２によりはたらく力の、センサー座標系における方向が変化するように姿勢を変えて）複数回の推定処理を行うとよい。姿勢変更は例えば、図１８（Ａ）の状態から図１８（Ｂ）の状態への変更により行われる。

具体的には、質量中心位置の推定処理ができるか否かの判定を行い、推定できないと判定された場合には、姿勢を変更する。判定は上述したように、上式（７）で示した行列Ｍが正則であるか否かに基づいて行えばよい。そして、変更後の姿勢により再度質量中心位置の推定を行う。このようにすれば、質量中心位置の推定ができない場合にも適切に対応できる。推定精度を問題にしないのであれば、これにより質量中心位置を求めることができないという事態は回避できる。

ただし、力の方向が座標系の原点方向と完全に一致しない場合であっても、原点に近い位置を通る場合にはモーメントの値が極端に小さくなる。そのため、ノイズ等の影響が大きくなり、上式（８）を解いて一応の解を求めることはできるが、その精度に問題が残る。また、センサー等のノイズが非常に大きい場合等では、姿勢にかかわらず推定精度が低くなる可能性はある。

よって推定精度の向上を考慮するのであれば、推定不可と判定された場合に姿勢を変えてやり直すだけでなく、推定可能と判定され実際に質量中心位置を推定できた場合にも、姿勢変更及び再度の推定処理を行うとよい。例えば、少なくともｋ（ｋは２以上の整数）個の推定値を取得して、それらの平均等を最終的な質量中心位置とすればよい。このようにすれば、推定精度の向上が期待できる。

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

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

固定部に関する補正処理後の力覚値（ステップＳ１０２の処理の出力値）が得られたら、その値に基づいて上式（７）を求める（ステップＳ１０３）。具体的には上述したように、３軸の並進力及び各軸まわりのモーメントから、３つの平面を求め、それぞれの切片の値から行列Ｍ及びベクトルｑを決めればよい。

行列Ｍが求められたら、Ｍが正則であるか否かの判定を行う（ステップＳ１０４）。上述したように、Ｍが正則でない場合とは、エンドエフェクター１２のよる力の方向がセンサー座標系の原点方向に一致する場合であり、質量中心位置の推定ができない場合に対応する。よって、Ｍが正則でない場合（ステップＳ１０４でＮｏの場合）には、制御部１１０に対して姿勢変更を指示してステップＳ１０１に戻る。つまり、姿勢を変更した上で力覚値の取得からやり直すことになる。

一方、Ｍが正則の場合（ステップＳ１０４でＹｅｓの場合）には、Ｍの逆行列を求められるということであるから、上式（８）に従って質量中心位置を求めるとともに、図１６（Ｂ）に示したように各平面上の力ベクトル（Ｂ２〜Ｂ４）の合成ベクトルの大きさとして質量ｍを求め、（ｘ，ｙ，ｚ，ｍ）を実行時パラメーターとする（ステップＳ１０６）。

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

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

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

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

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

以上の本実施形態では、制御システム１００とは、ロボット（機械装置）１０の制御システムであり、図８に示したようにロボット１０のエンドエフェクター（機械機構）１２に対応して設けられる力覚センサー２０から力覚値を取得して、力覚値の補正処理を行う力覚値補正部１２０と、補正処理後の力覚値に基づいて、ロボット１０の制御を行う制御部１１０を含む。そして力覚値補正部１２０は、ロボット１０の複数の姿勢において得られた複数の力覚値に基づいて、補正処理を行う。

これにより、ロボット１０の姿勢を変化させ、それぞれの姿勢で得られた力覚値に基づいて、力覚値の補正処理を行うことが可能になる。上述したように、力覚センサー２０からの力覚値には、ロボット１０の制御においては不要となる力に起因する要素も含まれ、具体的には力覚センサー２０よりも先に設けられたエンドエフェクター１２によるものが考えられる。ここで、エンドエフェクター１２による力覚センサー２０への影響はロボット１０の姿勢により変動することが想定されるため、複数の姿勢を用いて補正処理を行うことで、異なる状況を複合的に用いた補正処理が可能になる。つまり、ロボット１０の適正な制御のためには力覚値の補正処理が必要なところ、複数の姿勢を用いればより精度の高い補正処理を行うことが可能になる。この点から考えれば、本実施形態での複数の姿勢とは、エンドエフェクター１２による力覚センサー２０への影響が異なる複数の姿勢であることが望ましく、例えば図１８（Ａ）と図１８（Ｂ）のように、ロボットの手首部分を回す等の変化をつけるとよい。

また、力覚値補正部１２０は、ロボット１０の第１〜第Ｎ（Ｎは２以上の整数）の姿勢において得られた第１〜第Ｎの力覚値に基づいて、エンドエフェクター１２の質量中心位置を推定し、推定した質量中心位置に基づいて、補正処理を行ってもよい。具体的には、力覚値補正部１２０は、エンドエフェクター１２の質量、質量中心位置、及びロボット１０の姿勢に基づいて、エンドエフェクター１２により力覚センサー２０に作用する力覚値を推定することで、補正処理を行ってもよい。

これにより、エンドエフェクター１２の物理モデルとして質量中心位置（ｘ，ｙ，ｚ）を用いることが可能になる。エンドエフェクター１２を質量中心位置にある質量ｍの物体であるとモデル化できれば、当該エンドエフェクター１２による力覚センサー２０への影響は、図１０及び上式（１）において説明したように、力学的に解析可能となる。上式（１）では、質量中心位置が並進ベクトルｔに対応し、ロボット１０の姿勢から回転Ｒが求められることになる。

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

これにより、適切な質量中心位置の推定が可能になる。第１の姿勢とは図１７（Ａ）に示すように、エンドエフェクター１２によりはたらく力の方向がセンサー座標系の原点を向いている場合であり、この場合上述したようにＭが正則でないため、上式（８）を解くことができない。これは、エンドエフェクター１２によりはたらく力の方向がセンサー座標系の原点を向いているという状況さえ回避すればよいため、ロボット１０の姿勢を変更することで解決可能である。なお、上述したように、本実施形態における姿勢変更は推定ができない状況に限定して行われるものではなく、精度の向上を目的として必ず行われるものであってもよい。

また、力覚センサー２０が、図９に示したようにＸ軸、Ｙ軸及びＺ軸の並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚを取得する６軸力覚センサーである場合に、力覚値補正部１２０は、Ｆｘ，Ｆｙ及びＭｚにより決定される第１の平面と、Ｆｙ，Ｆｚ及びＭｘにより決定される第２の平面と、Ｆｚ，Ｆｘ及びＭｙにより決定される第３の平面を求め、第１〜第３の平面の交点を質量中心位置として推定してもよい。具体的には、第１の平面のｘ切片をＡ１，ｙ切片をＢ１、ｚ切片をＣ１とし、第２の平面のｘ切片をＡ２，ｙ切片をＢ２、ｚ切片をＣ２とし、第３の平面のｘ切片をＡ３，ｙ切片をＢ３、ｚ切片をＣ３として、行列Ｍ及びベクトルｑを上式（７）に示したものとした場合に、力覚値補正部１２０は、質量中心位置を表す３次元ベクトルｐをＭｐ＝ｑに従って求めることで、質量中心位置を推定してもよい。

これにより、図１５〜図１６（Ｂ）、及び上式（６）〜（８）に示した手法により、質量中心位置を推定することが可能になる。

また、力覚値補正部１２０は、行列Ｍが正則でない場合に、質量中心位置の推定が不可であると判定してもよい。

これにより、図１７（Ａ）に示したように質量中心位置の推定ができない状況であるかを、Ｍが正則か否かに基づいて判定することが可能になる。推定不可とされた場合には、上述したように姿勢変更を行えばよく、こうすることで推定値（実行時パラメーター）がまったく得られないという状況を回避することができる。

また、力覚値補正部１２０は、図８に示したように質量中心位置及び、エンドエフェクター１２の質量を含む情報を実行時パラメーターとして算出する実行時パラメーター算出部１２６と、算出された実行時パラメーターを記憶する実行時パラメーター記憶部１２７を含んでもよい。具体的には、力覚値補正部１２０は、実行時パラメーター記憶部１２７に記憶された実行時パラメーターを読み出し、実行時パラメーターとロボット１０の姿勢とに基づいて、エンドエフェクター１２が力覚センサー２０に及ぼす力をオフセット値として算出し、算出したオフセット値により力覚値の補正処理を行う。

これにより、質量中心位置等の物理モデルを実行時パラメーターとして算出（推定）して、記憶することが可能になる。記憶した実行時パラメーターを用いて、図２７（Ｃ）に示したように固定部と同様の補正処理を行うことで、物理モデルが未知のエンドエフェクター１２についても適切に補正処理を行うことができる。

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

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

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

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

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

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

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

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

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

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

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

以上の本実施形態では、力覚値補正部１２０は、推定した質量中心位置が所定誤差以内ではないと判定された場合には、エラー処理を実行する。具体的には、力覚値補正部１２０により推定された質量中心位置が所定誤差以内ではないと判定された場合には、制御部１１０は、ロボット１０の姿勢を所与の姿勢とする制御を行い、力覚値補正部１２０は、制御部１１０によりロボット１０が所与の姿勢となる制御が行われた後の力覚値を取得し、取得した力覚値に対応するオフセット値を設定し、設定したオフセット値によるオフセット除去処理を、エラー処理として実行する。

これにより、図５（Ａ）、図５（Ｂ）に示したように、エンドエフェクター１２がそもそも物理モデル化できないような場合に、制御システム１００が自動的にそのことを検出することが可能になり、さらに物理モデル化が不可であるとされた場合の例外処理を行うことが可能になる。なお、第１の実施形態で上述したように、制御システム１００は質量中心位置が所定誤差以内か否かの判定を行うのみで、所定誤差以上であると判定された場合にはエラーを出力するだけであってもよい。その場合には、エラーに対する対応はユーザー側に委ねられることになる。ただし、本実施形態で説明したように、所定誤差以上の場合には自動的にリセット処理に移行するようにすれば、ユーザーが手動でシステム操作を行う頻度低減等が可能になり、利便性向上等が期待できる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

以上の本実施形態では、制御システム１００は図２３に示したように、エンドエフェクター１２が有する可変部（例えば図２４の１２−４）の姿勢情報を取得する姿勢情報取得部１３０を含む。そして、力覚値補正部１２０は、可変部の姿勢情報に基づいて、可変部の質量中心位置を推定して補正処理を行う。

また、以上の本実施形態では、制御システム１００はロボット１０の制御システムであり、ロボット１０のエンドエフェクター１２に対応して設けられる力覚センサー２０から力覚値を取得して、力覚値の補正処理を行う力覚値補正部１２０と、補正処理後の力覚値に基づいて、ロボット１０の制御を行う制御部１１０と、エンドエフェクター１２が有する可変部の姿勢情報を取得する姿勢情報取得部１３０を含み、力覚値補正部１２０は、可変部の姿勢情報に基づいて、可変部の質量中心位置を推定して補正処理を行ってもよい。

これにより、図６（Ａ）〜図６（Ｃ）に示したように、エンドエフェクター１２が可変部を含む場合であっても、可変部の状態に応じてエンドエフェクター１２の物理モデルを適切に求めて補正処理を行うことが可能になる。可変部の状態は当該可変部の姿勢情報により表すことができ、姿勢情報とは例えば図２６に示した角度θ等である。なお、姿勢情報は可変部の可動部分（ロボットであれば関節部分）の数に対応してそのパラメーター数が決定されることになる。

また、姿勢情報取得部１３０は、ロボット１０の姿勢情報を取得し、力覚値補正部１２０は、エンドエフェクター１２に含まれる固定部（例えば図２４の１２−３）の質量中心位置と、ロボット１０の姿勢情報に基づく第１の補正処理と、可変部の姿勢情報に基づいて推定された可変部の質量中心位置と、ロボット１０の姿勢情報に基づく第２の補正処理を、補正処理として実行してもよい。

ここで、第１の補正処理は図２３の第１の補正処理部１２１で行われる処理に対応し、第２の補正処理は第３の補正処理部１２２で行われる処理に対応する。

これにより、図２４に示したように、エンドエフェクター１２を可動部を持たない固定部と、可動部を有する可変部とに分割し、それぞれについて補正処理を行うことで、エンドエフェクター１２全体の補正処理を行うことが可能になる。

また、力覚値補正部１２０は、固定部の質量中心位置と、固定部の質量を含む情報を固定部パラメーターとして記憶する固定部パラメーター記憶部１２４と、可変部に対応して設定された座標系における可変部の質量中心位置と、可変部の質量を含む情報を可変部パラメーターとして記憶する可変部パラメーター記憶部１２５を含んでもよい。

そして、力覚値補正部１２０は、固定部パラメーター記憶部１２４に記憶された固定部パラメーターを読み出し、固定部パラメーターとロボット１０の姿勢情報とに基づいて、固定部が力覚センサー２０に及ぼす力をオフセット値として算出し、算出したオフセット値により第１の補正処理を行う。また、力覚値補正部１２０は、可変部パラメーター記憶部１２５に記憶された可変部パラメーターを読み出し、可変部パラメーター、可変部の姿勢情報、及びロボット１０の姿勢情報とに基づいて、可変部が力覚センサー２０に及ぼす力をオフセット値として算出し、算出したオフセット値により第２の補正処理を行う。

これにより、固定部及び可変部のそれぞれについて、必要な情報をパラメーターとして記憶した上で、当該パラメーターを用いた補正処理を行うことが可能になる。具体的には図２７（Ａ）、図２７（Ｂ）に示すようになり、固定部パラメーターは例えば質量中心位置（ｘ，ｙ，ｚ）と質量ｍであり、可変部パラメーターとは可変部の構造に応じてその内容が変化するパラメーターである。固定部については図２７（Ａ）に示したように、固定部パラメーターに対して、姿勢情報から得られる回転行列Ｒによる変換を行い、Ｆ’及びＭ’をオフセット値として求める。また、可変部については図２７（Ｂ）に示したように、可変部パラメーターに対して、可変部の姿勢情報を用いた処理を行うことで、質量中心位置（ｘ，ｙ，ｚ）と質量ｍを求め、その後固定部と同様に回転行列Ｒによる変換を行い、Ｆ’及びＭ’をオフセット値として求める。つまり、可変部パラメーターとは、可変部の姿勢情報と当該可変部パラメーターを合わせることで、質量中心位置と質量とを推定できるだけの情報であり、一例は上述したように、可変部座標系（狭義には指回転座標系）の原点から質量中心位置までのベクトルｔ２と、センサー座標系の原点から可変部座標系の原点までのベクトルｔ３と、センサー座標系と可変部座標系との間の回転を表すロール、ピッチ、ヨーの各角度情報が考えられる。

また、本実施形態において上述してきたように、機械装置は、ロボット１０であってもよく、機械機構は、ロボット１０のエンドエフェクター１２であってもよく、可変部は、エンドエフェクター１２を構成するパーツであってもよい。その場合、姿勢情報取得部１３０は、エンドエフェクター１２のパーツである可変部の姿勢情報を取得し、力覚値補正部１２０は、ロボット１０のエンドエフェクター１２に対応して設けられる力覚センサー２０から力覚値を取得し、可変部の姿勢情報に基づいて、可変部の質量中心位置を推定して力覚値の補正処理を行い、制御部１１０は、補正処理後の力覚値に基づいて、ロボット１０の制御を行うことになる。

これにより、本実施形態において説明した制御システム１００についても、ロボットシステムに対して適用することができ、具体的にはロボット１０の力制御等を行うことが可能になる。

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

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

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

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

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

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

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

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

Claims (8)

1. ロボットシステムであって、
   ロボットと、
   前記ロボットの機械機構に対応して設けられる力覚センサーから力覚値を取得して、前記力覚値の補正処理を行う力覚値補正部と、
   前記補正処理後の前記力覚値に基づいて、前記ロボットの制御を行う制御部と、
   を含み、
   前記力覚値補正部は、
   前記ロボットの第１〜第Ｎ（Ｎは２以上の整数）の姿勢において得られた第１〜第Ｎの力覚値に基づいて、前記機械機構の質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行い、
   前記力覚センサーが、Ｘ軸、Ｙ軸及びＺ軸の並進力Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚを取得する６軸力覚センサーである場合に、
   前記力覚値補正部は、
   Ｆｘ，Ｆｙ及びＭｚにより決定される第１の平面と、Ｆｙ，Ｆｚ及びＭｘにより決定される第２の平面と、Ｆｚ，Ｆｘ及びＭｙにより決定される第３の平面を求め、
   前記第１〜第３の平面の交点を前記質量中心位置として推定することを特徴とするロボットシステム。
2. 請求項１において、
   前記力覚値補正部は、
   前記機械機構の質量、前記質量中心位置、及び前記ロボットの前記姿勢に基づいて、前記機械機構により前記力覚センサーに作用する前記力覚値を推定することで、前記補正処理を行うことを特徴とするロボットシステム。
3. 請求項１又は２において、
   前記力覚値補正部は、
   前記ロボットの第１の姿勢において得られた第１の力覚値に基づいて、前記機械機構の前記質量中心位置を推定する処理を行い、
   前記制御部は、
   前記力覚値補正部により前記質量中心位置の推定が不可と判定された場合には、前記ロボットの姿勢を前記第１の姿勢とは異なる第２の姿勢に変更する制御を行い、
   前記力覚値補正部は、
   前記制御部により前記ロボットが第２の姿勢となる制御が行われた後に取得された第２の力覚値に基づいて、前記機械機構の前記質量中心位置を推定し、推定した前記質量中心位置に基づいて、前記補正処理を行うことを特徴とするロボットシステム。
4. 請求項１乃至３のいずれかにおいて、
   前記力覚値補正部は、
   推定した前記質量中心位置が所定誤差以内ではないと判定された場合には、エラー処理を実行することを特徴とするロボットシステム。
5. 請求項４において、
   前記力覚値補正部により推定された前記質量中心位置が所定誤差以内ではないと判定された場合には、
   前記制御部は、
   前記ロボットの姿勢を所与の姿勢とする制御を行い、
   前記力覚値補正部は、
   前記制御部により前記ロボットが前記所与の姿勢となる制御が行われた後の前記力覚値を取得し、取得した前記力覚値に対応する値をオフセット値として設定し、設定した前記オフセット値によるオフセット除去処理を、前記エラー処理として実行することを特徴とするロボットシステム。
6. 請求項１乃至５のいずれかにおいて、
   前記第１の平面のｘ切片をＡ１，ｙ切片をＢ１、ｚ切片をＣ１とし、前記第２の平面のｘ切片をＡ２，ｙ切片をＢ２、ｚ切片をＣ２とし、前記第３の平面のｘ切片をＡ３，ｙ切片をＢ３、ｚ切片をＣ３として、行列Ｍ及びベクトルｑを
   

   

   とした場合に、
   前記力覚値補正部は、
   前記質量中心位置を表す３次元ベクトルｐを
   Ｍｐ＝ｑ
   に従って求めることで、前記質量中心位置を推定することを特徴とするロボットシステム。
7. 請求項６において、
   前記力覚値補正部は、
   前記行列Ｍが正則でない場合に、前記質量中心位置の推定が不可であると判定することを特徴とするロボットシステム。
8. 請求項１乃至７のいずれかにおいて、
   前記力覚値補正部は、
   前記質量中心位置及び、前記機械機構の質量を含む情報を実行時パラメーターとして算出する実行時パラメーター算出部と、
   算出された前記実行時パラメーターを記憶する実行時パラメーター記憶部と、
   を含むことを特徴とするロボットシステム。

Images