Manipulator Control

1The City College of New York

Jizhong XiaoDepartment of Electrical Engineering

City College of New [email protected]

Manipulator Control

Introduction to ROBOTICS


Outline

• Homework Highlights• Robot Manipulator Control

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

• Midterm Exam Scope


Homework 2

yandxforyandxforyandxforyandxfor

xya

09090180

18090900

),(2tan

21sin x

Joint variables ?

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

Why use atan2 function?

Inverse trigonometric functions have multiple solutions:

?x23cos 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


Homework 3

Z1

x1y1

x2

y2

Z2

m 2

m 1

1

2

LL

10000100

1210121212101212

21

10

20

SSCSCCSC

TTT

1000

20

zzzz

yyyy

xxxx

pasnpasnpasn

T

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

y

x

nn

)sin()cos(

21

21

y

x

pp

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


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(

)(

ijforijforTQT

qTU

ijj

j

j

i

ij 01

100

)()(

),(),(

2

1

2

1

2

1

2221

1211

2

1

cc

hh

DDDD

• Calculate D, H, C terms directly

Physical meaning?

kiorjikjiTQTQTjkiTQTQT

UqU i

jjjkk

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


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 qL

qL

dtd

)(

PKL

)(),()( qCqqHqqD

• Derivation of L-E Formula

1i

i

i

ii z

yx

r

i

prp

Tir

Tii

iiip

i

rii qqUdmrrUTrdKK

1 1

)(21

i

j

iijij

ii

i

jj

j

iii

i rqUrqqTr

dtdVV

11

000 )()(

point at link i

Velocity of point

Kinetic energy of link iiJ

Erroneous answer

100L

r ii


Homework 4

X0

Y0

X1Y1

1

L

m

1001

1

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

21 mlK

18.9 SlmP

PKL

112

11

8.9)(

ClmmlLLdtd

According to physical meaning:


Manipulator Control


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


Jacobian Matrix Revisit

44

60 1000

pasnT

)()()(

3

2

1

qhqhqh

zyx

p

)()()(

)()()(

},,{

6

5

4

qhqhqh

qqq

asn

Forward Kinematics

)(

)()(

)(

6

2

1

16

qh

qhqh

qhY

1616 nnqJY

zyx

Y

1

2

1

6

)(

nn

n

q

qq

dqqdh

nn

n

n

n

qh

qh

qh

qh

qh

qh

qh

qh

qh

dqqdhJ

6

6

2

6

1

6

2

2

2

1

2

1

2

1

1

1

6

)(


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

hh

llll

yx

)cos()cos(cos)sin()sin(sin

21221211

21221211

2

2

1

2

2

1

1

1

llllll

hh

hh

J

2

1

Jyx

Y


Robot Manipulator Control

• Joint Level Controller

)()(),()(

qhYqCqqHqqD

• Task Level Controller

Find a control input (tor), tasqq d

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

R obotC ontro lle r_

tor qqdq dq

dqTra jectory

P lanner

qqe d e e

Task leve lP lanner

R obotD ynam icsC ontro ller

_

tor qqdY dY Forw ard

K inem aticsY Y

YYe d ee

• Robot System:


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• ….


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 controle= 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


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


Control Performance, P-type

Kp = 20

Kp = 200

Kp = 50

Kp = 500


Control Performance, PI - type

Kp = 100

Ki = 50 Ki = 200


You’ve been integrated...

Kp = 100

unstable &

oscillation


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

Kd = 10 Kd = 20

Kd = 5


PID final control


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

uxxx

2

21

uxx

xx

10

0010

2

1

2

1

0 AI 00

1 2

AI

(Equ. 1)


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 xx

kku

xBKAx )(

(Equ. 2)

01

' 122

21

kkkk

AI


Control Theory Review (III)• Feedback linearization

– Nonlinear system

– Example:

UxGxfX )()(

])()()([ 11 VxGxfxGU

VX

Dynam icSystem

U xNonlinearFeedback

Linear System

V

Uxx cos

VxU cos Vx

Original system:

Nonlinear feedback:

Linear system:


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

RobotContro ller_

tor qqdq dq

dqTrajectory

P lanner

qqe d e e


Robot Motion Control (II)• Computed torque method

– Robot system:

– Controller:

)()(),()(

qhYqCqqHqqD

)(),()]()()[( 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


Robot Motion Control (II)

0 ekeke pv

exex

2

1

122

21

xkxkxxx

pv

How to chose Kp, Kv to make the system

stable?Error dynamics

Define states:

AXxx

kkxx

vp

2

1

2

1 10

01 2

pvvp

kkkk

AI

2

42

2,1pvv kkk

In matrix form:

Characteristic equation:

The eigenvalue of A matrix is:

Condition: have negative real part0vk0pk

One of a selections:


Robot Motion Control (III)

)()(),()(

qhYqCqqHqqD

qJqqhdqdY )]([

• Non-linear Feedback Control

qJqJY )(1 qJYJq

)(),()()( 1 qCqqHqJYJqD

Task levelP lanner

RobotD ynam ics

LinearContro ller_

U

qqdY

dYForw ard

K inem aticsYY

YYe d

ee Nonlinear

Feedbacktor

L inear System

Jocobian:

Robot System:


Robot Motion Control (III)

Task leve lP lanner

RobotD ynam ics

LinearController_

U

qqdY

dYForw ard

K inem aticsYY

YYe d

ee Nonlinear

Feedbacktor

Linear 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


Project

Task levelP lanner

RobotDynam ics

LinearControlle r_

U

qqdY

dYForw ard

K inem aticsYY

YYe d

ee Nonlinear

Feedbacktor

Linear System

• Simulation study of Non-linear Feedback Control


Thank you!

x

yz

x

yz

x

yz

x

z

y

HWK 5 posted on the web, Due: Oct. 23 before class

Next Class (Oct. 23): Midterm Exam

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

Manipulator Control

Documents

Transcript of Manipulator Control