JPH1058368A - Manipulator control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To control the motion and angle of a joint with high reliability and high accuracy by estimating the fluctuation of a dynamic characteristic with high accuracy. SOLUTION: The control gains of the second and the third PID controllers 13 and 15 for applying torque to the second and the third joints of a manipulator, as well as the control gain of a PID controller 17 for applying torque to a space navigation body, are evaluated by means of fuzzy inference based on a manipulator joint angle θ 3, so as to be set to an optimum value. In addition, the operation of respective joints is controlled, according to the control gains preliminarily established, when the adaptability of the gain adjusting fuzzy rule to the value reduces or the result of the inference is unsuitable.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator control device suitable for operation control of an articulated manipulator constructed in, for example, outer space and used for various operations.

[0002]

2. Description of the Related Art As is well known, in the field of space development, a work spacecraft is equipped with an articulated manipulator, and the spacecraft travels through space to perform various operations such as construction of a space station. There is a plan to carry out the work. As means for controlling the joint angle of such a multi-joint manipulator, a method of setting a target value for the joint angle and controlling the joint angle in accordance with the target value is known.

[0003] However, when a manipulator is mounted on a spacecraft navigating in outer space, when controlling the operation of moving the tip position of the manipulator to a fixed point, the position of the spacecraft during the operation is controlled. -The posture is changed. Therefore, in the manipulator mounted on the spacecraft, if the target trajectory of the joint angle during the operation period is determined in advance before starting the operation control, even if the joint angle completely follows the target value during the actual operation, However, there is a risk that the position of the leading end of the spacecraft may deviate from a target fixed point due to a change in the position / posture of the spacecraft.

When a manipulator mounted on a spacecraft grasps an object whose mass is unknown (working object) during the operation, the dynamic characteristics of the manipulator / space navigation system fluctuate greatly. For this reason, as a means for ensuring high-precision control performance, an algorithm that estimates fluctuations in the dynamic characteristics of the manipulator / space navigation system and performs control in consideration of the results is required.

However, in the above-mentioned method for obtaining dynamic characteristics based on a mathematical model of a manipulator / space navigation system,
Since the modeling error of the mathematical model has a large effect on the estimation result, there is a problem that reliability is poor.

Therefore, it has been considered that a known control response (state of a control target) is evaluated by fuzzy inference without using a mathematical model to estimate dynamic characteristic fluctuation. However, in the method of determining the operation amount of the manipulator based on the fuzzy inference, when a control response does not conform to any fuzzy rule at a certain time during the control operation (or when the degree of conformity is low), a fuzzy The output of the inference, that is, the manipulated variable becomes an indefinite or inappropriate value, and there is a possibility that stable operation control becomes difficult.

[0007] The problem of such fuzzy inference is that the more the controlled object becomes complicated such as an articulated manipulator and the size of the state space that must be evaluated by fuzzy rules increases, the more the entire state space is appropriately evaluated. It becomes difficult to design fuzzy rules that can be used, and the difficulty of stable operation control is increased.

[0008]

As described above, in the conventional manipulator control device, it is difficult to accurately estimate the dynamic characteristic fluctuation of the manipulator / space navigation system.
There is a problem that it is difficult to perform highly reliable and accurate operation control.

SUMMARY OF THE INVENTION The present invention has been made in view of the above circumstances, and has been made to be able to estimate highly accurate dynamic characteristic fluctuations and to realize highly reliable and highly accurate joint angle operation control. An object is to provide a manipulator control device.

[0010]

According to the present invention, there is provided a manipulator mounted on a spacecraft, target position generating means for generating a target tip position of the tip of the manipulator in time series based on command information, Target joint angle generating means for calculating a target joint angle of a joint based on the target tip position generated by the target position generating means and the center of gravity of the spacecraft, and the spacecraft based on the attitude angle of the spacecraft. First control means for calculating a torque to be applied; second control means for calculating torque to be applied to the manipulator based on a target joint angle of the joint generated by the target joint angle generation means and the joint angle of the joint; A gay for setting each control gain by evaluating control gains of the first and second control means by fuzzy inference based on joint angles of joints of the manipulator. It is obtained by constituting the manipulator control device and a regulating means.

According to the above configuration, the first and second control means for applying torque to the manipulator and the spacecraft are inferred by the gain adjustment means by fuzzy inference based on the joint angle of the manipulator. The control gain is evaluated and the control gain is set to an optimal value, and the control gain is set to a preset control gain value in a state where the degree of conformity of the fuzzy rule is low or the inference result is indefinite. This ensures reliable control of joint angles according to the dynamic characteristics of manipulators and space navigation systems, and enables stable output of fuzzy inference to be obtained, achieving highly safe operation control. .

[0012]

Embodiments of the present invention will be described below in detail with reference to the drawings. FIG. 1 shows a manipulator control device according to an embodiment of the present invention. Here, a manipulator system includes a shoulder joint (first joint), an elbow joint (second joint), and a wrist joint (third joint).
3 shows a case in which a three-joint type manipulator is mounted on a spacecraft. However, FIG. 1 illustrates a case where the shoulder joint, which is the first joint of the manipulator, is fixed.

That is, the target tip position generator 10 includes:
The target joint angle generator 11 is connected, generates target tip position information assuming an operation of moving the manipulator tip in only one direction based on a command signal from a command control unit (not shown), and generates a target joint angle. Output to the container 11.

The target tip position generator 10 controls the manipulator tip so that the manipulator tip moves at a speed shown in FIG. 2 with respect to a preset maximum speed Vmax and maximum acceleration Amax of the manipulator tip. Each time r during operation
The signals are sequentially generated at 1, r2, and r3 and output to the target joint angle generator 11.

The target joint angle generator 11 receives information on the position of the center of gravity of the spacecraft from the manipulator / space navigation system 12, and outputs the information on the position of the center of gravity and the target tip position generator 10.
Kinematics is solved based on the target tip position information from the robot and the angle information θ of the second and third joints of the manipulator
2ref and θ2ref are calculated.

The target joint angle generator 11 has a second PID (Proption / Integral / D).
An optional controller 13 is connected via an adder 14. The adder 14 receives the joint angle θ2 of the second joint from the manipulator / space navigation system 12, adds the joint angle θ2 and the angle information θ2ref, and outputs the result to the second PID controller 13. The second PID controller 13 has a control gain value set in advance, and calculates a torque α2 to be given to the second joint of the manipulator based on the input joint angle information.

[0017]

(Equation 1) And the second joint is driven and controlled in accordance with the torque α2.

Here, e 2 is the angle following error of the second joint,
Kp2, Ki2, and Kd2 are control gains for proportional, integral, and differential operations with respect to the angle following error of the second joint of the manipulator.

The target joint angle generator 11 includes:
Third PID (Proption / Integra)
l · Differential) controller 15 is the adder 1
6 are connected. The adder 16 receives the joint angle θ3 of the third joint of the manipulator from the manipulator / space navigation system 12, adds the joint angle θ3 and the angle information θ3ref, and outputs the result to the third PID controller 15. The third PID controller 15 has a control gain value set in advance, and outputs a torque α3 to be applied to the third joint of the manipulator based on the input joint angle information.

[0020]

(Equation 2) The third joint is driven and controlled in accordance with the torque α3.

Here, e 3, the angle following error of the second joint,
Kp3, Ki3, and Kd3 are control gains for proportional, integral, and differential operations with respect to the angle following error of the third joint of the manipulator.

The manipulator / space navigation system 12 has an attitude control PID (Proportion I
(integral / Differential) controller 1
7 is connected. The PID controller 17 controls the attitude angle ψ of the spacecraft from the manipulator / spacecraft 12.
Is input, and the torque α0 to be given to the spacecraft based on the attitude angle ψ is

[0023]

(Equation 3) The attitude of the manipulator / space navigation system 12 is driven based on the torque α0 to control the attitude of the spacecraft.

Here, Kp0, Ki0, and Kd0 are control gains for proportional, integral, and differential operations with respect to the attitude angle ψ of the spacecraft, respectively. Further, a gain adjuster 18 is connected to the second and third PID controllers 13 and 15 and the PID controller 17. The gain adjuster 18 includes the third manipulator from the manipulator / space navigation system 12.
The joint angle θ3 of the joint is input, and fuzzy inference is executed based on the joint angle θ3.

The gain adjuster 18 evaluates the joint angle θ3 of the third joint of the manipulator immediately after the start of the control operation using a fuzzy rule (hereinafter referred to as a gain adjusting fuzzy rule), and performs second and third PID control. Vessels 13, 15
And the control gains Kp, Ki, and Kd of the PID controller 17 are adjusted.

When the joint angle θ3 of the third joint of the manipulator immediately after the start of the control operation is input in a time series r1, r2, r3 as shown in FIG. 3, the gain adjuster 18 receives the time series r1, r2, Let r1 be the size of R1: if r1 is B1 and r2 isB2 and r3 isB3 ten Kp0 = Kp01, Ki0 = Ki01, Kd0 = Kd01 Kp2 = Kp21, Ki2 = Ki21, Kd2 = Kd21 Kp3 = KpK = 31 Kd31 R2: if r1 is M1 and r2 isM2 and r3 isM3 ten Kp0 = Kp02, Ki0 = Ki02, Kd0 = Kd02 Kp2 = Kp22, Ki2 = Ki22, Kd2 = Kd22 Kp3 = Kp32, Ki3 = Kp32, Ki3 = Kp32, Ki3 r1 is S1 and r2 isS2 and r3 isS3 ten Kp0 = Kp03, Ki0 = Ki03, Kd0 = Kd03 Kp2 = Kp23, Ki2 = Ki23, Kd2 = Kd23 Kp3 = Kp33, Ki3 = Ki33, K33 = K33 The second and third PID controllers 13, 15, P
Each gain adjustment is performed by obtaining each control gain of the ID controller 17.

The antecedent part (if part) includes a time series r1
, R2, r3, is a proposition for estimating dynamic characteristics using fuzzy labels B, M, S for evaluating the magnitude of the joint angle θ3.

The fuzzy labels B, M, and S are represented by Big, M
4 shows Edium and Small, which are defined by the membership functions shown in FIG. And the consequent part (the
The n part) is a proposition for estimating appropriate control gains Kp, Ki, and kd for the dynamic characteristics described in the antecedent part (if part).

The gain adjustment fuzzy rule states that "if the displacement of the joint angle θ3 is large, it is determined that the dynamic characteristics greatly fluctuate due to the mass of the object gripped by the manipulator. If it is small, it is determined that the fluctuation of the dynamic characteristic due to the mass of the object gripped by the manipulator is small, and the optimal control gains Kp, Ki, and Kd according to the respective displacement states are used. "

That is, the gain adjuster 18 inputs according to the gain adjustment fuzzy rule, obtains the membership function values μs1,... ΜB3 of the fuzzy labels S1,..., B3 for r1, r2, and r3, and a1 = μS1 · μS2 μS3 a2 = μM1 μM2 μM3 a3 = Calculate the fitness a1, a2, and a3 of each rule by a well-known inference method called the algebraic product, addition, and height method expressed by the formula of μB1, μB2, μB3. I do.

Next, the gain adjuster 18 determines the fitness a 1,
The control gain K which is weighted and finally applied to the consequent part (then part) of each rule by a2 and a3
p ′, Ki ′, and Kd ′ are calculated based on the following equation (4), and the second and third PID controllers 13 and
15. The control gain of the PID controller 17 is variably set.

[0032]

(Equation 4)

In the above configuration, the gain adjuster 18
Based on the joint angle θ3 of the third joint of the manipulator of the manipulator / space navigation system 12, the fuzzy inference is executed as described above, and the control gain Kp ′ corresponding to the dynamic characteristic,
The second and third PID controllers 1 determine Ki ', kd'.
3, 15, the control gain of the PID controller 17 is variably set.

Here, the PID controller 17 obtains the torque applied to the spacecraft according to the control gain set by the gain adjuster 18 and controls the attitude of the spacecraft as described above. At the same time, the second and third PID controllers 1
As described above, the third and third motors obtain the torque applied to the second and third joints of the manipulator, and drive-control the second and third joints to move the manipulator tip position to the target point.

Then, during the operation control of the manipulator, at a predetermined time, the degree of conformity to all the fuzzy adjustment rules becomes low, and a situation occurs in which the estimation result becomes insufficient or inconsistent. Then, the gain adjuster 18 controls the control gains Kp ', Ki',
The output of Kd 'to the PID controller 17, the second and third PID controllers 13, 15 is stopped.

Here, the PID controller 17 executes the attitude control of the spacecraft according to the control gain set in advance as described above. At the same time, the second and third P
The ID controllers 13 and 15 drive and control the second and third joints of the manipulator according to the control gain set in advance as described above, and the operation control of the manipulator is continued.

As described above, the manipulator control device includes the second and third PID controllers 13 and 15 for applying torque to the second and third joints of the manipulator, and the PID controllers for applying torque to the spacecraft. The control gain of the controller 17 is evaluated by fuzzy inference based on the joint angle θ3 of the manipulator, the control gain is set to an optimum value, the conformity of the gain adjustment fuzzy rule becomes low, or the inference result becomes incompatible. In the state, the operation is controlled in accordance with the control gain set in advance.

According to this, the control gains of the second and third PID controllers 13 and 15 and the PID controller 17 are set in accordance with the dynamic characteristics of the manipulator / space navigation system 12, thereby enabling space navigation. Highly reliable manipulator joint angle control is realized without being affected by body posture fluctuations, and even if fuzzy inference is inconsistent or the degree of conformity is low, safety is ensured. Highly stable operation control is realized.

In the above embodiment, the first to third
Although the case where the present invention is applied to a manipulator system having a joint has been described, the present invention is not limited to this, and can be applied to manipulator systems of various types having a joint.

Further, in the above embodiment, the case where the first to third joints of the manipulator are applied to a fixed operation mode has been described. However, the present invention is not limited to this. , And substantially the same effects are expected in any of the operation modes.

Further, in the above-described embodiment, the case has been described in which the gain adjuster 18 is configured to execute the fuzzy inference of the control gain based on the joint angle θ3 of the third joint of the manipulator. Without
It is also possible to perform a fuzzy inference based on the joint angles of the other joints of the manipulator to adjust the control gain. Therefore, it is needless to say that the present invention is not limited to the above-described embodiment, but can be variously modified without departing from the gist of the present invention.

[0042]

As described above in detail, according to the present invention, highly accurate dynamic characteristic fluctuation can be estimated, and highly reliable and accurate joint angle operation control can be realized. Manipulator control device can be provided.

[Brief description of the drawings]

FIG. 1 is a diagram showing a manipulator control device according to an embodiment of the present invention.

FIG. 2 is a view for explaining a method of setting a target tip position in FIG. 1;

FIG. 3 is a view shown for explaining a time series of a joint angle of a third joint in FIG. 1;

FIG. 4 is a diagram shown to explain the gain adjustment fuzzy rule of FIG. 1;

[Explanation of symbols]

 10. Target tip position generator. 11 Target joint angle generator. 12. Manipulator / space navigation system. 13 ... second PID controller. 14, 16 ... adder, 15 ... third PID controller. 17 ... PID controller. 18 Gain adjuster.

Claims (4)

[Claims] 1. A manipulator mounted on a spacecraft, target position generating means for generating a target tip position of the tip of the manipulator in a time series based on command information, and a target generated by the target position generating means. Target joint angle generation means for calculating a target joint angle of a joint based on the tip position and the position of the center of gravity of the spacecraft; and a first calculating means for calculating torque to be applied to the spacecraft based on the attitude angle of the spacecraft. Control means; second control means for calculating a torque applied to the manipulator based on a target joint angle of the joint generated by the target joint angle generation means and the joint angle of the joint; and the first and second controls Gain adjusting means for evaluating the control gain of the means by fuzzy inference based on the joint angles of the joints of the articulated manipulator and setting each control gain. Manipulator control device. 2. The manipulator control device according to claim 1, wherein the manipulator has multiple joints. 3. The manipulator control device according to claim 2, wherein said gain adjusting means evaluates a control response by fuzzy inference based on a joint angle of a predetermined joint of the manipulator. 4. The manipulator control device according to claim 3, wherein said gain adjusting means evaluates a control gain by fuzzy inference based on a joint angle of a joint closest to a tip portion of said manipulator.