A Hierarchical Route Guidance Framework for Off-Road Connected Vehicles

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 [1–3]; 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 [5–7]. 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,9–11]. 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 [12–16] 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 [17–20]. 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.

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.

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 km² 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 [27–30]. 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.

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.

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 w_k. Therefore, J₁ represents the distance cost, J₂ encourages choosing soil patches with higher trafficability rating, J₃ penalizes absolute elevation change, and J₄ is the cost associated with LoS. DP finds the path, which minimizes the total cost from an origin to a final destination.

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.

where Δs_k is the displacement of the center of gravity in the x–y 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 x–y 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 x–y location.

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 w_i. The six terms of the cost functions are, respectively:

• (i)
  Δl_k 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 x_k, y_k, and ψ_k are the three states of the system that can be obtained recursively from the state Eq. (3).
• (ii)

  The function soil_k 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 obstacle_k 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 (w₃), obstacle detection (w₄), and the line of sight (w₅) 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.

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.

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.

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 (2R_horizon), 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.

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.

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.

• 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.