JP6248544B2 - ロボット、制御装置、ロボットシステム - Google Patents

Description

本発明は、ロボット、制御装置、ロボットシステムに関する。

複数の関節を備えるアームを胴体部に装着し、アームの先端にハンド部などを取り付けることによって、様々な作業を行うロボットが知られている。このようなロボットは、複数の関節を動かしてアームを変形させることによって、アームの先端に取り付けられたハンド部などを目的の位置に移動させて作業を行う。もっとも、アームやハンド部などには重さがあるので、ハンド部などを目的の位置まで移動させて停止させても、暫くの間はアームの先端が振動する。このため、ハンド部などを用いて実際に作業を開始するのは、アームの振動が収まってからになってしまい、作業効率を向上させることが困難となる。

そこで、アームの先端側に慣性センサーを搭載しておき、慣性センサーの出力が小さくなるように、アームの各関節の動きを制御することで、アームの振動を抑制する技術が提案されている（特許文献１）。

特開２０１０−２８４７７０号公報

しかし、上記の提案されている技術では、依然として、アームの振動を十分に抑制することができず、ロボットの作業効率を向上させることが難しいという問題があった。

この発明は、従来の技術が有する上述した課題を解決するためになされたものであり、アームの振動を抑制することで、ロボットの作業効率を向上させることが可能な技術の提供を目的とする。

上述した課題の少なくとも一部を解決するために、本発明のロボットは次の構成を採用した。すなわち、
基台と、
前記基台に対して基台関節部を介して回転可能に設けられ、複数の関節を含む第１関節部を有する第１アームと、
前記基台関節部よりも前記第１アームの先端側で、且つ、前記第１関節部よりも前記第１アームの前記基台側に設けられ、慣性力を検出する基台側慣性センサーと、
前記第１関節部よりも前記第１アームの先端側に設けられ、慣性力を検出する第１慣性センサーと、
前記基台側慣性センサーによって検出された基台側慣性力と、前記第１慣性センサーによって検出された第１慣性力とに基づいて前記第１関節部を制御する制御部と、
を備えることを特徴とする。

こうすれば、第１アームに含まれる複数の関節（第１関節部）を制御する際に、第１アームの先端側で検出された第１慣性力だけでなく、第１アームの基台側で検出された基台側慣性力も用いることができるので、第１アームの振動を十分に抑制することができ、ロボットの作業効率を向上させることが可能となる。

また、上述した本発明のロボットにおいては、第１アームの第１関節部に含まれる複数の関節の角度を検出して、以下のようにして第１関節部を制御しても良い。先ず、第１関節部の複数の関節の角度と、第１アームの基台側で検出された慣性力（基台側慣性力）とを用いて、第１アームの先端側の移動速度（第１移動速度）を演算する。また、第１アームの先端で検出された第１慣性力から、第１アームの先端側の実速度（第１実速度）を演算する。そして、得られた第１実速度と第１移動速度との偏差に基づいて、第１関節部を制御することとしてもよい。

第１実速度と第１移動速度との偏差は、第１アームを振動させる原因となる。従って、第１実速度と第１移動速度との偏差に基づいて第１関節部を制御してやれば、第１アームの振動を抑制することが可能となる。

また、上述した本発明のロボットにおいては、第１実速度と第１移動速度との偏差に基づいて、次のようにして第１関節部を制御してもよい。先ず、第１実速度と第１移動速度との偏差に基づいて、第１関節部での歪み速度である第１歪み速度を演算する。そして、第１関節部の複数の関節についての角度と、第１歪み速度とに基づいて、第１関節部を制御することとしてもよい。

第１歪み速度を抑制すれば、第１アームの振動を抑制することが可能である。また、詳細には後述するが、第１関節部の各関節の動きが第１歪み速度に与える影響は、各関節の角度に依存する。従って、第１歪み速度だけでなく、第１関節部の複数の関節での角度も考慮して第１関節部を制御すれば、第１アームの振動をより一層抑制することが可能となる。

また、上述した本発明のロボットにおいては、第１実速度と第１移動速度との偏差に含まれる変動成分を抽出することによって、第１歪み速度を演算することとしてもよい。

こうすれば、第１実速度と第１移動速度との偏差から、第１アームの振動の原因となる成分を抽出することができるので、第１アームの振動をより一層抑制することが可能となる。

また、上述した本発明のロボットにおいては、基台関節部に含まれる関節の角度を検出し、検出した角度の変化速度に基づいて、第１アームの基台側の移動速度（基台側移動速度）を演算してもよい。そして、第１アームの基台側で実測された慣性力（基台側慣性力）から求められた基台側実速度と、基台側移動速度との偏差に基づいて、基台関節部を制御することとしてもよい。

こうすれば、基台側関節部で生じる振動を抑制することができるので、第１アームの振動をより一層抑制することが可能となる。

また、上述した本発明のロボットにおいては、複数の関節を含む第２関節部を有する第２アームを、基台関節部を介して基台に回転可能に設けることとしてもよい。そして、第２アームの先端側に設けた第２慣性センサーで第２慣性力を検出し、基台側慣性力と第２慣性力とに基づいて第２関節部を制御することとしてもよい。

こうすれば、第１アームと同様な理由から、第２アームについても振動を抑制することが可能となる。

また、第２アームを備える上述した本発明のロボットにおいては、第２関節部に含まれる複数の関節の角度を検出して、以下のようにして第２関節部を制御しても良い。すなわち、第２関節部の複数の関節の角度と、第２アームの基台側で検出された基台側慣性力とを用いて、第２アームの先端側での第２移動速度を演算する。また、第２アームの先端で検出された第２慣性力から、第２アームの先端側での第１実速度を演算する。そして、第２実速度と第２移動速度との偏差に基づいて、第２関節部を制御することとしてもよい。

第２実速度と第２移動速度との偏差は、第２アームを振動させる原因となる。従って、第２実速度と第２移動速度との偏差に基づいて第２関節部を制御してやれば、第２アームについても振動を抑制することが可能となる。

また、上述した本発明は、ロボットを制御する制御装置の態様で把握することもできる。すなわち、本発明は、
基台関節部を介して基台に回転可能に設けられ、複数の関節を含む第１関節部を有する第１アームを備えるロボットの制御装置であって、
前記基台関節部よりも前記第１アームの先端側で、且つ、前記第１関節部よりも前記第１アームの前記基台側に設けられ、慣性力を検出する基台側慣性センサーと、
前記第１関節部よりも前記第１アームの先端側に設けられ、慣性力を検出する第１慣性センサーと、
を備え、
前記基台側慣性センサーによって検出された基台側慣性力と、前記第１慣性センサーによって検出された第１慣性力とに基づいて前記第１関節部を制御することを特徴とする制御装置として把握することができる。

更には、上述した本発明は、ロボットシステムとしての態様で把握することもできる。すなわち、本発明のロボットシステムは、
ロボットと前記ロボットを制御する制御装置とを備えるロボットシステムであって、
前記ロボットは、
基台と、
前記基台に対して基台関節部を介して回転可能に設けられ、複数の関節を含む第１関節部を有する第１アームと、
前記基台関節部よりも前記第１アームの先端側で、且つ、前記第１関節部よりも前記第１アームの前記基台側に設けられ、慣性力を検出する基台側慣性センサーと、
前記第１関節部よりも前記第１アームの先端側に設けられ、慣性力を検出する第１慣性センサーと、
を備えており、
前記制御装置は、前記基台側慣性センサーによって検出された基台側慣性力と、前記第１慣性センサーによって検出された第１慣性力とに基づいて前記第１関節部を制御する制御装置である
ことを特徴とするロボットシステムとして把握することもできる。

上述した本発明の制御装置、ロボットシステムによっても、アームの振動を抑制して、ロボットの作業効率を向上させることが可能となる。

第１実施例のロボットの全体構造を示す説明図である。 第１実施例の制御部がデータをやり取りする様子を示した説明図である。 第１実施例の制御部の内部構成を概念的に示したブロック図である。 第１実施例の第１移動速度演算部についての説明図である。 第１実施例の第１歪み速度演算部についての説明図である。 第１実施例の第１補正速度演算部についての説明図である。 第１実施例の第１モーター駆動部についての説明図である。 制御部が基台側のモーターを制御する動作を示した説明図である。 制御部が実行する制御処理のフローチャートである。 第２実施例のロボットの大まかな構成を示す説明図である。 第２実施例の制御部がデータをやり取りする様子を示した説明図である。 第２実施例の制御部の内部構成を概念的に示したブロック図である。 第２実施例の基台側移動速度演算部についての説明図である。 第２実施例の基台側補正速度演算部についての説明図である。 第２実施例の他の態様のロボットの大まかな構造を示す説明図である。 第３実施例のロボットの大まかな構成を示す説明図である。 第３実施例の制御部がデータをやり取りする様子を示した説明図である。 第３実施例の制御部の内部構成を概念的に示したブロック図である。

Ａ．第１実施例 ：
Ａ−１．第１実施例の装置構成 ：
図１は、第１実施例のロボット１の全体構造を示す説明図である。図１（ａ）には、第１実施例のロボット１の大まかな外形形状が示されている。図示されるように、第１実施例のロボット１は、地面に設置される基台１０と、基台１０に対して回転可能に取り付けられた第１アーム２０と、基台１０内に搭載されてロボット１の全体の動作を制御する制御部５０とを備えている。

第１アーム２０は、６つのリンク２１〜２６と、５つの関節４２〜４６とを備えている。この中のリンク２１は、関節４１によって回転可能に基台１０に取り付けられている。また、リンク２２は、関節４２によって屈曲可能にリンク２１に取り付けられており、リンク２３は、関節４３によって屈曲可能にリンク２２に取り付けられている。更に、リンク２４は、関節４４によって回転可能にリンク２３に取り付けられており、リンク２５は、関節４５によって屈曲可能にリンク２４に取り付けられ、リンク２６は、関節４６によって回転可能にリンク２５に取り付けられている。尚、リンク２６の先端には、ロボットハンド（いわゆるハンド部）や、溶接治具などの図示しない各種の治具（いわゆるエンドエフェクター）が装着される。

また、関節４１の部分には、関節４１を駆動するためのモーター４１ｍが搭載されている。同様に、関節４２の部分には関節４２を駆動するためのモーター４２ｍが搭載され、関節４３の部分には関節４３を駆動するためのモーター４３ｍが、関節４４の部分にはモーター４４ｍが、関節４５の部分にはモーター４５ｍが、関節４６の部分にはモーター４６ｍが搭載されている。

これら６つのリンク２１〜２６のうちで最も基台１０側のリンク２１には、ジャイロセンサー３０が取り付けられており、最も先端側のリンク２６には、ジャイロセンサー３１が取り付けられている。ここで、ジャイロセンサー３０，３１は、予め定められた直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）を回転軸とする角速度（あるいは慣性力）を出力可能なセンサーである。ジャイロセンサー３０は、関節４１の回転軸がジャイロセンサー３０のＺ軸と一致する向きに取り付けられている。また、ジャイロセンサー３１は、関節４６の回転軸がジャイロセンサー３１のＺ軸と一致する向きに取り付けられている。尚、本実施例では、慣性力として角速度を検出するものとして説明するが、角速度の代わりに速度を検出してもよい。また、ジャイロセンサー３０，３１の代わりに加速度センサーを用いても良い。

図１（ｂ）には、第１実施例のロボット１が備えるリンク２１〜２６や、関節４１〜４６、ジャイロセンサー３０，３１の位置関係が模式的に示されている。以下では、関節４１の角度を角度θ１で表し、関節４２の角度を角度θ２、関節４３の角度を角度θ３、関節４４の角度を角度θ４、関節４５の角度を角度θ５、関節４６の角度を角度θ６で表すものとする。

尚、第１実施例においては、第１アーム２０に含まれる関節４２〜４６が本発明における「第１関節部」に対応し、第１アーム２０と基台１０とを接続する関節４１が本発明における「基台関節部」に対応する。また、ジャイロセンサー３０は本発明における「基台側慣性センサー」に対応し、ジャイロセンサー３１は本発明における「第１慣性センサー」に対応する。また、以下の明細書中では、第１アーム２０の関節４２〜４６を駆動するモーター４２ｍ〜４６ｍを「第１モーター」と称し、関節４１を駆動するモーター４１ｍを「基台側モーター」と称することがあるものとする。

図２は、制御部５０を中心として、モーター４１ｍ〜４６ｍや、ジャイロセンサー３０，３１がデータをやり取りする様子を示した説明図である。基台側のモーター４１ｍには、モーター４１ｍの回転角度を検出する角度センサー４１ｓが搭載されている。同様に、第１モーター（モーター４２ｍ〜４６ｍ）にも角度θ２〜θ６を検出する角度センサー４２ｓ〜４６ｓが搭載されている。これら角度センサー４１ｓ〜４６ｓの出力や、ジャイロセンサー３０，３１の出力は、制御部５０に入力されている。制御部５０は、これらの出力に基づいて、基台側のモーター４１ｍや、第１モーター（モーター４２ｍ〜４６ｍ）の動作を制御する。詳細な制御内容については後述する。また、制御部５０に内蔵されたメモリー５０ｍには、制御に際して参照する各種のデータなどが記憶されている。

図３は、第１実施例の制御部５０の内部構成を概念的に示したブロック図である。図示されるように、第１実施例の制御部５０は、第１実速度演算部５１と、第１移動速度演算部５２と、第１歪み速度演算部５４と、第１補正速度演算部５５と、第１モーター駆動部５６と、基台側実速度演算部５３と、基台側移動速度演算部５７と、基台側歪み速度演算部５８と、基台側モーター駆動部５９の合計９つの部を備えている。尚、これら９つの部は、制御部５０がモーター４１ｍ〜４６ｍの動作を制御する機能に着目して、便宜的に制御部５０の内部を分類したものであり、制御部５０が物理的に９つの部分に分割可能なことを意味するわけではない。これら９つの部は、ＬＳＩなどを用いたハードウェアによって実現することもできるし、コンピュータープログラムを用いたソフトウェアによって実現することもできる。

詳細には後述するが、制御部５０の内部でのデータの流れは、基台側のモーター４１ｍを制御するためのデータの流れと、第１モーター（モーター４２ｍ〜４６ｍ）を制御するためのデータの流れの大きく２つの流れに分けて考えることができる。このうちの基台側のモーター４１ｍを制御するためのデータの流れでは、ジャイロセンサー３０の出力と、角度センサー４１ｓの出力とを受け取って、制御部５０の内部で所定の演算を行った後、モーター４１ｍにデータを出力する。また、第１モーター（モーター４２ｍ〜４６ｍ）を制御するためのデータの流れでは、ジャイロセンサー３１の出力と、角度センサー４２ｓ〜４６ｓの出力とを受け取り、更に、モーター４１ｍの制御のための途中の演算結果も受け取って、制御部５０の内部で所定の演算を行った後、第１モーター（モーター４２ｍ〜４６ｍ）にそれぞれデータを出力する。以下では、第１実施例の制御部５０の内部で行われる具体的な演算内容について説明する。

Ａ−２．第１実施例の制御方法 ：
Ａ−２−１．第１移動速度演算部 ：
図４は、制御部５０の第１移動速度演算部５２が実行する演算内容についての説明図である。第１移動速度演算部５２は、次のような原理に基づいて、第１アーム２０の先端部分（リンク２６）の移動速度を演算する。先ず、図１（ｂ）に示したように、リンク２６は関節４１〜４５を介して基台１０に接続されている。従って、関節４１を回転させると、基台１０に接続されたリンク２１〜２５と共に、リンク２６が移動する。また、この時のリンク２６の移動速度は関節４１の回転速度に依存する。同じく、関節４２を回転させると、リンク２１に接続されたリンク２２〜２５と共にリンク２６の位置が移動し、この時の移動速度は関節４２の回転速度（角速度ω２）に依存する。同様に、関節４３〜４５を回転させてもリンク２６が移動し、この時のリンク２６の移動速度は、関節４３の回転速度（角速度ω３）や、関節４４の回転速度（角速度ω４）、関節４５の回転速度（角速度ω５）、関節４６の回転速度（角速度ω６）に依存する。

ここで、関節４２の角度θ２は角度センサー４２ｓによって検出することができるから、角度センサー４２ｓの出力の微分値（簡易的には変化量）を求めれば、関節４２の角速度ω２を得ることができる。角速度ω３〜ω６についても同様に、角度センサー４３ｓ〜４６ｓの出力の微分値（簡易的には変化量）から求めることができる。また、前述したようにジャイロセンサー３０は、ジャイロセンサー３０のＺ軸が、関節４１の回転軸と同じ向きとなるように取り付けられているから、ジャイロセンサー３０のＺ軸回りの角速度Ｒ0zを検出すれば、関節４１が回転する角速度を得ることができる。

従って、リンク２６の移動速度は、ジャイロセンサー３０の出力（角速度Ｒ0z）と、第１アーム２０の関節４２〜４６に設けられた角度センサー４２ｓ〜４６ｓの出力（角度θ２〜θ６）とを用いて演算することが可能な筈である。尚、リンク２６は３次元的に移動するから、移動速度も３つの成分を有している。各成分の座標軸は、原理的にはどのような座標軸としても構わないが、制御の都合上、ジャイロセンサー３１の直交３軸（ＸＹＺ軸）を使用する。従って、リンク２６の移動速度は、ジャイロセンサー３１のＸ軸、Ｙ軸、Ｚ軸方向の各成分（Ｃ1x、Ｃ1y、Ｃ1z）によって表される。

第１移動速度演算部５２は、以上の原理に基づいて、リンク２６のジャイロセンサー３１が取り付けられた箇所での移動速度（Ｃ1x，Ｃ1y，Ｃ1z）を演算する。すなわち、図４（ａ）に示されるように、ジャイロセンサー３０で計測された角速度Ｒ0zと、角度センサー４２ｓで検出した関節４２の角度θ２と、角度センサー４３ｓで検出した関節４３の角度θ３と、角度センサー４４ｓで検出した関節４４の角度θ４と、角度センサー４５ｓで検出した関節４５の角度θ５と、角度センサー４６ｓで検出した関節４６の角度θ６とを取得する。続いて、角度θ２〜θ６の微分値（あるいは時間あたりの変化量）を演算することによって、角度θ２〜θ６を角速度ω２〜ω６に変換する。尚、角度センサー４１ｓで検出した関節４２の角度θ１は、後述するヤコビアンを演算するために使用される。そして、関節４１での角速度Ｒ0zおよび関節４２〜４６での角速度ω２〜ω６に対してヤコビアンを作用させることによって、リンク２６の移動速度の各成分Ｃ1x、Ｃ1y、Ｃ1zを演算することができる。

図４（ｂ）には、リンク２６の移動速度（Ｃ1x，Ｃ1y，Ｃ1z）を求めるために用いるヤコビアン（以下、第１アームヤコビアンＪ１とよぶ）が示されている。ここで、第１アームヤコビアンＪ１の意味する処について概要を説明する。例えば、リンク２６の移動速度のＸ成分Ｃ1xについて着目する。前述したようにリンク２６の移動速度は、関節４１での角速度Ｒ0zや、関節４２〜４６での角速度ω２〜ω６に依存する。従って、移動速度のＸ成分Ｃ1xについても、角速度Ｒ0zや角速度ω２〜ω６の線形結合によって、図４（ｃ）に示すような形式で表すことができる。ここで、角速度Ｒ0zにかかる係数（ｄＣ1x／ｄθ１）は、角速度Ｒ0zの変化がＸ成分Ｃ1xに与える寄与度を示す係数と考えることができる。

図１（ａ）あるいは図１（ｂ）を参照すれば明らかなように、関節４１を回転させた時のリンク２６の移動速度は、関節４１からリンク２６までにある各リンク２１〜２５までの長さ（リンク長）や、各関節４１〜４６での角度θ１〜θ６に依存する。更に、各リンク２１〜２５のリンク長は変化することはないが、各関節４１〜４６での角度θ１〜θ６は変化する。従って、リンク２６の移動速度のＸ成分Ｃ1xは、各関節４１〜４６での角度θ１〜θ６を変数とする関数となる。このことから、角速度Ｒ0zの寄与度を示す係数（ｄＣ1x／ｄθ１）は、移動速度のＸ成分Ｃ1xを変数θ１で偏微分した偏微分係数によって与えられる。

角速度ω２の寄与度を示す係数（ｄＣ1x／ｄθ２）や、角速度ω３の寄与度を示す係数（ｄＣ1x／ｄθ３）、角速度ω４の寄与度を示す係数（ｄＣ1x／ｄθ４）、角速度ω５の寄与度を示す係数（ｄＣ1x／ｄθ５）、角速度ω６の寄与度を示す係数（ｄＣ1x／ｄθ６）についても同様に、移動速度のＸ成分Ｃ1xを、それぞれの変数θ２〜θ６で偏微分した偏微分係数によって与えられる。

以上では、リンク２６の移動速度のＸ成分Ｃ1xについて説明したが、移動速度のＹ成分Ｃ1yおよびＺ成分Ｃ1zについても、全く同様なことが成り立つ。すなわち、移動速度のＹ成分Ｃ1yおよび移動速度のＺ成分Ｃ1zは、関節４１の角速度Ｒ0z、関節４２〜４６の角速度ω２〜ω６の線形結合によって表され、それぞれの寄与度を表す係数は、移動速度のＹ成分Ｃ1yあるいは移動速度のＺ成分Ｃ1zを、それぞれ変数θ１〜θ６で偏微分した偏微分係数となる。そして、これらを行列の形式でまとめれば、図４（ｂ）に示す式を得ることができる。

尚、第１アームヤコビアンＪ１に関する図４（ｂ）に示した式について補足して説明しておく。第１アームヤコビアンＪ１が図４（ｂ）に示した行列となるのは、ジャイロセンサー３０が検出する回転軸（ここではＲ0z軸）と、関節４１の回転軸とが一致しているためである。しかし、ジャイロセンサー３０が検出する回転軸と、関節４１の回転軸とが一致しているとは限らない。このような場合は、ジャイロセンサー３０の姿勢角を、θ_Ｒ０ｘ、θ_Ｒ０ｙ、θ_Ｒ０ｚとすると、図４（ｄ）に示す式が成り立つ。ここで、姿勢角θ_Ｒ０ｘを時間微分した値が角速度Ｒ0xであり、姿勢角θ_Ｒ０ｙを時間微分した値が角速度Ｒ0yであり、姿勢角θ_Ｒ０ｚを時間微分した値が角速度Ｒ0zでとなる。そして、ここでは、ジャイロセンサー３０のＲ0z軸とθ１の軸とが一致しているものとしているから、図４（ｄ）で、θ_Ｒ０ｙやθ_Ｒ０ｚでの偏微分係数は全て０となり、更に、θ_Ｒ０ｘはθ１で置き換えることができる。このようにして得られた式が、図４（ｂ）に示した式となる。

以上のことから明らかなように、第１アームヤコビアンＪ１は、第１アーム２０を形成する各リンク２１〜２６のリンク長や形状に依存し、関節４１〜４６の角度θ１〜θ６を変数とする行列である。各リンク２１〜２６のリンク長は予め分かっているから、角度センサー４１ｓ〜４６ｓの出力から角度θ１〜θ６を検出すれば、その時点での第１アームヤコビアンＪ１を決定することができる。第１移動速度演算部５２は、以上のようにして、ジャイロセンサー３０の出力や、角度センサー４１ｓ〜４６ｓの出力から、第１アームヤコビアンＪ１を用いてリンク２６の移動速度の各成分Ｃ1x、Ｃ1y、Ｃ1zを演算する。尚、第１実施例においては、第１アーム２０の各関節４２〜４６の角度を検出する角度センサー４２ｓ〜４６ｓが、本発明における「第１角度検出部」に対応する。また、第１アームヤコビアンＪ１を用いた演算によって得られたリンク２６の移動速度は、本発明における「第１移動速度」に対応する。

Ａ−２−２．第１歪み速度演算部 ：
図５は、制御部５０の第１歪み速度演算部５４が実行する演算内容についての説明図である。第１歪み速度演算部５４は、リンク２６の移動速度の各成分について、前述した第１移動速度演算部５２で得られた演算値Ｃ1x，Ｃ1y，Ｃ1zと、ジャイロセンサー３１で検出した実際の移動速度との偏差を演算する。

ジャイロセンサー３１の３軸の出力は、第１実速度演算部５１でＡＤ変換された後、所定の変換係数が乗算されることによって、リンク２６の実際の移動速度の各成分Ｒ1x，Ｒ1y，Ｒ1zに変換される。また、前述したように第１移動速度演算部５２で演算する移動速度のＸＹＺ各成分は、ジャイロセンサー３１で検出するＸＹＺの３軸成分と同じ方向成分となるように設定されている。このため、第１歪み速度演算部５４は、第１実速度演算部５１の出力から、第１移動速度演算部５２の出力を成分毎に減算することができる。そして、得られた偏差をハイパスフィルター（ＨＰＦ）に通すことによって、リンク２６の第１歪み速度の各成分Ｄ1x，Ｄ1y，Ｄ1zを求めることができる。尚、ジャイロセンサー３１で検出した実際の移動速度は、本発明における「第１実速度」に対応する。

このようにして求めた第１歪み速度（Ｄ1x，Ｄ1y，Ｄ1z）は、第１移動速度演算部５２がリンク２６の移動速度（Ｃ1x，Ｃ1y，Ｃ1z）を演算する際に考慮しなかった、各リンク２１〜２６や関節４１〜４６での変形に起因する。そして、第１アーム２０の先端に取り付けたハンド部などを目的の位置まで移動させて停止させた時に、ハンド部が振動する現象も、各リンク２１〜２６や関節４１〜４６での変形が原因となって発生する。従って、第１歪み速度演算部５４で得られた第１歪み速度を、第１アーム２０の各関節４１〜４６に搭載された第１モーター（モーター４２ｍ〜４６ｍ）および基台側のモーター４１ｍにフィードバックすることで、リンク２１〜２６や関節４１〜４６での変形の影響を抑制してやれば、第１アーム２０を停止した時の振動を抑制することが可能である。第１歪み速度演算部５４は、このような着想に基づいて、第１歪み速度を成分毎に演算する。

Ａ−２−３．第１補正速度演算部 ：
図６は、制御部５０の第１補正速度演算部５５が実行する演算内容についての説明図である。図６（ａ）に示されるように、第１補正速度演算部５５は、上述した第１歪み速度演算部５４で得られた第１歪み速度（Ｄ1x，Ｄ1y，Ｄ1z）を、第１アーム２０の各関節４２〜４６についての補正速度Ｄω２〜Ｄω６（以下、第１補正速度と称することがある）に変換する演算を行う。すなわち、第１アーム２０のリンク２１〜２６や関節４２〜４６での変形によって生じた第１歪み速度は、ジャイロセンサー３１のＸＹＺ軸を基準とする各成分について得られている。従って、第１歪み速度の抑制を、関節４２〜４６がどのように分担して実現するかを決定する必要がある。

そこで、第１補正速度演算部５５は、上述した第１歪み速度演算部５４で得られた第１歪み速度の各成分Ｄ1x，Ｄ1y，Ｄ1zから、関節４２〜４６の角速度ω２〜ω６についての補正速度Ｄω２〜Ｄω６を演算する。尚、関節４１の角速度ω１の補正速度については、後述する基台側歪み速度演算部５８が演算するので、第１補正速度演算部５５は、関節４２〜４６についての補正速度Ｄω２〜Ｄω６を演算すればよい。

ここで、図４を用いて前述した第１アームヤコビアンＪ１は、関節４１〜４６の角速度Ｒ0z、ω２〜ω６を、リンク２６の移動速度Ｃ1x，Ｃ1y，Ｃ1zに変換する行列であった。これに対して、リンク２６の移動速度の補正量（すなわち、第１歪み速度Ｄ1x，Ｄ1y，Ｄ1z）から、関節４２〜４６の角速度ω２〜ω６についての補正量（すなわち、補正速度Ｄω２〜Ｄω６）を決定するための行列は、ちょうど第１アームヤコビアンＪ１とは逆の変換を行う行列となる。従って、この第１アームヤコビアンＪ１とは逆の変換を行う行列を、以下では、第１アーム逆ヤコビアンＲＪ１と称する。第１アーム逆ヤコビアンＲＪ１を用いれば、図６（ｂ）に示した行列演算を行うことによって、第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zから、第１補正速度Ｄω２〜Ｄω６を決定することができる。

もっとも、第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zから、関節４２〜４６についての第１補正速度Ｄω２〜Ｄω６を決定する演算は、３つの入力値（Ｄ1x，Ｄ1y，Ｄ1z）から、５つの変数値（Ｄω２〜Ｄω６）を決定する逆問題となる。このため、第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zから、第１補正速度Ｄω２〜Ｄω６を一意的に決定できるわけではない。しかし、リンク２１〜２６や関節４２〜４６の変形のし易さを拘束条件として用いれば、３つの入力値（Ｄ1x，Ｄ1y，Ｄ1z）から、５つの変数値（Ｄω２〜Ｄω６）を決定することが可能となる。

そこで、図６（ｃ）に示すように、第１アームヤコビアンＪ１から角速度Ｒ0zに関する成分を除いた部分行列ＰＪ１と、角度θ２〜θ６に重みを付けた５行５列の重付行列Ｗとを想定し、図６（ｄ）に示す行列演算によって、第１アーム逆ヤコビアンＲＪ１を決定する。尚、重付行列Ｗは、リンク２１〜２６や関節４２〜４６での変形のし易さを表す行列である。また、図６（ｄ）中の「−１」という表記は逆行列を表しており、図６（ｄ）中の「Ｔ」という表記は転置行列を表している。

第１実施例の第１補正速度演算部５５は、第１歪み速度演算部５４から第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zを受け取ると、以上のようにして、関節４２〜４６の角速度ω２〜ω６についての補正量（第１補正速度Ｄω２〜Ｄω６）を演算する。そして、得られた第１補正速度Ｄω２〜Ｄω６を、第１モーター駆動部５６に出力する。

Ａ−２−４．第１モーター駆動部 ：
図７は、制御部５０の第１モーター駆動部５６が実行する演算内容についての説明図である。第１モーター駆動部５６は、第１補正速度演算部５５から関節４２〜４６についての第１補正速度Ｄω２〜Ｄω６を受け取ると、それぞれの関節４２〜４６に搭載された第１モーター（モーター４２ｍ〜４６ｍ）を、モーター毎に制御する。

Ａ−２−５．基台側のモーターについての制御内容 ：
図８は、制御部５０の基台側実速度演算部５３、基台側移動速度演算部５７、基台側歪み速度演算部５８および基台側モーター駆動部５９が、基台側のモーター４１ｍを制御する動作を示した説明図である。

基台側実速度演算部５３は、リンク２１に取り付けられたジャイロセンサー３０からの出力を受け取ると、ＡＤ変換を施した後、所定の変換係数を乗算することによって、基台１０に対するリンク２１の実速度Ｒ0zを演算する。また、基台側移動速度演算部５７は、基台１０に対するリンク２１の角度θ１を角度センサー４１ｓから受け取ると、角度θ１の微分値（あるいは時間あたりの変化量）を算出することによって、関節４１の角速度ω１を演算する。尚、第１実施例においては、関節４１の角度を検出する角度センサー４１ｓが、本発明における「基台側角度検出部」に対応する。また、角速度ω１本発明における「基台側移動速度」に対応する。

そして、基台側歪み速度演算部５８は、基台側実速度演算部５３で求めた実速度Ｒ0zから、基台側移動速度演算部５７で求めた角速度ω１を減算し、得られた減算結果をハイパスフィルター（ＨＰＦ）に通すことによって、基台側歪み速度Ｄ0zを演算する。尚、図４を用いて前述した第１移動速度演算部５２では、関節４２〜４６の角速度ω２〜ω６を、第１アームヤコビアンＪ１を用いてジャイロセンサー３１と同じ座標軸の角速度Ｃ1x，Ｃ1y，Ｃ1zに変換してから、歪み速度を演算していた。これに対して、図１を用いて前述したように、ジャイロセンサー３０は、ジャイロセンサー３０のＺ軸が関節４１の回転軸と同じ向きとなるように取り付けられているので、座標軸の変換は不要である。このため、ジャイロセンサー３０から得られた実速度Ｒ0zから、関節４１の角速度ω１を減算するだけで、直ちに基台側歪み速度Ｄ0zを演算することができる。

基台側モーター駆動部５９は、こうして得られた基台側歪み速度Ｄ0zに基づいて、基台側のモーター４１ｍの動作を制御する。尚、図７を用いて前述したように第１モーター駆動部５６は、第１歪み速度演算部５４で得られた第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zを、第１補正速度演算部５５で、関節４２〜４６の角速度ω２〜ω６を用いた座標系に変換した後、第１モーター（モーター４２ｍ〜４６ｍ）を制御していた。これに対して、ジャイロセンサー３０のＺ軸は関節４１の回転軸と同じ向きとなっているので、座標軸を変換することなく、直ちに基台側歪み速度Ｄ0zに基づいて基台側のモーター４１ｍを制御することができる。

以上に説明した第１実施例のロボット１では、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形に起因して発生する歪み速度（第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zや基台側歪み速度Ｄ0z）を求めて、その結果に基づいて、関節４２〜４６の第１モーター（モーター４２ｍ〜４６ｍ）や、関節４１のモーター４１ｍの動作をフィードバック制御する。一般に、第１アーム２０の先端を移動させて停止させた時に生じる振動は、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形に起因する。しかし、第１実施例のロボット１では、歪み速度を検出してモーター４１ｍ〜４６ｍをフィードバック制御しているので、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形による影響を抑制することができる。その結果、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形に起因して発生する第１アーム２０の振動も抑制することが可能となるので、第１アーム２０の振動を速やかに停止させて、ハンド部などを用いた作業を開始することが可能となる。

尚、上述した第１実施例では、図３に示した制御部５０内の９つの「部」が、ＬＳＩなどを用いたハードウェアか、コンピュータープログラムを用いたソフトウェア、若しくはそれらの組合せによって実現されているものとして説明した。もちろん、上述した全ての演算を実行するコンピュータープログラムをメモリー５０ｍに記憶しておくこともできる。図９は、このようなコンピュータープログラムを用いて制御部５０が実行する制御処理のフローチャートである。

図９に示した制御処理を開始すると、制御部５０は、基台１０側のリンク２１に取り付けられたジャイロセンサー３０、および第１アーム２０の先端のリンク２６に取り付けられたジャイロセンサー３１の出力を取得する（ステップＳ１００）。そして、それらの出力に基づいて、基台１０側のリンク２１の実速度（基台側実速度Ｒ0z）およびリンク２６の実速度（第１実速度Ｒ1x，Ｒ1y，Ｒ1z）を演算する（ステップＳ１０１）。

続いて、関節４１〜４６のモーター４１ｍ〜４６ｍに内蔵されている角度センサー４１ｓ〜４６ｓから、関節４１〜４６の角度θ１〜θ６を取得する（ステップＳ１０２）。そして、角度θ１の微分値（あるいは時間あたりの変化量）を求めることによって、基台側移動速度ω１を演算する。同様に、角度θ２〜θ６の微分値（あるいは時間あたりの変化量）を求めることによって得られた角速度ω２〜ω６に、前述した第１アームヤコビアンＪ１を作用させることによって、第１移動速度Ｃ1x，Ｃ1y，Ｃ1zを演算する（ステップＳ１０３）。

そして、ステップＳ１０１で求めた基台側実速度Ｒ0zと、ステップＳ１０３で求めた基台側移動速度ω１との偏差を取り、ハイパスフィルターを通すことによって、基台側歪み速度Ｄ0zを演算する。同様に、ステップＳ１０１で求めた第１実速度Ｒ1x，Ｒ1y，Ｒ1zと、ステップ１０３で求めた第１移動速度Ｃ1x，Ｃ1y，Ｃ1zとの偏差を取ってハイパスフィルターを通すことにより、第１歪み速度Ｃ1x，Ｃ1y，Ｃ1zを演算する（ステップＳ１０４）。

続いて、第１歪み速度Ｃ1x，Ｃ1y，Ｃ1zに対して、前述した第１アーム逆ヤコビアンＲＪ１を作用させることによって、第１モーター（モーター４２ｍ〜４６ｍ）の角速度の補正量（第１補正速度Ｄω２〜Ｄω６）を演算する（ステップＳ１０５）。

そして、基台側歪み速度Ｄ0zに従って基台側のモーター４１ｍの動作を制御し、同様に、第１補正速度Ｄω２〜Ｄω６に従って第１モーター（モーター４２ｍ〜４６ｍ）の動作を制御する（ステップＳ１０６）。その後、制御を終了するか否かを判断し（ステップＳ１０７）、制御を継続する場合は（ステップＳ１０７：ｎｏ）、処理の先頭に戻って上述した一連の処理（ステップＳ１００〜Ｓ１０７）を繰り返す。これに対して、制御を終了する場合は（ステップＳ１０７：ｙｅｓ）、図９の制御処理を終了する。

以上のようなプログラムを実行することによっても、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形に起因して発生する歪み速度（第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zや基台側歪み速度Ｄ0z）を検出して、モーター４１ｍ〜４６ｍの動作にフィードバック制御することで、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形による影響を抑制することができる。このため、第１アーム２０のリンク２１〜２６や関節４１〜４６の変形に起因して発生する第１アーム２０の振動も抑制することが可能となる。

Ｂ．第２実施例 ：
上述した第１実施例では、第１アーム２０が取り付けられる基台１０は、第１アーム２０との間の関節４１の他に関節を有しておらず、基台１０が地面に固定されているものとして説明した。しかし、基台１０が、関節４１以外の関節を介して地面に固定されるようにしても良い。以下では、このような第２実施例について説明する。

図１０は、第２実施例のロボット２の大まかな構成を示す説明図である。第２実施例のロボット２においても、第１アーム２０については、前述した第１実施例と同様である。すなわち、第１アーム２０は、６つのリンク２１〜２６と、これらリンク２１〜２６を連結する５つの関節４２〜４６とを備えている。また、それぞれの関節４２〜４６には、モーター４２ｍ〜４６ｍや、角度センサー４２ｓ〜４６ｓを内蔵されている。更に、リンク２１およびリンク２６には、それぞれジャイロセンサー３０，ジャイロセンサー３１が取り付けられている。

また、第２実施例のロボット２においても、第１アーム２０は関節４１を介して基台１０に連結されており、関節４１には、モーター４１ｍや角度センサー４１ｓが内蔵されている。しかし、第２実施例のロボット２では、基台１０も一種のリンク（以下、リンク１０）となっており、基台１０（リンク１０）が関節１８を介してリンク１４に連結され、リンク１４が関節１７を介してリンク１３に連結され、リンク１３が関節１６を介してリンク１２に連結され、リンク１２が関節１５を介してリンク１１に連結されて、リンク１１が地面に固定されている。換言すれば、第２実施例のロボット２は、５つのリンク１０〜１４と４つの関節１５〜１８からなるアームの先端に、関節４１を介して第１アーム２０が取り付けられた構造となっている。また、それぞれの関節１５〜１８には、他の関節４１〜４６と同様に、関節を駆動するための後述するモーター１５ｍ〜１８ｍや、角度センサー１５ｓ〜１８ｓが内蔵されている。尚、第２実施例においては、モーター１５ｍ〜１８ｍ、およびモーター４１ｍを、「基台側モーター」と称することがある。また、第２実施例においては、角度センサー１５ｓ〜１８ｓが本発明における「基台側角度検出部」に対応する。

図１１は、第２実施例のロボット２の各関節１５〜１８，４１〜４６に設けられたモーター１５ｍ〜１８ｍ，４１ｍ〜４６ｍや、角度センサー１５ｓ〜１８ｓ，４１ｓ〜４６ｓと、制御部５０との接続関係についての説明図である。尚、第１モーター（モーター４２ｍ〜４６ｍ）や、これら第１モーターに内蔵された角度センサー４２ｓ〜４６ｓについては、前述した第１実施例のロボット１と同様であるため、これらについては１つにまとめて表示されている。第２実施例においても、角度センサー４２ｓ〜４６ｓの出力や、ジャイロセンサー３０，３１の出力は、制御部５０に入力されている。制御部５０は、これらの出力に基づいて、第１モーター（モーター４２ｍ〜４６ｍ）の動作を制御する。

また、関節１５を駆動するためのモーター１５ｍには、関節１５の角度θ１５を検出する角度センサー１５ｓが内蔵されており、角度センサー１５ｓの出力は制御部５０に入力されている。同様に、関節１６を駆動するためのモーター１６ｍには、関節１６の角度θ１６を検出する角度センサー１６ｓが内蔵されており、関節１７を駆動するためのモーター１７ｍには、関節１７の角度θ１７を検出する角度センサー１７ｓが、関節１８を駆動するためのモーター１８ｍには、関節１８の角度θ１８を検出する角度センサー１８ｓが内蔵されている。そして、角度センサー１６ｓ〜１８ｓの出力も、制御部５０に入力されている。そして、第２実施例の制御部５０は、基台側のモーター１５ｍ〜１８ｍ，４１ｍの動作を制御する。

図１２は、第２実施例の制御部５０の内部構成を概念的に示したブロック図である。第２実施例の制御部５０においても、図３を用いて前述した第１実施例の制御部５０と同様に、第１実速度演算部５１と、第１移動速度演算部５２と、第１歪み速度演算部５４と、第１補正速度演算部５５と、第１モーター駆動部５６と、基台側実速度演算部５３と、基台側移動速度演算部５７と、基台側歪み速度演算部５８と、基台側モーター駆動部５９とを備えている。この中で、第１実速度演算部５１と、第１移動速度演算部５２と、第１歪み速度演算部５４と、第１補正速度演算部５５と、第１モーター駆動部５６については、第１移動速度演算部５２にジャイロセンサー３０のＸ軸回りの角速度Ｒ0xの項、およびＹ軸回りの角速度Ｒ0yの項が増えることを除いては、前述した第１実施例と同様である。これは、第２実施例のロボット２においても、第１実施例のロボット１と全く同様な構成の第１アーム２０を備えることによるものである。

一方、第２実施例のロボット２は、第１アーム２０が取り付けられる基台１０も、５つのリンク１０〜１４と４つの関節１５〜１８を備えており、第１アーム２０と同様な構造となっている。このため、基台側のモーター１５ｍ〜１８ｍ，４１ｍは、第１モーター（モーター４２ｍ〜４６ｍ）と同様にして制御される。すなわち、ジャイロセンサー３１から第１実速度を演算する第１実速度演算部５１に対応して、ジャイロセンサー３０から基台側実速度を演算する基台側実速度演算部５３が設けられており、角度センサー４２ｓ〜４６ｓで検出した角度θ２〜θ６を用いて第１移動速度を演算する第１移動速度演算部５２に対応して、角度センサー１５ｓ〜１８ｓ，４１ｓで検出した角度θ１５〜θ１８，θ１を用いて基台側移動速度（ジャイロセンサー３１を搭載した位置の移動速度）を演算する基台側移動速度演算部５７が設けられている。この基台側移動速度演算部５７は、後述する基台側ヤコビアンを用いて、基台側移動速度を演算する。また、第１歪み速度演算部５４に対応して基台側歪み速度演算部５８が設けられ、第１補正速度演算部５５に対応して基台側補正速度演算部６０が設けられ、第１モーター駆動部５６に対応して基台側モーター駆動部５９が設けられている。基台側補正速度演算部６０は、後述する基台側逆ヤコビアンを用いて、基台側の補正速度を演算する。

図１３は、第２実施例の基台側移動速度演算部５７が、ジャイロセンサー３０が搭載された箇所の移動速度（基台側移動速度Ｃ0x，Ｃ0y，Ｃ0z）を演算する方法を示した説明図である。図４を用いて前述した第１移動速度演算部５２と同様に、基台側移動速度演算部５７も、角度センサー１５ｓ〜１８ｓ，４１ｓで得られた角度θ１５〜１８，θ１から基台側ヤコビアンＪ０を用いて基台側移動速度Ｃ0x，Ｃ0y，Ｃ0zを演算することができる。尚、前述した第１アームヤコビアンＪ１は、第１アーム２０のリンク２１〜２６のリンク長や形状に依存し、関節４１〜４６の角度θ１〜θ６を変数とする行列で与えられた。これと全く同様に、基台側ヤコビアンＪ０は、基台側のリンク１０〜１４のリンク長や形状に依存し、関節１５〜１８，４１の角度θ１５〜θ１８，θ１を変数として、図１３（ｂ）に示す行列として与えられる。

図１４は、第２実施例の基台側補正速度演算部６０が、基台側のモーター１５ｍ〜１８ｍ，４１ｍについての補正速度（基台側補正速度Ｄω１，Ｄω１５〜Ｄω１８）を演算する方法を示した説明図である。図６を用いて前述した第１補正速度演算部５５は、第１歪み速度演算部５４で得られた第１歪み速度Ｄ1x，Ｄ1y，Ｄ1zを、第１アーム逆ヤコビアンＲＪ１を用いて、第１アーム２０の各関節４２〜４６の補正速度Ｄω２〜Ｄω６に変換した。これと全く同様に、基台側補正速度演算部６０は、基台側歪み速度演算部５８で得られた基台側歪み速度Ｄ0x，Ｄ0y，Ｄ0zを、基台側の関節４１，１５〜１８の補正速度（基台側補正速度Ｄω１，Ｄω１５，Ｄω１６，Ｄω１７，Ｄω１８）に変換する。

この時の基台側歪み速度Ｄ0x，Ｄ0y，Ｄ0zから基台側補正速度Ｄω１，Ｄω１５，Ｄω１６，Ｄω１７，Ｄω１８への変換は、図１４（ｂ）に示すように基台側逆ヤコビアンＲＪ０を用いて行う。また、基台側逆ヤコビアンＲＪ０は、第１アーム逆ヤコビアンＲＪ１と同様にして、基台側の関節４１，１５〜１８の角度θ１，θ１５〜１８に重みを付けた重付行列Ｗ０を用いて、図１４（ｃ）の行列演算によって求めることができる。

こうして求めた基台側補正速度Ｄω１，Ｄω１５〜１８を、基台側モーター駆動部５９に供給して、基台側のモーター４１ｍ，モーター１５ｍ〜１８ｍを制御する。こうすれば、第１アーム２０が接続された基台１０が、他のリンク１１〜１４を介して地面に固定されていた場合でも、基台１０（リンク１０）の振動を抑制することができる。その結果、第１アーム２０の先端の振動も抑制することができるので、第１アーム２０の先端に取り付けたハンド部などを目的の位置に移動させた後、速やかに作業を開始することが可能となる。

尚、上述した第２実施例では、リンク１０〜１４や関節４１，１５〜１８によって形成された基台側のアームの先端に、第１アーム２０が取り付けられているものとして説明した。しかし、その第１アーム２０の先端に、更に別のアームを取り付けてもよい。

図１５は、第１アーム２０の先端に、先端側アーム９０を連結した第２実施例の他の態様のロボット３の大まかな構造を示す説明図である。図示されるように他の態様のロボット３は、第２実施例のロボット２に対して、第１アーム２０の先端に先端側アーム９０が連結されている。このような他の態様のロボット３においても、先端側アーム９０の先端のリンク９１にジャイロセンサー３２を取り付けてやれば、第２実施例の第１アーム２０と全く同様にして、リンク９１の振動を抑制することができる。その結果、先端側アーム９０の先端に取り付けたハンド部などを目的の位置に移動させた後、速やかに作業を開始することが可能となる。

また、前述した第２実施例のロボット２，３は、多数のリンクおよび関節が接続された１本のアームに、複数のジャイロセンサーを取り付けたものと考えることができる。すると、各関節についての制御は、図１２を用いて前述したように、ジャイロセンサーの位置によって、複数の小さなアームの単位で行われる。この小さなアーム単位での制御では、ヤコビアンや逆ヤコビアンを用いた行列演算が行われるが、アームを構成する関節の数が多くなるほど、行列の規模が大きくなるために迅速な演算が困難となる。このような観点からすると、第２実施例のロボット２，３は、多数の関節を有するアームの途中にジャイロセンサーを取り付けることで、複数の小さなアームに分割して、小さなアーム毎に各関節の動作を制御することで、迅速な制御の実現したものとなっている。

Ｃ．第３実施例 ：
上述した第２実施例では、複数のアームが直列に連結されているものとして説明した。しかし、複数のアームを並列に連結しても構わない。

図１６は、第３実施例のロボット４の大まかな構成を示す説明図である。図示したように第３実施例のロボット４が備える第１アーム２０は、関節４１を介して基台１０に接続されたリンク２１と、関節４２および関節４３を介してリンク２１に接続されたリンク２２と、関節４４および関節４５を介してリンク２２に接続されたリンク２３と、関節４６および関節４７を介してリンク２３に接続されたリンク２４と、関節４８を介してリンク２４に接続されたリンク２５とを備えている。そして、それぞれの関節４１〜４８には、関節を駆動するためのモーター４１ｍ〜４８ｍが内蔵されており、更に、それぞれのモーター４１ｍ〜４８ｍには、関節の角度を検出する後述の角度センサー４１ｓ〜４８ｓが組み込まれている。

また、第３実施例のロボット４には、第１アーム２０とリンク２１を共有した状態で、第２アーム７０も搭載されている。第２アーム７０の構成は、第１アーム２０と同様である。すなわち、リンク２１と、関節７２および関節７３を介してリンク２１に接続されたリンク６２と、関節７４および関節７５を介してリンク６２に接続されたリンク６３と、関節７６および関節７７を介してリンク６３に接続されたリンク６４と、関節７８を介してリンク６４に接続されたリンク６５とを備えている。また、関節７２〜７８にはモーター７２ｍ〜７８ｍ、および後述の角度センサー７２ｓ〜７８ｓが内蔵されている。更に、リンク２１には、ジャイロセンサー３０が取り付けられ、第１アーム２０の先端のリンク２５にはジャイロセンサー３１が、第２アーム７０の先端のリンク６５にはジャイロセンサー３３が取り付けられている。また、第３実施例のジャイロセンサー３０も、前述した第１実施例のジャイロセンサー３０と同様に、ジャイロセンサー３０のＺ軸が、関節４１の回転軸と同じ方向となるように取り付けられている。尚、第３実施例の第２アーム７０が備える関節７２〜７８は、本発明における「第２関節部」に対応する。また、第２アーム７０の先端に取り付けられたジャイロセンサー３３が、本発明における「第２慣性センサー」に対応する。更に、関節７２〜７８の角度を検出する角度センサー７２ｓ〜７８ｓが、本発明における「第２角度検出部」に対応する。

更に、第３実施例のロボット４においても、基台１０には制御部５０が内蔵されており、この制御部５０には、第１アーム２０の関節４２〜４８に取り付けられたモーター４２ｍ〜４８ｍや、角度センサー４２ｓ〜４８ｓ、第２アーム７０のモーター７２ｍ〜７８ｍ、角度センサー７２ｓ〜７８ｓ、基台１０側のモーター４１ｍ、角度センサー４１ｓ、ジャイロセンサー３０，３１，３３が接続されている。また、第３実施例では、基台１０を車輪９５によって移動させることが可能であり、基台１０を所望の位置まで移動させた後は、ストッパー９６によって地面に固定することが可能となっている。尚、第３実施例においても、第１アーム２０に取り付けられたモーター４２ｍ〜４８ｍを「第１モーター」と称し、基台側のモーター４１ｍを「基台側モーター」と称することがあるものとする。また、第２アーム７０に取り付けられたモーター７２ｍ〜７８ｍは「第２モーター」と称することがあるものとする。

図１７は、第３実施例のロボット４の各関節４１〜４８，７２〜７８に設けられたモーター４１ｍ〜４８ｍ，７２ｍ〜７８ｍや、角度センサー４１ｓ〜４８ｓ，７２ｓ〜７８ｓ、ジャイロセンサー３０，３１，３３と、制御部５０との接続関係についての説明図である。第３実施例においても、角度センサー４１ｓ〜４８ｓ，７２ｓ〜７８ｓの出力や、ジャイロセンサー３０，３１，３３の出力は、制御部５０に入力されている。制御部５０は、これらの出力に基づいて、各関節４１〜４８，７２〜７８に搭載されたモーター４１ｍ〜４８ｍ，７２ｍ〜７８ｍの動作を制御する。

図１８は、第３実施例の制御部５０の内部構成を概念的に示したブロック図である。基台側のモーター４１ｍおよび第１モーター（モーター４２ｍ〜４８ｍ）を制御するための構成は、図３を用いて前述した第１実施例の構成と同様である。すなわち、ジャイロセンサー３０および角度センサー４１ｓの出力に対して、基台側実速度演算部５３、基台側移動速度演算部５７、基台側歪み速度演算部５８、および基台側モーター駆動部５９で、前述した所定の演算を施し、その結果に基づいて基台側のモーター４１ｍを制御する。また、第１モーター（モーター４２ｍ〜４８ｍ）に関しては、ジャイロセンサー３１や、角度センサー４２ｓ〜４８ｓ、基台側実速度演算部５３での演算結果（リンク２１の実速度）に対して、第１実速度演算部５１、第１移動速度演算部５２、第１歪み速度演算部５４、第１補正速度演算部５５、および第１モーター駆動部５６で、前述した所定の演算を施して、その結果に基づいて第１モーター（モーター４２ｍ〜４８ｍ）の動作を制御する。

また、上述したように、第３実施例のロボット４は第２アーム７０を搭載している。このことに対応して、制御部５０には、第２アーム７０の第２モーター（モーター７２ｍ〜７８ｍ）を制御するために、第１アーム２０を制御するための構成と同様な構成が設けられている。すなわち、第１アーム２０の第１実速度演算部５１に対応する第２実速度演算部８１と、第１移動速度演算部５２に対応する第２移動速度演算部８２と、第１歪み速度演算部５４に対応する第２歪み速度演算部８４と、第１補正速度演算部５５に対応する第２補正速度演算部８５と、第１モーター駆動部５６に対応する第２モーター駆動部８６とを備えている。このうちの第２移動速度演算部８２の第２アームヤコビアンや、第２補正速度演算部８５の第２逆ヤコビアンは、第１アームヤコビアンＪ１や第１アーム逆ヤコビアンＲＪ１と同様に、第２アーム７０の各リンク６２〜６４や関節７２〜７８の寸法や変形のし易さに応じて決定される。

そして、ジャイロセンサー３３や、角度センサー７２ｓ〜７８ｓ、基台側実速度演算部５３での演算結果（リンク２１の実速度）に対して、第２実速度演算部８１、第２移動速度演算部８２、第２歪み速度演算部８４、第２補正速度演算部８５、および第２モーター駆動部８６で、第１実施例と同様な処理を施すことにより、第２モーター（モーター７２ｍ〜７８ｍ）の動作を制御する。尚、第３実施例においては、ジャイロセンサー３３の出力によって得られる角速度が、本発明における「第２実速度」に対応し、第２アームヤコビアンを用いて求められる角速度が、本発明における「第２移動速度」に対応する。

こうすれば、第２アーム７０についても、第１アーム２０と同様に振動を抑制することができる。その結果、第２アーム７０の先端の振動も抑制することができるので、第２アーム７０の先端に取り付けたハンド部などを目的の位置に移動させた後、速やかに作業を開始することが可能となる。

以上、各種の実施例について説明したが、本発明は上記の実施例に限られるものではなく、その要旨を逸脱しない範囲において種々の態様で実施することが可能である。例えば、上述した第３実施例では、第１アーム２０および第２アーム７０の２つのアームが並列に接続されているものとして説明した。しかし、３つ以上のアームを並列に接続することも可能である。

１〜４…ロボット、 １０…基台（リンク）、 １１〜１４…リンク、 １５〜１８…関節、 １５ｍ〜１８ｍ…モーター、 １５ｓ〜１８ｓ…角度センサー、 ２０…第１アーム、 ２１〜２６…リンク、 ３０〜３３…ジャイロセンサー、 ４１〜４８…関節、 ４１ｍ〜４８ｍ…モーター、 ４１ｓ〜４８ｓ…角度センサー、 ５０…制御部、 ５０ｍ…メモリー、 ５１…第１実速度演算部、 ５２…第１移動速度演算部、 ５３…基台側実速度演算部、 ５４…第１歪み速度演算部、 ５５…第１補正速度演算部、 ５６…第１モーター駆動部、 ５７…基台側移動速度演算部、 ５８…基台側歪み速度演算部、 ５９…基台側モーター駆動部、 ６０…基台側補正速度演算部、 ６２〜６５…リンク、 ７０…第２アーム、 ７２〜７８…関節、 ７２ｍ〜７８ｍ…モーター、 ７２ｓ〜７８ｓ…角度センサー、 ８１…第２実速度演算部、 ８２…第２移動速度演算部、 ８４…第２歪み速度演算部、 ８５…第２補正速度演算部、 ８６…第２モーター駆動部、 ９０…先端側アーム、 ９１…リンク、 ９５…車輪、 ９６…ストッパー。

Claims (8)

1. 基台と、
   前記基台に対して基台関節部を介して回転可能に設けられ、複数の関節を含む第１関節部を有する第１アームと、
   前記基台関節部よりも前記第１アームの先端側で、且つ、前記第１関節部よりも前記第１アームの前記基台側に設けられ、角速度を検出する基台側慣性センサーと、
   前記第１関節部よりも前記第１アームの先端側に設けられ、角速度を検出する第１慣性センサーと、
   前記基台側慣性センサーによって検出された基台側慣性力と、前記第１慣性センサーによって検出された第１慣性力とに基づいて前記第１関節部を制御する制御部と、
   前記第１アームの前記第１関節部に含まれる複数の前記関節に設けられ、前記複数の関節の角度を検出する第１角度検出部を備え、
   前記制御部は、
   前記第１アームの前記基台側の実速度である基台側実速度を、前記基台側慣性力に基づいて演算する基台側実速度演算部と、
   前記第１アームの先端側の実速度である第１実速度を、前記第１慣性力に基づいて演算する第１実速度演算部と、
   前記第１角度検出部によって得られた前記複数の関節についての角度の変化速度と、前記基台側実速度とに基づいて、前記第１アームの先端側の移動速度である第１移動速度を演算する第１移動速度演算部と、を備え、
   前記第１実速度と前記第１移動速度との偏差に基づいて、前記第１アームの前記第１関節部を制御する、
   ことを特徴とするロボット。
2. 請求項１に記載のロボットであって、
   前記制御部は、
   前記第１実速度と前記第１移動速度との偏差に基づいて、前記第１関節部での歪み速度である第１歪み速度を演算する第１歪み速度演算部を備え、
   前記第１角度検出部によって得られた前記複数の関節についての角度と、前記第１歪み速度とに基づいて前記第１アームの前記第１関節部を制御する
   ことを特徴とするロボット。
3. 請求項２に記載のロボットであって、
   前記第１歪み速度演算部は、前記第１実速度と前記第１移動速度との偏差に含まれる変動成分を抽出することによって、前記第１歪み速度を演算する
   ことを特徴とするロボット。
4. 請求項２または３に記載のロボットであって、
   前記基台関節部は少なくとも１つの関節を含み、
   前記基台関節部に含まれる前記関節の角度を検出する基台側角度検出部を備え、
   前記制御部は、
   前記基台側角度検出部によって得られた前記関節の角度の変化速度に基づいて、前記第１アームの前記基台側の移動速度である基台側移動速度を演算する基台側移動速度演算部を備え、
   前記基台側実速度と前記基台側移動速度との偏差に基づいて、前記基台関節部を制御する
   ことを特徴とするロボット。
5. 請求項１ないし請求項４の何れか一項に記載のロボットであって、
   前記基台関節部を介して前記基台に回転可能に設けられ、複数の関節を含む第２関節部を有する第２アームと、
   前記第２関節部よりも前記第２アームの先端側に設けられ、慣性力を検出する第２慣性センサーと、
   を備え、
   前記制御部は、前記基台側慣性センサーによって検出された基台側慣性力と、前記第２慣性センサーによって検出された第２慣性力とに基づいて前記第２関節部を制御する
   ことを特徴とするロボット。
6. 請求項５に記載のロボットであって、
   前記第２慣性センサーは、角速度に対応する前記第２慣性力を検出するセンサーであり、
   前記第２アームの前記第２関節部に含まれる複数の前記関節に設けられ、前記複数の関節の角度を検出する第２角度検出部を備え、
   前記制御部は、
   前記第２アームの先端側の実速度である第２実速度を、前記第２慣性力に基づいて演算する第２実速度演算部と、
   前記第２角度検出部によって得られた前記複数の関節についての角度の変化速度と、前記基台側実速度とに基づいて、前記第２アームの先端側の移動速度である第２移動速度を演算する第２移動速度演算部と
   を備え、
   前記第２実速度と前記第２移動速度との偏差に基づいて、前記第２関節部を制御する
   ことを特徴とするロボット。
7. 基台関節部を介して基台に回転可能に設けられ、複数の関節を含む第１関節部を有する第１アームを備えるロボットの制御装置であって、
   前記ロボットは、
   前記基台関節部よりも前記第１アームの先端側で、且つ、前記第１関節部よりも前記第１アームの前記基台側に設けられ、角速度を検出する基台側慣性センサーと、
   前記第１関節部よりも前記第１アームの先端側に設けられ、角速度を検出する第１慣性センサーと、
   前記第１アームの前記第１関節部に含まれる複数の前記関節に設けられ、前記複数の関節の角度を検出する第１角度検出部と、を備え、
   前記制御装置は、
   前記基台側慣性センサーによって検出された基台側慣性力と、前記第１慣性センサーによって検出された第１慣性力とに基づいて前記第１関節部を制御する制御部を備え、
   前記制御部は、
   前記第１アームの前記基台側の実速度である基台側実速度を、前記基台側慣性力に基づいて演算する基台側実速度演算部と、
   前記第１アームの先端側の実速度である第１実速度を、前記第１慣性力に基づいて演算する第１実速度演算部と、
   前記第１角度検出部によって得られた前記複数の関節についての角度の変化速度と、前記基台側実速度とに基づいて、前記第１アームの先端側の移動速度である第１移動速度を演算する第１移動速度演算部と、を備え、
   前記第１実速度と前記第１移動速度との偏差に基づいて、前記第１アームの前記第１関節部を制御する、
   ことを特徴とする制御装置。
8. ロボットと前記ロボットを制御する制御装置とを備えるロボットシステムであって、
   前記ロボットは、
   基台と、
   前記基台に対して基台関節部を介して回転可能に設けられ、複数の関節を含む第１関節部を有する第１アームと、
   前記基台関節部よりも前記第１アームの先端側で、且つ、前記第１関節部よりも前記第１アームの前記基台側に設けられ、角速度を検出する基台側慣性センサーと、
   前記第１関節部よりも前記第１アームの先端側に設けられ、角速度を検出する第１慣性センサーと、
   前記第１アームの前記第１関節部に含まれる複数の前記関節に設けられ、前記複数の関節の角度を検出する第１角度検出部と、を備え、
   前記制御装置は、前記基台側慣性センサーによって検出された基台側慣性力と、前記第１慣性センサーによって検出された第１慣性力とに基づいて前記第１関節部を制御する制御部を備え、
   前記制御部は、
   前記第１アームの前記基台側の実速度である基台側実速度を、前記基台側慣性力に基づいて演算する基台側実速度演算部と、
   前記第１アームの先端側の実速度である第１実速度を、前記第１慣性力に基づいて演算する第１実速度演算部と、
   前記第１角度検出部によって得られた前記複数の関節についての角度の変化速度と、前記基台側実速度とに基づいて、前記第１アームの先端側の移動速度である第１移動速度を演算する第１移動速度演算部と、を備え、
   前記第１実速度と前記第１移動速度との偏差に基づいて、前記第１アームの前記第１関節部を制御する、
   ことを特徴とするロボットシステム。

Images