Trajectory control device

• US 9,098,077 B2
• Filed: 07/28/2011
• Issued: 08/04/2015
• Est. Priority Date: 08/25/2010
• Status: Active Grant

• Current Assignee: Mitsubishi Electric Corporation
• Original Assignee: Mitsubishi Electric Corporation
• Inventors: Nagaoka, Kotaro
• Primary Examiner(s): Hartman, Jr., Ronald
• Application Number:
  US13/814,758
• Publication Number:
  US 20130138236A1
• Time in Patent Office: 1,468 Days
• Field of Search: 700186-190, 318/560, 318/567
• US Class Current: 1/1
• CPC Class Codes: G05B 13/02 electric
• CPC Class Codes: G05B 19/19 characterised by positioning or contouring control systems, e.g. to control position from one programmed point to another or to control movement along a programmed continuous path
• CPC Class Codes: G05B 19/404 characterised by control arrangements for compensation, e.g. for backlash, overshoot, tool offset, tool wear, temperature, machine construction errors, load, inertia G05B19/19, G05B19/41 take precedence
• CPC Class Codes: G05B 19/4097 characterised by using design data to control NC machines, e.g. CAD/CAM G05B19/4093 takes precedence
• CPC Class Codes: G05B 19/41 characterised by interpolation, e.g. the computation of intermediate points between programmed end points to define the path to be followed and the rate of travel along that path G05B19/25, G05B19/31, G05B19/37, G05B19/39, G05B19/40 take precedence
• CPC Class Codes: G05B 19/4103 Digital interpolation
• CPC Class Codes: G05B 19/416 characterised by control of velocity, acceleration or deceleration G05B19/19 takes precedence
• CPC Class Codes: G05B 2219/34135 Spline
• CPC Class Codes: G05B 2219/42342 Path, trajectory tracking control
• CPC Class Codes: G11B 19/28 Speed controlling, regulating, or indicating G11B19/24 takes precedence

Abstract

A trajectory control device controlling a trajectory of a movable portion includes a servo-system response-trajectory calculation unit that computes a servo-system response trajectory based on a position command of each movable axis, a shape-feature determination unit that outputs a shape feature amount including information of a position of a boundary point in a path shape and a running direction near the boundary point based on a determination from the position command whether the shape of the commanded path is straight or curved line, a position-vector correction unit that corrects a position vector based on the position command, the servo-system response trajectory and the shape feature amount, and outputs a corrected position command, and servo control units that control a motor of each movable axis by outputting a motor drive torque so that a position of each movable axis follows the corrected position command.

Claims

1. A trajectory control device in which a movable portion of a machine is driven by a plurality of movable axes, and a trajectory of the movable portion is controlled by simultaneously controlling motors of the movable axes, the trajectory control device comprising:

a servo-system response-trajectory calculation unit that computes a servo-system response trajectory based on a position command of each movable axis;

a shape-feature determination unit that determines whether a shape of a commanded path is a straight line or a curved line based on the position command and outputs, as a feature amount of the shape of the commanded path, a shape feature amount including a boundary point position of the shape of the commanded path and a running direction of the shape near the boundary point;

a position-vector correction unit that corrects a position command by performing vector computations based on the position command, the servo-system response trajectory and the shape feature amount, and outputs a corrected position command; and

a plurality of servo control units that control a motor of each movable axis by outputting a motor drive torque so that a position of each movable axis follows the corrected position command.

2. The trajectory control device according to claim 1, wherein the position-vector correction unit designates a position obtained by adding to the position command a correction vector obtained by multiplying a difference between the servo-system response trajectory and the position command by a correction gain, as a temporarily corrected position, and computes a corrected position vector based on a positional relation among the position command, the temporarily corrected position and the boundary point position included in the shape feature amount to output each axial component of the position vector as the corrected position command.

3. The trajectory control device according to claim 2, wherein

the shape-feature determination unit extracts a portion in which a trajectory of the position command changes from a straight line to a curved line, and outputs a position of a boundary point thereof and a running direction immediately before the boundary point as the shape feature amount, and

the position-vector correction unit designates a position at which the temporarily corrected position is shifted onto a reference straight line determined by the running direction immediately before the boundary point as the corrected position command, when the position command and the temporarily corrected position become opposite to each other with respect to the reference straight line near the boundary point, so as to compute the corrected position vector so that a position command before correction and a corrected position command do not have a positional relation with the commands being opposite to each other with respect to the reference straight line.

4. The trajectory control device according to claim 2, wherein

the shape-feature determination unit extracts a portion in which a trajectory of the position command changes from a curved line to a straight line, and outputs a position of a boundary point thereof and a running direction immediately after the boundary point as the shape feature amount, and

the position-vector correction unit designates a position at which the temporarily corrected position is shifted onto a reference straight line determined by the running direction immediately after the boundary point as the corrected position command, when the position command and the temporarily corrected position become opposite to each other with respect to the reference straight line near the boundary point, so as to compute the corrected position vector so that a position command before correction and a corrected position command do not have a positional relation with the commands being opposite to each other with respect to the reference straight line.

5. The trajectory control device according to claim 2, wherein

the shape-feature determination unit extracts a portion in which a direction of a normal vector of a trajectory of the position command is inverted, and outputs a position of a boundary point thereof and a running direction at the boundary point as the shape feature amount, and

the position-vector correction unit designates a position at which the temporarily corrected position is shifted onto a reference straight line determined by a running direction at the boundary point as the corrected position command, when the position command and the temporarily corrected position become opposite to each other with respect to the reference straight line near the boundary point, so as to compute the corrected position vector so that a position command before correction and a corrected position command do not have a positional relation with the commands being opposite to each other with respect to the reference straight line.

6. The trajectory control device according to claim 1, wherein

the servo control unit includes a two-degree-of-freedom controller using a reference model unit, and

the servo-system response-trajectory calculation unit outputs an output trajectory at the time of giving the position command to a model same as the reference model unit used in the servo control unit, as the servo-system response trajectory.

7. The trajectory control device according to claim 1, wherein the servo-system response-trajectory calculation unit outputs a trajectory obtained by subtracting a vector obtained by multiplying an acceleration vector of the position command at each time by a predetermined coefficient determined by responsiveness of the servo control unit from the trajectory of the position command, as the servo-system response trajectory.