COM Planning algorithm

Let us approximate the robot as a 3D linear inverted pendulum. Let us assume that all the mass of the robot is concentrated in its centre of mass (COM) and that it is connected to the ground (a huge 3D plane) by means of a massless leg, as depicted in Figure 24.

Figure 24: leg-hip system approximated as an inverted pendulum (Kuo, 2007)

27

It is possible to decompose the force 𝑓, named kick force in the three components parallel to the three axes:

It can be noticed that only gravity and the kick force act on the inverted pendulum. Its dynamics equations are then:

Let’s define the constraint plane on which the inverted pendulum will move:

𝑧 = 𝑘_𝑥𝑥 + 𝑘_𝑦𝑦 + 𝑧_𝑐 (3)

𝑘_𝑥 and 𝑘_𝑦 define the slope of the constraint plane while 𝑧_𝑐 define the height of the plane. The inverted pendulum can move on this plane only if its acceleration is perpendicular to it. Thus,

[𝑓 (^𝑥

If one solves equation (4) for 𝑓 and substitute it in the equation of the constraint plane, one get (5)

28 𝑓 =^𝑀𝑔𝑟

𝑧𝑐 (5)

Substituting equation (5) in the dynamic’s equation leads to the acceleration component of the inverted pendulum in the cartesian plane one gets:

𝑥̈ = ^𝑔 inverted pendulum moving along the constraint plane. The analytical solution of equations (6) is as follows:

For the x. For the y, similarly, one gets:

𝑦(𝑡) = 𝑦(0) cosh (^𝑡

29

Figure 26 depicts instead a walking pattern generated with the inverted pendulum model dynamics.

Every piece of the trajectory coloured with a different colour is the motion of an inverted pendulum having its own support base. The 𝑧_𝑐 parameter is considered fixed for every inverted pendulum model.

Figure 27 depicts one of the pieces of the generated walking pattern with the diagrams of the components of the velocity in the cartesian plane.

The piece of the computed trajectory depicted in Figure 27, is computed in the time period [0, 𝑇_𝑠𝑢𝑝] where 𝑇_𝑠𝑢𝑝 is called support period and it represents the time period in which one of the two feet of the robot is the support foot. This piece of trajectory is called walking primitive and, as it is possible to see in the figure, has a hyperbolic behaviour with a symmetry along the y axis.

Figure 25: Inverted pendulum moving on its constraint plane (Lee et.al., 2008)

30

Figure 26: Walking patter generated with the inverted pendulum model dynamics (Arbulú et.al., 2010)

Figure 27: inverted pendulum motion in the generated walking pattern and diagrams of the velocity on x and y with time (Kajita et.al., 2014)

31

Due to its symmetry, the walking primitive is defined only by its terminal position (𝑥̅ 𝑦̅). It is possible to compute the terminal speed (𝑣̅̅̅𝑥 𝑣̅̅̅) analytically. As a matter of facts, considering the _𝑦 walking primitive depicted in Figure 27, the initial conditions of the x are (−𝑥̅ 𝑣̅̅̅) and the initial _𝑥 condition of the y are (𝑦̅ −𝑣̅̅̅). By substituting the initial condition in the analytical solution of the _𝑦 inverted pendulum dynamic equation one get:

𝑥̅ = −𝑥̅𝐶 + 𝑇_𝐶𝑣̅̅̅𝑆_𝑥

𝑦̅ = 𝑦̅𝐶 − 𝑇_𝐶𝑣̅̅̅𝑆_𝑦 (10)

Solving equation (10) for the terminal velocity leads to:

𝑣_𝑥 purpose, it is frequently required to directly specify the foot placement (Kajita et.al., 2014).

The first footstep (𝑝_𝑥⁽⁰⁾ 𝑝_𝑦⁽⁰⁾) is the initial position of the first support foot. From the footstep positions, it is possible to determine the walk primitive as:

[𝑥̅^(𝑛)

32 Where 𝑠_𝑥 and 𝑠_𝑦are respectively step length and width.

As mentioned before, the walk primitive can be determined just by its terminal state. This can be noted in the equation where the n^th walk primitive depends from the n+1^th walk parameters.

The terminal velocity can then be calculated by (11) as follows:

[𝑣̅_𝑥^(𝑛) 𝑣̅_𝑦^(𝑛)] = [

(𝐶+1) 𝑇𝑐𝑆 𝑥̅^(𝑛)

(𝐶−1)

𝑇𝑐𝑆 𝑦̅^(𝑛)] (14)

The method creates a series of walk primitives which compose a walking pattern. The walk primitive computed in this way though are discontinuous and the walking pattern thus is not realizable. To solve this problem; it is possible to adjust its foot placement in order to control its speed. The intuitive explanation of this is depicted in Figure 28. On the left side of the figure it is depicted the process of speeding up by taking a shorter step and to slow down taking a longer step.

Figure 28: Intuitive explanation of the modified foot placement method (Kajita et.al., 2014)

Considering the modified foot placement in the dynamic equation of the inverted pendulum leads to (15).

33

From this set of equation, one can find the relation between the modified foot placement and the final state of the n^th step:

Consider as the target position, the final state of the walk primitive presented in the ground frame described by the (18) and (19) as shown in (20) and (21)

34 [𝑥^𝑑

𝑥̇^𝑑] = [𝑝_𝑥^(𝑛)+ 𝑥̅^(𝑛)

𝑣̅_𝑥^(𝑛) ] (20)

[𝑦^𝑑

𝑦̇^𝑑] = [𝑝_𝑦^(𝑛)+ 𝑦̅^(𝑛)

𝑣̅_𝑦^(𝑛) ] (21)

In order to compute the foot placement which ends with the closest final state to the target one can define the evaluation function:

𝑁_𝑥= 𝑎(𝑥^𝑑 − 𝑥_𝑓^(𝑛))² + 𝑏(𝑥̇^𝑑 − 𝑥̇_𝑓^(𝑛))² (22) 𝑁_𝑦 = 𝑎(𝑦^𝑑 − 𝑦_𝑓^(𝑛))²+ 𝑏(𝑦̇^𝑑 − 𝑦̇_𝑓^(𝑛))² (23)

With 𝑎 and 𝑏 positive weights. By setting to zero the derivatives of 𝑁_𝑥 and 𝑁_𝑦 with respect of 𝑝_𝑥^∗ and 𝑝_𝑦^∗ one can compute the modified foot placement which will minimize 𝑁_𝑥 and 𝑁_𝑦:

𝑝_𝑥^∗ = −^{𝑎(𝐶−1)}

𝐷 (𝑥^𝑑− 𝐶𝑥_𝑖^(𝑛)− 𝑇_𝐶𝑆𝑥̇_𝑖^(𝑛)) − ^𝑏𝑆

𝑇𝐶𝐷(𝑥̇^𝑑− ^𝑆

𝑇𝐶𝑥_𝑖^(𝑛)− 𝐶𝑥̇_𝑖^(𝑛)) 𝑝_𝑦^∗ = −^{𝑎(𝐶−1)}

𝐷 (𝑦^𝑑− 𝐶𝑦_𝑖^(𝑛)− 𝑇_𝐶𝑆𝑦̇_𝑖^(𝑛)) − ^𝑏𝑆

𝑇_𝐶𝐷(𝑦̇^𝑑− ^𝑆

𝑇_𝐶𝑦_𝑖^(𝑛)− 𝐶𝑦̇_𝑖^(𝑛)) 𝐷 = 𝑎(𝐶 − 1)²+ 𝑏 (^𝑆

𝑇𝐶)²

(24)

An example of walking pattern computed with the foot placement modification explained above is showed in Figure 29. In this figure the walking pattern generated is along a straight line. For changing the direction of motion, it is just necessary to make a few changings in the method described above.

35

Figure 29: Walking pattern generated with the modified foot placement method (Kajita et.al., 2014)

For changing the direction of motion, one just need to consider also the parameter 𝑠_𝜃 indicating the heading of the feet.

The walk primitive of the n^th steps is then:

[𝑥̅^(𝑛)

𝑦̅^(𝑛)] = [cos 𝑠_𝜃^(𝑛+1) − sin 𝑠_𝜃^(𝑛+1) sin 𝑠_𝜃^(𝑛+1) cos 𝑠_𝜃^(𝑛+1) ] [

𝑠_𝑥^(𝑛+1) 2

(−1)^{𝑛 𝑠𝑦}

(𝑛+1) 2

] (25)

And finally, the speed of the walk primitive becomes:

[𝑣̅_𝑥^(𝑛)

𝑣̅_𝑦^(𝑛)] = [cos 𝑠_𝜃^(𝑛+1) −sin 𝑠_𝜃^(𝑛+1) sin 𝑠_𝜃^(𝑛+1) cos 𝑠_𝜃^(𝑛+1) ] [

1+𝐶 𝑇𝐶𝑆𝑥̅^(𝑛)

𝐶−1

𝑇_𝐶𝑆𝑦̅^(𝑛)] (26)

Substituting these last equations with the correspondent ones in the method explained before will lead to a walking pattern with a direction changing. It is possible to note that, if 𝑠_𝜃^(𝑛) is zero, the equations will result in a straight trajectory as the one in Figure 29.

36

Figure 30 shows the walking pattern generated with the inclusion of the last walking parameter.

Figure 30: walking pattern with changing direction (Kajita et.al., 2014)

Im Dokument Footstep Planner and Environment Sensing System for a Cost-Oriented Humanoid Robot (Seite 37-47)