Motion Planning and Obstacle Avoidance

Abstract

This chapter describes motion planning and obstacle avoidance for mobile robots. We will see how the two areas do not share the same modeling background. From the very beginning of motion planning, research has been dominated by computer sciences. Researchers aim at devising well-grounded algorithms with well-understood completeness and exactness properties.

The challenge of this chapter is to present both nonholonomic motion planning (Sects. 47.1–47.6) and obstacle avoidance (Sects. 47.7–47.10) issues. Section 47.11 reviews recent successful approaches that tend to embrace the whole problem of motion planning and motion control. These approaches benefit from both nonholonomic motion planning and obstacle avoidance methods.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

The introduction of nonholonomic constraints has entailed revisiting these algorithms via the introduction of differential geometry materials. Such a combination has been made possible for certain classes of systems, the so-called small-time controllable ones. It remains that the underlying hypothesis of motion planning algorithms is the knowledge of a global and accurate map of the environment. More than that the considered system is a formal system of equations that does not account for the entire physical system: uncertainties in the world or system modeling are not considered. Such hypotheses are too strong in practice. This is why other complementary research has been done in parallel in a more pragmatic but realistic manner. Such research deals with obstacle avoidance. The problem here is not to deal with complicated systems like a car with multiple trailers. The considered systems are much simpler with respect to their geometric shape. The problem considers sensor-based motions to face the physical issues of a real system navigating in a real world better than motion planning algorithms. How can we navigate toward a goal in a cluttered environment when the obstacles to avoid have just been discovered in real time? This is the question that obstacle avoidance addresses.

The appearance of mobile robots in the late 1960s early 1970s initiated a new research domain: autonomous navigation. It is interesting to note that the first navigation systems were published at the very first International Joint Conferences on Artificial Intelligence (GlossaryTerm

IJCAI

 1969). These systems were based on seminal ideas, which have been very fruitful in the development of robot motion planning algorithms. For instance, in 1969, the mobile robot Shakey used a grid-based approach to model and explore the environment [47.1], in 1977 Jason used a visibility graph built from the corners of the obstacles [47.2], and in 1979 Hilare decomposed the environment into collision-free convex cells [47.3].

In the late 1970s, studies of robot manipulators popularized the notion of the configuration space of a mechanical system [47.4]. In the configuration space the piano becomes a point. The motion planning problem for a mechanical system was thus reduced to finding a path for a point in the configuration space. The way was open to extending the seminal ideas and to developing new and well-grounded algorithms (see Latombe’s book [47.5]).

One decade later, the notion of nonholonomic systems (also borrowed from mechanics) appeared in the literature [47.6] on robot motion planning through the problem of car parking. This problem had not been solved by the pioneering works on mobile robot navigation. Nonholonomic motion planning then became an attractive research field [47.7].

Besides this research effort in path planning, work was initiated in order to make robots move out of their initially artificial environments where the world was cylindrical and composed of wooden vertical boards. Robots started to move in laboratory buildings, with people walking around. Inaccurate localization, uncertain and incomplete maps of the world, and unexpected moving or static obstacles made roboticists aware of the gap between planning a path and executing a motion. Since then the domain of obstacle avoidance has been very active.

In the 2000s a lot of effort has been made toward the integration of motion planning and obstacle avoidance. This effort was stimulated by the DARPA Urban challenge competition. The most successful integration was done by the Carnegie Mellon University team, who won the competition [47.8].

1 Nonholonomic Mobile Robots: Where Motion Planning Meets Control Theory

Nonholonomic constraints are nonintegrable linear constraints over the velocity space of a system. For instance, the rolling without slipping constraint of a differentially-driven mobile robot (Fig. 47.1) is linear with respect to the velocity vector (vector of linear and angular velocities) of the differentially-driven robot and is non integrable (it cannot be integrated into a constraint over the configuration variables). As a consequence, a differentially-driven mobile robot can go anywhere but not following any trajectory. Other types of nonholonomic constraints arise when considering second-order differential equations such as the conservation of inertial momentum. A number of papers investigate the famous falling cat problem or free-floating robots in space [47.7]. This chapter is devoted to nonholonomic constraints for mobile robots with wheels.

While the constraints due to obstacles are expressed directly in the configuration space, that is a manifold, nonholonomic constraints are expressed in the tangent space. In the presence of a linear kinematic constraint, the first question that naturally arises is: does this constraint reduce the space reachable by the system? This question can be answered by studying the structure of the distribution spanned by the Lie algebra of the control system.

Even in the absence of obstacles, planning admissible motions (i. e., that satisfy the kinematic constraints) for a nonholonomic system between two configurations is not an easy task. Exact solutions have been proposed only for some classes of systems but a lot of systems remain without exact solution. In the general case, however, approximate solutions can be used.

The motion planning problem for a nonholonomic system can be stated as follows: given a map of the environment with obstacles in the workspace, a robot subject to nonholonomic constraints, an initial configuration and a goal configuration, find an admissible collision-free path between the initial and goal configurations. Solving this problem requires taking into account both the configuration space constraints due to obstacles and the nonholonomic constraints. The tools developed to address this issue thus combine motion planning and control theory techniques. Such a combination is possible for the class of so-called small-time controllable systems due to topological arguments (Theorem 47.2 in Sect. 47.3).

2 Kinematic Constraints and Controllability

In this section, we give the main definition of controllability, using Sussman’s terminology [47.9].

2.1 Definitions

Let us denote by $\mathcal{C}\mathcal{S}$ the configuration space of dimension n of a given mobile robot and by q the configuration of this robot. If the robot is mounted on wheels, it is subject to kinematic constraints, linear in the velocity vector

$${\omega }_i\left(\mathit{q}\right)\dot{\mathit{q}}=0,i\in \{1,\mathrm{\dots },k\}.$$

We assume that these constraints are linearly independent for any q. Equivalently, for each q, there exists $m=n-k$ linearly independent vectors $f_1\left(\mathit{q}\right),\mathrm{\dots },f_m\left(\mathit{q}\right)$ such that the above constraints are equivalent to

$$\exists (u_1,\mathrm{\dots },u_m)\in {\mathbb{R}}^m,\dot{\mathit{q}}=\sum _{i=1}^m{u}_i{f}_i\left(\mathit{q}\right).$$

(47.1)

Let us note that the choice of vectors $f_i\left(\mathit{q}\right)$ is not unique. Fortunately, all the following developments are valid whatever choice we make. Moreover, if the linear constraints are smooth, vector fields $f_1,\mathrm{\dots },f_m$ can be chosen smooth with respect to q. We assume this condition from now on.

Let us define by $\mathcal{U}$ a compact subset of ${\mathbb{R}}^m$. We denote by Σ the control system defined by (47.1), with $(u_1,\mathrm{\dots },u_m)\in \mathcal{U}$.

Definition 47.1

A local and small-time controllability:

• 1. Σ is locally controllable about configuration q iff the set of configurations reachable from q by an admissible trajectory contains a neighborhood of q.

• 2. Σ is small-time controllable about configuration q iff the set of configurations reachable from q by an admissible trajectory in time less than T contains a neighborhood of q for any T.

$f_1,\mathrm{\dots },f_m$ are called control vector fields of Σ. A system’s small-time controllable about each configuration is said to be small-time controllable.

2.2 Controllability

Checking the controllability properties of a system requires the analysis of the control Lie algebra associated with the system. Let us illustrate in an informal way what the Lie bracket of two vector fields is. Consider two basic motions: go along a straight line and turn on the spot, supported by vector fields denoted by f and g, respectively. Now consider the following combination: go forward during a time t, turn clockwise during the same time t, go backward during the same time t and then turn counterclockwise during the time t. The system reaches a configuration that is not the starting one. Of course when t tends to 0 the goal configuration is very close to the starting one. The direction indicated by such goal configurations when t tends to 0 correspond to a new vector field, which is the Lie bracket of f and g. In a more mathematical formulation, the Lie bracket $[f,g]$ of two vector fields f and g is defined as being the vector field $\partial f.g-\partial g.f$. The k-th coordinate of $[f,g]$ is

$$[f,g]\left[k\right]=\sum _{i=1}^n\left(g,\left[,i,\right],\frac{\partial }{\partial x_i},f,\left[,k,\right],-,f,\left[,i,\right],\frac{\partial }{\partial x_i},g,\left[,k,\right]\right).$$

The following theorem [47.10] gives a powerful result for symmetric systems (a system is said to be symmetric when $\mathcal{U}$ is symmetric with respect to the origin).

Theorem 47.1

A symmetric system is small-time controllable about configuration q iff the rank of the vector space spanned by the family of vector fields f_i together with all their brackets is n at q.

Checking the Lie algebra rank condition (GlossaryTerm

LARC

) on a control system consists in trying to build a basis of the tangent space from a basis (e. g., a P. Hall family) of the free Lie algebra spanned by the control vector fields. An algorithm is proposed in [47.11, 47.12].

2.3 Example: The Differentially-Driven Mobile Robot

Fig. 47.1

A differentially-driven mobile robot is subject to one linear kinematic constraint, due to the rolling without slipping constraint of the wheel axis

To illustrate the notions developed in this section, we consider the differentially-driven mobile robot displayed in Fig. 47.1. The configuration space of this robot is ${\mathbb{R}}^2\times {\mathrm{S}}^1$, and a configuration can be represented by $\mathit{q}=(x,y,\theta )$ where $(x,y)$ is the position in the horizontal plane of the center of the wheel axis of the robot and θ the orientation with respect to the x-axis. The rolling without slipping kinematic constraint

$$-\dot{x}\sin \theta +\dot{y}\cos \theta =0$$

is linear with respect to the velocity vector $(\dot{x},\dot{y},\dot{\theta })$. Therefore, the subspace of admissible velocities is spanned by two vector fields, for instance,

$$f_1\left(\mathit{q}\right)=\left(\begin{array}{c}\cos \theta \\ \sin \theta \\ 0\end{array}\right)\text{and}{f}_2\left(\mathit{q}\right)=\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right).$$

(47.2)

The Lie bracket of these two vector fields is

$$f_3\left(\mathit{q}\right)=\left(\begin{array}{c}\sin \theta \\ -\cos \theta \\ 0\end{array}\right).$$

This implies that about any configuration q, the rank of the vector space spanned by $f_1\left(\mathit{q}\right),f_2\left(\mathit{q}\right),f_3\left(\mathit{q}\right)$ is 3 and, therefore, that the differentially-driven mobile robot is small-time controllable.

3 Motion Planning and Small-Time Controllability

Motion planning raises two problems: the first one addresses the existence of a collision-free admissible path; this is the decision problem. The second one addresses the computation of such a path; this is the complete problem.

3.1 The Decision Problem

From now on, we assume that the set of collision-free configurations is an open subset. This implies that contact configurations are assumed in collision.

Theorem 47.2

The existence of a collision-free admissible path for a symmetric small-time controllable mobile robot between two configurations is equivalent to the existence of a collision-free (not necessarily admissible) path between these configurations.

Proof:

Let us consider a not necessarily admissible collision-free path between two configurations ${\mathit{q}}_1$ and ${\mathit{q}}_2$ as a continuous mapping $\mathrm{\Gamma }$ from interval $[0,1]$ into the configuration space $\mathcal{C}\mathcal{S}$ such that:

• 1. $\mathrm{\Gamma }\left(0\right)={\mathit{q}}_1$, $\mathrm{\Gamma }\left(1\right)={\mathit{q}}_2$,

• 2. for any $t\in [0,1]$, $\mathrm{\Gamma }\left(t\right)$ is collision free.

Point 2 implies that for any t there exists a neighborhood U(t) of $\mathrm{\Gamma }\left(t\right)$ included in the collision-free subset of the configuration space.

Let us denote by $\epsilon \left(t\right)$ the bigger lower bound of the time to collision of all the trajectories starting from $\mathrm{\Gamma }\left(t\right)$. As the control vector $(u_1,\mathrm{\dots },u_m)$ remains in the compact set $\mathcal{U}$, $\epsilon \left(t\right)>0$.

As the system is small-time controllable about $\mathrm{\Gamma }\left(t\right)$, the set reachable from $\mathrm{\Gamma }\left(t\right)$ in time less than $\epsilon \left(t\right)$ is a neighborhood of $\mathrm{\Gamma }\left(t\right)$, which we denote by V(t).

The collection $\left\{V\right(t),t\in [0,1\left]\right\}$ is an open covering of the compact set $\left\{\mathrm{\Gamma }\right(t),t\in [0,1\left]\right\}$. Therefore, we can extract a finite covering, $\left\{V\right(t_1),\mathrm{\dots },V(t_{\mathrm{l}}\left)\right\}$, where $t_1=0<t_2<\mathrm{\dots }<t_{l-1}<t_l=1$ such that for any i between 1 and $l-1$, $V\left(t_i\right)\cap V\left(t_{i+1}\right)\ne \mathrm{\varnothing }$. For each i between 1 and $l-1$, we choose one configuration r_i in $V\left(t_i\right)\cap V\left(t_{i+1}\right)$. As the system is symmetric, there exists an admissible collision-free path between $\mathit{q}\left(t_i\right)$ and r_i and between r_i and $\mathit{q}\left(t_{i+1}\right)$. The concatenation of these paths is a collision-free admissible path between ${\mathit{q}}_1$ and ${\mathit{q}}_2$.

3.2 The Complete Problem

In the former section, we have established that the decision problem, i. e., determining whether there exists a collision-free admissible path between two configurations, is equivalent to determining whether the configurations lie in the same connected component of the collision-free configuration space. In this section we present the tools necessary to solve the complete problem. These tools blend ideas from the classical motion planning problem addressed in Chap. 7 and from open loop control theory, but require specific developments that we are going to present in the next section. Two main approaches have been devised in order to plan admissible collision-free motions for nonholonomic systems. The first one proposed by [47.13] exploits the idea of the proof of Theorem 47.2 by recursively approximating a not necessarily admissible collision-free path by a sequence of feasible paths. The second approach replaces the local method of probabilistic roadmap method (GlossaryTerm

PRM

) algorithms (Chap. 7) by a local steering method that connects configuration pairs by admissible paths ( ).

Both approaches use a steering method. Before briefly describing them, we give the definition of a local steering method.

Definition 47.2

A local steering method for system $\mathrm{\Sigma }$ is a mapping

$$\begin{array}{rl}{\mathcal{S}}_{\text{loc}}:& \mathcal{C}\mathcal{S}\times \mathcal{C}\mathcal{S}\to C_{\text{pw}}^1\left(\right[0,1],\mathcal{C}\mathcal{S})\\ & ({\mathit{q}}_1,{\mathit{q}}_2)\mapsto {\mathcal{S}}_{\text{loc}}({\mathit{q}}_1,{\mathit{q}}_2)\end{array}$$

5 where ${\mathcal{S}}_{\text{loc}}({\mathit{q}}_1,{\mathit{q}}_2)$ is a piecewise continuously differentiable curve in $\mathcal{C}\mathcal{S}$ satisfying the following properties:

• 1. ${\mathcal{S}}_{\text{loc}}({\mathit{q}}_1,{\mathit{q}}_2)$ satisfies the kinematic constraints associated to $\mathrm{\Sigma }$,

• 2. ${\mathcal{S}}_{\text{loc}}({\mathit{q}}_1,{\mathit{q}}_2)$ connects ${\mathit{q}}_1$ to ${\mathit{q}}_2$: ${\mathcal{S}}_{\text{loc}}({\mathit{q}}_1,{\mathit{q}}_2)\left(0\right)={\mathit{q}}_1$, ${\mathcal{S}}_{\text{loc}}({\mathit{q}}_1,{\mathit{q}}_2)\left(1\right)={\mathit{q}}_2$.

3.2.1 Approximation of a Not Necessarily Admissible Path

A not necessarily admissible collision-free path $\mathrm{\Gamma }\left(t\right),t\in [0,1]$ connecting two configurations and a local steering method ${\mathcal{S}}_{\text{loc}}$ being given, the approximation algorithm proceeds recursively, by calling function approximation defined by Algorithm 47.1 with input $\mathrm{\Gamma }$, 0 and 1.

Algorithm 47.1

approximation function: inputs are a path $\mathrm{\Gamma }$ and two abscissas $t1$ and $t2$ along this path

if ${\mathcal{S}}_{\text{loc}}$($\mathrm{\Gamma }$(t1), $\mathrm{\Gamma }$(t2)) collision free then

 return ${\mathcal{S}}_{\text{loc}}$($\mathrm{\Gamma }$(t1), $\mathrm{\Gamma }$(t2))

else

end if

3.2.2 Sampling-Based Roadmap Methods

Most sampling-based roadmap methods as described in Chap. 6 can be adapted to nonholonomic systems by replacing the connection method between pairs of configurations by a local steering method. This strategy is rather efficient for PRM algorithms. For the rapidly-exploring random tree (GlossaryTerm

RRT

) method, the efficiency strongly depends on the metric used to choose the nearest neighbor. The distance function between two configurations needs to account for the length of the path returned by the local steering method to connect these configurations [47.14].

4 Local Steering Methods and Small-Time Controllability

The approximation algorithm described in the former section is recursive and raises the completeness question: does the algorithm finish in finite time or may it fail to find a solution?

A sufficient condition for the approximation algorithm to find a solution in a finite number of iterations is that the local steering method accounts for small-time controllability.

Definition 47.3

A local steering method ${\mathcal{S}}_{\text{loc}}$ accounts for the small-time controllability of system $\mathrm{\Sigma }$ iff

For any $\mathit{q}\in \mathcal{C}\mathcal{S}$, for any neighborhood U of q, there exists a neighborhood V of q such that for any $\mathit{r}\in V,{\mathcal{S}}_{\text{loc}}(\mathit{q},\mathit{r})\left(\right[0,1\left]\right)\subset U$.

In other words, a local steering method accounts for the small-time controllability of a system if it produces paths getting closer to the configurations it connects when these configurations get closer to each other.

This property is also sufficient for probabilistic completeness of roadmap sampling-based methods.

4.1 Local Steering Methods Accounting for Small-Time Controllability

Constructing a local steering method that accounts for small-time controllability is a difficult task that has been achieved only for a few classes of systems. Most mobile robots studied in the domain of motion planning are wheeled mobile robots towing trailers or not.

4.1.1 Steering Using Optimal Control

The simplest system, namely the differentially-driven robot presented in Sect. 47.2.3 with bounded velocities or the so-called Reeds and Shepp [47.15] car with bounded curvature have the same control vector fields (47.2). The difference lies in the domain of the control variables:

• $-1\le u_1\le 1$, $\left|u_2\right|\le \left|u_1\right|$ for RS car,

• $\left|u_1\right|+b\left|u_2\right|\le 1$ for the differentially-driven robot,

where b is half of the distance between the right and left wheels. For these systems, a local steering method accounting for small-time controllability can be constructed using optimal control theory. For any admissible path defined over an interval I of one of these systems, we define a length as follows

$$\begin{array}{rl}& {\int }_I\left|u_1\right|\text{for the RS car},\\ & {\int }_I\left|u_1\right|+b\left|u_2\right|\text{for the differentially-driven robot}.\end{array}$$

The length of the shortest path between two configurations in both cases defines a metric over the configuration space. The synthesis of the shortest paths, i. e., the determination of the shortest path between any pair of configurations was achieved by [47.16] for the RS car and later by [47.17] for the differentially-driven robot. Figure 47.2 shows a representation of the balls corresponding to these metrics.

Fig. 47.2

(a) Two perspective views of an RS (Reeds and Shepp) ball: set of configurations reachable by a path of length less than a given distance for the Reeds and Shepp car. (b) Perspective views of two DD (differentially driven) balls: set of configurations reachable by a path of a length less than a given distance for the differentially-driven robot. Orientation θ is represented on z-axis

Optimal control naturally defines a local steering method that associates a shortest path between these configurations to any pair of configurations. Let us note that the shortest path is unique between most pairs of configurations. A general result states that the collection of balls of radius r > 0 centered about q induced by nonholonomic metrics constitutes an increasing collection of neighborhoods of q, the intersection of which is $\left\{\mathit{q}\right\}$. This property directly implies that local steering methods based on shortest paths account for small-time controllability.

The main advantage of optimal control is that it provides both a local steering method and a distance metric consistent with the steering method. This makes the steering methods well suited for path planning algorithms designed for holonomic systems and using a distance function, such as RRT (Chap. 7), for instance.

Unfortunately, the synthesis of the shortest paths has been realized only for the two simple systems described in this section. For more complex systems, the problem remains open.

The main drawback of the shortest path-based steering methods described in this section is that input functions are not continuous. This requires an additional step to compute a time-parameterization of the paths before motion execution. Along this time-parameterization, input discontinuities force the robot to stop. For instance, to follow two successive arcs of circles of opposite curvature a mobile robot needs to stop between the arcs of circle in order to ensure continuity of the linear and angular velocities u₁ and u₂.

4.1.2 Steering Chained-Form Systems

Some classes of systems can be put into the form called the chained-form system by a change of variable

$${\dot{z}}_1=u_1,$$

(47.3)

$${\dot{z}}_2=u_2,$$

(47.4)

$${\dot{z}}_3=z_2{u}_1,$$

(47.5)

$$\mathrm{⋮}\mathrm{⋮}{\dot{z}}_n=z_{n-1}{u}_1.$$

(47.6)

Let us consider the following inputs [47.18]

$$\left\{\begin{array}{ll}{u}_1\left(t\right)& =a_0+a_1\sin \omega t,\\ u_2\left(t\right)& =b_0+b_1\cos \omega t\\ & +\mathrm{\dots }+b_{n-2}\cos (n-2)\omega t.\end{array}\right.$$

(47.7)

Let $Z^{\text{start}}\in {\mathbb{R}}^n$ be a starting configuration. Each $z_i\left(1\right)$ can be computed from the coordinates of Z^start and parameters $(a_0,a_1,b_0,b_1,\mathrm{\dots },b_{n-2})$. For a given $a_1\ne 0$ and a given configuration Z^start, the mapping from $(a_0,b_0,b_1,b_2,b_3)$ to $Z\left(1\right)$ is a C¹-diffeomorphism at the origin; the system is then invertible. For n smaller or equal to 5, parameters $(a_0,b_0,b_1,\mathrm{\dots },b_{n-2})$ can be analytically computed from the coordinates of the two configurations Z^start and Z^goal. The corresponding sinusoidal inputs steer the system from Z^start to Z^goal. The shape of the path only depends on parameter a₁. Each value of a₁ thus defines a local steering method denoted by ${\mathcal{S}}_{\text{sin}}^{a_1}$. None of these steering methods account for small-time controllability since for any $Z\in {\mathbb{R}}^n$, ${\mathcal{S}}_{\text{sin}}^{a_1}(Z,Z)\left(\right[0,1\left]\right)$ is not reduced to $\left\{Z\right\}$. To construct a local steering method accounting for small-time controllability from the collection of ${\mathcal{S}}_{\text{sin}}^{a_1}$, we need to make a₁ depend on the configurations Z₁ and Z₂ that we want to connect

$$\begin{array}{rl}& \underset{Z^2\to Z^1}{lim}{a}_1(Z^1,Z^2)=0,\\ & \underset{Z^2\to Z^1}{lim}{a}_0\left[Z^1,,,Z^2,,,a_1,(,Z^1,,,Z^2,)\right]=0,\\ & \underset{Z^2\to Z^1}{lim}{b}_i\left[Z^1,,,Z^2,,,a_1,(,Z^1,,,Z^2,)\right]=\mathrm{0\hspace{0.33em}.}\end{array}$$

Such a construction is achieved in [47.19].

4.1.3 Steering Feedback-Linearizable Systems

The concept of feedback linearizability (or differential flatness ) was introduced by Fliess et al. [47.20, 47.21].

A system is said to be feedback linearizable if there exists an output (i. e., function of the state, input and input derivatives) called the linearizing output, such that the state and the input of the system is a function of the linearizing output and its derivatives. The dimension of the linearizing output is the same as the dimension of the input.

Let us illustrate this notion with a simple example. We consider a differentially-driven mobile robot towing a trailer hitched on top of the wheel axis of the robot, as displayed in Fig. 47.3. The tangent to the curve followed by the center of the wheel axis of the trailer gives the orientation of the trailer. From the orientation of the trailer along the curve, we can deduce the curve followed by the center of the robot. The tangent to the curve followed by the center of the robot gives the orientation of the robot. Thus the linearizing output of this system is the center of the wheel axis of the trailer. By differentiating the linearizing output twice, we can reconstruct the configuration of the system.

Fig. 47.3

A differentially-driven robot towing a trailer. Hitched on top of the wheel axis of the robot is a feedback-linearizable system. The linearizing output is the center of the wheel axis of the trailer. The configuration of the system can be reconstructed by differentiating the curve $\mathit{y}\left(s\right)$, where s is an arc-length parameterization, followed by the linearizing output. The orientation of the trailer is given by $\tau =\arctan ({\dot{y}}_2/{\dot{y}}_1)$. The angle between the robot and the trailer is given by $\varphi =-l\arctan (\mathrm{d}\tau /\mathrm{d}s)$. l is the length of the trailer connection

Feedback linearizability is very interesting for steering purposes. Indeed, the linearizing output is not subject to any kinematic constraints. Therefore, if we know the relation between the state and the linearizing output, planning an admissible path between two configurations simply consists in building a curve in ${\mathbb{R}}^m$, where m is the dimension of the input with differential constraints at both ends. This problem can be easily solved using, for instance, polynomials.

For two input driftless systems like $\mathrm{\Sigma }$, the linearizing output only depends on state q. The state q depends on the linearizing output through the parameterization invariant values, namely, the linearizing output y, the orientation τ of the vector tangent to the curve followed by y and the successive derivatives of τ with respect to the curvilinear abscissa s.

Thus, the configuration of a two input feedback-linearizable driftless system of dimension n can be represented by a vector $(\mathit{y},\tau ,{\tau }^1,\mathrm{\dots },{\tau }^{n-3})$ representing the geometric properties of the curve followed by the linearizing output along an admissible path passing through the configuration.

Therefore, designing a local steering method for such a system is equivalent to associating to any pair of vectors $({\mathit{y}}_1,{\tau }_1,{\tau }_1^1,\mathrm{\dots },{\tau }_1^{n-3})$, $({\mathit{y}}_2,{\tau }_2,{\tau }_2^1,\mathrm{\dots },{\tau }_2^{n-3})$ a curve in the plane starting from ${\mathit{y}}_1$ and ending at ${\mathit{y}}_2$ with orientation of the tangent vector and successive derivatives with respect to s and equal to ${\tau }_1,{\tau }_1^1,\mathrm{\dots },{\tau }_1^{n-3}$ at the beginning and to ${\tau }_2,{\tau }_2^1,\mathrm{\dots },{\tau }_2^{n-3}$ at the end. This exercise is relatively easy using polynomials and transforming the boundary conditions into linear equations over the coefficients of the polynomials. However, taking into account small-time controllability is a little bit more tricky. Reference [47.22] proposes a flatness-based steering method built on convex combinations of canonical curves.

4.2 Equivalence Between Chained-Formed and Feedback-Linearizable Systems

In the previous section, we proposed methods to steer feedback-linearizable control systems or systems that can be put into chained-form. We now give a necessary and sufficient condition for feedback-linearizability.

4.2.1 Feedback-Linearizability: A Necessary and Sufficient Condition

In [47.23], Rouchon gives conditions to check whether or not a system is feedback-linearizable. For two-input driftless systems a necessary and sufficient condition is the following: let us define as ${\mathrm{\Delta }}^k$, k > 0 the collection of distributions (i. e., set of vector fields) iteratively defined by: ${\mathrm{\Delta }}_0=\mathrm{span}\{f_1,f_2\}$, ${\mathrm{\Delta }}_1=\mathrm{span}\{f_1,f_2,[f_1,f_2\left]\right\}$ and ${\mathrm{\Delta }}_{i+1}={\mathrm{\Delta }}_0+[{\mathrm{\Delta }}_i,{\mathrm{\Delta }}_i]$ with $[{\mathrm{\Delta }}_i,{\mathrm{\Delta }}_i]=\mathrm{span}\left\{\right[f,g],f\in {\mathrm{\Delta }}_i,g\in {\mathrm{\Delta }}_i\}$. A system with two-dimensional input is feedback-linearizable iff $\mathrm{rank}\left({\mathrm{\Delta }}_i\right)=2+i$.

4.2.2 Example: The Chained-Form System

Let us consider the chained-form system defined by (47.3)–(47.6). The control vector fields of this system are

$$f_1=(1,0,z_2,\mathrm{\dots },z_{n-1}),f_2=(0,1,\mathrm{\dots },0),$$

$\mathrm{rank}{\mathrm{\Delta }}_0=2$. If we compute $f_3=[f_1,f_2]=(0,0,1,0,\mathrm{\dots },0)$, we note that $\mathrm{rank}{\mathrm{\Delta }}_1=3$. By computing $f_i=[f_1,f_{i-1}]$ for i up to n, we find a sequence $f_i=(0,\mathrm{\dots },0,1,0,\mathrm{\dots },0)$, where 1 is at position i. Therefore, $\mathrm{rank}{\mathrm{\Delta }}_i=2+i$ for i up to $n-2$ and the chained-form system is feedback linearizable. This conclusion could have been drawn in a more straightforward way by noticing that the state can be reconstructed from $(z_1,z_2)$ and its derivatives. $(z_1,z_2)$ is thus the linearizing output of the chained-form system.

5 Robots and Trailers

Robotics systems studied in motion planning are mainly those composed of a mobile robot alone or towing one or several trailers. The input of these systems is two dimensional.

5.1 Differentially-Driven Mobile Robots

The simplest mobile robot, namely the differentially-driven mobile robot displayed in Fig. 47.1, is obviously feedback-linearizable. the trajectory of the center of the wheel axis (the linearizing output) of the robot gives the orientation of the robot. It can thus be steered using a flatness-based local steering method.

5.2 Differentially-Driven Mobile Robots Towing One Trailer

The differentially-driven mobile robot towing a trailer hitched on top of the wheel axis of the robot displayed in Fig. 47.3 is feedback linearizable and the linearizing output is the center of the wheel axis of the trailer. The differentially-driven mobile robot towing a trailer with hitched kingpin (Fig. 47.4) is also feedback linearizable [47.24] but the relation between linearizing output and configuration variables is more intricate

$$\left\{\begin{array}{rl}{y}_1& =x_1-b\cos \theta +L\left(\varphi \right)\frac{b\sin \theta +a\sin (\theta +\varphi )}{\sqrt{a^2+b^2+2ab\cos \varphi }},\\ y_2& =x_2-b\sin \theta -L\left(\varphi \right)\frac{a\cos (\theta +\varphi )+b\cos \theta }{\sqrt{a^2+b^2+2ab\cos \varphi }},\end{array}\right.$$

where L is defined by the following elliptic integral

$$L\left(\varphi \right)=ab{\int }_0^{\varphi }\frac{\cos \sigma }{\sqrt{a^2+b^2+2ab\cos \left(\sigma \right)}}\mathrm{d}\sigma ,$$

which is the linearizing output of the system. These relations together with the following ones

$$\begin{array}{rl}\tan \tau & =\frac{a\sin (\theta +\varphi )+b\sin \theta }{b\cos \theta +a\cos (\theta +\varphi )},\\ \kappa & =\frac{\sin \left(\varphi \right)}{\cos \varphi \sqrt{a^2+b^2+2ab\cos \varphi }+L\left(\varphi \right)\sin \left(\varphi \right)},\end{array}$$

make it possible to reconstruct the configuration variables from the linearizing output and its two first derivatives.

Fig. 47.4

A differentially-driven mobile robot towing a trailer with hitched kingpin is feedback linearizable

5.3 Car-Like Mobile Robots

A car-like mobile robot is constituted of a fixed rear wheel axis and of two steerable front wheels, the axes of which intersect at the center of curvature (Fig. 47.5). A car-like mobile robot is kinematically equivalent to a differentially-driven mobile robot towing a trailer hitched on top of the wheel axis of the robot (Fig. 47.3): the virtual front wheel corresponds to the differentially-driven robot, while the body of the car corresponds to the trailer.

Fig. 47.5

Car-like mobile robot. The axes of the front wheels intersect at the center of curvature. The steering angle ϕ is the angle between the longitudinal axis of the car and a virtual front wheel in the middle of the two front wheels

5.4 Bi-Steerable Mobile Robots

The bi-steerable mobile robot (Fig. 47.6) is a car with front and rear steerable wheels and with a relation between the front and rear steering angles. This system has been proved to be feedback linearizable in [47.25]. As for the mobile robot towing a trailer with hitched kingpin, the linearizing output is a moving point in the robot reference frame.

Fig. 47.6

Bi-steerable robot. The front and rear wheels are steerable and there is a relation between the rear steering angle and the front steering angle: $\alpha =f\left(\varphi \right)$

5.5 Differentially-Driven Mobile Robots Towing Trailers

Let us consider the differentially-driven mobile robot towing a trailer connected on top of the wheel axis of the robot, and let us add an arbitrary number of trailers, each one connected on top of the wheel axis of the previous one. By differentiating once the curve followed by the center of the last trailer, we get the orientation of the last trailer (this orientation coincides with the orientation of the tangent to the curve). If we know the orientation and the position of the center of the last trailer along the path, we can reconstruct the curve followed by the center of the previous trailer. Repeating this reasoning, we can reconstruct the trajectory of the whole system by differentiating a sufficient number of times. The system is therefore feedback linearizable and the linearizing output is the center of the last trailer.

Combining the above reasoning with the systems reviewed in this section, we can build hybrid feedback-linearizable trailer systems. For instance, a differentially-driven mobile robot towing an arbitrary number n of trailers each one connected on top of the wheel axis of the previous trailer, except the last trailer with hitched kingpin is feedback linearizable. Simply consider the two last trailers as a feedback-linearizable system composed of a mobile robot with one trailer as in Fig. 47.4. The linearizing output of this system enables us to reconstruct the trajectory of the two last trailers. The center of trailer $n-1$ is a linearizing output for the mobile robot towing the $n-1$ last trailers.

5.6 Open Problems

Finally, all the systems for which we are able to plan exact motions between arbitrary pairs of configurations are included in the large class of feedback-linearizable systems. We have indeed seen that chained-form systems are also part of this class. For other systems, no exact solution has been proposed up to now. For instance, neither of the systems displayed in Fig. 47.7 is feedback linearizable. They do not satisfy the necessary condition of Sect. 47.4.2.

Fig. 47.7

A truck towing a towbar trailer with hitched kingpin and a differentially-driven mobile robot towing two trailers with hitched kingpin: two open problems for exact path planning for nonholonomic systems

6 Approximate Methods

To deal with nonholonomic systems not belonging to any class of systems for which exact solutions exist, numerical approximate solutions have been developed. We review some of these methods in this section.

6.1 Forward Dynamic Programming

In [47.26] Barraquand and Latombe propose a dynamic programming approach to nonholonomic path planning. Admissible paths are generated by a sequence of constant input values, applied over a fixed interval of time $\mathrm{\delta }\mathrm{t}$. Starting from the initial configuration the search generates a tree: the children of a given configuration q are obtained by setting the input to a constant value and integrating the differential system over $\mathrm{\delta }\mathrm{t}$. The configuration space is discretized into an array of cells of equal size (i. e., hyper-parallelepipeds). A child ${\mathit{q}}^{\prime }$ of a configuration q is inserted in the search tree if and only if the computed path from q to ${\mathit{q}}^{\prime }$ is collision free and ${\mathit{q}}^{\prime }$ does not belong to a cell containing an already generated configuration. The algorithm stops when it generates a configuration belonging to the same cell as the goal (i. e., it does not necessarily reach the goal exactly).

The algorithm has been proved to be asymptotically complete with respect to both $\mathrm{\delta }\mathrm{t}$ and the size of the cells. As a brute force method, it remains quite time consuming in practice. Its main interest is that the search is based on Dijkstra’s algorithm, which allows taking into account optimality criteria such as the path length or the number of reversals. Asymptotical optimality to generate the minimum of reversals is proved for the car-like robot alone.

6.2 Discretization of the Input Space

In [47.27] Divelbiss and Wen propose a method to produce an admissible collision-free path for a nonholonomic mobile robot in the presence of obstacles. They restrict the set of input functions over the subspace spanned by Fourier basis over interval $[0,1]$. An input function is thus represented by a finite-dimensional vector λ. Reaching a goal configuration thus becomes a nonlinear system of equations, the unknowns of which are the coordinates λ_i’s of λ. The authors use the Newton–Raphson method to find a solution. Obstacles are defined by inequality constraints over the configuration space. The path is discretized into N samples. The noncollision constraint expressed at these sample points yields the inequality constraint over vector λ. These inequality constraints are turned into equality constraints through function g defined as

$$g\left(c\right)=\left\{\begin{array}{ll}(1-{\mathrm{e}}^c{)}^2& \text{if}c>0,\\ 0& \text{if}c\le \mathrm{0\hspace{0.33em}.}\end{array}\right.$$

Reaching a goal configuration while avoiding obstacles thus becomes a nonlinear system of equations over vector λ again solved using the Newton–Raphson method. The method is rather efficient for short maneuvers. The main difficulty is to tune the order of the Fourier expansion. Long motions in cluttered environments require a higher order, while motion in empty space can be solved with a low order. The authors do not mention the problem of numerical instability in the integration of the dynamic system.

6.3 Input-Based Rapidly Exploring Random Trees

The RRT algorithms described in Chap. 7 can be used without a local steering method to plan paths for nonholonomic systems. New nodes can be generated from existing nodes by applying random input functions over an interval of time [47.28]. The main difficulty consists in finding a distance function that really accounts for the distance the system needs to travel to go from one configuration to another. Moreover, the goal is never exactly reached. This latter drawback can be overcome by post-processing the path returned by RRT using a path deformation method, as described in [47.29].

7 From Motion Planning to Obstacle Avoidance

Up to now we have described motion planning techniques. Their objective is to compute a collision-free trajectory to the target configuration that complies with the vehicle constraints. They assume a perfect model of the robot and scenario. The advantage of these techniques is that they provide complete and global solutions of the problem. Nevertheless, when the surroundings are unknown and unpredictable, these techniques fail.

A complementary way to face the motion problem is obstacle avoidance. The objective is to move a vehicle towards a target location free of collisions with the obstacles collected by the sensors during motion execution. The advantage of reactive obstacle avoidance is to compute motion by introducing the sensor information within the control loop, used to adapt the motion to any contingency incompatible with initial plans.

The main cost of considering the reality of the world during execution is locality. In this instance, if global reasoning is required, a trap situation could occur. Despite this limitation, obstacle avoidance techniques are mandatory to deal with mobility problems in unknown and evolving surroundings.

Notice that methods have been developed to combine both the global point of view of motion planning and the local point of view of obstacle avoidance. How can we consider robot perception at the planning level? This is so-called sensor-based motion planning. Several variants exists, such as the Bug algorithms initially introduced in [47.30]. However, non of them consider the practical context of nonholonomic mobile robots.

8 Definition of Obstacle Avoidance

Let $\mathcal{A}$ be the robot (a rigid object) moving in the workspace $\mathcal{W}$, whose configuration space is $\mathcal{C}\mathcal{S}$. Let q be a configuration, q_t this configuration in time t, $\mathcal{A}\left({\mathit{q}}_t\right)\in \mathcal{W}$ the space occupied by the robot in this configuration. In the vehicle there is a sensor, which in q_t measures a portion of the space $\mathcal{S}\left({\mathit{q}}_t\right)\subset \mathcal{W}$ identifying a set of obstacles $\mathcal{O}\left({\mathit{q}}_t\right)\subset \mathcal{W}$.

Let u be a constant control vector and $\mathit{u}\left({\mathit{q}}_t\right)$ this control vector applied in q_t during time $\mathrm{\delta }\mathrm{t}$. Given $\mathit{u}\left({\mathit{q}}_t\right)$, the vehicle describes a trajectory ${\mathit{q}}_{t+\mathrm{\delta }\mathrm{t}}=f(\mathit{u},{\mathit{q}}_t,\mathrm{\delta }\mathrm{t})$, with $\mathrm{\delta }\mathrm{t}\ge 0$. Let ${\mathcal{Q}}_{t,T}$ be the set of configurations of the trajectory followed from q_t with $\mathrm{\delta }\mathrm{t}\in [0,\mathrm{T}]$, a given time interval. T > 0 is called the sampling period. Let $F:\mathcal{C}\mathcal{S}\times \mathcal{C}\mathcal{S}\to {\mathbb{R}}^{+}$ be a function that evaluates the progress of one configuration to another.

8.1 Obstacle Avoidance Problem

Let q_target be a target configuration. Then, in time t_i the robot $\mathcal{A}$ is in ${\mathit{q}}_{t_i}$, where a sensor measurement is obtained $\mathcal{S}\left({\mathit{q}}_{t_i}\right)$, and thus an obstacle description $\mathcal{O}\left({\mathit{q}}_{t_i}\right)$.

The objective is to compute a motion control ${\mathit{u}}_i$, such that $\left(i\right)$ the trajectory generated is free of collisions with the obstacles $\mathcal{A}\left({\mathcal{Q}}_{t_i,T}\right)\bigcap \mathcal{O}\left({\mathit{q}}_{t_i}\right)=\mathrm{\varnothing }$; and (ii) it makes the vehicle progress to the target location $F({\mathit{q}}_{t_i},{\mathit{q}}_{\mathrm{target}})<F({\mathit{q}}_{t_i+T},{\mathit{q}}_{\mathrm{target}})$.

The result of solving this problem at each sample time (Fig. 47.8a) is a sequence of motion controls $\left\{{\mathit{u}}_1\mathrm{\dots }{\mathit{u}}_n\right\}$ computed in execution time that avoids the obstacles gathered by the sensors, while making the vehicle progress towards the target location in each configuration $\left\{{\mathit{q}}_{t_1}\mathrm{\dots }{\mathit{q}}_{\text{target}}\right\}$ (Fig. 47.8b). Notice that the motion problem is a global problem. The obstacle avoidance methods are local and iterative techniques to address this problem. The disadvantage of locality (trap situations) is counteracted with the advantage of introducing the sensory information within the control cycle (to take into account the reality of the world in execution).

Fig. 47.8

(a) Obstacle avoidance problem consists of computing motion control that avoids collisions with the obstacles gathered by the sensors whilst driving the robot towards the target location. (b) The result of applying this technique at each time is a sequence of motions that drive the vehicle free of collisions to the target

There are at least three aspects that affect the development of an obstacle avoidance method: the avoidance technique, the type of robot-sensor and the type of scenario. These subjects correspond to the next three sections. First, we describe the obstacle avoidance techniques (Sect. 47.9). Second, we discuss the techniques to adapt a given obstacle avoidance method to work on a vehicle taking into account the shape, kinematics, and dynamics (Sect. 47.10). The sensory processing is detailed in Chaps. 5 and 25 of this book. Finally, the usage of an obstacle avoidance technique on a vehicle in a given scenario is highly dependent on the scenario nature (static or dynamic, unknown or known, structured or not, or its size, for example). Usually, this problem is associated with the integration of planning and obstacle avoidance (Sect. 47.11).

9 Obstacle Avoidance Techniques

We describe here a taxonomy of obstacle avoidance techniques and some representative methods. First there are two groups, methods that compute the motion in one step and methods that do it in more than one. Methods of one step directly reduce the sensor information to motion control. There are two types:

• The heuristic methods were the first techniques used to generate motion based on sensors. The majority of this work was derived in classic planning methods and will not be described here. See [47.1, 47.30, 47.31, 47.32, 47.33] for some representative work.

• The methods of physical analogies assimilate the obstacle avoidance to a known physical problem. We discuss here the potential field methods [47.34, 47.35]. Other works are variants adapted to uncertain models [47.36] or that use other analogies [47.37, 47.38, 47.39].

Methods with more than one step compute some intermediate information, which is processed next to obtain the motion:

• The methods of subset of controls compute an intermediate set of motion controls, and next they choose one of them as a solution. There are two types: (i) methods that compute a subset of motion directions. We describe here the vector field histogram (GlossaryTerm

  VFH

  ) [47.40] and the obstacle restriction method [47.41] (see [47.42] for three-dimensional (GlossaryTerm

  3-D

  ) workspaces). Another method is the steer angle field approach [47.43]. (ii) Methods that compute a subset of velocity controls. We describe here the dynamic window approach (GlossaryTerm

  DWA

  ) [47.44] and velocity obstacles [47.45] (see [47.46] for moving obstacles). Another method based on similar principles but developed independently is the curvature velocity method [47.47].

• Finally, there are methods that compute some high-level information such as intermediate information, which is translated next in motion. We describe the basic versions of the nearness diagram navigation  [47.48, 47.49]. The reader is directed to [47.50, 47.51, 47.52] for further improvements.

All the methods outlined here have advantages and disadvantages, depending on the navigation context, like uncertain worlds, motion at high speeds, motion in confined or troublesome spaces, etc. Unfortunately there is no metric available to quantitatively measure the performance of the methods. However, for an experimental comparison in terms of their intrinsic problems see [47.48].

9.1 Potential Field Methods

The potential field method (GlossaryTerm

PFM

) uses an analogy in which the robot is a particle that moves in the configuration space under the influence of a force field. While the target location exerts a force that attracts the particle F_att, the obstacles exert repulsive forces F_rep. At each time t_i, the motion is computed to follow the direction of the artificial force induced by the sum of both potentials ${\mathit{F}}_{\text{tot}}\left({\mathit{q}}_{t_i}\right)={\mathit{F}}_{\text{att}}\left({\mathit{q}}_{t_i}\right)+{\mathit{F}}_{\text{rep}}\left({\mathit{q}}_{t_i}\right)$ (the most promising motion direction), Fig. 47.9.

Fig. 47.9

(a) Computation of the motion direction with a potential field method. The target attracts the particle F_att while the obstacle exerts a repulsive force F_rep. The resulting force F_tot is the most promising motion direction. (b) Motion directions computed in each point of the space with the classic method

9.1.1 Example

$${\mathit{F}}_{\text{att}}\left({\mathit{q}}_{t_i}\right)=K_{\text{att}}{\mathit{n}}_{q_{\text{target}}}$$

(47.8)

$${\mathit{F}}_{\text{rep}}\left({\mathit{q}}_{t_i}\right)=\left\{\begin{array}{ll}{K}_{\text{rep}}{{\displaystyle \sum }}_j\left({\displaystyle \frac{1}{d({\mathit{q}}_{t_i},{\mathit{p}}_j)}},-,{\displaystyle \frac{1}{d_0}}\right){\mathit{n}}_{p_j}& \text{if}d({\mathit{q}}_{t_i},{\mathit{p}}_j)<d_0,\\ 0& \text{otherwise}\end{array}\right.$$

(47.9)

where K_att and K_rep are the constants of the forces, d₀ is the influence distance of the obstacles p_j, ${\mathit{q}}_{t_i}$ is the current vehicle configuration and ${\mathit{n}}_{{\mathit{q}}_{\text{target}}}$ and ${\mathit{n}}_{p_j}$ are the unitary vectors that point from ${\mathit{q}}_{t_i}$ to the target and each obstacle p_j. From ${\mathit{F}}_{\text{tot}}\left({\mathit{q}}_{t_i}\right)$, the control u_i can be obtained with a position or force control [47.53].

This is the classic version, where the potentials only depend on the current vehicle configuration. Complementarily the generalized potentials depend also on the instantaneous robot velocity and vehicle accelerations.

9.1.2 Example

$${\mathit{F}}_{\text{rep}}\left({\mathit{q}}_{t_i}\right)=\left\{\begin{array}{l}{K}_{\text{rep}}\sum _j\left(\frac{a{\dot{\mathit{q}}}_{t_i}}{\left[2ad\right({\mathit{q}}_{t_i},{\mathit{p}}_j)-{{\dot{\mathit{q}}}_{t_i}}^2]}\right){\mathit{n}}_{p_j}\cdot {\mathit{n}}_{{\dot{\mathit{q}}}_{t_i}}\\ \text{if}{\dot{\mathit{q}}}_{t_i}>0,\\ 0\text{otherwise}\end{array}\right.$$

(47.10)

where ${\dot{\mathit{q}}}_{t_i}$ is the current robot velocity, ${\mathit{n}}_{{\dot{\mathit{q}}}_{t_i}}$ the unitary vector pointing in the direction of the robot velocity, and a is the maximum vehicle acceleration. This expression arises when the repulsive potential is defined as the inverse of the difference between the estimated time until a collision takes place and the time needed to stop the robot until full backward acceleration. Notice how the repulsive only affect in the direction of motion of the vehicle, as opposed to the classic potential. For a comparison of the classic and generalized versions, and the relation with the way to compute the motion controls next, see [47.53]. This method is widely used because it is easy to understand and due to its clear mathematic formalism.

9.2 Vector Field Histogram

The vector field histogram (GlossaryTerm

VFH

) solves the problem in two steps by computing a set of candidate motion directions and then selecting one of them.

9.2.1 Candidate Set of Directions

Firstly the space is divided into sectors from the robot location. The method uses a polar histogram H constructed around the robot, where each component represents the obstacle polar density in the corresponding sector. The function that maps the obstacle distribution in sector k on the corresponding component of the histogram $h^k\left({\mathit{q}}_{t_i}\right)$ is

$$h^k\left({\mathit{q}}_{t_i}\right)={\int }_{{\mathrm{\Omega }}_k}P(\mathit{p}{)}^n.{\left(1,-,\frac{d({\mathit{q}}_{t_i},\mathit{p})}{d_{\text{max}}}\right)}^m\mathrm{d}\mathit{p}.$$

(47.11)

The dominion of integration is ${\mathrm{\Omega }}_k=\{\mathit{p}\in \mathcal{W}\backslash \mathit{p}\in k\wedge d({\mathit{q}}_{t_i},\mathit{p})<d_0\}$. The density $h^k\left({\mathit{q}}_{t_i}\right)$ is proportional to the probability $P\left(\mathit{r}\right)$ that a point was occupied by an obstacle, and to a factor that increases when the distance to the point decreases.

Typically the resulting histogram has peaks (directions with high density of obstacles) and valleys (directions with low density). The set of candidate directions is the set of adjacent components with density lower than a given threshold, and closest to the component that contains the target direction. This set of components (sectors) is called the selected valley, and represents a set of candidate directions (Fig. 47.10).

Fig. 47.10a,b

Computation of the motion direction θ_sol with VFH. (a) Robot and obstacle occupancy distribution. (b) The candidate valley is the set of adjacent components with values lower than the threshold. The navigation case is Case 3, since the sector of the target k_target is not in the valley and the number of sectors is lower than a fixed quantity m (m = 8, i. e., 45 °). Thus the solution is $k_{\text{sol}}=(k_i+k_j)/2$, whose bisector is θ_sol in (a). The bisectors of k_i and k_j are θ_i and θ_j, respectively

9.2.2 Motion Computation

The objective of the next step is to choose a direction of this set. The strategy is to apply three heuristics that depend on the component that contains the target or on the size of the selected valley. The cases are checked in sequence:

• 1. Case 1: goal sector in the selected valley. Solution: $k_{\text{sol}}=k_{\text{target}}$, where k_target is the sector that contains the goal location.

• 2. Case 2: goal sector not in the selected valley and number of sectors of the valley greater than m. Solution: $k_{\text{sol}}=k_i\pm m/2$, where m is a fixed number of sectors and k_i the sector of the valley closer to the k_target.

• 3. Case 3: goal sector not in the selected valley and number of sectors of the valley lower or equal to m. Solution: $k_{\text{sol}}=(k_i+k_j)/2$, where k_i and k_j are the extreme sectors of the valley.

The result is a component or sector k_sol, whose bisector is the direction solution θ_sol. The velocity v_sol is inversely proportional to the distance to the closest obstacle. The control is ${\mathit{u}}_i=(v_{\text{sol}},{\theta }_{\text{sol}})$.

VFH is a method formulated to work with probability obstacle distributions, and thus, it is well adapted to work with uncertain sensors as ultrasonic sonars.

9.3 The Obstacle Restriction Method

The obstacle restriction method (GlossaryTerm

ORM

) solves the problem in three steps, where the result of the two first steps is a set of candidate motion directions. The first step is to compute an instantaneous subgoal if necessary. The second one associates a motion constraint to each obstacle and joins them next to compute the set of desirable directions. The last step is an strategy to compute the motion given this set.

9.3.1 Instantaneous Target Selection

This step computes a subgoal when is better to direct the motion towards a given zone of the space (which ameliorates the situation to reach the goal latter), rather than directly towards the goal itself. The subgoals are located in between obstacles or in the edge of an obstacle (Fig. 47.11a). Next, the process is to check with a local algorithm whether the goal can be reached from the robot location. If it is not, then the closest reachable subgoal to the goal is selected. To check if a point can be reached, there is a local algorithm that computes the existence of a local path that joins two locations [47.48].

Fig. 47.11

(a) Distribution of the subgoals x_i. The selected instantaneous target location is x₂. The set of candidate directions is $S_{n\text{D}}$ and the solution is θ_sol (the second case). (b) The two sets of not desirable directions S₁ and S₂ for a given obstacle

Let x_a and x_b be two locations of the space, R the robot radius, and L a list of obstacle points, where p_i is an obstacle of the list. Let $\mathcal{A}$ and $\mathcal{B}$ be the two semiplanes divided by the line that joins x_a and x_b. Then, if for all the points of L, $d({\mathit{p}}_j,{\mathit{p}}_k)>2R$ (with ${\mathit{p}}_j\in \mathcal{A}$ and ${\mathit{p}}_k\in \mathcal{B}$), there is a collision free path that joins both locations. If this condition is not satisfied, then there is no local path (although a global one could exist). The interesting result is when the result is positive, since the guarantee that one point can be reached from the other exists.

The result of the process is the goal or an instantaneous subgoal (from now this location is called the target location). Note that this process is general and can be used as a preprocess step by other methods to instantaneously validate the goal location or to compute an instantaneous subgoal to drive the vehicle.

9.3.2 Candidate Set of Directions

For each obstacle i a set of not desirable directions for motion ${\mathcal{S}}_{n\text{D}}^i$ is computed (motion constraint). This set is the union of two subsets ${\mathcal{S}}_1^i$ and ${\mathcal{S}}_2^i$. ${\mathcal{S}}_1^i$ represents the side of the obstacle not suitable to do the avoidance and ${\mathcal{S}}_2^i$ is an exclusion area around the obstacle (Fig. 47.11b). The motion constraint for the obstacle is the union of both sets $S_{n\text{D}}^i=S_1^i\cup S_2^i$. The set of desired directions of motion is the complementary $S_{\text{D}}=\left\{\right[-\pi ,\pi ]\backslash {\mathcal{S}}_{n\text{D}}\}$, where ${\mathcal{S}}_{n\text{D}}={\cup }_i{\mathcal{S}}_{n\text{D}}^i$.

9.3.3 Motion Computation

The final step is to select a direction of motion. There are three cases that depend on the set of desirable directions S_D and on the target direction θ_target. The cases are checked in sequence:

• 1. Case 1: $S_{\text{D}}\ne \mathrm{\varnothing }$ and ${\theta }_{\text{target}}\in S_{\text{D}}$. Solution: ${\theta }_{\text{sol}}={\theta }_{\text{target}}$.

• 2. Case 2: $S_{\text{D}}\ne \mathrm{\varnothing }$ and ${\theta }_{\text{target}}\notin S_{\text{D}}$. Solution: ${\theta }_{\text{sol}}={\theta }_{\text{lim}}$, where θ_lim is the direction of S_D closest to θ_target.

• 3. Case 3: $S_{\text{D}}=\mathrm{\varnothing }$. Solution: ${\theta }_{\text{sol}}=({\varphi }_{\text{lim}}^l+{\varphi }_{\text{lim}}^r)/2$, where ${\varphi }_{\text{lim}}^l$ and ${\varphi }_{\text{lim}}^r$ are the directions of $S_{{\text{D}}_{\mathrm{r}}}$ and $S_{{\text{D}}_{\mathrm{l}}}$ closer to θ_target ($S_{{\text{D}}_{\mathrm{r}}}$ and $S_{{\text{D}}_{\mathrm{l}}}$ are the set of desirable directions of obstacles on the left and right-hand sides of the target, respectively).

The result is a direction of motion solution θ_sol. The velocity v_sol is inversely proportional to the distance to the closest obstacle. The control is ${\mathit{u}}_i=(v_{\text{sol}},{\theta }_{\text{sol}})$. This is a geometric-based method based on cases. The advantage is that it has been demonstrated to address effective motion in confined spaces.

9.4 Dynamic Window Approach

The dynamic window approach (GlossaryTerm

DWA

) ( ) is a method that solves the problem in two steps, by computing as intermediate information a subset of the control space $\mathcal{U}$. For simplicity, we consider a motion control such as translational and rotational velocity $(v,w)$; $\mathcal{U}$ is defined by

$$\mathcal{U}=\left\{\right(v,w)\in {\mathbb{R}}^2\backslash v\in [-v_{\text{max}},v_{\text{max}}]\wedge w\in [-w_{\text{max}},w_{\text{max}}\left]\right\}.$$

(47.12)

9.4.1 Set of Candidate Controls

The candidate set of controls ${\mathcal{U}}_{\text{R}}$ contains the controls: (i) within the maximum velocities of the vehicle $\mathcal{U}$, (ii) that generate safe trajectories ${\mathcal{U}}_{\text{A}}$, and (iii) that can be reached within a short period of time given the vehicle accelerations ${\mathcal{U}}_{\text{D}}$. The set ${\mathcal{U}}_{\text{A}}$ contains the admissible controls. These controls can be canceled before collision by applying the maximum deceleration $(a_v,a_w)$

$${\mathcal{U}}_{\text{A}}=\left\{(,v,,,w,),\in ,\mathcal{U},|,v,\le ,\sqrt{2d_{\text{obs}}{a}_v},\wedge ,w,\le ,\sqrt{2{\theta }_{\text{obs}}{a}_w}\right\},$$

(47.13)

where d_obst and θ_obst are the distance to the obstacle and the orientation of the tangent to the trajectory in the obstacle. The set ${\mathcal{U}}_{\text{D}}$ contains the controls reachable in a short period

$${\mathcal{U}}_{\text{D}}=\left\{\right(v,w)\in \mathcal{U}\backslash v\in [v_o-a_{v}T,v_o+a_{v}T]\wedge w\in [w_o-a_{w}T,w_o+a_{w}T\left]\right\},$$

(47.14)

where ${\dot{\mathit{q}}}_{t_i}=(v_o,w_o)$ is the current velocity.

The resulting subset of controls is (Fig. 47.12)

$${\mathcal{U}}_{\text{R}}=\mathcal{U}\cap {\mathcal{U}}_{\text{A}}\cap {\mathcal{U}}_{\text{D}}.$$

(47.15)

Fig. 47.12

Subset of controls ${\mathcal{U}}_{\text{R}}=\mathcal{U}\cap {\mathcal{U}}_{\text{A}}\cap {\mathcal{U}}_{\text{D}}$, where $\mathcal{U}$ contains the controls within the maximum velocities, ${\mathcal{U}}_{\text{A}}$ the admissible controls, and ${\mathcal{U}}_{\text{D}}$ the controls reachable in a short period of time

9.4.2 Motion Computation

The next step is to select one control ${\mathit{u}}_i\in {\mathcal{U}}_{\text{R}}$. The problem is set out as the maximization of an objective function

$$G\left(\mathit{u}\right)={\alpha }_1\cdot \text{Goal}\left(\mathit{u}\right)+{\alpha }_2\cdot \text{Clearance}\left(\mathit{u}\right)+{\alpha }_3\cdot \text{Velocity}\left(\mathit{u}\right).$$

(47.16)

This function is a compromise among $\mathrm{Goal}\left(\mathit{u}\right)$ that favors velocities that offer progress to the goal, $\mathrm{Clerance}\left(\mathit{u}\right)$ that favors velocities far from the obstacles, and $\mathrm{Velocity}\left(\mathit{u}\right)$ that favors high speeds. The solution is the control u_i that maximizes this function.

DWA solves the problem in the control space using information of the vehicle dynamics, thus the method is well adapted to work on vehicles with slow dynamic capabilities or that work at high speeds.

9.5 Velocity Obstacles

The method of velocity obstacles (GlossaryTerm

VO

) solves the problem in two steps, by computing as intermediate information a subset of the $\mathcal{U}$. The framework is equal to that of DWA. The difference is that the computation of the set of safe trajectories ${\mathcal{U}}_{\text{A}}$ takes into account the velocity of the obstacles, which is described next.

Let v_i be the velocity of obstacle i (which became enlarged with the vehicle radius occupying an area B_i) and u a given vehicle control. The set of colliding relative velocities is called collision cone

$$C{C}_i=\left\{{\mathit{u}}_i,|,{\lambda }_i,\bigcap ,B_i,\ne ,\mathrm{\varnothing }\right\},$$

(47.17)

where λ_i is the direction of the unitary vector ${\mathit{u}}_i={\mathit{u}}_i-{\mathit{v}}_i$. The velocity obstacle is this set in a common absolute system of reference

$$V{O}_i=C{C}_i\oplus {\mathit{v}}_i,$$

(47.18)

where $\oplus$ is the Minkowski vector sum. The set of unsafe trajectories is the union of the velocity obstacles for each moving obstacle ${\overline{\mathcal{U}}}_{\text{A}}={\cup }_{i}V{O}_i$ (Fig. 47.13). The advantage of this method is that takes into account the velocity of the obstacles; thus it is well suited in dynamic scenarios.

Fig. 47.13

Subset of not safe controls $\overline{{\mathcal{U}}_{\text{A}}}=V{O}_1\cup V{O}_2$. A control vector out of this velocity generates a collision-free motion with the moving obstacles

9.6 Nearness Diagram Navigation

This method is more a methodology to design obstacle avoidance methods rather than a method in itself. Nearness diagram navigation (GlossaryTerm

ND

) is an obstacle avoidance method obtained with a geometric implementation following this methodology. The idea behind is to employ a divide and conquer strategy based on situations to simplify the obstacle avoidance problem following the situated-activity paradigm [47.54]. First, there is a set of situations that represent all the cases among robot locations, obstacles, and target locations. Also, for each case there is a motion law associated with it. During the execution phase, at time t_i one situation is identified and the corresponding law is used to compute the motion.

9.6.1 Situations

The situations are represented in a binary decision tree. The selection of a situation depends on the obstacles $\mathcal{O}\left({\mathit{q}}_{t_i}\right)$, on the robot location ${\mathit{q}}_{t_i}$ and target q_target. The criteria are based on high-level entities, like a security distance around the robot bounds and a motion area (that identifies suitable areas of motion). For example, one criterion is if there are obstacles within the security zone. Another is if the motion area is large or narrow. The result is only one situation, since by definition and representation (binary decision tree) the set of situations is complete and exclusive (Fig. 47.14).

Fig. 47.14

(a) Design diagram of the method. Given the obstacle information and the target location, one situation is identified given some criteria. Next the associated action is executed computing the motion. (b) Example of computation of the solution of the ND (geometric implementation). The first step is to identify the situation. There are no obstacles closer than the security distance D_s. Next q_target is not within the motion area. Third, the motion area is wide. With these three criteria we identify the current situation, HSWR. In this situation, the associated action computes the control as ${\mathit{u}}_i=(v_{\text{sol}},{\theta }_{\text{sol}})$, where v_sol is the maximum velocity and θ_sol is computed as a deviation α from the limit direction θ_disc of the motion area closer to the target direction

9.6.2 Actions

Associated to any situation there is an action that computes the motion to adapt the behavior to the case represented by each situation. At a high level the actions describe the behavior desired in each situation. For example, one situation is when there are no obstacles within a security area and the goal is within the motion area (GlossaryTerm

HSGR

). The solution is to move towards the goal. Another situation is when there are no obstacles within a security area, and the goal is not within the motion area but is wide (GlossaryTerm

HSWR

). The solution is to move towards the limit of the motion area but clearing the security area of obstacles.

The interesting aspect of this method is that it employs a divide and conquer strategy to solve the navigation. As a consequence, it simplifies the difficulty of the problem. The first advantage is that the methodology is described at a symbolic level and, thus, there are many ways to implement the method. Second, a geometric implementation of this methodology (ND method) has been demonstrated to address difficult navigation cases, which translate in to achieve safe navigation in dense, complex, and difficult scenarios.

10 Robot Shape, Kinematics, and Dynamics in Obstacle Avoidance

There are three aspects of the vehicle that have to be taken into account during the obstacle avoidance process: shape, kinematics, and dynamics. The shape with the kinematics is a geometric problem that involves the representation of the vehicle configurations in collision, given the admissible trajectories ${\mathcal{Q}}_{t,\mathrm{\infty }}$. The dynamics involves the accelerations and temporal considerations, and is derived in two aspects: (i) choosing a control reachable in a short period of time T given the current velocity ${\dot{\mathit{q}}}_{t_i}$ and the maximum accelerations. (ii) Taking the braking distance into account, so that after a control execution, the vehicle can always stop before collision by applying the maximum deceleration (improving safety).

The problem of shape, kinematics, and dynamics in obstacle avoidance has been taken into account from three different points of view:

• 1. Designing a way to incorporate the constraints within the method (Sect. 47.9.4)

• 2. Developing techniques that abstract the vehicle aspects from the application of the method [47.55, 47.56, 47.57, 47.58]

• 3. By techniques that break down the problem into subproblems and incorporate the aspects in sequence [47.59, 47.60, 47.61] after method usage.

10.1 Techniques that Abstract Vehicle Aspects

These techniques are based on constructing an abstraction layer between the aspects of the vehicle and the obstacle avoidance method, in such a way that when the method is applied its solutions already take into account these aspects [47.55, 47.56, 47.57, 47.58]. We consider here vehicles whose elementary paths obtained under execution of constant controls can be approximated by circular arcs (for example, a differential-drive robot, syncro-drive, or a tri-cycle). To simplify, a control is translational and rotational velocity $\mathit{u}=(v,w)$. The set of reachable controls ${\mathcal{U}}_{\text{A}}$ given the current velocity ${\dot{\mathit{q}}}_{t_i}=(v_0,w_0)$ and the maximum accelerations $(a_v,a_w)$ is obtained by (47.14).

10.1.1 Abstraction Construction

For these vehicles, the configuration space $\mathcal{C}\mathcal{S}$ is three-dimensional. The idea is to construct, centered in the robot at each time t_i, the manifold of the configuration space $ARM\left({\mathit{q}}_{t_i}\right)\equiv ARM$ defined by elementary circular paths. The function that defines the manifold is

$$\theta =f(x,y)=\left\{\begin{array}{ll}\arctan 2\left(x,,,\frac{x^2-y^2}{2y}\right)& \text{if}y\ge 0,\\ -\arctan 2\left(x,,,-,\frac{x^2-y^2}{2y}\right)& \text{otherwise}.\end{array}\right.$$

(47.19)

It is easy to see that function f is differentiable in ${\mathbb{R}}^2\backslash (0,0)$. Thus $(x,y,f(x,y\left)\right)$ defines a two-dimensional manifold in ${\mathbb{R}}^2\times {\mathcal{S}}^1$ when $(x,y)\in {\mathbb{R}}^2\backslash (0,0)$. This manifold ARM contains all the configurations that can be reached at each step of the obstacle avoidance.

Next, in the ARM one computes the exact region of the configurations in collision CO_ARM given any shape of the robot (i. e., obstacle representation in the manifold). Given an obstacle point $(x_p,y_p)$ and a point of the robot bounds $(x_r,y_r)$, a point $(x_s,y_s)$ of the CO_ARM boundary is obtained by

$$x_s=(x_f+x_i)\cdot a,y_s=(y_f-y_i)\cdot a,$$

(47.20)

with

$$a=\left\{\phantom{{\left(x_f^2,-,x_i^2\right)}^2},\left[\left(y_f^2,-,y_i^2\right),+,\left(x_f^2,-,x_i^2\right)\right],\left[(,y_f,-,y_i,{)}^2,+,(,x_f,-,x_i,{)}^2\right]\right\}\times \left[\right(y_f-y_i{)}^4+2\left(x_f^2,+,x_i^2\right)(y_f-y_i{)}^2+{\left(x_f^2,-,x_i^2\right)}^2{]}^{-1}.$$

This result is used to map the robot bounds for all obstacles in the manifold, computing the exact shape of the CO_ARM. Next, one compute the nonadmissible configurations CNA_ARM in the manifold ARM, which correspond to configurations that once reached at a given velocity in time T, the vehicle cannot be stopped by applying the maximum deceleration without collision (i. e., there is not enough braking distance). These regions CNA_ARM are the CO_ARM enlarged by a magnitude that depends on the maximum vehicle accelerations. The set of configurations RC_ARM reachable by controls in a short period of time ${\mathcal{U}}_{\text{A}}$ is also computed in the ARM. Finally, a change of coordinates is applied to ARM leading to ARM^p. Its effect is that the elementary circular paths become straight segments in the manifold. As a consequence, the problem now is to move an omnidirectional point in a bidimensional space free of any constraint.

10.1.2 Method Application

The final step is to apply the avoidance method on the ARM^p to avoid the $CN{A}_{ARM}^p$ regions. The method solution β_sol is the most promising direction in ARM^p. This direction is used to select and admissible location ${\mathit{q}}_{\text{sol}}^p\notin CN{A}_{ARM}^p$ in the set of dynamic reachable configurations $R{C}_{ARM}^p$. Finally, the control u_sol that leads to this location at time T is selected. By construction, this control is kinematically and dynamically admissible, avoids collisions with the exact vehicle shape, and takes into account the braking distance (Fig. 47.15).

Fig. 47.15

(a) Robot location, obstacle information and goal location. (b) The abstraction layer in ARM^p. In this representation the obstacles are the nonadmissible region $CN{A}_{ARM}^p$ in ARM and that the motion is omnidirectional (applicability conditions of many obstacle avoidance methods). The method is applied to obtain the most promising direction β_sol, used to obtain the configuration solution ${\mathit{q}}_{\text{sol}}^p$ in the set of reachable configurations $R{C}_{ARM}^p$. Finally, the solution is the control u_sol that reaches this configuration at time T. This control complies with the kinematics and dynamics and takes into account the exact vehicle shape

Note how with these techniques, the three-tridimensional obstacle avoidance problem is converted into the simple problem of moving a point in a two-dimensional space without kinematic and dynamic restrictions (applicability conditions of many existing methods). Thus, many existing methods can be applied in this abstraction, and as a consequence, the solutions take the vehicle constraints into account.

10.2 Techniques of Decomposition in Subproblems

These techniques address the obstacle avoidance problem taking into account the vehicle constraints by breaking down this problem into subproblems:

• 1. Obstacle avoidance

• 2. Kinematics and dynamics

• 3. The shape.

Each subproblem is dealt with in sequence (Fig. 47.16).

Fig. 47.16

Decomposition of the obstacle avoidance problem into subproblems that are addressed in steps, successively incorporating the vehicle aspects

10.2.1 Obstacle Avoidance

First the obstacle avoidance method is used by assuming a circular and omnidirectional vehicle. The solution is the most promising motion direction and the velocity ${\mathit{u}}_1=(v_1,{\theta }_1)$ to direct the vehicle towards the target.

10.2.2 Kinematics and Dynamics

Second, this control is converted into a control that complies with the kinematics and dynamics, which tends to align the vehicle with the instantaneous motion direction θ₁ of ${\mathit{u}}_1$. For example, [47.60, 47.61] modify the output of the obstacle avoidance method by a feedback action that aligns the vehicle with the direction solution in a least squares fashion. Moreover, [47.59] use a dynamic controller of the vehicle. This controller models the behavior of the vehicle as it would be pulled by a virtual force, and computes the motion that would result after applying this force during a short period of time. The motion generated is the new control. In order to use the controller, the previous control ${\mathit{u}}_1$ is converted into an instantaneous force $\mathit{F}=({\theta }_1,F_{\text{max}}(v_1/v_{\text{max}}\left)\right)$ input of the controller, which computes a control ${\mathit{u}}_2$ that complies with the kinematic and dynamics.

10.2.3 Shape

The final step is to assure that the control ${\mathit{u}}_2$ obtained in the previous step avoids collisions with the exact shape of the vehicle. To do this, a shape corrector is used to check collisions by dynamic simulation of the control. If there is collision, there is a search in the set of reachable controls (47.14) until one of them is collision free. The result of this process is a motion control u_i that guarantees obstacle avoidance and complies with the kinematics and dynamics of the vehicle.

11 Integration Planning – Reaction

In this section we show how the obstacle avoidance methods are integrated in real systems. On the one hand, the obstacle avoidance methods are local techniques to address the motion problem. Thus, they are doomed to fall in local minima that translate in trap situations or cyclic motions. This reveals the necessity of a more global reasoning. On the other hand, motion planning techniques compute a geometric path free of collisions, which guarantees global convergence. However, when the scenarios are unknown and evolve, these techniques fail, since the precomputed paths will almost surely collide with obstacles. It seems clear that one key aspect to build a motion system is to combine the best of both worlds: the global knowledge given by motion planning and the reactivity of the obstacle avoidance methods.

The most extended ways to specify the interaction between the deliberation and reaction are: (i) to precompute a path to the target deformed in execution as a function of the changes in the scenario obtained from the sensor information (systems of path deformation), for example, [47.62, 47.63, 47.64, 47.65, 47.66, 47.67]. (ii) To use a planner at a high frequency with a tactical role, leaving the degree of execution to the reactor [47.68, 47.69, 47.70, 47.71, 47.72, 47.73].

11.1 Systems of Path Deformation

Elastic bands is a method that initially assumes the existence of a geometric path to the target location (computed by a planner). The path is assimilated with a band, subjected to two types of forces: an inner contraction force and an external force. The inner force simulates the tension of a strip and maintains the stress. The external force is exerted by the obstacles and moves the band far from them. During the execution, the new obstacles produce forces that remove the band far from them, guaranteeing their avoidance. These methods are described in Chap. 37.

An extension of the method of path deformation was proposed in [47.64] for nonholonomic systems ( ). Even though the objective is the same as for mobile robots without kinematic constraints, avoiding obstacle while following a trajectory, the concepts are completely different. The trajectory $\mathrm{\Gamma }$ of a nonholonomic system is completely defined by an initial configuration $\mathrm{\Gamma }\left(0\right)$ and the value of the input function $\mathit{u}\in C^1(I,{\mathbb{R}}^m)$, where I is an interval. The trajectory deformation method for nonholonomic systems is thus based on the perturbation of the input function of the current trajectory in order to achieve three objectives:

• 1. Keeping the nonholonomic constraints satisfied

• 2. Getting away from obstacles detected online by onboard sensors

• 3. Keeping unchanged the initial and last configurations of the trajectory after deformation.

Perturbing the input function of $\mathrm{\Gamma }$, by a vector-valued input perturbation $\mathit{v}\in C^1(I,{\mathbb{R}}^m)$, yields a trajectory deformation $\eta \in C^1(I,{\mathbb{R}}^n)$

$$\mathit{u}\leftarrow \mathit{u}+\tau \mathit{v}\Rightarrow \mathrm{\Gamma }\leftarrow \mathrm{\Gamma }+\tau \eta ,$$

where τ is a positive, asymptotically small real number. As an approximation of order 1, the relation between u and η is given by the linearized system, which we do not express here.

11.1.1 Potential Field of Obstacles

Obstacles are taken into account by defining a potential field over the configuration space, which increases when the robot gets closer to obstacles. This potential field is lifted into the space of trajectories by integration along the trajectory of the configuration potential value.

11.1.2 Discretization of the Input Space

The space $C^1(I,{\mathbb{R}}^m)$ of input perturbations is an infinite-dimensional vector space. The choice of an input perturbation is restricted to a finite-dimensional subspace spanned by p arbitrary test functions, ${\mathit{e}}_1,\mathrm{\dots },{\mathit{e}}_p$ where p is a positive integer. An input perturbation: $\mathit{u}={\sum }_{i=1}^p{\lambda }_i{\mathit{e}}_i$ is thus defined by a vector $\lambda \in {\mathbb{R}}^p$. The variation of the trajectory potential is linear with respect to λ.

11.1.3 Boundary Conditions

The boundary conditions consisting in applying a trajectory perturbation equal to zero at both ends of interval I is linear with respect to λ. It is, therefore, easy to find a vector λ making the potential decrease and satisfying the boundary conditions.

11.1.4 Nonholonomic Constraint Deviation

The approximation of order 1 in the relation between the input perturbation and trajectory deformation induces a side effect: after a few iterations, the nonholonomic constraints are no longer satisfied. This side effect is corrected by considering an augmented system with n control vector fields $f_1,\mathrm{\dots },f_n$ spanning ${\mathbb{R}}^n$ for each configuration and by keeping the input components $u_{m+1}$ to u_n along the additional vector fields as close as possible to 0. Figure 47.17 shows an example of the trajectory deformation algorithm applied to a nonholonomic system.

Fig. 47.17

A differentially-driven mobile robot towing a trailer applying the trajectory deformation algorithm to avoid obstacles detected online

11.2 Systems of Tactical Planning

The systems of tactical planning recompute at a high frequency a path to the target location, and use the main course to advise the obstacle avoidance module. The design of these motion systems involves at least the synthesis of three functionalities. The construction of a model, the deliberative planning, and the obstacle avoidance. The modeler constructs a representation base for deliberation and memory for the reactive behavior. The planner generates global plans used tactically to guide the obstacle avoidance module, which generates the local motion. Next we give a perspective of the three functionalities and three possible tools to implement them [47.73].

11.2.1 Model Builder Module

Construction of a model of the environment (to increase the spatial domain of the planning and used as local memory for obstacle avoidance). One possibility is to use a binary occupancy grid that is updated whenever a new sensory measurement is available, and to employ a scan matching technique  [47.74, 47.75] to improve the vehicle odometry before integrating any new measure in the grid.

11.2.2 Planner Module

Extraction of the connectivity of the free space (used to avoid the cyclical motions and trap situations). A good choice is a dynamic navigation function such as the D${}^{*}$ [47.76, 47.77]. The idea behind the planner is to focus the search locally in the areas where the changes in the scenario structure have occurred and affect the computation of the path. The planner avoids the local minima and is computationally very efficient for real-time implementations.

Fig. 47.19

(a) Snapshot of an experiment carried out with the motion system described working on a wheelchair robot equipped with a planar range laser sensor. (b) Information of the motion system at a given time: current accumulated map of the scenario, path computed with the planner and tactical direction of motion, and solution of the reactive obstacle avoidance method

11.2.3 Obstacle Avoidance Module

Computation of the collision-free motion. Any of the methods described in this chapter could be used. One possibility is the ND method (Sect. 47.9.6), since it has been demonstrated to be very efficient and robust in environments with little space to manoeuvre.

Globally the system works as follows (Fig. 47.18): given a laser scan and the odometry of the vehicle, the model builder incorporates this information into the existing model. Next, the information of the changes in obstacle and free space in the model is used by the planner module to compute the course to follow to reach the goal. Finally, the avoidance module uses the information of the obstacles contained in the grid and information of this tactical planner to generate the motion (to drive the vehicle free of collisions towards the goal). The motion is executed by the vehicle controller and the process restarts with a new sensorial measurement. It is important to stress that the three modules should work synchronously within the perception – action cycle for consistency reasons. The advantage of these systems is that the synergy of the modules allows to avoid trap situations and the cyclic motions (limitations associated to the local nature of the avoidance methods). Figure 47.19 shows a snapshot of an experiment carried out with this motion system.

Fig. 47.18

Overview of the system of motion

12 Conclusions, Future Directions, and Further Reading

The algorithmic tools presented in this chapter show that motion planning and obstacle avoidance research techniques have reached a level of maturity that allow their transfer onto real platforms ( , ). Today, several indoor mobile robots use obstacle avoidance techniques on a daily basis to guide visitors in museums. Outdoor applications still require some developments in perception and modeling. For these applications, 3-D sensing capabilities are necessary to model the environment but also to detect obstacles. As an example, several car manufacturer are working on parallel parking assistance. The difficult point of this application is to build a model of a parking spot from 3-D data in a great variety of environments.

The main challenge of autonomous motion in mobile robots currently consists in integrating techniques from different robotics research domains in order to plan and execute motions for very complex systems, like humanoid robots, for instance. Integration in the sense that different software components need to work together on a machine but also in a scientific meaning: the classical motion planning formulation with configuration variables that locate the robot with respect to a global reference frame does not fit partially known environments with imprecise maps. Robotic tasks like motion need to be specified with respect to landmarks of the environment. For instance, grasping an object is by definition a motion specified with respect to the position of the object. Very little has been done to design a general framework in this direction.

For useful complementary reading on motion planning and obstacle avoidance for mobile robots see [47.5, 47.78, 47.79, 47.80].

Video-References

:

Sensor-based trajectory deformation and docking for nonholonomic mobile robots available from http://handbookofrobotics.org/view-chapter/47/videodetails/80

:

Autonomous robotic smart wheelchair navigation in an urban environment available from http://handbookofrobotics.org/view-chapter/47/videodetails/707

:

Sena wheelchair: Autonomous navigation at University of Malaga (2007) available from http://handbookofrobotics.org/view-chapter/47/videodetails/708

:

Robotic wheelchair: Autonomous navigation with Google Glass available from http://handbookofrobotics.org/view-chapter/47/videodetails/709

:

A ride in the google self driving car available from http://handbookofrobotics.org/view-chapter/47/videodetails/710

:

Mobile robot navigation system in outdoor pedestrian environment available from http://handbookofrobotics.org/view-chapter/47/videodetails/711

:

Mobile robot autonomous navigation in Gracia district, Barcelona available from http://handbookofrobotics.org/view-chapter/47/videodetails/712

:

Autonomous navigation of a mobile vehicle available from http://handbookofrobotics.org/view-chapter/47/videodetails/713

:

Autonomous robot cars drive DARPA Urban challenge available from http://handbookofrobotics.org/view-chapter/47/videodetails/714