Planning Robot Motion Strategies under Geometric Uncertainty Constraints José Najera, Fernando De la Rosa and Christian Laugier LIFIA & INRIA Rhône Alpes 46, Avenue Félix Viallet 38031 Grenoble cedex, France [email protected] Abstract This paper presents an approach to plan robust motion strategies of a robot navigating through an environment composed of polygonal obstacles subject to geometric uncertainty constraints. The contacts of the robot against the obstacles are used to reduce its position and orientation uncertainties and to guide the robot to its goal configuration. We describe a motion planner based on this approach which generates both, free space motions and compliant motions (contact based motions). An explicit geometric model for the uncertainty is used to estimate the reachability of the goal and the intermediate subgoals (these are generated when the final goal is not reachable from the current robot configuration). Two functions are combined to explore the valid space : (1) a contact-based potential field function is used to generate continuous motions pulling the robot towards target attractors; (2) an exploration function is used to determine possible subgoals allowing to progress towards the ultimate goal when local minima of the previous function occur.

1 Introduction Designing a path planner is a central topic in robotics research. The ultimate goal of a path planner is to produce a valid path from an initial to a final configuration. However, the discrepancies between the geometric virtual world where the actions are planned, and the mechanical world where the robot operates, constrain the selection of actions which can be included in a solution path. These differences are called uncertainties and can be the cause of failure of a robot motion plan or a manipulation program. Therefore, the consistency of the trajectories produced by a robot motion planner depends on both, the models used to represent the uncertainty, and the strategies used to deal with it. Many path planners work in a “non-collision” basis, trying to decrease the distance to the goal while pushing the robot away from the obstacles. Following a different approach, the obstacles can be used as a geometric guide to plan contact-based motions which reduce the position uncertainty of the robot. These motions can be later executed as proximity guided motions for a mobile robot (no real contact), or as compliant motions for an arm manipulator (force sensing). Under this approach, a motion program can be seen as a sequence of uncertainty constrained actions moving the robot from one contact situation to the next, until the final configuration is reached. We present an alternative method to plan motion strategies for robotics tasks constrained by uncertainty in position and control. The position uncertainty is implicitly reduced by planning contact-based motions. The control uncertainty is explicitely considered by introducing a geometric model to evaluate the reachability of the obstacles

when the robot translates in free space. The following sections present the formal definition of the motion planning problem and reference some previous work in the area. A geometric construction used to estimate the reachability of the goal and subgoals for the planner is then presented. Next, the adapted potential field function is explained together with the explore function used to determine new sub-goals when local minima situations occur. An example illustrates the application of the planning algorithm. The last section summarizes the approach and suggests future directions of research.

The motion planning problem Let A be a mobile rigid object (a mobile robot or a manipulated object) in the Euclidean workspace W , and B1 ; : : :; Bn, be a set of rigid fixed obstacles in W . The corresponding configuration space is denoted by C = IR2 SO(1), where A has two degrees of freedom in translation and one in rotation. A configuration in C is defined as a tuple q = (x; y; ), where x; y and represent, respectively the translation and orientation of A in C . The set of configurations where A is free in C represents the free space, Cfree . The set of configurations where A touches one or more of the B1 ; : : :; Bn without overlapping them is called the contact space, Ccontact. A configuration space obstacle is denoted by CB j (or C-obstacle). S CThe set, of valid configurations of A, Cvalid = Cfree contact represents the configurations where A does not overlap the obstacles. The geometry of A, and the Bi ’s is accurately known, and the boundaries of CB i are denoted by @ CB i . Then, our motion planning problem can be stated as follows : given an initial and a final configurations of A, q0; qf 2 Cvalid , find a path of connected subpaths 0 which can be specified either, as a sequence of configurations of A in Cfree , or as a sequence of constrained configurations of A in Ccontact, or as a combination of both. The reachability of a goal configuration means that any trajectory resulting from commanding a motion will actually attain the configuration given the position/orientation and the control uncertainties. The recognizability of the goal configuration is the ability to determine the success or failure of a commanded motion. These criteria constrains the choice of possible configurations which can be included in .

Previous Work Uncertainty has always been a central problem in robotics. Different approaches use implicit or explicit representations to estimate its effects, and take preemptive or corrective actions to cope with them. In the assembly planning domain, some techniques have been developed to construct complete fine motion strategies. Some strategies

use the contact space to guide the robot to the final configuration reducing its position uncertainty with the contacts. A general planning framework for the planning problem using an explicit model of uncertainty was introduced by Lozano-Perez, Mason and Taylor, [6]. The main idea is to determine regions known as “preimages” from which the execution of a commanded action is guaranteed to succeed despite the sensing and control uncertainty. The plans are produced by recursively backchaining from the goal. The main drawback of this approach concerns the complexity of the method. Erdmann [2] proposed a way to compute the preimages known as “backprojections.” The regions are computed by erecting constraints that geometrically capture the uncertainty in motion. More recently, Alami and Simeon [1] proposed a planning strategy to produce robust paths for a translating punctual mobile robot using an explicit model of uncertainty in sensing and control. Their strategy uses a set of sensor-based motion primitives to guide the robot based on sensing conditions about contacts with the obstacles. In addition, the problem of error accumulation is considered and solved by placing the robot in specific sensing positions where the uncertainty is minimal. Designing a path planner is the central topic in the motion planning domain. See [4] for a detailed description of the state of the art. Local planning approaches require only a partial knowledge of the configuration space. The decisions to move the robot are taken using local criterions and heuristics to choose the most promising directions. Consequently, these methods are fast but incomplete. It may happen that even when a solution path exists it might not be found. The potential field methods treat the robot as a point in configuration space under the influence of a magnetic field affecting its position [3]. Some local methods consider the planning process as an optimization problem, where finding a path to the goal corresponds to the optimization of some given function [7]. As any optimization method, local methods are subject to fall in local minimums. The Ariadne’s Clew Algorithm [7] allows breaking such situations by exploring the surrounding space progressively trying to determine new subgoal configurations to continue the search of a path to the final configuration.

2 Planning robot motions A robot manipulation program must be sensible to the physical interactions between the manipulated object or a mobile robot (for simplicity it will be refered as the robot) and its environment. When the robot hits an obstacle, some reaction forces partially oppose the direction of the commanded motion. In order to further execute the commanded motion while keeping the contact along the surface, the robot controller has to be able to perform compliant motions. The planner must select those contacts which reduce the uncertainty in the relative positions of the manipulated object, and are sufficiently unobstructing to further guide the robot towards the goal configuration. Our approach is based in an incremental exploration of the surrounding contact space of a particular contact configuration. A contact-based potential field function is used to generate continuous motions for the robot in valid space. Two types of motions are produced by this function: compliant motions to slide the robot along the surfaces of the obstacles, and free space motions to execute jumps between them. When local minima are detected, an ex-

ploration function is applied to place subgoals within the unexplored contact space trying to find a free path to the ultimate goal. The explore function works over two spaces : the adjacent contact space which is composed by the surfaces of the C-obstacles blocking the robot, and the visible contact space composed by the surfaces of the C-obstacles which are visible from the blocked configuration according to the translational freedom of the robot. This section describes the geometric uncertainty model used to determine the reachability of the visible surfaces, together with the potential field and explore functions. The next section explains how these are combined in a planning algorithm illustrated with an example.

Uncertainty The uncertainties in position and control must be taken into account to determine the robustness of every planned motion. Motions through free space are constrained by positioning errors which accumulate due to control errors. As a consequence, the robot may reach or not a target configuration depending on these factors. Initially, there is no explicit representation of the configuration space obstacles, these are progressively constructed as the planner iterates. A simple algorithm [4] is used to calculate the polygons corresponding to the “slices” of the C-obstacles whenever the orientation of the robot changes. While the punctual robot navigates among these polygons trying to reach a target configuration, it might be completely blocked determining a local minimum. This happens when the robot falls into a concavity of a Cobstacle, or when the direction of motion matches the ingoing normal of the C-obstacle surface in the point of contact. In such cases, the translational freedom of the robot determines the visible space for the local minimum configuration q, and all the contact configurations (surfaces of the visible C-obstacles) within this space determine the visible contact space, V CS (q). Analogously, the adjacent contact space, ACS (q), is composed by all contact configurations associated to the C-obstacles blocking the motion of the robot (see figure 1). A strategy to push the

Local minimum

CB2

Visible space

q CB1 ACS(q) VCS(q) Figure 1: A local minimum configuration with its corresponding adjacent and visible contact spaces.

robot away from the local minimum configuration is to move it to another configuration within the adjacent or the visible contact spaces. A subgoal in the adjacent contact space can be reached using compliant motions following the contour of the obstacle(s). The position uncertainty of the robot is completely eliminated when the different edges of the obstacle(s) are recognized. Unfortunately, reaching a subgoal in the visible contact space is more complicated given the position and control uncertainties associated to a free space motion necessary to jump to another obstacle. The choice of possible configurations which can be selected as targets is restricted to those whose “reachability” is guaranteed according to the uncertainty in position and control. A geometric construction refered as the forward projection is used to estimate this reachability. It is applied whenever a jump through free space is necessary to reach other obstacles. Given that the position uncertainty of a

intersect behind. The resulting cone depict the potential velocities that could attain the surface. 3. Narrow each side of the cone of potential velocities with the maximal deviation control error (the angle of the control uncertainty cone). The resulting cone depicts the set of safe velocities which can be commanded to attain the surface. 4. Project the cone of safe velocities from q onto the surface obtaining the reachable area of contact. The purpose of the forward projection is to identify the reachable region of a C-obstacle surface, and the set of velocities necessary to attain it from a nominal configuration of the robot, according to the uncertainty constraints. Using this, the correct execution of jumps between the obstacles is guaranteed.

An adapted potential field function

C−obstacle Reachable Region Safe velocities

ρ

η

η

q

Figure 2: The forward projection is constructed to determine the reachable region of a surface and the set of velocities which can be commanded to achieve it. configuration q is modeled as a disk of radius (this radius is constant for a manipulator arm, for a mobile robot its length is a function of the traversed distance), and a commanded velocity, ~vc 2

1 Introduction Designing a path planner is a central topic in robotics research. The ultimate goal of a path planner is to produce a valid path from an initial to a final configuration. However, the discrepancies between the geometric virtual world where the actions are planned, and the mechanical world where the robot operates, constrain the selection of actions which can be included in a solution path. These differences are called uncertainties and can be the cause of failure of a robot motion plan or a manipulation program. Therefore, the consistency of the trajectories produced by a robot motion planner depends on both, the models used to represent the uncertainty, and the strategies used to deal with it. Many path planners work in a “non-collision” basis, trying to decrease the distance to the goal while pushing the robot away from the obstacles. Following a different approach, the obstacles can be used as a geometric guide to plan contact-based motions which reduce the position uncertainty of the robot. These motions can be later executed as proximity guided motions for a mobile robot (no real contact), or as compliant motions for an arm manipulator (force sensing). Under this approach, a motion program can be seen as a sequence of uncertainty constrained actions moving the robot from one contact situation to the next, until the final configuration is reached. We present an alternative method to plan motion strategies for robotics tasks constrained by uncertainty in position and control. The position uncertainty is implicitly reduced by planning contact-based motions. The control uncertainty is explicitely considered by introducing a geometric model to evaluate the reachability of the obstacles

when the robot translates in free space. The following sections present the formal definition of the motion planning problem and reference some previous work in the area. A geometric construction used to estimate the reachability of the goal and subgoals for the planner is then presented. Next, the adapted potential field function is explained together with the explore function used to determine new sub-goals when local minima situations occur. An example illustrates the application of the planning algorithm. The last section summarizes the approach and suggests future directions of research.

The motion planning problem Let A be a mobile rigid object (a mobile robot or a manipulated object) in the Euclidean workspace W , and B1 ; : : :; Bn, be a set of rigid fixed obstacles in W . The corresponding configuration space is denoted by C = IR2 SO(1), where A has two degrees of freedom in translation and one in rotation. A configuration in C is defined as a tuple q = (x; y; ), where x; y and represent, respectively the translation and orientation of A in C . The set of configurations where A is free in C represents the free space, Cfree . The set of configurations where A touches one or more of the B1 ; : : :; Bn without overlapping them is called the contact space, Ccontact. A configuration space obstacle is denoted by CB j (or C-obstacle). S CThe set, of valid configurations of A, Cvalid = Cfree contact represents the configurations where A does not overlap the obstacles. The geometry of A, and the Bi ’s is accurately known, and the boundaries of CB i are denoted by @ CB i . Then, our motion planning problem can be stated as follows : given an initial and a final configurations of A, q0; qf 2 Cvalid , find a path of connected subpaths 0 which can be specified either, as a sequence of configurations of A in Cfree , or as a sequence of constrained configurations of A in Ccontact, or as a combination of both. The reachability of a goal configuration means that any trajectory resulting from commanding a motion will actually attain the configuration given the position/orientation and the control uncertainties. The recognizability of the goal configuration is the ability to determine the success or failure of a commanded motion. These criteria constrains the choice of possible configurations which can be included in .

Previous Work Uncertainty has always been a central problem in robotics. Different approaches use implicit or explicit representations to estimate its effects, and take preemptive or corrective actions to cope with them. In the assembly planning domain, some techniques have been developed to construct complete fine motion strategies. Some strategies

use the contact space to guide the robot to the final configuration reducing its position uncertainty with the contacts. A general planning framework for the planning problem using an explicit model of uncertainty was introduced by Lozano-Perez, Mason and Taylor, [6]. The main idea is to determine regions known as “preimages” from which the execution of a commanded action is guaranteed to succeed despite the sensing and control uncertainty. The plans are produced by recursively backchaining from the goal. The main drawback of this approach concerns the complexity of the method. Erdmann [2] proposed a way to compute the preimages known as “backprojections.” The regions are computed by erecting constraints that geometrically capture the uncertainty in motion. More recently, Alami and Simeon [1] proposed a planning strategy to produce robust paths for a translating punctual mobile robot using an explicit model of uncertainty in sensing and control. Their strategy uses a set of sensor-based motion primitives to guide the robot based on sensing conditions about contacts with the obstacles. In addition, the problem of error accumulation is considered and solved by placing the robot in specific sensing positions where the uncertainty is minimal. Designing a path planner is the central topic in the motion planning domain. See [4] for a detailed description of the state of the art. Local planning approaches require only a partial knowledge of the configuration space. The decisions to move the robot are taken using local criterions and heuristics to choose the most promising directions. Consequently, these methods are fast but incomplete. It may happen that even when a solution path exists it might not be found. The potential field methods treat the robot as a point in configuration space under the influence of a magnetic field affecting its position [3]. Some local methods consider the planning process as an optimization problem, where finding a path to the goal corresponds to the optimization of some given function [7]. As any optimization method, local methods are subject to fall in local minimums. The Ariadne’s Clew Algorithm [7] allows breaking such situations by exploring the surrounding space progressively trying to determine new subgoal configurations to continue the search of a path to the final configuration.

2 Planning robot motions A robot manipulation program must be sensible to the physical interactions between the manipulated object or a mobile robot (for simplicity it will be refered as the robot) and its environment. When the robot hits an obstacle, some reaction forces partially oppose the direction of the commanded motion. In order to further execute the commanded motion while keeping the contact along the surface, the robot controller has to be able to perform compliant motions. The planner must select those contacts which reduce the uncertainty in the relative positions of the manipulated object, and are sufficiently unobstructing to further guide the robot towards the goal configuration. Our approach is based in an incremental exploration of the surrounding contact space of a particular contact configuration. A contact-based potential field function is used to generate continuous motions for the robot in valid space. Two types of motions are produced by this function: compliant motions to slide the robot along the surfaces of the obstacles, and free space motions to execute jumps between them. When local minima are detected, an ex-

ploration function is applied to place subgoals within the unexplored contact space trying to find a free path to the ultimate goal. The explore function works over two spaces : the adjacent contact space which is composed by the surfaces of the C-obstacles blocking the robot, and the visible contact space composed by the surfaces of the C-obstacles which are visible from the blocked configuration according to the translational freedom of the robot. This section describes the geometric uncertainty model used to determine the reachability of the visible surfaces, together with the potential field and explore functions. The next section explains how these are combined in a planning algorithm illustrated with an example.

Uncertainty The uncertainties in position and control must be taken into account to determine the robustness of every planned motion. Motions through free space are constrained by positioning errors which accumulate due to control errors. As a consequence, the robot may reach or not a target configuration depending on these factors. Initially, there is no explicit representation of the configuration space obstacles, these are progressively constructed as the planner iterates. A simple algorithm [4] is used to calculate the polygons corresponding to the “slices” of the C-obstacles whenever the orientation of the robot changes. While the punctual robot navigates among these polygons trying to reach a target configuration, it might be completely blocked determining a local minimum. This happens when the robot falls into a concavity of a Cobstacle, or when the direction of motion matches the ingoing normal of the C-obstacle surface in the point of contact. In such cases, the translational freedom of the robot determines the visible space for the local minimum configuration q, and all the contact configurations (surfaces of the visible C-obstacles) within this space determine the visible contact space, V CS (q). Analogously, the adjacent contact space, ACS (q), is composed by all contact configurations associated to the C-obstacles blocking the motion of the robot (see figure 1). A strategy to push the

Local minimum

CB2

Visible space

q CB1 ACS(q) VCS(q) Figure 1: A local minimum configuration with its corresponding adjacent and visible contact spaces.

robot away from the local minimum configuration is to move it to another configuration within the adjacent or the visible contact spaces. A subgoal in the adjacent contact space can be reached using compliant motions following the contour of the obstacle(s). The position uncertainty of the robot is completely eliminated when the different edges of the obstacle(s) are recognized. Unfortunately, reaching a subgoal in the visible contact space is more complicated given the position and control uncertainties associated to a free space motion necessary to jump to another obstacle. The choice of possible configurations which can be selected as targets is restricted to those whose “reachability” is guaranteed according to the uncertainty in position and control. A geometric construction refered as the forward projection is used to estimate this reachability. It is applied whenever a jump through free space is necessary to reach other obstacles. Given that the position uncertainty of a

intersect behind. The resulting cone depict the potential velocities that could attain the surface. 3. Narrow each side of the cone of potential velocities with the maximal deviation control error (the angle of the control uncertainty cone). The resulting cone depicts the set of safe velocities which can be commanded to attain the surface. 4. Project the cone of safe velocities from q onto the surface obtaining the reachable area of contact. The purpose of the forward projection is to identify the reachable region of a C-obstacle surface, and the set of velocities necessary to attain it from a nominal configuration of the robot, according to the uncertainty constraints. Using this, the correct execution of jumps between the obstacles is guaranteed.

An adapted potential field function

C−obstacle Reachable Region Safe velocities

ρ

η

η

q

Figure 2: The forward projection is constructed to determine the reachable region of a surface and the set of velocities which can be commanded to achieve it. configuration q is modeled as a disk of radius (this radius is constant for a manipulator arm, for a mobile robot its length is a function of the traversed distance), and a commanded velocity, ~vc 2