Multi-Hypothesis Motion Planning under Uncertainty Using Local Optimization

Sammanfattning: Motion planning is defined as the problem of computing a feasible trajectory for an agent to follow. It is a well-studied problem with applications in fields such as robotics, control theory and artificial intelligence. In the last decade there has been an increased interest in algorithms for motion planning under uncertainty where the agent does not know the state of the environment due to, e.g. motion and sensing uncertainties. One approach is to generate an initial feasible trajectory using for example an algorithm such as RRT' and then improve that initial trajectory using local optimization. This thesis proposes a new modification of the RRT' algorithm that can be used to generate initial paths from which initial trajectories for the local optimization step can be generated. Unlike standard RRT', the modified RRT' generates multiple paths at the same time, all belonging to different families of solutions (homotopy classes). Algorithms for motion planning under uncertainty that rely on local optimization of trajectories can use trajectories generated from these paths as initial solutions. The modified RRT' is implemented and its performance with respect to computation time and number of paths found is evaluated on simple scenarios. The evaluations show that the modified RRT' successfully computes solutions in multiple homotopy classes. Two methods for motion planning under uncertainty, Trajectory-optimized LQG (T-LQG), and a belief space variant of iterative LQG (iLQG) are implemented and combined with the modified RRT'. The performance with respect to cost function improvement, computation time and success rate when following the optimized trajectories for the two methods are evaluated in a simulation study. The results from the simulation studies show that it is advantageous to generate multiple initial trajectories. Some initial trajectories, due to for example passing through narrow passages or through areas with high uncertainties, can only be slightly improved by trajectory optimization or results in trajectories that are hard to follow or with a high collision risk. If multiple initial trajectories are generated the probability is higher that at least one of them will result in an optimized trajectory that is easy to follow, with lower uncertainty and lower collision risk than the initial trajectory. The results also show that iLQG is much more computationally expensive than T-LQG, but that it is better at computing control policies to follow the optimized trajectories.