JP3878054B2 - Robot controller - Google Patents

Description

しきい値演算手段４には、手先負荷の属する各グループ毎の上側しきい値及び下側しきい値のテーブルを備えており、手先負荷設定値読み出し手段から入力されるグループ番号の上側しきい値及び下側しきい値をそれぞれ上側しきい値及び下側しきい値として出力する。
【００６２】
実施の形態２０．
この発明の実施の形態２０によるロボット制御装置を示すブロック図は図１であり、しきい値演算手段４以外の各手段の動作は、実施の形態１と同様のため、説明は省略する。
【００６３】
次に動作について説明する。
図示しない検出感度読み出し手段では、まず、現在の検出感度の設定値を読み出す。検出感度の設定はプログラム内に記載された命令を用いて、もしくは制御装置の手動操作盤から設定される。次に、検出感度読み出し手段では、読み出した検出感度から倍率αを計算する。例えば、検出感度Ｘ％のとき、倍率αはα＝１００／Ｘで算出する。算出した倍率は、しきい値演算手段４に出力される。しきい値演算手段４には、検出感度が１００％の時の上側しきい値及び下側しきい値が予め記憶されている。しきい値演算手段４では、記憶されている上側しきい値及び下側しきい値に検出感度読み出し手段から入力される倍率αを乗じたものをそれぞれ上側しきい値、下側しきい値として出力する。
【００６４】
実施の形態２１．
図１２はこの発明の実施の形態２１によるロボット制御装置を示すブロック図であり、図において、４７は同定進行度判別手段、４８はしきい値演算手段である。
その他の構成については図４と同一のため、説明は省略する。
【００６５】
次に動作について説明する。
同定進行度判別手段４７は、各軸の同定進行度から構成される同定進行度ベクトルを出力する。まず、同定進行度判別手段４７には各軸の速度が入力される。同定進行度判別手段４７は、予め各軸の速度規定値及び各軸の時間規定値が記憶されている。同定進行度判別手段４７では、軸毎に、速度が速度既定値以上となっている累積時間を算出しており、累積時間が記憶されている規定時間以上となった場合、当該軸の同定進行度を１とする。累積時間が規定時間未満の場合は、当該軸の同定進行度は０とする。また、モータ電源が遮断されると、累積時間は０にリセットされる。
しきい値演算手段４８の内部には、パラメータ同定進行前の上側しきい値及び下側しきい値と、パラメータ同定進行後の上側しきい値及び下側しきい値が記憶されている。同定進行度判別手段４７から出力される同定進行度が１の軸に関しては、パラメータ同定進行後の上側しきい値及び下側しきい値が設定され、同定進行度判別手段４７から出力される同定進行度が０の軸に関しては、パラメータ同定進行前の上側しきい値及び下側しきい値が設定される。
【００６６】
実施の形態２２．
図１３はこの発明の実施の形態２２によるロボット制御装置を示すブロック図であり、図において、４９は同定進行度判別手段である。
その他の構成については図１２と同一のため、説明は省略する。
【００６７】
次に動作について説明する。
同定進行度判別手段４９には、まず、パラメータ推定手段１４からパラメータ同定周期毎のパラメータ同定値が入力される。同定進行度判別手段４９では、入力されたパラメータ同定値から各軸のクーロン摩擦係数と粘性摩擦係数を選択し、それぞれの変化量を算出する。同定進行度判別手段４９には各軸のクーロン摩擦係数推定値変化量及び粘性摩擦係数推定値変化量の規定値を記憶しておき、実際のクーロン摩擦係数変化量及び粘性摩擦係数変化量の両方が規定値以下の軸に関しては同定進行度を１とする。実際のクーロン摩擦係数変化量及び粘性摩擦係数変化量の少なくとも一方が規定値以上の軸は同定進行度を０とする。
【００６８】
実施の形態２３．
図１４はこの発明の実施の形態２３によるロボット制御装置を示すブロック図であり、図において、５０はトルク指令絶対値最大値演算手段、５１はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
【００６９】
次に動作について説明する。
トルク指令絶対値最大値演算手段５０には、まず、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτ_md、遠心コリオリ力指令要素ベクトルτ_hdが入力される。次に、慣性力指令要素ベクトルτ_md、遠心コリオリ力指令要素ベクトルτ_hdの各軸要素を加算し、加算結果の絶対値を求め、求めた絶対値を各軸の減速比、トルク定数を考慮して各軸のモータ電流指令のスケールに換算する。さらに、各軸モータの定格電流に基づいて正規化する。各軸毎に算出した各軸モータの定格電流で正規化した慣性力指令と遠心コリオリ力指令の和の絶対値の中から最大のものをＴ_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を算出する。次に、全軸一定の重み係数ｗを用いて第ｉ軸の要素が、前記（３０）式で算出されるから構成されるｗτ_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で算出される。
【００７８】
実施の形態２８．
図１９はこの発明の実施の形態２８によるロボット制御装置を示すブロック図であり、図において、６０はしきい値倍率要素演算手段、６１はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
【００７９】
次に動作について説明する。
しきい値倍率要素演算手段６０には、まず、位置指令、速度指令、加速度指令が入力される。速度指令及び加速度指令は減速比を考慮し、モータでの速度指令、加速度指令のスケールに変換される。さらに、スケール変換後の各軸の速度指令の最大値であるモータレベル速度指令最大値と、スケール変換後の各軸の加速度指令の最大値であるモータレベル加速度指令最大値を算出する。位置指令の属する範囲、モータレベル速度指令最大値、モータレベル加速度指令最大値のそれぞれに対応するしきい値倍率のテーブルをしきい値倍率要素演算手段６０の内部に用意しておき、現在の位置指令、速度指令、加速度指令に対応する倍率ｂｐ、ｂｖ、ｂａをテーブルから読み出して出力する。
しきい値演算手段６１には、倍率１の時の上側しきい値及び下側しきい値を各軸毎に記憶しておく。次に、しきい値倍率要素演算手段６０から出力される倍率ｂｐ、ｂｖ、ｂａから、各軸毎の倍率１の時の上側しきい値及び下側しきい値をそれぞれｂｐ×ｂｖ×ｂａ倍してから出力する。
【００８０】
実施の形態２９．
図２０はこの発明の実施の形態２９によるロボット制御装置を示すブロック図であり、図において、６２は停止判別手段、６３はしきい値演算手段である。
その他の構成については図１と同一のため、説明は省略する。
【００８１】
次に動作について説明する。
停止判別手段６２に、まず、速度指令及びモータ速度を入力する。停止判別手段６２は、軸毎に停止判別用規定値を記憶しており、速度指令及びモータ速度の両方を当該軸の停止判別用規定値と比較する。速度指令の各軸要素及びモータ速度の各軸要素の絶対値を算出し、速度指令絶対値及びモータ速度絶対値の両方が規定値以下の軸は停止中と判別し、当該軸の停止判別ベクトルの要素の値を１とする。速度指令絶対値及びモータ速度絶対値の少なくとも一方が規定値以上の場合は動作中と判別し、当該軸の停止判別ベクトルの要素の値を０とする。
しきい値演算手段６３の内部に、各軸の上側しきい値、下側しきい値、クーロン摩擦係数を記憶しておく。停止判別手段６２から出力される停止判別ベクトルの要素の値が０の軸に関しては、上側しきい値及び下側しきい値をそのまま出力する。停止判別手段６２から出力される停止判別ベクトルの要素の値が１の軸に関しては、記憶してある上側しきい値にクーロン摩擦係数を加算した値を上側しきい値として出力する。また、記憶してある下側しきい値にクーロン摩擦係数を減算した値を下側しきい値として出力する。
【００８２】
実施の形態３０．
図２１はこの発明の実施の形態３０によるロボット制御装置を示すブロック図であり、図において、６４は特殊作業判別手段、６５はしきい値演算手段、６６はプログラム解析手段である。
その他の構成については図１と同一のため、説明は省略する。
【００８３】
次に動作について説明する。
プログラム解析手段６６では、実行するプログラムを順次読み込んでいき、指令値生成手段１等に必要な情報を順次送信している。プログラム解析手段６６がハンド開閉命令を処理すると、図示しないハンド開閉手段にハンド開平命令を送信すると共に、特殊作業判別手段６４にもハンド開閉命令を送信する。特殊作業判別手段６４は、ハンド開閉命令を受信すると予め記憶している規定時間の間、特殊作業判別パラメータの値を１にする。特殊作業判別パラメータの初期値は０となっており、ハンド開閉命令受信後規定時間経過すると再び０に戻される。
しきい値演算手段６５には、通常作業用上側しきい値及び下側しきい値と特殊作業用上側しきい値及び下側しきい値が記憶されている。特殊作業判別手段６４から出力される特殊作業判別パラメータの値が１のときは、特殊作業用上側しきい値及び下側しきい値が設定され、特殊作業判別パラメータの値が０のときは、通常作業用上側しきい値及び下側しきい値が設定される。
【００８４】
実施の形態３１．
図２２はこの発明の実施の形態３１によるロボット制御装置を示すブロック図であり、図において、６７は平滑化手段である。
その他の構成については図１５と同一のため、説明は省略する。
【００８５】
次に動作について説明する。
平滑化手段６７は、トルク余裕演算手段３１から上側トルク余裕値及び下側トルク余裕値を順次受信する。平滑化手段６７の内部には２段の移動平均フィルタ手段が設けられており、受信した上側トルク余裕値及び下側トルク余裕値を順次２段の移動平均フィルタに通した結果を上側トルク余裕値及び下側トルク余裕値としてトルク制限値演算手段３２に送信する。２段の移動平均フィルタを通すことにより、しきい値演算手段２８で演算される低レベル上側しきい値及び低レベル下側しきい値がステップ状に変化する場合にもトルク制限値演算手段３２から出力される上側モータ電流制限値及び下側モータ電流制限値は滑らかに変化する。
【００８６】
実施の形態３２．
図２３はこの発明の実施の形態３２によるロボット制御装置を示すブロック図であり、図において、６８はトルク制限値保持手段である。
その他の構成については図２２と同一のため、説明は省略する。
【００８７】
次に動作について説明する。
トルク制限値保持手段６８には、通常状態では内部で保持している衝突状態判別パラメータの値が０になっており、前記衝突状態判別パラメータの値が０の時は、今回周期にトルク制限値演算手段３２から入力される上側トルク制限値及び下側トルク制限値をトルク制限値記憶エリアに書き込み記憶させる。また、トルク制限値演算手段３２から入力される上側トルク制限値及び下側トルク制限値をそのまま出力する。第１衝突判別手段２９もしくは第２衝突判別手段３７から衝突検知信号を受信すると衝突状態判別パラメータの値を１にする。衝突状態判別パラメータの値が１の時は、今回周期の上側トルク制限値及び下側トルク制限値の記憶エリアへの書き込みを停止し、記憶エリアに格納されている上側トルク制限値及び下側トルク制限値を出力する。衝突状態判別パラメータの値はモータ電源遮断時に０に戻される。
【００８８】
実施の形態３３．
図２４はこの発明の実施の形態３３によるロボット制御装置を示すブロック図であり、図において、６９は手先負荷設定値読み出し手段、７０は平滑化手段である。
その他の構成については図２３と同一のため、説明は省略する。
【００８９】
次に動作について説明する。
手先負荷設定値読み出し手段６９では、時々刻々の手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを読み出して出力する。平滑化手段７０は、２段の移動平均フィルタを備えており、手先負荷設定値読み出し手段６９から出力される時々刻々の手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを前記２段の移動平均フィルタに通した結果を必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３に出力する。必要駆動トルク指令要素演算手段２及び必要駆動トルク演算手段３では、平滑化手段７０から出力された手先負荷質量、手先負荷重心位置、手先負荷慣性モーメントを用いて運動方程式の演算を行う。
【００９０】
実施の形態３４．
この発明の実施の形態３４によるロボット制御装置を示すブロック図は図１であり、衝突判別手段５以外の各手段の動作は、実施の形態２と同様のため、説明は省略する。
【００９１】
次に動作について説明する。
衝突判別手段５の内部には、衝突状態判別パラメータを設ける。衝突状態判別パラメータの初期値は０である。衝突状態判別パラメータの値が０の場合、衝突判別手段５では、モータ制御手段６から入力されるモータ電流から実際各軸のモータ駆動トルクを演算する。次に、必要駆動トルク演算手段３で演算した必要駆動トルクと実際のモータ駆動トルクの差を各軸について計算する。いずれかの軸における必要駆動トルクとモータ駆動トルクの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成部１に停止命令を送信すると共に、衝突状態判別パラメータの値を１とする。
衝突状態判別パラメータの値が１の場合も必要駆動トルクと実際のモータ駆動トルクの差を各軸について計算し、いずれかの軸における必要駆動トルクとモータ駆動トルクの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別するが、指令値生成部１に停止命令を送信することは行わない。衝突状態判別パラメータの値は制御装置の電源を遮断し再投入した場合と、プログラムもしくは制御装置の操作盤から衝突判別機能の再開が設定された場合に０に戻す。
【００９２】
実施の形態３５．
この発明の実施の形態３５によるロボット制御装置を示すブロック図は図１であり、衝突判別手段５以外の各手段の動作は、実施の形態２と同様のため、説明は省略する。
【００９３】
次に動作について説明する。
衝突判別手段５の内部には、１段の移動平均フィルタ手段を設ける。衝突判別手段５に入力される必要駆動トルクを前記移動平均フィルタに通したときの出力をフィルタ後必要駆動トルクとする。次に、衝突判別手段５に入力されるモータ電流から実際の各軸モータ駆動トルクを算出し、算出した各軸モータ駆動トルクを前記移動平均フィルタに通したときの出力をフィルタ後モータ駆動トルクとする。フィルタ後必要駆動トルクとフィルタ後モータ駆動トルクの差を各軸について計算する。いずれかの軸におけるフィルタ後必要駆動トルクとフィルタ後モータ駆動トルクとの差が上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成手段１に停止命令を送信する。
【００９４】
実施の形態３６．
この発明の実施の形態３６によるロボット制御装置を示すブロック図は図１であり、指令値生成手段１以外の各手段の動作は、実施の形態３４と同様のため、説明は省略する。
【００９５】
次に動作について説明する。
指令値生成手段１では、ロボットに実行させる各動作毎に０以上１以下の値を持つ媒介変数で位置指令を表現し、媒介変数を０から１まで滑らかに増加させていくことにより時々刻々の位置指令を生成する。衝突判別手段５より停止命令を受信すると、媒介変数の現在値Ｘ_cを記憶すると共に、単位時間当たりの媒介変数の増加量を滑らかに０まで減少させる。単位時間当たりの媒介変数の増加量が０になった地点の媒介変数値をＸ_sとする。指令値生成手段１には、媒介変数戻し量Ｘ_lを予め記憶しておく。Ｘ_c−Ｘ_lと０とを比較し、大きい方をＸ_tとする。単位時間当たりの媒介変数の増加量が０になった後、直ちに媒介変数をＸ_sからＸ_tまで滑らかに減少させることにより衝突状態から回避する位置指令を生成する。
【００９６】
実施の形態３７．
図２５はこの発明の実施の形態３７によるロボット制御装置を示すブロック図であり、図において、７４は第１衝突判別手段、７５は第２衝突判別手段、７６は位置速度記憶手段、７７は過度反転判別手段である。
その他の構成については実施の形態１６と同一のため、説明は省略する。
【００９７】
次に動作について説明する。
第１衝突判別手段７４では、位置速度記憶手段７６に衝突検知信号を送信する。また、第２衝突判別手段７５では、位置速度記憶手段７６に衝突検知信号を送信する。
位置速度記憶手段７６は、第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、受信したときの各軸の関節変位と関節速度を記憶する。過度反転判別手段７７では、位置速度記憶手段７６に記憶された関節変位と関節速度を読み込み、モータ制御手段３３から入手する現在の関節変位が、位置速度記憶手段７６に記憶された関節変位から、位置速度記憶手段７６に記憶された関節速度とは逆向きに規定値以上離れた場合に、過度反転が発生したと判別し、モータ制御手段３３に即時停止命令を送信する。
【００９８】
実施の形態３８．
この発明の実施の形態３８によるロボット制御装置を示すブロック図は図２５であり、過渡反転判別手段７７以外の各手段の動作は、実施の形態３７と同様のため、説明は省略する。
【００９９】
次に動作について説明する。
過度反転判別手段７７では、位置速度記憶手段７６に記憶された関節変位と関節速度を読み込み、モータ制御手段３３から入手する現在の関節速度が、位置速度記憶手段７６に記憶された関節関節速度とは逆向きでかつその絶対値が規定値以上の場合に、過度反転が発生したと判別し、モータ制御手段３３に即時停止命令を送信する。
【０１００】
実施の形態３９．
図２６はこの発明の実施の形態３９によるロボット制御装置を示すブロック図であり、図において、７９は第１衝突判別手段、８０はモータ制御手段、８１は第２衝突判別手段、８２は第２指令値生成手段である。
その他の構成については実施の形態１６と同一のため、説明は省略する。
【０１０１】
次に動作について説明する。
第１衝突判別手段７９では、まずモータ制御手段８０から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１及び第２指令値生成手段８２に停止命令を送信する。
第２衝突判別手段８１では、まずモータ制御手段８０で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段８１は停止命令を指令値生成手段１及び第２指令値生成手段８２に送信する。
第２指令値生成手段８２は、第１衝突判別手段７９もしくは第２衝突判別手段８１から停止命令を受信すると、ロボットを駆動する各軸ごとに現在速度から速やかに減速停止する減速停止指令を生成する。モータ制御手段８０では指令値生成手段１から送られてくる指令値を無視して、第２指令値生成手段８２で生成される減速停止指令に基づいてモータ制御を行う。
【０１０２】
実施の形態４０．
図２７はこの発明の実施の形態４０によるロボット制御装置を示すブロック図であり、図において、８３は第１衝突判別手段、８４はモータ制御手段、８５は第２衝突判別手段、８６はトルク制限値演算手段である。
その他の構成については図２６と同一のため、説明は省略する。
【０１０３】
次に動作について説明する。
第１衝突判別手段８３では、まずモータ制御手段８４から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させるとともにタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信すると共に、トルク制限値演算手段８６に衝突検知信号と衝突検知軸番号を送信する。
第２衝突判別手段８５では、まずモータ制御手段８４で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段８５は指令値生成手段１に停止命令を送信すると共に、トルク制限値演算手段８６に衝突検知信号と衝突検知軸番号を送信する。
トルク制限値演算手段８６の衝突検知信号受信前の動作は図３７のトルク制限値演算手段３２の動作と全く同一であるが、第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号及び衝突検知軸番号を受信すると、衝突検知軸番号と同一の番号の軸のみ上側トルク制限値及び下側トルク制限値を現在値に固定する。残りの軸の上側トルク制限値及び下側トルク制限値は衝突検知信号受信前と全く同一の手法で算出を続ける。
【０１０４】
実施の形態４１．
この発明の実施の形態４１によるロボット制御装置を示すブロック図は図２７であり、トルク制限値演算手段８６以外の各手段の動作は、実施の形態４０と同様のため、説明は省略する。
【０１０５】
次に動作について説明する。
トルク制限値演算手段８６の衝突検知信号受信前の動作は、図２５のトルク制限値演算手段３２の動作と全く同一であるが、第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号及び衝突検知軸番号を受信すると、衝突検知軸番号と同一の番号の軸のみ必要駆動トルク指令要素演算手段２から送信されてくる重力指令トルク要素に上側トルク余裕値を加算したものを上側トルク制限値とし、重力指令トルク要素に下側トルク余裕値を加算したものを下側トルク制限値とする。ただし、突然切り替えるとトルク制限値がステップ上に変化するため、滑らかに切り替える。具体的には、衝突検知軸番号と同一の番号の軸に関しては、衝突検知信号受信後は、必要駆動トルク指令要素演算手段２から送信されてくるトルク指令要素の和に基づくトルク制限値と重力指令に基づくトルク制限値の重み付和をモータ電流制限手段３５に送信するトルク制限値とし、両者の重みの合計値を１に保ちながら、トルク指令要素の和に基づくトルク制限値の重みを１から０に滑らかに減少させると共に、重力指令に基づくトルク制限値の重みを０から１に滑らかに増加させる。
【０１０６】
実施の形態４２．
この発明の実施の形態４２によるロボット制御装置を示すブロック図は図２７であり、トルク制限値演算手段８６以外の各手段の動作は、実施の形態４０と同様のため、説明は省略する。
【０１０７】
次に動作について説明する。
トルク制限値演算手段８６の内部に衝突検知後のトルク余裕値倍率βをあらかじめ記憶しておく。第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号を受信するまでは、図２５のトルク制限値演算手段３２と全く同様に上側トルク制限値及び下側トルク制限値を計算する。第１衝突判別手段８３もしくは第２衝突判別手段８５から衝突検知信号を受信した後は、上側トルク余裕値及び下側トルク余裕値にトルク余裕値倍率βを乗じてから、必要駆動トルク指令要素演算手段２から送信されてくるトルク指令要素の和に加算してモータ電流制限手段３５に送信するトルク制限値を算出する。ただし、突然切り替えるとトルク制限値がステップ上に変化するため、上側トルク余裕値及び下側トルク余裕値に乗じる値をいきなりβにするのではなく、滑らかにβに変化させる。
【０１０８】
実施の形態４３．
図２８はこの発明の実施の形態４３によるロボット制御装置を示すブロック図であり、図において、８９はしきい値演算手段、９０は衝突仮想判別手段、９１は判別データ蓄積手段、９２はモータ制御手段である。
その他の構成については実施の形態４２と同一のため、説明は省略する。
【０１０９】
次に動作について説明する。
この実施の形態４３においては、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図２８は実施の形態４３におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４３におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
しきい値演算手段８９には、しきい値の算出に使用されるパラメータの値の候補が複数設定される。パラメータの値の候補設定は、図示しない手動操作盤、もしくはロボット動作を記述するプログラムから行われる。しきい値演算手段８９は設定されたパラメータとそのパラメータに基づいて算出されるしきい値を衝突仮想判別手段９０に送信する。
衝突仮想判別手段９０では、まず、モータ制御手段９２から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。次にしきい値演算手段８９から送信される複数のしきい値のそれぞれに対して演算した各軸のトルク差と各軸のしきい値のどちらが大きいか比較し、それぞれのしきい値に対して、各軸でトルク差がしきい値を連続して超えた回数である連続超過回数を算出する。算出した連続超過回数としきい値の算出に用いられたパラメータ値と軸番号は判別データ蓄積手段９１に送信される。
判別データ蓄積手段９１では、連続超過回数が、パラメータ値と軸番号ごとに保存される。保存されている連続超過回数よりも大きな値が連続超過回数として送信されてきた場合は保存されている値を書き換える。判別データ蓄積手段９１に蓄積されたパラメータ値、軸番号、連続超過回数は、図示しない手動操作盤やロボット制御装置に接続されたパーソナルコンピュータから閲覧できる。ロボット制御装置の管理者は閲覧したパラメータ値、軸番号、連続超過回数に基づいてしきい値演算手段８９で用いるパラメータ値を決定する。
【０１１０】
実施の形態４４．
図２９はこの発明の実施の形態４４によるロボット制御装置を示すブロック図であり、図において、９３はパラメータ決定手段である。
その他の構成については図２８と同一のため、説明は省略する。
【０１１１】
次に動作について説明する。
この実施の形態４４においても、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図２９は、実施の形態４４におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４４におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
パラメータ決定手段９３では図示しない手動操作盤からパラメータ決定指令を受け取ると、ロボットを駆動する軸ごとにパラメータ値と連続超過回数データを判別データ蓄積手段９１より受信する。次にあらかじめ記憶されている許容最大連続回数とパラメータ値ごとの連続超過回数を比較し、連続超過回数が許容最大連続回数以下となるパラメータ値の中で最小のパラメータ値を求め、さらに、あらかじめ記憶されている補正値を加算したものをしきい値演算用パラメータとして、しきい値演算手段８９に送信する。
【０１１２】
実施の形態４５．
図３０はこの発明の実施の形態４５によるロボット制御装置を示すブロック図であり、図において、９４は感度設定手段である。
その他の構成については図２８と同一のため、説明は省略する。
【０１１３】
次に動作について説明する。
この実施の形態４５においても、通常モードと試行動作モードとの２通りを設け、試行動作モードの動作結果から衝突判別に用いるしきい値を算出するもとになるパラメータを決定する。図３０は、実施の形態４５におけるロボット制御方式の試行動作モードの構成を示すブロック図であり、実施の形態４５におけるロボット制御方式の通常モードの構成を示すブロック図は図２５である。
まず、感度設定手段９４では図示しない手動操作盤から、感度候補値が複数設定され、設定された感度候補値は全てしきい値演算手段８９に送信される。
しきい値演算手段８９では、あらかじめ記憶されているパラメータに基づいて時々刻々のしきい値を算出する。次に算出したしきい値に感度候補値のそれぞれを乗算し、乗算結果と感度候補値の組み合わせを衝突仮想判別手段にすべて送信する。
衝突仮想判別手段９０では、まずモータ制御手段９２から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。次にしきい値演算手段８９から送信される複数のしきい値のそれぞれに対して演算した各軸のトルク差と各軸のしきい値のどちらが大きいか比較し、それぞれのしきい値に対して、各軸でトルク差がしきい値を連続して超えた回数である連続超過回数を算出する。算出した連続超過回数、軸番号としきい値の算出に用いられた感度候補値は判別データ蓄積手段９１に送信される。
判別データ蓄積手段９１では、連続超過回数が、軸番号、感度候補値ごとに保存される。保存されている連続超過回数よりも大きな値が連続超過回数として送信されてきた場合は保存されている値を書き換える。判別データ蓄積手段９１に蓄積された感度候補値、軸番号、連続超過回数は、図示しない手動操作盤やロボット制御装置に接続されたパーソナルコンピュータから閲覧できる。ロボット制御装置の管理者は閲覧した感度候補値、軸番号、連続超過回数に基づいて実際にロボットを動作させる際の感度設定値を決定する。
【０１１４】
実施の形態４６．
図３１はこの発明の実施の形態４６によるロボット制御装置を示すブロック図であり、図において、９５は表示手段である。
その他の構成については図２５と同一のため、説明は省略する。
【０１１５】
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。また表示手段９５に衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令を指令値生成手段１に送信する。また表示手段９５に衝突検知信号を送信する。
表示手段９５は衝突検知信号を受信すると、手動操作盤の画面を衝突検知機能切り替え専用画面に自動的に切り替える。衝突検知機能切り替え画面には、１を入力すると衝突検知機能を有効としたまま元の画面に戻り、０を入力すると衝突検知機能を無効にして元の画面に戻る旨記載されている。ロボット操作者は画面表示に従って手動操作盤を操作することにより衝突検知機能を直ちに無効にして、衝突により押し込み状態にあるロボットを迅速に退避させることができる。
【０１１６】
実施の形態４７．
図３２はこの発明の実施の形態４７によるロボット制御装置を示すブロック図であり、図において、９６は衝突状態判別手段である。
その他の構成については図２５と同一のため、説明は省略する。
【０１１７】
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段９６に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段９６に停止命令と衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令と衝突検知信号を衝突状態判別手段９６に送信する。
衝突状態判別手段９６には衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令はそのまま指令値生成手段１に出力すると共に衝突判別フラグの値を１に変更する。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段１に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。
【０１１８】
実施の形態４８．
この発明の実施の形態４８によるロボット制御装置を示すブロック図は図３１であり、指令値生成手段１以外の各手段の動作は、実施の形態４６と同様のため、説明は省略する。
【０１１９】
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、指令値生成手段１に停止命令を送信する。また指令値生成手段１及び表示手段９５に衝突検知信号を送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は停止命令を指令値生成手段１に送信する。また指令値生成手段１及び表示手段９５に衝突検知信号を送信する。
指令値生成手段１では、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止信号を受信すると直ちに減速停止指令曲線を生成する。第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、指令値生成手段１内部に備えてあるリングバッファ（記憶手段）からあらかじめ規定された時間前の位置指令を読み出し、読み出した位置を衝突退避動作目標地点として記憶する。手動操作盤の規定されたボタンが押し込まれることにより衝突退避指令が入力されると、減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点とする直線動作指令を、あらかじめ規定された低速度で生成する。
表示手段９５は衝突検知信号を受信すると、手動操作盤の画面を衝突退避動作専用画面に自動的に切り替える。衝突退避動作専用画面では、衝突退避動作を指令するために押さねばならないボタンの番号が示される。
【０１２０】
実施の形態４９．
この実施の形態４９におけるロボット制御方式の構成を示すブロック図も図３１であり、指令値生成手段１以外の各要素は実施の形態４８の各要素と同一なので説明は省略する。
【０１２１】
次に動作について説明する。
指令値生成手段１では、第１衝突判別手段７４もしくは第２衝突判別手段７５から停止信号を受信すると直ちに減速停止指令曲線を生成する。第１衝突判別手段７４もしくは第２衝突判別手段７５から衝突検知信号を受信すると、衝突検知信号受信時に実行している動作が１つの場合は当該動作の始点を衝突退避動作目標地点として記憶する。衝突検知信号受信時に実行している動作が２つ以上ある場合は、最新の動作が実際に開始された地点を算出し、算出した地点を衝突退避動作目標地点として記憶する。手動操作盤の規定されたボタンが押し込まれることにより衝突退避指令が入力されると、衝突検知信号受信時に実行している動作が１つの場合は、当該動作と同じ補間方法で減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点する動作をあらかじめ規定された低速度で生成する。衝突検知信号受信時に実行している動作が２つ以上ある場合は、減速停止した後の現在位置を始点とし、衝突退避動作目標点を終点とする直線動作指令を、あらかじめ規定された低速度で生成する。
【０１２２】
実施の形態５０．
図３３はこの発明の実施の形態５０によるロボット制御装置を示すブロック図であり、図において、９８は衝突状態判別手段、９９は指令値生成手段、１００は表示手段である。
その他の構成については図２５と同一のため、説明は省略する。
【０１２３】
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段９８に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段９８に衝突検知信号と停止命令を送信する。また衝突と判定された軸の番号である衝突検知軸番号と、その軸の差トルクの符号である衝突検知方向を衝突状態判別手段９８に送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は衝突検知信号と停止命令を衝突状態判別手段９８に送信する。また衝突と判定された軸の番号である衝突検知軸番号と、その軸の差トルクの符号である衝突検知方向を衝突状態判別手段９８に送信する。
衝突状態判別手段９８には衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号はそのまま指令値生成手段９９に出力すると共に衝突判別フラグの値を１に変更する。衝突検知信号は表示手段１００にも送信される。また、受信した衝突検知軸番号と衝突検知方向を指令値生成手段９９及び表示手段１００に送信する。衝突判別フラグの値が１のときは衝突状態にあると判別していることを意味している。衝突判別フラグの値が１の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信しても、停止命令は指令値生成手段９９に送信されない。第１衝突判別手段７４から衝突状態脱出信号を受信すると衝突判別フラグの値は０に変更される。衝突判別フラグが１から０に変わると衝突状態脱出信号を指令値生成手段９９及び表示手段１００に送信する。
指令値生成手段９９では、衝突状態判別手段９８から停止信号及び衝突検知信号を受信すると直ちに減速停止指令曲線を生成する。減速停止指令曲線の生成完了後に、衝突検知軸番号と同一の軸の指令方向チェックフラグを１にする。指令方向チェックフラグが１になっている軸の指令方向が衝突検知方向と一致する場合は、生成した指令値を指令値生成手段９９の外部には出力せず、指令値生成周期の前回周期の値を外部に出力すると共に、表示手段１００に動作方向警告信号を送信する。指令方向チェックフラグは衝突状態脱出信号を受信すると０に戻される。
表示手段１００では、衝突状態判別手段９８から、衝突検知信号、衝突検知軸番号、衝突検知方向を受信すると、手動操作盤の画面を衝突状態表示画面に切り替える。衝突状態表示画面では、衝突検知軸番号及び衝突検知方向が表示される。また、指令値生成手段９９から動作方向警告信号を受信すると警告音を発生すると共に衝突検知軸番号及び衝突検知方向を点滅させながら手動操作盤の画面に表示させる。
【０１２４】
実施の形態５１．
図３４はこの発明の実施の形態５１によるロボット制御装置を示すブロック図であり、図において、１０１は衝突状態判別手段である。
その他の構成については図３３と同一のため、説明は省略する。
【０１２５】
次に動作について説明する。
第１衝突判別手段７４では、まずモータ制御手段３３から入力されるモータ電流から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。すべての軸で規定時間以上連続してトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となっているときは、衝突状態判別手段１０１に衝突状態脱出信号を送信する。一方、いずれかの軸においてトルク差が上側低レベルしきい値以上もしくは下側低レベルしきい値以下になると衝突判定タイマーをスタートさせる。当該軸のトルク差が上側低レベルしきい値以下かつ下側低レベルしきい値以上となったときは前記タイマーを停止させると共にタイマーの内部時間を０にリセットする。前記タイマーの内部時間がしきい値演算手段２８で設定された衝突判別時間以上となった場合にロボットが衝突したと判別する。ロボットの衝突を判別した場合は、衝突状態判別手段１０１に衝突検知信号と停止命令を送信する。また、衝突と判別した瞬間の各軸のトルク差を衝突作用トルクとして衝突状態判別手段１０１に送信する。
第２衝突判別手段７５では、まずモータ制御手段３３で保持しているモータ電流値から実際の各軸のモータ駆動トルクを演算する。次に演算したモータ駆動トルクと必要駆動トルク演算手段３で演算した必要駆動トルクとの差であるトルク差の演算を行う。いずれかの軸においてトルク差が上側高レベルしきい値以上もしくは下側高レベルしきい値以下の時はロボットが衝突したと判別する。衝突を判別したときは、第２衝突判別手段７５は衝突検知信号と停止命令を衝突状態判別手段１０１に送信する。また、衝突と判別した瞬間の各軸のトルク差を衝突作用トルクとして衝突状態判別手段１０１に送信する。
衝突状態判別手段１０１は衝突状態判別フラグを備えている。通常の動作状態では衝突判別フラグは０となっている。衝突判別フラグの値が０のときは衝突状態にないと判別していることを意味している。衝突判別フラグの値が０の時に第１衝突判別手段７４もしくは第２衝突判別手段７５から停止命令と衝突検知信号を受信すると、停止命令と衝突検知信号をそのまま指令値生成手段９９に出力すると共に衝突判別フラグの値を１に変更する。衝突検知信号は表示手段１００にも送信される。また、受信した衝突作用トルクとモータ制御手段３３から入力される関節変位から直交座標系における衝突方向を下記の手順で算出する。
まず、各軸の衝突作用トルクから構成される衝突作用トルクベクトルをτ_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、クーロン摩擦係数をf_ci、各軸の現在速度をｖ_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が入力される。次にτ_mdの各軸の符号を求め、求めた符号に応じて加減速方向重みを決定する。具体的にはτ_mdの各軸の符号と同じ方向の重みをｋ１、逆方向の重みをｋ２とする。例えば第２軸のτ_mdの符号が＋ならば、第２軸の上側しきい値にかける重みｕｋの値をｋ１、下側しきい値にかける重みｌｋの値をｋ２とする。各軸ごとの加減速方向重みはしきい値演算手段１０６に出力される。
しきい値演算手段１０６には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０５から出力されるＴ_maxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔ_max、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔ_maxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。さらに設定された上側低レベルしきい値、及び上側高レベルしきい値のそれぞれに加減速方向重み決定手段１０４から入力された加減速方向重みｕｋが乗じられる。また、下側低レベルしきい値、及び下側高レベルしきい値のそれぞれに加減速方向重み決定手段１０４から入力された加減速方向重みｌｋが乗じられる。加減速方向重みが乗じられた上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、加減速方向重みが乗じられた上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
【０１３４】
実施の形態５６．
図３８はこの発明の実施の形態５６によるロボット制御装置を示すブロック図であり、図において、１０７はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
【０１３５】
次に動作について説明する。
しきい値演算手段１０７には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴ_maxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔ_max、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔ_maxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。
しきい値演算手段１０７には、モータ制御手段３３から各軸のモータ速度も入力される。しきい値演算手段１０７では各軸モータ速度の現在値と前回の各軸モータ速度より、各軸モータ加速度を算出する。しきい値演算手段１０７には、モータ加速度の絶対値と衝突判別時間のテーブルがあらかじめ作成されており、このテーブルを参照することにより各軸の衝突判別時間が設定される。
設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
【０１３６】
実施の形態５７．
図３９はこの発明の実施の形態５７によるロボット制御装置を示すブロック図であり、図において、１０８はしきい値演算手段である。
その他の構成については図３６と同一のため、説明は省略する。
【０１３７】
次に動作について説明する。
しきい値演算手段１０８には、衝突判別時間、上側しきい値ベース分、下側しきい値ベース分、上側しきい値トルク指令倍率、下側しきい値トルク指令倍率、上側低レベルしきい値クランプ値、下側低レベルしきい値クランプ値、高レベル加算値を各軸毎に記憶しておき、トルク指令絶対値最大値演算手段１０３から出力されるＴ_maxを用いて、各軸の上側しきい値基準値は、上側しきい値ベース分＋上側しきい値トルク指令倍率×Ｔ_max、下側しきい値基準値は下側しきい値ベース分＋下側しきい値トルク指令倍率×Ｔ_maxで算出される。次に上側しきい値基準値が上側低レベルしきい値クランプ値以下の場合は、上側しきい値基準値が上側低レベルしきい値として設定され、上側しきい値基準値が上側低レベルしきい値クランプ値以上の場合は、上側低レベルしきい値クランプ値が上側低レベルしきい値として設定される。下側しきい値基準値が下側低レベルしきい値クランプ値以上の場合は、下側しきい値基準値が下側低レベルしきい値として設定され、下側しきい値基準値が下側低レベルしきい値クランプ値以下の場合は、下側低レベルしきい値クランプ値が下側低レベルしきい値として設定される。上側高レベルしきい値は上側低レベルしきい値＋高レベル加算値で設定され、下側高レベルしきい値は下側低レベルしきい値−高レベル加算値で設定される。
しきい値演算手段１０８には、必要駆動トルク指令要素演算手段２から出力された慣性力指令要素ベクトルτ_mdも入力される。次にτ_mdの各軸の絶対値が算出される。しきい値演算手段１０８には、τ_mdの各軸の絶対値と衝突判別時間のテーブルがあらかじめ作成されており、このテーブルを参照することにより各軸の衝突判別時間が設定される。
設定された上側低レベルしきい値、下側低レベルしきい値、衝突判別時間は第１衝突判別手段７４に出力され、上側高レベルしきい値、下側高レベルしきい値は第２衝突判別手段７５に出力される。
【０１３８】
【発明の効果】
以上のように、この発明によれば、衝突の判別に使用する必要駆動トルクを必要駆動トルク指令要素及びロボットを駆動するモータの位置、速度、加速度から演算するため、軌跡精度を向上させるためのフィードフォワード制御と付加的なセンサを用いずに衝突を検知する方式を両立させる際、計算量を削減し、かつ高精度の衝突の判別が行える効果がある。
【０１３９】
この発明によれば、必要駆動トルク指令要素及び位置指令、速度指令、加速度指令から近似的に演算したモータの位置、速度、加速度から衝突の判別に使用する必要駆動トルクを演算するため、軌跡精度を向上させるためのフィードフォワード制御と付加的なセンサを用いずに衝突を検知する方式を両立させる際、計算量を削減し、かつ高精度の衝突の判別が行える効果がある。
【０１４０】
この発明によれば、必要駆動トルク指令要素から衝突の判別に使用する必要駆動トルクを演算するため、軌跡精度を向上させるためのフィードフォワード制御と付加的なセンサを用いずに衝突を検知する方式を両立させる際、計算量を削減し、かつ高精度の衝突の判別が行える効果がある。
【０１４１】
この発明によれば、摩擦係数等の動作条件に応じて変動するパラメータをロボットの動作中に逐次適に同定するため、高精度で衝突の判別が行える効果がある。
【０１４２】
この発明によれば、衝突の判別に使用する必要駆動トルクの演算を行う際、動作中に同定した摩擦係数等のパラメータ推定値の現在値ではなく衝突以前の同定値を用いるため、衝突以前に同定したパラメータ値を用いて衝突の判別に使用する必要駆動トルクの演算を行うため、摩擦係数等のパラメータ推定値が衝突の影響による必要駆動トルク演算値とモータ電流との偏差を減少させるように変動して衝突の判別精度が低下することを防止する効果がある。
【０１４３】
この発明によれば、同定精度の高くなることが期待できるデータのみから摩擦係数等のパラメータ同定を行うため、高精度にパラメータ同定が実施でき、衝突の判別の用いる必要駆動トルクを高精度に演算できる効果がある。
【０１４４】
この発明によれば、同定精度が低いために、返って衝突の判別精度が低下するのを防止する効果がある。
【０１４５】
この発明によれば、比較的演算周期が長くても良いパラメータ同定を長い演算周期で行うため、衝突判別精度を落とすことなく、制御装置における単位時間当たりの計算量を低減できる効果がある。
【０１４６】
この発明によれば、ロボットが周辺対象物と衝突していることにより同定するパラメータ値が真値からずれることによる必要駆動トルクの演算精度が低下することを防止できる効果がある。
【０１４７】
この発明によれば、柔らかい対象物に衝突する場合と堅い対処物に衝突する場合のいずれにおいても高精度に衝突を判別することができる効果がある。
【０１４８】
この発明によれば、衝突検知後の停止時にロボットの各軸駆動用モータが発生するトルクを高精度に制限できるため、ロボット及び衝突した対象物に過大な力が作用するのを防止できる効果がある。
【０１４９】
この発明によれば、モータ電源投入時に誤って衝突を検知することを防止する効果がある。
【０１５０】
この発明によれば、手先負荷に応じた最適な感度を設定できるため、必要駆動トルクの推定精度が高い手先負荷条件時に必要以上に衝突判別の精度を低下させることを防止する効果がある。
【０１５１】
この発明によれば、ロボットのユーザが望む感度で衝突を検知することが可能となる効果がある。
【０１５２】
この発明によれば、自動運転時の感度に影響を与えることなく、衝突させてしまう危険が高い教示動作における衝突検知の感度を予め高くしておくことが可能となる効果がある。
【０１５３】
この発明によれば、パラメータの同定値が真値から大きくはずれているために必要駆動トルクの演算精度が低下している時に誤って衝突を検知してしまうことを防止しながら、パラメータ同定精度が高いときには高精度に衝突検知が可能となる効果がある。
【０１５４】
この発明によれば、ロボットの動作状態に応じてきめ細かく感度を設定できるため、常に高精度に衝突を検知できる効果がある。
【０１５５】
この発明によれば、静止摩擦の影響でトルクの推定精度が低下する停止時のみしきい値を大きくすることが可能なため、停止している軸で誤って衝突を検知することを防止しながら、動作中は高精度に衝突を検知できる効果がある。
【０１５６】
この発明によれば、外乱力の作用する作業をロボットに行わせる場合のみ自動的にしきい値を大きくすることができるため、外乱力の作用する作業をロボットに行わせる場合に衝突を誤って検知することを防止できる効果がある。
【０１５７】
この発明によれば、トルク制限値が急激に変化することによりモータに与えられるトルク指令が急激に変化し、ロボットの挙動が振動的になることを防止する効果がある。
【０１５８】
この発明によれば、トルク制限値が振動的に変化し、ロボットの挙度が振動的になることを防止する効果がある。
【０１５９】
この発明によれば、ハンドでワークを把持した場合のように手先負荷の質量がステップ状に変化する場合にも必要駆動トルク指令要素及び必要駆動トルクがステップ状に変化し、ロボットの挙動が振動的になることを防止する効果がある。
【０１６０】
この発明によれば、衝突検知によりロボットが対象物を押し当てている状態で停止したときに、対象物を除去することなく手動運転でロボットを待避させることが可能となる効果がある。
【０１６１】
この発明によれば、電流もしくは加速度信号に加わる観測ノイズの影響で衝突を誤って検知することを防止できる効果がある。
【０１６２】
この発明によれば、衝突検知後の待避中に再度衝突を起こさないようにロボットを待避させることが可能になる効果がある。
【０１６３】
この発明によれば、過度反転判別手段で過度反転を検出した場合にロボットを緊急停止するため、衝突後にロボットが過度に反転することを防止することができる効果がある。
【０１６４】
この発明によれば、過度反転判別手段で過度反転を検出した場合にロボットを緊急停止するため、衝突後にロボットが過度に反転することを防止することができる効果がある。
【０１６５】
この発明によれば、衝突判別後の停止中に運動方程式に基づいて各軸のトルク制限値を生成することによって得られる衝突時の押し付け力を低減する能力と、減速停止指令の減速度を別個に指定することにより、減速停止時の振動を防止しながら衝突時の押し付け力を大幅に低減することができる効果がある。
【０１６６】
この発明によれば、衝突時にロボットが思わぬ方向へ動作することを防止しながら、衝突後の押し付け力を低減することができる効果がある。
【０１６７】
この発明によれば、衝突時にロボットが思わぬ方向へ動作することを防止しながら、衝突後の押し付け力を低減することができる効果がある。
【０１６８】
この発明によれば、衝突判別までの期間はしきい値を小さくしておくことにより押し付け力を低減し、衝突判別により停止する際は、しきい値を大きくして、押し付け力低減効果は若干残しながら、ロボットが指令から大きく逸脱しないようにする。また、逆に衝突判別後のしきい値をさらに小さくすることにより、衝突判別により停止する際の押し付け力をさらに低減することも可能となる効果がある。
【０１６９】
この発明によれば、実際の動作データ（動作させた結果）に応じてパラメータが設定されるため、適切なパラメータを容易かつ短時間に設定できる効果がある。
【０１７０】
この発明によれば、実際の動作データ（動作させた結果）に応じて感度が設定されるため、適切な（可能な限り高い感度を実現し、かつ、誤検知はしない）感度を容易かつ短時間に設定できる効果がある。
【０１７１】
この発明によれば、押し付け状態で停止した場合に、回避動作を行わせようとすると再度衝突検知で停止するために、衝突検知機能を解除しないとロボットを動かせない場合がある。そうした場合に、衝突判別機能を速やかに解除し、押し付け状態から手動操作でロボットを回避させることが可能になる効果がある。
【０１７２】
この発明によれば、押し付け状態で停止した場合に、衝突を再度検知するためにロボットを回避できない現象を自動的に防止する。また、押し付け状態から逃れると自動的に衝突検知機能が再び有効となるため、再度別のところに衝突した場合のダメージを低減できる効果がある。
【０１７３】
この発明によれば、ロボットを衝突以前の地点まで容易に戻すことができる効果がある。
【０１７４】
この発明によれば、衝突によりロボットが停止している際、回避動作を行おうとして間違えて、さらに押し込む方向の指令を与えてもロボットが動作せず、さらに押し込むことを防止する。また、警告を発することにより、回避動作を行う方向を間違えたことを操作者は容易に気づくことができる効果がある。
【０１７５】
この発明によれば、教示時の軽い接触の場合は、警告が発せられ接触していることが操作者に認識されるが、ロボットが停止したり電源遮断されないため、そのまま教示作業が継続し、接触地点付近の教示を短時間で行える効果がある。
【０１７６】
この発明によれば、クーロン摩擦モデルがより正確になるため、運動方程式に基づいて算出したトルクの推定精度が高くなる効果がある。
【０１７７】
この発明によれば、しきい値を単純な関数に基づいて設定しながら、しきい値が過大になり、衝突検知感度が低下することを防止することができる効果がある。
【０１７８】
この発明によれば、しきい値に加減速方向に応じた重みを付けることにより、誤検知を防止しながら衝突検知の感度を上げることができる効果がある。
【０１７９】
この発明によれば、しきい値そのものだけでなく、トルク推定誤差がしきい値以上となる回数の連続値も動作条件に応じて可変にすることにより、誤検知を防止しながら衝突検知の感度を上げることができる効果がある。
【図面の簡単な説明】
【図１】 この発明の実施の形態１によるロボット制御装置を示すブロック図である。
【図２】 この発明の実施の形態３によるロボット制御装置を示すブロック図である。
【図３】 この発明の実施の形態４によるロボット制御装置を示すブロック図である。
【図４】 この発明の実施の形態６によるロボット制御装置を示すブロック図である。
【図５】 この発明の実施の形態７によるロボット制御装置を示すブロック図である。
【図６】 この発明の実施の形態８によるロボット制御装置を示すブロック図である。
【図７】 この発明の実施の形態９によるロボット制御装置を示すブロック図である。
【図８】 この発明の実施の形態１０によるロボット制御装置を示すブロック図である。
【図９】 この発明の実施の形態１２によるロボット制御装置を示すブロック図である。
【図１０】 この発明の実施の形態１４によるロボット制御装置を示すブロック図である。
【図１１】 この発明の実施の形態１５によるロボット制御装置を示すブロック図である。
【図１２】 この発明の実施の形態２１によるロボット制御装置を示すブロック図である。
【図１３】 この発明の実施の形態２２によるロボット制御装置を示すブロック図である。
【図１４】 この発明の実施の形態２３によるロボット制御装置を示すブロック図である。
【図１５】 この発明の実施の形態２４によるロボット制御装置を示すブロック図である。
【図１６】 この発明の実施の形態２５によるロボット制御装置を示すブロック図である。
【図１７】 この発明の実施の形態２６によるロボット制御装置を示すブロック図である。
【図１８】 この発明の実施の形態２７によるロボット制御装置を示すブロック図である。
【図１９】 この発明の実施の形態２８によるロボット制御装置を示すブロック図である。
【図２０】 この発明の実施の形態２９によるロボット制御装置を示すブロック図である。
【図２１】 この発明の実施の形態３０によるロボット制御装置を示すブロック図である。
【図２２】 この発明の実施の形態３１によるロボット制御装置を示すブロック図である。
【図２３】 この発明の実施の形態３２によるロボット制御装置を示すブロック図である。
【図２４】 この発明の実施の形態３３によるロボット制御装置を示すブロック図である。
【図２５】 この発明の実施の形態３７及び５３によるロボット制御装置を示すブロック図である。
【図２６】 この発明の実施の形態３９によるロボット制御装置を示すブロック図である。
【図２７】 この発明の実施の形態４０によるロボット制御装置を示すブロック図である。
【図２８】 この発明の実施の形態４３によるロボット制御装置を示すブロック図である。
【図２９】 この発明の実施の形態４４によるロボット制御装置を示すブロック図である。
【図３０】 この発明の実施の形態４５によるロボット制御装置を示すブロック図である。
【図３１】 この発明の実施の形態４６によるロボット制御装置を示すブロック図である。
【図３２】 この発明の実施の形態４７によるロボット制御装置を示すブロック図である。
【図３３】 この発明の実施の形態５０によるロボット制御装置を示すブロック図である。
【図３４】 この発明の実施の形態５１によるロボット制御装置を示すブロック図である。
【図３５】 この発明の実施の形態５２によるロボット制御装置を示すブロック図である。
【図３６】 この発明の実施の形態５４によるロボット制御装置を示すブロック図である。
【図３７】 この発明の実施の形態５５によるロボット制御装置を示すブロック図である。
【図３８】 この発明の実施の形態５６によるロボット制御装置を示すブロック図である。
【図３９】 この発明の実施の形態５７によるロボット制御装置を示すブロック図である。
【図４０】 この発明の実施の形態１６によるロボット制御装置を示すブロック図である。
【符号の説明】
１，１８，９９ 指令値生成手段、２ 必要駆動トルク指令要素演算手段、３，１２，１５ 必要駆動トルク演算手段、４，２８，４８，５１，５３，５５，５７，５９，６１，６３，６５，８９，１０６〜１０８ しきい値演算手段、５，９，２７ 衝突判別手段、６，１０，１７，３３，８０，８４，９２ モータ制御手段、８ 加算手段、１１ 位置速度加速度推定手段、１４，１６ パラメータ推定手段、１９ パラメータ蓄積手段、２０ 同定ゲイン演算手段、２１ パラメータ推定値制限手段、２５ 同定ゲイン演算手段、２９，７４，７９，８３ 第１衝突判別手段、３０，３７，７５，８１，８５ 第２衝突判別手段、３１ トルク余裕演算手段、３２，８６ トルク制限値演算手段、３４ モータ電流指令生成手段、３５ モータ電流制限手段（モータ駆動トルク制限手段）、３６ モータ電流制御手段、４７，４９ 同定進行度判別手段、５０ トルク指令絶対値最大値演算手段、５２ モータ電流絶対値最大値演算手段、５４ トルク指令トルク指令差分絶対値最大値演算手段、５６ トルク指令トルク指令差分絶対値演算手段、５８ 加速度指令加加速度指令絶対値演算手段、６０ しきい値倍率要素演算手段、６２ 停止判別手段、６４ 特殊作業判別手段、６６ プログラム解析手段、６７，７０ 平滑化手段、６８ トルク制限値保持手段、６９ 手先負荷設定値読み出し手段、７６ 位置速度記憶手段、７７ 過度反転判別手段、８２ 第２指令値生成手段、９０ 衝突仮想判別手段、９１ 判別データ蓄積手段、９３ パラメータ決定手段、９４ 感度設定手段、９５，１００ 表示手段、９６，９８，１０１ 衝突状態判別手段、１０２ モード判別手段、１０３，１０５ トルク指令絶対値最大値演算手段、１０４ 加速度方向重み決定手段。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a robot control apparatus that drives a robot using a motor, and more particularly to a robot control apparatus that detects that a robot has collided with a peripheral object without using an additional sensor and prevents damage to the robot and the peripheral object. Is.
[0002]
[Prior art]
Conventionally, as a method for detecting that a robot has collided with a surrounding object, for example, as described in Japanese Patent Application Laid-Open No. 63-127890, current detection means for detecting the current of a motor that drives each joint axis And calculating means for calculating the required driving torque of the motor from the joint angle of each joint axis, and calculating the current limit value of the motor, thereby calculating the current detected by the current detecting means and the calculating means. A method of detecting that the robot has touched the obstacle by comparing with the current limit value is used.
On the other hand, in order to improve the trajectory accuracy of the hand of the robot, a method of calculating a driving torque command required from a position command, a speed command, and an acceleration command and performing torque feed forward has been conventionally performed.
[0003]
Further, for example, as described in JP-A-1-310889, not only a collision is detected by comparing a current limit value and an actual current, but an allowable torque is set based on a necessary driving torque, A method of constraining the output torque of the motor within the allowable torque has also been used.
[0004]
[Problems to be solved by the invention]
Since the conventional robot control apparatus is configured as described above, the required drive torque can be calculated from the joint angle of each joint axis without using an additional sensor and the method of performing torque feed forward in order to improve the trajectory accuracy. If the calculation method is used together as it is, it is necessary to perform both the calculation of the driving torque based on the command value and the calculation of the driving torque based on the actual joint angle. Therefore, there is a problem that the amount of calculation that must be processed in the control device increases.
In addition, it is conceivable to use the torque feed forward calculation result as it is as the driving torque used for the collision determination. However, if the torque feed forward calculation result is used as it is, the estimation accuracy of the drive torque is low, and the collision determination There was a problem that accuracy also decreased.
Furthermore, the coefficient of viscous friction and Coulomb friction acting on each joint axis of the robot varies depending on conditions such as ambient temperature. Therefore, when calculating the required drive torque, if constant values are used as the viscous friction coefficient and the Coulomb friction coefficient, there is a problem that the calculation accuracy of the required drive torque is reduced and the accuracy of the collision determination is also reduced. Further, even when the mass of the load attached to the hand of the robot is unknown, there is a problem that the calculation accuracy of the required driving torque is lowered and the accuracy of the collision determination is also lowered.
Furthermore, since the output of the motor that drives each axis of the robot could not be limited with high accuracy, there was a problem that excessive force acted on the robot and the peripheral objects that collided with the robot during the stop operation after collision detection.
Furthermore, the accuracy of torque estimation varies depending on the mass of the load attached to the hand of the robot, the acceleration, speed, position, etc. when the robot is operated. Also, when the robot performs a deburring operation, etc., the robot receives a reaction force when opening and closing the hand. Therefore, if the collision is determined at a certain threshold, the accuracy of the collision determination is reduced to match the worst case. Or, there is a problem that a collision is erroneously determined by a work / operation that receives a reaction force.
[0005]
Furthermore, the allowable torque is set based on the calculated required drive torque, and the output torque of the motor is limited to within the allowable torque, so the servo rigidity is equivalently reduced and acts when the robot collides with an obstacle. When the impact force is large, the robot may return too much due to the reaction of the collision. In addition, when the required drive torque is calculated based on the command value, when the deceleration stop command is generated, the acceleration / deceleration torque during deceleration stop becomes a torque in the direction opposite to the operation direction at the time of the collision. The function of limiting the output torque within the permissible torque acts in the direction of reducing the pressing force, which is preferable for protecting the robot and the colliding object. However, if the acceleration / deceleration torque based on the deceleration stop command is greater than the pressing force, it is equivalent to giving a torque command that accelerates in the opposite direction to the previous movement direction, and the robot moves too much in the opposite direction. There was a fear. On the other hand, if the acceleration / deceleration torque based on the deceleration stop command is brought close to the pressing force to limit the force acting on the robot or the object that has collided, the deceleration must be increased, and vibration at the time of stopping is likely There were also challenges.
Furthermore, in such a robot control device, if the reference value for comparing the current detected by the current detector and the current limit calculated by the current limit calculator is too large, the collision detection sensitivity is reduced and May not be detected, or even if a collision is detected, a large force is applied to the robot or the object to which the robot collided. On the other hand, if the reference value for comparing the current detected by the current detecting means and the current limit value calculated by the current limit value calculating means is too small, it is erroneously detected that there is no collision, If necessary, the robot is stopped. Therefore, it is necessary to set an appropriate reference value, but nothing has been said about a method for setting an appropriate reference value.
Further, in such a robot control device, when the robot hand collides with the target object while operating downward, the robot control apparatus detects and stops the collision, but stops with the robot pushing the target object. If the robot stops in a state where the object is being pushed down, the object cannot be removed as it is and the collision state cannot be resolved. Therefore, it is necessary to move the robot hand upward. However, when the robot is lifted, the robot is in a collision state, and thus the collision is detected again and stopped. Therefore, there is a problem that the robot cannot be lifted to resolve the collision state.
Furthermore, when an operator performs an avoidance operation on the robot after it has accidentally collided with the robot, if it collides during automatic operation, it takes time to change to manual operation mode and perform an appropriate avoidance operation. There was a case. In addition, since the worker is panicked, there is a problem that the avoidance operation is performed in the wrong direction, and the operator may push in.
In addition, if you want to teach a target point that touches the object lightly or if you want to teach the vicinity of the object, the motor power is turned on frequently when the robot stops each time it detects a collision and shuts off the motor power. Therefore, there is a problem that the teaching work takes time.
Furthermore, if the calculation of the required drive torque used for collision detection is calculated using a Coulomb friction model that changes stepwise at a point of speed 0, the actual friction does not change stepwise. The estimation accuracy deteriorates near zero speed, and there is a problem that a collision cannot be detected by mistake or the accuracy of collision detection cannot be increased.
Furthermore, when the discriminant value for collision detection is a fixed value, the discriminant value cannot be set finely according to the operating state, and there is a problem that the sensitivity for collision detection cannot be increased.
[0006]
The present invention has been made to solve the above-described problems, and an object of the present invention is to obtain a robot control apparatus that reduces the amount of calculation and performs collision determination with high accuracy.
[0007]
[Means for Solving the Problems]
The robot control apparatus according to the present invention is a robot that uses a required drive torque command element calculated from at least one of a position command, a speed command, and an acceleration command and at least one of the position, speed, and acceleration of a motor that drives each joint. The required drive torque is calculated without recalculating all of the equations of motion, and the collision is determined by comparing the calculated required drive torque and the current of the motor that drives each joint.
[0008]
The robot control device according to the present invention includes a required drive torque command element calculated from at least one of a position command, a speed command, and an acceleration command, and a position of a motor that drives each joint estimated from the position command, the speed command, and the acceleration command, The required drive torque is calculated from at least one of speed and acceleration without recalculating all the equations of motion of the robot, and the calculated required drive torque is compared with the current of the motor that drives each joint. This is to make a determination.
[ 0009 ]
The robot control device according to the present invention sequentially estimates at least one of parameters necessary for calculation of a motion equation of the robot such as a friction coefficient during the operation of the robot, and uses the sequentially estimated parameters to determine a position command, a speed command, The required drive torque command element is calculated from at least one of the acceleration commands, and the equation of motion of the robot is re-established from at least one of the calculated required drive torque command element and the position, speed, and acceleration of the motor that drives each joint. The calculation is made using parameters obtained by sequentially estimating the required driving torque without recalculating all of them, and the collision is determined by comparing the calculated required driving torque and the current of the motor driving each joint.
[ 0010 ]
The robot control apparatus according to the present invention calculates the required driving torque using the estimated value of a predetermined time before, instead of the current estimated value of the parameter value that is sequentially estimated during the operation of the robot, and the calculated required driving torque And the current of the motor that drives each joint are compared to determine the collision.
[ 0011 ]
The robot control device according to the present invention provides a collision detection criterion twice, and detects a collision when the low level criterion is exceeded for a certain period of time or when the high level criterion is exceeded even for a moment.
[ 0012 ]
The robot control apparatus according to the present invention limits the torque for driving the motor based on the required driving torque calculated by the required driving torque calculating means.
[ 0013 ]
The robot control apparatus according to the present invention limits the torque for driving the motor based on the required drive torque command element calculated by the required drive torque command element calculation means.
[ 0014 ]
The robot control device according to the present invention includes: Using the measured motor drive current, Threshold value when comparing required drive torque and current of motor driving each joint Calculation Thus, the collision is determined.
[ 0015 ]
The robot control device according to the present invention includes an acceleration command and a jerk command. Using Threshold value for comparing required drive torque and motor current to drive each joint Calculation Thus, the collision is determined.
[ 0016 ]
The robot control device according to the present invention does not use the current hand load setting value or the hand load estimated value when performing the calculation in the required driving torque calculating means and the required driving torque command element calculating means, The calculation is performed using the hand load estimated value passed through the smoothing means.
[ 0017 ]
DETAILED DESCRIPTION OF THE INVENTION
An embodiment of the present invention will be described below.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a robot control apparatus according to Embodiment 1 of the present invention. In the figure, 1 is a command value generating means for generating a position command, a speed command, and an acceleration command, 2 is a position command, a speed command, Required drive torque command element calculation means for calculating a required drive torque command element from the acceleration command, 3 is a required drive torque calculation means for calculating the required drive torque from the position, speed, acceleration, required drive torque command element, speed command, and acceleration command 4 is a threshold value calculating means for calculating an upper threshold value and a lower threshold value, and 5 is a collision for determining a collision based on a necessary driving torque, an upper threshold value, a lower threshold value, and a motor driving current. The discriminating means 6 is a motor control means for controlling a motor for driving each axis of the robot.
[ 0018 ]
Next, the operation will be described.
First, in the command value generating means 1, when the robot is automatically operated, the operation command instructed by the program for operating the robot is interpreted, and the position that is the target position of each axis of the robot every moment. Generate directives. When the robot is manually operated, a position command that is a target position of each axis of the robot is generated every moment according to an input from a robot operation panel (not shown). Furthermore, in both cases of automatic operation and manual operation, a speed command and an acceleration command are also generated from a position command generated every moment.
[ 0019 ]
The required drive torque command element calculation means 2 inputs the position command, speed command, and acceleration command generated by the command value generation means 1 and calculates each element of the required drive torque command using the motion equation of the robot. The equation of motion of the robot is as follows: τ is a drive torque vector composed of the drive torque of each axis, q is a position vector composed of the position of each axis, v is a velocity vector composed of the speed of each axis, If the acceleration vector composed of acceleration is a,
τ = M (q) a + h (q, v) + g (q) + f (v) (1)
Can be written. Here, M (q) is the inertia matrix, h (q, v) is the centrifugal Coriolis force vector composed of the centrifugal Coriolis force of each axis, g (q) is the gravity vector composed of the gravity of each axis, f (V) is a friction force vector composed of the friction force of each axis. Using each command output from the command value generating means 1, a position command vector composed of position commands for each axis is represented by q _d , The speed command vector composed of speed commands for each axis _d , An acceleration command vector composed of acceleration commands for each axis is represented by a _d And the inertia force command element vector τ based on the equation (1) _md , Centrifugal Coriolis force command element vector τ _hd , Gravity command element vector τ _gd , Friction force command element vector τ _fd Are calculated as follows.
τ _md = M (q _d ) A _d ... (2)
τ _hd = H (q _d , V _d (3)
τ _gd = G (q _d (4)
τ _fd = F (v _d (5)
[ 0020 ]
The necessary drive torque calculating means 3 originally wants to calculate the drive torque τ based on the equation (1). However, when the number of axes of the robot is large, the amount of calculation of equation (1) is large, and the amount of calculation that must be processed per unit time in the control device increases. Therefore, first, the actual speed of each axis is monitored from the motor control means 6, and the speed vector v composed of the speed of each axis and the speed vector v monitored last time are displayed. _p , The acceleration of each axis is obtained, and an acceleration vector a composed of the acceleration of each axis is calculated. Next, the speed command vector v obtained by the necessary drive torque command element calculation means 2 _d , Acceleration command vector a _d , Inertial force command element vector τ _md , Centrifugal Coriolis force command element vector τ _hd , Gravity command element vector τ _gd , Friction force command element vector τ _fd And the required drive torque vector τ composed of the required drive torque of each axis from the velocity vector v and the acceleration vector a as follows. _e Calculate
I _md = Τ _md / N (a _d (6)
τ _me = N (a) * I _md ... (7)
I _hd = Τ _hd / (N (v _d )) 2 (8)
τ _he = (N (v)) 2 * I _hd ... (9)
τ _e = Τ _me + Τ _he + Τ _gd + Τ _fd ... (10)
I calculated by equation (6) _md And I calculated by equation (8) _hd Stores the previous calculation result, and n (a _d ) Or (n (v _d )) When 2 is less than or equal to a predetermined value, the calculation of the expression (6) or (8) is not performed, and the previous I _md Or I _hd (7) or (9) is calculated using the value of. Note that n () represents a vector norm.
[ 0021 ]
The threshold value calculation means 4 compares the required drive torque with the motor current and calculates a threshold value that serves as a reference for detecting a collision. In the first embodiment, an upper threshold value and a lower threshold value stored in advance for each axis are set.
[ 0022 ]
In the collision determination unit 5, first, the motor driving torque of each axis is calculated from the motor current input from the motor control unit 6. Next, the required drive torque calculated by the required drive torque calculation means 3 is input, and the difference from the actual motor drive torque is calculated for each axis. If the robot collides when the difference between the required drive torque and the motor drive torque on any of the axes is greater than or equal to the upper threshold value output from the threshold value calculation means 4, or less than the lower threshold value A determination is made and a stop command is transmitted to the command value generating means 1.
[ 0023 ]
The motor control means 6 controls the motor that drives the robot using the position command generated by the command value generation means 1, the speed command, the acceleration command, and the torque command element calculated by the necessary drive torque command element calculation means 2. I do.
[ 0024 ]
Embodiment 2. FIG.
FIG. 1 is a block diagram showing a robot control apparatus according to Embodiment 2 of the present invention, and the operation of each means other than the required drive torque calculating means 3 is the same as that in Embodiment 1, and thus the description thereof is omitted.
[ 0025 ]
Next, the operation will be described.
In the required drive torque calculation means 3, first, the speed vector v composed of the speeds of the respective axes inputted from the motor control means 6 and the previous speed vector v _p , The acceleration of each axis is obtained, and an acceleration vector a composed of the acceleration of each axis is calculated. Next, a speed command vector v composed of a speed command for each axis output from the necessary drive torque command element calculation means 2 _d , Centrifugal Coriolis force command element vector τ _hd , Gravity command element vector τ _gd , Friction force command element vector τ _fd Each axis position vector q, each axis velocity vector v, and each axis acceleration vector a output from the motor control means 6 are input, and a required drive torque vector τ composed of the required drive torque of each axis as follows. _e Calculate
τ _me = M (q) a (11)
I _hd = Τ _hd / (N (v _d )) ² (12)
τ _he = (N (v)) ² * I _hd ... (13)
τ _e = Τ _me + Τ _he + Τ _gd + F (v) (14)
I calculated by equation (12) _hd Stores the previous calculation result, and (n (v _d )) ² Is less than or equal to a predetermined value, the calculation of equation (12) is not performed and the previous I _hd (7) or (13) is calculated using the value of.
[ 0026 ]
Embodiment 3 FIG.
FIG. 2 is a block diagram showing a robot control apparatus according to Embodiment 3 of the present invention. In the figure, 8 is an adding means provided in place of the collision determining means 5 of Embodiments 1 and 2, and 9 is a collision determination. Means 10 are motor control means.
Other configurations are the same as those in FIG.
[ 0027 ]
Next, the operation will be described.
The adding means 8 adds each axis element of the upper threshold value calculated by the threshold value calculating means 4 to each axis element of the required driving torque calculated by the required driving torque calculating means 3, and an upper torque allowable value for each axis. Is calculated. Next, each axis element of the lower threshold value calculated by the threshold value calculation means 4 is added to each axis element of the necessary drive torque, and the lower torque allowable value for each axis is calculated.
[ 0028 ]
The collision determination means 9 compares the actual driving torque calculated from the motor current and the upper torque allowable value for each axis, and determines that the robot has collided when the actual driving torque is greater than the upper torque allowable value. It is also determined that the robot has collided when the actual driving torque is equal to or lower than the lower allowable value. When the collision is determined, the collision determination unit 9 transmits a stop command to the motor control unit 10 and also outputs a collision detection signal to the command value generation unit 1.
[ 0029 ]
The motor control means 10 is the same as the operation of the motor control means 6 shown in FIG. 1 except that the motor control means 10 incorporates the collision determination means 9 in the normal state. After the collision is detected, when a stop command is received from the collision determination means 9, a position command for stopping within the motor control means 10 is generated, and the position control of each axis is performed according to the generated position command.
[ 0030 ]
Embodiment 4 FIG.
FIG. 3 is a block diagram showing a robot control apparatus according to Embodiment 4 of the present invention. In the figure, 11 is a position / velocity acceleration estimating means, and 12 is a required drive torque calculating means.
Other configurations are the same as those in FIG.
[ 0031 ]
Next, the operation will be described.
First-order lag filter means simulating the relationship between the position command and the motor position is provided inside the position / velocity acceleration estimating means 11. Each axis element of the position command, speed command, and acceleration command generated by the command value generating means 1 is input to the first-order lag filter means, and the output from the first-order lag filter means is estimated position, estimated speed, and estimated, respectively. Acceleration.
In the necessary drive torque calculation means 12, a speed command vector v composed of speed commands for each axis. _d , An acceleration command vector a composed of acceleration commands for each axis _d , Inertial force command element vector τ _md , Centrifugal Coriolis force command element vector τ _hd , Gravity command element vector τ _gd , Friction force command element vector τ _fd And the necessary driving torque vector τ composed of the necessary driving torque of each axis from the estimated velocity vector v and estimated acceleration vector a estimated by the position / velocity acceleration estimating means 11 as follows. _e Calculate
I _md = Τ _md / N (a _d (15)
τ _me = N (a) * I _md ... (16)
I _hd = Τ _hd / (N (v _d )) ² ... (17)
τ _hd = (N (v)) ² * I _hd ... (18)
τ _e = Τ _me + Τ _he + Τ _gd + Τ _fd ... (19)
I calculated by equation (15) _md And I calculated by the equation (17) _hd Stores the previous calculation result, and n (a _d ) Or (n (v _d )) ² Is less than or equal to a predetermined value, the calculation of equation (15) or (17) is not performed and the previous I _md Or I _hd (16) or (18) is calculated using the value of.
[ 0032 ]
Embodiment 5 FIG.
A block diagram showing a robot control apparatus according to Embodiment 5 of the present invention is shown in FIG. 1, and the operation of each means other than the required drive torque calculating means 3 is the same as that in Embodiment 1, and therefore the description thereof is omitted.
[ 0033 ]
Next, the operation will be described.
The required driving torque calculation means 3 is provided with four first-order lag filter means of filter 1, filter 2, filter 3, and filter 4, and the time constants of these four first-order lag filter means are set independently. Make it possible. Inertial force command element vector τ calculated by necessary drive torque command element calculation means 2 _md , Centrifugal Coriolis force command element vector τ _hd , Gravity command element vector τ _gd , Friction force command element vector τ _fd Are input to the filter 1, filter 2, filter 3, and filter 4, respectively, and the output from the filter is the inertial force command element vector τ. _me , Centrifugal Coriolis force command element vector τ _he , Gravity command element vector τ _ge , Friction force command element vector τ _fe And required drive torque τ _e The
τ _e = Τ _me + Τ _he + Τ _ge + Τ _fe ... (20)
Calculate with
[ 0034 ]
Embodiment 6 FIG.
FIG. 4 is a block diagram showing a robot control apparatus according to Embodiment 6 of the present invention. In the figure, command value generation means 1, threshold value calculation means 4, collision determination means 5, and motor control means 6 are shown in FIG. Exactly the same as described. The required drive torque command element calculation means 2 has the same configuration and operation as the required drive torque command element calculation means shown in FIG. 1, but the parameters estimated by the parameter estimation means 14 are used when calculating each element. The point is different. The configuration and operation of the required drive torque calculation means 3 are exactly the same as those described in the second embodiment.
τ _p = Τ _me + Τ _he + Τ _gd (21)
Τ calculated by _p Is output to the parameter estimation means 14.
[ 0035 ]
Next, the operation will be described.
The parameter estimation means 14 identifies the viscous friction coefficient and the Coulomb friction coefficient of each axis online. First, from the motor drive current to the actual drive torque τ _r Is calculated. Next, the sum τ of the estimated torque other than the friction force input from the speed of the i-th axis, the actual drive torque, and the required drive torque calculation means 3 _p , V _i , Τ _ri , Τ _pi And the vector y _i , Parameter vector p _i Each
y _i = [V _i , Sgn (v _i ]] (22)
p _i = [P _ni , P _ci ] (23)
It is defined as Where p _ni , P _ci Are the viscous friction coefficient identification value and the Coulomb friction coefficient identification value of the i-th axis, respectively. Sgn () is a function that outputs -1 when the input is negative, 0 when the input is 0, and 1 when the input is positive. If the value in the k-th identification period is expressed as [k], the i-th axis friction coefficient identification value vector p in the k-th identification period _i [K] is
τ _mi [K] = τ _ri [K] -τ _pi [K] (24)
R _i [K] = R _i [K-1] + moit
* (− Σ _i * R _i [K-1] + y _i [K] ^T y _i [K])
... (25)
r _i [K] = r _i [K-1] + moit
* (− Σ _i * Q _i [K-1] + τ _mi [K] * y _i [K] ^T )
... (26)
p _i [K] = p _i [K-1] -moit
* G _i ・ [R _i [K] ・ p _i [K-1] -r _i [K])
... (27)
Thus, the identification is performed sequentially. Where G _i And σ _i Are a gain matrix and a weighting coefficient for adjusting the speed of identification, respectively. “Moit” is a period for performing parameter identification calculation.
The parameter estimation unit 14 transmits the friction coefficient of each axis identified as described above to the necessary drive torque command element calculation unit 2 and the necessary drive torque calculation unit 3. In the necessary drive torque command element calculation means 2, the frictional force command element vector τ of each axis _fd Is calculated using the friction coefficient transmitted from the parameter estimation means 14.
[ 0036 ]
Embodiment 7 FIG.
FIG. 5 is a block diagram showing a robot control apparatus according to Embodiment 7 of the present invention. In the figure, 15 is a required drive torque calculation means, 16 is a parameter estimation means, 17 is a motor control means, and 18 is a command value generation means. It is.
Other configurations are the same as those in FIG.
[ 0037 ]
Next, the operation will be described.
When the robot is automatically operated, the command value generation means 18 interprets an operation command instructed by a program for operating the robot and generates a position command that is a target position of each axis of the robot every moment. To do. When the robot is manually operated, a position command that is a target position of each axis of the robot is generated from moment to moment according to an input from a robot operation panel (operation means) (not shown). Further, in both cases of automatic operation and manual operation, a speed command is also generated from a position command that is generated every moment.
In the necessary drive torque calculation means 15, the sum τ of estimated torques other than the frictional force output to the parameter estimation means 16 _p , And the required drive torque τ output to the collision determination means 5 _e Is calculated as follows. First, an acceleration vector a composed of the acceleration of each axis is calculated from the velocity vector v composed of the speed of each axis and the previous velocity vector vp. Next, from the position vector q, velocity vector v, and acceleration vector a composed of the positions of each axis
τ _p = M (q) a + h (q, v) + g (q) (28)
The sum of estimated torques other than friction force τ _p To calculate
τ _f = F (v) (29)
The frictional force is calculated by Necessary driving torque τ output to the collision determination means 5 _e Is τ _p And τ _f The sum of
In the parameter estimation means 16, the sum τ of estimated torques other than the friction force _p Then, the friction coefficient of each axis is estimated from the motor drive current and the speed of each axis in the same manner as the parameter estimation means 14 in the sixth embodiment. The estimated friction coefficient is used when calculating the frictional force (29) in the required drive torque calculation means 15.
The motor control means 17 controls the position of the motor that drives the robot using the position command and speed command generated by the command value generation means 18.
[ 0038 ]
Embodiment 8 FIG.
6 is a block diagram showing a robot control apparatus according to an eighth embodiment of the present invention. In the figure, reference numeral 19 denotes parameter storage means.
Other configurations are the same as those in FIG.
[ 0039 ]
Next, the operation will be described.
The parameter accumulating unit 19 stores the parameter value estimated by the parameter estimating unit 14 for the designated period. For example, when it is specified that parameters for 10 cycles are stored, an area for storing parameters for 10 cycles is secured, and an initial value of the parameter is written in the area. For each parameter identification cycle, the parameter 10 cycles before is output to the required drive torque command element calculation means 2 and the required drive torque calculation means 3, and the parameter storage area for the remaining nine cycles is shifted by one cycle. Further, the parameter identification value in the latest parameter identification cycle input from the parameter estimation means 14 is written in the area for storing the parameters in the latest cycle. The required drive torque command element calculation means 2 and the required drive torque calculation means 3 calculate each element of the equation of motion using the parameters output from the parameter storage means 19.
[ 0040 ]
Embodiment 9 FIG.
FIG. 7 is a block diagram showing a robot control apparatus according to Embodiment 9 of the present invention. In the figure, 20 is an identification gain calculating means.
Other configurations are the same as those in FIG.
[ 0041 ]
Next, the operation will be described.
In the identification gain calculating means 20, a matrix G for adjusting the speed of identification described in the equation (27). _i Determine the value of. First, the speed of each axis is input from the motor control means 6. The absolute value of the input speed signal is obtained and compared with a reference value stored in advance. If the absolute speed value of the i-th axis is greater than or equal to the reference value, the pre-stored matrix G _i The value of is output. When the absolute velocity value of the i-th axis is less than or equal to the reference value, the matrix G _i The value of each element is output as 0. The parameter estimation means 14 outputs G output from the identification gain calculation means 20. _i Since the parameter estimated value is updated based on the parameter, if the velocity absolute value of a certain axis is less than or equal to the preset reference value, the parameter estimated value for that axis is exactly the same as the estimated value of the previous period, and the parameter estimated Is not done.
[ 0042 ]
Embodiment 10 FIG.
FIG. 8 is a block diagram showing a robot control apparatus according to Embodiment 10 of the present invention. In FIG. 8, reference numeral 21 denotes parameter estimated value limiting means.
Other configurations are the same as those in FIG.
[ 0043 ]
Next, the operation will be described.
The parameter estimated value limiting means 21 stores an upper limit value and a lower limit value of each parameter in advance. If the parameter output from the parameter storage means 19 is equal to or greater than the upper limit value, it is rewritten to the upper limit value. If the parameter output from the parameter storage means 19 is less than or equal to the lower limit value, it is rewritten to the lower limit value. When the parameter output from the parameter accumulating means 19 is not less than the lower limit and not more than the upper limit, the parameter is not rewritten. The required drive torque command element calculation means 2 and the required drive torque calculation means 3 perform calculations using the parameter values rewritten as necessary by the parameter estimated value limiting means 21.
[ 0044 ]
Embodiment 11 FIG.
FIG. 8 is also a block diagram showing a robot control apparatus according to Embodiment 11 of the present invention.
[ 0045 ]
Next, the operation will be described.
Among the blocks shown in FIG. 8, the motor control means 6 performs calculations with a period of x [ms], and each of the components 1 to 5 performs calculations with a period of n times x [ms]. , 20, and 21 perform calculations at a cycle of n × m times x [ms]. The required drive torque command element calculation means 2 and the required drive torque calculation means 3 perform calculations based on the input parameters in the period in which the parameter estimation values are input from the parameter estimation value limiting means 21. In the period when the parameter estimated value is not input from the parameter estimated value limiting means 21, the calculation is performed using the parameters input before the previous time.
[ 0046 ]
Embodiment 12 FIG.
FIG. 9 is a block diagram showing a robot control apparatus according to Embodiment 12 of the present invention. In the figure, reference numeral 25 denotes identification gain calculation means.
Other configurations are the same as those in FIG.
[ 0047 ]
Next, the operation will be described.
A parameter identification execution determination parameter is provided inside the identification gain calculation means 25. The parameter identification execution determination parameter has a value of 1 in the initial state immediately after the power is turned on. When the collision determination unit 5 detects a collision, a collision detection signal is transmitted to the identification gain calculation unit 25. When the identification gain calculation unit 25 receives the collision detection signal, the parameter identification execution determination parameter value is set to zero. The parameter identification execution determination parameter value holds 0 until the power is turned off and the power is turned on again or until the parameter identification restart is designated from the control panel (not shown).
Inside the identification gain calculation means 25, the parameter identification gain matrix G is determined according to the value of the parameter identification execution determination parameter. _i The value of each element of is selected. When the parameter identification execution determination parameter value is 1, the identification gain matrix G of each axis _i A value stored in advance as a value of each element is selected. When the parameter identification execution decision parameter value is 0, the identification gain matrix G of each axis _i 0 is selected as the value of each element. Inside the parameter estimation means 14, the G calculated by the identification gain calculation means 25 _i Since the parameter estimated value is updated based on the parameter identification value does not change until the power is shut off and the power is turned on again after the collision is detected or the parameter identification restart is designated from the control panel of the control device. Parameter identification is stopped.
[ 0048 ]
Embodiment 13 FIG.
FIG. 1 is a block diagram showing a robot control apparatus according to Embodiment 13 of the present invention, and the operation of each means other than threshold value calculation means 4 and collision determination means 5 is the same as that in Embodiment 2 and will be described. Is omitted.
[ 0049 ]
Next, the operation will be described.
The threshold value calculation means 4 reads the parameter values stored in advance, and the upper high level threshold value, the lower high level threshold value, the upper low level threshold value, the lower low level threshold value, the collision The discrimination time is set for each axis and output to the collision discrimination means 5. In the collision determination means 5, first, the actual motor driving torque of each axis is calculated from the motor current input from the motor control means 6. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. The collision determination timer is started when the torque difference in any of the axes becomes greater than or equal to the upper low level threshold or less than the lower low level threshold. When the torque difference of the shaft becomes equal to or lower than the upper low level threshold and equal to or higher than the lower low level threshold, the timer is stopped and the timer internal time is reset to zero. When the internal time of the timer is equal to or longer than the collision determination time set by the threshold value calculation means 4, it is determined that the robot has collided. When the collision of the robot is determined, a stop command is transmitted to the command value generation unit.
[ 0050 ]
Embodiment 14 FIG.
10 is a block diagram showing a robot control apparatus according to Embodiment 14 of the present invention. In the figure, 28 is a threshold value calculation means, 29 is a first collision determination means, and 30 is a second collision determination means.
Other configurations are the same as those in FIG.
[ 0051 ]
Next, the operation will be described.
The threshold value calculation means 28 reads the parameter values stored in advance, and the upper high level threshold value, the lower high level threshold value, the upper low level threshold value, the lower low level threshold value, the collision The discrimination time is set for each axis, and the upper low level threshold, the lower low level threshold, and the collision discrimination time are output to the first collision discrimination means 29, and the upper high level threshold and the lower high level are output. The threshold value is output to the second collision determination means 30.
The first collision determination means 29 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 10. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. The collision determination timer is started when the torque difference in any of the axes becomes greater than or equal to the upper low level threshold or less than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When it is determined that the robot has collided, a stop command is transmitted to the command value generating means 1.
The second collision determination unit 30 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control unit 10. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 30 transmits a stop command to the motor control unit 10 and also outputs a collision detection signal to the command value generation unit 1.
[ 0052 ]
Embodiment 15 FIG.
11 is a block diagram showing a robot control apparatus according to Embodiment 15 of the present invention. In the figure, 31 is a torque margin calculating means, 32 is a torque limit value calculating means, 33 is a motor control means, and 34 is a motor current command. The generating means 35 is a motor current limiting means (motor driving torque limiting means), 36 is a motor current control means, and 37 is a second collision determining means.
Other configurations are the same as those shown in FIGS.
[ 0053 ]
Next, the operation will be described.
The torque margin calculating means 31 reads the upper low level threshold value and the lower low level threshold value from the threshold value calculating means 28. The torque margin calculating means 31 stores the magnification α in advance, and the upper torque margin value of each axis is obtained by multiplying each of the upper low level threshold value and the lower low level threshold value of each axis by α. And a lower torque margin value is calculated.
The torque limit value calculation means 32 adds the upper torque margin value of each axis to the required drive torque of each axis calculated by the required drive torque calculation means 3, and calculates the drive torque upper limit value of each axis. Similarly, the lower torque margin value of each axis is added to the required drive torque of each axis calculated by the required drive torque calculation means 3, and the lower limit value of the drive torque of each axis is calculated. The upper torque limit value and the lower torque limit value calculated by the torque limit value calculation means 32 are transmitted to the motor current limit means 35.
The motor current limiting means 35 converts the received upper torque limit value and lower torque limit value into the upper motor current limit value and the lower current limit value, respectively, taking into consideration the reduction ratio, the motor torque constant, and the like. Further, the current command is limited so that the current command output from the motor current command generating means 34 is not more than the upper motor current limit value and not less than the lower current limit value. Specifically, when the current command is equal to or higher than the upper current limit value, the current command is replaced with the upper motor current limit value. When the current command is below the lower current limit value, the current command is replaced with the lower motor current limit value. The motor current control means 36 controls the current of the motor that drives each axis based on the current command restricted by the motor current restriction means 35.
First, the second collision determination unit 37 calculates a drive torque command for the actual motor of each axis from the motor current command output from the motor current command generation unit 34 in consideration of the reduction ratio and the torque constant. Next, a torque difference that is the difference between the calculated motor drive torque command and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 37 transmits a stop command to the motor control unit 33 and also outputs a collision detection signal to the command value generation unit 1.
When the motor control unit 33 receives a stop command from the second collision determination unit 37 after detecting a collision, the motor control unit 33 generates a position command for stopping in the motor control unit 33 and performs position control according to the generated position command.
[ 0054 ]
Embodiment 16 FIG.
A block diagram showing a robot control apparatus according to Embodiment 16 of the present invention is shown in FIG. 40, and the operation of each means other than torque limit value calculation means 32 is the same as that in Embodiment 15, and thus the description thereof is omitted.
[ 0055 ]
Next, the operation will be described.
In the torque limit value calculating means 32, first, the inertia force command element vector τ output from the necessary drive torque command element calculating means 2 is used. _md , Centrifugal Coriolis force command element vector τ _hd , Gravity command element vector τ _gd , Friction force command element vector τ _fd Torque command vector τ composed of torque commands for each axis _d Is calculated. Next, the calculated torque command vector τ _d Are input to the first-order lag filter means, and a vector composed of the outputs is τ _df And In addition, τ _df The upper torque margin value of each axis is added to each axis element to calculate the upper limit value of driving torque for each axis. Similarly τ _df The lower torque margin value of each axis is added to each of the shaft elements, and the drive torque lower limit value of each axis is calculated. The upper torque limit value and the lower torque limit value calculated by the torque limit value calculation means 32 are transmitted to the motor current limit means 35.
[ 0056 ]
Embodiment 17. FIG.
A block diagram showing a robot control apparatus according to Embodiment 17 of the present invention is shown in FIG. 1, and the operations of the respective means other than the threshold calculation means 4 are the same as those in Embodiment 1, and therefore description thereof is omitted.
[ 0057 ]
Next, the operation will be described.
In the robot control apparatus according to the seventeenth embodiment, only when the teaching mode changeover switch is pushed into the teaching side, the robot can be manually moved based on the operation on the manual operation panel. Only when the mode switch is pushed to the automatic operation side, automatic operation based on the program can be performed. The teaching mode determination means (not shown) sets the value of the teaching mode determination parameter according to the state of the teaching mode changeover switch attached to the operation panel of the control device (not shown). Specifically, when the teaching mode change switch is pushed to the teaching side, the value of the teaching mode discrimination parameter is set to 1. When the teaching mode change switch is pushed to the automatic operation side, the value of the teaching mode discrimination parameter is set to 0. And
The threshold value calculation means 4 includes an upper threshold value and a lower threshold value for each axis for operating the robot from the manual operation panel, and an upper threshold value for each axis for automatic operation based on a program. The value and the lower threshold value are stored in advance. When the value of the teaching mode discrimination parameter input from the teaching mode discrimination means is 1, the upper threshold value and the lower threshold value of each axis for operating the robot from the manual operation panel are adopted. When the value of the teaching mode discrimination parameter input from the teaching mode discrimination means is 0, the upper threshold value and the lower threshold value of each axis for automatic operation based on the program are adopted.
[ 0058 ]
Embodiment 18 FIG.
FIG. 1 is a block diagram showing a robot control apparatus according to Embodiment 18 of the present invention, and the operation of each means other than threshold value calculation means 4 is the same as that in Embodiment 1, and therefore description thereof is omitted.
[ 0059 ]
Next, the operation will be described.
The timer means (not shown) starts the timer from 0 when the motor power is turned on and outputs the elapsed time after the power is turned on. When the motor power is cut off, the timer is stopped and the elapsed time after turning on the power is reset to zero.
In the threshold value calculation means 4, the first upper threshold value and the first lower threshold value used when the specified time value, the elapsed time since turning on the motor power is less than the specified value, and the elapsed time since turning on the motor power A second upper threshold value and a second lower threshold value that are used when the time is equal to or greater than the specified value are stored in advance. The stored prescribed value of the time is compared with the elapsed time after turning on the motor power output from the timer means, and when the prescribed time value is larger, the first upper threshold value and the first lower threshold value When the time regulation value is smaller, the second upper threshold value and the second lower threshold value are adopted.
[ 0060 ]
Embodiment 19. FIG.
A block diagram showing a robot control apparatus according to Embodiment 19 of the present invention is shown in FIG. 1, and the operation of each means other than the threshold value calculation means 4 is the same as that in Embodiment 1 and will not be described.
[ 0061 ]
Next, the operation will be described.
A hand load setting value reading means (not shown) reads a hand load parameter value specified by a program instruction or a hand load parameter value stored in the control device. The hand load setting value reading means has a table according to the hand load and the center of gravity position of the hand load as shown in the table below, and outputs the group number to which the current hand load setting value belongs. .
[Table 1]

The threshold value calculation means 4 is provided with an upper threshold value table and a lower threshold value table for each group to which the hand load belongs, and the upper threshold value of the group number input from the hand load set value reading means. The value and the lower threshold value are output as the upper threshold value and the lower threshold value, respectively.
[ 0062 ]
Embodiment 20. FIG.
A block diagram showing a robot control apparatus according to Embodiment 20 of the present invention is shown in FIG. 1, and the operations of the respective means other than threshold value calculating means 4 are the same as those in Embodiment 1, and will not be described.
[ 0063 ]
Next, the operation will be described.
A detection sensitivity reading means (not shown) first reads the current detection sensitivity setting value. The detection sensitivity is set using a command described in the program or from a manual operation panel of the control device. Next, the detection sensitivity reading means calculates the magnification α from the read detection sensitivity. For example, when the detection sensitivity is X%, the magnification α is calculated as α = 100 / X. The calculated magnification is output to the threshold value calculation means 4. The threshold value calculation means 4 stores in advance an upper threshold value and a lower threshold value when the detection sensitivity is 100%. In the threshold value calculation means 4, the stored upper and lower threshold values are multiplied by the magnification α inputted from the detection sensitivity reading means as the upper threshold value and the lower threshold value, respectively. Output.
[ 0064 ]
Embodiment 21. FIG.
FIG. 12 is a block diagram showing a robot control apparatus according to Embodiment 21 of the present invention. In the figure, 47 is an identification progress determination means, and 48 is a threshold value calculation means.
Other configurations are the same as those in FIG.
[ 0065 ]
Next, the operation will be described.
The identification progress determination means 47 outputs an identification progress vector composed of the identification progress of each axis. First, the speed of each axis is input to the identification progress determination means 47. The identification progress degree discriminating means 47 stores in advance a speed regulation value for each axis and a time regulation value for each axis. The identification progress degree discriminating means 47 calculates, for each axis, the accumulated time during which the speed is equal to or greater than the speed default value. The degree is 1. If the accumulated time is less than the specified time, the identification progress of the axis is 0. When the motor power is cut off, the accumulated time is reset to zero.
The threshold value calculation means 48 stores therein an upper threshold value and a lower threshold value before the parameter identification progress, and an upper threshold value and a lower threshold value after the parameter identification progress. For the axis with the identification progress degree 1 output from the identification progress degree judging means 47, the upper threshold value and the lower threshold value after the progress of parameter identification are set, and the identification output from the identification progress degree judging means 47 For an axis with a progression degree of 0, an upper threshold value and a lower threshold value before parameter identification progress are set.
[ 0066 ]
Embodiment 22. FIG.
FIG. 13 is a block diagram showing a robot control apparatus according to Embodiment 22 of the present invention. In the figure, reference numeral 49 denotes identification progress determination means.
Other configurations are the same as those in FIG.
[ 0067 ]
Next, the operation will be described.
First, a parameter identification value for each parameter identification period is input from the parameter estimation unit 14 to the identification progress determination unit 49. The identification progress degree discriminating means 49 selects the Coulomb friction coefficient and the viscous friction coefficient of each axis from the input parameter identification values, and calculates the respective variations. The identification progress degree discriminating means 49 stores prescribed values of the estimated Coulomb friction coefficient variation and viscous friction coefficient variation for each axis, and stores both actual Coulomb friction coefficient variation and viscous friction coefficient variation. The identification progress is set to 1 with respect to an axis whose is less than or equal to the specified value. An axis in which at least one of the actual Coulomb friction coefficient change amount and viscous friction coefficient change amount is not less than a specified value has an identification progress of 0.
[ 0068 ]
Embodiment 23. FIG.
FIG. 14 is a block diagram showing a robot control apparatus according to Embodiment 23 of the present invention. In the figure, 50 is a torque command absolute value maximum value calculating means, and 51 is a threshold value calculating means.
Other configurations are the same as those in FIG.
[ 0069 ]
Next, the operation will be described.
First, the torque command absolute value maximum value calculation means 50 first receives the inertial force command element vector τ output from the necessary drive torque command element calculation means 2. _md , Centrifugal Coriolis force command element vector τ _hd Is entered. Next, inertial force command element vector τ _md , Centrifugal Coriolis force command element vector τ _hd Each axis element is added, the absolute value of the addition result is obtained, and the obtained absolute value is converted into the scale of the motor current command of each axis in consideration of the reduction ratio and torque constant of each axis. Further, normalization is performed based on the rated current of each axis motor. T is the maximum of the absolute values of the sum of the inertia force command and the centrifugal Coriolis force command normalized by the rated current of each axis motor calculated for each axis. _max Output as.
The threshold value calculation means 51 stores the upper threshold base portion, the lower threshold base portion, the upper threshold torque command magnification, and the lower threshold torque command magnification for each axis. T output from torque command absolute value maximum value calculation means 50 _max , The upper threshold value of each axis is (upper threshold base amount) + (upper threshold torque command magnification) × T _max The lower threshold value is (lower threshold base portion) + (lower threshold torque command magnification) × T _max Is calculated by
[ 0070 ]
Embodiment 24. FIG.
15 is a block diagram showing a robot control apparatus according to Embodiment 24 of the present invention. In the figure, reference numeral 52 denotes a motor current absolute value maximum value calculation means, and 53 denotes a threshold value calculation means.
Other configurations are the same as those in FIG.
[ 0071 ]
Next, the operation will be described.
The motor current absolute value maximum value calculation means 52 first inputs the motor current measurement value of each axis, calculates the absolute value of the current measurement value of each axis, and then normalizes it with the rated current of each axis motor. Next, among the normalized absolute values of the current measured values for each axis, _max Output as.
In the threshold value calculation means 53, the upper threshold base portion, the lower threshold base portion, the upper threshold current magnification, and the lower threshold current magnification are stored for each axis. D output from absolute value maximum value calculation means 52 _max The upper threshold value of each axis is (upper threshold base portion) + (upper threshold current magnification) × D _max The lower threshold is (lower threshold base portion) + (lower threshold current magnification) × D _max Is calculated by
[ 0072 ]
Embodiment 25. FIG.
16 is a block diagram showing a robot control apparatus according to Embodiment 25 of the present invention. In the figure, 54 is a torque command torque command difference absolute value maximum value calculating means, and 55 is a threshold value calculating means.
Other configurations are the same as those in FIG.
[ 0073 ]
Next, the operation will be described.
In the torque command torque command difference absolute value maximum value calculating means 54, first, the inertial force command element vector τ output from the necessary drive torque command element calculating means 2. _md , Centrifugal Coriolis force command element vector τ _hd Is entered. Next, inertial force command element vector τ _md , Centrifugal Coriolis force command element vector τ _hd Τ with each axis element added _mhd Is calculated. τ _mhd Each axis element of is stored for one period, and this time τ _mhd Value and previous τ _mhd Is the difference between the values of dτ _mhd To calculate τ _mhd And dτ _mhd A vector composed of absolute values of each axis _mhd And adτ _mhd Is calculated. Next, using the constant weighting factor w for all axes,
wτ _mhdi = Aτ _mhdi + W × adτ _mhdi ... (30)
Wτ composed of calculated by _mhd Is calculated. In the equation (30), the subscript i means an element on the i-th axis. Furthermore, wτ _mhdi Is converted into a motor current command scale using the reduction ratio and torque constant of the shaft, and normalized with the rated current of the shaft motor is sτ _mhdi Calculate as sτ _mhdi The maximum value of S _max Output as.
The threshold calculation means 55 includes an upper threshold base portion, a lower threshold base portion, an upper threshold torque command torque command difference magnification, and a lower threshold torque command torque command difference magnification for each axis. S is output from the torque command torque command difference absolute value maximum value calculation means 54. _max The upper threshold value of each axis is (upper threshold base amount) + (upper threshold torque command torque command difference magnification) × S _max The lower threshold value is (lower threshold base portion) + (lower threshold torque command torque command differential magnification) × S _max Is calculated by
[ 0074 ]
Embodiment 26. FIG.
FIG. 17 is a block diagram showing a robot control apparatus according to Embodiment 26 of the present invention. In FIG. 17, 56 is a torque command torque command difference absolute value calculation means, and 57 is a threshold value calculation means.
Other configurations are the same as those in FIG.
[ 0075 ]
Next, the operation will be described.
First, the torque command torque command difference absolute value calculation means 56 first receives the inertial force command element vector τ output from the necessary drive torque command element calculation means 2. _md , Centrifugal Coriolis force command element vector τ _hd Is entered. Next, inertial force command element vector τ _md , Centrifugal Coriolis force command element vector τ _hd Τ with each axis element added _mhd Is calculated. τ _mhd Each axis element of is stored for one period, and this time τ _mhd Value and previous τ _mhd Is the difference between the values of dτ _mhd To calculate τ _mhd And dτ _mhd A vector composed of absolute values of each axis _mhd And adτ _mhd Is calculated. Next, wτ configured by calculating the element of the i-th axis using the constant weighting factor w for all the axes by the equation (30). _mhd Is calculated. In the equation (30), the subscript i means an element on the i-th axis. Furthermore, wτ _mhdi Is converted into a motor current command scale using the reduction ratio and torque constant of the shaft, and normalized with the rated current of the shaft motor is sτ _mhdi Calculate as Finally, sτ _mhdi Is input to the first-order lag filter means, and the output of the first-order lag filter means is fτ _mhdi Output as.
The threshold value calculation means 57 includes an upper threshold base portion, a lower threshold base portion, an upper threshold torque command torque command difference magnification, and a lower threshold torque command torque command difference magnification for each axis. Fτ output from the torque command torque command difference absolute value maximum value calculating means 56 _mhdi The upper threshold value of each axis is (upper threshold base amount) + (upper threshold torque command torque command differential magnification) × fτ _mhdi The lower threshold value is (lower threshold base portion) + (lower threshold torque command torque command differential magnification) × fτ _mhdi Is calculated by
[ 0076 ]
Embodiment 27. FIG.
18 is a block diagram showing a robot control apparatus according to Embodiment 27 of the present invention. In the figure, 58 is an acceleration command jerk command absolute value calculation means, and 59 is a threshold value calculation means.
Other configurations are the same as those in FIG.
[ 0077 ]
Next, the operation will be described.
First, the acceleration command jerk command absolute value calculation means 58 receives an acceleration command a for each axis. _d Is entered. a _d Each axis element is stored for one period, and this time a _d Value and previous a _d J which is the difference between _d Is calculated for each axis. Next, a _d And j _d Vector aa composed of absolute values of each axis element _d And aj _d Is calculated. Next, using the constant weighting factor w for all axes,
waj _di = Aa _di + W × aj _di ... (31)
Waj composed of calculated by _d Is calculated. In the formula (31), the subscript i means an element on the i-th axis. waj _d Is output from the acceleration command jerk command absolute value calculation means 58 to the threshold value calculation means 59.
The threshold value calculation means 59 includes an upper threshold base portion, a lower threshold base portion, an upper threshold acceleration command jerk command magnification, and a lower threshold acceleration command jerk command magnification for each axis. Waj output from the acceleration command jerk command absolute value calculation means 58 _di The upper threshold value of each axis is (upper threshold base amount) + (upper threshold acceleration command jerk command magnification) × waj _di The lower threshold value is (lower threshold base portion) + (lower threshold acceleration command jerk command magnification) × waj _di Is calculated by
[ 0078 ]
Embodiment 28. FIG.
FIG. 19 is a block diagram showing a robot control apparatus according to Embodiment 28 of the present invention. In the figure, reference numeral 60 denotes a threshold multiplication factor calculation means, and 61 denotes a threshold calculation means.
Other configurations are the same as those in FIG.
[ 0079 ]
Next, the operation will be described.
First, a position command, a speed command, and an acceleration command are input to the threshold value factor calculation unit 60. The speed command and the acceleration command are converted into a speed command and acceleration command scale in the motor in consideration of the reduction ratio. Further, a motor level speed command maximum value that is the maximum value of the speed command of each axis after the scale conversion and a motor level acceleration command maximum value that is the maximum value of the acceleration command of each axis after the scale conversion are calculated. A threshold magnification table corresponding to each of the range to which the position command belongs, the motor level speed command maximum value, and the motor level acceleration command maximum value is prepared in the threshold magnification element calculation means 60, and the current position The magnifications bp, bv, and ba corresponding to the command, speed command, and acceleration command are read from the table and output.
The threshold value calculation means 61 stores an upper threshold value and a lower threshold value at a magnification of 1 for each axis. Next, from the magnifications bp, bv, and ba output from the threshold magnification factor calculation means 60, the upper threshold value and the lower threshold value at the magnification of 1 for each axis are respectively multiplied by bp × bv × ba. Then output.
[ 0080 ]
Embodiment 29. FIG.
FIG. 20 is a block diagram showing a robot control apparatus according to Embodiment 29 of the present invention. In the figure, 62 is a stop determining means, and 63 is a threshold value calculating means.
Other configurations are the same as those in FIG.
[ 0081 ]
Next, the operation will be described.
First, a speed command and a motor speed are input to the stop determination means 62. The stop determination means 62 stores a stop determination specified value for each axis, and compares both the speed command and the motor speed with the stop determination specified value of the axis. Calculate the absolute value of each axis element of the speed command and each axis element of the motor speed, determine that the axis whose both speed command absolute value and motor speed absolute value are below the specified value is stopped, The value of the element is set to 1. When at least one of the speed command absolute value and the motor speed absolute value is equal to or greater than a specified value, it is determined that the motor is operating, and the value of the stop determination vector element of the axis is set to zero.
The upper threshold value, the lower threshold value, and the Coulomb friction coefficient of each axis are stored in the threshold value calculation means 63. For the axis where the element value of the stop determination vector output from the stop determination means 62 is 0, the upper threshold value and the lower threshold value are output as they are. For the axis whose stop discriminating vector element value output from the stop discriminating means 62 is 1, a value obtained by adding the Coulomb friction coefficient to the stored upper threshold value is output as the upper threshold value. Further, a value obtained by subtracting the Coulomb friction coefficient from the stored lower threshold value is output as the lower threshold value.
[ 0082 ]
Embodiment 30. FIG.
FIG. 21 is a block diagram showing a robot control apparatus according to Embodiment 30 of the present invention. In the figure, reference numeral 64 denotes special work determination means, 65 denotes threshold value calculation means, and 66 denotes program analysis means.
Other configurations are the same as those in FIG.
[ 0083 ]
Next, the operation will be described.
The program analysis unit 66 sequentially reads the program to be executed and sequentially transmits necessary information to the command value generation unit 1 and the like. When the program analysis unit 66 processes the hand opening / closing command, it transmits a hand opening / closing command to a hand opening / closing unit (not shown) and also transmits a hand opening / closing command to the special work determination unit 64. When receiving the hand opening / closing command, the special work determination unit 64 sets the value of the special work determination parameter to 1 for a predetermined time stored in advance. The initial value of the special work determination parameter is 0, and is returned to 0 again after a specified time elapses after receiving the hand opening / closing command.
The threshold value calculation means 65 stores a normal work upper threshold value and a lower threshold value, and a special work upper threshold value and a lower threshold value. When the value of the special work determination parameter output from the special work determination means 64 is 1, the special work upper threshold and the lower threshold are set, and when the special work determination parameter is 0, A normal work upper threshold and a lower threshold are set.
[ 0084 ]
Embodiment 31. FIG.
FIG. 22 is a block diagram showing a robot control apparatus according to Embodiment 31 of the present invention, in which 67 is a smoothing means.
Other configurations are the same as those in FIG.
[ 0085 ]
Next, the operation will be described.
The smoothing unit 67 sequentially receives the upper torque margin value and the lower torque margin value from the torque margin calculation unit 31. The smoothing means 67 is provided with a two-stage moving average filter means. The result obtained by sequentially passing the received upper torque margin value and lower torque margin value through the two-stage moving average filter is the upper torque margin value. And the lower torque margin value is transmitted to the torque limit value calculating means 32. Even when the low-level upper threshold value and the low-level lower threshold value calculated by the threshold value calculating means 28 change stepwise by passing through the two-stage moving average filter, the torque limit value calculating means 32. The upper motor current limit value and the lower motor current limit value output from are smoothly changed.
[ 0086 ]
Embodiment 32. FIG.
FIG. 23 is a block diagram showing a robot control apparatus according to Embodiment 32 of the present invention. In FIG. 23, reference numeral 68 denotes torque limit value holding means.
Other configurations are the same as those in FIG.
[ 0087 ]
Next, the operation will be described.
In the torque limit value holding means 68, the value of the collision state determination parameter held internally in the normal state is 0, and when the value of the collision state determination parameter is 0, the torque limit value is stored in the current cycle. The upper torque limit value and the lower torque limit value input from the calculation means 32 are written and stored in the torque limit value storage area. Further, the upper torque limit value and the lower torque limit value input from the torque limit value calculation means 32 are output as they are. When a collision detection signal is received from the first collision determination unit 29 or the second collision determination unit 37, the value of the collision state determination parameter is set to 1. When the value of the collision state determination parameter is 1, writing of the upper torque limit value and the lower torque limit value in the current cycle to the storage area is stopped, and the upper torque limit value and the lower torque stored in the storage area are stopped. Output the limit value. The value of the collision state determination parameter is returned to 0 when the motor power is shut off.
[ 0088 ]
Embodiment 33. FIG.
FIG. 24 is a block diagram showing a robot control apparatus according to Embodiment 33 of the present invention. In the figure, 69 is a hand load set value reading means, and 70 is a smoothing means.
Other configurations are the same as those in FIG.
[ 0089 ]
Next, the operation will be described.
The hand load set value reading means 69 reads and outputs the hand load mass, the hand load center of gravity position, and the hand load inertia moment moment by moment. The smoothing means 70 is provided with a two-stage moving average filter, and the hand load mass, hand load center of gravity position, and hand load inertia moment output from the hand load set value reading means 69 are moved in two stages. The result passed through the average filter is output to the required drive torque command element calculation means 2 and the required drive torque calculation means 3. The required drive torque command element calculation means 2 and the required drive torque calculation means 3 calculate the equation of motion using the hand load mass, the hand load gravity center position, and the hand load inertia moment output from the smoothing means 70.
[ 0090 ]
Embodiment 34. FIG.
A block diagram showing a robot control apparatus according to Embodiment 34 of the present invention is shown in FIG. 1, and the operations of the respective means other than the collision determination means 5 are the same as those in Embodiment 2 and will not be described.
[ 0091 ]
Next, the operation will be described.
A collision state determination parameter is provided inside the collision determination means 5. The initial value of the collision state determination parameter is zero. When the value of the collision state determination parameter is 0, the collision determination unit 5 actually calculates the motor driving torque of each axis from the motor current input from the motor control unit 6. Next, the difference between the required drive torque calculated by the required drive torque calculation means 3 and the actual motor drive torque is calculated for each axis. When the difference between the required drive torque and the motor drive torque on any axis is greater than or equal to the upper threshold value or less than or equal to the lower threshold value, it is determined that the robot has collided and the command value generation unit 1 stops. The command is transmitted and the value of the collision state determination parameter is set to 1.
Even when the value of the collision state determination parameter is 1, the difference between the required drive torque and the actual motor drive torque is calculated for each axis, and the difference between the required drive torque and the motor drive torque on any axis is equal to or greater than the upper threshold value. In this case, it is determined that the robot has collided when the value is equal to or lower than the lower threshold value, but the stop command is not transmitted to the command value generation unit 1. The value of the collision state determination parameter is reset to 0 when the control device is turned off and then turned on again, or when the restart of the collision determination function is set from the operation panel of the program or the control device.
[ 0092 ]
Embodiment 35. FIG.
A block diagram showing a robot control apparatus according to Embodiment 35 of the present invention is shown in FIG. 1, and the operation of each means other than collision determination means 5 is the same as that in Embodiment 2 and will not be described.
[ 0093 ]
Next, the operation will be described.
A single stage moving average filter means is provided inside the collision determination means 5. The output when the required drive torque input to the collision determination means 5 is passed through the moving average filter is set as the required drive torque after filtering. Next, the actual motor driving torque for each axis is calculated from the motor current input to the collision determination means 5, and the output when the calculated motor driving torque for each axis is passed through the moving average filter is the filtered motor driving torque. To do. The difference between the post-filter required drive torque and the post-filter motor drive torque is calculated for each axis. It is determined that the robot has collided when the difference between the post-filter drive torque and the post-filter motor drive torque on any axis is greater than or equal to the upper threshold value or less than the lower threshold value. A stop command is transmitted to the generation means 1.
[ 0094 ]
Embodiment 36. FIG.
A block diagram showing a robot control apparatus according to Embodiment 36 of the present invention is shown in FIG. 1, and the operation of each means other than the command value generating means 1 is the same as that in Embodiment 34, and thus description thereof is omitted.
[ 0095 ]
Next, the operation will be described.
In the command value generating means 1, the position command is expressed by a parameter having a value between 0 and 1 for each operation to be executed by the robot, and the parameter is increased smoothly from 0 to 1 every moment. Generate a position command. When a stop command is received from the collision determination means 5, the current value X of the parameter is _c And the increase in the parameter per unit time is smoothly reduced to zero. The parameter value at the point where the increase in parameter per unit time becomes zero is X _s And The command value generation means 1 includes a parameter return amount X _l Is stored in advance. X _c -X _l And 0 and compare the larger X _t And Immediately after the increase of the parameter per unit time becomes zero, the parameter is set to X _s To X _t A position command for avoiding the collision state is generated by smoothly reducing the position command to.
[ 0096 ]
Embodiment 37. FIG.
FIG. 25 is a block diagram showing a robot control apparatus according to Embodiment 37 of the present invention. In the figure, 74 is first collision determination means, 75 is second collision determination means, 76 is position speed storage means, and 77 is excessive. It is a reverse discrimination means.
Other configurations are the same as those in the sixteenth embodiment, and a description thereof will be omitted.
[ 0097 ]
Next, the operation will be described.
The first collision determination unit 74 transmits a collision detection signal to the position / velocity storage unit 76. In addition, the second collision determination unit 75 transmits a collision detection signal to the position / speed storage unit 76.
When the position / velocity storage unit 76 receives a collision detection signal from the first collision determination unit 74 or the second collision determination unit 75, the position / velocity storage unit 76 stores the joint displacement and the joint velocity of each axis when received. The excessive inversion determination means 77 reads the joint displacement and the joint speed stored in the position / speed storage means 76, and the current joint displacement obtained from the motor control means 33 is obtained from the joint displacement stored in the position / speed storage means 76. If the joint speed stored in the position speed storage means 76 is away from the specified value in the opposite direction, it is determined that excessive reversal has occurred, and an immediate stop command is transmitted to the motor control means 33.
[ 0098 ]
Embodiment 38. FIG.
FIG. 25 is a block diagram showing a robot control apparatus according to Embodiment 38 of the present invention. The operations of the respective means other than the transient inversion determination means 77 are the same as those in Embodiment 37, and thus description thereof is omitted.
[ 0099 ]
Next, the operation will be described.
The excessive inversion determination means 77 reads the joint displacement and the joint speed stored in the position / speed storage means 76, and the current joint speed obtained from the motor control means 33 is the joint joint speed stored in the position / speed storage means 76. Is reversed and the absolute value is equal to or greater than the specified value, it is determined that excessive reversal has occurred, and an immediate stop command is transmitted to the motor control means 33.
[ 0100 ]
Embodiment 39. FIG.
26 is a block diagram showing a robot control apparatus according to Embodiment 39 of the present invention. In the figure, 79 is a first collision determination means, 80 is a motor control means, 81 is a second collision determination means, and 82 is a second collision determination means. Command value generation means.
Other configurations are the same as those in the sixteenth embodiment, and a description thereof will be omitted.
[ 0101 ]
Next, the operation will be described.
First collision discriminating means 79 first calculates the actual motor driving torque of each axis from the motor current input from motor control means 80. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. The collision determination timer is started when the torque difference in any of the axes becomes greater than or equal to the upper low level threshold or less than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When it is determined that the robot has collided, a stop command is transmitted to the command value generation means 1 and the second command value generation means 82.
The second collision discriminating means 81 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 80. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 81 transmits a stop command to the command value generation unit 1 and the second command value generation unit 82.
When receiving the stop command from the first collision determination unit 79 or the second collision determination unit 81, the second command value generation unit 82 generates a deceleration stop command for quickly decelerating and stopping from the current speed for each axis that drives the robot. To do. The motor control unit 80 ignores the command value sent from the command value generation unit 1 and performs motor control based on the deceleration stop command generated by the second command value generation unit 82.
[ 0102 ]
Embodiment 40. FIG.
27 is a block diagram showing a robot control apparatus according to Embodiment 40 of the present invention. In the figure, 83 is a first collision determination means, 84 is a motor control means, 85 is a second collision determination means, and 86 is a torque limiter. It is a value calculation means.
Other configurations are the same as those in FIG.
[ 0103 ]
Next, the operation will be described.
First collision determination means 83 first calculates the actual motor drive torque of each axis from the motor current input from motor control means 84. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. The collision determination timer is started when the torque difference in any of the axes becomes greater than or equal to the upper low level threshold or less than the lower low level threshold. When the torque difference of the shaft becomes equal to or lower than the upper low level threshold and equal to or higher than the lower low level threshold, the timer is stopped and the timer internal time is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When it is determined that the robot has collided, a stop command is transmitted to the command value generation unit 1 and a collision detection signal and a collision detection axis number are transmitted to the torque limit value calculation unit 86.
The second collision discriminating means 85 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 84. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 85 transmits a stop command to the command value generation unit 1 and transmits a collision detection signal and a collision detection axis number to the torque limit value calculation unit 86.
The operation of the torque limit value calculating means 86 before receiving the collision detection signal is exactly the same as the operation of the torque limit value calculating means 32 of FIG. 37, but the collision detection signal from the first collision determining means 83 or the second collision determining means 85. When the collision detection axis number is received, the upper torque limit value and the lower torque limit value are fixed to the current value only for the axis having the same number as the collision detection axis number. The upper and lower torque limit values for the remaining shafts continue to be calculated in exactly the same manner as before the collision detection signal is received.
[ 0104 ]
Embodiment 41. FIG.
A block diagram showing a robot control apparatus according to Embodiment 41 of the present invention is shown in FIG. 27, and the operation of each means other than torque limit value calculating means 86 is the same as that in Embodiment 40, and will not be described.
[ 0105 ]
Next, the operation will be described.
The operation of the torque limit value calculating means 86 before receiving the collision detection signal is exactly the same as the operation of the torque limit value calculating means 32 of FIG. 25, but the collision detection from the first collision determining means 83 or the second collision determining means 85 is performed. When the signal and the collision detection axis number are received, only the axis having the same number as that of the collision detection axis number is obtained by adding the upper torque margin value to the gravity command torque element transmitted from the necessary drive torque command element calculation means 2 as the upper torque The lower limit torque value is obtained by adding the lower torque margin value to the gravity command torque element. However, since the torque limit value changes on the step when suddenly switching, switching is performed smoothly. Specifically, for the axis having the same number as the collision detection axis number, after receiving the collision detection signal, the torque limit value based on the sum of the torque command elements transmitted from the necessary drive torque command element calculation means 2 and the gravity The weighted sum of the torque limit values based on the command is used as the torque limit value to be transmitted to the motor current limit means 35, and the weight of the torque limit value based on the sum of the torque command elements is set to 1 while the total value of the weights of both is kept at 1. From 0 to 1 and the weight of the torque limit value based on the gravity command is increased smoothly from 0 to 1.
[ 0106 ]
Embodiment 42. FIG.
FIG. 27 is a block diagram showing a robot control apparatus according to Embodiment 42 of the present invention, and the operation of each means other than torque limit value calculating means 86 is the same as that in Embodiment 40, and thus description thereof is omitted.
[ 0107 ]
Next, the operation will be described.
A torque margin value magnification β after collision detection is stored in advance in the torque limit value calculation means 86. Until the collision detection signal is received from the first collision determination unit 83 or the second collision determination unit 85, the upper torque limit value and the lower torque limit value are calculated in the same manner as the torque limit value calculation unit 32 of FIG. After receiving the collision detection signal from the first collision determination unit 83 or the second collision determination unit 85, the upper torque margin value and the lower torque margin value are multiplied by the torque margin value magnification β, and then the necessary drive torque command element calculation is performed. A torque limit value that is added to the sum of the torque command elements transmitted from the means 2 and transmitted to the motor current limiting means 35 is calculated. However, since the torque limit value changes stepwise when switching suddenly, the values multiplied by the upper torque margin value and the lower torque margin value are not suddenly changed to β, but are smoothly changed to β.
[ 0108 ]
Embodiment 43. FIG.
FIG. 28 is a block diagram showing a robot control apparatus according to Embodiment 43 of the present invention. In the figure, 89 is a threshold value calculation means, 90 is a collision virtual discrimination means, 91 is a discrimination data storage means, and 92 is a motor control. Means.
Since other configurations are the same as those in the forty-second embodiment, description thereof is omitted.
[ 0109 ]
Next, the operation will be described.
In the forty-third embodiment, there are provided two modes, a normal mode and a trial operation mode, and a parameter based on which a threshold value used for collision determination is calculated from the operation result of the trial operation mode. FIG. 28 is a block diagram showing the configuration of the trial operation mode of the robot control system in the forty-third embodiment, and FIG. 25 is a block diagram showing the configuration of the normal mode of the robot control system in the forty-third embodiment.
In the threshold value calculation means 89, a plurality of parameter value candidates used for threshold value calculation are set. The parameter value candidate setting is performed from a manual operation panel (not shown) or a program describing the robot operation. The threshold value calculation unit 89 transmits the set parameter and the threshold value calculated based on the parameter to the collision virtual determination unit 90.
The virtual collision determination unit 90 first calculates the actual motor driving torque of each axis from the motor current input from the motor control unit 92. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. Next, the torque difference of each axis calculated for each of the plurality of threshold values transmitted from the threshold value calculation means 89 is compared with which of the threshold values of each axis is greater. The number of consecutive excesses, which is the number of times that the torque difference has continuously exceeded the threshold value for each axis, is calculated. The calculated number of consecutive excesses, the parameter value used for calculating the threshold value, and the axis number are transmitted to the discrimination data storage unit 91.
In the discrimination data storage unit 91, the number of consecutive excesses is stored for each parameter value and axis number. When a value larger than the stored continuous excess number is transmitted as the continuous excessive number, the stored value is rewritten. The parameter value, the axis number, and the number of consecutive excesses accumulated in the discrimination data accumulating unit 91 can be viewed from a manual operation panel (not shown) or a personal computer connected to the robot controller. The administrator of the robot controller determines a parameter value to be used in the threshold value calculation unit 89 based on the browsed parameter value, the axis number, and the number of consecutive excesses.
[ 0110 ]
Embodiment 44. .
FIG. Is a block diagram showing a robot control apparatus according to Embodiment 44 of the present invention, in which 93 is a parameter determining means.
Other configurations are the same as those in FIG.
[ 0111 ]
Next, the operation will be described.
Also in the forty-fourth embodiment, there are provided two modes, the normal mode and the trial operation mode, and the parameter from which the threshold value used for collision determination is calculated from the operation result of the trial operation mode. FIG. 29 is a block diagram showing the configuration of the trial operation mode of the robot control system in the forty-fourth embodiment, and FIG. 25 is a block diagram showing the configuration of the normal mode of the robot control system in the forty-fourth embodiment.
When the parameter determination unit 93 receives a parameter determination command from a manual operation panel (not shown), the parameter determination unit 93 receives a parameter value and continuous excess count data from the discrimination data storage unit 91 for each axis that drives the robot. Next, the allowable maximum continuous count stored in advance is compared with the number of consecutive excesses for each parameter value, and the minimum parameter value among the parameter values whose continuous excess count is less than or equal to the allowable maximum continuous count is determined. A value obtained by adding the corrected values is transmitted to the threshold value calculation means 89 as a threshold value calculation parameter.
[ 0112 ]
Embodiment 45. FIG.
30 is a block diagram showing a robot control apparatus according to Embodiment 45 of the present invention. In the figure, 94 is sensitivity setting means.
Other configurations are the same as those in FIG.
[ 0113 ]
Next, the operation will be described.
Also in the forty-fifth embodiment, there are provided two modes, the normal mode and the trial operation mode, and the parameter from which the threshold value used for collision determination is calculated from the operation result of the trial operation mode. FIG. 30 is a block diagram showing the configuration of the trial operation mode of the robot control system in the forty-fifth embodiment, and FIG. 25 is a block diagram showing the configuration of the normal mode of the robot control system in the forty-fifth embodiment.
First, the sensitivity setting unit 94 sets a plurality of sensitivity candidate values from a manual operation panel (not shown), and all the set sensitivity candidate values are transmitted to the threshold value calculation unit 89.
The threshold value calculation means 89 calculates a threshold value every moment based on parameters stored in advance. Next, the calculated threshold value is multiplied by each of the sensitivity candidate values, and all combinations of the multiplication result and the sensitivity candidate value are transmitted to the collision virtual determination means.
The virtual collision determination unit 90 first calculates the actual motor driving torque of each axis from the motor current input from the motor control unit 92. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. Next, the torque difference of each axis calculated for each of the plurality of threshold values transmitted from the threshold value calculation means 89 is compared with which of the threshold values of each axis is greater. The number of consecutive excesses, which is the number of times that the torque difference has continuously exceeded the threshold value for each axis, is calculated. The calculated number of consecutive excesses, the axis number, and the sensitivity candidate value used for calculating the threshold value are transmitted to the discrimination data storage unit 91.
In the discrimination data storage unit 91, the number of consecutive excesses is stored for each axis number and sensitivity candidate value. When a value larger than the stored continuous excess number is transmitted as the continuous excessive number, the stored value is rewritten. The sensitivity candidate value, the axis number, and the number of consecutive excesses stored in the discrimination data storage unit 91 can be viewed from a manual operation panel (not shown) or a personal computer connected to the robot controller. The administrator of the robot control device determines the sensitivity setting value for actually operating the robot based on the browsed sensitivity candidate value, the axis number, and the number of consecutive excesses.
[ 0114 ]
Embodiment 46. FIG.
FIG. 31 is a block diagram showing a robot control apparatus according to Embodiment 46 of the present invention, in which 95 is a display means.
Other configurations are the same as those in FIG.
[ 0115 ]
Next, the operation will be described.
The first collision discriminating means 74 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. The collision determination timer is started when the torque difference in any of the axes becomes greater than or equal to the upper low level threshold or less than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When it is determined that the robot has collided, a stop command is transmitted to the command value generating means 1. Further, a collision detection signal is transmitted to the display means 95.
The second collision discriminating means 75 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 75 transmits a stop command to the command value generation unit 1. Further, a collision detection signal is transmitted to the display means 95.
When receiving the collision detection signal, the display means 95 automatically switches the screen of the manual operation panel to the collision detection function switching dedicated screen. The collision detection function switching screen describes that if 1 is input, the screen returns to the original screen while the collision detection function is enabled, and if 0 is input, the collision detection function is disabled and the screen returns to the original screen. The robot operator can immediately invalidate the collision detection function by operating the manual operation panel according to the screen display, and can quickly retreat the robot in the pushed state due to the collision.
[ 0116 ]
Embodiment 47. FIG.
FIG. 32 is a block diagram showing a robot control apparatus according to Embodiment 47 of the present invention. In FIG. 32, reference numeral 96 denotes a collision state determination means.
Other configurations are the same as those in FIG.
[ 0117 ]
Next, the operation will be described.
The first collision discriminating means 74 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is not more than the upper low level threshold and not less than the lower low level threshold for all the axes continuously for a specified time, a collision state escape signal is transmitted to the collision state determination means 96. On the other hand, the collision determination timer is started when the torque difference in any of the axes becomes equal to or higher than the upper low level threshold or lower than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When the collision of the robot is determined, a stop command and a collision detection signal are transmitted to the collision state determination means 96.
The second collision discriminating means 75 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 75 transmits a stop command and a collision detection signal to the collision state determination unit 96.
The collision state determination unit 96 includes a collision state determination flag. The collision determination flag is 0 in the normal operation state. When the value of the collision determination flag is 0, it means that it is determined that there is no collision state. If the stop command and the collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75 when the value of the collision determination flag is 0, the stop command is output to the command value generation unit 1 as it is and the collision determination flag Change the value to 1. When the value of the collision determination flag is 1, it means that it is determined that the vehicle is in a collision state. Even if a stop command and a collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75 when the value of the collision determination flag is 1, the stop command is not transmitted to the command value generation unit 1. When a collision state escape signal is received from the first collision determination means 74, the value of the collision determination flag is changed to zero.
[ 0118 ]
Embodiment 48. FIG.
A block diagram showing a robot control apparatus according to Embodiment 48 of the present invention is shown in FIG. 31, and the operation of each means other than the command value generating means 1 is the same as that in Embodiment 46, and thus the description thereof is omitted.
[ 0119 ]
Next, the operation will be described.
The first collision discriminating means 74 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. The collision determination timer is started when the torque difference in any of the axes becomes greater than or equal to the upper low level threshold or less than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When it is determined that the robot has collided, a stop command is transmitted to the command value generating means 1. Further, a collision detection signal is transmitted to the command value generating means 1 and the display means 95.
The second collision discriminating means 75 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 75 transmits a stop command to the command value generation unit 1. Further, a collision detection signal is transmitted to the command value generating means 1 and the display means 95.
When the command value generating means 1 receives a stop signal from the first collision discriminating means 74 or the second collision discriminating means 75, it generates a deceleration stop command curve immediately. When a collision detection signal is received from the first collision determination unit 74 or the second collision determination unit 75, a position command before a predetermined time is read from a ring buffer (storage unit) provided in the command value generation unit 1, The read position is stored as a collision retreat operation target point. When a collision evacuation command is input by pressing a specified button on the manual operation panel, a linear operation command with the current position after the deceleration stop as the start point and the collision evacuation operation target point as the end point is specified in advance. Generate at low speed.
When receiving the collision detection signal, the display means 95 automatically switches the screen of the manual operation panel to a screen dedicated for collision evacuation operation. The screen dedicated to collision evacuation operation indicates the number of a button that must be pressed to command the collision evacuation operation.
[ 0120 ]
Embodiment 49. FIG.
A block diagram showing the configuration of the robot control system in the forty-ninth embodiment is also shown in FIG. 31, and the elements other than the command value generating means 1 are the same as the respective elements in the forty-eighth embodiment, and the description thereof will be omitted.
[ 0121 ]
Next, the operation will be described.
When the command value generating means 1 receives a stop signal from the first collision discriminating means 74 or the second collision discriminating means 75, it generates a deceleration stop command curve immediately. When a collision detection signal is received from the first collision determination unit 74 or the second collision determination unit 75, when one operation is performed when the collision detection signal is received, the start point of the operation is stored as a collision retreat operation target point. When there are two or more operations executed when the collision detection signal is received, a point where the latest operation is actually started is calculated, and the calculated point is stored as a collision evacuation operation target point. When a collision evacuation command is input by pressing a specified button on the manual operation panel, if one operation is being performed at the time of receiving a collision detection signal, it is An operation that starts from the current position and ends at the target point of the collision evacuation operation is generated at a predetermined low speed. If there are two or more actions that are being performed when a collision detection signal is received, a linear motion command with the current position after deceleration stop as the start point and the collision evacuation action target point as the end point is sent at a predetermined low speed. Generate.
[ 0122 ]
Embodiment 50. FIG.
FIG. 33 is a block diagram showing a robot control apparatus according to Embodiment 50 of the present invention. In FIG. 33, reference numeral 98 denotes a collision state determining means, 99 is a command value generating means, and 100 is a display means.
Other configurations are the same as those in FIG.
[ 0123 ]
Next, the operation will be described.
The first collision discriminating means 74 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is not more than the upper low level threshold and not less than the lower low level threshold for all the axes continuously for a specified time, a collision state escape signal is transmitted to the collision state determination means 98. On the other hand, the collision determination timer is started when the torque difference in any of the axes becomes equal to or higher than the upper low level threshold or lower than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When the collision of the robot is determined, a collision detection signal and a stop command are transmitted to the collision state determination means 98. Further, the collision detection axis number which is the number of the axis determined to be a collision and the collision detection direction which is the sign of the differential torque of the axis are transmitted to the collision state determination means 98.
The second collision discriminating means 75 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 75 transmits a collision detection signal and a stop command to the collision state determination unit 98. Further, the collision detection axis number which is the number of the axis determined to be a collision and the collision detection direction which is the sign of the differential torque of the axis are transmitted to the collision state determination means 98.
The collision state determination unit 98 includes a collision state determination flag. The collision determination flag is 0 in the normal operation state. When the value of the collision determination flag is 0, it means that it is determined that there is no collision state. When a stop command and a collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75 when the value of the collision determination flag is 0, the stop command and the collision detection signal are output to the command value generation unit 99 as they are. The value of the collision determination flag is changed to 1. The collision detection signal is also transmitted to the display means 100. Further, the received collision detection axis number and the collision detection direction are transmitted to the command value generation means 99 and the display means 100. When the value of the collision determination flag is 1, it means that it is determined that the vehicle is in a collision state. Even if a stop command and a collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75 when the value of the collision determination flag is 1, the stop command is not transmitted to the command value generation unit 99. When a collision state escape signal is received from the first collision determination means 74, the value of the collision determination flag is changed to zero. When the collision determination flag changes from 1 to 0, a collision state escape signal is transmitted to the command value generation means 99 and the display means 100.
The command value generating means 99 generates a deceleration stop command curve as soon as a stop signal and a collision detection signal are received from the collision state determination means 98. After completion of the deceleration stop command curve generation, the command direction check flag of the same axis as the collision detection axis number is set to 1. If the command direction of the axis for which the command direction check flag is 1 matches the collision detection direction, the generated command value is not output to the outside of the command value generation means 99, and the previous cycle of the command value generation cycle is not output. The value is output to the outside and an operation direction warning signal is transmitted to the display means 100. The command direction check flag is reset to 0 when a collision state escape signal is received.
When the display unit 100 receives the collision detection signal, the collision detection axis number, and the collision detection direction from the collision state determination unit 98, the screen of the manual operation panel is switched to the collision state display screen. On the collision state display screen, the collision detection axis number and the collision detection direction are displayed. When an operation direction warning signal is received from the command value generation means 99, a warning sound is generated and the collision detection axis number and the collision detection direction are displayed on the screen of the manual operation panel while blinking.
[ 0124 ]
Embodiment 51. FIG.
FIG. 34 is a block diagram showing a robot control apparatus according to Embodiment 51 of the present invention. In FIG. 34, reference numeral 101 denotes a collision state determination means.
Other configurations are the same as those in FIG.
[ 0125 ]
Next, the operation will be described.
The first collision discriminating means 74 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is not more than the upper low level threshold value and not less than the lower low level threshold value for all the shafts continuously for a predetermined time or longer, a collision state escape signal is transmitted to the collision state determination means 101. On the other hand, the collision determination timer is started when the torque difference in any of the axes becomes equal to or higher than the upper low level threshold or lower than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When the collision of the robot is determined, a collision detection signal and a stop command are transmitted to the collision state determination unit 101. Further, the torque difference between the axes at the moment when the collision is determined is transmitted to the collision state determination means 101 as a collision action torque.
The second collision discriminating means 75 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 75 transmits a collision detection signal and a stop command to the collision state determination unit 101. Further, the torque difference between the axes at the moment when the collision is determined is transmitted to the collision state determination means 101 as a collision action torque.
The collision state determination unit 101 includes a collision state determination flag. The collision determination flag is 0 in the normal operation state. When the value of the collision determination flag is 0, it means that it is determined that there is no collision state. When a stop command and a collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75 when the value of the collision determination flag is 0, the stop command and the collision detection signal are output to the command value generation unit 99 as they are. The value of the collision determination flag is changed to 1. The collision detection signal is also transmitted to the display means 100. Further, the collision direction in the orthogonal coordinate system is calculated from the received collision action torque and the joint displacement input from the motor control means 33 according to the following procedure.
First, the collision action torque vector composed of the collision action torque of each axis is expressed as τ _c , F is the collision force vector seen with the robot hand _c Then,
F _c = (Jv ^T ) ^-1 τ _c ... (32)
Call it. Where (Jv ^T ) ^-1 Is a matrix that is a function of joint displacement called a Jacobian matrix. Each axis velocity vector composed of the velocity of each axis is v, and the hand velocity vector composed of the hand velocity is v _x Then,
v _x = J _v v (33)
There is a relationship. Collision force vector F based on equation (32) _c To calculate F _c If the absolute value of the X, Y, and Z components is greater than the specified value, the direction of the component is set as the collision direction in the orthogonal coordinate system. For example, when the X component is greater than or equal to a specified value in the + direction, the collision direction is set to the X axis + direction. The calculated collision direction is transmitted to the command value generation means 99 and the display means 100. When the value of the collision determination flag is 1, it means that it is determined that the vehicle is in a collision state. Even if a stop command and a collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75 when the value of the collision determination flag is 1, the stop command is not transmitted to the command value generation unit 99. When a collision state escape signal is received from the first collision determination means 74, the value of the collision determination flag is changed to zero. When the collision determination flag changes from 1 to 0, a collision state escape signal is transmitted to the command value generation means 99 and the display means 100.
The command value generation unit 99 generates a deceleration stop command curve immediately after receiving the stop signal and the collision detection signal from the collision state determination unit 101. After the generation of the deceleration stop command curve is completed, the command direction check flag for the collision direction is set to 1. When the command direction of the orthogonal coordinate component for which the command direction check flag is 1 is the same as the collision direction, the generated command value is not output to the outside of the command value generation means 99, and the command value generation is performed. The value of the previous cycle of the cycle is output to the outside and an operation direction warning signal is transmitted to the display means 100. The command direction check flag is reset to 0 when a collision state escape signal is received.
When receiving the collision detection signal and the collision direction from the collision state determination unit 101, the display unit 100 switches the screen of the manual operation panel to the collision state display screen. The collision direction is displayed on the collision state display screen. When an operation direction warning signal is received from the command value generation means 99, a warning sound is generated and displayed on the screen of the manual operation panel while blinking the collision direction.
[ 0126 ]
Embodiment 52. FIG.
FIG. 35 is a block diagram showing a robot control apparatus according to Embodiment 52 of the present invention. In FIG. 35, reference numeral 102 denotes mode discrimination means.
Other configurations are the same as those in FIG.
[ 0127 ]
Next, the operation will be described.
The first collision discriminating means 74 first calculates the actual motor driving torque of each axis from the motor current input from the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is not more than the upper low level threshold and not less than the lower low level threshold for all the axes continuously for a specified time, a collision state exit signal is transmitted to the mode discrimination means 102. On the other hand, the collision determination timer is started when the torque difference in any of the axes becomes equal to or higher than the upper low level threshold or lower than the lower low level threshold. When the torque difference of the shaft becomes lower than the upper low level threshold and higher than the lower low level threshold, the timer is stopped and the internal time of the timer is reset to zero. When the internal time of the timer exceeds the collision determination time set by the threshold value calculation means 28, it is determined that the robot has collided. When the collision of the robot is determined, a stop command and a collision detection signal are transmitted to the mode determination means 102.
The second collision discriminating means 75 first calculates the actual motor driving torque of each axis from the motor current value held by the motor control means 33. Next, a torque difference that is a difference between the calculated motor drive torque and the required drive torque calculated by the required drive torque calculation means 3 is calculated. When the torque difference is higher than the upper high level threshold value or lower than the lower high level threshold value in any of the axes, it is determined that the robot has collided. When the collision is determined, the second collision determination unit 75 transmits a stop command and a collision detection signal to the mode determination unit 102.
The mode determination means 102 has a mode flag. The value of the mode flag is 1 if the current teaching mode is selected, and the mode flag is 0 if the current mode is the automatic mode. When the mode flag is 1, even if a stop command and a collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75, the stop command is not output to the command value generation unit 97. A collision detection signal is output to the display means 100 from when the collision detection signal is received until the collision state escape signal is received. When the mode flag is 0, when the stop command and the collision detection signal are received from the first collision determination unit 74 or the second collision determination unit 75, the stop command and the collision detection signal are output to the command value generation unit 97. In addition, a collision detection signal is output to the display unit 100 from when the collision detection signal is received until the collision state escape signal is received.
The display unit 100 generates a warning sound while receiving the collision detection signal from the mode determination unit 102.
[ 0128 ]
Embodiment 53. FIG.
FIG. 25 is a block diagram showing the configuration of the robot control system in the embodiment 53, and since each element other than the necessary drive torque calculating means 3 is the same as each element in the embodiment 37, the description thereof is omitted.
[ 0129 ]
Next, the operation will be described.
In the required drive torque calculation means 3, the required drive torque τ _e The
τ _e = Τ _me + Τ _he + Τ _gd + Τ _fe ... (34)
Calculate with The last τ is different from equation (10) _fe Only τ _fe The calculation of will be described. First, τ _fe Is the friction force vector,
τ _fe = Τ _fb + Τ _fc ... (35)
Is calculated by The viscous friction coefficient of each axis is f _bi , Coulomb friction coefficient f _ci , The current speed of each axis v _i Then,
τ _fbi = F _bi * V _i ... (36)
τ _fc0i = F _ci (V _i > 0) (37)
τ _fc0i = 0 (v _i = 0) (38)
τ _fc0i = -F _ci (V _i <When 0) (39)
Calculated by τ _fc0i The result of passing through the first-order lag filter is τ _fci And τ of each axis _fci Τ _fc And τ of each axis _fbi Τ _fb And
[ 0130 ]
Embodiment 54. FIG.
FIG. 36 is a block diagram showing a robot control apparatus according to Embodiment 54 of the present invention. In FIG. 36, reference numeral 103 denotes torque command absolute value maximum value calculation means.
Other configurations are the same as those in FIG.
[ 0131 ]
Next, the operation will be described.
First, the torque command absolute value maximum value calculation means 103 first receives the inertial force command element vector τ output from the necessary drive torque command element calculation means 2. _md , Centrifugal Coriolis force command element vector τ _hd Is entered. Next, inertial force command element vector τ _md , Centrifugal Coriolis force command element vector τ _hd Each axis element is added, the absolute value of the addition result is obtained, and the obtained absolute value is converted into the scale of the motor current command of each axis in consideration of the reduction ratio and torque constant of each axis. Furthermore, normalization is performed based on the rated current of each axis motor. T is the maximum of the absolute values of the sum of the inertia force command and the centrifugal Coriolis force command normalized by the rated current of each axis motor calculated for each axis. _max Output as.
The threshold calculation means 28 includes a collision determination time, an upper threshold base portion, a lower threshold base portion, an upper threshold torque command magnification, a lower threshold torque command magnification, and an upper low level threshold. The value clamp value, the lower low level threshold value clamp value, and the high level addition value are stored for each axis, and output from the torque command absolute value maximum value calculation means 103. _max The upper threshold reference value for each axis is the upper threshold base amount + upper threshold torque command magnification × T _max Lower threshold reference value is lower threshold base amount + lower threshold torque command magnification x T _max Is calculated by Next, if the upper threshold reference value is less than or equal to the upper low level threshold clamp value, the upper threshold reference value is set as the upper low level threshold, and the upper threshold reference value becomes the upper lower level. If it is greater than or equal to the threshold clamp value, the upper low level threshold clamp value is set as the upper low level threshold. If the lower threshold reference value is greater than or equal to the lower low level threshold clamp value, the lower threshold reference value is set as the lower low level threshold and the lower threshold reference value is lower. If the lower low level threshold clamp value is less than or equal to, the lower low level threshold clamp value is set as the lower low level threshold. The upper high level threshold value is set by the upper low level threshold value + the high level addition value, and the lower high level threshold value is set by the lower low level threshold-the high level addition value. The set upper low level threshold, lower low level threshold, and collision determination time are output to the first collision determination means 74, and the upper high level threshold and lower high level threshold are the second collision. It is output to the discrimination means 75.
[ 0132 ]
Embodiment 55. FIG.
FIG. 37 is a block diagram showing a robot controller according to Embodiment 55 of the present invention. In FIG. 37, reference numeral 104 denotes acceleration / deceleration direction weight determination means, 105 denotes torque command absolute value maximum value calculation means, and 106 denotes threshold calculation. Means.
Other configurations are the same as those in FIG.
[ 0133 ]
Next, the operation will be described.
First, the acceleration / deceleration direction weight determination means 104 first receives the inertial force command element vector τ output from the necessary drive torque command element calculation means 2. _md Is entered. Then τ _md The sign of each axis is obtained, and the acceleration / deceleration direction weight is determined according to the obtained sign. Specifically, τ _md The weight in the same direction as the sign of each axis is k1, and the weight in the reverse direction is k2. For example, the second axis τ _md If the sign of + is +, the value of the weight uk applied to the upper threshold value of the second axis is k1, and the value of the weight lk applied to the lower threshold value is k2. The acceleration / deceleration direction weight for each axis is output to the threshold value calculation means 106.
The threshold calculation means 106 includes a collision determination time, an upper threshold base portion, a lower threshold base portion, an upper threshold torque command magnification, a lower threshold torque command magnification, and an upper low level threshold. The value clamp value, the lower low level threshold value clamp value, and the high level addition value are stored for each axis, and output from the torque command absolute value maximum value calculation means 105. _max The upper threshold reference value for each axis is the upper threshold base amount + upper threshold torque command magnification × T _max Lower threshold reference value is lower threshold base amount + lower threshold torque command magnification x T _max Is calculated by Next, if the upper threshold reference value is less than or equal to the upper low level threshold clamp value, the upper threshold reference value is set as the upper low level threshold, and the upper threshold reference value becomes the upper lower level. If it is greater than or equal to the threshold clamp value, the upper low level threshold clamp value is set as the upper low level threshold. If the lower threshold reference value is greater than or equal to the lower low level threshold clamp value, the lower threshold reference value is set as the lower low level threshold and the lower threshold reference value is lower. If the lower low level threshold clamp value is less than or equal to, the lower low level threshold clamp value is set as the lower low level threshold. The upper high level threshold value is set by the upper low level threshold value + the high level addition value, and the lower high level threshold value is set by the lower low level threshold-the high level addition value. Furthermore, the acceleration / deceleration direction weight uk input from the acceleration / deceleration direction weight determination means 104 is multiplied to each of the set upper low level threshold value and upper upper level threshold value. Further, the acceleration / deceleration direction weight lk input from the acceleration / deceleration direction weight determining means 104 is multiplied to each of the lower low level threshold and the lower high level threshold. The upper low level threshold multiplied by the acceleration / deceleration direction weight, the lower low level threshold, and the collision determination time are output to the first collision determination means 74, and the upper high level threshold multiplied by the acceleration / deceleration direction weight. The value and the lower high level threshold value are output to the second collision determination means 75.
[ 0134 ]
Embodiment 56. FIG.
FIG. 38 is a block diagram showing a robot control apparatus according to Embodiment 56 of the present invention. In the figure, reference numeral 107 denotes a threshold value calculation means.
Other configurations are the same as those in FIG.
[ 0135 ]
Next, the operation will be described.
The threshold calculation means 107 includes a collision determination time, an upper threshold base portion, a lower threshold base portion, an upper threshold torque command magnification, a lower threshold torque command magnification, and an upper low level threshold. The value clamp value, the lower low level threshold value clamp value, and the high level addition value are stored for each axis, and output from the torque command absolute value maximum value calculation means 103. _max The upper threshold reference value for each axis is the upper threshold base amount + upper threshold torque command magnification × T _max Lower threshold reference value is lower threshold base amount + lower threshold torque command magnification x T _max Is calculated by Next, if the upper threshold reference value is less than or equal to the upper low level threshold clamp value, the upper threshold reference value is set as the upper low level threshold, and the upper threshold reference value becomes the upper lower level. If it is greater than or equal to the threshold clamp value, the upper low level threshold clamp value is set as the upper low level threshold. If the lower threshold reference value is greater than or equal to the lower low level threshold clamp value, the lower threshold reference value is set as the lower low level threshold and the lower threshold reference value is lower. If the lower low level threshold clamp value is less than or equal to, the lower low level threshold clamp value is set as the lower low level threshold. The upper high level threshold value is set by the upper low level threshold value + the high level addition value, and the lower high level threshold value is set by the lower low level threshold-the high level addition value.
The motor speed of each axis is also input from the motor control means 33 to the threshold calculation means 107. The threshold calculation means 107 calculates the motor acceleration of each axis from the current value of the motor speed of each axis and the previous motor speed of each axis. In the threshold value calculation means 107, a table of absolute values of motor acceleration and collision determination time is created in advance, and the collision determination time of each axis is set by referring to this table.
The set upper low level threshold, lower low level threshold, and collision determination time are output to the first collision determination means 74, and the upper high level threshold and lower high level threshold are the second collision. It is output to the discrimination means 75.
[ 0136 ]
Embodiment 57. FIG.
FIG. 39 is a block diagram showing a robot control apparatus according to Embodiment 57 of the present invention. In FIG. 39, 108 is a threshold value calculation means.
Other configurations are the same as those in FIG.
[ 0137 ]
Next, the operation will be described.
The threshold calculation means 108 includes a collision determination time, an upper threshold base portion, a lower threshold base portion, an upper threshold torque command magnification, a lower threshold torque command magnification, and an upper low level threshold. The value clamp value, the lower low level threshold value clamp value, and the high level addition value are stored for each axis, and output from the torque command absolute value maximum value calculation means 103. _max The upper threshold reference value for each axis is the upper threshold base amount + upper threshold torque command magnification × T _max Lower threshold reference value is lower threshold base amount + lower threshold torque command magnification x T _max Is calculated by Next, if the upper threshold reference value is less than or equal to the upper low level threshold clamp value, the upper threshold reference value is set as the upper low level threshold, and the upper threshold reference value becomes the upper lower level. If it is greater than or equal to the threshold clamp value, the upper low level threshold clamp value is set as the upper low level threshold. If the lower threshold reference value is greater than or equal to the lower low level threshold clamp value, the lower threshold reference value is set as the lower low level threshold and the lower threshold reference value is lower. If the lower low level threshold clamp value is less than or equal to, the lower low level threshold clamp value is set as the lower low level threshold. The upper high level threshold value is set by the upper low level threshold value + the high level addition value, and the lower high level threshold value is set by the lower low level threshold-the high level addition value.
The threshold value calculation means 108 includes an inertial force command element vector τ output from the necessary drive torque command element calculation means 2. _md Is also entered. Then τ _md The absolute value of each axis is calculated. The threshold value calculation means 108 includes τ _md A table of absolute values of each axis and collision determination time is prepared in advance, and the collision determination time of each axis is set by referring to this table.
The set upper low level threshold, lower low level threshold, and collision determination time are output to the first collision determination means 74, and the upper high level threshold and lower high level threshold are the second collision. It is output to the discrimination means 75.
[ 0138 ]
【The invention's effect】
As described above, according to the present invention, the required drive torque used for collision determination is calculated from the required drive torque command element and the position, speed, and acceleration of the motor that drives the robot. When both the feedforward control and the method of detecting a collision without using an additional sensor are made compatible, there is an effect that the amount of calculation can be reduced and the collision can be discriminated with high accuracy.
[ 0139 ]
According to this invention, since the required drive torque used for discrimination of the collision is calculated from the position, speed, and acceleration of the motor approximately calculated from the required drive torque command element and the position command, speed command, and acceleration command, the trajectory accuracy When both the feedforward control for improving the system and the method of detecting the collision without using an additional sensor are made compatible, there is an effect that the calculation amount can be reduced and the collision can be discriminated with high accuracy.
[ 0140 ]
According to this invention, in order to calculate the necessary drive torque used for collision determination from the necessary drive torque command element, the feedforward control for improving the trajectory accuracy and the method for detecting the collision without using an additional sensor When both are satisfied, the calculation amount can be reduced and the collision can be discriminated with high accuracy.
[ 0141 ]
According to the present invention, parameters that change in accordance with operating conditions such as a friction coefficient are identified appropriately and appropriately during the operation of the robot.
[ 0142 ]
According to the present invention, when calculating the necessary driving torque used for the collision determination, the identification value before the collision is used instead of the current value of the parameter estimation value such as the friction coefficient identified during the operation. In order to calculate the required drive torque used for collision discrimination using the identified parameter value, parameter estimation values such as friction coefficient reduce the deviation between the required drive torque calculated value due to the impact and the motor current. There is an effect of preventing the collision discrimination accuracy from being lowered.
[ 0143 ]
According to the present invention, parameter identification such as the coefficient of friction is performed only from data that can be expected to have high identification accuracy. Therefore, parameter identification can be performed with high accuracy, and the required drive torque used for collision determination can be calculated with high accuracy. There is an effect that can be done.
[ 0144 ]
According to the present invention, since the identification accuracy is low, there is an effect of preventing the collision discrimination accuracy from being lowered.
[ 0145 ]
According to the present invention, parameter identification, which may have a relatively long calculation cycle, is performed with a long calculation cycle, so that the amount of calculation per unit time in the control device can be reduced without degrading the collision discrimination accuracy.
[ 0146 ]
According to the present invention, there is an effect that it is possible to prevent the calculation accuracy of the required driving torque from being lowered due to the parameter value to be identified deviating from the true value due to the robot colliding with the surrounding object.
[ 0147 ]
According to the present invention, there is an effect that the collision can be discriminated with high accuracy both in the case of collision with a soft object and the case of collision with a hard countermeasure object.
[ 0148 ]
According to this invention, since the torque generated by each axis driving motor of the robot when stopping after collision detection can be limited with high accuracy, it is possible to prevent an excessive force from acting on the robot and the colliding object. is there.
[ 0149 ]
According to the present invention, there is an effect of preventing a collision from being erroneously detected when the motor power is turned on.
[ 0150 ]
According to the present invention, it is possible to set the optimum sensitivity according to the hand load, and therefore, there is an effect of preventing the collision determination accuracy from being lowered more than necessary when the required load torque estimation accuracy is high.
[ 0151 ]
According to the present invention, it is possible to detect a collision with the sensitivity desired by the user of the robot.
[ 0152 ]
According to the present invention, there is an effect that it is possible to increase the sensitivity of collision detection in a teaching operation that has a high risk of causing a collision without affecting the sensitivity during automatic driving.
[ 0153 ]
According to the present invention, the parameter identification accuracy is improved while preventing the collision from being erroneously detected when the calculation accuracy of the required drive torque is reduced because the parameter identification value is greatly deviated from the true value. When it is high, there is an effect that the collision can be detected with high accuracy.
[ 0154 ]
According to the present invention, since the sensitivity can be set finely according to the operation state of the robot, there is an effect that the collision can always be detected with high accuracy.
[ 0155 ]
According to the present invention, the threshold value can be increased only at the time of stopping when the estimation accuracy of the torque is lowered due to the influence of static friction, so that it is possible to prevent a collision from being erroneously detected on the stopped shaft. During operation, there is an effect that a collision can be detected with high accuracy.
[ 0156 ]
According to the present invention, the threshold value can be automatically increased only when the robot performs the work with the disturbance force. Therefore, when the robot performs the work with the disturbance force, the collision is erroneously detected. There is an effect that can be prevented.
[ 0157 ]
According to the present invention, there is an effect of preventing the torque command given to the motor from changing abruptly due to a sudden change in the torque limit value, and the behavior of the robot from becoming vibrational.
[ 0158 ]
According to the present invention, there is an effect of preventing the torque limit value from changing vibrationally and preventing the robot from moving in vibration.
[ 0159 ]
According to the present invention, the required drive torque command element and the required drive torque change stepwise even when the mass of the hand load changes stepwise as in the case of gripping the workpiece with the hand, and the behavior of the robot vibrates. There is an effect to prevent becoming.
[ 0160 ]
According to the present invention, when the robot stops in a state where the robot is pressing the object by collision detection, there is an effect that the robot can be retracted by manual operation without removing the object.
[ 0161 ]
According to the present invention, it is possible to prevent the collision from being erroneously detected due to the influence of the observation noise applied to the current or the acceleration signal.
[ 0162 ]
According to the present invention, there is an effect that the robot can be retracted so that the collision does not occur again during the retreat after the collision detection.
[ 0163 ]
According to the present invention, since the robot is urgently stopped when excessive reversal is detected by the excessive reversal determination means, it is possible to prevent the robot from reversing excessively after the collision.
[ 0164 ]
According to the present invention, since the robot is urgently stopped when excessive reversal is detected by the excessive reversal determination means, it is possible to prevent the robot from reversing excessively after the collision.
[ 0165 ]
According to the present invention, the ability to reduce the pressing force at the time of collision obtained by generating the torque limit value of each axis based on the equation of motion during stop after collision determination and the deceleration of the deceleration stop command are separated. In this case, it is possible to significantly reduce the pressing force at the time of collision while preventing vibration at the time of deceleration stop.
[ 0166 ]
According to the present invention, it is possible to reduce the pressing force after the collision while preventing the robot from moving in an unexpected direction at the time of the collision.
[ 0167 ]
According to the present invention, it is possible to reduce the pressing force after the collision while preventing the robot from moving in an unexpected direction at the time of the collision.
[ 0168 ]
According to the present invention, the pressing force is reduced by reducing the threshold value during the period until the collision determination, and when stopping by the collision determination, the threshold value is increased to slightly reduce the pressing force reduction effect. The robot should not deviate greatly from the command while leaving. Conversely, by further reducing the threshold value after the collision determination, there is an effect that the pressing force when stopping by the collision determination can be further reduced.
[ 0169 ]
According to the present invention, parameters are set according to actual operation data (results of operation), so that there is an effect that appropriate parameters can be set easily and in a short time.
[ 0170 ]
According to the present invention, since sensitivity is set according to actual operation data (result of operation), appropriate sensitivity (which realizes as high a sensitivity as possible and does not cause false detection) can be easily and short. There is an effect that can be set to time.
[ 0171 ]
According to the present invention, when the robot is stopped in the pressing state, if the avoidance operation is to be performed, the robot is stopped again by collision detection. Therefore, the robot may not be moved unless the collision detection function is released. In such a case, it is possible to quickly cancel the collision determination function and to avoid the robot by manual operation from the pressed state.
[ 0172 ]
According to the present invention, when stopping in a pressed state, a phenomenon in which the robot cannot be avoided to detect a collision again is automatically prevented. Further, since the collision detection function is automatically re-enabled when the pressing state is escaped, there is an effect of reducing damage in the case of collision with another place again.
[ 0173 ]
According to the present invention, there is an effect that the robot can be easily returned to the point before the collision.
[ 0174 ]
According to the present invention, when the robot is stopped due to a collision, the robot is prevented from operating even if it is mistakenly attempted to perform the avoiding operation and even if a command for further pressing is given, and further pressing is prevented. Further, by issuing a warning, there is an effect that the operator can easily notice that the direction of performing the avoidance operation is wrong.
[ 0175 ]
According to the present invention, in the case of light contact at the time of teaching, a warning is issued and the operator recognizes that contact is being made, but the teaching operation continues as it is because the robot does not stop or power is shut off. There is an effect that teaching near the contact point can be performed in a short time.
[ 0176 ]
According to this invention, since the Coulomb friction model becomes more accurate, there is an effect that the estimation accuracy of the torque calculated based on the equation of motion is increased.
[ 0177 ]
According to the present invention, it is possible to prevent the threshold value from becoming excessive and the collision detection sensitivity from being lowered while setting the threshold value based on a simple function.
[ 0178 ]
According to the present invention, there is an effect that the sensitivity of collision detection can be increased while preventing erroneous detection by attaching a weight according to the acceleration / deceleration direction to the threshold value.
[ 0179 ]
According to the present invention, not only the threshold value itself, but also the continuous value of the number of times that the torque estimation error is equal to or greater than the threshold value is made variable according to the operating conditions, so that the sensitivity of collision detection is prevented while preventing erroneous detection. There is an effect that can raise.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a robot control apparatus according to Embodiment 1 of the present invention.
FIG. 2 is a block diagram showing a robot control apparatus according to Embodiment 3 of the present invention.
FIG. 3 is a block diagram showing a robot control apparatus according to Embodiment 4 of the present invention.
FIG. 4 is a block diagram showing a robot control apparatus according to Embodiment 6 of the present invention.
FIG. 5 is a block diagram showing a robot control apparatus according to a seventh embodiment of the present invention.
FIG. 6 is a block diagram showing a robot control apparatus according to an eighth embodiment of the present invention.
FIG. 7 is a block diagram showing a robot control apparatus according to Embodiment 9 of the present invention.
FIG. 8 is a block diagram showing a robot control apparatus according to Embodiment 10 of the present invention.
FIG. 9 is a block diagram showing a robot control apparatus according to Embodiment 12 of the present invention.
FIG. 10 is a block diagram showing a robot control apparatus according to Embodiment 14 of the present invention.
FIG. 11 is a block diagram showing a robot control apparatus according to Embodiment 15 of the present invention.
FIG. 12 is a block diagram showing a robot control apparatus according to a twenty-first embodiment of the present invention.
FIG. 13 is a block diagram showing a robot control apparatus according to a twenty-second embodiment of the present invention.
FIG. 14 is a block diagram showing a robot control apparatus according to a twenty-third embodiment of the present invention.
FIG. 15 is a block diagram showing a robot control apparatus according to Embodiment 24 of the present invention.
FIG. 16 is a block diagram showing a robot control apparatus according to a twenty-fifth embodiment of the present invention.
FIG. 17 is a block diagram showing a robot control apparatus according to a twenty-sixth embodiment of the present invention.
FIG. 18 is a block diagram showing a robot control apparatus according to a twenty-seventh embodiment of the present invention.
FIG. 19 is a block diagram showing a robot control apparatus according to an embodiment 28 of the present invention.
FIG. 20 is a block diagram showing a robot control apparatus according to an embodiment 29 of the present invention.
FIG. 21 is a block diagram showing a robot control apparatus according to a thirtieth embodiment of the present invention.
FIG. 22 is a block diagram showing a robot control apparatus according to Embodiment 31 of the present invention.
FIG. 23 is a block diagram showing a robot control apparatus according to a thirty-second embodiment of the present invention.
FIG. 24 is a block diagram showing a robot control apparatus according to a thirty-third embodiment of the present invention.
FIG. 25 is a block diagram showing a robot control apparatus according to Embodiments 37 and 53 of the present invention.
FIG. 26 is a block diagram showing a robot control apparatus according to an embodiment 39 of the present invention.
FIG. 27 is a block diagram showing a robot control apparatus according to a fortieth embodiment of the present invention.
FIG. 28 is a block diagram showing a robot control apparatus according to embodiment 43 of the present invention.
FIG. 29 is a block diagram showing a robot control apparatus according to embodiment 44 of the present invention.
FIG. 30 is a block diagram showing a robot control apparatus according to embodiment 45 of the present invention.
FIG. 31 is a block diagram showing a robot control apparatus according to embodiment 46 of the present invention.
32 is a block diagram showing a robot control apparatus according to an embodiment 47 of the present invention. FIG.
FIG. 33 is a block diagram showing a robot control apparatus according to embodiment 50 of the present invention.
FIG. 34 is a block diagram showing a robot control apparatus according to embodiment 51 of the present invention.
FIG. 35 is a block diagram showing a robot control apparatus according to embodiment 52 of the present invention.
FIG. 36 is a block diagram showing a robot control apparatus according to embodiment 54 of the present invention.
FIG. 37 is a block diagram showing a robot control apparatus according to embodiment 55 of the present invention.
FIG. 38 is a block diagram showing a robot control apparatus according to an embodiment 56 of the present invention.
FIG. 39 is a block diagram showing a robot control apparatus according to an embodiment 57 of the present invention.
FIG. 40 is a block diagram showing a robot control apparatus according to a sixteenth embodiment of the present invention.
[Explanation of symbols]
1, 18, 99 Command value generation means, 2 Required drive torque command element calculation means, 3, 12, 15 Required drive torque calculation means, 4, 28, 48, 51, 53, 55, 57, 59, 61, 63, 65, 89, 106 to 108 Threshold value calculation means, 5, 9, 27 Collision discrimination means, 6, 10, 17, 33, 80, 84, 92 Motor control means, 8 Addition means, 11 Position velocity acceleration estimation means, 14, 16 Parameter estimation means, 19 Parameter storage means, 20 Identification gain calculation means, 21 Parameter estimated value restriction means, 25 Identification gain calculation means, 29, 74, 79, 83 First collision determination means, 30, 37, 75, 81, 85 Second collision discriminating means, 31 Torque margin calculating means, 32, 86 Torque limit value calculating means, 34 Motor current command generating means, 35 Motor current limiting means (Mode 36, motor current control means, 47, 49 identification progress determination means, 50 torque command absolute value maximum value calculation means, 52 motor current absolute value maximum value calculation means, 54 torque command torque command difference absolute value Maximum value calculating means, 56 Torque command torque command difference absolute value calculating means, 58 Acceleration command jerk command absolute value calculating means, 60 Threshold magnification factor calculating means, 62 Stop determining means, 64 Special work determining means, 66 Program analysis Means, 67, 70 smoothing means, 68 torque limit value holding means, 69 hand load set value reading means, 76 position speed storage means, 77 excessive inversion determination means, 82 second command value generation means, 90 collision virtual determination means, 91 discriminating data storage means, 93 parameter determining means, 94 sensitivity setting means, 95, 100 display means, 96, 98, 1 01 collision state determination means, 102 mode determination means, 103, 105 torque command absolute value maximum value calculation means, 104 acceleration direction weight determination means.

Claims (12)

Required drive torque command element calculating means for calculating each element of the required drive torque command from at least one of position command, speed command, and acceleration command, at least one of position, speed, and acceleration, and the required drive torque Necessary driving torque calculating means for calculating the required driving torque using at least one of the elements of the required driving torque command calculated by the command element calculating means, and the necessity calculated by the necessary driving torque command element calculating means The maximum value of the weighted sum of the absolute value of the difference between the drive torque command element and the required drive torque command element is calculated, and the upper threshold base value stored in advance, the upper threshold torque command torque command difference magnification, the weight From the maximum value of the mitsuki sum (upper threshold base) + (upper threshold torque command torque command differential magnification) x (weighted sum The upper threshold value is calculated by (large value), and the lower threshold base value stored in advance, the lower threshold torque command torque command difference magnification, and the maximum value of the weighted sum (lower threshold value) Base value) + (Lower threshold torque command torque command differential magnification) × (Maximum weighted sum) Threshold value calculation means for calculating the lower threshold value and calculation by the required drive torque calculation means A collision determination means for determining that the robot has collided when the difference between the required drive torque and the actual motor drive torque is greater than or equal to the upper threshold value or less than or equal to the lower threshold value. In addition, at least one of the required drive torque command element calculating means and the required drive torque calculating means, the position, speed, and acceleration of each axis are input, and each axis is driven as the sum of inertial force, centrifugal Coriolis force, gravity, and frictional force. torque Calculating robot control apparatus characterized by performing the calculation of the equation of motion of robot. Necessary driving torque command element calculation means for calculating each element of the required driving torque command from at least one of position command, speed command, and acceleration command, and estimated values of position, speed, and acceleration by simulating the control system of each axis A position / speed / acceleration estimating means for calculating the position / speed / acceleration command from at least one of the position / speed / acceleration estimated by the position / speed / acceleration estimating means and the required drive torque command element calculating means. A required drive torque calculating means for calculating the required drive torque using at least one of the elements of the calculated required drive torque command; a required drive torque command element calculated by the required drive torque command element calculating means; calculates the maximum value of the weighted sum of the difference absolute value of the required driving torque command element, pre-stored upper threshold based , Upper threshold torque command torque command difference magnification, from maximum value of weighted sum (for upper threshold base) + (upper threshold torque command torque command difference magnification) × (maximum value of weighted sum) The upper threshold value is calculated with the lower threshold base value stored in advance, the lower threshold torque command torque command differential magnification, and the maximum value of the weighted sum (lower threshold base value) Threshold value calculation means for calculating the lower threshold value by + (lower threshold torque command torque command difference magnification) × (maximum value of weighted sum), and calculated by the required drive torque calculation means Collision determining means for determining that the robot has collided when the difference between the driving torque and the actual motor driving torque is equal to or greater than the upper threshold value or equal to or less than the lower threshold value; Drive torque command At least one at the position of each axis of the elementary calculation means and the required driving torque calculation means, speed, and an input acceleration, inertial force, centrifugal Coriolis force, gravity, the robot for calculating a driving torque of each axis as the sum of the frictional force A robot controller characterized by calculating an equation of motion. The friction coefficient required when calculating the equation of motion of the robot inside the required drive torque command element calculation means and the required drive torque calculation means is calculated from the sum of the estimated torque other than the friction force, the motor drive current, and the speed of each axis. with an estimation for parameter estimation means, the friction coefficient estimated by the parameter estimation section when performing the calculation of the frictional force of the equation of motion of a robot within the required driving torque command element calculation means and said demanded drive torque calculating means 3. The robot control apparatus according to claim 1, wherein the robot control apparatus is used. When calculating the frictional force of the equation of motion of the robot inside the required drive torque calculation means and the required drive torque command element calculation means, use the value before a certain time instead of the current value of the friction coefficient estimated by the parameter estimation means. 4. The robot control apparatus according to claim 3, wherein the robot control device performs the calculation.   A high level threshold value and a low level threshold value are provided for both the upper threshold value and the lower threshold value, and the required drive torque calculated by the required drive torque calculation means and the current of the motor that drives each axis motor When the difference between the calculated actual drive torques is greater than or equal to the upper high level threshold value or less than the lower high level threshold value, or each axis motor is driven with the required drive torque calculated by the required drive torque calculation means Collision discrimination means was provided to discriminate a collision when the difference in the actual driving torque calculated from the motor current was continuously above the upper low level threshold or below the lower low level threshold for a predetermined time or longer. The robot control device according to any one of claims 1 to 4, wherein Torque margin calculation means for calculating the upper torque margin value and the lower torque margin value by multiplying the upper threshold value and the lower threshold value calculated by the threshold value calculation means in advance, and the necessary drive torque Torque limit value calculating means for calculating a drive torque upper limit value and a drive torque lower limit value based on the required drive torque calculated by the calculation means, the upper torque margin value, and the lower torque margin value; and motor drive torque 6. A motor drive torque limiting means for limiting the motor drive torque so that is always equal to or less than the drive torque upper limit value and equal to or greater than the drive torque lower limit value. The robot control device according to any one of the above. Torque margin calculation means for calculating the upper torque margin value and the lower torque margin value by multiplying the upper threshold value and the lower threshold value calculated by the threshold value calculation means in advance, and the required drive torque Torque limit value calculating means for calculating a drive torque upper limit value and a drive torque lower limit value based on the required drive torque command element calculated by the command element calculating means, the upper torque margin value and the lower torque margin value; The motor drive torque limiting means for limiting the motor drive torque so that the motor drive torque is always equal to or lower than the drive torque upper limit value and equal to or higher than the drive torque lower limit value. The robot control device according to any one of items 5 to 6. Required drive torque command element calculating means for calculating each element of the required drive torque command from at least one of position command, speed command, and acceleration command, at least one of position, speed, and acceleration, and the required drive torque Required drive torque calculating means for calculating the required drive torque using at least one of the elements of the required drive torque command calculated by the command element calculating means, and calculating the maximum value of the measured absolute value of the motor drive current From the upper threshold base value stored in advance, the upper threshold current magnification, and the maximum value of the measured absolute value of the motor drive current (upper threshold base) + (upper threshold current magnification) × ( The upper threshold value is calculated by the maximum absolute value of the measured motor drive current value), and the lower threshold base amount stored in advance, the lower threshold current magnification, the motor drive Calculate the lower threshold value from the absolute value of the absolute value of the current measurement value (for the lower threshold value base) + (lower threshold current magnification) x (maximum absolute value of the measured motor drive current value) When the difference between the threshold value calculation means and the required drive torque calculated by the required drive torque calculation means and the actual motor drive torque is equal to or greater than the upper threshold value, or less than the lower threshold value A collision determination means for determining that the robot has collided, and at least one of the required drive torque command element calculation means and the required drive torque calculation means is input with the position, speed, and acceleration of each axis, A robot control apparatus that calculates a motion equation of a robot that calculates a drive torque of each axis as a sum of centrifugal Coriolis force, gravity, and friction force. Necessary driving torque command element calculation means for calculating each element of the required driving torque command from at least one of position command, speed command, and acceleration command, and estimated values of position, speed, and acceleration by simulating the control system of each axis A position / speed / acceleration estimating means for calculating the position / speed / acceleration command from at least one of the position / speed / acceleration estimated by the position / speed / acceleration estimating means and the required drive torque command element calculating means. Required drive torque calculating means for calculating the required drive torque using at least one of the elements of the calculated required drive torque command, the maximum value of the measured absolute value of the motor drive current is calculated and stored in advance From the upper threshold base, the upper threshold current magnification, and the maximum value of the measured absolute value of the motor drive current (upper threshold base) + (upper Calculating an upper threshold in threshold current magnification) × (maximum value of the motor driving current measurements absolute value), pre-stored lower threshold base From the maximum value of the absolute value of the motor drive current measured value (lower threshold base amount) + (lower threshold current magnification) x (motor drive current measured value) The threshold value calculating means for calculating the lower threshold value with the maximum absolute value) and the difference between the required driving torque calculated by the required driving torque calculating means and the actual motor driving torque is the upper threshold value. In the above case, or a collision determination unit that determines that the robot has collided when the value is equal to or lower than the lower threshold value, at least one of the required drive torque command element calculation unit and the required drive torque calculation unit A robot control device characterized by calculating a robot's equation of motion that calculates the driving torque of each axis as the sum of inertial force, centrifugal Coriolis force, gravity, and frictional force by inputting the position, velocity, and acceleration of each axis. . Required drive torque command element calculating means for calculating each element of the required drive torque command from at least one of position command, speed command, and acceleration command, at least one of position, speed, and acceleration, and the required drive torque Required drive torque calculation means for calculating the required drive torque using at least one of the elements of the required drive torque command calculated by the command element calculation means, and the acceleration command absolute value and the difference absolute value of the acceleration command The maximum value of the weighted sum is calculated, and the upper threshold base amount stored in advance, the upper threshold acceleration command jerk command magnification, and the maximum value of the weighted sum (upper threshold base portion) + ( Upper threshold acceleration command jerk command multiplication factor) x (maximum value of weighted sum) is used to calculate the upper threshold value and to store the lower threshold base. Lower threshold acceleration command jerk command magnification, from the maximum value of the above weighted sum (for the lower threshold base) + (lower threshold acceleration command jerk command magnification) x (maximum of the weighted sum) Value) and a difference between the required driving torque calculated by the required driving torque calculating means and the actual motor driving torque is greater than or equal to the upper threshold value, Or a collision discriminating unit for discriminating that the robot has collided when the value falls below the lower threshold, and the position of each axis in at least one of the required driving torque command element calculating unit and the required driving torque calculating unit. A robot control apparatus that calculates a motion equation of a robot that receives a speed and an acceleration and calculates a driving torque of each axis as a sum of inertial force, centrifugal Coriolis force, gravity, and frictional force. Necessary driving torque command element calculation means for calculating each element of the required driving torque command from at least one of position command, speed command, and acceleration command, and estimated values of position, speed, and acceleration by simulating the control system of each axis A position / speed / acceleration estimating means for calculating the position / speed / acceleration command from at least one of the position / speed / acceleration estimated by the position / speed / acceleration estimating means and the required drive torque command element calculating means. Required drive torque calculating means for calculating the required drive torque using at least one of the calculated elements of the required drive torque command, and the maximum of the weighted sum of the acceleration command absolute value and the difference absolute value of the acceleration command The value is calculated and the upper threshold base value stored in advance, the upper threshold acceleration command jerk command magnification, the maximum value of the weighted sum (Upper threshold base portion) + (Upper threshold acceleration command jerk command magnification) x (Maximum weighted sum) is calculated as the upper threshold value, and the previously stored lower threshold base portion , Lower threshold acceleration command jerk command magnification, from the maximum value of the weighted sum (for lower threshold base) + (lower threshold acceleration command jerk command magnification) x (weighted sum of Threshold value calculating means for calculating a lower threshold value with a maximum value), and the difference between the required driving torque calculated by the required driving torque calculating means and the actual motor driving torque is equal to or greater than the upper threshold value. Or a collision discriminating unit that discriminates that the robot has collided when the value falls below the lower threshold value, and at least one of the required driving torque command element calculating unit and the required driving torque calculating unit Position, speed An input acceleration, inertial force, centrifugal Coriolis force, gravity, the robot control apparatus characterized by performing the calculation of the equation of motion of the robot for calculating a driving torque of each axis as the sum of the frictional force. Instead of using the set values of the hand load as it is as the hand load mass, hand load center of gravity position, and hand load inertia moment used for the calculation of the equation of motion inside the required drive torque command element calculation means and the required drive torque calculation means, the load set value input to the smoothing means, the hand load mass output of the smoothing means, the hand load center of gravity position, movement within the required driving torque command element calculation means and the required driving torque calculating means as the end effector load inertia The robot control apparatus according to claim 1, wherein an equation is calculated.

Images