Trajectory Design of Autonomous Vehicles Based on Motion Primitives and Heuristic Cost-to-Go Functions

The task of trajectory design of autonomous vehicles is typically two-fold. First, it needs to take into account the intrinsic dynamics of the vehicle, which are sometimes termed local constraints. Second, on a higher level, the designed trajectories must allow the vehicle to achieve some application-specific task. The specification of the task results in the so-called global constraints. Both of these two components of trajectory design are generally nontrivial problems, and very often, they are pursued as two parallel areas. When the results drawn from the two areas are applied in conjunction, the synthesis is usually somewhat arbitrary. In this paper, we assume some optimal control strategy that addresses the vehicle dynamics is available as a set of motion primitives. The trajectories that achieve the task are determined solely through the primitives and do not reference the vehicle dynamics directly. For the higher level, we translate the task into a very special type of cost-to-go function, which is partially specified artificially, and partially determined by an admissibility condition imposed by the set of primitives. The optimality feature of the primitives is formally extended to the final trajectory design. We illustrate our result with the example of a mobile robot retrieving an object, which is an interesting problem of its own right. In this case, the cost-to-go function for the task is specified arbitrarily for all vehicle positions and zero velocity, and determined automatically elsewhere in the position-velocity space