Today, I'll show some of the work I did **last year** in my university. I worked in a project titled *"Stability Control of a Humanoid Robot"*. The project goal was to test a stability control algorithm on a humanoid robot in a virtual environment. This project was a first step toward the actual implementation of a humanoid robot.

I'm going to show the steps I followed to achieve a full simulation environment. And I hope these steps allow other people to do simulations of robots (not necessarily bipedal robots).

###
Mechanical design

The project started with the mechanical design of the humanoid robot. Since my faculty had servomotors, they were used as part of the design. The mechanical design was developed on Solidworks, and was inspired on various commercial designs. The robot has 18 degrees of freedom, 5 per leg and 4 per arm, and has no head!

Solidworks render of the mechanical design

The solidworks design provides pretty much all the necessary information (dimensions, mass and inertia) for a dynamic simulation.

Mass properties from Solidworks

###
The simulation environment

Next task was selecting a simulation environment, at that time my team and I only knew the Simulink environment from MATLAB, so the choice was limited. The Simmechanics toolbox was used to simulate the multibody dynamics of the robot.

The simulink environment

###
Bodies and joints

Simmechanics provides body and joint blocks to construct various mechanical assemblies. A body holds the inertial information necessary for a dynamic simulation. And two bodies can be connected via a joint, this joint can add zero or more degrees of freedom between the bodies.

6 DOF Joint between the humanoid torso and the ground (absolute reference point)

The bodies and joints are referenced to each other via Coordinates Systems (CS), which can be absolute or relative. Also, sensors and actuators can be attached to the bodies or to the joints for control applications.

Torso (body) properties: Inertial parameters and CS

There is a plugin for Solidworks called Simmechanics Link, that allows the conversion from a Solidworks assembly to a Simulink model. However, the bodies and joints were laid out manually for greater flexibility and control.

###
Modeling the floor

The problem with the Simmechanics toolbox was the lack of object
collision, which is crucial for the simulation, as the normal and
friction forces between the floor and the robot are key for gait.

To solve this problem, the floor was modeled as a PD controller. This controller exerted normal forces on a few selected points of the robot sole, **only when these points were on or below the floor level**.

8 support points selected per foot.

The error input of this PD controller was the deviation of these points from the floor level. High proportional and derivative gains were used to minimize the sinking of the feet on the floor. The floor also exerted viscous friction forces, when the feet was in contact with the floor.

Floor controller block diagram.

###
Modeling the servomotor

For the servomotor model, specifications like the maximum angular speed and the maximum torque were used to produce an accurate DC motor model. For the servomotor controller model, a position + speed controller with gravity compensation and angular speed feedforward was used. Full block diagram is below.

Servomotor model

Each servomotor was attached to each joint via a joint actuator and joint sensors, as shown in the following image.

Actuated joint

###
Putting it all together

After modelling the servomotor and the floor, the rest of the work consisted in assigning each body its inertial parameters and its 3d model (.stl) and then gluing everything together with the servomotors. The final result is shown below.

High level assembly

###
Animation

The simulation of this whole system can take a considerable amount of time, depending on the number of degrees of freedom, CPU power, etc. Here's a tip about the animation, you can speed up a little the animation (not the simulation), by accessing the menu Simulation > Control Animation Speed and reducing the "Delay per frame" and tweaking the "Visualization sample time".

Controlling the animation speed

###
Making it move

Moving the robot is as simple as giving some references to the servomotors, but making it walk is a different history. The references that can be send to the servomotors are angular positions (denoted as Q), but usually you want the robot to follow trajectories in the XYZ space.

The process of going from the Q space or articular space to the XYZ space is called forward kinematics, and the reverse process is called inverse kinematics. The forward kinematics is usually easier to compute, and is required to compute the inverse kinematics.

###
Forward kinematics

To compute the forward kinematics, the Denavit Hartenberg approach was used. I won't go into much detail but basically, the Denavit Hartenberg transformation matrices can map positions and orientations from one coordinate system to another.

With these Denavit Hartenberg transformation matrices, the position of any body part seen from any other body part (generally one of the feet) can be easy computed.

###
Inverse Kinematics

Using the forward kinematics, an analytic expression of the position of any body part seen from any other body part can be derived, the inputs of this expression are the joint angles (Q) and the output is the XYZ position. Computing the inverse of this expression is impossible, as there are many solutions.

Since no analytical expression for the inverse kinematics can be computed, an iterative method name Damped Least Squares (DLS) was used. I won't cover the mathematics of the DLS method here, but it uses the pseudo jacobian to approach the desired position iteratively.

###
The trajectory

The chosen gait trajectory consist of a spline for the foot trajectory and another spline for the hip trajectory for the single support phase, i.e. when only one foot is in contact with the floor.

Gait trajectory

This continuous trajectory was discretized in 50 points, and these points were transformed to the Q space using the inverse kinematics. The Q coordinates computed were finally fed at 50Hz to the servomotors.

###
The stability indicator

Trying to walk just by using the previous computed references will result in a failure, it's necessary to implement a stability control. And for that we need to select a stability indicator.

For this project, we used the Zero Moment Point (ZMP) indicator, which is basically an extension of the concept of center of mass in a dynamic context. You might know that a static object can stay in equilibrium as long as the projection (along the gravity axis) of its center of mass is inside its support polygon.

The ZMP takes in consideration the center of mass and the acceleration of the body and must reside inside the support polygon to guarantee the stability of the body.

The ZMP indicator computation can be reduced to the cart table problem, as sketched in the following image, for each plane of interest.

Cart table model for the XZ plane.

###
The stability control

Our problem reduces to keeping the ZMP inside the support polygon, to achieve this we implement two separate controls.

- Feet control, maintains the foot parallel to the floor maximizing the support polygon.

Left: Feet control disabled. Right: Feet control enabled

- Hip control, move the ZMP inside the support polygon.

Hip control in action, with disturbance at t = .25s.

Yellow: ZMP, Cyan/Magente: Support polygon

###
The final result

Here is your reward, for getting this far.

###
Acknowledgements

- Prof. Jose Oliden, our advisor
- Victor Paredes and Santiago Cortijo, team members
- Control systems and Artificial Intelligence Research Group (GISCIA)
- National University of Engineering (UNI)

###
Related