Control device, actuator, and collision detection method

Abstract

To accurately detect a collision of a moving body with an object.SOLUTION: A control device 10 estimates the position of a moving object 27 on the basis of a position command value regarding a moving object 27, and determines that the moving object 27 has collided with an object when a current value of a motor 22 exceeds a first threshold and the difference between the estimated position of the moving object 27 and the current position of the moving object 27 exceeds a second threshold.SELECTED DRAWING: Figure 1

Description

Technical Field

[0001] The present invention relates to a control device, an actuator, and a collision detection method.

Background Art

[0002] For example, Patent Document 1 describes an actuator control device that converts the rotational motion of a motor into linear motion or the like of a moving body such as a slider. This actuator control device detects that the moving body has collided with an object such as an obstacle when the value of the current flowing through the motor exceeds a predetermined threshold during a period in which the moving speed of the moving body is constant.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] However, with the collision detection technology based on comparing the motor current value and the threshold as described above, collision cannot be detected during acceleration and deceleration of the moving body. More specifically, since the current value becomes relatively large during acceleration and deceleration of the moving body, using the same threshold will result in erroneous detection.

[0005] The present invention has been made in consideration of the above facts, and an object thereof is to provide a control device, an actuator, and a collision detection method capable of accurately detecting collision of a moving body with an object.

Means for Solving the Problems

[0006] To achieve the above object, a control device according to a first aspect is a control device that controls a motor for moving a moving body, and comprises: an estimation unit that estimates the position of the moving body based on a position command value related to the moving body; and a collision determination unit that determines that the moving body has collided with an object when a current value of the motor exceeds a first threshold, and a difference between an estimated position estimated by the estimation unit and a current position of the moving body exceeds a second threshold.

[0007] According to the first aspect, collision of the moving body with an object can be detected with high accuracy.

[0008] Further, in the control device according to the second aspect based on the control device according to the first aspect, the estimated position is calculated by executing low-pass filter processing using the reciprocal of a position gain as a time constant.

[0009] According to the second aspect, a highly accurate estimated position can be obtained by executing the low-pass filter processing.

[0010] Further, in the control device according to the third aspect based on the control device according to the first aspect or the second aspect, the current value is a detected current value of the motor, or a current command value calculated based on the position command value.

[0011] According to the third aspect, the current value can be obtained as the detected current value or the current command value.

[0012] Further, in the control device according to the fourth aspect based on the control device according to any one of the first to third aspects, the collision determination unit determines that the moving body has collided with the object when a state where the current value exceeds the first threshold and the difference between the estimated position and the current position exceeds the second threshold continues for a predetermined time.

[0013] According to the fourth aspect, erroneous determinations can be suppressed by adding the condition that the state satisfying the collision determination condition continues for the predetermined time.

[0014] Furthermore, in the control device according to the fifth embodiment, in the control device according to any one of the first to fourth embodiments, the collision determination unit determines that the moving body has collided with the object when the current value exceeds the first threshold, the difference between the estimated position and the current position is less than or equal to the second threshold, and the current position is outside a predetermined range with respect to the target position when the estimated position reaches the target position which is the position that the moving body is intended to reach.

[0015] According to the fifth embodiment, collisions near the target position can be detected with high accuracy.

[0016] Furthermore, in the control device according to the sixth embodiment, the collision determination unit determines that the moving body has collided with the object when the current value exceeds the first threshold, the difference between the estimated position and the current position is less than or equal to the second threshold, and the current position remains outside a predetermined range relative to the target position for a predetermined period of time when the estimated position reaches the target position.

[0017] According to the sixth embodiment, misjudgments can be suppressed by adding a condition that the state satisfying the collision judgment conditions continues for a predetermined time.

[0018] Furthermore, the control device according to the seventh embodiment is a control device for controlling a motor that moves a moving body, and comprises: an estimation unit that estimates the position of the moving body based on a position command value relating to the moving body; and a collision determination unit that determines that the moving body has collided with an object when the current value of the motor exceeds a first threshold and the estimated position estimated by the estimation unit reaches a target position which is the target position to be reached by the moving body, and the current position of the moving body is outside a predetermined range with respect to the target position.

[0019] According to the seventh embodiment, collisions near the target position can be detected with high accuracy.

[0020] Furthermore, in order to achieve the above objective, the actuator according to the eighth embodiment includes a mobile body, a motor for moving the mobile body, and a control device according to any one of the first to seventh embodiments for controlling the motor.

[0021] According to the eighth aspect, an actuator capable of accurately detecting collisions between a moving object and another object can be provided.

[0022] Furthermore, in order to achieve the above objective, the collision detection method according to the ninth embodiment is a collision detection method executed by a control device that controls a motor that moves a moving body, which estimates the position of the moving body based on a position command value relating to the moving body, and determines that the moving body has collided with an object when the current value of the motor exceeds a first threshold and the difference between the estimated position and the current position of the moving body exceeds a second threshold.

[0023] According to the ninth aspect, similar to the first aspect described above, collisions of moving objects can be detected with high accuracy. [Effects of the Invention]

[0024] As described above, the present invention provides the effect of being able to accurately detect collisions between moving objects. [Brief explanation of the drawing]

[0025] [Figure 1] This figure schematically shows an example of the configuration of the control device and actuator according to the first embodiment. [Figure 2] This is a block diagram showing an example of the electrical configuration of a control unit included in the control device according to the first embodiment. [Figure 3] This waveform diagram shows examples of changes in the movement speed of a moving object in various movement commands. [Figure 4] This figure illustrates the collision detection function related to the comparative example. [Figure 5] This is a block diagram showing an example of the functional configuration of a control device according to the first embodiment. [Figure 6] This diagram schematically shows the state of the actuator during collision detection according to the first embodiment. [Figure 7] This figure illustrates the collision detection process using an actuator according to the first embodiment. [Figure 8] This diagram illustrates the collision detection process until the collision detection status is turned on. [Figure 9] This flowchart shows an example of the collision detection process flow by the control program according to the first embodiment. [Figure 10] This figure schematically shows the state of the actuator during collision detection according to the second embodiment. [Figure 11] This figure illustrates the collision detection process using an actuator according to the second embodiment. [Figure 12] This diagram schematically illustrates examples of when the current position of a moving object is outside the positioning range and when it is within the positioning range. [Figure 13] This flowchart shows an example of the collision detection process flow by the control program according to the second embodiment. [Modes for carrying out the invention]

[0026] Hereinafter, an example of an embodiment for carrying out the technology of this disclosure will be described in detail with reference to the drawings. Components and processes that perform the same operation, action, or function are given the same reference numerals throughout the drawings, and redundant explanations may be omitted as appropriate. Each drawing is only a schematic representation to the extent that the technology of this disclosure can be fully understood. Therefore, the technology of this disclosure is not limited to the illustrated examples. Furthermore, in this embodiment, explanations of configurations not directly related to the present invention or well-known configurations may be omitted.

[0027] [First Embodiment] Figure 1 is a schematic diagram showing an example of the configuration of the control device 10 and actuator 20 according to the first embodiment.

[0028] As shown in Figure 1, the actuator 20 comprises an encoder 21, a motor 22, an output shaft 23, a coupling 24, a ball screw shaft 25, a ball screw nut 26, a movable body 27, and a housing 28. Note that the ball screw shaft 25 and ball screw nut 26 may be a sliding screw shaft and sliding screw nut. Also, in the example in Figure 1, the movable body 27 is shown as a table type, but it may be a rod type.

[0029] The encoder 21 is attached to the motor 22. The encoder 21 detects the rotational position of the motor 22 and outputs the detected position as a feedback position signal to the control device 10.

[0030] Motor 22 is controlled by the control device 10 and is the drive source for moving the mobile body 27. Motor 22 is, for example, a three-phase AC servo motor. However, it is not limited to this, and motor 22 may be a DC brushless motor or a stepping motor, etc.

[0031] The output shaft 23 of the motor 22 is connected to a ball screw shaft 25 via a coupling 24. The ball screw shaft 25, together with a ball screw nut 26, constitutes a mechanical component for converting the rotational motion of the motor 22 into translational motion. The movable body 27 is joined to the ball screw shaft 25 via the ball screw nut 26.

[0032] The housing 28 accommodates the output shaft 23, coupling 24, ball screw shaft 25, and ball screw nut 26. The ball screw shaft 25 is rotatably supported within the housing 28 by bearings (not shown). The top surface of the housing 28 is open to allow the movable body 27 to move on the upper part of the housing 28. However, this opening is covered by a sheet member such as a stainless steel sheet (not shown) in a structure that does not obstruct the movement of the movable body 27 on the upper part of the housing 28.

[0033] The control device 10 comprises a control unit 100, an inverter unit 101, and a current sensor 29, and is a controller that controls the motor 22 by vector control using d-axis current command values ​​and q-axis current command values. The control unit 100, inverter unit 101, and current sensor 29 are housed, for example, in a housing. The d-axis current command value and q-axis current command value are command values ​​for controlling the energizing current of the motor 22. The d-axis is the direction of magnetic flux, and the q-axis is the direction perpendicular to the d-axis and contributes to the magnitude of torque. In Figure 1, the control device 10 is provided separately from the actuator 20, but it may be built into the actuator 20. That is, the actuator 20 may include the control device 10. Also, the inverter unit 101 and current sensor 29 may be built into the actuator 20. That is, the actuator 20 may include the inverter unit 101 and current sensor 29, in which case the control device 10 may consist only of the control unit 100.

[0034] The inverter unit 101 is a drive circuit (driver) that converts DC power supplied via an AC power source and converter (not shown) into AC power and supplies it to the motor 22. The inverter unit 101 is provided in accordance with the multiple windings of the motor 22 (U, V, and W phase windings in the case of three phase) and is composed of multiple switching elements whose on / off drive is controlled by the control unit 100. The current sensor 29 is composed of, for example, a magnetic detection element such as a Hall element. More specifically, the current sensor 29 is provided in the current paths between the inverter unit 101 and the multiple windings of the motor 22, specifically in the current paths leading to the U phase winding and the W phase winding, and outputs a signal to the control unit 100 corresponding to the current flowing through the motor 22.

[0035] Figure 2 is a block diagram showing an example of the electrical configuration of the control unit 100 included in the control device 10 according to the first embodiment.

[0036] As shown in Figure 2, the control unit 100 of the control device 10 according to this embodiment includes a CPU (Central Processing Unit) 11, a ROM (Read Only Memory) 12, a RAM (Random Access Memory) 13, an input / output interface (I / O) 14, a storage unit 15, and a connection unit 16.

[0037] The CPU 11, ROM 12, RAM 13, and I / O 14 are connected to each other via a bus. Each functional unit, including the storage unit 15 and the connection unit 16, is connected to the I / O 14. These functional units are capable of communicating with the CPU 11 via the I / O 14.

[0038] The CPU 11, ROM 12, RAM 13, and I / O 14 constitute, for example, a microcomputer (MCU). The MCU may be configured as a sub-control unit that controls the operation of a part of the control unit 100, or as part of the main control unit that controls the operation of the entire control unit 100. Some or all of the blocks of the MCU may use integrated circuits such as LSIs (Large Scale Integration) or ICs (Integrated Circuit) chipsets. Individual circuits may be used for each of the above blocks, or circuits that integrate some or all of them may be used. The above blocks may be provided as a single unit, or some of the blocks may be provided separately. Furthermore, parts of each of the above blocks may be provided separately. For the integration of the MCU, dedicated circuits or general-purpose processors may be used, not just LSIs.

[0039] For the storage unit 15, for example, an HDD (Hard Disk Drive), SSD (Solid State Drive), or flash memory may be used. The ROM 12 or storage unit 15 stores a control program for controlling the motor 22, various setting values ​​necessary for controlling the motor 22, a data table, and the like.

[0040] The control program may be pre-installed in the control unit 100, for example. The control program may also be stored on a non-volatile storage medium or distributed via a network and installed in the control unit 100 as appropriate. Examples of non-volatile storage media include CD-ROMs (Compact Disc Read Only Memory), magneto-optical disks, HDDs, DVD-ROMs (Digital Versatile Disc Read Only Memory), flash memory, and memory cards.

[0041] The connection section 16 is an interface for connecting to the encoder 21, the motor 22, and a higher-level device such as a PLC (Programmable Logic Controller).

[0042] Figure 3 is a waveform diagram showing examples of the transition in the movement speed of the mobile body 27 under various movement commands. The vertical axis shows the movement speed of the mobile body 27, and the horizontal axis shows time.

[0043] Examples of movement commands include positioning operation commands and home position return operation commands. Positioning operation is an operation in which the moving body 27 is moved from the starting point of the positioning operation to the target position of the positioning operation in accordance with the rotation of the motor 22. The target position is the position that the moving body 27 is intended to reach. For example, as shown by the arrow in Figure 1, in a positioning operation to the forward end, the moving body 27 moves from the home (rear end) to the forward end, and in a positioning operation to the home (rear end), it moves from the forward end to the home (rear end). Home position return operation is an operation in which the moving body 27 is moved from the starting point of the home position return operation to the home (target position of the home position return operation) in accordance with the rotation of the motor 22. For example, the moving body 27 moves from the forward end to the home (rear end).

[0044] Figure 3 shows an example of the time change in the movement speed of the moving body 27 during various operations. More specifically, Figure 3 shows the time change in the movement speed of the moving body 27 when, for example, the operations are performed in the order of returning to the origin, positioning to the forward end, and positioning to the origin (reverse end). As shown in Figure 3, the moving body 27 is moved at a relatively high speed during the positioning operation and at a relatively low speed during the returning to the origin operation. In the example shown in Figure 3, the relationship of the movement speed of the moving body 27 in each operation is as follows: speed during positioning to the forward end > speed during positioning to the origin (reverse end) > speed during returning to the origin operation.

[0045] By the way, as mentioned above, in the collision detection function that compares the feedback current value (hereinafter simply referred to as "FB current value") as the motor current value with a threshold value, the current value becomes relatively large during acceleration and deceleration of the moving object, so using the same threshold value will result in false detections.

[0046] Figure 4 illustrates the collision detection function in a comparative example and shows the relationship between the velocity of the moving object and the FB current. In Figure 4, the horizontal axis represents the position of the moving object, the left vertical axis represents the current value, and the right vertical axis represents the velocity of the moving object.

[0047] As shown in Figure 4, the collision detection function in the comparative example is set only during the period when the moving object travels at a constant speed from the starting position to the target position. In other words, a collision is determined when the FB current value (for example, the torque current detection value (q-axis current detection value)) exceeds a threshold during the constant speed period. In this case, collisions cannot be detected outside the constant speed period, i.e., during acceleration and deceleration near the starting position and near the target position. Furthermore, if this collision detection function is applied during acceleration and deceleration, the FB current value will be larger during acceleration and deceleration than during the constant speed period, so using the same threshold will result in false detections.

[0048] In contrast, the control device 10 according to this embodiment estimates the position of the moving body 27 based on the position command value for the moving body 27, and determines that the moving body 27 has collided with an object when the current value of the motor 22 exceeds a first threshold and the difference between the estimated position and the current position of the moving body 27 exceeds a second threshold. Here, "object" refers to obstacles and the like that present in the movement path of the moving body 27. Furthermore, "collision" refers to the state in which the moving body 27 hits an object while moving. Furthermore, the estimated position is a hypothetical position to which the moving body 27 would have moved if there were no obstacles or other objects.

[0049] Specifically, the CPU 11 of the control device 10 according to this embodiment functions as the various parts shown in Figure 5 by writing the control program stored in the ROM 12 or memory unit 15 to the RAM 13 and executing it.

[0050] Figure 5 is a block diagram showing an example of the functional configuration of the control device 10 according to the first embodiment.

[0051] As shown in Figure 5, the CPU 11 of the control device 10 according to this embodiment functions as a movement command acquisition unit 111, a driving plan generation unit 112, a first subtractor 113, a first position controller 114, a second subtractor 115, a speed controller 116, a third subtractor 117, a current control unit 118, an estimation unit 119, a collision determination unit 123, a position information acquisition unit 124, a current speed conversion unit 125, an electrical angle conversion unit 126, and a current detection unit 127. The estimation unit 119 also includes a fourth subtractor 120, a second position controller 121, and an integrator 122.

[0052] The movement command acquisition unit 111 acquires movement commands from a higher-level device such as a PLC. If the control device 10 has a program creation function, the control device 10 itself may function as a PLC. These movement commands are, for example, either a positioning operation command or a return-to-home operation command. A positioning operation command is a command for realizing a positioning operation, consisting of parameters including position information, such as a target position indicating the end position of the positioning operation (e.g., the forward end); speed information, such as a target speed indicating the upper limit speed of the positioning operation; and acceleration / deceleration information, consisting of acceleration and deceleration. The positioning operation speed is set to a relatively high target speed so that the positioning operation time is shortened. A return-to-home operation command is a command for realizing a return-to-home operation, consisting of parameters including position information, such as the origin (reverse end) indicating the end position of the return-to-home operation; speed information, such as a target speed indicating the upper limit speed of the return-to-home operation; and acceleration / deceleration information. The return-to-home operation speed is set to a relatively lower target speed than the positioning operation speed (e.g., 20 mm / s or less). The movement command acquisition unit 111 determines the type of movement command from the movement command and outputs the determination result to the operation plan generation unit 112.

[0053] The operation plan generation unit 112 generates a position command value for the target position of the moving body 27 and outputs the generated position command value to the first subtractor 113 and the fourth subtractor 120, respectively. Specifically, the operation plan generation unit 112 generates a position command value based on the current position and parameters including acceleration, deceleration, target speed, and target position of the moving body 27. The target position, target speed, and parameters including acceleration and deceleration of the moving body 27 are obtained from various movement commands. The operation plan generation unit 112 generates a position command value such that the moving speed of the moving body 27 changes over time in a trapezoidal shape with the target speed as the upper limit, as shown in Figure 3 above, for example.

[0054] More specifically, when the operation plan generation unit 112 acquires various movement commands, it determines the distance the moving body 27 should move based on the difference between the current position command value and the target position, and sequentially generates position command values ​​(for example, with a control cycle of 1 ms) so that the speed of the moving body 27 takes the shape of a trapezoid with the target speed as its upper base (or a triangular shape where the target speed is not reached depending on the distance traveled). In other words, the operation plan generation unit 112 accelerates the moving body 27 by the acceleration parameter until the speed reaches the target speed during acceleration, maintains the target speed during constant speed, and decelerates it by the deceleration parameter until the speed becomes zero during deceleration, and sequentially generates position command values ​​so that the area of ​​the trapezoid (or triangular shape where the target speed is not reached depending on the distance traveled), which represents the change in speed over time from the start of movement to the end of movement, i.e., the time integral of the speed, is equal to the distance determined above. Furthermore, the amount of change in the sequentially generated position command values ​​increases during acceleration, decreases during deceleration, and is adjusted so as not to exceed the target speed parameter when the speed is constant. In other words, the target speed acts as a limiter on the rate of change of the position command value. This generates position command values ​​that achieve the behavior shown in Figure 3, i.e., the time-dependent change in speed for various movement commands.

[0055] The first subtractor 113 outputs the position deviation obtained by subtracting the current position (position detection value) from the position information acquisition unit 124 from the position command value from the operation plan generation unit 112 to the first position controller 114.

[0056] The first position controller 114 generates a speed command value by multiplying the position deviation obtained from the first subtractor 113 by a position control gain (set value), and outputs the generated speed command value to the second subtractor 115. The first position controller 114 performs proportional control.

[0057] The second subtractor 115 outputs the speed deviation obtained by subtracting the current speed (speed detection value) from the current speed conversion unit 125 from the speed command value from the first position controller 114 to the speed controller 116.

[0058] The speed controller 116 generates a torque current command value by multiplying the speed deviation obtained from the second subtractor 115 by the proportional gain / integral gain (set value) for speed control and integrating it, and outputs the generated torque current command value to the third subtractor 117. The speed controller 116 performs proportional / integral control. Here, in this embodiment, the d-axis current command value I * In order to control the motor 22 with d=0, in the example of Figure 5, the d-axis current command value I * The diagram of the control block d is omitted. Therefore, the torque current command value is the q-axis current command value I that contributes to the generation of torque in the motor 22. * Let's explain it assuming that it is q.

[0059] The third subtractor 117 receives the torque current command value (=q-axis current command value I) from the speed controller 116. * The current deviation obtained by subtracting the q-axis current detection value Iq, which is the FB current value from the current detection unit 127 described later, from q) is output to the current control unit 118.

[0060] The position information acquisition unit 124 acquires a feedback position signal of the motor 22 from the encoder 21 attached to the motor 22. Based on the feedback position signal, the position information acquisition unit 124 outputs the detected position value of the moving body 27 as the current position to the first subtractor 113, the current speed conversion unit 125, the collision determination unit 123, and the electrical angle conversion unit 126, respectively.

[0061] The current speed conversion unit 125 converts the current position from the position information acquisition unit 124 into a current speed (speed detection value), and outputs the converted current speed (speed detection value) to the second subtractor 115.

[0062] The electrical angle conversion unit 126 converts the current position from the position information acquisition unit 124 into the electrical angle of the motor 22 and outputs it to the current control unit 118.

[0063] The current detection unit 127 detects the current by calculating the q-axis current detection value as the FB current value based on the output from the current sensor 29. More specifically, the current detection unit 127 first receives the U-phase current detection value Iu and the W-phase current detection value Iw from the current sensor 29, and detects the V-phase current detection value from the relationship Iu + Iv + Iw = 0. Next, the current detection unit 127 converts the U, V, and W-phase currents Iu, Iv, and Iw into α and β-axis current detection values ​​in a stationary coordinate system (α and β coordinate system). After that, the current detection unit 127 uses the electrical angle from the electrical angle conversion unit 126 to calculate the d-axis current detection value Id and the q-axis current detection value Iq in a rotating coordinate system (d and q axis coordinate system) from the α and β-axis current detection values. The current detection unit 127 outputs the q-axis current detection value Iq as the FB current value to the third subtractor 117 and the collision determination unit 123. On the other hand, the current detection unit 127, although not shown in the diagram, outputs the d-axis current detection value Id to the current control unit 118.

[0064] The current control unit 118 controls the motor 22 using the current deviation from the third subtractor 117 and the electrical angle from the electrical angle conversion unit 126. More specifically, if the motor 22 is a three-phase AC servo motor, the current control unit 118 first calculates the d-axis current command value I * Let d be 0 (I * When d=0, the current deviation with respect to the d axis is calculated from the detected d-axis current value Id. Next, the current control unit 118 generates d and q axis voltage command values ​​Vd and Vq based on the current deviation with respect to the d and q axes. Next, the current control unit 118 converts the d and q axis voltage command values ​​Vd and Vq into voltage command values ​​for the α and β axes using the electrical angle. After that, the current control unit 118 calculates command values ​​to turn on and off multiple switching elements provided in correspondence with the U, V, and W phase windings of the inverter unit 101 based on the voltage command values ​​for the α and β axes, and controls the drive of the motor 22 by controlling the inverter unit 101 based on these values.

[0065] Meanwhile, the estimation unit 119 estimates the position of the moving body 27 based on the position command value for the moving body 27. The estimation unit 119 calculates the estimated position by, for example, executing a low-pass filter with the reciprocal of the position control gain as the time constant. Specifically, the fourth subtractor 120 outputs the estimated position deviation obtained by subtracting the estimated position obtained from the integrator 122 from the position command value from the operation plan generation unit 112 to the second position controller 121.

[0066] The second position controller 121 generates a velocity estimate by multiplying the estimated position deviation obtained from the fourth subtractor 120 by a position control gain (set value), and outputs the generated velocity estimate to the integrator 122.

[0067] The integrator 122 outputs the estimated position, obtained by integrating the velocity estimate from the second position controller 121, to the fourth subtractor 120 and the collision detection unit 123, respectively. The estimated position is calculated as the ideal position relative to the position command value, taking into account the same position control gain as the position control gain multiplied by the position deviation by the first position controller 114.

[0068] The collision determination unit 123 determines that the moving body 27 has collided with an object if the current value of the motor 22 (FB current value) exceeds a first threshold, and the difference between the estimated position estimated by the estimation unit 119 and the current position of the moving body 27 obtained from the position information acquisition unit 124 exceeds a second threshold. The first and second thresholds are stored in advance in, for example, the ROM 12 or the storage unit 15, and appropriate values ​​are set based on past knowledge, experimental results, etc. In other words, in this embodiment, condition 1 is that the FB current value exceeds the first threshold, and condition 2 is that the difference between the estimated position and the current position exceeds the second threshold, and a collision is determined when both conditions 1 and 2 are met. The collision determination of position using the second threshold is performed, for example, based on the count value of the encoder 21. In this case, the second threshold can be converted to the count value of the encoder 21 and used. For example, if the second threshold is 2 mm, the lead width of the ball screw shaft 25 is 20 mm, and the encoder resolution is 16384 counts, then the second threshold converted to the count value of the encoder 21 will be 2 / 20 × 16384 ≈ 1638.

[0069] Furthermore, the collision determination unit 123 may determine that the moving body 27 has collided with an object if the current value (FB current value) exceeds a first threshold and the difference between the estimated position and the current position exceeds a second threshold for a predetermined period of time. Here, the predetermined period is set to an appropriate value within the range of 5 [ms] to 15 [ms], and preferably to 10 [ms]. Hereinafter, this predetermined period will be referred to as the "collision determination time".

[0070] Next, the collision detection process according to the first embodiment will be specifically described with reference to Figures 6 to 8.

[0071] Figure 6 is a schematic diagram showing the state of the actuator 20 during collision detection according to the first embodiment.

[0072] As shown in Figure 6, when the moving body 27 collides with the object 30, the FB current value exceeds the first threshold, thus satisfying condition 1. However, condition 1 alone cannot rule out the possibility that the moving body 27 is accelerating or decelerating. Furthermore, even at a constant speed, the FB current value may increase due to resistance such as bearings. Therefore, condition 2, which is that the difference D1 between the current position and the estimated position of the moving body 27 exceeds the second threshold, is also determined. If condition 2 is also satisfied, it is determined that the increase in the FB current value is not due to acceleration or deceleration, or resistance such as bearings; in other words, the possibility of acceleration or deceleration, or resistance such as bearings, is ruled out. As a result, collisions can be detected with high accuracy regardless of whether acceleration or deceleration is occurring, or regardless of resistance such as bearings. Therefore, compared to the comparative example described with reference to Figure 4 above, the range in which collisions can be detected can be expanded. In addition, by determining a collision when condition 1 is satisfied along with condition 2, the accuracy of collision detection can be improved compared to determining a collision based solely on the satisfaction of condition 2.

[0073] Figure 7 is a diagram illustrating the collision detection process by the actuator 20 according to the first embodiment. In the example shown in Figure 7, the collision detection time is set to, for example, 10 [ms].

[0074] In FIG. 7, the graph shown in (A) indicates the temporal change in the position of the moving body 27 according to condition 2, and the graph shown in (B) indicates the temporal change in the FB current value according to condition 1. The timing chart shown in (C) illustrates the timing at which collision detection based on the current (FB current value) is turned on, and the timing at which collision detection based on the position difference (the difference between the estimated position and the current position) is turned on. The graph shown in (D) indicates the temporal change in the count value of a collision detection counter (hereinafter also simply referred to as "counter") that measures the collision determination time. The timing chart shown in (E) illustrates the timing at which the collision detection status is turned on. Although not illustrated, the counter is implemented by, for example, a timer function provided in the control device 10.

[0075] However, in (A), P th denotes the second threshold, in (B), I th denotes the first threshold, and in (D), T th denotes the collision determination time (for example, 10 ms).

[0076] In (A), time T1 indicates the time at which the moving body 27 collides with an object. The position of the moving body 27 at time T1 corresponds to the current position. Further, time T3 indicates the time at which the difference between the estimated position and the current position of the moving body 27 exceeds the second threshold P th . At time T3, collision detection based on the position difference shown in (C) is turned on. That is, condition 2 is satisfied.

[0077] In (B), time T2 indicates the time at which the FB current value exceeds the first threshold I th . At time T2, collision detection based on the current shown in (C) is turned on. That is, condition 1 is satisfied.

[0078] In (D), at the point in time when it is determined that conditions 1 and 2 are satisfied, that is, at time T3, the counter starts counting the collision determination time T th . Time T4 is the time at which the elapsed time from time T3 reaches the collision determination time T thThis indicates the time when the collision detection status is reached. At time T4, the collision detection status shown in (E) is turned on. In other words, the counter count value is set to the collision detection time T th When it reaches this point, a collision is detected, and the collision detection status shown in (E) is turned on.

[0079] Based on the above, the state that satisfies conditions 1 and 2 corresponds to a collision detection time T. th If the above conditions persist, it is determined that the moving object 27 has collided with the object. If collision detection is performed immediately when conditions 1 and 2 are met, there is a possibility of false detection. Therefore, in order to further reduce the possibility of false detection, the collision detection time T is set. th It is desirable to provide a collision detection time T. The FB current value also changes depending on the operating state and installation state of the actuator 20. Furthermore, external disturbances such as friction are not taken into account when determining the estimated position of the moving body 27. For this reason, there is a possibility that slight errors may occur in the difference between the current position and the estimated position of the moving body 27 at the start and just before stopping. In order to prevent false collision detection due to such errors, a collision detection time T should be provided. th It is desirable to provide this.

[0080] Figure 8 illustrates the collision detection process until the collision detection status is turned on.

[0081] In Figure 8, the graph shown in (A) shows the time change in the position of the moving body 27 under condition 2, the graph shown in (B) shows the time change in the FB current value under condition 1, and the graph shown in (C) shows the time change in the count value of the counter that measures the collision detection time.

[0082] In (A), the moving object 27 collides with the object at time T11. The position of the moving object 27 at time T11 corresponds to its current position. Also, the difference between the estimated position of the moving object 27 and its current position at time T12 is the second threshold P. th It exceeds the limit and satisfies condition 2.

[0083] (B) In (B), the FB current value between time T11 and time T12 is the first threshold I th It exceeds the limit and satisfies condition 1.

[0084] In (C), when it is determined that conditions 1 and 2 are met, that is, at time T12 the counter is at collision detection time T th The count begins. Time T13 is the elapsed time from time T12, which is the collision detection time T. th This indicates the time when the target was reached. The counter's count value corresponds to the collision detection time T. th When it reaches this point, a collision is detected and the collision detection status is turned on.

[0085] In other words, the collision detection time T12 is from the time T12 when it is determined that conditions 1 and 2 are met. th Start counting, collision detection time T th When this is reached, the collision detection status is turned on. At this time, the collision detection time T th If, before reaching the target, at least one of conditions 1 and 2 is not met, the count value decreases. In the example in Figure 8, the count value decreases because condition 1 is not met between time T12 and time T13, but then increases again when condition 1 is met.

[0086] Next, with reference to Figure 9, the operation of the control device 10 according to the first embodiment will be described.

[0087] Figure 9 is a flowchart showing an example of the collision detection process flow by the control program according to the first embodiment.

[0088] First, when a motor control instruction is given to the control device 10, the CPU 11 starts the control program and executes the following processes. This collision detection process is executed repeatedly at predetermined intervals (for example, a 1ms period). The estimated position is calculated at a shorter time interval than the execution period of this collision detection process (for example, a 100μs period).

[0089] In step S111 of Figure 9, the CPU 11 calculates the difference [Dn] between the estimated position and the current position of the moving object 27.

[0090] In step S112, the CPU 11 determines that the difference [Dn] calculated in step S111 is the second threshold P th Determine whether it exceeds the second threshold P. th If it is determined that the value exceeds (YES: positive determination), the process proceeds to step S113, and the difference [Dn] is the second threshold P th If the following is determined (NO: Negative determination), proceed to step S115.

[0091] In step S113, the CPU 11 checks if the FB current value is the first threshold I th Determine whether or not it exceeds the first threshold I. th If it is determined that the value exceeds the first threshold I (YES: positive determination), the process proceeds to step S114, and the FB current value exceeds the first threshold I th If the following is determined (NO: Negative determination), proceed to step S115.

[0092] In step S114, the CPU 11 determines that a collision has occurred and proceeds to step S116.

[0093] On the other hand, in step S115, the CPU 11 determines that there is no collision and proceeds to step S116.

[0094] In step S116, the CPU 11 determines whether or not a collision has occurred. If a collision is determined (YES: positive determination), the process proceeds to step S117; if no collision is determined (NO: negative determination), the process proceeds to step S118.

[0095] In step S117, the CPU 11 increments the count value of the collision detection counter and proceeds to step S121.

[0096] Meanwhile, in step S118, the CPU 11 decrements the count value of the collision detection counter and proceeds to step S119.

[0097] In step S119, the CPU 11 determines whether the count value of the collision detection counter is less than zero (=0). If it determines that the count value is less than zero (YES: positive determination), it proceeds to step S120. If it determines that the count value is greater than or equal to zero (NO: negative determination), it proceeds to step S121.

[0098] In step S120, the CPU 11 sets the count value of the collision detection counter to "zero (=0)" and proceeds to step S121.

[0099] In step S121, the CPU 11 determines the count value of the collision detection counter during the collision determination time T. th Determine whether the count is greater than or equal to the value of the collision detection time T. th If it is determined that the above is true (YES: positive determination), the process proceeds to step S122, and the count value is determined to be the collision detection time T. th If it is determined to be less than (NO: negative result), proceed to step S124.

[0100] In step S122, the CPU 11 sets the collision detection counter to "Collision determination time T". th Set "".

[0101] In step S123, the CPU 11 turns on the collision detection status as a collision detection, and terminates the collision determination process by this control program.

[0102] On the other hand, in step S124, the CPU 11 turns off the collision detection status, indicating that no collision was detected, and terminates the collision determination process by this control program.

[0103] Here, after collision detection, the control device 10 may notify the system of the collision detection itself by sounding a warning, or it may notify a higher-level device such as a PLC of the collision detection. Alternatively, after collision detection, the control device 10 may decelerate and stop the motor 22.

[0104] Furthermore, a second threshold (e.g., 2 mm) is user-configurable. The second threshold should ideally be set considering factors such as the user's operating conditions, the actuator's size (stroke, lead width of the ball screw as the drive mechanism for the moving body), and the actuator's installation status.

[0105] Furthermore, in constant-speed sections, collisions may be detected based on whether condition 1 is met, and in acceleration / deceleration sections, collisions may be detected based on whether conditions 1 and 2 are met. In this case, whether it is a constant-speed section or an acceleration / deceleration section can be determined by setting a detection zone and determining whether the current position is within the detection zone, as described in Patent Document 1 (Japanese Patent Application Publication No. 2014-087235), or by comparing the target speed of the movement command with the detected speed.

[0106] Furthermore, if the operation plan is "home position return operation" or "pressing operation" based on the information included in the movement command, the collision detection function may be turned off. Although "home position return operation" and "pressing operation" also constitute collisions, they are intentional collisions and are therefore excluded from collision detection. Note that "pressing operation" refers to the operation of pressing the moving body against a workpiece that has been prepared in advance as the target of the pressing, rather than against an obstacle.

[0107] Furthermore, the collision detection time value (e.g., 10ms) can be set by the user. As mentioned above, external disturbances such as friction are not taken into account when generating the estimated position of the moving object. In other words, the difference between the current position and the estimated position of the moving object may become large at the start of operation and just before stopping, so setting whether or not the difference has continued for a predetermined time helps to suppress misjudgments.

[0108] As explained above, according to this embodiment, a collision between a moving object and another object is determined when both conditions 1 and 2 are met. Therefore, the collision can be determined with greater accuracy compared to when only condition 1 is used to determine the collision.

[0109] Here, as the moving object moves from its starting position to the target position, it accelerates, moves at a constant speed at the target speed, and then decelerates; in other words, the speed of the moving object changes in a trapezoidal or triangular shape. Here, the current supplied to the motor during acceleration and deceleration is larger than when moving at a constant speed. For this reason, condition 1 alone does not allow us to determine whether the moving object has collided with an object or is simply accelerating or decelerating. Therefore, by adding condition 2 to collision detection, it becomes possible to detect that the difference between the estimated position that the object would have reached if there had been no collision and the current position has become large, so regardless of whether it is accelerating or decelerating or not, it can be determined that the reason condition 1 is met is due to a collision.

[0110] Furthermore, by adding the condition that the state in which conditions 1 and 2 are met continues for a predetermined period of time, the accuracy of collision detection can be further improved.

[0111] [Second Embodiment] In the first embodiment described above, a method was described in which a collision is determined when conditions 1 and 2 are met. In the second embodiment, in addition to conditions 1 and 2, condition 3 is added, which is that when the estimated position of the moving object reaches the target position, the current position is outside a predetermined range relative to the target position, and a method is described in which a collision is determined taking condition 3 into consideration.

[0112] Referring to Figure 5 above, the functional configuration of the control device 10 according to the second embodiment will be described.

[0113] The collision determination unit 123 determines that the moving body 27 has collided with an object if the current value of the motor 22 exceeds a first threshold, the difference between the estimated position of the moving body 27 and its current position is less than or equal to a second threshold, and the current position is outside a predetermined range relative to the target position when the estimated position of the moving body 27 reaches the target position. Condition 3 is that the current position is outside a predetermined range relative to the target position when the estimated position of the moving body 27 reaches the target position. The predetermined range here is, for example, a range centered on the target position. In other words, the unit determines that the moving body 27 has collided with an object if condition 2 is not met, and conditions 1 and 3 are met.

[0114] Furthermore, the collision determination unit 123 may determine that the moving body 27 has collided with an object if the current value of the motor 22 exceeds a first threshold, the difference between the estimated position of the moving body 27 and its current position is less than or equal to a second threshold, and the state in which the current position is outside a predetermined range relative to the target position when the estimated position of the moving body 27 reaches the target position continues for a predetermined time (i.e., collision determination time).

[0115] Next, the collision detection process according to the second embodiment will be specifically described with reference to Figures 10 to 12.

[0116] Figure 10 is a schematic diagram showing the state of the actuator 20 during collision detection according to the second embodiment.

[0117] As shown in Figure 10, when the moving body 27 collides with the object 30, the FB current value exceeds the first threshold, thus satisfying condition 1. However, if the current position of the moving body 27 is near the target position, the difference D1 between the estimated position and the current position of the moving body 27 becomes small, that is, difference D1 < second threshold P. th Therefore, condition 2 is not met. If condition 1 is met and condition 2 is not met, for example, the possibility of deceleration cannot be ruled out. For this reason, condition 3, which states that when the estimated position of the moving body 27 reaches the target position, the current position is outside a predetermined range relative to the target position (hereinafter referred to as the "positioning range"), is also determined. If condition 3 is also met, even if the current position is near the target position (i.e., the difference between the estimated position of the moving body 27 and the current position is small), the increase in the FB current value is not due to deceleration, meaning the possibility of deceleration is ruled out. As a result, collisions near the target position can be detected with high accuracy, regardless of whether deceleration is occurring or not.

[0118] Figure 11 is a diagram illustrating the collision detection process by the actuator 20 according to the second embodiment. In the example in Figure 11, the case where the collision detection time is, for example, 10 [ms] is shown.

[0119] In Figure 11, the graph shown in (A) shows the time change in the position of the moving body 27 related to condition 3, and the graph shown in (B) shows the time change in the FB current value related to condition 1. The timing chart shown in (C) shows the timing for turning on target position = estimation device detection, timing for turning on positioning completion detection, timing for turning on collision detection by position, timing for turning on collision detection by current (FB current value), and timing for turning on collision detection status.

[0120] However, in (A), P th This indicates the second threshold, and in (B), I th This indicates the first threshold, and in (C), T th This indicates the collision detection time (e.g., 10ms).

[0121] In (A), time T21 indicates the time when the moving object 27 collides with the object. The position of the moving object 27 at time T21 corresponds to its current position. Time T23 indicates the time when the estimated position of the moving object 27 reaches the target position. When the estimated position of the moving object 27 reaches the target position, the difference between the estimated position of the moving object 27 and its current position < second threshold P th Therefore, the current position of the moving object 27 is outside the positioning range relative to the target position. At time T23, the detection of the target position = estimated position shown in (C) is turned on, the detection of positioning completion remains off, and collision detection by position is turned on. In other words, condition 2 is not met, and condition 3 is met.

[0122] In (B), at time T22, the FB current value is the first threshold I th This indicates the time when the threshold was exceeded. At time T22, collision detection using the current shown in (C) is turned on. In other words, condition 1 is satisfied.

[0123] The counter reaches the point at time T23 when it is determined that condition 2 is not met and conditions 1 and 3 are met, i.e., at collision detection time T th The count begins. Time T24 is the elapsed time from time T23, which corresponds to the collision detection time T. thThis indicates the time when the collision detection status is reached. At time T24, the collision detection status shown in (C) is turned on. In other words, the counter count value is equal to the collision detection time T th When this point is reached, a collision is detected, and the collision detection status shown in (C) is turned on.

[0124] Based on the above, the state that satisfies conditions 1 and 3 corresponds to a collision detection time T. th If the above conditions persist, it is determined that the moving object 27 has collided with the object. If collision detection is performed immediately when conditions 1 and 3 are met, there is a possibility of false detection. Therefore, in order to further reduce the possibility of false detection, the collision detection time T is set. th It is desirable to provide this.

[0125] Figure 12 schematically shows examples where the current position of the moving body 27 is outside the positioning range and within the positioning range.

[0126] As shown in Figure 12, a positioning range is set centered on the target position. The positioning range can be pre-set by the user. In the example in Figure 12, the positioning range is set to be -a[mm] to +a[mm] centered on the target position. Positioning position 1 is at (target position - a), and positioning position 2 is at (target position + a).

[0127] For example, if the movement distance of the moving body 27 is 300 [mm], that is, the target position is 300 [mm], and the positioning range is -0.1 [mm] to +0.1 [mm], then in the example outside the positioning range shown in Figure 12, when the current position of the moving body 27 is 299 [mm], the remaining movement is 300 - 299 = 1 [mm], and since 1 [mm] is greater than 0.1 [mm], it is determined that the body is moving (positioning not yet complete), i.e., outside the positioning range. On the other hand, in the example within the positioning range, when the current position of the moving body 27 is 299.95 [mm], the remaining movement is 300 - 299.95 = 0.05 [mm], and since 0.05 [mm] is less than 0.1 [mm], it is determined that the movement is complete (positioning complete), i.e., within the positioning range.

[0128] Next, with reference to Figure 13, the operation of the control device 10 according to the second embodiment will be described.

[0129] Figure 13 is a flowchart showing an example of the collision detection process flow by the control program according to the second embodiment.

[0130] First, when a motor control instruction is given to the control device 10, the CPU 11 starts the control program and executes the following processes. This collision detection process is executed repeatedly at predetermined intervals (for example, a 1ms period). The estimated position is calculated at a shorter time interval than the execution period of this collision detection process (for example, a 100μs period).

[0131] In step S131 of Figure 13, the CPU 11 calculates the difference [Dn] between the estimated position and the current position of the moving object 27.

[0132] In step S132, the CPU 11 determines that the difference [Dn] calculated in step S131 is the second threshold P th Determine whether it exceeds the second threshold P. th If it is determined that the value exceeds (YES: positive determination), the process proceeds to step S133, and the difference [Dn] is the second threshold P. th If the following is determined (NO: Negative determination), proceed to step S136.

[0133] In step S133, the CPU 11 checks if the FB current value is the first threshold I th Determine whether or not it exceeds the first threshold I. th If it is determined that the value exceeds the first threshold I (YES: positive determination), the process proceeds to step S134, and the FB current value exceeds the first threshold I th If the following is determined (NO: Negative determination), proceed to step S135.

[0134] In step S134, the CPU 11 determines that a collision has occurred and proceeds to step S140.

[0135] On the other hand, in step S135, the CPU 11 determines that there is no collision and proceeds to step S140.

[0136] On the other hand, in step S136, the CPU 11 determines whether the estimated position of the moving body 27 has reached the target position and whether the current position of the moving body 27 is outside the positioning range relative to the target position. If it is determined that the estimated position of the moving body 27 has reached the target position and the current position of the moving body 27 is outside the positioning range relative to the target position (YES: affirmative determination), the process proceeds to step S137. If it is determined that the estimated position of the moving body 27 has not reached the target position, or that the current position of the moving body 27 is within the positioning range relative to the target position (NO: negative determination), the process proceeds to step S139.

[0137] In step S137, the CPU 11 determines that the FB current value is the first threshold I th Determine whether or not it exceeds the first threshold I. th If it is determined that the value exceeds the first threshold I (YES: positive determination), the process proceeds to step S138, and the FB current value exceeds the first threshold I th If the following is determined (NO: Negative determination), proceed to step S139.

[0138] In step S138, the CPU 11 determines that a collision has occurred and proceeds to step S140.

[0139] On the other hand, in step S139, the CPU 11 determines that there is no collision and proceeds to step S140.

[0140] In step S140, the CPU 11 determines whether or not a collision has occurred. If a collision is determined (YES: positive determination), the process proceeds to step S141; if no collision is determined (NO: negative determination), the process proceeds to step S142.

[0141] In step S141, the CPU 11 increments the count value of the collision detection counter and proceeds to step S145.

[0142] Meanwhile, in step S142, the CPU 11 decrements the count value of the collision detection counter and proceeds to step S143.

[0143] In step S143, the CPU 11 determines whether the count value of the collision detection counter is less than zero (=0). If it determines that the count value is less than zero (YES: positive determination), it proceeds to step S144. If it determines that the count value is greater than or equal to zero (NO: negative determination), it proceeds to step S145.

[0144] In step S144, the CPU 11 sets the count value of the collision detection counter to "zero (=0)" and proceeds to step S145.

[0145] In step S145, the CPU 11 determines that the count value of the collision detection counter corresponds to the collision determination time T. th Determine whether the count is greater than or equal to the value of the collision detection time T. th If it is determined that the above is true (YES: positive determination), the process proceeds to step S146, and the count value is determined to be the collision detection time T. th If it is determined to be less than (NO: negative result), proceed to step S148.

[0146] In step S146, the CPU 11 sets the collision detection counter to "Collision detection time T". th Set "".

[0147] In step S147, the CPU 11 turns on the collision detection status as a collision detection, and terminates the collision determination process by this control program.

[0148] On the other hand, in step S148, the CPU 11 turns off the collision detection status, indicating that no collision was detected, and terminates the collision determination process by this control program.

[0149] In this case, collisions may be detected in constant-speed sections based on whether condition 1 is met, and in acceleration / deceleration sections based on whether conditions 1 to 3 are met.

[0150] Furthermore, the positioning range centered on the target position (e.g., ±0.1 mm) is user-configurable. The positioning range may be sufficiently smaller than the second threshold.

[0151] As explained above, according to this embodiment, a collision is determined to have occurred when both conditions 1 and 3 are met. Therefore, even when the current position is near the target position (i.e., when the difference between the estimated position of the moving body and the current position is small), the collision can be determined with greater accuracy compared to when only condition 1 is used for determination.

[0152] Here, if a collision occurs when the current position is near the target position, such as when the moving object is decelerating, the estimated position may reach the target position, but since the estimated position does not change any further, the difference between the estimated position and the current position is small, and condition 2 may not be met. In this case, condition 1 may be met due to an increase in the current value during deceleration, so it may not be possible to accurately detect whether or not a collision has occurred. Therefore, by adding condition 3, a collision can be accurately detected even when the current position is near the target position. Thus, the range in which collisions can be detected can be further expanded compared to the comparative example described with reference to Figure 4 above. In this case as well, as described above, by determining a collision when condition 1 is met along with condition 3, the accuracy of collision detection can be improved compared to determining a collision only when condition 3 is met.

[0153] In other words, even when the estimated position of the moving object has reached the target position and will not change further, such as when the moving object is decelerating, if the current position is outside a predetermined range relative to the target position, it can be determined that a collision has occurred between the moving object and the object.

[0154] Modifications of the above embodiments will now be described. Specifically, it is possible to determine whether only conditions 1 and 3 are met without considering condition 2. In this case, the collision determination unit 123 determines that the moving body 27 has collided with an object if the current value of the motor 22 (FB current value) exceeds the first threshold, and when the estimated position of the moving body 27 reaches the target position, its current position is outside a predetermined range (positioning range) relative to the target position.

[0155] In each of the above embodiments, the first threshold value was compared with the FB current value detected by the current detection unit 127 (a current detection value that contributes to the generation of torque of the motor 22 (for example, the q-axis current detection value)) to determine whether condition 1 is met, but this is not the only method. For example, instead of the FB current value, a current command value that contributes to the generation of torque, calculated based on the position command value, may be used as the current value compared with the first threshold value. More specifically, the current command value is the torque current command value (for example, the q-axis current command value I) which is the output of the speed controller 116. * q) is the answer.

[0156] The control devices according to each embodiment have been described with illustrative examples. The embodiments may take the form of a program that causes a computer to execute the functions of the control device. The embodiments may also take the form of a non-temporary storage medium that is readable by a computer and stores these programs.

[0157] Furthermore, the configuration of the control device described in the above embodiment is merely an example, and may be modified as needed without departing from the main purpose.

[0158] Furthermore, the program processing flow described in the above embodiment is just one example, and unnecessary steps may be deleted, new steps added, or the processing order rearranged, as long as it does not deviate from the main purpose.

[0159] Furthermore, although the above embodiment describes a case in which the process according to the embodiment is realized by a software configuration using a computer by executing a program, the embodiment is not limited to this. The embodiment may also be realized by a hardware configuration or a combination of a hardware configuration and a software configuration. [Explanation of Symbols]

[0160] 10 Control device 11 CPU 12 ROM 13 RAM 14 I / O 15 Storage section 16 Connection part 20 Actuators 21 Encoders 22 motors 23 Output shaft 24 Coupling 25 Ball screw shaft 26 Ball screw nut 27 Mobile Unit 28 Housing 29 Current Sensor 100 Control Unit 101 Inverter section

Claims

1. A control device for controlling a motor that moves a mobile object, An estimation unit that estimates the position of the moving body based on the position command value of the moving body, In addition to determining that the moving body has collided with an object when the current value of the motor exceeds a first threshold and the difference between the estimated position estimated by the estimation unit and the current position of the moving body exceeds a second threshold, the collision determination unit determines that the moving body has collided with an object when the current value exceeds the first threshold, the difference between the estimated position and the current position is less than or equal to the second threshold, and the current position is outside a predetermined range centered on the target position when the estimated position reaches the target position which is the position that the moving body is intended to reach. Equipped with, The difference between the upper and lower limits defining the predetermined range and the target position is set to a value smaller than the second threshold. Control device.

2. The estimation unit calculates the estimated position by performing a low-pass filter process with the reciprocal of the position gain as the time constant. The control device according to claim 1.

3. The current value is the current detection value of the motor, or the current command value calculated based on the position command value. The control device according to claim 1.

4. The collision determination unit determines that the moving body has collided with the object if the current value exceeds the first threshold and the difference between the estimated position and the current position exceeds the second threshold for a predetermined period of time. The control device according to claim 1.

5. The collision determination unit determines when the current value exceeds the first threshold, the difference between the estimated position and the current position is less than or equal to the second threshold, and the estimated position reaches the target position. If the current position remains outside a predetermined range relative to the target position for a predetermined period of time, it is determined that the moving body has collided with the object. The control device according to claim 1.

6. Mobile and A motor for moving the aforementioned mobile body, A control device according to any one of claims 1 to 5 for controlling the motor, Actuators including

7. A collision detection method performed by a control device that controls a motor that moves a moving object, Based on the position command value for the moving body, the position of the moving body is estimated. In addition to determining that the moving body has collided with an object when the current value of the motor exceeds a first threshold and the difference between the estimated position and the current position of the moving body exceeds a second threshold, the moving body has also been determined to have collided with an object when the current value exceeds the first threshold, the difference between the estimated position and the current position is less than or equal to the second threshold, and the current position is outside a predetermined range centered on the target position when the estimated position reaches the target position which is the position the moving body is intended to reach. The difference between the upper and lower limits defining the predetermined range and the target position is set to a value smaller than the second threshold. Collision detection method.

Images