Improved Optimization of Motion Primitives for Motion Planning in State Lattices

Abstract

In this paper, we propose a framework for generating motion primitives for lattice-based motion planners automatically. Given a family of systems, the user only needs to specify which principle types of motions, which are here denoted maneuvers, that are relevant for the considered system family. Based on the selected maneuver types and a selected system instance, the algorithm not only automatically optimizes the motions connecting pre-defined boundary conditions, but also simultaneously optimizes the end-point boundary conditions as well. This significantly reduces the time consuming part of manually specifying all boundary value problems that should be solved, and no exhaustive search to generate feasible motions is required. In addition to handling static a priori known system parameters, the framework also allows for fast automatic re-optimization of motion primitives if the system parameters change while the system is in use, e.g, if the load significantly changes or a trailer with a new geometry is picked up by an autonomous truck. We also show in several numerical examples that the framework can enhance the performance of the motion planner in terms of total cost for the produced solution.

Improved Path Planning by Tightly Combining Lattice-Based Path Planning and Optimal Control

“…Then, the motion primitive set P is computed by solving the N m OCPs defined by the user without considering obstacles. For a detailed explanation of this step, the reader is referred to [20], [32]. The online procedure is initiated whenever a new path planning problem from x init to x term is requested be solved.…”

“…The motion-primitive sets are automatically computed using the approach described in [32], where the sets are composed of heading changes and parallel maneuvers according to Table I. These maneuvers are optimized using the cost functions defined in (13) and (14).…”

“…For the simplified vehicle models, the neglected states are disregarded in the cost functions. For a more detailed description of the state-lattice construction, the reader is referred to [32].…”

On Motion Planning Using Numerical Optimal Control

“…Most nonlinear ip methods associate the problem of solving (2.1) with the following barrier problem: 11) where the inequality constraints from (2.1) are reformulated as equality constraints by introducing non-negative slack variables s 0 with s ∈ R p . The barrier parameter µ is given by a positive scalar, and log(•) represents the natural logarithm function.…”

“…In this section, we propose to use numerical optimal control to improve the approximate solution computed by the lattice planner. By letting the system mode sequence σ = {q k } M k=1 be fixed to the solution from the lattice planner, the following ocp is obtained: 11) where the optimization variables are the control signals u k (•) and lengths S k of the M phases. The difference compared to the optimal path planning problem (5.1) is that the combinatorial aspect of selecting the system mode sequence is already specified.…”