Application of Hybrid Optimal Control to

Multi-vehicle Path Planning

Investigators: Shangming Wei and Dr. Miloš Žefran

Problem Description

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.

Methodology

Model

Considering the case of only one vehicle, choose state variables s=[x, y, v_x, v_y]^T, control input u=[a_x, a_y]^T. Then we have

.

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₀, t_f] into N subintervals with the endpoints

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:

Accomplishments

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 paper Hybrid 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₀ to the origin within a specified time.

To stabilize from the initial state z₀ to the y axis with some constant forward velocity v₀ within a specified time.

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.

Future Work

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.

References

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, Minneapolis, MN, USA, 2006.

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.