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

Abstract

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.

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

2. 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 𝐗_𝐒 and 𝐗_𝐅, inverse pose kinematics calculate the required 𝑛 joint value sets for achieving each of these; call them Θ_𝐒 and Θ_𝐅. Note subscript 𝐒 stands for start (or initial) and subscript 𝐅 stands for finish (or final). The standard joint-space trajectory method then moves smoothly from Θ_𝐒 and Θ_𝐅  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).

2.1. 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 𝜃₁(𝑡)=𝑎₁₃𝑡³+𝑎₁₂𝑡²+𝑎₁₁𝑡+𝑎₁₀,𝜃₂(𝑡)=𝑎₂₃𝑡³+𝑎₂₂𝑡²+𝑎₂₁𝑡+𝑎₂₀,𝜃̇₁(𝑡)=3𝑎₁₃𝑡²+2𝑎₁₂𝑡+𝑎₁₁,𝜃̇₂(𝑡)=3𝑎₂₃𝑡²+2𝑎₂₂𝑡+𝑎₂₁,𝜃̈₁(𝑡)=6𝑎₁₃𝑡+2𝑎₁₂,𝜃̈₂(𝑡)=6𝑎₂₃𝑡+2𝑎₂₂. (1)

These two polynomials require eight constraints. Four constraints come from the initial and final time points: 𝜃₁(0)=𝜃_𝑆,𝜃₂𝑡₂=𝜃_𝐹,𝜃̇₁(0)=0,𝜃̇₂𝑡₂=0. (2) It is convenient to shift the time axis so that the 𝜃₂(𝑡) polynomial has zero time starting at the via time. 𝑡₁ is the end time of the first range (𝑡₁=𝑡_𝑉) and 𝑡₂ is the relative end time of the second range (𝑡₂=𝑡_𝐹−𝑡_𝑉).

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 𝜃₁𝑡₁=𝜃_𝑉,𝜃̇₁𝑡₁=𝜃̇₂(0),𝜃₂(0)=𝜃_𝑉,𝜃̈₁𝑡₁=𝜃̈₂(0). (3)

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: 𝑎₁₀=𝜃_𝑆, 𝑎₁₁=0, and 𝑎₂₀=𝜃_𝑉. The simplified 5×5 matrix/vector equation to solve for the remaining three unknowns is 1𝑡₁000001𝑡₂𝑡²₂0012𝑡₂3𝑡²₂2𝑡₁3𝑡²₁−10026𝑡₁𝑎0−20¹²𝑎₁₃𝑎₂₁𝑎₂₂𝑎₂₃=𝜃_𝑉−𝜃_𝑆𝑡²₁𝜃_𝐹−𝜃_𝑉𝑡₂000. (4) Solve (4) for the remaining five unknowns for use in (12). For the special case of 𝑡₁=𝑡₂=𝑡_𝐹/2=𝑡_𝑉=𝑇, the analytical solution is 𝑎₁₀=𝜃_𝑆, 𝑎₁₁=0, and 𝑎₂₀=𝜃_𝑉, and 𝑎₁₂=−3𝜃_𝐹−𝜃_𝑉+9𝜃_𝑉−𝜃_𝑆4𝑇²,𝑎₁₃=3𝜃_𝐹−𝜃_𝑉−5𝜃_𝑉−𝜃_𝑆4𝑇³,𝑎₂₁=3𝜃_𝐹−𝜃_𝑆,𝑎4𝑇₂₂=3𝜃_𝐹−𝜃_𝑉−3𝜃_𝑉−𝜃_𝑆2𝑇²,𝑎₂₃=−5𝜃_𝐹−𝜃_𝑉+3𝜃_𝑉−𝜃_𝑆4𝑇³. (5)

2.1.1. 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.5) sec, find 𝜃₁(𝑡) and 𝜃₂(𝑡): 𝜃₁(𝑡)=−68.89𝑡³+170𝑡²𝜃+30,₂(𝑡)=55.56𝑡³−140𝑡²𝜃̇+45𝑡+180,₁(𝑡)=−206.67𝑡²𝜃̇+340𝑡,₂(𝑡)=166.67𝑡²𝜃̈−280𝑡+45,₁(𝑡)𝜃̈=−413.33𝑡+340,₂(𝑡)𝜃⃛=333.33𝑡−280,₁(𝑡)𝜃⃛=−413.33,₂(𝑡)=333.33. (6) deg units are used throughout this example. These results are plotted in Figure 1.

(a)

(b)

(c)

(d)

Figure 1 

Two third-order polynomials with via point.

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 𝜃₂(𝑡), 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.

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

2.3. 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): 𝜃(0)=𝜃_𝑆𝜃̇(0)𝜃𝑡=0^𝑉=𝜃_𝑉𝜃𝑡_𝑓=𝜃_𝐹𝜃̇𝑡_𝑓=0 (7) 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: 𝜃(𝑡)=𝑎₄𝑡⁴+𝑎₃𝑡³+𝑎₂𝑡²+𝑎₁𝑡+𝑎₀,𝜃̇(𝑡)=4𝑎₄𝑡³+3𝑎₃𝑡²+2𝑎₂𝑡+𝑎₁. (8)

Five linear equations in the five unknown polynomial coefficients 𝑎_𝑖, 𝑖=0,1,2,3,4 result from the five constraints (7). Two of the unknown polynomial coefficients are found immediately, from the initial time constraints: 𝑎₀=𝜃_𝑆 and 𝑎₁=0. The simplified 3×3 matrix/vector equation to solve for the remaining three unknowns is 1𝑡_𝑉𝑡²_𝑉1𝑡_𝐹𝑡²_𝐹23𝑡_𝐹4𝑡²_𝐹𝑎₂𝑎₃𝑎₄=𝜃_𝑉−𝜃_𝑆𝑡²_𝑉𝜃_𝐹−𝜃_𝑆𝑡²_𝐹0. (9) The single fourth-order polynomial solution for each joint is 𝑎₂=1𝑡_𝐹−𝑡_𝑉²𝜃_𝑉−𝜃_𝑆𝑡²_𝐹𝑡²_𝑉−𝜃_𝐹−𝜃_𝑆4𝑡_𝐹−3𝑡_𝑉𝑡_𝑉𝑡²_𝐹,𝑎₃=2𝑡_𝐹−𝑡_𝑉²−𝜃_𝑉−𝜃_𝑆𝑡_𝐹𝑡²_𝑉+𝜃_𝐹−𝜃_𝑆2𝑡²_𝐹−𝑡²_𝑉𝑡³_𝐹,𝑎₄=1𝑡_𝐹−𝑡_𝑉²𝜃_𝑉−𝜃_𝑆𝑡²_𝑉−𝜃_𝐹−𝜃_𝑆3𝑡_𝐹−2𝑡_𝑉𝑡³_𝐹. (10)

For the special case of 𝑡_𝑉=𝑡_𝐹/2, the solution is 𝑎₂=1𝑡²_𝐹16𝜃_𝑉−𝜃_𝑆−5𝜃_𝐹−𝜃_𝑆,𝑎₃2=−𝑡³_𝐹16𝜃_𝑉−𝜃_𝑆−7𝜃_𝐹−𝜃_𝑆,𝑎₄=8𝑡⁴_𝐹2𝜃_𝑉−𝜃_𝑆−𝜃_𝐹−𝜃_𝑆. (11)

2.3.1. Example: Single Fourth-Order Polynomial with a via Point

Use a single fourth-order polynomial for smooth joint-space trajectory generation, plus motion through a via point, for one joint. Given 𝜃_𝑆=30^∘, 𝜃_𝑉=180^∘, 𝜃_𝐹=120^∘, 𝑡_𝑉=1.5,and𝑡_𝐹=3sec, find 𝜃(𝑡): 𝜃(𝑡)=20.74𝑡⁴−131.11𝑡³+216.67𝑡²𝜃̇(𝑡)+30,=82.96𝑡³−393.33𝑡²𝜃̈(𝑡)+433.33𝑡,=248.89𝑡²𝜃⃛(𝑡)−786.67𝑡+433.33,=497.78𝑡−786.67. (12) Again, deg units are used throughout this example. These results are plotted in Figure 2.

(a)

(b)

(c)

(d)

Figure 2 

Single fourth-order polynomial with via point.

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 fourth-order 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.

2.4. 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: 𝜃(0)=𝜃_𝑆𝜃̇(0)𝜃̈(0)=0𝜃𝑡=0^𝑉=𝜃_𝑉𝜃𝑡_𝑓=𝜃_𝐹𝜃̇𝑡_𝑓𝜃̈𝑡=0_𝑓=0 (13) With the seven constraints, a single sixth-order polynomial is required: 𝜃(𝑡)=𝑎₆𝑡⁶+𝑎₅𝑡⁵+𝑎₄𝑡⁴+𝑎₃𝑡³+𝑎₂𝑡²+𝑎₁𝑡+𝑎₀,𝜃̇(𝑡)=6𝑎₆𝑡⁵+5𝑎₅𝑡⁴+4𝑎₄𝑡³+3𝑎₃𝑡²+2𝑎₂𝑡+𝑎₁,𝜃̈(𝑡)=30𝑎₆𝑡⁴+20𝑎₅𝑡³+12𝑎₄𝑡²+6𝑎₃𝑡+2𝑎₂. (14)

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, and 𝑎₂=0. The simplified 4×4 matrix/vector equation to solve for the remaining four unknowns is 1𝑡_𝑉𝑡²_𝑉𝑡³_𝑉1𝑡_𝐹𝑡²_𝐹𝑡³_𝐹34𝑡_𝐹5𝑡²_𝐹6𝑡³_𝐹612𝑡_𝐹20𝑡²_𝐹30𝑡³_𝐹𝑎₃𝑎₄𝑎₅𝑎₆=𝜃_𝑉−𝜃_𝑆𝑡³_𝑉𝜃_𝐹−𝜃_𝑆𝑡³_𝐹00. (15) The single sixth-order polynomial solution for each joint is 𝑎₃=1𝑡_𝐹−𝑡_𝑉³𝜃_𝑉−𝜃_𝑆𝑡³_𝐹𝑡³_𝑉−𝜃_𝐹−𝜃_𝑆15𝑡²_𝐹−24𝑡_𝐹𝑡_𝑉+10𝑡²_𝑉𝑡_𝑉𝑡³_𝐹,𝑎₄=3𝑡_𝐹−𝑡_𝑉³−𝜃_𝑉−𝜃_𝑆𝑡²_𝐹𝑡³_𝑉+𝜃_𝐹−𝜃_𝑆5𝑡³_𝐹−9𝑡_𝐹𝑡²_𝑉+5𝑡³_𝑉𝑡⁴_𝐹,𝑎₅=3𝑡_𝐹−𝑡_𝑉³𝜃_𝑉−𝜃_𝑆𝑡_𝐹𝑡³_𝑉−𝜃_𝐹−𝜃_𝑆8𝑡³_𝐹−9𝑡²_𝐹𝑡_𝑉+2𝑡³_𝑉𝑡⁵_𝐹,𝑎₆=1𝑡_𝐹−𝑡_𝑉³𝜃_𝑉−𝜃_𝑆𝑡³_𝑉−𝜃_𝐹−𝜃_𝑆10𝑡²_𝐹−15𝑡_𝐹𝑡_𝑉+6𝑡²_𝑉𝑡⁵_𝐹. (16) For the special case of 𝑡_𝑉=𝑡_𝐹/2, the analytical solution is 𝑎₃=2𝑡³_𝐹32𝜃_𝑉−𝜃_𝑆−11𝜃_𝐹−𝜃_𝑆,𝑎₄3=−𝑡⁴_𝐹64𝜃_𝑉−𝜃_𝑆−27𝜃_𝐹−𝜃_𝑆,𝑎₅=3𝑡⁵_𝐹64𝜃_𝑉−𝜃_𝑆−30𝜃_𝐹−𝜃_𝑆,𝑎₆=−32𝑡⁶_𝐹2𝜃_𝑉−𝜃_𝑆−𝜃_𝐹−𝜃_𝑆. (17)

2.4.1. Example: Single Sixth-Order Polynomial with a via Point

Use a single sixth-order polynomial for smooth joint-space trajectory generation, plus motion through a via point, for one joint. Given 𝜃_𝑆=30^∘, 𝜃_𝑉=180^∘, 𝜃_𝐹=120^∘, 𝑡_𝑉=1.5, and 𝑡_𝐹=3sec, find 𝜃(𝑡): 𝜃(𝑡)=−9.22𝑡⁶+85.19𝑡⁵−265.56𝑡⁴+282.22𝑡³𝜃̇(𝑡)+30,=−55.3𝑡⁵+425.9𝑡⁴−1062.6𝑡³+846.7𝑡²,𝜃̈(𝑡)=−276.5𝑡⁴+1703.7𝑡³−3186.7𝑡²𝜃⃛(𝑡)+1693.3𝑡,=−1106.2𝑡³+5111.1𝑡²−6373.3𝑡+1693.3. (18) Again, deg units are used throughout this example. These results are plotted in Figure 3.

(a)

(b)

(c)

(d)

Figure 3 

Single sixth-order polynomial with via point.

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 sixth-order 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.

3. 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–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.

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