JP7613738B2 - Control method and control device - Google Patents

Description

The present invention relates to a control method and a control device.

Non-Patent Document 1 discloses an adaptive robot control algorithm consisting of a PD feedback part and a full dynamics feedforward compensation part, which estimates unknown dynamic parameters of the manipulator and the payload online. This algorithm is computationally simple because it effectively utilizes the structure of the manipulator dynamics.

Non-patent document 2 discloses global asymptotic stability of the entire system that requires the transfer function of the linear controller to be positively real.

Non-Patent Document 3 discloses a control framework for torque-controlled humanoid robots that efficiently minimizes tracking error using quadratic programming (QP) formulated as a multi-objective weighted task with constraints.

J. -J. E. Slotine et al., “On the adaptive control of robot manipulators.” The International Journal of Robotics Research, 6(3), 1987. I. D. Landau et al., “Applications of the Passive Systems Approach to the Stability Analysis of Adaptive Controllers for Robot International Journal of Adaptive Control and Signal Processing, 3, 1989. R. Cisneros Limon, M. Benallegue, A. Benallegue, M. Morisawa, H. Audren, P. Gergondet, A. Escande, A. Kheddar, F. Kanehiro, “Robust Humanoid Control Using a QP Solver with Integral Gains.” IROS 2018: pp. 7472-7479.

Traditional torque or current control is susceptible to modeling errors, for example when mass distribution and load loading are incorrect. Therefore, high-gain position control is typically used, which leads to tracking errors in dynamic motion and dangerous conditions for surrounding personnel and the environment, especially when unexpected contact occurs.

Therefore, the present invention has been made in consideration of the above problems, and the object of the present invention is to provide a control method and a control device that can further improve energy efficiency and robustness.

The present invention was made based on the above findings, and its gist is as follows:

[1] A control method comprising the steps of:
Measuring a target process variable to be controlled;
calculating an error value e(t) defined as the difference between a reference value and the process variable;
calculating the manipulated variable u using the sum of the product of a derivative gain _Ks and the derivative of the error value with respect to time s(t), the product of a proportional gain _Ke and the error value e(t), and the product of an integral gain _KE and the integral of the error value with respect to time E(t);
and controlling the target based on the manipulated variable u;
The state vector x(t) defined as equation (1) is expressed as equation (2)

Fulfilling
A method in which t represents time, and α ₀ and α ₁ represent positive numbers.
[2] The control method according to [1], wherein the differential gain _Ks satisfies the formula (3), the proportional gain _Ke satisfies the formula (4), and the product of the integral gain _KE satisfies the formula (5),
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
where λ, μ, and γ are positive scalars, γ is less than one, K _a and L are positive definite matrices, and M is a mass matrix.
[3] The control method according to [1] or [2], wherein the target has n joints, and each of the n joints has one position;
The operation variable u is expressed by the following equation (6):

is defined as
where q is the joint position,

is the joint velocity,

is the joint acceleration, C is the Coriolis matrix, G is the gravity vector of the target, J _i is the Jacobian matrix of joint point i, and F _i is the generalized force at joint point i, where i is a value between 0 and n.
[4] A control device comprising:
a sensor unit configured to sense a target process variable to be controlled;
a memory unit configured to store the reference value to be input;
a calculation unit configured to calculate the manipulated variable u using a sum of a product of a derivative gain _Ks and a derivative s(t) of the error value e(t) with respect to time, a product of a proportional gain _Ke and the error value e(t), and a product of an integral gain _KE and an integral E(t) of the error value with respect to time;
a control unit configured to control the target based on the manipulated variable u;
The state vector x(t) defined as equation (1) is expressed as equation (2)

Fulfilling
where e(t) is defined as the difference between a reference value and a process variable, t represents time, and α ₀ and α ₁ represent positive numbers.
[5] The control device according to [4], wherein the differential gain _Ks satisfies the formula (3), the proportional gain _Ke satisfies the formula (4), and the _KE satisfies the formula (5);
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
where λ, μ, and γ are positive scalars, γ is less than 1, K _a and L are positive definite matrices, and M is a mass matrix.
[6] The control device according to [4] or [5], wherein the target has n joints, and each of the n joints has one position;
The operation variable u is expressed by the following equation (6):

is defined as
where q is the joint position,

is the joint velocity,

is the joint acceleration, C is the Coriolis matrix, G is the gravity vector of the target, J _i is the Jacobian matrix of joint point i, and F _i is the generalized force at joint point i, where i is a value between 0 and n.

As described above, the present invention makes it possible to provide a control method and a control device that can further improve energy efficiency and robustness.

4 is a flowchart showing an example of the flow of a control method according to an embodiment of the present invention. FIG. 2 illustrates components of a control device on which a control device may be mounted according to one embodiment. FIG. 1 shows a robot with joints to be controlled according to an example of the present invention. FIG. 13 illustrates the root mean square (RMS) error of joint velocity (v) in radians/sec and position (q) with respect to time in milliseconds for the perfect model under kinematic feedback and PID based passivity in accordance with an example of the present invention. FIG. 13 illustrates the RMS error of joint velocity (vq) in radians/sec and position (q) with respect to time (ms) for the bias model under kinematic feedback and passivity based PID in accordance with a comparative example of the present invention. FIG. 2 is a block diagram of a controller.

Hereinafter, an embodiment of the present disclosure will be described in detail with reference to the accompanying drawings. In addition, in this specification and the drawings, identical components having substantially the same functions and configurations are designated by the same reference numerals, and duplicated descriptions are omitted.

Fig. 1 is a flowchart showing an example of a flow of a control method according to an embodiment of the present invention. As shown in Fig. 1, the control method according to the embodiment of the present invention is characterized by:
(a) measuring a target process variable to be controlled;
(b) calculating an error value e(t), defined as the difference between a reference value and the process variable;
(c) calculating the manipulated variable u using the sum of the product of a derivative gain _Ks and the derivative of the error value with respect to time s(t), the product of a proportional gain _Ke and the error value e(t), and the product of an integral gain _KE and the integral of the error value with respect to time E(t);
(d) controlling the target based on the manipulated variable u;
The state vector x(t) defined as equation (1) is expressed as equation (2)

Fulfilling
t represents time, and α ₀ and α ₁ represent positive numbers.

(Step of measuring the target process variable to be controlled)
In an embodiment of the present invention, a process variable of a target to be controlled is measured. The process variable and the target are not limited. The target can have n joints. Each of the n joints can have one position. The target can be a robot. A sensor for measuring the process variable of the target can be used as the measurement device. In an embodiment of the present invention, the sensor is a joint position sensor. In addition, the sensor can further include a torque sensor, a pressure sensor, an inertial sensor, or a similar sensor.

(Calculating an error value e(t) defined as the difference between a reference value and the process variable)
In an embodiment of the present invention, the error value e(t) is calculated by using a reference value and a process variable. The reference value is a preset value. The error value e(t) is defined as the difference between the reference value and the process variable.

(Step of calculating the feedback operation variable u)
In an embodiment of the present invention, the feedback manipulated variable u _f is calculated using the sum of the product of the derivative gain K _s and the derivative of the error value with respect to time s(t), the product of the proportional gain K _e and the error value e(t), and the product of the integral gain K _E and the integral of the error value with respect to time E(t). The feedback manipulated variable u _f is calculated using the formula

can be expressed as:

The differential gain _Ks preferably satisfies equation (3), the proportional gain _Ke preferably satisfies equation (4), and the product of the integral gain _KE preferably satisfies equation (5).
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
where λ, μ, and γ are positive scalars, γ is less than one, K _a and L are positive definite matrices, and M is the joint mass matrix.

When the target has n joints, each of the n joints has one position, and the operation variable u is preferably expressed by equation (6).

It is defined as:
where q is the joint position,

is the joint velocity,

is the joint acceleration, C is the Coriolis matrix, G is the gravity vector of the target, J _i is the Jacobian matrix of joint point i, and F _i is the generalized force at joint point i, where i ranges from 0 to n.

(Step of controlling the target based on the manipulated variable u)
In an embodiment of the present invention, the target is controlled based on the manipulated variable u.

According to an embodiment of the present invention, the manipulated variable u comprises the product of the integral gain K _E and the integral of the error value with respect to time E(t), thus allowing the correction of modeling errors at low frequencies and having a safer compliant behavior in case of unexpected contact.

Furthermore, according to an embodiment of the present invention, the state vector x(t) defined as equation (1) is expressed as equation (2)

Fulfilling
t represents time, and _α0 and _α1 represent positive numbers. Therefore, the state vector x(t) has a first-order exponential convergence. For this reason, the convergence proof provides a guarantee of fast convergence.

When a robot has n joints, each has one position represented by a real number. The vector of these joint positions is denoted as q. Each of these joints is actuated by a torque generator. The vector of these torques is represented by τ. One important factor to note is that τ and q are vectors with the same size. The robot is also subjected to external forces at the nodes i, each represented by F _i . The time derivative operator d/dt is expressed as a dot

The quadratic d ² /dt ² is expressed as

is expressed as q,

,

The equations of motion linking , τ, and F _i are called Lagrangian mechanics,

and
The symbols may be defined as follows:
q = q(t): current joint position ^qr = ^qr (t): reference joint position

: Current joint speed

: Reference joint speed

: Current joint acceleration

: Reference joint acceleration M = M(q) : Mass matrix/inertia matrix e = q - q ^r : Joint position error

Coriolis matrix such that dM/dt=C+ ^Ct

: joint velocity error G = G(q): robot gravity vector

: Integral of joint position error J _i = J _i (q): Jacobian matrix of contact point i λ, μ, γ: Positive scalars (γ<1)
F _i =F _i (t): generalized force K _a at tangent point i, L: positive definite matrix

If it is desired that the robot follow a given reference trajectory ^qr that depends on time, then the reference trajectory should be twice differentiable, so that the derivative

and

is also defined. The question is how to generate the torque τ. According to the above reference trajectory, it is difficult to guarantee that q converges to ^qr when t goes to infinity.

but

and when t goes to infinity

but

It is difficult to obtain the desirable property of converging to .

On the other hand, the control method according to the embodiment of the present invention can solve the above-mentioned problems.

Control methods according to embodiments of the present invention include a PID (proportional integral derivative) control method based on passivity, which is the property of a system not to generate energy in excess of the energy introduced by the input sources and external sources.

The energy of the system is given by the formula

It is expressed as
P is the potential gravitational energy. The time derivative of energy is the mechanical power, given by the formula

is given by
Next,

then we obtain the following formula:

In the second section

is added, giving the following formula:

Matrix

is skew-symmetric. This means that any vector

About

means you get.

In this system, passivity

Therefore, the gain matrices K _s , K _e , and K _E can be expressed as follows:
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
This is the formula

Gives.
where these gains are q and

depends on and therefore varies over time.

The control method according to the embodiment of the present invention has Lyapunov stability. Lyapunov stability is one of various types of stability or convergence for a solution of a differential or difference equation describing a dynamical system. When a solution starting from near the equilibrium point xe stays near the equilibrium point _xe forever, the equilibrium point _xe is Lyapunov stable. More precisely, when the equilibrium point _xe is Lyapunov stable and all solutions starting from near the equilibrium point _xe converge to the equilibrium point _xe , _{the equilibrium point xe is} asymptotically stable.

When all solutions starting from any point in the state space converge to the equilibrium point _xe , the equilibrium point _xe is globally asymptotically stable (condition 1).
The solution starting at time t = 0 at any point in the state space R ⁿ is

When there exist two positive numbers _{α 0} and α ₁ such that

There exists a constant positive definite matrix H,

There exist two other positive definite matrices H ₀ (t)>H and H ₁ (t)>H such that the solution starting from any place in the state space R ⁿ has the characteristic

When we have (condition 3), we get the following:

The state x converges to the equilibrium point _xe in a globally exponential manner. Condition 3 is a slightly stronger condition than condition 1 and is called first-order global exponential convergence. The control method according to the embodiment of the present invention preferably satisfies first-order global exponential convergence.

The state vector x is

When defined as
formula

is the formula

is replaced in closed loop dynamics

is obtained,
this is,

Gives.
This is the autonomous state dynamics

Given
I _3×3 is a 3×3 identity matrix and 0 _3×3 is a 3×3 zero matrix.

where this is an autonomous linear dynamics with a time-varying matrix. This means that x _e = (0 0 0) is an equilibrium point of the system. Now, it is important to note that once we have global asymptotic stability of this equilibrium point, it is possible to guarantee that q converges to q ^r as t goes to infinity. Also,

but

and when t goes to infinity

but

It is possible to obtain the desirable property that it converges to

When the values of _Ks , _Ke , and _KE are replaced with the values from equations (3), (4), and (5), respectively:

A constant positive definite matrix H in and

There exist two other positive definite matrices H ₀ (t)>H, H ₁ (t)>H in

and when this satisfies condition 3, this means that condition 2 is also satisfied, which means that the system is globally exponentially stable.

The control method according to the embodiment of the present invention can achieve theoretical proof of first-order exponential convergence in the velocity error, position error, and integral of the position error, where the error converges to zero. The position error is usually simply called the error. When the position error converges to zero, this means that in the case of a perfect system, the tracking is good and the dynamics are stable. When the velocity error converges to zero, it means that the moving object tracking is good and there is no delay. When the integral of the position error converges to zero, it means that there is no steady-state error when there is a small mismatch between the model and reality.

The presence of the integral of the position error allows the system to have no static errors, the steady-state errors being the result of zero-frequency disturbances. Similarly, when there is a low-frequency disturbance, this integral term will also compensate it, since the integral amplifies the low-frequency components. This term allows us to keep a low gain without steady-state errors, allowing more compliance and usually being safer in case of interaction between robots and humans.

Next, a control device according to one embodiment of the present invention will be described in detail with reference to the accompanying drawings.

Figure 2 is a diagram showing components of a control device in which the control device according to this embodiment is installed. As shown in Figure 2, the control device 100 according to this embodiment includes a sensor unit 200, a memory unit 300, a calculation unit 400, and a control unit 500.

(Sensor unit)
The sensor unit 200 according to the embodiment of the present invention senses a target process variable to be controlled. For example, the sensor unit 200 can be realized by a joint position sensor. In addition, the sensor unit 200 can further include a torque sensor, a pressure sensor, an inertial sensor, or a similar sensor.

(Memory Unit)
The memory unit 300 according to an embodiment of the present invention stores the reference value to be input. For example, the memory unit 300 may be realized by a read-only memory (ROM), a random access memory (RAM), a hard disk drive (HDD), a flash memory, or the like.

(Calculation Unit)
The calculation unit 400 according to an embodiment of the present invention calculates the manipulated variable u using the sum of the product of the derivative gain _Ks and the derivative s(t) of the error value e(t) with respect to time, the product of the proportional gain _Ke and the error value e(t), and the product of the integral gain _KE and the integral E(t) of the error value with respect to time. For example, the calculation unit 400 can be an arithmetic logic unit (ALU).

The calculation unit 400 according to an embodiment of the present invention calculates the manipulated variable u such that the derivative gain _Ks satisfies equation (3), the proportional gain _Ke satisfies equation (4), and the product of the integral gain _KE satisfies equation (5).
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
where λ, μ, and γ are positive scalars, γ is less than one, K _a and L are positive definite matrices, and M is a mass matrix.

When the target has n joints, each of the n joints has one position, and the operation variable u is preferably expressed by equation (6).

It is defined as:
where q is the joint position,

is the joint velocity,

A control method is provided in which: j is the joint acceleration, C is the Coriolis matrix, G is the gravity vector of the target, J _i is the Jacobian matrix of joint point i, and F _i is the generalized force at joint point i, where i is a value between 0 and n.

(Control Unit)
The control unit 500 according to the embodiment of the present invention controls a target based on the manipulated variable u. For example, the control unit 500 may be realized by a processor such as a central processing unit (CPU) or a graphics processing unit (GPU) that executes a program (software). In addition, some or all of these components may be realized by hardware (including a circuit unit or circuit) such as a large scale integrated circuit (LSI), an application specific integrated circuit (ASIC), or a field programmable gate array (FPGA), or may be realized by cooperation of software and hardware.

The above described embodiment can be expressed as follows.
A control device including a storage device that stores a program and a processor, the processor being configured to execute the program and execute the following operations: measuring a process variable of a target to be controlled; calculating an error value e(t) defined as a difference between a reference value and the process variable; calculating a manipulated variable u using a sum of a product of a differential gain K _s and a time-dependent differential s(t) of the error value, a product of a proportional gain K _e and the error value e(t), and a product of an integral gain K _E and a time-dependent integral E(t); and controlling the target based on the manipulated variable u, wherein a state vector x(t) defined as equation (1) is expressed by equation (2):

where t represents time, and α ₀ and α ₁ represent positive numbers.

Next, an example of the present invention will be described. The conditions in the examples are one example of conditions adopted to confirm the feasibility and effects of the present invention, but the present invention is not limited to this example of conditions. Various conditions can be adopted in the present invention as long as the object of the present invention is achieved without departing from the gist of the present invention.

Figure 3 shows a robot with joints to be controlled according to an example of the present invention. In the example, the joint torque is

is set to

Therefore, we obtained a torque with the following characteristics: When there are no disturbances and the desired trajectory starts from the current trajectory,

,

, q = ^qr is obtained. Thus, in perfect conditions, the control method according to the embodiment does not degrade the performance of the system. In the presence of disturbances or when the desired trajectory starts from a state different from the current trajectory, convergence of s, e, E to zero is obtained in first-order exponential dynamics. Thus, the system is robust to perturbations. This convergence is quite robust to modeling errors, since it exploits the natural passivity of the system. This formulation is suitable for adaptive control.

Fig. 4 shows the RMS error of joint velocity (vq) in radians/sec and position (q) in radians over time in milliseconds for the perfect model under kinematic feedback and passivity-based PID according to an example of the present invention. Fig. 5 shows the RMS error of joint velocity (vq) in radians/sec and position (q) in radians over time (ms) for the biased model under kinematic feedback and passivity-based PID according to a comparative example of the present invention. Figs. 4 and 5 show the root mean square (RMS) error for joint velocity error ||s|| and joint position error ||e|| over sampled time. The axes are x-axis: sample time [ms], y-axis: joint velocity [rad/s] on the left, joint angle [rad] on the right. The classical PID (kf) signal is in pink and the passivity-based PID signal is in green.
A simulation model of the manipulator arm Sawyer (Rethink Robotics) tracks the desired trajectory. Two different algorithms are tested, the first is a traditional PID control called kinematic feedback (kf) and the second is a passivity-based PID expressed as integral based passivity (ip). Figure 5 shows the RMS error in tracking the desired joint trajectory with respect to the sampled time. In perfect conditions, the errors are both zero as well, as shown in Figure 4.

However, as shown in Figure 5, when modeling errors were added, for example by adding a bias to the torque, the joint error of the conventional controller increased, while the passivity-based one kept the error very close to zero.

As shown in the right of Fig. 5, the control scheme is known from the state of the art (Slotine 1987) (Landau 1989) for PD control, but has never been used for PID control. Fig. 6 is a block diagram of the controller. The reference acceleration generator is the unit that generates the desired position, velocity, and acceleration. The torque/current control is used to generate the acceleration of the motion using the inverse dynamics. Passivity-based PID improves on the usual inverse dynamics by adding correction terms that improve the robustness of the system. As shown in the example, when the system is perfect, there are no errors, so the additional terms are zero and do not affect the inverse dynamics. These terms appear to be corrected only when the system does not behave perfectly like the model. The global exponential convergence ensures that the control is stable and has good performance.

While preferred embodiments of the present invention have been described and illustrated above, it should be understood that these are illustrative of the present invention and should not be considered as limiting. Additions, omissions, substitutions, and other modifications may be made without departing from the spirit or scope of the present invention. Thus, the present invention should not be considered as limited by the foregoing description, but only by the scope of the appended claims. A control device according to an embodiment of the present invention may be a robot with either hydraulic or electric actuators.

According to the present invention, it is possible to provide a control method and a control device that can further improve energy efficiency and robustness. Therefore, the present invention has high industrial applicability.

100 Control device 200 Sensor unit 300 Memory unit 400 Calculation unit 500 Control unit

Claims (6)

1. A control method comprising:
Measuring a target process variable to be controlled;
calculating an error value e(t) defined as the difference between a reference value and the process variable;
calculating a manipulated variable u using the sum of a product of a derivative gain _Ks and a derivative of the error value with respect to time s(t), a product of a proportional gain _Ke and the error value e(t), and a product of an integral gain _KE and an integral of the error value with respect to time E(t);
controlling the target based on the manipulated variable u;
The state vector x(t) defined as equation (1) is expressed as equation (2)

Fulfilling
A control method in which t represents time, and α ₀ and α ₁ represent positive numbers. The differential gain K _s satisfies equation (3), the proportional gain K _e satisfies equation (4), and the product of the integral gain K _E satisfies equation (5),
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
2. The control method of claim 1, wherein λ, μ, and γ are positive scalars, γ is less than 1, K _a and L are positive definite matrices, and M is a mass matrix. the target has n joints, each of the n joints having a position;
The operation variable u is expressed by the following equation (6):

is defined as
where q is the joint position,

is the joint velocity,

3. The control method according to claim 1 or 2, wherein: j is the joint acceleration, C is the Coriolis matrix, G is the gravity vector of the target, J _i is the Jacobian matrix of joint point i, and F _i is the generalized force at joint point i, where i is a value between 0 and n. A control device,
a sensor unit configured to sense a target process variable to be controlled;
a memory unit configured to store the reference value to be input;
a calculation unit configured to calculate a manipulated variable u using a sum of a product of a derivative gain _Ks and a derivative s(t) of an error value e(t) with respect to time, a product of a proportional gain _Ke and said error value e(t), and a product of an integral gain _KE and an integral E(t) of said error value with respect to time;
a control unit configured to control the target based on the manipulated variable u;
The state vector x(t) defined as equation (1) is expressed as equation (2)

Fulfilling
A control device, wherein e(t) is defined as the difference between said reference value and said process variable, where t represents time, and α ₀ and α ₁ represent positive numbers. The differential gain K _s satisfies Equation (3), the proportional gain K _e satisfies Equation (4), and K _E satisfies Equation (5),
K _s =λM+K _a +C Formula (3)
K _e =μM+λK _a +L+λC Formula (4)
K _E =μK _a +γλL+μC Formula (5)
The controller of claim 4 , wherein λ, μ, and γ are positive scalars, γ is less than 1, K _a and L are positive definite matrices, and M is a mass matrix. the target has n joints, each of the n joints having a position;
The operation variable u is expressed by the following equation (6):

is defined as
where q is the joint position,

is the joint velocity,

6. The control device according to claim 4 or 5, wherein: j is the joint acceleration, C is the Coriolis matrix, G is the gravity vector of the target, J _i is the Jacobian matrix of joint point i, and F _i is the generalized force at joint point i, where i is a value between 0 and n.

Images