This paper presents a safe, efficient, and agile ground vehicle navigation algorithm for 3D off-road terrain environments. Off-road navigation is subject to uncertain vehicle-terrain interactions caused by different terrain conditions on top of 3D terrain topology. The existing works are limited to adopt overly simplified vehicle-terrain models. The proposed algorithm learns the terrain-induced uncertainties from driving data and encodes the learned uncertainty distribution into the traversability cost for path evaluation. The navigation path is then designed to optimize the uncertainty-aware traversability cost, resulting in a safe and agile vehicle maneuver. Assuring real-time execution, the algorithm is further implemented within parallel computation architecture running on Graphics Processing Units (GPU).

This article proposes a distributed estimation algorithm that uses local information about the neighbors through sensing or communication to design an estimation-based cooperative control of the stochastic multiagent system (MAS). The proposed distributed estimation algorithm solely relies on local sensing information rather than exchanging estimated state information from other agents, as is commonly required in conventional distributed estimation methods, reducing communication overhead. Furthermore, the proposed method allows interactions between all agents, including non-neighboring agents, by establishing a virtual fully connected network with the MAS state information independently estimated by each agent. The stability of the proposed distributed estimation algorithm is theoretically verified. Numerical simulations demonstrate the enhanced performance of the estimation-based linear and nonlinear control. In particular, using the virtual fully connected network concept in the MAS with the sensing/communication range, the flock configuration can be tightly controlled within the desired boundary, which cannot be achieved through the conventional flocking methods.