MIXED INTEGER PROGRAMMING FOR MULTI-VEHICLE PATH PLANNING
Tom Schouwenaars , Bart De Moor , Eric Feron , Jonathan How
Esat-Sista, Katholieke Universiteit Leuven, Belgium Laboratory for Information and Decision Systems, Space Systems Laboratory, Massachusetts Institute of Technology, Cambridge, MA 02139 fax: +(1) 617-253 5779 e-mail:
[email protected]
ECC2001 Conference e-mail:
[email protected] http://www.fe.up.pt/ecc2001/ Keywords: autonomous vehicles, path planning, collision avoidance
Abstract This paper presents a new approach to fuel-optimal path planning of multiple vehicles using a combination of linear and integer programming. The basic problem formulation is to have the vehicles move from an initial dynamic state to a final state without colliding with each other, while at the same time avoiding other stationary and moving obstacles. It is shown that this problem can be rewritten as a linear program with mixed integer/linear constraints that account for the collision avoidance. A key benefit of this approach is that the path optimization can be readily solved using the CPLEX optimization software with an AMPL/Matlab interface. An example is worked out to show that the framework of mixed integer/linear programming is well suited for path planning and collision avoidance problems. Implementation issues are also considered. In particular, we compare receding horizon strategies with fixed arrival time approaches.
1 Introduction The problem discussed in this paper is to use mathematical programming techniques to find the optimal path between two states for a single vehicle or a group of vehicles. This path will be optimized with respect to both fuel and/or time, and must ensure that the vehicles do not collide with each other or with obstacles, which can be fixed or moving. A classical approach to collision avoidance, especially in the field of robot motion planning, is the use of potential functions [1]. Other recently developed techniques include such approaches as randomized algorithms [2]. Path planning techniques using graph searching and surface covering [3] were developed earlier. This paper presents an alternative technique that uses integer programming as a modeling framework to formulate appropriate obstacle and inter-vehicle collision avoidance constraints on the
Proceedings of the European Control Conference 2001
motion of the dynamic system. The paper first presents the basic problem formulation for a single vehicle, which is followed by the introduction of the constraints for obstacle avoidance. Note that we distinguish between (i) collision avoidance with stationary obstacles, (ii) collision avoidance with moving obstacles that have predefined paths, and (iii) mutual collision avoidance between multiple vehicles. A software implementation using AMPL and CPLEX with a Matlab interface to solve the mathematical program is presented. Section 6 compares different implementation strategies including receding horizon and fixed arrival time approaches.
2 Problem formulation 2.1 Single vehicle The basic path planning problem discussed in this paper is the problem of finding a fuel-optimal path between the two states of one or more moving objects. Classical approaches to this optimization problem include the minimization of a quadratic cost function whose variables must satisfy the state space equations of the dynamical system
[4].
#%$ " (1) ! & ('*),+-'/.%01)32 01546 subject to ' 7 (2)
'.% 0 In this problem formulation, '98;:=< is the state vector and 0 8>: ¤1¥9'v 'v = 'v 'v =z ¢ZÌ d 8¹£ r *tututudU Å=¤G zp/8£ r *tut*tvN>¤1¥ d§ d" 5 d§ ¿Í = . ©ª6(*Í d"®a «¡ d§ «Í =z . ©ª6(*Í d¯ ¯ Y Í = . ©ª6(*Í «Yd3 } «Í = Î. ©ª6(*Í }°c e 3 6(uÍ d3} a 6 uÍ A or r ¢ d§£ j¦ . ut*tutvD¤{¥ d p=8H£ r ut*tutvN>¤{dd ¥^´j8¹ d§¦ r d" d ¹¿µ d§¦ 4\¶gH©½À µ d® a ¿µ d » d¨¦ 4\¶gH©½À µ dÁ¯ ¯ ¹µ 4\¸`©½À µ µ ¹ d3 } ±4\¸`©½À µ }°c e À µ d3} a À µ A or r
(20)
writes the resulting solution to an output file readable by Matlab. Other Matlab-scripts then plot the path and visualize the state and input sequence.
(21)
6 Planning Strategies
(22)
This section presents two planning strategies: a receding horizon strategy and a fixed arrival time approach. The latter can also be used to calculate the shortest time path. 6.1 Receding horizon
(23)
When applying a receding horizon strategy the path of the vehicles is composed of a sequence of locally optimal segments. At a certain time step the MILP from Section 4 is solved for the N future time steps, which provides the input commands for these N future time steps. However, only a subset of these N input commands are actually implemented. Instead, the process is repeated periodically and a new set of commands is developed for each time window. Usually the applied subset is restricted to the first control input. Hence a new set of input commands is calculated at each time step. 6.2 Fixed arrival time
(24)
5 Software The total optimization problem from the previous section can be readily solved by a mixed integer program solver implemented in the CPLEX software package. CPLEX requires the standard MPS-format as input [8], which can be generated by running a specific m-script in Matlab. However, writing such an MPS-file generating script is an error prone task and such scripts are usually not very flexible in adapting the cost function or the type of constraints. We have therefore used the AMPLlanguage [9] to formulate the path planning problem. Implementing the constraints in AMPL is straightforward, requiring minimum translation from the form shown in Section 4. The forms of the problem and the constraints are defined in a model file, while the parameter values are in a separate data file. Therefore, changes to the problem may be made without rebuilding the constraints expressions. The data files can be edited directly or generated by a simple Matlab script. AMPL combines the model and data to an MPS-file, which is then solved by CPLEX. Moreover, AMPL can handle piece-wise linear variables and thus absolute values: it hence generates the necessary slack variables and extra constraints automatically.
An alternative approach is to use a fixed arrival time, which means that the arrival time is specified and the mathematical program is solved once over the entire time range. The final state (at time step N ) can be constrained in this approach ' X ' instead of including it in the cost function. The cost function for a single vehicle then reduces to
where step p .
]
don
X`c de _ba nc < e ? n don à F F & a
(25)
denotes the w th component of the input vector at time Thus the calculated path is designed to minimize the fuel used for that specific time range and weight vector C . The extension to the multiple vehicle case is straightforward. In case the arrival times of all vehicles are the same, the cost function becomes
don
X c d"e _ba nc < e ? n d n ce ` ð F F Ï D & a a
(26)
where denotes the w th component of the input vector of the th vehicle at time step p . Note that it is also possible to specify different arrival times for each vehicle.
We thus specify the system matrices, the obstacle coordinates and other optional dynamical parameters in Matlab, and use a specific script to generate an AMPL input file. An AMPL script selects the appropriate model and data file, calls the solver and
The fixed arrival time approach can also be used to calculate the minimum-time path between two points. This can be done by solving a feasibility problem corresponding to the constraints of the MILP, while decreasing the horizon length. The shortest time path can then be computed for the minimum horizon length for which the mathematical program is still feasible.
Proceedings of the European Control Conference 2001
2606
6.3 Optimality and computation time The major difference between the fixed arrival time and the receding horizon approach is the difference in the optimization criteria. By specifying the arrival time and accounting for the final state in the constraints, the cost function (6) is reduced to only contain the input variables. Therefore there is no trade-off between input and state minimization and the computed input sequence is thus the fuel-optimal one for that specific horizon length and those specific weight factors, taking into account the dynamic and collision avoidance constraints. When using a receding horizon (RH), however, the solution is locally optimal on every segment. Thus the total RH-path probably requires more fuel than the vehicle would consume if the path was calculated over the entire time range. Consider an autonomous ground vehicle that has to manoeuvre between a set of rectangular obstacles. The vehicle is modeled as a simple 2-D discrete double integrator with discretization Q step 6 1 shows the trajectory for a fixed arM{Aаt Ñr Ñ . Figure rival time or 80 time steps. The initial position ÓÒ is rÈÔ Ò ; the destination is zPA5t Ô u Ô . The vehicle mostly moves around the obstacles, which consumes the least amount of fuel. Although the vehicle could arrive earlier, it uses the complete time range to arrive at its destination. The velocity and acceleration thus remain as low as possible, which corresponds to the minimum fuel use. In order to compare the different strategies the total sum Õ`Öq×zÖ of the absolute values of the input components is considered as a measure for fuel consumption:
XÙc de ØzÚGÚ
Õ«Öq×zÖ
&
N аÑÑ
d
Destination reached in 7.3 seconds
15
d
F ¿¶ F . F 5¸ F
(27)
10
5
0
−5
−10
−5
0
In Figure 2 the fixed arrival time approach is used to compute the shortest time path between the two points as described in
10
15
20
±
d
Figure 2: Fuel-optimal shortest time path of unmanned vehicle:
Mbá
< ªÛ t
d ± á M Section 6.2. This shortest time is ±@< â ¼Û ± t . To reach the destination within Û t , the vehicle now has to manoeuvre bet of fuel. tween the obstacles, requiring Õ Öq×zÖ ± Figure Mfã Ñ 3 ± shows the receding horizon path for a horizon or 30 time steps. The fuel consumption is now × ÕMfаÖqÑ×JÖÑ Ô t Û and the vehicle reaches the destination after Ý t Þ or 92 iterations. The fuel consumption is thus 59.3% greater than in the fixed arrival time case with M аÑÑ ßÒ . As now the “distance” to the final location is in±
cluded in the cost function and is thus minimized, the vehicle starts manoeuvring between the obstacles, which explains the increased fuel consumption. Destination reached in 9.2 seconds
15
is the last but one time step, corresponding to the last аÑÑ input. In the fixed arrival time example N ÜÛÝ and the vehicle consumes Õ Öq×zÖ ßÞ¡r t Þ of fuel.
5
10
5
0 Destination reached in 8 seconds
−5
15
−10
10
−5
5
0
5
10
15
20
±
Figure 3: Trajectory of unmanned vehicle under receding horiMfã Ñ zon control with ×
0
−5
Figure 1: Fuel-optimal path of unmanned vehicle for a fixed M аÑÑ arrival time àÒ
Of course, the key benefit of the receding horizon approach is that the solution can be calculated much faster than the fixedtime problem. This follows from the fact that the number of variables and constraints increases polynomially with the horizon length, but the worst-case solution time increases exponentially [5]. The trade-off between optimality and computation time is important for a real-time implementation of these path
Proceedings of the European Control Conference 2001
2607
−10
−5
0
5
10
15
20
planning algorithms. This is shown in Table 1, where the arrival M аÑÑ , the total fuel consumption ÕkÖq×JÖ , the total computatime d M á tion time ÍI× and the average computation time per iteration M Ö are shown for different horizon lengths M ã × Ñ . Although the vehicle arrives earlier, in this scenario, the fuel consumption decreases with the horizon length. For comparison, Table 2 M á gives the fuel consumption Õ Öq×zÖ and computation time M1аÑÑ ÍI× for a fixed arrival time planning for the arrival times in Table 1. With a fixed time approach the vehicle clearly consumes less fuel, but the computation time increases strongly. Future d research will explore these observations in more detail.
M ã × Ñ Y 3.0 3.5 4.0 4.5 5.0
M vÐ ÑÑ ( Y 9.2 9.0 8.4 8.2 8.2
Õ«Öq×zÖ
35.7 34.1 31.6 30.4 30.4
M ÍI× á · (È 129 267 441 728 1213
M Ö° Y
1.40 2.97 5.25 8.88 14.79
Table 1: Arrival times, fuel consumption and computation times for different horizon lengths.
M °Ð ÑÑ Y 9.2 9.0 8.4 8.2 8
ÕPÖq×JÖ
17.4 17.8 19.2 20.0 21.2
M ÍI× á ¡ Y 613 442 193 189 162
Table 2: Arrival times, fuel consumption and computation times for fixed arrival time planning 6.4 Safety Although the solution to the MILP yields a collision free path, in a receding horizon setting this still does not guarantee that no collisions will occur. Indeed, in a RH-setting a single vehicle or a set of vehicles can be led to a critical situation for which the MILP becomes unfeasible in the next iteration. This can happen if a vehicle has moved too close to an obstacle or if multiple vehicles have moved too close to each other. If the remaining distance is too short to stop or to avoid the obstacle or other vehicles, a collision will occur. Such critical situations typically occur when the horizon is too short to “spot” obstacles or other vehicles in time.
7 Conclusion This paper presented a new approach to trajectory planning of multiple vehicles that directly incorporates collision avoidance as mixed integer/linear constraints. A mathematical programming formulation has been proposed including binary constraints for stationary and moving obstacle avoidance and multi-vehicle path planning. Several strategies with corresponding trade-offs were discussed and examples were given to show the potential of our approach. It was shown that receding horizon strategies, while computationally more attractive than strategies aimed at computing complete trajectories a priori, can lead the system to unsafe conditions. Future work will therefore concentrate on procedures ensuring that a single vehicle or a set of vehicles can always return to a “safe” state from wherever it stands.
Acknowledgments The authors would like to thank Emilio Frazzoli, Sommer Gentry, Mao Zhi-Hong, Arthur Richards (MIT) and Jeroen Buijs (KU Leuven) for their support during this work.
References [1] Jean-Claude Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991. [2] J. Barraquand, L.E. Kavraki, J.C. Latombe, T.Y. Li, R.Motwani, and P.Raghavan. A random sampling scheme for path planning. International Journal of Robotics Research, 16(6):759–774, 1997. [3] T. Lozano-Perez. Spatial planning with polyhedral models. June 1980. MIT Ph.D. Thesis (E.E.). [4] Michael Athans and Peter L. Falb. Optimal Control, An Introduction to the Theory and Its Applications. McGrawHill, 1966. [5] Dimitris Bertsimas and John N. Tsitsiklis. Introduction to Linear Optimization. Athena Scientific, 1997. [6] Hamdy A. Taha. Operations Research, An Introduction. Macmillan Publishing Company, New York, 4th edition, 1987.
One thus has to guarantee that a vehicle can always stop relative to the surrounding environment or has enough time to carry out a collision avoidance manoeuvre. More generally, one needs policies that guarantee a priori that no inter-vehicle collisions nor collision with obstacles will occur. Thus strategies are needed in which a vehicle can always return to a safe state whenever it faces a critical situation. However, general “safe” guidance policies are not readily available, which is a topic of further research.
[7] Andrew Robertson, Gokhan Inalhan, and Jonathan P. How. Spacecraft formation flying control design for the orion mission. Proceedings of AIAA/GNC, August 1999.
Proceedings of the European Control Conference 2001
2608
[8] ILOG CPLEX User’s guide. ILOG, 1999. [9] R. Fourer, D. M. Gay, and B. W. Kernighar. AMPL, A modeling language for mathematical programming. The Scientific Press, 1993.
