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  and , 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.
Considering the case of only one vehicle, choose state variables s=[x, y, vx, vy]T, control input u=[ax, ay]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:
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  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.
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  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.
To numerically solve the EOCP, a variation of direct collocation  is used.
Partition the time interval [t0, tf] into N subintervals with the endpoints
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)  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.
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 z0 to the origin within a specified time.
To stabilize from the initial state z0 to the y axis with some constant forward velocity v0 within a specified time.
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.
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.