CN102081354B - Multi-axis motion control interpolation algorithm based on high-speed field bus - Google Patents

Abstract

An interpolation algorithm for multi-axis motion control based on high-speed fieldbus. This invention is mainly used in the fields of mechatronic motion controllers, numerical control systems, robot control systems, etc., especially in applications that require multi-axis high-speed and high-precision linkage control. The realization of this algorithm is to divide a target position sequence into three processes of rough interpolation, speed planning and fine interpolation to reconstruct the position and speed again, communicate with the servo motor driver through the high-speed field bus, and finally achieve high speed and high speed. Precision position control. It is characterized in that the multi-axis interpolation algorithm includes rough interpolation, speed planning, fine interpolation and high-speed industrial Ethernet bus.

Description

A kind of multi-axis motion control interpolation algorithm based on high-speed field bus

Technical field:

A kind of multi-axis motion control interpolation algorithm based on high-speed field bus.This invention is mainly used in the motion controller of electromechanical integration, digital control system, and the fields such as robot control system, especially need the interlock of multiaxis high speed and super precision to control in application.

Background technology:

Realize at present interpolating method that multiaxis controls and be motion control interpolator according to the difference of target location and current location, calculate the distance that each axle need to move, be converted into pulsed quantity or magnitude of voltage is connected to servo-driver.The realization of this method has significant limitation:

1. consider current location and target location, at the one section of movement locus being formed by short line segment sequence, cannot reach the level and smooth effect of actual motion process medium velocity;

2. travelling speed and precision cannot improve;

3. can not Real-time Feedback to the operational factor of servomechanism, the correlated variables of actual motion can not be participated in to interpolation and calculate.

4. the interlock and the closed-loop control that realize more than 5 axles are very difficult.

Summary of the invention:

This multi-axis interpolation algorithm is, by high-speed field bus, the operational factor of servo-driver is fed back to the poor device of mending, and the poor device of mending carries out rough interpolation to the target location sequence consisting of short line segment, speed planning, more smart interpolation.In essence Interpolation Process, according to the actual operation parameters feeding back to, carrying out target adjusts again.Technical scheme of the present invention is: for the track that will move, first carries out rough interpolation, once reads in many little line segments, and with p (position), v (speed), a (acceleration), t (time) is parameter; The current residing motion state of coupling system, as acceleration and deceleration or state at the uniform velocity, provides suitable p of this little line segment, v.And then carry out speed planning and use the little a serial of line sections of i mark, j mark line segment chain-ordering, v _ibe i-1 and i the tarnsition velocity between little line segment, v _jfor the tarnsition velocity between rear j-1 and j line segment chain is processed in link.If j line segment chain length is s _j, starting velocity and end speed are respectively v _jand v _j+1, the peak acceleration of line segment chain is a _max, a minute situation is calculated.Finally carry out smart interpolation and from bus, obtain the operational factors such as physical location and speed, in conjunction with uncompleted Place object sequence, with natural curve shape, with the equation of higher degree, again describe, carry out spline interpolation.

Accompanying drawing explanation: Fig. 1 is the interpolation algorithm process flow diagram based on high-speed bus;

Fig. 2 is that the interpolation algorithm based on high-speed bus calculates design sketch;

Embodiment:

1. for the track that will move, first carry out rough interpolation, once read in many little line segments, with p (position), v (speed), a (acceleration), t (time) is parameter; The current residing motion state of coupling system, as acceleration and deceleration or state at the uniform velocity, provides suitable p of this little line segment, and (this process has three kinds of situations to v, and the first: P is final position.The second situation: this little line segment is oversize, certain position that p just mediates so.The third situation: need several little line segment splicings just can obtain this p); Obtaining this distal point position p is exactly the point that rough interpolation obtains, and then this location point is just done the distal point of smart interpolation by taking away.

2. the data of pair rough interpolation are carried out speed planning, use the little a serial of line sections of i mark, j mark line segment chain-ordering, v _ibe i-1 and i the tarnsition velocity between little line segment, v _jfor the tarnsition velocity between rear j-1 and j line segment chain is processed in link.If j line segment chain length is s _j, starting velocity and end speed are respectively v _jand v _j+1, the peak acceleration of line segment chain is a _maxif had

process in two kinds of situation below.

Situation one: if v _j> v _j+1, want reverse scan line segment chain, adjust v _jsize, order

$v_j=\sqrt{v_{j+1}^2+{2a}_{\max }{s}_j}---\left(1\right)$

V _jafter change, to rejudge v _j-1if,

V _j-1> v _jand

To adjust v _j-1size, order

$v_{j-1}=\sqrt{v_j^2+{2a}_{\max }{s}_{j-1}}---\left(2\right)$

If v _j-1> v _jand

or v _j-1≤ v _j, need not adjust v _j-1, reverse scan stops.V _j-2the rest may be inferred.

Situation two: if v _j< v _j+1, this section of actual speed, add less than v _j+1, adjust the speed v of this point _j+1make

$v_{j+1}=\sqrt{v_j^2+2a_{\max }{s}_j}---\left(3\right)$

If do not adjusted v _j+1affect the velocity analysis of subsequent segment.This algorithm is simple, effective, and machining precision is high, and resource consumption is little, and efficiency is high.

3. by high-speed bus, obtain in real time the parameters such as speed in operational process and position;

4. according to the physical location obtaining, the operational factors such as speed, in conjunction with uncompleted Place object sequence, describe with the equation of higher degree with natural curve shape, carry out spline interpolation.Drafting natural curve equation is:

y＝ax^3+bx^2+cx+d

A, with tangential linear interpolation, realize curve interpolating.

B, at every turn to x axle feeding one step, revise the direction of tangent line (recalculating four parameters), to approach straight line.

Claims (2)

1. A multi-axis motion control interpolation algorithm based on high-speed field bus. The realization of this algorithm is to divide a target position sequence into three processes: rough interpolation, speed planning, and fine interpolation to reconstruct the position and speed again. The high-speed field bus communicates with the servo motor driver, and finally realizes high-speed and high-precision position control. It is characterized in that the multi-axis interpolation algorithm includes rough interpolation, speed planning, fine interpolation and high-speed industrial Ethernet bus; The rough interpolation refers to reading in a plurality of small line segments at one time, with the parameter p representing the position, v representing the velocity, a representing the acceleration, and t representing the time; combined with the current state of motion of the system such as acceleration and deceleration or a constant speed state, the given This small line segment has a suitable p and v. This process is divided into the following three situations: The first type: p is the end position; The second type: this small line segment is too long, then p is somewhere in the middle; The third type: several small line segments need to be stitched together to obtain this p; Obtaining the position p of this end point is the point obtained by rough interpolation, and then this position point is used as the end point of fine interpolation; The speed planning refers to the speed planning of the rough interpolation data, specifically: mark the sequence of small line segments with i, mark the sequence of line segment chains with j, and v _i is the angular velocity between the i-1th and i-th small line segments , v _j is the angular velocity between the j-1th line segment chain and the j-th line segment chain after link processing; let the length of the j-th line segment chain be s _j , the starting speed and the ending speed are v _j and v _j+1 respectively, The maximum acceleration of the line segment chain is a _max , according to

The value of v _j >v _j+1 and v _j <v _j+1 are divided into two cases for processing; The fine interpolation is to use the operating parameters obtained from the bus, such as actual position and speed, in combination with the unfinished position target sequence, to describe again with a high-order equation in the shape of a natural curve, and perform spline interpolation; The spline interpolation is: A. Curve interpolation is realized by linear interpolation along the tangential direction; B. Feed one step to the X axis each time, and modify the direction of the tangent to approach the straight line. 2. The multi-axis motion control interpolation algorithm based on high-speed field bus as claimed in claim 1, characterized in that: said high-speed field bus is a real-time bus based on industrial Ethernet protocol such as EtherCAT, PowerLink.

Images