Double Inverted Pendulum on a Cart

Example

DoubleInvertedPendulum/

Where you can find this system

A. Bogdanov, “Optimal control of a double inverted pendulum on a cart,” Oregon Health and Science University, Tech. Rep. CSE-04-006, OGI School of Science and Engineering, Beaverton, OR, 2004.

Problem Description

Swing-up control of a double inverted pendulum on a cart is a benchmark problem for NMPC algorithms due to its high nonlinearity. The pendulum we want to swing up is:

• The state vector of the system is $x = [\theta_0,\dot{\theta}_0 , \theta_1 ,\dot{\theta}_1 ,\theta_2 ,\dot{\theta}_2]^{T}$, where $\theta_0$ is the displacement of the cart, and where $\theta_1$, $\theta_2$ are the pendulum angles. The displacement $\theta_0$ is constrained with $|\theta_0|\leq 0.2$ m

• The control force $u$ is constrained with $|u|\leq 15$.

• The system is modeled as $D(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+G(\theta)=Hu$ with $\theta=[\theta_0,\theta_1,\theta_2]^T$.

• A terminal penalty function is imposed to swing up the pendulum.

The task is to swing up the pendulum from the initial state $x_0 = [0,\pi,\pi,0,0,0]^{T}$.

OCP in ParNMPC

The inequality constraints on the input $u$ are handled using the logarithmic barrier functions, and the inequality constraints on $\theta_0$ are softened using the KS functions.

The underlying OCP defined in ParNMPC is formulated as:

• State: $x=[\theta_0,\dot{\theta}_0, \theta_1 ,\dot{\theta}_1 ,\theta_2 ,\dot{\theta}_2]^T=[\theta,\dot{\theta}]^T$.
• Input: $u$.
• Parameter: $p=[Q_d,R_d,\gamma_u,\gamma_{\theta_0},\beta_{\theta_0}]$.

• $L(u,x,p) = L_s(u,x,p) + L_{u}(u,x,p) + L_{\theta_0}(u,x,p)$, where $L_s(u,x,p) = \frac{1}{2}\|x\|_{Q}^2+\frac{1}{2}\|u\|_{R}^2$ with $Q=\text{diag}(Q_d)$ and $R=\text{diag}(R_d)$, $L_u(u,x,p) = -\gamma_u\text{log}(1-u)-\gamma_u\text{log}(u)$, and $L_{\theta_0}(u,x,p) = \beta_{\theta_0}\text{log}(1+e^{\frac{1}{\gamma_{\theta_0}}(\theta_0-0.19)}) + \beta_{\theta_0}\text{log}(1+e^{\frac{1}{\gamma_{\theta_0}}(-0.19-\theta_0)})$.

• $f(u,x,p)=[\dot{\theta};Hu-G(\theta)-C(\theta,\dot{\theta})\dot{\theta}]$.

• $M(u,x,p) = \text{blkdiag}(eye(3),D(\theta))$.

• Prediction horizon $T=1.5$.

• Number of the discritization grids $N=48$.
• Discretization method: Euler in reverse time (backward Euler).

Closed-loop Simulation using ParNMPC

Step 1. NMPC problem formulation

See Workflow of ParNMPC > NMPC Problem Formulation.

Example

DoubleInvertedPendulum/NMPC_Problem_Definition.m

Step 2. Code generation and deployment in Simulink

See Workflow of ParNMPC > Code Generation and Deployment > Simulink.

1. Code generation

   Example

   DoubleInvertedPendulum/Simu_Simulink_Setup.m

2. Deployment

   Example

   DoubleInvertedPendulum/Simu_Simulink.slx

Step 2. Speed up the closed-loop simulation in Matlab using mex

1. Code generation

   Example

   DoubleInvertedPendulum/Simu_Simulink_Setup.m

   Modify the generation target to mex:

   NMPC_Iter_CodeGen('mex','C',args_NMPC_Iter);
   

   and run.

2. Deployment

   Example

   DoubleInvertedPendulum/Simu_Matlab.m

   Modify NMPC_Iter to NMPC_Iter_mex to call the generated mex function:

   [lambdaSplit,muSplit,uSplit,xSplit,...
    LAMBDASplit,cost,error,timeElapsed] =
    NMPC_Iter_mex(x0,lambdaSplit,muSplit,...
                  uSplit,xSplit,pSplit,...
                  LAMBDASplit,discretizationMethod,...
                  isMEnabled);
   

   and run.