Manipulator Control

The City College of New York

1

Jizhong Xiao

Department of Electrical Engineering

City College of New York

[email protected]

Manipulator Control

Introduction to ROBOTICS


2

Outline

• Homework Highlights

• Robot Manipulator Control– Control Theory Review– Joint-level PD Control– Computed Torque Method– Non-linear Feedback Control

• Midterm Exam Scope


3

Homework 2

yandxfor

yandxfor

yandxfor

yandxfor

xya

090

90180

18090

900

),(2tan

2

1sin x

Joint variables ?

Find the forward kinematics, Roll-Pitch-Yaw representation of orientation

Why use atan2 function?

Inverse trigonometric functions have multiple solutions:

?x2

3cos x?x

150,30)2/1(sin 1

30,30)2/3(cos 1

30)2/3,2/1tan( a

)tan()tan( kxx Limit x to [-180, 180] degree


4

Homework 3

Z1

x1y1

x2

y2

Z2

m 2

m 1

1

2

LL

1000

0100

12101212

12101212

21

10

20

SSCS

CCSC

TTT

1000

20

zzzz

yyyy

xxxx

pasn

pasn

pasn

T

Find kinematics model of 2-link robot, Find the inverse kinematics solution

y

x

n

n

)sin(

)cos(

21

21

y

x

p

p

121

121

sin)sin(

cos)cos(

Inverse: know position (Px,Py,Pz) and orientation (n, s, a), solve joint variables.

),(2tan21 xy nna

),(2tan1 xxyy npnpa


5

Homework 4

Z1

x1y1

x2

y2

Z2

m 2

m 1

1

2

LLFind the dynamic model of 2-link

robot with mass equally distributed

)(),()( qCqqHqqD

n

kij

Tjijjkik UJUTrD

),max(

)(

ijfor

ijforTQT

q

TU

ijj

j

j

i

ij0

11

00

)(

)(

),(

),(

2

1

2

1

2

1

2221

1211

2

1

c

c

h

h

DD

DD

• Calculate D, H, C terms directly

Physical meaning?

kiorji

kjiTQTQT

jkiTQTQT

Uq

U ijj

jkk

k

ikk

kjj

j

ijkk

ij

01

11

10

11

11

0

jjji

n

ijji rUgmC

n

mkij

Tjijjkmikm UJUTrh

),,max(

)(

Interaction effects of motion of joints j & k on link i


6

Homework 4

Z1

x1y1

x2

y2

Z2

m 2

m 1

1

2

LLFind the dynamic model of 2-link

robot with mass equally distributed

iii q

L

q

L

dt

d

)(

PKL

)(),()( qCqqHqqD

• Derivation of L-E Formula

1i

i

i

ii z

y

x

r

i

prp

Tir

Tii

iiip

i

rii qqUdmrrUTrdKK

1 1

)(2

1

i

j

iijij

ii

i

jj

j

iii

i rqUrqq

Tr

dt

dVV

11

000 )()(

point at link i

Velocity of point

Kinetic energy of link iiJ

Erroneous answer

1

0

0

L

r ii


7

Homework 4

X0

Y0

X1Y1

1

L

m

1

0

011

l

r

Example: 1-link robot with point mass (m) concentrated at the end of the arm.

Set up coordinate frame as in the figure

21

2

2

1 mlK

18.9 SlmP

PKL

112

11

8.9)(

ClmmlLL

dt

d

According to physical meaning:


8

Manipulator Control


9

Manipulator Dynamics Revisit

)(),()( qCqqHqqD • Dynamics Model of n-link Arm

The Acceleration-related Inertia term, Symmetric Matrix

The Coriolis and Centrifugal terms

The Gravity terms

n

1 Driving torque applied on each link

Non-linear, highly coupled , second order differential equation

Joint torque Robot motion


10

Jacobian Matrix Revisit

44

60 1000

pasnT

)(

)(

)(

3

2

1

qh

qh

qh

z

y

x

p

)(

)(

)(

)(

)(

)(

},,{

6

5

4

qh

qh

qh

q

q

q

asn

Forward Kinematics

)(

)(

)(

)(

6

2

1

16

qh

qh

qh

qhY

1616 nnqJY

z

y

x

Y

1

2

1

6

)(

nn

n

q

q

q

dq

qdh

nn

n

n

n

q

h

q

h

q

h

q

h

q

h

q

hq

h

q

h

q

h

dq

qdhJ

6

6

2

6

1

6

2

2

2

1

2

1

2

1

1

1

6

)(


11

Jacobian Matrix Revisit• Example: 2-DOF planar robot arm

– Given l1, l2 , Find: Jacobian2

1

(x , y)

l2

l1

),(

),(

)sin(sin

)cos(cos

212

211

21211

21211

h

h

ll

ll

y

x

)cos()cos(cos

)sin()sin(sin

21221211

21221211

2

2

1

2

2

1

1

1

lll

lllhh

hh

J

2

1

Jy

xY


12

Robot Manipulator Control

• Joint Level Controller

)(

)(),()(

qhY

qCqqHqqD

• Task Level Controller

Find a control input (tor), tasqq d

tasYY d 0 YYe d Find a control input (tor),

R obotC ontro ller_

tor qqdq dq

dqTrajectory

P lanner

qqe d e e

Task levelP lanner

R obotD ynam ics

C ontro ller_

tor qqdY dY Forw ard

K inem atics

Y YYYe d

ee

• Robot System:


13

Robot Manipulator Control

• Control Methods– Conventional Joint PID Control

• Widely used in industry

– Advanced Control Approaches• Computed torque approach• Nonlinear feedback • Adaptive control• Variable structure control• ….


14

Control Theory Review (I)

actual adesired d V

Motor

actual a

- compute V using PID feedback

d a

Error signal e

PID controller: Proportional / Integral / Derivative control

e= d a

V = Kp • e + Ki ∫ e dt + Kd )d e dt

Closed Loop Feedback Control

Reference book: Modern Control Engineering, Katsuhiko Ogata, ISBN0-13-060907-2


15

Evaluating the response

How can we eliminate

the steady-state error?

steady-state error

settling time

rise time

overshoot

overshoot -- % of final value exceeded at first oscillation

rise time -- time to span from 10% to 90% of the final value

settling time -- time to reach within 2% of the final value

ss error -- difference from the system’s desired value


16

Control Performance, P-type

Kp = 20

Kp = 200

Kp = 50

Kp = 500


17

Control Performance, PI - type

Kp = 100

Ki = 50 Ki = 200


18

You’ve been integrated...

Kp = 100

unstable &

oscillation


19

Control Performance, PID-typeKp = 100 Ki = 200 Kd = 2

Kd = 10 Kd = 20

Kd = 5


20

PID final control


21

Control Theory Review (II)• Linear Control System

– State space equation of a system

– Example: a system:

– Eigenvalue of A are the root of characteristic equation

– Asymptotically stable all eigenvalues of A have negative real part

BuAxx

ux

xx

2

21

ux

x

x

x

1

0

00

10

2

1

2

1

0 AI 00

1 2

AI

(Equ. 1)


22

Control Theory Review (II)– Find a state feedback control such that the

closed loop system is asymptotically stable

– Closed loop system becomes

– Chose K, such that all eigenvalues of A’=(A-BK) have negative real parts

A

B

-K

x xu

xKu

2

121 x

xkku

xBKAx )(

(Equ. 2)

01

' 122

21

kkkk

AI


23

Control Theory Review (III)• Feedback linearization

– Nonlinear system

– Example:

UxGxfX )()(

])()()([ 11 VxGxfxGU

VX

D ynam icSystem

U xN onlinearFeedback

Linear System

V

Uxx cos

VxU cos Vx

Original system:

Nonlinear feedback:

Linear system:


24

Robot Motion Control (I)• Joint level PID control

– each joint is a servo-mechanism– adopted widely in industrial robot– neglect dynamic behavior of whole arm– degraded control performance especially in

high speed– performance depends on configuration

R obotC ontro ller_

tor qqdq dq

dqTrajectory

P lanner

qqe d e e


25

Robot Motion Control (II)• Computed torque method

– Robot system:

– Controller:

)(

)(),()(

qhY

qCqqHqqD

)(),()]()()[( qCqqHqqkqqkqqDtor dp

dv

d

0)()()( qqkqqkqq dp

dv

d

0 ekeke pv Error dynamics

How to chose Kp,

Kv ?

Advantage: compensated for the dynamic effects

Condition: robot dynamic model is known


26

Robot Motion Control (II)

0 ekeke pv

ex

ex

2

1

122

21

xkxkx

xx

pv

How to chose Kp, Kv to make the system

stable?Error dynamics

Define states:

AXx

x

kkx

x

vp

2

1

2

1 10

01

2

pvvp

kkkk

AI

2

42

2,1

pvv kkk

In matrix form:

Characteristic equation:

The eigenvalue of A matrix is:

Condition: have negative real part0vk0pk

One of a selections:


27

Robot Motion Control (III)

)(

)(),()(

qhY

qCqqHqqD

qJqqhdq

dY )]([

• Non-linear Feedback Control

qJqJY )(1 qJYJq

)(),()()( 1 qCqqHqJYJqD

Task levelP lanner

R obotD ynam ics

LinearC ontro ller

_

U

q

qdY

dYForw ard

K inem aticsY

Y

YYe d

ee N onlinear

Feedback

tor

L inear System

Jocobian:

Robot System:


28

Robot Motion Control (III)

Task levelP lanner

R obotD ynam ics

LinearC ontro ller

_

U

q

qdY

dYForw ard

K inem aticsY

Y

YYe d

ee N onlinear

Feedback

tor

L inear System

)(),()()( 1 qCqqHqJUJqDtor

UJqDYJqD 11 )()(

Design the nonlinear feedback controller as:

• Non-linear Feedback Control

Then the linearized dynamic model:

UY

Design the linear controller: )()( YYkYYkYU dpdvd

Error dynamic equation: 0 ekeke pv


29

Midterm Exam Scopes


30

Thank you!

x

yz

x

yz

x

yz

x

z

y

HWK 5 posted on the web,

Next Class: Midterm Exam

Time: 6:30-9:00, Please on time!

Manipulator Control

Documents

Transcript of Manipulator Control