User Tools
Differences
This shows you the differences between two versions of the page.
| Both sides previous revision Previous revision Next revision | Previous revision | ||
|
cs:control:start [2016/11/23 23:06] Ryan Summers |
cs:control:start [2018/01/28 23:05] (current) Ryan Summers |
||
|---|---|---|---|
| Line 1: | Line 1: | ||
| ====== Control System ====== | ====== Control System ====== | ||
| - | <WRAP center round important 60%> | + | The control system follows basic [[http://www.eurotherm.com/pid-control-made-easy|PID theory]], but has two additional parameters called hysteresis (H) and windup (W). The hysteresis essentially creates a deadband. If the error $E$ defined in ($\ref{eq:error}$) is below the hysteresis level, the error can be assumed as (set to) zero, and thus no corrective action is required. The windup term acts to limit the integral portion of the accumulated error. If the integral value is above the windup level, it will be truncated to the windup value. Therefore, the integral can never increase or decrease infinitely because it will always be capped by the windup. |
| - | Note: This section was determined through reverse-engineering by Ryan and is pending confirmation. | + | |
| - | </WRAP> | + | |
| - | The control system follows basic [[http://www.eurotherm.com/pid-control-made-easy|PID theory]], but has two additionally parameters called hysteresis (H) and windup (W). The hysteresis essentially creates a deadband. If the error is below the hysteresis level, the error can be assumed as zero, and thus no corrective action is required. The windup term acts to limit the integral portion of the PID controller. If the integral value is above the windup level, it will be truncated to the windup value. Therefore, the integral can never increase or decrease infinitely because it will always be capped by the windup. | + | |
| Now that we have explained the P, I, D, W, and H parameters of the control system, we're going to discuss the basic functions of the control system. Below is a simple linear algebra equation that represents our submarine's controls. | Now that we have explained the P, I, D, W, and H parameters of the control system, we're going to discuss the basic functions of the control system. Below is a simple linear algebra equation that represents our submarine's controls. | ||
| Line 13: | Line 10: | ||
| $$ | $$ | ||
| - | In ($\ref{eq:control}$), parameter $M$ is the maximum available force each thruster can apply on the submarine and parameter $C$ is the thruster control signal (Note that this parameter is normalized and within the bounds [-1,1]). By multiplying the total available force of a thruster by the command signal, we can represent the force on the submarine by the thruster as parameter $F$. | + | In ($\ref{eq:control}$), $M$ is the maximum available force each thruster can apply on the submarine and $C$ is the thruster control signal (Note that this parameter is normalized and within the bounds [-1,1]). By multiplying the total available force of a thruster by the command signal, we can represent the force on the submarine by the thruster as $F$. |
| ===== Thruster Force ===== | ===== Thruster Force ===== | ||
| - | Let's first take a closer look at parameter $M$ in ($\ref{eq:control}$). All of our thrusters are identical, which means the total force that they can output is the same. However, the position and orientation of each thruster changes how much force it can output along the X, Y, Z, and rotational axes. | + | Let's first take a closer look at $M$ in ($\ref{eq:control}$). All of our thrusters are identical, which means the total force that they can output is the same. However, the position and orientation of each thruster changes how much force it can output along the X, Y, Z, and rotational axes. |
| By breaking down forces into translational forces and rotational forces, it can be seen that each thruster will cause translational movement in the submarine in the form of moving forward, strafing, or diving and rotational movement in the form of pitching, rolling, or yawing. Therefore, it is useful to represent a thruster as a combination of all six forces. | By breaking down forces into translational forces and rotational forces, it can be seen that each thruster will cause translational movement in the submarine in the form of moving forward, strafing, or diving and rotational movement in the form of pitching, rolling, or yawing. Therefore, it is useful to represent a thruster as a combination of all six forces. | ||
| Line 30: | Line 27: | ||
| $$ | $$ | ||
| - | where $F_{x}$ is the force in the x direction (forward), $F_{y}$ is the force in the y direction (strafe), $F_{z}$ is the force in the z direction (dive), $M_{y}$ is the yaw moment (the tendency to cause the submarine to yaw), $M_{p}$ is the pitch moment, and $M_{r}$ is the roll moment. | + | where $F_{x}$ is the force in the x direction (forward), $F_{y}$ is the force in the y direction (strafe), $F_{z}$ is the force in the z direction (dive), $M_{r}$ is the roll moment (the tendency to cause the submarine to roll), $M_{p}$ is the pitch moment, and $M_{y}$ is the yaw moment. |
| - | We can therefore express the parameter $M$ in ($\ref{eq:control}$) as a matrix of each of the thruster forces and moments | + | We can therefore express $M$ in ($\ref{eq:control}$) as a matrix of each of the thruster forces and moments |
| $$ | $$ | ||
| Line 48: | Line 45: | ||
| $$ | $$ | ||
| - | where moments are calculated by multiplying position by force.$ $ | + | where moments $M_x$ are calculated by crossing the position with the orientation. |
| ===== Control Command ===== | ===== Control Command ===== | ||
| Line 63: | Line 60: | ||
| $$ | $$ | ||
| - | where $C_{x}$ represents the scale factor for thruster x. Take note that conceptually, it is easy to differentiate translational and rotational goals | + | where $C_{x}$ represents the scale factor for thruster x. Take note that conceptually, it is easy to separate translational from rotational goals |
| $$ | $$ | ||
| Line 75: | Line 72: | ||
| ===== Total Force ===== | ===== Total Force ===== | ||
| - | By substituting ($\ref{eq:control:M}$) and ($\ref{eq:control:C}$) into ($\ref{eq:control}$), it can be seen that | + | $F$ in ($\ref{eq:control}$) is the necessary force to apply to the submarine to cause a transition from the current state to a desired state. By substituting ($\ref{eq:control:M}$) and ($\ref{eq:control:C}$) into ($\ref{eq:control}$), it can be seen that the dimensions of F are also known |
| $$ | $$ | ||
| Line 98: | Line 95: | ||
| $$ | $$ | ||
| - | Therefore, $F$ is also a vector with 6 elements in it. Each element of $F$ is the summation of each of the rows of $M$ multiplied by the weights in $C$. | + | Therefore, $F$ is also a vector with 6 elements in it. Each element of $F$ is the summation of each of the rows of $M$ multiplied by the weights in $C$. Because the mass and inertial moments of the submarine are also known, $F$ can also be calculated utilizing the current and desired state. |
| ===== Solving the Equation ===== | ===== Solving the Equation ===== | ||
| - | Now that all of the parameters of ($\ref{eq:control}$) have been defined, we can use it to find our control. Notice that there are two knowns within the system, both $M$ and $F$ can be analytically determined. $M$ can be found by measuring the position and orientation of each thruster. $F$ results from the desired position of the sub. To determine $F$, P, I, D, W, H, and Error need to be known. Let | + | Now that all of the parameters of ($\ref{eq:control}$) have been defined, we can use it to find our control. Notice that there are two knowns within the system, both $M$ and $F$. $M$ can be found by measuring the position and orientation of each thruster. $F$ results from the desired position of the sub. To determine $F$, P, I, D, W, H, and Error need to be known. Let |
| $$ | $$ | ||
| Line 120: | Line 117: | ||
| $$ | $$ | ||
| - | where $P_n$ is the value for the current position and R_n is the value for the current rotation. Then $F$ can be found by | + | where $P_n$ is the value for the current position and $R_n$ is the value for the current rotation. Then $F$ can be found by |
| $$ | $$ | ||
| - | F = K_p E + K_i I + K_d S' | + | F = K_p E + K_i I + K_d E' |
| $$ | $$ | ||
| - | after both windup and hysteresis are applied to Error. Note that $S'$ is the derivative of $S$, $I$ is the integral state for each of the states within $S$, and $K_n$ are the respective P, I, and D weights. | + | after both windup and hysteresis are applied to $E$. Note that $E'$ is the derivative of $E$ over time, $I$ is the integral state for the accumulated error for each of the states within $E$, and $K_x$ are the respective P, I, and D weights. |
| Because both $F$ and $M$ are known, ($\ref{eq:control}$) can be solved for $C$ | Because both $F$ and $M$ are known, ($\ref{eq:control}$) can be solved for $C$ | ||
| Line 138: | Line 135: | ||
| Therefore, to solve for the control messages to send to the thruster, the inverted matrix $M^{-1}$ can be multiplied into the variable parameter $F$. The control packet $C$ can then be sent to the thruster controller to result in the desired movement and the cycle can be completed. | Therefore, to solve for the control messages to send to the thruster, the inverted matrix $M^{-1}$ can be multiplied into the variable parameter $F$. The control packet $C$ can then be sent to the thruster controller to result in the desired movement and the cycle can be completed. | ||
| - | Note that $M$ must be an invertible matrix. If $M$ has fewer than 6 thrusters, the solution is not solvable because the submarine does not have control of 6 axes (it has less than 6 motors, so it can therefore not have control of 6 axes). If $M$ has greater than 6 thrusters, than the system is over-actuated and a [[https://en.wikipedia.org/wiki/Moore%E2%80%93Penrose_pseudoinverse|pseudo-inverse]] calculation can be done to determine $M^{-1}$. | + | Note that $M$ must be an invertible matrix. If $M$ has fewer than 6 thrusters, the solution is not solvable because the submarine does not have control of 6 axes (it has less than 6 motors, so it can therefore not have control of 6 axes). If $M$ has greater than 6 thrusters, then the system is over-actuated and a [[https://en.wikipedia.org/wiki/Singular_value_decomposition|SVD pseudo-inverse]] calculation can be done to determine $M^{-1}$. |
| Also note that the following simplification can be made to solve for $C_r$ and $C_t$ from ($\ref{eq:c:simplification}$) by setting $F_a$ or $M_a$ to zero in $F$ | Also note that the following simplification can be made to solve for $C_r$ and $C_t$ from ($\ref{eq:c:simplification}$) by setting $F_a$ or $M_a$ to zero in $F$ | ||
| Line 161: | Line 158: | ||
| ====== Control System Notes====== | ====== Control System Notes====== | ||
| - | 1. When tuning pitch and roll tune PID params while the sub is moving. This is because we only currently care about keeping the sub level while moving. | + | 1. When tuning pitch and roll PID params, tune them just to keep the sub stable when moving forward/strafing. This is because we only currently care about keeping the sub level while moving, don't really care as much about overshoot when moving between different pitch/roll angles. |
