Kinematics and Dynamics of Lower body of

Autonomous Humanoid Biped Robot

Abstract: Humanoid robots are typically complex in design, having numerous Degree-of-freedom.
This paper focus on the lower body of humanoid robot design with five degree of freedom per
each leg. This work present close equation and transform approach for the forward and inverse
kinematics by dividing the static walking gait into the sagittal and frontal plane. This paper
explains the mathematical model of the dynamics equations for the legs into the sagittal and frontal
plane by further applying the principles of Lagrangian dynamics. Finally, the partitioned
proportional derivative control law with gravity compensation was recurred in order to control the
desired trajectory. The paper contains several simulations to prove the analytical results, using
SimMechanics of MATLAB toolbox to verify the analytical result.
Index Terms: Humanoid Robot, Transform approach, Partitioned-proportional derivative, Lagrangian

Introduction:

MATLAB Model
Figure 1.Shows the model of lower body of humanoid robot which consists ten degree of freedom . Each leg has 5
degree of freedom with base attached to the top end of the torso.

Figure 1.Model prepared in Robo Analyzer Software

A complete link frame assignment [2] as shown in figure 2. Denavit_Hartenberg method used to calculate the
workspace of each leg. In this paper workspace of right leg is calculated and assume same for left leg. Humanoid body
is symmetrical about the sagittal plane. Table 1 gives the joint link parameter of leg. Applying the principle of
Denavit_Hartenberg step by step, further the workspace created by the right leg as ;
0
1𝑇 ∶= Rot(z, Ɵ1 )*Trans(0,0, l1)* Trans(0,0, 0)*Rot(x,90) (1)

1
2𝑇 ∶= Rot(z, Ɵ2 +90)*Trans(0,0, l2)* Trans(0,0, 0)*Rot(x,-90) (2)

2
3𝑇 ∶= Rot(z, Ɵ3-90)*Trans(0,0, 0)* Trans(0,0, l3)*Rot(x,90) (3)

3
4𝑇 ∶= Rot(z, Ɵ4 )Trans(0,0, 0)* Trans(0,0, l4)*Rot(x,0) (4)

4
5𝑇 ∶= Rot(z, Ɵ)*Trans(0,0, 0)* Trans(0,0, l5)*Rot(x,0) (5)

Where 𝑖−1𝑖𝑇 is a transformation matrix relating the i-th coordinate frame to the( i-1)th coordinate

frame.

Figure4. Link-frame assignment of lower body

Table1: Joint link Parameters.

DH Parameters Link

1 2 3 4 5

Ɵi Ɵ1 Ɵ2+90 Ɵ3-90 Ɵ4 Ɵ5

di l1 l2 0 0 0

ai 0 0 l3 l4 l5

αi 90 -90 90 0 0

2. FORWARD AND INVERSE KINEMATICS

Forward kinematic [3] of lower body of humanoid robot depends on the link length and joint values. Knowing these
parameters, the workspace created by the right leg is
0 0 1 2 3 4
5𝑇 ∶= 1𝑇* 2𝑇* 3𝑇* 4𝑇* 5𝑇 =𝑇𝑡𝑜𝑒 (6)
Computing the workspace of lower body of humanoid robot, the legs are capable to travel on the ground to reach the
target.
4.1 Inverse kinematics
In the present environment, robot has to travel on the straight line path and incline path. Inverse kinematics [4] gives
the information about the joint values to reach in under different condition. Transform approach has been used to
calculate the inverse of the manipulator.
Path1 : frame{0)------frame{4}--------frame{5}

While moving in forward direction joint1, joint 4 and joint 5 are coming in action. Applying these concept, the joint
values are obtained as.
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝒑𝒙
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝒑𝒚
𝑇𝑡𝑜𝑒 = [ ]= 05𝑇 (7)
𝑛𝑧 𝒏𝒛 𝒂𝒛 𝒑𝒛
0 0 0 1
Along this path the transformation 05𝑇 can be obtained as
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝒑𝒙
0 0 3 4 𝑛𝑦 𝑜𝑦 𝑎𝑦 𝒑𝒚
5𝑇 = 1𝑇 * 4𝑇 ∗ 5𝑇 = [
𝑛𝑧 𝒏𝒛 𝒂𝒛 𝒑𝒛
]
0 0 0 1
Applying the inverse approach of matrix, the joint values obtained as
(𝒑𝒚 −𝑙5 ∗𝑛𝑦)
Ɵ1 =tan−1 (𝒑
𝒙 −𝑙5 ∗𝑛𝑥 )

(𝒑𝒛 −𝑙5 ∗𝑛𝑧 −𝑙1 )

Ɵ4 = tan−1 2
(±√((𝒑𝒙 −𝑙5 ∗𝑛𝑥 )2 )+((𝒑𝒚 −𝑙5 ∗𝑛𝑦 ) ))

{(𝒑𝒚 +𝑙5 𝑛𝑦 )𝑙4 𝑛𝑥 −(𝒑𝒙 +𝑙5 𝑛𝑥 )𝑙4 𝑛𝑦}

Ɵ5 = tan−1
{(𝒑𝒙 +𝑙5 𝑛𝑥 )𝑙4 𝑜𝑦 +(𝒑𝒚 +𝑙5 𝑛𝑦)𝑙4 𝑜𝑥 }
To calculate the other joint values following the path2.

Path2: frame {0) ------frame {1} --------frame {2} -------frame {3} -----Frame {4} ------frame {5}

0 0 1
5𝑇 = 1𝑇 * 2𝑇 ∗ 23𝑇 * 34𝑇 * 45𝑇 =
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝒑𝒙
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝒑𝒚
[ ]
𝑛𝑧 𝒏𝒛 𝒂𝒛 𝒑𝒛
0 0 0 1
Since Ɵ2 and Ɵ3 are unknown. The inverse of matrix 45𝑇 and 34𝑇 can be computed. Substituting these values and post
multiplying with the matrix 05𝑇 can be computed.
0 4 -1 * 3 -1 0 1
5𝑇 * 5𝑇 4𝑇 = 1𝑇 * 2𝑇 ∗ 23𝑇 (8)

Values of Ɵ2 and Ɵ3 are obtained as follows.

(−(𝑜𝑧 ∗𝐶Ɵ45 +𝑛𝑧 ∗𝑆Ɵ45 ))
Ɵ2 =tan−1 2
(±√(𝑜𝑥 ∗𝐶Ɵ45 +𝑛𝑥 ∗𝑆Ɵ45 )2 +((𝑜𝑦 ∗𝐶Ɵ45 +𝑛𝑦 ∗𝑆Ɵ45 ) ))

(𝑛𝑧 ∗𝐶Ɵ45 −𝑜𝑧 ∗𝑆Ɵ45 )

Ɵ3 = tan−1 (−𝑎𝑧 )

3. DYNAMIC ANALYSIS OF TORQUE

Lagrangian _Euler [5] approach used to calculate the joint torque. Torque is responsible to move the
lower body from starting point of sewage water treatment plant to target point near by the solid waste
particle.

The torque equation for the joint can be calculate as the Lagrangian- Euler mechanics.

1 𝑗−1
𝐿= 2 ∑𝑛𝑖=1 ∑𝑖𝑗=𝑖 ∑𝑖𝑘 𝑇𝑟 [( 𝑗−10𝑇 𝑄𝑗 𝑖 𝑇) 𝐼𝑖 ( 𝑘−10𝑇 𝑄𝑘 𝑘−1 𝑇
𝑖𝑇 )]𝑞̇ 𝑗 𝑞̇ 𝑘 +∑𝑛𝑖=1 𝑚𝑖 g 0𝑖𝑇 𝑖𝑖𝑟̅ (8)

Where 𝐿 is the Lagrangian , K is the kinetic energy of the system , and P is the potential energy o f the
system . The Lagrangian can now be differentiated in order to from the dynamic equation of motion.
According to the Lagrangian- Euler dynamics formulation, the generalized torque τi of the actuator at
joint i, to drive link i of the manipulator is given by
𝑑 𝛿𝐿 𝛿𝐿
τi =𝑑𝑡 (𝛿𝑞̇ )-𝛿𝑞 (9)
𝑖 𝑖
By substituting 𝐿 and carrying out the differentiation , the generalized torque τi applied to link I to n-DOF
manipulator is obtained.

τi =∑𝑛𝑗=1 𝑀𝑖𝑗 (𝑞)𝑞̈ 𝑗 +∑𝑛𝑗=1 ∑𝑛𝑘=1 ℎ𝑖𝑗𝑘 𝑞̇ 𝑗 𝑞̇ 𝑘 +𝐺𝑖 (10)

for i=1, 2………n

where

𝑀𝑖𝑗 = ∑𝑛𝑝=max(𝑖,𝑗) 𝑇𝑟 [ 𝑑𝑝𝑗 𝐼𝑝 𝑝𝑖𝑇𝑑 ]

𝜕(𝑑𝑝𝑘 )
ℎ𝑖𝑗𝑘 = ∑𝑛𝑝=max(𝑖,𝑗,𝑘) 𝑇𝑟 [ 𝜕𝑞𝑝
𝐼𝑝 𝑝𝑖𝑇𝑑]

𝑝
𝐺𝑖 = -∑𝑛𝑝=𝑖 𝑚𝑝 𝑔𝑑𝑝𝑖 𝑝𝑟̅𝑝

and
0 𝑗−1
𝑇 𝑄𝑗 𝑖𝑇 𝑓𝑜𝑟 𝑗 ≤ 𝑖
𝑑𝑖𝑗 = { 𝑗−1
0 𝑓𝑜𝑟 𝑗 > 𝑖
and
𝑂 𝑗−1 𝑘−1
𝑗−1𝑇𝑄𝑗 𝑘−1𝑇 𝑄𝑘 𝑖𝑇 𝑓𝑜𝑟 𝑖 ≥ 𝑘 ≥ 𝑗
𝜕𝑑𝑖𝑗
= { 𝑘−1 𝑗−1
𝜕𝑞𝑘
𝑂
𝑇𝑄𝑘 𝑘−1
𝑗−1𝑇 𝑄𝑗 𝑖𝑇 𝑓𝑜𝑟 𝑖 ≥ 𝑗 > 𝑘
0 𝑓𝑜𝑟 𝑖 < 𝑗 𝑜𝑟 𝑖 < 𝑘
From equation (8), (9) and (10)

The joint torque can be computed for all the joint. Joint torque depends on the joint values trajectory.

6. TRAJECTORY OF THE JOINT

Trajectory is the sequence of movement of the joint with respect to the time. Based on the study biomechanics of
human body the joint motion [6] range values are in the table2. When the torso of lower body has given an input in
Cartesian trajectory [7] to move the body from one point to another point. The joint trajectory [8] converts the
Cartesian space vales in the joint vales form. The fifth order polynomial is assuming by initial condition as initial
velocity and acceleration and final velocity and acceleration is zero.
Table2. Joint range motion
Joint Human leg(deg) Lower

Body

leg(deg)

Waist Joint1 Roll -15 to 130 -15 to 100

Joiint2 Pitch -30 to 45 0 to 45

Joint 3 Yaw -45 to 50 -45 to 45

Knee Joint4 -10 to 155 0 to 120

Ankle Joint5 -20 to 50 -20 to 40

Based on the polynomial calculation the joint trajectory for the lower body of each individual joint is
Ɵ1 =−45𝑜 + 7.2 𝑡 3 − 2.16 𝑡 4 + .1728𝑡 5 (11)

Ɵ2 =−15𝑜 + 9.2𝑡 3 − 2.76𝑡 4 + .2208𝑡 5 (12)

Ɵ3 = 3.6𝑡 3 − 1.08𝑡 4 + .0864𝑡 5 (13)

Ɵ4 = 9.6𝑡 3 − 2.88𝑡 4 + .2304𝑡 5 (14)

Ɵ5 =−20 + 4.8𝑡 3 − 1.44𝑡 4 + .1152𝑡 5 (15)
So when the lower body is reaching near by the target it will be in stable condition.