|
| 1 | +# # Control |
| 2 | + |
| 3 | +#- |
| 4 | + |
| 5 | +# A simple control problem on a system usually involves a variable $x(t)$ |
| 6 | +# that denotes the state of the system over time, and a variable $u(t)$ that |
| 7 | +# denotes the input into the system over time. Linear constraints are used to |
| 8 | +# capture the evolution of the system over time: |
| 9 | +# |
| 10 | +# $$ |
| 11 | +# x(t) = Ax(t - 1) + Bu(t), \ \text{for} \ t = 1,\ldots, T, |
| 12 | +# $$ |
| 13 | +# |
| 14 | +# where the numerical matrices $A$ and $B$ are called the dynamics and input matrices, |
| 15 | +# respectively. |
| 16 | +# |
| 17 | +# The goal of the control problem is to find a sequence of inputs |
| 18 | +# $u(t)$ that will allow the state $x(t)$ to achieve specified values |
| 19 | +# at certain times. For example, we can specify initial and final states of the system: |
| 20 | +# |
| 21 | +# $$ |
| 22 | +# \begin{aligned} |
| 23 | +# x(0) &= x_i \\ |
| 24 | +# x(T) &= x_f |
| 25 | +# \end{aligned} |
| 26 | +# $$ |
| 27 | +# |
| 28 | +# Additional states between the initial and final states can also be specified. These |
| 29 | +# are known as waypoint constraints. Often, the input and state of the system will |
| 30 | +# have physical meaning, so we often want to find a sequence inputs that also |
| 31 | +# minimizes a least squares objective like the following: |
| 32 | +# |
| 33 | +# $$ |
| 34 | +# \sum_{t = 0}^T \|Fx(t)\|^2_2 + \sum_{t = 1}^T\|Gu(t)\|^2_2, |
| 35 | +# $$ |
| 36 | +# |
| 37 | +# where $F$ and $G$ are numerical matrices. |
| 38 | +# |
| 39 | +# We'll now apply the basic format of the control problem to an example of controlling |
| 40 | +# the motion of an object in a fluid over $T$ intervals, each of $h$ seconds. |
| 41 | +# The state of the system at time interval $t$ will be given by the position and the velocity of the |
| 42 | +# object, denoted $p(t)$ and $v(t)$, while the input will be forces |
| 43 | +# applied to the object, denoted by $f(t)$. |
| 44 | +# By the basic laws of physics, the relationship between force, velocity, and position |
| 45 | +# must satisfy: |
| 46 | +# |
| 47 | +# $$ |
| 48 | +# \begin{aligned} |
| 49 | +# p(t+1) &= p(t) + h v(t) \\ |
| 50 | +# v(t+1) &= v(t) + h a(t) |
| 51 | +# \end{aligned}. |
| 52 | +# $$ |
| 53 | +# |
| 54 | +# Here, $a(t)$ denotes the acceleration at time $t$, for which we we use |
| 55 | +# $a(t) = f(t) / m + g - d v(t)$, |
| 56 | +# where $m$, $d$, $g$ are constants for the mass of the object, the drag |
| 57 | +# coefficient of the fluid, and the acceleration from gravity, respectively. |
| 58 | +# |
| 59 | +# Additionally, we have our initial/final position/velocity conditions: |
| 60 | +# |
| 61 | +# $$ |
| 62 | +# \begin{aligned} |
| 63 | +# p(1) &= p_i\\ |
| 64 | +# v(1) &= v_i\\ |
| 65 | +# p(T+1) &= p_f\\ |
| 66 | +# v(T+1) &= 0 |
| 67 | +# \end{aligned} |
| 68 | +# $$ |
| 69 | +# |
| 70 | +# One reasonable objective to minimize would be |
| 71 | +# |
| 72 | +# $$ |
| 73 | +# \text{objective} = \mu \sum_{t = 1}^{T+1} (v(t))^2 + \sum_{t = 1}^T (f(t))^2 |
| 74 | +# $$ |
| 75 | +# |
| 76 | +# We would like to keep both the forces small to perhaps save fuel, and keep |
| 77 | +# the velocities small for safety concerns. |
| 78 | +# Here $\mu$ serves as a parameter to control which part of the objective we |
| 79 | +# deem more important, keeping the velocity small or keeping the force small. |
| 80 | +# |
| 81 | +# The following code builds and solves our control example: |
| 82 | + |
| 83 | + |
| 84 | +using Convex, SCS, Plots |
| 85 | + |
| 86 | +## Some constraints on our motion |
| 87 | +## The object should start from the origin, and end at rest |
| 88 | +initial_velocity = [-20; 100] |
| 89 | +final_position = [100; 100] |
| 90 | + |
| 91 | +T = 100 # The number of timesteps |
| 92 | +h = 0.1 # The time between time intervals |
| 93 | +mass = 1 # Mass of object |
| 94 | +drag = 0.1 # Drag on object |
| 95 | +g = [0, -9.8] # Gravity on object |
| 96 | + |
| 97 | +## Declare the variables we need |
| 98 | +position = Variable(2, T) |
| 99 | +velocity = Variable(2, T) |
| 100 | +force = Variable(2, T - 1) |
| 101 | + |
| 102 | +## Create a problem instance |
| 103 | +mu = 1 |
| 104 | + |
| 105 | +## Add constraints on our variables |
| 106 | +constraints = Constraint[ position[:, i + 1] == position[:, i] + h * velocity[:, i] for i in 1 : T - 1] |
| 107 | + |
| 108 | + |
| 109 | +for i in 1 : T - 1 |
| 110 | + acceleration = force[:, i]/mass + g - drag * velocity[:, i] |
| 111 | + push!(constraints, velocity[:, i + 1] == velocity[:, i] + h * acceleration) |
| 112 | +end |
| 113 | + |
| 114 | +## Add position constraints |
| 115 | +push!(constraints, position[:, 1] == 0) |
| 116 | +push!(constraints, position[:, T] == final_position) |
| 117 | + |
| 118 | +## Add velocity constraints |
| 119 | +push!(constraints, velocity[:, 1] == initial_velocity) |
| 120 | +push!(constraints, velocity[:, T] == 0) |
| 121 | + |
| 122 | +## Solve the problem |
| 123 | +problem = minimize(sumsquares(force), constraints) |
| 124 | +solve!(problem, SCSSolver(verbose=0)) |
| 125 | + |
| 126 | +# We can plot the trajectory taken by the object. |
| 127 | + |
| 128 | +pos = evaluate(position) |
| 129 | +plot([pos[1, 1]], [pos[2, 1]], st=:scatter, label="initial point") |
| 130 | +plot!([pos[1, T]], [pos[2, T]], st=:scatter, label="final point") |
| 131 | +plot!(pos[1, :], pos[2, :], label="trajectory") |
| 132 | + |
| 133 | +# We can also see how the magnitude of the force changes over time. |
| 134 | + |
| 135 | +plot(vec(sum(evaluate(force).^2, dims=1)), label="force (magnitude)") |
0 commit comments