PID controller simulation?

Questions & Answers
1

Hi there,

for my robot simulation I need a simulated PID controller. In several states of the simulation the robot AI tries to rotate the robot to a specific variable angle. Since the robot body is a rigid body it behaves like the real robot with inertia and stuff. So on a constant force at two points near the playingfield (contact points of the wheels) the robot begins to rotate with increasing speed at a constant rate (integrating behaviour). The PID controller should be able to stop the rotation once the desired angle has been reached by reversing the forces. While a simple P controller would result in a permanent frequency in which the robot rotates from the left to the right and back, a PID controller should be able to reduce the forces to a value that lets the robot reach the desired angle.

Well, to cut a long story short… is it applicable to use a discrete PID simulation algorithm within Unity? Or is Unity, as I assume, by far too slow to achieve a reasonable calculation speed for a good simulation?

What other ways are there to achieve a rotation to a desired angle with the premise that it has to use a rigid body and forces (i.e. the physics engine), instead of manipulating the transform directly?

Thanks a lot!

Hendrik

2

You can use a true PID controller in Unity for position, but not for rotation. The problem is the feedback signal: Unity stores the rotation as a quaternion, which cannot be translated to the 3-axes notation reliably (not a quaternion’s fault: 3-axes notation is redundant, thus many different representations may be returned for the same quaternion).

But the show can go on! The quaternion->angle conversion is unreliable, but you can bet your life on the conversion angle->quaternion! The solution is to keep the current angle in a variable, calculate the proportional and differential errors (integral error only mess things up in this case), add them to generate the desired “torque” (angular acceleration, to be more precise), apply it to the angle, convert the angle to a quaternion and assign it to the object’s rotation. It’s simulated, but is physically correct, and works very well. The simulated PD controller (set to rotate around the Y axis) is:

var targetAngle: float = 0; // the desired angle
var curAngle: float; // current angle
var accel: float; // applied accel
var angSpeed: float = 0; // current ang speed
var maxAccel: float = 180; // max accel in degrees/second2
var maxASpeed: float = 90; // max angular speed in degrees/second
var pGain: float = 20; // the proportional gain
var dGain: float = 10; // differential gain
private var lastError: float; 

function Start(){
  targetAngle = transform.eulerAngles.y; // get the current angle just for start
  curAngle = targetAngle;
}

function FixedUpdate(){
  var error = targetAngle - curAngle; // generate the error signal
  var diff = (error - lastError)/ Time.deltaTime; // calculate differential error
  lastError = error;
  // calculate the acceleration:
  accel = error * pGain + diff * dGain;
  // limit it to the max acceleration
  accel = Mathf.Clamp(accel, -maxAccel, maxAccel);
  // apply accel to angular speed:
  angSpeed += accel * Time.deltaTime; 
  // limit max angular speed
  angSpeed = Mathf.Clamp(angSpeed, -maxASpeed, maxASpeed);
  curAngle += angSpeed * Time.deltaTime; // apply the rotation to the angle...
  // and make the object follow the angle (must be modulo 360)
  rigidbody.rotation = Quaternion.Euler(0, curAngle%360, 0); 
}

You can set positive or negative angles, and the object will rotate to the desired angle - including angles > 360 or < -360.

BONUS SCRIPT: Since you’re interested in PID controllers, that’s a simple PID position controller entirely based on physics, that tries to reach the position set in targetPos. It works fine, but the parameters must be fine tuned to reach a stable operation:

var targetPos = Vector3.zero; // the desired position
var maxForce: float = 100; // the max force available
var pGain: float = 20; // the proportional gain
var iGain: float = 0.5; // the integral gain
var dGain: float = 0.5; // differential gain
private var integrator = Vector3.zero; // error accumulator
private var lastError = Vector2.zero; 
var curPos = Vector3.zero; // actual Pos
var force = Vector3.zero; // current force

function Start(){
  targetPos = transform.position;
}

function FixedUpdate(){
  curPos = transform.position;
  var error = targetPos - curPos; // generate the error signal
  integrator += error * Time.deltaTime; // integrate error
  var diff = (error - lastError)/ Time.deltaTime; // differentiate error
  lastError = error;
  // calculate the force summing the 3 errors with respective gains:
  force = error * pGain + integrator * iGain + diff * dGain;
  // clamp the force to the max value available
  force = Vector3.ClampMagnitude(force, maxForce);
  // apply the force to accelerate the rigidbody:
  rigidbody.AddForce(force);
}