JPH0242502A - Digital control system for robot - Google Patents

Abstract

PURPOSE:To always keep all action processes of a robot under the optimum conditions by deciding in real time both spring and viscosity constants of a servo system from the decided frequency and gain value and controlling the actions of each movable part based on said spring and viscosity constants. CONSTITUTION:A robot control subject 16 which is driven by each actuator motor 14 is changed into a robot model which uses the moment of inertia and the spring and viscosity constants as parameters. Then a cubic transmission function is decided for a servo system including said robot model. Furthermore the frequency and the gain corresponding to the resonance point or an anti- resonance point of the cubic transmission function set on a board diagram are decided from the sample value of the on-line relation between the torque and the speed of the robot model. Then both spring and viscosity constants of the servo system are decided in real time from the frequency and gain. Thus the actions of each movable part are controlled based on the spring and viscosity constants.

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、産業用ロボットの制御方式に関し、特に、制
御対象のロボット可動部をＡＣサーボモータ等の電動サ
ーボモータをアクチュエータとしてディジタル制御方式
でフィードバック制御を行うときに、各ロボット可動部
の動作制御をオン・ラインで最適化する適応制御機能を
有したロボット制御方式に関するものである。[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a control method for an industrial robot, and in particular, the present invention relates to a control method for an industrial robot, and in particular, a method in which a movable part of a robot to be controlled is digitally controlled using an electric servo motor such as an AC servo motor as an actuator. The present invention relates to a robot control system that has an adaptive control function that optimizes the motion control of each robot movable part online when performing feedback control.

〔従来の技術〕[Conventional technology]

最近、産業用ロボットの動作制御にソフトウェアサーボ
技術が導入されて実用化されつつある。Recently, software servo technology has been introduced to control the motion of industrial robots and is being put into practical use.

すなわち、マイクロプロセッサの発展と高速演算能力を
有したディジタルシグナルプロセッサの出現に伴い、駆
動モータのサーボ機構をアナログサーボ機構からディジ
タルサーボ機構に転換し、アナログサーボ機構では実現
困難であった高機能化が図られつつあり、例えば、制御
対象の不安定特性の改善等に種々の提案がなされている
。In other words, with the development of microprocessors and the emergence of digital signal processors with high-speed calculation capabilities, the servo mechanism of the drive motor was changed from an analog servo mechanism to a digital servo mechanism, achieving higher functionality that was difficult to achieve with an analog servo mechanism. For example, various proposals have been made to improve unstable characteristics of a controlled object.

〔発明が解決すべき課題〕[Problem to be solved by the invention]

然しなから、産業用ロボットにディジタルサーボ技術を
適用した場合には、制御対象であるロボットの可動部、
即ち、ロボット旋回胴、ロボット腕、ロボット手首等の
動作部は、ＮＣ工作機械のテーブルや主軸頭等と異なり
、片持ち構造を有し、しかもロボット腕等は長尺構造で
あるために制御対象としては低剛性体であり、また、イ
ナーシャ比が大きいという宿命を負っている。故に、斯
かる低剛性体からなる制御対象をサーボモータからなる
アクチュエータにより制御する場合には必然的にバネ定
数、粘性抵抗を無視した制御が不可能である。しかも、
ロボット可動部は、３次元空間内で種々姿勢を変化させ
ながらロボット動作するために、可動部自体のイナーシ
ャ、バネ定数、粘性係数等のパラメータも時々刻々変化
する。従って、制御対象の特性安定化を図るには実時間
制御、つマリ、オンライン制御によりパラメータを修正
し、また、ディジタルサーボ系のサーボゲインも調整、
変更しながら制御を遂行しなければ、真のソフトウェア
サーボまたはディジタルサーボを遂行し得ないと言う問
題点がある。他方、近時のマイクロプロセッサの発展の
一貫として高速演算機能を有した演算ハードウェアとし
てディジタルシグナルプロセッサ（ＤＳＰ）が市場に提
供されている。このＤＳＰは、機能面で、通常は、加減
乗算機能しか有しないながら、極めて演算速度が高速で
ある利点を有していることから、ディジタルサーボ機構
の演算制御部として適用される傾向にある。依って、本
発明の目的は、産業用ロボットの動作制御に当たり、高
速演算機能を有したＤＳＰを内蔵するディジタルサーボ
機構を介してロボット可動部の動作制御を遂行するに当
たり、制御系のパラメータをオンラインで推定し、また
、制御対象の安定化に適したループゲインをオンライン
で都度、演算・決定し、更にそのループゲインを上記オ
ンラインで推定したパラメータを取り入れることにより
最適化する適応制御型のロボットの制御方式を提供せん
とするものである。However, when applying digital servo technology to industrial robots, the movable parts of the robot to be controlled,
In other words, unlike the table and spindle head of an NC machine tool, the operating parts such as the robot rotating trunk, robot arm, and robot wrist have a cantilevered structure, and since the robot arm has a long structure, it cannot be controlled. As such, it is a low-rigid body and is also destined to have a large inertia ratio. Therefore, when controlling an object made of such a low-rigidity body with an actuator made of a servo motor, it is inevitably impossible to perform control that ignores the spring constant and viscous resistance. Moreover,
Since the robot movable part operates while changing various postures in a three-dimensional space, parameters such as the inertia, spring constant, and viscosity coefficient of the movable part itself change from time to time. Therefore, in order to stabilize the characteristics of the controlled object, parameters must be modified using real-time control, compression, and online control, and the servo gain of the digital servo system must also be adjusted.
There is a problem in that true software servo or digital servo cannot be performed unless control is performed while making changes. On the other hand, as part of the recent development of microprocessors, digital signal processors (DSPs) have been provided on the market as arithmetic hardware having high-speed arithmetic functions. In terms of functionality, this DSP usually only has addition, subtraction, and multiplication functions, but it has the advantage of extremely high calculation speed, so it tends to be applied as a calculation control section of a digital servo mechanism. Therefore, an object of the present invention is to control the motion of an industrial robot by controlling the motion of the robot's movable parts online through a digital servo mechanism incorporating a DSP with high-speed calculation functions. This is an adaptive control type robot that calculates and determines the loop gain suitable for stabilizing the controlled object online each time, and further optimizes the loop gain by incorporating the parameters estimated online. The purpose is to provide a control method.

〔解決手段〕[Solution]

即ち、上述の発明目的に鑑みて、本発明は、先ず、ロボ
ットの夫々の可動部を制御対象とする各アクチュエータ
モータを高速演算装置を備えたディジタルサーボ系によ
りフィードバック制御するロボット制御方式において、
前記各アクチュエータモータで駆動される制御対象を慣
性モーメントとバネ定数と粘性係数とをパラメータとす
るロボットモデルにモデル化し、該ロボットモデルを含
めたサーボ系の３次伝達関数を決定し、前記ロボットモ
デルを含めたサーボ系の３次伝達関数のボード線図上の
共振点または***振点に対応した周波数とゲインとを前
記ロボットモデルのトルクと速度とのオンラインにおけ
るサンプル値から決定し、該決定した周波数とゲインと
の値から前記サーボ系のバネ定数と粘性係数とを実時間
決定し、決定されたバネ定数と粘性係数とにより、前記
各可動部の動作を制御するロボットの制御方式を提供す
るものである。That is, in view of the above-mentioned object of the invention, the present invention first provides a robot control method in which each actuator motor, which controls each movable part of the robot, is feedback-controlled by a digital servo system equipped with a high-speed calculation device.
The controlled object driven by each of the actuator motors is modeled into a robot model having a moment of inertia, a spring constant, and a viscosity coefficient as parameters, and a third-order transfer function of the servo system including the robot model is determined, and the robot model is The frequency and gain corresponding to the resonance point or anti-resonance point on the Bode diagram of the third-order transfer function of the servo system including are determined from online sample values of torque and speed of the robot model, and the determined A robot control method is provided in which the spring constant and viscosity coefficient of the servo system are determined in real time from the values of frequency and gain, and the operation of each of the movable parts is controlled by the determined spring constant and viscosity coefficient. It is something.

また、本発明は、ロボットの夫々の可動部を制御対象と
する各アクチュエータモータを高速演算装置を備えたデ
ィジタルサーボ系によりフィードバック制御するロボッ
ト制御方式において、前記各アクチュエータモータで駆
動される制御対象を慣性モーメントとバネ定数と粘性係
数とをパラメータとするロボットモデルにモデル化し、
該ロボットモデルを含めたサーボ系の運動方程式を決定
し、該運動方程式から該ロボットモデルを含めたアクチ
エエータモータの実時間等価慣性モーメントに関する状
態方程式を決定し、該状態式から前記アクチュエータモ
ータの実時間等価慣性モーメントに関する特性方程式を
求め、該特性方程式からラウスハルビッツの安定条件を
満たす前記サーボ系のループゲインを前記高速演算装置
で高速演算により決定するロボットの制御方式を提供す
るものである。更に、本発明は、上述のようにして制御
系のパラメータであるバネ定数と粘性係数とをオンライ
ンで決定すると共にオンラインで都度決定されたそのパ
ラメータに基づき、かつ、制御対象の慣性モーメントを
モータ部における等価慣性モーメントに換算したサーボ
ループの安定制御用ループゲインをオンラインで決定、
変更して制御の最適化を図るロボットの制御方式を提供
するものである。以下、本発明を添付図面に示す実施例
に基づいて、更に詳細に説明する。The present invention also provides a robot control method in which each actuator motor, which controls each movable part of the robot, is feedback-controlled by a digital servo system equipped with a high-speed calculation device. Modeled into a robot model with moment of inertia, spring constant, and viscosity coefficient as parameters,
Determine the equation of motion of the servo system including the robot model, determine the equation of state regarding the real-time equivalent moment of inertia of the actuator motor including the robot model from the equation of motion, and determine the equation of state for the actuator motor including the robot model from the equation of state. The present invention provides a robot control method in which a characteristic equation regarding the real-time equivalent moment of inertia is determined, and a loop gain of the servo system that satisfies the Raus-Harwitz stability condition is determined from the characteristic equation by high-speed calculation using the high-speed calculation device. . Furthermore, the present invention determines the spring constant and viscosity coefficient, which are the parameters of the control system, online as described above, and also determines the moment of inertia of the controlled object in the motor section based on the parameters determined each time online. Determine online the loop gain for stable control of the servo loop converted to the equivalent moment of inertia at
This provides a robot control method that can be modified to optimize control. Hereinafter, the present invention will be described in more detail based on embodiments shown in the accompanying drawings.

〔実施例〕〔Example〕

第１Ａ図は、本発明の１実施例に係る産業用ロボットの
制御方式を実施する場合におけるある１軸に関するロボ
ット可動部を制御対象にするサーボループを示した基本
ブロック図、第１Ｂ図は、第１Ａ図のディジタルサーボ
機構を、アクチュエータ、制御対象を含めて機能表示し
た機能ブロック図、第２図は、同制御方式を適用する場
合に各ロボット可動部をアクチュエータから見たロボッ
トモデルを示す機構図、第３図は、第２図示したロボッ
トモデルのボード線図の概略を説明するグラフ図、第４
図は、本発明のロボット制御方式の実施において、制御
対象であるロボット可動部の慣性モーメントを、その駆
動モータ部の等価慣性モーメントに換算して構成したサ
ーボループを示すブロック図である。FIG. 1A is a basic block diagram showing a servo loop that controls a robot movable part related to one axis when implementing a control method for an industrial robot according to an embodiment of the present invention, and FIG. Figure 1A is a functional block diagram showing the functions of the digital servo mechanism, including the actuator and the controlled object. Figure 2 is a mechanism showing a robot model of each robot movable part viewed from the actuator when the same control method is applied. Figures 3 and 3 are graphs illustrating the outline of the Bode diagram of the robot model shown in Figure 2, and Figure 4.
The figure is a block diagram showing a servo loop constructed by converting the moment of inertia of the movable part of the robot to be controlled into the equivalent moment of inertia of the drive motor part in implementing the robot control method of the present invention.

先ず、第１Ａ図を参照すると、産業用ロボットにはホス
トＣＰＵｌ０が具備され、このホストＣＰＵｌ０にはロ
ボットの所望動作に応じて教示入力が作業者により、人
力可能になっている。この教示入力に従って、ホストＣ
ＰＵ１０は各軸回りのロボット可動部に対して成る位置
から他の成る目的位置への移動指令をディジタル指令と
して出力する（図示例は６軸ロボツトにおき６軸夫々に
対して位置指令を示すディジタル指令を出力することを
示している。）。各軸系にはディジタルサーボ機構１２
が設けられ、このディジタルサーボ機構１２はホストＣ
ＰＵｌ０からのディジタル位置指令に従って、当該軸系
のアクチュエータ１４へ作動指令を出力する。ここで、
ディジタルサーボ機構１２は高速プロセッサである周知
のディジタルシグナルプロセッサＤＳＰから成る演算機
能部を具備し、アクチュエータ１４を形成する駆動モー
タ（通常はＡＣサーボモータ）に対する指令量と制御対
象１６の位置を示すフィードバック信号とから新たな電
気指令信号を作成、送出する。First, referring to FIG. 1A, the industrial robot is equipped with a host CPU 10, into which an operator can manually input instructions in accordance with the desired movement of the robot. According to this teaching input, host C
The PU 10 outputs a movement command for the robot movable part around each axis from one position to another target position as a digital command (the example shown is a 6-axis robot, and a digital command is output for each of the six axes). (Indicates that a command is to be output.) Digital servo mechanism 12 for each axis system
is provided, and this digital servo mechanism 12 is connected to a host C
In accordance with the digital position command from PU10, an operation command is output to the actuator 14 of the relevant shaft system. here,
The digital servo mechanism 12 is equipped with an arithmetic function section consisting of a well-known digital signal processor DSP, which is a high-speed processor, and provides feedback indicating the command amount to the drive motor (usually an AC servo motor) forming the actuator 14 and the position of the controlled object 16. Create and send a new electrical command signal from the signal.

アクチュエータ１４は上記電気指令信号から駆動信号を
作成するアンプと上記駆動モータとから構成され、作動
量をディジタルサーボ機構にフィードバックする構成を
有すると共に制御対象１６を所望量だけ動作させるもの
である。このようなサーボ制御ループは多軸（６軸）ロ
ボットの各軸系に具備されている。The actuator 14 is composed of an amplifier that generates a drive signal from the electric command signal and the drive motor, has a configuration that feeds back the amount of operation to the digital servo mechanism, and operates the controlled object 16 by a desired amount. Such a servo control loop is provided in each axis system of a multi-axis (six-axis) robot.

ここで、第１Ｂ図を参照すると、ディジタルサーボ機構
１２は、既述のＤＳＰと共に位置制御ユニット２２（ゲ
インに、）、速度制御ユニット２４（ゲインＫ）の機能
部を有した構成にあり、ＤＳＰから位置制御ユニット２
２へ位置の作動指令が印加される。この位置の作動指令
は制御対象（または、他の実施例としてアクチュエータ
とすることもある。）から構成される装置フィードバッ
ク信号と加算点で加算され上記位置制御ユニット２２へ
送出される。次ぎに位置の作動指令は、位置制御ユニッ
ト２２から速度指令に転換されて速度制御ユニット２４
へ印加される。このとき、アクチュエータ１４から帰還
される速度フィードバック信号と加算点で加算されて速
度制御ユニット２４に印加される。ＤＳＰへは位置及び
速度フィードバック信号がいずれも帰還されている。Here, referring to FIG. 1B, the digital servo mechanism 12 has a configuration including the above-described DSP, a position control unit 22 (gain), a speed control unit 24 (gain K), and a DSP. From position control unit 2
A position actuation command is applied to 2. This position actuation command is added to a device feedback signal composed of a controlled object (or in other embodiments, an actuator) at a summing point and sent to the position control unit 22. Next, the position operation command is converted into a speed command from the position control unit 22 and is converted into a speed command by the speed control unit 24.
is applied to. At this time, it is added to the speed feedback signal fed back from the actuator 14 at the addition point and applied to the speed control unit 24. Both position and velocity feedback signals are fed back to the DSP.

アクチュエータ１４は、（ＫＬ　／Ｊ　Ｓ　）の特性を
有し、また、ロボット可動部からなる制御対象は（１／
ｓ）の特性を有している。The actuator 14 has a characteristic of (KL /JS), and the controlled object consisting of the robot movable part has a characteristic of (1/JS).
s).

さて、本発明によると、制御対象１６をアクチュエータ
１４により駆動（回転駆動）する場合には既述のように
、各ロボット可動部は低剛性、片持ち構造を有して摩擦
抵抗下で作動することからハネ定数、粘性抵抗とを有し
たイナーシャ体であると考察することができ、従って、
各アクチュエータから見た制御対象１６は、第２図に示
すようにモデル化することができる。即ち、アクチュエ
ータ１４を構成する駆動モータの慣性モーメントＪ５、
人力トルクをＴ、モータ回転角度を０９、モータ回転角
速度０４、制御対象１６の慣性モーメントＪＩ＋、回転
角ＯＲ１Ｒ１回転変速Ｒ、バネ定数をＫｃ、粘性係数を
ＢＫと定義すると、制御対象を駆動する過程における駆
動モータの伝達関数は、 Ｇ　＝　ｅ　Ｍ　／　Ｔ　＝　１７ｓ　ｘここで、この
制御対象１６の制御系は、バネ定数Ｋｃと粘性係数を８
８とを有した閉ループ制御であるから、周波数応答を考
察すべく、上記伝達関数を変換すると、次の式になる。Now, according to the present invention, when the controlled object 16 is driven (rotationally driven) by the actuator 14, each robot movable part has a low rigidity and cantilever structure and operates under frictional resistance, as described above. Therefore, it can be considered that it is an inertia body with a Hane constant and viscous resistance, and therefore,
The controlled object 16 seen from each actuator can be modeled as shown in FIG. That is, the moment of inertia J5 of the drive motor constituting the actuator 14,
If we define human torque as T, motor rotation angle as 09, motor rotation angular velocity as 04, moment of inertia of controlled object 16 as JI+, rotation angle as OR1R1 as rotation speed change R, spring constant as Kc, and viscosity coefficient as BK, then the process of driving the controlled object The transfer function of the drive motor in is G = e M / T = 17s
Since it is a closed loop control having 8, when the above transfer function is transformed to consider the frequency response, the following equation is obtained.

この（１）式で示すボード線図を考察すると、上記式は
分子が２次系、分母が３次系であるから３つの極を有し
た第３図に図示のグラフ図のようになる。Considering the Bode diagram represented by this equation (1), since the numerator of the above equation is a quadratic system and the denominator is a cubic system, the graph has three poles as shown in FIG. 3.

第３図において、ω、及びω２は夫々、分子側及び分母
側がゼロに限り無く近づく周波数で、つまり、***振点
と共振点とを示す周波数であり、産業用ロボットでは共
振点ω２がせいぜいｌ０８２程度の低周波数である。Ｉ
Ｇ、ｌ　　１０２１は夫々、***振点ω、と共振点ω２
とにおけるゲインを示している。In FIG. 3, ω and ω2 are frequencies at which the numerator and denominator approaches zero, respectively, that is, the frequencies that indicate an anti-resonance point and a resonance point.In an industrial robot, the resonance point ω2 is at most 1082 It is a low frequency. I
G, l 1021 are antiresonance point ω and resonance point ω2, respectively.
It shows the gain in and.

上記の（１）式から、 ・・・　（１） この（２）式において、Ｊ、とＪＲとはロボット動作中
における各時点の姿勢により、決定される慣性モーメン
トであり、ロボット自身が姿勢毎に知っている値である
。From the above equation (1), ... (1) In this equation (2), J and JR are moments of inertia determined by the posture at each point during robot operation, and the robot itself This is the value you know.

いま、***振点ω１を考察すると、（２）弐から、Ｋｃ
　／Ｊｓ　　−ＪＲ１／ＪＭ　　・ω”　＝Ｏが成立す
る条件より、 Ｋｃ＝Ｊ＊ω１　　・・・　（３） となり、バネ定数に、が決定する。即ち、ロボット動作
の各姿勢状態における***振点を見出せことでバネ定数
Ｋｃが決定できるのである。また、同様に、***振点ω
１におけるゲインＩＣＩから、サーボ系の粘性係数ＢＫ
は、 となる。ただし、Ａ−（１／Ｊ、４＋１／ＪＲ）、Ｂ　
＝　１　／　Ｊ　Ｍ　　、 Ｃ＝１　／　Ｊ　、４　　・Ｊ、ｌである。Now, considering the anti-resonance point ω1, from (2) 2, Kc
/Js - JR1/JM ・ω" = O From the condition that holds, Kc = J * ω1 (3), and the spring constant is determined. In other words, the anti-resonance point in each posture state of robot operation The spring constant Kc can be determined by finding the anti-resonance point ω.
From the gain ICI at 1, the viscosity coefficient BK of the servo system
becomes . However, A-(1/J, 4+1/JR), B
= 1/JM, C=1/J, 4.J, l.

上記の（３）式、（４）式から***振点ω１を見出せば
、その時点における当該制御対象に関するサーボ系のパ
ラメータであるバネ定数Ｋｃと粘性係数Ｂ、とのオンラ
インでの現在値を知ることができ、故に制御操作を例え
ば、最適化すること等に利用できることになる。このと
き、上記***振点を見出すことに替えて、共振点ω２を
見出すことでもこれらのパラメータをオンラインで知る
ことが出来る。If you find the anti-resonance point ω1 from the above equations (3) and (4), you will know the online current values of the spring constant Kc and viscosity coefficient B, which are the parameters of the servo system related to the controlled object at that time. Therefore, it can be used, for example, to optimize control operations. At this time, these parameters can be known online by finding the resonance point ω2 instead of finding the anti-resonance point.

さて、ボード線図上における***振点ω、をオンライン
で見出すには、以下の演算をサーボ機構１２のＤＳＰで
遂行する。すなわち、アクチュエータ１４からの入力ト
ルクＴとその時点におけるアクチュエータの速度出力（
アクチュエータを構成する駆動モータのパルスコーダに
より検出することができる。）とからサンプル値制御手
法で演算を行うのである。Now, in order to find the anti-resonance point ω on the Bode diagram online, the following calculation is performed by the DSP of the servo mechanism 12. That is, the input torque T from the actuator 14 and the speed output of the actuator at that time (
It can be detected by the pulse coder of the drive motor that constitutes the actuator. ), calculations are performed using the sample value control method.

サンプル値制御手法によるため、周知の適応同定法によ
り前記伝達関数（１）式をＺ変換すると間代は、一般式
として、 系が決定される。Ａ、〜Ａ３、Ｂ、〜Ｂ、は一般的係数
で、これをＳ関数系へ移行させると、周知のＺ＝ｅｓＴ
を代入して、 ・　・　・　（５） となる。Since the sample value control method is used, when the transfer function equation (1) is Z-transformed using a well-known adaptive identification method, the system is determined as a general equation. A, ~A3, B, ~B are general coefficients, and when these are transferred to the S function system, the well-known Z=esT
Substituting , we get ・ ・ ・ (5).

今、***振点の検出から、分子側だけ注目すると、周波
数領域では、 Ｆ　（ω）　＝Ｂ、ｅ””＋Ｂｚｅ”＋８３＝　Ｂ　＋
（ｃｏｓ２ωＴ＋ｊｓｉｎ２ωＴ）＋Ｂ、（ｃｏｓωＴ
十ｊｓｉｎωＴ）＋８３ ＝　（ＢＩＣＯ３２（Ｌ）Ｔ　＋ＢｚｃｏｓωＴ　＋Ｂ
ｌ）十ｊ（Ｂ、５ｉｎ２ωＴ　＋ＢｚｓｉｎωＴ）・　
・　・　（６） となる。故に、 Ｆ（ω）１＝ となる。Now, if we focus only on the molecular side from the detection of the anti-resonance point, in the frequency domain, F (ω) = B, e"" + Bze" + 83 = B +
(cos2ωT+jsin2ωT)+B, (cosωT
10jsinωT)+83 = (BICO32(L)T +BzcosωT +B
l) 10j (B, 5in2ωT +BzsinωT)・
・ ・ (6) becomes. Therefore, F(ω)1=.

この式（７）を用いてサーボ機構１２のＤＳＰで極値ω
１を計算で見出すことは、高速演算機能を有したＤＳＰ
によっても極めて難しいが、ロボットの場合には、−船
釣に極値が各動作毎にそんなに大きくは変化しないこと
を考慮すると、予め***振点の最大及び最小ω値を実験
的に決定しておき、それを必要な精度で等分割して当該
サーボ機構１２のディジタルシグナルプロセッサと結合
した記憶手段に記憶しておけば、上記分割幅で前回の動
作過程における極値周波数ω、の推定値から始めて該ω
１を前後に移動し、サンプルしたロボット動作１（）ル
クＴ、速度０．４）に関して、最小ゲインＧ１になる周
波数ω１を今回のω、として決定でき、前述した（３）
式、（４）式からバネ定数Ｋｃと粘性係数ＢＫとを演算
決定することができるのである。しかも、ＤＳＰを用い
ればホス）ＣＰＵＩＯ（第１図参照）から順次に送出さ
れる隣接入力の間に上述の演算過程を実行してオンライ
ンにより、バネ定数Ｋｃと粘性係数ＢＫの両パラメータ
を決定できるのである。こうして決定されたオンライン
のパラメータを利用すれば各制御対象の閉ループ制御系
の制御性を改善させることができ、例えば、後述のよう
にループゲインを変更調節して制御の最適化を果たすこ
ともできるのである。Using this equation (7), the extreme value ω is determined by the DSP of the servo mechanism 12.
Finding 1 by calculation requires a DSP with high-speed calculation function.
However, in the case of robots, the maximum and minimum ω values of the anti-resonance point should be determined experimentally in advance, considering that the extreme values do not change that much with each movement. By dividing it into equal parts with the necessary precision and storing it in a storage means connected to the digital signal processor of the servo mechanism 12, the estimated value of the extreme frequency ω in the previous operation process can be calculated using the above division width. First time
For the sampled robot motion 1 (lux T, speed 0.4), the frequency ω1 at which the minimum gain G1 is obtained can be determined as the current ω, and as described above (3).
The spring constant Kc and the viscosity coefficient BK can be calculated and determined from the equation (4). Moreover, by using a DSP, it is possible to determine both the spring constant Kc and the viscosity coefficient BK online by executing the above calculation process between adjacent inputs sequentially sent from the CPUIO (see Figure 1). It is. By using the online parameters determined in this way, it is possible to improve the controllability of the closed-loop control system for each controlled object. For example, as described below, the loop gain can be changed and adjusted to achieve control optimization. It is.

他方、第２図のロボットモデルによれば、運動方程式は
、 Ｔ＝ＪｌＩ　Ｑ＋ＢＫ　　ｌ＋４−ＯＲ）＋ＫＣ（ｅｘ
ＯＲ）　　　　　　　　　　・・・　（８）０＝ＪＲ６
１＋ＢＫ　　（ｅｘ　−ＯＮ　）＋Ｋｃ　（θえ一θＭ
）　　　　・・・　（９）ここで、周知のシステムの構
造理論（システム制御理論入門、第４章、昭和５４年１
２月、実教出版社刊、）に準拠すべく、上記運動方程式
に対して、新たな状態変数としてθ、−ｅＭ＝ｅ、、Ｑ
、−６，＝白、を導入すると、上記運動方程式は、次の
状態方程式として書き換えることができる。On the other hand, according to the robot model in Figure 2, the equation of motion is T=JlI Q+BK l+4-OR)+KC(ex
OR) ... (8) 0=JR6
1+BK (ex -ON)+Kc (θE1θM
) ... (9) Here, the well-known structural theory of systems (Introduction to System Control Theory, Chapter 4, 1978 1)
In order to comply with Jikkyo Publishing Co., Ltd. (February), new state variables θ, −eM=e, ,Q are added to the above equation of motion.
, -6,=white, the above equation of motion can be rewritten as the following equation of state.

ｙ＝　［１，Ｏ，Ｏ］　　ｘ”　　・　・　・　（１１
）となる。なお、 ａｚ＝０．　　ａ目＝　　ＢＫ　／ＪＮ　ｒ　　ａ、＝
−Ｋｃ　／ＪＭ、 ａｔｌ＝０．　　ａ、、＝　　（ＢＫ　／　Ｊｐｉ　＋
ＢＫ　／　ＪＲ）　。y= [1, O, O] x” ・ ・ ・ (11
). Note that az=0. a-th = BK /JN r a, =
−Kc/JM, atl=0. a,, = (BK / Jpi +
BK/JR).

ａ　２３＝　　（Ｋｃ　／　ＪＭ　＋ＫＣ／　Ｊ　＊　
）　。a23=(Kc/JM+KC/J*
).

ｂｔ　＝　ｂｔ　＝　１　／　ＪＭ この場合に、制御対象１６の速度出力を近似的にバルス
コーダから検出可能な駆動モータの速度出力に置き換え
て考察すると、第２図のロボットモデルを含めたサーボ
ループは、第４図のような速度ループ系と考察すること
ができる。なお、第４図におけるＫはループゲインであ
る。bt = bt = 1 / JM In this case, if the speed output of the controlled object 16 is approximately replaced with the speed output of the drive motor that can be detected from the pulse coder, the servo loop including the robot model in Fig. 2 will be: It can be considered as a velocity loop system as shown in FIG. Note that K in FIG. 4 is the loop gain.

このサーボループ系の状態式は、 となる。なお、■ｏは速度指令値を意味する。The state equation of this servo loop system is becomes. Note that ■o means a speed command value.

従って、この状態方程式の特性方程式から上記第４図の
サーボ系の安定条件として周知のラウスフルビッツの安
定条件を満たすようにループゲインＫを決定すれば、制
御対象１６の制御安定性を改善することができる。Therefore, if the loop gain K is determined from the characteristic equation of this state equation so as to satisfy the well-known Lausfurwitz stability condition as the stability condition for the servo system shown in FIG. 4 above, the control stability of the controlled object 16 can be improved. Can be done.

特性方程式は、 Ｓ’　　　（ａｚ　　Ｋｂ＋　＋ａｚｚ）　Ｓ”　＋　
（ａｚ＋ａｚｚ　　Ｋａｚｚｌ）＋　　　ａｔｓ　　ａ
＋ｚａｚ＋ＫｂＩ　ａ１２）ｓ＋　　（−ａｔ：＋ａｚ
＋＋ａｚ３ａ　目　　Ｋａｚ：＋ｂ＋　　）・・・　（
１３） となる。The characteristic equation is S' (az Kb+ +azz) S” +
(az+azz Kazzl)+ ats a
+zaz+KbI a12)s+ (-at:+az
++az3a eyes Kaz:+b+ )... (
13) It becomes.

故に、ラウスフルビッツの安定条件を満たすためには、
次の諸条件を満たすことである。Therefore, in order to satisfy the Lausfurwitz stability condition,
The following conditions must be met.

■　Ｋ＞ａｚ＋ａｚｚ／ｂｌ　、（ｂｔ　＞Ｏ）Ｋ＜ａ
ｌｌ＋ａｚｔ／ｂ＋　、（ｂｔ　＜Ｏ）ａ　Ｉ＋ａｔｚ
　　　ａｔ３　　ａ　　ＩｔａｚＫ＜− ■　Ｋ”　　ｂｔ　　（ｂｔ　　ａＩ２　ａｚｔｂ＋　
　）＝ＡＫＸ　　（ａｔｓＦｘ　　＋ｔｚ　　（ａ＋＋
ａｚｚ　　ａｚ３ａ＋ｚａ＋＋）　　　（ｂｔ　　ａｄ
ｚ　　ａｚｔｌ）＋　　）Ｘ　　（ａｚ＋ａｚｚ）　）
　　−Ｂ ａｚｓａＩＩ　　ａ＋ｚａｚ＋　　　（ａｚ＋ａｔｔ）
Ｘ　ＣａＩＩａｔｚ　　ａｔ３　ａｔｚａ目）＝Ｃただ
し、 Ａ＜０のとき、 Ｂ”−４ＡＣ＜Ｑ、または、 Ａｇｏのとき、 Ｂ”　−４ＡＣ＞Ｏ且つ、 これら■〜■を満たすようにループゲインにの最大値を
決定すれば、制御系は安定条件下で最も高いゲインにの
もとに制御作動されることになるのである。このような
ループゲインにの決定過程もサーボ機構１２のディジタ
ルシステムプロセッサＤＳＰにより、高速度で演算を行
うことができることにより、オンラインで遂行すること
ができる。つまり、試行錯誤的に制御系の安定かつ高速
ループゲインＫを見出すのと違い、本発明の方式によれ
ば、実時間でゲイン決定が遂行できる有利を有するので
ある。■ K>az+azz/bl, (bt>O)K<a
ll+azt/b+ , (bt <O)a I+az
at3 a ItazK<- ■ K” bt (bt aI2 aztb+
)=AKX (atsFx +tz (a++
azz az3a+za++) (bt ad
z aztl)+ )X (az+azz) )
-B azsaII a+zaz+ (az+att)
X CaIIatz at3 atzath)=C However, when A<0, B"-4AC<Q, or when Ago, B"-4AC>O, and set the maximum to the loop gain so as to satisfy these ■～■ Once the value is determined, the control system will be operated with the highest gain under stable conditions. The process of determining the loop gain can also be performed online because the digital system processor DSP of the servo mechanism 12 can perform calculations at high speed. That is, unlike finding a stable and fast loop gain K for a control system through trial and error, the method of the present invention has the advantage of being able to determine the gain in real time.

そして、上述のようにして、各制御対象であるロボット
可動部に就き、オンラインによりパラメータ（バネ定数
、粘性係数）を決定することが可能であり、かつ、ルー
プゲインＫを閉ループ制御系の安定で且つ高速性を備え
た値に選定できることから、サーボ機構１２は両者を総
合して、時々刻々の制御系パラメータに対応したループ
ゲインＫを時々刻々に決定し、制御動作をオンラインで
常に最適化することができるのである。As described above, it is possible to determine the parameters (spring constant, viscosity coefficient) online for each movable part of the robot that is to be controlled, and it is also possible to determine the loop gain K to ensure the stability of the closed-loop control system. In addition, since it is possible to select a value with high speed, the servo mechanism 12 integrates the two, determines the loop gain K corresponding to the control system parameters from time to time, and constantly optimizes the control operation online. It is possible.

その結果、従来のアナログサーボ機構がロボット姿勢に
おける最悪条件を考慮した固定ゲインにより制御する場
合と対比すると、本発明によれば、ロボットの姿勢条件
に拘束されることなく、その都度、最適の制御条件にパ
ラメータやゲインを設定して各ロボット可動部を最適条
件下で動作制御することが可能となるのである。As a result, compared to the case where the conventional analog servo mechanism performs control using a fixed gain that takes into account the worst conditions in the robot posture, the present invention allows for optimal control each time without being constrained by the robot posture conditions. By setting parameters and gains to the conditions, it becomes possible to control the operation of each robot movable part under optimal conditions.

〔発明の効果〕〔Effect of the invention〕

以上の説明から明らかなように、本発明によれば、高速
演算機能を有したディジタルシステムプロセッサを内蔵
したディジタルサーボ機構を介してロボット可動部にお
ける各可動部を制御対象としたロボット制御がロボット
モデルにおけるパラメータをオンラインで決定し、また
制御系を安定条件の下で高速制御する最大のループゲイ
ンを時々刻々決定して制御できるから、ロボットの全て
の動作過程を常に最適制御条件下で制御することができ
ると言うディジタルサーボないしソフトウェアサーボの
特徴を如°何なく発揮させ得る効果が得られるのである
。As is clear from the above description, according to the present invention, the robot model can control each movable part of the robot via a digital servo mechanism incorporating a digital system processor with high-speed calculation function. The parameters of the robot can be determined online, and the maximum loop gain that controls the control system at high speed under stable conditions can be determined and controlled from time to time, so all motion processes of the robot can always be controlled under optimal control conditions. This provides an effect that fully utilizes the characteristics of digital servo or software servo, which is capable of servo control.

【図面の簡単な説明】[Brief explanation of the drawing]

第１Ａ図は、本発明の１実施例に係る産業用ロボットの
制御方式を実施する場合におけるある１軸に関するロボ
ット可動部を制御対象にするサーボループを示した基本
ブロック図、第１Ｂ図は、第１Ａ図のディジタルサーボ
機構を、アクチュエータ、制御対象を含めて機能表示し
た機能ブロック図、第２図は、同制御方式を適用する場
合に各ロボット可動部をアクチュエータから見たロボッ
トモデルを示す機構図、第３図は、第２図示したロボッ
トモデルのボード線図の概略を説明するグラフ図、第４
図は、本発明のロボット制御方式の実施において、制御
対象であるロボット可動部の慣性モーメントを、その駆
動モータ部の慣性モーメントに換算して構成したサーボ
ループを示すブロック図。 １０・・・ホストｃｐｕ。 １２・・・ディジタルサーボ機構、 ＤＳＰ・・・ディジタルシステムプロセッサ、１４・・
・アクチュエータ、　　１６・・・制御対象、ω、・・
・***振点周波数、　ω２・・・共振点周波数、Ｋｃ・
・・バネ定数、　　　　ＢＩ［・・・粘性係数、Ｋ・・
・ループゲイン。FIG. 1A is a basic block diagram showing a servo loop that controls a robot movable part related to one axis when implementing a control method for an industrial robot according to an embodiment of the present invention, and FIG. Figure 1A is a functional block diagram showing the functions of the digital servo mechanism, including the actuator and the controlled object. Figure 2 is a mechanism showing a robot model of each robot movable part viewed from the actuator when the same control method is applied. Figures 3 and 3 are graphs illustrating the outline of the Bode diagram of the robot model shown in Figure 2, and Figure 4.
The figure is a block diagram showing a servo loop constructed by converting the moment of inertia of the movable part of the robot to be controlled into the moment of inertia of its drive motor part in implementing the robot control method of the present invention. 10...Host CPU. 12...Digital servo mechanism, DSP...Digital system processor, 14...
・Actuator, 16...Controlled object, ω,...
・Anti-resonance point frequency, ω2...Resonance point frequency, Kc・
・・・Spring constant, BI[...Viscosity coefficient, K...
・Loop gain.

Claims (1)

【特許請求の範囲】 １、ロボットの夫々の可動部を制御対象とする各アクチ
ュエータモータを高速演算装置を備えたディジタルサー
ボ系によりフィードバック制御するロボット制御方式に
おいて、前記各アクチュエータモータで駆動される制御
対象を慣性モーメントとバネ定数と粘性係数とをパラメ
ータとするロボットモデルにモデル化し、該ロボットモ
デルを含めたサーボ系の３次伝達関数を決定し、前記ロ
ボットモデルを含めたサーボ系の３次伝達関数のボード
線図上の共振点または***振点に対応した周波数とゲイ
ンとを前記ロボットモデルのトルクと速度とのオンライ
ンにおけるサンプル値から決定し、該決定した周波数と
ゲインとの値から前記サーボ系のバネ定数と粘性係数と
を実時間決定し、決定されたバネ定数と粘性係数とによ
り、前記各可動部の動作を制御することを特徴としたロ
ボットの制御方式。 ２、ロボットの夫々の可動部を制御対象とする各アクチ
ュエータモータを高速演算装置を備えたディジタルサー
ボ系によりフィードバック制御するロボット制御方式に
おいて、前記各アクチュエータモータで駆動される制御
対象を慣性モーメントとバネ定数と粘性係数とをパラメ
ータとするロボットモデルにモデル化し、該ロボットモ
デルを含めたサーボ系の運動方程式を決定し、該運動方
程式から該ロボットモデルを含めたアクチュエータモー
タの実時間等価慣性モーメントに関する状態方程式を決
定し、該状態式から前記アクチュエータモータの実時間
等価慣性モーメントに関する特性方程式を求め、該特性
方程式からラウスハルビッツの安定条件を満たす前記サ
ーボ系のループゲインを前記高速演算装置で高速演算に
より決定することを特徴したロボットの制御方式。 ３、ロボットの夫々の可動部を制御対象とする各アクチ
ュエータモータを高速演算装置を備えたディジタルサー
ボ系によりフィードバック制御するロボット制御方式に
おいて、前記各アクチュエータモータで駆動される制御
対象を慣性モーメントとバネ定数と粘性係数とをパラメ
ータとするロボットモデルにモデル化し、該ロボットモ
デルを含めたサーボ系の３次伝達関数を決定し、前記ロ
ボットモデルを含めたサーボ系の３次伝達関数のボード
線図上の共振点または***振点に対応した周波数とゲイ
ンとを前記アクチュエータモデルのトルクと速度とのオ
ンラインにおけるサンプル値から決定し、該決定した周
波数とゲインとの値から前記サーボ系のバネ定数と粘性
係数とを実時間決定し、該決定されたバネ定数と粘性係
数とをパラメータとして前記ロボットモデルを含めたサ
ーボ系の運動方程式を決定し、該運動方程式から該ロボ
ットモデルを含めたアクチュエータモータの実時間等価
慣性モーメントに関する状態方程式を決定し、該状態式
から前記アクチュエータモータの実時間等価慣性モーメ
ントに関する特性方程式を求め、該特性方程式からラウ
スハルビッツの安定条件を満たす前記サーボ系の最適ル
ープゲインを前記高速演算装置で高速演算により決定す
ることを特徴としたロボットの制御方式。[Scope of Claims] 1. In a robot control method in which each actuator motor, which controls each movable part of the robot, is feedback-controlled by a digital servo system equipped with a high-speed calculation device, the control driven by each actuator motor; The target is modeled as a robot model whose parameters are a moment of inertia, a spring constant, and a viscosity coefficient, and the third-order transfer function of the servo system including the robot model is determined. The frequency and gain corresponding to the resonance point or anti-resonance point on the Bode diagram of the function are determined from online sample values of the torque and speed of the robot model, and the servo control is determined from the determined frequency and gain values. A control method for a robot, characterized in that a spring constant and a viscosity coefficient of a system are determined in real time, and the operation of each of the movable parts is controlled based on the determined spring constant and viscosity coefficient. 2. In a robot control method in which each actuator motor, which controls each movable part of the robot, is feedback-controlled by a digital servo system equipped with a high-speed calculation device, the control object driven by each actuator motor is controlled by a moment of inertia and a spring. Model a robot model with constants and viscosity coefficients as parameters, determine the equation of motion of the servo system including the robot model, and determine the state regarding the real-time equivalent moment of inertia of the actuator motor including the robot model from the equation of motion. determine an equation, obtain a characteristic equation regarding the real-time equivalent moment of inertia of the actuator motor from the state equation, and calculate the loop gain of the servo system that satisfies the Raus-Harwitz stability condition from the characteristic equation using the high-speed calculation device. A robot control method characterized by determining the 3. In a robot control method in which each actuator motor, which controls each movable part of the robot, is feedback-controlled by a digital servo system equipped with a high-speed calculation device, the control object driven by each actuator motor is controlled by a moment of inertia and a spring. Model a robot model with constants and viscosity coefficients as parameters, determine the third-order transfer function of the servo system including the robot model, and calculate the Bode diagram of the third-order transfer function of the servo system including the robot model. The frequency and gain corresponding to the resonance point or anti-resonance point are determined from online sample values of the torque and speed of the actuator model, and the spring constant and viscosity of the servo system are determined from the determined frequency and gain values. The equation of motion of the servo system including the robot model is determined using the determined spring constant and viscosity coefficient as parameters, and the actual actuator motor including the robot model is determined from the equation of motion. Determine an equation of state regarding the time equivalent moment of inertia, obtain a characteristic equation regarding the real time equivalent moment of inertia of the actuator motor from the equation of state, and determine the optimal loop gain of the servo system that satisfies the Raus-Harwitz stability condition from the characteristic equation. A robot control method characterized in that the high-speed calculation device performs a high-speed calculation.