JPH0426990B2 - - Google Patents

Description

[Detailed Description of the Invention] [Table of Contents] Overview Industrial Application Fields Conventional Technology Problems to be Solved by the Invention Means for Solving the Problems (Fig. 1) Working Example (a) One Implementation Overall explanation of the example (Figures 2, 3, 4)
(b) Explanation of the configuration of the control unit (Figures 5, 6, 7)
(Fig. 8, Fig. 9) (c) Explanation of the operation of the control unit (d) Explanation of other embodiments (Fig. 10, Fig. 11) Effects of the invention [Summary] Moving means for moving a moving object A mobile body control device that servo-controls a mobile body, comprising: a plurality of state detection means for detecting external operating environment conditions of the mobile body;
By providing a servo control means for servo-controlling the moving means by taking the difference between the outputs of the plurality of state detection means as state quantities and the movement command amount, the outputs of the plurality of state detection means can be fed back to the control system as state quantities. The movement is controlled by

[Industrial application field]

The present invention relates to a mobile body control device that controls the movement of a mobile body, and more particularly to a mobile body control device that can control the movement of a mobile body by incorporating the outputs of a plurality of external environmental state detection means.

2. Description of the Related Art Mobile object control devices that control the movement (speed control, position control) of a mobile object (moving object) are widely used. For example, in a robot, a hand, which is an end effector, is used as a moving body and performs desired work by controlling movement such as position control.

In such movement control, an internal servo sensor is used to detect the current, speed, position, etc. of the actuator (e.g. motor) that is the movement means, and the feedback of the state quantity of the internal servo sensor is used to control the drive. Movement is controlled to stabilize the system.

In other words, the servo internal sensor is an essential sensor for the servo system, and it constantly outputs a signal while the servo system is operating.In order to ensure stable system operation, the output signal is fed back to the servo system. This is a sensor that has been

On the other hand, an external sensor is a sensor whose output signal does not affect the stability of the servo system in any way, and the servo system does not become unstable even if the signal is cut off.

Control using only internal sensors only has the function of enabling stable servo control, so if the moving object is restricted by the environmental conditions in the moving space, it cannot be controlled according to the conditions, and it may cause collisions with objects or Position errors occur. Therefore, it is necessary to perform state adaptive control in which the external operating environment state (the state of the environment around the moving object in the space in which the moving object operates) is detected and reflected in the movement control.

[Conventional technology]

In order to perform such external state control, an external state detection sensor such as visual or tactile sense is generally provided to detect the operating environment of the robot, and adaptive control is performed according to the output of the sensor. There is.

For example, the publication ``Introduction to Robotics Engineering'' (1978)
In the content of pages 73 to 81 of the magazine "Nikkei Mechanical 1985.4.8 issue" (published by Nikkei McGraw-Hill on April 8, 1985), such adaptive control is described. There are various ways to do this.

In such conventional technology, the control side constantly monitors the output of the external state sensor, determines the environmental state, determines whether to perform control according to the state quantity, and changes the movement command accordingly. It is used.

That is, the external state sensor is placed outside the loop of the movement control system, and the movement command is changed by determining the output state of the sensor to perform state-adaptive control. This is sometimes called semi-closed loop control.

[Problem that the invention seeks to solve]

However, in such conventional technology, since the process of judgment by the control unit is involved, it is necessary to process at extremely high speed in order to perform real-time processing, and therefore the burden on the control unit is heavy, and the There was a problem that state quantities could not be reflected in real time.

In particular, as operations have become more sophisticated in recent years, it has become necessary to process multiple sensors, sensors with different properties, in combination, and it has become difficult for conventional control techniques to handle this in real time. For example, if a plurality of sensors feed back signals at the same time, processing becomes impossible and the only way to deal with it is by selecting information from one of the sensors.

An object of the present invention is to provide a mobile object control device that can incorporate signals from a plurality of external state sensors into a control system and process them in real time.

[Means for solving problems]

FIG. 1 is a diagram explaining the principle of the present invention.

Figure 1A is a configuration diagram of the present invention, Figure 1B,
C is an explanatory diagram thereof.

In FIG. 1A, M is a moving body, such as a robot arm or hand, and MT is a moving means, which moves the moving body M, such as motors, NS1 to NSn. are environmental state sensors that detect the environmental state of the moving body M and generate state outputs PS1 to PSn, and CT is a control means (servo control unit) that detects the movement command amount.
Status output PS of PC and environmental status sensors NS1 to NSn
1 to PSn, the moving means MT is servo-controlled.

That is, the present invention includes a moving means MT for moving a moving body M, and a plurality of state detection means NS1 to NSn for detecting the external operating environment state of the moving body M.
and a servo control means CT that servo-controls the moving means MT based on a movement command amount, the servo control means CT is provided with outputs of the movement command amount and the plurality of state detection means NS1 to NSn. Provide a difference calculation means for calculating the difference between the state quantity and the state quantity,
The present invention is characterized in that servo control is performed using the output from the difference calculation means as a new movement command amount.

Therefore, in the present invention, a plurality of state detection means NS
A system is constructed in which the state outputs PS1 to PSn of 1 to NSn are taken as state quantities and are incorporated into the servo control system so as to take the difference from the movement command amount PC.

[Effect]

In the present invention, the movement of a moving device (hereinafter referred to as a robot) including a moving body M and a moving means MT is controlled in the following manner.

That is, regarding a position control system that stably realizes high rigidity and high-speed response in a robot, let's consider one in which the moving means MT is composed of a DC motor.

It is assumed that the characteristics of one axis of a robot driven by a DC motor are expressed by the following voltage equation and motion equation.

v=R・i+L・i+Bl・x〓……(1) f=Bl・i……(2) f=M・x¨+D・x〓+Ff……(3) However, v: Between the terminals of the DC motor Voltage R: DC resistance of DC motor i: Current of DC motor L: Inductance of DC motor Bl: Force constant of DC motor f: Force generated by DC motor M: Mass of moving part D: Viscous braking coefficient of moving part Ff: Frictional force of the moving part x: Displacement of the moving part s: Laplace operator The control block diagram when controlling the position of a robot with the characteristics of equations (1), (2), and (3) is shown in Figure 1B. becomes.

Expressing this as a transfer function, the transfer function of the displacement X(s) of the robot to the displacement command P(s) of the control system shown in FIG. 1B is as follows.

X(s)/P(s)=A _P・Bl・k ₃ /a ₃・s ³ +a ₂・s ² +a ₁
・s+a ₀ ...(4) However, a ₀ =A _P・Bl・k ₃ a ₁ =A _P・Bl・k ₂ +A _P・k ₁・D+R・D+Bl ² a ₂ =A _P・k ₁・M+L・D+R・M a ₃ =L・M A _P : Operational amplifier open loop gain P : Displacement command k ₁ : Current feedback gain k ₂ : Speed feedback gain k ₃ : Displacement feedback gain Here, A _P = ∞ (generally, A _P is large, 100 to 120 db) and modifying equation (4): X(s)/P(s) = Bl・k ₃ /k ₁ /M・s ² + ( Bl・k ₂ /k ₁ +D)・s+Bl・k ₃
/k ₁ ...(5).

From equation (5), the robot is equivalent to a moving object in which a spring SP and a dowel DP are connected to external restraints as shown in FIG. 1C.

That is, it is a moving object whose stiffness K is (Bl·k ₃ /k ₁ ) and whose viscous damping coefficient is (Bl·k ₂ /k ₁ +D).

Therefore, by increasing _K3 and stabilizing _K1 and _K2, it is possible to stably control the position of a highly rigid and responsive robot.

On the other hand, in the present invention, the state sensor as shown in FIG.
Detected state quantities PS1 (s) to PSn (s) of NS1 to NSn
has a control system that is synthesized with the command P(s), so the control block diagram in Figure 1B shows the control system as shown by the arrow.
Since PS1(s) to PSn(s) are added, the transfer function of equation (5) is X(s)/P(s)-(α1・PS1X(s)+
α2・PS2X (s) +……+αn・PSnX (s)) = Bl・k ₃ /k ₁ /M・S ² + (Bl・k ₂ /k ₁ +
D) S+Bl・k ₃ /k ₁ ...(6).

Therefore, even if the outputs of a plurality of sensors are incorporated into the control system as state variables, the stability of the system is maintained.

From equation (6), the force F(s) generated by the motor is
is, F(s)={P(s)-(α1・PS1X(s)+… +αn・PSnX(s))−X(s)}・Bl・k ₃ /k ₁
...(7) becomes. However, α1...αn are gains.

Therefore, when no signal is generated from any sensor, the generated force F(s) becomes zero and the motor stops in the state of X(s)=P(s) (8).

On the other hand, when the mth sensor generates a signal, the force F(s) is: F(s) = {P(s) − αm・PSmX(s) −X(s)}・Bl・K ₃ /K ₁ ...(9) Then, it will either stop at the position of X(s) = P(s) - αm・PSmX(s) ...(10) or contact with the force of F(s).

Therefore, in the present invention, state quantities of a plurality of sensors can be incorporated into a control system, and state adaptive control can be performed autonomously without complicated control. For example, it is possible to autonomously avoid obstacles or make a soft landing toward an object based on sensor output. By controlling the values of the gains α1 .

Also, as is clear from equation (7), the linearity of the sensor,
Since nonlinearity does not affect the stability of the system, there is also the advantage that the type of sensor does not matter. That is,
Proximity switches, contact switches, analog outputs, sensors, non-contact displacement meters, etc. can all be used.

〔Example〕

(a) Overall description of one embodiment.

FIG. 2 is an overall configuration diagram of an embodiment of the present invention,
This example uses a 4-axis orthogonal robot.

In the figure, 1 is the base of the robot body, which has an X-axis drive source (motor) 11 that drives an arm to be described later in the arrow A (X-axis) direction, and 2 is the first arm. It has a Z-axis drive source (motor) 12 that drives a second arm (described later) in the direction of arrow B (Z-axis), and the X-axis drive source 1 of the base 1
1 moves in the X-axis direction, 3 is a second arm, and arrow C
Y-axis drive source (motor) 1 that drives in the (Y-axis) direction
3, and moves in the Y-axis direction.

4 is the θ-axis rotation part, and the θ-axis drive source (motor)
14, which is supported by the second arm 3 and rotates a hand to be described later about the θ-axis; 5 is a hand that grips the article and is attached to the θ-axis rotation unit 4; It is.

Therefore, the robot's hand 5 is positioned three-dimensionally on the X, Y, and Z axes, and rotated about the θ axis.

In this embodiment, the arms 2 and 3, the rotating part 4, and the hand 5 correspond to the moving body M in FIG. 1, and the motors 11, 12, 13, and 14 correspond to the moving means MT.

6 is a control unit, which, as will be _described later in FIG _.
~V〓, current position X _P ~γ _P motor 11 of each axis
- 14, and synthesizes the state quantities from each state sensor (7a to 7c described later) and the displacement commands X _C , Y _C , Z _C , θ _C of each axis,
Reference numeral 7a denotes an ultrasonic sensor, which is installed at the tip of the arm 3 and emits ultrasonic waves in three-dimensional (X, Y, Z) directions, receives the reflected waves, and measures the distance to an object in the three-dimensional direction. 7b, which is a well-known distance sensor for measuring and outputs outputs PSX1 to PSZ1 according to the distance of the hand 5 to an external object in the X, Y, and Z directions;
is a force sensor, which is installed between the hand 5 and the θ-axis rotation unit 4, is composed of a parallel plate spring and a strain gauge, and detects external forces applied to the hand 5 in the X, Y, Z, and θ directions individually. 7CX is a limit sensor that outputs the outputs PSX2 to PSθ2, which is composed of a photoelectric sensor that is a nonlinear sensor, and is installed at both ends of the X-axis movement limit position. 20 is an interrupter and the first arm 2
The limit sensor 7CX detects the limit sensor 7CX.

Note that only the X-axis of the limit sensor is shown, but
A pair of limit sensors 7 are also provided for the Y-axis, Z-axis, and θ-axis.
CX, 7CZ, 7Cθ are provided, and limit signals PSY3, PSZ3,
Output PSθ3 to the control section.

Therefore, as environmental state sensors, an ultrasonic sensor 7a (non-contact sensor) that measures the distance between the hand 5 and an object in the moving space, and a force detection sensor 7b (contact sensor) that detects the external force of the hand 5 from the object. and limit sensors 7CX to 7Cθ (non-contact sensors or contact sensors) for detecting the movement limit position of the hand 5.

FIG. 3 is an explanatory diagram of the operation of an embodiment of the configuration shown in FIG. Although FIG. 3 only shows the operation on the X axis, the same applies to the other axes.

As shown in Figure 3B, the first arm 2 moves from P ₀ to P ₁
It is commanded to move along the X axis until
Assume that the limit sensor 7CX is provided at the limit position of _P2 , and the obstacle 8 is located at the position of _P3 , as shown in FIG.

The speed command in this case is V _CX in FIG. 3B, and therefore, the first arm 2 is moved by the X-axis motor 11 in the direction of the arrow (limit sensor 7X1). Along with this movement, the ultrasonic sensor 7a measures the distance to the obstacle 8 on the X axis, and outputs an X axis output.
Emit PSX1. Therefore, in the control unit 6, the X-axis command speed V _CX is subtracted by α·PSX1, which is the X-axis output PSX1 multiplied by the gain α1, and as shown in FIG. 3C,
Since the X-axis motor 11 is controlled using (V _CX -α1·PSX1) as the command speed, the X-axis speed decreases and the movement continues. Due to this movement, the first arm 2 reaches the limit sensor 7CX, and the interrupter 2
0 crosses the photoelectric switch of the limit sensor 7CX, a detection signal PSX3 is generated from the limit sensor 7CX, and the control section 6 multiplies the detection signal PSX3 by a gain α3 to further set the command speed as (V _CX −α
1.PSX1-α3.PSX3), and drives and controls the X-axis motor 11. Therefore, the resultant speed command becomes zero (or has the opposite polarity) as shown in Figure 3C, and the first
arm 2 makes an emergency stop at position _P2 . If the polarity is reversed, the limit sensor will vibrate. The gains α1 and α3 change the output characteristics of the ultrasonic sensor 7a and limit sensor 7CX,
In an emergency stop, the gain α3 is set so that α3・PSX3≧V _C.

Therefore, the presence of the obstacle 8 causes the vehicle to decelerate and stop at the left end position of the limit sensor 7CX, thereby avoiding a collision with the obstacle 8.

The same applies to the left end limit sensor 7CX side in FIG. 2, and the same applies to the other Y, Z, and θ axes.

In this way, by capturing the output of the state sensor as a state quantity (energy) and incorporating it into the control system, it becomes possible to autonomously switch from movement mode to deceleration and emergency stop mode within the control system.

FIG. 4 is an explanatory diagram of another embodiment of the configuration shown in FIG. 2. This example shows a case where the obstacle 8 is present in front (on the left side) of the limit sensor 7CX with respect to the first arm 2.

As in Figure 3, the X-axis will be explained, and Figure 3B
Assume that the first arm 2 is commanded to move along the X axis from P ₀ to P ₁ as shown in FIG.

When the first arm 2 starts moving at a speed V _CX ,
Distance detection output from the ultrasonic sensor 7a to the obstacle 8
PSX1 is issued, and the commanded speed is the same as above.
As shown in Figure C, it becomes (V _CX -α1・PSX1) and starts to decelerate. While decelerating in this way,
When the hand 5 contacts the obstacle 8 at position _P3 , the force sensor 7b is deflected in the X-axis direction, and the strain gauge of the force sensor 7b emits an X-axis external force detection signal PSX2.
Since the feedback is sent back to the control unit 6, the speed command from point _P3 is (V _CX −α1・PSX1−α2・PSX
2), it suddenly goes to zero and stops at position _P4 .

At this time, the hand 5 comes into contact with the obstacle 8 with a contact force corresponding to the displacement (deflection) of the force sensor 3.

Furthermore, if the Y-axis is also moving, it is possible to bypass the obstacle 8 and move to the target position.

Furthermore, the same applies when there is an obstacle on the Y-axis or Z-axis side.

Next, when the detection signal PSX2 of the force sensor 3 is subjected to dead zone processing with a dead zone width W in the control unit 6, the composite input from the force sensor 3 becomes α2・(PSX2−W), and the contact position is as shown in FIG. 4B. The composite input is zero from _P3 to position _P5 , and therefore, from position _P5 , where the force sensor 7b is further displaced by the dead zone width W, the speed rapidly decreases to zero and stops at position _P6 .

_{The contact} force F at this time is as follows _{, where} K is the stiffness.

(P ₃ - P ₅ ) is determined by the dead band width W, and (P ₅
-P ₆ ) is determined by the gain α2, and the contact force can be appropriately controlled by these. When α2 is set to infinity, (P ₅ −P ₆ )≈0, and the contact force can be controlled by the dead zone.

(b) Description of the configuration of the control unit.

FIG. 5 is a block diagram of the control section 6 in the configuration shown in FIG.

60 is a processor (hereinafter referred to as CPU),
A device that issues displacement commands X _C , Y _C , Z _C , and θ _C for each axis according to teaching data (stored in a memory not shown);
61, 62, 63, and 64 are X, Z, Y, and θ-axis servo controllers, respectively, which handle displacement commands X _C , Y _C , Z _C , and θ _C and sensor signal processing, as detailed in Fig. 9. The motors 11 to 14 of each axis are servo-controlled by combining the state quantities SX, SY, SZ, and SC from the section (described later), and 65 is a sensor signal processing section, which will be explained in detail in Fig. 6 and below. Similarly, each status sensor 7a to 7
Detection signals PS1 to PSn from n are subjected to dead band processing and multiplied by a gain to be output to each servo controller 61.
~64. 110 to 140 are current detectors, respectively, and the motor 11 of the corresponding axis
111 to 141 are tacho generators (speed detectors) that detect the actual current _I Detects the speed _V
Those with speed feedback from 1 to 64, 112
~142 is an encoder, which is composed of a rotary encoder, and is connected to the motors 11 to 14 of the corresponding axis.
In order to detect the position of
It is used to input position feedback. Note that these are internal sensors of the servo system.
66 is a data bus, which connects the CPU 60, each servo controller 61 to 64, and the sensor signal processing section 6.
An address bus 67 is used for exchanging data with the CPU 60 and sends addresses to the servo controllers 61 to 64 and the sensor signal processing section 65.

Therefore, the CPU 60 selects the sensor signal processing unit 65 via the address bus 67 and selects the sensor signal processing section 65 via the data bus 66.
, set the dead band width W and gain α, select the servo controllers 61 to 64, and connect the data bus 6
6 through current feedback gain k ₁ , speed feedback gain k ₂ ,
The position feedback gain _k3 is set, the servo controllers 61 to 64 of the required axes are selected via the address bus 67 according to the teaching data, and the displacement command is sent to the servo controllers 61 to 64 via the data bus 66.
Set X _C , Y _C , Z _C , and θ _C . In addition, the current position X _P of each axis from each servo controller 61 to 64,
Y _P , Z _P , and θ _P are read via the data bus 66.

Each servo controller 61 to 64 commands displacement
By combining X _C , Y _C , Z _C , θ _C and the state quantities SX, SY, SZ, S θ of each axis from the sensor signal processing unit 65, the motors 11 to 14 of the corresponding axes are servo-controlled.

FIG. 5 is a detailed circuit diagram of the sensor signal processing section 65 configured in FIG. 4.

In the figure, the same parts as those shown in FIG. The detection outputs PS1 to PSn of 7a to 7n are decomposed into components PSX1 to PSθ1, . As shown, it is provided corresponding to each component decomposition section 65a1 to 65an, and performs nonlinear processing called a dead zone on each axis component of each component decomposition section and outputs it, and the absolute value of the input is the dead zone width setting value. When it is smaller than W, the output is set to zero. On the other hand, when the absolute value of the input is larger than the dead band width setting value W, when the input is a positive value, (input - dead band width setting value)
is the output, and when the input is a negative value, (input + dead band width setting value)
The output is

Therefore, for example, the dead zone processing section 65b1 processes each axis component PSX1', PSY1', PSY1',
Output PSZ1' and PSθ1'.

65X to 65θ are X-axis, Y-axis, Z-axis, and θ-axis sensor addition units, respectively, and as detailed in FIG. 8,
From the dead zone processing units 65b1 to 65bn, the X-axis components PSX1' to PSXn', the Y-axis components PSY1' to PSYn',
Z-axis component PSZ1'~PSZn', θ-axis component PSθ1'~
Receive PSθn′ and corresponding gains αX1 to αXn, αY
1 to αYn, αZ1 to αZn, and αθ1 to αθn are multiplied and then added (synthesized), and the state quantities SX to Sθ are output to the corresponding servo controllers 61 to 64.

FIG. 7 is an internal configuration diagram of the dead zone processing section in the configuration of FIG. 6.

Although only the dead zone processing section 65b1 is shown in the figure, the other dead zone processing sections 65b2 to 65bn also have the same configuration.

In the figure, 65bX to 65bθ are respectively dead zone processing parts 6
This is a dead zone processing section for the X, Y, Z, and θ axes of 5b1, and the dead zone width WX1 to Wθ from the data bus 66.
1 is set and input sensor component PSX1
〜PSθ1 is subjected to the above-mentioned dead band processing, and the component
It outputs PSX1' to PSθ1'.

650 is an analog latch circuit, which, as detailed in FIG. 9, has a chip select section connected to the address bus 67, a data register connected to the data bus 66, and the contents of the data register according to the chip select of the chip select section. It has a latch section that latches the latching section, and a digital/analog converter (hereinafter referred to as a D/A converter) that converts the digital output of the latch section into an analog quantity. It latches, converts to analog dead band width, and outputs it. 651 and 652 are inverting amplifiers, respectively. The inverting amplifier 651 inverts the polarity of the dead band width WX1 of the analog latch circuit 650 and outputs it to the summing amplifier 653 (described later). The inverting amplifier 652 inverts the output of the inverting amplifier 651. and restore it to the original state, adding amplifier 654 (described later)
653 and 654 are summing amplifiers that add the component signal PSX1 of the sensor and the input dead band widths WX1 and -WX1, and 655 is a polarity determination unit that outputs the sensor component
Determine the polarity of PSX1 and instruct UP according to the polarity.
Those that output DOWN instructions, 656a, 656
b are analog switches, respectively, which connect the outputs of the summing amplifiers 653 and 65S to the polarity determining section 655.
It outputs in response to UP/DOWN instructions.

In addition, the Y-axis, Z-axis, and θ-axis dead zone processing parts 65bY,
65bZ and 65bθ also have the same configuration.

The operation of this dead zone processing section 65bX is based on the input component
The polarity of the PSX is determined by the polarity determining section 655, and the UP/
Generates DOWN (negative/positive) output.

In addition, the input PSX1 is connected to each adding amplifier 653, 65.
4 is added to the dead band width -WX1, WX1, and -
(PSX1-WX1), -(PSX1+WX1) are output. With the above UP/DOWN instruction, output PSX1
When PSX1 is positive, the analog switch 656b is selected, and when the output PSX1 is negative, the analog switch 656a is selected, so if PSX is positive,
(-PSX+W), if PSX is negative, (PSX-W) is X
It is output as the axis component PSX1'. This dead band processing prevents the output from being output if the output of the state sensor is less than the dead band width, thereby preventing the generation of the state quantity SX due to minute outputs due to characteristic errors of the state sensor, etc.
This allows stable external state feedback operation to be performed even if the output characteristics of the state sensor vary. Further, a desired contact force is applied to the contact sensor (force sensor 7b in FIG. 2) at the time of contact.

FIG. 8 is an internal configuration diagram of the sensor addition section configured in FIG. 6.

Note that although only the X-axis sensor addition section 65X is shown in the figure, other Y-axis, Z-axis, and θ-axis sensor addition sections 6
5Y, 65Z, and 65θ also have the same configuration.

In the figure, 65X1 to 65Xn are each sensor 7a.
~7n, and the outputs PSX1'~ of dead zone processing units 65b1~65bn, respectively.
PSXn' is multiplied by gains αX1 to αXn to output gain multiplication outputs G1 to Gn.

657 is an analog latch circuit, which has the same configuration as the analog latch circuit 650 of the dead zone processing section 65bX, which latches the gain αX1 from the CPU 60 and outputs it as an analog quantity; 658 is an analog multiplier; Output of each analog switch 656a, 656b of processing unit 65bX
RX is an output resistor that multiplies PSX1' by gain αX1 of analog latch circuit 657 and outputs the result.

Note that the other gain multipliers 65X2 to 65Xn have the same configuration, and each output G2 (αX2・PSX
2′)~Gn(αXn・PSXn′) is generated.

65Xm is an addition section, and each gain multiplication section 65
It generates the X-axis state quantity SX by adding the outputs G1 to Gn of X1 to 65Xn, and AMP is the addition amplifier, which adds each output G1 to Gn and outputs it. 659a is the voltage/frequency Converter (V/
(referred to as an F converter), and a summing amplifier AMP
A pulse with a frequency corresponding to the absolute value of the addition output (voltage) of is output as the X-axis state quantity SX,
659b is a polarity determination section, which is an addition amplifier AMP
Determine the polarity of the addition output (G1+...+Gn),
It outputs UP/DOWN commands to the servo controller depending on positive/negative.

FIG. 9 is a detailed circuit diagram of the servo controller.

Although only the X-axis servo controller 61 is shown in FIG. 9, the other Y, Z, and θ-axis servo controllers 62 to 64 have the same configuration.

In the figure, the same parts as those shown in FIG.
11, 612, data registers 614, 615, 616 connected to the data bus 66, and data registers 614, 615, 61 according to select instructions from the chip select sections 610, 611, 612.
latch parts 617, 618, which latch the contents of 6;
619, and a D/A that converts the contents (digital values) of the latch sections 617, 618, and 619 into analog quantities.
The analog latch circuit 61I latches a current feedback gain _k1 and outputs it as an analog quantity, and the analog latch circuit 61V latches a speed feedback gain _K2 and outputs it as an analog quantity. The position feedback gain _K3 is latched in the analog latch circuit 61P and output as an analog quantity.

61a is an encoder processing circuit that determines the rotation direction from the two-phase position pulses from the encoder 112 and outputs four times as many position pulses;
b is a current position counter, which counts up or down the position pulse according to the determined rotation direction of the encoder processing circuit 61a, and indicates the X-axis current position _XP ; 61c is a D/A converter; 61M is a displacement command generation circuit that converts the current position _XP of 61b into an analog current position _XP , and a chip select section 613 connected to an address bus 67,
and a latch unit 620 that latches the contents of the data register 625 in response to a selection instruction from the chip select unit 613.
and a pulse generator 624 which outputs displacement command pulses _XC with a number corresponding to the contents of the latch section 620 and a cycle corresponding to the maximum acceleration,
Latch the displacement command X _C instructed by the CPU 60,
The number of displacement command pulses X _C corresponding to the value of the displacement command is generated at the period of maximum acceleration.

61g is a pulse addition circuit which switches and outputs the displacement command pulse _XC to the up side or down side according to the polarity of the displacement command _XC set in the latch circuit 620, and also outputs the displacement command pulse XC to the X axis of the sensor signal processing section 65. State quantity (pulse) SX from sensor addition unit 65X
UP/DOWN of the polarity determination section 659b (Fig. 8)
61h is a command position counter that switches output to the up side or down side according to the instruction.
The up-side pulses of the pulse addition circuit 61g are up-counted and the down-side pulses are down-counted to create a composite command position _X'C , which is then D/A converted. 6d, 61e, 61f, 6
1i is an analog multiplier, and the multiplier 61d multiplies the actual current _IX from the current detector 110 by the current feedback gain _k1 of the analog latch circuit 61I.
The multiplier 61e receives the actual speed from the speed detector 111.
The multiplier 61f multiplies the current position X _{P of the D/A converter 61 by the position feedback gain k 3 of} the _analog latch circuit 61P. 61i is for multiplying the composite command position _X'C of the command position counter 61h by the position feedback gain _k3 of the analog latch circuit 61P, R1 to _R4 are addition resistors, and the multipliers 61d, 61e, 61f,
This is to add the outputs of 61i. Therefore, the addition output U is (k ₃・X′ _C −k ₃・X _P −k ₂・V _X −k ₁・
_IX ), and shows the format shown in Figure 1B.

61h is a power amplifier section, which includes an amplifier AMP that amplifies the addition input U and a pair of transistors.
If the addition input U is positive, the transistor TR2 is turned on and the drive current ib is transferred to the motor 1.
1, and if the addition input U is negative, the transistor TR
1 is turned on, and a drive current ia in the pole direction is caused to flow through the motor 11.

(c) Description of the operation of the control section.

The CPU 60 first inputs current, speed, and position feedback gains k ₁ , k ₂ , and k ₃ to each analog latch circuit 61.
I, 61V, and 61P are selected by the address bus 67, sent out from the data bus 66, and latched. In addition, dead band widths WX1 to Wθ1, .
..., WXn ~ Wθn and gain αX1 ~ αXn, ...
..., αθ1 to αθn are set.

Next, the CPU 60 latches the displacement command X _C via the data bus 66 to the displacement command generation circuit 61M. As a result, the pulse generator 62
4 to maximum acceleration frequency and displacement command X _C
A displacement command pulse X _C is output according to the displacement command X C, and according to the direction (direction or direction) of the displacement command X _C ,
Up side of pulse adder circuit 61g (in case)
Or it is output from the down side (in case) to the command position counter 61h. Command position counter 61
Since h counts up the pulses from the up side and down counts the pulses from the down side, if the displacement command X _C is a direction, the displacement command pulse X _C is counted up.

Therefore, the addition input U(X) is (k ₃・X′ _C −
k ₃・X _P −k ₂・V _X −k ₁・_I to drive the X-axis in the direction.

During this time, while the X-axis state quantity SX is not issued from the sensor signal processing unit 65, the X-axis motor 11 is servo-controlled in accordance with the displacement command _XC .

On the other hand, in the sensor signal processing unit 65, the X, Y, Z, and θ-axis components of each state sensor 7a to 7n are decomposed by component decomposition units 65a1 to 65an, and dead band processing is performed by dead band processing units 65b1 to 65bn, and each The axial sensor addition units 65X to 65θ combine (add) the coaxial components.

Therefore, the dead band width corresponding to the X-axis components PSX1 to PSXn of any of the status sensors 7a to 7n
When WX1 to WXn are exceeded, the X-axis state quantity SX is generated from the X-axis sensor addition section 65X.

For example, when the signal PSX1 is generated from the ultrasonic sensor 7a as shown in FIG. 3B, αX1・(PSX1~WX
A state quantity SX corresponding to 1) is generated and input to the pulse addition circuit 61g of the servo controller 61,
With the UP/DOWN output of the polarity determining section 659b, in this case, a DOWN instruction, the state quantity pulse SX is output to the down side, thus causing the command position counter 61h to count down.

Therefore, the composite displacement command _X'C of the command position counter 61h decreases, so that the addition input U(X) decreases or becomes negative, and the motor 11 is decelerated.

Furthermore, when the signal PSX3 is generated from the limit sensor CX, the state quantity SX further increases αX3・(PSX3−
WX3) increases, the composite displacement command _X'C further decreases, and the motor 11 stops.

Similarly, for the Y, Z, and θ axes, each servo controller 62, 63, and 6S controls the Y, Z, and θ axes of the sensor signal processing section.
Deceleration and stopping are performed according to the Z and θ-axis state quantities SY, SZ, and Sθ.

In this way, the detected energies of the plurality of sensors are combined and taken into the control system, and motor control, that is, movement control, is performed according to the outputs of the sensors.
If a sensor for measuring the temperature of the heat source is used as the state sensor, it is possible to move without approaching the heat source, and if a rimia area is used, smooth deceleration is possible.

(d) Description of other embodiments.

FIG. 10 is an explanatory diagram of another embodiment of the present invention.

In FIG. 10A, the same objects as those shown in FIG. , is fitted into the hole 91.

This example shows assembly (fitting) work performed by a robot. As shown in FIG. 10B, as the hand 5 descends in the Z-axis direction at a speed V _CZ and approaches the object 9, the ultrasonic sensor 7a outputs a signal PSZ1, so the Z-axis speed is (V _CZ −αZ・PCZ
1) to decelerate. At position _ZP1 , when the article 90 of the hand 5 contacts the tapered surface of the hole 91 of the object 9 as shown in FIG. C) occurs, which drives the Y-axis in the direction in which the external force is zero, and thus the article 90 is fitted along the inner wall of the hole 91.

That is, even if there is a relative positional shift with respect to the hole 91 in the Y-axis direction, this can be absorbed.

Moreover, since the Z-axis direction is decelerated with respect to the object 9 at this time, it is possible to make soft contact, absorb positional deviation, and prevent damage to the object 9 and the article 90 due to contact.

Therefore, position control (in the case of zero sensor output), proximity control, and force control coexist, and the control mode is determined autonomously depending on the external state.

By changing the gains αZ1 and αZ2, these degrees can be selected as appropriate depending on the work content, object, or article.

FIG. 11 is an explanatory diagram of another embodiment of the present invention.

In the figure, reference numeral 92 denotes an object to be copied by a robot.

First, a command value for the movement path of the robot tip is set inside the object 92 like CP. Then, after confirming the contact between the robot and the object 92, the movement route is sequentially instructed. In this way, the robot cannot penetrate inside the object 92, so it moves along the periphery of the object 92 as shown by the dotted line.

According to this method, it is possible to easily generate the movement route CP during tracing work, which has been considered difficult in the past, and
Since there is almost no burden on the software, tracing work can be done in real time.

As mentioned above, the ultrasonic sensor 7a and the force sensor 7b
Depending on the gain setting, contact copying work and non-contact copying work can be selected and executed.

In the above example, a 4-axis orthogonal robot was used as an example, but the number of axes is not limited to 4, and may be one or multiple axes, and an articulated rotating coordinate robot may also be used. It may be. Also, not only robots,
The present invention can be applied to any moving body, and may be a rotating body such as a θ-axis.

In the above example, a DC motor is used as the moving means, but other actuators such as a pulse motor or an AC motor may be used.

In addition, although ultrasonic sensors and photoelectric sensors have been explained as examples of non-contact sensors, other well-known sensors such as laser distance meters and temperature sensors may also be used, and various sensors may be used depending on the required control. However, it may not be provided on the moving body itself, but may be provided at another observation point.

Although the present invention has been described above with reference to examples, the present invention can be modified in various ways according to the gist of the present invention.
These are not excluded from the present invention.

〔Effect of the invention〕

As explained above, according to the present invention, the following effects are achieved.

The servo control means is provided with a difference calculation means, the environmental state detection signals of the plurality of state detection means are used as state quantities, and the signals can be incorporated into the servo control system for servo control, so there is no need to judge the signals of the plurality of state detection means. The control mode can also be changed autonomously and in real time depending on the operating environment.

Even if signals from a plurality of state detection means occur simultaneously, they can be processed in real time.

Since the outputs of the plurality of state detection means are used as state quantities and can be incorporated into the servo control system for servo control, the load on the control unit can be reduced, adaptive control to the operating environment can be performed, and movement control can be performed with high performance and high intelligence. I can do it.

[Brief explanation of drawings]

Fig. 1 is an explanatory diagram of the principle of the present invention, Fig. 2 is an overall configuration diagram of an embodiment of the present invention, Figs. 3 and 4 are explanatory diagrams of the operation of the configuration shown in Fig. 2, and Fig. 5 is the configuration shown in Fig. 2. 6 is a detailed circuit diagram of the sensor signal processing section configured in FIG. 5, FIG. 7 is an internal configuration diagram of the dead zone processing section configured in FIG. 6, and FIG. 8 is a detailed circuit diagram of the sensor signal processing section configured in FIG. 6. 9 is a detailed circuit diagram of the servo controller configured in FIG. 5, FIG. 10 is an explanatory diagram of another embodiment of the present invention, and FIG. 11 is another embodiment of the present invention. It is an explanatory diagram. In the diagram, M...Moving object, MT...Moving means, NS
1~NSn...state detection means, CT...control means.

Claims (1)

[Claims] 1. A moving means for moving a moving body; a plurality of state detection means for detecting external operating environment conditions of the moving body; and servo control for servo-controlling the moving means based on a movement command amount. a moving body control device having a means for controlling a moving body, wherein the servo control means is provided with a difference calculation means for calculating the difference between the movement command amount and the state amount obtained by the outputs of the plurality of state detection means, and the output from the difference calculation means is A moving object control device characterized by performing servo control as a new movement command amount.