Group information

Name	Student number	E-mail
M.J.W. Verhagen	0810317	m dot j dot w dot verhagen at student dot tue dot nl
L.W. Feenstra	0847751	l dot w dot feenstra at student dot tue dot nl
A.J.J. Steinbusch	0903892	a dot j dot j dot steinbusch at student dot tue dot nl
J.T. Galen	0897620	j dot t dot v dot galen at student dot tue dot nl

Introduction

This project is part of the course Embedded Motion Control in which software is developed and applied to autonomously control a robot in real-life. The project consists of two challenges: the Escape Room challenge and the Hospital challenge. The Escape Room challenge will be an intermediate challenge that serves as preparation for the final challenge which is the Hospital challenge. In both challenges a robot has to fulfill its task autonomously. To begin with, an initial design is made in which the requirements and specifications of the two challenges are touched upon, functions that the software will be divided in, the components of the robot that will be used with their specifications, and the interfaces that will be used.

Initial Design

In this section, the initial design is discussed. A list of requirements, functions, components, and specifications is presented. Finally, the communication of the functions among each other is visualized using an interface, which clarifies the structure of the software that will be implemented.

File:InitialDesign.pdf

Requirements

There are two competitions in which PICO will be active. Therefore, the requirements are split up in general requirements, which are of importance for both competitions, requirements considering the Escape room challenge, and requirements that concern the Hospital challenge.

General requirements that hold for both challenges are:

PICO must move autonomously.
PICO is not allowed to bump into walls.
PICO gets two opportunities to perform the entire task.
PICO cannot stand still for a period longer than 30 seconds.
The total time limit for PICO to accomplish its tasks is 5 minutes.
The challenges must be finished as quickly as possible.
PICO should have a minimal distance from the wall of 25 cm from its center point.
The software must be easy to set-up.

Additional requirements concerning the Escape Room challenge are:

Finding the corridor.

Additional requirements concerning the Hospital challenge are:

PICO has to explore the rooms and build a map.
PICO has to be able to park backwards to the wall behind the starting position.
PICO must be able to recognize a change in the map and by means of that find an object in one of the rooms.
PICO must stop and stand still close to the object after finding it.

Functions

Here, the functions that will be implemented in the software of PICO are presented. The functions are split up in different categories, namely Task, Skill, Action and World model.

Task: Contains the main goal that the robot needs to fulfill. For the first challenge this is escaping the room. For the second challenge this is mapping the hospital, parking backwards and subsequently finding an object.

Escape a room
Mapping
Park backwards
Find object

Skill: For each task certain skills are needed. When PICO is escaping the room or mapping the hospital it will have to make decisions where to go next. It can choose different directions and/or rotations. While driving around in the room or hospital PICO has to recognize exits, walls, corners and the object that needs to be found while avoiding hitting any walls or the object. PICO also has to be able to know where it is while driving through the hospital and plan a certain path to find the object.

Decision making
- Go left
- Go right
- Go straight ahead
- Go backwards
- Rotate
Object avoidance
Path planning
Object recognition
- Detect exit
- Detect wall
- Detect corner
- Detect object
Localisation

Action: To every skill certain actions correspond such as measurements, movements or shutting down.

Measurement:
- Read encoder
- Read LRF
Move:
- Stop
- Translate:
  - Left, Right, Back, Straight ahead or x and y direction
- Rotate:
  - Direction and angle or +/- 90/180/360 degrees
Shut down

World model: Contains the map of the environment, the starting position of the robot, the current position of the robot and a function that can obtain this information from other models and can send this information to other models.

Initial position
Current position
Position of the walls, exits and corridors
Communicates with functions in other models

Components

The robot that will be used in this project is PICO which is a telepresence robot from Aldebaran. PICO moves on omni-wheels so that it can move in all directions and it can rotate along the z-axis. It contains a Laser Range Finder (LRF) to detect the walls and objects. Also, PICO is provided with wheel encoders to estimate the change in distance from its starting position.

PICO runs on an Intel i7 on Ubuntu 16.04. A software layer has been created on the Robot Operating System to simplify its use. The software of this project will be programmed in C++.

Specifications

A list of specifications is made regarding PICO and the environment in which PICO will be active. First, the specifications regarding PICO are shown, followed by the specifications that concern the Escape Room challenge and the hospital challenge.

Specifications of PICO:

The maximum translational speed is 0.5 m/s.
The maximum rotational speed is 1.2 rad/s.
PICO is approximately 40 cm wide.
Laser Range Finder has a range of -135 to 135 degrees with 1081 data points.

Specifications concerning the Escape Room challenge are:

PICO has escaped the room when its entire rear wheel is across the finish line.
The shape of the room is rectangular and the dimensions of the room are to be revealed at the start of the challenge.
The start position of PICO will be at a random position in the room.
The orientation of the corridor will be approximately perpendicular to the wall and the wall on the far end of the corridor will be open.
The walls might not be perfectly straight, nor perfectly perpendicular and parallel to other walls.
The width of the corridor will be in-between 0.5 and 1.5 meters.
The finish line will be more than 3 meters into the corridor to give PICO some more time to align itself with the walls of the corridor.

Specifications concerning the Hospital challenge are:

PICO's starting position will be in the hallway.

Interfaces

Escape room Challenge

In the Escape room challange PICO was placed on an arbitrary location in a rectangular room from which it had to escape through a corridor. This had to be done as fast as possible.

Wall follower

In order to complete this challenge, a so-called Wall follower algorithm was built and implemented. The steps that the algorithm makes in order to complete the challenge are as follows:

Drive to the right until a wall is detected at the right side,
Follow this wall by driving forward until a wall is detected,
If the distance to the right wall becomes larger than a predefined threshold, compensate by driving sideways to the wall,
When a corner is detected ahead, rotate 90 degrees to the left and align to the wall perpendicular to the previous wall,
If the corridor is detected by an increase in distance measured at the wall side (defined by a certain threshold), PICO will drive another 25 cm forward and then rotate 90 degrees to the right,
PICO will drive forward and then align to the right wall again, allowing it to exit the room.

For detection, PICO uses sensor-data of the sensors located perpendicular to its forward driving direction. These sensors are used to find the distance to walls at its left and right side, defined as LeftDistance and RightDistance respectively. The sensor that is aligned with the forward driving position is used to find the distance to to a wall that is in the forward driving direction, defined as ForwardDistance. The detection of a corner as described in step 5 is done by looking at ForwardDistance and RightDistance. When both of these distances become lower than a predefined threshold, a corner must be present at the right side, and therefore, PICO should rotate to the left. By using this algorithm, PICO is able to find the corridor of the room and escape the room.

Testing

The testing sessions with PICO were used for testing the algorithm and fine tuning some parameters in the model, such as velocities, rotational velocities and required minimal distances to walls. During the testing, it was found that there is a large difference between the simulation and reality. First, it was concluded that the odometry of PICO should not be trusted too much for localization and moving around. This could be noticed at the 90 degrees rotation that was used. The actual rotation PICO made had a noticeable offset. A possible solution to this problem was using the wall at the side to determine if PICO is aligned with the wall by using additional sensor data. This solution was implemented and tested in the simulation. In the simulation, the solution worked quite well for making the corners, but it also caused some unexplainable and unwanted behaviour. Due to a lack of time, no solution to this problem could be found and therefore, this was not implemented for the Escape room challenge. Another flaw that occurred during testing was that PICO did not always detect the wall at its right side in step 1, and because of this, PICO hit the wall. This did not happen in any of the simulations, and it remains unclear why this happened during testing. Despite these two problems, PICO was able to get out of the room once during testing.

VIDEO SIMULATION

The Challenge

During the challenge, the second problem that was discussed above obstructed PICO from finding the exit. PICO failed to detect the wall at its right side and drove into it, which is shown in the animation below. unfortunately, this happened at both trials. After evaluation of the Escape room challenge, it was concluded that a different approach was required for the Hospital Challenge. This means that from this point on, the Wall follower algorithm is abandoned and a new a plan for a new approach is made.

Conclusion

A relatively simple algorithm was made for the Escape room challenge, which finds and then follows the wall at its right side until it detects the exit. Based on simulations, this algorithm was sufficient for completing the Escape room challenge. However, during testing, multiple differences were observed between the simulation and reality. It is learned that navigating using purely PICOs odometry is overall not a smart strategy, and therefore this strategy will no longer be used from this point. Hitting the wall due to a detection error was the reason the Escape room challenge ended premature for PICO, the exact cause of this error has not been found. This is however not an issue for the final challenge, because the next step is going back to the drawing table and come up with a new plan.

Hospital Challenge

The second and final challenge is the Hospital Challenge. In this challenge PICO has to find an unknown object in a hospital area which consists of a hallway and multiple rooms. After the Escape Room Challenge a new plan is made to successfully complete the Hospital Challenge. This plan consists of two phases: Mapping the hospital and Finding the object. During these phases PICO needs functions such as World mapping and object detection. Further, it needs to be able to move around without hitting a wall and plan a path towards a given setpoint. How Mapping and Finding the object are performed and how these functions are implemented is explained here.

[EXPLAIN WHAT MAKES YOUR SOLUTION UNIQUE; WHAT ARE YOU MOST PROUD OF?]

The solution that is found is a relatively simple, yet very effective solution. Libraries that are available to the public are used in a very smart way in order to achieve the goal of this challenge. A perfect example of this is the mapping phase, for which choosing a set-point on the outside of the hospital is already enough to obtain a world map. Next to obtaining a world map, it is also easily ensured that PICO does not bump into walls, without having to implement complicated detection/force-field algorithms. [WE MIGHT WANT TO ADD SOMETHING ELSE? AND WHERE DO WE PLACE THIS?]

PLAATJE SOFTWARE ARCHITECTURE VAN DROPBOX

World mapping

In the first phase of the challenge, PICO has to make a map of the environment. To accomplish this, an external library called MRPT (Mobile Robot Programming Toolkit, https://www.mrpt.org/) is used. This library contains some useful classes, that help us to efficiently program the software required for PICO to accomplish its task. The first task is to process the laser range data of PICO. The class CObservation2DRangeScan is used, which takes as input a vector with range data, and a binary validity vector and the aperture of the range scan. The validity vector is used to consider which data points are valid to for example filter out the outer data points which contain a significant amount of noise. The Cobservation2DRangeScan object is inserted in a second class, the CSimplePointsMap. Each valid point is inserted into the map which contains the 2D coordinates of the points where the laser range finder detected a wall. The first set of data is used to create a points map for the world map. Now of course the world map needs to be updated with the new measurements. A second CSimplePointsMap object is created to store the current range data of PICO. Then, an iterative closest point algorithm (Levenberg-Marquardt) is used and implemented in the CICP class to fit the points of the world map and current view together. From the displacement and rotation of the current view with respect to the world map, the location and alignment of PICO is obtained with respect to its starting position. This way both PICO’s position and world map are continuously updated. The map can be output as a bitmap images and can be used to recognize the location of the rooms and doors.

Path planning

For the navigation of PICO, the path planning algorithm implemented in the PlannerSimple2D class is being used. This class only takes a gridmap in the form of a COccupancyGridMap2D object, therefore, the point map is first converted to a gridmap object. Each point from the world map is inserted in the COccupancyGridMap2d object, which based on input of the maximum x- and y-range determines which cells are occupied and which not. The gridmap is inserted in the PlannerSimple2D class which utilizes a wavefront propagation algorithm to find the shortest distance from the current location to the target set by the waypoint. The PlannerSimple2D object also contains the value for the radius of the robot, to make sure the found path does intersect with the walls. The path is output as a set of points with a given step size, which can then be used for navigation.

Phase 1: SLAM/mapping the hospital

[Jasper, how computes(pathplanner)/ which libraries etc./ think about copyright/gridmap/]

In phase 1, PICO will be building a map of the hospital by using the MRPT World Mapping and Path planner. Once this is done, PICO will return to its starting position and park there. Next to this, PICO has to use the map for defining rooms and doors. These features are further explained here.

Initialize map

[Aaron] At the start, PICO will be placed at an arbitrary angle with respect to the hospital walls. In the "Initialize map" phase, PICO uses the initial view to compensate this arbitrary starting angle. In order to do this, data form the walls that are initially spotted is used to determine the angle of PICO with respect to these walls. The World map orientation is compensated with this angle to get a nicely oriented World map. In order to clarify the result of this initialization a bit more, the situation before and after the initialization is shown in Figure X.

Figure X: Initialization of the worldmap

Building the map

The World Mapping algorithm and the Path planning algorithm are combined in order to map the hospital. The path planning algorithm is provided with a setpoint that is located outside the hospital. The setpoint is positioned sufficiently far away guaranteeing that it will be located outside the hospital. The Path planning algorithm will render a path to the setpoint and PICO will follow this path while scanning its environment and creating the map. While following the rendered path the hospital will be further explored and the path will be blocked by means of walls. At this point, the path planning algorithm will render a new path to the given setpoint taking the known map into account. This procedure, of following the path until the path is blocked and finding a new path, is repeated till the path planner cannot generate any path to the setpoint, e.g. the contour of the hospital is found. At this point, PICO will return to the starting position and will park backwards against the wall. In Video X a simulation of the mapping procedure is shown.

Parking

[Aaron] Once the map of the hospital is finished PICO will start the parking phase. The path planner will generate the shortest path to the starting position based the world map that is made. After arriving at the starting position PICO will rotate until it is aligned with the wall in front. The frontal distance to the wall is measured and PICO will rotate until it faces the wall backwards.

PICO will start driving backwards with a reduced speed for the measured distance till it touches the wall. With the use of the control effort, PICO will stop driving backwards once it touches the wall. PICO is now parked and ready to find the object! The parking of PICO is shown in the video below.

NEED FOR CODE EXPLANATION?

Define rooms

With the use of the worldmap rooms and doors are located. To locate the rooms first a set of image adjustments are performed on the worldmap, with the use of ‘opencv’ [opencv]. First the image is processed using erosion.[erosion] With the erosion process the walls in the image are thickend till the hallway and doors are all filled. This results in white spaces that represents a piece of the rooms, see Figure X. In the processed image the contours are found using the opencv function ‘findContours’ which is able to find contours in a binary image. [Suzuki][findcontours]. The function is set to construct a full hierarchy of nested contours, i.e. of the contours the children and parent contours are known. This is necessary since the largest contour is represents the entire hospital, the parent contour, and this is not of interest. For the contours of the rooms the moments are calculated using Green’s formula [moments]:

$[math]\displaystyle{ m_{ji}= \sum_{x,y}(array(x,y) \cdot x^j \cdot y^i) }[/math]$ .

With the moments the center of the room $[math]\displaystyle{ (\bar x,\bar y) }[/math]$ is calculated as:

$[math]\displaystyle{ \bar x = \frac{m_{10}}{m_{00}},\qquad \bar y=\frac{m_{01}}{m_{00}} }[/math]$

In Figure X an example is given for the contours and center points.

Locate Doors

After the world map is obtained, the inside of the hospital is marked by coloring the pixels. The walls and the surroundings of the hospital are marked with a different color. To be able to do this, the inside of the hospital needs to be surrounded by a closed contour, so no gaps are allowed in the walls of the hospital. This is achieved by using a combination of the opencv functions 'erode' and 'findContours'. The function 'erode' thickens lines and pixels in any direction. This is applied until all the gaps in the walls are closed, doors and hall way remain open. Subsequently, the function 'findContours' can now find the closed contour which surrounds the inside of the hospital. Now the inside of the hospital can be marked and it surroundings can be marked with a different colour, see Figure X.a.

Using the opencv function 'cornerHarris' all corners of the hospital are detected. Subsequently, two types of corners are distinguished, namely corners that cover 90 degrees or 270 degrees of the inside of the hospital. Only the corners that are 270 degrees are used for further analysis, see Figure X.b. The set of corners that are 270 degrees are sorted in vertical and/or horizontal pairs only if they are 1.5 m apart or less and either the x-coordinate (vertical pair) or the y-coordinate (horizontal pair) is (approximately) the same.

To find doors that consist of 4 corners, the vertical pairs of two are sorted in quartets only if the y-coordinates of the two pairs are aligned such that together they could form a door. Subsequently, it is checked if the whole space in between the 4 corners of each quartet is marked as the inside of the hospital. If this is the case it is also checked if at least on the right or top of the so called door the pixels are marked as wall or surrounding. The latter is done to ensure that open spaces in between two doors that in front of each other are not detected as a door as well. If both requirements are fulfilled a 4 corner door is found. In Figure X.c can be seen that the door detection works for the possible hospital map that was provided. A similar procedure was thought of for doors consisting of 2 corners. For vertical pairs it is checked if either left or right of the pair the pixels are marked as wall or surrounding. Suppose that on the right size of the vertical pair the pixels are marked as wall or surrounding, then the width of the door is determined by finding the nearest wall to the left of this vertical pair. For horizontal pairs it is checked if either above or below of the pair the pixels are marked as wall or surrounding and the width of the door is determined by finding the nearest wall to the top or bottom of this horizontal pair.


Figure X.a: Marking the inside and outside of the hospital in simulation	Figure X.b: Detecting 270 degrees corners in simulation	Figure X.c: Doors in simulation

When a door is located it is key that the pixels which are identified as the door opening are calculated. Combining this information with the center points of the rooms the hint can be used. The path planner is asked to plan a path to the center point of a room. When the path is know it can be checked through how many door openings the path goes to get to the room. When this is applied for all rooms it can be calculated for which room the most doors must be passed to get to it and the room in which the object is found.

Unfortunately, since the opencv function 'erode' might need some manual tuning for the amount of erosion it was considered unreliable. In Figure X.a the result of successful erosion can be seen after manually tuning. Furthermore, erosion has as a consequence that corners of rooms get misshaped and can no longer be detected by the opencv function 'cornerHarris', see Figure X.b. Therefore it was decided to not implement this part for the final challenge.


Figure X.a: Marking the inside and outside of the hospital of the final challenge	Figure X.b: Detecting 270 degrees corners of the final challenge

Phase 2: Finding the object

With phase 1 completed, PICO now has access to room and door locations. Next to this, it knows the full layout of the hospital, which means that it can navigate anywhere in the hospital using the Path planning algorithm. The final piece of information that is used in order to find the object is the hint that is provided:

The object will be located in the room for which most openings (i.e., doors) have to be passed through to enter it.

Based on all of this information, a strategy is made in order to find the object as fast as possible. This means that, based on the hint and the locations of the doors and rooms, PICO should immediately identify in which room the object is located, and identify the object once it is this room. How this is realized, is explained here.

Connection diagram

With the known locations of the rooms and doors a connection diagram can be drawn, see Figure X. In this diagram the connections between the rooms and the hallways are drawn. For example, if one is standing in the hallway and would want to visit room 4 it has to pass through room 3 first.


Figure X: Room connection diagram

To connect the doors to the right room, the path planning algorithm is used. For every room the shortest path from the origin is planned. An iteration over the setpoints of the path is made and it is checked if the setpoints lie in the contour of the four points of the doors, by means of the opencv’s function pointPolygonTest. If a setpoint lies in the contour of a door, it means the door must be passed through in order to reach the room from the origin. The number of doors that are passed through are counted and the room that needs the most doors to be passed through is passed through to the find-object phase. It is taken into account that a door can be passed through only once in every path, i.e. if more setpoint of a single path lie in the contour of the same door, the door is only counted once.

Object detection

For PICO to complete the find object phase, it has to stop near the object; Therefore, only using the high-level hint is not enough. While driving to the target room, PICO starts it object detection phase. In this phase PICO constantly compares the map of the hospital, made in the mapping phase, with the current updated map by subtracting the maps from each other. The result is a map where only differences are visible.

Since both the map made in the mapping phase and the current map can have noise in it, the noise must be filtered and not be confused for the object. For the difference map all the contours are computed (findContours) and only the largest contour is checked, i.e. the largest difference. For the largest difference in the map the bounding rectangle is calculated using opencv’s boundingRect, i.e. a rectangle that fits the contour. If this rectangle has dimensions larger than a predetermined minimal height and width, the rectangle is identified as the object and the coordinates of the object are saved. If the rectangle does not suffice for the predetermined dimensions, the routine of comparing both the current map with the hospital map is repeated.

When PICO has identified the object the path planning algorithm computes a path to the object and PICO is to follow the path until it is at a minimal set distance of the object. Once it is at the object location, PICO stops driving and will speak: “I have found the object”. A video of PICO successfully finding an object is given below.


Figure X: PICO finding and driving towards the object

The Challenge

During the hospital challenge PICO was able to successfully map the hospital and park backwards. PICO moved at a slow pace but because of the path planning algorithm, used to map the hospital, PICO was able to map the hospital quickly. The map of the hospital is given in the Figure X.a. Besides the rooms and hallway of the hospital some noise was measured outside the hospital. This is due to the readings of the LRF over the walls. With the complete map PICO was able to plan a path back to its origin and park backwards. After the hospital map was completed, the map was postprocessed as explained in Define rooms to find the rooms. The noise mapped outside the hospital was not off influence on the detection. The rooms that were detected are illustrated in FIGURE X.b.



Figure X.a: Worldmap created during the challange	Figure X.b: Midpoints and contours of hospital challenge

During the challenge the door detection was not yet implemented as described in Locate doors ;therefore, the hint was not implemented. In the object detection phase PICO was set to drive towards the midpoints of the detected rooms in random order to search for the object. However, the object detection was not yet robust enough. While PICO was searching for the object, it detected a change in the scenario outside the hospital. The change was identified as the object and the path planner tried to computed a path to the object. Because the object was outside the hospital, PICO could not find a path to the object and the challenge could not be finished.

FIGURE MAP OF OBJECT

VIDEO CHALLENGE

One improvement on PICO should be the drive control while following the path. PICO is driving very slowly, which could be improved by tuning the drive control algorithm further. Another improvement should be the manner of how PICO follows the path. Currently, PICO first rotates towards the next setpoint and then drives forward to it. Since the setpoints lie close to each other, it should be possible for PICO to drive towards the setpoint immediately, by means of simultaneous X and Y translation. Last, an improvement should be the robustness of the object detection. As seen in Figure X not all the walls are completed during the mapping phase, which has no influence on the door and room detection. While detection objects, most of the straight lines are filtered to identify difference between the object and not yet fully discovered walls. However, small changes in worldmap, like initially or further wrong readings, smaller part of walls and the detection outside of the hospital could cause PICO to identify the object falsely.

OTHER IMPROVEMENTS?

Conclusion

Part of code

code uitzoeken

to do:

- waar zijn we trots op evt. uitbreiden

-spelling check

-all caps/foto's videos toegevoegd

-copywrite check/ hoe references erin zetten/ benoemen van opencv functions, etc

-code snip

-reference in correct paragraph to the code snips

References

[x] Eroding and Dilating (2017). In OpenCV 2.4.13.4 documentation. Consulted on May 22, 2018. https://docs.opencv.org/2.4.13.4/doc/tutorials/imgproc/erosion_dilatation/erosion_dilatation.html

[x] Harris corner detector (2018). In OpenCV 2.4.13.6 documentation. Consulted on May 28, 2018. https://docs.opencv.org/2.4/doc/tutorials/features2d/trackingmotion/harris_detector/harris_detector.html

[x] Finding contours in your image (2017). In OpenCV 2.4.13.4 documentation. Consulted on May 31, 2018. https://docs.opencv.org/2.4.13.4/doc/tutorials/imgproc/shapedescriptors/find_contours/find_contours.html

[x] Point Polygon Test (2018). In OpenCV 2.4.13.6 documentation. Consulted on ...., 2018. https://docs.opencv.org/2.4/doc/tutorials/imgproc/shapedescriptors/point_polygon_test/point_polygon_test.html

Embedded Motion Control 2018 Group 5

Contents