UAV-UGV Cooperative Trajectory Optimization

• UAV-UGV cooperative trajectory optimization is a framework for designing synchronized, collision-free paths for aerial and ground vehicles with dynamic constraints.
• It uses a two-stage pipeline: RRT* for initial path planning followed by nonlinear least squares for trajectory refinement and obstacle avoidance.
• Field tests validate the approach, demonstrating high accuracy, effective tether modeling, and robust coordination under strict dynamic and safety constraints.

Unmanned Aerial Vehicle–Unmanned Ground Vehicle (UAV-UGV) cooperative trajectory optimization addresses the problem of generating feasible, efficient, and safe control and motion plans for multi-agent systems consisting of aerial and ground vehicles operating in coordinated missions. These missions (e.g., exploration, coverage, search and rescue, mobile recharging, or communications support) exploit the complementary mobility, endurance, and payload capabilities of UAVs and UGVs. Trajectory optimization entails both the joint selection of geometric paths and the time-parameterization (velocities, accelerations, synchronizations) for each vehicle, subject to their dynamic constraints and mission objectives. In tightly coupled systems—such as marsupial teams linked by a tether—optimization must respect physical couplings, obstacle avoidance for all system components (including tether), and synchronization for feasibility and safety (Mart/'inez-Rozas et al., 2023).

1. Problem Formulation and Modeling Approaches

In cooperative UAV–UGV trajectory optimization, the joint state space includes both aerial and ground vehicle poses, and, in tethered/marsupial systems, also the tether configuration or length. The planning state is typically

$x = [p_g,\, p_a,\, l],$

where $p_g$ and $p_a$ are the UGV and UAV 3D positions, and $l$ is the tether length (for marsupial systems) constrained by $l \geq \|p_a - p_g\|$ (Mart/'inez-Rozas et al., 2023).

Optimization objectives target collision-free, smooth, and synchronized trajectories; cost functions often combine travel distance or time for both vehicles, energy or fuel expenditure, and slack variables representing physical or operational margins (e.g., tether clearance, safety distances).

Key hard constraints include:

• Obstacle avoidance for UAV, UGV, and extended system elements (tether);
• Physical couplings, such as the tether non-taut catenary, requiring feasibility checks for every candidate waypoint/interpolation;
• Dynamic limits (velocity, acceleration, maximum and minimum allowable speeds);
• Synchronization constraints to guarantee rendezvous and coordinated motion;
• Tether length bounds: $l \in [l_{\min}, l_{\max}]$ with hard enforcement of $l \geq \|p_a - p_g\|$.

The complexity is exacerbated by the need to capture full 3D environments, account for terrain traversability (for UGVs), and accurately model static or dynamic obstacles encountered by all system elements (Mart/'inez-Rozas et al., 2023).

2. Cooperative Path and Trajectory Planning Algorithms

Two-Stage Planning Pipeline

(Mart/'inez-Rozas et al., 2023) introduces a robust two-stage pipeline:

1. Path Planning via RRT*
   • A variant of optimal Rapidly-exploring Random Trees (RRT*) is employed in the joint state space $(p_g, p_a)$.
   • Sampling: UGV states are sampled only from traversable ground points; UAV states from obstacle-free 3D airspace.
   • Steering: Three prioritized modes—UAV-only, UGV+UAV jointly, UGV-only—are used to attempt connections, accelerating convergence.
   • Each proposed edge is accepted only if a feasible, collision-free catenary (tether) exists given the environmental distance field.
   • Edge cost is the sum of Euclidean distances traversed by UAV and UGV.
2. Trajectory Optimization via Nonlinear Least Squares (NLS)
   • The raw path is post-processed through NLS minimization (using Ceres Solver), introducing residuals for:
     • Path equidistance,
     • Collision avoidance (UGV/UAV/tether),
     • UGV traversability,
     • Path smoothness (penalizing sharp turns),
     • Kinematic limits (velocity, acceleration),
     • Tether feasibility (softened for optimization).
   • Penalty terms are strongly weighted for collision or physical infeasibilities.

The resulting trajectory is time-parameterized, with both vehicles sharing identical knot times for strict synchronization.

3. Tether Modeling and Constraints

A distinctive aspect of marsupial UAV-UGV systems is the non-taut tether, introducing a third agent (the tether) whose configuration must remain feasible at every instant. The static tether shape follows a catenary,

$z(x) = a \cosh\left(\frac{x-x_0}{a}\right) + c,$

determined by matching endpoints ($p_g, p_a$) and arc length $l$. At each planning step:

• The minimum feasible $l \geq \|p_a - p_g\|$ is computed such that the discretized catenary is collision-free across all points.
• If no feasible $l$ within $[l_\text{min}, l_\text{max}]$ exists for an edge, the edge is rejected.
• In trajectory optimization, soft-penalty residuals penalize any catenary-point proximity to obstacles, strongly discouraging but not absolutely forbidding collision proximity.

This modeling allows the system to plan around obstacles not visible to both UAV and UGV simultaneously, but at substantial computational cost, particularly for dense or cluttered environments.

4. Synchronization and Multi-Agent Coordination

Synchronization is explicit: both UGV and UAV must reach corresponding waypoints simultaneously. This is enforced by

• Equal $\Delta t^i$ intervals for both vehicles at each optimization knot,
• Coordinated progression of the tether-reel mechanism.

The RRT* nearest-neighbor metric is weighted ($w_g > w_a$) to prefer UAV motion (minimizing UGV idle/wait time, which is energetically less costly than UAV hovering). Three-mode steering introduces flexibility for progression even in challenging environments, and prioritizes growth of the solution tree toward the UAV.

5. Computational Performance, Success Rates, and Field Validation

Simulation results indicate:

• RRT* path computation succeeds in all tested scenarios (up to 28 seconds for difficult maps, typically ~5 seconds).
• Nonlinear least squares trajectory optimization achieves 86.6% feasible rate; infeasibility arises for heavily cluttered environments (catenary cannot avoid obstacles).
• Velocity and acceleration profiles track their targets closely (error $<0.05\, \mathrm{m/s}$ and $0.05\, \mathrm{m/s}^2$), and safety margins for obstacles are strictly respected.

Field trials:

• Experiments with a DJI M210 UAV and omnidirectional ARCO UGV using fully onboard localization validate the plan execution with mean position errors $<0.06\, \mathrm{m}$, demonstrating practical viability under real-world sensing and control (Mart/'inez-Rozas et al., 2023).

6. Extension and Generalization

This tethered cooperative trajectory optimization framework generalizes readily:

• Untethered systems: Remove catenary constraints, the pipeline adapts to classical multi-agent coordination.
• Looser Coupling: Framework extends to multiple tethers or hybrid commutation/constraint coupling; additional residuals and constraints can be incorporated without fundamental redesign.
• Incorporation of communication constraints: Additional penalties on the distance between UAV and UGV can model radio-link requirements.
• Scalability: The approach is computationally efficient for moderate-size problems, but worst-case complexity scales with the number of knots (problematic in extremely dense environments or with many obstacles).

A hard tether constraint is the defining feature distinguishing this work from pure rendezvous or loosely-coupled cooperative planning.

7. Context, Limitations, and Future Work

This is the first method to provide complete trajectory planning for marsupial UGV-UAV systems with a non-taut tether, incorporating both path and full trajectory optimization (with physical, dynamic, and collision constraints for all three agents) (Mart/'inez-Rozas et al., 2023). The major limitations are:

• Feasibility loss in extremely cluttered spaces (catenary cannot avoid all obstacles),
• Increased computational load due to catenary modeling and collision checks,
• No explicit handling of highly dynamic environments or moving obstacles,
• Limited discussion of optimality guarantees (solution is locally optimal in the space of smooth, feasible trajectories).

Possible extensions include dynamic or predictive obstacle avoidance, hybrid relational constraints (e.g., for cable-towed sensor suites), and the inclusion of multi-agent teams with more complicated coupling structures. The architecture provides a foundation for real-time, large-scale, and safety-critical cooperative robotics deployment in environments with challenging topology and coupling constraints between agents.

---

Key Reference

For detailed methodology, mathematical formulation, and field validation, see:

• "Path and trajectory planning of a tethered UAV-UGV marsupial robotic system" (Mart/'inez-Rozas et al., 2023).