On path planning and optimization using splines Mikael Norrlöf Division of Automatic Control Department of Electrical Engineering Linköpings universitet, SE-58 83 Linköping, Sweden WWW: http://www.control.isy.liu.se E-mail: mino@isy.liu.se 7th February 3 AUTOMATIC CONTROL COMMUNICATION SYSTEMS LINKÖPING Report no.: LiTH-ISY-R-49 Submitted to Technical reports from the Control & Communication group in Linköping are available at http://www.control.isy.liu.se/publications.
Abstract This report covers many aspects of path and trajectory generation for industrial robots. Path generation in Cartesian space is discussed, with the limitation that only linear motion is considered. Path generation in joint space is also discussed and in particular representation of the joint path using cubic splines is presented. Different algorithms for spline generation are also discussed and tested on an example. The last step, the trajectory generation is also covered. Two preliminary algorithms that gives limited speed and acceleration in Cartesian space and in joint space are given. It is also indicated that there is more to be done in order to reach an optimal trajectory. At the end of the report a simple spline toolbox for Matlab is described. Keywords: path planning, trajectory generation, cubic spline, cartesian space, joint space
Contents Introduction and overview Generate a path in Cartesian space. Motion along straight line.................................. A three point linear motion.................................3 A general case........................................4 Second order polynomial.................................. 3.5 Two third order polynomials............................... 4.6 Two fourth and two third order polynomials....................... 5.7 Matlab implementation of the movel instruction.................... 5 3 Path generation in joint space 6 3. An iterative spline solution that gives continuous derivative.............. 7 3.. knots....................................... 7 3.. 3 knots....................................... 8 3..3 4 knots....................................... 9 3..4 Results from a simulation with the three algorithms.............. 9 3. An iterative solution that gives continuous first and second derivative........ 4 Trajectory generation 4. Constant velocity along the Cartesian path....................... 4.. Linear motion in Cartesian space......................... 4 4.. Motion along a general path............................ 4 4..3 Limitations in Cartesian space.......................... 6 4..4 Joint space limitations............................... 8 4..5 Issues to be studied in more detail........................ 9 4. A combined algorithm for Cartesian and joint limitations............... 5 Conclusions and further study Appendix A: A basic spline toolbox i
ii
Introduction and overview The problem of trajectory generation for industrial robots consists of a number of subproblems. Many research contributions have been in the direction of optimal path planning with velocity, acceleration, and jerk constraints of the robot. Craig [], Sciavicco [], and Chernousko [3] contain good introductions to the topic. Also the PhD thesis of Dahl [4] covers the same problem although there the path generation problem is not covered in such detail. The solution to the trajectory generation problem is a time history of joint position, speed and acceleration for each degree of freedom of the robot. Those trajectories are then used by the robot control system to make the robot follow the programmed path. Trajectory generation as it is defined here assumes that a path in Cartesian space is given by a user. As input to the robot control system we therefore have a high level command such as movel p,v,z,tool movel p3,v3,fine,tool This means that the robot should move along a straight line from a position and orientation in p (not explicitly given) towards the position and orientation given by p and then when inside a zone of mm start moving towards p3. To simplify the path generation in Cartesian space only the positioning problem is considered. The robots that will be used in the examples are the IRB4 robot, described in e.g., [5], and the similar IRB4. The spline generation and analysis is covered in [6] and there is also a toolbox available for Matlab [7] although in this report a toolbox developed by the author (see Appendix A) is used. Generate a path in Cartesian space The representation of the path in Cartesian space is here assumed to be done using polynomials starting with the straight line.. Motion along straight line To give an overview of the problem we start from the simple test case given above, i.e., the movel command. The first step is to parameterize the Cartesian path. This is easy for the linear motion problem where the position along the path can be expressed as p = p + L(p p ), L () The robot is not controlled in Cartesian space and therefore this path has to be transformed into a joint space trajectory. This will be discussed in Section 3.. A three point linear motion When having a motion that passes more than two points the general description of the path is not trivial. The case that will be studied here is given by the following user commands, movel p,v,z,tool movel p3,v3,fine,tool which was also discussed in Section. In principle what we want to achieve is shown in Figure.
.5.4 p3 y [m].3.. p...3.4 p.5...3.4.5.6.7 x [m] Figure : The result of two movel instructions with a zone at point p. We will now present three different ways of representing the path inside the zone. The first is based upon a second order polynomial while the second results in two third order polynomials inside the zone. The third solution gives a path inside the zone represented by four polynomials, first a fourth order polynomial, then two third order polynomials and finally a fourth order polynomial..3 A general case When discussing the different ways of representing the path inside the zone, a particular case will be studied. This particular case, referred to as a general case, is shown in Figure. The idea with this general case is that it is always possible to find a coordinate transformation such that the two movel instructions are in a plane and the angle with respect to the y-axis is symmetric and that the second point is in the origin in the x-y-plane. y [m].5.45.4.35.3.5..5..5.5.4.3.....3 x [m] Figure : The general case which will be studied in the following sections. A number of requirements can be formulated for the behavior of the path inside the zone. The first and most important is that when interconnecting the two linear sections the path inside the zone must be symmetric. The second requirement that will be adopted here is that the path inside the zone should be close to the one that is achieved by the second order polynomial solution (shown in Figure ). A fundamental requirement is also that the path is continuous with continuous derivative. Two solutions that give a path which has a continuous second derivative will also be presented. The path is represented by sections, P j(l) withl in [ l z,l z] andj as the section number. Notice that l is a path index in p + l(p p ). Since different sections can have different length the
derivative of p j,x(l) with respect to l will not necessarily be a continuous function in the point where we go from section j tosectionj. This is the reason why the sections are grouped according to Figure 3. Inside the second section the index l is with respect to p + l(p 3 p ). For the second section l z is the l where p + l(p 3 p ) intersects the zone and l z =. y [m].5.45.4.35.3.5..5..5.5.4.3.....3 x [m] Figure 3: Representation of the path in sections. Solid line is section, dashed line is section..4 Second order polynomial We will only consider the polynomials representing the path in x and y since z inthecase described in Section.3. The representation will be as described in the previous section, i.e., the path is represented by sections. The first section from a fine point is trivial to represent since it is enough to represent it as a first order polynomial, where l z can be computed from P (l) =p + l(p p ), l [,l z] () with z thesizeofthezoneatp. l z = z p p (3) The second section (and the general description of a movel section) will consist of two spline sections. The first being a second order polynomial in y and a first order polynomial in x and the second a first order polynomial in x and y. First let p = p + l z(p p ) (4) then the polynomial inside the zone is given by p,x + x(l + l z) P (l) = y l z (l + l z) y(l + l z)+ p,y, l z l l z (5) with and outside the zone x = y = p 3 (6) z P (l) =p + l(p 3 p ), l z <l l z (7) 3
Since p, p and p 3 are not in the x-y-plane with p in the origin, p, p =and p 3 are used instead. Now p and p 3 can be found from p, p and p 3 using the following transformations [ p p p 3 p ] = [ xe y e z e ][ p p 3 ] (8) where x e, y e,andz e are the vectors that tells how the x-y-z-axes in Figure 3 are represented in the world coordinates. This mapping can be found by doing the following computations, p + p3 p y e = p + p 3 p x e = (p3 p) ( ) (p 3 p ) y e ye (p 3 p ) ( ) (p 3 p ) y e ye z e = xe ye x e y e (9) The description of the path inside the zone becomes P (l) = [ x e y e z e ] P(l)+p, l z l l z () This kind of description will be used also in the next sections..5 Two third order polynomials The description of the path outside the zone is exactly as in Section.4. The difference is the description inside the zone where P (l) becomes p,x + x(l + l z) y P (l) = (l + l 3l z) 3 y(l + l z)+ p,y, l z l () z and P (l) = y 3l z p,x + x(l + l z) l 3 y l z l + p,y l z y 3, l l z () where is defined in (6). P (l) is computed as in (). In Figure 4 a comparison between the result in this section and what is achieved by using the second order solution from the previous section is shown..45.4.35 y [m].3.5..5...5.5. x [m] Figure 4: Comparison of the result when using the second order solution in Section.4 (dotted) and the two third order spline solution in Section.5 (solid). 4
.6 Two fourth and two third order polynomials To be able to specify the position of the points inside the zone and also include that the function has a continuous second derivative it is necessary to have four conditions in each inner point. Three for continuity and one extra for specifying the position. With two fourth order and two third order polynomials the number of parameters becomes 5 + 4 = 8 and the number of conditions 3 +4 3 = 8. The positions inside the zone are symmetric, i.e., the first v andthethirdpoint v 3 correspond to each other in the following way, v 3,x v,x v 3 = v 3,y = v,y v 3,z v,z with v,z = v 3,z =. Now P,x(l) and P,z(l) arethesameasin()while (3) P,y(l) = P,y(l) = 9 p,y 7 ylz/ v,y +3v,y 5(l z/) 4 (l + l z) 4 + 4 p,y +6 ylz +7v,y 3v,y 5(l z/) (l + l z) 3 y(l + l z)+ p,y, l z l lz 6 p,y +3 ylz/+3v,y 7v,y 5(l z/) 3 (l + lz )3 + + 3(4 p,y ylz 7v,y +3v,y) 5(l z/) (l + lz ) 3( p,y + ylz/+v,y + v,y) 5l z/ (l + lz lz )+v,y, l (4) (5) P,y(l) = 6 p,y 3 ylz/ 3v,y +7v,y 5(l z/) 3 (l + l z) 3 + 3( p,y + ylz/+6v,y 4v,y) (l + l z) + v,y, l lz 5(l z/) (6) P,y(l) = 9 p,y 7 ylz/ v,y +3v,y 5(l z/) 4 (l lz )4 p,y +8 ylz +3v,y 9v,y + (l lz 5(l z/) 3 )3 3(4 p,y yl z 7v,y +3v,y) (l lz 5(l z/) ) 3( p,y + ylz + v,y + v,y) (l lz 5(l z/) )+v,y, l z l lz P (l) is computed as in (). The expression in Equations 4 to 7 have been found using Mathematica. Other combinations of polynomials are easily found using a tool such as Mathematica but those results will not be given as solutions in this report. Interested readers are instead referred to a Mathematica notebook, movelzone.nb, by the author of this report. In Figure 5 a comparison between the result from using the solution described in this section and using the second order spline solution from Section.4 is shown. The resulting path is very similar in the two cases but with the difference that the second derivative for the resulting path described in this section is continuous. (7).7 Matlab implementation of the movel instruction The three different ways to represent the path inside a zone are all implemented in the function movelzone, described below. 5
.45.4.35 y [m].3.5...5.5. x [m] Figure 5: Comparison of the result when using the second order solution from Section.4 (dotted) and the four spline solution in Section.6 (solid). Syntax Description Example [sec,coeffs] = movelzone(p,p,p3,zone,zone3,method); Computes a section represented by a spline in the spline toolbox format described in Appendix A. The argument method can be one of the following: (a) If method== (default) the second order polynomial solution from Section.4 is used. (b) If method== the two third order polynomial solution from Section.5 is used. (c) If method==3 the method described in Section.6 is used. The points p to p3 are the one shown in Figure and the section, returned in sec, is the dashed part in Figure 3. The parameters zone and zone3 are the size of the zones at p and p3, respectively. The result from running the Matlab code below is shown in Figure 6. For a more thorough discussion of the basic spline toolbox used in the example see Appendix A. p = [.5,.5,] ; p = [.,.45,.] ; p3 = [.5,.6,.5] ; zone =.; zone3 = ; sec = movelzone(p,p,p3,zone,zone3,3) plot3([p() p() p3()],... [p() p() p3()],[p(3) p(3) p3(3)], : ) hold on pos = evalsp(sec,sec.brp():.:sec.brp(end)) ; plot3(pos(:,),pos(:,),pos(:,3)); 3 Path generation in joint space Given a path representation in Cartesian space this path has to be transformed into a path representation in joint space. This is in general not possible to do analytically. Instead the path must be transformed using the inverse kinematic model of the robot manipulator at discrete points. The idea that will be adopted here is that given the discrete points in joint space we want to find a spline representation (in joint space) that approximates the true path with a given accuracy. In the next section two different methods to do this will be described and the resulting algorithms will also be analyzed both from an analytical point of view as well as a simulation point of view. 6
.5..5 z [m]..5.8.6 y [m].4....3 x [m].4.5 Figure 6: The result from running the Matlab code in the example describing the movelzone command. 3. An iterative spline solution that gives continuous derivative Three different versions of the algorithm will be discussed in this section. One based on knots, the second on 3 knots and the last on 4 knots. The resulting splines are computed using estimates of the derivative of the path with respect to l (the path index). The estimated derivative can be found in many different ways but here it will be assumed that it can be computed using the Jacobian [5], another solution is to find the inverse kinematic solution close to the endpoints and compute the estimate of the derivative using a difference approximation. 3.. knots The first solution is based on knots. This means that only one cubic spline has to be found. Given the joint position and the joint position derivative in the two knots, φ j and φ j with j = k, k + the solution is found as ˆφ(l) = (φ k φ k+ )+ k (φ k + φ k+) (l l 3 k ) 3 k 3(φ k φ k+ )+ k (φ k + φ k+) (l l k ) k (8) + φ k(l l k )+φ k, l k l l k+ To evaluate the solution the direct kinematics is computed in the intermediate point, l k+/ = l k + l k+ l k, and the resulting Cartesian point is compared to the true position from the spline representation of the path in Cartesian space. If T ( ˆφ(l k+/ )) P (l k+/ ) <γfor some norm and γ then the spline is considered ok, otherwise the step length is changed to k /andanewspline is computed as above. T ( ) represents the direct kinematic model. Algorithm (An iterative algorithm for spline interpolation using knots). Compute the inverse kinematic solution at P (l) and P (l+ l ),resultinginφ(l) and φ(l+ l ).. Find an estimate of dφ(l) and dφ(l+ l) (using for example the Jacobian). dl dl 3. Compute the coefficients in the cubic spline from (8). 4. Evaluate the resulting spline, ˆφ(l) in l + l / T ( ˆφ(l + l /)) P (l + l /) <γ (9) 7
where P ( ) is assumed to be the path representation in Cartesian space. If the -norm is used γ represents the maximum error in Cartesian space that is allowed. a. If the inequality in (9) is fulfilled then keep the spline and let l = l + l, l = l b. else let l = l 5. Goto. 3.. 3 knots The 3 knots solution gives as a result cubic splines given the joint position and the joint position derivative at the first and the last knot, φ j and φ j with j = k, k + and joint position in the middle knot, φ k+. The solution is found as ˆφ(l) = k+ (φ k φ k+ )+ k k+ (4(φ k φ k+ )+ k+ φ k )+ k ( 3φ k++3φ k+ + k+ φ k k+φ k+ ) k k+( k + k+ ) (l l k ) 3 + + 3 k+ (φ k+ φ k ) 3 k k+ ((φ k φ k+ )+ k+ φ k )+ k (3(φ k+ φ k+ ) 4 k+ φ k + k+φ k+ ) k k+ ( k + k+ ) () (l l k ) + φ k(l l k )+φ k, l k l l k+ ˆφ(l) = k (φ k+ φ k+ )+ k k+ (4(φ k+ φ k+ )+ k φ k+ )+ k+ (3(φ k+ φ k ) k φ k + kφ k+ ) k k+ ( k+ k+ ) (l l k+ ) 3 + + 3 k(φ k+ φ k+ )+ k+ (3(φ k φ k+ )+ k (φ k φ k+ )) k k+ ( k + k+ ) (l l k+ ) () 3 k (φ k+ φ k+ )+ k+ (3(φ k φ k+ )+ k φ k )+ k k+φ k+ k k+ ( k + k+ (l l ) k+ ) + φ k+, l k+ l l k+ The result in () and () can be simplified if it can be assumed that k = k+. To find the solution in () and () it is assumed that ˆφ(l) ineachintervalisgivenby For the first spline it follows that ˆφ(l) =a + a (l l k )+a 3(l l k ) + a 4(l l k ) 3 () ˆφ (l k )=φ(l k ), ˆφ(l k+ )=φ(l k+ ), ˆφ (l k )=φ (l k ) and for the second spline the conditions become ˆφ (l k+ )=φ(l k+ ), ˆφ (l k+ )=φ (l k+ ) To get a continuous function with continuous first and second derivative at the inner knot we also have the conditions ˆφ (l k+ )= ˆφ (l k+ ), ˆφ (l k+ )= ˆφ (l k+ ), ˆφ (l k+ )= ˆφ (l k+ ) which give a total of 8 conditions. From () we see that each spline has 4 parameters which gives a total of 8 parameters and a system of equations that has one unique solution. This solution is given by () and (). Algorithm (An iterative algorithm for spline interpolation using 3 knots) The limit is not guaranteed and therefore a larger error can be achieved in some other point along the spline. 8
. Compute the inverse kinematic solution at P (l), P (l + l ) and P (l + l ),resultinginφ(l), φ(l + l ) and φ(l + l ).. Find an estimate of dφ(l) and dφ(l+ l) (using for example the Jacobian). dl dl 3. Compute the coefficients in the cubic splines from () and (). 4. Evaluate the resulting spline, ˆφ(l) in l + l / and l +3 l / T ( ˆφ(l + l /)) P (l + l /) <γ, T ( ˆφ(l +3 l /)) P (l +3 l /) <γ (3) where P ( ) is assumed to be the path representation in Cartesian space. If the -norm is used γ represents the maximum error in Cartesian space that is allowed. a. If the inequality in (3) is fulfilled then keep the spline and let l = l + l, l = l b. else let l = l 5. Goto. 3..3 4 knots The solution for this case is not given since it becomes too complex to grasp. To find the solution the same kind of spline representation as given in () is used. For the first spline the following conditions must be fulfilled and for the last spline the conditions become ˆφ (l k )=φ(l k ), ˆφ(l k+ )=φ(l k+ ), ˆφ (l k )=φ (l k ) ˆφ 3(l k+3 )=φ(l k+3 ), ˆφ 3 (l k+3 )=φ (l k+3 ) To get a continuous function with continuous first and second derivative at the inner knot we also have the conditions and ˆφ (l k+ )= ˆφ (l k+ ), ˆφ (l k+ )= ˆφ (l k+ ), ˆφ (l k+ )= ˆφ (l k+ ) ˆφ (l k+ )= ˆφ 3(l k+ ), ˆφ (l k+ )= ˆφ 3(l k+ ), ˆφ (l k+ )= ˆφ 3 (l k+ ) This gives a total of conditions. From () we see that each spline has 4 parameters which gives a total of parameters and a system of equations that has one unique solution. The algorithm for this case is similar to Algorithm with obvious extensions (compare going from Algorithm to Algorithm ). 3..4 Results from a simulation with the three algorithms Next the results from using the algorithms presented in the three previous sections. The test case that will be used is the path described in the example in Section.7, i.e., p = [.5,.5,] ; p = [.,.45,.] ; p3 = [.5,.6,.5] ; zone =.; zone3 = ; sec = movelzone(p,p,p3,zone,zone3,j) 9
This means the path shown as a solid line in Figure. In all the simulations j =,, 3 will be tested to see how the different algorithms for creating the path in Cartesian space give an impact on the resulting joint space path. The parameter γ in the algorithms is chosen γ = 5, i.e., the maximum error allowed becomes µm. In Tables to 3 the results from using the three algorithms is shown. In each case the three methods for generating the path in Cartesian space (described in Sections.4 to.6) is tested. From the number of splines it is clear that the path generated with method, a path that has continuous derivative and continuous second derivative, is the easiest to follow using a cubic spline in joint space. Method No. of splines Min/Max step Min/Max error 3.5/. 8. /8.9 6.5/.45.7 7 /9. 6 3 7.5/..7 7 /5. 6 Table : Results from using Algorithm..5 φ φ φ 3.5.5.5 φ φ.5.5.4...4.6.8 l Path index.5.5.4...4.6.8 l Path index.9.8.7.6 x 5 Method 3 φ Error.5.4.3.5...4...4.6.8 l Path index.4...4.6.8 Spline number Figure 7: Results from using Algorithm. Angles for method (upper left), (upper right), and 3 (lower left). The resulting estimated error on the arm-side is shown in the lower right plot. The dotted line shows where the zone starts and ends. It is worth noting also that the step length is adjusted in a very simple way in the algorithms presented in Section 3.. The resulting spline does not have a continuous second derivative in joint space either which might be a limitation in the next step when a trajectory in joint space is to be found. In Figures 7 to 9 the resulting knots are shown for the three algorithms (and three cases for each algorithm). For method, which gives a discontinuous second derivative in the Cartesian path when passing the zone, it is clear that it takes many more splines to approximate the function in
Method No. of splines Min/Max step Min/Max error./..8 /7.7 6 6.5/..4 7 /4.7 6 3 6.5/. 4. 9 /6.9 6 Table : Results from using Algorithm. the neighborhood of the discontinuity. This is common for all the three algorithms. The path that is most difficult to approximate in all the three cases is the one generated by method 3. This path tries to mimic the one generated by method but still keeps a continuous second derivative. Method No. of splines Min/Max step Min/Max error 4.5/. 8.8 /9.9 6.5/.67.9 8 /.5 6 3 3.5/..8 8 /5.4 6 Table 3: Results from using 3 splines. 3. An iterative solution that gives continuous first and second derivative A tempting solution to the problem of generating a cubic spline function in joint space that has continuous derivative and continuous second derivative is to specify the position, the derivative and the second derivative in the first knot and let the derivative and second derivative be free in the final point. This algorithm can however be proven to be unstable. An alternative that seems more promising is to use a fourth order plus one or more third order polynomials in the computation of the joint path. This method uses a specified position, derivative and second derivative in the starting point but incorporates also the derivative in the final point. This algorithm has shown a stable behavior in simulations and also in a preliminary analysis. 4 Trajectory generation When planning a trajectory in joint space, the first question that will be addressed is how to achieve a constant velocity in Cartesian space. 4. Constant velocity along the Cartesian path From Section it is clear that, at least for some special cases, it is possible to find an analytic description of the path (as a function of path index l) in Cartesian space. From now on it will be assumed that this is the case. To achieve a constant speed along the path it is necessary that P = v, for some desired speed v. If P (l) is given from the geometrical interpolation explained in Section it means that, in general, P (l) isavector,p (l) = ( p x(l) p y(l) p ) T z(l),wherep( ) (l) can be assumed to be a cubic spline (or a lower order polynomial). Along a straight line in Cartesian space the polynomials are only of degree one and therefore the problem becomes much less complicated. We will now however focus on the cubic spline case, since this is the most general situation. When using the geometrical representation from Section it is clear that the following relation holds dp dt = dp dl dl (4) dt
.5 φ φ φ 3.5.5.5 φ φ.5.5.4...4.6.8 l Path index.5.5.4...4.6.8 l Path index.9.8.7.6 x 5 Method 3 φ Error.5.4.3.5...4...4.6.8 l Path index.4...4.6.8 Spline number Figure 8: Results from using Algorithm. Angles for method (upper left), (upper right), and 3 (lower left). The resulting estimated error on the arm-side is shown in the lower right plot. The dotted line shows where the zone starts and ends.
.5 φ φ φ 3.5.5.5 φ φ.5.5.4...4.6.8 l Path index.5.5.4...4.6.8 l Path index.9.8.7.6 x 5 Method 3 φ Error.5.4.3.5...4...4.6.8 l Path index.4...4.6.8 Spline number Figure 9: Results from using 3 splines. Angles for method (upper left), (upper right), and 3 (lower left). The resulting estimated error on the arm-side is shown in the lower right plot. The dotted line shows where the zone starts and ends. 3
where the chain rule is used. Now, since the velocity along the path is defined as dp it comes as dt a result from (4) that dp dt = dp dl dl (5) dt where it is assumed that dl >. This assumption must hold since we move along the path (in dt Cartesian or joint space) from, e.g., index to, and this must be a monotonic motion. If we assume P (l) to be the general function described above, then dp dl (p = x(l)) +(p y(l)) +(p z(l)) (6) where p ( )(l) is the derivative with respect to l. Whenp ( ) (l) is a cubic spline the derivative becomes a second order polynomial and the corresponding square becomes a fourth order polynomial. The sum of the squared derivatives in (6) therefore becomes a general fourth order polynomial. From (5) and (6) and the assumption that P = v(t) weget If we now introduce a new variable l c that fulfills dlc dt dl dt = v(t) (7) (p x (l)) +(p y(l)) +(p z(l)) = v(t) weget dl = dl c (p x (l)) +(p y(l)) +(p z(l)) Remark The interpretation of l c is that it is equal to the true length of the Cartesian path. Instead of directly considering the cubic spline case we start from the linear motion case. 4.. Linear motion in Cartesian space To describe a linear motion in Cartesian space P (l) becomes a vector of first order polynomials and, hence, dp is a constant vector. This implies that dp is constant and dl dl l c l = dp/dl, dl c(t) = v(t) (8) dt to give a certain velocity profile along the linear motion. 4.. Motion along a general path If the polynomials in P (l) are general polynomials, then dp becomes a higher order polynomial and dl the normalized spline description P (l c) becomes not so easy to compute analytically. It is however always possible to compute the approximate solution using a sum approximation of the integral along the path. In Figure the test-case is shown that will be used in the evaluation of the algorithms for trajectory generation. It consists of four movel instructions with zones of mm in Cartesian coordinates. The kinematic model that has been used in this test-case is the IRB4. The orientation of the tool is included in this path and therefore the resulting joint path includes all 6 joints. By computing l c(l) = l l P ( l + l ) P ( l) (9) the relation between the path index l and the normalized path index l c can be found. The cost of computing the sum in (9) with a high resolution, i.e., a small l is not so high in practice and 4
z [m].4.8.6.4 y [m] 3. 4..4 x [m].6.8 Figure : The test-case used in the evaluation of the trajectory generation algorithm..4.5..8.5 l c φ.6.4..5.5.5 3 3.5 4 4.5 path index.5 j 3 4 5 6..4.6.8. l c Figure : The normalized path index l c as a function of the traditional path index l (left). The spline representation of the path in joint space, computed using the normalized path index l c (right). 5
this approximation can therefore be made well. In Figure l c(l) is shown for the example treated in this section. Next the idea is to compute the path in joint space as a function of the new index l c according to the algorithms described in Section 3. The result is shown to the right in Figure. 4..3 Limitations in Cartesian space The first problem that will be considered is limitation in Cartesian coordinates. In the first suggested algorithm limitation with respect to speed and acceleration will be considered. The input to the algorithm is desired velocity v d, and maximum acceleration a max. Algorithm 3 (Limitation in Cartesian space). v p =, l p =,andt p =. For each spline interval, do: (a) Get v d for the current spline (b) l n = l c at the beginning of next spline (c) if v p v d % Constant speed t = ln lp v d l c(t) =v d (t t p)+l p else % Acceleration vd a =min(a max, v p ) (l n l p) if a<ɛ t = ln lp v d l c(t) =v d (t t p)+l p else t = vp a ( + vp ) + ln lp a a l c(t) =a/(t tp) + v p(t t p)+l p end end (d) l p = l n (e) t p = t p + t (f) v p = dlc(t) dt t=tp In Algorithm 3 the function l c(t) is created locally for each spline. The spline length is decided based upon the specified path accuracy, compare Algorithms, and the corresponding 4 knots solution from Section 3..3. This is clearly not an optimal solution which is shown in the next example. Algorithm 3 is applied to the test-case shown in Figure with the desired speed set to v d =[.,.5,.5,.5] m/s and the maximum acceleration is set to m/s. The results from applying Algorithm 3 on this case is shown in Figure. Notice that the spline lengths are not optimal with respect to the performance. For example, if the first spline had been shorter it should have been possible to use thefullaccelerationm/s and hence reach the desired speed earlier. This means a shorter total cycle time. Also when going from. m/s to.5 m/s, and.5 m/s to.5 m/s there is time to be gained by changing the length of the splines. In Figure 3 the resulting joint trajectory is shown. 6
.5.45.9.4.8.35.7.3.6 d l c (t) / dt.5. d l c (t)/dt.5.4.5.3...5. 3 4 5 6 7 t [sec] 3 4 5 6 7 t [sec] Figure : The resulting Cartesian path speed (left) and the corresponding Cartesian path acceleration (right)..5 d φ(t)/dt d φ(t)/dt.5 j 3 4 5 6 3 4 5 6 7 t [sec] 3 j 3 4 4 5 6 5 3 4 5 6 7 t [sec] Figure 3: The resulting joint angular velocity (left) and the corresponding joint angular acceleration (right) after applying Algorithm 3. 7
4..4 Joint space limitations The second step after considering the limitations in the Cartesian space is to consider joint space limitations. The input to this algorithm is the maximum velocity, φmax R n, and the maximum acceleration, φ max R n. The number of joints is equal to n. In general the limits in joint space will depend on the dynamical model of the system but in this simplified case fixed limits are considered. Algorithm 4 (Joint space limitations). v p =, l p =,andt p =. For each spline interval, do: (a) Get φ max and φ max for the current spline (b) l n = l c at the beginning of next spline (c) t n = time at the end of the current l c(t) spline (d) % Check speed at a grid φ eval =[ d ˆφ(l c) dl c dlc tn tp ], t = tp : : dt n tn eval For each joint j [,n], [ ˆφj,max,t j]=max φ j,eval (t) (t j is the time where the maximum is found) (e) % Check speed levels against max levels ˆφ [r, m joint] =max[ ˆφ,max,..., n,max, ] φ,max φ n,max if r> % Reduce the speed v n = dlc(t) dt t=tmjoint /r vn a n = v p (l c(t mjoint ) l p) ( t = vp vp ) a n + a n + ln lp a n t n = t p + t l c(t) = an (t tp) + v p(t t p)+l p end (f) % Check acceleration at a grid φ eval =[ d ˆφ(l c) ( dl c ) + d ˆφ(l c) dl c dt dl c dlc ], t = t dt p : tn tp : n tn eval For each joint j, [ ˆφj,max,t j]=max φ j,eval (t) (g) % Check acceleration levels against max levels ˆφ [r, m joint] =max[ ˆφ,max,..., n,max, ] φ,max φ n,max if r> % Reduce the acceleration v n = v p/ r a n = a r if a n < ɛ t = ln lc v n else t = vp a n + ( vp a n ) + ln lp a n end l c(t) = an (t tp) + v n(t t p)+l p end (h) l p = l n (i) t p = t p + t (j) v p = dlc(t) dt t=tp After applying Algorithm 4 to the test case in Figure the Cartesian speed and acceleration become like in Figure 4. The maximum angular velocity is here set to φ max =[4, 4, 4, 4, 4, 4] 8
and the maximum angular acceleration φ max =[,,,,, ]. Compare to the result in Figure 3. Clearly there are no active restrictions until the final segment where the speed is actually too high. This shows that the speed also gives raise to an acceleration term when considering angular acceleration and Cartesian path velocity. In the last segment the path acceleration is limited because joint has an acceleration greater than rad/s. In Figure 5 the resulting angular velocity and acceleration is shown. By applying Algorithm 4, only velocity and acceleration constraints are considered. Deceleration constraints are not taken into account and the deceleration ramp necessary to make the robot stop at the final point is for example not implemented..5.45.9.4.8.35.7.3.6 d l c (t) / dt.5. d l c (t)/dt.5.4.5.3...5. 3 4 5 6 7 t [sec] 3 4 5 6 7 t [sec] Figure 4: The resulting Cartesian path speed (left) and the corresponding Cartesian path acceleration (right) after applying Algorithm 4..5.5.5 j 3 4 5 6 d φ(t)/dt d φ(t)/dt.5 j 3 4 5 6 3 4 5 6 7 t [sec].5.5 3 4 5 6 7 t [sec] Figure 5: The resulting joint angular velocity (left) and the corresponding joint angular acceleration (right) after applying Algorithm 4. 4..5 Issues to be studied in more detail Clearly many aspects need to be considered before claiming that the presented solution is ready. As indicated in the remarks after the discussion on Algorithm 3 this algorithm does not give an optimal solution. To achieve a more optimal solution the length of the splines describing l c(t) needs to be shorter than the corresponding splines used to represent the path, ˆφ(l c). This comment also applies to Algorithm 4. In neither of the two algorithms the deceleration is considered and therefore the limit on the acceleration is not valid for deceleration. The deceleration case should be implemented at least 9
in the Cartesian case since otherwise it is possible that the deceleration will be higher than the specified Cartesian acceleration value. 4. A combined algorithm for Cartesian and joint limitations Instead of considering the limitations in Cartesian and joint space separated we will now consider an algorithm that takes both into account. Algorithm 5 (Cartesian and joint space limitations in parallel). v p =, l p =,andt p =. For each spline interval, do: (a) Get φ max and φ max for the current spline (b) l n = l c at the beginning of next spline (c) t n = time at the end of the current l c(t) spline (d) % Check speed at a grid φ eval =[ d ˆφ(l c) dl c dlc tn tp ], t = tp : : dt n tn eval For each joint j [,n], [ ˆφj,max,t j]=max φ j,eval (t) (t j is the time where the maximum is found) (e) % Check speed levels against max levels ˆφ [r, m joint] =max[ ˆφ,max,..., n,max, ] φ,max φ n,max if r> % Reduce the speed v n = dlc(t) dt t=tmjoint /r vn a n = v p (l c(t mjoint ) l p) ( t = vp vp ) a n + a n + ln lp a n t n = t p + t l c(t) = an (t tp) + v p(t t p)+l p end (f) % Check acceleration at a grid φ eval =[ d ˆφ(l c) ( dl c ) + d ˆφ(l c) dl c dt dl c dlc ], t = t dt p : tn tp : n tn eval For each joint j, [ ˆφj,max,t j]=max φ j,eval (t) (g) % Check acceleration levels against max levels ˆφ [r, m joint] =max[ ˆφ,max,..., n,max, ] φ,max φ n,max if r> % Reduce the acceleration v n = v p/ r a n = a r if a n < ɛ t = ln lc v n else t = vp a n + ( vp a n ) + ln lp a n end l c(t) = an (t tp) + v n(t t p)+l p end (h) l p = l n (i) t p = t p + t (j) v p = dlc(t) dt t=tp
This algorithm suffer from some of the problems indicated in the previous section. It still does not give an optimal solution and the acceleration is reduced more than what is actually necessary. 5 Conclusions and further study This report contains three major parts, path generation in Cartesian space, transformation and representation of the path in joint space, and finally generation of trajectories in Cartesian space and in joint space. It does not give the final answers on exactly how to solve the different problems in these areas but does provide the reader with ideas that can be used in order to solve the different problems. A number of things are however still to do and this is a list of some of the most important items: Path generation in Cartesian space: Generation of other paths than lines. To include also the orientation of the tool in the path representation. Implementation of more general instructions for path generation in Matlab. Considering the problem of connecting circles to lines and circles to circles using zones. Path generation in joint space: Development of algorithms that give continuous first and second derivative with respect to path index. Development of the algorithms so that when reaching a new segment it is guaranteed to be a new spline. Analytical results on which method to choose for interpolating the splines in joint space. A theoretical analysis of the error in the approximation that is achieved when estimating the maximum Cartesian error for the interpolated joint path. Trajectory generation: Sensitivity analysis of the method to compute l c from the Cartesian path using a finite sum approximation of an integration. Development of a more general optimal trajectory generation algorithm that considers Cartesian and joint space in parallel. Further improvements on Algorithm 3 and Algorithm 4 to make them more optimal by making the l c(t) splines not fixed to the joint spline description. Study the effect of the smoothness of the path generated in Cartesian space and in joint space (different levels of continuity. References [] John J. Craig. Introduction to robotics: Mechanics and control. Addison-Wesley Publishing Company, second edition, 989. [] L. Sciavicco and B. Siciliano. Modelling and Control of Robot Manipulators. Springer,. [3] F.L. Chernousko. Optimization in control of robots. International Series of Numerical Mathematics, 5:9 8, 994. [4] Ola Dahl. Path Constrained Robot Control. PhD thesis, Department of Automatic Control, Lund Institute of Technology, 99. [5] M. Norrlöf. Modeling of industrial robots. Technical Report LiTH-ISY-R-8, Department of Electrical Engineering, Linköpings universitet, Dec 999. [6] Carl de Boor. A Practical Guide to Splines. Springer-Verlag, 978. [7] Carl de Boor. Spline Toolbox, For Use with MATLAB TM. The MathWorks Inc., Natric, MA, USA, Jan. 999.
Appendix A: A basic spline toolbox To be able to use splines without the commercial spline toolbox in Matlab, a basic set of functions has been developed. This includes a data structure to represent splines as well as some functions to construct a spline and to evaluate the spline at certain points plus some more functions. Next the functions in the basic spline toolbox is described. Syntax Description Example sp = makesp(brp,coefs,order); Constructs a spline from a set of break points (knots), brp, coefficientsincoefs, and an optional vector order that contains the order of the different polynomials (if they are not the same). Creation of a simple spline function >> sp = makesp([ ],[ 3 6]) sp = brp: [ ] coefs: [ 3 6] order: maxorder: dim: Syntax value = evalsp(sp,x); Description Evaluate the spline sp in the point given by x. Example Evaluation of the spline created in the makesp example >> evalsp(sp,.56) ans = 4.4336 Syntax Description Example dsp = dersp(sp); Differentiate the spline sp and returns the result as a spline in dsp. Differentiate the polynomial created in the makesp example >> dsp = dersp(sp) dsp = brp: [ ] maxorder: coefs: [ 4 ] order: dim: Syntax Description Example nsp = appendsp(sp,asp); Append the spline given by asp to the splines in sp. The result is returned as a new spline. If the two splines are of different order the result is always of the maximum order of the two. Append a spline to the spline created in the makesp example (note that the order of sp is one less than that of sp while the result is that of sp) >> sp = makesp([ 3],[- 9]); >> nsp = appendsp(sp,sp) nsp = brp: [ 3] maxorder: coefs: [ 3 6-9] order: dim:
Syntax Description Example csp = completesp(brp,p,[d d]); Compute the complete spline, i.e., a number of cubic splines with knots in brp having functions values given by p. The derivative in the first and the last knot is given by [d d]. Compute the complete spline with knots in [.5.5] with function value [.5.5] and derivative [.5 -] >> csp = completesp([.5.5],[.5.5],[.5 -]) csp = brp: [.5.5] coefs: [x double] order: 3 maxorder: 3 dim: In Figure 6 the resulting function and its derivative is shown (generated by using plot(:.:.5,evalsp(csp,:.:.5)) and plot(:.:.5,evalsp(dersp(csp),:.:.5)))..5 3.5 f(x) f (x) 3 4.5 5 6..4.6.8..4 x 7..4.6.8..4 x Figure 6: Example of a complete spline (function left and its derivative right). 3
Avdelning, Institution Division, Department Division of Automatic Control Department of Electrical Engineering Datum Date 3--7 Språk Language Svenska/Swedish X Engelska/English... Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats X Övrig rapport... ISBN... ISRN... Serietitel och serienummer Title of series, numbering ISSN 4-39... URL för elektronisk version LiTH-ISY-R-49 http://www.control.isy.liu.se Titel Title Författare Author On path planning and optimization using splines Mikael Norrlöf, Sammanfattning Abstract This report covers many aspects of path and trajectory generation for industrial robots. Path generation in Cartesian space is discussed, with the limitation that only linear motion is considered. Path generation in joint space is also discussed and in particular representation of the joint path using cubic splines is presented. Different algorithms for spline generation are also discussed and tested on an example. The last step, the trajectory generation is also covered. Two preliminary algorithms that gives limited speed and acceleration in Cartesian space and in joint space are given. It is also indicated that there is more to be done in order to reach an optimal trajectory. At the end of the report a simple spline toolbox for Matlab is described.. Nyckelord Keywords path planning, trajectory generation, cubic spline, cartesian space, joint space