Circular Obstacle Avoidance Control of the Compass-Type Biped Robot Based on a Blending Method of Discrete Mechanics and Nonlinear Optimization

This paper considers an obstacle avoidance control problem for the compass-type biped robot, especially circular obstacles are dealt with. First, a sufficient condition such that the swing leg does not collide the circular obstacle is derived. Next, an optimal control problem for the discrete compass-type robot is formulated and a solving method of the problem by the sequential quadratic programming is presented in order to calculate a discrete control input. Then, a transformation method that converts a discrete control input into a continuous zero-order hold input via discrete Lagrange-d’ Alembert principle is explained. From the results of numerical simulations, it turns out that obstacle avoidance control for the continuous compass-type robot can be achieved by the proposed method.


Introduction
Humanoid robots have been energetically researched in the fields of robotics and control theory so far.Especially, the compass-type biped robot has been mainly studied as one of the simplest models of humanoid robots.For example, theoretical analysis of passive walking [1]- [4], researches associated with nonlinear mechanics such as Poincáre section and limit cycles [5]- [8], gait pattern generation based on ZMP (zeromoment point) [9]- [11], self-motivating acquirement of gaits by learning theory and evolutionary computing [12]- [15].Generally, it is quite difficult to realize stable gaits for humanoid robots in terms of nonlinear problems, and hence there is still a lot of problems left to solve.In [16]- [19], the authors have studied gait generation problems for the compass-type biped robot based on discrete mechanics, which is a new discretizing tool for nonlinear mechanical systems and is derived by discretization of basic principles and equations of classical mechanics [20]- [23].It is known that a discrete model (the discrete Euler-Lagrange equations) in discrete mechanics has some interesting characteristics: 1) less numerical error in comparison with other numerical solutions such as Euler method and Runge-Kutta method; 2) it can describe energies for both conservative and dissipative systems with less errors; 3) some laws of physics such as Noether's theorem are satisfied; 4) simulations can be performed for large sampling times.Hence, discrete mechanics has a possibility of analysis and controller synthesis with high compatibility with computers.The authors have developed a new blending approach of discrete mechanics and nonlinear optimization for gait generation problems on flats, slopes and irregular grounds [16]- [19].
In this paper, we deal with an obstacle avoidance control problem for the compass-type biped robot via discrete mechanics.The contents of this paper is as follows.In Section 2, some fundamental concepts on discrete mechanics are summed up.Next, we derive the continuous and discrete compass-type biped robots based on both continuous and discrete mechanics, respectively in Section 3.Then, in Section 4, we formulate an obstacle avoidance control problem for the discrete compass-type biped robot and propose a solving method of it by the sequential quadratic programming to calculate a discrete control input.Furthermore, a transformation method from a discrete control input into a continuous zero-order hold input based on discrete Lagrange-d'Alembert principle is developed.In Section 5, we show some numerical simulations for the continuous compass-type biped robot in order to confirm the effectiveness of our method.Finally, we make a conclusion in Section 6.

Discrete Mechanics
This section summarizes fundamental concepts of discrete mechanics.See [16]- [28] for more details.Let Q be an n-dimensional configuration manifold and n q ∈ R be a generalized coordinate of Q.We also refer to q T Q as the tangent space of Q at a point q Q ∈ and q q T Q ∈  denotes a generalized velocity.Moreover, we consider a time-invariant Lagrangian as ( ) , : c L q q TQ →  R .We first explain about the discretization method.
The time variable t ∈ R is discretized as ( )  by using a sampling interval 0 h > .We denote k q as a point of Q at the time step k, that is, a curve on Q in the continuous setting is represented as a sequence of points in the discrete setting.The transformation method of discrete mechanics is carried out by the replacement: ( ) where q is expressed as a internally dividing point of k q and 1 k q + with an internal division ratio ( ) We then define a discrete Lagrangian: and a discrete action sum: Next, the discrete equations of motion is summarized.Consider a variation of points on Q as ( ) with the fixed condition 0 0 N q q δ δ = = as shown in Figure 1.In analogy with the continuous setting, we define a variation of the discrete action sum (3) as The discrete Hamilton's principle states that only a motion which makes the discrete action sum (3) stationary is realized.Calculating (4), we have where 1 D and 2 D denotes the partial differential operators with respect to the first and second arguments, respectively.Consequently, from the discrete Hamilton's principle and (5), we obtain the discrete Euler-Lagrange equations: with the initial and terminal equations: It turns out that ( 6) is represented as difference equations which contains three points , , k k k q q q − + , and we need 0 1 , q q as initial conditions when we simulate (6).Then, we consider a method to add external forces to the discrete Euler-Lagrange equations.By an analogy of continuous mechanics, we denote discrete external forces by ( ) where we define right/left discrete external forces: * , : F q q q F q q q F q q q F q q q δ δ δ δ respectively.By right/left discrete external forces, a continuous external force .
Calculating variations for (8), we obtain the discrete Euler-Lagrange equations with discrete external forces: with the initial and terminal equations:

Setting of Compass-Type Biped Robot
In this subsection, we first give a problem setting of the compass-type biped robot.In this paper, we consider a simple compass-type biped robot which consists of two rigid bars (Leg 1 and 2) and a joint without rotational friction (Waist) as shown in Figure 1.In Figure 2, Leg 1 is called the swing leg which is ungrounded and Leg 2 is called the supporting leg which connects to the ground.Moreover, for the sake of simplicity, we give the following assumptions; 1) the supporting leg does not slip at the contact point with the ground, 2) the swing leg hits the ground with completely inelastic collision, 3) the compass-type biped robot is supported by two legs for just a moment, 4) the length of the swing leg gets smaller by infinitely small when the swing leg and the supporting leg pass each other.Let θ and φ be the angles of Leg 1 and 2, respectively.We also use the notations: m: the mass of the legs, M: the mass of the waist, I: the inertia moment of the legs, a: the length between the center of gravity and the toe of the leg, b: the length between the waist and the center of gravity,

( )
l a b = + : the length between the waist and the toe of the leg.

Continuous Compass-Type Biped Robot (CCBR)
This subsection derives a model of continuous compass-type biped robot (CCBR) by using usual continuous mechanics.Denote the angles of Leg 1 and 2 by , θ φ , respectively, and assume that Leg 1 is the swing leg and Leg 2 is the supporting leg.In addition, , θ φ   denote their angular velocities.We now derive a model of the CCBR.We assume that the torque at the waist can be controlled, and denote it by v ∈ R .The Lagrangian of this system c L is given by ( Substituting the Lagrangian (13) into the Euler-Lagrange equations and adding the control input to the right-hand sides of them, we have the model of the CCBR as , , ,  , , ,  d .

Discrete Compass-Type Biped Robot (DCBR)
Next, we derive a model of discrete compass-type biped robot (CCBR) via discrete mechanics in this subsection.We here use the notations; h: the sampling time; 1, 2, , k N =  : the time step; 1 2 α = : the internal division ratio in discrete mechanics; , k k θ φ : the angles of Leg 1 and 2 at the k-th step.In this paper, we use only the model of the DCBR in the swing phases, and hence we will derive it.By using the transformation law from a continuous Lagrangian into a discrete Lagrangian (2), we obtain the discrete Lagrangian as from (13).Since the left and right discrete external forces (9) satisfy , , , we set a discrete control input that consists of only the left discrete external force d F − as ( ) Then, substituting ( 13) and ( 16) into the discrete Euler-Lagrange Equation ( 11) and the initial and terminal Equations (12), and adding the discrete control input (17) to these systems, we have the model of the DCBR as ( ) ( ) ( ) ( ) ( ) ( ) , , , , , , 0, ( ) ( ) , , , , , , 0, It is noted that the detailed model of the CCBR can be derived by calculating ( 18)-( 23) with ( 13) and ( 16).

Setting on Circular Obstacle
In this subsection, we first give the problem setting of circular obstacles.As shown in Figure 3, we set the x and z axes to the horizontal and vertical directions, respectively, and O denotes the origin of the xz-plane.In the xzplane, as a obstacle for the robot, we consider a semicircular region where o x is the center of a circle and o r is a radius of a circle.We call the resion (24) a circular obstacle.In addition, we set a point P as a desired grounding point for the swing leg of the robot.Based on the setting above, we consider the following problem on the gait generation for the compass-type biped robot.
Problem 1: For the continuous compass-type biped robot (CCBR) ( 14), (15), we find a control input v such that the swing leg of the CCBR lands at a desired grounding point P with avoiding collision with a circular obstacle o C . In order to solve Problem 1 above, a new method based on discrete mechanics will be developed.The method consists of the two steps: 1) calculation of a discrete control input by solving a finite dimensional constrained nonlinear optimization problem (Subsection 4.2); 2) transformation of a discrete control input into a zero-order hold input by discrete Lagrange-d'Alembert principle (Subsection 4.3).

Discrete Gait Generation Problem
Next, we consider an obstacle avoidance control problem of a discrete gait for the DCBR instead of the CCBR.The discrete obstacle avoidance control problem for the DCBR is stated as follows.
Problem 2: For the discrete compass-type biped robot (DCBR) ( 18)-( 23), we find a sequence of the control input k u such that the swing leg of the DCBR lands at a reference grounding point P with avoiding collision with a circular obstacle o C . Before formulation of Problem 2, we have to consider a condition on collision with a circular obstacle.The following theorem gives us a condition such that collision with a circular obstacle does not occur for the CCBR.
Theorem 1: If the next inequality ( ) ( ) ) holds, then the swing leg of the CCBR does not collide the circular obstacle o C .(Proof) From Figure 1, it turns out that all the point on the swing leg can be represented by ( ) with a parameter s satisfying 0 1 s ≤ ≤ .Now, we denote the point ( 26) by ( ) Q s .A sufficient condition such that the swing leg of the CCBR does not collide the circular obstacle o C is that the distance between ( ) Q s and P is equal to or more than o r .Therefore, we can obtain the inequality condition (25). When we implement the inequality condition (25) for numerical simulations, we have to make a modification for discretization.Hence, ( 25) is modified as where S is the number of s.That is to say, we check the inequality condition for a finite number of s.Then, Problem 2 can be formulated as the following nonlinear optimization problem: 1 1 1 1 given , , , , , , , .In the formulation above, ( 28) is a cost function on a sum of square of a discrete control input.We can see that the optimization control problem ( 28)-( 30) is represented as a finite dimensional constrained nonlinear optimization problem with respect to the ( ) . Therefore, we can solve it by the sequential quadratic programming [23] [29] [30], and obtain a sequence of discrete control input 1 1 , , N u u −  .

Transformation to Continuous Zero-Order Hold Input
In the previous subsection, we show a synthesis method of a discrete control input for the DCBR by solving a finite dimensional constrained nonlinear optimization problem.However, the obtained discrete control input cannot be utilized for the CCBR.So, we here consider transformation of a discrete control input into a continuous one.
There exist infinite methods to generate a continuous control input from a given discrete one, and a continuous control input generated from a given discrete input has to be consistent with laws of physics.Hence, in this paper, we deal with a zero-order hold input in the form: which is one of the simplest continuous inputs.We need to derive a relationship between a discrete input ( ) and a zero-order hold input (31).By using discrete Lagrange-d'Alembert's principle which is explained in Section 2, we can obtain the following theorem.
Theorem 2: A zero-order hold input (31) that satisfies discrete Lagrange-d'Alembert's principle is given by 2 .
(Proof) For the time interval ( ) , substituting (17) and (31) into the definition of the left discrete external force in (9): Hence, we can have (32).
 By using (32) in Theorem 2, we can easily calculate a zero-order hold input from , 1, , which are obtained by solving a finite dimensional constrained nonlinear optimization problem ( 28)- (30).In addition, it must be noted that since we use discrete Lagrange-d'Alembert's principle to prove Theorem 1, a zero-order hold input with a gain (32) is consistent with laws of physics.

Simulations
In this section, we shall carry out some numerical simulations on continuous gait generation on slopes for the CCBR via the method proposed in the previous section, and confirm the effectiveness of our method.First, the problem setting is given.we set parameters as follows; parameters of the DCBR and the CCBR:    9, a snapshot of the continuous gait is depicted.From these results, it can be confirmed that the control purpose on obstacle avoidance control is achieved by the proposed approach.It is noted that we ignore collision with the ground in the simulations, however, it can be avoided by adding a constraint to the optimization problem ( 28)- (30).

Conclusions
This paper has developed a new approach to circular obstacle avoidance control for the compass-type biped robot from the view points of discrete mechanics and nonlinear optimization.Simulation results have shown that a gait does not collide a circular obstacle can be generated, and hence we have confirmed the effectiveness of our new approach.
Our future work on control of humanoid robots via discrete mechanics includes the following themes: 1) extensions to various obstacles; 2) experimental evaluation of the proposed control method; 3) applications of discrete mechanics to more human-like robots and systems represented by partial differential equations.

Figure 3 .
Figure 3. Circular obstacle.The initial and desired states of the CCBR are illustrated in Figure 4.Moreover, an illustration of obstacle avoidance control for the CCBR is depicted in Figure 5.In order to solve Problem 1 above, a new method based on discrete mechanics will be developed.The method consists of the two steps: 1) calculation of a discrete control input by solving a finite dimensional constrained nonlinear optimization problem (Subsection 4.2); 2) transformation of a discrete control input into a zero-order hold input by discrete Lagrange-d'Alembert principle (Subsection 4.3).

Figures 6 - 9
Figures 6-9show the simulation results.Figure6illustrates the time series of Leg 1 and 2 (θ and φ ).Figure7shows the plot of solution trajectory in the phase space of θ φ − .The time series of the continuous control input is shown in Figure8.In Figure9, a snapshot of the continuous gait is depicted.From these results, it can be confirmed that the control purpose on obstacle avoidance control is achieved by the proposed approach.It is noted that we ignore collision with the ground in the simulations, however, it can be avoided by adding a constraint to the optimization problem (28)-(30).

Figure 6
Figures 6-9show the simulation results.Figure6illustrates the time series of Leg 1 and 2 (θ and φ ).Figure7shows the plot of solution trajectory in the phase space of θ φ − .The time series of the continuous control input is shown in Figure8.In Figure9, a snapshot of the continuous gait is depicted.From these results, it can be confirmed that the control purpose on obstacle avoidance control is achieved by the proposed approach.It is noted that we ignore collision with the ground in the simulations, however, it can be avoided by adding a constraint to the optimization problem (28)-(30).

Figure 7
Figures 6-9show the simulation results.Figure6illustrates the time series of Leg 1 and 2 (θ and φ ).Figure7shows the plot of solution trajectory in the phase space of θ φ − .The time series of the continuous control input is shown in Figure8.In Figure9, a snapshot of the continuous gait is depicted.From these results, it can be confirmed that the control purpose on obstacle avoidance control is achieved by the proposed approach.It is noted that we ignore collision with the ground in the simulations, however, it can be avoided by adding a constraint to the optimization problem (28)-(30).

Figure 8 .
Figure 8.Time series of control input.