MAVLINKHUD

Spline Trajectories

Executive Summary

For multirotors, stopping at every waypoint is inefficient and visually jarring. Spline Navigation allows the vehicle to fly a smooth, continuous curve through a series of waypoints without stopping.

ArduPilot uses Cubic Hermite Splines, which are defined by two points (Start, End) and two tangent vectors (Start Velocity, End Velocity). This ensures continuity of both Position ($C^0$) and Velocity ($C^1$) across segments.

Theory & Concepts

1. Why Splines?

  • Straight Lines: Requires stopping at corners (infinite acceleration) or "cutting the corner" (leash logic).
  • Splines: Define a curve where the velocity vector enters and exits the waypoint exactly aligned with the path, allowing high-speed transit.

2. Cubic Hermite Spline Math

The position $P(t)$ for $t \in [0, 1]$ is calculated using four basis functions ($h_{00}, h_{10}, h_{01}, h_{11}$) acting on the control points:

$$ P(t) = h_{00}(t)P_0 + h_{10}(t)M_0 + h_{01}(t)P_1 + h_{11}(t)M_1 $$

Where:

  • $P_0$: Origin Point
  • $P_1$: Destination Point
  • $M_0$: Origin Velocity Tangent
  • $M_1$: Destination Velocity Tangent

3. Centripetal Acceleration Limits

A tighter curve requires higher lateral acceleration ($a_c = v^2 / r$). The controller dynamically limits the speed along the spline to ensure the vehicle never exceeds WPNAV_ACCEL_C (Corner Acceleration) or the physical lean angle limits.

Codebase Investigation

1. Setting the Spline: AC_WPNav::set_spline_destination()

Located in libraries/AC_WPNav/AC_WPNav.cpp.

  • It calculates the Tangents (Velocity Vectors) for the spline.
  • Crucial Logic: The tangent at Waypoint $N$ is parallel to the line connecting $N-1$ and $N+1$. This creates the smooth "flow" through the point.

2. The Solver: SplineCurve::update_solution()

Located in libraries/AP_Math/SplineCurve.cpp.

  • It pre-calculates the Hermite coefficients so the realtime loop is fast. 
    _hermite_solution[0] = origin;
_hermite_solution[1] = origin_vel;
_hermite_solution[2] = -origin*3.0f -origin_vel*2.0f + dest*3.0f - dest_vel;
_hermite_solution[3] = origin*2.0f + origin_vel -dest*2.0f + dest_vel;

3. Execution: advance_target_along_track()

  • Moves the "Target Point" along the mathematical curve.
  • It adjusts dt (time step) based on the kinematic limits. If the curve is tight, it slows down the "time" variable to keep physical acceleration within bounds.

Source Code Reference

Practical Guide: Mission Planning with Splines

1. The "Loop-de-Loop" Problem

A common issue is the drone making a weird loop or S-turn before a waypoint.

  • Cause: The Tangents are too aggressive. If Waypoint 1, 2, and 3 form a sharp "V", the spline math tries to maintain velocity through the corner, forcing a wide overshoot.
  • Fix: Avoid acute angles (< 90 degrees) in spline missions. Use two waypoints to round a sharp corner.

2. Mixing Splines and Lines

You can mix NAV_WAYPOINT (Straight) and NAV_SPLINE_WAYPOINT (Curved) in the same mission.

  • Use Case: Fly a straight grid for mapping, then use splines to turn around smoothly at the end of each row to save battery and time.

3. Tuning for Cinematography

For smooth camera work:

  • Reduce WPNAV_ACCEL: Makes the drone accelerate/decelerate slower along the curve.
  • Reduce WPNAV_JERK: Softens the start/stop of movements.
  • Increase WPNAV_RADIUS: Does NOT affect Splines (Splines always pass through the point), but affects straight-line transitions.