diff --git a/controls/README.md b/controls/README.md index 29c4bc1e2d9d0bced898e76bbd4448fad85b9172..8a1d285f263eea8ecc25b67f4ec5d80e7ee04709 100644 --- a/controls/README.md +++ b/controls/README.md @@ -3,4 +3,5 @@ This folder contains the files used in developing the model of the quadcopter. ## Documentation -[Measuring Motor Resistance](documentation/MeasuringMotorResistance.pdf) \ No newline at end of file +[Measuring Motor Resistance](documentation/MeasuringMotorResistance.pdf) +[Simulink Model](documentation/SimulinkModel.md) \ No newline at end of file diff --git a/controls/documentation/SimulinkModel.md b/controls/documentation/SimulinkModel.md new file mode 100644 index 0000000000000000000000000000000000000000..5deb8593aef0ea67688ed9b7c1554781684efd90 --- /dev/null +++ b/controls/documentation/SimulinkModel.md @@ -0,0 +1,142 @@ +# Simulink Model + +This documentation is about the Simulink model of the quadcopter found in `controls/model/`. +Specifically, the description and images are from the file +[`Quadcopter_Model_R2015A.mdl`](../model/Quadcopter_Model_R2015_A.mdl). + +## 1 Top Level + +![Top-Level Simulink Model][top_level] + +At the top level Simulink model (shown above), there are three primary components: + +- Control System +- Actuation +- Sensors + +There is also the input of setpoints, a vector of the four setpoint values: x, y, z, and yaw. + +### 1.1 Variables +The variables used at the top level of the model are as follows: + +- $`\Theta_{\text{filtered}}`$ - The current vector of roll, pitch, and yaw +- $`\frac{d\Theta_{\text{gyro}}}{dt}`$ - Time derivative of $`\Theta`$, as gvien by the gyro +- $`^Er_0`$ - Position of the quad with respect to the static reference frame +- $`P`$ - Vector of PWM percentages sent to the ESCs +- $`^B\Omega`$ - Vector of angular velocities around $`b_x, b_y, b_z`$ +- $`\Theta`$ - Roll, pitch, and yaw in the inertal reference frame +- $`\frac{d^Bv_0}{dt}`$ - Time derivative of velocity in quadcopter reference frame +- $`^Bg`$ - Gravity in quadcopter reference frame + +The three components are further discussed in the sections below : + +## 2 Control System +![Control System Simulink Model][control_sys] + +The two central components of the Control System are the Controller and the +Signal Mixer. + +### 2.1 Variables +The variables used in the controls system not listed above are enumerated below: + +- $`u_T`$ - Controller output for thrust +- $`u_A`$ - Controller output for "ailerons", used to control pitch +- $`u_E`$ - Controller output for "elevators", used to control roll +- $`u_R`$ - Controller output for "rudder", used to control yaw + +### 2.2 Controller +The controller contains the actual PIDs for each element of the setpoint, +and outputs control values to the signal mixer. + +<!---TODO ADR make new doc or write more about controller---> + +### 2.3 Signal Mixer +The Signal Mixer is simply a $`4 \times 4`$ matrix that is multiplied by the vector +$`\begin{pmatrix}u_T & u_A & u_E & u_R\end{pmatrix}^\top`$ to produce a vector +of PWM commands for the motor. As such, each row of the matrix corresponds to one +of the rotors, while each column corresponds to one of the elements of the input vector. +```math +S = \left(\begin{array}{rrrr} +1 & -1 & -1 & -1\\ +1 & 1 & -1 & 1\\ +1 & -1 & 1 & 1\\ +1 & 1 & 1 & -1 +\end{array}\right) +``` +For example, the fourth column is $`\begin{pmatrix}-1 & 1 & 1 & -1\end{pmatrix}^\top`$, +so when the controller commands a positive rudder (i.e. positive change in yaw), +the speed of rotors 1 and 4 decrease and 2 and 3 increase. + +<!---TODO ADR The matrix is copied from MATLAB, but that example can't be right---> + +## 3 Actuation +![Actuation Simulink Model][act_sys] + +The Actuation component of the model takes the PWM percentages as inputs +and simulates the motion of the quadcopter after being given a command from the control system. + +### 3.1 Variables +The variables in Actuation that haven't previously been covered are below: + +- $`^EF_g, ^BF_g`$ - Force of gravity on the quad, in inertial and body reference frames, respectively +- $`V_{b_\text{eff}}`$ - Effective batter voltage, explained further in the ESC subsection +- $`\omega`$ - Vector of angular speeds of the motors +- $`\alpha`$ - Vector of angular accelerations of the motors +- $`L_{BE}`$ - Matrix to transform a vector from the inertial reference frame to the quad's frame +- $`L_{EB}`$ - The transpose (and thus inverse) of $`L_{BE}`$; transforms from the quad's reference frame to the inertial frame +- $`A_{EB}`$ - Matrix to transform a vector of angular velocities from the quad body reference frame to the derivative of the Euler angles + + +### 3.2 ESC System +The ESC System takes in the input vector of PWM percents, +scales them between the max and min allowable duty cycles, and then calculates +the effective battery voltage given to each motor (The motors are brushless, so effective +voltage is the DC voltage that would need to be applied to a similar brushed motor to +achieve the same output). + +### 3.3 Motor System +Given the vector of effective voltages and a vector of current angular velocities +for each motor, this block computes the angular acceleration for each. + +### 3.4 Rotor System +The Rotor System first computes the total force and torque acting on the quadcopter body. +The force consists of the thrust produced by the rotors as well as the downward +force from gravity. The torque consists of components caused by in-plane drag, +changes in rotor angular momentum, and thrusts at a distance from the center of mass. +These values are then used to compute the outputs: the linear and angular accelerations +in the quadcopter body frame of reference + +### 3.5 Other Components +There are a number of smaller components in the Actuation model. +There are several blocks that simply perform multiplication of the transformation +matrices (covered in the Variables subsection) and a block that outputs a constant +gravity vector. +In addition to this, there are several integrators that accumulate linear and +angular velocity and acceleration to accumulate current values for +velocities and position/orientation. + +## 4 Sensors +![Sensors Simulink Model][sensor_sys] + +The Sensors component of the model performs no calculations (except for some basic +trigonometry to convert gyroscope accelerometer values). +All components of this function to simulate the actual system that the +data would be sent through. +These simulation components add noise to an input value (calculated in the Actuation +phase), sample, quantize, and delay their readings. +Without these, the Simulink model would only be good to prove math, and would not +actually provide valuable insight into the physical system. + +### 4.1 Variables +There are no new quantities that haven't already been explained in a previous section. + +## 5 Theoretical Foundations +A good resource to better understand the mathematical process of this model can be found +in Matthew Rich's 2012 thesis, +[Model development, system identification, and control of a quadrotor helicopter][model_dev]. + +[top_level]: ../../documentation/images/simulink_top_level.png +[control_sys]: ../../documentation/images/simulink_control_sys.png +[act_sys]: ../../documentation/images/simulink_act_sys.png +[sensor_sys]: ../../documentation/images/simulink_sensor_sys.png +[model_dev]: http://lib.dr.iastate.edu/cgi/viewcontent.cgi?article=3777&context=etd diff --git a/controls/model/README.md b/controls/model/README.md deleted file mode 100644 index ace46416279e601fc30ce98cb975978d5194c0db..0000000000000000000000000000000000000000 --- a/controls/model/README.md +++ /dev/null @@ -1 +0,0 @@ -# Model \ No newline at end of file diff --git a/documentation/images/simulink_act_sys.png b/documentation/images/simulink_act_sys.png new file mode 100644 index 0000000000000000000000000000000000000000..0ab33008ab63759d5a46c348dd491aa4c1ba3a5e Binary files /dev/null and b/documentation/images/simulink_act_sys.png differ diff --git a/documentation/images/simulink_control_sys.png b/documentation/images/simulink_control_sys.png new file mode 100644 index 0000000000000000000000000000000000000000..a27baacab776e0378dcbf4ba8db516c97c79afd4 Binary files /dev/null and b/documentation/images/simulink_control_sys.png differ diff --git a/documentation/images/simulink_sensor_sys.png b/documentation/images/simulink_sensor_sys.png new file mode 100644 index 0000000000000000000000000000000000000000..9aee76afa3766413919aead937dac47526eb6d48 Binary files /dev/null and b/documentation/images/simulink_sensor_sys.png differ diff --git a/documentation/images/simulink_top_level.png b/documentation/images/simulink_top_level.png new file mode 100644 index 0000000000000000000000000000000000000000..be065fdef2f60666f6bb674082788e1cf4df377d Binary files /dev/null and b/documentation/images/simulink_top_level.png differ