M.S. Seminar – METU Aerospace Engineering Department January 2006 Steering of Redundant Robotic...

M.S. Seminar – METU Aerospace Engineering Department January 2006

Steering of Redundant Robotic Manipulators and Spacecraft Integrated Power and Attitude Control-Control Moment

Gyroscopes

M.S. Thesis Presentation

on

MIDDLE EAST TECHNICAL UNIVERSITY Aerospace Engineering Department

Presentation By : Alkan Altay

Thesis Supervisor : Assoc. Prof. Dr. Ozan Tekinalp


Presentation Outline

Robotic Manipulator Simulations

IPAC-CMG Cluster & IPACS Simulations

IPAC-CMG Systems

Robotic Manipulators

Mechanical Analogy

Inverse Kinematics Problem & Solutions

Blended Inverse Steering Logic

Redundant Actuator Systems

Steering of Redundant Actuators

Thesis Work and Results

Conclusion & Future Work

2/34


Integrated Power and Attitude Control System (IPACS)

IPACS

A Variable Speed CMG That Stores Energy

IPAC – CMG Cluster

3/34


Integrated Power and Attitude Control - Control Moment Gyroscope (IPAC-CMG)• A CMG variant, whose flywheel spin rate is altered by a motor/generator

ikτ

hδhh

τ

kh

...

.

δJJdtd

J

Due to spin acceleration

Due to gimbal velocity

4/34


IPAC-CMG Cluster- Single IPAC-CMG, single direction

- At least 3 IPAC-CMGs for 3-axis attitude control

- 1 redundancy

- Nearly spherical momentum envelope with β= 54.73 deg,

n

iicluster

1

hh

PYRAMID CONFIGURATION

5/34


Robotic Manipulators

• An actuator system composed of joints and series of segments

• Tasked to travel its end-effector on a certain trajectory

• Redundancy Applied To Increase Motion Capability

• Mechanically analog to CMG cluster

6/34


The Mechanical Analogy

Total Ang. Mom. Position

IPAC-CMG Momentum Link Length

Torque End Effector Velocity

Steering Problem Steering Problem

7/34

θθJxω

δωδ,Jh

h

θxxωδ,hh

i

).( ).(

l

)( )(

i


Inverse Kinematics Calculations Steering Laws

Steering Laws For Redundant Systems

Minimum 2-Norm Solution

Singularity Avodiance Steering Logic

Singularity Robust Inverses

xJθ .1?

Steer the actuator through the desired path

Calculate the angular speed of each actuator

Invert a rectangular matrix ?

What if singular ?

8/34


Moore Penrose Pseudo Inverse (Minimum 2-Norm Solution)

).)((or .)( 11 τJJJδxJJJθ TTMP

TTMP

• Minimum normed vector; the solution that requires minimum energy

• Singularity is a problem

• Most steering laws are variants of this pseudo inverse

9/34

OTHER SOLUTIONS :

• Singularity Avoidance Steering Logic

• Singularity Robust Inverse, Damped Least Squares Method

• Extended Jacobian Method, Normal Form Approach, Modified Jacobian Method


Blended InverseSatisfy two objectives; realize the desired path in desired configuration

}...{2

1min err

Terrerrerr

T xRxθQθθ

xθJx .err desirederr θθθ

and Q and R are symmetric positive definite weighting matrices

where,

PROBLEM SOLUTION

)...()..( 1 xRJθQJRJQθ Tdes

TBI

The proper desired quantity is injected through this term

10/34

Pre-plannedSteering


rad/sec 10 and 5.0 with

3,2,1for 1.001.0

iuiii

3-link planar robot manipulator dynamics :

Robotic Manipulator Simulations

max

max

max if

if

)sgn(. u

u

u i

i

i

i

i

uSteering Logic

Direct Kinematical Relationship

11/34


Robotic Manipulator Simulations (Test Case I)

AIMS :

• Repeatability performance of B-inverse on a routinely followed closed path

• Tracking performance of B-inverse, when supplied with false desθ

)15

cos(1

)15

sin(

0

22

0

11

txx

txx

)15

cos(5.05.2

)15

sin(5.0

0

22

0

11

txx

txx

sec 210135 and

sec 1050for

t

t

sec 135105for t

12/34

-1 0 1 2

-3

-2.5

-2

-1.5

-1

-0.5

0

x1 [ m ]

x 2

[ m

]

xrealized

x0

xend


Robotic Manipulator Simulations (Test Case I –MP-inverse Results)

13/34

0

50

100

join

t an

gles

[

deg

]

20

40

60

80

100

120

140

160

180

200

220

0 100 200-30

-20

-10

0

10

join

t ve

loci

ties

[ d

eg/s

ec ]

0 100 200-10

-5

0

5

10

time [ sec ]0 100 200

-5

0

5

10

15

0 100 2000

1

2

3

4

5

time [ sec ]

man

ipul

abili

ty m

easu

re


Robotic Manipulator Simulations (Test Case I –B-inverse Results)

14/34

-20

0

20

40

60

80

join

t an

gles

[

deg

]

40

60

80

100

120

140

60

80

100

120

joint anglesnode

0 100 200-15

-10

-5

0

5

join

t ve

loci

ties

[ d

eg/s

ec ]

0 100 200-20

-10

0

10

time [ sec ]0 100 200

-20

-10

0

10 0 100 2000

1

2

3

4

5

time [ sec ]

man

ipul

abili

ty m

easu

re


Robotic Manipulator Simulations (Test Case II)

AIM :

• The singularity avoidance performance of B-inverse

• MP-inverse drives the system close to an escapable singularity at [ x1 , x2 ] = [-2 , 0 ]

)30

sin(

)30

cos(1

0

22

0

11

txx

txx

sec400for t

Escapable Singularity

15/34

-3 -2.5 -2 -1.5 -1-2

-1.5

-1

-0.5

0

x1 [ m ]

x 2

[ m

]

xrealized

x0

xend


Robotic Manipulator Simulations (Test Case II –MP-inverse Results)

16/34

160

180

200

220

join

t an

gles

[

deg

]

-50

0

50

100

-100

-50

0

50

100

0 20 40-30

-20

-10

0

10

join

t ve

loci

ties

[ d

eg/s

ec ]

0 20 40-40

-20

0

20

40

time [ sec ]0 20 40

-10

0

10

20

30

0 20 400

0.5

1

1.5

2

2.5

time [ sec ]

man

ipul

abili

ty m

easu

re


Robotic Manipulator Simulations (Test Case II –B-inverse Results)

17/34

200

220

240

260

280

join

t an

gles

[

deg

]

60

80

100

120

140

-300

-250

-200

-150

-100

-50joint anglesnode

0 20 40-2

0

2

4

6

join

t ve

loci

ties

[ d

eg/s

ec ]

0 20 40-15

-10

-5

0

5

time [ sec ]0 20 40

-20

-15

-10

-5

0

5

0 20 400

1

2

3

4

5

time [ sec ]

man

ipul

abili

ty m

easu

re


18/34

Robotic Manipulator Simulations (Test Case II – Results)

Steering with MP-inverse Steering with B-inverse

Escapable Singularity Simulations


Robotic Manipulator Simulations (Test Case III)

AIM :

• Singularity transition performance of B-inverse

• The path passes an inescapable singularity at [ x1 , x2 ] = [ 0 , 0 ]

)30

cos(1

)30

sin(

0

22

0

11

txx

txx

sec300for t

Inescapable Singularity

19/34

0 0.5 1 1.5 2 2.5

-1

-0.5

0

0.5

1

x1 [ m ]

x 2

[ m

]

xrealized

x0

xend


Robotic Manipulator Simulations (Test Case III –MP-inverse Results)

20/34

0

20

40

60

join

t an

gles

[

deg

]

260

280

300

320

340

360

180

200

220

240

260

280

0 20 400

1

2

3

4

5

join

t ve

loci

ties

[ d

eg/s

ec ]

0 20 400

20

40

60

80

time [ sec ]0 20 40

-150

-100

-50

0

50

0 20 400

0.5

1

1.5

2

2.5

time [ sec ]

man

ipul

abili

ty m

easu

re


Robotic Manipulator Simulations (Test Case III –B-inverse Results)

21/34

-10

-5

0

5

10

15

join

t an

gles

[

deg

]

250

300

350

400

450

50

100

150

200

250

300joint anglesnode

0 20 40-6

-4

-2

0

2

4

join

t ve

loci

ties

[ d

eg/s

ec ]

0 20 400

2

4

6

8

10

time [ sec ]0 20 40

-15

-10

-5

0

0 20 400

0.5

1

1.5

2

2.5

time [ sec ]

man

ipul

abili

ty m

easu

re


22/34

Robotic Manipulator Simulations (Test Case III – Results)

Steering with B-inverse

Inescapable Singularity Simulations


IPAC-CMG Cluster Simulations

23/34

IPAC-CMG Cluster

Torque and Power Commands

STEERING ALGORITHMS

Realized Torque and Power

Rate Command to each IPAC-CMG

AIMS :

• Investigate the performance of IPAC-CMG cluster

• Investigate the performance of B-inverse



24/34

Generic simulation model

( used in MP-inverse simulations )

B-inverse simulation model

Two different simulation models are employed to steer IPAC-CMG cluster


0.1

0.2

0.3

0.4

0.5

x [

N.m

]

-1

-0.5

0

0.5

1

y [

N.m

]-1

-0.5

0

0.5

1

z [

N.m

]

0 50 100 150 2000

10

20

30

40

50

60

t [ sec ]

h x [

N.m

.s ]

0 50 100 150 200-1

-0.5

0

0.5

1

t [ sec ]

h y [

N.m

.s ]

0 50 100 150 200-1

-0.5

0

0.5

1

t [ sec ]

h z [

N.m

.s ]


25/34

0 50 100 150 20030

35

40

45

50

t [ sec ]

Eki

netic

[

Wat

t-h

]

0 50 100 150 200-350

-300

-250

-200

-150

-100

t [ sec ]

Pco

mm

and

[ W

att

]

Torque Command

Power Command

Min Ang.Mom.of each IPAC-CMG [Nms]

7.7

IPAC-CMG Flywh. Spin Interval [kRPM]

15 – 60

Initial Flywheel Spin Rates (kRPM) [40, 40, 40, 40]

Initial Gimbal Angles (deg) [0, 0, 0, 0]


IPAC-CMG Cluster Simulations – MP-inverse Results

26/34

-0.1

0

0.1

0.2

0.3

0.4

x [

N.m

]

-1

-0.5

0

0.5

1

y [

N.m

]-1

-0.5

0

0.5

1

z [

N.m

]0 50 100 150 200

0

5

10

15

20

25

t [ sec ]

h x [

N.m

.s ]

0 50 100 150 200-1

-0.5

0

0.5

1

t [ sec ]

h y [

N.m

.s ]

0 50 100 150 200-1

-0.5

0

0.5

1

t [ sec ]h z

[ N

.m.s

]

real

comm

Torque & Angular Momentum RealizedSingularity Measure

0 50 100 150 2000

0.5

1

1.5

t [ sec ]

Sin

gula

rity

Mea

sure

0 50 100 150 200-100

-80

-60

-40

-20

0

1 [

deg

]

0 50 100 150 200-1

-0.5

0

0.5

1

2 [

deg

]

0 50 100 150 2000

20

40

60

80

100

t [ sec ]

3 [

deg

]

0 50 100 150 200-1

-0.5

0

0.5

1

t [ sec ]

4 [

deg

]

Gimbal Angle History

0 50 100 150 20032

34

36

38

40

1

[

kRP

M ]

0 50 100 150 20032

34

36

38

40

2

[

kRP

M ]

0 50 100 150 20032

34

36

38

40

t [ sec ]

3

[

kRP

M ]

0 50 100 150 20032

34

36

38

40

t [ sec ]

4

[

kRP

M ]

Flywheel Spin Rates

0 50 100 150 20030

35

40

45

50

t [ sec ]

Eki

netic

[

Wat

t-h

]

0 50 100 150 200-350

-300

-250

-200

-150

-100

t [ sec ]

Pre

aliz

ed

[ W

att

]

Energy and Power Profiles


IPAC-CMG Cluster Simulations – B-inverse Results

27/34

Torque Error & Ang. Mom. Profile

-4

-3

-2

-1

0

1x 10

-6

x err

or

[ N

.m ]

-1

0

1

2

3x 10

-8

y err

or

[ N

.m ]

-6

-4

-2

0

2x 10

-8

z err

or

[ N

.m ]

0 50 100 150 2000

10

20

30

40

50

60

t [ sec ]

h x [ N

.m.s

]

0 50 100 150 200-5

0

5

10

15x 10

-4

t [ sec ]

h y [ N

.m.s

]

0 50 100 150 200-5

0

5

10

15x 10

-4

t [ sec ]h z

[ N.m

.s ]

Singularity Measure

0 50 100 150 2000

0.5

1

1.5

2

t [ sec ]

Sin

gula

rity

Mea

sure

Gimbal Angle History

0 50 100 150 200-150

-100

-50

0

1 [

deg

]

0 50 100 150 200-50

0

50

100

150

200

2 [

deg

]

nodes

0 50 100 150 2000

50

100

150

200

t [ sec ]

3 [

deg

]

0 50 100 150 200-60

-40

-20

0

t [ sec ]

4 [

deg

]

0 50 100 150 20032

34

36

38

40

1

[

kRP

M ]

0 50 100 150 20032

34

36

38

40

2

[

kRP

M ]

0 50 100 150 20032

34

36

38

40

t [ sec ]

3

[

kRP

M ]

0 50 100 150 20032

34

36

38

40

t [ sec ]

4

[

kRP

M ]

Flywheel Spin Rates

0 50 100 150 20030

35

40

45

50

t [ sec ]

Eki

netic

[

Wat

t-h

]

0 50 100 150 200-350

-300

-250

-200

-150

-100

t [ sec ]

Pre

aliz

ed

[ W

att

]



IPACS Simulations

28/34

Spacecraft Inertias [ kgm2 ] [15, 15, 10]

Initial Orientation of S/C [deg] [0, 0, 0]

IPAC-CMG Flywh. Spin Interval [kRPM] 15 - 60

Initial Flywheel Spin Rates [kRPM] [39, 40, 41, 42]

Initial Gimbal Angles [deg] [-75, 0, 75, 0]


IPACS Simulations

29/34

Spacecraft IPACS Simulation Model


IPACS Simulations

30/34

0

20

40

60

[ de

g ]

-1

0

1

[

deg

]

0 50 100 150 200 250 300-1

0

1

t [ sec ]

[ d

eg ]

0 50 100 150 200 250 300-300

-200

-100

0

100

200

t [ sec ]

Pco

mm

and

[ W

att

]

Attitude Command

Power Command


IPACS Simulations – MP-inverse Results

31/34

Attitude Profile

0

200

400

600

[ de

g ]

-4

-2

0

2

[ de

g ]

0 50 100 150 200 250 300-4

-2

0

2

t [ sec ]

[ d

eg ]

RPYreal

RPYcomm

0 100 200 3000

0.1

0.2

t [ sec ]

Sin

gula

rity

Mea

sure

Singularity Measure

0 50 100 150 200 250 30035

36

37

38

39

40

41

42

t [ sec ]

[ kR

PM

]

1

2

3

4

IPAC-CMG Flywheel Spin RatesGimbal Angles

-95

-90

-85

-80

-75

-70

1

[ de

g ]

-3

-2

-1

0

1

2

[ de

g ]

0 100 200 30070

75

80

85

90

t [ sec ]

3

[ de

g ]

0 100 200 300-1

0

1

2

3

t [ sec ]

4

[ de

g ]

-0.4

-0.2

0

0.2

0.4

0.6

x [

N.m

]

-0.04

-0.02

0

0.02

0.04

y [

N.m

]

real

comm

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

z [

N.m

]0 100 200 300

22

22.5

23

23.5

24

24.5

t [ sec ]

h x [

N.m

.s ]

0 100 200 300-0.4

-0.2

0

0.2

0.4

0.6

t [ sec ]

h y [

N.m

.s ]

0 100 200 3000.4

0.5

0.6

0.7

0.8

0.9

t [ sec ]h z

[ N

.m.s

]

Torque and Angular Momentum History

0 100 200 30040

42

44

46

48

50

t [ sec ]

Eki

netic

[

Wat

t-h

]

0 100 200 300-400

-200

0

200

400

t [ sec ]

Pre

aliz

ed

[ W

att

]

Energy and Power Profile


IPACS Simulations – B-inverse Results

32/34

Attitude Profile

-50

0

50

100

roll,

[ d

eg ]

-2

0

2

4x 10

-6

pitc

h,

[ de

g ]

0 50 100 150 200 250 300-4

-2

0

2x 10

-6

t [ sec ]

yaw

,

[ de

g ]

RPYreal

RPYcom

0 100 200 3000

0.5

1

1.5

t [ sec ]

Sin

gula

rity

Mea

sure

Singularity Measure

-80

-70

-60

-50 1

[ de

g ]

0

50

100

150

2 [

deg

]

nodes

0 100 200 30060

80

100

120

140

t [ sec ]

3 [

deg

]

0 100 200 300-100

-80

-60

-40

-20

0

t [ sec ]

4 [

deg

]

Gimbal Angles

-4

-2

0

2

4x 10

-8

x err

or

[

N.m

]

-6

-4

-2

0

2

4x 10

-9

y err

or

[

N.m

]-2

-1

0

1

2

3x 10

-8

z err

or

[

N.m

]0 100 200 300

23.2

23.4

23.6

23.8

24

24.2

t [ sec ]

h x [

N.m

.s ]

0 100 200 300-0.4

-0.2

0

0.2

0.4

0.6

t [ sec ]

h y [

N.m

.s ]

0 100 200 3000.6

0.7

0.8

0.9

1

t [ sec ]h z

[ N

.m.s

]

Torque Error and Ang.Mom. Profile

0 50 100 150 200 250 30035

36

37

38

39

40

41

42

t [ sec ]

[ kR

PM

]

1

2

3

4

IPAC-CMG Flywheel Spin Rates

0 100 200 30040

42

44

46

48

50

t [ sec ]

Eki

netic

[

Wat

t-h

]

0 100 200 300-400

-200

0

200

400

t [ sec ]

Pre

aliz

ed

[ W

att

]



Conclusion

33/34

• B-inverse is employed in robotic manipulators :

Singularity Avoidance

Singularity Transition

Repeatability

• IPACS is discussed :

Comparison to Current Technologies

Algorithm Construction

Theoretical Performance

• B-inverse is employed in IPACS :

In IPAC-CMG Clusters & S/C IPACS

Singularity Avoidance & Multi Steering


Future Work

B-inverse in highly redundant robotic mechanisms

34/34

Detail Design of IPAC-CMG Capabilities of B-inverse


TBI


Singularity in Robotic Manipulators and CMG Systems • Physically, no end effector

velocity (torque) can be produced in a certain direction

• Controllability in that direction is lost.

• Mathematically, Jacobian Matrix loses its rank.Thus;

1. det(J)= 0 ( or det(JJT)=0 )

2. Singularity Measure m=det(JJT)

3. J-1 ( or (JJT)-1 ) becomes undefined

#/30


Singularity Avoidance Steering Logic

nxJJJθ ..).( 1 TT

Particular Solution

Homogeneous Solution

θθJx ).(

0. nJ

Addition of null motion, n, in the proper amount (determined by γ)

12/40


Singularity Robust Solutions

• Disturbs the pseudo solution near singularities to artificially generate a well –conditioned matrix

• Increases the tracking error, causes sharp velocity changes around singularities

• Another example may be the Damped Least Squares Method

Singularity Robust Inverse : xJJIJθ .).( 1 TT

SR k

k = 0 for m > mcr

k0(1-m/m0)2 for m < mcr

13/40


Singularity Robust SolutionsNew generation of solutions, offering accurate and smooth singularity transitions, not mature yet

• Extended Jacobian Method

• Normal Form Approach

• Modified Jacobian Method

Extends the jacobian matrix with additional functions, creating a well –conditioned one, belonging to a “virtual” system

square matrix

)(

)(

θ

θJJ

fvir

singularity

Proposes to transform the kinematics to its quadratic normal form, employing equivalence transformation, around singularities

Proposes to replace the linearly dependent row of Jacobian Matrix, to remove the singularity, with a derivative of a configuration dependent function

J

)(θf

14/40


Thesis Objectives

Blended Inverse on IPAC-CMG clusters

Blended Inverse on Redundant Robotic Manipulators

Spacecraft Energy Storage & Attitude Control

IPAC-CMG based IPACS

0 100 200-50

0

50

100

join

t an

gles

[

deg

]

0 100 20040

60

80

100

120

140

time [ sec ]0 100 200

40

60

80

100

120

0 100 200-15

-10

-5

0

5

join

t ve

loci

ties

[ d

eg/s

ec ]

0 100 200-20

-10

0

10

20

time [ sec ]0 100 200

-20

-10

0

10

20

joint anglesknot

3/40


Spacecraft Energy Storage and Attitude Control

• Spacecraft store & drain energy periodically.

• Rotating flywheels for smooth attitude control

• Integrate energy storage & attitude control

Electrochemical Batteries

vs.Flywheel Energy Storage Systems (FES)

4/40


Blended Inverse

How to select ?desθ


TBI

11/40

Pre-planned Steering

cur

curknotdes tk

t

θθθ

pk

tkttk cur

,...,1 where

)1(

M.S. Seminar – METU Aerospace Engineering Department January 2006 Steering of Redundant Robotic...

Documents

Transcript of M.S. Seminar – METU Aerospace Engineering Department January 2006 Steering of Redundant Robotic...