108
Control problems for floating-base manipulators Gianluca Antonelli Universit`a di Cassino e del Lazio Meridionale [email protected] http://webuser.unicas.it/lai/robotica http://www.eng.docente.unicas.it/gianluca antonelli Trondheim, 23 April 2014 Gianluca Antonelli Trondheim, 23 april 2014

Control problems for floating-base manipulators

Embed Size (px)

Citation preview

Page 1: Control problems for floating-base manipulators

Control problems for floating-base manipulators

Gianluca Antonelli

Universita di Cassino e del Lazio Meridionale

[email protected]

http://webuser.unicas.it/lai/robotica

http://www.eng.docente.unicas.it/gianluca antonelli

Trondheim, 23 April 2014

Gianluca Antonelli Trondheim, 23 april 2014

Page 2: Control problems for floating-base manipulators

Outline

Introduction & variable definition

Inverse Kinematics

A possible kinematic solution: NSB behavioral control

A possible dynamic solution: Virtual decomposition

Simulation/experiments

Perspectives

Gianluca Antonelli Trondheim, 23 april 2014

Page 3: Control problems for floating-base manipulators

Space, aerial and underwater vehicle-manipulators

DLRCanadian Space Agency

ALIVE

normal robots but

floating base

kinematic coupling

dynamic coupling

unstructured environment

Gianluca Antonelli Trondheim, 23 april 2014

Page 4: Control problems for floating-base manipulators

Cooperation

RedSea project

RedSea project

AMADEUS

ARCAS

Gianluca Antonelli Trondheim, 23 april 2014

Page 5: Control problems for floating-base manipulators

Aerial Robotics Cooperative Assembly

Gianluca Antonelli Trondheim, 23 april 2014

Page 6: Control problems for floating-base manipulators

Aerial Robotics Cooperative Assembly

ΣoΣe,1 Σe,2

Σv,1 Σv,2

Our task: cooperative control of the bar

Gianluca Antonelli Trondheim, 23 april 2014

Page 7: Control problems for floating-base manipulators

Marine Autonomous Robotics for InterventionS

Gianluca Antonelli Trondheim, 23 april 2014

Page 8: Control problems for floating-base manipulators

Marine Autonomous Robotics for InterventionS

Our task: cooperative control of the bar (and logo design. . . )

Gianluca Antonelli Trondheim, 23 april 2014

Page 9: Control problems for floating-base manipulators

Floating robots kinematics

Oi

η1

ηee

❅❅❘end­effector velocities

❍❍❍❍❍❍❍❍❍❍❍❍❥Jacobian

system velocitiesηee =

[ηee1

ηee2

]= J(RI

B, q)ζ ζ =

ν1

ν2

q

Gianluca Antonelli Trondheim, 23 april 2014

Page 10: Control problems for floating-base manipulators

UVMS dynamics in matrix form

M(q)ζ +C(q, ζ)ζ +D(q, ζ)ζ + g(q,RIB) = τ

formally equal to a ground­fixed industrial manipulator 1

however. . .

Model knowledge

Bandwidth of the sensor’s readings

Vehicle hovering control

Dynamic coupling between vehicle and manipulator

External disturbances (current)

Kinematic redundancy of the system

1[Siciliano et al.(2009)Siciliano, Sciavicco, Villani, and Oriolo][Fossen(2002)] [Schjølberg and Fossen(1994)]

Gianluca Antonelli Trondheim, 23 april 2014

Page 11: Control problems for floating-base manipulators

Dynamics

Movement of vehicle and manipulator coupled

movement of the vehicle carrying the manipulator

law of conservation of momentum

Need to coordinate

at velocity level ⇒ kinematic control

at torque level ⇒ dynamic control 2

2[McLain et al.(1996b)McLain, Rock, and Lee][McLain et al.(1996a)McLain, Rock, and Lee]

Gianluca Antonelli Trondheim, 23 april 2014

Page 12: Control problems for floating-base manipulators

Outline

Introduction & variable definition

Inverse Kinematics

A possible kinematic solution: NSB behavioral control

A possible dynamic solution: Virtual decomposition

Simulation/experiments

Perspectives

Gianluca Antonelli Trondheim, 23 april 2014

Page 13: Control problems for floating-base manipulators

Kinematic control in pills -1-

✛✚

✘✙

ζ

✛✚

✘✙

σ

Starting from a generic m-dimensional task (e.g., the e.e. position)

σ = f(η, q) ∈ Rm σ = J(η, q)ζ

An inverse mapping is required

Gianluca Antonelli Trondheim, 23 april 2014

Page 14: Control problems for floating-base manipulators

Kinematic control in pills -1-

✛✚

✘✙

ζ

✛✚

✘✙

σ

✖✕✗✔

Starting from a generic m-dimensional task (e.g., the e.e. position)

σ = f(η, q) ∈ Rm σ = J(η, q)ζ

An inverse mapping is required

Gianluca Antonelli Trondheim, 23 april 2014

Page 15: Control problems for floating-base manipulators

Kinematic control in pills -2-

σ = Jζ inverted by solving proper optimization problems

Pseudoinverseζ = J †σ = JT

(JJT

)−1σ

Transpose-basedζ = JTσ

Weighted pseudoinverse

ζ = J†W

σ = W−1JT(JW−1JT

)−1σ

Damped Least-Squares

ζ = JT(JJT + λ2Im

)−1σ

need for closed­loop also. . .

Gianluca Antonelli Trondheim, 23 april 2014

Page 16: Control problems for floating-base manipulators

Kinematic control in pills -3-

A robotic system is kinematically redundant when it possesses moredegrees of freedom than those required to execute a given task

Gianluca Antonelli Trondheim, 23 april 2014

Page 17: Control problems for floating-base manipulators

Kinematic control in pills -3-

A robotic system is kinematically redundant when it possesses moredegrees of freedom than those required to execute a given task

Redundancy may be used to add additional tasks

✛✚

✘✙

ζ

✛✚

✘✙

σ

✖✕✗✔

Gianluca Antonelli Trondheim, 23 april 2014

Page 18: Control problems for floating-base manipulators

Kinematic control in pills -3-

A robotic system is kinematically redundant when it possesses moredegrees of freedom than those required to execute a given task

Redundancy may be used to add additional tasks

✛✚

✘✙

ζ

✛✚

✘✙

σ

✖✕✗✔

■ σa

✚✙✛✘

σb

✖✕✗✔

Gianluca Antonelli Trondheim, 23 april 2014

Page 19: Control problems for floating-base manipulators

Kinematic control scheme

second. tasks

ηd, qd τ η, q

IKmain task

Control

Output of kinematic control is the variable to be controlled by theactuators (vehicle thrusters and joints’ torques)

Gianluca Antonelli Trondheim, 23 april 2014

Page 20: Control problems for floating-base manipulators

A first kinematic solution

Assuming the vehicle in hovering is not the best strategy to e.e. finepositioning3, better to kinematically compensate with the manipulator

3[Hildebrandt et al.(2009)Hildebrandt, Christensen, Kerdels, Albiez, and Kirchner]Gianluca Antonelli Trondheim, 23 april 2014

Page 21: Control problems for floating-base manipulators

Handling several tasks

Extended Jacobian4

Add additional (6 + n)−m constraints

h(η, q) = 0 with associated Jh

such that the problem is squared with

0

]=

[J

Jh

4[Chiaverini et al.(2008)Chiaverini, Oriolo, and Walker]Gianluca Antonelli Trondheim, 23 april 2014

Page 22: Control problems for floating-base manipulators

Handling several tasks -2-

Augmented JacobianAn additional task is given

σh = h(η, q) with associated Jh

such that the problem is squared with

σh

]=

[J

Jh

Gianluca Antonelli Trondheim, 23 april 2014

Page 23: Control problems for floating-base manipulators

Handling several tasks -3-

Task priority redundancy resolution

σh = h(η, q) with associated Jh

further projected on the the null space of the higher priority one

ζ = J†σ +[Jh

(I − J †J

)]† (σh − JhJ

†σ)

Gianluca Antonelli Trondheim, 23 april 2014

Page 24: Control problems for floating-base manipulators

Handling several tasks -4-

Singularity robust task priority redundancy resolution 5

σh = h(η, q) with associated Jh

further projected on the the null space of the higher priority one

ζ = J †σ +(I − J†J

)J†

hσh

5we are talking about algorithmic singularitieshere. . . [Chiaverini(1997)]

Gianluca Antonelli Trondheim, 23 april 2014

Page 25: Control problems for floating-base manipulators

Handling several tasks -5-

AMADEUS

Agility task priority6

Task priority framework to handle both precision and set tasksEach task is the norm of the corresponding error (i.e., mi = 1)Recursive constrained least-squares within the set satisfyinghigher-priority tasks

6[Casalino et al.(2012)Casalino, Zereik, Simetti, Sperinde, and Turetta]Gianluca Antonelli Trondheim, 23 april 2014

Page 26: Control problems for floating-base manipulators

Handling several tasks -6-

Behavioral algorithms (behavior=task), bioinspired, artificialpotentials, neuro-fuzzy, cognitive approaches, etc.

btw. . . mood ?

Gianluca Antonelli Trondheim, 23 april 2014

Page 27: Control problems for floating-base manipulators

But. . .

What are these tasks we are talking about ?

Gianluca Antonelli Trondheim, 23 april 2014

Page 28: Control problems for floating-base manipulators

Tasks to be controlled

Given 6 + n DOFs and m-dimensional tasks: End-effector

position, m = 3

pos./orientation, m = 6

distance from a target, m = 1

alignment with the line of sight, m = 2

Gianluca Antonelli Trondheim, 23 april 2014

Page 29: Control problems for floating-base manipulators

Tasks to be controlled

Manipulator joint-limits

several approaches proposed, m = 1 to n, e.g.

h(q) =n∑

i=1

1

ci

qi,max − qi,min

(qi,max − qi)(qi − qi,min)

Gianluca Antonelli Trondheim, 23 april 2014

Page 30: Control problems for floating-base manipulators

Tasks to be controlled

Drag minimization, m = 1 7

h(q) = DT(q, ζ)WD(q, ζ)

within a second order solution

ζ = J †(σ − Jζ

)− k

(I − J†J

)([ ∂h∂η∂h∂q

]+

∂h

∂ζ

)

7[Sarkar and Podder(2001)]Gianluca Antonelli Trondheim, 23 april 2014

Page 31: Control problems for floating-base manipulators

Tasks to be controlled

Manipulability/singularity, m = 1

h(q) =∣∣det

(JJT

)∣∣(In 8 priorities dynamically swapped between singularity and e.e.)

joints

inhibited direction

singularitysingularity

setclose to

8[Kim et al.(2002)Kim, Marani, Chung, and Yuh,Casalino and Turetta(2003)] [Chiacchio et al.(1991)Chiacchio, Chiaverini, Sciavicco, and

Gianluca Antonelli Trondheim, 23 april 2014

Page 32: Control problems for floating-base manipulators

Tasks to be controlled

Restoring moments:

m = 3 keep close gravity-buoyancy of the overall system 9

m = 2 align gravity and buoyancy (SAUVIM is 4 tons) 10

f b

f g

τ 2

9[Han and Chung(2008)]10[Marani et al.(2010)Marani, Choi, and Yuh]

Gianluca Antonelli Trondheim, 23 april 2014

Page 33: Control problems for floating-base manipulators

Tasks to be controlled

Obstacle avoidance m = 1

Gianluca Antonelli Trondheim, 23 april 2014

Page 34: Control problems for floating-base manipulators

Tasks to be controlled

Workspace-related variablesVehicle distance from the bottom, m = 1Vehicle distance from the target, m = 1

Gianluca Antonelli Trondheim, 23 april 2014

Page 35: Control problems for floating-base manipulators

Tasks to be controlled

Sensors configuration variables

Vehicle roll and pitch, m = 2Misalignment between the camera optical axis and the target lineof sight, m = 2

Gianluca Antonelli Trondheim, 23 april 2014

Page 36: Control problems for floating-base manipulators

Tasks to be controlled

Visual servoing variables

Features in the image plane 11

11[Mebarki et al.(2013)Mebarki, Lippiello, and Siciliano,Mebarki and Lippiello(in press, 2014)]

Gianluca Antonelli Trondheim, 23 april 2014

Page 37: Control problems for floating-base manipulators

Outline

Introduction & variable definition

Inverse Kinematics

A possible kinematic solution: NSB behavioral control

A possible dynamic solution: Virtual decomposition

Simulation/experiments

Perspectives

Gianluca Antonelli Trondheim, 23 april 2014

Page 38: Control problems for floating-base manipulators

Behavioral control in pills

Inspired from animal behavior

sensorsbehavior a

actuators

behavior bactuators

behavior cactuators

How to combine them in one single behavior?

Gianluca Antonelli Trondheim, 23 april 2014

Page 39: Control problems for floating-base manipulators

Behavioral control in pills

Inspired from animal behavior

sensorsbehavior a

actuators

behavior bactuators

behavior cactuators

How to combine them in one single behavior?

Gianluca Antonelli Trondheim, 23 april 2014

Page 40: Control problems for floating-base manipulators

Competitive behavioral control

Behaviors are in competitions and the higher priority can subsume thelower ones12

sensorsbehavior b

ζ2

behavior a

ζ1

behavior c

ζ3 ζd

12[Brooks(1986)]Gianluca Antonelli Trondheim, 23 april 2014

Page 41: Control problems for floating-base manipulators

Cooperative behavioral control

Behaviors cooperate and the priority is embedded in the gains13

sensorsbehavior b

ζ2 ⊗

α2

behavior a

ζ1

supervisor

α1

behavior c

ζ3 ⊗

α3

∑ ζd

13[Arkin(1989)]Gianluca Antonelli Trondheim, 23 april 2014

Page 42: Control problems for floating-base manipulators

NSB

Null Space-based Behavioral control

Each action is decomposed in elementary behaviors/tasks

motion reference command for each task

ζd = J †(σd +Λσ

)σ = σd−σ

Gianluca Antonelli Trondheim, 23 april 2014

Page 43: Control problems for floating-base manipulators

NSB: Merging different tasks

NSB inherits the approach of the singularity-robust task priorityinverse kinematics technique

ζd = J †a

(σa,d +Λaσa

)

︸ ︷︷ ︸+ J

†b

(σb,d +Λbσb

)

︸ ︷︷ ︸primary secondary

Thus, defining:

ζa = J †a

(σa,d +Λaσa

)Na =

(I − J†

aJa

)

ζb = J†b

(σb,d +Λbσb

)

Gianluca Antonelli Trondheim, 23 april 2014

Page 44: Control problems for floating-base manipulators

NSB: Merging different tasks

NSB inherits the approach of the singularity-robust task priorityinverse kinematics technique

ζd = J †a

(σa,d +Λaσa

)

︸ ︷︷ ︸+(I − J†

aJa

)

︸ ︷︷ ︸J†b

(σb,d +Λbσb

)

︸ ︷︷ ︸primary null space secondary

Thus, defining:

ζa = J †a

(σa,d +Λaσa

)Na =

(I − J†

aJa

)

ζb = J†b

(σb,d +Λbσb

)

Gianluca Antonelli Trondheim, 23 april 2014

Page 45: Control problems for floating-base manipulators

NSB: Merging different tasks

NSB inherits the approach of the singularity-robust task priorityinverse kinematics technique

ζd = J †a

(σa,d +Λaσa

)

︸ ︷︷ ︸+(I − J†

aJa

)

︸ ︷︷ ︸J†b

(σb,d +Λbσb

)

︸ ︷︷ ︸primary null space secondary

Thus, defining:

ζa = J †a

(σa,d +Λaσa

)Na =

(I − J†

aJa

)

ζb = J†b

(σb,d +Λbσb

)

Gianluca Antonelli Trondheim, 23 april 2014

Page 46: Control problems for floating-base manipulators

NSB: Merging different tasks

NSB inherits the approach of the singularity-robust task priorityinverse kinematics technique

ζd = J †a

(σa,d +Λaσa

)

︸ ︷︷ ︸+(I − J†

aJa

)

︸ ︷︷ ︸J†b

(σb,d +Λbσb

)

︸ ︷︷ ︸primary null space secondary

Thus, defining:

ζa = J †a

(σa,d +Λaσa

)Na =

(I − J†

aJa

)

ζb = J†b

(σb,d +Λbσb

)

ζd = ζa +Naζb

Gianluca Antonelli Trondheim, 23 april 2014

Page 47: Control problems for floating-base manipulators

NSB: Three-task example

ζa = J †a

(σa,d +Λaσ1

)

ζb = J†b

(σb,d +Λbσ2

)

ζc = J †c

(σc,d +Λcσ3

)

Successive projection approach

Na =(I − J †

aJa

)

N b =(I − J

†bJ b

)

ζd = ζa +Naζb +NaN bζc

Augmented projection approach

Jab =

[Ja

J b

]

Nab =(In − J

†abJab

)

ζd = ζa +Naζb+Nabζc

Gianluca Antonelli Trondheim, 23 april 2014

Page 48: Control problems for floating-base manipulators

NSB: Three-task example

ζa = J †a

(σa,d +Λaσ1

)

ζb = J†b

(σb,d +Λbσ2

)

ζc = J †c

(σc,d +Λcσ3

)

Successive projection approach

Na =(I − J †

aJa

)

N b =(I − J

†bJ b

)

ζd = ζa +Naζb +NaN bζc

Augmented projection approach

Jab =

[Ja

J b

]

Nab =(In − J

†abJab

)

ζd = ζa +Naζb+Nabζc

Gianluca Antonelli Trondheim, 23 april 2014

Page 49: Control problems for floating-base manipulators

NSB: Three-task example

ζa = J †a

(σa,d +Λaσ1

)

ζb = J†b

(σb,d +Λbσ2

)

ζc = J †c

(σc,d +Λcσ3

)

Successive projection approach

Na =(I − J †

aJa

)

N b =(I − J

†bJ b

)

ζd = ζa +Naζb +NaN bζc

Augmented projection approach

Jab =

[Ja

J b

]

Nab =(In − J

†abJab

)

ζd = ζa +Naζb+Nabζc

Gianluca Antonelli Trondheim, 23 april 2014

Page 50: Control problems for floating-base manipulators

From behaviors to actions

sensing/perception

elementary behaviors actions

commands

supervisor

Gianluca Antonelli Trondheim, 23 april 2014

Page 51: Control problems for floating-base manipulators

Simple comparison: move to goal with obstacleavoidance

obstacle avoidance

σ1 = ‖p− po‖ ∈ R

σ1,d = d

J1 = rT ∈ R1×2

r =p− po

‖p− po‖

ζ1 = J†1λ1 (d− ‖p−po‖)

N (J1) = I − J†1J1 = I − rrT

move to goal

σ2 = p ∈ R2

σ2,d = pg

J2 = I ∈ R2×2

ζ2 = Λ2

(pg − p

)

Gianluca Antonelli Trondheim, 23 april 2014

Page 52: Control problems for floating-base manipulators

Simple comparison: competitive approach

Gianluca Antonelli Trondheim, 23 april 2014

Page 53: Control problems for floating-base manipulators

Simple comparison: competitive approach

❆❆❆❆❆❯

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Page 54: Control problems for floating-base manipulators

Simple comparison: competitive approach

only obstacle avoidance

Gianluca Antonelli Trondheim, 23 april 2014

Page 55: Control problems for floating-base manipulators

Simple comparison: competitive approach

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Page 56: Control problems for floating-base manipulators

Simple comparison: cooperative approach

Gianluca Antonelli Trondheim, 23 april 2014

Page 57: Control problems for floating-base manipulators

Simple comparison: cooperative approach

❆❆❆❆❯

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Page 58: Control problems for floating-base manipulators

Simple comparison: cooperative approach

❇❇❇❇◆

linear combination: higher task is corrupted

Gianluca Antonelli Trondheim, 23 april 2014

Page 59: Control problems for floating-base manipulators

Simple comparison: cooperative approach

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Page 60: Control problems for floating-base manipulators

Simple comparison: NSB

Gianluca Antonelli Trondheim, 23 april 2014

Page 61: Control problems for floating-base manipulators

Simple comparison: NSB

❆❆❆❆❯

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Page 62: Control problems for floating-base manipulators

Simple comparison: NSB

❇❇❇◆

null­space­projection: higher task is fulfilled

Gianluca Antonelli Trondheim, 23 april 2014

Page 63: Control problems for floating-base manipulators

Simple comparison: NSB

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Page 64: Control problems for floating-base manipulators

Gain tuning

Cooperative

task a b c

situation 1 α1,1 α1,2 α1,3

sit. 2 α2,1 α2,2 α2,3

sit. 3 α3,1 α3,2 α3,3

sit. 4 α4,1 α4,2 α4,3

NSB

Each behavior tuned as if it was alone butin each situation the priority needs to be designed

Gianluca Antonelli Trondheim, 23 april 2014

Page 65: Control problems for floating-base manipulators

Gain tuning

Cooperative

task a b c d

situation 1 α1,1 α1,2 α1,3 α1,4

sit. 2 α2,1 α2,2 α2,3 α2,4

sit. 3 α3,1 α3,2 α3,3 α3,4

sit. 4 α4,1 α4,2 α4,3 α4,4

NSB

Each behavior tuned as if it was alone butin each situation the priority needs to be designed

Gianluca Antonelli Trondheim, 23 april 2014

Page 66: Control problems for floating-base manipulators

Gain tuning

Cooperative

task a b c

situation 1 α1,1 α1,2 α1,3

sit. 2 α2,1 α2,2 α2,3

sit. 3 α3,1 α3,2 α3,3

sit. 4 α4,1 α4,2 α4,3

NSB

Each behavior tuned as if it was alone butin each situation the priority needs to be designed

Gianluca Antonelli Trondheim, 23 april 2014

Page 67: Control problems for floating-base manipulators

Stability analysis

Lyapunov function14

V (σ) = 1

2σTσ > 0 where σ =

[σT

a σT

b σT

c

]T

V = −σT

Ja

Jb

Jc

v = −σTMσ = −σ

T

Λa Oma,mbOma,mc

JbJ†aΛa JbNaJ

†bΛb JbNJ

†cΛc

JcJ†aΛa JcNaJ

†bΛb JcNJ

†cΛc

σ

14[Antonelli(2009)]Gianluca Antonelli Trondheim, 23 april 2014

Page 68: Control problems for floating-base manipulators

Stability results

V < 0 depending on the mutual relationships among the Jacobians:

dependent

ρ(J †x) + ρ(J †

y) > ρ([J†x J†

y

])

independent

ρ(J †x) + ρ(J †

y) = ρ([J†x J†

y

])

orthogonal

JxJ†y = Omx×my

Gianluca Antonelli Trondheim, 23 april 2014

Page 69: Control problems for floating-base manipulators

(Dis)advantages

PROS

Priorities strictly satisfiedAnalitical convergence resultsEasiest gain tuningClear handling of the DOFsCompetitive and cooperative as particular cases

CONS

Couples all the behaviorsImpedance control not possible

Gianluca Antonelli Trondheim, 23 april 2014

Page 70: Control problems for floating-base manipulators

However. . .

End effector going out of the workspace and one (eventually weighted)task always leads to singularity

❅❅❅❘

manipulator stretched

Gianluca Antonelli Trondheim, 23 april 2014

Page 71: Control problems for floating-base manipulators

Balance movement between vehicle and manipulator

Need to distribute the motion e.g.:

move mainly the manipulator when target in workspace

move the vehicle when approaching the workspace boundaries

move the vehicle for large displacement

Some solutions, among them dynamic programming or fuzzy logic

Gianluca Antonelli Trondheim, 23 april 2014

Page 72: Control problems for floating-base manipulators

Fuzzy logic to balance the movement15

Within a weighted pseudoinverse framework

J†W

= W−1JT(JW−1JT

)−1W−1(β) =

[(1− β)I6 O6×n

On×6 βIn

]

with β ∈ [0, 1] output of a fuzzy inference engineSecondary tasks activated by additional fuzzy variables αi ∈ [0, 1]

ζ = J†W

(σa +Λaσa) +(I − J

†W

JW

)(∑

i

αiJ†s,iws,i

)

Only one αi active at onceNeed to be complete, distinguishable, consistent and compactBeyond the dichotomy fuzzy/probability theory very effective intransferring ideas

15[Antonelli and Chiaverini(2003)]Gianluca Antonelli Trondheim, 23 april 2014

Page 73: Control problems for floating-base manipulators

Fuzzy logic to balance the movement15

Within a weighted pseudoinverse framework

J†W

= W−1JT(JW−1JT

)−1W−1(β) =

[(1− β)I6 O6×n

On×6 βIn

]

with β ∈ [0, 1] output of a fuzzy inference engineSecondary tasks activated by additional fuzzy variables αi ∈ [0, 1]

ζ = J†W

(σa +Λaσa) +(I − J

†W

JW

)(∑

i

αiJ†s,iws,i

)

Only one αi active at onceNeed to be complete, distinguishable, consistent and compactBeyond the dichotomy fuzzy/probability theory very effective intransferring ideas

15[Antonelli and Chiaverini(2003)]Gianluca Antonelli Trondheim, 23 april 2014

Page 74: Control problems for floating-base manipulators

Fuzzy logic to balance the movement15

Within a weighted pseudoinverse framework

J†W

= W−1JT(JW−1JT

)−1W−1(β) =

[(1− β)I6 O6×n

On×6 βIn

]

with β ∈ [0, 1] output of a fuzzy inference engineSecondary tasks activated by additional fuzzy variables αi ∈ [0, 1]

ζ = J†W

(σa +Λaσa) +(I − J

†W

JW

)(∑

i

αiJ†s,iws,i

)

Only one αi active at onceNeed to be complete, distinguishable, consistent and compactBeyond the dichotomy fuzzy/probability theory very effective intransferring ideas

15[Antonelli and Chiaverini(2003)]Gianluca Antonelli Trondheim, 23 april 2014

Page 75: Control problems for floating-base manipulators

Dynamic programming to balance the movement16

Freeze, as a free parameter, the vehicle velocity ν and implementthe agility task priority to the sole manipulator ⇒ qd

Freeze the manipulator velocity qd and then find the vehiclevelocity νd needed for the remaining tasks components notsatisfied ⇒ ζd

ν

νe

16[Casalino et al.(2012)Casalino, Zereik, Simetti, Sperinde, and Turetta]Gianluca Antonelli Trondheim, 23 april 2014

Page 76: Control problems for floating-base manipulators

Dynamic programming to balance the movement16

Freeze, as a free parameter, the vehicle velocity ν and implementthe agility task priority to the sole manipulator ⇒ qd

Freeze the manipulator velocity qd and then find the vehiclevelocity νd needed for the remaining tasks components notsatisfied ⇒ ζd

ν

νe

16[Casalino et al.(2012)Casalino, Zereik, Simetti, Sperinde, and Turetta]Gianluca Antonelli Trondheim, 23 april 2014

Page 77: Control problems for floating-base manipulators

Outline

Introduction & variable definition

Inverse Kinematics

A possible kinematic solution: NSB behavioral control

A possible dynamic solution: Virtual decomposition

Simulation/experiments

Perspectives

Gianluca Antonelli Trondheim, 23 april 2014

Page 78: Control problems for floating-base manipulators

Dynamic control

Classical vs modular approaches (both compatible with NSB)

second. tasks

ηd, qd τ η, q

IKmain task

Control

Classical model-based

natural extension ofwell-known control laws

adaptive and robustversions

Virtual decomposition

modular ⇒ same controllaw for all rigid bodies

adaptive ⇒ integral-likeaction

Gianluca Antonelli Trondheim, 23 april 2014

Page 79: Control problems for floating-base manipulators

Dynamic control

Classical vs modular approaches (both compatible with NSB)

second. tasks

ηd, qd τ η, q

IKmain task

Control

Classical model-based

natural extension ofwell-known control laws

adaptive and robustversions

Virtual decomposition

modular ⇒ same controllaw for all rigid bodies

adaptive ⇒ integral-likeaction

Gianluca Antonelli Trondheim, 23 april 2014

Page 80: Control problems for floating-base manipulators

Dynamic control

Classical vs modular approaches (both compatible with NSB)

second. tasks

ηd, qd τ η, q

IKmain task

Control

Classical model-based

natural extension ofwell-known control laws

adaptive and robustversions

Virtual decomposition

modular ⇒ same controllaw for all rigid bodies

adaptive ⇒ integral-likeaction

Gianluca Antonelli Trondheim, 23 april 2014

Page 81: Control problems for floating-base manipulators

Virtual Decomposition in a nutshell

based on Newton-Euler formulation

forward propagation of kinematicerrors assuming 6 DOFs

ν1

ν2

backward computation and projectionof control forces/torques

Gianluca Antonelli Trondheim, 23 april 2014

Page 82: Control problems for floating-base manipulators

VD: forward propagation, from base to e.e.

6-DOF kinematicerrors νi ∈ R

6

each link consideredas a 6 DOFs body

νi

νi+1

qi+1

propagation

forward

νi+1i+1

= U iT

i+1νii + qi+1z

i+1i

Gianluca Antonelli Trondheim, 23 april 2014

Page 83: Control problems for floating-base manipulators

VD: forward propagation, from base to e.e.

6-DOF kinematicerrors νi ∈ R

6

each link consideredas a 6 DOFs body

νi

νi+1

qi+1

propagation

forward

νi+1i+1

= U iT

i+1νii︸ ︷︷ ︸+ qi+1z

i+1i

velocity propagation

Gianluca Antonelli Trondheim, 23 april 2014

Page 84: Control problems for floating-base manipulators

VD: forward propagation, from base to e.e.

6-DOF kinematicerrors νi ∈ R

6

each link consideredas a 6 DOFs body

νi

νi+1

qi+1

propagation

forward

νi+1i+1

= U iT

i+1νii + qi+1z

i+1i︸ ︷︷ ︸

joint contribution

Gianluca Antonelli Trondheim, 23 april 2014

Page 85: Control problems for floating-base manipulators

VD: backward propagation, from e.e. to base

6-DOF generalizedcontrol forceshc ∈ R

6 hi

hi+1τi

propagation

backward

hic,i = Y

(Ri,ν

ii,ν

ir,i, ν

ir,i

)θi +Kv,is

ii +U i

i+1hi+1c,i+1

Gianluca Antonelli Trondheim, 23 april 2014

Page 86: Control problems for floating-base manipulators

VD: backward propagation, from e.e. to base

6-DOF generalizedcontrol forceshc ∈ R

6 hi

hi+1τi

propagation

backward

hic,i = Y

(Ri,ν

ii,ν

ir,i, ν

ir,i

)θi︸ ︷︷ ︸

+Kv,isii +U i

i+1hi+1c,i+1

model-based compensation

Gianluca Antonelli Trondheim, 23 april 2014

Page 87: Control problems for floating-base manipulators

VD: backward propagation, from e.e. to base

6-DOF generalizedcontrol forceshc ∈ R

6 hi

hi+1τi

propagation

backward

hic,i = Y

(Ri,ν

ii,ν

ir,i, ν

ir,i

)θi +Kv,is

ii︸ ︷︷ ︸+U i

i+1hi+1c,i+1

feedback term

Gianluca Antonelli Trondheim, 23 april 2014

Page 88: Control problems for floating-base manipulators

VD: backward propagation, from e.e. to base

6-DOF generalizedcontrol forceshc ∈ R

6 hi

hi+1τi

propagation

backward

hic,i = Y

(Ri,ν

ii,ν

ir,i, ν

ir,i

)θi +Kv,is

ii +U i

i+1hi+1c,i+1︸ ︷︷ ︸

force propagation

Gianluca Antonelli Trondheim, 23 april 2014

Page 89: Control problems for floating-base manipulators

VD: cooperative control

1 Forward propagation until the object

2 Object control forces for the movement+internal

3 Forces projected to the link n of the two robots

4 Backward propagation

hc,o

hc,n+1 hc,n+1

Gianluca Antonelli Trondheim, 23 april 2014

Page 90: Control problems for floating-base manipulators

Outline

Introduction & variable definition

Inverse Kinematics

A possible kinematic solution: NSB behavioral control

A possible dynamic solution: Virtual decomposition

Simulation/experiments

Perspectives

Gianluca Antonelli Trondheim, 23 april 2014

Page 91: Control problems for floating-base manipulators

Numerical simulation: underwater6-DOF vehicle + 6-DOF manipulator

Reach a pre-grasp configuration in terms of end-effector position andorientation

initial configuration

priority-1 task: e.e. configuration

priority-2 task: vehicle roll+pitch

priority-3 task: position of joint 2

only e.e. ⇒

complete solution ⇒

Gianluca Antonelli Trondheim, 23 april 2014

Page 92: Control problems for floating-base manipulators

Numerical simulation: underwater6-DOF vehicle + 7-DOF manipulator

Cameraman action: keep the object in the field of view

initial configuration

priority-1 task: field of view

priority-2 task: vehicle roll+pitch

priority-3 task: mechanical jointlimits

animation ⇒

Gianluca Antonelli Trondheim, 23 april 2014

Page 93: Control problems for floating-base manipulators

Numerical simulation:cooperative transportation of a bar

The final objective for ARCAS

kinematics-only

no perception problems

switch among actions

Gianluca Antonelli Trondheim, 23 april 2014

Page 94: Control problems for floating-base manipulators

First HW tests

Tests made in Seville, November 2013

vehicle controller not implemented

task: e.e. position+orientation

simple e.e. hold

no priority yet

Gianluca Antonelli Trondheim, 23 april 2014

Page 95: Control problems for floating-base manipulators

Additional HW tests

Tests made in Seville, February 2014

vehicle controller to be tuned/improved

vehicle obstacle avoidance

e.e. + arm reconfiguration

Gianluca Antonelli Trondheim, 23 april 2014

Page 96: Control problems for floating-base manipulators

Outline

Introduction & variable definition

Inverse Kinematics

A possible kinematic solution: NSB behavioral control

A possible dynamic solution: Virtual decomposition

Simulation/experiments

Perspectives

Gianluca Antonelli Trondheim, 23 april 2014

Page 97: Control problems for floating-base manipulators

Perspectives: the underactuated case

Motivated by quadrotors control

Roll and pitch functional to the horizontalmovement ⇒ kinematic disturbances!

pitch

end effector

Uncontrolled DOF affect the tasks

Possible to handle in a task-priority approach?

Gianluca Antonelli Trondheim, 23 april 2014

Page 98: Control problems for floating-base manipulators

Perspectives: Null base reaction forces

Dynamic model

[Mv M12

MT12 M q

] [ν

q

]+

[f c,v

f c,q

]+

[fg,v

f g,q

]=

[τ v

τ q

]

for the sole vehicle:

Mvν +M12q + f c,v︸ ︷︷ ︸try to zeroing

+fg,v = τ v,

achievable with:q = −M

†12f c,v + Nqo︸ ︷︷ ︸

yet another null space. . .

Gianluca Antonelli Trondheim, 23 april 2014

Page 99: Control problems for floating-base manipulators

Perspectives: arm’s motors not controllable in torque

Several manipulators on the market equipped with position/velocityfeedback control loopsNeed to design a proper controller for the sole vehicle

Partially coupled model

Iterative formulation

Adaptive

Simplest version: only gravity

Mvν +M 12q + f c,v + f g,v = τ v,

compensate only for f g,v

h00=Y (0)·θ0+U

01

(

Y (1) · θ1 + U12h

22

)

+. . .=[

Y (0) U01Y (1) U

01U

12Y (2) . . . U

01U

12 . . .Un−1

nY (n)

]

︸ ︷︷ ︸

Y

θ0θ1θ2

.

.

.

θn

Gianluca Antonelli Trondheim, 23 april 2014

Page 100: Control problems for floating-base manipulators

Perspectives: 2-arm-equipped vehicle

Proposal to be submitted in few hours to H2020-ICT23

Fada­Catec

Gianluca Antonelli Trondheim, 23 april 2014

Page 101: Control problems for floating-base manipulators

Thanks for the attention

The presented results are the outcome of the work of several

colleagues from the University of Cassino, the Consortium ISME

and PRISMA, the projects ARCAS and MARIS

Filippo Arrichiello, Khelifa Baizid, Elisabetta Cataldi, Stefano Chiaverini,

Amal Meddahi

Gianluca Antonelli Trondheim, 23 april 2014

Page 102: Control problems for floating-base manipulators

Bibliography I

G. Antonelli.

Stability analysis for prioritized closed-loop inverse kinematic algorithms forredundant robotic systems.

IEEE Transactions on Robotics, 25(5):985–994, October 2009.

G. Antonelli.

Underwater robots.

Springer Tracts in Advanced Robotics, Springer-Verlag, Heidelberg, D, 3rdedition, January 2014.

G. Antonelli and S. Chiaverini.

Fuzzy redundancy resolution and motion coordination for underwatervehicle-manipulator systems.

IEEE Transactions on Fuzzy Systems, 11(1):109–120, 2003.

R.C. Arkin.

Motor schema based mobile robot navigation.

The International Journal of Robotics Research, 8(4):92–112, 1989.

Gianluca Antonelli Trondheim, 23 april 2014

Page 103: Control problems for floating-base manipulators

Bibliography II

R.A. Brooks.

A robust layered control system for a mobile robot.

IEEE Journal of Robotics and Automation, 2(1):14–23, 1986.

G. Casalino and A. Turetta.

Coordination and control of multiarm, nonholonomic mobile manipulators.

In Proceedings IEEE/RSJ International Conference on Intelligent Robots andSystems, pages 2203–2210, Las Vegas, NE, Oct. 2003.

G. Casalino, E. Zereik, E. Simetti, S. Torelli A. Sperinde, and A. Turetta.

Agility for underwater floating manipulation: Task & subsystem priority basedcontrol strategy.

In 2012 IEEE/RSJ International Conference on Intelligent Robots andSystems, Vilamoura, PT, october 2012.

Gianluca Antonelli Trondheim, 23 april 2014

Page 104: Control problems for floating-base manipulators

Bibliography III

P. Chiacchio, S. Chiaverini, L. Sciavicco, and B. Siciliano.

Closed-loop inverse kinematics schemes for constrained redundant manipulatorswith task space augmentation and task priority strategy.

The International Journal Robotics Research, 10(4):410–425, 1991.

S. Chiaverini.

Singularity-robust task-priority redundancy resolution for real-time kinematiccontrol of robot manipulators.

IEEE Transactions on Robotics and Automation, 13(3):398–410, 1997.

S. Chiaverini, G. Oriolo, and I. D. Walker.

Springer Handbook of Robotics, chapter Kinematically RedundantManipulators, pages 245–268.

B. Siciliano, O. Khatib, (Eds.), Springer-Verlag, Heidelberg, D, 2008.

Gianluca Antonelli Trondheim, 23 april 2014

Page 105: Control problems for floating-base manipulators

Bibliography IV

T.I. Fossen.

Marine Control Systems: Guidance, Navigation and Control of Ships, Rigs andUnderwater Vehicles.

Marine Cybernetics, Trondheim, Norway, 2002.

J. Han and W.K. Chung.

Coordinated motion control of underwater vehicle-manipulator system withminimizing restoring moments.

In Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ InternationalConference on, pages 3158–3163. IEEE, 2008.

M. Hildebrandt, L. Christensen, J. Kerdels, J. Albiez, and F. Kirchner.

Realtime motion compensation for ROV-based tele-operated underwatermanipulators.

In IEEE OCEANS 2009-Europe, pages 1–6, 2009.

Gianluca Antonelli Trondheim, 23 april 2014

Page 106: Control problems for floating-base manipulators

Bibliography V

J. Kim, G. Marani, WK Chung, and J. Yuh.

Kinematic singularity avoidance for autonomous manipulation in underwater.

Proceedings of PACOMS, 2002.

G. Marani, S.K. Choi, and J. Yuh.

Real-time center of buoyancy identification for optimal hovering in autonomousunderwater intervention.

Intelligent Service Robotics, 3(3):175–182, 2010.

T.W. McLain, S.M. Rock, and M.J. Lee.

Coordinated control of an underwater robotic system.

In Video Proceedings of the 1996 IEEE International Conference on Roboticsand Automation, pages 4606–4613, 1996a.

T.W. McLain, S.M. Rock, and M.J. Lee.

Experiments in the coordinated control of an underwater arm/vehicle system.

Autonomous robots, 3(2):213–232, 1996b.

Gianluca Antonelli Trondheim, 23 april 2014

Page 107: Control problems for floating-base manipulators

Bibliography VI

R. Mebarki and V. Lippiello.

Image-based control for aerial manipulation.

Asian Journal of Control, in press, 2014.

R. Mebarki, V. Lippiello, and B. Siciliano.

Exploiting image moments for aerial manipulation control.

In ASME Dynamic Systems and Control Conference, Palo Alto, CA, USA,2013.

N. Sarkar and T.K. Podder.

Coordinated motion planning and control of autonomous underwatervehicle-manipulator systems subject to drag optimization.

Oceanic Engineering, IEEE Journal of, 26(2):228–239, 2001.

I. Schjølberg and T. Fossen.

Modelling and control of underwater vehicle-manipulator systems.

In in Proc. 3rd Conf. on Marine Craft maneuvering and control, pages 45–57,Southampton, UK, 1994.

Gianluca Antonelli Trondheim, 23 april 2014

Page 108: Control problems for floating-base manipulators

Bibliography VII

B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo.

Robotics: modelling, planning and control.

Springer Verlag, 2009.

Gianluca Antonelli Trondheim, 23 april 2014