- Model
- Simulator
- Results

The **Gymnastic humanoid robot** can be seen as being compound of three main blocks. One block
representing the arms, a second block representing the torso and a third block representing the
legs. The joints of the robot are therefore the hands, the shoulders and the hips. Both shoulders
and hips are actuated by two servos in each side. The hands are not actuated indeed, and therefore
the system can be approximated by a three link underactuated pendulum.

Equations of Motion

In order to describe the dynamic behavior of this multi-body robotic system, it is necessary to de
termine its equations of motion. These equations can be derived from the classical Euler-Lagrange
equations.

where

- q_i is the angle of joint i in respect to the previous link.
- m_i is the mass of block i.
- I_i is the inertia of block i.
- tal_i is the torque actuated on the active joint i.
- l_i is the length between joint i and joint i+1.
- lc_i is the length between joint i and the center of gravity of the mass i.

Deduction of Euler-Lagrange equations:

where x_i and y_i are the coordinates of the center of gravity of mass i.

Kinetic energy (T):

Potential energy (U):

Lagrangian of the system (L):

Solving the Lagrange equations for each element of the system, and linearthe following equations of motion result:

where

- m_ij are the inertial terms.
- phi_i are the gravitational terms.
- h_i are the Corriolis and the centrifugal terms.
- tal_i are the input torques.

The above equations of motion of the system are highly nonlinear, but since the goal of the controller is to stabilize the humanoid at the vertical unstable equilibrium, the above system can linearized at the vertical unstable position, i.e.:

Hence, using a ﬁrst order Taylor’s expansion, results the following linearized system for the vertical unstable equilibrium.

where,

Continuous state space model

Given the above linearization of a generalized n-link pendulum system, the state space model is
easily obtained.

state vector:

input vector:

Space model:

where

**For a more detailed and extended explanation, please check master_thesis.pdf, where a solution is described for a generic N-pendulum system.**

Using Simulink and SimMechanics ut was possible to simulate and control the physical behavior of the
humanoid while doing a handstand on a high bar. The next figure shows a Linear Quadratic Regulator (LQR) controler, while using as plant, a SimMechanics model mimic the real one, with all the physical properties set.

Linear Quadratic Regulator:

This control technique provides a linear state feedback control law for the system. This law has the following form:

where k is a 6x2 matrix that contains the state feedback gains (Kalman gains) and can be obtained by minimizing the following performance index:

where Q and R are the "weighting matrices". Q is an any 6x6 matrix and R is an any 2x2 one. The solution of the above equation is found by solving to P (Lyapunov function matrix) the Algebraic Riccati Equation:

**For a more detailed and extended explanation, please check master_thesis.pdf, where a solution is also described for discrete cases.**

The plots in the left side shown in the above figure, show the behavior of the LQR controller system when the state vector is deﬁned as

As it can be seen, the system
could not be stabilized. The reason is due to the fact that the real position of the centers of gravity

of the arms, torso and legs were not taken into account when building the model.
Nevertheless, a **simple solution** for this problem consists in compensating the system with a angle obtained from the resultant center of gravity of the three bodies about the vertical:

Defining the new state vector as

the controller is now able to stabilize the system (plots on the right side of the first figure).

**For a more detailed and extended explanation, please check master_thesis.pdf, where results are presented for the discrete case.**