WO2012049866A1 - 制御装置、制御システムおよび制御方法 - Google Patents

制御装置、制御システムおよび制御方法 Download PDF

Info

Publication number
WO2012049866A1
WO2012049866A1 PCT/JP2011/056227 JP2011056227W WO2012049866A1 WO 2012049866 A1 WO2012049866 A1 WO 2012049866A1 JP 2011056227 W JP2011056227 W JP 2011056227W WO 2012049866 A1 WO2012049866 A1 WO 2012049866A1
Authority
WO
WIPO (PCT)
Prior art keywords
target
speed
target trajectory
acceleration
end point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2011/056227
Other languages
English (en)
French (fr)
Inventor
守 恵木
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Omron Corp
Original Assignee
Omron Corp
Omron Tateisi Electronics Co
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Omron Corp, Omron Tateisi Electronics Co filed Critical Omron Corp
Priority to CN201180049705.7A priority Critical patent/CN103180791B/zh
Priority to US13/877,930 priority patent/US9606524B2/en
Priority to EP11832309.6A priority patent/EP2629163B1/en
Publication of WO2012049866A1 publication Critical patent/WO2012049866A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Program-control systems
    • G05B19/02—Program-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of program data in numerical form
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Program-control systems
    • G05B19/02—Program-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of program data in numerical form
    • G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of program data in numerical form characterised by control of velocity, acceleration or deceleration
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/41—Servomotor, servo controller till figures
    • G05B2219/41408—Control of jerk, change of acceleration
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/42—Servomotor, servo controller kind till VSS
    • G05B2219/42244—Enter acceleration, jerk, generator outputs acceleration, speed, position by integration
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/43—Speed, acceleration, deceleration control ADC
    • G05B2219/43061—Maximum acceleration deceleration lookup table as function of distance

Definitions

  • the present invention relates to a control device for controlling a device having a movable part movable by a plurality of drive shafts, a control system including the control device, and a control method.
  • Such an apparatus has movable parts such as an arm for gripping a workpiece and an end mill for processing the workpiece.
  • movable parts such as an arm for gripping a workpiece and an end mill for processing the workpiece.
  • Such a movable part can be moved by a plurality of drive shafts, and each of these drive shafts is driven by a servo motor or a stepping motor.
  • Such behavior determination includes an offline calculation method in which the behavior is calculated in advance and an online calculation method in which the behavior is dynamically calculated during operation.
  • a numerical control device disclosed in Japanese Patent Application Laid-Open No. 07-066420 can perform high-speed and high-accuracy speed control in consideration of acceleration / deceleration control, and can smoothly drive a spline curve. Abnormal vibration can be reduced, and speed priority and accuracy priority can be selected. More specifically, the numerical control device disclosed in Patent Document 1 suppresses a sudden acceleration / deceleration at the start point and the end point, thereby suppressing mechanical vibration and specifying the target position without exceeding the target position. Decelerate and stop at acceleration (deceleration) speed.
  • the curve interpolation acceleration / deceleration control method disclosed in Japanese Patent Laid-Open No. 2000-148223 obtains a smooth acceleration / deceleration that moves at a high speed suitable for the shape of the curve.
  • Patent Document 3 the interpolating curve acceleration / deceleration processing method disclosed in Japanese Patent Laid-Open No. 2000-163114 (Patent Document 3) can move the machine according to the command trajectory regardless of the curvature of the curve.
  • Patent Document 4 adds a new sampling point only to a portion with a large curvature, thereby minimizing the number of sampling points (data). Approximate the curve with, and reproduce the original figure with smooth and little shape difference.
  • Patent Document 5 performs interpolation control by spline interpolation control.
  • a set value such as a target trajectory is set in advance, and the device may be operated according to this set value.
  • machining program such as a target trajectory
  • the moving speed of a tool such as an end mill affects the cutting amount, so it is important to control not only the position of the tool but also the moving speed.
  • fluctuations in the acceleration of the tool lead to abnormal appearance and poor quality of the processed surface. Therefore, there is a need to make the acceleration during movement as constant as possible. In particular, in the straight portion, a problem is likely to occur if the acceleration varies.
  • a machining process for a certain section may be followed by a machining process for another section. Therefore, it is preferable that the moving speed at the end point of the set target trajectory is maintained at a predetermined target value. Otherwise, the moving speed becomes discontinuous near the end point of the target trajectory, or the acceleration becomes discontinuous in the subsequent straight section.
  • Patent Documents 1 to 5 described above do not solve such problems.
  • the speed command is calculated after calculating the trajectory length (remaining distance) from the current position to the end point for each control cycle. This increases the amount of calculation. For this reason, it is not easy to complete such arithmetic processing within each control period due to processing capacity limitations.
  • the present invention has been made to solve such a problem, and its purpose is to set the speed at the end point of the target trajectory to the target value even when the target trajectory is dynamically generated. It is an object to provide a control device, a control system, and a control method that can be maintained.
  • a control device for controlling a device having a movable part movable by a plurality of drive shafts.
  • the control device includes a target trajectory generating means for generating a target trajectory for a movement section defined by a start point and an end point, a remaining distance estimating means for estimating a remaining distance from the current position to the end point in the target trajectory, When the current moving speed is changed to the target speed that is the moving speed that the movable part should satisfy at the end point of the target trajectory, the change is made so that the distance that the movable part moves substantially matches the remaining distance.
  • Acceleration calculating means for calculating acceleration
  • moving speed updating means for calculating a new target value of the moving speed by correcting the current moving speed with the calculated acceleration.
  • the acceleration calculating means calculates the acceleration based on the current moving speed, the target speed, and the distance.
  • the acceleration calculating means calculates the acceleration so that the current moving speed is changed at a constant rate to the target speed in each control cycle.
  • the remaining distance estimating means sets a predetermined number of sample points for the section from the current position to the end point in the target trajectory, and estimates the remaining distance by interpolating the set sample points.
  • the remaining distance estimation means for the section from the current position in the target trajectory to the end point, the section closer to the current position compared to the section far from the current position, so that the interval between the sample points is closer, Set the sample point.
  • the target trajectory is defined in the form of a function that includes parameters.
  • the control device further includes a movement command means for outputting a command indicating an amount by which the movable portion should move within one control cycle so that the new target value of the movement speed calculated by the movement speed update means is obtained.
  • the movement command means includes means for calculating a distance that the movable part should move corresponding to the new target value of the moving speed, and corresponding to the distance that the movable part should move, Means for calculating a position to be moved, and means for calculating a movement command for each of the plurality of drive shafts corresponding to the position to which the movable portion should move.
  • the target trajectory is defined by a Bezier curve.
  • the target trajectory generating means generates a target trajectory in response to an externally input signal.
  • the moving speed update means calculates a new target value of the moving speed so that the amount of change in acceleration and / or jerk does not exceed a predetermined limit.
  • a control system includes a movable mechanism having a movable portion movable by a plurality of drive shafts, a driver for driving the movable mechanism, and a control device for giving a control command to the driver.
  • the control device includes a target trajectory generating means for generating a target trajectory for a movement section defined by a start point and an end point, a remaining distance estimating means for estimating a remaining distance from the current position to the end point in the target trajectory, When the current moving speed is changed to the target speed that is the moving speed that the movable part should satisfy at the end point of the target trajectory, the change is made so that the distance that the movable part moves substantially matches the remaining distance.
  • Acceleration calculating means for calculating acceleration
  • moving speed updating means for calculating a new target value of the moving speed by correcting the current moving speed with the calculated acceleration.
  • a control method for controlling a device having a movable part movable by a plurality of drive shafts includes a step of generating a target trajectory for a movement section defined by a start point and an end point, a step of estimating a remaining distance from the current position to the end point in the target trajectory, and a current movement speed of the movable unit, Calculating the acceleration associated with the change so that the distance traveled by the movable part substantially coincides with the remaining distance when the target speed is changed to the target speed that the movable part should satisfy at the end point of the target trajectory; And a step of calculating a new target value of the moving speed by correcting the current moving speed with the calculated acceleration.
  • the speed at the end point of the target trajectory can be maintained at the target value.
  • FIG. 1 is a schematic diagram showing an appearance of a system SYS according to the present embodiment.
  • FIG. 2 is a schematic diagram showing an appearance of control device 1 included in system SYS according to the present embodiment.
  • FIG. 3 is a schematic diagram showing a hardware configuration of control device 1 included in system SYS according to the present embodiment.
  • system SYS is driven by control device 1, measure 3 indicating the dimensions of workpiece 2 such as wood or metal to be cut, and a command from control device 1. And a switch 5 that outputs an event such as a trajectory change in response to an operation by an operator or the like.
  • control device 1 will be described in detail later with reference to FIGS.
  • the workpiece 2 is placed on the base and cut into a desired shape by an operation of an operator or the like.
  • a measure 3 is provided along the workpiece 2, and the operator operates the switch 5 when the movable mechanism 4 performs cutting along a certain track and reaches a target position.
  • the trajectory is changed according to the control logic described later.
  • the movable mechanism 4 is a device having a movable part (arm part) movable by a plurality of drive shafts, and a tool (typically an end mill) for cutting the workpiece 2 is provided at the tip of the arm part. It is installed. As will be described later, the arm portion moves in accordance with a plurality of drive shafts (typically, three or six axes) each having a motor mechanically coupled thereto.
  • the control device 1 controls the position of the end mill according to a control logic as will be described later, thereby processing the workpiece 2 into a part having a desired size and shape.
  • the position of this end mill is also called “TCP (Tool Center Point)” below.
  • TCP speed The moving speed of the arm portion of the movable mechanism 4 (endmill moving speed) is also referred to as “TCP speed”.
  • the control device 1 dynamically determines / changes the target trajectory in response to an operation on the switch 5 by the operator. For example, considering that an approximately rectangular part having a different length depending on the type is manufactured for the same workpiece 2, the part to start processing (that is, the length of the straight part) is different, but the content to be processed (Ie, the curvature of the machined surface, etc.) is often the same. In such a case, using the same machining program (information such as the target trajectory on the machining surface is defined), the absolute target trajectory can be dynamically determined according to the product type. . Such dynamic determination of the target trajectory is executed by the operator operating the switch 5 after confirming the positional relationship between the major 3 (and the arrow provided on the scale) and the movable mechanism 4.
  • the part to start processing that is, the length of the straight part
  • the content to be processed Ie, the curvature of the machined surface, etc.
  • control device 1 is typically configured based on a so-called PLC (Programmable Logic Controller). Specifically, the control device 1 includes a PLC 20 and motor drivers 31, 32, and 33.
  • PLC Process Control Circuit
  • the PLC 20 is a processing entity that executes control logic according to the present embodiment to be described later.
  • the control logic is generally referred to as a motion program, and is a program for controlling a motor for driving the movable mechanism 4.
  • a sequence of such a motion program may be described in the form of a sequence program using a ladder diagram (ladder diagram) language or the like. Further, in addition to such a sequence program, a program (function) for performing various arithmetic operations is executed.
  • the PLC 20 includes a power supply unit 21, a main arithmetic unit 22, an IO (Input Output) unit 23, and an MC (Motion Control) unit 24.
  • the power supply unit 21, main arithmetic unit 22, IO unit 23, and MC unit 24 are electrically connected to each other via a backplane 25.
  • the power supply unit 21 generates DC power for driving each unit and DC power for IO.
  • the electric power generated by the power supply unit 21 is distributed to each unit via the backplane 25.
  • the main arithmetic unit 22 executes a sequence program.
  • the IO unit 23 receives a detection signal from a sensor or the like installed in a device to be controlled, and outputs a control command to an actuator or a display device installed in the device to be controlled.
  • the MC unit 24 executes a motion program in accordance with a command from the main arithmetic unit 22 and outputs a control command calculated as a result to the motor drivers 31, 32 and 33.
  • the MC unit 24 outputs a series of control commands to the motor drivers 31, 32, 33 as time elapses.
  • the MC unit 24 reports the execution status and execution result of the motion program to the main arithmetic unit 22.
  • FIG. 2 illustrates a configuration in which the MC unit 24 independent of the main arithmetic unit 22 is provided, but the dedicated MC unit 24 is omitted because the main arithmetic unit 22 also has the function of the MC unit 24.
  • a configuration can also be adopted.
  • the main arithmetic unit 22 includes a microprocessor (MPU: Micro-Processing Unit) 221, a system program memory (SROM) 222, a buffer memory (BROM: Buffer Read Only Memory) 223, A work memory (WRAM: Working Random Access Memory) 224, a communication interface (communication I / F) 225, a user memory (UM: User Memory) 226, and an IO memory (IOM: Input Output Memory) 228 are included.
  • MPU Micro-Processing Unit
  • SROM system program memory
  • BROM Buffer Read Only Memory
  • WRAM Working Random Access Memory
  • communication interface communication I / F
  • UM User Memory
  • UM User Memory
  • IOM Input Output Memory
  • the microprocessor (MPU) 221 controls the entire unit. More specifically, the microprocessor 221 sequentially reads out each instruction word constituting the user program from the user memory 226 and executes it.
  • a system program memory (SROM) 222 stores a program executed by the microprocessor 221.
  • a buffer memory (BROM) 223 stores user programs.
  • the work memory (WRAM) 224 is used as a work area when the microprocessor 221 executes the system program.
  • the communication interface 225 performs data communication with a PC (personal computer) 10 as will be described later.
  • a user memory (UM) 226 stores a user program transferred from the buffer memory 223 when the power is turned on.
  • the IO memory (IOM) 228 holds the input / output states of the IO unit 23 and the MC unit 24 to reflect them.
  • ASIC application specific integrated circuit
  • MPU microprocessor
  • the IO unit 23 includes a processing circuit (MPU) 231, an input circuit 232, and an output circuit 233.
  • the input circuit 232 receives detection signals from various sensors provided in the movable mechanism 4 and outputs the state values to the main arithmetic unit 22.
  • the input circuit 232 includes a photocoupler circuit or the like.
  • the output circuit 233 outputs a control command to an actuator or a display device installed in the device to be controlled.
  • the processing circuit 231 exchanges internal commands with the input circuit 232 and the output circuit 233.
  • the MC unit 24 includes a processing circuit (MPU) 241, a controller interface (controller I / F) 242, an input circuit 243, and an output circuit 244.
  • the controller interface 242 manages data transmission with the motor drivers 31, 32, and 33 (see FIG. 2).
  • the input circuit 243 and the output circuit 244 are the same as the input circuit 232 and the output circuit 233, respectively. However, the input circuit 243 and the output circuit 244 mainly exchange information relating to motion control (motor drivers 31, 32, 33 and motors 41, 42, 43).
  • the motor drivers 31, 32, 33 drive the corresponding motors 41, 42, 43 in response to a control command from the MC unit 24. That is, the motor drivers 31, 32, and 33 supply driving power to the motors 41, 42, and 43.
  • the motors 41, 42, and 43 are components of the movable mechanism 4 (see FIG. 1).
  • the movable mechanism 4 moves its arm portion when these motors are rotationally driven.
  • As the motors 41, 42, 43, servo motors, stepping motors, etc. are employed. In this case, a motor driver corresponding to the type of motor to be used is used.
  • FIG. 2 shows a configuration example in which the motor drivers 31, 32, and 33 are connected to the MC unit 24 via a communication line 60 such as EtherCAT (registered trademark).
  • EtherCAT registered trademark
  • the MC unit 24 and the motor drivers 31, 32, 33 may be directly connected.
  • the control commands given from the MC unit 24 to the motor drivers 31, 32, 33 are values such as the motor position, speed, and torque.
  • position commands (target positions) for each of the motor drivers 31, 32, and 33 are given from the MC unit 24 at a constant cycle.
  • This target position may be the rotation position (rotation angle) of the motors 41, 42, 43, or the position of a specific part of the movable mechanism 4 driven by the motors 41, 42, 43. Also good. That is, it is a question of which coordinate is used as a reference to determine the target position, and an appropriate reference is adopted according to the situation.
  • the target position is expressed as “the number of pulses sent within the control cycle”.
  • the number of pulses indicates a relative position (position deviation / change) from the position in the immediately preceding control cycle.
  • the motor drivers 31, 32, and 33 interpret position commands (target positions) and set information necessary for driving the motors 41, 42, and 43 (for example, coefficients for converting the position commands into current values) )have. For example, information on the number of pulses corresponding to one rotation of the motors 41, 42, 43 is preset in the motor drivers 31, 32, 33.
  • Motor drivers 31, 32, and 33 receive feedback of current positions (actual values) from encoders that detect the rotational positions of servo motors provided in the motors 41, 42, and 43.
  • the motor drivers 31, 32, and 33 control the magnitude of drive power so that the actual value follows the command value using the actual value and the actual speed calculated as the change in the actual value (feedback control).
  • the MC unit 24 information on the number of pulses corresponding to one rotation of the motors 41, 42, and 43 is set in advance similarly to the motor drivers 31, 32, and 33, and one rotation of the motors 41, 42, and 43 is performed.
  • the amount of movement information about the position of the specific part of the movable mechanism 4 corresponding to is preset. Using these set values, the MC unit 24 calculates the number of pulses necessary to realize the motion of the control target programmed for each control cycle, and sends it to the motor drivers 31, 32, 33.
  • the PC 10 is configured based on a general-purpose computer, and is connected to the main arithmetic unit 22 via a communication line such as a USB (Universal Serial Bus). From the PC 10, a program for realizing the control logic according to the present embodiment is transferred to the main arithmetic unit 22, and on the display of the PC 10, the operation status of the PLC 20 and a log can be confirmed. Since the configuration of the PC 10 is known, a detailed description is not given here.
  • control device 1 is illustrated in FIG. 2, it is not limited to this configuration.
  • control device 1 may be configured by combining a general-purpose computer-based processing device and an input / output device that exchanges signals with the motor drivers 31, 32, 33, sensors, and the like.
  • a general-purpose module provided by an OS (Operating System) may be used in order to realize the control logic according to the present embodiment. That is, there is a form that does not have some common functions as a program for providing the control logic according to the present embodiment, but such a form is within the scope of the present invention. It is clear that it is included.
  • OS Operating System
  • control device 1 may be embodied as a combination of software and a dedicated hardware circuit.
  • the current speed is accelerated / decelerated with as little fluctuation as possible so that the TCP speed at the end point of the target trajectory set in advance matches the final target speed. That is, the TCP speed smoothly changes toward the final target speed, and continuity with the target speed in the subsequent trajectory can be maintained. This improves the uniformity of the finished surface.
  • the target acceleration for changing the current speed to the final target speed is estimated based on the estimated remaining distance and the remaining distance from the current position to the end point on the target trajectory in each control cycle. An optimal value is calculated. In this way, in each control cycle, the target acceleration is sequentially calculated while taking into account the end point in the target trajectory, so it is possible to eliminate a situation in which the speed or acceleration suddenly increases / decreases at the end point of the target trajectory. .
  • the current speed and the target speed are calculated based on a speed trajectory obtained by linear interpolation or polynomial interpolation.
  • the calculation time becomes deterministic and the configuration is more suitable for the online calculation method.
  • the amount of change in acceleration and jerk associated with the change in TCP speed is restricted so as not to exceed a predetermined limit value. Therefore, occurrence of “chatter” and torque over in the movable mechanism 4 can be suppressed.
  • FIG. 4 is a flowchart showing an outline of the processing procedure of the control logic according to the present embodiment. Note that each step shown in FIG. 4 is executed by the control device 1, but the execution subject differs depending on the implementation. That is.
  • the main arithmetic unit 22 (see FIG. 2) may perform all the processes, or the main arithmetic unit 22 and the MC unit 24 (see FIG. 2) may perform the processes jointly.
  • step S4 control in the system SYS is started. At this time, cutting of the workpiece 2 by the movable mechanism 4 is started (see FIG. 1) (step S2). Subsequently, it is determined whether it is the generation timing of the target trajectory (step S4). That is, when the movable mechanism 4 reaches a predetermined position of the workpiece 2, a target trajectory corresponding to the position is generated. It is determined whether or not this target trajectory should be generated. More specifically, it is determined whether or not a signal (detection signal) is externally input from the switch 5 as shown in FIG. If it is not the generation timing of the target trajectory (NO in step S4), the process of step S4 is repeated.
  • a signal detection signal
  • the target trajectory is generated for the movement section defined by the start point and the end point (step S6). That is, a target trajectory is generated in response to a signal input from the outside.
  • the start point and the end point are determined based on information set in advance. In other words, the start point and the end point may be set in association with the actual position of the work 2, or the start point and the end point may be set independently of the actual position of the work 2. And the process for moving the movable mechanism 4 along a target track
  • the remaining distance from the current position to the end point in the target trajectory is estimated (step S8). That is, the current value of TCP (information indicating the current position) of the movable mechanism 4 is acquired, and the distance to the end point of the target trajectory is estimated using the determined target trajectory (a function indicating the target trajectory).
  • the current TCP speed of the movable mechanism 4 (moving speed of the movable part) is changed to the final target speed that is the moving speed that the movable part of the movable mechanism 4 should satisfy at the end point of the target trajectory.
  • the target acceleration is calculated so that the distance that the movable part moves substantially matches the remaining distance estimated in step S8 (step S10).
  • a new target value of the TCP speed is calculated by correcting the current TCP speed with the target acceleration calculated in step S10 (step S12).
  • step S14 it is determined whether or not the end point of the target trajectory set by the TCP has been reached. That is, it is determined whether or not the motion control along the target trajectory is finished. If the end point of the target trajectory set by TCP has been reached (YES in step S14), the motion control based on the current target trajectory ends. In addition, when programmed to set a plurality of target trajectories for the same workpiece 2, the processes after step S2 are executed again.
  • step S16 A command indicating the amount to be moved within one control cycle is output (step S16). That is, at the start timing of the next control cycle, a predetermined number of pulses (dwell pulses) are given to the motor drivers 31, 32, 33 so that the TCP moves to a position corresponding to the new target value. . And it waits until the next control period (step S18), and the process after step S8 is repeated.
  • step S6 Generation of target trajectory (step S6)> (D1: Issues in the prior art)
  • FIG. 5 is a diagram illustrating an example of a cutting process performed on the workpiece 2.
  • FIG. 5 for example, consider a case where a plurality of types of products (product type A and product type B) whose one side is cut into the same shape are manufactured.
  • the shape of cutting is the same between the types A and B shown in FIG. 5, but the positions at which the cutting is started are different.
  • the types A and B can be manufactured only by changing the cutting start timing. In this case, if it sees about the part cut, it will suffice if it cuts with the same track.
  • a common machining program is prepared without creating a machining drawing or machining program for each product type, and the relative positional relationship between the workpiece 2 and the robot is detected to move the target trajectory. There is a need to set it up (ie online). By sharing such a machining program, the production cost can be reduced.
  • FIG. 6 is a diagram showing a processing example for generating a target trajectory using circular interpolation when the workpiece 2 is cut into an L shape.
  • FIG. 6 (a) shows an example in which circular interpolation is appropriately set.
  • FIGS. 6B and 6C show examples in which the circular interpolation is set inappropriately.
  • FIG. 6B shows an example in which the direction vector on the trajectory is discontinuous as a result of the curvature being set inappropriately in order to maintain the cutting shape of the workpiece 2.
  • FIG. 6C in order to maintain the continuity of the direction vector on the trajectory, the direction vector is forcibly continued using an arc having a certain curvature, and as a result, the target cutting shape cannot be realized. Shows an example in which the work 2 has bite into the machined surface.
  • FIG. 7 is a diagram illustrating a processing example for generating a target trajectory using a free curve when the corner of the workpiece 2 is cut.
  • FIG. 7 (a1) shows an example of the TCP speed of the movable mechanism 4 that moves along the target trajectory generated according to the prior art
  • FIG. 7 (a2) shows an example of the target trajectory generated according to the prior art. .
  • FIG. 7B1 shows an example of the TCP speed of the movable mechanism 4 controlled by the control logic according to the present embodiment
  • FIG. 7B2 shows the target trajectory generated by the control logic according to the present embodiment. An example is shown.
  • control in order to maintain continuity with the subsequent trajectory speed, control is performed so that the TCP speed at the end point of the target trajectory matches the predetermined final target speed. .
  • control is performed so as to maintain the continuity of the velocity vector on the target trajectory.
  • FIG. 7B2 a desired target trajectory can be generated.
  • a target trajectory is generated mainly using a free curve.
  • the TCP speed on the target trajectory can be matched with the target speed.
  • the TCP speed (particularly the end point speed) on the target trajectory is set to the target speed.
  • the completion of the arithmetic processing within the control cycle is not guaranteed. That is, in order to maintain the velocity vector at the end point of the target trajectory at the target value, it is necessary to calculate the remaining distance (remaining trajectory length) online.
  • the remaining distance can be calculated by using numerical integration, if the integration process is performed while changing the number of divisions according to the curvature in order to calculate an accurate remaining distance, the remaining distance within one control period is calculated. Completion of the distance calculation process cannot be guaranteed.
  • control logic sets a target trajectory using a free curve in order to maintain the continuity of each direction vector at the start point and the end point of the target trajectory.
  • a target trajectory is defined in the form of a function including parameters.
  • interpolation curves such as Bezier curves, ⁇ -spline curves, NURBUS, Hermite curves, cut-maru-rom spline curves, and Ferguson / Coons curves can be used.
  • a “Bézier curve” is adopted among these interpolation curves will be described as an example.
  • FIG. 8 is a diagram for explaining a method for generating a target trajectory (Bézier curve) according to the present embodiment.
  • a target trajectory is generated using a Bezier curve
  • a movement section including at least a start point and an end point needs to be defined.
  • a first trajectory serving as a reference for setting the target trajectory and a second trajectory that is a trajectory connected to the first trajectory are defined.
  • the start point of the target trajectory is located on the first trajectory, and the end point of the target trajectory is located on the second trajectory.
  • the first and second trajectories can be arbitrarily set, but in the system SYS as shown in FIG. 1, the work surface of the workpiece is defined as the first and second trajectories.
  • the starting point P1 (x1, x2, x3) may be determined in response to the work 2 reaching a predetermined position by the switch 5 (see FIG. 1) or the like. Since two control vectors are required to set up a Bezier curve, control points P2 and P3 are determined as the start point P1 is determined, and control vectors 1 and 2 are also determined by these points.
  • control points P2 (x2, y2, z2) and P3 (x3, y3, z3) are generally determined in advance. Therefore, when the start point P1 is determined, the control vector 1 is determined as a line segment connecting the start point P1 and the control point P2. Further, the end point P4 (x4, y4, z4) is determined corresponding to the positional relationship between the start point P1 and the control point P2. Then, the control vector 2 is determined as a line segment connecting the control point P3 and the end point P4.
  • the Bezier curve that is, the target trajectory is determined by determining the control vectors 1 and 2 in this way.
  • the target position P (r) on the target trajectory can be expressed as follows.
  • the target position P (r) is defined as a value on three-dimensional coordinates.
  • the control vector may be set at a position independent of the first and second trajectories. Further, the first track and the second track are not necessarily orthogonal.
  • start point P1, the control point P2, the end point P3, and the end point P4 can be arbitrarily set. That is, these points may be determined according to detection by the sensor, or a fixed value may be set in advance.
  • a target trajectory is generated between the two control vectors as shown in FIG. 8, so that it is allowed to bite into the machining surface such as woodworking. Suitable for generating target trajectories in unseen applications. Further, since the start point and end point of the target trajectory coincide with the start point and end point of the control vector, the control vector can be easily generated.
  • step S8 the remaining distance estimation processing from the current position to the end point in the target trajectory, which is a reference for calculating the target acceleration, will be described.
  • the Bezier curve is a high-order function defined by a parametric variable, and the relationship between the parametric variable and the orbit length cannot be expressed by an elementary function. Therefore, in order to accurately calculate the remaining distance from a certain position (current position) on the target trajectory to the end point, it is necessary to use numerical integration. On the other hand, if a large number of division points for numerical integration are set, the calculation process increases, and the calculation process may not be completed within one control cycle.
  • a predetermined number of sample points are set for a section from the current position to the end point in the target trajectory, and the set sample points are linearly or polynomially interpolated.
  • the remaining distance is estimated.
  • FIG. 9 is a diagram for explaining a remaining distance estimation method according to the present embodiment.
  • 9A shows a case where the current position is close to the start point of the target trajectory (time t1)
  • FIG. 9B shows a case where the current position is close to the end point of the target trajectory (time t2).
  • the remaining distance lLeft (t) is the sum of distances L1, L2,..., Ln between adjacent sample points P1, P2,. Calculated as ⁇ Li.
  • the number of sample points to be set may be determined depending on the processing capability of the control device 1, but 4 to 6 points are preferable, for example.
  • the current position on the target trajectory is set on the target trajectory regardless of whether it is close to the start point or close to the end point.
  • the number of sample points to be maintained is kept constant. Therefore, if the current position on the target trajectory is close to the start point, the distance between the sample points will be relatively large and the estimation accuracy will decrease, but conversely, if the current position on the target trajectory is close to the end point In this case, the distance between the sample points becomes relatively small, and the estimation accuracy is improved. That is, as the curvature of the target trajectory increases, the estimation error for the remaining distance increases, but as the end point approaches, the error disappears.
  • the estimated remaining distance lLeft (t) determines the target acceleration (that is, the speed change of the TCP speed on the target trajectory), but the current position on the target trajectory is close to the end point. The higher the accuracy of the calculated target acceleration. As a result, as the end point is approached, the TCP speed matches the final target speed at the end point.
  • the sample point is a section closer to the current position than the section far from the current position in the section from the current position to the end point in the target trajectory.
  • the interval between the sample points is set to be closer.
  • a sample point may be set in consideration of the point where the curvature in the target trajectory is maximized. That is, the parameter r having the maximum curvature on the target trajectory is calculated according to the following equation.
  • a predetermined number of sample points may be set on the basis of the position of the parametric variable r that maximizes the curvature.
  • step S10 Calculation of target acceleration (step S10)> Next, processing for calculating the target acceleration while moving on the target trajectory will be described.
  • the speed change (acceleration) from each current position to the target value of the TCP speed is linear.
  • the target acceleration is calculated.
  • FIG. 10 is a diagram for describing a target acceleration calculation method according to the present embodiment.
  • the TCP speed on the target trajectory at a certain time is the current speed Vel
  • the target value of the TCP speed at the end of the target trajectory is the final target speed vEnd
  • the acceleration that linearly connects Vel and the final target speed vEnd is defined as a target acceleration AccRef.
  • the target acceleration AccRef is calculated so that the current speed Vel is changed at a constant rate up to the final target speed vEnd. That is, the target acceleration AccRef is calculated according to the following formula.
  • AccRef (vEnd ⁇ Vel) / Tnec
  • Tnec is a time required to move by the estimated remaining distance lLeft, and is calculated according to the following equation.
  • AccRef (vEnd 2 ⁇ Vel 2 ) / (2 ⁇ lLeft) That is, the target acceleration AccRef is calculated based on the current speed Vel, the final target speed vEnd, and the estimated remaining distance lLeft.
  • step S12 Calculation of new target value of TCP speed (step S12)> Next, a process for calculating a target value (target speed) of the updated TCP speed in each control cycle will be described.
  • the target value (target speed VelRef) of the TCP speed in the next control period is calculated. That is, the target speed VelRef is calculated according to the following formula.
  • VelRef Vel + AccRef ⁇ H.
  • Output of movement command (step S16)>
  • FIG. 11 is a diagram showing the characteristics of the Bezier curve and its trajectory length according to the present embodiment.
  • a Bezier curve as shown in FIG. 11A is set as the target trajectory, the position on the target trajectory is defined by the parameter r.
  • the trajectory length L of the target trajectory has a non-linear relationship with respect to the parameter r.
  • FIG. 12 is a diagram showing the relationship between the displacement amount of the parameter r and the displacement amount of the trajectory length L in the Bezier curve.
  • the parameter increment ⁇ r when the arm portion of the movable mechanism 4 is moved by the change amount ⁇ L along the target trajectory within a certain control cycle changes depending on the value of r. become. That is, since the relationship between the parametric variable r and the trajectory length L is non-linear, even if the parametric variable increment ⁇ r is a constant value, the TCP speed (current speed Vel) does not become a constant value.
  • the moving speed on the target trajectory is controlled by sequentially calculating the parameter increment ⁇ r depending on the value of the parameter r. More specifically, as described in detail below, the nonlinear relationship is simply linearized using a value obtained by partial differentiation of the trajectory length L with respect to the parameter r.
  • FIG. 13 is a diagram for describing a method for calculating parameter increment ⁇ r with respect to change amount ⁇ L in the trajectory length in the control logic according to the present embodiment.
  • the trajectory length change ⁇ L corresponding to the target speed VelRef corresponds to the parametric variable increment ⁇ r_linear.
  • the change amount of the trajectory length corresponding to the parametric variable increment ⁇ r_linear is as calculated by the target velocity VelRef and the parametric variable increment ⁇ r_linear. Corresponds to the total travel length overshoot.
  • the parameter increment ⁇ r necessary to realize the target velocity VelRef is equal to the gradient a of the trajectory length L to the parameter increment ⁇ r_linear. It can be calculated as a value multiplied by the reciprocal.
  • parameter increment ⁇ r parameter variable increment ⁇ r_linear ⁇ 1 / a.
  • a position command (target position) is given so that the arm portion of the movable mechanism 4 moves to a position (target position P (r (t + 1))) on the target trajectory indicated by the updated parameter r. That is, a droop pulse for moving the arm portion of the movable mechanism 4 to a newly calculated position is given from the MC unit 24 to the motor drivers 31, 32, 33.
  • the movement command given to the movable mechanism 4 corresponds to the distance to which the arm portion of the movable mechanism 4 should move corresponding to the new target value (target speed VelRef) of the moving speed (change amount ⁇ L of the track length). ) And the position on the target trajectory on which the arm portion of the movable mechanism 4 should move on the target trajectory (target position) corresponding to the distance (the change amount ⁇ L of the trajectory length) to which the arm portion of the movable mechanism 4 should move.
  • P (r)) is calculated, and is calculated as a movement command (dwelling pulse) for each of the plurality of drive shafts corresponding to the position to which the arm portion of the movable mechanism 4 should move.
  • FIG. 14 is a schematic diagram showing functional blocks for realizing the control logic according to the present embodiment.
  • control logic includes a Jerk processing module 301, an acceleration processing module 302, a target speed calculation module 303, a delay module 304, a parameter variable increment calculation module 305, and a slope calculation.
  • a module 306, a parameter calculation module 307, a Bezier curve calculation module 308, and a droop pulse calculation module 309 are included.
  • the Jerk processing module 301 is a limit value (Jerk limit value) in which jerk (also referred to as “Jerk” or “jump”) generated by a control command given to the motors 41, 42, and 43 (FIG. 2) is predetermined. jMaxLim) is not exceeded. That is, the Jerk processing module 301 calculates the target acceleration AccRef and evaluates the magnitude of the jerk by comparing the calculated target acceleration AccRef and the current acceleration Acc.
  • the Jerk processing module 301 determines whether or not the calculated target jerk JerkRef exceeds a predetermined Jerk limit value jMaxLim. Then, if the calculated target jerk JerkRef exceeds the Jerk limit value jMaxLim, the Jerk processing module 301 outputs the value of the Jerk limit value jMaxLim as the target jerk JerkRef, and if not, it is calculated. The target jerk JerkRef is output as it is.
  • the acceleration processing module 302 limits the acceleration generated by the control command given to the motors 41, 42, and 43 (FIG. 2) so as not to exceed a predetermined limit value (acceleration limit value aMaxLim). That is, the acceleration processing module 302 calculates the target acceleration AccRef and evaluates the magnitude of the calculated target acceleration AccRef.
  • the target speed calculation module 303 it is not necessary to perform a restriction process on the calculated target speed VelRef. This is because the limit value of the target speed VelRef is the final target speed vEnd, and the target jerk JerkRef and the target acceleration AccRef are calculated using the final target speed vEnd.
  • the parametric variable increment calculation module 305 calculates a parametric variable increment ⁇ r corresponding to the target speed VelRef (orbital length change amount ⁇ L) calculated by the target speed calculation module 303.
  • the slope calculation module 306 typically calculates a derivative (first order) of the parameter increment ⁇ r calculated from a function of the trajectory length L corresponding to the target position P (r) which is a preset Bezier curve. Differential equation) is held in advance.
  • the Bezier curve calculation module 308 calculates the target position P (r) based on the parameter r calculated by the parameter calculation module 307. That is, the calculated target position P (r) indicates the coordinates to which the arm portion of the movable mechanism 4 should move.
  • the droop pulse calculation module 309 calculates the amount that the motors 41, 42, and 43 should move within the current control period. Then, the droop pulse calculation module 309 calculates the number of droop pulses P 1 , P 2 , and P 3 to be given to the motor drivers 31, 32, and 33, respectively.
  • FIG. 15 is a time chart showing an example of behavior of the movable mechanism 4 obtained by the control logic according to the present embodiment.
  • FIG. 15A shows an example of the temporal change in the moving speed (TCP speed) of the arm portion of the movable mechanism 4
  • FIG. 15B shows an example of the temporal change in the acceleration of the arm portion of the movable mechanism 4. Indicates.
  • FIGS. 15A and 15B show an example in which the deceleration of the TCP speed of the arm portion of the movable mechanism 4 starts at a predetermined rate (acceleration—Dec1) from time t1 as preprocessing for starting cutting. Indicates. Thereafter, at time t2, it is assumed that the TCP speed decreases to the interpolation start speed vStart.
  • This interpolation start speed vStart may be determined according to the material of the workpiece to be machined.
  • a target trajectory is generated at time t2. That is, the position of the arm portion of the movable mechanism 4 at time t2 is determined as the start point P1, and the end point P4 is determined corresponding to the start point P1. Then, the control vector 1 is determined from the start point P1 and the control point P2, and the control vector 2 is determined from the control point P3 and the end point P4. Finally, a Bezier curve, that is, a target trajectory is determined from these control vectors 1 and 2 (see FIG. 8).
  • the TCP speed is gradually changed so that the TCP speed becomes the final target speed vEnd at the end point P4.
  • the interpolation start acceleration aStart at time t2 is maintained at substantially the same magnitude as the immediately preceding acceleration ( ⁇ Dec1).
  • the target acceleration AccRef is calculated based on the remaining distance lLeft estimated in each control cycle.
  • the difference between the interpolation start speed vStart and the final target speed vEnd is large immediately after the start of movement, the amount of change in the TCP speed is restricted by the limit on jerk (Jerk limit value jMaxLim).
  • the target acceleration AccRef does not fluctuate greatly and gradually approaches a predetermined limit value (acceleration limit value aMaxLim). I understand that.
  • FIG. 16 is a flowchart showing an overall processing procedure of the control logic according to the present embodiment. 16 that are substantially the same as the steps included in the flowchart of FIG. 4 among the steps included in the flowchart of FIG. 16 are assigned the same reference numerals.
  • each step shown in FIG. 16 is executed by the control device 1, its execution subject differs depending on the implementation form. That is.
  • the main arithmetic unit 22 may perform all the processes, or the main arithmetic unit 22 and the MC unit 24 (see FIG. 2) may perform the processes jointly.
  • control device 1 receives the parameter initial value and the processing set value (step S1).
  • the initial parameter values include an interpolation start speed vStart, a final target speed vEnd, an interpolation start acceleration aStart, an acceleration limit value aMaxLim, a Jerk limit value jMaxLim, and the like.
  • the processing set value includes a machining program for generating a target trajectory, a control point setting condition, and the like. Subsequently, cutting of the workpiece 2 by the movable mechanism 4 is started (step S2).
  • step S4 it is determined whether or not it is the generation timing of the target trajectory. If it is not the generation timing of the target trajectory (NO in step S4), the process of step S4 is repeated. On the other hand, if it is the generation timing of the target trajectory (YES in step S4), the target trajectory is generated for the movement section defined by the start point and the end point (step S6).
  • the position information of the start point P1 is acquired (step S61), the position of the end point P4 corresponding to the start point P1 is determined (step S62), the control vector 1 based on the start point P1 and the control point P2, and the control A process of determining the control vector 2 based on the point P3 and the end point P4 (step S63) and determining a Bezier curve based on the control vector 1 and the control vector 2 (step S64) is included.
  • the remaining distance from the current position to the end point in the target trajectory is estimated (step S8).
  • the TCP current value (information indicating the current position) of the movable mechanism 4 is acquired (step S81), and a predetermined number of predetermined intervals are set for the section from the current position to the end point in the target trajectory.
  • Processing includes setting sample points (step S82), calculating the distance between adjacent sample points (step S83), and calculating the remaining distance by summing the distances between the sample points (step S84).
  • the target acceleration is calculated (step S10).
  • the target acceleration AccRef is calculated using the current speed Vel, the final target speed vEnd, and the estimated remaining distance lLeft (step S101), and the target jerk JerkRef is calculated using the calculated target acceleration AccRef.
  • Calculation step S102), limit processing of the calculated target jerk JerkRef (step S103), calculation of the target acceleration AccRef using the target jerk JerkRef after the limit processing (step S104), limitation of the calculated target acceleration AccRef
  • step S105 is included.
  • a new target value (target speed VelRef) of the TCP speed is calculated by correcting the current TCP speed (current speed Vel) with the target acceleration (target acceleration AccRef) calculated in step S10 (step S12). .
  • step S14 It is determined whether or not the end point of the target trajectory set by the TCP has been reached (step S14). If the end point of the target trajectory set by TCP has been reached (YES in step S14), the motion control based on the current target trajectory ends. On the other hand, if the end point of the target trajectory set by TCP has not been reached (NO in step S14), a command indicating the amount by which the movable part of the movable mechanism 4 should move within one control cycle is output ( Step S16).
  • step S163 Calculation of variable increment ⁇ r (step S163), calculation of parameter r in the current control cycle using calculated parameter increment ⁇ r (step S164), target position P (r) using calculated parameter r Calculation (step S165), calculation of the amount to be moved within the current control cycle using the calculated target position P (r) (step S166), motor driver Calculation of the number of droop pulses P 1 , P 2 , P 3 to be given to each of the inverters 31, 32, 33 (step S167), and the calculated number of droop pulses P 1 , P 2 , P 3 from the MC unit 24 A process of outputting to the motor drivers 31, 32, and 33 (step S168) is included.
  • step S18 it waits until the next control cycle (step S18), and the processing from step S8 onward is repeated.
  • step S16 the acceleration actually generated from the encoder that detects the rotational position of the servo motor provided in the motors 41, 42, 43 is calculated, and the difference from the target acceleration (target acceleration AccRef) is determined as the acceleration.
  • the acceleration error may be evaluated as an error and reflected in the processing in the next control cycle. Thereby, even when the motor does not follow the command due to overload or the like, the behavior can be corrected in a feedback manner.
  • the TCP speed is maintained at the interpolation speed Vint to the extent that the final target speed can be reached at the end point of the target trajectory under the situation limited by the acceleration limit value. That is, after the TCP speed is reduced to the interpolation speed Vint, the timing for starting the increase of the TCP speed is delayed as much as possible in order to reach the final target speed vEnd at the end point of the target trajectory.
  • FIG. 17 is a time chart showing the TCP speed of the movable mechanism 4 obtained by the control logic according to the modification of the present embodiment.
  • the current speed Vel current speed Vel1 at the time of start
  • the target acceleration AccRef is calculated according to the following equation.
  • AccRef (Vint ⁇ Vel)
  • AccRef Dec1 Therefore, after the current speed Vel decreases to the interpolation speed Vint, the target acceleration AccRef becomes zero, so the current speed Vel maintains the interpolation speed Vint.
  • the speed change start distance Lapp is calculated in advance according to the following equation.
  • Acc2 is an acceleration limit value.
  • Lapp abs ((vEnd 2 ⁇ Vel 2 ) / 2 ⁇ 1 Left) / (2 ⁇ Acc 2) Then, in each control cycle, the remaining distance lLeft is sequentially estimated, and when the estimated remaining distance lLeft substantially coincides with the speed change start distance Lapp, the TCP speed is the final target speed vEnd under the limit of the acceleration limit value Acc2. Increase to.
  • the section in which the TCP speed can be maintained at the interpolation speed Vint can be maximized, so that the quality of the work surface of the workpiece can be improved.
  • the current speed is accelerated / decelerated with as little fluctuation as possible so that the TCP speed at the end point of the target trajectory set in advance matches the final target speed. That is, the TCP speed smoothly changes toward the final target speed, and continuity with the target speed in the subsequent trajectory can be maintained. Thereby, the uniformity of a finishing surface can be improved.
  • control device 2 work, 3 major, 4 movable mechanism, 5 switch, 21 power supply unit, 22 main arithmetic unit, 23 IO unit, 24 MC unit, 25 backplane, 31, 32, 33 motor driver, 41, 42, 43 motor, 60 communication lines, 221 microprocessor, 223 buffer memory, 225 communication interface, 226 user memory, 231 processing circuit, 232, 243 input circuit, 233, 244 output circuit, 242 controller interface, 301 Jerk processing module, 302 acceleration Processing module, 303 target speed calculation module, 304 delay module, 305 parameter increment calculation module, 306 slope calculation module, 307 parameter calculation module Lumpur, 308 Bezier curve calculation module, 309 droop pulses calculating module, SYS system.

Landscapes

  • Engineering & Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)

Abstract

複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置を提供する。制御装置は、始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、目標軌道における現在位置から終点までの残距離を推定する残距離推定手段と、可動部の現在の移動速度を目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む。

Description

制御装置、制御システムおよび制御方法
　本発明は、複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置、当該制御装置を含む制御システム、および、制御方法に関する。
　従来から、さまざまな産業分野において、いわゆる産業用ロボットやＮＣ（Numerical　Control）工作機械といった自動化装置が導入されている。このような装置の導入により、生産効率を高めることができる。
　このような装置は、ワークを掴んだりするためのアームや、ワークを加工するためのエンドミルといった可動部を有している。このような可動部は、複数の駆動軸により移動可能となっており、これらの各駆動軸は、サーボモータやステッピングモータによって駆動される。
　このような装置を用いてワークに対して加工などを行なう場合には、その挙動を決定する必要がある。このような挙動の決定には、予め挙動を計算しておくオフライン計算方式と作動中に動的に計算するオンライン計算方式とが存在する。
　このような装置の挙動を制御するための従来技術としては、種々のものが提案されている。たとえば、特開平０７－０６４６２０号公報（特許文献１）に開示される数値制御装置は、加減速制御を考慮した高速かつ高精度な速度制御が行なえ、スプライン曲線上を滑らかに駆動でき、工作機械の異常振動が低減でき、速度優先と精度優先の選択を可能とする。より具体的には、特許文献１に開示された数値制御装置は、始点および終点での急な加減速を抑制することで、機械振動を抑制するとともに、目標位置を超えることなく、かつ指定した加（減）速度で減速停止できる。
　また、特開２０００－１４８２２３号公報（特許文献２）に開示される曲線補間加減速制御方法は、曲線の形状に適した速い速度で移動するなめらかな加減速度を得る。
　また、特開２０００－１６３１１４号公報（特許文献３）に開示される補間曲線内加減速処理方法は、曲線の曲率にかかわらず、指令軌道の通りに機械を動かすことができる。
　また、特開平０６－０７５６２１号公報（特許文献４）に開示されるＮＣ加工法は、曲率の大きな部分に対してのみ新たなサンプリングポイントを追加することにより、最小限のサンプリングポイント（データ）数で曲線を近似し、滑らかで形状差の少ない元の図形を再現する。
　また、特開平０１－１４７６１０号公報（特許文献５）に開示される数値制御装置は、スプライン補間制御により補間制御を行なう。
　ところで、同一の種類の製品を大量生産する生産ラインに設置される装置では、目標軌道などの設定値（加工プログラム）を予め設定しておき、この設定値に従って動作させればよい。しかしながら、さまざまな製品を生産する多種少量生産の生産ラインなどでは、このような設定値（加工プログラム）を製品毎に設定することはコスト面などから難しい。そこで、たとえば、共通の加工プログラムを利用しつつ、商品仕様などに応じて、目標軌道などを動的に設定することで、生産コストを低減したいというニーズがある。
特開平０７－０６４６２０号公報 特開２０００－１４８２２３号公報 特開２０００－１６３１１４号公報 特開平０６－０７５６２１号公報 特開平０１－１４７６１０号公報
　上述のような目標軌道を動的に設定した場合には、その軌道長を正確に算出することが難しい。そのため、目標軌道上の速度設定が難しい。
　また、ワークを切削加工するような処理では、エンドミルなどのツールの移動速度が切削量を左右するので、ツールの位置だけでなく移動速度の制御が重要である。すなわち、ツールの加速度の変動は、加工面の外観異常や品質不良につながる。そのため、移動中の加速度は可能な限り一定にしたいというニーズがある。特に、直線部分においては、加速度が変動すると不具合が発生し易い。
　さらに、ワークを切削加工するような処理では、ある区間に対する加工処理の後、続いて別の区間に対して加工処理がなされる場合がある。そのため、設定した目標軌道の終点における移動速度が所定の目標値に維持されていることが好ましい。そうでなければ、目標軌道の終点の近くで移動速度が不連続になったり、後続の直線区間において加速度が不連続になったりする。
　上述した特許文献１～５に開示される技術は、このような課題を解決するものではなかった。
　たとえば、特許文献１に開示されるような加速度を一定値に制限する方法では、算出される終点までの距離についての誤差や速度上昇中の速度変動によって、目標軌道の終点に到達するタイミングがばらつくため、終点の移動速度を所定の目標値に維持することができない。
　また、特許文献２および３に開示される技術は、予め補間曲線や目標速度を設計（オフライン計算方式）していたため、目標軌道や終点を動的に変更することが困難であった。仮に、目標軌道や終点を動的に変更できたとしても、目標速度が固定的に設計されているので、移動中に出力飽和などによって速度変動があると、その速度変動からの回復のために過剰な速度変動が発生し、加工面の外観異常や品質変動といった不具合が生じる可能性があった。
　さらに、移動速度が所定の目標値となるように目標軌道の終点へ到達させるためには、現在位置から終点までの軌道長（残距離）を制御周期毎に算出した上で、速度指令を計算する必要があり演算量が増大する。そのため、各制御周期内でこのような演算処理を完結するためには、処理能力の制約などによって容易ではなかった。
　そこで、本発明は、このような課題を解決するためになされたものであり、その目的は、目標軌道が動的に生成された場合であっても、その目標軌道の終点における速度を目標値に維持することのできる制御装置、制御システムおよび制御方法を提供することである。
　本発明のある局面によれば、複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置を提供する。制御装置は、始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、目標軌道における現在位置から終点までの残距離を推定する残距離推定手段と、可動部の現在の移動速度を、目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む。
　好ましくは、加速度算出手段は、現在の移動速度、目標速度、および、距離に基づいて加速度を算出する。
　好ましくは、加速度算出手段は、各制御周期において、現在の移動速度を目標速度まで一定レートで変化させるように、加速度を算出する。
　好ましくは、残距離推定手段は、目標軌道における現在位置から終点までの区間に対して予め定められた数のサンプル点を設定するとともに、設定したサンプル点を補間することで残距離を推定する。
　さらに好ましくは、残距離推定手段は、目標軌道における現在位置から終点までの区間について、現在位置に遠い区間に比較して現在位置に近い区間ほど、サンプル点の間隔がより密になるように、サンプル点を設定する。
　好ましくは、目標軌道は、媒介変数を含む関数の形で定義される。
　好ましくは、制御装置は、移動速度更新手段によって算出された移動速度の新たな目標値となるように、可動部が１制御周期内に移動すべき量を示す指令を出力する移動指令手段をさらに含み、移動指令手段は、移動速度の新たな目標値に対応して、可動部が移動すべき距離を算出する手段と、可動部が移動すべき距離に対応して、目標軌道における可動部の移動すべき位置を算出する手段と、可動部の移動すべき位置に対応して、複数の駆動軸の各々に対する移動指令を算出する手段とを含む。
　さらに好ましくは、目標軌道は、ベジェ曲線により定義される。
　好ましくは、目標軌道生成手段は、外部入力される信号に応答して、目標軌道を生成する。
　好ましくは、移動速度更新手段は、加速度および／または加加速度の変化量が予め定められた制限を超過しないように、移動速度の新たな目標値を算出する。
　この発明の別の局面に従う制御システムは、複数の駆動軸により移動可能な可動部を有する可動機構と、可動機構を駆動するためのドライバと、ドライバに対して制御指令を与える制御装置とを含む。制御装置は、始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、目標軌道における現在位置から終点までの残距離を推定する残距離推定手段と、可動部の現在の移動速度を、目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む。
　この発明のさらに別の局面に従えば、複数の駆動軸により移動可能な可動部を有する装置を制御するための制御方法を提供する。制御方法は、始点および終点により定義される移動区間に対して目標軌道を生成するステップと、目標軌道における現在位置から終点までの残距離を推定するステップと、可動部の現在の移動速度を、目標軌道の終点において可動部が満たすべき移動速度である目標速度まで変更した場合に、可動部が移動する距離が残距離と実質的に一致するように、当該変更に係る加速度を算出するステップと、現在の移動速度を算出した加速度で補正することで移動速度の新たな目標値を算出するステップとを含む。
　本発明によれば、目標軌道が動的に生成された場合であっても、その目標軌道の終点における速度を目標値に維持することができる。
本実施の形態に従うシステムの外観を示す模式図である。 本実施の形態に従うシステムに含まれる制御装置の外観を示す模式図である。 本実施の形態に従うシステムに含まれる制御装置のハードウェア構成を示す模式図である。 本実施の形態に従う制御ロジックの処理手順の概略を示すフローチャートである。 ワークへの切削加工処理の一例を示す図である。 ワークをＬ字状に切削する場合に円弧補間を用いて目標軌道を生成する処理例を示す図である。 ワークのコーナーを切削する場合に自由曲線を用いて目標軌道を生成する処理例を示す図である。 本実施の形態に従う目標軌道（ベジェ曲線）の生成方法について説明するための図である。 本実施の形態に従う残距離の推定方法について説明するための図である。 本実施の形態に従う目標加速度の算出方法について説明するための図である。 本実施の形態に従うベジェ曲線およびその軌道長の特性を示す図である。 ベジェ曲線における媒介変数の変位量と軌道長の変位量との関係を示す図である。 本実施の形態に従う制御ロジックにおける軌道長の変化量に対する媒介変数増分の算出方法について説明するための図である。 本実施の形態に従う制御ロジックを実現するための機能ブロックを示す模式図である。 本実施の形態に従う制御ロジックによって得られる可動機構の挙動例を示すタイムチャートである。 本実施の形態に従う制御ロジックの全体処理手順を示すフローチャートである。 本実施の形態の変形例に従う制御ロジックによって得られる可動機構のＴＣＰ速度を示すタイムチャートである。
　本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰返さない。
　＜Ａ．システム構成＞
　まず、本実施の形態に従うシステムＳＹＳの全体構成について説明する。以下の説明では、本実施の形態に従うシステムＳＹＳの適用例として、ワークを切削加工する生産ラインについて説明する。このような切削加工の典型例としては、木材加工、ガラス加工、金属加工などが挙げられる。当然のことながら、本発明は、これらの実施の形態に記載された分野に限定されるものではなく、各種の産業分野へ適用可能である。
　図１は、本実施の形態に従うシステムＳＹＳの外観を示す模式図である。図２は、本実施の形態に従うシステムＳＹＳに含まれる制御装置１の外観を示す模式図である。図３は、本実施の形態に従うシステムＳＹＳに含まれる制御装置１のハードウェア構成を示す模式図である。
　図１を参照して、本実施の形態に従うシステムＳＹＳは、制御装置１と、切削対象となる木材や金属などのワーク２の寸法などを示すメジャー３と、制御装置１からの指令によって駆動されるロボット（可動機構）４と、オペレータなどの操作に応答して軌道変更などのイベントを出力するスイッチ５とを含む。制御装置１については、図２および図３を参照して後に詳述する。
　ワーク２は、ベース上に置載されるとともに、オペレータなどの操作によって、それぞれ所望の形状に切断される。典型的には、このワーク２に沿って、メジャー３が設けられており、可動機構４がある軌道に沿って切削を行い、目的の位置まで到達すると、オペレータがスイッチ５を操作する。これにより、後述するような制御ロジックに従って、軌道が変更される。
　可動機構４は、複数の駆動軸により移動可能な可動部（アーム部分）を有する装置であり、このアーム部分の先端には、ワーク２を切削するためのツール（典型的には、エンドミル）が装着されている。このアーム部分は、後述するように、それぞれにモータが機械的に連結された複数の駆動軸（典型的には、３軸または６軸）に従って移動する。
　制御装置１は、このエンドミルの位置を、後述するような制御ロジックに従って制御することで、ワーク２を目的の大きさや形状を有する部品に加工する。なお、このエンドミルの位置を、以下では「ＴＣＰ（Tool　Center　Point）」とも称する。また、可動機構４のアーム部分の移動速度（エンドミルの移動速度）を「ＴＣＰ速度」とも称する。
　本実施の形態に従う制御装置１は、オペレータによるスイッチ５に対する操作などに応答して、目標軌道を動的に決定／変更する。たとえば、同一のワーク２に対して、品種によって異なる長さを有する略長方形の部品を製作することを考えると、加工を開始する部分（すなわち、直線部分の長さ）は異なるが、加工する内容（すなわち、加工面の曲率など）は同じとなることも多い。このような場合には、同一の加工プログラム（加工面における目標軌道などの情報が定義されている）を利用して、その絶対的な目標軌道を品種に応じて動的に決定することができる。このような目標軌道の動的な決定は、オペレータがメジャー３（および、その目盛り上に設けられた矢印）と可動機構４との位置関係を確認して、スイッチ５を操作することで実行される。
　図２を参照して、制御装置１は、典型的には、いわゆるＰＬＣ（Programmable　Logic　Controller）をベースとして構成されている。具体的には、制御装置１は、ＰＬＣ２０と、モータドライバ３１，３２，３３とを含む。
　ＰＬＣ２０は、後述する本実施の形態に従う制御ロジックを実行する処理主体である。なお、制御ロジックは、一般的にモーションプログラムと称されるものであり、可動機構４を駆動するためのモータを制御するプログラムである。このようなモーションプログラムのシーケンスは、ラダー図（ラダーダイアグラム）言語などを用いてシーケンスプログラムの形で記述される場合もある。さらに、このようなシーケンスプログラムに加えて、各種の算術演算を行なうためのプログラム（ファンクション）が実行される。
　より具体的には、ＰＬＣ２０は、電源ユニット２１と、主演算ユニット２２と、ＩＯ（Input　Output）ユニット２３と、ＭＣ（Motion　Control）ユニット２４とを含む。電源ユニット２１、主演算ユニット２２、ＩＯユニット２３、および、ＭＣユニット２４は、バックプレーン２５を介して互いに電気的に接続される。
　電源ユニット２１は、各ユニットを駆動するための直流電力や、ＩＯ用の直流電力を発生する。この電源ユニット２１で発生した電力は、バックプレーン２５を介して各ユニットに配分される。
　主演算ユニット２２は、シーケンスプログラムを実行する。
　ＩＯユニット２３は、制御対象の装置などに設置されたセンサなどからの検出信号を受け取るとともに、制御対象の装置に設置されたアクチュエータや表示装置などに対して制御指令を出力する。
　ＭＣユニット２４は、主演算ユニット２２からの指令に従って、モーションプログラムを実行し、その結果算出された制御指令をモータドライバ３１，３２，３３へ出力する。ＭＣユニット２４は、時間の経過に従ってモータドライバ３１，３２，３３へ一連の制御指令を出力する。ＭＣユニット２４は、モーションプログラムの実行状況および実行結果を主演算ユニット２２へ報告する。
　なお、図２には、主演算ユニット２２とは独立したＭＣユニット２４を設けた構成を例示するが、主演算ユニット２２がＭＣユニット２４の機能を兼ね備えることで、専用のＭＣユニット２４を省略する構成を採用することもできる。
　図３を参照して、ＰＬＣ２０のハードウェア構成について説明する。主演算ユニット２２内には、マイクロプロセッサ（ＭＰＵ：Micro-Processing　Unit）２２１と、システムプログラムメモリ（ＳＲＯＭ：System-program　Read　Only　Memory）２２２と、バッファメモリ（ＢＲＯＭ：Buffer　Read　Only　Memory）２２３と、ワークメモリ（ＷＲＡＭ：Working　Random　Access　Memory）２２４と、通信インターフェイス（通信Ｉ／Ｆ）２２５と、ユーザメモリ（ＵＭ：User　Memory）２２６と、ＩＯメモリ（ＩＯＭ：Input　Output　Memory）２２８とを含む。
　マイクロプロセッサ（ＭＰＵ）２２１は、ユニット全体を統括制御する。より具体的には、マイクロプロセッサ２２１は、ユーザメモリ２２６から順次ユーザプログラムを構成する各命令語を読み出しては実行する。システムプログラムメモリ（ＳＲＯＭ）２２２は、マイクロプロセッサ２２１で実行されるプログラムを格納する。バッファメモリ（ＢＲＯＭ）２２３は、ユーザプログラムを格納する。ワークメモリ（ＷＲＡＭ）２２４は、マイクロプロセッサ２２１がシステムプログラムを実行する際のワークエリアとして使用される。通信インターフェイス２２５は、後述するように、ＰＣ（パーソナルコンピュータ）１０とのデータ通信を行なう。ユーザメモリ（ＵＭ：User　Memory）２２６は、電源投入と共にバッファメモリ２２３から転送されるユーザプログラムを格納する。ＩＯメモリ（ＩＯＭ）２２８は、ＩＯユニット２３やＭＣユニット２４の入出力状態を反映するように保持する。
　なお、マイクロプロセッサ（ＭＰＵ）に代えて、あるいは、それに加えて、特定用途向集積回路（ＡＳＩＣ：Application　Specific　Integrated　Circuit）を採用してもよい。
　ＩＯユニット２３内には、処理回路（ＭＰＵ）２３１と、入力回路２３２と、出力回路２３３とを含む。入力回路２３２は、可動機構４に設けられた各種センサなどからの検出信号を受け取って、その状態値を主演算ユニット２２へ出力する。典型的には、入力回路２３２は、フォトカプラ回路などで構成される。出力回路２３３は、制御対象の装置などに設置されたアクチュエータや表示装置などに対して制御指令を出力する。処理回路２３１は、入力回路２３２および出力回路２３３との間で内部的な指令を遣り取りする。
　ＭＣユニット２４内には、処理回路（ＭＰＵ）２４１と、コントローラインターフェイス（コントローラＩ／Ｆ）２４２と、入力回路２４３と、出力回路２４４とを含む。コントローラインターフェイス２４２は、モータドライバ３１，３２，３３（図２参照）との間のデータ伝送を管理する。入力回路２４３および出力回路２４４は、それぞれ入力回路２３２および出力回路２３３と同様である。但し、入力回路２４３および出力回路２４４は、主として、モーション制御（モータドライバ３１，３２，３３およびモータ４１，４２，４３）に関する情報を遣り取りする。
　再度、図２を参照して、モータドライバ３１，３２，３３は、ＭＣユニット２４からの制御指令を受けて、対応するモータ４１，４２，４３を駆動する。すなわち、モータドライバ３１，３２，３３は、モータ４１，４２，４３に駆動電力を供給する。
　モータ４１，４２，４３は、可動機構４（図１参照）の構成要素である。可動機構４は、これらのモータが回転駆動することによって、そのアーム部分を移動させる。なお、モータ４１，４２，４３としては、サーボモータやステッピングモータなどが採用される。この場合、採用するモータの種類に応じたモータドライバが用いられる。
　図２には、モータドライバ３１，３２，３３は、イーサキャット（EtherCAT）（登録商標）などの通信線６０を介してＭＣユニット２４と接続される構成例を示す。但し、ＭＣユニット２４とモータドライバ３１，３２，３３とを直接的に接続してもよい。
　ＭＣユニット２４からモータドライバ３１，３２，３３へ与えられる制御指令は、モータの位置、速度、トルクなどの値である。本実施の形態においては、モータドライバ３１，３２，３３の各々に対する位置指令（目標位置）がＭＣユニット２４から一定周期で与えられるものとする。この目標位置としては、モータ４１，４２，４３の回転位置（回転角度）である場合であってもよいし、モータ４１，４２，４３によって駆動される可動機構４の特定部位の位置であってもよい。すなわち、いずれの座標を基準として目標位置を定めるかの問題であって、状況に応じて、適切な基準が採用される。
　なお、本実施の形態においては、目標位置として、いわゆる「溜りパルス」が用いられる。すなわち、目標位置が「制御周期内に送られるパルスの数」として表現される。この場合、パルス数は、直前の制御周期における位置からの相対位置（位置の偏差量／変化量）を示す。
　モータドライバ３１，３２，３３は、位置指令（目標位置）を解釈して、モータ４１，４２，４３を駆動するために必要な設定情報（たとえば、位置指令を電流値に変換するための係数など）を有している。たとえば、モータドライバ３１，３２，３３には、モータ４１，４２，４３の１回転に対応するパルス数の情報が予め設定される。
　モータドライバ３１，３２，３３は、モータ４１，４２，４３に設けられたサーボモータの回転位置を検出するエンコーダから、現在位置（実測値）のフィードバックを受ける。モータドライバ３１，３２，３３は、実測値および実測値の変化分として算出される実測速度を用いて、実測値が指令値に追従するように駆動電力の大きさを制御する（フィードバック制御）。
　また、ＭＣユニット２４には、モータドライバ３１，３２，３３と同様にモータ４１，４２，４３の１回転に対応するパルス数の情報が予め設定されるほか、モータ４１，４２，４３の１回転に対応する可動機構４の特定部位の位置についての移動量の情報が予め設定される。これらの設定値を用いて、ＭＣユニット２４は、制御周期ごとにプログラムされた制御対象の運動を実現するのに必要な数のパルスを算出してモータドライバ３１，３２，３３に送る。
　一方、ＰＣ１０は、汎用的なコンピュータをベースとして構成され、ＵＳＢ（Universal　Serial　Bus）などの通信線を介して主演算ユニット２２に接続される。ＰＣ１０からは、本実施の形態に従う制御ロジックを実現するためのプログラムが主演算ユニット２２へ転送されるとともに、ＰＣ１０のディスプレイ上では、ＰＬＣ２０の動作状況やログなどを確認することができる。なお、ＰＣ１０の構成について、公知であるので、ここでは詳細な説明は行なわない。
　なお、図２には、ＰＬＣベースの制御装置１を図示したが、この構成に限られることはない。たとえば、汎用的なコンピュータベースの処理装置と、モータドライバ３１，３２，３３やセンサなどとの間で信号を遣り取りする入出力装置とを組み合わせて、制御装置１を構成してもよい。
　このような構成を採用した場合には、本実施の形態に従う制御ロジックを実現するために、ＯＳ（Operating　System）が提供する汎用モジュールを利用する場合もある。すなわち、本実施の形態に従う制御ロジックを提供するためのプログラムとして、一部の共通的な機能を有さないような形態も存在するが、そのような形態であっても、本発明の範囲に含まれることは明らかである。
　さらに、制御装置１が提供する機能を、ソフトウェアと専用ハードウェア回路との組み合わせとして具現化してもよい。
　＜Ｂ．概要＞
　本発明の実施の形態に従う制御ロジックでは、予め設定された目標軌道の終点におけるＴＣＰ速度が最終目標速度と一致するように、現在速度をなるべく変動の少ない加速度で加減速させる。すなわち、ＴＣＰ速度が最終目標速度に向けて滑らかに変化するとともに、後続の軌道における目標速度との連続性も維持できる。これにより、仕上げ面の均一性を向上させる。
　このような現在速度を最終目標速度へ変化させるための目標加速度は、各制御周期において、対象の目標軌道上における現在位置から終点までの残距離を推定するとともに、当該推定した残距離に基づいて最適な値が算出される。このように、各制御周期において、目標軌道における終点を考慮しながら、目標加速度が逐次算出されるので、目標軌道の終点において速度や加速度が急激に増加／減少するといった事態を解消することができる。
　本実施の形態に従う制御ロジックにおいては、現在速度および目標速度を線形補間または多項式補間した速度軌道に基づいて算出する。特に、区間を分割するためのサンプル数を残距離にかかわらず一定数にするので、計算時間が確定的となり、オンライン計算方式により適した構成となる。これにより、目標軌道としてベジェ曲線といった自由関数を用いた場合であっても、上述のような残距離の推定における処理時間を１制御周期内で完了することを保証できる。
　本実施の形態に従う制御ロジックにおいては、ＴＣＰ速度の変更に係る、加速度および加加速度の変化量がそれぞれ予め定められた制限値を超過しないように制約される。そのため、可動機構４における「びびり」やトルクオーバーの発生を抑制できる。
　＜Ｃ．全体処理手順＞
　次に、本実施の形態に従う制御ロジックの概略について説明する。
　図４は、本実施の形態に従う制御ロジックの処理手順の概略を示すフローチャートである。なお、図４に示す各ステップは制御装置１によって実行されるが、その実行主体は実装形態によって異なる。すなわち。主演算ユニット２２（図２参照）がすべての処理を行なう場合もあるし、主演算ユニット２２とＭＣユニット２４（図２参照）とが共同して処理を行なう場合もある。
　図４を参照して、まず、システムＳＹＳでの制御が開始される。このとき、可動機構４によるワーク２の切削が開始される（図１参照）（ステップＳ２）。続いて、目標軌道の生成タイミングであるか否かが判断される（ステップＳ４）。すなわち、可動機構４がワーク２の所定の位置まで到達した場合に当該位置に応じた目標軌道が生成されることになるが、この目標軌道を生成すべきか否かが判断される。より具体的には、図１に示すようなスイッチ５から信号（検出信号）が外部入力されたか否かが判断される。目標軌道の生成タイミングでなければ（ステップＳ４においてＮＯ）、ステップＳ４の処理が繰返される。
　これに対して、目標軌道の生成タイミングであれば（ステップＳ４においてＹＥＳ）、始点および終点により定義される移動区間に対して目標軌道が生成される（ステップＳ６）。すなわち、外部入力される信号に応答して目標軌道が生成される。なお、始点および終点は、予め設定されている情報に基づいて決定される。言い換えれば、ワーク２の現実の位置に関連付けて始点および終点が設定されてもよいし、ワーク２の現実の位置とは独立して始点および終点が設定されてもよい。そして、以下に示すような、可動機構４を目標軌道に沿って移動させるための処理が開始される。
　まず、目標軌道における現在位置から終点までの残距離が推定される（ステップＳ８）。すなわち、可動機構４のＴＣＰの現在値（現在位置を示す情報）が取得されるとともに、決定された目標軌道（を示す関数）を用いて、目標軌道の終点までの距離が推定される。
　続いて、可動機構４の現在のＴＣＰ速度（可動部の移動速度）を目標軌道の終点において可動機構４の可動部が満たすべき移動速度である最終目標速度まで変更した場合に、可動機構４の可動部が移動する距離がステップＳ８において推定された残距離と実質的に一致するように、当該目標加速度が算出される（ステップＳ１０）。さらに、現在のＴＣＰ速度をステップＳ１０において算出した目標加速度で補正することでＴＣＰ速度の新たな目標値が算出される（ステップＳ１２）。
　ここで、ＴＣＰが設定した目標軌道の終点に到達しているか否かが判断される（ステップＳ１４）。すなわち、目標軌道に沿ったモーション制御が終了したか否かが判断される。ＴＣＰが設定した目標軌道の終点に到達していれば（ステップＳ１４においてＹＥＳ）、今回の目標軌道に基づくモーション制御は終了する。なお、同一のワーク２に対して複数の目標軌道を設定するようにプログラムされている場合には、ステップＳ２以下の処理が再度実行される。
　これに対して、ＴＣＰが設定した目標軌道の終点に到達していなければ（ステップＳ１４においてＮＯ）、ステップＳ１２において算出されたＴＣＰ速度の新たな目標値となるように、可動機構４の可動部が１制御周期内に移動すべき量を示す指令が出力される（ステップＳ１６）。すなわち、次の制御周期の開始タイミングにおいて、ＴＣＰが新たな目標値に対応した位置に移動しているように、所定数のパルス（溜りパルス）がそれぞれのモータドライバ３１，３２，３３へ与えられる。そして、次の制御周期まで待機し（ステップＳ１８）、ステップＳ８以下の処理が繰返される。
　以下、図４に示す主要ステップの内容について詳述する。
　＜Ｄ．目標軌道の生成（ステップＳ６）＞
　（ｄ１：従来技術における課題など）
　まず、本実施の形態に従う目標軌道の生成処理について説明する前に、従来技術における課題などについて説明する。
　図５は、ワーク２への切削加工処理の一例を示す図である。図５に示すように、たとえば、一辺が同様の形状に切削される複数種類の製品（品種Ａおよび品種Ｂ）を製作する場合を考える。図５に示す品種Ａと品種Ｂとの間では、切削の形状は同一であるが、その切削を開始する位置が異なっている。この場合、ワーク２は長手方向に搬送されるのであるから、この切削の開始タイミングを異ならせるだけで、品種Ａおよび品種Ｂを製作することができる。この場合、切削される部分について見れば、同じ軌道で切削すればよいことになる。
　このようなアプリケーションにおいては、各品種について加工図面や加工プログラムを作成することなく、共通の加工プログラムを用意するとともに、ワーク２とロボットとの間の相対位置関係を検出して、目標軌道を動的に（すなわち、オンラインで）設定したいというニーズがある。このような加工プログラムを共用することにより、生産コストを低減できる。
　まず、このようなオンライン計算方式を利用する場合には、制御周期（サンプリング間隔）内に新たな目標軌道の生成処理を完了して、それまで使用していた目標軌道を変更する必要がある。すなわち、新たな目標軌道の設定が一制御周期内に完了することを保証する。そのため、目標軌道としては、複雑なもの、あるいは、対象の形状に依存して処理量が変化するものを採用することは現実的ではない。
　そこで、従来からある一つのアプローチとしては、円弧補間を用いる方法が存在していた。
　図６は、ワーク２をＬ字状に切削する場合に円弧補間を用いて目標軌道を生成する処理例を示す図である。
　図６（ａ）には、円弧補間が適切に設定された例を示す。一方、図６（ｂ）および図６（ｃ）には、円弧補間が不適切に設定された例を示す。
　図６（ｂ）は、ワーク２の切削形状を維持するために、曲率が不適切に設定された結果、軌道上での方向ベクトルが不連続になっている例を示す。図６（ｃ）は、軌道上での方向ベクトルの連続性を維持するために、一定の曲率を有する円弧を用いて無理に方向ベクトルを連続化し、その結果、目的の切削形状を実現できずにワーク２の加工面への食い込みが発生した例を示す。
　また、始点の速度ベクトルおよび終点の速度ベクトルをそれぞれ連続にしたいというニーズも存在する。速度ベクトルが不連続になると、面のつなぎ目が顕在化したり、削り量の変化が発生したりしてしまい、加工後の仕上がり（外観）の品質が低下してしまうからである。しかしながら、円弧補間を用いた場合には、始点ベクトルと終点ベクトルとが同一面内にはない場合に、軌道上での速度ベクトルが不連続になるという本質的な課題もある。
　また、円弧補間においては、面取りのカーブの曲率設計が必要となるが、目標形状に含まれるすべてのコーナーについて、この作業をすることはコスト的および時間的に難しい。そのため、円弧の大まかな曲率半径だけを設定しておき、この予め設定した曲率半径では対処できないコーナーについては、目標軌道の再設計を自動化したいというニーズも存在する。しかしながら、従来の円弧補間では、このようなニーズに対応することができなかった。
　従来からある別のアプローチとしては、自由曲線を用いる方法が存在していた。
　図７は、ワーク２のコーナーを切削する場合に自由曲線を用いて目標軌道を生成する処理例を示す図である。図７（ａ１）は、従来技術に従って生成された目標軌道に沿って移動する可動機構４のＴＣＰ速度の例を示し、図７（ａ２）は、従来技術に従って生成された目標軌道の例を示す。
　図７（ａ１）に示すように、終点の速度ベクトルが不連続になると、端面の切削量が急激に変化して仕上がり（外観）にムラが生じる。すなわち、終点において可動機構４が急加速して切削面の荒れや膨らみが生じ得る。これにより、外観を損なうほか、隙間があくなどの機能上の問題が発生する。また、可動機構４の急加速によって、方向ベクトルも不連続化して、その切削面に不連続な線が生じてしまう。
　図７（ｂ１）は、本実施の形態に従う制御ロジックによって制御される可動機構４のＴＣＰ速度の例を示し、図７（ｂ２）は、本実施の形態に従う制御ロジックによって生成された目標軌道の例を示す。
　本実施の形態においては、図７（ｂ１）に示すように、後続の軌道速度との連続性を保つために、目標軌道の終点におけるＴＣＰ速度が所定の最終目標速度と一致するように制御する。同時に、図７（ｂ１）に示すように、目標軌道上における速度ベクトルの連続性を保つように制御する。これにより、図７（ｂ２）に示すように、所望の目標軌道を生成することができる。
　本実施の形態に従う制御ロジックでは、主として、自由曲線を用いて目標軌道を生成する。
　しかしながら、上述したような円弧補間では、目標軌道上のＴＣＰ速度を目標速度に一致させることができるが、それ以外の自由曲線による補間では、目標軌道上のＴＣＰ速度（特に終点速度）を目標速度に一致させることができていない。また、自由曲線を用いて目標軌道を生成する場合の課題としては、制御周期内での演算処理の完了が保証されない可能性がある。すなわち、目標軌道の終点における速度ベクトルを目標値に維持するためには、オンラインで残距離（残軌道長）を算出することが必要となる。しかしながら、自由曲線（円弧補間を除く）では、目標軌道の軌道長を初等関数で表現することができないため、目標軌道の軌道長さを短時間で算出することは難しい。また、数値積分を用いれば残距離を計算することもできるが、正確な残距離を算出するために、曲率に応じて分割数を変更しつつ積分処理を行なうと、一制御周期内での残距離の算出処理の完了を保証できない。
　（ｄ２：本実施の形態における方法）
　上述のような課題を考慮して、本実施の形態に従う制御ロジックでは、目標軌道の始点および終点におけるそれぞれの方向ベクトルの連続性を維持するために自由曲線を用いて、目標軌道を設定する。このような目標軌道は、媒介変数を含む関数の形で定義する。
　具体的には、ベジェ曲線、βスプライン曲線、ＮＵＲＢＵＳ、エルミート曲線、カットマル－ロムスプライン曲線、Ferguson/Coons曲線といった補間曲線を用いることができる。以下に説明する制御ロジックでは、これらの補間曲線のうち「ベジェ曲線」を採用した場合を例に説明する。
　このような補間曲線（ベジェ曲線）を採用することで、図６（ｃ）に示すような、目標軌道のワークへの食い込みを防止することができる。
　図８は、本実施の形態に従う目標軌道（ベジェ曲線）の生成方法について説明するための図である。図８を参照して、ベジェ曲線を用いて目標軌道を生成する場合には、少なくとも、始点および終点を含む移動区間が定義される必要がある。
　このとき、目標軌道を設定するための基準となる第１軌道および当該第１軌道に連なる軌道である第２軌道が定義される。目標軌道の始点は、第１軌道上に位置し、目標軌道の終点は、第２軌道上に位置することになる。
　第１および第２軌道は任意に設定できるが、図１に示すようなシステムＳＹＳでは、ワークの加工面が第１および第２軌道として定義される。始点Ｐ１（ｘ１，ｘ２，ｘ３）は、スイッチ５（図１参照）などでワーク２が所定位置に到達したことに応答して決定されてもよい。ベジェ曲線を設定するためには、２つのコントロールベクトルが必要であるため、始点Ｐ１の決定に伴って、コントロール点Ｐ２およびＰ３が決定されるとともに、これらの点によってコントロールベクトル１および２も決定される。
　なお、図１に示すような切削への適用例においては、コントロール点Ｐ２（ｘ２，ｙ２，ｚ２）およびＰ３（ｘ３，ｙ３，ｚ３）は予め定められていることが一般的である。そのため、始点Ｐ１が決定されると、この始点Ｐ１とコントロール点Ｐ２とを結ぶ線分として、コントロールベクトル１が決定される。また、始点Ｐ１とコントロール点Ｐ２との位置関係などに対応して、終点Ｐ４（ｘ４，ｙ４，ｚ４）が決定される。すると、コントロール点Ｐ３と終点Ｐ４とを結ぶ線分として、コントロールベクトル２が決定される。
　このようにコントロールベクトル１および２が決定されることで、ベジェ曲線、すなわち、目標軌道が決定される。
　より具体的には、媒介変数ｒ（０≦ｒ≦１）を用いて、目標軌道上の目標位置Ｐ（ｒ）は、以下のように表すことができる。なお、一般的には、目標位置Ｐ（ｒ）は、３次元座標上の値として定義される。
　Ｐ（ｒ）＝（１－ｒ）３×Ｐ１＋３（１－ｒ）２×ｒ×Ｐ２＋３（１－ｒ）×ｒ２×Ｐ３＋ｒ３×Ｐ４
　なお、ベジェ曲線以外の補間曲線を用いる場合には、第１および第２軌道とは独立した位置にコントロールベクトルを設定してもよい。また、第１軌道と第２軌道とは必ずしも直交する必要はない。
　さらに、始点Ｐ１、コントロール点Ｐ２おおびＰ３、終点Ｐ４についても任意に設定することができる。すなわち、これらの点をセンサによる検出に応じて定めてもよいし、予め固定された値を設定しておいてもよい。
　このような、ベジエ曲線を用いて目標軌道を設定することで、図８に示すように、２つのコントロールベクトルの間に目標軌道が生成されるので、木工加工などの加工面への食い込みが許されないアプリケーションでの目標軌道の生成に適している。また、目標軌道の始点および終点がコントロールベクトルの始点および終点にそれぞれ一致するので、コントロールベクトルを容易に生成できる。
　＜Ｅ．残距離の推定（ステップＳ８）＞
　次に、目標加速度を算出するための基準となる、目標軌道における現在位置から終点までの残距離の推定処理について説明する。
　後述するように、ベジェ曲線は、媒介変数により定義される高次の関数となり、媒介変数と軌道長との関係を初等関数で表現することができない。そのため、目標軌道上のある位置（現在位置）から終点までの残距離を正確に算出しようとすると、数値積分を用いる必要がある。一方で、数値積分をするための分割点を多く設定してしまうと、演算処理が増大して、算出処理が一制御周期内で完了しない可能性がある。
　そこで、本実施の形態に従う制御ロジックにおいては、目標軌道における現在位置から終点までの区間に対して予め定められた一定数のサンプル点を設定するとともに、設定したサンプル点を線形補間または多項式補間することで残距離を推定する。
　図９は、本実施の形態に従う残距離の推定方法について説明するための図である。図９（ａ）は、現在位置が目標軌道の始点に近い場合（時刻ｔ１）を示し、図９（ｂ）は、現在位置が目標軌道の終点に近い場合（時刻ｔ２）を示す。
　図９（ａ）および図９（ｂ）に示すように、残距離ｌＬｅｆｔ（ｔ）は、隣接するサンプル点Ｐ１，Ｐ２，…，Ｐｎ－１の間の距離Ｌ１，Ｌ２，…，Ｌｎの合計ΣＬｉとして算出される。なお、設定するサンプル点の数は、制御装置１の処理能力などに依存して決定すればよいが、たとえば、４～６点が好ましい。
　図９（ａ）と図９（ｂ）とを比較してわかるように、目標軌道上の現在位置が始点に近い場合であっても、終点に近い場合であっても、目標軌道上に設定するサンプル点は一定数に維持される。そのため、目標軌道上の現在位置が始点に近い場合には、サンプル点の間の距離が相対的に大きくなり、推定精度が低下するが、逆に、目標軌道上の現在位置が終点に近い場合には、サンプル点の間の距離が相対的に小さくなり、推定精度が向上する。すなわち、目標軌道の曲率が大きくなる程、残距離についての推定誤差は大きくなるが、終点に近付く程にその誤差は解消する。
　後述するように、この推定の残距離ｌＬｅｆｔ（ｔ）は、目標加速度（すなわち、目標軌道上のＴＣＰ速度の速度変化分）を決定することになるが、目標軌道上の現在位置が終点に近くなる程、その算出される目標加速度の精度が高くなる。その結果、終点に近付く程、ＴＣＰ速度が終点での最終目標速度に一致することになる。
　好ましくは、図９（ａ）および図９（ｂ）に示すように、サンプル点は、目標軌道における現在位置から終点までの区間について、現在位置に遠い区間に比較して現在位置に近い区間ほど、サンプル点の間隔がより密になるように設定される。
　上述したように、目標軌道の始点は媒介変数ｒ＝０に相当し、目標軌道の終点は媒介変数ｒ＝１に相当する。すなわち、媒介変数ｒを０から１まで増加させることで、目標軌道上の現在位置が始点から終点まで移動する。そこで、各現在位置を示す媒介変数ｒの値と目標軌道の終点に相当する媒介変数ｒ＝１との差を算出し、その差についての相対比率が所定値となる位置にポイントをそれぞれ設定する。
　たとえば、現在位置と終点との間に４つのサンプル点を設定する場合には、媒介変数ｒの差に対して、２／１２，４／１２，６／１２，９／１２に相当する媒介変数ｒの位置にそれぞれサンプル点を設定することが考えられる。具体的には、現在位置が媒介変数ｒ＝「０．４」に相当する位置であれば、終点における媒介変数ｒ＝「１．０」との差である「０．６」に対して、上述のような比率でサンプル点が設定される。この場合には、媒介変数ｒ＝「０．５」，「０．６」，「０．７」，「０．８５」にそれぞれ対応する位置に、サンプル点が設定される。
　代替の方法として、目標軌道における曲率が最大となる点を考慮して、サンプル点を設定してもよい。すなわち、目標軌道上において曲率が最大となる媒介変数ｒは、以下の式に従って算出される。
　ｒ＝［√｛（Ｐ１－Ｐ２）×Ｐ４＋Ｐ３２＋（－Ｐ２－Ｐ１）×Ｐ３＋Ｐ２２｝－Ｐ３＋２×Ｐ２－Ｐ１］／（Ｐ４－３×Ｐ３＋３×Ｐ２－Ｐ１）
　したがって、この曲率が最大となる媒介変数ｒの位置を基準として、所定数のサンプル点が設定されてもよい。
　＜Ｆ．目標加速度の算出（ステップＳ１０）＞
　次に、目標軌道上を移動中における目標加速度を算出する処理について説明する。
　本実施の形態に従う制御ロジックにおいては、目標軌道の終点におけるＴＣＰ速度の目標値が与えられているので、各現在位置から当該ＴＣＰ速度の目標値への速度変化（加速度）が線形的となるように、目標加速度が算出される。
　図１０は、本実施の形態に従う目標加速度の算出方法について説明するための図である。図１０を参照して、ある時点における目標軌道上のＴＣＰ速度を現在速度Ｖｅｌとし、目標軌道の終点におけるＴＣＰ速度の目標値を最終目標速度ｖＥｎｄとすると、時間－ＴＣＰ速度の座標において、現在速度Ｖｅｌと最終目標速度ｖＥｎｄとを直線的につなぐ加速度を目標加速度ＡｃｃＲｅｆとする。言い換えれば、各制御周期において、現在速度Ｖｅｌを最終目標速度ｖＥｎｄまで一定レートで変化させるように、目標加速度ＡｃｃＲｅｆが算出される。すなわち、目標加速度ＡｃｃＲｅｆは、以下の式に従って算出される。
　ＡｃｃＲｅｆ＝（ｖＥｎｄ－Ｖｅｌ）／Ｔｎｅｃ
　ここで、時間Ｔｎｅｃは、推定された残距離ｌＬｅｆｔだけ移動するために必要な時間であり、以下の式に従って算出される。
　Ｔｎｅｃ＝２×ｌＬｅｆｔ／（Ｖｅｌ＋ｖＥｎｄ）
　したがって、これらの２式を組み合わせると、目標加速度ＡｃｃＲｅｆは、以下の式に従って算出される。
　ＡｃｃＲｅｆ＝（ｖＥｎｄ２－Ｖｅｌ２）／（２×ｌＬｅｆｔ）
　すなわち、現在速度Ｖｅｌ、最終目標速度ｖＥｎｄ、および推定された残距離ｌＬｅｆｔに基づいて目標加速度ＡｃｃＲｅｆが算出される。
　＜Ｇ．ＴＣＰ速度の新たな目標値の算出（ステップＳ１２）＞
　次に、各制御周期において、更新されるＴＣＰ速度の目標値（目標速度）を算出する処理について説明する。
　すなわち、各制御周期において算出された目標加速度ＡｃｃＲｅｆで、現在のＴＣＰ速度（現在速度Ｖｅｌ）を補正することで、次の制御周期におけるＴＣＰ速度の目標値（目標速度ＶｅｌＲｅｆ）が算出される。すなわち、目標速度ＶｅｌＲｅｆは、以下の式に従って算出される。
　ＶｅｌＲｅｆ＝Ｖｅｌ＋ＡｃｃＲｅｆ
　＜Ｈ．移動指令の出力（ステップＳ１６）＞
　次に、目標速度ＶｅｌＲｅｆを実現するために必要な移動すべき量を示す指令を出力する処理について説明する。ここで、目標速度ＶｅｌＲｅｆは、可動機構４のアーム部分についての１制御周期内での軌道長の変化量ΔＬ（＝移動距離／制御周期）に相当する。
　図１１は、本実施の形態に従うベジェ曲線およびその軌道長の特性を示す図である。図１１（ａ）に示すようなベジェ曲線を目標軌道に設定した場合には、その目標軌道上の位置が媒介変数ｒによって定義されることになる。一方で、図１１（ｂ）に示すように、目標軌道の軌道長Ｌは、媒介変数ｒに関して非線形の関係を有している。
　図１２は、ベジェ曲線における媒介変数ｒの変位量と軌道長Ｌの変位量との関係を示す図である。図１２に示すように、ある制御周期内において、可動機構４のアーム部分を目標軌道に沿って変化量ΔＬだけ移動させた場合の媒介変数増分Δｒは、ｒの値に依存して変化することになる。すなわち、媒介変数ｒと軌道長Ｌとの関係は非線形であるので、媒介変数増分Δｒを一定値にしても、ＴＣＰ速度（現在速度Ｖｅｌ）は一定値にはならない。
　したがって、各制御周期において、次の制御周期までの変化量ΔＬが算出されたとしても、目標軌道上のいずれの位置まで移動する必要があるかについては単純な計算では算出できないことになる。
　そこで、本実施の形態に従う制御ロジックにおいては、媒介変数ｒの値に依存して、媒介変数増分Δｒを逐次算出することで、目標軌道上における移動速度を制御する。より具体的には、以下に詳述するように、軌道長Ｌを媒介変数ｒについて偏微分して得られた値を用いて、非線形の関係を簡易的に線形化する。
　図１３は、本実施の形態に従う制御ロジックにおける軌道長の変化量ΔＬに対する媒介変数増分Δｒの算出方法について説明するための図である。
　基本的には、図１３（ａ）に示すように、ある媒介変数ｒにおける、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）を算出しておく。このとき、媒介変数ｒと軌道長Ｌとの関係が線形である（破線で示す特性）とすると、目標速度ＶｅｌＲｅｆに対応する軌道長の変化量ΔＬは、媒介変数増分Δｒ＿ｌｉｎｅａｒに相当する。
　一方、媒介変数ｒと軌道長Ｌとの本来の関係（非線形の関係）によれば、媒介変数増分Δｒ＿ｌｉｎｅａｒに対応する軌道長の変化量は、目標速度ＶｅｌＲｅｆと媒介変数増分Δｒ＿ｌｉｎｅａｒで算出した時の移動長の行き過ぎ量との合計に相当する。
　したがって、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）を用いて、目標速度ＶｅｌＲｅｆを実現するために必要な媒介変数増分Δｒは、媒介変数増分Δｒ＿ｌｉｎｅａｒに軌道長Ｌの傾きａの逆数を乗じた値として算出することができる。
　すなわち、媒介変数増分Δｒ＝媒介変数増分Δｒ＿ｌｉｎｅａｒ×１／ａとして算出される。
　なお、軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）は、軌道長Ｌについての媒介変数増分Δｒについての導関数（１次微分式）を予め保持しておき、この導関数を逐次算出することで得られる。
　さらに、上述のような算出処理によって得られた媒介変数増分Δｒに基づいて、次の制御周期における目標軌道上の目標位置を示す媒介変数ｒが更新される（ｒ（ｔ＋１）＝ｒ（ｔ）＋Δｒ（ｔ））。可動機構４のアーム部分がこの更新後の媒介変数ｒによって示される目標軌道上の位置（目標位置Ｐ（ｒ（ｔ＋１）））に移動するように位置指令（目標位置）が与えられる。すなわち、新たに算出された位置に可動機構４のアーム部分を移動させるための溜りパルスがＭＣユニット２４からモータドライバ３１，３２，３３へ与えられる。
　以上のように、可動機構４に与えられる移動指令は、移動速度の新たな目標値（目標速度ＶｅｌＲｅｆ）に対応して、可動機構４のアーム部分が移動すべき距離（軌道長の変化量ΔＬ）を算出し、可動機構４のアーム部分が移動すべき距離（軌道長の変化量ΔＬ）に対応して、目標軌道における可動機構４のアーム部分が移動すべき目標軌道上の位置（目標位置Ｐ（ｒ））を算出し、可動機構４のアーム部分の移動すべき位置に対応して、複数の駆動軸の各々に対する移動指令（溜りパルス）として算出される。
　＜Ｉ．機能ブロック図＞
　次に、上述の一連の処理を実現するための機能ブロックについて説明する。
　図１４は、本実施の形態に従う制御ロジックを実現するための機能ブロックを示す模式図である。
　図１４を参照して、本実施の形態に従う制御ロジックは、Ｊｅｒｋ処理モジュール３０１と、加速度処理モジュール３０２と、目標速度算出モジュール３０３と、遅延モジュール３０４と、媒介変数増分算出モジュール３０５と、傾き算出モジュール３０６と、媒介変数算出モジュール３０７と、ベジェ曲線算出モジュール３０８と、溜りパルス算出モジュール３０９とを含む。
　Ｊｅｒｋ処理モジュール３０１は、モータ４１，４２，４３（図２）に与える制御指令により生じる加加速度（「Ｊｅｒｋ」あるいは「躍度」とも称される。）が予め定められた制限値（Ｊｅｒｋ制限値ｊＭａｘＬｉｍ）を超過しないように制限する。すなわち、Ｊｅｒｋ処理モジュール３０１は、目標加速度ＡｃｃＲｅｆを算出するとともに、この算出した目標加速度ＡｃｃＲｅｆと現在加速度Ａｃｃとを比較することで、加加速度の大きさを評価する。
　より具体的には、Ｊｅｒｋ処理モジュール３０１には、現在加速度Ａｃｃ、現在速度Ｖｅｌ、最終目標速度ｖＥｎｄ、（推定された）残距離ｌＬｅｆｔが入力される。そして、Ｊｅｒｋ処理モジュール３０１は、目標加速度ＡｃｃＲｅｆをＡｃｃＲｅｆ＝（ｖＥｎｄ２－Ｖｅｌ２）／（２×ｌＬｅｆｔ）に従って算出する。続いて、この算出した目標加速度ＡｃｃＲｅｆに対応する目標加加速度ＪｅｒｋＲｅｆをＪｅｒｋＲｅｆ＝ＡｃｃＲｅｆ－Ａｃｃに従って算出する。
　続いて、Ｊｅｒｋ処理モジュール３０１は、算出された目標加加速度ＪｅｒｋＲｅｆが予め定められたＪｅｒｋ制限値ｊＭａｘＬｉｍを超えているか否かを判断する。そして、Ｊｅｒｋ処理モジュール３０１は、算出された目標加加速度ＪｅｒｋＲｅｆがＪｅｒｋ制限値ｊＭａｘＬｉｍを超えている場合には、目標加加速度ＪｅｒｋＲｅｆとしてＪｅｒｋ制限値ｊＭａｘＬｉｍの値を出力し、そうでなければ、算出された目標加加速度ＪｅｒｋＲｅｆの値をそのまま出力する。
　加速度処理モジュール３０２は、モータ４１，４２，４３（図２）に与える制御指令により生じる加速度が予め定められた制限値（加速度制限値ａＭａｘＬｉｍ）を超過しないように制限する。すなわち、加速度処理モジュール３０２は、目標加速度ＡｃｃＲｅｆを算出するとともに、この算出した目標加速度ＡｃｃＲｅｆの大きさを評価する。
　より具体的には、加速度処理モジュール３０２には、現在加速度Ａｃｃおよび目標加加速度ＪｅｒｋＲｅｆが入力される。そして、加速度処理モジュール３０２は、目標加速度ＡｃｃＲｅｆをＡｃｃＲｅｆ＝Ａｃｃ＋ＪｅｒｋＲｅｆに従って算出する。続いて、加速度処理モジュール３０２は、算出された目標加速度ＡｃｃＲｅｆが予め定められた加速度制限値ａＭａｘＬｉｍを超えているか否かを判断する。この算出した目標加速度ＡｃｃＲｅｆが加速度制限値ａＭａｘＬｉｍを超えている場合には、目標加速度ＡｃｃＲｅｆとして加速度制限値ａＭａｘＬｉｍの値を出力し、そうでなければ、算出された加速度制限値ａＭａｘＬｉｍの値をそのまま出力する。
　目標速度算出モジュール３０３は、目標速度ＶｅｌＲｅｆを算出する。より具体的には、目標速度算出モジュール３０３には、遅延モジュール３０４を通じて、直前の制御周期における目標速度ＶｅｌＲｅｆが入力されるとともに、加速度処理モジュール３０２で算出された目標加速度ＡｃｃＲｅｆが入力される。そして、目標速度算出モジュール３０３は、今回の制御周期における目標速度ＶｅｌＲｅｆ（ｔ）をＶｅｌＲｅｆ（ｔ）＝ＶｅｌＲｅｆ（ｔ－１）＋ＡｃｃＲｅｆに従って算出する。
　なお、目標速度算出モジュール３０３では、算出された目標速度ＶｅｌＲｅｆに対する制限処理を行なう必要がない。なぜならば、目標速度ＶｅｌＲｅｆの制限値は、最終目標速度ｖＥｎｄであり、この最終目標速度ｖＥｎｄを用いて目標加加速度ＪｅｒｋＲｅｆおよび目標加速度ＡｃｃＲｅｆが算出されているからである。
　媒介変数増分算出モジュール３０５は、目標速度算出モジュール３０３により算出された目標速度ＶｅｌＲｅｆ（軌道長の変化量ΔＬ）に対応する媒介変数増分Δｒを算出する。この際、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）が必要となるので、媒介変数増分算出モジュール３０５により処理に同期して、傾き算出モジュール３０６が当該時点の媒介変数ｒに応じた、軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）を算出する。媒介変数増分算出モジュール３０５における詳細な処理については、図１３を参照して説明したので、ここではその内容については繰返さない。
　傾き算出モジュール３０６は、典型的には、予め設定されているベジェ曲線である目標位置Ｐ（ｒ）に対応する軌道長Ｌの関数から算出される、媒介変数増分Δｒについての導関数（１次微分式）を予め保持している。
　媒介変数算出モジュール３０７には、媒介変数増分算出モジュール３０５において算出された媒介変数増分Δｒが入力される。そして、媒介変数算出モジュール３０７は、今回の制御周期における媒介変数ｒ（ｔ）をｒ（ｔ）＝ｒ（ｔ－１）＋ＡｃｃＲｅｆに従って算出する。
　ベジェ曲線算出モジュール３０８は、媒介変数算出モジュール３０７において算出された媒介変数ｒに基づいて、目標位置Ｐ（ｒ）を算出する。すなわち、算出された目標位置Ｐ（ｒ）は、可動機構４のアーム部分が移動すべき座標を示す。
　溜りパルス算出モジュール３０９は、ベジェ曲線算出モジュール３０８により算出された目標位置Ｐ（ｒ）に基づいて、モータ４１，４２，４３がそれぞれ現制御周期内で移動すべき量を算出する。そして、溜りパルス算出モジュール３０９は、モータドライバ３１，３２，３３のそれぞれへ与える溜りパルスＰ１，Ｐ２，Ｐ３の数を算出する。
　＜Ｊ．動作例＞
　次に、本実施の形態に従う制御ロジックによって得られる可動機構４の挙動の一例について説明する。
　図１５は、本実施の形態に従う制御ロジックによって得られる可動機構４の挙動例を示すタイムチャートである。図１５（ａ）は、可動機構４のアーム部分の移動速度（ＴＣＰ速度）の時間的変化の例を示し、図１５（ｂ）は、可動機構４のアーム部分の加速度の時間的変化の例を示す。
　なお、ワークを切削加工するような処理では、ワーク２に接近する速度に比較して切削時の速度が相対的に低く設定されている場合が多い。そのため、図１５（ａ）および図１５（ｂ）には、切削開始の前処理として、可動機構４のアーム部分のＴＣＰ速度が所定レート（加速度－Ｄｅｃ１）での減速を時刻ｔ１から開始する例を示す。その後、時刻ｔ２において、ＴＣＰ速度が補間開始速度ｖＳｔａｒｔまで低下したとする。この補間開始速度ｖＳｔａｒｔは、加工対象のワークの材質などに応じて定められてもよい。
　そして、時刻ｔ２において目標軌道が生成されたとする。すなわち、時刻ｔ２における可動機構４のアーム部分の位置が始点Ｐ１として決定されるとともに、この始点Ｐ１に対応して終点Ｐ４が決定される。すると、始点Ｐ１とコントロール点Ｐ２とからコントロールベクトル１が決定されるとともに、コントロール点Ｐ３と終点Ｐ４とからコントロールベクトル２が決定される。最終的に、これらのコントロールベクトル１および２からベジェ曲線、すなわち目標軌道が決定される（図８参照）。
　時刻ｔ２以降では、ＴＣＰ速度が終点Ｐ４において最終目標速度ｖＥｎｄとなるように、ＴＣＰ速度が徐々に変更される。なお、速度の連続性を維持するため、時刻ｔ２における補間開始加速度ａＳｔａｒｔは、直前の加速度（－Ｄｅｃ１）とほぼ同様の大きさに維持されている。
　上述したように、目標加速度ＡｃｃＲｅｆは、各制御周期において推定される残距離ｌＬｅｆｔに基づいて算出される。但し、移動開始直後では、補間開始速度ｖＳｔａｒｔと最終目標速度ｖＥｎｄとの差が大きいため、加加速度についての制限（Ｊｅｒｋ制限値ｊＭａｘＬｉｍ）によってＴＣＰ速度の変化量が制約される。
　しかしながら、時刻ｔ３以降では、ＴＣＰ速度（現在速度Ｖｅｌ）と最終目標速度ｖＥｎｄとの差が小さくなるため、各制御周期において算出される目標加速度ＡｃｃＲｅｆに従って、ＴＣＰ速度（現在速度Ｖｅｌ）が徐々に最終目標速度ｖＥｎｄに近付くことがわかる。そして、時刻ｔ５において、可動機構４のアーム部分が終点に到達すると、そのときのＴＣＰ速度（現在速度Ｖｅｌ）が最終目標速度ｖＥｎｄに一致していることがわかる。
　なお、図１５（ｂ）に示すように、時刻ｔ３～ｔ５の期間においては、目標加速度ＡｃｃＲｅｆが大きく変動することなく、徐々に予め定められた制限値（加速度制限値ａＭａｘＬｉｍ）に接近していることがわかる。
　＜Ｋ．全体処理フロー＞
　上述した本実施の形態に従う制御ロジックの全体処理手順としては、以下のようにまとめることができる。
　図１６は、本実施の形態に従う制御ロジックの全体処理手順を示すフローチャートである。なお、図１６のフローチャートに含まれるステップのうち図４のフローチャートに含まれるステップと実質的に同じものについては、同一の参照番号を付与している。
　また、図１６に示す各ステップは制御装置１によって実行されるが、その実行主体は実装形態によって異なる。すなわち。主演算ユニット２２（図２参照）がすべての処理を行なう場合もあるし、主演算ユニット２２とＭＣユニット２４（図２参照）が共同して処理を行なう場合もある。
　図１６を参照して、制御装置１は、パラメータ初期値および処理設定値を受付ける（ステップＳ１）。このパラメータ初期値には、補間開始速度ｖＳｔａｒｔ、最終目標速度ｖＥｎｄ、補間開始加速度ａＳｔａｒｔ、加速度制限値ａＭａｘＬｉｍ、Ｊｅｒｋ制限値ｊＭａｘＬｉｍなどが含まれる。また、処理設定値には、目標軌道を生成するための加工プログラムや、コントロール点の設定条件などが含まれる。続いて、可動機構４によるワーク２の切削が開始される（ステップＳ２）。
　続いて、目標軌道の生成タイミングであるか否かが判断される（ステップＳ４）。目標軌道の生成タイミングでなければ（ステップＳ４においてＮＯ）、ステップＳ４の処理が繰返される。これに対して、目標軌道の生成タイミングであれば（ステップＳ４においてＹＥＳ）、始点および終点により定義される移動区間に対して目標軌道が生成される（ステップＳ６）。
　この目標軌道の生成においては、始点Ｐ１の位置情報の取得（ステップＳ６１）、始点Ｐ１に対応する終点Ｐ４の位置決定（ステップＳ６２）、始点Ｐ１とコントロール点Ｐ２とによるコントロールベクトル１、および、コントロール点Ｐ３と終点Ｐ４とによるコントロールベクトル２の決定（ステップＳ６３）、ならびに、コントロールベクトル１およびコントロールベクトル２に基づくベジェ曲線の決定（ステップＳ６４）という処理が含まれる。
　続いて、目標軌道における現在位置から終点までの残距離が推定される（ステップＳ８）。この残距離の推定においては、可動機構４のＴＣＰの現在値（現在位置を示す情報）の取得（ステップＳ８１）、目標軌道における現在位置から終点までの区間に対して予め定められた一定数のサンプル点の設定（ステップＳ８２）、隣接するサンプル点の間の距離を算出（ステップＳ８３）、各サンプル点の間の距離を合計することで残距離の算出（ステップＳ８４）という処理が含まれる。
　続いて、目標加速度が算出される（ステップＳ１０）。この目標加速度の算出においては、現在速度Ｖｅｌ、最終目標速度ｖＥｎｄ、推定された残距離ｌＬｅｆｔを用いて目標加速度ＡｃｃＲｅｆの算出（ステップＳ１０１）、算出された目標加速度ＡｃｃＲｅｆを用いて目標加加速度ＪｅｒｋＲｅｆの算出（ステップＳ１０２）、算出された目標加加速度ＪｅｒｋＲｅｆの制限処理（ステップＳ１０３）、制限処理後の目標加加速度ＪｅｒｋＲｅｆを用いて目標加速度ＡｃｃＲｅｆの算出（ステップＳ１０４）、算出された目標加速度ＡｃｃＲｅｆの制限処理（ステップＳ１０５）という処理が含まれる。
　続いて、現在のＴＣＰ速度（現在速度Ｖｅｌ）をステップＳ１０において算出した目標加速度（目標加速度ＡｃｃＲｅｆ）で補正することでＴＣＰ速度の新たな目標値（目標速度ＶｅｌＲｅｆ）が算出される（ステップＳ１２）。
　ＴＣＰが設定した目標軌道の終点に到達しているか否かが判断される（ステップＳ１４）。ＴＣＰが設定した目標軌道の終点に到達していれば（ステップＳ１４においてＹＥＳ）、今回の目標軌道に基づくモーション制御は終了する。これに対して、ＴＣＰが設定した目標軌道の終点に到達していなければ（ステップＳ１４においてＮＯ）、可動機構４の可動部が１制御周期内に移動すべき量を示す指令が出力される（ステップＳ１６）。
　この指令の出力処理においては、媒介変数ｒと軌道長Ｌとが線形であると仮定して、算出された目標速度ＶｅｌＲｅｆに相当する軌道長の変化量ΔＬに対応する媒介変数増分Δｒ＿ｌｉｎｅａｒの算出（図１３参照）（ステップＳ１６１）、媒介変数ｒに関する軌道長Ｌの傾きａ（＝ｄＬ／ｄｒ）の算出（ステップＳ１６２）、媒介変数増分Δｒ＿ｌｉｎｅａｒに軌道長Ｌの傾きａの逆数を乗じることで媒介変数増分Δｒの算出（ステップＳ１６３）、算出された媒介変数増分Δｒを用いて今回の制御周期における媒介変数ｒの算出（ステップＳ１６４）、算出された媒介変数ｒを用いた目標位置Ｐ（ｒ）の算出（ステップＳ１６５）、算出された目標位置Ｐ（ｒ）を用いた現制御周期内で移動すべき量の算出（ステップＳ１６６）、モータドライバ３１，３２，３３のそれぞれへ与えるべき溜りパルスＰ１，Ｐ２，Ｐ３の数の算出（ステップＳ１６７）、算出された数の溜りパルスＰ１，Ｐ２，Ｐ３のＭＣユニット２４からモータドライバ３１，３２，３３への出力（ステップＳ１６８）という処理が含まれる。
　そして、次の制御周期まで待機し（ステップＳ１８）、ステップＳ８以下の処理が繰返される。
　なお、ステップＳ１６の処理において、モータ４１，４２，４３に設けられたサーボモータの回転位置を検出するエンコーダから現実に生じた加速度を算出するとともに、目標加速度（目標加速度ＡｃｃＲｅｆ）との差を加速度誤差として評価し、この加速度誤差を次の制御周期における処理に反映してもよい。これにより、過負荷などでモータが指令に追従しない場合であっても、フィードバック的にその挙動を補正することができる。
　＜Ｌ．変形例＞
　（ｌ１：目標軌道上での移動速度を一定に維持できる範囲を最大化する処理例）
　対象のアプリケーションによっては、目標軌道を移動中（典型的には、切削中）のＴＣＰ速度を可能な限り一定化した場合も存在する。このようにＴＣＰ速度を一定値に維持することで、加工面の仕上がり品質を向上させることができる。
　この変形例に従う制御ロジックでは、加速度制限値によって制限された状況下において、目標軌道の終点において最終目標速度に到達することができる範囲まで、ＴＣＰ速度を補間速度Ｖｉｎｔに維持する。すなわち、ＴＣＰ速度を補間速度Ｖｉｎｔまで低下させた後、目標軌道の終点において最終目標速度ｖＥｎｄに到達するためにＴＣＰ速度の増加を開始するタイミングを可能な限り遅くする。
　図１７は、本実施の形態の変形例に従う制御ロジックによって得られる可動機構４のＴＣＰ速度を示すタイムチャートである。図１７を参照して、まず、処理が開始されると、現在速度Ｖｅｌ（開始時の現在速度Ｖｅｌ１）を減速度制限Ｄｅｃ１の制限下で補間速度Ｖｉｎｔまで低減させる。このとき、目標加速度ＡｃｃＲｅｆは、以下のような式に従って算出される。
　ＡｃｃＲｅｆ＝（Ｖｉｎｔ－Ｖｅｌ）
　但し、ＡｃｃＲｅｆ＜Ｄｅｃ１であれば、ＡｃｃＲｅｆ＝Ｄｅｃ１
　したがって、現在速度Ｖｅｌが補間速度Ｖｉｎｔまで低下した後は、目標加速度ＡｃｃＲｅｆがゼロとなるので、現在速度Ｖｅｌは補間速度Ｖｉｎｔを維持する。
　また、速度変更開始距離Ｌａｐｐが以下のような式に従って予め算出される。但し、Ａｃｃ２は、加速度制限値である。
　Ｌａｐｐ＝ａｂｓ（（ｖＥｎｄ２－Ｖｅｌ２）／２×ｌＬｅｆｔ）／（２×Ａｃｃ２）
　そして、各制御周期において、残距離ｌＬｅｆｔが逐次推定されるとともに、この推定される残距離ｌＬｅｆｔが速度変更開始距離Ｌａｐｐと略一致すると、ＴＣＰ速度は加速度制限値Ａｃｃ２の制限下で最終目標速度ｖＥｎｄまで増加する。
　このような処理を採用することで、ＴＣＰ速度を補間速度Ｖｉｎｔに維持できる区間を最大化できるため、ワークの加工面の品質などを向上させることができる。
　＜Ｍ．作用効果＞
　本発明の実施の形態によれば、予め設定された目標軌道の終点におけるＴＣＰ速度が最終目標速度と一致するように、現在速度をなるべく変動の少ない加速度で加減速させる。すなわち、ＴＣＰ速度が最終目標速度に向けて滑らかに変化するとともに、後続の軌道における目標速度との連続性も維持できる。これにより、仕上げ面の均一性を向上させることができる。
　今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、請求の範囲によって示され、請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
　１　制御装置、２　ワーク、３　メジャー、４　可動機構、５　スイッチ、２１　電源ユニット、２２　主演算ユニット、２３　ＩＯユニット、２４　ＭＣユニット、２５　バックプレーン、３１，３２，３３　モータドライバ、４１，４２，４３　モータ、６０　通信線、２２１　マイクロプロセッサ、２２３　バッファメモリ、２２５　通信インターフェイス、２２６　ユーザメモリ、２３１　処理回路、２３２，２４３　入力回路、２３３，２４４　出力回路、２４２　コントローラインターフェイス、３０１　Ｊｅｒｋ処理モジュール、３０２　加速度処理モジュール、３０３　目標速度算出モジュール、３０４　遅延モジュール、３０５　媒介変数増分算出モジュール、３０６　傾き算出モジュール、３０７　媒介変数算出モジュール、３０８　ベジェ曲線算出モジュール、３０９　溜りパルス算出モジュール、ＳＹＳ　システム。

Claims (12)

  1. 　複数の駆動軸により移動可能な可動部を有する装置を制御するための制御装置であって、
    　始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、
    　前記目標軌道における現在位置から前記終点までの残距離を推定する残距離推定手段と、
    　前記可動部の現在の移動速度を、前記目標軌道の終点において前記可動部が満たすべき移動速度である目標速度まで変更した場合に、前記可動部が移動する距離が前記残距離と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、
    　前記現在の移動速度を前記算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを備える、制御装置。
  2. 　前記加速度算出手段は、前記現在の移動速度、前記目標速度、および、前記残距離に基づいて前記加速度を算出する、請求項１に記載の制御装置。
  3. 　前記加速度算出手段は、各制御周期において、前記現在の移動速度を前記目標速度まで一定レートで変化させるように、前記加速度を算出する、請求項１に記載の制御装置。
  4. 　前記残距離推定手段は、前記目標軌道における前記現在位置から前記終点までの区間に対して予め定められた数のサンプル点を設定するとともに、設定したサンプル点を補間することで前記残距離を推定する、請求項１～３のいずれか１項に記載の制御装置。
  5. 　前記残距離推定手段は、前記目標軌道における前記現在位置から前記終点までの区間について、前記現在位置に遠い区間に比較して前記現在位置に近い区間ほど、前記サンプル点の間隔がより密になるように、前記サンプル点を設定する、請求項４に記載の制御装置。
  6. 　前記目標軌道は、媒介変数を含む関数の形で定義される、請求項１～５のいずれか１項に記載の制御装置。
  7. 　前記移動速度更新手段によって算出された前記移動速度の新たな目標値となるように、前記可動部が１制御周期内に移動すべき量を示す指令を出力する移動指令手段をさらに備え、
    　前記移動指令手段は、
    　　前記移動速度の新たな目標値に対応して、前記可動部が移動すべき距離を算出する手段と、
    　　前記可動部が移動すべき距離に対応して、前記目標軌道における前記可動部の移動すべき位置を算出する手段と、
    　　前記可動部の移動すべき位置に対応して、前記複数の駆動軸の各々に対する移動指令を算出する手段とを含む、請求項１～４のいずれか１項に記載の制御装置。
  8. 　前記目標軌道は、ベジェ曲線により定義される、請求項６または７に記載の制御装置。
  9. 　前記目標軌道生成手段は、外部入力される信号に応答して、前記目標軌道を生成する、請求項１に記載の制御装置。
  10. 　前記移動速度更新手段は、加速度および／または加加速度の変化量が予め定められた制限を超過しないように、前記移動速度の新たな目標値を算出する、請求項１に記載の制御装置。
  11. 　制御システムであって、
    　複数の駆動軸により移動可能な可動部を有する可動機構と、
    　前記可動機構を駆動するためのドライバと、
    　前記ドライバに対して制御指令を与える制御装置とを備え、
    　前記制御装置は、
    　　始点および終点により定義される移動区間に対して目標軌道を生成する目標軌道生成手段と、
    　　前記目標軌道における現在位置から前記終点までの残距離を推定する残距離推定手段と、
    　　前記可動部の現在の移動速度を、前記目標軌道の終点において前記可動部が満たすべき移動速度である目標速度まで変更した場合に、前記可動部が移動する距離が前記残距離
    と実質的に一致するように、当該変更に係る加速度を算出する加速度算出手段と、
    　　前記現在の移動速度を前記算出した加速度で補正することで移動速度の新たな目標値を算出する移動速度更新手段とを含む、制御システム。
  12. 　複数の駆動軸により移動可能な可動部を有する装置を制御するための制御方法であって、
    　始点および終点により定義される移動区間に対して目標軌道を生成するステップと、
    　前記目標軌道における現在位置から前記終点までの残距離を推定するステップと、
    　前記可動部の現在の移動速度を、前記目標軌道の終点において前記可動部が満たすべき移動速度である目標速度まで変更した場合に、前記可動部が移動する距離が前記残距離と実質的に一致するように、当該変更に係る加速度を算出するステップと、
    　前記現在の移動速度を前記算出した加速度で補正することで移動速度の新たな目標値を算出するステップとを備える、制御方法。
PCT/JP2011/056227 2010-10-13 2011-03-16 制御装置、制御システムおよび制御方法 Ceased WO2012049866A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN201180049705.7A CN103180791B (zh) 2010-10-13 2011-03-16 控制装置、控制系统及控制方法
US13/877,930 US9606524B2 (en) 2010-10-13 2011-03-16 Control apparatus, control system and control method
EP11832309.6A EP2629163B1 (en) 2010-10-13 2011-03-16 Control device, control system and control method

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2010-230342 2010-10-13
JP2010230342A JP5146512B2 (ja) 2010-10-13 2010-10-13 制御装置、制御システムおよび制御方法

Publications (1)

Publication Number Publication Date
WO2012049866A1 true WO2012049866A1 (ja) 2012-04-19

Family

ID=45938106

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2011/056227 Ceased WO2012049866A1 (ja) 2010-10-13 2011-03-16 制御装置、制御システムおよび制御方法

Country Status (5)

Country Link
US (1) US9606524B2 (ja)
EP (1) EP2629163B1 (ja)
JP (1) JP5146512B2 (ja)
CN (1) CN103180791B (ja)
WO (1) WO2012049866A1 (ja)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020049809A1 (ja) * 2018-09-05 2020-03-12 日本電気株式会社 モーション制御装置、モーション制御方法、非一時的なコンピュータ可読媒体、及びモーション制御システム
US11008039B2 (en) * 2017-04-12 2021-05-18 Toyota Jidosha Kabushiki Kaisha Lane change assist apparatus for vehicle
CN114657688A (zh) * 2022-02-28 2022-06-24 浙江大豪科技有限公司 横织机的控制方法、装置、设备及介质
CN117555288A (zh) * 2024-01-09 2024-02-13 山东大学 基于弧长预测与迭代修调的样条曲线插补方法及系统

Families Citing this family (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6128767B2 (ja) * 2012-07-05 2017-05-17 キヤノン株式会社 ロボット制御装置、及びロボット制御方法
CN103568054B (zh) * 2013-10-29 2016-08-17 南通恒康数控机械股份有限公司 一种海绵环刀切割机切割时的软件限位控制方法
JP5908544B2 (ja) 2014-08-11 2016-04-26 ファナック株式会社 駆動軸のジャークを低下させるロボットプログラムを生成するロボットプログラム生成装置
JP6133825B2 (ja) * 2014-08-28 2017-05-24 ファナック株式会社 反転時の最適な加速度を算出する数値制御装置
CN105511337B (zh) * 2014-10-15 2018-11-09 关隆股份有限公司 无线系统的近端与远程控制协调方法
JP6321605B2 (ja) * 2015-10-30 2018-05-09 ファナック株式会社 曲率と曲率変化量による速度制御を行う数値制御装置
EP3449321A4 (en) 2016-04-27 2019-05-22 Skogsrud, Simen DEVICE AND METHOD FOR MOTION CONTROL
EP3449322A4 (en) * 2016-04-27 2019-05-22 Skogsrud, Simen METHOD OF ITERATIVE MOTION CONTROL
FR3051782B1 (fr) * 2016-05-24 2018-07-06 Fmc Technologies Sa Dispositif de commande de deplacement, procede et dispositif d'acquisition et de calcul pour celui-ci, ainsi que bras articule de chargement de fluide le comportant.
WO2018068302A1 (zh) * 2016-10-14 2018-04-19 深圳配天智能技术研究院有限公司 一种轨迹速度规划方法及轨迹速度规划装置
CN106227152A (zh) * 2016-10-17 2016-12-14 合肥工业大学 一种cnc加工的平滑过渡方法及其平滑过渡装置
CN109074067B (zh) * 2016-12-28 2021-11-05 深圳配天智能技术研究院有限公司 机器人运动控制方法及相关装置
CN107031741B (zh) * 2017-04-24 2019-06-04 北京京东尚科信息技术有限公司 车体位姿校正方法及装置
JP6881174B2 (ja) * 2017-09-13 2021-06-02 オムロン株式会社 制御装置および制御方法
JP6900863B2 (ja) * 2017-09-22 2021-07-07 オムロン株式会社 制御装置、制御方法および制御プログラム
US11554461B1 (en) 2018-02-13 2023-01-17 Omax Corporation Articulating apparatus of a waterjet system and related technology
CN108388206B (zh) * 2018-03-07 2020-11-24 深圳市汇川技术股份有限公司 进给速度实时动态规划方法及系统
TWI662781B (zh) * 2018-03-29 2019-06-11 東元電機股份有限公司 馬達控制系統及其方法
CN111722640B (zh) * 2019-03-21 2024-04-09 北京京东乾石科技有限公司 无人机的控制方法、装置和计算机可读存储介质
JP6899099B2 (ja) * 2019-04-22 2021-07-07 株式会社安川電機 機械制御システム、機械制御装置、及び制振指令生成方法
JP6863497B1 (ja) * 2020-03-11 2021-04-21 オムロン株式会社 コントローラシステムおよびその制御方法
JP6801802B1 (ja) * 2020-03-11 2020-12-16 オムロン株式会社 コントローラシステムおよびその制御方法
CN113965140B (zh) * 2020-07-17 2026-01-16 配天机器人技术有限公司 一种伺服电机控制方法、装置、设备及介质
US20220186469A1 (en) * 2020-12-14 2022-06-16 Cnh Industrial America Llc System and method for controlling implement operation of a work vehicle using a speed-based parameter
JP7276359B2 (ja) * 2021-01-07 2023-05-18 株式会社安川電機 動作指令生成装置、機構制御システム、コンピュータプログラム、動作指令生成方法及び機構制御方法
JP7739757B2 (ja) * 2021-05-20 2025-09-17 オムロン株式会社 制御システム、制御方法及びプログラム
EP4386146B1 (en) * 2021-09-29 2026-02-04 Kobelco Construction Machinery Co., Ltd. Trajectory-generating system and work machine comprising same
CN114019904B (zh) * 2021-11-09 2024-05-17 深圳市英威腾电气股份有限公司 一种运动控制方法、系统及装置
JP2023159641A (ja) * 2022-04-20 2023-11-01 セイコーエプソン株式会社 外部機器からの指令を用いてロボットの経路を生成する経路生成方法、及び、ロボット制御システム
CN115314534A (zh) * 2022-07-12 2022-11-08 埃夫特智能装备股份有限公司 一种基于EtherCAT通信协议的实时性优化模拟机器人系统
CN116690308A (zh) * 2023-07-11 2023-09-05 通用技术集团机床工程研究院有限公司 超精密机床多轴联动轨迹精度检测方法、装置及系统
CN117104749B (zh) * 2023-09-21 2025-08-29 南京音飞储存设备（集团）股份有限公司 一种环轨行驶的穿梭车控制方法及控制系统

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61156309A (ja) * 1984-12-27 1986-07-16 Toshiba Mach Co Ltd 速度段差平滑機能を備えた数値制御装置
JPH01147610A (ja) 1987-12-03 1989-06-09 Fanuc Ltd 数値制御装置
JPH0675621A (ja) 1992-08-21 1994-03-18 Nippon Sheet Glass Co Ltd Ｎｃ加工法
JPH0764620A (ja) 1993-08-31 1995-03-10 Mitsubishi Electric Corp 数値制御装置
JPH07104824A (ja) * 1993-09-29 1995-04-21 Yaskawa Electric Corp ロボット制御装置
JPH07210233A (ja) * 1994-01-27 1995-08-11 Matsushita Electric Ind Co Ltd ロボットの軌道の補間方法と補間装置
JPH11179685A (ja) * 1997-12-19 1999-07-06 Yaskawa Electric Corp 産業用ロボットの制御方法
JP2000148223A (ja) 1998-11-06 2000-05-26 Yaskawa Electric Corp 曲線補間加減速制御方法
JP2000163114A (ja) 1998-11-26 2000-06-16 Yaskawa Electric Corp 補間曲線内加減速処理方法
JP2003015714A (ja) * 2001-06-29 2003-01-17 Toshiba Tungaloy Co Ltd 数値制御工作機械
JP2006072598A (ja) * 2004-09-01 2006-03-16 Yaskawa Electric Corp サーボ制御装置およびサーボ制御方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
ES364521A1 (es) * 1968-03-12 1971-03-01 Kautex-Werk Reinold Hagen Un dispositivo para la fabricacion de cuerpos huecos de material sintetico termoplástico
US5740327A (en) * 1994-12-27 1998-04-14 Nec Corporation Method of and apparatus for robot tip trajectory control
ATE161638T1 (de) 1995-10-10 1998-01-15 Siemens Ag Taktsynchrone bewegungsführung in zeitdiskreten abtastsystemen
JPH11514774A (ja) * 1996-09-02 1999-12-14 フィリップス エレクトロニクス ネムローゼ フェンノートシャップ 設定値生成器を用いての制御
JPH11149306A (ja) * 1997-11-14 1999-06-02 Fanuc Ltd 加工機の制御装置
EP2184581A4 (en) * 2007-07-06 2012-02-29 Sony Corp INPUT DEVICE, CONTROL, CONTROL SYSTEM, CONTROL PROCEDURE AND HANDSET
US7919940B2 (en) * 2007-10-21 2011-04-05 Ge Intelligent Platforms, Inc. System and method for jerk limited trajectory planning for a path planner

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61156309A (ja) * 1984-12-27 1986-07-16 Toshiba Mach Co Ltd 速度段差平滑機能を備えた数値制御装置
JPH01147610A (ja) 1987-12-03 1989-06-09 Fanuc Ltd 数値制御装置
JPH0675621A (ja) 1992-08-21 1994-03-18 Nippon Sheet Glass Co Ltd Ｎｃ加工法
JPH0764620A (ja) 1993-08-31 1995-03-10 Mitsubishi Electric Corp 数値制御装置
JPH07104824A (ja) * 1993-09-29 1995-04-21 Yaskawa Electric Corp ロボット制御装置
JPH07210233A (ja) * 1994-01-27 1995-08-11 Matsushita Electric Ind Co Ltd ロボットの軌道の補間方法と補間装置
JPH11179685A (ja) * 1997-12-19 1999-07-06 Yaskawa Electric Corp 産業用ロボットの制御方法
JP2000148223A (ja) 1998-11-06 2000-05-26 Yaskawa Electric Corp 曲線補間加減速制御方法
JP2000163114A (ja) 1998-11-26 2000-06-16 Yaskawa Electric Corp 補間曲線内加減速処理方法
JP2003015714A (ja) * 2001-06-29 2003-01-17 Toshiba Tungaloy Co Ltd 数値制御工作機械
JP2006072598A (ja) * 2004-09-01 2006-03-16 Yaskawa Electric Corp サーボ制御装置およびサーボ制御方法

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11008039B2 (en) * 2017-04-12 2021-05-18 Toyota Jidosha Kabushiki Kaisha Lane change assist apparatus for vehicle
WO2020049809A1 (ja) * 2018-09-05 2020-03-12 日本電気株式会社 モーション制御装置、モーション制御方法、非一時的なコンピュータ可読媒体、及びモーション制御システム
JPWO2020049809A1 (ja) * 2018-09-05 2021-08-26 日本電気株式会社 モーション制御装置、モーション制御方法、モーション制御プログラム、及びモーション制御システム
JP7078120B2 (ja) 2018-09-05 2022-05-31 日本電気株式会社 モーション制御装置、モーション制御方法、モーション制御プログラム、及びモーション制御システム
CN114657688A (zh) * 2022-02-28 2022-06-24 浙江大豪科技有限公司 横织机的控制方法、装置、设备及介质
CN114657688B (zh) * 2022-02-28 2023-09-29 浙江大豪科技有限公司 横织机的控制方法、装置、设备及介质
CN117555288A (zh) * 2024-01-09 2024-02-13 山东大学 基于弧长预测与迭代修调的样条曲线插补方法及系统
CN117555288B (zh) * 2024-01-09 2024-04-05 山东大学 基于弧长预测与迭代修调的样条曲线插补方法及系统

Also Published As

Publication number Publication date
JP5146512B2 (ja) 2013-02-20
EP2629163B1 (en) 2019-12-04
US20130197688A1 (en) 2013-08-01
JP2012083982A (ja) 2012-04-26
CN103180791B (zh) 2017-03-01
EP2629163A4 (en) 2015-05-20
EP2629163A1 (en) 2013-08-21
US9606524B2 (en) 2017-03-28
CN103180791A (zh) 2013-06-26

Similar Documents

Publication Publication Date Title
JP5146512B2 (ja) 制御装置、制御システムおよび制御方法
CN109725600B (zh) 后处理器装置、加工程序生成方法、cnc加工系统及计算机可读信息记录介质
CN104204977B (zh) 轨迹控制装置
KR100284668B1 (ko) 수치제어장치
CN109834714B (zh) 一种用于多机器人的轨迹控制系统与方法
CN121057984A (zh) 控制装置以及计算机可读取的记录介质
CN109954955B (zh) 机器人系统
US20190086898A1 (en) Software module, precision machine, method and component
WO1989006066A1 (fr) Procede de commande de vitesse pour servomoteur
JPH0378006A (ja) 数値制御装置の加減速制御方法
US20150378343A1 (en) Numerical controller having tool tip point control function
JPH06274228A (ja) 数値制御装置
EP3304228B1 (en) Robot system for synchronizing the movement of the robot arm
WO2022138843A1 (ja) 数値制御装置
JP3004651B2 (ja) 数値制御装置
JPH0354610A (ja) インボリュート補間誤差補正方式
CN117548845A (zh) 龙门从轴控制方法、装置、设备及存储介质
CN109884982B (zh) 数值控制装置
WO2022264338A1 (ja) 制御装置、干渉チェック装置、及び制御システム
JPH11194813A (ja) 産業用機械の動作指令作成方法
CN112114558A (zh) 数值控制装置、cnc机床、数值控制方法以及记录有数值控制用程序的计算机可读介质
Qitao et al. Polishing Machine Control System Based on B-Spline Curve Trajectory
JP7594180B2 (ja) 工作機械
CN119645137B (zh) 一种多轴连续跟踪动态主轴同步到达的控制方法及装置
CN102033514A (zh) 一种基于高速现场总线的多轴运动控制器

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 11832309

Country of ref document: EP

Kind code of ref document: A1

DPE1 Request for preliminary examination filed after expiration of 19th month from priority date (pct application filed from 20040101)
WWE Wipo information: entry into national phase

Ref document number: 13877930

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 2011832309

Country of ref document: EP