# Multiphase Trajectory Generation for Planar Biped Robot Using Direct Collocation Method

Authors: Yubin Liu; Shuai Heng; Xizhe Zang; Zhenkun Lin; Jie Zhao
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

Stability and energy efficiency are the main focuses in the bipedal robot field. In this paper, we apply a multiphase gait, which is different from the widely used two-phase gait, to improve the stability at the moment, when a biped robot transfers from the double support phase to the single support phase. Then, we create dynamic equations with contact forces in each phase using Lagrangian formulation. Furthermore, the direct collocation method is utilized to generate the optimal trajectory toward both stability and energy efficiency. Finally, the comparison between multiphase gait and two-phase gait is performed with numerical simulations. The results prove that multiphase gait increases the stability margin in the cost of slightly decreasing energy efficiency. Besides, both gaits show a similar human-like characteristic in hip height variation during walking.


---

## Body

## 1. Introduction

In the field of bipedal robot research, both stability and energy efficiency are always the main goals. At present, the bipedal robot still experiences low stability and high energy cost problem. This paper aims to dig toward this orientation further.Raibert developed a successful running bipedal robot with one-foot gait, which distinguishes the state of the leg into two phases, support or stance phase and flight phase [1]. The support phase is when the leg provides support, and the flight phase is when the leg is in the air. The leg switches two phases in strict alternation. As for the walk of the bipedal robot, most researchers suppose it consists of two successive phases, single support phase (SSP) and double support phase (DSP). These two phases are easily understood according to the above definition of support phase: SSP is a period when only one leg is in the support phase, and DSP is when both legs are in the support phase. Actually, the studies of human walking gait find that there are two subphases of DSP and SSP each [2]. Then, these two foot rotation subphases are applied to biped robot [3–5]. The degree of freedom (DoF) of the biped robot, which is used in this paper, varies in different phases. In SSP, the robot is fully actuated, and when considering the heel lift subphase, it is underactuated; in DSP, it is overactuated [6]. In this paper, we add the foot rotation subphase in DSP based on the two-phase gait, so there are only fully actuated SSP and overactuated DSP. The theoretical benefit of this gait is illustrated in Section 2.2. The finding of the pros and cons of this multiphase gait is our focus in this paper. This gait is close to that used in paper [7], except we use different methods to generate trajectory and give out the comparison with two-phase gait to analyze the benefit of this multiphase gait.Energy efficiency and stability have long been objectives of trajectory generation. For stability, the zero moment point (ZMP) [8] has been used since the last century. The ZMP is the point on the ground, where the aggregation of moments about this point is zero. If ZMP is within the convex sets consisting of regions where feet contact the ground, the robot feet would not rotate around ZMP, and the robot is considered stable. This stability criterion has been widely applied in biped like ASIMO, NAO, and HRP-2. There is a different method called hybrid zero dynamics (HZD) [9], which can be used to analyze the stability of periodic orbits. This method is good at solving underactuated systems, but comparing to the ZMP method, this method is relatively complex and hard to understand. Besides, we do not consider the underactuated phase here; thus, we choose the ZMP method in this paper.For optimal trajectory generation, the most frequently used method is parameter optimization, which firstly represents the joint trajectories as functions of time decided by a vector of real numbers and then takes these numbers as the decision variables of the optimization problem. Different gradient-free optimization methods, which do not need gradients to determine the search direction, are utilized to find these decision variables. Farzdpour et al. [7] used the genetic algorithm (GA) to obtain the key parameters in trajectory generation for a seven-link planar bipedal robot with multiphase. Elhosseini et al. [10] used A-C parametric whale optimization algorithm (WOA) to find optimal parameters of the hip trajectory making ZMP close to the middle of the support polygon in two-phase gait. This method is an efficient and tractable way of generating suboptimal trajectories. However, since this method reduced discrete optimization variables to a finite number, path constraints, which are along the whole trajectory period, like dynamic equation constraints, are difficult to satisfy. Besides, polynomial functions may arise undesirable oscillations of optimized functions or jerky variations at connecting points [11].Some researchers applied the differential dynamic programming (DDP) method on trajectory generation. Feng et al. [12] achieved online trajectory optimization by using the DDP method to derive the trajectory of the center of mass of the robot as a high-level trajectory. Budhiraja et al. [13] used DDP with Karush–Kuhn–Tucker constraint to generate the whole-body trajectory while tracking the centroidal trajectory. This method usually combines with a simplified model and can generate the trajectory in milliseconds so it can be applied online. However, the DDP approach needs the second-order derivatives of the system dynamics, which leads to the issue that is unsuitable for large problems, and does not have a unified solution for the problem with general inequality constraints [14].Except for the above parametric optimization and DDP method, the direct collocation method [15–17] is one promising alternative to effectively solve trajectory optimization problems [18]. It is especially suitable for highly complex problems. Besides, it can be solved with present nonlinear programming (NLP) solvers, which can converge with poor initial guesses and are extremely computationally efficient [19]. Most importantly, it overcomes some limitations of the above methods since it can satisfy a bunch of path constraints and do not need the second-order derivatives of the system dynamics, which is hard to derive for the complex full-dimensional dynamic equations.There are many works about the direct collocation method applied in the legged robot area. In 1999, Hardt et al. [20] implemented direct collocation method using software DIRCOL to generate minimum energy symmetric, periodic gaits for a five-link bipedal robot with SSP, and instantaneous DSP gait. Xi et al. [21] used both direct collocation and multiple shooting methods to figure out how increasing speed affects the choice of gait and also inverse, under the objective of minimizing the cost of transport for both a planar biped and planar quadruped. Multiple shooting method is one of the trajectory optimization methods. Compared to the direct collocation method, this method has benefits like simplicity and straightforward, but it suffers slow convergence speed and is sensitive to initial guesses [22]. Ma et al. [23] used nonlinear programming toolbox FROST [24] to generate walking gait on the slippery surface combining with the hybrid zero dynamics control method to achieve stable walking for the AMBER-3M planar robot with point feet. Li et al. [25] utilized a direct collocation method with the closed-form of centroidal momentum (CM), which is CM generated by ground reaction forces (GRFs) that should match with CM calculated from the robot’s generalized coordinates and velocities, to generate the trajectory for a five-link legged robot with the reduced numerical error.This paper aims to testify the benefits of three phases (SSP, DSP subphase1, and DSP subphase2) walking style and to generate the optimal trajectory for our seven-link planar biped robot toward energy efficiency and stability for this walking style. In this paper, we use the direct collocation method to generate the optimal trajectory for this walking style, which can increase the ZMP stability margin when biped robot transfers from DSP to SSP. The holonomic dynamic equations with contact forces in each phase are created by the Lagrangian method, while the contact impulse is not considered in our paper to keep the velocity continuity. Various constraints are considered in our optimization, like unilateral contact, friction cone, and given speed. We compare the generated two trajectories (three-phase gait and two-phase gait) in energy efficiency and stability and find that three-phase gait increases the stability although it is in the cost of slight energy cost, and direct collocation method brings about the human-like hip height trajectory during SSP, which is the cycloid curve.The rest of the paper is organized as follows.Section 2 illustrates the differences between the two-phase walking style and three-phase walking style. The procedure for deriving the multiphase dynamic model is also presented. Section 3 is about the implementation details of direct collocation for generating optimal trajectory. The objectives and constraints of optimization are provided. Section 4 presents the generated three-phase and two-phase gaits. The comparison between these two gaits is conducted. In the end, conclusions and discussion are presented in Section 5.


## 2. Multiphase Biped Model

### 2.1. Model Symbol Definition

The bipedal robot in this study is a seven-link planar biped robot, which owns six actuators. Every two adjacent links are connected by an actuated revolute joint. Figure1 presents the schematic biped robot. The symbols’ meanings and values are given in Table 1, and they are consistent in the paper. Most of the parameters match with our biped robot [26] except the parameters of torso since our robot now does not have the upper body. We add it here because the upper body can equilibrate the angular momentum of the swing leg. The parameters of the upper body, mainly the mass and the length, are calculated according to the ratio between the upper body and lower body in paper [27].Figure 1
Biped robot model.Table 1
Physical parameters of the biped model.
SymbolValueNamem1,m70.75 kgMass of footm2,m61 kgMass of tibiam3,m52.5 kgMass of femurm421.25 kgMass of torsol1,l70.16 mLength of footh1,h20.035 mHeight of footr1,r20.5Ratio of forefootl2,l60.298 mLength of tibial3,l50.279 mLength of femurl40.5678 mLength of torsoI1,I70.0025kgm2Rotational inertia of footI2,I60.0296kgm2Rotational inertia of tibiaI3,I50.0649kgm2Rotational inertia of femurI40.5709kgm2Rotational inertia of torsoForefoot is the length from toe to ankle. Inertia is about its center of mass.There are other data in Figure1 needed to be illustrated. The fixed coordinate is set in the heel of the support leg (in red). The direction of the X-axis is along the walk direction, and the direction of the Y-axis is vertical to the ground and point to the top. q=q1,q2,q3,q4,q5,q6,q7 are the generalized coordinates used for depicting the configuration of robot. They are measured from the horizontal line through the revolute point to the link, which is called absolute angle usually, and the positive direction is counterclockwise same with convention. P0,P1,P2, etc. are the position of revolute joints, which can be derived by q and robot parameters through kinematics. G1,G2,G3, etc. are points of center of mass (CoM) of each link. Usually, we set CoM in the geometry middle point of the link except for the foot we put it in the middle of the sole.


### 2.2. The Biped Robot Dynamic Model

Before we start to derive the dynamic model, we need to illustrate “multiphase” in the heading. In this paper, we only consider periodic gait. A gait is defined from one foot off the ground until the other foot leaves the ground.In this paper, we consider two different walking styles shown in Figure2. The upper one includes two phases: SSP (right half) and DSP (left half), and this walking style is widely used. To make it more clear, we picture the middle state in SSP and DSP. The single support phase is when one leg (swing leg) of the robot swings, and the other leg (support leg) is on the ground. The double support phase is the state when both legs are touching the ground. When using ZMP as a stability criterion, we need ZMP to stay within the support polygon (SP), which is the convex sets consisting of regions where feet contact with the ground, and the minimum distance from ZMP to the boundary of SP is called the stability margin. The more the stability margin, the more stable the robot. Since we limit our research to planar walk, the support polygon is a line along the X-axis and ZMP is a point in the X-axis.Figure 2
Two kinds of walking styles.For the upper walking style, in DSP, the SP is fromxAtoxB, where xA is the horizontal position of swing leg toe and xB is the position of support leg heel. In SSP, the SP is from xBtoxC, where xC is the horizontal position of support leg toe. So, when the robot switches from DSP to SSP, ZMP must go through the intersection point xB (xB=xAxB∩xBxC); i.e., the SP is only a point; thus, at this moment, the stability margin is zero, a small deviation will cause the robot to fall. To prevent this situation, the below walking style is given in Figure 2. This walking style divided the double support phase into two subphases (subphase1 and subphase2). In subphase1, the support foot rotates about its heel to flat, while the swing foot rotates about its toe to an intermediary angle, and its SP is from xAtoxB. In subphase2, the support foot keeps flat and the swing foot continues rotating until toe-off, which is the initial state of SSP and its SP is from xAtoxC. Therefore, at the moment, when robot switches from DSP subphase2 to SSP, the stability margin is a line between xB and xC (xBxC=xAxC∩xBxC). Comparing to the first walking style, the stability margin at transferring moment increases.Note when the robot transfers from SSP to DSP, the swing leg (in blue) becomes the support leg (in red), likewise in original support leg.The following is the procedure of deriving the equation of motion (EoM) for each phase. To simplify our procedure, we make the following modeling assumptions:The support leg foot is in full contact with the ground during SSP, and there is no slippage.During DSP, the support leg foot and swing foot rotate around a pin point, which does not slip.The instantaneous impact event, when swing foot contacts with ground, is avoided by making the contact velocity zero.


#### 2.2.1. Single Support Phase

Multibody dynamics with contact usually formulate as(1)Mqq¨+bq,q˙q˙+gq=τ+JTλ,where Mq∈Rnq×nq is the generalized mass matrix; nq is the dimension of q; q,q˙,q¨∈Rnq are the generalized position, velocity, and acceleration, respectively; bq,q˙∈Rnq×nq is the Coriolis and centrifugal matrix; gq∈Rnq is the gravitational term; τ∈Rnq is the generalized actuator forces; J∈Rnc×nq is the Jacobian matrix of contact points; nc is the number of contact points; and λ∈Rnc is the Cartesian contact forces.Then, we use the Euler–Lagrange formalism to derive the EoM of the single support phase. Since, in SSP, the link 1 does not move, i.e., can be regarded as the ground, we can eliminate link 1 and its correspondingq1. In the end, the EoM of single support phase can be written as(2)M1q6×6q¨+b1q,q˙6×6q˙+g1q6×1=B16×6u,where u=u1;u2;u3;u4;u5;u6 is the vector of actuator torques and B1 is the distribution matrix, which maps the actuator torques to generalized actuator forces. The actual dimension of matrices is illustrated in the bracket.


#### 2.2.2. Double Support Subphase1

In DSP, the biped robot is overactuated because there are two contacts (ground with support leg and swing leg). In kinematics, the robot is the closed chain, and contacts can be handled as kinematics constraints. In subphase1, the heel of support legP0 and toe of swing leg P7 are both in contact with the ground. Suppose the distance between P0 and P7 is Ds. We use P0x, P7x and P0y, P7y represent the X coordinate and Y coordinate of each point. Then, the holonomic kinematic constraints are as follows:(3)Ω1q=P0x−P7x−DsP0y−P7y=0.The Jacobian of holonomic constraints is(4)Jc1q=∂Ω1q∂q.Hence, the EoM of the robot in subphase1 formulates as follows:(5)M2qq¨+b2q,q˙q˙+g2q=Bu+Jc1qλ1.λ1 is unknown here, so we could not solve this equation now. One way to solve it is eliminating the λ1. First, we assume Jc1 is the full rank, so there is the orthogonal complement matrix of it, Jc1null, satisfying Jc1Jc1null=Jc1nullTJc1T=0. Now we transfer to derive Jc1null. We divide the generalized coordinates into two categories: independent (qin) and dependent coordinates (qde). In this phase, we can choose them as(6)qin,q1q2q3q4a5T,qde,q6q7T.Taking the derivative of equation (3), we gain(7)Jc1qq˙=0.Then, we combine equations (6) and (7):(8)Jc1inJc1deq˙inq˙de=Jc1inq˙in+Jc1deq˙de=0,where Jc1in=∂Ω1/∂qin and Jc1de=∂Ω1/∂qde.Then, we shiftq˙de to the left side yield:(9)q˙de=−Jc1de−1Jc1inq˙in.Thus,q˙ can be expressed as(10)q˙=q˙inq˙de=I−Jc1de−1Jc1inq˙in.Replacingq˙ in equation (7),(11)Jc1qI−Jc1de−1Jc1inq˙in=0.Given thatq˙in≠0, we can derive(12)Jc1qI−Jc1de−1Jc1in=0,Jc1null=I−Jc1de−1Jc1in.NowJc1null is derived. Thus, we can multiply both sides of equation (5) with it:(13)Jc1nullTM2qq¨+Jc1nullTb2q,q˙q˙+Jc1nullTg2q=Jc1nullTBu+Jc1nullTJc1qλ1=Jc1nullTBu.Taking the second derivative of equation (3), we get(14)Ω¨1q=J˙c1qq˙+Jc1qq¨=0.Combining equations (13) and (14) together, the final EoM of subphase1 can be written as(15)M2q¯q¨+b¯2q,q˙q˙+g2q¯=B¯2u,where(16)M¯2q=Jc1nullTM2Jc17×7,b¯2q,q˙=Jc1nullTb2J˙c17×7,g¯2q=Jc1nullTg202×17×1,B¯2=Jc1nullTB02×67×6.


#### 2.2.3. Double Support Subphase2

The procedure of deriving the dynamic model of subphase2 is similar to subphase1, except we need to add one more constraint forq1:(17)Ω2=Ω1q1−3π2=0.Then, the same procedure of subphase1 is followed to compute EoM, but with caution when choosing the dependent (qde2) and independent (qin2) coordinates. If we choose like in subphase1, corresponding Jc2de will be rank deficient; i.e., no inverse matrix exists; thus, we need to adjust qde2 and qin2 into(18)qin2,q4q5q6q7T,qde2,q1q2q3T.


#### 2.2.4. Transitions: SSP to DSP1, DSP1 to DSP2, and DSP2 to SSP

When the robot changes from SSP to DSP1 (an abbreviation of DSP subphase1), the role of legs switches; i.e., the original swing leg becomes the support leg and the same to the original support leg. Also, the fixed coordinate changes to the new support leg heel. Here, we do not consider the impact effect since impact could induce the destabilizing effects on the motion control of the robot. Thus, transition can be represented using the following equation:(19)qSSP=TqDSP1,q˙SSP=Tq˙DSP1,T=110111011.The transition from DSP1 to DSP2 (an abbreviation of DSP subphase2) and DSP2 to SSP, the generalized coordinates, and velocities are continuous.


## 2.1. Model Symbol Definition

The bipedal robot in this study is a seven-link planar biped robot, which owns six actuators. Every two adjacent links are connected by an actuated revolute joint. Figure1 presents the schematic biped robot. The symbols’ meanings and values are given in Table 1, and they are consistent in the paper. Most of the parameters match with our biped robot [26] except the parameters of torso since our robot now does not have the upper body. We add it here because the upper body can equilibrate the angular momentum of the swing leg. The parameters of the upper body, mainly the mass and the length, are calculated according to the ratio between the upper body and lower body in paper [27].Figure 1
Biped robot model.Table 1
Physical parameters of the biped model.
SymbolValueNamem1,m70.75 kgMass of footm2,m61 kgMass of tibiam3,m52.5 kgMass of femurm421.25 kgMass of torsol1,l70.16 mLength of footh1,h20.035 mHeight of footr1,r20.5Ratio of forefootl2,l60.298 mLength of tibial3,l50.279 mLength of femurl40.5678 mLength of torsoI1,I70.0025kgm2Rotational inertia of footI2,I60.0296kgm2Rotational inertia of tibiaI3,I50.0649kgm2Rotational inertia of femurI40.5709kgm2Rotational inertia of torsoForefoot is the length from toe to ankle. Inertia is about its center of mass.There are other data in Figure1 needed to be illustrated. The fixed coordinate is set in the heel of the support leg (in red). The direction of the X-axis is along the walk direction, and the direction of the Y-axis is vertical to the ground and point to the top. q=q1,q2,q3,q4,q5,q6,q7 are the generalized coordinates used for depicting the configuration of robot. They are measured from the horizontal line through the revolute point to the link, which is called absolute angle usually, and the positive direction is counterclockwise same with convention. P0,P1,P2, etc. are the position of revolute joints, which can be derived by q and robot parameters through kinematics. G1,G2,G3, etc. are points of center of mass (CoM) of each link. Usually, we set CoM in the geometry middle point of the link except for the foot we put it in the middle of the sole.


## 2.2. The Biped Robot Dynamic Model

Before we start to derive the dynamic model, we need to illustrate “multiphase” in the heading. In this paper, we only consider periodic gait. A gait is defined from one foot off the ground until the other foot leaves the ground.In this paper, we consider two different walking styles shown in Figure2. The upper one includes two phases: SSP (right half) and DSP (left half), and this walking style is widely used. To make it more clear, we picture the middle state in SSP and DSP. The single support phase is when one leg (swing leg) of the robot swings, and the other leg (support leg) is on the ground. The double support phase is the state when both legs are touching the ground. When using ZMP as a stability criterion, we need ZMP to stay within the support polygon (SP), which is the convex sets consisting of regions where feet contact with the ground, and the minimum distance from ZMP to the boundary of SP is called the stability margin. The more the stability margin, the more stable the robot. Since we limit our research to planar walk, the support polygon is a line along the X-axis and ZMP is a point in the X-axis.Figure 2
Two kinds of walking styles.For the upper walking style, in DSP, the SP is fromxAtoxB, where xA is the horizontal position of swing leg toe and xB is the position of support leg heel. In SSP, the SP is from xBtoxC, where xC is the horizontal position of support leg toe. So, when the robot switches from DSP to SSP, ZMP must go through the intersection point xB (xB=xAxB∩xBxC); i.e., the SP is only a point; thus, at this moment, the stability margin is zero, a small deviation will cause the robot to fall. To prevent this situation, the below walking style is given in Figure 2. This walking style divided the double support phase into two subphases (subphase1 and subphase2). In subphase1, the support foot rotates about its heel to flat, while the swing foot rotates about its toe to an intermediary angle, and its SP is from xAtoxB. In subphase2, the support foot keeps flat and the swing foot continues rotating until toe-off, which is the initial state of SSP and its SP is from xAtoxC. Therefore, at the moment, when robot switches from DSP subphase2 to SSP, the stability margin is a line between xB and xC (xBxC=xAxC∩xBxC). Comparing to the first walking style, the stability margin at transferring moment increases.Note when the robot transfers from SSP to DSP, the swing leg (in blue) becomes the support leg (in red), likewise in original support leg.The following is the procedure of deriving the equation of motion (EoM) for each phase. To simplify our procedure, we make the following modeling assumptions:The support leg foot is in full contact with the ground during SSP, and there is no slippage.During DSP, the support leg foot and swing foot rotate around a pin point, which does not slip.The instantaneous impact event, when swing foot contacts with ground, is avoided by making the contact velocity zero.


### 2.2.1. Single Support Phase

Multibody dynamics with contact usually formulate as(1)Mqq¨+bq,q˙q˙+gq=τ+JTλ,where Mq∈Rnq×nq is the generalized mass matrix; nq is the dimension of q; q,q˙,q¨∈Rnq are the generalized position, velocity, and acceleration, respectively; bq,q˙∈Rnq×nq is the Coriolis and centrifugal matrix; gq∈Rnq is the gravitational term; τ∈Rnq is the generalized actuator forces; J∈Rnc×nq is the Jacobian matrix of contact points; nc is the number of contact points; and λ∈Rnc is the Cartesian contact forces.Then, we use the Euler–Lagrange formalism to derive the EoM of the single support phase. Since, in SSP, the link 1 does not move, i.e., can be regarded as the ground, we can eliminate link 1 and its correspondingq1. In the end, the EoM of single support phase can be written as(2)M1q6×6q¨+b1q,q˙6×6q˙+g1q6×1=B16×6u,where u=u1;u2;u3;u4;u5;u6 is the vector of actuator torques and B1 is the distribution matrix, which maps the actuator torques to generalized actuator forces. The actual dimension of matrices is illustrated in the bracket.


### 2.2.2. Double Support Subphase1

In DSP, the biped robot is overactuated because there are two contacts (ground with support leg and swing leg). In kinematics, the robot is the closed chain, and contacts can be handled as kinematics constraints. In subphase1, the heel of support legP0 and toe of swing leg P7 are both in contact with the ground. Suppose the distance between P0 and P7 is Ds. We use P0x, P7x and P0y, P7y represent the X coordinate and Y coordinate of each point. Then, the holonomic kinematic constraints are as follows:(3)Ω1q=P0x−P7x−DsP0y−P7y=0.The Jacobian of holonomic constraints is(4)Jc1q=∂Ω1q∂q.Hence, the EoM of the robot in subphase1 formulates as follows:(5)M2qq¨+b2q,q˙q˙+g2q=Bu+Jc1qλ1.λ1 is unknown here, so we could not solve this equation now. One way to solve it is eliminating the λ1. First, we assume Jc1 is the full rank, so there is the orthogonal complement matrix of it, Jc1null, satisfying Jc1Jc1null=Jc1nullTJc1T=0. Now we transfer to derive Jc1null. We divide the generalized coordinates into two categories: independent (qin) and dependent coordinates (qde). In this phase, we can choose them as(6)qin,q1q2q3q4a5T,qde,q6q7T.Taking the derivative of equation (3), we gain(7)Jc1qq˙=0.Then, we combine equations (6) and (7):(8)Jc1inJc1deq˙inq˙de=Jc1inq˙in+Jc1deq˙de=0,where Jc1in=∂Ω1/∂qin and Jc1de=∂Ω1/∂qde.Then, we shiftq˙de to the left side yield:(9)q˙de=−Jc1de−1Jc1inq˙in.Thus,q˙ can be expressed as(10)q˙=q˙inq˙de=I−Jc1de−1Jc1inq˙in.Replacingq˙ in equation (7),(11)Jc1qI−Jc1de−1Jc1inq˙in=0.Given thatq˙in≠0, we can derive(12)Jc1qI−Jc1de−1Jc1in=0,Jc1null=I−Jc1de−1Jc1in.NowJc1null is derived. Thus, we can multiply both sides of equation (5) with it:(13)Jc1nullTM2qq¨+Jc1nullTb2q,q˙q˙+Jc1nullTg2q=Jc1nullTBu+Jc1nullTJc1qλ1=Jc1nullTBu.Taking the second derivative of equation (3), we get(14)Ω¨1q=J˙c1qq˙+Jc1qq¨=0.Combining equations (13) and (14) together, the final EoM of subphase1 can be written as(15)M2q¯q¨+b¯2q,q˙q˙+g2q¯=B¯2u,where(16)M¯2q=Jc1nullTM2Jc17×7,b¯2q,q˙=Jc1nullTb2J˙c17×7,g¯2q=Jc1nullTg202×17×1,B¯2=Jc1nullTB02×67×6.


### 2.2.3. Double Support Subphase2

The procedure of deriving the dynamic model of subphase2 is similar to subphase1, except we need to add one more constraint forq1:(17)Ω2=Ω1q1−3π2=0.Then, the same procedure of subphase1 is followed to compute EoM, but with caution when choosing the dependent (qde2) and independent (qin2) coordinates. If we choose like in subphase1, corresponding Jc2de will be rank deficient; i.e., no inverse matrix exists; thus, we need to adjust qde2 and qin2 into(18)qin2,q4q5q6q7T,qde2,q1q2q3T.


### 2.2.4. Transitions: SSP to DSP1, DSP1 to DSP2, and DSP2 to SSP

When the robot changes from SSP to DSP1 (an abbreviation of DSP subphase1), the role of legs switches; i.e., the original swing leg becomes the support leg and the same to the original support leg. Also, the fixed coordinate changes to the new support leg heel. Here, we do not consider the impact effect since impact could induce the destabilizing effects on the motion control of the robot. Thus, transition can be represented using the following equation:(19)qSSP=TqDSP1,q˙SSP=Tq˙DSP1,T=110111011.The transition from DSP1 to DSP2 (an abbreviation of DSP subphase2) and DSP2 to SSP, the generalized coordinates, and velocities are continuous.


## 2.2.1. Single Support Phase

Multibody dynamics with contact usually formulate as(1)Mqq¨+bq,q˙q˙+gq=τ+JTλ,where Mq∈Rnq×nq is the generalized mass matrix; nq is the dimension of q; q,q˙,q¨∈Rnq are the generalized position, velocity, and acceleration, respectively; bq,q˙∈Rnq×nq is the Coriolis and centrifugal matrix; gq∈Rnq is the gravitational term; τ∈Rnq is the generalized actuator forces; J∈Rnc×nq is the Jacobian matrix of contact points; nc is the number of contact points; and λ∈Rnc is the Cartesian contact forces.Then, we use the Euler–Lagrange formalism to derive the EoM of the single support phase. Since, in SSP, the link 1 does not move, i.e., can be regarded as the ground, we can eliminate link 1 and its correspondingq1. In the end, the EoM of single support phase can be written as(2)M1q6×6q¨+b1q,q˙6×6q˙+g1q6×1=B16×6u,where u=u1;u2;u3;u4;u5;u6 is the vector of actuator torques and B1 is the distribution matrix, which maps the actuator torques to generalized actuator forces. The actual dimension of matrices is illustrated in the bracket.


## 2.2.2. Double Support Subphase1

In DSP, the biped robot is overactuated because there are two contacts (ground with support leg and swing leg). In kinematics, the robot is the closed chain, and contacts can be handled as kinematics constraints. In subphase1, the heel of support legP0 and toe of swing leg P7 are both in contact with the ground. Suppose the distance between P0 and P7 is Ds. We use P0x, P7x and P0y, P7y represent the X coordinate and Y coordinate of each point. Then, the holonomic kinematic constraints are as follows:(3)Ω1q=P0x−P7x−DsP0y−P7y=0.The Jacobian of holonomic constraints is(4)Jc1q=∂Ω1q∂q.Hence, the EoM of the robot in subphase1 formulates as follows:(5)M2qq¨+b2q,q˙q˙+g2q=Bu+Jc1qλ1.λ1 is unknown here, so we could not solve this equation now. One way to solve it is eliminating the λ1. First, we assume Jc1 is the full rank, so there is the orthogonal complement matrix of it, Jc1null, satisfying Jc1Jc1null=Jc1nullTJc1T=0. Now we transfer to derive Jc1null. We divide the generalized coordinates into two categories: independent (qin) and dependent coordinates (qde). In this phase, we can choose them as(6)qin,q1q2q3q4a5T,qde,q6q7T.Taking the derivative of equation (3), we gain(7)Jc1qq˙=0.Then, we combine equations (6) and (7):(8)Jc1inJc1deq˙inq˙de=Jc1inq˙in+Jc1deq˙de=0,where Jc1in=∂Ω1/∂qin and Jc1de=∂Ω1/∂qde.Then, we shiftq˙de to the left side yield:(9)q˙de=−Jc1de−1Jc1inq˙in.Thus,q˙ can be expressed as(10)q˙=q˙inq˙de=I−Jc1de−1Jc1inq˙in.Replacingq˙ in equation (7),(11)Jc1qI−Jc1de−1Jc1inq˙in=0.Given thatq˙in≠0, we can derive(12)Jc1qI−Jc1de−1Jc1in=0,Jc1null=I−Jc1de−1Jc1in.NowJc1null is derived. Thus, we can multiply both sides of equation (5) with it:(13)Jc1nullTM2qq¨+Jc1nullTb2q,q˙q˙+Jc1nullTg2q=Jc1nullTBu+Jc1nullTJc1qλ1=Jc1nullTBu.Taking the second derivative of equation (3), we get(14)Ω¨1q=J˙c1qq˙+Jc1qq¨=0.Combining equations (13) and (14) together, the final EoM of subphase1 can be written as(15)M2q¯q¨+b¯2q,q˙q˙+g2q¯=B¯2u,where(16)M¯2q=Jc1nullTM2Jc17×7,b¯2q,q˙=Jc1nullTb2J˙c17×7,g¯2q=Jc1nullTg202×17×1,B¯2=Jc1nullTB02×67×6.


## 2.2.3. Double Support Subphase2

The procedure of deriving the dynamic model of subphase2 is similar to subphase1, except we need to add one more constraint forq1:(17)Ω2=Ω1q1−3π2=0.Then, the same procedure of subphase1 is followed to compute EoM, but with caution when choosing the dependent (qde2) and independent (qin2) coordinates. If we choose like in subphase1, corresponding Jc2de will be rank deficient; i.e., no inverse matrix exists; thus, we need to adjust qde2 and qin2 into(18)qin2,q4q5q6q7T,qde2,q1q2q3T.


## 2.2.4. Transitions: SSP to DSP1, DSP1 to DSP2, and DSP2 to SSP

When the robot changes from SSP to DSP1 (an abbreviation of DSP subphase1), the role of legs switches; i.e., the original swing leg becomes the support leg and the same to the original support leg. Also, the fixed coordinate changes to the new support leg heel. Here, we do not consider the impact effect since impact could induce the destabilizing effects on the motion control of the robot. Thus, transition can be represented using the following equation:(19)qSSP=TqDSP1,q˙SSP=Tq˙DSP1,T=110111011.The transition from DSP1 to DSP2 (an abbreviation of DSP subphase2) and DSP2 to SSP, the generalized coordinates, and velocities are continuous.


## 3. Trajectory Optimization

In the prior section, we get the dynamic models of the biped robot in all phases. In this section, we use the direct collocation method to generate the optimal trajectory.The direct collocation method converts the trajectory optimization problem into a nonlinear programming problem by approximating all the continuous functions in the original problem as polynomial splines. In this paper, we utilize the standard collocation method [22] with the Hermite–Simpson method. Our problem has three phases, so we need to use the multiphase method [18, 25]. It is like solving multiple single-phase problems simultaneously. The difference is there are linkage constraints between any two connected phases, hence combining into an integrated trajectory.The following are the details of the optimal trajectory problem.


### 3.1. Optimization Objective

In this paper, we aim to generate low energy costs and high stability trajectory. For the energy cost, one common criterion is the cost of transport (CoT), which is the energy cost of the average distance moved by the robot, but CoT is a hard cost function to optimize over since it usually derives discontinuous solutions [27]. Therefore, we use a simple but effective function—torque squared cost function:(20)ℒu=∫0T∑i=16uit2dt,where T is the time of trajectory.This cost function usually leads to smooth solutions. The smooth solution has three advantages. The first one is that it is easy to approximate for piecewise polynomial spline and thus decreases the error because of approximation. The second one is that it is tractable to apply to the real robot. The last one is that it can prevent the torque from becoming too large.As for stability, we use ZMP stability criterion. The ZMP point can be derived by the following equation:(21)ZMP=∑i=1nmiy¨i+gxi−∑i=1nmix¨iyi−∑i=1nIiq¨i∑i=1nmiy¨i+g,where x¨i, y¨i are the horizontal and vertical accelerations of Gi, n is the number of links, and g is the gravity acceleration.Then, we limit the actual ZMP point to our preplanned ZMP curve. Note we cannot directly useZMP−ZMPdes to measure the deviation from actual ZMP to the desired ZMP since the absolute function is not continuous. Therefore, we use the below hyperbolic tangent function to approximate absolute function:(22)ℒZMP=ZMP−ZMPdestanhZMP−ZMPdesα≈ZMP−ZMPdes,where ZMPdes is the desired ZMP and α is a parameter adjusting the smooth of the function.Combining these two objectives, the final objective is(23)ℒ=ℒu+kzmpℒZMP,where kzmp is the coefficient to balance the effects of ZMP and energy cost since the actual ℒZMP is much smaller than ℒu. By chosen suitable kzmp such that two costs are at the same order of magnitude; otherwise, the effects of ZMP would not arise.


### 3.2. Constraints

A parade of constraints is required to generate a feasible trajectory. For different phases, different parts are constrained to produce an ideal solution. Usually, the constraints can be divided into path constraints and boundary constraints. Path constraints are the restriction along the trajectory, while boundary constraints only take effect at the beginning and the end. The constraints applied in this paper are explained as follows (path constraints first):(1)
One of the basic constraints is avoiding the hyperextension of the knee joint and preventing the foot contact with the shin:(24)q2−q3≤0,q6−q5≤0,q7−q6−3π2≤0.(2)
Contact force constraints. Since we assume there is no slippage, the contact force must be within the friction cone. Besides, the contact is unilateral. These constraints correspond to the first two modeling assumptions:(25)Fx−μFy≤0,−Fy≤0,λx−μλy≤0−λy≤0DSP,whereFx and Fy denote forces in the support leg, which can be solved by Newton’s second law. λx and λy are forces in the swing leg, which occur in the DSP. μ is the friction coefficient.(3)
During SSP, the swing leg should be constrained to above the ground, and setting the ground clearance can make the trajectory more desirable. Two options to do this: one way is to keep the swing foot above continuous-time functionft, and the other way is to prescribe continuous state function fx. Here, we choose time-based foot clearance:(26)ft=ht−0t−TSSPTSSP/2−0TSSP/2−TSSP2,t∈0,TSSP,ft−P7yt≤0,ft−P71yt≤0,whereTSSP means the duration of SSP, h is a set parameter of the highest height, and P71 represents the position of swing foot heel.(4)
At the beginning of SSP (toe-off), theY-axis velocity of swing foot toe should be positive:(27)−P˙7y≤0.(5)
At the beginning of SSP, swing foot toe should be at the ground, and at the end of SSP, swing foot heel should be at the ground:(28)P7y0=0,P71TSSP=0,where 0 means the time is zero, i.e., beginning of SSP.TSSP means the duration time of SSP, i.e., end of SSP.(6)
We set the robot walk at a given speed (1 m/s). The ankle horizontal displacement in a step is constrained to 0.8 m, and the cycle time is prescribed asT=TDSP+TSSP=0.8s. However, in this paper instead of the constraint of ankle movement, we constrain the distance from swing toe to support heel at the beginning of SSP and support toe to swing heel at the end of SSP. Besides, according to the knowledge that the percentage of DSP is about 20% of the time and the other 80% is SSP [2], the TDSP and TSSP are determined:(29)P0x0−P7x0=LToe2Heel,P71xTSSP−P00xTSSP=LToe2Heel,whereP00 represents the position of support foot toe.(7)
Since no impact, we constrain the swing heel velocity is zero just before contact. This constraint corresponds to the third modeling assumption:(30)P˙71=0.(8)
Phase transition (linkage constraints): these constraints are corresponding to the transitions part of the last section. SSP to DSP1, there is the reset map (Equation (18)); DSP1 to DSP2, keep continuous; and DSP2 to SSP, keep periodic.The parameters used in optimization are shown in Table2.Table 2
Parameters of optimization.
SymbolValuekZMP1e4TDSP1[0.1 0.16] sTDSP0.2 sTSSP0.6 sT=TDSP+TSSP0.8 sα0.01h0.055 mLToe2Heel0.2495 mThe value of limits ofTDSP1 is determined by trial and error.


## 3.1. Optimization Objective

In this paper, we aim to generate low energy costs and high stability trajectory. For the energy cost, one common criterion is the cost of transport (CoT), which is the energy cost of the average distance moved by the robot, but CoT is a hard cost function to optimize over since it usually derives discontinuous solutions [27]. Therefore, we use a simple but effective function—torque squared cost function:(20)ℒu=∫0T∑i=16uit2dt,where T is the time of trajectory.This cost function usually leads to smooth solutions. The smooth solution has three advantages. The first one is that it is easy to approximate for piecewise polynomial spline and thus decreases the error because of approximation. The second one is that it is tractable to apply to the real robot. The last one is that it can prevent the torque from becoming too large.As for stability, we use ZMP stability criterion. The ZMP point can be derived by the following equation:(21)ZMP=∑i=1nmiy¨i+gxi−∑i=1nmix¨iyi−∑i=1nIiq¨i∑i=1nmiy¨i+g,where x¨i, y¨i are the horizontal and vertical accelerations of Gi, n is the number of links, and g is the gravity acceleration.Then, we limit the actual ZMP point to our preplanned ZMP curve. Note we cannot directly useZMP−ZMPdes to measure the deviation from actual ZMP to the desired ZMP since the absolute function is not continuous. Therefore, we use the below hyperbolic tangent function to approximate absolute function:(22)ℒZMP=ZMP−ZMPdestanhZMP−ZMPdesα≈ZMP−ZMPdes,where ZMPdes is the desired ZMP and α is a parameter adjusting the smooth of the function.Combining these two objectives, the final objective is(23)ℒ=ℒu+kzmpℒZMP,where kzmp is the coefficient to balance the effects of ZMP and energy cost since the actual ℒZMP is much smaller than ℒu. By chosen suitable kzmp such that two costs are at the same order of magnitude; otherwise, the effects of ZMP would not arise.


## 3.2. Constraints

A parade of constraints is required to generate a feasible trajectory. For different phases, different parts are constrained to produce an ideal solution. Usually, the constraints can be divided into path constraints and boundary constraints. Path constraints are the restriction along the trajectory, while boundary constraints only take effect at the beginning and the end. The constraints applied in this paper are explained as follows (path constraints first):(1)
One of the basic constraints is avoiding the hyperextension of the knee joint and preventing the foot contact with the shin:(24)q2−q3≤0,q6−q5≤0,q7−q6−3π2≤0.(2)
Contact force constraints. Since we assume there is no slippage, the contact force must be within the friction cone. Besides, the contact is unilateral. These constraints correspond to the first two modeling assumptions:(25)Fx−μFy≤0,−Fy≤0,λx−μλy≤0−λy≤0DSP,whereFx and Fy denote forces in the support leg, which can be solved by Newton’s second law. λx and λy are forces in the swing leg, which occur in the DSP. μ is the friction coefficient.(3)
During SSP, the swing leg should be constrained to above the ground, and setting the ground clearance can make the trajectory more desirable. Two options to do this: one way is to keep the swing foot above continuous-time functionft, and the other way is to prescribe continuous state function fx. Here, we choose time-based foot clearance:(26)ft=ht−0t−TSSPTSSP/2−0TSSP/2−TSSP2,t∈0,TSSP,ft−P7yt≤0,ft−P71yt≤0,whereTSSP means the duration of SSP, h is a set parameter of the highest height, and P71 represents the position of swing foot heel.(4)
At the beginning of SSP (toe-off), theY-axis velocity of swing foot toe should be positive:(27)−P˙7y≤0.(5)
At the beginning of SSP, swing foot toe should be at the ground, and at the end of SSP, swing foot heel should be at the ground:(28)P7y0=0,P71TSSP=0,where 0 means the time is zero, i.e., beginning of SSP.TSSP means the duration time of SSP, i.e., end of SSP.(6)
We set the robot walk at a given speed (1 m/s). The ankle horizontal displacement in a step is constrained to 0.8 m, and the cycle time is prescribed asT=TDSP+TSSP=0.8s. However, in this paper instead of the constraint of ankle movement, we constrain the distance from swing toe to support heel at the beginning of SSP and support toe to swing heel at the end of SSP. Besides, according to the knowledge that the percentage of DSP is about 20% of the time and the other 80% is SSP [2], the TDSP and TSSP are determined:(29)P0x0−P7x0=LToe2Heel,P71xTSSP−P00xTSSP=LToe2Heel,whereP00 represents the position of support foot toe.(7)
Since no impact, we constrain the swing heel velocity is zero just before contact. This constraint corresponds to the third modeling assumption:(30)P˙71=0.(8)
Phase transition (linkage constraints): these constraints are corresponding to the transitions part of the last section. SSP to DSP1, there is the reset map (Equation (18)); DSP1 to DSP2, keep continuous; and DSP2 to SSP, keep periodic.The parameters used in optimization are shown in Table2.Table 2
Parameters of optimization.
SymbolValuekZMP1e4TDSP1[0.1 0.16] sTDSP0.2 sTSSP0.6 sT=TDSP+TSSP0.8 sα0.01h0.055 mLToe2Heel0.2495 mThe value of limits ofTDSP1 is determined by trial and error.


## 4. Results

We solve trajectory optimization using ICLOCS2 [28] software with IPOPT [29] and linear solver MUMPS. We use random values between the lower limit and upper limit as the initial guess. For the two kinds of walking styles, we both solve 10 times from 10 random initial guesses. All optimizations are solved on a laptop computer (processor: 1.8 GHz quad-core Intel i7-8550U) with MATLAB R2019a. The average iterations and computation time are 593 iterations and 181 seconds for the first kind walking style. As for the second one, it takes much longer, and the average iterations and computation time are 1782 iterations and 546 seconds. Note here that we do not use any analytic information, and we only use IPOPT’s numerical approximation. Stability [30] is not considered here. We give out the details of the generated trajectory for each walking style and the comparison of these two walking styles.


### 4.1. Results of the First Walking Style

The curves of each joint angle and joint velocity are given in Figures3 and 4. The middle vertical line depicted in all figures of this section represents the dividing line, the left part is DSP, which also be divided into DSP1 and DSP2 by a middle vertical line in the second walking style, and the right is SSP. From these two figures, we can see the curves are smooth, and state switching from DSP to SSP is continuous. Since the ranges of q1 and q7 (angle of the foot) are different from others, they are above the others. Also, q˙5,q˙6, and q˙7 all experience large fluctuation during SSP since they swing from back to fore. We observe that the swing foot experiences the most fluctuation like a roller coaster, then the swing knee, and the last one is the swing hip. From the view of minimizing energy, it is reasonable first to adjust the minimum energy cost part. This gives us a hint of control method, and we should prioritize the control of the minimum energy cost part. The stick diagram is shown in Figure 5. During the medium of SSP, the knee joint is at the singularity position, and this leads to the cycloid curve of hip height, which is shown straightly in the next figure. The swing foot toe and heel are all above the ground clearance curve (green and yellow, respectively) during SSP. The hip height curve is shown in Figure 6. It depicts a more straightforward view of the cycloid curve during SSP.Figure 3
The joint angle trajectories of gait1.Figure 4
The joint velocity trajectories of gait1.Figure 5
Stick diagram of gait1.Figure 6
The hip height curve of gait1.


### 4.2. Results of the Second Walking Style

The optimal value ofTDSP1 is 0.1161 s, which is about 58% of the whole DSP period. The joint angle and velocity trajectories of this walking style are plotted in Figures 7 and 8, respectively. They are also smooth and continuous when switching between different phases. Figure 9 illustrates the stick diagram. The hip height trajectory is depicted in Figure 10. The height of the hip joint is also a cycloid curve.Figure 7
The joint angle trajectories of gait2.Figure 8
The joint velocity trajectories of gait2.Figure 9
Stick diagram of gait2.Figure 10
The hip height curve of gait2.


### 4.3. Comparison of Two Gaits

Comparing Figures3 and 7, the joint angle trajectories are basically the same. As for the joint velocity (Figures 4 and 8), comparing to the first walking style, the scope of fluctuation in the second walking style when transferring from DSP to SSP is slightly bigger and at the end of SSP is much smaller. The energy cost of these two walking styles is different. Since both walking styles experience the same distance and time, we can directly use the equation below without dividing the distance to compare:(31)power=∑k=1Nτkqk,where N is the number of collocation points (20 in this paper) and τ is the generalized actuator torques in equation (1).The energy cost of gait2 is 1040 joule, and it is slightly more than gait1 (915 joule). Although the second gait increases the cost of energy a little, it raises the stability. Figure11 illustrates the trajectory of ZMP, the left one is gait1, and the right one is gait2. Both ZMP trajectories follow the desired ZMP and within the support polygon, which is the region between the black dotted line. In gait1, at the transferring moment from DSP to SSP, ZMP (depicts as the red circle) is at the ZMP upper limit of DSP, which leads to the trend of biped robot falling forward; also, it is at the ZMP lower limit of SSP, which leads to the trend of biped robot falling backward. In contrast, for gait2, ZMP at transferring moment is 0.053, which is one-third the distance between the ZMP lower limit and upper limit of SSP, so the stability margin is increased from 0 to 0.053 m, which prevents the falling down caused by small deviation, as we mentioned the benefit of this walking style at Section 2.2.Figure 11
The ZMP trajectories of two gaits. (a) The first gait. (b) The second gait.
(a)(b)


### 4.4. Effect of Weight between Energy Cost and Stability

We also investigate the influence of the weight between energy cost and stability on the result. We add a weightw to our objective function (Equation (21)):(32)ℒ=wℒu+1−wkzmpℒZMP,w∈0,1.We set five different weight valuesw∈0,0.25,0.5,0.75,1 and find that the energy cost decreases with the increase in weight in both gaits (Figure 12). Besides, when w<0.5, the energy cost of two-phase gait is more than three-phase gait, but when w≥0.5, it is the opposite. Note that when w=1, the two-phase gait could not get the optimal solution, so this point is not in Figure 12.Figure 12
The effect of weight on energy cost.For the effect of weight on ZMP (Figure13), when w≤0.5, although actual ZMP is slightly getting worse with the increase in weight, it still can follow the desired ZMP, but when w>0.5, it is getting very bad. Note that when w=1, the two-phase gait could not get the optimal solution, so there is only the ZMP curve of three-phase gait. Although when w<0.5, the energy cost of gait1 is more than gait2, and it seems opposite to our conclusion, but at this time, the energy cost is more than w=0.5, so our conclusion is still correct when considering energy cost and stability equally (w=0.5).Figure 13
The effect of weight on ZMP. (a)w = 0 (left: gait1, right: gait2). (b) w = 0.25. (c) w = 0.5. (d) w = 0.75. (e) w = 1.
(a)(b)(c)(d)(e)


## 4.1. Results of the First Walking Style

The curves of each joint angle and joint velocity are given in Figures3 and 4. The middle vertical line depicted in all figures of this section represents the dividing line, the left part is DSP, which also be divided into DSP1 and DSP2 by a middle vertical line in the second walking style, and the right is SSP. From these two figures, we can see the curves are smooth, and state switching from DSP to SSP is continuous. Since the ranges of q1 and q7 (angle of the foot) are different from others, they are above the others. Also, q˙5,q˙6, and q˙7 all experience large fluctuation during SSP since they swing from back to fore. We observe that the swing foot experiences the most fluctuation like a roller coaster, then the swing knee, and the last one is the swing hip. From the view of minimizing energy, it is reasonable first to adjust the minimum energy cost part. This gives us a hint of control method, and we should prioritize the control of the minimum energy cost part. The stick diagram is shown in Figure 5. During the medium of SSP, the knee joint is at the singularity position, and this leads to the cycloid curve of hip height, which is shown straightly in the next figure. The swing foot toe and heel are all above the ground clearance curve (green and yellow, respectively) during SSP. The hip height curve is shown in Figure 6. It depicts a more straightforward view of the cycloid curve during SSP.Figure 3
The joint angle trajectories of gait1.Figure 4
The joint velocity trajectories of gait1.Figure 5
Stick diagram of gait1.Figure 6
The hip height curve of gait1.


## 4.2. Results of the Second Walking Style

The optimal value ofTDSP1 is 0.1161 s, which is about 58% of the whole DSP period. The joint angle and velocity trajectories of this walking style are plotted in Figures 7 and 8, respectively. They are also smooth and continuous when switching between different phases. Figure 9 illustrates the stick diagram. The hip height trajectory is depicted in Figure 10. The height of the hip joint is also a cycloid curve.Figure 7
The joint angle trajectories of gait2.Figure 8
The joint velocity trajectories of gait2.Figure 9
Stick diagram of gait2.Figure 10
The hip height curve of gait2.


## 4.3. Comparison of Two Gaits

Comparing Figures3 and 7, the joint angle trajectories are basically the same. As for the joint velocity (Figures 4 and 8), comparing to the first walking style, the scope of fluctuation in the second walking style when transferring from DSP to SSP is slightly bigger and at the end of SSP is much smaller. The energy cost of these two walking styles is different. Since both walking styles experience the same distance and time, we can directly use the equation below without dividing the distance to compare:(31)power=∑k=1Nτkqk,where N is the number of collocation points (20 in this paper) and τ is the generalized actuator torques in equation (1).The energy cost of gait2 is 1040 joule, and it is slightly more than gait1 (915 joule). Although the second gait increases the cost of energy a little, it raises the stability. Figure11 illustrates the trajectory of ZMP, the left one is gait1, and the right one is gait2. Both ZMP trajectories follow the desired ZMP and within the support polygon, which is the region between the black dotted line. In gait1, at the transferring moment from DSP to SSP, ZMP (depicts as the red circle) is at the ZMP upper limit of DSP, which leads to the trend of biped robot falling forward; also, it is at the ZMP lower limit of SSP, which leads to the trend of biped robot falling backward. In contrast, for gait2, ZMP at transferring moment is 0.053, which is one-third the distance between the ZMP lower limit and upper limit of SSP, so the stability margin is increased from 0 to 0.053 m, which prevents the falling down caused by small deviation, as we mentioned the benefit of this walking style at Section 2.2.Figure 11
The ZMP trajectories of two gaits. (a) The first gait. (b) The second gait.
(a)(b)


## 4.4. Effect of Weight between Energy Cost and Stability

We also investigate the influence of the weight between energy cost and stability on the result. We add a weightw to our objective function (Equation (21)):(32)ℒ=wℒu+1−wkzmpℒZMP,w∈0,1.We set five different weight valuesw∈0,0.25,0.5,0.75,1 and find that the energy cost decreases with the increase in weight in both gaits (Figure 12). Besides, when w<0.5, the energy cost of two-phase gait is more than three-phase gait, but when w≥0.5, it is the opposite. Note that when w=1, the two-phase gait could not get the optimal solution, so this point is not in Figure 12.Figure 12
The effect of weight on energy cost.For the effect of weight on ZMP (Figure13), when w≤0.5, although actual ZMP is slightly getting worse with the increase in weight, it still can follow the desired ZMP, but when w>0.5, it is getting very bad. Note that when w=1, the two-phase gait could not get the optimal solution, so there is only the ZMP curve of three-phase gait. Although when w<0.5, the energy cost of gait1 is more than gait2, and it seems opposite to our conclusion, but at this time, the energy cost is more than w=0.5, so our conclusion is still correct when considering energy cost and stability equally (w=0.5).Figure 13
The effect of weight on ZMP. (a)w = 0 (left: gait1, right: gait2). (b) w = 0.25. (c) w = 0.5. (d) w = 0.75. (e) w = 1.
(a)(b)(c)(d)(e)


## 5. Conclusions and Discussion

Aiming to increase the stability of the biped robot when transferring from DSP to SSP, this paper divides DSP into two subphases. Then, we apply the direct collocation method, which is suitable for our problem and overcomes some limitations of parametric optimization and the DDP method, to generate the optimal multiphase trajectory toward stability and energy efficiency. In the end, we compare the three-phase trajectory with the traditional two phases. The comparison confirms that multiphase gait can increase stability although it consumes slightly more energy. Besides, both trajectories show the human-like cycloid curve, which makes the gait look more natural. In the future, we will use more accurate and effective direct collocation method, like orthogonal collocation methods [31] instead of the standard collocation methods, which we use here. Besides, we will apply this trajectory to a physical robot and try to reduce computation time through a less complex dynamic model like centroidal momentum model. We will generate various trajectories with different velocities and different foot ground clearances to build a library, which can make the robot more adaptive to different situations.



---

*Source: 6695528-2021-01-30.xml*