JP5732985B2 - トルク算出装置、トルク算出方法、及びプログラム - Google Patents

Description

本発明は、多関節ロボットにおける各関節のトルクを高精度に算出できるトルク算出装置、トルク算出方法、及びプログラムに関するものである。

ヒューマノイドロボットなどの多関節ロボットにおいて各関節をアクチュエータにより駆動するために、大電流を流すための太いハーネスや多くの制御信号を伝達するための多数のハーネスが各関節を跨ぐように設けられている。また、各関節が回転駆動するとハーネスは屈曲し、そのハーネスの屈曲に起因したトルクが生じる。このハーネスの屈曲によるトルクは、関節トルクを計測したい場合、それと比較して小さいものとは言えず、計測誤差として現れる。また、トルクセンサは、一般的に減速機の出力部に配置され、減速機内の摩擦力の影響を受けずに外部からのトルクを計測できるが、ハーネスはトルクセンサの外側に装着される。このため、ハーネスによるトルクは、トルクセンサの計測値に含まれることとなる。

上記問題に対して、例えば、ケーブルの重量及び硬さによる影響を排除してモータを制御する装置が知られている（特許文献１参照）。この装置においては、モータの回転角度と、そのときのケーブルによる重量変動分を補償するのに必要なケーブル重量補償トルクとの関係を示すテーブルを用いてモータを制御している。

特開平０５−２６１６８５号公報

しかしながら、上記特許文献１に示す装置においては、例えば、ロボットの姿勢が変化してハーネスが鉛直方向からずれる場合にその重力の影響が誤差となり得る。また、ロボットの姿勢が速度や加速度を有して変化する場合に、その慣性力、遠心力、コリオリ力などの影響が誤差となり得る。

本発明は、このような問題点を解決するためになされたものであり、多関節ロボットにおける各関節のトルクを高精度に算出できるトルク算出装置、トルク算出方法、及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数のリンクと各リンクを回転可能に連結する複数の関節と、前記関節を跨ぐようにして配設されるハーネスと、を有する多関節ロボットに所望の動作を行わせるための前記関節のトルクを算出するトルク算出装置であって、一方側の前記リンクと他方側の前記リンクとの間の前記関節を跨ぐ前記ハーネスの形状を直線近似し、前記直線近似されたハーネスの形状に従って、前記一方側のリンクと他方側のリンクとに前記ハーネスを夫々分割する近似分割手段と、前記近似分割手段により直線近似され分割されたハーネスの重心位置を算出する重心算出手段と、前記各リンクの質量と前記各リンクに分割されたハーネスの質量とを加算し、補正した前記各リンクの質量を算出し、前記各リンクの質量及び重心位置と、前記各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、補正した各リンクの重心位置を算出する補正手段と、前記補正手段により算出された前記補正した各リンクの質量及び重心位置に基づいて、前記多関節ロボットの逆運動力学演算を行い、前記各関節のトルクを算出するトルク算出手段と、を備える、ことを特徴とするトルク算出装置である。本態様によれば、多関節ロボットにおける各関節のトルクを高精度に算出できる。

この一態様において、前記近似分割手段は、前記関節を跨ぐハーネスの両端が前記一方側及び他方側のリンクに夫々固定される固定位置を結ぶ直線の垂直二等分線を生成し、前記ハーネスの固定位置と前記垂直二等分線上の点とを結び、前記ハーネスの長さが１／２となる直線に、前記ハーネスを直線近似し分割してもよい。

この一態様において、前記重心算出手段は、直線近似され分割されたハーネスの中点を求め、前記中点を前記重心位置としてもよい。

この一態様において、前記各関節の回転角度と、前記ハーネスの屈曲時の弾性力又は摩擦力によって発生する前記ハーネスのトルクと、の関係を示す参照テーブルを記憶する記憶手段を更に備え、前記トルク算出手段は、前記参照テーブルを用いて算出した前記ハーネスの弾性力又は摩擦力によるトルクと、前記逆運動力学演算を行って算出した前記各関節のトルクとを加算して、最終的な前記各関節のトルクを算出してもよい。これにより、ハーネスによる重力、慣性力、遠心力、コリオリ力だけでなく、ハーネスの屈曲による弾性力又は摩擦力をも考慮した、より高精度な各関節のトルクを算出することができる。

この一態様において、前記トルク算出手段により算出された前記各関節のトルクに基づいて、前記各関節を制御する制御手段を更に備えていてもよい。

この一態様において、前記トルク算出手段により算出された前記各関節のトルクに基づいて、前記各関節のトルクの校正を行う校正手段を更に備えていてもよい。

他方、上記目的を達成するための本発明の一態様は、複数のリンクと各リンクを回転又は伸縮可能に連結する複数の関節と、前記関節を跨ぐようにして配設されるハーネスと、を有する多関節ロボットに所望の動作を行わせるための前記関節のトルクを算出するトルク算出方法であって、一方側の前記リンクと他方側の前記リンクとの間の前記関節を跨ぐ前記ハーネスの形状を直線近似し、前記直線近似されたハーネスの形状に従って、前記一方側のリンクと他方側のリンクとに前記ハーネスを夫々分割するステップと、前記直線近似され分割されたハーネスの重心位置を算出するステップと、前記各リンクの質量と前記各リンクに分割されたハーネスの質量とを加算し、補正した前記各リンクの質量を算出し、前記各リンクの質量及び重心位置と、前記各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、補正した各リンクの重心位置を算出するステップと、前記補正した各リンクの質量及び重心位置に基づいて、前記多関節ロボットの逆運動力学演算を行い、前記各関節のトルクを算出するステップと、を含む、ことを特徴とするトルク算出方法であってもよい。

また、上記目的を達成するための本発明の一態様は、複数のリンクと各リンクを回転又は伸縮可能に連結する複数の関節と、前記関節を跨ぐようにして配設されるハーネスと、を有する多関節ロボットに所望の動作を行わせるための前記関節のトルクを算出するプログラムであって、一方側の前記リンクと他方側の前記リンクとの間の前記関節を跨ぐ前記ハーネスの形状を直線近似し、前記直線近似されたハーネスの形状に従って、前記一方側のリンクと他方側のリンクとに前記ハーネスを夫々分割する処理と、前記直線近似され分割されたハーネスの重心位置を算出する処理と、前記各リンクの質量と前記各リンクに分割されたハーネスの質量とを加算し、補正した前記各リンクの質量を算出し、前記各リンクの質量及び重心位置と、前記各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、補正した各リンクの重心位置を算出する処理と、前記補正した各リンクの質量及び重心位置に基づいて、前記多関節ロボットの逆運動力学演算を行い、前記各関節のトルクを算出する処理と、をコンピュータに実行させることを特徴とするプログラムであってもよい。

本発明によれば、多関節ロボットにおける各関節のトルクを高精度に算出できるトルク算出装置、トルク算出方法、及びプログラムを提供することができる。

本発明の実施の形態１に係るトルク算出装置の概略的なシステム構成を示すブロック図である。 ３次元座標上でモデル化された多関節ロボットの一例を示す図である。 各リンク間の関節を跨ぐ曲線状のハーネスをモデル化した一例を示す図である。 各関節のトルクを算出する方法を説明するための図である。 本発明の実施の形態１に係るトルク算出方法の一例を示すフローチャートである。 各関節の回転角度とハーネスの屈曲時の弾性力によって発生するハーネスのトルクとの関係を示す参照テーブルの一例を示す図である。 ３次元空間内においてハーネスをモデル化する方法を説明するための図である。 ３次元空間内においてハーネスをモデル化する方法を説明するための図である。 ３次元空間内においてハーネスをモデル化する方法を説明するための図である。 偏って湾曲しているハーネスをモデル化する方法を説明するための図である。 複数の関節を跨ぐハーネスをモデル化する方法を説明するための図である。

実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。複数のリンクと各リンクを回転又は伸縮可能に連結する複数の関節と、を有する多関節ロボットにおいて、各リンク間には関節を跨ぐようにしてハーネスが設けられている。本発明の実施の形態１に係るトルク算出装置１０は、各関節を跨ぐハーネスによる重力、慣性力、遠心力、コリオリ力などの影響を考慮して、多関節ロボットに所望の動作を行わせるための各関節のトルクを高精度に算出することができる。

トルク算出装置１０は、例えば、ハーネス形状の簡易モデル化し、各リンク間の関節を跨ぐハーネスの影響を考慮した、各リンクの重心位置及び質量を算出し、各リンク間の関節のトルクを算出する。

なお、本実施の形態１においては、例えば、２次元の簡易モデルを利用して各リンク間の関節を跨ぐハーネスの影響を考慮した各リンク間の関節のトルクを算出する。本手法は、例えば、「２次元平面内で動作するスカラーロボット」や「３次元で動作するロボットであって、ハーネスが跨いでいる部分を２次元平面に投影可能なロボット」などに利用できる簡易なモデル化手法であり、計算量を効果的に減少させることができる。

図１は、本実施の形態１に係るトルク算出装置の概略的なシステム構成を示すブロック図である。トルク算出装置１０は、近似分割部１と、重心算出部２と、補正部３と、トルク算出部４と、を備えている。なお、トルク算出装置１０は、例えば、演算処理、制御処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。

多関節（多リンク）ロボットの一部の構成は、図２に示すように、３次元座標（Ｘ_ｉ、Ｙ_ｉ、Ｚ_ｉ）上でモデル化して表わすことができる。

近似分割部１は、近似分割手段の一具体例であり、一方側のリンクｋと他方側のリンクｋ−１、ｋ＋１との間の関節１１を跨ぐハーネスの形状を直線近似し、直線近似されたハーネスの形状に従って、一方側のリンクｋと他方側のリンクｋ−１、ｋ＋１とにハーネスを夫々分割する。

ここで、各リンクｋ−１、ｋ、ｋ＋１間の関節１１を跨ぐ曲線状のハーネスを、例えば、図３に示すように略中央で屈曲する直線によって近似することができる。このように、近似分割部１は、図３に示すように、各リンクｋ−１、ｋ、ｋ＋１間の各関節１１を跨ぐ曲線状のハーネスの形状に従って直線近似する。

例えば、一方側のリンクｋの重心位置をｓ_ｋとし、一方側のリンクｋの質量をｍ_ｋとする。また、他方側のリンクｋ−１と一方側のリンクｋ間の関節１１を跨ぐハーネスＨｋの質量をｍ_Ｈｋとし、そのハーネスＨｋの長さをＬ_Ｈｋとする。同様に、一方側のリンクｋと他方側のリンクｋ＋１間の関節１１を跨ぐハーネスＨｋ＋１の質量をｍ_Ｈｋ＋１とし、そのハーネスＨｋ＋１の長さをＬ_Ｈｋ＋１とする。

次に、近似分割部１は、直線近似されたハーネスの形状に従って、ハーネスＨｋ、Ｈｋ＋１を、例えば、二等分に夫々分割する。ここで、分割されたハーネスＨｋの質量ｍ_Ｈｋ／２とハーネスＨｋ＋１の質量ｍ_Ｈｋ＋１／２とが、夫々リンクｋに属すると考える。

このように、近似分割部１は、一方側のリンクｋと他方側のリンクｋ−１、ｋ＋１との間の各関節１１を跨ぐ各ハーネスＨｋ、Ｈｋ＋１の形状を直線近似し、直線近似された各ハーネスＨｋ、Ｈｋ＋１の形状に従って、一方側のリンクｋと他方側のリンクｋ−１、ｋ＋１とに各ハーネスＨｋ、Ｈｋ＋１を夫々分割する。

より具体的には、まず、近似分割部１は、ハーネスＨｋ、Ｈｋ＋１の両端が夫々固定された固定位置Ｐを結んだ直線に対して垂直二等分線Ａを生成する。ここで、固定位置Ｐは、図２に示す３次元座標系でＲＯＭやＲＡＭに予め設定されている。

次に、近似分割部１は、一端がハーネスＨｋ、Ｈｋ＋１の固定位置Ｐであり、他端が垂直二等分線Ａ上の点Ｑであり、その長さがＬ_Ｈｋ／２、Ｌ_Ｈｋ＋１／２となる直線Ｂに、ハーネスＨｋ、Ｈｋ＋１を夫々直線近似し、分割する。

重心算出部２は、重心算出手段の一具体例であり、近似分割部１により直線近似され分割されたハーネスの重心位置を算出する。

例えば、重心算出部２は、近似分割部１により直線近似され分割された直線Ｂの中点を算出し、この中点にハーネスＨｋ、Ｈｋ＋１の半分の質量ｍ_Ｈｋ／２、ｍ_Ｈｋ＋１／２が夫々あるとして、この中点を分割されたハーネスＨｋ、Ｈｋ＋１の重心位置ｓ_{Ｈｋ_ｋ}、ｓ_{Ｈｋ＋１_ｋ}とする。

以上のようにして、トルク算出装置１０の近似分割部１は、ハーネスＨｋ、Ｈｋ＋１の形状に従って、ハーネスＨｋ、Ｈｋ＋１を直線近似し分割を行い、重心算出部２は、その直線近似され分割されたハーネスＨｋ、Ｈｋ＋１の重心位置ｓ_{Ｈｋ_ｋ}、ｓ_{Ｈｋ＋１_ｋ}を夫々算出する。

ここで、分割されたハーネスＨｋ、Ｈｋ＋１をリンクｋと統合することで、補正した新たなリンクｋ１（以下、補正リンクｋ１と称す）を定義する。

補正部３は、補正手段の一具体例であり、各リンクの質量と各リンクに分割されたハーネスの質量とを加算し、各補正リンクの質量を算出し、各リンクの質量及び重心位置と、各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、各補正リンクの重心位置を算出する。

補正部３は、例えば、リンクｋの質量ｍ_ｋと、そのリンクｋに分割されたハーネスの質量ｍ_Ｈｋ／２、ｍ_Ｈｋ＋１／２と、を加算し、補正リンクｋ１の質量ｍ_ｋ１を、下記（１）式を用いて算出する。

また、補正部３は、リンクｋの質量ｍ_ｋ及び重心位置ｓ_ｋと、分割されたハーネスＨｋ、Ｈｋ＋１の質量ｍ_Ｈｋ／２、ｍ_Ｈｋ＋１／２及び重心位置ｓ_{Ｈｋ_ｋ}、ｓ_{Ｈｋ＋１_ｋ}と、に基づいて、補正リンクｋ１の重心位置ｓ_ｋ１を、下記（２）式を用いて算出する。

トルク算出部４は、トルク算出手段の一具体例であり、補正部３により算出された補正リンクｋ１の質量ｍ_ｋ１及び重心位置ｓ_ｋ１を用いて、後述のように、多関節ロボットの逆運動力学演算を行うことで、所望の運動を行うために必要な各関節のトルクτ_ｉをハーネスによって発生するトルクを考慮して算出する。

これにより、各リンク間の関節１１を跨ぐハーネスによる重力、慣性力、遠心力、コリオリ力等の影響を考慮して、各関節１１のトルクを高精度に算出することができる。なお、上記一例では、リンクｋに関して説明を行ったが、リンクｋ−１、ｋ＋１に関しても、リンクｋと同様に補正リンクｋ１−１、ｋ１＋１の質量ｍ_ｋ１−１、ｍ_ｋ１＋１及び重心位置ｓ_ｋ１−１、ｓ_ｋ１＋１を夫々求めることができる。また、トルク算出部４は、ハーネスによって発生するトルクのみを算出する場合は、下記（３）式及び（４）式を用いて、逆運動力学演算を行ってもよい。

トルク算出部４は、逆運動力学演算において、上記算出された各補正リンクｋ１の質量ｍ_ｋ１、重心位置ｓ_ｋ１、及び慣性テンソルを利用して、多関節ロボットが所望の動作を行うために必要な各関節１１のトルクを算出する。例えば、各関節１１が回転関節でありニュートン・オイラー法を用いて各関節１１のトルクを求める場合に、以下のようにして各関節１１のトルクを算出することができる。

なお、トルク算出部４は、ラグランジュ運動方程式などを用いて各関節１１のトルクを求めることもできるが、これらの方法に限らず任意の演算手法を用いることができる。例えば、トルク算出部４は、「重力ベクトル」と「関節からのその関節より末端側のリンクの重心位置までのベクトル」との外積を用いて各関節１１のトルクを算出してもよい。

まず、トルク算出部４は、例えば、体幹などに設定される根元側のリンクより順に、すなわち、ｉ＝１、２、・・・、ｎの順に下記（５）式〜（１０）式を演算する。次に、トルク算出部４は、先端側のリンクより順に、すなわち、ｉ＝ｎ、ｎ−１、・・・、１の順に下記（１１）式〜（１３）式を演算し、各関節１１のトルクτ_ｉを算出する（図４）。

図５は、本実施の形態１に係るトルク算出方法の一例を示すフローチャートである。
トルク算出装置１０の近似分割部１は、一方側のリンクｋと他方側のリンクｋ−１、ｋ＋１との間の関節１１を跨ぐハーネスＨｋ、Ｈｋ＋１の形状を夫々直線近似し、直線近似された各ハーネスＨｋ、Ｈｋ＋１の形状に従って、一方側のリンクｋと他方側のリンクｋ−１、ｋ＋１とにハーネスＨｋ、Ｈｋ＋１を夫々分割する（ステップＳ１０１）。

次に、重心算出部２は、近似分割部１により直線近似され分割されたハーネスＨｋ、Ｈｋ＋１の重心位置ｓ_{Ｈｋ_ｋ}、ｓ_{Ｈｋ＋１_ｋ}を夫々算出する（ステップＳ１０２）。

その後、補正部３は、各リンクｋの質量ｍ_ｋと各リンクｋに分割されたハーネスＨｋ、Ｈｋ＋１の質量ｍ_Ｈｋ／２、ｍ_Ｈｋ＋１／２とを上記（１）式を用いて加算し、各補正リンクｋ１の質量ｍ_ｋ１を算出する（ステップＳ１０３）。さらに、補正部３は、各リンクｋの質量ｍ_ｋ及び重心位置ｓ_ｋと、各リンクｋに分割されたハーネスＨｋ、Ｈｋ＋１の質量ｍ_Ｈｋ／２、ｍ_Ｈｋ＋１／２及び重心位置ｓ_{Ｈｋ_ｋ}、ｓ_{Ｈｋ＋１_ｋ}と、に基づいて、上記（２）式を用いて各補正リンクｋ１の重心位置ｓ_ｋ１を算出する（ステップＳ１０４）。

次に、トルク算出部４は、補正部３により算出された各補正リンクｋ１の質量ｍ_ｋ１及び重心位置ｓ_ｋ１に基づいて、上記（５）〜（１３）式を用いて多関節ロボットの逆運動力学演算を行い、各関節１１のトルクτ_ｉを算出する（ステップＳ１０５）。さらに、例えば、トルク算出装置１０の制御部（制御手段）５は、トルク算出部４により算出された各関節１１のトルクを用いて、各関節１１を駆動するアクチュエータを制御してもよく、また、校正部（校正手段）６は、トルク算出部４により算出された各関節１１のトルクを用いて、各関節１１のトルクの校正を行っても良い（ステップＳ１０６）。

以上、本実施の形態１に係るトルク算出装置１０によれば、各リンク間の各関節１１を跨ぐハーネスによる重力、慣性力、遠心力、コリオリ力などの影響を考慮して、多関節ロボットに所望の動作を行わせるための各関節１１のトルクを算出することができる。すなわち、多関節ロボットにおける各関節のトルクを高精度に算出できる。

実施の形態２．
本発明の実施の形態２に係るトルク算出装置１０において、トルク算出部４は、上記実施の形態１のように、各関節１１を跨ぐハーネスによる重力、慣性力、遠心力、コリオリ力などの影響を考慮するだけでなく、各関節１１の回転角度に依存して発生するハーネスの屈曲による弾性力は考慮して、各関節１１のトルクを算出する。

ここで、各関節１１の回転角度に依存して発生するハーネスのトルクは、各関節１１の回転に伴って、各関節１１を跨ぐハーネスの引張方向やせん断方向の力によって発生する。したがって、例えば、ハーネスが中空軸受け内部を通る場合や、ハーネスが硬い場合に、ハーネスの屈曲による弾性力が大きく各関節１１のトルクに影響することとなる。

例えば、トルク算出装置１０のＲＯＭやＲＡＭなど記憶部（記憶手段）に、各関節１１の回転角度θと、ハーネスの屈曲時の弾性力によって発生するハーネスのトルクと、の関係を示す参照テーブル（図６）を予め設定する。トルク算出部４は、関節１１の任意の回転角度に対して発生するハーネスのトルクを、例えば、参照テーブル上の直近の関節１１の回転角度と、その回転角度で発生するハーネスのトルクと、に基づいて、直線補間等を用いて算出する。

トルク算出部４は、上述のようにして算出したハーネスのトルクを、上記実施の形態１に係るトルク算出方法で算出した各関節１１のトルクに加算することで、ハーネスによる重力、慣性力、遠心力、コリオリ力だけでなく、ハーネスの屈曲による弾性力をも考慮した、より高精度な各関節１１のトルクを算出することができる。

なお、トルク算出部４は、各関節１１の回転角度に依存して発生するハーネスの屈曲による弾性力は考慮して各関節１１のトルクを算出しているが、これに限らず、例えば、各関節１１の回転角度に依存して発生するハーネスの屈曲による摩擦力を考慮して各関節１１のトルクを上記同様の方法で算出してもよい。

実施の形態３．
上記実施の形態１においては、２次元の簡易モデルを利用して各リンク間の関節１１を跨ぐハーネスの影響を考慮した各リンク間の関節１１のトルクを算出しているが、本実施の形態３においては、３次元の簡易モデルを利用して、各リンク間の関節１１を跨ぐハーネスの影響を考慮した各リンク間の関節１１のトルクをより高精度に算出する。本手法は、３次元空間内で動作する任意のロボットに適用することができる。

上記実施の形態１において、生成された垂直二等分線Ａは、３次元空間内では無限に存在することとなる。そこで、以下に述べる手法を用いて、１つの直線に決定する。

まず、図７に示す如く、関節位置を代表する点（例えば、関節軸の中心）をａとし、各ハーネスの固定位置をｂ及びｃとすると、近似分割部１は、ベクトルａｂとベクトルａｃの両者に垂直なベクトル（図７では紙面垂直）を求める。次に、近似分割部１は、この垂直ベクトルと直線ｂｃの両方に垂直で、かつ、直線ｂｃの中点を通る直線を、直線Ａ（上記実施の形態１の垂直二等分線Ａに対応する）とする。本実施の形態３において、以降の方法は、上記実施の形態１と同一であるため、詳細な説明は省略する。

なお、近似分割部１は、上記直線ａｂの代わりに、予め設定された各リンクの代表的なベクトルを利用してもよい。例えば、近似分割部１は、関節１１間を結ぶベクトル（図８におけるベクトルａｄ、ベクトルａｅなど）や、リンクの特定の辺を構成している直線（図９におけるベクトルｆｇ、ベクトルｈｉ）などを利用してもよい。

実施の形態４．
本実施の形態４において、各リンク間の関節１１を跨ぐハーネスが偏って湾曲している場合における上記簡易モデルの生成方法について、詳細に説明する。例えば、図１０に示す如く、ハーネスの固定位置ｂ及びｃの様に取り付け方向が異なることが原因でハーネスが均等ではない形状になる場合（一方側又は他方側のリンクにハーネスが偏っている場合）に、本手法を用いるのが好ましい。これにより、ハーネスのモデル化の精度が向上し、より高精度に、ハーネスによる重力、慣性力、遠心力、コリオリ力などの影響を考慮して関節トルクを算出することができる。

まず、近似分割部１は、直線ｂｃを分割する点ｊを、ハーネスの湾曲状態に基づいて適当な位置に設定する。ここで、上記実施の形態１に示すように２次元平面内を想定する場合は、近似分割部１は、上記垂直二等分線Ａの代わりに、点ｊを通り直線ｂｃに垂直な直線Ａを利用する。また、実施の形態３に示す３次元空間内を想定する場合は、近似分割部１は、点ｊを通り、ベクトルｂｃと、ベクトルａｂ及びベクトルａｃに垂直なベクトルと、に垂直な直線Ａを利用する。

次に、近似分割部１は、直線Ａ上の点ｋを、直線ｂｋの長さＬ_ｂｋと直線ｃｋの長さＬ_ｃｋの合計がＬ_Ｈｋがとなるように選択する。
（Ｌ_ｂｊ ^２＋Ｌ_ｋｊ ^２）^１／２＋（Ｌ_ｃｊ ^２＋Ｌ_ｋｊ ^２）^１／２＝Ｌ_Ｈｋ （１４）式

ここで、上記（１４）式の関係を用いて、既知のＬ_ｂｊ、Ｌ_ｃｊ、及びＬ_ＨｊからＬ_ｊｋの長さを決定することができる。なお、上記実施の形態１乃至３においては、分割された各ハーネスの中点にハーネスの半分の質量が夫々存在するとしたが、本実施の形態４においては、分割されたハーネスＨ_ｂｋ、Ｈ_ｃｋの長さの比によって各ハーネスＨ_ｂｋ、Ｈ_ｃｋの質量を夫々決定する。

すなわち、直線ｂｋの中点である重心位置に質量Ｌ_ｂｋｍ_Ｈｋ／Ｌ_Ｈｋが存在し、直線ｃｋの中点である重心位置に質量Ｌ_ｃｋｍ_Ｈｋ／Ｌ_Ｈｋが存在する。このようにして、近似分割部１は、各関節１１を跨ぐハーネスの湾曲形状を直線近似し、直線近似されたハーネスの形状に従って、一方側のリンクと他方側のリンクとにハーネスを夫々分割し、分割された各ハーネスの質量を夫々求める。本実施の形態４において、以降の方法は、上記実施の形態１と略同一であるため詳細な説明は省略する。

実施の形態５．
上記実施の形態１乃至４においては、単一の関節１１を跨ぐハーネスをモデル化する方法について説明したが、本実施の形態５においては、複数の関節１１を跨ぐハーネスをモデル化する方法について説明する（図１１）。

なお、２次元平面内においては、上記実施の形態１に示す方法を用いて、複数の関節１１を跨ぐハーネスのモデル化を行うことができる。また、３次元空間内においてハーネスのモデル化を行う場合は、図１１に示す如く、ハーネスの固定位置ｂ、ｃの中間付近に位置するリンク上の点を代表点ａとして設定し、上記実施の形態３と同様の方法でモデル化を行うことができる。

ここで、多関節ロボットにおいては、２つ又は３つの関節１１の回転軸が１箇所で交わるように設計されている場合が多いが、その場合はその交点を代表点ａに設定することができる。さらに、ハーネスが偏って湾曲している場合（図１０）、上記実施の形態４の方法を用いて、代表点ａを設定し、複数の関節１１を跨ぐハーネスをモデル化できる。これにより、ハーネスのモデル化の精度が向上し、より高精度に、ハーネスによる重力、慣性力、遠心力、コリオリ力などの影響を考慮して関節トルクを算出することができる。なお、本実施の形態５において、以降の方法は上記実施の形態１と略同一であるため、詳細な説明は省略する。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

例えば、上記実施の形態において、複数のリンクと各リンクを回転可能に連結する複数の関節と、を有する多関節ロボットに適用されているが、これに限らず、各リンクを伸縮可能に連結する直動関節を有する多関節ロボットに対しても上記同様に適用可能である。

上記実施の形態において、直線近似を行いハーネスの重心位置を決定し、各リンクの重心を補正しているが、これと同時に、慣性テンソルの補正を行ってもよい。これにより、より高精度に関節トルクを算出することができる。

また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、図５に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

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

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

１ 近似分割部
２ 重心算出部
３ 補正部
４ トルク算出部
５ 制御部
６ 校正部
１０ トルク算出装置
１１ 関節

Claims (8)

1. 複数のリンクと各リンクを回転又は伸縮可能に連結する複数の関節と、前記関節を跨ぐようにして配設されるハーネスと、を有する多関節ロボットに所望の動作を行わせるための前記関節のトルクを算出するトルク算出装置であって、
   一方側の前記リンクと他方側の前記リンクとの間の前記関節を跨ぐ前記ハーネスの形状を直線近似し、前記直線近似されたハーネスの形状に従って、前記一方側のリンクと他方側のリンクとに前記ハーネスを夫々分割する近似分割手段と、
   前記近似分割手段により直線近似され分割されたハーネスの重心位置を算出する重心算出手段と、
   前記各リンクの質量と前記各リンクに分割されたハーネスの質量とを加算し、補正した前記各リンクの質量を算出し、前記各リンクの質量及び重心位置と、前記各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、補正した各リンクの重心位置を算出する補正手段と、
   前記補正手段により算出された前記補正した各リンクの質量及び重心位置に基づいて、前記多関節ロボットの逆運動力学演算を行い、前記各関節のトルクを算出するトルク算出手段と、
   を備える、ことを特徴とするトルク算出装置。
2. 請求項１記載のトルク算出装置であって、
   前記近似分割手段は、前記関節を跨ぐハーネスの両端が前記一方側及び他方側のリンクに夫々固定される固定位置を結ぶ直線の垂直二等分線を生成し、
   前記一方側及び他方側のリンクに固定されるハーネスの固定位置と前記垂直二等分線上の点とを夫々結び、前記ハーネスの長さが１／２となる直線に、前記ハーネスを直線近似し分割する、ことを特徴するトルク算出装置。
3. 請求項１又は２記載のトルク算出装置であって、
   前記重心算出手段は、直線近似され分割されたハーネスの中点を求め、前記中点を前記重心位置とする、ことを特徴するトルク算出装置。
4. 請求項１乃至３のうちいずれか１項記載のトルク算出装置であって、
   前記各関節の回転角度と、前記ハーネスの屈曲時の弾性力又は摩擦力によって発生する前記ハーネスのトルクと、の関係を示す参照テーブルを記憶する記憶手段を更に備え、
   前記トルク算出手段は、前記参照テーブルを用いて算出した前記ハーネスの弾性力又は摩擦力によるトルクと、前記逆運動力学演算を行って算出した前記各関節のトルクとを加算して、最終的な前記各関節のトルクを算出する、ことを特徴とするトルク算出装置。
5. 請求項１乃至４のうちいずれか１項記載のトルク算出装置であって、
   前記トルク算出手段により算出された前記各関節のトルクに基づいて、前記各関節を制御する制御手段を更に備える、ことを特徴とするトルク算出装置。
6. 請求項１乃至５のうちいずれか１項記載のトルク算出装置であって、
   前記トルク算出手段により算出された前記各関節のトルクに基づいて、前記各関節のトルクの校正を行う校正手段を更に備える、ことを特徴とするトルク算出装置。
7. 複数のリンクと各リンクを回転又は伸縮可能に連結する複数の関節と、前記関節を跨ぐようにして配設されるハーネスと、を有する多関節ロボットに所望の動作を行わせるための前記関節のトルクを算出するトルク算出方法であって、
   一方側の前記リンクと他方側の前記リンクとの間の前記関節を跨ぐ前記ハーネスの形状を直線近似し、前記直線近似されたハーネスの形状に従って、前記一方側のリンクと他方側のリンクとに前記ハーネスを夫々分割するステップと、
   前記直線近似され分割されたハーネスの重心位置を算出するステップと、
   前記各リンクの質量と前記各リンクに分割されたハーネスの質量とを加算し、補正した前記各リンクの質量を算出し、前記各リンクの質量及び重心位置と、前記各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、補正した各リンクの重心位置を算出するステップと、
   前記補正した各リンクの質量及び重心位置に基づいて、前記多関節ロボットの逆運動力学演算を行い、前記各関節のトルクを算出するステップと、
   を含む、ことを特徴とするトルク算出方法。
8. 複数のリンクと各リンクを回転又は伸縮可能に連結する複数の関節と、前記関節を跨ぐようにして配設されるハーネスと、を有する多関節ロボットに所望の動作を行わせるための前記関節のトルクを算出するプログラムであって、
   一方側の前記リンクと他方側の前記リンクとの間の前記関節を跨ぐ前記ハーネスの形状を直線近似し、前記直線近似されたハーネスの形状に従って、前記一方側のリンクと他方側のリンクとに前記ハーネスを夫々分割する処理と、
   前記直線近似され分割されたハーネスの重心位置を算出する処理と、
   前記各リンクの質量と前記各リンクに分割されたハーネスの質量とを加算し、補正した前記各リンクの質量を算出し、前記各リンクの質量及び重心位置と、前記各リンクに分割されたハーネスの質量及び重心位置と、に基づいて、補正した各リンクの重心位置を算出する処理と、
   前記補正した各リンクの質量及び重心位置に基づいて、前記多関節ロボットの逆運動力学演算を行い、前記各関節のトルクを算出する処理と、
   をコンピュータに実行させることを特徴とするプログラム。

Images