Torso trajectories

Up to now, the foot trajectories are derived with defined constraints. In this section, the torso trajectories are formulated based on the physical constraints. Usually θh(t) that denotes the angle of the torso in sagittal plane is selected as constant in walking to maintain the stability of robot. The common value is θh(t) = 90 degrees. Similarly, αh(t) and βh(t) which are the angles of the torso in transversal and frontal plane, respectively, are assumed to be constant and in special case equal to zero during walking.

49

Motion of the torso in the vertical direction z_h does not affect directly on the stability of the robot. So in order to match with human walking, a trajectory is specified that varies within a fixed range. Assuming that the hip is at its highest position 𝐻𝑚𝑎𝑥 at time 𝑇𝑚𝑎𝑥 that is usually selected at the middle of single-support phase, and at its lowest position 𝐻_𝑚𝑖𝑛 at time 𝑇_𝑚𝑖𝑛 that is usually selected at the middle of double-support phase. To control more on the shape of trajectory, it is desirable to specify position of 𝐻0 for 𝑡= 0 and 𝑡 = Tc. So zh has the constructed such that satisfy above constraints and the second derivative continuity.

The most important factor that affects the stability of biped robot walking in sagittal plane is x_h. To define constraints for x_h, two important parameters are defined; 𝑥_𝑠𝑑 and 𝑥_𝑒𝑑 that represent distance along the x-axis from the hip to the ankle of the support foot at the start and end of the single-support phase, respectively (see Fig. 4.6). so we have:

⎩⎪

To construct a smooth trajectory for x_h , the formulas presented in (Huang et al., 2001) are used. This formulas were derived using the third-order periodic spline interpolation.

So we have

50 The side movement of torso 𝑦h is very important for the stability of biped robot. In order to maintain stability of robot, the robot's center of gravity, in both case of static and dynamic walking, must be transferred from the rear foot to the front foot during the short double-support phase. This ability is added to our robot Archie by additional ankle joint that moves in frontal plane (Dezfouli, 2013). Hence we have the following constraints for 𝑦_h :

where 𝑤_𝑚 is the maximum distance that the torso should pass the support foot to maintain the stability of the robot during the single-support phase, see Fig. 4.8. Based on these constraints, the torso moves in y direction only in double-support phase and then during single-support phase has no side movement. In this manner, with proper selecting the parameter 𝑤𝑚, the moment of the center of gravity cancel the moment of the swing leg around the support area to maintain the stability of the robot. The trajectory of yh is constructed using cubic spline interpolation to meet the requirements of constraints.

51

Up to know the position and orientation trajectories of the feet and the torso are designed with respect to the base frame. Therefore, the trajectories of 𝑅₀^B, 𝑃₀^B, 𝑅₆^B and 𝑃₆^B are specified for a walking cycle. But as stated in chapter 3, in order to find the inverse kinematics of each leg, the position and orientation of the frame attached to the torso is needed with respect to the frame attached to the foot, i.e. 𝑃₆⁰ and 𝑅₆⁰ . Hence in the following, the needed position vector and rotation matrix are derived according to the planned trajectories using geometric relations.

In order to find the rotation matrix 𝑅₆⁰, we know from definition of the rotation matrix that 𝑅₆⁰ =𝑅_B⁰.𝑅₆^B 4-18 where

𝑅_B⁰ = (𝑅₀^B)^T 4-19 So 𝑅_B⁰ can be found as

𝑅₆⁰ = (𝑅₀^B)^T.𝑅₆^B 4-20 Therefore finding the rotation matrix of frame {0} ,attached to the foot, with respect to the base frame, i.e. 𝑅₀^B, and also the rotation matrix of frame {6} ,attached to the torso, with respect to the base frame, i.e. 𝑅6B, leads to find the rotation matrix of the frame {6} with respect to the frame {0}.

Fig. 4.9 shows the coordinates {0} and {6} which are attached to the left foot and the torso based on the D-H convention presented in chapter 3, respectively. The base frame {𝐵}

attached to the ground as defined in Fig. 4.5 is also shown in Fig. 4.9. This frame was used to define the trajectories of the feet and the torso during walking.

According to the Fig. 4.9 the frame {𝐵} can be rotated as follows to construct the frame {0}

for the left leg:

First 𝜋

�2 𝑑𝑒𝑔 rotation around the 𝑌𝐵 axis followed by 𝜋 𝑑𝑒𝑔 rotation around the new 𝑍 axis of the rotated coordinate. Then a rotation of 𝜃𝑎 around the new 𝑌 axis. Then we have two rotation of 𝛽_𝑎 and 𝛼_𝑎 around the previous fixed 𝑍 and 𝑋 axis, respectively. Now the rotation matrix can be found as:

𝑅₀^B= 𝑅_{𝑋,𝛼𝑎}𝑅_{𝑍,𝛽𝑎} 𝑅_𝑌,^𝜋

2 𝑅_𝑍,𝜋 𝑅_{𝑌,𝜃𝑎} 4-21 It is noted that the rotation matrix of a rotation around a new axis is post-multiplied and conversely the rotation matrix of a rotation around a fixed axis is pre-multiplied in above formula (Spong et al., 2006) .

52

Fig. 4.9 Position and orientation of the frames attached to the left foot and the torso with respect to the base frame

The rotation matrices around principal axes 𝑋, 𝑌 and 𝑍 can be found as:

𝑅_𝑋,𝜃 =�1 0 0 0 cos𝜃 −sin𝜃

0 sin𝜃 cos𝜃 � 4-22

𝑅_𝑌,𝜃 = � cos𝜃 0 sin𝜃

0 1 0

−sin𝜃 0 cos𝜃� 4-23

𝑅𝑍,𝜃 =�cos𝜃 −sin𝜃 0 sin𝜃 cos𝜃 0

0 0 1� 4-24 Substituting into the formula for 𝑅₀^B we have:

53 Similarly, the frame attached to the torso {6} can be constructed from the base frame {𝐵} by following rotation matrix:

54

Fig. 4.10 Position and orientation of the frames attached to the right foot and the torso with respect to the base frame

Similarly, according to Fig. 4.10, for the right foot we have the same formula for 𝑅₀^B and 𝑃₆⁰. The only difference is that the trajectories specified for the right foot should be utilized in the formulas.

The trajectory of angle 𝜃𝑎 for both feet that is specified in previous section, are used in above formula by multiplying by -1 due to different rotation definition.