YANE-Framework Tutorial 1.1.0

Mathematical Setting of the Example

The "Inverted Pendulum on a Cart" problem is given by the differential equation

\begin{eqnarray*} \dot{x}_1(t) & = & x_2(t) \\ \dot{x}_2(t) & = & -\frac{g}{l} \sin(x_1(t)) - \frac{k_L}{l} x_2(t) | x_2(t) | - u(t) \cos(x_1(t)) - k_R \mbox{sgn}(x_2(t)) \\ \dot{x}_3(t) & = & x_4(t) \\ \dot{x}_4(t) & = & u(t) \end{eqnarray*}

with parameters $ g = 9.81 $, $ l = 1.25 $, $ k_L = 0.007 $ and $ k_R = 0.197 $. Here, $ x_1(\cdot) $ denotes the angle of the pendulum, $ x_2(\cdot) $ the angular velocity of the pendulum, $ x_3(\cdot) $ the position and $ x_4(\cdot) $ the velocity of the cart. Within this system the second equation is numerically problematic for an adaptive differential equation solver:

\[ \dot{x}_2(t) = -\frac{g}{l}\sin(x_1(t)) - \underbrace{\frac{k_L}{l} x_2(t) | x_2(t) |}_{\mbox{non diff'able}} - \underbrace{u(t) \cos(x_1(t))}_{\mbox{measurable}} - \underbrace{k_R \mbox{sgn}(x_2(t))}_{\mbox{discontinuous}}. \]

Hence, this differential equation causes a reduction of the order of consistence of the differential equation solver to $ \mathcal{O}(n) $ at points where the right hand side is not differentiable. Moreover, stability of the solver is lost at the discontinuity points.

Since we are going to use a receding horizon controller, i.e. a sample--and--hold strategy for the contol $ u(\cdot) $, we do not have to modify the term concerning the control since within the receding horizon controller setting the points of discontinuity are known in advance and can be added to the discretization grid. Hence, the order of consistence is preserved although $ u(\cdot) $ is discontinuous.

For both other terms we use smooth approximations which allow us to mimic the behaviour of the original equation but retain the oder of consistence of the differential equation solver:

\begin{eqnarray*} \dot{x}_1(t) & = & x_2(t) \\ \dot{x}_2(t) & = & -\frac{g}{l}\sin(x_1(t)) - \frac{k_L}{l} \arctan(1000 x_2(t)) x_2^2(t) - u(t) \cos(x_1(t)) \nonumber \\ & & \qquad - k_R \left(\frac{4 x_2(t)}{1 + 4 (x_2(t))^2} + \frac{2 \arctan(2 x_2(t))}{\pi}\right) \\ \dot{x}_3(t) & = & x_4(t) \\ \dot{x}_4(t) & = & u(t). \end{eqnarray*}

The dynamic of the system is implemented in the method InvertedPendulum::odeFunction.

The goal for this example is to stabilize the pendulum in the upright position using a NMPC control law. The upright unstable equilibria are given by the set $ \mathcal{S} = \{((k + 1) \pi, 0, 0, 0)^\top \, | \; k \in 2 \mathbf{Z} \} $. To develop a cost functional we make use of the geometry of the dynamic of the system. To stabilize an arbitrary point of this set, we consider the cost functional

\[ J_N(x_0, u) := 0.1 \sum\limits_{i = 0}^{N - 1} l(x(i), u(i)) + F(x(N)) \]

where the stage costs are given by

\begin{eqnarray*} l(x(i), u(i)) & := & 0.1 \int\limits_{t_i}^{t_{i+1}} \Big( 3.51 \sin(x_1(t) - \pi)^2 + 4.82 \sin(x_1(t) - \pi) x_2(t) + 2.31 x_2(t)^2 \\ & & \quad + 2 \left( (1 - \cos(x_1(t) - \pi)) \cdot (1 + \cos(x_2(t))^2) \right)^2 + x_3(t)^2 + x_4(t)^2 \Big)^2 dt \end{eqnarray*}

and the terminal cost is defined via

\begin{eqnarray*} F(x(N)) & := & \Big( 3.51 \sin(x_1(t_N) - \pi)^2 + 4.82 \sin(x_1(t_N) - \pi) x_2(t_N) + 2.31 x_2(t_N)^2 \\ & & \quad + 2 \left( (1 - \cos(x_1(t_N) - \pi)) \cdot (1 + \cos(x_2(t_N))^2) \right)^2 + x_3(t_N)^2 + x_4(t_N)^2 \Big)^2. \end{eqnarray*}

Within the implementation, you will find these functions in the methods InvertedPendulum::objectiveFunction, InvertedPendulum::pointcostFunction and InvertedPendulum::objectiveWeight.

Moreover, we impose constraints on the state and the control vectors by defining

\begin{eqnarray*} \mathbf{X} & := & \mathbf{R} \times \mathbf{R} \times [ -5, 5] \times [-10, 10] \\ \mathbf{U} & := & [-5, 5] \end{eqnarray*}

which can be found in the constructor method InvertedPendulum::InvertedPendulum.

Note that every point in the set $ \mathcal{S} $ is treated equally by this cost functional. Hence, the outcome of the receding horizon control problem is depending on the initial value $ x_0 $ of the system, but also on the optimization horizon $ N $. While the first dependency is due to the periodicity of the cost functional, the latter one follows from possible changes of the optimum if different horizon lengths are considered.

Authors: