Motion Planning and Control of Mobile Manipulators Marilena Vendittelli Dipartimento di Informatica...

Motion Planning and Control

of Mobile Manipulators

Marilena Vendittelli

Dipartimento di Informatica e Sistemistica

Università di Roma “La Sapienza”

Roma, Italy

Rescue Robotics Camp 2006


M. Vendittelli: Motion planning and control of mobile manipulators

2

a mobile manipulator

merges dexterity of a standard manipulator increased (~ infinite) workspace capabilities of a mobile platform



3

notation

robot configuration vector

(platform + manipulator)

robot forward kinematics

platform velocity inputs

manipulator velocity inputs

dimension of the robot configuration space

number of degrees of freedom (number of velocity inputs)

dimension of the task



4

classification

holonomic MM

nonholonomic MM

statically redundant MM

kinematically redundant MM



5

example 1: omnidirectional platform + RPP arm

holonomic

redundant



6

example 2: unicycle + RPP arm

nonholonomic

redundant



7

example 3: unicycle + gripper

nonholonomic

non-redundant



8

nonholonomic MMs (NMMs) include wheeled mobile platforms allowing for precise locomotion mechanisms with a reduced number of actuators

the End-Effector (EE) does not always inherit the nonholonomic constraint of the base

redundancy “frees” from nonholonomy, helps to stay away from singularities and allows obstacle avoidance



9

NMMs: basic planning and control problems/1

configuration-level kinematic inversion given an EE pose, generate robot configurations which realize it

standard (includes static redundancy resolution)

velocity-level kinematic inversion

given an EE trajectory, generate velocity commands which allow to track the EE trajectory when starting on it

solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward

path planning (w/o obstacles) conf-to-conf: plan a feasible path for the NH platform (NHPP) and any path for the

manipulator EE pose to EE pose: 2 approaches

generate an EE trajectory joining the two poses and then (velocity-level) kinematic inversion

generate two configurations corresponding to the two poses by (configuration-level) kinematic inversion and then the problem is conf-to-conf



10


motion planning (with obstacles)

conf-to-conf: conventional randomized techniques accounting for the nonholonomy of the platform

EE pose to EE pose: 2 approaches generate a collision-free EE trajectory joining the two poses (easy because

formulated in the operational space where the obstacles are defined) and then motion planning on a given EE path (weak solution because it uselessly constrains the search)

generate two confs corresponding to the two poses by (conf-level) kinematic inversion and then the problem is conf-to-conf (also constrains the search but in a less severe way)

on a given EE path: special randomized technique where generated configurations are constrained to belong to self-motion manifolds



11


kinematic control given an EE trajectory, generate velocity commands which allow to track the EE

trajectory also in the presence of initial displacements

solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward plus a feedback correction term (recovers from initial errors and is also robust wrt linearization errors)

dynamic control

configuration space trajectory: use the NMM dynamic model (standard - but includes platform/manipulator interactions plus the nonholonolomy of the platform), use feedback linearization to reduce to second-order NMM kinematic model (platform: kinematic model + dynamic extension; manipulator: double integrators); then backstepping can be used to transform a kinematic to a dynamic controller

EE trajectory: as above but use operational space formulation (includes inversion of the NMM jacobian)



12

what’s in this talk

derivation of a simple and general model for robotic systems made of a nonholonomic mobile platform carrying a manipulator by combining the manipulator differential kinematics with the admissible differential motion of the platform

extension of classical redundancy resolution schemes, originally developed for standard manipulators, to kinematically redundant NMMs

path planning and kinematic control of NMMs without obstacles

motion planning along given end effector paths for NMMs in environments cluttered with obstacles



13

the platform kinematic model is given by the driftless system , where the columns of the matrix span the platform admissible velocity space at each configuration

the unicycle example

generalized coordinates nonholonomic constraint

the kinematic model

with = driving, = steering velocity inputs

kinematic modeling



14

the manipulator is unconstrained, i.e.,

the NMM differential kinematics is then

kinematic modeling



15

redundancy resolution

the two concepts of

static redundancy, i.e.,

kinematic redundancy, i.e.,

are relevant for a NMM (they collapse for holonomic MMs)

when the objective is velocity-level kinematic inversion (i.e., generating velocity commands that guarantee the execution of a given task), kinematic redundancy definition is used



16

Projected Gradient (PG)

for a given , all inverse kinematic solutions can be expressed as , where is usually chosen so as to locally optimize a given criterion

for fixed-base redundant manipulators: assuming that the manipulator has N joints, the mechanical joint limits

may, for example, be avoided by minimizing the cost function

where is the available range for joint and is

its midpoint



17


for NMMs: the analysis of the time-derivative of

shows that the command which realizes the maximum local improvement of

is

the generalized velocity of the platform is then



18


tracking tasks for planning, for kinematic

control, with a diagonal matrix

regulation tasks

in both cases, the closed-loop system is described by

yielding a linear, decoupled, and exponentially stable behavior for each error component of the task



19

Reduced Gradient (RG)

an alternative, computationally lighter strategy is to directly use commands for task execution and to perform optimization w.r.t. the reduced set of “extra” commands

if the robot Jacobian matrix is full rank, it is always possible to find a permutation matrix such that with nonsingular

this induce a reordering of the velocity input vector

with and

the differential kinematics becomes



20

Reduced Gradient (RG)

the choice satisfies the kinematic task, while

realizes the maximum local improvement of

[note: is a reduction of to the subspace of commands that automatically satisfy the task constraint]

when and the manipulator is not in a singular configuration

one can choose and so that



21

example 1: unicycle + 2R planar manipulator

5 generalized coordinates

4 velocity commands

the 2x4 Jacobian of the EE planar positioning task is never singular if (base offset)



22

trajectory tracking + planar pointing

the end-effector must follow a given circular trajectory while the first link points towards a fixed target point

the single degree of redundancy left is used to maximize the manipulability measure



23

trajectory tracking + planar pointing

RG inversion scheme

Platform center trajectory

EE reference trajectory

First link pointing direction

QuickTime™ and aMicrosoft Video 1 decompressorare needed to see this picture.



24

comparison of performance maximizing manipulability

0 5 10 15 20 250

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

time [s]

Manipulability

RGPG



25

example 2: unicycle + 3R spatial robot

6 generalized coordinates

5 velocity commands

the 3x5 Jacobian of the EE spatial positioning task is never singular if and the manipulator arm is not stretched or folded along the vertical direction



26

trajectory tracking + spatial pointing

a task involving spatial tracking and pointing:

the end-effector must follow a circular trajectory

the two degrees of redundancy are used to force the third link to point towards a fixed point , by minimizing a suitable



27

trajectory tracking + spatial pointing

RG inversion schemeTarget EE ref. trajectory

“EE point of view” QuickTime™ and aMicrosoft Video 1 decompressorare needed to see this picture.



28

comparison of performance minimizing the pointing error norm

0 5 10 15 20 25 30 350

0.005

0.01

0.015

0.02

0.025

0.03

0.035

0.04

time [s]

H(q)

RGPG



29

Motion Planning for MM along Given End-effector Paths(the MPEP problem)

objective

in a known environment, plan collision-free motions for a kinematically

redundant mobile manipulator whose end-effector path is assigned



30

motivation

in many applications the end-effector must trace a given path while the manipulator moves among obstacles

e.g., camera inspection, object transfer,. . .

it is necessary to generate joint motions that keep the end-effector on the path while avoiding collisions between the robot bodies and the obstacles (MPEP: Motion Planning along End-effector Paths)

kinematic redundancy should help in avoiding obstacles, but existing solutions based on optimal redundancy resolution are unsatisfactory

a promising approach is based on a randomized motion planner which takes into account the nonholonomy of the mobile base and the natural partition of dof’s



31

the MPEP problem

given the end-effector path ,

find a path , in configuration space, such that

and no collision occurs between the robot body and the obstacles the path is feasible w.r.t. the nonholonomic constraints manipulator joint limits and self-collisions are avoided as well

a solution may exist or not depending on the connectivity of the free configuration space region that is compatible with the given end-effector constraint

the initial configuration q(0) may or may not be assigned



32

we seek a solution in the form of a sequence of collision-free configurations

a continuous path in configuration space is derived from a solution by interpolating the sequence by local paths

for the mobile platform, local feasible paths are automatically produced by the planners

for the manipulator, simple linear interpolation is used

End-effector tracking accuracy may be improved by increasing the path sampling parameter s

in any case, interpolation schemes accounting for the end-effector path constraint may be easily designed, e.g., based on pseudoinverse control



33

generation of random configurations

ideagenerate random collision-free samples of self-motion manifolds

tools1. random generation of a sample of Mi:

• q ! (qb, qr), with qb 2 IRm, qr 2 IRn−m (base and redundant variables)

2. collision checking on (qb, qr) as well as along the local connecting path

• random generation of qr, inverse kinematics computation of



34

it depends on the particular mobile manipulator; for illustration, consider a

unicycle, controlled by pseudovelocities v, , carrying a spatial 3R arm

how do we choose the base and the redundant variables?

here

chose : redundant variables and : base variables



35

generation of a random sample of Mi , the self-motion manifold corresponding to

with

(optional) biases the generated distribution:

is used as a starting point for the platform

(limited joint displacement) is enforced by INV KIN

this allows the incremental construction of a solution:

when a belonging to Mi is available, it can be used asto generate a belonging to



36

generation of random pseudovelocities

admissible pseudovelocity set

1. completely random pseudovelocities: , are generated according to a uniform probability distribution in

2. constant-energy pseudovelocities: , must satisfy v2 +c !2 = 2 : energy level

3. optimal pseudovelocities: , are chosen in a finite set of candidates (e.g., random forward-right, forward-left, backward-right, backward-left motions) so as to optimize a criterion index:

Idist = kqdes−qnewk: the distance between the new configuration and some desired configuration Icomp: the task compatibility of , which quantifies the capability of moving along the given end-effector path starting from



37

greedy planner

a depth-first search starting from the initial end-effector pose

1. given , an initial collision-free configuration is randomly generated on the self-motion manifold

2. starting from , a sequence of random collision-free configurations . on the self-motion manifolds M1,M2,M3, . . . is generated; each is biased by the previous to enforce the incremental buildup of a feasible path

3. if the last self-motion manifold Ms is not reached, a new is generated and the process restarted

very effective in dealing with easy problems must generate a new to backtrack may prove inefficient in complex problems



38

RRT-like planners

generate multiple samples for each self-motion manifold by adapting the

concept of Rapidly-exploring Random Tree (RRT)

the extension procedure takes place in the platform configuration space;the integer associated to each node identifies the self-motion manifold towhich the node belongs



39

basic RRT-like

1. given , an initial configuration is randomly generated on the

selfmotion manifold

2. the tree rooted at is expanded by

(a) generating a random configuration

(b) identifying in the node whose is closest to , and the associated end-effector pose

(c) computing a new node as follows:

• generate random pseudovelocities , and apply them from to

obtain the corresponding

• generate from by kinematic inversion of (with

as bias)

3. if a node belonging to the final self-motion manifold of has not been

generated after a maximum number of expansion steps, a new is randomly generated and

the process is restarted to retain a RRT-like behavior (tree expands toward ), one may

generate pseudovelocities which optimize , with



40

variations

the breadth-first nature of RRT LIKE can be modified

RRT GREEDY alternates depth-first phases (as in GREEDY) with RRT LIKE phases

RRT BIDIR expands two trees, one rooted at the start and the other at the goal self-motion manifolds; when the trees meet on an intermediate manifold, a self-motion is generated to join them



41

planning experiments

pseudovelocity generation (RRT LIKE)

completely random optimization of



42

starting from a low-compatibility configuration

optimization of optimization of



43

on the average

GREEDY performs effectively in relatively simple problems

RRT LIKE and RRT GREEDY become much more effective in complex planning instances

RRT BIDIR is convenient when the search space is large and there is enough room for reconfiguration



44

manipulability optimization

QuickTime™ and aCinepak decompressor

are needed to see this picture.



45

room crossing





46

narrow passage (1)





47

narrow passage (2)

QuickTime™ and aBMP decompressor




48

self-motion





49


configuration-level kinematic inversion standard (includes static redundancy resolution)

velocity-level kinematic inversion

given an EE trajectory, generate velocity commands which allow to track the EE trajectory when starting on it

solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward

path planning (w/o obstacles) conf-to-conf: plan a feasible path for the NH platform (NHPP) and any path for the

manipulator EE pose to EE pose: 2 approaches

generate an EE trajectory joining the two poses and then (velocity-level) kinematic inversion

generate two configurations corresponding to the two poses by (configuration-level) kinematic inversion and then the problem is conf-to-conf



50


motion planning (with obstacles)

conf-to-conf: conventional randomized techniques accounting for the nonholonomy of the platform

EE pose to EE pose: 2 approaches generate a collision-free EE trajectory joining the two poses (easy because

formulated in the operational space where the obstacles are defined) and then motion planning on a given EE path (weak solution because it uselessly constrains the search)

generate two confs corresponding to the two poses by (conf-level) kinematic inversion and then the problem is conf-to-conf (also constrains the search but in a less severe way)

on a given EE path: special randomized technique where generated configurations are constrained to belong to self-motion manifolds



51


kinematic control given an EE trajectory, generate velocity commands which allow to track the EE

trajectory also in the presence of initial displacements

solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward plus a feedback correction term (recovers from initial errors and is also robust wrt linearization errors)

dynamic control

configuration space trajectory: use the NMM dynamic model (standard - but includes platform/manipulator interactions plus the nonholonolomy of the platform), use feedback linearization to reduce to second-order NMM kinematic model (platform: kinematic model + dynamic extension; manipulator: double integrators); then backstepping can be used to transform a kinematic to a dynamic controller

EE trajectory: as above but use operational space formulation (includes inversion of the NMM jacobian)



52

bibliography

L. Sciavicco and B. Siciliano, Modelling and Control of Robot Manipulators, Springer, 2000. Y. Nakamura, Advanced Robotics: Redundancy and Optimization, Addison-Wesley, 1991. H.Seraji,``A unified approach to motion control of mobile manipulators,'’ Int. J. of Robotics Research, vol. 17, no.

2, pp. 107-118, 1998. Y. Yamamoto and X. Yun, “Unified analysis on mobility and manipulability of mobile manipulators,” in 1999 IEEE

Int. Conf. on Robotics and Automation, 1999, pp. 1200–1206. F. G. Pin, K. A. Morgansen, F. A. Tulloch, C. J. Hacker, and K. B. Gower, ``Motion planning for mobile

manipulators with a non-holonomic constraint using the FSP method,'’ J. of Robotic Systems, vol. 13, no. 11, pp. 723--736, 1996.

J.-Y. Fourquet, B. Bayle, and M. Renaud, “Manipulability of wheeled mobile manipulators: Application to motion generation,” Int. J. Of Robotics Research, vol. 22, no. 7-8, pp. 565–581, 2003.

A. De Luca, G. Oriolo, P. Robuffo Giordano, "Kinematic modeling and redundancy resolution for nonholonomic mobile manipulators," 2006 IEEE International Conference on Robotics and Automation, Orlando, FL, 2006.

S. M. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” Technical Report No. 98-11, Computer Science Dept., Iowa State University., 1998.

G. Oriolo, C. Mongillo, "Motion planning for mobile manipulators along given end-effector paths," 2005 IEEE International Conference on Robotics and Automation, Barcelona, SP, pp. 2166-2172, 2005.

G. Oriolo, M. Vendittelli, L. Freda, and G. Troso, “The srt method: Randomized strategies for exploration,” 2004 IEEE Int. Conf. On Robotics and Automation, pp. 4688–4694, 2004.

Motion Planning and Control of Mobile Manipulators Marilena Vendittelli Dipartimento di Informatica...

Documents

Transcript of Motion Planning and Control of Mobile Manipulators Marilena Vendittelli Dipartimento di Informatica...