# Humanoid Walking Robot: Modeling, Inverse Dynamics, and Gain Scheduling Control

Authors: Elvedin Kljuno; Robert L. Williams
Journal: Journal of Robotics
 (2010)

---

## Abstract

This article presents reference-model-based control design for a 10 degree-of-freedom bipedal walking robot, using nonlinear gain scheduling. The main goal is to show concentrated mass models can be used for prediction of the required joint torques for a bipedal walking robot. Relatively complicated architecture, high DOF, and balancing requirements make the control task of these robots difficult. Although linear control techniques can be used to control bipedal robots, nonlinear control is necessary for better performance. The emphasis of this work is to show that the reference model can be a bipedal walking model with concentrated mass at the center of gravity, which removes the problems related to design of a pseudo-inverse system. Another significance of this approach is the reduced calculation requirements due to the simplified procedure of nominal joint torques calculation. Kinematic and dynamic analysis is discussed including results for joint torques and ground force necessary to implement a prescribed walking motion. This analysis is accompanied by a comparison with experimental data. An inverse plant and a tracking error linearization-based controller design approach is described. We propose a novel combination of a nonlinear gain scheduling with a concentrated mass model for the MIMO bipedal robot system.


---

## Body

## 1. Introduction

Bipedal robots have been one of the central topics for relatively long time. In 1970s Honda started wide research on bipedal robots, which was followed by numerous research project performed by mainly Japan’s companies and universities. Even though the research topic is relatively old, still, the technology maturity has not been reached and many aspects of the bipedal walking robotics need to be improved. This fact indicates that the bipedal robots overall design is a very complex task. This is particularly the case with the control aspect of the robots. Very high trajectory tracking precision is generally required in order to perform balancing and achieve sufficiently high motion speeds. In order to achieve high performances of the control system, advanced hardware implementations have internal model of their kinematics and dynamics embedded into the main digital computer system.The internal model that runs simultaneously with the robot takes the same information about the desired robot motion as the real hardware and based on the dynamical model of the system, it calculates the required nominal control inputs that are needed to realize the desired joint trajectories.It is clear that the performance of the robot depends very much on the accuracy of the model, that is, how well the model represents the real-world system. This is the reason that many dynamical models have been developed for the bipedal robots. The next couple of sections give an overview of those models with the discussion about the advantages and disadvantages.Modeling of human body walk has been evolved from simple models such as, an inverted pendulum model [1–3] and a mass-spring model [4, 5] to relatively complicated models that include relatively high number of degrees of freedom [6–10]. Primary goal of those models is to predict the internal and external forces during a regular walking cycle.Detailed human body models can include calculations of the most important muscle forces for particular types of motion. However, for a robot that is actuated via rotational motors, calculation of particular muscle forces is not needed, but the moments they produce about the corresponding joints. There are two main reasons for the calculation of the joint torques.(a)
Based on a walking model, one can predict maximum torques that are necessary to generate particular motion of a robotic structure. The maximum torque and maximum power are necessary data to choose joint actuators. The procedure of the actuators (motors) selection based on the robot dynamic model is inherently iterative, since the mass distribution and consequently, the static and dynamic forces are significantly dependent on the sizes and locations of the chosen actuators.(b)
Torques at relevant joints for particular motion, calculated based on a walking model, can be used to generate nominal control trajectories for a complex walking robotic architecture. Those nominal trajectories can be calculated offline or in real-time using an inverse plant model. The advantage of the later is that the reference (desired) trajectories can be changed online depending on the conditions imposed by environment rather than relaying on the precalculated reference trajectories. This is essential for obstacle avoidance, which requires that the reference trajectories are adjusted accordingly.Inclusion of more degrees of freedom in a walking robot model normally leads to more precise results, but also it leads to more efforts needed to understand the process and what is happening with internal variables in the robotic structure. Following sections give an overview of the simplified models and general features of a more complex model.


### 1.1. Definitions and Notation

Bipedal walking robotics related scientific reports regularly include numerous terms and definitions that are specific for this area. The majority of those terms are uniquely defined and generally accepted by researchers in the area; however, several key terms are still in the process of clarification and are topics of research debates and analyses. Moreover, a particular definition in a paper, such as the definition of Zero Moment Point (ZMP), might significantly deviate from the corresponding definition in another paper. Since this paper also uses some of those terms, it is appropriate to give the most important definitions.A very important control concept in the field of bipedal robotics is the concept of, so-called, “Zero Moment Point” or shortly ZMP. This is a point at the foot/ground contact area where, for a dynamically stable walk, the sum of all moments about any axis laying in the tangential plane of the contact equals zero, including moments due to external forces and due to inertial forces acting on the walking robot architecture. The concept was introduced by Vukobratovic et al. in 1969 [11].Although the concept has been in application for bipedal robots control tasks for more than 30 years, another concept related to the same problem has been introduced by Goswami in 1999 [12], so-called, “Foot Rotation Indicator” or shortly FRI, with intention of ZMP concept generalization and extension of its applicability.  Foot Rotation Indicator is a point where the total ground reaction should act such that the supporting foot stays parallel to the ground, without any rotation.A bipedal robot loses stability if FRI point leaves the foot ground contact area and, similarly, if ZMP reaches the foot edge, the robot walk is marginally stable or unstable. Although the definition of ZMP is more general, our discussion within this paper will consider a stable walking cycle where the ZMP will be considered as the center of pressure (CoP) at the foot/ground contact area.Finally, during the double support phase, when both legs are in the contact with the ground, CoP and ZMP can be defined by combining two centers of pressure CoP1 and CoP2, corresponding to separate ground contact areas.These definitions and abbreviations will be used in the following sections to describe the models with concentrated and models with distributed mass, as well as in the controller design section.


### 1.2. Models with Concentrated Mass

The group of relatively simple walking models is based on the inverted pendulum structure with variations combining a spring or two springs and dampers. The simple models include one or two variables with overall mass concentrated into a point, a center of mass (COM).Six Determinants model [1] has been used in the past to analyze the human gait cycle. Due to inconsistency with theoretical and experimental results, this model has been replaced by Inverted Pendulum model, which gives results much closer to those obtained experimentally. One of the major inconsistencies of the regular Inverted Pendulum model results with the experimental results is the zero energy input, which means that the model does not consider the energy lost during the gait cycle.A more advanced, but still one of the simplest models for human gait analysis is Mass and Spring Inverted Pendulum (MSIP) model [4]. The point mass is equal to the total mass of the body concentrated into the center of gravity. The spring connects the ground contact point (the center of pressure CP) and the center of gravity (CG), and its deflections include all changes of the distance between the CP and the CG points due to flexions/extensions of the hip,  knee, and ankle joints. Figure 1 shows the concept of representing a complex human body (or a bipedal robot) by a mass and spring model.Mass and spring inverted pendulum walking gait representation: (a) single support, (b) double support represented by two springs, and (c) double support represented by a single spring.
(a)(b)(c)The spring connects the center of pressure (CP) and the center of mass (COM) (a). The COM point can be considered as very close to hips since its position varies relatively close to the hip joint during the regular walking cycle. Double support period (b) can be modeled using mass and two springs connecting the COM and centers of pressures CP1 and CP2. However, double support period is relatively short compared to the duration of the gait cycle and frequently can be considered as instantaneous.Besides this case, a single spring model can cover a double support period in the way of interpreting the spring-ground connection as a zero moment (ZM) point (c). In other words, the total moment ofFy1 and Fy2 forces for ZM point is equal to zero (about any axis lying in the tangential plane of the contact with the ground). A state space representation of the simple mass-spring inverted pendulum, shown in Figure 2, is
(1)[ẋ1ẋ2ẋ3ẋ4]=[ṙr̈θ̇θ̈]=[x2x1x42-gcos(x3)+km(l0-x1)-bmx2x4(gsin(x3)-2x2x4)x1+τmx12],
where x1 represents the radial distance of the COM from the ground contact point, is the radial velocity, ẋ2 is radial acceleration, x3 is the angle of the spring axis with respect to the vertical line, x4 is the angular velocity of the link between the ground contact and the COM, which is approximately equal to the angular velocity of the stance leg during the single support phase of the walking, and ẋ4 is the corresponding angular acceleration.Figure 2
Mass-spring inverted pendulum model.The state space differential equation (1) can be used to solve for the trajectory of the center of mass, as well as the ground reaction and the spring forces.  Roughly speaking, the trajectory obtained using this model approximates the trajectory of the real bipedal system center of mass. Similarly, force plate measurements [13] show that the ground reaction force of a real bipedal system can be fairly approximated by the force calculated using (1).Besides use of this model for studying different parameters of the walking cycle mechanical energy optimization by biological systems, the significance of this result is reflected in the fact that this model with concentrated mass can be used as an approximate reference model for complex robotic bipedal systems control.The idea is to construct a mathematical model based on the equation set (1) to calculate the reaction force repeatedly at specified time steps and use that result to calculate required torques for a complex architecture bipedal robotic system such that total reaction force is equal to the one obtained by the concentrated mass model. The block diagram of this control system concept is shown in Figure 3.  One of the main advantages of this reference model is the possibility for dynamic system inversion without facing the issues of the inverse system stabilization [14].Figure 3
Control system based on the torque prediction using the concentrated mass reference model,τnom is the nominal torque vector calculated based on the ground force provided by the reference model (mass-spring model), τcorr is the correction calculated based on the measurements of the robot joints angles and acceleration vector, and τ is the commanded torque signal that is sent to the actuators.Namely, the main problem of using dynamic inversion system to calculate the nominal control input vector is the difficulty of designing the exact causal inversion resulting in design of a pseudoinverse system that very often needs to be stabilized. In this case of the inverse system, when the model with concentrated mass is used for nominal input prediction, there is no need for stabilization.The model includes damping represented by the coefficientb and a torque τ for the case when the center of pressure at a foot is not taken as a spring-ground contact point. The ground contact force that occurs during human walking can be predicted using the mass-spring model.Potential energy accumulated in the spring and the gravitational potential energy interchange with the kinetic energy of the point mass. A portion of the mechanical energy is lost during the walking half-cycle due to the inelastic collision of the foot with ground [15], and the rest of the accumulated energy continues to interchange between kinetic and potential energy in the next half-cycle which is described by the percentage of recovery parameter [16].Although those models are very simplified representations of the real human body anatomy, they provide very convenient way to interpret and analyze majority of relevant parameters of the gait cycle. Integral features of the human gait cycle, such as, the overall kinetic energy, potential energy, angular momentum with respect to the ground contact point, center of mass trajectory and laws of motion, and ground force in sagittal plane, can be considered using a simplified model (e.g., MSIP).Using the state space model (1), the ground force can be obtained (Figure 4) and can be used to qualitatively predict the real ground reaction force which occurs during bipedal structures walking.Figure 4
Ground reaction force (vertical component) obtained using the mass-spring inverted pendulum model (including a damper) compared to the real ground reaction force occurring during a single leg stance phase, measured using force plates [13].Besides the ground reaction force, the mass-spring model can predict qualitatively potential, kinetic and total mechanical energy of walking. Figure5 shows the energy change during walking, predicted by the mass-spring model.Figure 5
Potential, kinetic, and total mechanical energy change (relative energy with respect to the initial energy level).


### 1.3. Models with Distributed Mass and Multiple Degrees of Freedom

Complex bipedal walking models and their practical implementation bipedal robots are generally based on the human body anatomy [6–8]. However, all those models and implementations include fewer degrees of freedom (DOF) of motion than the DOF existing in the human body. The spinal region of the body has relatively high number of freedom. The motion within the spinal region influences the walking cycle behavior, but this motion can be neglected and majority of the models with distributed mass represent the trunk region as a rigid body or as two rigid bodies with a single revolute joint in the trunk. Motion of the system within the human body (approximate) symmetry plane is called the sagittal plane motion. Dominant accelerations of the body parts centers of gravity occur parallel to the sagittal plane which results in dominant inertial forces and moments due to the motion parallel to this plane. Unless there is a sharp change in the walking direction, relatively negligible inertial forces occur due to the accelerations perpendicular to this plane at every instant of the walking cycle.Due to the dominance of the inertial forces parallel to the sagittal plane and for the sake of the model simplification, the analysis and control design shown in this paper is restricted on the planar motion of the model/bipedal robot.


## 1.1. Definitions and Notation

Bipedal walking robotics related scientific reports regularly include numerous terms and definitions that are specific for this area. The majority of those terms are uniquely defined and generally accepted by researchers in the area; however, several key terms are still in the process of clarification and are topics of research debates and analyses. Moreover, a particular definition in a paper, such as the definition of Zero Moment Point (ZMP), might significantly deviate from the corresponding definition in another paper. Since this paper also uses some of those terms, it is appropriate to give the most important definitions.A very important control concept in the field of bipedal robotics is the concept of, so-called, “Zero Moment Point” or shortly ZMP. This is a point at the foot/ground contact area where, for a dynamically stable walk, the sum of all moments about any axis laying in the tangential plane of the contact equals zero, including moments due to external forces and due to inertial forces acting on the walking robot architecture. The concept was introduced by Vukobratovic et al. in 1969 [11].Although the concept has been in application for bipedal robots control tasks for more than 30 years, another concept related to the same problem has been introduced by Goswami in 1999 [12], so-called, “Foot Rotation Indicator” or shortly FRI, with intention of ZMP concept generalization and extension of its applicability.  Foot Rotation Indicator is a point where the total ground reaction should act such that the supporting foot stays parallel to the ground, without any rotation.A bipedal robot loses stability if FRI point leaves the foot ground contact area and, similarly, if ZMP reaches the foot edge, the robot walk is marginally stable or unstable. Although the definition of ZMP is more general, our discussion within this paper will consider a stable walking cycle where the ZMP will be considered as the center of pressure (CoP) at the foot/ground contact area.Finally, during the double support phase, when both legs are in the contact with the ground, CoP and ZMP can be defined by combining two centers of pressure CoP1 and CoP2, corresponding to separate ground contact areas.These definitions and abbreviations will be used in the following sections to describe the models with concentrated and models with distributed mass, as well as in the controller design section.


## 1.2. Models with Concentrated Mass

The group of relatively simple walking models is based on the inverted pendulum structure with variations combining a spring or two springs and dampers. The simple models include one or two variables with overall mass concentrated into a point, a center of mass (COM).Six Determinants model [1] has been used in the past to analyze the human gait cycle. Due to inconsistency with theoretical and experimental results, this model has been replaced by Inverted Pendulum model, which gives results much closer to those obtained experimentally. One of the major inconsistencies of the regular Inverted Pendulum model results with the experimental results is the zero energy input, which means that the model does not consider the energy lost during the gait cycle.A more advanced, but still one of the simplest models for human gait analysis is Mass and Spring Inverted Pendulum (MSIP) model [4]. The point mass is equal to the total mass of the body concentrated into the center of gravity. The spring connects the ground contact point (the center of pressure CP) and the center of gravity (CG), and its deflections include all changes of the distance between the CP and the CG points due to flexions/extensions of the hip,  knee, and ankle joints. Figure 1 shows the concept of representing a complex human body (or a bipedal robot) by a mass and spring model.Mass and spring inverted pendulum walking gait representation: (a) single support, (b) double support represented by two springs, and (c) double support represented by a single spring.
(a)(b)(c)The spring connects the center of pressure (CP) and the center of mass (COM) (a). The COM point can be considered as very close to hips since its position varies relatively close to the hip joint during the regular walking cycle. Double support period (b) can be modeled using mass and two springs connecting the COM and centers of pressures CP1 and CP2. However, double support period is relatively short compared to the duration of the gait cycle and frequently can be considered as instantaneous.Besides this case, a single spring model can cover a double support period in the way of interpreting the spring-ground connection as a zero moment (ZM) point (c). In other words, the total moment ofFy1 and Fy2 forces for ZM point is equal to zero (about any axis lying in the tangential plane of the contact with the ground). A state space representation of the simple mass-spring inverted pendulum, shown in Figure 2, is
(1)[ẋ1ẋ2ẋ3ẋ4]=[ṙr̈θ̇θ̈]=[x2x1x42-gcos(x3)+km(l0-x1)-bmx2x4(gsin(x3)-2x2x4)x1+τmx12],
where x1 represents the radial distance of the COM from the ground contact point, is the radial velocity, ẋ2 is radial acceleration, x3 is the angle of the spring axis with respect to the vertical line, x4 is the angular velocity of the link between the ground contact and the COM, which is approximately equal to the angular velocity of the stance leg during the single support phase of the walking, and ẋ4 is the corresponding angular acceleration.Figure 2
Mass-spring inverted pendulum model.The state space differential equation (1) can be used to solve for the trajectory of the center of mass, as well as the ground reaction and the spring forces.  Roughly speaking, the trajectory obtained using this model approximates the trajectory of the real bipedal system center of mass. Similarly, force plate measurements [13] show that the ground reaction force of a real bipedal system can be fairly approximated by the force calculated using (1).Besides use of this model for studying different parameters of the walking cycle mechanical energy optimization by biological systems, the significance of this result is reflected in the fact that this model with concentrated mass can be used as an approximate reference model for complex robotic bipedal systems control.The idea is to construct a mathematical model based on the equation set (1) to calculate the reaction force repeatedly at specified time steps and use that result to calculate required torques for a complex architecture bipedal robotic system such that total reaction force is equal to the one obtained by the concentrated mass model. The block diagram of this control system concept is shown in Figure 3.  One of the main advantages of this reference model is the possibility for dynamic system inversion without facing the issues of the inverse system stabilization [14].Figure 3
Control system based on the torque prediction using the concentrated mass reference model,τnom is the nominal torque vector calculated based on the ground force provided by the reference model (mass-spring model), τcorr is the correction calculated based on the measurements of the robot joints angles and acceleration vector, and τ is the commanded torque signal that is sent to the actuators.Namely, the main problem of using dynamic inversion system to calculate the nominal control input vector is the difficulty of designing the exact causal inversion resulting in design of a pseudoinverse system that very often needs to be stabilized. In this case of the inverse system, when the model with concentrated mass is used for nominal input prediction, there is no need for stabilization.The model includes damping represented by the coefficientb and a torque τ for the case when the center of pressure at a foot is not taken as a spring-ground contact point. The ground contact force that occurs during human walking can be predicted using the mass-spring model.Potential energy accumulated in the spring and the gravitational potential energy interchange with the kinetic energy of the point mass. A portion of the mechanical energy is lost during the walking half-cycle due to the inelastic collision of the foot with ground [15], and the rest of the accumulated energy continues to interchange between kinetic and potential energy in the next half-cycle which is described by the percentage of recovery parameter [16].Although those models are very simplified representations of the real human body anatomy, they provide very convenient way to interpret and analyze majority of relevant parameters of the gait cycle. Integral features of the human gait cycle, such as, the overall kinetic energy, potential energy, angular momentum with respect to the ground contact point, center of mass trajectory and laws of motion, and ground force in sagittal plane, can be considered using a simplified model (e.g., MSIP).Using the state space model (1), the ground force can be obtained (Figure 4) and can be used to qualitatively predict the real ground reaction force which occurs during bipedal structures walking.Figure 4
Ground reaction force (vertical component) obtained using the mass-spring inverted pendulum model (including a damper) compared to the real ground reaction force occurring during a single leg stance phase, measured using force plates [13].Besides the ground reaction force, the mass-spring model can predict qualitatively potential, kinetic and total mechanical energy of walking. Figure5 shows the energy change during walking, predicted by the mass-spring model.Figure 5
Potential, kinetic, and total mechanical energy change (relative energy with respect to the initial energy level).


## 1.3. Models with Distributed Mass and Multiple Degrees of Freedom

Complex bipedal walking models and their practical implementation bipedal robots are generally based on the human body anatomy [6–8]. However, all those models and implementations include fewer degrees of freedom (DOF) of motion than the DOF existing in the human body. The spinal region of the body has relatively high number of freedom. The motion within the spinal region influences the walking cycle behavior, but this motion can be neglected and majority of the models with distributed mass represent the trunk region as a rigid body or as two rigid bodies with a single revolute joint in the trunk. Motion of the system within the human body (approximate) symmetry plane is called the sagittal plane motion. Dominant accelerations of the body parts centers of gravity occur parallel to the sagittal plane which results in dominant inertial forces and moments due to the motion parallel to this plane. Unless there is a sharp change in the walking direction, relatively negligible inertial forces occur due to the accelerations perpendicular to this plane at every instant of the walking cycle.Due to the dominance of the inertial forces parallel to the sagittal plane and for the sake of the model simplification, the analysis and control design shown in this paper is restricted on the planar motion of the model/bipedal robot.


## 2. Bipedal Robot Model with 10 DOF

The model shown in Figure6 is a bipedal structure with 10 internal rotational degrees of freedom. Three additional degrees are an absolute (external) rotational degree (for the motion parallel to the sagittal plane), which is chosen to be the angle of the trunk with respect to the vertical direction, and two translational degrees with a reference point which is chosen to be at the hip joint. Human joints allow much more freedom than just a rotation about a single axis (a revolute joint). Generally, human ankle and hip joints can be considered as spherical (ball and socket) joints. The knee joint can be approximately considered as a revolute joint (one degree of freedom).Model with distributed mass: (a) absolute angles, (b) generalized coordinates (angles are relative joint coordinates).
(a)(b)However, the model shown in Figure6 includes only revolute joints with parallel axes, since the analysis is restricted on the sagittal plane motion. As described earlier, the spinal region, including the neck and head, is considered as a rigid body, since there is no significant bending during the gait cycle and, consequently, the joint trajectories and generalized forces are not affected significantly.External coordinates of the hip joint (xh and yh), as well as an additional external (absolute) angle, are dependent (assuming no-slip condition) on the other generalized coordinates (angles) of lower extremities within a walking cycle. However, in the case when a running phase is considered, those external coordinates become independent on the joint coordinates over the time interval when there is no contact with the ground and are necessary.Two types of coordinates are shown in Figure6: absolute and relative. Generally, it is easier to work with the absolute coordinates (angles) when we consider the mathematical description of the model’s behavior and response to the input torques and forces. However, very significant argument is on the side of relative coordinates and this is the sensors way of coordinate measurements. The joint sensors (e.g., encoders, potentiometers) measure relative angles, which are then directly used in a feedback fashion to guide and control the robot.A special attention in the model is given to the foot design (Figure7) in such a way that the transition phase between the walking cycles becomes smooth as much as possible. A walking robot flat foot design requires that the foot is always parallel to the ground during the stance phase, which is significantly different to the human walking stance phase. Although a flat foot design is commonly used in walking robotics, it causes certain discontinuities in the kinematic relations and sudden changes in the nominal generalized forces, required to mimic a human-like walking. Besides that, the stabilization of the walking cycle becomes difficult, since the contact is theoretically at the edge of the foot during the significant part of the cycle time. Consequently, the zero moment point (ZMP) is then tied to the foot edge, which complicates the control operations.Foot design: (a) two cylindrical surfaces with different radii, (b) ellipsoidal cylinder (ankle joint is marked byA).
(a)(b)


### 2.1. Kinematic and Dynamic Analysis of the Model

Forward kinematics gives Cartesian coordinates of any point of the structure for a given set of the joint and the external variables. The kinematic relations are necessary to close the set of differential equations that describes the robot dynamics.The forward kinematics of the robot model can be expressed as follows:(2)[PC1PC2⋮PC11]=f([q1q2⋯q10]T),
where PCi (i=1,…,11) are position vectors of the center of gravity of the ith segment and has the form [xCiyCi0]T.  Generalized coordinates in right-hand side of (2) can be the absolute angles of the segment and external coordinates, or they can represent the joint angles and external coordinates. Besides (2), complete kinematic analysis includes the first and second time derivative of (2). Since the expanded equations in (2) and its first two time derivatives are too long to be shown in this paper, they are omitted and we proceed with the dynamic analysis.Since the control law, which is discussed in the following section, is based on an inverse model and an error dynamics, it is necessary to analyze both, forward and inverse dynamic equations. The derivation of those equations is done using energy-based method-Lagrange equations. The inverse model has to provide nominal generalized forces (the joint torques), based on nominal joint angular trajectories, which is, by definition, the inverse dynamics.On the other hand, the error-based controller uses the differential equations of motion to generate a correction value/signal for the input torques. The differential equations are also needed for numerical simulation of the robot and evaluation of the controller performances. Figure8 shows a partial free-body diagram, which includes the right standing leg only.Figure 8
Planar partial free-body diagram for the right standing leg.Besides the reaction forces/torques from the rest of the system, the set of external forces/torques for the subsystem of the support leg during the single support phase includes the torques at the joints, since they perform work and influence the mechanical energy distribution. Such approach is convenient for application of the Lagrange method which provides the way to analyze the relevant forces/torques, without necessity to calculate the internal forces, such as the internal forces at the joints.Lagrange equations for the walking robot model have the following form:(3)ddt∂L(t,qi,q̇i)∂q̇r-∂L(t,qi,q̇i)∂qr+∂R(t,qi,q̇i)∂q̇r=Qrnc(t,qi,q̇i),
where L(t,qi,q̇i)=EK(t,qi,q̇i)-EP(t,qi) is the Lagrange function generally depending on time, generalized coordinates, and generalized velocities, R(t,qi,q̇i) is the dissipation (Rayleigh’s) function, and Qrnc(t,qi,q̇i) is the rth generalized force (calculated based on the work of nonconservative forces related to the rth gen. coordinate excluding friction forces). The dissipation function can be used to model the lost of energy at the instant of feet collision with the ground. The generalized forces are torques generated by motors at the robot joints.Kinetic energy of the structure is the sum of the kinetic energy for translational and rotational motion for each segment(4)EK=∑i=111(mivCi22+ICiωi22),
where vCi is the ith segment center of gravity velocity magnitude, ICi is the ith segment moment of inertia for the center of gravity Ci, and ωi is the angular velocity of the ith segment. The centers of gravity velocity can be expressed in terms of the joint angular velocities using the kinematic relations (2). The potential energy is basically the gravitational potential energy since no springs are included in the model.An alternative way to derive the model dynamics is to apply the combination of the Newton’s and Euler-Dalembert’s law for the translation and rotation, respectively. In either way, the following form of dynamic equations is obtained(5)[M(φ)][φ̈]+[C(φ,φ̇)][φ̇]+[G(φ)]=[D(φ)][τ],
where φ=[φ1φ2φ4φ5φ6]T is the vector of the absolute angles (the foot dynamics has been neglected), [M(φ)] is the matrix of inertia, [C(φ,φ̇)] represents cross-correlation of the segments angular velocities, [G(φ)] is the gravity influence vector, [τ] is the control torque vector, and  [D(φ)] represents the matrix of the control torques distribution.Although the equations are implemented in the simulation model for the full model (10 DOF), the equations and analysis that are shown here are for the reduced model, for which the arms are considered sufficiently light such that their dynamics is not significant for the system stability. Similarly, the dynamics of the swing leg foot is neglected and the mass is considered lumped at the ankle.The kinetic energy function for the model is(6)EK=12φ̇12(m2l1d2+I2+(2m3+m2′+m4)l12)+12φ̇22(m3l2d2+I3+(2m3+m2′+m4)l22)+12φ̇42(m2′l1u2+I2)+12φ̇52(m3l2u2+I3+m2′l22)+φ̇1φ̇2c21((m2′+m3+m4)l1l2+m3l1l2d)-φ̇1φ̇5c51(m3l1l2u+m2′l1l2)-φ̇2φ̇5c52(m3l1l2u+m2′l22)-φ̇1φ̇4c41m2′l1l1u-φ̇2φ̇4c42m2′l2l1u+φ̇4φ̇5c54m2′l2l1u+φ̇1φ̇6c61m4l1l3d+φ̇2φ̇6c62m4l2l3d+12φ̇62(m4l3d2+I4),
where cij=cos(φi-φj), (i,j=1,2,4,5,6), mi is the ith segment mass, m2′ is the foot-lower leg combined mass, liu and lid are the ith segment center of gravity position lengths with respect to the neighboring upper and lower joints, respectively,  li is the ith segment length, Ii is the ith segment moment of inertia with respect to the axis perpendicular to the sagittal plane.The potential energy function is(7)EPg=(m2l1d+(2m3+m4+m2′)l1)c1+(m3l2d+(m3+m4+m2′)l2)c2-m2′l1uc4-(m3l2u+m2′l2)c5+m4l3dc6,whereci=cos(φi),(i=1,2,4,5,6). The Rayleigh’s dissipation function reflects the energy lost due to the viscous friction in the joints and has the following form for the model:
(8)R=12(b1φ̇12+b2(φ̇2-φ̇1)2+b3(φ̇6-φ̇2)2)+12(b2(φ̇5-φ̇4)2+b3(φ̇6-φ̇5)2),
where bi (i=1,2,3) are the friction coefficients for the ankle, knee, and hip joints, respectively.The final equations in the form (5) are obtained using the energy-based method and the matrices/vectors denoted in (5) are as follows.The symmetric matrix of inertia is the following:(9)M(:,1)=[m2l1d2+I2+(2m3+m2′+m4)l12((m2′+m3+m4)l1l2+m3l1l2d)c21-m2′l1l1uc41-(m3l1l2u+m2′l1l2)c51m4l1l3dc61],M(:,2)=[((m2′+m3+m4)l1l2+m3l1l2d)c21m3l2d2+I3+(m3+m2′+m4)l22-m2′l2l1uc42-(m3l2l2u+m2′l22)c52m4l2l3dc62],M(:,3:5)=[-m2′l1l1uc41-m2′l2l1uc42m2′l1u2+I2m2′l2l1uc540|-(m3l1l2u+m2′l1l2)c51-(m3l2l2u+m2′l22)c52m2′l2l1uc54m3l2u2+I3+m2′l220|m4l1l3dc61m4l2l3dc6200I4+m4l3d2].The matrixC(φ,φ̇) is given in segments as follows:
(10)C(:,1)=[b1+b2-b2+((m2′+m3+m4)l1l2+m3l1l2d)s21φ̇1-m2′l1l1us41φ̇1-(m2′l1l2+m3l1l2u)s51φ̇1m4l1l3ds61φ̇1],C(:,2)=[-b2+((m2′+m3+m4)l1l2+m3l1l2d)s21φ̇2b2+b3-m2′l2l1us42φ̇2-(m2′l22+m3l2l2u)s52φ̇2-b3+m4l2l3ds62φ̇2],C(:,3)=[m2′l1l1us41φ̇4m2′l2l1us42φ̇4b2-b2+m2′l2l1us54φ̇40],C(:,4:5)=[(m2′l1l2+m3l1l2u)s51φ̇5(m2′l22+m3l2l2u)s52φ̇5-b2-m2′l2l1us54φ̇5b2+b3-b3|-m4l1l3ds61φ̇6-b3-m4l2l3ds62φ̇60-b32b3],
where sij=sin(φi-φj), (i,j=1,2,4,5,6).The gravity vectorG(φ) is as follows:
(11)G=[(m2l1d+(2m3+m4+m2′)l1)s1(m3l2d+(m3+m4+m2′)l2)s2-m2′l1us4-(m3l2u+m2′l2)s5m4l3ds6]g,
where si=sin(φi), (i=1,2,4,5,6), and g is the gravity acceleration.The torques distribution matrixD(φ) is, actually, a constant matrix as follows:
(12)D=[110000-1-10000010000-1-100101].
Finally, the joints torques vector is τ=[τ1τ2τ4τ5τ6]T, that has components corresponding to joints torques in the following order: right ankle, right knee, right hip, left knee, and left hip, respectively. Again, the swing leg ankle torque is neglected, as well as the dynamics of the swing leg foot.The equations of the form (5) will be used to derive the control law for each actuated joint of the robot model. This is shown in the following controller design section.


### 2.2. Controller Design

The controller design is based on a nonlinear system control approach. A combination of the inverse plant with modified gain scheduling is applied.


#### 2.2.1. Desired Robot Motion

Based on a typical human walking, the corresponding angular trajectories for the hip, knee and ankle joints can be recorded using cameras and markers positioned on human body segments. In this way, following trajectories are obtained and used for the desired robot motion (Figure9).Figure 9
The robot joint trajectories based on a human walking cycle (positive angles denote flexion/dorsiflexion).The figure shows that the trajectories do not vary on the left side of the interval as much as they vary on the rest of the walking cycle. The subinterval (the swing phase) contains relatively high derivatives and it is very important to determine precisely the toe off instant, since any variation the toe off instant would cause significant change in the nominal torques and the calculated ground reaction force.From the mechanical structure basis, there is a configuration change since the structure changes from the double support, which represents the structure with a closed loop, to the single support configuration, which is an open chain configuration.Therefore, when the configuration change occurs it triggers the control law change. Basically, the instant of this configuration change can be determined based on the ground force reaction which is measured using pressure pads positioned at the feet.The first two derivatives of the desired trajectories are shown in Figures10 and 11, respectively. Relatively high derivatives are noticeable in the swing phase of the cycle. The nominal trajectories recorded based on human motion usually have significant noise embedded due to errors during the measurements.Figure 10
The hip, knee, and ankle joint angular velocities.Figure 11
The hip, knee, and ankle joint angular accelerations.Since the control system input requires nominal angular velocity and nominal angular acceleration per each actuated joint, it is necessary to smooth out the nominal trajectories.Although the amplitude of the noise is usually small relative to the nominal signal, its derivatives can dominate the nominal angular velocities and accelerations, which would cause significant errors in the desired motion (errors in the reference input vector).This problem can be solved using filtering of the recorded signal. The cut-off frequency should be set-up such that the steepest parts of the nominal trajectory can pass through the filter without significant change. In this case, the low-pass filter cut-off frequency was 30 rad/s, for the joint trajectories and their first derivatives.The nominal trajectories are used for the inverse dynamics to generate nominal torques at the joints, which is discussed next.


#### 2.2.2. Inverse System

Figure13 shows the inverse plant block and its role in the control system. The inverse system takes the desired joint trajectories and corresponding derivatives and provides the nominal torques, based on the inverse dynamics equation formed using (5)
(13)[τ]=[D(q)]-1{[M(q)][q̈]+[C(q,q̇)][q̇]+[G(q)]}.
One significant problem with the inverse plant design is that the derivatives of the desired trajectories (desired output) include the points placed at future time instants, which makes the control system noncausal. A possible solution to this problem is to use an estimator by setting up a Hurwitz polynomial. However, the inverse plant may become unstable using this predictor. In the frame of this work, back-stepping method [14] has been used to stabilize the inverse plant. Since the stabilized equations (13) of the inverse plant using the Hurwitz polynomials are long, they will not be shown here. In essence, the main result of the applied method is global exponential stability of the inverse system, which means that the inverse system will unconditionally provide the nominal torques, no matter what the desired continuous joint trajectories are, and the rate of the convergence toward the nominal torques will be exponential.It is very important to obtain exponential stability of the inverse system (pseudoinverse), since any other stability (e.g., asymptotical) would cause further delay and generate further error generation due to slow convergence in the rest of the control system.Besides the design of the pseudoinverse system of the bipedal robot dynamics, it was interesting to investigate possible simplifications that would reduce the high computational requirements of the pseudoinverse system. Therefore, we used the mass-spring inverted pendulum model presented previously in the text, with the objective to show that the computationally costly and complicated pseudoinverse system, that regularly requires stabilization, can be replaced by a mass-spring model in order to obtain the nominal joint torques.The main requirement for using the mass-spring model is that the desired joint trajectories have to be adjusted such that the robot’s COM follows the trajectory of the concentrated mass of the mass-spring model. This is possible to do even in a closed mathematical form if the center of pressure is considered to be fixed during the single support phase. The approximate solution of the COM trajectory can be obtained analytically for the mass-spring model and then, the ankle and knee joint angles can be calculated (Figure12) in the following way:
(14)rcosθ=l2cosφ1+l3cosφ2,rsinθ=-lP-l2sinφ1-l3sinφ2,
where the quantities are explained earlier (see Sections 1.2 and 1.3) and are shown again in Figure 12.Figure 12
Calculation of ankle and knee joint angles using the COM trajectory of the mass-spring model.Figure 13
The nonlinear system control based on a gain scheduling approach and the inverse plant prediction. The controller switches between two configurations corresponding to the single support and the double support phase.Equations (14) have a fairly concise analytical solution for lP=0 (in any case a numerical solution can be used, however; there is an analytical solution for lp≠0, as well) in the following form:
(15)cosφ1=12l2[rcosθ+1r×(sinθ-l24+2l22l32-l34+2(l22+l32)r2-r4+(l22-l32)cosθsinθ-l24+2l22l32-l34+2(l22+l32)r2-r4)],cosφ2=12l3[rcosθ-1r×(sinθ-l24+2l22l32-l34+2(l22+l32)r2-r4+(l22-l32)cosθsinθ-l24+2l22l32-l34+2(l22+l32)r2-r4)].
Finally, the hip angle trajectory should be chosen such that the center of pressure P is maintained inside the foot/ground contact area and that the center of mass of the robot remains approximately at a constant location relative to the robot’s hip joint.After designing the joint trajectories as described, the nominal torques can be obtained using the solution for the spring/damper force. The total radial force is then,(16)Fr=k(l0-r)-bṙ,
where r and ṙ are obtained using the mass-spring model (1) described earlier (see Section 1.2).Finally, the torques at ankle and knee joints can be calculated to produce the same resulting radial forceFr as obtained by the mass-spring model. 										(17)τanklenominal=FrlPcosθ,τkneenominal=Frl3sin(θ+φ1+φ2),
where Fr is given by (16), and φ1 and φ2 are given by (15). The hip joint nominal torque is calculated to maintain the angle as described earlier.


#### 2.2.3. Balancing Using ZMP Criterion

Inverse dynamics provides the nominal torques that provide an approximate balancing and the controller provides a corrective torque value to ensure that the desired joint trajectories are followed. However, since the foot/ground contact provides only one-sided normal force, the calculated ankle torque, which would provide balancing and joint trajectories tracking, might not be possible to compensate by the ground contact, due to limited size of the foot and limited torque; that is, if the calculated torque is too high, the foot will not stay flat on the ground and will start rotating which results in robot losing its stability if the following step does not compensate the error before the joint trajectories exit the prescribed tolerance intervals.This is why, besides using the inverse dynamics to calculate nominal torques that would provide approximate balancing, another  criterion for control algorithm must be invoked that considers the center of pressure position on the foot. This criterion is called Zero Moment Point criterion.(1)
Zero Moment Point criterion [11] can be stated in the following way: ZMP criterion requires that there is a point inside the ground/foot contact area (excluding the edge) for which total moment about any axis laying in the tangential plane of the ground/foot contact must be equal to zero for the single support phase, Figure 14(a).  Similarly for the double support phase  shown in  Figure 14(b),  the ZMP must lay inside the area (A) formed by enclosing the two feet/ground contact areas shown in Figure 14(c).Desired and realized ZMP during (a) the single support and (b) the double support phase.
(a)(b)(c)The requirement for zero moment considers only the moment component laying in the tangential plane of the contact, the perpendicular componentM⃗nreact of the moment resulting from the tangential forces of the contact still might not be zero (but relatively low, unless a cornering motion is performed by the robot).Figure14 shows that inertial forces are collected into the main vector F⃗in acting at the center of mass C. The reduction of the inertial force onto the center C is accompanied with the resulting inertial moment M⃗Cin.  Mathematically, the condition can be expressed as
(18)PC⃗×(F⃗in+mg⃗)+M⃗Cin=M⃗react=M⃗nreact,[PC⃗×(F⃗in+mg⃗)+M⃗Cin]×n⃗=M⃗nreact×n⃗=0⃗,
meaning that reaction moment can have only the perpendicular component.The condition can be considered directly, without reducing the inertial forces to the center of mass, in the following way:(19)∑i=1ns[PCi⃗×(F⃗iin+mig⃗)+M⃗Ciin]=M⃗react=M⃗nreact,∑i=1ns[PCi⃗×(F⃗iin+mig⃗)+M⃗Ciin]×n⃗=M⃗nreact×n⃗=0⃗,
where ns is the number of structural elements, PCi⃗ is the relative position vector of the center of mass Ci of the ith structural segment, F⃗iin and M⃗Ciin are the inertial force and inertial moment of the ith segment.Equation (19) is used for calculation of relative coordinates of the point P (ZMP/CP point).If the nominal joint angle trajectories are precalculated, they must provide location of P inside the contact area without reaching the foot edge, for the single support phase. Similarly, during the single support phase, the controller must take corrective actions such that P is kept from reaching the foot edge.


#### 2.2.4. Error Dynamics

The nominal inputs to the plant (nominal torques), generated by the inverse plant (Figure13), are sufficient to drive an ideal system (exactly represented by the model) to follow precisely the desired joint trajectories. However, a mathematical representation has always more or less errors with respect to the system which is represented by that model.Disturbances can appear due to(i)
initial conditions,(ii)
effects which are not included in the model, since their way of action and the system response on them are unknown,(iii)
unavoidable delay in sensors (encoders, potentiometers, and A/D conversion),(iv)
intentionally simplifying the model mathematical description, since we rely on the control law to minimize the error, (e.g., leaving some friction effect not modeled), and so forth.The robot model architecture in the anatomical position has a vertical plane of symmetry (sagittal plane). Due to this symmetry, and cyclic nature of the nominal trajectories, the leg and arm joints on one side of the body have same nominal torques over the walking cycle, just shifted in phase, when the motion is restricted to the planar motion. Then, the control design of two single support phases and a double support phase completing a walking cycle can be narrowed down to a single support and a double support phase.Since the dynamics of the robot is significantly different during the single and the double support phase, it was convenient for this paper to consider two controller configurations. A short analysis shows that three degrees of freedom are removed for the planar consideration after both feet are fully in the contact with the ground, without slipping. Two translational degrees of freedom that are considered in the planar case and one rotational degree are removed after the second foot becomes immovable relative to the ground. Similarly, six degrees of freedom are removed in the 3D motion analysis; 3 translational and 3 rotational degrees are removed since the second foot becomes immovable relative to the ground.Moreover, for the sake of the simplicity of this discussion, the influence of the arms relative dynamics with respect to the trunk will be neglected and considered as a disturbance whose effect has to be minimized by the controller. The system (walking robot mathematical model) can be represented as follows. The state space representation of the three joint variables on the one leg of the robot model is(20)[ξ̇1ξ̇2ξ̇3ξ̇4ξ̇5ξ̇6]=[θ̇1θ̈1θ̇2θ̈2θ̇3θ̈3]=[ξ2f2(ξ)ξ4f4(ξ)ξ6f6(ξ)]+[000b21(ξ)b22(ξ)b23(ξ)000b41(ξ)b42(ξ)b43(ξ)000b61(ξ)b62(ξ)b63(ξ)][τ1τ2τ3].
Equation (20) can be expressed in a simplified form
(21)[ξ̇]=[f(ξ)]+[b(ξ)][τ],
where ξ is the state space vector related to the three joint angles and its derivative, as shown in (20). Although the torques distribution functions bij(ξ) (i=1,…,6;j=1,2,3) are shown to depend on entire state vector, precisely they depend on just evenly numbered components of the state vector (on the angles of the structure, not angular velocities). Further on, (21) will be used instead of (20) for the sake of simplicity.The real state space vector of the system isξ, which generally deviates from the nominal state vector ξ̅. The difference between the two vectors is denoted as the error vector ξ̃=ξ-ξ̅.Using the time derivative of the error vector and (21), the following is obtained
(22)ξ̃̇=ξ̇-ξ̅̇=f(ξ)+b(ξ)τ-f(ξ̅)-b(ξ̅)τ̅=f(ξ̃+ξ̅)+b(ξ̃+ξ̅)(τ̃+τ̅)-f(ξ̅)-b(ξ̅)τ̅=(f(ξ̃+ξ̅)-f(ξ̅))+(b(ξ̃+ξ̅)(τ̃+τ̅)-b(ξ̅)τ̅)≈∂f(ξ)∂ξ|ξ=ξ̅ξ̃+∂b(ξ)∂ξ|ξ=ξ̅ξ̃τ̅+b(ξ)|ξ=ξ̅τ̃,
that is
(23)ξ̃̇≈A(ξ̅)ξ̃+(bd(ξ̅)⊗ξ̃)τ̅+b(ξ̅)τ̃.
Equation (23) is used for the linearization-based controller design, where the matrix A(ξ̅) and the tensor of third-order bd(ξ̅) contain the derivatives which are used for the scheduled gains of the controller. The tensor bd(ξ̅) represents the change of the input torques distribution matrix function due to the change in the state space vector ξ, and it has the following form where every entry (bd)ij is a vector ((bd(ξ))ij)k=∂bij(ξ)/∂ξk, that is,
(24)bd(ξ)=[[∂b11(ξ)∂ξk]T[∂b21(ξ)∂ξk]T[∂b31(ξ)∂ξk]T[∂b41(ξ)∂ξk]T[∂b51(ξ)∂ξk]T[∂b61(ξ)∂ξk]T[∂b12(ξ)∂ξk]T[∂b22(ξ)∂ξk]T[∂b32(ξ)∂ξk]T[∂b42(ξ)∂ξk]T[∂b52(ξ)∂ξk]T[∂b62(ξ)∂ξk]T[∂b13(ξ)∂ξk]T[∂b23(ξ)∂ξk]T[∂b33(ξ)∂ξk]T[∂b43(ξ)∂ξk]T[∂b53(ξ)∂ξk]T[∂b63(ξ)∂ξk]T].
The symbol ⊗ denotes a tensor product such that every vector-entry (bd)ij performs a dot product with the error vector ξ̃ and finally the result (bd)ijkξ̃k performs the matrix multiplication with the column vector of the nominal input torques τ̅.The gain scheduled controller, based on (23) alone, cannot cope with the steady state errors. This is the reason why the state space vector ξ̃ is augmented with three integrators (they integrate the angle error for the ankle, knee and hip joints, resp.) as follows:
(25)[ξ̃aug]=[[ξ̃]Tς1ς2ς3]T,[ξ̃̇aug]=[[ξ̃̇]ς̇1ς̇2ς̇3]=[[A(ξ̅)][ξ̃]+([bd(ξ̅)]⊗[ξ̃])[τ̅]+[b(ξ̅)][τ̃]ξ̃1ξ̃2ξ̃3].
Stability of the gain scheduling controller is limited by the maximum allowed time derivatives of the desired joint angle trajectories, which are used as the scheduling variables
(26)∥q̇des(t)∥≤μ,
where qdes(t) is the vector of desired joint angles trajectories and μ is a positive constant  (Theorem 12.1 in [14]). The scheduling vector is of the nominal angles at the ankle, knee, and hip joints
(27)vs(t)=[ξ1d(t)ξ3d(t)ξ5d(t)]=[ξ̅1(t)ξ̅3(t)ξ̅5(t)],
and the condition (26) in the case of the numerical norm l1(28)∥[ξ̅̇1(t)ξ̅̇3(t)ξ̅̇5(t)]∥1=|ξ̅̇1(t)||ξ̅̇3(t)||ξ̅̇5(t)|≤μ,∀t.
The scheduling could have included all nominal state space variables, and can include also additional inputs for gain scheduling. However, the problem arises when nominal states are not known in advance (or partially known). In this case, one can use the inverse system to generate the nominal states based on the reference input/desired output.The constraint (26) is a serious limitation of the controller as a result of the “frozen time” concept application. However, application of the inverse system (Figure 13) significantly improved the controller capabilities, since the inverse system includes the time derivatives of the desired joint trajectories and the limitation (26) on the time derivatives of the scheduling variables (desired joint angles) is replaced by the limitation on the time derivatives of the error vector, that is the vector of the angular speeds deviations from the nominal values. For this design, μ is about 6 rad/s.The novelty in this design compared to traditional PID scheduled design is that this controller design includes nonlinear relations to schedule the gains based on the nonlinear system dynamics. The result is better capability of the controller to track the trajectories with higher time derivatives.The performances of the gain scheduled controller, combined with the inverse plant, are discussed following.


## 2.1. Kinematic and Dynamic Analysis of the Model

Forward kinematics gives Cartesian coordinates of any point of the structure for a given set of the joint and the external variables. The kinematic relations are necessary to close the set of differential equations that describes the robot dynamics.The forward kinematics of the robot model can be expressed as follows:(2)[PC1PC2⋮PC11]=f([q1q2⋯q10]T),
where PCi (i=1,…,11) are position vectors of the center of gravity of the ith segment and has the form [xCiyCi0]T.  Generalized coordinates in right-hand side of (2) can be the absolute angles of the segment and external coordinates, or they can represent the joint angles and external coordinates. Besides (2), complete kinematic analysis includes the first and second time derivative of (2). Since the expanded equations in (2) and its first two time derivatives are too long to be shown in this paper, they are omitted and we proceed with the dynamic analysis.Since the control law, which is discussed in the following section, is based on an inverse model and an error dynamics, it is necessary to analyze both, forward and inverse dynamic equations. The derivation of those equations is done using energy-based method-Lagrange equations. The inverse model has to provide nominal generalized forces (the joint torques), based on nominal joint angular trajectories, which is, by definition, the inverse dynamics.On the other hand, the error-based controller uses the differential equations of motion to generate a correction value/signal for the input torques. The differential equations are also needed for numerical simulation of the robot and evaluation of the controller performances. Figure8 shows a partial free-body diagram, which includes the right standing leg only.Figure 8
Planar partial free-body diagram for the right standing leg.Besides the reaction forces/torques from the rest of the system, the set of external forces/torques for the subsystem of the support leg during the single support phase includes the torques at the joints, since they perform work and influence the mechanical energy distribution. Such approach is convenient for application of the Lagrange method which provides the way to analyze the relevant forces/torques, without necessity to calculate the internal forces, such as the internal forces at the joints.Lagrange equations for the walking robot model have the following form:(3)ddt∂L(t,qi,q̇i)∂q̇r-∂L(t,qi,q̇i)∂qr+∂R(t,qi,q̇i)∂q̇r=Qrnc(t,qi,q̇i),
where L(t,qi,q̇i)=EK(t,qi,q̇i)-EP(t,qi) is the Lagrange function generally depending on time, generalized coordinates, and generalized velocities, R(t,qi,q̇i) is the dissipation (Rayleigh’s) function, and Qrnc(t,qi,q̇i) is the rth generalized force (calculated based on the work of nonconservative forces related to the rth gen. coordinate excluding friction forces). The dissipation function can be used to model the lost of energy at the instant of feet collision with the ground. The generalized forces are torques generated by motors at the robot joints.Kinetic energy of the structure is the sum of the kinetic energy for translational and rotational motion for each segment(4)EK=∑i=111(mivCi22+ICiωi22),
where vCi is the ith segment center of gravity velocity magnitude, ICi is the ith segment moment of inertia for the center of gravity Ci, and ωi is the angular velocity of the ith segment. The centers of gravity velocity can be expressed in terms of the joint angular velocities using the kinematic relations (2). The potential energy is basically the gravitational potential energy since no springs are included in the model.An alternative way to derive the model dynamics is to apply the combination of the Newton’s and Euler-Dalembert’s law for the translation and rotation, respectively. In either way, the following form of dynamic equations is obtained(5)[M(φ)][φ̈]+[C(φ,φ̇)][φ̇]+[G(φ)]=[D(φ)][τ],
where φ=[φ1φ2φ4φ5φ6]T is the vector of the absolute angles (the foot dynamics has been neglected), [M(φ)] is the matrix of inertia, [C(φ,φ̇)] represents cross-correlation of the segments angular velocities, [G(φ)] is the gravity influence vector, [τ] is the control torque vector, and  [D(φ)] represents the matrix of the control torques distribution.Although the equations are implemented in the simulation model for the full model (10 DOF), the equations and analysis that are shown here are for the reduced model, for which the arms are considered sufficiently light such that their dynamics is not significant for the system stability. Similarly, the dynamics of the swing leg foot is neglected and the mass is considered lumped at the ankle.The kinetic energy function for the model is(6)EK=12φ̇12(m2l1d2+I2+(2m3+m2′+m4)l12)+12φ̇22(m3l2d2+I3+(2m3+m2′+m4)l22)+12φ̇42(m2′l1u2+I2)+12φ̇52(m3l2u2+I3+m2′l22)+φ̇1φ̇2c21((m2′+m3+m4)l1l2+m3l1l2d)-φ̇1φ̇5c51(m3l1l2u+m2′l1l2)-φ̇2φ̇5c52(m3l1l2u+m2′l22)-φ̇1φ̇4c41m2′l1l1u-φ̇2φ̇4c42m2′l2l1u+φ̇4φ̇5c54m2′l2l1u+φ̇1φ̇6c61m4l1l3d+φ̇2φ̇6c62m4l2l3d+12φ̇62(m4l3d2+I4),
where cij=cos(φi-φj), (i,j=1,2,4,5,6), mi is the ith segment mass, m2′ is the foot-lower leg combined mass, liu and lid are the ith segment center of gravity position lengths with respect to the neighboring upper and lower joints, respectively,  li is the ith segment length, Ii is the ith segment moment of inertia with respect to the axis perpendicular to the sagittal plane.The potential energy function is(7)EPg=(m2l1d+(2m3+m4+m2′)l1)c1+(m3l2d+(m3+m4+m2′)l2)c2-m2′l1uc4-(m3l2u+m2′l2)c5+m4l3dc6,whereci=cos(φi),(i=1,2,4,5,6). The Rayleigh’s dissipation function reflects the energy lost due to the viscous friction in the joints and has the following form for the model:
(8)R=12(b1φ̇12+b2(φ̇2-φ̇1)2+b3(φ̇6-φ̇2)2)+12(b2(φ̇5-φ̇4)2+b3(φ̇6-φ̇5)2),
where bi (i=1,2,3) are the friction coefficients for the ankle, knee, and hip joints, respectively.The final equations in the form (5) are obtained using the energy-based method and the matrices/vectors denoted in (5) are as follows.The symmetric matrix of inertia is the following:(9)M(:,1)=[m2l1d2+I2+(2m3+m2′+m4)l12((m2′+m3+m4)l1l2+m3l1l2d)c21-m2′l1l1uc41-(m3l1l2u+m2′l1l2)c51m4l1l3dc61],M(:,2)=[((m2′+m3+m4)l1l2+m3l1l2d)c21m3l2d2+I3+(m3+m2′+m4)l22-m2′l2l1uc42-(m3l2l2u+m2′l22)c52m4l2l3dc62],M(:,3:5)=[-m2′l1l1uc41-m2′l2l1uc42m2′l1u2+I2m2′l2l1uc540|-(m3l1l2u+m2′l1l2)c51-(m3l2l2u+m2′l22)c52m2′l2l1uc54m3l2u2+I3+m2′l220|m4l1l3dc61m4l2l3dc6200I4+m4l3d2].The matrixC(φ,φ̇) is given in segments as follows:
(10)C(:,1)=[b1+b2-b2+((m2′+m3+m4)l1l2+m3l1l2d)s21φ̇1-m2′l1l1us41φ̇1-(m2′l1l2+m3l1l2u)s51φ̇1m4l1l3ds61φ̇1],C(:,2)=[-b2+((m2′+m3+m4)l1l2+m3l1l2d)s21φ̇2b2+b3-m2′l2l1us42φ̇2-(m2′l22+m3l2l2u)s52φ̇2-b3+m4l2l3ds62φ̇2],C(:,3)=[m2′l1l1us41φ̇4m2′l2l1us42φ̇4b2-b2+m2′l2l1us54φ̇40],C(:,4:5)=[(m2′l1l2+m3l1l2u)s51φ̇5(m2′l22+m3l2l2u)s52φ̇5-b2-m2′l2l1us54φ̇5b2+b3-b3|-m4l1l3ds61φ̇6-b3-m4l2l3ds62φ̇60-b32b3],
where sij=sin(φi-φj), (i,j=1,2,4,5,6).The gravity vectorG(φ) is as follows:
(11)G=[(m2l1d+(2m3+m4+m2′)l1)s1(m3l2d+(m3+m4+m2′)l2)s2-m2′l1us4-(m3l2u+m2′l2)s5m4l3ds6]g,
where si=sin(φi), (i=1,2,4,5,6), and g is the gravity acceleration.The torques distribution matrixD(φ) is, actually, a constant matrix as follows:
(12)D=[110000-1-10000010000-1-100101].
Finally, the joints torques vector is τ=[τ1τ2τ4τ5τ6]T, that has components corresponding to joints torques in the following order: right ankle, right knee, right hip, left knee, and left hip, respectively. Again, the swing leg ankle torque is neglected, as well as the dynamics of the swing leg foot.The equations of the form (5) will be used to derive the control law for each actuated joint of the robot model. This is shown in the following controller design section.


## 2.2. Controller Design

The controller design is based on a nonlinear system control approach. A combination of the inverse plant with modified gain scheduling is applied.


### 2.2.1. Desired Robot Motion

Based on a typical human walking, the corresponding angular trajectories for the hip, knee and ankle joints can be recorded using cameras and markers positioned on human body segments. In this way, following trajectories are obtained and used for the desired robot motion (Figure9).Figure 9
The robot joint trajectories based on a human walking cycle (positive angles denote flexion/dorsiflexion).The figure shows that the trajectories do not vary on the left side of the interval as much as they vary on the rest of the walking cycle. The subinterval (the swing phase) contains relatively high derivatives and it is very important to determine precisely the toe off instant, since any variation the toe off instant would cause significant change in the nominal torques and the calculated ground reaction force.From the mechanical structure basis, there is a configuration change since the structure changes from the double support, which represents the structure with a closed loop, to the single support configuration, which is an open chain configuration.Therefore, when the configuration change occurs it triggers the control law change. Basically, the instant of this configuration change can be determined based on the ground force reaction which is measured using pressure pads positioned at the feet.The first two derivatives of the desired trajectories are shown in Figures10 and 11, respectively. Relatively high derivatives are noticeable in the swing phase of the cycle. The nominal trajectories recorded based on human motion usually have significant noise embedded due to errors during the measurements.Figure 10
The hip, knee, and ankle joint angular velocities.Figure 11
The hip, knee, and ankle joint angular accelerations.Since the control system input requires nominal angular velocity and nominal angular acceleration per each actuated joint, it is necessary to smooth out the nominal trajectories.Although the amplitude of the noise is usually small relative to the nominal signal, its derivatives can dominate the nominal angular velocities and accelerations, which would cause significant errors in the desired motion (errors in the reference input vector).This problem can be solved using filtering of the recorded signal. The cut-off frequency should be set-up such that the steepest parts of the nominal trajectory can pass through the filter without significant change. In this case, the low-pass filter cut-off frequency was 30 rad/s, for the joint trajectories and their first derivatives.The nominal trajectories are used for the inverse dynamics to generate nominal torques at the joints, which is discussed next.


### 2.2.2. Inverse System

Figure13 shows the inverse plant block and its role in the control system. The inverse system takes the desired joint trajectories and corresponding derivatives and provides the nominal torques, based on the inverse dynamics equation formed using (5)
(13)[τ]=[D(q)]-1{[M(q)][q̈]+[C(q,q̇)][q̇]+[G(q)]}.
One significant problem with the inverse plant design is that the derivatives of the desired trajectories (desired output) include the points placed at future time instants, which makes the control system noncausal. A possible solution to this problem is to use an estimator by setting up a Hurwitz polynomial. However, the inverse plant may become unstable using this predictor. In the frame of this work, back-stepping method [14] has been used to stabilize the inverse plant. Since the stabilized equations (13) of the inverse plant using the Hurwitz polynomials are long, they will not be shown here. In essence, the main result of the applied method is global exponential stability of the inverse system, which means that the inverse system will unconditionally provide the nominal torques, no matter what the desired continuous joint trajectories are, and the rate of the convergence toward the nominal torques will be exponential.It is very important to obtain exponential stability of the inverse system (pseudoinverse), since any other stability (e.g., asymptotical) would cause further delay and generate further error generation due to slow convergence in the rest of the control system.Besides the design of the pseudoinverse system of the bipedal robot dynamics, it was interesting to investigate possible simplifications that would reduce the high computational requirements of the pseudoinverse system. Therefore, we used the mass-spring inverted pendulum model presented previously in the text, with the objective to show that the computationally costly and complicated pseudoinverse system, that regularly requires stabilization, can be replaced by a mass-spring model in order to obtain the nominal joint torques.The main requirement for using the mass-spring model is that the desired joint trajectories have to be adjusted such that the robot’s COM follows the trajectory of the concentrated mass of the mass-spring model. This is possible to do even in a closed mathematical form if the center of pressure is considered to be fixed during the single support phase. The approximate solution of the COM trajectory can be obtained analytically for the mass-spring model and then, the ankle and knee joint angles can be calculated (Figure12) in the following way:
(14)rcosθ=l2cosφ1+l3cosφ2,rsinθ=-lP-l2sinφ1-l3sinφ2,
where the quantities are explained earlier (see Sections 1.2 and 1.3) and are shown again in Figure 12.Figure 12
Calculation of ankle and knee joint angles using the COM trajectory of the mass-spring model.Figure 13
The nonlinear system control based on a gain scheduling approach and the inverse plant prediction. The controller switches between two configurations corresponding to the single support and the double support phase.Equations (14) have a fairly concise analytical solution for lP=0 (in any case a numerical solution can be used, however; there is an analytical solution for lp≠0, as well) in the following form:
(15)cosφ1=12l2[rcosθ+1r×(sinθ-l24+2l22l32-l34+2(l22+l32)r2-r4+(l22-l32)cosθsinθ-l24+2l22l32-l34+2(l22+l32)r2-r4)],cosφ2=12l3[rcosθ-1r×(sinθ-l24+2l22l32-l34+2(l22+l32)r2-r4+(l22-l32)cosθsinθ-l24+2l22l32-l34+2(l22+l32)r2-r4)].
Finally, the hip angle trajectory should be chosen such that the center of pressure P is maintained inside the foot/ground contact area and that the center of mass of the robot remains approximately at a constant location relative to the robot’s hip joint.After designing the joint trajectories as described, the nominal torques can be obtained using the solution for the spring/damper force. The total radial force is then,(16)Fr=k(l0-r)-bṙ,
where r and ṙ are obtained using the mass-spring model (1) described earlier (see Section 1.2).Finally, the torques at ankle and knee joints can be calculated to produce the same resulting radial forceFr as obtained by the mass-spring model. 										(17)τanklenominal=FrlPcosθ,τkneenominal=Frl3sin(θ+φ1+φ2),
where Fr is given by (16), and φ1 and φ2 are given by (15). The hip joint nominal torque is calculated to maintain the angle as described earlier.


### 2.2.3. Balancing Using ZMP Criterion

Inverse dynamics provides the nominal torques that provide an approximate balancing and the controller provides a corrective torque value to ensure that the desired joint trajectories are followed. However, since the foot/ground contact provides only one-sided normal force, the calculated ankle torque, which would provide balancing and joint trajectories tracking, might not be possible to compensate by the ground contact, due to limited size of the foot and limited torque; that is, if the calculated torque is too high, the foot will not stay flat on the ground and will start rotating which results in robot losing its stability if the following step does not compensate the error before the joint trajectories exit the prescribed tolerance intervals.This is why, besides using the inverse dynamics to calculate nominal torques that would provide approximate balancing, another  criterion for control algorithm must be invoked that considers the center of pressure position on the foot. This criterion is called Zero Moment Point criterion.(1)
Zero Moment Point criterion [11] can be stated in the following way: ZMP criterion requires that there is a point inside the ground/foot contact area (excluding the edge) for which total moment about any axis laying in the tangential plane of the ground/foot contact must be equal to zero for the single support phase, Figure 14(a).  Similarly for the double support phase  shown in  Figure 14(b),  the ZMP must lay inside the area (A) formed by enclosing the two feet/ground contact areas shown in Figure 14(c).Desired and realized ZMP during (a) the single support and (b) the double support phase.
(a)(b)(c)The requirement for zero moment considers only the moment component laying in the tangential plane of the contact, the perpendicular componentM⃗nreact of the moment resulting from the tangential forces of the contact still might not be zero (but relatively low, unless a cornering motion is performed by the robot).Figure14 shows that inertial forces are collected into the main vector F⃗in acting at the center of mass C. The reduction of the inertial force onto the center C is accompanied with the resulting inertial moment M⃗Cin.  Mathematically, the condition can be expressed as
(18)PC⃗×(F⃗in+mg⃗)+M⃗Cin=M⃗react=M⃗nreact,[PC⃗×(F⃗in+mg⃗)+M⃗Cin]×n⃗=M⃗nreact×n⃗=0⃗,
meaning that reaction moment can have only the perpendicular component.The condition can be considered directly, without reducing the inertial forces to the center of mass, in the following way:(19)∑i=1ns[PCi⃗×(F⃗iin+mig⃗)+M⃗Ciin]=M⃗react=M⃗nreact,∑i=1ns[PCi⃗×(F⃗iin+mig⃗)+M⃗Ciin]×n⃗=M⃗nreact×n⃗=0⃗,
where ns is the number of structural elements, PCi⃗ is the relative position vector of the center of mass Ci of the ith structural segment, F⃗iin and M⃗Ciin are the inertial force and inertial moment of the ith segment.Equation (19) is used for calculation of relative coordinates of the point P (ZMP/CP point).If the nominal joint angle trajectories are precalculated, they must provide location of P inside the contact area without reaching the foot edge, for the single support phase. Similarly, during the single support phase, the controller must take corrective actions such that P is kept from reaching the foot edge.


### 2.2.4. Error Dynamics

The nominal inputs to the plant (nominal torques), generated by the inverse plant (Figure13), are sufficient to drive an ideal system (exactly represented by the model) to follow precisely the desired joint trajectories. However, a mathematical representation has always more or less errors with respect to the system which is represented by that model.Disturbances can appear due to(i)
initial conditions,(ii)
effects which are not included in the model, since their way of action and the system response on them are unknown,(iii)
unavoidable delay in sensors (encoders, potentiometers, and A/D conversion),(iv)
intentionally simplifying the model mathematical description, since we rely on the control law to minimize the error, (e.g., leaving some friction effect not modeled), and so forth.The robot model architecture in the anatomical position has a vertical plane of symmetry (sagittal plane). Due to this symmetry, and cyclic nature of the nominal trajectories, the leg and arm joints on one side of the body have same nominal torques over the walking cycle, just shifted in phase, when the motion is restricted to the planar motion. Then, the control design of two single support phases and a double support phase completing a walking cycle can be narrowed down to a single support and a double support phase.Since the dynamics of the robot is significantly different during the single and the double support phase, it was convenient for this paper to consider two controller configurations. A short analysis shows that three degrees of freedom are removed for the planar consideration after both feet are fully in the contact with the ground, without slipping. Two translational degrees of freedom that are considered in the planar case and one rotational degree are removed after the second foot becomes immovable relative to the ground. Similarly, six degrees of freedom are removed in the 3D motion analysis; 3 translational and 3 rotational degrees are removed since the second foot becomes immovable relative to the ground.Moreover, for the sake of the simplicity of this discussion, the influence of the arms relative dynamics with respect to the trunk will be neglected and considered as a disturbance whose effect has to be minimized by the controller. The system (walking robot mathematical model) can be represented as follows. The state space representation of the three joint variables on the one leg of the robot model is(20)[ξ̇1ξ̇2ξ̇3ξ̇4ξ̇5ξ̇6]=[θ̇1θ̈1θ̇2θ̈2θ̇3θ̈3]=[ξ2f2(ξ)ξ4f4(ξ)ξ6f6(ξ)]+[000b21(ξ)b22(ξ)b23(ξ)000b41(ξ)b42(ξ)b43(ξ)000b61(ξ)b62(ξ)b63(ξ)][τ1τ2τ3].
Equation (20) can be expressed in a simplified form
(21)[ξ̇]=[f(ξ)]+[b(ξ)][τ],
where ξ is the state space vector related to the three joint angles and its derivative, as shown in (20). Although the torques distribution functions bij(ξ) (i=1,…,6;j=1,2,3) are shown to depend on entire state vector, precisely they depend on just evenly numbered components of the state vector (on the angles of the structure, not angular velocities). Further on, (21) will be used instead of (20) for the sake of simplicity.The real state space vector of the system isξ, which generally deviates from the nominal state vector ξ̅. The difference between the two vectors is denoted as the error vector ξ̃=ξ-ξ̅.Using the time derivative of the error vector and (21), the following is obtained
(22)ξ̃̇=ξ̇-ξ̅̇=f(ξ)+b(ξ)τ-f(ξ̅)-b(ξ̅)τ̅=f(ξ̃+ξ̅)+b(ξ̃+ξ̅)(τ̃+τ̅)-f(ξ̅)-b(ξ̅)τ̅=(f(ξ̃+ξ̅)-f(ξ̅))+(b(ξ̃+ξ̅)(τ̃+τ̅)-b(ξ̅)τ̅)≈∂f(ξ)∂ξ|ξ=ξ̅ξ̃+∂b(ξ)∂ξ|ξ=ξ̅ξ̃τ̅+b(ξ)|ξ=ξ̅τ̃,
that is
(23)ξ̃̇≈A(ξ̅)ξ̃+(bd(ξ̅)⊗ξ̃)τ̅+b(ξ̅)τ̃.
Equation (23) is used for the linearization-based controller design, where the matrix A(ξ̅) and the tensor of third-order bd(ξ̅) contain the derivatives which are used for the scheduled gains of the controller. The tensor bd(ξ̅) represents the change of the input torques distribution matrix function due to the change in the state space vector ξ, and it has the following form where every entry (bd)ij is a vector ((bd(ξ))ij)k=∂bij(ξ)/∂ξk, that is,
(24)bd(ξ)=[[∂b11(ξ)∂ξk]T[∂b21(ξ)∂ξk]T[∂b31(ξ)∂ξk]T[∂b41(ξ)∂ξk]T[∂b51(ξ)∂ξk]T[∂b61(ξ)∂ξk]T[∂b12(ξ)∂ξk]T[∂b22(ξ)∂ξk]T[∂b32(ξ)∂ξk]T[∂b42(ξ)∂ξk]T[∂b52(ξ)∂ξk]T[∂b62(ξ)∂ξk]T[∂b13(ξ)∂ξk]T[∂b23(ξ)∂ξk]T[∂b33(ξ)∂ξk]T[∂b43(ξ)∂ξk]T[∂b53(ξ)∂ξk]T[∂b63(ξ)∂ξk]T].
The symbol ⊗ denotes a tensor product such that every vector-entry (bd)ij performs a dot product with the error vector ξ̃ and finally the result (bd)ijkξ̃k performs the matrix multiplication with the column vector of the nominal input torques τ̅.The gain scheduled controller, based on (23) alone, cannot cope with the steady state errors. This is the reason why the state space vector ξ̃ is augmented with three integrators (they integrate the angle error for the ankle, knee and hip joints, resp.) as follows:
(25)[ξ̃aug]=[[ξ̃]Tς1ς2ς3]T,[ξ̃̇aug]=[[ξ̃̇]ς̇1ς̇2ς̇3]=[[A(ξ̅)][ξ̃]+([bd(ξ̅)]⊗[ξ̃])[τ̅]+[b(ξ̅)][τ̃]ξ̃1ξ̃2ξ̃3].
Stability of the gain scheduling controller is limited by the maximum allowed time derivatives of the desired joint angle trajectories, which are used as the scheduling variables
(26)∥q̇des(t)∥≤μ,
where qdes(t) is the vector of desired joint angles trajectories and μ is a positive constant  (Theorem 12.1 in [14]). The scheduling vector is of the nominal angles at the ankle, knee, and hip joints
(27)vs(t)=[ξ1d(t)ξ3d(t)ξ5d(t)]=[ξ̅1(t)ξ̅3(t)ξ̅5(t)],
and the condition (26) in the case of the numerical norm l1(28)∥[ξ̅̇1(t)ξ̅̇3(t)ξ̅̇5(t)]∥1=|ξ̅̇1(t)||ξ̅̇3(t)||ξ̅̇5(t)|≤μ,∀t.
The scheduling could have included all nominal state space variables, and can include also additional inputs for gain scheduling. However, the problem arises when nominal states are not known in advance (or partially known). In this case, one can use the inverse system to generate the nominal states based on the reference input/desired output.The constraint (26) is a serious limitation of the controller as a result of the “frozen time” concept application. However, application of the inverse system (Figure 13) significantly improved the controller capabilities, since the inverse system includes the time derivatives of the desired joint trajectories and the limitation (26) on the time derivatives of the scheduling variables (desired joint angles) is replaced by the limitation on the time derivatives of the error vector, that is the vector of the angular speeds deviations from the nominal values. For this design, μ is about 6 rad/s.The novelty in this design compared to traditional PID scheduled design is that this controller design includes nonlinear relations to schedule the gains based on the nonlinear system dynamics. The result is better capability of the controller to track the trajectories with higher time derivatives.The performances of the gain scheduled controller, combined with the inverse plant, are discussed following.


## 2.2.1. Desired Robot Motion

Based on a typical human walking, the corresponding angular trajectories for the hip, knee and ankle joints can be recorded using cameras and markers positioned on human body segments. In this way, following trajectories are obtained and used for the desired robot motion (Figure9).Figure 9
The robot joint trajectories based on a human walking cycle (positive angles denote flexion/dorsiflexion).The figure shows that the trajectories do not vary on the left side of the interval as much as they vary on the rest of the walking cycle. The subinterval (the swing phase) contains relatively high derivatives and it is very important to determine precisely the toe off instant, since any variation the toe off instant would cause significant change in the nominal torques and the calculated ground reaction force.From the mechanical structure basis, there is a configuration change since the structure changes from the double support, which represents the structure with a closed loop, to the single support configuration, which is an open chain configuration.Therefore, when the configuration change occurs it triggers the control law change. Basically, the instant of this configuration change can be determined based on the ground force reaction which is measured using pressure pads positioned at the feet.The first two derivatives of the desired trajectories are shown in Figures10 and 11, respectively. Relatively high derivatives are noticeable in the swing phase of the cycle. The nominal trajectories recorded based on human motion usually have significant noise embedded due to errors during the measurements.Figure 10
The hip, knee, and ankle joint angular velocities.Figure 11
The hip, knee, and ankle joint angular accelerations.Since the control system input requires nominal angular velocity and nominal angular acceleration per each actuated joint, it is necessary to smooth out the nominal trajectories.Although the amplitude of the noise is usually small relative to the nominal signal, its derivatives can dominate the nominal angular velocities and accelerations, which would cause significant errors in the desired motion (errors in the reference input vector).This problem can be solved using filtering of the recorded signal. The cut-off frequency should be set-up such that the steepest parts of the nominal trajectory can pass through the filter without significant change. In this case, the low-pass filter cut-off frequency was 30 rad/s, for the joint trajectories and their first derivatives.The nominal trajectories are used for the inverse dynamics to generate nominal torques at the joints, which is discussed next.


## 2.2.2. Inverse System

Figure13 shows the inverse plant block and its role in the control system. The inverse system takes the desired joint trajectories and corresponding derivatives and provides the nominal torques, based on the inverse dynamics equation formed using (5)
(13)[τ]=[D(q)]-1{[M(q)][q̈]+[C(q,q̇)][q̇]+[G(q)]}.
One significant problem with the inverse plant design is that the derivatives of the desired trajectories (desired output) include the points placed at future time instants, which makes the control system noncausal. A possible solution to this problem is to use an estimator by setting up a Hurwitz polynomial. However, the inverse plant may become unstable using this predictor. In the frame of this work, back-stepping method [14] has been used to stabilize the inverse plant. Since the stabilized equations (13) of the inverse plant using the Hurwitz polynomials are long, they will not be shown here. In essence, the main result of the applied method is global exponential stability of the inverse system, which means that the inverse system will unconditionally provide the nominal torques, no matter what the desired continuous joint trajectories are, and the rate of the convergence toward the nominal torques will be exponential.It is very important to obtain exponential stability of the inverse system (pseudoinverse), since any other stability (e.g., asymptotical) would cause further delay and generate further error generation due to slow convergence in the rest of the control system.Besides the design of the pseudoinverse system of the bipedal robot dynamics, it was interesting to investigate possible simplifications that would reduce the high computational requirements of the pseudoinverse system. Therefore, we used the mass-spring inverted pendulum model presented previously in the text, with the objective to show that the computationally costly and complicated pseudoinverse system, that regularly requires stabilization, can be replaced by a mass-spring model in order to obtain the nominal joint torques.The main requirement for using the mass-spring model is that the desired joint trajectories have to be adjusted such that the robot’s COM follows the trajectory of the concentrated mass of the mass-spring model. This is possible to do even in a closed mathematical form if the center of pressure is considered to be fixed during the single support phase. The approximate solution of the COM trajectory can be obtained analytically for the mass-spring model and then, the ankle and knee joint angles can be calculated (Figure12) in the following way:
(14)rcosθ=l2cosφ1+l3cosφ2,rsinθ=-lP-l2sinφ1-l3sinφ2,
where the quantities are explained earlier (see Sections 1.2 and 1.3) and are shown again in Figure 12.Figure 12
Calculation of ankle and knee joint angles using the COM trajectory of the mass-spring model.Figure 13
The nonlinear system control based on a gain scheduling approach and the inverse plant prediction. The controller switches between two configurations corresponding to the single support and the double support phase.Equations (14) have a fairly concise analytical solution for lP=0 (in any case a numerical solution can be used, however; there is an analytical solution for lp≠0, as well) in the following form:
(15)cosφ1=12l2[rcosθ+1r×(sinθ-l24+2l22l32-l34+2(l22+l32)r2-r4+(l22-l32)cosθsinθ-l24+2l22l32-l34+2(l22+l32)r2-r4)],cosφ2=12l3[rcosθ-1r×(sinθ-l24+2l22l32-l34+2(l22+l32)r2-r4+(l22-l32)cosθsinθ-l24+2l22l32-l34+2(l22+l32)r2-r4)].
Finally, the hip angle trajectory should be chosen such that the center of pressure P is maintained inside the foot/ground contact area and that the center of mass of the robot remains approximately at a constant location relative to the robot’s hip joint.After designing the joint trajectories as described, the nominal torques can be obtained using the solution for the spring/damper force. The total radial force is then,(16)Fr=k(l0-r)-bṙ,
where r and ṙ are obtained using the mass-spring model (1) described earlier (see Section 1.2).Finally, the torques at ankle and knee joints can be calculated to produce the same resulting radial forceFr as obtained by the mass-spring model. 										(17)τanklenominal=FrlPcosθ,τkneenominal=Frl3sin(θ+φ1+φ2),
where Fr is given by (16), and φ1 and φ2 are given by (15). The hip joint nominal torque is calculated to maintain the angle as described earlier.


## 2.2.3. Balancing Using ZMP Criterion

Inverse dynamics provides the nominal torques that provide an approximate balancing and the controller provides a corrective torque value to ensure that the desired joint trajectories are followed. However, since the foot/ground contact provides only one-sided normal force, the calculated ankle torque, which would provide balancing and joint trajectories tracking, might not be possible to compensate by the ground contact, due to limited size of the foot and limited torque; that is, if the calculated torque is too high, the foot will not stay flat on the ground and will start rotating which results in robot losing its stability if the following step does not compensate the error before the joint trajectories exit the prescribed tolerance intervals.This is why, besides using the inverse dynamics to calculate nominal torques that would provide approximate balancing, another  criterion for control algorithm must be invoked that considers the center of pressure position on the foot. This criterion is called Zero Moment Point criterion.(1)
Zero Moment Point criterion [11] can be stated in the following way: ZMP criterion requires that there is a point inside the ground/foot contact area (excluding the edge) for which total moment about any axis laying in the tangential plane of the ground/foot contact must be equal to zero for the single support phase, Figure 14(a).  Similarly for the double support phase  shown in  Figure 14(b),  the ZMP must lay inside the area (A) formed by enclosing the two feet/ground contact areas shown in Figure 14(c).Desired and realized ZMP during (a) the single support and (b) the double support phase.
(a)(b)(c)The requirement for zero moment considers only the moment component laying in the tangential plane of the contact, the perpendicular componentM⃗nreact of the moment resulting from the tangential forces of the contact still might not be zero (but relatively low, unless a cornering motion is performed by the robot).Figure14 shows that inertial forces are collected into the main vector F⃗in acting at the center of mass C. The reduction of the inertial force onto the center C is accompanied with the resulting inertial moment M⃗Cin.  Mathematically, the condition can be expressed as
(18)PC⃗×(F⃗in+mg⃗)+M⃗Cin=M⃗react=M⃗nreact,[PC⃗×(F⃗in+mg⃗)+M⃗Cin]×n⃗=M⃗nreact×n⃗=0⃗,
meaning that reaction moment can have only the perpendicular component.The condition can be considered directly, without reducing the inertial forces to the center of mass, in the following way:(19)∑i=1ns[PCi⃗×(F⃗iin+mig⃗)+M⃗Ciin]=M⃗react=M⃗nreact,∑i=1ns[PCi⃗×(F⃗iin+mig⃗)+M⃗Ciin]×n⃗=M⃗nreact×n⃗=0⃗,
where ns is the number of structural elements, PCi⃗ is the relative position vector of the center of mass Ci of the ith structural segment, F⃗iin and M⃗Ciin are the inertial force and inertial moment of the ith segment.Equation (19) is used for calculation of relative coordinates of the point P (ZMP/CP point).If the nominal joint angle trajectories are precalculated, they must provide location of P inside the contact area without reaching the foot edge, for the single support phase. Similarly, during the single support phase, the controller must take corrective actions such that P is kept from reaching the foot edge.


## 2.2.4. Error Dynamics

The nominal inputs to the plant (nominal torques), generated by the inverse plant (Figure13), are sufficient to drive an ideal system (exactly represented by the model) to follow precisely the desired joint trajectories. However, a mathematical representation has always more or less errors with respect to the system which is represented by that model.Disturbances can appear due to(i)
initial conditions,(ii)
effects which are not included in the model, since their way of action and the system response on them are unknown,(iii)
unavoidable delay in sensors (encoders, potentiometers, and A/D conversion),(iv)
intentionally simplifying the model mathematical description, since we rely on the control law to minimize the error, (e.g., leaving some friction effect not modeled), and so forth.The robot model architecture in the anatomical position has a vertical plane of symmetry (sagittal plane). Due to this symmetry, and cyclic nature of the nominal trajectories, the leg and arm joints on one side of the body have same nominal torques over the walking cycle, just shifted in phase, when the motion is restricted to the planar motion. Then, the control design of two single support phases and a double support phase completing a walking cycle can be narrowed down to a single support and a double support phase.Since the dynamics of the robot is significantly different during the single and the double support phase, it was convenient for this paper to consider two controller configurations. A short analysis shows that three degrees of freedom are removed for the planar consideration after both feet are fully in the contact with the ground, without slipping. Two translational degrees of freedom that are considered in the planar case and one rotational degree are removed after the second foot becomes immovable relative to the ground. Similarly, six degrees of freedom are removed in the 3D motion analysis; 3 translational and 3 rotational degrees are removed since the second foot becomes immovable relative to the ground.Moreover, for the sake of the simplicity of this discussion, the influence of the arms relative dynamics with respect to the trunk will be neglected and considered as a disturbance whose effect has to be minimized by the controller. The system (walking robot mathematical model) can be represented as follows. The state space representation of the three joint variables on the one leg of the robot model is(20)[ξ̇1ξ̇2ξ̇3ξ̇4ξ̇5ξ̇6]=[θ̇1θ̈1θ̇2θ̈2θ̇3θ̈3]=[ξ2f2(ξ)ξ4f4(ξ)ξ6f6(ξ)]+[000b21(ξ)b22(ξ)b23(ξ)000b41(ξ)b42(ξ)b43(ξ)000b61(ξ)b62(ξ)b63(ξ)][τ1τ2τ3].
Equation (20) can be expressed in a simplified form
(21)[ξ̇]=[f(ξ)]+[b(ξ)][τ],
where ξ is the state space vector related to the three joint angles and its derivative, as shown in (20). Although the torques distribution functions bij(ξ) (i=1,…,6;j=1,2,3) are shown to depend on entire state vector, precisely they depend on just evenly numbered components of the state vector (on the angles of the structure, not angular velocities). Further on, (21) will be used instead of (20) for the sake of simplicity.The real state space vector of the system isξ, which generally deviates from the nominal state vector ξ̅. The difference between the two vectors is denoted as the error vector ξ̃=ξ-ξ̅.Using the time derivative of the error vector and (21), the following is obtained
(22)ξ̃̇=ξ̇-ξ̅̇=f(ξ)+b(ξ)τ-f(ξ̅)-b(ξ̅)τ̅=f(ξ̃+ξ̅)+b(ξ̃+ξ̅)(τ̃+τ̅)-f(ξ̅)-b(ξ̅)τ̅=(f(ξ̃+ξ̅)-f(ξ̅))+(b(ξ̃+ξ̅)(τ̃+τ̅)-b(ξ̅)τ̅)≈∂f(ξ)∂ξ|ξ=ξ̅ξ̃+∂b(ξ)∂ξ|ξ=ξ̅ξ̃τ̅+b(ξ)|ξ=ξ̅τ̃,
that is
(23)ξ̃̇≈A(ξ̅)ξ̃+(bd(ξ̅)⊗ξ̃)τ̅+b(ξ̅)τ̃.
Equation (23) is used for the linearization-based controller design, where the matrix A(ξ̅) and the tensor of third-order bd(ξ̅) contain the derivatives which are used for the scheduled gains of the controller. The tensor bd(ξ̅) represents the change of the input torques distribution matrix function due to the change in the state space vector ξ, and it has the following form where every entry (bd)ij is a vector ((bd(ξ))ij)k=∂bij(ξ)/∂ξk, that is,
(24)bd(ξ)=[[∂b11(ξ)∂ξk]T[∂b21(ξ)∂ξk]T[∂b31(ξ)∂ξk]T[∂b41(ξ)∂ξk]T[∂b51(ξ)∂ξk]T[∂b61(ξ)∂ξk]T[∂b12(ξ)∂ξk]T[∂b22(ξ)∂ξk]T[∂b32(ξ)∂ξk]T[∂b42(ξ)∂ξk]T[∂b52(ξ)∂ξk]T[∂b62(ξ)∂ξk]T[∂b13(ξ)∂ξk]T[∂b23(ξ)∂ξk]T[∂b33(ξ)∂ξk]T[∂b43(ξ)∂ξk]T[∂b53(ξ)∂ξk]T[∂b63(ξ)∂ξk]T].
The symbol ⊗ denotes a tensor product such that every vector-entry (bd)ij performs a dot product with the error vector ξ̃ and finally the result (bd)ijkξ̃k performs the matrix multiplication with the column vector of the nominal input torques τ̅.The gain scheduled controller, based on (23) alone, cannot cope with the steady state errors. This is the reason why the state space vector ξ̃ is augmented with three integrators (they integrate the angle error for the ankle, knee and hip joints, resp.) as follows:
(25)[ξ̃aug]=[[ξ̃]Tς1ς2ς3]T,[ξ̃̇aug]=[[ξ̃̇]ς̇1ς̇2ς̇3]=[[A(ξ̅)][ξ̃]+([bd(ξ̅)]⊗[ξ̃])[τ̅]+[b(ξ̅)][τ̃]ξ̃1ξ̃2ξ̃3].
Stability of the gain scheduling controller is limited by the maximum allowed time derivatives of the desired joint angle trajectories, which are used as the scheduling variables
(26)∥q̇des(t)∥≤μ,
where qdes(t) is the vector of desired joint angles trajectories and μ is a positive constant  (Theorem 12.1 in [14]). The scheduling vector is of the nominal angles at the ankle, knee, and hip joints
(27)vs(t)=[ξ1d(t)ξ3d(t)ξ5d(t)]=[ξ̅1(t)ξ̅3(t)ξ̅5(t)],
and the condition (26) in the case of the numerical norm l1(28)∥[ξ̅̇1(t)ξ̅̇3(t)ξ̅̇5(t)]∥1=|ξ̅̇1(t)||ξ̅̇3(t)||ξ̅̇5(t)|≤μ,∀t.
The scheduling could have included all nominal state space variables, and can include also additional inputs for gain scheduling. However, the problem arises when nominal states are not known in advance (or partially known). In this case, one can use the inverse system to generate the nominal states based on the reference input/desired output.The constraint (26) is a serious limitation of the controller as a result of the “frozen time” concept application. However, application of the inverse system (Figure 13) significantly improved the controller capabilities, since the inverse system includes the time derivatives of the desired joint trajectories and the limitation (26) on the time derivatives of the scheduling variables (desired joint angles) is replaced by the limitation on the time derivatives of the error vector, that is the vector of the angular speeds deviations from the nominal values. For this design, μ is about 6 rad/s.The novelty in this design compared to traditional PID scheduled design is that this controller design includes nonlinear relations to schedule the gains based on the nonlinear system dynamics. The result is better capability of the controller to track the trajectories with higher time derivatives.The performances of the gain scheduled controller, combined with the inverse plant, are discussed following.


## 3. Simulation Results

Using the gain scheduling-based controller in the way shown in the block diagram of Figure13, the following results are obtained. Figure 15 shows the nominal torques at the hip, knee, and ankle joint, which are obtained using the inverse plant system.Nominal torques at the ankle, knee, and hip joint obtained via the inverse plant of the controller system.
(a)(b)(c)These nominal inputs to the controlled system are normalized with respect to the body weight (BW) multiplied by the total leg length (LL), such that the results are scalable with respect to the body mass (if the proportion of the body segments lengths and mass is maintained constant). The tracking performance of the controller for the ankle joint is shown in Figure16.Figure 16
Controller tracking performance for the ankle joint.The controller tracking performance for the knee joint angle is shown in Figure17.Figure 17
Controller tracking performance for the knee joint angle.For the hip joint, the goal of the controller was a regulation of the trunk angle with respect to the vertical direction. The controller performance of keeping this angle relatively small during the walking cycle is shown in Figure19.Finally, the ground force from the robot model is calculated and shown in Figure18, besides the ground force of the model with distributed mass.Ground reaction force calculated using the distributed mass model and compared to the measured force using the force plates method [13].
(a)(b)Figure 19
The controller tracking performance of the hip joint angle and regulation performance of the trunk angle via the hip torque.The controller design performances with results shown in the Figures16–18 are discussed next.


## 4. Results Discussion

Initial system response with a zero initial conditions (zero angles, which means the standing anatomical position, and zero angular velocities) looked a bit worse than the system response for the second cycle, whose results are shown in the figures. However, the steady walking cycle response is achieved very fast after the (zero) initial conditions and the second cycle is shown in the figures.The tracking error for the ankle and knee joints angles is not significant as shown in Figures16 and 17. Although the time derivatives are very high in a couple of subintervals of the cycle period, the gain scheduled controller succeeds to keep the response angle close to the desired path. The high time derivatives is always a questionable issue with gain scheduled controllers, since gain scheduled controllers are based on the frozen time concept. Accepting that the time derivatives are relatively high for this type of the control, the successful control performance can be justified in this case by the usage of the inverse plant such that the error dynamics-based controller has to correct just a small deviation from the nominal torques provided by the inverse plant.Although the desired angle trajectories (Figure9) showed that there is a small variation of the trunk angle with respect to the vertical direction, the control goal for the hip joint was set-up to maintain (regulate) a zero trunk angle. The results shown in the figure indicate a good regulation performance, since the angle variation remains within 1° range.Finally, the ground force, calculated based on the centers of gravity acceleration, shows relatively good agreement with the experimentally measured data. Small deviations can be partially explained by eventual deviations of the segmental lengths of the walking model from the real human segments, accompanied with some deviations of the recorded joints angle versus time functions from the real joint angle trajectories.


## 5. Conclusion

Advanced bipedal robot control techniques rely on an inverted dynamics of the bipedal robot model (reference model). Therefore, it is essential to create a precise but still relatively simple model that will provide accurate calculation of the relevant forces and torques needed to obtain particular set of desired joint trajectories.Two main groups of the bipedal walking models can be used for this purpose. The first group is consisted of relatively simple models but very useful for ground force analysis, kinetic, potential, and total energy balance analysis, energy recovered from potential to kinetic and vice versa by the walking mechanism, and other integral features of the bipedal body considered as a concentrated mass at the center of gravity. The second group of models consists of the models with distributed mass, which allow more detailed analysis such as torques at the walking structure joints (which cannot be measured directly in a human body), inertial effects due to the segmental motion with respect to the center of gravity, and control low design used to guide a bipedal robot.As a significant simplification of walking models with respect to the human body structure, one can consider the entire spinal region as a rigid body, since there is no significant bending and no significant contribution of the trunk bending dynamics to the overall walking structure dynamics. Other simplifications can be related to the types of the joints, the foot shape, and so forth.A walking robot tracking control can be based on human body joint trajectories. Those time-angle functions can be recorded for each joint using cameras, segmental markers, and image processing. The control system input requires nominal angular velocity and nominal angular acceleration per each actuated joint. Therefore, it is necessary to smooth out the nominal trajectories and pass the trajectories through a low-pass filter, since any discontinuity or a sudden change in the recorded data generates very high derivatives and can make the control system unstable.The cutoff frequency should be set-up such that the steepest parts of the nominal trajectory can pass through the filter without significant change. In this case, the low-pass filter cutoff frequency was 30 rad/s, for the joint trajectories and their first derivatives.The control system can be successfully designed and implemented in a form of a combination of an inverse plant model and a linearization based controller with gain scheduled according to the nominal state space vector change. The inverse plant basically generates the nominal control inputs (the joint torques). However, the inverse plant becomes noncausal if precise values of time derivatives of the reference input (desired output) are necessary. Alternatively, the derivatives can be predicted by the inverse plant. A disadvantage of this approach is the fact that the system can become unstable and need to be stabilized. In this work, a back stepping method was used.The gain scheduled error dynamics controller successfully made the system output follows the desired joint trajectories. Although the gain scheduling is a frozen time concept, which means that the time derivatives must (their absolute values) stay within a certain range in order to maintain controller performances and stability of the system, the simulation results showed remarkable tracking performances, which can be explained as a good prediction of the nominal torques via the inverse plant.The comparison of the model results for the ground force shows a satisfactory good agreement between the model results and measured data.The main problem with traditional control techniques for a bipedal robot is that a multiple input-multiple outputs systems (MIMOs), such as bipedal robots, are controlled joint by joint independently and the cross coupling of different input-output pairs is modeled as an external disturbance. These techniques are convenient in the cases when it is difficult to develop any reasonable model of the controlled system, but generally cannot provide high performances in cases of MIMO systems control.Systems such as bipedal robots require very precise desired trajectories tracking in order to balance the architecture during the walking steps. The idea proposed in this paper is that those cross couplings can be modeled precisely, rather then treat them as an external disturbance, and significantly improve the overall system performances. Particularly, the paper showed a novel nonlinear gain scheduled controller design combined with a concentrated mass reference model.Modeling the bipedal robot as a MIMO system, rather then considering the system as a number of independent joints with disturbance, means that we consider how anith actuator/torque influences a jth joint motion, in order to adjust the combination of the torques (inputs), such that the joints follow prescribed joint trajectories.



---

*Source: 278597-2010-09-30.xml*