Skip to main content

Polynomial Robot Motion Planning

MATLAB project comparing cubic, quintic, and corrected LSPB joint-space trajectories with boundary-condition verification, trajectory derivatives, and explicit correction of archived formulation errors.

GitHub
Comparison of cubic, quintic, and corrected LSPB joint-space position trajectories

This MATLAB project compares three classical joint-space trajectory-generation methods: a cubic polynomial, a quintic polynomial, and a linear-segment-with-parabolic-blends (LSPB) construction. I reorganized the archived coursework into one reproducible workflow that verifies endpoint conditions numerically, compares trajectory derivatives, and explicitly documents two source inconsistencies that had to be corrected before publication.

The complete implementation and generated results are available here: GitHub repository

The cleaned version keeps the original boundary-value problem and trajectory families, but it does not silently repeat the flawed archived formulas. Instead, it validates each construction directly and separates what the cubic, quintic, and corrected LSPB trajectories can actually satisfy.

Source-derived boundary-value problem

The archived motion-planning task defines a single generalized coordinate q(t) with the following endpoint conditions:

Boundary condition Value
q(0)0.3
q̇(0)2.0
q̈(0)1.0
q(tf)0.2
q̇(tf)1.0
q̈(tf)0.0
tf5.0 s

One subtle feature of this problem is that the net displacement is negative (qf − q0 = −0.1) while both endpoint velocities are positive. A smooth polynomial satisfying those boundary conditions must therefore overshoot and reverse direction before ending at the lower final position. The overshoot visible in the polynomial plots is a mathematical consequence of the requested boundary conditions, not a plotting or simulation error.

Cubic versus quintic construction

The cubic trajectory uses four coefficients and can satisfy only four independent endpoint constraints. In this project it enforces position and velocity at both endpoints, so it cannot also satisfy both specified accelerations. The quintic trajectory has six coefficients and can satisfy all six endpoint conditions.

Boundary verification check Result
Cubic max residual for q and q̇ constraints2.220 × 10⁻¹⁶
Cubic acceleration mismatch at t = 0−3.024
Cubic acceleration mismatch at t = tf+1.624
Archived quintic closed-form formula9.500 × 10⁰ — FAIL
Corrected quintic max q/q̇/q̈ residual1.987 × 10⁻¹⁴

The archived closed-form quintic coefficient expressions do not satisfy the stated boundary-value problem. The public version therefore solves the six boundary equations directly to recover a mathematically correct quintic trajectory while preserving the original endpoint conditions.

Position trajectories

Comparison of cubic, corrected quintic, and corrected LSPB position trajectories
Figure 1. Joint-space position trajectories. The cubic and corrected quintic trajectories overshoot and then reverse direction because the requested displacement is negative while both endpoint velocities are positive.

Velocity trajectories

Comparison of cubic, corrected quintic, and corrected LSPB velocity trajectories
Figure 2. Joint-space velocity trajectories. Both polynomial trajectories change sign during the motion so that they can return to the lower final position while still ending with positive velocity.

Acceleration trajectories

Comparison of cubic, corrected quintic, and corrected LSPB acceleration trajectories
Figure 3. Joint-space acceleration trajectories. The quintic satisfies the requested endpoint accelerations, while the cubic cannot because it has insufficient degrees of freedom.

Polynomial jerk comparison

Cubic and quintic jerk comparison
Figure 4. Cubic and quintic jerk. The cubic has constant jerk, while the quintic produces a time-varying jerk profile. The corrected LSPB is omitted from this jerk comparison because ideal LSPB acceleration is piecewise constant and its jerk contains impulses at the blend transitions.

Boundary-condition residuals

Absolute boundary-condition residuals for cubic and corrected quintic trajectories
Figure 5. Boundary-condition residuals. The corrected quintic satisfies all six endpoint conditions to numerical precision, whereas the cubic shows the expected acceleration mismatch.

Position–velocity phase plane

Position-velocity plane for cubic, corrected quintic, and corrected LSPB trajectories
Figure 6. Position–velocity trajectory plane. The phase-plane view makes the reversal behavior of the polynomial trajectories particularly clear.

Corrected LSPB construction

The archived LSPB fragment used a positive cruise velocity even though the requested displacement was from 0.3 down to 0.2. That makes the original blend and cruise segments inconsistent. Keeping the source-derived blend time ta = 2 s, the cleaned implementation computes the only symmetric rest-to-rest LSPB cruise velocity that makes the trajectory continuous.

Corrected LSPB quantity Value
Blend time, ta2.0 s
Corrected cruise velocity, vc−0.033333333
Blend acceleration−0.016666667
Rest-to-rest continuity residual0.000 × 10⁰
Corrected LSPB position velocity and acceleration profiles
Figure 7. Corrected LSPB continuity. The corrected LSPB is a separate rest-to-rest point-to-point comparison. Position and velocity remain continuous across both blend transitions, while acceleration changes discontinuously as expected for standard LSPB motion.

Trajectory-shape metrics

The following quantities are trajectory-shape metrics, not actuator-energy measurements. They help compare how aggressively each trajectory varies in velocity, acceleration, and jerk.

Metric Cubic Corrected quintic Corrected LSPB
Peak |q̇|2.00002.09400.0333
Peak |q̈|2.02402.21650.0167
∫ q̈² dt5.745010.09280.0011
∫ q⃛² dt2.661622.9800—

These values should not be used to declare one trajectory family universally superior. The corrected LSPB solves a different endpoint problem than the polynomial trajectories, and the polynomial endpoint conditions themselves force reversal and overshoot.

Scope and limitations

This is a single-joint trajectory-generation study, not a full manipulator path planner. There are no joint limits, collision constraints, actuator limits, or torque-level dynamic models. The cleaned repository emphasizes mathematical validation: it shows what the original cubic setup can satisfy, corrects the quintic boundary-value construction, and repairs the inconsistent LSPB sign convention.

Reproducibility

The public repository contains one self-contained MATLAB runner that reproduces all results, figures, coefficient exports, and boundary-verification tables without requiring external toolboxes. The generated outputs were validated in MATLAB R2022b.