Trajectory Bundle Method in SE(3) for Black-Box Fixed-Wing Aircraft Trajectory Optimization

Optimal trajectory generation for rigid-body motions on Lie groups can be formulated as a variational problem that minimizes energy functionals defined by Riemannian metrics. While closed-form solutions exist for special cases such as product metrics and rest-to-rest boundary conditions, solving the general problem with arbitrary boundary states and coupled rotational-translational metrics often requires computationally expensive numerical boundary value solvers. These limitations restrict the use of geometrically consistent trajectory generation in real-time robotic planning and control. This paper presents a learning-based framework for approximating higher-order smooth trajectories on SE(3) under general left-invariant Riemannian metrics. The method parameterizes body-twist trajectories using high-order polynomials and relies on a neural network to learn a subset of the polynomial coefficients and the trajectory duration. The remaining coefficients are analytically determined to enforce the boundary conditions. The training of the network is guided by losses derived from Euler-Lagrange optimality conditions, metric-weighted smoothness objectives, and feasibility constraints. The metric-conditioned framework enables generalization across diverse metric structures and motion conditions. Extensive numerical experiments demonstrate that the proposed approach generates smooth trajectories that closely approximate solutions from numerical optimization while achieving millisecond-level inference times. We demonstrate two practical applications of the proposed framework: real-time generation of diverse motion primitives with waypoint traversal, and refinement for quadrotor flight under dynamic conditions. These results suggest that learning-based motions with geometric structure can provide an efficient alternative to conventional optimization-based methods for trajectory generation on SE(3).

Designing dynamically feasible trajectories for rigid bodies is a fundamental problem in robotics. While direct methods are widely used, the existing constrained optimizers typically operate in Euclidean space and ignore the manifold structure of rigid body motions. This mismatch may introduce singularities or lead to poorly conditioned optimization problems. To bridge this gap, we develop a structure-aware framework for constrained trajectory optimization directly on matrix Lie groups. Our approach is based on the second-order rigid body models utilizing Lie group structures, which enables efficient Newton-type updates while preserving the underlying geometry. Building on this model, we propose a line-search Lie Group Interior Point Method (LieIPM) to handle constraints on the manifolds. We instantiate the framework for rigid body motion planning using Lie group variational integrators and derive closed-form intrinsic derivatives that exploit group symmetries. The LieIPM preserves the topology of rotation motions by construction and avoids singularities. Numerical results demonstrate superior robustness and faster convergence compared to general-purpose solvers and structure-exploiting optimal control methods.

The Lie group SE(3) of isometric orientation preserving transformation is used for modeling multibody systems, robots, and Cosserat continua. The use of these models in numerical simulation and optimization schemes necessitates the exponential map, its right-trivialized differential (often referred to as tangent operator), as well as higher derivatives in closed form. The $6\times 6$ matrix representation of the differential, $\mathbf{dexp}_{\mathbf{X}}:se\left( 3\right) \rightarrow se\left( 3\right)$ , and its first derivative were reported using a $3\times 3$ block partitioning. In this paper, the differential, its first and second derivative, as well as the Jacobian and Hessian of the evaluation maps, $\mathbf{dexp}_{\mathbf{X}}\mathbf{Z}$ and $\mathbf{dexp}_{\mathbf{X}}^{T}$, are reported avoiding the block partitioning. For all of them, higher-order approximations are derived. Besides the compactness, the advantage of the presented closed form relations is their numerical robustness when combined with the local approximation. The formulations are demonstrated for computation of the deformation field and the strain rates of an elastic Cosserat-Simo-Reissner rod.