JP2009090462A - ロボットの制御方法および制御装置 - Google Patents

Abstract

【課題】 ロボットの衝突検出後の停止動作には、衝突検出位置へ戻す方法、動作方向に対しモータ最大の逆トルクで制止させる方法がある。前者は応答性が悪く、停止時間が長引き、衝突による損傷防止ができない可能性がある。後者はトルクの印加時間を予め設定する必要があり、印加時間が短いと減速が不十分で衝突損傷が大きくなり、印加時間が長いと逆方向へな動作を行い再衝突の可能性がある。
【解決手段】 衝突検出後、モータ回転方向と衝突方向が逆の場合は、位置制御から電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させモータ回転速度を減速し、衝突エネルギーを緩和する。その後、モータ回転速度が設定以下になれば、柔軟制御に切り換え、衝突で生じた減速機等での歪みを解消する。モータ回転方向と衝突方向が同じ場合は、位置制御から直接柔軟制御に切り換える。
【選択図】 図１６

Description

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

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

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

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

フィードバック制御において電流指令を抑える手段としては、特開平09-179632(米国特許5994864)に示されるリミットをかける方式や、特開平08-155868に示されるフィードバ
ックゲインを下げる方式等が提案されている。以上の様に、ロボットアームの柔軟性(Compliance)を高めるためには、フィードバック制御における電流指令をどこまで抑えられるかが重要となる。フィードバック制御による電流指令が抑えられなければ、通常のサーボ剛性に近づくことになり、サーボ制御の柔軟性は低くなる。
特開平09-179632号公報(米国特許5994864) 特開平08-155868号公報

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

図３は従来例における実速度を用いた摩擦補償の制御方法を示すブロック線図である。図中、参照番号１はモータ回転角指令 θｃｏｍ、２：フィードバック制御器、３：電流
制限手段、４：フィードバック制御電流指令 Ｉｃｏｍ、５：モータ電流 Ｉｍ、６：モータ＋実負荷を示す範囲、７：モータトルク定数 Ｋｔ、８：モータ発生トルク τｍｍ、９：モータに加わる外力 τμ＋τｄｙｎ＋τｄｉｓ、１０：モータイナーシャの伝達関数
、１１：モータ回転角 θｆｂ、１２：微分計算子、１３：モータ角速度指令 ωｃｏｍ、１４：微分計算子、１５：モータ角加速度指令 αｃｏｍ、１６：モータイナーシャ（ロ
ータ＋減速機１次側）Ｊ、１７：ロボット動作に必要なモータ電流 Ｉｍｌ、１８：モー
タトルク定数の逆数 １／Ｋｔ、１９：動力学トルク計算値 τｄｙｎ、２０：摩擦計算値τμ、２１：摩擦計算ブロック、２２：動力学計算ブロック、２３：モータ角速度 ωｆｂ、２４：微分演算子、２５：他軸モータ回転角、を示す。

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

τmm = Kt * Im ・・・ (1)
τml = J * α + τμ + τdyn + τdis ・・・ (2)
ただし、式(1)、(2)における記号は以下のとおりである。

Kt： モータトルク定数
Im： モータ電流
α： モータ角加速度
ω： モータ角速度
J： モータイナーシャ(ロータ+減速機１次側)
τμ： 摩擦トルク(モータ軸端に換算)
τdyn： 動力学トルク(重力トルク、慣性力、遠心力、コリオリ力、弾性力の
和。モータ軸端に換算)
τdis： 外乱トルク(外部からの接触トルク、パラメータ誤差。モータ軸端に
換算)
式（２）で外乱トルクτｄｉｓ＝０とすれば、式（１）、（２）よりロボットアームの動作に必要なモータ電流Ｉｍｌを計算することができる。

Ｉml = (J * α +τμ + τdyn) / Kt ・・・ (3)
図３に示すように、この式（３）で計算したＩｍｌをフィードバック電流指令Ｉｃｏｍに加算すれば、外乱トルクτｄｉｓ＝０であれば、フィードバック電流が０でも、目標位置に追従することが可能となる。

図３において、フィードバック電流指令Ｉｃｏｍ（４）は、回転角指令θｃｏｍ（１）と実際のモータ回転角θｆｂから、ＰＩＤ計算等をフィードバック制御器（２）で行い、電流制限（３）をすることで求められる。電流制限（３）の手段としては、リミットをかける方式やフィードバックゲインを下げる方式等がある。

一方、モータ回転指令θｃｏｍ（１）を微分計算（１２）（１４）を２回行うことにより求められる角加速度αｃｏｍ（１５）にモータイナーシャＪ（１６）を乗じたものに、摩擦トルクτμ（２０）と動力学トルクτｄｙｎ（１９）を加算し、モータトルク定数の逆数１／Ｋｔ（１８）を乗じれば、式（３）で計算されるＩｍｌ（１７）を算出することができる。

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

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

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

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

また、式（３）で計算するロボット動作に必要なモータ電流Ｉｍｌの主要項の一つである摩擦トルクτμは、力の作用方向で決まる静止摩擦トルクτμｓと動摩擦トルクτμｍ、そして速度の大きさに比例する粘性摩擦トルクτμｄ（粘性係数Ｄ）で構成される。

τμ =τμs + τμm + τμd ・・・ (4)
ただし、式(４)においてそれぞれの項は以下のとおり計算する
τμs = τμs0 * sgn1(ω) ・・・ (5)
sgn1(ω)＝ 1 (ω=0、動作方向>0)
sgn1(ω)＝ 0 (ω≠0)
sgn1(ω)＝ -1 (ω=0、動作方向<0)
τμm = τμm0 * sgn2(ω) ・・・ (6)
sgn2(ω) ＝ -1 (ω<0)
sgn2(ω) ＝ 0 (ω＝0)
sgn2(ω) ＝ 1 (ω>0)
τμd = D *ω ・・・ (7)
式（５）〜（７）から明らかなように、全ての摩擦トルクは角速度ωに依存して計算される。

この摩擦トルクの計算で使用する速度ωについて、前出の従来例（特開平9-179632）ではフィードバックされる角速度ωＦｂを、また別の従来例（特開平10-180663）では位置
指令θｃｏｍを微分した角速度指令ωｃｏｍを用いていた。

しかしながら、ロボットの柔軟制御においては、位置指令θｃｏｍに従って能動的に動作する場合と、外力に押されて受動的に動作する場合がある。

図３に示すように、摩擦トルクτμ（２０）を式（５）で計算ブロック（２１）によって計算する際に用いる角速度として、実際のモータ回転角θｆｂ（１１）を微分（２４）することにより求められる実角速度ωｆｂ（２３）を用いると、モータに加わる外乱トルク（９）の一部である外力τｄｉｓによる速度変動が反映され、摩擦トルクτμ（２０）の計算精度は上がる。

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

また、ロボットが動作するまでは実角速度ωＦｂは０であり、式（６）、（７）で計算される動摩擦トルクτμｍ、粘性摩擦トルクτμｄも当然０であるので、式（４）で計算される摩擦トルクτμは０であり、ロボットを動作させるためのモータトルクは一切発生しない。

この状況で、フィードバック制御の電流指令Ｉｃｏｍ（４）によるトルクが、柔軟性を高めるために抑制され、実際の静止摩擦トルクτμｓを下回っていると、回転角指令θｃｏｍ（１）が発生してもロボットは動かない。

一方、図４に示すように、摩擦トルクτμ（２０）計算（２１）に用いる角速度ωにモータ回転角指令θｃｏｍ（１）を微分（１２）することにより得られる角速度指令ωｃｏｍ（１３）を用いるとこれらの問題は解消される。つまり、ロボットが動作していなくても、角速度指令ωｃｏｍ（１３）を用いて摩擦トルクτμ（２０）を式（４）で計算（２１）し、フィードバック電流指令Ｉｃｏｍ（４）に加算することにより、実摩擦トルクを補償でき、フィードバック制御による電流指令Ｉｃｏｍ（４）が抑えられていても、ロボ
ットの動作が可能となる。

ただし、摩擦トルクτμ（２０）の計算（２１）に用いる角速度ωに角速度指令ωｃｏｍ（１３）を用いると、角速度指令ωｃｏｍ（１３）に従って能動的に動作するが、動作途中で外乱トルクτｄｉｓにより角速度変動が発生した場合は角速度指令ωｃｏｍ（１３）と実角速度ωｆｂ（図３の２３）の誤差が大きくなり、式（７）で計算する粘性摩擦トルクτμｄの計算誤差も増大する。

また、角速度指令ωｃｏｍ（１３）が０で停止していた時、外力により押されて動作した場合は、式（４）で計算される摩擦トルクτμは常に０であるので、実摩擦トルクのトルク補償は一切されなくなる。

さらに外力で押されない場合でも、実際の動作ではフィードバック制御器（２）による制御において追従遅れが発生するので、動作停止時には、実角速度ωｆｂ（図３の２３）が０になる前に、角速度指令ωｃｏｍ（１３）が０になるので、その時点から式（４）で計算される摩擦トルクτも０になり、摩擦補償がされなくなる。つまり、角速度指令ωｃｏｍ（１３）が０に到達した時点で急激に停止し、目標点への未到達や振動発生等が発生する可能性がある。

ここで、フィードバック制御の電流指令Ｉｃｏｍ（４）によるモータの発生トルクが実摩擦トルク以下に抑えられている場合、外力によって位置偏差が大きくなってもロボットは動作せず、位置偏差を小さくすることができない。

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

以上、説明した様に、式（４）で摩擦トルクτμを計算する場合、角速度信号として実角速度ωｆｂか角速度指令ωｃｏｍのいずれか一方を用いる方法では、計算した摩擦トルクτμを用いて式（３）で計算した電流Ｉｍｌを、フィードバック制御電流に加算しても、実摩擦トルクを補償できない場合が発生する。

また、図３に示すように摩擦トルクτμ（２０）を式（５）で計算（２１）する際に用いる角速度に、実角速度ωｆｂ（２３）を用いて、摩擦トルクτμを１００％補償すると、制御系のフィードバック特性が摩擦補償されることになり、あたかも摩擦が無い状態として振る舞うので、柔軟性は高まるものの、フィードバック特性は振動的になる。

一方、図４に示すように、摩擦トルクτμ（２０）計算（２１）に用いる角速度ωに角速度指令ωｃｏｍ（１３）を用いた場合は、制御系のフィードバック特性には影響を与えないので、目標追従特性改善のためには１００％補償することが望ましい。

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

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

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

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

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

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

τmm = Kt * Im - (J * α + D * ω + τμ sgn(ω)) ・・・ (8)
τml = τdyn + τdis ・・・ (9)
ただし、式(1)、(2)における記号は以下のとおりである。

Kt： モータトルク定数
Im： モータ電流
α： モータ角加速度
ω： モータ角速度
J： モータイナーシャ(ロータ+減速機１次側)
D： 粘性摩擦係数(モータ軸端に換算)
τμ： 摩擦トルク(モータ軸端に換算)
τｇ： 重力トルク(モータ軸端に換算)
τdyn： 動力学トルク(重力トルク、慣性力、遠心力、コリオリ力、弾性力の
和。モータ軸端に換算)
τdis： 外乱トルク(衝突トルク、パラメータ誤差。モータ軸端に換算)
sgn(ω) ＝ 1 (ω>0)
sgn(ω) ＝ 0 (ω≠0)
sgn(ω) ＝ -1 (ω<0)
モータとロボットアームは減速機を介して接続しているので、式（9）においてモータ
イナーシャＪ以外の項は、減速比を用いてモータ軸端に換算する必要がある。

衝突トルクτｄｉｓは、（8）、（9）においてτｍｍ＝でｍ１と仮定する事により、以下の式（10）に変形して求めることが出来る。
τｄｉｓ = Kt * Im - (J * α + D * ω + τμ * sgn(ω) + τdyn) ・・・(10)
従来例では、式（10）における動摩擦項τμを固定値として計算していた。しかし、動摩擦トルクを固定値として演算すると、加減速時等のモータ発生トルクが大きい場合に、モータ発生トルクの１０％程度の大きな計算誤差を生じていた。

一方、サーボ制御特性の改善を目的とするフィードフォワード制御は、外乱トルクτｄｉｓ＝０の条件、つまり、外部には接触せず、パラメータ誤差は無い条件で式（10）よりモータ電流Ｉｍについて解いた値をＩｆｆとし、Ｉｆｆを電流指令に加算することにより実現できる。

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

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

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

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

使用するロボットアームは、可搬質量が６ｋｇ、総アーム長が約１．３ｍの６軸垂直多関節ロボットであり、図12は手首３軸を省略し基本３軸を示したもので、計測には第３軸のＦＡ軸を動作させている。

ここでは、外乱トルクτｄｉｓ＝０の条件（外部接触は無く、パラメータ誤差もない条件）で測定している。

図11より、τｍｍ（１）とτｍ１（２）ではピークトルクの１０％程度の誤差（４）が発生しており、式(8)の中に誤差要因か有ることがわかる。

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

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

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

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

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

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

次に、衝突検出後に停止する方法について説明する。衝突検出位置へ戻す方法（特開２００１−１１７６１８(米国特許6429617)）や、速度指令を強制的に０に設定し停止させ
る方法（特開２０００−５２２８６）、さらには、モータ回転方向に対しモータ最大の逆トルクで停止させる方法（特許第３２１２５７１号(米国特許6298283)）、（特許第２８
７１９９３号（米国特許5418440））が提案されている。

衝突検出位置へ戻す方法では位置制御で元の位置へ戻すため、停止時間は位置制御の応答性に依存する。一般的に位置制御の応答性は高くても数十Ｈｚであるので、衝突停止としては応答性が悪く、停止時間が長引き、衝突による損傷をくい止められない可能性がある。

速度指令を強制的に０に設定し停止させる方法では、停止時間は速度制御の応答性に依存する。応答性は数百Ｈｚで位置制御よりは良いものの、電流制御の応答性（数ｋＨｚ）には及ばない。

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

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

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

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

図15（ａ）において、ＵＡ軸（４１）は角速度ωｆｂ（１）の方向、ＦＡ軸（４２）は角速度ωｆｂ’（６）の方向へ動作しているとする。時間が経過し、図15（ｂ）の方向へ各々のアームは動作し、障害物（４３）に衝突すると、衝突力（４４）が発生し、ＵＡ軸（４１）にはモータ回転方向とは逆方向の力、すなわち減速させる方向に衝突トルクτｄｉｓ（９）が加わる。一方ＦＡ軸（４２）には、モータ回転方向とは同方向の力、すなわち加速させる方向に衝突トルクτｄｉｓ’（１０）が加わる。

この後、ＦＡ軸（４２）を衝突検出位置へ戻したり、急停止させるためには、モータ回転を減速させるトルクをモータで発生させる必要があるが、衝突トルクτｄｉｓ’（１０）に逆らった方向のトルクとなるため、かえって衝突トルクを増大させることになる。

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

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

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

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

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

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

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

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

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

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

本発明によれば、モータにより駆動されるロボットの柔軟制御において、フィードバック制御による電流制限を摩擦トルク以下に抑え、より柔軟性を高くできる。また、本発明によれば、センサレス衝突検出において、減速機の動摩擦トルクを固定値ではなく、実際の特性に合致させることにより、高精度な衝突トルク検出をすることができる。また、本発明によれば、ロボットの駆動力における減速機の動摩擦トルクを固定値ではなく、実際の特性に合致させることにより、動作前のモータトルク演算精度を向上させ、位相遅れの少ない最適なフィードフォワード補償を得ることができる。

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

（第１の実施例）
図１は、本発明の制御方法を示すブロック線図である。図１において、２６は速度切換器、２７は速度切換器により選択された角速度ωである。フィードバック電流指令Ｉｃｏｍ（４）は、回転角指令θｃｏｍ（１）と実際のモータ回転角θｆｂから、ＰＩＤ計算等をフィードバック制御器（２）で行い、電流制限（３）をすることで求められる。電流制限（３）の手段としては、リミットをかける方式やフィードバックゲインを下げる方式等がある。

一方、モータ回転指令θｃｏｍ（１）を微分計算（１２）（１４）を２回行うことにより求められる角加速度αｃｏｍ（１５）にモータイナーシャＪ（１６）を乗じたものに、摩擦トルクτμ（２０）と動力学トルクτｄｙｎ（１９）を加算し、モータトルク定数の逆数１／Ｋｔ（１８）を乗じれば、式（３）で計算されるＩｍｌ（１７）を算出することができる。

この摩擦トルクτμ（２０）を（数３）の式（５）〜（９）で計算（２１）する際に用いる角速度ω（２７）に、モータ回転角指令θｃｏｍ（１）を微分（１２）した角速度指
令ωｃｏｍ（１３）と、実モータ回転角θｆｂ（１１）を微分（２４）した実角速度ωｆｂ（２３）のいずれかを選択するのが、速度切換器（２６）である。

速度切換器（２６）において、速度切換は以下の式で行う。

ω ＝ ωfb (｜ωcom｜ ≦ ｜ωfb｜)
ω ＝ ωcom (｜ωcom｜ > ｜ωfb｜) ・・・ （12）
式（12）では、角速度指令ωｃｏｍ（１３）と実角速度ωｆｂ（２３）の絶対値の大小を比較して、大きい方をω（２７）として選択する。

そして、このω（２７）を用いて摩擦トルクτμ（２０）を式（５）〜（７）で計算する。

このように選択することで、柔軟制御が有効になっていても、位置指令θｃｏｍ（１）が入力されロボットアームが能動的に動作する場合と、外力τｄｉｓに押されて受動的に動作する場合のいずれの場合でも、摩擦トルクτμ（２０）は、０になることはなく適切な大きさで計算される。

また、ロボットが能動的に動作している間に、外力τｄｉｓに押されて位置偏差が大きくなり、角速度指令ωｃｏｍ（１３）と実角速度ωｆｂ（２３）間に差違が出ても、実角速度ωｆｂ（２３）の絶対値が角速度指令ωｃｏｍ（１３）の絶対値より大きくなれば、ω（２７）としては、ωｆｂ（２３）が採用されるので、摩擦トルクτμ（２０）の計算における誤差要因を減らすことが出来る。

さらに、フィードバック制御器（２）での制御遅れにより、動作停止時には、実角速度ωｆｂ（２３）が０になる前に、角速度指令ωｃｏｍ（１３）が０になっても、実角速度ωｆｂ（２３）の絶対値が角速度指令ωｃｏｍ（１３）の絶対値より大きくなれば、ω（２７）としては、ωｆｂ（２３）が採用されるので、その時点から式（４）で計算される摩擦トルクτによる摩擦補償も継続される。つまり、角速度指令ωｃｏｍ（１３）が０に到達した時点で、摩擦補償が無くなることによる急激な停止を防止でき、目標位置への未到達や振動発生等の問題が発生する可能性を低く出来る。

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

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

ω ＝ ωfb (｜kc1 * ωcom + kc2｜ ≦ ｜ωfb｜)
ω ＝ ωcom (｜kc1 * ωcom + kc2｜ > ｜ωfb｜) ・・・ （13）
式（13）の様に構成することにより、速度指令値と実測値のいずれかを優先的に速度として採用することが出来る。

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

kc1 > 1かつkc2 > 0 ・・・ (14)
（第３の実施例）
図２は第３の実施例の制御方法を示すブロック線図である。

速度切換機（２６）に入力される実角速度ωｆｂ（２３）に摩擦補償率ｋμを乗じる。式で表すと、式（15）の様になる。
ω ＝ kμ * ωfb (｜kc1 * ωcom + kc2｜ ≦ ｜ωfb｜)
ω ＝ωcom (｜kc1 * ωcom + kc2｜ > ｜ωfb｜) ・・・ （15）
ただし、kμ：摩擦補償率
式（15）で得られた角速度ω（２７）を用いることにより、摩擦トルクτμ（２０）を（数３）の式（５）で計算（２１）する際に用いる角速度に、実角速度ωＦｂ（２３）を選択した場合、摩擦補償率ｋμを１以下にすると、摩擦トルクτμが１００％補償されなくなり、フィードバック特性が振動的になることを調整できる。

一方、摩擦トルクτμ（２０）計算（２１）に用いる角速度ωに角速度指令ωｃｏｍ（１３）を用いた場合は、制御系のフィードバック特性には影響を与えず、摩擦トルクτμが１００％補償されので、目標追従特性が改善される。

（第４の実施例）
ロボットに用いられる代表的な減速機における動摩擦トルクを外乱トルクτｄｉｓ＝０の条件で、動力学トルクτｄｙｎの変動に対し測定したものが、図６、図７である。図６はハーモニック減速機、図７は偏心差動式減速機の一種であるＲＶ減速機における特性である。

図６、図７とも、動力学トルクτｄｙｎの増大に応じて、動摩擦トルクが増大していることがわかり、式（16）で近似することが出来る。

τμa ＝ A * τdyn + B (τdyn ≧ τth)
τμa ＝ C * τdyn² + D ( ｜τdyn｜< τth)
τμa ＝ E * τdyn + F (τdyn ≦ -τth) ・・・ (16)
式（16）において、Ａ、Ｂ、Ｃ、Ｄ、Ｅ、Ｆは近似定数、τｔｈは設定閥値である。図６、図７における上記パラメータは図８のようになる。

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

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

τdisa = Kt * Im
- (J*α + D*ω+ τμａ * sgn(ω) + τdyn) ・・・ (17)
この式（17）で衝突トルクτｄｉｓａを計算することで、式（10）に比べ、特に衝突前の誤差を小さくすることができ、衝突検出感度を鈍くする必要がなくなり、衝突検出精度を高めることが出来る。

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

図５において、モータ回転角指令値θｒｅｆ（１１）とモータ＋実負荷を示す点線内（１２）より得られるモータ回転角θＭ（１３）とを比較して、制御器（１４）は電流Ｉｍ（１５）をモータに供給する。モータでは電流Ｉｍ（１５）にトルク定数Ｋｔ（１６）を乗じたトルクが発生し、このトルクから動力学トルクτｙｎ、衝突トルクτｄｉｓ及び摩
擦トルクτμ×ｓｇｎ（ω）の和（１７）を差し引いたトルクが、ブロック（１８）で表されるモータ単体の駆動に用いられる。このモータの回転角θＭ（１３）と電流Ｉｍ（１５）より式（17）で、衝突トルク検出値τｄｉｓａ（２０）を計算するブロックが衝突トルク計算部（１９）である。

尚、誤差は増えるものの式（17）で、動力学トルクτｄｙｎの代わりにモータ電流トルクτｍ０＝Ｋｔ×Ｉｍを用いても良い。

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

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

Iff = (J*α + D*ω + τμa * sgn(ω) + τdyn) / Kt ・・・ (18)
この（18）式において角速度ωと角加速度αをモータ回転角指令θｒｅｆから微分して計算すると、式（19）の様に変形でき、モータに必要なトルクを発生させる電流Ｉｆｆを、フイードバック信号を用いずに計算することができる。
Iff = (J*s² (θref) + D*s (θref) +τμa * sgn(ω) + τdyn) / Kt ・・・ (19)
図１０は、このフィードフォワード電流Ｉｆｆで、フィードフォワード補償を行った場合の実施の形態例を示すブロック線図である。

図１０において、モータ回転角指令値θｒｅｆ（１１）とモータ＋実負荷を示す点線内（１２）より得られるモータ回転角θＭ（１３）とを比較して、フィードバック制御（２１）はモータに流す電流指令Ｉｃｏｍ（２２）を出力する。このフィードバック制御で得られた電流指令Ｉｃｏｍ（２２）に、（数８）の式（９）を示すブロック（２３）で得られるフィードフォワード電流指令Ｉｆｆ（２４）を加算することにより、誤差と推定遅れの少ないフィードフォワード制御が実現できる。

（第７の実施例）
図１６は本発明の第７の実施例を示す図である。

図１６において、（２６）はロボットのアームに加わる衝突力がアームを駆動するモータに加える衝突トルクτｄｉｓｄ（２７）を検出する衝突トルク検出手段、（２５）は衝突トルク検出値τｄｉｓｄ（２７）の大きさと設定した衝突トルク閾値を比較して衝突を判定し、衝突検出信号Ｄｃｏｌ（３０）を出力する衝突判定手段、（２４）はモータ回転角θｆｂ（２２）よりモータの角速度ωｆｂ（１）を検出するモータ回転検出手段、（２３）は衝突トルク検出方向とモータ回転方向を比較し衝突方向フラグＤｉｒ（３１）を出力するする衝突方向判別手段、（３２）はモータ角速度ωｆｂ（１）の大きさと設定した閾値を比較してモータの減速を確認しモータ減速判定信号Ｄｔｈ（３３）を出力するモータ減速判定手段、（１５）は、モータ回転方向と衝突トルク方向が逆の軸では、モータ回転角θｆｂを回転角指令θｃｏｍ（１１）に追従させるための電流指令を生成する位置制御手段（１２）から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御手段（１３）に切り換えることによりモータ回転速度を減速し、速度が設定値以下になれば、衝突力方向に倣う柔軟制御手段（１４）に切り換え、モータ回転方向と衝突トルク方向が同じ軸では、位置制御手段（１２）から柔軟制御手段（１４）に切り換える制御モード切換手段である。

以下、図１６を用いて衝突検出後の停止制御方法について詳細に説明する。
位置制御するための電流指令Ｉｃｏｍ１（２）は、モータ回転角指令θｃｏｍ（１１）と実際のモータ回転角θｆｂ（２２）から、位置制御手段であるフィードバック制御器（１２）で求められる。フィードバック制御器（１２）は通常、ＰＩＤ制御等で構成される。

衝突検出前の通常の位置制御では、制御モード切換ブロック（１５）でモータ電流Ｉｍ（１６）として、この電流指令Ｉｃｏｍ１（２）を選択し、モータ＋実負荷（１７）に印加する。

モータイナーシャＪを使って記述されるモータ伝達関数（２１）には、モータ電流Ｉｍ（１６）をトルク定数Ｋｔ（１８）で乗じたモータトルクτｍｍ（１９）と外乱トルク（２０）が印加される。

外乱トルク（２０）は摩擦トルクτμ、重力トルクτｇ、動力学トルクτｄｙｎ（慣性力、遠心力、コリオリ力の和）、衝突トルクτｄｉｓの和である。

モータ伝達関数（２１）からモータ回転角θｆｂ（２２）が出力され、通常は光学式あるいは磁気式エンコーダで検出する。

衝突トルク検出ブロック（２６）では、このモータ回転角（２２）と他軸のモータ回転角（２９）及びそれらを時間微分することにより導出できる角速度、角加速度、さらにはロボット機械パラメータの情報を用い、衝突トルクτｄｉｓが発生していない条件で、モータに必要なトルクを逆動力学演算で求め、実際のモータ電流Ｉｍｍ（１６）にトルク定数Ｋｔ（１８）を乗じたものを減算することで、衝突トルク検出値τｄｉｓｄ（２７）を求める。

衝突判定ブロック（２５）では、この衝突トルク検出値τｄｉｓｄ（２７）と、他軸でも同様に求めた他軸衝突トルク検出値（２８）の内、１軸でも所定の衝突検出閾値τｃｔｈを越えた場合は、衝突が発生したと判定し、衝突検出信号Ｄｃｏｌ（３０）を制御モード切換ブロック（１５）へ送る。

モータ角速度検出ブロック（２４）では、モータ回転角θｆｂ（２２）に微分等を行いモモータ角速度ωｆｂ（１）を求める。このモータ角速度ωｆｂ（１）と衝突トルク検出値τｄｉｓｄ（２７）から、衝突方向判別ブロック（２３）は、以下の式で衝突方向フラグＤｉｒ（３１）を演算する。

Dir ＝ １（ωfb * τdisd ＜ ０）
Dir ＝ ０（ωfb * τdisd ≧ ０） ・・・ （20）
式（20）では、モータ角速度ωｆｂ（１）と衝突トルク検出値τｄｉｓｄ（２７）の方向が反対の時、衝突方向フラグＤｉｒ（３１）は１となり、それ以外は０となる。

図１５の動作においては、ＵＡ軸（４１）はＤｉｒ＝１、ＦＡ軸（４２）はＤｉｒ’＝０である。

制御モード切換ブロック（１５）は、衝突検出信号Ｄｃｏｌ（３０）が入力された時点で、衝突方向フラグＤｉｒ（３１）の情報を元に制御モードを切り換える。

Ｉｍ＝ Ｉｃｏｍ２ （Ｄｉｒ＝１） 電流制御モード
Ｉｍ＝ Ｉｃｏｍ３ （Ｄｉｒ＝０） 柔軟制御モード ・・・ （21）
ＵＡ軸は、Ｄｉｒ＝１であるので、モータ角速度ωｆｂ（１）から、電流制御ブロック（１３）でモータ回転方向と逆方向のトルクを発生させる電流Ｉｃｏｍ２（３）を生成し
、図１７に示すように、制御モード切換ブロック（１５）はＩｃｏｍ２（３）をモータ電流Ｉｍ（１６）として選択する。（電流制御モードへ移行）
この様に構成することにより、ＵＡ軸（４１）は急速に減速するので、衝突検出後の衝突トルクτｄｉｓ（９）を小さくすることが出来る。

ＵＡ軸（４１）が減速し、その角速度ωｆｂ（１）の絶対値が所定の減速判定閾値ωｔｈ（５）より小さくなると、モータ減速判定ブロック（３２）で、モータ減速判定信号Ｄｔｈ（３３）を出力する。

Ｄｔｈ＝１ （｜ωｆｂ｜ ＜ ωｔｈ） ・・・ （22）
このモータ減速判定信号Ｄｔｈ（３３）が出力されると、制御モード切換ブロック（１５）はＩｃｏｍ３（４）をモータ電流Ｉｍ（６）として選択し、図１８で示す柔軟制御モードへ移行する。

尚、衝突検出時において、角速度ωｆｂ（１）の絶対値が所定の閾値ωｔｈ（５）より小さく式（22）の条件が満たされている場合には、通常制御モード（図１６）から電流制御モード（図１７）へは移行せず、モータに逆トルクを印加して減速させることは無しに、柔軟制御モード（図１８）へと移行する。

ここで、柔軟制御ブロック（１４）はフィードバック制御器（１２）から出力される電流指令Ｉｃｏｍ１（２）に対し、電流を制限した上で、ロボットの自重落下防止のため重力補償電流を付加することにより柔軟制御を実現する。

このことにより、モータ回転角指令θｃｏｍ（１１）とモータ回転角θｆｂ（２２）間の偏差が大きくなってもモータ電流が制限されることにより、位置制御のサーボ剛性が弱くなり柔軟性が生まれる。

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

式（22）でモータ減速判定信号Ｄｔｈ＝１になった時には、モータ角速度ωｆｂ（１）が閾値ωｔｈｒより小さくなっているので、ほぼ停止状態であり慣性エネルギーも小さくなっており、柔軟制御モードへ移行することで衝突により生じた減速機等での歪みを解消することが出来る。

一方ＦＡ軸（４２）では、（数２）の式（２）の衝突方向フラグＤｉｒ’＝０であり、もしＵＡ軸（４１）と同じ様に急減速させると、かえって衝突トルクτｄｉｓ’（１０）を増大させてしまう。

そこで、衝突発生時に衝突方向フラグＤｉｒ’＝０の場合には、制御モード切換ブロック（１５）で、電流制御モード（図１７）を介さずに通常制御モード（図１６）から、柔軟制御モード（図１８）へ切り換える。

以上の様に構成することにより、ＦＡ軸（４２）は柔軟制御で衝突力に倣って動作することになり、衝突トルクを緩和することができる。

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

（第８の実施例）
図１９は本発明の第８の実施例を示す図である。

図１９は、第７の実施例を示す図１６に対し、新たに衝突トルク検出値τｄｉｓｄ（２７）の大きさと衝突方向判別可能トルク閾値を比較する衝突トルク閾値判定手段（３４）を設け、衝突トルク検出値τｄｉｓｄ（２７）の大きさが衝突方向判別可能トルク閾値を下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御手段（１２）から電流制御手段（１３）に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度ωｆｂ（１）を減速し、速度が設定値以下になれば、柔軟制御手段（１４）に切り換える制御モード切換手段（１５）を有することを特徴とする。

以下、図１９を用いて追加機能である衝突トルク閾値判定ブロック（３４）の動作、機能について説明する。

第７の実施例において、電流制御モード（図１７）への移行は、式（20）で定める衝突方向フラグＤｉｒ（３１）のみで決めていた。

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

そのため、衝突トルク検出値τｄｉｓｄ（２７）が小さく、０に近い軸では、検出誤差により衝突トルク検出値τｄｉｓｄ（２７）の符号に誤りが発生する可能性がある。

つまり、モータ速度ωｆｂ（１）が大きい、つまり慣性エネルギーが大きい軸でもτｄｉｓｄ（２７）が小さい場合には、衝突方向フラグＤｉｒ（３１）を誤って出力する可能性がある。

衝突判定ブロック（２５）では、衝突トルク検出値τｄｉｓｄ（２７）が所定の衝突判定閾値τｄｔｈより大きくなった軸が有れば、衝突と判断するので、衝突判定閾値より大きくなった軸では前記の衝突方向フラグＤｉｒ（３１）の誤出力は無い。

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

そこで、図１９で示す様に、新たに追加した衝突トルク閾値判定ブロック（３４）は、衝突トルク閾値判定信号Ｄｔｈｔ（３５）を制御モード切換ブロック（１５）へ出力する。

Ｄｔｈｔ ＝ １ （｜τｄｉｓｄ｜ ＜ τｔｈｒ）
Ｄｔｈｔ ＝ ０ （｜τｄｉｓｄ｜ ≧ τｔｈｒ）・・・ （23）
ただし、τｔｈｒ：衝突方向判別可能トルク閾値
０ ＜ τｔｈｒ ≦ τｃｔｈ（衝突検出判定閾値）
衝突閾値信号Ｄｔｈｔ（３５）は式（23）に示す通り、衝突トルク検出値τｄｉｓｄ（２７）の絶対値が、所定の衝突方向判別可能トルク閾値τｔｈｒを下回ったとき、１となる。

式（23）における衝突方向判別可能トルク閾値τｔｈｒは、衝突検出判定閾値τｃｔｈ以下の条件で、衝突トルク検出値τｄｉｓｄ（２７）の検出誤差より大きく設定すれば良い。

衝突トルク閾値判定信号Ｄｔｈｔ＝１の時は、衝突速度・トルク方向判定ブロック（２３）の出力信号である衝突方向フラグＤｉｒ（３１）の値に関わらず、衝突が検出されると、通常制御モード（図１９）から電流制御モード（図２０）へ移行し、モータ回転を減速させる。その後は第７の実施例と同じ処理を行う。

この時、実施例７の式（22）における閾値ωｔｈ（５）を、ロボットが損傷しない速度レベルで設定することで、衝突検出時の角速度ωｆｂ（１）の絶対値が閾値ωｔｈ（５）より小さく、ロボットに損傷を与えないレベルである場合は、通常制御モード（図１６）から電流制御モード（図１７）へは移行せず、モータに逆トルクを印加して減速させることは無しに、柔軟制御モード（図１８）へと移行する。

また、衝突検出時の角速度ωｆｂ（１）が大きい場合でも、ロボットが損傷しない速度レベルである閾値ωｔｈ（５）まで減速すれば、柔軟制御モード（図１８）へと移行することとなる。

衝突トルク閾値判定信号Ｄｔｈｔ＝０の時は、第７の実施例と同じ処理を行う。

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

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

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

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

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

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

以下、図２２を用いて追加手段である衝突速度判定手段（３７）と、変更手段であるモータ減速＆反転判定手段（３９）の動作、機能について説明する。

第７の実施例において、電流制御モード（図１７）への移行は、式（20）で定める衝突方向フラグＤｉｒ（３１）のみで決めていた。

第９の実施例では、第８の実施例で加えた衝突トルク閾値判定手段（３４）を設け、式（23）において、Ｄｔｈｔ＝０の場合、つまり、衝突トルク検出値τｄｉｓｄ（２７）の絶対値が、所定の衝突方向判別可能トルク閾値τｔｈｒ以上で、かつ、衝突方向フラグＤｉｒ＝１の時（モータ回転方向と衝突トルク方向が逆の時）のみを取り扱う。

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

衝突検出時のモータ角速度ωｆｂ（１）の絶対値が所定の衝突速度判定閾値ωｔｈｓ（３９）より大きい場合、衝突速度判定手段（３７）は、衝突速度判定信号Ｄｔｈｓ（３８）を出力する。

Ｄｔｈｓ＝１ （｜ωｆｂ｜ ＞ ωｔｈｓ） ・・・ （24）
ただし、ωｔｈｓ≧ωｔｈ（減速判定閾値）
衝突トルク閾値判定信号Ｄｔｈｔ＝０かつ衝突方向フラグＤｉｒ＝１の時に、このモータ減速判定信号Ｄｔｈｓ＝１が出力されると、制御モード切換ブロック（１５）はＩｃｏｍ２（３）をモータ電流Ｉｍ（６）として選択し、通常制御モード（図２２）からモータに逆トルクを印加して減速させる電流制御モード（図２３）へと移行する。

この逆トルク印加で、モータ速度ωｆｂ（１）が減速し、反転したことをモータ減速手段（３２）で判定する。具体的には、式（22）において、モータ減速判定信号Ｄｔｈ（３３）が「１（衝突時）」→「０（減速）」→「１（反転）」と遷移すれば、制御モード切換ブロック（１５）はＩｃｏｍ３（４）をモータ電流Ｉｍ（６）として選択し、図２４で示す柔軟制御モードへ移行する。

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

（第１０の実施例）
図２１は、本発明の第１０の実施例を示す図である。

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

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

第７の実施例を示す図１６、第９の実施例を示す図２２においても同様の変更が可能な
ことは言うまでもない。

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

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

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

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

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

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

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

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

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

本発明は、フィードバック制御による電流制限を摩擦トルク以下に抑えることができ、より柔軟性の高い制御方法が実現できるという効果を有し、ロボットの制御装置等に有用である。

第１、第２の実施例における摩擦補償の制御方法を示すブロック線図 第３の実施例における摩擦補償の制御方法を示すブロック線図 従来例における実速度を用いた摩擦補償の制御方法を示すブロック線図 従来例における速度指令を用いた摩擦補償の制御方法を示すブロック線図 本発明の実施の形態１における衝突トルク検出方法のブロック線図 ハーモニック減速機における減速機出力で必要なトルク動摩擦トルク特性の一例を示す図 ＲＶ減速機における減速機出力で必要なトルク動摩擦トルク特性の一例を示す図 動摩擦トルク近似式におけるパラメータを示す図 本発明おける動摩擦トルク計算方法による減速機出力で必要なトルク誤差を示す図 本発明の実施の形態２における減速機出力で必要なトルクフィードフォワード制御のブロツク線図 従来例における動摩擦トルク計算方法による減速機出力で必要なトルク誤差を示す図 減速機出力で必要なトルク測定時の動作を示す図 動摩擦トルクを大きくした場合の減速機出力で必要なトルク誤差を示す図 第１の実施例における衝突停止制御方法を示すタイムチャート 衝突時の速度方向と衝突トルク方向を示すロボット動作図 第１の実施例における衝突停止制御装置（位置制御モード）を示すブロック線図 第１の実施例における衝突停止制御装置（電流制御モード）を示すブロック線図 第１の実施例における衝突停止制御装置（柔軟制御モード）を示すブロック線図 第２の実施例における衝突停止制御装置（位置制御モード）を示すブロック線図 第２の実施例における衝突停止制御装置（電流制御モード）を示すブロック線図 第４の実施例における衝突停止制御装置（電流制御モード）を示すブロック線図 第３の実施例における衝突停止制御装置（位置制御モード）を示すブロック線図 第３の実施例における衝突停止制御装置（電流制御モード）を示すブロック線図 第３の実施例における衝突停止制御装置（柔軟制御モード）を示すブロック線図 第３の実施例における衝突停止制御方法を示すタイムチャート

２ フィードバック制御器
３ 電流制限手段
５ モータ電流
１０ モータイナーシャの伝達関数
２１ 摩擦計算ブロック
２２ 動力学計算ブロック

Claims (8)

1. モータにより駆動されるロボットの衝突検出後の停止動作において、
   モータ回転方向と衝突トルク方向が逆の軸では、実位置を位置指令に追従させるための電流指令を生成する位置制御から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御に切り換えることによりモータ回転速度を減速し、速度が設定値以下になれば、衝突力方向に倣う柔軟制御に切り換える一方、モータ回転方向と衝突トルク方向が同じ軸では、位置制御から柔軟制御に切り換えることを特徴とするロボットの制御方法。
2. 衝突トルク検出値の大きさが設定値を下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御から電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、速度が設定値以下になれば、柔軟制御に切り換えることを特徴とする請求項１記載のロボットの制御方法。
3. モータ回転方向と衝突トルク方向が逆で、モータ回転速度及び衝突トルク検出値の大きさが設定値を上回った軸で、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータ回転速度が減速した後、速度方向が反転するまで逆方向のトルクを印可し続け、速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換えることを特徴とする請求項１記載のロボットの制御方法。
4. 衝突検出後、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータの最大トルクを発生させる電流を指令することを特徴とする請求項１記載のロボットの制御方法。
5. ロボットのアームに加わる衝突力が前記アームを駆動するモータに加えるトルクの大きさ及び方向を検出する衝突トルク検出手段と、
   前記衝突トルク検出値の大きさと設定した衝突トルク閾値を比較して衝突を判定する衝突判定手段と、
   モータの回転速度の大きさ及び方向を検出するモータ回転検出手段と、
   前記衝突トルク検出方向とモータ回転方向を比較する衝突方向判別手段と、
   モータ回転速度の大きさと設定した閾値を比較してモータの減速を確認するモータ減速判定手段を設け、
   モータ回転方向と衝突トルク方向が逆の軸では、実位置を位置指令に追従させるための電流指令を生成する位置制御手段から、モータ回転と逆方向のトルクをモータで発生させる電流を指令する電流制御手段に切り換えることによりモータ回転速度を減速し、速度が設定値以下になれば、衝突力方向に倣う柔軟制御手段に切り換え、
   モータ回転方向と衝突トルク方向が同じ軸では、位置制御手段から柔軟制御手段に切り換える制御モード切換手段を有することを特徴とするロボットの制御装置。
6. 衝突トルク検出値の大きさと衝突方向判別可能トルク閾値を比較する衝突トルク閾値判定手段を設け、
   衝突トルク検出値の大きさが衝突方向判別可能トルク閾値下回った軸では、モータ回転方向と衝突トルク方向には関わらず、位置制御手段から電流制御手段に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、速度が設定値以下になれば、柔軟制御手段に切り換える制御モード切換手段を有することを特徴とする請求項５記載のロボットの制御装置。
7. モータ回転方向と衝突トルク方向が逆で、モータ回転速度及び衝突トルク検出値の大きさが設定値を上回った軸で、モータ回転と逆方向のトルクをモータで発生させる電流制御を行う場合、モータ回転速度が減速した後、速度方向が反転するまで逆方向のトルクを印可し続け、反転した速度が設定値以上になれば、衝突力方向に倣う柔軟制御に切り換えることを特徴とする請求項５記載のロボットの制御装置。
8. 衝突検出後、制御モード切換手段において電流制御手段を選択する場合、モータ回転と逆方向のモータの最大トルクを発生させる電流を指令する電流制御手段を有することを特徴とする請求項５記載のロボットの制御装置。

Images