8 Inverse position kinematics

The inverse position kinematics (“IPK”) solves the following problem:

Given the actual end-effector pose $eebsT$ , what are the corresponding joint positions $q = (q1 ... qn)T$ ?

In contrast to the forward problem, the solution of the inverse problem is not always unique: the same end-effector pose can be reached in several conﬁgurations, corresponding to distinct joint position vectors. It can be proven, [35, 56], that a 6R manipulator (a serial chain with six revolute joints, as in Figs 1, 2, and 4), with a completely general geometric structure, has sixteen different inverse kinematics solutions, found as the solutions of a sixteenth order polynomial.

As for the forward position and velocity kinematics, this Section presents both a numerical procedure for general serial structures, and the dedicated closed-form solution for robots of the 321 type, as described in [21]. Some older references describe similar solution approaches but in less detail, [27, 55, 60].

The IK of a serial arm is more complex than its FK. However, many industrial applications don’t need IK algorithms, since the desired positions and orientations of their end-effectors are manually taught: a human operator steers the robot to its desired pose, by means of control signals to each individual actuator; the operator stores the sequence of corresponding joint positions into the robot’s memory; during subsequent task execution, the robot controller moves the robot to this set of taught joint coordinates. However, the current trends towards off-line programming does require IK algorithms, and hence calibrated robots. Recall that such calibrated robots have a general kinematic structure.

8.1 General IPK: Newton-Raphson iteration

Inverse position kinematics for serial robot arms with a completely general kinematic structure (but with six joints) are solved by iterative procedures, based on the Newton-Raphson approach, [55, 66]:

Numerical IPK

Step 1

Start with an estimate $ˆq = (ˆq1...ˆq6)T$ of the vector of six joint positions. This estimate is, for example, the solution corresponding to a previous nearby pose, or, for calibrated robots, the solution calculated by the nominal model (using the procedure of the next Section if this nominal model has a 321 structure). As with all iterative algorithms, the better the initial guess, the faster the convergence.

Step 2

Denote the end-effector pose that corresponds to this estimated vector of joint positions by $T (qˆ)$ . The difference between the desired end-effector pose $T (q)$ (with $q$ the real joint positions which have to be found) and the estimated pose is “inﬁnitesimal,” as assumed in any iterative procedure:

T (q) = T (ˆq) TΔ(Δq).

(22)

$Δq = q - ˆq$ is the joint position increment to be solved by the iteration. Solving for $TΔ( Δq)$ yields

TΔ(Δq) = T -1(ˆq)T(q).

(23)

Step 3

The coordinate expression of the inﬁnitesimal pose $TΔ(Δq)$ is:

( ) 1 - δz δy dx TΔ = || δz 1 - δx dy|| . ( - δy δx 1 dz) 0 0 0 1

(24)

The inﬁnitesimal displacement twist $tΔ(ˆq) = (δx δyδzdxdydz)T$ corresponding to $TΔ(Δq)$ is easily identiﬁed from Eq. (24). On the other hand, it depends linearly on the joint increment $Δq$ through the Jacobian matrix $J(ˆq)$ , Eq. (11):

2 tΔ( ˆq) = J(ˆq)Δq + 𝒪(Δq ).

(25)

$J(ˆq)$ is calculated by the numerical FVK algorithm in Sect. 7.2.

Step 4

Hence, the joint increment $Δq$ is approximated by

|-----------------| |Δq = J -1(qˆ) tΔ(ˆq).| -------------------

(26)

The inverse of the Jacobian matrix exists only when the robot arm has six independent joints. Section 16 explains how to cope with the case of more or less than six joints.

Step 5

If $Δq$ is “small enough,” the iteration stops, otherwise Steps 2–4 are repeated with the new estimate $ˆqi+1 = ˆqi + Δq$ .

This procedure gives an idea of the approach, but real implementations must take care of several numerical details, such as, for example:

Inverting a $6× 6$ Jacobian matrix (which is required in motion control) is not an insurmountable task for modern microprocessors (even if the motion controller runs at a frequency of 1000Hz or more), but nevertheless the implementation should be done very carefully, in order not to loose numerical accuracy.
In order to solve a set of linear equations $Ax = b$ , it is, from a numerical point of view, not a good idea to ﬁrst calculate the inverse $A -1$ of the matrix $A$ explicitly, and then to solve the equation by multiplying the vector $b$ by this inverse, as might be suggested by Eq. (26). Numerically more efficient and stable algorithms exist, [22, 66], the simplest being the Gaussian elimination technique and its extensions.
The numerical procedure ﬁnds only one solution, i.e., the one to which the iteration converges. Some more elaborate numerical techniques exist to ﬁnd all solutions, such as for example the continuation, dialytic elimination and homotopy methods, [57, 68, 72].

8.2 Closed-form IPK for 321 structure

The efficient closed-form IPK solution for the 321 structure relies again on the decoupling at the wrist centre point, [21]:

Closed-form IPK

Step 1

The position of the wrist centre point is simply given by the inverse of Eq. (8):

bspwr = bspee - eebsR (00 l6)T.

(27)

Step 2

Hence, the ﬁrst joint angle is

wr wr q1 = atan2(bspx ,±bspy ).

(28)

The robot conﬁguration corresponding to a positive $bspwr y$ is called the “forward” solution, since the wrist centre point is then in front of the “body” of the robot; if $bspwr y$ is negative, the conﬁguration is called “backward.”

Step 3

The horizontal and vertical distances $dh$ and $dv$ of the wrist centre point with respect to the shoulder frame ${1}$ are found by inspection of Fig. 9:

h ∘ ---wr-2-----wr-2- v wr d = (bspx ) + (bspy ) , d = bspz - l1.

(29)

Step 4

Now, look at the planar triangles formed by the second and third links (Fig. 9).

Step 4.1

The cosine rule gives

( ) (dh)2 +-(dv)2---(l2)2---(l3)2 q3 = ±arccos 2l2l3 .

(30)

A positive $q3$ gives the “elbow up” conﬁguration (Fig. 10); the conﬁguration with negative $q3$ is called “elbow down.”

Figure 10:

Four of the eight conﬁgurations corresponding to the same end effector pose, for a 321 type of manipulator. The four other conﬁgurations are similar to these four, except for a change in the wrist conﬁguration from “ﬂip” to “no ﬂip.”

Step 4.2

The tangent rules yield

q₂	= atan2- α,	(31)
with

α	= atan2.	(32)

Step 5

The inverse position for the ZXZ wrist uses $64R$ as input, which is straightforwardly derived from $b7sR$ (a known input parameter) and $b4sR$ (which can be calculated as soon as the ﬁrst three joint angles are known):

6 4R	= 7 4R = bs 4 R 7 bs R,	(33)
with

4 bsR	= R(Z,q₁)R(X,-q₂ - q₃).	(34)

Two solutions exist: one with $q5 > 0$ (called the “no-ﬂip” conﬁguration) and one with $q5 < 0$ (called the “ﬂip” conﬁguration).

In the algorithm above, a “conﬁguration” (Sect. 3) corresponds to a particular choice of IPK solution. In total, the 321 manipulator has eight different conﬁgurations, by combining the binary decisions “forward/backward,” “elbow up/elbow down,” and “ﬂip/no ﬂip.” Note that these names are not standardised: the robotics literature contains many alternatives.

[next] [prev] [prev-tail] [front] [up]