Description Assignment 4: Control

Objectives

The objectives of the fourth assignment is to get a basic understanding of control. The examples used will be connected with autonomous systems but the theory is very general.

Prerequisite

Before starting, make sure that you have done the assignments setup.

There was a bug-fix in the code connected to assignment 4 on Oct 8. Make sure to get the latest version of the code with

cd ~/ros2_ws/src/wasp_autonomous_systems
git pull

Deliverable

At the end of the assignment you submit your work here Assignment 4. You will be asked questions below, the answers to which will be part of what you need to deliver, so take notes.

Introduction

In this assignment we will work with control of flying robots. To start with, we will control a DJI Mavic 2 PRO seen below.

We will start by looking at controlling the height of the drone. The output that we want to control is thus the z-coordinate and the control signal that we have at our disposal is the thrust of the propellers. To begin with we will simplify things and think of the thrust as one signal but as you can see there are four propellers which we need to control individually if we actually wanted the drone to stay upright. In the video below we see the drone in its start position that then takes off. The thrust is here set manually as you would if you flew with a remote control. Your first task is to design a controller that allows the drone to reach the specified target height automatically.

The Mavic 2 PRO simulator

Before we start with the control let us start the simulator. Execute the following in a terminal:

ros2 launch assignment_4 mavic_simulator.launch.py

This should open up the webots simulator with the Mavic drone sitting in the start area similar to the first video above.

If the above environment is too heavy for your computer you may run a simpler environment:

ros2 launch assignment_4 mavic_simulator_simple.launch.py

Manual height control

Before we start with the automatic control, we want you to get a feeling for manual control to get to know the system a bit better. In another terminal execute the following:

ros2 run assignment_4 altitude_manual

In another terminal window execute

ros2 run rqt_reconfigure rqt_reconfigure

Select altitude_manual in the list to the left. You might need to press Refresh depending on the order you did the above things. You should get something like what you see below.

The program altitude_manual that you started above allows you to set five parameters. The first is a thrust signal that will influence all propellers the same way. This is what we will use in this assignment to start with. The signals m1_offset, etc allow you to influence the individual propellers. The sum of the two will be sent to each motor. That is, thrust+m1_offset will be sent to motor1, etc.

Q: What thrust signal roughly is needed to make the drone lift?

Open a new rqt window by executing the following in another terminal

ros2 run rqt_plot rqt_plot /mavic_2_pro/gps/point/z

This allows you to get a better view of the actual height of the drone. You can add additional topic to plot by using the + sign.

Try to control it to a height of 10m.

Q: Does it stay at the same height? Screenshot an image of a nice plot illustrating your answer.

Q: How fast are you able to move it from one height to another? What is your strategy? Screenshot an image of a nice plot illustrating your answer.

Now you have acted as the controller and we are ready to see how we can do this by using automatic control!

Background and Theory I

If you have no or only little experience of control you might want to start with watching the introduction videos 1 and 2 (out of 46) in this series of control lectures. Links to an external site.

Make sure you understand the fundamental difference between open loop control and feedback control. The main need for the use of feedback control is the presence of uncertainty (disturbances, imperfect models etc...). Without uncertainty there would be little need for feedback. In the safe world of simulations, where you can assume perfect knowledge of system dynamics and where there are no disturbances you can easily miss the importance. You however quickly notice if you, for some strange reason, want to move from theory to practice.

Watching the videos above is enough to continue, but if you want even more background before going further, we recommend reading Chapter 1 in the book Feedback Systems, by Åström and Murray. Links to an external site. (Karl Johan Åström is professor emeritus in Lund, Richard Murray is professor at Caltech, and member of the advisory board of WASP)

A control design is typically a trade-off between competing objectives. To do a good design one needs to understand the existing trade-offs and then find a good compromise. The conclusion can sometimes be that the objectives are impossible to achieve because of fundamental limitations, and that the only solution is to redesign the system, for instance adding new sensors getting a stronger motor. A good design method should provide the user with appropriate "tuning knobs" for investigating and understanding these trade-offs.

We will start by doing a control design using the most popular method there is: the PID controller, which has three tuning knobs, that often are tuned by hand.

If you want more background on PID control before we start, you can watch these videos

PID intro Links to an external site.

PID examples Links to an external site.

Height control with PID control

The PID controller consists of three parts:

a proportional part considering the present error,
an integral part, considering the error history,
a derivative part considering the current error trend.

The control signal u (input to the system), i.e. the thrust in our case, is of the form

$u (t) = k_{p} e (t) + k_{i} \int_{0}^{t} e (τ) d τ + k_{d} \frac{d e (t)}{d t}$ $u(t) = k_pe(t) + k_i \int_0^t e(\tau)\,d\tau + k_d \frac{de(t)}{dt}$ ,

where LaTeX: e(t)=r(t)-y(t) $e(t)=r(t)-y(t)$ is the control error, LaTeX: y(t) $y(t)$ is the output of the system (i.e. the height), and $r (t)$ $r(t)$ is the reference input (i.e. desired height).

Reasons for the popularity of PID control is that the three tuning knobs $(k_{p}, k_{i}, k_{d})$ $(k_p, k_i, k_d)$ are easy to understand and can often be tuned manually even without deep process knowledge, and that the control structure is often sufficient for acceptable performance.

Let us get our hands dirty!

Make sure that you have closed altitude_manual (from above) and instead start altitude_pid with

ros2 run assignment_4 altitude_pid

Make sure that you have rqt_reconfigure running and select altitude_pid. NOTE: If you restart altitude_pid you might need to refresh and select again to ensure that you are connected with the new process.

P-controller

Set r = 10 in rqt_reconfigure, i.e. let the reference height be 10m.

Q: Guided by your previous experiment with the thrust to lift and the equation of a P-controller $u(t)=k_pe(t)$ , what is the minimum value of $k_p$ for the drone to lift and why? Tip: Look at the equation for the P-controller.

Make so called step response experiments by resetting the webot simulator and let the drone fly from the ground up with r=10.

Note when you try different $k_p$ that there is a trade-off between rise time (how fast it approaches height 10), overshoot, and the time time it takes to settle.

Q: What is your best setting of kp? How did you reason? Save a plot of the step response (with r=10).

Q: What height does it reach with your best setting?

Q: What is the so called steady state error (ie the error when any oscillations have stopped)? It is not 0, why?

PI-controller

Let us try to remove the error by introducing some integral action with a PI-controller, $LaTeX: u(t)=k_pe(t)+k_i\int_0^te(\tau)\,d\tau$ $u(t)=k_pe(t)+k_i\int_0^te(\tau)\,d\tau$ . The integral term will change as long as there is an error. For the system to reach steady state the error therefore must go to zero (assuming a stable system).

Q: What are your best settings of kp and ki? Again what was your considerations? Save a step response plot.

PID-controller

As you probably noted the system had problems with oscillations. We can reduce these by introducing derivative action and use the full PID-controller $u (t) = k_{p} e (t) + k_{i} \int_{0}^{t} e (τ) d τ + k_{d} \frac{d e (t)}{d t}$ $u(t) = k_pe(t) + k_i \int_0^t e(\tau)\,d\tau + k_d \frac{de(t)}{dt}$ . The derivate part is proportional to the change in error. That is, it will dampen the system by "breaking" more the faster the error is reduced. This stabilizes the system. Note though that taking the derivative of a noisy signal typically means that you need some form of filter.

Q: What is your best settings of kp, ki and kd? Your considerations? Save the step response plot.

PID + feedforward

Let us take a step back and consider a simplified dynamical model of the height control

$LaTeX: m\ddot h(t) = f(t)-mg$ $m\ddot h(t) = f(t)-mg$

Here $h (t)$ $h(t)$ denotes the height, which we want to follow a certain reference signal $r (t)$ $r(t)$ , for instance $r (t)$ $r(t)$ =10 in the experiments above. The signal $f (t)$ $f(t)$ is the vertical force which we can control by adjusting the rotor speed (thrust). The parameter $g$ $g$ is the constant of gravity and LaTeX: m $m$ is the mass. To hover at a constant height we clearly need LaTeX: f(t)=mg $f(t)=mg$ . We actually already worked out roughly how big $f$ has to be to correspond to LaTeX: mg $mg$ at the start of this assignment. So a better way to do the control in our case is to divide the control signal into a so called feed-forward and a feedback component, i.e. $LaTeX: f\left(t\right)=f_{ff}(t) + f_{fb}(t)$ $f\left(t\right)=f_{ff}(t) + f_{fb}(t)$ . The feed-forward part of the signal is an open-loop type control, which in this case would correspond to $LaTeX: f_{ff}=mg$ $f_{ff}=mg$ . The feedback part of the control, i.e. the PID-controller in our case, can now operate around something close to an equilibrium point.

Q: Without any feed-forward term, which of the three terms in the PID-controller will at steady state have a value generating a thrust close to $mg$ ?

To use feed-forward + PID you can use the last field in the rqt_reconfigure window.

Q: What is your best setting of kp,ki,kd, u_feedforward? Your considerations? Save the step response plot.

Remark about PID extra features

The basic PID controller is popular and easy to understand. To make it work properly in practice, there are however several possible issues to handle. A controller with an I-term can experience "windup problems" when an actuator hits a limitation. This can fortunately often be handled by introducing anti-windup Links to an external site.. Other issues to care about is noise filtering for the D-part, bumpless changes of reference values and parameters, as well as bumpless transitions between manual and PID control. Implementation of a well functioning PID controller can become somewhat involved, as the following figure illustrates (taken from Ch 11 in the book Feedback Systems Links to an external site.). We will not investigate any of these features here (unless you miss the deadline).

Extra task (if you miss the deadline)

Go into the code altitude_pid.py in the folder ros2_ws/src/wasp_autonomous_systems/assignment_4/assignment_4 and add anti-windup functionality. Start by limiting the magnitude of the I-part, i.e., $LaTeX: k_i\int_0^te(\tau)\,d\tau$ $k_i\int_0^te(\tau)\,d\tau$ , from becoming larger than half of the thrust needed to lift.

Submission: Include your results in the report by extending the template appropriately.

Submission

Assignment 4