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.