Optimized and kinematically feasible multi-agent motion planning

Multi-agent motion planning (MAMP) is an important problem for autonomous systems with multiple agents. In this work we propose a two-step method for finding optimized and kinematically feasible solutions to MAMP problems. The first step finds an initial feasible solution using state-of-the-art methods such as conflict-based search (CBS) or priority-based search (PBS), and the second step is an improvement step which improves the solution by solving a multi-phase optimal control problem (OCP) where the initial solution is used to warm-start the solver. We also propose a method for generating motion primitives in an optimized way under the constraint that the primitive durations are all multiples of the same sample time. We evaluate our proposed framework on a MAMP problem for tractor-trailer systems. We extend the safe interval path planning with interval projections (SIPP-IP) algorithm so it can handle more general cost functions and larger agents, but our results show that for the tractor-trailer system a simple lattice-based planner performs better due to less conservative collision checks. Our experiments also indicate that CBS performs better than PBS for this system as it achieves a higher success rate in environments with obstacles and had a lower average runtime, although both planners achieve solutions of similar quality after the improvement step.

Optimization-based motion planning for multi-steered articulated vehicles

The task of maneuvering a multi-steered articulated vehicle in confined environments is difficult even for experienced drivers. In this work, we present an optimization-based trajectory planner targeting low-speed maneuvers in unstructured environments for multi-steered N-trailer vehicles, which are comprised of a car-like tractor and an arbitrary number of interconnected trailers with fixed or steerable wheels. The proposed trajectory planning framework is divided into two steps, where a lattice-based trajectory planner is used in a first step to compute a resolution optimal solution to a discretized version of the trajectory planning problem. The output from the lattice planner is then used in a second step to initialize an optimal control problem solver, which enables the framework to compute locally optimal trajectories that start at the vehicle's initial state and reaches the goal state exactly. The performance of the proposed optimization-based trajectory planner is evaluated in a set of practically relevant scenarios for a multi-steered 3-trailer vehicle with a car-like tractor where the last trailer is steerable.