Control problems for floating-base manipulators

Preview:

Citation preview

Control problems for floating-base manipulators

Gianluca Antonelli

Universita di Cassino e del Lazio Meridionale

antonelli@unicas.it

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

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

Trondheim, 23 April 2014

Gianluca Antonelli Trondheim, 23 april 2014

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

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

Cooperation

RedSea project

RedSea project

AMADEUS

ARCAS

Gianluca Antonelli Trondheim, 23 april 2014

Aerial Robotics Cooperative Assembly

Gianluca Antonelli Trondheim, 23 april 2014

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

Marine Autonomous Robotics for InterventionS

Gianluca Antonelli Trondheim, 23 april 2014

Marine Autonomous Robotics for InterventionS

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

Gianluca Antonelli Trondheim, 23 april 2014

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Handling several tasks -6-

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

btw. . . mood ?

Gianluca Antonelli Trondheim, 23 april 2014

But. . .

What are these tasks we are talking about ?

Gianluca Antonelli Trondheim, 23 april 2014

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

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

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

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

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

Tasks to be controlled

Obstacle avoidance m = 1

Gianluca Antonelli Trondheim, 23 april 2014

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

From behaviors to actions

sensing/perception

elementary behaviors actions

commands

supervisor

Gianluca Antonelli Trondheim, 23 april 2014

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

Simple comparison: competitive approach

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: competitive approach

❆❆❆❆❆❯

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: competitive approach

only obstacle avoidance

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: competitive approach

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: cooperative approach

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: cooperative approach

❆❆❆❆❯

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: cooperative approach

❇❇❇❇◆

linear combination: higher task is corrupted

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: cooperative approach

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: NSB

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: NSB

❆❆❆❆❯

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: NSB

❇❇❇◆

null­space­projection: higher task is fulfilled

Gianluca Antonelli Trondheim, 23 april 2014

Simple comparison: NSB

only move­to­goal

Gianluca Antonelli Trondheim, 23 april 2014

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

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

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

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

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

(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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Perspectives: 2-arm-equipped vehicle

Proposal to be submitted in few hours to H2020-ICT23

Fada­Catec

Gianluca Antonelli Trondheim, 23 april 2014

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

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

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

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

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

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

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

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