JP2006263916A - ロボット制御装置 - Google Patents
ロボット制御装置 Download PDFInfo
- Publication number
- JP2006263916A JP2006263916A JP2006135450A JP2006135450A JP2006263916A JP 2006263916 A JP2006263916 A JP 2006263916A JP 2006135450 A JP2006135450 A JP 2006135450A JP 2006135450 A JP2006135450 A JP 2006135450A JP 2006263916 A JP2006263916 A JP 2006263916A
- Authority
- JP
- Japan
- Prior art keywords
- value
- collision
- command
- torque
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Manipulator (AREA)
Abstract
【課題】計算量を削減し、高精度な衝突判定と過度な反転動作判断を行うロボット制御装置を提供すること。
【解決手段】位置指令、速度指令、加速度指令の少なくとも１つから演算した必要駆動トルク指令要素と各関節を駆動するモータの位置、速度、加速度の少なくとも１つから、改めてロボットの運動方程式を全て再度計算することなしに必要駆動トルクを演算し、演算した必要駆動トルクと各関節を駆動するモータの電流を比較することにより衝突の判別を行う。また、衝突判定時の位置・速度を記憶し、現在の位置・速度と比較し、過度な反転動作と判定された場合にはロボットを緊急停止させる。
【選択図】図３１
【解決手段】位置指令、速度指令、加速度指令の少なくとも１つから演算した必要駆動トルク指令要素と各関節を駆動するモータの位置、速度、加速度の少なくとも１つから、改めてロボットの運動方程式を全て再度計算することなしに必要駆動トルクを演算し、演算した必要駆動トルクと各関節を駆動するモータの電流を比較することにより衝突の判別を行う。また、衝突判定時の位置・速度を記憶し、現在の位置・速度と比較し、過度な反転動作と判定された場合にはロボットを緊急停止させる。
【選択図】図３１
Description
この発明は、モータを用いてロボットを駆動するロボット制御装置、特に付加的なセンサを用いずにロボットが周辺物に衝突したことを検知し、ロボット及び周辺物の破損を防止するロボット制御装置に関するものである。
従来、ロボットが周辺物に衝突したことを検知する方式としては、例えば、特開昭６３−１２７８８９号公報に記載されているように、各関節軸を駆動するモータの電流を検出する電流検出手段と、各関節軸の関節角度から当該モータの必要駆動トルクを計算し、これによりモータの電流制限値を演算する演算手段とを有し、電流検出手段により検出された電流と演算手段により演算された電流制限値とを比較することにより、ロボットが障害物に接触したことを検知する方式が用いられている。
一方、ロボットの手先の軌跡精度を向上するため、位置指令、速度指令、加速度指令より必要となる駆動トルク指令を計算し、トルクフィードフォワードを行う方式も従来より行われてきた。
一方、ロボットの手先の軌跡精度を向上するため、位置指令、速度指令、加速度指令より必要となる駆動トルク指令を計算し、トルクフィードフォワードを行う方式も従来より行われてきた。
また、例えば、特開平１−３１０８８９号公報に記載されているように、電流制限値と実際の電流を比較して衝突を検知するだけでなく、必要駆動トルクに基づいて許容トルクを設定し、モータの出力トルクを当該許容トルク以内に拘束する方式も用いられていた。
従来のロボット制御装置は以上のように構成されているので、軌跡精度を向上させるためにトルクフィードフォワードを行う方式と、付加的なセンサを用いずに各関節軸の関節角度から必要駆動トルクを演算する方式とをそのまま併用すると、指令値から行う駆動トルクの計算と実際の関節角度から行う駆動トルクの計算とを両方行う必要があり、計算量の多い駆動トルクの計算を２度行うことになるため、制御装置内で処理しなければならない計算量が増加するという課題があった。
また、衝突の判定に用いる駆動トルクとして、トルクフィードフォワードの計算結果をそのまま用いることも考えられるが、トルクフィードフォワードの計算結果をそのまま用いたのでは、駆動トルクの推定精度が低く、衝突判定の精度も低下する課題があった。
さらに、ロボットの各関節軸に作用する粘性摩擦、クーロン摩擦の係数は、周辺の温度の等の条件に応じて変動する。従って、必要駆動トルクを演算する際、粘性摩擦係数及びクーロン摩擦係数として一定値を用いると、必要駆動トルクの計算精度が低下し、衝突判定の精度も低下する課題があった。また、ロボットの手先に取り付ける負荷の質量が未知の場合も必要駆動トルクの計算精度が低下し、衝突判定の精度も低下する課題があった。
さらに、ロボットの各軸を駆動するモータの出力を高精度に制限できなかったため、衝突検知後の停止動作中に過大な力がロボット及びロボットと衝突した周辺物に作用する課題があった。
さらに、ロボットの手先に取り付けた負荷の質量、ロボットを動作させる際の加速度、速度、位置等に応じてトルク推定精度は異なる。また、バリ取り等の作業をロボットが行う場合、ハンド開閉を行う際にはロボットが反力を受けるため一定のしきい値で衝突を判別すると最悪の場合に合わせるため衝突判定の精度が低下する、あるいは、反力を受ける作業・動作で誤って衝突を判別してしまう課題があった。
また、衝突の判定に用いる駆動トルクとして、トルクフィードフォワードの計算結果をそのまま用いることも考えられるが、トルクフィードフォワードの計算結果をそのまま用いたのでは、駆動トルクの推定精度が低く、衝突判定の精度も低下する課題があった。
さらに、ロボットの各関節軸に作用する粘性摩擦、クーロン摩擦の係数は、周辺の温度の等の条件に応じて変動する。従って、必要駆動トルクを演算する際、粘性摩擦係数及びクーロン摩擦係数として一定値を用いると、必要駆動トルクの計算精度が低下し、衝突判定の精度も低下する課題があった。また、ロボットの手先に取り付ける負荷の質量が未知の場合も必要駆動トルクの計算精度が低下し、衝突判定の精度も低下する課題があった。
さらに、ロボットの各軸を駆動するモータの出力を高精度に制限できなかったため、衝突検知後の停止動作中に過大な力がロボット及びロボットと衝突した周辺物に作用する課題があった。
さらに、ロボットの手先に取り付けた負荷の質量、ロボットを動作させる際の加速度、速度、位置等に応じてトルク推定精度は異なる。また、バリ取り等の作業をロボットが行う場合、ハンド開閉を行う際にはロボットが反力を受けるため一定のしきい値で衝突を判別すると最悪の場合に合わせるため衝突判定の精度が低下する、あるいは、反力を受ける作業・動作で誤って衝突を判別してしまう課題があった。
さらに、演算した必要駆動トルクに基づいて許容トルクを設定し、モータの出力トルクを許容トルク以内に制限しているため等価的にサーボ剛性が低下し、ロボットが障害物に衝突したときに作用する衝撃力が大きい場合等にはロボットが衝突の反動で戻り過ぎる恐れがあった。また、必要駆動トルクを指令値に基づいて算出している場合、減速停止指令が生成されると、減速停止中の加減速トルクは衝突時の動作方向とは反対方向のトルクとなるため、モータの出力トルクを許容トルク以内に制限する機能により押し付け力を低減する方向に作用するためロボットや衝突した対象物の保護には好ましい。しかし、減速停止指令に基づく加減速トルクが押し付け力以上に大きい場合は、今までの移動方向とは反対方向に加速するトルク指令が与えられることと等価になり、反対方向にロボットが動作し過ぎる恐れがあった。一方、減速停止指令に基づく加減速トルクを押し付け力に近づけることによりロボットや衝突した対象物に作用する力を制限しようとすると、減速度を大きくしなければならないため、停止時の振動が起こりやすい課題もあった。
さらに、このようなロボット制御装置において、電流検出手段により検出された電流と電流制限値演算手段により演算された電流制限値を比較する基準値が大き過ぎると衝突検知感度が低下し、衝突しても検知しない場合があったり、衝突を検知してもロボットやロボットが衝突した対象物に大きな力が作用してしまう。一方、電流検出手段により検出された電流と電流制限値演算手段により演算された電流制限値を比較する基準値が小さ過ぎると、衝突していないのに衝突していると誤って検知し、不必要にロボットを停止させてしまう。そのため、適切な基準値を設定する必要があるが、適切な基準値の設定方法については何ら触れられていなかった。
さらに、このようなロボット制御装置において、ロボット手先が下向きに動作しているときに対象物に衝突すると、衝突を検知して停止するが、ロボットが対象物を押し込んでいる状態で停止する。ロボットが対象物を押し下げている状態で停止すると、そのままでは対象物を除去して衝突状態を解消することができないため、ロボット手先を上方に移動させる必要がある。しかし、ロボットを上昇させようとすると、衝突状態にあるため、再度衝突を検知して停止してしまうため、ロボットを上昇させて衝突状態を解消することができない課題があった。
さらに、ロボットを誤って衝突させてしまって停止した後、作業者がロボットに回避動作を行う際、自動運転中に衝突した場合は手動操作モードに変更し適切な回避動作を行うまで時間が掛かってしまうことがあった。また、作業者が慌てているため間違った方向に回避動作を行ってしまい、さらに、押し込んでしまうことがあるという課題があった。
さらに、対象物と軽く接触する目標地点を教示したい場合や、対象物近傍を教示したい場合、ロボットが衝突を検知する際にいちいち停止してモータ電源遮断されると、モータ電源投入を頻繁に行わなければならず、教示作業に時間が掛るという課題があった。
さらに、衝突検出に使用する必要駆動トルクの演算を、速度０の地点でステップ状に変化するクーロン摩擦モデルを使用して算出すると、実際の摩擦はステップ状には変化しないため、必要駆動トルクの推定精度が速度０付近で悪化し、衝突を誤って検知したり、衝突検知の精度を上げられない課題があった。
さらに、衝突検出の判別値が固定値では、動作状態に応じてきめ細かく判別値を設定することができないため、衝突検出の感度を上げることができない課題があった。
さらに、このようなロボット制御装置において、電流検出手段により検出された電流と電流制限値演算手段により演算された電流制限値を比較する基準値が大き過ぎると衝突検知感度が低下し、衝突しても検知しない場合があったり、衝突を検知してもロボットやロボットが衝突した対象物に大きな力が作用してしまう。一方、電流検出手段により検出された電流と電流制限値演算手段により演算された電流制限値を比較する基準値が小さ過ぎると、衝突していないのに衝突していると誤って検知し、不必要にロボットを停止させてしまう。そのため、適切な基準値を設定する必要があるが、適切な基準値の設定方法については何ら触れられていなかった。
さらに、このようなロボット制御装置において、ロボット手先が下向きに動作しているときに対象物に衝突すると、衝突を検知して停止するが、ロボットが対象物を押し込んでいる状態で停止する。ロボットが対象物を押し下げている状態で停止すると、そのままでは対象物を除去して衝突状態を解消することができないため、ロボット手先を上方に移動させる必要がある。しかし、ロボットを上昇させようとすると、衝突状態にあるため、再度衝突を検知して停止してしまうため、ロボットを上昇させて衝突状態を解消することができない課題があった。
さらに、ロボットを誤って衝突させてしまって停止した後、作業者がロボットに回避動作を行う際、自動運転中に衝突した場合は手動操作モードに変更し適切な回避動作を行うまで時間が掛かってしまうことがあった。また、作業者が慌てているため間違った方向に回避動作を行ってしまい、さらに、押し込んでしまうことがあるという課題があった。
さらに、対象物と軽く接触する目標地点を教示したい場合や、対象物近傍を教示したい場合、ロボットが衝突を検知する際にいちいち停止してモータ電源遮断されると、モータ電源投入を頻繁に行わなければならず、教示作業に時間が掛るという課題があった。
さらに、衝突検出に使用する必要駆動トルクの演算を、速度０の地点でステップ状に変化するクーロン摩擦モデルを使用して算出すると、実際の摩擦はステップ状には変化しないため、必要駆動トルクの推定精度が速度０付近で悪化し、衝突を誤って検知したり、衝突検知の精度を上げられない課題があった。
さらに、衝突検出の判別値が固定値では、動作状態に応じてきめ細かく判別値を設定することができないため、衝突検出の感度を上げることができない課題があった。
この発明は上記のような課題を解消するためになされたもので、計算量を削減し、高精度に衝突判定を行うロボット制御装置を得ることを目的とする。
この発明に係るロボット制御装置は、ロボットの衝突を判別する衝突判別手段と、衝突判別手段による衝突判別時の位置及び速度を記憶する位置速度記憶手段と、位置速度記憶手段に記憶された衝突判別時の位置及び速度と現在の位置とに応じて、少なくとも１つの軸が衝突判別時の動作方向とは逆向きに規定値以上動作した場合にロボットが反転し過ぎていると判別し、ロボットを緊急停止させる過度反転判別手段とを備えたものである。
この発明に係るロボット制御装置は、過度反転判別手段において、位置速度記憶手段に記憶された衝突判別時の速度と現在の速度とに応じて、少なくとも１つの軸の速度が衝突判別時の動作方向とは逆向き、かつ、その大きさが規定値以上である場合にロボットが反転し過ぎていると判別するものである。
この発明によれば、衝突の判別に使用する必要駆動トルクを必要駆動トルク指令要素及びロボットを駆動するモータの位置、速度、加速度から演算するため、軌跡精度を向上させるためのフィードフォワード制御と付加的なセンサを用いずに衝突を検知する方式を両立させる際、計算量を削減し、かつ高精度の衝突の判別が行える効果がある。
この発明によれば、必要駆動トルク指令要素及び位置指令、速度指令、加速度指令から近似的に演算したモータの位置、速度、加速度から衝突の判別に使用する必要駆動トルクを演算するため、軌跡精度を向上させるためのフィードフォワード制御と付加的なセンサを用いずに衝突を検知する方式を両立させる際、計算量を削減し、かつ高精度の衝突の判別が行える効果がある。
この発明によれば、必要駆動トルク指令要素から衝突の判別に使用する必要駆動トルクを演算するため、軌跡精度を向上させるためのフィードフォワード制御と付加的なセンサを用いずに衝突を検知する方式を両立させる際、計算量を削減し、かつ高精度の衝突の判別が行える効果がある。
この発明によれば、摩擦係数等の動作条件に応じて変動するパラメータをロボットの動作中に逐次適に同定するため、高精度で衝突の判別が行える効果がある。
この発明によれば、衝突の判別に使用する必要駆動トルクの演算を行う際、動作中に同定した摩擦係数等のパラメータ推定値の現在値ではなく衝突以前の同定値を用いるため、衝突以前に同定したパラメータ値を用いて衝突の判別に使用する必要駆動トルクの演算を行うため、摩擦係数等のパラメータ推定値が衝突の影響による必要駆動トルク演算値とモータ電流との偏差を減少させるように変動して衝突の判別精度が低下することを防止する効果がある。
この発明によれば、同定精度の高くなることが期待できるデータのみから摩擦係数等のパラメータ同定を行うため、高精度にパラメータ同定が実施でき、衝突の判別の用いる必要駆動トルクを高精度に演算できる効果がある。
この発明によれば、同定精度が低いために、返って衝突の判別精度が低下するのを防止する効果がある。
この発明によれば、比較的演算周期が長くても良いパラメータ同定を長い演算周期で行うため、衝突判別精度を落とすことなく、制御装置における単位時間当たりの計算量を低減できる効果がある。
この発明によれば、ロボットが周辺対象物と衝突していることにより同定するパラメータ値が真値からずれることによる必要駆動トルクの演算精度が低下することを防止できる効果がある。
この発明によれば、柔らかい対象物に衝突する場合と堅い対処物に衝突する場合のいずれにおいても高精度に衝突を判別することができる効果がある。
この発明によれば、衝突検知後の停止時にロボットの各軸駆動用モータが発生するトルクを高精度に制限できるため、ロボット及び衝突した対象物に過大な力が作用するのを防止できる効果がある。
この発明によれば、モータ電源投入時に誤って衝突を検知することを防止する効果がある。
この発明によれば、手先負荷に応じた最適な感度を設定できるため、必要駆動トルクの推定精度が高い手先負荷条件時に必要以上に衝突判別の精度を低下させることを防止する効果がある。
この発明によれば、ロボットのユーザが望む感度で衝突を検知することが可能となる効果がある。
この発明によれば、自動運転時の感度に影響を与えることなく、衝突させてしまう危険が高い教示動作における衝突検知の感度を予め高くしておくことが可能となる効果がある。
この発明によれば、パラメータの同定値が真値から大きくはずれているために必要駆動トルクの演算精度が低下している時に誤って衝突を検知してしまうことを防止しながら、パラメータ同定精度が高いときには高精度に衝突検知が可能となる効果がある。
この発明によれば、ロボットの動作状態に応じてきめ細かく感度を設定できるため、常に高精度に衝突を検知できる効果がある。
この発明によれば、静止摩擦の影響でトルクの推定精度が低下する停止時のみしきい値を大きくすることが可能なため、停止している軸で誤って衝突を検知することを防止しながら、動作中は高精度に衝突を検知できる効果がある。
この発明によれば、外乱力の作用する作業をロボットに行わせる場合のみ自動的にしきい値を大きくすることができるため、外乱力の作用する作業をロボットに行わせる場合に衝突を誤って検知することを防止できる効果がある。
この発明によれば、トルク制限値が急激に変化することによりモータに与えられるトルク指令が急激に変化し、ロボットの挙動が振動的になることを防止する効果がある。
この発明によれば、トルク制限値が振動的に変化し、ロボットの挙度が振動的になることを防止する効果がある。
この発明によれば、ハンドでワークを把持した場合のように手先負荷の質量がステップ状に変化する場合にも必要駆動トルク指令要素及び必要駆動トルクがステップ状に変化し、ロボットの挙動が振動的になることを防止する効果がある。
この発明によれば、衝突検知によりロボットが対象物を押し当てている状態で停止したときに、対象物を除去することなく手動運転でロボットを待避させることが可能となる効果がある。
この発明によれば、電流もしくは加速度信号に加わる観測ノイズの影響で衝突を誤って検知することを防止できる効果がある。
この発明によれば、衝突検知後の待避中に再度衝突を起こさないようにロボットを待避させることが可能になる効果がある。
この発明によれば、過度反転判別手段で過度反転を検出した場合にロボットを緊急停止するため、衝突後にロボットが過度に反転することを防止することができる効果がある。
この発明によれば、過度反転判別手段で過度反転を検出した場合にロボットを緊急停止するため、衝突後にロボットが過度に反転することを防止することができる効果がある。
この発明によれば、衝突判別後の停止中に運動方程式に基づいて各軸のトルク制限値を生成することによって得られる衝突時の押し付け力を低減する能力と、減速停止指令の減速度を別個に指定することにより、減速停止時の振動を防止しながら衝突時の押し付け力を大幅に低減することができる効果がある。
この発明によれば、衝突時にロボットが思わぬ方向へ動作することを防止しながら、衝突後の押し付け力を低減することができる効果がある。
この発明によれば、衝突時にロボットが思わぬ方向へ動作することを防止しながら、衝突後の押し付け力を低減することができる効果がある。
この発明によれば、衝突判別までの期間はしきい値を小さくしておくことにより押し付け力を低減し、衝突判別により停止する際は、しきい値を大きくして、押し付け力低減効果は若干残しながら、ロボットが指令から大きく逸脱しないようにする。また、逆に衝突判別後のしきい値をさらに小さくすることにより、衝突判別により停止する際の押し付け力をさらに低減することも可能となる効果がある。
この発明によれば、実際の動作データ（動作させた結果）に応じてパラメータが設定されるため、適切なパラメータを容易かつ短時間に設定できる効果がある。
この発明によれば、実際の動作データ（動作させた結果）に応じて感度が設定されるため、適切な（可能な限り高い感度を実現し、かつ、誤検知はしない）感度を容易かつ短時間に設定できる効果がある。
この発明によれば、押し付け状態で停止した場合に、回避動作を行わせようとすると再度衝突検知で停止するために、衝突検知機能を解除しないとロボットを動かせない場合がある。そうした場合に、衝突判別機能を速やかに解除し、押し付け状態から手動操作でロボットを回避させることが可能になる効果がある。
この発明によれば、押し付け状態で停止した場合に、衝突を再度検知するためにロボットを回避できない現象を自動的に防止する。また、押し付け状態から逃れると自動的に衝突検知機能が再び有効となるため、再度別のところに衝突した場合のダメージを低減できる効果がある。
この発明によれば、ロボットを衝突以前の地点まで容易に戻すことができる効果がある。
この発明によれば、衝突によりロボットが停止している際、回避動作を行おうとして間違えて、さらに押し込む方向の指令を与えてもロボットが動作せず、さらに押し込むことを防止する。また、警告を発することにより、回避動作を行う方向を間違えたことを操作者は容易に気づくことができる効果がある。
この発明によれば、教示時の軽い接触の場合は、警告が発せられ接触していることが操作者に認識されるが、ロボットが停止したり電源遮断されないため、そのまま教示作業が継続し、接触地点付近の教示を短時間で行える効果がある。
この発明によれば、クーロン摩擦モデルがより正確になるため、運動方程式に基づいて算出したトルクの推定精度が高くなる効果がある。
この発明によれば、しきい値を単純な関数に基づいて設定しながら、しきい値が過大になり、衝突検知感度が低下することを防止することができる効果がある。
この発明によれば、しきい値に加減速方向に応じた重みを付けることにより、誤検知を防止しながら衝突検知の感度を上げることができる効果がある。
この発明によれば、しきい値そのものだけでなく、トルク推定誤差がしきい値以上となる回数の連続値も動作条件に応じて可変にすることにより、誤検知を防止しながら衝突検知の感度を上げることができる効果がある。
以下、この発明の実施の一形態を説明する。
実施の形態１．
図１はこの発明の実施の形態１によるロボット制御装置を示すブロック図であり、図において、１は位置指令、速度指令、加速度指令を生成する指令値生成手段、２は位置指令、速度指令、加速度指令から必要駆動トルク指令要素を演算する必要駆動トルク指令要素演算手段、３は位置、速度、加速度、必要駆動トルク指令要素、速度指令、加速度指令から必要駆動トルクを計算する必要駆動トルク演算手段、４は上側しきい値及び下側しきい値を演算するしきい値演算手段、５は必要駆動トルク、上側しきい値、下側しきい値、モータ駆動電流に基づいて衝突を判別する衝突判別手段、６はロボットの各軸を駆動するモータを制御するモータ制御手段である。
実施の形態１．
図１はこの発明の実施の形態１によるロボット制御装置を示すブロック図であり、図において、１は位置指令、速度指令、加速度指令を生成する指令値生成手段、２は位置指令、速度指令、加速度指令から必要駆動トルク指令要素を演算する必要駆動トルク指令要素演算手段、３は位置、速度、加速度、必要駆動トルク指令要素、速度指令、加速度指令から必要駆動トルクを計算する必要駆動トルク演算手段、４は上側しきい値及び下側しきい値を演算するしきい値演算手段、５は必要駆動トルク、上側しきい値、下側しきい値、モータ駆動電流に基づいて衝突を判別する衝突判別手段、６はロボットの各軸を駆動するモータを制御するモータ制御手段である。
次に動作について説明する。
まず、指令値生成手段１においては、ロボットを自動運転させる際には、ロボットを動作させるためのプログラムにて指示された動作命令を解釈して時々刻々のロボットの各軸の目標位置である位置指令を生成する。手動でロボットを動作させる際には、図示しないロボット操作盤からの入力に応じて時々刻々のロボットの各軸の目標位置である位置指令を生成する。さらに、自動運転時、手動運転時のいずれの場合も、時々刻々生成される位置指令より、速度指令、加速度指令の生成も行う。
まず、指令値生成手段１においては、ロボットを自動運転させる際には、ロボットを動作させるためのプログラムにて指示された動作命令を解釈して時々刻々のロボットの各軸の目標位置である位置指令を生成する。手動でロボットを動作させる際には、図示しないロボット操作盤からの入力に応じて時々刻々のロボットの各軸の目標位置である位置指令を生成する。さらに、自動運転時、手動運転時のいずれの場合も、時々刻々生成される位置指令より、速度指令、加速度指令の生成も行う。
必要駆動トルク指令要素演算手段２では、指令値生成手段１で生成された位置指令、速度指令、加速度指令を入力して、ロボットの運動方程式を用いて必要駆動トルク指令の各要素を演算する。ロボットの運動方程式は、各軸の駆動トルクから構成される駆動トルクベクトルをτ、各軸の位置から構成される位置ベクトルをｑ、各軸の速度から構成される速度ベクトルをｖ、各軸の加速度から構成される加速度ベクトルをａとすると、
τ＝Ｍ（ｑ）ａ＋ｈ（ｑ，ｖ）＋ｇ（ｑ）＋ｆ（ｖ）・・・（１）
と書ける。ここで、Ｍ（ｑ）は慣性行列、ｈ（ｑ，ｖ）は各軸の遠心コリオリ力から構成される遠心コリオリ力ベクトル、ｇ（ｑ）は各軸の重力から構成される重力ベクトル、ｆ（ｖ）は各軸の摩擦力から構成される摩擦力ベクトルである。指令値生成手段１から出力された各指令を用いて各軸の位置指令から構成される位置指令ベクトルをｑd、各軸の速度指令から構成される速度指令ベクトルをｖd、各軸の加速度指令から構成される加速度指令ベクトルをａdとし、（１）式に基づいて、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfdをそれぞれ下記のようにして算出する。
τmd＝Ｍ（ｑd）ａd ・・・（２）
τhd＝ｈ（ｑd，ｖd） ・・・（３）
τgd＝ｇ（ｑd） ・・・（４）
τfd＝ｆ（ｖd） ・・・（５）
τ＝Ｍ（ｑ）ａ＋ｈ（ｑ，ｖ）＋ｇ（ｑ）＋ｆ（ｖ）・・・（１）
と書ける。ここで、Ｍ（ｑ）は慣性行列、ｈ（ｑ，ｖ）は各軸の遠心コリオリ力から構成される遠心コリオリ力ベクトル、ｇ（ｑ）は各軸の重力から構成される重力ベクトル、ｆ（ｖ）は各軸の摩擦力から構成される摩擦力ベクトルである。指令値生成手段１から出力された各指令を用いて各軸の位置指令から構成される位置指令ベクトルをｑd、各軸の速度指令から構成される速度指令ベクトルをｖd、各軸の加速度指令から構成される加速度指令ベクトルをａdとし、（１）式に基づいて、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfdをそれぞれ下記のようにして算出する。
τmd＝Ｍ（ｑd）ａd ・・・（２）
τhd＝ｈ（ｑd，ｖd） ・・・（３）
τgd＝ｇ（ｑd） ・・・（４）
τfd＝ｆ（ｖd） ・・・（５）
必要駆動トルク演算手段３では、本来なら（１）式に基づいて駆動トルクτを計算したいところである。しかしロボットの軸数が多い場合（１）式の計算量は多く、制御装置において単位時間あたりに処理しなければならない計算量が増加してしまう。そこでまず、モータ制御手段６から実際の各軸の速度をモニターして各軸の速度から構成される速度ベクトルｖと前回モニターした速度ベクトルｖpから各軸の加速度を求め、当該各軸の加速度から構成される加速度ベクトルａを演算する。次に、必要駆動トルク指令要素演算手段２で求めた速度指令ベクトルｖd、加速度指令ベクトルａd、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfd及び速度ベクトルｖ、加速度ベクトルａから下記のようにして各軸の必要駆動トルクから構成される必要駆動トルクベクトルτeを計算する。
Ｉmd＝τmd／ｎ（ａd） ・・・（６）
τme＝ｎ（ａ）＊Ｉmd ・・・（７）
Ｉhd＝τhd／（ｎ（ｖd））２ ・・・（８）
τhe＝（ｎ（ｖ））２＊Ｉhd ・・・（９）
τe＝τme＋τhe＋τgd＋τfd ・・・（１０）
（６）式で演算されるＩmd及び（８）式で演算されるＩhdは、前回の演算結果を記憶しておき、ｎ（ａd）もしくは（ｎ（ｖd））２が予め定められている値以下となる場合は、（６）式もしくは（８）式の演算は行わず、前回のＩmdもしくはＩhdの値を用いて（７）式もしくは（９）式の演算を行う。なお、ｎ（）はベクトルノルムを示している。
Ｉmd＝τmd／ｎ（ａd） ・・・（６）
τme＝ｎ（ａ）＊Ｉmd ・・・（７）
Ｉhd＝τhd／（ｎ（ｖd））２ ・・・（８）
τhe＝（ｎ（ｖ））２＊Ｉhd ・・・（９）
τe＝τme＋τhe＋τgd＋τfd ・・・（１０）
（６）式で演算されるＩmd及び（８）式で演算されるＩhdは、前回の演算結果を記憶しておき、ｎ（ａd）もしくは（ｎ（ｖd））２が予め定められている値以下となる場合は、（６）式もしくは（８）式の演算は行わず、前回のＩmdもしくはＩhdの値を用いて（７）式もしくは（９）式の演算を行う。なお、ｎ（）はベクトルノルムを示している。
しきい値演算手段４では、必要駆動トルクとモータ電流を比較し衝突を検知する基準となるしきい値を算出する。この実施の形態１では各軸毎に予め記憶されている上側しきい値及び下側しきい値を設定する。
衝突判別手段５では、まず、モータ制御手段６から入力されるモータ電流から各軸のモータ駆動トルクを演算する。次に、必要駆動トルク演算手段３で演算した必要駆動トルクを入力して実際のモータ駆動トルクとの差を各軸ごとについて計算する。いずれかの軸における必要駆動トルクとモータ駆動トルクの差がしきい値演算手段４から出力された上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成手段１に停止命令を送信する。
モータ制御手段６では、指令値生成手段１で生成された位置指令、速度指令、加速度指令、及び必要駆動トルク指令要素演算手段２で演算されたトルク指令要素を用いてロボットを駆動するモータの制御を行う。
実施の形態２．
この発明の実施の形態２によるロボット制御装置を示すブロック図は図１であり、必要駆動トルク演算手段３以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
この発明の実施の形態２によるロボット制御装置を示すブロック図は図１であり、必要駆動トルク演算手段３以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
次に動作について説明する。
必要駆動トルク演算手段３では、まず、モータ制御手段６から入力される各軸の速度から構成される速度ベクトルｖと前回の速度ベクトルｖpから各軸の加速度を求め、当該各軸の加速度から構成される加速度ベクトルａを演算する。次に必要駆動トルク指令要素演算手段２から出力される各軸の速度指令から構成される速度指令ベクトルｖd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfd及びモータ制御手段６から出力される各軸位置ベクトルｑ、各軸速度ベクトルｖ、各軸加速度ベクトルａを入力して以下のようにして各軸の必要駆動トルクから構成される必要駆動トルクベクトルτeを計算する。
τme＝Ｍ（ｑ）ａ ・・・（１１）
Ｉhd＝τhd／（ｎ（ｖd））2 ・・・（１２）
τhe＝（ｎ（ｖ））2＊Ｉhd ・・・（１３）
τe＝τme＋τhe＋τgd＋ｆ（ｖ） ・・・（１４）
（１２）式で演算されるＩhdは、前回の演算結果を記憶しておき、（ｎ（ｖd））2が予め定められている値以下となる場合は、（１２）式の演算は行わず、前回のＩhdの値を用いて（７）式もしくは（１３）式の演算を行う。
必要駆動トルク演算手段３では、まず、モータ制御手段６から入力される各軸の速度から構成される速度ベクトルｖと前回の速度ベクトルｖpから各軸の加速度を求め、当該各軸の加速度から構成される加速度ベクトルａを演算する。次に必要駆動トルク指令要素演算手段２から出力される各軸の速度指令から構成される速度指令ベクトルｖd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfd及びモータ制御手段６から出力される各軸位置ベクトルｑ、各軸速度ベクトルｖ、各軸加速度ベクトルａを入力して以下のようにして各軸の必要駆動トルクから構成される必要駆動トルクベクトルτeを計算する。
τme＝Ｍ（ｑ）ａ ・・・（１１）
Ｉhd＝τhd／（ｎ（ｖd））2 ・・・（１２）
τhe＝（ｎ（ｖ））2＊Ｉhd ・・・（１３）
τe＝τme＋τhe＋τgd＋ｆ（ｖ） ・・・（１４）
（１２）式で演算されるＩhdは、前回の演算結果を記憶しておき、（ｎ（ｖd））2が予め定められている値以下となる場合は、（１２）式の演算は行わず、前回のＩhdの値を用いて（７）式もしくは（１３）式の演算を行う。
実施の形態３．
図２はこの発明の実施の形態３によるロボット制御装置を示すブロック図であり、図において、８は実施の形態１，２の衝突判別手段５に換えて設けられた加算手段、９は衝突判別手段、１０はモータ制御手段である。
その他の構成については図１と同一のため、説明は省略する。
図２はこの発明の実施の形態３によるロボット制御装置を示すブロック図であり、図において、８は実施の形態１，２の衝突判別手段５に換えて設けられた加算手段、９は衝突判別手段、１０はモータ制御手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
加算手段８では、必要駆動トルク演算手段３で演算した必要駆動トルクの各軸要素にしきい値演算手段４で演算した上側しきい値の各軸要素を加算し、各軸毎の上側トルク許容値を算出する。次に必要駆動トルクの各軸要素にしきい値演算手段４で演算した下側しきい値の各軸要素を加算し、各軸毎の下側トルク許容値を算出する。
加算手段８では、必要駆動トルク演算手段３で演算した必要駆動トルクの各軸要素にしきい値演算手段４で演算した上側しきい値の各軸要素を加算し、各軸毎の上側トルク許容値を算出する。次に必要駆動トルクの各軸要素にしきい値演算手段４で演算した下側しきい値の各軸要素を加算し、各軸毎の下側トルク許容値を算出する。
衝突判別手段９では、モータ電流から算出した実駆動トルクと上側トルク許容値を各軸毎に比較し、実駆動トルクが上側トルク許容値以上の場合はロボットが衝突したと判別する。また、実駆動トルクが下側許容値以下となった場合もロボットが衝突したと判別する。衝突を判別したときは、衝突判別手段９はモータ制御手段１０に停止命令を送信するとともに、指令値生成手段１にも衝突検知信号を出力する。
モータ制御手段１０は、通常状態では、衝突判別手段９を内蔵していること以外は、図１に記載されているモータ制御手段６の作用と同一である。衝突検知後は、衝突判別手段９から停止命令を受けると、モータ制御手段１０内部で停止するための位置指令を生成し、生成した位置指令に沿って各軸の位置制御を行う。
実施の形態４．
図３はこの発明の実施の形態４によるロボット制御装置を示すブロック図であり、図において、１１は位置速度加速度推定手段、１２は必要駆動トルク演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図３はこの発明の実施の形態４によるロボット制御装置を示すブロック図であり、図において、１１は位置速度加速度推定手段、１２は必要駆動トルク演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
位置速度加速度推定手段１１の内部に位置指令とモータ位置の関係を模擬した１次遅れフィルタ手段を設ける。指令値生成手段１で生成された位置指令、速度指令、加速度指令の各軸要素をそれぞれ１次遅れフィルタ手段に入力し、その１次遅れフィルタ手段からの出力をそれぞれ推定位置、推定速度、推定加速度とする。
必要駆動トルク演算手段１２では、各軸の速度指令から構成される速度指令ベクトルｖd、各軸の加速度指令から構成される加速度指令ベクトルａd、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfd及び位置速度加速度推定手段１１で推定された推定速度ベクトルｖ、推定加速度ベクトルａから下記のようにして各軸の必要駆動トルクから構成される必要駆動トルクベクトルτeを計算する。
Ｉmd＝τmd／ｎ（ａd） ・・・（１５）
τme＝ｎ（ａ）＊Ｉmd ・・・（１６）
Ｉhd＝τhd／（ｎ（ｖd））2 ・・・（１７）
τhd＝（ｎ（ｖ））2＊Ｉhd ・・・（１８）
τe＝τme＋τhe＋τgd＋τfd ・・・（１９）
（１５）式で演算されるＩmd及び（１７）式で演算されるＩhdは、前回の演算結果を記憶しておき、ｎ（ａd）もしくは（ｎ（ｖd））2が予め定められている値以下となる場合は、（１５）式もしくは（１７）式の演算は行わず、前回のＩmdもしくはＩhdの値を用いて（１６）式もしくは（１８）式の演算を行う。
位置速度加速度推定手段１１の内部に位置指令とモータ位置の関係を模擬した１次遅れフィルタ手段を設ける。指令値生成手段１で生成された位置指令、速度指令、加速度指令の各軸要素をそれぞれ１次遅れフィルタ手段に入力し、その１次遅れフィルタ手段からの出力をそれぞれ推定位置、推定速度、推定加速度とする。
必要駆動トルク演算手段１２では、各軸の速度指令から構成される速度指令ベクトルｖd、各軸の加速度指令から構成される加速度指令ベクトルａd、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfd及び位置速度加速度推定手段１１で推定された推定速度ベクトルｖ、推定加速度ベクトルａから下記のようにして各軸の必要駆動トルクから構成される必要駆動トルクベクトルτeを計算する。
Ｉmd＝τmd／ｎ（ａd） ・・・（１５）
τme＝ｎ（ａ）＊Ｉmd ・・・（１６）
Ｉhd＝τhd／（ｎ（ｖd））2 ・・・（１７）
τhd＝（ｎ（ｖ））2＊Ｉhd ・・・（１８）
τe＝τme＋τhe＋τgd＋τfd ・・・（１９）
（１５）式で演算されるＩmd及び（１７）式で演算されるＩhdは、前回の演算結果を記憶しておき、ｎ（ａd）もしくは（ｎ（ｖd））2が予め定められている値以下となる場合は、（１５）式もしくは（１７）式の演算は行わず、前回のＩmdもしくはＩhdの値を用いて（１６）式もしくは（１８）式の演算を行う。
実施の形態５．
この発明の実施の形態５によるロボット制御装置を示すブロック図は図１であり、必要駆動トルク演算手段３以外の各手段の動作は、実施の形態１と同様なため、説明は省略する。
この発明の実施の形態５によるロボット制御装置を示すブロック図は図１であり、必要駆動トルク演算手段３以外の各手段の動作は、実施の形態１と同様なため、説明は省略する。
次に動作について説明する。
必要駆動トルク演算手段３の内部には、フィルタ１、フィルタ２、フィルタ３、フィルタ４の４個の１次遅れフィルタ手段を設け、それら４個の１次遅れフィルタ手段の時定数は独立に設定可能とする。必要駆動トルク指令要素演算手段２で算出された、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfdをそれぞれフィルタ１、フィルタ２、フィルタ３、フィルタ４に入力し、フィルタからの出力をそれぞれ慣性力指令要素ベクトルτme、遠心コリオリ力指令要素ベクトルτhe、重力指令要素ベクトルτge、摩擦力指令要素ベクトルτfeとし、必要駆動トルクτeを
τe＝τme＋τhe＋τge＋τfe ・・・（２０）
で算出する。
必要駆動トルク演算手段３の内部には、フィルタ１、フィルタ２、フィルタ３、フィルタ４の４個の１次遅れフィルタ手段を設け、それら４個の１次遅れフィルタ手段の時定数は独立に設定可能とする。必要駆動トルク指令要素演算手段２で算出された、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfdをそれぞれフィルタ１、フィルタ２、フィルタ３、フィルタ４に入力し、フィルタからの出力をそれぞれ慣性力指令要素ベクトルτme、遠心コリオリ力指令要素ベクトルτhe、重力指令要素ベクトルτge、摩擦力指令要素ベクトルτfeとし、必要駆動トルクτeを
τe＝τme＋τhe＋τge＋τfe ・・・（２０）
で算出する。
実施の形態６．
図４はこの発明の実施の形態６によるロボット制御装置を示すブロック図であり、図において、指令値生成手段１、しきい値演算手段４、衝突判別手段５、モータ制御手段６は図１に記載のものと全く同一である。必要駆動トルク指令要素演算手段２の構成・作用も図１に記載の必要駆動トルク指令要素演算手段と全く同一であるが、各要素の演算を行う際、パラメータ推定手段１４で推定したパラメータを用いる点が異なっている。必要駆動トルク演算手段３の構成・作用も実施の形態２に記載のものと全く同一であるが、
τp＝τme＋τhe＋τgd ・・・（２１）
で算出されるτpをパラメータ推定手段１４に出力する点が異なっている。
図４はこの発明の実施の形態６によるロボット制御装置を示すブロック図であり、図において、指令値生成手段１、しきい値演算手段４、衝突判別手段５、モータ制御手段６は図１に記載のものと全く同一である。必要駆動トルク指令要素演算手段２の構成・作用も図１に記載の必要駆動トルク指令要素演算手段と全く同一であるが、各要素の演算を行う際、パラメータ推定手段１４で推定したパラメータを用いる点が異なっている。必要駆動トルク演算手段３の構成・作用も実施の形態２に記載のものと全く同一であるが、
τp＝τme＋τhe＋τgd ・・・（２１）
で算出されるτpをパラメータ推定手段１４に出力する点が異なっている。
次に動作について説明する。
パラメータ推定手段１４では、各軸の粘性摩擦係数とクーロン摩擦係数をオンラインで同定する。まず、モータ駆動電流から実駆動トルクτrを算出する。次に第ｉ番目の軸の速度、実駆動トルク、必要駆動トルク演算手段３から入力される摩擦力以外の推定トルクの和τp、をそれぞれｖi、τri、τpiとし、ベクトルｙi、パラメータベクトルｐiをそれぞれ
ｙi＝［ｖi，ｓｇｎ（ｖi）］ ・・・（２２）
ｐi＝［ｐni，ｐci］ ・・・（２３）
と定義する。ここで、ｐni、ｐciはそれぞれ第ｉ番目の軸の粘性摩擦係数同定値、クーロン摩擦係数同定値である。また、ｓｇｎ（）は入力が負の時−１、０の時０、正の時１を出力する関数である。第ｋ回目の同定周期における値を［ｋ］と表記することにすると、第ｋ回目の同定周期における第ｉ軸の摩擦係数同定値ベクトルｐi［ｋ］は
τmi［ｋ］＝τri［ｋ］−τpi［ｋ］ ・・・（２４）
Ｒi［ｋ］＝Ｒi［ｋ−１］＋ｍｏｉｔ
＊（−σi＊Ｒi［ｋ−１］＋ｙi［ｋ］Tｙi［ｋ］）
・・・（２５）
ｒi［ｋ］＝ｒi［ｋ−１］＋ｍｏｉｔ
＊（−σi＊ｑi［ｋ−１］＋τmi［ｋ］＊ｙi［ｋ］T）
・・・（２６）
ｐi［ｋ］＝ｐi［ｋ−１］−ｍｏｉｔ
＊Ｇi・［Ｒi［ｋ］・ｐi［ｋ−１］−ｒi［ｋ］）
・・・（２７）
により、逐次同定されていく。ここで、Ｇi及びσiは、それぞれ、同定の早さを調整するためのゲイン行列、重み係数である。また、ｍｏｉｔはパラメータ同定演算を行う周期である。
パラメータ推定手段１４は、前記のように同定した各軸の摩擦係数を必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に送信する。必要駆動トルク指令要素演算手段２では、各軸の摩擦力指令要素ベクトルτfdの計算を、パラメータ推定手段１４から送信された摩擦係数を用いて実施する。
パラメータ推定手段１４では、各軸の粘性摩擦係数とクーロン摩擦係数をオンラインで同定する。まず、モータ駆動電流から実駆動トルクτrを算出する。次に第ｉ番目の軸の速度、実駆動トルク、必要駆動トルク演算手段３から入力される摩擦力以外の推定トルクの和τp、をそれぞれｖi、τri、τpiとし、ベクトルｙi、パラメータベクトルｐiをそれぞれ
ｙi＝［ｖi，ｓｇｎ（ｖi）］ ・・・（２２）
ｐi＝［ｐni，ｐci］ ・・・（２３）
と定義する。ここで、ｐni、ｐciはそれぞれ第ｉ番目の軸の粘性摩擦係数同定値、クーロン摩擦係数同定値である。また、ｓｇｎ（）は入力が負の時−１、０の時０、正の時１を出力する関数である。第ｋ回目の同定周期における値を［ｋ］と表記することにすると、第ｋ回目の同定周期における第ｉ軸の摩擦係数同定値ベクトルｐi［ｋ］は
τmi［ｋ］＝τri［ｋ］−τpi［ｋ］ ・・・（２４）
Ｒi［ｋ］＝Ｒi［ｋ−１］＋ｍｏｉｔ
＊（−σi＊Ｒi［ｋ−１］＋ｙi［ｋ］Tｙi［ｋ］）
・・・（２５）
ｒi［ｋ］＝ｒi［ｋ−１］＋ｍｏｉｔ
＊（−σi＊ｑi［ｋ−１］＋τmi［ｋ］＊ｙi［ｋ］T）
・・・（２６）
ｐi［ｋ］＝ｐi［ｋ−１］−ｍｏｉｔ
＊Ｇi・［Ｒi［ｋ］・ｐi［ｋ−１］−ｒi［ｋ］）
・・・（２７）
により、逐次同定されていく。ここで、Ｇi及びσiは、それぞれ、同定の早さを調整するためのゲイン行列、重み係数である。また、ｍｏｉｔはパラメータ同定演算を行う周期である。
パラメータ推定手段１４は、前記のように同定した各軸の摩擦係数を必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に送信する。必要駆動トルク指令要素演算手段２では、各軸の摩擦力指令要素ベクトルτfdの計算を、パラメータ推定手段１４から送信された摩擦係数を用いて実施する。
実施の形態７．
図５はこの発明の実施の形態７によるロボット制御装置を示すブロック図であり、図において、１５は必要駆動トルク演算手段、１６はパラメータ推定手段、１７はモータ制御手段、１８は指令値生成手段である。
その他の構成については図１と同一のため、説明は省略する。
図５はこの発明の実施の形態７によるロボット制御装置を示すブロック図であり、図において、１５は必要駆動トルク演算手段、１６はパラメータ推定手段、１７はモータ制御手段、１８は指令値生成手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
指令値生成手段１８では、ロボットを自動運転させる際には、ロボットを動作させるためのプログラムにて指示された動作命令を解釈して時々刻々のロボットの各軸の目標位置である位置指令を生成する。手動でロボットを動作させる際には、図示しないロボット操作盤（操作手段）からの入力に応じて時々刻々のロボットの各軸の目標位置である位置指令を生成する。さらに、自動運転時、手動運転時のいずれの場合も、時々刻々生成される位置指令より、速度指令の生成も行う。
必要駆動トルク演算手段１５では、パラメータ推定手段１６に出力する摩擦力以外の推定トルクの和τp、及び衝突判別手段５に出力する必要駆動トルクτeを下記のように算出する。まず、各軸の速度から構成される速度ベクトルｖと前回の速度ベクトルｖｐから各軸の加速度から構成される加速度ベクトルａを演算する。次に各軸の位置から構成される位置ベクトルｑ、速度ベクトルｖ、加速度ベクトルａから
τp＝Ｍ（ｑ）ａ＋ｈ（ｑ，ｖ）＋ｇ（ｑ） ・・・（２８）
により、摩擦力以外の推定トルクの和τpを算出し、
τf＝ｆ（ｖ） ・・・（２９）
により摩擦力を算出する。衝突判別手段５に出力する必要駆動トルクτeはτpとτfの和とする。
パラメータ推定手段１６では、摩擦力以外の推定トルクの和τp、モータ駆動電流、各軸の速度から実施の形態６におけるパラメータ推定手段１４と全く同様に各軸の摩擦係数を推定する。推定した摩擦係数は必要駆動トルク演算手段１５内部で摩擦力の計算（２９）式を行う際に使用される。
モータ制御手段１７では、指令値生成手段１８で生成された位置指令、速度指令を用いてロボットを駆動するモータの位置制御を行う。
指令値生成手段１８では、ロボットを自動運転させる際には、ロボットを動作させるためのプログラムにて指示された動作命令を解釈して時々刻々のロボットの各軸の目標位置である位置指令を生成する。手動でロボットを動作させる際には、図示しないロボット操作盤（操作手段）からの入力に応じて時々刻々のロボットの各軸の目標位置である位置指令を生成する。さらに、自動運転時、手動運転時のいずれの場合も、時々刻々生成される位置指令より、速度指令の生成も行う。
必要駆動トルク演算手段１５では、パラメータ推定手段１６に出力する摩擦力以外の推定トルクの和τp、及び衝突判別手段５に出力する必要駆動トルクτeを下記のように算出する。まず、各軸の速度から構成される速度ベクトルｖと前回の速度ベクトルｖｐから各軸の加速度から構成される加速度ベクトルａを演算する。次に各軸の位置から構成される位置ベクトルｑ、速度ベクトルｖ、加速度ベクトルａから
τp＝Ｍ（ｑ）ａ＋ｈ（ｑ，ｖ）＋ｇ（ｑ） ・・・（２８）
により、摩擦力以外の推定トルクの和τpを算出し、
τf＝ｆ（ｖ） ・・・（２９）
により摩擦力を算出する。衝突判別手段５に出力する必要駆動トルクτeはτpとτfの和とする。
パラメータ推定手段１６では、摩擦力以外の推定トルクの和τp、モータ駆動電流、各軸の速度から実施の形態６におけるパラメータ推定手段１４と全く同様に各軸の摩擦係数を推定する。推定した摩擦係数は必要駆動トルク演算手段１５内部で摩擦力の計算（２９）式を行う際に使用される。
モータ制御手段１７では、指令値生成手段１８で生成された位置指令、速度指令を用いてロボットを駆動するモータの位置制御を行う。
実施の形態８．
図６はこの発明の実施の形態８によるロボット制御装置を示すブロック図であり、図において、１９はパラメータ蓄積手段である。
その他の構成については図４と同一のため、説明は省略する。
図６はこの発明の実施の形態８によるロボット制御装置を示すブロック図であり、図において、１９はパラメータ蓄積手段である。
その他の構成については図４と同一のため、説明は省略する。
次に動作について説明する。
パラメータ蓄積手段１９では、パラメータ推定手段１４で推定されたパラメータ値を指定された周期分だけ記憶しておく。例えば、１０周期分のパラメータを蓄積しておくことが指定されている場合、１０周期分のパラメータを蓄積する領域を確保し、パラメータの初期値を確保した領域に書き込んでおく。パラメータ同定周期毎に１０周期前のパラメータを必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に出力し、残りの9周期分のパラメータの記憶領域を1周期ずつシフトさせる。また、パラメータ推定手段１４から入力された最新のパラメータ同定周期におけるパラメータ同定値を最新周期のパラメータを記憶しておくエリアに書き込む。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、パラメータ蓄積手段１９から出力されたパラメータを用いて運動方程式の各要素の計算を行う。
パラメータ蓄積手段１９では、パラメータ推定手段１４で推定されたパラメータ値を指定された周期分だけ記憶しておく。例えば、１０周期分のパラメータを蓄積しておくことが指定されている場合、１０周期分のパラメータを蓄積する領域を確保し、パラメータの初期値を確保した領域に書き込んでおく。パラメータ同定周期毎に１０周期前のパラメータを必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に出力し、残りの9周期分のパラメータの記憶領域を1周期ずつシフトさせる。また、パラメータ推定手段１４から入力された最新のパラメータ同定周期におけるパラメータ同定値を最新周期のパラメータを記憶しておくエリアに書き込む。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、パラメータ蓄積手段１９から出力されたパラメータを用いて運動方程式の各要素の計算を行う。
実施の形態９．
図７はこの発明の実施の形態９によるロボット制御装置を示すブロック図であり、図において、２０は同定ゲイン演算手段である。
その他の構成については図６と同一のため、説明は省略する。
図７はこの発明の実施の形態９によるロボット制御装置を示すブロック図であり、図において、２０は同定ゲイン演算手段である。
その他の構成については図６と同一のため、説明は省略する。
次に動作について説明する。
同定ゲイン演算手段２０では、（２７）式に記載されている同定の早さを調整するための行列Ｇiの値を決定する。まず、各軸の速度がモータ制御手段６から入力される。入力された速度信号の絶対値を求め、予め記憶されている基準値と比較する。第ｉ軸の速度絶対値が基準値以上の場合は、予め記憶されている行列Ｇiの値を出力する。第ｉ軸の速度絶対値が基準値以下の場合は、行列Ｇiの各要素の値を０として出力する。パラメータ推定手段１４では同定ゲイン演算手段２０から出力されるＧiに基づいてパラメータ推定値の更新を行うため、ある軸の速度絶対値が予め設定されている基準値以下の場合は、当該軸のパラメータ推定値は前回の周期の推定値と全く同一となりパラメータ推定が行われない。
同定ゲイン演算手段２０では、（２７）式に記載されている同定の早さを調整するための行列Ｇiの値を決定する。まず、各軸の速度がモータ制御手段６から入力される。入力された速度信号の絶対値を求め、予め記憶されている基準値と比較する。第ｉ軸の速度絶対値が基準値以上の場合は、予め記憶されている行列Ｇiの値を出力する。第ｉ軸の速度絶対値が基準値以下の場合は、行列Ｇiの各要素の値を０として出力する。パラメータ推定手段１４では同定ゲイン演算手段２０から出力されるＧiに基づいてパラメータ推定値の更新を行うため、ある軸の速度絶対値が予め設定されている基準値以下の場合は、当該軸のパラメータ推定値は前回の周期の推定値と全く同一となりパラメータ推定が行われない。
実施の形態１０．
図８はこの発明の実施の形態１０によるロボット制御装置を示すブロック図であり、図において、２１はパラメータ推定値制限手段である。
その他の構成については図７と同一のため、説明は省略する。
図８はこの発明の実施の形態１０によるロボット制御装置を示すブロック図であり、図において、２１はパラメータ推定値制限手段である。
その他の構成については図７と同一のため、説明は省略する。
次に動作について説明する。
パラメータ推定値制限手段２１には、予め各パラメータの上限値及び下限値を記憶しておく。パラメータ蓄積手段１９から出力されたパラメータが上限値以上の場合は上限値に書き換える。パラメータ蓄積手段１９から出力されたパラメータが下限値以下の場合は下限値に書き換える。パラメータ蓄積手段１９から出力されたパラメータが下限値以上でかつ上限値以下のときはパラメータは書き換えない。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、パラメータ推定値制限手段２１で必要に応じて書き換えられたパラメータ値を用いて演算を行う。
パラメータ推定値制限手段２１には、予め各パラメータの上限値及び下限値を記憶しておく。パラメータ蓄積手段１９から出力されたパラメータが上限値以上の場合は上限値に書き換える。パラメータ蓄積手段１９から出力されたパラメータが下限値以下の場合は下限値に書き換える。パラメータ蓄積手段１９から出力されたパラメータが下限値以上でかつ上限値以下のときはパラメータは書き換えない。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、パラメータ推定値制限手段２１で必要に応じて書き換えられたパラメータ値を用いて演算を行う。
実施の形態１１．
この発明の実施の形態１１によるロボット制御装置を示すブロック図も図８である。
この発明の実施の形態１１によるロボット制御装置を示すブロック図も図８である。
次に動作について説明する。
図８に示す各ブロックのうち、モータ制御手段６はｘ［ｍｓ］の周期で演算を行い、１〜５の各構成要素はｘ［ｍｓ］のｎ倍の周期で演算を行い、１４，１９，２０，２１の各構成要素はｘ［ｍｓ］のｎ×ｍ倍の周期で演算を行う。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、パラメータ推定値制限手段２１からパラメータ推定値が入力される周期では入力されたパラメータに基づいて演算する。パラメータ推定値制限手段２１からパラメータ推定値が入力されない周期では前回以前に入力されたパラメータを用いて演算を行う。
図８に示す各ブロックのうち、モータ制御手段６はｘ［ｍｓ］の周期で演算を行い、１〜５の各構成要素はｘ［ｍｓ］のｎ倍の周期で演算を行い、１４，１９，２０，２１の各構成要素はｘ［ｍｓ］のｎ×ｍ倍の周期で演算を行う。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、パラメータ推定値制限手段２１からパラメータ推定値が入力される周期では入力されたパラメータに基づいて演算する。パラメータ推定値制限手段２１からパラメータ推定値が入力されない周期では前回以前に入力されたパラメータを用いて演算を行う。
実施の形態１２．
図９はこの発明の実施の形態１２によるロボット制御装置を示すブロック図であり、図において、２５は同定ゲイン演算手段である。
その他の構成については図８と同一のため、説明は省略する。
図９はこの発明の実施の形態１２によるロボット制御装置を示すブロック図であり、図において、２５は同定ゲイン演算手段である。
その他の構成については図８と同一のため、説明は省略する。
次に動作について説明する。
同定ゲイン演算手段２５の内部にパラメータ同定実行決定パラメータを備えている。パラメータ同定実行決定パラメータは、電源投入直後の初期状態では１の値を持っている。衝突判別手段５が衝突を検知すると、衝突検知信号が同定ゲイン演算手段２５に送信され、同定ゲイン演算手段２５は、衝突検知信号を受け取るとパラメータ同定実行決定パラメータの値を０とする。パラメータ同定実行決定パラメータ値は、電源遮断及び電源再投入が行われるか、図示しない制御装置の操作盤からパラメータ同定再開が指定されるまでの間は０を保持する。
同定ゲイン演算手段２５の内部では、パラメータ同定実行決定パラメータの値に応じて、パラメータ同定ゲイン行列Ｇiの各要素の値が選択される。パラメータ同定実行決定パラメータの値が１のときは各軸の同定ゲイン行列Ｇiの各要素の値として予め記憶されている値が選択される。パラメータ同定実行決定パラメータの値が０のときは各軸の同定ゲイン行列Ｇiの各要素の値として０が選択される。パラメータ推定手段１４の内部では、同定ゲイン演算手段２５で演算されたＧiに基づいてパラメータ推定値が更新されるため、衝突検知後電源遮断及び電源再投入が行われるか、制御装置の操作盤からパラメータ同定再開が指定されまでの間はパラメータ同定値は変化せず、パラメータ同定が停止される。
同定ゲイン演算手段２５の内部にパラメータ同定実行決定パラメータを備えている。パラメータ同定実行決定パラメータは、電源投入直後の初期状態では１の値を持っている。衝突判別手段５が衝突を検知すると、衝突検知信号が同定ゲイン演算手段２５に送信され、同定ゲイン演算手段２５は、衝突検知信号を受け取るとパラメータ同定実行決定パラメータの値を０とする。パラメータ同定実行決定パラメータ値は、電源遮断及び電源再投入が行われるか、図示しない制御装置の操作盤からパラメータ同定再開が指定されるまでの間は０を保持する。
同定ゲイン演算手段２５の内部では、パラメータ同定実行決定パラメータの値に応じて、パラメータ同定ゲイン行列Ｇiの各要素の値が選択される。パラメータ同定実行決定パラメータの値が１のときは各軸の同定ゲイン行列Ｇiの各要素の値として予め記憶されている値が選択される。パラメータ同定実行決定パラメータの値が０のときは各軸の同定ゲイン行列Ｇiの各要素の値として０が選択される。パラメータ推定手段１４の内部では、同定ゲイン演算手段２５で演算されたＧiに基づいてパラメータ推定値が更新されるため、衝突検知後電源遮断及び電源再投入が行われるか、制御装置の操作盤からパラメータ同定再開が指定されまでの間はパラメータ同定値は変化せず、パラメータ同定が停止される。
実施の形態１３．
この発明の実施の形態１３によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４及び衝突判別手段５以外の各手段の動作は、実施の形態２と同様なため、説明は省略する。
この発明の実施の形態１３によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４及び衝突判別手段５以外の各手段の動作は、実施の形態２と同様なため、説明は省略する。
次に動作について説明する。
しきい値演算手段４では、予め記憶されているパラメータ値を読み出して、上側高レベルしきい値、下側高レベルしきい値、上側低レベルしきい値、下側低レベルしきい値、衝突判別時間を各軸毎に設定し、衝突判別手段５に出力する。衝突判別手段５では、まず、モータ制御手段６から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に、演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時は、ロボットが衝突したと判別する。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは、前記タイマーを停止させると共に、タイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段４で設定された衝突判別時間以上となった場合もロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成部に停止命令を送信する。
しきい値演算手段４では、予め記憶されているパラメータ値を読み出して、上側高レベルしきい値、下側高レベルしきい値、上側低レベルしきい値、下側低レベルしきい値、衝突判別時間を各軸毎に設定し、衝突判別手段５に出力する。衝突判別手段５では、まず、モータ制御手段６から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に、演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時は、ロボットが衝突したと判別する。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは、前記タイマーを停止させると共に、タイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段４で設定された衝突判別時間以上となった場合もロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成部に停止命令を送信する。
実施の形態１４．
図１０はこの発明の実施の形態１４によるロボット制御装置を示すブロック図であり、図において、２８はしきい値演算手段、２９は第１衝突判別手段、３０は第２衝突判別手段である。
その他の構成については図２と同一のため、説明は省略する。
図１０はこの発明の実施の形態１４によるロボット制御装置を示すブロック図であり、図において、２８はしきい値演算手段、２９は第１衝突判別手段、３０は第２衝突判別手段である。
その他の構成については図２と同一のため、説明は省略する。
次に動作について説明する。
しきい値演算手段２８では、予め記憶されているパラメータ値を読み出して、上側高レベルしきい値、下側高レベルしきい値、上側低レベルしきい値、下側低レベルしきい値、衝突判別時間を各軸毎に設定し、上側低レベルしきい値、下側低レベルしきい値、衝突判別時間を第1衝突判別手段２９に出力し、上側高レベルしきい値、下側高レベルしきい値を第２衝突判別手段３０に出力する。
第1衝突判別手段２９では、まず、モータ制御手段１０から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に、演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。
第２衝突判別手段３０では、まず、モータ制御手段１０で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に、演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段３０は、モータ制御手段１０に停止命令を送信すると共に、指令値生成手段１にも衝突検知信号を出力する。
しきい値演算手段２８では、予め記憶されているパラメータ値を読み出して、上側高レベルしきい値、下側高レベルしきい値、上側低レベルしきい値、下側低レベルしきい値、衝突判別時間を各軸毎に設定し、上側低レベルしきい値、下側低レベルしきい値、衝突判別時間を第1衝突判別手段２９に出力し、上側高レベルしきい値、下側高レベルしきい値を第２衝突判別手段３０に出力する。
第1衝突判別手段２９では、まず、モータ制御手段１０から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に、演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。
第２衝突判別手段３０では、まず、モータ制御手段１０で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に、演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段３０は、モータ制御手段１０に停止命令を送信すると共に、指令値生成手段１にも衝突検知信号を出力する。
実施の形態１５．
図１１はこの発明の実施の形態１５によるロボット制御装置を示すブロック図であり、図において、３１はトルク余裕演算手段、３２はトルク制限値演算手段、３３はモータ制御手段、３４はモータ電流指令生成手段、３５はモータ電流制限手段（モータ駆動トルク制限手段）、３６はモータ電流制御手段、３７は第２衝突判別手段である。
その他の構成については図１から図１０と同一のため、説明は省略する。
図１１はこの発明の実施の形態１５によるロボット制御装置を示すブロック図であり、図において、３１はトルク余裕演算手段、３２はトルク制限値演算手段、３３はモータ制御手段、３４はモータ電流指令生成手段、３５はモータ電流制限手段（モータ駆動トルク制限手段）、３６はモータ電流制御手段、３７は第２衝突判別手段である。
その他の構成については図１から図１０と同一のため、説明は省略する。
次に動作について説明する。
トルク余裕演算手段３１は、しきい値演算手段２８から上側低レベルしきい値及び下側低レベルしきい値を読み込む。トルク余裕演算手段３１には、予め倍率αが記憶されており、各軸の上側低レベルしきい値及び下側低レベルしきい値のそれぞれをα倍することにより、各軸の上側トルク余裕値及び下側トルク余裕値が計算される。
トルク制限値演算手段３２では、必要駆動トルク演算手段３で演算された各軸の必要駆動トルクに各軸の上側トルク余裕値を加算し、各軸の駆動トルク上側制限値を計算する。同様に必要駆動トルク演算手段３で演算された各軸の必要駆動トルクに各軸の下側トルク余裕値を加算し、各軸の駆動トルク下側制限値を計算する。トルク制限値演算手段３２で演算された上側トルク制限値及び下側トルク制限値はモータ電流制限手段３５に送信される。
モータ電流制限手段３５では、減速比、モータのトルク定数等を考慮して、受信した上側トルク制限値及び下側トルク制限値をそれぞれ上側モータ電流制限値及び下側電流制限値に変換する。さらに、モータ電流指令生成手段３４から出力される電流指令が上側モータ電流制限値以下でかつ下側電流制限値以上となるように電流指令に制限を加える。具体的には電流指令が上側電流制限値以上の時は電流指令を上側モータ電流制限値に置き換える。電流指令が下側電流制限値以下の時は、電流指令を下側モータ電流制限値に置き換える。モータ電流制御手段３６では、モータ電流制限手段３５で制限された電流指令に基づき各軸を駆動するモータの電流制御を行う。
第２衝突判別手段３７では、まず、モータ電流指令生成手段３４から出力されるモータ電流指令から実際の各軸のモータにおける駆動トルク指令を減速比、トルク定数を考慮して算出する。次に、演算したモータ駆動トルク指令と必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段３７は、停止命令をモータ制御手段３３に停止命令を送信すると共に、指令値生成手段１にも衝突検知信号を出力する。
モータ制御手段３３では、衝突検知後、第２衝突判別手段３７から停止命令を受けると、モータ制御手段３３内部で停止するための位置指令を生成し、生成した位置指令に従って位置制御を行う。
トルク余裕演算手段３１は、しきい値演算手段２８から上側低レベルしきい値及び下側低レベルしきい値を読み込む。トルク余裕演算手段３１には、予め倍率αが記憶されており、各軸の上側低レベルしきい値及び下側低レベルしきい値のそれぞれをα倍することにより、各軸の上側トルク余裕値及び下側トルク余裕値が計算される。
トルク制限値演算手段３２では、必要駆動トルク演算手段３で演算された各軸の必要駆動トルクに各軸の上側トルク余裕値を加算し、各軸の駆動トルク上側制限値を計算する。同様に必要駆動トルク演算手段３で演算された各軸の必要駆動トルクに各軸の下側トルク余裕値を加算し、各軸の駆動トルク下側制限値を計算する。トルク制限値演算手段３２で演算された上側トルク制限値及び下側トルク制限値はモータ電流制限手段３５に送信される。
モータ電流制限手段３５では、減速比、モータのトルク定数等を考慮して、受信した上側トルク制限値及び下側トルク制限値をそれぞれ上側モータ電流制限値及び下側電流制限値に変換する。さらに、モータ電流指令生成手段３４から出力される電流指令が上側モータ電流制限値以下でかつ下側電流制限値以上となるように電流指令に制限を加える。具体的には電流指令が上側電流制限値以上の時は電流指令を上側モータ電流制限値に置き換える。電流指令が下側電流制限値以下の時は、電流指令を下側モータ電流制限値に置き換える。モータ電流制御手段３６では、モータ電流制限手段３５で制限された電流指令に基づき各軸を駆動するモータの電流制御を行う。
第２衝突判別手段３７では、まず、モータ電流指令生成手段３４から出力されるモータ電流指令から実際の各軸のモータにおける駆動トルク指令を減速比、トルク定数を考慮して算出する。次に、演算したモータ駆動トルク指令と必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段３７は、停止命令をモータ制御手段３３に停止命令を送信すると共に、指令値生成手段１にも衝突検知信号を出力する。
モータ制御手段３３では、衝突検知後、第２衝突判別手段３７から停止命令を受けると、モータ制御手段３３内部で停止するための位置指令を生成し、生成した位置指令に従って位置制御を行う。
実施の形態１６．
この発明の実施の形態１６によるロボット制御装置を示すブロック図は図４０であり、トルク制限値演算手段３２以外の各手段の動作は、実施の形態１５と同様なため、説明は省略する。
この発明の実施の形態１６によるロボット制御装置を示すブロック図は図４０であり、トルク制限値演算手段３２以外の各手段の動作は、実施の形態１５と同様なため、説明は省略する。
次に動作について説明する。
トルク制限値演算手段３２の内部では、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfdを加算し、各軸のトルク指令から構成されるトルク指令ベクトルτdを算出する。次に算出したトルク指令ベクトルτdの各軸要素を１次遅れフィルタ手段に入力し、その出力から構成されるベクトルをτdfとする。さらに、τdfの各軸要素に各軸の上側トルク余裕値を加算して各軸の駆動トルク上側制限値を計算する。同様にτdfの各軸要素に各軸の下側トルク余裕値を加算し、各軸の駆動トルク下側制限値を計算する。トルク制限値演算手段３２で演算された上側トルク制限値及び下側トルク制限値は、モータ電流制限手段３５に送信される。
トルク制限値演算手段３２の内部では、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhd、重力指令要素ベクトルτgd、摩擦力指令要素ベクトルτfdを加算し、各軸のトルク指令から構成されるトルク指令ベクトルτdを算出する。次に算出したトルク指令ベクトルτdの各軸要素を１次遅れフィルタ手段に入力し、その出力から構成されるベクトルをτdfとする。さらに、τdfの各軸要素に各軸の上側トルク余裕値を加算して各軸の駆動トルク上側制限値を計算する。同様にτdfの各軸要素に各軸の下側トルク余裕値を加算し、各軸の駆動トルク下側制限値を計算する。トルク制限値演算手段３２で演算された上側トルク制限値及び下側トルク制限値は、モータ電流制限手段３５に送信される。
実施の形態１７．
この発明の実施の形態１７によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
この発明の実施の形態１７によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
次に動作について説明する。
この実施の形態１７におけるロボットの制御装置では、教示モード切替スイッチを教示側に押し込んでいる場合のみ、手動操作盤での操作に基づいて手動で動かすことができるようになっており、また、教示モード切替スイッチを自動運転側に押し込んでいる場合のみ、プログラムに基づく自動運転ができるようになっている。図示しない教示モード判別手段では、図示しない制御装置の操作盤に取り付けられている教示モード切替スイッチの状態に応じて教示モード判定パラメータの値を設定する。具体的には教示モード切替スイッチが教示側に押し込まれているときには教示モード判別パラメータの値を１とし、教示モード切替スイッチが自動運転側に押し込まれているときは教示モード判別パラメータの値を０とする。
しきい値演算手段４には、手動操作盤からロボットを動作させるとき用の各軸の上側しきい値及び下側しきい値と、プログラムに基づき自動運転する場合用の各軸の上側しきい値及び下側しきい値が予め記憶されている。教示モード判別手段から入力される教示モード判別パラメータの値が１の時は、手動操作盤からロボットを動作させるとき用の各軸の上側しきい値及び下側しきい値を採用する。教示モード判別手段から入力される教示モード判別パラメータの値が０の時は、プログラムに基づき自動運転する場合用の各軸の上側しきい値及び下側しきい値を採用する。
この実施の形態１７におけるロボットの制御装置では、教示モード切替スイッチを教示側に押し込んでいる場合のみ、手動操作盤での操作に基づいて手動で動かすことができるようになっており、また、教示モード切替スイッチを自動運転側に押し込んでいる場合のみ、プログラムに基づく自動運転ができるようになっている。図示しない教示モード判別手段では、図示しない制御装置の操作盤に取り付けられている教示モード切替スイッチの状態に応じて教示モード判定パラメータの値を設定する。具体的には教示モード切替スイッチが教示側に押し込まれているときには教示モード判別パラメータの値を１とし、教示モード切替スイッチが自動運転側に押し込まれているときは教示モード判別パラメータの値を０とする。
しきい値演算手段４には、手動操作盤からロボットを動作させるとき用の各軸の上側しきい値及び下側しきい値と、プログラムに基づき自動運転する場合用の各軸の上側しきい値及び下側しきい値が予め記憶されている。教示モード判別手段から入力される教示モード判別パラメータの値が１の時は、手動操作盤からロボットを動作させるとき用の各軸の上側しきい値及び下側しきい値を採用する。教示モード判別手段から入力される教示モード判別パラメータの値が０の時は、プログラムに基づき自動運転する場合用の各軸の上側しきい値及び下側しきい値を採用する。
実施の形態１８．
この発明の実施の形態１８によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
この発明の実施の形態１８によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
次に動作について説明する。
図示しないタイマー手段では、モータ電源投入時にタイマーを０から開始し、電源投入後の経過時間を出力する。モータ電源が遮断されるとタイマーを停止し、内部で持っている電源投入後の経過時間を０にリセットする。
しきい値演算手段４では、時間の規定値、モータ電源投入からの経過時間が規定値以下の場合に用いる第１上側しきい値及び第１下側しきい値と、モータ電源投入からの経過時間が規定値以上の場合に用いる第２上側しきい値及び第２下側しきい値が予め記憶されている。記憶されている時間の規定値とタイマー手段から出力されるモータ電源投入後の経過時間を比較し、時間規定値の方が大きい場合は、第１上側しきい値及び第１下側しきい値を採用し、時間規定値の方が小さい場合は、第２上側しきい値及び第２下側しきい値を採用する。
図示しないタイマー手段では、モータ電源投入時にタイマーを０から開始し、電源投入後の経過時間を出力する。モータ電源が遮断されるとタイマーを停止し、内部で持っている電源投入後の経過時間を０にリセットする。
しきい値演算手段４では、時間の規定値、モータ電源投入からの経過時間が規定値以下の場合に用いる第１上側しきい値及び第１下側しきい値と、モータ電源投入からの経過時間が規定値以上の場合に用いる第２上側しきい値及び第２下側しきい値が予め記憶されている。記憶されている時間の規定値とタイマー手段から出力されるモータ電源投入後の経過時間を比較し、時間規定値の方が大きい場合は、第１上側しきい値及び第１下側しきい値を採用し、時間規定値の方が小さい場合は、第２上側しきい値及び第２下側しきい値を採用する。
実施の形態１９．
この発明の実施の形態１９によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
この発明の実施の形態１９によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
次に動作について説明する。
図示しない手先負荷設定値読み出し手段では、プログラムの命令によって指定される手先負荷パラメータ値もしくは制御装置内に記憶されている手先負荷パラメータ値を読み込む。また、手先負荷設定値読み出し手段内部には、下表に示すような、手先負荷と手先負荷の重心位置とに応じたテーブルを持っており、現在の手先負荷設定値が属するグループ番号を出力する。
しきい値演算手段４には、手先負荷の属する各グループ毎の上側しきい値及び下側しきい値のテーブルを備えており、手先負荷設定値読み出し手段から入力されるグループ番号の上側しきい値及び下側しきい値をそれぞれ上側しきい値及び下側しきい値として出力する。
図示しない手先負荷設定値読み出し手段では、プログラムの命令によって指定される手先負荷パラメータ値もしくは制御装置内に記憶されている手先負荷パラメータ値を読み込む。また、手先負荷設定値読み出し手段内部には、下表に示すような、手先負荷と手先負荷の重心位置とに応じたテーブルを持っており、現在の手先負荷設定値が属するグループ番号を出力する。
実施の形態２０．
この発明の実施の形態２０によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
この発明の実施の形態２０によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
次に動作について説明する。
図示しない検出感度読み出し手段では、まず、現在の検出感度の設定値を読み出す。検出感度の設定はプログラム内に記載された命令を用いて、もしくは制御装置の手動操作盤から設定される。次に、検出感度読み出し手段では、読み出した検出感度から倍率αを計算する。例えば、検出感度Ｘ％のとき、倍率αはα＝１００／Ｘで算出する。算出した倍率は、しきい値演算手段４に出力される。しきい値演算手段４には、検出感度が１００％の時の上側しきい値及び下側しきい値が予め記憶されている。しきい値演算手段４では、記憶されている上側しきい値及び下側しきい値に検出感度読み出し手段から入力される倍率αを乗じたものをそれぞれ上側しきい値、下側しきい値として出力する。
図示しない検出感度読み出し手段では、まず、現在の検出感度の設定値を読み出す。検出感度の設定はプログラム内に記載された命令を用いて、もしくは制御装置の手動操作盤から設定される。次に、検出感度読み出し手段では、読み出した検出感度から倍率αを計算する。例えば、検出感度Ｘ％のとき、倍率αはα＝１００／Ｘで算出する。算出した倍率は、しきい値演算手段４に出力される。しきい値演算手段４には、検出感度が１００％の時の上側しきい値及び下側しきい値が予め記憶されている。しきい値演算手段４では、記憶されている上側しきい値及び下側しきい値に検出感度読み出し手段から入力される倍率αを乗じたものをそれぞれ上側しきい値、下側しきい値として出力する。
実施の形態２１．
図１２はこの発明の実施の形態２１によるロボット制御装置を示すブロック図であり、図において、４７は同定進行度判別手段、４８はしきい値演算手段である。
その他の構成については図４と同一のため、説明は省略する。
図１２はこの発明の実施の形態２１によるロボット制御装置を示すブロック図であり、図において、４７は同定進行度判別手段、４８はしきい値演算手段である。
その他の構成については図４と同一のため、説明は省略する。
次に動作について説明する。
同定進行度判別手段４７は、各軸の同定進行度から構成される同定進行度ベクトルを出力する。まず、同定進行度判別手段４７には各軸の速度が入力される。同定進行度判別手段４７は、予め各軸の速度規定値及び各軸の時間規定値が記憶されている。同定進行度判別手段４７では、軸毎に、速度が速度既定値以上となっている累積時間を算出しており、累積時間が記憶されている規定時間以上となった場合、当該軸の同定進行度を１とする。累積時間が規定時間未満の場合は、当該軸の同定進行度は０とする。また、モータ電源が遮断されると、累積時間は０にリセットされる。
しきい値演算手段４８の内部には、パラメータ同定進行前の上側しきい値及び下側しきい値と、パラメータ同定進行後の上側しきい値及び下側しきい値が記憶されている。同定進行度判別手段４７から出力される同定進行度が１の軸に関しては、パラメータ同定進行後の上側しきい値及び下側しきい値が設定され、同定進行度判別手段４７から出力される同定進行度が０の軸に関しては、パラメータ同定進行前の上側しきい値及び下側しきい値が設定される。
同定進行度判別手段４７は、各軸の同定進行度から構成される同定進行度ベクトルを出力する。まず、同定進行度判別手段４７には各軸の速度が入力される。同定進行度判別手段４７は、予め各軸の速度規定値及び各軸の時間規定値が記憶されている。同定進行度判別手段４７では、軸毎に、速度が速度既定値以上となっている累積時間を算出しており、累積時間が記憶されている規定時間以上となった場合、当該軸の同定進行度を１とする。累積時間が規定時間未満の場合は、当該軸の同定進行度は０とする。また、モータ電源が遮断されると、累積時間は０にリセットされる。
しきい値演算手段４８の内部には、パラメータ同定進行前の上側しきい値及び下側しきい値と、パラメータ同定進行後の上側しきい値及び下側しきい値が記憶されている。同定進行度判別手段４７から出力される同定進行度が１の軸に関しては、パラメータ同定進行後の上側しきい値及び下側しきい値が設定され、同定進行度判別手段４７から出力される同定進行度が０の軸に関しては、パラメータ同定進行前の上側しきい値及び下側しきい値が設定される。
実施の形態２２．
図１３はこの発明の実施の形態２２によるロボット制御装置を示すブロック図であり、図において、４９は同定進行度判別手段である。
その他の構成については図１２と同一のため、説明は省略する。
図１３はこの発明の実施の形態２２によるロボット制御装置を示すブロック図であり、図において、４９は同定進行度判別手段である。
その他の構成については図１２と同一のため、説明は省略する。
次に動作について説明する。
同定進行度判別手段４９には、まず、パラメータ推定手段１４からパラメータ同定周期毎のパラメータ同定値が入力される。同定進行度判別手段４９では、入力されたパラメータ同定値から各軸のクーロン摩擦係数と粘性摩擦係数を選択し、それぞれの変化量を算出する。同定進行度判別手段４９には各軸のクーロン摩擦係数推定値変化量及び粘性摩擦係数推定値変化量の規定値を記憶しておき、実際のクーロン摩擦係数変化量及び粘性摩擦係数変化量の両方が規定値以下の軸に関しては同定進行度を１とする。実際のクーロン摩擦係数変化量及び粘性摩擦係数変化量の少なくとも一方が規定値以上の軸は同定進行度を０とする。
同定進行度判別手段４９には、まず、パラメータ推定手段１４からパラメータ同定周期毎のパラメータ同定値が入力される。同定進行度判別手段４９では、入力されたパラメータ同定値から各軸のクーロン摩擦係数と粘性摩擦係数を選択し、それぞれの変化量を算出する。同定進行度判別手段４９には各軸のクーロン摩擦係数推定値変化量及び粘性摩擦係数推定値変化量の規定値を記憶しておき、実際のクーロン摩擦係数変化量及び粘性摩擦係数変化量の両方が規定値以下の軸に関しては同定進行度を１とする。実際のクーロン摩擦係数変化量及び粘性摩擦係数変化量の少なくとも一方が規定値以上の軸は同定進行度を０とする。
実施の形態２３．
図１４はこの発明の実施の形態２３によるロボット制御装置を示すブロック図であり、図において、５０はトルク指令絶対値最大値演算手段、５１はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図１４はこの発明の実施の形態２３によるロボット制御装置を示すブロック図であり、図において、５０はトルク指令絶対値最大値演算手段、５１はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
トルク指令絶対値最大値演算手段５０には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算し、加算結果の絶対値を求め、求めた絶対値を各軸の減速比、トルク定数を考慮して各軸のモータ電流指令のスケールに換算する。さらに、各軸モータの定格電流に基づいて正規化する。各軸毎に算出した各軸モータの定格電流で正規化した慣性力指令と遠心コリオリ力指令の和の絶対値の中から最大のものをＴmaxとして出力する。
しきい値演算手段５１には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段５０から出力されるＴmaxを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値トルク指令倍率）×Ｔmax、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値トルク指令倍率）×Ｔmaxで算出される。
トルク指令絶対値最大値演算手段５０には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算し、加算結果の絶対値を求め、求めた絶対値を各軸の減速比、トルク定数を考慮して各軸のモータ電流指令のスケールに換算する。さらに、各軸モータの定格電流に基づいて正規化する。各軸毎に算出した各軸モータの定格電流で正規化した慣性力指令と遠心コリオリ力指令の和の絶対値の中から最大のものをＴmaxとして出力する。
しきい値演算手段５１には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段５０から出力されるＴmaxを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値トルク指令倍率）×Ｔmax、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値トルク指令倍率）×Ｔmaxで算出される。
実施の形態２４．
図１５はこの発明の実施の形態２４によるロボット制御装置を示すブロック図であり、図において、５２はモータ電流絶対値最大値演算手段、５３はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図１５はこの発明の実施の形態２４によるロボット制御装置を示すブロック図であり、図において、５２はモータ電流絶対値最大値演算手段、５３はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
モータ電流絶対値最大値演算手段５２では、まず、各軸のモータ電流測定値を入力し、各軸の電流測定値の絶対値を算出してから各軸モータの定格電流で正規化する。次に、正規化した各軸の電流測定値絶対値のなかで最大のものをＤmaxとして出力する。
しきい値演算手段５３には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値電流倍率、下側しきい値電流倍率を各軸毎に記憶しておき、モータ電流絶対値最大値演算手段５２から出力されるＤmaxを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値電流倍率）×Ｄmax、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値電流倍率）×Ｄmaxで算出される。
モータ電流絶対値最大値演算手段５２では、まず、各軸のモータ電流測定値を入力し、各軸の電流測定値の絶対値を算出してから各軸モータの定格電流で正規化する。次に、正規化した各軸の電流測定値絶対値のなかで最大のものをＤmaxとして出力する。
しきい値演算手段５３には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値電流倍率、下側しきい値電流倍率を各軸毎に記憶しておき、モータ電流絶対値最大値演算手段５２から出力されるＤmaxを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値電流倍率）×Ｄmax、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値電流倍率）×Ｄmaxで算出される。
実施の形態２５．
図１６はこの発明の実施の形態２５によるロボット制御装置を示すブロック図であり、図において、５４はトルク指令トルク指令差分絶対値最大値演算手段、５５はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図１６はこの発明の実施の形態２５によるロボット制御装置を示すブロック図であり、図において、５４はトルク指令トルク指令差分絶対値最大値演算手段、５５はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
トルク指令トルク指令差分絶対値最大値演算手段５４には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算したτmhdを算出する。τmhdの各軸要素は１周期分記憶しておき、今回のτmhdの値と前回のτmhdの値の差であるｄτmhdを算出し、τmhd及びｄτmhdの各軸の絶対値から構成されるベクトルａτmhd及びａｄτmhdを算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が
ｗτmhdi＝aτmhdi＋ｗ×ａｄτmhdi・・・（３０）
で算出されるから構成されるｗτmhdを算出する。（３０）式において、添え字ｉは第ｉ軸の要素を意味する。さらに、ｗτmhdiを当該軸の減速比、トルク定数を用いてモータ電流指令のスケールに換算し、当該軸モータの定格電流で正規化したものをｓτmhdiとして算出する。ｓτmhdiの最大値をＳmaxとして出力する。
しきい値演算手段５５には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令トルク指令差分倍率、下側しきい値トルク指令トルク指令差分倍率を各軸毎に記憶しておき、トルク指令トルク指令差分絶対値最大値演算手段５４から出力されるＳmaxを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値トルク指令トルク指令差分倍率）×Ｓmax、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値トルク指令トルク指令差分倍率）×Ｓmaxで算出される。
トルク指令トルク指令差分絶対値最大値演算手段５４には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算したτmhdを算出する。τmhdの各軸要素は１周期分記憶しておき、今回のτmhdの値と前回のτmhdの値の差であるｄτmhdを算出し、τmhd及びｄτmhdの各軸の絶対値から構成されるベクトルａτmhd及びａｄτmhdを算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が
ｗτmhdi＝aτmhdi＋ｗ×ａｄτmhdi・・・（３０）
で算出されるから構成されるｗτmhdを算出する。（３０）式において、添え字ｉは第ｉ軸の要素を意味する。さらに、ｗτmhdiを当該軸の減速比、トルク定数を用いてモータ電流指令のスケールに換算し、当該軸モータの定格電流で正規化したものをｓτmhdiとして算出する。ｓτmhdiの最大値をＳmaxとして出力する。
しきい値演算手段５５には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令トルク指令差分倍率、下側しきい値トルク指令トルク指令差分倍率を各軸毎に記憶しておき、トルク指令トルク指令差分絶対値最大値演算手段５４から出力されるＳmaxを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値トルク指令トルク指令差分倍率）×Ｓmax、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値トルク指令トルク指令差分倍率）×Ｓmaxで算出される。
実施の形態２６．
図１７はこの発明の実施の形態２６によるロボット制御装置を示すブロック図であり、図において、５６はトルク指令トルク指令差分絶対値演算手段、５７はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図１７はこの発明の実施の形態２６によるロボット制御装置を示すブロック図であり、図において、５６はトルク指令トルク指令差分絶対値演算手段、５７はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
トルク指令トルク指令差分絶対値演算手段５６には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算したτmhdを算出する。τmhdの各軸要素は１周期分記憶しておき、今回のτmhdの値と前回のτmhdの値の差であるｄτmhdを算出し、τmhd及びｄτmhdの各軸の絶対値から構成されるベクトルａτmhd及びａｄτmhdを算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が、前記（３０）式で算出されるから構成されるｗτmhdを算出する。（３０）式において、添え字ｉは第ｉ軸の要素を意味する。さらに、ｗτmhdiを当該軸の減速比、トルク定数を用いてモータ電流指令のスケールに換算し、当該軸モータの定格電流で正規化したものをｓτmhdiとして算出する。最後に、ｓτmhdiを１次遅れフィルタ手段に入力し、１次遅れフィルタ手段の出力をfτmhdiとして出力する。
しきい値演算手段５７には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令トルク指令差分倍率、下側しきい値トルク指令トルク指令差分倍率を各軸毎に記憶しておき、トルク指令トルク指令差分絶対値最大値演算手段５６から出力されるfτmhdiを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値トルク指令トルク指令差分倍率）×fτmhdi、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値トルク指令トルク指令差分倍率）×fτmhdiで算出される。
トルク指令トルク指令差分絶対値演算手段５６には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に、慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算したτmhdを算出する。τmhdの各軸要素は１周期分記憶しておき、今回のτmhdの値と前回のτmhdの値の差であるｄτmhdを算出し、τmhd及びｄτmhdの各軸の絶対値から構成されるベクトルａτmhd及びａｄτmhdを算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が、前記（３０）式で算出されるから構成されるｗτmhdを算出する。（３０）式において、添え字ｉは第ｉ軸の要素を意味する。さらに、ｗτmhdiを当該軸の減速比、トルク定数を用いてモータ電流指令のスケールに換算し、当該軸モータの定格電流で正規化したものをｓτmhdiとして算出する。最後に、ｓτmhdiを１次遅れフィルタ手段に入力し、１次遅れフィルタ手段の出力をfτmhdiとして出力する。
しきい値演算手段５７には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令トルク指令差分倍率、下側しきい値トルク指令トルク指令差分倍率を各軸毎に記憶しておき、トルク指令トルク指令差分絶対値最大値演算手段５６から出力されるfτmhdiを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値トルク指令トルク指令差分倍率）×fτmhdi、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値トルク指令トルク指令差分倍率）×fτmhdiで算出される。
実施の形態２７．
図１８はこの発明の実施の形態２７によるロボット制御装置を示すブロック図であり、図において、５８は加速度指令加加速度指令絶対値演算手段、５９はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図１８はこの発明の実施の形態２７によるロボット制御装置を示すブロック図であり、図において、５８は加速度指令加加速度指令絶対値演算手段、５９はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
加速度指令加加速度指令絶対値演算手段５８には、まず、各軸の加速度指令ａdが入力される。ａdの各軸要素は１周期分記憶しておき、今回のａdの値と前回のａdの差であるｊdを各軸毎に計算する。次に、ａd及びｊdの各軸要素の絶対値から構成されるベクトルａａd及びａｊdを算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が
ｗａｊdi＝ａａdi＋ｗ×ａｊdi ・・・（３１）
で算出されるから構成されるｗａｊdを算出する。（３１）式において、添え字ｉは第ｉ軸の要素を意味する。ｗａｊdは加速度指令加加速度指令絶対値演算手段５８からしきい値演算手段５９に出力される。
しきい値演算手段５９には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値加速度指令加加速度指令倍率、下側しきい値加速度指令加加速度指令倍率を各軸毎に記憶しておき、加速度指令加加速度指令絶対値演算手段５８から出力されるｗａｊdiを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値加速度指令加加速度指令倍率）×ｗａｊdi、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値加速度指令加加速度指令倍率）×ｗａｊdiで算出される。
加速度指令加加速度指令絶対値演算手段５８には、まず、各軸の加速度指令ａdが入力される。ａdの各軸要素は１周期分記憶しておき、今回のａdの値と前回のａdの差であるｊdを各軸毎に計算する。次に、ａd及びｊdの各軸要素の絶対値から構成されるベクトルａａd及びａｊdを算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が
ｗａｊdi＝ａａdi＋ｗ×ａｊdi ・・・（３１）
で算出されるから構成されるｗａｊdを算出する。（３１）式において、添え字ｉは第ｉ軸の要素を意味する。ｗａｊdは加速度指令加加速度指令絶対値演算手段５８からしきい値演算手段５９に出力される。
しきい値演算手段５９には、上側しきい値ベース分、下側しきい値ベース分、上側しきい値加速度指令加加速度指令倍率、下側しきい値加速度指令加加速度指令倍率を各軸毎に記憶しておき、加速度指令加加速度指令絶対値演算手段５８から出力されるｗａｊdiを用いて、各軸の上側しきい値は、（上側しきい値ベース分）＋（上側しきい値加速度指令加加速度指令倍率）×ｗａｊdi、下側しきい値は、（下側しきい値ベース分）＋（下側しきい値加速度指令加加速度指令倍率）×ｗａｊdiで算出される。
実施の形態２８．
図１９はこの発明の実施の形態２８によるロボット制御装置を示すブロック図であり、図において、６０はしきい値倍率要素演算手段、６１はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図１９はこの発明の実施の形態２８によるロボット制御装置を示すブロック図であり、図において、６０はしきい値倍率要素演算手段、６１はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
しきい値倍率要素演算手段６０には、まず、位置指令、速度指令、加速度指令が入力される。速度指令及び加速度指令は減速比を考慮し、モータでの速度指令、加速度指令のスケールに変換される。さらに、スケール変換後の各軸の速度指令の最大値であるモータレベル速度指令最大値と、スケール変換後の各軸の加速度指令の最大値であるモータレベル加速度指令最大値を算出する。位置指令の属する範囲、モータレベル速度指令最大値、モータレベル加速度指令最大値のそれぞれに対応するしきい値倍率のテーブルをしきい値倍率要素演算手段６０の内部に用意しておき、現在の位置指令、速度指令、加速度指令に対応する倍率ｂｐ、ｂｖ、ｂａをテーブルから読み出して出力する。
しきい値演算手段６１には、倍率１の時の上側しきい値及び下側しきい値を各軸毎に記憶しておく。次に、しきい値倍率要素演算手段６０から出力される倍率ｂｐ、ｂｖ、ｂａから、各軸毎の倍率１の時の上側しきい値及び下側しきい値をそれぞれｂｐ×ｂｖ×ｂａ倍してから出力する。
しきい値倍率要素演算手段６０には、まず、位置指令、速度指令、加速度指令が入力される。速度指令及び加速度指令は減速比を考慮し、モータでの速度指令、加速度指令のスケールに変換される。さらに、スケール変換後の各軸の速度指令の最大値であるモータレベル速度指令最大値と、スケール変換後の各軸の加速度指令の最大値であるモータレベル加速度指令最大値を算出する。位置指令の属する範囲、モータレベル速度指令最大値、モータレベル加速度指令最大値のそれぞれに対応するしきい値倍率のテーブルをしきい値倍率要素演算手段６０の内部に用意しておき、現在の位置指令、速度指令、加速度指令に対応する倍率ｂｐ、ｂｖ、ｂａをテーブルから読み出して出力する。
しきい値演算手段６１には、倍率１の時の上側しきい値及び下側しきい値を各軸毎に記憶しておく。次に、しきい値倍率要素演算手段６０から出力される倍率ｂｐ、ｂｖ、ｂａから、各軸毎の倍率１の時の上側しきい値及び下側しきい値をそれぞれｂｐ×ｂｖ×ｂａ倍してから出力する。
実施の形態２９．
図２０はこの発明の実施の形態２９によるロボット制御装置を示すブロック図であり、図において、６２は停止判別手段、６３はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
図２０はこの発明の実施の形態２９によるロボット制御装置を示すブロック図であり、図において、６２は停止判別手段、６３はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
停止判別手段６２に、まず、速度指令及びモータ速度を入力する。停止判別手段６２は、軸毎に停止判別用規定値を記憶しており、速度指令及びモータ速度の両方を当該軸の停止判別用規定値と比較する。速度指令の各軸要素及びモータ速度の各軸要素の絶対値を算出し、速度指令絶対値及びモータ速度絶対値の両方が規定値以下の軸は停止中と判別し、当該軸の停止判別ベクトルの要素の値を１とする。速度指令絶対値及びモータ速度絶対値の少なくとも一方が規定値以上の場合は動作中と判別し、当該軸の停止判別ベクトルの要素の値を０とする。
しきい値演算手段６３の内部に、各軸の上側しきい値、下側しきい値、クーロン摩擦係数を記憶しておく。停止判別手段６２から出力される停止判別ベクトルの要素の値が０の軸に関しては、上側しきい値及び下側しきい値をそのまま出力する。停止判別手段６２から出力される停止判別ベクトルの要素の値が１の軸に関しては、記憶してある上側しきい値にクーロン摩擦係数を加算した値を上側しきい値として出力する。また、記憶してある下側しきい値にクーロン摩擦係数を減算した値を下側しきい値として出力する。
停止判別手段６２に、まず、速度指令及びモータ速度を入力する。停止判別手段６２は、軸毎に停止判別用規定値を記憶しており、速度指令及びモータ速度の両方を当該軸の停止判別用規定値と比較する。速度指令の各軸要素及びモータ速度の各軸要素の絶対値を算出し、速度指令絶対値及びモータ速度絶対値の両方が規定値以下の軸は停止中と判別し、当該軸の停止判別ベクトルの要素の値を１とする。速度指令絶対値及びモータ速度絶対値の少なくとも一方が規定値以上の場合は動作中と判別し、当該軸の停止判別ベクトルの要素の値を０とする。
しきい値演算手段６３の内部に、各軸の上側しきい値、下側しきい値、クーロン摩擦係数を記憶しておく。停止判別手段６２から出力される停止判別ベクトルの要素の値が０の軸に関しては、上側しきい値及び下側しきい値をそのまま出力する。停止判別手段６２から出力される停止判別ベクトルの要素の値が１の軸に関しては、記憶してある上側しきい値にクーロン摩擦係数を加算した値を上側しきい値として出力する。また、記憶してある下側しきい値にクーロン摩擦係数を減算した値を下側しきい値として出力する。
実施の形態３０．
図２１はこの発明の実施の形態３０によるロボット制御装置を示すブロック図であり、図において、６４は特殊作業判別手段、６５はしきい値演算手段、６６はプログラム解析手段である。
その他の構成については図１と同一のため、説明は省略する。
図２１はこの発明の実施の形態３０によるロボット制御装置を示すブロック図であり、図において、６４は特殊作業判別手段、６５はしきい値演算手段、６６はプログラム解析手段である。
その他の構成については図１と同一のため、説明は省略する。
次に動作について説明する。
プログラム解析手段６６では、実行するプログラムを順次読み込んでいき、指令値生成手段１等に必要な情報を順次送信している。プログラム解析手段６６がハンド開閉命令を処理すると、図示しないハンド開閉手段にハンド開平命令を送信すると共に、特殊作業判別手段６４にもハンド開閉命令を送信する。特殊作業判別手段６４は、ハンド開閉命令を受信すると予め記憶している規定時間の間、特殊作業判別パラメータの値を１にする。特殊作業判別パラメータの初期値は０となっており、ハンド開閉命令受信後規定時間経過すると再び０に戻される。
しきい値演算手段６５には、通常作業用上側しきい値及び下側しきい値と特殊作業用上側しきい値及び下側しきい値が記憶されている。特殊作業判別手段６４から出力される特殊作業判別パラメータの値が１のときは、特殊作業用上側しきい値及び下側しきい値が設定され、特殊作業判別パラメータの値が０のときは、通常作業用上側しきい値及び下側しきい値が設定される。
プログラム解析手段６６では、実行するプログラムを順次読み込んでいき、指令値生成手段１等に必要な情報を順次送信している。プログラム解析手段６６がハンド開閉命令を処理すると、図示しないハンド開閉手段にハンド開平命令を送信すると共に、特殊作業判別手段６４にもハンド開閉命令を送信する。特殊作業判別手段６４は、ハンド開閉命令を受信すると予め記憶している規定時間の間、特殊作業判別パラメータの値を１にする。特殊作業判別パラメータの初期値は０となっており、ハンド開閉命令受信後規定時間経過すると再び０に戻される。
しきい値演算手段６５には、通常作業用上側しきい値及び下側しきい値と特殊作業用上側しきい値及び下側しきい値が記憶されている。特殊作業判別手段６４から出力される特殊作業判別パラメータの値が１のときは、特殊作業用上側しきい値及び下側しきい値が設定され、特殊作業判別パラメータの値が０のときは、通常作業用上側しきい値及び下側しきい値が設定される。
実施の形態３１．
図２２はこの発明の実施の形態３１によるロボット制御装置を示すブロック図であり、図において、６７は平滑化手段である。
その他の構成については図１５と同一のため、説明は省略する。
図２２はこの発明の実施の形態３１によるロボット制御装置を示すブロック図であり、図において、６７は平滑化手段である。
その他の構成については図１５と同一のため、説明は省略する。
次に動作について説明する。
平滑化手段６７は、トルク余裕演算手段３１から上側トルク余裕値及び下側トルク余裕値を順次受信する。平滑化手段６７の内部には２段の移動平均フィルタ手段が設けられており、受信した上側トルク余裕値及び下側トルク余裕値を順次２段の移動平均フィルタに通した結果を上側トルク余裕値及び下側トルク余裕値としてトルク制限値演算手段３２に送信する。２段の移動平均フィルタを通すことにより、しきい値演算手段２８で演算される低レベル上側しきい値及び低レベル下側しきい値がステップ状に変化する場合にもトルク制限値演算手段３２から出力される上側モータ電流制限値及び下側モータ電流制限値は滑らかに変化する。
平滑化手段６７は、トルク余裕演算手段３１から上側トルク余裕値及び下側トルク余裕値を順次受信する。平滑化手段６７の内部には２段の移動平均フィルタ手段が設けられており、受信した上側トルク余裕値及び下側トルク余裕値を順次２段の移動平均フィルタに通した結果を上側トルク余裕値及び下側トルク余裕値としてトルク制限値演算手段３２に送信する。２段の移動平均フィルタを通すことにより、しきい値演算手段２８で演算される低レベル上側しきい値及び低レベル下側しきい値がステップ状に変化する場合にもトルク制限値演算手段３２から出力される上側モータ電流制限値及び下側モータ電流制限値は滑らかに変化する。
実施の形態３２．
図２３はこの発明の実施の形態３２によるロボット制御装置を示すブロック図であり、図において、６８はトルク制限値保持手段である。
その他の構成については図２２と同一のため、説明は省略する。
図２３はこの発明の実施の形態３２によるロボット制御装置を示すブロック図であり、図において、６８はトルク制限値保持手段である。
その他の構成については図２２と同一のため、説明は省略する。
次に動作について説明する。
トルク制限値保持手段６８には、通常状態では内部で保持している衝突状態判別パラメータの値が０になっており、前記衝突状態判別パラメータの値が０の時は、今回周期にトルク制限値演算手段３２から入力される上側トルク制限値及び下側トルク制限値をトルク制限値記憶エリアに書き込み記憶させる。また、トルク制限値演算手段３２から入力される上側トルク制限値及び下側トルク制限値をそのまま出力する。第１衝突判別手段２９もしくは第２衝突判別手段３７から衝突検知信号を受信すると衝突状態判別パラメータの値を１にする。衝突状態判別パラメータの値が１の時は、今回周期の上側トルク制限値及び下側トルク制限値の記憶エリアへの書き込みを停止し、記憶エリアに格納されている上側トルク制限値及び下側トルク制限値を出力する。衝突状態判別パラメータの値はモータ電源遮断時に０に戻される。
トルク制限値保持手段６８には、通常状態では内部で保持している衝突状態判別パラメータの値が０になっており、前記衝突状態判別パラメータの値が０の時は、今回周期にトルク制限値演算手段３２から入力される上側トルク制限値及び下側トルク制限値をトルク制限値記憶エリアに書き込み記憶させる。また、トルク制限値演算手段３２から入力される上側トルク制限値及び下側トルク制限値をそのまま出力する。第１衝突判別手段２９もしくは第２衝突判別手段３７から衝突検知信号を受信すると衝突状態判別パラメータの値を１にする。衝突状態判別パラメータの値が１の時は、今回周期の上側トルク制限値及び下側トルク制限値の記憶エリアへの書き込みを停止し、記憶エリアに格納されている上側トルク制限値及び下側トルク制限値を出力する。衝突状態判別パラメータの値はモータ電源遮断時に０に戻される。
実施の形態３３．
図２４はこの発明の実施の形態３３によるロボット制御装置を示すブロック図であり、図において、６９は手先負荷設定値読み出し手段、７０は平滑化手段である。
その他の構成については図２３と同一のため、説明は省略する。
図２４はこの発明の実施の形態３３によるロボット制御装置を示すブロック図であり、図において、６９は手先負荷設定値読み出し手段、７０は平滑化手段である。
その他の構成については図２３と同一のため、説明は省略する。
次に動作について説明する。
手先負荷設定値読み出し手段６９では、時々刻々の手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを読み出して出力する。平滑化手段７０は、２段の移動平均フィルタを備えており、手先負荷設定値読み出し手段６９から出力される時々刻々の手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを前記２段の移動平均フィルタに通した結果を必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に出力する。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、平滑化手段７０から出力された手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを用いて運動方程式の演算を行う。
手先負荷設定値読み出し手段６９では、時々刻々の手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを読み出して出力する。平滑化手段７０は、２段の移動平均フィルタを備えており、手先負荷設定値読み出し手段６９から出力される時々刻々の手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを前記２段の移動平均フィルタに通した結果を必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に出力する。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、平滑化手段７０から出力された手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを用いて運動方程式の演算を行う。
実施の形態３４．
この発明の実施の形態３４によるロボット制御装置を示すブロック図は図１であり、衝突判別手段５以外の各手段の動作は、実施の形態２と同様のため、説明は省略する。
この発明の実施の形態３４によるロボット制御装置を示すブロック図は図１であり、衝突判別手段５以外の各手段の動作は、実施の形態２と同様のため、説明は省略する。
次に動作について説明する。
衝突判別手段５の内部には、衝突状態判別パラメータを設ける。衝突状態判別パラメータの初期値は０である。衝突状態判別パラメータの値が０の場合、衝突判別手段５では、モータ制御手段６から入力されるモータ電流から実際各軸のモータ駆動トルクを演算する。次に、必要駆動トルク演算手段３で演算した必要駆動トルクと実際のモータ駆動トルクの差を各軸について計算する。いずれかの軸における必要駆動トルクとモータ駆動トルクの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成部１に停止命令を送信すると共に、衝突状態判別パラメータの値を１とする。
衝突状態判別パラメータの値が１の場合も必要駆動トルクと実際のモータ駆動トルクの差を各軸について計算し、いずれかの軸における必要駆動トルクとモータ駆動トルクの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別するが、指令値生成部１に停止命令を送信することは行わない。衝突状態判別パラメータの値は制御装置の電源を遮断し再投入した場合と、プログラムもしくは制御装置の操作盤から衝突判別機能の再開が設定された場合に０に戻す。
衝突判別手段５の内部には、衝突状態判別パラメータを設ける。衝突状態判別パラメータの初期値は０である。衝突状態判別パラメータの値が０の場合、衝突判別手段５では、モータ制御手段６から入力されるモータ電流から実際各軸のモータ駆動トルクを演算する。次に、必要駆動トルク演算手段３で演算した必要駆動トルクと実際のモータ駆動トルクの差を各軸について計算する。いずれかの軸における必要駆動トルクとモータ駆動トルクの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成部１に停止命令を送信すると共に、衝突状態判別パラメータの値を１とする。
衝突状態判別パラメータの値が１の場合も必要駆動トルクと実際のモータ駆動トルクの差を各軸について計算し、いずれかの軸における必要駆動トルクとモータ駆動トルクの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別するが、指令値生成部１に停止命令を送信することは行わない。衝突状態判別パラメータの値は制御装置の電源を遮断し再投入した場合と、プログラムもしくは制御装置の操作盤から衝突判別機能の再開が設定された場合に０に戻す。
実施の形態３５．
この発明の実施の形態３５によるロボット制御装置を示すブロック図は図１であり、衝突判別手段５以外の各手段の動作は、実施の形態２と同様のため、説明は省略する。
この発明の実施の形態３５によるロボット制御装置を示すブロック図は図１であり、衝突判別手段５以外の各手段の動作は、実施の形態２と同様のため、説明は省略する。
次に動作について説明する。
衝突判別手段５の内部には、１段の移動平均フィルタ手段を設ける。衝突判別手段５に入力される必要駆動トルクを前記移動平均フィルタに通したときの出力をフィルタ後必要駆動トルクとする。次に、衝突判別手段５に入力されるモータ電流から実際の各軸モータ駆動トルクを算出し、算出した各軸モータ駆動トルクを前記移動平均フィルタに通したときの出力をフィルタ後モータ駆動トルクとする。フィルタ後必要駆動トルクとフィルタ後モータ駆動トルクの差を各軸について計算する。いずれかの軸におけるフィルタ後必要駆動トルクとフィルタ後モータ駆動トルクとの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成手段１に停止命令を送信する。
衝突判別手段５の内部には、１段の移動平均フィルタ手段を設ける。衝突判別手段５に入力される必要駆動トルクを前記移動平均フィルタに通したときの出力をフィルタ後必要駆動トルクとする。次に、衝突判別手段５に入力されるモータ電流から実際の各軸モータ駆動トルクを算出し、算出した各軸モータ駆動トルクを前記移動平均フィルタに通したときの出力をフィルタ後モータ駆動トルクとする。フィルタ後必要駆動トルクとフィルタ後モータ駆動トルクの差を各軸について計算する。いずれかの軸におけるフィルタ後必要駆動トルクとフィルタ後モータ駆動トルクとの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成手段１に停止命令を送信する。
実施の形態３６．
この発明の実施の形態３６によるロボット制御装置を示すブロック図は図１であり、指令値生成手段１以外の各手段の動作は、実施の形態３４と同様のため、説明は省略する。
この発明の実施の形態３６によるロボット制御装置を示すブロック図は図１であり、指令値生成手段１以外の各手段の動作は、実施の形態３４と同様のため、説明は省略する。
次に動作について説明する。
指令値生成手段１では、ロボットに実行させる各動作毎に０以上１以下の値を持つ媒介変数で位置指令を表現し、媒介変数を０から１まで滑らかに増加させていくことにより時々刻々の位置指令を生成する。衝突判別手段５より停止命令を受信すると、媒介変数の現在値Ｘcを記憶すると共に、単位時間当たりの媒介変数の増加量を滑らかに０まで減少させる。単位時間当たりの媒介変数の増加量が０になった地点の媒介変数値をＸsとする。指令値生成手段１には、媒介変数戻し量Ｘlを予め記憶しておく。Ｘc−Ｘlと０とを比較し、大きい方をＸtとする。単位時間当たりの媒介変数の増加量が０になった後、直ちに媒介変数をＸsからＸtまで滑らかに減少させることにより衝突状態から回避する位置指令を生成する。
指令値生成手段１では、ロボットに実行させる各動作毎に０以上１以下の値を持つ媒介変数で位置指令を表現し、媒介変数を０から１まで滑らかに増加させていくことにより時々刻々の位置指令を生成する。衝突判別手段５より停止命令を受信すると、媒介変数の現在値Ｘcを記憶すると共に、単位時間当たりの媒介変数の増加量を滑らかに０まで減少させる。単位時間当たりの媒介変数の増加量が０になった地点の媒介変数値をＸsとする。指令値生成手段１には、媒介変数戻し量Ｘlを予め記憶しておく。Ｘc−Ｘlと０とを比較し、大きい方をＸtとする。単位時間当たりの媒介変数の増加量が０になった後、直ちに媒介変数をＸsからＸtまで滑らかに減少させることにより衝突状態から回避する位置指令を生成する。
実施の形態３７．
図２５はこの発明の実施の形態３７によるロボット制御装置を示すブロック図であり、図において、７４は第１衝突判別手段、７５は第２衝突判別手段、７６は位置速度記憶手段、７７は過度反転判別手段である。
その他の構成については実施の形態１６と同一のため、説明は省略する。
図２５はこの発明の実施の形態３７によるロボット制御装置を示すブロック図であり、図において、７４は第１衝突判別手段、７５は第２衝突判別手段、７６は位置速度記憶手段、７７は過度反転判別手段である。
その他の構成については実施の形態１６と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、位置速度記憶手段７６に衝突検知信号を送信する。また、第２衝突判別手段７５では、位置速度記憶手段７６に衝突検知信号を送信する。
位置速度記憶手段７６は、第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、受信したときの各軸の関節変位と関節速度を記憶する。過度反転判別手段７７では、位置速度記憶手段７６に記憶された関節変位と関節速度を読み込み、モータ制御手段３３から入手する現在の関節変位が、位置速度記憶手段７６に記憶された関節変位から、位置速度記憶手段７６に記憶された関節速度とは逆向きに規定値以上離れた場合に、過度反転が発生したと判別し、モータ制御手段３３に即時停止命令を送信する。
第１衝突判別手段７４では、位置速度記憶手段７６に衝突検知信号を送信する。また、第２衝突判別手段７５では、位置速度記憶手段７６に衝突検知信号を送信する。
位置速度記憶手段７６は、第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、受信したときの各軸の関節変位と関節速度を記憶する。過度反転判別手段７７では、位置速度記憶手段７６に記憶された関節変位と関節速度を読み込み、モータ制御手段３３から入手する現在の関節変位が、位置速度記憶手段７６に記憶された関節変位から、位置速度記憶手段７６に記憶された関節速度とは逆向きに規定値以上離れた場合に、過度反転が発生したと判別し、モータ制御手段３３に即時停止命令を送信する。
実施の形態３８．
この発明の実施の形態３８によるロボット制御装置を示すブロック図は図２５であり、過渡反転判別手段７７以外の各手段の動作は、実施の形態３７と同様のため、説明は省略する。
この発明の実施の形態３８によるロボット制御装置を示すブロック図は図２５であり、過渡反転判別手段７７以外の各手段の動作は、実施の形態３７と同様のため、説明は省略する。
次に動作について説明する。
過度反転判別手段７７では、位置速度記憶手段７６に記憶された関節変位と関節速度を読み込み、モータ制御手段３３から入手する現在の関節速度が、位置速度記憶手段７６に記憶された関節速度とは逆向きでかつその絶対値が規定値以上の場合に、過度反転が発生したと判別し、モータ制御手段３３に即時停止命令を送信する。
過度反転判別手段７７では、位置速度記憶手段７６に記憶された関節変位と関節速度を読み込み、モータ制御手段３３から入手する現在の関節速度が、位置速度記憶手段７６に記憶された関節速度とは逆向きでかつその絶対値が規定値以上の場合に、過度反転が発生したと判別し、モータ制御手段３３に即時停止命令を送信する。
実施の形態３９．
図２６はこの発明の実施の形態３９によるロボット制御装置を示すブロック図であり、図において、７９は第１衝突判別手段、８０はモータ制御手段、８１は第２衝突判別手段、８２は第２指令値生成手段である。
その他の構成については実施の形態１６と同一のため、説明は省略する。
図２６はこの発明の実施の形態３９によるロボット制御装置を示すブロック図であり、図において、７９は第１衝突判別手段、８０はモータ制御手段、８１は第２衝突判別手段、８２は第２指令値生成手段である。
その他の構成については実施の形態１６と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７９では、まずモータ制御手段８０から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１及び第２指令値生成手段８２に停止命令を送信する。
第２衝突判別手段８１では、まずモータ制御手段８０で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段８１は停止命令を指令値生成手段１及び第２指令値生成手段８２に送信する。
第２指令値生成手段８２は、第１衝突判別手段７９もしくは第２衝突判別手段８１から停止命令を受信すると、ロボットを駆動する各軸ごとに現在速度から速やかに減速停止する減速停止指令を生成する。モータ制御手段８０では指令値生成手段１から送られてくる指令値を無視して、第２指令値生成手段８２で生成される減速停止指令に基づいてモータ制御を行う。
第１衝突判別手段７９では、まずモータ制御手段８０から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１及び第２指令値生成手段８２に停止命令を送信する。
第２衝突判別手段８１では、まずモータ制御手段８０で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段８１は停止命令を指令値生成手段１及び第２指令値生成手段８２に送信する。
第２指令値生成手段８２は、第１衝突判別手段７９もしくは第２衝突判別手段８１から停止命令を受信すると、ロボットを駆動する各軸ごとに現在速度から速やかに減速停止する減速停止指令を生成する。モータ制御手段８０では指令値生成手段１から送られてくる指令値を無視して、第２指令値生成手段８２で生成される減速停止指令に基づいてモータ制御を行う。
実施の形態４０．
図２７はこの発明の実施の形態４０によるロボット制御装置を示すブロック図であり、図において、８３は第１衝突判別手段、８４はモータ制御手段、８５は第２衝突判別手段、８６はトルク制限値演算手段である。
その他の構成については図２６と同一のため、説明は省略する。
図２７はこの発明の実施の形態４０によるロボット制御装置を示すブロック図であり、図において、８３は第１衝突判別手段、８４はモータ制御手段、８５は第２衝突判別手段、８６はトルク制限値演算手段である。
その他の構成については図２６と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段８３では、まずモータ制御手段８４から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させるとともにタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信すると共に、トルク制限値演算手段８６に衝突検知信号と衝突検知軸番号を送信する。
第２衝突判別手段８５では、まずモータ制御手段８４で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段８５は指令値生成手段１に停止命令を送信すると共に、トルク制限値演算手段８６に衝突検知信号と衝突検知軸番号を送信する。
トルク制限値演算手段８６の衝突検知信号受信前の動作は図３７のトルク制限値演算手段３２の動作と全く同一であるが、第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号及び衝突検知軸番号を受信すると、衝突検知軸番号と同一の番号の軸のみ上側トルク制限値及び下側トルク制限値を現在値に固定する。残りの軸の上側トルク制限値及び下側トルク制限値は衝突検知信号受信前と全く同一の手法で算出を続ける。
第１衝突判別手段８３では、まずモータ制御手段８４から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させるとともにタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信すると共に、トルク制限値演算手段８６に衝突検知信号と衝突検知軸番号を送信する。
第２衝突判別手段８５では、まずモータ制御手段８４で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段８５は指令値生成手段１に停止命令を送信すると共に、トルク制限値演算手段８６に衝突検知信号と衝突検知軸番号を送信する。
トルク制限値演算手段８６の衝突検知信号受信前の動作は図３７のトルク制限値演算手段３２の動作と全く同一であるが、第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号及び衝突検知軸番号を受信すると、衝突検知軸番号と同一の番号の軸のみ上側トルク制限値及び下側トルク制限値を現在値に固定する。残りの軸の上側トルク制限値及び下側トルク制限値は衝突検知信号受信前と全く同一の手法で算出を続ける。
実施の形態４１．
この発明の実施の形態４１によるロボット制御装置を示すブロック図は図２７であり、トルク制限値演算手段８６以外の各手段の動作は、実施の形態４０と同様のため、説明は省略する。
この発明の実施の形態４１によるロボット制御装置を示すブロック図は図２７であり、トルク制限値演算手段８６以外の各手段の動作は、実施の形態４０と同様のため、説明は省略する。
次に動作について説明する。
トルク制限値演算手段８６の衝突検知信号受信前の動作は、図２５のトルク制限値演算手段３２の動作と全く同一であるが、第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号及び衝突検知軸番号を受信すると、衝突検知軸番号と同一の番号の軸のみ必要駆動トルク指令要素演算手段２から送信されてくる重力指令トルク要素に上側トルク余裕値を加算したものを上側トルク制限値とし、重力指令トルク要素に下側トルク余裕値を加算したものを下側トルク制限値とする。ただし、突然切り替えるとトルク制限値がステップ上に変化するため、滑らかに切り替える。具体的には、衝突検知軸番号と同一の番号の軸に関しては、衝突検知信号受信後は、必要駆動トルク指令要素演算手段２から送信されてくるトルク指令要素の和に基づくトルク制限値と重力指令に基づくトルク制限値の重み付和をモータ電流制限手段３５に送信するトルク制限値とし、両者の重みの合計値を１に保ちながら、トルク指令要素の和に基づくトルク制限値の重みを１から０に滑らかに減少させると共に、重力指令に基づくトルク制限値の重みを０から１に滑らかに増加させる。
トルク制限値演算手段８６の衝突検知信号受信前の動作は、図２５のトルク制限値演算手段３２の動作と全く同一であるが、第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号及び衝突検知軸番号を受信すると、衝突検知軸番号と同一の番号の軸のみ必要駆動トルク指令要素演算手段２から送信されてくる重力指令トルク要素に上側トルク余裕値を加算したものを上側トルク制限値とし、重力指令トルク要素に下側トルク余裕値を加算したものを下側トルク制限値とする。ただし、突然切り替えるとトルク制限値がステップ上に変化するため、滑らかに切り替える。具体的には、衝突検知軸番号と同一の番号の軸に関しては、衝突検知信号受信後は、必要駆動トルク指令要素演算手段２から送信されてくるトルク指令要素の和に基づくトルク制限値と重力指令に基づくトルク制限値の重み付和をモータ電流制限手段３５に送信するトルク制限値とし、両者の重みの合計値を１に保ちながら、トルク指令要素の和に基づくトルク制限値の重みを１から０に滑らかに減少させると共に、重力指令に基づくトルク制限値の重みを０から１に滑らかに増加させる。
実施の形態４２．
この発明の実施の形態４２によるロボット制御装置を示すブロック図は図２７であり、トルク制限値演算手段８６以外の各手段の動作は、実施の形態４０と同様のため、説明は省略する。
この発明の実施の形態４２によるロボット制御装置を示すブロック図は図２７であり、トルク制限値演算手段８６以外の各手段の動作は、実施の形態４０と同様のため、説明は省略する。
次に動作について説明する。
トルク制限値演算手段８６の内部に衝突検知後のトルク余裕値倍率βをあらかじめ記憶しておく。第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号を受信するまでは、図２５のトルク制限値演算手段３２と全く同様に上側トルク制限値及び下側トルク制限値を計算する。第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号を受信した後は、上側トルク余裕値及び下側トルク余裕値にトルク余裕値倍率βを乗じてから、必要駆動トルク指令要素演算手段２から送信されてくるトルク指令要素の和に加算してモータ電流制限手段３５に送信するトルク制限値を算出する。ただし、突然切り替えるとトルク制限値がステップ上に変化するため、上側トルク余裕値及び下側トルク余裕値に乗じる値をいきなりβにするのではなく、滑らかにβに変化させる。
トルク制限値演算手段８６の内部に衝突検知後のトルク余裕値倍率βをあらかじめ記憶しておく。第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号を受信するまでは、図２５のトルク制限値演算手段３２と全く同様に上側トルク制限値及び下側トルク制限値を計算する。第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号を受信した後は、上側トルク余裕値及び下側トルク余裕値にトルク余裕値倍率βを乗じてから、必要駆動トルク指令要素演算手段２から送信されてくるトルク指令要素の和に加算してモータ電流制限手段３５に送信するトルク制限値を算出する。ただし、突然切り替えるとトルク制限値がステップ上に変化するため、上側トルク余裕値及び下側トルク余裕値に乗じる値をいきなりβにするのではなく、滑らかにβに変化させる。
実施の形態４３．
図２８はこの発明の実施の形態４３によるロボット制御装置を示すブロック図であり、図において、８９はしきい値演算手段、９０は衝突仮想判別手段、９１は判別データ蓄積手段、９２はモータ制御手段である。
その他の構成については実施の形態４２と同一のため、説明は省略する。
図２８はこの発明の実施の形態４３によるロボット制御装置を示すブロック図であり、図において、８９はしきい値演算手段、９０は衝突仮想判別手段、９１は判別データ蓄積手段、９２はモータ制御手段である。
その他の構成については実施の形態４２と同一のため、説明は省略する。
次に動作について説明する。
この実施の形態４３においては、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図２８は実施の形態４３におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４３におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
しきい値演算手段８９には、しきい値の算出に使用されるパラメータの値の候補が複数設定される。パラメータの値の候補設定は、図示しない手動操作盤、もしくはロボット動作を記述するプログラムから行われる。しきい値演算手段８９は設定されたパラメータとそのパラメータに基づいて算出されるしきい値を衝突仮想判別手段９０に送信する。
衝突仮想判別手段９０では、まず、モータ制御手段９２から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。次にしきい値演算手段８９から送信される複数のしきい値のそれぞれに対して演算した各軸のトルク差と各軸のしきい値のどちらが大きいか比較し、それぞれのしきい値に対して、各軸でトルク差がしきい値を連続して超えた回数である連続超過回数を算出する。算出した連続超過回数としきい値の算出に用いられたパラメータ値と軸番号は判別データ蓄積手段９１に送信される。
判別データ蓄積手段９１では、連続超過回数が、パラメータ値と軸番号ごとに保存される。保存されている連続超過回数よりも大きな値が連続超過回数として送信されてきた場合は保存されている値を書き換える。判別データ蓄積手段９１に蓄積されたパラメータ値、軸番号、連続超過回数は、図示しない手動操作盤やロボット制御装置に接続されたパーソナルコンピュータから閲覧できる。ロボット制御装置の管理者は閲覧したパラメータ値、軸番号、連続超過回数に基づいてしきい値演算手段８９で用いるパラメータ値を決定する。
この実施の形態４３においては、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図２８は実施の形態４３におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４３におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
しきい値演算手段８９には、しきい値の算出に使用されるパラメータの値の候補が複数設定される。パラメータの値の候補設定は、図示しない手動操作盤、もしくはロボット動作を記述するプログラムから行われる。しきい値演算手段８９は設定されたパラメータとそのパラメータに基づいて算出されるしきい値を衝突仮想判別手段９０に送信する。
衝突仮想判別手段９０では、まず、モータ制御手段９２から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。次にしきい値演算手段８９から送信される複数のしきい値のそれぞれに対して演算した各軸のトルク差と各軸のしきい値のどちらが大きいか比較し、それぞれのしきい値に対して、各軸でトルク差がしきい値を連続して超えた回数である連続超過回数を算出する。算出した連続超過回数としきい値の算出に用いられたパラメータ値と軸番号は判別データ蓄積手段９１に送信される。
判別データ蓄積手段９１では、連続超過回数が、パラメータ値と軸番号ごとに保存される。保存されている連続超過回数よりも大きな値が連続超過回数として送信されてきた場合は保存されている値を書き換える。判別データ蓄積手段９１に蓄積されたパラメータ値、軸番号、連続超過回数は、図示しない手動操作盤やロボット制御装置に接続されたパーソナルコンピュータから閲覧できる。ロボット制御装置の管理者は閲覧したパラメータ値、軸番号、連続超過回数に基づいてしきい値演算手段８９で用いるパラメータ値を決定する。
実施の形態４４．
図２９はこの発明の実施の形態４４によるロボット制御装置を示すブロック図であり、図において、９３はパラメータ決定手段である。
その他の構成については図２８と同一のため、説明は省略する。
図２９はこの発明の実施の形態４４によるロボット制御装置を示すブロック図であり、図において、９３はパラメータ決定手段である。
その他の構成については図２８と同一のため、説明は省略する。
次に動作について説明する。
この実施の形態４４においても、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図２９は、実施の形態４４におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４４におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
パラメータ決定手段９３では図示しない手動操作盤からパラメータ決定指令を受け取ると、ロボットを駆動する軸ごとにパラメータ値と連続超過回数データを判別データ蓄積手段９１より受信する。次にあらかじめ記憶されている許容最大連続回数とパラメータ値ごとの連続超過回数を比較し、連続超過回数が許容最大連続回数以下となるパラメータ値の中で最小のパラメータ値を求め、さらに、あらかじめ記憶されている補正値を加算したものをしきい値演算用パラメータとして、しきい値演算手段８９に送信する。
この実施の形態４４においても、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図２９は、実施の形態４４におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４４におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
パラメータ決定手段９３では図示しない手動操作盤からパラメータ決定指令を受け取ると、ロボットを駆動する軸ごとにパラメータ値と連続超過回数データを判別データ蓄積手段９１より受信する。次にあらかじめ記憶されている許容最大連続回数とパラメータ値ごとの連続超過回数を比較し、連続超過回数が許容最大連続回数以下となるパラメータ値の中で最小のパラメータ値を求め、さらに、あらかじめ記憶されている補正値を加算したものをしきい値演算用パラメータとして、しきい値演算手段８９に送信する。
実施の形態４５．
図３０はこの発明の実施の形態４５によるロボット制御装置を示すブロック図であり、図において、９４は感度設定手段である。
その他の構成については図２８と同一のため、説明は省略する。
図３０はこの発明の実施の形態４５によるロボット制御装置を示すブロック図であり、図において、９４は感度設定手段である。
その他の構成については図２８と同一のため、説明は省略する。
次に動作について説明する。
この実施の形態４５においても、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図３０は、実施の形態４５におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４５におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
まず、感度設定手段９４では図示しない手動操作盤から、感度候補値が複数設定され、設定された感度候補値は全てしきい値演算手段８９に送信される。
しきい値演算手段８９では、あらかじめ記憶されているパラメータに基づいて時々刻々のしきい値を算出する。次に算出したしきい値に感度候補値のそれぞれを乗算し、乗算結果と感度候補値の組み合わせを衝突仮想判別手段にすべて送信する。
衝突仮想判別手段９０では、まずモータ制御手段９２から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。次にしきい値演算手段８９から送信される複数のしきい値のそれぞれに対して演算した各軸のトルク差と各軸のしきい値のどちらが大きいか比較し、それぞれのしきい値に対して、各軸でトルク差がしきい値を連続して超えた回数である連続超過回数を算出する。算出した連続超過回数、軸番号としきい値の算出に用いられた感度候補値は判別データ蓄積手段９１に送信される。
判別データ蓄積手段９１では、連続超過回数が、軸番号、感度候補値ごとに保存される。保存されている連続超過回数よりも大きな値が連続超過回数として送信されてきた場合は保存されている値を書き換える。判別データ蓄積手段９１に蓄積された感度候補値、軸番号、連続超過回数は、図示しない手動操作盤やロボット制御装置に接続されたパーソナルコンピュータから閲覧できる。ロボット制御装置の管理者は閲覧した感度候補値、軸番号、連続超過回数に基づいて実際にロボットを動作させる際の感度設定値を決定する。
この実施の形態４５においても、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図３０は、実施の形態４５におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４５におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
まず、感度設定手段９４では図示しない手動操作盤から、感度候補値が複数設定され、設定された感度候補値は全てしきい値演算手段８９に送信される。
しきい値演算手段８９では、あらかじめ記憶されているパラメータに基づいて時々刻々のしきい値を算出する。次に算出したしきい値に感度候補値のそれぞれを乗算し、乗算結果と感度候補値の組み合わせを衝突仮想判別手段にすべて送信する。
衝突仮想判別手段９０では、まずモータ制御手段９２から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。次にしきい値演算手段８９から送信される複数のしきい値のそれぞれに対して演算した各軸のトルク差と各軸のしきい値のどちらが大きいか比較し、それぞれのしきい値に対して、各軸でトルク差がしきい値を連続して超えた回数である連続超過回数を算出する。算出した連続超過回数、軸番号としきい値の算出に用いられた感度候補値は判別データ蓄積手段９１に送信される。
判別データ蓄積手段９１では、連続超過回数が、軸番号、感度候補値ごとに保存される。保存されている連続超過回数よりも大きな値が連続超過回数として送信されてきた場合は保存されている値を書き換える。判別データ蓄積手段９１に蓄積された感度候補値、軸番号、連続超過回数は、図示しない手動操作盤やロボット制御装置に接続されたパーソナルコンピュータから閲覧できる。ロボット制御装置の管理者は閲覧した感度候補値、軸番号、連続超過回数に基づいて実際にロボットを動作させる際の感度設定値を決定する。
実施の形態４６．
図３１はこの発明の実施の形態４６によるロボット制御装置を示すブロック図であり、図において、９５は表示手段である。
その他の構成については図２５と同一のため、説明は省略する。
図３１はこの発明の実施の形態４６によるロボット制御装置を示すブロック図であり、図において、９５は表示手段である。
その他の構成については図２５と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。また表示手段９５に衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令を指令値生成手段１に送信する。また表示手段９５に衝突検知信号を送信する。
表示手段９５は衝突検知信号を受信すると、手動操作盤の画面を衝突検知機能切り替え専用画面に自動的に切り替える。衝突検知機能切り替え画面には、１を入力すると衝突検知機能を有効としたまま元の画面に戻り、０を入力すると衝突検知機能を無効にして元の画面に戻る旨記載されている。ロボット操作者は画面表示に従って手動操作盤を操作することにより衝突検知機能を直ちに無効にして、衝突により押し込み状態にあるロボットを迅速に退避させることができる。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。また表示手段９５に衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令を指令値生成手段１に送信する。また表示手段９５に衝突検知信号を送信する。
表示手段９５は衝突検知信号を受信すると、手動操作盤の画面を衝突検知機能切り替え専用画面に自動的に切り替える。衝突検知機能切り替え画面には、１を入力すると衝突検知機能を有効としたまま元の画面に戻り、０を入力すると衝突検知機能を無効にして元の画面に戻る旨記載されている。ロボット操作者は画面表示に従って手動操作盤を操作することにより衝突検知機能を直ちに無効にして、衝突により押し込み状態にあるロボットを迅速に退避させることができる。
実施の形態４７．
図３２はこの発明の実施の形態４７によるロボット制御装置を示すブロック図であり、図において、９６は衝突状態判別手段である。
その他の構成については図２５と同一のため、説明は省略する。
図３２はこの発明の実施の形態４７によるロボット制御装置を示すブロック図であり、図において、９６は衝突状態判別手段である。
その他の構成については図２５と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段９６に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段９６に停止命令と衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令と衝突検知信号を衝突状態判別手段９６に送信する。
衝突状態判別手段９６には衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令はそのまま指令値生成手段１に出力すると共に衝突判別フラグの値を１に変更する。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段１に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段９６に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段９６に停止命令と衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令と衝突検知信号を衝突状態判別手段９６に送信する。
衝突状態判別手段９６には衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令はそのまま指令値生成手段１に出力すると共に衝突判別フラグの値を１に変更する。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段１に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。
実施の形態４８．
この発明の実施の形態４８によるロボット制御装置を示すブロック図は図３１であり、指令値生成手段１以外の各手段の動作は、実施の形態４６と同様のため、説明は省略する。
この発明の実施の形態４８によるロボット制御装置を示すブロック図は図３１であり、指令値生成手段１以外の各手段の動作は、実施の形態４６と同様のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。また指令値生成手段１及び表示手段９５に衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令を指令値生成手段１に送信する。また指令値生成手段１及び表示手段９５に衝突検知信号を送信する。
指令値生成手段１では、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止信号を受信すると直ちに減速停止指令曲線を生成する。第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、指令値生成手段１内部に備えてあるリングバッファ（記憶手段）からあらかじめ規定された時間前の位置指令を読み出し、読み出した位置を衝突退避動作目標地点として記憶する。手動操作盤の規定されたボタンが押し込まれることにより衝突退避指令が入力されると、減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点とする直線動作指令を、あらかじめ規定された低速度で生成する。
表示手段９５は衝突検知信号を受信すると、手動操作盤の画面を衝突退避動作専用画面に自動的に切り替える。衝突退避動作専用画面では、衝突退避動作を指令するために押さねばならないボタンの番号が示される。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。また指令値生成手段１及び表示手段９５に衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令を指令値生成手段１に送信する。また指令値生成手段１及び表示手段９５に衝突検知信号を送信する。
指令値生成手段１では、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止信号を受信すると直ちに減速停止指令曲線を生成する。第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、指令値生成手段１内部に備えてあるリングバッファ（記憶手段）からあらかじめ規定された時間前の位置指令を読み出し、読み出した位置を衝突退避動作目標地点として記憶する。手動操作盤の規定されたボタンが押し込まれることにより衝突退避指令が入力されると、減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点とする直線動作指令を、あらかじめ規定された低速度で生成する。
表示手段９５は衝突検知信号を受信すると、手動操作盤の画面を衝突退避動作専用画面に自動的に切り替える。衝突退避動作専用画面では、衝突退避動作を指令するために押さねばならないボタンの番号が示される。
実施の形態４９．
この実施の形態４９におけるロボット制御方式の構成を示すブロック図も図３１であり、指令値生成手段１以外の各要素は実施の形態４８の各要素と同一なので説明は省略する。
この実施の形態４９におけるロボット制御方式の構成を示すブロック図も図３１であり、指令値生成手段１以外の各要素は実施の形態４８の各要素と同一なので説明は省略する。
次に動作について説明する。
指令値生成手段１では、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止信号を受信すると直ちに減速停止指令曲線を生成する。第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、衝突検知信号受信時に実行している動作が１つの場合は当該動作の始点を衝突退避動作目標地点として記憶する。衝突検知信号受信時に実行している動作が２つ以上ある場合は、最新の動作が実際に開始された地点を算出し、算出した地点を衝突退避動作目標地点として記憶する。手動操作盤の規定されたボタンが押し込まれることにより衝突退避指令が入力されると、衝突検知信号受信時に実行している動作が１つの場合は、当該動作と同じ補間方法で減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点する動作をあらかじめ規定された低速度で生成する。衝突検知信号受信時に実行している動作が２つ以上ある場合は、減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点とする直線動作指令を、あらかじめ規定された低速度で生成する。
指令値生成手段１では、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止信号を受信すると直ちに減速停止指令曲線を生成する。第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、衝突検知信号受信時に実行している動作が１つの場合は当該動作の始点を衝突退避動作目標地点として記憶する。衝突検知信号受信時に実行している動作が２つ以上ある場合は、最新の動作が実際に開始された地点を算出し、算出した地点を衝突退避動作目標地点として記憶する。手動操作盤の規定されたボタンが押し込まれることにより衝突退避指令が入力されると、衝突検知信号受信時に実行している動作が１つの場合は、当該動作と同じ補間方法で減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点する動作をあらかじめ規定された低速度で生成する。衝突検知信号受信時に実行している動作が２つ以上ある場合は、減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点とする直線動作指令を、あらかじめ規定された低速度で生成する。
実施の形態５０．
図３３はこの発明の実施の形態５０によるロボット制御装置を示すブロック図であり、図において、９８は衝突状態判別手段、９９は指令値生成手段、１００は表示手段である。
その他の構成については図２５と同一のため、説明は省略する。
図３３はこの発明の実施の形態５０によるロボット制御装置を示すブロック図であり、図において、９８は衝突状態判別手段、９９は指令値生成手段、１００は表示手段である。
その他の構成については図２５と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段９８に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段９８に衝突検知信号と停止命令を送信する。また衝突と判定された軸の番号である衝突検知軸番号と、その軸の差トルクの符号である衝突検知方向を衝突状態判別手段９８に送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は衝突検知信号と停止命令を衝突状態判別手段９８に送信する。また衝突と判定された軸の番号である衝突検知軸番号と、その軸の差トルクの符号である衝突検知方向を衝突状態判別手段９８に送信する。
衝突状態判別手段９８には衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号はそのまま指令値生成手段９９に出力すると共に衝突判別フラグの値を１に変更する。衝突検知信号は表示手段１００にも送信される。また、受信した衝突検知軸番号と衝突検知方向を指令値生成手段９９及び表示手段１００に送信する。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段９９に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。衝突判別フラグが１から０に変わると衝突状態脱出信号を指令値生成手段９９及び表示手段１００に送信する。
指令値生成手段９９では、衝突状態判別手段９８から停止信号及び衝突検知信号を受信すると直ちに減速停止指令曲線を生成する。減速停止指令曲線の生成完了後に、衝突検知軸番号と同一の軸の指令方向チェックフラグを１にする。指令方向チェックフラグが１になっている軸の指令方向が衝突検知方向と一致する場合は、生成した指令値を指令値生成手段９９の外部には出力せず、指令値生成周期の前回周期の値を外部に出力すると共に、表示手段１００に動作方向警告信号を送信する。指令方向チェックフラグは衝突状態脱出信号を受信すると０に戻される。
表示手段１００では、衝突状態判別手段９８から、衝突検知信号、衝突検知軸番号、衝突検知方向を受信すると、手動操作盤の画面を衝突状態表示画面に切り替える。衝突状態表示画面では、衝突検知軸番号及び衝突検知方向が表示される。また、指令値生成手段９９から動作方向警告信号を受信すると警告音を発生すると共に衝突検知軸番号及び衝突検知方向を点滅させながら手動操作盤の画面に表示させる。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段９８に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段９８に衝突検知信号と停止命令を送信する。また衝突と判定された軸の番号である衝突検知軸番号と、その軸の差トルクの符号である衝突検知方向を衝突状態判別手段９８に送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は衝突検知信号と停止命令を衝突状態判別手段９８に送信する。また衝突と判定された軸の番号である衝突検知軸番号と、その軸の差トルクの符号である衝突検知方向を衝突状態判別手段９８に送信する。
衝突状態判別手段９８には衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号はそのまま指令値生成手段９９に出力すると共に衝突判別フラグの値を１に変更する。衝突検知信号は表示手段１００にも送信される。また、受信した衝突検知軸番号と衝突検知方向を指令値生成手段９９及び表示手段１００に送信する。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段９９に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。衝突判別フラグが１から０に変わると衝突状態脱出信号を指令値生成手段９９及び表示手段１００に送信する。
指令値生成手段９９では、衝突状態判別手段９８から停止信号及び衝突検知信号を受信すると直ちに減速停止指令曲線を生成する。減速停止指令曲線の生成完了後に、衝突検知軸番号と同一の軸の指令方向チェックフラグを１にする。指令方向チェックフラグが１になっている軸の指令方向が衝突検知方向と一致する場合は、生成した指令値を指令値生成手段９９の外部には出力せず、指令値生成周期の前回周期の値を外部に出力すると共に、表示手段１００に動作方向警告信号を送信する。指令方向チェックフラグは衝突状態脱出信号を受信すると０に戻される。
表示手段１００では、衝突状態判別手段９８から、衝突検知信号、衝突検知軸番号、衝突検知方向を受信すると、手動操作盤の画面を衝突状態表示画面に切り替える。衝突状態表示画面では、衝突検知軸番号及び衝突検知方向が表示される。また、指令値生成手段９９から動作方向警告信号を受信すると警告音を発生すると共に衝突検知軸番号及び衝突検知方向を点滅させながら手動操作盤の画面に表示させる。
実施の形態５１．
図３４はこの発明の実施の形態５１によるロボット制御装置を示すブロック図であり、図において、１０１は衝突状態判別手段である。
その他の構成については図３３と同一のため、説明は省略する。
図３４はこの発明の実施の形態５１によるロボット制御装置を示すブロック図であり、図において、１０１は衝突状態判別手段である。
その他の構成については図３３と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段１０１に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段１０１に衝突検知信号と停止命令を送信する。また、衝突と判別した瞬間の各軸のトルク差を衝突作用トルクとして衝突状態判別手段１０１に送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は衝突検知信号と停止命令を衝突状態判別手段１０１に送信する。また、衝突と判別した瞬間の各軸のトルク差を衝突作用トルクとして衝突状態判別手段１０１に送信する。
衝突状態判別手段１０１は衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号をそのまま指令値生成手段９９に出力すると共に衝突判別フラグの値を１に変更する。衝突検知信号は表示手段１００にも送信される。また、受信した衝突作用トルクとモータ制御手段３３から入力される関節変位から直交座標系における衝突方向を下記の手順で算出する。
まず、各軸の衝突作用トルクから構成される衝突作用トルクベクトルをτc、ロボット手先で見た衝突力ベクトルをＦcとすると、
Ｆc＝（ＪｖT）-1τc ・・・（３２）
とかける。ここで、（ＪｖT）-1はヤコビ行列と呼ばれる関節変位の関数となっている行列で、各軸の速度から構成される各軸速度ベクトルをｖ、手先の速度から構成される手先速度ベクトルをｖxとすると、
ｖx＝Ｊvｖ ・・・（３３）
なる関係がある。（３２）式に基づいて衝突力ベクトルＦcを算出し、ＦcのＸ、Ｙ、Ｚ成分の中で絶対値が規定値以上のものがあれば、その成分の方向を直交座標系における衝突方向とする。例えば、のＸ成分が＋方向に規定値以上となっている場合は、衝突方向をＸ軸＋方向とする。算出した衝突方向は指令値生成手段９９及び表示手段１００に送信される。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段９９に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。衝突判別フラグが１から０に変わると衝突状態脱出信号を指令値生成手段９９及び表示手段１００に送信する。
指令値生成手段９９では、衝突状態判別手段１０１から停止信号及び衝突検知信号を受信すると直ちに減速停止指令曲線を生成する。減速停止指令曲線の生成完了後に、衝突方向の指令方向チェックフラグを１にする。指令方向チェックフラグが１になっている直交座標成分の指令方向が、衝突方向と同一になっている場合は、生成した指令値を指令値生成手段９９の外部には出力せず、指令値生成周期の前回周期の値を外部に出力すると共に、表示手段１００に動作方向警告信号を送信する。指令方向チェックフラグは衝突状態脱出信号を受信すると０に戻される。
表示手段１００では、衝突状態判別手段１０１から、衝突検知信号、衝突方向を受信すると、手動操作盤の画面を衝突状態表示画面に切り替える。衝突状態表示画面では、衝突方向が表示される。また、指令値生成手段９９から動作方向警告信号を受信すると警告音を発生すると共に衝突方向を点滅させながら手動操作盤の画面に表示させる。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段１０１に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段１０１に衝突検知信号と停止命令を送信する。また、衝突と判別した瞬間の各軸のトルク差を衝突作用トルクとして衝突状態判別手段１０１に送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は衝突検知信号と停止命令を衝突状態判別手段１０１に送信する。また、衝突と判別した瞬間の各軸のトルク差を衝突作用トルクとして衝突状態判別手段１０１に送信する。
衝突状態判別手段１０１は衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号をそのまま指令値生成手段９９に出力すると共に衝突判別フラグの値を１に変更する。衝突検知信号は表示手段１００にも送信される。また、受信した衝突作用トルクとモータ制御手段３３から入力される関節変位から直交座標系における衝突方向を下記の手順で算出する。
まず、各軸の衝突作用トルクから構成される衝突作用トルクベクトルをτc、ロボット手先で見た衝突力ベクトルをＦcとすると、
Ｆc＝（ＪｖT）-1τc ・・・（３２）
とかける。ここで、（ＪｖT）-1はヤコビ行列と呼ばれる関節変位の関数となっている行列で、各軸の速度から構成される各軸速度ベクトルをｖ、手先の速度から構成される手先速度ベクトルをｖxとすると、
ｖx＝Ｊvｖ ・・・（３３）
なる関係がある。（３２）式に基づいて衝突力ベクトルＦcを算出し、ＦcのＸ、Ｙ、Ｚ成分の中で絶対値が規定値以上のものがあれば、その成分の方向を直交座標系における衝突方向とする。例えば、のＸ成分が＋方向に規定値以上となっている場合は、衝突方向をＸ軸＋方向とする。算出した衝突方向は指令値生成手段９９及び表示手段１００に送信される。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段９９に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。衝突判別フラグが１から０に変わると衝突状態脱出信号を指令値生成手段９９及び表示手段１００に送信する。
指令値生成手段９９では、衝突状態判別手段１０１から停止信号及び衝突検知信号を受信すると直ちに減速停止指令曲線を生成する。減速停止指令曲線の生成完了後に、衝突方向の指令方向チェックフラグを１にする。指令方向チェックフラグが１になっている直交座標成分の指令方向が、衝突方向と同一になっている場合は、生成した指令値を指令値生成手段９９の外部には出力せず、指令値生成周期の前回周期の値を外部に出力すると共に、表示手段１００に動作方向警告信号を送信する。指令方向チェックフラグは衝突状態脱出信号を受信すると０に戻される。
表示手段１００では、衝突状態判別手段１０１から、衝突検知信号、衝突方向を受信すると、手動操作盤の画面を衝突状態表示画面に切り替える。衝突状態表示画面では、衝突方向が表示される。また、指令値生成手段９９から動作方向警告信号を受信すると警告音を発生すると共に衝突方向を点滅させながら手動操作盤の画面に表示させる。
実施の形態５２．
図３５はこの発明の実施の形態５２によるロボット制御装置を示すブロック図であり、図において、１０２はモード判別手段である。
その他の構成については図３１と同一のため、説明は省略する。
図３５はこの発明の実施の形態５２によるロボット制御装置を示すブロック図であり、図において、１０２はモード判別手段である。
その他の構成については図３１と同一のため、説明は省略する。
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、モード判別手段１０２に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、モード判別手段１０２に停止命令と衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令と衝突検知信号をモード判別手段１０２に送信する。
モード判別手段１０２には、モードフラグが存在し、現在教示モードであるならばモードフラグの値は１、自動モードであるならばモードフラグは０となっている。モードフラグが１の時は、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令を指令値生成手段９７には出力しない。衝突検知信号を受信してから衝突状態脱出信号を受信するまでの間は表示手段１００に衝突検知信号を出力する。モードフラグが０の時は第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号を指令値生成手段９７に出力する。また、衝突検知信号を受信してから衝突状態脱出信号を受信するまでの間は表示手段１００に衝突検知信号を出力する。
表示手段１００では、モード判別手段１０２から衝突検知信号を受信している間は警告音を発生する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、モード判別手段１０２に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、モード判別手段１０２に停止命令と衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令と衝突検知信号をモード判別手段１０２に送信する。
モード判別手段１０２には、モードフラグが存在し、現在教示モードであるならばモードフラグの値は１、自動モードであるならばモードフラグは０となっている。モードフラグが１の時は、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令を指令値生成手段９７には出力しない。衝突検知信号を受信してから衝突状態脱出信号を受信するまでの間は表示手段１００に衝突検知信号を出力する。モードフラグが０の時は第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号を指令値生成手段９７に出力する。また、衝突検知信号を受信してから衝突状態脱出信号を受信するまでの間は表示手段１００に衝突検知信号を出力する。
表示手段１００では、モード判別手段１０２から衝突検知信号を受信している間は警告音を発生する。
実施の形態５３．
この実施の形態５３におけるロボット制御方式の構成を示すブロック図は図２５であり、必要駆動トルク演算手段３以外の各要素は実施の形態３７の各要素と同一なので説明は省略する。
この実施の形態５３におけるロボット制御方式の構成を示すブロック図は図２５であり、必要駆動トルク演算手段３以外の各要素は実施の形態３７の各要素と同一なので説明は省略する。
次に動作について説明する。
必要駆動トルク演算手段３では必要駆動トルクτeを
τe＝τme＋τhe＋τgd＋τfe ・・・（３４）
で算出する。（１０）式と異なるのは最後のτfeのみなので、以下でτfeの算出について説明する。まず、τfeは摩擦力ベクトルであり、
τfe＝τfb＋τfc ・・・（３５）
で算出される。各軸の粘性摩擦係数をｆbi、クーロン摩擦係数をfci、各軸の現在速度をｖiとすると、
τfbi＝ｆbi＊ｖi ・・・（３６）
τfc0i＝ｆci （ｖi＞０の時）・・・（３７）
τfc0i＝０ （ｖi＝０の時）・・・（３８）
τfc0i＝−ｆci （ｖi＜０の時）・・・（３９）
で算出され、τfc0iを１次遅れフィルタに通した結果をτfciとし、各軸のτfciを集めたベクトルをτfcとし、各軸のτfbiを集めたベクトルをτfbとする。
必要駆動トルク演算手段３では必要駆動トルクτeを
τe＝τme＋τhe＋τgd＋τfe ・・・（３４）
で算出する。（１０）式と異なるのは最後のτfeのみなので、以下でτfeの算出について説明する。まず、τfeは摩擦力ベクトルであり、
τfe＝τfb＋τfc ・・・（３５）
で算出される。各軸の粘性摩擦係数をｆbi、クーロン摩擦係数をfci、各軸の現在速度をｖiとすると、
τfbi＝ｆbi＊ｖi ・・・（３６）
τfc0i＝ｆci （ｖi＞０の時）・・・（３７）
τfc0i＝０ （ｖi＝０の時）・・・（３８）
τfc0i＝−ｆci （ｖi＜０の時）・・・（３９）
で算出され、τfc0iを１次遅れフィルタに通した結果をτfciとし、各軸のτfciを集めたベクトルをτfcとし、各軸のτfbiを集めたベクトルをτfbとする。
実施の形態５４．
図３６はこの発明の実施の形態５４によるロボット制御装置を示すブロック図であり、図において、１０３はトルク指令絶対値最大値演算手段である。
その他の構成については図３１と同一のため、説明は省略する。
図３６はこの発明の実施の形態５４によるロボット制御装置を示すブロック図であり、図において、１０３はトルク指令絶対値最大値演算手段である。
その他の構成については図３１と同一のため、説明は省略する。
次に動作について説明する。
トルク指令絶対値最大値演算手段１０３には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算し、加算結果の絶対値を求め、求めた絶対値を各軸の減速比、トルク定数を考慮して各軸のモータ電流指令のスケールに換算する。さらに各軸モータの定格電流に基づいて正規化する。各軸毎に算出した各軸モータの定格電流で正規化した慣性力指令と遠心コリオリ力指令の和の絶対値の中から最大のものをＴmaxとして出力する。
しきい値演算手段２８には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
トルク指令絶対値最大値演算手段１０３には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdが入力される。次に慣性力指令要素ベクトルτmd、遠心コリオリ力指令要素ベクトルτhdの各軸要素を加算し、加算結果の絶対値を求め、求めた絶対値を各軸の減速比、トルク定数を考慮して各軸のモータ電流指令のスケールに換算する。さらに各軸モータの定格電流に基づいて正規化する。各軸毎に算出した各軸モータの定格電流で正規化した慣性力指令と遠心コリオリ力指令の和の絶対値の中から最大のものをＴmaxとして出力する。
しきい値演算手段２８には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
実施の形態５５．
図３７はこの発明の実施の形態５５によるロボット制御装置を示すブロック図であり、図において、１０４は加減速方向重み決定手段、１０５はトルク指令絶対値最大値演算手段、１０６はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
図３７はこの発明の実施の形態５５によるロボット制御装置を示すブロック図であり、図において、１０４は加減速方向重み決定手段、１０５はトルク指令絶対値最大値演算手段、１０６はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
次に動作について説明する。
加減速方向重み決定手段１０４には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmdが入力される。次にτmdの各軸の符号を求め、求めた符号に応じて加減速方向重みを決定する。具体的にはτmdの各軸の符号と同じ方向の重みをｋ１、逆方向の重みをｋ２とする。例えば第２軸のτmdの符号が＋ならば、第２軸の上側しきい値にかける重みｕｋの値をｋ１、下側しきい値にかける重みｌｋの値をｋ２とする。各軸ごとの加減速方向重みはしきい値演算手段１０６に出力される。
しきい値演算手段１０６には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０５から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。さらに設定された上側低レベルしきい値、及び上側高レベルしきい値のそれぞれに加減速方向重み決定手段１０４から入力された加減速方向重みｕｋが乗じられる。また、下側低レベルしきい値、及び下側高レベルしきい値のそれぞれに加減速方向重み決定手段１０４から入力された加減速方向重みｌｋが乗じられる。加減速方向重みが乗じられた上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、加減速方向重みが乗じられた上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
加減速方向重み決定手段１０４には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmdが入力される。次にτmdの各軸の符号を求め、求めた符号に応じて加減速方向重みを決定する。具体的にはτmdの各軸の符号と同じ方向の重みをｋ１、逆方向の重みをｋ２とする。例えば第２軸のτmdの符号が＋ならば、第２軸の上側しきい値にかける重みｕｋの値をｋ１、下側しきい値にかける重みｌｋの値をｋ２とする。各軸ごとの加減速方向重みはしきい値演算手段１０６に出力される。
しきい値演算手段１０６には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０５から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。さらに設定された上側低レベルしきい値、及び上側高レベルしきい値のそれぞれに加減速方向重み決定手段１０４から入力された加減速方向重みｕｋが乗じられる。また、下側低レベルしきい値、及び下側高レベルしきい値のそれぞれに加減速方向重み決定手段１０４から入力された加減速方向重みｌｋが乗じられる。加減速方向重みが乗じられた上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、加減速方向重みが乗じられた上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
実施の形態５６．
図３８はこの発明の実施の形態５６によるロボット制御装置を示すブロック図であり、図において、１０７はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
図３８はこの発明の実施の形態５６によるロボット制御装置を示すブロック図であり、図において、１０７はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
次に動作について説明する。
しきい値演算手段１０７には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。
しきい値演算手段１０７には、モータ制御手段３３から各軸のモータ速度も入力される。しきい値演算手段１０７では各軸モータ速度の現在値と前回の各軸モータ速度より、各軸モータ加速度を算出する。しきい値演算手段１０７には、モータ加速度の絶対値と衝突判別時間のテーブルがあらかじめ作成されており、このテーブルを参照することにより各軸の衝突判別時間が設定される。
設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
しきい値演算手段１０７には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。
しきい値演算手段１０７には、モータ制御手段３３から各軸のモータ速度も入力される。しきい値演算手段１０７では各軸モータ速度の現在値と前回の各軸モータ速度より、各軸モータ加速度を算出する。しきい値演算手段１０７には、モータ加速度の絶対値と衝突判別時間のテーブルがあらかじめ作成されており、このテーブルを参照することにより各軸の衝突判別時間が設定される。
設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
実施の形態５７．
図３９はこの発明の実施の形態５７によるロボット制御装置を示すブロック図であり、図において、１０８はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
図３９はこの発明の実施の形態５７によるロボット制御装置を示すブロック図であり、図において、１０８はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
次に動作について説明する。
しきい値演算手段１０８には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。
しきい値演算手段１０８には、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmdも入力される。次にτmdの各軸の絶対値が算出される。しきい値演算手段１０８には、τmdの各軸の絶対値と衝突判別時間のテーブルがあらかじめ作成されており、このテーブルを参照することにより各軸の衝突判別時間が設定される。
設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
しきい値演算手段１０８には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴmaxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔmax、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔmaxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。
しきい値演算手段１０８には、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτmdも入力される。次にτmdの各軸の絶対値が算出される。しきい値演算手段１０８には、τmdの各軸の絶対値と衝突判別時間のテーブルがあらかじめ作成されており、このテーブルを参照することにより各軸の衝突判別時間が設定される。
設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
１，１８，９９ 指令値生成手段、２ 必要駆動トルク指令要素演算手段、３，１２，１５ 必要駆動トルク演算手段、４，２８，４８，５１，５３，５５，５７，５９，６１，６３，６５，８９，１０６〜１０８ しきい値演算手段、５，９，２７ 衝突判別手段、６，１０，１７，３３，８０，８４，９２ モータ制御手段、８ 加算手段、１１ 位置速度加速度推定手段、１４，１６ パラメータ推定手段、１９ パラメータ蓄積手段、２０ 同定ゲイン演算手段、２１ パラメータ推定値制限手段、２５ 同定ゲイン演算手段、２９，７４，７９，８３ 第１衝突判別手段、３０，３７，７５，８１，８５ 第２衝突判別手段、３１ トルク余裕演算手段、３２，８６ トルク制限値演算手段、３４ モータ電流指令生成手段、３５ モータ電流制限手段（モータ駆動トルク制限手段）、３６ モータ電流制御手段、４７，４９ 同定進行度判別手段、５０ トルク指令絶対値最大値演算手段、５２ モータ電流絶対値最大値演算手段、５４ トルク指令トルク指令差分絶対値最大値演算手段、５６ トルク指令トルク指令差分絶対値演算手段、５８ 加速度指令加加速度指令絶対値演算手段、６０ しきい値倍率要素演算手段、６２ 停止判別手段、６４ 特殊作業判別手段、６６ プログラム解析手段、６７，７０ 平滑化手段、６８ トルク制限値保持手段、６９ 手先負荷設定値読み出し手段、７６ 位置速度記憶手段、７７ 過度反転判別手段、８２ 第２指令値生成手段、９０ 衝突仮想判別手段、９１ 判別データ蓄積手段、９３ パラメータ決定手段、９４ 感度設定手段、９５，１００ 表示手段、９６，９８，１０１ 衝突状態判別手段、１０２ モード判別手段、１０３，１０５ トルク指令絶対値最大値演算手段、１０４ 加速度方向重み決定手段。
Claims (2)
- ロボットの衝突を判別する衝突判別手段と、前記衝突判別手段による衝突判別時の位置及び速度を記憶する位置速度記憶手段と、前記位置速度記憶手段に記憶された衝突判別時の位置及び速度と現在の位置とに応じて、少なくとも１つの軸が衝突判別時の動作方向とは逆向きに規定値以上動作した場合にロボットが反転し過ぎていると判別し、ロボットを緊急停止させる過度反転判別手段とを備えたロボット制御装置。
- 過度反転判別手段は、位置速度記憶手段に記憶された衝突判別時の速度と現在の速度とに応じて、少なくとも１つの軸の速度が衝突判別時の動作方向とは逆向き、かつ、その大きさが規定値以上である場合にロボットが反転し過ぎていると判別することを特徴とする請求項１記載のロボット制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2006135450A JP2006263916A (ja) | 2001-05-08 | 2006-05-15 | ロボット制御装置 |
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001137837 | 2001-05-08 | ||
| JP2006135450A JP2006263916A (ja) | 2001-05-08 | 2006-05-15 | ロボット制御装置 |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002132952A Division JP3878054B2 (ja) | 2001-05-08 | 2002-05-08 | ロボット制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2006263916A true JP2006263916A (ja) | 2006-10-05 |
Family
ID=37200401
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2006135450A Pending JP2006263916A (ja) | 2001-05-08 | 2006-05-15 | ロボット制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2006263916A (ja) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008183680A (ja) * | 2007-01-31 | 2008-08-14 | Yaskawa Electric Corp | 負荷機械の制御装置とその衝突検出しきい値更新方法 |
| JP2009196030A (ja) * | 2008-02-21 | 2009-09-03 | Nachi Fujikoshi Corp | 産業用ロボットの出力トルク制限回路 |
| JP5927440B2 (ja) * | 2012-10-25 | 2016-06-01 | パナソニックＩｐマネジメント株式会社 | ロボットの異常表示方法 |
| JP2016179510A (ja) * | 2015-03-23 | 2016-10-13 | ファナック株式会社 | 物または人との接触を検知する機能を有するロボット制御装置 |
| WO2016185589A1 (ja) * | 2015-05-20 | 2016-11-24 | 日産自動車株式会社 | 故障診断装置及び故障診断方法 |
| JP2017019058A (ja) * | 2015-07-13 | 2017-01-26 | セイコーエプソン株式会社 | ロボット制御装置、ロボットおよびロボットシステム |
| JP2018083274A (ja) * | 2016-11-25 | 2018-05-31 | 株式会社東芝 | ロボット制御装置、ロボット制御方法及びピッキング装置 |
| US20210268657A1 (en) * | 2018-08-02 | 2021-09-02 | Kabushiki Kaisha Kobe Seiko Sho (Kobe Steel, Ltd.) | Robot control device, robot control method, and robot control program |
| CN114186896A (zh) * | 2021-12-16 | 2022-03-15 | 东莞先知大数据有限公司 | 一种走廊安全监管方法、电子设备和存储介质 |
| WO2022224425A1 (ja) * | 2021-04-23 | 2022-10-27 | ファナック株式会社 | 数値制御装置及び数値制御システム |
| JP7260728B1 (ja) * | 2022-11-07 | 2023-04-18 | ファナック株式会社 | 数値制御装置及び数値制御システム |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6186194A (ja) * | 1984-10-04 | 1986-05-01 | 株式会社日立製作所 | ロボツトの衝突回避制御方法 |
-
2006
- 2006-05-15 JP JP2006135450A patent/JP2006263916A/ja active Pending
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6186194A (ja) * | 1984-10-04 | 1986-05-01 | 株式会社日立製作所 | ロボツトの衝突回避制御方法 |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008183680A (ja) * | 2007-01-31 | 2008-08-14 | Yaskawa Electric Corp | 負荷機械の制御装置とその衝突検出しきい値更新方法 |
| JP2009196030A (ja) * | 2008-02-21 | 2009-09-03 | Nachi Fujikoshi Corp | 産業用ロボットの出力トルク制限回路 |
| US9701022B2 (en) | 2012-10-25 | 2017-07-11 | Panasonic Intellectual Property Management Co., Ltd. | Robot malfunction indication method |
| JP5927440B2 (ja) * | 2012-10-25 | 2016-06-01 | パナソニックＩｐマネジメント株式会社 | ロボットの異常表示方法 |
| JPWO2014064926A1 (ja) * | 2012-10-25 | 2016-09-08 | パナソニックＩｐマネジメント株式会社 | ロボットの異常表示方法 |
| US9776328B2 (en) | 2015-03-23 | 2017-10-03 | Fanuc Corporation | Robot control apparatus having function of detecting contact with object or person |
| JP2016179510A (ja) * | 2015-03-23 | 2016-10-13 | ファナック株式会社 | 物または人との接触を検知する機能を有するロボット制御装置 |
| WO2016185589A1 (ja) * | 2015-05-20 | 2016-11-24 | 日産自動車株式会社 | 故障診断装置及び故障診断方法 |
| JP2017019058A (ja) * | 2015-07-13 | 2017-01-26 | セイコーエプソン株式会社 | ロボット制御装置、ロボットおよびロボットシステム |
| JP2018083274A (ja) * | 2016-11-25 | 2018-05-31 | 株式会社東芝 | ロボット制御装置、ロボット制御方法及びピッキング装置 |
| US20210268657A1 (en) * | 2018-08-02 | 2021-09-02 | Kabushiki Kaisha Kobe Seiko Sho (Kobe Steel, Ltd.) | Robot control device, robot control method, and robot control program |
| US11813755B2 (en) * | 2018-08-02 | 2023-11-14 | Kobe Steel, Ltd. | Robot control device, robot control method, and robot control program |
| WO2022224425A1 (ja) * | 2021-04-23 | 2022-10-27 | ファナック株式会社 | 数値制御装置及び数値制御システム |
| CN114186896A (zh) * | 2021-12-16 | 2022-03-15 | 东莞先知大数据有限公司 | 一种走廊安全监管方法、电子设备和存储介质 |
| JP7260728B1 (ja) * | 2022-11-07 | 2023-04-18 | ファナック株式会社 | 数値制御装置及び数値制御システム |
| WO2024100721A1 (ja) * | 2022-11-07 | 2024-05-16 | ファナック株式会社 | 数値制御装置及び数値制御システム |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3878054B2 (ja) | ロボット制御装置 | |
| JP2006263916A (ja) | ロボット制御装置 | |
| CN107813345B (zh) | 机器人碰撞检测方法及装置 | |
| US10350765B2 (en) | Switching a control of a robot into a manual operating mode | |
| JP6706741B2 (ja) | ロボットの衝突検出方法 | |
| JP6238021B2 (ja) | ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム | |
| JP4109280B2 (ja) | サーボモータによって駆動制御される可動部を有する機械 | |
| US11040449B2 (en) | Robot control system and method of controlling a robot | |
| JP2732159B2 (ja) | 異常負荷検出方法 | |
| CN109291055B (zh) | 机器人运动控制方法、装置、计算机设备和存储介质 | |
| JP4261470B2 (ja) | 制御装置 | |
| WO2015087455A1 (ja) | 数値制御装置および数値制御方法 | |
| JP2013180204A (ja) | 掃除システムおよびその制御方法 | |
| JP2009090462A (ja) | ロボットの制御方法および制御装置 | |
| JP2003236787A (ja) | 駆動制御方法および駆動制御装置 | |
| JP3367641B2 (ja) | ロボットの制御装置 | |
| US10583558B2 (en) | Robot control device | |
| CN112809667B (zh) | 工业机器人的力控制方法、装置及其应用 | |
| JP6731583B2 (ja) | モータ制御装置 | |
| JP4440072B2 (ja) | ロボットの制御方法 | |
| KR101307782B1 (ko) | 로봇의 직접 교시 및 재생 방법 및 이를 구현하는 로봇 제어 장치 | |
| JP2013169609A (ja) | ロボットの衝突検出方法 | |
| JPWO2005035205A1 (ja) | ロボットの制御装置 | |
| JP3204207B2 (ja) | ロボットの制御装置とその制御方法 | |
| JP5436160B2 (ja) | 力制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20070920 |
|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20080820 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20090616 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20090811 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20100209 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20100608 |