I'm trying to write a simple quadcopter simulation. It doesn't need to be 100% accurately simulated and I don't intend to write an actual drone controller, it's just supposed to "look" right in the simulation, and I'm trying to get a better understanding of PIDs. As such, I don't need to worry about sensors, or an accelerometer or gyroscope, I get all of the information from the simulation directly.

I'm new to quadcopter physics and PIDs, so there are a few things I'm a bit unclear on. I'm not asking for concrete solutions, I'd just like to know if my ideas go in the right direction and/or if I'm missing anything.

1)

Let's say the quadcopter has the world coordinates Q(0 -50 0) (y-axis being the up-axis) and I want it to fly to O(0 0 0). The velocity I need is O -Q, so I need to accelerate the drone until I reach that velocity (Correcting it every simulation tick). To do that, I need a PID, with the setpoint being the desired velocity O -Q, and the process variable being the current velocity of the drone (both on the y-axis), and 1/4th of the PID output is then applied as force to each rotor (Let's ignore the RPM of the rotors and assume I can just apply force directly). Is that correct?

2)

The drone is at the world coordinates Q(0 0 0) and should fly to P(100 0 100). It also has the orientation (p(itch)=0, y(aw)=0 and r(oll)=0). I want the drone to fly to point P, without changing its yaw-orientation. How would I calculate the target pitch- and roll-angles I need? I can work with euler angles and quaternions (not sure if I need to worry about gimbal lock in this case), I just need to know what formulas I need. Of course I also want to make sure the drone doesn't overturn itself.

3)

I want to change the drone's orientation from (p=0, y=0, r=0) to (p=20, y=0 r=0) (With positive pitch pointing downwards). To do so, I'd have to increase the force that is being applied by the front rotors, and/or decrease the force of the back rotors. I can use a PID to calculate the angular velocity I need for the drone (similar as in 1) ), but how do I get from there to the required forces on each rotor?

4)

Am I correct in assuming that I need 6 PIDs in total, 3 for the angular axes and 3 for the linear axes?

1 hour ago, Silverlan said:

1)

The velocity I need is O -Q

That's the velocity needed to travel from Q to O in one time unit.  Which...is probably not what you want? But yes, if you want to move at a certain velocity then your setpoint would be the target velocity and your process variable would be the current velocity.  But then you have to figure out how to set the target velocity over the whole path, and it's not so great for maintaining position.  I would more likely have the setpoint be a target location (and orientation) and the process variable be the current location.  Then you can plan a path and move the target along it at a comfortable cruising speed and the PIDs should make it follow fairly nicely.

Also, the "apply 1/4 force to each rotor" isn't exactly correct.  Maybe you were just glossing over the details?  To accelerate, you need to tip the quadcopter in the direction you want to accelerate and then apply enough force to the rotors to maintain height.  So the horizontal acceleration you get is based on the orientation as much as the rotor lift.

2) I would think you would use the cross-product of the yaw (up) axis and the direction that you want to go to get an axis of rotation.  Then convert axis-angle (I want to tilt this much on this axis) and convert to yaw/pitch/roll so you can apply it to the rotors easily (left/right pairs and forward/back pairs).  Here I would definitely feed the orientations to the PID rather than the angular velocities. You can probably get away without worrying about gimbal lock for a while since you'll only be making small orientation changes. But if something bad happens (you crash into something, or whatever) then you might run into trouble.

3) I would do this empirically: that's kind of what the PID is for.  Feed in the target orientation and the current orientation, and then tune the PID until the output values give you nice behavior.  IIRC Wikipedia has a procedure for manual tuning that works pretty well.

4) Yes, the linear axes are independent, so one PID for each.  There might be some better way to do the angular one?  But one per axis should work well enough to get you up and running...

Controlling the position of the drone is a very high level task, so you're coming at this from a top-down direction. I would recommend going bottom-up instead.

In order of simplicity, I'd make a PID controller for:

1. Rate / acro mode: This is what "drone racers" fly with. Stabilization is based on a gyroscope (or angular velocity from your physics system). The PID controller trying to reduce the error between a requested angular velocity (from the pilot's sticks) and the measured angular velocity. When you let go of the sticks, the drone should remain at the current orientation (i.e. it should make it appear that angular momentum doesn't exist).
2. Angle / level mode: This is what most "toy drones" fly with. Stabilization is based on an accelerometer (or orientation from your physics system). The PID controller is trying to reduce the error between the requested orientation (from the pilot's sticks) and the measured orientation. When you let go of the sticks, the drone should return to 0º pitch/roll. Yaw is actually processed the same way as in #1.
3. Hover -- this can be added on top of either of the above. A second layer of stabilization is based on an altimeter (or Y position from the physics system). An additional PID controller is trying to maintain the requested altitude. At middle throttle this PID controller is in full effect, or at negative/positive throttle stick positions this PID controller is attenuated to allow the drone to gain/lose altitude.
4. Positional movement from waypoint to waypoint.

Unless you already know how to fly an acrobatic drone and are interested in it, you can probably skip #1 and just do #2 instead (they are mutually exclusive controllers anyway). #3 is a secondary PID controller that is layered over the top of #2.

For doing autonomous flying (move from position to position), you would then add another PID controller on top (#4) which controls the virtual "pilot's sticks", which are fed into the PID controllers from step #2 and step #3. This layer probably doesn't even need to be a PID controller -- you can just get the direction to the target and directly feed that value to the "sticks" without any filtering at all.

So that gives:
Layer #2 -- converts the throttle/yaw and pitch/roll stick commands into motor outputs. Throttle inputs are not stabilized, but pitch/roll are converted into a target absolute orientation, and yaw is converted into a target angular velocity.
Layer #3 -- generates throttle stick movements based on a requested altitude.
Layer #4 -- generates yaw/pitch/roll stick movements based on a requested position/speed.

You should be able to disable layer 3/4 and fly it manually. 

So for your questions --
2) don't. The waypoint (#4) layer just needs to know what direction it wants to fly in and how fast. It then pushes the pitch/roll stick on that direction with a magnitude proportional to the speed that it wants. The angle stabilization layer (#2) then converts that stick position into a desired pitch/roll angle, and its PID controller adjusts the motor speeds until the drone's actual pitch/roll converges to the desired one. The conversion from stick position to angle can be as simple as:

float StickToAngle( float stick ) { /*stick is from -1 to +1*/ return stick * 30; }//at full stick movement, the drone will rotate 30 degrees.

When the drone rotates 30º, it will start to move sideways and also begin to lose altitude. The hovering layer's PID controller (#3) will then kick in due to the change in altitude, and will increase the throttle stick in order to increase thrust an maintain altitude. Layer #2 will read the throttle/yaw stick at the same time as it reads the pitch/roll stick, and adjust the motor speeds accordingly.

3) Make a map of which axis requests affect each motor. e.g. a pitch request (nose up please) will have +1 for the front two motors and -1 for the back two motors // a roll request (right wing down) will have +1 for the left two motors and -1 for the right two motors.

Make a sum for each motor and initialize it to zero. For each angular velocity output, add it to each motor based on the map.
e.g. if the request is pitch=0.5, roll=0.3
front-left = 0.5+0.3 = 0.7
front-right = 0.5-0.3 = 0.2
back-left = -0.5+0.3 = -0.2
back-right = -0.5-0.3 = -0.7
If any of these numbers go above 1.0 (100%), then you can divide all of them by the largest value to avoid loss of control during harsh manouvres. You make sure that the motors responses are not too high or too low by tweaking the P value in your Layer#2 PID.

4) Layer #2 needs 3 PIDs, Layer #3 needs 1 PID, Layer #4 needs 4 PIDs.

Thanks, that helps quite a bit! Still having a bit of trouble, however.

For testing, I disabled angular movement for the drone, as well as movement on the x- and z- axes (so it can only move up and down), and I've tried to implement #3 (keeping a specific altitude). However, no matter what I set the parameters of the PID to, I can't get it to change the rotor speed quickly enough, so it ends up overshooting the target massively:

(The target altitude is 0, which is more or less central between the green block and the ground floor.)

Here's the test-code I've used:

local pidAltitude = math.PIDController(100,20,5) -- Parameters: proportional, integral, derivative
function PhysDrone:Simulate(dt)
  local pos = self:GetPos() -- Current position
  local posTgt = Vector(0,0,0) -- Target position; x and z are ignored for now
  local v = pidAltitude:Calculate(pos.y,posTgt.y,dt) -- Parameters: processFeedback, setpoint, delta time
  for rotorId,entRotor in ipairs(self.m_rotors) do -- for each of the 4 rotors
    entRotor:ApplyTorque(Vector(0,v,0)) -- PID output is applied as a torque force to the rotor

    local up = entRotor:GetUp() -- Normalized vector pointing upwards from the rotor (Always the same direction as the up-vector of the drone)
    local liftForce = entRotor:GetLiftForce() -- How much lift force should be applied to the drone by this rotor (Depending on current revolutions per second)
    local posRotor = self:WorldToLocal(entRotor:GetPos()) -- Where the lift force originates from, relative to the drone
    self:ApplyForce(up *liftForce *dt,posRotor)
  end
end

I've tried following the tuning instructions for PIDs on wikipedia, but haven't had any luck, the result is always almost exactly what you see in the video. There are two different forces involved, which I suppose complicates things a little, but I'm not sure what to do here.

What are the two forces involved?

A PID that overshoots usually can be reigned in by tweaking the D term. You may need a smaller P term too.

13 hours ago, alvaro said:

What are the two forces involved?

The torque force of the rotors, as well as the lift force applied to the drone, as shown in the code snippet.

14 hours ago, alvaro said:

A PID that overshoots usually can be reigned in by tweaking the D term. You may need a smaller P term too.

The P-term has to be that high, otherwise the rotors take too long to reach the required RPS.

It gets increasingly unstable with higher values though.