Hybrid Controls for Integrated Planning and Control | Projects

Hybrid Controls for Integrated Planning and Control

Research Description:

This work considers the construction of control policies that address the problem of simultaneously planning and controlling the motion of a convex-bodied wheeled mobile robot in a cluttered planar environment. This navigation problem is challenging because of the relationships among control, nonholonomic constraints, and obstacle avoidance. The objective is to move the robot through its environment such that it arrives at a designated goal set without coming into contact with any obstacle, while respecting the nonholonomic constraints and velocity (input) bounds inherent in the system.

Conventional approaches addressing this problem typically decouple navigation and control. First, a planner finds a path, and then a feedback control strategy attempts to follow that path. Often the methods assume a point or circular robot body; other non-trivial robot body shapes complicate the problem of finding a safe path, as the path must be planned in the free configuration space of the robot. This leaves the challenging problem of designing a control law that converges to the one-dimensional path, while remaining safe with respect to obstacles. This decoupled approach leads to a loss of robustness that may necessitate replanning during operation, or possibly lead to collision as the robot tries to converge to a safe path. If obstacle avoidance is layered onto the control law, it may invalidate the convergence guarantees of the control law.

We address the coupled navigation and control problem by generating a vector field along which the robot can ``flow.'' Unfortunately, determining a global vector field that satisfies all of these objectives for constrained systems can be quite difficult. Our approach, inspired by the idea of sequential composition, uses a collection of local feedback control policies coupled with a switching strategy to generate the vector field. The conceptual idea is shown below.

Four different initial conditions converge to the overall goal specified by the composition of local feedback control policies. The plot shows actual robot data.

Instead of attempting to design a complicated global control-law, much simpler control policies are defined over local regions. Composing these relatively simple policies induces a piecewise continuous vector field over the union of the policy domains. A policy is specified by a configuration dependent vector field defined over a local region of the robot's free configuration space, termed a cell. From this vector field and knowledge of the current robot state, the control inputs are determined such that the closed-loop dynamics flows to the specified policy goal set within the cell.

"Complicated" potential based control policy.

Composition of simple control policies

A discrete transition relation, which can be represented by a graph, is induced by the transition between the domain of one policy to the domain of a second policy containing the goal set of the first policy. On-line planning, and re-planning under changing conditions, becomes more tractable on the graph, allowing us to bring numerous discrete planning tools to bear on this essentially continuous problem. By sequencing the local policies according to the ordering determined by a discrete planner, the closed loop dynamics induce the discrete transitions desired by the discrete plan. The overall hybrid (switched) control policy responds to system perturbations without the need for re-planning. In the face of changing environmental conditions, the discrete graph allows for fast online re-planning, while continuing to respect the system constraints.

Policy schematic with goal set to right

Induced transition graph

By coupling planning and control in this way, the hybrid control system plans in the discrete space of control policies; thus, the
approach represents a departure from conventional techniques. A ``thin'' path or trajectory is never explicitly planned; instead, the
trajectory induced by the closed-loop dynamics flows along the vector field defined by the active policy, which is chosen according to an
ordering determined by a discrete planner. A plan over the discrete graph associated with the collection of policies corresponds to a
``thick'' set of configurations within the domains of the associated local policies.

Simple Systems:

Our early work focused on simple systems where the robot was considered a fully actuated point in its configuration space. Two basic policy types were developed over convex polytopes. The first type of policy generated a vector field whose integral curves flowed through a designated goal set, as shown in the left-hand figure below. The second policy type used a vector field that converged to a single point contained in the interior. For kinematic systems, the control law simply followed the vector field. For second-order systems, the control law converged to the vector field flow in a way that guaranteed the system did not exit the cell except via the designated goal set. By properly scaling the vector field, the control policies could be applied to systems with velocity and/or acceleration constraints.

Given a convex decomposition of the free configuration space, these two types of policies can be composed to generate a globally convergent switched control policy as shown on the right-hand figure below.

Vector field defined over convex polytope

Globally convergent policy showing integral curves

By allowing overlapping policy cells, the global policy domain in state space can be increased for second-order systems. While a small region may not be able to steer a high speed initial condition, a larger region may be able to capture and steer the initial condition. Consider the cases shown in the three figures below. The same policy deployment and ordering is used for three different initial conditions. In the left-hand figure, the system starts from rest and switches through three policies before converging to the goal. For the middle figure, the system starts with a non-zero velocity that requires a different initial policy; this results in a different induced path. Finally, the right-hand figure starts with high initial speed. The policy defined over the large rectangle to the left of the central obstacle applies braking and steering to capture the system and bring it into the domain of another policy. At this point, the system takes the path around the central obstacle. This policy-based switching is based entirely on the state of the system, and the ordering of the policies. The system is reactive and does not require re-planning.

Path starting from rest

Path with medium initial velocity

Path with large initial velocity

In these plots, the large red dots indicate a dedicated braking control law, the blue dots indicate the basic control that steers to a specific facet of the local convex region. The green dots indicate that a convergent control policy is active.

Nonholonomic Convex-bodied Systems:

Our current work seeks to extend the results to wheeled mobile robots, where the wheel-to-ground contact induces nonholonomic velocity constraints on the system. Realistic wheeled mobile robots also have constraints on speed and acceleration that must be considered by the control policies. Finally, many robots are non-circular; therefore, the control policies must consider the robot shape and orientation in cluttered environments.

Non-circular robot used in experiments

As in the simple case, the local feedback control policies are defined over local regions of the robot configuration space, which we term cells. Each cell has a designated goal set; the cells are deployed such that the goal set of one policy is contained in the interior of another cell. Over each cell, the local control policy induces a vector field whose flow leads to the designated goal set. In order to realize the sequential composition approach for these nonholonomic systems with multiple constraints, each policy must satisfy four requirements.

First, the cell must have simple representations that allow fast inclusion tests. That is, the test to see if a particular policy is valid must be fast.

Second, the cell must be contained in the free configuration space. This can be tested by expanding the cell to account for the extent of the robot body at each orientation on the cell boundary, and projecting this expanded cell into the workspace. The projected set, which represents the swept volume of the robot body over the configurations in the cell, is then tested for intersection with a workspace obstacle. We have developed a non-conservative analytic mapping for determining the expanded cell given a piecewise analytic cell boundary and analytic definition of the robot body; see CMU-RI-TR-06-34 for more details.

Funnel shaped cell - goal set to right

Expanded cell accounts for body extent

Projection of expanded cell into workspace

Third, given the input constraints and nonholonomic velocity constraints, it must be possible to generate a velocity that keeps the system within the cell on the cell boundary, with the exception of points contained in the goal set. This guarantees that the control system can keep the robot configuration within the cell. Coupled with the free space requirement in the second condition, this guarantees that the policy can be executed in a way that avoids collision.

Finally, the control policy must guarantee convergence to some configuration in the designated goal set in finite time for any point on the cell interior. This requirement guarantees that the ordered policies will transition to a higher priority policy, and thus eventually reach the overall goal.

This research has developed policies that satisfy these four requirements, enabling the implementation of the approach on the robot pictured above. The policies have been deployed in the robot environment shown below, and the discrete policy graph that represents transitions between policies specified. A partial deployment of the policies is shown below.

Partial deployment of cells in workspace

3D view of partial deployment

Given the policy graph encoding the discrete transition relations among policies, a discrete planner determines an ordering of the policies given an overall goal. Given an initial condition for the robot, the control system searches for the highest priority policy that contains the initial condition. From then on, the policies are executed according to the specified ordering. An explicit path is never planned; the resulting closed loop dynamics induce a path that satisfies the system constraints and reaches the overall goal. Results for four initial conditions, executed on the non-circular laboratory mobile robot, are shown at the top of this page. Two additional experimental runs are shown below.

Policy composition can generate complex behaviors such as this 3-point turn

During execution the policies passing through the red circle were invalidated; the system quickly reorders the policies, which induces a new route.

The policies and policy graph are also amenable to use with discrete planners using temporal logic to specify multiple goals, as detailed in CMU-RI-TR-06-34.

Personnel:

David C. Conner
Howie Choset
Alfred A. Rizzi

Publications:

Valet Parking Without a Valet (IROS '07) (finalist for best paper)

Integrated Planning and Control for Convex-bodied Nonholonomic Systems using Local Feedback Control Policies (RSS '06)

Integrated Planning and Control for Convex-bodied Nonholonomic Systems using Local Feedback Control Policies (CMU-RI-TR-06-34)

Towards Provable Navigation and Control of Nonholonomically Constrained Convex-bodied Systems (ICRA '06 poster)

Construction and Automated Deployment of Local Potential Functions for Global Robot Control and Navigation (CMU-RI-TR-03-22)

Composition of Local Potential Functions for Global Robot Control and Navigation (IROS '03)