Inverse Kinematics of a Humanoid Robot with Non-Spherical Hip: A Hybrid Algorithm Approach

Abstract

This paper describes an approach to solve the inverse kinematics problem of humanoid robots whose construction shows a small but non negligible offset at the hip which prevents any purely analytical solution to be developed. Knowing that a purely numerical solution is not feasible due to variable efficiency problems, the proposed one first neglects the offset presence in order to obtain an approximate “solution” by means of an analytical algorithm based on screw theory, and then uses it as the initial condition of a numerical refining procedure based on the Levenberg-Marquardt algorithm. In this way, few iterations are needed for any specified attitude, making it possible to implement the algorithm for real-time applications. As a way to show the algorithm's implementation, one case of study is considered throughout the paper, represented by the SILO2 humanoid robot.

Keywords

Humanoid Robots Inverse Kinematics Non-Spherical Hip

1. Introduction

The kinematic modeling of humanoid robots is in fact a more difficult problem when compared to the classical industry manipulators, due to the locomotion that these robots are expected to perform while maintaining, at the same time, their postural stability. It is a formidable computational challenge, especially for real-time applications, due to the high number of degrees of freedom, the complex kinematics configurations that may lead to non-analytical solutions and the constantly changing point of support that is not rigidly attached to the floor [1]. In order to control the motion of these humanoid robots, specific trajectories in task space are required; that is, a desired sequence of attitudes for the feet and the body are required for accomplishing the desired stable locomotion task.

In the context of rigid multi-body systems, as is the case of robots, task space is different from joint space where motor commands are issued. Hence, movement planning requires either a preset sequence generated with play-back techniques or a suitable coordinate transformation from task to joint space. The former method is very simple as it does not require a complex mathematical analysis; however, it is not systematic as it is based on empirical tests for each movement, which make it tedious and non practical. The latter one, based on coordinates transformation, is supposed to be found by solving the so called Inverse Kinematics (IK) problem, which arises from the fact that the inverse transformation is often complex and ill-posed [2].

It is known that the IK problem cannot be solved by means of systematic processes as it is highly dependent on the robot's kinematic structure, and that it also either yields to infinite, multiple, one or none solutions. Only in simple cases, as in the case when the kinematic chain has no more than 6 degrees-of-freedom (dof) and three joint axes intersect, the IK problem for position and orientation can be decoupled, as stated by the so popular method proposed by Pieper in his PhD thesis [3, 4]. This is the case of many industrial manipulators and some of the most popular humanoid robots, i.e. ASIMO of Honda, HRP-2 of AIST and Kawada Industries or HOAP-2 of Fujitsu [5]. However, even for singularity-free mechanisms, the presence of offsets prevents this fact. Therefore, closed-form solutions are difficult, if not impossible, to find [6]. Or if they exist, they are very specific for a given kinematic chain [7, 8, 9].

For this reason, numerical methods have been commonly used to solve the IK problem, by using the pseudoinverse of the Jacobian (or a modification of it) [2, 10], the Cyclic Coordinate Descent (CCD) method [11, 12] or a procedure based on iterative triangulation [13, 14]. Some authors also use neural approaches [15, 16] or computational approaches based on conformal geometric algebra [17] or symbolic preprocessing [18]. However, these ones cannot be as efficient as the analytical ones, as they may require several iterations (not known a priori) in order to reach a particular precision, leading in many cases to computationally expensive solutions, which can be numerically unstable [19, 20]. Not to mention that a numerical algorithm may converge to a local minima instead of the desired global one, represented by the desired solution [21].

This makes it even more difficult to develop real-time applications, as these ones require fast and efficient algorithms for computing the gait patterns required to achieve dynamical stability. Therefore, finding an analytical solution to the IK problem, or a solution that takes a minimum of iterations, makes it possible to develop controllers able to compensate dynamical effects by changing motion patterns in real-time.

2. SILO2 Humanoid Robot

SILO2 stands for Locomotion System based on two limbs. This humanoid robot belongs to a family of walking prototypes that have been developed in the Department of Automatic Control at the Institute of Industrial Automation in Madrid, Spain [22].

This one in particular is currently on development and it is planned to be upgraded until getting a complete humanoid kinematic structure of 23 dof. However, in the current stage only the locomotor unit has been entirely developed (Figure 1).

Figure 1.

SILO2 Humanoid Robot [23]

As can be seen on this picture, this robot possesses 6 dof per leg: 3 at the hip, 1 at the knee, and 2 at the ankle. That is, each leg's mechanism is determined, so that each foot (or the body, depending on the basement link's election) can reach any desired attitude inside its workspace, defined by geometrical and mechanical constraints.

Joints are driven with two different types of actuators, as can be also noticed: Standard DC servomotors placed at the joint axis and non-linear transmission ratio actuators, used to improve the mechanical design by decreasing the energy consumption. The latter ones, patented as SMART (Special Mechatronic Actuator for Robot JoinTs) by the Department of Automatic Control at the Institute of Industrial Automation, are implemented as standard DC servomotors and four-bar linkages which provide the so called non-linear transmission ratio between the input angle driven by the servo and the output angle related to the joint variable.

Actually, three joints on each leg are driven by SMART actuators: One at the hip (abduction / adduction), one at the knee (flexion / extension), and one at the toe (flexion / extension), as can be seen on Figure 2a, 2b, and 2c.

Figure 2.

SMART actuators at (a) the hip, (b) the knee, (c) the ankle [23]

3. Humanoid Forward Kinematics

3.1. Kinematic diagram

In order to build the kinematic diagram of this robot it is worth to notice that the presence of SMART actuators does not add complexity to the kinematic model, as could be thought in a first sight. The four-bar linkages are just transmission mechanisms placed between the servomotors and the actual joints, such that the joint variable is indeed a function of the corresponding servomotor's angle.

Keeping this in mind, the kinematic structure of this robot can be represented with the diagram shown in Figure 3. In this diagram, joints are represented by cylinder-shaped icons, whose arrow represents its positive sense of motion. Each joint is labeled according to the following nomenclature: $J_{s i},$ where

Figure 3.

SILO2's kinematic diagram

$s : l (l e f t) or r (r i g h t) .$

$i :$ A number starting at 1 from the joint next to the body frame (base frame) and increasing until reaching the last joint in each kinematic chain.

Joint variables are labeled as $θ_{s i}$ according to the corresponding joint $J_{s i},$ such that each leg has a configuration determined by the vector $θ_{s} = {θ_{s 1}, θ_{s 2}, \dots, θ_{s 6}} .$

Links follow a similar nomenclature, $L_{s i},$ where $L_{s 0}$ refers to the base link (the body). In this way, joint $J_{s i}$ directly drives link $L_{s i} .$

Link dimensions are displayed along with the corresponding constant denoted by $l_{i},$ where side information is omitted due to symmetry reasons.

The body frame ${D}$ of the robot is oriented in such a way that the y axis points up and the x axis points in its movement direction. The z axis is then established to complete a right-hand frame.

At the reference attitude $(θ_{s} = 0),$ the feet frames (referred as ${S_{F}}$ ) have the same orientation of the body frame, so that their configuration with respect to the frame ${D}, {\bar{g}}_{d, s f} (0)$ [24], can be described as

{\bar{g}}_{d, s f} (0) = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & - (l_{1} + l_{2} + l_{3} + l_{4} + l_{f}) \\ 0 & 0 & 1 & ζ l_{0} \\ 0 & 0 & 0 & 1 \end{matrix}]

(1)

Where

(s = r) \Rightarrow (ζ = 1) (s = l) \Rightarrow (ζ = - 1)

(2)

3.2. Screw motion

Forward kinematics can be described in a more geometric way if a screw motion is associated to every joint [24]. A screw motion represents a rotation by an amount of θ about an axis l followed by a translation by an amount d parallel to that axis, as can be seen on Figure 4. This axis is represented as a directed line ω through a point $q;$ such that $l = {q + λ ω | λ \in ℝ} .$ The ratio $h = d / θ$ is known as the pitch of the screw.

Figure 4.

Generalized Screw Motion [24]

In this way, if a point p is operated with the equivalent rigid motion transformation $g,$ its final location (Figure 4) is given by

g p = q + e^{\overset{⌢}{ω} θ} (p - q) + h θ ω

(3)

Where $e^{\overset{⌢}{ω} θ}$ is the exponential matrix of the skew-symmetric matrix $\overset{⌢}{ω}$ corresponding to the unit vector $ω;$ that is, $e^{\overset{⌢}{ω} θ}$ is just another way for representing the rotation matrix $R (ω, θ) .$

By representing (3) in homogeneous coordinates, we have

\bar{g} \bar{p} = [\begin{matrix} e^{\overset{⌢}{ω} θ} & (I - e^{\overset{⌢}{ω} θ}) q + h θ ω \\ 0^{T} & 1 \end{matrix}] [\begin{matrix} p \\ 1 \end{matrix}]

(4)

The transformation $\bar{g}$ in (4) can also be represented as an exponential matrix, $\bar{g} = e^{\overset{⌢}{ξ} θ},$ of a matrix defined as

\overset{⌢}{ξ} = [\begin{matrix} \overset{⌢}{ω} & - ω \times q + h ω \\ 0^{T} & 1 \end{matrix}]

(5)

In this way, a screw motion associated to a rotational joint $(h = 0)$ with joint variable θ is given by

e^{\overset{⌢}{ξ} θ} = [\begin{matrix} e^{\overset{⌢}{ω} θ} & (I - e^{\overset{⌢}{ω} θ}) q \\ 0^{T} & 1 \end{matrix}]

(6)

3.3. Product of exponentials formula

The transformation ${\bar{g}}_{d, s f} (0)$ shown in (1) describes the attitude of the corresponding foot with respect to the body frame.

Suppose that we fix every joint except the last one $(J_{s 6}),$ which directly affects the foot's attitude, then the new attitude can be calculated as

{\bar{g}}_{d, s f} (θ_{s 6}) = e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} {\bar{g}}_{d, s f} (0)

(7)

In the same way, we can fix every joint except $J_{s 5},$ knowing that the foot's attitude has been modified by the $J_{s 6}' s$ action, then

\begin{array}{l} {\bar{g}}_{d, s f} (θ_{s 5}, θ_{s 6}) = e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} {\bar{g}}_{d, s f} (θ_{s 6}) \\ {\bar{g}}_{d, s f} (θ_{s 5}, θ_{s 6}) = e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} {\bar{g}}_{d, s f} (0) \end{array}

(8)

By composition, after taking into account every joint in the corresponding kinematic chain the foot configuration becomes

{\bar{g}}_{d, s f} (θ_{s}) = e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} \dots e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} {\bar{g}}_{d, s f} (0)

(9)

The expression in (9) is called the product of exponentials formula for the humanoid forward kinematics; that is, it describes the foot's attitude as a function of every joint in the kinematic chain [24].

3.4. Forward Kinematics parameters

As can be seen from expressions (9) and (6), in order to calculate the forward kinematics it is necessary to specify each exponential matrix $e^{{\overset{⌢}{ξ}}_{s i} θ_{s i}};$ that is, apart from the joint variable we need to define each axis direction $ω_{s i}$ and any point on each axis $q_{s i},$ as described in the body's reference frame ${D} .$

From the kinematic diagram shown in Figure 3, each axis direction is defined as

ω_{s 1} = {[\begin{matrix} 0 & 1 & 0 \end{matrix}]}^{T}

(10)

ω_{s 2} = {[\begin{matrix} 0 & 0 & - ζ \end{matrix}]}^{T}

(11)

ω_{s 3} = {[\begin{matrix} - 1 & 0 & 0 \end{matrix}]}^{T}

(12)

ω_{s 4} = {[\begin{matrix} 0 & 0 & ζ \end{matrix}]}^{T}

(13)

ω_{s 5} = {[\begin{matrix} 0 & 0 & ζ \end{matrix}]}^{T}

(14)

ω_{s 6} = {[\begin{matrix} 1 & 0 & 0 \end{matrix}]}^{T}

(15)

And a point on each axis is proposed as

q_{s 1} = {[\begin{matrix} 0 & 0 & ζ l_{0} \end{matrix}]}^{T}

(16)

q_{s 2} = {[\begin{matrix} 0 & - l_{1} & 0 \end{matrix}]}^{T}

(17)

q_{s 3} = {[\begin{matrix} 0 & - (l_{1} + l_{2}) & ζ l_{0} \end{matrix}]}^{T}

(18)

q_{s 4} = {[\begin{matrix} 0 & - (l_{1} + l_{2} + l_{3}) & 0 \end{matrix}]}^{T}

(19)

q_{s 5} = {[\begin{matrix} 0 & - (l_{1} + l_{2} + l_{3} + l_{4}) & 0 \end{matrix}]}^{T}

(20)

q_{s 6} = {[\begin{matrix} 0 & - (l_{1} + l_{2} + l_{3} + l_{4}) & ζ l_{0} \end{matrix}]}^{T}

(21)

3.5. Orientation coordinates

Consider the representation of the humanoid's foot (and its frame) as shown in Figure 5. The chosen orientation coordinates are three angles, taken as follows: First, consider a rotation of $θ_{s}$ about the Z axis (pitch), followed by a rotation of $θ_{f}$ about the X axis of the current frame (roll). Finally, consider a rotation of $θ_{t}$ about the Y axis (yaw) of the current frame; that is, about a normal vector to the foot's sole. In this way, the resulting rotation matrix $R_{f o o t}$ is given by

Figure 5.

Foot's frame definition

\begin{array}{l} R_{foot} = R (Z, θ_{s}) R (X, θ_{f}) R (Y, θ_{t}) \\ R_{foot} = [\begin{matrix} c_{s} c_{t} - s_{s} s_{f} s_{t} & - s_{s} c_{f} & c_{s} c_{t} + s_{s} s_{f} c_{t} \\ s_{s} c_{t} - c_{s} s_{f} s_{t} & c_{s} c_{f} & s_{s} s_{t} - c_{s} s_{f} c_{t} \\ - c_{f} s_{t} & s_{f} & c_{f} c_{t} \end{matrix}] \end{array}

(22)

Such that, from an arbitrary rotation matrix these rotation coordinates can be calculated as

θ_{f} = \arctan 2 (r_{32}, \pm \sqrt{1 - r_{32}^{2}})

(23)

θ_{s} = \arctan 2 (- r_{12}, r_{22})

(24)

θ_{t} = \arctan 2 (- r_{31}, r_{33})

(25)

4. Humanoid Inverse Kinematics

By looking at the kinematic diagram shown in Figure 3, it is possible to notice that the implementation of the humanoid's hip is not spherical; that is, the first three axes do not intersect in one point. There is an offset represented by the link $L_{s 2}$ which prevents the usage of the kinematic decoupling method proposed by Pieper [3] to solve the IK problem. In this way, alternative geometric analytical solutions were also sought without success. However, a purely numerical solution is not feasible either due to the existence of a variable efficiency on its implementation.

For this reason, a hybrid approach is proposed. It consists on first finding an approximate analytical “solution” for the inverse kinematics problem by neglecting the presence of the offset at the hip, as it is small when compared to the actual size of the robot, and then to use this “solution” as the initial condition of a numerical method, so that the number of iterations needed be drastically reduced.

This idea is similar to the one proposed in [6]. However, the last one only deals with the case of a double universal joint placed at the wrist. On the other hand, the following procedure is not restricted to any particular kinematic configuration, as long as the offset is small compared to the size of the robot.

4.1. Approximate analytical “solution”

First, suppose that the three axes at the hip intersect in just one point, represented by the actual intersection of the first two axes. Likewise, suppose that the length of the third link is not $l_{3},$ but $l_{2} + l_{3},$ in order to diminish the error.

By doing this, a purely analytical “solution” exists, so that it can be found by using the so-called Paden-Kahan methodology [24], which is based on the product of exponentials formula shown in (9) [1, 25].

The equation to be solved is

{\bar{g}}_{d, s f} (θ) = e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} \dots e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} {\bar{g}}_{d, s f} (0) : = {\bar{g}}_{d}

(26)

e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} \dots e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} = {\bar{g}}_{d} {\bar{g}}_{d, s f}^{- 1} (0) : = {\bar{g}}_{1}

(27)

To do that, let us propose the following four points based on the reference attitude of the robot shown in Figure 3:

A point $p_{H}$ located at the position of the supposed spherical hip; that is, at the intersection of the axes of the first two joints:

p_{H} : = {[\begin{matrix} 0 & - l_{1} & l_{0} \end{matrix}]}^{T}

(28)

A point $p_{A}$ located at the position of the ankle; that is, at the intersection of the axes of the two last joints:

p_{A} : = {[\begin{matrix} 0 & - (l_{1} + l_{2} + l_{3} + l_{4}) & l_{0} \end{matrix}]}^{T}

(29)

A point $p_{L}$ located on the axis of the first joint:

p_{L} : = {[\begin{matrix} 0 & 0 & l_{0} \end{matrix}]}^{T}

(30)

A point $p_{D}$ located at the origin of the body frame:

p_{D} : = {[\begin{matrix} 0 & 0 & 0 \end{matrix}]}^{T}

(31)

Having done this, the joint variables can be calculated as it is explained in the following [24]:

Calculation of $θ_{s 4}$ Let us apply both sides of (27) to the point $p_{A}$ and then, find the Euclidian distance between the transformation's result and the point $p_{H} .$ Notice that the point $p_{A}$ is located on the axes of the joints $J_{s 5}$ and $J_{s 6},$ so that the corresponding operators have no effect on this point and can be omitted. That is,

\begin{array}{l} ‖ e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} \dots e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} p_{A} - p_{H} ‖ = ‖ {\bar{g}}_{1} p_{A} - p_{H} ‖ \\ ‖ e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} \dots e^{{\overset{⌢}{ξ}}_{s 4} θ_{s 4}} p_{A} - p_{H} ‖ = δ \end{array}

(32)

In the same way, given that we have a similar situation at the hip

p_{H} = e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} e^{{\overset{⌢}{ξ}}_{s 3} θ_{s 3}} p_{H}

(33)

Equation (32) can be rewritten as

‖ e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} e^{{\overset{⌢}{ξ}}_{s 3} θ_{s 3}} (e^{{\overset{⌢}{ξ}}_{s 4} θ_{s 4}} p_{A} - p_{H}) ‖ = δ

(34)

Now then, recalling that a rigid body transformation preserves distances between points, the joint variable $θ_{s 4}$ can be found by recognizing the equivalent expression as the third Paden-Kahan sub problem (P-K-3) [24], whose solution is already known:

‖ e^{{\overset{⌢}{ξ}}_{s 4} θ_{s 4}} p_{A} - p_{H} ‖ = δ \underset{\to}{P - K - 3} θ_{s 4}

(35)

Calculation of $θ_{s 5}$ and $θ_{s 6}$ Let us reorder (27) and apply both sides to the point $p_{H},$ considering the fact shown in (33). In this way,

\begin{array}{l} e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} g_{1}^{- 1} p_{H} = e^{- {\overset{⌢}{ξ}}_{s 4} θ_{s 4}} e^{- {\overset{⌢}{ξ}}_{s 3} θ_{s 3}} \dots e^{- {\overset{⌢}{ξ}}_{s 1} θ_{s 1}} p_{H} \\ e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} g_{1}^{- 1} p_{H} = e^{- {\overset{⌢}{ξ}}_{s 4} θ_{s 4}} p_{H} \end{array}

(36)

Now then, as $θ_{s 4}$ is already known, it is possible to define the following points:

p : = g_{1}^{- 1} p_{H}

(37)

q : = e^{- {\overset{⌢}{ξ}}_{s 4} θ_{s 4}} p_{H}

(38)

So that, both $θ_{s 5}$ and $θ_{s 6}$ can be found by recognizing the expression in (36) as the second Paden-Kahan sub problem (P-K-2), after the proper substitution of $p and q$ [24]; that is,

e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} p = q \underset{\to}{P - K - 2} {θ_{s 5}, θ_{s 6}}

(39)

Calculation of $θ_{s 2}$ and $θ_{s 3}$ Let us reorder again (27) and apply both sides to the point $p_{L} .$

Remember that this point is located on the axis of $J_{s 1},$ so that the joint has no effect on this point. In this way,

\begin{array}{l} e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} e^{{\overset{⌢}{ξ}}_{s 3} θ_{s 3}} e^{{\overset{⌢}{ξ}}_{s 4} θ_{s 4}} \dots e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} g_{1}^{- 1} p_{L} = e^{- {\overset{⌢}{ξ}}_{s 1} θ_{s 1}} p_{L} \\ e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} e^{{\overset{⌢}{ξ}}_{s 3} θ_{s 3}} (e^{{\overset{⌢}{ξ}}_{s 4} θ_{s 4}} \dots e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} g_{1}^{- 1} p_{L}) = p_{L} \end{array}

(40)

Now then, given that $θ_{s 4}, θ_{s 5},$ and $θ_{s 6}$ are known, it is possible to define

p = e^{{\overset{⌢}{ξ}}_{s 4} θ_{s 4}} e^{{\overset{⌢}{ξ}}_{s 5} θ_{s 5}} e^{{\overset{⌢}{ξ}}_{s 6} θ_{s 6}} g_{1}^{- 1} p_{L}

(41)

In such a way that $θ_{s 2} y θ_{s 3}$ can be found by recognizing the expression in (40) as the second Paden-Kahan sub problem (P-K-2) again, after the proper substitution of p [24]; that is,

e^{{\overset{⌢}{ξ}}_{s 2} θ_{s 2}} e^{{\overset{⌢}{ξ}}_{s 3} θ_{s 3}} p = p_{L} \underset{\to}{P - K - 2} {θ_{s 2}, θ_{s 3}}

(42)

Calculation of $θ_{s 1}$ Reorder again (27) and apply both sides to the point $p_{D} .$ In this way,

e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} p_{D} = g_{1} e^{- {\overset{⌢}{ξ}}_{s 6} θ_{s 6}} \dots e^{- {\overset{⌢}{ξ}}_{s 2} θ_{s 2}} p_{D}

(43)

Now then, as all the other angles are already known, it is possible to define

q = g_{1} e^{- {\overset{⌢}{ξ}}_{s 6} θ_{s 6}} \dots e^{- {\overset{⌢}{ξ}}_{s 2} θ_{s 2}} p_{D}

(44)

In such a way that $θ_{s 1}$ can be finally found by recognizing the expression in (43) as the first Paden-Kahan sub problem (P-K-1), after the substitution of q [24]; that is,

e^{{\overset{⌢}{ξ}}_{s 1} θ_{s 1}} p_{D} = q \underset{\to}{P - K - 1} θ_{s 1}

(45)

It is worth to remark that this procedure may lead to a maximum of eight different joint configurations, due to the multiple solutions of P-K-2 and P-K-3 for equations (35), (39), and (42) (each one has two different solutions). However, it is possible to get rid of four among these ones, as it is natural to pre-select the knee-front configuration, and then to notice that the remaining four are not all valid ones, as not every constraint is considered in every calculation (the procedure required to get rid of information on constraints in order to apply the Paden-Kahan sub-problems).

By choosing the knee-front configuration only two solutions are valid, which can be selected by calculating the four candidate joint configurations and measuring the attitude's error generated by each one. Ideally, the two valid ones must lead to a zero attitude error; but, as this method is based on an approximation, only a minimum error may be achieved. In this way, the joint configuration which lead to lowest error is then selected as the “solution” of the problem.

4.2. Numerical refining

The approximate “solution” obtained by supposing a spherical hip generates significant errors that can not be neglected when dealing with gait generation. However, these ones are small compared to the actual size of the robot. This fact can be depicted in Figure 6, where both the real kinematic structure and the supposed one are shown having the same joint values.

Figure 6.

Real kinematic structure (lighter one) vs. supposed one (darker one)

As can be seen, for $θ_{s 3} = 16.35 °$ (which is actually the joint limit), there is an error of 2.787 cm with respect to the end-effector's position, whereas the robot's size is approximately 1 m. In this way, if such approximate “solution” is used as the initial condition of a numerical method, the algorithm will likely converge to the desired solution instead of getting trapped in a local minima. Furthermore, the number of iterations needed for accomplishing a negligible error can be drastically reduced in most of the cases. These statements rely in the fact that the convergence of any numerical method almost entirely depends on the selection of the initial values [20].

Numerical algorithms

A numerical algorithm for this problem basically increments (or decrements) the joint configuration θ by means of an increment vector $Δ θ,$ in a certain number of iterations; that is, $θ : = θ + Δ θ$ in each one. To compute this $Δ θ$ there are many available methods which primarily use the Jacobian $J,$ by taking advantage of the fact that [19]

Δ x \approx J Δ θ

(46)

Where $Δ x$ is an increment of the end-effector's attitude.

One way of doing this is using the Moore-Penrose's pseudoinverse of the Jacobian $J^{†},$ such that

Δ θ = J^{†} e

(47)

Where e is the error vector between the desired attitude and the actual one.

The pseudoinverse provides the “best fit” solution to a system of linear equations that lacks of unique solution. However, it is prone to be unstable in the neighborhood of singularities, as the singular values of $J^{†}$ are calculated from the inverse of the singular values of $J,$ which are small in the neighborhood of singularities [19].

The Levenberg-Marquardt's method (also known as the damped least squares' method) improves the pseudoinvere's behavior near the singularities, resulting in a stable way for computing $Δ θ .$ This is done by including a damping factor $λ,$ such that [19]

Δ θ = {(J^{T} J + λ^{2} I)}^{- 1} J^{T} e

(48)

The reason for such improvement is because the singular values $f_{i}$ of the matrix ${(J^{T} J + λ^{2} I)}^{- 1} J^{T}$ can also be expressed as

f_{i} = \frac{σ_{i}}{σ_{i}^{2} + λ^{2}}

(49)

Where $σ_{i}, i = 1, \dots, ρ {J}$ are singular values of $J .$

So, given that λ is small, if $σ_{i} ≫ λ,$ then $\frac{σ_{i}}{σ_{i}^{2} + λ^{2}} \approx \frac{1}{σ_{i}},$ such that both methods behave in a similar way. But, if $σ_{i} \approx λ,$ then $\forall λ > 0, \frac{σ_{i}}{σ_{i}^{2} + λ^{2}} \to 0$ as $σ_{i} \to 0,$ behaving in a stable manner [19]. This fact gives us the clue to calculate λ as

λ = \min {σ_{i}, λ_{\max}}

(50)

Attitude's error computation

In order to apply the Levenberg-Marquardt's method it is first necessary to define the error vector $e \in ℝ^{6},$ which is obtained by a concatenation of the positioning error $e_{t} \in ℝ^{3}$ and the orientation error $e_{R} \in ℝ^{3};$ that is

e = {[\begin{matrix} e_{t}^{T} & e_{R}^{T} \end{matrix}]}^{T}

(51)

Recall that the Jacobian relates differential increments in position and orientation of the end-effector to the differential increments of joint variables. Position's differential increments are effectively vectors with differential magnitude pointing in the movement's direction. In the other hand, orientation's ones are also vectors with differential magnitude, but pointing in the direction of the axis of rotation (unitary angular velocity vector).

Numerical algorithms deal with finite increments (errors), instead of differential ones (as an approximation). In this way, position and orientation errors are vectors with a finite magnitude, that point in the direction of the increment in the case of the former ones or in the direction of the axis of rotation in the case of latter ones.

So, whereas position error's calculation is straightfor-ward $e_{t} = {[Δ X Δ Y Δ Z]}^{T},$ orientation error's one is not so obvious. This is because the chosen orientation coordinates are not the Cartesian components of a rotational increment vector.

In order to obtain the desired orientation error from the chosen orientation coordinates error, it is necessary first to obtain an expression that relates the rate of change of those coordinates $({\dot{θ}}_{s}, {\dot{θ}}_{f}, {\dot{θ}}_{t})$ with respect to the angular velocity vector Ω of the corresponding foot.

Consider the matrix in (22), the correspondent skew-symmetric matrix of $Ω,$ written as $\overset{⌢}{Ω},$ can be calculated from [24]

\overset{⌢}{Ω} = {\dot{R}}_{foot} R_{foot}^{T}

(52)

From which the vector Ω can be extracted, so that

Ω = [\begin{matrix} 0 & c_{s} & - s_{s} c_{f} \\ 0 & s_{s} & c_{s} c_{f} \\ 1 & 0 & s_{f} \end{matrix}] [\begin{matrix} {\dot{θ}}_{s} \\ {\dot{θ}}_{f} \\ {\dot{θ}}_{t} \end{matrix}]

(53)

Having done this, the orientation error can be obtained by arbitrarily replacing the orientation coordinates rate of change with the corresponding error to build $e_{R};$ that is,

e_{R} = [\begin{matrix} 0 & c_{s} & - s_{s} c_{f} \\ 0 & s_{s} & c_{s} c_{f} \\ 1 & 0 & s_{f} \end{matrix}] [\begin{matrix} Δ θ_{s} \\ Δ θ_{f} \\ Δ θ_{t} \end{matrix}]

(54)

4.3. Workspace

Let us consider two types of workspace: the ideal one, $W_{i},$ defined by geometrical constraints; that is, by considering non-limited joints, and the real one, $W_{r},$ which takes into account the joint limits, so that $W_{r} \subset W_{i} .$

By having an analytical algorithm for the IK problem, it is possible to check a priori if a given attitude is inside the ideal workspace or not, in order to assure that a “solution” exists and avoid failures at the time of calculation.

In this case, an approximate workspace can be found by noticing that only the solution for P-K-3 considers a restricted-domain function, represented by the arccosine, arccos(.): $[- 1, 1] \to [0, π] .$ So that if an unreachable attitude is specified, the argument of the arccosine function $(D)$ will be a number outside its domain. In this way, if $- 1 < D < 1$ an approximate analytical “solution” exists.

5. Simulation results

In order to visually validate the algorithm proposed in this paper, a kinematic simulator was developed under the name of ARMS (Advanced Robot Motion Simulator) (Figure 7) [7].

Figure 7.

ARMS Kinematic Simulator

This simulator uses a VRML (Virtual Reality Modeling Language) model of the humanoid robot, which is driven by specifying its joint coordinates. These ones are directly specified by the user, or calculated from a desired attitude which is specified in task space.

For testing purposes a series of attitudes were specified for each foot, by considering the body as the base link. Some attitudes were proposed as lying inside the real workspace and others, outside, or as lying only inside the ideal workspace but not inside the real one, in order to test the algorithm's behavior. One example of each case is reported, along with some screenshots of the corresponding results.

5.1. Attitude inside the real workspace

Figure 8 shows the attitude reached by the model of the robot (frontal and sagittal views) after specifying that the right foot's sole should be located 75 cm below with respect to the pelvis and 20 cm just in front of it, while maintaining the reference attitude's distance between this foot and the sagittal plane of symmetry (17.3 cm). The right foot's orientation is also specified with a pitch angle of −20°. In the other hand, the location of the left foot's sole is specified to be 80 cm below with respect to the pelvis, 10 cm in front of it and 17.3 cm away from the sagittal plane of symmetry, while its orientation is specified with a roll angle of −10°, followed by a yaw angle of 30°.

Figure 8.

Resulting configuration after specifying an attitude inside the real workspace

As can be seen with the aid of superimposed labels and the screenshot of the specified attitude and the obtained one (Figure 9) the virtual robot not only reaches the desired goal, but it can be achieved with only 2 iterations.

Figure 9.

Control and feedback panels associated With Figure 8

5.2. Attitude inside the ideal workspace, but outside the real one defined by mechanical constraints

Figure 10 shows again an attitude reached by the model of the robot. In this case, the right foot's sole was specified to be located 75 cm below with respect to the pelvis and 20 cm just in front of it as before, but now it is also desired a distance of 30 cm between this foot and the sagittal plane of symmetry. In the other hand, the left foot's location is specified to be a the same height but 20 cm behind the pelvis, at a distance of 5 cm from the sagittal plane of symmetry. Both feet are expected to maintain the reference orientation; that is, 0° for all the orientation coordinates.

Figure 10.

Resulting configuration after specifying an attitude inside the ideal workspace, but outside the real one

However, the desired attitude cannot be reached as it can be confirmed by noticing that the right foot's sole is a little below of the desired one (marked with the aid of superimposed labels), and because the orientation of the left foot is not the reference one. This can also be confirmed in Figure 11, where the difference between the desired attitude and the obtained one is noticeable, due to the saturation of joint's values. The process needed 3 iterations in this case.

Figure 11.

Control and feedback panels associated with Figure 10

5.3. Attitude outside the ideal workspace

Consider now the case when a desired attitude is outside the ideal workspace, represented by the case when the feet are specified to reach a distance impossible to achieve, even at full extension of the legs. In this case the kinematic simulator performs no action at all, as indicated by the label “Ideally impossible attitude” (Figure 12).

Figure 12.

The attitude is not inside the ideal workspace

6. Evaluation

The performance of the proposed hybrid algorithm was evaluated with respect to the number of iterations needed to accomplish a desired precision by means of statistical comparison with a purely numerical solution, as well as with respect to the mean time required for performing the requested computations in both cases, as a way to measure the achieved improvement of performance.

This evaluation was carried out over different sets of data consisting on 1000 random 6-tuples of joint variables ${θ_{s 1}, θ_{s 2}, \dots, θ_{s 6}}$ each one. The leg was also chosen in a random manner.

Each 6-tuple, along with the leg's choice, was used to obtain the corresponding foot's attitude by means of applying the forward kinematics algorithm. The resulting attitude was then taken as the input of the IK algorithm. In this way, it was assured that this input would lead to a solution, which was already known for testing and comparison purposes.

When testing the proposed algorithm, the foot's attitude was used as the input of the approximate analytical algorithm in such a way that the approximate “solution” was taken as the initial condition of the Levenberg-Marquardt method.

When testing the purely numerical solution, the initial condition of the Levenberg-Marquardt method was set arbitrarily as $θ_{s} = 0.$

In both cases, an error vector magnitude less than 0.1 or a maximum of 1500 iterations were used as stop conditions. The maximum value for the parameter $λ_{\max}$ was also set to 0.01 (as this value led to the best performance through a set of tests). Then, the error between the desired attitude and the obtained one was computed and recorded, as well as the number of iterations required and the mean computation time.

This evaluation consisted on two different statistical populations described as follows:

The first one considered randomly generated attitudes inside the ideal workspace, but not necessarily inside the real one. These ones were obtained by constraining the random joint variables' values to −90^° and 90^°.

The second one considered randomly generated attitudes only inside the real workspace. In this case, the joint variables' values were constrained to the actual joint limits of the robot (Table 1).

Table 1.

Range of motion of each joint of SILO2 [23]

Joint		Range of motion
Hip rotation	Left Right	−180° ≤ θ_l1 ≤ 15° −15° ≤ θ_r1 ≤ 180°
Hip flexion/extension	Left Right	−70° ≤θ_l2 ≤ 70° −70° ≤ θ_r2 ≤ 70°
Hip abduction/adduction	Left Right	73° ≤ θ_l3 ≤ 106° −106° ≤ θ_r3 ≤ 73°
Knee	Left Right	−64° ≤ θ_l4 ≤−3° −64° ≤ θ_r4 ≤ −3°
Ankle flexion/extension	Left Right	−15° ≤ θ_l5 ≤ 20° −15° ≤ θ_r5 ≤ 20°
Ankle abduction/adduction	Left Right	−12° ≤ θ_l6 ≤ 12° −12° ≤ θ_r6 ≤ 12°

This evaluation tests were implemented in Matlab R2012a, and performed on a Laptop Dell XPS L702X, which has an Intel(R) Core(TM) i7-2670QM CPU @ 2.20 GHz, 8 GB in RAM, and a 64-bit Operating System (Windows 7 Utimate).

6.1. Random attitudes generated inside the ideal workspace, but not necessarily inside the real one

Figure 13a shows the histogram generated by the purely numerical approach, whereas Figure 13b shows the one generated by the proposed hybrid algorithm, obtained by constraining the random joint variables' values to −90° and 90°. In both cases, the histogram's range is set to 1 – 50 or more iterations (samples over 50 iterations are accumulated over 50 in the histogram).

Figure 13.

Computation efficiency comparison between (a) a purely numerical approach and (b) the proposed hybrid method, considering randomly-generated attitudes inside the ideal workspace

Table 2 also shows some numerical data associated with the evaluation; that is, the percentage of samples that succeeded to converge (within the specified precision) in 1500, 50, and 10 iterations, as well as the average error in position and orientation between the desired attitude and the obtained one (taking into account only the samples that succeeded to converge) and the mean time required per sample in both cases.

Table 2.

Performance comparison of both algorithms considering randomly-generated attitudes inside the ideal workspace

Indicator	Purely numerical	Hybrid solution
Convergence in 1500 it.	97.5%	99.5 %
Convergence in 50 it.	39.8%	92.6 %
Convergence in 10 it.	13.5%	91.4 %
Mean position error	0.0176 cm	0.0011 cm
Mean orientation error	0.0284°	0.0023°
Mean CPU time	71.663 ms	17.057 ms

As can be seen, the proposed algorithm shows an outstanding performance over the purely numerical one used in [19] or [21], as more than 91.4% of the samples are able to converge in 10 or less iterations, in contrast with the latter one which may need 50, 100, 1000 or more iterations to converge. In addition, the proposed algorithm is able to achieve a lower average error (with respect to position or orientation) while being almost 4 times faster.

6.2. Random attitudes generated inside the real workspace only

By taking into account the actual joint limits of the robot, both algorithms were compared once again. Figure 14a shows the histogram generated by the purely numerical approach, whereas Figure 14b shows the one generated by the proposed hybrid algorithm. The histogram's range is set again to 1 – 50 or more iterations. Table 2 shows the corresponding numerical data associated with this evaluation.

Figure 14.

Computation efficiency comparison between (a) a purely numerical approach and (b) the proposed hybrid method, considering randomly-generated attitudes inside the real workspace

Surprisingly, the proposed algorithm not only shows an outstanding performance over the purely numerical one, but a very good one feasible for implementation on real-time, as 99.5% of the samples converged in less than 10 iterations (in fact, 97.2% did it within 2 iterations). Notice that the average error is also lower than the previous case, and that the hybrid algorithm performs almost 5 times faster.

Table 3.

Performance comparison of both algorithms considering randomly-generated attitudes inside the real workspace

Indicator	Purely numerical	Hybrid solution
Convergence in 1500 it.	99.3%	99.9%
Convergence in 50 it.	61.8%	99.8 %
Convergence in 10 it.	45.0%	99.5 %
Mean position error	0.0025 cm	0.0006 cm
Mean orientation error	0.0042°	0.0002°
Mean CPU time	41.046 ms	7.762 ms

7. Conclusions

This paper proposed a semi-analytical algorithm for computing the IK of humanoid robots whose construction shows a small but non negligible offset at the hip (compared to its actual height) which prevents any purely analytical solution to be developed.

As a case of study, a robot with a non-typical kinematic structure, as it is the SILO2 prototype, is analyzed. However, the algorithm can be applied to any structure that possesses negligible offsets, so that an approximate analytical “solution” can be found after neglecting them, and then be used as the initial condition of a stable numerical method, ensuring in this way that the number of iterations needed will be drastically reduced (compared with the required ones if an arbitrary initial condition is proposed).

Besides that, as the approximate analytical “solution” is in the neighborhood of the desired target in the joint configuration space, it will be more likely that the numerical method reaches the desired solution, as the right minima is very near to the initial condition. So, this method will be as efficient as desired so long as the concerned offsets are small enough. This fact was also evaluated, as this algorithm needed only very few iterations:

Less than 10 in most of the cases, or a maximum of 2 if the desired attitudes belonged to a typical gait; that is, attitudes that do not require big joint ranges to be performed. However, even non-typical motions can be achieved in an affordable computation time.

Concerning to the SILO2 robot, this method effectively solved its IK problem, so that it was possible to calculate precise gaits in real-time, as they were required by the control laws that had been previously developed by the team work at the Institute of Industrial Automation. Furthermore, the kinematic simulator developed during this work represented a useful tool for testing desired attitudes on a virtual model before doing it on the real prototype, avoiding in this way failures that may cause severe damages to the robot if it falls down. The simulator was very useful to test attitudes in this way, as many of them cannot be realized due to mechanical constraints, even when they were feasible during a certain gait. By recognizing these attitudes, the gait could be modified until getting a realizable and useful one, or they may also be used to provide guidelines for modifying the SMART actuators, by redesigning the size of the crank and the connecting road in every four-link mechanism, in order to improve the design of the humanoid.

References

Arbulú

Mario

Pardos

José M.

Cabas

Luis

Staroverov

Pavel

Kaynov

Dmitry

Pérez

Carlos

Rodríguez

Miguel

, and Balaguer

Carlos

. Rh-0 humanoid full size robot's control strategy based on the Lie logic technique. In Proceedings of the 5th. IEEE-RAS International Conference on Humanoid Robots, pages 271–276, Tsukuba, Japan, December 2005.

Tevatia

Gaurav

and Schaal

Stefan

. Inverse kinematics for humanoid robots. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pages 294–299, California, USA, April 2000.

Pieper

D.L.

The Kinematics of Manipulators under Computer Control. PhD thesis, Stanford University, California, USA, 1968.

Spong

Mark W.

and Vidyasagar

Robot dynamics and control, chapter 3. John Wiley & Sons, USA, 1989.

Ali

Muhammad A.

Andy

Park H.

, and George

Lee C.S.

Closed-form inverse kinematic joint solution for humanoid robots. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 704–709, Taipei, Taiwan, October 2010.

Williams

Robert L.

II . Inverse kinematics and singularities of manipulators with offset wrist. IASTED International Journal of Robotics and Automation, 14(1):1–8, 1999.

Ibarra

Juan Manuel

and Cisneros

Rafael

. Forward and inverse kinematics for a small-sized humanoid robot. In Proceedings of the 19th. IEEE International Conference on Electronics, Communications and Computers (CONIELECOMP), pages 111–118, Cholula, Puebla, Mexico, February 2009.

Zeki

Mohammed

Al-Faiz

Hussain

and Saleh

Mohammed S.

Inverse kinematics analysis for manipulator robot with wrist offset based on the closed-form algorithm. International Journal of Robotics and Automation (IJRA), 2(4):256–264, September / October 2011.

Singh

Giresh K.

and Claassens

Jonathan

. An analytical solution for the inverse kinematics of a redundant 7dof manipulator with link offsets. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 2976–2982, Taipei, Taiwan, December 2010.

10.

Mistryl

Michael

Nakanishi

Iun

Cheng

Gordon

, and Schaal

Stefan

. Inverse kinematics with floating base and constraints for full body humanoid robot control. In Proceedings of the 8th. IEEE-RAS International Conference on Humanoid Robots, pages 22–27, Daejeon, South Korea, February 2009.

11.

Wang

Li-Chun Tommy

and Chen

Chih Cheng

. A combined optimization method for solving the inverse kinematics problem of mechanical manipulators. IEEE Transactions on Robotics and Automation, 7(4):489–499, August 1991.

12.

Canutescu

Adrian A.

and Dunbrack

Roland L.

Jr . Cyclic coordinate descent: A robotics algorithm for protein loop closure. Protein Science, 12(5):963–972, May 2003.

13.

Müller

Cajar R.

and Mukundan

Triangulation: A new algorithm for inverse kinematics. In Proceedings of the 22th. International Image and Vision Computing New Zealand Conference (IVCNZ), pages 181–186, Hamilton, New Zealand, December 2007.

14.

Pashkevich

Real-time inverse kinematics for robots with offset and reduced wrist. Control Engineering Practice, 5(10):1443–1450, October 1997.

15.

de Lope

Javier

Zarraonandia

Telmo

Careaga

Rafaela González

, and Maravall

Darío

. Solving the inverse kinematics in humanoid robots: A neural approach. In Proceedings of the 7th. International Work-Conference on Artificial and Natural Neural Networks (IWANN), LNCS 2687, pages 177–184, Menorca, Spain, June 2003. Springer-Verlag, Berlin Heidelberg.

16.

Bingul

Ertunc

H.M.

, and Oysu

Comparison of inverse kinematics solutions using neural network for 6r robot manipulator with offset. In Proceedings of the ICSC Congress on Computational Intelligence Methods and Applications, Istanbul, Turkey, December 2005.

17.

Pitt

Joachim

Hildebrand

Dietmar

Stelzer

Maximilian

, and Koch

Andreas

. Inverse kinematics of a humanoid robot based on conformal geometric algebra using optimized code generation. In Proceedings of the 8th. IEEE-RAS International Conference of Humanoid Robots, pages 681–686, Daejeon, South Korea, December 2008.

18.

Manocha

Dinesh

and Canny

John F.

Efficient inverse kinematics for general 6r manipulators. IEEE Transactions on Robotics and Automation, 10(5):648–657, October 1994.

19.

Samuel

Buss. Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods. Technical report, University of California, California, USA, April 2004. Unpublished (available via the Internet).

20.

Parikh

Patrick J.

and Lam

Sarah S. Y.

A hybrid strategy to solve the forward kinematics problem in parallel manipulators. IEEE Transactions on Robotics, 21(1):18–25, February 2005.

21.

Tomomichi Sugihara . Solvability-unconcerned inverse kinematics by the levenberg-marquardt method. IEEE Transactions on Robotics, 27(5):984–991, October 2011.

22.

Armada

Caballero

Akinfiev

Montes

Manzano

Pedraza

Ríos

, and De Santos

González P.

Design of SILO2 humanoid robot. In Proceedings of the 3rd. IARP International Workshop on Humanoid and Human Friendly Robotics, pages 37–42, Tsukuba, Japan, December 2002.

23.

Franceschi

Héctor Montes

. Análisis, Diseño y Evaluación de Estrategias de Control de Fuerza en Robots Caminantes. PhD thesis, Universidad Complutense de Madrid, Madrid, Spain, 2006.

24.

Murray

Richard M.

Zexiang

, and Shankar

Sastry S.

A Mathematical Introduction to Robotic Manipulation, chapter 2–3. CRC Press LLC, USA, 1994.

25.

Tolani

Deepak

Goswami

Ambarish

, and Badler

Norman I.

Real-time inverse kinematics techniques for anthropomorphic limbs. Graphical Models, 62(5):353–388, September 2000.