controllers::PredictiveStanleyController Class Reference

This class implements the Predictive Stanley Controller. This controller is used to follow a path using the Stanley method with predictive control. More...

#include <PredictiveStanleyController.h>

Collaboration diagram for controllers::PredictiveStanleyController:

Classes
struct	DriveVector

Public Member Functions
	PredictiveStanleyController ()
	Construct a new Predictive Stanley Controller:: Predictive Stanley Controller object.
	PredictiveStanleyController (const double dControlGain, const double dSteeringAngleLimit, const double dWheelbase, const int nPredictionHorizon, const double dPredictionTimeStep)
	Construct a new Predictive Stanley Controller:: Predictive Stanley Controller object.
	~PredictiveStanleyController ()
	Destroy the Predictive Stanley Controller:: Predictive Stanley Controller object.
DriveVector	Calculate (const geoops::RoverPose &stCurrentPose, const double dMaxSpeed=constants::NAVIGATING_MOTOR_POWER)
	Calculate an updated steering angle for the rover based on the current pose using the predictive stanley controller.
void	SetReferencePath (const std::vector< geoops::Waypoint > &vReferencePath)
	Sets the path that the controller will follow.
void	SetReferencePath (const std::vector< geoops::UTMCoordinate > &vReferencePath)
	Sets the path that the controller will follow.
void	SetReferencePath (const std::vector< geoops::GPSCoordinate > &vReferencePath)
	Sets the path that the controller will follow.
void	SetControlGain (const double dControlGain)
	Setter for the control gain of the stanley controller.
void	SetSteeringAngleLimit (const double dSteeringAngleLimit)
	Setter for the steering angle limit of the stanley controller.
void	SetWheelbase (const double dWheelbase)
	Setter for the wheelbase of the stanley controller.
std::vector< geoops::Waypoint >	GetReferencePath () const
	Accessor for the reference path that the controller is following.
double	GetControlGain () const
	Accessor for the control gain of the stanley controller.
double	GetSteeringAngleLimit () const
	Accessor for the steering angle limit of the stanley controller.
double	GetWheelbase () const
	Accessor for the wheelbase of the stanley controller.
double	GetReferencePathTargetIndex () const
	Accessor for the current target index in the reference path.

Private Member Functions
geoops::Waypoint	FindClosestWaypointInPath (const geoops::UTMCoordinate &stCurrentPosition, const double dCurrentHeading)
	Given the current position of the rover, find the point on the reference path that is closest to the rover's front axle position (based on the wheelbase). This is what makes sure the rover progresses forward in indexes along the path.

Private Attributes
BicycleModel	m_BicycleModel
double	m_dControlGain
double	m_dSteeringAngleLimit
double	m_dWheelbase
int	m_nPredictionHorizon
double	m_dPredictionTimeStep
int	m_nCurrentReferencePathTargetIndex
std::vector< double >	m_vReferencePathCurvature
std::vector< geoops::Waypoint >	m_vReferencePath

Detailed Description

This class implements the Predictive Stanley Controller. This controller is used to follow a path using the Stanley method with predictive control.

Note

See docs/WhitePapers/2020-A-Path-Tracking-Algorithm-Using-Predictive-Stanley-Lateral-Controller.pdf

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

Constructor & Destructor Documentation

PredictiveStanleyController() [1/2]

controllers::PredictiveStanleyController::PredictiveStanleyController

(

)

Construct a new Predictive Stanley Controller:: Predictive Stanley Controller object.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Initialize member variables.
        m_dControlGain                     = constants::STANLEY_CROSSTRACK_CONTROL_GAIN;
        m_dWheelbase                       = constants::STANLEY_DIST_TO_FRONT_AXLE;
        m_dSteeringAngleLimit              = constants::STANLEY_STEERING_ANGLE_LIMIT;
        m_nPredictionHorizon               = constants::STANLEY_PREDICTION_HORIZON;
        m_dPredictionTimeStep              = constants::STANLEY_PREDICTION_TIME_STEP;
        m_nCurrentReferencePathTargetIndex = 0;
        m_BicycleModel                     = BicycleModel(m_dWheelbase, 0.0, 0.0, 0.0);
    }

PredictiveStanleyController() [2/2]

controllers::PredictiveStanleyController::PredictiveStanleyController	(	const double	dControlGain,
		const double	dSteeringAngleLimit,
		const double	dWheelbase,
		const int	nPredictionHorizon,
		const double	dPredictionTimeStep
	)

Construct a new Predictive Stanley Controller:: Predictive Stanley Controller object.

Parameters

dControlGain	- The control gain for the controller.
dSteeringAngleLimit	- The maximum steering angle the rover can turn.
dWheelbase	- The distance between the front and rear axles of the rover.
nPredictionHorizon	- The number of predictions to make.
dPredictionTimeStep	- The time step to predict the future state. How far into the future to predict.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Initialize member variables.
        m_dControlGain                     = dControlGain;
        m_dSteeringAngleLimit              = dSteeringAngleLimit;
        m_dWheelbase                       = dWheelbase;
        m_nPredictionHorizon               = nPredictionHorizon;
        m_dPredictionTimeStep              = dPredictionTimeStep;
        m_nCurrentReferencePathTargetIndex = 0;
        m_BicycleModel                     = BicycleModel(m_dWheelbase, 0.0, 0.0, 0.0);
    }

~PredictiveStanleyController()

controllers::PredictiveStanleyController::~PredictiveStanleyController

(

)

Destroy the Predictive Stanley Controller:: Predictive Stanley Controller object.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Nothing to do yet.
    }

Member Function Documentation

Calculate()

PredictiveStanleyController::DriveVector controllers::PredictiveStanleyController::Calculate	(	const geoops::RoverPose &	stCurrentPose,
		const double	dMaxSpeed = constants::NAVIGATING_MOTOR_POWER
	)

Calculate an updated steering angle for the rover based on the current pose using the predictive stanley controller.

Parameters

stCurrentPose	- The current pose of the rover.
dMaxSpeed	- The maximum speed the rover can travel.

Returns

double - The new output steering angle for the rover.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Create instance variables.
        double dSteeringAngle = 0.0;
        std::vector<BicycleModel::Prediction> vPredictions;
 
        // Check if the reference path is empty.
        if (m_vReferencePath.empty())
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger, "PredictiveStanleyController::Calculate: Reference path is empty. Cannot calculate drive powers.");
 
            return DriveVector{0.0, 0.0};
        }
 
        // Check if we are at the end of the path. Normally stanley would continue driving in the last direction of the calculated path
        // headings, but we want to make sure we get to the end point, so we'll just drive straight to it once at the end of the path.
        if (m_nCurrentReferencePathTargetIndex >= static_cast<int>(m_vReferencePath.size()) - 1)
        {
            // Get the last point in the path.
            geoops::Waypoint stLastWaypoint = m_vReferencePath.back();
            // Calculate the heading to the last point.
            double dHeadingToLastWaypoint = geoops::CalculateGeoMeasurement(stCurrentPose.GetUTMCoordinate(), stLastWaypoint.GetUTMCoordinate()).dStartRelativeBearing;
 
            return DriveVector{dHeadingToLastWaypoint, 1.0};
        }
 
        // Update the bicycle model with the current state.
        m_BicycleModel.UpdateState(stCurrentPose.GetUTMCoordinate().dEasting, stCurrentPose.GetUTMCoordinate().dNorthing, stCurrentPose.GetCompassHeading());
        // Predict the future state of the model.
        m_BicycleModel.Predict(m_dPredictionTimeStep, m_nPredictionHorizon, vPredictions);
 
        // Loop through all the predicted future states to compute the steering angle.
        for (size_t nIter = 0.0; nIter < vPredictions.size(); ++nIter)
        {
            // Create instance variables.
            double dPredictedXPosition = vPredictions[nIter].dXPosition;
            double dPredictedYPosition = vPredictions[nIter].dYPosition;
            double dPredictedTheta     = vPredictions[nIter].dTheta;
 
            // Create a UTM coordinate for the predicted position.
            geoops::UTMCoordinate stPredictedPosition = stCurrentPose.GetUTMCoordinate();
            stPredictedPosition.dEasting              = dPredictedXPosition;
            stPredictedPosition.dNorthing             = dPredictedYPosition;
            // Find the closest point to the reference path.
            geoops::Waypoint stClosestWaypoint = FindClosestWaypointInPath(stPredictedPosition, dPredictedTheta);
 
            // Compute the heading error. This is the difference between the heading of the rover and the heading or curvature of the path.
            double dHeadingError = numops::AngularDifference(m_vReferencePathCurvature[m_nCurrentReferencePathTargetIndex], dPredictedTheta);
 
            /*
                Compute the cross track error. This is the distance between the predicted position and the closest point on the path. The sign of the cross track error
                indicates which side of the path the rover is on. Left is positive, right is negative. The sign of the crosstrack error is determined by the sign of the
            */
 
            // Get the reference path vector: from the closest waypoint to the next waypoint.
            double dForwardVectorX = m_vReferencePath[m_nCurrentReferencePathTargetIndex + 1].GetUTMCoordinate().dEasting - stClosestWaypoint.GetUTMCoordinate().dEasting;
            double dForwardVectorY =
                m_vReferencePath[m_nCurrentReferencePathTargetIndex + 1].GetUTMCoordinate().dNorthing - stClosestWaypoint.GetUTMCoordinate().dNorthing;
            // Compute the norm and unit vector for the path segment.
            double dForwardNorm = sqrt(dForwardVectorX * dForwardVectorX + dForwardVectorY * dForwardVectorY);
            double dFwdUnitX    = dForwardVectorX / dForwardNorm;
            double dFwdUnitY    = dForwardVectorY / dForwardNorm;
            // Get the vehicle's position vector relative to the closest waypoint.
            double dVehicleVectorX = dPredictedXPosition - stClosestWaypoint.GetUTMCoordinate().dEasting;
            double dVehicleVectorY = dPredictedYPosition - stClosestWaypoint.GetUTMCoordinate().dNorthing;
            // Project the vehicle vector onto the path unit vector to obtain the longitudinal component.
            double dLongitudinal = dVehicleVectorX * dFwdUnitX + dVehicleVectorY * dFwdUnitY;
            // Compute the lateral error vector by subtracting the longitudinal projection from the vehicle vector.
            double dLateralX = dVehicleVectorX - dLongitudinal * dFwdUnitX;
            double dLateralY = dVehicleVectorY - dLongitudinal * dFwdUnitY;
            // The cross-track error is the magnitude of this lateral vector.
            double dLateralDistance = sqrt(dLateralX * dLateralX + dLateralY * dLateralY);
            // Determine the sign of the cross-track error using the cross product (left positive, right negative).
            int nCrossTrackErrorSign = (dFwdUnitX * dVehicleVectorY - dFwdUnitY * dVehicleVectorX) > 0 ? 1 : -1;
            // Final cross-track error.
            double dCrossTrackError = nCrossTrackErrorSign * dLateralDistance;
 
            // Apply an exponential weight factor that decreases as we predict further into the future.
            double dTimeWeight = std::exp(-1.5 * static_cast<double>(nIter));
            // Limit the cross track error steering angle.
            dCrossTrackError = std::clamp(m_dControlGain * dCrossTrackError, -m_dSteeringAngleLimit, m_dSteeringAngleLimit);
            // Calculate the steering angle using lateral and heading errors, weighted by the time step.
            dSteeringAngle += dTimeWeight * (dCrossTrackError - dHeadingError);
 
            // Limit the steering angle to the given limit.
            dSteeringAngle = std::clamp(dSteeringAngle, -m_dSteeringAngleLimit, m_dSteeringAngleLimit);
        }
 
        // The new steering heading must be from 0-360 degrees.
        double dAbsoluteHeadingGoal = numops::InputAngleModulus(stCurrentPose.GetCompassHeading() + dSteeringAngle, 0.0, 360.0);
 
        return DriveVector{dAbsoluteHeadingGoal, dMaxSpeed};
    }

Here is the call graph for this function:

Here is the caller graph for this function:

SetReferencePath() [1/3]

void controllers::PredictiveStanleyController::SetReferencePath

(

const std::vector< geoops::Waypoint > &

vReferencePath

)

Sets the path that the controller will follow.

Parameters

vReferencePath

- A reference to a vector of waypoints that make up the path.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Create instance variables.
        double dCurvature = 0.0;
 
        // Apply path smoothing first.
        std::vector<geoops::Waypoint> vSmoothedPath = pathplanners::postprocessing::FitPathWithBSpline(vReferencePath);
 
        // Loop through the reference path and calculate the curvature at each point.
        for (size_t nIter = 0; nIter < vSmoothedPath.size(); ++nIter)
        {
            // Calculate the curvature at this point.
            if (nIter > 0 && nIter < vSmoothedPath.size() - 1)
            {
                // Calculate the curvature.
                dCurvature = geoops::CalculateGeoMeasurement(vSmoothedPath[nIter - 1].GetUTMCoordinate(), vSmoothedPath[nIter].GetUTMCoordinate()).dStartRelativeBearing;
 
                // If this is the second iteration, then also set the curvature of the previous point.
                if (nIter == 1)
                {
                    m_vReferencePathCurvature[0] = dCurvature;
                }
            }
 
            // Store the waypoint and curvature.
            m_vReferencePathCurvature.push_back(dCurvature);
        }
 
        // Reset the current target index.
        m_nCurrentReferencePathTargetIndex = 0;
        // Reset the bicycle model.
        m_BicycleModel.ResetState();
        // Set the reference path.
        m_vReferencePath = vSmoothedPath;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

SetReferencePath() [2/3]

void controllers::PredictiveStanleyController::SetReferencePath

(

const std::vector< geoops::UTMCoordinate > &

vReferencePath

)

Sets the path that the controller will follow.

Parameters

vReferencePath

- A reference to a vector of UTM coordinates that make up the path.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Create instance variables.
        double dCurvature = 0.0;
 
        // Apply path smoothing first.
        std::vector<geoops::Waypoint> vSmoothedPath = pathplanners::postprocessing::FitPathWithBSpline(vReferencePath);
 
        // Loop through the reference path and calculate the curvature at each point.
        for (size_t nIter = 0; nIter < vSmoothedPath.size(); ++nIter)
        {
            // Calculate the curvature at this point.
            if (nIter > 0 && nIter < vSmoothedPath.size() - 1)
            {
                // Calculate the curvature.
                dCurvature = geoops::CalculateGeoMeasurement(vSmoothedPath[nIter - 1].GetUTMCoordinate(), vSmoothedPath[nIter].GetUTMCoordinate()).dStartRelativeBearing;
 
                // If this is the second iteration, then also set the curvature of the previous point.
                if (nIter == 1)
                {
                    m_vReferencePathCurvature[0] = dCurvature;
                }
            }
 
            // Store the waypoint and curvature.
            m_vReferencePathCurvature.push_back(dCurvature);
        }
 
        // Reset the current target index.
        m_nCurrentReferencePathTargetIndex = 0;
        // Reset the bicycle model.
        m_BicycleModel.ResetState();
        // Set the reference path.
        m_vReferencePath = vSmoothedPath;
    }

Here is the call graph for this function:

SetReferencePath() [3/3]

void controllers::PredictiveStanleyController::SetReferencePath

(

const std::vector< geoops::GPSCoordinate > &

vReferencePath

)

Sets the path that the controller will follow.

Parameters

vReferencePath

- A reference to a vector of GPS coordinates that make up the path.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Create instance variables.
        double dCurvature = 0.0;
 
        // Apply path smoothing first.
        std::vector<geoops::Waypoint> vSmoothedPath = pathplanners::postprocessing::FitPathWithBSpline(vReferencePath);
 
        // Loop through the reference path and calculate the curvature at each point.
        for (size_t nIter = 0; nIter < vSmoothedPath.size(); ++nIter)
        {
            // Calculate the curvature at this point.
            if (nIter > 0 && nIter < vSmoothedPath.size() - 1)
            {
                // Calculate the curvature.
                dCurvature = geoops::CalculateGeoMeasurement(vSmoothedPath[nIter - 1].GetUTMCoordinate(), vSmoothedPath[nIter].GetUTMCoordinate()).dStartRelativeBearing;
 
                // If this is the second iteration, then also set the curvature of the previous point.
                if (nIter == 1)
                {
                    m_vReferencePathCurvature[0] = dCurvature;
                }
            }
 
            // Store the waypoint and curvature.
            m_vReferencePathCurvature.push_back(dCurvature);
        }
 
        // Reset the current target index.
        m_nCurrentReferencePathTargetIndex = 0;
        // Reset the bicycle model.
        m_BicycleModel.ResetState();
        // Set the reference path.
        m_vReferencePath = vSmoothedPath;
    }

Here is the call graph for this function:

SetControlGain()

void controllers::PredictiveStanleyController::SetControlGain

(

const double

dControlGain

)

Setter for the control gain of the stanley controller.

Parameters

dControlGain

- The control gain for the controller.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        m_dControlGain = dControlGain;
    }

Here is the caller graph for this function:

SetSteeringAngleLimit()

void controllers::PredictiveStanleyController::SetSteeringAngleLimit

(

const double

dSteeringAngleLimit

)

Setter for the steering angle limit of the stanley controller.

Parameters

dSteeringAngleLimit

- The steering angle limit for the controller.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-11

    {
        m_dSteeringAngleLimit = dSteeringAngleLimit;
    }

Here is the caller graph for this function:

SetWheelbase()

void controllers::PredictiveStanleyController::SetWheelbase

(

const double

dWheelbase

)

Setter for the wheelbase of the stanley controller.

Parameters

dWheelbase

- The distance between the front and rear axles of the rover.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-11

    {
        m_dWheelbase = dWheelbase;
    }

Here is the caller graph for this function:

GetReferencePath()

std::vector< geoops::Waypoint > controllers::PredictiveStanleyController::GetReferencePath

(

)

const

Accessor for the reference path that the controller is following.

Returns

std::vector<geoops::Waypoint> - A copy of the reference path.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        return m_vReferencePath;
    }

Here is the caller graph for this function:

GetControlGain()

double controllers::PredictiveStanleyController::GetControlGain

(

)

const

Accessor for the control gain of the stanley controller.

Returns

double - The control gain for the controller.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        return m_dControlGain;
    }

Here is the caller graph for this function:

GetSteeringAngleLimit()

double controllers::PredictiveStanleyController::GetSteeringAngleLimit

(

)

const

Accessor for the steering angle limit of the stanley controller.

Returns

double - The steering angle limit for the controller.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-11

    {
        return m_dSteeringAngleLimit;
    }

Here is the caller graph for this function:

GetWheelbase()

double controllers::PredictiveStanleyController::GetWheelbase

(

)

const

Accessor for the wheelbase of the stanley controller.

Returns

double - The wheelbase of the controller.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-11

    {
        return m_dWheelbase;
    }

Here is the caller graph for this function:

GetReferencePathTargetIndex()

double controllers::PredictiveStanleyController::GetReferencePathTargetIndex

(

)

const

Accessor for the current target index in the reference path.

Returns

double - The current target index in the reference path.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        return m_nCurrentReferencePathTargetIndex;
    }

Here is the caller graph for this function:

FindClosestWaypointInPath()

geoops::Waypoint controllers::PredictiveStanleyController::FindClosestWaypointInPath ( const geoops::UTMCoordinate &  stCurrentPosition, const double  dCurrentHeading  )

private

Given the current position of the rover, find the point on the reference path that is closest to the rover's front axle position (based on the wheelbase). This is what makes sure the rover progresses forward in indexes along the path.

Parameters

stCurrentPosition	- The current position of the rover.
dCurrentHeading	- The current heading of the rover in degrees from 0-360.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2025-01-10

    {
        // Initialize variables.
        geoops::Waypoint stClosestWaypoint;
        geoops::UTMCoordinate stFrontAxlePosition = stCurrentPosition;
        double dClosestDistance                   = std::numeric_limits<double>::max();
 
        // Convert the heading to radians.
        double dCurrentHeadingRad = dCurrentHeading * M_PI / 180.0;
 
        // Calculate the current position of the front axle based on the wheelbase, heading, and current position.
        stFrontAxlePosition.dEasting  = stCurrentPosition.dEasting + m_dWheelbase * std::sin(dCurrentHeadingRad);
        stFrontAxlePosition.dNorthing = stCurrentPosition.dNorthing + m_dWheelbase * std::cos(dCurrentHeadingRad);
 
        // Loop through the reference path.
        for (size_t nIter = 0; nIter < m_vReferencePath.size(); ++nIter)
        {
            // Calculate the distance to the current waypoint.
            double dDistance = geoops::CalculateGeoMeasurement(stFrontAxlePosition, m_vReferencePath[nIter].GetUTMCoordinate()).dDistanceMeters;
 
            // Check if this waypoint is closer.
            if (dDistance < dClosestDistance)
            {
                // Update the closest waypoint.
                stClosestWaypoint = m_vReferencePath[nIter];
                dClosestDistance  = dDistance;
 
                // Update the current target index based on the location of the closest waypoint in the vector path.
                m_nCurrentReferencePathTargetIndex = nIter;
            }
        }
 
        // Return the closest waypoint.
        return stClosestWaypoint;
    }

Here is the call graph for this function:

Here is the caller graph for this function:

---

The documentation for this class was generated from the following files:

• src/algorithms/controllers/PredictiveStanleyController.h
• src/algorithms/controllers/PredictiveStanleyController.cpp