Problem statement

Given a C-space Q, planning a trajectory is finding a curve

where [0,1] is the parameterization interval and s is the arc-length parameter of C. If xstart and xgoal are the start and the goal configurations respectively, then C(0) = xstart and C(1) =

An optimal trajectory is a curve C that minimizes a set of internal and external constraints (time, fuel consumption or danger for instance). It is assumed in this chapter that the complete set of constraints is described in a cost function t:

2.3 Metric space

In this chapter the metric space Q we refer to is the usual C-space equipped with the metric p defined as:

where C is a trajectory between two configurations x1 and x2, and t is the cost function. This metric can be seen as the "cost-to-go" for a specific robot to reach x2 from x1. At a configuration x, t(x) can be interpreted as the cost of one step from x to its neighbours. If a C-obstacle in some region S is impenetrable, then t(S) will be infinite. The function t is supposed to be strictly positive for an obvious physical reason: t(x) = 0 would mean that free transportation from some configuration x is possible. 