CN111522313B - Method for controlling servo motor curve motion based on CANOPEN protocol - Google Patents

Abstract

The invention discloses a method for controlling servo motor curvilinear motion based on a CANOPEN protocol, which realizes that a plurality of servo motors drive a workpiece to complete curvilinear motion. According to the invention, independent servo motor drivers are controlled in a networking mode, periodic SYNC instructions are sent to read feedback information to form a closed-loop system, and the position of the next movement is adjusted according to the received position information in the movement process, so that the servo motor drives a workpiece to complete curvilinear movement. A CAN bus is adopted to establish cluster control of one master and a plurality of slaves, and a CANOPEN protocol is utilized to complete data interaction in a coordinated mode, so that the position synchronization of the servo motor is realized.

Description

Method for controlling servo motor curve motion based on CANOPEN protocol

Technical Field

The application belongs to the field of industrial control, and particularly relates to a method for controlling the curvilinear motion of a servo motor based on a CANOPEN protocol.

Background

CANOPEN is a high-level communication protocol over a Control Area Network (CAN), including communication sub-protocols and device sub-protocols, commonly used in embedded systems, and is also a field bus commonly used for industrial control. It implements agreements above (including at) the network layer in the OSI model. The CANOPEN standard includes an addressing scheme, several small communication sub-protocols, and an application layer defined by the device sub-protocols. CANOPEN supports network management, equipment monitoring and inter-node communications, including a simple transport layer that handles segmented delivery of data and combinations thereof. Typically, the data link layer and the physical layer are implemented using CAN. In addition to CANOPEN, there are other communication protocols (e.g., EtherCAT) that implement the CANOPEN device sub-protocol.

The servo interface buses widely adopted in the industry include CANOPEN, Powerlink, EtherCAT and the like, and the schemes can realize distributed multi-axis control through all-digital information transmission, so that the complexity of the system is reduced, and the expansion is easy. Although industrial ethernet such as EtherCAT and Powerlink has the advantages of high communication rate and large data transmission amount, the industrial ethernet is mostly realized by adopting special hardware or large-capacity FPGAs, and has the defects of high hardware cost, 100m maximum transmission distance and the like. However, at present, most of MCUs contain a CAN peripheral, and a CANOPEN network CAN be realized through the CAN peripheral, so that the cost of the system CAN be reduced, and the anti-interference capability of the system is increased.

In addition, in the industrial control, most of the traditional servo motors are independently operated at present, and the operation of a curve track is difficult to realize, so that certain limitation is brought to the industrial control processing.

Disclosure of Invention

The application aims to provide a method for controlling the curvilinear motion of a servo motor based on a CANOPEN protocol, which is used for networking and controlling independent servo motor drivers to realize the curvilinear motion of a workpiece.

In order to achieve the purpose, the technical scheme adopted by the application is as follows:

a method for controlling servo motor curvilinear motion based on CANOPEN protocol realizes that a plurality of servo motors drive workpieces to complete curvilinear motion, the CANOPEN protocol comprises a master station and a slave station, a plurality of servo drivers for controlling the motion of each servo motor are taken as master controllers, the slave stations are taken as slave controllers, and the method for controlling the servo motor curvilinear motion based on the CANOPEN protocol comprises the following steps:

step S1, the master station detects the on-line slave station in the current CAN bus;

step S2, the master station sends a broadcast instruction to inform each online slave station to enter a pre-operation mode;

step S3, the master station sends a TPDO instruction, configures communication parameters and mapping parameters of each online slave station, sends a broadcast instruction and informs each online slave station to control the servo motor to move to a zero position and enter an operation mode;

step S4, after receiving the displacement instruction, the master station analyzes the displacement instruction to obtain a target curve motion track contained in the displacement instruction, discretizes the target curve motion track into a plurality of small line segments, and sequentially takes the small line segments as the current ideal motion track;

step S5, the master station sends periodic SYNC commands, and the current position information (x) of the workpiece is obtained according to the feedback of each slave station_i，y_i) Wherein i represents the current period and is based on the current position information (x)_i，y_i) Calculating a first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀) Difference in vertical direction from the current ideal motion trajectory, where D₀Taking the predicted position corresponding to the minimum difference value as a target position of the next period for the moving distance, and distributing the target position to a corresponding online slave station through a TPOD (transport format object) instruction until the workpiece moves to the tail end of the current ideal motion track;

step S6, judging whether the current ideal motion track is the last small line segment in the motion track of the target curve, if so, finishing and ending; if not, a small line segment is taken down as the current ideal motion track, and step S5 is executed again.

Preferably, the step S5 specifically includes the following steps:

s5.1, setting the trajectory equation of the current ideal motion trajectory as y ═ kx + b, sending SYNC instructions by the master station to obtain the positions fed back by the on-line slave stations, and combining to obtain the current position information (x) of the workpiece_i，y_i)，i≥1；

S5.2, enabling the moving distance of the workpiece on the X axis in each period to be D₀The distance of movement in the Y axis being 0 or D₀Therefore, two predicted positions of the workpiece in the next cycle are obtained: first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀)；

S5.3, obtaining the trace position (x) of the workpiece in the next period according to a trace equation_i+1′，y_i+1') and y)_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀Wherein i +1 represents the next cycle;

s5.4, respectively calculating the distance between the predicted position and the track position:

d₁＝|y_i+1′-y_i|＝|kx_i+1′+b-y_i|

d₂＝|y_i+D₀-y_i+1′|＝|y_i+D₀-kx_i+1′-b|

s5.5 according to the distance d₁、d₂Determining a target position (x) for a next cycle_i+1，y_i+1) The method comprises the following steps:

if d is₁＞d₂Indicates a second predicted position (x)_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀；

When d is₁≤d₂Indicates the first predicted position (x)_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i；

S5.6, the master station distributes the target position to the corresponding online slave station through the TPOD instruction, and controls the movement distance of the slave station in the next period to enable the workpiece to reach the target position in the next period;

and S5.7, repeatedly executing the step S5.1 to the step S5.6 until the workpiece moves to the tail end of the current ideal motion track.

Preferably, the step S5 specifically includes the following steps:

s5.1, setting the trajectory equation of the current ideal motion trajectory as y ═ kx + b, sending SYNC instructions by the master station to obtain the positions fed back by the on-line slave stations, and combining to obtain the current position information (x) of the workpiece_i，y_i)，i≥1；

S5.2, enabling the moving distance of the workpiece on the X axis in each period to be D₀The distance of movement in the Y axis being 0 or D₀Therefore, two predicted positions of the workpiece in the next cycle are obtained: first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀)；

S5.3, obtaining the trace position (x) of the workpiece in the next period according to a trace equation_i+1′，y_i+1') and y)_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀Wherein i +1 represents the next cycle;

s5.4, respectively calculating the Y-axis position difference between the predicted position and the track position:

d₃＝y_i+1′-y_i＝kx_i+1′+b-y_i

d₄＝y_i+D₀-y_i+1′＝y_i+D₀-kx_i+1′-b

s5.5 according to the distance d₃、d₄Determining a target position (x) for a next cycle_i+1，y_i+1) The method comprises the following steps:

let C_i＝Δx×(d₃-d₄)＝2Δy×x_i-2Δx×y_i+f

Wherein Δ x ═ x₂-x₁)，Δy＝(y₂-y₁)，(x₁，y₁) And (x)₂，y₂) The first point and the second point on the current ideal motion track are shown, and k is delta y/delta x, f is 2 delta y multiplied by D₀+Δx(2b-D₀)；

For the i +1 th cycle, C_i+1＝Δx×(d₃-d₄)＝2Δy×x_i+1-2Δx×y_i+1+ f, and x_i+1＝x_i+D₀；

Subtracting the two equations to obtain a recursion formula: c_i+1＝C_i+2Δy×D₀-2Δx(y_i+1-y_i)；

Assuming that the initial motion point of the workpiece is the first point on the current ideal motion track, the following conditions are satisfied: y is₁＝kx₁+b；

Thus C₁＝2Δy×D₀-Δx×D₀；

According to C_iCan be known by the calculation formula of C_iAnd d₃-d₄Have the same sign, so the following decision rule can be obtained: if k > 0, and C_iWhen the value is more than or equal to 0, indicating the second prediction position (x)_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀And C can be obtained according to a recurrence formula_i+1＝C_i+2D₀(Δy-Δx)；

When k > 0, and C_iWhen < 0, the first predicted position (x) is indicated_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_iAnd C can be obtained according to a recurrence formula_i+1＝C_i+2Δy×D₀；

If k is less than 0, and C_iWhen < 0, a second predicted position (x) is indicated_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀And C can be obtained according to a recurrence formula_i+1＝C_i+2D₀(Δy-Δx)；

When k > 0, and C_iWhen the value is more than or equal to 0, the first prediction position (x) is indicated_i+D₀，y_i) Distance from current ideal fortuneIf the moving track is closer, the first predicted position is selected as the target position of the next period, i.e. the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_iAnd C can be obtained according to a recurrence formula_i+1＝C_i+2Δy×D₀；

S5.6, the master station distributes the target position to the corresponding online slave station through the TPOD instruction, and controls the movement distance of the slave station in the next period to enable the workpiece to reach the target position in the next period;

and S5.7, repeatedly executing the step S5.1 to the step S5.6 until the workpiece moves to the tail end of the current ideal motion track.

According to the method for controlling the servo motor to move in the curve based on the CANOPEN protocol, independent servo motor drivers are controlled in a networking mode, periodic SYNC instructions are sent to read feedback information to form a closed-loop system, the position of the next movement is adjusted according to the received position information in the movement process, and therefore the servo motor drives a workpiece to complete the curve movement. A CAN bus is adopted to establish cluster control of one master and a plurality of slaves, and a CANOPEN protocol is utilized to complete data interaction in a coordinated mode, so that the position synchronization of the servo motor is realized.

Drawings

FIG. 1 is a flow chart of a method for controlling a servo motor to move in a curve based on a CANOPEN protocol according to the present application;

fig. 2 is a schematic diagram of a current ideal motion trajectory in embodiment 1 of the present application;

fig. 3 is a schematic diagram of a current ideal motion trajectory in embodiment 2 of the present application.

Detailed Description

The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.

Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used in the description of the present application herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application.

In one embodiment, a method for controlling the curvilinear motion of the servo motors based on the CANOPEN protocol is provided, so that a plurality of servo motors drive a workpiece to complete the curvilinear motion, and the problem that the traditional servo motors mostly operate independently and are difficult to realize the curvilinear motion is solved.

The CANOPEN protocol generally comprises a master station and a slave station, and has a communication function of one master station to a plurality of slave stations. In this embodiment, among a plurality of servo drivers for controlling the movement of each servo motor, a master controller is used as a master station, a slave controller is used as a slave station, the master station and the slave station are connected in parallel, and communication is performed based on the CANOPEN protocol in a CAN bus.

As shown in fig. 1, the method for controlling the curvilinear motion of the servo motor based on the CANOPEN protocol of the present embodiment includes the following steps:

and step S1, the master station detects the on-line slave station in the current CAN bus.

The master station is firstly electrified to configure peripheral parameters, and then the current online slave station is detected to finish preparation work. The peripheral configuration parameter is the conventional operation, and is mainly used for configuring the equipment into a required state, for example, the CAN is configured to work in a normal mode by power-on, the communication baud rate is 500kb, and the receiving interruption is enabled.

In step S2, the master station transmits a broadcast command to notify each online slave station to enter the pre-operation mode.

And step S3, the master station sends a TPDO instruction, configures communication parameters and mapping parameters of each on-line slave station, and sends a broadcast instruction to inform each on-line slave station to control the servo motor to move to the zero position and enter an operation mode.

It should be noted that the primary station needs to configure the TPDO and RPDO parameters of each secondary station through the SDO instruction, and send the TPDO instruction based on the configured TPDO and RPDO parameters. And the configuration or command transceiving between the master station and the slave station is the conventional operation of CANOPEN protocol communication, and the detailed description is omitted here.

And step S4, after the master station receives the displacement instruction, analyzing to obtain a target curve motion track contained in the displacement instruction, discretizing the target curve motion track into a plurality of small line segments, and sequentially taking the small line segments as the current ideal motion track.

It should be noted that, if the target motion trajectory is a straight-line segment, the discretization step may be omitted, and the straight-line segment is used as the current ideal motion trajectory for the subsequent operation.

Step S5, the master station sends periodic SYNC commands, and the current position information (x) of the workpiece is obtained according to the feedback of each slave station_i，y_i) Wherein i represents the current period and is based on the current position information (x)_i，y_i) Calculating a first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀) Difference in vertical direction from the current ideal motion trajectory, where D₀And taking the predicted position corresponding to the minimum difference value as the target position of the next period for the movement distance, and distributing the target position to the corresponding on-line slave station through the TPOD instruction until the workpiece moves to the tail end of the current ideal motion track.

Step S6, judging whether the current ideal motion track is the last small line segment in the motion track of the target curve, if so, finishing and ending; if not, a small line segment is taken down as the current ideal motion track, and step S5 is executed again. It should be noted that, if the target motion trajectory is a straight line segment, the current ideal motion trajectory is the last small line segment in the target curved motion trajectory.

According to the servo motor control system, independent servo motor drivers are controlled in a networking mode, periodic SYNC instructions are sent to read feedback information to form a closed-loop system, the target position of the next periodic movement is adjusted according to the received position information in the movement process, therefore, the servo motor drives a workpiece to complete curvilinear movement, and the problem that multi-dimensional, high-precision and low-cost movement control of the motor is difficult to achieve in industrial control is solved.

The following further illustrates the process of adjusting the target position of the next periodic movement according to the received position information by the embodiment.

Example 1

As shown in fig. 2, step S5 specifically includes the following steps:

s5.1, setting the trajectory equation of the current ideal motion trajectory to be y ═ kx + b (shown in fig. 2 as a schematic diagram when k > 0), the master station sends a SYNC command to obtain the positions fed back by the online slave stations, and the positions are combined to obtain the current position information (x is x > 0) of the workpiece_i，y_i)，i≥1

S5.2, enabling the moving distance of the workpiece on the X axis in each period to be D₀The distance of movement in the Y axis being 0 or D₀Therefore, two predicted positions of the workpiece in the next cycle are obtained: first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀) (ii) a To reduce motion errors, D₀Preferably one pixel point, i.e. D₀＝1。

S5.3, obtaining the trace position (x) of the workpiece in the next period according to a trace equation_i+1′，y_i+1') and y)_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀Wherein i +1 represents the next cycle;

s5.4, respectively calculating the distance between the predicted position and the track position:

d₁＝|y_i+1′-y_i|＝|kx_i+1′+b-y_i|

d₂＝|y_i+D₀-y_i+1′|＝|y_i+D₀-kx_i+1′-b|

s5.5 according to the distance d₁、d₂Determining a target position (x) for a next cycle_i+1，y_i+1) The method comprises the following steps:

if d is₁＞d₂Indicates a second predicted position (x)_i+D₀，y_i+D₀) Distance from current ideal motion railIf the trace is closer, the second predicted position is selected as the target position of the next period, i.e. the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀；

When d is₁≤d₂Indicates the first predicted position (x)_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i；

S5.6, the master station distributes the target position to the corresponding online slave station through the TPOD instruction, and controls the movement distance of the slave station in the next period to enable the workpiece to reach the target position in the next period;

and S5.7, repeatedly executing the step S5.1 to the step S5.6 until the workpiece moves to the tail end of the current ideal motion track.

The method for determining the target position of the next period is simple, the deviation between the determined target position and the current ideal motion trajectory is small, and the target curvilinear motion trajectory can be restored to a higher degree. However, the above process is only a preferred method, and is not limited to the above process, for example, in another embodiment, in order to further increase the speed of determining the target position in the next period, increase the response capability of the servo motor, and reduce the operation error, the process of adjusting the target position of the next period movement according to the received position information may also be as follows.

Example 2

As shown in fig. 3, step S5 specifically includes the following steps:

s5.1, setting the trajectory equation of the current ideal motion trajectory to be y ═ kx + b (shown in fig. 2 as a schematic diagram when k > 0), the master station sends a SYNC command to obtain the positions fed back by the online slave stations, and the positions are combined to obtain the current position information (x is x > 0) of the workpiece_i，y_i)，i≥1；

S5.2, enabling the moving distance of the workpiece on the X axis in each period to be D₀The distance of movement in the Y axis being 0 or D₀Therefore, two predicted positions of the workpiece in the next cycle are obtained: first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀) (ii) a To reduce motion errors, D₀Preferably one pixel point, i.e. D₀＝1。

S5.3, obtaining the trace position (x) of the workpiece in the next period according to a trace equation_i+1′，y_i+1') and y)_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀Wherein i +1 represents the next cycle;

s5.4, respectively calculating the Y-axis position difference between the predicted position and the track position:

d₃＝y_i+1′-y_i＝kx_i+1′+b-y_i

d₄＝y_i+D₀-y_i+1′＝y_i+D₀-kx_i+1′-b

s5.5 according to the distance d₃、d₄Determining a target position (x) for a next cycle_i+1，y_i+1) The method comprises the following steps:

let C_i＝Δx×(d₃-d₄)＝2Δy×x_i-2Δx×y_i+f

Wherein Δ x ═ x₂-x₁)，Δy＝(y₂-y₁)，(x₁，y₁) And (x)₂，y₂) The first point and the second point on the current ideal motion track are shown, and k is delta y/delta x, f is 2 delta y multiplied by D₀+Δx(2b-D₀)；

For the i +1 th cycle, C_i+1＝Δx×(d₃-d₄)＝2Δy×x_i+1-2Δx×y_i+1+ f, and x_i+1＝x_i+D₀；

Subtracting the two equations to obtain a recursion formula: c_i+1＝C_i+2Δy×D₀-2Δx(y_i+1-y_i)；

Suppose thatThe initial motion point of the workpiece is the first point on the current ideal motion track, and then the following conditions are met: y is₁＝kx₁+b；

Thus C₁＝2Δy×D₀-Δx×D₀；

According to C_iCan be known by the calculation formula of C_iAnd d₃-d₄Have the same sign, so the following decision rule can be obtained: if k > 0, and C_iWhen the value is more than or equal to 0, indicating the second prediction position (x)_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀And C can be obtained according to a recurrence formula_i+1＝C_i+2D₀(Δy-Δx)；

When k > 0, and C_iWhen < 0, the first predicted position (x) is indicated_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_iAnd C can be obtained according to a recurrence formula_i+1＝C_i+2Δy×D₀；

If k is less than 0, and C_iWhen < 0, a second predicted position (x) is indicated_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀And C can be obtained according to a recurrence formula_i+1＝C_i+2D₀(Δy-Δx)；

When k > 0, and C_iWhen the value is more than or equal to 0, the first prediction position (x) is indicated_i+D₀，y_i) From the current ideal motion trackAnd when the target position is closer, selecting the first predicted position as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_iAnd C can be obtained according to a recurrence formula_i+1＝C_i+2Δy×D₀；

S5.6, the master station distributes the target position to the corresponding online slave station through the TPOD instruction, and controls the movement distance of the slave station in the next period to enable the workpiece to reach the target position in the next period;

and S5.7, repeatedly executing the step S5.1 to the step S5.6 until the workpiece moves to the tail end of the current ideal motion track.

The parameter C in example 2 can be obtained from the above calculation process_i+1The calculation can be completed by arithmetic left shift, so the calculation method has high speed and is easy to realize by hardware.

The method for controlling the curvilinear motion of the servo motor based on the CANOPEN protocol provided by the embodiment can realize multi-axis synchronous control, and the speed of determining the target position in the next period according to the current feedback position is high, and the error between the target position and the target motion trajectory is small, so that the displacement of the workpiece can be quickly and accurately completed.

The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.

The above-mentioned embodiments only express several embodiments of the present application, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the concept of the present application, which falls within the scope of protection of the present application. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims (1)

1. A method for controlling servo motor curvilinear motion based on a CANOPEN protocol realizes that a plurality of servo motors drive workpieces to complete curvilinear motion, the CANOPEN protocol comprises a master station and a slave station, and is characterized in that a master controller is used as a master station and a slave controller is used as a slave station in a plurality of servo drivers for controlling the motion of each servo motor, and the method for controlling the servo motor curvilinear motion based on the CANOPEN protocol comprises the following steps:

step S1, the master station detects the on-line slave station in the current CAN bus;

step S2, the master station sends a broadcast instruction to inform each online slave station to enter a pre-operation mode;

step S3, the master station sends a TPDO instruction, configures communication parameters and mapping parameters of each online slave station, sends a broadcast instruction and informs each online slave station to control the servo motor to move to a zero position and enter an operation mode;

step S4, after receiving the displacement instruction, the master station analyzes the displacement instruction to obtain a target curve motion track contained in the displacement instruction, discretizes the target curve motion track into a plurality of small line segments, and sequentially takes the small line segments as the current ideal motion track;

step S5, the master station sends periodic SYNC commands, and the current position information (x) of the workpiece is obtained according to the feedback of each slave station_i，y_i) Wherein i represents the current period and is based on the current position information (x)_i，y_i) Calculating a first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀) Difference in vertical direction from the current ideal motion trajectory, where D₀Taking the predicted position corresponding to the minimum difference value as a target position of the next period for the moving distance, and distributing the target position to a corresponding online slave station through a TPOD (transport format object) instruction until the workpiece moves to the tail end of the current ideal motion track;

step S6, judging whether the current ideal motion track is the last small line segment in the motion track of the target curve, if so, finishing and ending; if not, taking down a small line segment as the current ideal motion track, and re-executing the step S5;

wherein, the step S5 specifically includes the following steps:

s5.1, setting the trajectory equation of the current ideal motion trajectory as y ═ kx + b, sending SYNC instructions by the master station to obtain the positions fed back by the on-line slave stations, and combining to obtain the current position information (x) of the workpiece_i，y_i)，i≥1；

S5.2, enabling the moving distance of the workpiece on the X axis in each period to be D₀The distance of movement in the Y axis being 0 or D₀Therefore, two predicted positions of the workpiece in the next cycle are obtained: first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀)；

S5.3, obtaining the trace position (x) of the workpiece in the next period according to a trace equation_i+1′，y_i+1') and Y)_i+1y_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀Wherein i +1 represents the next cycle;

s5.4, respectively calculating the distance between the predicted position and the track position:

d₁＝|y_i+1′-y_i|＝|kx_i+1′+b-y_i|

d₂＝|y_i+D₀-y_i+1′|＝|y_i+D₀-kx_i+1′-b|

s5.5 according to the distance d₁、d₂Determining a target position (x) for a next cycle_i+1，y_i+1) The method comprises the following steps:

if d is₁＞d₂Indicates a second predicted position (x)_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀；

When d is₁≤d₂Indicates the first predicted position (x)_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i；

S5.6, the master station distributes the target position to the corresponding online slave station through the TPOD instruction, and controls the movement distance of the slave station in the next period to enable the workpiece to reach the target position in the next period;

s5.7, repeatedly executing the step S5.1 to the step S5.6 until the workpiece moves to the tail end of the current ideal motion track;

alternatively, the step S5 specifically includes the following steps:

s5.1, setting the trajectory equation of the current ideal motion trajectory as y ═ kx + b, sending SYNC instructions by the master station to obtain the positions fed back by the on-line slave stations, and combining to obtain the current position information (x) of the workpiece_i，y_i)，i≥1；

S5.2, enabling the moving distance of the workpiece on the X axis in each period to be D₀The distance of movement in the Y axis being 0 or D₀Therefore, two predicted positions of the workpiece in the next cycle are obtained: first predicted position (x)_i+D₀，y_i) And a second predicted position (x)_i+D₀，y_i+D₀)；

S5.3, obtaining the trace position (x) of the workpiece in the next period according to a trace equation_i+1′，y_i+1') and y)_i+1′＝kx_i+1′+b，x_i+1′＝x_i+D₀Wherein i +1 represents the next cycle;

s5.4, respectively calculating the Y-axis position difference between the predicted position and the track position:

d₃＝y_i+1′-y_i＝kx_i+1′+b-y_i

d₄＝y_i+D₀-y_i+1′＝y_i+D₀-kx_i+1′-b

s5.5 according to the distance d₃、d₄Determining the next cycleTarget position (x)_i+1，y_i+1) The method comprises the following steps:

let C_i＝Δx×(d₃-d₄)＝2Δy×x_i-2Δx×y_i+f

Wherein Δ x ═ x₂-x₁)，Δy＝(y₂-y₁)，(x₁，y₁) And (x)₂，y₂) The first point and the second point on the current ideal motion track are shown, and k is delta y/delta x, f is 2 delta y multiplied by D₀+Δx(2b-D₀)；

For the i +1 th cycle, C_i+1＝Δx×(d₃-d₄)＝2Δy×x_i+1-2Δx×y_i+1+ f, and x_i+1＝x_i+D₀；

Subtracting the two equations to obtain a recursion formula: c_i+1＝C_i+2Δy×D₀-2Δx(y_i+1-y_i)；

Assuming that the initial motion point of the workpiece is the first point on the current ideal motion track, the following conditions are satisfied: y is₁＝kx₁+b；

Thus C₁＝2Δy×D₀-Δx×D₀；

According to C_iCan be known by the calculation formula of C_iAnd d₃-d₄Have the same sign, so the following decision rule can be obtained: if k > 0, and C_iWhen the value is more than or equal to 0, indicating the second prediction position (x)_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀And C can be obtained according to a recurrence formula_i+1＝C_i+2D₀(Δy-Δx)；

When k > 0, and C_iWhen < 0, the first predicted position (x) is indicated_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the next periodTarget position (x) of one cycle_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_iAnd C can be obtained according to a recurrence formula_i+1＝C_i+2Δy×D₀；

If k is less than 0, and C_iWhen < 0, a second predicted position (x) is indicated_i+D₀，y_i+D₀) When the distance between the target position and the current ideal motion track is shorter, the second predicted position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_i+D₀And C can be obtained according to a recurrence formula_i+1＝C_i+2D₀(Δy-Δx)；

When k > 0, and C_iWhen the value is more than or equal to 0, the first prediction position (x) is indicated_i+D₀，y_i) When the distance between the target position and the current ideal motion track is shorter, the first prediction position is selected as the target position of the next period, namely the target position (x) of the next period_i+1，y_i+1) In x_i+1＝x_i+D₀，y_i+1＝y_iAnd C can be obtained according to a recurrence formula_i+1＝C_i+2Δy×D₀；

S5.6, the master station distributes the target position to the corresponding online slave station through the TPOD instruction, and controls the movement distance of the slave station in the next period to enable the workpiece to reach the target position in the next period;

and S5.7, repeatedly executing the step S5.1 to the step S5.6 until the workpiece moves to the tail end of the current ideal motion track.

Images