During the last decades, motion planning for autonomous systems has become an important area of research. The high interest is not the least due to the development of systems such as self-driving cars, unmanned aerial vehicles and robotic manipulators. In this thesis, the objective is not only to find feasible solutions to a motion planning problem, but solutions that also optimize some kind of performance measure. From a control perspective, the resulting problem is an instance of an optimal control problem. In this thesis, the focus is to further develop optimal control algorithms such that they be can used to obtain improved solutions to motion planning problems. This is achieved by combining ideas from automatic control, numerical optimization and robotics.

First, a systematic approach for computing local solutions to motion planning problems in challenging environments is presented. The solutions are computed by combining homotopy methods and numerical optimal control techniques. The general principle is to define a homotopy that transforms, or preferably relaxes, the original problem to an easily solved problem. The approach is demonstrated in motion planning problems in 2D and 3D environments, where the presented method outperforms both a state-of-the-art numerical optimal control method based on standard initialization strategies and a state-of-the-art optimizing sampling-based planner based on random sampling.

Second, a framework for automatically generating motion primitives for lattice-based motion planners is proposed. Given a family of systems, the user only needs to specify which principle types of motions that are relevant for the considered system family. Based on the selected principle motions and a selected system instance, the algorithm not only automatically optimizes the motions connecting pre-defined boundary conditions, but also simultaneously optimizes the terminal state constraints as well. In addition to handling static a priori known system parameters such as platform dimensions, the framework also allows for fast automatic re-optimization of motion primitives if the system parameters change while the system is in use. Furthermore, the proposed framework is extended to also allow for an optimization of discretization parameters, that are are used by the lattice-based motion planner to define a state-space discretization. This enables an optimized selection of these parameters for a specific system instance.

Finally, a unified optimization-based path planning approach to efficiently compute locally optimal solutions to advanced path planning problems is presented. The main idea is to combine the strengths of sampling-based path planners and numerical optimal control. The lattice-based path planner is applied to the problem in a first step using a discretized search space, where system dynamics and objective function are chosen to coincide with those used in a second numerical optimal control step. This novel tight combination of a sampling-based path planner and numerical optimal control makes, in a structured way, benefit of the former method’s ability to solve combinatorial parts of the problem and the latter method’s ability to obtain locally optimal solutions not constrained to a discretized search space. The proposed approach is shown in several practically relevant path planning problems to provide improvements in terms of computation time, numerical reliability, and objective function value.