In this work, a combined task and motion planner for a tractor and a set of trailers is proposed and it is shown that it is resolution complete and resolution optimal. The proposed planner consists of a task planner and a motion planner that are both based on heuristically guided graph-search. As a step towards tighter integration of task and motion planning, we use the same heuristic that is used by the motion planner in the task planner as well. We further propose to use the motion planner heuristic to give an initial underestimate of the motion costs that are used as costs during the task planning search, and increase this estimate gradually by using the motion planner to verify the cost and feasibility of actions along paths of interest. To limit the time spent in the motion planner, the use of time and cost limits to pause or prematurely abort the motion planner is proposed, which does not affect the resolution completeness or resolution optimality. The planner is evaluated on numerical examples and the results show that the proposed planner can significantly reduce the execution time compared to a baseline resolution optimal task and motion planner.

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. The objective in optimal motion planning problems is to find feasible motion plans that also optimize a performance measure. From a control perspective, the problem is an instance of an optimal control problem. This thesis addresses optimal motion planning problems for complex dynamical systems that operate in unstructured environments, where no prior reference such as road-lane information is available. Some example scenarios are autonomous docking of vessels in harbors and autonomous parking of self-driving tractor-trailer vehicles at loading sites. The focus is to develop optimal motion planning algorithms that can reliably be applied to these types of problems. This is achieved by combining recent ideas from automatic control, numerical optimization and robotics.

The first contribution is a systematic approach for computing local solutions to motion planning problems in challenging unstructured environments. The solutions are computed by combining homotopy methods and direct 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 a state-of-the-art asymptotically optimal motion planner based on random sampling.

The second contribution is an optimization-based framework for automatic generation of motion primitives for lattice-based motion planners. 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 framework computes a library of motion primitives by simultaneously optimizing the motions and the terminal states.

The final contribution of this thesis is a motion planning framework that combines the strengths of sampling-based planners with direct optimal control in a novel way. The sampling-based planner is applied to the problem in a first step using a discretized search space, where the system dynamics and objective function are chosen to coincide with those used in a second step based on optimal control. This combination ensures that the sampling-based motion planner provides a feasible motion plan which is highly suitable as warm-start to the optimal control step. Furthermore, the second step is modified such that it also can be applied in a receding-horizon fashion, where the proposed combination of methods is used to provide theoretical guarantees in terms of recursive feasibility, worst-case objective function value and convergence to the terminal state. The proposed motion planning framework is successfully applied to several problems in challenging unstructured environments for tractor-trailer vehicles. The framework is also applied and tailored for maritime navigation for vessels in archipelagos and harbors, where it is able to compute energy-efficient trajectories which complies with the international regulations for preventing collisions at sea.

 This paper presents a unified optimization-based path planning approach to efficiently compute locally optimal solutions to optimal path planning problems in unstructured environments. The approach is motivated by showing that a lattice-based planner can be cast and analyzed as a bilevel optimization problem. This insight is used to integrate a lattice-based planner and an optimal control-based method in a novel way. The lattice-based planner is applied to the problem in a first step using a discretized search space. In a second step, an optimal control-based method is applied using the lattice-based solution as an initial iterate. In contrast to prior work, the system dynamics and objective function used in the first step are chosen to coincide with those used in the second step. As an important consequence, the lattice planner provides a solution which is highly suitable as a warm-start to the optimal control step. This proposed combination makes, in a structured way, benefit of sampling-based methods ability to solve combinatorial parts of the problem and optimal control-based methods ability to obtain locally optimal solutions. Compared to previous work, the proposed approach is shown in simulations to provide significant improvements in terms of computation time, numerical reliability and objective function value.

The task of maneuvering ships in confined environments is a difficult task for a human operator. One major reason is due to the complex and slow dynamics of the ship which need to be accounted for in order to successfully steer the vehicle. In this work, a two-step optimization-based motion planner is proposed for autonomous maneuvering of ships in constrained environments such as harbors. A lattice-based motion planner is used in a first step to compute a feasible, but suboptimal solution to a discretized version of the motion planning problem. This solution is then used to enable efficient warm-start and as a terminal manifold for a second recedinghorizon improvement step. Both steps of the algorithm use a high-fidelity model of the ship to plan feasible and energy-efficient trajectories. Moreover, a novel algorithm is proposed for automatic computation of spatial safety envelopes around the trajectory computed by the lattice-based planner. These safety envelopes are used in the second improvement step to obtain collision-avoidance constraints which complexity scales very well with an increased number of surrounding obstacles. The proposed optimization-based motion planner is evaluated with successful results in a simulation study for autonomous docking problems in a model of the Cape Town harbor.

This paper presents an optimization-based receding horizon trajectory planning algorithm for dynamical systems operating in unstructured and cluttered environments. The proposed approach is a two-step procedure that uses a motion planning algorithm in a first step to efficiently find a feasible, but possibly suboptimal, nominal solution to the trajectory planning problem where in particular the combinatorial aspects of the problem are solved. The resulting nominal trajectory is then improved in a second optimization-based receding horizon planning step which performs local trajectory refinement over a sliding time window. In the second step, the nominal trajectory is used in a novel way to both represent a terminal manifold and obtain an upper bound on the cost-to-go online. This enables the possibility to provide theoretical guarantees in terms of recursive feasibility, objective function value, and convergence to the desired terminal state. The established theoretical guarantees and the performance of the proposed algorithm are verified in a set of challenging trajectory planning scenarios for a truck and trailer system.   

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.