Mixed integer linear programming trajectory generation for autonomous nap-of-the-earth flight in a threat environment

[0001] This invention was made under contract number F33615-99-C-3613. The United States Government has rights in this invention under the contract.

[0002] This invention relates to methods for controlling autonomous vehicles, and more particularly to methods for controlling autonomous air vehicles for nap-of-the-earth flight.

[0003] The technique of using mixed integer linear programming (MILP) for multi-vehicle path planning purposes in an obstacle environment has been recently developed. The technique has been proven useful at least in an experimental context and laboratory systems that implement MILP for path planning have been built as shown in M. G. Earl and R. Dandrea, “Modeling and control of a multi-agent system using mixed integer linear programming”, In Proceedings of the 41^st IEEE Conference on Decision and Control, December 2002.

[0004] A mixed integer linear programming (MILP) problem is very similar to a linear programming (LP) problem in that there is a set of linear objective functions and constraints, under which an optimal solution may be found. A MILP problem differs from an LP problem only in that some of the variables are restricted to be integers and thus the solution space for a MILP problem is a subset of that of a similar LP problem (the MILP problem without the integer restrictions). A major advantage of using MILP as a problem formulation is that fast, parallelizable, and commercially available MILP solvers such as ILOG CPLEX are readily available.

[0005] Mathematical programming modeling languages such as AMPL, have been developed to take advantage of higher level constructs in expressing a complex problem and the ability to be mapped to multiple solvers, much like a high level computer programming language such as C++ is more versatile for many programmers and can be mapped (or compiled) onto assembly languages for multiple platforms.

[0006] T. Schouwenaars, B. De Moor, E. Feron, and J. How, “Mixed integer programming for multi-vehicle path planning”, In 2001 European Control Conference, September 2001, describes a basic MILP problem formulation combining fuel-optimal path planning for multiple vehicles with double-integrator vehicle dynamics and obstacles in two-dimensional space.

[0007] T. Schouwenaars, E. Feron, and J. How, “Safe receding horizon path planning for autonomous vehicles”, In 40^th Annual Allerton Conference on Communication, Control, and Computing, October 2002, guarantees a priori safety for a MILP path planning implementation with fixed obstacles and limited planning horizon length by incorporating safe maneuvers.

[0008] J. Bellingham, A. Richards, and J. How, “Receding horizon control of autonomous aerial vehicles”, In 2002 American Control Conference, May 2002, augments a receding-horizon implementation of MILP path planning with a cost-to-go map in order to avoid entrapment behind relatively large obstacles.

[0009] T. Schouwenaars, B. Mettler, E. Feron, and J. How, “Hybrid architecture for full envelope autonomous rotorcraft guidance”, In Proceedings of the 59^th Forum of the American Helicopter Society, May 2003, allows the use of multiple linear time invariant (LTI) modes and discrete finite time maneuvers in a MILP path planning problem as a first step in providing full flight envelope trajectory planning.

[0010] The above references do not address path planning in an environment containing threats. There is a need for a method of generating air vehicle flight trajectories for flying over a terrain and avoiding obstacles and with multiple threats.

[0011] This invention provides a method of planning a path of a vehicle comprising the steps of: defining a first plurality of constraints representing dynamics of the vehicle; defining a second plurality of constraints representing collisions with obstacles; defining a third plurality of constraints representing visibility to threats; and using the first, second and third plurality of constraints with mixed integer linear programming to generate a trajectory for a vehicle from a starting point to a destination point in an environment containing the obstacles and the threats.

[0012] In another aspect, the invention provides a method of controlling a path of a vehicle, the method comprising the steps of: defining a first plurality of constraints representing dynamics of the vehicle; defining a second plurality of constraints representing collision constraints with obstacles; defining a third plurality of constraints representing visibility to threats; and using the first, second and third plurality of constraints and mixed integer linear programming to generate a set of control actions to move the vehicle from a starting point to a destination point.

[0024] This invention extends a recently developed approach to the optimal path planning of an autonomous vehicle in an obstacle field to three dimensions with specific applications to terrestrial navigation, with obstacle, terrain, and threat avoidance. This approach can be applied to a rotorcraft performing nap-of-the-earth flight in challenging terrain with multiple known surface threats, in both urban and non-urban type settings. Mixed Integer Linear Programming (MILP) is the underlying problem formulation, from which an optimal solution can be obtained through the use of a commercially available MILP solver such as CPLEX. The optimal solution obtained would be optimal in terms of a cost function specified in terms of fuel, time, altitude and threat exposure, and other predefined criteria. A receding horizon implementation of this MILP algorithm can be used to make it feasible to implement in real-time or near real-time. Estimates of the typical problem size and computation time are provided for a rotorcraft nap-of-the-earth flight scenario.

[0025] A standard mixed integer linear programming (MILP) problem has the following form:

[0026] $\begin{array}{cc}\min f\left(x\right)=c^{T}x& \left(1\right)\end{array}$
b_L≦Ax≦b_U  (2)
x_L≦x≦x_U  (3)
where
Aε^m×n,c,x,x_L,x_Uεⁿ,b_L,b_Uε^m
and a subset of x is restricted to be integers. For the constraints of equation (2), the rows corresponding to the portion(s) of x that are restricted to be integers are integer constraints, while the rest of the rows are continuous constraints.

[0027] An optimal solution x, if a feasible solution exists, is a vector of variables for which the value under the objective function (1) is smallest, such that x satisfies the constraints (2) and bounds (3). That is, for any other x*≠x, either f(x*)≧f(x), or x does not satisfy at least one of (2), (3), and the integer restriction.

[0028] Mixed integer linear programming is especially suited for path planning in an environment with obstacles because the continuous constraints can represent the dynamics of the vehicle, while the integer constraints can represent collision constraints with obstacles.

[0029] For linear, time invariant (LTI), continuous time plant dynamics:
{dot over (s)}=A_cs+B_cu  (4)
and the initial and final conditions are defined to be s₀ and s_F, respectively.

[0030] In particular, and for the rest of this description, the plant dynamics consist of a double integrator model with acceleration as input:
{umlaut over (x)}=u_x
ÿ=u_y
{umlaut over (z)}=u_z  (5)

[0031] By defining
v_x={dot over (x)},v_y={dot over (y)},v_z=ż,
equations (5) can be rewritten in the form of (4) by setting
s=[x y z v_x v_y v_z]^T
and
u=[u_x u_y u_z]^T
to get

[0032] $\stackrel{•}{s}=\left[\begin{array}{cc}0& I\\ 0& 0\end{array}\right]s+\left[\begin{array}{c}0\\ I\end{array}\right]u$

[0033] For the purpose of formulating a trajectory planning problem as an LP or MILP problem, the plant dynamics need to be discretized with a chosen period to yield:
s[k+1]=As[k]+Bu[k]  (6)

[0034] Using equation (6) recursively, and assuming the notation s_k=s[k], u_k=u[k], then:
s_k=A^ks₀+A^k−1Bu₀+ . . . +ABu_k−2+Bu_k−1  (7)

[0035] Assuming a problem with N time steps, then:
s_F=s_N

[0036] Applying this to equation (7) yields a set of destination constraints:
s_F−A^Ns₀=A^N−1Bu₀+ . . . +ABu_N−2+Bu_N−1  (8)

[0037] In the absence of other constraints, a solution to equation (8) is a set of N piecewise constant controls {u₀, . . . , u_N−1} that will steer the continuous system (5) from s₀ to s_F in N time periods.

[0038] Velocity constraints on v_x, v_y, v_z can be expressed as the following:
v_min≦v_k≦v_max  (9)
where:
v=[v_xv_yv_z]^T,
v_k=v[k]=Vs_k,
V=[0I],
v_min=[v_x,minv_y,minv_z,min]^T,
v_max=[v_x,maxv_y,maxv_z,max]^T

[0039] Applying equations (7) to (9) to obtain the velocity constraints for time kε{0, . . . , N−1}:
v_min−VA^ks₀≦VA^k−1Bu₀+ . . . +VABu_k−2+VBu_k−1≦v_max−VA^ks₀  (10)

[0040] Control limits on (u_x u_y u_z) can be expressed as the following, for kε{0, . . . , N−1}
u_min≦u_k≦u_max  (11)

• • [0041] where: u_min=[U_x,min u_y,min u_z,min]^T, u_max=[u_x,max u_y,max u_z,max]^T

[0042] For a solution that is both fuel and time optimal, we would like the objective function to be the following:

[0043] $\begin{array}{cc}f\left(x\right)=\sum _{k=0}^{N-1}r{u_k}_1+\sum _{k=1}^{N-1}q{X_k-X_F}_1& \left(12\right)\end{array}$
where:
X=[x y z]^T, X_k=X[k]=Ws_k, X_F=Ws_F, W=[10]

[0044] ∥X_k−X_F∥₁ is the 1-norm distance in (x, y, z) between s_k and s_F, and r and q are non-negative weight factors. The first and second term in (12) are known as fuel cost and non-arrival cost, respectively. The non-arrival cost in the objective function is the key to a minimum-time formulation.

[0045] Since a 1-norm function is not linear, additional slack variables v_k and w_k can be introduced to create the desired effect with the following constraints:
−u_k≦u_k≦v_k  (13)
−w_k≦W_sk−W_sF≦w_k  (14)

[0046] When the constraints (13) and (14) are combined with non-negative weights q and r, they also guarantee that
v_k=∥u_k∥₁ and w_k=∥WS_k−WS_F∥₁=∥X_k−X_F∥₁
respectively, in the optimal solution.

[0047] Constraints (13) can be decomposed into two separate inequalities as MILP constraints for kε{0, . . . , N−1}:
u_k−u_k≦0, −u_k−u_k≦0  (15)

[0048] Similarly, constraints (14) can be decomposed into two separate inequalities, expanded out using equation (7), and written as MILP constraints for kε{1, . . . , N−1}:
W(A^k−1Bu₀+ . . . +ABU_k−2+Bu_k−1)−w_k≦W(s_F−A^ks₀)
−W(A^k−1Bu₀+ . . . +ABu_k−2+Bu_k−1)−w_k≦−W(s_F−A^ks₀)  (16)

[0049] The slack variables defined in (13) and (14), can be used to conveniently express the objective function (12) as:

[0050] $\begin{array}{cc}f\left(x\right)=r\sum _{k=0}^{N-1}{v}_k+q\sum _{k=1}^{N-1}{w}_k& \left(17\right)\end{array}$

[0051] The basic LP trajectory planning problem thus consists of the destination constraints (8), velocity constraints (10), slack variable constraints for energy (15) and time (16), control limits (11), and objective function (17). An optimal solution under these conditions is said to be a set of control actions that would take the system (5) from s₀ to s_F in N time periods with minimum cost as defined by (17).

[0052] The existing LP problem can be expanded to include obstacle collision constraints. It can be shown that for any arbitrary convex polyhedral bounded by n number of planes in 3-D, the interior of the polyhedral can be represented by the combination of the following set of inequalities:

[0053] $\begin{array}{cc}\begin{array}{c}{a}_{1}x+b_{1}y+c_{1}z>d_1\\ ⋮\\ a_{n}x+b_{n}y+c_{n}z>d_n\end{array}& \left(18\right)\end{array}$

[0054] The collision constraints requiring that s_k be outside the region defined by (18) can be rewritten as:

[0055] $\begin{array}{cc}\left[\begin{array}{cccccc}{a}_1& b_1& c_1& 0& 0& 0\\ & & ⋮& & & \\ a_n& b_n& c_n& 0& 0& 0\end{array}\right]s_k-M\left[\begin{array}{c}{b}_{k_1}\\ ⋮\\ b_{k_n}\end{array}\right]\le \left[\begin{array}{c}{d}_1\\ ⋮\\ d_n\end{array}\right]& \left(19\right)\\ b_{k_1}+\cdots +b_{k_n}\le n-1& \left(20\right)\end{array}$
where M>>0, is an arbitrarily large number, and b_k1 are binary variables. The constraints (19) and (20) essentially negate at least one set of the inequalities in (18) and force s_k to be outside the region defined by (18). By writing (19) in short form as
θ_is_k−Mb_kⁱ≦θ′_i
for each obstacle i, and by substituting s_k with equation (7), the final MILP obstacle collision constraints for obstacle
iε{1, . . . , p}
would be, for kε{1, . . . , N}:
θ_i(A^k−1Bu₀+ . . . +ABu_k−2+Bu_k−1)−Mb_kⁱ≦θ′_i−θ_iA^ks₀
[1 . . . 1]_1×nib_kⁱ≦n_i−1
0≦b_kⁱ≦1
b_kⁱεintegers  (21)

[0056] The MILP trajectory planning problem with obstacle avoidance includes the constraints in the basic LP trajectory planning problem, i.e. expressions (8), (10), (11), (15), (16), and (17), plus obstacle collision constraints (21).

[0057] Assuming that N is the number of time steps, the above MILP problem will consist of

[0058] $6+3N*2+3\left(N-1\right)*2+3N+N\sum _{i=1}^p\left(n_i+1\right)$
constraints, 3N+3N+3(N−1) continuous variables, and

[0059] $N\sum _{i=1}^p{n}_i$
discrete variables, where p is the number of obstacles and n_i is the number of planes that define obstacle i.

[0060] The MILP trajectory planning problem can be implemented in a receding horizon framework to reduce the computational effort required to solve the MILP problem by reducing the problem size. Receding horizon trajectory planning with obstacle avoidance can be implemented as a series of MILP problems. Instead of planning the entire optimal trajectory from s₀ to s_F, which may be located very far apart and may require many time steps to reach, the problem can be broken down into many subproblems of smaller size with fewer time steps N, such that within each subproblem there is no longer the requirement to reach a particular destination state s_F (destination constraint (8)). Instead, the first subproblem uses s₀ as the initial state, and only a subset of the planned trajectory from the first subproblem is executed (i.e. only s₁ and s₂) Then s₂ from the first subproblem is used as the initial state in the second subproblem, and so on, until a subproblem with s_N=s_F is calculated, in which case the entire trajectory of the subproblem is executed and the vehicle reaches the destination.

[0061] In order to encourage the overall trajectory, as well as the individual trajectories of each subproblem, to get closer to the destination, an additional term known as “terminal cost” enters the objective function

[0062] $\begin{array}{cc}f\left(x\right)=\sum _{k=0}^{N-1}r{u_k}_1+\sum _{k=1}^{N-1}q{X_k-X_F}_1+p{X_N-X_F}_1& \left(22\right)\end{array}$

[0063] The objective function thus becomes:

[0064] $\begin{array}{cc}f\left(x\right)=r\sum _{k=0}^{N-1}{\upsilon }_k+q\sum _{k=1}^{N-1}{w}_k+{\mathrm{pw}}_N& \left(23\right)\end{array}$
and to accommodate the term
∥X_N−X_F∥₁,
an additional slack variable w_N is introduced according to equation (14) so that constraints (16) still hold but is now applied through the range
kε{1, . . . , N−1}.

[0065] “Terrain flight” is the tactic of using the terrain's vegetation and man-made objects to degrade the enemy's ability to visually, optically, or electronically detect or locate the aircraft. Nap-of-the-Earth (NOE) flight is a category of terrain flight characterized by maneuvers as close to the earth's surface as vegetation, obstacles, or ambient light will permit. This invention provides a novel method for enabling autonomous NOE flight that not only avoids obstacles but also actively uses them for minimizing detection by known enemy elements.

[0066] Terrain flight can be considered to be a variation of obstacle avoidance, with some important differences: (1) obstacles are generally scattered in various locations, while terrain is constantly under the vehicle; (2) it usually suffices to avoid collision with the obstacles, but in terrain flight, the objective is to stay low and out of sight of the enemy. This invention approaches basic terrain flight by constructing the terrain as a series of obstacles that act as three-dimensional “floor tiles” that the vehicle travels over, while introducing an altitude cost to keep the trajectory close to the terrain:

[0067] $\begin{array}{cc}f\left(x\right)=\sum _{k=0}^{N-1}r{u_k}_1+\sum _{k=1}^{N-1}q{X_k-X_F}_1+p{X_N-X_F}_1+\sum _{k=1}^N{a}_z{z}_k& \left(24\right)\end{array}$
where z_k=z[k]=Zs_k, Z=[0 0 1 0 0 0] and a_z is a non-negative weight factor. To make the objective function (24) implementable in MILP, slack variables z_k and the following constraints can be introduced by substituting equation (7) in
ZA^k−1Bu₀+ . . . +ZABu_k−2+ZBu_k−1−z_k=−ZA^ks₀  (25)

[0068] Together, expressions (24) and (25) associate a cost to the trajectory proportional to the altitude at each point. By tuning the weight factor a_z against other weight factors r, q, and p in the original objective function (22), NOE flight of varying aggressiveness can be achieved.

[0069] In one embodiment of the invention, the terrain can be represented as triangulated irregular networks (TIN), since it is a straightforward way of modeling grid-based terrain. TIN is simply a pattern of similarly oriented triangles that is laid on top of a regular square-based grid pattern, with an elevation assigned to each point on the grid, such as the one shown in FIG. 1. FIG. 1 is a schematic representation of terrain 10 using triangulated irregular networks. In the figures, a diamond represents a starting point and an * represents a destination.

[0070] A straightforward way of implementing terrain flight in MILP would be to model each triangle in a TIN terrain as an obstacle with three vertical side walls, each of which is positioned adjacent to and directly facing the side wall associated with an adjacent triangle, as illustrated in FIG. 2. FIG. 2 is a schematic representation of an obstacle 12 using terrain tiles.

[0071] The points s₀ and s_F would be positioned somewhere above the terrain and the trajectory would be required to avoid collision with the terrain while staying as close to the terrain as possible per the objective function.

[0072] The threats considered in this context are known as surface or ground threats such as surface-to-air missile (SAM) sites, anti-aircraft artillery (AAA) units, and radar sites. There are various approaches that deal with the problem of threat avoidance. In this broader context, MILP is considered to be unique as a mathematical programming tool because it allows the formulation of a threat avoidance problem that takes both obstacles and terrain into account, which is not the case in the other existing methods.

[0073] A key premise in this threat avoidance formulation is the assumptions of line-of-sight visibility and opaque objects. Threats are represented by individual points in space whose locations are known and are fixed to the surface of the terrain, although in general they can be fixed anywhere in space. Consider FIGS. 3, 4 and 5 where a threat and an obstacle are present. In FIGS. 3, 4 and 5, items 20, 22, 24, 26 and 28 are obstacles, and points 30, 32, 34 and 36 represent threat positions. The shaded areas 38, 40, 42, 44, 46, 48 and 50 are safe zones. The trajectory generation algorithm aims to take advantage of the “shadow” behind the obstacle and seeks to minimize visibility exposure to all known threats by preferentially selecting routes that are in the shadows as much as possible. This line-of-sight visibility assumption is equally applicable to all threats that use optical and radio-based detection mechanisms such as radar, infrared, and optical sensors. For the purpose of formulating a MILP problem, two types of threat avoidance are considered: absolute and relative. In absolute threat avoidance, visibility exposure to all threats is completely avoided, so that at no time will the vehicle be visible to any known threats. In relative threat avoidance, visibility exposure to all known threats is avoided as much as possible, but a balance is struck between threat exposure and other objective criteria, such as fuel consumption and timely arrival at a target. Because it is not often possible to achieve absolute threat avoidance in a realistic environment with multiple threats, only relative threat avoidance is considered in this description.

[0074] FIGS. 3, 4 and 5 are a schematic representation of safe zones. The area 52 with darker shading in FIG. 5 is technically in two different safe zones. This analogy can be extended to a scenario with multiple threats and multiple obstacles, such as the one shown in FIG. 4. In that case, there is one “shadow” corresponding to each combination of threat and obstacle. So for m obstacles and n threats, there are m×n shadows. However, as FIG. 5 shows, these shadows may partially or completely overlap one another, or in some cases, even eclipse or be eclipsed by another.

[0075] Theoretically, the MILP logic of threat avoidance can be implemented in two ways: (1) consider the shadows as “safe zones” and formulate the MILP problem to encourage travel within them, or (2) consider the non-shadow regions as “unsafe zones” and formulate the MILP problem to avoid them. For the second implementation, the unsafe zones can be treated as similar to obstacles in a sense that they should be avoided, but in a relative sense. A positive cost can be assigned to each time the trajectory intersects an unsafe zone. With safe zones, on the other hand, a negative cost can be assigned in a similar manner. Though the two approaches initially appear to be very similar, the safe zone formulation is easier to work with due to its inherent desirable geometrical as well as topological properties. As described above, the obstacles are assumed to be convex. Due to this convexity assumption, a safe zone is easily defined. Qualitatively speaking, it is bounded by (1) the set of “front faces” (the sides of an obstacle that are facing the threat), plus (2) the set of planes that intersect both (a) the threat (a point), and (b) the edges of these “front faces” that border the “back faces” (the sides that face away from the threat). FIG. 3 illustrates this in two dimensions. Notice that a corresponding safe zone in three dimensions (for a cube) would be a pyramid of infinite height, minus a smaller pyramid with the “front face” of the cube as its base and sharing the same top. In a single threat, multiple obstacle scenario, the region invisible to the threat, or the overall safe zone, is simply a union of all the safe zones associated with each obstacle and the threat. This holds true even if one obstacle partially or totally blocks another obstacle from view of the threat, such as the case shown in FIG. 5, because once we are inside one of the shadows, we are safe whether or not it is eclipsed by another shadow. Unsafe zones, on the other hand, do not have similar topological properties.

[0076] For multiple threats and multiple obstacles, there may exist a region in which one cannot be seen by any threats. This is defined as the intersection of sets of safe zones, with each set corresponding to a different threat, as the darkest region in FIG. 5 illustrates. However, these regions do not always exist, and are useful only for absolute threat avoidance, so they are not relevant to this description. To paint a more mathematical picture of what was just described, point-set topology notation can be used. Suppose there are t threats and o obstacles. There would be t×o safe zones, with each one associated with a single threat and a single obstacle. A safe zone can be labeled with subscript and superscript as in:
S_jⁱ,
where,
iε{1, . . . , o}
denotes the obstacle and,
jε{1, . . . , t}
denotes the threats. The safe zone corresponding to threat j would be the following:
(S_j¹∪ . . . ∪S_j^o)

[0077] Suppose a safe zone is a convex polyhedral bounded by n planes in three dimensions, and the interior of which is represented by the combination of the following set of inequalities:
a₁x+b₁y+c₁z≦d₁
•
•
•
a_nx+b_ny+c_nz≦d_n  (26)

[0078] Suppose we have the following constraints:

[0079] $\begin{array}{cc}\left[\begin{array}{cccccc}{a}_1& b_1& c_1& 0& 0& 0\\ & & ⋮& & & \\ a_n& b_n& c_n& 0& 0& 0\end{array}\right]s_k-M\left[\begin{array}{c}{b}_{k_1}\\ ⋮\\ b_{k_n}\end{array}\right]\le \left[\begin{array}{c}{d}_1\\ ⋮\\ d_n\end{array}\right]& \left(27\right)\\ b_{k_1}+\cdots +b_{k_n}-{\mathrm{Mb}}_k^{\prime }\le 0& \left(28\right)\end{array}$
where M>>0 is an arbitrarily large number, and b_ki and b′_k are binary variables.

[0080] It can be easily seen that b′_k=0 implies that s_k is located within the safe zone defined by (26).

[0081] By writing (27) in short form as:
θ_i,js_k−Mb_k^i,j≦θ′_i,j
for each obstacle i and threat j, and by substituting s_k with equation (7), the final MILP threat avoidance constraints for obstacles:
iε{1, . . . , o}
and threats:
jε{1, . . . , t}
would be, for kε{1, . . . , N}:
θ_i,j(A^k-1Bu₀+ . . . +ABu_k−2+Bu_k-1)−Mb_k^i,j≦θ′_i,j−θ_i,jA^ks₀[1 . . . 1]_1×nib_k^i,j−Mb′_k^i,j≦0  (29)
0≦b_k^i,j≦1, 0≦b′_k^i,j≦1b_k^i,j, b′_k^i,jεintegers b′_k^1,j+ . . . +b′_k^o,j−Mb_k^j≦o−1  (30)

[0082] From the constraint (30), it can be seen that b_k^j=0 implies that at least one member of the set
{b′_k^1,j, . . . , b′_k^o,j}
is zero. That is, at time k, the trajectory is hidden from view from threat j behind at least one obstacle. This can be expressed as the following:
b_k^j=0s_kε(S_j¹∪ . . . ∪S_j^o)  (31)

[0083] We consider the constraints (29), (30) with an objective function with an additional term added to (24), with

[0084] $\begin{array}{cc}{a}_{\mathrm{Tj}}>0\text{:}f\left(x\right)=\sum _{k=0}^{N-1}r{u_k}_1+\sum _{k=1}^{N-1}q{X_k-X_F}_1+p{X_N-X_F}_1+\sum _{k=1}^N{a}_z{z}_k+\sum _{j=1}^t{a}_{T_j}\sum _{k=1}^N{b}_k^j& \left(32\right)\end{array}$

[0085] Constraints (29), (30), (31), and (32) guarantee the following:
b_k^j=0s_kε(S_j¹∪ . . . ∪S_j^o)  (33)

[0086] To establish (33), it suffices to prove the converse of (31). The rationale is that if the trajectory at time k is truly invisible from threat j, or equivalently,
s_kε(S_j¹∪ . . . ∪S_j^o),
then due to the positive cost associated with
b_k^j=1
in (32), the solution
b_k^j=0
would be preferred by the solver. Thus, through (29), (30), (31), and (32), we are essentially assigning a positive cost a_Tj for each time step k at which the trajectory is visible from threat j. This can be considered an objective penalty on visibility exposure to each separate threat. Notice that the threat exposure cost weight factor a_Tj is dependent on the threat j because we allow differential weighting of threats. Threats thought to be further away or of less important can be given a lower weight so that the visibility exposure to certain threats can be emphasized over that to other relatively minor threats.

[0087] The implementation of a MILP trajectory planning algorithm based on the problem formulation presented above, can now be described. In particular, the algorithm can use a MILP receding horizon method to generate a terrain-flight trajectory for a helicopter over a grid-based terrain. The first important step toward a realizable MILP generated trajectory is a realistic parametrization of the helicopter performance characteristics. Since we are using double integrator model (5) for vehicle dynamics, we need to specify the helicopter's velocity limits (9) and acceleration limits (11). If a more advanced vehicle model is used in place of (5), then one can more closely parametrize the helicopter characteristics. However, in a MILP problem formulation, the overall vehicle dynamics must be linear, and due to the fact that the equations of motion involves a nonlinear transformation between the body frame and inertial frame, we are largely restricted to modeling the vehicle in the inertial frame. Although it may be possible to derive a linear approximation of the nonlinear equations of motion, or any nonlinear equations for that matter, the technique involves the introduction of new variables that does not scale well with the problem and may lead to intractability. The second important consideration toward a successful MILP algorithm is the selection of weight values in the objective function (22). Because the objective function is the measure by which an optimal solution is determined, the weight values have great influence over the behavior of the optimal MILP trajectory. For each mission type, there is a unique set of weight values that are most suitable for the generation of the appropriate trajectory. For instance, a time-critical mission would have a very different set of weights than that of a fuel-critical mission. With a terrain flight formulation, the altitude weight must be chosen properly with relative to other weights to achieve the desired effect.

[0088] One embodiment of the invention uses a scenario in which a vehicle with dynamics (5) flies a NOE trajectory over a grid-based terrain with obstacles on top of that terrain, as FIG. 6 illustrates. The MILP problem can be constructed in a MATLAB environment, following the constraints and problem setup outlined above to define the composite vectors and matrices in equations (1), (2), and (3). Then, a text file in the mps format that describes the overall MILP problem is written, and it is read by the CPLEX MILP solver. After the optimal solution is computed, the result is loaded back into MATLAB for plotting and analysis. Since we are using a receding horizon method with a period of one time step, the MILP solver is called repeatedly at the end of each time step. We did not use the multi-resolution terrain triangulation techniques suggested above, but we predict that the application of the multi-resolution technique would yield a lower cost solution where for the same problem size and similar solve time, the NOE trajectory would be able to consider terrain features further away from the vehicle, and the resulting cost would be closer to the global optimum. An example scenario can demonstrate threat avoidance with multiple obstacles and multiple threats has been created. FIG. 7 shows threat avoidance with multiple threats 60 and 62, and obstacles 64, 66, 68, 70, 72 and 74. The optimal trajectory has minimal exposure to the threats (stars).

[0089] Due to the relative complexity in constructing, visualizing, and analyzing the threat avoidance scenario, the example shown in FIG. 7 is intentionally kept very simple, although the MILP solvers are capable of solving more complicated scenarios within a reasonable time. The production of a MILP trajectory such as the one shown in FIGS. 6 and 7, presents a few interesting challenges worthy of mention here. First, since the solution we look for is usually the optimal one, when the MILP problem is not properly constrained, the optimal solution may exhibit undesirable behavior, for instance because the terrain map is defined as a series of triangle “floor tiles” with side walls, the collision constraints in MILP is defined such that a trajectory point on the surface of an obstacle is not considered a collision, without a slight overlap between the triangle “floor tiles.” FIG. 8 shows valid trajectories 80 for underconstrained MILP problems without terrain overlap, that go under and between the terrain obstacles. FIG. 9 shows a trajectory 82 under a receding horizon with high altitude cost and no bounding walls. A trajectory shown in FIG. 8 is considered valid by the MILP solver since it does not violate any collision constraints. The apparent solution is to include a finite but small overlap in the definition of terrain blocks, as illustrated in FIG. 10. Similarly, a trajectory in the form of FIG. 9 is equally valid because the solution runs along the edge of the terrain blocks and does not violate any collision constraints. This type of solution is common in a receding horizon context with a high altitude weight value, because seeking a lower altitude would have a higher reward than getting closer to the destination. The solution is to construct four barrier planes surrounding the terrain map. Finally, for a threat avoidance such as the one shown in FIG. 7, due to the way that the obstacles and safe zones are modeled as (18) and (26), respectively, the “front faces,” or the surfaces of the obstacles that face the threat, are considered to be both inside the safe zones and outside the obstacles at the same time. The solution to this problem is to move the planar “front faces” of the safe zone slightly inward.

[0090] Up to this point, this description has not yet addressed how one goes about implementing the MILP planned trajectory such as the trajectory 52 shown FIG. 6, on an actual flight trajectory. This is where we need to consider the interfaces between MILP and other components in the actual flight hardware and software. In the above description, we have briefly considered the time discretization of a continuous time LTI system leading to equation (6). The issue that the time discretization introduces is the lack of continuity between the points in the planned trajectory. As a result, the obstacle as well as other constraints that are specified in the problem formulation are only enforced at these collocation points in the planned trajectory, and not in-between. In the case of obstacle avoidance, in particular, an optimal solution tends to be one that barely skirts the obstacles in order to minimize the objective function, as FIG. 7 illustrates. Therefore, if one were to follow a series of straight-lines that connect the collocation points, one is likely to collide with the obstacles. The solution to the problem lies in modeling the obstacles as larger than their actual sizes to account for the distance between the collocation points. This technique also addresses a similar issue that arises for a vehicle that is not a point mass. Each obstacle can be similarly expanded according to the dimensions of the vehicle. In addition, in a threat avoidance formulation, one must take care to expand the boundaries of the obstacles while at the same time contract the boundaries of the safe zones by the same amount, the logic being that a non-point-mass vehicle makes it easier to collide with obstacles and more difficult to evade detection. In a more advanced implementation, the vehicle would have one dimension for obstacle avoidance purposes and a separate and smaller one for threat avoidance.

[0091] Another fundamental issue that arises is the mechanism with which a vehicle follows the trajectory produced by the MILP problem. This issue is closely related to the selection of the length of period for time discretization, whose tradeoffs are an increased problem size for the same planning horizon vs. a sparser separation of trajectory points. The simplest way to implement a MILP trajectory is to have a waypoint follower track the planned trajectory points one after another. A more advanced technique would involve a nonlinear outer loop that considers the equations of motion as well as the dynamics of the vehicle. We believe that a combination of this advanced nonlinear technique together with a MILP-based approach to path planning provides the best overall closed-loop solution for autonomous nap-of-the-earth flight. An important element of the interface between the MILP path planner and other system components is the input to the MILP path planner. Since the algorithm presented in this description primarily performs the obstacle avoidance function, there must be means to incorporate obstacle data into a MILP-based path planner so that the appropriate MILP problem can be constructed. Most likely, the information would come from an off-line database, processed data from an optical sensor, or a combination of both. However, it is paramount for any constraints to a MILP problem to be as relevant as possible. Therefore, there must be methods devised specifically to select the obstacles that are most relevant to the current MILP problem, that allows a tradeoff of problem size with levels of detail in modeling the environment.

[0092] This invention extends a basic three-dimensional MILP obstacle avoidance path planning algorithm to perform terrain flight where staying close to the terrain has just as much priority as avoiding collision with obstacles, and introduces a novel threat avoidance formulation designed to minimize visibility exposure to known threats through proper path planning. Examples of MILP generated trajectories flying over a terrain and avoiding obstacles in both a fixed and receding horizon context have been described, as well as a threat avoidance scenario with multiple threats and obstacles.

[0093] The described embodiment of this invention uses a receding horizon approach to solve longer timescale MILP trajectory planning problem, but is restricted to a single vehicle with static obstacles and terrain in three dimensions, and does not have special means to avoid entrapment behind large obstacles. Constraints to the problem have been expressed explicitly in matrix form.

[0094] An example problem formulation for optimal trajectory generation in terms of a Mixed Integer Linear Program (MILP) will now be described. We first formulate a simple optimal trajectory generation problem in MILP, then build on it to expand the scope of our problem to include receding horizon, nap-of-the-earth flight, visibility constraints, and safety guarantees. We also include a description of an algorithm to calculate visibility regions.

[0095] The Basic Trajectory Planning LP Problem starts with approximate linear plant dynamics:

[0096] $\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{z}\\ {\stackrel{.}{\upsilon }}_x\\ {\stackrel{.}{\upsilon }}_y\\ {\stackrel{.}{\upsilon }}_z\end{array}\right]=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ {\upsilon }_x\\ {\upsilon }_y\\ {\upsilon }_z\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{u}_x\\ u_y\\ u_z\end{array}\right]$

[0097] The problem is formulated as a fixed arrival time problem. Pick a time step ts=TotalTime=N, where N is the number of time steps, and TotalTime is the time in which the vehicle will be driven from its initial state to final state. Then discretize the above dynamics so that:
s[k+1]=As[k]+Bu[k]
where:

[0098] $s=\left[\begin{array}{c}x\\ y\\ z\\ {\upsilon }_x\\ {\upsilon }_y\\ {\upsilon }_z\end{array}\right],u=\left[\begin{array}{c}{u}_x\\ u_y\\ u_z\end{array}\right]$

[0099] Based on the following initial and final constraints:
s₀=[x₀ y₀ z₀ v_x0 v_y0 v_z0]^T
s_F=[x_F y_F z_F v_xF v_yF v_zF]^T
and assuming the notation s_k=s[k]; u_k=u[k−1], we have the following set of equations:

[0100] $s_1={\mathrm{As}}_0+{\mathrm{Bu}}_1$ $s_2=A^2{s}_0+{\mathrm{ABu}}_1+{\mathrm{Bu}}_2$ $s_3=A^3{s}_0+A^2{\mathrm{Bu}}_1+{\mathrm{ABu}}_2+{\mathrm{Bu}}_3$ $⋮$ $s_k=A^k{s}_0+A^{k-1}{\mathrm{Bu}}_1+\cdots +{\mathrm{ABu}}_{k-1}+{\mathrm{Bu}}_k$

[0101] Since s_N=s_F, we have:
S_F=A^Ns₀+A^N−1Bu₁+ . . . +ABu_N−1+Bu_N

[0102] Or in matrix form:

[0103] $\begin{array}{cccc}\underbrace{\left[A^{N-1}B\cdots \mathrm{AB}B\right]}& \underbrace{\left[\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}\right]}& =& \underbrace{s_F-A^N{s}_0}\\ D_A& u& =& D_B\end{array}$

[0104] Next consider the Energy Cost Function Slack Variables. The standard linear programming (LP) problem has the following form:

[0105] $\underset{x}{\min }f\left(x\right)=c^{T}x$ $b_L\le \mathrm{Ax}\le b_U$ $x_L\le x\le x_U$
but so far we have only derived the following:
D_B≦D_Au≦D_B

[0106] One way to implement a minimum-time, minimum energy function is to define the cost function as:

[0107] $\begin{array}{c}f\left(x\right)=r\left({u_1}_1+\cdots +{u_N}_1\right)+q\left(d_1\left(s_1,s_F\right)+\cdots +d_1\left(s_{N-1},s_F\right)\right)\\ =r\left({\begin{array}{c}{u}_{1x}\\ u_{1y}\\ u_{1z}\end{array}}_1+\cdots +{\begin{array}{c}{u}_{\mathrm{Nx}}\\ u_{\mathrm{Ny}}\\ u_{\mathrm{Nz}}\end{array}}_1\right)+q\left({\begin{array}{c}{x}_1-x_F\\ y_1-y_F\\ z_1-z_F\end{array}}_1+\cdots +{\begin{array}{c}{x}_{N-1}-x_F\\ y_{N-1}-y_F\\ z_{N-1}-z_F\end{array}}_1\right)\\ =r\left(u_{1x}+u_{1y}+u_{1z}+\cdots +u_{\mathrm{Nx}}+u_{\mathrm{Ny}}+u_{\mathrm{Nz}}\right)+\\ q(x_1-x_F+y_1-y_F+z_1-z_F+\cdots +x_{N-1}-x_F+\\ y_{N-1}-y_F+z_{N-1}-Z_F)\end{array}$
where: d₁(s₁, s₂) is the 1-norm distance between 2 points.

[0108] One way to implement such an energy function as a linear function is to introduce slack variables w and v such that
∀iε{1 . . . N}, jε{1 . . . N−1}, kε{x,y,z}:
−v_ik≦u_ik≦v_ik
−w_jk≦s_jk−s_Fk≦w_jk
(note: s_ix=x_i, s_iy=y_i, s_iz=z_i)

[0109] This essentially makes,
∀iε{1 . . . N}, jε{1 . . . N−1}, kε{x,y,z},
v_ik≧∥u_ik∥≧0
w_jk≧∥s_jk−s_Fk∥≧0

[0110] By making the cost vector and state vector:

[0111] $c=\left[\begin{array}{c}{0}_{3\times 1}\\ ⋮\\ 0_{3\times 1}\\ r_{3\times 1}\\ ⋮\\ r_{3\times 1}\\ q_{3\times 1}\\ ⋮\\ q_{3\times 1}\end{array}\right]=\left[\begin{array}{c}{0}_{3N\times 1}\\ r_{3N\times 1}\\ q_{3\left(N-1\right)\times 1}\end{array}\right],x=\left[\begin{array}{c}{u}_1\\ ⋮\\ u_N\\ {\upsilon }_1\\ ⋮\\ {\upsilon }_N\\ w_1\\ ⋮\\ w_{N-1}\end{array}\right]$

[0112] We are essentially minimizing v_ik and w_ik such that
v_ik=∥u_ik∥ and w_ik=∥s_ik−s_Fk∥,
or equivalently,

[0113] ${\upsilon }_i={\begin{array}{c}{u}_{\mathrm{ix}}\\ u_{\mathrm{iy}}\\ u_{\mathrm{iz}}\end{array}}_1\mathrm{and}{w}_i={\begin{array}{c}{x}_i-x_F\\ y_i-y_F\\ z_i-z_F\end{array}}_1.$

[0114] Therefore we have:

[0115] $\begin{array}{c}f\left(x\right)=c^{T}x\\ =r\left({\upsilon }_1+\cdots +{\upsilon }_N\right)+q\left(w_1+\cdots +w_{N-1}\right)\\ =r\left({\begin{array}{c}{u}_{1x}\\ u_{1y}\\ u_{1z}\end{array}}_1+\cdots +{\begin{array}{c}{u}_{\mathrm{Nx}}\\ u_{\mathrm{Ny}}\\ u_{\mathrm{Nz}}\end{array}}_1\right)+\\ q\left({\begin{array}{c}{x}_1-x_F\\ y_1-y_F\\ z_1-z_F\end{array}}_1+\cdots +{\begin{array}{c}{x}_{N-1}-x_F\\ y_{N-1}-y_F\\ z_{N-1}-z_F\end{array}}_1\right)\\ =r\left({u_1}_1+\cdots +{u_N}_1\right)+q\left(d_1\left(s_1,s_F\right)+\cdots +d_1\left(s_{N-1},s_F\right)\right)\end{array}$

[0116] To actually implement the above cost function in matrix form, we observe that for the following inequalities:
−v_ik≦u_ik≦v_ik
−w_jk≦s_jk−s_Fk≦w_jk

[0117] The equivalent is:
u_ik−v_ik≦0
−u_ik−v_ik≦0
(s_jk−s_Fk)−w_jk≦0
−(s_jk−s_Fk)−w_jk≦0

[0118] If we sum up
k:∀kε{x,y},
then we have:
u_i−v_i≦0_3×1
−u_i−v_i≦0_3×1
(Ws_j−Ws_F)−w_j≦0_3×1
−(Ws_j−Ws_F)−w_j≦0_3×1
where

[0119] $W=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

[0120] Since we know:
s_k=A^ks₀+A^k−1Bu₁+ . . . +ABu_k−1+Bu_k

[0121] We have, after some rearrangements:
u_i−v_i≦0_3×1
−u_i−v_i≦0_3×1
(WA^j−1Bu₁+ . . . +WABu_j-1+WBu_j)−w_j≦W(s_F−A^js₀)
−(WA^j−1Bu₁+ . . . +WABu_j-1+WBu_j)−w_j≦−W(s_F−A^js₀)

[0122] Or in matrix form:

[0123] $[\begin{array}{ccc}{I}^{3\times 3}& & \\ & ⋰& \\ & & I^{3\times 3}\end{array}][\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}]-\left[\begin{array}{ccc}{I}^{3\times 3}& & \\ & ⋰& \\ & & I^{3\times 3}\end{array}\right][\begin{array}{c}{\upsilon }_1\\ ⋮\\ {\upsilon }_N\end{array}]\le \hspace{1em}\left[\begin{array}{c}{0}_{3\times 1}\\ ⋮\\ 0_{3\times 1}\end{array}\right]-[\begin{array}{ccc}{I}^{3\times 3}& & \\ & ⋰& \\ & & I^{3\times 3}\end{array}][\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}]-[\begin{array}{ccc}{I}^{3\times 3}& & \\ & ⋰& \\ & & I^{3\times 3}\end{array}][\begin{array}{c}{\upsilon }_1\\ ⋮\\ {\upsilon }_N\end{array}]\le [\begin{array}{c}{0}_{3\times 1}\\ ⋮\\ 0_{3\times 1}\end{array}]\hspace{1em}\left[\begin{array}{cccc}WB& \cdots & 0& 0\\ ⋮& ⋰& 0& 0\\ W{A}^{N-2}B& \cdots & WB& 0\end{array}\right][\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}]-\left[\begin{array}{ccc}{I}^{3\times 3}& & \\ & ⋰& \\ & & I^{3\times 3}\end{array}\right]\left[\begin{array}{c}{w}_1\\ ⋮\\ w_{N-1}\end{array}\right]\le \hspace{1em}\left[\begin{array}{c}W(s_F-{\mathrm{As}}_0\\ ⋮\\ W(s_F-A^{N-1}{s}_0\end{array}\right]-\left[\begin{array}{cccc}WB& \cdots & 0& 0\\ ⋮& ⋰& 0& 0\\ W{A}^{N-2}B& \cdots & WB& 0\end{array}\right][\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}]-\hspace{1em}\left[\begin{array}{ccc}{I}^{3\times 3}& & \\ & ⋰& \\ & & I^{3\times 3}\end{array}\right]\left[\begin{array}{c}{w}_1\\ ⋮\\ w_{N-1}\end{array}\right]\le -\left[\begin{array}{c}W(s_F-{\mathrm{As}}_0\\ ⋮\\ W(s_F-A^{N-1}{s}_0\end{array}\right]$

[0124] If we define:

[0125] $W_A=\left[\begin{array}{cccc}WB& \cdots & 0& 0\\ ⋮& ⋰& 0& 0\\ W{A}^{N-2}B& \cdots & WB& 0\end{array}\right],W_B=\left[\begin{array}{c}W\left(s_F-{\mathrm{As}}_0\right)\\ ⋮\\ W\left(s_F-A^{N-1}{s}_0\right)\end{array}\right]$ $u=\left[\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}\right],\upsilon =\left[\begin{array}{c}{\upsilon }_1\\ ⋮\\ {\upsilon }_N\end{array}\right],w=\left[\begin{array}{c}{w}_1\\ ⋮\\ w_{N-1}\end{array}\right],x=\left[\begin{array}{c}u\\ \upsilon \\ w\end{array}\right]$

[0126] And introduce an upper bound u_max to u such that ∀i, k:
−u_max≦u_ik≦u_max

[0127] Then the LP problem formulation becomes:

[0128] $\begin{array}{c}\underset{x}{\min }f\left(x\right)={\left[\begin{array}{c}{0}_{3N\times 1}\\ r_{3N\times 1}\\ q_{3\left(N-1\right)\times 1}\end{array}\right]}^T\left[\begin{array}{c}u\\ \upsilon \\ w\end{array}\right]\\ [\begin{array}{c}{D}_B\\ -{\infty }_{3N\times 1}\\ -{\infty }_{3N\times 1}\\ -{\infty }_{3\left(N-1\right)\times 1}\\ -{\infty }_{3\left(N-1\right)\times 1}\end{array}]\le [\begin{array}{ccc}{D}_A& 0& 0\\ I^{3N\times 3N}& -I^{3N\times 3N}& 0\\ -I^{3N\times 3N}& -I^{3N\times 3N}& 0\\ W_A& 0& -I^{3\left(N-1\right)\times 3\left(N-1\right)}\\ -W_A& 0& -I^{3\left(N-1\right)\times 3\left(N-1\right)}\end{array}]\\ [\begin{array}{c}u\\ \upsilon \\ w\end{array}]\le [\begin{array}{c}{D}_B\\ 0_{3N\times 1}\\ 0_{3N\times 1}\\ W_B\\ -W_B\end{array}]\\ [\begin{array}{c}-u_{\max 3N\times 1}\\ -{\infty }_{3N\times 1}\\ -{\infty }_{3\left(N-1\right)\times 1}\end{array}]\le [\begin{array}{c}u\\ \upsilon \\ w\end{array}]\le \left[\begin{array}{c}{u}_{\max 3N\times 1}\\ {\infty }_{3N\times 1}\\ {\infty }_{3\left(N-1\right)\times 1}\end{array}\right]\end{array}$

[0129] The velocity constraints consist of an octagon-shaped bound on the velocity on the x-y plane and a max bound on the z component:
−v_h,max≦v_x[k]≦v_h,max
−v_h,max≦v_y[k]≦v_h,max
−v_z,max≦v_z[k]≦v_z,max
−√{square root over (2)}v_h,max≦v_x[k]+v_y[k]≦√{square root over (2)}v_h,max
−√{square root over (2)}v_h,max≦v_x[k]−v_y[k]≦√{square root over (2)}v_h,max

[0130] Since
v_x[k]=[0 0 0 1 0 0]s_k
v_y[k]=[0 0 0 0 1 0]s_k
v_z[k]=[0 0 0 0 0 1]s_k
we can have the following constraints:

[0131] $-\left[\begin{array}{c}{\upsilon }_{h,\max }\\ {\upsilon }_{h,\max }\\ {\upsilon }_{z,\max }\\ {\upsilon }_{h,\max }\\ {\upsilon }_{h,\max }\end{array}\right]\le \left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& \frac{1}{\sqrt{2}}& \frac{1}{\sqrt{2}}& 0\\ 0& 0& 0& \frac{1}{\sqrt{2}}& -\frac{1}{\sqrt{2}}& 0\end{array}\right]s_k\le \left[\begin{array}{c}{\upsilon }_{h,\max }\\ {\upsilon }_{h,\max }\\ {\upsilon }_{z,\max }\\ {\upsilon }_{h,\max }\\ {\upsilon }_{h,\max }\end{array}\right]$

[0132] We set:

[0133] $V=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& \frac{1}{\sqrt{2}}& \frac{1}{\sqrt{2}}& 0\\ 0& 0& 0& \frac{1}{\sqrt{2}}& -\frac{1}{\sqrt{2}}& 0\end{array}\right],V_{\max }=\left[\begin{array}{c}{\upsilon }_{h,\max }\\ {\upsilon }_{h,\max }\\ {\upsilon }_{z,\max }\\ {\upsilon }_{h,\max }\\ {\upsilon }_{h,\max }\end{array}\right]$

[0134] Since we know:
s_k=A^ks₀+A^k-1Bu₁+ . . . +ABu_k−1+Bu_k

[0135] We have, for each time k:
−V_max≦VA^ks₀+VA^k-1Bu₁+ . . . +VABu_k−1+VBu_k≦V_max

[0136] Or in matrix form after rearrangements:

[0137] $-V_{\max }-V{A}^k{s}_0\le \left[V{A}^{k-1}B\cdots V\mathrm{AB}V{\mathrm{Bu}}_k\right]\left[\begin{array}{c}{u}_1\\ ⋮\\ u_k\end{array}\right]\le V_{\max }-V{A}^k{s}_0$

[0138] The composite form would be:

[0139] $\begin{array}{cc}\left[\begin{array}{c}-V_{\max }-V{\mathrm{As}}_0\\ ⋮\\ -V_{\max }-V{A}^N{s}_0\end{array}\right]\le \left[\begin{array}{cccc}VB& 0& \cdots & 0\\ V\mathrm{AB}& VB& ⋰& 0\\ ⋮& & ⋰& 0\\ V{A}^{N-1}B& \cdots & V\mathrm{AB}& VB\end{array}\right][\begin{array}{c}{u}_1\\ ⋮\\ u_k\end{array}]\le \hspace{1em}[\begin{array}{c}{V}_{\max }-V{\mathrm{As}}_0\\ ⋮\\ V_{\max }-V{A}^N{s}_0\end{array}]\hspace{1em}& \end{array}$

[0140] Or in short form:
V_BL≦V_Au≦V_BU

[0141] Next consider the Obstacle Integer Constraints. Assume a rectangular box in 3-D defined by:
x_min≦x≦x_max
y_min≦y≦y_max
z_min≦z≦z_max

[0142] We introduce binary variables be b_*ε{0, 1} and a number M>>0 sufficiently large such that:
x−Mb_xmin≦x_min
y−Mb_ymin≦y_min
z−Mb_zmin≦z_min
−x−Mb_xmax≦−x_max
−y−Mb_ymax≦−y_max
−z−Mb_zmax≦−z_max
b_xmin+b_ymin+b_zmin+b_xmax+b_ymax+b_zmax≦5

[0143] The above constraints ensure that (x, y, z) is outside the box because the last inequality requires at least one of the binary variables b_* to be zero, which negates at least one of the inequalities that we established for a point to be inside the box, which also implies that the point (x, y, z) lies outside at least one of the six box boundaries.

[0144] The object above is described by 6 Euclidean planar inequalities. The above concept can be generalized to an object that may be defined by non-Euclidean planar inequalities. A non-Euclidean plane in 3-D can be described by the following equation:
z=ax+by+d

[0145] We can turn the non-Euclidean planar inequalities into integer obstacle constraints:
z≦ax+by+d_min−ax−by+z−Mb_dmin≦d_min
z≧ax+by+d_maxax+by−z−Mb_dmax≦−d_max

[0146] For ease of manipulation, we turn the Euclidean as well as non-Euclidean inequalities described above into matrix form by introducing the following auxiliary matrices and variables:
θ_xmin=[1 0 0 0 0 0], θ′_xmin=x_min
θ_xmax=[−1 0 0 0 0 0], θ′_xmax=−x_max
θ_ymin=[0 1 0 0 0 0], θ′_ymin=y_min
θ_ymax=[0 −1 0 0 0 0], θ′_ymax=−y_max
θ_zmin=[0 0 1 0 0 0], θ′_zmin=z_min
θ_zmax=[0 0 −1 0 0 0], θ′_zmax=−z_max
θ_dmin=[−a −b 1 0 0 0], θ′_dmin=d_min
θ_dmax=[a b −1 0 0 0], θ′_dmax=−d_max
such that for any n-sided arbitrary convex polyhedral at time k, we have the following inequalities:

[0147] $\begin{array}{ccc}{\theta }_1{s}_k-{\mathrm{Mb}}_{k_1}& \le & {\theta }_1^{\prime }\\ & ⋮& \\ {\theta }_n{s}_k-{\mathrm{Mb}}_{k_n}& \le & {\theta }_n^{\prime }\\ b_{k_1}+\cdots +b_{k_n}& \le & n-1\end{array}$

[0148] If we set:

[0149] $\theta =\left[\begin{array}{c}{\theta }_1\\ ⋮\\ {\theta }_n\end{array}\right],b_k=\left[\begin{array}{c}{b}_{k_1}\\ ⋮\\ b_{k_n}\end{array}\right],{\theta }^{\prime }=\left[\begin{array}{c}{\theta }_1^{\prime }\\ ⋮\\ {\theta }_n^{\prime }\end{array}\right]$

[0150] The above inequalities become:
θs_k−Mb_k≦θ′
b_k1+ . . . +b_kn≦n−1

[0151] We illustrate the above with a simple example. Suppose we have a pyramid (n=5) defined by the following inequalities (as shown in FIG. 11):
z<−x+1
z<x+1
z<−y+1
z<y+1
z>0

[0152] For a point (x, y, z) to be outside the pyramid, at least 4 of the following 5 inequalities must be true:
z≧−x+1
z≧x+1
z≧−y+1
z≧y+1
z≦0
we have the following constraints:
−x−z−Mb₁≦−1
x−z−Mb₂≦−1
−y−z−Mb₃≦−1
y−z−Mb₄≦−1
z−Mb₅≦0
b₁+b₂+b₃+b₄+b₅≦5−1=4
b₁,b₂,b₃,b₄,b₅ε{0,1}

[0153] We can arrive at the same conclusion by using the auxiliary matrices and variables:

[0154] $z\ge -x+1\iff a=-1,b=0,d_{\max }=1\iff \underset{{\theta }_1}{\underbrace{\left[\begin{array}{cccccc}-1& 0& -1& 0& 0& 0\end{array}\right]}}{s}_k-{\mathrm{Mb}}_{k_1}\le \underset{\underset{{\theta }_1^{\prime }}{︸}}{-1}$ $z\ge x+1\iff a=1,b=0,d_{\max }=1\iff \underset{{\theta }_2}{\underbrace{\left[\begin{array}{cccccc}1& 0& -1& 0& 0& 0\end{array}\right]}}{s}_k-{\mathrm{Mb}}_{k_2}\le \underset{\underset{{\theta }_2^{\prime }}{︸}}{-1}$ $z\ge -y+1\iff a=0,b=-1,d_{\max }=1\iff \underset{{\theta }_1}{\underbrace{\left[\begin{array}{cccccc}1& -1& -1& 0& 0& 0\end{array}\right]}}{s}_k-{\mathrm{Mb}}_{k_3}\le \underset{\underset{{\theta }_3^{\prime }}{︸}}{-1}$ $z\ge y+3\iff a=0,b=1,d_{\max }=1\iff \underset{{\theta }_4}{\underbrace{\left[\begin{array}{cccccc}0& 1& -1& 0& 0& 0\end{array}\right]}}{s}_k-{\mathrm{Mb}}_{k_4}\le \underset{\underset{{\theta }_4^{\prime }}{︸}}{-1}$ $z\le 0\iff z_{\min }=0\iff \underset{\underset{{\theta }_5}{︸}}{\left[\begin{array}{cccccc}0& 0& 1& 0& 0& 0\end{array}\right]}{s}_k-{\mathrm{Mb}}_{k_5}\le \underset{\underset{{\theta }_4}{︸}}{0}$

[0155] In matrix form, this becomes:

[0156] $\underset{\underset{\theta }{︸}}{\left[\begin{array}{cccccc}-1& 0& -1& 0& 0& 0\\ 1& 0& -1& 0& 0& 0\\ 0& -1& -1& 0& 0& 0\\ 0& 1& -1& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]}{s}_k-M\underset{b_k}{\underbrace{\left[\begin{array}{c}{b}_{k_1}\\ b_{k_2}\\ b_{k_3}\\ b_{k_4}\\ b_{k_5}\end{array}\right]}}\le \underset{{\theta }^{\prime }}{\underbrace{\left[\begin{array}{c}-1\\ -1\\ -1\\ -1\\ 0\end{array}\right]}}$

[0157] Recall from previous sections that
s_k=A^ks₀+A^k−1Bu₁+ . . . +ABu_k−1+Bu_k

[0158] The inequality above can be rewritten as:
θA^ks₀+θA^k−1Bu₁+ . . . +θABu_k−1+θBu_k−Mb_k≦θ′

[0159] Or in matrix form:

[0160] $\left[\begin{array}{cccc}\theta A^{k-1}B& \cdots & \theta \mathrm{AB}& \theta B\end{array}\right]\left[\begin{array}{c}{u}_1\\ ⋮\\ u_k\end{array}\right]-{\mathrm{Mb}}_k\le {\theta }^{\prime }-\theta A^k{s}_0$

[0161] If we sum up all the inequalities for k time steps, we have the following composite constraints:

[0162] $[\begin{array}{cccc}\theta B& 0& \cdots & 0\\ \theta \mathrm{AB}& \theta B& ⋰& ⋮\\ ⋮& & ⋰& 0\\ \theta A^{N-1}B& \cdots & \theta \mathrm{AB}& \theta B\end{array}][\begin{array}{c}{u}_1\\ ⋮\\ u_N\end{array}]+[\begin{array}{cccc}-M{I}^{n\times n}& 0& \cdots & 0\\ 0& ⋰& ⋰& ⋮\\ ⋮& ⋰& ⋰& 0\\ 0& \cdots & 0& -M{I}^{n\times n}\end{array}][\begin{array}{c}{b}_1\\ ⋮\\ b_N\end{array}]\le [\begin{array}{c}{\theta }^{\prime }-\theta {\mathrm{As}}_0\\ ⋮\\ {\theta }^{\prime }-\theta A^N{s}_0\end{array}]\hspace{1em}[\begin{array}{cccc}{\left[1\cdots 1\right]}_{1\times n}& 0& \dots & 0\\ 0& ⋰& ⋰& ⋮\\ ⋮& ⋰& ⋰& 0\\ 0& \dots & 0& {\left[1\dots 1\right]}_{1\times n}\end{array}][\begin{array}{c}{b}_1\\ ⋮\\ b_N\end{array}]\le [\begin{array}{c}n-1\\ ⋮\\ n-1\end{array}]$

[0163] If we define:

[0164] ${\theta }_{A_1}=\left[\begin{array}{cccc}\theta B& 0& \cdots & 0\\ \theta \mathrm{AB}& \theta B& ⋰& ⋮\\ ⋮& & ⋰& 0\\ \theta A^{N-1}B& \cdots & \theta \mathrm{AB}& \theta B\end{array}\right],{\theta }_{B_1}=\left[\begin{array}{c}{\theta }^{\prime }-\theta {\mathrm{As}}_0\\ ⋮\\ {\theta }^{\prime }-\theta A^N{s}_0\end{array}\right],b=[\begin{array}{c}{b}_1\\ ⋮\\ b_N\end{array}]$ ${\theta }_{A_2}=[\begin{array}{cccc}{\left[1\cdots 1\right]}_{1\times n}& 0& \dots & 0\\ 0& ⋰& ⋰& ⋮\\ ⋮& ⋰& ⋰& 0\\ 0& \dots & 0& {\left[1\dots 1\right]}_{1\times n}\end{array}]$