JP5730614B2

JP5730614B2 - トルクセンサ校正装置、校正方法、及びプログラム

Info

Publication number: JP5730614B2
Application number: JP2011041247A
Authority: JP
Inventors: 中村　仁彦; 仁彦中村; 拓神永; 光鮎澤; 高橋　太郎; 太郎高橋
Original assignee: University of Tokyo NUC; Toyota Motor Corp
Current assignee: University of Tokyo NUC; Toyota Motor Corp
Priority date: 2011-02-28
Filing date: 2011-02-28
Publication date: 2015-06-10
Anticipated expiration: 2031-02-28
Also published as: JP2012176465A

Description

本発明は、多関節ロボットに搭載されたトルクセンサを校正するためのトルクセンサ校正装置、校正方法、及びプログラムに関するものである。

一般に、トルクセンサは温度変化などの環境変化の影響を受けるため、多関節ロボットなどに搭載された後、トルクセンサの零点調整を行うための校正が必要となる。特に、ヒューマノイド型のロボットなどの複雑な構成の多関節ロボットにおいては、校正を行うために一定の姿勢をとるために、複雑な操作を要することがある。

これに対し、例えば、予め、所定姿勢時に所定負荷を作用させたときの各関節のトルクを計測し初期値として記憶し、校正時にその初期値とその所定姿勢での計測値とを比較して校正を行うロボットの制御装置が知られている（特許文献１参照）。

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

しかしながら、上記ロボットの制御装置においては、予め、所定姿勢で各関節のトルクを計測し、校正時にも所定姿勢で校正を行うため、その所定姿勢での校正に制約されることとなる。

また、多関節ロボットに装着されたトルクセンサの校正を行う場合に、その多関節ロボットの質量、重心位置、慣性テンソルなどの力学パラメータの同定が必要となる（例えば、特許文献２参照）。さらに、これら力学パラメータが変動している場合に、正確に校正を行うことが困難となるため、トルクセンサの校正前に、これら力学パラメータの同定を行う必要が生じる。このとき、各関節のトルク値が必要となるが、例えば、トルクセンサを有しない多関節ロボットや、トルクセンサを有しても校正前で正確なセンサ値が得られない多関節ロボットの場合、アクチュエータの電流値などを用いてそのトルク値を換算することとなる。

この場合、換算に利用したトルク値には、トルク定数の誤差や完全にモデル化できない機械的誤差（ギア摩擦等）などを含むこととなるため、このトルク値を用いて同定した力学パラメータも誤差を含むこととなる。さらに、力学パラメータの同定は、容易ではなく一定の作業時間を要することとなる。

本発明は、このような問題点を解決するためになされたものであり、多関節ロボットに搭載されたトルクセンサの校正を高精度かつ容易に行うことができるトルクセンサ校正装置、校正方法、及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数のリンクと該各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットに搭載され、前記関節のトルクを計測するトルクセンサと、前記多関節ロボットの接触点に作用する外力を計測する力センサと、前記トルクセンサにより計測された前記関節のトルクと、前記力センサにより計測された前記外力と、前記多関節ロボットのベースリンクに関する運動方程式と、に基づいて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する同定手段と、を備える、ことを特徴とするトルクセンサ校正装置である。この一態様によれば、多関節ロボットに搭載されたトルクセンサの校正を高精度かつ容易に行うことができる。

この一態様において、前記同定手段は、前記多関節ロボットのベースリンクに関する運動方程式である下記（７）式を用いて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定してもよい。これにより、最小力学パラメータと、トルクセンサのオフセット値と、を同時に同定できるため、トルクセンサの校正を高精度かつ容易に行うことができる。

この一態様において、前記同定手段は、前記多関節ロボットのベースリンクに関する運動方程式である下記（６）式を用いて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、前記力センサの力のオフセット値と、を同時に同定してもよい。これにより、最小力学パラメータと、トルクセンサのオフセット値と、力センサのオフセット値と、を同時に同定できるため、トルクセンサ及び力センサの校正をより高精度かつ容易に行うことができる。

この一態様において、前記最小力学パラメータとは、前記多関節ロボットの各リンクの質量、重心位置、及び慣性テンソルの関数であってもよい。

この一態様において、前記トルクセンサにより計測された前記関節のトルクと、前記同定手段により同定された前記トルクセンサのオフセット値と、に基づいて、前記トルクセンサのトルクを補正する補正手段を更に備えていてもよい。これにより、補正したトルクを用いて、多関節ロボットを高精度に制御できる。

この一態様において、前記補正手段は、前記力センサにより計測された前記外力と、前記同定手段により同定された前記力センサのオフセット値と、に基づいて、前記力センサの外力を補正してもよい。これにより、補正した外力を用いて、多関節ロボットを高精度に制御できる。

この一態様において、前記同定手段は、例えば、最小二乗法を用いて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同定してもよい。

この一態様において、前記同定手段により同定された前記最小力学パラメータにより制御パラメータを補正してもよい。

他方、上記目的を達成するための本発明の一態様は、複数のリンクと該各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットの前記関節のトルクを計測する工程と、前記多関節ロボットの接触点に作用する外力を計測する工程と、前記計測された関節のトルクと、前記計測された外力と、前記多関節ロボットのベースリンクに関する運動方程式と、に基づいて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する工程と、を含む、ことを特徴とする校正方法であってもよい。

また、上記目的を達成するための本発明の一態様は、複数のリンクと該各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットの前記関節のトルクと、前記多関節ロボットの接触点に作用する外力と、前記多関節ロボットのベースリンクに関する運動方程式と、に基づいて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する処理を、コンピュータに実行させることを特徴とするプログラムであってもよい。

本発明によれば、多関節ロボットに搭載されたトルクセンサの校正を高精度かつ容易に行うことができるトルクセンサ校正装置、校正方法、及びプログラムを提供することができる。

本発明の一実施形態に係るトルクセンサ校正装置の概略的な構成を示すブロック図である。本発明の一実施形態に係るトルクセンサ校正装置の処理フローの一例を示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の一実施形態に係るトルクセンサ校正装置の概略的な構成を示すブロック図である。本実施形態に係るトルクセンサ校正装置１０は、多関節ロボット１００に搭載されたトルクセンサ１及び力センサ２と、トルクセンサ１及び力センサ２の校正を行う校正装置３と、を備えている。

多関節ロボット１００は、例えば、ヒューマノイド型ロボットであり、複数のリンクと、各リンクを回転可能に連結する複数の関節と、から構成される多関節リンク機構を有している。

トルクセンサ１は、多関節ロボット１００の各関節に搭載されており、各関節の回転トルクを計測する。トルクセンサ１は校正装置３に接続されており、計測した各関節のトルクを校正装置３に対して出力する。

同様に、力センサ２は、多関節ロボット１００（例えば、足首や足裏など）に搭載されており、多関節ロボット１００の接触点に作用する外力を計測する。力センサ２は校正装置３に接続されており、計測した外力を校正装置３に対して出力する。

校正装置３は、多関節ロボット１００の力学パラメータと、トルクセンサ１のトルクのオフセット値と、力センサ２の外力のオフセット値と、を同時に同定する同定部３１と、同定部３１により同定されたオフセット値に基づいて、トルクセンサ１の計測値及び力センサ２の計測値の補正を行う補正部３２と、を有している。

なお、校正装置３は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。

同定部３１は、同定手段の一具体例であり、トルクセンサ１により計測された多関節ロボット１００の各関節のトルクと、力センサ２により計測された多関節ロボット１００の接触点に作用する外力と、多関節ロボット１００のベースリンクに関する運動方程式と、に基づいて、その運動方程式の最小力学パラメータと、トルクセンサ１のトルクのオフセット値と、力センサ２の外力のオフセット値と、を同時に同定する。

ここで、同定部３１が用いる多関節ロボット１００の運動方程式の導出方法について、詳細に説明する。

多関節ロボット１００のベースリンクに関する運動方程式は、一般に、下記（１）式により表わすことができる。なお、下記（１）式において、Ｍは慣性マトリックス、Ｂはバイアスベクトル（重力、遠心力、コリオリ力などを含む）、τは関節のトルク、Ｆは多関節ロボットに作用する力（Ｆ＝Ｍ・ａ）、ｑは関節の回転角度、を表している。

上記（１）式において、多関節ロボット１００の各リンクの質量、重心位置、慣性テンソルなどの力学パラメータは、慣性マトリクスＭやバイアスベクトルＢの中に含まれ、非線形式となっているが、下記（２）式のように線形式として表すこともできる。

但し、ベースリンクは、基底でありどのリンクにも設定可能であるが、例えばヒューマノイドロボットでは体幹リンクとすることが多い。力学パラメータφは、一般に、動力学モデルを記述する上で冗長であり一意に同定できないため、動力学モデルの計算に必要最小限のパラメータ（最小力学パラメータ）に定式化する必要がある。

また、同定可能な最小力学パラメータφ_B∈R^NBは動力学モデルの構造に依存し、運動方程式において冗長なパラメータを削減・再構成して得られる。なお、この最小力学パラメータを数値的及び解析的に計算する方法は周知であり、例えば、最小力学パラメータφ_Ｂを用いて下記（３）式のように表わすことができる。

上記（３）式に基づいて、例えば、最小二乗法を用いて、最小力学パラメータφ_Ｂの推定値φ_Ｂ（ハット）を同定することができる。この最小二乗法を用いた手法、及び上記最小力学パラメータの計算方法は、例えば、論文「内界センサを用いたヒューマノイドの力学パラメータの同定、ベンチャー、鮎澤、中村、第１４回ロボティクスシンポジア２００９」に開示されており、これを援用することができる。

一方、トルクセンサ１で計測されるトルク値には、オフセット値が含まれており、下記（４）式のように表わすことができる。下記（４）式において、トルクセンサ１の計測値をτ_measured、トルクの真値をτ、トルクセンサ１のオフセット値をτ_offset、とする。
τ_measured＝τ＋τ_offset （４）式

また、力センサ２で計測される計測値（外力）にもオフセット値が含まれており、下記（５）式のように表わすことができる。下記（５）において、ｋ番目の力センサ２の計測値をＦ_{k_measured}、真値をＦ_k、力センサ２のオフセット値をＦ_{k_offset}、とする。
Ｆ_{k_measured}＝Ｆ_k＋Ｆ_{k_offset} （５）式

さらに、上記（３）式に上記（４）式及び（５）式を代入することで、下記（６）式を得ることができる。下記（６）式において、Ｉは単位行列とする。

上記（６）式において、例えば、最小二乗法を用いて、最小力学パラメータφ_B（リンクの質量、重心位置、慣性テンソルなどの力学パラメータ）を同定すると共に、力センサ２のオフセット値Ｆ_{k_offset}及びトルクセンサ１のオフセット値τ_offsetを、同時に同定することができる。

なお、力センサ２のオフセット値Ｆ_{k_offset}を同定する必要がない場合は、上記（６）式からオフセット値Ｆ_{k_offset}に対応する部分を除いた、下記（７）式を用いて、最小力学パラメータφ_Bと、トルクセンサ１のオフセット値τ_offsetとを同時に同定することができる。

同定部３１は、トルクセンサ１により計測された多関節ロボット１００の各関節のトルクτ_measuredと、力センサ２により計測された外力Ｆ_{k_measured}と、上述のようにして導出した多関節ロボット１００のベースリンクに関する運動方程式（７）式と、に基づいて、この運動方程式の最小力学パラメータφ_Ｂと、トルクセンサ１のトルクのオフセット値τ_offsetとを同時に同定する。

同様に、同定部３１は、トルクセンサ１により計測された多関節ロボット１００の各関節のトルクτ_measuredと、力センサ２により計測された外力Ｆ_{k_measured}と、上述のようにして導出した多関節ロボット１００のベースリンクに関する運動方程式（６）式と、に基づいて、この運動方程式の最小力学パラメータφ_Ｂと、トルクセンサ１のトルクのオフセット値τ_offsetと、力センサ２の外力のオフセット値Ｆ_{k_offset}と、を同時に同定する。

このように、多関節ロボット１００が任意の姿勢で動作中にもかかわらず、未知のリンクの質量、重心位置、慣性テンソルなどの最小力学パラメータφ_Ｂと、トルクセンサ１のオフセット値τ_offsetとを同時に同定することができる。したがって、誤差を含む換算したトルク値を用いることなく、高精度に最小力学パラメータを同定でき、さらに、同時に、トルクセンサ１のオフセット値τ_offsetを同定することで、校正の作業時間を大幅に短縮することができる。

同定部３１は、補正部３２に接続されており、同定したトルクセンサ１のオフセット値τ_offsetと力センサ２のオフセット値Ｆ_{k_offset}とを、補正部３２に対して出力する。

補正部３２は補正手段の一具体例であり、同定部３１からのトルクセンサ１のオフセット値τ_offsetと、トルクセンサ１の計測値τ_measuredと、上記（４）式と、に基づいて、トルクセンサ１の計測値を補正した真値τを算出する。

同様に、補正部３２は、同定部３１からの力センサ２のオフセット値Ｆ_{k_offset}と、力センサ２の計測値Ｆ_{k_measured}と、上記（５）式と、に基づいて、力センサ２の計測値を補正した真値Ｆ_kを算出する。

例えば、補正部３２は、補正後のトルクセンサ１の真値τと力センサ２の真値Ｆ_kとを、多関節ロボット１００の各関節の回転駆動を制御する制御装置４に対し出力してもよい（図１）。これにより、リアルタイムにトルクセンサ１及び力センサ２の計測値を補正した真値τ及び真値Ｆ_kに基づいて、より高精度に多関節ロボット１００を制御することができる。

次に、本実施形態に係るトルクセンサ校正装置１０による校正方法について、詳細に説明する。図２は、本実施形態に係るトルクセンサ校正装置の処理フローの一例を示すフローチャートである。なお、図２に示す処理は、所定時間毎に繰り返し実行される。

まず、トルクセンサ１は、任意の姿勢にある多関節ロボット１００の関節のトルクτ_measuredを計測し（ステップＳ１０１）、校正装置３に対して出力する。同様に、力センサ２は、任意の姿勢にある多関節ロボット１００に作用する外力Ｆ_{k_measured}を計測し（ステップＳ１０２）、校正装置３に対して出力する。

校正装置３の同定部３１は、トルクセンサ１により計測された多関節ロボット１００の各関節のトルクτ_measuredと、力センサ２により計測された外力Ｆ_{k_measured}と、上述の多関節ロボット１００のベースリンクに関する運動方程式（６）式と、に基づいて、運動方程式の最小力学パラメータφ_Ｂと、トルクセンサ１のトルクのオフセット値τ_offsetと、力センサ２の外力のオフセット値Ｆ_{k_offset}と、を同時に同定し（ステップＳ１０３）、同定したトルクセンサ１のオフセット値τ_offsetと力センサ２のオフセット値Ｆ_{k_offset}とを、補正部３２に対して出力する。

補正部３２は、同定部３１からのトルクセンサ１のオフセット値τ_offsetと、トルクセンサ１の計測値τ_measuredと、上記（４）式と、に基づいて、トルクセンサ１の計測値を補正した真値τを算出し（ステップＳ１０４）、制御装置４に対して出力する。また、補正部３２は、同定部３１からの力センサ２のオフセット値Ｆ_{k_offset}と、力センサ２の計測値Ｆ_{k_measured}と、上記（５）式と、に基づいて、力センサ２の計測値を補正した真値Ｆ_kを算出し（ステップＳ１０５）、制御装置４に対して出力する。

制御装置４は、補正部３２から出力されたトルクセンサ１の真値τと、力センサ２の真値Ｆ_kと、に基づいて、多関節ロボット１００のアクチュエータを制御する（ステップＳ１０６）。

以上、本実施形態に係るトルクセンサ校正装置１０によれば、多関節ロボット１００のベースリンクに関する運動方程式を用いて、多関節ロボット１００の質量、重心位置、及び慣性テンソルなどの最小力学パラメータφ_Ｂと、多関節ロボット１００に搭載されたトルクセンサ１のオフセット値τ_offsetとを同時に同定することで、多関節ロボット１００に搭載されたトルクセンサ１の校正を高精度かつ容易に行うことができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施形態において、同定部３１は、多関節ロボット１００のベースリンクに関する運動方程式の最小力学パラメータφ_Ｂと、トルクセンサ１のオフセット値τ_offsetと、力センサ２のオフセット値Ｆ_{k_offset}と、を同時に同定しているが、これに限らず、最小力学パラメータφ_Ｂと、トルクセンサ１のオフセット値τ_offsetと、多関節ロボット１００に搭載され、力やトルクに関係する任意のセンサのオフセット値と、を同時に同定してもよい。

また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、校正装置３の同定部３１及び補正部３２が実行する処理（図２に示す処理）を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１トルクセンサ
２力センサ
３校正装置
４制御装置
１０トルクセンサ校正装置
３１同定部
３２補正部
１００多関節ロボット

Claims

複数のリンクと該各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットに搭載され、前記関節のトルクを計測するトルクセンサと、
前記多関節ロボットの接触点に作用する外力を計測する力センサと、
前記トルクセンサにより計測された前記関節のトルクと、前記力センサにより計測された前記外力と、前記多関節ロボットのベースリンクに関する運動方程式と、に基づいて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する同定手段と、を備える、ことを特徴とするトルクセンサ校正装置。
請求項１記載のトルクセンサ校正装置であって、
前記同定手段は、前記多関節ロボットのベースリンクに関する運動方程式である下記式を用いて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する、ことを特徴とするトルクセンサ校正装置。

但し、前記式において、φ_Ｂは前記最小力学パラメータ、τ_measuredは前記トルクセンサにより計測された前記関節のトルク、τ_offsetは前記トルクセンサのオフセット値、Ｙ_Ｂ１及びＹ_Ｂ２は、ベースリンクの一般化座標、各関節角度及び該各関節角度の速度、加速度、から求められる観測行列、Ｆ_{k_measured}は前記力センサにより計測される接触点ｋに作用する外力、Ｋ_ｋ１及びＫ_ｋ２は接触点ｋにおける前記外力を一般化力へ変換する行列、Ｎ_ｃは前記多関節ロボットと周囲環境との接触点ｋの総数、及び、Ｉは単位行列、を表している。
請求項１記載のトルクセンサ校正装置であって、
前記同定手段は、前記多関節ロボットのベースリンクに関する運動方程式である下記式を用いて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、前記力センサの力のオフセット値と、を同時に同定する、ことを特徴とするトルクセンサ校正装置。

但し、前記式において、φ_Ｂは最小力学パラメータ、τ_measuredは前記トルクセンサにより計測された前記関節のトルク、τ_offsetは前記トルクセンサのオフセット値、Ｙ_Ｂ１及びＹ_Ｂ２は、ベースリンクの一般化座標、各関節角度及び該各関節角度の速度、加速度、から求められる観測行列、Ｆ_{k_measured}は前記力センサにより計測される接触点ｋに作用する前記外力、Ｋ_ｋ１及びＫ_ｋ２は接触点ｋにおける外力を一般化力へ変換する行列、Ｆ_{1_offset}〜Ｆ_{Nc_offset}は前記力センサの外力のオフセット値、Ｎ_ｃは前記多関節ロボットと周囲環境との接触点ｋの総数、及び、Ｉは単位行列、を表している。
請求項１乃至３のうちいずれか１項記載のトルクセンサ校正装置であって、
前記トルクセンサにより計測された前記関節のトルクと、前記同定手段により同定された前記トルクセンサのオフセット値と、に基づいて、前記トルクセンサのトルクを補正する補正手段を更に備える、ことを特徴とするトルクセンサ校正装置。
請求項４記載のトルクセンサ校正装置であって、
前記補正手段は、前記力センサにより計測された前記外力と、前記同定手段により同定された前記力センサのオフセット値と、に基づいて、前記力センサの外力を補正する、ことを特徴とするトルクセンサ校正装置。
請求項１乃至５のうちいずれか１項記載のトルクセンサ校正装置であって、
前記同定手段は、最小二乗法を用いて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同定する、ことを特徴とするトルクセンサ校正装置。
請求項１乃至６のうちいずれか１項記載のトルクセンサ校正装置であって、
前記同定手段により同定された前記最小力学パラメータにより制御パラメータを補正する、ことを特徴とするトルクセンサ校正装置。
複数のリンクと該各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットの前記関節のトルクを計測する工程と、
前記多関節ロボットの接触点に作用する外力を計測する工程と、
前記計測された関節のトルクと、前記計測された外力と、前記多関節ロボットのベースリンクに関する運動方程式と、に基づいて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する工程と、を含む、ことを特徴とする校正方法。
複数のリンクと該各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットの前記関節のトルクと、前記多関節ロボットの接触点に作用する外力と、前記多関節ロボットのベースリンクに関する運動方程式と、に基づいて、前記運動方程式の最小力学パラメータと、前記トルクセンサのトルクのオフセット値と、を同時に同定する処理を、コンピュータに実行させることを特徴とするプログラム。