Cruise Control in Vehicle AI with PID Control (UE5 , C++)

Hello again my genius developer dude,
I will talk about Cruise control system with PID control in Unreal engine. Cruise control system provide constant speed when vehicle is moving. If we want to stable system with desired value, you can use feedback function like PID. For example, You want to hover your drone 30 meters or set to 20 degrees centigrade to air-condition. Maybe, Firstly you think to use “If, Else” (if expected value less than current value, do something etc.). However, system can not stable and response as expected. At this point, the PID feedback control function comes into play.

P = Proportional, I = Integral, D = Derivative namely PID. As we understand from the name, there are 3 different section in this function. After each section calculation, do multiply constant value (Kp, Ki, Kd). After end up calculation, sum up with all. This summation is that the value you give to function. Subsequently your function produce output called “u”. The “error” value called “e” is calculated by subtracting this output from the desired value. As seen in the picture.

• ERROR VALUE Term P is proportional to the current value of the Desired Value − Sensed Value error e(t). For example, if the error is large, the control output will be proportionately large by using the gain factor “Kp”.
• TOTAL ACCUMULATED VALUE Term I accounts for past values of the Desired Value − Sensed Value error and integrates them over time to produce the I term. For example, if there is a residual Desired Value − Sensed Value error after the application of proportional control, the integral term seeks to eliminate the residual error by adding a control effect due to the historic cumulative value of the error. When the error is eliminated, the integral term will cease to grow. This will result in the proportional effect diminishing as the error decreases, but this is compensated for by the growing integral effect.
• RATE-OF-CHANGE VALUE Term D is a best estimate of the future trend of the Desired Value − Sensored Value error, based on its current rate of change. It is sometimes called “anticipatory control”, as it is effectively seeking to reduce the effect of the Desired Value − Sensed Value error by exerting a control influence generated by the rate of error change. The more rapid the change, the greater the controlling or damping effect

First, we will start by setting up the struct. We created constant value.We created Upper and Lower Limit output for normalize data. Lastly we created integral variable for holding summation value and holding for last error called PreviousError.

USTRUCT(BlueprintType)
struct FPIDData
{
    GENERATED_BODY()

    // Constant value
    UPROPERTY(EditAnywhere, BlueprintReadWrite)
    float Kp;
    UPROPERTY(EditAnywhere, BlueprintReadWrite) 
    float Ki;
    UPROPERTY(EditAnywhere, BlueprintReadWrite) 
    float Kd;

    UPROPERTY(EditAnywhere, BlueprintReadWrite) 
    float UpperLimit;
    UPROPERTY(EditAnywhere, BlueprintReadWrite) 
    float LowerLimit;
    
    private:
    
      float Integral;
      float PreviousError;
};

We need to create Controller for calculation proportional, integral, derivative process. This controller inherit from UBlueprintFunctionLibrary. Because we want to reach statically and easily. My PIDController header file like this :

UCLASS()
class CODEEXAMPLE_API UPIDController : public UBlueprintFunctionLibrary
{
    GENERATED_BODY()

public:
    
    //Calculate PID process
    UFUNCTION(BlueprintCallable, Category = "PID")
        static float Calculate(FPIDData& PIDData, float Error, float DeltaTime);

    
    //Resets the integral and derivative related variables.
    UFUNCTION(BlueprintCallable, Category = "PID")
        static void Clear(FPIDData& PIDData);
};

Let’s talk about Calculate function. As I told you error mean is Desired Value – Sensed Value ( For vehicle : Current Speed ). Also we use error term as a Proportional term. Because we can make proportional calculations by looking at the difference between two values. Finally, we multiply this data by the constant value Kp.

 // P -> Calculation Value
 float ProportionalValue =   Error  * PIDData.P;

Now let’s calculate the integral value. Integral mean is the cumulative sum of the time-dependent error variable. Finally, we multiply this data by the constant value Ki.

// I -> Calculaion Value
PIDData.Integral += Error * DeltaTime;
float IntegralValue = PIDData.Integral   * PIDData.I;

In the last part, we will calculate the D value. D, the derivative value, is the change of the difference between the previous error and the current error over time. Of course, we do not forget to multiply by the Kd value.

// D -> Calculation Value
float Derivative = (Error - PIDData.PreviousError) / DeltaTime;
float DerivativeValue = Derivative * PIDData.D;

Final PID Calculate Function

float UPIDController::Calculate(FPIDData& PIDData, float Error, float DeltaTime)
{
    float Result;

    PIDData.Integral += Error * DeltaTime;

    float Derivative = (Error - PIDData.PreviousError) / DeltaTime;
    PIDData.PreviousError = Error;

    Result = Error      * PIDData.Kp +         // P
        PIDData.Integral   * PIDData.Ki +      // I
        Derivative * PIDData.Kd;               // D

    return FMath::Clamp(Result, PIDData.LowerLimit, PIDData.UpperLimit);
}

void UPIDController::Clear(FPIDData& PIDData)
{
    PIDData.Integral = 0.0f;
    PIDData.PreviousError = 0.0f;
}

Cruise Control in Unreal Engine

Okeeey, we finished explanation of PID and coding Unreal engine side. Now, we are implementing Cruise Control system. I’m using ChaosVehicleMovementComponent for throttle and brake input.

Create CruiseController struct from FPIDData and set initialize value.

  UPROPERTY(EditAnywhere)
	FPIDData CruiseController;
	
	CruiseController.Kp = 0.05f;
	CruiseController.Ki = 0.001f;
	CruiseController.Kd = 0.03f;
	CruiseController.LowerLimit = -1.0f;
	CruiseController.UpperLimit = 1.0f;   // Clamp output value because throttle and brake input range -1 and 1

Firstly, we will get vehicle forward speed and convert to cm/s to km/h.

float ForwardSpeedKmH = GetForwardSpeed() * 3600.0f / 100000.0f;	//convert from cm/s to km/h

And then calculate error

float SpeedError = DesiredSpeed - ForwardSpeedKmH; // ForwardSpeedKmH = Sensed Value

Call UPIDController::Calculate function. Function output called “Thrust” is that the throttle or brake value to be applied to the vehicle.

float ThrustValue += UPIDController::Calculate(CruiseController, SpeedError, DeltaTime);

Finally set the vehicle throttle and brake input using ChaosVehicleMovementComponent’s functions called SetThrottleInput(); and SetBrakeInput();

    if (ThrustValue >= 0)
		  {
			  SetThrottleInput(ThrustValue );
		  }
	  else
		  {
			  SetBrakeInput(-ThrustValue );
		  }

Thank you for reading this far. I hope it was a useful writing.

Also read previous post : Path Smoothing for AI in Unreal Engine 5 with C++

REFERENCES : https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller