A new framework for route guidance, as part of a path decision support tool, for off-road driving scenarios is presented in this paper. The algorithm accesses information gathered prior to and during a mission which are stored as layers of a central map. The algorithm incorporates a priori knowledge of the low resolution soil and elevation information and real-time high-resolution information from on-board sensors. The challenge of high computational cost to find the optimal path over a large-scale high-resolution map is mitigated by the proposed hierarchical path planning algorithm. A dynamic programming (DP) method generates the globally optimal path approximation based on low-resolution information. The optimal cost-to-go from each grid cell to the destination is calculated by back-stepping from the target and stored. A model predictive control algorithm (MPC) operates locally on the vehicle to find the optimal path over a moving radial horizon. The MPC algorithm uses the stored global optimal cost-to-go map in addition to high resolution and locally available information. Efficacy of the developed algorithm is demonstrated in scenarios simulating static and moving obstacles avoidance, path finding in condition-time-variant environments, eluding adversarial line of sight detection, and connected fleet cooperation.

## Introduction

Frequent stops and goes of off-road vehicles not only increase their energy use but also slow or interrupted cruise can put these vehicles in vulnerable situations. Some slow-downs or stop and goes occur due to lack of awareness about the area. In off-road driving, it is critical to have advanced evaluation of the topology of upcoming terrain, since uninformed decisions can cause longer delays or potentially even loss of a vehicle in a geographic trap. Advances in connected vehicle technologies and in real-time access to computational clouds, which can learn from data and assess the situation, create new opportunities for more efficient fleet management. Wirelessly connected vehicles can talk between themselves, exchange information, and automatically learn from each other's trip experience. Availability of precise three-dimensional (3D) elevation profiles and soil trafficability and vegetation maps presents an opportunity to further improve off-road navigation.

The main objective of this paper is to develop and evaluate a decision support tool for route guidance of manned or unmanned convoys in off-road environments. Our proposed algorithm relies on recent advances in vehicular connectivity that enables individual vehicles to cooperate and exchange information within a fleet and to communicate to backend computational clouds. This provides more autonomy to each individual vehicle while offering some of the benefits of cooperative motion. For instance, in off-road scenarios with unknown terrain, each vehicle can communicate to a backend server perceived soil conditions, its chosen path, and its roll, yaw, and pitch. This generates a dynamic map of the unexplored terrain. Vehicles within a convoy can learn from the information gathered by the other vehicles and gradually reroute to faster, safer, or more energy efficient routes. Moreover, information from individual vehicles can be augmented with terrain, soil, and vegetation maps that are becoming available at higher resolution today. Previous work done in our group, by Zhang et al., had explored the role of terrain preview in energy management of on-road hybrid electric vehicles [13]; this paper addresses use of terrain information for off-road driving.

Off-road driving presents significant challenges when compared to on-road. Unlike motion of on-road vehicles, which are constrained on the road network, motion of off-road vehicles has more routes to consider for the optimization routines. Jackel et al. [4] reported the state of the art in the autonomous unmanned ground vehicle navigation. Access to computational clouds has advanced since then and needs to be reinvestigated. Detection of near-field environment, path planning, and tracking has been an area of immense interest since the 1990s [57]. These algorithms rely on a preplanned global path. Efforts have been made for improving the ride quality in off-road driving based on roughness of the terrain [8]. Additionally, the path optimization routine needs to efficiently access and handle 3D terrain information. In recent years, considerable effort has been spent on detecting and classifying the terrain [7,911]. Recent terrain mapping efforts can largely be classified into two approaches: developing a terrain model with obstacle constraints based on vehicle sensors and using high-resolution terrain information collected a priori. A gap exists in the current literature, which efficiently combines both sensor and terrain data to better inform the decision process and automate the route optimization routine given start and end points.

This paper investigates an efficient hierarchical approach for vehicle path planning across vast off-road terrains. A global optimization routine operates on low-resolution terrain information that is available prior to the mission to find the outlines of a best path. A local optimization routine builds on the results of the global routine and incorporates high-resolution terrain maps, and obstacle detection algorithms to fine tune the optimal prescribed route. Easy incorporation of a variety of informational maps to the proposed algorithm is also demonstrated. We will also incorporate in our algorithm soil conditions from publicly available maps for the continental U.S. For military applications, an important attribute for the safety of vehicles is the visibility of the vehicle from the communication towers or avoiding line of sight of adversarial observation towers. We will use the terrain maps to generate a visibility map and incorporate it in our optimization routine.

Various algorithms have been proposed for cooperative path planning of unmanned aerial vehicles [1216] in recent years. Constraints imposed due to the terrain make the translation of such algorithms for ground vehicles inefficient. Algorithms have been proposed for collaborative path finding for autonomous vehicles [1720]. For many critical missions like transportation of military cargo, personnel and mined ore, it is desirable that the vehicles are commandeered real time by humans. For remote fleet operation, assisting the vehicle operator with updated route guidance can be a critical difference. A framework for efficiently creating, updating, and sharing route critical information among fleet vehicles and a central server, and using the information to update the prescribed routes, is presented in this paper.

The paper is structured as follows: Sec. 2 lays out our proposed path planning architecture and discusses various layers of information maps that we use. Global path planning based on dynamic programming (DP) is discussed in Sec. 3 followed by a Model Predictive local route guidance algorithm described in Sec. 4. Section 5 described a decentralized Model Predictive route guidance for a fleet of connected vehicles. The performance of the proposed algorithms is illustrated in simulation case studies in Sec. 6 followed by conclusions in Sec. 7.

## Hierarchical Route Guidance Framework

### Architecture.

An overview of the software architecture and a flowchart of the developed hierarchical approach are shown in Fig. 1. A dynamic programming routine is used to generate the optimal cost-to-go map using low-resolution elevation information and soil trafficability information. The optimal cost-to-go map is a map whose value at each grid point is the optimal cost, to go from that grid point to the target destination and is discussed in more detail in Sec. 3. A model predictive-based local optimization routine uses the stored optimal cost-to-go map, high-resolution terrain, and on board sensor information to find the optimal path from the vehicle location to the boundary of its planning horizon. The path thus generated is tracked by low-level vehicle controller.

On-road applications typically optimize the route based on existing known road networks but in off-road application, no such predefined road network exists, which makes the optimization task even more challenging. The challenges of coarse resolution of the available terrain (and other informational) maps and high computational costs to find the optimal path over a large-scale high-resolution map for off-road applications motivated the development of the hierarchical framework. For dynamic programming, addition of x vertices requires exploration of 8x additional routes and increases the computing cost by roughly 10x times [21]. Our analysis of MP algorithm showed that doubling the prediction horizon steps leads to a four-fold increase in compute time. A typical multilevel optimization will calculate an optimized trajectory followed by a low-level controller. The optimized trajectory from high level may not be updated in real time due to computational cost especially for long-term/large-scope planning issue. Dynamically recalculating the optimal path for a fleet, which all have the same destination, but different real-time position, may be more challenging. The cost-to-go-based method, that we propose, does neither need to calculate the optimal path globally nor need memory to save the path. The backward-based cost-to-go could be used by all vehicles at different locations as long as they have the same target position. An offline optimization routine uses coarse resolution information to generate a globally optimal cost-to-go map. The first level of optimization is for slow vehicle dynamics, which can be achieved with low-resolution maps. For faster dynamics, each ground vehicle performs online optimization to find the optimal local path using high resolution information and the cost-to-go map. Since the on-board sensors can generate high-resolution information within the sensors' horizon, it is natural to partition the optimization into two levels. As an example, when traveling from point A to point B, which are many miles apart, it is important to know if there are hills or ravines along the path so that initial optimization can put a large cost to those areas. As finer details like presence of rocks become available from the onboard sensors, the local path optimization routine can factor those in and generate the most optimal path. The optimization routine uses a back-end server to obtain, store, and share relevant information among them.

### Information Layers.

An efficient path planning algorithm is predicated on efficient handling of the information database. A novel method of integrating the information layers to the decision process is presented here. Information collated before and during the mission are stored as maps. Multiple maps, each representing an information layer, are stored on a backend server. Information stored on the server is accessible by the vehicles on the ground as well as by a central computation server. The route guidance routines use these stored maps to perform their respective optimization. All the maps are stored with a corresponding timestamp representing the latest update. The maps are categorized into two types. Static maps represent information layers that remain constant over full duration of the mission. Dynamics maps contain information layers that are either updated or generated during the mission. Examples of the maps we have used in this work are shown in Fig. 2.

• (a)

Elevation information: Various digital terrain models exist that provide topographic elevation data of bare terrain. Over the past 15 years, the elevation information of the U.S. National Map [22] was mapped by U.S. Geological Survey and presented in the National Elevation Dataset. The National Elevation Dataset bare earth elevation information for the entire U.S. is available for public download at medium resolution (every 10 or 30 m). Since 2015, under its more recent 3D elevation program, U.S. Geological Survey now provides higher resolution elevation data (every 1/9 arc-second and 1 m) for selected locations in the U.S. and plans to expand this dataset in the coming year [23]. We have selected a representative site where the higher resolution data are available and have successfully retrieved the elevation. For our study, we chose a 10 km2 area near Denver, CO. The chosen area has a rough terrain with two peaks, captured with high 1 m resolution of elevation data as shown in Fig. 2(a).

Other existing digital terrain models' elevation sources are through the Shuttle Radar Topographic Mapping (SRTM, every 30 m) and Advanced Spaceborne Thermal Emission and Reflection Radiometer (ASTER, every 30 m) programs available by NASA and Japan's Space Agency. In the past, we have also worked with Intermap Technologies— a company that provides high-resolution data (every 5 m) for a wide range of locations across the world.

• (b)

Soil trafficability: The U.S. Department of Agriculture publishes soil information for the entire contiguous U.S. [24]. Among the publicly available data, soil trafficability is the most pertinent information when planning for off-road driving. The soil trafficability layer is intended to rate the capacity of the soil to support military vehicles. Soil trafficability ratings account for soil strength, soil slipperiness, stickiness, stoniness, and slope. The rating class indicate the verbal description of the expectation to complete the task for the given military vehicle category, number of passes to complete the task and, the season of the year. Military vehicles are categorized into seven types for trafficability rating. Table 1 shows various trafficability ratings and the expected task completion value. A manual [25] provides a guideline to convert the verbal soil rating to a probabilistic task completion value as indicated in the second column of Table 1. The route guidance routine has been developed for deterministic maps. Therefore, the probabilistic task completion values are converted to corresponding numeric values for computational ease as shown in the third column of Table 1.

To run the optimization routines, for the area corresponding to terrain map in Fig. 2(a) we have extracted the soil trafficability data from the U.S. Department of Agriculture database as shown in Fig. 2(b).

• (c)

Line of sight (LoS): The concept of line of sight had been initially used in guided missiles development in the 1940s [26]. Recent efforts in tactical path-finding use the concept wherein safety of the unit is taken into account in path optimization by being out of the LoS or enemy range while remaining in the friendly LoS [2730]. In mountainous or hilly terrain, there are issues related to failure of radio communication. So, the concept of LoS can be extended to find the optimal path keeping a fleet within range of the radio tower of the base station effectively improving safety of the operation. Carver and Washtell [31] have explored a voxel-based modeling approach for rapid viewshed calculation. Here, we have used the line of sight as an extra information layer, which aids in the trajectory optimization. The elevation layer is used to find the grids, which are visible from known locations of adversarial watch towers.

Figure 2(c) shows areas in the map, which are visible from the known location of three fixed towers in white. Darkly shaded areas are invisible parts of the terrain. The location of the towers is shown in Fig. 2(a) with conical markers. matlab function voxelviewshed was used to translate the elevation information and the tower location to the visibility map.

## Global Route Guidance Via Dynamic Programming

At the highest level, the global route guidance routine generates a map with a cost attached to each grid point based on coarse resolution information layers available prior to deployment. Dynamic programming is a common and powerful method in solving dynamic decision-making problems that explores the entire map and stores the results of each search in a map. DP was therefore a natural choice for global route guidance routine. Based on Bellman's principle of optimality, DP divides the whole optimization problem into smaller subproblems, solves them recursively, and stores the intermediate results. Therefore, if a subproblem occurs again, rather than solving it again, DP just looks up the stored results, which would reduce the computation time significantly.

In computer science literature, Dijkstra's algorithm is one of the most important and useful methods for solving a path planning/shortest path problem, where the environment is often represented as a graph with vertices and edges, and Dijkstra's algorithm finds the minimum cost (e.g., length) between each vertex and the given vertex in a graph. With a Fibonacci heap implementation, the algorithm is asymptotically the fastest single source path finding algorithm for non-negative cost [21]. Dijkstra's algorithm has been usually described as a greedy algorithm in computer science and operations research. Sniedovich [32] connects Dijkstra's algorithm with dynamic programming, and argues that Dijkstra's algorithm is based on Bellman's principle of optimality and should be viewed as a dynamic programming method.

Here, we use a dynamic programming framework in order to obtain the cost-to-go approximation from all grid points to a destination. Our implementation is based on Dijkstra's algorithm with advanced data structure implementation to achieve better computational performance. The entire terrain can be seen as a graph composed of vertices. The area is divided into smaller square grids and each grid intersection represents a vertex. A vertex only connects to its eight neighboring vertices. For any given two vertices i and j, a cost function is defined as the cost spent for a vehicle to travel directly from vertex i to vertex j. Note that the path is assumed to be undirected, which means that the cost is the same to travel from vertex j to vertex i. Our cost function takes information from different layers such as distance, elevation changes, soil condition, and line of sight into account. The path cost from vertex i to vertex j is defined as follows:
$J(i,j)=∑k=14wkJk(i,j)J1(i,j)=‖(x(i),y(i),z(i)),(x(j),y(j),z(j))‖2J2(i,j)=1soil(i)+1soil(j)J3(i,j)=|z(i)−z(j)|J4(i,j)=visibility(j)$
(1)

where x, y, and z are the 3D coordinates, soil is our assigned numerical value in Table 1 based on soil trafficability rating, and visibility is the visibility of the vertex. Assigned numerical value of zero for invisible vertices and a large numerical value for visible vertices discourage passage through areas that are seen by the watch towers. Penalty weights on each term are wk. Therefore, J1 represents the distance cost, J2 encourages choosing soil patches with higher trafficability rating, J3 penalizes absolute elevation change, and J4 is the cost associated with LoS. DP finds the path, which minimizes the total cost from an origin to a final destination.

Due to the nature of off-road environment, there are terrain areas with extreme steep slope, which is beyond vehicle's dynamic driving capability. Therefore, hard constraints are also defined and included to avoid impossible climbs and descents
$slope(i,j)
(2)

If the slope violates the constraints, the vertices are considered not directly connecting with each other and the stage cost is set to infinity.

Having defined the cost function, Dijkstra's algorithm computes the minimum cost-to-go from each grid to the destination grid (also known as optimal cost-to-go) and stores the optimal results in the cost-to-go map shown in Fig. 2(d). In Fig. 2, the origin is located at the origin and the destination is at the far corner.

The choice of resolution or the size of each grid determines the computational complexity and the needed memory. With finer resolution, DP results would achieve higher precision; however, the memory usage grows exponentially. Finer resolution also better approximates any nonlinear elevation and soil variance between neighboring vertices. Considering the characteristics of available data resources and the goal of the global path planning, a coarse square grid resolution of 50 m was chosen for this work.

The global path planning is solved offline before the fleet departs. The cost-to-go map is stored and fused with higher resolution information, and used in local path planning.

## Local Route Guidance Via Model Predictive Path Planning

Efficient integration of the high-level global route guidance with the lower level local route guidance is achieved through the intermediate cost-to-go map. The local route guidance routine is based on a model predictive control (MPC) approach. The optimal local route in the immediate neighborhood of a vehicle is calculated in a receding horizon manner as the vehicle navigates the terrain. We propose to use a kinematic model of the vehicle over the MPC horizon with three states: x and y are the coordinates of the center of gravity with respect to vehicle's initial position and ψ is the vehicle heading angle with respect to the fixed x coordinate. Typically, on off-road terrains, vehicle speed varies slowly. Vehicle speed as an additional degree-of-freedom is more critical for low-level path-following controller and for avoiding fast moving objects. Therefore, for the route guidance routine, the vehicle speed was assumed to be constant. The input to this model is the commanded change in heading angle and is denoted by δ. Therefore, the discretized model is
$xk+1=xk+Δsk cos(ψk)yk+1=yk+Δsk sin(ψk)ψk+1=ψk+δk$
(3)

where Δsk is the displacement of the center of gravity in the xy plane over step k. Note that the commanded heading angle δ can be issued to a low-level vehicle controller that adjusts the steering input to meet the desired heading angle. A higher fidelity vehicle model can be used to evaluate the performance of the vehicle when following the MPC command, which is out of scope and not discussed in this paper.

Typically, MPC is used for path planning and tracking [33,34], which lends itself to a fixed temporal horizon. Here, we have used a model predictive-based algorithm for local route guidance. The local route guidance uses the information from vehicle sensors' in addition to the cost-to-go map from the global routine. Therefore, we have used a fixed spatial horizon that encircles the vehicle as shown in Fig. 3. A circular horizon is consistent with the vehicle sensors' horizon and therefore simplifies the formulation. Accordingly, discretization in Eq. (3) and in the rest of this section is over position and not over time. More specifically, the circular horizon is divided into concentric circles centered at the current vehicle location. The increase in radii, Δr, of successive circles is fixed as the MPC's step length. The steps are incremented in the xy plane and because the vehicle is constrained to move on prescribed terrain, the change in elevation in the z direction is a function of the vehicle's xy location.

As schematically illustrated in Fig. 3, the MPC optimizes the local path (shown by solid spline) based on: (i) high-resolution map and sensor information over its circular horizon and (ii) the DP cost-to-go from perimeter of the horizon to destination point (represented by the varying fence heights). The optimization variables are commanded change in heading angle δk at each Δr increment. Hard symmetric constraints are imposed on δk and its rate of change over each step to more accurately represent the physical constraints on the vehicle motion (due to steering wheel dynamics and steering rate limitations)
$|δk|⩽δmax|δk+1−δk|⩽δmaxrate$
(4)
The structure of the cost function of the local routine echoes that of the global routine for penalty surface continuity. The objective of the optimization routine is to
$minδ1⋯δnJ=w1∑k=1nΔlk+w2∑k=1n1soilk+w3∑k=1n|zk−zk−1|+w4∑k=1nobstaclek+w5∑k=1nvisibilityk+JDP*(xn,yn)$
(5)

In the above cost function, n is the prediction horizon in radial steps and δi are the free optimization variables at successive radial steps; each represent the commanded change in heading angle at stage i. The penalty weights are design variables and shown by wi. The six terms of the cost functions are, respectively:

• (i)
Δlk is the 3D distance covered in the kth step which is computed as
$Δlk=‖Δsk,(zk−zk−1)‖2$
(6)
where Δs is the topographic distance (2D) and z is the elevation at each step point. As shown in detail in the Appendix and illustrated in Fig. 4, Δs is obtained recursively as follows:
$Δsk=(2k+1)(Δr)2+Γk2−Γk$
(7)
where
$Γk≜xk cos ψk+yk sin ψk$
and xk, yk, and ψk are the three states of the system that can be obtained recursively from the state Eq. (3).
• (ii)

The function soilk is the computed value associated with the terrain traversed as described in Sec. 2.1. Since the objective is to minimize the cost function, the inverse of the soil computed value is used in the cost function to ensure that the prescribed route utilizes the best soil possible.

• (iii)

The term $|zk−zk−1|$ penalizes the elevation change in one radial step to prevent selection of steep paths. A hard constraint on $(|zk−zk−1|)/Δsk$ can also be imposed to put an upper limit to the path steepness.

• (iv)
To ensure that the local route prescribed avoids collision with the obstacles by penalizing the locality of the detected obstacles heavily. For an obstacle q, let A(q) denote the circular area surrounding the obstacle centered at the geometric center of the obstacle q and whose circular radius is equal to the preferred safe distance between the vehicle and obstacle. The function obstaclek in Eq. (5) is defined as
$obstaclek={0if xk,yk∉∪q=1QA(q),Mif xk,yk∈∪q=1QA(q),$
(8)
where M represents a very large number.
• (v)
To reduce the chance of being detected, the cost function (5) penalizes areas, which can be seen from the adversarial towers by introducing the cost term
$visibilityk={0if xk,yk∈invisible1if xk,yk∈visible$
(9)
• (vi)

The last term $JDP*(xn,yn)$ represents the optimal cost-to-go, calculated by the dynamic program, from the end of the MPC horizon to the final target. This cost is schematically shown as fence heights in Fig. 3. Remember that the values for the optimal cost-to-go were obtained (offline) using a coarse dynamic program and stored as a layer of information maps as shown in Fig. 2(d). Inclusion of the cost-to-go is intended to prevent short-sighted decisions by the local path planner and ensures local decisions are guided by global objectives.

The weights on elevation change (w3), obstacle detection (w4), and the line of sight (w5) are used as soft constraints, i.e., if the respective parameter values are greater than a predetermined limit a high penalty is applied to that section of the path.

The cost function in Eq. (5) is minimized subject to the vehicle kinematics described in Eq. (3) and the input constraints defined in Eq. (4). This problem is solved numerically and using a sequential quadratic programming approach. Ideally, the cost function for MPC and DP should be the same so that the costs match at the boundary of the local prediction horizon. Note that as Eq. (5) of the paper shows, the ultimate cost that each vehicle minimizes is broken down to a shorter horizon MPC cost (with higher resolution sensory information) PLUS a longer DP horizon (with lower resolution map information). But the two costs are added together and do not overlap in their horizons. They should penalize the same terms. The only difference between them is the resolution of the available information. Note that because the cost function in Eq. (5) does not explicitly depend on control input δk, in the numerical routine, we minimize it with respect to heading angles $ψ1,…,ψk$ while still enforcing input constraints using Eq. (4); this simplifies execution of the nonlinear optimization routine.

## Problem Formulation for a Connected Fleet Formation

The presence of more than one vehicle in a fleet presents some opportunities and challenges. Information gathered by the on-board sensors of each vehicle can be used to improve the stored terrain maps and develop additional layers of maps containing vital risk information. One of the ways to improve the path optimality is to stagger the fleet formation so as to maximize the immediate horizon of the entire fleet. Increasing the fleet horizon increases the shared knowledge of the entire fleet and better prepares them to detect high risk zones and obstructions. In order to maximize the fleet horizon, a formation where all the vehicles are arranged so that their individual sensor horizon just overlap was investigated.

Figure 5 demonstrates the proposed formation. In the figure, the sensor horizon of each individual vehicle is shown by the dotted circle. To maximize the fleet horizon, the distance between two neighboring vehicles, $Lp|p−1$, should be twice the radius of the sensor horizon, Rhorizon. It is assumed that the vehicles begin their journey at the same time and alongside one another. This behavior is encouraged by introducing an additional fleet coordination term in the cost function of each individual vehicle. The coordination cost term was formulated to represent the energy stored by imaginary springs between neighboring vehicles. Consider Fig. 5, where the path of pth vehicle is under consideration. For the pth vehicle in the fleet, there will be two neighboring vehicles (except for end of fleet vehicles). If an imaginary spring with an equilibrium length of 2Rhorizon is placed between the vehicles p and p − 1, the energy stored in the spring will be function of $Lp|p−1−2Rhorizon$. At steady-state, the presence of the spring will try to maintain the neighboring vehicles at a distance 2Rhorizon. Accordingly, the coordination cost for the vehicles can be computed as
$JCON(p)=w6‖JCON(p|p−1),JCON(p|p+1)‖=w6‖(Lp|p−1−2Rhorizon),(Lp|p+1−2Rhorizon)‖$
(10)

This coordination cost is appended to the other path optimization costs defined in Eq. (5) with appropriate weights to ensure that the fleet vehicles are encouraged to maintain a staggered formation. Future iterations of the routine will incorporate additional penalties on the path traversed by the neighboring vehicles so that the fleet does not decompose to a leader-following formation. The fleet of vehicles is therefore coupled through the individual path optimization functions. The sharing of the path critical information allows for cooperative behavior.

It is important that the path optimization is computed efficiently and in a stable manner. In this paper, a hierarchical decentralized model predictive control approach was adopted. Richards and How [12] have explored the stability and efficiency of the decentralized MPC approach for multivehicle cooperative systems. The vehicles in the fleet are ordered according to their initial starting position. For this approach, the local route guidance routine is performed on each vehicle sequentially as shown in Fig. 6(a).

At each step (or time), the local route guidance routine divides the optimization problem into subproblems corresponding to the vehicles in the fleet. Each subproblem optimizes its local path (n-steps) in the vehicle's horizon. For simulating the fleet movement, the subproblems are solved sequentially according to the vehicle's order for the entire fleet and the optimized path results are stored. After the last subproblem has been solved, the first step of the solution for each vehicle in the fleet is applied. To ensure that all the subproblems use the latest available information, the flow of information among the vehicles is managed as shown in Fig. 6(b).

The latest optimized path of the preceding subproblems (1 to p − 1) and the optimized path from the previous step for the succeeding subproblems (p + 1 to P) are used by the current subproblem. At the initial step, to generate the fleet path, it was assumed that all the vehicles will travel along the path dictated by their initial heading angle.

## Illustrative Results

In this section, simulations of a few scenarios are discussed to demonstrate the effectiveness of the developed hierarchical algorithm. For all scenarios, vehicles were simulated over the 10 × 10 km terrain shown in Fig. 2. The objective of the mission was set to travel optimally from the start point at (0, 0) to the end point at (10,000, 10,000) m.

First simulations were conducted with only one vehicle. The DP algorithm is executed offline and then the local MPC path planner is run informed by the DP-calculated optimal cost-to-go map. Multiple layers of information were considered in path-planning decisions by both DP and MPC. The local routine at each step tries to navigate the vehicle to the lowest cost within its horizon as well as incorporates the high-resolution map information. The global optimality of the prescribed route is enriched since the local route guidance routine augments the results of the global route optimization routine (through the cost-to-go map).

The route prescribed by the route guidance routine is plotted over the terrain contour map in Fig. 7. In Fig. 7(a), both terrain and soil conditions are considered in addition to soft and hard constraints on stepwise elevation change. Furthermore, heading angle is constrained as described by Eq. (4). The chosen path balances traveled distance, road steepness, soil conditions, and high-level vehicle kinematic limitations. As a result, it chooses a relatively flat valley that goes around steep slopes.

An optimal path prescribed in presence of visibility information is shown in Fig. 7(b). This reflects a scenario where it is desirable to stay out of the line of sight, e.g., of adversarial watch towers. In Fig. 2(a), we have shown three watch tower locations with red markers and used the matlab toolbox Viewshed to find areas, which are visible from those watch towers taking into account the terrain information we already have. The visible portions in the map from the known watch tower locations are marked in white in Fig. 2(c) while the invisible portions are marked in black. To remain in invisible areas, the optimal path in Fig. 7(b) takes a longer stealth path passing in between the two peaks.

The impact of model predictive path planner is better observed in local obstacle avoidance maneuvers as shown in the zoomed plot in Fig. 8. The presence of the obstacles is detected by on-vehicle sensors. For this work, it is assumed that the obstacle position and motion can be accurately detected. It can be seen that the informed MPC path planner is able to adjust itself to reduce intrusion into obstacle zones.

A fleet of three identical vehicles is considered to demonstrate the effectiveness of the developed algorithm for a fleet of vehicles. The neighboring vehicles were initially separated by the equilibrium distance (2Rhorizon), heading due east (to the right). Two scenarios were simulated: In the first scenario, the motion of the three vehicles is not coordinated as shown in Fig. 9(a), and in the second case, the motions of the vehicles are coordinated as described in Sec. 5 as shown in Fig. 9(b). To highlight the difference, the initial 500 m × 500 m subsection is shown here. For both scenarios, all the vehicles eventually reach the same destination point.

We can observe that in the first scenario, each vehicle's local guidance routine determines the optimal path for the vehicle without consideration for the location of the other vehicles in the fleet. The three vehicles have different starting point. Therefore, their optimal path to the destination is different. The optimal path of one of the vehicles (solid route) would lead it to lose contact with the other vehicles in the fleet. Such a scenario might increase the risk to the vehicle. On the other hand, if the local path planner incorporates the location of its neighboring vehicles, the motion of the entire fleet is coordinated. The benefit of a coordinated fleet, from information resource point of view, is the locally contiguous enrichment of the information layers. On observing obstacles or steep elevation profiles with higher cost than the coordination cost, the vehicle's local path planner prescribes a path avoiding those grid points. When the risk reduces in the local vicinity, the local routines coordinate to bring the fleet vehicles back into a formation. Such a coordinated fleet behavior would mitigate the risk for the entire fleet since the fleet horizon is extended while traveling in a formation. The flexibility of the proposed route guidance framework was demonstrated with these simulations. The fleet operators can use the developed hierarchical framework for more complex scenarios by modifying the cost functions used at each level as per their requirements.

## Conclusions

In this paper, we formalized path planning for off-road terrain as a two-stage optimal control problem: The first stage relies on large-scale but perhaps coarse multilayer maps and employs dynamic programming to calculate the optimal cost-to-go from any point on the coarsely gridded map to a prespecified destination. The second stage is based on MPC and calculates the optimal path in a circular receding horizon, centered on a vehicle, and based on higher resolution data from on-board sensors. Instead of tracking the path prescribed by the DP algorithm, which is common practice, we proposed to append the DP optimal cost as a terminal cost to the MPC's horizon cost.

Our proposed approach ensures the correct break-down of the optimal control problem where the decisions over the immediate horizon are based on a fine grid and benefit from high-resolution information, while the far horizon optimization can be solved efficiently with a coarse grid and relying on lower resolution map sources. In the MPC algorithm, our choice of a circular receding horizon and radial steps in the spatial domain simplifies the problem formulation. Furthermore, a circular planning horizon may correspond to the vehicle sensor horizon, which could be circular or a sector of a circle.

The proposed approach also ensures efficient handling and integration of information layers with the route optimization routines. Static and dynamic information can be easily accessed by the route guidance algorithm. Information gathered by the vehicles enriched the existing database.

For a fleet of cooperating vehicles, we proposed tight formations to extend each vehicle's sensor horizon via vehicle to vehicle communication. The local path planning was then solved with the added goal of maintaining this tight formation. Adopting a decentralized MPC approach in a sequential manner across vehicles in the fleet resulted in a reasonable computational effort and stable simulation results.

Our ongoing work is focused on evaluating the traversability of an “optimal path” via high fidelity multibody vehicle dynamic models. While high-order models cannot be efficiently incorporated in MPC path planning optimizations, they can be used for a posteriori feasibility analysis and to set traversability constraints for the MPC path planner.

## Acknowledgment

Reference herein to any specific commercial company, product, process, or service by trade name, trademark, manufacturer, or otherwise, does not necessarily constitute or imply its endorsement, recommendation, or favoring by the United States Government or the Department of the Army (DoA). The opinions of the authors expressed herein do not necessarily state or reflect those of the United States Government or the DoA, and shall not be used for advertising or product endorsement purposes.

## Funding Data

• The Automotive Research Center (ARC) in accordance with Cooperative Agreement (W56HZV-14-2-0001) U.S. Army Tank Automotive Research, Development and Engineering Center, Warren, MI.

### Appendix

Here, we derive Eq. (7), which determines the increments in vehicle position Δsk at each MPC step k as illustrated in Fig. 4. At MPC step k + 1, the Cartesian coordinates of the vehicle on the perimeter of the (k + 1)th circle, xk+1 and yk+1 satisfy
$xk+12+yk+12=(k+1)2Δr2$
(A1)
Rewrite Eq. (A1) by substituting model states from Eq. (3)
$(xk+Δsk cos ψk)2+(yk+Δsk sin ψk)2=(k+1)2Δr2$
(A2)
which results in
$k2Δr2+(Δsk)2+2Δsk(xk cos ψk+yk sin ψk)=(k+1)2Δr2$
(A3)
Define $Γk=xk cos ψk+yk sin ψk$ and solve the above quadratic equation for Δsk to get
$Δsk=(2k+1)(Δr)2+Γk2−Γk$

## References

References
1.
Zhang
,
C.
, and
Vahidi
,
A.
,
2010
, “
Real-Time Optimal Control of Plug-In Hybrid Vehicles With Trip Preview
,” IEEE American Control Conference (
ACC
), Baltimore, MD, June 30–July 2, pp.
6917
6922
.
2.
Zhang
,
C.
, and
Vahidi
,
A.
,
2012
, “
Route Preview in Energy Management of Plug-In Hybrid Vehicles
,”
IEEE Trans. Control Syst. Technol.
,
20
(
2
), pp.
546
553
.
3.
Zhang
,
C.
,
Vahidi
,
A.
,
Pisu
,
P.
,
Li
,
X.
, and
Tennant
,
K.
,
2010
, “
Role of Terrain Preview in Energy Management of Hybrid Electric Vehicles
,”
IEEE Trans. Veh. Technol.
,
59
(
3
), pp.
1139
1147
.
4.
Jackel
,
L. D.
,
Krotkov
,
E.
,
Perschbacher
,
M.
,
Pippine
,
J.
, and
Sullivan
,
C.
,
2006
, “
The DARPA LAGR Program: Goals, Challenges, Methodology, and Phase I Results
,”
J. Field Rob.
,
23
(
11–12
), pp.
945
973
.
5.
Shiller
,
Z.
, and
Gwo
,
Y.-R.
,
1991
, “
Dynamic Motion Planning of Autonomous Vehicles
,”
IEEE Trans. Rob. Autom.
,
7
(
2
), pp.
241
249
.
6.
Thrun
,
S.
,
Montemerlo
,
M.
,
Dahlkamp
,
H.
,
Stavens
,
D.
,
Aron
,
A.
,
Diebel
,
J.
,
Fong
,
P.
,
Gale
,
J.
,
Halpenny
,
M.
,
Hoffmann
,
G.
,
Lau
,
K.
,
Oakley
,
C.
,
Palatucci
,
M.
,
Pratt
,
V.
,
Stang
,
P.
,
Strohband
,
S.
,
Dupont
,
C.
,
Jendrossek
,
L.-E.
,
Koelen
,
C.
,
Markey
,
C.
,
Rummel
,
C.
,
van Niekerk
,
J.
,
Jensen
,
E.
,
Alessandrini
,
P.
,
,
G.
,
Davies
,
B.
,
Ettinger
,
S.
,
Kaehler
,
A.
,
Nefian
,
A.
, and
Mahoney
,
P.
,
2006
, “
Stanley: The Robot That Won the DARPA Grand Challenge
,”
J. Field Rob.
,
23
(
9
), pp.
661
692
.
7.
Choi
,
J.
,
Lee
,
J.
,
Kim
,
D.
,
Soprani
,
G.
,
Cerri
,
P.
,
Broggi
,
A.
, and
Yi
,
K.
,
2012
, “
Environment-Detection-and-Mapping Algorithm for Autonomous Driving in Rural or Off-Road Environment
,”
IEEE Trans. Intell. Transp. Syst.
,
13
(
2
), pp.
974
982
.
8.
Stavens
,
D.
, and
Thrun
,
S.
,
2006
, “
A Self-Supervised Terrain Roughness Estimator for Off-Road Autonomous Driving
,” 22nd Conference on Uncertainty in Artificial Intelligence (
UAI'06
), Cambridge, MA, July 13–16, pp.
469
476
.https://dl.acm.org/citation.cfm?id=3020476
9.
Lalonde
,
J.-F.
,
Vandapel
,
N.
,
Huber
,
D. F.
, and
Hebert
,
M.
,
2006
, “
Natural Terrain Classification Using Three-Dimensional Ladar Data for Ground Robot Mobility
,”
J. Field Rob.
,
23
(
10
), pp.
839
861
.
10.
Manduchi
,
R.
,
Castano
,
A.
,
Talukder
,
A.
, and
Matthies
,
L.
,
2005
, “
,”
Auton. Robots
,
18
(
1
), pp.
81
102
.
11.
Broggi
,
A.
,
Cardarelli
,
E.
,
Cattani
,
S.
, and
Sabbatelli
,
M.
,
2013
, “
Terrain Mapping for Off-Road Autonomous Ground Vehicles Using Rational B-Spline Surfaces and Stereo Vision
,” IEEE Intelligent Vehicles Symposium (
IV
), Gold Coast, Australia, June 23–26, pp.
648
653
.
12.
Richards
,
A.
, and
How
,
J.
,
2004
, “
Decentralized Model Predictive Control of Cooperating UAVs
,” 43rd IEEE Conference on Decision and Control (
CDC 2004
), Nassau, Bahamas, Dec. 14–17, pp.
4286
4291
.
13.
Tisdale
,
J.
,
Kim
,
Z.
, and
Hedrick
,
J. K.
,
2009
, “
Autonomous UAV Path Planning and Estimation
,”
IEEE Rob. Autom. Mag.
,
16
(
2
), pp.
35
42
.
14.
Bellingham
,
J. S.
,
Tillerson
,
M.
,
Alighanbari
,
M.
, and
How
,
J. P.
,
2002
, “
Cooperative Path Planning for Multiple UAVs in Dynamic and Uncertain Environments
,” 41st IEEE Conference on Decision and Control (
CDC
), Las Vegas, NV, Dec. 10–13, pp.
2816
2822
.
15.
Tsourdos
,
A.
,
White
,
B.
, and
Shanmugavel
,
M.
,
2010
,
Cooperative Path Planning of Unmanned Aerial Vehicles
, Vol.
32
,
Wiley, Chichester, UK
.
16.
Chandler
,
P.
,
Rasmussen
,
S.
, and
Pachter
,
M.
,
2000
, “UAV Cooperative Path Planning,”
AIAA
Paper No. 2000-4370.
17.
Flint
,
M.
,
Polycarpou
,
M.
, and
Fernandez-Gaucherand
,
E.
,
2002
, “
Cooperative Path-Planning for Autonomous Vehicles Using Dynamic Programming
,”
IFAC 15th Triennial World Congress
, Barcelona, Spain, July 21–26, pp.
1694
1699
.
18.
Rhodes
,
C.
,
Blewitt
,
W.
,
Sharp
,
C.
,
Ushaw
,
G.
, and
Morgan
,
G.
,
2014
, “
Smart Routing: A Novel Application of Collaborative Path-Finding to Smart Parking Systems
,”
IEEE 16th Conference on Business Informatics
(
CBI
) Geneva, Switzerland, July 14–17, pp.
119
126
.
19.
Megherbi
,
D.
, and
Kim
,
M.
,
2015
, “
A Collaborative Distributed Multi-Agent Reinforcement Learning Technique for Dynamic Agent Shortest Path Planning Via Selected Sub-Goals in Complex Cluttered Environments
,”
IEEE International Inter-Disciplinary Conference on Cognitive Methods in Situation Awareness and Decision Support
(
CogSIMA
), Orlando, FL, Mar. 9–12, pp.
118
124
.
20.
Sturtevant
,
N. R.
, and
Buro
,
M.
,
2006
, “
Improving Collaborative Pathfinding Using Map Abstraction
,”
AIIDE
, American Association for Artificial Intelligence, Menlo Park, CA, pp.
80
85
.
21.
Fredman
,
M. L.
, and
Tarjan
,
R. E.
,
1987
, “
Fibonacci Heaps and Their Uses in Improved Network Optimization Algorithms
,”
J. ACM
,
34
(
3
), pp.
596
615
.
22.
U.S. Geological Survey, 2017, “
The National Map
,” U.S. Department of the Interior, Washington, DC, accessed Jan. 5, 2016, http://nationalmap.gov/index.html
23.
U.S. Geological Survey, 2016, “
3D Elevation Program (3DEP)
,” U.S. Department of the Interior, Washington, DC, accessed Jan. 5, 2016, http://nationalmap.gov/3DEP/index.html
24.
National Resources Conservation Service, 2016, “
Web Soil Survey
,” U.S. Department of Agriculture, Washington, DC, accessed Jan. 5, 2016, https://websoilsurvey.sc.egov.usda.gov/App/WebSoilSurvey.aspx
25.
U.S. Army, and U.S. Air Force,
1994
, “Planning and Design of Roads, Airfields, and Heliports in the Theater of Operations–Road Design,” Vol. 1, U.S. Department of the Army/U.S. Department of the Air Force, Washington, DC, Report No.
FM 5-430-00-1/AFJPAM 32-8013
.https://webapp1.dlib.indiana.edu/virtual_disk_library/index.cgi/821003/FID577/pubs/af/32/afjpam32-8013v1/afjpam32-8013v1.pdf
26.
Campbell
,
S.
,
Naeem
,
W.
, and
Irwin
,
G. W.
,
2012
, “
A Review on Improving the Autonomy of Unmanned Surface Vehicles Through Intelligent Collision Avoidance Manoeuvres
,”
Annu. Rev. Control
,
36
(
2
), pp.
267
283
.
27.
Li
,
P.
,
Zhu
,
J.
, and
Peng
,
F.
,
2014
, “
Comparison of A* and Lambda* Algorithm for Path Planning
,”
Engineering Technology and Applications: International Conference on Engineering Technology and Applications (ICETA 2014)
, Tsingtao, China, Apr. 29–30, p.
171
.
28.
Knowles
,
B. A.
,
2016
, “In the Face of Anticipation: Decision Making Under Visible Uncertainty as Present in the Safest-With-Sight Problem,”
Master's thesis & Specialist Projects, Western Kentucky University
, Bowling Green, KY.http://digitalcommons.wku.edu/theses/1565
29.
Sidran
,
D. E.
, 2007, “Good Decisions Under Fire,”
Doctoral thesis
, University of Iowa, Iowa City, IA.http://www.riverviewai.com/papers/GoodDecisionsUnderFire.pdf
30.
Kamphuis
,
A.
,
Rook
,
M.
, and
Overmars
,
M. H.
,
2005
, “
Tactical Path Finding in Urban Environments
,”
First International Workshop on Crowd Simulation, Lausanne, Switzerland, Nov. 24–25, pp. 51–60.
31.
Carver
,
S.
, and
Washtell
,
J.
,
2012
, “
Real-Time Visibility Analysis and Rapid Viewshed Calculation Using a Voxel-Based Modelling Approach
,”
GISRUK 2012 Conference
, Lancaster, UK, Apr. 11–13.
32.
Sniedovich
,
M.
,
2006
, “
Dijkstra's Algorithm Revisited: The Dynamic Programming Connexion
,”
Control Cybern.
,
35
(
3
), p.
599
.http://matwbn.icm.edu.pl/ksiazki/cc/cc35/cc3536.pdf
33.
Wang
,
Q.
, and
Müller
,
S.
,
2016
, “
A Hierarchical Controller for Path Planning and Path Following Based on Model Predictive Control
,”
(
AVEC'16
), Munich, Germany, Sept. 13–16, p.
195
.http://www.avec16.com/images/papers/47300.pdf
34.
Rasekhipour
,
Y.
,
Khajepour
,
A.
,
Chen
,
S.-K.
, and
Litkouhi
,
B.
,
2017
, “
A Potential Field-Based Model Predictive Path-Planning Controller for Autonomous Road Vehicles
,”
IEEE Trans. Intell. Transp. Syst.
,
18
(
5
), pp.
1255
1267
.