Embedded Motion Control 2017 Group 2

Group Members

Name:	Report name:	Student id:
Matthijs van der Burgh	M.F.B. van der Burgh	0771381
Joep Linssen	J.M.H.G.H. Linssen	0815502
Sil Schouten	S. Schouten	0821521
Daniël Pijnenborg	D.H.G. Pijnenborg	0821583
Rens Slenders	R. Slenders	1028611
Joeri Roelofs	J. Roelofs	1029491
Yanick Douven	Y.G.M. Douven	Tutor

Minutes

Minutes of team meetings are available here: [1]

Initial design

The initial design can also be found in File:Initial-design-document.pdf

Introduction: The goal of this project is to autonomously solve a maze with a robot (PICO) as fast as possible. The robot has sensors, actuators and a computer on board to navigate through the maze. The computer runs on Linux 14.04 and the program language used for the software is C++. In this section the initial design is discussed, which consist of the requirements, functions, components, specifications and interfaces. This design will be used to build structured software and define required interfaces.

Requirements: To solve the maze problem the following are required

• Finding the exit
• Finding and opening the door
• Avoiding obstacles at all times
• The maze should be solved within 5 min
• All tasks must be executed autonomously
• The system should work on any maze
• The robot mustn't get trapped in loops

Functions: The functions which have to be implemented are subdivided into the different contexts. Motion functions describe how the software gets input from and supplies output to PICO. Skill functions describe how different skills are executed in the software. Lastly Task functions implement task scheduling.

The motion functions have been determined as:

• Basic actuation

  Give PICO the ability to move around and rotate

• Getting information

  Receive information from LRF and odometry sensors.

• Opening door

  Make a sound to open door.

The more advanced skill functions are:

• Mapping

  Use information from ”Get information” to determine and save a map of the environment. This creates the world model for PICO.

• Localisation

  Determine where PICO is located in the map created in ”Mapping”. This will most likely be implemented in the same function.

• Object avoidance

  Use current LRF data to make sure PICO does never hit a wall.

• Maze solving

  Use the map in combination with current and previous locations within the map to determine a route out of the maze.

• Door and exit detection

  Using the map to detect if possible doors have been found and if PICO has finished the maze.

The task control implements switching between the following tasks:

• Map making

  Done throughout the challenge to improve current map.

• Finding doors

  Done in first stage of the challenge when no door has been found yet

• Finding the exit

  Once a door has been found and opened the task is to go through the door and finish the challenge.

Software architechture pre-corridor

The initial design can be put into a diagram, which is shown in Fig: figfigfigfigfigfigfigfigfigfigfigfigfigfigfigfigfigfig. Below the diagram a short recap of the skill, world model and task contexts are given, in order of execution.

Software architecture before corridor challenge

SLAM

The slam algorithm uses the raw odometry and laser data and processes this into a robot pose and world features. The robot pose is defined as the current position and rotation of Pico with respect to the initial pose of Pic. Features are defined as re-observable landmarks, for example corners of walls, and are saved as locations with respect to the initial pose of Pico. A possible method of SLAM is EKF SLAM, but many different types of SLAM exist.

After evaluating multiple SLAM algorithms, it has been decided that EKF SLAM fits bests with the problem. The reason for this choice is that it works well in circumstances where there are clear landmarks, in our case corners, these landmarks can be used in primitive detection as well. It is also effective for problems of moderate size, the data size will not become huge and slow to process for a few hundred landmarks.

World model

The world model is a data structure that holds the detected features, primitives and pose of the robot. It takes feature and pose data from the SLAM algorithm this information is used by primitive detection to find primitives within the world model. Primitives are locations where a major change of movement direction is possible, a point where a decision has to be made.

The data in the world model is used by the state machine and maze solver to solve the maze challenge. Note that the world model does not do any data processing itself, it is merely a container of data.

State machine

Solving the maze is done in three states:

1. Find door

   In this state the maze solver will have to detect if a possible door is near Pico, if so the state is changed to Open door.

2. Open door

   When a possible door has been found the bell is to be rang, after which it has to be checked if the door opened. There are two possible outcomes, either there was a door, which opened, or there was no door. In the first case state changes to Find exit, else the state reverts back to Find door

3. Find exit

   In the find exit state Pico does not search for doors anymore and has to detect if the exit is crossed in which case Pico has to stop.

Maze solver

The maze solver implements a maze solving algorithm that is able to cope with loops in a maze, for example Trémaux. It takes primitive and pose data from the world model. If Pico is within range of a primitive, a decision is made in which direction to move next and sends this data to the driving skill.

Driving

In the driving skill a driving strategy is to be implemented which is able to avoid obstacles while driving in a set direction, taken from the maze solver. Avoiding obstacles means that laser data is required to measure distances to objects in the world.

For the driving skill the best method was chosen to be a potential field method would be the best choice. Correct implementation makes sure Pico will never hit a wall while driving towards a desired location. A downside of this method is that Pico might get stuck if there a wrong decision is made.

Software architecture post-corridor

After the corridor challenge it was decided to take a slightly different approach on the SLAM algorithm and split it into SLAM for localisation and feature detection. This is change has been made since SLAM is already implemented and tested within ROS and known to be robust, and therefore focus can be placed on other crucial tasks as primitive detection.

Maze challenge

Toolset functions

A distinction is made between toolset functions, which implement supporting functionality. These functions are not necessary in ideal situation where gathered data is perfect, for example in the ideal situation Pico is placed aligned with the maze correctly every time and laser data is noise free. Also filtered laser data is preferable for all other functions, therefore it filtering data is done before calling functions such that there is only one filtering function to be implemented.

Initialisation

The maze solver relies on the directions from the primitive detection being in correct in the world frame, or differently stated directions a decision is made NORTH, EAST, SOUTH, WEST with respect to the initial rotation of the robot. This adds the requirement of the robot being initialised perpendicular to a wall. Since all corners are approximately right angled this results in directions at every decision point corresponding to NORTH, EAST, SOUTH, WEST.

Solving this problem is fairly straight forward using feature detection. From the first two features, which are always in the same section, the angle between the two points is calculated. This angle is then used to turn Pico in the same direction resulting in an alignment to the first wall one the right which Pico is able to see, this is repeated until the angle is within a threshold of approximately 3 degrees. This process is shown in simulation in Fig. figfigfigfigfigfigfigfigfigfigfigfigfigfigfigfigfigfigfigfig

After aligning with a wall the first decision is made according to a laser measurement which measures for open directions in the starting position. Finally the starting pose of the mapping is set to the aligned pose, which will be used to retrieve pose data at different locations.

Main skill set

The main skill set implements, as the name suggests, skill functions defined in the architecture.

SLAM

Gmapping is a SLAM algorithm present in ROS and is therefore used.

Feature detection

For determining the locations of decision points corner and segment features are required. Corner features are defined as locations where two, connected walls meet i.e. a corner, segment features are the features which are not corners which describe beginning and end points of wall segments. In Fig. figfigfigfigfigfigfigfigfigfigfigfigfigfigfig these features are shown, corner features in red and segment features in blue. Since the laser data contains values showing out-of-range data these values are set equal to 100 indicating an open section. In these out-of-range sections no features are set.

The strategy to find segment features is based on the difference between two consecutive laser points, if the difference is greater than a threshold value (set at ................) a segment is detected and both points are set as segment features. For the corner features a split and merge algorithm is applied to every segment (from an even segment feature to the next segment feature, i.e. from 0 to 1 and 2 to 3).

The split and merge algorithm works in the following sequence:

1. starting node = segment beginning, ending node = segment end
2. dx and dy are calculated between starting and ending node
3. distance between measured data and straight line is calculated as:

   dist = abs( dy*x - dx*y + x(start)*y(start) - y(end)*x(start) )/ sqrt( dy^2 + dx^2 )

4. check if all points are within threshold

   if(maximum distance > threshold)
   ending node = index of maximum value
   index of maximum value is a corner node
   goto 2

5. done

The split and merge algorithm is also shown in Fig. figfigfigfigfigfigfigfigfigfigfigfigfigfigfig, with some resulting examples in Fig. figfigfigfigfigfigfigfigfigfigfigfigfigfigfig. This split and merge algorithm is tested in Matlab first of which executable code is provided in File:Split-and-merge-matlab.zip.

Some examples of feature detection using split and merge strategy

Dependencies: Feature detection uses laser data as its input, therefore the quality of the output depends on the accuracy of the data. If accurate data is supplied the parameters defining the strictness of the detection (i.e. the maximum allowed distance between calculated and measured lines) can be increased. This however comes with a major drawback that shadow points result in walls being split into multiple sections, as the laser data makes a jump for shadow points. To prevent this a basic shadow-point filter is implemented.

Another possible cause of error is how out of range data is handled, if this data is discarded there is are circumstances where two sections are joint together. This happens when the range before an out of range section is equal to the range after an out of range section. A result is that two sections are joint and therefore primitive detection will give a wrong output. An example of this behaviour is shown in Fig. figfigfigfigfigfigfigfigfigfigfigfigfigfigfig where the difference is shown in the corridor challenge case.

Trade offs: As mentioned in the Dependencies, the shadow point filter plays a large role in the quality of the features. In this a trade-off has to be made between filtering bad data and removing relevant information. A strict shadow point filter takes out shadow points but also leads to far-away, correct laser data being removed as well (as one laser increment leads to large range difference.

Primitive detection

Straight

1. Find straight section.

2. Check if enough distance between robot and section to go straight.

Right

left

Files

Initial design document: File:Initial-design-document.pdf

Initial design presentation: File:Initial-design-presentation.pdf