阅读说明：本技术 机器人控制装置、机器人控制系统和机器人控制方法 (Robot control device, robot control system, and robot control method ) 是由 吉浦泰史 喜多川辉久 于 2021-03-16 设计创作，主要内容包括：机器人控制装置、机器人控制系统和机器人控制方法。判定并联连杆机器人的机构部的异常。机器人控制装置(3)对并联连杆机器人(4)进行控制,具有：驱动控制部(31),其对并联连杆机器人(4)的多个电机(44)进行控制；以及异常判定部(32),其根据多个电机(44)的状态数据,判定并联连杆机器人(4)的机构部(50)的碰撞和脱臼中的至少任意一方,在异常判定部(32)判定了碰撞的情况下,驱动控制部(31)以使得在XYZ轴直角坐标系的动作空间中的XY轴方向上的碰撞和Z轴方向上的碰撞中机构部(50)的动作不同的方式对多个电机(44)进行控制,XYZ轴直角坐标系的Z轴方向对应于铅垂方向。(A robot control device, a robot control system and a robot control method. An abnormality of a mechanism part of the parallel link robot is determined. A robot control device (3) controls a parallel link robot (4), and is provided with: a drive control unit (31) that controls a plurality of motors (44) of the parallel link robot (4); and an abnormality determination unit (32) that determines at least one of a collision and a dislocation of the mechanism unit (50) of the parallel link robot (4) on the basis of state data of the plurality of motors (44), wherein when the abnormality determination unit (32) determines a collision, the drive control unit (31) controls the plurality of motors (44) such that the movement of the mechanism unit (50) differs between a collision in the XY-axis direction and a collision in the Z-axis direction in a movement space of an XYZ-axis rectangular coordinate system, the Z-axis direction of which corresponds to the vertical direction.)

1. A robot control device for controlling a parallel link robot, characterized in that,

the robot control device includes:

a drive control unit that controls a plurality of drive shafts of the parallel link robot; and

and an abnormality determination unit that determines at least one of a collision and a dislocation of the mechanism unit of the parallel link robot based on the state data of the plurality of drive shafts.

2. The robot control apparatus according to claim 1,

when the abnormality determination unit determines the collision, the drive control unit controls the plurality of drive axes such that the movement of the mechanism unit differs between a collision in the XY-axis direction in a movement space of an XYZ-axis rectangular coordinate system and a collision in the Z-axis direction, the Z-axis direction of the XYZ-axis rectangular coordinate system corresponding to a vertical direction.

3. The robot control apparatus according to claim 2,

when the abnormality determination unit determines that the collision occurs in the XY axis direction, the drive control unit controls the plurality of drive shafts so that the end effector included in the mechanism unit moves to a predetermined position.

4. The robot control apparatus according to claim 2,

the drive control unit controls the plurality of drive shafts so as to stop movement of the end effector included in the mechanism unit when the abnormality determination unit determines that the collision in the XY axis direction occurs.

5. The robot controller according to any one of claims 2 to 4,

in the case where the abnormality determination section determines the collision in the Z-axis direction,

the drive control unit controls the drive shafts so as to move an end effector included in the mechanism unit to a predetermined position while limiting output torques of the plurality of drive shafts.

6. The robot controller according to any one of claims 1 to 4,

when the abnormality determination unit determines the dislocation, the drive control unit stops the control of the plurality of drive shafts.

7. A robot control system having a parallel link robot and a robot control device for controlling the parallel link robot,

the robot control device includes:

a drive control unit that controls a plurality of drive shafts of the parallel link robot; and

and an abnormality determination unit that determines at least one of a collision and a dislocation of the mechanism unit of the parallel link robot based on the state data of the plurality of drive shafts.

8. A robot control method executed by an arithmetic device provided in a robot control device for controlling a parallel link robot,

determining at least one of collision and dislocation of a mechanism part of the parallel link robot based on state data of a plurality of drive shafts of the parallel link robot,

executing a predetermined control on a plurality of drive axes in the parallel link robot when the collision is determined,

when the dislocation is determined, the control of the plurality of drive shafts is stopped.

Technical Field

The disclosed embodiments relate to a robot control device, a robot control system, and a robot control method.

Background

For example, patent document 1 describes an abnormality monitoring device as follows: the state information of the motor for driving the robot mechanism is extracted for each frequency band, and an abnormality is detected based on the value obtained by integrating the outputs for each frequency band.

Patent document 1: japanese patent laid-open publication No. 2020-022329

Disclosure of Invention

Problems to be solved by the invention

On the other hand, in a configuration in which a closed link mechanism is cooperatively controlled by a plurality of drive shafts, such as a parallel link robot, there is a situation in which the plurality of drive shafts exert different torques and cancel each other out, and the relationship of state information between the drive shafts is complicated. Therefore, in the control of the parallel link robot, it is difficult to determine an abnormality such as a collision or dislocation using only the state information of 1 drive shaft as in the above-described conventional technique.

The present invention has been made in view of the above problems, and an object thereof is to provide a robot control device, a robot control system, and a robot control method capable of determining an abnormality in a mechanism portion of a parallel link robot.

Means for solving the problems

In order to solve the above problem, according to an aspect of the present invention, there is provided a robot control device for controlling a parallel link robot, the robot control device including: a drive control unit that controls a plurality of drive shafts of the parallel link robot; and an abnormality determination unit that determines at least one of a collision and a dislocation of the mechanism unit of the parallel link robot based on the state data of the plurality of drive shafts.

Further, according to another aspect of the present invention, there is applied a robot control system including a parallel link robot and a robot control device that controls the parallel link robot, the robot control device including: a drive control unit that controls a plurality of drive shafts of the parallel link robot; and an abnormality determination unit that determines at least one of a collision and a dislocation of the mechanism unit of the parallel link robot based on the state data of the plurality of drive shafts.

Further, according to another aspect of the present invention, there is applied a robot control method executed by a computing device included in a robot control device that controls a parallel link robot, wherein at least one of collision and dislocation of a mechanism portion of the parallel link robot is determined based on state data of a plurality of drive shafts of the parallel link robot, and when the collision is determined, predetermined control is executed on the plurality of drive shafts in the parallel link robot, and when the dislocation is determined, the control of the plurality of drive shafts is stopped.

Effects of the invention

According to the present invention, it is possible to determine an abnormality in the mechanism portion of the parallel link robot.

Drawings

Fig. 1 is a perspective view showing an example of the overall configuration of a robot control system according to embodiment 1.

Fig. 2 is a diagram showing an example of a schematic configuration of a motor.

Fig. 3 is a diagram showing an example of the internal configuration of the robot controller and various information transmitted and received around the robot controller.

Fig. 4 is a diagram showing an example of a feedback loop processed in the servo.

Fig. 5 is a diagram illustrating an example of a single-axis drive mechanism model for explaining the collision determination method.

Fig. 6 is a diagram showing an example of a temporal change in the state data of the motor in the control procedure of the single-axis drive mechanism model.

Fig. 7 is a diagram showing an example of a mechanism model of the parallel link robot.

Fig. 8 is a diagram illustrating an example of a countermeasure operation against a collision in the XY axis direction.

Fig. 9 is a diagram illustrating an example of a countermeasure operation against a collision in the Z-axis direction.

Fig. 10 is a diagram illustrating an example of the operation for coping with dislocation.

Fig. 11 is a flowchart showing an example of a control procedure of the abnormality determination process.

Fig. 12 is a flowchart showing an example of a control procedure of the job control process.

Fig. 13 is a perspective view showing an example of the overall configuration of the robot control system according to embodiment 2.

Fig. 14 is a diagram showing an example of the internal configuration of the robot controller and various information transmitted and received around the robot controller.

Fig. 15 is a system block diagram showing a hardware configuration of the robot controller.

Description of the reference symbols

1. 1A robot control system; 2, a host control device; 3a robot control device; 4 connecting rod robots connected in parallel; 12 a motor main body portion; 13 a brake part; 14 an encoder section; 15 a reducer; 16 a three-dimensional acceleration sensor; 31 a drive control section; 32 an abnormality determination unit; 33 a work control unit; 34 a motion control section; 35a server; 41 a base part; 42 a movable part; 43 link mechanism part; 44 motor (drive shaft); 45 mounting a component; 46 drive link; 47a passive linkage; 48. 49 spherical bearings; 50 a mechanism part; the SH axis.

Detailed Description

<1 > embodiment 1 >

<1-1. schematic configuration of robot control System >

Next, a robot control system according to embodiment 1 will be described with reference to the drawings. The robot control system shown in the example of the present embodiment is a system for controlling a parallel link robot that performs transfer work of a workpiece such as picking and placing, for example. In fig. 1, a robot control system 1 includes a master control device 2, a robot control device 3, and a parallel link robot 4. In the following description, for convenience of explanation of the configuration of the parallel link robot or the like, directions such as up, down, left, right, front, and rear directions may be appropriately used, but the positional relationship of the respective configurations of the parallel link robot 4 or the like is not limited.

The host control device 2 determines, for example, the type of work to be executed by the parallel link robot 4 through a communication line not shown in particular by a user's input operation, and outputs a corresponding work command to the robot control device 3 described later.

The robot controller 3 controls the operation of the parallel link robot 4 by supplying motor drive power to each of the plurality of motors 44 of the parallel link robot 4 in a predetermined operation sequence corresponding to the operation command input from the host controller 2. As described later, the plurality of motors 44 of the parallel link robot 4 are provided with encoder units 14 that detect the rotational positions of the rotors as motor detection positions, and the robot controller 3 controls the supply of driving power to the motors with reference to the motor detection positions so as to realize operations based on the operation sequence. The internal configuration and processing of the robot controller 3 will be described in detail later (see fig. 3 described later).

In the illustrated example, the parallel link robot 4 is a mechanical system as follows: the movement control such as moving an end effector (not shown in particular) to an arbitrary coordinate position in the movement space can be performed by the shaft drive of each motor 44.

<1-2. Structure of parallel link robot >

As shown in fig. 1, the parallel link robot 4 of this example has a base portion 41, a movable portion 42, 3 link mechanism portions 43a, 43b, 43c, and 3 motors 44a, 44b, 44 c.

The 3 link mechanism portions 43a, 43b, and 43c are arranged along the circumferential direction around the central axis AX of the parallel link robot 4, and connect the base portion 41 and the movable portion 42. The 3 motors 44a, 44b, and 44c are disposed on the base portion 41 and drive the link mechanism portions 43a, 43b, and 43c, respectively. In this example, the base portion 41 is formed in a disk shape, and 3 motors 44a to 44c are fixed and housed therein. In this example, the movable portion 42 is formed in a disk shape and has a mounting member 45 at a lower end. An end effector, not shown, such as a robot hand, is attached to the attachment member 45. The structure of each of the motors 44a to 44c is described in detail later (see fig. 2 described later).

The 3 link mechanism portions 43a to 43c all have the same structure. The link mechanism portion 43a includes a drive link 46a coupled to the output shaft of the motor 44a, and 2 passive links 47a coupled to the drive link 46a and the movable portion 42. The 2 passive links 47a are coupled to the drive link 46a via spherical bearings 48a, and are coupled to the movable portion 42 via spherical bearings 49 a. The link mechanism portion 43b includes a drive link 46b coupled to an output shaft of the motor 44b, and 2 passive links 47b coupled to the drive link 46b and the movable portion 42. The 2 passive links 47b are coupled to the drive link 46b via spherical bearings 48b, and are coupled to the movable portion 42 via spherical bearings 49 b. The link mechanism portion 43c includes a drive link 46c coupled to an output shaft of the motor 44c, and 2 passive links 47c coupled to the drive link 46c and the movable portion 42. The 2 passive links 47c are coupled to the drive link 46c via spherical bearings 48c, and are coupled to the movable portion 42 via spherical bearings 49 c. The drive links 46a, 46b, and 46c are linear members and extend in a radial direction around the central axis AX. In the present embodiment, the 3 link mechanism portions 43a, 43b, and 43c, the movable portion 42, the mounting member 45, and the end effector (not particularly shown) are collectively referred to as a mechanism portion 50.

In the parallel link robot 4, a robot coordinate system of XYZ rectangular coordinates in which the Z-axis direction corresponds to the vertical direction is set in the motion space of the end effector. The motors 44 and the corresponding link mechanism portions 43 of the parallel link robot 4 are not limited to 3 sets in the illustrated example, and may have a configuration of 4 sets or more (that is, a configuration of a multi-axis drive type having 4 or more axes) (not particularly illustrated).

Fig. 2 illustrates an example of a schematic configuration of each of the motors 44a to 44 c. The 3 motors 44a to 44c have the same structure. As shown in fig. 2, the motor 44(44a to 44c) includes a motor main body 12, a brake unit 13, an encoder unit 14, and a reduction gear unit 15. The motors 44a to 44c having this configuration correspond to examples of the drive shaft described in the claims.

The motor main body 12 is a Rotary (rotation type) motor as follows: the motor drive device includes a stator and a rotor (not shown), and the rotor rotates relative to the stator when a motor drive power is supplied thereto.

The braking unit 13 brakes the rotation of the rotor by receiving a braking signal.

The encoder unit 14 detects a position (also referred to as a "rotational position" or a "rotational angle") of the rotor, and outputs the position as a motor detection position.

The speed reducer 15 uses a rotating shaft (not shown) of the rotor as an input shaft, and performs speed reduction conversion (position conversion, torque conversion) of the shaft output via a gear reduction mechanism provided inside, and outputs the shaft output to the shaft SH. The drive links 46(46a to 46c) are fixed to the shaft SH and driven to swing.

<1-3. detailed construction of robot control device >

Fig. 3 shows the internal structure of the robot controller 3 and various information transmitted and received around the robot controller. In fig. 3, the robot controller 3 includes a drive control unit 31 and an abnormality determination unit 32.

The drive control unit 31 controls the 3 motors 44a to 44c of the parallel link robot 4. The drive control unit 31 includes a work control unit 33, a motion control unit 34, and 3 servos 35a, 35b, and 35c corresponding to the motors 44a to 44 c.

The work control unit 33 outputs a coordinate position command, which is a movement destination position of the end effector of the parallel link robot 4, to the motion control unit 34 in a work order corresponding to the work command input from the host control device 2. The coordinate position command is a three-dimensional coordinate position on the robot coordinate system, and the work control unit 33 continuously outputs the coordinate position command as a destination position to which the end effector should move next (stops moving by continuously outputting the same coordinate position) at all times.

The work control unit 33 can output, to all the servos 35, a servo-off signal instructing to stop the supply of the motor drive power and a torque limit signal instructing to limit the output torque, as necessary, based on the abnormality determination information input from the abnormality determination unit 32 described later. Further, similarly, the work control unit 33 can output, to the braking units 13 of all the motors 44, a braking signal for braking the rotation of the motor 44 as necessary, based on the abnormality determination information. The operation of the end effector itself attached to the attachment member 45 of the parallel link robot 4 is also controlled by the operation control unit 33 in accordance with the above-described operation procedure, and for convenience of explanation, the illustration of the control of the end effector will be omitted. The processing in the work control unit 33 will be described in detail later.

The motion control unit 34 calculates a target position of each motor 44 necessary for the end effector to move in response to the coordinate position command input from the work control unit 33 by so-called inverse kinematics calculation, and outputs the target position to the corresponding servo 35 as a motor position command.

The servo 35 performs the following control of supply of drive power: the motor detection position detected by the encoder unit 14 of the corresponding motor 44 is referred to, and the motor 44 is drive-controlled (position-controlled in this case) in accordance with a motor position command input from the motion control unit 34. Each servo 35 sequentially outputs a motor torque command generated inside thereof, a motor detection position and a motor detection speed detected from the corresponding encoder unit 14, to the abnormality determination unit 32 described later as motor state data. The control of the servo 35 will be described in detail later (see fig. 4 described later).

The abnormality determination unit 32 determines whether or not an abnormal state such as a collision or dislocation described later occurs in the mechanism unit 50 of the parallel link robot 4 based on motor state data (the motor torque command, the motor detection position, and the motor detection speed in this example) input from each servo 35, and outputs abnormality determination information including the determination result and information related thereto to the work control unit 33. The processing in the abnormality determination unit 32 will be described in detail later.

The processing and the like in the drive control unit 31 (the work control unit 33, the motion control unit 34, and the servo 35), the abnormality determination unit 32, and the like are not limited to the examples of sharing these processing, and may be performed by a smaller number of processing units (for example, 1 processing unit), or may be performed by a more subdivided processing unit. The robot controller 3 may be installed in software by a program executed by a CPU901 (arithmetic unit: see fig. 15) described later, or may be installed in hardware by an actual device such as an ASIC, FPGA, or other circuit.

<1-4. control processing Structure in Servo >

Fig. 4 shows a feedback loop processed in the servo 35 described above. The feedback loop shown in fig. 4 represents the control process executed in the servo 35 in the form of a transfer function. In the example of the present embodiment, the servo 35 performs position control based on a motor position command output from the motion control unit 34, and performs double loop processing of the position control feedback loop and the speed control feedback loop, which are shown in the figure, in accordance with the position control command.

In the double loop processing, a deviation between a motor position command input from the motion control unit 34 and a motor detection position detected from the encoder unit 14 is obtained as a position deviation, and the position control unit 61 generates a speed command based on the position deviation. Further, a deviation between the speed command and the motor detection speed detected from the encoder unit 14 is obtained as a speed deviation, and the speed control unit 62 generates a motor torque command based on the speed deviation. As shown in the figure, the motor detection speed may be calculated by differentiating the motor detection position by 1-order time by the differential operator 65. Then, the PWM control section 64 supplies drive power in accordance with the motor torque command via the torque limiter 63, thereby driving the motor 44.

Here, the torque limiter 63 outputs the input motor torque command as an original value in a normal state, but limits and outputs the value of the motor torque command by a preset upper limit value (or lower limit value) while the torque limit signal is input from the operation control unit 33. In addition, although the PWM control unit 64 supplies the motor drive power by PWM control based on the motor torque command in a normal state, the supply of the motor drive power itself is stopped and the motor 44 is allowed to operate freely (hereinafter, referred to as servo-off) while the servo-off signal is input from the operation control unit 33.

<1-5 > characteristics of the present embodiment

As described above, the parallel link robot 4 generally includes the mechanism unit 50 of the closed link mechanism in which the plurality of link mechanism units 43 are connected in parallel, and the drive control unit 31 controls the plurality of motors (drive shafts) that individually drive the respective link mechanism units 43 in cooperation with each other, thereby controlling the entire mechanism unit 50 to perform an arbitrary operation.

In the case of a configuration in which the closing link mechanism is driven by complex cooperative control of a plurality of motors as described above, the plurality of motors may apply different torques to the respective link mechanisms to cancel each other out depending on the arrangement and posture of the mechanism portion 50 in order to stabilize the state thereof, and the relationship between these torques is complicated. Therefore, it is difficult to easily determine occurrence of an operation abnormality such as a collision or dislocation in the mechanism unit 50 only from behavior or state data of any one of the motors.

On the other hand, the present embodiment includes an abnormality determination unit 32, and the abnormality determination unit 32 determines at least one of collision and dislocation of the mechanism unit 50 of the parallel link robot 4 based on the state data of the plurality of motors (the motor torque command, the motor detection position, and the motor detection speed in the example of the present embodiment).

This makes it possible to accurately determine the occurrence of a collision with the outside in the mechanism unit 50 and dislocation in the spherical bearings 48 and 49 based on the state data of all the motors 44a to 44c that drive the mechanism unit 50 of the parallel link robot 4. The methods required to achieve the above functions will be described in order below.

<1-6. basic method for collision judgment >

First, a basic method of collision determination in the present embodiment will be described with reference to a single-axis drive mechanism shown in fig. 5 as a model example. The single-shaft drive mechanism 70 shown in fig. 5 is configured to rotate a ball screw 71 vertically erected from the ground by 1 motor 72 and to move a link 73 up and down. As a control procedure for the single-shaft drive mechanism 70 to cope with the collision, the following control procedure is executed: in the middle of the downward movement of the link 73 as shown in fig. 5(a), when an unexpected foreign object 74 collides with the link 73 as shown in fig. 5(b), the movement of the link 73 is stopped, and then, the upward movement of the link 73 is performed to avoid the foreign object 74 as shown in fig. 5 (c). Fig. 6 shows the time changes of the position command speed, the motor torque command, and the collision torque before and after the collision in such a control sequence. The collision torque corresponds to a value obtained by converting a contact external force acting as a resistance of the link 73 applied to the collision foreign object 74 into an external disturbance torque.

As shown in fig. 6, the motor torque command that is stable during the downward movement of the link 73 is greatly reduced immediately after the motor torque command comes into contact with the foreign object 74 and collides therewith, and the collision torque also greatly fluctuates. This is a variation resulting from the position command or the velocity command to be input after the collision being realized in the feedback loop of the servo that controls the motor 72. In the present control sequence, such a variation in the motor torque command and the collision torque is detected to determine the collision. In the present control procedure, in particular, immediately after the collision determination, the motor torque command is limited in the collision direction (torque limitation) and the downward movement is stopped, whereby the influence of the collision with the foreign object 74 can be suppressed, and then the link 73 is moved upward and retracted. Further, torque restriction is not required for the pull-back direction in the upward movement of the retreat.

Here, as shown in fig. 5(b), the influence of the resistance F1 received immediately after the link 73 contacts the foreign matter 74 and the foreign matter 74 is small, but when the descending movement is continued in this way, the received resistance F2 increases, and the influence in the foreign matter 74 also increases. Therefore, it is desirable to perform collision determination as quickly as possible after the link 73 actually contacts the foreign matter 74, and stop the descending movement thereof. For this reason, a method of performing collision determination by comparing the motor torque command with a threshold value having an absolute value as small as possible may be considered, but in this case, when stability of the up-down movement of the link 73 in a normal state is considered, it is difficult to set the threshold value.

That is, when the mass of the link 73 itself is very large or when the link is rapidly moved up and down, a large motor torque is required during the acceleration period and the deceleration period of the up-and-down movement, and the threshold value cannot be set lower. In addition, for example, when applied to a multi-axis drive articulated robot or the like that transfers a workpiece, the required motor torque changes in a complicated manner depending on the increase or decrease in the mass of the gripped workpiece, the arrangement and the posture of the link portion. In particular, when controlling the operation of the closed link mechanism such as the parallel link robot 4 of the present embodiment, depending on the arrangement and posture of each link mechanism portion 43, the plurality of motors 44 may apply different torques to each link mechanism portion 43 to cancel each other out in order to stabilize the state thereof, and the relationship between these torques may be complicated. As described above, it is difficult to determine a collision in the motor drive mechanism by simply comparing the motor torque command with the fixedly set 1 threshold value.

In contrast, the abnormality determination unit 32 of the present embodiment estimates the contact external force F itself acting as resistance from the foreign object 74 on the mechanism unit 50 at the time of collision, and performs collision determination quickly and with high accuracy by comparing the contact external force F with a threshold value set to be very low. The contact external force F is estimated by arithmetic processing based on motor state data (a motor torque command, a motor detection position, and a motor detection speed in the example of the present embodiment) of the plurality of motors 44.

<1-7. estimation calculation processing for external contact force >

Next, the content of the calculation process for estimating the contact external force F acting on the mechanism unit 50 of the parallel link robot 4 will be described in detail. In the example of the present embodiment, a lagrangian equation of motion is derived from the specification parameters defined by the mechanism model of the parallel link robot 4 shown in fig. 7, and the contact external force F acting on the mechanism unit 50 is calculated from the equation of motion.

First, the kinetic energy of the end effector can be described by the following equation.

Also, the kinetic energy of the drive link 46 and the driven link 47 can be described by the following equation.

Further, the potential energy of the end effector can be described by the following equation.

U_p＝m_pg_cP_z(t) … (formula 4)

Also, the potential energy of the driving link 46 and the driven link 47 can be described by the following equation.

In the above (formula 1) to (formula 6), the specification parameters not shown in fig. 7 are as follows.

m_p: mass of end effector

m_a: mass of the drive link

J_a: inertia moment of rotor and gear of motor

J_b(t，θ_b): inertia tensor matrix of passive links around center of gravity relative to XYZ plane

g_c: acceleration of gravity

Here, the first and second liquid crystal display panels are,

J_b＝Rot(δx)Rot(δy)J′_bRot(δy)^TRot(δx)^T

wherein the content of the first and second substances,

δ X, δ Y are rotation angles of the passive link 47 around the X axis and the Y axis. In addition to this, the present invention is,

m_b: mass of passive link

L₁: length of passive link

Furthermore, according to the above (equation 1) to (equation 6), a lagrangian function such as the following equation can be obtained.

L＝(T_p+T_a+T_b)-(U_p+U_a+U_b) … (formula 7)

Here, the generalized coordinates are set as

q＝[P_x P_y P_z θ_α1 θ_α2 θ_α3]

The generalized force applied to the mechanism portion 50 is defined as

Q＝[F_x F_y F_z Trq₁ Trq₂ Trq₃]

The motion equation can be derived by operating the following equation.

(wherein j is 1 to 6)

From the above (equation 8), the contact external force F (Fx, Fy, Fz) expressed by the vector in the XYZ axis direction and the axial torque (Trq) of each motor 44 can be obtained₁、Trq₂、Trq₃) The equation of motion of (a). In a normal state where the mechanism 50 is not in contact with a foreign object, the contact external force F is 0, and therefore Fx-Fy-Fz-0. In addition, each shaft torque (Trq)₁、Trq₂、Trq₃) Is the deceleration of each motor 44The torque output from the shaft SH of the device 15 to swing-drive the drive link 46 is a value corresponding to the motor torque command × η when the product of the reduction ratio and the reduction efficiency of the reduction gear 15 is η. In the example of the present embodiment, the contact external force F can be obtained by substituting motor state data (motor torque command, motor detection position, motor detection speed) of each motor 44 and the position (Px, Py, Pz), speed, and acceleration of the end effector into a differential equation obtained by transforming the motion equation of the above-described (equation 8). The abnormality determination unit 32 may calculate the position, velocity, and acceleration of the end effector by forward kinematics calculation based on the mechanical specifications shown by the mechanism model and the like in fig. 7 and the motor state data input from each servo 35.

<1-8. content of control for coping with each axial direction for collision >

In general, the operations of picking and placing performed by the plurality of parallel link robots 4 are classified into a movement operation in the horizontal direction (XY-axis direction) and a movement operation in the vertical direction (Z-axis direction) in terms of control, and in many cases, these operations are combined as necessary. In the working environment in which the above-described pick-and-place operation is performed, there may be a difference in the content of the response control that is required when the mechanism portion 50 collides with the foreign matter 74 or the like other than the workpiece in the horizontal direction and the foreign matter 74 or the like other than the workpiece in the vertical direction. Therefore, in the present embodiment, the operation control unit 33 of the drive control unit 31 cooperatively controls the plurality of motors 44 so as to perform different actions in the horizontal direction (XY-axis direction) and the vertical direction (Z-axis direction) when the mechanism unit 50 collides.

<1-8-1. content of control for coping with collision in XY-axis direction >

Fig. 8(a) shows, from the side, a state in which the movable portion 42 of the parallel link robot 4 mainly collides with the foreign object 74 in the XY axis direction (horizontal direction), and fig. 8(b) shows a state in which the work control portion 33 controls to retract the movable portion 42 in accordance with such a collision in the XY axis direction. As described above, the abnormality determination unit 32 can substitute motor state data (motor torque command, motor detected position, motor detected speed) of each motor 44 and the position, speed, and acceleration of the end effector into a predetermined calculation formula obtained from the motion equation, thereby sequentially calculating the contact external force F applied to the movable unit 42 at that time, and outputting the contact external force F to the work control unit 33 as abnormality determination information. The contact external force F is calculated by using the component forces Fx, Fy, and Fz in the X-axis direction, the Y-axis direction, and the Z-axis direction of the robot coordinate system.

Therefore, Fxy (Fx) can be used²+Fy²)^1/2The absolute value of a resultant component force Fxy obtained by combining component forces Fx and Fy in the X-axis direction and the Y-axis direction of the contact external force F is obtained. The direction of the combined component force Fxy can also be determined from the signs of the component forces Fx and Fy. Then, the abnormality determination unit 32 determines that a collision has occurred in the XY axis direction when the absolute value of the calculated combined component force Fxy exceeds a predetermined threshold value. Further, when the absolute value of the Z-axis direction component force Fz of the contact external force F exceeds another threshold value, it can be easily determined that a collision in the Z-axis direction has occurred. In the above 2 collision determinations, the objects to be compared with the threshold values are only the contact external force applied to the mechanism portion 50, and therefore, even if the threshold values are set to be extremely small, the collision determination with high reliability and sensitivity can be performed.

In the example shown in fig. 8(a), the component force Fz of the calculated contact external force F is extremely smaller than the corresponding threshold value, while the combined component force Fxy is extremely larger than the corresponding threshold value, and therefore the abnormality determination unit 32 determines that only a collision in the XY axis direction has occurred in the mechanism unit 50 (movable unit 42). In a general work site of the parallel link robot 4, when the foreign object 74 collides with the mechanism portion 50 in the horizontal direction as described above, the influence of the collision can be made less in both the mechanism portion 50 and the foreign object 74 by only moving the mechanism portion 50 so as to be apart from the collision point P in the horizontal direction along the collision direction. Therefore, in the present embodiment, when the abnormality determination unit 32 outputs the determination result that the collision in the XY axis direction has occurred and the synthesized component force Fxy as the abnormality determination information, the work control unit 33 that has received this information outputs a work command to move the movable unit 42 in the retracted state to a position (predetermined position) horizontally separated by a predetermined separation distance De set in advance in the direction of the synthesized component force Fxy, as shown in fig. 8 (b).

Although not particularly shown, instead of comparing the synthesized component force Fxy with the threshold value, the component force Fx in the X-axis direction and the component force Fy in the Y-axis direction may be individually compared with the threshold value, and a collision may be determined in accordance with the axis direction and dealt with in accordance with each collision determination.

<1-8-2. content of control for coping with collision in Z-axis direction >

Further, as in the example shown in fig. 9(a), when the calculated combined component force Fxy of the contact external force F is extremely smaller than the corresponding threshold value and the component force Fz is extremely larger than the corresponding threshold value, it is determined that only the collision in the Z-axis direction has occurred in the mechanism portion 50 (movable portion 42). In a work site of a general parallel link robot 4, when a collision occurs with the mechanism portion 50 in the vertical direction, the mechanism portion 50 may press down with a foreign object 74 interposed between a belt conveyor or the like, not shown, and the floor surface. Therefore, in the present embodiment, when the abnormality determination unit 32 outputs the determination result that the collision in the Z-axis direction has occurred and the component force Fz as the abnormality determination information, the work control unit 33 that has received this information outputs a work command to move the movable unit 42 in the retracted state to a position (predetermined position) separated by a predetermined separation distance Ue set in advance in the same direction (upper and lower) as the component force Fz, as shown in fig. 9 (b). At this time, the work control unit 33 outputs the torque limit signal to all the servos 35 to limit the output torque of each motor 44, thereby quickly and reliably reducing the influence of both the mechanism unit 50 and the foreign matter 74.

Further, depending on the contact direction between the mechanism portion 50 and the foreign matter 74, it may be determined that a collision has occurred in both the XY axis direction and the Z axis direction, and in this case, both of the countermeasure controls (retreat movements) may be executed at the same time (not shown in particular).

<1-9 > control content for coping with dislocation

For example, when the mechanism portion 50 suddenly collides with the foreign matter 74, as shown in fig. 10(a), the spherical joint may be in a dislocated state in which the spherical joint is separated from one of the spherical bearings 48 and 49 included in the mechanism portion 50. At this time, since the driving force of each motor 44 to the mechanism portion 50 greatly changes, the contact external force F calculated by the abnormality determination portion 32 also greatly changes. In the example of the present embodiment, the abnormality determination unit 32 discriminates between the collision determination and the dislocation determination by determining the difference in the contact external force F (or the time change thereof) between the collision and the dislocation.

When such a dislocated state occurs, it is preferable that the entire mechanism unit 50 be directly lowered by free operation due to its own weight, as shown in fig. 10(b), without applying unnecessary exciting force or restraining force to each link mechanism unit 43 of the mechanism unit 50. Therefore, in the present embodiment, when the abnormality determination unit 32 outputs the dislocation determination as the abnormality determination information, the work control unit 33 having received the information outputs the servo off signal to all the servos 35, and stops the supply of the drive power to each motor 44.

<1-10. control flow >

The control procedure of the abnormality determination process and the job control process executed in software by the CPU901 of the robot control device 3 to realize the functions of the abnormality determination unit 32 and the job control unit 33 described above will be described with reference to the flowcharts of fig. 11 and 12. Fig. 11 shows a flowchart in the case where the abnormality determination unit 32 is installed in software, and the flowchart is started to be executed when the parallel link robot 4 starts to be controlled.

First, in step S105, the CPU901 of the robot controller 3 acquires the motor torque command generated by each servo 35, and the detected motor detection position and motor detection speed as motor state data.

Next, the process proceeds to step S110, and the CPU901 of the robot controller 3 calculates the position, speed, and acceleration of the end effector (abbreviated as "EE" in the drawing) at that time, by forward kinematics calculation based on the motor state data acquired in step S105.

Next, the process proceeds to step S115, and the CPU901 of the robot controller 3 calculates the contact external force F (Fx, Fy, Fz) based on the motor state data acquired in step S105 and the position, speed, and acceleration of the end effector calculated in step S110. The calculation method in the present embodiment may be performed by arithmetic processing using the motion equation of (equation 8) described above.

Next, the process proceeds to step S120, and the CPU901 of the robot controller 3 calculates a synthesized component force Fxy from the XY-axis component forces Fx and Fy of the contact external force F calculated in step S115.

Next, the process proceeds to step S125, and the CPU901 of the robot controller 3 determines whether or not the combined component force Fxy calculated in step S120 is equal to or greater than a corresponding threshold value, in other words, whether or not a collision has occurred in the XY axis direction. When the combined component force Fxy is equal to or greater than the threshold value, the determination is satisfied (yes in S125), and the process proceeds to step S130.

In step S130, the CPU901 of the robot control device 3 outputs the determination result that a collision has occurred in the XY axis direction and the combined component force Fxy to the work control unit 33 as abnormality determination information. Then, the process proceeds to step S135.

On the other hand, if the combined component force Fxy is smaller than the threshold value in the determination of step S125, the determination is not satisfied (no in S125), and the process proceeds to step S135.

In step S135, the CPU901 of the robot controller 3 determines whether or not the Z-axis component Fz of the contact external force F calculated in step S115 is equal to or greater than a corresponding threshold value, in other words, whether or not a collision occurs in the Z-axis direction. When the component force Fz is equal to or greater than the threshold value, the determination is satisfied (yes in S135), and the process proceeds to step S140.

In step S140, the CPU901 of the robot control device 3 outputs the determination result that a collision has occurred in the Z-axis direction and the component force Fz to the work control unit 33 as abnormality determination information. Then, the process proceeds to step S145.

On the other hand, if the neutral force Fz is smaller than the threshold value in the determination of step S135, the determination is not satisfied (no in S135), and the process proceeds to step S145.

In step S145, the CPU901 of the robot controller 3 determines whether or not dislocation has occurred, based on the contact external force F (or its time change, etc.) calculated in step S115. When the dislocation has occurred, the judgment is satisfied (S145: YES), and the process proceeds to step S150.

In step S150, the CPU901 of the robot controller 3 outputs the determination result that the dislocation has occurred to the work control unit 33 as abnormality determination information. Then, the process returns to step S105, and the same procedure is repeated.

On the other hand, when dislocation does not occur in the determination of step S145, the determination is not satisfied (S145: NO), the process returns to step S105, and the same procedure is repeated.

By repeating the above steps, the abnormality determination unit 32 sequentially outputs information on the determination result to the work control unit 33 as abnormality determination information when the mechanism unit 50 of the parallel link robot 4 is collided or dislocated.

Next, the control procedure of the job control process will be described with reference to the flowchart of fig. 12. When the control of the parallel link robot 4 is started, the job control process is executed in parallel with the abnormality determination process of fig. 11.

First, in step S205, the CPU901 of the robot controller 3 acquires a job command input from the host controller 2.

Next, the process proceeds to step S210, and the CPU901 of the robot controller 3 calculates the next movement destination position of the end effector as a coordinate position command in accordance with the control procedure corresponding to the job command acquired in step S205, and outputs the coordinate position command to the motion control unit 34.

Next, the process proceeds to step S220, and the CPU901 of the robot controller 3 acquires the abnormality determination information input from the abnormality determination unit 32.

Next, the process proceeds to step S225, and CPU901 of robot controller 3 determines whether or not some abnormality determination result, in other words, whether or not a determination result such as collision or dislocation is included in the abnormality determination information acquired in step S220. If the abnormality determination result is not included, the determination is not satisfied (no in S225), and the process proceeds to step S230.

In step S230, the CPU901 of the robot controller 3 determines whether or not the job sequence executed at that time has ended. If the job sequence is not completed but is being executed, the determination is not satisfied (no in S230), the process returns to step S210, and the same procedure is repeated.

On the other hand, when the job sequence is completed, the judgment is satisfied (yes in S230), the process returns to step S205, and the same procedure is repeated.

On the other hand, if the abnormality determination result is included in the abnormality determination information in the determination of step S225, the determination is satisfied (yes in S225), and the process proceeds to step S235. At this time, the coordinate position command may be temporarily fixed to stop the output position of each motor 44.

In step S235, the CPU901 of the robot controller 3 determines whether or not it is determined that a collision in the XY axis direction has occurred, in other words, whether or not the abnormality determination information includes a determination result indicating that a collision in the XY axis direction has occurred. When it is determined that the collision in the XY axis direction has occurred, the determination is satisfied (yes in S234), and the process proceeds to step S240.

In step S240, the CPU901 of the robot control device 3 calculates a movement destination position at which the movable unit 42 is moved in the retreat direction from the synthesized component force Fxy included in the abnormality determination information, and outputs the calculated movement destination position to the motion control unit 34 as a coordinate position command. Then, the process proceeds to step S245.

On the other hand, if it is not determined in the determination of step S235 that the collision in the XY axis direction has occurred, the determination is not satisfied (no in S235), and the process proceeds to step S245.

In step S245, the CPU901 of the robot controller 3 determines whether or not it is determined that a collision in the Z-axis direction has occurred, in other words, whether or not the abnormality determination information includes a determination result indicating that a collision in the Z-axis direction has occurred. When it is determined that a collision in the Z-axis direction has occurred, the determination is satisfied (yes in S245), and the process proceeds to step S250.

In step S250, the CPU901 of the robot control device 3 outputs a torque limit signal to each servo 35, calculates a movement destination position for moving the movable unit 42 up and down in accordance with the component force Fz included in the abnormality determination information, and outputs the calculated movement destination position to the motion control unit 34 as a coordinate position command. Then, the process proceeds to step S255.

On the other hand, if it is not determined in the determination of step S245 that a collision in the Z-axis direction has occurred, the determination is not satisfied (no in S245), and the process proceeds to step S255.

In step S255, the CPU901 of the robot controller 3 determines whether or not the dislocation is determined to have occurred, in other words, whether or not the abnormality determination information includes the determination result indicating that the dislocation has occurred. When it is determined that dislocation has occurred, the determination is satisfied (yes in S255), and the process proceeds to step S260.

In step S260, the CPU901 of the robot controller 3 outputs a servo-off signal to each of the servers 35. Then, the process proceeds to step S265.

On the other hand, if it is not determined that dislocation has occurred in the determination of step S255, the determination is not satisfied (no in S255), and the process proceeds to step S265.

In step S265, CPU901 of robot controller 3 notifies the user of the occurrence of an abnormality such as a collision or dislocation in mechanism unit 50 of parallel link robot 4 via a display unit or the like not particularly shown. Then, the flow ends.

<1-11 > effects of the present embodiment

As described above, the robot controller 3 of the present embodiment includes: a drive control unit 31 that controls the plurality of motors 44 of the parallel link robot 4; and an abnormality determination unit 32 that determines at least one of collision and dislocation of the mechanism unit 50 of the parallel link robot 4 based on the state data (motor torque command, motor detection position, and motor detection speed) of the plurality of motors 44. As a result, it is possible to accurately determine the occurrence of a collision with the outside in the mechanism unit 50 and dislocation in each spherical bearing 48, 49 based on the state data of all the plurality of motors 44 that drive the mechanism unit 50 of the parallel link robot 4.

The robot control system 1 of the present embodiment uses the parallel link robot 4 controlled by cooperatively driving the plurality of rotary motors 44a to 44c, but can also be applied to a case of using a parallel link robot (not particularly shown) controlled by cooperatively driving a plurality of direct-acting (linear) motors. In this case, the encoder unit 14 is replaced with a linear scale capable of detecting the moving position (moving speed) of the movable element, and the reduction gear 15 is not required. In addition, in the derivation of the motion equation of the above (equation 8), the motion equation may be derived from a mechanism model of a parallel link robot using a direct-acting motor.

In the present embodiment, in particular, when the abnormality determination unit 32 determines a collision, the drive control unit 31 controls the plurality of motors 44 so that the movement of the mechanism unit 50 differs between a collision in the XY-axis direction and a collision in the Z-axis direction in the movement space of the XYZ-axis rectangular coordinate system, the Z-axis direction of which corresponds to the vertical direction. As a result, in the pick-and-place operation of the parallel link robot 4 that performs the movement operation in the horizontal direction (XY-axis direction) and the movement operation in the vertical direction (Z-axis direction) in combination, it is possible to perform functional handling in the case where a collision occurs.

In the present embodiment, in particular, when the abnormality determination unit 32 determines a collision in the XY axis direction, the drive control unit 31 controls the plurality of motors 44 so that the end effector included in the mechanism unit 50 moves to a relative position separated from the collision point P by a predetermined distance in the XY axis direction. For example, even if a collision occurs with the mechanism portion 50 in the horizontal direction, in many cases, the influence of the collision can be made less in both the mechanism portion 50 and the foreign matter 74 simply by moving the mechanism portion 50 away from the collision direction in the horizontal direction. In this way, in response to a collision in the horizontal direction, the drive control unit 31 can quickly and reliably respond appropriately by cooperatively controlling the plurality of motors 44 so as to move only to the predetermined position. The destination position at the time of collision determination is not limited to the relative position from the collision point P as described above, and may be moved to an absolute position such as a so-called home position set in advance in the robot coordinate system.

Further, the movement of the end effector in the XY-axis direction may also be stopped immediately for the collision in the XY-axis direction. That is, when the abnormality determination unit 32 determines that the collision occurs in the XY-axis direction, the drive control unit 31 may control the plurality of motors 44 so as to stop the movement of the end effector included in the mechanism unit 50. For example, when a collision occurs with the mechanism portion 50 in the horizontal direction, the movement in the XY axis direction needs to be stopped at minimum in order not to increase the influence of the collision in both the mechanism portion 50 and the foreign matter 74. In this way, by the drive control unit 31 cooperatively controlling the plurality of motors 44 so as to stop the movement in the XY axis direction in response to the collision in the horizontal direction, the burden of the arithmetic processing in the drive control unit 31 can be eliminated as compared with the movement to the predetermined position, and appropriate measures can be more quickly and reliably taken. In the movement stop control at this time, the work control unit 33 may continuously output the same coordinate position command to fix the position of the end effector. Alternatively, the work control unit 33 may output a servo-off signal to stop the supply of the motor drive power to each motor 44, and output a brake signal to brake the position of each motor 44.

In the present embodiment, in particular, when the abnormality determination unit 32 determines a collision in the Z-axis direction, the drive control unit 31 controls the plurality of motors 44 so as to move the end effector included in the mechanism unit 50 to a relative position separated from the collision point P by a predetermined distance in the Z-axis direction while limiting the output torque of the plurality of motors 44. For example, when the mechanism portion 50 collides in the vertical direction, the mechanism portion 50 may press downward with the foreign matter 74 interposed between the belt conveyor or the like and the floor surface. On the other hand, by limiting the output torque of each motor 44 in addition to moving the mechanism portion 50 from the collision direction in the vertical direction (ascending direction and descending direction), the influence of both the mechanism portion 50 and the foreign matter 74 can be quickly and reliably reduced. The destination position at the time of collision determination is not limited to the relative position from the collision point P as described above, and may be moved to an absolute position such as a so-called home position set in advance in the robot coordinate system.

In the present embodiment, in particular, when the abnormality determination unit 32 determines dislocation, the drive control unit 31 stops the control of the plurality of motors 44. Accordingly, for example, even when the spherical bearings 48 and 49 are dislocated and the mechanism unit 50 is disassembled, the operation control unit 33 stops the control by outputting the servo off signal and stopping the servo off for supplying the motor drive power to the motors 44, and thus the free operation can be allowed without applying unnecessary exciting force and restraining force to the respective link mechanisms, and therefore, the disturbance to the surroundings can be reduced.

<1-12. modified example >

The embodiments described above can be variously modified without departing from the spirit and technical idea thereof.

In the above embodiment, the abnormality determination unit 32 calculates the contact external force F estimated to be applied to the mechanism unit 50 based on the state data (the motor torque command, the motor detection position, and the motor detection speed) of each motor 44, and directly determines the occurrence of the collision and dislocation of the mechanism unit 50 based on the contact external force F, but is not limited thereto. Alternatively, the external disturbance torques of the motors 44 may be further calculated from the calculated contact external force F, and collision and dislocation may be determined from these disturbance torques. In this case, the external disturbance torque described above corresponds to a potential torque value included in the motor torque command of each motor 44 as a resistance amount against the contact external force F. In order to estimate the external disturbance torque, the contact external force F may be converted into an estimated external disturbance of each motor 44 by an external disturbance observer appropriately designed for each motor 44 in consideration of the mechanism model shown in fig. 7. The external disturbance observer may be designed based on the relationship between the contact external force F of the generalized force Q and the shaft torque Trq in the above (equation 8) (not shown in particular).

Since the external disturbance torque of each motor 44 estimated in this way is a torque value that simply corresponds to the contact external force F applied to the mechanism unit 50, a collision determination with high reliability and sensitivity can be performed by comparing the external disturbance torque with a threshold value set at a very low value. Further, by performing the determination based on the external disturbance torque, it is possible to perform the dislocation determination with high accuracy. In order to estimate the contact external force F and the external disturbance torque with higher accuracy, the encoder unit 14 of the motor 44 may detect the rotational position of the shaft SH as the output shaft of the reduction gear 15 as a motor detection position without detecting the rotor of the motor 44, and estimate the position, speed, acceleration, contact external force F, and the external disturbance torque of the encoder with reference to the corresponding motor torque command and motor detection speed.

<2 > embodiment 2 >

In the above-described embodiment 1, the contact external force F used in the determination of collision and dislocation is calculated from the equation of motion based on the state data of each motor 44, but is not limited thereto. In addition, the contact external force F may be obtained by comparing a detection value of a sensor provided in the mechanism unit 50 with motor state data, and embodiment 2 of the configuration will be described below. Further, illustration and description of the same configuration and processing as those of embodiment 1 are omitted.

Fig. 13 corresponding to fig. 1 shows a schematic configuration of a robot control system 1A according to the present embodiment. In fig. 13, the parallel link robot 4 has a three-dimensional acceleration sensor 16 on the upper surface of the movable portion 42. The three-dimensional acceleration sensor 16 detects accelerations of the movable portion 42 corresponding to the X-axis direction, the Y-axis direction, and the Z-axis direction of the robot coordinate system as coordinate detection accelerations, and outputs the coordinate detection accelerations to the robot control device 3. Fig. 14 corresponding to fig. 3 shows the internal configuration of the robot controller 3 and various information transmitted and received around the internal configuration in the present embodiment. In fig. 14, the coordinate detection acceleration detected by the three-dimensional acceleration sensor 16 is input to the abnormality determination unit 32 of the robot controller 3.

In the robot control system 1A having the above configuration, the abnormality determination unit 32 can calculate the acceleration (and the position and the velocity) of the end effector from the motor state data input from each servo 35, detect the acceleration from these calculated values and the coordinates input from the three-dimensional acceleration sensor 16, and estimate the contact external force F acting on the mechanism unit 50. For example, it is considered that the difference between the calculated acceleration of the end effector and the coordinate detection acceleration is proportional to the component forces Fx, Fy, and Fz of the contact external force F in each of the XYZ axes (F ═ a · m). From this study, the component forces Fx, Fy, and Fz of the contact external force F can be obtained in the respective axial directions of the XYZ axes in consideration of the respective specification parameters of the mechanism model shown in fig. 7. Further, the external disturbance torque of each motor 44 can be further estimated from the contact external force F thus obtained.

Thus, in embodiment 2, the contact external force F acting on the mechanism unit 50 in each axial direction can be easily calculated by calculation processing with less burden than in embodiment 1 described above using the equation of motion. Further, the external disturbance torque can be calculated by using the external disturbance observer designed according to the above-described (equation 8) with reference to the motor state data input from each servo 35.

<3. example of hardware configuration of robot control device >

Next, a description will be given of an example of a hardware configuration of the robot control device 3 that realizes processing performed by the drive control unit 31 (the work control unit 33, the motion control unit 34, the servo 35), the abnormality determination unit 32, and the like, which are installed in software by the program executed by the CPU901 described above, with reference to fig. 15.

As shown in fig. 15, the robot control device 3 includes, for example, an application specific integrated circuit 907 configured for a specific application such as a CPU901, a ROM903, a RAM905, an ASIC, or an FPGA, an input device 913, an output device 915, a recording device 917, a drive 919, a connection port 921, and a communication device 923. These components are connected to each other via a bus 909 and an input/output interface 911 so as to be able to transmit signals.

The program can be recorded in advance in the ROM903, the RAM905, the recording device 917, or the like, for example.

The program may be temporarily or permanently recorded in advance in a magnetic disk such as a flexible disk, an optical disk such as various CD, MO, or DVD, or a removable recording medium 925 such as a semiconductor memory, for example. Such a recording medium 925 can also be provided as a so-called software package. In this case, the programs recorded in the recording medium 925 may be read out by the drive 919 and recorded in the recording device 917 via the input/output interface 911, the bus 909, and the like.

The program may be recorded in advance in, for example, a download site, another computer, another recording device, or the like (not shown). In this case, the program is transmitted via a network NW such as a LAN or the internet, and the communication device 923 receives the program. The program received by the communication device 923 may be recorded in the recording device 917 via the input/output interface 911, the bus 909, or the like.

Further, the program can be recorded in advance in an appropriate external connection device 927, for example. In this case, the program may be transferred through an appropriate connection port 921 and recorded in the recording device 917 via the input/output interface 911, the bus 909, and the like.

The CPU901 executes various processes in accordance with the programs recorded in the recording device 917, thereby realizing the processes performed by the drive control unit 31 (the work control unit 33, the motion control unit 34, the servo 35), the abnormality determination unit 32, and the like. In this case, the CPU901 may read out a program directly from the recording device 917 and execute the program, or may temporarily load the program into the RAM905 and execute the program. Further, for example, when the CPU901 receives a program via the communication device 923, the drive 919, and the connection port 921, the received program may be directly executed without being recorded in the recording device 917.

The CPU901 may perform various processes according to signals and information input from an input device 913 such as a mouse, a keyboard, and a microphone (not shown), as needed.

The CPU901 may output the result of the execution of the processing from the output device 915 such as a display device or an audio output device, or the CPU901 may transmit the result of the processing via the communication device 923 or the connection port 921 as necessary, or may record the result in the recording device 917 or the recording medium 925.

In the above description, when there are descriptions such as "vertical", "parallel", and "planar", the description is not intended to be strict. That is, these terms "perpendicular", "parallel" and "planar" are intended to mean "substantially perpendicular", "substantially parallel" and "substantially planar", which allow design and manufacturing tolerances and errors.

In the above description, when there are descriptions such as "identical", "same", "equal", and "different" in terms of apparent size, shape, and position, the descriptions are not intended to be strict. That is, these terms "identical", "equal" and "different" are intended to mean "substantially identical", "substantially identical" and "substantially different", which allow design and manufacturing tolerances and errors.

In addition to the above, the methods of the above embodiments and the modifications may be used in combination as appropriate. In addition, although not illustrated, the above embodiment and each modification can be implemented by applying various modifications without departing from the scope of the invention.