KR20220076698A - Method, apparatus and system for controlling motion between multiple pcs - Google Patents

Abstract

The present invention relates to a method, apparatus and system for controlling motion between multiple workpiece coordinate systems. A motion control method according to an embodiment of the present invention is a method performed in a system including a motion controller for motion control between a plurality of product coordinate systems (PCS), and includes setting a driving route including a plurality of points. a first setting step; a second setting step of setting, for the workpiece coordinate system of each point of the driving path, a work coordinate system based on a machine coordinate system (MCS) of a machine located at the last point of the driving path as a reference; and a driving step of performing a motion operation according to a set work coordinate system and a driving path.

Description

Method, device and system for motion control between multiple workpiece coordinate systems

The present invention relates to a method, apparatus and system for motion control, and more particularly, to a method for controlling motion between a plurality of product coordinate systems (PCS) such as pick-and-place. , devices and systems.

In general, industrial controllers such as motion controllers are not limited to a specific maker or product and are designed to follow the PLCopen standard for mutual compatibility. That is, the PLCopen standard defines function blocks of various commands, and in particular, has standards for motion control and safety related technologies in addition to the I/O sequence control of the existing PLC. In this case, the motion control-related functions include single-axis and multi-axis control technology, and CAM and synchronize control technology for group operation are implemented together in the multi-axis control technology.

In this PLCopen standard, the synchronous control technology of multi-axis control is divided into a technology for 'shortened vs. shortened', a technology for 'single-to-multi-axis', and a technology for 'multi-axis versus multiple axes'. At this time, in addition to the machine coordinate system (MCS), the product coordinate system (PCS) (also referred to as “user coordinate system”) is set, and the linear/non-linear profile (trajectory) is tracked while the coordinate system is tracking. , is implemented to enable synchronization control.

1 shows an example of motion control according to the state of an object. That is, FIG. 1A shows an example of motion control when the object is in a stationary state, and FIG. 1B shows an example of motion control when the object is in a moving state.

That is, in the case of Figure 1 (a), the object, the workpiece is in a stationary state, and when the robot B holds the workpiece, the robot A performs the spot welding operation according to the position and posture of the workpiece. In addition, in the case of Figure 1 (b), the object cookie is in a moving state, and the orthogonal robot A performs the task of putting chocolate on the cookie moving on the conveyor belt B.

1(a) and (b), in the case where B becomes the master motion and A follows the motion, and the axis-group synchronization between MCS and PCS can be matched 1:1 corresponds to

In this case, “axis-group” refers to a group including multiple axes, in which the coordinate system for all other axes (slave axes) can be expressed through the expression of the coordinate system for one axis (master axis). In other words, axis-group means that each slave axis has a specific relationship with respect to the master axis (that is, a relationship in which the position of the slave axis is determined linearly or non-linearly depending on the position of the master axis), and the profile for this specific relationship is already known. is a group

In general, in applications utilizing robots (orthogonal robots, scara robots, vertical articulated robots, etc.), the robots are configured in axis-groups to enable synchronous control for one axis or multiple axes. However, unlike the simple case of FIG. 1 (that is, when 1:1 synchronous control is possible), multiple PCSs for different mechanical devices such as pick-and-place (hereinafter referred to as “PnP”) In the case of a complex situation in which tracking of the , it is difficult to operate the motion control only with the function block of the conventional PLCopen standard. This is because, in the case of PLCopen standard, only function blocks for simple cases where 1:1 synchronous control is possible are defined.

In order to solve the problems of the prior art as described above, the present invention is a complex type of operation situation that cannot be responded to with commands according to the existing PLCopen standard, such as Pick-and-Place (PnP) between multiple PCS coordinate systems. The purpose of this is to provide a technology capable of motion control.

That is, an object of the present invention is to provide a technology that enables PnP operation of a robot between a plurality of different PCS coordinate systems in which tracking is in progress on a specific axis or axis-group.

Another object of the present invention is to provide a technology capable of synchronizing an event input in an external environment and signaling an event during PnP operation.

Another object of the present invention is to provide a technique for configuring a library-type command (User defined function block) by utilizing the command defined in the PLCOpen standard as a basic building block.

However, the problems to be solved by the present invention are not limited to the problems mentioned above, and other problems not mentioned can be clearly understood by those of ordinary skill in the art to which the present invention belongs from the description below. There will be.

A motion control method according to an embodiment of the present invention for solving the above problems is a method performed in a system including a motion controller for motion control between a plurality of product coordinate systems (PCS), and a plurality of points A first setting step of setting a driving route including; a second setting step of setting, for the workpiece coordinate system of each point of the driving path, a work coordinate system based on a machine coordinate system (MCS) of a machine located at the last point of the driving path as a reference; and a driving step of performing a motion operation according to a set work coordinate system and a driving path.

In the driving path, the starting point and the ending point may be points in different workpiece coordinate systems according to different mechanical devices.

The second setting step may include reflecting tracking according to the movement of the mechanical device with respect to the workpiece coordinate system of the respective points.

The driving path may be an arch-shaped path for driving a pick and place (PnP).

The arcuate path includes a first motion path of upward movement of height h _up , a third motion path of downward movement of height h _down , and a second motion path of l width unidirectional movement between the first and third motions. two motion paths, a first curved motion path between the first and second motion paths, and a second curved motion path between the second and third motion paths.

the first curved motion path has a path of specific curvature for a distance d ₁ from h _up and l, respectively, and the second curved motion path has a specific curvature of a specific curvature for a distance d ₂ from h _down and l, respectively. has a path, and d1 and d1 can be obtained using the following formula.

(However, min(a, b) represents the smaller (minimum) length value among a and b, and ArchBlendRatio represents the ratio value to the minimum length value)

The first setting step may include setting a plurality of driving paths and indexes of input/output events for each driving path.

The second setting step may include setting a work coordinate system for each point of the driving path matching the index of the input/output event.

The driving may include driving a driving route matching an index of an input/output event.

The second setting step and the driving step may be sequentially and repeatedly performed for each of the set driving routes.

The motion control method according to an embodiment of the present invention is applicable when there is no command of the PLCopen standard for tracking a plurality of PCS coordinate systems, but the second setting step and the operation step include a command according to the PLCopen standard. It can be performed using

The command for performing the first setting step may receive an output setting value of an end-effector driver of the robot and a correction value of a sensor required for each driving path.

The command for performing the driving step may include an input for updating to compensate for the movement/rotation amount of the tracking object and an input for overriding a speed set for the current driving path, respectively.

A motion controller according to an embodiment of the present invention is a controller for motion control between a plurality of product coordinate systems (PCS). a path setting unit for setting a driving path that is a point of the work coordinate system; a coordinate system setting unit that sets the workpiece coordinate system of each point of the driving path to a work coordinate system based on a machine coordinate system (MCS) of a machine located at the last point of the driving path; and a driving control unit configured to operate a motion according to a set work coordinate system and a driving path.

The coordinate system setting unit may reflect tracking according to the movement of the mechanical device when setting the work coordinate system of the respective points.

A motion control system according to an embodiment of the present invention is a system for motion control between a plurality of product coordinate systems (PCS). A driver for driving a motion device, wherein the motion controller sets a driving path that includes a plurality of points, but a starting point and an ending point are points in different workpiece coordinate systems according to different mechanical devices, each of the driving paths Set the work coordinate system based on the machine coordinate system (MCS) of the machine located at the last point of the driving path with respect to the work coordinate system of the points, and control the operation of motion according to the set work coordinate system and driving path do.

The present invention configured as described above has the advantage of enabling motion control for complex types of driving situations that cannot be responded to with commands according to the existing PLCopen standard, such as Pick-and-Place (PnP) between multiple PCS coordinate systems. .

That is, the present invention has the advantage of enabling PnP operation of the robot between a plurality of different PCS coordinate systems in which tracking is in progress on a specific axis or axis-group.

In addition, the present invention has an advantage in that it is possible to synchronize an event input of an external environment during PnP operation and to signal an event.

In addition, the present invention has the advantage that it is possible to implement a library-type command (User defined function block) by utilizing the command defined in the PLCOpen standard as a basic building block.

In addition, in the present invention, when performing motion between different PCS coordinate system points according to different mechanical devices, the driving route can be updated based on the PCS coordinate system of the mechanical device to which the last point belongs, so memory is small and cost is low. There is an advantage.

The effects obtainable in the present invention are not limited to the above-mentioned effects, and other effects not mentioned may be clearly understood by those of ordinary skill in the art to which the present invention belongs from the following description. will be.

1 shows an example of motion control according to the state of an object.
2 is a block diagram showing a motion control system 10 according to an embodiment of the present invention.
3 shows the types of coordinate systems defined in the PLCopen standard.
4 shows an example of a profile and a command of the axis-to-axis synchronization of a master and a slave according to the PLCopen standard.
5 shows an example of commands and parameters for setting a work coordinate system (PCS) according to the PLCopen standard.
6 shows an example of an axis-group to axis-group tracking and a command thereof.
7 shows an example of a situation in which Pick-and-Place (PnP) is required between two PCSs being tracked for a specific axis.
8 shows the definition of an arc-shaped motion for Pick and Place.
9 shows the configuration of a state machine.
10 shows an interface configuration of a driving route teaching command.
11 shows an interface configuration of a route driving command.
12 shows another expanded example of a general form of tracking between an axis-group versus an axis-group.
13 shows another example of a conversion command from MCS to PCS.

The above object and means of the present invention and its effects will become more apparent through the following detailed description in relation to the accompanying drawings, and accordingly, those of ordinary skill in the art to which the present invention pertains can easily understand the technical idea of the present invention. will be able to carry out In addition, in describing the present invention, if it is determined that a detailed description of a known technology related to the present invention may unnecessarily obscure the gist of the present invention, the detailed description thereof will be omitted.

The terminology used herein is for the purpose of describing the embodiments, and is not intended to limit the present invention. In the present specification, the singular form also includes the plural form as the case may be, unless otherwise specified in the phrase. In this specification, terms such as “include”, “provide”, “provide” or “have” do not exclude the presence or addition of one or more other components other than the mentioned components.

In this specification, terms such as “or” and “at least one” may indicate one of the words listed together, or a combination of two or more. For example, “or B” and “at least one of B” may include only one of A or B, or both A and B.

In the present specification, descriptions according to “for example” and the like may not exactly match the information presented, such as recited properties, variables, or values, tolerances, measurement errors, limits of measurement accuracy, and other commonly known factors The embodiments of the present invention according to various embodiments of the present invention should not be limited by effects such as modifications including .

In this specification, when it is described that a certain element is 'connected' or 'connected' to another element, it may be directly connected or connected to the other element, but other elements may exist in between. It should be understood that there may be On the other hand, when it is mentioned that a certain element is 'directly connected' or 'directly connected' to another element, it should be understood that the other element does not exist in the middle.

In this specification, when it is described that a certain element is 'on' or 'in contact with' another element, it may be directly in contact with or connected to the other element, but another element may exist in the middle. It should be understood that On the other hand, when it is described that a certain element is 'directly on' or 'directly' of another element, it may be understood that another element does not exist in the middle. Other expressions describing the relationship between the elements, for example, 'between' and 'directly between', etc. may be interpreted similarly.

In this specification, terms such as 'first' and 'second' may be used to describe various components, but the components should not be limited by the above terms. In addition, the above terms should not be construed as limiting the order of each component, and may be used for the purpose of distinguishing one component from another. For example, a 'first component' may be referred to as a 'second component', and similarly, a 'second component' may also be referred to as a 'first component'.

Unless otherwise defined, all terms used herein may be used with meanings commonly understood by those of ordinary skill in the art to which the present invention pertains. In addition, terms defined in a commonly used dictionary are not to be interpreted ideally or excessively unless specifically defined explicitly.

Hereinafter, a preferred embodiment according to the present invention will be described in detail with reference to the accompanying drawings.

2 is a block diagram showing a motion control system 10 according to an embodiment of the present invention.

The motion control system 10 according to an embodiment of the present invention is a system for controlling a plurality of motion devices (or actuators) such as a servo motor, a step motor, and the like, and as shown in FIG. 2 , an external device 100 , a motion controller 200 , a driver 300 , and a motion device (not shown).

The external device 100 is a device that manages all control operations of the motion control system 10 . That is, a user or an administrator may access the motion controller 200 through the external device 100 to set and manage control of the operation of each drive 300 . For example, the external device 100 may transmit a control command such as a driving command or a tuning command to the motion controller 200 .

The external device 100 may be an electronic device such as a computing capable terminal or a programmable logic controller (PLC). For example, the terminal includes a desktop personal computer (PC), a laptop personal computer (PC), a tablet personal computer (PC), a netbook computer, a smart phone, and a smart pad. , or a personal digital assistant (PDA), etc., but is not limited thereto.

However, in addition to the external device 100 , a control command may be transmitted to the motion controller 200 through a user interface (not shown) such as a manipulation panel.

The motion controller 200 is connected to each drive 300 and controls the operation of each drive 300 according to a control command received from the external device 100 or a user interface. In this case, the motion controller 200 may control the operation of each drive 300 according to the PLCopen standard. For example, the motion controller 200 may drive each drive 300 based on data such as pre-stored driving profile information and tuning parameter information.

The drive 300 is a device for driving a motion device according to a control signal received from the motion controller 200 . The drive 300 is provided with a plurality of (300-1, ...300-n) (where n is a natural number greater than or equal to 2), and each drive 300 drives each motion device individually. For example, the drive 300 may be a step motor driver or a servo motor driver, but is not limited thereto.

The plurality of drives 300 may be connected to the motion controller 200 in various topologies. For example, the topology may be a bus topology, a star topology, a ring topology, a tree topology, or a mesh topology, but is not limited thereto. not. Also, among these drives 300 , a drive that is a master and a drive that is a slave may be included.

Meanwhile, the motion control system 10 according to an embodiment of the present invention may include a plurality of mechanical devices such as a robot, a conveyor, and a rotary table. In this case, at least one motion device may be provided in one mechanical device. Also, the position of the object may vary according to the driving of the motion device.

Meanwhile, as shown in FIG. 2 , the motion controller 200 may include a path setting unit 210 , a coordinate system setting unit 220 , and a driving control unit 230 , and these components are hardware processors. ) or may be included in a process that is software executed by the processor, but is not limited thereto. Also, the route setting unit 210 , the coordinate system setting unit 220 , and the driving control unit 230 may be included in the configuration of the external device 100 .

3 shows the types of coordinate systems defined in the PLCopen standard.

Referring to FIG. 3 , the types of coordinate systems defined in the PLCopen standard include an axis coordinate system (ACS), a machine coordinate system (MCS), and a product coordinate system (PCS). At this time, it is possible to obtain MCS from ACS and homogeneously convert to PCS.

4 shows an example of a profile and a command of the axis-to-axis synchronization of a master and a slave according to the PLCopen standard. That is, FIG. 4(a) shows an example of a profile of a master axis versus a slave axis, and FIG. 4(b) shows an example of a function block of a synchronization command for a master axis versus a slave axis.

Referring to FIG. 4 , for axis-to-axis synchronization, MC_CamIn defined as a synchronization command between axes may be used.

5 shows an example of commands and parameters for setting a work coordinate system (PCS) according to the PLCopen standard. That is, FIG. 5(a) shows an example of a function block of a PCS setting command, and FIG. 5(b) is an example of translation parameters, that is, an example of coordinate system transformation from MCS to PCS by translation and rotation. , and FIG. 5( c ) shows an example of rotation parameters.

Meanwhile, referring to FIG. 5 , the following process may be performed for PCS setting. That is, if A and B are configured as axis-groups, the MCS coordinate system of the A axis-group is expressed as MCS _A , the PCS coordinate system is expressed as PCS _A , the MCS coordinate system of the B axis-group is MCS _B , and the PCS coordinate system is PCS It can be expressed as _B. These coordinate systems can be transformed using Equations (1) to (3) below.

(1)

(One)

(2)

(2)

(3)

(3)

At this time, Equation (1) represents a homogeneous transformation matrix composed of the PCS coordinate system origin (P _PCSORG ) and the rotation matrix (R) for the P point of the MCS coordinate system, and Equation (2) is the MCS coordinate system and the PCS coordinate system. Represents the inverse matrix between In addition, Equation (3) represents the mapping of the A-axis-group PCS coordinate system and the B-axis-group PCS coordinate system to the coordinate system transformation between the A-axis-group and B-axis-group.

)은 펑션 블록으로 정의된 MC_SetDynCoordTransform의 명령어를 이용할 수 있다. 또한, 축-그룹 B 내에서 좌표계 변환(

)은 펑션 블록으로 정의된 MC_SetCoordinateTransform의 명령어를 이용할 수 있다.For example, a coordinate system transformation between axes-groups A and B (ie between axes-groups) (

) can use the command of MC_SetDynCoordTransform defined as a function block. Also, the coordinate system transformation within axis-group B (

) can use the command of MC_SetCoordinateTransform defined as a function block.

6 shows an example of an axis-group to axis-group tracking and a command thereof. That is, FIG. 6(a) shows tracking for a conveyor belt, and FIG. 6(b) shows a function block of commands for tracking the conveyor belt. In addition, that is, FIG. 6(c) shows tracking for a rotary table, and FIG. 6(d) shows a function block of commands for tracking the rotary table.

Axes-group versus axis-to-group tracking commands can be defined for synchronization between axis-groups. In this case, the synchronized motion trajectory can be viewed as a superposition of two independent motions. For the implementation of this axis-group versus axis-group tracking command, a method of applying multidimensional gear operation (linear superposition by coordinates) (the first implementation method) and applying a coordinate system transformation matrix (that is, a function block according to PLCOpen standard) There may be a method (second implementation method) of adopting .

6( a ) shows a case in which the robot tracks an object placed on a conveyor belt. In this case, a function block parameterized exclusively for the conveyor belt can be defined as a command of MC_TrackConveyorBelt, as shown in FIG. configurable. Specifically, based on the MCS coordinate system of the robot, the amount of movement of the conveyor belt (TransMCS_ _CB ) and the amount of rotation of the conveyor belt (RotMCS_ _CB ) can be obtained. In addition, based on the MCS coordinate system of the conveyor belt, the movement amount (TransCB_ _PCS ) of the PCS coordinate system and the rotation amount (RotCB_ _PCS ) of the PCS coordinate system can be obtained. In addition, based on the PCS coordinate system of the conveyor belt, the position of the object (P _PCS ) can be obtained.

On the other hand, Figure 6 (b) shows a case in which the robot is placed on a rotary table to track a rotating object. In this case, the function block parameterized exclusively for the rotary table can be defined as the command of MC_TrackRotaryTable, as shown in FIG. The interface is configurable. Specifically, the movement amount of the rotary table and the rotation amount of the rotary table can be obtained based on the MCS coordinate system of the robot. In addition, based on the MCS coordinate system of the rotary table, the movement amount and rotation amount of the PCS coordinate system can be obtained. In addition, the position of the object can be obtained based on the PCS coordinate system of the rotary table.

7 shows an example of a situation in which Pick-and-Place (PnP) is required between two PCSs being tracked for a specific axis. That is, FIG. 7 shows equipment for performing PnP operation using a robot between different pallets that are rotating/moving on a rotary table and a conveyor belt, respectively. The case of FIG. 7 corresponds to a complex case having a plurality of PCS, and corresponds to a case in which implementation is difficult only with motion control commands defined in the conventional PLCOpen standard.

Specifically, the operation scenario for the case of FIG. 7 is as follows.

< Operation Scenario >

- Pallets are placed on the conveyor belt at random positions and angles and move along the conveyor belt.

- A pallet is attached to the rotary table and rotates together with the rotary table.

- A sensor (rotary encoder) is attached to the conveyor to know the conveying amount of the conveyor belt from the reference position.

- A sensor (encoder) is attached to the rotary table to know the rotation amount of the rotary table from the reference position.

- The conveyor has a sensor that can detect the presence of pallets, and when the sensor detects, it is possible to measure the coordinates of the origin of the pallet at the time of detection and the misaligned angle of the origin through machine vision.

- Pallets of conveyor belts and rotary tables are grooved at regular intervals, and the grooves contain objects (eg, iron marbles) (hereinafter, it is assumed that the objects are iron marbles).

- Delta robot is equipped with an electromagnet on a gripper, and the electromagnet is used to pick up iron beads from a moving pallet on a conveyor belt in turn, and transfer the iron beads to a rotating pallet on a rotary table.

- The rotary table rotates in synchronization with the movement of the conveyor belt.

- After moving all the iron beads from the rotary table, on the contrary, the iron beads must be moved from the rotary table to the empty pallet on the conveyor belt again in turn.

In addition, in the operation scenario according to the case of FIG. 7 , the mechanical limiting conditions are as follows.

< Mechanical Restriction Conditions >

- For the machine origin of the delta robot, both the conveyor belt and the rotary table have their machine origins at different positions on the XY plane. For example, a conveyor belt is skewed by 20 degrees and a rotary table by 45 degrees with respect to the origin of a delta robot machine along the Z axis (with instrument interference at other positions or angles)

- Machine vision is aligned based on a specific position on the conveyor (1st eye: the camera is attached to the conveyor, 2nd eye: attached to the robot's gripper if the conveyor interferes too much)

- The rotary table must move or contain all the iron beads on the pallet passing the conveyor belt within a certain angle range (other angles interfere with the mechanism on the rotary table side)

- When the conveying speed of the conveyor belt increases, the speed of the synchronous section of the rotary table must also increase (assuming logistics collision avoidance)

On the other hand, a method for satisfying both the above-described operation scenario and the mechanical constraint condition is as follows.

<Method for Satisfying Operational Scenarios and Organizational Limitations>

- Synchronous rotation of the rotary table according to the speed change of the conveying axis of the conveyor:

Synchronization operation for the rotary table rotation axis (slave) is possible according to the CAM profile set according to the conveyor axis (master) by using the command (MC_CamIn) related to synchronization between axes

- Tracking of moving pallets on a conveyor belt:

After setting the PCS coordinate system for the pallet placed on the conveyor using the command (MC_SetCoordinateTransform) related to the coordinate system transformation within the axis-group, the robot moves the conveyor movement axis by using the command (MC_TrackConveyorBelt) to track the object placed on the conveyor belt. can be tracked for

- Tracking of rotating pallets on a rotary table:

After setting the PCS coordinate system for the pallet placed on the rotary table using the command (MC_SetCoordinateTransform) related to the coordinate system transformation within the axis-group, the robot is placed on the rotary table to track the rotating object by using the command ( MC_TrackRotaryTable). Can be tracked on the axis of rotation of the table

- Pick and Place (PnP) between the pallet of the conveyor belt and the pallet of the rotary table: According to the existing PLCOpen standard, there is no separate command for this PnP (that is, when problems of the prior art occur)

In other words, in relation to PnP between pallets on conveyor belts and pallets on rotary tables, simultaneous operation of two different PCS coordinate systems in one robot MCS is not separately defined in the PLCOpen standard. That is, according to the current PLCOpen standard, only one PCS coordinate system can be tracked as a target even between multiple MCS coordinate systems.

Accordingly, the following will present a technology that enables PnP operation of a robot between a plurality of different PCS coordinate systems in which tracking is in progress on a specific axis or axis-group. In addition, it is intended to present a technology capable of synchronizing an event input in an external environment and signaling an event during PnP operation. In addition, we would like to present a technique for constructing a library-type command (User defined function block) by using the commands defined in the PLCOpen standard as basic building blocks.

8 shows a definition of an arc-shaped motion for Pick and Place, and FIG. 9 shows a configuration of a state machine. Also, FIG. 10 shows an interface configuration of a driving route teaching command, and FIG. 11 shows an interface configuration of a route driving command.

The control according to FIGS. 8 to 11 may be performed through the external device 100 or the motion controller 200 . For example, the route setting according to FIG. 8 and control according to the driving route teaching command of FIG. 10 may be performed by the route setting unit 210, and the control according to S101 and S102 of FIG. 9 may be performed by the coordinate system setting unit 220. may be performed, and the control according to the route driving command of FIG. 11 and the control according to S103 to S105 of FIG. 9 may be performed by the driving controller 230 .

The driving path teaching command of FIG. 10 is a command for setting a driving path, and details of the input/output are shown in Table 1 below.

Input/Output LS_TRobotPathCfg PathCfg Store all configuration data in the function block Input BOOL Enable Input data is continuously updated at a high level LS_TRobotCoordCfg CoordCfg Coordinate system configuration data UINT PathType Point-to-Point movement: Adjacent to the path at intermittent speed (1: LinearTo, 2: ArchTo, 3: CircularEnd)
Point-via-Point movement:
Path without acceleration/deceleration velocity profile (4: LinearBy, 5: CircularMid) LREAL[0..2] MovePosXYZ Position of target [X, Y, Z] for path LREAL[0..2] MovePosABC Position of target [A, B, C] for path LREAL Move Velocity Set the speed of the route UINT TrajLinearType Types of velocity profiles for linear paths (0: (0: Trapezoid, 1: Sine1, 2: Sine2): Sine1 for low load and high speed;
Sine2 for high load and medium speed LREAL[0..2] TrajCircPar In the order of [Acceleration, Deceleration, Jerk], the velocity profile parameters of the circulation path LREAL ArchUpHeight Set the upper distance (h _up ) of the ArchTo path LREAL ArchDownHeight Set the lower distance (h _down ) of the ArchTo path LREAL ArchBlendRatio Set the blending ratio of the ArchTo path (0.0 to 1.0) UINT InSignalNum Setting of a unique event number to be signaled when the route starts (0: none, 1 to: event number) UINT OutSignalNum Setting of a unique event number to be signaled when a route is completed (0: none, 1 to: event number) UINT InWaitNum Setting of a unique event number to wait when a route starts (0: none, 1 to: event number)
In addition, the function block checks if the event number is 0, which may mean that there is no assigned event. UINT OutWaitNum Setting a unique event number to wait when path is completed (0: none, 1 to: event number)
In addition, the function block checks if the event number is 0, which may mean that there is no assigned event. LREAL EffectorForce At the end-effector at the beginning of the path, we set the required torque of the actuator or the force that represents the on/off state, and this setting remains at the end of the path. Output BOOL Valid true if valid output is available Busy BOOL Function block not complete Error BOOL Notifies that an error has occurred within a function block ErrorID WORD Error identification

In addition, the route driving command of FIG. 11 is a command for performing driving along a set route, and the contents of the input/output are shown in Table 2 below.

Input/Output UINT AxesGroup See axis group (1 to 16) UINT NextPathIdx After the function block executes the current path, the current path index in the PathCfg array will increase or decrease compared to StartPathIdx and EndPathIdx. Input BOOL Execute Sequentially executes the learned paths stored in the PathCfg array between StartPathIdx and EndPathIdx LS_TRobotPathCfg PathCfg Path configuration data indexed in PathCfg array BOOL EnableWaitNum Enables waiting for events that occur in the path's start and finish sessions BOOL EnableSetObj Must be set to true to update object position at path start UINT InWaitNum Event numbers that must match to start traversing the route UINT OutWaitNum Event number to complete route movement LREAL[0..5] ObjectPosition If EnableSetObj is true, the current object position can be changed to the newly measured position ObjectPosition[X, Y, Z, A, B, C] BOOL EnableTestStep Must be set to true to convert point-by-point movement to point-to-point movement to step through path LREAL VelOverFactor Overrides the speed set for the current driving route (0.0: zero speed, 0.5: half speed, 1.0: same speed) UINT StartPathIdx Set start step to index of PathCfg arrayNextPathIdx must initially have StartPathIdx value UINT EndPathIdx Set the end step to the index of the PathCfg array. Function block increases or decreases the NextPathIdx value compared to the EndPathIdx value. BOOL StopPathRun 1: Stop the current path 0: Wait for the execution trigger to restart the function block from the path marked by NextPathIdx Output BOOL Done Configured Full Path Step Completed BOOL Busy The function block is processing
Function block is being processed BOOL Active Indicates that the function block is controlling the axis group BOOL Error Notifies that an error has occurred within a function block BOOL CommandAborted Command is aborted by another command WORD ErrorID Error identification UINT SignalNum Signals event number both in the initiation of the path and in the completion of the path LREAL EffectorForce Outputs it at the beginning of the path and holds it until another function block changes it to 0.

First, it is necessary to define a specific path of PnP motion. That is, the path of a PnP motion may include multiple points, the starting point and the ending point of which are in different workpiece coordinate systems (i.e., along the conveyor belt) according to different mechanical devices (i.e., conveyor belts and rotary tables). It may be a point of the PCS and the PCS according to the rotary table). Referring to FIG. 8 , the path of the PnP motion may be defined in the form of an arc, and a first motion (the length of which is h _up ) moving in an upward direction; A third motion (the length of which is h _down ) moving in a downward direction and a second motion (the length of which is l) moving in one direction between the first motion and the third motion may be included, respectively. In addition, the path of the PnP motion is, between the first and second motions and between the second and third motions, a motion that moves in a curved shape in order to minimize the impact caused by a sudden change of direction (hereinafter referred to as “each”). It may be desirable to further include "first and second curved motions").

That is, between the first and second motions, the first curved motion may move in a curved form of a specific curvature for a distance of d ₁ in h _up and l, respectively (distance in the upper direction and one direction). Also, between the second and third motions, the second curved motion may move in a curved form of a specific curvature for a distance of d ₂ (a distance in a lateral direction and a downward direction) in h _down and l, respectively.

That is, the PnP path of FIG. 8 includes P ₀ (x ₀ , y ₀ , z ₀ ) that is information on the start point of the first motion (the start point of h _up ), and the last point of the first motion (the end of h _up ). P ₁ (x ₀ , y ₀ , z ₀ +h _up ), which is information about the point), information about the first and second curved motions, and information about the start point of the third motion (the starting point of h _down ) P ₂ (x ₁ , y ₁ , z ₁ +h _down ) and P ₃ (x ₁ , y ₁ , z ₁ ), which is information about the last point of the third motion (the last point of h _down ), respectively As such, it can be parameterized. At this time, P ₁ (x ₀ , y ₀ , z ₀ +h _up ), which is information about the last point of the first motion (the last point of h _up ), corresponds to information about the starting point (start point of l) of the second motion. and P ₂ (x ₁ , y ₁ , z ₁ +h _down ), which is information on the start point of the third motion (the start point of h _down ), corresponds to the information on the last point (the last point of l) of the second motion. do. That is, in one PnP path, P ₀ corresponds to the starting point of the path, and P ₃ corresponds to the last point of the path.

In particular, in order to obtain information on the first and second curved motions, the following Equations (4) and (5) may be used.

(4)

(4)

(5)

(5)

At this time, min(a, b) represents a smaller (minimum) length value among a and b, and ArchBlendRatio represents a ratio value (0.0 ~ 1.0) to the minimum length value. That is, d ₁ and d ₂ have a length corresponding to a specific selected ratio from the corresponding minimum length value.

That is, for PnP operation, three motion paths according to the motion path according to FIG. 7 , i.e. h _up , l and h _down , and 2 according to the curvature method of equations (4) and (5) at their junctions You can parameterize the curved motion paths and set them as one path. These parameters may be input (teaching) to the driving route teaching command of FIG. 10 , and as a result of the input, a corresponding route may be generated through real-time operation when an actual driving is performed according to the route driving command of FIG. 11 .

In the driving route teaching command of FIG. 9, the InWait event, the driving route including the complex route for the PnP motion of FIG. 7 (the corresponding route may have Discrete, Continuous, and Synchronized characteristics), and the OutWait event number Each input (teaching) can be received.

The state machine according to FIG. 9 performs initialization while outputting InSignal (S101), and after performing MCS/PCS coordinate system setting and tracking setting synchronized to a specific axis/axis-group (S102), a determined path is performed (S103), and then, while outputting an OutSignal event, driving for one route is finished (S104), and updating the route index value (S105). In particular, the state machine performs S103 only when an InWait event is received after performing S102, and outputs an OutWait event after S104. In this case, the InWait event may be generated according to an operation of a previous state machine, a sensor, or the like. Such an operation of the state machine may be performed for all paths (a plurality of arc-shaped paths, that is, a plurality of paths according to FIG. 7 ) for each path. That is, when the operation of one state machine is finished, the operation of the next state machine may be performed. In addition, operations of a plurality of state machines (ie, generation of each arch-shaped path and path operation) may be performed as many as the number of iron beads that are objects.

In particular, S102 includes a coordinate system setting (PCS Setting) step and a tracking setting (Tracking Setting) step.

In this case, the step of setting the coordinate system is a step of setting the PCS coordinate system for each point of the path at the first point of the path based on the MCS coordinate system of the mechanical device located at the last point of the path.

Specifically, the step of setting the coordinate system for the arc-shaped path of FIG. 7 will be described. That is, in the coordinate system setting step, P ₀ , which is the starting point of the arcuate path, is a point at which the robot picks the iron bead in the first mechanical device, and the point is set as the current position value of the robot. In addition, P ₃ , which is the end (completed) point of the arcuate path, is a point where the robot places the iron ball with the second mechanical device. Accordingly, in the coordinate system setting step, based on the MCS coordinate system of the second mechanical device, which is the part where P ₃ is located, each point of the corresponding arcuate path (ie, P ₀ , P ₃ , and the first and second curved motions) Set the PCS coordinate system for the point).

For example, when performing PnP from a conveyor belt to a rotary table, the point at which the robot picks the iron beads from the conveyor belt is P ₀ , and the point at the moment when the robot places the iron beads on the rotary table is P ₃ . . That is, the PnP route including these P ₀ and P ₃ as the starting point and the last point is set as one driving route (hereinafter referred to as the “first driving route”), and for the first driving route, the The PCS coordinate system based on the MCS coordinate system is set. Of course, when there are a plurality of iron beads on the pallet of the conveyor belt, the first driving path and its PCS coordinate system for PnP for the plurality of beads may be set at the time of execution of different state machines, respectively. At this time, since the positions of the iron beads are different, the positions of P ₀ and P ₃ in these first driving paths may also be different.

Conversely, when performing PnP from the rotary table to the conveyor belt, the point at the moment when the robot picks the iron beads from the rotary table is P ₀ , and the point at the moment when the robot places the iron beads on the conveyor belt is P ₃ . . That is, the PnP path including these P ₀ and P ₃ as the starting point and the last point is set as one driving path (hereinafter, referred to as a “second driving path”), and for the second driving path, the The PCS coordinate system based on the MCS coordinate system is set. Of course, when there are a plurality of iron marbles on the pallet of the rotary table, the second driving path and the PCS coordinate system for PnP for the plurality of marbles may be set when different state machines are executed. In this case, since the positions of the iron beads are different, the positions of P ₀ and P ₃ in these second driving paths may also be different.

On the other hand, the tracking setting step is a step of setting to reflect the movement of the mechanical device with respect to the PCS coordinate system of the path set in the coordinate system setting step.

That is, in the case of the step of setting the coordinate system for the arch-shaped path of FIG. 7, it is only the setting of the PCS coordinate system for the arch-shaped path between P ₀ and P ₃ , and during the time between the setting for the path and the actual operation, P ₃ Since the located second mechanical device can continue to move, it is necessary to reflect this movement, and this is performed in the tracking setting step.

For example, when performing PnP from a conveyor belt to a rotary table, the PCS coordinate system for the arch-shaped path (that is, any first driving path) is set in the coordinate system setting stage, and the tracking setting stage for the set PCS coordinate system The reflection of the movement of the mechanical device (rotary table) of the last point is set. In this case, one state machine performs PCS coordinate system setting and tracking setting for one first driving path.

That is, each of the first driving route setting, the PCS coordinate system setting, and the tracking setting may be performed one by one when different state machines are operated. That is, for PnP for any one metal ball of the conveyor belt, while one state machine is operating, any one first driving route setting, the PCS coordinate system setting, and tracking setting are sequentially performed. After that, for PnP of the other metal bead of the conveyor belt, while the other state machine is operating, the other first driving route setting, the PCS coordinate system setting, and the tracking setting are sequentially performed. This setting can be repeated until all the iron beads of the conveyor belt are PnP moved to the rotary table.

Conversely, when performing PnP from a rotary table to a conveyor belt, the PCS coordinate system for the arch-shaped path (that is, any one of the second driving paths) is set in the coordinate system setting stage, and the tracking setting stage for the set PCS coordinate system The reflection of the movement of the mechanical device (conveyor belt) of the last point is set. In this case, one state machine performs PCS coordinate system setting and tracking setting for one second driving path.

That is, each of the second driving route setting, the PCS coordinate system setting, and the tracking setting may be performed one by one when different state machines are operated. That is, for PnP for any one metal ball of the rotary table, while one state machine is operating, any one second driving route setting, the PCS coordinate system setting, and the tracking setting are sequentially performed. Thereafter, for PnP of the other metal ball of the rotary table, while the other state machine operates, another second driving route setting, the PCS coordinate system setting, and the tracking setting are sequentially performed. These settings can be repeated until all the iron beads on the rotary table are PnP moved to the conveyor belt.

As described above, the present invention is the last point when performing PnP between points in different workpiece coordinate systems (ie, PCS along conveyor belt and PCS along rotary table) according to different mechanical devices (ie, conveyor belt and rotary table). Because the driving route needs to be updated based on the PCS coordinate system of the mechanical device to which it belongs (ie, it is not necessary to update every PCS coordinates every time), memory and cost can be low.

13 shows another example of a conversion command from MCS to PCS.

In particular, the coordinate system setting step and the tracking setting step can be set using a command according to the PLCOpen standard. For example, the coordinate system setting step may be performed using commands such as MC_SetCoordinateTransfrom and MC_SetCartesianTransfroms, and the tracking setting step may be performed using commands such as MC_SetDynCoordTransform, MC_TrackConveyorBelt, MC_TrackRotaryTable, etc. not.

In this case, MC_SetCoordinateTransfrom and MC_SetCartesianTransfroms are commands for converting a coordinate system, and as shown in FIG. 13 , only the arguments of each command are different. That is, by using these commands, as shown in FIG. 5( b ), the coordinate system transformation is possible from the MCS corresponding to the World coordinate system to the PCS corresponding to the Local coordinate system. However, MC_SetCoordinateTransform is configured in a more general form. TransX, TransY, TransZ, RotAng1, RotAng2, and RotAng3 are not separately input to the input called CoordTransform, but a matrix including all of them is directly input.

The state machine of FIG. 9 is performed for every path within the path operation command of FIG. 11, and the PLCOpen command is used as it is for MCS/PCS coordinate system setting, tracking setting, and Discrete/Continuous/Synchronized motion.

In the driving path teaching command of FIG. 10 , the set value of the robot gripper actuator can be input (teaching) for every path, and output is possible for every path in the state machine of FIG. 9 .

The driving route teaching command of FIG. 10 may receive input (teaching) InSignal and OutSignal event numbers, and the route driving command of FIG. 11 may output corresponding InSignal and OutSignal event numbers through the state machine of FIG. 9 .

In the path driving command of FIG. 11 , an initial object position value (ObjectPosition) of the PCS coordinate system of the current path may be received, and may be updated with a new position value when tracking is set in the state machine of FIG. 9 .

In particular, since the driving route is set in the form corresponding to FIG. 8, collisions and speed discontinuities may not occur when performing PnP motion from a pallet at one point to a pallet at another point. At the same time, according to the parameterization of the PnP motion path through equations (4) and (5), the path operation command of FIG. 11 can be operated as a motion command of the PLCOpen standard through the state machine of FIG. 9 .

Theoretically, according to the present invention, PnP operation is possible between PCS coordinate systems set to infinite number (teaching). This PnP operation is possible because the state machine of FIG. 9 is executed in the route driving command block of FIG. 11 while switching to the PCS coordinate system number of the current route received (teaching) from the driving route teaching command of FIG. 10 .

Meanwhile, according to the present invention, even when switching to the PCS coordinate system of the current route while driving, an object of the newly recognized PCS coordinate system can be accurately tracked. This is because, in the route driving command of FIG. 11, the initial object position value (ObjectPosition) of the PCS coordinate system of the current route can be received and updated with a new position value when tracking is set in the state machine of FIG. will be.

Also, according to the present invention, it is possible to determine the exact timing of switching to the PCS coordinate system of the current route while driving. This is because the InWait and OutWait event numbers input (teaching) from the driving path teaching command of FIG. 10 are monitored by receiving the input from the path driving command of FIG. 11, and the state transitions in the state machine of FIG. 9 in synchronization with the input. Such an accurate timing judgment is possible.

The InWait and OutWait event numbers may be provided by the user calling the route driving command of FIG. 11 . This is because the state machine of FIG. 9 signals the InSignal and OutSignal event numbers to the user as the route driving command output of FIG. 11 , so the user can know the timing of providing the InWait and OutWait event numbers.

Through the input (teaching) of the entire driving path using the driving path teaching command of FIG. 10 and the call of the path driving command of FIG. 11, the robot can interact with the complex surrounding environment in addition to driving the inputted entire path do. This makes interaction possible by sequence coding events such as InWait, OutWait, InSignal, and OutSignal with respect to changes in the surrounding environment, and the driving path teaching command of FIG. Since it includes the actuator's setpoint, the required sensor correction value, and the speed override value, such a mutual coordination is possible. That is, the driving path teaching command of FIG. 10 includes input of an output setting value of an end-effector driver of the robot and a correction value of a sensor required for each driving path, and the path driving command of FIG. 11 is being tracked. It may include an input for updating to compensate the movement/rotation amount of the object, and an input for overriding a speed set for the current driving route.

12 shows another expanded example of a general form of tracking between an axis-group versus an axis-group.

That is, although PnP between the PCS coordinate system for tracking the movement/rotation amount of the conveyor and the rotary table has been described above with reference to FIG. 7, the tracking principle between these different PCS coordinate systems can be applied similarly to the configuration shown in FIG. 12.

Specifically, FIG. 12 corresponds to a case where two robots prepare an iron plate, and the other robot performs spot welding sequentially while tracking the two iron plates. That is, in the case of the above-described conveyor belt and rotary table, the movement and rotation axes are tracked by the PCS coordinate system, and in the case of FIG. 12, the axis-group called the robot is tracked by the PCS coordinate system. In the case of FIG. 12, the state machine of FIG. 9 may be extended to call a coordinate system transformation command (MC_SetDynCoordTransform) between axes-groups for tracking between axes-groups.

For example, for the operation of the robot, it may be necessary to drive a motion according to the first driving path, the second driving path, and the third driving path. That is, the motion of the robot arm must be performed according to the path sequence of point 1 -> point 2 -> point 3 -> point 1. In this case, the first point is the start point of the first route and the last point of the third route, the second point is the last point of the first route and the starting point of the second route, and the third point is the last point of the second route. That is, Points 1 and 2 are points in different PCS coordinate systems, Points 2 and 3 are also points in different PCS coordinate systems, and Points 3 and 1 are also points in different PCS coordinate systems.

In this case, when the robot arm is positioned at one point, the first state machine is operated and the first setting is performed. That is, the first setting includes setting of the first driving route, setting of the PCS coordinate system based on the MCS of the next two points, and setting of tracking, respectively. According to the first setting, the motion driving according to the corresponding driving path is performed in the first state machine.

Thereafter, as the robot arm is positioned at the two points, the second state machine operates, and the second setting is performed. That is, the second setting includes setting of the second driving route, setting of the PCS coordinate system based on the MCS of the next three points, and setting of tracking, respectively. According to the second setting, the motion driving according to the corresponding driving path is performed in the second state machine.

Thereafter, as the robot arm is positioned at the third point, the third state machine operates and the third setting is performed. That is, the third setting includes setting of the third driving route, setting of the PCS coordinate system based on the MCS of the next point, point 1, and setting of tracking, respectively. According to this third setting, the third state machine performs motion driving according to the corresponding driving path.

The present invention configured as described above has the advantage of enabling motion control for complex types of driving situations that cannot be responded to with commands according to the existing PLCopen standard, such as Pick-and-Place (PnP) between multiple PCS coordinate systems. have. That is, the present invention has the advantage of enabling PnP operation of the robot between a plurality of different PCS coordinate systems in which tracking is in progress on a specific axis or axis-group. In addition, the present invention has an advantage in that it is possible to synchronize an event input of an external environment during PnP operation and to signal an event. In addition, the present invention has the advantage that it is possible to implement a library-type command (User defined function block) by utilizing the command defined in the PLCOpen standard as a basic building block. In addition, in the present invention, when performing motion between different PCS coordinate system points according to different mechanical devices, the driving route can be updated based on the PCS coordinate system of the mechanical device to which the last point belongs, so memory is small and cost is low. There is an advantage.

In the detailed description of the present invention, although specific embodiments have been described, various modifications are possible without departing from the scope of the present invention. Therefore, the scope of the present invention is not limited to the described embodiments, and should be defined by the following claims and their equivalents.

10: motion control system 100: external device
200: motion controller 210: path setting unit
2220: coordinate system setting unit 230: operation control unit
300: driver

Claims (13)

A method performed in a system comprising a motion controller for motion control between multiple product coordinate systems (PCS), the method comprising:
a first setting step of setting a driving route including a plurality of points;
a second setting step of setting, for the workpiece coordinate system of each point of the driving path, a work coordinate system based on a machine coordinate system (MCS) of a machine located at the last point of the driving path as a reference; and
a driving step of performing a motion operation according to the set workpiece coordinate system and driving path;
A motion control method comprising a.
According to claim 1,
In the driving path, the starting point and the ending point are points in different workpiece coordinate systems according to different mechanical devices.
3. The method of claim 2,
The second setting step includes the step of reflecting tracking according to the movement of the mechanical device with respect to the work coordinate system of the respective points.
4. The method of claim 3,
The driving path is a motion control method that is an arch-shaped path for a pick and place (PnP) driving.
5. The method of claim 4,
The arcuate path includes a first motion path of upward movement of height h _up , a third motion path of downward movement of height h _down , and a second motion path of l width unidirectional movement between the first and third motions. A motion control method comprising: two motion paths; a first curved motion path between the first and second motion paths; and a second curved motion path between the second and third motion paths.
6. The method of claim 5,
the first curved motion path has a path of specific curvature for a distance d ₁ from h _up and l, respectively, and the second curved motion path has a specific curvature of a specific curvature for a distance d ₂ from h _down and l, respectively. A motion control method having a path, and d1 and d1 are obtained using the following equation.

(However, min(a, b) represents the smaller (minimum) length value among a and b, and ArchBlendRatio represents the ratio value to the minimum length value)
4. The method of claim 3,
The first setting step includes setting a plurality of driving paths and an index of input/output events for each driving path,
The second setting step includes setting a work coordinate system for each point of the driving path matching the index of the input/output event,
The driving step includes performing driving on a driving route matching an index of an input/output event.
8. The method of claim 7,
A motion control method in which the second setting step and the driving step are sequentially and repeatedly performed for each of the set driving paths.
4. The method of claim 3,
It is applicable when there is no command of the PLCopen standard for tracking a plurality of PCS coordinate systems, but the second setting step and the operation step are performed using a command according to the PLCopen standard.
8. The method of claim 7,
The command for performing the first setting step may receive an output setting value of an end-effector driver of the robot and a correction value of a sensor required for each driving path,
The command for performing the driving step includes an input for updating to compensate for a movement/rotation amount of an object being tracked, and an input for overriding a speed set for a current driving path, respectively.
A motion controller for motion control between multiple product coordinate systems (PCS), comprising:
a path setting unit for setting a driving path including a plurality of points, but having a starting point and an ending point of a different work coordinate system according to different mechanical devices;
a coordinate system setting unit that sets the workpiece coordinate system of each point of the driving path to a work coordinate system based on a machine coordinate system (MCS) of a machine located at the last point of the driving path;
an operation control unit for performing motion operation according to the set workpiece coordinate system and operation path;
A motion controller comprising a.
11. The method of claim 10,
The coordinate system setting unit is a motion controller that reflects the tracking (tracking) according to the movement of the mechanical device when setting the work coordinate system of the respective points.
A motion control system for motion control between multiple product coordinate systems (PCS), comprising:
It includes a motion controller for controlling the operation of the drive, and a driver for driving the motion device according to a control signal received from the motion controller,
The motion controller is
Set a driving path that includes multiple points, but the starting and ending points are points in different workpiece coordinate systems according to different machine tools, A motion control system that sets the workpiece coordinate system based on the machine coordinate system (MCS) of the machine and controls the operation of motion according to the set workpiece coordinate system and driving path.

Images