Embedded Motion Control 2019 Group 8

Group Members

Name	Student ID	E-mail
Stan (C.M.) den Hartog	0953184	c.m.d.hartog@student.tue.nl
Elise (E.D.T.) Verhees	0950109	e.d.t.verhees@student.tue.nl
Rob (R.J.G.) Dorussen	0968849	r.j.g.dorussen@student.tue.nl
Gosse (G.) Bijlenga	0950642	g.bijlenga@student.tue.nl
Max (M.J.) van Haren	0953564	m.j.v.haren@student.tue.nl

Introduction

The goal of the course Embedded Motion Control (EMC) is to design software such that the robot (PICO) can perform tasks autonomously in a real life setting. The implementation of the software skills to fulfill real-life robotic tasks in real-time situations, takes into account both planning and basic programming. In order to see how well the objective is reached, PICO is tested in the following two challenges.

The first challenge is the Escape Room Challenge, in which the robot is placed in a room with a single exit with a random orientation. Upon placement, the robot should autonomously find the exit, move towards the exit and exit the room without touching the surroundings. The second challenge is the Hospital Challenge, in which the robot should autonomously function within a hospital setting, based on a specific map. The goal of the robot is to go from cabinet to cabinet, while on the way encountering random disturbances as deemed normal in a hospital.

This wiki page will give an overview of the designs for both the Escape Room and Hospital challenges.

Escape Room Challenge

Design Document

For the Escape Room Challenge a design document has been made. This document contains a description of the requirements, specifications, components, functions and interfaces. The file can be found at Deliverables.

Escape Room Execution

A short summary of the actions the robot will undertake can be seen in the State machine showed below.

Figure 1: State machine used in the escape room challenge.

Wall Following

A robust solution is to follow a wall, this is chosen to be implemented first. The decision has been made to keep the wall on the left side of PICO. The wall following function works as follows: first, PICO checks if there is a wall in front of it. This is done by checking if the minimum distance in a small cone in front of PICO is lower than a certain threshold. If there is a wall in front of PICO, it realigns with the wall using the angle at which it found the minimum distance. Secondly, PICO has to remain close to the wall. The robot checks what the minimum distance is in a cone on the left side. If this distance violates a minimum threshold, PICO will strafe away from the wall. But if the minimum distance on the left is too big, PICO will strafe towards the wall. Thirdly, PICO should have an exit/outer wall detection. A smaller cone on the left will be checked. If every distance in that small cone is larger than 1.2 meters, PICO recognizes this as an exit. It will move a little bit further forward, and then drive into the 'exit'. Good results are achieved in the 2 random maps that are shown in Figure 2 and 3.

Scanning for an Exit

After that, the robot has been made a bit 'smarter'; It will first scan the room for an exit. PICO checks the difference between two LRF measurements next to each other. If this distance is larger than 0.5 meter, PICO recognizes it as an exit. If an exit is found, it will stop rotating, rotate a bit back, to make sure it will head for the left wall, start heading for the exit, and then continue with the wall following procedure to exit the corridor. The result using both wall following and scanning for an exit can be seen in Figure 4.

• 

  Figure 2: Test 1 in simulator using wall following only.

• 

  Figure 3: Test 2 in simulator using wall following only.

• 

  Figure 4: Test 1 in simulator using wall following and scanning for an exit.

Escape Room Result

The result in the escape room challenge is that PICO was able to get to the corridor, but it was not able to finish. Halfway through the corridor, PICO recognized another 'exit' and bumped into the wall. The final run can be seen in Figure 5.

Figure 5: Result in the escape room challenge.

Evaluation of the Escape Room

The escape room challenge clearly showed robustness is key in solving robotics problems.

First of all, PICO was able to detect the exit of the room correctly. However, as Figure 5 shows, PICO does not drive straight to the exit. This is because the robot will turn slightly back from the exit so the robot can use wall following again to enter the corridor. The angle which the robot will turn is predefined as a constant and therefore is far from optimal. This could have been improved by constantly detecting for the exit and facing towards it.

Next to that, Figure 5 shows the robot is stuck in the corridor. This can be explained since there was a wall placed skewed in the corridor which created a hole in the wall.

PICO detected this hole as an exit and headed for it. When turning to align with the "exit", PICO didn't see any walls to the left or right and therefore was stuck in the statemachine.

Implementing the ideas itself did not seem a big issue, however robustly solving these problems did. In the hospital challenge, the effort has been made to tackle all the problems as robustly as possible.

Hospital Challenge

Hospital Challenge Brainstorm Session

To prepare for the hospital challenge, a brainstorm session is done and several new requirements, specifications, functions and components are defined.

Requirements and specifications

Requirements	Specifications
The robot should act autonomously.	The robot should visit all cabinets in the correct order.
	The robot should be able to plan a route based on its location.
	The robot should be able to plan an alternative route based on static objects.
	The robot should finish within 10 minutes and 2 trials.
	The robot should be able to localize itself.
The robot should not bump into objects.	The robot should test multiple hypotheses in order to categorize obstacles.
	The robot should adjust its direction and/or speed based on the dynamic nature of the objects.
The robot may not tip over.	Maximum translational speed: 0.5 m/s.
	Maximum rotational speed: 1.2 rad/s.
	The robot should stop before rotating angles larger than 45 degrees.
	Maximum inclination angle: ~5 degrees.
The robot should make progress every 30 seconds.	Once entering a new state, the robot should report its status.
The robot should be able to make a snapshot at a cabinet.	The robot should align with the cabinet.
The robot should function robustly.	The functions must be scalable.
	The definition of data ownership must be explicit.
	The robot should be able to keep functioning when data is not accurate.
	If no data is available, the robot should stop.
The software should be easy to set up.	The software can be updated by one command.
	The software is compatible with cmake/make.

Components

The following diagram schematically illustrates the components needed to fulfill the requirements and specifications.

Figure 6: Components used in the hospital challenge

Functions

• Route planning.
• Localize.
• Drive.
• Detect object.
• Avoid object.
• Align with cabinet.
• Take snapshot.
• Visualize.

Components of the Hospital Challenge

The components seen in Figure 6 are further elaborated on in this section.

Localization

---

In order to be able to find the cabinets, it is important to know where the robot is located. A scalable solution, with a fixed calculation time, is using a particle filter with a fixed amount of particles. The particle filter concentrates particles around the location of the robot, which is described in the first subsection. The LRF data is used to validate the likelihood of a particle, explained in the second subsection. Particles with a high likelihood survive, while others fade away. Geometric properties from the map are used to add a weighting factor to the particles from the current LRF data. In the evaluation some possible extensions of the current filter are suggested to make it more robust.

Particle Filter

A particle filter is a filter where the concentration of the particles determines the position of the robot. A particle has four properties: an x and y location in the 2D plane, an orientation and a probability. One particle can be seen as a guess of the current position and orientation of the robot. First, the particles have to be created. The initial particles are placed in the starting area on the given map. Secondly, after the initialization, some noise is added to the location and orientation of the particles. Adding noise results in particles with a slightly changed position and orientation. Besides the noise, there are also some random particles placed around the map. This is done for relocalization in case the robot has lost its position. Now the particles are repositioned and reorientated. Thirdly, the particles that fit the current position of the robot have to survive, while others are removed. The particles are resampled based on their weighting factor (explained in the next subsection). Particles with a high probability have a higher chance of being stored (sometimes multiple times), while others are removed. Note that random particles over the map normally do not fit the LRF data well. In the simulation below one can see only a few cases where a random particle is stored after resampling, while these particles are relatively close to the robot position. Then step two is done again: adding noise. Followed by step three: determining the probability and resampling. During each location update ,iteration step two and three will be done. Over time, the position of the robot is determined through determining the mean position and orientation of the particles. When the LRF data gives a clear position on the map, the particles are concentrated. When the LRF data is badly correlated to the position, the particles spread out over the map. The current position is determined by taking the average position and orientation of all particles. The mean weighting factor of the sampled particles is used as a measure of how good the particles are fitted on the map and is therefore a measure whether the current guess is believed to be correct (if there is only one particle cluster).

Geometric Properties

After adding noise to a particle, a weighting factor has to be calculated in order to determine how 'good' the new particle is. On a conceptual level, the distance measured with the LRF is compared to the expected LRF distance. By using an example (see figure below) it is illustrated how the weighting factor is implemented. The particle in this example is located at [math]\displaystyle{ (x,y) }[/math] with an absolute orientation [math]\displaystyle{ \phi }[/math]. The LRF beam that is checked has an absolute orientation of [math]\displaystyle{ \theta }[/math], which is in the first quadrant I. A vertical wall is located from point [math]\displaystyle{ p_1 }[/math] to point [math]\displaystyle{ p_2 }[/math]. The intersection of this wall and the LRF beam is on the [math]\displaystyle{ x }[/math]-axis at [math]\displaystyle{ x }[/math]+[math]\displaystyle{ \Delta x }[/math] (the position of the particle plus the horizontal distance of the beam). The relative height (in the vertical direction) of the beam can be determined using the tangent function by [math]\displaystyle{ \Delta }[/math]y=tan([math]\displaystyle{ \theta }[/math])[math]\displaystyle{ \cdot }[/math][math]\displaystyle{ \Delta }[/math]x. Using the relative height one can check if the absolute height is between the height of point [math]\displaystyle{ p_1 }[/math] and point [math]\displaystyle{ p_2 }[/math]. When this is not the case, the probability of detecting the wall is zero. When this is however the case (as can be seen in the example below), the expected LRF distance is calculated using the cosine function by dLRFest = [math]\displaystyle{ \Delta }[/math][math]\displaystyle{ x }[/math]/cos([math]\displaystyle{ \theta }[/math]). Using a normal distribution, the probability is calculated with the expected value dLRFest and the measured LRF distance. A similar calculation is done for the quadrants II, III and IV. Particles that match the position and orientation of the robot well get a higher probability while the sensor data is closer (thus getting a higher probability) to the expected LRF data of the particle. Note that this calculation is done for all beams, except for the beams at the outer edges, as these sometimes 'see' the robot itself.

Figure 7: Example case geometric properties

Below, the particle filter in action is shown.

• 

  Figure 8: Particle filter simulation.

Route planning

---

As in this challenge our focus lies on robustness, and not on the speed at which the task is fulfilled, throughout the map there are waypoints that are placed at convenient places. These waypoints can be seen as yellow little circles in one of the simulation GIFs on this wiki, for example Figure 8. Note that these waypoints are not exact points, but they cover an area around the waypoints. When the robot passes through the area, the waypoint is reached. The waypoints are placed in such a manner that the robot could drive in straight lines from waypoint to waypoint, thus at corners and corridors or doors. In order to decide the order of the waypoints for the path of the PICO robot, some paths are hardcoded. Each path starts in the hallway and goes to each cabinet, as it is assumed that each cabinet will be accessible through the corridor. For each cabinet, there are multiple paths that are hardcoded. PICO cycles through these paths if obstacles are found to be blocking the path in such a manner that continuing along the current path would be impossible.

State machine

---

In Figure 9, the state machine can be seen. The state machine cycles through the steps of route planning and actuator control in order to fulfill the task. At the start of each assignment, there is an initialization state where the robot starts up and checks functionality. Next, it will place itself using the localization, where it will determine its route to the next cabinet. Then, the robot will rotate itself in such a way that a straight path can be made towards the next waypoint, after which PICO will drive towards that waypoint.

Once a waypoint is reached, there are three options. The first option is that the robot must go to the next waypoint, after which the robot will orient itself again. The second option is that the robot has arrived at a cabinet, and is required to orient itself to be aligned with the cabinet, take a snapshot, and afterwards determine a new route, towards a new waypoint. The third option is that a new route is required, due to obstacles, doors, etc.

Throughout the state machine, localization and object detection/avoidance are running, which both override the state machine in importance. If localization cannot take place, the robot will pause the state machine in order to regain the location. If an object is detected and required to be avoided, the state machine will be paused in order to keep PICO from touching foreign objects.

Figure 9: Finite state machine of the PICO robot during the hospital challenge

Object detection/avoidance

---

In order to be able to drive from waypoint to waypoint without any collisions, there are two different options hardcoded. The first option is when driving in open space and the second options is when driving through a narrow opening, such as a door. Each case will be explained below.

Open space object avoidance

In the open space, objects can be encountered, which could be either dynamic objects, such as people walking around, or static objects, such as cupboards. In order for PICO to know how to act upon sensing an object nearby, an area around PICO is taken as seen in Figure 10.

If an object is detected in the green space, which is more than half a meter away from PICO, PICO will not react at all as there is no interference with the foreseeable path of PICO. Once an object enters the yellow space, PICO pauses and tests hypotheses. The first hypothesis to be tested is whether it is a static or dynamic object. If the shortest distance to the sensed object stays within a minimum angle difference and minimum difference in shortest distance, the object is assumed to stand still. If the object is not in the direction that PICO is desired to move in, PICO will continue on its path. If the object is in the way, PICO will sense the quadrant where the static object is in, and move to the open space in the opposing quadrant, until the object is no longer within the circle and the path will be free. If PICO cannot find a way around the static object after 3 trials, PICO will delete the waypoint for this trajectory and continue by finding a different path.

The second set of hypotheses to be tested is the direction of the dynamic object. PICO measures the angle and magnitude of the minimum distance to the object and saves this in the world model. PICO pauses for a second and measures the magnitude and angle of the minimum distance again. If the difference in angle of the minimum distance differs minimally, whereas the minimum distance decreases/increases, the hypotheses can be made that the object is moving closer/further away. If the object is moving closer, PICO will check in which quadrant the object is entering, and check for free space in the opposing quadrant. If there is free space, PICO will move into that quadrant until the threat is no longer moving towards PICO. If the object is moving further away, PICO waits until the object exits the circle and continues driving. Next, if the difference in angle of the minimum distance changes after the waiting time, whereas the minimum distance can change, the object is assumed to move across from PICO, without hitting PICO. PICO then waits until the object moves across from the path that PICO desires and PICO then continues driving.

Once an object crosses the yellow circle, by for example unexpected behavior by dynamic objects, and enters the red circle, PICO does not test hypotheses. PICO checks in which quadrant the object enters, and checks the minimum distance in the opposing quadrant and drives there if said quadrant is free. If the opposing quadrant is not free, PICO will drive backwards until the object is no longer in the red circle, and until there is free space in the opposing quadrant. Here, it is assumed that the path that PICO just traveled is free to drive backwards.

Narrow way object avoidance

Once in between doors, the second option is utilized. In this case there is only a small circle as seen in Figure 11. In this case the red zone is slightly larger than the red zone as in the open space in order to give more time to build hypotheses. Once an object enters the red zone, quick hypotheses are tested as in the yellow circle of the open space object avoidance. Once an object enters the space, a very short time is waited in order to determine whether an object is static or dynamic. If the object is dynamic, the quadrant is tested and PICO moves into the opposing quadrant if the quadrant is free, else PICO moves backwards. If an object is static, it is tested whether it is a door, by testing whether there are areas where the maximum distance seen when facing towards the wall is larger than expected. If it is a door, PICO requests for the door to be opened. If it is a static object in the way, the waypoint is deleted for this trajectory and a new path is decided. Once an object enters the circle on the right or left, PICO moves in the opposite direction in order to stay around the middle of the doorway, to avoid collision with the walls.

• 

  Figure 10:The different areas around PICO once in an open space.

• 

  Figure 11:The different areas around PICO once in a narrow space.

Motor Control

---

Through investigations, it was already clear that no complicated control was necessary. To make the movements of PICO slightly more smooth, a feedback proportional controller has been implemented.

The function furthermore returns the relative distance traveled each sampling interval and saves the current velocity in X- and Y-direction and the rotational velocity in the worldmodel.

Visualization

---

Visualization may not be a component in itself, but this section shortly summarizes what visualization capabilities have been incorporated in the code and why.

First of all, the given .json file, containing the walls and cabinets is used to plot the general map of the hospital. To better understand the behavior of the robot, and make debugging easier, both the waypoints and the localization (current position and rotation) are added to the visualization. This makes it easier to see where the robot thinks it is and where it is heading.

All interaction between the simulation/real time run and the visualization is done through the world model.

Note that the visualization is run on a separate thread. This is done because the visualization is run at a way lower frequency than the control of the robot. Partly because running it at the same frequency would kill the program, but foremost because it is just not needed to run your visualization that often because PICO cannot travel that fast.

Simulation results of the Hospital Challenge

Two simulation examples have been made for the hospital challenge. One nominal situation and one situation where clutter objects are apparant on the map. These can be seen below.

• 

  Figure 12: Simulation of Pico in nominal environment.

• 

  Figure 13: Simulation of Pico in cluttered environment.

Both videos show desired behavior of the robot. In the nominal situation, the robot even succeeds to visit all the cabinets. However, in the cluttered environment, PICO's localization fails due to the environment not matching the map. This can be seen at some point when PICO is in the hallway. Below a Figure is shown with 2 consecutive location samples, the green one is an accurate guess while the red one is estimated badly.

Figure 14: Error of the localization, the green circle is the correct position while the red one is estimated badly.

Results of the Hospital Challenge

In order to give a complete overview of the Hospital Challenge, a video compilation is made of the performance of PICO on the actual challenge day. It covers a trial with all static objects, doors and dynamic objects included (which was tried twice during the challenge), and a nominal run with only a dynamic object and no doors or static objects from directly after the challenge. A GIF of this video compilation can be seen in Figure 15. Also, in the link given below, the full video in high quality can be seen.

The results were as expected. The simulation results already showed there were flaws in the localization when the deviation from the nominal situation was too large. However, PICO is able to fully complete the nominal challenge, as can be seen in Figure 15.

Figure 15: Gif of the compilation video of the Hospital Challenge

https://www.youtube.com/embed/D3zYT49ZT44

Evaluation of the Hospital Challenge

An evaluation of the Hospital challenge is done to review the performance of the software. A division is made between improvements, which would make the robot finish the Hospital Challenge, and recommendations, which would further optimize the performance.

Improvements

Localization
As can be seen in the trial runs in the Hospital Challenge, the main issue in the concept lies within the localization. The location is determined at every iteration and the constant addition of random particles could create a close match everywhere on the map. This results in the position possibly flying around the map. This relocatization should only be done when the robot is sure that it has lost its position. It should not be possible to jump the map when there are only small changes from odometry measurements.

The update of particles should take into account the relative odometry data. Particles should all be shifted with the relative movement before noise is added.

The localization is now considered to be the mean of all particles. This could be improved by including a cluster detection. When there are two locations on the map, which have a high likelihood of being the true position, this should be the output. Instead of just taking the mean of both clusters.

Furthermore, when the robot reaches the area where the 'couch', 'cupboard' and 'door' are placed, the robot is unable to locate itself, even though PICO has access to its location from a previous time step, which is reliable. When the location is reliable, the LRF data can be used to validate different hypotheses. For example, PICO could test if a door is closed, which could be added to the map. PICO could determine static objects, which also could be added to the map. This creates a more robust algorithm for localization, where unexpected changes are no longer disturbances to PICO, but are considered as localization points.

Finally, the localization could be improved by adding a weighting factor to different type of objects. Walls for example are always present, and if a fit can be made on a wall this should have a higher addition of probability compared to making a fit to a dynamic obstacle for example.

Path Planning
During the Hospital Challenge, PICO is lifted and put inside the right room, which it recognized. However, the path planning was designed in such a way that PICO had to follow a list of waypoints until it got stuck. Once regaining its location in the right room, PICO's path planning intended for PICO to drive back to the waypoint on the way to the cabinet. As an improvement, the path planning should have taken into account the actual location and closest distances to different waypoints and planned its route accordingly.

Object Detection
Lastly, PICO bases its hypotheses of off short time periods. In order to increase robustness, PICO should test certain hypotheses, such as the hypothesis whether a static object is truly static throughout the entire Hospital Challenge or is static for certain periods of time, multiple times and should have the ability to turn certain waypoints on and off.

Recommendations

Path Planning
Because the hospital which had to be navigated during the challenge was not large, the paths could easily be hard coded in a short period of time. However, if the hospital would be larger, this would take significantly more time. Therefore, a simple algorithm could be implemented, returning a short path to the next cabinet. A suitable algorithm could for example make use of dynamic programming, which would use the absolute distance from waypoint to waypoint as costs.

Sampling Rate of Localization
As stated before, the odometry data should be included to determine the position of the robot. If this would have been done, the verification of the robot position using the laser range finder could have been sampled less frequent. This would lower the computational load.

Code Snippets

In the first code snippit it can be seen that for each waypoint, there are three possibilities. The first possibility is that the task is finished, as no cabinets are left in the list. The second possibility is that the waypoint list is empty. In this case, PICO arrives either at a cabinet or in the hallway. At the cabinet PICO will be facing it and taking a snapshot, then the desired direction is given as the opposite of the direction facing the cabinet and the cabinet is removed from the queue and a new route is determined to the next cabinet or hallway. If PICO arrives in the hallway, a new path is planned to the desired cabinet. The last option is that PICO arrives at an intermediate waypoint, where PICO stops, determines the required angle to the next waypoint and will orient itself towards it.

Waypoint Arrival decision making

In order to increase robustness of PICO, difference in expected sizes of objects, or lack of objects is to be accounted for. For example, if a static object is placed where PICO is moving, however it is not completely in its path, this snippit works simultaneously with driving towards the path, leading to a smooth motion to avoid the object. Also in corridors, PICO can use this to avoid walls effectively, while still continuing its path.

Small circle object avoidance

In the next code snippit, a hypothesis is checked when an object is in the circle. PICO checks whether the object is static or not, and in this snippit, PICO checks whether the object is coming towards PICO. In this case, PICO will check which quadrant the object is coming from, check whether the opposing side is free to move in, move there and as last resort will drive backwards until the object is no longer in range of PICO, or until the opposing quadrant is free to move in. This is done in order to dodge an incoming object without harm to PICO.

Object hypothesis checking

The last code snippit shows how the particle filter updates are done. Note that three dots (...) means that there is some code removed for clarity. First some noise is added to all particles and the particles get a weighting factor. Then the normalization is done using the summation of the weighting factors of the particles. After the weighting factor is normalized, the cumulative weighting factor is determined. Note that this normalized factor sums to one. Then by choosing a random value between zero and one, a sample from the cumulative probability is taken. Particles with a high weighting factor are sampled more likely and they are stored. Then the cycle starts again.

Particle filter update

Deliverables

• File:Design document.pdf
• File:Presentation EscapeRoom Challenge.pdf
• File:Presentation Hospital Challenge.pdf