RoboSim

By Dimitris Xanthopoulos

1. Abstract

This report is about a simulator program which was written in C++ programming language in order to experiment with navigational methods, involving algorithms and Artificial Neural Networks (ANNs), for mobile robots. The first part of the report reviews some basic concepts about mobile robots and the theory behind Artificial Neural Networks. Additionally, in the first part, a C++ library which was written for implementing ANNs is explained. In the second part of the report the techniques used for programming the simulator and the methods employed for controlling the robot are discussed. The report concludes with an overview of the simulator's performance and suggestions for future work.

2. Introduction

Programming an autonomous mobile robot so that it reliably navigates in an unknown or a dynamic environment is a difficult thing to do. This is due to missing information during programming, the dynamic nature of the environment and the inherent noise in the robot's sensors and actuators. One common approach for the control of autonomous robots is that of reactive systems where a set of rules (an algorithm) is used to govern the robot's behaviour. An alternative and much more promising solution is the use of ANN controllers which allow the robot to navigate in unknown environments more 'intelligently'.

Programming a mobile robot is also a time consuming process. It involves writing, compiling and downloading the program into the robot's controller. This means that testing (debugging) the program can be only performed after it is already 'installed' in the controller. Robot simulators can help solve this problem. Their purpose is to create a virtual model of the actual robot on which various programs can be tested easily. This allows programmers to preview what their programs actual do before downloading them into the controller thus saving time and facilitating the whole process.

The purpose of RoboSim was to simulate a small two-wheeled mobile robot as that is shown in the Figure 1.
.

Figure 1: Robot Model

3. Background

3.1 Robot's Kinematics

The term 'robot' throughout the report refers to a small, two wheeled mobile platform. Two motors on each side of this platform are used to manouevre it in a dual-differential drive ("tank-style") configuration as shown in Figure 2 (a small non-driven tail caster is used to balance the robot). In order to simulate such a model it was first necessary to derive the equations which described its motion.

Figure 2: Differential Drive

Before examining the differential drive equations, a coordinate system for the robot's world had to be defined. Although a 3-D simulator was to be implemented, it was assumed that the robot "lived" in a two-dimensional world. This meant that at any given time its position could be defined in terms of two-dimensional Cartesian coordinates and the direction it was facing could be defined as an angle measured from one of the axis (φ).

In order to keep with mathematical conventions, it was assumed that the angle that defined the orientation of the robot (φ) was measured counter clockwise from the x-axis as shown in Figure 3. It was also assumed that the position coordinates, x and y, were in metres, and the orientation angle, φ, was in radians.

Figure 3: Robot's Coordinate System

The theory behind differential drive kinematics is straightforward. The robot in a state of motion must always rotate about a point that lies somewhere on the common axis of its two wheels. This point is often called the Instantaneous Centre of Curvature (ICC). In order to derive the differential drive equations, the model shown in Figure 4 was used.

Figure 4: Differential Drive Model

Each wheel follows a path that moves around ICC at the same angular rate ω, thus:

(1)
(2)

where L is the distance between the centres of the two wheels and are the right and left wheel speeds respectively. R is the distance from the ICC to the midpoint between the wheels. Combining the above two equations:

(3)

(4)

Two special cases had to be considered:

• : the radius R is infinite and the robot moves in a straight line. (division by zero occurs. This had to be avoided during programming)
• : the radius R is zero and the robot rotates around its centre.

In all other cases the robot moves in a trajectory around ICC in some angular rate ω.

The next step was to find out how robot's coordinates (x, y) and its orientation angle (φ) changed with respect to time. If m(t) and θ(t) are the functions of time representing speed and orientation of the robot, then the solution will be in the form:

(5)
(6)

The change of orientation with respect to time is the same as the angular rate ù:

(7)

Integrating equation (7) yields a function for the robot's orientation with respect to time. The robot's initial orientation φ(0) is replaced by :

(8)

Since the velocity in equations (5) and (6) above simply equals the average speed for the two wheels, that is , integrating this in equations (5) and (6) yields:

(9)
(10)

Integrating equations (9) and (10), and taking the initial positions to be and yields:

(11)
(12)

Note that for the sake of simplicity, physical issues such as friction or inertia were not considered in the above calculations.

3.2 Robot's Sensors

Mobile robots use a variety of sensors in order to interact with their environment. The sensors that are most commonly used in mobile robotics are Infa-Red (IR) and Ultrasonic sensors. Both of these types of sensors have an emitter and a receiver side. The emitter continuously emits short bursts of IR light or ultrasonic sound which is reflected back to the receiver indicating that an obstacle was encountered.

In their simplest form the output of these sensors is binary: it simply indicates the presence (or absence) of an obstacle in front of the robot. In most cases though the output is analogue i.e. it reports the distance of an obstacle in front of the robot. Important characteristics of both types of sensors are the maximum detectable distance, the geometry of the detection beam and the reading resolution.

Figure 5: Robot's sensors

In the simulation program, it was assumed that the robot had five IR ranging sensors mounted in the front part of its chassis as shown in Figure 5. It was also assumed that the maximum detectable range of those sensors was 30cm and their resolution was very high. As it can be seen from the Figure, the sensors were placed in such a way that their beams did not overlap. This was done in order to avoid interference between the sensors.

3.3 The Robot Class

Having derived the differential drive equations and decided upon the form and number of sensors, a small C++ class was written to mathematically represent the robot. The table below lists the data members of this class

The centrepiece of the Robot class is the function Robot::UpdatePosition which is shown below

(B_DIAM is a constant representing the diameter of the robot's base) Note that time is not used in calculations (as in equations 8, 11 and 12). Instead the robot's previous position (i.e. the position before calling the function) is used to derive its new position. Small increments (or decrements) of the robot's orientation angle (dphi) are accumulated to determine the current orientation angle (phi). Phi is then used with the kinematics equations to derive the new robot's coordinates (x, y). ( The commented section 'Controller Part' is to be explained later)

3.4 Introduction to Artificial Neural Networks

An Artificial Neural Network is an information processing paradigm that is inspired by the way biological nervous systems, such as the brain, process information. It is composed of a large number of highly interconnected processing elements (neurons) working together to solve specific problems. ANNs, like humans, learn by example. An ANN is configured for a specific application, such as pattern recognition or data classification, through a learning process. Learning in biological systems involves adjustments to the synaptic connections that exist between the neurons. This is true of ANNs as well.

Neural networks take a different approach to problem solving than that of conventional computers. Conventional computers use an algorithmic approach i.e. the computer follows a set of instructions in order to solve a problem. Unless the specific steps that the computer needs to follow are known the computer cannot solve the problem. That restricts the problem solving capability of conventional computers to problems that are already well understood and it is known how to be solved.

Neural networks learn by example. The examples must be selected carefully otherwise useful time is wasted or even worse the network might be functioning incorrectly. The disadvantage is that because the network finds out how to solve the problem by itself, its operation can be unpredictable

On the other hand, conventional computers use a cognitive approach to problem solving; the way the problem is to be solved must be known and stated in small unambiguous instructions. These instructions are then converted to a high level language program and then into machine code that the computer can understand. These machines are totally predictable; if anything goes wrong is due to a software or hardware fault.

Neural networks and conventional algorithmic computers are not in competition but complement each other. There are tasks are more suited to an algorithmic approach like arithmetic operations and tasks that are more suited to neural networks. Even more, a large number of tasks, require systems that use a combination of the two approaches (normally a conventional computer is used to supervise the neural network) in order to perform at maximum efficiency

In the case of mobile robotics ANNs are indeed very useful. They can be used in a wide range of applications. Using the appropriate sensors they can be employed in complex tasks such as object recognition, voice recognition, intelligent path-planning etc. In RoboSim though their task was simpler; they were used as an alternative form of controller for implementing obstacle avoidance.

3.5 Artificial Neuron

The artificial neuron (also called "node") simulates the operation of a biological neuron. It receives a number of inputs, multiplies each of them with a corresponding weight to produce a set of weighted inputs, adds together these weighted inputs to produce a sum which is finally passed through an 'activation function' (see chapter 3.6) which produces the output of the neuron. The operation of a typical artificial neuron is illustrated in Figure 6

Figure 6: Artificial Neuron Diagram

The output of a neuron with K inputs and an activation function f(x), can be obtained using the formula:

where

Artificial neurons may be implemented in an algorithm (e.g. using a programming language) or they can be actual hardware devices.

3.6 Activation Function

The activation function receives as input the weighted sum of all inputs of the neuron, operates on it and produces a value which represents the output of the neuron. The most commonly used activation function is the sigmoid (or logistic) function:

Figure 7 illustrates this function

Figure 7: The Sigmoid Function

As it can be seen from the graph, the output of this function approaches to 0 as the input approaches to minus infinity and the output approaches to +1 as the input approaches to plus infinity. This means that no matter how large (negative or positive) the input is, the output will never be 0 or 1 (respectively).

Other activation functions which may be used in artificial neurons include the step function:

if sum ? 0
if sum > 0

and the hyperbolic tangent function:

3.7 The Neuron Class

The Neuron class simulates the operation of an artificial neuron as explained in sections 3.5 and 3.6 (see appendix for the full source code). Note that the activaton function used is the sigmoid function.

Examples:

• Create a neuron (named neuron1) having 5 inputs
  Neuron neuron1;
  neuron1.Initialise(5);

• Assign neuron's inputs
  double inputs[] = { 0.52, 0.23, 0.89, 0.12, 0.67 };
  neuron1.SetInputs(inputs);

• Calculate neuron's output
  neuron1.CalculateOutput();
  /* neuron1.output now contains the neuron's output */

3.8 Artificial Neural Layer

ANNs are typically organised in layers. An artificial neural layer can be simply thaught as a container of artificial neurons. There is no limitation in the number of neurons that can be contained in a layer. In Figure 8, a layer with K inputs and L neurons is illustrated:

Figure 8: Artificial Neural Layer Diagram

In fully interconnected ANNs, every input of a layer is connected to one input of every neuron which is contained within the layer. Hence if the layer has K inputs and L outputs then the number of inputs for all L neurons contained in the layer has to be K as well. Layers can be categorised into three groups: input, hidden and output layers (see chapter 3.10).

3.9 The Layer Class

The Layer class simulates the operation of an artificial layer as explained in section 3.8

Examples:

• Create a layer (named layer1) having 3 inputs and 5 outputs
  Layer layer1;
  layer1.Initialise(3, 5);

• Assign layer's inputs
  double inputs[] = { 0.41, 0.89, 0.33 };
  layer1.SetInputs(inputs);

• Calculate layer's outputs
  layer1.CalculateOutputs();
  /* The array layer1.outputs now contains layer's outputs */

3.10 Artificial Neural Network

An artificial neural network is simply a container of interconnected neural layers. Although many different types of networks exist, the one that is most commonly used is the "fully interconnected feed-forward" network. In such a network, all the outputs of any given layer are accepted as inputs to the next layer as shown in Figure 9. The only exception to that rule is the output layer: the outputs of the output layer are considered as the outputs of the network. Also the outputs of the input layer are considered as the inputs to the network (i.e. the input layer simply forwards its inputs to its outputs)

Figure 9: Artificial Neural Network Diagram

The layers contained in an ANN are categorised into three groups: the 'input layer' which is the first layer of the network, the 'output layer' which is the last layer of the network and the hidden layer(s) which is/are the intermediate layer(s) of the network. Any number of layers can be contained in a network. For any given layer, the number of its inputs is equal to the number of outputs of the previous layer (except the input layer).

3.11 Back-Propagation

Backpropagation is a method for training artificial neural networks. It starts with the errors at the output of the network and propagates these errors backwards through the network to yield output error values for all neurons in the network. Training occurs as neuron weights are adjusted in an attempt to reduce the output error for a particular input.

For the output layer of the network the errors can be easily defined and calculated. Assuming an output layer which contains L neurons, the errors are calculated as follows:

(for i = 1, 2, 3.L)

where outpust[ ] is the array containing the actual outputs of the layer's neurons, and  desired[ ] is the array containing the desired outputs of the network.

Assuming any other layer of the network (not the output layer) which contains L neurons and is followed by a layer with M neurons, the errors are calculated as follows:

(for i = 1, 2, 3.L)

Figure 10: Back-propagation

where outputs[ ] is the array containing the actual outputs of the layer's neurons and NextWeight[ ][ ], NextErrors[ ] are the arrays containing the weights and errors of the following layer.

Once the errors of all neurons in the network are calculated, back-propagation proceeds by calculating the suitable adjustments that need to be made in the weights of each neuron in order to reduce the errors. For a neuron with an input array inputs[ ] and an error error, the weight adjustment Äw is given by:

where learningRate is a value which indicates how fast the network 'learns' (i.e. how fast the weights are changed in order to minimise the errors).

When training a network, there has to be a measure of the overall error of the outputs. This measure is the Root Mean Square (RMS) error of the outputs. This is the square root of the mean value of the squared errors of the network or mathematically represented as:

where errors[ ] is an array representing the errors of the M outputs of the network.

Note that if a network is to be trained using a set of N training pairs then there will be N overallErrors. Hence in order to monitor the progress of the training process, the N overallErrors are added to master sum call trainingError :

The back-propagation algorithm can be summarised with the pseudo-code shown below:

3.12 The Network Class

The Network class simulates the operation of an artificial neural network able to be trained using the back-propagation algorithm as described in sections 3.10, 3.11.

Examples:

• Create a network (named Net) having an input layer of 5 neurons, a hidden layer of 25 neurons, an output layer of 2 neurons and a learning rate of 0..5
  Network Net;
  int layerSizes[] = { 5, 25, 2 };
  Net.Initialise(3, layerSizes, 0.5);

• Try to load from a file called "data.txt" 3125 training patterns. Each pattern has 5 inputs and 2 outputs. If loading failed display an error message
  if(Net.trainingSet.Load("data.txt", 3125, 5, 2)==false)
  {
  cout << "Failed to open data.txt" << endl;
  getch();
  }

• Keep training the netowork until a key is pressed.
  while(!kbhit())
  {
  /* Reset the training error to zero */
  Net.trainingError = 0.0;
  
  /* Loops through all the training patterns */
  for(int i=0; i<Net.trainingSet.nofPatterns; i++)
  
  {
  Net.SetInputs(Net.trainingSet.inputs[i]);
  Net.SetDesired(Net.trainingSet.outputs[i]);
  Net.CalculateOutputs();
  Net.CalculateErrors();
  Net.trainingError += Net.CalculateOverallError();
  Net.AdjustWeights();
  }
  
  /* Display the repetition number and the training error */
  counter++;
  gotoxy(1,1); cout << counter << " " << Net.trainingError;
  }

• Save network weights to a file. Display an error message if saving failed
  if(!Net.SaveWeights("Weights.txt"))
  cout << endl << "Failed to save the weights";

4. Implementation

The goal of the simulation was to create a 'virtual' robot that would be able to randomly navigate in an area avoiding any obstacles in its path as shown in Figure 11. In analogous commercial packages, the simulator software allows the user to modify the navigational method (or even the robot's characteristic and its environment) with the aid of a scripting language. In the case of RoboSim, the controlling method was fixed and could only be changed by the programmer (after rewriting and recompiling the souce code).

Figure 11: Screenshot from RoboSim

Writing the simulation program involved two major parts

1. Creating the simulation 'engine'
2. Writing controller programs for the simulation engine

4.1 Graphical Represenation

In order to graphically represent the robot and its environment, the Open Graphics Library (OpenGL) was used. OpenGL is a software interface to graphics hardware. This interface consists of about 250 distinct commands that are used to specify the objects and operations needed to produce interactive 3-D applications. OpenGL does not provide high-level commands for describing models of 3-D objects (such as spheres, cylinders, etc.)  Instead, complex objects (such as the robot and its surroundings) are created using a small set of geometric primitives (points, lines and polygons).

Figure 12: Geometric primitives forming the robot model

Creating a realistic robot model (such as the one shown in Figure 12) by manually hard-coding three-dimmensional coordinates in the program was clearly unrealistic. In order to produce the desired set of coordinates, VariCAD, a 3-D modelling program, was used. With the aid of VariCAD, the various parts of the robot (chassis, wheels, sensors etc.) were designed and saved in files as a sequence of three-dimmensional coordinates. Some simpler shapes (like the conical beams of the sensors and the obstacles) where created using geometric primitives (triangles) in the source code. Once the 3-D models were created and saved in files, they could be loaded into the simulation engine. This was achieved using the 'Milkshape' library. The purpose of this library was to 'decode' the 3-D files and produce the corresponding 3-D objects (represented by variables). Further changes to those objects (such as rotation and translation) could be then easily performed using OpenGL's set of transformation functions.

4.2 Collision detection

Another important task of the simulation engine was to perform collision detection. In order to keep things simple, only collisions between the sensor beams, the walls and the obstacles were considered. The purpose of the collision detection process was not only to determine whether a collision occurred but also to provide the distance of the collision point from the corresponding sensor (and hence provide the reading to that sensor). In order to facilitate this process, ColDet C++ library was used. With the aid of this library, the set of coordinates previously used to represent the robot's parts were also used to create the corresponding 'collision objects'. At any time, a collision check could be performed against two collision objects. If the result was positive (i.e. a collision was detected) another ColDet function could be used to return the distance of the collision point. Note that the collision objects representing the sensor beams had to be transformed (translated and rotated) by the engine in an analogous way as their graphical counter-parts to reflect the constant movement of the robot.

4.3 The Simulation Engine

The basic operations carried out by the simulation engine are shown in Figure 13.

Figure 13: Overview of the Simulation Engine

The first task of the engine is to perform initialisation. During this process, the initial robot's coordinates, orientation angle and wheel speeds are set along with other typical OpenGL settings (see comments in the source code for the full description of the initialisation process). Furthermore, the 3-D models representing the robot's parts are loaded from files and stored in memory. Finally the collision objects (beams, walls, obstacles) are created.

The next operation that the engine performs is to update the robot's position. This is done by calling the function Robot::UpdatePosition (See Appendix for the source code). The work done by this function can be split into two parts. Firstly the function calculates the new wheel speeds based on the readings of the robot's sensors and some form of control logic (either an algorithm of an ANN). Secondly, using the new wheels speeds and the differential drive equations (chapter 3.1), it calculates the new coordinates and orientation angle of the robot.

Once the necessary parameters have been calculated, the engine uses them to produce the output to the screen. The static parts of the environment (walls, obstacles) are first drawn. Then the engine uses two OpenGL functions to translate (move) and rotate the robot according to its new coordinates and orientation angle respectively. The same transformation functions are then sequentially applied to all 5 sensor beams. After each transformation is completed, a collision check is performed in order to yield the corresponding sensor's output.

4.4 Programming the Controller

As it can be seen from Figure 13, the behaviour of the robot depends on how the controller produces outputs (wheel speeds) based on its inputs (sensor readings). This means that several different behaviours can be implemented by simply changing a specific part of the code (the controller part) while keeping the rest of the code (the engine part) unchanged.

4.4.1 Algorithmic Control

The first and easier approach to control the robot was using an algorithm. In that case, a set of rules was used to produce outputs based on inputs. The flow chart of the algorithm is shown in Figure 14

Figure 14: Obstacle Avoidance Flow-Chart

Source Code Explanation:

• "Obstacle(s) detected?"

• "Obstacle in the front side?"

• "Obstacle(s) closer on the right side?"

• "Obstacle(s) closer on the left side?"

In order to check whether obstacles are closer on the right or left side, the sum of the left and centre-left IR (lIR+clIR) is compared with the sum of the right and centre-right IR (rIR+crIR)sensors. If obstacles are closer on the right side, the robot has to turn left to avoid them. This is done by gradually increasing the right wheel speed while gradually decreasing the left wheel speed. Like in the previous cases, the wheel speeds are increased / decreased inversely proportionally to the readings of the corresponding sensors. Note that the centre (left or right) sensors are given a little bit more 'weight' (smaller denominator) to make the robot react more aggressively when obstacles are detected in the centre.

4.2.2 Artificial Neural Networks Control

The first step towards implementing a 'neural controller' was to create and train an artificial neural network. The network used consisted of 3 layers; an input layer of 5 neurons (to accommodate the 5 readings from the sensors), a hidden layer of 20 neurons (initial random guess) and an output layer consisting of 2 neurons (which represented the motors' outputs). A 'global' learning rate of 0.5 was initially chosen.

In order to create a suitable training set, a separate program was written. The inputs and outputs of the training set were scaled in the range 0.1. This was done to avoid dealing with very large numbers and the problems caused by that (overflows). Five nested loops were used in order to create 3125 training pairs in the way shown in the flow-chart below:

Figure 15: Method used for creating the training set

a, b, c, d, e represent the left, centre-left, centre, centre-right and right sensor readings and x, y represent the left and right motor outputs respectively. Each time a loop is executed, x and y are assigned with the weighted average of the left and right sensor readings. (the coefficients 0.6 and 0.4 are used to give more 'weight' on the centre sensors). Then three mutually exclusive 'if' statements are used to produce the outputs. Finally the pair of inputs/outputs is saved (appended) to a file
Once the training set was created, the network could be trained. Again, in order to avoid complexity in the source code, a different program was used to train the network. This program read the file containing the training set previously created, trained the network and produced an output file containing the weights of the trained network. Several settings involving different number of layers, different number of hidden neurons and different learning rates were used in the training process. After a large number of trials, it was decided that the best setting was a 3 layer network with a hidden layer of 25 neurons and a learning rate of 0.5. The training process was stopped when an overall training error of 80 was obtained. This figure meant that the average training error for every input pattern was 80/3125 = 0.0256 which was satisfactory.

The following piece of code was added to the Robot class constructor. It has the effect of creating the network and loading the 'trained' weights from the file "Weights.txt" upon program initialiasation

The next step was to modify the 'controller' part of the simulation engine so that it would use an ANN instead of an algorithm. The source code explained in Section 4.4.1 was replaced by the code shown below:

The array inputs[] contains the scaled sensor readings which are fed as inputs to the network. The outputs of the network are then calculated and assigned as robot's left and right wheel speeds after multiplied by 5. The only thing that had to be taken care of using a rule (using an 'if' statement) was the case where the wheel speeds were equal.

The results of the simulation using the ANN were satisfactory. It has to be noted though that when no obstacles were detected by the sensors, the robot slightly bore to left when it was supposed to move in a straight line. This problem could possibly be solved by further reducing the error of the training process.

5. Conclusion

Although the simulator worked quite satisfactorily, it lacked several characteristics that would enable it to be a useful tool for robot programmers.

• Programmability: Most importantly, the simulator lacked programmability. The program that defined the robot's behaviour was hard-coded in the simulator and could not be changed by the user. The addition of a simple 'scripting language' would enable the user to implement different behaviours without needing to recompile the whole program.

• Customisability: Robot's important characteristics such as chassis geometry, wheel diameters, number and position of sensors etc. were fixed. This meant that only a certain type of robot could be simulated. Enhancing the program with customisability would greatly improve the simulator.

6. References

Lundgren, M. (2003) "Path Tracking and Obstacle Avoidance for a Miniature Robot",
URL: http://www.cs.umu.se/education/examina/Rapporter/465.pdf

Lucas, G.W. (2001) "A Tutorial and Elementary Trajectory Model for the Differential Steering System of Robot Wheel Actuators",
URL: http://rossum.sourceforge.net/papers/DiffSteer

Stergiou, C., Siganos D. (1996) "Neural Networks"
URL: http://www.doc.ic.ac.uk/~nd/surprise_96/journal/vol4/cs11/report.html

7. Bibliography

Braunl, T. (2003) "Embedded Robotics"

McComb G. (2000) "Robot Builder's Bonanza"

Nehmzow U. (2000) "Mobile Robotics: A Practical Introduction"

Ruocco S. "Robot Sensors and Transducers"

Walters D. (2000) "Implementing Dead Reckoning by Odometry on a Robot with R/C
Servo Differential Drive", URL:http://www.seattlerobotics.org/encoder/200010/dead_reckoning_article.html