Feedback

Drone Simulation and Control, Part 2: How Do You Get a Drone to Hover?

From the series: Drone Simulation and Control

In the last video, I showed that we can manipulate the four motors of a quadcopter to maneuver it in 3D space by getting it to roll, pitch, yaw and change its thrust.  We also covered the four sensors that we have at our disposal to estimate the system states.  In this video, we’re going to use that knowledge to design a control system architecture for hovering a quadcopter.  That means, we’re going to figure out which states we need to feedback, how many controllers we need to build and how those controllers interact with each other. I’m Brian, and welcome to a MATLAB Tech Talk.

In the control system we’re developing, the plant is the minidrone itself. It takes fourmotor speeds as inputs, which then spin the propellers, generating forces and torques that affect its output state. The output that we want is to have the the minidrone hover at a fixed altitude.  So the question becomes, how do we command these four motors autonomously so that happens? 

Well, to start, let’s assume that rather than do it autonomously, we want to command the minidrone manually. That is, we have a remote control with toggles that directly control all four motor speeds.  The left toggle would control the two front motors and the right toggle would control the two rear motors. This puts you in the feedback path because you could see where the drone is, and then react to its position and attitude by moving the four toggles in very specific ways. 

If you want to increase thrust, you’d speed up all four motors by moving the two toggles in this direction. Yaw requires that two opposing motors increase speed and the other two decrease speed so that yawing left, for example, would require this kind of toggle motion.  Then to roll the vehicle, you would increase one of the left/right pairs, and decrease the other.  And to pitch the vehicle, you would increase one of the front/back pairs and decrease the other.  In this way, you, as the feedback controller, could get the drone to hover by expertly changing the commands to these four motors.

While possible, thinking in terms of motor speed seems really hard, and we want to make our job easier, so instead we can use the motor mixing algorithm we created in the last video and command thrust, roll, pitch, and yaw directly. When we command thrust, for example, this single thrust command gets split evenly to all four motors, the yaw command is distributed positive to two motors and negative to the other two, and so on.

Now our remote control toggles are aligned with the intuitive roll, pitch, yaw, and thrust rather than mind bending motor speeds. This is the controller configuration that a lot of operators use when manually flying their quadcopters. But it turns out, thinking in terms of roll, pitch, yaw, and thrust is also beneficial when we’re developing an autonomous control system.  So we’ll keep the motor mixing algorithm and build a feedback control system with it in the loop as well.

Alright, let’s get rid of the human operator and think about how we can accomplish the same thing autonomously.  We’ll start by focusing on the thrust command.  Thrust is always in the same direction relative to the drone airframe, it’s along the Z axis of the minidrone. That means that if the drone is flying level, and the z-axis is aligned with the gravity vector, then increasing thrust causes the drone to increase its altitude rate, which is how fast it is rising, and decreasing thrust drops the altitude rate. That is pretty straight forward. However, if our drone is flying at a steep pitch or roll angle, then increasing thrust is coupled to both the altitude rate and the horizontal speed.

If we’re building a controller for a racing drone that is likely to fly at extreme roll and pitch angles then we need to take this coupling into account.  However, for our simple hover controller, I’m going to assume that the roll and pitch angles are always really small.  This way, changing the thrust only meaningfully impacts altitude rate and nothing else.  With this information, we can start our control design.

To begin, let’s build a controller that uses thrust to adjust the altitude. If we are able to measure the drone altitude then we can feed it back to compare it to an altitude reference.  The resulting error is then fed into an altitude controller that is using that information to determine how to increase or decrease thrust.  For now, we can think of this controller as some form of a PID controller and we’ll talk about tuning it in a future video in this series.

With this controller, if the drone is hovering too low, the altitude error will be positive and the thrust command will increase causing all four motors to speed up at the same time, and the drone will rise.  If the drone is too high, then all four motors will slow down so that the drone will descend.

So that’s it, right?  Our job is done. We’ve developed our simple altitude controller which will hover our minidrone.  And it’s perfect! Except, you know that’s not the case, because there are disturbances, like wind gusts, that will induce a little roll or pitch into the system and when that happens, the thrust will not only adjust altitude but also create some horizontal motion and the drone will start to move away from you.  And what good is a hover controller that maintains altitude but requires you to chase after it or it crashes into walls? That’s hardly hovering.

Nope, we clearly need a better control system architecture, and we should start by trying to maintain level flight by controlling roll and pitch angles to zero degrees.  If we can keep the mini drone level, then thrust once again will only impact altitude and the drone won’t wander away. 

We know from the last video, that we are able to command thrust, roll, pitch and yaw independently, that is, we can command one action without affecting the others.  Knowing this, we can create three more feed back controllers, one for roll, pitch, and yaw, exactly the same way we did for thrust.  To give us a little more room, I’m going to redraw the block diagram and will condense the motor mixing algorithm block to just say MMA.

Now, the output of the plant is more than just altitude, we’re also going to need to measure or estimate the roll, pitch, and yaw angles as well.  I’ll feed back all of the system states and then label which state I’m using for each controller so hopefully this all makes sense to you.  I’m feeding the estimated roll angle into the roll controller, and the yaw angle into the yaw controller, and so on.

Now, what do we have?

We have four independent or decoupled controllers, one for thrust, which is really an altitude controller since we’re claiming small roll and pitch angles. And then three controllers that are trying to maintain 0 degrees in roll, pitch, and yaw, respectively.  This should maintain altitude, keep the minidrone facing forward and level with the ground.

This is a better hover controller than our first one. But again, it’s still not perfect.  To understand why, let’s play our hypothetical wind gust through this system.  The wind might initially introduce a little roll angle, but our roll controller will remove that error and get the drone back to level flight.  However, for a very brief time during the roll the thrust vector is not straight up and down and therefore the drone will have moved horizontally a little bit.  Then another gust comes and causes another roll or pitch error and the drone walks away a little bit more.  So even though the drone won’t run away continuously like our first controller, this controller will still allow it to meander away slowly.  There is nothing in our control system architecture that will bring the drone back to its starting position.

Let’s improve this control system to do just that. I’m going to once again clean up our block diagram to make some room for additional control logic. OK, now that we have some room, let’s think about what roll and pitch are doing while hovering.  It’s tempting to say that both angles should be zero and that’s how we set up this current version of the controller. However, they may need to be non-zero in order to hover.  For example, if we want to hover in a strong constant wind, then the drone will have to lean into the wind, at some angle, to maintain its ground position. So rather than specifying that we want level flight, really what we need is a ground position controller, something that will recognize when the drone is wandering off and make the necessary corrections to bring it back to the reference point X and Y.

But just because we don’t want to pick specific roll and pitch angles, that doesn’t mean we don’t need the roll and pitch controllers.  Remember from the first video, that a quadcopter is incapable of moving left, right, forward, or backward without first rolling or pitching into the desired direction of travel. So our control system needs to couple position errors with roll and pitch.

This is a complicated sounding set of maneuvers, but luckily the resulting controller is pretty simple to understand. We can feedback the mini drone’s measured X, Y position and compare it to the reference to get the position error.  For now, we’ll say that the reference position is 0, 0. This way our controller will cause the drone to hover right above the take off point.

Our position controller takes the position error as an input, and then outputs roll and pitch angles. These are the reference angles that our roll and pitch controllers are trying to follow.  So instead of us, as the designer, picking roll and pitch reference angles, we’re letting the position controller create them for us. In this way, the position controller is the outer loop and it is generating the reference commands for the inner loop roll and pitch controllers. These are cascaded loops.

As a quick side note, the measured yaw angle also feeds into the position controller.  The reason, very briefly, is that the X, Y position error is relative to the ground, or the world reference frame, whereas roll and pitch are relative to the body of the drone.  Therefore, pitch doesn’t always move the drone in the X world direction and roll doesn’t always move the drone in the Y world direction. It depends on how the drone is rotated, or its yaw angle. So, if we need to move the drone to a very specific spot in the room, then it needs to know yaw in order to determine whether roll, pitch or some combine of two is needed to achieve that. So our position controller uses yaw to convert between the world X, Y from and the body X, Y frame.

Alright, let’s walk through a thought exercise to see how all of these controllers work together to maintain position and altitude.  Let’s say the mini drone is flying level at the correct altitude but a little too far left of where it took off. This will result in a position error that feeds into the position controller.  The proportional part of the controller will multiply that error by a constant which will request that the drone roll to the right.  The roll controller will see there is a roll error because the drone is still level and request a roll torque. This will play through the motor mixing algorithm and request that the motors on the left side of the drone speed up and the motors on the right side slow down.  This will roll the drone to the commanded angle. Now the drone will begin to move to the right, but since the vertical component of thrust is slightly smaller when rolled, the drone will also start to lose altitude. The altitude controller will sense this error and increase the thrust command accordingly. 

As the drone continues moving right, the position error is dropping and therefore the requested roll angle through the proportional path is also dropping, bringing the drone back level.  But if the drone is traveling too fast, then there is some horizontal momentum in the direction of travel that needs to be removed.  This is where the derivative term in the position controller is useful.  It can sense that the drone is closing in on the reference position quickly, and start to remove roll earlier, even causing it to roll in the opposite direction in order to slow the drone down and stop right where we want it to.

This is now a good architecture for our hover control system, but there are two glaring obstacles to creating and tuning it.  First, this requires having a way to estimate the states yaw, roll, pitch, altitude, and XY position.  These are the signals that we’re feeding back.  And second, we need to tune six PID controllers that all interact with each other, and specifically four of them directly coupled in cascaded loops. The way that we’re going to handle the first problem is by combining the measurements from the four sensors we have and in some cases using a model and a Kalman filter to estimate each of those feedback states.  For the second problem, we need a good model of our system so that we can use Model-Based Design and MATLAB and Simulink to tune our six PID controllers.

In fact, if we just look at the controller portion of this feedback system you can see that the Simulink model has pretty much the exact same controller architecture that we’ve built up in this video. The outer loop XY position controller is generating the reference pitch and roll angles for the inner loop pitch/roll controller.  There is also the Yaw and altitude controllers and each of them feed into the motor mixing algorithm.  In the next video, we’re going to leave the world of concepts and drawings and move over to this Simulink model to look at the actual software for this control system and the plant and environment dynamics.

So if you don’t want to miss the next tech talk video, don’t forget to subscribe to this channel. Also, if you want to check out my channel, control system lectures, I cover more control theory topics there as well. Thanks for watching, I’ll see you next time.

Other Resources