lpz-Robots simulation with a fourwheeled robot, driving on a playground
and trying to avoid collisions with other objects.

Implement the obstacle avoidance feature of the two-wheeled Differential robotcar for a four-wheeled car using infrared (IR) or other type of sensors. Place random obstacles or design a maze to demonstrate its functionality.

## Robot Setup

Figure 1: Screenshot of our robot

LPZ-Robots is a C++ based package, wich provides classes and functions to physically simulate a robot in a given enviroment. If you want to learn more about lpz-robots, visit their hompage at Uni Leipzig.

In Figure 1 you can see a picture of our robot-setup. We used the $$\texttt{fourwheeled}$$ template with the fourwheeled $$\texttt{nimm4}$$ robot. We then added the IR sensors and $$\texttt{basiccontroller}$$ of the basic example. The complete source code is available in the download section.

We had two ideas to improve the given basic-controller:
The first idea was to integrate a fuzzy logic to the control-sequencies in order to make the movement of the robot smoother and let it appear more natural. Our second idea was called 'vector controller'. You will find more details in the corresponding sections of this documentation.

### The If Controller

At first we made the robot work with a very simple controller that uses if-else clauses to decide wether to turn left/right or go straight ahead.
Here is the slightly modified code of the $$\texttt{basiccontroller}$$:

if (sensors[SIdx("IR front left")] > threshold_backward &&
sensors[SIdx("IR front right")] > threshold_backward) {
// move backward if something's in front of the robot
motors[0] = -.5; // right motor
motors[1] = -.5; // left motor
}
else if (sensors[SIdx("IR left")] > threshold_sides ||
sensors[SIdx("IR front left")] > threshold ||
sensors[SIdx("IR front left left")] > threshold_front_sides) {
// turn right if something's on the left side
motors[0] = -.25; // right motor
motors[1] = .75;  // left motor
}
else if (sensors[SIdx("IR right")] > threshold_sides ||
sensors[SIdx("IR front right")] > threshold ||
sensors[SIdx("IR front right right")] > threshold_front_sides) {
// turn left if something's on the right side
motors[0] = .75;  // right motor
motors[1] = -.25; // left motor
}
else {
// else move forward
motors[0] = 1.; // right motor
motors[1] = 1.; // left motor
}

Codebox 1: The Basic Controller with if-clauses

With the different thresholds $$\texttt{threshold}$$, $$\texttt{threshold_front_sides}$$, $$\texttt{threshold_sides}$$ and $$\texttt{threshold_backward}$$ we can change the sensitivity of the different IR sensors. The sensors return values between $$0$$ and $$1$$, where $$0$$ means no obstacle in range of the sensor and $$1$$ means there is something right in front of it. The robot is steered like a tank, wich is the reason why we have only two different motorspeed values $$\texttt{motors[0]}$$ and $$\texttt{motors[1]}$$.

In Video 1 one can see the behaviour of the robot with the simple If Controller. Figure 2 shows more precisely the arrangement of the IR-sensors.

Video 1: The behaviour of the robot using the If Controller in the $$\texttt{Corridor}$$ playground.
Figure 2: Schematic drawing of our robot with all its IR sensors, motors and their respective identifiers.

As you can see in Video 1, the If Controller works quite well in the choosen $$\texttt{Corridor}$$ playground: The robot does not collide with the walls and finds its way through the corridor.

#### Notation

For reasons of simplification, one can introduce some symbols representing the used variables and assignments in Codebox 1. The vector $$\vec{M} = \left( \texttt{motors[0]}, \ \texttt{motors[1]} \right)^T$$ represents the actual motor values at a given point in time. The vectors $$\vec{M}_i$$, with $$i \in \{ 1, 2, 3, 4 \}$$ represent the four different modes of movement (cf. Codebox 1): \begin{align} \vec{M}_1 &= \left( \begin{matrix} - 0.5 \\ - 0.5 \end{matrix} \right) \nonumber \\ \vec{M}_2 &= \left( \begin{matrix} - 0.25 \\ + 0.75 \end{matrix} \right) \nonumber \\ \vec{M}_3 &= \left( \begin{matrix} + 0.75 \\ - 0.25 \end{matrix} \right) \nonumber \\ \vec{M}_4 &= \left( \begin{matrix} + 1.0 \\ + 1.0 \end{matrix} \right) \ , \\ \end{align} where $$\vec{M}_1$$ represents the backward, $$\vec{M}_4$$ the forward movement, $$\vec{M}_2$$ a turn to the right and $$\vec{M}_3$$ a turn to the left.

The assignment of a value, that corresponds to a $$\texttt{=}$$ in the Code, will in the following be denoted by the mathematical symbol $$\mapsto$$. The idea of replacing the if structure in Codebox 1 by sigmoid functions is based on the fact, that in principle one can rewrite the code in the following way: $$\vec{M} \quad \mapsto \quad \sum_{i=1}^4 c_i \vec{M}_i \ , \label{eq:assignment_motor_values}$$ where $$c_i$$ with $$i \in \{ 1, 2, 3, 4 \}$$ are prefactors that mirror the behaviour and hierarchy of the if queries used in the If Controller, i.e. they are $$c_i = 1$$, corresponding to the truth value True, if condition $$i$$ is met and the others are not and $$c_i = 0$$, corresponding to False, otherwise. As the hierarchy of if queries should be the same as in the If Controller, there is always only one of the $$c_i$$'s that is equal to $$1$$, the other ones beeing equal to $$0$$.

In the next two sections there is a detailed description of how $$c_i$$'s can be calculated with the help of Heaviside functions, in order to get a controller equal to the If Controller. When one has found a proper way of representing the $$c_i$$'s with Heaviside functions, one can simply replace the Heaviside functions with sigmoid functions, that allows the $$c_i$$'s to be in the interval $$c_i \in [ 0, 1 ]$$ and hence, enables the mixing of different modes of movement and a smooth transition between those modes. As the $$c_i$$'s in this case are no binary truth values any more, they resemble the idea of continuous truth values that appear in the so called Fuzzy Logic. Each $$c_i$$ is then a continuous measure of how True or False an if query is.

## Sigmoid Controller

#### Boolean algebra with Heaviside functions

In order to introduce a smooth transition between the different modes, in a first step the upper code (Codebox 1) for the If Controller with several if blocks was reformulated with the help of the Heaviside function $$\Theta (x)$$, for which $$\Theta(x) = \begin{cases} 1 &\mbox{if } \ x \geq 0 \\ 0 &\mbox{otherwise } \end{cases} \label{eq:heaviside}$$ holds true.

When one defines the integer 1 as True and the integer 0 as False, one can write all if queries of the type $$a \geq b \quad \Leftrightarrow \quad a - b \geq 0$$ as $$\Theta(a-b) \ .$$ As $$\Theta (x)$$ only takes the values 0 or 1, one can use basic algebra in order to formulate the boolean operation A AND B, in the following denoted by $$A \wedge B$$, where $$A = \Theta(a)$$ and $$B = \Theta(b)$$ are logic values, as a multiplication of $$\Theta$$ functions: $$\text{AND:} \quad A \wedge B \quad \hat{=} \quad \Theta(a) \cdot \Theta(b) \ . \label{eq:and}$$

The boolean operation NOT A, which in the following will be denoted as $$\lnot A$$ can be translated into the Heaviside representation as follows: $$\text{NOT:} \quad \lnot A \quad \hat{=} \quad \Theta(-a) \ . \label{eq:not}$$ However, there is a problem at $$a = 0$$, as for the given definition of the Heaviside function from Equation (\ref{eq:heaviside}), it is $$\Theta(- 0) = \Theta(0) = 1$$. On the other hand, for $$a = 0$$ it is $$A = \Theta(a) = 1$$ and therefore $$\lnot A = 0$$. Accordingly, Equation (\ref{eq:not}) only holds true for $$a \neq 0$$. With this subtlety in mind, one can go on and check, how the logical operation A OR B could look like, when written with Heaviside functions.

The logical A OR B operation, in the following denoted as $$A \vee B$$, can, according to basic boolean algebra, be written as $$A \vee B \quad = \quad \lnot \left\{ \left( \lnot A \right) \wedge \left( \lnot B \right) \right\} \ ,$$ and hence, can be reduced to the already discussed operations AND and NOT. However, due to the aforementioned subtlety, that causes $$\Theta(\Theta(x)) \neq \Theta(x)$$ for values of $$x < 0$$, it is no good idea to do nesting of Heaviside functions in order to compute truth values. It turns out, that there is another reason concerning the later discussed replacement of Heaviside with sigmoid functions, that turns the nesting into a bad idea.

However, there is a different representation of $$A \vee B$$ that proofs helpful: $$A \vee B \quad = \quad \left( A \wedge B \right) \quad \vee \quad \left( (\lnot A) \wedge B \right) \quad \vee \quad \left( A \wedge (\lnot B ) \right) \ . \label{eq:or_repr}$$ From the first sight, Equation (\ref{eq:or_repr}) only turns one OR expression into two. However, we know that in any case, either one or none of the three terms is True, while the others are False. Hence, in terms of Heaviside functions, we can simply write the OR operation as follows: $$\text{OR:} \quad A \vee B \quad \hat{=} \quad \Theta(a) \cdot \Theta(b) \quad + \quad \Theta(-a) \cdot \Theta(b) \quad + \quad \Theta(a) \cdot \Theta(-b) \ . \label{eq:or}$$ Note that the algebraic representation of OR as $$+$$ is not directly applicable to $$A$$ and $$B$$, in the sense, that $$A \vee B \quad \neq \quad \Theta(a) + \Theta(b) \ . \label{eq:wrong_or}$$ The right hand side of Equation (\ref{eq:wrong_or}) is not limited to the values 0 and 1, as when both $$A$$ and $$B$$ are True, we have $$1 + 1 = 2$$. With nesting the result into another Heaviside function, this problem could be solved, however the nesting ought to be avoided here.

Now we have all expressions we need, to find the correct expressions for $$c_i$$'s. The details on this can be found in the next section.

#### Replacing if clauses by Heaviside formalism

All logical queries of the If Controller (cf. Codebox 1), are of type $$S - T > 0 , \label{eq:basicineq}$$ where $$S$$ is some sensor value and $$T$$ the corresponding threshold. It makes little to no difference for the controller, whether there is a $$>$$ or a $$\geq$$ in Inequality (\ref{eq:basicineq}). Therefore, the corresponding truth value for Inequality (\ref{eq:basicineq}) will be calculated by $$\Theta(S-T)$$, although the above definition of $$\Theta(x)$$ uses the $$\geq$$ sign.

In Codebox 2 the result of above discussions in terms of a programm code for the Heaviside Controller is presented. Variables $$\texttt{A1, A2, A3, B1, B21, B22, B31, B32}$$ are auxiliary variables storeing the left hand side of Inequality (\ref{eq:basicineq}) for all sensors and their corresponding thresholds. These auxiliary variables are the arguments of the Heaviside function, which is here calculated via the function $$\texttt{double activation(double x)}$$. The results of the Heaviside function are then used in order to apply the obtained expressions for logical operations from the preceding section and therefore calculate the truth values for Conditions 1, 2, 3 and store them in $$\texttt{C1, C2, C3}$$. The variables $$\texttt{NOTC1, NOTC2, NOTC3}$$ are calculated according to the same algebraic expressions of logical operators and represent the negations of $$\texttt{C1, C2, C3}$$.

In the last few lines of Codebox 2 the proper motor values are calculated from $$\texttt{C1, C2, C3}$$ and $$\texttt{NOTC1, NOTC2, NOTC3}$$ and are assigned to $$\texttt{motors[0]}$$ and $$\texttt{motors[1]}$$. The hierarchy of if queries from the If Controller is hereby preserved, due to the following correspondence: \begin{align} c_1 &= \texttt{C1} \nonumber \\ c_2 &= \texttt{NOTC1} \cdot \texttt{C2} \nonumber \\ c_3 &= \texttt{NOTC1} \cdot \texttt{NOTC2} \cdot \texttt{C3} \nonumber \\ c_4 &= \texttt{NOTC1} \cdot \texttt{NOTC2} \cdot \texttt{NOTC3} \ , \end{align} where $$c_i \ , i \in \{1, 2, 3, 4\}$$ refers to the prefactors that were introduced above. With this correspondence the assignments of motor values simply follows Equation (\ref{eq:assignment_motor_values}).

It is noteworthy to mention, that in some cases, when one of the arguments of the Heaviside function is exactly equal to $$0$$, there might be some strange behaviour, as $$\texttt{C1, C2, C3}$$ and $$\texttt{NOTC1, NOTC2, NOTC3}$$ are no longer truth values. They might get greater than $$1$$ and it is possible, that $$\texttt{NOTC1, NOTC2, NOTC3}$$ is no longer the negation of $$\texttt{C1, C2, C3}$$. There are two reasons why this problem can be ignored. The first reason beeing, that the frequency and duration of this problem happening during the simulation is negligible. The second reason is, that the Heaviside function gets replaced by the sigmoid function anyways. The hypothesis, that the problem can be ignored is supported by the fact, that no difference in the behaviour of the robot when switching between If Controller and Heaviside Controller can be observed.

// auxiliary variables; arguments of Heavisides
double A1, A2, A3;
double B1, B21, B22, B31, B32;

double C1, C2, C3;             // Conditions 1, 2, 3
// (results of algebraic representation of logical operators as Heavisides)
double NOTC1, NOTC2, NOTC3;    // Negations of Conditions 1, 2, 3

// ------------ Condition 1 ------------
A1    = sensors[SIdx("IR front left")] - threshold_backward;
B1    = sensors[SIdx("IR front right")] - threshold_backward;

// C1 = A1 AND B1
C1    = activation( A1) * activation( B1);
NOTC1 = activation(-A1) * activation( B1) +
activation( A1) * activation(-B1) +
activation(-A1) * activation(-B1)

// ------------ Condition 2 ------------
A2    = sensors[SIdx("IR left")] - threshold_sides;
B21   = sensors[SIdx("IR front left")] - threshold;
B22   = sensors[SIdx("IR front left left")] - threshold_front_sides;

// C2 = A2 OR B21 OR B22
C2    = activation( A2) * activation(-B21) * activation(-B22) +
activation(-A2) * activation( B21) * activation(-B22) +
activation(-A2) * activation(-B21) * activation( B22) +
activation( A2) * activation( B21) * activation(-B22) +
activation( A2) * activation(-B21) * activation( B22) +
activation(-A2) * activation( B21) * activation( B22) +
activation( A2) * activation( B21) * activation( B22);
NOTC2 = activation(-A2) * activation(-B21) * activation(-B22);

// ------------ Condition 3 ------------
A3    = sensors[SIdx("IR right")] - threshold_sides;
B31   = sensors[SIdx("IR front right")] - threshold;
B32   = sensors[SIdx("IR front right right")] - threshold_front_sides;

// C3 = A3 OR B31 OR B32
C3    = activation( A3) * activation(-B31) * activation(-B32) +
activation(-A3) * activation( B31) * activation(-B32) +
activation(-A3) * activation(-B31) * activation( B32) +
activation( A3) * activation( B31) * activation(-B32) +
activation( A3) * activation(-B31) * activation( B32) +
activation(-A3) * activation( B31) * activation( B32) +
activation( A3) * activation( B31) * activation( B32);
NOTC3 = activation(-A3) * activation(-B31) * activation(-B32);

// ------------ Assign Motor values ------------
//          backward +  turn right      +  turn left             + forward
motors[0] = -0.5*C1  +  (-.25)*NOTC1*C2 +  ( .75)*NOTC1*NOTC2*C3 + 1.0*NOTC1*NOTC2*NOTC3;   // right motor
motors[1] = -0.5*C1  +  ( .75)*NOTC1*C2 +  (-.25)*NOTC1*NOTC2*C3 + 1.0*NOTC1*NOTC2*NOTC3;   // left motor

Codebox 2: Basic controller with Heaviside ($$\texttt{activation}$$) function

#### Replacing Heaviside with sigmoid function

The sigmoid function $$\text{sig}(x)$$ is defined as follows: $$\text{sig}(x) = \frac{1}{1 + \exp\left\{-s\left(x-b\right)\right\}} \quad \in [0, 1] \ ,$$ where $$s$$ determines the slope of the transition between $$0$$ and $$1$$ and $$b$$ is a bias that shifts the function along the $$x$$ axis. In our project we found $$b = 0.05$$ and $$s = 10.0$$ to be suitable. The function $$\text{sig}(x)$$ for these parameters can be seen in Figure 3.

In the downloadable code $$b$$ corresponds to the variable $$\texttt{bias}$$ of the $$\texttt{activation}$$ function and $$s$$ corresponds to the parameter $$\texttt{slope_sigm}$$ that can be changed during runtime. The behaviour of the $$\texttt{activation}$$ function either being a Heaviside or a sigmoid function can be changed during runtime by the boolean variable $$\texttt{heaviside}$$. To switch from the If Controller to Heaviside or Sigmoid Controller one can set the runtime parameter $$\texttt{use_if}$$ to $$0$$.

First thing to notice when investigating the behaviour of the robot for the Sigmoid Controller, is the smooth transition to the Heaviside Controller, when taking the limit of $$s \rightarrow \infty$$. In this limit we easily see, that the variable $$b$$ only shifts our previously introduced threshold $$T$$. When $$s$$ takes a finite value, $$b$$ determines the point where $$\text{sig}(x=b) = \frac{1}{2}$$. Additionally, $$b$$ determines how much $$\text{sig}(x)$$ and $$\text{sig}(-x)$$ overlap and is therefore important when the logic negation operation is calculated - cf. Equation (\ref{eq:not}).

As mentioned previously the nested versions of algebraic expressions for truth value calculation is a bad idea when using Heaviside functions. This also holds true for the Fuzzy Logic with sigmoid functions: The result of the sigmoid function is strictly in the interval $$\text{sig}(x) \in [0, 1]$$, while the result for nested sigmoid functions only makes up subintervalls of $$[0, 1]$$, e.g.: $$\text{sig}(\text{sig}(x))\Big\vert_{b=0} \in [0.5, \text{sig}(1)]$$. This issue can in principle be solved by rescaling the outcome of the inner sigmoid function. However, here the solution of avoiding nested Heavisides and sigmoids was chosen instead.

The $$\texttt{activation}$$ function allows via the boolean variable $$\texttt{heaviside}$$ to switch between Heaviside and sigmoid output during runtime. Accordingly the code for the sigmoid controller remains unchanged compared to the heaviside controller, except for a small change in the assignment of motor values. The values $$\texttt{C1, C2, C3}$$ and $$\texttt{NOTC1, NOTC2, NOTC3}$$ for the Heaviside case are either $$0$$ or $$1$$, which allows the controller to use the full range of possible motor values from the interval $$[-1, 1]$$. For the Sigmoid Controller the motor values tend to be in a smaller interval, which makes the robot drive more slowly. In order to address this problem, the motor values were mapped to the interval $$[-1, 1]$$ via a $$\tanh(x)$$ function with a suitable slope of $$\texttt{slope_tanh} = 25.0$$ - cf. Codebox 3.

// ------------ Assign Motor values ------------
//                           backward +  turn right      +  turn left             + forward
motors[0] = tanh(slope_tanh*(-0.5*C1  +  (-.25)*NOTC1*C2 +  ( .75)*NOTC1*NOTC2*C3 + 1.0*NOTC1*NOTC2*NOTC3));   // right motor
motors[1] = tanh(slope_tanh*(-0.5*C1  +  ( .75)*NOTC1*C2 +  (-.25)*NOTC1*NOTC2*C3 + 1.0*NOTC1*NOTC2*NOTC3));   // left motor

Codebox 3: Changes of the Heaviside Controller for the Sigmoid Controller

In Video 2 one can see the comparison between the If Controller that causes a jittering movement and the smooth movements of the robot for the Sigmoid Controller. As expected the behaviour of the robot is in principle the same, however, the transition between the different modes of movements is smooth for the Sigmoid Controller.

Video 2: Comparison between If and Sigmoid controller
Figure 3: Sigmoid function with $$b = 0.05$$ and $$s = 10$$.

## Vector Controller

We also thought about a completely different approach to design a controller. Our robot perceives his environment only through the infrared sensors, attached around his body. The idea was to "draw a mental picture" of the environment measured by the sensors and then head in the direction where most likely is no obstacle blocking the way.

To know which direction is safe to go, a vector is calculated, that points in the direction of the - on average - largest distance to surrounding obstacles, calculated by the sensor data. In Codebox 4 the components of the vector $$\vec{V} = ( \texttt{vecx},\texttt{vecy} )^T$$ are calculated and assigned to the motors.

// computing the x- and y- components of the vector
double vecx =  - 1.0 *(1-sensors[SIdx("IR left")])            - 0.7*(1-sensors[SIdx("IR front left left")])
- 0.15*(1-sensors[SIdx("IR front left")])      - 0.3*(1-sensors[SIdx("IR back left")])
+ 0.15*(1-sensors[SIdx("IR front right")])     + 0.7*(1-sensors[SIdx("IR front right right")])
+ 1.0 *(1-sensors[SIdx("IR right")])           + 0.3*(1-sensors[SIdx("IR back right")]) ;

double vecy =    0.7 *(1-sensors[SIdx("IR front left left")]) + 0.98*(1-sensors[SIdx("IR front left")])
+ 0.98*(1-sensors[SIdx("IR front right")])     + 0.7 *(1-sensors[SIdx("IR front right right")])
- 1.68*(1-sensors[SIdx("IR back left")])       - 1.68*(1-sensors[SIdx("IR back right")]) ;

// set the motor values
motors[0] = base_speed + vecy - vecx; // right motor
motors[1] = base_speed + vecy + vecx; // left motor

Codebox 4: The Vector Controller

As mentioned earlier, an IR sensor returns $$1$$ when an obstacle is right in front of it. To obtain a vector, pointing in the opposite direction of the obstacle, we use $$\texttt{ 1-sensors[SIdx("IR ...")] }$$. For $$\texttt{vecx}$$ the prefactors of the sensor values are a result of simple vector-algebra: The $$x$$ component of e.g. $$\texttt{IR front left}$$ is: $$v_x^{\text{FL}} = \sin(\pi / 20) = 0.15$$ (cf. Figure 4). It is required that $$\vec{V} = (0,0)^T$$, when there is no obstacle around the robot (in the range of the IR sensors), i.e. when all sensors return $$0$$. This works well for $$\texttt{vecx}$$, since the arrangement of the sensors is symmetrical to the $$y$$ axis.

As for $$\texttt{vecy}$$ the arrangement of the sensors is not symmetrical to the $$x$$ axis, but we still demand $$\vec{V}$$ to be $$\vec0$$, when no obstacle is around, the two back sensors have to cancel out the front sensors, so their prefactors are adjusted to be the sum of the front sensors.

To let the robot go forward, when no obstacle is around, we set the $$\texttt{ base_speed }$$ variable. Its default value is $$1$$ and it is modifiable at runtime.

Figure 4: Schematic that shows the angular arrangement of the IR-sensors
Video 3: Behaviour of the Vector Controller.

A little flaw of this controller occurs, whenever $$\vec{V} = ( 0 , -\texttt{ base_speed } )^T$$ is met. Then both, $$\texttt{motors[0]}$$ and $$\texttt{motors[1]}$$ become $$0$$ and the robot gets stuck. This can be observed in Video 5.

## Comparison of all controllers

In Video 4 and 5 one can see the behaviour of the robot for all discussed controllers in a different playground called $$\texttt{SquareGround}$$ either with or without random obstacles. Due to the hierarchical structure for the different modes of movement for both If and Sigmoid Controller a turn to the right is not equitable compared to a turn to the left. This can be observed especially in Video 4.

In Video 5 obviously for both If and Vector Controller the robot tends to get stuck at some stage. Concerning the If Controller one can again observe the jittering movement when switching between different modes of movement at a high obstacle density. In turn the Vector Controller gets stuck whenever $$\vec{V} = (0, -\texttt{base_speed})^T$$.

However, there is also a drawback for the Sigmoid Controller: When the obstacle density is too high the robot often doesn't fulfill the task of avoiding obstacles and instead bumps into them and pushes them out of the way. This issue might be solvable by using more sensors around the robot or by finetuning the existing thresholds.

Video 4: Comparison of all controllers in playground environment without any obstacles
Video 5: Comparison of all controllers in playground environment with random obstacles

The files of the complete project are available on the