JP2013018075A - ロボット、ロボット制御装置、ロボット制御方法、およびプログラム - Google Patents

Abstract

【課題】ロボットの姿勢ごとのハンドに加わる重力の影響を適切に補正するロボット、ロボット制御装置、ロボット制御方法、およびロボット制御プログラムを提供することを目的としている。
【解決手段】ハンドとアーム２との間に介挿されている力センサー３０に対して入力される重力の影響をモデル化した定数を有するモデル式を記憶する記憶部１０４と、ハンドが対象物を把持していない状態における力センサーが出力する検出値を用いて、モデル式の定数の値を算出する変数算出部１０６と、変数算出部が算出した定数をモデル式に代入して力センサーに対して入力される重力の影響の補正値を算出し、力センサーが出力する検出値から算出した補正値を差し引くことで、力センサーの検出値を補正する補正部１０７とを備える。
【選択図】図３

Description

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

近年、製造現場等では、スカラロボットや多軸ロボット等が製品の組み立てや検査等に用いられている。製品の組み立てや検査を行うときに物体を運搬する場合、ロボットは、物体を吸着したり、ハンドにより把持したりする。アームの先端には対象物を把持するハンドが取り付けられている。そして、ハンドに加わる力は、アームとハンドの間に取り付けられた力センサーにより検出している。この力センサーが検出する力には、ハンドに加わる重力の影響がある。このハンドに加わる重力の影響により、ロボットは、アームを高い精度で制御することが困難である。

このため、特許文献１に記載の発明では、重力補償をロボットの姿勢と、力センサーより手先部分に取り付けられた指部（ハンド）の質量から重力補償値を算出して、力センサーの検出値に対して重力補償している。

また、特許文献２には、力センサーが検出するハンドの自重を予め算出することで、力センサーが検出する値からハンドの自重をキャンセルしている。また、特許文献２に記載の発明では、力センサーが検出するハンドの自重を、ハンドを質点と見なし、力センサーとその質点が完全な剛体で接続されているものとしてハンドの自重を算出している。

また、特許文献３に記載の発明では、モデル化した数式中に含まれる変数を、ハンドに複数の姿勢を取らせた時の力センサーの出力値を用いて決定する。そして、特許文献３に記載の発明では、各時刻のハンドの姿勢に応じたハンドに加わる重力の影響を、モデル化した式を用いて求め、各時刻の力センサーの出力値から差し引くことで、ハンドが対象物に対して加えている力を検出している。

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

しかしながら、特許文献１には、ロボットの姿勢と指部の質量を用いて、重力補償値を算出する具体的な方法が記載されていない。このため、ロボットの姿勢ごとのハンドに加わる重力の影響を適切に補正できないという課題があった。

また、特許文献２に記載の発明では、力センサーとその質点が完全な剛体で接続されているものとして算出している。このため、有限の大きさをもつハンドが少なからず弾性のある物体で力センサーと接続されている実際のロボット装置に特許文献２に記載の発明を適用すると、力センサーの検出値に誤差が生じてしまう。従って、特許文献２に記載の発明では、ロボットの姿勢ごとのハンドに加わる重力の影響を適切に補正できないという課題があった。

また、特許文献３に記載の発明では、ハンドに加わる重力の影響を、ハンドを質点と見なし、力センサーとその質点が完全な剛体で接続されているものとしてモデル化している。このため、特許文献２に記載の発明と同様に、有限の大きさをもつハンドが少なからず弾性のある物体で力センサーと接続されている実際のロボット装置に特許文献３に記載の発明を適用すると、力センサーの検出値に誤差が生じてしまう。従って、ハンドが対象物に対して加えている力を正しく検出することが出来ず、ハンドに加わる重力の影響を適正に補正することが困難であるという課題があった。

本発明は、上記の問題点に鑑みてなされたものであって、ロボットの姿勢ごとのハンドに加わる重力の影響を適切に補正するロボット、ロボット制御装置、ロボット制御方法、およびプログラムを提供することを目的としている。

上記目的を達成するため、本発明のロボットは、ハンドとアームとの間に介挿されている力センサーと、前記力センサーが出力する検出値に対する重力の影響をモデル化した定数を有するモデル式を記憶する記憶部と、前記ハンドの姿勢を変化させた場合の前記力センサーが出力する検出値を用いて、前記モデル式の定数の値を算出する変数算出部と、前記変数算出部が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する補正部と、を備えることを特徴としている。

本発明によれば、ロボットのハンドの姿勢が変化した場合でも、ハンドの姿勢を変化さえて算出した定数を代入した力センサーの検出値への重力の影響のモデル式を用いて、力センサーの検出値を補正するため、ハンドの姿勢が変化した場合でもロボットのハンドに加わる重力の影響を適切に補正することができる。

また、本発明のロボットにおいて、前記変数算出部は、前記力センサーが出力する検出値と、前記ハンドの姿勢を変化させた場合の前記モデル式における算出値との２乗誤差の総和を算出し、前記２乗誤差の総和が予め定められているしきい値より小さい場合の定数の値を算出し、前記算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出するようにしてもよい。

また、本発明のロボットにおいて、前記変数算出部は、前記ハンドの姿勢を変化させた場合、前記力センサーが出力する検出値と前記各姿勢の前記モデル式における算出値との２乗誤差を算出し、前記２乗誤差が予め定められているしきい値より小さい場合の定数の値を算出し、前記算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出するようにしてもよい。

本発明によれば、ハンドの姿勢に応じて算出したモデルの定数を、力センサーの実測値とモデル式による算出値との２乗誤差により、モデル式の定数を算出するようにしたので、モデル誤差の演算量が少なくて済み、この算出されたモデル式の定数を用いて力センサーの検出値を補正することで、ハンドの姿勢が変化した場合でもロボットのハンドに加わる重力の影響を精度良く補正することができる。

また、本発明のロボットにおいて、前記モデル式は、前記力センサーが出力する検出値の力成分を、３次元空間上で前記ハンドの姿勢を表す３変数及び該３変数の三角関数で構成される高々３次式で表すようにしてもよい。

また、本発明のロボットにおいて、前記モデル式は、前記力センサーが出力する検出値のトルク成分を、３次元空間上で前記ハンドの姿勢を表す３変数及び該３変数の三角関数で構成される高々３次式で表わすようにしてもよい。

本発明によれば、ハンドに加わる重力の影響を、力センサーの力成分またはトルク成分の少なくとも１つの成分についてハンドの姿勢を表す３変数とそれらの三角関数の高々３次式で表した。この結果、ハンドを構成する部材や、その構造を考慮する必要がなく、最小２乗法を用いた一般的な計算でモデル式の各定数を算出することができる。このため、有限要素法を用いた数値計算などのように経験的に習得する知見は必要なく、簡便に、更には高い精度で測定値をモデル式で再現することができる。

また、本発明のロボットにおいて、前記ハンドにより対象物が把持されている状態である把持状態の変化を判別する把持状態判別部と、を備え、前記変数算出部は、前記把持状態判別部により前記ハンドの把持状態が変化したと判別された場合、前記モデル式の定数の値を算出し、前記補正部は、前記変数算出部が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正するようにしてもよい。

また、本発明のロボットにおいて、前記把持状態判別部は、前記ハンドが把持する対象物の姿勢状態が変化したか否か、または、前記ハンドが把持する対象物が変化したか否かにより把持状態の変化を判別するようにしてもよい。

本発明によれば、把持状態判別部が、ハンドの把持状態が変化したか否かを判別し、ハンドの把持状態が変化した場合に変数算出部がモデル式の定数を算出するようにした。なお、ハンドの把持状態とは、ハンドが対象物を把持しているか否か、ハンドが把持する対象物の大きさや質量が変化したか否か、ハンドが把持している対象物が把持する対象物の質量の中心点が変化したか否かなどである。この結果、ロボットが作業中に、ハンドが把持する対象物に応じて、力センサーの検出値を算出したモデル式の定数を用いて補正できるので、ハンドの把持状態が変化した場合でもロボットのハンドに加わる重力の影響を精度良く補正することができる。

また、本発明のロボットにおいて、前記ハンドを含む画像を撮像する撮像装置と、モデル式の定数を記憶する記憶部と、を備え、前記変数算出部は、予め前記ハンドの把持状態毎の前記モデル式の定数の値を算出し、前記算出したモデル式の定数を前記把持状態判別部が判別した把持状態と関連付けて前記記憶部に記憶させ、前記把持状態判別部により前記ハンドの把持状態が変化したと判別された場合、把持状態が変化したと判別された場合の前記把持状態と関連付けられて記憶されている前記モデル式の定数を前記記憶部から読み出し、前記補正部は、前記変数算出部が読み出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正するようにしてもよい。

本発明によれば、予めモデル式の定数を算出して記憶部に記憶させ、把持状態判別部によりハンドの把持状態が変化したと判別された場合に、記憶されているモデル式の定数を読み出して補正するようにした。この結果、ロボットの作業中に随時、モデル式の定数を算出する場合より、ロボット作業中の演算量を削減することができる。

また、本発明のロボットにおいて、前記モデル式は、前記ロボットが、地表面に対して垂直に設置されている場合、前記ハンドの姿勢を表す３変数の内、地表面に対して垂直な軸に関する回転に相当するものを除く２変数及び該２変数の三角関数で構成される高々２次式で表わすようにしてもよい。

本発明によれば、ロボットが地表面に対して垂直に設置されている場合、モデル式をハンドの姿勢の２変数とそれらのの三角関数による高々２次式で表したため、モデル式の変数が３姿勢の場合の高々３次式に比べて削減できるので、定数の値を算出する演算コストを削減できる。また、定数の値を算出するために必要な、対象物をハンドが把持していないときに力センサーが出力する検出値も、３姿勢の場合より少なくできる。また、ハンドを構成する部材や、その構造を考慮する必要がなく、最小２乗法を用いた一般的な計算でモデル式の各定数を算出することができる。このため、有限要素法を用いた数値計算などのように経験的に習得する知見は必要なく、簡便に、更には高い精度で測定値をモデル式で再現することができる。

また、本発明のロボットにおいて、前記しきい値は、前記ロボットを制御するに当たって許容される前記力センサーが出力する検出値に対する誤差の最大値を２乗した値に、前記２乗誤差の総和を算出するために用いたデータ数を乗じた値より小さい値であるようにしてもよい。

また、本発明のロボットにおいて、前記アームは６軸であり、前記６軸のアームの一方端がベースに連結され、前記６軸のアームの他方端と前記ハンドの間に前記力センサーが介挿されているようにしてもよい。

また、本発明のロボットにおいて、前記アームは７軸であり、前記７軸のアームの一方端がベースに連結され、前記７軸のアームの他方端と前記ハンドの間に前記力センサーが介挿されているようにしてもよい。

上記目的を達成するため、本発明のロボット制御装置は、ハンドとアームとの間に介挿されている力センサーに対して入力される重力の影響をモデル化した定数を有するモデル式を記憶する記憶部と、前記力センサーが出力する検出値を用いて、前記モデル式の定数の値を算出する変数算出部と、前記変数算出部が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正するとで、前記力センサーの検出値を補正する補正部と、前記補正部が補正した前記力センサーの検出値を用いて、前記アーム及び前記ハンドの動作を制御するアーム制御部と、を備えることを特徴としている。

上記目的を達成するため、本発明は、ハンドとアームとの間に介挿されている力センサーを有するロボットを制御するロボット制御装置のロボット制御方法であって、変数算出部が、前記力センサーが出力する検出値を用いて、記憶部に記憶されている前記力センサーに対して入力される重力の影響をモデル化した定数を有するモデル式の定数の値を算出する変数算出工程と、補正部が、前記変数算出工程が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する補正工程と、アーム制御部が、前記補正部が補正した前記力センサーの検出値を用いて、前記アーム及び前記ハンドの動作を制御するアーム制御工程と、を含むことを特徴としている。

上記目的を達成するため、本発明は、ハンドとアームとの間に介挿されている力センサーを有するロボットを制御するロボット制御の処理をコンピューターに実行させるためのプログラムであり、前記力センサーが出力する検出値を用いて、記憶部に記憶されている前記力センサーに対して入力される重力の影響をモデル化した定数を有するモデル式の定数の値を算出する変数算出手順と、前記変数算出手段が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する補正手順と、前記補正手順が補正した前記力センサーの検出値を用いて、前記アーム及び前記ハンドの動作を制御するアーム制御手順と、をコンピューターに実行させる。

本発明によれば、ロボットのハンドの姿勢が変化した場合でも、ハンドの姿勢を変化さえて算出した定数を代入した力センサーの検出値への重力の影響のモデル式を用いて、力センサーの検出値を補正するため、ハンドの姿勢が変化した場合でもロボットのハンドに加わる重力の影響を適切に補正することができる。ロボット制御装置、ロボット制御方法、およびプログラムを提供できる。

第１実施形態に係るロボット１の概略構成を示す斜視図である。 同実施形態に係るアーム２と力センサー３０の概念図である。 同実施形態に係る制御装置１００のブロック図である。 同実施形態に係るハンド４０による把持状態を説明する図である。 記憶部１０４が記憶するハンド４０の姿勢α、β、及びγと力センサー３０の出力の一例を示す図である。 記憶部１０４が記憶する把持状態とモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数の一例を示す図である。 力センサーの座標系を説明する図である。 ハンド４０とハンドに把持された対象物７０に加わる重力について説明する図である。 同実施形態に係る力センサー３０が検出するハンド４０に加わる重力の影響による定数の算出手順のフローチャートである。 同実施形態に係る力センサー３０が検出するハンド４０に加わる重力の影響を補正する手順のフローチャートである。 ハンド４０を力センサー３０に取り付けた場合の力センサー３０の出力値の一例を説明する図である。 ハンド４０を力センサー３０に取り付けた場合における、式（２２）を用いて算出した力センサー３０の予測値の一例を説明する図である。 第３実施形態に係る力センサー３０が検出するハンド４０に加わる重力の影響を補正する手順のフローチャートである。

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、本発明はかかる実施形態に限定されず、その技術思想の範囲内で種々の変更が可能である。また、以下の図面においては、各構成をわかりやすくするために、実際の構造と各構造における縮尺や数等が異なっている。

［第１実施形態］
図１は、本実施形態に係るロボット１の概略構成を示す斜視図である。図１に示すように、ロボット１は、１本のアーム２、固定部１０、力センサー３０、及びハンド４０を備える。また、ロボット１は、制御装置１００の制御により動作する。また、対象物７０は、ステージ７２の上面に配置されている。また、ハンド４０による物体の把持状態を撮像する撮像装置５０が、ステージ７２の上面に配置されている。

固定部１０は、例えば床、壁、天井、移動可能な台車の上などに固定される。
制御装置１００は、アーム２、及びハンド４０を制御する。制御装置１００の詳細は後述する。なお、制御装置１００は、固定部１０の外部ではなく、固定部１０の内部に設けるようにしてもよい。

アーム２は、第１フレーム２１、第２フレーム２２、第３フレーム２３、第４フレーム２４および第５フレーム２５から構成されている。第１フレーム２１は、回転屈折軸を介して、固定部１０に回転可能または屈折可能に接続されている。第２フレーム２２は、回転屈折軸を介して、第１フレーム２１および第３フレーム２３に接続されている。第３フレーム２３は、回転屈折軸を介して、第２フレーム２２および第４フレーム２４に接続されている。第４フレーム２４は、回転屈折軸を介して、第３フレーム２３および第５フレーム２５に接続されている。第５フレーム２５の一方端は、回転屈折軸を介して、第４フレーム２４に接続されている。アーム２は、制御装置１００の制御によって、各フレーム２１〜２５が各回転屈折軸を中心に複合的に回転または屈折し、動く。

アーム２の第５フレーム２５の他方端には、ハンド４０が設けられている。ハンド４０は、直接的に、対象物を把持する。なお、ハンド４０は、例えば、複数のフレームを有し、対象物を把持できる構造である。

アーム２の第５フレーム２５とハンド４０の間には、力センサー３０が介挿されている。力センサー３０は、ハンド４０に加わる力の各空間成分（力成分という）とトルクの各空間成分（トルク成分という）を検出する。力センサー３０は、検出したハンド４０に加わる力成分とトルク成分を含む情報を制御装置１００に出力する。

制御装置１００は、撮像装置５０が撮像した画像データに基づき、ロボット１のアーム２、ハンド４０を制御し、例えば、ステージ上の対象物７０を把持して、予め定められた位置へ移動させる。また、制御装置１００は、アーム２の姿勢（α、β、γ）の場合の力センサー３０が出力するハンド４０に加わる力成分とトルク成分を含む情報と、制御装置１００の記憶部に記憶されているモデル式を用いて、モデル式の各定数の値を算出する。また、制御装置１００は、算出した各定数を記憶されているモデル式に組み込んで、センサー値の補正値を算出する。また、制御装置１００は、算出した補正値を、力センサー３０が検出した検出値から差し引くことで、力センサー３０の検出値を補正する。

撮像装置５０は、ステージ７２の上面に配置されている。撮像装置５０は、例えばＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）カメラである。撮像装置５０は、対象物７２を撮影し、撮影したアナログ値の画像データを制御装置１００に出力する。なお、撮像装置５０は、ロボット１のアーム２に取り付けるようにしてもよい。

図２は、本実施形態に係るアーム２と力センサー３０の概念図である。
図２に示すように、アーム２は、回転部８２、８３及び８５と、屈折部８１、８４及び８６とを有している。そして、アーム２は、ハンド４０との間に力センサー３０を有している。

屈折部８１は、固定部１０と第１フレーム２１との間に設けられている。回転部８２は、第１フレーム２１と第２フレーム２２との間に設けられている。回転部８３は、第２フレーム２２と第３フレーム２３との間に設けられている。屈折部８４は、第３フレーム２３と第４フレーム２４との間に設けられている。回転部８５は、第４フレーム２４と第５フレーム２５との間に設けられている。屈折部８６は、第５フレーム２５のうち第４フレーム２４が設けられた他方に設けられている。

図２に示すように、アーム２は、６自由度を有する６軸アームである。すなわち、アーム２の先端のハンド４０は、これらの各軸の回転部と屈折部の動作により、移動する。このように、アーム２が６軸アームであるため、互いに独立な６つの変数で制御することが可能であり、例えば、位置（ｘ、ｙ、ｚ）の３つの変数（成分）、姿勢（α、β、γ）の３つの成分（変数）に対応して制御することができる。

図３は、本実施形態に係る制御装置１００のブロック図である。
図３に示すように、制御装置１００は、アーム制御部１０１、駆動部１０２、センサー出力取得部１０３、記憶部１０４、姿勢算出部１０５、変数算出部１０６、センサー値補正部１０７、画像取得部１０８、画像記憶部１０９、把持状態判別部１１０、および制御部１１１を備えている。

制御部１１１は、変数取得モードを示す情報、または、アーム制御モードを示す情報を生成し、生成した変数取得モードを示す情報、または、アーム制御モードを示す情報をアーム制御部１０１、把持状態判別部１１０に出力する。

画像取得部１０８は、撮像装置５０が出力する画像データを取得し、取得した画像データをデジタルデータに変換する。例えば、撮像装置５０の撮影タイミングが３０ｆｐｓ（フレーム／秒）の場合、画像取得部１０８は、撮像装置６０が撮像した画像を３３［ｍｓｅｃ］毎に取得する。
画像取得部１０８は、変換した画像データをフレーム番号と関連付けて画像記憶部１０９に記憶させる。なお、撮像装置５０が出力する画像データがデジタル信号の場合、画像取得部１０８は、取得した画像データを、変換せずにそのまま画像記憶部１０９に記憶させる。

把持状態判別部１１０は、画像記憶部１０９に記憶されている現在の画像データ、センサー出力取得部１０３が取得した力センサー３０の検出値、制御部１１１が出力する現在の動作のうち、少なくとも１つを用いて、現在のハンドの把持状態を判別し、判別結果を変数算出部１０６に出力する。なお、ハンドの把持状態とは、ハンドが対象物を把持しているか否か、ハンドが把持する対象物の大きさや質量が変化したか否か、ハンドが把持している対象物が把持する対象物の質量の中心点が変化したか否かなどである。
図４は、本実施形態に係るハンド４０による把持状態を説明する図である。図４（ａ）は、対象物７０の質量重心が、破線２０１の位置に把持されている状態を説明する図であり、図４（ｂ）は、対象物７０の質量重心が、破線２０１からｙ軸の負方向に距離Ｌだけずれ破線２０２の位置に把持されている状態を説明する図である。例えば、図４（ａ）が１フレーム前における画像データであり、図４（ｂ）が最新のフレームの画像データである。
把持状態判別部１１０は、例えば、このような図４（ａ）の画像データと、図４（ｂ）の画像データとを比較して、対象物７０の把持状態が変化しているか否かを判別する。

アーム制御部１０１は、アーム２、ハンド４０を制御する駆動信号を生成し、生成した駆動信号を駆動部１０２に出力する。また、アーム制御部１０１は、制御部１１１が出力する情報が変数取得モードを示す情報の場合、ハンド４０の重力による影響分を算出するため、後述するようにハンド４０の姿勢（α、β、γ）の３つの成分α、β、及びγを順次、変化させる。また、アーム制御部１０１は、センサー値補正部１０１が出力する補正された力センサー３０の出力値を用いてアーム２の制御を行う。

駆動部１０２は、アーム制御部１０１が出力する駆動信号に応じて、ロボット１のアーム２を駆動する。

センサー出力取得部１０３は、力センサー３０が出力するハンド４０に加わる力成分とトルク成分を含む情報を取得し、取得した力成分とトルク成分を示す情報を変数算出部１０６、センサー値補正部１０７、把持状態判別部１１０、及び制御部１１１に出力する。

記憶部１０４には、ハンド４０の姿勢（α、β、γ）の３つの成分α、β、及びγと力センサー３０の出力と画像データとが関連付けられて記憶されている。なお、これらの値は、例えば、制御部１００の利用者が、予め行うフィッティングにより取得して記憶させておく。また、関連付けられて記憶されている画像データは、例えば、ハンド４０が対象物７０を把持していない状態、ハンド４０が対象物７０を把持している状態である。また、ハンド４０が対象物７０を把持している状態の画像データは、対象物７０の大きさや、質量の中心位置が異なる状態を、記憶部１０４に予め記録させておくようにしてもよい。そして、把持状態毎に、モデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を関連付けて、記憶部１０４に予め記憶させておく。
図５は、記憶部１０４が記憶するハンド４０の姿勢（α、β、γ）の３つの成分α、β、及びγと力センサー３０の出力の一例を示す図である。図５に示すように、記憶部１０４には、例えば、姿勢（α、β、γ）の３つの成分α_１、β_１、γ_１の場合の力センサー３０の出力値ｆ_１１１が関連づけられて記憶されている。また、記憶部１０４には、後述する力センサー３０の出力値のモデル式が記憶されている。
図６は、記憶部１０４が記憶する把持状態とモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数の一例を示す図である。なお、モデル式とは、後述するように、力センサー３０の出力ベクトルをモデル化した式である。図６に示すように、記憶部１０４には、番号と姿勢と定数値が関連づけられて記憶されている。番号は識別子であり、把持状態は、把持状態判別部１１０が判別した結果である。変数値は、算出されたモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数である。
例えば、把持状態１は、ハンド４０が対象物７０を把持していない状態の状態番号である。そして、この把持状態１と関連づけられて記憶されているモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各変数は、ハンド４０が対象物７０を把持していない場合に変数算出部１０６が算出したモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数である。
例えば、把持状態２は、ハンド４０が対象物７０を把持している状態の状態番号である。そして、この把持状態２と関連づけられて記憶されているモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各変数は、ハンド４０が対象物７０を把持している場合に変数算出部１０６が算出したモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数である。

姿勢算出部１０５は、エンコーダー９０が出力する現在のハンド４０の姿勢（α、β、γ）を取得し、取得した現在のハンド４０の姿勢（α、β、γ）を変数算出部１０６とセンサー値補正部１０７に出力する。

変数算出部１０６は、モデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を、変数取得モードでは算出し、アーム制御モードでは記憶部１０４から読み出して、センサー値補正部１０７に出力する。

（変数取得モード）
（測定）変数算出部１０６は、変数取得モードの場合、制御部１１１がハンド４０の姿勢を変化させるのに合わせて、ハンド４０の姿勢毎に、センサー出力取得部１０３が取得した力センサー３０の検出値（力成分とトルク成分）と、姿勢取得部１０６が取得したハンドの姿勢を記憶部１０４に一時的に記憶させる。
（フィッティング）全てのハンド４０の姿勢の変化が完了した後、変数算出部１０６は、任意の姿勢における力センサー３０の出力を模したモデル式が、記憶部１０４に一次記憶したハンド４０の姿勢と、それに対応する力センサー３０の検出値を再現するように、後述するようにモデル式を構成する各項に含まれる定数を決定する。
（データの格納）定数決定後、変数算出部１０６は、決定した各定数を把持状態判別部１１０が出力する現在の把持状態の判別結果と関連づけて記憶部１０４に記憶させる。

（アーム制御モード）変数算出部１０６は、アーム制御モードの場合、把持状態判別部１１０により把持状態が変化したと判別される毎に、判別結果に基づき現在の把持状態に対応するモデル式を構成する各項に含まれる定数を記憶部１０４から読み出し、読み出した定数をセンサー値補正部１０７に出力する。

センサー値補正部１０７（補正部）は、変数算出部１０６が出力するモデル式を構成する各項に含まれる定数と、姿勢取得部１０５が出力する姿勢とをモデル式に代入して、現在、力センサー３０に入力される重力の影響を算出する。
センサー値補正部１０７は、センサー出力取得部１０３が出力する現在の力センサー３０の検出値から、算出した重力の影響を差し引いた値をアーム制御部１０１に出力する。

図７は、力センサー３０の座標系を説明する図である。図７（ａ）は、座標系Σと座標系ζを説明する図である。図７（ｂ）は、座標系Σにおいて、Ｚ軸周りに角度γ回転させた座標系Σ’を説明する図である。図７（ｃ）は、座標系Σにおいて、Ｙ軸周りに角度β回転させた座標系Σ’’を説明する図である。
図７（ａ）において、各軸がＸ、Ｙ、およびＺで表現される座標系Σは、６軸のアーム２の座標系である。また、各軸がｘ、ｙ、およびｚで表現される座標系ζは、ハンド４０（手先とも言う）の座標系（ＺＹＺ固定角座標）である。また、図７（ａ）〜図７（ｃ）において、符号３０１は、力センサー３０の表面である。
図７（ａ）〜図７（ｃ）に示すように、最初のＺ軸に関する回転角をγ、次のＹ軸に関する回転角をβ、最後のＺ軸に関する回転角をαとする。ＺＹＺ固定角による表現では、まず、図７（ｂ）に示すように、座標系ΣをＺ軸周りに角度γ回転させて、座標系Σ’を作る。次に、図７（ｃ）に示すように、回転させた座標系Σ’をＹ軸周りに角度β回転させて、座標系Σ’’を作る。そして、回転させた座標系Σ’’をＺ軸周りに角度α回転させて、座標系Σ’’’を作る。この座標系Σ’’は、座標系ζである。なお、ＺＹＺ固定角表現は、回転後の軸ではなく、最初に設定した軸に対して回転させた表現である。

力センサー３０は、検出したアーム２に関する力成分とトルク成分とを座標系ζの各成分に分解して制御装置１００に出力する。座標系ζの各軸の単位ベクトルをそれぞれベクトルｅ_ｘ、ベクトルｅ_ｙ、ベクトルｅ_ｚとすると、各単位ベクトルは、次式（１）〜（３）のように表される。

次に、ハンド４０に加わる重力の影響を考慮したモデル式について説明する。
本実施形態では、モデル式を、次式（４）〜（９）、及び定数を含む高々３次式とする。

式（４）〜（９）において、力センサー３０が検出する力への重力の影響は、ハンド４０やハンド４０に把持されている対象物７０に加わる重力の影響が、ハンド４０の姿勢を表すα、β、及びγの三角関数で表される対称性を持っている。さらに、式（４）〜（９）は、力センサー３０の構造等の影響により、上述した各三角関数がオフセットしていることも加味してある。

次に、ハンド４０及びハンド４０に把持されている対象物７０が、質量ｍの質点であるとする。
図８は、ハンド４０とハンドに把持された対象物７０に加わる重力について説明する図である。図８に示すように、ハンド４０とハンド４０に把持されている対象物７０に加わる重力ベクトルｆ_ｈの向きが、アーム２の座標系ΣのＸＹ平面にあるとする。また、ハンド４０とハンド４０に把持されている対象物７０に加わる重力ベクトルｆ_ｈとＺ軸とのなす角がβ_０であるとする。
図８において、ベクトルｆ_ｈを座標系Σで表現すると、次式（１０）のように表される。

なお、式（１０）において、ｇは重力加速度である。
上述した式（１）〜（３）と、式（１０）より、力センサー３０の力成分は、それらの内積である。このため、力センサー３０の出力ベクトルｆ_{ｓｅｎｓｏｒ}は、次式（１１）のように表される。

式（１１）において、ｓｉｎβ_０、ｃｏｓβ_０は定数であるので、ｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚはそれぞれα、β、及びγの三角関数の高々３次式のモデル式で表される。なお、α、β、及びγの三角関数の高々３次式とは、α、β、γ、ｓｉｎ（α＋ｃ）、ｃｏｓ（α＋ｃ） 、ｓｉｎ（β＋ｃ）、ｃｏｓ（β＋ｃ） 、ｓｉｎ（γ＋ｃ）、ｃｏｓ（γ＋ｃ）の中から、同じものを何回抜き出してもよい（例えば、αを３回抜き出した場合α^３）という前提で、組み合わせた全ての項の和である。
式（１１）のｆ_ｍｘのα、β、及びγの三角関数の高々３次式のモデル式は、例えば、次式（１２）のように、で表される。

なお、式（１２）のＡ，Ｂ，Ｃ，Ｄ，Ｅは、次式（１３）〜（１９）である。

式（１３）〜（１７）において、ｃ_ｊ（ｉ＝１，・・・，３２０）は定数である。また、ｆ_ｍｙ、ｆ_ｍｚについても、ｆ_ｍｘと同様に各々高々３次式で表される。変数算出部１０６は、この各定数ｃ_ｊを後述するように、ハンド４０の姿勢（α、β、γ）の３つの成分α、β、及びγを順次、変化させながら力センサー４０が取得した出力値に基づき算出する。変数算出部１０６は、ｆ_ｙとｆ_ｚの各定数ｃ_ｊを後述するように、ハンド４０の姿勢のα、β、及びγを変化させながら力センサー４０が取得した出力値に基づき算出する。
そして、センサー値補正部１０５は、算出した定数をｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚに組み込み、ハンドに加わる重力の影響による補正値を算出する。
力センサー３０のトルク成分のモデル式は、３次元空間上で姿勢を表す３つの変数と、それらの三角関数の高々３次式で表される。変数算出部１０６は、ハンド４０の姿勢のα、β、及びγを変化させながら力センサー４０が取得した出力値に基づき、モデル式の各定数を算出する。センサー値補正部１０５は、モデル式に算出した定数を組み込み、ハンドに加わる重力の影響による補正値を算出する。

次に、力センサー３０が検出するハンド４０に加わる重力の影響による定数の値を算出する手順について説明する。図９は、本実施形態に係る力センサー３０が検出するハンド４０に加わる重力の影響による定数の算出手順のフローチャートである。

（ステップＳ１０１）
まず、アーム制御部１０１は、制御部１１１が出力する変数取得モードを示す情報に基づき、駆動部１０２を介して、ハンド４０の姿勢（α、β、γ）の３つの成分α、β、及びγを順次、変化させる。姿勢算出部１０６は、制御部１１１がハンド４０の姿勢を変化させるのに合わせて、ハンド４０の姿勢毎に、センサー出力取得部１０３が取得した力センサー３０の検出値（力成分とトルク成分）である出力値ベクトルｆ（α_ｉ、β_ｉ、γ_ｉ）と、姿勢取得部１０６が取得したハンドの姿勢を記憶部１０４に一時的に記憶させる。
なお、変化させる範囲は、αが０［ｄｅｇ］〜３６０［ｄｅｇ］、βが０［ｄｅｇ］〜１８０［ｄｅｇ］、γが０［ｄｅｇ］〜３６０［ｄｅｇ］である。この角度の範囲で、例えば、１０［ｄｅｇ］間隔で、アーム制御部１０１は、α、β、及びγの全ての組み合わせについて姿勢を変化させる。ステップＳ１０１終了後、ステップＳ１０２に進む。

（ステップＳ１０２）
次に、変数算出部１０６は、記憶部１０４に記憶されているモデル式を読み出し、読み出したモデル式のｘｙｚの各成分（以下、各モデル式という）ｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚの各定数（パラメーターともいう）ｃ_ｊの初期値を設定する。なお、各定数ｃ_ｊの初期値は、例えば、乱数を用いて予め定めておき、記憶部１０４に記憶しておく。なお、何度かフィッティングをしている場合は、前回のフィッティングした結果を初期値として用いることで、繰り返し計算の回数を低減できる場合もある。
次に、変数算出部１０６は、設定した各定数ｃ_ｊの初期値を、各モデル式ｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚに組み込み（代入し）、モデル式出力値ベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）を算出する。このモデル式出力値ベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）で算出される値が、ハンド４０に加わる重力の影響であるため、力センサー３０の検出値に対する補正値である。
ステップＳ１０２終了後、ステップＳ１０３に進む。

全てのハンド４０の姿勢の変化が完了した後、変数算出部１０６は、任意の姿勢における力センサー３０の出力を模したモデル式が、記憶部１０４に一次的に記憶したハンド４０の姿勢と、それに対応する力センサー３０の検出値を再現するように、ステップＳ１０３〜Ｓ１０７により、モデル式を構成する各項に含まれる定数を決定する。
（ステップＳ１０３）
次に、変数算出部１０６は、記憶部１０４に記憶されているハンド４０の姿勢毎の出力値ベクトルｆ（α_ｉ、β_ｉ、γ_ｉ）を順次、読み出し、読み出したハンド４０の姿勢毎の出力値ベクトルｆ（α_ｉ、β_ｉ、γ_ｉ）をモデル式に代入する。次に、変数算出部１０６は、出力値ベクトルｆ（α_ｉ、β_ｉ、γ_ｉ）とモデル式出力値ベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）との２乗誤差の総和Ｓを、次式（１８）により算出する。ステップＳ１０３終了後、ステップＳ１０４に進む。

（ステップＳ１０４）
次に、変数算出部１０６は、ステップＳ１０３で算出した２乗誤差の総和Ｓが、予め定めたしきい値よりも小さいか否かを判別する。また、変数算出部１０６は、各姿勢において、読み出した出力値ベクトルｆ（α_ｉ、β_ｉ、γ_ｉ）とモデル式出力値ベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）との２乗誤差を算出し、算出した２乗誤差が予め定めたしきい値よりも小さいか否かを判別する。各姿勢の２乗誤差が予め定めたしきい値よりも小さいとは、各姿勢における最大の２乗誤差も予め定めたしきい値より小さいことを意味している。なお、予め定めたしきい値は、例えば、以下のように求める。
まず、ロボット１を制御するに当たって許容される力センサー３０の値に対する誤差の最大値を決定し、その値をεとする。ステップＳ１０３で計算する２乗誤差は、全てのデータ点における誤差の２乗を足し合わせた値であるので、データ点の数で割ることで誤差の２乗の平均を算出できる。算出した誤差の２乗の平均が、誤差の最大値εの２乗よりも小さければ、ロボット１を制御するに当たって求められる力センサーの値に対する誤差が、設定された最大値よりも小さいことを表している。以上のことから、しきい値は、次式（１９）のように、データ数ｎに誤差の最大値の２乗を乗じた値より小さい値である。

なお、式（１９）において、ｎはデータ点の数である。
２乗誤差の総和Ｓが予め定めたしきい値よりも小さく、各姿勢の２乗誤差が予め定めたしきい値よりも小さく場合（ステップＳ１０４；Ｙｅｓ）、ステップＳ１０８に進む。また、２乗誤差の総和Ｓが、予め定めたしきい値よりも小さくないか、各姿勢の２乗誤差が予め定めたしきい値よりも小さくない場合（ステップＳ１０４；Ｎｏ）、ステップＳ１０５に進む。

（ステップＳ１０５）
次に、変数算出部１０６は、ステップＳ１０３〜Ｓ１０６の処理が予め設定されているｎ回（ｎは１以上の自然数）行われたか否かを判別する。ステップＳ１０３〜Ｓ１０６の処理が予め設定されているｎ回行われた場合（ステップＳ１０５；Ｙｅｓ）、ステップＳ１０７に進む。ステップＳ１０３〜Ｓ１０６の処理が予め設定されているｎ回行われていない場合（ステップＳ１０５；Ｎｏ）、ステップＳ１０６に進む。

（ステップＳ１０６）
２乗誤差の総和Ｓが、予め定めたしきい値よりも小さくない場合（ステップＳ１０４；Ｎｏ）、変数算出部１０６は、ステップＳ１０２で設定した定数を変更する。なお、初期値の変更は、例えば勾配法、モンテカルロ法、遺伝アルゴリズムなどを用いて行う。
定数を変更後、ステップＳ１０３に戻り、ステップＳ１０３〜Ｓ１０５を、ステップＳ１０４の判定がＹｅｓになるまで再帰的に繰り返す。

（ステップＳ１０７）
ステップＳ１０３〜Ｓ１０６の処理が予め設定されているｎ回行われた場合（ステップＳ１０５；Ｙｅｓ）、ステップＳ１０４の２乗誤差の総和Ｓが閾値よりも小さい値を見つけられなかったことになる。このため、変数算出部１０６は、ステップＳ１０３〜Ｓ１０６をｎ回行った中で、２乗誤差の総和Ｓが最小の値となる各定数（パラメーター）ｃ_ｊの値を選択する。
同様に、ステップＳ１０１〜Ｓ１０７の処理により、ｆ_ｍｙ、ｆ_ｍｚの各定数ｃ_ｊの値を算出する。ステップＳ１０７終了後、ステップＳ１０８に進む。

（ステップＳ１０８）
ステップＳ１０７で各定数（パラメータ）ｃｊを算出後、または、２乗誤差の総和Ｓが予め定めたしきい値よりも小さく、各姿勢の２乗誤差が予め定めたしきい値よりも小さく場合（ステップＳ１０４；Ｙｅｓ）、変数算出部１０６は、算出したｆ_ｍｙ、ｆ_ｍｚの各定数ｃ_ｊの値を把持状態判別部１１０が出力する現在の把持状態の判別結果と関連づけて記憶部１０４に記憶させる。

ステップＳ１０１〜Ｓ１０７における力センサー３０が検出するハンド４０に加わる重力の影響による定数の算出手順は、アーム２を用いた作業を行う前に予め行う。定数の算出は、アーム２を用いた作業で想定されるアーム２と対象物７０との状態毎に変数算出部１０６が算出する。アーム２を用いた作業で想定されるアーム２と対象物７０との状態毎とは、例えば、ハンド４０が何も把持していない状態、ハンド４０が第１の対象物であるドライバーを把持している状態、ハンド４０がドライバーを把持し且つドライバーにねじが嵌合されている状態などである。このような各状態において、変数算出部１０６は、さらにハンド４０の位置・姿勢毎、対象物７０毎、ハンド４０に対する対象物７０の重心の位置毎などについて、力センサー３０が検出するハンド４０に加わる重力の影響による定数を算出する。
また、ステップＳ１０３〜Ｓ１０７において、力センサー３０の実測値と、モデル式の算出値との２乗誤差を用いてモデル式の定数を算出するようにしたので、モデル誤差の演算量が少なくて済む効果がある。

次に、力センサー３０が検出するハンド４０に加わる重力の影響を補正する手順について説明する。図１０は、本実施形態に係る力センサー３０が検出するハンド４０に加わる重力の影響を補正する手順のフローチャートである。なお、初期状態では、ハンド４０が、何も把持していないとして説明する。

（ステップＳ２０１）
アーム制御部１０１は、制御部１１１が出力するアーム制御モードを示す情報に基づき、駆動部１０２を介してハンド４０に対象物７０を把持させるように制御する。ステップＳ２０１終了後、ステップＳ２０２に進む。
（ステップＳ２０２）
次に、画像取得部１０８は、撮像装置５０が出力する画像データを、順次取得し、取得した画像データを画像記憶部１０９に記憶させる。ステップＳ２０２終了後、ステップＳ２０３に進む。

（ステップＳ２０３）
次に、変数算出部１０６は、把持状態判別部１１０により把持状態が変化したと判別される毎に、判別結果に基づき現在の把持状態に対応するモデル式を構成する各項に含まれる定数を記憶部１０４から読み出し、読み出した定数をセンサー値補正部１０７に出力する。ステップＳ２０３終了後、ステップＳ２０４に進む。

（ステップＳ２０４）
次に、センサー値補正部１０５は、変数算出部１０６が出力するモデル式を構成する各項に含まれる定数と、姿勢取得部１０５が出力する姿勢とをモデル式に代入して、現在、力センサー３０に入力される重力の影響を算出する。
次に、センサー値補正部１０７は、センサー出力取得部１０３が出力する現在の力センサー３０の検出値から、算出した重力の影響を差し引いた値をアーム制御部１０１に出力する。ステップＳ２０４終了後、ステップＳ２０５に進む。

（ステップＳ２０５）
次に、アーム制御部１０１は、センサー値補正部１０７が出力する補正された力センサー３０の出力値を用いてアーム２の制御を行う。ステップＳ２０５終了後、ステップＳ２０６に進む。

（ステップＳ２０６）
次に、把持状態判別部１１０は、画像記憶部１０９に記憶されている現在の画像データ、センサー出力取得部１０３が取得した力センサー３０の検出値、制御部１１１が出力する現在の動作のうち、少なくとも１つを用いて、現在のハンドの把持状態を判別し、判別結果を変数算出部１０６に出力する。ステップＳ２０６終了後、ステップＳ２０７に進む。

（ステップＳ２０７）
把持状態が変化していると判別された場合（ステップＳ２０７；Ｙｅｓ）、ステップＳ２０８に進む。
把持状態が変化していないと判別された場合（ステップＳ２０７；Ｎｏ）、ステップＳ２０２に戻る。

（ステップＳ２０８）
把持状態が変化していると判別された場合（ステップＳ２０７；Ｙｅｓ）、現在の把持状態に対応するモデル式を構成する各項に含まれる定数を記憶部１０４から読み出し、読み出した定数をセンサー値補正部１０７に出力する。ステップＳ２０８終了後、ステップＳ２０９に進む。

（ステップＳ２０９）
次に、センサー値補正部１０５は、ステップＳ２０４と同様に、変数算出部１０６が出力するモデル式を構成する各項に含まれる定数と、姿勢取得部１０５が出力する姿勢とをモデル式に代入して、現在、力センサー３０に入力される重力の影響を算出する。次に、センサー値補正部１０７は、センサー出力取得部１０３が出力する現在の力センサー３０の検出値から、算出した重力の影響を差し引いた値をアーム制御部１０１に出力する。ステップＳ２０９終了後、ステップＳ２１０に進む。
（ステップＳ２１０）
次に、アーム制御部１０１は、センサー値補正部１０７が出力する補正された力センサー３０の出力値を用いてアーム２の制御を行う。ステップＳ２１０終了後、ステップＳ２０２に戻る。
以後、所定の作業が終了するまで、例えば、ハンド４０が把持した対象物７０を、予め定めた位置まで移動させる作業において、ステップＳ２０２〜Ｓ２１０を繰り返す。

なお、ステップＳ２０１〜Ｓ２１０では、把持状態判別部１１０の判別結果に基づいてハンド４０が把持している対象物７０の大きさや重量が変化したことを検出する例を説明した。ロボット１が非図示の制御プログラムで制御されている場合、制御プログラムに含まれている対象物７０に応じて、アーム２の制御開始時に、記憶部１０４に記憶されているモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を読み出して、力センサー３０の検出値を補正するようにしてもよい。

以上のように、本実施形態によれば、ハンドに取り付けられた力センサーに対して入力されるハンドやそれに把持されたツールなどの対象物７０に加わる重力の影響をモデル化した式を、姿勢を表す３変数の三角関数の高々３次式で構成した。このため、ハンドを構成する部材や、その構造を考慮する必要がなく、最小２乗法を用いた一般的な計算で、このモデル式を構成する各項の定数を算出することができる。また、有限要素法を用いた数値計算などのように経験的に習得する知見は必要なく、簡便に、更には高い精度で測定値をモデル式で再現することが可能である。従って、センサー値補正部１０７は、モデル式を用いて変数算出部１０６が算出した値を力センサーの検出値から差し引くことでハンドが対象物に対して加えている力を正しく検出することが可能となる。この結果、ロボットの姿勢ごとのハンドに加わる重力の影響を適切に補正することができる。

なお、ステップＳ１０１〜Ｓ１０７の処理により、ｆ_ｍｙ、ｆ_ｍｚの各定数の値を算出する処理は、ロボット１を用いて作業を行う前に行ってもよく、あるいは、ロボット１の作業中に行うようにしてもよい。
各定数の値の算出をロボット１が用いて作業を行う前に行う場合、ハンド４０が何も把持していない状態、ハンド４０が対象物７０を把持している状態、あるいは、ハンド４０が対象物である図示しないドライバーを把持し、さらに図示しないドライバーにねじが嵌合されている場合などの作業中に想定される状態についてフィッティングを行い、制御部１００が、力センサー３０及びエンコーダー９０の出力を取得して各定数の値を算出する。そして、作業中に、算出した各状態の補正値を用いて、制御部１００は、ハンドに加わる重力の影響を補正する。
各定数の値の算出をロボット１の作業中に行う場合、ハンド４０の把持している状態が変化した場合、ハンド４０の姿勢を変化させて重力の影響を測定し、制御部１００が、それをフィッティングする定数を算出する。そして、作業中に、算出した各状態の補正値を用いて、制御部１００は、ハンドに加わる重力の影響を補正する。

［第２実施形態］
図１において、アーム２を有するロボット１が地表面に対して垂直に設置されている場合、ハンド４０やハンド４０に把持されている対象物７０に加わる重力の影響は、地表面に対して垂直な軸に関する回転に相当する角度αに依存しない。この場合、上述した式（１１）は、式（６）〜（９）及び定数による次式（２０）と次式（２１）のように高々２次式で表せる。

式（２０）において、ｃ_ｊ（ｉ＝１，２，・・・，５６）は定数である。変数算出部１０６は、この各定数ｃ_ｊを第１実施形態のステップＳ１０１〜Ｓ１０５により、ハンド４０の姿勢のα、β、及びγを変化させながら力センサー３０が取得した出力値に基づき算出する。

式（２１）において、ｃ_ｊ（ｉ＝１，２，・・・，５６）は定数である。変数算出部１０６は、この各定数ｃ_ｊを第１実施形態のステップＳ１０１〜Ｓ１０５により、ハンド４０の姿勢のα、β、及びγを変化させながら力センサー３０が取得した出力値に基づき算出する。
そして、センサー値補正部１０５は、算出した定数をｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚに組み込み、ハンドに加わる重力の影響による補正値を算出する。

［実験結果の説明］
次に、アーム２を有するロボット１が地表面に対して垂直に設置され、ハンド４０やハンド４０に把持されている対象物７０に加わる重力の影響が、角度αに依存しない場合の実測値と、本実施形態による算出値の例を説明する。
以下、図１において、ハンド４０として、質量約８００［ｇ］、高さ約２０［ｃｍ］で３本の指を有する多目的ハンドを力センサー３０に取り付けた場合について説明する。
式（２０）及び、ステップＳ１０１〜Ｓ１０５を用いて、ｆ_ｘの各定数を算出した結果、ｆ_ｘは次式（２２）のような結果が得られた。

なお、式（２２）において、各定数ｃ_３６＝１７．２０、ｃ_３７＝１．５７、ｃ_３８＝−０．７８、ｃ_５０＝０．３６、ｃ_５１＝０．０８、ｃ_５６’（＝ｃ_５６／ｃ_５０）＝−０．８８である。また、式（２０）における他の定数ｃ_ｊの値は０である。

図１１は、ハンド４０を力センサー３０に取り付けた場合の力センサー３０の出力値の一例を説明する図である。図９は、ハンド４０を力センサー３０に取り付けた場合における、式（２２）を用いて算出した力センサー３０の予測値の一例を説明する図である。また、図１１と図１２において、縦軸（ｚ軸）は力［Ｎ］、ｘ軸は姿勢γ、ｙ軸は姿勢βである。

図１１に示すように、力センサー３０により検出される力ｆは、力センサー３０に取り付けられているハンド４０の影響により、ハンド４０の姿勢β、及びγに応じて異なっている。すなわち、姿勢βの変化に応じて、力センサー３０により検出される力ｆは正弦波状に変化している。同様に、姿勢γの変化に応じて、力センサー３０により検出される力ｆは正弦波状に変化している。

式（２２）を用いて、姿勢β、及びγを変化させて算出した力センサー３０の予測値（補正値）をグラフで表した結果、図１２に示すように、力センサー３０により検出される予測値の力ｆ’は、ほぼ図１１の実測された力センサー３０の出力値と同様の値である。
このように、変数算出部１０６が、力センサー３０により検出される予測値を算出し、センサー値補正部１０７が、算出された予測値の力ｆ’を実際の力センサー３０の検出値から差し引いて補正することで、ハンド４０が対象物７０に対して加えている力を、ロボットの姿勢によらず高い精度で検出できる。この結果、ロボットの姿勢ごとのハンドに加わる重力の影響を適切に補正することができる。

［第３実施形態］
第１実施形態では、予めモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を算出して記憶部１０４に記憶させておき、アーム２の制御の場合に、記憶部１０４に記憶されているモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を読み出して力センサー３０の検出値を補正する例を説明した。本実施形態では、モデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数をアーム２の制御中に取得する例を、図１３を用いて説明する。
図１３は、本実施形態に係る力センサー３０が検出するハンド４０に加わる重力の影響を補正する手順のフローチャートである。

（ステップＳ３０１）
アーム制御部１０１は、制御部１１１が出力するアーム制御モードを示す情報に基づき、駆動部１０２を介してハンド４０に対象物７０を把持させるように制御する。ステップＳ３０１終了後、ステップＳ３０２に進む。
（ステップＳ３０２）
次に、変数算出部１０６は、記憶部１０４に記憶されているモデル式ベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）を読み出し、ｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚの各定数ｃ_ｊの初期値を設定する。
次に、変数算出部１０６は、設定した各定数ｃ_ｊの初期値を、ｆ_ｍｘ、ｆ_ｍｙ、ｆ_ｍｚに組み込み（代入し）、モデル式出力値ベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）を算出する。ステップＳ３０２終了後、ステップＳ３０３に進む。

（ステップＳ３０３）
次に、画像取得部１０８は、撮像装置５０が出力する画像データを、順次取得し、取得した画像データを画像記憶部１０９に記憶させる。ステップＳ３０３終了後、ステップＳ３０４に進む。
（ステップＳ３０４）
次に、変数算出部１０６は、ステップＳ１０３〜Ｓ１０７の処理により、モデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を算出する。変数算出部１０６は、算出したモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を、把持状態判別部１１０が出力する現在の把持状態の判別結果と関連付けて記憶部１０４に記憶させる。

（ステップＳ３０５）
次に、センサー値補正部１０５は、変数算出部１０６が出力するモデル式を構成する各項に含まれる定数と、姿勢取得部１０５が出力する姿勢とをモデル式に代入して、現在、力センサー３０に入力される重力の影響を算出する。
センサー値補正部１０７は、センサー出力取得部１０３が出力する現在の力センサー３０の検出値から、算出した重力の影響を差し引いた値をアーム制御部１０１に出力する。ステップＳ３０５終了後、ステップＳ３０６に進む。

（ステップＳ３０６）
次に、アーム制御部１０１は、センサー値補正部１０７が出力する補正された力センサー３０の出力値を用いてアーム２の制御を行う。ステップＳ３０６終了後、ステップＳ３０７に進む。
（ステップＳ３０７）
次に、画像取得部１０８は、撮像装置５０が出力する画像データを、順次取得し、取得した画像データを画像記憶部１０９に記憶させる。ステップＳ３０７終了後、ステップＳ３０８に進む。

（ステップＳ３０８）
次に、把持状態判別部１１０は、画像記憶部１０９に記憶されている現在の画像データ、センサー出力取得部１０３が取得した力センサー３０の検出値、制御部１１１が出力する現在の動作のうち、少なくとも１つを用いて、現在のハンドの把持状態を判別し、判別結果を変数算出部１０６に出力する。ステップＳ３０８終了後、ステップＳ３０９に進む。

（ステップＳ３０９）
把持状態が変化していると判別された場合（ステップＳ３０９；Ｙｅｓ）、ステップＳ３０４に戻る。
把持状態が変化していないと判別された場合（ステップＳ３０９；Ｎｏ）、ステップＳ３０６に戻る。

以上のステップＳ３０１〜Ｓ３０９を、予め定められた作業が終了するまで繰り返す。これにより、アーム２の制御中に、制御装置１００は、ハンド４０が対象物７０を把持している状態の変化毎にモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を算出し、算出したモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を用いて力センサー３０の検出値を補正して制御できる。
なお、ステップＳ３０９；Ｙｅｓにおいて、把持状態が変化していると判別された場合、モデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数が記憶部１０４に記憶済みであれば、記憶済みのモデル式におけるｆ_ｘ、ｆ_ｙ、ｆ_ｚの各定数を記憶部１０４から読み出して用いるようにしてもよい。

なお、本実施形態では、予測値をｆ_ｘ、ｆ_ｙ、ｆ_ｚについて算出する例を説明したが、力センサーが力成分のみではなく、トルク成分も検出して出力するタイプの場合は、各軸に対するトルク成分ｔ_ｘ、ｔ_ｙ、ｔ_ｚに対しても予測値を算出するようにしてもよい。この場合、トルク成分ｔ_ｘ、ｔ_ｙ、ｔ_ｚに対しても、高々３次式、または、高々２次式で表されるモデル式の各定数の値を算出する。そして算出された定数を組み込んだモデル式を用いて、トルク成分ｔ_ｘ、ｔ_ｙ、ｔ_ｚのそれぞれの予測値を算出するようにしてもよい。

なお、本実施形態では、ロボット１が６自由度を有する６軸アームの例を説明したが、本発明は係る実施形態に限定されず、例えば、７自由度を有する７軸アームのロボットであってもよい。７軸アームのロボット１は、例えば、図１において、固定部１０と屈折部８１との間に、さらに７軸目の回転部を備えている。この場合においても、まず、力センサー３０の検出値への重力の影響のモデル式を作成し、変数算出部１０６が、このモデル式の定数をハンド４０の状態毎に、ハンドの姿勢を変化させて算出する。そして、ハンドの状態毎に算出した定数を代入したモデル式を用いて、力センサー３０の検出値を補正して、アーム制御部１０１は、この補正した値を用いてハンドの動作を制御する。

７軸の場合アーム２は、７自由度を有する７軸アームである。すなわち、アーム２の先端のハンド４０は、これらの各軸の回転部と屈折部の動作により、移動する。このように７軸アームにおいても、モデル式は、６軸アームの場合と同様に、３次元空間上で姿勢を表す３つの変数（α、β、γ）と、これら３つの変数の三角関数の高々３次式で表される。

なお、実施形態の図３の制御部の各部の機能を実現するためのプログラムをコンピューター読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピューターシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピューターシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ−ＲＯＭ等の可搬媒体、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ） Ｉ／Ｆ（インタフェース）を介して接続されるＵＳＢメモリー、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、サーバーやクライアントとなるコンピューターシステム内部の揮発性メモリーのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。

１・・・ロボット、 ２・・・アーム、 １０・・・固定部
３０・・・力センサー、４０・・・ハンド、５０・・・撮像装置、７０・・・対象物、
７２・・・ステージ、８２、８３、８５・・・回転部、８１、８４、８６・・・屈折部、
９０、９１〜９６・・・エンコーダー、１００・・・制御部、１０１・・・アーム制御部、
１０２・・・駆動部、１０３・・・センサー出力取得部、１０４・・・記憶部、
１０５・・・姿勢算出部、１０６・・・変数算出部、１０７・・・センサー値補正部、
１０８・・・画像取得部、１０９・・・把持状態判別部、
α、β、γ・・・ハンド４０の姿勢の成分
Ｓ・・・力センサー３０の計測値のベクトルｆ（α_ｉ、β_ｉ、γ_ｉ）とモデル式計算値のベクトルｆ_ｍ（α_ｉ、β_ｉ、γ_ｉ）との２乗誤差の総和
ｃ_ｊ・・・モデル式の定数（パラメーター）

Claims (16)

1. ハンドとアームとの間に介挿されている力センサーと、
   前記力センサーが出力する検出値に対する重力の影響をモデル化した定数を有するモデル式を記憶する記憶部と、
   前記ハンドの姿勢を変化させた場合の前記力センサーが出力する検出値を用いて、前記モデル式の定数の値を算出する変数算出部と、
   前記変数算出部が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する補正部と、
   を備えることを特徴とするロボット。
2. 前記変数算出部は、
   前記力センサーが出力する検出値と、前記ハンドの姿勢を変化させた場合の前記モデル式における算出値との２乗誤差の総和を算出し、前記２乗誤差の総和が予め定められているしきい値より小さい場合の定数の値を算出し、前記算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出する
   ことを特徴とする請求項１に記載のロボット。
3. 前記変数算出部は、
   前記ハンドの姿勢を変化させた場合、前記力センサーが出力する検出値と前記各姿勢の前記モデル式における算出値との２乗誤差を算出し、前記２乗誤差が予め定められているしきい値より小さい場合の定数の値を算出し、前記算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出する
   ことを特徴とする請求項１または請求項２に記載のロボット。
4. 前記モデル式は、
   前記力センサーが出力する検出値の力成分を、３次元空間上で前記ハンドの姿勢を表す３変数及び該３変数の三角関数で構成される高々３次式で表わす
   ことを特徴とする請求項１から請求項３のいずれか１項に記載のロボット。
5. 前記モデル式は、
   前記力センサーが出力する検出値のトルク成分を、３次元空間上で前記ハンドの姿勢を表す３変数及び該３変数の三角関数で構成される高々３次式で表わす
   ことを特徴とする請求項１から請求項４のいずれか１項に記載のロボット。
6. 前記ハンドにより対象物が把持されている状態である把持状態の変化を判別する把持状態判別部と、
   を備え、
   前記変数算出部は、
   前記把持状態判別部により前記ハンドの把持状態が変化したと判別された場合、前記モデル式の定数の値を算出し、
   前記補正部は、
   前記変数算出部が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する
   ことを特徴とする請求項１から請求項５のいずれか１項に記載のロボット。
7. 前記把持状態判別部は、
   前記ハンドが把持する対象物の姿勢状態が変化したか否か、または、前記ハンドが把持する対象物が変化したか否かにより把持状態の変化を判別する
   ことを特徴とする請求項６に記載のロボット。
8. 前記ハンドを含む画像を撮像する撮像装置と、
   モデル式の定数を記憶する記憶部と、
   を備え、
   前記変数算出部は、
   予め前記ハンドの把持状態毎の前記モデル式の定数の値を算出し、前記算出したモデル式の定数を前記把持状態判別部が判別した把持状態と関連付けて前記記憶部に記憶させ、
   前記把持状態判別部により前記ハンドの把持状態が変化したと判別された場合、把持状態が変化したと判別された場合の前記把持状態と関連付けられて記憶されている前記モデル式の定数を前記記憶部から読み出し、
   前記補正部は、
   前記変数算出部が読み出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する
   ことを特徴とする請求項６または請求項７に記載のロボット。
9. 前記ハンドの動作を制御する制御部を備え、
   前記把持状態判別部は、
   前記ハンドの把持状態が変化したか否かを、前記撮像装置が撮像した画像データ、前記力センサーが出力する検出値、前記制御部による前記ハンドへの制御指示の少なくとも１つを用いて判別する
   ことを特徴とする請求項６から請求項８のいずれか１項に記載のロボット。
10. 前記モデル式は、
    前記ロボットが、地表面に対して垂直に設置されている場合、前記ハンドの姿勢を表す３変数の内、地表面に対して垂直な軸に関する回転に相当するものを除く２変及び該２変数の三角関数で構成される高々２次式で表わす
    ことを特徴とする請求項１から請求項９のいずれか１項に記載のロボット。
11. 前記しきい値は、
    前記ロボットを制御するに当たって許容される前記力センサーが出力する検出値に対する誤差の最大値を２乗した値に、前記２乗誤差の総和を算出するために用いたデータ数を乗じた値より小さい値である
    ことを特徴とする請求項２に記載のロボット。
12. 前記アームは６軸であり、前記６軸のアームの一方端がベースに連結され、前記６軸のアームの他方端と前記ハンドの間に前記力センサーが介挿されている
    ことを特徴とする請求項１から請求項１０のいずれか１項に記載のロボット。
13. 前記アームは７軸であり、前記６軸のアームの一方端がベースに連結され、前記７軸のアームの他方端と前記ハンドの間に前記力センサーが介挿されている
    ことを特徴とする請求項１から請求項１０のいずれか１項に記載のロボット。
14. ハンドとアームとの間に介挿されている力センサーに対して入力される重力の影響をモデル化した定数を有するモデル式を記憶する記憶部と、
    前記力センサーが出力する検出値を用いて、前記モデル式の定数の値を算出する変数算出部と、
    前記変数算出部が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正するとで、前記力センサーの検出値を補正する補正部と、
    前記補正部が補正した前記力センサーの検出値を用いて、前記アーム及び前記ハンドの動作を制御するアーム制御部と、
    を備えることを特徴とするロボット制御装置。
15. ハンドとアームとの間に介挿されている力センサーを有するロボットを制御するロボット制御装置のロボット制御方法であって、
    変数算出部が、前記力センサーが出力する検出値を用いて、記憶部に記憶されている前記力センサーに対して入力される重力の影響をモデル化した定数を有するモデル式の定数の値を算出する変数算出工程と、
    補正部が、前記変数算出工程が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する補正工程と、
    アーム制御部が、前記補正部が補正した前記力センサーの検出値を用いて、前記アーム及び前記ハンドの動作を制御するアーム制御工程と、
    を含むことを特徴とするロボット制御方法。
16. ハンドとアームとの間に介挿されている力センサーを有するロボットを制御するロボット制御の処理をコンピューターに実行させるためのプログラムであり、
    前記力センサーが出力する検出値を用いて、記憶部に記憶されている前記力センサーに対して入力される重力の影響をモデル化した定数を有するモデル式の定数の値を算出する変数算出手順と、
    前記変数算出手段が算出した定数を前記モデル式に代入して前記力センサーに対して入力される重力の影響の補正値を算出し、前記力センサーが出力する検出値を、前記算出した補正値を用いて補正することで、前記力センサーの検出値を補正する補正手順と、
    前記補正手順が補正した前記力センサーの検出値を用いて、前記アーム及び前記ハンドの動作を制御するアーム制御手順と、
    をコンピューターに実行させるためのプログラム。

Images