Simplified Robotics Joint-Space Trajectory Generation with a via Point Using a Single Polynomial

This paper presents novel fourth- and sixth-order polynomials to solve the problem of joint-space trajectory generation with a via point. These new polynomials use a single-polynomial function rather than two-polynomial functions matched at the via point as in previous methods. The problem of infinite spikes in jerk is also addressed.


Introduction
Joint-space trajectory generation is in common usage in robotics to provide smooth, continuous motion from one set of joint angles to another, for instance, for moving between two distinct Cartesian poses for which the inverse pose solution has yielded two distinct sets of joint angles. The joint-space trajectory generation occurs at runtime for all joints independently but simultaneously.
There is an entire body of literature devoted to trajectory generation (aka motion planning and path planning) at the joint level. Paul and Zhong [1] were among the first to suggest the use of polynomials for robot trajectory generation. A common joint-space trajectory generation method (linearly changing joint velocity using starting and ending parabolic blends) is identically presented by many authors [2][3][4][5][6][7]. Despite the widespread popularity of this method, it suffers from infinite spikes in jerk (the derivative of acceleration) and requires three separate functions instead of one. An identical third-order polynomial joint-space trajectory generation approach is also presented by many authors [2,[4][5][6][7]. Further, an identical fifth-order polynomial joint-space trajectory generation approach is presented by many authors [2,4,5,7]. From these authors' lists, it may appear that Koivo was the first to present these methods, when in fact they were already presented in Craig's first edition in 1986. Fu et al. [8] depart from these standard methods, suggesting initial, intermediate, and final polynomials of order 4-3-4, 3-5-3, or 5 third-order polynomials, for a single joint motion. These are by far the most (unnecessarily) complicated methods and are presented without justification or comparison with simpler methods. For dealing with a via point in which the robot need not stop at the via point (such as for obstacle avoidance), Craig [5] suggests matching two third-order polynomials.
Apparently alone amongst all of the major robotics textbook authors, Angeles [9, Section 6.5] derives a 4-5-6-7 seventh-order polynomial to fit two via points and ensure finite joint jerk at the start and end of motion. Angeles [9, Section 6.6] also presents approximation of this seventh-order polynomial with a cubic spline and discusses the associated errors.
An early version of the current paper was presented by the author at a conference [10]. The original contribution of this paper is a single sixth-order polynomial to provide smooth, continuous joint motion through a via point. Previous approaches have matched two third-order polynomials at the via point. We also present a single fourth-order polynomial to accomplish the same purpose. However, this case should not be used since it leads to discontinuous accelerations, leading to infinite spikes in jerk, which is unacceptable for reliable, smooth, and long-life robotic systems. This infinite jerk problem, prevalent in all of the robotics textbooks, has also been pointed out by Macfarlane and Croft [11], who present jerk-bounded trajectories and Gosselin and Hadj-Messaoud [12] and Petrinec and Kovacic [13] who go one step further to ensure continuous, not just bounded, jerk.

Joint-Space Trajectory Generation with a via Point
Standard joint-space trajectory generation assumes that two sets of discrete joint parameters are known (angles for revolute joints, lengths for prismatic joints), and it is required to move smoothly in joint-space from one set to the next. For instance, given required Cartesian poses X S and X F , inverse pose kinematics calculate the required joint value sets for achieving each of these; call them Θ S and Θ F . Note subscript S stands for start (or initial) and subscript F stands for finish (or final). The standard joint-space trajectory method then moves smoothly from Θ S and Θ F for all joints independently but simultaneously. Polynomials are natural choices for providing smooth, continuous motion, with some level of continuous derivatives. In many robotics motion planning problems, the robot must pass through intermediate point(s) between the start and finish poses, such as for obstacle avoidance. The joint rates and accelerations need not go to zero at these so-called via points, but they must be matched between functions meeting at the via point(s).

Two Third-Order Polynomials.
Craig [5] suggested the use of two third-order polynomials meeting at the via point. Section 2.1 is entirely a summary of Craig's work (with an original example by the current author). Two third-order polynomials will provide smooth motion with continuous position and velocity and zero velocity at the start and end. The two third-order polynomials are 1 ( ) = 13 3 + 12 2 + 11 + 10 , These two polynomials require eight constraints. Four constraints come from the initial and final time points: It is convenient to shift the time axis so that the 2 ( ) polynomial has zero time starting at the via time. 1 is the end time of the first range ( 1 = ) and 2 is the relative end time of the second range ( 2 = − ). We need four more constraints. We force the first polynomial to end at the via angle and the second polynomial to start at the via angle . We also ensure that the velocities and accelerations match at the via point (they need not go to zero). This will ensure that the jerk stays finite during this via point transition between the two polynomials. The four additional constraints are The constraints yield eight linear equations in the eight unknowns (two third-order polynomials, four unknown coefficients each). Three of the unknown polynomial coefficients are found immediately, from the initial and via time constraints: 10 = , 11 = 0, and 20 = . The simplified 5 × 5 matrix/vector equation to solve for the remaining three unknowns is Solve (4) for the remaining five unknowns for use in (12). For the special case of 1 = 2 = /2 = = , the analytical solution is 10 = , 11 = 0, and 20 = , and

Example: Two Third-Order Polynomials with a via Point.
Use two third-order polynomials for smooth joint-space trajectory generation, plus motion through a via point, for one joint. Given = 30 ∘ , = 180 ∘ , = 120 ∘ , = 1.5, and = 3 ( 1 = 2 = 1.5) sec, find 1 ( ) and 2 ( ):   Figure 1. Note that the joint angle passes through the via point = 180 ∘ and keeps going for a brief time; this is because the velocity is still positive beyond the first 1.5 sec. The maximum angle is MAX = 183.9 ∘ , occurring at = 1.68 sec. At MAX , the angular velocity goes to zero (the slope of the angle is zero at that point since the angle is changing in direction). Also note that the velocity term in 2 ( ), the coefficient, is nonzero since the velocity does not need to go to zero at this via point. Further, note that the velocity and acceleration have been successfully matched at the 1.5 sec via point transition as required. Note that the jerk is not matched at the transition (the slopes of the two polynomial accelerations are of the same magnitude but of different signs), but jerk remains finite since the acceleration is matched. The jerk has infinite spikes at the start and end, which is common in the robotics books but is unacceptable.

Two Fifth-Order Polynomials.
The two third-order polynomials presented above, matched at a via point, appear in many robotics textbooks (e.g., [5]). However, this approach suffers from discontinuous acceleration functions at the initial and final time, leading to infinite spikes in jerk at these points in time. This is unacceptable for the repeated motions of mechanical systems due to unacceptable wear, noise, and dynamic excitation. To fix this, we could match two fifth-order polynomials at the via point, specifying zero acceleration at the initial and final times (not at the via time-instead matching velocities and accelerations between the two fifth-order polynomial functions at the via time). However, this will not be presented because we came up with a better method, a single polynomial versus two matched polynomials-this original work is presented in the next two subsections.

Single Fourth-Order
Polynomial. The first original contribution of this paper is presented in this subsection. We can achieve the same goals as the two third-order polynomials meeting at a via point much more simply; let us use only one polynomial, forced to go through the via point. Here are the constraints for meeting the required angles with smooth motion (since we use a single continuous polynomial, the velocity and accelerations are guaranteed to match and be continuous at the via point): Note in this case since there is only one time range, it is convenient to treat all times as absolute, rather than relative as when we did matching of two third-order polynomials. With five constraints, a single fourth-order polynomial is required: The single fourth-order polynomial solution for each joint is Again, deg units are used throughout this example. These results are plotted in Figure 2. The ( ) shape for the single fourth-order polynomial is very similar to that for the two third-order polynomials. Again, the joint angle passes through the via point = 180 ∘ and keeps going briefly, due to the fact that the velocity is still positive beyond the first 1.5 sec. The peak for this fourthorder case is slightly greater and occurs in time slightly after the peak for the two third-order polynomials example. The maximum angle is MAX = 185.4 ∘ , occurring at = 1.74 sec. For the fourth-order polynomial, a side benefit has arisen: the jerk is now continuous at the via time, where it was discontinuous for the two matched third-order polynomials.
The jerk still has an infinite spike at the start and end as we saw with the two third-order polynomials example, which is unacceptable. We now improve upon this with a single sixth-order polynomial in the next subsection; again, this next subsection is original work.

Single Sixth-Order
Polynomial. This section presents another original contribution. We can achieve the same goals as the single fourth-order polynomial with a via point and eliminate the infinite spikes in jerk at the start and end as follows. To the previous five constraints, add two more, for zero acceleration at the start and end of the single motion range. Here are the seven constraints: Seven linear equations in the seven unknown polynomial coefficients , = 0, 1, 2, 3, 4, 5, 6 result from the seven constraints (13). Three of the unknown polynomial coefficients are found immediately, from the initial time constraints: 0 = , 1 = 0, and 2 = 0. The simplified 4 × 4 matrix/vector equation to solve for the remaining four unknowns is The single sixth-order polynomial solution for each joint is For the special case of = /2, the analytical solution is Again, deg units are used throughout this example. These results are plotted in Figure 3. Again, the ( ) shape is similar to the previous two examples. The maximum angle is MAX = 185.6 ∘ , occurring at = 1.70 sec. Note the magnitude of the angular velocity, acceleration, and jerk are not greatly different for the three cases we have presented with via point. Generally they are lowest for the two third-order polynomials. This single sixthorder polynomial approach has eliminated the problem of infinite spikes in jerk at the start and end of each motion. Here, the jerk is discontinuous but it remains finite, which obeys the rule of thumb for mechanical design/motion.

Discussion
All joint-space trajectory generation methods presented by previous authors and the new ones proposed in this paper can easily be applied to multiple joints, simultaneously but independently.
This main original contribution of this paper is to present a new joint-space trajectory generation approach using a single polynomial for motion through a given via point. The robot does not need to stop at the via point, but the motion must be smooth and continuous through the via point. A single fourth-order polynomial was developed to achieve this smooth motion through a via point and to replace the two three-order polynomials matched at the via point in common usage today. With the new approach, this via point matching comes automatically and there is no need for two-polynomial functions.
An important secondary contribution of this paper is to expose a bad practice in common usage in joint-space trajectory generation in robotics today. We are not the first to notice this [11][12][13], but judging from the four robotics textbooks in the list of references published within the past five years, the message has not been widely understood (only [9] is concerned with preventing infinite jerk). For any functions in which the acceleration is discontinuous, the associated jerk (time derivative of acceleration) function will have infinite spikes at those acceleration discontinuities. Cam's design teaches that these infinite spikes in jerk are unacceptable and must be avoided by the designed/motion controller. By extension, in robot joint-space trajectory generation, any method which allows discontinuous acceleration functions is unacceptable and must not be used. Unfortunately, the linear velocity with parabolic blends, the third-order polynomial, and the two third-order polynomials matched to meet at a via point all suffer from discontinuity in acceleration and thus infinite spikes in jerk. These methods are still very common in usage in the standard robotics textbooks today; these methods must be discarded. Otherwise, unacceptable noise, wear, stress, reduced life, and the introduction of bad dynamics may result.
The novel single fourth-order polynomial introduced in this paper to go through a via point also suffers from this discontinuous acceleration. Therefore, to remedy this we introduced a novel sixth-order polynomial to move smoothly and continuously through a via point, with finite jerk throughout the entire motion, which is acceptable.

Conclusion
For joint-space trajectory generation in robotics, the recommendations of this paper are simple; for all joint motions without a via point, use the standard fifth-order polynomial from Craig [5]. For all joint motions with a via point, use the new single sixth-order polynomial introduced in this paper. Not only does this new sixth-order polynomial keep the jerk finite, it also achieves the via point motion with only one function.