The Rossum Project / Papers / Calculations / Cubic Path

Revision 1.0

16 January 2006

 

 

A Path Based on Third-Degree Polynomials,

Constrained at its Endpoints by Position,

†Orientation, and Speed

 

by G.W. Lucas (gwlucas@users.sourceforge.net)

 

 

 

 

Contents

 

1.

Introduction

 

2.

Derivation

 

3

Results

 

4.

Additional Results: Speed, acceleration, orientation, and rotational velocity

 

5.

Selecting velocity vectors in threading applications

 

6.

Estimating travel time to goal position

 

7.

Conclusion

 

Introduction

There is one important limitation to the circular trajectory that was considered in the previous article in this series (see A Path Following a Circular Arc).† When applying a circular trajectory calculation, we cannot specify an arbitrary orientation for the robot or vehicle at its final position. We can overcome this shortcoming by using a somewhat more complicated curve than the circular arc discussed previously. Figure 1 shows an example of such a curve. In the figure, we use a pair of third-degree polynomials to generate a trajectory that begins at the first point and ends at the second. The robotís orientation and speed as it enters the trajectory and as it crosses each waypoint point is arbitrary. We do not know what direction it came from when it reached the first point, but assume that it is specified as part of the problem. We could also choose any orientation for the robot when it reaches the second. For this example, however, we selected an orientation for the robot at point so that it was facing point when it completed the first leg of its maneuvers.

 


 

 

 

Figure 1 Ė A trajectory based on third-degree polynomials

 

 

 

In the discussion below, we will focus first on the case where the robotís trajectory is defined by just two points (with appropriate selections of course and speed). Later we will discuss threading applications in which a robot is required to transit along a sequence of multiple waypoints (see below for Selecting velocity vectors in threading applications). Such applications are common in real-world robotics. For example, some robot competitions feature precision maneuvering in slalom-style events. Even in ordinary operations, a threading approach can provide an efficient path for a robot to turn a corner or weave through a series of obstacles. The smooth transitions it provides help preserve momentum, reduce navigation error due to loss of wheel traction or backlash in the robotís drive mechanisms, and allow the robot to maintain its speed throughout its maneuvers.

 

Figure 2 Ė A series of maneuvers threading a sequence of waypoints

 

 

 

In threading applications, the robotís operation is divided into segments based on a sequence of waypoints. In Figure 2, each segment is indicated by straight lines connecting pairs of points. The curved gray line represents the results from the third-degree polynomial method discussed in this article.† Short line segments are also drawn tangent to the curves at the waypoints to indicate the robotís direction of travel at the instant it crosses the point. The curve-fitting method discussed below generates a trajectory as a set of polynomials for a single pair of points.† So, in threading applications, we generate a distinct set of polynomials for each pair of waypoints in the robotís path. By specifying that the final orientation and speed at the end of one segment be used as the initial conditions for the next, we ensure continuity and smooth transitions throughout the robotís maneuvers. Typically, we choose orientations as shown in Figure 2, so that the robotís direction of travel as it passes through one point represents half the turn from the prior segment to the next. Doing so helps keep the robot close to the direct path between points, but it is not absolutely required. In fact, there are many cases where the presence of obstacles or a desire to avoid making too sharp a turn at high speed requires that we use some other input for determining the robotís path.

 

The mathematical technique that we will use in this section is based on Hermite Curves (after Charles Hermite, 1822-1901) though the derivation presented here is limited to a special case and a very much simplified treatment. Readers may recognize related techniques from computer graphics or numerical analysis since it also resembles the clamped cubic spline sometimes used to generate curves in graphics programs.

 

Derivation

 

When we dealt with paths based on an arc of circle in the earlier article, we developed a pair of functions giving the robot or vehicleís x and y position as a function of time. For this article, we will accomplish something analogous using cubic equations in the form:

 

††††††††††††††††††††††††††† [1]

 

When threading a series of points, we develop a new set of equations for each segment of the maneuver. To ensure continuity, we simply require that the position, speed, and orientation at the end of one segment are the same values we specify for the beginning of the next.

 

The form shown in equation [1] has several advantages. Both the functions and their derivatives are continuous and differentiable at all points in the interval across all segments. The second derivative is also continuous throughout and differentiable across each interval except at the transition point between segments. These factors give the resulting trajectory a distinct quality of smoothness. Throughout the interval, changes in speed, orientation, and velocity happen gradually. In practical terms, this smoothness means that there is less need for abrupt changes in power output for the robotís drive system, thus reducing navigation errors and helping to moderate the robotís energy consumption. And, again, this form allows us considerable freedom in selecting the robotís orientation at the end of each interval of its transit. As we will see later on when we discuss threading applications, this freedom permits us to specify maneuvers that conserve both battery power while reducing travel time and minimizing navigational errors.

 

In the discussion below, we will examine a technique for finding coefficients for the third degree polynomials given in [1]. The discussion will be divided into two parts. Initially, we will assume that we know the value of , the time at which the robot or vehicle reaches the goal point . In practice, this value is often unavailable to us. Occasionally, we do encounter a problem where it is required that a robot reaches a goal position at a specified time, but more often we find that we need to compute the arrival time at the goal as an output product of our computations. So, in the second part of this discussion, we will examine ways for estimating a value for †based on speed, orientation and distance between points (see below for Estimating travel time to goal position).

 

Table 1 Ė Variables and specifications

 

Variable

Description

Specified positions of robot (third specification is optional).

Specified orientation of robot at the initial and final points of its first segment.

Specified speed of robot initial and final points of its first segment.

Specified time that robot reaches initial and final points of its first segment (the final time may optionally be calculated),

Time variable.

Derived, third-degree polynomials giving x and y-coordinates of the robotís position as a function of time.

Derived, robotís speed as a function of time.

Derived, robotís acceleration as a function of time.

Derived, robotís orientation as a function of time.

Derived, robotís rotational velocity as a function of time.

 

In equation [1], the x and y-components of the position are given by third degree polynomials. We will focus on the development of coefficients for. Coefficients for can be found in a similar manner.

 

We begin with the assumption that the robot always moves in a forward direction so that we can base its velocity vector on its orientation and speed. If we have a pair of cubic equations giving the robotís position as a function of time, the x and y-components of the velocity vector are found by taking the first derivative of each of those equations with respect to time. That operation produces a pair of second-degree polynomials. Acceleration components follow by taking the second derivative to produce a pair of linear equations.

 

Letís find coefficients for those equations, beginning with the x-components. Assume that the x-component of the robotís acceleration vector is given by

 

††††††††††††††††††††††††††††††††††††††††††††††††††††††††††††††††††††† [2]

 

where †and †are unknown.† Integrating, we have

 

†††††††††††††††††††††††††††††††††††††††††††††† [3]

 

and

 

††††††††††††††††††† [4]

 

where †and †are arbitrary integration constants.† We can find values for these constants by applying information about our initial conditions. Recall that the position, speed, and orientation for the end points of the trajectory are specified. As noted above these values are arbitrary and we can select whatever values suit our application. If we are threading a series of points, we can set the terminal orientation of the first segment so that the robot is pointed toward the terminal point of the next segment. If we have orientation and speed as specifications, we can find the components of the velocity using

 

 

Later in these notes, we shall discuss techniques for selecting velocity vectors when we are threading a series of points. For now, assume that values are available. No matter how we come up with the x and y-components for our velocity vectors, the next step is the same. We simply use the velocity vector as an initial condition to solve for the integration constant in equation [3]. At time all of the time-related factors in [3] drop out and we have

 

 

We can apply a similar approach using the robotís initial position to solve for the integration constant in equation [4].

 

 

We can then find values for the coefficients †and based on what we know about the terminal conditions at time . Substituting the x-component from our final velocity vector into equation [3] and position into equation [4] to get

 

†††††††††††††††††††††††††††† [5]

 

 

 

Results

We can conclude our derivation of the position functions by solving for the parameters in [5]. Since the only remaining unknown values in [5] are †and , we treat these as variables. When we do so, the expressions in [5] form a system of liner equations. Solving for† †and , we find

 

†††††††††††††††††††††††††††††††††††††††††††††† [6]

 

 

We can combine the results from [5] and [6] to write an equation for the x-component of our robotís velocity and position as a function of time

 

 

 

 

The derivation for the y-component is symmetrical to that of the x-component. We can obtain it by substituting the appropriate variables into the equations above with

 

 

 

 

 

and

 

 

 

At this point, we have completed what we set out to do and have developed calculations for the robotís position as a function of time.† We also have several other useful pieces of information that we can use to describe the robotís behavior during its transit. Most importantly, we have the x and y-components for its velocity and acceleration vectors (from equations [2] and [3]). In the next section, we will see how these components can be combined to find values such as speed, turn rate, linear acceleration, etc.

 

 

 

 

 

 

 

Additional Results: Speed, acceleration, orientation, and rotational velocity

In practical applications, it would be useful to know the speed, linear acceleration, orientation, and rotational velocity of our robot. The values are useful in computing control input values and also for ensuring that the trajectory that we would specify does not exceed real-world performance limits for our robot.

 

To see how these values can be obtained, consider the problem of finding the robotís speed. We already know something about this value as a by-product of the work above. In equation [3], we found the x-component of the robots velocity vector (that is, the component of its motion in the direction of the x axis). The y-component is found easily by substituting the appropriate variables. By combining these the components, we can find the robotís speed using an elementary technique from multi-variable calculus

 

 

 

Which gives us

 

†††† †[7]

 

 

 

Next, we will consider some of the other relationships. Before we do, we will introduce one simplifying assumption. There are a lot of terms in some of the equations that follow, and in order to present them in a compact and readable form, we will assume that the time value for the initial point,, is zero. This assumption will reduce time elements in the equations to a single variable, t. In practice, we can adjust actual values as required when writing code or performing manual calculations.

 

Let


Using our simplifying assumption that †we can write equation [7] as

 

†††††††††††††††††††††† [7a]

 

 

 

†Linear acceleration is given by taking the derivative of the speed equation to obtain

 

 

Intuitively, we realize that the robot will reach its maximum (or minimum) speed at a point where acceleration (or deceleration) equals zero. This insight is basis of the classic calculus technique for finding extreme values: look for places where the derivative of a function equals zero. These critical points are candidates for the extreme values of a function (but only candidates... while all extreme values occur at critical points, not all critical points yield extreme values). With our equation for , we have a derivative for the speed function. And because at the critical points, we can ignore the denominator in the expression and focus on the cubic polynomial in the numerator. Cubics always have at least one and potentially three real-valued solutions. Solving for them will allow us to find critical points for speed. Now these critical points may not be in the time interval covered by the robotís maneuver. If theyíre not, then the extreme value must occur at the endpoint of the transit. If they do occur inside the interval, we need only evaluate the speed function at those times and inspect the result to see if they are extreme values.

 

Algorithms for evaluating cubic polynomials are abundant on the web (for an example, see http://www.worldserver.com/turk/opensource/#CubicRoots).

 

Recall that one of our assumptions is that the motion of the robot is always in the direction to which it is pointed. So we can find the orientation of the robot by the arctangent of the ratio of the y and x-components of the velocity vector (again using the simplifying assumption):

 

 

 

 

An equation for rotational velocity is given by

 

 

 

 

Finally, arc length is found by integrating the speed over time

 

Using the speed function from [7] (or [7a]), we have

 

†††††

 

 

 

Unfortunately, this integral is intractable for most sets of coefficients and cannot be solved directly. Fortunately, it is well behaved and can be evaluated numerically using Simpsonís Rule from elementary calculus or more advanced techniques from numeric analysis.

 

 

 

 

Selecting velocity vectors in threading applications

We introduced the idea of threading a series of points at the beginning of this article. As mentioned above, one of the requirements of a threading application is that we need to select the velocity vector (particularly the direction) at each waypoint. In doing so, we have the opportunity to use whatever strategy best suits our needs. The following text discusses one particular algorithm that was used in Figure 2.

 

Assuming that a speed is specified at each point, a velocity vector can be found by developing a unit vector giving the direction of travel and then scaling it according to speed. In the steps to follow, we will focus on finding the unit direction vector. Referring to Figure 2, note that for every pair of adjacent segments, the tangent is chosen so that it is the same angle from both of the segments. In effect, the tangent splits the turn from segment to segment. To find it, we start by constructing unit vectors along both of the segments in the pair. We then take the sum of those two unit vectors, and convert that sum to be a unit vector itself.

 

Given three waypoints expressed as vectors †in order

 

Let

 

 

Then

 

††††††††††††††††††††††††††††††††††††††††††††††††††† [8]

 

where †is the unit direction vector at waypoint.† Given a specification for a speed value †at the waypoint, the velocity vector is just

 

 

From the perspective of writing software, the form shown in equation [8] is probably the best choice. But a little algebra allows us to put it into a form that reveals more about what is happening

 

 

††††††††††††††††††††††††††††††††††† [9]

 

 

 

 

 

 

Trigonometry tells us that in a two-dimensional Euclidean space, the vector dot product can be used to find the cosine of the angle between two vectors, as in the expression

 

††††††††††††††††††††††††††††††††††††††††††††††††††††††††† [10]

 

The angle †gives the cosine for the angle between the two segments meeting at point. When that turn covers a full 180 degrees, †and [9] becomes undefined. Therefore, in the event of a full 180-degree turn, we construct a direction vector for a 90-degree turn in a different manner. Recall that in a two-dimensional Euclidean space, the perpendicular to a vector can be found by swapping the x and y components and negating one of them:

 

 

So we can find the perpendicular †to †using

 

.

 

Of course, both †and †are perpendicular to.† So, which perpendicular do we use? One gives us a 90-degree turn to the left, the other a 90-degree turn to the right. As usual, this selection depends on the nature of the application and judgment of the designer. Figure 3 illustrates one solution. Initially, the robot is oriented according to its initial direction vector. As it moves forward, it turns toward its next waypoint. So, to choose the direction vector at the end of the segment, we simply pick the one that will continue the motion in the direction of the turn. If the next waypoint is to the robotís right, then we shall choose a direction vector that is pointing to its right. If the waypoint is to the robotís left, then we shall choose a direction vector that is pointing to its left.

 

Figure 3 Ė Negotiating a 180-degree turn

 

 

 

Trigonometry tells that we can compute the sine of the angle between two vectors †and †in a two-dimensional Euclidean space using the relationship

 

††††††

 

The sign of the sine function tells us the direction of the turn. So we simply compare directions of the two turns of interest here Ė the turn from initial velocity vector toward the next waypoint, and the turn from the initial velocity vector toward the perpendicular Ėand see if they are in the same direction. If they are not, we simply negate the perpendicular before using it as the direction vector at the next waypoint.

 

Finally, there are two waypoints in the sequence where we cannot construct an angle between segments: the initial and final waypoints. At the initial point, we may have a speed and orientation value based on whatever the robot was doing before we began constructing the trajectory. Alternatively, we can construct a velocity vector by using a specified speed value and determining the robotís direction based on the direction of the line segment between the first two waypoints. We can use the same approach at the final waypoint.

 

We end this section with an aside. We note that the expression under the radical in [9] turns out to be the half-angle formula for a cosine from trigonometry. There is an alternate method for constructing the direction vector by using trigonometry to construct a triangle from the two segments between waypoints and bisecting the angle at the turn. Naturally, this bisection produces the half-angle relationship.

 

 

 

Estimating travel time to goal position

We will now turn our attention to the problem of estimating the travel time to the goal position. This is necessary as a practical matter because, in most cases, a specification for the arrival time is not part of the problem with which we are presented. Because the geometry of the robotís path depends on the value we choose for , we need some method for producing an acceptable estimate.† One way of doing so is to estimate the distance that the robot will need to travel between points and divide that by its average speed (based on the specified speed value or values) to determine a time value. Once we have found an estimate for , we can compute coefficients for the interpolating polynomials. If desired, we can also compute maximum speed, approximate maximum turn rate and acceleration and verify that the operation of the robot is within the limits of our mechanical apparatus. If it is not, we simple increase, compute new parameters, and test until we obtain acceptable values.

 

The key problem in estimating the distance traveled in the interval between two waypoints is that the robotís initial and final velocity vectors may push it substantially off the straight-line segment between them. To estimate the length of the path that the robot will travel, we use the Circle Arc Trajectory method described in the previous article in this series.

 

Consider the conditions when the robot is located at its initial point. Because we assume that it travels forward in the direction of its orientation, its initial frame of reference is based on its initial direction vector. In that context, equation [10] can be applied to find the bearing to the next waypoint. The range is just the distance between waypoints. So to find the length †for the circular arc trajectory we have

 

 

and

 

It is necessary to take the absolute value when evaluating the length because the construction does not otherwise guarantee a positive value for the calculation.

 

Of course, itís not just the first velocity vector that pushes the robot away from the straight-line path between waypoints.† We must also take into account velocity vector at the second waypoint in the pair. The construction is similar

 

 

The estimated time required for the transit is just the average length divided by the average speed as specified at the two points:

 

 

 

Recall that the values for speed and acceleration can vary continuously throughout the time period †and that their maximum values are driven by the selection of a value for . It is possible that, through a combination of geometry and time specifications, we may obtain values for maximum speed and acceleration that would be outside the capabilities of our real-world mechanism. Thus it may be necessary to find the extreme values for speed and acceleration and, if they exceed reasonable limits, adjust our value for time †accordingly.

 

 

Conclusion

Once we have established interpolating functions to produce a trajectory for the robot or vehicle, we are still faced with the problem of translating that information to control inputs for our mechanical system. It is easy to compute velocity, rotational velocity, and acceleration at points throughout the trajectory. These values can be applied as inputs to control system calculations. Methods of doing so can be found in books such as Introduction to Autonomous Mobile Robots by Siegwart and Nourbakhsh, or Computational Principles of Mobile Robotics by Dudek and Jenkin.

 

The graphics showing robot paths for a series of waypoints were generated using a program that embodies the algorithms discussed in this article. The program is not yet ready for distribution, but will be released as soon as possible.