JP7422215B2

JP7422215B2 - 多関節ロボット

Info

Publication number: JP7422215B2
Application number: JP2022508631A
Authority: JP
Inventors: 啓祐名桐
Original assignee: Individual
Current assignee: Individual
Priority date: 2020-03-16
Filing date: 2020-03-16
Publication date: 2024-01-25
Anticipated expiration: 2040-03-16
Also published as: JPWO2021186518A1; WO2021186518A1

Description

本明細書は、多関節ロボットについて開示する。

従来、ロボットアームが障害物に衝突したことが検知されると、ロボットを停止させる多関節ロボットが知られている。例えば、特許文献１には、ロボットアームと、トルク指令を指令する制御部と、制御部が供給する電流により回転してロボットアームを駆動するサーボモータと、サーボモータの位置を検出するエンコーダと、衝突検出装置と、衝突検出装置監視部と、ロボット停止処理部と、を備える多関節ロボットが開示されている。衝突検出装置は、制御部のトルク指令とエンコーダの回転信号を受けてロボットアームが受ける外乱を推定する。衝突検出装置監視部は、推定された外乱トルクが既定値以上になったとき、衝突が生じていると判断する。ロボット停止処理部は、衝突検出装置監視部で衝突と判断されると、ロボットを停止させる。

特開２０１０－１３７３１２号公報

しかしながら、多関節ロボットは、アームに過大な外力が加わったときに、アームの停止のさせ方によっては、アームから外力に釣り合うトルクが出力され続け、ダメージを受ける場合がある。

本開示は、アームに過大な外力が加わったときに、モータから外力に釣り合うトルクが発生され続けるのを回避して、ロボットが受けるダメージを最小限にすることができる多関節ロボットを提供することを主目的とする。

本開示は、上述の主目的を達成するために以下の手段を採った。

本開示の多関節ロボットは、
関節を介して連結された複数のアームを有する多関節ロボットであって、
前記複数のアームを回動させる複数のモータと、
前記複数のモータをそれぞれ対応するトルク指令に基づいて駆動する駆動部と、
前記複数のアームの位置が目標位置に達するようフィードバック制御により前記複数のモータのトルク指令を設定すると共に前記複数のモータのトルクを監視し、前記複数のモータのうちいずれかのモータのトルクが閾値を超えると、前記複数のモータのうち少なくとも一つのモータのトルク指令を零に設定する制御部と、
を備えることを要旨とする。

この本開示の多関節ロボットは、複数のアームの位置が目標位置に達するようフィードバック制御により複数のモータのトルク指令を設定すると共に複数のモータのトルクを監視する。そして、多関節ロボットは、複数のモータのうちいずれかのモータのトルクが閾値を超えると、複数のモータのうち少なくとも一つのモータのトルク指令を零に設定する。これにより、障害物との衝突などアームに過大な外力が加わったときに、フィードバック制御によりモータから外力に釣り合うトルクが発生され続けるのを回避して、ロボットが受けるダメージを最小限にすることができる。

本実施形態の作業ロボットの概略構成図である。ロボット本体と制御装置との電気的な接続関係を示すブロック図である。モータ制御の機能ブロック図である。衝突判定処理の一例を示すフローチャートである。停止処理の一例を示すフローチャートである。

次に、本開示を実施するための形態について図面を参照しながら説明する。

図１は、本実施形態の作業ロボットの概略構成図である。図２は、ロボット本体と制御装置との電気的な接続関係を示すブロック図である。作業ロボット１０は、例えば、工具でワークを加工する加工作業やチャックでワークを把持して別位置へ搬送する搬送作業、チャックでワークを把持して対象物に組み付ける組付作業などの作業を行なうものであり、図示するように、ロボット本体２０と制御装置７０と備える。

ロボット本体２０は、本実施形態では、５軸の垂直多関節ロボットとして構成されている。このロボット本体２０は、図１に示すように、ベース２１と、ベース２１に対して各関節軸（関節軸Ｊ１～Ｊ５）を介して直列に接続された複数のアーム（第１～第５アーム３１～３５）と、を備える。各関節軸Ｊ１～Ｊ５には、図２に示すように、対応する関節軸を回転駆動するサーボモータ（第１～第５モータ４１～４５）と、対応するサーボモータの回転角度Θ１～Θ５を検出するロータリエンコーダ（第１～第５エンコーダ５１～５５）とが配置されている。

第１～第５アーム３１～３５のうち先端に配置される第５アーム３５には、作業ロボット１０が行なう作業に適したエンドエフェクタＥＦが装着される。

制御装置７０は、図２に示すように、ＣＰＵ７１を中心としたマイクロプロセッサとして構成され、ＣＰＵ７１の他に、ＲＯＭ７２やＨＤＤ７３（記憶装置）、ＲＡＭ７４、入出力インタフェース（図示せず）などを備える。制御装置７０には、各関節軸Ｊ１～Ｊ５の第１～第５エンコーダ５１～５５からの検知信号である回転角度Θ１～Θ５が入出力インタフェースを介して入力される。また、制御装置７０からは、各関節軸Ｊ１～Ｊ５の第１～第５モータ４１～４５に駆動用の電流を印加する各アンプ（ドライバ）６１～６５への制御信号であるトルク指令Ｔｍ１＊～Ｔｍ５＊が入出力インタフェースを介して出力される。更に、制御装置７０には、マウスやキーボードなどの入力装置８１や、ディスプレイなどの出力装置８２も接続されている。

次に、こうして構成された作業ロボット１０の動作について説明する。制御装置７０のＣＰＵ７１は、まず、エンドエフェクタＥＦの目標位置および目標姿勢を取得する。続いて、ＣＰＵ７１は、エンドエフェクタＥＦを目標位置および目標姿勢に移動させるための各関節軸Ｊ１～Ｊ５の目標回転角度Θ１＊～Θ５＊を逆運動学を用いて計算する。次に、ＣＰＵ７１は、第１～第５エンコーダ５１～５５により検出される各関節軸Ｊ１～Ｊ５の回転角度Θ１～Θ５が対応する目標回転角度Θ１＊～Θ５＊に一致するようフィードバック制御により第１～第５モータ４１～４５のトルク指令Ｔｍ１＊～Ｔｍ５＊を設定して対応するアンプ６１～６５に出力する。

図３は、モータ制御の機能ブロック図である。図示するように、モータ制御の機能ブロックとしては、フィードフォワードトルク設定部９１と、位置フィードバック部９２と、速度フィードバック部９３と、を有する。フィードフォワードトルク設定部９１は、各関節軸Ｊｎ（ｎ＝１，…，５）に連結されるアームの自重によるモーメントを相殺するトルク（自重釣り合いトルク）を計算してフィードフォワードトルクＴｆｎ（ｎ＝１，…，５）を設定する。位置フィードバック部９２は、各関節軸Ｊｎ（ｎ＝１，…，５）の目標回転角度Θｎ＊（ｎ＝１，…，５）と第１～第５エンコーダ５１～５５からの回転角度Θｎ（ｎ＝１，…，５）との差分（Θｎ＊－Θｎ）を入力し、入力した差分（Θｎ＊－Θｎ）に基づく比例積分制御あるいは比例積分微分制御によって対応する関節軸の目標回転速度Ｎｍｎ＊（ｎ＝１，…，５）を設定する。速度フィードバック部９３は、位置フィードバック部９２で設定された各関節軸Ｊ１～Ｊ５の目標回転速度Ｎｍｎ＊と回転角度Θｎから角度／速度変換部９４によって変換された回転速度Ｎｍｎ（ｎ＝１，…，５）との差分（Ｎｍｎ＊－Ｎｍｎ）を入力し、入力した差分（Ｎｍｎ＊－Ｎｍｎ）に基づく比例積分制御あるいは比例積分微分制御によってフィードバックトルクＴｂｎ（ｎ＝１，…，５）を設定する。各アンプ６１～６５は、対応するフィードフォワードトルクＴｆｎとフィードバックトルクＴｂｎとの和のトルクをトルク指令Ｔｍｎ＊として入力し、入力したトルク指令Ｔｍｎ＊に応じたトルクが出力されるよう電流フィードバック制御によって対応するモータに電流を印加する。

次に、作業ロボット１０のアーム（第１から第５アーム３１～３５のいずれか）が障害物に衝突した際の動作について説明する。まず、アームが障害物に衝突したか否かを判定する動作を説明し、次に、アームが衝突したと判定した際にアームを停止させるための動作を説明する。図４は、制御装置７０のＣＰＵ７１により実行される衝突判定処理の一例を示すフローチャートである。この処理は、作業ロボット１０が作業を行なっている最中に所定時間毎（例えば、数ｍｓｅｃ毎）に繰り返し実行される。

衝突判定処理が実行されると、制御装置７０のＣＰＵ７１は、まず、変数ｎを値１に初期化する（ステップＳ１００）。続いて、ＣＰＵ７１は、関節軸Ｊｎの対応するモータのフィードバックトルクＴｂｎと対応するエンコーダからの回転角度Θｎとを入力する（ステップＳ１１０）。次に、ＣＰＵ７１は、回転角度Θｎを回転速度Ｎｍｎに変換し（ステップＳ１２０）、回転速度Ｎｍｎが所定速度Ｎｒｅｆ以上であるか否かを判定する（ステップＳ１３０）。ＣＰＵ７１は、回転速度Ｎｍｎが所定速度Ｎｒｅｆ以上であると判定すると、衝突判定閾値Ｔｒｅｆに第１所定値αを設定し（ステップＳ１４０）、回転速度Ｎｍｎが所定速度Ｎｒｅｆ未満であると判定すると、衝突判定閾値Ｔｒｅｆに第１所定値αよりも小さい第２所定値βを設定する（ステップＳ１５０）。そして、ＣＰＵ７１は、ステップＳ１１０で入力したフィードバックトルクＴｂｎが衝突判定閾値Ｔｒｅｆ以上であるか否かを判定する（ステップＳ１６０）。ＣＰＵ７１は、フィードバックトルクＴｂｎが衝突判定閾値Ｔｒｅｆ以上であると判定すると、関節軸Ｊｎに連結されているアームが障害物と衝突したと判定する（ステップＳ１７０）。一方、ＣＰＵ７１は、フィードバックトルクＴｂｎが衝突判定閾値Ｔｒｅｆ未満であると判定すると、関節軸Ｊｎに連結されているアームは障害物と衝突していないと判定する。そして、ＣＰＵ７１は、変数ｎが値５以上であるか否かを判定し（ステップＳ１８０）、変数ｎが値５未満であると判定すると、衝突の有無を判定していない関節軸があると判断し、変数ｎを値１だけインクリメントして（ステップＳ１９０）、ステップＳ１１０に戻り、次の関節軸Ｊｎについて衝突の有無を判定する。一方、ＣＰＵ７１は、変数ｎが値５以上であると判定すると、全ての関節軸に対して衝突の有無を判定したと判断して、衝突判定処理を終了する。

上述したように、フィードバックトルクＴｂｎは、関節軸Ｊｎの回転角度Θｎを目標回転角度Θｎ＊に一致させるために目標回転角度Θｎ＊と回転角度Θｎとの差分（Θｎ＊－Θｎ）に基づいて設定される。このため、いずれかのアームが障害物に衝突して当該アームに過大な外力が加わると、フィードバックトルクＴｂｎには、アームの位置および姿勢を保持するために当該外力と釣り合うトルクが設定され続けることになる。本実施形態では、ＣＰＵ７１は、各関節軸Ｊｎにおいて設定されるフィードバックトルクＴｂｎを監視することで、いずれかのアームに過大な外力が加わったか否か、すなわち、いずれかのアームが障害物に衝突したか否かを判定することができる。ここで、関節軸Ｊｎの回転速度Ｎｍｎが高速（所定速度Ｎｒｅｆ以上）である場合には、目標回転角度Θｎ＊と回転角度Θｎとの差分が比較的大きく、フィードバックトルクＴｂｎには、比較的大きなトルクが設定される。一方、関節軸Ｊｎの回転速度Ｎｍｎが低速（所定速度Ｎｒｅｆ未満）である場合には、目標回転角度Θｎ＊と回転角度Θｎとの差分が比較的小さく、フィードバックトルクＴｂｎには、比較的小さなトルクが設定される。このため、関節軸Ｊｎの回転速度Ｎｍｎに拘わらず一律の値を衝突判定閾値Ｔｒｅｆに設定してフィードバックトルクＴｂｎと衝突判定閾値Ｔｒｅｆとの比較により衝突判定を行なうと、衝突を誤判定するおそれが生じる。本実施形態では、関節軸Ｊｎの回転速度Ｎｍｎの高低に応じて衝突判定閾値Ｔｒｅｆを高低するから、上述した誤判定が生じるのを抑制することができる。

次に、衝突が発生した際にアームを停止させる処理について説明する。図５は、制御装置７０のＣＰＵ７１により実行される停止処理の一例を示すフローチャートである。この処理は、衝突判定処理が実行された後に実行される。

停止処理が実行されると、制御装置７０のＣＰＵ７１は、まず、上述した衝突判定処理の結果として、いずれかのアームに衝突が発生したか否かを判定する（ステップＳ２００）。ＣＰＵ７１は、いずれのアームにも衝突が発生していないと判定すると、停止処理を終了する。一方、ＣＰＵ７１は、いずれかのアームに衝突が発生したと判定すると、変数ｎを値１に初期化し（ステップＳ２１０）、関節軸Ｊｎがいなし対象軸であるか否かを判定する（ステップＳ２２０）。ここで、いなし対象軸とは、障害物との衝突によりアームに加わる過大な外力をいなすような動作を行なう関節軸である。本実施形態では、いなし対象軸には、関節軸Ｊ１～Ｊ５のうち入力装置８１を介してユーザにより予め選択されたものが設定されるものとした。ＣＰＵ７１は、関節軸Ｊｎがいなし対象軸であると判定すると、当該関節軸Ｊｎの対応するモータのトルク指令Ｔｍｎ＊を値０に設定する（ステップＳ２３０）。一方、ＣＰＵ７１は、関節軸Ｊｎがいなし対象軸でない（非いなし対象軸である）と判定すると、当該関節軸Ｊｎに連結されるアームが通常停止するよう当該関節軸Ｊｎの対応するモータのトルク指令Ｔｍｎ＊を設定する（ステップＳ２４０）。通常停止は、具体的には、関節軸Ｊｎに連結されるアームの自重によるモーメントを相殺するトルク（自重釣り合いトルク）が当該関節軸Ｊｎの対応するモータから出力されるようトルク指令Ｔｍｎ＊を設定することにより行なわれる。そして、ＣＰＵ７１は、変数ｎが値５以上であるか否かを判定し（ステップＳ２５０）、変数ｎが値５未満であると判定すると、変数ｎを値１だけインクリメントして（ステップＳ２６０）、ステップＳ２２０に戻り、次の関節軸Ｊｎの対応するモータのトルク指令Ｔｍｎ＊を設定する。一方、ＣＰＵ７１は、変数ｎが値５以上であると判定すると、全ての関節軸Ｊｎの対応するモータのトルク指令Ｔｍｎ＊を設定したと判断して、停止処理を終了する。

このように、本実施形態では、ＣＰＵ７１は、いずれかのアームに衝突が発生すると、関節軸Ｊ１～Ｊ５のうちいなし対象軸については、対応するモータのトルク指令Ｔｍｎ＊に値０を設定する。これにより、いなし対象軸に連結されるアームは、衝突により加わる外力によって移動させられることで、当該外力をいなすことができる。したがって、アームに過大な外力が継続して加わるのを回避して、作業ロボット１０が受けるダメージを最小限にすることが可能となる。また、ＣＰＵ７１は、関節軸Ｊ１～Ｊ５のうち非いなし対象軸については、対応するモータのトルク指令Ｔｍｎ＊に通常停止用のトルク（自重釣り合いトルク）を設定する。これにより、例えば、非いなし対象軸として、重力方向に駆動成分を有する関節軸が指定されることで、自重によるアームの落下を防止することが可能となる。

ここで、本実施形態の構成要素と本発明の構成要素との対応関係を明らかにする。本実施形態の第１～第５関節軸Ｊ１～Ｊ５が関節に相当し、第１～第５アーム３１～３５が複数のアームに相当し、アンプ（ドライバ）６１～６５が駆動部に相当し、制御装置７０が制御部に相当する。

なお、本発明は上述した実施形態に何ら限定されることはなく、本発明の技術的範囲に属する限り種々の態様で実施し得ることはいうまでもない。

例えば、上述した実施形態では、いなし対象軸（非いなし対象軸）は、関節軸Ｊ１～Ｊ５のうち入力装置８１を介してユーザにより選択されたものが設定されるものとした。しかし、いなし対象軸（非いなし対象軸）は、予め定められてもよい。この場合、例えば、本実施形態のロボット本体２０（垂直多関節ロボット）において、いなし対象軸を関節軸Ｊ１，Ｊ４，Ｊ５とし、非いなし対象軸を関節軸Ｊ２，Ｊ３とするなど、アームが自重によって落下するのを防止するために、非いなし対象軸として、重力方向に駆動成分を有する関節軸が定められることが望ましい。

上述した実施形態では、ＣＰＵ７１は、関節軸Ｊｎの回転速度Ｎｍｎが所定速度Ｎｒｅｆ以上の場合には、衝突判定閾値Ｔｒｅｆに第１所定値αを設定し、関節軸Ｊｎの回転速度Ｎｍｎが所定速度Ｎｒｅｆ未満の場合には、衝突判定閾値Ｔｒｅｆに第１所定値αよりも小さい第２所定値βを設定を設定するものとした。しかし、ＣＰＵ７１は、関節軸Ｊｎの回転速度Ｎｍｎが低くなるにつれて小さくなるように衝突判定閾値Ｔｒｅｆを設定してもよい。また、ＣＰＵ７１は、関節軸Ｊｎの回転速度Ｎｍｎに拘わらず衝突判定閾値Ｔｒｅｆに一定値を定めても差し支えない。

上述した本実施形態では、作業ロボット１０は、垂直多関節ロボットとして構成されたが、これに限られず、水平多関節ロボットとして構成されてもよい。水平多関節ロボットにおいて、アームの落下を防止するために、いなし対象軸には、各関節軸のうち水平旋回する関節軸が設定され、非いなし対象軸には、昇降する関節軸が設定されることが望ましい。

以上説明したように、本開示の多関節ロボットは、関節を介して連結された複数のアームを有する多関節ロボットであって、前記複数のアームを回動させる複数のモータと、前記複数のモータをそれぞれ対応するトルク指令に基づいて駆動する駆動部と、前記複数のアームの位置が目標位置に達するようフィードバック制御により前記複数のモータのトルク指令を設定すると共に前記複数のモータのトルクを監視し、前記複数のモータのうちいずれかのモータのトルクが閾値を超えると、前記複数のモータのうち少なくとも一つのモータのトルク指令を零に設定する制御部と、を備えることを要旨とする。

こうした本開示の多関節ロボットにおいて、前記制御部は、前記アームの自重と釣り合うトルクを含むトルクを出力するためのフィードフォワードトルクと、前記アームの位置を前記目標位置へ近づけるためのフィードバックトルクとの和に基づいて対応するモータのトルク指令を設定すると共に前記フィードバックトルクにより前記対応するモータのトルクを監視するものとしてもよい。フィードバックトルクには、アームに外力が加わった際に当該アームの姿勢を維持するために外力に釣り合うトルクが設定される。したがって、フィードバックトルクを監視することで、アームの衝突をより正確に判定することができる。

また、本開示の多関節ロボットにおいて、前記閾値は、前記モータの回転速度が所定速度未満の場合には前記所定速度以上の場合に比して小さな値が設定されるものとしてもよい。こうすれば、モータの回転速度に拘わらずアームの衝突をより正確に判定することができる。

さらに、本開示の多関節ロボットにおいて、前記制御部は、前記複数のモータのうちいずれかのモータのトルクが前記閾値を超えると、前記複数のモータのうち一部のモータのトルク指令を零に設定すると共に、他のモータから対応するアームを停止させると共に停止させたアームの自重と釣り合うトルクが出力されるよう該他のモータのトルク指令を設定するものとしてもよい。この場合、前記他のモータは、重力方向の駆動成分を含むモータであるものとしてもよい。こうすれば、アームに加わる外力を一部の関節軸によって適切にいなしつつ、自重によりアームが落下するのを防止することができる。

本開示は、多関節ロボットの製造産業などに利用可能である。

１０作業ロボット、２０ロボット本体、２１ベース、３１第１アーム、３２第２アーム、３３第３アーム、３４第４アーム、３５第５アーム、４１第１モータ、４２第２モータ、４３第３モータ、４４第４モータ、４５第５モータ、５１、第１エンコーダ、５２第２エンコーダ、５３第３エンコーダ、５４第４エンコーダ、５５第５エンコーダ、６１～６５アンプ、７０制御装置、７１ＣＰＵ、７２ＲＯＭ、７３ＨＤＤ、７４ＲＡＭ、８１入力装置、８２出力装置、９１フィードフォワードトルク設定部、９２位置フィードバック部、９３速度フィードバック部、９４角度／速度変換部。

Claims

ベースに対して関節軸を介して連結された複数のアームを有し、前記アームの関節軸として、鉛直方向に延在する鉛直関節軸と、水平方向に延在する水平関節軸と、を含む多関節ロボットであって、
前記複数のアームの関節軸を回転駆動する複数のモータと、
前記複数のモータをそれぞれ対応するトルク指令に基づいて駆動する駆動部と、
前記複数のアームの自重によるモーメントを相殺するようフィードフォワード制御により算出するトルクと前記複数のアームの位置が目標位置に達するようフィードバック制御により算出するトルクとに基づいて前記複数のモータのトルク指令を設定すると共に前記複数のモータのトルクを監視し、前記複数のモータのうちいずれかのモータのトルクが閾値を超えると、前記複数のアームの関節軸のうち予め設定された対象関節軸を回転駆動するモータのトルク指令を零に設定し、前記複数のアームの関節軸のうち前記対象関節軸でない関節軸を回転駆動するモータのトルク指令を前記フィードフォワード制御により算出するトルクに設定する制御部と、
を備え、
前記対象関節軸は、前記モータのトルク指令が零に設定されても、対応する前記アームが自重により落下しない前記鉛直関節軸である、
多関節ロボット。
請求項１に記載の多関節ロボットであって、
前記制御部は、前記アームの自重と釣り合うトルクを含むトルクを出力するためのフィードフォワードトルクと、前記アームの位置を前記目標位置へ近づけるためのフィードバックトルクとの和に基づいて対応するモータのトルク指令を設定すると共に前記フィードバックトルクにより前記対応するモータのトルクを監視する、
多関節ロボット。
請求項１または２に記載の多関節ロボットであって、
前記閾値は、前記モータの回転速度が所定速度未満の場合には前記所定速度以上の場合に比して小さな値が設定される、
多関節ロボット。
請求項１ないし３いずれか１項に記載の多関節ロボットであって、
前記制御部は、前記複数のモータのうちいずれかのモータのトルクが前記閾値を超えると、前記複数のモータのうち前記対象関節軸を回転駆動するモータのトルク指令を零に設定すると共に、他のモータから対応するアームを停止させると共に停止させたアームの自重と釣り合うトルクが出力されるよう該他のモータのトルク指令を設定する、
多関節ロボット。
請求項４に記載の多関節ロボットであって、
前記他のモータは、重力方向の駆動成分を含むモータである、
多関節ロボット。