Preface

One of the topics of interest and technical development is the automatic control system re‐ search. From the beginning, it has been a growing booster of different knowledge areas. Re‐ member, for example, first developments such as the water clocks and the watt regulator that helped at that time with the development of the industrial revolution.

Along the history, we can find different mathematical tools that have allowed the develop‐ ment of single-input single-output (SISO) control systems, which demanded a certain de‐ gree of knowledge in the feedback systems in contrast to the multivariable systems, modeling and physical representation, stability concepts, observability and regulator design, etc., of today.

In case of robotic development, we consider multivariable control systems, where each of the variables must follow the instructions of the kinematic control with the aim of trajectory following in which each of the joint movements must be required to comply the specific tasks.

This book contains a selection of research works focused on robot control applications where the reader can appreciate the development and progress of these systems. There are descriptions of projects in each section in areas such as mobile robotics, navigation systems, trajectory-planning navigation systems, and non-holonomic systems.

On the other hand, one of the areas of knowledge that has been developed and that eventu‐ ally helped the development of robotic control is without doubt artificial intelligence. In this book, we will find control schemes for robotics using artificial intelligence concepts in order to face highly non-linear systems.

The editor specially appreciates and thanks authors of the chapters who shared their knowl‐ edge with the scientific community and other potential readers.

> **Dr. Eng. Efrén Gorrostieta** Engineering Faculty of the Autonomous University of Querétaro Cerros de las Campanas Queretaro Qro Mexico

#### **A Review of Compliant Movement Primitives A Review of Compliant Movement Primitives**

Miha Deniša, Tadej Petrič, Andrej Gams and Aleš Ude Miha Deniša, Tadej Petrič, Andrej Gams and Aleš Ude

Additional information is available at the end of the chapter Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/64058

#### **Abstract**

Dynamical models of robots performing tasks in contact with objects or the environment are difficult to obtain. Therefore, different methods of learning the dynamics of tasks have been proposed. In this chapter, we present a method that provides the joint torques needed to execute a task in a compliant and at the same time accurate manner. The presented method of compliant movement primitives (CMPs), which consists of the task kinematical and dynamical trajectories, goes beyond mere reproduction of previously learned motions. Using statistical generalization, the method allows to generate new, previously untrained trajectories. Furthermore, the use of transition graphs allows us to combine parts of previously learned motions and thus generate new ones. In the chapter, we provide a brief overview of this research topic in the literature, followed by an in-depth explanation of the compliant movement primitives framework, with details on both statistical generalization and transition graphs. An extensive experimental evaluation demonstrates the applicability and the usefulness of the approach.

**Keywords:** compliant movements, adaptive system, learning system, robot control, learning by demonstration

#### **1. Introduction**

The need to operate in unstructured environments, such as human everyday environments and homes, drives the development of algorithms for fast generation of new trajectories of robots in compliant behavior mode without sacrificing tracking accuracy. Operating in unstructured environments and with humans requires compliant robot behavior because of possible unplanned contact with objects, and more importantly, with humans themselves. To achieve such behavior, accurate dynamical models of the robot and the task are needed [1].

© 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. © 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Yet, acquisition of accurate dynamical models of robots and more specifically of robots performing a task, are difficult to obtain. Therefore, different methods, including biologically inspired methods, were proposed for robot control [2]. Other approaches have been proposed for torque learning. For example, Nguyen-Tuong and Peters [3, 4] relied on local Gaussian process regression (GPR) and used it for on-line dynamic model learning. Their approaches improve the accuracy of the model while avoiding high feedback gains. On the other hand, their method requires the availability of a large quantity of data in order to fully learn a complete dynamic model, and not only task-specific torques. Learning of torques for a specific task can be utilized using iterative learning control (ILC) [5] as was shown in the paper of Schwarz and Behnke [6], who used ILC to learn motor and friction models. Similarly, Gautier et al. [7] proposed an iterative learning identification and control method for dynamic robot control.

The latest generations of robotic mechanisms, such as the Kuka LWR-4, are equipped with joint-torque sensors [8], which can be used to measure the torques during the operation. The possibility of recording joint torques has been exploited in several approaches for learning of task-specific joint torques. One such method is with the use of compliant movement primitives (CMPs), first introduced by Petrič et al. [9] and later adopted by [10, 11], which encode both the kinematic trajectory as well as the corresponding joint torques.

The main topic of this chapter is a review of compliant movement primitives (CMPs), which are suitable for robots with active torque control. CMPs enable accurate execution while maintaining a compliant mode of operation, without requiring explicit models of task dynamics. CMPs draw their inspiration from the human ability to learn arbitrary dynamical tasks [12]. They can be easily learned from user demonstrations. In this paper, we present the basics of the CMPs, followed by the means to surpass the limited applicability of pure imitation through generation of new, previously untrained movements. In order to do so, two mathematical means were exploited: statistical generalization, which allows for variation in task configuration, and hierarchical database search, which allows combinations of previously trained tasks.

Both statistical generalization and hierarchical database search have been previously employed in robotics on a kinematic level. For example, generation of kinematic trajectories with statistical generalization using locally weighted regression was shown in [13]. The method was applied to dynamic movement primitives (DMPs) [14], which also constitute the kinematic part of the CMPs. Similarly, Forte et al. used Gaussian process regression to generalize between the weights of the DMPs [15]. Other approaches of generalization, not relying on DMPs, are thoroughly discussed in [16]. Hierarchical database presented in this chapter consist of transition graphs, that is, motion graphs on each level. It was shown that smooth transitions between movements can be found if they are organized in motion graphs [17]. While Koval et al. [18] used motion graphs to generate different styles of locomotion, Yamane et al. [19, 20] combined them with a binary tree database. Similar work on hierarchical databases was also done by Deniša et al. [11, 21].

Compliant movement primitives are composed of both kinematic and dynamic trajectories. While the first, kinematic part, is encoded as the aforementioned DMPs, the latter, dynamic part, is encoded as a set of radial basis functions. Only the combination of both allows accurate and compliant execution of trajectories. Similar approaches have recently appeared in the literature. An approach that utilizes tactile sensors to determine the force of contact with the environment on the iCub robot, and calculate the joint torques from the measured arm pose was proposed by Calandra et al. [22]. The authors propose using calculated joint-torques in a feed-forward manner for the control, just as is the case in CMPs. Similarly, Steinmetz et al. [23] recorded joint torques along the kinematic trajectory, encoded as a DMP, and used these torques as a feed-forward signal for the controller to increase the accuracy in the next execution of the in-contact task.

The need for robot operations in unstructured environments, combined with the complexity of the acquisition of dynamical models of various tasks, and the occurrence of similar methods in the literature all point at the applicability of the proposed compliant movement primitives approach for robots that go beyond the factory floor.

This chapter is structured as follows. We first provide the details the CMPs, explaining both the kinematic and the dynamic aspects. Generation of new trajectories with statistical generalization and hierarchical database search is explained in Section 3. Experimental evaluation is given in Section 4, followed by a Discussion and a Conclusion.

#### **2. Compliant movement primitives**

Yet, acquisition of accurate dynamical models of robots and more specifically of robots performing a task, are difficult to obtain. Therefore, different methods, including biologically inspired methods, were proposed for robot control [2]. Other approaches have been proposed for torque learning. For example, Nguyen-Tuong and Peters [3, 4] relied on local Gaussian process regression (GPR) and used it for on-line dynamic model learning. Their approaches improve the accuracy of the model while avoiding high feedback gains. On the other hand, their method requires the availability of a large quantity of data in order to fully learn a complete dynamic model, and not only task-specific torques. Learning of torques for a specific task can be utilized using iterative learning control (ILC) [5] as was shown in the paper of Schwarz and Behnke [6], who used ILC to learn motor and friction models. Similarly, Gautier et al. [7] proposed an iterative learning identification and control method for dynamic robot

The latest generations of robotic mechanisms, such as the Kuka LWR-4, are equipped with joint-torque sensors [8], which can be used to measure the torques during the operation. The possibility of recording joint torques has been exploited in several approaches for learning of task-specific joint torques. One such method is with the use of compliant movement primitives (CMPs), first introduced by Petrič et al. [9] and later adopted by [10, 11], which encode both

The main topic of this chapter is a review of compliant movement primitives (CMPs), which are suitable for robots with active torque control. CMPs enable accurate execution while maintaining a compliant mode of operation, without requiring explicit models of task dynamics. CMPs draw their inspiration from the human ability to learn arbitrary dynamical tasks [12]. They can be easily learned from user demonstrations. In this paper, we present the basics of the CMPs, followed by the means to surpass the limited applicability of pure imitation through generation of new, previously untrained movements. In order to do so, two mathematical means were exploited: statistical generalization, which allows for variation in task configuration, and hierarchical database search, which allows combinations of previously

Both statistical generalization and hierarchical database search have been previously employed in robotics on a kinematic level. For example, generation of kinematic trajectories with statistical generalization using locally weighted regression was shown in [13]. The method was applied to dynamic movement primitives (DMPs) [14], which also constitute the kinematic part of the CMPs. Similarly, Forte et al. used Gaussian process regression to generalize between the weights of the DMPs [15]. Other approaches of generalization, not relying on DMPs, are thoroughly discussed in [16]. Hierarchical database presented in this chapter consist of transition graphs, that is, motion graphs on each level. It was shown that smooth transitions between movements can be found if they are organized in motion graphs [17]. While Koval et al. [18] used motion graphs to generate different styles of locomotion, Yamane et al. [19, 20] combined them with a binary tree database. Similar work on hierarchical databases was also

Compliant movement primitives are composed of both kinematic and dynamic trajectories. While the first, kinematic part, is encoded as the aforementioned DMPs, the latter, dynamic

the kinematic trajectory as well as the corresponding joint torques.

control.

2 Robot Control

trained tasks.

done by Deniša et al. [11, 21].

Compliant movement primitives CMPs are defined as a combination of Dynamic movement primitives (DMPs) for encoding kinematic trajectories and corresponding task-specific dynamics encoded in Torque Primitives (TPs). They are defined as

$$\mathbf{h}(t) = \left[ \ddot{\mathbf{p}}\_d(t), \dot{\mathbf{p}}\_d(t), \, \mathbf{p}\_d(t), \mathbf{f}\_f(t) \right],\tag{1}$$

where ¨ , ˙ , , are the desired acceleration, velocity, and position encoded in the DMPs and are the corresponding task-specific dynamics, that is, joint torques or forces, encoded in TPs.

The learning of CMPs is done in three different stages. The first stage is learning the kinematic trajectory for the DMPs. The second stage is learning of corresponding torque or force profiles for TPs, and the last, third, stage is the execution of CMPs. While the learning of kinematic trajectories in the form of DMPs is well documented [13–15, 24], the literature for learning TPs is not as vast [10, 25].

#### **2.1. Control structure**

The basic control structure for robot control using CMPs is shown in **Figure 1**. The main advantage of the proposed control architecture is the model free approach which in the execution step enables natural compliant behavior while ensuring sufficient accuracy of the desired movement. Natural compliance is the compliance of the mechanism itself. Because the robot is compliant while executing the task, the forces in case of an unforeseen collision are small. Thus, the robot can perform tasks in unstructured environment and safely interact with humans.

**Figure 1.** Block diagram of the multi-layered and multi-stage control system. The colored lines (green, orange, and blue) are only active when a chosen state is active (see text for detailed description).

**Figure 1** shows the structure of the proposed multi-step control algorithm. The colors indicate which block is active in each step: green—learning of the DMPs, orange—learning of the TPs, and blue—execution of the CMPs. Note that black connections are always active, regardless of the step.

Assuming the robot consists of rigid bodies, the joint space equations of motion can be written in a form

$$H(q)\ddot{q} + C(q,\dot{q}) + \mathbf{g}(q) + \dot{\boldsymbol{\phi}}(\ddot{q},\dot{q},q) = \mathbf{r}\_{\bullet} \tag{2}$$

where ¨ , ˙ , are the joint acceleration, velocity, and position, respectively; *H*(**q**) is the inertia matrix, , ˙ are the Coriolis and centripetal forces, are the gravity forces, and ¨ , ˙ , are additional nonlinearities, for example, friction. We denote the full robot dynamic model as ¨ , ˙ , . Note that this inverse dynamic model ¨ , ˙ , does not include dynamics of the task.

In general, the common approach for tracking the desired motion is using the impedance control given with

$$\mathbf{r}\_u = \mathbf{K}\left(\mathbf{q}\_d - \mathbf{q}\right) + \mathbf{D}\left(\dot{\mathbf{q}}\_d - \dot{\mathbf{q}}\right) + f\_{robu}(\ddot{\mathbf{q}}, \dot{\mathbf{q}}, \mathbf{q}) + \mathbf{r}\_f,\tag{3}$$

where **K** and **D** are the diagonal matrices of the desired stiffness and damping respectively [1], *<sup>q</sup>d* is the vector of desired motion encoded in DMPs, and is the vector of task-specific torques encoded in TPs. Here, if the values of **K** are high, the robot is accurately tracking the trajectory with high error rejection ratio, that is, with stiff behavior. Vice versa, if the values of **K** are low, the robot cannot accurately track the desired trajectory unless additional task-specific torques are provided.

In the following, we explain the three steps of the CMPs learning approach: (1) learning of DMPs, (2) learning of TPs, C) execution of CMPs with accurate trajectory tracking and compliant behavior.

#### **2.2. Learning of DMPs**

execution step enables natural compliant behavior while ensuring sufficient accuracy of the desired movement. Natural compliance is the compliance of the mechanism itself. Because the robot is compliant while executing the task, the forces in case of an unforeseen collision are small. Thus, the robot can perform tasks in unstructured environment and safely interact with

**Figure 1.** Block diagram of the multi-layered and multi-stage control system. The colored lines (green, orange, and

**Figure 1** shows the structure of the proposed multi-step control algorithm. The colors indicate which block is active in each step: green—learning of the DMPs, orange—learning of the TPs, and blue—execution of the CMPs. Note that black connections are always active, regardless

Assuming the robot consists of rigid bodies, the joint space equations of motion can be written

where ¨ , ˙ , are the joint acceleration, velocity, and position, respectively; *H*(**q**) is the inertia matrix, , ˙ are the Coriolis and centripetal forces, are the gravity forces, and ¨ , ˙ , are additional nonlinearities, for example, friction. We denote the full robot dynamic model as

In general, the common approach for tracking the desired motion is using the impedance

where **K** and **D** are the diagonal matrices of the desired stiffness and damping respectively [1], *<sup>q</sup>d* is the vector of desired motion encoded in DMPs, and is the vector of task-specific torques encoded in TPs. Here, if the values of **K** are high, the robot is accurately tracking the trajectory with high error rejection ratio, that is, with stiff behavior. Vice versa, if the values of **K** are low,

t ,

= -+ -+ + ( )( )& && & & ) ,( *<sup>u</sup> <sup>d</sup> <sup>d</sup> robot <sup>f</sup> τ K q q Dq q f q,q,q τ* (3)

(2)

¨ , ˙ , does not include dynamics

*H q q+C q,q g q q q qò* () ( ) () (,, ) && & && & ++ =

blue) are only active when a chosen state is active (see text for detailed description).

¨ , ˙ , . Note that this inverse dynamic model

humans.

4 Robot Control

of the step.

in a form

of the task.

control given with

The aim of the first step was to learn the task-specific trajectories of motion, encoded in DMPs. There are several possibilities on how to gain and encoded the motion in DMPs for both periodic and point-to-point motions [13–14, 24]. A short recap fallows on how to encode motions based on human demonstrations for point-to-point DMPs. The equations below are valid for one DOF and can be used in parallel for multiple DOFs. A DMPs is defined as a nonlinear system of differential equations

$$
\nu \dot{z} = a\_{\varepsilon} \left( \beta\_{\varepsilon} (\mathbf{g} - \mathbf{y}) - z \right) + f \left( \mathbf{s} \right), \tag{4}
$$

$$\mathbf{v}\dot{\mathbf{y}} = \mathbf{z}.\tag{5}$$

Here, the linear part ensures that y converges to the desired goal configuration once *f*(*s*) becomes zero. *f*(*s*) is a nonlinear part that defines the shape of the movement. It is given by

$$f\left(\mathbf{s}\right) = \frac{\sum\_{b=1}^{L\_{\mathcal{I}}} \mathcal{W}\_{\theta^{b}} \mathcal{W}\_{b}(\mathbf{s})}{\sum\_{b=1}^{L\_{\mathcal{I}}} \mathcal{W}\_{b}(\mathbf{s})}.\tag{6}$$

Here, the Gaussian basis functions are defined as

$$\psi\_{\boldsymbol{\nu}}\left(\mathbf{s}\right) = \exp\left(-\boldsymbol{d}\_{\boldsymbol{\nu}}\left(\mathbf{s} - \mathbf{c}\_{\boldsymbol{\nu}}\right)^{2}\right),\tag{7}$$

where *cb* are centers and *db* are widths of the Gaussians. Since *f*(*s*) is not directly time dependent, the phase variable *s* was introduced as

$$\mathbf{v}\dot{\mathbf{s}} = -\alpha\_s \mathbf{s}.\tag{8}$$

The phase variable is common across all DMPs and TPs, for example, it is common for all CMPs. With proper selection of parameters *αz*, *βz*, *αs* and *v*, the convergence of the system is guaranteed. For evaluation, the parameters were set empirically to *αy* = 48, *βz* = *αz*/4 and *αs* = 2. To acquire weights *wqb* that represent the desired motion, the target for learning is derived from Eqs. (4) and (5). It is given by

$$f\_{\rm target} = \mathbf{v}^2 \ddot{q}\_{\rm an}(t\_i) + \alpha\_z \dot{q}\_{\rm an}(t\_i) - \alpha\_z \beta\_z \left(\mathbf{g} - q\_{\rm an}(t\_i)\right), i = 1, \ldots, T,\tag{9}$$

where the goal *g* is defined by the end value of the example trajectory *qxn*(*tT*). To calculate weights *wqb*, the overdetermined system (Eq. (9)) is solved using regression technic for each joint. Note that recursive regression [26] is also possible, which is a common choice for online imitation learning.

#### **2.3. Learning of TPs**

Once DMPs are learned, learning of TPs follows. This step is denoted with orange color in the block scheme given in **Figure 1**. Here, we give a short recap of the method that exploits stiff and accurate robot behavior in a supervised environment.

The TPs are given as a combination of Gaussian functions denoted by

$$\pi\_{\boldsymbol{\gamma}} = \frac{\sum\_{b=1}^{L\_{\boldsymbol{\delta}}} \boldsymbol{\mu}\_{ib} \boldsymbol{\mu}\_{b}(\mathbf{s})}{\sum\_{b=1}^{L\_{\boldsymbol{\delta}}} \boldsymbol{\mu}\_{b}(\mathbf{s})} \,. \tag{10}$$

To gain corresponding torques for accurate tracking of the trajectory, the gain *K* is set to "high." Note that this usually implies stiff robot behavior; therefore, the action must be performed under human supervision. To acquire weights *wtb* that represent the corresponding torques, the target for learning is given in a form of

$$f\_{TP-target} = K\left(q\_d - q\right) + D\left(\dot{q}\_d - \dot{q}\right). \tag{11}$$

where *y* is the desired motion trajectory encoded in DMPs (Eqs. (4) and (5)). Using the same approach as in DMPs, the weights *wtb* from Eq. (10) are calculated using a recursive regression [26] for each joint. The details on learning TPs are in [10].

#### **2.4. Execution of CMPs**

Once both, the DMPs and TPs are learned, the CMPs can be executed using the control law given by Eq. (3). This step is denoted with blue color in the block scheme given in **Figure 1**. Because the task-specific dynamics is provided in a feed-forward manner, the tracking accuracy remains high even if the feedback gains are much lower that during learning. By selecting lower feedback gains, it is assumed that the robot behavior is compliant. However, since feedback gains are low and the robot behavior is complaint, the error rejection ratio is small, from which follows that the tracking is only accurate as long as the system is not heavily perturbed.

Note that the main idea of the CMPs is that the feed-forward part assures the nominal behavior of the robot for a specific given task even in cases when low feedback gains are used. In this way, good tracking and compliant behavior are achieved at the same time.

#### **3. Autonomous generation of CMPs**

To acquire weights *wqb* that represent the desired motion, the target for learning is derived from

where the goal *g* is defined by the end value of the example trajectory *qxn*(*tT*). To calculate weights *wqb*, the overdetermined system (Eq. (9)) is solved using regression technic for each joint. Note that recursive regression [26] is also possible, which is a common choice for online

Once DMPs are learned, learning of TPs follows. This step is denoted with orange color in the block scheme given in **Figure 1**. Here, we give a short recap of the method that exploits stiff

> 1 1 ( ) . ( ) = = S= S *d d L b tb b*

*b b w s s* y

y

To gain corresponding torques for accurate tracking of the trajectory, the gain *K* is set to "high." Note that this usually implies stiff robot behavior; therefore, the action must be performed under human supervision. To acquire weights *wtb* that represent the corresponding torques,

where *y* is the desired motion trajectory encoded in DMPs (Eqs. (4) and (5)). Using the same approach as in DMPs, the weights *wtb* from Eq. (10) are calculated using a recursive regression

Once both, the DMPs and TPs are learned, the CMPs can be executed using the control law given by Eq. (3). This step is denoted with blue color in the block scheme given in **Figure 1**. Because the task-specific dynamics is provided in a feed-forward manner, the tracking accuracy remains high even if the feedback gains are much lower that during learning. By selecting lower feedback gains, it is assumed that the robot behavior is compliant. However, since feedback gains are low and the robot behavior is complaint, the error rejection ratio is small, from which follows that the tracking is only accurate as long as the system is not heavily

*f L*

(9)

(10)

( )( ). - = -+ - & & *TP target <sup>d</sup> <sup>d</sup> f Kq q Dq q* (11)

() () ( ) ( ) <sup>2</sup> = + - - =¼ && & , 1, , , *target xn i z xn i z z xn i f vq t q t g q t i T*

 ab

a

and accurate robot behavior in a supervised environment.

[26] for each joint. The details on learning TPs are in [10].

the target for learning is given in a form of

**2.4. Execution of CMPs**

perturbed.

The TPs are given as a combination of Gaussian functions denoted by

t

Eqs. (4) and (5). It is given by

imitation learning.

6 Robot Control

**2.3. Learning of TPs**

Learning CMPs by human demonstration greatly simplifies execution of tasks in a compliant manner, as it does not need mathematically defined dynamic models in advance. But doing this for each variation of a dynamically versatile task is unpractical and time-consuming. In the following, two example expansions of the initial CMP database are presented to overcome this issue. While the first subsection tackles the issue of generating completely CMPs using a search in hierarchical databases with transition graphs, the second delivers the methodology for using statistical techniques in order to generalize CMPs.

#### **3.1. Hierarchical database search**

This section presents combining available trajectories to generate new trajectories with the corresponding feed-forward torque control signals using hierarchical graph search. Generating new robot movements through hierarchical graph search as such is not new, see for example, [20, 21]. To apply the hierarchical graph search on CMPs, the database of CMPs needs to be divided into two parts: the primary part that includes the kinematic trajectories and the secondary one that includes the dynamic part of CMPs, that is, corresponding torques. As new kinematic trajectories are synthesized through hierarchical search in the initial database, corresponding torques are extracted from the secondary part of the database.

#### *3.1.1. Building the database*

The primary part of the database, which stores kinematic part of CMPs, that is, position trajectories *qd*, is a binary tree-like structure with transition graphs at each level. As the primary database is built, the secondary part is added. It encodes the dynamic part of CMPs, that is, corresponding torques *τ<sup>f</sup>* . Database construction begins with concatenating initial example position trajectories into a sample position matrix

$$X = \begin{bmatrix} \mathbf{x}\_1, \mathbf{x}\_2, \dots, \mathbf{x}\_F \end{bmatrix},\tag{12}$$

where *x<sup>i</sup>* denotes a state vector sampled at a given discrete time interval, and *F* the total number of all samples belonging to all example trajectories included in the database. A state vector, defined as

$$\mathbf{x}\_{i} = \left[ q\_{\times 1}, \dot{q}\_{\times 1}, q\_{\times 2}, \dot{q}\_{\times 2}, \dots, q\_{\times p}, \dot{q}\_{\times p} \right],\tag{13}$$

where subscript *x* denotes examples, and *P* number of DOF represents positions and velocities of an example trajectory at a given discrete time interval.

The sample motion matrix, encapsulating all example potion trajectories, represents a root node of a binary tree. See **Figure 2** for a simple representation of the database. A *k*-means algorithm, with *k* = 2, is used to cluster similar state vectors and split the initial root node into two child nodes. Clustering is then used again on each of the two child nodes, and thus, the nodes at the next level are gained. This can also be seen in **Figure 2** depicting a simple example of a database. The nodes keep splitting until the criterion, based on the variability of the data in node, is met. The mean distance *dk* of a node *k*, used as a "stop split" criterion, is defined as

$$d\_k = \frac{\Sigma\_{i=1}^{n\_k} d\left(\mathbf{x}\_k, \mathbf{c}\_k\right)}{n\_k},\tag{14}$$

where *nk* denotes the number of state vectors clustered in node *k*. Euclidian distance *d*(*xki*,*ck*) is calculated between each state vector in the node and the node's centroid *ck*, gained by the *k*means algorithm. As this criterion indicates the similarity of the clustered state vectors, the database does not get unnecessary deep, while the precision of the representation remains satisfactory. The clustering is continued until all nodes meet the "stop split" criterion. Every branch is extended as a leaf node until the last layer. In this way, all the state vectors are represented at all the database levels.

**Figure 2.** A simple representation of used database. The primary part encoding kinematic trajectories and with transition graphs is shown on the left. The secondary part which encapsulates corresponding torques and has multiple layers per level is depicted on the right side.

Every level of the database includes a transition graph representing all possible transitions between the nodes (see **Figure 2**). The graph's edge weights define the probability of transition from one node to the other. The transition probability from node *k* to node *l* is estimated by

$$P\_{kl} = \frac{m\_{kl}}{n\_k} \tag{15}$$

where *mkl* denotes the number of all state vectors clustered in node *k* that have a successor in node *l*, that is, the number of transitions observed in all trajectories of the original data. As state vectors are not needed any more, they are omitted, and at each node, a mean of corresponding state vectors is saved instead. At this point, the time component is also lost, which is tackled later on in this section.

While the already constructed primary part of database encodes CMP's kinematic trajectories, the secondary part will encode the dynamic part. The torques signals are not separately clustered, but rather associated with corresponding nodes in the primary database. For each part of the kinematic trajectories, represented in a single node through the mean of state vectors, a corresponding mean of the torques signal and its time durations is stored. As the same kinematic movement can have different dynamic parts, this is done multiple times for each dynamic task descriptor. See **Figure 2** for example representation of the whole database.

#### *3.1.2. Synthesizing new CMPs*

where subscript *x* denotes examples, and *P* number of DOF represents positions and velocities

The sample motion matrix, encapsulating all example potion trajectories, represents a root node of a binary tree. See **Figure 2** for a simple representation of the database. A *k*-means algorithm, with *k* = 2, is used to cluster similar state vectors and split the initial root node into two child nodes. Clustering is then used again on each of the two child nodes, and thus, the nodes at the next level are gained. This can also be seen in **Figure 2** depicting a simple example of a database. The nodes keep splitting until the criterion, based on the variability of the data in node, is met. The mean distance *dk* of a node *k*, used as a "stop split" criterion, is defined as

> <sup>1</sup> ( ) , , <sup>S</sup> <sup>=</sup> <sup>=</sup> *k n i ki k*

*d <sup>d</sup> <sup>n</sup>*

*k*

*x c*

where *nk* denotes the number of state vectors clustered in node *k*. Euclidian distance *d*(*xki*,*ck*) is calculated between each state vector in the node and the node's centroid *ck*, gained by the *k*means algorithm. As this criterion indicates the similarity of the clustered state vectors, the database does not get unnecessary deep, while the precision of the representation remains satisfactory. The clustering is continued until all nodes meet the "stop split" criterion. Every branch is extended as a leaf node until the last layer. In this way, all the state vectors are

**Figure 2.** A simple representation of used database. The primary part encoding kinematic trajectories and with transition graphs is shown on the left. The secondary part which encapsulates corresponding torques and has multiple lay-

(14)

*k*

of an example trajectory at a given discrete time interval.

8 Robot Control

represented at all the database levels.

ers per level is depicted on the right side.

A new trajectory is defined by first selecting the desired start and end points on two different trajectories. A dynamic task descriptor, for example, the desired task time multiplier *k*, is also selected. As the level determines, the fidelity of reproduction compared to the original trajectories, the last level of the hierarchical database is usually selected. A path between the nodes corresponding to the desired start and end joint position needs to be found. To achieve that A\* search algorithm, it is used on the transition graph at the desired level. As long as the two trajectories share a similar enough part, the most probable path is found and with it a sequence of nodes, that is, mean state vectors.

Based on the selected dynamic task descriptor, for example, *k*, we add the corresponding mean torques to the state vectors of the discovered sequence. We enhance this sequence further with time durations *td* corresponding to the added torques. A sequence of positions, torques, and their time durations is gained. The newly discovered sequence can be written as

$$\left\{ (\overline{\mathbf{x}}\_{1}, \mathbf{r}\_{f1}, \mathbf{0}), \left( \overline{\mathbf{x}}\_{2}, \mathbf{r}\_{f2}, \frac{t\_{d1} + t\_{d2}}{2} \right), \dots, \left( \overline{\mathbf{x}}\_{r}, \mathbf{r}\_{\rho}, \frac{t\_{d(r-1)} + t\_{d\nu}}{2} \right) \right\},\tag{16}$$

where *r* denotes the number of nodes on the trajectory.

A trajectory from each discovered sequence is generated by encoding it as a DMP. The DMPs describing newly synthesized kinematic trajectories and the corresponding torque control signals (encoded as TPs) can then be used to execute new, not directly shown, movements while remaining compliant.

#### **3.2. Statistical generalization**

Using programming by demonstration approaches to learn CMPs can simplify compliant execution of dynamically versatile tasks. But executing demonstrations for each possible variation of the task would be time-consuming and cumbersome. For each new task descriptor, a new CMP needs to be learned, that is, motion trajectory needs to be learned through human demonstrations and executed on a robot for torque learning. Even if the deviation happens just on the torque level, for example, because of different payload, supervised learning of the torque is needed. Besides using actions graphs to generate new CMPs, as seen in the previous section, statistical generalization techniques can be employed. For that, a set of learned CMPs which transition smoothly between each other as a function of task descriptors, that is, query points, is needed. Using generalization, a task can be executed at an arbitrary query point *c* within the learned query space.

For statistical generalization, we use Gaussian process regression (GPR), which can be used to learn a function

$$F: \mathcal{C} \mapsto [\mathbf{w}\_q, \mathbf{g}\_q, \mathbf{w}\_\iota, \mathbf{v}] \tag{17}$$

A CMP, defined by *wq*, *gq*, *wτ*, and *v*, can be used to execute a task, defined by a query *c*, in a compliant manner. By the above definition, *F* computes appropriate CMP parameters at the given query *c*, that is, at a given task variation.

Once the Gaussian process is *trained* using example training sets of CMPs, new appropriate CMPs for given queries *c* can be calculated by simple matrix multiplications, which can easily be accomplished in real time.

#### **4. Evaluation**

To evaluate the CMPs, a robotic arm with a mounted hand was used. The Kuka LWR-IV anthropomorphic arm was used. It has seven degrees of freedom and, important for the presented approach, torque sensors at each joint. We omit further details, for example, internal parameters, of the robotic arm and refer the reader to [8]. In order to grasp objects, a three fingered BarrettHand BH8-280 was mounted at the end of the robotic arm. Further details on grasping are omitted, as this is not the focus of the chapter. While the approach based on the CMPs does not need the dynamical model of the robot, the dynamical model of the Kuka robot was used in all experiments. Note that the controller for the Kuka robot does not allow to fully disengage the dynamical model. However, using the CMPs, any possible model errors are compensated.

Evaluation of CMPs first focuses on estimating the quality of tracking the desired trajectory under various stiffness settings by measuring the errors between desired and actual robot trajectory. Here, the CMP approach was compared to a standard high-gain feedback control approach. Next the behavior of both systems when colliding with an unforeseen object was evaluated. The last part is concerned with evaluation of autonomously generated CMPs using the statistical graph search and evaluation of generalized CMPs.

#### **4.1. Tracking accuracy evaluation**

A trajectory from each discovered sequence is generated by encoding it as a DMP. The DMPs describing newly synthesized kinematic trajectories and the corresponding torque control signals (encoded as TPs) can then be used to execute new, not directly shown, movements

Using programming by demonstration approaches to learn CMPs can simplify compliant execution of dynamically versatile tasks. But executing demonstrations for each possible variation of the task would be time-consuming and cumbersome. For each new task descriptor, a new CMP needs to be learned, that is, motion trajectory needs to be learned through human demonstrations and executed on a robot for torque learning. Even if the deviation happens just on the torque level, for example, because of different payload, supervised learning of the torque is needed. Besides using actions graphs to generate new CMPs, as seen in the previous section, statistical generalization techniques can be employed. For that, a set of learned CMPs which transition smoothly between each other as a function of task descriptors, that is, query points, is needed. Using generalization, a task can be executed at an arbitrary query point *c*

For statistical generalization, we use Gaussian process regression (GPR), which can be used to

A CMP, defined by *wq*, *gq*, *wτ*, and *v*, can be used to execute a task, defined by a query *c*, in a compliant manner. By the above definition, *F* computes appropriate CMP parameters at the

Once the Gaussian process is *trained* using example training sets of CMPs, new appropriate CMPs for given queries *c* can be calculated by simple matrix multiplications, which can easily

To evaluate the CMPs, a robotic arm with a mounted hand was used. The Kuka LWR-IV anthropomorphic arm was used. It has seven degrees of freedom and, important for the presented approach, torque sensors at each joint. We omit further details, for example, internal parameters, of the robotic arm and refer the reader to [8]. In order to grasp objects, a three fingered BarrettHand BH8-280 was mounted at the end of the robotic arm. Further details on grasping are omitted, as this is not the focus of the chapter. While the approach based on the CMPs does not need the dynamical model of the robot, the dynamical model of the Kuka robot was used in all experiments. Note that the controller for the Kuka robot does not allow to fully

t

(17)

*Fc wg wv* : [ , , ,] a *q q*

while remaining compliant.

10 Robot Control

**3.2. Statistical generalization**

within the learned query space.

be accomplished in real time.

**4. Evaluation**

given query *c*, that is, at a given task variation.

learn a function

The analysis of tracking was performed on a pick-and-place task. The performance of the approach based on CMPs was compared with a common feedback approach. The evaluation setup can be seen in **Figure 6**, where a snapshot shows an example of pick-and-place movement. The initial movement trajectory for pick and place was demonstrated using kinesthetic guidance and encoded in DMPs as a part of CMPs. As the DMPs have been studied previously, we omit their specific evaluation and refer the reader to [13, 14, 24]. The demonstrated trajectory was then executed several times, using stiff robot control, that is, high feedback gains, which ensure accurate trajectory tracking. While executing the motion, the corresponding torques were obtained and encoded as CMPs. For evaluation, the mass of the object that the robot was holding was changed from 0.5 kg up to 4.5 kg with a step of 1 kg.

The movement was then executed using the complete CMPs, that is, including feed-forward torques, under different feedback gain settings. For comparison, for each object mass, several executions were performed by varying feedback gain parameters without using feed-forward torques, that is, using a common stiffens controller. To identify the tracking accuracy of the controllers, the maximum tracking error for each task execution was calculated. The maximal tracking error is defined as

$$e\_n = \max\_{\iota} \left( ||\!| \: p\_a(t) - \!| \!|\_d(t) \| \!| \right), \tag{18}$$

Where () is the current robot position on the trajectory, and the () is the desired position of the trajectory, both in Cartesian space. The task was performed for several different feedback gains settings going from 50 Nm/rad up to 2000 Nm/rad, selected so that covered a wide spectrum of compliance exhibited by a Kuka LWR-IV robot.

Results of the evaluation are shown in **Figure 3**, where we can see the mean and standard deviation of the *em* over all feedback gain settings and for each object weights. We can clearly see here that the tracking accuracy is much larger if only feedback control is used compared to the novel approach based on CMPs. The plot also shows the point where the tracking error starts to increase notably, regardless of the approach, which was 50 Nm/rad. Based on this results, the feedback gains for future evaluation were set to *K* = 50 Nm/rad.

**Figure 3.** Mean and standard deviation of task's maximum error for different the object masses. The CMP indicates the performance CMPs system, while the noFF indicates the mean and standard deviation for the feedback control without feed-forward torque signal. The gCMP indicates the performance of the generalized CMPs trajectories.

#### **4.2. Contact evaluation**

To evaluate the behavior of CMP-based control approach, while colliding with an unforeseen object, or an environment, a downward motion was demonstrated first, and then executed to train the CMP. During learning the movement was executed in a free space, without any contacts. To evaluate the forces and behavior when robot collides with an unforeseen object or environment, an object was placed in the path of the robot. The behavior of the robot when colliding was analyzed for three different cases: impedance control with high gains (*K* = 2000 Nm/rad), impedance control with low gains (*K* = 50 Nm/rad), and previously learned CMP with (*K* = 50 Nm/rad).

The results of impact are shown in **Figure 4**, where we can see that in case of high feedback gains the tracking error remains relatively small thorough the movement. However, in this

**Figure 4.** Robot colliding with an object while using different stiffness settings and control approaches. The graphs present collision trajectories and forces under two different stiffness settings (*K* = 1000 Nm/rad and *K* = 50 Nm/rad) and with two different control approaches (noFF and CMP). The top plot shows the actual robot task space position in vertical (*z*) axis, and the bottom plot shows TCP forces in the vertical (*z*) axis.

case, the forces rise significantly when the robot is in the contact with the environment. Vice versa, if the robot is compliant, that is, if the feedback gains are low, the forces are small, but the tracking accuracy is poor. Finally, CMPs combine both positive aspects: good tracking accuracy before contact and low interaction forces once the contact is established.

#### **4.3. Hierarchical graph evaluation**

**Figure 3.** Mean and standard deviation of task's maximum error for different the object masses. The CMP indicates the performance CMPs system, while the noFF indicates the mean and standard deviation for the feedback control without

To evaluate the behavior of CMP-based control approach, while colliding with an unforeseen object, or an environment, a downward motion was demonstrated first, and then executed to train the CMP. During learning the movement was executed in a free space, without any contacts. To evaluate the forces and behavior when robot collides with an unforeseen object or environment, an object was placed in the path of the robot. The behavior of the robot when colliding was analyzed for three different cases: impedance control with high gains (*K* = 2000 Nm/rad), impedance control with low gains (*K* = 50 Nm/rad), and previously learned CMP

The results of impact are shown in **Figure 4**, where we can see that in case of high feedback gains the tracking error remains relatively small thorough the movement. However, in this

**Figure 4.** Robot colliding with an object while using different stiffness settings and control approaches. The graphs present collision trajectories and forces under two different stiffness settings (*K* = 1000 Nm/rad and *K* = 50 Nm/rad) and with two different control approaches (noFF and CMP). The top plot shows the actual robot task space position in ver-

tical (*z*) axis, and the bottom plot shows TCP forces in the vertical (*z*) axis.

feed-forward torque signal. The gCMP indicates the performance of the generalized CMPs trajectories.

**4.2. Contact evaluation**

12 Robot Control

with (*K* = 50 Nm/rad).

Hierarchical graph approach was also evaluated using a Kuka LWR-IV robot arm. The demonstrator taught the robot several reaching movements while kinesthetically guiding the arm. Two examples of the learned movements that intersect each were shown. DMPs were used to encode all kinematic trajectories. They were used in the second step to obtain corresponding torque control signals, as described previously. Each movement was executed three times with different task time multipliers *κ* = {1,2,3}. The learned movement trajectories *qd* and the corresponding torque control signals *τ<sup>f</sup>* were used to execute the learned reaching CMP using a low-gain feedback controller.

As described in Section 3.1, the database was built using learned CMPs and used to find new reaching movements. A\* search algorithm found new sequences of nodes, as the demonstrated trajectories had parts that were sufficiently similar. Each new sequence started in the first node of one of the demonstrated trajectories and ended in the final node of one of the others. Each new sequence of mean position was then enhanced with mean torques and corresponding time duration three times, once per task time multiplier. Using DMPs, a complete representation of new reaching movement's trajectories was synthesized. A close-up of transitions of two example demonstrated and newly synthesized movements is shown in **Figure 5**. Smooth and continuous transitions can be observed. In order to evaluate transitions of corresponding torque signals, tracking error was observed during executions of example demonstrated CMPs and newly generated CMPs. No significant rise in errors could be observed.

**Figure 5.** Smooth and continuous transition can be observed in a close-up of two example demonstrated and newly synthesized movements. Original demonstrated movements are denoted by red color, while newly synthesized are marked with a dashed blue line.

#### **4.4. Statistical generalization evaluation**

Evaluation of statistical generalization can be done by comparing generalized CMPs to learned non-generalized CMPs. The selected task was a pick-and place task. Evaluation setup can be seen in **Figure 6**. A trajectory (), which successfully moves a hand weight from the initial position to the final position, was demonstrated by kinesthetic guidance. It was then executed five times using a standard high-gain controller which ensures high tracking accuracy. Mass of the object was changed each time. In this way, five different CMPs were learned, each having the same kinematic component and a different dynamic one. This set was used with statistical generalization techniques in order to generalize them over a one dimensional query, that is, a varying object mass. Generalized CMPs could compliantly move an object with arbitrary mass within the training space. New, generalized, CMPs were executed for nine different queries, covering demonstrated weights as well as points in between. Tracking errors were recorded for each execution. Each new generalized CMP was executed at eight different stiffness settings. Tracking errors were recorded for each execution. By comparing the maximum tracking errors of generalized CMPs to maximum tracking errors gained by executing learned CMPs, a slight but not statistically significant, increase in tracking error was observed. The errors prevailed at lower stiffness settings as the system is more susceptible to inaccuracies in generalized torque signals.

**Figure 6.** Experimental set-up for statistical generalization evaluation. The robot executed a pick-and-place task. Each execution the object weight was varied.

#### **5. Discussion**

In order to be efficient, robots need to autonomously react to the changes of the external conditions, such as changes in the environment or in the task. Relying on pure reproduction of learned motion will not provide the robots sufficient possibilities to reach to different situations, as learning to cover for all situations is simply impossible—the number of possibilities is far too great.

In this aspect, both presented methods of autonomous CMP generation, either through a hierarchical database search or through statistical generalization, expand the realm of learned motions to new, previously unexplored solutions. While the first method combines parts of demonstrated trajectories, the second one generates completely new trajectories within the database of learned motions. These new trajectories resemble the previously learned ones, that is, they have similar properties. The challenge of generating completely new motions using CMPs remains an open issue. While explorative methods, such as reinforcement learning methods, could be used to learn completely new motions with respect to a given reward, such methods are also known for requiring a large number of iterations. These might make practical use less convenient.

As an alternative to CMPs, dynamical models could be used. Here, it is important to note that CMPs model the complete task, for example, the task of interaction with a deformable object. Obtaining a dynamical model of a robot might be feasible, specifically for an expert, but obtaining a dynamical model for numerous tasks, such as the aforementioned task of interaction with a deformable object, might be extremely difficult, if not impossible. Thus, CMPs offer the means to solve this problem, as modeling is not needed, but they tend to solve it for one task at a time. Again, the presented methods of adapting to the changes of the task allow for learning of complete families of tasks from a small set of demonstrations.

Obtaining the CMPs is another area which requires further research, as currently these are obtained in a supervised manner during stiff robot behavior. Thus, the effort for the user still remains considerable. While we have proposed initial solutions in autonomously obtaining torque profiles in [25], a general method still remains an open research issue.

In conclusion, we have presented the CMP framework and two methods that expand the realm of possible application beyond simple imitation. The CMP framework, which bypasses the need for accurate dynamical modeling, was extensively evaluated and has been shown to offer a viable opportunity for compliant and at the same time accurate robotic behavior.

## **Acknowledgements**

five times using a standard high-gain controller which ensures high tracking accuracy. Mass of the object was changed each time. In this way, five different CMPs were learned, each having the same kinematic component and a different dynamic one. This set was used with statistical generalization techniques in order to generalize them over a one dimensional query, that is, a varying object mass. Generalized CMPs could compliantly move an object with arbitrary mass within the training space. New, generalized, CMPs were executed for nine different queries, covering demonstrated weights as well as points in between. Tracking errors were recorded for each execution. Each new generalized CMP was executed at eight different stiffness settings. Tracking errors were recorded for each execution. By comparing the maximum tracking errors of generalized CMPs to maximum tracking errors gained by executing learned CMPs, a slight but not statistically significant, increase in tracking error was observed. The errors prevailed at lower stiffness settings as the system is more susceptible to inaccuracies in

**Figure 6.** Experimental set-up for statistical generalization evaluation. The robot executed a pick-and-place task. Each

In order to be efficient, robots need to autonomously react to the changes of the external conditions, such as changes in the environment or in the task. Relying on pure reproduction of learned motion will not provide the robots sufficient possibilities to reach to different situations, as learning to cover for all situations is simply impossible—the number of possi-

In this aspect, both presented methods of autonomous CMP generation, either through a hierarchical database search or through statistical generalization, expand the realm of learned motions to new, previously unexplored solutions. While the first method combines parts of demonstrated trajectories, the second one generates completely new trajectories within the database of learned motions. These new trajectories resemble the previously learned ones, that is, they have similar properties. The challenge of generating completely new motions using CMPs remains an open issue. While explorative methods, such as reinforcement learning methods, could be used to learn completely new motions with respect to a given reward, such methods are also known for requiring a large number of iterations. These might make practical

generalized torque signals.

14 Robot Control

execution the object weight was varied.

**5. Discussion**

bilities is far too great.

use less convenient.

The research leading to these results has been partially founded from the European Community's Seventh Framework Programme FP7/2007-2013 (Specific Programme Cooperation, Theme 3, Information and Communication Technologies) under Grant agreement no. 600578, ACAT.

#### **Author details**

Miha Deniša\* , Tadej Petrič, Andrej Gams and Aleš Ude

\*Address all correspondence to: miha.denisa@ijs.si

Department of Automatics, Biocybernetics and Robotics, Jožef Stefan Institute, Ljubljana, Slovenija

#### **References**

[1] Albu-Schaffer A., Eiberger O., Grebenstein M., Haddadin S., Ott C., Wimbock T., Wolf S., and Hirzinger G. Soft robotics. IEEE Robotics and Automation Magazine. 2008;15(3):20–30.


[15] Forte D., Gams A., Morimoto J., and Ude A. On-line motion synthesis and adaptation using a trajectory database. Robotics and Autonomous Systems. 2012;60(10):1327–1339.

[2] Franklin D.W., and Wolpert D.M. Computational mechanisms of sensorimotor control.

[3] Nguyen-Tuong D., and Peters J. Learning robot dynamics for computed torque control using local Gaussian processes regression. In: ECSIS symposium learning and adaptive

[4] Nguyen-Tuong D., and Peters J. Model learning for robot control: a survey. Cognitive

[5] Bristow D.A., Tharayil M., and Alleyne A.G. A survey of iterative learning control. IEEE

[6] Schwarz M., and Behnke S. Compliant robot behavior using servo actuator models identified by iterative learning control. In: Proceedings of 17th robo cup international

[7] Gautier M., Jubien A., and Janot A. Iterative learning identification and computed torque control of robots. In: IEEE/RSJ international conference on intelligent robots and

[8] Bischoff R., Kurth J., Schreiber G., Koeppe R., Albu-Schaeffer A., Beyer A., Eiberger O., Haddadin S., Stemmer A., Grunwald G., and Hirzinger G. The KUKA-DLR Lightweight Robot arm—a new reference platform for robotics research and manufacturing.

[9] Petrič T., Gams A., Žlajpah L., and Ude A. Online learning of task-specific dynamics for periodic tasks. In: Intelligent robots and systems (IROS 2014); Chicago, IL, USA.

[10] Deniša M., Gams A., Ude A., and Petrič T. Learning compliant movement primitives through demonstration and statistical generalization. IEEE/ASME Transactions on

[11] Deniša M., Petrič T., Asfour T., and Ude A. Synthesizing compliant reaching movements by searching a database of example trajectories. In: International conference on

[12] Krakauer J. W., Ghilardi M.F., and Ghez C. Independent learning of internal models for kinematic and dynamic control of reaching. Nature Neuroscience. 1999;2(11):1026–

[13] Ude A., Gams A., Asfour T., and Morimoto J. Task-specific generalization o discrete and periodic dynamic movement primitives. IEEE Transactions on Robotics.2010;26(5):

[14] Ijspeert A.J., Nakanishi J., Hoffmann H., Pastor P., and Schaal S. Dynamical movement primitives: learning attractor models for motor behaviors. Neural Computing.

behaviors for robotic systems; Edinburgh, Scotland, UK. 2008. p. 59–64.

Neuron. 2011;72(3):425–442.

16 Robot Control

Processing. 2011;12(4):319–340.

2014. p. 1790–1795.

1031.

800–815.

2013;25(2):328–373.

Mechatronics. 2015; (99):1.

Control Systems. 2006;26(3):96–114.

symposium; Eindhoven, Netherlands. 2013. p. 207–218.

systems (IROS); Tokyo, Japan. 2013. p. 3419–3424.

In: 41st international symposium on robotics; 2010.

humanoid robots; Atlanta, GA, USA. 2013. p. 540–546.


## **Induced Force Hovering of Spherical Robot by Under-Actuated Control of Dual Rotor**

G. Santos-Medina, K.Y. Heras-Gaytán, E.A. Martínez-García, R. Torres-Córdoba and V. Carrillo-Saucedo

Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/63548

#### **Abstract**

This chapter discusses the design and modelling of a spherical flying robot. The main objective is to control its hovering and omnidirectional mobility by controlling the air mass differential pressure between two asynchronous coaxial rotors that are aligned collinearly. The spherical robot design has embedded a gyroscopic mechanism of three rings that allow the rotors' under-actuated mobility with 3DOF. The main objective of this study is to maintain the thrust force with nearly vertical direction. The change in pressure between rotors allows to vary the rotors' tilt and pitch. The system uses special design propellers to improve the laminar air mass flux. A nonlinear fitting model automatically calibrates the rotors' angular speed as a function of digital values. This model is the functional form that represents the reference input to control the rotors' speed, validated by three types controllers: P, PI, and PID. The robot's thrust and induced forces and flight mechanics are proposed and analysed. The simulation results show the feasibility of the approach.

**Keywords:** flight mechanics, flying control, robot modelling, thrust force, UAV, under-actuation

#### **1. Introduction**

Nowadays, unmanned aerial vehicle (UAV) robots are being deployed at an increased rate for numerous applications falling into a variety of engineering fields. There exist numer‐

© 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

ous kinds of rotary-wing-based robotic technologies in particular with active devices. Robots with rotating wings are capable to self-control over lift, propulsion, landing, hovering And take-off tasks [1–4]. Overcoming vertical flight (with minimum energy cost) is fundamen‐ tal to accomplish autonomous precise tasks. One fundamental aspect in controlling and designing rotary-wing-based intelligent machines is to consider under-actuated issues to reduce the number of actuators. Under-actuated flying robots perform motion tasks more naturally, taking advantage of the inertial and gravitational forces, consequently, reducing the use of electrical energy. Biological flying birds are instances of extremely efficient underactuated bodies. Therefore, in order to design flying machines with a reduced number of actuators, it is essential to model and understand the mechanical nature of the robot mechanics, the fluids and their physics-based relationship.

The present work has foundations on the prototype of a home-made spherical aerial robot. Some experiments can be viewed at https://www.youtube.com/watch?v=rrGH1Oh\_beM. Nevertheless, the purpose of this chapter is not on showing and discussing experimental results, but on mathematically sustaining the hypothesis of the robot's flight mechanics and control. Unlike, known spherical design approaches [5–7], rather than deploying aileron-like propellers, we proposed yaw, pitch and roll changes through under-actuation exerting an inner gyroscopic mechanism. In the present chapter, the authors are particularly interested in disclosing the physical model of a dual rotary-wing spherical robot with an under-actuated gyroscopic mechanism. The model has been divided into four major areas: the robot's flight mechanics with direct and inverse solution, the thrusting or induced force model, the rotors control model and a proportional integral derivative (PID) based control with non-stationary reference values.

This chapter is organised as follows. In Section 2, the design and mechanical aspects of the aerial robot are presented. Section 3 presents the kinematic direct and inverse solutions of the flight mechanics. In Section 4, the acceleration components and forces involved in the robot's aerodynamics are discussed. Section 5 presents the robot's thrusting force model that involves two collinear induced forces. Section 6 presents the rotors' actuator speed models that are proposed from empirical measurements, and subsequently, the analytical solution is obtained. In Section 7, the actuators' feedback linear control is described. Finally, in Section 8, conclusions are drawn.

#### **2. Spherical gyroscopic robot**

Unlike other reported approaches [8–13], in this study, the authors have proposed an omnidirectional spherical design with two rotors vertically collinear (see **Figure 1**, right, and **Figure 2**).

Induced Force Hovering of Spherical Robot by Under-Actuated Control of Dual Rotor http://dx.doi.org/10.5772/63548 21

**Figure 1.** Left: Aerial robot real prototype. Right: Robot's main actuated and under-actuated mechanical elements.

**Figure 2.** Left: Robot's global and local spherical coordinates. Centre: Robot's flying space (Eqs. (1)–(3)). Right: recur‐ sive trajectory generation (Texto) and (Texto).

#### **3. Flight mechanics model**

ous kinds of rotary-wing-based robotic technologies in particular with active devices. Robots with rotating wings are capable to self-control over lift, propulsion, landing, hovering And take-off tasks [1–4]. Overcoming vertical flight (with minimum energy cost) is fundamen‐ tal to accomplish autonomous precise tasks. One fundamental aspect in controlling and designing rotary-wing-based intelligent machines is to consider under-actuated issues to reduce the number of actuators. Under-actuated flying robots perform motion tasks more naturally, taking advantage of the inertial and gravitational forces, consequently, reducing the use of electrical energy. Biological flying birds are instances of extremely efficient underactuated bodies. Therefore, in order to design flying machines with a reduced number of actuators, it is essential to model and understand the mechanical nature of the robot

The present work has foundations on the prototype of a home-made spherical aerial robot. Some experiments can be viewed at https://www.youtube.com/watch?v=rrGH1Oh\_beM. Nevertheless, the purpose of this chapter is not on showing and discussing experimental results, but on mathematically sustaining the hypothesis of the robot's flight mechanics and control. Unlike, known spherical design approaches [5–7], rather than deploying aileron-like propellers, we proposed yaw, pitch and roll changes through under-actuation exerting an inner gyroscopic mechanism. In the present chapter, the authors are particularly interested in disclosing the physical model of a dual rotary-wing spherical robot with an under-actuated gyroscopic mechanism. The model has been divided into four major areas: the robot's flight mechanics with direct and inverse solution, the thrusting or induced force model, the rotors control model and a proportional integral derivative (PID) based control with non-stationary

This chapter is organised as follows. In Section 2, the design and mechanical aspects of the aerial robot are presented. Section 3 presents the kinematic direct and inverse solutions of the flight mechanics. In Section 4, the acceleration components and forces involved in the robot's aerodynamics are discussed. Section 5 presents the robot's thrusting force model that involves two collinear induced forces. Section 6 presents the rotors' actuator speed models that are proposed from empirical measurements, and subsequently, the analytical solution is obtained. In Section 7, the actuators' feedback linear control is described. Finally, in Section 8, conclusions

Unlike other reported approaches [8–13], in this study, the authors have proposed an omnidirectional spherical design with two rotors vertically collinear (see **Figure 1**, right, and

mechanics, the fluids and their physics-based relationship.

reference values.

20 Robot Control

are drawn.

**Figure 2**).

**2. Spherical gyroscopic robot**

Flight mechanics refers to the study of geometry of flight of a heavier-than-air aircraft, considering aerodynamic aspects. Expressions (1)–(3) model the three-dimension robot's Cartesian kinematic components that describe its motion. The components, namely, *x*, *y* and *z*, are the space positions w.r.t. the location of the robot's starting flight. The proposed kinematic model is constrained with initial posture as the inertial frame origin, where *d* is the distance between the robot's instantaneous 3D position and its Cartesian origin. Azimuth angle ϕ0 is w.r.t. the plane *XY*, and the elevation angle ϕ1 is w.r.t. the *Y*-axis:

$$\mathbf{x} = d \cos(\phi \mathbf{j}) \sin(\phi\_0) \tag{1}$$

The *y* component (vertical) is expressed as

$$y = d \sin\left(\phi\right) \tag{2}$$

And, the *z* component is expressed as

$$z = d \cos(\phi\_l) \cos(\phi\_b) \tag{3}$$

For further purpose, the inverse solution is obtained by an algebraic arrangement of deriva‐ tives. The first-order derivatives of Eqs. (1)–(3) are obtained and shown in Eqs.(4)–(6),

$$\dot{\mathbf{x}} = \dot{d}\cos(\phi\_l)\sin(\phi\_0) + d\cos(\phi\_l)\cos(\phi\_0)\dot{\phi}\_0 - d\sin(\phi\_l)\sin(\phi\_0)\dot{\phi}\_l \tag{4}$$

The vertical component is expressed as

$$
\dot{\varphi} = \dot{d}\sin(\phi\_l) + d\cos(\phi\_l)\dot{\phi}\_l \tag{5}
$$

And, the *z* component is expressed as

$$\dot{z} = \dot{d}\cos(\phi\_{\text{l}})\cos(\phi\_{\text{o}}) + d\cos(\phi\_{\text{l}})\cos(\phi\_{\text{o}})\dot{\phi}\_{\text{o}} \tag{6}$$

**Figure 2** centre depicts the robot's flying space, which is spherical with the Cartesian origin at robot's starting flying task.

Expressing in the matrix form, the first-order derivative *d***p**/*dt* w.r.t. time is

$$
\dot{p} = \begin{pmatrix} \dot{\mathbf{x}} \\ \dot{\mathbf{y}} \\ \dot{z} \end{pmatrix} = \begin{pmatrix} d\mathbf{C}\_1 \mathbf{C}\_0 & -d\mathbf{S}\_1 \mathbf{S}\_0 & \mathbf{C}\_1 \mathbf{S}\_0 \\ \mathbf{0} & d\mathbf{C}\_1 & \mathbf{S}\_1 \\ -d\mathbf{C}\_1 \mathbf{C}\_0 & -d\mathbf{S}\_1 \mathbf{C}\_0 & \mathbf{C}\_1 \mathbf{C}\_0 \end{pmatrix} \begin{pmatrix} \dot{\phi}\_0 \\ \dot{\phi}\_1 \\ \dot{d} \end{pmatrix} \tag{7}
$$

By simplifying, the linear equation of direct kinematic is denoted by the Jacobian matrix **J** and the first-order vector of independent variables,

$$
\dot{p} = J \dot{\Phi} \tag{8}
$$

In order to obtain a recursive functional form equivalent to previous state variables, the derivatives are expressed in the following manner:

() ( ) 1 0 *x d* = cos sin f

> ( ) <sup>1</sup> *y d* = sin f

() ( ) 1 0 *z d* = cos cos f

tives. The first-order derivatives of Eqs. (1)–(3) are obtained and shown in Eqs.(4)–(6),

() ( ) () ( ) () ( ) 1 0 1 00 1 01 =+ - cos sin cos cos sin sin

() () <sup>1</sup> 1 1 *yd d* & = + &sin cos f

() ( ) () ( ) 1 0 1 00 = + cos cos cos cos

**Figure 2** centre depicts the robot's flying space, which is spherical with the Cartesian origin at

1 0 10 10 0

10 10 10

By simplifying, the linear equation of direct kinematic is denoted by the Jacobian matrix **J** and

 <sup>×</sup> æö æ - <sup>ö</sup> æ ö ç÷ ç <sup>÷</sup> ç ÷ = = ç÷ ç <sup>÷</sup> ç ÷ ç÷ ç <sup>÷</sup> ç ÷ èø è- - <sup>ø</sup> è ø

*z dC C dS C C C d*

& & & & & & &

*x dC C dS S C S p y dC S*

1 11

f

f

 f

Expressing in the matrix form, the first-order derivative *d***p**/*dt* w.r.t. time is

0

 f

The *y* component (vertical) is expressed as

22 Robot Control

And, the *z* component is expressed as

f

The vertical component is expressed as

And, the *z* component is expressed as

the first-order vector of independent variables,

robot's starting flying task.

 f

f

 f

 f

For further purpose, the inverse solution is obtained by an algebraic arrangement of deriva‐

 ff

> fj

 f

 ff& & & *d dz*(6)

& & & *x d* & *d d* (4)

 f  ff

(1)

(2)

(3)

& (5)

(7)

$$\frac{d}{dt}\,\,p = J\,\frac{d}{dt}\Phi\tag{9}$$

Time differentials are eliminated, and the integration operators complete the remaining differentials *dp* and *dϕ*:

$$\bigwedge\_{\blackleftrightarrow}^{\flat\_2} dp = J \bigwedge\_{\blackleftrightarrow}^{\flat\_2} d\Phi \tag{10}$$

Thus, to solve for the robot's Cartesian position a recursive form is obtained by algebraically reordering. Next, robot's position **p***<sup>t</sup>*+1 is obtained by successive approximations of ϕ*<sup>t</sup>* until ϕ*<sup>f</sup>* :

$$p\_{\iota+1} = p\_{\iota} + J\left(\phi\_{\iota} - \phi\_{\iota}\right) \tag{11}$$

In addition, the kinematic inverse solution requires the inverse-squared Jacobian matrix, assuming that it is an invertible and non-singular matrix, with non-zero determinant:

$$J^{-1} = \frac{1}{d^2 C\_1 \left( \left[ C\_1 C\_0 \right]^2 + \left[ S\_0 S\_1 \right] \right)} \begin{pmatrix} dC\_0 & 0 & -d S\_0 \\ dC\_1 S\_0 S\_1 & dC\_1^2 & -d C\_1 C\_0 S\_1 \\ \left( dC\_1 \right)^2 S\_0 & -d C\_1 C\_0 S\_1 & \left( d C\_1 \right)^2 C\_0 \end{pmatrix} \tag{12}$$

Therefore, the first-order inverse kinematic is obtained by an algebraic approach:

$$
\dot{\Phi} = J^{-1} \dot{p} \tag{13}
$$

As described earlier, we complete the differentials *dp* and *dϕ*:

$$\int\_{\dot{\Phi}\_{l}}^{\dot{\Phi}\_{2}} d\Phi = J^{-1} \int\_{\rho\_{l}}^{\rho\_{2}} dp \tag{14}$$

By integrating both sides of the equality, respectively, a recursive inverse solution in terms of the rotor's angular speed is obtained,

$$
\dot{\Phi}\_{\iota+1} = \dot{\Phi}\_{\iota} + J^{-1} \left( \dot{p}\_f - \dot{p}\_\iota \right) \tag{15}
$$

where **p***<sup>t</sup>* is the actual robot 3D position and **p***<sup>f</sup>* represents the final desired position in space. To achieve such location, the robot recursively approximates the next desired rotor's controlled velocity. The next figure depicts a simulation result where the aerial robot successively approached the final desired position, staring from the Cartesian origin.

#### **4. Aerodynamic robot's model**

The aerodynamic robot's model refers to the application of the Newton's second law of motion in three dimensions to infer the thrusting force *T* and other involved forces that produce the Cartesian accelerations:

$$\ddot{\mathbf{x}} = \ddot{d}c\_1\mathbf{s}\_0 - \dot{d}s\_1\mathbf{s}\_0\dot{\boldsymbol{\phi}}\_1 + \dot{d}c\_1c\_0\dot{\boldsymbol{\phi}}\_0 + \dot{d}c\_1c\_0\boldsymbol{\phi}\_1 - d\mathbf{s}\_1c\_0\dot{\boldsymbol{\phi}}\_0\dot{\boldsymbol{\phi}}\_1 - d c\_1c\_0\dot{\boldsymbol{\phi}}\_0^2$$

$$\dots \quad \dots \quad \dots \quad \dots \quad \dots \quad \dots \tag{16}$$

$$-d\mathbf{c}\_1 \mathbf{c}\_0 \ddot{\boldsymbol{\phi}}\_0 - \dot{d}\mathbf{s}\_1 \mathbf{s}\_0 \dot{\boldsymbol{\phi}}\_1 - d\mathbf{c}\_1 \mathbf{s}\_0 \dot{\boldsymbol{\phi}}\_1^2 - d\mathbf{s}\_1 \mathbf{c}\_0 \dot{\boldsymbol{\phi}}\_0 \dot{\boldsymbol{\phi}}\_0 - d\mathbf{s}\_1 \mathbf{s}\_0 \ddot{\boldsymbol{\phi}}\_1$$

$$\ddot{y} = \ddot{d}\,\mathbf{s}\_1 + dc\_1\dot{\phi}\_1 + \dot{d}\,c\_1\phi\_1 - ds\_1\dot{\phi}\_1^2 + dc\_1\ddot{\phi}\_1 \tag{17}$$

$$\begin{aligned} \ddot{z} = \ddot{d}c\_1 c\_0 - \dot{d}s\_0 c\_0 \dot{\phi}\_l - \dot{d}c\_1 s\_0 \dot{\phi}\_0 - d s\_1 c\_0 \phi\_l - d c\_1 c\_0 \dot{\phi}\_l^2 + d s\_1 s\_0 \dot{\phi}\_0 \dot{\phi}\_l \\ -d s\_1 c\_0 \ddot{\phi}\_1 - \dot{d}c\_1 s\_0 \dot{\phi}\_0 + \dot{d} s\_1 s\_0 \dot{\phi}\_0 \dot{\phi}\_l - d c\_1 c\_0 \dot{\phi}\_0^2 - d c\_1 s\_0 \ddot{\phi}\_0 \end{aligned} \tag{18}$$

In the matrix form, the following equation represents the direct kinematic solution for the Cartesian accelerations:

$$
\ddot{p} = \begin{pmatrix} d\mathbf{c}\_{\mathbf{l}}\mathbf{c}\_{0} & -d\mathbf{s}\_{\mathbf{l}}\mathbf{s}\_{0} & \mathbf{c}\_{\mathbf{l}}\mathbf{s}\_{0} \\ 0 & dc\_{\mathbf{l}} & \mathbf{s}\_{\mathbf{l}} \\ -d\mathbf{c}\_{\mathbf{l}}\mathbf{c}\_{0} & -d\mathbf{s}\_{\mathbf{l}}\mathbf{c}\_{0} & \mathbf{c}\_{\mathbf{l}}\mathbf{s}\_{0} \end{pmatrix} \begin{pmatrix} \dot{\phi}\_{0} \\ \dot{\phi}\_{1} \\ \dot{d} \end{pmatrix} + \begin{pmatrix} -dc\_{\mathbf{l}}\mathbf{c}\_{0} & -dc\_{\mathbf{l}}\mathbf{c}\_{0} & -2dc\_{\mathbf{l}}\mathbf{c}\_{0} & 2dc\_{\mathbf{l}}\mathbf{c}\_{0} & -2ds\_{\mathbf{l}}\mathbf{s}\_{0} \\ 0 & -ds\_{\mathbf{l}} & 0 & 0 & 2c\_{\mathbf{l}} \\ -dc\_{\mathbf{l}}\mathbf{c}\_{0} & -dc\_{\mathbf{l}}\mathbf{c}\_{0} & -2ds\_{\mathbf{l}}\mathbf{s}\_{0} & 2dc\_{\mathbf{l}}\mathbf{s}\_{0} & -2ds\_{\mathbf{l}}\mathbf{c}\_{0} \end{pmatrix} \tag{19}
$$

or

$$
\ddot{p} = J\_1 \cdot \dot{\Phi} + J\_2 \cdot \dot{\Phi}
$$

From the previous expression, the vector acceleration is substituted into the Newton's second law of motion,

$$f = m \cdot \ddot{p} \tag{20}$$

where the Cartesian force components defined in robot's local inertial frame are expressed as follows:

$$f\_x = T\cos(\theta\_1)\sin(\theta\_0)\tag{21}$$

The force component along the robot's local *y* component is expressed as

( ) <sup>1</sup>

*t t ft* <sup>+</sup> = F&& + *Jp p* & &-F (15)

2

(16)

(18)

 f

 ff

22 2

22 2

*f mp* = × && (20)

*ds*

 f

¨

 f

&& & & & && & *y d s dc d c ds dc* & (17)

2

 f

2

& & &&

 f  ff


where **p***<sup>t</sup>* is the actual robot 3D position and **p***<sup>f</sup>* represents the final desired position in space. To achieve such location, the robot recursively approximates the next desired rotor's controlled velocity. The next figure depicts a simulation result where the aerial robot successively

The aerodynamic robot's model refers to the application of the Newton's second law of motion in three dimensions to infer the thrusting force *T* and other involved forces that produce the

10 101 10 0 101 10 01 10 0

 f

&& & & & && &

 ff

f ff

*x d c s ds s dc c dc c ds c dc c*

 f


f

=- + + - -

f

f

f

&

f

&

 f

 f

f

f

&&

&& & & & & &

*dc c ds s dc s ds c ds s*

*z d c c ds c dc s ds c dc c ds s*

 f

=- - - - +


*ds c dc s ds s dc c dc s*

 f

<sup>2</sup> =+ + - + 1 1 1 11 11 11

& & & &&

2 10 101 101 1010 10 0 1

&& &&

10 101 10 0 101 101 10 01

 f

& & & & & &&&

In the matrix form, the following equation represents the direct kinematic solution for the

10 10 10 0 10 10 10 10 10

1 2 *pJ J* =×+ × F

& & &&

From the previous expression, the vector acceleration is substituted into the Newton's second

0 0 0 02

<sup>æ</sup> - ö æ æ ö --- - <sup>ö</sup> <sup>ç</sup> ÷ ç ç ÷ <sup>÷</sup> <sup>=</sup> ç ÷ + - <sup>ç</sup> ÷ ç <sup>÷</sup> <sup>ç</sup> ÷ ç ç ÷ <sup>÷</sup> è- - ø è è ø --- - <sup>ø</sup>

*dc c ds s c s dc c dc c dc c dc c ds s*

1 11 1 1 10 10 10 10 10 10 10 10

& (19)

 F

*dc s ds c dc c ds c c s d dc c dc c ds s dc s c*

10 10 0 10 01 10 0 10 1 0

 ff

1

approached the final desired position, staring from the Cartesian origin.

**4. Aerodynamic robot's model**

&& &&

& &

Cartesian accelerations:

24 Robot Control

Cartesian accelerations:

*p*

law of motion,

or

$$f\_{\circ} = T \sin \left(\theta\_{\text{l}}\right) \tag{22}$$

And, the force component along the robot's local *Z* component is expressed as

$$\begin{aligned} \,\_{1}f\_{z} &= T\cos(\theta\_{1})\cos(\theta\_{0}) \end{aligned} \tag{23}$$

By simplifying the Cartesian force components in the matrix form

$$\begin{aligned} \mathbf{J} &= T \begin{pmatrix} \mathbf{c}\_1 \mathbf{s}\_0 \\ \mathbf{s}\_1 \\ \mathbf{c}\_1 \mathbf{c}\_0 \end{pmatrix} \end{aligned} \tag{24}$$

And, by substituting the functional form of the vector force *f* into the Newton's second law,

$$\mathbf{m} \cdot \ddot{\mathbf{p}} = T \begin{pmatrix} \mathbf{c}\_{1} \mathbf{s}\_{0} \\ \mathbf{s}\_{1} \\ \mathbf{c}\_{1} \mathbf{c}\_{0} \end{pmatrix} \tag{25}$$

Thus, by reordering the previous equation, we substitute the vector constraints **w**T =(*C*1*S*0, *S*1, *C*1*C*0). The acceleration vector *d*<sup>2</sup> **p**/*dt*<sup>2</sup> is a function of the next position **p***<sup>t</sup>*+1 and the rotors variables:

$$Tw = m \, \vec{p} \left( p\_{\iota \ast 1}, \Phi\_{\lrcorner \prime}, \dot{\Phi}\_{\lrcorner} \right) \tag{26}$$

By dropping off the induced robot's force *T*,

$$T = \stackrel{\circ}{w}^{-1} m \ddot{p} \left( p\_{\iota \ast 1}, \Phi\_f, \dot{\Phi}\_\iota \right) \tag{27}$$

So far, in this expression, the total thrusting induced force *T* represents the robot's global flying force. Thus, *T* is an arithmetic result produced by the sum of the top rotor's induced force *T*<sup>1</sup> and the below rotor's induced force *T*2 according to the following governing constraints:


**(c)** For *T*1<*T*2, opposed to constraint (b), then *T*2 = *α*2*T*1 and *T* = *T*1(1+*α*2).

Here, the numerical factors *α*1,2 are gains denoting rotors' speed-rate differences. For either constraint (b) or (c), the gyroscopic mechanism angles' tilt and pitch are affected, consequently changing the robot's azimuth and elevation angles.

#### **5. Induced force model**

According to the depictions of **Figure 3**, the rotors are continuously pushing the air down. As per Newton's third law, an equal and opposite reaction force, denoted as rotor thrust, is acting on the rotor due to air. The induced force model refers to the thrusting force exerted to accelerate the robot. And at a constant velocity the quasi-static hovering is achieved [1–4].

**Figure 3.** Rotors' flow conditions in the slip streams.

The momentum conservation is obtained by relating the induced force *T*2 to the rate of momentum change. It is the mass rate and the far-field wake-induced velocity *vw* below rotor 2, where *dm*/*dt*=ρ*Av*2 and the rotor disk area *A* = π*R*<sup>2</sup> . Thus, the moment conservation

$$T = \dot{m}\upsilon\_w\tag{28}$$

The energy conservation per unit time

$$T\nu = \frac{1}{2}\dot{m}\nu\_w^2\tag{29}$$

To obtain a relationship between *v* and *vw*, let us substitute *T* and *dm*/*dt*,

$$
\dot{m}\upsilon\_{\rm w}\upsilon = \frac{1}{2}\dot{m}\upsilon\_{\rm w}^2\tag{30}
$$

#### Induced Force Hovering of Spherical Robot by Under-Actuated Control of Dual Rotor http://dx.doi.org/10.5772/63548 27

Algebraically simplifying,

$$\mathbf{v} = \frac{1}{2}\mathbf{v}\_w; \mathbf{v}\_w = \mathbf{2}\mathbf{v} \tag{31}$$

Hence, substituting *vw* into *T*,

$$T = \dot{m}v\_w = 2\dot{m}v\tag{32}$$

Then,

**(c)** For *T*1<*T*2, opposed to constraint (b), then *T*2 = *α*2*T*1 and *T* = *T*1(1+*α*2).

changing the robot's azimuth and elevation angles.

**5. Induced force model**

26 Robot Control

**Figure 3.** Rotors' flow conditions in the slip streams.

The energy conservation per unit time

2, where *dm*/*dt*=ρ*Av*2 and the rotor disk area *A* = π*R*<sup>2</sup>

Here, the numerical factors *α*1,2 are gains denoting rotors' speed-rate differences. For either constraint (b) or (c), the gyroscopic mechanism angles' tilt and pitch are affected, consequently

According to the depictions of **Figure 3**, the rotors are continuously pushing the air down. As per Newton's third law, an equal and opposite reaction force, denoted as rotor thrust, is acting on the rotor due to air. The induced force model refers to the thrusting force exerted to accelerate the robot. And at a constant velocity the quasi-static hovering is achieved [1–4].

The momentum conservation is obtained by relating the induced force *T*2 to the rate of momentum change. It is the mass rate and the far-field wake-induced velocity *vw* below rotor

> 1 <sup>2</sup> 2

To obtain a relationship between *v* and *vw*, let us substitute *T* and *dm*/*dt*,

. Thus, the moment conservation

<sup>=</sup> & *T mvw* (28)

*Tv mv* = *<sup>w</sup>* & (29)

(30)

$$T = \text{2}\,\rho A \text{v}^2\tag{33}$$

Dropping off *v*, the following expression is obtained;

$$\mathbf{v} = \sqrt{\frac{T}{2\rho A}}\tag{34}$$

The propulsive power *Pw* is the thrusting force *T* capable to move the robot at a given velocity (distance over time):

$$P\_w = T\nu = T\sqrt{\frac{T}{2\rho A}}\tag{35}$$

The induced power per unit thrust for a hovering rotor can be written as

$$\frac{P\_w}{T} = \nu = \sqrt{\frac{T}{2\rho\mathcal{A}}}\tag{36}$$

The above expression indicates that, for a low inflow velocity, the efficiency is higher. This is possible if the rotor has a low disk loading (*T*/*A*). Note that the parameter determining the induced power is essentially *T*/(*A*). Therefore, the effective disk loading increases with an increase in altitude and temperature.

From previous analysis, let us now precisely define the thrusting force for rotors 1 and 2, according to **Figure 3** (left side). For rotor 1, the air mass flow is

$$
\rho \mathbf{u}\_1 = \rho \mathbf{A}\_1 \mathbf{v}\_1 \tag{37}
$$

Hence, considering only rotor 1, the induced force is

$$T\_1 = m\_1 v\_2 \tag{38}$$

The energy conservation principle for rotor 1 is expressed as

$$T\_1 \upsilon\_1 = \frac{1}{2} \dot{m}\_1 \upsilon\_2^2 \tag{39}$$

And finding a relationship between *v*1 and *v*2 in accordance with **Figure 3** (left side),

$$\mathbf{v}\_2 = \mathbf{2}\mathbf{v}\_1\tag{40}$$

Thus,

$$T\_1 = \mathcal{D}\rho\_1 A\_1 \mathbf{v}\_1^\star$$

The induced air velocity induced by rotor 1 is modelled by

$$\nu\_{\rm l} = \sqrt{\frac{T\_{\rm l}}{2\rho\_{\rm l}A\_{\rm l}}} \tag{42}$$

Similarly, modelling both the induced force *T*2 and velocity *v*2 for rotor 2, the following analysis is developed. The airflow rate,

$$
\rho m\_2 = \rho\_2 A\_2 v\_3 \tag{43}
$$

And the induced force *T*2 considers the inflow air mass and the far-field wake-induced velocity *vw*,

$$T\_2 = \dot{m\_2} \upsilon\_{\text{w}} \tag{44}$$

The energy conservation for the second rotor is expressed as

$$T\_2 \upsilon\_3 = \frac{1}{2} \mathfrak{m}\_2 \upsilon\_\mathrm{w}^2 \tag{45}$$

The relationship between far-field wake-induced air velocity *vw* and *v*<sup>3</sup> is given by *vw*=2*v*3, and

$$T\_2 = \mathcal{D}\rho\_2 A\_2 \mathbf{v}\_3^2 \tag{46}$$

Therefore, the second rotor's air induced velocity is

$$\nu\_3 = \sqrt{\frac{T\_2}{2\rho\_2 A\_2}}\tag{47}$$

From the three previous postulates of **Figure 3**, let us deduce the conditions when both rotors, although asynchronous, simultaneously induce the airflow equally, when *v*1=*v*3 (**Figure 3a**). For this case, the total robot's thrusting force *T*=*T*1+*T*2,

$$T = 2\rho\_1 A\_1 \mathbf{v}\_1^2 + 2\rho\_2 A\_2 \mathbf{v}\_2^2 \tag{48}$$

For case 1, let us assume *A*1=*A*2 and *v*2=*v*3 through the second rotor's disc area. And,

$$\upsilon\_{\mathsf{W}} = 2\upsilon\_3; \textit{like} \mathsf{w} \mathsf{i}\text{e}, \upsilon\_2 = 2\upsilon\_1; \textit{hence} \upsilon\_{\mathsf{W}} = 2(2\upsilon\_1) = 4\upsilon\_1 \tag{49}$$

Rewriting total *T* as a function of *v*1, thus we have

$$
\rho\_2 = \frac{m\_c}{\nu\_c} \tag{50}
$$

The air mass variation is denoted as the mass derivative w.r.t. time,

$$
\dot{m} = \rho\_2 A V\_2 \tag{51}
$$

Now, for the case *v*1>*v*3,

(39)

(43)

(44)

(45)

The energy conservation principle for rotor 1 is expressed as

The induced air velocity induced by rotor 1 is modelled by

The energy conservation for the second rotor is expressed as

Therefore, the second rotor's air induced velocity is

is developed. The airflow rate,

Thus,

28 Robot Control

*vw*,

And finding a relationship between *v*1 and *v*2 in accordance with **Figure 3** (left side),

1 1 11 *T Av* = 2r

1

*<sup>T</sup> <sup>v</sup>* r

2

1

1 1 2

Similarly, modelling both the induced force *T*2 and velocity *v*2 for rotor 2, the following analysis

And the induced force *T*2 considers the inflow air mass and the far-field wake-induced velocity

The relationship between far-field wake-induced air velocity *vw* and *v*<sup>3</sup> is given by *vw*=2*v*3, and

2

2 2 2

3

*<sup>T</sup> <sup>v</sup>* r

2 2 23 *T Av* = 2r

2

2 1 *v v* <sup>=</sup> <sup>2</sup> (40)

(41)

*<sup>A</sup>* <sup>=</sup> (42)

(46)

*<sup>A</sup>* <sup>=</sup> (47)

$$T = T\_1 + \alpha\_2 T\_2 \tag{52}$$

Let us substitute our *T*1 and *T*2 models previously analysed,

$$T = 2\rho\_1 A\_1 \mathbf{v}\_1^2 + 2\rho\_2 A\_2 \mathbf{v}\_2^2 \tag{53}$$

In addition, *A*1=*A*2, *v*2=2*v*1 and *vw*=2*v*3, but *v*2>*v*3 hence *v*3=α2*v*2. Thus, *vw*=2(α2 2(2*v*1)), and we obtain the relationship between the far-field wake-induced velocity and *v*1:

$$
\omega\_w = 6\alpha \omega\_2 \nu\_1 \tag{54}
$$

By substituting *v*1 into expression *T*, developing and factorising algebraically,

$$T = 2A\nu\_1^2 \left(\rho\_1 + 4\alpha\_2\rho\_2\right) \tag{55}$$

Similarly, for the case when *v*1<*v*3, *T*=*T*1(1+α2),

$$T = \mathcal{D}\rho\_1 A\_1 \mathbf{v}\_1^2 + \mathcal{D}\rho\_2 A\_2 \mathbf{v}\_2^2 \tag{56}$$

for this case, *A*1=*A*2, *v*2=2*v*1 and *vw*=2*v*3. However, just above and below rotor 2, *v*2<*v*3, and therefore *v*2=α2*v*3, then *v*3=2*v*2/α2.

$$T = 2\rho\_1 A \nu\_1^2 + 2\rho\_2 A \left(2\frac{\nu\_1}{a\_2}\right)^2\tag{57}$$

Factorizing and algebraically arranging,

$$T = 2A\nu\_1^2 \left(\rho\_1 + \frac{8\rho\_2}{a\_2^2}\right) \tag{58}$$

Solving the parameter *α*2 for each of the three cases,

$$\alpha\_2^2 = \begin{cases} 4, & v\_1 = v\_3 \\ 1, & v\_1 > v\_3 \\ \sqrt{2}, & v1 < v\_3 \end{cases} \tag{59}$$

Therefore, we synthesise the total thrusting force *T* for all cases by

$$T = 2A\nu\_1^2 \left(\rho\_1 + \alpha\_2^2 \rho\_2\right) \tag{60}$$

Therefore, from Eq. (60), now we have a functional form for the thrusting force *T*. Thus, to reach a controlled rotors' velocity, we must establish a relationship between the induced ve‐ locity *v*1 and the rotor angular velocity *dϕ*/*dt* using the tip speed of the rotor blade as refer‐ ence. The rotor inflow is represented in non-dimensional form as

$$
\lambda = \frac{\upsilon\_3}{\not\!\!\!\!\rho\!R} = \sqrt{\frac{C\_T}{2}} \tag{61}
$$

where *C*T is the thrust coefficient modelled by

$$\mathcal{L}\_T = \frac{T}{\rho\_2 A \left(\dot{\phi}\_t R\right)^2} \tag{62}$$

#### Induced Force Hovering of Spherical Robot by Under-Actuated Control of Dual Rotor http://dx.doi.org/10.5772/63548 31

Therefore, the following equality allow us to deduce *CT* with more

$$\dot{\phi} = \frac{\upsilon\_1}{R\sqrt{\frac{C\_T}{Z}}}\tag{63}$$

Thus, substituting (Texto) into *v*1,

2 2

(56)

è ø (58)

(60)

(57)

(59)

(61)

(62)

 r

for this case, *A*1=*A*2, *v*2=2*v*1 and *vw*=2*v*3. However, just above and below rotor 2, *v*2<*v*3, and

2 1

2 2 1 1 2 2

æ ö = + ç ÷

( ) 2 2 1 1 22 *T Av* = + 2 r ar

Therefore, from Eq. (60), now we have a functional form for the thrusting force *T*. Thus, to reach a controlled rotors' velocity, we must establish a relationship between the induced ve‐ locity *v*1 and the rotor angular velocity *dϕ*/*dt* using the tip speed of the rotor blade as refer‐

r

r

a

 r

= + ç ÷

11 2

<sup>8</sup> *T Av* <sup>2</sup>

2 22 *<sup>v</sup> T Av A*

r

2

2

a

è ø

æ ö

1 11 2 2 3 *T Av Av* = + 2 2 r

therefore *v*2=α2*v*3, then *v*3=2*v*2/α2.

30 Robot Control

Factorizing and algebraically arranging,

Solving the parameter *α*2 for each of the three cases,

Therefore, we synthesise the total thrusting force *T* for all cases by

ence. The rotor inflow is represented in non-dimensional form as

where *C*T is the thrust coefficient modelled by

$$w\_1 = \left(\not{\phi}R\sqrt{\frac{C\_T}{Z}}\right)^2\tag{64}$$

And subsequently, *v*<sup>1</sup> 2 is substituted into *T*,

$$T = A\{\phi^2 R^2 C\_T\}(\rho\_1 + \alpha\_2^2 \rho\_2) \tag{65}$$

Therefore, the induced force *T* arising from the fluid mechanics equation (65) is equated with the induced force of the flight mechanics equation (27), the following expression is ob‐ tained:

$$2A\left(\dot{\phi}^2 R^2 \frac{\mathcal{L}\_T}{2}\right) \left(\rho\_1 + a\_2^2 \rho\_2\right) = \mathcal{W}^{-1} m \ddot{p} \tag{66}$$

Our objective is to find an analytical solution for the rotor speed required to reach the in‐ duced velocity *v*1 and the induced force *T*, which is governed by the flight mechanics law. The induced air mass velocity *v*<sup>1</sup> can be expressed in terms of the rotor speed that is control‐ led to obtain the desired angular velocity,

$$\dot{\phi} = \sqrt{\frac{m\nu^{-1}\ddot{p}}{AR^2C\_T(\rho\_1 + a\_2^2\rho\_2)}}\tag{67}$$

This model represents the independent variable to control the motors' Speed of Eq. (73) or Eq. (74):

Thus, it follows a set of empirical temperature measurements where some experimental hovering experiments were carried out. The plots in **Figure 4** depict how the air pressure is affected as the temperature varies over time.

**Figure 4.** Left: Thrusting force w.r.t. induced velocity. Right: Air mass density as temperature varies.

#### **6. Actuators' speed model**

The actuators' self-calibration speed model is discussed in this section. The real rotary velocity in a range from minimal to maximal values approached a logarithmic model. It considered the empirical set of angular speed measurements w.r.t. digital controls. The inherent physical variations, such as temperature, air pressure, density and air dust particles, affected the actuators' performance. Since the angular speed value capable to hover the spherical robot's body is disturbed, a self-calibration is required to maintain position control as accurate as possible. From experiments, the empirical models that obtained (**Figure 5**) φ vs. *d* are fitted according to the next model. The parameters *A* and β are unknown and must fit the speed measurements φ, w.r.t to digital word *d*,

$$
\dot{\phi} = A + \beta \ln(d) \tag{68}
$$

We temporally substitute *d*'=ln(d), and thus the rotary speed

$$
\phi = A = \beta d'\tag{69}
$$

To estimate the unknown parameter *β*, a linear mean-squared method is applied,

$$\beta = \frac{n\left(\sum d'\dot{\phi} - \langle \Sigma \, d'\rangle \langle \Sigma \, \dot{\phi} \rangle\right)}{n(\Sigma \, d'^2) - (\Sigma \, d')^2} \tag{70}$$

Subsequently, the previous parameter solution is used in the next expression *A*,

Induced Force Hovering of Spherical Robot by Under-Actuated Control of Dual Rotor http://dx.doi.org/10.5772/63548 33

**Figure 5.** Actuator's raw measurements. Left: Direct solution. Right: Inverse solution.

$$A = \frac{\Sigma \cdot \phi}{n} - \beta \frac{\Sigma \cdot d'}{n} \tag{71}$$

By substituting the parameters numerical values, the rotary speed model is obtained as follows:

$$
\dot{\phi} = 10838.57637 \ln(d) - 46776.63059 \tag{72}
$$

In addition, in order to obtain the inverse solution, we algebraically drop off the variable *d* from Eq. (72),

$$d = e^{\frac{\phi - \mathcal{A}}{\beta}}\tag{73}$$

And numerical parametric values from Eq. (72) are substituted into (73) to obtain

$$d = e^{\frac{\dot{\phi} - 46776.6306}{10838.5764}}\tag{74}$$

In order to compare how our theoretical model fits the empirical model, both inverse and direct operation control modes are depicted in **Figure 5**.

#### **7. Rotor's speed control**

(68)

(69)

(70)

**Figure 4.** Left: Thrusting force w.r.t. induced velocity. Right: Air mass density as temperature varies.

The actuators' self-calibration speed model is discussed in this section. The real rotary velocity in a range from minimal to maximal values approached a logarithmic model. It considered the empirical set of angular speed measurements w.r.t. digital controls. The inherent physical variations, such as temperature, air pressure, density and air dust particles, affected the actuators' performance. Since the angular speed value capable to hover the spherical robot's body is disturbed, a self-calibration is required to maintain position control as accurate as possible. From experiments, the empirical models that obtained (**Figure 5**) φ vs. *d* are fitted according to the next model. The parameters *A* and β are unknown and must fit the speed

**6. Actuators' speed model**

32 Robot Control

measurements φ, w.r.t to digital word *d*,

We temporally substitute *d*'=ln(d), and thus the rotary speed

To estimate the unknown parameter *β*, a linear mean-squared method is applied,

Subsequently, the previous parameter solution is used in the next expression *A*,

From the previous section, the actuators' speed model is now used to formulate a feedback linear control. Let us assume that a rotor control variable (i.e., angle, velocity and acceleration) should ideally be equal to the real sensed control variable, as expressed by the next equality (75). Nevertheless, in a realistic scenario real and ideal control variables are different due to a number of factors, such as frictions, inertial and gravity forces, motor electromagnetic performance and so on. Thus, both variables are approached by a multiplicative gain or factor alpha, which approximates both numerical values according to the relation

$$
\varphi = k \clubsuit \tag{75}
$$

Assuming an arbitrary actual derivative order, the equation is equivalently expressed as

$$\frac{d}{dt}\,\varphi = k \frac{d}{dt}\,\phi\tag{76}$$

The time differentials are eliminated and the remaining differentials are obtained by solving the following definite integrals,

$$\int\_{\varphi\_1}^{\varphi\_2} d\,\varphi = k \int\_{\varphi\_1}^{\varphi\_2} d\,\phi\tag{77}$$

By solving the definite integrals,

$$
\varphi\_2 - \varphi\_1 = k(\phi\_2 - \phi\_1) \tag{78}
$$

In this case, *φ* ^ <sup>2</sup> is the expected or the reference value to be reached ideally, *φ* ^ <sup>2</sup> <sup>=</sup>*<sup>φ</sup>* ref. Thus, by adjusting the times sub-interval labels, and algebraically reordering, the next recursive numerical successive approximation equation is expressed as follows:

$$
\varphi\_{t+1} = \varphi\_t + k \{ \varphi^{ref} - \phi\_t \} = \varphi\_t + ke\_t \tag{79}
$$

From the previous expression, the error *ket* is spanned into the past (angular displacement), the actual (rotary speed) and the future (angular accelerations) errors in order to cover the whole error history. And the general constant gain *k* is proportional to *kp*, *k*I and *kd*. Thus,

$$
\sigma\_{\iota+1} = \sigma\_{\iota} + \left(k\_{\rho}\mathbf{e}\_{\rho} + k\_{\iota}\mathbf{e}\_{\iota} + k\_{d}\mathbf{e}\_{d}\right) \tag{80}
$$

Therefore, the next feedback proportional error *ep* (rad/s) with proportional gain *kp* (dimen‐ sionless) is obtained with the observation ϕ*<sup>t</sup>* measured online. And the reference model (Texto) is established in terms of the instantaneous control word *δ<sup>t</sup>* :

$$k\_p e\_p = k\_p \{ \dot{\phi}^{ref} - \dot{\phi}\_t \} = k\_p \{ aln(\delta) - b - \dot{\phi}\_t \} \tag{81}$$

Induced Force Hovering of Spherical Robot by Under-Actuated Control of Dual Rotor http://dx.doi.org/10.5772/63548 35

**Figure 6.** Left: Measurement and reference proportional models. Right: Proportional error.

For illustrative purpose, an accelerative rotor's task to exert robot's propulsion was performed. **Figure 6** (left) depicts both the reference model ϕref and the observation ϕ(*t*).

In addition, **Figure 6** (right) shows the proportional error behaviour ϕref-ϕ(*t*) without *kp*. Furthermore, the feedback integral error *e*I (rad/s) with integral gain *k*I (dimensionless) is expressed by the time integration of the difference of (*d*<sup>2</sup> ϕ/*dt*<sup>2</sup> – *d*<sup>2</sup> ϕ(*t*)/*dt*<sup>2</sup> ).

$$k\_I e\_I = k\_I \left( \int \left( \ddot{\phi}^{ref} - \ddot{\phi}\_t \right) dt \right) \tag{82}$$

The observation model *d*<sup>2</sup> ϕ/*dt*<sup>2</sup> was obtained online by the numerical derivatives of the optical encoder according to the following relationship:

$$
\ddot{\phi}(t) = \frac{\dot{\phi}\_{t\_2} - \dot{\phi}\_{t\_1}}{t\_2 - t\_1} \tag{83}
$$

In addition, since the observation model inherently poses perturbations, an analytical reference model *d*<sup>2</sup> ϕref/*dt*<sup>2</sup> was obtained using a nonlinear regressive fitting process for parameters identification,

$$
\begin{pmatrix}
\sum \ddot{\phi}\_{i} \\
\sum \ddot{\phi}\_{i} \\
\sum \ddot{\phi}\_{i} \, t\_{i}^{2}
\end{pmatrix} = \begin{pmatrix}
n & \sum t\_{i} & \sum t\_{i}^{2} \\
\sum t\_{i} & \sum t\_{i}^{2} & \sum t\_{i}^{3} \\
\sum t\_{i}^{2} & \sum t\_{i}^{3} & \sum t\_{i}^{4}
\end{pmatrix} \cdot \begin{pmatrix} a\_{0} \\ a\_{1} \\ a\_{2} \end{pmatrix} \tag{84}
$$

where the previous expression is similarly expressed as

$$\mathbf{y} = \mathbf{A}^{\top}\mathbf{x}\tag{85}$$

And by solving vector **x**,

number of factors, such as frictions, inertial and gravity forces, motor electromagnetic performance and so on. Thus, both variables are approached by a multiplicative gain or factor

Assuming an arbitrary actual derivative order, the equation is equivalently expressed as

The time differentials are eliminated and the remaining differentials are obtained by solving

<sup>2</sup> is the expected or the reference value to be reached ideally, *φ*

numerical successive approximation equation is expressed as follows:

j

is established in terms of the instantaneous control word *δ<sup>t</sup>*

 j

adjusting the times sub-interval labels, and algebraically reordering, the next recursive

the actual (rotary speed) and the future (angular accelerations) errors in order to cover the whole error history. And the general constant gain *k* is proportional to *kp*, *k*I and *kd*. Thus,

Therefore, the next feedback proportional error *ep* (rad/s) with proportional gain *kp* (dimen‐

(75)

(76)

(77)

(78)

(79)

(81)

<sup>2</sup> <sup>=</sup>*<sup>φ</sup>* ref. Thus, by

^

is spanned into the past (angular displacement),

measured online. And the reference model (Texto)

*t t pp II dd* <sup>+</sup><sup>1</sup> =+ + + (*ke ke ke* ) (80)

:

alpha, which approximates both numerical values according to the relation

the following definite integrals,

By solving the definite integrals,

From the previous expression, the error *ket*

sionless) is obtained with the observation ϕ*<sup>t</sup>*

^

In this case, *φ*

34 Robot Control

$$\mathbf{x} = \mathbf{A}^{-1} \begin{array}{c} \mathbf{\color{red}{a}} \\ \mathbf{\color{red}{y}} \end{array} \tag{86}$$

Hence, the reference model is a theoretical nonlinear function of time,

$$
\bar{\phi}^{ref}(t) = 34.7950 - 1.2832t + 0.0145t^2 \tag{87}
$$

Therefore, for the sake of the integral control *u*I (rad/s), the angular acceleration reference model (Texto) is substituted next in its general form:

$$u\_I = k\_I \left( \int (a\_0 - a\_{1t} + a\_2 t^2) - \ddot{\phi}(t) dt \right) \tag{88}$$

Finally, in order to keep data homogeneity (numerical data subtraction), time integration is obtained by the trapezoid rule for numerical integration,

$$\begin{split} \phi\_{\kappa} = \frac{t\_f - t\_0}{2n} \Big[ \left( \ddot{\phi}\_0^{ref} - \ddot{\phi}\_0(t) \right) + 2 \sum \left( \ddot{\phi}\_t^{ref} - \ddot{\phi}\_t \right) \\ + \left( \ddot{\phi}\_{t\_f}^{ref}(t) - \ddot{\phi}\_{t\_f}(t) \right) \Big] \end{split} \tag{89}$$

**Figure 7.** Left: Measurement and reference integral models. Right: Integral error.

**Figure 7** (left) depicts both reference and empirical models integrated with time. **Figure 7** (right) shows the integral error behaviour.

In addition, the derivative control *ud*=*kded* (rad/s) with feedback derivative error *ed*, and with derivative gain *kd* (dimensionless), improves the closed-loop stability as follows:

$$k\_d \mathbf{e}\_d = k\_d \left(\frac{d}{dt} \phi^{\rm ref} - \frac{d}{dt} \phi\_t\right) \tag{90}$$

In order to obtain the time derivative observation model, the rotor's angle evolution ϕ(*t*) (rad) is observed online using an optical encoder during the time slot where velocity and acceleration are also measured. As the measurements are read with noise, the analytical reference model is fitted as a nonlinear polynomial of the following form:

$$
\phi^{\rm ref} \left( t \right) = -3709.6273 + 2116.9484t + 27.9289t^2 \tag{91}
$$

where the derivative error general form is expressed as

(86)

(87)

(88)

(89)

Hence, the reference model is a theoretical nonlinear function of time,

model (Texto) is substituted next in its general form:

36 Robot Control

obtained by the trapezoid rule for numerical integration,

**Figure 7.** Left: Measurement and reference integral models. Right: Integral error.

(right) shows the integral error behaviour.

Therefore, for the sake of the integral control *u*I (rad/s), the angular acceleration reference

Finally, in order to keep data homogeneity (numerical data subtraction), time integration is

**Figure 7** (left) depicts both reference and empirical models integrated with time. **Figure 7**

In addition, the derivative control *ud*=*kded* (rad/s) with feedback derivative error *ed*, and with

ref *dd d t d d ke k*

æ ö = - ç ÷

*dt dt* f

 f

è ø (90)

derivative gain *kd* (dimensionless), improves the closed-loop stability as follows:

$$k\_d \mathbf{e}\_d = k\_d \frac{d}{dt} \left( -b\_0 + b\_{1l} + b\_2 t^2 - \phi(t) \right) \tag{92}$$

**Figure 8** (left) depicts both reference ϕref(*t*) and observation ϕ(*t*) models together. Although both curves are apparently fitted, the vertical scale is provided in thousands of radians. **Figure 8** (right) shows the derivative error scale.

**Figure 8.** Left: Measurement and reference derivative models. Right: Derivative error.

Generally, the PID controller is expressed by the next expression

$$\begin{split} u\_t &= k\_p \left( \left( a \ln(d) - b \right) - \dot{\phi}\_t \right) + k\_l \left( \int\_t \left( a\_0 - a\_{1t} + a\_{2t}^2 - \ddot{\phi}\_t \right) dt \right) \\ &+ k\_d \left( \frac{d}{dt} \left( -b\_0 + b\_{1t} + b\_{2t}^2 - \phi(t) \right) \right) \end{split} \tag{93}$$

Therefore, the controlled rotor's velocity that is recursively calculated by *dφ*t+1/*dt*= *d*φ<sup>t</sup> /*dt* + *ut* is applied, and we established the following controller choices: proportional (*P*), proportional integral (PI) and proportional-integral-derivative. **Figure 9** (left) depicts the rotors' angular speed without control and with three types of controllers. The constant parameters were adjusted accordingly to obtain such results. We can see that after 25 s the responses P and PI gradually converge w.r.t. the raw rotor's speed (**Figure 9**, left).

**Figure 9.** Controllers P, PI and PID. Left: Rotor's angular speed. Right: Induced Cartesian forces.

In addition, with the controlled rotor's speed output, the induced force is iteratively calculated by

$$T(\phi\_{t+1}) = A\phi\_{t+1}^2 R^2 \mathcal{C}\_T (\rho\_1 + a\_2^2 \rho\_2) \tag{94}$$

Thus, **Figure 9** (right) depicts the induced component forces that are produced using three types of controllers.

#### **8. Conclusions**

This work briefly introduced the design of an aerial spherical robot with under-actuated gyroscopic mechanism. Although the purpose of this study was not to describe the robot flying and physical capabilities, the main objective was to demonstrate the induced force model deploying dual collinear rotary wings with no steering actuators. This study describes the following four major areas: the robot flight mechanics, the model of the induced thrusting forces, the self-calibration actuators' Speed model validated with three types of controllers (P, PI, PID) to drive the rotors' motor speed. The proposed aerodynamic mechanism poses neither ailerons nor propellers for steering control. Although the platform is a home-made laboratory prototype with special arrangements, the main focus of this chapter is to model the hypothesis of controlling the robot's directions by varying rotors' asynchronous speed. To achieve this, the under-actuated gyroscopic mechanism provides the ability to control its inner yaw, pitch

and roll angles. Until this stage, the robot development is under an early control capability. However, this study presents mathematical solutions and simulation results to demonstrate the proposed aerodynamic hypothesis.

#### **Author details**

integral (PI) and proportional-integral-derivative. **Figure 9** (left) depicts the rotors' angular speed without control and with three types of controllers. The constant parameters were adjusted accordingly to obtain such results. We can see that after 25 s the responses P and PI

gradually converge w.r.t. the raw rotor's speed (**Figure 9**, left).

**Figure 9.** Controllers P, PI and PID. Left: Rotor's angular speed. Right: Induced Cartesian forces.

by

38 Robot Control

types of controllers.

**8. Conclusions**

In addition, with the controlled rotor's speed output, the induced force is iteratively calculated

Thus, **Figure 9** (right) depicts the induced component forces that are produced using three

This work briefly introduced the design of an aerial spherical robot with under-actuated gyroscopic mechanism. Although the purpose of this study was not to describe the robot flying and physical capabilities, the main objective was to demonstrate the induced force model deploying dual collinear rotary wings with no steering actuators. This study describes the following four major areas: the robot flight mechanics, the model of the induced thrusting forces, the self-calibration actuators' Speed model validated with three types of controllers (P, PI, PID) to drive the rotors' motor speed. The proposed aerodynamic mechanism poses neither ailerons nor propellers for steering control. Although the platform is a home-made laboratory prototype with special arrangements, the main focus of this chapter is to model the hypothesis of controlling the robot's directions by varying rotors' asynchronous speed. To achieve this, the under-actuated gyroscopic mechanism provides the ability to control its inner yaw, pitch

G. Santos-Medina, K.Y. Heras-Gaytán, E.A. Martínez-García\* , R. Torres-Córdoba and V. Carrillo-Saucedo

\*Address all correspondence to: edmartin@uacj.mx

Laboratorio de Robótica, Institute of Engineering and Technology, Universidad Autónoma de Ciudad Juárez Ave. Del Charro, Cd. Juarez, Chihuahua, Mexico

#### **References**

(94)


validation. International Conference on Unmanned Aircraft Systems, USA, pp. 1102– 1111, 2015.


#### **Adaptive Steering and Trajectory Control of Wheeled Mobile Robots for Autonomous Navigation Adaptive Steering and Trajectory Control of Wheeled Mobile Robots for Autonomous Navigation**

Mariam Al-Sagban and Rached Dhaouadi Mariam Al-Sagban and Rached Dhaouadi

Additional information is available at the end of the chapter Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/64227

#### **Abstract**

validation. International Conference on Unmanned Aircraft Systems, USA, pp. 1102–

[11] Kawasaki K, Zhao M, Okada K, Inaba M. MUWA: multi-field universal wheel for airland vehicle with quad variable-pitch propellers. IEEE/RSJ IROS, Japan, pp. 1880–1885,

[12] Ho-Joon L, Han-Woong A, Jae-Kwang L, Ju L, Sung-Hong W. Newly proposed hybrid type mutli-DOF operation motor for multi-copter UAV systems. IEEE Energy Conver‐

[13] Zhang W, Fan N, Wang Z, Wu Y, Modeling and aerodynamic analysis of a ducted-fan micro aerial vehicle. International Conference on Modelling, Identification & Control,

sion Congress and Exposition, Canada, pp. 2782–2790, 2015.

1111, 2015.

China, pp. 768–773, 2012.

2013.

40 Robot Control

This chapter presents a new reactive navigation algorithm for a wheeled mobile robot (WMR) with a differential drive mechanism moving in unknown environments [1]. The mobile robot is controlled to travel to a predefined goal position safely and efficiently without any prior map of the environment. The navigation is achieved by modulating the steering angle and turning radius. To avoid obstacles while seeking the goal position, the dimensions and shape of the robot are incorporated to determine the set of all possible collision-free steering angles. The algorithm then selects the optimum steering angle candidate to contour the obstacle. Simulation and experimental results on a WMR prototype are used to validate the proposed algorithms.

**Keywords:** recurrent neural networks, obstacle avoidance, robots

#### **1. Introduction**

Over the years, mobile robots have evolved rapidly incorporating a wide spectrum of applications. They aid within the field of medical technologies, assist in vehicle driving, and can be occupied for use within hazardous rescue missions. Mobile robots helped monitor the spread of oil during the catastrophic spill on the Gulf of Mexico [2]. Additionally, robots were of great aid during the Japanese Fukushima nuclear crisis in monitoring the radiation levels and cleaning up leftover debris [3].

In performing all previous tasks, mobile robots must be equipped with autonomous navigation. This is described as the arrival at of the robot at a target location without any assistance, and whilst avoiding obstacles present around. Perception, path planning, localization, and

motion control are the key elements that build toward autonomous navigation. With perception, sensors pick up information regarding the robots immediate environment that are then perceived and translated within the robot. The next step is path planning, which is the robot's ability to come to a consensus regarding the required action as a result of its expected goal. The final step occurs with motion control through which the robot executes the required action through its actuators [4].

In this chapter, we focus on path planning in unknown environments [5]. Path planning represents a key feature of autonomous navigation. The problem of path planning is usually classified in three categories according to the given environment and constraints:


Due to the inherent nature of the obstacle avoidance problem, navigation algorithms do not produce efficient paths and do not guarantee global convergence as would global path planning algorithms. This would result the robot in producing inefficient paths or failing to reach the goal position (trap position).

The implementation of path planning and obstacle avoidance techniques on a real mobile robot imposes different types of constraints including kinematic, dynamic, and time constraints. The differential drive robot must follow a curve path due to the kinematic constraint as it is unable to reach the desired point place instantly and in a short period of time. Disregarding this limitation, when dealing with path planning and obstacle avoidance techniques, would lead to an unsafe design as the transitional curve may potentially intersect with obstacles and, therefore, result in a collision.

#### **2. Autonomous navigation concepts**

#### **2.1. The configuration space**

For a two-dimensional robot, the robot configuration can be fully described by rigidly attaching a frame to the robot and then specifying the position and orientation of this frame in the global frame. The complete specification of the location of every point on the robot is called a configuration, *q*. The set of all possible configurations is called a configuration space or Cspace (). A rigid object moving in a plane is, therefore, specified by the triple configuration, *q* = (*x*, *y*, *θ*), and the configuration space can be represented by = ℝ<sup>2</sup> × <sup>2</sup> , where *SO*(2) is the special orthogonal group of 2-D rotations [6].

The introduction of new notation is important for the description of collisions. The workspace in which the robot moves will be denoted as . When the robot moves in a plane, the workspace is now denoted as *W* = ℝ<sup>2</sup> . The subset of this workspace occupied by obstacles is then denoted as , and the subset occupied by the robot at configuration is denoted as . The robot must avoid a configuration causing it to come into physical contact with any obstacle, as this would cause a collision otherwise. The set of configurations in which the robot would come into a collision with an obstacle is defined as the obstacle configuration space,

$$\mathcal{C}\_{\text{obs}} = \{ q \in \mathcal{C} \mid \mathcal{A}(q) \bigcap \mathcal{O} = \emptyset \}. \tag{1}$$

On the other hand, the set of all collision-free configurations is defined as the free configuration space. It is defined as the set difference

$$\mathcal{C}\_{free} = \mathcal{C} \backslash \mathcal{C}\_{obs} \tag{2}$$

The configuration space of a rigid robot translating in the plane = ℝ2 is two-dimensional, easily visualized in **Figure 1**. The circular-shape robot is presented with an obstacle in the workspace. When sliding the robot around the obstacle, the boundary configuration can be determined. As a result, motion planning for the robot in the workspace is converted to motion planning for a point robot in the configuration space [7].

**Figure 1.** Construction of the configuration space.

motion control are the key elements that build toward autonomous navigation. With perception, sensors pick up information regarding the robots immediate environment that are then perceived and translated within the robot. The next step is path planning, which is the robot's ability to come to a consensus regarding the required action as a result of its expected goal. The final step occurs with motion control through which the robot executes the required action

In this chapter, we focus on path planning in unknown environments [5]. Path planning represents a key feature of autonomous navigation. The problem of path planning is usually

**•** Global Path Planning—This framework requires full knowledge of the robot workspace; a

**•** Local Path Planning (Sensor-Based Path Planning)—This framework requires partial

**•** Reactive Navigation (Obstacle Avoidance)—In this framework, no a priori information is required about the workspace. Instead, obstacles are discovered in real time while the robot

Due to the inherent nature of the obstacle avoidance problem, navigation algorithms do not produce efficient paths and do not guarantee global convergence as would global path planning algorithms. This would result the robot in producing inefficient paths or failing to

The implementation of path planning and obstacle avoidance techniques on a real mobile robot imposes different types of constraints including kinematic, dynamic, and time constraints. The differential drive robot must follow a curve path due to the kinematic constraint as it is unable to reach the desired point place instantly and in a short period of time. Disregarding this limitation, when dealing with path planning and obstacle avoidance techniques, would lead to an unsafe design as the transitional curve may potentially intersect with obstacles and,

For a two-dimensional robot, the robot configuration can be fully described by rigidly attaching a frame to the robot and then specifying the position and orientation of this frame in the global frame. The complete specification of the location of every point on the robot is called a configuration, *q*. The set of all possible configurations is called a configuration space or Cspace (). A rigid object moving in a plane is, therefore, specified by the triple configuration, *q* = (*x*, *y*, *θ*), and the configuration space can be represented by = ℝ2 × <sup>2</sup> , where *SO*(2) is

classified in three categories according to the given environment and constraints:

knowledge of the workspace. Therefore, only an incomplete map is supplied.

through its actuators [4].

42 Robot Control

is executing its motion.

therefore, result in a collision.

**2.1. The configuration space**

**2. Autonomous navigation concepts**

the special orthogonal group of 2-D rotations [6].

reach the goal position (trap position).

global map is supplied as given input.

#### **2.2. Definition of obstacle avoidance**

Let *q*target be a target configuration. At time *ti* the robot is in configuration *q*(*ti* ). The robot senses a portion of the environment using its onboard sensors. Let the set of workspace obstacles seen at configuration *q*(*ti* ) be ⊂ . The objective is to compute a motion control vector *ui* such that

	- of configurations of the trajectory followed from *q*(*ti* ) to *q*(*ti* + *T*). *T* > 0 is the sampling period.

The solution of the problem is a sequence of control vectors {*u*1, …, *un*} computed in real time that guide the robot eventually to the target configuration while avoiding the sensed obstacles in the environment as shown in **Figure 2**.

**Figure 2.** Obstacle avoidance problem [8].

#### **2.3. Kinematics of a two-wheel differential drive robot**

A differential drive robot is composed of one passive wheel and two coaxial wheels. The passive wheel provides stability, while the coaxial pair steer the robot through carefully modulating their velocities. A straight line motion is achieved through equal velocities in both wheels, while left and right motion occurs if the right wheel is faster than the left and the left wheel is faster than the right, respectively. Pivoting is noticed when both wheels steer equally as fast, but in opposite directions. A zero turning radius is a major advantage with this motion configuration. An initial rotation can trigger motion in any direction. Further advantages to this robot configuration include the simple mechanical structure and kinematic model and the low fabrication cost. However, this robot configuration has also a few drawbacks: the wheels must be driven with exactly the same velocity profile, which can be challenging considering the actual variations between wheels, motors, and environmental differences. It is also difficult for the robot to move on irregular surfaces. Moreover, the orientation of the robot may change abruptly if one active wheel loses contact with the ground [9].

There are two types of nonholonomic constraints governing the motion of the robot platform: Pure rolling constraint and no lateral slip constraint [10]. The pure rolling constraint implies that the robot wheels have a pure rolling motion without any slipping. This constraint is described by the following equations

$$
\dot{x}\cos\theta + \dot{y}\sin\theta + L\dot{\theta} = \alpha\_r R\_{w'} \tag{3}
$$

$$
\dot{\mathbf{x}}\cos\theta + \dot{\mathbf{y}}\sin\theta - \mathbf{L}\dot{\theta} = a\rho\mathbf{R}\_w.\tag{4}
$$

The no lateral slip constraint implies that the robot's center point velocity is only in the direction of the axis of symmetry and its lateral component is zero. It is given by

$$
\dot{y}\cos\theta - \dot{\mathbf{x}}\sin\theta = 0.\tag{5}
$$

Without reference to forces and masses, robot kinematics implies a relationship between the position of the robot and its wheels, velocities, and the equations of motion. This section analyzes the mathematical kinematic relationship related to a differentially driven vehicle. The robot configuration is illustrated in **Figure 3**.

**Figure 3.** Kinematics of a two-wheel robot.

at configuration *q*(*ti*

such that

44 Robot Control

) be

**•** The robot progresses to the target location *F*(*q*(*ti*

of configurations of the trajectory followed from *q*(*ti*

**2.3. Kinematics of a two-wheel differential drive robot**

abruptly if one active wheel loses contact with the ground [9].

in the environment as shown in **Figure 2**.

**Figure 2.** Obstacle avoidance problem [8].

⊂ . The objective is to compute a motion control vector *ui*

) to *q*(*ti*

: × ℝ+ is a function that evaluates the progress of one configuration to another [8].

The solution of the problem is a sequence of control vectors {*u*1, …, *un*} computed in real time that guide the robot eventually to the target configuration while avoiding the sensed obstacles

A differential drive robot is composed of one passive wheel and two coaxial wheels. The passive wheel provides stability, while the coaxial pair steer the robot through carefully modulating their velocities. A straight line motion is achieved through equal velocities in both wheels, while left and right motion occurs if the right wheel is faster than the left and the left wheel is faster than the right, respectively. Pivoting is noticed when both wheels steer equally as fast, but in opposite directions. A zero turning radius is a major advantage with this motion configuration. An initial rotation can trigger motion in any direction. Further advantages to this robot configuration include the simple mechanical structure and kinematic model and the low fabrication cost. However, this robot configuration has also a few drawbacks: the wheels must be driven with exactly the same velocity profile, which can be challenging considering the actual variations between wheels, motors, and environmental differences. It is also difficult for the robot to move on irregular surfaces. Moreover, the orientation of the robot may change

**•** The trajectory does not collide with the obstacles ,where

), *q*target) < *F*(*q*(*ti*

+ *T*), *q*target), where

+ *T*). *T* > 0 is the sampling period.

, is the set

Let the rotational velocities of the left and right wheel be *ωL* and *ωR*, respectively, and *Rw* be the wheel radius then. Then, assuming no wheel slippage, the translational velocities of the wheels are given by

$$
\omega v\_l = \alpha \rho\_l \mathbf{R}\_{w'} \tag{6}
$$

$$
\Delta v\_r = \alpha\_r R\_w.\tag{7}
$$

Let the robot forward velocity in the local frame be *v*, the angular velocity about its Instantaneous Center of Rotation (ICR) axis be *ω*, and let *L* be half the distance between the wheels, as shown in **Figure 4**. Then the forward and angular velocities of the robot can be derived from the wheels velocities as follows:

$$
\begin{bmatrix} \upsilon \\ \upsilon \end{bmatrix} = \begin{bmatrix} \frac{1}{2} & \frac{1}{2} \\ \frac{-1}{2L} & \frac{1}{2L} \end{bmatrix} \begin{bmatrix} \upsilon\_l \\ \upsilon\_r \end{bmatrix}. \tag{8}
$$

**Figure 4.** Instantaneous turning radius.

Let *θ* be the robot orientation with respect to the global *x*-axis, then the robot velocity vector in the global frame is given by

$$
\begin{bmatrix}
\dot{x} \\
\dot{y} \\
\dot{\theta}
\end{bmatrix} = \begin{bmatrix}
\cos\theta & 0 \\
\sin\theta & 0 \\
0 & 1
\end{bmatrix} \begin{bmatrix}
v \\
w
\end{bmatrix}.\tag{9}
$$

**Figure 4** shows the instantaneous turning radius *rc* that can be evaluated by

$$r\_c = L\left(\frac{\upsilon\_r + \upsilon\_l}{\upsilon\_r - \upsilon\_l}\right). \tag{10}$$

#### **2.4. Odometry**

= . *r rw v R* w

*v v w v* -

*L L r*

Let *θ* be the robot orientation with respect to the global *x*-axis, then the robot velocity vector

*v*

*w*

(9)

(10)

cos 0 sin 0 . 0 1

éù é ù êú ê úé ù êú ê ú = ê ú ë û êú ê ú ëû ë û

q

= . *r l*

*v v r L v v* æ ö <sup>+</sup> ç ÷ - è ø

*r l*

q

*x*

& & &

*y*

q

**Figure 4** shows the instantaneous turning radius *rc* that can be evaluated by

*c*

. *l*

the wheels velocities as follows:

46 Robot Control

**Figure 4.** Instantaneous turning radius.

in the global frame is given by

Let the robot forward velocity in the local frame be *v*, the angular velocity about its Instantaneous Center of Rotation (ICR) axis be *ω*, and let *L* be half the distance between the wheels, as shown in **Figure 4**. Then the forward and angular velocities of the robot can be derived from

(7)

é ù é ùé ù ê ú <sup>=</sup> ê úê ú ë û ê ú ë ûë û (8)

A robot's global frame position is measured via the dead reckoning method. This method integrates incremental movements measured through wheel encoders and a compass to estimate position, given a known initial start location. The compass delivers the robot's orientation, *θ*. The incremental movements are measured through wheel encoders and a compass. The robot orientation is *θ*, while the angular velocities of the left and right wheels *ωl* and *ωr*, respectively, estimate the two-dimensional position (*x*, *y*) via encoders. Encoders utilize encoder pulses to deliver accurate arrival times through the measurement of angular velocities. For encoder resolution, *p*, and elapsed time, *Δt*, the angular velocities of the wheels is then defined as

$$
\rho \alpha\_{r,l} = \frac{2\pi}{p\Delta t}.\tag{11}
$$

Next, the robot kinematic Eqs (6–9) are used to find the robot velocities and . Let *T* denote a fixed sampling time. Then, the robot position (*x*, *y*) in the global frame is found by performing trapezoidal integration

$$\mathbf{x} = \mathbf{x}\_{\text{old}} + \frac{T}{2} (\dot{\mathbf{x}} + \dot{\mathbf{x}}\_{\text{old}}) \,\tag{12}$$

$$
\dot{y} = y\_{\text{old}} + \frac{T}{2} (\dot{y} + \dot{y}\_{\text{old}}).\tag{13}
$$

Since the position estimation involves a numerical integration of the measurements, there will be an error accumulation over time. As a result, a meaningful estimate of the position cannot be attained with the dead reckoning method.

Systematic and nonsystematic errors are usually encountered with the dead reckoning method. Systematic errors arise due to the misalignment and the unequal diameter of both wheels, while nonsystematic errors may occur as a result of wheel slippage incidents or nonhomogenous environment with uneven floors.

#### **3. Collision avoidance algorithm**

In this section, the proposed collision avoidance algorithm is developed using the following parameters. The configuration *q*(*ti* ) denotes the position and orientation of the robot in the global frame, while represents only the sensed portion of the environment. This measured portion of the environment is used to construct the robot's workspace polar map, which allows the set of obstacles to be identified and the configuration space to be computed. Next, the operation of the robot is performed in the C-space to simplify the motion planning and navigation. The motion of the two-dimensional robot in the global frame can be simplified to that of a one point (robot reference point) in the C-space. The optimum steering angle *γ*desired is selected by identifying all the workspace obstacles and classifying the available gaps that can be accessed by the robot. The nonholonomic constraints are taken into account by computing the required radius of curvature *rc* such that , which is the set of configura-

tions of the trajectory followed from *q*(*ti* ) to *q*(*ti* + *T*) does not intersect with any obstacle, . This is achieved by restricting the radius of curvature to an adaptive upper bound. Finally, the robot executes the control action *ui* = (*γ*desired, *rc*). The process is repeated until the robot converges to the target position *q*target. The algorithm is pictorially illustrated in **Figure 5**.

**Figure 5.** Reactive navigation algorithm in action.

#### **3.1. Identification of reference steering angle**

which allows the set of obstacles

tions of the trajectory followed from *q*(*ti*

**Figure 5.** Reactive navigation algorithm in action.

illustrated in **Figure 5**.

48 Robot Control

by computing the required radius of curvature *rc* such that

upper bound. Finally, the robot executes the control action *ui*

to be identified and the configuration space to be

, which is the set of configura-

= (*γ*desired, *rc*). The process is

+ *T*) does not intersect with any obstacle,

. This is achieved by restricting the radius of curvature to an adaptive

computed. Next, the operation of the robot is performed in the C-space to simplify the motion planning and navigation. The motion of the two-dimensional robot in the global frame can be simplified to that of a one point (robot reference point) in the C-space. The optimum steering angle *γ*desired is selected by identifying all the workspace obstacles and classifying the available gaps that can be accessed by the robot. The nonholonomic constraints are taken into account

) to *q*(*ti*

repeated until the robot converges to the target position *q*target. The algorithm is pictorially

The steering angle with which the robot takes in the absence of obstacles is referred to as the reference steering angle, *γ*ref. It is an intermediate variable that will later help us find the desired steering angle *γ*desired, which is derived as follows. Let the robot configuration shown in **Figure 6** be:

$$q\_r = (\mathbf{x}\_{r'} y\_{r'} \theta\_r)\_r \tag{14}$$

**Figure 6.** The robot's trajectory to *q*target in the absence of obstacles.

where (*xr*, *yr*) is the position of the robot in the *x* − *y* plane, and *θr* ∈ [0, 2*π*) is the robot's orientation with respect to the *x*-axis.

Let the target configuration be

$$\boldsymbol{q}\_{\text{target}} = (\boldsymbol{\chi}\_{\text{target}}, \boldsymbol{y}\_{\text{target}}, \boldsymbol{\theta}\_{\text{target}}).\tag{15}$$

Let be the vector connecting the robot reference point to the target location. The phase angle of is given by

$$\alpha = \arctan \frac{y\_{\text{target}} - y\_r}{x\_{\text{target}} - x\_r}. \tag{16}$$

Orientation error is corrected for by turning the robot in an angle defined as follows: *γref* = *α* − *θr* , − *π* ≤ *γref* ≤ *π*. The range of *γref* is chosen in such a way that the smaller turning angle is selected. The robot can turn clockwise (right) or counter clockwise (left). **Figure 6** illustrates the trajectory taken by the robot to move to the target configuration. The robot finally achieves a straight line path once its *x*-axis is on line with the error vector.

#### **3.2. Model of robot environment**

The robot environment is modeled by constructing a polar map of the workspace in the local robot frame. At a given instant of time, the distances from the robot to all the surrounding obstacles are measured by a laser range finder and used to build the partial polar map. The laser range sensor is calibrated to scan the 200° front view of the robot in 20 sectors with a 10° angular resolution, as illustrated in **Figure 7**. The measured data is returned as a set of data points:

$$\mathcal{P}(q(t\_i)) = \{p\_{1'}, p\_{2'}, \dots, p\_{|\,^\prime}, \dots, p\_{20}\}. \tag{17}$$

A point *pj* is expressed by a pair (*dj* , *βj* ) where *dj* is the distance between the robot and the obstacle at sector *j*. *βj* is the orientation of the *j* th sector, *Sj* , with respect to the local *x*-axis. The subset of workspace obstacles seen at configuration *q*(*ti* ) is identified by applying a threshold on *dj* ,

$$\mathcal{O}(\mathfrak{q}(t\_i)) = \{ \mathcal{p}\_j \in \mathcal{P}(\mathfrak{q}(t\_i)) \mid d\_j \le R\_{\mathfrak{q}\mathfrak{q}e} \}. \tag{18}$$

**Figure 7.** Polar map of the workspace.

The choice of the threshold *R*safe plays an important role in the obstacle avoidance algorithm. If *R*safe is large, then the obstacle avoidance will start too soon which results in a suboptimal path. Also, by selecting a large *R*safe, the algorithm may fail to detect any gaps in the environment and, therefore, incorrectly report a trap situation. For example, the robot in **Figure 8** successfully detects a gap in the environment with *R*safe1 but fails to do so when using a large value *R*safe2.

**Figure 8.** The effect of using a large value for *R*safe.

the trajectory taken by the robot to move to the target configuration. The robot finally achieves

The robot environment is modeled by constructing a polar map of the workspace in the local robot frame. At a given instant of time, the distances from the robot to all the surrounding obstacles are measured by a laser range finder and used to build the partial polar map. The laser range sensor is calibrated to scan the 200° front view of the robot in 20 sectors with a 10° angular resolution, as illustrated in **Figure 7**. The measured data is returned as a set of data

1 2 <sup>20</sup> ( ( )) = { , ,..., ,..., }. *i j* P *qt p p p p* (17)

( ( )) = { ( ( ))| }. *i j i j safe* O P *qt p qt d R* Î £ (18)

is the distance between the robot and the

, with respect to the local *x*-axis. The

) is identified by applying a threshold

a straight line path once its *x*-axis is on line with the error vector.

, *βj*

is the orientation of the *j*

) where *dj*

th sector, *Sj*

**3.2. Model of robot environment**

is expressed by a pair (*dj*

subset of workspace obstacles seen at configuration *q*(*ti*

points:

50 Robot Control

A point *pj*

on *dj* ,

obstacle at sector *j*. *βj*

**Figure 7.** Polar map of the workspace.

The detection range threshold *R*safe is allowed to take different values depending on the situation encountered:

$$R\_{\text{scale}} = \begin{cases} 0.1 \,\text{m} & \text{if robotis close to target configuration;}\\ 0.5 \,\text{m} & \text{otherwise.} \end{cases} \tag{19}$$

The robot is considered close to the target configuration if:

$$\left(\left(\mathbb{X}\_{\text{target}} - \mathbb{X}\_{r}\right)^{2} + \left(y\_{\text{target}} - y\_{r}\right)^{2} \leq \varepsilon\right) \tag{20}$$

where *ε* is the target threshold.

#### **3.3. Evaluation of configuration space**

The 2D robot with radius *R* computes the *Cobst* for a given set of workspace obstacles, . Assume for a moment that a single obstacle exists, = . As illustrated in **Figure 9**, obst is found through tracing the robot's configuration as it slides around *pj* . Hence, the following relations can be written for Circle *Cj* enclosing obst with Radius *R* and center *Ij* = (*Ij*,*<sup>x</sup>*, *Ij*,*<sup>y</sup>*):

$$\mathcal{L}\_{\text{obst}} = \{ q \in \mathcal{C} \mid (\mathbf{x} - I\_{/,x})^2 + (y - I\_{/,y})^2 \le \mathbb{R}^2 \},\tag{21}$$

$$I\_{\boldsymbol{\beta},\boldsymbol{\alpha}} = \mathcal{R} + d\_{\boldsymbol{\beta}} \cos \beta\_{\boldsymbol{\beta}}.\tag{22}$$

, = sin , 100 100 . *jy j j <sup>j</sup> I d* b b- ££ o o (23)

**Figure 9.** C-space algorithm.

Next, we find the radial distance *Li* which is the radial distance between the robot and the boundary of at angle *βi* . The equation of *Cj* in polar coordinates is

$$\rho\_{\rangle} = \sqrt{I\_{\rangle,x}^2 + I\_{\rangle,y}^2} \qquad \qquad \qquad \phi\_{\rangle} = \operatorname{atan2}(\frac{I\_{\rangle,y}}{I\_{\rangle,x}}) , \tag{24}$$

$$\mathbf{L}\_{\parallel}^{2} + \boldsymbol{\rho}\_{\parallel}^{2} - 2\boldsymbol{\rho}\_{\parallel}\mathbf{L}\_{\perp}\cos(\boldsymbol{\beta}\_{\parallel} - \boldsymbol{\phi}\_{\parallel}) = \mathbf{R}^{2} \,. \tag{25}$$

Equation 24 can be solved for *Li* , giving:

$$L\_i = \min \{ \rho\_\neq \cos(\beta\_i - \phi\_\neq) \pm \sqrt{\mathbf{R}^2 - \rho\_\neq^2 \sin^2(\beta\_i - \phi\_\neq)} \}, \qquad \alpha\_{\text{min}} \le \beta\_i \le \alpha\_{\text{max}}.\tag{26}$$

Equation 26 has a real value if *α*min and *α*max are selected as:

$$a\_{\min} = \min \{ \phi\_{\rangle} \pm \sin \frac{R}{\rho\_{\rangle}} \} \qquad \qquad \qquad \qquad a\_{\max} = \max \{ \phi\_{\rangle} \pm \sin \frac{R}{\rho\_{\rangle}} \} \tag{27}$$

The above analysis is for the case when contains a single obstacle point. In the common case where consists of *m* obstacle points, *C*obst is found by:

$$\mathcal{C}\_{obs} = \bigcup\_{1 \le j \le m} \mathcal{C}\_j. \tag{28}$$

The exact robots radius was utilized to enlarge the obstacle points. However, control errors arise within the algorithm even when using accurate robot dimensions. Therefore, the radius is modified to *Rs* = *R* + *d*safe. This serves as a space buffer that adds a safety margin. In our implementation *d*safe is chosen to be 20% of the robot radius.

#### **3.4. Selection of desired steering angle**

2 22

b

 b- ££ o o

, = cos , *j x j j I Rd* +

, = sin , 100 100 . *jy j j <sup>j</sup> I d* b

**Figure 9.** C-space algorithm.

boundary of

52 Robot Control

Next, we find the radial distance *Li*

Equation 24 can be solved for *Li*

at angle *βi*

r

r

af

 bf

Equation 26 has a real value if *α*min and *α*max are selected as:

. The equation of *Cj*

*j jx jy j*

 r

, giving:

r

, ,

r

2 2 ,

 f

= , = 2( ), *j y*

*I I atan*

2 2 <sup>2</sup> 2 cos( ) = . *i j ji i j LL R* +- -

2 2 2 min max = min{ cos( ) sin ( )}, . *i j ij j ij <sup>i</sup> L R*

min max = min{ sin }, = max{ sin } *j j*

 r  bf

> bf

*j j R R*

 a

obst , , = { |( ) ( ) }, *j x j y* C C *q xI yI R* Î - +- £ (21)

(22)

which is the radial distance between the robot and the

,

*I*

*I*

 a

 f

± ± (27)


 ba

> r

+ (24)

*j x*

(25)

in polar coordinates is

(23)

The sectors in are classified as free or occupied. The *j* th sector *Sj* is occupied if *Lj* ≤ *R*safe; otherwise, it is free. Adjacent free sectors are grouped together to form gaps. Let *N*free denote the number of sectors forming a gap. The gaps are classified as follows:

$$\begin{array}{ll} \mathit{map} = \begin{cases} \mathit{wide} & \text{if } N\_{\mathit{fwo}} > 3, \\ \mathit{medium} & \text{if } N\_{\mathit{fwo}} = 3, \\ \mathit{narrow} & \text{if } N\_{\mathit{fwo}} < 3. \end{cases} \end{array} \tag{29}$$

The desired steering angle is set as the angle of the gap edge with minimum cost. To ensure the selection of the widest possible gap, the search at the beginning is performed over the free wide gaps. if no solution exists within this category, the algorithm searches for a gap in the medium category. The algorithm searches within the narrow gaps, only if the latter two categories did not contain any solution.

$$\text{Cost}(\beta\_{\slash}) = c\_1(\gamma\_{\not\pi\ell} - \beta\_{\not\gamma}) + c\_2\beta\_{\not\gamma}.\tag{30}$$

The equation is explained as follows: the term *c*1(*γ*ref − *βj* ) refers to the closeness of the goal location to the desired steering direction. The second term, *c*2*βj* , indicates how close the current robot heading is to the current steering direction. The coefficients are chose to be *c*1 = 0.3 and *c*2 = 0.7, as with this, more weight is given to the steering angles resulting in a smoother trajectory.

In **Figure 10**, the oscillatory trajectory of the robot is exemplified. At the initial start time *t*0, the robot's polar map has gaps *G*1 and *G*2. At this time, the robots steers toward *G*2 as it closer to the *q*target. After an elapsed time *T*, the robot no longer has *G*2 within its range as it achieves a better view, steering the robot toward *G*1. However, this action brings the robots back to its initial state at *t*0 where both gaps are visible. With this repetitive motion, the robot gets trapped in an infinite loop of repetitive actions. One way to solve this problem is to adjust the steering angle adaptively and smooth the trajectory while avoiding the trap situations as described later in the chapter.

**Figure 10.** Trajectory oscillation scenario due to a trapped situation.

The algorithm used to select the desired steering angle is summarized as follows:

**if** ref ∈ **then**

$$\mathcal{Y}\_{\text{desired}} = \mathcal{Y}\_{\text{net}}$$

**elseif** wide ≠ **then**

$$\mathcal{Y}\_{\text{desired}} = \arg\min\_{\mathcal{J}\_{\boldsymbol{\beta}} \in \mathcal{G}\_{\text{ads}}} \text{Cost}(\boldsymbol{\beta}\_{\boldsymbol{\beta}}).$$

**else if** medium ≠ **then**

$$\mathcal{Y}\_{\text{desired}} = \arg\min\_{\boldsymbol{\beta}\_{\boldsymbol{\beta}} \in \mathcal{G}\_{\text{modum}}} \text{Cost}(\boldsymbol{\beta}\_{\boldsymbol{\beta}}).$$

**else if** narrow ≠ **then**

$$\mathcal{Y}\_{\text{desired}} = \arg\min\_{\boldsymbol{\beta}\_{\boldsymbol{\beta}} \in \mathcal{\mathcal{J}}\_{\text{narrow}}} \text{Cost}(\boldsymbol{\beta}\_{\boldsymbol{\beta}}).$$

**else** Turn 180° around. **end if end if**

angle adaptively and smooth the trajectory while avoiding the trap situations as described

later in the chapter.

54 Robot Control

**if** ref ∈ **then**

**elseif** wide ≠ **then**

**else if** medium ≠ **then**

**else if** narrow ≠ **then**

**Figure 10.** Trajectory oscillation scenario due to a trapped situation.

The algorithm used to select the desired steering angle is summarized as follows:

g

g

desired

desired

g

g

desired ref

 g=

wide desired = arg min ( ). *j*

> medium = arg min ( ). *<sup>j</sup>*

narrow = arg min ( ). *<sup>j</sup>*

*j*

*j*

ÎG

b ÎG

b

b

ÎG

*Cost <sup>j</sup>*

*Cost*

*Cost*

 b

 b

 b

#### **3.5. Identification of adaptive radius of curvature**

The robot follows a circular arc with constant wheel velocities instead of the desired steering angle due to the nonholonomic kinematic constraint. A collision could take place when going from the initial to the final path configuration as it may be interested with . An example of a collision is demonstrated in **Figure 11**, where the robot had steered with a relatively large radius.

**Figure 11.** The robot collides with an obstacle because it uses a large turning radius.

The sector along the local *x*-axis is labeled as *S*0 and the sector along the desired steering angle is labeled as *S*desired. Next, define as the distance between the robot front reference point and the obstacle point *oj* as shown in **Figure 12**. This distance is evaluated in terms of the variable *Lj* as follows:

$$L\_{\rangle}^{w} = \sqrt{(L\_{\rangle}\cos\beta\_{\rangle} + a)^{2} + (L\_{\rangle}\sin\beta\_{\rangle})^{2}} \,\, \, \, \, \tag{35}$$

**Figure 12.** Turning radius selection.

where *a* is the actual distance separating the middle point *m* of the wheels axis from the robot front reference point. *L*min is then defined as the minimum distance to the nearest obstacle point situated between *S*desired and *S*0. The optimum turning radius *rc* is selected so that the robot trajectory goes through (*L*min, *γ*desired) as shown in **Figure 12**. The turning radius is derived as follows. Consider the isosceles triangle where the two equal sides have length *rc* and the remaining side has length *L*min. From the law of cosines,

$$L\_{\text{min}}^2 = 2r\_c^2 - 2r\_c^2 \cos a\_1. \tag{36}$$

*α*1 can be found as

$$2a\_1 + 2a\_2 = 180,\tag{37}$$

$$
\alpha\_2 = \Re 0 - \gamma\_{\text{dostrad}} \,\prime \tag{38}
$$

$$
\omega \Rightarrow \alpha\_1 = \mathcal{D} \gamma\_{\text{desired}}.\tag{39}
$$

Using the double angle formula and equation 32, we can find *rc* as

$$r\_c = \frac{L\_{\text{min}}}{2\sin(\mathcal{Y}\_{\text{desired}})}.\tag{40}$$

A safety margin is introduced by reducing the turning radius so that the robot passes through the point (*L*min − *d*safe2, *γ*desired) instead. Also, the turning radius *rc* is forced to saturate if it is greater than a threshold value *r*large. In our implementation, *d*safe2 is selected to be 1.2*R* and *r*large = 0.5m.

#### **4. Experimental results**

#### **4.1. Mobile robot platform**

**Figure 12.** Turning radius selection.

56 Robot Control

*α*1 can be found as

remaining side has length *L*min. From the law of cosines,

where *a* is the actual distance separating the middle point *m* of the wheels axis from the robot front reference point. *L*min is then defined as the minimum distance to the nearest obstacle point situated between *S*desired and *S*0. The optimum turning radius *rc* is selected so that the robot trajectory goes through (*L*min, *γ*desired) as shown in **Figure 12**. The turning radius is derived as follows. Consider the isosceles triangle where the two equal sides have length *rc* and the

a

(36)

+ 2 = 180, (37)

= 90 , - (38)

=2 . (39)

2 22 min <sup>1</sup> = 2 2 cos . *c c L rr* -

> 1 2 a

a

 a

2 desired

1 desired Þa

 g

 g A prototype robot platform was designed and built to validate the proposed algorithms. The platform has a differential drive mechanism and is designed to operate indoors on flat solid surfaces. Forward, backward, and steered motion is generated by controlling the right and left wheel velocities based on the differential steering concept. The platform control system includes a single board computer and a microcontroller, thus providing a dependable and strong computing environment. The platform comprises also a large range of sensors including ultrasonic sensors and a laser range sensor for obstacle detection, as well as a compass and encoders for localization. **Figure 13** shows a front view picture of the mobile robot platform.

The obstacle avoidance algorithms described earlier are tested on the mobile robot platform in different environment settings. The testing is conducted indoors in a lab environment where the lab furniture is to be avoided. The obstacles are arranged in five different scenarios that vary in difficulty. For all scenarios, the sample time is *T* = 1 s, the robot initial configuration is (0, 0, − 90∘) while the target *x*–*y* location is (1.6 m, − 1.5 m). Hence, the initial error in position is 2.1932 m.

#### **4.2. Environment setting 1**

The robot's initial configuration is connected to the target configuration through a direct path indicated by a straight line as observed in **Figure 14a**. The trajectory, of length 2.2711 m, is depicted in **Figure 14b**. In **Figure 14c**, the robot's velocities are presented and are smooth in the global frame. **Figure 14d** exemplifies the control action. An infinite radius of 0.5 is set for keeping the plot in rage as the robot would steer in an infinite radius when moving in a straight line.

**Figure 13.** Mobile robot platform.

**Figure 14.** Experimental results. (a) Robot initial position, target position, and the surrounding obstacles; (b) The obstacle points in green, the area occupied by the robot at each instance in time in red, and the reference point trajectory in blue; (c) The robot velocities; (d) The robot control action.

Adaptive Steering and Trajectory Control of Wheeled Mobile Robots for Autonomous Navigation http://dx.doi.org/10.5772/64227 59

**Figure 13.** Mobile robot platform.

58 Robot Control

**Figure 14.** Experimental results. (a) Robot initial position, target position, and the surrounding obstacles; (b) The obstacle points in green, the area occupied by the robot at each instance in time in red, and the reference point trajectory in

blue; (c) The robot velocities; (d) The robot control action.

**Figure 15.** Illustration of the trajectory control algorithm at different time intervals. The obstacles surrounding the robot at a given instant of time are shown as black dots in the Cartesian coordinate frame. A rough estimate of the obstacle contour is defined by the solid yellow line. The classified sectors are shown in the polar histogram. The desired steering angle is indicated by a dashed green line, while the reference steering angle is indicated by a solid red line.

Critical time samples with some intermediate values are shown in **Figure 15**. At a sample of 12 s, the robot is only capable of viewing the square obstacles front side. This classifies the front gaps as occupied. The reference steering angle is classified to be in an occupied sector as it is of −20° value. Hence, the reference angle *γ*desired with a value 30° is selected as the next best alternative, and the corresponding turning radius is approximately 0.205 m as shown in **Figure 14d**. **Figure 15b** illustrates the robot at a sample time of 26 s. At this sample, the robot can only observe the obstacles' right side and has a *γ*ref of −54° and a *γ*desired of −10°. Additionally, the robot takes a turn with a 0.5 m radius. The sample at 49 s is seen in **Figure 15c**. *γ*desired is simply equated to *γ*ref as it resides in a free sector. This moves the robot straight to the target. This scenario course was completed within 70 s.

**Figure 16.** Experiment 2 results. (a) The robot initial position, target position, and the surrounding obstacles; (b) The obstacle points in green, the area occupied by the robot at each instance in time in red, and the reference point trajectory in blue. (c) The robot velocities; (d) The robot control vector.

Adaptive Steering and Trajectory Control of Wheeled Mobile Robots for Autonomous Navigation http://dx.doi.org/10.5772/64227 61

Critical time samples with some intermediate values are shown in **Figure 15**. At a sample of 12 s, the robot is only capable of viewing the square obstacles front side. This classifies the front gaps as occupied. The reference steering angle is classified to be in an occupied sector as it is of −20° value. Hence, the reference angle *γ*desired with a value 30° is selected as the next best alternative, and the corresponding turning radius is approximately 0.205 m as shown in **Figure 14d**. **Figure 15b** illustrates the robot at a sample time of 26 s. At this sample, the robot can only observe the obstacles' right side and has a *γ*ref of −54° and a *γ*desired of −10°. Additionally, the robot takes a turn with a 0.5 m radius. The sample at 49 s is seen in **Figure 15c**. *γ*desired is simply equated to *γ*ref as it resides in a free sector. This moves the robot straight to the target.

**Figure 16.** Experiment 2 results. (a) The robot initial position, target position, and the surrounding obstacles; (b) The obstacle points in green, the area occupied by the robot at each instance in time in red, and the reference point trajecto-

This scenario course was completed within 70 s.

60 Robot Control

ry in blue. (c) The robot velocities; (d) The robot control vector.

**Figure 17.** (a) Shows the robot entering the passage; (b) shows the robot inside the passage; (c) shows the robot exiting the passage.

**Figure 18.** Experiment 3 results. (a) The robot testing environment; (b) the robot trajectory; (c) the robot velocities; (d) the robot control vector.

#### **4.3. Environment setting 2**

A scenario of wide entrance but narrow exit was modeled as in **Figure 16a**. The robot was able to make it through the passage within 74 s and with a 2.2539 m trajectory length, shown in **Figure 16b** below. Figue 16c and 16d depict the robot velocities and control actions, respectively. **Figure 17** illustrates with polar histograms for when the robot first enters the passageway, moves within it and then exits.

#### **4.4. Environment setting 3**

The difference in this scenario is that the narrow nature of the passage way is greater than that depicted in scenario 2, as shown in **Figure 18a**. As a result, the robots ability to pick up on and detect the narrow gaps that are only slightly larger than its size is tested. As shown in **Figure 18b**, the robot successfully makes it through the pathway and its corresponding Adaptive Steering and Trajectory Control of Wheeled Mobile Robots for Autonomous Navigation http://dx.doi.org/10.5772/64227 63

**Figure 18.** Experiment 3 results. (a) The robot testing environment; (b) the robot trajectory; (c) the robot velocities; (d)

A scenario of wide entrance but narrow exit was modeled as in **Figure 16a**. The robot was able to make it through the passage within 74 s and with a 2.2539 m trajectory length, shown in **Figure 16b** below. Figue 16c and 16d depict the robot velocities and control actions, respectively. **Figure 17** illustrates with polar histograms for when the robot first enters the

The difference in this scenario is that the narrow nature of the passage way is greater than that depicted in scenario 2, as shown in **Figure 18a**. As a result, the robots ability to pick up on and detect the narrow gaps that are only slightly larger than its size is tested. As shown in **Figure 18b**, the robot successfully makes it through the pathway and its corresponding

the robot control vector.

62 Robot Control

**4.3. Environment setting 2**

**4.4. Environment setting 3**

passageway, moves within it and then exits.

**Figure 19.** (a) Shows the robot entering the passage; (b) Shows the robot inside the passage; (c) Shows the robot exiting the passage.

velocities and control actions during the trajectory are illustrated in **Figure 18c** and **18d**, respectively. **Figure 19** provides details via histograms from when the robot enters to when it leaves the passageway 19.

#### **4.5. Environment setting 4**

The difference between this scenario, depicted in **Figure 20a**, and scenario 3 is the addition of an obstacle to block the exit from the passageway, forming a dead end for the robot. The trajectory taken by the robot is presented in **Figure 20b**, **20c**, and **20d** represent the robot's velocity and control actions through this passageway, respectively. In **Figure 20c**, fluctuations left and right can be seen for the turning angle, *γ*desired. However, at *t* = 31 s the robot approaches the dead end and comes to realize that the narrow gap is in fact blocked. The robot thus steers left and now envisions the dead end as a gap, resulting in the robots attempt to steer toward it once more. This is illustrated in **Figure 21c**. After 179 s, the robot completes the mission of contouring the obstacles and overcoming the oscillations back and forth having travelled a total of 6.0243 m.

**Figure 20.** Experiment 4 results (a) The robot initial position, target position, and the surrounding obstacles; (b) The obstacle points in green, the area occupied by the robot at each instance in time in red, and the reference point trajectory in blue; (c) The robot velocities; (d) The robot control vector.

Adaptive Steering and Trajectory Control of Wheeled Mobile Robots for Autonomous Navigation http://dx.doi.org/10.5772/64227 65

**4.5. Environment setting 4**

64 Robot Control

total of 6.0243 m.

The difference between this scenario, depicted in **Figure 20a**, and scenario 3 is the addition of an obstacle to block the exit from the passageway, forming a dead end for the robot. The trajectory taken by the robot is presented in **Figure 20b**, **20c**, and **20d** represent the robot's velocity and control actions through this passageway, respectively. In **Figure 20c**, fluctuations left and right can be seen for the turning angle, *γ*desired. However, at *t* = 31 s the robot approaches the dead end and comes to realize that the narrow gap is in fact blocked. The robot thus steers left and now envisions the dead end as a gap, resulting in the robots attempt to steer toward it once more. This is illustrated in **Figure 21c**. After 179 s, the robot completes the mission of contouring the obstacles and overcoming the oscillations back and forth having travelled a

**Figure 20.** Experiment 4 results (a) The robot initial position, target position, and the surrounding obstacles; (b) The obstacle points in green, the area occupied by the robot at each instance in time in red, and the reference point trajecto-

ry in blue; (c) The robot velocities; (d) The robot control vector.

**Figure 21.** (a) Robot detects a passage; (b) robot discovers a dead end and attempt to turn away; (c) after the robot moves away, the dead end appears as a gap.

#### **4.6. Environment setting 5**

The three obstacles in scenario 5 are placed in such a way to form a narrow gap that is smaller than the robot size. This makes the robot's initial steering to fall into a blocked path as depicted in **Figure 22a**. The robot ends up at the target location due to its nature to persistently search for a gap, as illustrated in **Figure 22b**, the target location as shown in **Figure 22b**. The robot velocities and control actions are depicted in **Figure 22c** and **Figure 22d**, once more, depict the robot's respective velocities and control actions.

**Figure 22.** Experiment 5 results. (a) The robot testing environment; (b) The robot trajectory; (c) The robot velocities; (d) The robot control vector.

#### **5. Conclusion**

This chapter presents a reactive navigation algorithm for a wheeled mobile robot under nonholonomic constraints and in unknown environments. The mobile robot can travel safely and efficiently to a preset destination having no prior knowledge of the environment. The shape and dimensions of the robot are all incorporated to produce the control algorithm that determines the set of all steering angles that result in no collisions. The selection of the steering angle depends on the one that is closest to the target and is identified as the widest gap. In addition, the algorithm takes into account the nonholonomic constraints of differentially steered robots by computing circular trajectories with adaptive radius of curvature. A mobile robot platform was built and used to assess and validate the performance of the algorithms over a variety of unstructured indoor environments. The results demonstrate that the navigation algorithm is capable of driving the robot safely through different obstacle arrangements and avoids successfully trap situations.

## **Author details**

**4.6. Environment setting 5**

66 Robot Control

The robot control vector.

**5. Conclusion**

depict the robot's respective velocities and control actions.

The three obstacles in scenario 5 are placed in such a way to form a narrow gap that is smaller than the robot size. This makes the robot's initial steering to fall into a blocked path as depicted in **Figure 22a**. The robot ends up at the target location due to its nature to persistently search for a gap, as illustrated in **Figure 22b**, the target location as shown in **Figure 22b**. The robot velocities and control actions are depicted in **Figure 22c** and **Figure 22d**, once more,

**Figure 22.** Experiment 5 results. (a) The robot testing environment; (b) The robot trajectory; (c) The robot velocities; (d)

This chapter presents a reactive navigation algorithm for a wheeled mobile robot under nonholonomic constraints and in unknown environments. The mobile robot can travel safely and efficiently to a preset destination having no prior knowledge of the environment. The

Mariam Al-Sagban\* and Rached Dhaouadi

\*Address all correspondence to: g00006931@aus.edu

American University of Sharjah, Sharjah, the United Arab Emirates

#### **References**


Heidelberg, 2008, pp. 827–852, 10.1007/978-3-540-30301-5\_36. [Online]. Available: http://dx.doi.org/10.1007/978-3-540-30301-5\_36


#### **Occupancy Map Construction for Indoor Robot Navigation Occupancy Map Construction for Indoor Robot Navigation**

Dora-Luz Almanza-Ojeda, Yazmín Gomar-Vera and Mario-Alberto Ibarra-Manzano Dora-Luz Almanza-Ojeda, Yazmín Gomar-Vera and Mario-Alberto Ibarra-Manzano

Additional information is available at the end of the chapter Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/64871

#### **Abstract**

Heidelberg, 2008, pp. 827–852, 10.1007/978-3-540-30301-5\_36. [Online]. Available:

[9] G. Campion and W. Chung, "Wheeled robots," in *Springer Handbook of Robotics*, B. Siciliano and O. Khatib, Eds. Springer Berlin Heidelberg, 2008, pp. 391–410,

[10] O. Mohareri and R. Dhaouadi, "A neural network based adaptive tracking controller for nonholonomic wheeled mobile robots with unknown dynamics," in *Proc. of the ASME 2010 International Mechanical Engineering Congress & Exposition (IM‐*

10.1007/978-3-540-30301-5\_18. [Online]. Available: http://dx.doi.org/

http://dx.doi.org/10.1007/978-3-540-30301-5\_36

*ECE2010)*, vol. 6, November 12–18, 2010.

10.1007/978-3-540-30301-5\_18

68 Robot Control

Robot mobile navigation is a hard task that requires, essentially, avoiding static and dynamic objects. This chapter presents a strategy for constructing an occupancy map by proposing a probabilistic model of an ultrasonic sensor, during robot indoor navigation. A local map is initially constructed using the ultrasonic sensor mounted in the front of the robot. This map provides the position of the nearest obstacles in the scene useful for achieving the reactive navigation. The encoders allow computing the robot location in the initial local map. A first path for robot navigation based on the initial local map is estimated using the potential field strategy. As soon as the robot starts its trajectory in real indoor environments with obstacles, the sensor continuously detects and updates the occupancy map by the logsig strategy. A Gaussian function is used for modelling the ultrasonic sensor with the aim of reaching higher precision of the distance measured for each obstacle in the scene. Experiments on detecting, mapping and avoiding obstacles are performed using the mobile robotic platform DaNI 2.0 and the VxWorks system. The resulted occupancy grid is analysed and discussed at the end of this document.

**Keywords:** occupancy map, obstacle detection, path planning, robot mobile naviga‐ tion, Gaussian model

#### **1. Introduction**

Nowadays, the artificial intelligent field has developed service task in robotic systems with the aim of providing help or comfort to humans. This is called service robotic [1], and it is

© 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. © 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

supposed that the robot acts total or partially with autonomy. In particular, the service robotic tackles the problem of domestic robots. Such robots require highly autonomy and real‐time processing for recognizing unknown environments. To do this, they use stereovision [2] and laser range finders [3, 4]. Furthermore, these robots must be able to autolocalizing and to navigate without human supervision, detect and locate obstacles and constructing its own map of the environment, using only their sensors and eventually a communication system with a central computer. The environment and workspace of the industrial robots are especially adapted for them and for performing special tasks; thus, these robots are program‐ med for knowing the environment in accordance with their physical dimensions. On the contrary, service robotics interacts with a changing and initially unknown environment.

Topologic and geometric maps could be constructed for perceiving robot environment. As proposed by Thrun et al. [5], a topologic map contains nodes and lines that joint these nodes representing the possible trajectories from one node to another. Furthermore, the author proposes the efficient mapping of the space and the low complexity as advantages, however, to recognize the place is difficult, depends of personal interpretation, and it could produce suboptimal trajectories. In contrast, the geometric maps numerically represent the coordinates and properties of the environment of the robot. These maps could efficiently represent big regions with few numeric parameters. Thus, the environment is descripted through geometric characteristics such as segments, corners, among others and their corresponding relationships (distance positions) [6].

On the other hand, the occupancy grid is a technique based on the discretization of the space in equal cells with a probability, which represents an occupied, empty or unknown area. This technique has been largely used due to it requires basic concepts for constructing, representing and updating and it allows to compute shorter trajectories. The main disadvantage of the occupancy grid is that requires the robot position.

The ultrasound is mechanical radiations with frequency higher to the audible range (>20 kHz) when these waves are reflected by an object in the environment. The ultrasonic sensors contain a piezoelectric transducer that is used as a transmitter and receptor for emitting and receiving the ultrasonic waves, respectively. This project proposes a strategy focused on ultrasonic sensors since this provides the distance from each obstacle in the environment to the sensor mounted on the robot. Recently, a self‐configuring network of ultrasonic sensors has been proposed for tracking moving target [7], providing that these sensors provide an economical and basic solution to object detection in indoor environments.

In the development of novel strategies for proposing autonomous robotic systems, this work tackles with the problem of obstacle detection and avoiding in real time using an ultrasonic sensor embedded in a robot mobile during indoor navigation [8]. The environment is initially unknown; thus, first of all, the robot constructs an initial version of a two‐dimensional (2D) occupancy map. Several experimental tests are performed for modelling the sensor error and for measuring the precision of measurements with the aim of assuring a reliable map.

#### **2. Overall strategy for robot navigation**

supposed that the robot acts total or partially with autonomy. In particular, the service robotic tackles the problem of domestic robots. Such robots require highly autonomy and real‐time processing for recognizing unknown environments. To do this, they use stereovision [2] and laser range finders [3, 4]. Furthermore, these robots must be able to autolocalizing and to navigate without human supervision, detect and locate obstacles and constructing its own map of the environment, using only their sensors and eventually a communication system with a central computer. The environment and workspace of the industrial robots are especially adapted for them and for performing special tasks; thus, these robots are program‐ med for knowing the environment in accordance with their physical dimensions. On the contrary, service robotics interacts with a changing and initially unknown environment.

Topologic and geometric maps could be constructed for perceiving robot environment. As proposed by Thrun et al. [5], a topologic map contains nodes and lines that joint these nodes representing the possible trajectories from one node to another. Furthermore, the author proposes the efficient mapping of the space and the low complexity as advantages, however, to recognize the place is difficult, depends of personal interpretation, and it could produce suboptimal trajectories. In contrast, the geometric maps numerically represent the coordinates and properties of the environment of the robot. These maps could efficiently represent big regions with few numeric parameters. Thus, the environment is descripted through geometric characteristics such as segments, corners, among others and their corresponding relationships

On the other hand, the occupancy grid is a technique based on the discretization of the space in equal cells with a probability, which represents an occupied, empty or unknown area. This technique has been largely used due to it requires basic concepts for constructing, representing and updating and it allows to compute shorter trajectories. The main disadvantage of the

The ultrasound is mechanical radiations with frequency higher to the audible range (>20 kHz) when these waves are reflected by an object in the environment. The ultrasonic sensors contain a piezoelectric transducer that is used as a transmitter and receptor for emitting and receiving the ultrasonic waves, respectively. This project proposes a strategy focused on ultrasonic sensors since this provides the distance from each obstacle in the environment to the sensor mounted on the robot. Recently, a self‐configuring network of ultrasonic sensors has been proposed for tracking moving target [7], providing that these sensors provide an economical

In the development of novel strategies for proposing autonomous robotic systems, this work tackles with the problem of obstacle detection and avoiding in real time using an ultrasonic sensor embedded in a robot mobile during indoor navigation [8]. The environment is initially unknown; thus, first of all, the robot constructs an initial version of a two‐dimensional (2D) occupancy map. Several experimental tests are performed for modelling the sensor error and

for measuring the precision of measurements with the aim of assuring a reliable map.

(distance positions) [6].

70 Robot Control

occupancy grid is that requires the robot position.

and basic solution to object detection in indoor environments.

**Figure 1** depicts the block diagram of the global strategy for constructing a local occupancy map of indoor environments. As the operating range of the ultrasonic sensor is known, this sets the size of the area covered by the sensor, which will be directly updated in the local map at each instant time *t*. This area establishes the size of an initial local map previously provided to the robot. On the other hand, before robot starts to move, the ultrasonic sensor is used for detecting objects in front of him. To do this, the ultrasonic sensor carries out a "sweep" in the range −90° to 90° with respect to X axis of the robot (see **Figure 2a**). The local map also uses the initial robot location, due to location of the objects are given with respect to the ultrasonic sensor. The path‐planning module computes an initial path for robot navigation. The potential field's technique is used to plan the best trajectory for the robot. In real time, robot odometric location is obtained from the encoders for updating the map and continuously constructing the global map of the robot scene. Both global and local maps store the probability of occupancy around the robot during navigation.

**Figure 1.** Global strategy for constructing an occupancy map during indoor navigation.

Every update of the global map is stored in a file internally on the robot. The map construction consists in dividing the environment in small uniform cells, which will be labelled as occupied or free in accordance with the ultrasonic measures. An intensive calibration strategy is required with the aim of obtaining an accurate digital representation of the real scene. The robot navigates from a predefined initial position to a goal position; then, the algorithm ends when the robot reaches such predefined goal position.

**Figure 2.** (a) Reference axis of the mobile robot DaNI 2.0. (b) Graphical representation of the robot in the local and global reference axis, P represents the reference point of the position.

#### **3. Robot model description**

The mobile robot used in this project is shown in **Figure 2a**. This is a robotic platform called NI LabVIEW robotics Starter Kit® [9], also known as DaNI 2.0, developed by National Instru‐ ments company® (NI). This mobile robot was designed to develop and run algorithms in real time for autonomous system applications. The components of the mobile robot DaNI 2.0 are essentially: two DC motors, two encoders and a reconfigurable card sbRIO‐9632 (Single Board Reconfigurable I/O), an ultrasonic sensor mounted on a servomotor for providing to sensor rotational motion. **Figure 2b** illustrates a graphical representation of the robot, with the global and local reference axis of the robot. The X and Y axes are defined arbitrarily in the plane as the reference of the global coordinates.

The kinematic model of the robot DaNI 2.0 consists in a differential configuration: each wheel of the robot is connected to a DC motor, which provides the traction force and a stabilization wheel for balancing the robot. The basic model for representing the robot position considers the robot as a single point in the space. Thus, the robot position is specified by choosing a point *P* in the robot chassis as a reference; usually, this point is the centre of the wheel axis. In addition, the point *P* represents the origin of the robot axis Xr and Yr indicating the local reference of the robot position, see **Figure 2a**.

In particular, the sbRIO‐9632 card is a heterogeneous‐embedded platform, developed by NI®, which contains a real‐time processor and serves as main control unit of the robot. Besides, this platform includes a Field Programming Gate Array (FPGA) Xilinx Spartan‐3, which is a reconfigurable device that executes programmed tasks in real time, that is, the active response of the system to external events. This real‐time execution uses a low‐level programming to perform tasks, such as motor control, signal acquisition by means of digital or analogue inputs and monitoring digital and analogue inputs/outputs, among others. However, a higher level of programming is possible through the NI LabVIEW® robotics software, which is a graphical language. Programming languages such as C, C++ or Java could be also used.

#### **3.1. Robot odometric localization**

navigates from a predefined initial position to a goal position; then, the algorithm ends when

**Figure 2.** (a) Reference axis of the mobile robot DaNI 2.0. (b) Graphical representation of the robot in the local and

The mobile robot used in this project is shown in **Figure 2a**. This is a robotic platform called NI LabVIEW robotics Starter Kit® [9], also known as DaNI 2.0, developed by National Instru‐ ments company® (NI). This mobile robot was designed to develop and run algorithms in real time for autonomous system applications. The components of the mobile robot DaNI 2.0 are essentially: two DC motors, two encoders and a reconfigurable card sbRIO‐9632 (Single Board Reconfigurable I/O), an ultrasonic sensor mounted on a servomotor for providing to sensor rotational motion. **Figure 2b** illustrates a graphical representation of the robot, with the global and local reference axis of the robot. The X and Y axes are defined arbitrarily in the plane as

The kinematic model of the robot DaNI 2.0 consists in a differential configuration: each wheel of the robot is connected to a DC motor, which provides the traction force and a stabilization wheel for balancing the robot. The basic model for representing the robot position considers the robot as a single point in the space. Thus, the robot position is specified by choosing a point *P* in the robot chassis as a reference; usually, this point is the centre of the wheel axis. In addition, the point *P* represents the origin of the robot axis Xr and Yr indicating the local reference of the

In particular, the sbRIO‐9632 card is a heterogeneous‐embedded platform, developed by NI®, which contains a real‐time processor and serves as main control unit of the robot. Besides, this platform includes a Field Programming Gate Array (FPGA) Xilinx Spartan‐3, which is a reconfigurable device that executes programmed tasks in real time, that is, the active response

the robot reaches such predefined goal position.

72 Robot Control

global reference axis, P represents the reference point of the position.

**3. Robot model description**

the reference of the global coordinates.

robot position, see **Figure 2a**.

During navigation, the robot needs to know its position and orientation with respect to its local and global axis of reference. To do this, the most common method used is based on geometric equations providing an estimation of the robot location by combining information obtained from the encoders on the wheels and from the propulsion components. This method is known as odometric estimation [10], and it is commonly used due to it only employs the kinematic model of the robot without including forces or torques in the mechanism. The main constraint of this method is that, a small error at the beginning of the estimation increases with the time, due to it is accumulated. Another strategy for avoiding this incremental error is to correct the robot position at regular times of movement, using landmarks [11]. Nevertheless, the cost of installation of the landmarks could be considerably high. For this reason, the odometric location strategy is commonly used providing enough results in short trajectories at low cost.

In order to compute the robot position *P*(*X, Y*) in the global coordinate system, it is necessary to know the rotational angle between the local and global coordinate axis, given by *θ* and the origin coordinates of the robot local axis. The geometric equations for computing the point *P* position in global coordinates are as follows:

$$\begin{cases} X = X\_r \cos(\theta) \\ Y = Y\_r \sin(\theta) \end{cases} \tag{1}$$

The robot motion *dr* is computed over the time by considering the robot geometry and its angular velocity on the wheels, using the following equation:

$$d\_r = \frac{2\pi r}{rev} \varphi\_o t \tag{2}$$

where *r* is the wheel radius (1.16 dm for the DaNI 2.0 robot), the angular velocity of the wheel and *t* is the time.

In accordance with the robot geometry and the wheel type, the movement of the robot *Xr* is only in one direction (X direction is used). Furthermore, in the case of one wheel is keeping fix to the floor, while the second wheel moves with an angular velocity; then, the robot will draw a circle around the fixed wheel with a radius of *2l*; being *l* the distance from the point *P* in the chassis to the wheel (1.778 dm for DaNI 2.0 robot). This movement only affects *θ* angle:

$$
\theta = \frac{r\varphi\_{o0}t}{2l} \tag{3}
$$

In order to estimate the location errors due to the odometry, a square trajectory was imple‐ mented on the robot. It was found that such errors in the trajectory are mainly due to the errors in the turns, performed to 86° approximately but expected to 90°. Besides, small changes in the wheels' trajectory are not registered by the encoders, these changes are minimal, and however, they produce a notable final error.

The odometric errors exist essentially due to the robot construction, that is, there is a small difference between the diameters of the wheels. Furthermore, the finite resolution of the encoders and the irregularities in the floor where the trajectory is performed avoid an ideal execution of the trajectory.

#### **3.2. Ultrasonic sensors**

As it was mentioned in the introduction section, the ultrasonic sensors consist of one trans‐ mitter and receptor of the sound. Once the ultrasonic wave has been sent, when such wave found an object a signal, this is reflected as an echo and can be detected by the same transmitter, which acting also like a receptor. In general, the applications of the ultrasonic sensors are based on estimate the lapse of time between such emission and reception of the ultrasonic waves. This lapse of time is known as time of flight (ToF), the corresponding distance to the object that reflected the wave is estimated by means of:

$$d = \frac{1}{2}\mathbf{v} \cdot \mathbf{t}\_f \tag{4}$$

where *v* represents the velocity of sound and *tf* the time of flight.

On the other hand, one particular problem with the ultrasonic sensors is the mirror reflection. This reflection happens mainly in the corners and it is occasioned for several reflections of the ultrasonic waves before to return to the sensor (see **Figure 3**). As a consequence of this phenomenon, some objects of the environment of small size or orientation cannot be detected by the sensor, or in some cases, they are detected farther than really they are. Thus, only the readings taken when the ultrasonic wave impacts perpendicular to the surface will be taken as correct measures.

The cone of sensibility, also known as acoustic sensor aperture, introduces incertitude in the position and distance of the reflected object if the robot is in motion. This is illustrated in **Figure 4**: if one object is detected in the cone of sensibility, therefore, the measured distance will correspond to an object "in front" of the sensor, even if the object is located with an orientation with respect to the robot.

Occupancy Map Construction for Indoor Robot Navigation http://dx.doi.org/10.5772/64871 75

**Figure 3.** Mirror reflection of the ultrasonic waves. (a) Extended trajectory of the ultrasonic wave. (b) Ultrasonic echo that does not return to sensor.

**Figure 4.** Main effect of the acoustic aperture of the ultrasonic sensor that produces incertitude in the object position, positioning the object in an unreal location.

#### **3.3. Gaussian model of the ultrasonic sensor**

2 *r t l* jw

In order to estimate the location errors due to the odometry, a square trajectory was imple‐ mented on the robot. It was found that such errors in the trajectory are mainly due to the errors in the turns, performed to 86° approximately but expected to 90°. Besides, small changes in the wheels' trajectory are not registered by the encoders, these changes are minimal, and

The odometric errors exist essentially due to the robot construction, that is, there is a small difference between the diameters of the wheels. Furthermore, the finite resolution of the encoders and the irregularities in the floor where the trajectory is performed avoid an ideal

As it was mentioned in the introduction section, the ultrasonic sensors consist of one trans‐ mitter and receptor of the sound. Once the ultrasonic wave has been sent, when such wave found an object a signal, this is reflected as an echo and can be detected by the same transmitter, which acting also like a receptor. In general, the applications of the ultrasonic sensors are based on estimate the lapse of time between such emission and reception of the ultrasonic waves. This lapse of time is known as time of flight (ToF), the corresponding distance to the object that

1

On the other hand, one particular problem with the ultrasonic sensors is the mirror reflection. This reflection happens mainly in the corners and it is occasioned for several reflections of the ultrasonic waves before to return to the sensor (see **Figure 3**). As a consequence of this phenomenon, some objects of the environment of small size or orientation cannot be detected by the sensor, or in some cases, they are detected farther than really they are. Thus, only the readings taken when the ultrasonic wave impacts perpendicular to the surface will be taken

The cone of sensibility, also known as acoustic sensor aperture, introduces incertitude in the position and distance of the reflected object if the robot is in motion. This is illustrated in **Figure 4**: if one object is detected in the cone of sensibility, therefore, the measured distance will correspond to an object "in front" of the sensor, even if the object is located with an

= (3)

<sup>2</sup> *<sup>f</sup> d vt* = × (4)

the time of flight.

q

however, they produce a notable final error.

reflected the wave is estimated by means of:

where *v* represents the velocity of sound and *tf*

execution of the trajectory.

**3.2. Ultrasonic sensors**

74 Robot Control

as correct measures.

orientation with respect to the robot.

The experimental test for modelling the sensor was performed at different sampling times with the aim of considering the effect of the robot motion in the measurements. The sampling times used were 60, 80, 100, 200 and 400 ms, in the range of −65° to 65° with intervals of 5° at distances of 6, 8 and 10 dm from the wall. **Figure 5** illustrates the average of these different test performed using a sampling time of 80 ms. The chart (a) shows a stable range of measurements among −20° to 40° confirmed by the standard deviation shown in chart (b). Note that different sensor positions (6, 8 and 10 dm) do not affect the measurements. Similar results have been obtained for a sampling time of 100 ms; however, as this project will be performed in real time, 80 ms was chosen due to it represents the best trade-off between stability and time of detection.

**Figure 5.** Charts of the measurement averages. (a) The sampling time used is 80 ms, for three distances 6, 8 and 10 dm. (b) The standard deviation of measurements performed in chart (a), note the range of stability is among −20° to 40°.

In order to reduce the measurement error of the ultrasonic sensor and to validate that the detected object is inside a specific region, it is used a probabilistic technique based on a Gaussian function. The Gaussian model implies to know the errors due to the distance and angle detection [12]. This model considers the measured distance, denoted here as *d*, and its uncertainty (), and orientation angle *θ* and its uncertainty (). Therefore, the real measure is in the range of ±, and the orientation ± is denoted by:

$$P\left(z|d,\theta\right) = \frac{1}{2\pi\sigma\_d\sigma\_\theta}e^{-\left(\frac{d-z}{2\sigma\_d^2} + \frac{\theta^2}{\sigma\_\theta^2}\right)}\tag{5}$$

where *z* is the variable in the workspace of the ultrasonic sensor measurements. This equation represents the probability that the object be in the position measured by the sensor and uses two standard deviations, range and angle (see **Figure 6a**). The error measured on the angle for one object located at 10 dm is 0.12 dm that represents an error of ±0.7°. This effect is small; therefore, it will be ignored for constructing the occupancy map, considering only the 1D model based on distance only (see Eq. 5). **Figure 6b** depicts in blue the 1D model of the ultrasonic sensor considering only the distance and the uncertainty (). Using a confiability value of 0.8, the resulted plot is shown in green. Once the object has been detected, the environment behind him is unknown. Thus, the probability of such cells is considered 0.5 because the cell value cannot be known (red plot in **Figure 6b**). Furthermore, this plot represents the probability of the object be in the distance describe in the x axis. This result will be used for occupancy map construction.

#### Occupancy Map Construction for Indoor Robot Navigation http://dx.doi.org/10.5772/64871 77

**Figure 6.** Gaussian model of the ultrasonic sensor. (a) Object position in coordinates (d, θ) for a 2D probability model. (b) Probability model of object position detected by ultrasonic sensor in 1D.

#### **4. Global map construction**

−20° to 40° confirmed by the standard deviation shown in chart (b). Note that different sensor positions (6, 8 and 10 dm) do not affect the measurements. Similar results have been obtained for a sampling time of 100 ms; however, as this project will be performed in real time, 80 ms was chosen due to it represents the best trade-off between stability and time of detection.

**Figure 5.** Charts of the measurement averages. (a) The sampling time used is 80 ms, for three distances 6, 8 and 10 dm. (b) The standard deviation of measurements performed in chart (a), note the range of stability is among −20° to 40°.

In order to reduce the measurement error of the ultrasonic sensor and to validate that the detected object is inside a specific region, it is used a probabilistic technique based on a Gaussian function. The Gaussian model implies to know the errors due to the distance and angle detection [12]. This model considers the measured distance, denoted here as *d*, and its uncertainty (), and orientation angle *θ* and its uncertainty (). Therefore, the real measure

2 2 ( ) <sup>1</sup> <sup>2</sup> | , <sup>2</sup>

q

where *z* is the variable in the workspace of the ultrasonic sensor measurements. This equation represents the probability that the object be in the position measured by the sensor and uses two standard deviations, range and angle (see **Figure 6a**). The error measured on the angle for one object located at 10 dm is 0.12 dm that represents an error of ±0.7°. This effect is small; therefore, it will be ignored for constructing the occupancy map, considering only the 1D model based on distance only (see Eq. 5). **Figure 6b** depicts in blue the 1D model of the ultrasonic sensor considering only the distance and the uncertainty (). Using a confiability value of 0.8, the resulted plot is shown in green. Once the object has been detected, the environment behind him is unknown. Thus, the probability of such cells is considered 0.5 because the cell value cannot be known (red plot in **Figure 6b**). Furthermore, this plot represents the probability of the object be in the distance describe in the x axis. This result will be

*d*

ps s ( )<sup>2</sup> <sup>2</sup>

q

= (5)

q

 s

*d d z*


s

is in the range of ±, and the orientation ± is denoted by:

( )

used for occupancy map construction.

76 Robot Control

q

*P zd e*

The occupancy maps are a probabilistic technique based on small cells that divide the sur‐ rounding space of the indoor or outdoor environments. The probability of one cell is occupied which is estimated using robot sensors. Each cell in the map represents the information contained in the physical space in front of the sensors used to measure the environment. The values in the cell describe the following situations:

$$\begin{cases} \rightharpoonup 0.5 & \text{free cell} \\ = 0.5 & \text{unknown} \\ > 0.5 & \text{occupied cell} \end{cases} \tag{7}$$

#### **4.1. Static occupancy map**

The initial local map uses the initial position of the robot, which is always knowing as navigation is performed only in indoor environments for constructing a static occupancy map considering that the robot is not moving in the environment. The ultrasonic sensor in front of the robot rotates in the range of −90° to 90° with respect to the x axis of the robot; nevertheless, the real range used was in the range of −20° to 40° in order to acquire more stable and accurate measures. Another advantage is that a reduced amount of measurements avoiding synchro‐ nization problems and allowing the real‐time execution. The rest of the angle range will be covered as the robot navigates in the environment.

In this work, an initial local map is constructed since a static position of the robot in the environment which is represented by a grid as illustrated in **Figure 7b**. This map indicates the starting point of the robot trajectory and the corresponding state of the surrounding environ‐ ment in accordance with Eq. (7). Each cell of the map represents 1 dm2 of the environment minimizing the location errors due to the odometry of the robot. Note that, the red cells in the front and in both sides of the robot represent the obstacles in the scene (probability of the cells are higher to 0.5). This map is updated based on the log‐odds algorithm for mapping the environment in a global map.

**Figure 7.** Initial local map. (a) Indoor scene with obstacle, (b) blue cells represent free space, red cells represent obsta‐ cles and green cells are unknown space.

#### **4.2. Dynamic occupancy map**

**Figure 8** illustrates the block diagram for constructing the dynamic occupancy map. The measures obtained from the encoders in order to locate the robot in the global reference system. Once the current robot location in the environment is known, a homogenous transformation is carried out with the aim of updating the cells covered by the ultrasonic sensor based on the Eq. (7) and finally assigning them in the global map.

Note that, in the odometric localization block of **Figure 8**, the local reference axis of the robot has been rotated and angle *β* with respect to global axis. Therefore, one point (X1, Y1) in the local robot axis oriented with an angle *α* will be located in the global reference axis by means of:

$$
\begin{bmatrix} X \\ Y \\ 1 \end{bmatrix} = \begin{bmatrix} \cos \alpha & -\sin \alpha & O\_X \\ \sin \alpha & \cos \alpha & O\_Y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X\_1 \\ Y\_1 \\ 1 \end{bmatrix} \tag{8}
$$

Occupancy Map Construction for Indoor Robot Navigation http://dx.doi.org/10.5772/64871 79

**Figure 8.** Local and global reference axis for constructing the dynamic occupancy map.

#### **4.3. Updating the global map**

In this work, an initial local map is constructed since a static position of the robot in the environment which is represented by a grid as illustrated in **Figure 7b**. This map indicates the starting point of the robot trajectory and the corresponding state of the surrounding environ‐

minimizing the location errors due to the odometry of the robot. Note that, the red cells in the front and in both sides of the robot represent the obstacles in the scene (probability of the cells are higher to 0.5). This map is updated based on the log‐odds algorithm for mapping the

**Figure 7.** Initial local map. (a) Indoor scene with obstacle, (b) blue cells represent free space, red cells represent obsta‐

**Figure 8** illustrates the block diagram for constructing the dynamic occupancy map. The measures obtained from the encoders in order to locate the robot in the global reference system. Once the current robot location in the environment is known, a homogenous transformation is carried out with the aim of updating the cells covered by the ultrasonic sensor based on the

Note that, in the odometric localization block of **Figure 8**, the local reference axis of the robot has been rotated and angle *β* with respect to global axis. Therefore, one point (X1, Y1) in the local robot axis oriented with an angle *α* will be located in the global reference axis by means

> cos sin sin cos 1 0 0 11

a

a

*X O X Y O Y*

éù é - ùé ù êú ê úê ú <sup>=</sup> êú ê úê ú êú ê úê ú ëû ë ûë û

 a

 a

1 1

*X Y* of the environment

(8)

ment in accordance with Eq. (7). Each cell of the map represents 1 dm2

environment in a global map.

78 Robot Control

cles and green cells are unknown space.

**4.2. Dynamic occupancy map**

of:

Eq. (7) and finally assigning them in the global map.

One commonly used technique for updating the occupancy map is the Bayesian strategy, described in Eq. (10), which defines the probability P of the cell state *s*(*Ci* ) be occupied, given the distance (*d*) measured by the sensor at the time *t+1*.

$$P\left[\left.\mathbf{s}\left(C\_{l}\right) = OCC\right|\_{d\_{l+1}}\right] = \frac{P\left[\left.d\_{l+1}\right|\mathbf{s}\left(C\_{l}\right) = OCC\right]P\left[\mathbf{s}\left(C\_{l}\right) = OCC\right|d\_{l}\right]}{\sum\_{s\left(C\_{l}\right)}P\left[\left.d\_{l+1}\right|\mathbf{s}\left(C\_{l}\right)\right]P\left[\mathbf{s}\left(C\_{l}\right)\middle|d\_{l}\right]}\tag{9}$$

Each lecture provides partial information of the environment; therefore, in order to establish the state of each cell, an updated equation is used for combining the prior and the current probabilities of the cell, yielding:

$$P\left(C\_{l}\right) = P(C\_{l})\_{t-1} + P(C\_{l})\_{t} \tag{10}$$

#### **4.4. The log-odds algorithm**

Eq. (10) could give numerical instabilities for probabilities closer to 0 or 1. To overcome this problem, Thrun [13] propose that the status "occupied" for each cell *i* at instant time *t* can be modelled as the logarithm of an occupied cell, divided by the probability of the cell be empty and represented by:

$$I\_{t,l} = \log \frac{P\left[\left(C\_l\right) \mid d\right]}{1 - P\left[\left(C\_l\right) \mid d\right]}\tag{11}$$

The probability could be obtained, easily:

$$P\left(C\_i\right) = 1 - \frac{1}{1 - e^{\int\_{t,i}}} \tag{12}$$

Besides, it is considered that the prior value of the occupied cells <sup>0</sup> takes a constant value given by:

$$d\_0 = \log \frac{P(C\_l = l)}{P(C\_l = 0)} = \log \frac{P(C\_l)}{1 - P(C\_l)}\tag{13}$$

The algorithm for updating the occupancy map is described as follows:


#### **5. Path planning**

A safe and coherent navigation of the robot in his workspace requires a path planning technique and an obstacle avoidance strategy. In this project, the path planning technique used is the potential fields which basically consist in the computation of the attraction forces produced by the goal position and the repulsion forces caused by the closer objects in the scenario. Thus, an artificial potential field guides the robot to goal position, while the obstacle avoidance method allows a safe navigation of the robot.

**4.4. The log-odds algorithm**

The probability could be obtained, easily:

Algorithm 1. Updating the occupancy map

is in the vision field of the sensor then

and represented by:

80 Robot Control

by:

Inputs: {*lt-1,i*,

if Ci

else *lt,i=lt−1,i* end if end for

for each cell *Ci*

lt,i=lt‐1,i+lsensor\_model−l0

*r,z*}

**5. Path planning**

do

Eq. (10) could give numerical instabilities for probabilities closer to 0 or 1. To overcome this problem, Thrun [13] propose that the status "occupied" for each cell *i* at instant time *t* can be modelled as the logarithm of an occupied cell, divided by the probability of the cell be empty

( )

*PC d*

*i*

*e*


<sup>=</sup> = = = - (13)

<sup>0</sup> takes a constant value given

ë û

(11)

*PC d* é ù ë û <sup>=</sup> - é ù

( ) , <sup>|</sup> log 1 | *i*

( ) , <sup>1</sup> <sup>1</sup> <sup>1</sup> *t i <sup>i</sup> <sup>l</sup> P C*

( 1) ( ) log log ( 0) 1 ( ) *i i i i*

*P C P C*

A safe and coherent navigation of the robot in his workspace requires a path planning technique and an obstacle avoidance strategy. In this project, the path planning technique used

*PC PC <sup>l</sup>*

The algorithm for updating the occupancy map is described as follows:

= -

*t i*

*l*

Besides, it is considered that the prior value of the occupied cells

0

The potential field acting on the robot is given by the attraction field towards the goal and the repulsion field produced by the obstacles, that is:

$$U\left(q\right) = U\_{\rm att}(q) + U\_{\rm rep}(q)\tag{14}$$

**Figure 9.** Block diagram of the path planning algorithm.

In the same way, the forces could be separated in attraction and repulsion forces yielding:

$$F(q) = F\_{\text{att}}(q) - U\_{\text{rep}}(q) \tag{15}$$

Furthermore, the attraction force could be described as:

$$F\_{\rm att} \left( q \right) = -k\_{\rm att} \cdot \left| \left( q - q\_{\rm goal} \right) \right| \tag{16}$$

where is the Euclidean distance between <sup>∥</sup> <sup>∥</sup>, and the repulsion force could be described as:

$$F\_{\rm rep}(q) = \begin{cases} \frac{1}{2}k\_{\rm rep} \left(\frac{1}{\rho(q)} - \frac{1}{\rho\_0}\right) \frac{1}{\rho^2(q)} \frac{q - q\_{\rm obstacle}}{\rho(q)} & \text{si } \rho(q) \le \rho\_0 \\\\ 0 & \text{si } \rho(q) \ge \rho\_0 \end{cases} \tag{17}$$

where *k*att and *k*rep are positive scalar factors, is the minimal distance from *q* to the object and 0 is the distance in which an object influences in the path.

The general strategy for programming the path planning algorithm is illustrated in **Figure 9**. If the current robot position is known, then the resultant force is estimated considering all the new directions that the robot can take. Such directions are chosen based on the angle of observation of the ultrasonic sensor and the probable position of the robot if he walks in that specific direction. Once each force has been computed, the smaller is selected due to this force moves the robot closer to the goal. Once the new direction has been selected, the motors are turned on to follow this direction. The algorithm ends when the goal has been reached by the robot.

#### **6. Experimental results**

Several tests were performed to validate the path planning algorithm. In the tests, the mobile robot DaNI 2.0 navigates in an indoor environment with obstacles located between the starting and goal position. Both starting and goal positions are the same during the experiments and only the obstacle locations change. The graphical representation of the occupancy map is labelled as: green colour represents the unknown environment, blue cells represent free path (empty cells) and red cells represent obstacles (occupied cells).

The first experimental test is performed in an environment without obstacles; the results are illustrated in **Figure 10**. The odd rows of this figure show different instant times of the whole sequence performed by the robot; in particular, the first picture shows the starting position, and the ninth picture shows the goal position of the robot. Even rows depict the occupancy map constructed at that time. Note how effectively the obstacles detected are located in the borders of the region navigated by the robot, that is, any obstacle is detected in the middle of the scene as is expected.

In the same way, the forces could be separated in attraction and repulsion forces yielding:

where is the Euclidean distance between <sup>∥</sup> <sup>∥</sup>, and the repulsion force could

rep 0 2

 r

0 ( )

( ) ( ) obstacle

r

<sup>ì</sup> æ ö - <sup>ï</sup> ç ÷ - £ <sup>=</sup> <sup>í</sup> è ø <sup>ï</sup> <sup>³</sup> <sup>î</sup>

where *k*att and *k*rep are positive scalar factors, is the minimal distance from *q* to the object

The general strategy for programming the path planning algorithm is illustrated in **Figure 9**. If the current robot position is known, then the resultant force is estimated considering all the new directions that the robot can take. Such directions are chosen based on the angle of observation of the ultrasonic sensor and the probable position of the robot if he walks in that specific direction. Once each force has been computed, the smaller is selected due to this force moves the robot closer to the goal. Once the new direction has been selected, the motors are turned on to follow this direction. The algorithm ends when the goal has been reached by the

Several tests were performed to validate the path planning algorithm. In the tests, the mobile robot DaNI 2.0 navigates in an indoor environment with obstacles located between the starting and goal position. Both starting and goal positions are the same during the experiments and only the obstacle locations change. The graphical representation of the occupancy map is labelled as: green colour represents the unknown environment, blue cells represent free path

The first experimental test is performed in an environment without obstacles; the results are illustrated in **Figure 10**. The odd rows of this figure show different instant times of the whole sequence performed by the robot; in particular, the first picture shows the starting position,

2 () ( ) ( )

*q q <sup>k</sup> si q F q q q <sup>q</sup>*

Furthermore, the attraction force could be described as:

rep 0

1 111

rr

and 0 is the distance in which an object influences in the path.

(empty cells) and red cells represent obstacles (occupied cells).

be described as:

82 Robot Control

robot.

**6. Experimental results**

() () att rep *Fq F q U q* = - ( ) (15)

*F q k qq* att att goal ( ) =- × - ( ) (16)

*si q*

r

r

0

(17)

 r

 r

**Figure 10.** Test 1 of the path planning algorithm. The navigation is performed without obstacles. The graphical repre‐ sentation of the occupancy map is updating using the measurement obtained from the ultrasonic sensor, during robot navigation in real time.

The second experimental test is performed in an environment with two obstacles; the results are illustrated in **Figure 11**. In this case, the first picture shows the starting position, and the ninth picture shows the goal position of the robot. Note that the final occupancy map con‐ structed is smaller than the map of the test 1. The last is due to obstacles are detected therefore included as red cells; however, behind the object, even if there are a free space, this area is not reached for the ultrasonic sensor.

**Figure 11.** Test 2 of the path planning algorithm. The navigation is performed with two obstacles in the environment. The graphical representation of the occupancy map is updating using the measurements obtained from the ultrasonic sensor, during robot navigation in real time.

The results of the last experimental test showed here are illustrated in **Figure 12**. As in the test 2, the occupancy map is small with respect to the real environment due to zones behind objects cannot be not established by the robot.

**Figure 12.** Test 3 of the path planning algorithm. The navigation is performed with two obstacles in the environment at different position with respect to test 2. The graphical representation of the occupancy map is updating using the measurements obtained from the ultrasonic sensor, during robot navigation in real time.

#### **7. Conclusions and perspectives**

The second experimental test is performed in an environment with two obstacles; the results are illustrated in **Figure 11**. In this case, the first picture shows the starting position, and the ninth picture shows the goal position of the robot. Note that the final occupancy map con‐ structed is smaller than the map of the test 1. The last is due to obstacles are detected therefore included as red cells; however, behind the object, even if there are a free space, this area is not

**Figure 11.** Test 2 of the path planning algorithm. The navigation is performed with two obstacles in the environment. The graphical representation of the occupancy map is updating using the measurements obtained from the ultrasonic

The results of the last experimental test showed here are illustrated in **Figure 12**. As in the test 2, the occupancy map is small with respect to the real environment due to zones behind

reached for the ultrasonic sensor.

84 Robot Control

sensor, during robot navigation in real time.

objects cannot be not established by the robot.

The construction of occupancy map using ultrasonic sensors is easily perturbed by environ‐ mental noise. To overcome this constraint, it is used a Gaussian model of the sensor, providing higher precision of the distance measured for each obstacle in the scene. In addition, another typical error found in the measurements performed by ultrasonic sensors is related with the cone produced at the time of sending the ultrasonic signal. In this project, it was found that this error is less than 1°; therefore, it could be ignored. Furthermore, the error produced by the irregularities in the floor is reduced by using the proportional integral‐derivative (PID) control of the robot. Updating the global map requires the synchronization between the ultrasonic measures and the robot location in the environment.

The occupancy map of a real indoor environment is constructed by an ultrasonic sensor and a mobile robot. The logsig strategy allows a safe indoor navigation of our robot by establishing a probability of occupancy to each cell in the map avoiding a collision of the robot with an obstacle. The global strategy is performed in real time. The time employed for the robot for constructing the map depends of the number of obstacle in the scene; however, a real‐time execution of the platform is assured by using the VxWorks platform of the robot DaNI 2.0.

Future works are focused on multiple robot navigation on dynamic environments. To do this, a proposed approach consists in sending the local map constructed for one robot to a master terminal by IP connection in order to incrementally construct the global map by adding local maps of the environment.

#### **Author details**

Dora‐Luz Almanza‐Ojeda\* , Yazmín Gomar‐Vera and Mario‐Alberto Ibarra‐Manzano

\*Address all correspondence to: luzdora@ieee.org

Electronics Engineering Department, DICIS, University of Guanajuato, Salamanca, Guanajuato, México

#### **References**


[5] Thrun S., Burgard W., Fox D. Probabilistic Robotics. 1st ed. Cambridge, MA: The MIT Press; 2005. pp. 672 p.

irregularities in the floor is reduced by using the proportional integral‐derivative (PID) control of the robot. Updating the global map requires the synchronization between the ultrasonic

The occupancy map of a real indoor environment is constructed by an ultrasonic sensor and a mobile robot. The logsig strategy allows a safe indoor navigation of our robot by establishing a probability of occupancy to each cell in the map avoiding a collision of the robot with an obstacle. The global strategy is performed in real time. The time employed for the robot for constructing the map depends of the number of obstacle in the scene; however, a real‐time execution of the platform is assured by using the VxWorks platform of the robot DaNI 2.0.

Future works are focused on multiple robot navigation on dynamic environments. To do this, a proposed approach consists in sending the local map constructed for one robot to a master terminal by IP connection in order to incrementally construct the global map by adding local

Electronics Engineering Department, DICIS, University of Guanajuato, Salamanca,

Ediciones de la Universidad de la Mancha ; 2002. pp. 285 p.

p. 318–331. doi:10.1007/978‐3‐319‐18615‐3\_26

Doi : 10.3390/electronics40110082

and Microsystem. 2015;39(7):494–503. doi:10.1016/j.micro.2015.08.002

[1] Somolinos Sánchez J. A., editor. Advanced robotics and computer vision, 1st ed., Spain :

[2] Tavera‐Vaca C.‐A., Almanza‐Ojeda D.‐L., Ibarra‐Manzano M.‐A. Analysis of the efficiency of the census transform algorithm implemented on FPGA. Microprocessors

[3] Ventura R., Ahmad A. Towards Optimal Robot Navigation in Domestic Spaces. In: Reinaldo A. C., Bianchi, H. Levent Akin, Subramanian Ramamoorthy, Komei Sugiura, editors. RoboCup 2014: Robot World Cup XVIII. 1st ed. Switzerland: Springer; 2015.

[4] Martínez M. A., Martínez J. L., Morales J. Motion Detection from Mobile Robots with Fuzzy Threshold Selection in Consecutive 2D Laser Scans. Electronics, 2015 ; 4(1) : 82‐93.

, Yazmín Gomar‐Vera and Mario‐Alberto Ibarra‐Manzano

measures and the robot location in the environment.

maps of the environment.

Dora‐Luz Almanza‐Ojeda\*

\*Address all correspondence to: luzdora@ieee.org

**Author details**

86 Robot Control

Guanajuato, México

**References**


#### **Analysis of a Sorter Cascade Applied to Control a Wheelchair Analysis of a Sorter Cascade Applied to Control a Wheelchair**

Marcos Figueredo , Alexandre Nascimento , Roberto L.S. Monteiro and Marcelo A. Moret Marcos Figueredo, Alexandre Nascimento, Roberto L.S. Monteiro and Marcelo A. Moret

Additional information is available at the end of the chapter Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/63816

#### **Abstract**

The precise eye state detection is a fundamental stage for various activities that require human-machine interaction (HMI). This chapter presents an analysis of the implementation of a system for navigating a wheelchair with automation (CRA), based on facial expressions, especially eyes closed using a Haar cascade classifier (HCC). Aimed at people with locomotor disability of the upper and lower limbs, the state detection was based on two steps: the capture of the image, which concentrates on the detection actions and image optimization; actions of the chair, which interprets the data capture and sends the action to the chair. The results showed that the model has excellent accuracy in identification with robust performance in recognizing eyes closed, bypassing well occlusion issues and lighting with about 98% accuracy. The application of the model in the simulations opens the implementation and marriage opportunity with the chair sensor universe aiming a safe and efficient navigation to the user.

**Keywords:** wheelchair, classifier cascade, detector eyes closed, active vision, eyes state

#### **1. Introduction**

A number of illnesses and accidents can lead to severe damage in the spinal cord of a patient resulting in the loss of motion in the lower and upper parts. According to [1], 14% of the population has some kind of disability, be it visual, motor, hearing, and others, representing a growth of 7% in recent years.

Among this group, about 4% do not have any kind of movement in the lower and upper limbs. This is due to very serious motor problems, such as hemiplegia of four members and the need

© 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. © 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

for ventilatory support. Also applies to patients with degenerative diseases of the neuromuscular system, for example, amyotrophic lateral sclerosis (ALS) in which in a progressive manner the person loses his movements, until completely paralyzed, thereby causing death by respiratory failure.

Among the various types of motor disabilities that can affect a person, quadriplegia (motor disability of the four members) and the neuromotor system diseases such as ALS are serious deficiencies, which lead the individual to an almost vegetative state, with difficulties with integration into society as useful as a capable person. However, in most cases such individuals have full brain capacity, and with the necessary physical media, they may participate productively in society. Thus, it is necessary to find ways to develop their personal and professional skills and have a professional activity with human dignity.

Typically, this patient uses a wheelchair to perform tasks such as come and go, always with the aid of a carer or relative. Some of these use couplings that allow the user to their locomotion in their environment and in general are very invasive.

The works of [2–5] indicate the concern of the academic community in realizing technology stocks, low cost, that make the everyday life of these more independent people, and these studies also indicate that these actions prolong the lives of patients and improve their quality of life [6, 7].

**Figure 1.** CRA used as prototype.

Several studies have been conducted by different research groups [8–10] to develop wheelchairs with some kind of intelligence or are simply able to understand voice commands, autonomous locomotion, deviation obstructions, and other functions as outlined in [2]. These models have a high cost and maintenance, having little or no type of embedded technology.

Motivated by this reality began an interdisciplinary nature project that aims to provide a common wheelchair with elements that make it both possible and economically viable the mobility of a patient without movements of the upper and lower limbs. Patients with this degree of disability have only a means of interaction with the machine elements present in one's face, which will are the facial expressions and the device that will allow the mobility automation with wheelchair (CRA), as shown in **Figure 1**.

This navigation is limited in some aspects, as some movements of facial expressions were extremely tiring, such as opening and closing the mouth, and others were too difficult to be captured or required equipment that are still under development, such as the retina of the eye movement. So two expressions have proved less susceptible to failure, namely low intrusiveness and low learning curve. They are the opening and closing of the eyes and the movement of turning one's head to right and left. These expressions were evaluated by 20 individuals simulating the state of a quadriplegic and generated evaluation described in **Table 1**.


**Table 1.** Percentage of success and failure detection of facial expressions. Average score of the difficulty of learning to use facial expressions, and how this application was not intrusive or in a total of 20 individuals.

Hence, the model for navigating a CD is based on two expressions:

**•** Open and close your eyes (move the seat forward or stop);

for ventilatory support. Also applies to patients with degenerative diseases of the neuromuscular system, for example, amyotrophic lateral sclerosis (ALS) in which in a progressive manner the person loses his movements, until completely paralyzed, thereby causing death

Among the various types of motor disabilities that can affect a person, quadriplegia (motor disability of the four members) and the neuromotor system diseases such as ALS are serious deficiencies, which lead the individual to an almost vegetative state, with difficulties with integration into society as useful as a capable person. However, in most cases such individuals have full brain capacity, and with the necessary physical media, they may participate productively in society. Thus, it is necessary to find ways to develop their personal and professional

Typically, this patient uses a wheelchair to perform tasks such as come and go, always with the aid of a carer or relative. Some of these use couplings that allow the user to their locomotion

The works of [2–5] indicate the concern of the academic community in realizing technology stocks, low cost, that make the everyday life of these more independent people, and these studies also indicate that these actions prolong the lives of patients and improve their quality

Several studies have been conducted by different research groups [8–10] to develop wheelchairs with some kind of intelligence or are simply able to understand voice commands, autonomous locomotion, deviation obstructions, and other functions as outlined in [2]. These models have a high cost and maintenance, having little or no type of embedded technology.

Motivated by this reality began an interdisciplinary nature project that aims to provide a common wheelchair with elements that make it both possible and economically viable the mobility of a patient without movements of the upper and lower limbs. Patients with this degree of disability have only a means of interaction with the machine elements present in one's face, which will are the facial expressions and the device that will allow the mobility

skills and have a professional activity with human dignity.

in their environment and in general are very invasive.

automation with wheelchair (CRA), as shown in **Figure 1**.

by respiratory failure.

90 Robot Control

of life [6, 7].

**Figure 1.** CRA used as prototype.

**•** Turning the head to the left or right (moves the seat through 90° about the axis thereof)

This work aims to present the architecture to detect the action of opening and closing the eyes based on HCC characteristics and evaluate the performance of the detector according to efficiency and effectiveness.

The effectiveness of the detector was tested in the prototype, evaluating their response to commands, since its effectiveness was evaluated from:


According to [11], the model [12] uses a classifier cascade and features of HCC. It works great on face detection and has already become standard for its high hit rate and low false positive rate.

The model [12] is adaptive and widely used by its degree of robustness and speed. In our implementation, the detection is based on HCC, with parameters evaluated, and a positive bank with about 10,000 images of frontal eyes closed, as shown in **Figure 2**.

This chapter is organized as follows: Section 2 presents the state of the art related to work developed here as well as our contribution. Section 3 describes the materials and the method used for the realization and implementation of the architecture. Section 4 presents the tests and results. Section 5 in turn brings the conclusions and future work.

**Figure 2.** Sample det positive.

#### **2. State of the art**

According to [2], there are more than 35 CRI projects around the world, all of these projects vary in many ways. Initially, they include the project IntellWheels [2] which aims to create a development platform for intelligent wheelchairs, facilitating the design and testing of new methods and techniques for the CRIs, which takes as its premise aspects of low cost, comfort, and ergonomics.

Elizabeth and Roger [13] propose a seat which makes use of the head movement to perform navigation with a video system and a helmet with sensors. The study by [14] provides that the construction of an ellipsoidal 3D model of the head interprets the flow of this movement and establishes, according to the author, a more effective methodology than 2D approach.

Based on the Kalman filter to predict the possible movement of the head [15], attempts to reduce the action of the head movement assuming that with few moves the chair can now follow a route estimated by this position. This action, according to the author, reduces user effort.

Taylor and Nguyen [16] and Nguyen et al. [17] use similar ideas to use sensors placed on the user's head (especially accelerometers) to detect motion in a system designed to free platform. Wei-Kai and Isaac [18] address the recognition of facial gestures in interactive environments and aims to recognize the gestures through points of interest in the face of the individual by using Active Shape Models to extract face features to then assemble a 3D model in line with Actinos Facial Coding System (FACS).

Manogna et al. [19] create a system to directly control the engine speed based on the movement of the head. The device is fixed on the patient's head and sensor-based motion produces CRI. The same study was performed by [20], who evaluated a similar device in a closed environment. It describes the difficulty in understanding the face and his intentions and suggests using the same type of sensor.

The study by [19, 20] has been updated with the study by [21] that detects the opening and closing of the mouth for extra movement and studying the problem of head ergonomics. Zheng et al. [5] describe the Lucas-Kanade algorithm for detecting facial movement and discusses a method efficiency, accuracy, and response time.

Control of the CRA's status based on the opening and closing of the eyes is a simple method used to control the same as the state of the art analyzed but widely applied in research-related fatigue drivers to avoid traffic accidents.

Martin and colleagues [22, 23] describe a system that locates and tracks your eyes for algebraic operations on the face of the image extracted by the method of [20], which notably is also used in [24–30]. In all, eyes are found from the face detection which reduces the search space and facilitates significantly the detection task to then perform detection. Some utilize filters that improve the detection condition.

It is noticed that the utilization of computer vision for CRI movement was gradual. It was also almost constant that the use of accelerometers or cameras on the individual's head makes the equipment very intrusive and requires specific training to use the hardware and therefore difficult to adapt. The hit rates always fluctuate above 90%, but these mentioned, none is to analyze the influence of detector creation parameters. The majority does not have a specific database for the purpose and generally uses the same standard database available on the Web.

The contribution of this work is focused on the identification of the influence of the classifier training parameters and the relationship between complexity and efficiency/effectiveness of the detector.

Formation of a database of 10,000 images and analyze the influence of regionalism in the detector as opposed to embedded detectors in public libraries. The analysis of processing time both the detector's response to its creation.

## **3. Materials and method**

The model for the detection of eye state uses a simple idea. If a detector can detect a human eye on a front face then it must also recognize one eye closed. To this end, a trained classifier cascade is used to identify this object in the input image. The choice of [12] model mainly provides for its simplicity, speed of execution, and the outstanding performance [8]. The method basically combines four key concepts:


used for the realization and implementation of the architecture. Section 4 presents the tests

According to [2], there are more than 35 CRI projects around the world, all of these projects vary in many ways. Initially, they include the project IntellWheels [2] which aims to create a development platform for intelligent wheelchairs, facilitating the design and testing of new methods and techniques for the CRIs, which takes as its premise aspects of low cost, comfort,

Elizabeth and Roger [13] propose a seat which makes use of the head movement to perform navigation with a video system and a helmet with sensors. The study by [14] provides that the construction of an ellipsoidal 3D model of the head interprets the flow of this movement and

Based on the Kalman filter to predict the possible movement of the head [15], attempts to reduce the action of the head movement assuming that with few moves the chair can now follow a route estimated by this position. This action, according to the author, reduces user effort.

Taylor and Nguyen [16] and Nguyen et al. [17] use similar ideas to use sensors placed on the user's head (especially accelerometers) to detect motion in a system designed to free platform. Wei-Kai and Isaac [18] address the recognition of facial gestures in interactive environments and aims to recognize the gestures through points of interest in the face of the individual by using Active Shape Models to extract face features to then assemble a 3D model in line with

Manogna et al. [19] create a system to directly control the engine speed based on the movement of the head. The device is fixed on the patient's head and sensor-based motion produces CRI. The same study was performed by [20], who evaluated a similar device in a closed environment. It describes the difficulty in understanding the face and his intentions and suggests using

The study by [19, 20] has been updated with the study by [21] that detects the opening and closing of the mouth for extra movement and studying the problem of head ergonomics. Zheng et al. [5] describe the Lucas-Kanade algorithm for detecting facial movement and discusses a

establishes, according to the author, a more effective methodology than 2D approach.

and results. Section 5 in turn brings the conclusions and future work.

**Figure 2.** Sample det positive.

92 Robot Control

**2. State of the art**

and ergonomics.

Actinos Facial Coding System (FACS).

method efficiency, accuracy, and response time.

the same type of sensor.


The combination of these ideas permits simultaneous selection of key features and trains the classifier cascade, and then next steps will be described.

#### **3.1. Features of Haar**

The Haar features encode the existence of contrasts between the targeted regions of the image. A set of these resources is used to encode the contrasts displayed by a human face, on this work, eyes closed, and their spatial relationships. These characteristics are called Haar, because its concept is similar to the coefficients of the Haar wavelet, set in a detection window W×H pixels according to the formula:

$$\text{features} = \sum\_{i=1}^{N} o\_i \text{RecSum} \left( r\_i \right) \tag{1}$$

where *ωi* is arbitrarily singling as weight factor, and ) is the sum of the intensities of the pixels, which was described by [12] as a full image. The rectangle r\_i and described as a function of five parameters:*r = (x, y, w, h, ϕ)*, where *x, y* are the coordinates of the top position of the pixel array, *w* and *h* define the dimensions of the rectangle, and *ϕ = {0°, 45°}* represents the degree of rotation.

The presence of a characteristic Haar, **Figure 3**, is determined by subtracting the average pixel value of the region by the average pixel value of the clear area. If the difference is above a threshold (set during learning), the characteristic is present.

**Figure 3.** Some features of Haar and the detection window.

Paul and Michael [12] reported the fact that the choice of the characteristics of use, rather than models based on pixel points of the image statistics, is important because of the benefits of the ad hoc domain knowledge, which can be extracted knowledge hidden in images hardly found in a finite set of training.

In the case of blink detection, this fact is used to represent the approximate information and also related to the test image backgrounds. This knowledge becomes very thin with respect to open and closed eye, the use of the two is hardly found in other models with appearance-based approach.

In general, therefore, the characteristics are nothing more than rating information for a set of light intensity of a pixel. This process consists of the sum of the intensity of the pixels of white regions of characteristics and the intensity subtracted from the sum of the gray balance of the image. The results are used as the characteristic value of a given location and can be combined to form weak hypotheses in the images [31].

Typically, the model adopts the rectangles seen in **Figure 3** and determines the presence or absence of thousands of Haar features in each image position and with different scales, Paul and Michael [12] used a technique called integral image.

#### **3.2. Full image**

work, eyes closed, and their spatial relationships. These characteristics are called Haar, because its concept is similar to the coefficients of the Haar wavelet, set in a detection window W×H

of the pixels, which was described by [12] as a full image. The rectangle r\_i and described as a function of five parameters:*r = (x, y, w, h, ϕ)*, where *x, y* are the coordinates of the top position of the pixel array, *w* and *h* define the dimensions of the rectangle, and *ϕ = {0°, 45°}* represents

The presence of a characteristic Haar, **Figure 3**, is determined by subtracting the average pixel value of the region by the average pixel value of the clear area. If the difference is above a

Paul and Michael [12] reported the fact that the choice of the characteristics of use, rather than models based on pixel points of the image statistics, is important because of the benefits of the ad hoc domain knowledge, which can be extracted knowledge hidden in images hardly found

In the case of blink detection, this fact is used to represent the approximate information and also related to the test image backgrounds. This knowledge becomes very thin with respect to open and closed eye, the use of the two is hardly found in other models with appearance-based

In general, therefore, the characteristics are nothing more than rating information for a set of light intensity of a pixel. This process consists of the sum of the intensity of the pixels of white

1

=

*i* w

is arbitrarily singling as weight factor, and

*N*

features

threshold (set during learning), the characteristic is present.

**Figure 3.** Some features of Haar and the detection window.

in a finite set of training.

approach.

( )

<sup>=</sup> å (1)

) is the sum of the intensities

*i i*

*RecSum r*

pixels according to the formula:

where *ωi*

94 Robot Control

the degree of rotation.

The complete image created from the original image, a new representation of the image, simply sums the values of each pixel to the left and above, inclusively. The idea to use this representation is to increase the speed of feature extraction, as any rectangle of an image can be calculated by means of this idea. Only four indices are required to calculate any rectangle, and as an immediate consequence, one needs only one pass for desired data in subregions of an image, see **Figures 4** and **5**.

**Figure 4.** Representation of full image: (a) area calculation, (b) sum of areas A−B−C+D, (c) rationed area calculation, (d) quick sum A−B−C+D.


**Figure 5.** Integral calculus image representation. It should be noted that the sum of the region (a) is equal to seven in (b) represented as 108−73−80+52.

#### **3.3. Adaboost**

The source Boosting problem in computational area, known as machine learning, can be exposed informally as follows: suppose there is a sorting method which is slightly better than a random choice for any distribution X, called weak learner or weak classifier. The existence of a classifier weak implies the existence of a strong classifier (strong learner), with small error on the entire space X.

In statistics, it is asked if given a reasonable estimation method, you can get a method close to great. This problem was solved by [32], which presented an algorithm that transforms a weak classifiers into a strong classifier.

From then on, several algorithms were developed within the context of boosting. One of the most recent and successful algorithms is known as AdaBoost that comes from the fact that boosting generates in every step a distribution on the observations of the sample and gives greater weight (most likely to be in the disturbed sample) to misclassified observations in the previous step. The basic algorithm is shown in **Figure 6**.

**Figure 6.** Adaboost algorithm.

In this sense, AdaBoost is focused on the bad ratings, or else the data difficult to classify, and this is the main feature of this algorithm: minimize error over a training set. One of the advantages of AdaBoost, as studied by [33, 34], is the existence of other parameters, in addition to T shifts, to improve learning.

The result, after successive iterations of the algorithm, a set of hypotheses with weights wherein those having lower classification errors become more important, is called strong hypothesis or strong classifier.

#### **3.4. Classifier cascaded**

Increasing the speed of a classification task, in general, results in an increase in errors associated. However, for this purpose to be effective, we would have to reduce the number of evaluation of the weak classifiers, which would result in a loss in accuracy of the system. So Paul and Michael [12] propose a degenerative tree decision, decision stump, the structure that contains the binder thread from general to more specific, according to which the first cascade levels are not very accurate, although able to sort a large number of samples with a small amount of characteristics.

The use of cascade is characterized by the fact that, in an image during a detection task, the most sub-window analyzed by classifier is rejected. For this reason, a generalization in the early stages should be high enough to avoid the transition to subsequent stages of subwindows classified as false positive [35], as shown in **Figure 7**.

**Figure 7.** Estructure of the classifier.

#### **3.5. The chair**

a random choice for any distribution X, called weak learner or weak classifier. The existence of a classifier weak implies the existence of a strong classifier (strong learner), with small error

In statistics, it is asked if given a reasonable estimation method, you can get a method close to great. This problem was solved by [32], which presented an algorithm that transforms a weak

From then on, several algorithms were developed within the context of boosting. One of the most recent and successful algorithms is known as AdaBoost that comes from the fact that boosting generates in every step a distribution on the observations of the sample and gives greater weight (most likely to be in the disturbed sample) to misclassified observations in the

In this sense, AdaBoost is focused on the bad ratings, or else the data difficult to classify, and this is the main feature of this algorithm: minimize error over a training set. One of the advantages of AdaBoost, as studied by [33, 34], is the existence of other parameters, in addition

The result, after successive iterations of the algorithm, a set of hypotheses with weights wherein those having lower classification errors become more important, is called strong hypothesis

Increasing the speed of a classification task, in general, results in an increase in errors associated. However, for this purpose to be effective, we would have to reduce the number of evaluation of the weak classifiers, which would result in a loss in accuracy of the system. So Paul and Michael [12] propose a degenerative tree decision, decision stump, the structure that contains the binder thread from general to more specific, according to which the first cascade levels are not very accurate, although able to sort a large number of samples with a small

The use of cascade is characterized by the fact that, in an image during a detection task, the most sub-window analyzed by classifier is rejected. For this reason, a generalization in the

on the entire space X.

96 Robot Control

**Figure 6.** Adaboost algorithm.

or strong classifier.

**3.4. Classifier cascaded**

amount of characteristics.

to T shifts, to improve learning.

classifiers into a strong classifier.

previous step. The basic algorithm is shown in **Figure 6**.

One of the general proposed projects involves low cost as the minimum use of equipment is attached to the chair. This comprises only a webcam coupled to the PC and a Kinect® sensor (see **Figure 8**) coupled to the associated chair or independently allows the model to obtain various environmental information capable of determining actions of the chair, for example the sudden passage of someone in front of the same or the proximity of an obstacle.

A notebook with processor 1.6 GHz dual-core Intel Core i5 (Turbo Boost up to 2.7 GHz) with 3 MB and 4 GB of memory was used. The connection between the PC and the chair uses an ATmega328 microcontroller board (arduino UNO), which has 14 digital pins input/output, 6 analog inputs, and USB connection.

#### **3.6. Architecture**

The software implements the model, which seeks, from the detection of the eye status (open, closed) to inform the chair the action that should be taken especially to move forward or stop. The command "move on" triggered when the user closes his eyes unnaturally or stays with the eye closed for a period of time greater than 2 s, in about 10 frames, is analyzed, and if the detection occurs in a percentage above 90%, a command is sent, otherwise nothing is done. The command acts in a similar way as opposed to the motion to move forward as a way to clear up this script diagrammed in **Figure 9**. It was found that according to [36], the human eye takes 280 ms to blink.

In the proposed architecture, **Figure 9**, CAM/frame image captures through a common webcam, performs the face detection, and focuses on this area of interest; then the image is treated to minimize or neutralize lighting noise and send the information for treatment. In CHAIR ACTION, capturing is carried out prior to information and detects by means of an algebraic operation described in [36], the region where the eyes in the face are properly separated and sent to analyze the state close left eye (CL) and close right eye (CR). In the first step, the image is captured and represented in gray tones; there are 28 frames per second. In each, we use facial detector defined in [12] widely used and recognized efficiency, according to [37]. Around this region of interest are made two image optimization operations:


**Figure 10.** Eye detection structure.

Filtering through the algorithm in [38] called retinal filter cancels much of the image distortion and improves cleaning detail, even in low light (<100 lux) also keeps the naturalness. After correction of the image, an algebraic operation is made to detect the eyes of the person in the image. The completion of this step proved to be quite efficient with the use in [27] or [28] due to the fact that even after correction of the image both methods provide great instability regarding the detection of the eyes, even using different parameters as those used for the same. The extraction assumes that the person is in front of the camera and below the horizontal line of the eyes, while maintaining a fixed distance to the camera in a variable angle between **30 <** *θ* **< 70** as described in **Figure 10**.

#### **3.7. Kinematic model and motion control**

the eye closed for a period of time greater than 2 s, in about 10 frames, is analyzed, and if the detection occurs in a percentage above 90%, a command is sent, otherwise nothing is done. The command acts in a similar way as opposed to the motion to move forward as a way to clear up this script diagrammed in **Figure 9**. It was found that according to [36], the human

In the proposed architecture, **Figure 9**, CAM/frame image captures through a common webcam, performs the face detection, and focuses on this area of interest; then the image is treated to minimize or neutralize lighting noise and send the information for treatment. In CHAIR ACTION, capturing is carried out prior to information and detects by means of an algebraic operation described in [36], the region where the eyes in the face are properly separated and sent to analyze the state close left eye (CL) and close right eye (CR). In the first step, the image is captured and represented in gray tones; there are 28 frames per second. In each, we use facial detector defined in [12] widely used and recognized efficiency, according

to [37]. Around this region of interest are made two image optimization operations:

Filtering through the algorithm in [38] called retinal filter cancels much of the image distortion and improves cleaning detail, even in low light (<100 lux) also keeps the naturalness. After correction of the image, an algebraic operation is made to detect the eyes of the person in the image. The completion of this step proved to be quite efficient with the use in [27] or [28] due

eye takes 280 ms to blink.

98 Robot Control

**Figure 9.** How to detect closed eyes.

**•** Inversion;

**•** Retinal filter.

**Figure 10.** Eye detection structure.

The kinematic model of the chair is presented as a process in which each wheel contributes both to the movement of the chair as to his/her mistake notably associated with obstacles and soil deformation. While our research ambience is indoors and appropriate to the patient, these errors can be minimized with the use of inexpensive sensors that can identify and predict possible problems in navigation.

Here we describe the mathematical idea of the chair movement that follows a traditional model of representation of the world. This model, whose movement and orientation are performed by two independent actuators, considers the rectangular object moving at speed *V*. The state plan of the chair in the Cartesian plane (*x, y*) is defined by the vector:

$$\left(\mathbf{x}\_{\varepsilon}, \mathbf{y}\_{\varepsilon}, \theta, \mathbf{v}\_{\varepsilon}, \mathbf{o}\_{\varepsilon}\right)^{\mathsf{t}}\tag{2}$$

where *xc* and *yc*, are the coordinates of the central point of the wheel axle, **θ** is the angle formed between the base of the chair *C*(*xc*, *yc*), *vc* is the linear velocity at the point C, and *ωc* is the angular velocity,, as described in **Figure 11**.

**Figure 11.** Cartesian representation of the chair.

The chair should move only in the direction normal to the axis of the driving wheels and can restrict the analytic relationship:

$$
\sqrt{\cos \theta - \mathbf{x}' \sin \theta} \tag{3}
$$

Based on the information that is obtained during the navigation, the calculation of the linear speed of each wheel is deducted for subsequent adjustment and especially in cases of unevenness through a relation between the number of pulses of the encoder N and its sampling period T.

$$\nu = \frac{N\pi D}{R\_e T} \tag{4}$$

where *v* and *D* are, respectively, the linear velocity and the diameter of the wheel, and *Re* is the encoder resolution. A possibility of representation of state variables is based on the speed at the contact point between the right wheel (*vD*) and left (*vE*) with the floor.

$$\left(\mathbf{x}\_c, \mathbf{y}\_c, \theta, \mathbf{v}\_D, \mathbf{v}\_E\right)^T \tag{5}$$

The choice of this form of representation is essentially the ease of mediating these quantities by odometry system.

Considering the continuous system, we have:

$$\begin{cases} \mathbf{v}\_D = \left(L + \frac{b}{2}\right) a = r\_D a o\_D \\ \mathbf{v}\_E = \left(L - \frac{b}{2}\right) a = r\_E o\_E \end{cases} \tag{6}$$

Besides that

$$\begin{cases} \mathbf{v}\_D = \mathbf{v} + \left(\frac{b}{2}\right)a \\\\ \mathbf{v}\_E = \mathbf{v} - \left(\frac{b}{2}\right)a \end{cases} \tag{7}$$

Also,

#### Analysis of a Sorter Cascade Applied to Control a Wheelchair http://dx.doi.org/10.5772/63816 101

$$\begin{cases} \mathbf{v}\_D + \mathbf{v}\_E = \mathbf{2}\alpha r = \mathbf{2}\mathbf{v} \\ \mathbf{v}\_D - \mathbf{v}\_E = \alpha L \end{cases} \tag{8}$$

or alternatively

Based on the information that is obtained during the navigation, the calculation of the linear speed of each wheel is deducted for subsequent adjustment and especially in cases of unevenness through a relation between the number of pulses of the encoder N and its

*e*

( , ,, , )

2

*<sup>b</sup> vL r*

<sup>ì</sup> æ ö <sup>ï</sup> =+ = ç ÷ ïè ø

<sup>ï</sup> æ ö =- = ç ÷ ïî è ø

*D*

í

*v v*

<sup>ì</sup> æ ö <sup>ï</sup> = + ç ÷ ï èø

<sup>ï</sup> æ ö = - ç ÷ ïî è ø

*v v*

*E*

*<sup>b</sup> vL r*

*D D D*

w

w

2

w

w

*b*

2

*b*

 w

 w

2

*E E E*

*c c DE xy vv* q

where *v* and *D* are, respectively, the linear velocity and the diameter of the wheel, and *Re* is the encoder resolution. A possibility of representation of state variables is based on the speed at

The choice of this form of representation is essentially the ease of mediating these quantities

*T*

*R T* p

*N D*

= (4)

(5)

(6)

(7)

*v*

the contact point between the right wheel (*vD*) and left (*vE*) with the floor.

sampling period T.

100 Robot Control

by odometry system.

Besides that

Also,

Considering the continuous system, we have:

í

$$\begin{cases} \nu = o\_D \frac{r\_D}{2} + o\_E \frac{r\_E}{2} \\\\ o = o\_D \frac{r\_D}{2} + o\_E \frac{r\_E}{b} \end{cases} \tag{9}$$

Consider *b* the distance between the contact points of the wheels, rD radius of the wheels, *L* the distance from point *C* to the center of rotation of the chair, and , , the angular velocities of the right, left, and center wheel movement of the chair.

From the speeds of the wheels of the robot, we may calculate the linear and angular velocities.

$$\begin{cases} \nu = \frac{\nu\_D + \nu\_E}{\mathfrak{Q}} \\\\ \phi = \frac{\nu\_D - \nu\_E}{b} \end{cases} \tag{10}$$

To find out its position in the reference plane, we must know the chair state space, and how it will evolve over time with the *vD* and *vE* speeds.

Considering the condition called no slip can descrier the kinematic equations of motion of point *C* with respect to the linear (*v*) and angular (*ω*) velocity [4, 26]:

$$\begin{cases} \mathbf{x}\_c = \nu \cos \theta \\ \mathbf{y}\_c = \nu \sin \theta \\ \theta = \alpha \end{cases} \tag{11}$$

or matrix form:

$$
\begin{bmatrix} \boldsymbol{x}\_c \\ \boldsymbol{\nu}\_c \\ \boldsymbol{\theta} \end{bmatrix} = \begin{bmatrix} \cos \boldsymbol{\theta} & \mathbf{0} \\ \sin \boldsymbol{\theta} & \mathbf{0} \\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol{\nu} \\ \boldsymbol{\nu} \\ \boldsymbol{\theta} \end{bmatrix} \tag{12}
$$

#### **4. Testing and results**

The performance of the closed eye detection system was built based on a set of 10,000 images of various persons. Images were acquired in a controlled lighting environment with 2048 × 1536 pixels in JPG format. For accurate detection classifier, various parameters can be changed during the training process. The influence of these parameters changes the complexity of weak classifiers, and therefore aspects as positive and false positives are influenced.

The standard size entry was set to 24 × 24 pixels. All images in our base were taken in the same, uniform background. Immediately, we wanted to see if the negative assembly constructed from the same occluded images with faces is sufficient to distinguish between open and closed eyes; using the classifier with default parameters, the following results were obtained, as described in **Figure 11**.

**Figure 12.** Comparison between detector 1 taken with random sample negatives and diverse background and using samples only performing the occlusion of the region of interest.

This analysis would serve to decrease the cost of both image search optimization and scheduling but has not proven satisfactory for such small images. Then the other negative training set was created, gathered randomly about 9000 different images that do not contain any references to human eyes. **Figure 12** shows some examples of negative training sets. As Rainer and Jochen [39] showed, the version of AdaBoost gave best results for facial detection, Gentle AdaBoost, with a ratio of false positives required cascades set to 10e−6.

Due to small size of the training image, we limited rectangle Haar that was used. Although the number of ways in which the rectangles may be arranged is large, for practical reasons, we limited the time with the following steps:


A total of 408,564 characteristic Haar were obtained by imaging under the above conditions with a satisfactory number of resources. For this problem, four detectors were used which were constructed and differed primarily by the type of boost used and parameter variation as described in **Table 2**, wherein MinHitRate is the minimum hit rate desired for each phase of the classifier; MaxFalseAlarm is the maximum false alarm rate desired for each phase of the classifier; Nstages is the number of cascaded stages; BTYPE is the kind of boost used (type of boosted classifiers: DAB— Discrete AdaBoost, RAB—Real AdaBoost, LB—LogitBoost, GAB— Gentle AdaBoost); WTRate is the cutting line and the weight used in the boost; and Wcout is the maximum count of false trees for all stages of the cascade.


**Table 2.** Presentation of parameters and classifiers.

As shown in **Figure 13**, the results are established with great parameter, and the results are described in **Table 3**.

**Figure 13.** Negative sample.

cos 0 sin 0

q

éùé ùé ù êúê ú <sup>=</sup> ê ú êúê úë û êúê ú

ëûë û

classifiers, and therefore aspects as positive and false positives are influenced.

q

*c c*

*x*

*y*

q

**4. Testing and results**

102 Robot Control

bed in **Figure 11**.

0 1

The performance of the closed eye detection system was built based on a set of 10,000 images of various persons. Images were acquired in a controlled lighting environment with 2048 × 1536 pixels in JPG format. For accurate detection classifier, various parameters can be changed during the training process. The influence of these parameters changes the complexity of weak

The standard size entry was set to 24 × 24 pixels. All images in our base were taken in the same, uniform background. Immediately, we wanted to see if the negative assembly constructed from the same occluded images with faces is sufficient to distinguish between open and closed eyes; using the classifier with default parameters, the following results were obtained, as descri-

**Figure 12.** Comparison between detector 1 taken with random sample negatives and diverse background and using

This analysis would serve to decrease the cost of both image search optimization and scheduling but has not proven satisfactory for such small images. Then the other negative training set was created, gathered randomly about 9000 different images that do not contain any references to human eyes. **Figure 12** shows some examples of negative training sets. As Rainer and Jochen [39] showed, the version of AdaBoost gave best results for facial detection, Gentle

samples only performing the occlusion of the region of interest.

AdaBoost, with a ratio of false positives required cascades set to 10e−6.

*v*

w

(12)


**Table 3.** Presentation of results and better processing time.

**Figure 14** shows the difference between the result of the public detector and the detector with better response. Both have a good recognition rate, but positive for higher rates against false positives makes the classifier "olhosfechadosGAB" provide better performance.

**Figure 14.** ROC graph with the best parameters and analysis for different versions of AdaBoost.

Simulating what will happen directly in the chair is crucial. Therefore there were two tests: the first with 12 volunteers, conducted during the same period of time (2 min) recording a video with one's image of the front face held at specified times to opening and closing the eyes, with the following results described in **Figure 15**.

**Figure 15.** Comparison of our classifier and public distribution.

**Parameter Sorter 1 Sorter 2 Sorter 3 Sorter 4** MinHitRate 0.999 0.987 0.985 0.999 MaxFalseAlarm 0.5 0.5 0.4 0.5 Nstages 20 25 20 25 Btype GAB RAB LB DAB WTRate 0.97 0.98 0.95 0.95 Wcount 100 100 100 100 Processing time 4 days 5 days 4 days 7 days

**Figure 14** shows the difference between the result of the public detector and the detector with better response. Both have a good recognition rate, but positive for higher rates against false

positives makes the classifier "olhosfechadosGAB" provide better performance.

**Figure 14.** ROC graph with the best parameters and analysis for different versions of AdaBoost.

the following results described in **Figure 15**.

Simulating what will happen directly in the chair is crucial. Therefore there were two tests: the first with 12 volunteers, conducted during the same period of time (2 min) recording a video with one's image of the front face held at specified times to opening and closing the eyes, with

**Table 3.** Presentation of results and better processing time.

104 Robot Control

In the second test, the same group of volunteers held navigation CD from point A to point B, which would use a region of 1 square meter stopping place, as shown in **Figure 16**.

**Figure 16.** Result detection of volunteers.

Volunteers can perform spins of the head that do not exceed 30 degrees, in an environment with good lighting conditions (above 200 lux). In carrying out the detection was operated with frame size 640 by 320 with 28 FPS. In all, the volunteers were able to make the journey without difficulty with stopped or early movement. During the test, we observed the total time hardware response to commands, and these times ranged from 250 to 300 ms.

## **5. Conclusion**

The tests clearly demonstrated that HCC can be successfully used in a blink detection system, and the combination of the classifier set to closed eyes and face resulted in a fast and efficient system.

The trained detector (one classifier) with the parameters described in **Table 2** exceeded the detector OpenCV framework proposed by both the rate of detection and computational efficiency. In this study, we were able to detect approximately 98% of the results with about 9% false positives.

The results showed that the use of regionalized data enables more efficient detection. We observed that the detector does not fail to be submitted to the people with whom he/she was trained. To set a robust system, the patient must have a face image trained classifier with a significant number of possessions.

#### **Figure 17.** Conducted in chair.

The average error was 0.058, while applying the minimum neighboring detector returned a number of 90% positive, while for the maximum number of neighbors obtained 98% as detection windows scan across the image with the maximum neighboring the intersection between the windows is unique and therefore there is no possibility of true or false.

Another crucial point is the detector oscillation while the individual closes and opens his eyes. This set of errors stabilizes in a few milliseconds (would not be a problem if this time (**Figure 17**) did not result in "leaps" in the chair). The solution to this problem was given by using a waiting time for stabilization of detection (about 1 s), and thereafter the chair had its start and stop smoothly performed.

Using our PC, it was possible to obtain an average rate of 280 ms detector response, and the architecture proved to be quite stable, no crash or time-consuming to user responses. Individuals engaged in the work reported that it would not require extensive training to use the chair and (after 30 min of test) and did not experience discomfort when using the system (**Figure 18**).

**Figure 18.** Detector response analysis.

As future work, we highlight the implementation of the navigation control with the head that will allow the user to perform turns and spins with the chair and the analysis of all these ideas applied to a larger architecture and its integration with other low-cost sensors that allow to bypass obstacles.

#### **Author details**

Volunteers can perform spins of the head that do not exceed 30 degrees, in an environment with good lighting conditions (above 200 lux). In carrying out the detection was operated with frame size 640 by 320 with 28 FPS. In all, the volunteers were able to make the journey without difficulty with stopped or early movement. During the test, we observed the total time

The tests clearly demonstrated that HCC can be successfully used in a blink detection system, and the combination of the classifier set to closed eyes and face resulted in a fast and efficient

The trained detector (one classifier) with the parameters described in **Table 2** exceeded the detector OpenCV framework proposed by both the rate of detection and computational efficiency. In this study, we were able to detect approximately 98% of the results with about

The results showed that the use of regionalized data enables more efficient detection. We observed that the detector does not fail to be submitted to the people with whom he/she was trained. To set a robust system, the patient must have a face image trained classifier with a

The average error was 0.058, while applying the minimum neighboring detector returned a number of 90% positive, while for the maximum number of neighbors obtained 98% as detection windows scan across the image with the maximum neighboring the intersection

between the windows is unique and therefore there is no possibility of true or false.

hardware response to commands, and these times ranged from 250 to 300 ms.

**5. Conclusion**

9% false positives.

significant number of possessions.

**Figure 17.** Conducted in chair.

system.

106 Robot Control

Marcos Figueredo1\*, Alexandre Nascimento2 , Roberto L.S. Monteiro1,3 and Marcelo A. Moret3,4

\*Address all correspondence to: mbfigueredo@uneb.br

1 Department of Exact Sciences and Earth—DECET II, College of Information Systems— Alagoinhas, State University of Bahia—UNEB, Bahia, Brazil

2 College ÁREA1, Salvador, Bahia, Brazil

3 Doctoral Program in Computational Modeling and Industrial Technology, College of SEN-AI-CIMATEC Technology, Salvador, Bahia, Brazil

4 Department of Exact Sciences and Earth—DCET, Campus I, State University of Feira de Santana—UESF, Salvador, Bahia, Brazil

## **References**


[14] BASU, Sumit; ESSA, Irfan; PENTLAND, Alex. Motion regularization for model-based head tracking. In: Proceedings of the IEEE International Conference on Pattern Recognition (ICPR '96). Vienna, Austria, 1996, 3, pp. 611–616.

**References**

108 Robot Control

[1] Instituto Brasileiro de Geografia e Estatística—IBGE. Características da população e dos domicílios resultados do universo, Censo Demográfico 2010, vol. 1, p. 161, 2011.

[2] BRAGA, Rodrigo António Marques et al. Plataforma de desenvolvimento de cadeiras de rodas inteligentes. PhD, Thesis, Faculty of Engineering, University of Puerto, PhD

[3] HALAWANI, Alaa et al. Active vision for controlling an electric wheelchair. Intelligent

[4] SONG, You; LUO, Yunfeng; LIN, Jun. Detection of movements of head and mouth to provide computer access for disabled. In: 2011 International Conference on Technologies and Applications of Artificial Intelligence (TAAI). IEEE, 2011, pp. 223–226.

[5] ZHAO, Zheng; WANG, Yuchuan; FU, Shengbo. Head movement recognition based on Lucas-Kanade algorithm. In: 2012 International Conference on Computer Science &

[6] RUMÃO DE MELO, Valdenice. Avaliação da qualidade de vida de pacientes com lesão medular acompanhados em regime ambulatorial. Master's thesis - Federal University

[7] CEZAR DA CRUZ, Daniel Marinho; AUGUSTO IOSHIMOTO, Maria Teresa. Tecnologia assistiva para as atividades de vida diária na tetraplegia completa c6 pós-lesão

[8] Learning OPENCV. Computer vision with the OpenCV library. GaryBradski & Adrian

[9] BRAGA, Rodrigo António Marques et al. Concept and design of the intellwheels platform for developing intelligent wheelchairs. In: Informatics in Control, Automation

[10] JIAN-ZHENG, Liu; ZHENG, Zhao. Head movement recognition based on lk algorithm and gentleboost. In: 2011 7th International Conference on Networked Computing and

[12] VIOLA, Paul; JONES, Michael. Rapid object detection using a boosted cascade of simple features. In: Proceedings of the 2001 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 2001. CVPR 2001. IEEE, 2001, 1, pp. I-511–I-518.

[13] DYMOND, Elizabeth; POTTER, Roger. Head movements for control of assistive technology. Engineering in Medicine and Biology Society. In: 14th Annual International

of Pernambuco. CCS. Neuropsychiatry and Behavioral Sciences, 2009.

and Robotics, Springer-Verlag, Berlin Heidelberg, 2009, pp. 191–203.

Advanced Information Management (NCM). IEEE, 2011, pp. 232–236.

[11] JOLLIFFE, Ian. Principal Component Analysis. John Wiley & Sons, Ltd, 2002.

Program in Computer Science, Porto, Portugal, set 2012.

Service Robotics, v. 5, n. 2, pp. 89–98, 2012.

Service System (CSSS). IEEE, 2012, pp. 2303–2306.

medular. Magazine Triângulo, v. 3, n. 2, 2011.

Conference of the IEEE, 1992, 4, pp. 1527–1528.

Kaebler. O'Reilly, 2008.


#### **Watch Your Step! Terrain Traversability for Robot Control Watch Your Step! Terrain Traversability for Robot Control**

#### Mauro Bellone Mauro Bellone Additional information is available at the end of the chapter

[26] ARAI, Kohei; MARDIYANTO, Ronny. Eyes based electric wheel chair control system. International Journal of Advanced Computer Science and Applications, v. 2, n. 12, 2011.

[27] CHAU, Michael; BETKE, Margrit. Real time eye tracking and blink detection with USB cameras. Boston University Computer Science, v. 2215, n. 2005–2012, pp. 1–10, 2005.

[28] FAZLI, Saeid; ESFEHANI, Parisa. Tracking eye state for fatigue detection. In: International Conference on Advances in Computer and Electrical Engineering (ICACEE

[29] ALSHAQAQI, Belal et al. Driver drowsiness detection system. In: 2013 8th International Workshop on Systems, Signal Processing and their Applications (WoSSPA). IEEE,

[30] SAINI, Vandna; SAINI, Rekha. Driver drowsiness detection system and techniques: a review. International Journal of Computer Science and Information Technologies, v. 5,

[31] HJELMÅS, Erik; LOW, Boon Kee. Face detection: a survey. Computer Vision and Image

[32] SCHAPIRE, Robert E. The strength of weak learnability. Machine Learning, v. 5, n. 2,

[33] NOCK, Richard; NIELSEN, Frank. A real generalization of discrete adaboost. Frontiers

[34] GAO, Wei; ZHOU, Zhi-Hua. Approximation stability and boosting. Algorithmic

[35] HORTON, Michael; CAMERON-JONES, Mike; WILLIAMS, Raymond. Multiple classifier object detection with confidence measures. In: AI 2007: Advances in Artificial

[36] EKMAN, Paul; ROSENBERG, Erika L. What the face reveals: Basic and applied studies of spontaneous expression using the Facial Action Coding System (FACS). Oxford

[37] TURK, Matthew; PENTLAND, Alex. Eigenfaces for recognition. Journal of Cognitive

[38] BENOIT, Alexandre et al. Using human visual system modeling for bio-inspired low level image processing. Computer Vision and Image Understanding, v. 114, n. 7, pp.

[39] LIENHART, Rainer; MAYDT, Jochen. An extended set of haar-like features for rapid object detection. In: Proceedings of the 2002 International Conference on Image

in Artificial Intelligence and Applications, v. 141, p. 509, 2006.

Intelligence. Springer, Berlin, Heidelberg, 2007, pp. 559–568.

2012). 2012. pp. 17–20.

110 Robot Control

2013, pp. 151–155.

pp. 197–227, 1990.

n. 3, pp. 4245–4249, 2014.

Undertanding, v. 83, n. 3, pp. 236–274, 2001.

Learning Theory, v. 21, pp. 59–73, 2010.

Neuroscience, v. 3, n. 1, pp. 71–86, 1991.

Processing. IEEE, 2002. pp. I-900–I-903, vol. 1.

University Press, USA, 1997.

758–773, 2010.

Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/64489

#### **Abstract**

Watch your step! Or perhaps, watch your wheels. Whatever the robot is, if it puts its feet, tracks, or wheels in the wrong place, it might get hurt; and as robots are quickly going from structured and completely known environments towards uncertain and unknown terrain, the surface assessment becomes an essential requirement. As a result, future mobile robots cannot neglect the evaluation of terrain's structure, according to their driving capabilities. With the objective of filling this gap, the focus of this study was laid on terrain analysis methods, which can be used for robot control with particular reference to autonomous vehicles and mobile robots. Giving an overview of theory related to this topic, the investigation not only covers hardware, such as visual sensors or laser scanners, but also space descriptions, such as digital elevation models and point descriptors, introducing new aspects and characterization of terrain assessment. During the discussion, a wide number of examples and methodologies are exposed according to different tools and sensors, including the description of a recent method of terrain assessment using normal vectors analysis. Indeed, normal vectors has demonstrated great potentialities in the field of terrain irregularity assessment in both on‐road and off‐road environments.

**Keywords:** traversability, terrain assessment, terrain analysis, UGV, mobile robots

#### **1. Introduction**

From an analysis in the United States, the automated guided vehicles (AGVs) market will be worth 2240 million dollars by 2020, due to growing automation investments across all major industries [1]. Besides, BI Intelligence estimates a number of 10 million cars and trucks featuring self‐driving capabilities by the same year [2]. On the other side, during the DARPA Robotics Challenge 2015, worldwide universities and their humanoids have raced among challenging scenarios, and a number of robots lost their balance traveling

© 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. © 2016 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

across rubble [3], and some of them even used semi‐autonomous systems to overcome this challenge by manually sending commands about specific locations where to put their feet on. Additionally, the Curiosity rover, recently sent on Mars by NASA, demonstrates the growing utilization of robotics technologies in planetary exploration as they require high level of reliability during their surveys, and rocks or terrain irregularities may cause irreparable damages to on‐board instrumentation [4].

The common element among all these types of robots consists of the necessity of a high level of driving capability; though motion control has made great strides, it may fail in case of unexpected circumstances, including road hazards, pavement distresses, and rubble. As a result, from widely known AGVs, spread in industries since years, to modern unmanned ground vehicles (UGVs) [5], the high level of driving capabilities is perceived an essential requirement. In order to enhance robustness and reliability, future mobile robots should be designed including custom hardware and software components, helping UGVs to adapt their driving behavior according to surface irregularities. In robotics, the assessment of terrain conditions is generally referred to as *"terrain traversability analysis;"* even though traversability has been explored from various perspectives, a thorough survey on this topic suggests that a specific definition is still missing in the robotic community [6]. On the other hand, as robots' diffusion increases braking up new boundaries in their application, the use of visual technol‐ ogies for traversability assessment will improve their reliability; consequently, the acquisition of information about the terrain is a prerequisite capacity and recent advances in sensors and perception encourage future researched in this field.

Among the number of methods and models for terrain analysis, there are at least two large categories, (i) *classification‐based* methods and (ii) *cost‐assessment* methods. In the former, it is possible to count all the approaches that consider a binary distinction of the terrain as two classes, traversable or non‐traversable; to cite an example, in [7], the authors use an on‐line trained classifier to distinguish traversable and non‐traversable regions. Widely spread in research, occupancy maps also fall in this category as they use the elevation of surrounding objects to construct a map of occupied regions on the base of sensor measurements [8]. Whereas in cost‐assessment methods is common to assign a continuous cost index, to better describe the traversability characteristics of terrain according to a specific cost function [9]. As advances on the same line, Tanaka et al. implemented a fuzzy‐based traversability analysis, considering terrain roughness and slope as input for a fuzzy inference system and then generating a vector field histogram for navigation purposes [10].

A further classification of methods commonly used in this field distinguishes between *geometric‐* or *appearance‐based* methods. Used in a large number of works in research [11–13], geometric‐based analyses aim to detect traversability using geometric properties of surfaces such as distances in space and shapes. Whereas appearance methods, to a greater extent related to camera images processing and cognitive analyses, have the objective of recognize colors and patterns not related to the common appearance of terrain, such as grass, rocks or vegetation [14, 15]. In spite of the clear potentialities of appearance‐based methods, still geometric ones are mostly common in robotics, because they can be easily used for path‐planning purposes, where also probabilistic methods are gaining interest. Indeed, in 2006, Thrun et al. [16] presented a probabilistic algorithm for terrain classification on a fast moving robot platform, constituting a part of their autonomous vehicle during the Darpa Grand Challenge in 2005. As a recent example, in [17], the authors describe a terrain classification approach for an autono‐ mous robot based on Markov random fields (MRFs) on fused 3D laser and camera image data.

In the light of glaring requirements of terrain analysis for future UGVs, this discussion aims at exploring some of the basic concepts of traversability; the focus was laid on geometric methods. This study introduces a definition of traversability and its application to robot control and autonomous ground vehicles. This directly leads to the contributions of this chapter, which attempts to compare different methodologies and fill the gap between theory and practical applications giving a definition that can be of general value for terrain traversability analysis in terms of a fuzzy set, including practical examples to foregoing functions available in the literature. Furthermore, the potentialities of novel methods based on the normal vectors analysis will be explored, providing some practical examples of application.

The chapter is structured as follow: Section 2 will provide an overview and basic knowledge about the field with focus on related works and recent techniques for visual terrain analysis, used sensors and space representation. Later, in Section 3, a theoretical background will help, who unfamiliar with the topic, to understand the basic concepts related to robot models and state spaces, introducing a definition of traversability in terms of a fuzzy set. Examples, results and comparisons are exposed during a thorough discussion in Section 4, which will cover basic functions and recent researches in the field applied on both synthetic data and real scenarios. Conclusions are drawn in Section 5.

## **2. Overview**

across rubble [3], and some of them even used semi‐autonomous systems to overcome this challenge by manually sending commands about specific locations where to put their feet on. Additionally, the Curiosity rover, recently sent on Mars by NASA, demonstrates the growing utilization of robotics technologies in planetary exploration as they require high level of reliability during their surveys, and rocks or terrain irregularities may cause

The common element among all these types of robots consists of the necessity of a high level of driving capability; though motion control has made great strides, it may fail in case of unexpected circumstances, including road hazards, pavement distresses, and rubble. As a result, from widely known AGVs, spread in industries since years, to modern unmanned ground vehicles (UGVs) [5], the high level of driving capabilities is perceived an essential requirement. In order to enhance robustness and reliability, future mobile robots should be designed including custom hardware and software components, helping UGVs to adapt their driving behavior according to surface irregularities. In robotics, the assessment of terrain conditions is generally referred to as *"terrain traversability analysis;"* even though traversability has been explored from various perspectives, a thorough survey on this topic suggests that a specific definition is still missing in the robotic community [6]. On the other hand, as robots' diffusion increases braking up new boundaries in their application, the use of visual technol‐ ogies for traversability assessment will improve their reliability; consequently, the acquisition of information about the terrain is a prerequisite capacity and recent advances in sensors and

Among the number of methods and models for terrain analysis, there are at least two large categories, (i) *classification‐based* methods and (ii) *cost‐assessment* methods. In the former, it is possible to count all the approaches that consider a binary distinction of the terrain as two classes, traversable or non‐traversable; to cite an example, in [7], the authors use an on‐line trained classifier to distinguish traversable and non‐traversable regions. Widely spread in research, occupancy maps also fall in this category as they use the elevation of surrounding objects to construct a map of occupied regions on the base of sensor measurements [8]. Whereas in cost‐assessment methods is common to assign a continuous cost index, to better describe the traversability characteristics of terrain according to a specific cost function [9]. As advances on the same line, Tanaka et al. implemented a fuzzy‐based traversability analysis, considering terrain roughness and slope as input for a fuzzy inference system and then generating a vector

A further classification of methods commonly used in this field distinguishes between *geometric‐* or *appearance‐based* methods. Used in a large number of works in research [11–13], geometric‐based analyses aim to detect traversability using geometric properties of surfaces such as distances in space and shapes. Whereas appearance methods, to a greater extent related to camera images processing and cognitive analyses, have the objective of recognize colors and patterns not related to the common appearance of terrain, such as grass, rocks or vegetation [14, 15]. In spite of the clear potentialities of appearance‐based methods, still geometric ones are mostly common in robotics, because they can be easily used for path‐planning purposes, where also probabilistic methods are gaining interest. Indeed, in 2006, Thrun et al. [16]

irreparable damages to on‐board instrumentation [4].

112 Robot Control

perception encourage future researched in this field.

field histogram for navigation purposes [10].

As humans themselves rely on their five senses to know where to walk or drive a vehicle on, creating an implicit space representation in the brain, robots perceive and interpret the space using exteroceptive and proprioceptive transducers as a sensing aid. In order to build an effective exteroceptive traversability analysis tool two elements are required: (i) visual sensors and (ii) a mathematical space representation. The former comprises any exteroceptive sensor such as cameras, depth cameras, or time‐of‐flight sensors, which endow robots with sensing capabilities; whereas the latter provides a spatial organization of sensory data and build an abstract representation of the 3D environment. As a result, the approach to terrain traversa‐ bility analysis may change according to space representation, as much as the available data may vary according to the type of sensor. Even though the most common methods for terrain traversability analysis are based on exteroceptive perception [9], for the sake of completeness, it is important to cite that proprioceptive sensors are also successfully used for terrain analysis [18–20], measuring and interpreting quantities such as vibrations or slippage, but their study is out of the scope of this study.

To facilitate the comprehension of the content of this discussion, following a short review on space representations and sensor technologies available for terrain analysis in mobile robotics is reported.

#### **2.1. Sensors for terrain analysis**

Sensing denotes a group of techniques used in robotics to measure any physical quantity interacting with the robot. Hence, any device used to acquire information can be counted in this category. Although the general concept of sensing as the problem of understanding how a robot see the world, by means of a set of visual sensors, has been addressed following various approaches, in the specific topic of traversability, there are a number of open issues still to be solved. In [21], the author has accurately described the problem of semantic perception for a robot operating in human‐living environments, approaching the problem from sensors and data point of view. Notwithstanding the valuable work done in the field of perception, the indoor structured environments introduce a number of simplifications which are never applicable in outdoor unstructured environments. First of all, indoor scenarios are generally characterized by smooth ground surfaces and high‐size objects represented as vertical planes. For this reason, AGVs, commonly used in indoor industrial environments, do not consider any terrain representation at all. Moreover, indoor robots generally move at low speed, and consequently, they do not require any sophisticated system for terrain analysis. The situation changes totally in the case of planetary rovers [4], driving on sandy terrains featuring rocks, varying in size and shape. Furthermore, recent driverless cars are quickly going towards public roads; in such situations, rocks, road hazards, and pavement distresses may put the vehicle, and its passengers, in serious danger [22].

**Figure 1.** Examples of sensing devices in which: (a) is a depth camera, the Kinect sensor, mounted on an experimental planetary rover, (b) is a stereovision system including the XB3 Bumblebee camera used on an agricultural tractor, in (c) an autonomous electric car featuring a Sick laser, and (d) is an ultrasonic sensor‐based mechatronic device.

Since this discussion examines terrain analysis, a distinction between acquisition and repre‐ sentation of information should be done. On one hand, the space acquisition strongly depends on the typology of sensors and applications; on the other hand, its representation depends on the perception meaning and its content. From a purely geometrical point of view, the most primitive representation of a point in the space is the 3D Euclidean metric. However, the information about the real 3D coordinates of a specific point can be obtained by triangulation techniques [23, 24] on stereocamera images, or by directly measuring its distance using time‐ of‐flight (TOF) systems [25]. **Figure 1** shows typical image sensors assembled on several UGVs in order to acquire some of the images used for the experimental discussion in this work. Specifically, **Figure 1a** depicts a depth sensor, the Kinect camera, used in [26] for a novel approach to terrain analysis, whereas in **Figure 1b** a more sophisticated vision system designed for an agricultural tractor is shown [27], the red circle marks a trinocular stereocamera. **Figure 1c** and **Figure 1d** show two examples of time‐of‐flight sensors, a Sick laser range finder and a sonar sensing system. Following, the technology at the base of such sensors will be briefly recalled.

#### *2.1.1. Stereovision*

**2.1. Sensors for terrain analysis**

114 Robot Control

and its passengers, in serious danger [22].

Sensing denotes a group of techniques used in robotics to measure any physical quantity interacting with the robot. Hence, any device used to acquire information can be counted in this category. Although the general concept of sensing as the problem of understanding how a robot see the world, by means of a set of visual sensors, has been addressed following various approaches, in the specific topic of traversability, there are a number of open issues still to be solved. In [21], the author has accurately described the problem of semantic perception for a robot operating in human‐living environments, approaching the problem from sensors and data point of view. Notwithstanding the valuable work done in the field of perception, the indoor structured environments introduce a number of simplifications which are never applicable in outdoor unstructured environments. First of all, indoor scenarios are generally characterized by smooth ground surfaces and high‐size objects represented as vertical planes. For this reason, AGVs, commonly used in indoor industrial environments, do not consider any terrain representation at all. Moreover, indoor robots generally move at low speed, and consequently, they do not require any sophisticated system for terrain analysis. The situation changes totally in the case of planetary rovers [4], driving on sandy terrains featuring rocks, varying in size and shape. Furthermore, recent driverless cars are quickly going towards public roads; in such situations, rocks, road hazards, and pavement distresses may put the vehicle,

(a)t (b)

(c) (d)

an autonomous electric car featuring a Sick laser, and (d) is an ultrasonic sensor‐based mechatronic device.

**Figure 1.** Examples of sensing devices in which: (a) is a depth camera, the Kinect sensor, mounted on an experimental planetary rover, (b) is a stereovision system including the XB3 Bumblebee camera used on an agricultural tractor, in (c) Stereocameras constitute a family of cameras composed by two or more lenses with separated image sensors. They provide a visual image for each lens and post‐elaboration attempts to estimate the distance of each point from the sensor by means of connections between corre‐ spondences seen by two different lenses at the same time, simulating the human binocular vision. In order to provide accurate measures, the sensors require the perfect calibration with respect to each other, done by the extrapolation of their intrinsic and extrinsic parameters.

In the literature, a large number of methods for camera calibration are available. As an example, Kearney et al. propose a method for the calibration using geometric constraints in [28] and then Puget and Skordas present a method for optimizing the calibration [29]. Later, many researchers studied methods for fast and accurate calibration of multiple cameras [30], in anticipation of the most recent researches of automatic calibration for cars, for example [31]. Recent sensors use more than two cameras for the triangulation in order to increase the accuracy in both short and long range. The 3D representation of the environment is inferred detecting the same point into both camera images, and the bigger the set of points the richer will be the 3D space reconstruction.

Simplifying the concept, said *d* the distance from a point *p* measured by a binocular stereoca‐ mera, then:

$$d = \frac{f\mathbf{b}}{||\!|\!|\!|\!|\_1 - \!|\!|\_2 ||\!|} \, \tag{1}$$

where *f* is the focal distance of the sensors, *b* is the baseline, that is, the spacing between the sensors, and *x*1, *x*2 are the coordinates of *p* in the two images expressed in terms of pixels.

An example of a trinocular camera featuring multiple baseline can be seen in **Figure 1b**, where the sensor has been mounted as visual aid on an experimental tractor [27].

#### *2.1.2. Time‐of‐flight 3D sensors*

In contrast to stereocameras, TOF‐based systems, such as lasers and sonars, directly evaluate distances by the measurement of the delay until an emitted signal hits a surface and returns back to the receiver, thus estimating the true distance from the sensor to the surface. Also in this case, a simplified relation can calculate the distance between the sensor and a point in the space as follows:

$$d = \frac{ct}{2} \,' \tag{2}$$

where *c* is the speed of the ray, light in case of lasers, and *t* is the amount of time since the emission until the reception. However, in case of ultrasonic sensors, the speed of the ray depends of its wavelength and the estimation of the distance as well as the localization problem become harder due to the wider beam which may be cause of multiple reflections. As an example, in [32], the authors propose three different mathematical approaches to detect position and orientation of an observer, such as a robot, with respect to a smooth surface. Such ultrasonic‐based system is depicted in **Figure 1d**. In contrast to ultrasonic technology, laser scanners are much more precises and reliables for environment description. To underline the global diffusion of laser scanners, **Figure 1c** shows a Sick 3D laser range finder applied on an electric autonomous vehicle at University of Almería (Spain) [33]. As proof of the higher performance of lasers, Borrmann et al. obtained an accurate space description from a laser scanner and use laser information to build a global map in outdoor urban environment [34]. Besides this research, a large number of scientists continuously propose new methods for the 3D space reconstruction using 3D laser scanner technologies.

Thanks to their properties of accuracy and reliability, the research involving vision for mobile robot shifted towards the use of laser technologies as an aid for space reconstruction.

#### **2.2. Space representations**

The term *space representation* roboticists refer to an abstract depiction of robots' surrounding environment. As robots live in the three‐dimensional space, the most natural space represen‐ tation should be the Euclidean 3D space, but handling 3D space data may be hard and time‐ consuming. Thus, for computational performance purposes, the most used foregoing space representation has been the 21 2‐dimensional, such as digital elevation models (DEM) better described later in this section. Only recently, thanks to the high performing CPU and GPUs, 3D point descriptors are gaining interest in this field.

#### *2.2.1. Digital elevation maps*

An example of a trinocular camera featuring multiple baseline can be seen in **Figure 1b**, where

In contrast to stereocameras, TOF‐based systems, such as lasers and sonars, directly evaluate distances by the measurement of the delay until an emitted signal hits a surface and returns back to the receiver, thus estimating the true distance from the sensor to the surface. Also in this case, a simplified relation can calculate the distance between the sensor and a point in the

= , <sup>2</sup>

3D space reconstruction using 3D laser scanner technologies.

where *c* is the speed of the ray, light in case of lasers, and *t* is the amount of time since the emission until the reception. However, in case of ultrasonic sensors, the speed of the ray depends of its wavelength and the estimation of the distance as well as the localization problem become harder due to the wider beam which may be cause of multiple reflections. As an example, in [32], the authors propose three different mathematical approaches to detect position and orientation of an observer, such as a robot, with respect to a smooth surface. Such ultrasonic‐based system is depicted in **Figure 1d**. In contrast to ultrasonic technology, laser scanners are much more precises and reliables for environment description. To underline the global diffusion of laser scanners, **Figure 1c** shows a Sick 3D laser range finder applied on an electric autonomous vehicle at University of Almería (Spain) [33]. As proof of the higher performance of lasers, Borrmann et al. obtained an accurate space description from a laser scanner and use laser information to build a global map in outdoor urban environment [34]. Besides this research, a large number of scientists continuously propose new methods for the

Thanks to their properties of accuracy and reliability, the research involving vision for mobile

The term *space representation* roboticists refer to an abstract depiction of robots' surrounding environment. As robots live in the three‐dimensional space, the most natural space represen‐ tation should be the Euclidean 3D space, but handling 3D space data may be hard and time‐ consuming. Thus, for computational performance purposes, the most used foregoing space

described later in this section. Only recently, thanks to the high performing CPU and GPUs,

2‐dimensional, such as digital elevation models (DEM) better

robot shifted towards the use of laser technologies as an aid for space reconstruction.

*ct <sup>d</sup>* (2)

the sensor has been mounted as visual aid on an experimental tractor [27].

*2.1.2. Time‐of‐flight 3D sensors*

space as follows:

116 Robot Control

**2.2. Space representations**

representation has been the 21

3D point descriptors are gaining interest in this field.

Organizing sensors data is a mandatory step to reconstruct information for geometric inter‐ pretation purposes, and digital elevation models (DEM) [35] are widely used as space repre‐ sentation in mobile robotics. Although topography and large areas terrain mapping constitute the original use of DEMs, their use for traversability analysis has been demonstrated as successful in mobile robotics [4]. As further example, Larson et al. discuss a real‐time approach to analyze the traversability of off‐road terrain for UGVs considering positive and negative obstacles through elevation information [36].

DEMs have been introduced as a compact 21 2‐dimensional representation, which assumes that

a surface can be represented as an elevation function (,), :ℝ2 <sup>ℝ</sup>, where *x* and *y* are the coordinates on a regularly sampled plane. As a result, a grid‐based space representation is obtained, in which a surface is described by a finite number of points collected in a fixed size grid structure. **Figure 2** shows an example of a DEM representation obtained from a stereo‐ camera images, the entire procedure shows the process from a camera image, see **Figure 2a**, to point cloud in **Figure 2b**, and DEM, **Figure 2c**. Though compact the DEM representation requires a further step from acquisition to 3D reconstruction and DEM generation, whereas working on purely 3D data implies that one step can be skipped.

**Figure 2.** Example of two different space representations in which (b) is a point cloud representation, whereas (c) is the relative DEM, both geometrically describing the scene in the camera image (a).

The classical DEM approach constitutes an efficient representation, but it lacks of accuracy in space description since objects are described as surfaces using their elevation without taking into account their real shape. For instance, a tunnel cannot be represented using a digital elevation model. As an improvement of classical DEMs approach, Pfaff et al. [37] proposed the extended DEMs or the so‐called *extended elevation maps* (EEM). Such technique involves the use of additional information in order to have a better description of objects and space; further‐ more, the authors also used a Kalman filter to enhance the terrain description in a DEM taking into account measurements error and uncertainties. Recently, in [38], the researchers used EEM as multilayer digital maps for the description of volcano areas.

In conclusion, though suitable due to its compactness and simplicity, in each DEM formaliza‐ tion, there is the assumption of regularity in the surface and it turns into a not‐complete space representation. As the matter of fact, it fails in a large number of practical situations; never‐ theless, it is extensively used in robotics since it is simply applicable in low‐performance embedded controllers.

#### *2.2.2. Point descriptors*

A recent space description, used in robotics for traversability purposes, consists in the representation of each point simply by its 3D Cartesian coordinates [24]. Hence, let us define a *point cloud* as a set of scattered 3D points, that is:

$$\mathcal{P} = \left\{ p\_i(\mathbf{x}\_i, \mathbf{y}\_i, z\_i) \in \mathbb{R}^3 \; ; \; i = 1, 2, \dots, n, n \in \mathbb{N} \right\},\tag{3}$$

where *n* is the number of elements in the set. In order to provide a coherent space represen‐ tation, the coordinates of each point have to be given respect to a common coordinate system. The origin of such reference frame is usually located into the robot's geometric center or the sensing device, defined as camera reference frame , , , , . For this reason, generally distance data need an additional coordinate transformation using appropriate rotation matrices. As a result, 3D space description in form as point cloud constitutes a simple and robust solution to represent environments for robotic purposes. In the most recent data representation, the RGB color information is added to points obtaining the so‐called RGB‐D point clouds. As an example, **Figure 2b** shows an RGB‐D point cloud obtained as triangulation of stereopairs in outdoor road environment. Nowadays, it is common to think the 3D points as defined in the three‐dimensional meaning of Euclidean metric and represented by its Cartesian coordinates (*x*, *y*, *z*). However, problems such as perception and recognition in point clouds are ill‐posed, if only the geometric coordinates of points are considered. In spite of the addition of new characteristics of points, such as color or intensity, may help, the problem remains ill‐posed due to the ambiguity of matching between points. In particular, a point in a cloud can be seen as a single point, yet it could represent the intersection of perpendicular planes representing the sides of an object, and therefore, it could be described using semantic meanings such as "vertex" or "edge." The set of characteristics used to describe a point defines a *local descriptor.* As a result, in the context of perception, the concept of 3D point as described only by its coordinates is substituted by the concept of local descriptor.

The classical DEM approach constitutes an efficient representation, but it lacks of accuracy in space description since objects are described as surfaces using their elevation without taking into account their real shape. For instance, a tunnel cannot be represented using a digital elevation model. As an improvement of classical DEMs approach, Pfaff et al. [37] proposed the extended DEMs or the so‐called *extended elevation maps* (EEM). Such technique involves the use of additional information in order to have a better description of objects and space; further‐ more, the authors also used a Kalman filter to enhance the terrain description in a DEM taking into account measurements error and uncertainties. Recently, in [38], the researchers used EEM

In conclusion, though suitable due to its compactness and simplicity, in each DEM formaliza‐ tion, there is the assumption of regularity in the surface and it turns into a not‐complete space representation. As the matter of fact, it fails in a large number of practical situations; never‐ theless, it is extensively used in robotics since it is simply applicable in low‐performance

A recent space description, used in robotics for traversability purposes, consists in the representation of each point simply by its 3D Cartesian coordinates [24]. Hence, let us define

where *n* is the number of elements in the set. In order to provide a coherent space represen‐ tation, the coordinates of each point have to be given respect to a common coordinate system. The origin of such reference frame is usually located into the robot's geometric center

generally distance data need an additional coordinate transformation using appropriate rotation matrices. As a result, 3D space description in form as point cloud constitutes a simple and robust solution to represent environments for robotic purposes. In the most recent data representation, the RGB color information is added to points obtaining the so‐called RGB‐D point clouds. As an example, **Figure 2b** shows an RGB‐D point cloud obtained as triangulation of stereopairs in outdoor road environment. Nowadays, it is common to think the 3D points as defined in the three‐dimensional meaning of Euclidean metric and represented by its Cartesian coordinates (*x*, *y*, *z*). However, problems such as perception and recognition in point clouds are ill‐posed, if only the geometric coordinates of points are considered. In spite of the addition of new characteristics of points, such as color or intensity, may help, the problem remains ill‐posed due to the ambiguity of matching between points. In particular, a point in a cloud can be seen as a single point, yet it could represent the intersection of perpendicular planes representing the sides of an object, and therefore, it could be described using semantic meanings such as "vertex" or "edge." The set of characteristics used to describe a point defines

{ ( ) ∈ ∈ } <sup>3</sup> = , , , = 1,2,..., , , *i iii p x y z i nn* (3)

, , ,

, . For this reason,

as multilayer digital maps for the description of volcano areas.

a *point cloud* as a set of scattered 3D points, that is:

or the sensing device, defined as camera reference frame

embedded controllers.

118 Robot Control

*2.2.2. Point descriptors*

Let be given a point cloud , defined as in Eq. (3), and let us consider a point *pq* the so‐called *query point,* the neighborhood of *pq* in can be defined as the set of points such that:

$$P^{\mathbb{A}} = \left\{ p\_i \in \mathcal{P} \subset \mathbb{R}^3 : \left| p\_i - p^{\mathbb{A}} \right| \le d\_n \,\forall i = 1, 2, \ldots, k \right\},\tag{4}$$

where *dm*, the so‐defined as *search radius,* is the maximum distance between *pq* and each neighbor, *k* is the number of neighbors of *pq* in *Pq* , and |·| is a generic norm (without loss of generality, it is possible to refer to the Euclidean distance).

A local descriptor of *pq* can be defined as the vector function *F* that describes the information content of *Pd* according to a specific characteristic:

$$F\left(p^{q}, P^{q}\right) = \left\{\mathbf{x}\_{1}^{q}, \mathbf{x}\_{2}^{q}, \dots, \mathbf{x}\_{n}^{q}\right\},\tag{5}$$

where is the *ith* dimension of the descriptor. By comparing the local descriptors of two points, namely *p*1 and *p*2, it is possible to estimate their differences. Let Г be the measure of similarity between *p*1 and *p*2, with their associated descriptors *F*1 and *F*2, and let *d* be their distance:

$$
\Gamma \vdash d\left(F\_{1'}F\_2\right).\tag{6}
$$

Then, *d* is a scalar function and can be considered as the degree of similarity between points. If Г → 0 two points can be considered similar according to the specific characteristics set. Conversely, if Г increases the points will have different properties. It is important to note that the effectiveness of the explicit expression of descriptors is given by its ability to differentiate points in the presence of rigid transformations, noise, sampling variations, changes in scale, or illumination. Moreover, the generality of the representation of points using descriptors allows to collect points and their characteristics such as color, but also traversability, as a vector in the form of a point cloud.

A possible application of point clouds for traversability analysis can be found in [14], where the authors describe a method for terrain classification using point clouds data obtained by stereovision. They propose the use of superpixels as the visual primitives for traversability estimation using a learning algorithm. A different approach can be found in [39]; here, the authors acquire information about terrain by a LIDAR and, using local 3D point statistics, segment it into three classes: clutter to capture grass and tree canopy, linear to capture thin objects such as wires or tree branches, and finally surface to capture solid objects such as ground terrain surface, rocks or tree trunks. As further example, in [40], the authors use a Sick lidar to acquire point cloud and build a traversability cost‐to‐go function for navigation purposes.

#### *2.2.3. A comparison among methods for terrain analysis*

To finalize this overview, it is worth to compare different methods according to their use in the scientific community and provide a classification of the used approaches. **Table 1** presents a summary of references in the field of terrain analysis and traversability, classifying them for space representation and used sensor, the full bullet indicates the classification. More specifi‐ cally, the classification of used sensors distinguishes between ToF and stereocameras as method to acquire information, whereas the space description classification differentiates between DEMs and point clouds, including in the last category also point descriptors.


The full bullet indicates the classification.

**Table 1.** Comparison of the literature, the table classifies space representations and used sensors for traversability purposes

This analysis suggests that both DEMs and point clouds are used for traversability analysis; however, one can consider as a possible trend, the use of point clouds for terrain traversability, since recent researches are going towards this direction. Contrary, DEMs constitute a stable and robust tool, widely used in all the fields of robotics, and it is even possible to find recent extensions of research. To cite one of them, in [38], the authors use an extended elevation models as improvement to DEMs. The historical predominant application of traversability is in natural outdoor environments, where the assumptions of surface regularity cannot be applied. Only recently, the study of surfaces is gaining interest in the automotive sector, in which all researches are quite recent, since this technology was never required in the field. Possible uses are as follows: pavement distress detection [41], sidewalk detection [12], or segment terrain's inliers and outliers to be used for obstacles detection [13].

From sensors point of view, laser scanner are commonly used for specific applications such as planetary or search and rescue, whereas stereocameras are preferred in applications where the cost‐effectiveness of cameras can be attractive. However, it is important to cite that ToF sensors are commonly used for geometry‐based traversability techniques, whereas cameras are used in the case of appearance‐based classification.

### **3. Terrain traversability analysis**

lidar to acquire point cloud and build a traversability cost‐to‐go function for navigation

To finalize this overview, it is worth to compare different methods according to their use in the scientific community and provide a classification of the used approaches. **Table 1** presents a summary of references in the field of terrain analysis and traversability, classifying them for space representation and used sensor, the full bullet indicates the classification. More specifi‐ cally, the classification of used sensors distinguishes between ToF and stereocameras as method to acquire information, whereas the space description classification differentiates between

**ToF | Stereo**

**Space representation DEM | Pt.C**

DEMs and point clouds, including in the last category also point descriptors.

**Table 1.** Comparison of the literature, the table classifies space representations and used sensors for traversability

*Bellone et al*. [27] Natural ●|● ○|● *Bellone and Reina* [41] Automotive ●|○ ○|● *Braun et al.* [42] Natural ○|● ●|○ *Broggi et al.* [13] Automotive ○|● ●|○ *Cafaro et al*. [43] Search and rescue ●|○ ○|● *Dargazanv and Berns* [44] Natural ○|● ○|● *Dongshin et al.* [14] Natural ○|● ○|● *Haselich et al.* [17] Natural ●|○ ●|○ *Ishigami et al.* [45] Planetary ●|○ ●|○ *Kubota et al.* [46] Planetary ●|○ ●|○ *Larson et al.* [36] Natural ●|○ ●|○ *Neuhus et al.* [25] Automotive ●|○ ○|● *Ohki et al*. [38] Field ●|○ ●|○ *Oniga et al*. [12] Automotive ○|● ●|○ *Papadakis et al.* [9] Search and rescue ●|○ ●|○ *Pfaff et al*. [37] Natural ●|○ ●|○ *Rohmer et al.* [47] Planetary ●|○ ●|○ *Roccacio et al.* [7] Natural ○|● ●|○ *Suger et al.* [11] Natural ●|○ ○|● *Thruh et al.* [16] Automotive ●|○ ○|● *Vandapel et al.* [39] Natural ○|● ○|● *Whitty et al.* [40] Field ●|○ ○|●

purposes.

120 Robot Control

*2.2.3. A comparison among methods for terrain analysis*

**Reference Application Sensors**

The full bullet indicates the classification.

purposes

From a dictionary definition, the word "traversability" denotes *"the condition of being travers‐ able"* and traversable concerns the capability *"to travel across or through."*<sup>1</sup> This linguistic definition does not explicitly refer to means; for instance, if one is conducting a car the word traversable better characterizes the action of "driving across or through," whereas going by feet may refer to the natural process of "walking across or through." However, an allusion to two elements exists in the definition: (i) the space, to be traversed, and (ii) the mean, to traverse the space. In classical control theory, such elements are expressed using concepts such as controllability or reachability, and they are related to the properties of a system to reach a generic state from the origin or the other way round, according to a specific physical model of the process. Whereas a thorough survey on traversability assessment suggests that its formal definition is still missing in the robotic community [6]. In the same survey, a qualitative definition of traversability in the context of UGVs appears, stating:

*"The capability of a ground vehicle to reside over a terrain region under an admissible state wherein it is capable of entering given its current state, this capability being quantified by taking into account a terrain model, the robotic vehicle model, the kinematic constraints of the vehicle and a set of criteria based on which the optimality of an admissible state can be assessed [6]."*

Though descriptive and valuable, this definition only provides ingredients to reach a more general and formal definition of traversability. First of all, it is important to consider few

<sup>1</sup> Definition from Oxford Dictionaries.

aspects: (i) a robot model including its motion constraints, (ii) space representation, for example, the terrain model, and (iii) a set of criteria to express the traversability properties. All these concepts will be later recalled.

Since this topic is attracting further researches, a more general definition of traversability is given later by Cafaro et al. [43]. The authors have made a valuable work on the theory of space description using point clouds, introducing the definitions of *traversable region* and *traversability map* in the context of graph theory, thus defining traversability as the existence of a connection (i.e., a branch) between two vertexes of a graph. A different characterization in terms of fuzzy sets was already provided by Seraji [48], and even though it was not general, the author distinguishes among different types of terrain providing the introduction of this topic in the robotic community. In the light of all relevant works made in research a clear discrepancy between theory and application appears. This section will attempt to fill this gap, using the elements in the literature to reach a definition in terms of control space which can consider the robot model, its operating environment and an evaluation criterion.

#### **3.1. Robot models and configuration space**

Prom the basis of control theory, it is well known that the robot control includes three different, but fundamental, items: process, controller, and sensors. This concept perfectly describes the ancient meaning of the word *control,* which refers to the capacity of inducing a specific behavior to a process based on observations of its evolution. Starting from simple regulators, the control theory evolved towards robot control, regarding robots considered as complex processes. Obviously, as processes complexity increases, the complexity of controllers increases itself. The reason of the growing complexity of robotic systems is furthermore referred to the requirement of a higher level of interaction between robots and real world.

The physical description of robots in control theory typically is expressed through a process and a state space. Thus, given the state , where ℝ is referred to as *state space*, and the command with ℝ, called *command space,* a discrete system can be defined as:

$$
\pi(k+1) = f(\pi(k), \iota(k)) \quad f: \mathbb{R}^n \to \mathbb{R}^n. \tag{7}
$$

The function *f*, referred to as *transition function,* denotes the behavior of a system, from simple systems to complex mobile robots. The generality of this definition expresses the evolution of any physical process and though usable in any possible situation, its elements, including space structures and transition function, must be explicitly expressed in practical applications. The command space can be easily defined given the kinematic/dynamic properties of the robot and its actuators, and it can be considered as a finite set of possible actions. Whereas the state space may be uncountable, open set and even featuring time‐variant elements (e.g., moving obsta‐ cles); as a consequence, it deserves a specific description.

For the sake of clarity, let us mention an example, the state space for a planar vehicle may be defined as = ℝ2 × (2) denoting ℝ2 the translations on *x* and *y* axis, respectively, and *SO*(2) the rotation around the axis orthogonal to the motion plane, also known as *SE*(2), special Euclidean group. This state space constitutes an open and uncountable set. Considering the 3D space, it is also common to find the state space as <sup>3</sup> <sup>×</sup> (3) referring to 3D translations and rotations, for example, in the case of position and orientation of UAVs (unmanned aerial vehicles) or even simply the end effector's pose in manipulators. Talking about traversability and driving on not‐flat terrains, the use of 3D representation is also

aspects: (i) a robot model including its motion constraints, (ii) space representation, for example, the terrain model, and (iii) a set of criteria to express the traversability properties. All

Since this topic is attracting further researches, a more general definition of traversability is given later by Cafaro et al. [43]. The authors have made a valuable work on the theory of space description using point clouds, introducing the definitions of *traversable region* and *traversability map* in the context of graph theory, thus defining traversability as the existence of a connection (i.e., a branch) between two vertexes of a graph. A different characterization in terms of fuzzy sets was already provided by Seraji [48], and even though it was not general, the author distinguishes among different types of terrain providing the introduction of this topic in the robotic community. In the light of all relevant works made in research a clear discrepancy between theory and application appears. This section will attempt to fill this gap, using the elements in the literature to reach a definition in terms of control space which can consider the

Prom the basis of control theory, it is well known that the robot control includes three different, but fundamental, items: process, controller, and sensors. This concept perfectly describes the ancient meaning of the word *control,* which refers to the capacity of inducing a specific behavior to a process based on observations of its evolution. Starting from simple regulators, the control theory evolved towards robot control, regarding robots considered as complex processes. Obviously, as processes complexity increases, the complexity of controllers increases itself. The reason of the growing complexity of robotic systems is furthermore referred to the requirement

The physical description of robots in control theory typically is expressed through a process and a state space. Thus, given the state , where ℝ is referred to as *state space*, and the

The function *f*, referred to as *transition function,* denotes the behavior of a system, from simple systems to complex mobile robots. The generality of this definition expresses the evolution of any physical process and though usable in any possible situation, its elements, including space structures and transition function, must be explicitly expressed in practical applications. The command space can be easily defined given the kinematic/dynamic properties of the robot and its actuators, and it can be considered as a finite set of possible actions. Whereas the state space may be uncountable, open set and even featuring time‐variant elements (e.g., moving obsta‐

For the sake of clarity, let us mention an example, the state space for a planar vehicle may be defined as = ℝ2 × (2) denoting ℝ2 the translations on *x* and *y* axis, respectively, and *SO*(2)

(7)

command with ℝ, called *command space,* a discrete system can be defined as:

robot model, its operating environment and an evaluation criterion.

of a higher level of interaction between robots and real world.

cles); as a consequence, it deserves a specific description.

these concepts will be later recalled.

122 Robot Control

**3.1. Robot models and configuration space**

becoming common for a more accurate design of autonomous navigation systems for UGVs. As a general definition, in robotics, it is possible to find the name *configuration space* or simply C‐Space [49, 50] describing the set of all possible configurations of the robot. C‐Space refers to a broad family of constructions closely related to the *state space* notion in physics which is common in general control theory.

Now, let us suppose that the C‐Space contains a forbidden region ; moreover, since the mobile robot will also live in C‐Space, we can denote the robot geometry as a subset , all sets may be expressed using polygonal or polyhedral models. At this point, let us denote as a possible configuration of our mobile robot , as a result is the configuration of the entire robot geometry in C‐Space, note that in the case of *SE*(2), the configuration of the robot at the time *k* will be *q* = (*xk*, *yk*, *θk*). Under the aforementioned assumptions, an *obstacle region* can be expressed as follows:

$$\mathcal{C}\_{\text{obs}} = \left\{ q \in \mathcal{C} \mid \mathcal{M}(q) \cap \mathcal{O} \neq \emptyset \right\} \subseteq \mathcal{C}. \tag{8}$$

The obstacle region constitutes the set of all robot's configurations intersecting the forbidden subspace. All the other configurations can be denoted as *free space,* , and obviously . Let us note that the sets and must be closed set in , as a consequence must be open, this will ensure the possibility to formalize an optimization problem in ; moreover, it ensures that the robot can drive arbitrary close to an obstacle without colliding it. As last consideration, though different in the formulation, the configura‐ tion and the state in Eq. (7) may be considered similar; as a consequence, there exists a transition function to go from a configuration *q*1 at a time *t*1, to another configuration *q*2 at the time *t*2. A rough analogy between *states* and *configurations* suggests that the transition function can be expressed as *qk*+1 = *f* (*qk*, *uk*); clearly defining the robot in the configuration *q* moving according to the equation of motion *f*.

This discussion does not pretend to be a complete description of spaces and sets, but it only gives the preliminary knowledge for the reading of this text, for additional details about assumptions, demonstrations, and definitions please refer to [50] as a relevant reference in the field.

The reason of the diffusion of C‐Spaces in robotics research resides in the possibility of describing them as manifolds, i.e., topological spaces that behave at every point like our intuitive notion of a surface, and the best way of describing the terrain is to consider its topological properties. Hence, considering a ground vehicle, the configuration space cannot be other than the terrain region it is driving on, described as a manifold.

#### **3.2. Traversability characterization**

The previous theory considers the robot moving in a configuration space composed by a free space part and a forbidden region . Yet, considering the concept of traversability as the condition of being traversable, then it is simple to understand that the free space can be considered as traversable, while the forbidden space may be not traversable. This definition would be perfectly enough for a binary classification of traversability.

Nevertheless, we are looking for a more general definition; thus, the traversability can be seen as the capability to travel across of through, which implies that the aforementioned binary definition could be extended. Indeed, the set could be forbidden (i.e., not traversable at all) or partially forbidden (i.e., traversable with some grade of membership). This clearly recalls the fuzzy logic2 that can be considered as an extension of the binary logic, such that statements need not be true or false, but they may have a grade of truth between 0 and 1. As a result, one can suppose the existence of a fuzzy set defined following.

**Definition 1** *Let be given a robot expressed as a closed subset where is a possible configuration of the mobile robot* , *and denotes its C‐Space. Let us suppose the existence of a not empty free space* , *with* . *Moreover, let us suppose be defined a traversability function , the traversable region will, be the defined by the following fuzzy set:*

$$\mathcal{C}\_{tr} = \left\{ (q \in \mathcal{C}\_{fuc}, \mathcal{T}(q)) \mid \mathcal{M}(q) \in \mathcal{C}\_{fuc} \right\}. \tag{9}$$

First of all, let us note that the traversable set is included into the C‐Space by definition, , because the membership function is defined in ; moreover and also . The traversability function used in this definition can be considered as a clear analogous of the more general membership functions which are common in the theory of fuzzy sets. As a result, when goes to 1 the statement "is traversale" will be true, whereas if the statement "is traversable" will be false.

The aforementioned definition considers all the elements previously indicated, i.e., a robot model , a space structure and a set of traversability criteria . The use of this definition, according to an explicit expression of , can also be used to solve optimal control problems.

In order to better clarify the concept, **Figure 3** expresses the difference between a simple occupancy map in **Figure 3a**, where free space and obstacles are clearly distinguished through a binary classification black/white, whereas the concept of fuzzy set in **Figure 3b** better characterizes the terrain according to the membership function . Its values are expressed

<sup>2</sup> Definition of fuzzy set: Given a generic set X and a membership function : [0; 1], the fuzzy set A is defined as

according to a degree of membership in gray scale, denoting in white high values of ; on the contrary, black corresponds to low values of . The presence of the region in **Figure 3b** can be interpreted as a region "less‐traversable" than usual, but still not classifiable as an obstacle, a politic of driving control may generate safe plans.

**Figure 3.** Depiction of the free space and fuzzy traversability characterization, the entire area inside the rectangle can be considered as in (a), whereas the gray‐scale gradient indicates the value of the membership function for each point of in (b). The presence of the region *A* denotes a portion of the free space featuring different values of traversability.

#### **4. Discussion**

topological properties. Hence, considering a ground vehicle, the configuration space cannot

The previous theory considers the robot moving in a configuration space composed by a free space part and a forbidden region . Yet, considering the concept of traversability as the condition of being traversable, then it is simple to understand that the free space can be considered as traversable, while the forbidden space may be not traversable. This definition

Nevertheless, we are looking for a more general definition; thus, the traversability can be seen as the capability to travel across of through, which implies that the aforementioned binary definition could be extended. Indeed, the set could be forbidden (i.e., not traversable at all) or partially forbidden (i.e., traversable with some grade of membership). This clearly recalls the

need not be true or false, but they may have a grade of truth between 0 and 1. As a result, one

**Definition 1** *Let be given a robot expressed as a closed subset where is a possible configuration of the mobile robot* , *and denotes its C‐Space. Let us suppose the existence of a not empty free space* , *with* . *Moreover, let us suppose be defined a traversability function , the traversable region will, be the defined by the following fuzzy set:*

First of all, let us note that the traversable set is included into the C‐Space by definition, , because the membership function is defined in ; moreover and also . The traversability function used in this definition can be considered as a clear

analogous of the more general membership functions which are common in the theory of fuzzy sets. As a result, when goes to 1 the statement "is traversale" will be true, whereas if

The aforementioned definition considers all the elements previously indicated, i.e., a robot model , a space structure and a set of traversability criteria . The use of this definition, according to an explicit expression of , can also be used to solve optimal control problems.

In order to better clarify the concept, **Figure 3** expresses the difference between a simple occupancy map in **Figure 3a**, where free space and obstacles are clearly distinguished through a binary classification black/white, whereas the concept of fuzzy set in **Figure 3b** better characterizes the terrain according to the membership function . Its values are expressed

Definition of fuzzy set: Given a generic set X and a membership function : [0; 1], the fuzzy set A is defined as

that can be considered as an extension of the binary logic, such that statements

C CT M C *tr* = ( , ( ))| ( ) . { *q qq* Î Î *free free*} (9)

be other than the terrain region it is driving on, described as a manifold.

would be perfectly enough for a binary classification of traversability.

can suppose the existence of a fuzzy set defined following.

the statement "is traversable" will be false.

**3.2. Traversability characterization**

fuzzy logic2

124 Robot Control

2

As the definition of traversability previously introduced can be of general value for geometry‐ based terrain analysis purposes, how to use it in order to build practical traversability functions will be following shown, including the re‐definition of classical methods, such as elevation models and roughness models. The exposed examples cover both binary classification methods and cost‐based assessment methods. Along the discussion, an irregular terrain model in the form of a DEM of about 20 m × 20 m, featuring a 0.25m grid size, has been used in order to compare different methods. Let us note that the terrain model, considered as sample model, expressed as a DEM is stored into a 80 × 80 size matrix, that is, 6400 elements. The same data in form of a point cloud, storing only the points' Cartesian coordinates, will take 6400 × 3 points. This clearly demonstrates the advantage in handling DEMs instead of point clouds; however, using DEMs part of the information is lost due to the assumption of terrain regularity, which is not always applicable. Moreover, ToF sensors as well as stereocamera triangulation always provide a set of distances between the cameras and sampled points in the space, that is, a point cloud, thus a transformation is required, including its computational cost, to build the digital map.

#### **4.1. Binary classification for traversability**

Let us consider the example of a binary classification and apply the aforementioned definition to find a member function such that the traversability region corresponds to the free part of the configuration space. Given a generic robot , in the configuration space , in this simple case can be expressed by the function:

$$\mathcal{T} = \begin{cases} \mathcal{T}(q) = 1 & \forall q \in \mathcal{C} | \mathcal{M}(q) \subseteq \mathcal{C}\_{free} \\ \mathcal{T}(q) = 0 & \forall q \in \mathcal{C} | \mathcal{M}(q) \cap \mathcal{C}\_{obs} \neq \emptyset \end{cases} \tag{10}$$

Note that, even though this function is the simplest possible, it works regardless of the particular structure of the C‐Space, and it converges into the general theory of configuration space. However, in practical cases, it is expected the free space to be explicitly expressed. To prove that is true in the case of binary classification, let us consider that by definition that . As a result, the only part that should be proven is , if is defined as in Eq. (10). Hence, let us suppose that exists a configuration *qk* such that but . This implies ; hence, , but this is absurd because *qk* would belong to both and . As a result, if is defined as in Eq. (10).

**Figure 4.** Binary traversability rule applied on an elevation model. In (a) and (b) the 3D‐view and *xy*‐view are shown. The red color labels not traversable regions (i.e., ), whereas the cyan color denotes the traversable parts of the terrain, .

As an example of functionality, **Figure 4** presents a binary classification applied to a sample terrain model. For the sake of the example, given = (,,), has been defined as the set of points such that ≤ . The result is a cyan region which can be considered as traversable, that is, belonging to , and a red region which can be considered as not traversable, that is, . The example explicitly refers to the 3D space; however, the definition in Eq. (10) has general value, since the structure of the configuration space has not been explicitly given. Though simple and widely used this method neglects information about intermediate levels of elevation or local irregularities; hence, it is much more used in indoor structured environ‐ ments where there are strong discontinuities (e.g., floor, walls) and under the assumption of regular flat floor surface. A different way to see this concept consists in the occupancy maps, which consider a cell as not traversable, if its elevation is higher than a threshold, that is, obstacle.

#### **4.2. Elevation terrain model for traversability**

**4.1. Binary classification for traversability**

126 Robot Control

case can be expressed by the function:

and . As a result, if is defined as in Eq. (10).

terrain, .

Let us consider the example of a binary classification and apply the aforementioned definition to find a member function such that the traversability region corresponds to the free part of the configuration space. Given a generic robot , in the configuration space , in this simple

Note that, even though this function is the simplest possible, it works regardless of the particular structure of the C‐Space, and it converges into the general theory of configuration space. However, in practical cases, it is expected the free space to be explicitly expressed. To prove that is true in the case of binary classification, let us consider that by definition that . As a result, the only part that should be proven is , if is defined as in Eq. (10). Hence, let us suppose that exists a configuration *qk* such that but . This implies ; hence, , but this is absurd because *qk* would belong to both

**Figure 4.** Binary traversability rule applied on an elevation model. In (a) and (b) the 3D‐view and *xy*‐view are shown. The red color labels not traversable regions (i.e., ), whereas the cyan color denotes the traversable parts of the

As an example of functionality, **Figure 4** presents a binary classification applied to a sample terrain model. For the sake of the example, given = (,,), has been defined as the set of points such that ≤ . The result is a cyan region which can be considered as traversable, that is, belonging to , and a red region which can be considered as not traversable, that is, . The example explicitly refers to the 3D space; however, the definition in Eq. (10) has general value, since the structure of the configuration space has not been explicitly given. Though simple and widely used this method neglects information about intermediate levels of elevation or local irregularities; hence, it is much more used in indoor structured environ‐

(10)

Typically used in mobile robotics, elevation models may be described using the formulation in Eq. (9). Let us suppose to have a ground vehicle that can move in three‐dimensional space. As indicated earlier, its configuration space can be expressed as = ℝ<sup>3</sup> , neglecting the orienta‐ tion terms to simplify the notation, the ground vehicle may be considered as a subset , and we can also consider the existence of a forbidden region . Now, let us construct a traversability function given a terrain model expressed as follows:

$$\mathcal{L}\_{\text{fuc}} = \left\{ q(\mathbf{x}, y, \mathbf{z}) \in \mathcal{C} \mid \mathcal{M}(q) \cap \mathcal{O} = \mathcal{Q} \right\},\tag{11}$$

where = (,), with :ℝ2 <sup>ℝ</sup> supposed to be regular; moreover, *x* and *y* are considered as limited, thus ≤ , ≤ . In this way, a bounded portion of the *x, y* plane has been defined. As a result, given a generic shaped robot in the configuration space , a traversa‐ bility function that considers an elevation terrain model can be expressed by the following:

$$\mathcal{T}(q) = 1 - \left| \frac{z\_q}{z\_{\max}} \right| \quad \forall q \in \mathcal{C}\_{tr}, z\_{\max} \neq 0 \mid \mathcal{M}(q) \subset \mathcal{C}\_{f:w}. \tag{12}$$

**Figure 5.** The elevation model better describes the sample terrain in **Figure 4**, the higher informative content allows to perform better cost‐based traversability analysis, in (a) the 3D mesh is presented, whereas the xy‐axis view is depicted in (b).

One should note that in Eq. (12) if then and the configuration will fall into low values of membership function and this implies that the point will not belong to . However, even though the configuration of the robot includes orientation angles in its formalization, this traversability function does not consider any orientation in its values and this results in a limitation in the practical application of pure elevation‐based methods. For the sake of completeness, we should consider the case of <sup>∞</sup>, where ∀ , but this case can be considered as trivial.

The example of this type of analysis is reported in **Figure 5**, where the values of are indicated as a color bar from blue corresponding to traversable regions, to red denoting not‐traversable part of terrain. It results evident that a control rule based on such analysis will bring the robot towards the lowest regions of the terrain, which though reasonable, it may be not the best behavior according to the objective of the robot movements. Let us observe that in this method, the robot shape is considered as a single point in the calculation of the traversability function, hence considering only the terrain elevation.

#### **4.3. Traversability model based on roughness index**

A widely used approach, for geometry and cost‐based terrain traversability analysis, consists in the definition of the roughness index [47]. It is defined as the standard deviation of the elevation values in a specific region of the terrain, given by the projection of the robot shape on the ground.

Given a terrain region considered as free space , defined as in Eq. (11), and a robot model described using any polygonal model. Then, it is possible to define the roughness index of the terrain, when the robot is in the configuration as the standard deviation of the elevation values of the surface given by the intersection between and .

 m<sup>2</sup> = ( ), *q q B EZ* (13)

where is the set of all points that fall into the intersection between the robot and the free space, and is the average of the elevation values in the same region. Since the values of are not limited in [0, 1] the traversability function related to the roughness index, according to the definition in Eq. (9), may be considered using a normaliza‐ tion as following:

$$\forall \mathcal{T}(q) = 1 - \frac{B\_q}{B\_{\text{max}}} \quad \forall q \in \mathcal{C}, B\_{\text{max}} \neq 0 \mid \mathcal{M}(q) \subset \mathcal{C}\_{\text{free}}.\tag{14}$$

As in the previous case, Eq. (14) → 0 if and the configuration *q* will fall into low values of membership function and this implies that it does not belong to . Moreover,

, as a result is well defined. Also in this case, if <sup>∞</sup>, ∀ can be considered as trivial.

**Figure 6** shows an example of traversability map obtained using the roughness index, for the sake of this calculation, the robot has been considered to cover an area of about 8 × 8 cells of the map having grid size of 0.25 m, corresponding to 2 meters in size. The consideration of the standard deviation on a terrain region calculated according to the robot's geometry may be considered as a robust method and, for this reason, widely used for practical applications. One should note that between the pure elevation traversability analysis and the roughness analysis, a specific region of the terrain appears as irregular and dangerous, corresponding to a local surface minimum. This evaluation agrees with the reality that a robot may get stuck into a hole. On the contrary, the same analysis does not mark as irregular the peak of the hill that may be perfectly traversable as upland. However, it is clear that also this method may fail in the simple case of a surface featuring a slope, which though regular and traversable, it may present high values of variance in its elevation [51].

**Figure 6.** Roughness traversability analysis result based on the roughness index in Eq. (13), integrated into the mem‐ bership function in Eq. (14); (a) 3D surface model; (b) xy‐view. The color bar indicates increasing values of traversabili‐ ty, where red corresponds to not traversable regions, while blue corresponds to traversable portion of terrain.

#### **4.4. Unevenness point descriptor‐based model**

even though the configuration of the robot includes orientation angles in its formalization, this traversability function does not consider any orientation in its values and this results in a limitation in the practical application of pure elevation‐based methods. For the sake of

The example of this type of analysis is reported in **Figure 5**, where the values of are indicated as a color bar from blue corresponding to traversable regions, to red denoting not‐traversable part of terrain. It results evident that a control rule based on such analysis will bring the robot towards the lowest regions of the terrain, which though reasonable, it may be not the best behavior according to the objective of the robot movements. Let us observe that in this method, the robot shape is considered as a single point in the calculation of the traversability function,

A widely used approach, for geometry and cost‐based terrain traversability analysis, consists in the definition of the roughness index [47]. It is defined as the standard deviation of the elevation values in a specific region of the terrain, given by the projection of the robot shape

Given a terrain region considered as free space , defined as in Eq. (11), and a robot model described using any polygonal model. Then, it is possible to define the roughness index

of the terrain, when the robot is in the configuration as the standard deviation of the

 m

where is the set of all points that fall into the intersection between the robot and the free space, and is the average of the elevation values in the same

roughness index, according to the definition in Eq. (9), may be considered using a normaliza‐

T ( )=1 - "Î ¹ Ì C MC , 0| ( ) . *<sup>q</sup>*

values of membership function and this implies that it does not belong to . Moreover,

*max*

*B*

of the surface given by the intersection between and .

<sup>2</sup> = ( ), *q q B EZ* (13)

are not limited in [0, 1] the traversability function related to the

*max free*

*q qB q <sup>B</sup>* (14)

and the configuration *q* will fall into low

, but this case can be

completeness, we should consider the case of <sup>∞</sup>, where ∀

considered as trivial.

128 Robot Control

on the ground.

elevation values

tion as following:

region. Since the values of

As in the previous case, Eq. (14) → 0 if

hence considering only the terrain elevation.

**4.3. Traversability model based on roughness index**

As an alternative analysis to solve the problems related to the variance of the elevation in sloped regular surfaces, the use of normal vectors to estimate surface irregularities was presented in [27], where the authors defined the unevenness point descriptor (UPD), as a simple choice to extract traversability information from 3D point cloud data. Specifically, the UPD describes surfaces using a normal analysis in a neighborhood, resulting in an efficient description of both irregularities and inclination.

Summarizing the concept, let be a point cloud, that is, a set of points defined by their Cartesian coordinates defined as in Eq. (3), and let *pq* be a given point defined as the *query point.* The neighborhood of *pq* in can be defined as in Eq. (4), given a search radius *dm* > 0. Then, we define the unevenness point descriptor FU in *pq* , as: (,) = , , where = ( , , ) is given by the vector sum of all the vectors normal vectors in the neighbors *Pq* , that is, <sup>=</sup> ∑ = 1 with = 1,...,, is defined by <sup>=</sup> /, and *k* is the number of elements in .

The components of provide information about the global direction of the local surface in the sensor reference frame. Whereas can be interpreted as a local inverse "unevenness index," since it assesses the degree of local roughness, and it depends on the distribution of the direction of the normal vectors in the neighborhood. is normalized by *k*, i.e., the number of points in *Pq* ; hence, it is possible to compare the unevenness index of different points among each other. The main advantages of this descriptor reside in its simplicity and robustness for traversability evaluation. Contrary to other methods, UPD detects the variations in the surface orientation instead of the variation of the pure elevation, which leads to a general description of regularity in the surface. Moreover, the UPD can be easily adapted to the robot's specific task by appropriately setting the neighborhood size, *dm.* In practice, its value is fixed at the beginning of the operations based on the robot geometric size [26]. As further observation,

given a neighborhood denoting a certain region of the terrain, its orientation can be written as follows:

$$\theta(P^q) = \cos^{-1}\left(\frac{r\_z^q}{\|\vec{r}^q\|}\right),\tag{15}$$

where represents the third component of , orthogonal to the *xy*‐plane, as a consequence, *d*(*Pq* ) represents the global orientation of the surface portion *Pq* respect to the plane *xy*.

To bring the unevenness index into the definition of a traversable region, we can consider as given the C‐Space and a forbidden space ⊂ , then a free space can be defined as in the following Eq. (16):

$$\mathcal{C}\_{\circ\_{\mathcal{H}^{\mathcal{I}}}} = \left\{ q \in \mathcal{C} \cap \mathcal{P} \mid \mathcal{M}(q) \cap \mathcal{O} \neq \mathcal{Q} \right\},\tag{16}$$

where is a generic portion of space expressed as a point cloud. Let us note that the meaning of the intersection with consists in a practical limitation of the C‐Space in the part the robot can see or has information about. Then, let us suppose to be given the unevenness index , the traversability region may be identified by the set in Eq. (9), where the membership function is given by the following:

$$T(q) = 1 - \zeta^{\*q} \quad \forall q \in \mathcal{C} \mid \mathcal{M}(q) \subset \mathcal{C}\_{\wedge m}. \tag{17}$$

is given by the vector sum of all the vectors

the direction of the normal vectors in the neighborhood.

q

) represents the global orientation of the surface portion *Pq*


*<sup>r</sup> <sup>P</sup>*

*q z*

r <sup>1</sup> ( )= , cos

ç ÷ ç ÷ è ø

*q*

represents the third component of , orthogonal to the *xy*‐plane, as a consequence,

C C PM O *free* = | () , {*q q* Î Ç Ç ¹Æ} (16)

*free q qq* (17)

*q*


*r*

To bring the unevenness index into the definition of a traversable region, we can consider as given the C‐Space and a forbidden space ⊂ , then a free space can be defined as in the

where is a generic portion of space expressed as a point cloud. Let us note that the meaning of the intersection with consists in a practical limitation of the C‐Space in the part the robot can see or has information about. Then, let us suppose to be given the unevenness index

> T ( )=1 - "Î Ì z

, the traversability region may be identified by the set in Eq. (9), where the membership

CM C | () . *<sup>q</sup>*

is defined by

The components of provide information about the global direction of the local surface in

index," since it assesses the degree of local roughness, and it depends on the distribution of

each other. The main advantages of this descriptor reside in its simplicity and robustness for traversability evaluation. Contrary to other methods, UPD detects the variations in the surface orientation instead of the variation of the pure elevation, which leads to a general description of regularity in the surface. Moreover, the UPD can be easily adapted to the robot's specific task by appropriately setting the neighborhood size, *dm.* In practice, its value is fixed at the beginning of the operations based on the robot geometric size [26]. As further observation, given a neighborhood denoting a certain region of the terrain, its orientation can be written

; hence, it is possible to compare the unevenness index of different points among

with = 1,...,,

the sensor reference frame. Whereas

<sup>=</sup> ∑ = 1

of points in *Pq*

as follows:

where

following Eq. (16):

function is given by the following:

*d*(*Pq*

in .

130 Robot Control

normal vectors in the neighbors *Pq*

can be interpreted as a local inverse "unevenness

<sup>=</sup> /, and *k* is the number of elements

is normalized by *k*, i.e., the number

respect to the plane *xy*.

, that is,

(15)

**Figure 7.** Unevenness point descriptor‐based model for geometric traversability analysis applied on a point cloud de‐ picting the sample terrain model. In (a) and (b), the 3D view and xy‐view, respectively, are depicted. The search radius for the UPD calculation is dm **=** 1 *m*.

**Figure 8.** UPD point·descriptor analysis. In (b) the 3D point cloud is shown using the color bar to denote traversability value, whereas its relative ‐view is shown in (c). As the point cloud has been obtained by stereo‐triangulation, the left‐camera image is shown in (a). The roughness index‐based analysis in Eq. (14) produces poor results on the same scenario using a DEM approach, see 3D‐view (d) and xy‐view (e).

Now, let us observe that in its original form the UPD considers the robot model into the parameter , at least in its size, that has been said to be fixed at the beginning of robot operations, according to its shape. However, it is possible to generalize the concept of neigh‐ borhood considering the set of points which fall in the set not as a sphere neighborhood but as the intersection between a polyhedral robot model and the free part of C‐Space, hence This generalization allows the user to better define the robot shape into the descriptor.

The example of the UPD analysis, for the same terrain model considered as sample, is reported in **Figure 7**, for the sake of visibility, the values of *ζq* have been normalized to their minimum values in the region, since the results of variation were close to regularity. During the calculation, the search radius has been set to 1 m, according to the previous example of the roughness index. Contrary to the previous approaches, in the UPD analysis, the strong variations such as the depressions are now considered as not regular showing a different perception of the traversability of this terrain model.

As last example, in **Figure 8**, the UPD has been applied on a point cloud obtained by triangu‐ lation on a stereocamera in real environment, the value of the traversabilitv function is reported using in color scale, whereas the left‐camera image of the scenario is reported in **Figure 8a**. This scene has been extracted from a dataset thoroughly analyzed in [51]. It can be interesting to note that the presented case scenario features a ramp to access an indoor structure. The ramp is considered as regular via UPD analysis, whereas it may be misinterpreted considering elevation model as well as the roughness index. All the borders are correctly detected as not traversable regions. As the matter of fact, **Figure 8d** and **Figure 8e** present the same scenario described using a DEM and the traversability function in Eq. (14). The misunderstanding of the scenario leads to the erroneous classification of the ramp to access the building behind it as fully not traversable. On the contrary, in **Figure 8b** and **Figure 8c** the scene is properly interpreted using the UPD approach.

#### **5. Conclusion and further extensions**

Along the chapter, different methods of geometry‐based traversability for mobile robotics have been explored. A thorough review on the topic suggests that the future trend of sensors and space description for traversability purposes will refer to point clouds and time‐of‐flight sensors, or stereo‐3D reconstruction. The necessity to improve the description of terrain, removing the assumption of regularity, will bring the robot towards the full 3D reconstruction of the environment at least in short range visibility. Among different methods analyzed in the discussion, the UPD has demonstrated highest capability of recognition even though it could be costly in terms of computational performances. The contributions in this work are as follows: (i) a review of the field with comparison among technologies, (ii) a new definition of travers‐ ability that can be of general value for robot navigation purposes, and (iii) a comparison among literature methods including practical examples.

To conclude this chapter, it is worth to give some possible extensions of this work and future developments. One of them could be the definition of traversable regions in terms of proba‐ bility. Indeed, it should be possible to include a probability function in terms of risk‐of‐collision or probability of traverse, in which high values refer to minimum probability of collision (i.e., max traversing probability) or low values imply maximum probability of collision (i.e., min traversing probability). Moreover, the traversability regions as defined during this chapter may fit for navigation purposes using the common potential fields, where the potential function will consider traversable regions as "attractive." On the contrary, "repulsive" regions will coincide with low values of traversability function. Literature in this field typically considers potential functions that use the distance from obstacles instead of a complete traversability description.

### **Author details**

#### Mauro Bellone

Now, let us observe that in its original form the UPD considers the robot model into the parameter , at least in its size, that has been said to be fixed at the beginning of robot operations, according to its shape. However, it is possible to generalize the concept of neigh‐ borhood considering the set of points which fall in the set not as a sphere neighborhood but as the intersection between a polyhedral robot model and the free part of C‐Space, hence

The example of the UPD analysis, for the same terrain model considered as sample, is

minimum values in the region, since the results of variation were close to regularity. During the calculation, the search radius has been set to 1 m, according to the previous example of the roughness index. Contrary to the previous approaches, in the UPD analysis, the strong variations such as the depressions are now considered as not regular showing a different

As last example, in **Figure 8**, the UPD has been applied on a point cloud obtained by triangu‐ lation on a stereocamera in real environment, the value of the traversabilitv function is reported using in color scale, whereas the left‐camera image of the scenario is reported in **Figure 8a**. This scene has been extracted from a dataset thoroughly analyzed in [51]. It can be interesting to note that the presented case scenario features a ramp to access an indoor structure. The ramp is considered as regular via UPD analysis, whereas it may be misinterpreted considering elevation model as well as the roughness index. All the borders are correctly detected as not traversable regions. As the matter of fact, **Figure 8d** and **Figure 8e** present the same scenario described using a DEM and the traversability function in Eq. (14). The misunderstanding of the scenario leads to the erroneous classification of the ramp to access the building behind it as fully not traversable. On the contrary, in **Figure 8b** and **Figure 8c** the scene is properly

Along the chapter, different methods of geometry‐based traversability for mobile robotics have been explored. A thorough review on the topic suggests that the future trend of sensors and space description for traversability purposes will refer to point clouds and time‐of‐flight sensors, or stereo‐3D reconstruction. The necessity to improve the description of terrain, removing the assumption of regularity, will bring the robot towards the full 3D reconstruction of the environment at least in short range visibility. Among different methods analyzed in the discussion, the UPD has demonstrated highest capability of recognition even though it could be costly in terms of computational performances. The contributions in this work are as follows: (i) a review of the field with comparison among technologies, (ii) a new definition of travers‐ ability that can be of general value for robot navigation purposes, and (iii) a comparison among

reported in **Figure 7**, for the sake of visibility, the values of *ζq*

perception of the traversability of this terrain model.

interpreted using the UPD approach.

**5. Conclusion and further extensions**

literature methods including practical examples.

into the descriptor.

132 Robot Control

This generalization allows the user to better define the robot shape

have been normalized to their

Address all correspondence to: bellonemauro@gmail.com

Department of Applied Mechanics, Chalmers University of Technology, Gothenburg, Sweden

#### **References**


[19] E. Coyle and E. G. E. Jr., "A comparison of classifier performance for vibration‐based terrain classification," tech. rep., DTIC Document, 2008.

[7] H. Roncancio, M. Becker, A. Broggi, and S. Cattani, "Traversability analysis using terrain mapping and online‐trained terrain type classifier," in *Intelligent*

[8] S. Thrun, "Learning occupancy grid maps with forward sensor models,"

[9] P. Papadakis, F. Pirri "3D Mobility Learning and Regression of Articulated, Tracked Robotic Vehicles by Physics–based Optimization" International conference on Virtual Reality Interaction and Physical Simulation, Eurographics, Dec 2012,

[10] Y. Tanaka, Y. Ji, A. Yamashita, and H. Asama, "Fuzzy based traversability analysis for a mobile robot on rough terrain," in *Proceedings of the 2015*

[11] B. Suger, B. Steder, and W. Burgard, "Traversability analysis for mobile

[12] F. Oniga and S. Nedevschi, "Processing dense stereo data using elevation maps: Road surface, traffic isle, and obstacle detection," *IEEE Transactions on Vehicular*

[13] A. Broggi, E. Cardarelli, S. Cattani, and M. Sabbatelli, "Terrain mapping for off‐road autonomous ground vehicles using rational b‐spline surfaces and stereo vision," in *Intelligent Vehicles Symposium (IV), 2013 IEEE,* pp. 648–653, 2013. [14] K. Dongshin, M. O. Sang, and M. R. James, "Traversability classification for ugv navigation: A comparison of patch and superpixel representations," (San Diego, CA), pp. 3166‐3173, International Conference on Intelligent Robots and Systems, 2007. [15] A. Howard and H. Saraji, "Vision‐based terrain characterization and traversability assessment," *Journal of Robotic System,* vol. 18, no. 10, pp. 77–587, 2001. [16] S. Thrun, M. Montemerlo, and A. Aron, "Probabilistic Terrain Analysis For High–Speed Desert Driving" In Robotics: Science and Systems, pp. 16–19,

[17] M. Häselich, M. Arends, N. Wojke, F. Neuhaus, and D. Paulus, "Probabilistic terrain classification in unstructured environments," *Robotics and Autonomous*

[18] K. Iagnemma, H. Shibly, and S. Dubowsky, "On‐line terrain parameter estimation for planetary rovers," in *Robotics and Automation, 2002. Proceedings. ICRA '02.*

*IEEE International Conference on,* vol. 3, pp. 3142–3147, IEEE, 2002.

robots in outdoor environments: A semi‐supervised learning approach based on 3d‐lidar data," in *Robotics and Automation (ICRA), 2015 IEEE International*

*IEEE International Conference on Robotics and Automation,* 2015.

*Vehicles Symposium Proceedings, 2014 IEEE*, pp. 1239–1244, IEEE, 2014.

*Autonomous Robots,* vol. 15, no. 2, pp. 111–127, 2003.

Darmstadt, Germany.

134 Robot Control

*Conference on,* pp. 3941‐3946, 2015.

*Technology,* vol. 59, pp. 1172–1182, 2010.

Philadelphia, USA, August 2006.

*Systems,* vol. 61, no. 10, pp. 1051–1059, 2013.


[43] B. Cafaro, M. Gianni, F. Pirri, M. Ruiz, and A. Sinha, "Terrain traversability in rescue environments," in *2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR),* pp. 1–8, Oct 2013.

[32] L. Spedicato, N. I. Giannoccaro, G. Reina, and M. Bellone, "Three different approaches for localization in a corridor environment by means of an ultrasonic wide beam", International Journal of Advanced Robotic Systems, vol. 10, pp. 163–172, March 2013.

[33] J.L. Torres, J.L. Blanco, M. Bellone, F. Rodrìguez, A. Gimènez, and G. Reina – "A proposed software framework aimed at energyefficient autonomous driving of electric vehicles" – International Conference on Simulation, Modeling, and Programming for Autonomous Robots, Bergamo, Italy October 2014, pp. 219–230, ISBN 978–3–319–

[34] D. Borrmann, J. Elseberg, K. Lingemann, A. Nüchter, and J. Hertzberg, "Globally consistent 3d mapping with scan matching," *Robotics and Autonomous Systems,* vol. 56,

[35] I. S. Kweon and T. Kanade, "High‐resolution terrain map from multiple sensor data," *IEEE Transaction on Pattern and Machine Intelligence,* vol. 14, pp. 278–292, 1992.

[36] J. Larson, M. Trivedi, and M. Bruch, "Off‐road terrain traversability analysis and hazard avoidance for ugvs," (Baden‐Baden, Germany), IEEE Intelligent Vehicles Symposium,

[37] P. Pfaff, R. Triebel, and W. Burgard, "An efficient extension of elevation maps for outdoor terrain mapping," *In Proceedings of the International Conference on Field and*

[38] T. Ohki, K. Nagatani, and K. Yoshida, "Path planning for mobile robot on rough terrain based on sparse transition cost propagation in extended elevation maps," pp. 494–499, 2013 IEEE International Conference on Mechatronics and Automation (ICMA), Aug

[39] N. Vandapel, D. F. Huber, A. Kapuria, and M. Hebert, "Natural terrain classification using 3‐d ladar data," (New Orleans, LA, USA), pp. 5117–5122, IEEE International

[40] M. Whitty, S. Cossell, K. S. Dang, J. Guivant, and J. Katupitiya, "Autonomous navigation using a real‐time 3d point cloud," in *2010 Australasian Conference on Robotics and*

[41] M. Bellone and G. Reina, "Road surface analysis for driving assistance," in *Workshop Proceedings of IAS‐13 13th International Conference on Intelligent Autonomous Systems*

[42] T. Braun, H. Bitsch, and K. Berns, "Visual terrain traversability estimation using a combined slope/elevation model", Advances in Artificial Intelligence Volume 5243 of the series Lecture Notes in Computer Science pp 177–184, Springer, 2008, ISBN 978–3–

11899–4;

136 Robot Control

2011.

2013.

no. 2, pp. 130–142, 2008.

*Service Robotics (FSR),* pp. 165–176, 2005.

Conference on Robotics and Automation, 2004.

*Automation,* pp. 1–3, 2010.

540–85845–4.

*Padova (Italy),* pp. 226–234, 2014.


## *Edited by Efren Gorrostieta Hurtado*

This book includes a selection of research papers in robot control applications. The description of projects using robotic systems in areas such as vision, navigation, path planning, trajectories, non-holonomic systems, mobile robotics, robot control with very specific structures, as well as artificial intelligence systems is pointed out. It also presents several tools and mathematical concepts that allow the development and operation of robotic systems. Additionally, the development of different ideas in control systems that are useful and hopefully enriching for the reader are also presented in this book.

Photo by Ociacia / iStock

Robot Control

Robot Control

*Edited by Efren Gorrostieta Hurtado*