程序代写代做代考 arm chain Differential Kinematics

Differential Kinematics

Differential Kinematics

Elizabeth Sklar

Department of Informatics
King’s College London

12 November 2018
(version 1.0)

1 / 35

Outline

Differential Kinematics

2 / 35

Differential Kinematics

We have talked about:
I Forward Kinematics—figuring out the end point of a robot

arm given its starting point and angle for each link
I Inverse Kinematics—figuring out the angles necessary for each

link, given a starting and ending point for a robot arm
I Mostly we have talked about this in the context of figuring out

what positions to end up in, but not about how to get to these
positions.

I So now we’ll talk about how to get there.

3 / 35

Differential Kinematics, p2

I One way to program robots to move is through trial and error.
I We have talked about this kind of motion for mobile robots

with path planning.
I With robot arms, it is more common to solve the problem

mathematically, using Kinematics.
I So you can think of:
I Forward Kinematics as figuring out how a robot will move if its

motors work in a given way.
I Inverse Kinematics as figuring out how to move the motors to

get the robot to do what we want.

4 / 35

Differential Kinematics, p3

Here is a mathematical way to represent the problem.
Given:

I n joints:
θ1 . . . θn joint angles

I k end effectors:
−→s = (s1 . . . sk)T end effector starting positions
−→
t = (t1 . . . tk)

T end effector target positions
I Each position is a coordinate in space, e.g., (x , y) or (x , y , z).
I ei = ti − si desired change in position of end effector i
I The Inverse Kinematics problem:

Find values for all θj ’s such that ti = si (θ) ∀ i
I There may be multiple solutions (if there is a solution at all).

5 / 35

Differential Kinematics, p4

I There are many ways to solve this problem.
I We will look at one incremental way.
I The advantage of an incremental approach is that it can also

be used to choreograph the movement of the arm.

I There are multiple incremental approaches, such as:
I Gradient Descent — start with an initial guess, and

incrementally change values to reduce error
I Jacobian — we’ll look at this one here

6 / 35

Jacobian

I The Jacobian approach is based on a matrix of partial
derivatives, where each partial indicates the influence on the
end effector position (pi ) by the joint angle (θj).

I The Jacobian matrix J is defined as:

J(θ) =

[
∂pi
∂θj

]
i ,j

where ∂pi
∂θj

is the partial derivative of pi with respect to θj
I We assume that J is an m × n matrix of partial derivatives:

J(θ) =




∂px
∂θ0

∂px
∂θ1

. . .
∂px
∂θn

∂py
∂θ0

∂py
∂θ1

. . .
∂py
∂θn

∂ω
∂θ0

∂ω
∂θ1

. . . ∂ω
∂θn




(m = 3 in the case of a 2-DOF arm)
7 / 35

Maths Review

I Before we go further, let’s have a quick review of Derivatives…

8 / 35

Derivatives

I A derivative is a “measure of the rate of change of one
quantity with respect to another.” [MW, p47]

I The slope of a line is a derivative: change in x with respect to
change in y .

I A velocity is another derivative: change in distance with
respect to time.

I Definition:
The derivative y = f (x) at x0 is written f ′(x0):

f ′(x0) =
∆y

∆x
=

f (x0 + ∆x)− f (x0)
∆x

9 / 35

Partial Derivatives

I The partial derivatives “of a function of several variables are its
ordinary derivatives with respect to each variable separately.”
[MW, p686]

I When we take the partial derivative of a function with two
variables, we imagine one of the variables is a constant and
compute the partial with respect to the other variable.

I For example, given:

f (x) = x2 + xy + y2

the partial derivative of f (x) with respect to x is:

∂f (x)

∂x
= 2x + y

I We pretend that y is a constant and compute the derivative of
the function f (x) with respect to x .

10 / 35

Partial Derivatives, p2

I Below (a) is an illustration of the function
z = f (x , y) = x2 + xy + y2.

I The red line highlighted in (a), shows the value of the function
at the point y = 1.

I The red line is shown within the xz plane in (b), illustrating
the partial derivative of z with respect to x : ∂z

∂x
= 2x + y .

(a) (b)
11 / 35

Differential Kinematics and the Jacobian Method for
computing Inverse Kinematics

I Differential Kinematics gives the relationship between the joint
velocities (how the joints change) and the corresponding end
effector and its angular velocity

νe =

[
ṗe
ωe

]
= Jq̇

where:
I νe is the angular velocity vector of the end effector
I ṗe is the position velocity (change in position) of the end

effector
I ωe is the angular velocity (change in rotation) of the end

effector
I J is a square matrix representing the Jacobian—the partial

derivatives of joint positions with respect to the angular
velocity of the joints

I q̇ is a vector of joint velocities
12 / 35

Jacobian, p2

I For the contribution to the linear velocity (translation), we
have:

ṗe =
n∑

i=1

JPi q̇i

I We compute ṗe as the sum of the terms JPi ∗ q̇i , where JP is
the Jacobian matrix containing the partial derivatives of the
position components with respect to q̇ (joint velocities).

I Each term represents the contribution of the velocity of a
single joint (i) to the end effector, as if all the other joints
were stationary.

ṗe = [JP1JP2 . . . JPn]




q̇1

q̇2
q̇n




13 / 35

Jacobian, p3

I For the contribution to the angular velocity (rotation), we
have:

ω̇e =
n∑

i=1

Jθi q̇i

where ωi is the rotation (orientation) of the i-th joint.
I We compute ṗe as the sum of the terms Jθi ∗ q̇i , where Jθ is

the Jacobian matrix containing the partial derivatives of the
orientation components (joint angles) with respect to q̇.

I Each term represents the contribution of the velocity of a
single joint angle (i) to the end effector, as if all the other
joints were stationary.

I Thus:

ω̇e = [Jθ1Jθ2 . . . Jθn]




q̇1

q̇2
q̇n




14 / 35

Jacobian, p4

I We put these together in the Jacobian matrix:

J =


 position components

orientation components


 = [ JP

]

I and use it to compute the change in end effector position:

νe =

[
ṗe
ωe

]
=

[
JP1 JP2 . . . JPn
Jθ1 Jθ2 . . . Jθn

]
q̇1
q̇2

q̇n




15 / 35

Quick review: Cross Product

(Imagine A and B lie in a plane that is flat on a table; then n is sticking
straight up in the air, like a pencil perpendicular to the table top.)

I The cross product between two vectors, A and B , gives you a
vector that is perpendicular to both of them, or the normal, n.

I The cross product can be calculated as:

A× B = |A| |B| sin(θ) n
where: A and B are our vectors,
θ is the angle between them,
and n is the unit vector perpendicular to both A and B

16 / 35

Quick review: Cross Product, p2

I The cross product, when A and B intersect at the origin (0, 0)
is computed as follows:

C = A× B =


 AxAy

Az


×


 BxBy

Bz


=
[

0 −Az Ay
Az 0 −Ax

−Ay Ax 0

] [
Bx
By
Bz

]
=
[

(0 ∗ Bx ) + (−Az ∗ By ) + (Ay ∗ Bz )
(Az ∗ Bx ) + (0 ∗ By ) + (−Ax ∗ Bz )
(−Ay ∗ Bx ) + (Ax ∗ By ) + (0 ∗ Bz )

]

=


 −Az ∗ By + Ay ∗ BzAz ∗ Bx − Ax ∗ Bz
−Ay ∗ Bx + Ax ∗ By


17 / 35

Jacobian: Example 3-Link Arm

I For example, for a 3-link arm, J can be written as:

J =

[
JP
JR

]
=




∂px
∂θ1

∂px
∂θ2

∂px
∂θ3

∂py
∂θ1

∂py
∂θ2

∂py
∂θ3

∂ω
∂θ1

∂ω
∂θ2

∂ω
∂θ3




where:
I (px , py ) represent the position of the end effector
I ω represents the rotation of the end effector
I θi represents the angle at each of the 3 joints
I and

JP =




∂px
∂θ1

∂px
∂θ2

∂px
∂θ3

∂py
∂θ1

∂py
∂θ2

∂py
∂θ3


 JR = [ ∂ω∂θ1 ∂ω∂θ2 ∂ω∂θ3 ]

18 / 35

Jacobian: Example 3-Link Arm, p2

P0 =


 00

0


 P1 =


 L1 cos(ω1)L1 sin(ω1)

0


19 / 35

Jacobian: Example 3-Link Arm, p3

P2 =


 L1 cos(ω1) + L2 cos(ω1 + ω2)L1 sin(ω1) + L2 sin(ω1 + ω2)

0


20 / 35

Jacobian: Example 3-Link Arm, p4

P3 =


 L1 cos(ω1) + L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)L1 sin(ω1) + L2 sin(ω1 + ω2) + L3 sin(ω1 + ω2 + ω3)

0


21 / 35

Jacobian: Example 3-Link Arm, p5

I if we want to compute the position of the end effector, P3, we
can use the Jacobian matrix to do that.

I Given:

P3x = L1 cos(ω1) + L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)

the partial derivatives of P3x with respect to each ωi are:

∂P3x
∂ω1

= −L1 sin(ω1)− L2 sin(ω1 + ω2)− L3 sin(ω1 + ω2 + ω3)

∂P3x
∂ω2

= −L2 sin(ω1 + ω2)− L3 sin(ω1 + ω2 + ω3)

∂P3x
∂ω3

= −L3 sin(ω1 + ω2 + ω3)

22 / 35

Jacobian: Example 3-Link Arm, p6

I and for P3y :

P3y = L1 sin(ω1) + L2 sin(ω1 + ω2) + L3 sin(ω1 + ω2 + ω3)

the partial derivatives of P3y with respect to each ωi are:

∂P3y
∂ω1

= L1 cos(ω1) + L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)

∂P3y
∂ω2

= L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)

∂P3y
∂ω3

= L3 cos(ω1 + ω2 + ω3)

23 / 35

Jacobian: Example 3-Link Arm, p7

I and for ωtotal = ω1 + ω2 + ω3:
∂ωtotal
∂ω1

= 1 ∂ωtotal
∂ω2

= 1 ∂ωtotal
∂ω3

= 1

24 / 35

Jacobian: Example 3-Link Arm, p8

I We can put these all together into the Jacobian matrix as
follows:

J =




∂P3x
∂ω1

∂P3x
∂ω2

∂P3x
∂ω3

∂P3y
∂ω1

∂P3y
∂ω2

∂P3y
∂ω3

∂ωtotal
∂ω1

∂ωtotal
∂ω2

∂ωtotal
∂ω3




I Substitute for each of the partial derivatives the appropriate
values from the previous slides

I Then we can use this matrix to determine how the end effector
moves when each of the joint angles change—Differential
Kinematics !!

25 / 35

Jacobian: Example 3-Link Arm, p9

I Remember where we started today:
Differential Kinematics gives the relationship between the joint
velocities (how the joints change) and the corresponding end
effector and its angular velocity

νe =

[
ṗe
ωe

]
= Jq̇ =

[
JP1 JP2 . . . JPn
Jθ1 Jθ2 . . . Jθn

]
q̇1
q̇2

q̇n




where:
I νe is the angular velocity vector of the end effector
I ṗe is the position velocity (change in position) of the end

effector
I ωe is the angular velocity (change in rotation) of the end

effector
I q̇ is a vector of joint velocities

26 / 35

Jacobian: Example 3-Link Arm, p10

I What if we know νe and we want to know q̇?
I i.e., we want to determine what joint velocities are required to

obtain a desired position and orientation of the end effector?

I This can be obtained by inverting the Jacobian matrix like this:

q̇ = J−1νe

I So if we know J:
then we first compute its inverse: J−1

and then we multiply the inverse by the angular velocity vector
of the end effector (νe)

→ and that will tell us how much each joint angle changes

27 / 35

Jacobian: Example Code

float [] solveJacobian( px , py , pw ,w1,w2,w3, ṗx , ṗy , ˙pw ) {
float new_angles [] = { 0.0, 0.0, 0.0 };
float νe [] = { px − ṗx , py − ṗy , pw − ˙pw }; // velocity of pe
float J[][] = new float [3][3]; // Jacobian
J[0][0] = −L1 ∗ sin(w1)− L2 ∗ sin(w1 + w2)− L3 ∗ sin(w1 + w2 + w3);
J[0][1] = −L2 ∗ sin(w1 + w2)− L3 ∗ sin(w1 + w2 + w3);
J[0][2] = −L3 ∗ sin(w1 + w2 + w3);
J[1][0] = L1 ∗ cos(w1) + L2 ∗ cos(w1 + w2) + L3 ∗ cos(w1 + w2 + w3);
J[1][1] = L2 ∗ cos(w1 + w2) + L3 ∗ cos(w1 + w2 + w3);
J[1][2] = L3 ∗ cos(w1 + w2 + w3);
J[2][0] = 1;
J[2][1] = 1;
J[2][2] = 1;
float Jinv [][] = inv3( J ); // compute J−1

float q̇[] = multiply3x1( Jinv , νe ); // angle changes
new_angles [0] = w1 + q̇[0];
new_angles [1] = w2 + q̇[1];
new_angles [2] = w3 + q̇[2];
return( new_angles );

} // end of solveJacobian ()

28 / 35

Summary

I That’s all!
I Okay it’s a bit complicated
I But you should understand it better after completing

Coursework 2 part 2

I Coursework 2 is due in 2 weeks (27th November)
I Look at starter and helper code uploaded on KEATS

29 / 35

Derivatives: Some useful rules

I sum rule:
d

dx
(u + v) =

du

dx
+

dv

dx

I product rule:
d

dx
(u ∗ v) =

du

dx
v + u

dv

dx

I constant multiple rule:

d

dx
A ∗ u = A ∗

du

dx

30 / 35

Derivatives: Some useful rules, p2

I power rule:
d

dx
(xn) = n ∗ x (x−1)

I power of a function rule:

d

dx
un = n ∗ un−1

du

dx

I quotient rule:
d

dx

(u
v

)
=

du
dx
v − u dv

dx

v2

I reciprocal rule:
d

dx

(
1
u

)
= −

1
u2

du

dx

31 / 35

Derivatives: Some useful rules, p3

I chain rule:
dy

dx
=

dy

du
·
du

dx

if y is a function of u and u is a function of x

32 / 35

Derivatives: Some useful rules, p4

I quadratic function rule:

d(ax2 + bx + c)

dx
= 2ax + b

I linear function rule:

d(bx + c)

dx
= b

d(c)

dx
= 0

33 / 35

Derivatives: Some useful rules, p5

I The change in value of sin(x) with respect to x is cos(x):

d sin(x)

dx
= cos(x)

I The change in value of cos(x) with respect to x is −sin(x):

d cos(x)

dx
= −sin(x)

34 / 35

References

SNS Introduction to Autonomous Mobile Robots, by Roland
Siegwart, Illah R. Nourbakhsh and Davide Scaramuzza (2011),
MIT Press, chapter 3.

MW Calculus, by Jerrold Marsden and Alan Weinstein (1980), The
Benjamin/Cummings Publishing Company, Inc.

35 / 35

Differential Kinematics