(57)【要約】【課題】低剛性の機械の可動部位置制御において、応答遅れ及びそれにともなう振動発生を抑制する。【解決手段】低剛性の機械７を駆動するモータ３を位置、速度ループ制御する。位置指令を微分した指令速度に比例する補正量Ｑ１と、２階微分した指令加速度に比例する補正量Ｑ２を求める。補正量Ｑ１，Ｑ２を加算して推定ねじれ量に相当する速度オフセット量Ｖｏｆを求める。該速度オフセット量の微分値に係数βを乗じてトルクオフセット量Ｔｏｆを求める。速度オフセット量Ｖｏｆを位置ループ制御１で求められた速度指令Ｖｃｓに加算する。速度ループ制御２で出力されるトルク指令Ｔｃにトルクオフセット量Ｔｏｆを加算してモータへの駆動指令とする。速度、トルクオフセット量Ｖｏｆ、Ｔｏｆにより、モータ３と機械間のねじれ量が制御される。通常の位置、速度ループ制御により機械位置、速度が正確に制御される。【選択図】      図２
2004213472.tif 000002
【特許請求の範囲】
【請求項１】
指令位置と検出モータ位置との位置偏差を求めポジションゲインを乗じて速度指令を求める位置ループ制御と、速度指令と検出モータ速度に基づいて速度ループ制御を行ってサーボモータを制御し、該サーボモータで駆動される機械可動部の位置を制御する制御装置において、
指令加速度に比例した速度オフセット量を位置ループ制御によって出力される速度指令に加算し速度ループ制御への速度指令とする速度指令生成手段と、
該速度オフセット量を微分した微分値に基づいてトルクオフセット量を算出する手段と、
前記加算された速度指令に基づく速度ループ制御によって出力されるトルク指令に前記トルクオフセット量を加算し補正されたトルク指令とする手段と、
を備えたことを特徴とする制御装置。
【請求項２】
指令位置と検出モータ位置との位置偏差を求めポジションゲインを乗じて速度指令を求める位置ループ制御と、速度指令と検出モータ速度に基づいて速度ループ制御を行ってサーボモータを制御し、該サーボモータで駆動される機械可動部の位置を制御する制御装置において、
指令速度に比例したオフセット量と、指令加速度に比例したオフセット量とを加算して速度オフセット量を求める手段と、
位置ループ制御によって出力される速度指令に速度オフセット量を加算し速度ループ制御への速度指令とする速度指令生成手段と、
該速度オフセット量を微分した微分値に基づいてトルクオフセット量を算出する手段と、
前記加算された速度指令に基づく速度ループ制御によって出力されるトルク指令に前記トルクオフセット量を加算し補正されたトルク指令とする手段と、
を備えたことを特徴とする制御装置。
【請求項３】
指令位置とサーボモータで駆動される機械可動部に取り付けられた位置検出器によって検出された機械可動位置より位置偏差を求めポジションゲインを乗じて位置のフルクローズド・ループ制御により速度指令を求める手段を設け、前記速度指令生成手段の代わりに、前記位置ループ制御によって出力される速度指令に速度オフセット量を加算した指令の高周波成分と位置のフルクローズド・ループ制御による速度指令の低周波成分を加算し速度ループ制御への速度指令とする速度指令生成手段とした請求項１又は請求項２に記載の制御装置。
【請求項４】
前記速度指令生成手段は、位置のフルクローズド・ループ制御による速度指令から前記位置ループ制御による速度指令と速度オフセット量とを減算したものをローパスフィルタで処理して得られた低周波成分と、前記位置ループ制御によって出力される速度指令と、速度オフセット量とを加算して速度ループ制御への速度指令とする請求項３に記載の制御装置。
【請求項５】
指令位置とサーボモータの回転位置を検出する検出器によって検出された位置より位置偏差を求めポジションゲインを乗じて第１の速度指令を求める手段と、
指令位置とサーボモータで駆動される機械可動部に取り付けられた位置検出器によって検出された機械可動位置より位置偏差を求めポジションゲインを乗じて第２の速度指令を求める手段と、
速度指令とサーボモータの回転速度を検出する検出器によって検出された速度に基づいて速度ループ制御を行いトルク指令を求める速度制御手段と、
前記第２の速度指令と第１の速度指令の差の速度指令差を算出する手段と、
指令加速度に比例したオフセット量から前記速度指令差を減じ、その高周波成分を速度オフセット量として求める手段と、
該速度オフセット量を微分した微分値に基づいてトルクオフセット量を算出する手段と、
前記速度指令差の低周波成分と第１の速度指令と速度オフセット量を加算し速度制御手段への速度指令とする手段と、
速度制御手段から出力されるトルク指令に前記トルクオフセット量を加算し補正されたトルク指令とする手段と、
を備えたことを特徴とする制御装置。
【請求項６】
速度オフセット量として求める手段は、前記指令加速度に比例したオフセット量と共に指令速度に比例したオフセット量を加算して前記速度指令差を減じたものの高周波成分を速度オフセット量として求めようにした請求項５に記載の制御装置。
【請求項７】
前記指令加速度に比例したオフセット量は位置指令を２階微分して求める請求項１乃至６の内いずれか１項に記載の制御装置。
【請求項８】
前記トルクオフセット量を算出する手段は、前記速度オフセット量の微分値に所定比例係数を乗じて求める請求項１乃至７の内いずれか１項に記載の制御装置。
【請求項９】
前記トルクオフセット量を算出する手段は、前記速度オフセット量の微分値の大きさを分割し各分割領域における速度オフセット量の微分値に対するトルクオフセット量を直線式で表し、該直線式に基づいてトルクオフセット量を求める請求項１乃至７の内いずれか１項に記載の制御装置。
【請求項１０】
前記トルクオフセット量を算出する手段は、速度オフセット量の微分値に対応するトルクオフセット量をテーブルとして記憶しておき、該テーブルから求めるようにした請求項１乃至７の内いずれか１項に記載の制御装置。
【請求項１１】
指令位置と検出モータ位置との位置偏差を求めポジションゲインを乗じて速度指令を求める位置ループ制御と、速度指令と検出モータ速度に基づいて速度ループ制御を行ってサーボモータを制御し、該サーボモータで駆動される機械可動部の位置を制御する制御装置において、
指令加加速度に比例したトルクオフセット量を前記速度ループ制御によって出力されるトルク指令に加算し補正されたトルク指令とする手段を備えたことを特徴とする制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、サーボモータで駆動される機械可動部の位置を制御する制御装置に関する。
【０００２】
【従来の技術】
大型の機械などでは、機械可動部を駆動するモータと機械可動部の間の機械要素にねじれ要素等が存在し低剛性である。このような低剛性の場合には、このねじれ要素等は一定の範囲では加速度に比例してねじれ量が大きくなるバネ的な特性を示し、このモータと機械の間がバネ結合されているとみなされる。この場合、機械の移動開始時に機械の移動を遅らせたり、機械可動部が振動する原因となる。
【０００３】
このような大型の機械においては機械可動部の振動を抑えるために、位置ループのポジションゲインを大きくすることができず、その結果として機械可動部の移動の遅れが発生する。例えば、大型の工作機械においては、この遅れに起因して加工精度が低下するという問題がある。
【０００４】
この機械の低剛性による遅れを補償する方法として、加速度指令値にボールネジ駆動系の総合たて剛性の逆数を乗じたものと、ボールネジ駆動系の総合ねじれ剛性の逆数を乗じたものを加算し、その加算値に負荷側慣性モーメントを乗じてたわみ補償値を求め、これを微分してたわみ速度補償値を求め、さらにこれを微分してトルク補償値を求め、これらたわみ補償値、たわみ速度補償値、トルク補償値をそれぞれ位置指令値、速度指令値、トルク指令値に加算することによって、ボールネジ駆動系のたわみ補償を行う方法が知られている（例えば、特許文献１参照）。
【０００５】
又、移動指令の加速度に比例したオフセット量を位置ループ制御で求めた速度指令に加算して速度ループへの速度指令とすることによって、低剛性によるねじれ分の遅れを補償するようにした方法も知られている（例えば、特許文献２参照）。
【０００６】
【特許文献１】
特開平１０−３２６１１４号公報
【特許文献２】
特許第３３０８６５６号公報
【０００７】
【発明が解決しようとする課題】
本発明の目的は、機械可動部と該機械可動部を駆動するモータ間に低剛性の機械要素があるとき、この機械要素のねじれ等により、指令に対して機械可動部の移動が遅れることを抑制し、遅れに伴って発生する機械振動を低減することにある。
【０００８】
【課題を解決するための手段】
請求項１に係わる発明は、指令位置と検出モータ位置との位置偏差を求めポジションゲインを乗じて速度指令を求める位置ループ制御と速度指令と検出モータ速度に基づいて速度ループ制御を行ってサーボモータを制御し、該サーボモータで駆動される機械可動部の位置を制御する制御装置において、指令加速度に比例した速度オフセット量を位置ループ制御によって出力される速度指令に加算し速度ループ制御への速度指令とする速度指令生成手段と、該速度オフセット量の微分し該微分値に基づいてトルクオフセット量を算出する手段と、前記加算された速度指令に基づく速度ループ制御によって出力されるトルク指令に前記トルクオフセット量を加算し補正されたトルク指令とする手段とを備えることにより、低剛性の機械のねじれを補正し、応答遅れを低減させて振動発生を抑制した。又、請求項２に係わる発明は、速度オフセット量として、指令加速度に比例したオフセット量に指令速度に比例したオフセット量を加算したものとした。
【０００９】
請求項３に係わる発明は、指令位置とサーボモータで駆動される機械可動部に取り付けられた位置検出器によって検出された機械可動位置より位置偏差を求めポジションゲインを乗じて位置のフルクローズド・ループ制御により速度指令を求める手段を設け、請求項１，２に係わる発明の速度指令生成手段の代わりに、前記位置ループ制御によって出力される速度指令に速度オフセット量を加算した指令の高周波成分と位置のフルクローズド・ループ制御による速度指令の低周波成分を加算し速度ループ制御への速度指令とする速度指令生成手段とした。請求項４に係わる発明は、請求項３に係わる発明における速度指令生成手段を、位置のフルクローズド・ループ制御により速度指令から前記位置ループ制御のよる位置指令を減算したものをローパスフィルタで処理して得られた低周波成分と前記位置ループ制御によって出力される速度指令と速度オフセット量とを加算して速度ループ制御への速度指令とするものとした。
【００１０】
請求項５に係わる発明は、指令位置とサーボモータの回転位置を検出する検出器によって検出された位置より位置偏差を求めポジションゲインを乗じて第１の速度指令を求める手段と、指令位置とサーボモータで駆動される機械可動部に取り付けられた位置検出器によって検出された機械可動位置より位置偏差を求めポジションゲインを乗じて第２の速度指令を求める手段と、速度指令とサーボモータの回転速度を検出する検出器によって検出された速度に基づいて速度ループ制御を行いトルク指令を求める速度制御手段と、第２の速度指令と第１の速度指令の差の速度指令差を算出する手段と、指令加速度に比例したオフセット量から速度指令差を減じ、その高周波成分を速度オフセット量として求める手段と、該速度オフセット量の微分し該微分値に基づいてトルクオフセット量を算出する手段と、速度指令差の低周波成分と第１の速度指令と速度オフセット量を加算し速度制御手段への速度指令とする手段と、速度制御手段から出力されるトルク指令にトルクオフセット量を加算し補正されたトルク指令とする手段とを備え、ねじれをフィードバック制御するようにした。又、請求項６に係わる発明は、請求項５に係わる発明において、速度オフセット量として求める手段を、指令加速度に比例したオフセット量と共に指令速度に比例したオフセット量を加算して速度指令差を減じたものの高周波成分を速度オフセット量とした。
【００１１】
そして、請求項７に係わる発明は、前述の各発明において、位置指令を２階微分して指令加速度に比例したオフセット量を求めるようにしたものである。又、トルクオフセット量を算出する手段を、請求項８に係わる発明は、速度オフセット量の微分値に所定比例係数を乗じて求めるようにし、請求項９に係わる発明は、速度オフセット量の微分値の大きさを分割し各分割領域における速度オフセット量の微分値に対するトルクオフセット量を直線式で表し、該直線式に基づいてトルクオフセット量を求めるようにし、請求項１０に係わる発明は、速度オフセット量の微分値に対応するトルクオフセット量をテーブルとして記憶しておき、該テーブルから求めるようにした。
又、請求項１１に係わる発明は、単に、指令加加速度に比例したトルクオフセット量を前記速度ループ制御によって出力されるトルク指令に加算し補正されたトルク指令とする手段を設けることにより前記課題を解決するようにした。
【００１２】
【発明の実施の形態】
低剛性の機械をバネ結合したモデルで表すと図１に示すものとなる。バネ系の運動方程式は、バネ系への入力であるモータの出力をＦ、バネ定数をｋ、機械の停止位置からの移動量をＸ、モータの停止位置からの移動量（機械の移動量の単位と同一とする）をＸ０、機械の加速度をａ、モータの加速度をａ０、機械の質量をＭ、モータ駆動部の質量をｍとすると、次の１式、２式で表される。
Ｍａ＝ｋ（Ｘ０−Ｘ）            …（１）
ｍａ０＝Ｆ−ｋ（Ｘ０−Ｘ）        …（２）
すなわち、モータから伝えられる力はモータの加速とバネの伸縮になり、バネの伸縮が制御対象の機械を動かすことになる。
【００１３】
十分に長い時定数をもって力Ｆが増加する場合、微小時間での加速度は０とみなすことができる。この場合、上記（２）式より、バネの伸縮量（Ｘ０−Ｘ）は、Ｆ／ｋとなる。力Ｆを与える場合に理想的に伸縮量Ｆ／ｋが生じるならば、力Ｆを伸縮量Ｆ／ｋと一致させるように与えることで、バネ系は剛体と同じ挙動を示すことになる。
【００１４】
機械に対する移動指令に対してモータ駆動部の移動は上述したバネの伸縮量を含んだ移動量となるが、通常の位置ループ制御においては、制御対象の機械の位置（フルローズドループの場合）、若しくは該機械を駆動するモータ駆動部の位置（セミクローズド・ループの場合）と移動指令との位置偏差からモータ駆動部への速度指令を作成している。そのため、バネの伸縮量は位置ループの応答性に依存し、フルローズドループの場合には、ポジションゲインが小さいときには、モータ駆動部の動作が緩慢になり、バネ系は安定するが精度の低下を招く。又、逆にポジションゲインが大きいときには、モータ駆動部の動作が急峻になり、必要以上にバネを伸ばして振動を引き起こす結果となる。
【００１５】
又、セミクローズの場合においても、ポジションゲインが小さいときには、機械の追従が遅れ、指令位置に対する機械位置のずれが大きくなり精度を低下させる。又、ポジションゲインが大きいときには、前述したように振動を引き起こす原因となる。
【００１６】
そこで、本発明は、低剛性の機械において、低剛性の機械要素のねじれ等のバネとみなされる要素の伸縮量がモータの加速度ａ０に比例することを利用して、この加速度ａ０とほぼ同値である移動指令の２階微分値（指令加速度）からねじれ量（バネの伸縮量）を推定し、必要なねじれ量が発生するようにモータに対してフィードフォワード的に速度指令を補正することによって、安定にねじれを発生させ、位置ループ安定化させる。
なお、機械に摩擦ｆが存在する場合には、機械の運動方程式は、
Ｍａ＝ｋ（Ｘ０−Ｘ）−ｆ
となり、速度一定の場合には（ｆ／ｋ）に相当する位置偏差が生じる。フルクローズドループの場合には機械位置Ｘが指令位置に一致するように制御が働くが、セミクローズドループの場合にはモータ位置Ｘ０が指令位置に一致するため、機械位置Ｘに位置偏差が生じてしまう。これを解決するためには（ｆ／ｋ）に比例した速度オフセットを速度指令に加算して、機械位置Ｘが指令位置に一致するように補正すればよい。
【００１７】
さらに、低剛性の機械の場合、速度ループにおけるゲインも大きくすることはできない。そのため、フィードフォワード的に速度指令を補正しても、モータの回転は遅れ、必要なねじれ量が発生するには時間遅れが生じる。本発明は、この遅れを、トルク指令に補正することによって、モータの応答をよくして、必要なねじれ量を応答よく発生させるようにする。
【００１８】
図２は、本発明の第１の実施形態の機械可動部の位置を制御する制御装置のモータ制御系のブロック図である。
図２中、符号１は位置ループにおけるポジションゲインＫｐの伝達関数を示す項、符号２は速度制御部の項、符号３はサーボモータを示す。該サーボモータ３には、工作機械等の機械７の可動部が接続され、サーボモータ３と機械の間は低剛性の要素で接続されているとして、バネ８で結合されているものとして模式的に表している。なお、９は摩擦の項を示す。
【００１９】
さらに、本実施形態では、バネ８で示される機械とサーボモータ間の低剛性の機械要素のねじれを推測してそのねじれ量を補うための位置指令の加速度に補正係数α２を乗じて得られる加速度に比例量を出力する項４、指令速度に補正係数α１を乗じて得られるに機械の摩擦９の対応する指令速度に比例する成分を出力する項５を備え、項４、５の出力を加算器１３で加算して推定ねじれ量に相当の速度オフセット量Ｖｏｆを求め、該速度オフセット量を加算器１１で位置ループ制御１で求めた速度指令Ｖｃｓに加算し、速度制御部２への速度指令Ｖｃとするようにしている。
さらに、項６で速度制御部２の遅れを補うために速度オフセット量Ｖｏｆの微分値に補正係数βを乗じてトルクオフセット量Ｔｏｆを求め、トルク指令Ｔｃに加算器１２で加算してモータ３へのトルク指令（電流指令）としている。
【００２０】
減算器１０で位置指令とサーボモータ３に取り付けられた位置検出器からフィードバックされるモータ位置を減じて位置偏差を求め該位置偏差に項１のポジションゲインＫｐを乗じて速度指令Ｖｃｓを求める（位置ループ制御）。さらに、項５で位置指令を微分した指令速度に補正係数α１を乗じて、摩擦に対応するオフセット値を求める。又、項４で位置指令を２階微分した指令加速度に補正係数α２を乗じてねじれ量の加速度分に相当するオフセット値を求め、この項４、項５で求められた２つのオフセット値を加算器１３で加算して速度オフセット量Ｖｏｆとし、該速度オフセット量Ｖｏｆを加算器１１で位置ループ制御で求めた速度指令Ｖｃｓに加算して速度指令Ｖｃｓを補正する。この補正された速度指令Ｖｃを速度制御部２への指令とする。
【００２１】
速度制御部２では、この補正された速度指令Ｖｃとサーボモータ３に取り付けられた速度検出器からフィードバックされる実速度に基づいて、比例、積分制御等の速度ループ制御を行いトルク指令Ｔｃを出力する。一方、項６では、速度オフセット量Ｖｏｆを微分し、その微分値に補正係数βを乗じて、トルクオフセット量Ｔｏｆを求め、加算器１２で該トルクオフセット量Ｔｏｆを速度制御部２から出力されたトルク指令Ｔｃに加算して、サーボモータ３へ出力し駆動する。
【００２２】
速度制御部２に遅れがないとすれば、摩擦と加速度に比例した速度オフセット量Ｖｏｆによりバネ８が伸縮されるから、位置指令とフィードバックされるモータ位置による位置偏差にポジションゲインＫｐを乗じた位置ループ制御による速度指令Ｖｃｓは、バネを考慮することなく機械を移動させる速度指令となる。
【００２３】
しかし、速度制御部２においても制御対象の機械が低剛性であることから、ゲインを大きくとることができないので、この速度制御部２でも指令に対してモータ及び機械可動部の動作は遅れる。そこで、本実施形態では、速度オフセット量Ｖｏｆの微分値に比例するトルクオフセット量Ｔｏｆを速度制御部２から出力されるトルク指令Ｔｃに加算することによって、この速度制御部２の遅れを補正して、機械７を位置指令に応答よく追従させるようにしている。
【００２４】
図３は、速度オフセット量Ｖｏｆとトルクオフセット量Ｔｏｆの説明図である。
位置指令の微分値、すなわち、指令速度が図３（ａ）に示すものとすると、速度オフセット量Ｖｏｆにおける加速度比例成分は、この指令速度を微分した値に比例する図３（ｂ）に示す破線で示すようなものとなる。そして、速度オフセット量Ｖｏｆは図３（ｂ）の破線で示すこの加速度比例成分に図２の速度に比例する成分を加算したものとなる。
又、トルクオフセット量Ｔｏｆは、この速度オフセット量Ｖｏｆの微分に比例したものであるから図３（ａ）で実線で示すようなものとなる。
【００２５】
図４は、本発明の第２の実施形態のモータ制御系のブロック図である。
この第２の実施形態では、位置のフルクローズド・ループ制御を追加している点で第１の実施形態と相違する。
この第２の実施形態では、位置指令と機械可動部の位置を検出する位置検出器からフィードバックされる機械位置との位置偏差を減算器１７で求め、項１５でこの位置偏差にポジションゲインＫｐ（このポジションゲインは項１のポジションゲインと同じ値）を乗じて速度指令Ｖｃｆを求める。このフルローズドループ制御による速度指令から、セミクローズド・ループ制御で求めた速度指令Ｖｃｓに速度オフセット量Ｖｏｆを加算して補正した速度指令を減算器１８で減じて、ローパスフィルタ１６でフィルタ処理し、このローパスフィルタ１６の低周波成分の出力を加算器１１で速度オフセット量Ｖｏｆで補正された速度指令Ｖｃｓに加算して速度制御部２への指令Ｖｃとしている。速度制御部２以降の制御は第１の実施形態と同一である。
【００２６】
この第２の実施形態では、速度オフセット量Ｖｏｆで補正されたセミクローズド・ループの速度指令Ｖｃｓが減算器１８、ローパスフィルタ１６を通り、再びこの速度オフセットＶｏｆで補正されたセミクローズド・ループの速度指令Ｖｃｓに加算されることから、速度制御部２に入力される速度指令Ｖｃは、フルクローズド・ループ制御の速度指令Ｖｃｆの低周波成分と、速度オフセット量Ｖｏｆで補正されたセミクローズド・ループの速度指令Ｖｃｓから低周波成分を除去した高周波成分が合成されたものとなる。このことは、モータが加減速される過渡状態（高周波成分を含む状態）では、速度オフセット量Ｖｏｆで補正されたセミクローズド・ループの速度指令Ｖｃｓが有効に機能し、モータを制御する。一方、指令速度に達し速度一定となったような定常状態（高周波成分は含まず、低周波成分のみ）では、速度オフセット量Ｖｏｆで補正されたセミクローズド・ループの速度指令Ｖｃｓはカットされ、フルクローズド・ループ制御による速度指令Ｖｃｆのみが有効に機能し、定常状態では位置のフルクローズド・ループ制御が行われることになる。
【００２７】
ローパスフィルタ１４の出力をＶｃｆＬ−（ＶｃｓＬ＋ＶｏｆＬ）とすると、速度制御部に入力される速度指令Ｖｃは次のようになる。
2004213472.tif 000003
ここで、ＶｃｓＬはＶｃｓの低周波成分であるから、（Ｖｃｓ−ＶｃｓＬ）はセミクローズド・ループ制御による速度指令の高周波成分ＶｃｓＨを意味し、同様に（Ｖｏｆ−ＶｏｆＬ）は速度オフセット量Ｖｏｆの高周波成分ＶｏｆＨを意味する。すなわち、速度制御部に入力される速度指令Ｖｃは、セミクローズド・ループの速度指令Ｖｃｓと速度オフセット量Ｖｏｆの高周波成分ＶｃｓＨ，ＶｏｆＨと位置のフルクローズド・ループ制御による速度指令Ｖｃｆの低周波成分ＶｃｆＬで構成されることになる。
【００２８】
これにより、加減速時には高周波成分が存在するから、セミクローズド・ループ制御による速度指令が有効に機能する。しかし、速度が一定となった定常状態では、高周波成分はなく、Ｖｃｓ＝ＶｃｓＬ、Ｖｏｆ＝ＶｏｆＬとなり、速度制御部２に入力される速度指令Ｖｃ＝ＶｃｆＬ＝Ｖｃｆとなり、位置の制御はフルクローズド・ループ制御のみが有効になされることになる。なお、セミクローズド・ループの速度指令Ｖｃｓと速度オフセット量Ｖｏｆを加算したもに配パスフィルタを通し、高周波成分としたものと、フルクローズド・ループ制御による速度指令Ｖｃｆをローパスフィルタに通し低周波成分としたものを加算して速度制御部２に入力される速度指令としても等々の効果を得ることができる。
【００２９】
図５は、本発明の第３の実施形態のモータ制御系のブロック図である。
この第３の実施形態では、第２の実施形態と同様に、位置のセミクローズド・ループ制御とフルクローズド・ループ制御が行われるものである。
位置のフルクローズド・ループ制御により項１５より出力される速度指令Ｖｃｆから位置のセミクローズド・ループ制御による項１から出力される速度指令Ｖｃｓを減算器１８で減じて実ねじれ量に相当する分を求める。
また、項４の出力である位置指令の加速度に比例する部分Ｑ２と項５の出力である機械の摩擦９に対応する部分Ｑ１を加算した推定ねじれ量に相当する分から前記実ねじれ量に相当する分を減じる演算を加減速器２０で行い、この演算結果をハイパスフィルタ１９に通して速度オフセット量Ｖｏｆを求める。
【００３０】
加算器２１では、項１から出力される位置のセミクローズド・ループ制御による速度指令Ｖｃｓに求めた速度オフセット量Ｖｏｆを加算する共に、フルクローズド・ループ制御による速度指令Ｖｃｆとセミクローズド・ループ制御による速度指令Ｖｃｓとの差をローパスフィルタ１６に通したものを加算し、これを速度制御部２への速度指令Ｖｃとする。また、この速度制御部２の出力であるトルク指令Ｔｃに、項６で速度オフセット量Ｖｏｆを微分して補正係数βを乗じて求めたトルクオフセット量Ｔｏｆを加算して補正し、サーボモータ３への指令とする。
【００３１】
この第３の実施形態では、位置指令に対する機械位置の偏差をεｆ、モータ位置の偏差をεｓとすると、減算器１８で求められる値は「Ｋｐ×（εｆ−εｓ）」である。（εｆ−εｓ）は、機械位置とモータ位置の差を表すから、実際に生じているねじれ量を表している。よって、減算器１８の出力は実ねじれ量に相当する値が出力されていることになる。一方、指令速度に補正係数α１を乗じて得られた値Ｑ１と、指令加速度に補正係数α２を乗じて得られた値Ｑ２を加算して求めた推定ねじれ量に相当する値であり、この推定ねじれ量に相当する値から実ねじれ量に相当する値を減じた値は、ねじれ量の偏差である。推定ねじれ量に相当する値は指令するものであり、実ねじれ量に相当する値はフィードバックにより得られた値であるので、このねじれ量偏差をハイパスフィルタで処理して出力される速度オフセット量Ｖｏｆによって、このねじれ量偏差を「０」にするようにフィードバック制御されることになる。いわばこの第３の実施形態では、ねじれ量に対する独自のフィードバック制御を行うような形となる。
【００３２】
このことは、推定ねじれ量の指令によって、実際のねじれ量が制御されることになるから、機械位置は、セミクローズド・ループ制御（項１）またはフルクローズド・ループ制御（項１５）による速度指令Ｖｃｓ，Ｖｃｆで制御されることになる。しかも、加減速時の過渡状態では、ローパスフィルタ１６で高周波成分がカットされるから、セミクローズド・ループ制御の速度指令Ｖｃｓが作用するが、加減速が終了し、定常状態では、高周波成分はなく、低周波成分となることから、フルクローズド・ループ制御（項１５）による速度指令Ｖｃｆが有効に作用することになる。すなわち、ローパスフィルタ１６の出力を（ＶｃｆＬ−ＶｃｓＬ）とし、定常状態では上述したように高周波成分はなく速度オフセット量Ｖｏｆは「０」、Ｖｃｓ＝ＶｃｓＬとなる。よって、加算器２１から出力される速度指令Ｖｃは次のようになる。
【００３３】
2004213472.tif 000004
以上のようにして、加減速区間以外の定常状態では、位置の制御はフルクローズド・ループ制御がなされることになる。
【００３４】
図６は、上述した各実施形態を実施する制御装置５０の要部ブロック図である。但し、図１で示した実施形態の場合には、機械可動部の位置を検出する位置検出器５６がなく、機械可動部位置のフィードバックもないものである。
【００３５】
数値制御装置５１は移動指令等の各種指令指令を、共有メモリ５２を介してディジタルサーボ回路５３に出力する。ディジタルサーボ回路５３は、プロセッサやＲＯＭ，ＲＡＭ等のメモリ等で構成され、指令された移動指令と、サーボモータ３の回転位置、速度を検出する位置・速度検出器５５からフィードバックされるモータ位置、速度に基づいて、さらに、機械７の可動部位置を検出す位置検出器５６からフィードバックされる機械位置に基づいて、位置、速度のフィードバックら制御処理を行うことによって、図１または図４または図５の処理を処理してトルク指令を求めサーボアンプ５４を介してサーボモータ３を駆動し、機械７の可動部を移動させる。この図６に示す構成は、従来から公知のものであるので、詳細な構成の説明は省略する。
【００３６】
図７は、図１に示した第１の実施形態の制御を行うときのディジタルサーボ回路のプロセッサが所定周期毎実行する処理のフローチャートである。
数値制御装置５１は入力されたプログラムに基づいて、移動指令を分配周期毎共有メモリ５２を介してディジタルサーボ回路５３に出力する。ディジタルサーボ回路５３はこの移動指令を受けて所定周期毎図７の処理を開始し、まず、所定周期毎の移動指令を取得する（ステップ１００）。この移動指令は所定周期間内における移動指令であるから、実質的に指令速度となる。そして、位置・速度検出器５５からフィードバックされる位置、速度のフィードバック量を読み取る（ステップ１０１）。
【００３７】
次に、取得した所定周期毎の移動指令（指令速度）と前周期の移動指令の差より指令の加速度を求める（ステップ１０２）。指令速度に補正係数α１を乗じた値と、指令加速度に補正係数α２を乗じた値を加算して速度オフセット量Ｖｏｆを求める（ステップ１０３）。ステップ１００で求めた移動指令とステップ１０１で求めた位置のフィードバック量に基づいて位置偏差を求め、該位置偏差にポジションゲインＫｐを乗じて速度指令Ｖｃｓを求める（ステップ１０４）。
【００３８】
次に、ステップ１０３，１０４で求めた速度指令Ｖｃｓと速度オフセット量Ｖｏｆを加算して補正された速度指令Ｖｃを求める（ステップ１０５）。さらに、前周期と当該周期の速度オフセット量Ｖｏｆの差より速度オフセット量の変化量（微分値）を求め、この変化量に補正係数βを乗じてトルクオフセット量Ｔｏｆを求める（ステップ１０６，１０７）。
【００３９】
また、ステップ１０５で求めた速度指令とステップ１０１で求めた速度フィードバック量に基づいて、速度ループ処理を行いトルク指令Ｔｃを求める（ステップ１０８）。求めたトルク指令Ｔｃに、ステップ１０７で求めたトルクオフセット量Ｔｏｆを加算して電流制御部へトルク指令（電流指令）として出力する（ステップ１０９）。図示していないが、電流制御部では従来と同様にこのトルク指令に基づいて電流ループ処理等を行い、サーボアンプ５４を介してサーボモータ３を駆動制御する。
【００４０】
図８は、図４に示す第２の実施形態におけるディジタルサーボ回路５３のプロセッサが所定周期毎実施する処理のフローチャートである。
第１の実施形態の処理と比較して、ステップ２０２で機械位置のフィードバック量を読み取る点が追加されただけで、ステップ２００〜２０５までの処理は第１の実施形態のステップ１００〜１０４と同一である。この第２の実施形態では、ステップ２００で求めた所定周期の移動指令量とステップ２０２で読み取った機械位置フィードバック量とにより位置偏差を求め、該位置偏差にポジションゲインＫｐを乗じて基づいてフルクローズドの位置ループ処理を行い速度指令Ｖｃｆを求める（ステップ２０６）。
【００４１】
求めた速度指令Ｖｃｆから、ステップ２０４で求めた、速度オフセット量Ｖｏｆと、ステップ２０５での位置のセミクローズド・ループ処理（モータ位置のフィードバック量による位置ループ処理）で求めた速度指令Ｖｃｓを減じたものに対してローパスフィルタの処理を行う（ステップ２０７）。そして、このローパスフィルタ処理結果と速度オフセット量Ｖｏｆとセミクローズド・ループ処理で求めた速度指令Ｖｃｓを加算して速度指令Ｖｃを求める（ステップ２０８）。
【００４２】
この速度指令に基づいて速度ループ処理してトルク指令Ｔｃを求め、さらに速度オフセット量からトルクオフセット量Ｔｏｆを求めて軽く指令Ｔｃに加算して電流ループ処理経出力するステップ２０９〜２１２までの処理は、第１の実施形態の図７に示すフローチャートのステップ１０６〜１０９と同一である。
【００４３】
図９は、図５に示す第３の実施形態におけるディジタルサーボ回路５３のプロセッサが所定周期毎実施する処理のフローチャートである。
数値制御装置５１から出力される移動指令より所定周期毎の移動指令（速度指令）を求めると共に、モータ位置、機械位置のフィードバック量を読み取り、移動指令（速度指令）を微分して（前周期の移動指令との差より求め）、指令加速度を求める（ステップ３００〜３０３）。この点は第２の実施形態のステップ２００〜２０３と同一である。次に、速度指令（所定周期間の移動指令量）に補正係数α１を乗じた補正値Ｑ１と指令加速度に補正係数α２を乗じた補正値Ｑ２を算出する（ステップ３０４）。
【００４４】
さらに、移動指令量とモータ位置のフィードバック量より位置偏差を求め、その位置偏差にポジションゲインＫｐを乗じる位置のセミクローズド・ループ処理を行い速度指令Ｖｃｓを求める（ステップ３０５）。また、同様に、移動指令量と機械位置のフィードバック量より位置偏差を求め、その位置偏差にポジションゲインＫｐを乗じる位置のフルクローズド・ループ処理を行い速度指令Ｖｃｆを求める（ステップ３０６）。
【００４５】
ステップ３０４で求めた補正量Ｑ１，Ｑ２を加算して推定ねじれ量に相当する値（Ｑ１＋Ｑ２）を求め、かつ、ステップ３０６で求めたフルクローズド・ループ処理による速度指令Ｖｃｆからステップ３０５で求めたセミクローズド・ループ処理による速度指令Ｖｃｓを減じて、実際のねじれ量に相当する値（Ｖｃｆ−Ｖｃｓ）を求め、推定ねじれ量に相当する値（Ｑ１＋Ｑ２）から実際のねじれ量に相当する値（Ｖｃｆ−Ｖｃｓ）を減じてねじれ量偏差に対応する値（Ｑ１＋Ｑ２−（Ｖｃｆ−Ｖｃｓ））を求める。このねじれ量偏差に対応する値に対してハイパスフィルタ処理を行い速度オフセット量Ｖｏｆを求める（ステップ３０７）。
【００４６】
さらに、実ねじれ量に相当する値（Ｖｃｆ−Ｖｃｓ）に対しローパスフィルタの処理を行い（ステップ３０８）、このローパスフィルタ処理後の値とセミクローズド・ループ処理を行い速度指令Ｖｃｓ及び速度オフセット量Ｖｏｆを加算して速度指令Ｖｃを求める（ステップ３０９）。
【００４７】
速度指令Ｖｃを求めた後の処理は、第１、第２の実施形態のステップ１０６〜１０９またはステップ２０９〜２１２の処理と同一であり、速度オフセット量Ｖｏｆからトルクオフセット量Ｔｏｆを求め、速度指令Ｖｃに基づいて速度ループ処理して求めたトルク指令Ｔｃにトルクオフセット量Ｔｏｆに加算して電流ループ処理に出力する（ステップ３１０〜３１３）。
【００４８】
上述した各実施形態では、トルクオフセット量Ｔｏｆを速度オフセット量の変化量、すなわち速度オフセット量の微分値に所定補正係数βを乗じて求めた。この関係はグラフで示すと図１０（ａ）に示す関係となる。しかし、図１０（ｂ）に示すように、速度オフセット量の微分値とトルクオフセット量の関係を折れ線で定義し、速度オフセット量の大きさの範囲に応じて直線式を定義して記憶しておき、速度オフセット量の大きさによってトルクオフセット量をこの定義した直線式から求めるようにしてもよい。さらには、図１０（ｃ）に示すように、速度オフセット量の微分値とトルクオフセット量との対応関係をテーブルで記憶しておき、このテーブルに基づいて速度オフセット量からトルクオフセット量を求めるようにしてもよい。
【００４９】
なお、上述した各実施形態では、速度オフセット量を指令加速度に比例するオフセット量と指令速度に比例するオフセット量を加算して求めたが、指令速度に比例する摩擦に対応したオフセット量は無視し、指令加速度に比例したオフセット量のみで速度オフセット量を構成してもよい。
さらに、速度オフセット量の補正を行わずトルクオフセット量の補正のみでも、ある程度目的を達成することもできる。すなわち、位置指令を３階微分して指令の加加速度を求め、この指令加加速度に係数を乗じてトルクオフセット量を求め、速度ループ制御によって出力されるトルク指令にこのトルクオフセット量を加算してトルク指令を補正し、この補正されたトルク指令でサーボモータを駆動するようにすればよい。
【００５０】
そこで、本発明の効果を判断するために図１１に示すような構成で実験を行った。図１１において、サーボモータ３にねじれ要素６１を介して所定イナーシャを有する部材６０を取り付け、かつこの部材６０の回転位置を検出する位置検出器５６を取り付け、かつサーボモータ３のロータ軸にもその回転位置、速度を検出する位置・速度検出器５５を取り付け、位置検出器５６及び位置・速度検出器５５の出力を制御装置５０にフードバックするように構成した。
【００５１】
この実験装置で、まず、従来として、速度オフセット補正、トルクオフセット補正を行わないセミクローズド・ループによる位置、速度ループ制御により、速度４０００での移動指令を与えて、加減速したときの指令位置と機械位置との位置偏差を測定したものが図１２（ａ）で示すグラフである。加速時、減速時に位置偏差が増大し大きな振動が発生している。位置偏差の最大は３０μｍであった。
一方、図４に示す第２の実施形態の制御方式に制御系を変え、この実験装置を駆動し、速度４０００での移動指令を与えて、加減速したときの指令位置と機械位置との位置偏差を測定した。その結果が図１２（ｂ）で示すグラフである。この図１２（ｂ）に示されるように加減速時においても振動は発生せず、位置偏差も最大で１０μｍとなり、大きく改善されていることが分かる。
【００５２】
【発明の効果】
大型の機械等におけるモータと機械可動部間にねじれ要素等が存在する低剛性の機械可動部の位置を制御するとき、この低剛性によって生じる応答の遅れ、及びこの遅れによって生じる振動を、本発明は抑制し、遅れが少なく、振動を低減させることができる。
【図面の簡単な説明】
【図１】低剛性の機械をバネ結合したモデルのモデルの説明図である。
【図２】本発明の第１の実施形態のモータ制御系のブロック図である。
【図３】同実施形態における速度オフセット量、トルクオフセット量の説明図である。
【図４】本発明の第２の実施形態のモータ制御系のブロック図である。
【図５】本発明の第３の実施形態のモータ制御系のブロック図である。
【図６】本発明の各実施形態を実施する制御装置の要部とモータ、機械との結合を表すブロック図である。
【図７】同第１の実施形態の制御を行うときのディジタルサーボ回路のプロセッサが所定周期毎実行する処理のフローチャートである。
【図８】同第２の実施形態の制御を行うときのディジタルサーボ回路のプロセッサが所定周期毎実行する処理のフローチャートである。
【図９】同第３の実施形態の制御を行うときのディジタルサーボ回路のプロセッサが所定周期毎実行する処理のフローチャートである。
【図１０】各実施形態において、速度オフセット量の微分値とトルクオフセット量の関係を定義する例の説明図である。
【図１１】本発明の効果を判断するために行った実験装置の構成の説明図である。
【図１２】実験結果をグラフで表した図である。
【符号の説明】
８  バネ要素
９  摩擦要素
【図１】
2004213472.tif 000005
【図２】
2004213472.tif 000006
【図３】
2004213472.tif 000007
【図４】
2004213472.tif 000008
【図５】
2004213472.tif 000009
【図６】
2004213472.tif 000010
【図７】
2004213472.tif 000011
【図８】
2004213472.tif 000012
【図９】
2004213472.tif 000013
【図１０】
2004213472.tif 000014
【図１１】
2004213472.tif 000015
【図１２】
2004213472.tif 000016