Servo Drive Systems_Chapter 4_part1

Chapter 4Trajectory Control 1 @VLC-2015 4.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 𝑡𝑓 𝑡 Assuming that 𝐽𝑀 : inertial moment of motor 2 @VLC-2015 𝑇𝑀: torque of motor 4.1 Point-to-Point Control Note that the problem has plenty of solutions.2) Then the motion trajectory will be the third order form: 𝑞 𝑡 = 𝑎3 𝑡 3 + 𝑎2 𝑡 2 + 𝑎1 𝑡 + 𝑎0 3 @VLC-2015 (4. we include an optimal criterion: the least consuming energy  Mathematic description: 𝐽𝑀 𝜔 = 𝑇𝑀 where 𝑡𝑓 𝜔 0 subjec to: 𝑡 𝑑𝑡 = 𝑞𝑓 − 𝑞𝑖 𝑡𝑓 TM 2𝑑𝑡 0 (4. Therefore.1) → 𝑀𝐼𝑁 The general solution has the following form: 𝜔 𝑡 = 𝑎𝑡 2 + 𝑏𝑡 + 𝑐 (4.3) . 𝑎2 . 𝑎3 .5) We need to determine 4 coefficients: 𝑎0 .t.1 Point-to-Point Control The velocity will follow the second order form: 𝑞 𝑡 = 3𝑎3 𝑡 2 + 2𝑎2 𝑡 + 𝑎1 (4.4. 𝑎1 . We have the following equations: 𝑎0 = 𝑞𝑖 𝑎1 = 𝑞𝑖 𝑎3 𝑡𝑓 3 + 𝑎2 𝑡𝑓 2 + 𝑎1 𝑡𝑓 + 𝑎0 = 𝑞𝑓 3𝑎3 𝑡𝑓 2 + 2𝑎2 𝑡𝑓 + 𝑎1 = 𝑞𝑓 4 @VLC-2015 (4.6) .r.4) And the acceleration changes w. the first order rule 𝑞 𝑡 = 6𝑎3 𝑡 + 2𝑎2 (4. when 𝑡 = 0 𝑎𝑛𝑑 𝑡 = 1 5 The disadvantage of this rule is the maximum accelerations at the initial and final point. 𝑞𝑓 = 𝜋. when 𝑡 = 1/2 And the acceleration gets its the maximum value: 𝑞𝑚𝑎𝑥 = 6𝜋. the initial and final velocity 𝑞𝑖 = 𝑞𝑓 = 0 From the above equations.1 Point-to-Point Control For example: 𝑞𝑖 = 0. 𝑡𝑓 = 1 . we find the solutions: 𝑎0 = 𝑎1 = 0 𝑎2 = 3𝜋. @VLC-2015 .4. 𝑎3 = −2π The velocity gets the its maximum value: 𝑞𝑚𝑎𝑥 = 3𝜋/2. 4.1 Point-to-Point Control 𝑟𝑎𝑑 𝑞 3𝜋 𝑞𝑓 𝑞𝑚 0 𝑞𝑖 0 𝑞 𝑟𝑎𝑑/𝑠 0 1 3𝜋/2 𝑡𝑐 𝑡𝑚 𝑡𝑓 3𝜋/2 𝑞𝑐 𝑟𝑎𝑑/𝑠 2 6𝜋 6 @VLC-2015 −6𝜋 1 𝑞 𝑡𝑐 𝑡𝑓 𝑞𝑐 𝑡𝑐 −𝑞𝑐 𝑡𝑓 . the time of acceleration and deceleration is equal. That means: 𝑞𝑚 − 𝑞𝑐 𝑞𝑐 𝑡𝑐 = 𝑡𝑚 − 𝑡𝑐 (4. 𝑞𝑐 is the position at the time 𝑡𝑐 when the motor accelerates at 𝑞𝑐 (𝑟𝑎𝑑/𝑠 2 ) 7 @VLC-2015 . the velocities at the transition points have to be smooth. at 𝑡𝑚 = 𝑡𝑓 /2 (4.1 Point-to-Point Control The famous trajectory that is usually used in most industrial controllers is Blended Polynomial with the trapezoidal velocity. We have: 𝑞𝑚 = (𝑞𝑓 −𝑞𝑖 )/2.8) where.4.7) In order to guarantee the trajectory is continuous. Assuming that 𝑞𝑖 = 𝑞𝑓 = 0 . 13) .12) 𝑡𝑓2 4 𝑞𝑓 −𝑞𝑖 𝑡𝑓2 . 𝑞𝑐 ≥ 4 𝑞𝑓 −𝑞𝑖 It is obvious that if 𝑞𝑐 = 8 @VLC-2015 (4. finding 𝑡𝑐 ≤ 𝑡𝑓 /2. (4.10) 𝑞𝑐 𝑡𝑐2 − 𝑞𝑐 𝑡𝑓 𝑡𝑐 + 𝑞𝑓 − 𝑞𝑖 = 0 Given 𝑞𝑐 . we have: 𝑡𝑓 1 𝑡𝑓2 𝑞𝑐 − 4(𝑞𝑓 − 𝑞𝑖 ) 𝑡𝑐 = − 2 2 𝑞𝑐 where. 𝑞𝑓 .9).9) 𝑞𝑐 = 𝑞𝑖 + 𝑞𝑐 𝑡𝑐 2 From Eq. 𝑡𝑐 = 𝑡𝑚 = 𝑡𝑓 2 (4.8) & (4.11) (4.4. 𝑡𝑓 . we got: (4.1 Point-to-Point Control We have: 1 2 (4. 𝑞𝑖 . given 𝑞𝑖 .12) we calculate and choose 𝑞𝑐 . (4.11). The rated speed is 2000 (RPM).14) 𝑡𝑓 − 𝑡𝑐 ≤ 𝑡 ≤ 𝑡𝑓 Example: Determine the Blended Polynomial trajectory in order to control the position of the motor in 20 rounds within 2s.1 Point-to-Point Control Therefore. the trajectory is determined by a piecewise function 1 𝑞𝑖 + 𝑞𝑐 𝑡 2 2 𝑞 𝑡 = 𝑞𝑖 + 𝑞𝑐 𝑡𝑐 0 ≤ 𝑡 ≤ 𝑡𝑐 𝑡𝑐 𝑡− 2 1 𝑞𝑓 − 𝑞𝑐 𝑡𝑓 − 𝑡 2 𝑡𝑐 ≤ 𝑡 ≤ 𝑡𝑓 − 𝑡𝑐 2 (4. 𝑞𝑓 and 𝑡𝑓 . then calculate 𝑡𝑐 from Eq. 9 @VLC-2015 . from Eq.4. Finally. (4. 4. and speed 𝜔 are given by 2𝜋 𝛼= (𝑟𝑎𝑑) 𝑝𝑝𝑟 𝜃 = 𝑛𝛼 (𝑟𝑎𝑑) 𝛼 𝜔= (𝑟𝑎𝑑) 𝜏𝑑 where 𝑛 is the number of pulses (4. pulses must be generated at a steady rate 10 @VLC-2015 .2 Application for Step and DC motors  Step motor The motor step angle: 𝛼. position: 𝜃.15) 𝜏𝑑 is the delay time To rotate a stepper motor at a constant speed. control of the acceleration and deceleration is needed. and the resolution of the time delay between these steps is given by the frequency of the timer.4. Discrete steps control the stepper motor motion. 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. 11 @VLC-2015 .2 Application for Step and DC motors  Linear speed ramp To start and stop the stepper motor in a smooth way. speed and position. Figure in previous section shows the relation between acceleration. 𝜏𝑡 = (s) 𝑓𝑡 0 𝜏𝑑0 We have: t1 𝜏𝑑1 t2 𝜏 t3 t4 𝑑2 𝜏𝑑0 = 𝑐0 𝜏𝑡 𝜏𝑑1 = 𝑐1 𝜏𝑡 12 @VLC-2015 t 𝜏𝑑2 = 𝑐2 𝜏𝑡 (4.16) .4.2 Application for Step and DC motors ω Assuming that the time delay is generated by a timer with a frequency 1 𝑓𝑡 (Hz). 2 Application for Step and DC motors The position at the time 𝑡 is given by equation: 𝑡 1 2 𝜃 𝑡 = 𝜔 𝜏 𝑑𝜏 = 𝜔𝑡 2 0 Assuming that 𝜃 𝑡 is corresponding to the n’th pulse.18) .4.17) And the time delay between two steps is: 13 @VLC-2015 𝑐𝑛 𝜏𝑡 = 𝑡𝑛+1 − 𝑡𝑛 = 2𝛼 ( 𝑛 + 1 − 𝑛) 𝜔 (4. then 1 2 𝜃 𝑡 = 𝜔𝑡𝑛 = 𝑛𝛼 2 ⇒ 𝑡𝑛 = 2𝑛𝛼 𝜔 (4. 4.2 Application for Step and DC motors Finally the expression for the counter delay is found: 1 2𝛼 𝑐𝑛 = ( 𝑛 + 1 − 𝑛) 𝜏𝑡 𝜔 (4. we use the Taylor series approximation 14 @VLC-2015 .19) When 𝑛 = 0: 1 2𝛼 𝑐0 = 𝜏𝑡 𝜔 (4.20) ⇒ 𝑐𝑛 = 𝑐0 ( 𝑛 + 1 − 𝑛) In order to reduce the burden of calculations. 4.21) This calculation is much faster than the double square root. 2𝑐𝑛−1 ⇒ 𝑐𝑛 = 𝑐𝑛−1 − 4𝑛 + 1 (4.2 Application for Step and DC motors 𝑐𝑛 𝑐𝑛−1 𝑐0 ( 𝑛 + 1 − 𝑛) 4𝑛 − 1 = ≈ 𝑐0 ( 𝑛 − 𝑛 − 1) 4𝑛 + 1 Finally. 15 @VLC-2015 . but introduces an error at n =1.676. A way to compensate for this error is by multiplying 𝑐0 with 0. 4.  The velocity trajectory is therefore trapezoidal for a long move and triangular for a short move where maximum velocity was not reached. @VLC-2015 . 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.2 Application for Step and DC motors 16  DC servo motor A trajectory generation algorithm is essential for optimum motion control.  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. 4.2 Application for Step and DC motors velocity velocity limit Slope = Accel. limit short move long move time  The SetMove subroutine is invoked once at the beginning of a move to calculate the trajectory limits. (4.14)) 17 @VLC-2015 .  The DoMove routine is then invoked at every sample time to calculate new “desired” velocity and position values as follows (from Eq. 2 Application for Step and DC motors 𝑣𝑘 = 𝑣𝑘−1 + 𝑎𝑐𝑐𝑒𝑙.4. and the size of the movement. acceleration. 𝑇 2 2 In position mode. the actual shape of the velocity profile depends on the values of velocity. The block diagram of position controller Trajectory Generator Kp PI DC Velocity feedback 18 Position feedback @VLC-2015 1 𝑠 . 𝑇 (4.22) 1 𝑞𝑘 = 𝑞𝑘−1 + 𝑣𝑘−1 𝑇 + 𝑎𝑐𝑐𝑒𝑙. There are three cases that usually use in trajectory generator: @VLC-2015 .3 Continuous Path Motion In many applications. the servo systems (Robots. 𝑞 𝑞2 Π1 Π2 𝑞3 Π3 ⋯ 𝑞1 = 𝑞𝑖 𝑡1 = 0 19 ⋯ 𝑞𝑁 = 𝑞𝑓 𝑡2 𝑡3 𝑡𝑁 = 𝑡𝑓 The problem is to determine the trajectory that goes through N path points with the constraint conditions.4. CNC) have to be controlled in a predefined continuous path. in the case of the velocities are predetermined Assuming that the trajectory has N path points. 𝑞 𝑡 . 𝑞 𝑡 . at the path points are predetermined  The velocities.3 Continuous Path Motion  The velocities. 𝑞 𝑡 . at the path points are calculated by a specific criterion.23) . For example. 𝑞𝑘+1 ) . then the third order polynomial is adopted to interpolate two adjacent points ( 𝑞𝑘 .  Guarantee the continuity of acceleration.4. Each polynomial has to satisfy the following conditions: Π𝑘 𝑡𝑘 = 𝑞𝑘 Π𝑘 𝑡𝑘+1 = 𝑞𝑘+1 Π𝑘 𝑡𝑘 = 𝑞𝑘 20 @VLC-2015 Π𝑘 𝑡𝑘+1 = 𝑞𝑘+1 (4. at the path points.