Blending in pick and place applications

Transcription

1 Blending in pick and place applications X.G.P. Schuurbiers DCT 7.9 DCT report Technische Universiteit Eindhoven Department Mechanical Engineering Dynamics and Control Technology Group Eindhoven, January, 7

2 Abstract At the start of this internship a conventional pick and place algorithm was available to generate pick and place trajectories. To get a small motion time, time optimal point to points are constructed on the linear segments using the theory of Lambrechts, which was extended by van Dijk, while monitoring Cartesian constraints on velocity, acceleration and jerk using the procedure described by Macfarlane. The major disadvantage of this conventional algorithm is the use of a blend function during blending. On of the boundary conditions of this fixed blend function is, that the track velocity at the beginning of the blend should be equal to the track velocity at the end of the blend, i.e. no track acceleration can be attained during blending. When an application needs trajectories containing small linear segments with large blends, the manipulator must maintain that low velocity reached at the end of such a small segment during the whole blend path and this will cause more process time than necessary. In this report, a new blending algorithm is developed, which can have track acceleration during blending. In this case, the manipulator can accelerate during blending and this will decrease motion time. Theoretical results prove that the generated motion time will be in fact the time optimal one. With this new pick and place algorithm simulations with different trajectories are performed and compared to the conventional pick and place algorithm in generated motion time and computational time.

3 Contents Introduction 3. Introduction Motivation Problem Statement Contribution of this internship Outline of this report Literature overview on pick and place motions 7. Introduction Literature on pick and place motions High level path planner, performance criteria and point to point motions Blending Discussion A new blending algorithm 3. Introduction Requirements Time optimal motion during blending Repositioning the blend intersection points The update algorithm for approving the blend path Switching of the largest blend time Change of the value of τ Negative and zero segment direction The final update algorithm The final update laws The new pick and place algorithm 6 4. Introduction Trajectory generation Part one: Check user inputs Part two: Calculate all segment- and blend parameters Part three: Interpolating and plotting of the whole trajectory Simulations Introduction Simulations Simulation : a -D trajectory of five waypoints Discussion Conclusions and Recommendations 4 6. Conclusions Recommendations

4 CONTENTS A Calculation of the time increments for a third order profile 43 A. Acceleration phase A. Deceleration phase A.3 Constant velocity part B Different profiles along the linear segment 47 B. Acceleration phase B. Deceleration phase B.3 Constant velocity part and iteration C Get the same blend time in each coordinate direction 49 D The blending function of Lloyd and Hayward 5 D.. Constraints during blending E Numerical example of how the final algorithm works 58 F The experimental update law 64 *

5 3 Chapter Introduction. Introduction In this chapter, robotic devices are introduced and the problem statement is discussed. Robotic devices do not only consist of the robot machinery itself, but also include the trajectory generator and the motion control system. In this chapter we start with the motivation of this internship, whereafter the problem statement is introduced. In the last sections of this chapter, the contribution of this internship is mentioned and the outline of this report is discussed.. Motivation A robotic device is a mechanical structure containing two important aspects:. It is driven by several actuators and. It gets its information through sensors. The robot performs a certain task and has a workspace. There are two major categories in robotic machines, namely the mobile robot and the fixed base manipulator. A mobile robot is moving around in its workspace, whereas a fixed base manipulator stays in a fixed position all the time. Both categories could have similar tasks. Examples of a task that a robot could do, are picking dies of a wafer in semiconductor industry or moving patients in the next generation of MRI-scanners. The first robotic like machines came in the early 97s and were microcomputer-controlled robots. Since the introduction in the early 97s, the main goal is to create autonomous systems that could perform tasks without any human intervention. The user just specifies a task that the robot should do and the robot just does so repetitively until it is shut down. In order to achieve an autonomous robot two major aspects can be distinguished, see Fig..:. Robot motion planning and. Robot motion control. This report focuses on the part of robot motion planning. We can distinguish two parts in robot motion planning, namely:. Path planning and. Trajectory generation. Path planning gets the robot from its initial configuration to its final one, without hitting any obstacle in its workspace. Trajectory generation generates the trajectories for this path planning. These

6 .3. Problem Statement 4 Path planning Trajectory generation Controller Actual System Sensors Robot motion planner Robot motion control Figure.: Robot motion planner and robot motion control trajectories concern position-, velocity-, acceleration- and jerk profiles in order to satisfy a certain performance criterion. However, trajectory generation must also keep in mind the physical limitations on velocity, acceleration and jerk. In industrial applications, like the semiconductor industry, actuators must have as less wear as possible to save costs. Bang-bang accelerations, which result in discontinuous torque levels and actuator wear are unwanted phenomena, so also jerk profiles are included in the trajectory generation..3 Problem Statement There are two ways in which movements of a robot can be specified:. Movements can be specified in certain points and. Movements can be specified in following a path exactly. In the first task the user specifies a number of points, waypoints, which should be called-on by the robot. This is called a pick and place operation. The second task represents a movement where the robot should follow the generated path exactly. This operation is called a path constraint operation. This report focuses only on the first class of tasks, namely the pick and place tasks. These tasks are characterized by an initial position, where the robot picks a product and a final position, where the robot places this product, for example picking dies of a wafer or a crane transporting goods. A lot of freedom exists between the pick and the place position. Algorithms for pick and place operations can be divided into two parts. First the user specifies the pick and place position and maybe some additional points to guide the manipulator safely through its workspace. This is called path planning. After the path planning is finished, the actual trajectories are generated to follow that planned path. This is called trajectory generation. In this report, the focus is not on path planning algorithms. We assume a user who specifies the pick and place position and some intermediate points where the manipulator should travel through in order to avoid collisions in its workspace. This user can also be a path planning algorithm, but in this report by user is meant the costumer that uses the pick and place algorithm. In most applications, the user wants the robot to follow the generated path as fast as possible, to reduce costs of his process. This means that the generated trajectories should be time optimal. At the start of this internship, a conventional pick and place algorithm is available which interpolates straight lines between the defined waypoints and uses a polynomial function to construct the blend paths between those linear A trajectory is time optimal, when it generates the lowest motion time.

7 .4. Contribution of this internship 5 Figure.: Example of a trajectory where the conventional pick and place algorithm loses motion time due to the way of blending that is implemented segments. This algorithm can generate time optimal motions when the manipulator travels on straight lines, but can be very slow during blending. So this leads to the following problem statement: Enhance the currently available pick and place algorithm, by investigating new possibilities of blending. Select the best option and implement that option in the conventional pick and place algorithm..4 Contribution of this internship The major disadvantage of the available pick and place algorithm of van Dijk is the way of blending that is proposed. One of the constraints during blending is the track velocity of the manipulator. The track velocity can not change during blending from one segment to the other. This means, that when a trajectory contains small straight lines, where the robot has not enough space to reach maximum track velocity and large blend radii, where the track velocity can not increase, the present algorithm of van Dijk is not fast enough, see for a trajectory example Fig... The manipulator start its pick and place motion at the pick position in (, ) with zero track velocity. After the product is picked, the manipulator accelerates respecting acceleration and jerk constraints, but due to the short length of the first linear segment, the end velocity that can be reached is not very high. This low track velocity needs to be maintained during the whole blend, because the track velocity can not change during blending. If more combinations of short linear segments and large blends follow, the manipulator must keep a low track velocity throughout the whole trajectory, which results in a higher motion time. track means tangent to the generated path.

8 .5. Outline of this report 6 This report focusses on developing a new blending algorithm, that allows the manipulator to accelerate or decelerate during blending, while respecting all actuator constraints on velocity, acceleration and jerk. Theoretical results prove, that the blend time that is generated, is the time optimal one, because during blending always at least one actuator constraint is active and the lowest blend time is generated. So combining this time optimal blend algorithm with the already available time optimal motions on linear segment, we are capable of developing a new pick and place algorithm that will generate the lowest possible motion time on a given path..5 Outline of this report In chapter, a literature overview is presented, discussing different methods of trajectory generation in general and several blending methods. Chapter 3 contains the new algorithm for blending. In chapter 4 the total new pick and place algorithm is discussed. Chapter 5 contains simulations. In chapter 6 conclusions are drawn and recommendations are made.

9 7 Chapter Literature overview on pick and place motions. Introduction This chapter contains a literature overview on pick and place motions. It discusses several ways of generating trajectories for these tasks. In the first subsection three important aspects are discussed before the actual trajectory generation can be done. The first aspect is the high level path planner who defines points which the manipulator should vist. The second aspect is which performance criterion should be fulfilled. The last aspect is how to generate time optimal motion on linear segments. In the second subsection of this chapter, several ways of blending are discussed.. Literature on pick and place motions.. High level path planner, performance criteria and point to point motions A pick and place motion represents a movement of a robot where a high level of freedom exists on the path between pick and place location. In the early 97s people calculated trajectories using the equations of motion of the manipulator. Pfeiffer and Johanni introduced a method to transform these equations of motion into a one degree of motion using the path coordinate s, see Fig.. []. This decreases the computational time enormously. In some cases, the user, who specifies the motion, acts a high-level path planner []. A high level path planner defines two points, namely the pick point and the place point and the objective is to travel between these two points as fast possible, without hitting any obstacle in the workspace. In order to avoid these obstacles, the user has the opportunity to define extra points to get the manipulator around these obstacles, see Fig... Applications of a pick and place operation are for example picking and placing dies in semiconductor industry, movements of containers in a dock or movements between two welding operations. In, for example picking and placing dies, it is possible to get an update of the picking position. This means that in some applications the trajectory generation must happen online. Therefore, low computational time is required [3]. In order to reach that low computational time, a linearization of the complex robot dynamics is used by Park, to determine the trajectory generation [4]. Also assumptions are made considering the links of the robot. Zah used a simplification by treating the links of the manipulator as simple straight lines [5]. Another way of avoiding high computational time is calculating with joint variables in stead of the normal pick and place configuration. This transformation is called the inverse kinematic trans-

10 .. Literature on pick and place motions 8 Figure.: A manipulator with the path coordinate s Figure.: An example of a pick and place trajectory with obstacles present in the workspace of the manipulator. Point A represents the pick point, point B the place position. I and I are extra waypoints to avoid obstacles in the workspace formation [6]. When the trajectory is calculated in joint space, only the waypoints that are defined in Cartesian space need to be transformed to joint space. If the trajectory is calculated in Cartesian space, all calculated splines need to be transformed to Cartesian space, which takes more computational time. When a pick and place motion is executed, some performance criterion has to be fulfilled. There are two major performance criteria, namely: Energy optimality [7] and Time optimality [8]. When energy optimality is chosen, the trajectory generator tries to minimize the energy needed to travel from the pick position to the place position. In the case of time optimality, the trajectory generation tries to travel as fast as possible between pick and place position, while still respecting physical constraints. For productional or packaging manipulators a high throughput is desired such that costs of these processes are minimized. Therefore, the most common performance criterion for pick and place motions is the minimum time criterion []. Point to point motions are motions getting the manipulator from the pick position to the place position. Macfarlane [9] used fifth order functions (quintics) in order to travel between two points. In this way movements are created, which are third order continuous. This motion is near time-optimal [9]. Lambrechts introduced analytical methods to obtain time optimal motions on straight lines. Those position functions were third order, so that the physical constraints on velocity, acceleration and jerk still can be respected. By defining seven increments in time, Lambrechts could calculate the time optimal movement analytically []. Some applications require more than two points as input. For this kind of applications mostly splines are used. A spline function of order k is defined to be a piecewise polynomial function of order k on some (finite or infinite) interval with (k - ) continuous derivatives []. Using this definition, the lowest possible order of a spline would be third order, to guarantee a smooth acceleration profile. This is needed, because otherwise the acceleration profile would be discontinuous and also the torque in the actuator would be. This is physically impossible [6]. Also higher order

11 .. Literature on pick and place motions 9 splines are used, to even get a smooth jerk profile. This gives the actuator less wear [9], but increases computational time [4] as well as motion time. A simplification in trajectory generation is made by dividing the path in only linear segments. Blending then occurs between two of those linear segments in order to prevent discontinuities in the velocity profile. Because of that blending, the manipulator can not exactly reach every point on the generated path, unless it stops at each point and then move in the direction of the next point. This is of course very slow, so some sort of blending should be done and the manipulator can not follow the generated path exactly. But for most applications, this is allowed, since we are dealing with pick and place operations, where mostly only high accuracy is desired at the pick- and place position... Blending A method for blending could be motion blending. It is very useful for combining movements that come after the motion process has begun and with no rigorous demands on trajectory positions. The idea is simple. Having a motion from A to B and motion from B to C, the blended motion from A to C over B will start as motion from A to B and somewhere on the path smoothly transforms into motion to C without reaching point B. The disadvantage of this method is obvious. There is no guarantee where the motion point will be during the transition from AB to BC, but this is not always needed. This method is very popular in robotics for pick and place applications or for rounding motions on corners []. In motion blending Boric distinguishes two approaches: The first one is motion superposition. It is the simplest blending method, two trajectory generators are combined and their results are added together. The result represents the blending trajectory. It sounds simple but there are several critical points, like the trajectory generator code is run twice in one motion sample or the final position of the manipulator is still unknown [3]. Also no information is provided of how actuator constraints can be respected. Another approach is to connect the two motions with a polynomial function defined by the initial and final conditions of the blend. Having values of position, velocity and acceleration at the initial blend point and knowing the desired values at the second blend point, a smooth polynomial function can be defined to connect both points [4].The problems in this solution are the inability to control maximum values of acceleration and velocity during the transition phase. Additionally the uncertainty of trajectory is even greater than in the motion-superposition method []. Paul [5] and Taylor [6] introduced such a polynomial blending function to blend between segments. However they could not control the transition shape and assumed perfect tracking by the manipulator at the intersection points between the blends and the linear segments. Lloyd and Hayward [4] introduced a blending function, which is of fifth order to respect the boundary conditions on track position, track velocity and track acceleration on both linear segments. In this way, the problem of perfect tracking in the intersection points on the linear segments and the blends is solved due to the boundary conditions. They also introduced a way to control transition shape, by added an extra order to their blending function. Macfarlane [7] used the above described method of Lloyd and Hayward to blend from one segment to the other and extended this approach by bounding the maximum blend velocity by respecting the maximum acceleration and jerk due to the actuators. She came up with analytical results to determine the maximum allowable blend velocity while respecting the actuator constraints. This is however not time optimal, because during the blend, analytical results only guarantee actuator constraints at the initial and final blend point. In the rest of the blend path no actuator constraint activity can be forced.

12 .3. Discussion Segment Segment Figure.3: Wolters blend procedure. The initial blend path () is not approved, so both blend velocities are multiplied with a calculated factor c t. Then the blend path is approved (). Wolters [8] found a solution to make it possible to get a time optimal motion during blending. He introduced two velocity vectors, v to represent the end velocity on a previous segment and v to represent the initial velocity on the next segment. He could calculate a constant, c t, which scales both velocities v and v componentwise, by forcing the blend path, which is a function of v and v, to contact the sphere around the waypoint, see Fig..3. In this case, it is possible for the manipulator to accelerate or decelerate on the track to keep one constraint active throughout the whole time span. Russel had a different way of blending from one segment to the other. He did not use spheres to define where the robot can deviate from the calculated path, but a precomputed time τ. At τ seconds before the scheduled arrival at the waypoint the robot starts to blend to the next segment. At τ seconds after the scheduled arrival at the waypoint the manipulator should be on the next segment [9]. However, he assumed a uniform track velocity during blending and used a constant acceleration vector..3 Discussion The present pick and place algorithm uses the above discussed theory of Lambrechts to generate time optimal motions on linear segments. However the approach of Lambrechts only holds for point to point motion with begin and end velocity equal to zero. To achieve a fast motion time, we do not want the manipulator to stop at the beginning and ending of every linear segment, so van Dijk extended the approach of Lambrechts, so that time optimal point to points with begin and end velocity unequal to zero can be calculated. The blending function that is used is the blend function of Lloyd and Hayward, which does not allow any track velocity in the blend. The procedure described by Wolters is a better approach to construct time optimal blend paths. However recall from chapter, that the magnitude of the blend radius determines what the maximum blend space is, i.e. the blend must take place in the predefined sphere, not contact it. That is why the procedure of Wolters can not be used and a new blending method should be developed. This is done in the next chapter.

13 Chapter 3 A new blending algorithm 3. Introduction The newly developed algorithm should be able to determine a time optimal trajectory for the manipulator to follow in the blend. To accomplish this, a new blending method should be developed and a new way of monitoring actuator constraints. In the first section, six requirements are introduced which the algorithm should fulfill to be applicable in industry. In the second section, the theory behind time optimal motion during blending is discussed. In the third section, the repositioning of the intersection points is discussed if the initial blend path is not approved. In the fourth and last section an update algorithm is developed which will be called on if the initial blend path should be recalculated. First the requirements of the new pick and place algorithm are presented. 3. Requirements There are six requirements which the newly developed pick and place algorithm should satisfy, namely:. The position profile should be third order continuous. At least third order splines are necessary, because the acceleration should be a smooth function in order to have no discontinuity in the torque level. This kind of discontinuities can not be achieved by an electric motor, due to the internal dynamics. If second order position profiles are used, this results in fast actuator wear and position errors.. Generate the path that corresponds with the specified waypoints. The algorithm should be able to generate a trajectory through the waypoints specified by its user. Along the trajectory path, tracking is not that much important in pick and place applications. However in a repetitive motion between pick and place point, high accuracy at the pick point (the beginning) and the place point (the ending) is desired. 3. The trajectory should be time optimal. An important issue in for example the semiconductor industry is Units per Hour. In order to achieve maximum throughput, the trajectory time should be minimized. To achieve this, maximum velocity, maximum acceleration or maximum jerk should be active throughout the path. 4. The algorithm should have the following inputs: the user defines certain waypoints and blendradii at those waypoints. Using these inputs, the algorithm should calculate the time optimal path. Because the application is a pick and place one, a low computational time is required. 5. The calculation of the desired trajectory is offline. This means, that the whole path is known, before the manipulator starts its process. Therefore a low computational time is required. 6. The constraints should be Cartesian constraints, because this was also the case in the conventional pick and place algorithm.

14 3.3. Time optimal motion during blending Using these requirements, a new algorithm is developed in chapter 4. First a new blending method is discussed in the next section. 3.3 Time optimal motion during blending The blending function of Lloyd and Hayward consists of a polynomial function with a fixed start and end point, set by the waypoints and the blend radius. On these points, boundary conditions on velocity and acceleration can be added to maintain smooth motion profiles during blending, see Appendix D The new blending algorithm is not a polynomial function, but uses the theory of time optimal motion on linear segments to make a time optimal motion during blending. This time optimal motion during blending is visualized in Fig. 3. as an example. Here we have two linear segments, segment in the e -direction and segment in the e -direction. Both segments are long enough for the manipulator to reach the maximum allowable track velocity, v max,all. This v max,all is determined by mapping the Cartesian constraints on the segment direction and taking the minimum. At the end of segment, the manipulator travels with v max,all, in the e -direction. On the second segment, we assume that the manipulator travels with v max,all, in the e -direction, see Fig. 3.(a). So during blending, the velocity should be blended from v max,all, to v max,all,. Both vectors can be decomposed in each coordinate direction, resulting in a velocity difference that needs to be overcome per coordinate direction, see Fig. 3.(b). Acceleration- and jerk curves can be constructed that overcome these velocity differences, where always either the acceleration or the jerk constraint is active. This is done for all coordinate directions, resulting in at most six acceleration curves, see Fig. 3.(c). The amplitude of both the acceleration- and the jerk curves are equal to the maximum in that coordinate direction, so at least one Cartesian constraint will be active throughout the whole time span of the blend. However it is possible that the velocity difference along the X-axis is much larger than along the other axes. So the blend time in X-direction is also much larger than on the time used for blending in the other directions. In order to assure a smooth blend, the longest blending time of all coordinate directions is taken as the overall blending time for all coordinate directions. The acceleration and jerk amplitudes needed for the other directions to make the blend, are multiplied with a scaling factor to overcome the same velocity difference during a longer time period, see Appendix C. All scaled acceleration curves are integrated twice, to calculate the blend path in that coordinate direction. That blend path has a start and end point which are depending on v blend,ing and v blend,outg, see (3.) and Fig. 3.: p a = p e v blend,ing t blend p b = p + e v blend,outg t blend where p is the waypoint where the blend is made, v blend,ing is the ingoing blend speed, v blend,outg is the outgoing blend speed and t blend is the maximum of all blend times per coordinate direction. The magnitude of both v blend,ing and v blend,outg is controlled by comparing the space that the user gives the manipulator to blend with the space that the manipulator actually needs to overcome the total velocity difference. The user defines a blend radius, ε user, where the manipulator may deviate from its linear segments in order to blend to the next. The region where the robot may blend to the next segment can be defined by a sphere, with p as its center point and this ε user. This is called the (3.)

15 3.3. Time optimal motion during blending 3 a) x x e y e y v max,all, v max,all, b) The velocity difference per coodinate direction x x y v max, all,, y v max, all,, x y v max, all,, x v max, all,, y c) Constructing time optimal acceleration and jerk profiles and getting equal blend times in both directions a a max,x j a max,x t t blendy t blendx t tblendx Figure 3.: Blend procedure visualized

16 3.4. Repositioning the blend intersection points 4 e Segment p a ε user p p b Segment e Figure 3.: Visualization of both intersection points in a segment/blend combination feasible region. The space that the manipulator actually needs can be defined by (3.): ε blend = max( p p a, p b p ) (3.) where p is the end point of the next segment and p a and p b as defined in (3.). The calculated blend path is approved if p a and p b are located within the feasible region. However, if v blend,ing or v blend,outg are too large, the corresponding intersection point will move outside the feasible region. So we need some sort of criterion to check if the calculated blend path can be approved and how to adapt v blend,ing and/or v blend,outg when the calculated blend path is rejected. When (3.) is examined, a reduction of the blend velocities will lead to a movement of the intersection points towards p. This means, that by reducing the blend velocities, p a and p b will move in the feasible region and the calculated blend path will be approved. If we want to approve the calculated blend path, we need to adapt v blend,ing or/and v blend,outg in such a way to get the equality: ε user = ε blend (3.3) If we now combine (3.) and (3.) and substitute those expressions in (3.3) we can define a criterion which v blend,ing and v blend,outg need to fulfill in order to approve the blend: v blend,ing t blend v blend,outg t blend = ε user = ε user (3.4) In de next section, (3.4) is discussed in more detail, but only in -D Cartesian space for simplicity. However the described procedure is easy extendible to more degrees of freedom. 3.4 Repositioning the blend intersection points The velocity difference needed to overcome in the blend, is decomposed in the X- and Y -direction, resulting in two different blend times, t blendx and t blendy. The total blend time is now taken as the maximum of both blend times, see (3.5). [t blend, i] = max(t blendx, t blendy ) (3.5) According to which coordinate direction must overcome the largest velocity difference, the index i will be for the X-direction or for the Y -direction. In the same way that the blend time for the whole

17 3.4. Repositioning the blend intersection points 5 a a a max τ > τ = a max t t blend t t blend Figure 3.3: An acceleration profile with and without a constant acceleration part motion is defined, we also can define the maximum velocity difference that needs to be overcome in the blend: [ v blend, i] = max( v blendx, v blendy ) (3.6) where i has of course the same meaning as in (3.5). The equation that needs to be substituted in (3.4) for t blend depends on whether a constant acceleration part is reached when v blend is overcomed, see Fig To determine if a constant acceleration part is reached, the next equation needs to be evaluated []: τ = (e (i)v blend,outg e (i) v blend,ing a max(i) a max(i) j max(i) (3.7) where the subscript i denotes the Cartesian constraint in the ith coordinate direction. If 3.7 returns positive, a constant acceleration part is reached in the blend. If 3.7 is non-positive no constant acceleration part is present during blending. According to if a constant acceleration part is present, a different equation should be used to determine the blend time. t blend = a max(i) + (e (i) v blend,outg e (i) v blend,ing τ > j max(i) a max(i) e (i) v blend,outg e (i) v blend,ing t blend = τ = (3.8) a max(i) If the expressions from (3.8) are substituted in (3.4), two equations are obtained which link ε blend to ε user, where ε blend is expressed in Cartesian constraints and the yet unknown v blend,ing and v blend,outg : { amax(i) v blend,ing + e } (i)v blend,outg e (i) v blend,ing = ε user τ > j max(i) a max(i) e (i) v blend,outg e (i) v blend,ing v blend,outg = ε user τ = (3.9) a max(i) Because we do not have any information considering v blend,ing and v blend,outg and thus the positions of p a and p b, four different combinations of p a and p b can occur:. Both intersection points p a and p b are located within the predefined sphere

18 3.4. Repositioning the blend intersection points 6. Only p a is located outside the predefined sphere, v blend,ing should be adapted 3. Only p b is located outside the predefined sphere, v blend,outg should be adapted 4. Both intersection points p a and p b are located outside the predefined sphere, both v blend,ing and v blend,outg should be adapted In the next part each case is described in more detail. Case : both p a and p b are located inside the predefined sphere In this case, v blend,ing and v blend,outg are small enough to blend within the predefined sphere. The algorithm takes v blend,ing equal to the end velocity of the previous segment and takes the start velocity of the next segment to be v blend,outg. Case : p a is located outside the predefined sphere, constant acceleration part present p a is located outside the predefined sphere, so v blend,ing should be adapted to get p a in the feasible region. This can be done, using the first expression of (3.9). v blend,out is known in this case, because p b is already located in the predefined sphere. So the equation that needs to be solved to determine the correct v blend,ing is a quadratic equation in v blend,ing : v blend,ing { amax(i) j max(i) + e (i)v blend,outg e (i) v blend,ing a max(i) We remove the -sign from 3. to derive an analytical update law. e (i) } = ε user (3.) if v blend,outg > v blend,ing = ε user vblend,ing + a max(i) + e (i) v blend,outg a max(i) j max(i) a max(i) v blend,ing (3.) Which leads to: v blend,ing = e (i) a max(i) a max(i) j max(i) + e (i) v blend,outg a max(i) + { amax(i) + e (i) v } blend,outg 4 j max(i) a max(i) So (3.) will give a v blend,ing that places p a on a distance of ε user from p. The other option of the -sign is: else v blend,outg < v blend,ing = e (i) vblend,ing + a max(i) e (i)v blend,outg a max(i) j max(i) a max(i) v blend,ing ε user (3.3) e (i) ε user a max(i) (3.) Which leads to: v blend,ing = e (i) a max(i) a max(i) j max(i) e (i)v blend,outg a max(i) + { amax(i) e } (i)v blend,outg + 4 j max(i) a max(i) Using these solutions in (3.) we can calculate the final position of p a and the radius of the sphere were the blend is made: t blend v blend,ing = ε user see (3.) p a = p e ε user see (3.) ε blend = p p a = p p e ε user = ε user (3.5) e (i) ε user a max(i) (3.4)

19 3.4. Repositioning the blend intersection points 7 p a is located outside the predefined sphere, no constant acceleration part is reached In this case, the second expression of the blend time in (3.9) should be used. Because of the square root, the equation for v blend,ing that needs to be solved is not quadratic, but cubic: e (i) v blend,outg e (i) v blend,ing v blend,ing = ε user (3.6) a max(i) To again make analytic expressions for v blend,ing the -sign is divided two parts, where each part has its own update rule: if v blend,outg > v blend,ing e (i) j max(i) v 3 blend,ing + e (i) v blend,outg j max(i) v blend,ing ε user = else v blend,outg < v blend,ing e (i) vblend,ing 3 e (i) v blend,outg vblend,ing ε user j max(i) j max(i) = (3.7) The expressions in (3.7) have three solutions. The maximum of the positive real solution is taken as v blend,ing and that solution places p a on the edge of the predefined sphere: t blend v blend,ing = ε user see (3.6) p a = p e ε user see (3.) ε blend = p p a = p p e ε user = ε user (3.8) Because the magnitude of v blend,ing is adapted and v blend,outg remains the same, v blend and t blend will become smaller. When the actual position of both intersection points is calculated, this phenomenon causes p b to slide. So after p a is repositioned, also the final position of p b should be checked if it is still positioned in the feasible region. Case 3: p b is located outside the predefined sphere The same reasoning can be performed in the case that p b is located outside the predefined sphere. Recall (3.8) where this time v blend,outg is unknown and should be adapted. In exactly the same way as we did in case, we obtain the following solution to reposition p b on the edge of the predefined sphere if a constant acceleration part is reached: if v blend,outg > v blend,ing v blend,outg = a max(i) e (i) The other option of the abs-sign is: if v blend,outg < v blend,ing v blend,outg = a max(i) e (i) a max(i) (e(i)v blend,ing ) + j max(i) a max(i) a max(i) j max(i) + e (i) v blend,outg a max(i) + { amax(i) e } (i)v blend,ing + 4 j max(i) a max(i) { amax(i) + e (i) v } blend,outg 4 j max(i) a max(i) Using these solutions in (3.) we can calculate the location of p b and the radius of the sphere were the blend is made: t blend v blend,outg = ε user see (3.9), (3.) p b = p + e ε user see (3.) ε blend = p p b = p p + e ε user = ε user (3.) e (i) (3.9) ε user a max(i) e (i) (3.) ε user a max(i)

20 3.5. The update algorithm for approving the blend path 8 However, due to the adaption of v blend,outg, v blend and t blend will differ from the original values when the position of p a was calculated. The final position of p a should be checked and repositioned if it slides into the forbidden region. Case 4: Both p a and p b is located outside the predefined sphere In this case both the ingoing and outgoing blend speeds are too large, because both p a and p b are located outside the predefined sphere. So both v blend,ing and v blend,outg are unknown in (3.4). First v blend,ing is rescaled, so that p a is located at the edge of the sphere. This is done with the procedure described in case. v blend,ing is known, so v blend,outg can be rescaled using the procedure mentioned in case 3 and p b is located also on the sphere. But if the position of p a and p b are recalculated, the distance from p a to p is not ε user anymore. The new blend algorithm calculates the segments and blends in chronological order. First segment is calculated, then blend, then segment etc. Say we have the situation where both p a and p b are outside the predefined sphere in blend. To calculate the blend time, we must have an initial guess of both v blend,outg and v blend,ing, see (3.8). The blend algorithm already calculated linear segment, so in order to maintain a smooth velocity profile, we set v blend,ing = v e,. But on the second segment we have no information at all, so we set v blend,outg = v max,all,. When p a is rescaled using (3.4), the blend time that is used, is based on the velocity difference between v max,all, and v e,. When p b is rescaled, the blend time is based on the velocity difference between the calculated v blend,outg and v blend,ing. v blend,ing v e, and v blend,outg v max,all,, because otherwise both p a and p b were already located in the predefined sphere. So p a is rescaled using a different blend time then p b. However the positions of both p a and p b are recalculated after both intersection points are repositioned, so p a will move. Therefore after p b is repositioned, the final position of p a should be checked and adapted if it moved into the forbidden region. 3.5 The update algorithm for approving the blend path By the derivation of the equations that reposition both intersection points two assumptions are made, which are not always justifiable and can cause a rejection of the calculated blend path although the update law repositions p a and/or p b properly:. The coordinate direction which has the maximum of the initial blend times does not have to be the same as the coordinate direction which has the maximum of the final blend times after rescaling, for example the maximum initial blend time is in the X-direction, whereas the final blend time has its maximum in the Y -direction.. The initial blend time has reached a constant acceleration part, whereas the final blend time after rescaling does not. 3. Negative or zero entries in the segment direction vectors cause the coefficients of the update rule to switch sign. First the switch of the largest blend time is discussed in the next subsection Switching of the largest blend time We assume to have p a located outside the predefined sphere, so v blend,ing is too large. Just like in the previous Chapter, we determine the maximum of the two blend times, t blendx and t blendy and take that as the blend time of the overall motion. Assume that the maximum of both blend times is the one in the Y-direction, so p a is repositioned based on t blendy. After rescaling, a new v blend can be

21 3.5. The update algorithm for approving the blend path 9 v blend, ing y y v blend, ing y y v blend, outg, ini v blend, outg, resc x v blend, outg, resc x x x a v blend, outg, ini a a a a max a max a max a max τ = t t blendx t blendy t tblendy t blendx (a) Illustration of how the maximum value of the blend time can switch of coordinate direction t t blend t t blend (b) Illustration of switching between a constant acceleration part en no constant acceleration part Figure 3.4: Visualization of adjustments that are required for the update algorithm determined, based on the unchanged v blend,outg and the rescaled v blend,ing. If we again decompose this v blend to X and Y, we may find that this time the largest velocity difference needs to be overcome in the X-direction, see for an example Fig.3.4(a). So although p a is repositioned using t blendy, the actual position is determined by t blendx. This results in a situation, where p a slides in the forbidden region. In order to solve this problem, we determine which the error is made by the switching of the largest blend time to another coordinate direction. The ratio between the blend time used to scale v blend,ing and the blend time that is actually used to calculate the position of p a is: f bt = t blendy t blendx (3.) If we substitute (3.) in (3.), we can calculate the adjustment of p a due to the switch of the maximum blend time over different coordinate directions: p a,repos = p e v blend,ing t blendy p a,act = p e v blend,ing t blendx p a = p e v blend,outg f bt t blendy So to compensate for this phenomenon v blend,ing should be multiplied with f bt. In this case, the position of p a does not change when the maximum of the blend time changes in coordinate direction. If there is no switch in coordinate direction, f bt is set to. The same reasoning can be performed if p b is located outside the feasible region Change of the value of τ Another phenomenon that can occur has to do with the existence of a constant acceleration part in calculating the blend time before and after rescaling. We assume that p b is outside the predefined

22 3.5. The update algorithm for approving the blend path sphere, which implies that v blend,outg needs to be rescaled. In order to overcome v blend we assume that we need a constant acceleration part, i.e. τ >. Because p b is outside the predefined sphere, we can guarantee that the v blend,outg that places p b on the edge of the predefined sphere is smaller than the original one. Therefore, to overcome v blend no constant acceleration part is needed. So p b is repositioned using the first expression of (3.8) for the blend time, while the actual position of p b is calculated using the second expression of (3.8) for the blend time. Due to this error, the distance from p b to p is not ε user any more and p b could slide to the forbidden region. To compensate for this extra movement of p b we must determine the factor between the blend time when a constant acceleration part exits and the blend time when no constant acceleration part is reached. The ratio between those two blend times can be calculated: f c = t blend,withaccpart t blend,withoutaccpart (3.3) So the difference between the rescaled p b and the actual position of p b is: p b,repos = p + e v blend,outg t blend,withaccpart p b,act = p + e v blend,outg t blend,withoutaccpart p b = p + e v blend,outg f c t blend,withoutaccpart (3.4) In (3.4) we can easily determine that in order to compensate for the extra movement of p b we must multiply v blend,outg with f c. This procedure can also be used in repositioning p a and when both intersection points need to be repositioned Negative and zero segment direction To determine which coordinate direction must overcome the largest velocity difference, both the ingoing- and outgoing blend velocity are decomposed to check which velocity difference is the largest. i, which is still defined as in (3.4), determines which entry of the segment direction vector is substituted in the equations to rescale the blend velocities. If one of these entries is negative of zero the update law that is used should be adapted. Two situations can occur. First the situation is discussed if the initially determined blend time reaches a constant acceleration part. Then the same update law is adapted in case no constant acceleration part is achieved. Constant acceleration part present In all update laws, both segment directions, e and e occur. These vectors can have positive, negative or zero entries on the places that correspond with the coordinate direction that has the largest blend time. When e or/and e are pointing in a negative direction, the coefficients that correspond to e and/or e in the equations that need to be solved to position p a and/or p b on the predefined sphere just switch sign. However, if a segment is tangent to either the X- or Y -axis, the segment vector ε has a zero entry. In this case, the update law decreases one order with respect to the update law that would be used when all segment vector entries are nonzero. To illustrate this, we assume that we want to reposition p a because v blend,ing is too large. We first assume that a constant acceleration part is necessary to overcome the velocity difference in the blend, i.e. τ >. The maximum blend time is in the i-th direction according to (3.5). To determine the

23 3.5. The update algorithm for approving the blend path corresponding velocity difference in the i-th direction, we determine the i-th entry in e and e. If both entries are nonzero, the update laws mentioned in the previous section are capable of recalculating v blend,ing to reposition p a on the edge of the predefined sphere. If e (i) and e (i) are both zero, both e and e are tangent to the same axis and no blend is made. So only e (i) = or e (i) = can occur when the update algorithm is used. Lets first discuss when e (i) =. We start again with the equation to place p a on the predefined sphere, see (3.5). p a = p e v blend,ing t blend The equation for the blend time differs from (3.8), because e (i) = and reduces to: (3.5) t blend = a max(i) j max(i) + e (i) v blend,outg a max(i) (3.6) Substituting (3.6) in (3.5) and rearranging terms results in the following update rule that will give the proper ingoing blend velocity when e (i) = : v blend,ing = ε user (3.7) a max(i) j max(i) + e ( )iv blend,outg a max(i) (3.7) describes the update rule that must be used, when e (i) =. The second situation that can occur, is e (i) =. In that case, the blend time will become: t blend = a max(i) j max(i) + e (i) v blend,ing a max(i) (3.8) If we substitute (3.8) in (3.5) and rearrange terms, we need to solve in case that e ()i = : e ()i a max(i) v blend,ing + a max(i) j max(i) v blend,ing ε user = (3.9) The solution of (3.9) is: v blend,ing = a max(i) e (i) a max(i) + j max(i) { } amax(i) + 4 abs( e ()i )ε user j max(i) a max(i) (3.3) If p b is outside the predefined sphere and e (i) = or e (i) =, the above discussed procedure can also be used. Then the rescaled velocity is v blend,outg. If both v blend,ing and v blend,outg are too large, first p a is rescaled, then p b. No constant acceleration part present In the previous section, we have assumed that a constant acceleration part is needed to overcome the velocity difference in the blend. If this is not necessary, the update law to reposition the intersection point(s) will differ from the above derived (3.3) and (3.7). We again have two situations, e (i) = or e (i) = The equation that we need to substitute in (3.5) for the blending time, is when no constant acceleration part is needed and e (i) =, is: { } abs e (i) v blend,outg t blend = (3.3) a max(i)

24 3.6. The final update algorithm Substituting (3.3) in (3.5) and rearranging terms we get: e (i)v blend,outg a max v blend,ing ε user = (3.3) The root of (3.3)is: v blend,ing = a max(i) e (i) v blend,outg ε user (3.33) The other situation that can occur is when e (i) =. In this case the blend time reduces to: { } abs e (i) v blend,ing t blend = (3.34) a max(i) Substituting (3.34) in (3.5)results in: abs(e (i)) a max v 3 blend,ing ε user = (3.35) which has as its solution: v blending = 3 ε user abs(e (i)) a max (3.36) The same reasoning can be performed considering repositioning p b or both intersection points. 3.6 The final update algorithm In Fig.3.5 a flowchart of the total update algorithm is presented. As input for the first blend path calculation, the end velocity of segment i is taken as initial ingoing blend velocity and the maximum allowable track velocity on segment i + bis taken as initial outgoing blend velocity. With these inputs, the first blend path and the position of the intersection points are calculated and compared with the predefined sphere. If both p a and p b are located inside the predefined sphere, the algorithm has directly v blend,ing and v blend,outg as outputs and continues to the next segment/blend combination. If both intersection point are not located inside the predefined sphere, the update algorithm determines which of the three cases described in section 3.4 is happening. The next step is to determine which coordinate direction must overcome the largest velocity difference and thus has the largest blend time. The update algorithm determines if a constant acceleration part is needed to overcome the total velocity difference. The last step is to check if the entries which correspond to the coordinate direction with the largest blend time of both segment directions are positive, negative or zero. Each possible outcome of these three steps, correspond to their own update rule The final update laws In Table 3. the final update laws are presented in case that p a needs to be repositioned, τ > and e (i) and e (i) are both nonzero. In Table 3. the final update laws are presented in case that p a needs to be repositioned, τ = and e (i) and e (i) are both nonzero. In Table 3.3 the final update laws are presented in case that p b needs to be repositioned, τ > and e (i) and e (i) are both nonzero. In Table 3.4 the final update laws are presented in case that p b needs to be repositioned, τ = and e (i) and e (i) are both nonzero. In Table 3.5 the final update laws are presented in case that p a needs to be repositioned, τ > and

25 3.6. The final update algorithm 3 v e, i max a, all, i+ v Calculate intersectionpoints pa and pb ε > and ε > ε user ε < ε ε < and user ε a < ε a ε user user b a ε user b ε b > ε user Rescale Rescale v blend, ing v blend, outg τ >? τ >? y n y n e? e? i i e? e? i i y n y n y n y n Table 3. Table 3.5 Table 3. Table 3.6 Table 3.3 Table 3.7 Table 3.4 Table 3.8 v blend, ing v blend, outg ε b < ε user? n ε a < ε user? y y Algorithm output v blending, & v blend, outg Figure 3.5: Flowchart for the update algorithm

26 3.6. The final update algorithm 4 if p a p > ε user τ > amax(i) v blend,outg > v blend,ing v blend,ing = j + e (i) v blend,outg max(i) a + max(i) f bt f c a max(i) v blend,ing > v blend,outg v blend,ing = j e (i) v blend,outg max(i) a + max(i) f bt f c s amax(i) j max(i) + e(i)v blend,outg a max(i) 4 e (i) a max(i) ε user e (i) a s max(i) amax(i) j e(i)v blend,outg e (i) max(i) a +4 max(i) a ε user max(i) e (i) a max(i) Table 3.: Update rule if p a is outside the predefined sphere, τ > and e (i) and e (i) are both positive if p a p > ε user τ = v blend,outg > v blend,ing f bt f c v blend,ing > v blend,outg f bt f c e (i) j max(i) vblend,ing 3 + e (i) v blend,outg j max(i) vblend,ing ε user = e (i) j max(i) vblend,ing 3 e (i) v blend,outg j max(i) vblend,ing ε user = Table 3.: Update rule if p a is outside the predefined sphere, τ = and e (i) and e (i) are both nonzero if p b p > ε user τ > a max(i) v blend,outg > v blend,ing v blend,outg = j (e(i)v blend,ing ) max(i) a + max(i) f bt f c amax(i) v blend,ing > v blend,outg v blend,outg = j + e (i) v blend,outg max(i) a + max(i) f bt f c s amax(i) j e(i)v blend,ing e (i) max(i) a +4 max(i) a ε user max(i) e (i) a s max(i) amax(i) j max(i) + e(i)v blend,outg a max(i) e (i) a max(i) Table 3.3: Update rule if p b is outside the predefined sphere, τ > and e (i) and e (i) are both nonzero 4 e (i) a max(i) ε user if p b p > ε user τ = e v blend,outg > v (i) blend,ing j max(i) vblend,outg 3 e (i) v blend,ing j max(i) vblend,out ε user = v blend,ing > v blend,outg e (i) j max(i) vblend,outg 3 + e (i) v blend,ing j max(i) vblend,outg ε user = Table 3.4: Update rule if p b is outside the predefined sphere, τ = and e (i) and e (i) are both nonzero if p a p > ε user τ > e (i) = v blend,ing = e (i) = v blend,ing = a max(i) j + max(i) ε user a max(i) j + abs {e() i v blend,outg} max(i) a r max(i) amax(i) e() j 4 i max(i) a ε user max(i) e() i a max(i) Table 3.5: Update rule if p a is outside the predefined sphere, τ > and e (i) = or e (i) =

27 3.6. The final update algorithm 5 if p a p > ε user τ = e (i) = v blend,ing = e (i) = ε v blending = 3 user e (i) amax ε user j max(i) e (i) v blend,outg Table 3.6: Update rule if p a is outside the predefined sphere, τ = and e (i) = or e (i) = if p b p > ε user τ > e (i) = v blend,outg = e (i) = v blend,outg = s a max(i) amax(i) j + max(i) j +4 max(i) e() i a max(i) ε user a max(i) j + e() i v blend,ing max(i) a max(i) e() i εuser a max(i) Table 3.7: Update rule if p b is outside the predefined sphere, τ > and e (i) = or e (i) = e (i) = or e (i) =. In Table 3.6 the final update laws are presented in case that p a needs to be repositioned, τ = and e (i) = or e (i) =. In Table 3.7 the final update laws are presented in case that p b needs to be repositioned, τ > and e (i) = or e (i) =. In Table 3.8 the final update laws are presented in case that p b needs to be repositioned, τ = and e (i) = or e (i) =. The reason, that no equality between both blend velocities is included in the tables that describe all update laws, is that if both vectors are equal no blend should be constructed. The manipulator just keeps on traveling in the same direction with the same track velocity. if p b p > ε user τ = ε e (i) = v blendoutg = 3 user e (i) amax e (i) = v blend,outg = ε user j max(i) e (i) v blend,ing Table 3.8: Update rule if p b is outside the predefined sphere, τ = and e (i) = or e (i) =

28 6 Chapter 4 The new pick and place algorithm 4. Introduction In this chapter the implementation of the new blend procedure and the update algorithm is discussed. The time optimal linear segments of the pick and place algorithm of van Dijk are maintained and combined with the time optimal blend paths. This results in a time optimal motion on the whole generated trajectory. The next section discusses how these time optimal trajectories are generated. 4. Trajectory generation The new pick and place algorithm consists of three major parts. The steps must successfully be finished before the final trajectory can be generated. They are listed below:. User s check. Calculation of all segment- and blend parameters 3. Interpolation of all segments and blend paths In the next section the user s check will be discussed. 4.. Part one: Check user inputs The user of the new pick and place algorithm has three inputs which he can specify. He can specify the number of waypoints and the total degrees of freedom, the magnitude of the blend radii and the blend procedure to calculate the blend path. A short description of each input is given below, whereas each input is described more in detail in the next paragraphes: The waypoints. This includes the number of degrees of freedom and the number of waypoints. The number of degrees of freedom can vary between one and six. The number of waypoints should be at least three. The blend radii. Each waypoint could have its own blend radius, except the pick and place position, they have blend radius zero. The blendcase. The user has three options for each waypoint. In case normal the blend function of Lloyd and Hayward is used, with both the start component and the halt component are, see Fig. D.6. In case special also the blend function of Lloyd and Hayward is used, but now the value of the start component is zero and the value of the halt component is one, see Fig. D.6. In case fast the blending algorithm of section 3.3 is used to calculate the blend path. In the next paragraphs each input is discussed more in detail.

29 4.. Trajectory generation 7 Defining waypoints The first input of the new pick and place algorithm is called waypoints. The degrees of freedom of the waypoints can vary from one to six (three translations and three rotations). The minimum number of waypoints to be specified is three, so the trajectory should consist of at least two linear segments and one blend. There is no maximum. If the user specifies two adjacent waypoints with exactly the same coordinates, the algorithm detects the two duplicate waypoints and removes one. Which one depends on the corresponding blend radii. Recall that the blend radius defines a sphere from which the manipulator can deviate from its path. So in case of two different blend radii and thus two different points where the manipulator can deviate from its path, the algorithm deletes the smallest one. The algorithm only deletes one waypoint when two adjacent waypoints are the same. Defining blend radii If two adjacent waypoints are so near to each other, that their specified blend radii intersect, the algorithm detects the intersecting blend radii and starts to recalculate their blend size. In this part, the ratio between both blend radii is maintained and both blend radii are reduced until they are tangent. Defining blendcase The user has three options in this input. He can choose from norm, spec or fast. In case norm or spec the blend options of the conventional pick and place algorithm are selected. If the new blend procedure needs to be selected, fast should be chosen. 4.. Part two: Calculate all segment- and blend parameters The second part of the new pick and place algorithm calculates all segment- and blend parameters. The segment parameters consist of a start and end point, the segment length and a start and end velocity for each segment. The blend parameters consists of the final position of p a and p b and the final values of v blend,ing and v blend,outg for each blend. The algorithm calculates a trajectory from pick position to place position sequentially. The end point of a linear segment is p a (except for the last segment, there is the end point the place position). But p a depends on v blend,ing, which is unknown when the algorithm calculates the linear segment, because the blend is not yet calculated. So the algorithm does not know the actual segment length. In order to determine the actual segment length, three options can be tested:. The first option starts with blend and not with segment. In that case, the ingoing blend velocity is already known and thus the segment length is known, recall (3.). The procedure to generate the trajectory would be the following: The blend iteration is started using both maximum allowable velocities of segment and. After the blend iteration is finished, the ingoing blend velocity is known and the length of segment can be calculated. The algorithm checks whether this v blend,ing can be reached on the previous linear segment. If this is the case, the algorithm can proceed to the second blend. If v blend,ing can not be reached by the previous point to point, the algorithm determines the maximum end velocity that can be reached on segment. This means, that the blend procedure must be called on twice, which causes larger calculation times, so this approach can be rejected due to the requirement of low computational time.. An other option is to add a spline from the known p ε ε user and the calculated position of p a. Instead of executing the blend procedure twice, an extra segment is added to segment, see Fig. 4.. In comparison to the procedure mentioned above, this procedure will cost less computational time, however an other bigger problem is introduced. The intersection point p a,lh, where the subscript lh stands for the intersection point that is calculated using the theory

30 4.. Trajectory generation 8 p Segment p a, lw Extra spline p a p Segment p Figure 4.: Visualization of option ; add an extra spline to avoid the initial unknown segment length. The final position of p a is always on or more towards p after rescaling. of Lloyd and Hayward, is always located at the edge of the predefined sphere, whereas p a, is located at or inside the predefined sphere. In this case, a spline should be made between p al h and p a. The algorithm knows that p al is an intersection point of the blending function of h Lloyd and Hayward and sets the track acceleration at this point to be zero, due to the boundary conditions. So the ingoing blend velocity, calculated at p a is already reached at p a,lh. Because the manipulator is not allowed to have a nonzero track acceleration and track jerk at p a,lh, the interpolation between p a,lh and p a is a spline with constant velocity, namely the velocity reached at p a,lh. At p a,lh the manipulator already needs to slow down to make the blend, so the track velocity at p a,lh is smaller than v max,all on segment. This means that during this spline, no actuator constraint is active; the velocity at p a,lh is lower than v max,all and both track acceleration and jerk are zero. So no time optimal trajectory can be generated using this procedure and this in not a good option. 3. The last option is to calculate the length of segment with the use of p a,lh as temporary end point. This least to a maximum end velocity on segment which is used as input for the blend procedure. This blend procedure gives the actual v blend,ing which places p a in the feasible region and thus the actual position of p a. The length of the previous segment and the end velocity are adapted, so that v e, = v blend,ing and the algorithm can proceed to the next segment. In this option the point to point on segment is calculated twice. It can not be possible that the actual segment length is shorter than the initial one, because p a will always be positioned on p a,lh or more towards p, which makes the actual segment length equal or larger than the initial one, see again Fig. 4.. So it can not be the case, that v e, can not be reached. The third option is the best option and that one is implemented in the total pick and place algorithm. We now can proceed and calculate all segment- and blend parameters. A flowchart of this part can be found in Fig. 4.. The algorithm starts at the pick position p and will calculate the next segment/blend combinations sequentially. Before starting at the first segment, all segment directions can already be determined. This is possible because the user has specified all coordinates of the waypoints. Also all Cartesian constraints can be mapped on those segment directions, resulting in maximum allowable velocity, acceleration and jerk values on those segments. Next segment can be calculated.

31 4.. Trajectory generation 9 Determine all segment- and blend parameters Determine pp i=i+ Y ε = i = i i= j=? N i=i- p v s s = p = i p = p s s v = v b, i blend, out, i Y ε = i or i = imax N p = p e e v = i+ p = p s e v = v a, i+ max, all, i Actuator constraints Point to point calculation v e, i = vblend, ing, i Algorithm output Y N v reachable? v >? e e v s Determine blend i=i+ Y Y Previous blend recalculated? k= Specify blendcase: - normal - special - fast i = Get Fast Blend Parameters Y v blend, ing, i v blend, outg, i ε max k Y Y ε ε des? max i max e i+ N Adapt N Y?? N Blendcase: norm / spec? v blend, ing, i ε a > ε b N N calculate new v e with v s Specify blendcase: - normal - special - fast Y Calculate blend parameters using p a, i, p b, i and v e, i Constraint violation? Adapt v blend, outg, i N i=i- calculate new with v s v e Blendcase: norm / spec? Y Recalculate blendspeed Figure 4.: Flowchart of second part of the new pick and place algorithm: Calculate all segmentand blend parameters

32 4.. Trajectory generation 3 v s, = p Segment Actual segment length v e, p a,, p lh a, ε user p p p a, p a p p b p b, Initial segment length (a) Visualization of the first segment p (b) Blend procedure of blend visualized. First the blend path is rejected (). After rescaling the blend path is approved () Figure 4.3: Motion profiles for both pick and place algorithms for the first simulation Segment In order to determine the segment length, the end point of the segment needs to be known. However the end point of the segment depends on the unknown ingoing blend velocity, v blend,ing,, recall Section(4..). To still be able to determine the segment length, the initial end point of the segment is taken as the intersection point of the blend function of Lloyd and Hayward, p a,,lh. The starting point p is known, so the initial segment length can be determined. An initial guess of the end velocity v e, is, that v max,all, can be reached. If this is not the case, a new end velocity v e,new, is calculated given the start velocity v s, and the initial segment length. v e,new, is smaller v max,all,. The result is plotted in Fig. 4.3(a). Blend The end velocity of segment, is used as ingoing blend velocity for the new blend procedure. The initial outgoing blend velocity is taken as the maximum allowable velocity on the second segment, v max,all,, because no further information is known on that segment at that time. With these velocities, the initial blend path and the positions of p a, and p b, can be determined. If p a, and/or p b, are located outside the predefined sphere, v blend,ing, and/or v blend,outg, are rescaled using the update algorithm of Fig The final blend path and the final positions of p a, and p b, are recalculated using the rescaled v blend,ing, and/or v blend,outg,, see Fig. 4.3(b). Because the final position of p a, is known, the initial segment length of segment and the end velocity of segment can be adapted. The segment length is calculated using the final position of p a,. The end velocity is made equal to the ingoing blend velocity of blend. Next segment/blend combination The next segment is calculated using v blend,outg, as start velocity, v s,, on segment. The starting point of the next segment is p b,. The initial length of segment is calculated using the already known intersection point of the next blend, p a,,lh. Initially we assume that v max,all, can be reached. Using the v s,, v e,, the initial segment length and the Cartesian constraints on that segment, a point to point vector τ can be calculated from start to end velocity. If, given v,, v max,all can not be reached, this τ is a zero-vector and a new end velocity v e,new, needs to be determined that can be reached. With this new end velocity a time optimal motion along that segment can be calculated. This end velocity is used as input for the new blend procedure to calculate the blend path for blend. The initial outgoing blend velocity is set to be equal to the maximum allowable velocity on segment

33 4.. Trajectory generation 3 p Segment v = e, v blend, ing, p p a, p b, ε user v = s, v blend, outg, v = Segment e, v blend, ing, ε user p a,, ini p p a, v = p b, s, 3 v blend, outg, Segment 3 p 3 Figure 4.4: More segment/blend combinations added to the first segment/blend combination. The calculated blend path of blend is first rejected, because p a,,ini is located outside the predefined sphere (). After rescaling, the blend path is approved () Blend a Segment b Blend b Figure 4.5: Visualization of the case where the start velocity of a segment needs to be adapted 3. If for example, the blend path is not approved, because p a is located outside the predefined sphere. This blend path is plotted in red in Fig 4.4. After rescaling of v blend,ing, will the calculated blend path be approved. This blend path is plotted in black in Fig 4.4. Using the above described procedure, trajectories can be generated like in Fig If small linear segments are present in the trajectory and the ingoing blend velocity is rescaled, this magnitude must remain fixed in order to hold p a at the edge of the predefined sphere. If the fixed end velocity can not be reached due to the Cartesian constraints and the short segment length, the start velocity should be adapted. Consider a very short segment which is coupled to two blend paths, see Fig The algorithm has calculated the blend path of blend a and continues to calculate segment b. The start velocity of segment b is adapted to v blend,outg,a to maintain a smooth velocity profile from blend a to segment b. v max,all,b can not be reached, so a reachable v e,b is determined. This v e,b is used as initial v blend,ing,b but this velocity places p a outside the predefined sphere. After rescaling v blend,ing,b is reduced and p a is placed at the predefined sphere. v e,b should be adapted to the rescaled v blend,ing,b. However due to the short segment length, this end velocity can not be reached by the point to point. But to stop p a from entering the forbidden region, v blend,ing,b should stay fixed and thus v e,b should be fixed. So a new start velocity should be determined, from which v e,b just can be reached. Segment b can be recalculated using the new start and end velocity.

34 4.. Trajectory generation 3 In order to maintain a smooth velocity profile during this blend/segment/blend combination, blend a also needs to be adapted. A new blend path should be calculated for blend a. As inputs we have v blend,ing,a with is the same as in the initial case, and v e,b as initial outgoing blend speed. If both p a and p b are located inside the predefined sphere, the algorithm can continue to the next combinations. If v blend,ing,a also needs to be adapted the procedure is repeated again with the previous segment and blend until a segment in the sequence can reach the recalculated end velocity while keeping its original start velocity Part three: Interpolating and plotting of the whole trajectory After all segment- and blend parameters are calculated, the algorithm proceeds to the final step. The linear segments and blends are interpolated in the same way as they are calculated, from pick position to where the product must be placed. Position, velocity, acceleration and jerk profiles of all coordinate directions are calculated and plotted. Because also velocity, acceleration and jerk profiles of the whole trajectory are plotted, it is easy to verify if one actuator constraint is active throughout the whole time span. Also constraint violation is easy visible in this plots. So using this new pick and place algorithm, time optimal trajectories can be generated and plotted. This is shown in the next chapter.

35 33 Chapter 5 Simulations 5. Introduction In the previous chapter, the structure of the new algorithm for pick and place operations was discussed. In this Chapter simulations are performed to verify this new pick and place algorithm and this algorithm is compared with the conventional algorithm of van Dijk. In total three different simulations are done. The first simulation is a -D trajectory that consists of five waypoints. In this simulation, the update algorithm is present in the new algorithm. The other two simulations are 3-D to compare the new developed algorithm with the algorithm of van Dijk in more advanced trajectories. In 3-D cases the update algorithm can not be used, because this algorithm is only capable of calculating with -D vectors. That is why a simple update law is used in the 3-D case, see Appendix F. 5. Simulations The Cartesian constraints that are used in the simulation cases are plotted in Table 5.. In the next section, a simulation of generating a -D trajectory will be performed. 5.. Simulation : a -D trajectory of five waypoints The first simulation that is performed is the generation of a trajectory in the -D Cartesian space. This is the only way in which the fully new developed algorithm can be compared with the existing pick and place algorithm of van Dijk. The trajectory consists of five waypoints, which are presented in Table 5.. A detailed numerical example of how this trajectory is generated is included in Appendix E. Before the algorithm starts to calculate the first segment, waypoint five is excluded from the setpoint, because it is duplicate and positioned equally to waypoint four, but has a lower blend radius. Also the blend radii of waypoint two, three and four are reduced, because they were intersecting. The first two segment/blend combinations are calculated without much problems, but at segment 3 the algorithm needs to adapt the previous blend and segment, to assure a smooth velocity profile Constraint Value V max [ m s ] 5 a max [ m s ] 5 j max [ m s ] 3 5 Table 5.: Constraints for all simulation cases

36 Simulations X-coordinate[m].5.5 Y-Coordinate[m] Z-coordinate[m] Blend radius specified by the user[m]... Table 5.: setpoint of trajectory generated for simulation Figure 5.: Total trajectory generated for the first simulation case using fast as blend procedure. during the whole trajectory. The last segment is large enough to decelerate from vblend,oug,3 at the start point to zero at the end point. The resulting trajectory for the first simulation case is plotted in Fig. 5. and the motion profiles are plotted in Fig. 5.(a) and Fig. 5.(b). The new pick and place algorithm has the jerk constraint active throughout the whole time span. The velocity and acceleration constraint are not reached due to the compactness of the trajectory. If this is compared to the jerk profile of the conventional algorithm, especially between.3[s].3[s], we can see where the high gain in motion time is coming from. The generated motion time is the fastest for that particular defined path as in Fig. 5.(a) is visualized. However, the computational time of the new pick and place algorithm is worse than the conventional one, due to the blend procedure. The conventional algorithm calculates the blend paths immediately, whereas the new pick and place algorithm must calculated the blend paths twice (once to check the location of both intersection points and the second time to reposition them if necessary). Also third order equations need to be solved in the update algorithm which causes computational time. This lead to the final result which is plotted in Table 5.3. Simulation : A long trajectory of seven waypoints The second simulation consists of normal length segments with blends. The waypoints of this simulations are described in 3-D to compare the new blending method in more advanced trajectories. Because the developed update algorithm only deals with -D vectors, a conventional update law is used, described in Appendix F. This update law uses the same procedure of calculating the blend pick location place location

37 5.. Simulations 35 pos [pu] acc [pu/s jrk [pu/s 3 ] vel [pu/s] Blend Blend Blend Time [s] (a) Motion profiles using the new pick and place algorithm. The blend timings are indicated in the figure. Blue represents the x- direction, green the y-direction pos [pu] acc [pu/s jrk [pu/s 3 ] vel [pu/s] Blend Blend Blend Time [s] (b) Motion profiles using the conventional pick and place algorithm. T blend timings are indicated in the figure. Blue represents the x-direct green the y-direction Figure 5.: Motion profiles for both pick and place algorithms for the first simulation Motion time[s] Calculation time[s] Old algorithm.48.9 New algorithm.8.4 Table 5.3: Both algorithms compared on motion and computational time for the first simulation case

38 5.. Simulations 36 X-coordinate[m] Y-coordinate[m] Z-coordinate[m] Blend radius[m] pick location place location Table 5.4: setpoint for simulation paths, but uses a different strategy to reposition p a and/or p b if the blend path is not approved. The pick and place position of this second simulation is at (,, ) and the trajectory has seven waypoints in total.the setpoint of the trajectory of simulation is plotted in Table 5.4 where also the blend radii are defined. The second linear segment is a relatively short segment compared to the others. On this segment the algorithm calculates a point to point using v blend,out, as the start velocity and tries to reach v max,all,. This velocity difference can not be overcome considering the short segment length. So the maximum reachable end velocity of segment is calculated and used as v blend,ing, for the blend iteration of blend (when the blend path is calculated with the conventional update law, both blend velocities are lowered iteratively, that is why this method is referred to as blend iteration, see Appendix F. After this blend iteration, v end, has become much smaller and now the segment length is too short to decelerate from v start, to v end,new,. So given v end,new,, a new start velocity of the segment needs to be calculated so that v end,new, just can be reached. Also blend needs to be adapted to the new start velocity of segment in order to hold a smooth velocity profile. So v blend,out, is adapted to the new start velocity of segment. In the rest of the segments and blends, only new end velocities of linear segments need to be recalculated. Those new end velocities can all be reached by the point to point, so no further problems occur. The algorithm interpolates all segments and blends and generates the trajectory of Fig The corresponding motion profiles of both algorithms are plotted in Fig Using the new pick and place algorithm one actuator constraint is always active during blending and during the whole trajectory. It generates the lowest possible motion time on the trajectory of Fig. 5.3 without violating one of the actuator constraints, so the calculated motion time is indeed the time optimal one. The conventional pick and place algorithm has time optimal motions on the linear segments, but loses time during blending. No constraint is active during the blending periods, so the generated motion time is not the time optimal one. Below, Table 5.5 gives an overview of the scaling factor k and the actual needed blend radii. This table shows that all blends are in the predefined sphere and thus all blend paths were approved. Both algorithms are compared on generated motion time and computational time. The conventional algorithm generates a motion time of 3.9[s]. The new algorithm generates a motion time of 3[s], which is 3 percent faster. Simulation 3: The trajectory used as motivation example for a new blending algorithm The last simulation deals with the trajectory plotted in Fig... These kind of trajectories are the motivation for developing a new blending algorithm. This trajectory has short linear segments and relatively long blends. In Fig.. the blends are denotes with a red marker. The actuator constraints

39 Simulations Figure 5.3: Generated trajectories for the setpoint of simulation. The trajectory marked with () is generated by the new algorithm whereas the trajectory marked () is the trajectory generated by the old algorithm. The large spheres indicate the desired spheres, whereas the small spheres indicate the intersection points pa and pb for both trajectories Blend Blend acc [pu/s jrk [pu/s3] Time [s] Blend Blend 4 3 Blend Blend Blend 3 4 vel [pu/s] vel [pu/s] Blend acc [pu/s Blend 4 jrk [pu/s ] Blend 3 pos [pu] pos [pu] Time [s] (a) Motion profiles using the new pick and place algorithm. The blend (b) Motion profiles using the conventional pick and place algorithm. timings are indicated in the figure. Blue represents the x-direction, The blend timings are indicated in the figure. Blue represents the green the y-direction and yellow the z-direction x-direction, green the y-direction and yellow the z-direction Figure 5.4: Motion profiles for both pick and place algorithms in the second simulation case Blend number k actual needed blend radius[m] blend radius specified by the user[m] Table 5.5: Scaling factor k and the actual blend radius compared to the specified blend radius for the second simulation case

40 5.3. Discussion 38 Motion time[s] Old algorithm 3.9 New algorithm 3 Table 5.6: Both algorithms compared on motion time for the second simulation case X-coordinate[m] Y-Coordinate[m] Z-coordinate[m] Blend radius[m] Recalculated blend radius[m] Table 5.7: setpoint for simulation 3 are equal to the ones in simulation and are listed in Table 5.. The setpoint of this trajectory is displayed in Table 5.7. Because the segment lengths are very short, all blend radii need to be recalculated until they are tangent. Those new blend radii also listed in Table 5.7. These values are used in the blend iteration to approve or reject the calculated blend path. Only segment is large enough to immediately go from the calculated v blend,out, to v max,all,. On the rest of all segments, new end velocity need to be determined, because the segment lengths are that short. In this example the major difference between both algorithms is again the track acceleration during blending. Because segment is a short segment, the end velocity v e, is low. The conventional algorithm needs to maintain this track velocity during blend, because no track acceleration is allowed. On the second segment, the manipulator can speed up to v max,all,. But because segment 3 is again a short one and no track acceleration can be made during blend, v e, must be again low to prepare for blend. On segment 4 the manipulator can speed up again to v max,all,4 but because segment 5 is again short, v e,4 needs to be lowered again to prepare for blend 4. The new algorithm does not have these problems, the manipulator is allowed to accelerate during blend and gets a higher v blend,out,. This means that during segment the manipulators is already much faster than using the conventional algorithm. Also the robot is allowed to decelerate in blend to prepare for the short segment 3, instead of decelerating already on segment which was the case in the conventional algorithm. Then the manipulator accelerates again in blend 3 and segment 4 and decelerates in blend 4 to prepare for segment 5. Then the algorithm finishes the trajectory. Both trajectories are plotted in Fig The red path is the trajectory calculated by the new algorithm, whereas the blue path is calculated using the old algorithm. The corresponding motion profiles are plotted in Fig The conventional algorithm has a motion time of 5.3[s]. The new algorithm has a motion time of.[s]. The scaling factor k and the blend radii are listed in Table 5.8 So in this case, the new algorithm is 5 percent faster, see Table Discussion In this Chapter simulations are performed to verify the new pick and place algorithm. Also a comparison is made in generated motion time between the conventional pick and place algorithm of van Dijk

41 Discussion Figure 5.5: The generated trajectory for simulation 3 with the new () and the conventional () algorithm acc [pu/s jrk [pu/s3] Time [s] acc [pu/s. vel [pu/s] vel [pu/s] Blend 3 Blend 5 Blend Blend 4 Blend 6 Blend 4 jrk [pu/s3] Blend 5 Blend 6 Blend 3Blend 4 pos [pu] pos [pu] Blend Blend Time [s] (a) Motion profiles using the new pick and place algorithm. The blend (b) Motion profiles using the conventional pick and place algorithm timings are indicated in the figure. Blue represents the x-direction, The blend timings are indicated in the figure. Blue represents the xgreen the y-direction and yellow the z-direction direction, green the y-direction and yellow the z-direction Figure 5.6: Motion profiles for both pick and place algorithms Blend number k actual needed blend radius[m] specified blend radius[m] Table 5.8: Scaling factor k and actual needed blend radius compared to the specified blend radius for the third simulation case

42 5.3. Discussion 4 Motion time[s] Old algorithm 5.3 New algorithm. Table 5.9: Both algorithms compared on motion time for the third simulation and the newly developed pick and place. In the -D case, also the computational time is compared, because the update algorithm that is developed for the new blend procedure is yet only capable of operating in -D. The new algorithm is capable of generating faster motion times but uses more computational time. If the shape of the trajectory is fixed, i.e. all final locations of the blend intersection points are determined, the new pick and place algorithm is capable of generating the time optimal motion in -D or 3-D. However, computational time can only be tested in -D, but that was slower than the conventional algorithm. The conventional algorithm is capable of calculating the blend paths instantly, whereas the update algorithm needs to calculate the blend paths twice. This includes sometimes solving of third order equations numerically. Therefore, the computational time of the new pick and place algorithm is higher than the conventional one.

43 4 Chapter 6 Conclusions and Recommendations In this last chapter of this report, conclusions will be drawn and recommendations will be made. A new theoretical approach is presented to construct time optimal blend paths and a procedure to control the positions of the blend velocity dependent p a and p b. In the first section conclusions are drawn and in the second section recommendations for future research are made. 6. Conclusions A way of blending is proposed that uses the existing theory of time optimal motions on linear segments to construct time optimal blend paths. It is theoretically guaranteed that no Cartesian constraint will be violated, but that during the blend at least one is active. Because also the lowest motion time is generated, time optimal blends can be constructed. The intersection points, where the manipulator may deviate from its linear segment are depending on the ingoing- and the outgoing blend velocity. So a criterion is defined, which checks if the velocity dependent positions of those intersection points are not outside the predefined sphere. An update algorithm is developed which places those intersection points on the edge of the predefined sphere if they were initially positioned in the forbidden region. This update mechanism and the new blend procedure are implemented in the conventional pick and place algorithm. This algorithm has the ability to construct time optimal motions on linear segments. So combining this with the time optimal blends, time optimal trajectories on a given path in Cartesian space can be constructed. This new algorithm has been compared to the conventional one on motion time and computational time. In motion time, a lot of improvement is made especially in the class of applications where short segments and large blends are required. In -D and 3-D cases, the new algorithm generates the time optimal motion on the generated path. In computational time however, the new algorithm performs not so well as the conventional one. 6. Recommendations In this Section, some recommendations for future research are proposed. First, some recommendations regarding the improvement of the developed algorithm will be discussed. Next, some other recommendations are proposed. The recommendations have to do with the update algorithm that is developed. There are two important aspects that need to checked in future research:. The procedure of finding the rescaled blend velocities in one step must be extended into six DOF.

44 6.. Recommendations 4 In that case true simulations can be done to compare the computational time of the conventional algorithm with the new pick and place algorithm.. In order to reduce that computational time even more, a way should be found to rapidly solve third order equations. A fast solver is available but remains to be implemented in the update mechanism. Next some other recommendations are made. The first recommendation has to with the theory to construct time optimal motions on linear segments. Due to this theory, a zero acceleration should be attained when entering the blend interval. At the intersection points of the blend with the linear segments the manipulator can not accelerate or decelerate, simple because no end value of the acceleration can be defined. In future research the theory to construct time optimal motion on linear segments can be extended to also allow non-zero acceleration at the start and end point of a segment. If a large number of segment/blend combinations must be calculated, some criterion can be defined, that if a selection of that large number of segment/blend combination is fixed is already sent to the computer to be calculated. Such a criterion can decrease computational time drastically, because you can split up the total amount of segment/blend combinations that need to be calculated. This report only focusses on traject generation. The trajectory generator gets its inputs from the user itself. The user defines the path in the Cartesian space, but also path planning algorithms are available which optimize that path. In this case an optimized path is used as an input for the trajectory generator, which will result in lower motion times.

45 43 Appendix A Calculation of the time increments for a third order profile In this Appendix, the seven point to point time increments are calculated for one of the profiles in Fig.(B.). These point to point time increments can be divided into three parts: an acceleration phase, a constant velocity part and a deceleration phase. A. Acceleration phase In the example of Fig. A., we see immediately, that there is no constant deceleration time, so τ 6 = (A.) The other parameters are larger than zero, see Fig. A.. τ i > i =... 5, 7 (A.) Using (A.) we can conclude, that a constant velocity part exists, i.e. τ 4 >. That means, that the maximum velocity can be reached and is described by (A.3): v max = v(τ ) + v(τ ) + v(τ 3 ) (A.3) In the beginning of the segment we have an initial velocity v and at the end of the segment we have final velocity v e. So the difference in velocity along the segment can be calculated and is equal to the area under the acceleration plot, see Fig. A.. v a = v max v = O + O V + O V (A.4) The acceleration is the same as the deceleration, see Fig. A.(b): O = O V = a max τ = a max τ 3 τ = τ 3 (A.5) So, combining (A.4) and (A.5) yields: v max v a max (τ + τ ) (A.6) During τ we know that the jerk is maximal, see Fig. A.: a max = j max τ (A.7) So τ and τ 3 are τ = τ 3 = amax j max (A.8)

46 A.. Acceleration phase 44 Figure A.: Example of a third order profile and the meaning of the seven time increments Figure A.: Velocity and acceleration profiles along a linear segment

47 A.. Deceleration phase 45 Table A.: During acceleration always one constraint active Time Period j max a max v max < t τ Active Inactive Inactive τ < t τ + τ Inactive Active Inactive τ + τ < t τ + τ + τ 3 Active Inactive Inactive τ becomes, using (A.4) τ = v e v a max a max j max (A.9) So given the actuator constraints a max, v max and j max we are able to calculate the three time segments in order to get a time optimal velocity profile in the acceleration phase. The motion is time optimal, because during motion we have always at least one constraint active and it generates the lowest motion time, see Table A.: A. Deceleration phase The same reasoning can be performed in the deceleration phase. In this example no constant deceleration was present, recall (A.). So the maximum deceleration can not be reached, see Fig. A.3(b). The difference in velocity is again equal to the area under the deceleration curve, see Fig. A.3(b): v b = v max v e = O V + O X (A.) O V and O X be calculated, knowing that the jerk is maximal in those time segments, see Fig. A.. O V = O X = j max τ 5 Solving (A.) for τ 5 and τ 7 by using (A.)yields vmax v e τ 5 = τ 7 = and recall (A.): τ 6 = j max (A.) (A.) (A.3) So given the actuator constraints, a max, v max and j max, we are able to calculate three time segments in order to get a time optimal velocity profile in the deceleration phase, because during deceleration always at least one constraint is active and it generates the lowest motion time, see Table A.. A.3 Constant velocity part The only point to point time parameter that is not yet known is τ 4. This parameter denotes the constant velocity time and can be calculated using the actual traveled distance during acceleration and deceleration, x atd. x atd can be calculated using Fig. A.(a) and Fig. A.3(a): x atd = O + O + O V + O V (A.4) O = v (τ ) + (τ ) + (τ 3 ) (A.5) O = (v max v )(τ ) + (τ ) + (τ 3 ) (A.6) O V = v e (τ 5 ) + (τ 7 ) + (τ 3 ) (A.7) O V = (v max v e )(τ 5 ) + (τ 7 )) (A.8)

48 A.3. Constant velocity part 46 Figure A.3: Velocity and deceleration profiles on a linear segment Table A.: During deceleration always one constraint active Time Period j max a max v max τ a < t τ a + τ 5 Active Inactive Inactive τ a + τ 5 < t τ a + τ 5 + τ7 Active Inactive Inactive The distance actually traveled by accelerating and decelerating is compared with the length of the linear segment, x tot. Then the difference of the length of the linear segment and the traveled distance is the constant velocity part. This yields for τ 4 : τ 4 = x tot x atd v max (A.9) So using Table A. and Table A. and realizing that the maximum velocity constraint is active during τ 4 we have a constraint active throughout the whole time span. This is a necessary requirement for a time optimal motion. In this case, this calculated profile generates also the lowest motion time, so it is the time optimal motion.

49 47 Appendix B Different profiles along the linear segment In the previous Appendix, we have calculated the point to point time parameters for an optimal motion on a linear segment. This Appendix deals with how some other profiles of Fig.?? can occur. We have made an assumptions in appendix A, namely the presence of a constant velocity part. This constant velocity part is not always present and then the calculation of the point to point time increments is then different. B. Acceleration phase We have assumed that a constant acceleration part exits, i.e. τ >.We could define a condition whether a constant acceleration is present, by setting (A.9) larger of equal to zero. v e v a max a max j max (B.) Because τ is a time segment, it must always be larger or equal to zero in order to exist. If condition (B.) is satisfied, the point to point time parameters are calculated as stated in section A.. If condition (B.) does not hold, we don t have a constant acceleration part and we need to recalculate τ and τ 3. They become (for a detailed deviation, we refer to van Dijk []: τ = τ 3 = vmax v j max τ = (B.) (B.3) B. Deceleration phase In the same way, we assumed in Section(A.) that there was no constant deceleration part, i.e. τ 6 =. Applying Eq.(B.) to the deceleration phase, we have a condition for the presence of a constant deceleration part: v max v e a max a max j max (B.4) Because τ 6 is a time segment, it must always be larger or equal to zero in order to exist. If condition (B.4) holds, we need to recalculate τ 5, τ 6 and τ 7. In this case, the maximum deceleration can be reached and the maximum deceleration constraint is active. For a detailed deviation, we refer again to

50 B.3. Constant velocity part and iteration 48 Figure B.: All possible velocity profiles on a linear segment van Dijk []: τ 5 = τ 7 = a max j max τ 6 = v max v e a max and τ 4 is known by Eq.(3.). a max j max (B.5) (B.6) If condition(b.4) does not hold, the time increment are as stated in Section(A.). B.3 Constant velocity part and iteration It could be possible, that substituting the calculated point to point time parameters in (3.), results in a negative τ 4. This means that with the given parameters the maximum velocity is too high. This implies τ 4 = and the calculation should be repeated, this time with τ 4 =. So in total there are seventeen possible velocity profiles, each having there own combination of time increments, see Fig. B.. In the beginning of this chapter the difference in velocity over a linear segment is defined, (A.4) and (A.). In all calculations, the assumption is made, that the length of the segment on which the movement took place, was large enough to overcome this velocity difference. If this is not the case, either the initial or end velocity of the segment needs to be redefined.