JPH06289938A - サーボ送りにおける障害物検出装置 - Google Patents

Abstract

(57)【要約】 【目的】早期に障害物への衝突を検出することで、物体
の破損を防止すること。 【構成】現制御周期における、角度検出手段により検出
された回転角と、回転角の予測値と、回転角速度の予測
値と、外乱トルクの予測値と、指令された目標電流とか
ら、ディジィタルサーボ制御系に対する同一次元オブザ
ーバによる演算により(302) 、次の制御周期における外
乱トルクを予測し、その予測された外乱トルクを所定値
と比較して、外乱トルクが所定値以上となった時に、サ
ーボモータによる移動物体が障害物に衝突したと判定す
る(306-308) 。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は交流サーボモータをディ
ジィタル制御するためのディジィタルサーボ制御装置に
関し、特に、障害物への衝突を検出する装置に関する。

【０００２】

【従来技術】従来、ディジィタルサーボ制御装置は、位
置フィードバックループ、速度フィードバックループ、
電流フィードバックループを有している。このディジィ
タルサーボ制御装置は、ロボットのアーム駆動軸やＮＣ
工作機械等の送り軸の回転制御に用いられている。この
ような装置において、作業者の意図に反して移動体が障
害物に衝突したような場合には、直ちに、衝突を検出し
てサーボモータの回転を停止させることが必要である。
この障害物への衝突検出のために、サーボモータの負荷
電流を検出して、その負荷電流が所定値を越えた時に障
害物への衝突と判定している。

【０００３】

【発明が解決しようとする課題】したがって、サーボモ
ータの負荷電流の検出のための検出用ヘッド、増幅器等
の設備が必要となる。又、サーボモータの回転が障害物
によって強制的に停止されることにより、現実に負荷電
流が増大して、その結果、初めて、障害物への衝突と検
出されることから検出の時間遅れが生じる。その結果、
ロボットアーム、ＮＣ工作機械の工具、テーブル、障害
物等を破損する恐れがある。

【０００４】本発明は上記課題を解決するために成され
たものであり、その目的は、サーボ送りにおいて、特別
な設備なしに、早期に障害物への衝突を検出すること
で、物体の破損を防止することである。

【０００５】

【課題を解決するための手段】上記課題を解決するため
の発明の構成は、各フィードバックループに対する目標
値をディジィタル量で与えるサーボ送りにおける障害物
検出装置において、現制御周期における、角度検出手段
により検出された回転角と、回転角の予測値と、回転角
速度の予測値と、外乱トルクの予測値と、指令された目
標電流とから、ディジィタルサーボ制御系に対する同一
次元オブザーバによる演算により、次の制御周期におけ
る外乱トルクを予測し、その予測された外乱トルクを所
定値と比較して、外乱トルクが所定値以上となった時
に、サーボモータによる移動物体が障害物に衝突したと
判定することを特徴とする。

【０００６】

【作用】現制御周期における、角度検出手段により検出
された回転角と、回転角の予測値と、回転角速度の予測
値と、外乱トルクの予測値と、指令された目標電流とか
ら、ディジィタルサーボ制御系に対する同一次元オブザ
ーバによる演算により、次の制御周期における外乱トル
クが予測される。そして、次の制御周期における予測さ
れた外乱トルクは、所定値と比較され、外乱トルクが所
定値以上となった時に、サーボモータによる移動物体が
障害物に衝突したと判定される。

【０００７】従って、サーボモータの負荷電流が現実に
増大する前に、外乱トルクの増大が早期に検出される。

【０００８】

【発明の効果】本発明は、上記の如く、ディジィタルサ
ーボ制御系に対する同一次元オブザーバを用いた予測演
算により、次の制御周期に発生する外乱トルクを予測
し、その大きさを判定することで、検出ヘッド等の特別
な設備を必要とすることなく障害物へ衝突したか否かを
判定することができる。よって、障害物への衝突が早期
に検出され、移動体や障害物の破損を防止することがで
きる。

【０００９】

【実施例】以下、本発明を具体的な実施例に基づいて説
明する。図１は本発明に係るディジィタルサーボ制御装
置の構成を示したブロックダイヤグラムである。ディジ
ィタルサーボ制御装置１０は主として、ＣＰＵ１１、Ｒ
ＯＭ１２、ＲＡＭ１３、ディジィタルシグナルプロセッ
サ（以下「ＤＳＰ」という）１４、共通ＲＡＭ１７，Ｃ
ＰＵ２３、Ａ／Ｄ変換器１５ａ，１５ｂ，ＲＯＭ２０，
ＲＡＭ２４及び現在値カウンタ１６から構成されてい
る。ＣＰＵ１１にはインタフェース１９を介してキーボ
ード２１及びＣＲＴ表示装置２２が接続されている。

【００１０】ＤＳＰ１４は位置及び速度制御用の演算を
行い、ＤＳＰ１４の出力は電流制御用のＣＰＵ２３に入
力し、ＣＰＵ２３の出力はインバータ２５に入力されて
いる。そのインバータ２５はＣＰＵ２３の出力信号に応
じてサーボモータ３１を駆動する。サーボモータ３１に
は同期モータが用いられ、インバータ２５のＰＷＭ電圧
制御によりサーボモータ３１の負荷電流が制御され、そ
の結果、出力トルクが制御される。

【００１１】サーボモータ３１のu 相及びv 相の負荷電
流はＣＴ３２ａ，３２ｂにより検出され、増幅器１８
ａ，１８ｂにより増幅される。その増幅器１８ａ，１８
ｂの出力は、Ａ／Ｄ変換器１５ａ，１５ｂに入力され、
所定の周期でサンプリングされ、ディジィタル値に変換
される。そのサンプリングされた値は、瞬時負荷電流の
フィードバック値として、ＣＰＵ２３に入力する。又、
サーボモータ３１にはパルスエンコーダ３３が接続さ
れ、その現在位置（現在回転角）が検出される。パルス
エンコーダ３３の出力は波形成形・方向判別回路３４を
介して現在値カウンタ１６に接続されている。

【００１２】波形成形・方向判別回路３４を介して現在
値カウンタ１６に入力されるパルスエンコーダ３３から
の出力信号は現在値カウンタ１６の値を加減させる。Ｄ
ＳＰ１４により、現在値カウンタ１６の値は現在位置フ
ィードバック値として読み込まれ、ＤＳＰ１４により、
ＣＰＵ１１から出力された目標値（目標回転角）と比較
され位置偏差（角度偏差）が算出される。そして、ＤＳ
Ｐ１４により、その位置偏差に基づいて速度目標値（回
転角速度目標値）が算出される。

【００１３】又、ＤＳＰ１４に入力された現在位置フィ
ードバック値は微分され、速度フィードバック値が算出
される。ＤＳＰ１４により、位置偏差に応じて決定され
る速度目標値と速度フィードバック値とが比較され速度
偏差が算出され、その速度偏差に基づいて電流目標値が
算出される。

【００１４】一方、ＣＴ３２ａ，３２ｂにて検出された
負荷電流は、増幅器１８ａ，１８ｂ及びＡ／Ｄ変換器１
５ａ，１５ｂを介してＣＰＵ２３に入力する。そして、
後で詳しく説明するように、電流制御周期において検出
された回転角及び回転角速度、及び電流制御周期におい
て指令された目標トルク関連値に基づいて、次の電流制
御周期における外乱トルクが予測演算される。目標トル
ク関連値としては、最も具体的には、目標電流である。

【００１５】そして、ＣＰＵ２３により、電流目標値と
予測演算された電流フィードバック値と比較され、電流
偏差が算出される。その時の瞬時電流偏差と瞬時電流偏
差の累積値と電流目標値とに基づいて、即ち、比例積分
演算により、その電流制御時刻における瞬時電流指令値
が演算される。その瞬時電流指令値は高周波数の三角波
と比較され、インバータ２５の各相のトランジスタのオ
ンオフを制御する電圧制御ＰＷＭ信号が生成される。

【００１６】その電圧制御ＰＷＭ信号は、インバータ２
５に出力され、そのインバータ２５の各相のトランジス
タがそれぞれ駆動される。このインバータ２５のスイッ
チングにより、各相の負荷電流は電流目標値に制御され
ることになる。尚、サーボモータ３１の位置決めは、Ｃ
ＰＵ１１により、現在値カンウタ１６の出力値が位置の
目標値に等しくなったと判定された時に完了される。
又、Ａ／Ｄ変換器１５ａ，１５ｂによってサンプリング
されたu 相,v相の負荷電流値は、ＣＰＵ２３によりdq変
換される。

【００１７】本実施例のディジィタルサーボ制御装置
は、上述したように、位置、速度及び電流の３つのフィ
ードバックループにより構成されている。より下位のフ
ィードバックループ程、より高い応答性が要求され、例
えば、最下位の電流フィードバックループは 100μs 、
速度フィードバックループはその数倍、位置フィードバ
ックループは更にその数倍の時間間隔で同期をとってデ
ータのサンプリングが実行され、それぞれのフィードバ
ックループの処理が実行される。

【００１８】次に、本実施例装置の作動について説明す
る。図２はＲＯＭ１２に記憶されたＣＰＵ１１によって
実行されるプログラムを示したフローチャートである。
このプログラムが実行される前の状態では、サーボモー
タ３１は停止状態にある。

【００１９】ステップ２００において、ＲＡＭ１３から
１ブロックの移動指令データが読み込まれ、ステップ２
０２において、ＲＡＭ１３のフラグ領域における加減速
フラグがオンとされる。次に、ステップ２０４において
サーボモータ３１を停止状態から指令された定速度まで
に加速するための加速領域における補間目標位置（補間
目標回転角）が演算され、その補間目標位置は時々刻々
共通ＲＡＭ１７に出力されそこに記憶される。

【００２０】次に、加速が終了すると、ステップ２０６
において加減速フラグがオフとされ、次のステップ２０
８においてＲＡＭ１３のフラグ領域における定速フラグ
がオンとされる。次に、ステップ２１０において定速領
域における補間目標位置が演算され、その補間目標位置
は時々刻々共通ＲＡＭ１７に出力されそこに記憶され
る。

【００２１】次に、ステップ２１２において１ブロック
の移動指令データに指令目標位置で一旦停止する指令が
含まれているか否かが判定される。停止指令が付与され
ていない場合にはステップ２１４において、次のブロッ
クの移動指令データが入力され、ステップ２１０で定速
領域における補間目標位置が演算される。ステップ２１
０、２１４の繰り返しにより、定速で目標位置を順次更
新させ、補間目標位置を順次出力させることができる。

【００２２】ステップ２１２において移動指令データに
一旦停止指令が含まれていると判定された場合には、ス
テップ２１６において加減速フラグがオンされ、次のス
テップ２１８において定速フラグがオフされる。そし
て、ステップ２２０において減速領域の補間目標位置が
順次演算され、その補間目標位置は時々刻々共通ＲＡＭ
１７に出力され、そこに記憶される。

【００２３】次に、減速補間が完了した後は、ステップ
２２２においてサーボロック状態であることを示すため
にＲＡＭ１３のフラグ領域におけるサーボロックフラグ
がオンとされる。そして、ステップ２２４において同一
の目標位置が時々刻々共通ＲＡＭ１７に出力され、その
目標位置はそこに記憶される。その結果、サーボモータ
３１はサーボロック状態、即ち、通電状態で同一位置に
保持される。

【００２４】移動指令データにより指令された時間だけ
の一旦停止が完了した後は、ステップ２００に戻り、次
のブロックの移動指令データが読み込まれ、上述のステ
ップと同様にサーボモータの位置制御が行われる。この
ようにして、サーボモータの位置が指令される。

【００２５】次に、ＤＳＰ１４はＲＯＭ２０に記憶され
た図３のプログラムを実行し、ＣＰＵ２３はＲＯＭ２３
に記憶された図４に示すプログラムを実行して、サーボ
モータ３１の位置、速度、トルク制御を行う。図３、図
４のプログラムは、ＤＳＰ１４及びＣＰＵ２３によっ
て、所定の最小周期毎に繰り返し実行される。

【００２６】ステップ１００では、現実行サイクルが位
置偏差演算タイミングか否かが判定され、位置偏差演算
タンミングであればステップ１０２に移行し、共通ＲＡ
ＭからＣＰＵ１１によりその時刻で指令された目標位置
θ(i)(補間目標角度) が入力され記憶される。又、過去
一定時間内の目標位置は共通ＲＡＭ１７に保存されてい
る。

【００２７】次に、ステップ１０４において現在値カウ
ンタ１６に保持された現在位置（電気角）θa(i)が読み
込まれる。次に、ステップ１０６において、現時刻(i)
の目標位置θ(i) と現在位置θa(i)との位置偏差Δθ
(i) が演算される。次に、ステップ１０８において目標
速度Ｖ(i) が位置偏差ΔＰ(i) に比例した値、即ち、次
式により演算される。

【数１】Ｖ(i) ＝ｋ・ΔＰ(i)

【００２８】以上の位置のフィードバック制御は、図７
の信号S1で示したタイミングで実行される。

【００２９】次に、ステップ１１０において、現実行サ
イクルが速度偏差演算タイミングか否かが判定される。
速度偏差演算タンミングであればステップ１１２に移行
し、現在値カウンタ１６に保持された現在位置θa(n)が
読み込まれる。次にステップ１１４に移行して、現時刻
(n) における現在速度Ｖa(n)が演算される。現在速度Ｖ
a(n)は、前回の速度偏差演算タイミング時に読み込まれ
た現在位置θa(n-1)と、今回入力された現在位置θa(n)
と、速度制御周期Ｄとに基づいて次式によって演算され
る。

【数２】Ｖa(n)=(θa(n)- θa(n-1)) ／Ｄ

【００３０】次に、ステップ１１６において、ステップ
１０８で演算された目標速度Ｖ(i)と現在速度Ｖa(n)と
の偏差、即ち、速度偏差ΔＶ(n) が演算される。又、速
度偏差ΔＶ(n) の累積値（積分）ＳがＳ＝Ｓ＋ΔＶ(n)
により演算される。次に、ステップ１１８においてステ
ップ１１６で演算された速度偏差ΔＶ(n)及び速度偏差
の積分Ｓと、共通ＲＡＭ１７に設定されている比例利得
Ｋ_p 及び目標速度Ｖ(i) に対応して設定されている積分
時間Ｔ_i とを用いて、目標電流のｑ軸成分（有効電流で
サーボモータのトクルに比例する）Ｉq(n)が次式により
演算される。尚、目標電流のｄ軸成分（無効電流）は０
である。

【数３】Ｉq(n)=Ｋ_p(ΔＶ(i)+Ｓ／Ｔ_i )

【００３１】次に、ステップ１１９において、同一次元
オブザーバにより第ｎ制御周期における外乱トルクＴ_L
(n)'が演算される。同一次元オブザーバによる予測演算
は、図６に示すプログラムに従って実行される。予測演
算を行うに先立ち、サーボモータ３１及びパルスエンコ
ーダ３３は図８のようにモデル化できる。即ち、外乱ト
ルクＴ_L , 目標電流Ｉq,サーボモータ３１の慣性モーメ
ントＪ_m , 負荷の慣性モーメントＪ_L , 回転角速度ω及
び回転角θの間には次の関係が成り立つ。

【数４】Ｋ_tn・Ｉq −Ｔ_L ＝（Ｊ_m ＋Ｊ_L ) Ｓ² θ ただし、Ｋ_tnはトルク定数である。上記した式を基に図
８を作成し、状態方程式を導出すると、

【数５】 ただし、Ｊ＝Ｊ_m ＋Ｊ_L この式を離散化し、同一次元オブザーバを構成した式
（下記数７式）によって予測演算は行われるのである。
図６のステップ３００において、前回、即ち、第ｎ−１
制御周期における誤差が次式で演算される。

【数６】ｅ(n-1)=θ(n-1)'−θa(n-1) 但し、ｅ(n-1) 、θ(n-1)'、θa(n-1)は、それぞれ、第
ｎ−１制御周期における位置（回転角）の予測誤差、位
置（回転角）の予測演算値、回転角の実測値である。こ
こで、θ(n-1)'の初期値θ(1)'は特定する必要はない
が、通常は実測値θa(1)を用いる。

【００３２】次に、ステップ３０２において、上記数５
式を離散化した数７式により、第ｎ−１制御周期におけ
る目標電流Ｉq(n-1)と、回転角の予測値θ(n-1)'、その
時間微分である回転角速度の予測値ω(n-1)'、外乱トル
クの予測値Ｔ_L (n-1)'及び誤差ｅ(n-1) とから、第ｎ制
御周期における回転角の予測値θ(n)'、その時間微分で
ある回転角速度の予測値ω(n)'、外乱トルクの予測値Ｔ
_L (n)'が演算される。

【００３３】

【数７】

【００３４】但し、ｔは第ｎ−１制御周期と第ｎ制御周
期との間の時間間隔、 Jはサーボモータのロータの慣性
モーメント J_m と負荷の慣性モーメント J_L との和、 K
_tnはトルク定数である。

【００３５】次に、ステップ３０４において、外乱トル
クの予測値Ｔ_L (n)'に対応する換算電流Ｉ_L (n) がＴ_L
(n)'/K_tnにより演算される。次に、ステップ３０６にお
いて、換算電流Ｉ_L (n) が所定値Ｉ_th以上か否かが判定
される。換算電流Ｉ_L (n) が所定値Ｉ_th以上の場合に
は、外乱トルクの予測値Ｔ_L (n)'が 所定値K _tn×Ｉ_th
以上であることを意味しており、サーボモータ３１によ
って移動される物体が固定障害物に衝突したと判定され
る。よって、この場合には、ステップ３０８において、
サーボロックフラグがオンとされ、直ちに、所定のサー
ボロック状態の指令がＣＰＵ２３及びＣＰＵ１１に出力
される。即ち、ステップ３０８において、目標電流Ｉq
(n)＝０、目標速度Ｖ(i) ＝０、目標位置θ(i) ＝一定
の指令が電流フィードバックループ、速度フィードバッ
クループ、位置フィードバックループに対して与えられ
る。一方、換算電流Ｉ_L (n) が所定値Ｉ_thより小さい場
合には、障害物への衝突ではないので、ステップ１１８
において演算された目標電流Ｉq(n)、Ｉd(n)が、電流フ
ィードバックループにおける電流の制御目標となる。こ
のように、ＤＳＰ１４は速度制御周期で繰り返し速度制
御を実行する。この速度フィードバック制御は、図７の
信号S2で示したタイミングで実行される。

【００３６】電流制御用ＣＰＵ２３は、図４に示すプロ
グラムを実行する。ステップ１２０において、現実行サ
イクルが電流偏差演算タイミングか否かが判定される。
電流偏差演算タイミングであれば、ステップ１２２に移
行する。ステップ１２２以下は電流フィードバック制御
であり、この制御は、図７の信号S3に示したタイミング
で実行される。ステップ１２２では、電流制御期間の先
頭から測定した電流検出時刻Δｔ₁ 、電流制御期間の先
頭から測定した負荷電流の制御時刻Δｔ₂ と現在速度Ｖ
a(n)とを用いて、その時刻に対応した電気角である電流
検出時電気角θ₁ と制御時電気角θ₂ が補間演算され
る。

【００３７】

【数８】θ₁ ＝θa(n)+Ｖa(n)・Δｔ₁

【数９】θ₂ ＝θa(n)+Ｖa(n)・Δｔ₂

【００３８】この時刻Δｔ₁ ，Δｔ₂ と電気角θ₁ , θ
₂ とは図７に示したように対応している。次にステップ
１２４に移行して、ｕ相、ｖ相の負荷電流の現在値、即
ち、現在電流Ｉu,Ｉv がＡ／Ｄ変換器１５ａ，１５ｂか
ら読み込まれる。次に、ステップ１２６において、その
現在電流Ｉu,Ｉv はdq変換されて、ｄ軸成分Ｉadとｑ軸
成分Ｉaqとが次式によって演算される。

【数１０】Ｉad=2^1/2 ｛lusin(θ₁+2π/3)-Ivsinθ₁｝

【数１１】Ｉaq=2^1/2 ｛Iucos(θ₁+2π/3)-Ivcosθ₁｝

【００３９】尚、dq座標系は、良く知られたように、ｄ
軸は励磁磁場と同相にとられ、ｑ軸は励磁磁場と電気角
で90°の位相差にとられた座標系である。ｄ軸成分は無
効成分をｑ軸成分は有効成分を表す。次に、ステップ１
２８において、ステップ１１８で演算された目標電流Ｉ
q(n)、目標電流Ｉd(n)＝０と、ステップ１２６で求めら
れた現在電流のｄ軸成分Ｉad、ｑ軸成分Ｉaqとの偏差、
即ち、ｄ軸成分偏差、ｑ軸成分偏差が求められる。そし
て、そのｄ軸成分偏差、ｑ軸成分偏差に基づいて、比例
・積分演算により指令電流のｄ軸成分Ｉd(j)^# 、ｑ軸成
分Ｉq(j)^# が演算される。

【００４０】次に、ステップ１３０において、次式によ
り、指令電流のｄ軸成分、ｑ軸成分Ｉd(j)^# , Ｉq(j)^#
を逆dq変換して、各相電流指令値Ｉu(j)^# , Ｉv(j)^# ,
Ｉw(j)^# が演算される。

【数１２】Ｉu(j)^# =(2/3)^1/2 ・｛Ｉd(j)^#cosθ₂ −Ｉ
q(j)^#sinθ₂｝

【数１３】Ｉv(j)^#=(2/3)^1/2・｛Ｉd(j)^#cos(θ₂＋2π/
3)−Ｉq(j)^#sin(θ₂+2π/3)｝ 尚、Ｉw(j)^# は、Ｉw(j)^# =-( Ｉu(j)^# + Ｉv(j)^# ) に
よって演算される。

【００４１】次に、ステップ１３２，１３４において、
各相電流指令値Ｉu(j)^# , Ｉv(j)^#, Ｉw(j)^# と高周波
数の三角波とのレベル関係を利用して、即ち、平均電圧
法を用いて、１つの制御周期内における一連のＰＷＭ信
号が生成される。一連のＰＷＭ信号は、各相の電圧印加
状態を示した電圧ベクトルで表すことができる。回転磁
界ベクトルは、この電圧ベクトルの積分として表され
る。従って、各電圧ベクトル×継続時間の和によって回
転磁界ベクトルの先端の軌跡が描かれる。回転磁界を角
度２π／ｎ毎に円周上の点に最短経路で位置決めするた
めには、１制御周期毎に、隣接する２つの電圧ベクトル
と零ベクトルＶ₀ の３つのベクトルでインバータ２５が
制御される必要がある。この３つの電圧ベクトルの組合
せと回転磁界の位相とは一意的に対応する。回転磁界の
位相と電圧ベクトルの組合せの対応表（零ベクトルＶ₀
は必ず組合せの１要素となるので、２つの電圧ベクトル
の組だけで良い）が、予めＲＯＭ２４に記憶されてい
る。

【００４２】ステップ１３２では、制御時電気角θ
₂ （回転磁界の位相) から、ＲＯＭ２４におけるテーブ
ルを検索してその時の電圧ベクトルの組合せを求める。
ステップ１３４では、各電圧ベクトルの継続時間ｔ_1,ｔ
_2,ｔ₃ が演算される。例えば、その電圧ベクトルの組合
せが、Ｖ_n =(1,1,0), Ｖ₁ =(1,0,0), Ｖ₂ =(0,0,0)とな
ったとして、各電圧ベクトルの継続時間ｔ_1,ｔ_2,ｔ₃ が
演算される。その演算方法は、本実施例では、良く知ら
れた平均電圧法が用いられている。

【００４３】即ち、各相電流指令値Ｉu(j)^# , Ｉv
(j)^# , Ｉw(j)^# のうち、絶対値の大きい２つを大きい
順にI₁ ^*, I₂ ^*とするとき、継続時間ｔ_1,ｔ_2,ｔ₃ は次式
で求められる。

【数１４】ｔ₁ ＝｜２I₂ ^*＋I₁ ^*｜・Ｔ／Ｖdc

【数１５】ｔ₂ ＝｜I₁ ^*−I₂ ^*｜・Ｔ／Ｖdc

【数１６】ｔ₃ ＝Ｔ−（ｔ₁₊ｔ₂ ） 但し、Ｔは周期、Ｖdcは印加直流電圧である。

【００４４】次に、ステップ１３６において、１組の電
圧ベクトルによるＰＷＭ信号が、継続時間ｔ_1,ｔ_2,ｔ₃
だけ出力される。例えば、Ｖ₆ =(1,1,0)，Ｖ₁ =(1,0,
0)，Ｖ₀ =(0,0,0)の順に、継続時間ｔ_1,ｔ_2,ｔ₃ だけ出
力される。又、換言すれば、U相はｔ₁ ＋ｔ₂ だけ電圧
が印加され、V 相はｔ₁ だけ電圧が印加され、W 相には
その制御期間、電圧は印加されない。

【００４５】ＤＳＰ１４及びＣＰＵ２３の各実行サイク
ルは、最小の制御周期で実行されており、その整数倍ｎ
₁ で電流フィードバックループが制御され、その整数倍
ｎ₂で速度フィードバックループが制御され、その整数
倍ｎ₃ で位置フィードバックループが制御されるよう
に、ステップ１００，１１０，１２０で判定の基準とな
る回数が設定されている。但し、ｎ₁ ＜ｎ₂ ≦ｎ₃ であ
る。上記のサイクルが繰り返し実行されることで、図７
に示したタイミングで、位置、速度、電流のフィードバ
ック制御が行われる。但し、図７に示したタイミングは
ＣＰＵ１１によるプログラム実行時からの計時によって
検出される。

【００４６】上記のようなサーボ制御により、停止、加
速、定速度、減速、停止による位置決め動作が実行され
る。上記の位置、速度、電流のフィードバックループを
伝達関数で表示すると、図５に示すようになる。即ち、
速度フィードバックループにおいて、速度偏差の比例・
積分演算の結果得られる目標電流Iq(n) が電流フィード
バックループに対する目標電流となる。それと共に、目
標電流Iq(n-1) は、回転角θ(n-1) と共に、同一次元オ
ブザーバに対する入力変数となる。そして、同一次元オ
ブザーバにより予測演算された外乱トルクＴ_L (n)'に基
づいて、サーボモータ３３により移動される移動体の障
害物への衝突か否かが判定される。尚、上記実施例で
は、同一次元オブザーバによる演算において、指令され
た目標電流Iq(n) には、ローパスフィルタＡにより高周
波成分が除去された目標電流Iqが用いられている。

【図面の簡単な説明】

【図１】本発明の具体的な一実施例に係るディジィタル
サーボ制御装置の構成を示したブロックダイヤグラム。

【図２】同実施例装置のＣＰＵ１１によって処理される
目標位置の指令手順を示したフローチャート。

【図３】同実施例装置で使用されているＤＳＰによる処
理手順を示したフローチャート。

【図４】同実施例装置で使用されているＤＳＰによる処
理手順を示したフローチャート。

【図５】同実施例装置で使用されている位置、速度、電
流のフィードバックループにおける伝達関数を示したブ
ロック図。

【図６】同一次元オブザーバにより外乱トルクを予測
し、障害物衝突検出を判定する手順を示したフローチャ
ート。

【図７】位置、速度、電流フィードバック制御のタイミ
ングを示したタイミングチャート。

【図８】制御系の伝達関数を示したブロック図。

【符号の説明】

１０…ディジィタルサーボ制御装置 １１…ＣＰＵ １２…ＲＯＭ １３…ＲＡＭ １４…ＤＳＰ（ディジィタルシグナルプロセッサ）（角
度検出手段、外乱トルク予測手段、衝突判定手段） １５ａ，１５ｂ…Ａ／Ｄ変換器 １６…現在値カウンタ（角度検出手段） １７…共通ＲＡＭ ２０…ＲＯＭ（角度検出手段、外乱トルク予測手段、衝
突判定手段） ２５…インバータ ３１…サーボモータ ３２ａ,３２ｂ…カレントトランスフォーマ(ＣＴ) ３３…パルスエンコーダ ステップ１０４…角度検出手段 ステップ３００〜３０４…外乱トルク予測手段 ステップ３０６〜３０８…衝突判定手段

───────────────────────────────────────────────────── フロントページの続き (72)発明者 神谷 新吾 愛知県刈谷市朝日町１丁目１番地 豊田工 機株式会社内

Claims (1)

【特許請求の範囲】
1. 【請求項１】位置フィードバックループ、速度フィード
   バックループ、電流フィードバックループを有し、各フ
   ィードバックループに対する目標値をディジィタル量で
   与えるサーボ送りにおける障害物検出装置において、 前記速度フィードバックループから速度偏差に応じて出
   力され、前記電流フィードバックループに対して、順
   次、目標電流を指令する目標電流指令手段と、 各サンプリング時刻における前記サーボモータの回転角
   を検出する角度検出手段と、 現制御周期における、前記角度検出手段により検出され
   た前記回転角と、回転角の予測値と、回転角速度の予測
   値と、外乱トルクの予測値と、指令された前記目標電流
   とから、ディジィタルサーボ制御系に対する同一次元オ
   ブザーバによる演算により、次の制御周期における外乱
   トルクを予測する外乱トルク予測手段と、 次の制御周期における予測された外乱トルクを所定値と
   比較して、外乱トルクが所定値以上となった時に、サー
   ボモータによる移動物体が障害物に衝突したと判定する
   衝突判定手段とを有するサーボ送りにおける障害物検出
   装置。