JP6497575B2 - Drive control device and manipulator device - Google Patents

Description

  The present invention relates to a drive control device and a manipulator device.

  2. Description of the Related Art Conventionally, as a driving device for a joint portion of a manipulator device, a wire stretched between the driven pulley and the driving pulley (driving member) with respect to a driven pulley (driven member) that is interlocked with the operation of the joint portion 2. Description of the Related Art A driving device that transmits a rotational driving force via a stretched member is known.

  For example, Patent Document 1 discloses a robot arm drive device of a wire drive system that drives a robot arm having three joint portions. The drive device disclosed in Patent Document 1 employs a wire drive system because it does not cause harm even if the robot arm comes into contact with humans in the fields of medical care and welfare. In this drive device, a wire stretched between a drive pulley and a driven pulley is stretched via a tension pulley, a tension adjustment mechanism, or the like. In this drive device, when the arm in operation collides with an obstacle such as a human being, the tension adjusting mechanism is controlled so as to loosen the tension of the wire based on the detection result. As a result, the joint portion becomes almost free, the joint portion can move along the obstacle, and the impact on the obstacle can be reduced.

  In such a manipulator device, in recent years, there are increasing demands for accurately controlling the operation of the joint. More specifically, there are an increasing number of scenes where it is required to accurately control the operation start timing of the joint part and to accurately control the operation position of the joint part (such as the rotation angle of the joint part). In order to accurately control the operation of the joint portion of the manipulator device, the stretched member such as a wire is moved so as to accurately follow the drive of the drive member such as the drive pulley, thereby moving the driven pulley or the like. It is required to drive the drive member so as to accurately follow the drive of the drive member.

  However, the stretched member may be stretched due to various factors such as an excessive driving load applied to the joint portion or usage over time. If loosening occurs in the stretched portion of the stretched member that is stretched between the drive member and the driven member due to the extension of the stretched member, even if the drive of the drive member is started The driven member is not driven until the looseness of the stretched member is resolved. Therefore, problems such as the movement position of the stretched member deviate from the target position, or the drive start timing of the stretched member is delayed. When such a malfunction occurs, the operation of the joint portion of the manipulator device cannot be accurately controlled.

In order to solve the above-described problems, the present invention provides a driving profile generator that generates a driving profile according to a predetermined operation command, and a driving member and a driven member that are driven by a driving force from a driving source. The drive source is controlled according to the drive profile generated by the drive profile generation means for the drive device that drives the driven member by moving the stretched member stretched between To drive the driven member to a target position corresponding to the predetermined driving profile or to start driving of the driven member at a target timing corresponding to the predetermined driving profile. And a tension detecting means for detecting a tension of the stretched member, wherein the drive profile generating means When the tension of the tension member known means detects is equal to or greater than a prescribed value, the delay time is determined zero or pre from the driving source to start driving to the driving of the driven member is started and generate a pre-Symbol predetermined driving profile reference driving profile is the time, when the tension of the tension member, wherein the tension detection unit detects is below the prescribed value, the tension detection unit detects該被A corrected drive profile obtained by correcting the reference drive profile according to a delay time corresponding to the tension of the tension member is generated as the predetermined drive profile .

  According to the present invention, even if elongation occurs in the stretched member and loosening occurs in the stretched portion of the stretched member that is stretched between the driving member and the driven member, There is an excellent effect that it is possible to reduce the adverse effects caused by the deviation of the driving position of the driven member and the deviation of the driving start timing of the driven member with respect to the target timing.

It is a perspective view showing an example of a manipulator device concerning an embodiment. It is a perspective view which shows the structure of the 1st drive pulley and the 1st driven pulley which stretch the 1st wire relevant to operation | movement of the 1st joint in the manipulator apparatus. It is a control block diagram of a manipulator device that operates a first joint and a second joint. It is a perspective view which shows the manipulator apparatus which concerns on the modification 1. FIG. It is a perspective view which shows the drive device of the 1st joint in the manipulator apparatus which concerns on the modification 2. FIG. 10 is an exploded view of components constituting a first joint drive device according to Modification 2; (A) is the graph which compared the output signal of the 1st motor encoder when the expansion | extension has not generate | occur | produced in the 1st wire in the modification 1, and the output signal of the 1st joint axis encoder. (B) is the graph which compared the output signal of the 1st motor encoder when the expansion | extension has generate | occur | produced in the 1st wire in the modification 1, and the output signal of the 1st joint axis encoder. It is a control block diagram related to joint drive control by the drive control unit in the embodiment. It is a perspective view of the manipulator apparatus for demonstrating an example of a tension | tensile_strength detection means. 7 is a flowchart illustrating an example of a flow of joint drive control in Control Example 1. (A) is a graph which shows an example of the reference | standard drive profile for driving a 1st motor, (b) is a graph which shows an example of the reference | standard drive profile for driving a 2nd motor. 12 is a graph showing a corrected drive profile of a first motor obtained by correcting the reference drive profile shown in FIG. 12 is a graph showing a corrected drive profile of a second motor obtained by correcting the reference drive profile shown in FIG. (A) is a graph which shows the reference drive profile of a 2nd motor, (b) is a graph which shows the correction drive profile of the 2nd motor correct | amended by the correction process of the control example 2. FIG. 10 is a graph showing a corrected drive profile of a second motor corrected by the correction process of Control Example 3. 6 is a flowchart showing a flow of control in an implementation timing example 1; In the implementation timing example 5, the second motor is driven according to a driving profile in which the driving speed of the second motor is accelerated at a constant acceleration, then driven at a constant steady speed, and then decelerated and stopped at a constant acceleration. It is a graph which shows the output result of the 2nd motor encoder when it is made to do. In the implementation timing example 6, it is the graph which showed the output value of the 2nd joint axis encoder with the continuous line, and showed the output value of the 2nd motor encoder with the broken line.

Hereinafter, an embodiment in which a drive control device according to the present invention is applied to a drive control device that controls a drive device of a manipulator device will be described.
FIG. 1 is a perspective view showing an example of a manipulator device according to the present embodiment.
The manipulator device 100 according to the present embodiment is a horizontal manipulator device having two degrees of freedom of a first joint 181 and a second joint 182. This manipulator device includes a fixedly arranged first link 183, a second link 184 that rotates around the first joint 181 as a rotation axis, and a third link 185 that rotates around the second joint 182 as a rotation axis. ing. The manipulator device is installed such that the second link 184 and the third link 185 rotate around a rotation axis extending in the vertical direction. An end effector such as a picking hand or an air chuck is attached to the free end portion of the third link 185 according to the purpose of use.

  On the 1st link 183, the 1st motor 188 which is a drive source is installed. An encoder 189 is coaxially attached to the first motor 188, and the encoder 189 outputs a rotation detection signal corresponding to the rotation operation of the first motor 188. On the first link 183, a first motor speed reducer 190 mounted on the rotation shaft of the first motor 188 is installed. The first motor speed reducer 190 has a first drive as a drive member. A pulley 194 is attached. The first link 183 has a fixed shaft at the location of the first joint 181, and a first driven pulley 195 as a driven member is rotatably attached to the first joint 181. A first wire 186 as a stretched member is stretched between the first drive pulley 194 and the first driven pulley 195.

FIG. 2 is a perspective view showing the configuration of the first drive pulley 194 and the first driven pulley 195 that stretch the first wire 186 related to the operation of the first joint 181.
The first driving pulley 194 and the first driven pulley 195 are provided with a groove having a semicircular cross section for guiding the winding of the first wire 186. Fixing members 186A and 186B having a cross section larger than that of the wire main body portion are caulked at both ends of the first wire 186. The first wire 186 is fixed to the first drive pulley 194 by fitting these fixing members 186 </ b> A and 186 </ b> B into holes formed in the first drive pulley 194. The method for fixing the wire to the other pulleys is the same as this, but the method for fixing the wire to the pulley is not limited to this, and a widely known fixing method can be adopted.

  The driving force by the first motor 188 is transmitted to the first drive pulley 194 via the first motor speed reducer 190, and the first drive pulley 194 is rotationally driven. When the first drive pulley 194 is driven to rotate, the first wire 186 is wound around the first drive pulley 194 or drawn out from the first drive pulley 194, so that the first wire 186 moves. Along with the movement of the first wire 186, the first driven pulley 195 around which the first wire 186 is wound rotates around the first joint 181. Since the first driven pulley 195 is fixed to one end of the second link 184, the second link 184 rotates around the first joint 181 as the first driven pulley 195 rotates.

Next, a configuration related to the operation of the second joint 182 will be described. The basic configuration is the same as that of the first joint 181.
On the first link 183, a second motor 191 as a drive source is installed. An encoder 192 is coaxially attached to the second motor 191, and the encoder 192 outputs a rotation detection signal corresponding to the rotation operation of the second motor 191. On the first link 183, a second motor speed reducer 193 mounted on the rotation shaft of the second motor 191 is installed. The second motor speed reducer 193 has a second drive as a drive member. A pulley 196 is attached.

  A relay pulley 197 is rotatably attached to a fixed shaft provided at the location of the first joint 181 on the first link 183. The second link 184 has a fixed shaft at the second joint 182, and a second driven pulley 198 as a driven member is rotatably attached to the second joint 182. Between the second drive pulley 196 and the second driven pulley 198, a second wire 187 as a stretched member is stretched via a relay pulley 197.

  The driving force by the second motor 191 is transmitted to the second driving pulley 196 via the second motor speed reducer 193, and the second driving pulley 196 is rotationally driven. When the second drive pulley 196 is driven to rotate, the second wire 187 is wound around the second drive pulley 196 or is fed out of the second drive pulley 196, whereby the second wire 187 moves. As the second wire 187 moves, the relay pulley 197 around which the second wire 187 is wound is rotated, and the second driven pulley 198 around which the second wire 187 is wound rotates around the second joint 182. To do. Since the second driven pulley 198 is fixed to one end of the third link 185, the third link 185 also rotates around the second joint 182 as the second driven pulley 198 rotates.

FIG. 3 is a control block diagram of the drive control device of the manipulator device 100 that operates the first joint 181 and the second joint 182.
The manipulator device 100 mainly includes a device main body 110, a power supply unit 500, and a host controller 300. For example, when image information is necessary for controlling the manipulator device 100, the image input device 400 may be connected to the host controller 300. The power supply unit 500, the host controller 300, and the image input device 400 may be separate from the apparatus main body 110, but may be a stand-alone configuration mounted in the apparatus main body 110.

Next, the operation of the manipulator device 100 will be described.
The operation control of the manipulator device is performed by the CPU 210 which is an arithmetic processing unit. The host controller 300 and the CPU 210 are connected by a communication network 311. From the communication network 311, operation mode information, various parameters necessary for driving control of the joints 181 and 182, and the like are transmitted to the CPU 210. Further, signals from various sensors 220 and the switch 221 are also input to the CPU 210, and processing such as restriction of the operating area of the manipulator device and emergency stop are executed based on these signals.

  The operations of the joints 181 and 182 of the manipulator device are realized by driving and controlling the first motor 188 and the second motor 191. As drive information of the first motor 188 and the second motor 191, signals from the first motor encoder 189 and the second motor encoder 192 are input to the drive information detectors 236 and 238, and rotation such as movement amount, movement speed, and movement acceleration is performed. The information is converted into information and input to the drive control unit 213 in the CPU 210. The drive control unit 213 generates a target drive profile based on the rotation information and target movement position information commanded from the host controller 300, and the first motor 188 and the second motor 191 are driven along the target drive profile. Thus, a control command is issued to the motor driver 235.

[Modification 1]
FIG. 4 is a perspective view showing a modified example of the manipulator device according to the present embodiment (hereinafter referred to as “modified example 1”).
The manipulator device 100 according to the first modification includes two degrees of freedom of the first joint 181 and the second joint 182 as in the configuration of the above-described embodiment, but the second link 184 and the third link 185 are horizontal. It is a vertical manipulator device installed so as to rotate around a rotation axis extending in a direction.

  In the first modification, in addition to the configuration of the above-described embodiment, a torque limiter 171 as a load applying unit that applies a rotational load to the first driven pulley 195 is attached to the first joint 181. By providing such a torque limiter 171, the first driven pulley 195 does not rotate and the first joint 181 does not operate unless the first driven pulley 195 is applied with a rotational force exceeding the load of the torque limiter 171. The magnitude of the load by the torque limiter 171 is set to such a magnitude that the first driven pulley 195 is not rotated even if a normal external force that can be applied during operation is applied to the first driven pulley 195. The torque limiter 171 functions as a fall prevention brake. In the first modification, the first motor 188 is a motor that can obtain a driving torque sufficient to rotate the first driven pulley 195 with a rotational force exceeding the load of the torque limiter 171.

  In the first modification, the first joint 181 is provided with an optical first joint axis encoder 173 as rotation detection means. If the first wire 186 is stretched with sufficient tension and the first wire 186 does not stretch, the relationship between the detection result of the first motor encoder 189 and the rotational position of the first joint 181 is linear. The rotational position of the first joint 181 can be accurately grasped from the detection result of the first motor encoder 189. However, in a situation where the first wire 186 may be stretched, the linearity may be lost, and the rotational position of the first joint 181 may not be accurately grasped from the detection result of the first motor encoder 189. Assuming such a case, the first joint axis encoder 173 that directly detects the rotational position of the first joint 181 is provided.

  Similarly, in the first modification, a torque limiter 172 is also attached to the second joint 182 so that a load is generated when the third link 185 rotates with respect to the second joint 182. In the first modification, the second joint 182 is also provided with an optical second joint axis encoder 174 as rotation detection means.

  In the first modification, torque limiters 171 and 172 are installed as fall prevention brakes in the first joint 181 and the second joint 182, but the torque limiters are installed in the first motor 188 and the second motor 191. Also good. Further, instead of the torque limiter, other means such as an electromagnetic brake may be provided.

[Modification 2]
FIG. 5 is a perspective view showing a driving device for the first joint 181 in another modified example of the manipulator device according to the present embodiment (hereinafter referred to as “modified example 2”).
FIG. 6 is an exploded view of components constituting the driving device for the first joint 181 according to the second modification.
The manipulator device 100 of the second modification is a vertical manipulator device as in the first modification, but at least for the first joint 181, a pair of wires 186 made of individual wires is used instead of the first wire 186. 1, 186-2 is used. Specifically, the pair of wires 186-1 and 186-2 includes a pulley portion 194A and 194B provided on the first drive pulley 194 and a pulley portion 195A and 195B provided on the first driven pulley 195, respectively. Wound around and stretched.

  When the first drive pulley 194 is rotationally driven in the clockwise direction (first direction) in the figure, the first direction drive wire 186-1 of the pair of wires is wound around the first drive pulley 194, and accordingly Thus, the first driven pulley 195 also rotates in the clockwise direction (first direction) in the figure. At this time, the second-direction driving wire 186-2 of the pair of wires is drawn out from the first driving pulley 194 while being wound around the first driven pulley 195. On the other hand, when the first driving pulley 194 is rotationally driven in the counterclockwise direction (second direction) in the drawing, the second direction driving wire 186-2 is wound around the first driving pulley 194, and accordingly, the first driving pulley 194 is wound. The driven pulley 195 also rotates in the counterclockwise direction (second direction) in the figure. At this time, the first direction driving wire 186-1 is drawn out from the first driving pulley 194 while being wound around the first driven pulley 195.

Next, loosening of the wire will be described.
FIG. 7A is a graph comparing the output signal of the first motor encoder 189 and the output signal of the first joint axis encoder 173 when the first wire 186 is not stretched in the first modification described above. It is.
FIG. 7B is a graph comparing the output signal of the first motor encoder 189 and the output signal of the first joint axis encoder 173 when the first wire 186 is stretched in the first modification described above. It is.
In the following description, the driving device for the first joint 181 is described as an example, but the same applies to the second joint 182.

  For the first wire 186, it is preferable to use a material or a structure that does not substantially elongate as long as it is within the range of normal use. In particular, when an excessive load is applied, minute elongation can occur. If elongation occurs in the first wire 186, the tension (tension value) of the first wire 186 may decrease.

  If the tension of the first wire 186 is within an appropriate range, the operation of the first joint 181 is started without delaying the start of driving of the first motor 188 as shown in FIG. The first joint 181 can be operated without delay along the target drive profile created by H.213. However, if the first wire 186 is stretched and the first wire 186 is loose, as shown in FIG. 7B, the first joint 181 is delayed by Δt time from the start of driving of the first motor 188. Operation starts. This is because the first drive pulley 194 starts to rotate simultaneously with the start of driving of the first motor 188, but the first driven pulley until the extension (loosening) of the first wire 186 is eliminated and the tension is within an appropriate range. This is because 195 does not move. Therefore, the first joint 181 cannot be appropriately operated along the target drive profile created by the drive control unit 213.

  In addition, when the endurance deterioration or suddenly excessive load is applied and the first wire 186 is elongated slightly, the first joint 181 may move by the extension of the first wire 186. In this case, if the tension (tension value) of the first wire 186 is within an appropriate range, the first joint 181 can be operated without delaying the driving of the first motor 188. However, since the correspondence relationship between the rotation angle of the first motor 188 (the rotation angle of the first drive pulley 194) and the rotation angle of the first joint 181 (the rotation angle of the first driven pulley 195) is lost, the first motor A deviation occurs between the target angle of the first joint 181 corresponding to the rotation angle of 188 and the actual rotation angle of the first joint 181. Therefore, also in this case, the first joint 181 cannot be appropriately operated along the target drive profile created by the drive control unit 213.

  However, in this embodiment, even if the first wire 186 is extended, the first joint 181 is not rotated by the torque limiter 171. Therefore, in the present embodiment, when the first wire 186 extends, the first joint 181 does not move, and the tension of the first wire 186 decreases. Therefore, the following description will be made on the assumption that when the first wire 186 extends, the first joint 181 does not move and the tension of the first wire 186 decreases.

FIG. 8 is a control block diagram related to joint drive control by the drive control unit 213 performed by the CPU 210.
The drive control unit 213 is mainly composed of a command value generation unit 215 as a drive profile generation unit and a controller 216, and a control amount (control) of a control target that is a drive source (first motor 188, second motor 191). The feedback control is executed based on the target position, speed, acceleration, differential value of acceleration, and the like. In addition to feedback control, feedforward control may be performed based on a predicted disturbance or the like.

  The command value generation unit 215 uses the target movement position information commanded from the host controller 300, drive information from the drive information detection units 236 and 238 (current position information, etc.), information from various sensors 220, etc. as input information. A command value to 216 is generated. The command value generation unit 215 calculates the trajectory of the control target portion (in this embodiment, the free end portion of the third link 185 to which the end effector is attached) from the target movement position information, the current position information, and the like. So that the control target location moves, the drive profiles of the first motor 188 and the second motor 191 are generated, and command values based on the respective drive profiles are output to the controller 216.

  The command value generation unit 215 according to the present embodiment first generates a reference drive profile when the wires 186 and 187 are not loosened and the tension of the wires 186 and 187 is within an appropriate range in the reference command value generation unit 215A. Then, the reference command value generation unit 215A sequentially outputs reference command values based on the respective reference drive profiles.

  On the other hand, the detected value of the tension of the wires 186 and 187 detected by the tension detector 239 is input to the command value generator 215. The detected tension values of the wires 186 and 187 are input to the calculation unit 215B of the command value generation unit 215. The calculation unit 215B calculates and outputs a predetermined correction value when the tension of the input wires 186 and 187 is out of the appropriate range. In this case, the reference command value output from the reference command value generation unit 215A is corrected by the correction value output from the calculation unit 215B, and the command value to the controller 216 is generated. On the other hand, when the tension of the input wires 186 and 187 is within an appropriate range, no correction value is output. In this case, the reference command value output from the reference command value generation unit 215A is directly generated as a command value to the controller 216.

FIG. 9 is a perspective view of a manipulator device for explaining an example of the tension detecting means.
In the example shown in FIG. 9, in the manipulator device according to the second modification, a tension detecting unit using a strain gauge is provided on the stretched portion of the first direction driving wire 186-1. The tension detection unit includes a detection pulley 132 that comes into contact with the first direction drive wire 186-1 from above, and the first direction drive wire 186-1 on both sides of the detection pulley 132 from below. Idler pulleys 133 and 134 in contact therewith. The detection pulley 132 is attached to the strain gauge 131. When the tension of the first direction driving wire 186-1 changes, the force by which the first direction driving wire 186-1 pushes up the detection pulley 132 changes, and thereby the strain gauge 131 to which the detection pulley 132 is attached. The amount of distortion that occurs in the Since there is a high correlation between the strain amount output from the strain gauge 131 and the tension of the first direction driving wire 186-1, the first direction driving wire 186-1 is calculated from the output value from the strain gauge 131. Can be detected.

  In this example, a tension detecting unit using a strain gauge is employed as the tension detecting means, but the present invention is not limited to this. For example, a tension detecting means for detecting (estimating) the tension of the first direction driving wire 186-1 from the delay time Δt measured from the signal of the first motor encoder 189 and the signal of the first joint axis encoder 173 is employed. You can also

[Control example 1]
Next, one control example (this control example is referred to as “control example 1”) in the joint drive control in the present embodiment will be described.
FIG. 10 is a flowchart illustrating an example of the flow of joint drive control in the first control example.
In this control example 1, when the CPU 210 receives an operation command from the host controller 300 (Yes in S1), the CPU 210 calculates the trajectory of the control target location based on the target movement position information included in the operation command from the host controller 300. (S2). And the reference drive profile of the 1st motor 188 and the 2nd motor 191 for the said control object location to move on the calculated track | orbit is produced | generated (S3).

  Further, the CPU 210 acquires a detected value of the wire tension detected by the tension detecting unit 239 (S4). If the detected tension value of any wire exceeds the specified value (No in S5), the reference drive profile generated in the processing step S3 is used as the drive profile of each motor 188, 191 and the drive profile is used as the drive profile. The command value thus followed is output to the motor drivers 235 and 237 of the motors 188 and 191 (S8).

  On the other hand, when the tension detection value of one of the wires is equal to or less than the specified value (Yes in S5), for example, when the second wire 187 is stretched and the tension of the second wire 187 is equal to or less than the specified value, First, based on the detected tension detection value of the second wire 187, a delay time Δt from the start of driving of the second motor 191 to the start of driving of the second driven pulley 198 is calculated (S6). Since there is a high correlation between the wire tension and the delay time Δt, the delay time Δt can be calculated with high accuracy from the detected value of the wire tension. Then, a correction process for correcting the reference drive profiles for the motors 188 and 191 is executed (S7), and command values according to the corrected drive profiles are output to the motor drivers 235 and 237 of the motors 188 and 191 (S8). .

FIG. 11A is a graph showing an example of a reference drive profile for driving the first motor 188, and FIG. 11B shows an example of a reference drive profile for driving the second motor 191. It is a graph.
FIG. 12 is a graph showing a corrected drive profile of the first motor 188 obtained by correcting the reference drive profile shown in FIG. 11A by the correction process of the first control example.
FIG. 13 is a graph showing a corrected drive profile of the second motor 191 in which the reference drive profile shown in FIG. 11B is corrected by the correction process of the first control example.

  In the present control example 1, for the second wire 187 whose wire tension is equal to or less than the specified value, the correction for correcting the reference drive profile of the second motor 191 shown in FIG. 11B to the correction drive profile shown in FIG. Execute the process. Specifically, based on the delay time Δt calculated in the processing step S6, if it is originally (if the tension of the second wire 187 is within an appropriate range), until the delay time Δt elapses from the start of driving of the second motor 191. The rotation amount of the second driven pulley 198 that should have been rotated (hereinafter referred to as “insufficient rotation amount”) is calculated. This insufficient rotation amount can be estimated from the area of S2a indicated by hatching in FIG.

  When such a delay time Δt occurs, when the second motor 191 is driven with the reference drive profile shown in FIG. 11B, the rotation position of the second driven pulley 198 when the drive is completed (time T2E) is the target rotation. It will be out of position. Since the shift amount corresponds to the above-described insufficient rotation amount, in this control example 1, as shown in FIG. 13, the correction drive in which the deceleration start time T2b of the reference drive profile of the second motor 191 is delayed to time T2b ′. Generate a profile. Time T2b 'is calculated from the insufficient rotation amount so that the rotation position of the second driven pulley 198 matches the target rotation position. In other words, the time T2b ′ is calculated so that the area of S2b indicated by hatching in FIG. 13 matches the area of S2a. By controlling the driving of the second motor 191 using such a corrected driving profile, the second driven pulley 198 can be driven to the target rotational position even if the second wire 187 is loosened. The two joints can be operated appropriately.

  On the other hand, for the first wire 186 whose wire tension exceeds the specified value, a correction process for correcting the reference drive profile of the first motor 188 shown in FIG. 11A to the correction drive profile shown in FIG. 12 is executed. . Specifically, a corrected drive profile in which the drive start time of the reference drive profile of the first motor 188 is delayed by the delay time Δt calculated in process step S6 and the deceleration start time T1b is delayed to time T1b ′. Generate. Thus, by delaying the drive start time by the delay time Δt, it is possible to eliminate the deviation of the operation start timing of the first driven pulley 195 from the actual operation start timing of the second driven pulley 198.

  Further, by delaying the deceleration start time to time T1b ′, the rotation start position of the first driven pulley 195 at the time of completion of driving (time T1E) is shifted from the target rotation position by delaying the drive start time by the delay time ΔT. It suppresses it. This shift amount corresponds to an insufficient rotation amount of the first driven pulley 195 that should have been rotated unless the drive start time is delayed by the delay time Δt. This insufficient rotation amount can be estimated from the area of S1a indicated by hatching in FIG. The time T1b 'is calculated from this insufficient rotation amount so that the rotation position of the first driven pulley 195 coincides with the target rotation position. In other words, the time T1b ′ is calculated so that the area of S1b indicated by the oblique lines in FIG. 12 matches the area of S1a. Thereby, even if the drive start time is delayed by the delay time ΔT, the first driven pulley 195 can be driven to the target rotational position.

  As described above, the reference drive profiles of the motors 188 and 191 are corrected, and the motors 188 and 191 are driven and controlled according to the corrected drive profiles, so that the first joint 182 is operated in accordance with the delay of the operation start timing. The operation start timing of the joint 181 is also delayed. As a result, there is no deviation in the operation start timing between both joints. In addition, any of the joints 181 and 182 can move to the target movement position. As a result, the control target portion (the free end portion of the third link 185) can move to the target position without greatly deviating from the trajectory corresponding to the operation command.

  In a manipulator device having three or more joint portions, when a situation occurs in which two or more of the joint portions cause a delay time Δt, for example, it is adjusted to the joint portion having the longest delay time Δt. Thus, the drive start time for the other joints may be delayed.

  Further, in the present control example 1, both the movement start timing deviation between both joints and the movement position deviation of each joint are corrected. However, only one of the deviations may be corrected. Good. For example, when correcting the deviation of the operation position of each joint from the target position, a reference drive profile as shown in FIG. 11A is used as the drive profile of the first motor 188 in which the wire is not loosened. A correction drive profile as shown in FIG. 13 may be used as the drive profile of the second motor 191 in which the wire is loose. Further, for example, when correcting the deviation of the operation start timing between both joints, a correction drive profile as shown in FIG. 12 is used for the drive profile of the first motor 188 in which the wire is not loosened, and the wire is A reference drive profile as shown in FIG. 11B may be used as the drive profile of the loose second motor 191.

[Control example 2]
Next, another control example (this control example is referred to as “control example 2”) in the joint drive control in the present embodiment will be described.
This control example 2 is the same as the control example 1 described above except that the content of the correction process for the reference drive profile of the second motor 191 in which the wire has loosened is different. The description will focus on the points different from 1.

FIG. 14A is a graph showing a reference drive profile of the second motor 191, and FIG. 14B is a graph showing a corrected drive profile of the second motor 191 corrected by the correction process of the present control example 2. is there. The reference drive profile shown in FIG. 14A is the same as the reference drive profile shown in FIG.
In the second control example, for the second wire 187 whose wire tension is equal to or less than the specified value, the reference drive profile of the second motor 191 shown in FIG. 14A is changed to the corrected drive profile shown in FIG. Execute correction processing to correct. Specifically, a corrected drive profile is generated by accelerating the completion of drive profile acceleration from time T2a, which is the completion of acceleration of the reference drive profile, to time T2a ′ as shown in FIG. 14B.

  This time T2a ′ is calculated from the delay time Δt so that the rotational position of the second driven pulley 198 coincides with the rotational position on the reference drive profile at the time T2a. In other words, the time T2a ′ is calculated so that the area of S2c ′ indicated by hatching in FIG. 14B matches the area of S2c indicated by hatching in FIG. By controlling the drive of the second motor 191 using such a corrected drive profile, even if the second wire 187 is loosened, the second driven pulley 198 can be reached early (at time T2a). Can follow the rotational position on the reference drive profile.

  In this control example 2, the acceleration during the acceleration period of the second motor 191 increases due to the correction drive profile. However, if this acceleration becomes too large, the second motor 191 cannot be driven properly, and the first There is a possibility that the drive control of the two-motor 191 becomes inappropriate. Therefore, for example, when the acceleration during the acceleration period on the correction drive profile exceeds a predetermined threshold, the command value based on the correction drive profile is not output, and error processing such as maintaining the motor in a stopped state is performed. May be. As a result, it is possible to avoid the occurrence of a malfunction due to insufficient motor torque during operation of the manipulator device. Further, not limited to the present control example 2, when a situation occurs in which excessive acceleration is set in the correction drive profile, error processing can be performed by comparing the acceleration with a threshold value. Here, the error processing is performed by comparing the acceleration and the threshold value, but the error processing may be performed by comparing the speed and the threshold value.

[Control Example 3]
Next, still another control example (this control example is referred to as “control example 3”) in the joint drive control in the present embodiment will be described.
Since this control example 3 is the same as the control example 1 described above except that the content of the correction process of the reference drive profile of the second motor 191 in which the wire has loosened is different, the control example described above will be described below. The description will focus on the points different from 1.

FIG. 15 is a graph showing a correction drive profile of the second motor 191 corrected by the correction process of the third control example. Note that the reference drive profile of the second motor 191 is the same as the reference drive profile shown in FIG.
In the third control example, for the second wire 187 whose wire tension is equal to or less than the specified value, the correction for correcting the reference drive profile of the second motor 191 shown in FIG. 11B to the correction drive profile shown in FIG. Execute the process. Specifically, a corrected drive profile is generated by increasing the steady speed of the drive profile from the steady speed V2 of the reference drive profile to the steady speed V2 ′ as shown in FIG.

  This steady speed V2 'is calculated from the delay time Δt so that the rotational position of the second driven pulley 198 matches the rotational position on the reference drive profile at the time of the deceleration start time T2b. In other words, the steady-state speed V2 'is calculated so that the area of S2d' indicated by hatching in FIG. 15 coincides with the area of S2d indicated by hatching. By controlling the driving of the second motor 191 using such a corrected driving profile, even if the second wire 187 is loosened, the second driven pulley 198 is placed on the reference driving profile at time T2b. It is possible to follow the rotational position.

  According to the control example 2 and the control example 3 described above, the completion of driving of the second driven pulley 198 coincides with the completion of driving on the reference drive profile (time T2E), so the operation completion time of the second joint 182 is observed. can do. Since the first motor 188 uses the correction drive profile of the control example 1 described above, when the first driven pulley 195 is driven (time T1E ′), the drive on the reference drive profile is completed (time T1E). It will shift. When the driving of the first driven pulley 195 is also coincident with the driving completion time (time T1E) on the reference driving profile, the correction driving profile of the first motor 188 is also driven on the reference driving profile when the driving is completed. The time at completion of acceleration may be advanced or the steady speed may be increased so as to coincide with the time.

  In each of the above control examples, the driving of the motors 188 and 191 is started according to the driving profile while the second wire 187 is loosened. However, before the driving is started according to the driving profile, it is loosened. The tension return process for returning the tension of the second wire 187 in the slack state to the appropriate range is performed in advance, and the reference drive profile of the second motor 191 is corrected with the time required for the tension return process as the delay time Δt. May be. As the correction drive profile of the second motor 191 at this time, for example, a correction drive profile similar to the correction drive profile of the first motor 188 shown in FIG. 12 can be used.

  As the tension return processing at this time, for example, the second motor 191 is driven so that the second driven pulley 198 does not move, and the second motor 191 is driven when the detected tension value of the second wire 187 falls within an appropriate range. The process of stopping is mentioned. In addition, a tension changing unit that changes the tension of the second wire 187 without driving the second motor 191 may be provided, and the tension of the second wire 187 may be returned to an appropriate range using the tension changing unit. .

[Execution timing example 1]
Next, an example of timing for executing the process of generating a correction drive profile (reference drive profile correction process) (this example is referred to as “execution timing example 1”) will be described.
The manipulator device in the present embodiment includes a CP mode as the first processing mode and a PTP mode as the second processing mode as its operation mode.

  The CP mode is an operation mode in which the control target portion is moved along the movement path specified by the predetermined operation command to the target position specified by the predetermined operation command. That is, in the operation command of the CP mode, not only the target position of the movement destination of the control target part and the posture are specified as necessary, but also a movement route from the current position to the target position is specified. . On the other hand, the PTP mode is an operation mode that is arbitrary on the movement path as long as the control target portion is moved to the target position specified by a predetermined operation command. That is, in the operation command of the PTP mode, the target position of the movement destination of the control target part and the posture are designated as necessary, and the movement route from the current position to the target position is not designated.

  In the PTP mode, since the movement path is arbitrary, even if each motor 188, 191 is driven according to the reference drive profile when the second wire is loose and the delay time Δt occurs, the operation command is not changed. It is possible to perform the operation based on it appropriately. Specifically, for example, the first joint axis encoder 173 and the second joint axis encoder 174 described above are provided in each of the joints 181 and 182, and when the output values of these encoders match the target values, the motors 188, Control to stop 191 may be performed.

  On the other hand, in the case of the CP mode, if the same control as that of the PTP mode is performed, the movement path of the control target portion is greatly deviated from the movement path specified by the operation command. Therefore, it is necessary to correct the drive profiles of the motors 188 and 191 so that the movement path of the control target portion does not greatly deviate from the movement path specified by the operation command.

FIG. 16 is a flowchart showing the flow of control in the present timing example 1.
In the present timing example 1, when receiving an operation command from the host controller 300 (Yes in S1), the CPU 210 determines whether or not the operation command is in the CP mode (S11). At this time, when it is determined that the CP mode is set (Yes in S11), the joint drive control in each of the above control examples 1 to 3 is performed (S2 to S8). As a result, even if the wire is loosened, the correction processing of the reference drive profiles of the motors 188 and 191 is executed so that the movement path of the control target portion does not greatly deviate from the movement path specified by the operation command. Appropriate operation according to the operation command is realized.

  On the other hand, when it is determined that the mode is the PTP mode (No in S11), a reference drive profile for moving the control target portion to the target position specified by the operation command is generated (S12). Then, the command value according to the reference drive profile is output to the motor drivers 235 and 237 of the motors 188 and 191 (S13). Thereafter, the output values of the first joint axis encoder 173 and the second joint axis encoder 174 provided at each joint 181 and 182 are observed, and when these encoder output values coincide with the target values, respectively (Yes in S14). Then, the motors 188 and 191 are stopped (S15). As a result, even if the wire is loose, the control target location can be moved to the target position according to the operation command, and an appropriate PTP mode operation is realized.

[Execution timing example 2]
Next, another example (this example is referred to as “execution timing example 2”) of timing for executing the process of generating a correction drive profile (reference drive profile correction process) will be described.
In the implementation timing example 1 described above, when the operation command is in the CP mode and the condition that the wire tension is equal to or less than the specified value is satisfied, the process of generating the correction drive profile is performed. Timing example 2 is an example in which a process for generating a correction drive profile is performed when the condition that the first operation command after receiving the power supply of the manipulator device is satisfied. At this time, a condition that the wire tension is not more than a specified value may be added. According to this embodiment timing example 2, an appropriate operation can be realized even if the wire is loose for some reason during the period when the power is not turned on.

[Execution Timing Example 3]
Next, still another example (this example will be referred to as “execution timing example 3”) of the timing at which the correction drive profile generation process (reference drive profile correction process) is performed will be described.
In this embodiment timing example 3, the total driving time (cumulative operating distance of the wire) driven by each joint is measured for each of the joints 181 and 182 of the manipulator device, and the condition that the total driving time is equal to or longer than the specified time is set. This is an example in which a process for generating a correction drive profile is performed when the condition is satisfied. At this time, a condition that the wire tension is not more than a specified value may be added.

  As long as the total joint driving time (cumulative operating distance of the wire) is short, there is little possibility of a situation where the wire is loosened because elongation due to use over time has not yet occurred. Therefore, by not performing the process of generating the correction drive profile until the total joint drive time exceeds the specified time, the execution frequency of the process of generating the correction drive profile is reduced, and the productivity of the manipulator device is improved. It becomes possible to plan.

[Execution Timing Example 4]
Next, still another example (this example is referred to as “execution timing example 4”) of timing for executing the process of generating a correction drive profile (reference drive profile correction process) will be described.
In this embodiment timing example 4, an elapsed time from the start of operation of the manipulator device is measured, and a process of generating a correction drive profile is performed when the condition that the elapsed time is equal to or longer than a specified time is satisfied. . At this time, a condition that the wire tension is not more than a specified value may be added.

  As long as the elapsed time from the start of operation of the manipulator device is short, there is little possibility of a situation in which the wire is loosened because elongation due to use over time has not yet occurred. Therefore, by not performing the process of generating the correction drive profile until the elapsed time from the start of operation of the manipulator apparatus exceeds the specified time, the execution frequency of the process of generating the correction drive profile is reduced, and the manipulator apparatus Productivity can be improved.

[Example 5 of implementation timing]
Next, still another example (this example will be referred to as “execution timing example 5”) of the timing at which the correction drive profile generation process (reference drive profile correction process) is executed will be described.
The timing example 5 is an example in which a process for generating a corrected driving profile is performed when the condition that the settling time of the driving result in the motors 188 and 191 controlled according to a predetermined driving profile satisfies a predetermined time is satisfied. is there. At this time, a condition that the wire tension is not more than a specified value may be added.

  FIG. 17 shows that the second motor 191 is driven according to a driving profile in which the driving speed of the second motor 191 is accelerated at a constant acceleration, then driven at a constant steady speed, and then decelerated and stopped at a constant acceleration. It is a graph which shows the output result of the 2nd motor encoder 192 when it is made to do. In this graph, the drive profile is indicated by a broken line. Further, not the output result of the second motor encoder 192 but the output result of the second joint axis encoder 174 may be used.

  The settling time Ts in the graph shown in FIG. 17 is an index value for evaluating the amount of operation vibration of the manipulator device. The reference drive profile when the tension of the second wire 187 is within an appropriate range is designed so that the settling time Ts is shortened, and is controlled so as to minimize vibration. Therefore, the settling time Ts is measured, and when the settling time longer than the original is measured, it can be estimated that the second wire 187 has loosened. Therefore, if the process for generating the correction drive profile is performed when the condition that the settling time Ts is equal to or longer than the specified time is satisfied, the process can be performed at an appropriate timing that requires the generation of the correction drive profile.

[Execution Timing Example 6]
Next, still another example (this example will be referred to as “execution timing example 6”) of the timing at which the correction drive profile generation process (reference drive profile correction process) is performed will be described.
In this timing example 6, there is a deviation of a predetermined amount or more between the output values of the motor encoders 189 and 192 of the motors 188 and 191 controlled according to the predetermined drive profile and the output values of the joint axis encoders 173 and 174. This is an example in which a process of generating a correction drive profile is performed when the condition that it is detected is satisfied. At this time, a condition that the wire tension is not more than a specified value may be added.

FIG. 18 is a graph in which the output value of the second joint axis encoder 174 is indicated by a solid line and the output value of the second motor encoder 192 is indicated by a broken line.
In this timing example 6, when the operation delay of the output value of the second joint axis encoder 174 with respect to the second motor encoder 192 when the specific speed during the acceleration period is reached is detected, the operation delay amount is equal to or greater than the specified amount. In addition, a process of generating a correction drive profile is performed. This operation delay becomes an index value for evaluating deterioration of the trajectory accuracy of the operation of the manipulator device. The reference drive profile when the tension of the second wire 187 is within an appropriate range is designed so that this operation delay is shortened, and is controlled and designed so as to increase the trajectory accuracy. Therefore, this operation delay amount (operation delay time) is measured, and when the operation delay amount is larger than the original, it can be estimated that the second wire 187 has loosened. Therefore, if the process of generating the correction drive profile is performed when the condition that the operation delay amount is equal to or greater than the specified amount is satisfied, the process can be performed at an appropriate timing that requires generation of the correction drive profile.

What has been described above is merely an example, and the present invention has a specific effect for each of the following modes.
(Aspect A)
Drive profile generation means such as a command value generation unit 215 that generates a drive profile according to a predetermined operation command, and a first drive pulley 194 that is driven by a drive force from a drive source such as the first motor 188 and the second motor 191 The first wire 186, the second wire 187, etc. stretched between the driving member and the driven member such as the first driven pulley 195 and the second driven pulley 198 by the driving member such as the second driving pulley 196 and the like. By controlling the drive source in accordance with the drive profile generated by the drive profile generating means for a drive device that drives the driven member by moving the stretched member in the stretching direction, the driven member is Drive to a target position according to a predetermined drive profile, or start driving the driven member at a target timing according to the predetermined drive profile A drive control device having a control means such as a controller 216 for executing drive control, comprising a tension detection means such as a tension detector 239 for detecting the tension of the stretched member, and the drive profile generation means The predetermined drive profile such as a correction drive profile is generated based on the tension of the stretched member detected by the tension detecting means.
According to this, it is confirmed that the looseness has occurred in the stretched portion of the stretched member that is stretched between the drive member and the driven member. Tension). When looseness has occurred in the stretched part of the stretched member, even when driving of the drive member is started, a delay occurs until the driven member starts to be driven, or the loosening is eliminated In the case of carrying out the above, the start of driving of the driven member is delayed by the execution time of the process. According to this aspect, by generating a drive profile based on the tension of the stretched member, it is possible to generate a drive profile that takes into account the delay in starting the drive of such a driven member. Therefore, even if the stretched member is loosened, it is possible to reduce the adverse effects caused by the drive position deviation of the driven member with respect to the target position and the drive start timing deviation of the driven member with respect to the target timing.

(Aspect B)
In the aspect A, when the tension of the stretched member detected by the tension detection unit is equal to or higher than a predetermined value, the drive profile generation unit drives the driven member after the drive source starts driving. A reference drive profile having zero or a predetermined delay time until the start of the tension is generated as the predetermined drive profile, and the tension of the stretched member detected by the tension detecting means is below the specified value In this case, a corrected drive profile in which the reference drive profile is corrected according to the delay time Δt corresponding to the tension of the stretched member detected by the tension detecting means is generated as the predetermined drive profile. .
According to this, when the tension of the stretched member is below a specified value, the target position based on the delay time Δt is generated by generating a correction drive profile that takes into account the delay time Δt corresponding to the tension of the stretched member. It is possible to appropriately reduce the adverse effects caused by the deviation of the driving position of the driven member with respect to the above and the deviation of the driving start timing of the driven member with respect to the target timing.

(Aspect C)
In the aspect A or B, the control means executes the drive control by controlling the drive source of each drive apparatus according to an individual drive profile corresponding to each of the plurality of drive apparatuses, and the tension detection means Is for individually detecting the tension of the stretched member in each driving device, and the drive profile generating means has a prescribed value for the tension of the stretched member detected by the tension detecting means for at least one drive device. The tension of the stretch member detected by the tension detection means is lower than the low tension drive device such as the drive device of the second joint 182 where the tension of the stretch member detected by the tension detection means falls below a specified value. As a drive profile for a high-tension drive device such as the drive device of the first joint 181 having a high value, it corresponds to the delay time Δt corresponding to the tension of the stretched member of the low-tension drive device. And generating a correction drive profiles delayed driving start time of the reference driving profile of the high tension drive Te.
According to this, even if the drive source of the low tension drive device is driven with a drive profile whose drive start time is the same as that of the reference drive profile, the driven member can be moved between the low tension drive device and the high tension drive device. The drive start timing can be prevented from shifting.

(Aspect D)
In the aspect B or C, the corrected drive profile is corrected so that the drive end point coincides with the drive end point of the reference drive profile.
According to this, since the operation completion time of the driven member of the drive device driven by the correction drive profile can be made coincident with the operation completion time on the reference drive profile, it is possible to suppress the deviation of the operation completion time.

(Aspect E)
In any one of the aspects B to D, the corrected drive profile is a target position in the corrected drive profile at the end time T1a or T2a of the acceleration period from the drive start time in the reference drive profile to the target speed. Is corrected so as to coincide with the target position in the reference drive profile.
According to this, even if the stretched member is loosened, the drive position of the driven member driven by the corrected drive profile is set to the target on the reference drive profile as early as the end points T1a and T2a of the acceleration period. It is possible to follow the position.

(Aspect F)
In the aspect E, when the acceleration during the acceleration period in the correction drive profile is equal to or greater than a threshold value, the control unit does not start driving the drive source.
According to this, it is possible to avoid the occurrence of problems due to insufficient torque of the drive source during operation of the drive device.

(Aspect G)
In any one of the aspects A to F, the drive profile generation unit is configured to perform the predetermined measurement based on the tension of the stretched member detected by the tension detection unit when a predetermined tension lowering condition is satisfied. A drive profile is generated.
According to this, execution of a predetermined drive profile generation process based on the tension of the stretched member by the drive profile generation means while suppressing a decrease in productivity by setting the predetermined tension reduction condition as an appropriate condition. It is possible to reduce the frequency and reduce the processing load.

(Aspect H)
In the aspect G, the predetermined tension reduction condition includes a condition that power is supplied to the drive control device.
According to this, it is possible to appropriately cope with the slackness of the stretchable member generated for some reason during the period when the power is not turned on while suppressing the decrease in productivity.

(Aspect I)
In the aspect G or H, the measuring apparatus includes a measuring unit that measures the cumulative moving distance of the stretched member, and the predetermined tension reduction condition is defined by the cumulative moving distance of the stretched member measured by the measuring unit. It includes a condition that the distance is greater than or equal to the distance.
According to this, it is possible to appropriately cope with the looseness caused by the stretch of the stretched member due to the use over time while suppressing the decrease in productivity.

(Aspect J)
In any one of the above aspects G to I, the driving control device further includes a time-lapse unit that measures an elapsed time from the start of operation, and the predetermined tension reduction condition defines the elapsed time measured by the time-lapse unit. It is characterized by including the condition that it is more than time.
According to this, it is possible to appropriately cope with the looseness caused by the stretch of the stretched member due to the use over time while suppressing the decrease in productivity.

(Aspect K)
In any one of the above aspects G to J, there is provided time-lapse means for measuring a settling time Ts of a driving result in the driving source controlled according to the predetermined driving profile, and the predetermined tension lowering condition is the settling It includes the condition that the time is not less than the specified time.
According to this, it is possible to appropriately cope with the looseness of the stretched member while suppressing the decrease in productivity.

(Aspect L)
In any one of the aspects A to K, the control means controls the driving amount of the driving source or the driving member controlled according to the driving profile (output values of the motor encoders 189 and 192) and the driving profile. The drive control is executed by executing feedback control or feedforward control for reducing the deviation from the target amount.
According to this, drive control according to the drive profile can be realized more appropriately.

(Aspect M)
The driving force of the driving member is applied via one or more joint portions 181 and 182 and a stretched member stretched between the driven member and the drive member that are linked to the operation of the joint portion. In the manipulator device having a drive device that transmits the drive member to drive the joint portion, and a drive control device that controls the drive source of the drive device to control the operation of the joint portion, the drive control device includes: The drive control device according to any one of the aspects A to L is used.
According to this, even if looseness occurs in the stretched member, the control target portion of the manipulator device can be appropriately operated.

(Aspect N)
In the aspect M, the joint unit has two or more, the drive device is provided for each joint unit, the drive control device according to the aspect B or C is used as the drive control device, and the drive control device The drive profile generation means in each of the drive devices for moving the control target portion of the manipulator device along the movement path specified by the predetermined operation command to the target position specified by the predetermined operation command. A movement path to the target position is calculated based on a first processing mode such as a CP mode for generating a driving profile and a target position instructed by the predetermined operation command, and the control target portion of the manipulator device is moved to the target position. A second processing mode such as a PTP mode for generating a driving profile of each driving device for moving to the target position along the path. The drive profile generation unit generates the reference drive profile without generating the correction drive profile in the second processing mode, while generating the correction drive profile in the first processing mode. .
According to this, in the first processing mode, even if the stretched member is loosened, it is possible to move the control target location to the target position along the movement route instructed by the predetermined operation command.

100 Manipulator device 131 Strain gauges 173 and 174 Joint axis encoder 181 First joint 182 Second joint 186 First wire 187 Second wire 188 First motor 189 First motor encoder 191 Second motor 192 Second motor encoder 194 First drive Pulley 195 First driven pulley 196 Second drive pulley 197 Relay pulley 198 Second driven pulley 213 Drive control unit 215 Command value generation unit 215A Reference command value generation unit 215B Calculation unit 216 Controllers 235 and 237 Motor drivers 236 and 238 Drive information detection 239 Tension detector 300 Host controller

Japanese Patent No. 3578375

Claims (17)

Drive profile generation means for generating a drive profile according to a predetermined operation command;
A driving device that drives a driven member by moving a stretched member stretched between the drive member and the driven member in a stretching direction by a drive member driven by a driving force from a drive source The drive source is controlled according to the drive profile generated by the drive profile generation means to drive the driven member to a target position corresponding to the predetermined drive profile, or according to the predetermined drive profile A drive control device having control means for executing drive control to start driving of the driven member at a target timing,
Tension detecting means for detecting the tension of the stretch member,
When the tension of the stretched member detected by the tension detection unit is equal to or greater than a specified value , the drive profile generation unit is configured to start driving the driven member after the drive source starts driving. when the delay time is generate as before Symbol predetermined driving profile reference driving profile is zero or a predetermined time, the tension of the tension member, wherein the tension detection unit detects is below the prescribed value, A drive control device that generates a corrected drive profile in which the reference drive profile is corrected according to a delay time corresponding to the tension of the stretched member detected by the tension detection means as the predetermined drive profile . The drive control apparatus according to claim 1 ,
The control means executes the drive control by controlling the drive source of each drive device according to an individual drive profile corresponding to each of the plurality of drive devices,
The tension detecting means is for individually detecting the tension of the stretched member in each driving device,
When the tension of the stretched member detected by the tension detecting unit is less than a specified value for at least one drive device, the drive profile generating unit has a specified value of the tension of the stretched member detected by the tension detecting unit. As a drive profile for the high tension drive device in which the tension of the stretched member detected by the tension detecting means is higher than the low tension drive device, the delay time corresponding to the tension of the stretched member of the low tension drive device In response, a drive control device that generates a corrected drive profile in which the drive start time of the reference drive profile of the high-tension drive device is delayed.   Drive profile generation means for generating a drive profile according to a predetermined operation command;
  A driving device that drives a driven member by moving a stretched member stretched between the drive member and the driven member in a stretching direction by a drive member driven by a driving force from a drive source The drive source is controlled according to the drive profile generated by the drive profile generation means to drive the driven member to a target position corresponding to the predetermined drive profile, or according to the predetermined drive profile A drive control device having control means for executing drive control to start driving of the driven member at a target timing,
  Tension detecting means for detecting the tension of the stretch member,
  The control means executes the drive control by controlling the drive source of each drive device according to an individual drive profile corresponding to each of the plurality of drive devices,
  The tension detecting means is for individually detecting the tension of the stretched member in each driving device,
  When the tension of the stretched member detected by the tension detecting unit is less than a specified value for at least one drive device, the drive profile generating unit has a specified value of the tension of the stretched member detected by the tension detecting unit. As a drive profile for the high tension drive device in which the tension of the stretched member detected by the tension detecting means is higher than the low tension drive device, the delay time corresponding to the tension of the stretched member of the low tension drive device In response, a drive control device that generates a corrected drive profile in which the drive start time of the reference drive profile of the high-tension drive device is delayed. In the drive control device according to any one of claims 1 to 3 ,
The drive control apparatus according to claim 1, wherein the corrected drive profile is corrected so that a drive end point coincides with a drive end point of the reference drive profile. In the drive control device according to any one of claims 1 to 4,
The corrected drive profile is corrected so that the target position in the corrected drive profile coincides with the target position in the reference drive profile at the end of the acceleration period from the start of driving in the reference drive profile until reaching the target speed. A drive control device characterized by that. The drive control apparatus according to claim 5, wherein
The drive control device, wherein the control means does not start driving the drive source when an acceleration during an acceleration period in the corrected drive profile is equal to or greater than a threshold value. The drive control apparatus according to any one of claims 1 to 6,
The drive profile generation means generates the predetermined drive profile based on the tension of the stretched member detected by the tension detection means when a predetermined tension lowering condition is satisfied. apparatus. The drive control apparatus according to claim 7 ,
Measuring means for measuring the cumulative moving distance of the stretched member,
The predetermined tension lowering condition includes a condition that an accumulated movement distance of the stretched member measured by the measuring unit is a specified distance or more. In the drive control device according to claim 7 or 8 ,
Having time-lapse means for measuring the elapsed time from the start of operation of the drive control device,
The predetermined tension lowering condition includes a condition that an elapsed time measured by the aging means is a specified time or more. In the driving control device according to any one of claims 7乃Itaru 9,
A time passage unit for measuring a settling time of a driving result in the driving source controlled according to the predetermined driving profile;
The predetermined tension lowering condition includes a condition that the settling time is a specified time or more.   Drive profile generation means for generating a drive profile according to a predetermined operation command;
  A driving device that drives a driven member by moving a stretched member stretched between the drive member and the driven member in a stretching direction by a drive member driven by a driving force from a drive source The drive source is controlled according to the drive profile generated by the drive profile generation means to drive the driven member to a target position corresponding to the predetermined drive profile, or according to the predetermined drive profile A drive control device having control means for executing drive control to start driving of the driven member at a target timing,
  Tension detecting means for detecting the tension of the stretch member;
  Measuring means for measuring the cumulative travel distance of the stretched member,
  The drive profile generation means is detected by the tension detection means when a predetermined tension lowering condition including a condition that an accumulated movement distance of the stretched member measured by the measurement means is equal to or greater than a specified distance is satisfied. A drive control device that generates the predetermined drive profile based on the tension of the stretched member.   Drive profile generation means for generating a drive profile according to a predetermined operation command;
  A driving device that drives a driven member by moving a stretched member stretched between the drive member and the driven member in a stretching direction by a drive member driven by a driving force from a drive source The drive source is controlled according to the drive profile generated by the drive profile generation means to drive the driven member to a target position corresponding to the predetermined drive profile, or according to the predetermined drive profile A drive control device having control means for executing drive control to start driving of the driven member at a target timing,
  Tension detecting means for detecting the tension of the stretch member;
  Having a means for measuring the elapsed time from the start of operation of the drive control device,
  The drive profile generation means detects the tension of the stretched member detected by the tension detection means when a predetermined tension lowering condition including a condition that the elapsed time measured by the time-lapse means is equal to or longer than a predetermined time is satisfied. A drive control device that generates the predetermined drive profile based on   Drive profile generation means for generating a drive profile according to a predetermined operation command;
  A driving device that drives a driven member by moving a stretched member stretched between the drive member and the driven member in a stretching direction by a drive member driven by a driving force from a drive source The drive source is controlled according to the drive profile generated by the drive profile generation means to drive the driven member to a target position corresponding to the predetermined drive profile, or according to the predetermined drive profile A drive control device having control means for executing drive control to start driving of the driven member at a target timing,
  Tension detecting means for detecting the tension of the stretch member;
  Time measuring means for measuring a settling time of a driving result in the driving source controlled according to the predetermined driving profile;
  The drive profile generation unit is configured to perform the predetermined based on the tension of the stretched member detected by the tension detection unit when a predetermined tension reduction condition including a condition that the settling time is equal to or longer than a predetermined time is satisfied. A drive control device for generating a drive profile of   The drive control device according to any one of claims 7 to 13,
  The predetermined tension lowering condition includes a condition that power is supplied to the drive control apparatus. In the driving control device according to any one of claims 1乃Itaru 14,
The control unit performs feedback control or feedforward control for reducing a deviation between a drive amount of the drive source or the drive member controlled according to the drive profile and a target amount on the drive profile. A drive control device that executes control. One or more joints;
The driving force of the driving member is transmitted to the driven member via the stretched member stretched between the driven member and the driving member interlocked with the operation of the joint portion, and the joint portion is driven. A driving device;
A manipulator device having a drive control device that controls a drive source of the drive device to control an operation of the joint part,
As the drive control device, the manipulator device characterized by using the driving control device according to any one of claims 1乃optimum 15. The manipulator device according to claim 16 ,
Having two or more of the joints,
The driving device is provided for each joint,
The drive control device according to any one of claims 1 to 3 is used as the drive control device.
The drive profile generation means in the drive control device is configured to move a control target portion of the manipulator device along a movement path specified by the predetermined operation command to a target position specified by the predetermined operation command. Based on the first processing mode for generating the driving profile of each driving device and the target position specified by the predetermined operation command, the movement path to the target position is calculated, and the control target portion of the manipulator device is moved to the movement position. A second processing mode for generating a driving profile of each driving device for moving along the path to the target position,
The driving profile generation unit generates the reference driving profile without generating the correction driving profile in the second processing mode, while generating the correction driving profile in the first processing mode. apparatus.

Images