An important aspect in autonomous systems is the ability of a system to plan before acting. This includes both high-level task planning to determine what sequence of actions to take in order for the system to reach a goal state, as well as low-level motion planning to detail how to perform the actions required.

While it is sometimes possible to plan hierarchically, i.e., to first compute a task plan and then compute motion plans for each action in the task plan, there are also many problem instances where this approach fails to find a feasible plan as not all task plans lead to motion-planning problems that have feasible solutions. For this reason, it is desirable to solve the two problems jointly rather than sequentially. Additionally, it is often desirable to find plans that optimize a performance measure, such as the energy used, the length of the path travelled by the system or the time required. This thesis focuses on the problem of finding joint task and motion plans that optimize a performance measure.

The first contribution is a method for solving a joint task and motion planning problem, that can be formulated as a traveling salesman problem with dynamic obstacles and motion constraints, to resolution optimality. The proposed method uses a planner comprising two nested graph-search planners. Several different heuristics are considered and evaluated.

The second contribution is a method for solving a joint task and motion planning problem, in the form of a rearrangement problem for a tractor-trailer system, to resolution optimality. The proposed method combines a task planner with motion planners, all based on heuristically guided graph search, and uses branch-and-bound techniques in order to improve the efficiency of the search algorithm.

The final contribution is a method for improving task and motion plans for rearrangement problems using optimal control. The proposed method takes inspiration from finite-horizon optimal control and decomposes the optimization problem into several smaller optimization problems rather than solving one larger optimization problem. Compared to solving the original larger optimization problem, it is demonstrated that this can lead to reduced computation time without any significant decrease in solution quality.

Autonomous systems is a research field that has received significant attention during the last decades. An important requirement for such systems is the ability to plan before acting. This includes both higher-level task planning to determine what sequence of actions to take in order for the system to reach a goal, as well as lower-level motion planning in order to detail how to perform the actions required.

The first part of this thesis focuses on the problem of finding plans for task and motion planning (tamp) problems that optimize a given performance measure, such as energy consumption, path length or time. A method is presented for solving a tamp problem, that can be formulated as a traveling salesman problem with dynamic obstacles and motion constraints, to resolution optimality. The proposed method uses a planner consisting of two nested graph-search planners. Several different heuristics are considered and evaluated.

The main contribution in this part is a framework for solving a tamp problem, in the form of a rearrangement problem for a tractor-trailer system. In a first step, a method for finding a resolution-optimal solution is proposed. This method combines a task planner with motion planners, all based on heuristically guided graph search, and uses branch-and-bound techniques to improve the efficiency of the search algorithm. The efficiency is further improved by different strategies for recognizing equivalent problems. In a second step, the solution found in the first step is improved using optimal control. The proposed method takes inspiration from finite-horizon optimal control and decomposes the optimization problem into several smaller optimization problems. Compared to solving the original larger optimization problem, it is demonstrated that this can lead to reduced computation time without any significant decrease in solution quality.

The second part of this thesis focuses on finding kinematically feasible and optimized solutions to multi-agent motion planning (mamp) problems. A method for automatic generation of optimized motion primitives such that all primitive durations are a multiple of the same sample time is proposed. This facilitates collision checking. The proposed planner consists of two steps. In the first step a feasible solution is found using a state-of-the-art mamp planner such as conflict-based search (cbs) together with a single-agent planner. The proposed single-agent planner ensures kinematic feasibility, and allows more general cost functions and larger agents than previous approaches. In the second step, a multi-phase optimal control problem is posed and the solution found in the first step is used to warm start the solver.

For the special case where all agents are at rest initially and under the constraint of arriving at their goals simultaneously, it is shown that a feasible solution can be found by applying a standard mamp algorithm and searching backward. In particular, for certain choices of cost function and mamp algorithm it is (resolution) optimal. It is proposed to solve the optimization problem in the second step in a distributed and receding-horizon manner using the nonlinear alternating direction method of multipliers (nadmm).

Ett stort forskningsmål under de senaste årtiondena är utvecklingen av autonoma system, det vill säga system som självständigt utan mänsklig styrning kan lösa och genomföra olika uppdrag. För att kunna uppnå det krävs förmågan att kunna planera, det vill säga att på förhand avgöra vad som ska göras for att uppnå ett visst mål eller klara av ett visst uppdrag. Denna planering kan göras på olika nivåer. På en högre abstraktionsnivå används uppgiftsplanering (eng. task planning) som bestämmer vad som ska göras, och på en lägre nivå används rörelseplanering (eng. motion planning) som bestämmer hur det ska göras. Som exempel kan en robotarm som har i uppdrag att stapla ett antal klossar på varandra använda uppgiftsplanering for att bestämma i vilken ordning klossarna ska lyftas upp och staplas, och rörelseplanering for att avgöra hur robotarmen ska röras för att greppa en kloss eller förflyttas mellan olika positioner.

Många problem kräver såväl uppgiftsplanering som rörelseplanering. Ett sätt att lösa sådana problem ar att först lösa uppgiftsplaneringsproblemet och därefter lösa rörelseplaneringsproblem. Det är dock inte garanterat att det resulterar i en lösning till det ursprungliga problemet, eftersom systemet kan ha rörelsebegränsningar som inte fångas av uppgiftsplaneringen. Det är därför önskvärt att integrera uppgifts- och rörelseplanering tätare genom att ta hänsyn till rörelsebegränsningarna i rörelseplaneringsproblemet redan när uppgiftsplaneringen görs så att de båda delproblemen kan lösas samtidigt i stället för i sekvens.

I den första delen av denna avhandling är fokus att lösa kombinerade uppgifts- och rörelseplaneringsproblem på ett sätt som inte bara tar hänsyn till systemens begränsningar utan även optimerar ett prestandamått. Det kan exempelvis vara att minimera energiförbrukning, förflyttad sträcka eller tid. En metod for att lösa en typ av uppgifts- och rörelseplanering som uppkommer vid planering av borrning i gruvor presenteras. Den föreslagna metoden använder sig av grafsökning och resulterar i lösningar som är optimala med avseende på ett prestandamått, givet en diskretisering av problemet.

Huvudbidraget i denna del är ett ramverk för att lösa en typ av uppgifts- och rörelseplanering där en manipulator har i uppdrag att omarrangera ett antal objekt. Den presenterade metoden finner i ett första steg lösningar som är optimala med avseende på ett prestandamått givet en diskretisering av problemet och förbättrar dessa i ett andra steg med hjälp av optimal styrning. For att minska betalningstiden föreslås att lösa en serie av mindre optimeringsproblem i stället för det ursprungliga större optimeringsproblemet.

Ytterligare en planeringsaspekt att ta hänsyn till är om det är en agent som agerar ensam eller flera samarbetande agenter. Rörelseplanering för flera agenter är mer komplicerat eftersom agenterna inte får kollidera med varandra. Att planera för en agent i taget leder ofta till suboptimala planer eller att det är svårt att hitta en tillåten plan. Att planera för flera agenter samtidigt gör dock att betäckningstiden skalar dåligt med antalet agenter. Detta är fokus för avhandlingens andra del. Även här bygger de föreslagna metoderna på att i ett första steg finna tillåtna lösningar som optimerar ett prestandamått givet en diskretisering, och att i ett andra steg utföra lokal optimering runt de funna lösningarna för att förbättra dem. För att möjliggöra detta presenteras en metod för att automatiskt generera optimerade rörelseprimitiver med en gemensam tidsdiskretisering samt en förbättring av existerande algoritmer som tillåter mer allmänna kostnadsfunktioner och agenter med större utsträckning. För specialfallet där alla agenter initialt befinner sig i vila och under bivillkoret att de ska nå sina mål samtidigt presenteras en lösning där det visas att tillåtna lösningar kan beräknas genom att söka baklänges. För att den lokala optimeringen ska skala bättre med antalet agenter föreslås en metod som använder distribuerad optimering.