1. Introduction
As a representative achievement of mechatronics, the humanoid robot has attracted extensive research interest over the past few decades. Unlike wheeled or tracked mobile robots, a humanoid robot walks with discrete foot contacts, which allows it to select optimal supporting points and to perform legged locomotion in complex environments. The humanoid robot is particularly suitable for operating in human-centered environments, such as homes, hospitals, and disaster sites, because it can mimic human walking and interact with tools designed for humans. However, to truly adapt to unstructured environments, a humanoid robot must be able to handle obstacles. This paper focuses on the gait planning problem for a humanoid robot crossing obstacles, which is a fundamental capability for expanding its operational envelope.
Research on humanoid robots has evolved from simple planar walking machines to highly integrated humanoid systems with dozens of degrees of freedom. The history of humanoid robots began with the WAP-1 planar locomotive in 1969, followed by the WL-5 and WAROT-1 in the 1970s. In the 1990s, the Honda ASIMO series demonstrated dynamic walking, running, and even stair climbing. More recently, humanoid robots such as HRP-2, SDR-4X, and QRIO have shown advanced capabilities in balance recovery and whole-body coordination. In China, several universities have developed humanoid robots with different levels of autonomy. Nevertheless, most existing studies of humanoid robot walking focus on flat-ground locomotion, while obstacle-crossing remains a challenging research topic.
Gait planning is the core of humanoid robot motion control. It determines the joint trajectories that satisfy both stability constraints and geometric constraints imposed by the environment. The main contributions of this work are: (1) establishing a simplified but effective mathematical model of a laboratory humanoid robot; (2) deriving the static and dynamic stability conditions, including the zero moment point (ZMP) criterion; (3) analyzing the collision-free geometric constraints for both step-by-step obstacle crossing and direct obstacle crossing; (4) proposing a cubic spline interpolation based gait planning method that generates smooth trajectories for the ankles and hip; and (5) presenting a genetic algorithm based gait optimization approach for improving dynamic stability.
The remainder of this paper is organized as follows. Section 2 describes the humanoid robot model and its kinematics. Section 3 discusses stability and geometric constraints in obstacle environments. Section 4 details the gait planning for two obstacle-crossing strategies. Section 5 presents simulation results. Section 6 briefly introduces a genetic algorithm based gait optimization method. Section 7 concludes the paper.
2. Mathematical Model of the Humanoid Robot
2.1 System Structure
The laboratory humanoid robot prototype is 45 cm tall and weighs 2.4 kg. It has 25 degrees of freedom (DOF): each leg has 6 DOF (2 at the ankle, 1 at the knee, 3 at the hip), each arm has 5 DOF, and the waist and head provide additional actuation. The robot is driven by MG995 servo motors controlled by a Freescale MC9S12DG128 microcontroller. The frame is made of aluminum alloy, which gives the robot a high strength-to-weight ratio. The joint ranges of motion are listed in Table 1.
| Joint | Range (deg) | Joint | Range (deg) |
|---|---|---|---|
| Ankle/hip lateral | -30 ~ 30 | Shoulder forward | -90 ~ 90 |
| Ankle forward | -45 ~ 65 | Shoulder lateral | 0 ~ 90 |
| Knee | -10 ~ 90 | Elbow turn | -50 ~ 50 |
| Hip forward | -60 ~ 90 | Elbow lateral | -80 ~ 80 |
| Hip turn | -60 ~ 60 | Wrist | 0 ~ 60 |
| Waist | -50 ~ 50 | Head rotation | -50 ~ 50 |
For gait planning, the robot is modeled as a multi-link mechanism. Because the humanoid robot has many degrees of freedom, a full 3D dynamic model is complex and difficult to use for real-time planning. Therefore, we decouple the forward plane (sagittal plane) and the lateral plane (coronal plane), and build two planar models. This decoupling is widely accepted in humanoid robot research because the coupling between the two planes has limited influence on the essential gait characteristics when the trunk is kept upright.
2.2 Forward Kinematics in the Sagittal Plane
In the forward plane, the left and right legs are modeled as two links each (thigh and shank), and the torso is modeled as a single link. The arms are simplified as rigid links attached to the shoulder. The coordinate origin is at the ankle of the supporting foot. Let \(q_2, q_3, q_4\) represent the right ankle, knee, and hip forward joint angles, and \(q_9, q_{10}, q_{11}\) represent the left hip, knee, and ankle forward joint angles. Let \(\theta_i\) be the angle between link \(i\) and the vertical direction. Then the joint angle vector \(\mathbf{Q}\) and the link-angle vector \(\mathbf{\Theta}\) are related by
\[
\mathbf{Q} = \mathbf{K} \mathbf{\Theta}
\]
where
\[
\mathbf{Q} = [q_2\; q_3\; q_4\; q_9\; q_{10}\; q_{11}\; q_{13}\; q_{14}]^T
\]
\[
\mathbf{\Theta} = [\theta_2\; \theta_3\; \theta_4\; \theta_9\; \theta_{10}\; \theta_{11}\; \theta_{13}\; \theta_{14}]^T
\]
and \(\mathbf{K}\) is a constant matrix determined from the geometry. For the forward plane, the direct kinematics equations for the joint positions are straightforward. For example, the hip position \((x_4,z_4)\) is
\[
\begin{cases}
x_4 = l_2 \sin\theta_2 + l_3 \sin\theta_3 \\
z_4 = l_2 \cos\theta_2 + l_3 \cos\theta_3
\end{cases}
\]
The left ankle position is
\[
\begin{cases}
x_{11} = l_9 \sin\theta_9 + l_{10} \sin\theta_{10} \\
z_{11} = l_9 \cos\theta_9 + l_{10} \cos\theta_{10}
\end{cases}
\]
For inverse kinematics, when the hip position \((x_4,z_4)\) and the swing ankle position \((x_{11},z_{11})\) are known, the link angles can be obtained from geometric relationships. For instance, the right leg solution is
\[
\theta_2 = \arctan\left(\frac{x_4-x_2}{z_4-z_2}\right) + \arccos\left(\frac{R_2^2 + l_2^2 – l_3^2}{2 R_2 l_2}\right)
\]
\[
\theta_3 = \arctan\left(\frac{x_4-x_2}{z_4-z_2}\right) – \arccos\left(\frac{R_2^2 – l_2^2 + l_3^2}{2 R_2 l_3}\right)
\]
where \(R_2 = \sqrt{(x_4-x_2)^2+(z_4-z_2)^2}\). Similar equations hold for the left leg.
2.3 Kinematics in the Lateral Plane
In the lateral plane, the lower limbs are simplified as single links, and the hip is modeled as a horizontal link connecting the two hip joints. The trunk and arms are also treated as links. The joint variables include the ankle lateral, hip lateral, and shoulder lateral angles. Defining \(\theta_1\) as the angle of the right leg with respect to the vertical, \(\theta_5\) as the hip link angle, and \(\theta_8\), \(\theta_{12}\) for the left leg, the direct kinematics gives the ankle positions and the hip height. For example, the right hip position is
\[
\begin{cases}
y_5 = -l_1 \sin\theta_1 \\
z_5 = l_1 \cos\theta_1
\end{cases}
\]
The inverse kinematics in the lateral plane is used less frequently because the lateral motion is planned directly in joint-space using smooth functions, as described later.
2.4 Dynamics
The dynamic model of the humanoid robot is essential for stability analysis and control design. The Lagrangian formulation is used here. The Lagrangian \(L\) is defined as \(L = E – P\), where \(E\) is the total kinetic energy and \(P\) is the total potential energy. For a serial chain, the kinetic energy of each link can be computed from the link velocities and inertias. For example, the kinetic energy of link 2 is
\[
E_2 = \frac{1}{2} m_2 v_2^2 + \frac{1}{2} I_2 \dot{\theta}_2^2
\]
where \(v_2\) is the linear velocity of the link center of mass, \(m_2\) is the mass, and \(I_2\) is the moment of inertia. The potential energy is
\[
P_2 = m_2 g z_{c2}
\]
where \(z_{c2}\) is the height of the center of mass of link 2. Combining all links yields the full Lagrangian, and the equations of motion are obtained from
\[
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}_i}\right) – \frac{\partial L}{\partial \theta_i} = \tau_i
\]
where \(\tau_i\) is the generalized force/torque for joint \(i\). The full dynamic equations are too lengthy to list here, but they are implemented numerically in the simulation.
3. Stability and Geometric Constraints
3.1 Static Walking Stability
When the humanoid robot walks slowly, inertial forces are negligible, and the stability condition is that the ground projection of the center of gravity (CoG) remains inside the convex support polygon formed by the supporting foot (or feet). The CoG coordinates are computed as
\[
x_{CoG} = \frac{\sum_{i=1}^{n} m_i x_i^*}{\sum_{i=1}^{n} m_i}
\]
\[
y_{CoG} = \frac{\sum_{i=1}^{n} m_i y_i^*}{\sum_{i=1}^{n} m_i}
\]
where \(m_i\) is the mass of link \(i\) and \(x_i^*, y_i^*\) are the center-of-mass coordinates of link \(i\). Static stability is a necessary condition for slow walking, but it is not sufficient for dynamic walking.
3.2 Dynamic Walking Stability and ZMP
The zero moment point (ZMP) is the most widely used dynamic stability criterion for humanoid robots. The ZMP is defined as the point on the ground where the resultant moment of gravity and inertia forces about the horizontal axes is zero. It can also be interpreted as the point where the resultant ground reaction force acts. The ZMP coordinates can be computed from the acceleration of each link mass:
\[
x_{ZMP} = \frac{\sum_{i=1}^{n} m_i(\ddot{z}_i + g) x_i – \sum_{i=1}^{n} m_i \ddot{x}_i z_i – \sum_{i=1}^{n} I_i \ddot{\theta}_i}{\sum_{i=1}^{n} m_i(\ddot{z}_i + g)}
\]
\[
y_{ZMP} = \frac{\sum_{i=1}^{n} m_i(\ddot{z}_i + g) y_i – \sum_{i=1}^{n} m_i \ddot{y}_i z_i + \sum_{i=1}^{n} I_i \ddot{\theta}_i}{\sum_{i=1}^{n} m_i(\ddot{z}_i + g)}
\]
For stable dynamic walking, the ZMP must always lie within the convex support polygon. In the single support phase, the polygon is the sole of the supporting foot. In the double support phase, it is the convex hull of both feet. Additionally, the following conditions must be satisfied to avoid other destabilizing motions:
- The vertical ground reaction force must be positive: \(\sum_i m_i(\ddot{z}_i + g) > 0\), so that the supporting foot does not lose contact.
- The horizontal resultant force must be less than the friction limit: \(\sqrt{F_x^2+F_y^2} < \mu F_z\).
- The yaw moment must be less than the frictional moment to prevent foot rotation.
These conditions guarantee that the humanoid robot does not tip over, slide, or spin during walking.
3.3 Geometric Constraints in an Obstacle Environment
Consider a typical rectangular obstacle of length \(l\), width \(w\), and height \(h\), as shown in the conceptual diagram. The humanoid robot can cross the obstacle in two ways: (1) step-by-step crossing, where the robot first steps onto the top of the obstacle and then steps down; (2) direct crossing, where the robot steps over the obstacle in one stride. For each mode, we derive collision-free constraints in the sagittal plane.
3.3.1 Step-by-Step Obstacle Crossing
Let \(l_{17}\) be the distance from the supporting toe to the obstacle, \(h_f\) the ankle height, \(l_b\) the rear foot length, and \(l_f\) the front foot length. When the robot steps onto the obstacle, the supporting leg is the right leg, and the left leg (swing leg) must avoid collisions. The critical condition for the swing foot to clear the obstacle is that the ankle height must be greater than \(h\) when the ankle is above the obstacle. Additionally, the shank link must not touch the obstacle before the foot is raised. The constraint for the right shank when it is still on the ground is
\[
\theta_2 < \arctan\left(\frac{h – h_f}{l_{17}+l_f}\right)
\]
where \(\theta_2\) is the angle of the right shank with respect to the vertical. This inequality ensures that the shank does not intersect the obstacle. The foot is also constrained by the leg length:
\[
\sqrt{(x_4-x_2)^2+(z_4-z_2)^2} \le l_2 + l_3
\]
When the foot lands on the obstacle, it must be fully in contact, meaning the ankle horizontal position equals \(l_{17}+l_b+l_f\) and the ankle height equals \(h\). Similar constraints apply when stepping down.
3.3.2 Direct Obstacle Crossing
In the direct crossing mode, the swing foot must clear the obstacle entirely. Let \(l_{18}\) be the distance from the obstacle to the landing heel. The ankle trajectory must satisfy:
\[
z_{ankle} > h \quad \text{for} \quad x_{ankle} \in [l_{17}, l_{17}+w]
\]
The shank of the supporting leg must also avoid the obstacle. The geometric constraints are similar to those in the step-by-step mode but with different boundary conditions. These constraints are used as key-frame conditions in the gait planning process.
4. Gait Planning for Obstacle Crossing
4.1 Method Overview
We adopt an analytical and constructive approach to gait planning. The method consists of the following steps:
- Determine the key frames of the swing ankle and hip based on the geometric constraints and human walking characteristics.
- Interpolate the key frames using cubic spline functions to obtain smooth trajectories for the ankle and hip.
- Calculate the lower-limb joint angles using the inverse kinematics formulas.
- Plan the upper-limb motion in a coordinated manner with the lower limbs.
This approach guarantees that the planned trajectories satisfy the collision-free constraints and have continuous velocities and accelerations.
4.2 Step-by-Step Obstacle Crossing
4.2.1 Sagittal Plane Planning
The step-by-step crossing is divided into three phases for stepping up and three phases for stepping down. For stepping up, the initial time is \(t_0\), the time when the left foot reaches the obstacle edge is \(t_{01}\), the time when the left foot lands on the obstacle is \(t_1\), the end of the forward hip shift is \(t_2\), and the final time when both feet are on the obstacle is \(t_3\). The left ankle trajectory key frames are:
\[
x_{11}(t) =
\begin{cases}
0 & t=t_0 \\
l_{17} & t=t_{01} \\
l_{17}+l_b+l_f & t=t_1
\end{cases}
\]
\[
z_{11}(t) =
\begin{cases}
0 & t=t_0 \\
h+0.5 & t=t_{01} \\
h & t=t_1
\end{cases}
\]
with zero velocity at \(t_0\) and \(t_1\). The right ankle remains stationary during the first two phases and follows the same trajectory as the left ankle during the third phase. The hip trajectory key frames are:
\[
x_4(t) =
\begin{cases}
0 & t=t_0 \\
l_f & t=t_1 \\
l_{17}+l_f & t=t_2 \\
l_{17}+l_b+l_f & t=t_3
\end{cases}
\]
\[
z_4(t) =
\begin{cases}
l_0 & t=t_0 \\
\sqrt{l_0^2-l_f^2} & t=t_1 \\
\sqrt{l_0^2-l_f^2} & t=t_2 \\
h+l_0 & t=t_3
\end{cases}
\]
where \(l_0\) is the nominal distance from hip to ankle in the standing posture. The hip moves along an arc in the first phase, then translates horizontally, and finally moves up onto the obstacle. The stepping-down trajectories are analogous, with the height decreasing from \(h+l_0\) to \(l_0\).
4.2.2 Lateral Plane Planning
In the lateral plane, the robot shifts its center of mass to the supporting foot before lifting the swing leg. The lateral motion is planned directly in joint space. Let \(\theta_1\) be the right ankle lateral angle. The key frames for the step-up motion are:
\[
\theta_1(t) =
\begin{cases}
0 & t=t_0 \\
\delta_1 & t=\frac{3t_3}{4} \\
0 & t=\frac{t_3}{2} \\
-\delta_2 & t=\frac{3t_3}{4} \\
0 & t=t_3
\end{cases}
\]
where \(\delta_1\) and \(\delta_2\) are computed from the hip width and the leg length. The negative sign indicates a lateral shift to the left. The shoulder lateral angles are planned as cosine functions to assist balance.
4.3 Direct Obstacle Crossing
For direct obstacle crossing, the walking is divided into three phases: left foot swings over the obstacle, the hip translates forward, and the right foot swings over the obstacle. The left ankle key frames are:
\[
x_{11}(t) =
\begin{cases}
0 & t=t_0 \\
l_{17} & t=t_{01} \\
l_{17}+w+l_f & t=t_{02} \\
l_{17}+w+l_b+l_f & t=t_1
\end{cases}
\]
\[
z_{11}(t) =
\begin{cases}
0 & t=t_0 \\
h+0.5 & t=t_{01} \\
h+0.5 & t=t_{02} \\
0 & t=t_1
\end{cases}
\]
The right ankle trajectory is identical but time-shifted. The hip trajectory key frames are:
\[
x_4(t) =
\begin{cases}
0 & t=t_0 \\
l_f & t=t_1 \\
l_{17}+w+l_f & t=t_2 \\
l_{17}+w+l_b+l_f & t=t_3
\end{cases}
\]
\[
z_4(t) =
\begin{cases}
l_0 & t=t_0 \\
\sqrt{l_0^2-l_f^2} & t=t_1 \\
\sqrt{l_0^2-l_f^2} & t=t_2 \\
l_0 & t=t_3
\end{cases}
\]
The hip height stays at a lower value during the crossing phase, which requires the supporting leg to bend to allow the swing foot to clear the obstacle. The joint angles are again obtained by inverse kinematics.
4.3.1 Arm Motion Planning
For coordinated upper-limb motion, we define the shoulder forward angles \(\theta_{13}\) and \(\theta_{14}\) as:
\[
\theta_{13}(t) = \frac{\pi}{8}\left(1-\cos\left(\frac{\pi t}{t_3}\right)\right)
\]
\[
\theta_{14}(t) = \frac{\pi}{24}\left(1-\cos\left(\frac{2\pi t}{t_3}\right)\right)
\]
These cosine functions produce a smooth arm swing that helps counterbalance the lower-limb motion, improving the overall stability of the humanoid robot.
5. Simulation Results
The proposed gait planning method was simulated using MATLAB on the actual dimensions of the laboratory humanoid robot. The link lengths in the sagittal plane are listed in Table 2.
| \(l_2\) | \(l_3\) | \(l_9\) | \(l_{10}\) | \(h_f\) | \(l_b\) | \(l_f\) |
|---|---|---|---|---|---|---|
| 6 | 6 | 6 | 6 | 4.5 | 4.6 | 5.4 |
The lateral plane parameters are given in Table 3.
| \(l_1\) | \(l_4\) | \(l_5\) | \(l_6\) | \(l_{15}\) | \(l_{16}\) | \(r_w\) | \(l_w\) |
|---|---|---|---|---|---|---|---|
| 13 | 14.5 | 7.6 | 19 | 20 | 20 | 2.8 | 2.8 |
In the simulation, the obstacle height \(h\) was set to 3 cm, the distance \(l_{17}\) to 3 cm, and \(l_{18}\) to 3 cm. The time parameters were chosen as \(t_0=0\), \(t_1=0.4\) s, \(t_2=0.6\) s, and \(t_3=1.0\) s for the direct crossing case. The resulting trajectories of the left ankle, right ankle, and hip are shown in the following plots (not reproduced here but described qualitatively).
The simulation results showed that the ankle trajectories are smooth, and the foot clears the obstacle with a margin of at least 0.5 cm. The horizontal velocity of the hip remains almost constant during the translation phase, which is favorable for stability. The joint angles computed from inverse kinematics stay within the allowable ranges listed in Table 1. The shoulder forward angles follow the designed cosine patterns, ensuring coordinated arm-leg movement.
For the step-by-step crossing, the trajectories are more complex because the robot has to step onto and then off the obstacle. The hip vertical trajectory has a maximum point when the robot stands on the obstacle, and the joint angles remain continuous throughout the entire motion. The simulation confirmed that the planned gait is feasible for the given humanoid robot.
6. Gait Optimization Using Genetic Algorithm
Although the cubic spline based gait planning produces feasible trajectories, the resulting gait may not be dynamically optimal. To improve the walking stability of the humanoid robot, we formulate an optimization problem based on the ZMP criterion. The instability measure is defined as the distance between the ZMP and the center of the support polygon:
\[
D(t) = \sqrt{(x_{ZMP}(t)-x_c(t))^2 + (y_{ZMP}(t)-y_c(t))^2}
\]
where \((x_c,y_c)\) is the center of the support polygon. The goal is to minimize the maximum instability over the gait cycle:
\[
\min_{p_1,p_2} \max_{t \in [t_0,t_3]} D(t)
\]
where \(p_1\) and \(p_2\) are selected gait parameters, such as the time intervals \(t_1\) and \(t_2\). We use a genetic algorithm (GA) because it does not require gradient information and is well suited for nonlinear, multi-parameter optimization problems. The GA procedure is as follows:
- Encode the optimization variables as real-valued genes.
- Initialize a population of candidate solutions randomly.
- Evaluate the fitness of each individual by computing the maximum ZMP deviation over the gait cycle using the dynamic model.
- Apply selection, crossover, and mutation operators to generate a new population.
- Repeat steps 3-4 until the termination condition is met.
The optimal parameters produce a gait with the smallest maximum instability, thereby increasing the stability margin of the humanoid robot during obstacle crossing. This optimization method can be easily extended to other gait parameters, such as step length, step height, or hip swing amplitude.
7. Conclusion and Future Work
This paper addressed the gait planning problem for a humanoid robot crossing obstacles. A laboratory humanoid robot was modeled with decoupled sagittal and lateral plane kinematics. Stability constraints based on static CoG and dynamic ZMP criteria were formulated. Collision-free geometric constraints were derived for two obstacle-crossing strategies: step-by-step crossing and direct crossing. A cubic spline interpolation method was used to generate smooth trajectories for the feet and hip, and inverse kinematics was employed to compute joint angle trajectories. Simulation results demonstrated the effectiveness and feasibility of the proposed planning method. Additionally, a genetic algorithm based optimization approach was introduced to maximize the dynamic stability of the planned gait.
Future work will focus on the following aspects:
- Implementing the planned gait on the physical humanoid robot and validating the results in real time.
- Incorporating force/torque sensors and inertial measurement units to provide feedback for balance control.
- Extending the planning method to more complex environments, such as stairs, slopes, and uneven terrain.
- Integrating vision sensors to enable the humanoid robot to detect obstacles and generate adaptive gait plans autonomously.
The research on humanoid robot obstacle-crossing is still in its early stage. With advances in actuators, sensors, and artificial intelligence, humanoid robots will eventually achieve human-like mobility in complex environments, which will greatly expand their application in daily life and industrial scenarios.