Patent Description:
Patent Literature <NUM> discloses a method of controlling an operation of a robot, in which a control device for controlling a motion of the robot calculates a difference between a target trajectory and an actual motion trajectory with respect to a command value for each axis as a servo delay time, sets the shortest servo delay time as a reference time, calculates a compensation torque for each axis on the basis of the servo delay time for each axis and the reference time, and controls the operation of the robot by outputting a command value reflecting the compensation torque for each axis to each servo.

<CIT> discloses an industrial robot having an articulated arm, adjustment parts for time delay between position control servo systems and a robot track calculation part outputs operation commands of articulations which are relatively short in response time among the articulations of the articulated arm with previously set time delay to compensate deviations in response time between the articulations. Further, position control servo systems select a small integral time constant when articulation operation is at a stop and thus quicken the whole integrating operation to compensate the influence of friction in a short time.

<CIT> discloses a locus control device <NUM> for controlling the locus of a movable portion by simultaneously controlling a plurality of motors on movable shafts including a servo system response locus calculator for operating the response locus of a servo system; an error vector calculator for operating an error vector; a correction vector operator for operating a correction vector, based on the error vector and the moving direction at a predetermined reference point of the servo system response locus; shaft-by-shaft correction amount operator for operating the correction amount of each movable shaft by distributing the correction vector to each shaft; correction amount adders for adding the correction amount of each movable shaft to a corresponding shaft position command; and servo controllers for controlling each motor on the shaft by outputting the motor drive torque of each shaft in a manner that each movable shaft position follows a post-correction position command of each shaft.

In the above-described method, there is a problem that calculation of the compensation torque based on the servo delay time for each axis and the reference time and calculation of the command value reflecting the compensation torque are complicated.

The present invention has been made in view of the above problem, and it is an object of the present invention to realize a control device capable of suppressing trajectory misalignment due to a deviation between response delay times of a plurality of axes (a plurality of servomotors).

This control device is a control device which issues commands to a plurality of servo drivers corresponding to a plurality of servomotors, wherein the control device is adapted to set a servo driver corresponding to a standard servomotor having a maximum response delay time among the plurality of servomotors as a standard servo driver, and to delay a command timing to another servo driver from a command timing to the standard servo driver by a difference between a response delay time of the standard servomotor and a response delay time of a servomotor corresponding to the another servo driver. The commands are position commands adapted to a target trajectory. The control device is adapted to read a position loop gain from each of the plurality of servo drivers and calculate the response delay time of each of the plurality of servomotors as an inverse number of the read position loop gain.

A corresponding control method, control program and recording medium are also provided.

According to the control device, it is possible to suppress trajectory misalignment due to a deviation between response delay times of a plurality of servomotors.

An embodiment of the present invention will be described with reference to <FIG>. Hereinafter, description will be made on the assumption of a synchronous group having three axes (X axis, Y axis and Z axis) as illustrated in <FIG>, but the synchronous group of the embodiment may have a configuration having two or more axes.

In the three-axis configuration as illustrated in <FIG>, for example, an X axis servo driver which receives a command from a control device controls an X axis servomotor, and a workpiece Wx moves in an X-axial direction due to the X axis servomotor (operation information of the X axis servomotor is fed back to the X axis servo driver). Also, a Y axis servo driver which receives a command from the control device controls a Y axis servomotor, and a workpiece Wy moves in a Y-axial direction due to the Y axis servomotor (operation information of the Y axis servomotor is fed back to the Y axis servo driver). Also, a Z axis servo driver which receives a command from the control device controls a Z axis servomotor, and a workpiece Wz moves in a Z-axial direction due to the Z axis servomotor (operation information of the Z axis servomotor is fed back to the Z axis servo driver).

Generally, the commands to the X axis servo driver, the Y axis servo driver and the Z axis servo driver are synchronized, but a time (response delay time) until the corresponding servomotor responds after receipt of the command varies according to the axes.

For example, when there is almost no (small) response delay in a Y axis system and the response delay in a Z axis system is large, as illustrated in <FIG>, a command speed and a feedback speed almost coincide with each other in the Y axis system,
whereas the command speed and the feedback speed are temporally misaligned in the Z axis system, and thus a rise in the Z-axial direction on a corner trajectory is delayed, resulting in a trajectory located outward from a target trajectory.

That is, as illustrated in <FIG>, when it is assumed that the response delay time of the X axis servo driver (hereinafter, servo driver SDx) is Rx, the response delay time of the Y axis servo driver (hereinafter, servo driver SDy) is Ry, the response delay time of the Z axis servo driver (hereinafter, servo driver SDz) is Rz, and position commands from the control device <NUM> reach the servo driver SDx, the servo driver SDy and the servo driver SDz in synchronization with a time t, a response time of the servomotor SMx is Tx (= t+Rx), a response time of the servomotor SMy with the minimum response delay time is Ty (= t+Ry), the response time of the servomotor SMz with the maximum response delay time is Tz (= t+Rz), and the three servomotors respond at scattered times and pass through a trajectory misaligned from the target trajectory as illustrated in <FIG>.

Therefore, in the embodiment, in order to suppress the trajectory misalignment due to the difference between the response delay times of the axes, as illustrated in <FIG>, the position commands from the control device <NUM> to the servo driver SDx, the servo driver SDy and the servo driver SDz are issued at staggered times.

Specifically, the servomotor SMz having the maximum response delay time is set as a standard servomotor, and in order to cause the servomotor SMx and the servomotor SMy to respond in synchronization with a response time T of the servomotor SMz, a command time tx to the servo driver SDx is delayed by dx (a difference (= Rz-Rx) between the response delay times of the standard servomotor SMz and the servomotor SMx) and a command time ty to the servo driver SDy is delayed by dy (a difference (= Rz-Ry) between the response delay times of the standard servomotor SMz and the servomotor SMy) with respect to a command time tz to the servo driver SDz (standard servo driver) which controls the servomotor SMz (standard servomotor).

For example, when target position coordinates at a certain time are (Px, Py, Pz), the position command of Px to the servo driver SDx is delayed by dx from the position command of Pz to the servo driver SDz, and the position command of Py to the servo driver SDy is delayed by dy from the position command of Pz to the servo driver SDz. Therefore, as illustrated in <FIG>, it is possible to obtain a trajectory close to the target trajectory.

Although a response timing between the axes is aligned by the above-described method, there is a response delay of each axis itself. Therefore, as illustrated in <FIG>, when a speed command to the X axis servo driver is changed from small to large and the speed command to the Y axis servo driver is changed from large to small, the positional deviation (difference between the command position and the feedback position) of the X axis increases temporally while the positional deviation (difference between the command position and the feedback position) of the Y axis decreases temporally, and thus it may result in a trajectory located inward from the target trajectory, as illustrated in <FIG>.

In a model in which the command speed and the feedback speed are misaligned by the response delay time as illustrated in <FIG>, the inventor found that it is effective to correct a shaded portion with respect to the trajectory misalignment due to the positional deviation at the time of a change in acceleration (including a change in a positive direction and a change in a negative direction) as illustrated in <FIG>. Specifically, a correction of the command position is performed by an amount corresponding to (<NUM>/<NUM>) × the square of the response delay time × the acceleration (predictive acceleration) which corresponds to an area of the shaded portion.

In this way, even when a feedback position of a return trajectory as shown in <FIG> has not reached a return point, the feedback position reaches the turning point with the correction of the command position, as illustrated in <FIG>.

The control device according to the embodiment includes a processor <NUM> having function modules as illustrated in <FIG>. The function modules of the processor include a predictive synchronization calculation unit <NUM>, a command position generation unit <NUM>, an X axis position correction unit <NUM>, a Y axis position correction unit <NUM>, a Z axis position correction unit <NUM>, an X axis command unit <NUM>, a Y axis command unit <NUM>, and a Z axis command unit <NUM>.

The predictive synchronization calculation unit <NUM> reads out servo parameters from the servo driver SDx, the servo driver SDy and the servo driver SDz and calculates predictive position synchronization correction parameters including an X axis response delay time, a Y axis response delay time, a Z axis response delay time, and a standard response delay time.

Here, the X axis response delay time (Rx in <FIG> and <FIG>) is an inverse number of a position loop gain which is one of the servo parameters of the servo driver SDx, the Y axis response delay time (Ry in <FIG> and <FIG>) is an inverse number of a position loop gain which is one of the servo parameters of the servo driver SDy, the Z axis response delay time (Rz in <FIG> and <FIG>) is an inverse number of a position loop gain which is one of the servo parameters of the servo driver SDz, and a standard response delay time is a maximum value of Rx, Ry and Rz.

The command position generation unit <NUM> generates an X axis command position, a Y axis command position, and a Z axis command position on the basis of the target trajectory, inputs the X axis command position to the X axis position correction unit <NUM>, inputs the Y axis command position to the Y axis position correction unit <NUM> and inputs the Z axis command position to the Z axis position correction unit <NUM>.

The X axis position correction unit <NUM> performs a correction of the X axis command position (a correction relating to the difference between the response delay times of the axes and a correction relating to the position deviation generated by the change in the acceleration) on the basis of the predictive position synchronization correction parameter, and the X axis command unit <NUM> issues the position command to the servo driver SDx on the basis of the corrected X axis command position.

The Y axis position correction unit <NUM> performs a correction of the Y axis command position (the correction relating to the difference between the response delay times of the axes and the correction relating to the position deviation generated by the change in the acceleration) on the basis of the predictive position synchronization correction parameter, and the Y axis command unit <NUM> issues the position command to the servo driver SDy on the basis of the corrected Y axis command position.

The Z axis position correction unit <NUM> performs a correction of the Z axis command position (the correction relating to the difference between the response delay times of the axes and the correction relating to the position deviation generated by the change in the acceleration) on the basis of the predictive position synchronization correction parameter, and the Z axis command unit <NUM> issues the position command to the servo driver SDz on the basis of the corrected Z axis command position.

As illustrated in <FIG>, the servo driver SDx includes an X axis position control unit <NUM> which receives the position command from the control device <NUM>, an X axis speed control unit <NUM> which receives an output from the X axis position control unit <NUM>, and an X axis current control unit (X axis torque control unit) <NUM> which receives an output from the X axis speed control unit <NUM>, a rotating portion <NUM> of the servomotor SMx is driven by an output of the X axis current control unit <NUM>, and an output of an encoder <NUM> of the servomotor SMx is fed back to the X axis position control unit <NUM>, the X axis speed control unit <NUM> and the X axis current control unit <NUM>. The X axis position control unit <NUM> outputs a position loop gain to the control device <NUM>, and the X axis speed control unit <NUM> outputs a speed loop gain to the control device <NUM>.

Further, the servo driver SDy includes a Y axis position control unit <NUM> which receives the position command from the control device <NUM>, a Y axis speed control unit <NUM> which receives an output from the Y axis position control unit <NUM>, and a Y axis current control unit (Y axis torque control unit) <NUM> which receives an output from the Y axis speed control unit <NUM>, a rotating portion <NUM> of the servomotor SMy is driven by an output of the Y axis current control unit <NUM>, and an output of an encoder <NUM> of the servomotor SMy is fed back to the Y axis position control unit <NUM>, the Y axis speed control unit <NUM> and the Y axis current control unit <NUM>. The Y axis position control unit <NUM> outputs a position loop gain to the control device <NUM>, and the Y axis speed control unit <NUM> outputs a speed loop gain to the control device <NUM>.

Further, the servo driver SDz includes a Z axis position control unit <NUM> which receives the position command from the control device <NUM>, a Z axis speed control unit <NUM> which receives an output from the Z axis position control unit <NUM>, and a Z axis current control unit (Z axis torque control unit) <NUM> which receives an output from the Z axis speed control unit <NUM>, a rotating portion <NUM> of the servomotor SMz is driven by an output of the Z axis current control unit <NUM>, and an output of an encoder <NUM> of the servomotor SMz is fed back to the Z axis position control unit <NUM>, the Z axis speed control unit <NUM> and the Z axis current control unit <NUM>. The Z axis position control unit <NUM> outputs a position loop gain to the control device <NUM>, and the Z axis speed control unit <NUM> outputs a speed loop gain to the control device <NUM>.

The processor of the control device performs steps S1 to S8 in <FIG> by executing, for example, a control program according to the embodiment.

That is, the position loop gain of each axis (of a preset synchronization group) is acquired from the servo driver in Step S1, the response delay time of each of the axes is calculated in Step S2, the response delay times of the axes are compared with each other in Step S3, the maximum response delay time of the response delay times is calculated in Step S4, a difference between the response delay time and the maximum response delay time of each of the axes is calculated in Step S5, a delay correction with respect to the command position by the difference calculated for each of the axes is performed in Step S6, a correction of the position deviation generated by the change in the acceleration according to a result of Step S6, specifically, a correction of the position deviation generated in proportion to the acceleration, is performed in Step S7, and in Step <NUM>, the position command is issued to the corresponding servo driver on the basis of the corrected command position obtained in Step S7.

As described above, it can be understood that the misalignment from the target trajectory is suppressed by performing the correction of the difference between the response delay times of the axes and the correction of the position deviation at the time of the change in the acceleration, as illustrated in <FIG>.

In the X axis position correction unit of <FIG> or Step S7 of <FIG>, the correction of the difference between the response delay times of the axes may be realized by a first term of the following Equation <NUM>, and the correction of the position deviation at the time of the change in the acceleration may be realized by a second term of the following Equation <NUM>.

An effect of Equation <NUM> can be obtained by Equation <NUM>.

Here, regarding an acceleration Ax of the second term of Equation <NUM>, a primary delay calculation of a result obtained by (d<NUM>x/dt<NUM>) (t-(Rs-Rx)) is performed with an inverse number Rx of the position loop gain as a primary delay time constant, and a result obtained by performing the primary delay calculation with <NUM>/(2π×speed loop gain) as the primary delay time constant is used as the acceleration Ax.

Further, in the Y axis position correction unit of <FIG> or Step S7 of <FIG>, the correction of the difference between the response delay times of the axes may be realized by a first term of the following Equation <NUM>, and the correction of the position deviation at the time of the change in the acceleration may be realized by a second term of the following Equation <NUM>.

Here, regarding an acceleration Ay of the second term of Equation <NUM>, a primary delay calculation of a result obtained by (d<NUM>y/dt<NUM>) (t-(Rs-Ry)) is performed with an inverse number Ry of the position loop gain as a primary delay time constant, and a result obtained by performing the primary delay calculation with <NUM>/(2π×speed loop gain) as the primary delay time constant is used as the acceleration Ay.

Further, in the Z axis position correction unit of <FIG> or Step S7 of <FIG>, the correction of the difference between the response delay times of the axes may be realized by a first term of the following Equation <NUM>, and the correction of the position deviation at the time of the change in the acceleration may be realized by a second term of the following Equation <NUM>.

Here, regarding an acceleration Az of the second term of Equation <NUM>, a primary delay calculation of a result obtained by (d<NUM>z/dt<NUM>) (t-(Rs-Rz)) is performed with an inverse number Rz of the position loop gain as a primary delay time constant, and a result obtained by performing the primary delay calculation with <NUM>/(2π×speed loop gain) as the primary delay time constant is used as the acceleration Az.

Each of the functional modules of the control device may be realized by software using a central processing unit (CPU) or may be realized by a logic circuit (hardware) formed in an integrated circuit (IC chip) or the like.

In the former case, the control device includes a CPU which performs an instruction of a control program which is software realizing each function, a read only memory (ROM)) or storage device (these are referred to as "recording media") in which the control program and a variety of data are recorded to be readable by a computer (or CPU), a random access memory (RAM) which develops the control program, and so on. Additionally, the computer (or CPU) reads and executes the control program from the recording medium, thereby achieving an object of the embodiment. A "non-temporary tangible medium" such as a disk, a card, a semiconductor memory, a programmable logic circuit, or the like can be used as the recording medium. Furthermore, the control program may also be provided to the computer via any transmission medium (communication networks, broadcast waves, or the like) which can transmit the control program. In addition, the embodiment can also be realized in the form of data signals embedded in carrier waves in which the control program is embodied by electronic transmission.

According to the control device defined in claim <NUM>, since the response timings of the plurality of servomotors are aligned, it is possible to suppress the trajectory misalignment due to a deviation in the response delay time among the plurality of axes (the plurality of servomotors).

According to claims <NUM> and <NUM>, the position command may be issued with applying a correction of an amount proportional to acceleration at the time of a change in the acceleration of each of the servomotors.

According to the above-described configuration, it is possible to suppress the trajectory misalignment due to the positional deviation at the time of the change in the acceleration.

According to the method defined in claim <NUM>, since the response timings of the plurality of servomotors are aligned, it is possible to suppress the trajectory misalignment due to the deviation in the response delay time among the plural axes (plural servo motors).

Claim 1:
A control device (<NUM>, <NUM>, <NUM>) adapted to issue commands to a plurality of servo drivers (SDx, SDy, SDz) corresponding to a plurality of servomotors (SMx, SMy, SMz),
wherein the control device is adapted to set a servo driver among the plurality of servo drivers (SDx, SDy, SDz) corresponding to a standard servomotor having a maximum response delay time among the plurality of servomotors (SMx, SMy, SMz) as a standard servo driver, and to delay a command timing to another servo driver among the plurality of servo drivers (SDx, SDy, SDz) from a command timing to the standard servo driver by a difference between a response delay time of the standard servomotor and a response delay time of a servomotor among the plurality of servomotors (SMx, SMy, SMz) corresponding to the another servo driver, wherein the commands are position commands based on a target trajectory; the control device (<NUM>, <NUM>, <NUM>) being characterized in that
the control device (<NUM>, <NUM>, <NUM>) is adapted to read a position loop gain from each of the plurality of servo drivers (SDx, SDy, SDz) and calculate the response delay time of each of the plurality of servomotors (SMx, SMy, SMz) as an inverse number of the read position loop gain.