# CS计算机代考程序代写 arm chain CS 4610/5335

CS 4610/5335

Differential and Inverse Kinematics

Robert Platt Northeastern University

Material adapted from:

1. Lawson Wong, CS 4610/5335

2. Peter Corke, Robotics, Vision and Control

3. Marc Toussaint, U. Stuttgart Robotics Course 4. Oussama Khatib, Stanford CS 223A

Inverse Kinematics (IK) Problem

Given a desired position and orientation (pose) of the end effector, what arm joint angles achieve that pose?

IK Problem:

Given a desired position and orientation of the end effector …

Calculate the corresponding joint angles

q3

Two approaches to IK

Suppose we want to move the end effector here…

q2

q1

Approach #1: Analytic IK – solve for joint angles directly using algebra

Approach #2: Numerical IK – solve for joint angles using an iterative numerical technique like gradient descent

– this approach can also be used as an online controller

Analytic Inverse Kinematics

Motivating Example Suppose you want to move the end

Goal: move the end effector onto this line

effector above a specified point, Analytic IK:

xg

l

q

x xg

Inverse kinematics

There is no general analytical inverse kinematics solution

All analytical inverse kinematics solutions are specific to a robot or class of robots.

based on geometric intuition about the robot I’ll give one example – there are many variations.

Inverse kinematics

q3

q5

Inverse kinematics

q2

q4

q6

Spherical wrist: the axes of the last three joints intersect in a point.

q1

Consider this 6-joint robot

Puma 560 Robot

Inverse kinematics

q3

q5

Problem:

Given: desired transform,

q2 q1

q4

q6

Find: Note:

qq q q qq 1234 n

The desired transform (pose) encodes six degrees of freedom (this info can be represented by six numbers)

Since we only have six joints at our disposal, there is no manifold of redundant solutions.

For this manipulator, the problem can be decomposed into a position component (the first three joints) and an orientation component (the last three joints)

The first three joints tell you what the position of the spherical wrist

Example: Inverse kinematics q3 q2

q4

q6

Solution:

First, back out the position of the spherical wrist:

q1

Since it’s a spherical wrist, the last three joints can be thought of as rotating

about a point.

A constant transform exists that goes from the last wrist joint out to the end

swT eff

effector (sometimes this is called the “tool” transform):

Back out the position of the wrist:

1 sw eff eff

b b sw TT T

q5

Example: Inverse kinematics q3

Next, solve for the first three joints

q4

q6

•

q5 q2

First, solve for q1. (look down from above)

q atan2x ,y 1gg

or

q atan2x ,y 1gg

q1

Goal position in horizontal plane

q1

Example: Inverse kinematics

Finally, the last three joints completely specify the orientation of the end effector.

Note that the last three joints look just like ZYZ Euler angles

Determination of the joint angles is easy – just calculate the ZYZ Euler angles corresponding to the desired orientation.

q3

q5

q2 q1

q4

q6

Example: Inverse kinematics

Next, solve for q2 and q3 .

(look at the manipulator orthogonal to the plane of the

q3

q5

q2 first two links) q1

q4

q6

Given and , calculate joint angles that cause eff to reach

zyz

( )( )( ) 0 01−sinθ0cosθ0 01

ZYZ Euler Angles

cosφ −sinφ 0 cosθ 0 sinθ cosψ −sinψ 0 R (φ,θ,ψ)=sinφ cosφ 0 0 1 0 sinψ cosψ 0

cφcθcψ−sφsψ −cφcθsψ−sφcψ cφsθ Rzyz(φ,θ,ψ)= s c c +c s −s c s +c c s s

(φθψφψ φθψφψφθ) −sθ cψ sθ sψ cθ

θ=±a tan2 1−r ,r (√ 332 33)

atan2r ,r k 23 13

atan2r ,r 32 31

Inverse kinematics for a humanoid arm

You can do similar types of things for a humanoid (7-DOF) arm.

Since this is a redundant arm, there are a manifold of solutions…

Spherical wrist

Spherical shoulder

General strategy:

Solve for elbow angle

elbow

Solve for a set of shoulder angles that places the wrist in the right position (note that you have to choose an elbow orbit angle)

Solve for the wrist angles

Differential Kinematics

Up to this point, we have only considered the relationship of the joint angles to the Cartesian location of the end effector:

f (q) x

But what about the first derivative?

This would tell us the velocity of the end effector as a function of joint angle velocities.

f (q) q

Consider a one-link arm

As the arm rotates, the end effector sweeps out an arc

Let’s assume that we are only interested in the x coordinate…

Forward kinematics: Differentialkinematics:

l

q

Motivating Example

x l cos(q)

dx lsin(q)

x

dq

x l sin(q)q

q 1 x l sin(q)

Suppose you want to move the end effector

Goal: move the end effector onto this line

above a specified point, Analytic IK:

xg

Motivating Example

i0,q0 arbitrary 2. xi l cos(qi )

3. x xg xi

4.q 1 x l sin(qi )

Numerical IK: 1.

l

xg

q

x

5.

6. igoto2.

qi1 qi q

Motivating Example

l

xg

q

q qd

q

q

x

x

x

g

x

1

l sin(q)

joint ctlr

l cos()

joint position sensor

This controller moves the link asymptotically toward the goal position.

Jacobian

Jacobian

⃗x=[l1cos(q1)+l2 cos(q1+q2)] l1sin(q1)+l2sin(q1+q2)

Forward kinematics of the two- link manipulator

Velocity Jacobian

l2 q2

l1

y

q1

x

Jacobian

Chain rule: x Jq

If the Jacobian is square and full rank, then we can

l2 q2

l1

invert it:

1 qJ x

y q1

x

J1

joint ctlr

joint position sensor

Numerical IK Solution

Input: x*

Output: q*

1. repeat until dx is small:

2. 3. 4. 5. 6. 7. 8.

init q to random joint configuration repeat K times:

x = FK(q)

dx = x*-x

dq = stepsize * J-1 dx q = q + dq

return q* = q

Jacobian

The Jacobian relates joint velocities with end effector twist:

Jq

End effector twist Jacobian

Joint angle velocities

First derivative of joint angles:

It turns out that you can “easily” compute the Jacobian for arbitrary manipulator structures

This makes differential kinematics a much easier sub-problem than kinematics in general.

Jacobian

The Jacobian relates joint velocities with end effector twist: What is twist?

Jq

End effector twist Jacobian

Joint angle velocities

First derivative of joint angles:

It turns out that you can “easily” compute the Jacobian for arbitrary manipulator structures

This makes differential kinematics a much easier sub-problem than kinematics in general.

•

End effector twist:

Twist is a concatenation of linear velocity and angular velocity:

Linear velocity Angular velocity

What is Twist?

•

End effector twist:

Twist is a concatenation of linear velocity and angular velocity:

Linear velocity Angular velocity

What is Twist?

What is angular velocity?

Angular velocity is a vector that:

– points in the direction of the axis of rotation

– has magnitude equal to the velocity of rotation

What is Angular Velocity?

Angular velocity is a vector that:

– points in the direction of the axis of rotation

– has magnitude equal to the velocity of rotation

Symbol for angular velocity:

Relation between angular velocity and linear velocity:

Breakdown of the Jacobian:

v J q v

J q

ξ=Jv q ̇ [Jω]

Jv x but q

Jacobian

Relation to the derivative:

r J

q

That’s not an angular velocity

Jacobian in different reference frames

In the preceeding, the Jacobian has been expressed in the base frame

It can be expressed in other reference frames using rotation matrices

Velocity is transformed from one reference frame to another using:

3x 3y

q3

l3

l2

2x

1y

1x q2

2y

q1

0y

kkb 0 pR p

0x z

l1

b kkb

b pR p

Therefore, the velocity Jacobian can be

transformed using:

kkb

JR J vbv

Inverse Kinematics: An iterative approach

Jacobian

x Jq

If the Jacobian is square and full rank, then we

l2 q2

l1

can invert it:

1 qJ x

y q1

x

J1

joint ctlr

joint position sensor

Numerical IK Solution (method 1)

Input: x*

Output: q*

1. repeat until dx is small:

2. 3. 4. 5. 6. 7. 8.

init q to random joint configuration repeat K times:

x = FK(q)

dx = x*-x

dq = stepsize * J-1 dx q = q + dq

return q* = q

Numerical IK Solution (method 1)

Input: x*

Output: q*

1. repeat until dx is small:

Idea:

2. 3. 4. 5. 6. 7. 8.

init q to random joint configuration repeat K times:

x = FK(q)

dx = x*-x

dq = stepsize * J-1 dx q = q + dq

return q* = q

Numerical IK Solution (method 2)

Input: x*

Output: q*

1. repeat until dx is small:

This also works

2. 3. 4. 5. 6. 7. 8.

init q to random joint configuration repeat K times:

x = FK(q)

dx = x*-x

dq = stepsize * JT dx q = q + dq

return q* = q

This results in different behavior (see homework)

Non-square Jacobian matrix

Input: x*

Output: q*

1. repeat until dx is small:

2. 3. 4. 5. 6. 7. 8.

init q to random joint configuration repeat K times:

x = FK(q)

dx = x*-x

dq = stepsize * J-1 dx q = q + dq

return q* = q

Can only take inverse of Jacobian if it is square

Non-square Jacobian matrix

Example of a non-square Jacobian matrix:

3x 3y

l3 2x

l2

J (q)=[−l1 s1−l2 s12−l3 s123 l1 c1+l2 c12+l3 c123

−l1 s1−l2 s12 l1 c1+l2 c12

−l1 s1 ] l1 c1

1y 2y

q3

1x q2

q ̇ 1 q

0y

q1

x ̇ =J q []() ̇

0 x z

l

Two equations of three y ̇ [2] variableseach…

1

0

q ̇ 3

This is an under-constrained system of equations.

multiple solutions

there are multiple joint angle velocities that realize the same EFF velocity.

Non-square Jacobian matrix

If the Jacobian is not a square matrix, then you can’t invert it.

3x 3y

what next?

l3 2x

l2

q3

1x q2

1y 2y

1

0x

0y

We have:

We are looking for a matrix

qJ#x

x Jq

q

l

1

such that:

xJq

0z

Two cases:

Underconstrained manipulator (redundant) Overconstrained

Generalized inverse:

for the underconstrained manipulator: given that minimizes s.t.

, find a vector

Generalized inverse

for the overconstrained manipulator: given , find a vector s.t. is minimized

#TT1 J J JJ

#T1T J J J J

J# J1

Underconstrained case (if there are more columns than rows (m