JP7176255B2 - 電動機制御装置及び衝突検出方法 - Google Patents

Description

本発明は、電動機の駆動対象物が障害物に衝突した場合の衝突検出機能を備えた電動機制御装置及び衝突検出方法に関するものである。

工作機械やロボットアーム等を駆動する電動機の制御装置において、駆動対象物が周囲の障害物に衝突したことを検出する従来技術が存在する。
例えば特許文献１には、図８に示すサーボモータの位置制御装置が記載されている。図８において、位置制御部１０２は、位置検出器１０７による位置検出値Ｐ_ｆが位置指令発生器１０１からの位置指令値Ｐ_Ｃに追従するように速度指令値Ｖ_Ｃを演算し、速度制御部１０３は、速度検出手段１０８からの速度検出値Ｖ_ｆが速度指令値Ｖ_Ｃに追従するように電流指令値Ｉ_Ｃを演算する。

逆転制動制御部１１１には、電流指令値Ｉ_Ｃ及び速度検出値Ｖ_ｆと、制動トルク指令値と、後述する判定部１１０からの衝突判定信号とが入力されている。この逆転制動制御部１１１は、通常運転時には前記電流指令値Ｉ_Ｃを出力し、衝突判定信号の入力時には、制動トルク指令値に従ってモータ１０５を停止させるための電流指令値Ｉ_Ｓを演算して出力する。
電流制御部１０４は電流指令値Ｉ_ＣまたはＩ_Ｓに従ってモータ１０５を制御し、駆動対象物である機械部１０６を駆動または停止させる。

ここで、外乱オブザーバ１０９は、速度検出値Ｖ_ｆ及び電流指令値Ｉ_Ｃに基づいて機械部１０６に作用する外乱トルクを推定し、この推定外乱トルクτ_Ｌ ^＃を判定部１１０に出力する。判定部１１０は、推定外乱トルクτ_Ｌ ^＃の絶対値が、動作モード（位置決め運転モードまたは切削送りモード）判定部１１２からの許容外乱トルクτ_Ｌａを超える場合に、機械部１０６が障害物に衝突したと判定し、逆転制動制御部１１１に対する衝突判定信号と外部への警報とを出力する。
この従来技術と同一の原理に基づき、ロボットアームに異常な負荷が加わった場合にサーボモータを停止させるようにした制御装置が、特許文献２にも記載されている。

図９は、図８に示した位置制御装置の動作を示す波形図である。
図９（ａ）において、外乱オブザーバ１０９が外乱トルクの推定演算に用いる慣性モーメント値Ｊが実際値と一致している場合、時刻ｔ_ｃで機械部１０６が障害物に衝突すると、外乱オブザーバ１０９は推定外乱トルクτ_Ｌ ^＃を適正に演算し、この推定外乱トルクτ_Ｌ ^＃が許容外乱トルクτ_Ｌａを超えたことから機械部１０６の衝突を検出可能である。

しかし、推定演算に用いる慣性モーメント値Ｊが例えば実際値の１０倍であった場合には、図９（ｂ）に示すように、モータの加減速時のトルク成分による演算誤差が推定外乱トルクτ_Ｌ ^＃に現れるため、許容外乱トルクτ_Ｌａを小さくした場合に衝突を誤検出してしまう恐れがある。
これにより、特許文献１の従来技術では、許容外乱トルクτ_Ｌａを小さく設定して衝突検出感度を高めることが困難であった。

これに対し、特許文献３には、推定外乱トルクに含まれる誤差を低減して衝突検出感度を高めるようにした衝突検出装置が開示されている。
図１０は、この衝突検出装置のブロック図である。制御部２０２は、対象物２０１を駆動するサーボモータ２００を制御し、外乱オブザーバ２０３は、サーボモータ２００の速度検出値と制御部２０２のトルク指令値に基づいて対象物２０１に作用する外乱トルクを第１の外乱トルクτ_{ｃａｌｃ１}として推定する。

第１の推定外乱トルクτ_{ｃａｌｃ１}はハイパスフィルタ等からなるフィルタ２０４に入力され、低周波成分が除去された第２の推定外乱トルクτ_{ｃａｌｃ２}として衝突判定部２０５に入力される。衝突判定部２０５では、第２の推定外乱トルクτ_{ｃａｌｃ２}が感度設定値ｌｉｍｉｔを超えたときに対象物２０１の衝突を判定し、警報等の判定出力を行う。

この従来技術は、外乱オブザーバ２０３による推定外乱トルクは、推定演算に用いる慣性モーメント等のパラメータの誤差や重力、摩擦力に起因した計算誤差を含んでおり、その誤差の周波数成分は衝突による外乱トルクの周波数成分よりも十分に低いことに着目している。
すなわち、図１１（ａ）に示す外乱トルクの実際値τ_ｒｅａｌと推定外乱トルクτ_ｃａｌｃとの間の計算誤差τ_ｅｒｒを考慮した場合、このτ_ｅｒｒに相当する周波数成分をフィルタ２０４によって除去することで、図１１（ｂ）のようにτ_ｅｒｒを大きめに設定する（結果的に感度設定値ｌｉｍｉｔを大きくする）必要がなく、図１１（ｃ）のようにτ_ｅｒｒ≒０として感度設定値ｌｉｍｉｔを小さくすることができる。

特開平６－２８４７６４号公報（［００２２］～［００４３］、図１等） 特開平２－３０４８７号公報（第１図等） 特開２００１－５１７２１号公報（［０００５］～［００１０］、図３，図５等）

上記のように特許文献３の衝突検出装置では、衝突に起因した周波数成分を抽出することによって外乱トルク推定時の計算誤差τ_ｅｒｒをほぼゼロにし、感度設定値ｌｉｍｉｔを小さく設定可能として衝突検出感度の向上を図っている。
しかしながら、サーボモータ２００の速度が高く、高速で駆動されている対象物２０１が衝突した時の外乱トルク（衝突力）は低速時のそれに比べて大きい。従って、衝突判定に用いる感度設定値ｌｉｍｉｔが固定されていると、モータ速度によっては衝突を正確に検出できない場合があるため、衝突検出精度の更なる向上が要請されていた。

そこで、本発明の解決課題は、衝突の判定に電動機速度を反映させることにより、衝突検出精度を一層高めた電動機制御装置及び衝突検出方法を提供することにある。

上記課題を解決するため、請求項１に係る電動機制御装置は、電動機により駆動される負荷機械が障害物に衝突した際に前記電動機に作用する衝突力を、前記負荷機械の変位及び前記障害物のバネ定数に基づいて推定する衝突力推定手段と、
前記衝突力推定手段から出力される推定衝突力の所定の周波数成分を抽出する手段と、
前記周波数成分に基づいて前記負荷機械が前記障害物に衝突したことを検出する衝突検出手段と、
前記衝突検出手段による衝突検出時に前記電動機の速度を安全側に制御するための制御手段と、
を備えた電動機制御装置において、
前記電動機の速度に応じて変化する衝突検出用のしきい値を生成する手段を備え、
前記衝突検出手段は、前記周波数成分が前記しきい値を超えた時に前記負荷機械の衝突を検出することを特徴とする。

請求項２に係る電動機制御装置は、請求項１に記載した電動機制御装置において、前記しきい値を、前記電動機の速度に比例して変化させることを特徴とする。

請求項３に係る衝突検出方法は、電動機により駆動される負荷機械が障害物に衝突した際に前記電動機に作用する衝突力を、前記負荷機械の変位及び前記障害物のバネ定数に基づいて推定し、推定した衝突力の所定の周波数成分に基づいて前記負荷機械が前記障害物に衝突したことを検出する衝突検出方法において、
前記周波数成分と比較される衝突検出用のしきい値を前記電動機の速度に応じて変化させ、前記周波数成分が前記しきい値を超えた時に前記負荷機械の衝突を検出することを特徴とする。

請求項４に係る衝突検出方法は、請求項３に記載した衝突検出方法において、前記しきい値を、前記電動機の速度に比例して変化させることを特徴とする。

本発明によれば、負荷機械が衝突した際の推定衝突力の所定の周波数成分と比較されるしきい値を電動機速度に応じて変化させ、前記周波数成分がしきい値を超えた時に衝突を検出することにより、電動機速度が広範囲に変化する場合でも、推定誤差に対してロバストな電動機制御装置及び衝突検出方法を実現することができる。特に、衝突検出用のしきい値を電動機速度が高くなるほど大きくすることにより、衝突検出レベルの安定化が可能になる。
また、本発明によれば、衝突した障害物が弾性体である場合にも高精度に衝突を検出することができる。

本発明の第１実施形態のブロック図である。 図１における外乱オブザーバの構成を示すブロック図である。 本発明の第１参考形態におけるしきい値の特性図である。 本発明の第１参考形態における衝突時の動作を示す波形図である。 本発明の第１参考形態における衝突時の動作を示す波形図である。 本発明の第２参考形態の主要部を示すブロック図である。 本発明の実施形態の主要部を示すブロック図である。 特許文献１に記載された位置制御装置のブロック図である。 図８の動作を示す波形図である。 特許文献３に記載された衝突検出装置のブロック図である。 図１０の動作を示す波形図である。

以下、図に沿って本発明の実施形態を説明する。
図１は、本発明の第１参考形態に係る衝突検出機能を備えた電動機制御装置のブロック図である。図１において、１はサーボモータ等の電動機２により駆動される負荷機械（駆動対象物）、３は電動機２の回転子の位置を検出する位置検出器、４は位置検出値から速度を演算する速度検出器である。

また、５は電動機２の位置指令値を逐次的に出力する位置指令発生器、６はフィードバックされた位置検出値が位置指令値に追従するように速度指令値を演算する位置制御器、７は電動機２の速度検出値が速度指令値に追従するようにトルク指令値τ_ｒを演算する速度制御器、８はトルク指令値τ_ｒに従って電動機２の電流を制御する電流制御器である。

更に、９は、電動機２の速度検出値ｎ_ｍ及びトルク指令値τ_ｒから負荷機械１及び電動機２に作用する外乱トルクを推定する外乱オブザーバ、１０は推定外乱トルクτ_ｄ ^＃を近似微分する微分器である。また、１１は、微分器１０から出力される推定外乱トルク微分値τ_ｄ ^＃´と、外部から入力されたしきい値（その初期値）τ_ｔｈ０と、電動機２の速度検出値ｎ_ｍとに基づいて負荷機械１の衝突を検出する衝突検出器、１２は、衝突検出器１１からの衝突検出信号Ｓ_ｄに従って負荷機械１や電動機２、障害物の破損等を回避するための速度指令値を生成して電動機２の速度を安全側に制御する衝突制御器である。

次に、外乱オブザーバ９の構成を図２に基づいて説明する。
図２に示すように、外乱オブザーバ９では、電動機２の速度検出値ｎ_ｍを微分してノミナルの総慣性モーメント値Ｊ_ｎを乗じた結果をトルク指令値τ_ｒから減算し、時定数Ｔのローパスフィルタを通して推定外乱トルクτ_ｄ ^＃を演算する。なお、ｓは微分演算子である。

図３は、衝突検出器１１において、推定外乱トルク微分値τ_ｄ ^＃´と比較されるしきい値τ_ｔｈの特性図である。このしきい値τ_ｔｈは、外部から与えられるしきい値τ_ｔｈ０を初期値として、速度検出値ｎ_ｍがｎ_１に達するまでの範囲は速度に比例させ、速度検出値ｎ_ｍがｎ_１を超えると一定になるように重み付けされる。

この参考形態の通常時（非衝突時）の動作は図８等の従来技術とほぼ同様であり、電動機２の速度検出値ｎ_ｍが速度指令値に追従するように速度制御器７がトルク指令値を生成し、このトルク指令値に従って電流制御器８が電動機２を制御することにより負荷機械１を目標位置まで駆動する。

次いで、負荷機械１が障害物に衝突した場合の動作について説明する。
図４は、衝突時の動作を示す波形図である。時刻ｔ_ｃで負荷機械１が障害物に衝突すると、図１の外乱オブザーバ９により演算される推定外乱トルクτ_ｄ ^＃はステップ状に変化し、微分器１０は数式１により推定外乱トルク微分値τ_ｄ ^＃´を演算する。
［数式１］
τ_ｄ ^＃´＝ｓτ_ｄ ^＃

衝突の前後において推定外乱トルクτ_ｄ ^＃及びその微分値τ_ｄ ^＃´は、例えば図４のように変化し、推定外乱トルク微分値τ_ｄ ^＃´がしきい値τ_ｔｈを超えると衝突検出器１１が衝突を検出して衝突検出信号Ｓ_ｄを出力する。ここで、衝突による推定外乱トルクτ_ｄ ^＃の周波数成分は、外乱オブザーバ９の推定演算に用いるパラメータやモデルの誤差等に起因したトルクの周波数成分より高いため、推定外乱トルク微分値τ_ｄ ^＃´は衝突による外乱トルクの周波数成分が支配的になる。

衝突検出信号Ｓ_ｄが入力された衝突制御器１２では、従来と同様に電動機２の速度を安全側に制御して衝突に伴う事故の波及を防止する。すなわち、速度制御器７及び電流制御器８を介して電動機２への通電を強制的に停止し、または、電動機２が停止するまで制動トルクが発生するように制御し、停止後は衝突方向とは逆方向に負荷機械１を移動させるように電動機２を制御する。

ここで、本形態では、衝突検出に用いるしきい値τ_ｔｈが電動機２の速度検出値ｎ_ｍに応じて変化する。詳しくは、図３に示した如く、一定速度範囲（０～ｎ_１）では速度検出値ｎ_ｍに比例してしきい値τ_ｔｈが大きくなるように設定されている。

このため、負荷機械１が障害物に高速で衝突して推定外乱トルク微分値τ_ｄ ^＃´の振幅が図５（ａ）のように大きい場合には、この推定外乱トルク微分値τ_ｄ ^＃´と比較されるしきい値τ_ｔｈ１も大きく、これとは逆に、負荷機械１が低速で衝突して推定外乱トルク微分値τ_ｄ ^＃´の振幅が図５（ｂ）のように小さい場合には、この推定外乱トルク微分値τ_ｄ ^＃´と比較されるしきい値τ_ｔｈ２も小さくなる。
これにより、電動機速度に関わらずしきい値が固定されている場合に比べて、負荷機械１が低速で衝突した時にも確実に衝突を検出することができ、衝突検出精度の向上が可能になる。

なお、速度検出値ｎ_ｍの代わりに、位置制御器６から出力される速度指令値を衝突検出器１１に入力し、速度指令値の一定範囲ではしきい値τ_ｔｈを速度指令値に比例させて推定外乱トルク微分値τ_ｄ ^＃´と比較することにより、衝突を検出しても良い。
あるいは、位置検出器３から出力される位置検出値や位置指令発生器５から出力される位置指令値に応じてしきい値τ_ｔｈを変化させ、このしきい値τ_ｔｈを推定外乱トルク微分値τ_ｄ ^＃´との比較に用いても良い。

次に、図６は本発明の第２参考形態の主要部を示すブロック図である。
図６（ａ）は、上位コントローラ２０から負荷機械１に対する移動経路情報（所定の経路上を移動させるための速度指令値や位置指令値等）を衝突検出器１１に入力し、この移動経路情報に応じてしきい値（初期値τ_ｔｈ０）を変化させることにより図３のようなしきい値τ_ｔｈのパターンを作成し、このしきい値τ_ｔｈを推定外乱トルク微分値τ_ｄ ^＃´と比較して得た衝突検出信号Ｓ_ｄを衝突制御器１２へ出力しても良い。
また、図６（ｂ）に示すように、上位コントローラ２０により図３のようなしきい値τ_ｔｈのパターンを予め作成しておき、このしきい値τ_ｔｈを衝突検出器１１に入力して推定外乱トルク微分値τ_ｄ ^＃´と比較して得た衝突検出信号Ｓ_ｄを衝突制御器１２へ出力しても良い。

次いで、上述した各参考形態を踏まえて、本発明の実施形態を、図７に基づいて説明する。
衝突時の外乱トルクの時間変化や衝突力は、図１に示した微分器１０のように近似微分による演算処理のほか、ハイパスフィルタやバンドパスフィルタ等の周波数成分抽出手段により検出することが可能である。
特に、負荷機械１が弾性体等に衝突した場合、その衝突力Ｆ_ｄは弾性体のバネ定数Ｋ及び負荷機械１の変位Δｘから、数式２によって求めることができる。
［数式２］
Ｆ_ｄ＝ＫΔｘ

数式２の変位Δｘが電動機の一定速度ｘ´に従って変化すると仮定した場合、衝突した時点をｔ＝０とすれば、数式２は数式３のように変形される。
［数式３］
Ｆ_ｄ＝Ｋｘ´ｔ

従って、負荷機械１の衝突により発生する衝突力Ｆ_ｄはω＝Ｋｘ´［ｒａｄ／ｓ］なる周波数成分を持っているため、この周波数成分ωをハイパスフィルタやバンドパスフィルタ等により抽出し、その結果を衝突検出用のしきい値ω_ｔｈと比較することにより、障害物が弾性体であっても高精度に衝突を検出することができる。

図７は、この実施形態の主要部を示すブロック図であり、図１と同一の部分には同一の符号を付してある。
図７における衝突力推定手段９Ａ、ハイパスフィルタ等の周波数成分抽出手段１０Ａは、図１における外乱オブザーバ９、微分器１０にそれぞれ相当する。ここで、衝突力推定手段９Ａは、弾性体のバネ定数Ｋと電動機２の速度検出値ｎ_ｍ（数式３における速度ｘ´に相当）とを用いて数式３により衝突力Ｆ_ｄを推定し、その周波数成分ωを周波数成分抽出手段１０Ａにより抽出して衝突検出器１１Ａに入力する。衝突検出器１１Ａでは、入力された周波数成分ωが衝突検出用のしきい値ω_ｔｈ（初期値をω_ｔｈ０とする）を超えた場合に衝突と判定して衝突検出信号Ｓ_ｄを出力すれば良い。
この実施形態においても、衝突検出用のしきい値ω_ｔｈを電動機２の速度ｎ_ｍに比例して大きくなるように変化させることが望ましい。

本発明の原理は、サーボモータやステッピングモータ（回転型ステッピングモータ、または、回転型ステッピングモータにボールねじ等を組み合わせた直動型ステッピングモータ）、リニアモータ等を対象とした電動機制御装置及び衝突検出方法として、各種の製造・加工・搬送分野等に利用することができる。

１：負荷機械
２：電動機
３：位置検出器
４：速度検出器
５：位置指令発生器
６：位置制御器
７：速度制御器
８：電流制御器
９：外乱オブザーバ
９Ａ：衝突力推定手段
１０：微分器
１０Ａ：周波数成分抽出手段
１１，１１Ａ：衝突検出器
１２：衝突制御器
２０：上位コントローラ

Claims (4)

1. 電動機により駆動される負荷機械が障害物に衝突した際に前記電動機に作用する衝突力を、前記負荷機械の変位及び前記障害物のバネ定数に基づいて推定する衝突力推定手段と、
   前記衝突力推定手段から出力される推定衝突力の所定の周波数成分を抽出する手段と、
   前記周波数成分に基づいて前記負荷機械が前記障害物に衝突したことを検出する衝突検出手段と、
   前記衝突検出手段による衝突検出時に前記電動機の速度を安全側に制御するための制御手段と、
   を備えた電動機制御装置において、
   前記電動機の速度に応じて変化する衝突検出用のしきい値を生成する手段を備え、
   前記衝突検出手段は、前記周波数成分が前記しきい値を超えた時に前記負荷機械の衝突を検出することを特徴とする電動機制御装置。
2. 請求項１に記載した電動機制御装置において、
   前記しきい値を、前記電動機の速度に比例して変化させることを特徴とする電動機制御装置。
3. 電動機により駆動される負荷機械が障害物に衝突した際に前記電動機に作用する衝突力を、前記負荷機械の変位及び前記障害物のバネ定数に基づいて推定し、推定した衝突力の所定の周波数成分に基づいて前記負荷機械が前記障害物に衝突したことを検出する衝突検出方法において、
   前記周波数成分と比較される衝突検出用のしきい値を前記電動機の速度に応じて変化させ、前記周波数成分が前記しきい値を超えた時に前記負荷機械の衝突を検出することを特徴とする衝突検出方法。
4. 請求項３に記載した衝突検出方法において、
   前記しきい値を、前記電動機の速度に比例して変化させることを特徴とする衝突検出方法。

Images