Predictive Stanley Controller

The PredictiveStanleyController (src/algorithms/controllers/PredictiveStanleyController.h) implements an advanced lateral path-tracking controller based on the Stanley method, augmented with a kinematic prediction model to compensate for rover mass, skid-steer slip, and actuation latency.

---

1. Motivation: Stanley vs Pure Heading Control

Standard heading PID controllers orient the rover toward a target point, but cannot independently regulate the lateral offset from a curving reference path. If a rover deviates laterally, a pure heading controller only steers toward the next node, often resulting in path-cutting, corner-clipping, and lateral drift.

The Stanley Controller simultaneously minimizes two independent error terms:

1. Heading Error ($\theta_e$): The angular difference between the rover’s heading and the tangent of the nearest path segment.
2. Cross-Track Error ($e_{\text{ct}}$): The perpendicular distance from the center of the rover to the reference path.

---

2. Mathematical Formulation

Standard Stanley Control Law

For a front-steered vehicle at forward velocity $v$, the classic Stanley steering angle $\delta(t)$ is defined as:

\[\delta(t) = \theta_e(t) + \arctan\left(\frac{k \cdot e_{\text{ct}}(t)}{v(t) + v_{\text{soft}}}\right)\]

• $k$ (dControlGain): Cross-track control gain determining correction aggressiveness.
• $v_{\text{soft}}$ (STANLEY_MIN_STABLE_SPEED): Softening velocity parameter preventing numerical divergence or erratic steering at near-zero forward speeds.

Predictive Horizon with Unicycle Kinematics

Because a 50 kg skid-steer rover cannot instantaneously translate or rotate, applying the steering law strictly to the rover’s current coordinates results in overshoot and oscillation around the path.

The PredictiveStanleyController integrates a Unicycle Kinematic Model (src/algorithms/kinematics/UnicycleModel.hpp) to evaluate error over a prediction horizon:

1. Forward State Prediction: Using the rover’s current linear velocity $v$ and angular velocity $\omega$, the Unicycle model simulates state forward across $N$ steps (STANLEY_PREDICTION_HORIZON, default 5) with time step $\Delta t$ (STANLEY_PREDICTION_TIME_STEP, default 0.01 seconds): \(x_{k+1} = x_k + v \cos(\theta_k) \cdot \Delta t\) \(y_{k+1} = y_k + v \sin(\theta_k) \cdot \Delta t\) \(\theta_{k+1} = \theta_k + \omega \cdot \Delta t\)
2. Projected Error Evaluation: The cross-track error $e_{\text{ct}}$ and heading error $\theta_e$ are evaluated against the predicted future pose $(x_N, y_N, \theta_N)$ rather than the present pose.
3. Angular Velocity Clamping: The commanded steering rate is clamped by STANLEY_ANGULAR_VELOCITY_LIMIT (e.g., $90.0^\circ/\text{s}$) to prevent skid-steer track slip from exceeding the adhesion limit of the terrain.

---

3. Output Data Structure: DriveVector

The controller returns a PredictiveStanleyController::DriveVector struct:

struct DriveVector
{
    double dThetaHeading;  // Target absolute compass heading setpoint
    double dVelocity;      // Target linear velocity
};

This output is fed directly into DriveBoard::CalculateMove(), which uses the heading PID controller and differential drive kinematics to actuate the left and right tracks.

---

4. Tuning Constants in AutonomyConstants.cpp

Constant Name	Value	Purpose and Tuning Directive
STANLEY_CROSSTRACK_CONTROL_GAIN	0.1	Cross-track error scaling ($k$). Higher values pull the rover toward the path more aggressively but can induce weave oscillations.
STANLEY_ANGULAR_VELOCITY_LIMIT	90.0	Maximum turning rate allowed (deg/s). Prevents track slip on loose dirt or sand.
STANLEY_PREDICTION_HORIZON	5	Number of discrete forward simulation steps evaluated by the Unicycle model.
STANLEY_PREDICTION_TIME_STEP	0.01	Integration timestep (seconds) per prediction step.
STANLEY_MIN_STABLE_SPEED	0.1	Softening constant $v_{\text{soft}}$ in denominator (m/s). Prevents division by zero when stopped.
STANLEY_WHEELBASE	0.8	Distance between front and rear axle centers in meters.

---

5. Implementation Safeguards

• Minimum Path Node Requirement: The Calculate() method enforces that the reference path contains at least 2 points (m_vReferencePath.size() < 2). If the reference path has 0 or 1 waypoint, Calculate() logs a warning:

  PredictiveStanleyController::Calculate: Reference path has fewer than 2 points. Cannot calculate drive powers.
  

  and returns DriveVector{0.0, 0.0}. This prevents segmentation faults and undefined behavior when evaluating line segment tangents or cross-track projections near the terminal end of a path.

---

6. Usage Example

// Set the reference path generated by GeoPlanner
m_StanleyController.SetReferencePath(vGeoPlannerPath);

// Inside the navigation loop:
geoops::RoverPose stPose = globals::g_pStateMachineHandler->SmartRetrieveRoverPose();
PredictiveStanleyController::DriveVector stVector = m_StanleyController.Calculate(stPose, constants::NAVIGATING_MOTOR_POWER);

// Pass resulting heading setpoint and speed to DriveBoard kinematics
globals::g_pDriveBoard->CalculateMove(stVector.dVelocity, stVector.dThetaHeading, stPose.GetCompassHeading());
globals::g_pDriveBoard->SendDrive();