JPWO2017026045A1 - ハンド力覚計測装置、ハンド力覚計測方法、及びハンド力覚計測プログラム - Google Patents

Abstract

ハンド力覚計測装置は、リンク機構を備える多関節ロボットにおけるリンク間の位置及び姿勢関係を幾何的に表すリンクパラメータと、前記リンクパラメータに影響する前記多関節ロボットの個体誤差とに基づいて、前記多関節ロボットのハンドに作用する外力を算出する処理装置を含む。

Description

本開示は、ハンド力覚計測装置、ハンド力覚計測方法、及びハンド力覚計測プログラムに関する。

ロボットのハンド（アーム先端部などの先端）において作用している外力を、力覚センサを用いずに算出する方法が知られている。かかる方法としては、駆動トルク及びトルク指令値との差を外部トルクとして算出し、関節角度に基づいてヤコビ行列を算出し、ヤコビ行列及び外部トルクから外力を算出する方法がある。

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

しかしながら、従来の方法では、ロボットの個体毎に異なりうる誤差（個体誤差）を補正せずに外力を算出するので、外力の算出精度が悪くなる可能性がある。

そこで、本開示は、外力の算出精度が良好となるハンド力覚計測装置、ハンド力覚計測方法、及びハンド力覚計測プログラムの提供を目的とする。

本開示の一局面によれば、リンク機構を備える多関節ロボットにおけるリンク間の位置及び姿勢関係を幾何的に表すリンクパラメータと、前記リンクパラメータに影響する前記多関節ロボットの個体誤差とに基づいて、前記多関節ロボットのハンドに作用する外力を算出する処理装置を含む、ハンド力覚計測装置が提供される。

本開示によれば、外力の算出精度が良好となるハンド力覚計測装置等が得られる。

リンク機構を備える多関節ロボットの一例を示す図である。 多関節ロボットの機構モデルを示す図である。 多関節ロボットの幾何的関係を示す図である。 多関節ロボットにかかる外力の説明図である。 関節における駆動機構の説明図である。 多関節ロボット１の制御ブロックの一例を示す図である。 個体誤差の算出方法の説明図である。 観測データの取得方法の説明図である。 観測データの取得方法の説明図である。 観測データの取得方法の説明図である。 ハンド力覚計測装置の処理装置のハードウェア構成の一例を示す図である。 処理装置の機能の一例を示す機能ブロック図である。 処理装置により実行される処理の一例を示すフローチャートである。

以下、添付図面を参照しながら各実施例について詳細に説明する。

図１は、リンク機構を備える多関節ロボットの一例を示す図である。図１に示す例では、多関節ロボット１は、リンク機構として、２つの関節１１及び１２と３つのリンク２１〜２３とを備える。多関節ロボット１は、リンク機構の一端に基部２を備え、他端（先端）に手先３０を備える。手先３０には、ハンド３２が装着される。ハンド３２は、ワーク（図示せず）を把持する等の機能を有する。

図２は、多関節ロボットの機構モデルを示す図であり、図３は、多関節ロボットの幾何的関係を示す図であり、図４は、多関節ロボットにかかる外力の説明図である。

図２は、図１に示す多関節ロボット１の機構モデルを示す。以下では、各関節に基部２から手先３０に向かう順序で番号i = 0,1, 2, . . ., n（以下、「関節番号」と称する）を付与する。基部２は関節番号ｉ＝０とする。図３に示す例では、関節１１（ｉ＝１）に関して、関節角がθ_１で示され、リンク長がＬ_１で示されている。また、関節１２（ｉ＝２）に関して、関節角がθ_２で示され、リンク長がＬ_２で示されている。関節角は、特に言及しない限り、ｚ軸まわりの関節角を表す。座標軸は、リンクの方向がｘ軸であり、紙面に対して垂直な方向がｚ軸であり、ｙ軸はｘ軸及びｚ軸に垂直な軸である。図３における"Ｘ"は、ハンド３２の位置を表すパラメータであり、基部２を原点とする座標系で表される。図４に示すように、各関節には、後述の駆動機構を介してトルクＴ_１及びＴ_２がそれぞれ付与され、ハンド３２には、作業時に外力Ｆが付与される。尚、図４において、点線は、外力Ｆがかからない場合、指令値通りに駆動されたときの多関節ロボット１の状態を示す。外力Ｆがかかることで、ハンド３２の位置等が変化されることが分かる。

図５は、関節における駆動機構の説明図であり、駆動機構を概略的に示す図である。駆動機構は、関節毎に設けられる。駆動機構４０は、モータ４１と、減速機４２とを含む。モータ４１の回転トルクは、減速機４２を介して増大され、リンク２２とリンク２３との間の関節１２まわりの相対回転を生じさせる。モータ４１には、モータ４１の回転角を計測するエンコーダ４３が設けられる。

図６は、多関節ロボット１の制御ブロックの一例を示す図である。図６において、点線で囲まれた部分は、制御対象を表す。

図６に示すように、先ず、目標位置算出部６００においてハンド３２の目標位置Ｘ_ｒｅｆが算出される。次いで、コンプライアンス計算部６０１において、目標位置Ｘ_ｒｅｆと、外力Ｆとに基づいて、位置補正量Δxが算出される。コンプライアンス計算は、以下の特性方程式で表すことができる。

ここで、Δxは位置補正量、Ｍ（ハット付き）は仮想慣性係数、Ｄ（ハット付き）は仮想粘性係数、Ｋ（ハット付き）は仮想剛性係数を表す。Δxの上に付されたドットは、微分を表し、２つのドットは、２回微分を表す。コンプライアンス計算部６０１では、外力Ｆから特性方程式を満足するようにΔxが算出される。次いで、変換部６０２において、位置補正量Δxに基づいて、逆キネマティクス計算により関節角補正量Δθが算出される。次いで、位置・速度制御部６０３では、関節角補正量Δθと現在の関節角θ（エンコーダ４３の計測値）とに基づいて、各モータ４１に印加する電流値ｕを算出し、該電流値ｕに応じた電流を各モータ４１に印加する。

次に、多関節ロボット１の手先３０（又はハンド３２、以下同じ）にかかる外力Ｆを算出（計測）するハンド力覚計測方法（装置）について説明する。尚、図６に示す制御において、外力Ｆは、以下で説明するハンド力覚計測装置で算出（計測）される値を用いることができる。

本実施例では、ハンド力覚計測装置７０は、多関節ロボット１におけるリンク間の位置及び姿勢関係を幾何的に表すパラメータと、該パラメータに影響する多関節ロボット１の個体誤差とに基づいて、多関節ロボット１の手先３０にかかる外力Ｆを算出する。

多関節ロボット１におけるリンク間の位置及び姿勢関係を幾何的に表すパラメータとは、いわゆるリンクパラメータである。リンクパラメータは、隣接する2つのリンク間の位置・姿勢関係を、3個の並進成分、及び3個の回転成分で表す。リンクパラメータは、行列（後述の行列Ｍ_ｉ参照）により表現できる。但し、リンクパラメータは、D-H（Denabit-Hartenberg）パラメータであってもよい。

個体誤差は、静的誤差と姿勢依存誤差とを含む。静的誤差とは、多関節ロボット１の姿勢に依存しない誤差であり、例えば、部品の加工誤差、組付誤差、温度による変形、エンコーダ４３の0点誤差などがある。姿勢依存誤差とは、バックラッシュ、ガタを含んだ自重・荷重によるリンクのたわみ誤差、制御モデル誤差などがある。

個体誤差は、リンクパラメータに影響する個体誤差であり、例えば、リンクパラメータと同様、3個の並進成分、及び3個の回転成分で表される。

個体誤差は、多関節ロボット１の個体毎に、各種試験を行って得られたデータに基づいて導出できる。データは、例えば、多関節ロボット１の制御情報（モータトルクの指令値など）と、そのときの手先３０の位置（ハンド位置）や多関節ロボット１の姿勢（関節角度など）とに関するデータである。尚、手先３０の位置は、例えばカメラ（モーションキャプチャ）などの３次元位置計測装置５０（図１０参照）で計測できる。この種のセンサは、手先３０の位置決め用のセンサが用いられてもよい。

図７は、個体誤差の算出方法の説明図であり、個体誤差の算出方法の流れを示す概略的なフローチャートである。ここでは、ある１つの多関節ロボット１に対する個体誤差の算出方法について説明する。

ステップＳ７００では、設計者（又はその補助者、以下同じ）は、多関節ロボット１を実働させることで観測データを取得（収集）する。例えば、図８Ａに示すようなハンド３２の未装着状態、図８Ｂに示すようなハンド３２の装着状態、及び図８Ｃに示すようなワークＷの把持状態のそれぞれで、多関節ロボット１に同じ動作をさせる。例えば、ロボット動作範囲内に置いた格子点に、上下左右前後など複数の方向から手先３０をアプローチさせて静定する。このとき、設計者（又はその補助者）は、以下の情報を時系列データとして収集する。
（１）モータトルクの指令値のような制御情報
（２）ハンド位置及び姿勢を含む計測情報（モーションキャプチャ及びエンコーダ４３で計測）
ステップＳ７０２では、設計者は、計算機を用いて、多関節ロボット１に外力がかかっていない静止状態で得た観測データ（例えば、図８Ａの状態で得られる観測データ）に基づいて、静的誤差の各要素を計算する。具体的には、まず、個体誤差を含むリンク構造を、該個体誤差を幾何的に表現した行列Ｍ_ｉ'で表現する。

ここで、ｉは、上述した関節番号であり、行列Ｍ_ｉ'は関節番号毎に算出される。行列Ｍ_ｉは、上述したリンクパラメータを表す行列（隣接する2つのリンク間の変換行列）である。数２における行列Ｃ_ｉは、リンクパラメータの行列Ｍ_ｉを補正するための行列であり、以下、「補正行列Ｃ_ｉ」と称する。補正行列Ｃ_ｉは、数２に示すように、3個の並進成分のそれぞれに係る個体誤差Δｘ_ｉ,Δｙ_ｉ,Δｚ_ｉ（第１個体誤差の一例）、及び3個の回転成分のそれぞれに係る個体誤差δ_ｉｘ,δ_ｉｙ,δ_ｉｚ（第２個体誤差の一例）を含む。個体誤差Δｘ_ｉ,Δｙ_ｉ,Δｚ_ｉは、静的誤差のみを含む。他方、個体誤差δ_ｉｘは、静的誤差と、姿勢依存誤差とを含み、例えば以下のとおりである。

ここで、δ_０，ｉｘは静的誤差を表し、Ｄ_ｉｘは姿勢依存誤差（回転誤差として表現）を表す。個体誤差δ_ｉｙ、及びδ_ｉｚについても同様である。

この場合、設計者は、観測データに基づいて、以下の数４を最小とするΔを算出する。

ここで、Ｐは、ハンド３２の計測位置であり、観測データに基づく。Σは、Ｐの全数分を累算することを表す。φ（θ_０，θ_１，...，θ_ｎ）は、各関節角度がθ_０，θ_１，...，θ_ｎであるときのハンド３２の理論位置である。φ（θ_０，θ_１，...，θ_ｎ）は、以下の通りである。尚、各関節角度がθ_０，θ_１，...，θ_ｎは、観測データに基づく。

ここで、行列Ｍ_ｉ'は、数２で示した通りである。但し、数５では、行列Ｍ_ｉ'は、静的誤差のみを含むものとする。即ち、Ｄ_ｉｘ＝０、Ｄ_ｉｙ＝０、及びＤ_ｉｚ＝０として、Δを算出（同定）する。Δは、以下のとおりである。

また、数４において、Ｊはヤコビ行列であり、φ（数５）をΔで偏微分することで導出できる。

ステップＳ７０４では、設計者は、計算機を用いて、多関節ロボット１に外力がかかった静止状態で得た観測データ（例えば、図８Ｂ及び図８Ｃの状態で得られる観測データ）に基づいて、姿勢依存誤差の各要素を計算する。具体的には、設計者は、同様に、観測データに基づいて、上記の数４を最小とするΔを算出する。数５では、行列Ｍ_ｉ'は、以下のように、静的誤差及び姿勢依存誤差を含むものとする。このとき、静的誤差Δｘ_ｉ,Δｙ_ｉ,Δｚ_ｉ、及びδ_０，ｉｘ,δ_０，ｉｙ,δ_０，ｉｚはステップＳ７０２で同定した値を用いる。

上記の数４において、Δは、以下のとおりである。

ここで、Ｄ_ｉｘ、Ｄ_ｉｙ、及びＤ_ｉｚは、以下のとおりである。

ここで、τ_ｉは、モータトルクの指令値である。この場合、上記の数４を最小とする弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚを求めることになる。尚、姿勢依存誤差Ｄ_ｉｘ、Ｄ_ｉｙ、及びＤ_ｉｚは、弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚが求まると、数８から分かるように、弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚと、モータトルクの指令値τ_ｉとに基づいて算出できる。

このようにして図７に示す個体誤差の算出方法によれば、観測データに基づいて、個体誤差（静的誤差及び姿勢依存誤差）を算出できる。尚、図７に示す例では、設計者が計算機を用いて個体誤差（補正行列Ｃ_ｉ）を算出しているが、後述のように、処理装置（例えば後述の処理装置１００の個体誤差計算部１１４）が算出することも可能である。

次に、ハンド力覚計測装置７０の構成例及び動作例について説明する。

ハンド力覚計測装置７０は、処理装置１００を含む。

図９は、処理装置１００のハードウェア構成の一例を示す図である。

図９に示す例では、処理装置１００は、制御部１０１、主記憶部１０２、補助記憶部１０３、ドライブ装置１０４、ネットワークＩ／Ｆ部１０６、入力部１０７を含む。

制御部１０１は、主記憶部１０２や補助記憶部１０３に記憶されたプログラムを実行する演算装置であり、入力部１０７や記憶装置からデータを受け取り、演算、加工した上で、記憶装置などに出力する。

主記憶部１０２は、ＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）などである。主記憶部１０２は、制御部１０１が実行する基本ソフトウェアであるＯＳ（Operating System）やアプリケーションソフトウェアなどのプログラムやデータを記憶又は一時保存する記憶装置である。

補助記憶部１０３は、ＨＤＤ（Hard Disk Drive）などであり、アプリケーションソフトウェアなどに関連するデータを記憶する記憶装置である。

ドライブ装置１０４は、記録媒体１０５、例えばフレキシブルディスクからプログラムを読み出し、記憶装置にインストールする。

記録媒体１０５は、所定のプログラムを格納する。この記録媒体１０５に格納されたプログラムは、ドライブ装置１０４を介して処理装置１００にインストールされる。インストールされた所定のプログラムは、処理装置１００により実行可能となる。

ネットワークＩ／Ｆ部１０６は、有線及び／又は無線回線などのデータ伝送路により構築されたネットワークを介して接続された通信機能を有する周辺機器と処理装置１００とのインターフェースである。

入力部１０７は、カーソルキー、数字入力及び各種機能キー等を備えたキーボード、マウスやタッチパッド等を有する。

尚、図９に示す例において、以下で説明する各種処理等は、プログラムを処理装置１００に実行させることで実現することができる。また、プログラムを記録媒体１０５に記録し、このプログラムが記録された記録媒体１０５を処理装置１００に読み取らせて、以下で説明する各種処理等を実現させることも可能である。なお、記録媒体１０５は、様々なタイプの記録媒体を用いることができる。例えば、記録媒体１０５は、ＣＤ(Compact Disc)−ＲＯＭ、フレキシブルディスク、光磁気ディスク等の様に情報を光学的，電気的或いは磁気的に記録する記録媒体、ＲＯＭ、フラッシュメモリ等の様に情報を電気的に記録する半導体メモリ等であってよい。なお、記録媒体１０５には、搬送波は含まれない。

図１０は、処理装置１００の機能の一例を示す機能ブロック図である。図１０には、処理装置１００に接続される構成要素（３次元位置計測装置５０等）についても併せて図示されている。

処理装置１００は、図１０に示すように、観測データ取得部９０を含む。また、処理装置１００は、ハンド位置計測値取得部１１０、関節角取得部１１１、モータトルク取得部１１２、リンクパラメータ計算部１１３、個体誤差計算部１１４、リンクパラメータ補正部１１５、及びヤコビ行列計算部１１６を含む。また、処理装置１００は、運動パラメータ計算部１１７、トルク算出部１１８、及び外力計算部１１９を含む。各部９０、１１０〜１１９は、制御部１０１が主記憶部１０２に記憶された１つ以上のプログラムを実行することにより実現できる。また、処理装置１００は、観測データ記憶部１２０、個体誤差記憶部１２１、及び運動パラメータ記憶部１２２を含む。観測データ記憶部１２０、個体誤差記憶部１２１、及び運動パラメータ記憶部１２２は、例えば補助記憶部１０３により実現できる。

観測データ取得部９０は、多関節ロボット１の実働中の観測データを取得（収集）し、観測データ記憶部１２０に記憶する。例えば、観測データ取得部９０は、ハンド位置計測値取得部１１０からの情報に基づいて、多関節ロボット１が静定したタイミングを検出し、該タイミングにおける観測データを取得する。観測データは、上述のとおりであり、ハンド位置計測値取得部１１０、関節角取得部１１１及びモータトルク取得部１１２から取得できる。

ハンド位置計測値取得部１１０は、３次元位置計測装置５０からハンド３２の３次元位置の計測値を取得する。

関節角取得部１１１は、各エンコーダ４３から各関節の関節角の検出値を取得する。

モータトルク取得部１１２は、モータ駆動部５２から各関節に係るモータ４１の駆動トルクの指令値（モータトルクの指令値）を取得する。モータ駆動部５２は、例えばモータコントローラ（図６参照）により実現されてもよい。

リンクパラメータ計算部１１３は、関節角取得部１１１からの情報に基づいて、リンクパラメータを算出する。リンクパラメータ計算部１１３は、個体誤差を含まないリンクパラメータを算出する。即ち、リンクパラメータ計算部１１３は、行列Ｍ_ｉを算出する。行列Ｍ_ｉは上述のとおりである。

個体誤差計算部１１４は、観測データ記憶部１２０からの情報に基づいて、リンクパラメータに影響する多関節ロボット１の個体誤差を算出する。例えば、個体誤差計算部１１４は、補正行列Ｃ_ｉを算出する。補正行列Ｃ_ｉの算出方法は上述のとおりである。個体誤差計算部１１４は、算出した個体誤差を個体誤差記憶部１２１に記憶する。個体誤差計算部１１４は、実稼働中に、定期的に、観測データ記憶部１２０に蓄積された最新の観測データに基づいて、個体誤差を再算出（更新）してよい。或いは、個体誤差計算部１１４は、実稼働中に、新たな観測データが取得される毎に、観測データ記憶部１２０に蓄積された最新の観測データに基づいて、個体誤差を再算出（更新）してもよい。

リンクパラメータ補正部１１５は、関節角取得部１１１、リンクパラメータ計算部１１３、及び個体誤差記憶部１２１からの情報に基づいて、リンクパラメータ計算部１１３で算出されたリンクパラメータを補正する。即ち、リンクパラメータ補正部１１５は、行列Ｍ_ｉ'を算出する。行列Ｍ_ｉ'の算出方法は上述のとおりである。

ヤコビ行列計算部１１６は、関節角取得部１１１及びリンクパラメータ補正部１１５からの情報に基づいて、ヤコビ行列Ｊ１を算出する。ヤコビ行列の算出方法は上述のとおりである。

運動パラメータ計算部１１７は、個体誤差記憶部１２１に基づいて、個体誤差を反映した運動パラメータを算出する。即ち、個体誤差により各リンクの長さなどが変わるため、運動パラメータ計算部１１７は、個体誤差記憶部１２１から得られる個体誤差に基づいて、運動パラメータを算出する。運動パラメータは、例えば、ロボットアームの動力学運動方程式に係るパラメータであり、慣性行列、非線形項(遠心力、コリオリ力など)、及び重力項とを含む（後述の数１０参照）。運動パラメータ計算部１１７は、算出した運動パラメータを運動パラメータ記憶部１２２に記憶する。運動パラメータ計算部１１７は、実稼働中に、定期的に、個体誤差記憶部１２１に記憶された最新の個体誤差に基づいて、運動パラメータを再算出（更新）してよい。或いは、個体誤差計算部１１４は、実稼働中に、個体誤差記憶部１２１内の個体誤差が更新される毎に、個体誤差記憶部１２１に記憶された最新の個体誤差に基づいて、運動パラメータを再算出（更新）してもよい。

トルク算出部１１８は、運動パラメータ記憶部１２２からの運動パラメータと、関節角取得部１１１からの情報とに基づいて、各モータ４１の駆動トルクを算出する。駆動トルクは、ロボットアームの動力学運動方程式に基づいて、以下のように算出できる。

ここで、θは、以下のとおりである。
θ＝［θ_０，θ_１，...，θ_ｎ］
また、上に"〜"が付いたＭは、慣性行列を表し、上に"〜"が付いたＨは、非線形項を表し、上に"〜"が付いたＨは、重力項を表す。

外力計算部１１９は、モータトルク取得部１１２、ヤコビ行列計算部１１６、及びトルク算出部１１８からの情報に基づいて、手先３０に掛かる外力Ｆを求める。具体的には、まず、外力計算部１１９は、モータトルク取得部１１２からのモータトルクの指令値τ_ｉと、トルク算出部１１８からの算出値τ_ｃとの差に基づいて、外部に仕事するトルクτ_ｅを算出する。即ち、以下のとおりである。
τ_ｅ＝τ_ｉ-τ_ｃ
ここで、τ_ｅ、及びτ_ｃは、τ_ｉと同様、それぞれ、各関節に係るトルク値を含む１×ｎの行列である。そして、外力計算部１１９は、トルクτ_ｅと、ヤコビ行列計算部１１６で得られるヤコビ行列Ｊ１とに基づいて、以下のように手先３０に掛かる外力Ｆを求める。

ここで、Ｊ１^Ｔは、ヤコビ行列Ｊ１の転置を表す。尚、数１１の式は、仮想仕事の原理に基づく。

図１０に示す処理装置１００よれば、個体誤差計算部１１４は、観測データに基づいて、個体誤差（静的誤差及び姿勢依存誤差）を算出できる。また、運動パラメータ計算部１１７は、個体誤差に基づいて、個体誤差が反映された運動パラメータを算出できる。そして、外力計算部１１９は、補正行列Ｃ_ｉ及び運動パラメータに基づいて、手先３０に掛かる外力Ｆを算出するので、個体誤差に起因した誤差成分が低減された高精度な外力Ｆを算出できる。

図１１は、処理装置１００により実行される処理の一例を示すフローチャートである。図１１に示す処理は、例えば、多関節ロボット１の実稼働中に所定周期毎に実行される。

ステップＳ１１００では、観測データ取得部９０は、ハンド位置計測値取得部１１０からの情報に基づいて、多関節ロボット１が静定したか否かを判定する。多関節ロボット１が静定したと判定した場合は、ステップＳ１１０２に進み、それ以外の場合は、ステップＳ１１０４に進む。

ステップＳ１１０２では、観測データ取得部９０は、ハンド位置計測値取得部１１０、関節角取得部１１１及びモータトルク取得部１１２から得られる情報を観測データとして観測データ記憶部１２０に記憶する。

ステップＳ１１０４では、個体誤差計算部１１４は、個体誤差の算出条件が成立したか否かを判定する。個体誤差の算出条件は、任意であるが、例えばユーザから指示があった場合、前回の算出から所定時間経過した場合、観測データ記憶部１２０の観測データの更新があった場合等に満たされてよい。個体誤差の算出条件が成立した場合は、ステップＳ１１０６に進み、それ以外の場合は、ステップＳ１１０８に進む。

ステップＳ１１０６では、個体誤差計算部１１４は、観測データ記憶部１２０に蓄積された最新の観測データに基づいて、個体誤差を算出し、算出した個体誤差を個体誤差記憶部１２１に記憶する。

ステップＳ１１０８では、運動パラメータ計算部１１７は、運動パラメータの算出条件が成立したか否かを判定する。運動パラメータの算出条件は、任意であるが、例えばユーザから指示があった場合、前回の算出から所定時間経過した場合、個体誤差記憶部１２１の個体誤差の更新があった場合等に満たされてよい。運動パラメータの算出条件が成立した場合は、ステップＳ１１１０に進み、それ以外の場合は、ステップＳ１１１２に進む。尚、運動パラメータの算出条件が個体誤差記憶部１２１の個体誤差の更新があった場合に満たされる構成である場合は、ステップＳ１１０４の判定結果が"ＮＯ"である場合、ステップＳ１１１２に進むことになる。また、ステップＳ１１０４の判定結果が"ＹＥＳ"である場合、ステップＳ１１０８の判定処理を経ずにステップＳ１１１０に進むことになる。

ステップＳ１１１０では、運動パラメータ計算部１１７は、個体誤差記憶部１２１に記憶された最新の個体誤差に基づいて、運動パラメータを算出し、算出した運動パラメータを運動パラメータ記憶部１２２に記憶する。

ステップＳ１１１２では、リンクパラメータ計算部１１３は、関節角取得部１１１からの情報に基づいて、リンクパラメータを算出する。

ステップＳ１１１４では、リンクパラメータ補正部１１５は、関節角取得部１１１、リンクパラメータ計算部１１３、及び個体誤差記憶部１２１からの情報に基づいて、行列Ｍ_ｉ'を算出する。

ステップＳ１１１６では、ヤコビ行列計算部１１６は、関節角取得部１１１からの情報と、ステップＳ１１１４で得た行列Ｍ_ｉ'とに基づいて、ヤコビ行列Ｊ１を算出する。ヤコビ行列Ｊ１の算出方法は上述のとおりである。

ステップＳ１１１８では、トルク算出部１１８は、運動パラメータ記憶部１２２からの運動パラメータと、関節角取得部１１１からの情報とに基づいて、各モータ４１の駆動トルクを算出する。各モータ４１の駆動トルクの算出方法は上述のとおりである。

ステップＳ１１２０では、モータトルク取得部１１２からの情報と、ステップＳ１１１６で得たヤコビ行列Ｊ１と、ステップＳ１１１８で得た各モータ４１の駆動トルクとに基づいて、手先３０に掛かる外力Ｆを算出する。外力Ｆの算出方法は上述のとおりである。

図１１に示す処理よれば、多関節ロボット１の実稼働中に、個体誤差計算部１１４は、観測データに基づいて、個体誤差（静的誤差及び姿勢依存誤差）に応じた補正行列Ｃ_ｉを算出できる。これにより、多関節ロボット１の実稼働中に、個体誤差を更新できる。そして、外力計算部１１９は、多関節ロボット１の実稼働中に更新される補正行列Ｃ_ｉ及び運動パラメータに基づいて、手先３０に掛かる外力Ｆを算出するので、外力Ｆの算出精度を実稼働中に高めることができる。

以上、各実施例について詳述したが、特定の実施例に限定されるものではなく、特許請求の範囲に記載された範囲内において、種々の変形及び変更が可能である。また、前述した実施例の構成要素を全部又は複数を組み合わせることも可能である。

例えば、図１１に示す処理において、個体誤差記憶部１２１には、個体誤差のうちの、事前に導出された静的誤差（例えば図７に示す方法のステップＳ７０２参照）が初期的に記憶されてよい。そして、多関節ロボット１の実稼働中、姿勢依存誤差の算出に必要となる弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚが導出・更新されてもよい（例えば図７に示す方法のステップＳ７０４参照）。

１ 多関節ロボット
２１〜２３ リンク
３０ 手先
３２ ハンド
４０ 駆動機構
４１ モータ
７０ ハンド力覚計測装置
９０ 観測データ取得部
１００ 処理装置
１１０ ハンド位置計測値取得部
１１１ 関節角取得部
１１２ モータトルク取得部
１１３ リンクパラメータ計算部
１１４ 個体誤差計算部
１１５ リンクパラメータ補正部
１１６ ヤコビ行列計算部
１１７ 運動パラメータ計算部
１１８ トルク算出部
１１９ 外力計算部
１２０ 観測データ記憶部
１２１ 個体誤差記憶部
１２２ 運動パラメータ記憶部

Claims (11)

1. リンク機構を備える多関節ロボットにおけるリンク間の位置及び姿勢関係を幾何的に表すリンクパラメータと、前記リンクパラメータに影響する前記多関節ロボットの個体誤差とに基づいて、前記多関節ロボットのハンドに作用する外力を算出する処理装置を含む、ハンド力覚計測装置。
2. 前記個体誤差は、並進誤差に係る第１個体誤差と、回転誤差に係る第２個体誤差とを含む、請求項１に記載のハンド力覚計測装置。
3. 前記第２個体誤差は、静的誤差と、前記多関節ロボットの姿勢に依存する姿勢依存誤差とを含む、請求項２に記載のハンド力覚計測装置。
4. 前記処理装置は、
   前記個体誤差を算出する個体誤差算出部と、
   前記個体誤差を記憶する記憶部と、
   前記個体誤差に基づいて、リンクパラメータを補正するリンクパラメータ補正部と、
   前記個体誤差に基づいて、運動パラメータを算出する運動パラメータ算出部と、
   前記運動パラメータに基づいて、前記多関節ロボットの各関節に付与されるトルクを算出するトルク算出部と、
   前記多関節ロボットの各関節にトルクを付与するモータへの駆動トルクの指令値と、前記トルク算出部により算出されたトルクとに基づいて、前記外力を算出する外力算出部とを含む、請求項１〜３のうちのいずれか１項に記載のハンド力覚計測装置。
5. 前記個体誤差は、前記多関節ロボットの個体毎に算出される、請求項１〜４のうちのいずれか１項に記載のハンド力覚計測装置。
6. 前記個体誤差は、前記多関節ロボットを実際に動かしたときの前記ハンドの位置の観測データに基づいて導出される、請求項１〜５のうちのいずれか１項に記載のハンド力覚計測装置。
7. 前記観測データは、前記多関節ロボットのハンドの位置と、前記多関節ロボットの各関節にトルクを付与するモータへの駆動トルクの指令値と、前記多関節ロボットの各関節の関節角度とを含む、請求項６に記載のハンド力覚計測装置。
8. 前記個体誤差は、以下の数１２のパラメータを最小とするパラメータΔを算出することで算出され、
   

   

   Ｐは、前記観測データに基づく前記ハンドの位置であり、Σは、前記観測データの複数のサンプルを累算することを表し、φ（θ_０，θ_１，...，θ_ｎ）は、各関節角度がθ_０，θ_１，...，θ_ｎであるときの前記ハンドの理論位置を表し、各関節角度がθ_０，θ_１，...，θ_ｎは、前記観測データに基づき、
   前記ハンドの理論位置φ（θ_０，θ_１，...，θ_ｎ）は、前記パラメータΔ及び前記リンクパラメータに基づいて表される、請求項７に記載のハンド力覚計測装置。
9. 前記ハンドの理論位置φ（θ_０，θ_１，...，θ_ｎ）は、以下の数１３で表され、
   

   

   行列Ｍ_ｉ'は、前記パラメータΔ及び前記リンクパラメータに基づいて表される、請求項８に記載のハンド力覚計測装置。
10. リンク機構を備える多関節ロボットにおけるリンク間の位置及び姿勢関係を幾何的に表すリンクパラメータを算出し、
    前記リンクパラメータに影響する前記多関節ロボットの個体誤差を算出し、
    算出した前記リンクパラメータ及び前記個体誤差に基づいて、前記多関節ロボットのハンドに作用する外力を算出する、
    ことを含む、コンピューターにより実行されるハンド力覚計測方法。
11. リンク機構を備える多関節ロボットにおけるリンク間の位置及び姿勢関係を幾何的に表すリンクパラメータを算出し、
    前記リンクパラメータに影響する前記多関節ロボットの個体誤差を算出し、
    算出した前記リンクパラメータ及び前記個体誤差に基づいて、前記多関節ロボットのハンドに作用する外力を算出する、
    処理をコンピューターに実行させるハンド力覚計測プログラム。

Images