• No results found

On Path Planning and Optimization using Splines

N/A
N/A
Protected

Academic year: 2021

Share "On Path Planning and Optimization using Splines"

Copied!
28
0
0

Loading.... (view fulltext now)

Full text

(1)

On path planning and optimization using splines

Mikael Norrl¨

of

Division of Automatic Control

Department of Electrical Engineering

Link¨

opings universitet

, SE-581 83 Link¨

oping, Sweden

WWW:

http://www.control.isy.liu.se

E-mail:

mino@isy.liu.se

17th February 2003

AUTOMATIC CONTROL

COM

MUNICATION SYSTEMS

LINKÖPING

Report no.:

LiTH-ISY-R-2490

Submitted to

Technical reports from the Control & Communication group in Link¨oping are available athttp://www.control.isy.liu.se/publications.

(2)

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, carte-sian space, joint space

(3)

Contents

1 Introduction and overview 1

2 Generate a path in Cartesian space 1

2.1 Motion along straight line . . . 1

2.2 A three point linear motion . . . 1

2.3 A general case . . . 2

2.4 Second order polynomial . . . 3

2.5 Two third order polynomials . . . 4

2.6 Two fourth and two third order polynomials . . . 5

2.7 Matlab implementation of the moveL instruction . . . 5

3 Path generation in joint space 6 3.1 An iterative spline solution that gives continuous derivative . . . 7

3.1.1 2 knots . . . 7

3.1.2 3 knots . . . 8

3.1.3 4 knots . . . 9

3.1.4 Results from a simulation with the three algorithms . . . 9

3.2 An iterative solution that gives continuous first and second derivative . . . 11

4 Trajectory generation 11 4.1 Constant velocity along the Cartesian path . . . 11

4.1.1 Linear motion in Cartesian space . . . 14

4.1.2 Motion along a general path . . . 14

4.1.3 Limitations in Cartesian space . . . 16

4.1.4 Joint space limitations . . . 18

4.1.5 Issues to be studied in more detail . . . 19

4.2 A combined algorithm for Cartesian and joint limitations . . . 20

5 Conclusions and further study 21

(4)
(5)

1

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 [1], Sciavicco [2], 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 p2,v100,z100,tool0 moveL p3,v300,fine,tool0

This means that the robot should move along a straight line from a position and orientation in p1 (not explicitly given) towards the position and orientation given by p2 and then when inside a zone of 100 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 IRB1400 robot, described in e.g., [5], and the similar IRB2400.

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.

2

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.

2.1

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 = p1+ L(p2− p1), 0 ≤ L ≤ 1 (1)

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.

2.2

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 p2,v100,z100,tool0 moveL p3,v300,fine,tool0

(6)

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 0.5 x [m] y [m] p1 p2 p3

Figure 1: The result of two moveL instructions with a zone at point p2.

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.

2.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 2. 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.

−0.50 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 x [m] y [m]

Figure 2: 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 1). 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, Pj(l) with l in [−lz1, lz2] and j as the section number. Notice

(7)

derivative of pj,x(l) with respect to l will not necessarily be a continuous function in the point

where we go from section j − 1 to section j. This is the reason why the sections are grouped according to Figure 3. Inside the second section the index l is with respect to p2+ l(p3− p2). For the second section lz1 is the l where p2+ l(p3− p2) intersects the zone and lz2= 0.

−0.50 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 x [m] y [m]

Figure 3: Representation of the path in sections. Solid line is section 1, dashed line is section 2.

2.4

Second order polynomial

We will only consider the polynomials representing the path in x and y since z ≡ 0 in the case described in Section 2.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,

P1(l) = p1+ l(p2− p1), l ∈ [0, lz2] (2)

where lz2 can be computed from

lz2= p z2

2− p1 (3)

with z2 the size of the zone at p2.

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

˜

p1= p1+ lz2(p2− p1) (4)

then the polynomial inside the zone is given by

¯ P2(l) =  ∆y p˜1,x+ ∆x(l + lz1) 2lz1(l + lz1) 2− ∆ y(l + lz1) + ˜p1,y 0   , −lz1≤ l ≤ lz1 (5) with ∆ =  ∆∆xyz = ¯p3 (6)

and outside the zone

(8)

Since p1, p2and p3are not in the x-y-plane with p2in the origin, ¯p1, ¯p2= 0 and ¯p3are used instead. Now ¯p1 and ¯p3 can be found from p1, p2 and p3 using the following transformations



p1− p2 p3− p2=xe ye ze

  ¯

p1 p¯3 (8)

where xe, ye, and zeare 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,

ye= pp1+ p3− 2p2 1+ p3− 2p2 xe= (p3− p2)(p3− p2)· ye  ye (p3− p2)(p3− p2)· ye  ye ze= xxe× ye e× ye (9)

The description of the path inside the zone becomes

P2(l) =xe ye ze

¯

P2(l) + p2, − ≤ lz1≤ l ≤ lz1 (10)

This kind of description will be used also in the next sections.

2.5

Two third order polynomials

The description of the path outside the zone is exactly as in Section 2.4. The difference is the description inside the zone where ¯P2(l) becomes

¯ P2(l) =    ˜ p1,x+ ∆x(l + lz1) ∆y 3l2 z1(l + lz1 )3− ∆y(l + lz1) + ˜p1,y 0    , −lz1≤ l ≤ 0 (11) and ¯ P2(l) =    ˜ p1,x+ ∆x(l + lz1) y 3l2 z1l 3y lz1l 2+ ˜p 1,y−2lz13y 0    , 0 ≤ l ≤ lz1 (12)

where ∆ is defined in (6). P2(l) is computed as in (10). 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. −0.1 −0.05 0 0.05 0.1 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0.045 x [m] y [m]

Figure 4: Comparison of the result when using the second order solution in Section 2.4 (dotted) and the two third order spline solution in Section 2.5 (solid).

(9)

2.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× 2 + 4 × 2 = 18 and the number of conditions

3× 2 + 4 × 3 = 18. The positions inside the zone are symmetric, i.e., the first v1and the third point

v3 correspond to each other in the following way,

v3=  vv3,x3,y v3,z   =  −vv1,y1,x v1,z   (13)

with v1,z = v3,z= 0. Now ¯P2,x(l) and ¯P2,z(l) are the same as in (11) while

¯

P2,y(l) =p1,y− 7∆ylz1/2 − 12v1,y+ 3v2,y 5(lz1/2)4 (l + lz1

)4

+−14˜p1,y+ 6∆ylz1+ 17v1,y− 3v2,y

5(lz1/2)2 (l + lz1 )3 − ∆y(l + lz1) + ˜p1,y, −lz1≤ l ≤ − lz1 2 (14) ¯

P2,y(l) =−6˜p1,y+ 3∆ylz1/2 + 13v1,y− 7v2,y

5(lz1/2)3 (l +

lz1

2 )

3

+3(4˜p1,y− ∆ylz1− 7v1,y+ 3v2,y) 5(lz1/2)2 (l +

lz1

2 )

2

+3(−2˜p1,y+ ∆ylz1/2 + v1,y+ v2,y)

5lz1/2 (l + lz1 2 ) + v1,y, − l z1 2 ≤ l ≤ 0 (15) ¯

P2,y(l) =p1,y− 3∆ylz1/2 − 13v1,y+ 7v2,y

5(lz1/2)3 (l + lz1

)3

+3(−2˜p1,y+ ∆ylz1/2 + 6v1,y− 4v2,y)

5(lz1/2)2 (l + lz1

)2+ v2,y, 0≤ l ≤ lz1

2

(16)

¯

P2,y(l) =p1,y− 7∆ylz1/2 − 12v1,y+ 3v2,y

5(lz1/2)4 (l −

lz1

2 )

4

+−22˜p1,y+ 8∆ylz1+ 31v1,y− 9v2,y

5(lz1/2)3 (l −

lz1

2 )

3

3(4˜p1,y− ∆ylz1− 7v1,y+ 3v2,y)

5(lz1/2)2 (l −

lz1

2 )

2

3(−2˜p1,y+ ∆ylz1+ v1,y+ v2,y)

5(lz1/2) (l − lz1 2 ) + v1,y, lz1 2 ≤ l ≤ lz1 (17)

P2(l) is computed as in (10). The expression in Equations 14 to 17 have been found using Mathe-matica. 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 2.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.

2.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.

(10)

−0.1 −0.05 0 0.05 0.1 0.02 0.025 0.03 0.035 0.04 0.045 x [m] y [m]

Figure 5: Comparison of the result when using the second order solution from Section 2.4 (dotted) and the four spline solution in Section 2.6 (solid).

Syntax [sec,coeffs] = movelzone(p1,p2,p3,zone2,zone3,method);

Description 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==1 (default) the second order polynomial solution from Section 2.4 is used.

(b) If method==2 the two third order polynomial solution from Section 2.5 is used. (c) If method==3 the method described in Section 2.6 is used.

The points p1 to p3 are the one shown in Figure 1 and the section, returned in sec, is the dashed part in Figure 3. The parameters zone2 and zone3 are the size of the zones at p2 and p3, respectively.

Example 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.

p1 = [0.5,0.25,1]’; p2 = [0.1,0.45,1.1]’; p3 = [0.5,0.60,1.25]’; zone2 = 0.1; zone3 = 0; sec = movelzone(p1,p2,p3,zone2,zone3,3) plot3([p1(1) p2(1) p3(1)], ... [p1(2) p2(2) p3(2)],[p1(3) p2(3) p3(3)],’:’) hold on pos = evalsp(sec,sec.brp(1):0.01:sec.brp(end))’; plot3(pos(:,1),pos(:,2),pos(:,3));

3

Path generation in joint space

Given a path representation in Cartesian space this path has to be transformed into a path repre-sentation 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.

(11)

0.1 0.2 0.3 0.4 0.5 0.2 0.4 0.6 0.8 1 1.05 1.1 1.15 1.2 1.25 x [m] y [m] z [m]

Figure 6: The result from running the Matlab code in the example describing the movelzone command.

3.1

An iterative spline solution that gives continuous derivative

Three different versions of the algorithm will be discussed in this section. One based on 2 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.1.1 2 knots

The first solution is based on 2 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, φjand φjwith j = k, k + 1 the

solution is found as ˆ φ(l) = 2(φk− φk+1) + ∆k(φ  k+ φk+1) ∆3k (l − lk) 3 −3(φk− φk+1) + ∆k(2φk+ φk+1) ∆2k (l − lk) 2 + φk(l − lk) + φk, lk≤ l ≤ lk+1 (18)

To evaluate the solution the direct kinematics is computed in the intermediate point, lk+1/2 =

lk+lk+12−lk, and the resulting Cartesian point is compared to the true position from the spline

representation of the path in Cartesian space. IfT ( ˆφ(lk+1/2))−P (lk+1/2) < γ for some norm and

γ then the spline is considered ok, otherwise the step length is changed to ∆k/2 and a new spline

is computed as above. T (·) represents the direct kinematic model.

Algorithm 1 (An iterative algorithm for spline interpolation using 2 knots)

1. Compute the inverse kinematic solution at P (l) and P (l +∆l), resulting in φ(l) and φ(l +∆l).

2. Find an estimate of dφ(l)dl and dφ(l+∆l)

dl (using for example the Jacobian).

3. Compute the coefficients in the cubic spline from (18).

4. Evaluate the resulting spline, ˆφ(l) in l + ∆l/2

(12)

where P (·) is assumed to be the path representation in Cartesian space. If the 2-norm is used

γ represents the maximum error in Cartesian space that is allowed1.

a. If the inequality in (19) is fulfilled then keep the spline and let

l = l + ∆l,l= 2∆l b. else letl= ∆l 2 5. Goto 1. 3.1.2 3 knots

The 3 knots solution gives as a result 2 cubic splines given the joint position and the joint position derivative at the first and the last knot, φjand φjwith j = k, k + 2 and joint position in the middle

knot, φk+1. The solution is found as

ˆ φ(l) =∆2k+1(φk−φk+1)+∆kk+1(4(φk−φk+1)+∆k+1φk)+∆2k(−3φk+1+3φk+2+2∆k+1φk−∆k+1φk+2) 2∆2 kk+1(∆k+∆k+1) · (l − lk)3+ +3∆2k+1(φk+1−φk)−3∆kk+1(2(φk−φk+1)+∆k+1φk)+∆2k(3(φk+1−φk+2)−4∆k+1φk+∆k+1φk+2) 2∆kk+1(∆k+∆k+1) · (l − lk)2+ φk(l − lk) + φk, lk≤ l ≤ lk+1 (20) ˆ φ(l) = ∆2k(φk+1−φk+2)+∆kk+1(4(φk+1−φk+2)+∆kφk+2)+∆2k+1(3(φk+1−φk)−∆kφk+2∆kφk+2) 2∆k∆2k+1(∆k+∆k+1) · (l − lk+1)3+ +3∆k(φk+2−φk+1)+∆k+1(3(φk−φk+1)+∆k(φk−φk+1)) ∆kk+1(∆k+∆k+1) (l − lk+1) 2 3∆2k(φk+1−φk+2)+∆2k+1(3(φk−φk+1)+∆kφk)+∆2kk+1φk+2 2∆kk+1(∆k+∆k+1) (l − lk+1) + φk+1, lk+1≤ l ≤ lk+2 (21)

The result in (20) and (21) can be simplified if it can be assumed that ∆k = ∆k+1. To find the

solution in (20) and (21) it is assumed that ˆφ(l) in each interval is given by

ˆ

φ(l) = a1+ a2(l − lk) + a3(l − lk)2+ a4(l − lk)3 (22)

For the first spline it follows that ˆ

φ1(lk) = φ(lk), φˆ1(lk+1) = φ(lk+1), φˆ1(lk) = φ(lk)

and for the second spline the conditions become ˆ

φ2(lk+2) = φ(lk+2), φˆ2(lk+2) = φ(lk+2)

To get a continuous function with continuous first and second derivative at the inner knot we also have the conditions

ˆ

φ1(lk+1) = ˆφ2(lk+1), φˆ1(lk+1) = ˆφ2(lk+1), φˆ1(lk+1) = ˆφ2(lk+1)

which give a total of 8 conditions. From (22) 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 (11) and (12).

Algorithm 2 (An iterative algorithm for spline interpolation using 3 knots)

1The limit is not guaranteed and therefore a larger error can be achieved in some other point along the

(13)

1. Compute the inverse kinematic solution at P (l), P (l + ∆l) and P (l + 2∆l), resulting in φ(l),

φ(l + ∆l) and φ(l + 2∆l).

2. Find an estimate of dφ(l)dl and dφ(l+2∆l)

dl (using for example the Jacobian).

3. Compute the coefficients in the cubic splines from (11) and (12).

4. Evaluate the resulting spline, ˆφ(l) in l + ∆l/2 and l + 3∆l/2

T ( ˆφ(l + ∆l/2)) − P (l + ∆l/2) < γ, T ( ˆφ(l + 3∆l/2)) − P (l + 3∆l/2) < γ (23)

where P (·) is assumed to be the path representation in Cartesian space. If the 2-norm is used γ represents the maximum error in Cartesian space that is allowed.

a. If the inequality in (23) is fulfilled then keep the spline and let

l = l + ∆l,l= 2∆l b. else letl= ∆l 2 5. Goto 1. 3.1.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 (22) is used. For the first spline the following conditions must be fulfilled

ˆ

φ1(lk) = φ(lk), φˆ1(lk+1) = φ(lk+1), φˆ1(lk) = φ(lk)

and for the last spline the conditions become ˆ

φ3(lk+3) = φ(lk+3), φˆ3(lk+3) = φ(lk+3)

To get a continuous function with continuous first and second derivative at the inner knot we also have the conditions

ˆ

φ1(lk+1) = ˆφ2(lk+1), φˆ1(lk+1) = ˆφ2(lk+1), φˆ1(lk+1) = ˆφ2(lk+1)

and

ˆ

φ2(lk+1) = ˆφ3(lk+1), φˆ2(lk+1) = ˆφ3(lk+1), φˆ2(lk+1) = ˆφ3(lk+1)

This gives a total of 12 conditions. From (22) we see that each spline has 4 parameters which gives a total of 12 parameters and a system of equations that has one unique solution.

The algorithm for this case is similar to Algorithm 2 with obvious extensions (compare going from Algorithm 1 to Algorithm 2).

3.1.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 2.7, i.e.,

p1 = [0.5,0.25,1]’; p2 = [0.1,0.45,1.1]’; p3 = [0.5,0.60,1.25]’; zone2 = 0.1; zone3 = 0;

(14)

This means the path shown as a solid line in Figure 1. In all the simulations j = 1, 2, 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 γ = 10−5, i.e., the maximum error allowed becomes 10µm. In Tables 1 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 2.4 to 2.6) is tested. From the number of splines it is clear that the path generated with method 2, 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

1 13 0.0125/0.2 8.0 · 10−10/8.9 · 10−6 2 12 0.025/0.245 2.7 · 10−7/9.2 · 10−6 3 17 0.025/0.2 2.7 · 10−7/5.1 · 10−6

Table 1: Results from using Algorithm 1.

−0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ φ1 φ2 φ3 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ −0.40 −0.2 0 0.2 0.4 0.6 0.8 1 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1x 10 −5 Spline number Error Method 1 2 3

Figure 7: Results from using Algorithm 1. Angles for method 1 (upper left), 2 (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.1. 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 1, 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

(15)

Method No. of splines Min/Max step Min/Max error

1 20 0.01/0.2 2.8 · 10−11/7.7 · 10−6

2 16 0.025/0.2 1.4 · 10−7/4.7 · 10−6 3 26 0.025/0.2 4.2 · 10−9/6.9 · 10−6

Table 2: Results from using Algorithm 2.

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 1 but still keeps a continuous second derivative.

Method No. of splines Min/Max step Min/Max error

1 24 0.0125/0.1 8.8 · 10−10/9.9 · 10−6 2 21 0.025/0.1067 1.9 · 10−8/1.5 · 10−6 3 30 0.0125/0.1 1.8 · 10−8/5.4 · 10−6

Table 3: Results from using 3 splines.

3.2

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.1

Constant velocity along the Cartesian path

From Section 2 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  = v0, for some desired speed

v0. If P (l) is given from the geometrical interpolation explained in Section 2 it means that, in general, P (l) is a vector, P (l) =px(l) py(l) pz(l)

T

, where p(·)(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 2 it is clear that the following relation holds

dP

dt =

dP

(16)

−0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ φ1 φ2 φ3 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ −0.40 −0.2 0 0.2 0.4 0.6 0.8 1 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1x 10 −5 Spline number Error Method 1 2 3

Figure 8: Results from using Algorithm 2. Angles for method 1 (upper left), 2 (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.

(17)

−0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ φ1 φ2 φ3 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −1 −0.5 0 0.5 1 1.5 l − Path index φ −0.40 −0.2 0 0.2 0.4 0.6 0.8 1 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1x 10 −5 Spline number Error Method 1 2 3

Figure 9: Results from using 3 splines. Angles for method 1 (upper left), 2 (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.

(18)

where the chain rule is used. Now, since the velocity along the path is defined asdP

dt it comes as

a result from (24) that

 dP dt   =dP dl   ·dl dt (25)

where it is assumed that dl

dt > 0. This assumption must hold since we move along the path (in

Cartesian or joint space) from, e.g., index 0 to 1, and this must be a monotonic motion.

If we assume P (l) to be the general function described above, then 

dP dl



 =(px(l))2+ (py(l))2+ (pz(l))2 (26)

where p(·)(l) is the derivative with respect to l. When p(·)(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 (26) therefore becomes a general fourth order polynomial. From (25) and (26) and the assumption that ˙P  = v(t) we get

dl dt = v(t)  (px(l))2+ (py(l))2+ (pz(l))2 (27)

If we now introduce a new variable lcthat fulfills dldtc = v(t) we get

dl

dlc

= 1

(px(l))2+ (py(l))2+ (pz(l))2

Remark 1 The interpretation of lc 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.1.1 Linear motion in Cartesian space

To describe a linear motion in Cartesian space P (l) becomes a vector of first order polynomials and, hence, dPdl is a constant vector. This implies thatdPdl is constant and

l = dP/dllc , dlc(t)

dt = v(t) (28)

to give a certain velocity profile along the linear motion.

4.1.2 Motion along a general path

If the polynomials in P (l) are general polynomials, then dP

dl becomes a higher order polynomial and

the normalized spline description P (lc) 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 10 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 100 mm in Cartesian coordinates. The kinematic model that has been used in this test-case is the IRB2400. The orientation of the tool is included in this path and therefore the resulting joint path includes all 6 joints.

By computing

lc(l) =



¯l≤l

P (¯l+ ∆l)− P (¯l) (29)

the relation between the path index l and the normalized path index lc can be found. The cost of

(19)

0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1.4 0 x [m] 1 2 4 3 y [m] z [m]

Figure 10: The test-case used in the evaluation of the trajectory generation algorithm.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 0 0.2 0.4 0.6 0.8 1 1.2 1.4 path index lc 0 0.2 0.4 0.6 0.8 1 1.2 −1 −0.5 0 0.5 1 1.5 lc φ j − 1 2 3 4 5 6

Figure 11: The normalized path indexlc as a function of the traditional path indexl (left). The spline representation of the path in joint space, computed using the normalized path index lc (right).

(20)

this approximation can therefore be made well. In Figure 11 lc(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 lc according to

the algorithms described in Section 3. The result is shown to the right in Figure 11.

4.1.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 vd, and maximum acceleration amax.

Algorithm 3 (Limitation in Cartesian space)

1. vp= 0, lp= 0, and tp= 0

2. For each spline interval, do:

(a) Get vdfor the current spline

(b) ln= lc at the beginning of next spline

(c) if vp≥ vd % Constant speed t =ln−lp vd lc(t) = vd(t − tp) + lp else % Acceleration a = min(amax, vd2−v2 p 2(ln−lp)) if a < 20 t =ln−lp vd lc(t) = vd(t − tp) + lp else t = −vp a + vp a 2 + 2ln−lp a lc(t) = a/2(t − tp)2+ vp(t − tp) + lp end end (d) lp= ln (e) tp= tp+ t (f ) vp=dlcdt(t)t=t p

In Algorithm 3 the function lc(t) is created locally for each spline. The spline length is decided

based upon the specified path accuracy, compare Algorithms 1, 2 and the corresponding 4 knots solution from Section 3.1.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 10 with the desired speed set to

vd= [0.1, 0.25, 0.25, 0.5] m/s

and the maximum acceleration is set to 1 m/s2. The results from applying Algorithm 3 on this case is shown in Figure 12. 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 the full acceleration 1 m/s2 and hence reach the desired speed earlier. This means a shorter total cycle time. Also when going from 0.1 m/s to 0.25 m/s, and 0.25 m/s to 0.5 m/s there is time to be gained by changing the length of the splines. In Figure 13 the resulting joint trajectory is shown.

(21)

0 1 2 3 4 5 6 7 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 t [sec] d lc (t) / dt 0 1 2 3 4 5 6 7 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 t [sec] d 2lc (t)/dt 2

Figure 12: The resulting Cartesian path speed (left) and the corresponding Cartesian path acceleration (right). 0 1 2 3 4 5 6 7 −1 −0.5 0 0.5 t [sec] d φ (t)/dt j − 1 2 3 4 5 6 0 1 2 3 4 5 6 7 −5 −4 −3 −2 −1 0 1 2 t [sec] d 2φ (t)/dt 2 j − 1 2 3 4 5 6

Figure 13: The resulting joint angular velocity (left) and the corresponding joint angular acceleration (right) after applying Algorithm 3.

(22)

4.1.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∈ Rn, and the maximum

acceleration, ¨φmax∈ Rn. 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)

1. vp= 0, lp= 0, and tp= 0

2. For each spline interval, do:

(a) Get ˙φmax and ¨φmax for the current spline

(b) ln= lc at the beginning of next spline

(c) tn= time at the end of the current lc(t) spline

(d) % Check speed at a grid

˙

φeval= [d ˆφ(ldlcc) ·dldtc], t = tp:tnne−tvalp : tn

For each joint j ∈ [1, n], [φ˙ˆj,max, tj] = max ˙φj,eval(t) (tjis the time where the maximum

is found)

(e) % Check speed levels against max levels

[r, mjoint] = max[ ˙ˆφ1,max ˙φ1,max, . . . , ˙ˆφn,max ˙φn,max, 1] if r > 1

% Reduce the speed

vn=dldtc(t)t=t mjoint/r an= v2n−vp2 2(lc(tmjoint)−lp) t = −vp an+ vp an 2 + 2ln−lp an tn= tp+ t lc(t) = a2n(t − tp)2+ vp(t − tp) + lp end

(f ) % Check acceleration at a grid

¨ φeval= [d 2φ(lˆ c) dl2c · dl c dt 2 +d ˆφ(lc) dlc · d2lc dt2], t = tp: tn−tp neval : tn

For each joint j, [¨ˆφj,max, tj] = max ¨φj,eval(t)

(g) % Check acceleration levels against max levels

[r, mjoint] = max[ ¨ˆφ1,max ¨ φ1,max, . . . , ¨ˆφn,max ¨ φn,max, 1] if r > 1

% Reduce the acceleration

vn= vp/ r an=ar if an< 20 t =ln−lc vn else t = −vp an+ vp an 2 + 2ln−lp an end lc(t) = a2n(t − tp)2+ vn(t − tp) + lp end (h) lp= ln (i) tp= tp+ t (j) vp=dlcdt(t)t=t p

After applying Algorithm 4 to the test case in Figure 10 the Cartesian speed and acceleration become like in Figure 14. The maximum angular velocity is here set to ˙φmax = [4, 4, 4, 4, 4, 4]

(23)

and the maximum angular acceleration ¨φmax= [2, 2, 2, 2, 2, 2]. Compare to the result in Figure

13. 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 1 has an acceleration greater than 2 rad/s2. In Figure 15 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.

0 1 2 3 4 5 6 7 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 t [sec] d lc (t) / dt 0 1 2 3 4 5 6 7 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 t [sec] d 2lc (t)/dt 2

Figure 14: The resulting Cartesian path speed (left) and the corresponding Cartesian path acceleration (right) after applying Algorithm 4.

0 1 2 3 4 5 6 7 −1 −0.5 0 0.5 t [sec] d φ (t)/dt j − 1 2 3 4 5 6 0 1 2 3 4 5 6 7 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 t [sec] d 2φ (t)/dt 2 j − 1 2 3 4 5 6

Figure 15: The resulting joint angular velocity (left) and the corresponding joint angular acceleration (right) after applying Algorithm 4.

4.1.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 lc(t) needs

to be shorter than the corresponding splines used to represent the path, ˆφ(lc). 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

(24)

in the Cartesian case since otherwise it is possible that the deceleration will be higher than the specified Cartesian acceleration value.

4.2

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)

1. vp= 0, lp= 0, and tp= 0

2. For each spline interval, do:

(a) Get ˙φmax and ¨φmax for the current spline

(b) ln= lc at the beginning of next spline

(c) tn= time at the end of the current lc(t) spline

(d) % Check speed at a grid

˙ φeval= [d ˆφ(ldlc) c · dlc dt], t = tp: tn−tp neval : tn

For each joint j ∈ [1, n], [φ˙ˆj,max, tj] = max ˙φj,eval(t) (tjis the time where the maximum

is found)

(e) % Check speed levels against max levels

[r, mjoint] = max[˙ˆφ˙φ1,max

1,max, . . . ,

˙ˆφn,max

˙φn,max, 1]

if r > 1

% Reduce the speed

vn=dldtc(t)t=t mjoint/r an= v2n−v2 p 2(lc(tmjoint)−lp) t = −vp an+ vp an 2 + 2ln−lp an tn= tp+ t lc(t) = a2n(t − tp)2+ vp(t − tp) + lp end

(f ) % Check acceleration at a grid

¨ φeval= [d 2φ(lˆ c) dl2c · dl c dt 2 +d ˆφ(lc) dlc · d2lc dt2], t = tp: tn−tp neval : tn

For each joint j, [¨ˆφj,max, tj] = max ¨φj,eval(t)

(g) % Check acceleration levels against max levels

[r, mjoint] = max[¨ˆφφ¨1,max

1,max, . . . ,

¨ˆφn,max

¨

φn,max, 1]

if r > 1

% Reduce the acceleration

vn= vp/ r an=ar if an< 20 t =ln−lc vn else t = −vp an+ vp an 2 + 2ln−lp an end lc(t) = a2n(t − tp)2+ vn(t − tp) + lp end (h) lp= ln (i) tp= tp+ t (j) vp=dlcdt(t)t=t p

(25)

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 lcfrom 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 lc(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

[1] John J. Craig. Introduction to robotics: Mechanics and control. Addison-Wesley Publishing Company, second edition, 1989.

[2] L. Sciavicco and B. Siciliano. Modelling and Control of Robot Manipulators. Springer, 2000. [3] F.L. Chernousko. Optimization in control of robots. International Series of Numerical

Mathe-matics, 115:19–28, 1994.

[4] Ola Dahl. Path Constrained Robot Control. PhD thesis, Department of Automatic Control, Lund Institute of Technology, 1992.

[5] M. Norrl¨of. Modeling of industrial robots. Technical Report LiTH-ISY-R-2208, Department of Electrical Engineering, Link¨opings universitet, Dec 1999.

[6] Carl de Boor. A Practical Guide to Splines. Springer-Verlag, 1978.

[7] Carl de Boor. Spline Toolbox, For Use with MATLABTM. The MathWorks Inc., Natric, MA, USA, Jan. 1999.

(26)

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 sp = makesp(brp,coefs,order);

Description Constructs a spline from a set of break points (knots), brp, coefficients in coefs, and an optional vector order that contains the order of the different polynomials (if they are not the same).

Example Creation of a simple spline function

>> sp = makesp([0 1 2],[1 2 3 2 1 6]) sp = brp: [0 1 2] coefs: [1 2 3 2 1 6] order: 2 maxorder: 2 dim: 1

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,0.56) ans =

4.4336

Syntax dsp = dersp(sp);

Description Differentiate the spline sp and returns the result as a spline in dsp. Example Differentiate the polynomial created in the makesp example

>> dsp = dersp(sp) dsp = brp: [0 1 2] maxorder: 1 coefs: [2 2 4 1] order: 1 dim: 1 Syntax nsp = appendsp(sp,asp);

Description 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.

Example Append a spline to the spline created in the makesp example (note that the order of sp2 is one less than that of sp while the result is that of sp)

>> sp2 = makesp([2 3],[-1 9]); >> nsp = appendsp(sp,sp2) nsp = brp: [0 1 2 3] maxorder: 2 coefs: [1 2 3 2 1 6 0 -1 9] order: 2 dim: 1

(27)

Syntax csp = completesp(brp,p,[d1 d2]);

Description 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 [d1 d2].

Example Compute the complete spline with knots in [0 0.5 1 1.25] with function value [1 2 1.5 0.25] and derivative [0.25 -1] >> csp = completesp([0 0.5 1 1.25],[1 2 1.5 0.25],[0.25 -1]) csp = brp: [0 0.5000 1 1.2500] coefs: [1x12 double] order: 3 maxorder: 3 dim: 1

In Figure 16 the resulting function and its derivative is shown (gen-erated by using plot(0:0.001:1.25,evalsp(csp,0:0.001:1.25)) and plot(0:0.001:1.25,evalsp(dersp(csp),0:0.001:1.25))). 0 0.2 0.4 0.6 0.8 1 1.2 1.4 0 0.5 1 1.5 2 2.5 x f(x) 0 0.2 0.4 0.6 0.8 1 1.2 1.4 −7 −6 −5 −4 −3 −2 −1 0 1 2 3 x f‘(x)

(28)

Avdelning, Institution

Division, Department

Division of Automatic Control

Department of Electrical Engineering

Datum Date

2003-02-17

Spr˚ak Language 2 Svenska/Swedish 2 X Engelska/English 2 ... Rapporttyp Report category 2 Licentiatavhandling 2 Examensarbete 2 C-uppsats 2 D-uppsats 2 X ¨Ovrig rapport 2 ... URL f¨or elektronisk version

http://www.control.isy.liu.se

ISBN

...

ISRN

...

Serietitel och serienummer Title of series, numbering

LiTH-ISY-R-2490

ISSN

1400-3902

...

Titel

Title On path planning and optimization using splines

F¨orfattare

Author Mikael Norrl¨of,

Sammanfattning Abstract

This report covers many aspects of path and trajectory generation for industrial robots. Path gener-ation in Cartesian space is discussed, with the limitgener-ation 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

References

Related documents

We study the general problem of computing an obstacle-avoiding path that, for a prescribed weight, minimizes the weighted sum of a smoothness mea- sure and a safety measure of the

[r]

Ùu؞ΘլÔéÖ8Õ¬ØÐÙhÔ@ÙuØÐÛjÕ¨ÔΘÏ!Ûjá!ÔBÑ î!ÛغÖuØÐÙuáéÔ!î ÜWÕ¬Ô!Û×nÙhàRÎÐÏ!Û

Iceland’s Presidency of the Nordic Council of Ministers emphasises that the Nordic countries should continue to take an active part in international environmental

Here L(E, F ) is the space of all bounded linear operators from E into F endowed with the

1 I would like to express special thanks to Uwe Thieme, Polizeidirektor at the police headquarters in Dortmund, and his colleagues for the opportunity to gain insight into their

Ha Nypublicerad information om en förändring av ett företags ordinarie utdelning reflekteras inte direkt i aktiekursen och det går således att påträffa en

JPMC to negotiate military elements while having peace negotiations continue; the representation in the government delegation to Arusha of the major power groupings in Kigali