Optimal task and motion planning (TAMP) has seen an increase in interest in recent years. In this paper we propose methods for using numerical optimal control to improve upon a feasible solution to a TAMP rearrangement problem. The methods are extensions of existing improvement methods for pure motion planning. The first method poses an optimal control problem (OCP) to simultaneously improve all motions in the plan. The second method, which we denote multiple finite horizons (MFH), takes inspiration from finite horizon control and poses a sequence of finite horizon OCPs involving variables for the positions of temporary placements of movable objects as well as motions in the plan, such that after solving each problem a feasible plan is maintained and the plan cost is non-increasing after each step. The methods are evaluated on a TAMP problem for tractor-trailers in numerical experiments, and the results show that both methods improve the plan for the evaluated problems. The results also show that MFH can reduce the computation time compared to the first method, and that on one example problem it achieves plans of similar or better quality as when all the motions are optimized at the same time provided that the horizon length is sufficiently long.

This paper considers the problem of detecting and tracking objects in a sequence of images. The problem is formulated in a filtering framework, using the output of objectdetection algorithms as measurements. An extension to the filtering formulation is proposed that incorporates class information from the previous frame to robustify the classification. Further, the properties of the object-detection algorithm are exploited to quantify the uncertainty of the bounding box detection in each frame. The complete filtering method is evaluated on camera trap images of the four large Swedish carnivores, bear, lynx, wolf, and wolverine. The experiments show that the class tracking formulation leads to a more robust classification.

Motion planning and decision-making while considering uncertainty is critical for an autonomous vehicle to safely and efficiently drive on a highway. This paper presents a new combined two-step approach for this problem, where a partially observable Markov decision process (POMDP) is tightly coupled with a scenario model predictive control (SCMPC) step. To generate the scenarios in the SCMPC step, the solution to the POMDP is used together with a novel scenario-reduction procedure, which selects a small representative subset of all scenarios considered in the POMDP. The resulting planner is evaluated in a simulation study where the impact of the two-step approach and the scenario-reduction method is shown.

An approach for belief space planning is presented, where knowledge about the landmark density is used as prior, instead of explicit landmark positions. Having detailed maps of landmark positions in a previously unvisited environment is considered unlikely in practice. Instead, it is argued that landmark densities should be used, as they could be estimated from other sources, such as ordinary maps or aerial imagery. It is shown that it is possible to use virtual landmarks to approximate the landmark density to solve the presented problem. This approximation is also shown to give small errors during evaluation. The approach is tested in a simulated environment, in conjunction with an extended information filter (EIF), where the computed path is shown to be superior compared to other alternative paths used as benchmarks.

The design of reliable path-following controllers is a key ingredient for successful deployment of self-driving vehicles. This controller-design problem is especially challenging for a general 2-trailer with a car-like tractor due to the vehicle's structurally unstable joint-angle kinematics in backward motion and the car-like tractor's curvature limitations which can cause the vehicle segments to fold and enter a jackknife state. Furthermore, advanced sensors with a limited field of view have been proposed to solve the joint-angle estimation problem online, which introduce additional restrictions on which vehicle states that can be reliably estimated. To incorporate these restrictions at the level of control, a model predictive path-following controller is proposed. By taking the vehicle's physical and sensing limitations into account, it is shown in real-world experiments that the performance of the proposed path-following controller in terms of suppressing disturbances and recovering from non-trivial initial states is significantly improved compared to a previously proposed solution where the constraints have been neglected.

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 reoptimization 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.

This paper presents a systematic approach for computing local solutions to motion planning problems in nonconvex environments using numerical optimal control techniques. It extends the range of use of state-of-the-art numerical optimal control tools to problem classes where these tools have previously not been applicable. Today these problems are typically solved using motion planners based on randomized or graph search. The general principle is to define a homotopy that transforms, or preferably relaxes, the original problem to an easily solved problem. In this work, it is shown that by combining a Sequential Quadratic Programming (SQP) method with a homotopy approach that gradually transforms the problem from a relaxed one to the original one, practically relevant locally optimal solutions to the motion planning problem can be computed. The approach is demonstrated in motion planning problems in challenging 2D and 3D environments, where the presented method significantly outperforms both a state-of-the-art numerical optimal control method and a state-of-the-art open-source optimizing sampling-based planner commonly used as benchmark.

Motion planning for a general 2-trailer system poses a hard problem for any motion planning algorithm and previous methods have lacked any completeness or optimality guarantees. In this work we present a lattice-based motion planning framework for a general 2-trailer system that is resolution complete and resolution optimal. The solution will satisfy both differential and obstacle imposed constraints and is intended either as a part of an autonomous system or as a driver support system to automatically plan complicated maneuvers in backward and forward motion. The proposed framework relies on a precomputing step that is performed offline to generate a finite set of kinematically feasible motion primitives. These motion primitives are then used to create a regular state lattice that can be searched for a solution using standard graph-search algorithms. To make this graph-search problem tractable for real-time applications a novel parametrization of the reachable state space is proposed where each motion primitive moves the system from and to a selected set of circular equilibrium configurations. The approach is evaluated over three different scenarios and impressive real-time performance is achieved.

Reversing with a dolly steered trailer configuration is a hard task for any driver without extensive training. In this work we present a motion planning and control framework that can be used to automatically plan and execute complicated manoeuvres. The unstable dynamics of the reversing general 2-trailer configuration with off-axle hitching is first stabilised by an LQ-controller and then a pure pursuit path tracker is used on a higher level giving a cascaded controller that can track piecewise linear reference paths. This controller together with a kinematic model of the trailer configuration is then used for forward simulations within a Closed-Loop Rapidly Exploring Random Tree framework to generate motion plans that are not only kinematically feasible but also include the limitations of the controller's tracking performance when reversing. The approach is evaluated over a series of Monte Carlo simulations on three different scenarios and impressive success rates are achieved. Finally the approach is successfully tested on a small scale test platform where the motion plan is calculated and then sent to the platform for execution.

In this paper a cascaded approach for stabilization and path tracking of a general 2-trailer vehicle configuration with an off-axle hitching is presented. A low level Linear Quadratic controller is used for stabilization of the internal angles while a pure pursuit path tracking controller is used on a higher level to handle the path tracking. Piecewise linearity is the only requirement on the control reference which makes the design of reference paths very general. A Graphical User Interface is designed to make it easy for a user to design control references for complex manoeuvres given some representation of the surroundings. The approach is demonstrated with challenging path following scenarios both in simulation and on a small scale test platform.

In this paper an extension to the sampling based motion planning framework CL-RRT is presented. The framework uses a system model and a stabilizing controller to sample the perceived environment and build a tree of possible trajectories that are evaluated for execution. Complex system models and constraints are easily handled by a forward simulation making the framework widely applicable. To increase operational safety we propose a sampling recovery scheme that performs a deterministic brake profile regeneration using collision information from the forward simulation. This greatly increases the number of safe trajectories and also reduces the number of samples that produce infeasible results. We apply the framework to a Scania G480 mining truck and evaluate the algorithm in a simple yet challenging obstacle course and show that our approach greatly increases the number of feasible paths available for execution.