Embedded Motion Control 2017 Group 4

Reports

File:Report1.pdf

File:Final design presentation.pdf

Group 4

M.R.M. Beurskens	0843080
N.H.J. Janssen	0842075
M.C.W. Schouten	0869793
J.A.E. Verest	0844791
M.S. de Wildt	0776941
R.H.M. Winters	0854048

Initial design

Introduction

In this design chapter, the initial design of a maze solving robot is specified in terms of requirements, specifications, components, functions and interfaces. It is implemented on the PICO hardware platform. A task-skill-motion framework is used in order to describe the robot behaviour. This will also be implemented in C++.

Requirements and specifications

PICO is required to autonomously navigate through a maze and find the exit without bumping into any of the walls. A door will be present in the maze, and must be opened by "ringing the bell" in a suitable location whilst standing still. Open spaces might be present, and the walls are approximately axis aligned. The maze might also feature loops. Therefore PICO is required to do the following:

• Recognize walls and corridors in a robust manner. The walls are axis aligned, however not perfectly. PICO should allow for an alignment error. It also has to stay away from the walls with a sufficient margin (e.g. 10 cm between robot and wall). Bumping into a wall will count as a failed attempt.
• Identify possible "door areas" in a robust manner. "The door will be situated at a dead end, a dead end being defined as a piece of wall with minimum length 0.5 meter and maximum length 1.5 meter with side-walls of at least 0.3 meter length". Alignment errors again need to be taken into account.
• Map the maze to determine whether the area has been visited and to build a world model. Odometry and a Laser Range Finder (LRF) are used to build a world map and determine the location in the world. The odometry and LRF data should be synchronized to avoid drift in world location and mapping (SLAM).
• Recognize corners as nodes in order to build an abstract and simplified maze representation consisting of nodes and lines.
• Cary out a maze solving strategy based on its world model and solve and execute corresponding routing. The solving routine and the routing should be able to handle loops and open spaces and are based on the simplified maze representation. This enables more general strategy composition and faster computation.
• Open doors by ringing a bell in the door area. No part of PICO should be located outside the 1.3 meter range of the door and PICO should be standing still whilst ringing the bell. The door opens completely within 5 seconds after PICOs request. Once the door is completely open, PICO needs to pass the open doorway to find the exit of the maze.
• The software should be easy to set up and deploy, i.e. using git pull and with a single executable.
• Clear the maze as soon as possible, with a total available time of seven minutes for two attempts. A maximum speed of 0.5 m/s translational and 1.2 rad/s rotational is given.
• Do not stand still for more than thirty seconds when inside the maze.
• Recognize when the maze is solved.

Components

Pico features a Laser Range Finder (LRF) to detect surrounding obstacles and wheel encoders to measure odometry data. The wheel base is holonomic, enabling it to turn on the spot and drive in any direction. An emergency button is present on the remote. PICO runs on batteries and should always be charging when not driving around.

The software runs on an Intel i7 processor running Ubuntu 14.04 LTS. The Robotic Operating System (ROS) is used as a framework for PICOs software. However, an intermediate software layer is provided which can be used to access sensor data and actuate the robot. Our own software layer is divided into five main components: actuation, sensing, world model, task manager and skill.

Functions

Configuration file

• Configuration: Defines a number of parameters. This is a header file so no inputs or outputs.

Actuation

• Move: This function calculates the translational velocity in x and y direction and the rotational velocity corresponding to the direction vector and net velocity outputted by the path planning function. The calculated velocities are sent tot the base actuators.

Input: Direction vector, total velocity, Output: -.

• Stop: Stops the robot by sending the base velocities of magnitude zero.

Input:-, Output:-.

• Rotate: Rotates the robot 90 or 180 degrees. First checks the robot has enough space to rotate without hitting the wall. If this is not the case, first a call to Move is made to move the robot away from the wall.
• Open door: Makes sure the robot stands still, rings the bell to open the door and waits until the door is opened.

Input: LRF data, Output: -.

Sensing

• Get LRF data: Gets the data from the laser range finder.

Input: -, Output: LRF data

• Get odometry data: Gets position data from omni-wheels.

Input: -, Output: odometry data

World Model

• SLAM: Simultaneous Localization And Mapping, this updates the position of PICO in the world model using new sensor data.

Input: LRF data, Odometry data, Output: World model

• Object recognition: This function will estimate which objects there are in the World Model using the geometries of the environment.

Input: World model, Output: Objects with their coordinates.

Task Manager

• Task monitor: Determines which piece of code will be executed next.

Input: -, Output: -

Skill

• Filter data: Moving average filter to minimize noise.

Input: noisy data, Output: averaged data.

• Finish: Recognizes that the finish line has been crossed and that Pico can stop searching for the exit. The main script can be terminated.

Input: World model, Output: -

• Strategy: Gives a general direction based on the maze solving algorithm that is chosen.

Input: World model data, LRF data. Output: Target point.

• Path planning: Uses an Artificial Potential Field to output a direction vector considering the target point outputted by the strategy function and the LRF data.

Input: Target point, LRF data. Output: Direction vector

Interfaces

The software functions have multiple inputs and outputs and can be grouped by functionality. If data from the sensing component is used by a function from the skill component, the data is transferred between these components, this is referred to as an interface. A rough overview of the interfaces is shown in Figure 1. The dashed line represents that the actions of the robot will influence what the sensors measure.

Figure 1: Rough overview of the interfaces

Corridor Challenge

Repulsion & Angle correction

Figure 2:Artificial potential field

Figure 3: Angle correction

The first thing that was considered for the corridor challenge was some kind of repulsion from the walls to make sure Pico does not bump into anything. For the repulsion a simple version of an Artificial Potential Field is considered, which is given by the formula

[math]\displaystyle{ F_x = \frac{1}{b} \frac{1}{N_1} \sum_{i=1}^{N_1} \frac{1}{(r_i-a)^3} cos(\phi _i) }[/math] [math]\displaystyle{ F_y = \frac{1}{b} \frac{1}{N_2} \sum_{i=1}^{N_2} \frac{1}{(r_i-a)^3} sin(\phi _i) }[/math]

where b is a scaling variable that can be used for finetuning, N is the length of the considered laser data vector and \phi_i is the angle corresponding to measured distance distance r_i. Variable a is a correction factor for the width of pico that can also be used during the finetuning.

For the calculation of the force in the forward direction, [math]\displaystyle{ F_x }[/math], only laser data in front of Pico is used. Al the other laser data is used for the calculation of [math]\displaystyle{ F_y }[/math], force in side ward direction. This is also shown in the figure 2. The resulting netto forces are used as an input for the actuation function which is further explained below. Note that if the laser sees something at a very large distance or does not see anything at all, the returned distance is very small or even negative. Therefore all distances smaller then 0.01 meter are set to 10 meter.

For the angle correction all the laser data at the right side of Pico is considered. The smallest distance to the wall and the corresponding angle are found using a for-loop. After subtraction of 90 degrees, the resulting angle is used as input for the desired rotation speed. This approach can be interpreted as a torsional spring connected to Pico and is visualized in figure 3.

Actuation

Velocity scaling explained

The main functionality of the actuation script is to scale velocity signals that are sent to Pico in such a way that the constraints on maximum velocities are respected. Besides, effects of angle wrapping are compensated to create useful odometry data to rotate with a certain angle. Scaling of the translational speeds in x and y direction is done using

[math]\displaystyle{ \vec V_x = \vec F_x \sqrt{\frac{V_{max}^2}{\vec F_x^2 + \vec F_y^2}} }[/math]

[math]\displaystyle{ \vec V_y = \vec F_y \sqrt{\frac{V_{max}^2}{\vec F_x^2 + \vec F_y^2}} }[/math]

in which Fx and Fy are the force signals from the repulsion script sent to the actuation script. This is clarified in the figure on the right. If an angular velocity exceeds the maximum value, the angular velocity simply gets scaled back to the maximum angular velocity using

[math]\displaystyle{ \omega = sign(\omega) \cdot \omega_{max} }[/math]

such that it makes sure that Pico will still turn in the right direction.

When using the actuation script to rotate with a certain angle, the script will in each iteration return a relative rotation angle. The script in which the actuation script is called then has to integrate this relative angle until the desired angle has been rotated. However, the angle from the odometry data can jump from –π to π of vice-versa. If this happens, the relative angle will not be correct and has to be adjusted by adding or subtracting 2π.

Making the turn

Driving between two walls can be easily done using the the actuation, repulsion and angle correction code. To actually make a turn at the right moment to drive into the corridor, the corridor has to be recognized. From the world model, corner nodes and end nodes are received. If a corner node is recognized at the right or left of Pico (approximately 90 degrees) and if the distance to this corner node is reasonable, the distance to the closest next corner node is used to calculate the width of the corridor using the cosine rule. If no other corner node is recognized or this corner node is to far away, the distance the the first corner node (which is in this case seen at the right of pico) is taken as the corridor distance.

Final design

Interface

After the corridor challenge, the software architecture is changed a bit. Now the software consists of five main parts: Actuation, skill, sensing, worldmodel and Taskmanager. These different software parts with their corresponding in- and outputs are depicted in Figure 2. The software architecture is globally explained in this section. The important parts are further explained in the following sections.

Taskmanager

The taskmanager is the top part, which controls the whole software. Data comes in via the skill and sensing part, and data is sent to the strategy and actuation part. Based on the data from the other parts, the task manager makes decisions on what to do. First, it calls the variable ‘door detected’ to check whether Pico is in a door area. Then it will go to the initial state ‘start’ in which the startup procedure is implemented. It checks if there is a wall on the right side of Pico, and Pico will be aligned to this wall. When Pico is aligned to this wall, it will go to the following state ‘straight_pref’. In this state, Pico drives in its preferred direction and calls the boolean vector from the space recognition skill. This information is sent to the strategy skill, which sends the direction and state back. Based on this information it will stay in the case ‘straight_pref’ or switch to the state ‘straight’, ‘turn_left’, ‘turn_right’ or ‘turn_180’. When it stays in the same case, it will call the force vector from the repulsion skill. After that, the actuation vector is sent to the actuation part. When it switches to a state ‘straight’, the same is implemented as in the state ‘straight_pref’, but now the distance to the wall is added. This is implemented as a virtual spring, which will be further explained in the Maze solving algorithm section. In the states ‘turn_left’ and ‘turn_right’, the odometry data is used to check whether Pico has rotated enough.

Actuation

The input of the actuation part is an actuation vector, which contains the horizontal, vertical and angular velocity or a door request. The actuation part consists of four sub parts: move, stop, rotate and opendoor. The taskmanager decides which part is called. In the actuation part, the velocity is scaled, such that it does not exceed the maximum velocity. When the normalized velocity is calculated, this is sent to the actuators. When opendoor is called, an ‘opendoorrequest’ is sent to the actuator such that it will ring a bell. Sensing In the sensing part, the odometry and LRF data is obtained. This LRF data goes straight to different parts. By calculating the difference between the previous and the current data, the wrapping of the data is corrected and the relative rotation is calculated.

World Model

The world model is a function that detects nodes using the LRF. Using various algorithms, the data from the LRF can be filtered and the most important data points are saved as nodes. Important nodes concern mostly nodes that lie on the end of walls or that are corners. Using these nodes, the current world that PICO sees can be made. These nodes can also be used to recognize doors.

Skill

The skill part contains the skills of Pico, which are the calculation of the repulsion, the space recognition, the door recognition and the strategy. In the repulsion part, the repulsion forces are calculated using the LRF data. In the space recognition part, it is checked whether there is space to the left, right or front using the LRF data. In the door recognition, the door areas are calculated using the nodes from the World Model. When a Pico is in a door area the Boolean door detected will be ‘true’. The strategy part gets the boolean vector of the spaces from the task manager. Based on this spaces boolean vector, the strategy part will calculate the direction and desired state using the Pledge algorithm.

Figure 2: Software architecture

Space recognition

This function determines whether there is space or an obstacle to the left, right and top of Pico. In the config file, the beam width is determined in degrees. This value is converted to degrees and used to determine which LRF data points should be checked. As can be seen in the figure, there are 3 beams to be measured: left of Pico, above and to the right of Pico. By default, all spaces are defined true. If one measured LRF point is below the threshold value, indicated with an orange circle, the space_x variable will be set false. The threshold for the forward distance can be tuned independently of the others. The forward distance is slightly smaller than the other distances. This means that Pico will detect an obstacle in front of him later and will react later to this. This way a better wallfollowing can be realized. The function will need the scan information from the LRF as an input and will output variables space_left, space_top and space_right.

Figure 2:Space recognition

Maze solving algorithm

Figure 3: Space around Pico

The Pledge algorithm is chosen to solve te maze. The pledge algorithm has a preferred direction and counts the corners the robot takes. A corner left counts as -1 and a corner right as +1. When the counter is 0, the robot goes in its preferred direction. The pledge algorithm is implemented in the following manner:

• If Pico is driving in preferred direction:
  • if space_top = true: Go straight in preferred direction
  • if space_top = false & space_left = true: rotate 90 degrees left
  • if space top = false & spacel_left = false & space_right = false: rotate 180 degrees left
• Else
  • if space_right = true: rotate 90 degrees right
  • if space right = false & space top = true: go straight
  • if space_right = false & space_top = false & space_left = true: rotate 90 degrees left
  • if space_right = false & space_top = false & space_left = false: rotate 180 degrees left

In Figure 3 Pico is depicted with three beams: space_left, space_right and space_top. All beams are using the 10 degrees of lrf-data.

Repulsion & wall following

How the repulsion and angle correction are working is already explained in the section Corridor challenge. The final repulsion and angle correction are not changed very much in the final design, only some values are tuned. The wall follower is added in the final design, which makes sure Pico stays at a desired distance from the wall. The wall follower is implemented as a virtual spring. When Pico is too close to the wall, the repulsion becomes very high and Pico will be pushed from the wall. When Pico is too far from the wall, the virtual spring will give a force. Therefore, Pico will be pulled to the wall. Between the repulsion and virtual spring is some threshold, such that the forces are zero in that area. This makes sure that Pico does not correct itself the whole time.

World model

In order to recognize door areas a world model is constructed which uses landmark detection. Landmarks are defined as nodes, which are points in the LRF-data with certain properties which are collectively defined as either "end" nodes or "corned" nodes. The world model library provides two public functions which can be used in order to check whether doors are present:

header function with comments of scanNode and doorCheck

Node class

The Node class (data type) is defined as follows:

Overview of the Node Class used in the world model.

• ID (int): The ID of the nodes are set from right to left (in the scanning direction of the LRF) starting at ID 0.
• Type (int): The types are defined in the header file of the world model library worldmodel.h. Two types are currently in use: Cornernodes and Endnodes.
• Location (location): For each node the Location class is used to track the position in both carthesian and radial coordinates relative to PICO. When the location class is updated using the radial coordinates the carthesian coordinates are updated automatically. VOEG FIGUUR TOE ASSENSTELSELS
• Connections (std::vector<int>): A vector of node IDs. For each connected node the ID is stored in this vector. Thus it is possible to determine which nodes are connected by walls and which are not using the Node class.
• LRFindex (int): Index in the LRF scan of the particular data point corresponding to the current node. It is mainly used to determine the connection vector.

Node Recognition

ADD PIECE OF CODE

Visualization of the node recognition. Corner nodes are shown in white and end nodes are shown in red. PICO is the red block in the middle.

The world model is based on two types of nodes: endnodes and cornernodes. Finding nodes is done with the following checks over all data-points:

• The current data-point is a cornernode if the distance between the current data-point and the next data-point is larger than a given value.
• The current data-point is a cornernode if is passes the 90-degrees-test. This is a test that considers a previous data-point that lies at a set distance from the current data-point, and a next data-point that also lies at a set distance from the current data-point. Using the cosine rule, angles and distances to the three points, the actual distance between the previous and next datapoint can be compared to the distance between these points when applying Pythagoras. Pythagoras only holds if the angle is 90 degrees, so if the two values are equal, then Pythagoras is applicable and thus the corner is 90 degrees. [math]\displaystyle{ x_r }[/math]

[math]\displaystyle{ A = \sqrt{(x_2-x_1)^2+(y_2-y_1)^2} }[/math] [math]\displaystyle{ B = \sqrt{(x_3-x_1)^2+(y_3-y_1)^2} }[/math]

• The current data-point is an endnode if the distance between the current data-point and the next data-point is smaller than a given value.
• A data-point that lies at the maximum range or at the maximum angles is considered an endnode.

The visualization of the corner nodes and the end nodes serves to debug and tune the object recognition.

Several tunable parameters are associated with the node recognition:

Wall Recognition

In order to recognize whether nodes are connected the LRF index of the subsequent nodes in is checked in order to see whether nodes are set at subsequent indexes of the LRF data. Two nodes can only have subsequent LRF indexes when the are separated by a large distance because of both the SPIKE check which compares distances, and a tunable parameter which determines the allowed overlap of nodes. Thus the nodes are not connected when the check is true.

Each node is checked in sequence. A connection is thus always added "backwards", connecting the node checked to the previous one by modifying the Connections variable of both.

Door Recognition

SHOW SIMULATION

Maze challenge

Figure 88:Champions