Investigators: Shangming Wei and Dr. Miloš Žefran

The studied robotic system consists of a single autonomous vehicle
or a group of autonomous vehicles. Each vehicle has its own predefined initial
state and final state. The vehicles are powered by batteries. The environment
in which the vehicles are operating is a 2-dimensional manifold. There are a
number of static obstacles in the environment. The shape of the obstacles is
rectangle. The autonomous vehicles know their current states and the positions
of the obstacles. The problem of this research project is to find the optimal
path between two states for every vehicle. The path is optimized with respect
to energy. And it must be ensured
that the vehicles can not collide with each other or with the obstacles.

This is a typical problem in the field of robot motion planning.
And many techniques have been proposed to solve this problem, for example,
potential functions, randomized algorithms, etc. However, the drawback of these
approaches is that the performance can not be guaranteed. In [1] and [2], the
problem is formulated as a hybrid optimal control problem and numerically
solved using mixed integer programming. But the method does not scale well.

Our goal is to find fast and effective approaches to formulate and
numerically solve the above problem.

Model

Considering the case of only one vehicle, choose state variables *s*=[*x,
y, v _{x}, v_{y}*]

_{}.

Obstacle avoidance constraints can be written as:

_{}

The cost functional is:

_{}

If there are multiple vehicles, the above expressions can be
generalized easily. And the constraints of avoiding other vehicles are:

_{}

Overview

Our method can be depicted as the following diagram.

The methodology is based on an extension (see the paper *Applications of Numerical
Optimal Control to Nonlinear Hybrid Systems*) of the embedding technique
in [3] for the solution of the hybrid optimal control problem at each step of
the model predictive control (MPC) algorithm. The techniques help us transform
the switched optimal control problem into a **smooth** one which can be solved using traditional methods.

Embedding

Consider the
following system

_{}

where _{} (convex and
compact) is the continuous control input; _{} is the discrete
control input; and _{} is the autonomous
switch. The cost functional is

_{}

The hybrid
optimal control problem (HOCP) is

_{}.

Introduce
new discrete control input _{} and _{} . Let continuous
control input _{}. Define a new system

_{}

and the associated cost functional

_{}

We can get
the embedded optimal control problem (EOCP):

_{}

It is shown
in [3] that:

The
trajectory set of the hybrid system is dense (in the _{} sense) in the
trajectory set of the embedded system.

The optimal
solution of the embedded system is either the solution of the original problem,
or can be approximated arbitrarily closely with a trajectory of the hybrid
system.

Collocation

To numerically solve the EOCP, a variation of direct collocation
[4] is used.

Partition
the time interval [*t _{0}, t_{f}*] into

_{}

Let

_{}

The state trajectory is approximated by a piecewise-linear
function:

_{}

The control input is piecewise constant:

_{}

The system equations are enforced at the midpoints:

_{}

Model Predictive Control

A popular feedback control scheme for constrained optimization
problem, model predictive control (MPC) [5] is used. It can account for
external disturbances and modeling uncertainties. The strategy is shown below:

And the steps of MPC are shown in this diagram:

We have successfully applied the above methodology to solving the
control problem of a unicycle example and a Hilare
robot example.

Unicycle Example

For details, see the paper *Applications of Numerical
Optimal Control to Nonlinear Hybrid Systems*.

The unicycle drives on a horizontal plane. The wheel of the
unicycle can either roll or slide, resulting in autonomous switches. And it has
a regenerative brake that can be turned on or off. The objective is to drive
the unicycle to the origin within an allotted time while minimizing power
usage.

The first movie shows the actions
of the unicycle when there is no disturbance. The second movie shows that the robot
can still reach the origin in the required time despite disturbances and model
errors.

Hilare Robot
Example

For details, see the paperHybrid Model Predictive Control for Stabilization of Wheeled Mobile Robots Subject to Wheel Slippage.

The Hilare robot is a 2-wheel differentially
driven mobile robot on a horizontal plane. Again, the wheels of the robot can
either roll or slide, resulting in autonomous switches. And it has a
regenerative brake that can be turned on or off. The robot is required to
perform the following tasks while minimizing power usage:

To move from the initial state *z _{0}*
to the origin within a specified time.

To stabilize from the initial state *z _{0}* to the y axis with some constant forward velocity

The third movie
and the fourth movie
demonstrate the simulation results of the example. From the results we can see
that the approach achieves good performance.

For systems which include multiple vehicles and a number of
obstacles, in order to further decrease computation time, we need to improve
the numerical methods of solving hybrid optimal control problems. We are trying
to use some techniques such as decreasing horizon MPC, decentralized MPC, etc.

1.
Y. Kuwata, A. Richards, T. Schouwenaars, and J. P. How. Decentralized robust receding
horizon control for multi-vehicle guidance. In Proceedings of the 2006 American
Control Conference, pp. 2047-2052,

2.
L. Pallottino, E. Feron,
and A. Bicchi. Mixed integer programming for aircraft
conflict resolution. In AIAA Guidance, Navigation and Control Conference and
Exhibit, 2001.

3.
S. C. Bengea and R. A. DeCarlo. Optimal control of switching systems. Automatica, 41(1):11-27, 2005

4.
O. von Stryk, Numerical solution of
optimal control problems by direct collocation, in: Optimal Control (Freiburg, 1991), in: Internat.
Ser. Numer. Math., vol. 111, Birkhauser,
Basel, 1993, pp. 129-143.

5.
E. F. Camacho and C. Bordons, Model
Predictive Control. Springer Verlag, 2004.