JP2005078328A - State feedback controlling device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a simply-configurable state feedback controlling device capable of easily modeling a plurality of control objects at low degree with a small calculation amount, and allowing application even to a nonlinear control object. <P>SOLUTION: This state feedback controlling device has: position detection means for the control objects 1301-130m; feedback acceleration controllers 1201-120m outputting control instruction values to the control objects 1301-130m; feedback velocity controllers 1101-110m outputting acceleration instruction values to the acceleration controllers 1201-120m; and a feedback position controller 1000 outputting velocity instruction values to the velocity controllers 1101-110m. The control objects 1301-130m are respectively modeled by a primary delay system with the acceleration controllers 1201-120m and the velocity controllers 1101-110m included, and the position controller 1000 performs state feedback control by use of a model of the primary delay system. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

  The present invention relates to a state feedback control device.

Conventionally, a state feedback control apparatus that can be applied to a multi-input multi-output type is configured by using a device that is linearized and expressed as a state space model after identifying its control object with physical parameters.
For example, a technique relating to a multi-input multi-output type state feedback control device in which state feedback control is applied to a higher-order system is disclosed. (See Patent Document 1 and Patent Document 2)

In this way, when configuring a state feedback controller to control a complex control object of multiple inputs and multiple outputs, the control object is identified by physical parameters, and then linearized and expressed as a state space model. It is used. That is, to configure the state feedback controller, it is indispensable to model (formulate) the controlled object.
However, in a state feedback control apparatus that can be applied to a conventional multi-input multi-output type, it becomes very difficult to model a control object when it becomes complicated. Therefore, it is often impossible to actually construct a state feedback control apparatus that can be applied to a multi-input multi-output type.

In addition, since the actual model has a high order (third order or higher), the state feedback control configured using the model becomes complicated and the amount of calculation increases. As a result, it becomes difficult to actually realize a state feedback control apparatus that can be applied to a multi-input multi-output type.
Further, the conventional state feedback control is difficult to finely adjust, and the sensitivity of the model is high. Therefore, it is difficult to adapt to a control target in which the model changes in time series.
Basically, it cannot be applied to a non-linear control target or a non-linear region of a control target.

JP-A-5-53602 JP-A-5-53603

  An object of the present invention is to easily model a plurality of control objects with a low order (second order or less) and a small amount of calculation, and can be applied to a non-linear control object and can be configured easily. It is an object of the present invention to provide a state feedback control device that can perform this.

Such an object is achieved by the present invention described below.
The state feedback control device of the present invention includes a plurality of position detection means for detecting the positions of a plurality of control objects,
A plurality of acceleration controllers which are respectively provided corresponding to the plurality of control objects, perform feedback control based on input acceleration data and acceleration command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of acceleration controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of acceleration controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The position controller is configured to perform state feedback control using the first-order lag model.
Thereby, a complicated control object can be modeled. Further, the present invention can be applied to a non-linear control target and a non-linear region of the control target.
In the state feedback control device of the present invention, it is preferable that the input acceleration data and the input speed data are obtained from the position data detected by the position detecting means.
Thereby, the number of sensors can be further reduced, and the cost can be reduced.

The state feedback control device of the present invention includes a plurality of position detection means for detecting the positions of a plurality of control objects,
A plurality of current controllers that are respectively provided corresponding to the plurality of control objects, perform feedback control based on input current data and current command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of current controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of current controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The position controller is configured to perform state feedback control using the first-order lag model.
Thereby, a complicated servo system can be modeled. Further, the present invention can be applied to a non-linear control target and a non-linear region of the control target.
In the state feedback control device of the present invention, it is preferable that the input current data and the input speed data are obtained from the position data detected by the position detecting means.
Thereby, the number of sensors can be reduced and cost can be reduced.

The state feedback control device of the present invention includes a plurality of position detection means for detecting the positions of a plurality of control objects,
A plurality of detection means for detecting physical quantities capable of obtaining acceleration and velocity of the plurality of control objects;
A plurality of acceleration controllers which are respectively provided corresponding to the plurality of control objects, perform feedback control based on input acceleration data and acceleration command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of acceleration controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of acceleration controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The position controller is configured to perform state feedback control using the first-order lag model.
Thereby, a complicated control object can be modeled. Further, the present invention can be applied to a non-linear control target and a non-linear region of the control target.

In the state feedback control device according to the aspect of the invention, it is preferable that the plurality of detection units detect acceleration of the control target.
Thereby, a control object can be controlled suitably.
The state feedback control device of the present invention includes a plurality of position detection means for detecting the positions of a plurality of control objects,
A plurality of detection means for detecting a physical quantity capable of determining the current and speed of the plurality of control objects;
A plurality of current controllers that are respectively provided corresponding to the plurality of control objects, perform feedback control based on input current data and current command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of current controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of current controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The position controller is configured to perform state feedback control using the first-order lag model.
Thereby, a complicated servo system can be modeled. Further, the present invention can be applied to a non-linear control target and a non-linear region of the control target.

In the state feedback control device of the present invention, it is preferable that the plurality of detection means detect the speed of the control target.
Thereby, the number of sensors can be reduced and the cost can be further reduced.
In the state feedback control device of the present invention, it is preferable that the plurality of detecting means detect the current to be controlled.
Thereby, a control object can be controlled suitably.

In the state feedback control device of the present invention, the characteristics of the speed feedback controller including the control object, the acceleration controller, and the speed controller are represented by the following formula (1):
From the state equation shown in the following equation (2),
It is preferable to model approximately as shown in the following formula (3).

これにより、状態フィードバック制御の微妙な調整ができ、モデルの感度が高くても状態フィードバック制御の適応が可能となる。

As a result, the state feedback control can be finely adjusted, and the state feedback control can be adapted even if the sensitivity of the model is high.

In the state feedback control device of the present invention, it is preferable that the speed controller is a PID controller or a fuzzy controller.
As a result, the speed feedback controller can have a simple configuration, and when modeling a complex control target, the order of the speed feedback controller can be kept low and the amount of calculation can be reduced.
In the state feedback control device of the present invention, it is preferable that the control object is an actuator.
With this configuration, it can be suitably applied to a complicated mechanical body or the like.

The state feedback control device according to the present invention is preferably used for controlling an articulated arm robot including a plurality of joints and a plurality of actuators for driving the joints.
With this configuration, it can be applied to posture control of an articulated arm robot.
In the state feedback control device of the present invention, at least one of the yaw angle, yaw angular velocity, yaw angular acceleration, roll angle, roll angular velocity, roll angular acceleration, pitch angle, pitch angular velocity, and pitch angular acceleration of the articulated arm robot. Is preferably controlled.
With this configuration, the present invention can be more suitably applied to posture control of an articulated arm robot.

The state feedback control apparatus of the present invention will be described below with reference to FIGS. Note that this embodiment is given as an example, and the contents of the present invention should not be construed in a limited manner.
FIG. 1 is a block diagram schematically showing the configuration of the state feedback control apparatus of the present invention.

The state feedback control device of the present invention includes a control target group 1300, an acceleration control target group 1200, a speed controller group 1100, and a position controller 1000.
The control target group 1300 includes a plurality of control targets 1301 to 130m (m = 1, 2, 3,..., N). The acceleration of the plurality of control objects 1301 to 130m is controlled by the control from the corresponding acceleration controller among the acceleration controllers 1201 to 120m.

The acceleration controller group 1200 includes a plurality of acceleration controllers 1201 to 120m, and the plurality of acceleration controllers 1201 to 120m are provided corresponding to the plurality of control objects 1301 to 130m, respectively. Each of the acceleration controllers 1201 to 120m is based on a value obtained by subtracting acceleration data obtained by actually measuring the control objects 1301 to 130m from the acceleration command value data output from the speed controllers 1101 to 110m. To control the acceleration.
Here, the acceleration command value data is data (instruction data) for operating each control object 1301 to 130m at a predetermined acceleration.

The speed controller group 1100 includes a plurality of speed controllers 1101 to 110m, and the plurality of speed controllers 1101 to 110m are provided corresponding to the plurality of control objects 1301 to 130m, respectively. Each of the speed controllers 1101 to 110m is based on the value obtained by subtracting the speed data obtained by calculating the control objects 1301 to 130m from the acceleration data from the speed command value data output from the position controller 1000. Control the speed.
Here, the speed command value data is data (instruction data) for operating each of the control objects 1301 to 130m at a predetermined speed.

  The acceleration controllers 1201 to 120m and the speed controllers 1101 to 110m preferably use simpler configurations such as a PID controller or a fuzzy controller. Thereby, the speed feedback controller comprised from the control object 1301-130m, the acceleration controller 1201-120m, and the speed controller 1101-110m can be made into a simple structure. As a result, when modeling a complicated control target, the order of the speed feedback controller can be kept low and the amount of calculation can be reduced.

Position data corresponding to each of the plurality of control objects 1301 to 130m is input to the position controller 1000. State feedback control is performed using this position data and preset position command value data R, and the obtained speed command value data is output to the speed controllers 1101 to 110m. Thereby, the positions of the control objects 1301 to 130m are controlled.
Here, the position command value data R1 to Rn are data (instruction data) for moving the plurality of control objects 1301 to 130m to, for example, a predetermined position with respect to the absolute direction. The position data is detected by position detection means (not shown) and input to the position controller 1000.

In the position controller 1000 shown in FIG. 1, K is a feedback gain, H is an observer (observer) gain, u is an input, S is a square, x is a state variable, and x (n) is a state. A, B, and C, which are obtained by differentiating the variable x n times, indicate matrix vectors of elements of the state variable x.
In the present embodiment, state feedback control is performed using an estimated value x ′ by an observer (observer) instead of the state variable x.

Next, a configuration method of a position controller that is a state feedback controller will be described.
First, acceleration controllers 1201 to 120m and speed controllers 1101 to 110m are configured for a plurality of control objects 1301 to 130m, respectively.
The acceleration controllers 1201 to 120m and the speed controllers 1101 to 110m are constituted by feedback controllers.

The configuration of the speed feedback controller is not particularly limited, but is preferably configured by PID or fuzzy control. Thereby, a feedback controller can be made into a simple structure.
Further, all of the plurality of speed feedback controllers including the plurality of controlled objects 1301 to 130m and the plurality of acceleration controllers 1201 to 120m are each modeled by a first-order lag system.
At this time, the characteristics of the state feedback controller including the speed feedback controller can be expressed by Equation (1).

これにより、位置制御器を式（３）のように、低次かつ、簡易的なモデルで構成することができる。

Thereby, a position controller can be comprised with a low-order and simple model like Formula (3).

次に式（３）を式（２）に示す状態方程式により、記述する。

Next, equation (3) is described by the state equation shown in equation (2).

式（２）は、一般的な状態フィードバック制御における状態方程式であるため、数式についての詳細な説明は省略する。
一般的に、状態変数ｘは、入力をｕ、フィードバック係数行列をＫ（ゲインを表す）とするときに、式（４）で表される。

Since equation (2) is a state equation in general state feedback control, detailed description of the equation is omitted.
In general, the state variable x is expressed by Expression (4), where u is an input and K is a feedback coefficient matrix.

式（４）を式（５）に代入することにより、式（６）が得られる。

By substituting equation (4) into equation (5), equation (6) is obtained.

式（６）のＫを調整することにより、位置制御器の安定性（収束性）や応答性を求めることができる。

By adjusting K in Equation (6), the stability (convergence) and responsiveness of the position controller can be obtained.

Further, an observer (observer) is used as the state feedback controller to constitute a system for the entire position controller of the present embodiment.
The value multiplied by the feedback coefficient matrix K is output as new speed command value data to the corresponding speed controller 1101 to 110m.
As described above, the state feedback control apparatus of the present invention models all of the plurality of speed feedback controllers configured by the controlled object, the acceleration controller, and the speed controller, respectively, using a first-order lag system and uses them. Thus, since the state feedback control is performed, it is possible to easily model a complicated control object such as a complex mechanical body such as a robot or a plurality of control objects that interfere with each other.

In addition, since the state feedback control can be finely adjusted, it is possible to adapt to a control target in which the model has high sensitivity and the model changes in time series.
Further, the present invention can be applied to a non-linear control target and a non-linear region of the control target.
The state feedback control in the state feedback control device of the present invention includes other advanced control and the like.

Further, an angular acceleration controller may be used instead of the acceleration controller, an angular velocity controller may be used instead of the speed controller, and an angle controller may be used instead of the position controller.
In the present embodiment, the actually measured data is used as the speed data and acceleration data. However, the present invention is not limited to this. For example, the speed data and acceleration data obtained by calculating from other physical quantities such as position data are used. May be.

Note that the present invention can also be applied to a servo system by measuring current data instead of acceleration data.
Although it does not specifically limit as a control object of the state feedback control apparatus of this invention, For example, an actuator can be used. Specifically, for example, by using a plurality of actuators in an arm robot that rotationally drives a plurality of joints as control targets of the state feedback control device of the present invention, the posture of the arm robot can be easily controlled. become.

Here, FIG. 2 shows a schematic block configuration when the state feedback control device of the present invention shown in FIG. 1 is applied to posture control of an articulated arm robot. The configuration shown in FIG. 2 is almost the same as the configuration shown in FIG. 1, so only the differences will be described.
Further, in the present embodiment, an angular acceleration controller is used instead of the acceleration controller, an angular velocity controller is used instead of the speed controller, and an angle controller is used instead of the position controller.

The control target group 1300 includes actuators 1301 to 13012 as control targets. That is, the articulated arm robot includes a plurality of actuators that respectively control yaw (θz), roll (θx), and pitch (θy).
The angular acceleration controller group 1600 includes angular acceleration controllers 1601 to 16012 that control the actuators 1301 to 13012.

The angular velocity controller group 1500 includes angular velocity controllers 1501 to 15012 that control the angular acceleration controllers 1601 to 16012.
In addition, angle command value data for controlling the angles of the actuators 1301 to 13012 and angles 1 to 12 are input to the angle controller 1400 as angle command value data.

The angle controller 1400 performs state feedback control using the angle data and angle command value data, and the angles 1 to 12 measured for the actuators 1301 to 13012, respectively. Thereby, as will be described later, the angles of the yaw (θz), roll (θx), and pitch (θy) of the articulated arm robot are suitably controlled.
In FIG. 2, actuators (control objects) 1301 to 13012 are used for posture control of the articulated arm robot, but the control objects are not limited to these.

  Next, control processing of the state feedback control device shown in FIG. 2 will be described. FIG. 3 is a flowchart showing a state feedback control process in the state feedback control apparatus shown in FIG. First, in the state feedback control apparatus shown in FIG. 2, the parameters of the angle controller 1400, the angular velocity controllers 1101 to 11012, and the angular acceleration controllers 1601 to 16012 are initialized, and an integrator and a differentiator (not shown). Is initialized (step S101). The angle controller 1400, the angular velocity controllers 1501-1502, the angular acceleration controllers 1601-16012, the integrator, and the differentiator are initialized and detected by twelve angle detection means (not shown). The angle is sampled and output to the sensor signal input means as angle data (analog signal), and the current angular velocity detected by the twelve angular velocity detection means (not shown) is sampled and used as angular velocity data (analog signal). The current angular acceleration detected by the twelve angular acceleration detection means (not shown) output to the sensor signal input means is sampled and output as angular acceleration data (analog signal) to the sensor signal input means (not shown). (Step S102).

The sensor signal input means includes an ADC (analog / digital converter) and a counter, and performs A / D conversion (conversion from analog data to digital data) of angle data and angular velocity data. The measured angular acceleration data, angular velocity data, and angle data that have been A / D converted by the ADC are stored in the storage unit (steps S103 and S104).
In this embodiment, the angle data sampled by the detecting means is used. However, in the present invention, angle data obtained by integrating angular acceleration data or angular velocity data with an integrator may be used. .

In this embodiment, the angular velocity data sampled by the detecting means is used. However, in the present invention, angular velocity data obtained by differentiating the angle data with a differentiator may be used.
In this embodiment, the angular acceleration data sampled by the detecting means is used. However, in the present invention, the angular acceleration data obtained by differentiating the angular data or the angular velocity data with a differentiator may be used. Good.

The articulated arm robot receives angle command value data angles 1 to 12 for controlling the angles of the actuators 1301 to 13012, which are input and transmitted by a user (operator) from a remote controller (not shown) or the like (step S105). The actually measured angles stored in the storage unit are subtracted from the angle command value data (step S106).
Here, in step S107, it is determined whether or not the subtraction result (angle difference data) obtained by the above processing is an abnormal value. If the angle difference data is an abnormal value, the process proceeds to step S101 (returns), and the angle controller 1400, the angular velocity controllers 1501 to 15012, the angular acceleration controllers 1601 to 16012, the integrator, and the differentiator are turned on again. Initialize and repeat the same process. On the other hand, if the angle command value data is not an abnormal value (normal), the process proceeds to step S108.
In step S108, the actuators 1301 to 13012 and the corresponding angular acceleration controllers 1601 to 16012 and angular velocity controllers 1501 to 15012 are modeled by a first-order lag system and Expression (1), respectively.

次に、式（１）にて得られたモデルに対し、状態方程式（２）を用いて記述し（ステップＳ１０９）、角度制御器１４００を状態フィードバック制御アルゴリズムにしたがい構成する。

Next, the model obtained by the equation (1) is described using the state equation (2) (step S109), and the angle controller 1400 is configured according to the state feedback control algorithm.

前記ステップＳ１０３、Ｓ１０４により得られた、角度指令値データと実測角度データとを、前記状態フィードバック制御器に入力し、出力された角速度指令値データを記憶部に格納する（ステップＳ１０８）。
なお、この処理は、リアルタイムにモデル化を有する場合にのみ行うべき状態フィードバック制御のための処理である。すなわち、リアルタイムにモデル化を有していない場合には、オフラインで前記方法により状態フィードバック制御器を構成しておく。

The angle command value data and the measured angle data obtained in steps S103 and S104 are input to the state feedback controller, and the output angular velocity command value data is stored in the storage unit (step S108).
This process is a process for state feedback control that should be performed only when modeling is performed in real time. That is, when the modeling is not performed in real time, the state feedback controller is configured offline by the above method.

  Here, in step S110, it is determined whether or not the angular velocity command value data (angular velocity data) stored in the storage unit is an abnormal value. When the angular velocity command value data is an abnormal value, the process proceeds to step S101 (returns), and again, the angle controller 1400, the angular velocity controllers 1501 to 15012, the angular acceleration controllers 1601 to 16012, the integrator, and the differentiator. Is initialized and the same processing is repeated. On the other hand, if the angular velocity command value data is not an abnormal value (normal), the process proceeds to step S111.

If the angular velocity command value data is not an abnormal value, that is, if the angular velocity command value data is a normal value, the angular velocity command value data is output. The actual angular velocity data sampled by the ADC is subtracted from the angular velocity command value (step S111), and the subtraction result is input to the angular velocity controllers 1501-1502.
Based on the input subtraction result (angular velocity data), the angular velocity of the levitation body is PID-controlled and an angular acceleration command value is output (step S112).

  Here, in step S113, it is determined whether or not the angular acceleration command value data output in step S112 is an abnormal value. If the angular acceleration command value data is an abnormal value, the process proceeds to step S101 (returns), and again, the angle controller 1400, the angular velocity controllers 1501 to 15012, the angular acceleration controllers 1601 to 16012, the integrator, and the derivative Initialize the vessel and repeat the same process. On the other hand, if the angular velocity command value data is not an abnormal value (normal), the process proceeds to step S114.

If the angular acceleration command value data is not an abnormal value, that is, if the angular acceleration command value data is a normal value, the angular acceleration command value data is output. The actually measured angular acceleration data sampled by the ADC is subtracted from the angular acceleration command value (step S114), and the subtraction result is input to the angular acceleration controllers 1601 to 16012.
Based on the input subtraction result (angular velocity data), the angular acceleration of the actuator is PID-controlled and an angular acceleration difference control signal is output (step S115).
Based on the angular acceleration difference control signal, the articulated arm robot is controlled by outputting a drive command to the corresponding actuator and rotationally driving the actuator (step S116).

Next, the articulated arm robot 10 controlled by the state feedback control device of the present invention will be described with reference to FIGS. The articulated arm robot 10 is given as an example, and the contents of the state feedback control device of the present invention should not be interpreted in a limited manner.
4 and 5 show a configuration of an embodiment of an articulated arm robot 10 to which a state feedback controller including an angular acceleration and angular velocity controller according to the present invention is applied. In FIG. 4, an articulated arm robot 10 includes an upper body 11 as a main body, two arm portions 13L and 13R each having elbow portions 12L and 12R attached to both lower sides of the upper body 11, Hand portions 14L and 14R attached to the lower ends of the arm portions 13L and 13R are included.

  Here, the arm portions 13L and 13R have six joint portions, that is, the joint portions LA1 and RA1 for rotating the shoulder arm in the yaw direction (around the z axis) with respect to the upper body 11 in order from above, and the shoulder rolls. Joints LA2, RA2 in the direction (around the x-axis), joints LA3, RA3 in the shoulder pitch direction (around the y-axis), joints LA4, RA4 in the pitch direction of the elbows 12L, 12R, and the hand 14L , 14R are provided with joint portions LA5 and RA5 in the roll direction of the wrist portion and joint portions LA6 and RA6 in the pitch direction of the wrist portion. Each joint portion LA1, RA1 to LA6, RA6 is composed of a joint driving actuator.

Each joint driving actuator is provided with a rotary encoder for detection.
The wrist joint is composed of joint portions LA5, RA5, LA6, and RA6. Further, the shoulder joint and the elbow joint are connected by upper arm links 21L and 21R, and the elbow joint and the wrist joint are connected by forearm links 22L and 22R. As a result, the left and right arm portions 13L, 13R and the hand portions 14L, 14R of the multi-joint arm robot 10 are each given 6 degrees of freedom, and the 12 joint portions are respectively set at appropriate angles by the drive motor. By controlling the driving, the arm portions 13L and 13R and the hand portions 14L and 14R are given a desired motion, and the posture of the three-dimensional space can be arbitrarily controlled.

FIG. 5 shows an electrical configuration of the articulated arm robot 10 shown in FIG.
In FIG. 4, the multi-joint arm robot 10 includes a position command generation unit 24 that generates a position command value in response to a requested action, and a driving means, that is, each of the joints described above, that is, a joint, based on the position command value. And a control device 30 that controls driving of the driving actuators LA1, RA1 to LA6, RA6. As a coordinate system of the articulated arm robot 10, in FIG. 5, the front-rear direction is the pitch axis direction (forward +), the horizontal direction is the roll axis direction (inward +), and the vertical direction is the yaw axis direction (upward +). Use the xyz coordinate system.

  The control device 30 includes an angle measurement unit 31 and a control unit 32. The angle measurement unit 31 is configured so that each joint drive is performed by inputting angle information of each joint drive actuator from, for example, a rotary encoder provided in the joint drive actuator of each joint portion LA1, RA1 to LA6, RA6. The angle of the actuator is measured and output to the control unit 32.

The control unit 32 includes a state feedback controller (angle controller), an angular velocity controller, and an angular acceleration controller.
The control unit 32 outputs angular acceleration command value data based on the angle data (angle measured value) output from the angle measurement unit 31 and the angle command value from the position command generation unit.
The angular acceleration command value data is output to the corresponding actuators 1301 to 13012.

Next, the configuration of the state feedback controller (angle controller) according to this embodiment will be described.
First, the angular acceleration controllers 1601 to 16012 for controlling the actuators 1301 to 13012 are configured for the joint driving actuators 1301 to 13012 of the joint portions LA1, RA1 to LA6, and RA6, which are control targets, respectively.

Further, angular velocity controllers 1501 to 15012 for controlling the angular acceleration controllers 1601 to 16012 are configured for the angular acceleration controllers 1601 to 16012, respectively.
Note that the angular acceleration controllers 1601 to 16012 and the angular velocity controllers 1501 to 15012 are respectively constituted by feedback controllers.
The configuration of the feedback controller is not particularly limited, but is preferably configured by PID control or fuzzy control. Thereby, a feedback controller can be made into a simple structure.
Next, all of the plurality of angular acceleration feedback controllers and angular velocity feedback controllers including the actuators 1301 to 13012 are modeled in a first order lag system.
The modeled characteristic is described by equation (2), and the angle controller is configured according to the state feedback algorithm.

これにより、複数の制御対象のモデル化を低い次数かつ少ない計算量で簡易に構成することができる。
次に、多関節アームロボット１０に対して、本発明の状態フィードバック制御装置を実際に用いた場合の実験値を示す。図６〜図１１は、初期状態において０（ｒａｄ）を向いている多関節アームロボット１０の各関節部ＬＡ１ないしＬＡ６に、０．５秒の時点で、ヨー軸方向、ロール軸方向、ピッチ軸方向が最適となるような角度指令値を入力した際の、ステップ応答を示す図である。

Thereby, modeling of a plurality of controlled objects can be easily configured with a low order and a small calculation amount.
Next, experimental values when the state feedback control device of the present invention is actually used for the articulated arm robot 10 will be shown. FIGS. 6 to 11 show the joints LA1 to LA6 of the multi-joint arm robot 10 facing 0 (rad) in the initial state at the time of 0.5 seconds, the yaw axis direction, the roll axis direction, and the pitch axis. It is a figure which shows a step response at the time of inputting the angle command value which becomes the optimal direction.

In each figure, (A) shows the step response of the angle (experimental value) of the articulated arm robot 10, (B) shows the step response of the angular velocity (experimental value) of the articulated arm robot 10, and (C ) Shows the step response of the angular acceleration (experimental value) of the articulated arm robot 10.
6 shows LA1, FIG. 7 shows LA2, FIG. 8 shows LA3, FIG. 9 shows LA4, FIG. 10 shows LA5, and FIG. 11 shows LA6.
In these FIGS. 6-11, it turns out that the mutual interference is restrained small and it has converged to the commanded angle rapidly.

As described above, this multi-joint arm robot 10 can change its posture by each joint driving actuator, that is, can control its posture. The state feedback control device of the present invention is applied to this. By applying, the articulated arm robot 10 can be easily moved in an arbitrary direction (an arbitrary angle) with a simpler configuration.
In the present embodiment, the actuator for driving the joint is not particularly limited, and a servo motor, a normal motor, a hydraulic / pneumatic cylinder, a shape memory alloy, or the like may be used. Further, the mechanism for transmitting the driving force of the actuator to each joint is not limited to the example of the present embodiment, and the driving force of the actuator is transmitted in various modes such as via a gear, belt, wire, chain, etc. May be.

In this embodiment, the rotary encoder is used as the actuator detection device. However, the present invention is not limited to this. For example, a hall sensor, an insulation amplifier, a resolver, a tachometer, or the like may be used.
As described above, the state feedback control device of the present invention has been described based on the illustrated embodiments. However, the present invention is not limited to this, and each unit constituting the state feedback control device exhibits the same function. It can be replaced with any possible configuration. Moreover, other arbitrary components may be added to the state feedback control device of the present invention.

It is a block diagram which shows typically the structure of one Embodiment of the state feedback control apparatus of this invention. It is a block diagram which shows typically the structure at the time of applying the state feedback control apparatus shown in FIG. 1 to an articulated arm robot. It is a flowchart which shows the control processing of the state feedback control apparatus shown in FIG. It is a perspective view of an articulated arm robot. FIG. 3 is a perspective view of an articulated arm robot to which the state feedback control device shown in FIG. 2 is applied. It is a figure which shows the step response of the actuator 1 of the articulated arm robot shown in FIG. It is a figure which shows the step response of the actuator 2 of the articulated arm robot shown in FIG. It is a figure which shows the step response of the actuator 3 of the articulated arm robot shown in FIG. It is a figure which shows the step response of the actuator 4 of the articulated arm robot shown in FIG. It is a figure which shows the step response of the actuator 5 of the articulated arm robot shown in FIG. It is a figure which shows the step response of the actuator 6 of the articulated arm robot shown in FIG.

Explanation of symbols

10 ... Articulated arm robot 11 ... Upper body 12L, 12R ... Elbow 13L, 13R ... Arm 14L, 14R ... Hand 15 ... Position command value 21L, 21R ... Upper arm link 22L, 22R ... ... Forearm link 24 ... Position command generation unit 30 ... Control device 31 ... Angle measurement unit 32 ... Control unit 33 ... Angle command generation unit 331 ... Angle command value 1000 ... Position controller 1100 ... Speed control 1101 to 110n …… Speed controller 1200 …… Acceleration controller group 1201 to 120n …… Acceleration controller 1300 …… Control object group 1301 to 130n …… Control object 1400 …… Angle controller 1500 …… Angular speed controller Group 1501-1502: Angular velocity controller 1600: Angular acceleration controller group 1601-160112 ... Angular acceleration controller LA1-L 6 ...... joints, RA1~RA6 ...... joint S101~S116 ...... step

Claims (14)

A plurality of position detecting means for detecting positions of a plurality of control objects;
A plurality of acceleration controllers which are respectively provided corresponding to the plurality of control objects, perform feedback control based on input acceleration data and acceleration command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of acceleration controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of acceleration controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The state controller is configured to perform state feedback control using the first-order lag model. 2. The state feedback control device according to claim 1, wherein the input acceleration data and the input speed data are obtained from position data detected by the position detecting means. A plurality of position detecting means for detecting positions of a plurality of control objects;
A plurality of current controllers that are respectively provided corresponding to the plurality of control objects, perform feedback control based on input current data and current command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of current controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of current controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The state controller is configured to perform state feedback control using the first-order lag model. 4. The state feedback control device according to claim 3, wherein the input current data and the input speed data are obtained from position data detected by the position detection means. A plurality of position detecting means for detecting positions of a plurality of control objects;
A plurality of detection means for detecting physical quantities capable of obtaining acceleration and velocity of the plurality of control objects;
A plurality of acceleration controllers which are respectively provided corresponding to the plurality of control objects, perform feedback control based on input acceleration data and acceleration command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of acceleration controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of acceleration controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The state controller is configured to perform state feedback control using the first-order lag model. The state feedback control apparatus according to claim 5, wherein the plurality of detection units detect acceleration of the control target. A plurality of position detecting means for detecting positions of a plurality of control objects;
A plurality of detection means for detecting a physical quantity capable of determining the current and speed of the plurality of control objects;
A plurality of current controllers that are respectively provided corresponding to the plurality of control objects, perform feedback control based on input current data and current command value data, and output the control objects;
A plurality of speed controllers that are provided corresponding to the plurality of control objects, respectively, perform feedback control based on input speed data and speed command value data, and output to the plurality of current controllers;
Position control for performing feedback control using position data obtained based on the detection result of the position detection means and position command value data set in advance, and outputting the obtained speed command value data to the plurality of speed controllers, respectively. Equipped with
Each of the plurality of control objects, the plurality of current controllers, and the plurality of speed controllers are modeled in a first-order lag system,
The state controller is configured to perform state feedback control using the first-order lag model. The state feedback control device according to claim 5 and 7, wherein the plurality of detection means detect a speed of the control target. The state feedback control apparatus according to claim 7, wherein the plurality of detection units detect the current to be controlled. The characteristics of the speed feedback controller including the control object, the acceleration controller, and the speed controller are represented by the following formula (1):
From the state equation shown in the following equation (2),
The state feedback control apparatus according to any one of claims 1 to 9, which is approximately modeled by the following equation (3).

The state feedback control device according to claim 1, wherein the speed controller is a PID controller or a fuzzy controller. The state feedback control device according to claim 1, wherein the control target is an actuator. The state feedback control device according to any one of claims 1 to 12, which is used for controlling an articulated arm robot including a plurality of joints and a plurality of actuators for driving the joints. The yaw angle, yaw angular velocity, yaw angular acceleration, roll angle, roll angular velocity, roll angular acceleration, pitch angle, pitch angular velocity, and pitch angular acceleration of the articulated arm robot are controlled. State feedback control device.

Images