Robot control apparatus

Abstract

To provide a robot control apparatus capable of generating restitutive force appropriately in accordance with a direction of contact between a moving part and an article in a case where the moving part contacts the article while the moving part is on the move.SOLUTION: A robot control apparatus is configured, including a moving part and an actuator capable of changing a position of the moving part, to set restitutive force for moving the moving part toward a target position on the basis of a difference between the target position of the moving part and an actual position of the moving part and a virtual spring constant. In a case where the actual position is backward side against the target position in a direction of proceeding of the moving part, the restitutive force is set smaller than in a case where the actual position against the target position is forward side in the direction of proceeding of the moving part. The restitutive force may be determined by for example a machine learning.SELECTED DRAWING: Figure 5

Description

[Technical Field]

[0001] The present invention relates to a control device for a robot having a moving part that is moved in the forward / backward, left / right, or rotational direction by an actuator. [Background technology]

[0002] Patent Document 1 describes a control device for a robot having a main body and legs connected to the main body. The legs have at least one joint, and the joint is provided with a servo motor for changing the tilt angle of the portion of the leg closer to the tip than the joint. The output torque of the servo motor is controlled by impedance control. That is, a spring-mass-damper model is generated using parameters including inertia, damping coefficient, virtual spring constant, target value, and current value, and the current value is detected by a sensor to maintain a desired posture. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2022-099034 Summary of the Invention [Problem to be solved by the invention]

[0004] When controlling the torque of an actuator through impedance control as described in Patent Document 1, a large virtual spring constant is set to improve the positional accuracy of a joint. On the other hand, although torque is applied from an actuator to a moving part, such as an arm protruding from a robot's main body, to control the moving part to a target position, when the moving part comes into contact with an object, the deviation between the target position and the current position of the moving part increases. As a result, the spring load component calculated by multiplying the virtual spring constant by the deviation increases. This can increase the reaction force acting on the joint and the load acting on the object with which the moving part comes into contact. In particular, when a moving part comes into contact with an object in the direction of its movement while in operation, a spring load component is added to the load for operating the moving part, which can further increase the reaction force acting on the joint and the load acting on the object.

[0005] This invention was devised with an eye on the above-mentioned technical problems, and aims to provide a robot control device that can generate an appropriate restoring force depending on the direction of contact between a moving part and an object when the moving part comes into contact with the object while the moving part is moving. [Means for solving the problem]

[0006] In order to achieve the above-mentioned object, the present invention provides a control device for a robot comprising a moving unit and an actuator capable of changing the position of the moving unit, and configured to set a restoring force for moving the moving unit toward the target position based on the deviation between the target position and the actual position of the moving unit and a virtual spring constant, the control device comprising a controller for controlling the actuator, the controller being characterized in that when the actual position is behind the target position in the direction of travel of the moving unit, the controller sets the restoring force to be smaller than when the actual position is ahead of the target position in the direction of travel of the moving unit.

[0007] Furthermore, the controller in this invention may include a corrected virtual spring constant calculation unit that corrects a predetermined provisional virtual spring constant to set a virtual spring constant, and the corrected virtual spring constant calculation unit may correct the provisional virtual spring constant so that when the actual position is behind the target position in the direction of travel of the moving unit, the virtual spring constant for the deviation between the target position and the actual position is smaller than when the actual position is ahead of the target position in the direction of travel of the moving unit.

[0008] Furthermore, the corrected virtual spring constant calculation unit in this invention may correct a value obtained by dividing a predetermined drive-side upper limit restoring force by the deviation as the virtual spring constant when the actual position is on the rear side in the traveling direction of the moving part relative to the target position, and may correct a value obtained by dividing a predetermined brake-side upper limit restoring force that is greater than the drive-side upper limit restoring force by the deviation as the virtual spring constant when the actual position is on the front side in the traveling direction of the moving part relative to the target position.

[0009] In addition, the controller in this invention may include a corrected target position calculation unit that corrects the target position, and the corrected target position calculation unit may correct the target position so that the deviation from the actual position is smaller when the actual position is behind the target position in the direction of travel of the moving unit than when the actual position is ahead of the target position in the direction of travel of the moving unit.

[0010] The corrected target position calculation unit in this invention may correct the target position when the actual position is rearward of the target position in the traveling direction of the moving unit, based on a value obtained by dividing a predetermined drive-side upper limit restoring force by the virtual spring constant, and may correct the target position when the actual position is forward of the target position in the traveling direction of the moving unit, based on a value obtained by dividing a predetermined brake-side upper limit restoring force that is greater than the drive-side upper limit restoring force by the virtual spring constant. [Effects of the Invention]

[0011] The control device for the robot of this invention is configured to set a restoring force for moving the moving unit toward the target position based on the deviation between the target position and the actual position of the moving unit and the virtual spring constant. Therefore, if a load acts on the moving unit during its movement, for example, when the moving unit comes into contact with an obstacle, causing a deviation between the target position and the actual position of the moving unit, the restoring force can be increased to move the moving unit toward the target position.

[0012] Furthermore, when the actual position is behind the target position in the traveling direction of the moving unit, the restoring force is set smaller than when the actual position is ahead of the target position in the traveling direction of the moving unit. That is, when the moving unit comes into contact with an obstacle ahead of the moving unit in the traveling direction, the restoring force is set smaller. This reduces the restoring force applied to the load for moving the moving unit, preventing a large load from acting on the moving unit, the actuator for operating the moving unit, and components with which the moving unit comes into contact, such as the obstacle. Furthermore, when an obstacle comes into contact with the moving unit from behind the traveling direction of the moving unit, the restoring force is set relatively larger. This prevents the moving unit from being accelerated by the component in contact with the moving unit, preventing excessive movement of the moving unit. As a result, excessive loads on the moving unit and actuators can be prevented. [Brief explanation of the drawings]

[0013] [Figure 1] 1 is a diagram illustrating an example of a robot according to an embodiment of the present invention. [Figure 2] 10A and 10B are diagrams illustrating a difference in magnitude of a restoring torque generated in accordance with a target rotation angle and an actual rotation angle of a moving part. [Figure 3] FIG. 2 is a block diagram illustrating an example of a controller for determining a target torque of a motor. [Figure 4] FIG. 2 is a functional block diagram for explaining means for correcting a virtual spring constant and a target rotation angle by a correction processing unit. [Figure 5] 10A and 10B are diagrams illustrating a relationship between a rotation angle difference and a corrected virtual spring constant, and a relationship between a rotation angle difference and a restoring torque based on the corrected virtual spring constant. [Figure 6] 10A and 10B are diagrams illustrating a relationship between a rotation angle difference and a corrected target rotation angle, and a relationship between the rotation angle difference and a restoring torque based on the corrected target rotation angle. [Figure 7] 3 is a flowchart illustrating an example of control executed by a control device according to an embodiment of the present invention. [Figure 8] 10A and 10B are diagrams illustrating an example in which the front-side maximum restoring torque and the rear-side maximum restoring torque are changed according to the speed of the moving part. DETAILED DESCRIPTION OF THE INVENTION

[0014] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The following embodiments of the present invention will be described with reference to the accompanying drawings. Note that the following embodiments are merely examples of specific embodiments of the present invention and are not intended to limit the scope of the present invention.

[0015] An example of a robot according to an embodiment of the present invention is shown schematically in Fig. 1. The robot 1 shown in Fig. 1 includes a main body 2, a pair of drive wheels 3, a first link member 4, a second link member 5, and a manipulator 6.

[0016] The main body 2 is provided with an electronic control device (hereinafter referred to as the controller) 7 for controlling various actuators described below, a power storage device (not shown) that supplies power to the various actuators, sensors (not shown) such as lasers and cameras that detect the conditions around the robot 1, a motor (not shown) that serves as a driving force source, and a power transmission device (not shown) that transmits torque from the motor to the drive wheels.

[0017] One end of the first link member 4 is rotatably connected to the main body 2. A first joint 4a, which is the boundary between the first link member 4 and the main body 2, is configured to function as a so-called shoulder joint, and is provided with an actuator (not shown), such as a servo motor, for controlling the rotation angle of the first link member 4 relative to the main body 2.

[0018] One end of the second link member 5 is rotatably connected to the other end of the first link member 4. A second joint portion 5a, which is the boundary between the second link member 5 and the first link member 4, is configured to function as a so-called elbow joint, and is provided with an actuator (not shown), such as a servo motor, for controlling the rotation angle of the second link member 5 relative to the first link member 4.

[0019] The manipulator 6 is rotatably connected to the other end of the second link member 5. A third joint 6a, which is the boundary between the manipulator 6 and the second link member 5, is configured to function as a so-called radiocarpal joint, and is provided with an actuator (not shown), such as a servo motor, for controlling the rotation angle of the manipulator 6 relative to the second link member 5.

[0020] The controller 7 is mainly composed of a microcomputer, and is configured to receive signals from various sensors and determine the target torque τ_tgt of the actuator based on the received signals and pre-stored maps and arithmetic expressions.

[0021] The robot 1 configured as described above can move in the forward / backward direction and in the turning direction by driving the drive wheels 3, and can change the forward / backward position and tilt angle of the manipulator 6 relative to the main body 2 by controlling each actuator. In other words, the position of the manipulator 6 can be controlled by controlling at least one of the drive motor and each actuator.

[0022] For convenience, the following description will be given assuming that the position of the manipulator 6 is controlled by controlling the rotation angle of the second link member 5 relative to the first link member 4, and that the rotation angles of the portions corresponding to other joints are constant. That is, the following description will be given assuming that the second link member 5 and the manipulator 6 move as a unit. FIG. 2 shows a schematic model of the second joint portion 5a (hereinafter referred to as a rotating portion 8), the second link member 5, and the manipulator 6 (hereinafter collectively referred to as a moving portion 9). Note that the following description will be given taking an example in which a motor 10 serving as an actuator is provided in the rotating portion 8.

[0023] In the model shown in FIG. 2 , the rotation angle of the moving unit 9 is changed by controlling the motor 10 provided in the rotating unit 8. The rotation angle and speed of the moving unit 9 are determined based on signals input to the controller 7 from sensors provided in the main body 2, etc. For example, when an object is to be grasped by the manipulator 6, a trajectory for moving the moving unit 9 to the object is determined based on the horizontal distance between the object and the moving unit 9, the vertical distance, or the angle with respect to the horizontal plane, and the like, and a target rotation angle θ_ref of the motor 10 for moving the moving unit 9 along that trajectory is determined for each control cycle. A target speed (target angular velocity) Vel_ref of the motor 10 for moving the moving unit 9 toward the determined target rotation angle θ_ref is then determined. Then, based on the determined target rotation angle θ_ref, target speed Vel_ref, movement torque τmo for moving the moving part 9 at the target speed Vel_ref, virtual spring constant Kp_ref pre-stored in the controller 7, and control gain Kd_ref, a command signal (target torque τ_tgt) to be output to the motor 10 is determined.

[0024] Furthermore, the control device in this embodiment of the present invention is configured to change the restoring torque for moving the moving unit 9 toward the target rotation angle θ_ref when a load acts on the moving unit 9, depending on the direction in which the load acts. Specifically, when a load acts on the moving unit 9 from the front side in the traveling direction in FIG. 2 while the moving unit 9 is rotating clockwise as shown in FIG. 2, the actual rotation angle θ_cur becomes negative relative to the target rotation angle θ_ref, as shown by the dashed dotted line in FIG. 2. When the moving unit 9 receives a load from the front side in the traveling direction of the moving unit 9, the restoring torque shown by (1) in FIG. 2 for causing the actual rotation angle θ_cur to follow the target rotation angle θ_ref is configured to be smaller than the restoring torque shown by (2) in FIG. 2 when the moving unit 9 receives a load from the rear side in the traveling direction of the moving unit 9.

[0025] 3 is a block diagram illustrating an example of the controller 7 for determining the target torque τ_tgt (drive current value I_od) of the motor 10. As shown in FIG. 3, the controller 7 includes a correction processing unit 11, a control command value generating unit 12, and a motor drive circuit 13.

[0026] The correction processing unit 11 receives as input the target rotation angle θ_ref of the motor 10, a virtual spring constant Kp_ref, the front-side maximum restoring torque τre_ref_F, the rear-side maximum restoring torque τre_ref_R, and the current rotation angle θ_cur of the motor 10. The virtual spring constant Kp_ref, the front-side maximum restoring torque τre_ref_F, and the rear-side maximum restoring torque τre_ref_R are determined in advance based on experiments, simulations, etc., and are stored in the controller 7. The current rotation angle θ_cur of the motor 10 is input from a sensor (not shown) provided in the motor drive circuit 13. The correction processing unit 11 is configured to correct the virtual spring constant Kp_ref and the target rotation angle θ_ref based on the input data, as will be described later, and output the corrected values.

[0027] It should be noted that this virtual spring constant Kp_ref corresponds to the "provisional virtual spring constant" in the embodiment of the present invention, the front-side maximum restoring torque τre_ref_F corresponds to the "driving-side upper limit restoring force" in the embodiment of the present invention, and the rear-side maximum restoring torque τre_ref_R corresponds to the "braking-side upper limit restoring force" in the embodiment of the present invention.

[0028] The control command value generating unit 12 is configured to set the target torque τ_tgt of the motor 10, i.e., the drive current value I_od to be supplied to the motor 10, by adding the moving torque τmo required to rotate the motor 10 at the target speed Vel_ref and the restoration torque τre for reducing the difference between the actual rotation angle θ_cur of the motor 10 and the target rotation angle θ_ref.

[0029] 3, an actual speed calculator 14 calculates the current rotational speed of the motor 10 (hereinafter referred to as actual speed Vel_cur) based on the current rotational angle θ_cur of the motor 10 output from the motor drive circuit 13. Specifically, the difference between the current rotational angle θ_cur(n) and the previous rotational angle θ_cur(n-1) of the motor 10 is divided by the control cycle time. Next, a difference (speed difference) ΔV between a predetermined target speed Vel_ref and the actual speed Vel_cur is calculated by a differentiator 15, and the calculated speed difference ΔV is multiplied by a predetermined control gain Kd_ref by a multiplier 16 to convert it into a torque for reducing the speed difference ΔV.

[0030] Then, the torque value output from the multiplier 16 is added by an adder 17 to a feedforward torque τFF_ref determined in consideration of the inertia torque and sliding resistance of the motor 10 and the moving part 9, to determine the moving torque τmo required to rotate the motor 10 at the target rotational speed Vel_ref.

[0031] Furthermore, the corrected target rotation angle θ_ref_c and corrected virtual spring constant Kp_ref_c output from the correction processing unit 11 are input to the control command value generating unit 12, and a restoring torque τre is calculated based on the corrected target rotation angle θ_ref_c and the corrected virtual spring constant Kp_ref_c. Specifically, a difference (rotational angle difference) Δθ between the corrected target rotation angle θ_ref_c and the current rotational angle θ_cur of the motor 10 is calculated by a differentiator 18, and the calculated rotational angle difference Δθ is multiplied by the corrected virtual spring constant Kp_ref_c by a multiplier 19, thereby calculating a restoring torque τre for reducing the difference between the target rotation angle θ_ref and the actual rotational angle θ_cur of the motor 10.

[0032] The moving torque τmo and the restoring torque τre determined as described above are added by an adder 20 to determine a target torque τ_tgt, and the determined target torque τ_tgt is input to the motor drive circuit 13. The motor drive circuit 13 then determines a drive current value I_od for outputting the input target torque τ_tgt of the motor 10, and outputs the signal to a switch or the like for controlling the motor 10.

[0033] Fig. 4 shows a functional block diagram for explaining means for correcting the virtual spring constant Kp_ref and the target rotation angle θ_ref by the correction processing unit 11. The correction processing unit 11 shown in Fig. 4 is configured so that when the moving unit 9 collides at a relatively high speed, the virtual spring constant Kp_ref can be changed to quickly set the restoring torque τre to an appropriate value, and when the moving unit 9 collides at a relatively low speed, the target rotation angle θ_ref can be changed to set the restoring torque τre corresponding to the large rotation angle difference Δθ between the target rotation angle θ_ref (before correction) and the actual rotation angle θ_cur.

[0034] Specifically, when the moving part 9 collides at high speed, the signal of the sensor that detects the rotation angle θ_cur of the moving part 9 changes suddenly, that is, at a high frequency. Therefore, the correction processing part 11 shown in Fig. 4 includes a high-pass filter (HPF) 21 that extracts the high-frequency component θ_cur_H from the output signal of the sensor that detects the rotation angle θ_cur when the moving part 9 collides at high speed.

[0035] Similarly, when the moving part 9 collides at a low speed, the signal of the sensor detecting the rotation angle θ_cur of the moving part 9 changes slowly, that is, at a low frequency. Therefore, the correction processing part 11 shown in Fig. 4 includes a low-pass filter (LPF) 22 that extracts the low-frequency component θ_cur_L from the output signal of the sensor detecting the rotation angle θ_cur when the moving part 9 collides at a low speed.

[0036] 4, the rotational angle difference Δθ_H for calculating the restoring torque τre is determined by subtracting the output signal θ_cur_H of the high-pass filter 21 from the corrected target rotational angle θ_ref_c(n-1) determined in the previous control routine using a differentiator 24. Then, the rotational angle difference Δθ_H is multiplied by the corrected virtual spring constant Kp_ref_c(n-1) determined in the previous control routine using a multiplier 25 to determine the provisional restoring torque τre_te_H.

[0037] Then, a predetermined virtual spring constant Kp_ref, a front-side maximum restoring torque τre_ref_F, a rear-side maximum restoring torque τre_ref_R, the rotation angle difference Δθ_H calculated by the difference calculator 24, and the provisional restoring torque τre_te_H calculated by the multiplier 25 are input to a correction Kp calculation unit 26.

[0038] The correction Kp calculation unit 26 corresponds to the "correction virtual spring constant calculation unit" in the embodiment of the present invention, and is configured to correct the virtual spring constant Kp_ref based on the magnitude of the temporary restoring torque τre_te_H. Specifically, when the temporary restoring torque τre_te_H is equal to or greater than the front-side maximum restoring torque τre_ref_F, the correction virtual spring constant Kp_ref_c(n) is calculated by equation (1), when the temporary restoring torque τre_te_H is equal to or greater than the rear-side maximum restoring torque τre_ref_R and less than the front-side maximum restoring torque τre_ref_F, the correction virtual spring constant Kp_ref_c(n) is calculated by equation (2), and when the temporary restoring torque τre_te_H is less than the rear-side maximum restoring torque τre_ref_R, the correction virtual spring constant Kp_ref_c(n) is calculated by equation (3). The direction of torque acting to increase the rotation angle of the moving part 9 in the direction of travel of the moving part 9 is shown as positive. Kp_ref_c(n)=τre_ref_F / Δθ_H …(1) Kp_ref_c(n)=Kp_ref …(2) Kp_ref_c(n)=τre_ref_R / Δθ_H …(3)

[0039] 5 shows a graph summarizing the relationship between the rotation angle difference Δθ_H calculated by the above equations (1), (2), and (3) and the corrected virtual spring constant Kp_ref_c(n), as well as a graph summarizing the relationship between the rotation angle difference Δθ_H and the restoring torque based on the corrected virtual spring constant Kp_ref_c(n). Note that the provisional restoring torque τre_te_H is shown by a dashed dotted line.

[0040] 5, when the temporary restoring torque τre_te_H is equal to or greater than the rear-side maximum restoring torque τre_ref_R and less than the front-side maximum restoring torque τre_ref_F, the corrected virtual spring constant Kp_ref_c(n) is set to a predetermined virtual spring constant Kp_ref. Therefore, when a relatively small load acts on the moving part 9 and the rotational angle difference Δθ_H between the target rotational angle θ_ref and the actual rotational angle θ_cur is small, the restoring torque τre changes proportionally according to the rotational angle difference Δθ_H.

[0041] On the other hand, when the temporary restoring torque τre_te_H is equal to or greater than the front-side maximum restoring torque τre_ref_F, the corrected virtual spring constant Kp_ref_c(n) is set to decrease in inverse proportion to the rotational angle difference Δθ_H. That is, when a load acts on the moving unit 9 from the front side in the traveling direction of the moving unit 9, the corrected virtual spring constant Kp_ref_c(n) decreases in inverse proportion to the rotational angle difference Δθ_H. Therefore, the restoring torque τre calculated by multiplying the corrected virtual spring constant Kp_ref_c(n) by the rotational angle difference Δθ_H is maintained constant. In other words, the front-side maximum restoring torque τre_ref_F becomes the maximum value of the restoring torque τre.

[0042] On the other hand, when the provisional restoring torque τre_te_H is less than the rear-side maximum restoring torque τre_ref_R, the corrected virtual spring constant Kp_ref_c(n) is set to decrease in inverse proportion to the rotational angle difference Δθ_H. That is, when a load acts on the moving unit 9 from the rear side in the traveling direction of the moving unit 9, the corrected virtual spring constant Kp_ref_c(n) decreases in inverse proportion to the rotational angle difference Δθ_H. Therefore, the restoring torque τre calculated by multiplying the corrected virtual spring constant Kp_ref_c(n) by the rotational angle difference Δθ_H is maintained constant. In other words, if the direction of the torque toward the traveling direction of the moving unit 9 is positive, the rear-side maximum restoring torque τre_ref_R becomes the minimum value of the restoring torque τre. That is, the absolute value of the rear-side maximum restoring torque τre_ref_R becomes the maximum value of the restoring torque τre generated when the actual rotational angle θ_cur is forward of the target rotational angle θ_ref in the traveling direction of the moving unit 9.

[0043] Then, as described above, the corrected virtual spring constant Kp_ref_c(n) is output from the corrected Kp calculation unit 26. Note that since the corrected virtual spring constant Kp_ref_c(n) will be used in the next control routine, the corrected virtual spring constant Kp_ref_c(n) output from the corrected Kp calculation unit 26 is input to the delay circuit 27 and held until the next processing cycle.

[0044] 4, the rotational angle difference Δθ_L for calculating the restoring torque τre is determined by subtracting the output signal θ_cur_L of the low-pass filter 22 from the corrected target rotational angle θ_ref_c(n-1) determined in the previous control routine using a differentiator 28. Next, the rotational angle difference Δθ_L and the corrected virtual spring constant Kp_ref_c(n-1) determined in the previous control routine are multiplied by a multiplier 29 to determine the provisional restoring torque τre_te_L.

[0045] Then, the target rotation angle θ_ref, the predetermined virtual spring constant Kp_ref, the front-side maximum restoring torque τre_ref_F, the rear-side maximum restoring torque τre_ref_R, and the provisional restoring torque τre_te_L calculated by the multiplier 29 are input to a corrected rotation angle calculation unit 23, which corresponds to the “corrected target position calculation unit” in the embodiment of the present invention.

[0046] The corrected rotation angle calculation unit 23 is configured to correct the target rotation angle θ_ref based on the magnitude of the provisional restoring torque τre_te_L. Specifically, when the provisional restoring torque τre_te_L is equal to or greater than the front-side maximum restoring torque τre_ref_F, the corrected target rotation angle θ_ref_c(n) is calculated by equation (4), when the provisional restoring torque τre_te_L is equal to or greater than the rear-side maximum restoring torque τre_ref_R and less than the front-side maximum restoring torque τre_ref_F, the corrected target rotation angle θ_ref_c(n) is calculated by equation (5), and when the provisional restoring torque τre_te_L is less than the rear-side maximum restoring torque τre_ref_R, the corrected target rotation angle θ_ref_c(n) is calculated by equation (6). θ_ref_c(n)=(τre_ref_F / Kp_ref)+θ_cur_L …(4) θ_ref_c(n)=θ_ref …(5) θ_ref_c(n)=(τre_ref_R / Kp_ref)+θ_cur_L …(6)

[0047] 6 shows a graph summarizing the relationship between the rotational angle difference Δθ_L calculated by the above equations (4), (5), and (6) and the corrected target rotational angle θ_ref_c(n), as well as a graph summarizing the relationship between the rotational angle difference Δθ_L and the restoring torque based on the corrected target rotational angle θ_ref_c(n). Note that the target rotational angle θ_ref before correction (or the corrected target rotational angle θ_ref_c(n-1) determined in the previous control routine) is shown by a dashed line, and the provisional restoring torque τre_te_L is shown by a dashed line.

[0048] As shown in Fig. 6, when the provisional restoring torque τre_te_L is equal to or greater than the rear-side maximum restoring torque τre_ref_R and less than the front-side maximum restoring torque τre_ref_F, the corrected target rotational angle θ_ref_c(n) is set to the predetermined target rotational angle θ_ref (or the corrected target rotational angle θ_ref_c(n-1) determined in the previous control routine). That is, when a relatively small load acts on the moving part 9 and the rotational angle difference Δθ_L between the target rotational angle θ_ref and the actual rotational angle θ_cur is small, the restoring torque τre changes proportionally according to the rotational angle difference Δθ_L.

[0049] On the other hand, when the provisional restoring torque τre_te_L is equal to or greater than the front-side maximum restoring torque τre_ref_F, the corrected target rotation angle θ_ref_c(n) is set to change following (parallel to) the target rotation angle θ_ref before correction (or the corrected target rotation angle θ_ref_c(n-1) determined in the previous control routine). That is, when a load acts on the moving unit 9 from the front side in the traveling direction of the moving unit 9, the corrected target rotation angle θ_ref_c(n) decreases in parallel to the target rotation angle θ_ref before correction (or the corrected target rotation angle θ_ref_c(n-1) determined in the previous control routine). Therefore, the rotational angle difference Δθ_L is maintained constant, and the restoring torque τre_te calculated by multiplying the virtual spring constant Kp_ref and the rotational angle difference Δθ_L is maintained constant. In other words, the front-side maximum restoring torque τre_ref_F becomes the maximum value of the restoring torque τre.

[0050] On the other hand, when the provisional restoring torque τre_te is equal to or greater than the rear-side maximum restoring torque τre_ref_R, the corrected target rotation angle θ_ref_c(n) is set to change following (parallel to) the target rotation angle θ_ref before correction (or the corrected target rotation angle θ_ref_c(n-1) determined in the previous control routine). That is, when a load acts on the moving unit 9 from the front side in the traveling direction of the moving unit 9, the corrected target rotation angle θ_ref_c(n) increases in parallel with the target rotation angle θ_ref before correction (or the corrected target rotation angle θ_ref_c(n-1) determined in the previous control routine). Therefore, the rotational angle difference Δθ_L is maintained constant, and the restoring torque τre_te calculated by multiplying the virtual spring constant Kp_ref and the rotational angle difference Δθ_L is maintained constant. In other words, if the direction of the torque toward the traveling direction of the moving unit 9 is positive, the rear-side maximum restoring torque τre_ref_R becomes the minimum value of the restoring torque τre. That is, the absolute value of the rear-side maximum restoring torque τre_ref_R becomes the maximum value of the restoring torque τre generated when the actual rotation angle θ_cur is further forward in the traveling direction of the moving part 9 than the target rotation angle θ_ref.

[0051] Then, as described above, the corrected target rotation angle θ_ref_c(n) is output from the corrected rotation angle calculation unit 23. Since the corrected target rotation angle θ_ref_c(n) will be used in the next control routine, the target rotation angle θ_ref_c(n) output from the corrected rotation angle calculation unit 23 is input to the delay circuit 30 and held until the next processing cycle.

[0052] 7 shows a flowchart for explaining an example of control executed by the correction processing unit 11. The flowchart shown in FIG. 7 is repeatedly executed in a cycle synchronized with the processing cycle of the control command value generating unit 12.

[0053] 7, first, the actual rotation angle θ_cur of the moving unit 9 is acquired (step S1). In step S1, a signal from a sensor provided in the motor drive circuit 13 may be acquired. Next, of the acquired actual rotation angle θ_cur, the high-frequency component θ_cur_H is extracted by the high-pass filter 21, and the low-frequency component θ_cur_L is extracted by the low-pass filter 22 (step S2). Note that depending on the speed at which the moving unit 9 comes into contact with an obstacle or the like, it may be necessary to extract only either the high-frequency component or the low-frequency component.

[0054] Next, based on the extracted high-frequency components θ_cur_H and low-frequency components θ_cur_L of the actual rotation angle θ_cur, the rotational angle differences Δθ_H, Δθ_L between the corrected target rotational angle θ_ref_c and the current rotational angle θ_cur of the motor 10 are calculated (step S3), and based on the rotational angle differences Δθ_H, Δθ_L and the corrected virtual spring constant Kp_ref_c(n-1) determined in the previous control routine, the provisional restoring torques τre_te_H, τre_te_L are calculated (step S4).

[0055] Then, based on the magnitude relationship between the provisional restoring torque τre_te_H and the front-side maximum restoring torque τre_ref_F and the rear-side maximum restoring torque τre_ref_R, the correction Kp calculation unit 26 corrects the virtual spring constant Kp_ref to set the corrected virtual spring constant Kp_re_c(n) (step S5), and also based on the magnitude relationship between the provisional restoring force τre_te_L and the front-side maximum restoring torque τre_ref_F and the rear-side maximum restoring torque τre_ref_R, the correction Kp calculation unit 26 corrects the target rotation angle θ_ref to set the corrected target rotation angle θ_ref_c(n) (step S6).

[0056] The control device of the robot 1 configured as described above is configured to set a restoring force τre for moving the moving unit 9 toward the target rotation angle θ_ref based on the deviation Δθ between the target rotation angle θ_ref and the actual rotation angle θ_cur of the moving unit 9 and the virtual spring constant Kp_ref. Therefore, if a load acts on the moving unit 9 while the moving unit 9 is moving, for example when the moving unit 9 comes into contact with an obstacle, causing a deviation Δθ between the target rotation angle θ_ref and the actual rotation angle θ_cur of the moving unit 9, the restoring force τre increases, thereby moving the moving unit 9 toward the target rotation angle θ_ref.

[0057] Furthermore, when the actual rotation angle θ_cur is behind the target rotation angle θ_ref in the traveling direction of the moving unit 9, the virtual spring constant is corrected to be smaller than when the actual rotation angle θ_cur is ahead of the target rotation angle θ_ref in the traveling direction of the moving unit 9, or the target rotation angle θ_ref is corrected so that the deviation from the actual rotation angle θ_cur is smaller, thereby setting the restoring torque τre to be smaller. In other words, when the moving unit 9 comes into contact with an obstacle ahead in the traveling direction of the moving unit 9, the restoring torque τre is reduced. This makes it possible to reduce the restoring torque τre applied to the load for moving the moving unit 9, and to prevent a large load from acting on the moving unit 9, the motor 10 for operating the moving unit 9, and components with which the moving unit 9 comes into contact, such as obstacles.

[0058] Furthermore, if an obstacle comes into contact with the moving unit 9 from behind in the traveling direction of the moving unit 9, the restoring torque τre is made relatively large. This prevents the moving unit 9 from being accelerated by a member that comes into contact with the moving unit 9, and prevents the moving unit 9 from moving excessively. As a result, it is possible to prevent excessive load from being placed on the moving unit 9 and the motor 10.

[0059] In addition, the faster the speed of the moving part 9, the greater the inertia torque when the moving part 9 comes into contact with an obstacle, etc. Therefore, in order to reduce the load acting on the moving part 9, the motor 10, or components that come into contact with the moving part 9, the upper limit value of the restoring torque τre may be configured to be changed according to the speed of the moving part 9.

[0060] Specifically, as shown in FIG. 8, when the speed of the moving part 9 is less than a first predetermined speed V1, the front-side maximum restoring torque τre_ref_F indicated by the solid line and the rear-side maximum restoring torque τre_ref_R (absolute value) indicated by the dashed line may be set to the same value; when the speed becomes equal to or greater than the first predetermined speed V1, the rear-side maximum restoring torque τre_ref_R (absolute value) may be increased and the front-side maximum restoring torque τre_ref_F may be decreased according to the speed of the moving part 9; and when the speed of the moving part 9 becomes equal to or greater than a second predetermined speed V2, which is the upper limit speed, the rear-side maximum restoring torque τre_ref_R (absolute value) may be set to the maximum value τmax that can be set, and the front-side maximum restoring torque τre_ref_F may be set to the minimum value τmin that can be set.

[0061] In the above example, the moving unit 9 is rotated by the motor 10, but an actuator that applies a load in the propulsion direction to the moving unit 9 may be used. In that case, the various torques described above may be read as loads or forces, and the rotation angles may be read as positions in the propulsion direction.

[0062] Furthermore, in the above example, a rotating coordinate system (two-dimensional) centered on the rotating unit 8 has been taken as an example for explanation, but the restoring torque τre may be set in a three-dimensional coordinate system with the direction of travel (X axis) of the robot 1, the left-right direction (Y axis) of the robot 1, and the vertical direction (Z axis). In that case, the moving direction of the moving unit 9 is separated into three axial components for calculation, and the moving direction is set as a positive value, and the restoring torque (force) for each axial direction is set in accordance with the above example.

[0063] Furthermore, the actuator in the embodiment of the present invention only needs to be able to change the position of the moving unit 9. That is, in the example shown in Fig. 1, the position of the manipulator 6 corresponding to the moving unit 9 can be changed by controlling at least one of the drive motor and each actuator, so that the restoring torque (force) of the manipulator 6 may be controlled by at least one of the drive motor and each actuator, for example, by controlling the torque of the drive motor to control the restoring torque (force) of the manipulator 6. [Explanation of symbols]

[0064] 1. Robot 2 Main body 3 drive wheels 4,5 Link members 4a, 5a, 6a joints 6 Manipulator 7 Controller 8 Rotating part 9 Moving Part 10 Motor 11 Correction processing section 12 Control command value generator 13 Motor drive circuit 23 Correction rotation angle calculation unit 26 Corrected Kp calculation unit Kp_ref virtual spring constant Kp_ref_c Corrected virtual spring constant Δθ Rotation angle difference (deviation) θ_ref Target rotation angle θ_ref_c Correction target rotation angle τmo Moving torque τre_ref_F Front side maximum recovery torque τre_ref_R Rear maximum recovery torque τre restoring torque τre_te_H,τre_te_L Provisional restoration torque

Claims

1. 1. A robot control device comprising: a moving unit; and an actuator capable of changing a position of the moving unit; and configured to set a restoring force for moving the moving unit toward the target position based on a virtual spring constant and a deviation between a target position of the moving unit and an actual position of the moving unit, a controller for controlling the actuator; The controller When the actual position is on the rear side of the target position in the traveling direction of the moving part, the restoring force is set to be smaller than when the actual position is on the front side of the target position in the traveling direction of the moving part. A robot control device characterized by:

2. The robot control device according to claim 1, The controller a corrected virtual spring constant calculation unit that corrects a predetermined temporary virtual spring constant to set a virtual spring constant; The correction virtual spring constant calculation unit corrects the provisional virtual spring constant so that, when the actual position is on the rear side of the target position in the traveling direction of the moving unit, the virtual spring constant with respect to the deviation between the target position and the actual position is smaller than when the actual position is on the front side of the target position in the traveling direction of the moving unit. A robot control device characterized by:

3. The robot control device according to claim 2, The corrected virtual spring constant calculation unit correcting the value obtained by dividing a predetermined drive-side upper limit restoring force by the deviation as the virtual spring constant when the actual position is on the rear side in the traveling direction of the moving part with respect to the target position; A value obtained by dividing a predetermined braking-side upper limit restoring force that is greater than the driving-side upper limit restoring force by the deviation is corrected as the virtual spring constant when the actual position is forward in the traveling direction of the moving part with respect to the target position. A robot control device characterized by:

4. The robot control device according to claim 1, The controller a corrected target position calculation unit that corrects the target position, The corrected target position calculation unit corrects the target position so that a deviation from the actual position is smaller when the actual position is on the rear side of the target position in the traveling direction of the moving unit than when the actual position is on the front side of the target position in the traveling direction of the moving unit. A robot control device characterized by:

5. The robot control device according to claim 4, The corrected target position calculation unit correcting the target position when the actual position is on the rear side in the traveling direction of the moving part relative to the target position, based on a value obtained by dividing a predetermined drive-side upper limit restoring force by the virtual spring constant; The target position is corrected based on a value obtained by dividing a predetermined braking-side upper limit restoring force, which is greater than the driving-side upper limit restoring force, by the virtual spring constant when the actual position is forward of the moving part in the traveling direction relative to the target position. A robot control device characterized by:

Images