Professional Documents
Culture Documents
Servo Drive Systems - Chapter 5 - 2020
Servo Drive Systems - Chapter 5 - 2020
Motion Control
1 @VLC-2019
5.1 Point-to-Point Control
Point to point motion is applied for some kinds of robots such
as: point welding, packaging,… In this type of motion, we
only take into account the position of the beginning, ending
points and time of motion.
➢ Problem:
𝑞
We have to determine the rule of
𝑞𝑓
change of the angular q between
the initial position 𝑞𝑖 and the
𝑞𝑖 finally position 𝑞𝑓 in the time
interval 0, 𝑡𝑓 .
0 𝑡𝑓 𝑡
4 @VLC-2019
5.1 Point-to-Point Control
For example: 𝑞𝑖 = 0, 𝑞𝑓 = 𝜋, 𝑡𝑓 = 1 ; the initial and final
velocity 𝑞ሶ 𝑖 = 𝑞ሶ 𝑓 = 0
From the above equations, we find the solutions:
𝑎0 = 𝑎1 = 0
𝑎2 = 3𝜋, 𝑎3 = −2π
3𝜋 𝑞𝑓
𝑞𝑚
0 𝑞𝑖
0
𝑟𝑎𝑑/𝑠 0 1
𝑞ሶ
𝑡𝑐 𝑡𝑚 𝑡𝑓
3𝜋/2 3𝜋/2
𝑞ሶ 𝑐
𝑟𝑎𝑑/𝑠 2 1 𝑞ሷ 𝑡𝑐 𝑡𝑓
𝑞ሷ 𝑐
6𝜋
𝑡𝑓
𝑡𝑐
6 −6𝜋 −𝑞ሷ 𝑐
@VLC-2019
5.1 Point-to-Point Control
The famous trajectory that is usually used in most industrial
controllers is Blended Polynomial with the trapezoidal
velocity.
Assuming that 𝑞ሶ 𝑖 = 𝑞ሶ 𝑓 = 0 , the time of acceleration and
deceleration is equal. We have:
𝑞𝑚 = (𝑞𝑓 +𝑞𝑖 )/2, at 𝑡𝑚 = 𝑡𝑓 /2 (5.7)
9 @VLC-2019
5.2 Application for Step and DC motors
❖ Step motor
The motor step angle: 𝛼; position: 𝜃; and speed 𝜔 are given by
2𝜋
𝛼= (𝑟𝑎𝑑)
𝑝𝑝𝑟
𝜃 = 𝑛𝛼 (𝑟𝑎𝑑) (5.15)
𝛼
𝜔= (𝑟𝑎𝑑/𝑠)
𝜏𝑑
where 𝑛 is the number of pulses
𝜏𝑑 is the delay time
To rotate a stepper motor at a constant speed, pulses must be
generated at a steady rate
10 @VLC-2019
5.2 Application for Step and DC motors
➢ Linear speed ramp
To start and stop the stepper motor in a smooth way, control of
the acceleration and deceleration is needed. Figure in previous
section shows the relation between acceleration, speed and
position.
The time delay 𝜏𝑑 between the stepper motor pulses controls
the speed. These time delays must be calculated in order to
make the speed of the stepper motor follow the speed ramp as
closely as possible.
Discrete steps control the stepper motor motion, and the
resolution of the time delay between these steps is given by
the frequency of the timer.
11 @VLC-2019
5.2 Application for Step and DC motors
ω
0 𝜏𝑑0 t1 𝜏𝑑1 t2 𝜏 t3 t4 t
𝑑2
We have: 𝜏𝑑0 = 𝑐0 𝜏𝑡
𝜏𝑑1 = 𝑐1 𝜏𝑡 (5.16)
12 @VLC-2019
𝜏𝑑2 = 𝑐2 𝜏𝑡
5.2 Application for Step and DC motors
The position at the time 𝑡 is given by equation:
𝑡
1 2
𝜃 𝑡 = න 𝜔ሶ 𝜏 𝑑𝜏 = 𝜔𝑡
ሶ
0 2
Assuming that 𝜃 𝑡 is corresponding to the n’th pulse, then
1 2
𝜃 𝑡 = 𝜔𝑡 ሶ 𝑛 = 𝑛𝛼
2
2𝑛𝛼
⇒ 𝑡𝑛 = (5.17)
𝜔ሶ
And the time delay between two steps is:
2𝛼 (5.18)
𝑐𝑛 𝜏𝑡 = 𝑡𝑛+1 − 𝑡𝑛 = ( 𝑛 + 1 − 𝑛)
13 @VLC-2019 𝜔ሶ
5.2 Application for Step and DC motors
Finally the expression for the counter delay is found:
1 2𝛼 (5.19)
𝑐𝑛 = ( 𝑛 + 1 − 𝑛)
𝜏𝑡 𝜔ሶ
When 𝑛 = 0:
1 2𝛼
𝑐0 =
𝜏𝑡 𝜔ሶ (5.20)
⇒ 𝑐𝑛 = 𝑐0 ( 𝑛 + 1 − 𝑛)
This calculation is much faster than the double square root, but
introduces an error at n =1. A way to compensate for this error
is by multiplying 𝑐0 with 0,676.
15 @VLC-2019
5.2 Application for Step and DC motors
❖ DC servo motor
A trajectory generation algorithm is essential for optimum
motion control. A linear piecewise velocity trajectory is
implemented in this application.
▪ For a position move, the velocity is incremented by a
constant acceleration value until a specified maximum
velocity is reached.
▪ The maximum velocity is maintained for a required amount
of time and then decremented by the same acceleration
(deceleration) value until zero velocity is attained.
▪ The velocity trajectory is therefore trapezoidal for a long
move and triangular for a short move where maximum
velocity was not reached.
16
@VLC-2019
5.2 Application for Step and DC motors
velocity
velocity
long
limit
short move
Slope = move
Accel. time
limit
Trajectory 1
Kp PI DC
Generator 𝑠
Velocity feedback
Position feedback
18
@VLC-2019
5.3 Continuous Path Motion
In many applications, the servo systems (Robots, CNC) have
to be controlled in a predefined continuous path.
𝑞
𝑞2 𝑞𝑁 = 𝑞𝑓
Π2 ⋯
Π1 𝑞3 Π3
𝑞1 = 𝑞𝑖 ⋯
𝑡1 = 0 𝑡2 𝑡3 𝑡𝑁 = 𝑡𝑓
The problem is to determine the trajectory that goes through N
path points with the constraint conditions. There are three
19
cases that usually use in trajectory generator:
@VLC-2019
5.3 Continuous Path Motion
▪ The velocities, 𝑞ሶ 𝑡 , at the path points are predetermined
▪ The velocities, 𝑞ሶ 𝑡 , at the path points are calculated by a
specific criterion.
▪ Guarantee the continuity of acceleration, 𝑞ሷ 𝑡 , at the path
points.
For example, in the case of the velocities are predetermined
Assuming that the trajectory has N path points, then the third
order polynomial is adopted to interpolate two adjacent points
( 𝑞𝑘 , 𝑞𝑘+1 ) . Each polynomial has to satisfy the following
conditions:
Π𝑘 𝑡𝑘 = 𝑞𝑘
Π𝑘 𝑡𝑘+1 = 𝑞𝑘+1 (5.23)
Πሶ 𝑘 𝑡𝑘 = 𝑞ሶ 𝑘
20 Πሶ 𝑘 𝑡𝑘+1 = 𝑞ሶ 𝑘+1
@VLC-2019
5.4 Interpolation
5.4.1 Introduction
A CNC machine generally has more than two controlled axes
to machine complex shapes. Two kinds of control can be
carried out:
▪ The point-to-point control method is used to move along
the axis to the desired position
▪ The contour control method is used to move along an
arbitrary curve
𝑆 𝑡 = න 𝑉 𝜏 𝑑𝜏 ≅ 𝑉𝑖 ∆𝑡 (5.24)
0 𝑖=1
𝑆𝑘 = 𝑉𝑖 ∆𝑡 + 𝑉𝑘 ∆𝑡 (5.25)
𝑖=1
Or: 𝑆𝑘 = 𝑆𝑘−1 + ∆𝑆𝑘 (5.26)
where, ∆𝑆𝑘 = 𝑉𝑘 ∆𝑡 (5.27)
➢ DDA interpolation
DDA interpolation calculates the displacement and velocity of
24 each axis based on part shape and command velocity.
5.4.2 Reference Pulse Interpolator
𝑌
End point 𝑃1 (𝐴, 𝐵) Flowchart of linear
interpolation
𝐵
𝐴
Starting point
𝑋
Linear interpolation
𝐿= 𝐴2 + 𝐵 2
𝑄1 = 0, 𝑄2 = 0
25
5.4.2 Reference Pulse Interpolator
𝑌
End point
Flowchart of circular
interpolation
R
𝐴 𝐵
Starting point
𝑋
Circular interpolation
(𝑋 − 𝑅)2 +𝑌 2 = 𝑅2
𝑋 = 𝑅 1 − 𝑐𝑜𝑠𝜔𝑡 , 𝑌 = 𝑅𝑠𝑖𝑛𝜔𝑡
𝑃1 , 𝑃2 : the center position when the
start point of the circle is the origin of
26
the coordinate system
5.4.2 Reference Pulse Interpolator
Result of interpolation
using DDA algorithm
28
@VLC-2019
5.4.2 Reference Pulse Interpolator
In this section, the Stars Approximation interpolator for a
circle will be addressed and the algorithm for a line can be
easily determined from the algorithm for a circle.
Assuming the the tool reaches
the position (𝑋𝑘 , 𝑌𝑘 ) after the
𝑘 𝑡ℎ iteration. In this algorithm,
the variable 𝐷𝑘 is calculated by:
𝐷𝑘 = 𝑋𝑘2 + 𝑌𝑘2 − 𝑅2 (5.30)
𝑋0 , 𝑌0 = (0,10)
𝑋𝑓 , 𝑌𝑓 = (10,0)
𝐷𝑖,𝑗 = 𝑋𝑖2 + 𝑌𝑗2 − 102
33
@VLC-2019
5.4.2 Reference Pulse Interpolator
❖ Direct Search
One of the weaknesses of the Stairs Approximation algorithm
is that plenty of iterations are required. Furthermore, possible
error conditions are not considered when the interpolated
position is decided.
The Direct Search algorithm carries out optimal interpolation
because the algorithm searches through all possible directions
and finds a direction with the minimum path error.
Basically, the Direct Search algorithm is very similar to the
Stairs Approximation. However, the Direct Search algorithm
considers the simultaneous movement of axes based on path
error
34
@VLC-2019
5.4.2 Reference Pulse Interpolator
The 𝐷𝑖,𝑗 for each case is
evaluated and the case with
smallest absolute value is
selected for movement. The
flowchart of the Direct
Search for clockwise
circular movement in the
first quadrant.
Increasing 1 BLU for X-axis
Decreasing 1 BLU for Y-axis
Increasing 1 BLU for X-axis
and simultaneously decreasing
35
1 BLU for Y-axis
5.4.2 Reference Pulse Interpolator
Example:
𝑋0 , 𝑌0 = (0,10)
𝑋𝑓 , 𝑌𝑓 = (10,0)
𝐷𝑖,𝑗 = 𝑋𝑖2 + 𝑌𝑗2 − 102
36
@VLC-2019
5.4.3 Sampled-Data Interpolator
In the Sampled-Data interpolation method, the interpolation is
executed in two stages:
▪ In the first stage, an input contour is segmented into straight
line segments within an allowable tolerance (rough
interpolation)
▪ In the second stage, the approximating line segments are
interpolated and the interpolation result sent to the related
axis (fine interpolation)
The Euler method, Taylor method, and Tustin method are
typical algorithms for rough interpolation.
The Sampled-Data method is typical for modern CNC due to
its adequacy for high speed machining.
37
@VLC-2019
5.4.3 Sampled-Data Interpolator
❖ Reference Word Interpolator for Lines
In the reference word algorithm, linear interpolation is very
simple compared to circular interpolation. The concept and
flowchart for linear interpolation are illustrated as follow:
38
@VLC-2015
5.4.3 Sampled-Data Interpolator
The fundamental idea of this algorithm is segmentation of the
path by the interpolation time, 𝑇𝑖𝑝𝑜 . At each sampling time the
interpolated point can be defined:
𝑥𝑖+1 = 𝑥𝑖 + ∆𝑥 𝑦𝑖+1 = 𝑦𝑖 + ∆𝑦 (5.34)
where, 𝑥𝑒 − 𝑥𝑠
∆𝑥 = ∆𝐿𝑐𝑜𝑠𝜃 = ∆𝐿 (5.35)
𝐿
𝑦𝑒 − 𝑦𝑠
∆𝑦 = ∆𝐿𝑠𝑖𝑛𝜃 = ∆𝐿 (5.36)
𝐿
∆𝐿 = 𝑉𝑇𝑖𝑝𝑜 (5.37)
The total number of iterations for the interpolation is defined:
𝐿 (5.38)
39
𝑁 = 𝑖𝑛𝑡
@VLC-2019 ∆𝐿
5.4.3 Sampled-Data Interpolator
❖ Reference Word Interpolator for Circles
During circular interpolation, tangential velocity, V should be
held on the circular path. The individual velocity of the axes is
defined by equations:
𝑉𝑥 (𝑡) = 𝑉𝑠𝑖𝑛𝜃(𝑡) (5.39)
𝑉𝑦 (𝑡) = 𝑉𝑐𝑜𝑠𝜃(𝑡) (5.40)
where,
𝑉𝑡
𝜃(𝑡) = (5.41)
𝑅
In the reference word interpolator for a circle, a circular path
is approximated by small line segments. The larger the
number of line segments, the better the accuracy of
40 interpolation (the more computation power and iteration)
@VLC-2019
5.4.3 Sampled-Data Interpolator
𝐸𝑅 𝑖 = 𝑅 𝑖 − 𝑅 (5.48)
= 𝑋2 𝑖 + 𝑌2 𝑖 − 𝑅
ER error is accumulated with
iteration. At the 𝑖𝑡ℎ iteration, ER
can be computed approximately
using:
𝐸𝑅 𝑖 = 𝑖 𝐶 − 1 𝑅
(5.49)
where, C = 𝐴2 + 𝐵2
44
@VLC-2015
5.4.3 Sampled-Data Interpolator
➢ Radial Error (ER) and Chord Height Error (EH)
Unlike ER, chord height error, EH, is not accumulated. EH is
computed using:
𝐸𝐻 𝑖 = 𝑅 − 𝑅 𝑖 𝑐𝑜𝑠𝛼2 (5.50)
1 + 𝑐𝑜𝑠𝛼 1+𝐴
𝑐𝑜𝑠𝛼2 = =
2 2
Then,
1+𝐴
𝐸𝐻 𝑖 = 𝑅 − 𝑅(𝑖) (5.51)
2
Based on the above equations, 𝛼 should be determined such
45 that the value of ER or EH does not exceed 1 BLU
@VLC-2019
5.4.3 Sampled-Data Interpolator
➢ Euler Algorithm
In Euler algorithm, 𝑐𝑜𝑠𝛼 and 𝑠𝑖𝑛𝛼 are approximated by first-
order Taylor series expansion. Then,
𝐴 = 1, 𝐵 = 𝛼 (5.52)
𝜋
𝐸𝑅𝑚𝑎𝑥 = 1 + 𝛼2 − 1 𝑅
2𝛼
𝜋
(5.53)
𝐸𝑅𝑚𝑎𝑥 ≅ 𝛼𝑅 (for small 𝛼)
4
➢ Taylor Algorithm
1 2 (5.54)
𝐴 = 1 − 𝛼 ,𝐵 = 𝛼
2
𝜋 1 4
𝐸𝑅𝑚𝑎𝑥 = 1 + 𝛼 − 1 𝑅 ≅ 𝜋𝑅𝛼 3 /16 (5.55)
46
@VLC-2019 2𝛼 4
5.4.3 Sampled-Data Interpolator
1 4 𝛼2
𝐸𝐻(𝑖) = 𝑅 − 𝑅 𝑖 1 − 𝛼 = 𝑅 − 𝑅 𝑖 + 𝑅(𝑖) (5.56)
4 8
𝛼2 (5.57)
𝐸𝐻𝑚𝑎𝑥 =𝑅
8
➢ Tustin Algorithm
1 − 𝛼 Τ2 2 𝛼
𝐴= 2
,𝐵 = 2
(5.58)
1 + 𝛼 Τ2 1 + 𝛼 Τ2
𝐸𝑅 𝑖 = 0 (5.59)
𝑅
47
𝐸𝐻 𝑖 = 𝑅 − (5.60)
@VLC-2019 1 + 𝛼 Τ2 2