Source code: GitHub
...and follow me on Twitter: @eriknordeus
This is a tutorial on how you can minimize an error with the help of a PID controller. This is a technique used in real-life quadcopters, which is like a helicopter but with four rotors.
The difference between the last tutorials and this one is that you will need more than one PID controller to stabilize the quadcopter. You will need at least two, but we will use three to be on the safe side. The three errors you will need to minimize are: roll angle, pitch angle, and yaw angle velocity. If you minimize these three errors, the quadcopter will be able to hover in the air. If you don't, the quadcopter will crash and burn.
You need to build a simple quadcopter. Mine looks like this where the white sphere is there so we know which way is forward. I will refer to the different propellers as FL for front-left propeller, and so on.
But we also will need make the quadcopter fly around and not just hover in the air. This is how a real quadcopter is doing it:
This is the main quadcopter script. To control the quadcopter we will use WASD and the arrow keys. A real quadcopter is using similar controls:
There's a lot of different parameters you have to change to make everything work, and I've added mine in the comments in the code. My quadcopter's mass is 20 kg and its drag is 1, so add something like that to the rigidbody.
using UnityEngine;
using System.Collections;
public class QuadcopterController : MonoBehaviour
{
//The propellers
public GameObject propellerFR;
public GameObject propellerFL;
public GameObject propellerBL;
public GameObject propellerBR;
//Quadcopter parameters
[Header("Internal")]
public float maxPropellerForce; //100
public float maxTorque; //1
public float throttle;
public float moveFactor; //5
//PID
public Vector3 PID_pitch_gains; //(2, 3, 2)
public Vector3 PID_roll_gains; //(2, 0.2, 0.5)
public Vector3 PID_yaw_gains; //(1, 0, 0)
//External parameters
[Header("External")]
public float windForce;
//0 -> 360
public float forceDir;
Rigidbody quadcopterRB;
//The PID controllers
private PIDController PID_pitch;
private PIDController PID_roll;
private PIDController PID_yaw;
//Movement factors
float moveForwardBack;
float moveLeftRight;
float yawDir;
void Start()
{
quadcopterRB = gameObject.GetComponent<Rigidbody>();
PID_pitch = new PIDController();
PID_roll = new PIDController();
PID_yaw = new PIDController();
}
void FixedUpdate()
{
AddControls();
AddMotorForce();
AddExternalForces();
}
void AddControls()
{
//Change throttle to move up or down
if (Input.GetKey(KeyCode.UpArrow))
{
throttle += 5f;
}
if (Input.GetKey(KeyCode.DownArrow))
{
throttle -= 5f;
}
throttle = Mathf.Clamp(throttle, 0f, 200f);
//Steering
//Move forward or reverse
moveForwardBack = 0f;
if (Input.GetKey(KeyCode.W))
{
moveForwardBack = 1f;
}
if (Input.GetKey(KeyCode.S))
{
moveForwardBack = -1f;
}
//Move left or right
moveLeftRight = 0f;
if (Input.GetKey(KeyCode.A))
{
moveLeftRight = -1f;
}
if (Input.GetKey(KeyCode.D))
{
moveLeftRight = 1f;
}
//Rotate around the axis
yawDir = 0f;
if (Input.GetKey(KeyCode.LeftArrow))
{
yawDir = -1f;
}
if (Input.GetKey(KeyCode.RightArrow))
{
yawDir = 1f;
}
}
void AddMotorForce()
{
//Calculate the errors so we can use a PID controller to stabilize
//Assume no error is if 0 degrees
//Pitch
//Returns positive if pitching forward
float pitchError = GetPitchError();
//Roll
//Returns positive if rolling left
float rollError = GetRollError() * -1f;
//Adapt the PID variables to the throttle
Vector3 PID_pitch_gains_adapted = throttle > 100f ? PID_pitch_gains * 2f : PID_pitch_gains;
//Get the output from the PID controllers
float PID_pitch_output = PID_pitch.GetFactorFromPIDController(PID_pitch_gains_adapted, pitchError);
float PID_roll_output = PID_roll.GetFactorFromPIDController(PID_roll_gains, rollError);
//Calculate the propeller forces
//FR
float propellerForceFR = throttle + (PID_pitch_output + PID_roll_output);
//Add steering
propellerForceFR -= moveForwardBack * throttle * moveFactor;
propellerForceFR -= moveLeftRight * throttle;
//FL
float propellerForceFL = throttle + (PID_pitch_output - PID_roll_output);
propellerForceFL -= moveForwardBack * throttle * moveFactor;
propellerForceFL += moveLeftRight * throttle;
//BR
float propellerForceBR = throttle + (-PID_pitch_output + PID_roll_output);
propellerForceBR += moveForwardBack * throttle * moveFactor;
propellerForceBR -= moveLeftRight * throttle;
//BL
float propellerForceBL = throttle + (-PID_pitch_output - PID_roll_output);
propellerForceBL += moveForwardBack * throttle * moveFactor;
propellerForceBL += moveLeftRight * throttle;
//Clamp
propellerForceFR = Mathf.Clamp(propellerForceFR, 0f, maxPropellerForce);
propellerForceFL = Mathf.Clamp(propellerForceFL, 0f, maxPropellerForce);
propellerForceBR = Mathf.Clamp(propellerForceBR, 0f, maxPropellerForce);
propellerForceBL = Mathf.Clamp(propellerForceBL, 0f, maxPropellerForce);
//Add the force to the propellers
AddForceToPropeller(propellerFR, propellerForceFR);
AddForceToPropeller(propellerFL, propellerForceFL);
AddForceToPropeller(propellerBR, propellerForceBR);
AddForceToPropeller(propellerBL, propellerForceBL);
//Yaw
//Minimize the yaw error (which is already signed):
float yawError = quadcopterRB.angularVelocity.y;
float PID_yaw_output = PID_yaw.GetFactorFromPIDController(PID_yaw_gains, yawError);
//First we need to add a force (if any)
quadcopterRB.AddTorque(transform.up * yawDir * maxTorque * throttle);
//Then we need to minimize the error
quadcopterRB.AddTorque(transform.up * throttle * PID_yaw_output * -1f);
}
void AddForceToPropeller(GameObject propellerObj, float propellerForce)
{
Vector3 propellerUp = propellerObj.transform.up;
Vector3 propellerPos = propellerObj.transform.position;
quadcopterRB.AddForceAtPosition(propellerUp * propellerForce, propellerPos);
//Debug
//Debug.DrawRay(propellerPos, propellerUp * 1f, Color.red);
}
//Pitch is rotation around x-axis
//Returns positive if pitching forward
private float GetPitchError()
{
float xAngle = transform.eulerAngles.x;
//Make sure the angle is between 0 and 360
xAngle = WrapAngle(xAngle);
//This angle going from 0 -> 360 when pitching forward
//So if angle is > 180 then it should move from 0 to 180 if pitching back
if (xAngle > 180f && xAngle < 360f)
{
xAngle = 360f - xAngle;
//-1 so we know if we are pitching back or forward
xAngle *= -1f;
}
return xAngle;
}
//Roll is rotation around z-axis
//Returns positive if rolling left
private float GetRollError()
{
float zAngle = transform.eulerAngles.z;
//Make sure the angle is between 0 and 360
zAngle = WrapAngle(zAngle);
//This angle going from 0-> 360 when rolling left
//So if angle is > 180 then it should move from 0 to 180 if rolling right
if (zAngle > 180f && zAngle < 360f)
{
zAngle = 360f - zAngle;
//-1 so we know if we are rolling left or right
zAngle *= -1f;
}
return zAngle;
}
//Wrap between 0 and 360 degrees
float WrapAngle(float inputAngle)
{
//The inner % 360 restricts everything to +/- 360
//+360 moves negative values to the positive range, and positive ones to > 360
//the final % 360 caps everything to 0...360
return ((inputAngle % 360f) + 360f) % 360f;
}
//Add external forces to the quadcopter, such as wind
private void AddExternalForces()
{
//Important to not use the quadcopters forward
Vector3 windDir = -Vector3.forward;
//Rotate it
windDir = Quaternion.Euler(0, forceDir, 0) * windDir;
quadcopterRB.AddForce(windDir * windForce);
//Debug
//Is showing in which direction the wind is coming from
//center of quadcopter is where it ends and is blowing in the direction of the line
Debug.DrawRay(transform.position, -windDir * 3f, Color.red);
}
}
Next up is the PID controller. You can change a lot of settings in a PID controller. For example, when calculating the derivative of the error you can sometimes use an error from two time steps back and sometimes you use an error from the last time step. When calculating the sum of the errors, you can sometimes use a maximum value to not make the sum grow too big.
using UnityEngine;
using System.Collections;
public class PIDController
{
float error_old = 0f;
//The controller will be more robust if you are using a further back sample
float error_old_2 = 0f;
float error_sum = 0f;
//If we want to average an error as input
float error_sum2 = 0f;
//PID parameters
public float gain_P = 0f;
public float gain_I = 0f;
public float gain_D = 0f;
//Sometimes you have to limit the total sum of all errors used in the I
private float error_sumMax = 20f;
public float GetFactorFromPIDController(float error)
{
float output = CalculatePIDOutput(error);
return output;
}
//Use this when experimenting with PID parameters
public float GetFactorFromPIDController(float gain_P, float gain_I, float gain_D, float error)
{
this.gain_P = gain_P;
this.gain_I = gain_I;
this.gain_D = gain_D;
float output = CalculatePIDOutput(error);
return output;
}
//Use this when experimenting with PID parameters and the gains are stored in a Vector3
public float GetFactorFromPIDController(Vector3 gains, float error)
{
this.gain_P = gains.x;
this.gain_I = gains.y;
this.gain_D = gains.z;
float output = CalculatePIDOutput(error);
return output;
}
private float CalculatePIDOutput(float error)
{
//The output from PID
float output = 0f;
//P
output += gain_P * error;
//I
error_sum += Time.fixedDeltaTime * error;
//Clamp the sum
this.error_sum = Mathf.Clamp(error_sum, -error_sumMax, error_sumMax);
//Sometimes better to just sum the last errors
//float averageAmount = 20f;
//CTE_sum = CTE_sum + ((CTE - CTE_sum) / averageAmount);
output += gain_I * error_sum;
//D
float d_dt_error = (error - error_old) / Time.fixedDeltaTime;
//Save the last errors
this.error_old_2 = error_old;
this.error_old = error;
output += gain_D * d_dt_error;
return output;
}
}
And that's it! You can now take sneaky pictures of your neighbors:
