Upload
kenneth-hodges
View
219
Download
4
Tags:
Embed Size (px)
Citation preview
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
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
4
classification
holonomic MM
nonholonomic MM
statically redundant MM
kinematically redundant MM
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
5
example 1: omnidirectional platform + RPP arm
holonomic
redundant
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
6
example 2: unicycle + RPP arm
nonholonomic
redundant
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
7
example 3: unicycle + gripper
nonholonomic
non-redundant
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
10
NMMs: basic planning and control problems/2
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
11
NMMs: basic planning and control problems/3
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)
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
14
the manipulator is unconstrained, i.e.,
the NMM differential kinematics is then
kinematic modeling
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
17
Projected Gradient (PG)
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
18
Projected Gradient (PG)
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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)
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
41
planning experiments
pseudovelocity generation (RRT LIKE)
completely random optimization of
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
42
starting from a low-compatibility configuration
optimization of optimization of
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
44
manipulability optimization
QuickTime™ and aCinepak decompressor
are needed to see this picture.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
45
room crossing
QuickTime™ and aCinepak decompressor
are needed to see this picture.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
46
narrow passage (1)
QuickTime™ and aCinepak decompressor
are needed to see this picture.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
47
narrow passage (2)
QuickTime™ and aBMP decompressor
are needed to see this picture.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
48
self-motion
QuickTime™ and aCinepak decompressor
are needed to see this picture.
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
49
NMMs: basic planning and control problems/1
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
50
NMMs: basic planning and control problems/2
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
51
NMMs: basic planning and control problems/3
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)
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators
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.