Inverted Pendulum

We consider the mechanical model of an inverted rigid pendulum mounted on a carriage, see [Grune21], [GruneP17].

By means of physical laws an “exact” differential equation model can be derived. However, since in our case we like to obtain a linear quadratic problem, we linearize the differential equation at the origin. Thus, we obtain the system dynamics defined by

\[\begin{split}\dot{x}(t) = \left(\begin{array}{cccc} 0 & 1 & 0 & 0 \\ g & -k & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{array}\right) x(t) + \left(\begin{array}{c} 0 \\ 1 \\ 0 \\ 1 \end{array}\right) u(t).\end{split}\]

Here, the state vector \(x \in \mathbb{R}^4\) consists of 4 components. \(x_1\) corresponds to the angle \(\psi\) of the pendulum, which increases counterclockwise, where \(x_1 = 0\) corresponds to the upright pendulum. \(x_2\) is the angular velocity, \(x_3\) the position of the carriage and \(x_4\) its velocity. The control \(u\) is the acceleration of the carriage. The constant \(k=0.1\) describes the friction of the pendulum and the constant \(g \approx 9.81 m/s^2\) is the acceleration due to gravity.

Since the system dynamics are linear, we can initialize them using the LQP method.

g = 9.81
k = 0.1
A = nmpyc.array([[0, 1, 0, 0],
                 [g, -k, 0, 0],
                 [0, 0, 0, 1],
                 [0, 0, 0, 0]])
B = nmpyc.array([0, 1, 0, 1])
system = nmpyc.system.LQP(A, B, 4, 1, 'continuous',
                          sampling_rate=0.1, method='rk4')

Note that we have to use one of the fixed step methods as euler, heun or rk4 as integration method if we like to exploit the linear quadratic structure of the problem in the optimization.

In the next step, we have to define the objective of the optimal control problem. In doing so, we assume the stage cost

\[\ell(x,u) = 2x^Tx + 4u^Tu.\]

Since we assume no terminal cost, we can implement the objective as shown in the following code snippet.

Q = 2*nmpyc.eye(4)
R = 4*nmpyc.eye(1)
objective = nmpyc.objective.LQP(Q, R)

Again, we use the LQP method to exploit the linear quadratic structure of the problem later.

In terms of the constraints we assume the state constraints

\[-9 \leq x_i(t) \leq 5\]

for \(i=1,\ldots,4\) and the control constraint

\[-20 \leq u(t) \leq 6 \quad\]

This can be realized in the code as

constraints = nmpyc.constraints()
lbx = nmpyc.zeros(4)*(-9)
ubx = nmpyc.ones(4)*5
constraints.add_bound('lower','state', lbx)
constraints.add_bound('upper','state', ubx)
constraints.add_bound('lower', 'control', nmpyc.array([-20]))
constraints.add_bound('upper', 'control', nmpyc.array([6]))

After all components of the optimal control problem have been implemented, we can now combine them into a model and start the MPC loop. For this Purpose, we define the inital value

\[x(0) = (1,1,1,1)^T\]

and set \(N=20\), \(K=100\).

model = nmpyc.model(objective,system,constraints)
x0 = nmpyc.array([1, 1, 1, 1])
res = model.mpc(x0,20,100)

Since the problem is linear-quadratic, the program automatically takes advantage of this fact and uses the appropriate solver osqp. To change this and use for example the SciPy solver SLSQP, we can use the set_options method before calling model.mpc().

model.opti.set_options(dict(solver='SLSQP'))

Note that changing the optimizer usually does not have any advantage and is therefore not necessarily recommended. At this point we only like to demomnstrate the use of this function.

Following the simulation we can visualize the open and closed loop results by calling

res.plot() # plot closed loop results
res.plot('state', show_ol=True) # plot open loop states

which generates the plots bellow.