WO2005009692A1

WO2005009692A1 - ロボットアームの制御方法および制御装置

Info

Publication number: WO2005009692A1
Application number: PCT/JP2004/009781
Authority: WO
Inventors: Hiroyuki Nakata; Naoto Masunaga; Atsumi Hashimoto; Yasushi Mukai
Original assignee: Matsushita Electric Industrial Co., Ltd.
Priority date: 2003-07-29
Filing date: 2004-07-02
Publication date: 2005-02-03
Also published as: JP4985659B2; JP4294646B2; JP2009072913A; EP1652634A4; EP2380710A1; EP2380710B1; US20060071625A1; EP2380711A1; JPWO2005009692A1; EP1652634A1; JP2009090462A; JP4983812B2; EP1652634B1; US7102315B2

Abstract

摩擦トルク計算に用いる角速度として、指令値と実測値のいずれかを適宜選択することにより、角速度指令に従って能動的に動作する場合と、外力に押されて受動的に動作する場合の両方で常に摩擦補償が有効にすることができる。また、衝突検出後、モータ回転方向と衝突方向が逆の場合は、位置制御から電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、衝突エネルギーを緩和する。その後、モータ回転速度が設定値以下になれば、柔軟制御に切り換え、衝突で生じた減速機等での歪みを解消する。一方、モータ回転方向と衝突方向が同じ場合は、電流制御を介さずに、位置制御から直接、柔軟制御に切り換える。衝突力に倣って動作させることにより、衝突トルクを緩和することができる。

Description

明細書ロボットアームの制御方法および制御装置

<技術分野〉

本発明は、モータにより駆動されるロボットの制御方法およぴ制御装置に関する。特に、ロボットアームの柔軟なサーボ制御（Compliance Servo Control) に関し、口ボットアームの衝突検出後の停止制御における制御方法及び制御装置に関する。ぐ背景技術 >

近年、ロボットは、産業分野だけに限らず、民生分野でも利用されてきており、安全性の確保が重要な技術となってきている。しかしながら、力センサ等により衝突などの外力を検出して、ロボットアームを停止する手段では、コストや負荷重量の増大を招き好ましくない。そこで、センサ等を用いずに衝突検出および停止動作制御を含む柔軟なサーポ制御を高性能化することが求められている。

センサレスで柔軟制御を実現する方法としては、位置フィードバック制御において、位置偏差の増加に対する電流指令の増加分を抑えることにより、位置偏差が大きくなっても過大なトルクをモータに発生させない様にすることが一般的である。

フィードバック制御による電流指令を抑える量に応じて、位置偏差に対するモータ発生トルクも抑えられ、柔軟性を高めることができる。

フィードパック制御において電流指令を抑える手段としては、特開平 09_179632 (米国特許 5994864)に示されるリミットをかける方式や、特開平 08-155868に示されるフィードパックゲインを下げる方式等が提案されている。

以上の様に、ロボットアームの柔軟性（Compl iance)を高めるためには、フイードバック制御における電流指令をどこまで抑えられるかが重要となる。フィ一ドバック制御による電流指令が抑えられなければ、通常のサーポ剛性に近づくことになり、サーボ制御の柔軟性は低くなる。

しかしながら、ロボットアームを動作させるためには、慣性トルクや摩擦トルク、さらには重力トルク等を考慮した駆動トルクをモータで発生させなければならず、フィードバック制御のみで動作させようとすると、モータ電流指令を抑えることは困難である。

図 3は従来例における実速度を用いた摩擦補償の制御方法を示すプロック線図である。図中、参照番号 1はモータ回転角指令 0 c o m、 2 ：フィードバック制御器、 3：電流制限手段、 4 ：フィードバック制御電流指令 I c o m、 5 ：モータ電流 I m、 6 ：モータ +実負荷を示す範囲、 7 ：モータトルク定数 K t、 8 ：モータ発生トルクて mm, 9 ：モータにカロわる外力 τ μ + τ ά γ η + τ ά i s、 1 0 :モータイ "一シャの伝達関数、 1 1 ：モータ回転角 0 f b、 1 2 : 微分計算子、 1 3 ：モータ角速度指令 ω c o m、 1 4 :微分計算子、 1 5 ：モータ角加速度指令 a c om、 1 6 ：モータイナーシャ（ロータ +減速機 1次側） J、 1 7 ：ロポット動作に必要なモータ電流 I m 1、 1 8 :モータトルク定数の逆数 1 ZK t、 1 9 ：動力学トルク計算値 τ d y n、 2 0 ：摩擦計算値 τ μ、 2 1 ：摩擦計算プロック、 2 2 ：動力学計算プロック、 2 3 ：モータ角速度 ω f b、 2 4 ：微分演算子、 2 5 ：他軸モータ回転角、を示す。

ロボット動作時におけるモータ発生トルク τ πιは、において、モータ駆動側から見れば式（1)、負荷側から見れば（2) で表される。

τ ram = Kt * Im (1)

τ ml = J * a + τ μ + て dyn + て dis · · · (2)

ただし、式（1)、（2)における記号は以下のとおりである。

Kt ：モータトルク定数

Im：モータ電流

a ：モータ角加速度

ω ：モータ角; ι 度 J：モータイナーシャ（ロータ +減速機 1次側）

て μ ：摩擦トルク（モータ軸端に換算）

r dyn：動力学トルク（重力トルク、慣性力、遠心力、コリオリカ、弾性力の和。モータ軸端に換算）

τ dis：外乱トルク（外部からの接触トルク、パラメータ誤差。モータ軸端に換算）

式（ 2 ) で外乱トルク τ d i s = 0とすれば、式（1)、 ( 2) よりロボットアームの動作に必要なモータ電流 I m 1を計算することができる。

I ral = (J * a + T /i + T dyn) / Kt · · · (3) 図 3に示すように、この式（3 ) で計算した I m 1をフィードバック電流指令 I c o mに加算すれば、外乱トルクて d i s = 0であれば、フィードバック電流が 0でも、目標位置に追従することが可能となる。

図 3において、フィードバック電流指令 I c o m (4) は、回転角指令 Θ c o m ( 1 ) と実際のモータ回転角 0 f bから、 P I D計算等をフィードバック制御器（2) で行い、電流制限（3) をすることで求められる。電流制限（3) の手段としては、リミツトをかける方式やフィードバックゲインを下げる方式等がある。

一方、モータ回転指令 0 c o m ( 1 ) を微分計算（1 2) ( 1 4) を 2回行うことにより求められる角加速度 α c o m ( 1 5) にモータイ^ "一シャ J ( 1 6 ) を乗じたものに、摩擦トノレク τ μ ( 2 0) と動力学トルク r d y n ( 1 9) を加算し、モータトルク定数の逆数 1ZK t ( 1 8) を乗じれば、式（3) で計算される I m l ( 1 7) を算出することができる。

つまり、式（3) で、ロボット動作に必要なモータ電流 I m 1を精度良く計算できれば、フィードバック制御における電流指令を抑えることが可能となり、ロボットの柔軟性を高めることができる。

しかし現実には、式（3) におけるパラメータ誤差に起因する計算誤差があるので、フィードバック制御における電流指令を抑えすぎるとその誤差を補償できず、無制御状態となるので、位置偏差を減らすことができず、暴走状態になる可能性もある。

また、フィードバック制御における電流指令を 0にすると、外部からの接触トルクで位置偏差が広がれば、元の位置に戻す力が発生しない。

以上説明した様に、フィードバック制御による電流指令をどこまで抑えられるかは、式（3) の計算精度に依存する。

また、式（3) で計算するロボット動作に必要なモータ電流 I m 1の主要項の一つである摩擦トルクて μ は、力の作用方向で決まる静止摩擦トルク τ μ sと動摩擦トルク τ μ ΐη、そして速度の大きさに比例する粘性摩擦トルク τ μ d (粘性係数 D) で構成される。

τ ζ = τ μ 3 + τ μηι + τ μ (1 · · · (4)

ただし、式（ 4 )においてそれぞれの項は以下のとおり計算する

τ μ s = τ μ sQ * sgnl i, ω ) .·· (5)

1 (ω=0、動作方向〉 0)

sgnl ( ω ) 0 (ω≠0)

-1 (ω=0、動作方向く 0)

τ μ m = τ m0 * sgn2 ω ) (6)

τ μ d = Ό *ω … (7)

式（5) 〜（7) から明らかなよう全ての摩擦トルクは角速度 ω Ζ 依存して計算される。

この摩擦トルクの計算で使用する速度 ω について、前出の従来例（特開平 9- 179632) ではフィードバックされる角速度 ω F Βを、また別の従来例（特開平 10-180663)では位置指令 0 c o mを微分した角速度指令 ω c o mを用いていた。しかしながら、ロボットの柔軟制御においては、位置指令 Θ c o mに従つて能動的に動作する場合と、外力に押されて受動的に動作する場合がある。

図 3に示すように、摩擦トルクて μ ( 2 0) を式（5) で計算プロック (2 1 ) によって計算する際に用いる角速度として、実際のモータ回転角 Θ f b ( 1 1 ) を微分（2 4) することにより求められる実角速度 ω f b ( 2 3 ) を用レヽると、モータに加わる外乱トルク（9) の一部である外力 τ d i sによる速度変動が反映され、摩擦トルク τ μ ( 2 0) の計算精度は上がる。

しかしながら、ロポットが完全に停止状態から能動的に動作しようとした時は、ロボットが動作するまでは動作方向が不明であるので、静止摩擦トルク τ μ sを計算することは出来ない。

また、ロボットが動作するまでは実角速度 0 ：8は0でぁり、式（6)、 ( 7) で計算される動摩擦トルク τ μπι、粘性摩擦トルク τ Ai dも当然 0であるので、式（4) で計算される摩擦トルク τ μ は 0であり、ロボットを動作させるためのモータトルクは一切発生しない。

この状況で、フィードバック制御の電流指令 I c o m (4) によるトルク

1 柔軟性を高めるために抑制され、実際の静止摩擦トルクて μ sを下回っていると、回転角指令 0 c o m ( 1 ) が発生してもロボットは動かない。

—方、図 4に示すように、摩擦トルクて μ ( 2 0) 計算（2 1 ) に用いる角速度 ω にモータ回転角指令 0 c o m ( 1 ) を微分（1 2) することにより得られる角速度指令 ω c o m ( 1 3) を用いるとこれらの問題は解消される。つまり、ロボットが動作していなくても、角速度指令 ω c o m ( 1 3) を用いて摩擦トルク τ （2 0) を式（4) で計算（2 1 ) し、フィードバック電流指令 I c o m (4) に加算することにより、実摩擦トルクを補償でき、フィードバック制御による電流指令 I c o m (4) が抑えられていても、ロボットの動作が可能となる。

ただし、摩擦トルク τ μ ( 2 0) の計算（2 1 ) に用いる角速度 ω に角速度指令 ω c o m ( 1 3) を用いると、角速度指令 ω c o m ( 1 3 ) に従って能動的に動作するが、動作途中で外乱トルク τ d i sにより角速度変動が発生した場合は角速度指令 ω c oni (1 3) と実角速度 ω f b (図 3の 2 3) の誤差が大きくなり、式（7) で計算する粘性摩擦トルクて dの計算誤差も増大する。

また、角速度指令 a c om ( 1 3) が 0で停止していた時、外力により押されて動作した場合は、式（4) で計算される摩擦トルク τ μ は常に 0であるので、実摩擦トルクのトルク補償は一切されなくなる。

さらに外力で押されない場合でも、実際の動作ではフィードバック制御器 (2) による制御において追従遅れが発生するので、動作停止時には、実角速度 ω f b (図 3の 2 3) が 0になる前に、角速度指令 ω c om (1 3) が 0になるので、その時点から式（4) で計算される摩擦トルクても 0になり、摩擦補償がされなくなる。つまり、角速度指令 ω c om (1 3) が 0に到達した時点で急激に停止し、目標点への未到達や振動発生等が発生する可能性がある。

ここで、フィードバック制御の電流指令 I c om (4) によるモータの発生トルクが実摩擦トルク以下に抑えられている場合、外力によって位置偏差が大きくなつても口ポットは動作せず、位置偏差を小さくすることができない。

言い換えれば、摩擦補償をしているにも関わらず、フィードバック電流指令を実摩擦トルク以下には設定できないことになり、柔軟性を高めることができなくなる。

以上、説明した様に、式（4) で摩擦トルクて μ を計算する場合、角速度信号として実角速度 ω f bか角速度指令 ω c o mのいずれか一方を用いる方法では、計算した摩擦トルク τ μ を用いて式（3) で計算した電流 I m lを、フイードバック制御電流に加算しても、実摩擦トルクを補償できない場合が発生する。

また、図 3に示すように摩擦トルクて μ (2 0) を式（5) で計算（2 1)する際に用いる角速度に、実角速度 ω ί b (2 3)を用いて、摩擦トルクて μ を 1 0 0 %補償すると、制御系のフィードバック特性が摩擦補償されることになり、あたかも摩擦が無い状態として振る舞うので、柔軟性は高まるものの、フィ一ドバック特性は振動的になる。

一方、図 4に示すように、摩擦トルクて μ ( 2 0 ) 計算（2 1 ) に用いる角速度 ω に角速度指令 ω c o m ( 1 3 ) を用いた場合は、制御系のフィードバック特性には影響を与えないので、目標追従特性改善のためには 1 0 0 %補償することが望ましい。

つぎに、第 2の従来例について説明する。

衝突トルクをセンサレスで求める方法としては、モータの駆動電流で発生したトルクからモータ及び減速機で損失するトルクを差し引いたモータ発生トルクより、ロボットの逆動力学演算で求めた減速機出力で必要なトルク（以下、動力学トルクと略する）を差し引いて、衝突トルクを求める方法が一般的であった。

例えば、モータ発生トルクにおける損失分にあたる摩擦トルクを、速度比例項（粘性摩擦トルク）と固定項（クーロン摩擦トルク）の和として定義して演算していた（例えば特開 2002- 283276)。

また、特開平 6- 083403 (米国特許 6298283号）では、推定アルゴリズムにより口ポットのパラメータ変動を計算し、トルク（電流）指令に加算することにより、変動要因をキャンセルすることが提案されていた。この従来例では、モータ発生トルクにおける損失分にあたる摩擦トルクを、速度比例項と固定項の和として定義し、推定アルゴリズムで推定していた。

したがって、モータ発生トルクからロボットの動力学トルクを差し引いて、センサレスで衝突トルクを求める場合や、モータ駆動力最大限に発揮させるために、動力学トルクのフィードフォヮ一ド制御によるサーボ追従特性を改善する場合においては、モータ発生トルク及びロボットの減速機出力で必要なトルクを正確に計算することが求められていた。

このロボット動作時におけるモータ発生トルク τ πιは、モータ駆動側から見れば式（8)、負荷側から見れば式（9)で表される。

τ mm = Kt * Im - (J * o; + D * co + T: i sgn(ω)) · · · (8) τ ml = τ dyn + て dis · · · (9) ただし、式（1)、（2)における記号は以下のとおりである。

Kt ：モータトルク定数

Im：モータ電流

モータ角加速度

ω ：モータ角速度

J：モータイナーシャ（ロータ +減速機 1次側）

D ：粘性摩擦係数（モータ軸端に換算）

て μ ：摩擦トルク（モータ軸端に換算）

て g ：重力トルク (モータ軸端に換算）

τ dyn 動力学トルク（重力トルク、慣性力、遠心力、コリオリカ、弾性力の和。モータ軸端に換算）

r dis：外乱トルク（衝突トルク、パラメータ誤差。モータ軸端に

1 (ω〉0)

0 (ω≠0)

-1 (ω〈0)

モータとロボットアームは減速機を介して接続しているので、式 (9) においてモータイナーシャ J以外の項は、減速比を用いてモータ軸端に換算する必要がある。

衝突トルク τ d i sは、（8)、（⁹)において τ mm =で m 1と仮定する事により、以下の式 (10) に変形して求めることが出来る。

T d i s = Kt * Im — (J * a + D * co + T Ai * sgn (ω ) + τ dyn) · .. (10) 従来例では、式（10) における動摩擦項 τ μ を固定値として計算していた。しかし、動摩擦トルクを固定値として演算すると、加減速時等のモータ発生トルクが大きい場合に、モータ発生トルクの 1 0 %程度の大きな計算誤差を生じていた。

一方、サーボ制御特性の改善を目的とするフィードフォワード制御は、外乱トノレク τ d i s = 0の条件、つまり、外部には接触せず、パラメータ誤差は無い条件で式（10) よりモータ電流 I mについて解いた値を I f f とし、 I f f を電流指令に加算することにより実現できる。

Iff = (J * a + D * ω + τ μ * sgn ( o> ) + τ dyn) / Kt · · · (11) これまで従来例では、式（11) の演算ではなく、動摩擦項て I を推定ァルゴリズムで推定していた。し力し、経時変化ではなく、加減速の短期間に大きく摩擦トルクが変動するので、推定アルゴリズムで推定していたのでは、位相遅れが生じ、完全に補償しきれない可能性があった。

推定アルゴリズムではなく、動摩擦トルクで予め演算すると位相遅れは発生しないが、動摩擦トルクを固定値として式（11)を演算すると、加減速時等のモータ発生トルクが大きい場合に、モータ発生トルクの 1 0 %程度の大きな計算誤差を生じていた。

以下この誤差について説明する。

図 11は、ロボットに図 12に示す往復動作をさせたときの、式（8)、 (9) で演算されるモータ発生トルクて mm ( 1 )、 τ m 1 ( 2 ) と速度（3 ) を示したものである。

使用するロボットアームは、可搬質量が 6 k g、総アーム長が約 1 . 3 m の 6軸垂直多関節ロボットであり、図 12は手首 3軸を省略し基本 3軸を示したもので、計測には第 3軸の F A軸を動作させている。

ここでは、外乱トルクて d i s = 0の条件（外部接触は無く、パラメ一タ誤差もない条件）で測定している。

図 11より、 τ mm ( 1 ) と τ πι ΐ ( 2 ) ではピークトルクの 1 0 %程度の誤差（4 ) が発生しており、式（8)の中に誤差要因か有ることがわかる。

加速、減速時とも比較すると以下のようになる。

τ mm > τ ml

動作区間の加速と減速においては、角加速度は逆方向ではあるが大きさは等しく、角速度は向き、大きさとも等しい。また、重力に対しても対称なパターンで動作させている。

従って、て d y nを小さくして、誤差を減少させるためには、動摩擦トルクて μ を大きくするしかないが、一定値のまま大きくすると、図 13に示すように、ピークトルク時の誤差（4) は減少するものの、一定速度時の誤差（5) が大きくなる。

つまり、動摩擦トルク τ u を一定値として考えると、 τ μ に起因する誤差要因を無くすことが出来ず、て μ を含む（10)、式（11) においても、同様の誤差が発生することになる。

このため、センサレス衝突トルク検出においては、動摩擦トルクて ί を一定値として考えると、加減速時に衝突していないにも関わらず、式（11) が誤差分を衝突トルクとして出力するため、誤検出を防ぐためには、衝突検出感度を鈍らせる他なかった。

一方、減速機出力で必要なトルクのフィードフォワード制御においても、動摩擦トルク τ μ を一定値として考えると演算誤差が大きくなり、フィードフォワード補償トルクが不十分になる可能性がある。また、演算誤差を防ぐために推定アルゴリズムを用いると、加減速時に急変する摩擦トルクを位相遅れ無しに推定するのは困難なため、制御性能の劣化防止も不十分になる可能性があった。

次に、衝突検出後に停止する方法について説明する。衝突検出位置へ戻す方法（特開 20 0 1— 1 1 7 6 1 8 (米国特許 6429617)) や、速度指令を強制的に 0に設定し停止させる方法（特開 20 0 0— 5 2 2 8 6)、さらには、モータ回転方向に対しモータ最大の逆トルクで停止させる方法（特許第 3 2 1 2 5 7 1号 (米国特許 6298283))、（特許第 2 8 7 1 9 9 3号（米国特許 5418440) ) が提案されている。

衝突検出位置へ戻す方法では位置制御で元の位置へ戻すため、停止時間は位置制御の応答性に依存する。一般的に位置制御の応答性は高くても数十 Η ζであるので、衝突停止としては応答性が悪く、停止時間が長引き、衝突による損傷をくい止められない可能性がある。速度指令を強制的に 0に設定し停止させる方法では、停止時間は速度制御の応答性に依存する。応答性は数百 H zで位置制御よりは良いものの、電流制御の応答性（数 k H z ) には及ばない。

また、この方法では、速度制御の積分動作による位置制御サーポ剛性が高いため、衝突による歪みが残った状態で停止するので、特開 2000- 052286では速度積分ゲインを 0にすることで、位置制御剛性を柔らかくし、衝突による歪みを解消することが提案されている。しかしながら柔軟性を高めるためには、速度比例ゲインを小さくする必要があるが、速度応答性は悪くなり停止時間が延びてしまう。停止時間と柔軟性を両立させることは困難である。

モータ回転方向に対しモータ最大の逆トルクで停止させる方法では、逆トルクを発生させる電流制御の応答性は数 k H zと高く、応答性では優れているものの、特許 3212571の方法では、逆トルクを印加する時間を予め設定する必要がある。この印加時間が短いと十分に減速できないので衝突損傷が大きくなり、印加時間が長いと逆方向へ余分な動作を行い、再衝突を引き起こす可能性がある。特許 2871993では、モータが停止するまで最大の逆トルクを印加する方法が提案されており、逆トルクの印加時間を予め決めておく必要はないので、前記の課題は解決されている。しかしながら、モータを停止させるだけでは衝突による歪みは解消されない。また、最大逆トルクを発生させること自体は、制御がオープンループで最大出力を発生している状態であるので、衝突してもロボットが損傷しない程度の速度である場合には、逆トルクを印加することのリスクが大きくなる。

また、いずれの方式においても衝突方向とモータ回転方向が一致している軸においては、衝突検出位置へ戻したり、急停止させると、かえって衝突力を増大させることになる。

この状態を 2軸のロポットを用いて説明したものが図 15である。通常の垂直多関節ロボットは 6軸で構成されていることが一般的であるが、説明を簡単にするために、 2軸モデルで以下説明する。

図 15 ( a ) において、 U A軸（4 1 ) は角速度 ω ί b ( 1 ) の方向、 F A軸（42)は角速度 ω f b ' (6)の方向へ動作しているとする。時間が経過し、図 I⁵ (b) の方向へ各々のアームは動作し、障害物（43) に衝突すると、衝突力（44) が発生し、 UA軸（4 1) にはモータ回転方向とは逆方向の力、すなわち減速させる方向に衝突トルク τ d i s (9) が加わる。一方 FA軸（42) には、モータ回転方向とは同方向の力、すなわち加速させる方向に衝突トルク τ d i s ' (10) が加わる。

この後、 FA軸（42) を衝突検出位置へ戻したり、急停止させるためには、モータ回転を減速させるトルクをモータで発生させる必要があるが、衝突トノレク τ d i s ' (10)に逆らった方向のトルクとなるため、かえって衝突トルクを増大させることになる。

衝突検出位置へ戻す方法では、衝突方向とモータ回転方向が逆の軸（図 15では UA軸）は衝突検出位置へ戻すが、衝突方向とモータ回転方向が同じ軸（図 15では F A軸）は反転せず衝突前までの動作を継続することで、上記課題を解決する手段を提案している。

しかしながら、衝突検出をセンサレスで実現する場合、口ポットの機械パラメータや位置、速度、加速度、電流等の情報から衝突トルクを推定するため、衝突検出センサが有る場合に比べ、検出誤差は大きくなる。このため、衝突トルク検出値が小さい軸では、方向検出を誤る可能性があり、適切な停止手段を選択しない可能性がある。

衝突トルク検出値が小さい軸は、衝突方向の検出を行わないで、モータ回転速度を減速させ、運動エネルギーを減少させた方が安全ではある。しかし、衝突方向が不明である以上、実際には減速させない方が良い場合もあり、特許 2871993の様に、モータが停止するまで逆トルクを印加するべきではない。また、衝突してもロボットが損傷しない程度の速度である場合も逆トルクを印加するベきではない。

さらに、垂直多関節ロボットにおいては、軸間の干渉力が無視できないので、逆トルクを印加した軸からの干渉力で、減速すべきでない軸に減速する力が加わる可能性がある。いずれにせよ逆トルクは、必要な軸に最小限の時間で印加すべきである。く発明の開示 >

本発明は、上記の様な問題点を解決するためになされたものであり、モータにより駆動されるロボットの柔軟制御において、フィードバック制御による電流制限を摩擦トルク以下に抑え、より柔軟性を高くできる制御方法の提供を目的とする。

また、本発明は、センサレス衝突検出において、減速機の動摩擦トルクを固定値ではなく、実際の特性に合致させることにより、高精度な衝突トルク検出を可能とするロボットの制御方法の提供を目的とする。

さらに、ロボットの駆動力における減速機の動摩擦トルクを固定値ではなく、実際の特性に合致させることにより、動作前のモータトルク演算精度を向上させ、位相遅れの少ない最適なフィードフォヮ一ド補償を得ることを可能とするロボットの制御方法の提供を目的とする。

本発明によれば、ロボットを駆動するモータの回転角を検出し、前記回転角から角速度の実測値を算出し、前記モータに与える指令値から算出した指令値の角速度と前記実測値の角速度の絶対値の大きな方を用いて摩擦トルクを算出し、前記指令値に基づいてモータを駆動する際にモータへの指令値に前記摩擦トルクに対応する値を加えるロボットの制御方法である。この方法により、フィードバック制御による電流制限を摩擦トルク以下に抑えることができ、より柔軟性の高い制御方法が実現できる。

また、摩擦トルク計算に用いる角速度として、指令値と実測値のいずれかを適宜選択し、摩擦補償率も合わせて変化させることにより、フィードバック特性が振動的になることの防止と、目標追従特性の改善の両立が実現できる。

また、本発明では、減速機を介してモータにより駆動されるロボットの制御方法において、前記モータにより発生するトルクからロボットの逆動力学演算で求めた減速機出力で必要なトルクを差し引いて外力を算出する際に、減速機の動摩擦トルクを減速機出力で必要なトルクに応じて増大させて算出するロボットの制御方法である。

また、減速機を介してモータにより駆動される口ポットの制御方法において、前記減速機出力で必要なトルクを求める口ポットの逆動力学演算と減速機の動摩擦トルク演算に基づいてフィードフォヮ一ドでモータ出力トルク補償を行うとともに、フィードパック制御を行う際に、前記減速機の動摩擦トルクを減速機出力で必要なトルクに比例して増大させて演算するロボットの制御方法である。

また、本発明は、衝突検出後、モータ回転方向と衝突トルク方向が逆の軸では、実位置を位置指令に追従させるための電流指令を生成する位置制御から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御に切り換えることによりモータ回転速度を減速し、衝突エネルギーを緩和する。その後、モータ速度が設定値以下になれば、衝突力方向に倣う柔軟制御に切り換え、衝突で生じた減速機等での歪みを解消する。応答性が一番高い電流制御で停止減速させることができ、さらにモータ速度を監視することにより、モータ回転と逆方向のモータトルク印加時間が決まるので、印加時間を予め設定する必要も無レ、。

—方、モータ回転方向と衝突トルク方向が同じ軸では、電流制御を介さずに、位置制御から直接、柔軟制御に切り換える。衝突力に倣って動作させることにより、衝突トルクを緩和することができる。

衝突時のモータ回転速度が設定値以下の軸では、モータ回転方向と衝突トルク方向に関わらず、電流制御を介さずに、位置制御から直接、柔軟制御に切り換わることになり、電流制御のみのオープンループ状態を不必要に発生させないことになる。 <図面の簡単な説明 >

図 1は、第 1、第 2の実施例における摩擦補償の制御方法を示すブロック線図。図 2は、第 3の実施例における摩擦補償の制御方法を示すプロック線図。図 3は、従来例における実速度を用いた摩擦補償の制御方法を示すプロック線図。図 4は、従来例における速度指令を用いた摩擦補償の制御方法を示すプロック線図。

図 5は、本発明の実施の形態 1における衝突トルク検出方法のプロック線図。

図 6は、ハーモニック減速機における減速機出力で必要なトルク動摩擦トルク特性の一例を示す図。

図 7は、 R V減速機における減速機出力で必要なトルク動摩擦トルク特性の一例を示す図。

図 8は、動摩擦トルク近似式におけるパラメータを示す図。

図 9は、本発明おける動摩擦トルク計算方法による減速機出力で必要なトルク誤差を示す図。

図 1 0は、本発明の実施の形態 2における減速機出力で必要なトルクフィードフォヮード制御のプロック線図。

図 1 1は、従来例における動摩擦トルク計算方法による減速機出力で必要なトルク誤差を示す図。

図 1 2は、減速機出力で必要なトルク測定時の動作を示す図。

図 1 3は、動摩擦トルクを大きくした場合の減速機出力で必要なトルク誤差を示す図。

図 1 4は、第 1の実施例における衝突停止制御方法を示すタイムチヤ一ト。図 1 5は、衝突時の速度方向と衝突トルク方向を示すロボット動作図。図 1 6は、第 1の実施例における衝突停止制御装置（位置制御モード）を示すブロック線図。

図 1 7は、第 1の実施例における衝突停止制御装置（電流制御モード）を示すプロック線図。

図 1 8は、第 1の実施例における衝突停止制御装置（柔軟制御モード）を示すプロック線図。

図 1 9は、第 2の実施例における衝突停止制御装置（位置制御モード）を示すブロック線図。

図 2 0は、第 2の実施例における衝突停止制御装置（電流制御モード）を示すブロック線図。

図 2 1は、第 4の実施例における衝突停止制御装置（電流制御モード）を示すプロック線図。

図 2 2は、第 3の実施例における衝突停止制御装置（位置制御モード）を示すプロック線図。

図 2 3は、第 3の実施例における衝突停止制御装置（電流制御モード）を示すブロック線図。

図 2 4は、第 3の実施例における衝突停止制御装置（柔軟制御モード）を示すブロック線図。

図 2 5は、第 3の実施例における衝突停止制御方法を示すタイムチヤ一ト。

<発明を実施するための最良の形態 >

以下、本発明のロボットの制御方法を示す好ましい実施例について、図面を参照しながら説明する。

(第 1の実施例）

図 1は、本発明の制御方法を示すブロック線図である。図 1において、 2 6は速度切換器、 2 7は速度切換器により選択された角速度 ω である。フィードバック電流指令 I c o m ( 4 ) は、回転角指令 Θ c o m ( 1 ) と実際のモータ回転角 6 f から、 P I D計算等をフィードバック制御器（2 ) で行い、電流制限 ( 3 ) をすることで求められる。電流制限（3 ) の手段としては、リミットをかける方式やフィードバックゲインを下げる方式等がある。一方、モータ回転指令 Θ c o m ( 1 ) を微分計算（1 2) ( 1 4) を 2回行うことにより求められる角加速度 a c o m ( 1 5) にモータイナーシャ J ( 1 6) を乗じたものに、摩擦トルクて μ (2 0) と動力学トルク τ d y η ( 1 9) を加算し、モータトルク定数の逆数 1 /K t ( 1 8) を乗じれば、式（3) で計算される I m l ( 1 7) を算出することができる。

この摩擦トルクて μ ( 2 0) を（数 3) の式（5) 〜（9) で計算（2 1 ) する際に用いる角速度 ω ( 2 7) に、モータ回転角指令 Θ c o m ( 1 ) を微分（1 2) した角速度指令 ω c o m ( 1 3 ) と、実モータ回転角 0 f b ( 1 1 ) を微分（2 4) した実角速度 ω f b ( 2 3 ) のいずれかを選択するのが、速度切換器（2 6) である。

速度切換器（2 6) において、速度切換は以下の式で行う。

ω = I ω f b ( I ω com I ≤ | ω f b | ) ··· (12)

ω com ( I ω com | > | ω f b | )

式（12) では、角速度指令 ω c o m ( 1 3) と実角速度 ω f b ( 2 3) の絶対値の大小を比較して、大きい方を ω ( 2 7) として選択する。

そして、この ω ( 2 7) を用いて摩擦トルク τ μ ( 2 0) を式（5 ) 〜 ( 7) で計算する。

このように選択することで、柔軟制御が有効になっていても、位置指令 Θ c o m ( 1 ) が入力されロボットアームが能動的に動作する場合と、外力 τ d i sに押されて受動的に動作する場合のいずれの場合でも、摩擦トルク ( 2 0) は、 0になることはなく適切な大きさで計算される。

また、ロボットが能動的に動作している間に、外力 τ d i sに押されて位置偏差が大きくなり、角速度指令 ω c o m ( 1 3 ) と実角速度 ω f b ( 2 3) 間に差違が出ても、実角速度 ω f b ( 2 3 ) の絶対値が角速度指令 ω c o m ( 1 3 ) の絶対値より大きくなれば、 ω (2 7) としては、 co f b ( 2 3) が採用されるので、摩擦トルク τ μ ( 2 0) の計算における誤差要因を減らすことが出来る。さらに、フィードバック制御器（2) での制御遅れにより、動作停止時には、実角速度 ω f b (2 3) が 0になる前に、角速度指令 co c om (1 3) が 0 になっても、実角速度 ω f b (2 3) の絶対値が角速度指令 ω c om (1 3) の絶対値より大きくなれば、 ω (2 7) としては、 co f b (2 3) が採用されるので、その時点から式（4) で計算される摩擦トルク τ による摩擦補償も継続される。つまり、角速度指令 ω c om (1 3) が 0に到達した時点で、摩擦補償が無くなることによる急激な停止を防止でき、目標位置への未到達や振動発生等の問題が発生する可能性を低く出来る。

以上の様に構成することにより、より柔軟性を高くするために、フィードバック制御による電流 I c om (4) の制限を実摩擦トルク以下に抑えて設定しても、目標位置への未到達や停止振動の増大を防ぐことが出来る。

(第 2の実施例）

第 1の実施例における速度切換を示すの式（12) において、速度指令値と実測値の少なくとも一方に重み係数を乗算する。

ω = ωΆ ( | kcl * _ω com + kc2 | ≤ | ω f b | ) … （13) ω com ( I kcl * ω com + kc2 | > | ω f b | )

式（13) の様に構成することにより、速度指令値と実測値のいずれかを優先的に速度として採用することが出来る。

速度実測値 ω f bには測定誤差が含まれるので、式（13) における重み係数を例えば以下のように設定することで、速度指令 ω c omを優先的に選択することができる。

kcl > 1かつ kc2 > 0 ··· (14)

(第 3の実施例）

図 2は第 3の実施例の制御方法を示すブロック線図である。

速度切換機（2 6) に入力される実角速度 ω f b (2 3)

k μ を乗じる。

式で表すと、式（15) の様になる。 ω k μ * ω f b ( | kcl * ω com + kc2 | ≤ | ω f b | ) … (15) ω com ( I kcl * ω com + kc2 | > | ω f b | )

ただし、 k/i ：摩擦補償率

の式（15) で得られた角速度 c ( 2 7) を用いることにより、摩擦トルク τ μ ( 2 0) を（数 3) の式（5 ) で計算（2 1 ) する際に用いる角速度に、実角速度 co F B ( 2 3) を選択した場合、摩擦補償率 k /i を 1以下にすると、摩擦トルクて μ が 1 0 0 %補償されなくなり、フィードバック特性が振動的になることを調整できる。

一方、摩擦トルク τ μ ( 2 0 ) 計算（2 1 ) に用いる角速度 ω に角速度指令 ω c o m ( 1 3) を用いた場合は、制御系のフィードパック特性には影響を与えず、摩擦トルク τ μ が 1 0 0 %補償されので、目標追従特性が改善される。

(第 4の実施例）

ロボットに用いられる代表的な減速機における動摩擦トルクを外乱トルク τ d i s = 0の条件で、動力学トルク _τ d y ηの変動に対し測定したものが、図 6、図 7である。図 6はハーモニック減速機、図 7は偏心差動式減速機の一種である R V減速機における特性である。

図 6、図 7とも、動力学トルク _τ d y nの増大に応じて、動摩擦トルクが増大していることがわかり、式 (16) で近似することが出来る。

「 A * τ dyn + B ( τ dyn ≥ τ th) τ a = < C * て dyn² + D ( | τ dyn | < τ th) · . · (16) し E * τ dyn + F ( τ dyn ≤ - τ th) 式（16) において、 A、 B、 C、 D、 E、 Fは近似定数、 τ t hは設定閥値である。

図 6、図 7における上記パラメータは図 8のようになる。

式（16) に従って動摩擦トルク近似値 τ μ aを計算し、図 11と同じ減速機出力で必要なトルクの計算比較をしたものが図 9であり、図 11に比べ一定速時の誤差（5) が増えずに、ピークトルク時の誤差（3) が減少していることがわかる。

そこで、式（16) で計算した τ μ &を用い、式（10) を変形すると以下の様になる。

τ disa = Kt * Im

一 u* a + D*co+ τ μ a * sgn ( ω ) + τ dyn) · · · (17) この式 ⁷) で衝突トルクて d i _S aを計算することで、式（10) に比ベ、特に衝突前の誤差を小さくすることができ、衝突検出感度を鈍くする必要がなくなり、衝突検出精度を高めることが出来る。

この方法のブロック線図を図 5に示す。

図 5において、モータ回転角指令値 0 r e f ( 1 1 ) とモータ +実負荷を示す点線内（1 2) より得られるモータ回転角 Θ Μ ( 1 3) とを比較して、制御器（1 4) は電流 I m ( 1 5) をモータに供給する。モータでは電流 I m ( 1 5) にトルク定数 K t ( 1 6) を乗じたトルクが発生し、このトルクから動力学卜ノレクて y n、 ft] "突卜ノレク τ d i s ¾.び摩:)察卜ノレク τ μ X s g η ( ω ) の和 ( 1 7) を差し引いたトルクが、ブロック（1 8 ) で表されるモータ単体の駆動に用いられる。このモータの回転角 0 Μ ( 1 3 ) と電流 I m ( 1 5) より式（17) で、衝突トルク検出値て d i s a ( 2 0)を計算するブロックが衝突トルク計算部（1 9) である。

尚、誤差は増えるものの式（17) で、動力学トルク τ d y nの代わりにモータ電流トルク τ m 0 =K 1: X I mを用いても良い。

(第 5の実施例）

次に本発明の第 5の実施の形態例について説明する。

まず、動摩擦トルクとしては式（16) を用い、式（11) を変形すると、以下の式（18) となる。

Iff = (J* + ϋ*ω + τ ia * sgn(co) + τ dyn) / Kt · ·· (18) この（18) 式において角速度 ω と角加速度 α をモータ回転角指令 Θ r e ίから微分して計算すると、式（19) の様に変形でき、モータに必要なトルクを発生させる電流 I ί f を、フィードバック信号を用いずに計算することができる。

Iff = (J*s² ( Θ ref) + D*s ( Θ ref) + T /ia * sgn(_W ) + T dyn) / Kt · · · (19) 図 1 0は、このフィードフォヮ一ド電流 I f ίで、フィードフォヮード補償を行った場合の実施の形態例を示すプロック線図である。

図 1 0において、モータ回転角指令値 S r e f ( 1 1) とモータ +実負荷を示す点線内（1 2) より得られるモータ回転角 ( 1 3) とを比較して、フィードバック制御（2 1) はモータに流す電流指令 I c om (2 2) を出力する。このフィードバック制御で得られた電流指令 I c om (2 2) に、（数 8 ) の式（9) を示すブロック（2 3) で得られるフィードフォワード電流指令 I f f (24) を加算することにより、誤差と推定遅れの少ないフィードフォワード制御が実現できる。

(第 7の実施例）

図 1 6は本発明の第 7の実施例を示す図である。

図 1 6において、（2 6) は口ポットのアームに加わる衝突力がアームを駆動するモータに加える衝突トルク τ d i s d (2 7) を検出する衝突トルク検出手段、（2 5) は衝突トルク検出値て d i s d (2 7) の大きさと設定した衝突トルク閾値を比較して衝突を判定し、衝突検出信号 D c o 1 (3 0) を出力する衝突判定手段、（24) はモータ回転角 Θ f b (2 2) よりモータの角速度 ω f b (1) を検出するモータ回転検出手段、（2 3)は衝突トルク検出方向とモータ回転方向を比較し衝突方向フラグ D i r (3 1)を出力するする衝突方向判別手段、

(3 2) はモータ角速度 ω f b ( 1) の大きさと設定した閾値を比較してモータの減速を確認しモータ減速判定信号 D t h (3 3) を出力するモータ減速判定手段、（1 5) は、モータ回転方向と衝突トルク方向が逆の軸では、モータ回転角 e f bを回転角指令 0 c om (1 1) に追従させるための電流指令を生成する位置制御手段（1 2) から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御手段（1 3) に切り換えることによりモータ回転速度を減速し、速度が設定値以下になれば、衝突力方向に倣う柔軟制御手段（1 4) に切り換え、モータ回転方向と衝突トルク方向が同じ軸では、位置制御手段（1 2) から柔軟制御手段（1 4) に切り換える制御モード切換手段である。

以下、図 1 6を用いて衝突検出後の停止制御方法について詳細に説明する。位置制御するための電流指令 I c o m l ( 2)は、モータ回転角指令 Θ c om ( 1 1 ) と実際のモータ回転角 0 f b ( 2 2) から、位置制御手段であるフィードバック制御器（1 2) で求められる。フィードバック制御器（1 2) は通常、 P I D制御等で構成される。

衝突検出前の通常の位置制御では、制御モード切換ブロック（1 5) でモータ電流 I m ( 1 6 ) として、この電流指令 I c o m 1 ( 2) を選択し、モータ +実負荷（1 7) に印加する。

モータイナーシャ Jを使って記述されるモータ伝達関数（2 1 ) には、モータ電流 I m ( 1 6 ) をトルク定数 K t ( 1 8) で乗じたモータトルク τ ιηιη ( 1 9) と外舌 Lトノレク（2 0) が印加される。

外乱トルク（2 0) は摩擦トルク τ μ、重力トルク τ g、動力学トルク τ d y η (慣性力、遠心力、コリオリカの和）、衝突トルク τ d i sの和である。

モータ伝達関数（2 1 ) からモータ回転角 0 f b ( 2 2) が出力され、通常は光学式あるいは磁気式エンコーダで検出する。

衝突トルク検出ブロック（2 6) では、このモータ回転角（2 2) と他軸のモータ回転角（ 2 9 )及びそれらを時間微分することにより導出できる角速度、角加速度、さらにはロボット機械パラメータの情報を用い、衝突トルクて d i s が発生していない条件で、モータに必要なトルクを逆動力学演算で求め、実際のモータ電流 I mm ( 1 6) にトルク定数 K t ( 1 8) を乗じたものを減算することで、衝突トルク検出値 τ d i s d (2 7) を求める。

衝突判定ブロック（2 5) では、この衝突トルク検出値 τ d i s d ( 2 7) と、他軸でも同様に求めた他軸衝突トルク検出値（2 8 ) の内、 1軸でも所定の衝突検出閾値て c t hを越えた場合は、衝突が発生したと判定し、衝突検出信号

D c o 1 (3 0) を制御モード切換プロック（1 5) へ送る。

モータ角速度検出プロック（24) では、モータ回転角 0 f b (2 2) に微分等を行いモモータ角速度 ω f b ( 1 )を求める。このモータ角速度 ω f b ( 1 ) と衝突トルク検出値 τ d i s d (2 7) から、衝突方向判別ブロック（2 3) は、以下の式で衝突方向フラグ D i r (3 1) を演算する。

Dir = I 1 (cofb * τ disd < 0) ..· (20)

0 (wfb * て disd ≥ 0)

式（20) では、モータ角速度 ω f b (1) と衝突トルク検出値 τ d i s d (2 7) の方向が反対の時、衝突方向フラグ D i r (3 1) は 1となり、それ以外は 0となる。

図 1 5の動作においては、 UA軸（4 1) は D i r = 1、 FA軸（4 2) は D i r ' =0である。

制御モード切換プロック（1 5) は、衝突検出信号 D c o 1 (3 0) が入力された時点で、衝突方向フラグ D i r ( 3 1) の情報を元に制御モードを切り換える。

I m= 「 I c om2 (D i r = 1 ) 電流制御モード ··· (21)

I c om 3 (D i r = 0) 柔軟制御モード UA軸は、 D i r = 1であるので、モータ角速度 ω f b ( 1 ) から、電流制御ブロック（1 3) でモータ回転方向と逆方向のトルクを発生させる電流 I c om2 (3) を生成し、図 1 7に示すように、制御モード切換ブロック（1 5) は I c om2 (3) をモータ電流 I m (1 6) として選択する。（電流制御モードへ移行）

この様に構成することにより、 UA軸（4 1) は急速に減速するので、衝突検出後の衝突トルク τ d i s (9) を小さくすることが出来る。

UA軸（4 1) が減速し、その角速度 ω f b (1) の絶対値が所定の減速判定閾値 co t h (5) より小さくなると、モータ減速判定ブロック（3 2) で、モータ減速判定信号 D t h (3 3) を出力する。

D t h= 1 ( I ω f b I < ω t h) ··· (22) このモータ減速判定信号 D t h (3 3) が出力されると、制御モード切換ブロック（1 5) は I c om3 (4) をモータ電流 I m (6) として選択し、図 1 8で示す柔軟制御モードへ移行する。

尚、衝突検出時において、角速度 ω f b ( 1) の絶対値が所定の閾値 ω t h (5) より小さく式（22) の条件が満たされている場合には、通常制御モード (図 1 6) から電流制御モード（図 1 7) へは移行せず、モータに逆トルクを印加して減速させることは無しに、柔軟制御モード（図 1 8) へと移行する。

ここで、柔軟制御ブロック ( 1 4) はフィードバック制御器 ( 1 2) から出力される電流指令 I c om 1 (2) に対し、電流を制限した上で、ロボットの自重落下防止のため重力補償電流を付加することにより柔軟制御を実現する。

このことにより、モータ回転角指令 0 c om ( 1 1) とモータ回転角 Θ f b (2 2) 間の偏差が大きくなつてもモータ電流が制限されることにより、位置制御のサーポ剛性が弱くなり柔軟性が生まれる。

電流の制限に関しては、フィードバック制御器（1 2) 内のゲインを下げることでも実現できる。

式（22) でモータ減速判定信号 D t h= 1になった時には、モータ角速度 ω f b (1) が閾値 ω t h rより小さくなつているので、ほぼ停止状態であり慣性エネルギーも小さくなっており、柔軟制御モードへ移行することで衝突により生じた減速機等での歪みを解消することが出来る。

一方 FA軸（4 2) では、（数 2) の式（2) の衝突方向フラグ D i r ' = 0であり、もし UA軸（4 1 ) と同じ様に急減速させると、かえって衝突トルク r d i s ' (1 0) を増大させてしまう。

そこで、衝突発生時に衝突方向フラグ D i r ' = 0の場合には、制御モード切換ブロック（1 5) で、電流制御モード（図 1 7) を介さずに通常制御モード（図 1 6) から、柔軟制御モード（図 1 8) へ切り換える。以上の様に構成することにより、 FA軸（4 2) は柔軟制御で衝突力に倣つて動作することになり、衝突トルクを緩和することができる。

図 1 4は、以上の制御方法を時系列的に表すタイムチヤ一トである。 (第 8の実施例）

図 1 9は本発明の第 8の実施例を示す図である。

図 1 9は、第 7の実施例を示す図 1 6に対し、新たに衝突トルク検出値 τ d i s d (2 7) の大きさと衝突方向判別可能トルク閾値を比較する衝突トルク閾値判定手段（34) を設け、衝突トルク検出値 τ d i s d (2 7) の大きさが衝突方向判別可能トルク閾値を下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御手段（1 2) から電流制御手段（1 3) に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度 ω f b (1) を減速し、速度が設定値以下になれば、柔軟制御手段（1 4) に切り換える制御モード切換手段（1 5) を有することを特徴とする。

以下、図 1 9を用いて追加機能である衝突トルク閾値判定ブロック（3 4) の動作、機能について説明する。

第 7の実施例において、電流制御モード（図 1 7) への移行は、式（20) で定める衝突方向フラグ D i r (3 1) のみで決めていた。

し力しながら、式（20) の条件判断で用いている衝突トルク検出値て d i s d (2 7) は、第 7の実施例で示した様にトルクセンサレスで推定した場合、ロボットの機械パラメータやモータ位置、角速度、角加速度、電流等の情報から衝突トルクて d i sを推定するため、衝突検出センサが有る場合に比べ、検出誤差は大きくなる。

そのため、衝突トルク検出値 τ d i s d (2 7) が小さく、 0に近い軸では、検出誤差により衝突トルク検出値て d i s d (2 7) の符号に誤りが発生する可能性がある。

つまり、モータ速度 ω f b ( 1) が大きい、つまり慣性エネルギーが大きい軸でもて d i s d (2 7) が小さい場合には、衝突方向フラグ D i r (3 1) を誤って出力する可能性がある。

衝突判定ブロック（2 5) では、衝突トルク検出値 τ d i s d (2 7) 力 S 所定の衝突判定閾値 τ d t hより大きくなつた軸が有れば、衝突と判断するので、衝突判定閾値より大きくなった軸では前記の衝突方向フラグ D i r (3 1) の誤出力は無い。

衝突検出した以外の軸で、衝突トルク τ d i sが小さい軸では、その軸には大きな外力が加わっていないので、速やかに減速し、慣性エネルギーを減少させる方が安全である。しかし衝突方向が判定出来ない以上、実際には減速させない方が良い場合もあるので、衝突検出時の速度の絶対値が小さく、ロボットに損傷を与えないレベルである場合は、モータに逆トルクを印加して減速させない。また、速度が大きい場合でも、完全に停止するまで減速させるのではなく、ロボットが損傷しないレベルまで減速させることを考える。

そこで、図 1 9で示す様に、新たに追加した衝突トルク閾値判定プロック (34) は、衝突トルク閾値判定信号 D t h t (3 5) を制御モード切換プロック（ 1 5) へ出力する。

D t h t = ί 1 ( I τ d i s d I < τ t h r ) ··· (23)

0 ( I τ d i s d I ≥ τ t h r )

ただし、 τ t h r ：衝突方向判別可能トルク閾値

0 < τ t h r ≤ τ c t h (衝突検出判定閾値）

衝突閾値信号 D t h t (3 5) は式（23) に示す通り、衝突トルク検出値 r d i s d (2 7) の絶対値が、所定の衝突方向判別可能トルク閾値 τ t h rを下回ったとき、 1となる。

式（23) における衝突方向判別可能トルク閾値 τ t h rは、衝突検出判定閾値 τ c t h以下の条件で、衝突トルク検出値 τ d i s d (2 7) の検出誤差より大きく設定すれば良い。

衝突トルク閾値判定信号 D t h t = 1の時は、衝突速度 · トルク方向判定ブロック（2 3) の出力信号である衝突方向フラグ D i r (3 1) の値に関わらず、衝突が検出されると、通常制御モード（図 1 9) から電流制御モード（図 2 0) へ移行し、モータ回転を減速させる。その後は第 7の実施例と同じ処理を行ラ。

この時、実施例 7の式（22) における閾値 co t h (5) を、ロボットが損傷しない速度レベルで設定することで、衝突検出時の角速度 ω f b (1) の絶対値が閾値 ω t h (5) より小さく、ロボットに損傷を与えないレベルである場合は、通常制御モード（図 1 6) から電流制御モード（図 1 7) へは移行せず、モ一タに逆トルクを印加して減速させることは無しに、柔軟制御モード（図 1 8) へと移行する。

また、衝突検出時の角速度 ω f b ( 1) が大きい場合でも、ロボットが損傷しない速度レベルである閾値 ω t h (5) まで減速すれば、柔軟制御モード（図 1 8) へと移行することとなる。

衝突トルク閾値判定信号 D t h t = 0の時は、第 7の実施例と同じ処理を行う。

(第 9の実施例）

実施例 7では、衝突検出後、モータ回転方向と衝突トルク方向が逆の軸では、実位置を位置指令に追従させるための電流指令を生成する位置制御から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御に切り換えることによりモータ回転速度を減速し、衝突エネルギーを緩和する。その後、モータ速度が設定値以下になれば、衝突力方向に倣う柔軟制御に切り換え、衝突で生じた減速機等での歪みを解消する方法を説明した。

しかしながら、衝突検出時の速度が大きい場合は、モータ回転が減速した時点で柔軟制御に切り換えただけでは、衝突で生じた減速機等での歪みを十分に解消することができなレ、可能性がある。

そこで、モータ回転方向と衝突トルク方向が逆で、モータ回転速度及ぴ衝突トルク検出値の大きさが設定値を上回った軸では、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータ回転速度が減速した後、速度方向が反転するまで逆方向のトルクを印可し続け、衝突で生じた減速機等での歪みの一部分を解消した後、反転した速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換える。

以上の様な制御方法を実行すれば、制御方式が複雑で、逆トルク印加時間が長くなり、衝突方向とは逆方向に大きく跳ね返る可能性が発生する反面、減速機等での歪みの解消を早めることが可能となる。

図 2 2は、本発明の第 9の実施例を示す図である。

図 2 2は、第 7の実施例を示す図 1 6に対し、モータ減速判定手段（3 2 ) をモータ減速 &反転判定手段（3 9 ) に変更し、第 8の実施例で設けた衝突トルク閾値判定手段（3 4 ) に加え、衝突検出時のモータ速度の大きさを判定する衝突速度判定手段（3 7 ) を新たに設けることにより、モータ回転方向と衝突トルク方向が逆で、モータ回転速度及び衝突トルク検出値の大きさが設定値を上回つたと判定した軸では、モータ回転と逆方向のトルクをモータで発生させる電流制御モードに切り換え、モータ回転速度が減速した後も速度方向が反転するまで逆方向のトルクを印可し続け、衝突で生じた減速機等での歪みの一部分を解消した後、反転した速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換える制御モード切換手段（1 5 ) を有することを特徴とする。

以下、図 2 2を用いて追加手段である衝突速度判定手段（3 7 ) と、変更手段であるモータ減速 &反転判定手段（3 9 ) の動作、機能について説明する。

第 7の実施例において、電流制御モード（図 1 7 ) への移行は、式（20) で定める衝突方向フラグ D i r ( 3 1 ) のみで決めていた。

第 9の実施例では、第 8の実施例で加えた衝突トルク閾値判定手段（3 4 ) を設け、式（23) において、 D t h t = 0の場合、つまり、衝突トルク検出値て d i s d ( 2 7 ) の絶対値が、所定の衝突方向判別可能トルク閾値 τ t h r以上で、かつ、衝突方向フラグ D i r = lの時（モータ回転方向と衝突トルク方向が逆の時）のみを取り扱う。

上記以外の場合は、実施例 7あるいは実施例 8の制御方法に従うこととする。

衝突検出時のモータ角速度 ω f b (1) の絶対値が所定の衝突速度判定閾値 ω t h s (3 9) より大きい場合、衝突速度判定手段（3 7) は、衝突速度判定信号 D t h s (3 8) を出力する。

D t h s = 1 ( I ω f b I > ω t h s ) ··· (24) ただし、 ω t h s ω t h (減速判定閾値）

衝突トルク閾値判定信号 D t h t = 0かつ衝突方向フラグ D i r = 1の時に、このモータ減速判定信号 D t h s == 1が出力されると、制御モード切換ブロック（1 5) は I c om2 (3) をモータ電流 I m (6) として選択し、通常制御モード（図 2 2) からモータに逆トルクを印加して減速させる電流制御モード（図 2 3) へと移行する。

この逆トルク印加で、モータ速度 ω f b ( 1) が減速し、反転したことをモータ減速手段（3 2) で判定する。具体的には、式（²2) において、モータ減速判定信号 D t h (3 3) 力 S 「 1 (衝突時）」 →「0 (減速）」→「1 (反転）」と遷移すれば、制御モード切換ブロック（1 5) は I c om 3 (4) をモータ電流 I m (6) として選択し、図 24で示す柔軟制御モードへ移行する。

図 2 5は、以上の制御方法を時系列的に表すタイムチャートである。

(第 1 0の実施例）

図 2 1は、本発明の第 1 0の実施例を示す図である。

図 2 1は、第 8の実施例を示す図 1 9に対し、電流制御手段を（1 3) から（3 6) へ変更したものであり、制御モード切換手段（1 5) において電流制御手段を選択する場合、モータ回転と逆方向のモータの最大トルクを発生させる電流を指令する電流制御手段（3 6) を有することを特徴とする。

この様に構成することにより、モータに最大限のブレーキをかけることができ、衝突エネルギーを可能な限り緩和することができる。

第 7の実施例を示す図 1 6、第 9の実施例を示す図 2 2においても同様の変更が可能なことは言うまでもない。 <産業上の利用可能性 >

以上のように、本発明のロボットの制御方法は、ロボットを駆動するモータの回転角を検出し、前記回転角から角速度の実測値を算出し、前記モータに与える指令値から算出した指令値の角速度と前記実測値の角速度の絶対値の大きな方を用いて摩擦トルクを算出し、前記指令値に基づいてモータを駆動する際にモータへの指令値に前記摩擦トルクに対応する値を加えることにより、角速度指令に従って能動的に動作する場合と、外力に押されて受動的に動作する場合の両方で常に摩擦補償が有効になることを特徴とする口ポットの制御方法である。この特徴により、フィードバック制御による電流制限を摩擦トルク以下に抑えることができ、より柔軟性の高い制御方法が実現できる。

また、速度の絶対値比較の際に、少なくとも指令値と実測値の一方に重み係数を乗算あるいは加算することにより、速度指令値と実測値のいずれかを優先的に速度として採用することが出来、例えば測定誤差が少ない速度指令値を優先的に選択する設定が可能となる。

さらに、少なくとも速度の指令値と実測値の一方に摩擦補償率を乗じた値を角速度として用いることにより、フィードバック特性が振動的になることの防止と、目標追従特性の改善の両立が実現できる。

さらに、本発明のロボットの制御方法によれば、モータ発生トルクの損失分にあたる減速機の動摩擦トルクを、動力学トルクに比例して増大させて計算することにより、衝突トルク検出精度の向上を実現することができる。

また、減速機の動摩擦トルクを、動力学トルクに比例して増大させて演算することにより、動摩擦トルクの算出精度を向上させ、最適なフィードフォヮ一ド補償を実現することができる。

以上のように、本発明のロボットの制御方法は、衝突検出後、モータ回転方向と衝突方向が逆の場合は、位置制御モードから電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、衝突エネルギーを緩和する。その後、モータ回転速度が設定値以下になれば、柔軟制御に切り換え、衝突で生じた減速機等での歪みを解消する。一方、モータ回転方向と衝突方向が同じ場合は、電流制御を介さずに、位置制御から直接、柔軟制御に切り換える。衝突力に倣って動作させることにより、衝突トルクを緩和する。以上の内容で衝突停止動作を構成することで、衝突によるロボットのダメージを最小限に抑えることが可能となる。

さらに、衝突トルク検出値の絶対値が設定値を下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御から電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、速度が設定値以下になれば、柔軟制御モードに切り換える。この様に構成することにより、衝突トルクが大きく加わっている軸では、衝突方向とモータ回転方向に従って適切な停止動作を行い、それ以外の衝突トルクが大きく加わっていない軸は速やかに停止させ、慣性エネルギーを減少させることができ、センサレスの衝突トルク検出で検出誤差が大きな場合でも適切な停止手段を選択することが出来る。

さらに、モータ回転方向と衝突トルク方向が逆で、モータ回転速度及び衝突トルク検出値の大きさが設定値を上回った軸で、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータ回転速度が減速した後、速度方向が反転するまで逆方向のトルクを印可し続け、衝突で生じた減速機等での歪みの一部分を解消した後、反転した速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換えることにより減速機等での歪みの解消を早めることが出来る。

さらに、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータの最大トルクを発生させる電流を指令する。この様に構成することにより衝突エネルギーを最大限に緩和することができる。

Claims

請求の範囲

1 . ロボットアームを駆動するモータの回転角を検出する工程と、前記回転角から実測角速度を算出する工程と、

前記モータに与える指令値から指令角速度を算出する工程と、

前記指令角速度と前記実測角速度の絶対値の大きな方に基づいて摩擦トルクを算出する工程と、

前記指令値に基づいてモータを駆動する際に、モータへの指令値に前記摩擦トルクに対応する値を加える工程とを含むロボットアームの制御方法。

2 . 指令角速度と実測角速度の絶対値を比較する際に、指令角速度と実測角速度の少なくとも一方に重み係数を乗算あるいは加算する工程を含む請求項 1記載のロボットァームの制御方法。

3 . 前記指令角速度と実測角速度の少なくとも一方に摩擦補償率を乗じる工程を含む請求項 1記載の口ボットアームの制御方法。

4 . 減速機を介してモータにより駆動されるロボットアームの制御方法であって、

前記モータにより発生する発生トルクを算出する工程と、

前記トルクから逆動力学演算によつて減速機出力で必要な、少なくとも減速機の動摩擦トルクを含んだ、摩擦トルクを算出する工程、

上記減速機出力で必要な摩擦トルクに応じて減速機の動摩擦トルクを増大させる工程、

前記発生トルクから増大された摩擦トルクを差し引いて外力を算出する工程とを含むロボットの制御方法。

5 . 減速機を介してモータにより駆動されるロボットアームの制御方法であって、

前記減速機出力で必要なトルクを求めるロボットの逆動力学演算と減速機の動摩擦トルク演算に基づいてフィードフォヮ一ドでモータ出力トルク補償を行う工程と、

フィードバック制御を行う際に、前記減速機の動摩擦トルクを減速機出力で必要なトルクに比例して増大させて演算するロボットアームの制御方法。

6 . モータにより駆動されるロボットの衝突検出後の停止動作において、

モータ回転方向と衝突トルク方向が逆の軸では、実位置を位置指令に追従させるための電流指令を生成する位置制御から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御に切り換えることによりモータ回転速度を減速し、速度が設定値以下になれば、衝突力方向に倣う柔軟制御に切り換える

一方、モータ回転方向と衝突トルク方向が同じ軸では、位置制御から柔軟制御に切り換えることを特徴とするロボットの制御方法。

7 . 衝突トルク検出値の大きさが設定値を下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御から電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、速度が設定値以下になれば、柔軟制御に切り換えることを特徴とする請求項 6記載のロボットの制御方法。

8 . モータ回転方向と衝突トルク方向が逆で、モータ回転速度及び衝突トルク検出値の大きさが設定値を上回った軸で、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータ回転速度が減速した後、速度方向が反転するまで逆方向のトルクを印可し続け、速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換えることを特徴とする請求項 6記載の口ボットの制御方法。

9 . 衝突検出後、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータの最大トルクを発生させる電流を指令することを特徴とする請求項 6記載のロボットの制御方法。

1 0 . ロボットのアームに加わる衝突力が前記アームを駆動するモータに加えるトルクの大きさ及び方向を検出する衝突トルク検出手段と、

前記衝突トルク検出値の大きさと設定した衝突トルク閾値を比較して衝突を判定する衝突判定手段と、

モータの回転速度の大きさ及ぴ方向を検出するモータ回転検出手段と、前記衝突トルク検出方向とモータ回転方向を比較する衝突方向判別手段と、

モータ回転速度の大きさと設定した閾値を比較してモータの減速を確認するモータ減速判定手段を設け、

モータ回転方向と衝突トルク方向が逆の軸では、実位置を位置指令に追従させるための電流指令を生成する位置制御手段から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御手段に切り換えることによりモータ回転速度を減速し、速度が設定値以下になれば、衝突力方向に倣う柔軟制御手段に切り換え、

モータ回転方向と衝突トルク方向が同じ軸では、位置制御手段から柔軟制御手段に切り換える制御モード切換手段を有することを特徴とする口ボッ卜の制御装置。

1 1 . 衝突トルク検出値の大きさと衝突方向判別可能トルク閾値を比較する衝突トルク閾値判定手段を設け、

衝突トルク検出値の大きさが衝突方向判別可能トルク閾値下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御手段から電流制御手段に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、速度が設定値以下になれば、柔軟制御手段に切り換える制御モード切換手段を有することを特徴とする請求項 1 0記載のロボットの制

1 2 . モータ回転方向と衝突トルク方向が逆で、モータ回転速度及び衝突トルク検出値の大きさが設定値を上回った軸で、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータ回転速度が減速した後、速度方向が反転するまで逆方向のトルクを印可し続け、反転した速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換えることを特徴とする請求項 1 0記載のロボットの制御装置。

1 3 . 衝突検出後、制御モード切換手段において電流制御手段を選択する場合、モータ回転と逆方向のモータの最大トルクを発生させる電流を指令する電流制御手段を有することを特徴とする請求項 1 0記載のロボットの制御装置。