# Decoupled Closed-Form Solution for Humanoid Lower Limb Kinematics

Authors: Alejandro Said; Ernesto Rodriguez-Leal; Rogelio Soto; J. L. Gordillo; Leonardo Garrido
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

This paper presents an explicit, omnidirectional, analytical, and decoupled closed-form solution for the lower limb kinematics of the humanoid robot NAO. The paper starts by decoupling the position and orientation analysis from the overall Denavit-Hartenberg (DH) transformation matrices. Here, the joint activation sequence for the DH matrices is based on the geometry of a triangle. Furthermore, the implementation of a forward and a reversed kinematic analysis for the support and swing phase equations is developed to avoid matrix inversion. The allocation of constant transformations allows the position and orientation end-coordinate systems to be aligned with each other. Also, the redefinition of the DH transformations and the use of constraints allow  decoupling the shared DOF between the legs and the torso. Finally, a geometric approach to avoid the singularities during the walking process is indicated. Numerical data is presented along with an experimental implementation to prove the validity of the analytical results.


---

## Body

## 1. Introduction

Humanoid robotics has become a highly important subject for the academic community in recent years due to its potential use in domestic and medical applications. Several sophisticated humanoid robots have been developed, for example, the ASIMO robot [1], created by the Honda Motor Company; the QRIO robot [2], manufactured by Sony; and the HUBO robot [3], proposed by the KAIST.In addition to this, there is an increasing trend for the development of small-sized humanoid robots, for example, the NAO robot [4], created by Aldebaran Robotics; or the DARwIn-OP [5], manufactured by Robotis. This type of robots has found a commercial market niche in education and entertainment, offering an accessible platform for students, researchers, and hobbyists. Although commercial robots provide a simpler mechanical actuation and information processing than their noncommercial counterparts, there is an opportunity for the development of simple, analytical, and explicit kinematic models for gait kinematic control.It is well known in the literature that legged locomotion provides several advantages when compared to wheeled locomotion [6]; for example, legs can step over obstacles and achieve a smooth ride on uneven surfaces by varying the effective length of the legs in order to match the surface geometry. Nevertheless, the design of complex dynamic motions for humanoids is only achievable through the full understanding of kinematics [7], namely, the forward and inverse kinematics. The first concept concerns the determination of the position and orientation of the end-effector, when the active joint configurations of the robot are given while the second concept deals with determination of the joint variables for a particular position and orientation of the end-effector [8].Furthermore, the complexity of the inverse kinematics problem for open-kinematic chains has been exhaustively discussed in [9], particularly due to the fact that the nonlinear mapping of the joint and Cartesian spaces resulted in multiple solutions. Although closed-form solutions can be obtained for systems that provide six or less degrees-of-freedom (DOF), some studies prefer the determination of analytical solutions for real-time applications, since the computation time of the numerical solutions may vary significantly [10]. The computation of closed-form solutions requires the performance of complex algebraic and geometric tasks, where the challenge consists in finding the configurations in which a reduced number of unknowns can be used to express the position and orientation of the end-effector [11].Relevant work has been conducted towards the determination of simplified kinematic models in humanoid robots. Pieper has decoupled a six-DOF robot with three intersecting axes into sets of equations for the position and orientation in [12]; Graf et al. have solved a DH chain for a robot leg using a triangle-based geometric approach in [13]; Park et al. have used a forward and reverse decoupling method to solve the kinematics of a humanoid leg using the inverse transform method in [3]; Hernández-Santos et al. have divided the walking gait into the Sagittal and Frontal planes in order to obtain the closed-form solutions for the inverse and forward kinematics of a 16-DOF humanoid robot in [14]; and Kofinas et al. have manipulated both sides of the kinematic matrix equations in order to express the translation of one foot using only three variables in [15].This paper presents a comprehensive mathematical model that applies some of the methods listed above in addition to some new procedures, in order to determine the lower limb kinematics of the small-sized commercial humanoid NAO robot. This work approaches the position analysis by using a geometrical procedure based on triangular arrangements. Furthermore, the joint activation sequence resulting from this geometry is inserted in the analysis of the DH chain. This practice allows the conciliation of both analytical and geometrical equations in order to be solved simultaneously. Furthermore, a local coordinate frame is placed at the end-effector (either the hip or the foot). This allows the orientation functions to be obtained by equating the corresponding elements of the position and orientation matrix concatenations in a straightforward manner. The resulting orientation equations allow the feet to remain parallel to the ground, while the torso remains upright.Furthermore, an adjustment of the DH transformation matrices is performed in order to solve the kinematics of the joint that constrains the motion of the two legs and the torso, which presents a mechanical dependency. This is of particular interest for the turn-in-place motion [16], where some joints are constrained so that the mathematical complexity of the kinematic coupling is decreased. The result of this procedure is a set of functions that allows the robot to rotate about its own vertical axis while compensating the unwanted torso rotation. Finally, a simple geometric approach is proposed in order to avoid singularities in the gait postures. The equations obtained provide the total vertical displacement that the robot needs to experience in order to reach a given step distance, resulting in a gait workspace. This approach bypasses the use of the Jacobian matrix [17], reducing the mathematical complexity of this analysis. The gait workspaces with and without joint limitations are displayed and discussed.The remainder of this paper is organized as follows: Section2 describes the nomenclature of the humanoid joints. Also, the DH convention is presented. Section 3 is a position analysis of the humanoid robot leg, where the forward and reversed kinematics problems are analytically solved. In Section 4 the mechanically shared joint between the two legs is analyzed, where closed-form solutions are determined for the turn-in-place motion. Section 5 presents the analysis of the workspace of a footstep; in this section the workspace of a humanoid gait is geometrically deduced and plotted. In Section 6 numerical data is presented along with experimental robot positions to corroborate the feasibility of the results obtained. Finally, Section 7 presents important conclusions about the mathematical procedures presented in this work.


## 2. Humanoid Robot Description and Notation

This paper uses the humanoid robot NAO model H21 that is manufactured by Aldebaran Robotics [18] as an experimental platform. This robot has 21 rotational DOF and is actuated by servomotors. The wrists and hands of this model are not actuated; hence they are not considered in this work. Figure 1 shows the humanoid robot, including the navigation reference frame and the notation used to identify the joints that comprise the kinematic chains. Regarding the navigation coordinate frame of the robot, the x-axis points to the forward walking direction; the y-axis points to the left side of the robot; and the z-axis points upwards. Each leg of the robot has 5 DOF (namely, the AnkleRoll, AnklePitch, KneePitch, HipRoll, and HipPitch) and one special joint located between the hips (comprised of the RHipYawPitch and the LHipYawPitch), coupled by a gearbox that connects the two legs. This special joint is rotated and mirrored 45
° over the x-axis at each hip.Figure 1
Active joints of H21 NAO robot.The used DH transformation matrix structure can be found in [19] and is given by the sequence of matrix rotations and translations given by (1)
T
i
i
-
1
=
R
x
α
i
-
1
D
x
a
i
-
1
R
z
θ
i
D
z
d
i
.The result of the previous matrix multiplications is given by(2)
T
i
i
-
1
=
C
θ
i
-
S
θ
i
0
a
i
-
1
S
θ
i
C
α
i
-
1
C
θ
i
C
α
i
-
1
-
S
α
i
-
1
-
S
α
i
-
1
d
i
S
θ
i
S
α
i
-
1
C
θ
i
S
α
i
-
1
C
α
i
-
1
C
α
i
-
1
d
i
0
0
0
1
,where C and S are used as abbreviations for the cosine and sine functions, respectively.


## 3. Position and Orientation Analysis of the Humanoid Leg

In this section, the position of the hip, the position of the swinging foot, the orientation of the torso, and the plane orientation of the swinging foot are presented according to [20].For convenience of the reader, this work considers scalars, vectors, and matrices to be represented as lowercase italics, lowercase bold type, and uppercase bold type, respectively. Furthermore, Greek characters are used in the paper to express angular positions of the joints. Consider thatθ
ar, θ
ap, θ
kp, θ
hp, θ
hr, and θ
hy denote ankle roll, ankle pitch, knee pitch, hip pitch, hip roll, and hip yaw-pitch angle, respectively. Table 1 includes the dimensional parameters of the humanoid robot and the joint range of motions that have been used in the experiments of this work. Note that the robot joints have mechanical limits, and these joints do not have the same range of movement at the right and left legs. For instance, when the robot moves from left to right and takes a step, it results in a minimum absolute value of 22.27
° for the θ
ar articulation. Furthermore, the minimum joint angular displacement for θ
kp is 0
°, which corresponds to the angular position for a fully extended knee.Table 1
Used joint range of motions.
Joint
Min
Max
θ
ar
-
22.2
7
°
22.2
7
°
θ
ap
-
67.9
7
°
52.8
6
°
θ
kp
0
°
121.0
4
°Table2 includes the limb dimensional parameters of the humanoid robot, where l
ft, l
tb, and l
tg denote the foot height (i.e., the distance from the ground to the intersection of the ankle joints), the tibia length (i.e., the distance from the intersection of the ankle joints to the knee joint), and the thigh length (i.e., the distance from the knee joint to the intersection of the hip joints), respectively. Following the nomenclature used in [21], a letter L or R is added to the variables in order to indicate their location at the left or right leg, respectively. A subscript s is added to the coordinates to indicate that they belong to the swing leg [22].Table 2
Limb dimensions (mm).
Limb
Dimension
l
ft
45.11
l
tb
102.75
l
tg
100


### 3.1. Forward Kinematic Chain Analysis of the Humanoid Leg

The forward kinematic analysis presented in this section describes the hip position of the robot at the single and double support phases, that is, the system supported by one foot or both feet, respectively [23]. For simplification purposes, this work assumes that gravity acts on a unique point called the center of gravity (CoG) [24], which is located at the center of the hips. It also assumes that the CoG and the hips move together.For analysis purposes, the forward kinematic chain in Figure2 has been split into position and orientation expressions [12]. According to [25], this method allows the position column of the DH matrix concatenation to include three equations with three unknowns, which provides a solution to the kinematics problem.Figure 2
Forward position analysis of the humanoid leg.For the forward position analysis, the hip joint variables have not been included, but the distance from the knee joint to hip joints has been considered to determine the end-effector position. The distancel
ft has been removed from the kinematic analysis; therefore, the height of the robot is affected by a constant offset. The chest can be included into the analysis, as in [5] for humanoid DARwIn-OP, but in this work it has been decided to remove this part from the kinematic analysis to reduce the number of variables involved.Following the geometric approach presented in [13], it is possible to define a position vector r (see Figure 3). Here, the tibia, the thigh, and the vector form a triangle which rotates an angular displacement θ
ar about the ankle joint articulation. In this work, the joint activation sequence for the analytic kinematic analysis is taken in the same order, that is, the θ
ar joint first, followed by the θ
ap joint and the θ
kp joint at the end. Note that the constant coordinate system number one (see Figure 2) is used to set the joint activation sequence mentioned above (θ
ar, θ
ap, and θ
kp), while the coordinate system number six is also constant and is used to determine the end-effector orientation. Both the coordinate system zero and six are aligned with the robot navigation coordinate system (see Figure 1). The Z unit vector along every actuator points in the direction of a positive rotation relative to the ground. Note that using this joint activation sequence when solving for the DH matrix transformations results in a simplified calculation, since the equations are functions of the joint parameters that have been determined; for example, the equation for θ
ap uses the already solved value of θ
kp. Solving the position equations analytically allows for the determination of a specific orientation.Figure 3
Sequence of joint activations based on the triangular geometry.The DH parameter table used for the forward position analysis is shown in Table3.Table 3
Link parameters used for the forward position analysis.
i
α
i
-
1
a
i
-
1
θ
i
d
i
1
0
0
-
π
/
2
0
2
π
/
2
0
θ
ar
+
π
/
2
0
3
π
/
2
0
θ
ap
0
4
0
l
tb
θ
kp
0
5
π
/
2
l
tg
π
/
2
0
6
π
/
2
0
π
/
2
0Using the parameters presented in Table3, the position coordinates of the hip can be extracted from the fourth column of the DH matrix concatenation leading to (3)
x
=
-
l
tg
S
θ
ap
+
θ
kp
+
l
tb
l
tg
S
θ
ap
,
(4)
y
=
l
tg
S
θ
ar
C
θ
ap
+
θ
kp
+
l
tb
l
tg
C
θ
ap
,
(5)
z
=
l
tg
C
θ
ar
C
θ
ap
+
θ
kp
+
l
tb
l
tg
C
θ
ap
,where x, y, and z, represent the components of the position coordinates of the hip.The inverse kinematics concerning the position of the hip can be obtained by solving the nonlinear system given by (3), (4), and (5).The ankle roll angleθ
ar is obtained by dividing (4) by (5); namely,(6)
θ
ar
=
arctan
y
z
.Squaring and adding (3), (4), and (5) results in(7)
x
2
+
y
2
+
z
2
=
2
·
l
tg
·
l
tb
·
C
θ
kp
+
l
tg
2
+
l
tb
2
.Solving for the knee pitch angleθ
kp gives(8)
θ
kp
=
arccos
x
2
+
y
2
+
z
2
-
l
tb
2
-
l
tg
2
2
·
l
tb
·
l
tg
.Dividing (3) by the square root of the sum of the squares of (4) and (5) results in(9)
x
y
2
+
z
2
=
-
S
θ
ap
+
θ
kp
+
l
tb
/
l
tg
S
θ
ap
C
θ
ap
+
θ
kp
+
l
tb
/
l
tg
C
θ
ap
.Solving for the ankle pitch angleθ
ap gives(10)
θ
ap
=
-
arctan
y
2
+
z
2
S
θ
kp
+
x
C
θ
kp
+
l
tb
/
l
tg
x
y
2
+
z
2
C
θ
kp
+
l
tb
/
l
tg
y
2
+
z
2
-
x
S
θ
kp
.Regarding the hip position analysis, in addition to (6), (8), and (10), the torso orientation equations must be determined. Consider that for the decoupled orientation analysis, the hip yaw-pitch joint has been removed in order reduce the number of variables affecting the orientation equations. This is due to the fact that the hip yaw-pitch joint does not play any role during the forwards and sideways gait. The chosen coordinate systems can be seen in Figure 4.Figure 4
Forward orientation analysis of the humanoid leg.The DH parameters for the forward orientation analysis are shown in Table4. In this table, the coordinate system number nine aligns the last orientation coordinate system of the hip with the navigation coordinate system of the robot.Table 4
Link parameters used for the forward orientation analysis.
i
α
i
-
1
a
i
-
1
θ
i
d
i
7
π
/
2
0
θ
hp
+
π
/
2
0
8
-
π
/
2
0
θ
hr
+
π
/
2
0
9
π
/
2
0
-
π
/
2
0Applying the parameters of Table3 and the parameters of Table 4, the transformation T
0
9 represents the matrix concatenation from coordinate system number zero to coordinate system number nine.Maintaining the torso in an upright position improves the balance of the robot because it helps to keep the reaction of the CoG within the surface of the feet. To maintain the torso upright, the diagonal elements of the rotation matrix fromT
0
9 must be equal to one; that is, R
0
9
=
I. Considering that the constant coordinate system T
6
5 (see Figure 2) has been included in the hip position analysis and that the joints θ
ap and θ
hp have parallel axes and that θ
ar and θ
hr also have parallel axes, it is possible to write (11)
T
9
7
1,1
=
-
T
6
0
1,1
.Furthermore, the hip pitch angleθ
hp can be expressed as(12)
θ
hp
=
-
θ
ap
+
θ
kp
.By using the following transformation matrices:(13)
T
9
7
2,2
=
-
T
6
0
2,2
.The hip roll angleθ
hr can be obtained as(14)
θ
hr
=
-
θ
ar
.In this section, the humanoid leg has been analyzed in the forward direction, that is, from the ankle to the hip. The forward analysis provides a procedure for positioning the hip regarding the ground. In the next subsection the humanoid leg is analyzed in a reversed order, that is, from the hip to the ankle. The reversed analysis provides a method for positioning the foot regarding the hip.


### 3.2. Reversed Kinematic Chain Analysis of the Humanoid Leg

In this work the reversed kinematic chain analysis [3] describes the position of the foot as it moves across the air, that is, the foot position at the swing phase. Figure 5 shows the coordinate systems in the reversed analysis of the robot leg. The reversed DH table can be obtained in a similar manner as the forward analysis; but unlike the forward analysis, the reversed analysis is taken from the hip to the ankle. It is important to notice that the Z-unit vector along every actuator is arranged in the direction of a positive rotation relative to the hip and that the resulting equations take the coordinates of the swing foot x
s, y
s, z
s as input. The resulting swing foot coordinates are the coordinates of the foot regarding the hip, and they are used by the robot when lifting a leg to take a step.Figure 5
Reversed position and orientation analysis of the humanoid leg.Solving for the position joint variables results in(15)
θ
hr
=
arctan
-
y
s
z
s
,
θ
kp
=
arccos
x
s
2
+
y
s
2
+
z
s
2
-
l
tb
2
-
l
tg
2
2
·
l
tb
·
l
tg
,
θ
hp
=
-
arctan
y
s
2
+
z
s
2
S
θ
kp
+
x
s
C
θ
kp
+
l
tg
/
l
tb
x
s
y
s
2
+
z
s
2
C
θ
kp
+
l
tg
/
l
tb
y
s
2
+
z
s
2
-
x
s
S
θ
kp
.Solving for the orientation joint variablesθ
ap and θ
ar results in(16)
θ
ap
=
-
θ
hp
+
θ
kp
,
θ
ar
=
-
θ
hr
.Observe that the angular positions of the jointsθ
ap and θ
ar have changed their roles from variables used for position in the forward analysis to variables used for orientation in the reversed analysis.


## 3.1. Forward Kinematic Chain Analysis of the Humanoid Leg

The forward kinematic analysis presented in this section describes the hip position of the robot at the single and double support phases, that is, the system supported by one foot or both feet, respectively [23]. For simplification purposes, this work assumes that gravity acts on a unique point called the center of gravity (CoG) [24], which is located at the center of the hips. It also assumes that the CoG and the hips move together.For analysis purposes, the forward kinematic chain in Figure2 has been split into position and orientation expressions [12]. According to [25], this method allows the position column of the DH matrix concatenation to include three equations with three unknowns, which provides a solution to the kinematics problem.Figure 2
Forward position analysis of the humanoid leg.For the forward position analysis, the hip joint variables have not been included, but the distance from the knee joint to hip joints has been considered to determine the end-effector position. The distancel
ft has been removed from the kinematic analysis; therefore, the height of the robot is affected by a constant offset. The chest can be included into the analysis, as in [5] for humanoid DARwIn-OP, but in this work it has been decided to remove this part from the kinematic analysis to reduce the number of variables involved.Following the geometric approach presented in [13], it is possible to define a position vector r (see Figure 3). Here, the tibia, the thigh, and the vector form a triangle which rotates an angular displacement θ
ar about the ankle joint articulation. In this work, the joint activation sequence for the analytic kinematic analysis is taken in the same order, that is, the θ
ar joint first, followed by the θ
ap joint and the θ
kp joint at the end. Note that the constant coordinate system number one (see Figure 2) is used to set the joint activation sequence mentioned above (θ
ar, θ
ap, and θ
kp), while the coordinate system number six is also constant and is used to determine the end-effector orientation. Both the coordinate system zero and six are aligned with the robot navigation coordinate system (see Figure 1). The Z unit vector along every actuator points in the direction of a positive rotation relative to the ground. Note that using this joint activation sequence when solving for the DH matrix transformations results in a simplified calculation, since the equations are functions of the joint parameters that have been determined; for example, the equation for θ
ap uses the already solved value of θ
kp. Solving the position equations analytically allows for the determination of a specific orientation.Figure 3
Sequence of joint activations based on the triangular geometry.The DH parameter table used for the forward position analysis is shown in Table3.Table 3
Link parameters used for the forward position analysis.
i
α
i
-
1
a
i
-
1
θ
i
d
i
1
0
0
-
π
/
2
0
2
π
/
2
0
θ
ar
+
π
/
2
0
3
π
/
2
0
θ
ap
0
4
0
l
tb
θ
kp
0
5
π
/
2
l
tg
π
/
2
0
6
π
/
2
0
π
/
2
0Using the parameters presented in Table3, the position coordinates of the hip can be extracted from the fourth column of the DH matrix concatenation leading to (3)
x
=
-
l
tg
S
θ
ap
+
θ
kp
+
l
tb
l
tg
S
θ
ap
,
(4)
y
=
l
tg
S
θ
ar
C
θ
ap
+
θ
kp
+
l
tb
l
tg
C
θ
ap
,
(5)
z
=
l
tg
C
θ
ar
C
θ
ap
+
θ
kp
+
l
tb
l
tg
C
θ
ap
,where x, y, and z, represent the components of the position coordinates of the hip.The inverse kinematics concerning the position of the hip can be obtained by solving the nonlinear system given by (3), (4), and (5).The ankle roll angleθ
ar is obtained by dividing (4) by (5); namely,(6)
θ
ar
=
arctan
y
z
.Squaring and adding (3), (4), and (5) results in(7)
x
2
+
y
2
+
z
2
=
2
·
l
tg
·
l
tb
·
C
θ
kp
+
l
tg
2
+
l
tb
2
.Solving for the knee pitch angleθ
kp gives(8)
θ
kp
=
arccos
x
2
+
y
2
+
z
2
-
l
tb
2
-
l
tg
2
2
·
l
tb
·
l
tg
.Dividing (3) by the square root of the sum of the squares of (4) and (5) results in(9)
x
y
2
+
z
2
=
-
S
θ
ap
+
θ
kp
+
l
tb
/
l
tg
S
θ
ap
C
θ
ap
+
θ
kp
+
l
tb
/
l
tg
C
θ
ap
.Solving for the ankle pitch angleθ
ap gives(10)
θ
ap
=
-
arctan
y
2
+
z
2
S
θ
kp
+
x
C
θ
kp
+
l
tb
/
l
tg
x
y
2
+
z
2
C
θ
kp
+
l
tb
/
l
tg
y
2
+
z
2
-
x
S
θ
kp
.Regarding the hip position analysis, in addition to (6), (8), and (10), the torso orientation equations must be determined. Consider that for the decoupled orientation analysis, the hip yaw-pitch joint has been removed in order reduce the number of variables affecting the orientation equations. This is due to the fact that the hip yaw-pitch joint does not play any role during the forwards and sideways gait. The chosen coordinate systems can be seen in Figure 4.Figure 4
Forward orientation analysis of the humanoid leg.The DH parameters for the forward orientation analysis are shown in Table4. In this table, the coordinate system number nine aligns the last orientation coordinate system of the hip with the navigation coordinate system of the robot.Table 4
Link parameters used for the forward orientation analysis.
i
α
i
-
1
a
i
-
1
θ
i
d
i
7
π
/
2
0
θ
hp
+
π
/
2
0
8
-
π
/
2
0
θ
hr
+
π
/
2
0
9
π
/
2
0
-
π
/
2
0Applying the parameters of Table3 and the parameters of Table 4, the transformation T
0
9 represents the matrix concatenation from coordinate system number zero to coordinate system number nine.Maintaining the torso in an upright position improves the balance of the robot because it helps to keep the reaction of the CoG within the surface of the feet. To maintain the torso upright, the diagonal elements of the rotation matrix fromT
0
9 must be equal to one; that is, R
0
9
=
I. Considering that the constant coordinate system T
6
5 (see Figure 2) has been included in the hip position analysis and that the joints θ
ap and θ
hp have parallel axes and that θ
ar and θ
hr also have parallel axes, it is possible to write (11)
T
9
7
1,1
=
-
T
6
0
1,1
.Furthermore, the hip pitch angleθ
hp can be expressed as(12)
θ
hp
=
-
θ
ap
+
θ
kp
.By using the following transformation matrices:(13)
T
9
7
2,2
=
-
T
6
0
2,2
.The hip roll angleθ
hr can be obtained as(14)
θ
hr
=
-
θ
ar
.In this section, the humanoid leg has been analyzed in the forward direction, that is, from the ankle to the hip. The forward analysis provides a procedure for positioning the hip regarding the ground. In the next subsection the humanoid leg is analyzed in a reversed order, that is, from the hip to the ankle. The reversed analysis provides a method for positioning the foot regarding the hip.


## 3.2. Reversed Kinematic Chain Analysis of the Humanoid Leg

In this work the reversed kinematic chain analysis [3] describes the position of the foot as it moves across the air, that is, the foot position at the swing phase. Figure 5 shows the coordinate systems in the reversed analysis of the robot leg. The reversed DH table can be obtained in a similar manner as the forward analysis; but unlike the forward analysis, the reversed analysis is taken from the hip to the ankle. It is important to notice that the Z-unit vector along every actuator is arranged in the direction of a positive rotation relative to the hip and that the resulting equations take the coordinates of the swing foot x
s, y
s, z
s as input. The resulting swing foot coordinates are the coordinates of the foot regarding the hip, and they are used by the robot when lifting a leg to take a step.Figure 5
Reversed position and orientation analysis of the humanoid leg.Solving for the position joint variables results in(15)
θ
hr
=
arctan
-
y
s
z
s
,
θ
kp
=
arccos
x
s
2
+
y
s
2
+
z
s
2
-
l
tb
2
-
l
tg
2
2
·
l
tb
·
l
tg
,
θ
hp
=
-
arctan
y
s
2
+
z
s
2
S
θ
kp
+
x
s
C
θ
kp
+
l
tg
/
l
tb
x
s
y
s
2
+
z
s
2
C
θ
kp
+
l
tg
/
l
tb
y
s
2
+
z
s
2
-
x
s
S
θ
kp
.Solving for the orientation joint variablesθ
ap and θ
ar results in(16)
θ
ap
=
-
θ
hp
+
θ
kp
,
θ
ar
=
-
θ
hr
.Observe that the angular positions of the jointsθ
ap and θ
ar have changed their roles from variables used for position in the forward analysis to variables used for orientation in the reversed analysis.


## 4. Turn-In-Place Analysis

The robot NAO includes a special mechanism composed of two coupled joints at each hip that equips the pelvis, where only one actuator is needed, resulting in a reduction of the building costs and space [4]. However, the collateral effect of the hip yaw-pitch joint configuration is a rotation along the x-axis, when only a rotation about the z-axis is desired.In this section, the mathematical functions to decouple the hip yaw movement from the hip pitch movement are found, allowing the robot to make a turn-in-place movement [16, 17], that is, a rotation about the z-axis, while maintaining the torso in a vertical position. Given that the hip yaw-pitch joint is mechanically coupled to both legs (see Figure 1), it is enough to analyze this joint in the forward direction; that is, the turn-in-place rotation of the swing foot is attached to the turn-in-place rotation of the support foot.Table5 presents the link parameters, including the hip yaw-pitch joint (see Figure 6). Note that the transformation T
9
8, presented previously, has been modified from the forward analysis to include the hip yaw-pitch into the kinematic chain.Table 5
Link parameters used of a turn-place-movement.
i
α
i
-
1
a
i
-
1
θ
i
d
i
9
π
/
2
0
π
/
2
0
10
-
π
/
4
0
θ
hy
0
11
π
/
4
0
-
π
/
2
0
12
0
0
-
π
/
2
0Figure 6
Turn-in-place movement orientation analysis.The premultiplication of theR
x
R
y
R
z Euler angles is used in order to describe the hip motion, in a similar manner as in [13, 26]. Note that the leg position is not considered in this section; therefore θ
ar
=
θ
ap
=
θ
kp
=
0. Since the hip yaw-pitch joint produces undesired motion only in the hip pitch axis, the constraint θ
hr
=
0 is further considered to reduce the complexity of the equations. Multiplying the matrices of the forward kinematic analysis (including the hip yaw-pitch joint) and considering the constraints above resulted in(17)
R
12
0
=
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
,where(18)
r
11
=
C
θ
hp
C
θ
hy
-
2
2
S
θ
hp
S
θ
hy
,
r
12
=
-
2
2
C
θ
hp
S
θ
hy
+
2
2
S
θ
hp
C
θ
hy
+
1
2
S
θ
hp
,
r
13
=
-
2
2
C
θ
hp
S
θ
hy
+
2
2
S
θ
hp
C
hy
-
1
2
S
θ
hp
,
r
21
=
2
2
S
θ
hy
,
r
22
=
1
2
+
1
2
C
θ
hy
,
r
23
=
-
1
2
+
1
2
C
θ
hy
,
r
31
=
S
hp
C
θ
hy
+
2
2
C
θ
hp
S
θ
hy
,
r
32
=
-
2
2
S
θ
hp
S
hy
-
2
2
C
θ
hp
C
hy
-
1
2
C
θ
hp
,
r
33
=
-
2
2
S
θ
hp
S
θ
hy
-
2
2
C
θ
hp
C
θ
hy
+
1
2
C
θ
hp
.MakingR
x
R
y
R
z
=
R
12
0 leads to(19)
r
21
r
22
=
S
z
C
x
C
z
C
x
=
2
/
2
S
θ
hy
1
/
2
+
1
/
2
C
θ
hy
=
tan
θ
z
,where θ
z represents the turn-in-place rotation.The equationθ
hy is solved in order to determine the yaw rotation to be performed by the robot(20)
θ
hy
=
arctan
2
2
tan
θ
z
tan
2
θ
z
+
2
,
-
tan
2
θ
z
-
2
tan
2
θ
z
+
2
.The following operation is carried out in order to correct the undesired pitch rotationθ
y
(21)
r
13
r
33
=
C
x
S
y
C
x
C
y
=
-
2
/
2
C
θ
hp
S
θ
hy
+
2
/
2
S
θ
hp
C
θ
hy
-
1
/
2
S
θ
hp
-
2
/
2
S
θ
hp
S
hy
-
2
/
2
C
θ
hp
C
θ
hy
+
1
/
2
C
θ
hp
=
tan
θ
y
.The following consideration is made to remove the pitch rotation when the hip yaw-pitch is activated(22)
tan
θ
y
=
0
.Equation (22) ensures that θ
y
=
0 is valid. Furthermore, ϕ
hp is the parameter that must be added to the hip pitch angle in order to eliminate the pitch motion due to the coupled hip yaw-pitch rotation(23)
ϕ
hp
=
-
arctan
2
S
θ
hy
1
+
C
θ
hy
.Thus, the compensated pitch angleθ
hpc for a turn-in-place rotation is(24)
θ
hpc
=
θ
hp
+
ϕ
hp
.Certain practical aspects were considered for achieving the turn-in-place rotation. For instance, only the left hip yaw command has been used, while the right hip yaw command has been neglected due to its priority at software level [18]. Hence, when the analysis is made over the right foot as the support foot, θ
hy and θ
hp must have opposite signs in their respective solutions. Also, the position of the swing foot should be considered to achieve an effective robot rotation as can be seen in Figure 7.Figure 7
Position and orientation of the robot feet at the turn-in-place rotation.In addition, the motion at the turn-in-place rotation should be small in order to prevent the legs of the robot from knocking against each other. Finally, (25) should be used when it is desired that the sole of the swing foot remains parallel to the ground, where θ
apc is the compensated ankle pitch angle of the opposite foot. In addition, it is recommended to organize the commands for the joints in tuples of right and left commands of the same kind of articulation, in order to achieve optimal positioning (see Tables 7, 9, and 11):(25)
θ
apc
=
θ
ap
+
ϕ
hp
.In the next section the workspace of a humanoid step is analyzed. In addition to the equations discussed above, a three-dimensional space where the formulas are valid is deduced. A simple geometric approach is given to perform this task.


## 5. Analysis for the Workspace of a Humanoid Step

For the direct kinematics problem, the existence, uniqueness, and stability of the solution are guaranteed; in the case of an inverse problem, it may be difficult to obtain a solution because these conditions are not guaranteed [27]. In this work, the existence of the solution for a humanoid robot when it executes a step is ensured.For a small-sized humanoid robot, one way to avoid singularities when performing a step is to maintain the CoG at a certain height. This can be achieved by lowering the height of the hips. Although this is an intuitive action for the humanoid walking, in this work the previous forward and reversed kinematics are used to produce a simple geometrical solution.Here, the coordinates of the swing foot,x
s and y
s, that have been used in the reversed analysis in (15) lay on a plane that is parallel to the ground, which intersects the ankles. The coordinate z has been determined from (5) from the forward analysis, that is, the distance from the ankle joints to the hip joints. The step size s is given by (see Figure 8)(26)
s
=
x
s
2
+
y
s
2
.Figure 8
Geometric analysis for the size of a step.The distancez, the thigh, the tibia, and the step size s form a triangle. Furthermore, z is restricted to the condition given by(27)
z
⩽
l
tb
+
l
tg
2
-
s
2
.Figure9 shows the theoretical plot of the maximum displacement z that the robot must satisfy in order to produce a step size s, that is, the step workspace without considering the mechanical joint limits. It can be seen that the longer the step is desired to be, the lower the hips must be. Taking into account the joint limitations, the practical workspace is shown in Figure 10, where there is an important limitation at the lateral displacements. In both workspace graphs, there is no dexterous workspace because only one possible orientation for the robot is assumed [28], where the foot soles remain parallel to the ground and the torso remains upright.Figure 9
Maximum theoretical distancez for a desired step size.Figure 10
Maximum practical distancez for a desired step size.In the next section, the joint values for the ankles, the knee, and the hips regarding the forward and reverse equations are loaded into a real NAO robot to validate that the support and swing coordinates are effectively reached.


## 6. Experimental Results

The validation of the formulas is accomplished by executing three basic motions: (1) Forward walking (see Figure11), (2) Lateral walking (see Figure 12) and (3) Turn-in-place motion (see Figure 13). Given a desired position for the hip (x, y, z) and a desired position for the ankle (x
s, y
s, z
s), the results of the inverse kinematics, that is, the joint values for the ankle (θ
ap and θ
ar), the knee (θ
kp) and the hip (θ
hp and θ
hr) are loaded into the real platform. These experiments are intended to validate that the robot effectively reaches the desired positions at the support and swing phases. Here, the positions for the hip and the ankle joints define a walking posture. A sequence of walking postures is executed to form a quasi-static walking [29]; that is, the position of the CoG is projected within the support polygon at all times. The torso remains upright and the foot soles are parallel to the ground at all walking postures to help maintain balance. It is assumed that the transmission between the motors and the corresponding joints is completely rigid [30].Figure 11
Forward walking postures.Figure 12
Lateral walking postures.Figure 13
Turn-in-place postures.At these experimental results, the support equations or the swing equations are used depending on whether it is desired to move the hip or the foot. For every posture used for walking, the end-effector coordinates are written only when they change. Each posture that is presented differs from the rest although there are symmetric postures. Each table describes only one complete step.The end-effector coordinates for the forward walking are presented in Table6, and their corresponding joint angles are shown in Table 7. For the lateral walking, the end-effector coordinates are presented in Table 8, and their corresponding joint angles are shown in Table 9. Regarding the turn-in-place rotation, the movement is performed counterclockwise, the end-effector coordinates are presented in Table 10, and the corresponding joint angles are shown Table 11.Table 6
Forward walking end-effector coordinates (mm).
Stage
R
x
R
y
R
z
L
x
L
y
L
z
R
x
s
R
y
s
R
z
s
L
x
s
L
y
s
L
z
s
Description
0
0
0
185.75
0
0
185.75
Bends down
1
0
62
185.75
0
62
185.75
Moves to left
2
50
−62
−155.85
Moves the right foot forward
3
50
−62
−185.75
Lands the right foot
4
0
−62
185.75
50
−62
185.75
Moves hips to the right and forward
5
−50
62
−155.75
Lifts the left foot from behind
6
50
62
−155.75
Moves the lifted left foot forward
7
50
62
−185.75
Lands the left foot
8
50
62
185.75
0
62
185.75
Moves hips to the left and forward
9
−50
−62
−155.75
Lifts the right foot from behind
10
50
−62
−155.75
Moves the lifted right foot forward
*
Repeat stages 3, 4, and 5
11
0
62
−155.75
Aligns the left foot
12
0
62
−185.75
Lands the aligned left footTable 7
Forward walking angles (rad).
Stage
R
θ
ar
L
θ
ar
R
θ
ap
L
θ
ap
R
θ
kp
L
θ
kp
R
θ
hp
L
θ
hp
R
θ
hr
L
θ
hr
0
0.0
0.0
−0.406
−0.406
0.824
0.824
−0.418
−0.418
0.0
0.0
1
0.322
0.322
−0.258
−0.258
0.524
0.524
−0.265
−0.265
−0.322
−0.322
2
0.378
0.322
−0.232
−0.258
1.060
0.524
−0.827
−0.265
−0.378
−0.322
3
0.322
0.322
0.171
−0.258
0.159
0.524
−0.330
−0.265
−0.322
−0.322
4
−0.322
−0.322
−0.258
−0.328
0.524
0.159
−0.265
0.169
0.322
0.322
5
−0.322
−0.378
−0.258
−0.811
0.524
1.060
−0.265
−0.248
0.322
0.378
6
−0.322
−0.378
−0.258
−0.232
0.524
1.060
−0.265
−0.827
0.322
0.378
7
−0.322
−0.322
−0.258
0.171
0.524
0.159
−0.265
−0.330
0.322
0.322
8
0.322
0.322
−0.328
−0.258
0.159
0.524
0.169
−0.265
−0.322
−0.322
9
0.378
0.322
−0.811
−0.258
1.060
0.524
−0.248
−0.265
−0.378
−0.322
10
0.378
0.322
−0.232
−0.258
1.060
0.524
−0.827
−0.265
−0.378
−0.322
11
−0.322
−0.378
−0.258
−0.588
0.524
1.194
−0.265
−0.606
0.322
0.378
12
−0.322
−0.322
−0.258
−0.258
0.524
0.524
−0.265
−0.265
0.322
0.322Table 8
Lateral walking end-effector coordinates (mm).
Stage
R
x
R
y
R
z
L
x
L
y
L
z
R
x
s
R
y
s
R
z
s
L
x
s
L
y
s
L
z
s
Description
0
0
0
188.75
0
0
188.75
Bends down
1
0
62
188.75
0
62
188.75
Moves to left
2
0
−62
−176.75
Lifts the right foot
3
0
−45
−188.75
Lands the right foot
4
0
−62
188.75
0
−45
188.75
Moves hips to the right
5
−50
72
−176.75
Lifts left foot
6
0
72
−188.75
Lands the left foot
7
0
72
188.75
0
62
188.75
Moves hips to the left
8
0
−72
−176.75
Lifts the right foot
*
Repeat stages 3 and 4
9
0
62
−176.75
Aligns the left foot
10
0
−62
188.75
Moves to rightTable 9
Lateral walking angles (rad).
Stage
R
θ
ar
L
θ
ar
R
θ
ap
L
θ
ap
R
θ
kp
L
θ
kp
R
θ
hp
L
θ
hp
R
θ
hr
L
θ
hr
0
0.0
0.0
−0.368
−0.368
0.747
0.747
−0.379
−0.379
0.0
0.0
1
0.317
0.317
−0.198
−0.198
0.401
0.401
−0.203
−0.203
−0.317
−0.317
2
0.337
0.317
−0.385
−0.198
0.785
0.401
−0.398
−0.203
−0.337
−0.317
3
0.234
0.317
−0.290
−0.198
0.588
0.401
−0.298
−0.203
−0.234
−0.317
4
−0.317
−0.234
−0.198
−0.290
0.401
0.588
−0.203
−0.298
0.317
0.234
5
−0.317
−0.386
−0.198
−0.339
0.401
0.688
−0.203
−0.349
0.317
0.386
6
−0.317
−0.364
−0.198
−0.083
0.401
0.170
−0.203
−0.086
0.317
0.364
7
0.364
0.317
−0.083
−0.198
0.170
0.401
−0.086
−0.203
−0.364
−0.317
8
0.386
0.317
−0.339
−0.198
0.688
0.401
−0.349
−0.203
−0.386
−0.317
9
−0.317
−0.337
−0.198
−0.387
0.401
0.785
−0.203
−0.398
0.317
0.337
10
−0.317
−0.317
−0.198
−0.198
0.401
0.401
−0.203
−0.203
0.317
0.317Table 10
Turn-in-place coordinates (mm).
Stage
R
x
R
y
R
z
L
x
L
y
L
z
R
x
s
R
y
s
R
z
s
L
x
s
L
y
s
L
z
s
Description
0
0
0
191.75
0
0
191.75
Bends down
1
0
−62
191.75
0
−62
191.75
Moves to right
2
0
62
−179.75
Lifts the left foot
3
−10.766
61.058
−179.75
Positions the left foot
4
Rotates the left foot1
0
° ccw
5
−10.766
61.058
−191.75
Lands the left foot
6
14.772
−59.396
191.75
15
−62
191.75
Moves the hips forward
7
4.005
63.662
191.75
15
62
191.75
Moves hips to the left
8
−4.005
63.662
−179.75
Lifts the right foot
9
0
62
191.75
0
−62
−173.75
Aligns the right foot
10
0
−62
−191.75
Lands the right footTable 11
Turn-in-place angles (rad).
Stage
R
θ
ar
L
θ
ar
R
θ
ap
L
θ
apc
R
θ
kp
L
θ
kp
R
θ
hpc
L
θ
hp
R
θ
hr
L
θ
hr
L
θ
hy
0
0.0
0.0
−0.326
−0.326
0.661
0.661
−0.335
−0.335
0.0
0.0
0.0
1
−0.312
−0.312
−0.108
−0.108
0.220
0.220
−0.111
−0.111
0.312
0.312
0.0
2
−0.312
−0.332
−0.108
−0.349
0.220
0.709
−0.111
−0.359
0.312
0.332
0.0
3
−0.312
−0.327
−0.108
−0.406
0.220
0.709
−0.111
−0.302
0.312
0.327
0.0
4
−0.312
−0.327
−0.108
−0.406 + 0.174
0.220
0.709
−0.111 + 0.174
−0.302
0.312
0.327
−0.248
5
−0.312
−0.308
−0.108
−0.161 + 0.174
0.220
0.220
−0.111 + 0.174
−0.058
0.312
0.308
−0.248
6
−0.300
−0.312
−0.192
−0.154 + 0.174
0.239
0.162
−0.046 + 0.174
−0.008
0.300
0.312
−0.248
7
0.320
0.312
−0.099
−0.154 + 0.174
0.162
0.162
−0.062 + 0.174
−0.008
−0.320
−0.312
−0.248
8
0.340
0.312
−0.362
−0.154 + 0.174
0.692
0.162
−0.329 + 0.174
−0.008
−0.340
−0.312
−0.248
9
0.342
0.312
−0.421
−0.108
0.855
0.220
−0.434
−0.111
−0.342
−0.312
0.0
10
0.312
0.312
−0.108
−0.108
0.220
0.220
−0.111
−0.111
−0.312
−0.312
0.0Some observations can be outlined from the basic motions; regarding the forward walking, a step of 50 mm forwards was tested. However, although longer steps were tested, the temperature of the joints involved increased; therefore the workspace of the humanoid step is limited by the joint performance. Regarding the lateral walking, the mechanical range for the ankle roll jointθ
ar (see Figure 10) limits the size of the lateral step. Also, only small motions must be considered to avoid knocking one leg against the other. For the turn-in-place motion, the orientation formulas given by (20), (24), and (25) allow the robot to rotate the swing leg, but the position must be considered (see Figure 7) to achieve an effective motion. As in the lateral walking, small motions must be considered to avoid knocking one leg against the other. For clarity, the compensating angles and the compensated angles were written separately (see Table 11).


## 7. Conclusions and Future Work

This work has presented an analytical, omnidirectional, and closed-form solution for the humanoid lower limb kinematics for the NAO robot. Although other works have also studied the kinematics of the upper body limbs, the lower limb kinematics is crucial to maintain the balance of the robot. The results for the lower limbs presented here are complete, brief, and explicit closed-form solutions for the hip and the swing foot. The paper determined the forward and inverse kinematics for the swing phase and the support phase for the robot NAO by detaching the position and orientation DH matrices. This method avoided the matrix inversion operations that add mathematical complexity to the equations. The inclusion of the joint order used in the geometrical analysis within the DH procedure helped to compact the position equations analytically. Furthermore, the DH parameters were modified in order to include the shared hip joint with the no-motion constraints, which allowed the decoupled functions for the turn-in-place rotation to be obtained. Also, the insertion of the constant coordinate systems, aligned with the navigation coordinates, allowed the orientation functions to be obtained in a simple manner in order to keep the torso upright and the feet soles parallel to the ground. Finally, the analysis of the workspace of a humanoid step allowed the gait singularities to be avoided using a simple geometric approach. As future work it is proposed to study a new inverse kinematics procedure to make the robot walk with the support knees completely extended, where a better energy efficiency and human-like walking postures are to be developed for commercial humanoid robots. Also, new trajectory planning and new equilibrium control mechanisms will be explored.



---

*Source: 437979-2015-03-26.xml*