Position Control [PID dev] - clems4ever/ardrone_loclib GitHub Wiki

This page will attempt to present the PID development procedure to help you improve it.

Motivation

The drone have to moves. The best way to do it is via a controller. We choose a PID as it's a simple controller. The PID will check every period (1/200s, 200Hz) the error in position of the drone (target - position estimation) and apply a command to the drone to reduce this error.

Brief theory

The command applied by a PID is the sum of 3 parts:

  • Proportional: the more we are far from the target, the more important the command will be;
  • Integral: this term makes the command proportional to both the magnitude and duration of the error;
  • Derivate: predicts system behavior.

To each term, we associate a gain to tune its influence.

Matlab files

To develop the PID and tune it, we used Matlab with Simulink you can find the files on the repo. Simulink can generate code from models, so the code we run to control the drone is partially generated by Matlab from the model in the these files. (the generated code is in the folder Pos_Controller in the package ardrone_moves)

##Control Algorithm Principle We will present the development procedure to explain how the controller works. The objective is to develop a controller for each parameter:

  • x (front/back)
  • y (left/right)
  • z (altitude)
  • orientation

Position xyz

The first step was to create Proportional controllers for x, y and z to control the position (no orientation). As the behavior of the drone is very similar over x and y (front/back, left/right), we used identical parameters for these two components. The AR.drone driver we use take as inputs (commands) normalized x,y and z speed. (which are in fact roll, pitch and gaz commands). So the input of the P blocks are the errors on each components (target - position) and the outputs are − resp. for x y and z − roll, pitch and gaz. The PID controllers parameters were tuned first in Simulink, using a model of the drone and the "autotune" functionality of the simulink PID block, and then tested and re-tuned on a simulated drone in ROS.

Drone orientation

Now, problem is that the speed command given to the drone are relative to it, while the position estimation and target are absolute (or relative to the world, if you prefer). We have to change the coordinates of the command speeds. The formula is:

Vxd=Vx.cos(phi) + Vy.sin(phi)
Vyd=Vy.cos(phi) - Vx.sin(phi)

where V?d are speed x/y components in the drone coordinates and phi is the angle between x and xd (frame axis of the world and drone resp.).

Orientation Control

We just make a PID in a similar way for the yaw command. We added a feature to enable the yaw command to be relative to the direction. i.e. if we want the drone to fly forward, we enable this feature and set the desired yaw to 0, to fly backward, we set it to pi...

Result in Matlab

simulink screenshot of controller block

In the above image, which is a screenshot from Simulink of the controller, we can identify:

  • the sum block that generates error from desired (consigne) and estimated (position) positions
  • the demux to separate components of the error position vector (x, y, z)
  • the 3 PID over x y and z
  • the reference change block that acts on x and y to transform the absolute (xy) speeds into (xy) speed in the drone reference
  • the multiplexor that concatenates speed commands into a vector
  • the block that computes the angle of the (xy) speed command vector (angle between the world x axis and the speed command projected onto z=0 plane)
  • the if block to switch between relative and absolute yaw commands
  • on the last line, the 2 sum blocks that generate yaw error from yaw consigne, yaw (estimated) and speed angle
  • the modulo, to have a result in [-pi;+pi]
  • the yaw PID

Dimensioning the PIDs

The final PID for the x and y terms have been simply reduced to Proportional correctors. The justification of this choice is that the accuracy of our state estimation system is too poor and does not enable a precise positioning. The choice of the Kp parameter was simply made with this objective: when the drone is at less than 1 meter from the target, it slows down; otherwise it's at its maximum speed. As the speed command is normalized between 1 and -1, Kp=1.