Control

A simple control problem on a system usually involves a variable $x(t)$ that denotes the state of the system over time, and a variable $u(t)$ that denotes the input into the system over time. Linear constraints are used to capture the evolution of the system over time:

$$x(t) = Ax(t - 1) + Bu(t), \ \mbox{for} \ t = 1,\ldots, T,$$

where the numerical matrices $A$ and $B$ are called the dynamics and input matrices, respectively.

The goal of the control problem is to find a sequence of inputs $u(t)$ that will allow the state $x(t)$ to achieve specified values at certain times. For example, we can specify initial and final states of the system:

$$ \begin{align*} x(0) &= x_i \\ x(T) &= x_f \end{align*} $$

Additional states between the initial and final states can also be specified. These are known as waypoint constraints. Often, the input and state of the system will have physical meaning, so we often want to find a sequence inputs that also minimizes a least squares objective like the following:

$$ \sum_{t = 0}^T \|Fx(t)\|^2_2 + \sum_{t = 1}^T\|Gu(t)\|^2_2, $$

where $F$ and $G$ are numerical matrices.

We'll now apply the basic format of the control problem to an example of controlling the motion of an object in a fluid over $T$ intervals, each of $h$ seconds. The state of the system at time interval $t$ will be given by the position and the velocity of the object, denoted $p(t)$ and $v(t)$, while the input will be forces applied to the object, denoted by $f(t)$. By the basic laws of physics, the relationship between force, velocity, and position must satisfy:

$$ \begin{align*} p(t+1) &= p(t) + h v(t) \\ v(t+1) &= v(t) + h a(t) \end{align*}. $$

Here, $a(t)$ denotes the acceleration at time $t$, for which we we use $a(t) = f(t) / m + g - d v(t)$, where $m$, $d$, $g$ are constants for the mass of the object, the drag coefficient of the fluid, and the acceleration from gravity, respectively.

Additionally, we have our initial/final position/velocity conditions:

$$ \begin{align*} p(1) &= p_i\\ v(1) &= v_i\\ p(T+1) &= p_f\\ v(T+1) &= 0 \end{align*} $$

One reasonable objective to minimize would be

$$ \mbox{objective} = \mu \sum_{t = 1}^{T+1} (v(t))^2 + \sum_{t = 1}^T (f(t))^2 $$

We would like to keep both the forces small to perhaps save fuel, and keep the velocities small for safety concerns. Here $\mu$ serves as a parameter to control which part of the objective we deem more important, keeping the velocity small or keeping the force small.

The following code builds and solves our control example:

In [9]:
using Convex, SCS, Gadfly

# Some constraints on our motion
# The object should start from the origin, and end at rest
initial_velocity = [-20; 100]
final_position = [100; 100]

T = 100 # The number of timesteps
h = 0.1 # The time between time intervals
mass = 1 # Mass of object
drag = 0.1 # Drag on object
g = [0, -9.8] # Gravity on object

# Declare the variables we need
position = Variable(2, T)
velocity = Variable(2, T)
force = Variable(2, T - 1)

# Create a problem instance
mu = 1
constraints = []

# Add constraints on our variables
for i in 1 : T - 1
  constraints += position[:, i + 1] == position[:, i] + h * velocity[:, i]
end

for i in 1 : T - 1
  acceleration = force[:, i]/mass + g - drag * velocity[:, i]
  constraints += velocity[:, i + 1] == velocity[:, i] + h * acceleration
end

# Add position constraints
constraints += position[:, 1] == 0
constraints += position[:, T] == final_position

# Add velocity constraints
constraints += velocity[:, 1] == initial_velocity
constraints += velocity[:, T] == 0

# Solve the problem
problem = minimize(sumsquares(force), constraints)
solve!(problem, SCSSolver(verbose=0))

We can plot the trajectory taken by the object. The blue point denotes the initial position, and the green point denotes the final position.

In [10]:
pos = evaluate(position)
p = plot(
  layer(x=[pos[1, 1]], y=[pos[2, 1]], Geom.point, Theme(default_color=color("blue"))),
  layer(x=[pos[1, T]], y=[pos[2, T]], Geom.point, Theme(default_color=color("green"))),
  layer(x=pos[1, :], y=pos[2, :], Geom.line(preserve_order=true)),
  Theme(panel_fill=color("white"))
)
Out[10]:
x -300 -250 -200 -150 -100 -50 0 50 100 150 200 250 300 350 400 -250 -240 -230 -220 -210 -200 -190 -180 -170 -160 -150 -140 -130 -120 -110 -100 -90 -80 -70 -60 -50 -40 -30 -20 -10 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170 180 190 200 210 220 230 240 250 260 270 280 290 300 310 320 330 340 350 -400 -200 0 200 400 -250 -200 -150 -100 -50 0 50 100 150 200 250 300 350 y -250 -200 -150 -100 -50 0 50 100 150 200 250 300 350 400 450 -200 -190 -180 -170 -160 -150 -140 -130 -120 -110 -100 -90 -80 -70 -60 -50 -40 -30 -20 -10 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170 180 190 200 210 220 230 240 250 260 270 280 290 300 310 320 330 340 350 360 370 380 390 400 -200 0 200 400 -200 -180 -160 -140 -120 -100 -80 -60 -40 -20 0 20 40 60 80 100 120 140 160 180 200 220 240 260 280 300 320 340 360 380 400

We can also see how the magnitude of the force changes over time.

In [11]:
p = plot(x=1:T, y=sum(evaluate(force).^2, 1), Geom.line, Theme(panel_fill=color("white")))
Out[11]:
x -150 -100 -50 0 50 100 150 200 250 -100 -95 -90 -85 -80 -75 -70 -65 -60 -55 -50 -45 -40 -35 -30 -25 -20 -15 -10 -5 0 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75 80 85 90 95 100 105 110 115 120 125 130 135 140 145 150 155 160 165 170 175 180 185 190 195 200 -100 0 100 200 -100 -90 -80 -70 -60 -50 -40 -30 -20 -10 0 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170 180 190 200 y -2000 -1500 -1000 -500 0 500 1000 1500 2000 2500 3000 3500 -1500 -1450 -1400 -1350 -1300 -1250 -1200 -1150 -1100 -1050 -1000 -950 -900 -850 -800 -750 -700 -650 -600 -550 -500 -450 -400 -350 -300 -250 -200 -150 -100 -50 0 50 100 150 200 250 300 350 400 450 500 550 600 650 700 750 800 850 900 950 1000 1050 1100 1150 1200 1250 1300 1350 1400 1450 1500 1550 1600 1650 1700 1750 1800 1850 1900 1950 2000 2050 2100 2150 2200 2250 2300 2350 2400 2450 2500 2550 2600 2650 2700 2750 2800 2850 2900 2950 3000 -2000 0 2000 4000 -1500 -1400 -1300 -1200 -1100 -1000 -900 -800 -700 -600 -500 -400 -300 -200 -100 0 100 200 300 400 500 600 700 800 900 1000 1100 1200 1300 1400 1500 1600 1700 1800 1900 2000 2100 2200 2300 2400 2500 2600 2700 2800 2900 3000