JPS61265287A - Controller for moving body - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

[Detailed description of the invention] [Table of contents] Overview Industrial field of application Conventional technology Problems to be solved by the invention Means for solving the problems (Fig. 1) Working example (a) One embodiment Overall explanation (Fig. 2, Fig. 3, Fig. 4) (b) Explanation of the configuration of the control section (Fig. 5, Fig. 6, Fig. 7, Fig. 8, Fig. 9) (C)
Description of the operation of the control unit (d) Description of other embodiments (Figs. 10 and 11) Effects of the invention [Summary] In a mobile body control device that controls the movement of a mobile body by a moving means, the operation of the mobile body is By providing a plurality of state detection means for detecting environmental states and a control means for controlling the movement means by combining the outputs of the plurality of state detection means with a movement command as a state quantity, it is possible to detect multiple states. The output of the means is used as energy and incorporated into the control system to control movement.

[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.

Mobile object control devices that control the movement of mobile objects are widely used. For example, in a robot, a desired task is executed by controlling the movement of a hand, which is an end effector.

In such movement control, an internal servo sensor is used that detects the current, speed, position, etc. of the actuator (e.g. motor) that is the movement means, and the state quantity of the internal servo sensor is fed back to control the drive. 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, and is used to stabilize the system. This is a sensor whose output signal is fed back to the servo system for operation.

On the other hand, an external sensor is a sensor whose output signal does not have the same effect on the stability of the servo system, and the servo system will not become unstable even if the signal is cut off. Control using only internal sensors is , since it only has a function that enables stable servo control, when a moving body is restrained by the environmental conditions in the moving space, it is not possible to control according to the conditions, resulting in collisions with objects and position errors. Therefore, it is necessary to perform state adaptive control that detects the external environmental state and reflects it in movement control. [Prior Art] In order to perform such external state control, it is generally necessary to detect the operating environment of the robot. For example, in the publication ``Introduction to Robotics'' (September 10, 1980), external state detection sensors such as visual and tactile sensors are installed and adaptive control is performed according to the output of the sensors. published by Ohmsha) and the magazine “Nikkei Mechanical 198
5, No. 4.8 J (published by Nikkei McGraw-Hill on April 8, 1985), pages 73 to 81, there are various examples of such adaptive control.

In such conventional technology, a method is used in which 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 based on this. ing.

That is, the external state sensor is assumed to be 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 loose control.

[Problem that the invention seeks to solve]

However, in such conventional technology, since the processing 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 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 in combination with sensors with different properties, 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, it is impossible to process them, and the problem arises that the only way to deal with the problem is to select 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.

The first doujin is a configuration diagram of the present invention, and is shown in Figure 1 a3).

(q is the theory oIl1 diagram.

In Figure 1, 9M is a moving body, 9 such as a robot arm or hand, and MT is a moving means, which moves the moving body M. 1For example, it is composed of a motor,
NSI to NSn are environmental state sensors, and
by detecting the environmental conditions of.

CT is a control means (control unit) that generates the status outputs PS1 to PSn, and the movement command PC and the environmental status sensor N
The movement of the moving means MT is controlled by combining the state outputs Psi to PSn of SI to NSn.

Therefore, in the present invention, environmental condition sensors N81 to NSN
The state outputs PS1 to PSn are taken as state quantities, and these are synthesized and combined with the movement command PC to form a system incorporated into the control system.

[For production]

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 us consider one in which the moving means MT is composed of a DC motor.

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

・ v=R* i +L@i +Bj@x ・・・・・・
・・・・・・・・・・・・・・・・・・(1) f=Bj−i
・・・・・・・・・・・・・・・
・・・・・・・・・(2) f=M@x+D・x + Ff
・・・・・・・・・・・・・・・・・・・・・
(3) However.

V: Voltage between terminals of DC motor R: DC resistance i of DC motor: Current L of DC motor: Inductance Bl of DC motor: Force constant f of DC motor: Force generated by DC motor M: Mass of moving part D: Viscous braking coefficient Ff of the moving part: Frictional force of the moving part The block diagram is shown in FIG. 1(B).

If this is expressed as a transfer function, the transfer function of the displacement X (S) of the robot to the displacement command P (S) K of the control system, represented by CB in FIG. 1, is of the ninth order.

however.

a6== Ap-Bl * k3 a, :A, * Bl @ k, +A, a k, # D+
R@D + Bl”a, = A, *to, *M+LsD
+RAMa, = L a M AP: Open loop gain of operational amplifier P: Displacement command 1: Current feedback gain 2: Speed feedback gain 3: Displacement feedback gain Here, Ap = oo (generally Ap is 100 ~120db
and a dog) and transform equation (4).

becomes.

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

That is, it is a moving object with a stiffness K of (Bj 11 km, /)cl) and a viscous damping coefficient of (Bl·ni, /ni, +D).

Therefore, by increasing ks and stabilizing it by +kis k2, it is possible to stably control the position of a robot with high rigidity and good responsiveness.

On the other hand, in the present invention, like the first prisoner, the state sensors N81 to N
Detection state quantity P 81 (S) to P 8 n (S
) is a control system synthesized with the command P (s), so the control block diagram in Figure 1 (CB) shows the control system as shown by the arrow (Psi
(S) ~ PSn (s) is added, so
The transfer function of equation (5) is:

X(s) P(S)-(al・PS 1)Qs)+a2-PS
2) Qs)+ ·-+an-PSn)Qs)).

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

From the sixth equation, the force F (S) generated by the motor is.

F(s): (P(S)-(α1-PSl)QS)+=
・+αn−PSn)Qs)) −X(s) )・Bj
- to 3/to + ・・・・・・・・・・・・・・・
......(7). However, α1...αn are gains.

Therefore, when no signal is generated from any sensor.

X(s) = P(s)...
In the state of (8), the generated force F (s) becomes zero and it stops.

On the other hand, when the 2mth sensor generates a signal, the force F (s
)teeth.

F(S)=(P(s)-αm-PSmX(s)-X
(s)] −B no ka/kt −・・・・−(9
)tona.

X(s) 2P(s) −αm−PSmX(S)
...Turn around...--It will stop at the position (if), or it will come into contact with the force of F (s).

Therefore, in the present invention, the state quantities of a plurality of sensors are incorporated into the 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 on an object using the sensor output. By controlling the values of the gains α1, .

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 switch, contact switch, analog output, sensor,
Any non-contact displacement meter can be used.

〔Example〕

(a) Overall description of one embodiment.

FIG. 2 is an overall configuration diagram of an embodiment of the present invention.

This is an example of a 4-axis Cartesian robot.

In the figure, 1 is the base of the robot body.

It has an X-axis drive source (motor) 11 that drives an arm (described later) in the direction of arrow A (X-axis), 2 is the first arm, and the second arm (described later) is driven in the direction of arrow B (Z-axis). It has a two-axis drive source (motor) 12 that drives in the direction of the base 1.
3, which moves in the X-axis direction by the X-axis motive source 11 of
is the second arm, and with respect to the first arm 2, arrow C
It has a Y-axis drive source (motor) 13 that drives in the (Y-axis) direction.

It moves in the Y-axis direction.

Reference numeral 4 denotes a θ-axis rotation unit, which has a reel, a θ-axis drive source (motor) 14, is supported by the second arm 3, and rotates a hand to be described later about the θ-axis; 5 is a hand; It is attached to the θ-axis rotating section 4.

Therefore, the robot hand 5 is positioned three-dimensionally on the x, y, and z axes, and rotated around the θ axis.

In this embodiment, the arm 2, 39 rotating part 4° hand 5 corresponds to the moving body MK in FIG. 1, and its motors 11, 12
, 13.14 correspond to the moving means MT.

Reference numeral 6 is a control unit, which receives displacement commands and actual currents from internal sensors (encoders, etc.) for each axis, as described later in FIG. ~IT +Actual velocity (Actual angular velocity) Vx~■r rCurrent position X? ~γP servo drives the motors 11 to 14 of each axis, and each state sensor (described later in 7a)
~7c) State quantities and displacement commands for each axis xc, yc, z
c, which takes the composition with θC.

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 is a force sensor that outputs outputs P8X1 to P8Z1 according to the distance between the hand 5 and an external object in the X, Y, and Z directions. It is provided between the rotating part 4 and is composed of a parallel plate spring and a strain gauge.

It individually detects the external forces applied to the hand 5 in the X, Y, Z, and θ directions, and outputs outputs FSX2 to PSθ2.

Reference numeral 70X denotes a limit sensor, which is composed of a photoelectric sensor that is a nonlinear sensor, and is provided at both ends of the X-axis movement limit position, and generates a limit signal PSX3 by an interrupter to be described later.

20 is an interrupter provided on the first arm 2;
This is detected by limit sensor 7CXK.

Note that only the Y-axis limit sensor is shown.

A pair of limit sensors 7CY, 7 are also provided for the Y-axis, Z-axis, and θ-axis.
CZ, 7Cθ are provided, and limit signals P8Y3 . PSZ3°PSθ3 is output 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. , limit sensors 7CX to 7Cθ (non-contact sensors or contact sensors) for detecting the movement limit position of the hand 5 are provided.

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

As shown in Fig. 3 CB), the first arm 2 is commanded to move along the Y axis from Pa to B, and the third rotation is as follows.
A limit sensor 7CX is provided at the limit position of +P
Assume that there is an obstacle 8 at position S.

In this case, the speed command is Vex as shown in Figure 3 (CB).

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 Y axis, and emits an X axis output P8X1. Therefore, in the control unit 6, the X-axis command speed vcx is subtracted by α・PSXl, which is the X-axis output PSXI multiplied by the gain α1, and (■cx−α1・PSXl) is converted to the command speed as shown in FIG. 3 (Q). To and X
Since the axis motor 11 is controlled, the X-axis speed decreases and the movement continues. Due to this movement), when the first arm 2 reaches the limit sensor 7CX and the interrupter 20 crosses the photoelectric switch of the limit sensor 7CX, a detection signal P8X3 is generated from the limit sensor 7CX.

The control unit 6 multiplies the detection signal PSX3 by a gain α3.

Furthermore, the command speed (vcx-α1・PSXl-α3・PS
X3) and drives and controls the X-axis motor 11. Therefore, one composite speed command becomes zero (or has the opposite polarity) as shown in FIG. 30, and the first arm 2 comes to an emergency stop at position P2. If the polarity is reversed, the limit sensor section will vibrate. This gain α
1. α3 is the ultrasonic sensor 7a and limit sensor 7CX
It changes the output characteristics of α in an emergency stop.
3. Set the gain α3 so that P8X3≧VC.

Therefore, due to the presence of the obstacle 8, there is no need to decelerate and stop at the left end position of the limit sensor 7CX.
Collision with the obstacle 8 can be avoided.

The same is true for the left end/W IJ sensor 70X side of FIG. 2, and also for 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 7 #X with respect to the first arm 2.

Similar to FIG. 3, the Y-axis will be explained, and it is assumed that the first arm 2 is instructed to move along the Y-axis from the box to Pl as shown in FIG. 3 (5).

When the first arm 2 starts moving at a speed vcx.

Distance detection output PSX between ultrasonic sensor 7a and obstacle 8
1 is issued, and the commanded speed is as shown in Figure 3 (Q) as described above.
(VC, -α1・PSXl') and starts decelerating.

While decelerating in this manner, when the hand 5 comes into contact with the obstacle 8 at position Ps, 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 it is fed back to the control unit 6, the speed command from one point P3 is (Vex-α1-PSXI-α
2・PSX2) Tonashi suddenly heads towards zero and moves to position P4
Stop at.

At this time, the hand 5 comes into contact with the obstacle 8 with a contact force corresponding to the displacement (deflection) K 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 control unit 6 performs dead zone processing with a dead zone width W on the detection signal PSX2 of the force sensor 3, the combined input by the force sensor 3 becomes α2-(PSX2-W).

As shown in FIG. 4 (G3), the combined input is zero from the contact position P to the position P6, and therefore, from position P6, 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 assumed to be stiffness K.

F=K・((P, -P%) + < P, -pm month.

(Pj-PI) is determined by the dead band width W, and (P.

−几) is determined by the gain α2, and the contact force can be appropriately controlled by these. If α2 is infinite, then (”
5-Ps) 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).

Teaching data (stored in memory, not shown) K Therefore, displacement commands Xc, Yc, zc, θC for each axis are issued.
, 62, 63, 64 are each kXT z, y.

This is a θ-axis servo controller, and as detailed in Fig. 9, each axis is controlled by combining displacement commands The motors 11 to 14 are servo controlled, and 65 is a sensor signal processing unit, which processes the detection signals PS from each status sensor 7a to 7n, as will be explained in detail below in FIG.
1 to Pan are subjected to dead band processing, multiplied by a gain, and output to each servo controller 61 to 64. 1
Reference numerals 10 to 140 each indicate a current detector, which detects the actual current 1. - Those that detect the mechanical θ and feed back current to the servo controllers 61 to 64, 111 to 141 are tacho generators (speed detectors)
Yes, the actual speed of motors 11 to 14 of the corresponding axis x
■It detects o and feeds back the speed to the servo controllers 61-64, 112-142 are encoders, and it consists of a rotary encoder, which detects the position of the motors 11-14 of the corresponding axis every predetermined angular rotation. Generates two-phase detection pulses and controls the servo controllers 61 to 61.
64 for position feedback.

Note that these are internal sensors of the servo system. 66 is a data bus; 9. CPU 60 and each servo controller 6
1 to 64 and the sensor signal processing section 65 for exchanging data.

Reference numeral 67 denotes an address bus, which sends addresses from the CPU 60 to the servo controllers 61 to 64 and the senna signal processing unit 65.

Therefore, the CPU 60 selects the sensor signal processing section 65 via the address bus 67, sets the dead band width W and gain α via the data bus 66, and
4, and the current feedback gain kl is selected via the data bus 66.
+Set 21 position feedback gain in velocity feedback gain, select servo controllers 61 to 64 of the required axis via address bus 67 according to the teaching data, and transfer displacement to servo controllers 61 to 64 via data bus 66. Directive X
c.

Set Yc, Zc, and θC. Also, the current position of each axis X, , Y, is sent from each servo controller 61 to 64.

Zp and θP are read via the data bus 66.

Each servo controller 61 to 64 has a displacement command Xc, Yc
, Zc, θ. The motors 11 to 14 of the corresponding axis are servo-controlled by combining the state quantities sx, sy, sz, and sθ of each axis from the sensor signal processing unit 65.

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.
Detection outputs PS1 to PSn of each state sensor 7a to 7n are converted into components PSX1 to PSθ1, -, corresponding to each axis of the robot.
−, P 8 X n NP 8θn, 6
5bl to 65bn are each reeded at the dead zone processing section.

As described later in FIG. 7, each component decomposition unit 65a1 to 55
It is provided corresponding to an, and performs nonlinear processing called a dead zone on each axis component of each component decomposition section and outputs it.When the absolute value of the input is smaller than the dead zone width setting value W, the output is zero. do. 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, the output is (input - dead band width setting value).

When the input is a negative value, (input child dead band width setting value) is output.

Therefore, for example, the dead zone processing unit 65b1 generates each axis component PSXI', PSYI', P8Z1' which has undergone such dead zone processing.
.

Output PS01'.

65X to 65θ are X-axis, Y-axis, Z-axis, and θ-axis sensor addition units, respectively, and as detailed in FIG. 8, the dead zone processing unit 6
X-axis components PSXi' to P8X from 5b1 to 55bn, respectively
n', Y-axis component PSYI' ~ P8Yn', Z-axis component P8Z1' ~ PSZn', θ-axis component psθ1' ~ PSO
Receive n'.

Corresponding gains αX1 to αXn, αY1 to αYn, αZ
1 to αZn and αθ1 to αθn are multiplied and then added (synthesized).

Servo controller 61~ corresponding to state quantity 5X-Sθ
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 FIG. 1, the other dead zone processing sections 65b2 to 55bn have the same configuration.

In the figure, 65bX to 65bθ are respectively dead zone processing parts 65b1
This is the dead zone processing section for the X, Y, Z, and θ axes.

The dead band widths WX1 to Wθ1 from the data bus 66 are set, and the dead band processing described above is applied to the input sensor components PSXI to PSθ1, and the components P8X1' to P
It outputs Sθ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 a chip select section of the chip select section that selects the data register. It has a latch section that latches the contents, 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.

Dead band width WX1 from the CPU 60 via the data bus 66
is latched, converted to an analog dead band width, and output. 651 and 652 are inverting amplifiers 9
The inverting amplifier 651 inverts the polarity of the dead band width WX1 of the analog latch circuit 650 and outputs it to the adding amplifier 653 (described later).2 The inverting amplifier 652 inverts the output of the inverting amplifier 651 and returns it to the original state.

Output to addition amplifier 654 (described later), 653°6
54 are summing amplifiers, and each sensor component signal P
655 is a polarity determination unit that adds 8X1 and the input dead band width WXI, -WXI.

Determines the polarity of the sensor component PSXI and outputs an UP instruction/DOWN instruction depending on the polarity (D, 656a,
656b are analog switches, respectively, summing amplifiers 653, 654, O output all polarity determination unit 655) UP
It is output in response to the /DOVvN instruction.

Note that the dead zone processing section ssbY for the Y-axis, Z-axis, and θ-axis.

55bZ and 65bθ also have the same configuration.

The operation of the dead zone processing section 65bX is to determine the polarity of the input component PSX by the polarity determining section 655, and to issue an UP/DOWN (negative/positive) output.

In addition, the input PSXI is added to the dead band widths -WXI and WXI by each adding amplifier 653 and 654, and -(FSX1-
WXI).

-(PSX1+wX1) power output level. Above mentioned UP/D
With the OWN instruction, when the output PSXI is positive, the analog switch 656b is selected, and when the circular force PSXI is negative, the analog switch 656a is selected, so if FSX is positive, (-FSX+W), and if FSX is negative, (FSX-W
) is output as the X-axis component FSX1'. This dead band processing prevents the state sensor from emitting an output if the output is less than the dead band width, and prevents the generation of the state quantity SX due to minute outputs due to characteristic errors of the state sensor, thereby causing the output characteristics of the state sensor to fluctuate. This enables stable external state feedback operation even when the system is in use. 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.

Although FIG. 1 only shows the X-axis sensor addition section 65X, other Y-axis, Z-axis, θ-axis sensor addition sections 65Y, 65Z,
65θ also has the same configuration. In the figure, 65X1 to 65Xn are gain multipliers corresponding to the seven sensors 7a to 7n, respectively, and a dead zone processing unit 65b.
The outputs PSXI' to P8Xn' of 1 to 65 bn are multiplied by gains αX1 to αXn to output gain multiplication outputs 01 to Gn.

657 is an analog latch circuit, dead band processing section 65
It has the same configuration as the analog latch circuit 650 of bX,
658 is an analog multiplier that latches the gain αX1 from the CPU 60 and outputs it as an analog quantity.
Each analog multiplier f 656 of the dead zone processing section 65bX
RX is an output resistance that is output by multiplying the output P 8

Note that the other gain multipliers 65X2 to 65Xn have the same configuration, and each output G2(αX2-PSX2') to Gn
(αXn-PSXn') is generated.

65Xm is an adder, each gain multiplier 65X1 to 6
It adds the outputs G1 to Gn of 5Xn to generate the X-axis state quantity SX, and 4fi, AMP is its addition amplifier,
Adds each output 01 to Gn and outputs l 659
a is a voltage/frequency converter (referred to as a V/F converter), which outputs a pulse having a frequency corresponding to the absolute value of the addition output (voltage) of the addition amplifier AMP as the X-axis state quantity SX.

659b is a polarity determination unit that determines the polarity of the addition output (G1+...10n) of the addition amplifier AMP, and outputs an UP/DOWN command to the servo controller depending on whether it is positive or 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, other Y-, Z-, and θ-axis servo controllers 62 to 6 are shown.
4 also has the same configuration.

In the figure, the same parts as those shown in FIG. 612 and data registers 614 and 615 connected to the data bus 66
, 616 and chip select sections 610° 611 , 6
12 select instructions, the data register 614,
A latch unit 617 that latches the contents of 615 and 616.
, 618 , 619 and latch parts 617 , 618 ,
D to convert the contents of 619 (digital value) into analog quantity
/A converters 621, 622, and 623, the shear analog latch circuit 611 latches a current feedback gain of 1, and outputs it as an analog quantity.
Analog latch circuit 61V has speed feedback gain! is latched and output as an analog quantity.

The position feedback gain 4 is latched in the analog latch circuit 61P and output as an analog quantity.

61a is an encoder processing circuit, and encoder 112
61b is a current position counter that determines the rotation direction from two-phase position pulses from the encoder processing circuit 61a and outputs a position pulse that is four times as large as the position pulse. 61b is a current position counter that counts up or down the position pulse depending on the rotation direction determined by the encoder processing circuit 61a. 61C is a D/A converter which converts the current position XP of the current position counter 61b into an analog current position XP, and 61M is a displacement command generation circuit. a chip select section 613 connected to the address bus 67; a nine data register 625 connected to the data bus 66; The CPU 6 includes a pulse generator 624 that outputs displacement command pulses Xc of a number corresponding to the content and a period corresponding to the maximum acceleration.
Latch the specified displacement command Xc from 0, and generate a number of displacement command pulses X corresponding to the value of the displacement command at the period of maximum acceleration.
c.

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 outputs the displacement command pulse Xc to the The state quantity (pulse) SX from the adder 65X is sent to the polarity determining unit 659 b (eighth
Figure) tv UP/DOWN 61h is a command position counter that switches the output to the up side or down side according to the instruction, and counts up the up side pulse of the pulse addition circuit 61g and down counts the down side pulse. A synthesized command position X'c is created by using the synthesized command position X'c, which is then D/A converted. 61d, 61e, 61f, 61
1 is an analog multiplier, and 99 multiplier 61d is the actual current from the current detector 110.1. A multiplier 61e multiplies the current feedback gain of the analog latch circuit 61 by 1. A multiplier 61e multiplies the actual speed x from the speed detector 111 and the speed feedback gain of the analog latch circuit 61P by 1.9 Multiplier 61f 9 Multiplier 61i multiplies the current position XP of the D/A converter 61 and the position feedback gain of the analog latch circuit 61P by 3. The multiplier 61i is the composite command position X'c of the command position counter 61h and the position feedback gain of the analog latch circuit 61F. is multiplied by 9. R1-R4 are addition resistors.

The outputs of each multiplier 61d, 61e, 61f, and 611 are all added together. Therefore, the addition output U is (k, ・X'-
- to 3 ``Xp-to!IIV!-kt'' Ix), the format shown in Figure 1 (5) is shown.

61h is a power amplifier section, which includes an amplifier AMP that amplifies the addition input U and a pair of transistors TR1 and TR2.If the addition power U is positive, the transistor TR2 is turned on, and the drive current ib is passed to the motor 11. If the addition power U is negative, turn on the transistor TRI and increase the polar direction drive current ia
is supplied to the motor 11.

(C) Explanation of the operation of the control unit.

■ The CPU 60 first inputs current, speed 1 position feedback gain, □, kS to each analog latch circuit 611.61.
V, 61P is selected by the address bus 67, sent from the data bus 66, and latched. Further, dead band widths WX1 to Wθl, -.
−, WXn−Wθn and gains αX1 to αXn,...
, αθ1 to αθn are set.

■ Next, the CPU 60 sends the displacement command Xc to the data bus 66.
It is latched by the displacement command generation circuit 61M via. As a result, the pulse generator 624 outputs a displacement command pulse Xc at the frequency of the maximum acceleration and in accordance with the displacement command Xc, and the up side ( ■)
Or command position counter 61h from the down side (in case of O)
is output to. The command position counter 61h counts up pulses from the up side and down counts pulses from the down side, so if the displacement command Xc is in the direction,
The displacement command pulse Xa is counted up.

Therefore, the addition input tJ(x) is (ks-X'c-ks
-XP-!・vx −k, ・engineering
Drive the shaft in the ■ direction.

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

■ On the other hand, in the sensor signal processing section 65, each state sensor 7
The component decomposition unit 65a1 extracts the X, Y, Z, and θ axis components of a to 7n.
~55an, and further dead zone processing parts 65b1~65
Perform dead zone processing with bn, and add sensor addition section 65X for each axis.
Coaxial components are combined (added) at 65θ.

Therefore, the X-axis component P of any of the status sensors 7a to 7n
Dead band width WX1 to WXn corresponding to SXI to PSXn
When the value exceeds , the X-axis state quantity SX is generated from the X-axis sensor addition section 65X.

For example, when the signal PSXI is generated from the ultrasonic sensor 7a as shown in FIG. 3(c), αX1・(PSXI −WXI
) is generated, inputted to the pulse addition circuit 61g of the servo controller 61, and inputted to the polarity determination unit 6.
With the UP/DOWN output of 59b, in this case, the DOWN instruction and the state quantity pulse SX are output to the down side, thus causing the command position counter 6.1h to count down.

Therefore, the composite displacement command X'c of the command position counter 61h
decreases, so the simulator 11 whose additional human power U (X) decreases or is negative is decelerated.

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

1i[For the K, Y, Z, and θ axes, each servo controller 62, 63, and 64 is the Y sensor signal processing section.

Decelerate according to the Z and θ axis state quantities sy, sz, and sθ.

A stop takes place.

In this way, the detected energies of the plurality of sensors are combined and input to the control system, and motor control, that is, movement control, is performed in accordance with the outputs of the sensors.

If a sensor for measuring the temperature of the heat source is used as this condition sensor, it can be moved without approaching the heat source, and
Using the linear region allows smooth deceleration.

(d) Description of other embodiments.

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

In Fig. 10 (5), the same parts as shown in Fig. 2 are indicated by the same symbols, 9 is the target object, hole 91
90 is an article that is held by the hand 5 and fitted into the hole 91.

This example shows assembly (fitting) work by robot K. As shown in FIG. 10 (03), as the hand 5 descends in the biaxial direction at a speed vex and approaches the object 9, the signal pszi is output from the ultrasonic sensor 7a.
The shaft speed is decelerated by (Vex-αZ-PCZI). At position @zp1, when the article 9o of the hand 5 contacts the tapered surface of the hole 91 of the object 9 as shown in FIG. FIG. 10 (Q'') 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.

Also, at this time, since the Z-axis direction is decelerated relative to the object 9, the object 9 can make soft contact and absorb two positional deviations.
．． The article 90 will not be damaged by contact.

Therefore, one-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.

Gain αZ1. By changing α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 the 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 enter the inside of 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.

Since there is almost no burden on the software, tracing work can be done in real time.

As described above, contact copying work and non-contact copying work can be selected and executed by setting the gains of the ultrasonic sensor 7a and force sensor 7b.

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, it's not limited to robots.

The present invention can be applied to any moving object, and may be a rotating object such as a θ-axis.

In the above example, a DC motor is used as the transportation means, but
Other actuators such as a pulse motor-1 AC motor may also 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, depending on the required control. can be used.

Instead of providing it on the moving body itself, it may be provided at another observation point.

Although the present invention has been described above with reference to embodiments, the present invention can be modified in various ways according to the spirit 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 environmental state detection signals of a plurality of state sensors are synthesized to form a state quantity, which can be incorporated into a control system to control movement. It has the effect that autonomous movement control is possible according to the operating environment even without a sensor, and it also has the effect of being able to process signals from multiple status sensors even if they occur simultaneously, making real-time processing possible. Furthermore, the burden on the control unit is reduced, and movement control can be performed with high performance and high intelligence.

[Brief explanation of drawings]

FIG. 1 is a diagram explaining the principle of the present invention. FIG. 2 is an overall configuration diagram of an embodiment of the present invention. 3 and 4 are explanatory diagrams of the operation of the configuration in FIG. 2. FIG. 5 is a configuration diagram of the control section configured in FIG. 2. FIG. 6 is a detailed circuit diagram of the senna signal processing section configured in FIG. 5. FIG. 7 is an internal configuration diagram of the dead zone processing section configured in FIG. 6. FIG. 8 is an internal configuration diagram of the sensor addition section configured in FIG. 6. FIG. 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. FIG. 11 is an explanatory diagram of another embodiment of the present invention. 1M in the figure: moving object. MT...means of transportation! NSI to NSn...Status detection means. CT...control means.

Claims (1)

[Claims] A moving means for moving a moving body, a plurality of state detection means for detecting operating environment conditions of the moving body, a state quantity that is a combination of outputs of the plurality of state detection means; The control system includes a control means for controlling the movement of the moving means by combining a state quantity and a movement command, and is characterized in that the state quantity which is a combination of the outputs of the plurality of state detection means is incorporated into the control system. Mobile control device.