Decoupled Closed-Form Solution for Humanoid Lower Limb Kinematics

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 avoidmatrix 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.


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: Section 2 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.

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 -axis points to the forward walking direction; the -axis points to the left side of the robot; and the -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 -axis at each hip.The used DH transformation matrix structure can be found in [19] and is given by the sequence of matrix rotations and translations given by i−1 T i =   ( −1 )   ( −1 )   (  )   (  ) . ( The result of the previous matrix multiplications is given by where  and  are used as abbreviations for the cosine and sine functions, respectively.

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 2 includes the limb dimensional parameters of the humanoid robot, where  ft ,  tb , and  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  is added to the coordinates to indicate that they belong to the swing leg [22].

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 Figure 2 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.
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 endeffector position.The distance  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  unit vector along every actuator points in the direction of a positive rotation Table 3: Link parameters used for the forward position analysis.
/2 0 /2 0 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.
The DH parameter table used for the forward position analysis is shown in Table 3.
Using the parameters presented in Table 3, the position coordinates of the hip can be extracted from the fourth column of the DH matrix concatenation leading to where , , and , 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, Squaring and adding (3), (4), and ( 5) results in Solving for the knee pitch angle  kp gives Dividing (3) by the square root of the sum of the squares of ( 4) and (5) Solving for the ankle pitch angle  ap gives 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.
The DH parameters for the forward orientation analysis are shown in Table 4.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.
Applying the parameters of Table 3 and the parameters of Table 4, the transformation 0 T 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 from 0 T 9 must be equal to one; that is, 0 R 9 = .Considering that the constant coordinate system 5 T 6 (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 Furthermore, the hip pitch angle  hp can be expressed as By using the following transformation matrices: The hip roll angle  hr can be obtained as 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.

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 -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   ,   ,   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.
Solving for the position joint variables results in Solving for the orientation joint variables  ap and  ar results in 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.

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 -axis, when only a rotation about the -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 -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.
Table 5 presents the link parameters, including the hip yaw-pitch joint (see Figure 6).Note that the transformation 8 T 9 , presented previously, has been modified from the forward analysis to include the hip yaw-pitch into the kinematic chain.
The premultiplication of the R 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 yawpitch 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 where Making R x R y R z = 0 R 12 leads to where   represents the turn-in-place rotation.
The equation  hy is solved in order to determine the yaw rotation to be performed by the robot The following operation is carried out in order to correct the undesired pitch rotation The following consideration is made to remove the pitch rotation when the hip yaw-pitch is activated tan   = 0. ( Equation ( 22) ensures that   = 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 Thus, the compensated pitch angle  hpc for a turn-in-place rotation is 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.
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): 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.

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,   and   , 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  has been determined from (5) from the forward analysis, that is, the distance from the ankle joints to the hip joints.The step size  is given by (see Figure 8) The distance , the thigh, the tibia, and the step size  form a triangle.Furthermore,  is restricted to the condition given by Figure 9 shows the theoretical plot of the maximum displacement  that the robot must satisfy in order to produce a step size , 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.
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.

Experimental Results
The validation of the formulas is accomplished by executing three basic motions: (1) Forward walking (see Figure 11), (2) Lateral walking (see Figure 12) and (3) Turn-in-place motion (see Figure 13).Given a desired position for the hip (, , ) and a desired position for the ankle (  ,   ,   ), 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].
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 Table 6, 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.Some 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).

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

Figure 2 :
Figure 2: Forward position analysis of the humanoid leg.

Figure 3 :
Figure 3: Sequence of joint activations based on the triangular geometry.

Figure 4 :
Figure 4: Forward orientation analysis of the humanoid leg.

Figure 5 :
Figure 5: Reversed position and orientation analysis of the humanoid leg.

Figure 7 :
Figure 7: Position and orientation of the robot feet at the turn-inplace rotation.

Figure 8 :
Figure 8: Geometric analysis for the size of a step.

Figure 9 :Figure 10 :
Figure 9: Maximum theoretical distance  for a desired step size.

Table 1 :
Used joint range of motions.

Table 4 :
Link parameters used for the forward orientation analysis.

Table 5 :
Link parameters used of a turn-place-movement.