JP5891718B2 - ロボット - Google Patents

Description

本発明は、ロボットにかかわる。

多関節のリンク（以下アームと称す）を有するロボットが組み立て装置等の多くの装置に活用されている。ロボットがアームを移動させて停止するとき、アームが振動する。アームが振動している間はアームの先端に配置されたロボットハンドも振動する。そして、ロボットハンドが振動している間はロボットハンドがワークを把持する等の作業を施すことが難しいので、ロボットハンドの振動が停止するのを待つ必要がある。

ロボットの生産性を向上するために、ロボットハンドが振動する時間を短縮させる方法が特許文献１に開示されている。それによると、アームの回転角度を検出する角度センサーをアームのアクチュエーターに配置する。さらに、アームの振動を検出する角速度センサーをアームのハンド側に配置する。そして、ローパスフィルターを用いて角度センサーの低周波成分を抽出し、ハイパスフィルターを用いて角速度センサーの出力の高周波成分を抽出する。次に、角度センサーの出力と角速度センサーの出力とを合成してアームの動作を検出する。そして、アームの動作に対応してアームを制御することにより、アームの振動を抑えている。

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

ロボットが行う作業の効率を高めるには、アームの動作を早くする必要がある。アームの動作には加速、等速移動、減速停止のステップがある。そして、停止後にアームが静止するまでが減速停止の動作である。そして、アームに位置精度良く作動させるには、停止後に振動していない状態で次の動作に移行する必要がある。

特許文献１は静止する基台に接続されたアームの振動を減衰させる方法として有効な方法である。一方、双腕ロボットのように回動する本体にアームが接続されているロボットでは本体が振動する。従って、特許文献１の方法を用いてアームの振動を減衰させても本体の振動がアームに伝わる為、アームの振動が減衰し難いという課題がある。そこで、回動する本体に接続する腕部の振動を抑制することができるロボットが求められていた。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。

［適用例１］
本適用例にかかるロボットは、基台と、前記基台に回動可能に接続された本体部と、前記本体部に回動可能に接続された腕部と、前記本体部及び前記腕部を制御する制御部と、前記本体部の角速度を検出して前記本体部の角速度を示す本体角速度信号を前記制御部に出力する本体用角速度検出器と、前記腕部の角速度を検出して前記腕部の角速度を示す腕角速度信号を前記制御部に出力する腕用角速度検出器と、を備え、前記制御部は、前記本体角速度信号を用いて前記基台に対する前記本体部の振動を制御し、前記本体角速度信号と前記腕角速度信号との差分を示す差分角速度信号を算出し、前記差分角速度信号を用いて前記本体部に対する前記腕部の振動を制御することを特徴とする。

本適用例によれば、基台に本体部が接続され、基台に対して本体部が回動する。そして、本体用角速度検出器が本体角速度信号を制御部に出力する。そして、制御部は本体部が回動する動作を制御する。このとき、制御部は本体角速度信号を用いているので、本体部の振動を検出することができる。そして、本体部の振動と逆位相に振動させることにより振動を制御することができる。従って、制御部は本体角速度信号を用いないときに比べて本体部の振動を抑制することができる。

本体部には腕部が接続され、本体部に対して腕部が回動する。そして、腕用角速度検出器が腕角速度信号を制御部に出力する。制御部は腕部が回動する動作を制御する。このとき、制御部は本体角速度信号と腕角速度信号との差分を示す差分角速度信号を算出しているので、本体部に対する腕部の振動を検出することができる。そして、本体部に対する腕部の振動と逆位相に振動させることにより振動を制御することができる。これにより、制御部は腕角速度信号だけを用いて腕部の振動を減衰させるときに比べて本体部に対する腕部の振動を抑制することができる。従って、ロボットは本体部の振動が抑制され、本体部に対する腕部の振動が抑制される。その結果、ロボットは回動する本体部に接続する腕部の振動を抑制することができる。

［適用例２］
上記適用例にかかるロボットにおいて、前記腕用角速度検出器は各々直交する３軸回りの角速度を検出する３軸ジャイロセンサーであり、前記制御部は前記腕角速度信号のうち前記本体部が回動する回転軸の軸方向と同じ軸方向の軸回りの角速度成分を抽出して用いることを特徴とする。

本適用例によれば、腕用角速度検出器は３軸ジャイロセンサーであることから腕用角速度検出器は各々直交する３軸回りの角速度を検出する。そして、制御部は腕角速度信号のうち本体部が回動する回転軸の軸方向と同じ軸方向の軸回りの角速度成分を抽出する。従って、制御部は、本体部が回動する回転軸の軸方向の振動を抑制させることができる。

［適用例３］
上記適用例にかかるロボットにおいて、前記腕部は第１腕部と第２腕部とを備え、前記第１腕部と前記第２腕部とに前記腕用角速度検出器が設置され、前記制御部は、前記本体部に対する前記第１腕部の振動と前記第２腕部の振動とを並行して制御することを特徴とする。

本適用例によれば、ロボットは第１腕部と第２腕部とを備えている。そして、第１腕部と第２腕部とにそれぞれ腕用角速度検出器が設置され、腕角速度信号を制御部に出力する。従って、本体部、第１腕部、第２腕部が振動するときにも、制御部は本体部の振動を制御させるとともに、本体部に対する第１腕部と本体部に対する第２腕部との振動を制御する。従って、ロボットは振動する各場所の振動を並行して減衰させるので本体部、第１腕部、第２腕部の振動を抑制することができる。

［適用例４］
本適用例にかかるロボットの制御方法であって、基台に回動可能に接続された本体部の角速度を検出して前記本体部の角速度を示す本体角速度信号を出力し、前記本体部に回動可能に接続された腕部の角速度を検出して前記腕部の角速度を示す腕角速度信号を出力する角速度検出工程と、前記本体角速度信号を用いて前記基台に対する前記本体部の振動を減衰させ、前記本体角速度信号と前記腕角速度信号との差分を示す差分角速度信号を算出し、前記差分角速度信号を用いて前記本体部に対する前記腕部の振動を減衰させる振動減衰工程と、を有することを特徴とする。

本適用例によれば、基台に本体部が接続され、基台に対して本体部が回動する。そして、本体部には腕部が接続され、本体部に対して腕部が回動する。角速度検出工程では、本体部の角速度を検出して本体角速度信号を出力する。さらに、腕部の角速度を検出して腕角速度信号を出力する。本体部や腕部が回動することにより、本体部や腕部は振動する。そして、本体角速度信号は、本体部が振動することによる角速度の変動を含んだ信号となっている。振動減衰工程では本体角速度信号を用いることにより本体部の振動を検出し、基台に対する本体部の振動を減衰させている。

同様に、腕角速度信号は、腕部が振動することによる角速度の変動を含んだ信号となっている。振動減衰工程では本体角速度信号と腕角速度信号との差分を示す差分角速度信号を算出し、差分角速度信号を用いることにより本体部に対する腕部の振動を検出し、本体部に対する腕部の振動を減衰させている。これにより、ロボットは本体部の振動が減衰され、本体部に対する腕部の振動が減衰される。従って、ロボットは腕角速度信号だけを用いて腕部の振動を減衰させるときに比べて短い時間で本体部に対する腕部の振動を減衰させることができる。その結果、ロボットは回動する本体部に接続する腕部の振動を抑制することができる。

（ａ）は、双腕ロボットの構成を示す模式正面図、（ｂ）は、双腕ロボットの構成を示す模式平面図。 角速度検出器の構造を示す概略斜視図。 組立装置の電気制御ブロック図。 振動演算部の機能の一部を示すブロック図。 腕部の振動を減衰させる制御のフローチャート。 腕部の振動を減衰させる制御方法を説明するためのタイムチャート。

以下、実施例について図面に従って説明する。尚、各図面における各部材は、各図面上で認識可能な程度の大きさとするため、各部材毎に縮尺を異ならせて図示している。
（実施形態）
本実施形態では、双腕ロボットと、この双腕ロボットの姿勢を制御する方法の特徴的な例について、図１〜図６に従って説明する。

図１（ａ）は、双腕ロボットの構成を示す模式正面図であり、図１（ｂ）は、双腕ロボットの構成を示す模式平面図である。図１に示すように、ロボット１は床２に設置されている。ロボット１は直方体の基台３を備え、基台３は鉛直方向に長くなっている。そして、鉛直方向で床２から離れる方向をＺ方向とし、水平方向で基台３の側面を構成する面のうち直交する２面が向く方向をＸ方向とＹ方向とする。−Ｙ方向はロボット１の正面であり、Ｘ方向はロボット１の横側の面となっている。

基台３の内部の床２側にはロボット１の姿勢を制御する制御部４が設置されている。基台３の内部のＺ方向には本体回動機構５が設置されている。本体回動機構５はモーター、ロータリーエンコーダー、減速装置等から構成されている。そして、ロータリーエンコーダーがモーターの出力軸の回転角度を検出し、減速装置がモーターの出力軸の回転を減速して回転軸としての出力軸５ａに出力する。これにより、本体回動機構５は精度良く所定の回転角度にて出力軸５ａを回動させることができる。

出力軸５ａはＺ方向に突出して設置され、出力軸５ａには直方体の胴体部６が接続されている。本体回動機構５により胴体部６は基台３に対して回動することが可能になっている。胴体部６の上面６ｃには本体用角速度検出器７が設置されている。本体用角速度検出器７が備えるセンサーの種類は角速度を検出可能であれば良く特に限定されない。本実施形態では水晶振動子を用いたジャイロセンサーを採用している。水晶振動子を用いたジャイロセンサーは周波数の高い角速度の変化を検出することができる。従って、振動を検出するためのセンサーとしては水晶振動子を用いたジャイロセンサーが好ましい。

胴体部６の一面が正面を向くとき、胴体部６の内部のＸ方向の場所には第１肩用回動機構６ａが設置されている。そして、第１肩用回動機構６ａは回動する出力軸を有し、該出力軸には第１肩部８が接続されている。これにより、第１肩部８はＸ方向を回転軸として回動可能になっている。

第１肩部８にはＺ方向を回転軸として回動する出力軸を備える第１肩関節用回動機構８ａが設置されている。そして、第１肩関節用回動機構８ａの出力軸８ｂには第１肩関節部９が設置されている。第１肩関節用回動機構８ａにより第１肩関節部９はＺ方向を回転軸として回動可能になっている。

第１肩関節部９の胴体部６の反対側には第１肩関節部９と接続して第１元側腕部１０が設置されている。第１元側腕部１０の内部の第１肩関節部９側には第１腕捻り用回動機構１０ａが設置されている。そして、第１腕捻り用回動機構１０ａは回動する出力軸を有し、該出力軸の軸方向は第１元側腕部１０の長手方向となっている。第１腕捻り用回動機構１０ａの出力軸は第１肩関節部９と接続されている。第１腕捻り用回動機構１０ａにより、第１元側腕部１０は第１元側腕部１０の長手方向を回転軸として回動可能になっている。

第１元側腕部１０において第１肩関節部９の反対側には第１先側腕部１１が設置されている。第１先側腕部１１にはＺ方向を回転軸として回動する出力軸を備える第１腕関節用回動機構１１ａが設置されている。そして、第１腕関節用回動機構１１ａの出力軸１１ｂには第１元側腕部１０が接続されている。第１腕関節用回動機構１１ａにより第１元側腕部１０に対して第１先側腕部１１が回動可能になっている。第１先側腕部１１には第１腕用角速度検出器１２が設置されている。第１腕用角速度検出器１２は本体用角速度検出器７と同様に第１先側腕部１１の角速度を検出する。

第１先側腕部１１の第１元側腕部１０と反対側には第１先側腕部１１の内部に第１手首用回動機構１１ｃが設置されている。そして、第１手首用回動機構１１ｃは回動する出力軸を有し、該出力軸は第１先側腕部１１の長手方向を回転軸として回動可能になっている。第１手首用回動機構１１ｃの出力軸は第１手首部１３と接続されている。そして、第１手首用回動機構１１ｃにより第１手首部１３は第１先側腕部１１の長手方向を回転軸の方向にして回動可能になっている。

第１手首部１３の内部には第１手部用回動機構１３ａが設置されている。第１手部用回動機構１３ａは回動する出力軸を有し、該出力軸は第１手首用回動機構１１ｃの出力軸と直交する方向を回転軸として回動可能になっている。第１手部用回動機構１３ａの出力軸を軸方向から挟むように門型の第１手部１４が設置されている。第１手部用回動機構１３ａにより第１手部１４は第１手部用回動機構１３ａの出力軸を回転軸として回動可能になっている。

第１手部１４の内部には第１指用直動機構１４ａが設置されている。そして、第１指用直動機構１４ａには一対の直方体の第１指部１５が設置されている。そして、第１指用直動機構１４ａは第１指部１５の間隔を変更することが可能になっている。

第１肩関節部９、第１元側腕部１０、第１先側腕部１１、第１手首部１３、第１手部１４及び第１指部１５等により腕部としての第１腕部１６が構成されている。胴体部６において第１肩部８の反対側には第１肩部８及び第１腕部１６の同様な機能と構造とを備えた第２肩部１７及び腕部としての第２腕部１８が設置されている。

つまり、胴体部６において第１肩部８の反対側には第１肩用回動機構６ａ、第１肩部８、第１肩関節用回動機構８ａ、出力軸８ｂに対応して第２肩用回動機構６ｂ、第２肩部１７、第２肩関節用回動機構１７ａ、出力軸１７ｂが設置されている。

さらに、第１腕部１６の第１肩関節部９、第１元側腕部１０、第１腕捻り用回動機構１０ａに対応して第２腕部１８には第２肩関節部１９、第２元側腕部２０、第２腕捻り用回動機構２０ａが設置されている。さらに、第１腕部１６の第１先側腕部１１、第１腕関節用回動機構１１ａ、出力軸１１ｂ、第１手首用回動機構１１ｃに対応して第２腕部１８には第２先側腕部２３、第２腕関節用回動機構２３ａ、出力軸２３ｂ、第２手首用回動機構２３ｃが設置されている。

さらに、第１腕部１６の第１腕用角速度検出器１２と対応して第２腕部１８には第２腕用角速度検出器２４が設置されている。さらに、第１腕部１６の第１手首部１３、第１手部用回動機構１３ａ、第１手部１４、第１指用直動機構１４ａに対応して第２腕部１８には第２手首部２５、第２手部用回動機構２５ａ、連結部及び手部としての第２手部２６、第２指用直動機構２６ａが設置されている。さらに、第２腕部１８には第２指部２７が設置されている。

胴体部６、第１肩部８、第２肩部１７等により本体部２８が構成されている。ロボット１は基台３に対して本体部２８を回動させることができる。そして、本体部２８に対して第１腕部１６及び第２腕部１８を移動させることができる。図１（ｂ）において第１移動範囲１ａは本体部２８を固定した状態で第１腕部１６が移動可能な範囲を示している。そして、第２移動範囲１ｂは本体部２８を固定した状態で第２腕部１８が移動可能な範囲を示している。第３移動範囲１ｃは本体部２８を回動させて第１腕部１６及び第２腕部１８が移動可能な範囲を示している。

制御部４は、第１腕部１６の第１肩関節用回動機構８ａ、第１腕関節用回動機構１１ａ、第１手部用回動機構１３ａを駆動させることにより第１指部１５を第１移動範囲１ａの所望の場所へ移動させることができる。そして、制御部４、は第１腕捻り用回動機構１０ａを駆動させることにより第１手首部１３を上下に移動させたり水平に移動させたりすることができる。さらに、制御部４は、第１手首用回動機構１１ｃ及び第１手部用回動機構１３ａを駆動することにより、第１指部１５の長手方向をＺ方向、Ｘ方向、Ｙ方向等の所望の方向に移動させることができる。従って、制御部４は第１指部１５を所望の姿勢にして、第１指部１５がワークを挟んで把持することが可能になっている。

同様に、制御部４は、第２腕部１８の第２肩関節用回動機構１７ａ、第２腕関節用回動機構２３ａ、第２手部用回動機構２５ａを回動させることにより第２指部２７を第２移動範囲１ｂの所望の場所へ移動させることができる。第２腕部１８は第１腕部１６と同様な構造となっている。従って、制御部４は第２指部２７を所望の姿勢にして、第２指部２７がワークを挟んで把持することが可能になっている。さらに、制御部４は基台３に対して胴体部６を回動させることにより第３移動範囲１ｃの所望の場所へ第１指部１５または第２指部２７を移動させることができる。従って、第１移動範囲１ａの中に始動点２９と終止点３０とを設定し、制御部４は第１手部１４を始動点２９から終止点３０まで移動させることができる。

図２は角速度検出器の構造を示す概略斜視図である。本体用角速度検出器７、第１腕用角速度検出器１２、第２腕用角速度検出器２４は同様の構造となっている。本体用角速度検出器７について説明し、第１腕用角速度検出器１２、第２腕用角速度検出器２４の説明は省略する。

図２に示すように、本体用角速度検出器７は支持部３１を備え、支持部３１は直交する３つの方向を向く第１面３１ａ、第２面３１ｂ、第３面３１ｃを備えている。そして、第１面３１ａ、第２面３１ｂ、第３面３１ｃの各面の法線方向をそれぞれＸ方向、Ｙ方向、Ｚ方向とする。第１面３１ａ、第２面３１ｂ、第３面３１ｃにはそれぞれ第１ジャイロセンサー３２、第２ジャイロセンサー３３、第３ジャイロセンサー３４が設置されている。

各ジャイロセンサーには角速度を検出する水晶振動子が内蔵されている。そして、第１ジャイロセンサー３２はＸ方向を回転中心とする角速度を検出し、第２ジャイロセンサー３３はＹ方向を回転中心とする角速度を検出する。第３ジャイロセンサー３４はＺ方向を回転中心とする角速度を検出する。従って、本体用角速度検出器７が回転するときに、回転角度のＸＹＺ方向の各成分をそれぞれ第１ジャイロセンサー３２、第２ジャイロセンサー３３、第３ジャイロセンサー３４が検出する。従って、本体用角速度検出器７は３次元の回転速度を検出する３軸ジャイロセンサーとなっている。

図３は、組立装置の電気制御ブロック図である。図３において、ロボット１は動作を制御する制御部４を備えている。そして、制御部４はプロセッサーとして各種の演算処理を行うＣＰＵ（中央演算処理装置）３５と、各種情報を記憶するメモリー３６とを備えている。

本体駆動装置３７、第１腕部駆動装置３８、第２腕部駆動装置３９は入出力インターフェイス４２及びデータバス４３を介してＣＰＵ３５に接続されている。さらに、本体用角速度検出器７、第１腕用角速度検出器１２、第２腕用角速度検出器２４、入力装置４４、出力装置４５も入出力インターフェイス４２及びデータバス４３を介してＣＰＵ３５に接続されている。

本体駆動装置３７は本体回動機構５を駆動する装置である。本体駆動装置３７が本体回動機構５を駆動することにより、胴体部６は基台３に対して所定の相対角度で回動することができる。第１腕部駆動装置３８は第１腕部１６を駆動する装置である。第１腕部駆動装置３８は第１腕部１６に設置させた各回動機構を駆動することにより第１腕部１６を所望の姿勢に動かすことができる。同様に、第２腕部駆動装置３９は第２腕部１８を駆動する装置である。第２腕部駆動装置３９は第２腕部１８に設置させた各回動機構を駆動することにより第２腕部１８を所望の姿勢に動かすことができる。

入力装置４４はキーボードや外部機器との接続インターフェイス等のデータを入力する装置である。他にも入力装置４４にはロボット１の動作を入力するテーチング用の装置が含まれる。出力装置４５はロボット１の状態や各種データを表示する表示装置や、外部機器に出力する接続インターフェイス等の装置である。操作者は入力装置４４を用いて各種データを入力して出力装置４５にて確認することができるようになっている。

メモリー３６は、ＲＡＭ、ＲＯＭ等といった半導体メモリーや、ハードディスク、ＤＶＤ−ＲＯＭといった外部記憶装置を含む概念である。機能的には、ロボット１の動作の制御手順が記述されたプログラムソフト４６を記憶する記憶領域や、第１腕部１６及び第２腕部１８の長さや稼動域等の情報であるロボット属性データ４７を記憶する記憶領域が設定される。他にも、本体用角速度検出器７、第１腕用角速度検出器１２、第２腕用角速度検出器２４が検出する振動のデータである振動関連データ４８を記憶するための記憶領域が設定される。他にも、ＣＰＵ３５のためのワークエリアやテンポラリーファイル等として機能する記憶領域やその他各種の記憶領域が設定される。

ＣＰＵ３５は、メモリー３６内に記憶されたプログラムソフト４６に従って、ロボット１の制御を行うものである。具体的な機能実現部としてロボット１の姿勢を制御する姿勢制御部４９を有する。ロボット１が備える各回動機構は回転角度を検出するセンサーを備えている。そして、本体駆動装置３７、第１腕部駆動装置３８、第２腕部駆動装置３９は各センサーが出力する各回動機構の出力軸の角度情報を姿勢制御部４９に伝送する。そして、姿勢制御部４９は本体部２８、第１腕部１６、及び第２腕部１８の姿勢を認識することが可能になっている。姿勢制御部４９はロボット１の姿勢を認識して姿勢を変更する指示を本体駆動装置３７、第１腕部駆動装置３８、第２腕部駆動装置３９に出力する。そして、本体駆動装置３７、第１腕部駆動装置３８、第２腕部駆動装置３９がそれぞれ本体回動機構５、第１腕部１６、第２腕部１８を駆動することにより姿勢制御部４９はロボット１の姿勢を制御する。

他にも、ＣＰＵ３５は本体部２８、第１腕部１６、第２腕部１８の振動を解析する振動演算部５０を有する。振動演算部５０は本体用角速度検出器７が出力する信号を入力して基台３に対する本体部２８の振動を演算する。さらに、振動演算部５０は第１腕用角速度検出器１２が出力する信号を入力して本体部２８に対する第１腕部１６の振動を演算する。さらに、振動演算部５０は第２腕用角速度検出器２４が出力する信号を入力して本体部２８に対する第２腕部１８の振動を演算する。そして、振動演算部５０は振動のデータを姿勢制御部４９に伝達し、姿勢制御部４９は本体部２８、第１腕部１６、第２腕部１８の振動を減衰させる駆動をする。姿勢制御部４９は本体部２８、第１腕部１６、第２腕部１８の各振動を並行して減衰させる制御を行う。

図４は振動演算部の機能の一部を示すブロック図である。図４に示すように、振動演算部５０は本体用周波数フィルター５１、第１腕用周波数フィルター５２及び第２腕用周波数フィルター５３を有している。本体用周波数フィルター５１は本体用角速度検出器７が出力する本体角速度信号５４を入力し、本体部２８の振動に対応する周波数を通過させるフィルターである。そして、本体用周波数フィルター５１は本体部２８の振動に対応する角速度の信号である本体振動角速度信号５５を姿勢制御部４９に出力する。さらに、振動演算部５０は第１差分演算部５６と第２差分演算部５７とを備えている。そして、本体用周波数フィルター５１は本体振動角速度信号５５を第１差分演算部５６及び第２差分演算部５７にも出力する。

同様に、第１腕用周波数フィルター５２は第１腕用角速度検出器１２が出力する腕角速度信号としての第１腕角速度信号５８を入力し、振動に対応する周波数を通過させるフィルターである。そして、第１腕用周波数フィルター５２は第１腕部１６の振動に対応する角速度の信号である第１腕振動角速度信号６１を第１差分演算部５６に出力する。

第２腕用周波数フィルター５３は第２腕用角速度検出器２４が出力する腕角速度信号としての第２腕角速度信号６２を入力し、振動に対応する周波数を通過させるフィルターである。そして、第２腕用周波数フィルター５３は第２腕部１８の振動に対応する角速度の信号である第２腕振動角速度信号６３を第２差分演算部５７に出力する。

第１差分演算部５６は本体振動角速度信号５５と第１腕振動角速度信号６１とを入力する。第１差分演算部５６は本体振動角速度信号５５から本体回動機構５の出力軸５ａと平行な軸回りの角速度成分を抽出する。さらに、第１差分演算部５６は第１腕振動角速度信号６１から本体回動機構５の出力軸５ａと平行な軸回りの角速度成分を抽出する。そして、第１差分演算部５６は本体振動角速度信号５５から抽出した角速度と第１腕振動角速度信号６１から抽出した角速度との差分を演算し、演算結果である第１腕差分角速度信号６４を姿勢制御部４９に出力する。第１腕差分角速度信号６４は本体部２８に対する第１腕部１６の振動による角速度の変動のうち本体回動機構５の出力軸５ａと平行な軸回りの角速度の信号である。

同様に、第２差分演算部５７は本体振動角速度信号５５と第２腕振動角速度信号６３とを入力する。第２差分演算部５７は本体振動角速度信号５５から本体回動機構５の出力軸５ａと平行な軸回りの角速度成分を抽出する。さらに、第２差分演算部５７は第２腕振動角速度信号６３から本体回動機構５の出力軸５ａと平行な軸回りの角速度成分を抽出する。そして、第２差分演算部５７は本体振動角速度信号５５から抽出した角速度と第２腕振動角速度信号６３から抽出した角速度との差分を演算した第２腕差分角速度信号６５を姿勢制御部４９に出力する。第２腕差分角速度信号６５は本体部２８に対する第２腕部１８の振動による角速度の変動のうち本体回動機構５の出力軸５ａと平行な軸回りの角度成分の信号である。

本体角速度信号５４から本体振動角速度信号５５を算出する演算、第１腕角速度信号５８から第１腕差分角速度信号６４を算出する演算、第２腕角速度信号６２から第２腕差分角速度信号６５を算出する演算、の各演算は独立して並行して行われる。これらの演算はプログラムソフト４６によりＣＰＵ３５が行うが、これらの演算を行う電子機器を用いても良い。電子機器にすることにより高速に処理することができる。

姿勢制御部４９は振動演算部５０から本体振動角速度信号５５を入力する。そして、姿勢制御部４９は本体回動機構５を駆動して基台３に対する本体部２８の出力軸５ａ回りの回転振動を減衰させる。さらに、姿勢制御部４９は振動演算部５０から第１腕差分角速度信号６４を入力する。そして、姿勢制御部４９は第１肩用回動機構６ａ、第１肩関節用回動機構８ａ、第１腕捻り用回動機構１０ａ、第１腕関節用回動機構１１ａを駆動して本体部２８に対する第１腕部１６の出力軸５ａと同じ軸方向を回転中心とする回転振動を減衰させる。さらに、姿勢制御部４９は振動演算部５０から第２腕差分角速度信号６５を入力する。そして、姿勢制御部４９は第２肩用回動機構６ｂ、第２肩関節用回動機構１７ａ、第２腕捻り用回動機構２０ａ、第２腕関節用回動機構２３ａを駆動して本体部２８に対する第２腕部１８の出力軸５ａと同じ軸方向を回転中心とする回転振動を減衰させる。姿勢制御部４９は、基台３に対する本体部２８、本体部２８に対する第１腕部１６、本体部２８に対する第２腕部１８の各回転振動を並行して減衰させる。

次に、上述したロボット１が第１腕部１６及び第２腕部１８の振動を減衰させるときにロボット１を制御する方法について図５及び図６にて説明する。図５は、腕部の振動を減衰させる制御のフローチャートであり、図６は腕部の振動を減衰させる制御方法を説明するためのタイムチャートである。

図５のフローチャートにおいて、ステップＳ１は腕移動工程に相当し、第１手部１４を始動点２９から終止点３０へ移動させる工程である。次にステップＳ２に移行する。ステップＳ２は腕停止工程に相当し、第１腕部１６を終止点３０に停止させる工程である。次にステップＳ５に移行する。ステップＳ３は、角度検出工程に相当し、本体用角速度検出器７、第１腕用角速度検出器１２、第２腕用角速度検出器２４が検出した角速度信号を振動演算部５０に出力する工程である。次にステップＳ５に移行する。ステップＳ４は、振動減衰工程に相当する。振動演算部５０が第１腕差分角速度信号６４及び第２腕差分角速度信号６５を算出し、姿勢制御部４９が本体部２８、第１腕部１６及び第２腕部１８の振動を減衰させる工程である。次にステップＳ５に移行する。尚、ステップＳ１〜ステップＳ２とステップＳ３とステップＳ４とは並行して行われる。

ステップＳ５は、終了判断工程に相当する。この工程は、作業を継続するか終了するかを判断する工程である。作業を継続するときは次にステップＳ１に移行する。作業を終了するときは腕部の振動を減衰させる制御を終了する。

次に、図６を用いて、図５に示したステップと対応させて、腕部の振動を減衰させる制御方法を詳細に説明する。図６において、本体推移線６６は本体部２８のＺ方向を回転中心とした角度の推移を示し、本体用角速度検出器７が出力する本体角速度信号５４の推移を示している。本体推移線６６の縦軸は角度を示し、図中上側が反時計回りの角度であり、図中下側が時計回りの角度となっている。横軸は時間の推移を示し時間は図中左から右へ推移する。

腕部差分推移線６７は本体部２８に対する第１先側腕部１１の位置の推移を示し、腕部推移線６８は基台３に対する第１先側腕部１１の位置の推移を示している。腕部差分推移線６７及び腕部推移線６８の縦軸は第１手首部１３の位置を示し、始動点２９と終止点３０とを通る直線上の位置を示している。図中上側が終止点３０側であり、図中下側が始動点２９側となっている。横軸は時間の推移を示し時間は図中左から右へ推移する。

横軸において移動開始時６９ｂの左側は移動前静止区間６９ａであり、ロボット１は本体部２８、第１腕部１６ともに静止状態である。従って、本体推移線６６では振動が小さい状態となっている。この区間においてもステップＳ３の角度検出工程及びステップＳ４の振動減衰工程が行われている。そして、移動開始時６９ｂにおいてステップＳ１の腕移動工程が開始され第１腕部１６が動き出す。移動開始時６９ｂから停止時６９ｄまでの区間が移動区間６９ｃとなっている。移動区間６９ｃはステップＳ１の腕移動工程の区間である。移動開始時６９ｂにおいて、本体部２８及び第１腕部１６には加速度が作用するので、本体部２８、第１腕部１６ともに大きく振動する。

ステップＳ１の腕移動工程においてステップＳ３の角度検出工程及びステップＳ４の振動減衰工程が並行して行われる。振動演算部５０は本体用角速度検出器７から本体角速度信号５４を入力し振動成分を抽出する。そして、振動演算部５０は姿勢制御部４９に本体振動角速度信号５５を出力する。姿勢制御部４９は本体振動角速度信号５５を入力し、本体駆動装置３７に本体部２８の振動が減衰するように本体回動機構５を駆動させる。詳細には本体部２８の回転振動と逆位相の駆動波形にて本体駆動装置３７が本体回動機構５を駆動する。これにより、本体推移線６６に示すように振動の振幅が減衰する。

第１腕部１６においても、振動演算部５０は本体用角速度検出器７から本体角速度信号５４を入力し振動成分の信号である本体振動角速度信号５５を抽出する。さらに、振動演算部５０は第１腕用角速度検出器１２から第１腕角速度信号５８を入力し振動成分である第１腕振動角速度信号６１を抽出する。そして、振動演算部５０の第１差分演算部５６は本体振動角速度信号５５と第１腕振動角速度信号６１との差分である第１腕差分角速度信号６４を演算する。振動演算部５０は、本体部２８に対する第１腕部１６の振動を示す第１腕差分角速度信号６４を姿勢制御部４９に出力する。

姿勢制御部４９は第１腕差分角速度信号６４を入力し、第１腕部駆動装置３８に第１腕部１６の振動が減衰するように第１腕部駆動装置３８を駆動させる。詳細には本体部２８に対する第１先側腕部１１の回転振動と逆位相の駆動波形にて第１腕部駆動装置３８が第１肩関節用回動機構８ａ及び第１腕関節用回動機構１１ａ等の回動機構を駆動する。これにより、腕部差分推移線６７に示すように振動の振幅が減衰する。本体部２８の振動が減衰し、第１腕部１６の振動も減衰することから腕部推移線６８が示すように基台３に対しても第１腕部１６の振動は減衰する。

そして、停止時６９ｄにおいてステップＳ２の腕停止工程が開始され第１腕部１６が停止する。停止時６９ｄ以降の区間が停止区間６９ｅとなっている。停止区間６９ｅはステップＳ２の腕停止工程の区間である。停止時６９ｄにおいて、本体部２８及び第１腕部１６には減速するために移動開始時６９ｂのときの逆方向の加速度が加わる。この加速度の作用により本体部２８及び第１腕部１６はともに大きく振動する。

ステップＳ２の腕停止工程においてもステップＳ３の角度検出工程とステップＳ４の振動減衰工程とが並行して行われる。このため、本体部２８及び第１腕部１６は振動が減衰する。従って、本体推移線６６、腕部差分推移線６７及び腕部推移線６８において振幅が小さくなる。

尚、腕部差分推移線６７及び腕部推移線６８は第１腕部１６を移動させたときの挙動を示した。第２腕部１８を単独で移動させるときも腕部差分推移線６７及び腕部推移線６８と同様の挙動となる。さらに、第１腕部１６と第２腕部１８とを同時に移動させるときにも、振動演算部５０が本体部２８の振動、第１腕部１６の振動及び第２腕部１８の振動を演算して抽出する。そして、姿勢制御部４９が本体駆動装置３７、第１腕部駆動装置３８及び第２腕部駆動装置３９を駆動させて本体回動機構５、第１腕部１６及び第２腕部１８の振動を減衰させる。従って、本体推移線６６、腕部差分推移線６７及び腕部推移線６８と同様に振幅を小さくすることができる。

上述したように、本実施形態によれば、以下の効果を有する。
（１）本実施形態によれば、基台３に本体部２８が設置され、基台３に対して本体部２８が回動する。そして、本体部２８には本体用角速度検出器７が設置され、本体用角速度検出器７は本体角速度信号５４を制御部４に出力する。制御部４は本体部２８が回動する動作を制御する。このとき、制御部４は本体角速度信号５４を用いているので、本体部２８の振動を検出することができる。従って、制御部４は本体角速度信号５４を用いないときに比べて短い時間で本体部２８の振動を減衰させることができる。

（２）本実施形態によれば、本体部２８には第１腕部１６が接続され、本体部２８に対して第１腕部１６が回動する。そして、第１腕部１６には第１腕用角速度検出器１２が設置され、第１腕用角速度検出器１２は第１腕角速度信号５８を制御部４に出力する。制御部４は第１腕部１６が回動する動作を制御する。このとき、制御部４は本体角速度信号５４と第１腕角速度信号５８とを用いているので、本体部２８に対する第１腕部１６の振動を検出することができる。そして、制御部４は第１腕部１６の振動を減衰させる制御を行う。さらに、ロボット１は本体部２８に対する第１腕部１６の振動が減衰される。従って、制御部４は第１腕角速度信号５８だけを用いて第１腕部１６の振動を減衰させるときに比べて短い時間で本体部２８に対する第１腕部１６の振動を減衰させることができる。その結果、ロボット１は第１腕部１６の振動を短時間で減衰させることができる。

（３）本実施形態によれば、第１腕用角速度検出器１２は３軸ジャイロセンサーであることから第１腕用角速度検出器１２は各々直交する３軸回りの角速度を検出する。そして、制御部４は第１腕角速度信号５８のうち本体部２８が回動する回転軸と平行な軸回りの角速度成分を抽出する。従って、制御部４は第１腕部１６の振動のうち本体部２８が回動する回転軸回りの振動を精度良く減衰させることができる。

（４）本実施形態によれば、ロボット１は第１腕部１６と第２腕部１８とを備えている。第１腕部１６には第１腕用角速度検出器１２が設置され、第２腕部１８には第２腕用角速度検出器２４が設置されている。そして、第１腕用角速度検出器１２は第１腕角速度信号５８を制御部４に出力し、第２腕用角速度検出器２４は第２腕角速度信号６２を制御部４に出力する。従って、本体部２８、第１腕部１６、第２腕部１８が振動するときにも本体部２８の振動を減衰させるとともに、本体部２８に対する第１腕部１６の振動と本体部２８に対する第２腕部１８の振動とを減衰させる。従って、制御部４は振動する各場所の振動を並行して減衰させるので短時間で本体部２８、第１腕部１６、第２腕部１８を減衰させることができる。

（５）本実施形態によれば、ステップＳ３の角度検出工程では、本体部２８の角速度を検出して本体角速度信号５４を出力する。ステップＳ４の振動減衰工程では本体角速度信号５４を用いることにより本体部２８の振動を検出し、基台３に対する本体部２８の振動を減衰させている。

同様に、ステップＳ４の振動減衰工程では本体角速度信号５４と第１腕角速度信号５８とを用いることにより本体部２８に対する第１腕部１６の振動を検出し、本体部２８に対する第１腕部１６の振動を減衰させている。並行して、本体角速度信号５４と第２腕角速度信号６２とを用いることにより本体部２８に対する第２腕部１８の振動を検出し、本体部２８に対する第２腕部１８の振動を減衰させている。これにより、ロボット１は本体部２８の振動が減衰され、本体部２８に対する第１腕部１６及び第２腕部１８の振動が減衰される。従って、ロボット１は腕角速度信号だけを用いて腕部の振動を減衰させるときに比べて短い時間で第１腕部１６及び第２腕部１８の振動を減衰させることができる。

尚、本実施形態は上述した実施形態に限定されるものではなく、種々の変更や改良を加えることも可能である。変形例を以下に述べる。
（変形例１）
前記実施形態では、ロボット１は双腕であったが、腕の数は２つに限らず３つ以上でもよい。このときにも、本体部２８の振動と各腕の振動を減衰させることができるので、短時間で各腕の振動を減衰させることができる。

（変形例２）
前記実施形態では、本体用角速度検出器７、第１腕用角速度検出器１２、第２腕用角速度検出器２４は３軸ジャイロセンサーであったが、１軸ジャイロセンサーを直交する３方向に設置しても良い。このときにも、３方向の角速度を検出できる。他にも、１軸ジャイロセンサーと２軸ジャイロセンサーとを組み合わせても良い。このときにも、本体部２８が回動する出力軸５ａと同じ回転軸回りの角速度を検出することができる。

３…基台、４…制御部、５ａ…回転軸としての出力軸、７…本体用角速度検出器、１２…腕用角速度検出器としての第１腕用角速度検出器、１６…腕部としての第１腕部、１８…腕部としての第２腕部、２４…腕用角速度検出器としての第２腕用角速度検出器、２８…本体部、５４…本体角速度信号、５８…腕角速度信号としての第１腕角速度信号、６２…腕角速度信号としての第２腕角速度信号。

Claims (5)

1. 基台と、
   前記基台に回動可能に接続された本体部と、
   前記本体部に回動可能に接続された第１腕部と、
   前記本体部に回動可能に接続された第２腕部と、
   前記本体部に設けられた第１角速度検出器と、
   前記第１腕部に設けられた第２角速度検出器と、を備え、
   前記第１角速度検出器からの出力に基づいて、前記本体部の振動を制御することを特徴とするロボット。
2. 請求項１に記載のロボットであって、
   前記第１角速度検出器からの出力と、前記第２角速度検出器からの出力とに基づいて、前記第１腕部の振動を制御することを特徴とするロボット。
3. 請求項１または２に記載のロボットであって、
   前記第２角速度検出器は、３軸ジャイロセンサーであることを特徴とするロボット。
4. 請求項１ないし３のいずれか一項に記載のロボットであって、
   前記第２腕部に設けられた第３角速度検出器と、を備え、ことを特徴とするロボット。
5. 請求項４に記載のロボットであって、
   前記第１角速度検出器からの出力と、前記第３角速度検出器からの出力とに基づいて、前記第２腕部の振動を制御することを特徴とするロボット。

Images