Embedded Motion Control 2018 Group 3

Group Members

TU/e Number	Name	E-mail
0848904	Luc (L.L.M.) van den Aker	l dot l dot m dot v dot d dot aker at student dot tue dot nl
0852908	Thomas (T.) Neilen	t dot neilen at student dot tue dot nl
0909434	Jeroen (J.W.) van de Valk	j dot w dot v dot d dot valk at student dot tue dot nl
0896947	Nourdin (N.) Kaai	n dot kaai at student dot tue dot nl
0883056	Peter (P.) van Dooren	p dot v dot dooren at student dot tue dot nl
0861750	Ties (T.J.) van Loon	t dot j dot v dot loon at student dot tue dot nl

Wiki Info

NOTE: This is just for the menbers of Group 3 and will be deleted before the deadline!

Wiki Deadline

The Wiki, code and peer review deadline is: Wednesday, 20 June, 23:59h.

Wiki Content

Make sure the Wiki of your group contains:

• An overview of the software architecture and approach used. How does it map to the paradigms explained in this course?
• A description of the methods that make your solution work so well, preferably using nice explanatory images (explain what makes your solution unique; what are you most proud of?)
• Which difficult problems were identified, and how were they solved?
• An evaluation of the hospital challenge: What went well? What went wrong? Why? How could it be improved?
• Videos / GIFs / animations / diagrams / pictures are very welcome! The simulator map for the Hospital Room challenge will be posted after the challenge (of course), so you can show that you are able to solve the challenge in simulation.

Publishing Code

A part of the grading criteria in this course is the quality of your code. We will not dive into your full code base. Instead, link to one or a few pieces of code or code files of which you are particularly proud, and which you think reflect the lessons taught: you can use the code snippet system as included in Gitlab. Do ensure we can access the code! Of course, feel free to also add some introductory/explanatory remarks on the Wiki, or as comments in the code. Make sure the links to your code snippets are clearly visible from your Wiki. For example, put them in a separate section called: Code.

Initial Design

The initial design is explained in the following parts:

• Requirements
• Functions
• Components
• Specifications
• Interfaces

Note that the report of the initial design is shared in Documents.

Requirements

• PICO should make no collisions with any walls
• PICO cannot stand still for 30 seconds or longer
• For the escape room challenge, the robot should leave the room within 5 minutes
• For the hospital challenge, within 5 minutes, mapping, reverse parking and obtaining the object should be done
• Its surroundings should be made visible in a map
• PICO should be able to reach every position in the room
• PICO should be able to leave every room
• PICO should make distinction between the hall and rooms
• It should run autonomously, so in each challenge there should be no additional input
• To start your software, only one executable has to be called
• PICO should be able to recognize an object and stand still close to it

Functions

• PICO should be able to drive in any direction
• PICO should be able to turn around
• It should be able to detect walls
• PICO has to know its own position within the mapped map
• PICO is able to construct a map
• It should be possible to create a trajectory to a room
• PICO should be able to recognize an object

Components

• Sensors:
  • Laser Range Finder (LRF)
  • Wheel encoders (odometry)
• Actuator:
  • Holonomic base (omni-wheels)
• Computer:
  • Intel i7
  • Ubuntu 16.04

Specifications

• Translation maximum: 0.5 m/s
• Rotation maximum: 1.2 rad/s
• Range of Laser Range Finder is assumed to be bigger than maximum room dimension
• The field of view of the Laser Range Finder is from -2 rad to 2 rad, divided in pieces of 0.004004
• The Laser Range Finder can only measure at one height

Interfaces

The interfaces of the initial design are shown here:

As shown in Figure 1, a world model is the link between all other parts. The tasks represent the highest level, meaning the most global work PICO has to do. The skills are needed to complete these tasks and some motions together form a skill. The perception contains the sensors, the continuous data. Below an overview of the different parts are given:

World model:

• Compose map
• Object recognition

Tasks:

• Mapping of the environment
• Backward parking at the specified location
• Searching for the object

Skills:

• Planning a trajectory
• Collision avoidance
• Backward parking
• Navigate
• Positioning in front of an opening
• Compose maps

Motion:

• Translation
• Rotation

Perception:

• Odometry
• Laser Range Finder
• Control effort

Escape Room Challenge

Since our code to map a room was not finished completely, a new code for the escape room challenge was made. This code was kept simple and effective. PICO should drive to a wall, turn to the left when it detects a wall within 0.4 meters from itself, and follow the wall (without hitting it of course). Then it will scan for a hallway to exit the room. When he sees a way to exit the room, PICO will go through the opening in the wall.

Our first attempt during the escape room challange was very successful! PICO drove out of the room, but we were convinced he could do it even faster. So we set the speed higher for the second try, and it was even faster. PICO drove out of the room within 29 seconds. This resulted in a second place and being one of the two teams that made it out of the room.

Here a shot video is shown of PICO leaving a room. Note that this is not filmed during the 'official' Escape Room Challenge. Due to our enthousiasm, we forgot to film, so we reconstructed the room during one of our test sessions.

Final Design

First off, the architecture is explained. This architecture is based on the theorem and takes the requirements of the hospital challenge into account. The architecture is explained in four steps:

• World model
• Localization and detection
• Motion control and trajectory planning
• Task manager

The overall architecture is visualized here:

World Model

To build a map of the hospital a two fold mapping procedure will be used. First PICO will map the room it is currently standing in. The parameters of this room are then stored. Secondly a graph connecting all mapped routes with each other is build. By using this two level mapping abstraction it is easier to locate PICO in a certain room. Also trajectory planning to a desired room can be more efficiently by using the graph.

The mapping of a room is done by scanning the environment using the laser range scanner. Through this data lines are fitted. By using these lines, a room is fitted, giving the dimensions of the room and the locations of the doors.

Line Extraction

The first step in mapping a room is to extract line segments from the obtained laser data. For this a pre-made laser line extraction package was used. This code was slightly modified to remove the ROS layer. the source code can be found on: https://github.com/kam3k/laser_line_extraction This package employs a split and merge algorithm[1]. The obtained laser data for the rooms was relatively clean, thus no filtering before the line extraction was applied. The parameters used in the algorithm needed some additional tuning to give the optimal result. In the figures below, the lines can be seen:

Angle Estimation

The odometry on PICO gives an estimate of the orientation of PICO. However due to the nature of odometry this estimate will drift over time. Therefore an absolute measurement of the angle is necessary to reset the odometry measurement. This is done using the lines extracted by PICO. These lines have a radius and an angle relative to PICO. Since the hospital is contructed of perpendicular lines PICO can determine it's angle relative to the walls. By using an initial guess, which will give the general direction PICO is oriented in, ie whether its angle is around [math]\displaystyle{ 0, \frac{1}{2}\pi, \pi, or -\frac{1}{2}\pi }[/math]. Then the angle is corrected y using the following equation [math]\displaystyle{ estimate +mod_{0.5 * \pi}(measuredAngle) }[/math] on the measurement.

Merge Lines

To map a room PICO needs to look 360 degrees around him, however the laser range finder only has a limited range. Therefore a PICO will scan lines, turn around 180 degrees and scan another time. It will then combine these two sets to end up with a set of lines which describe the entire room around PICO.

In the figures below, the two sets of lines can be seen:

Fit Room

Once PICO has assembled a set of lines all around him, he can determine the properties of the room he is in. This is done in two steps: First the dimensions of the room are found, then all the exits of the room are found.

Graph

Every room (square in figure) and door (circle in figure) will be numbered with an ID and then stored in the world model in the form of a graph. An visual representation of such graph can be seen below:

Using the grapgh, it can be easily seen what trajectory PICO should take to find the object. As the hint states, the object will be located in the room for which most openings (i.e., doors) have to be passed through to enter it. With the use of this hint and the graph, it can be quickly determined what sequence of room ID and door IDs is the longest. Knowing this makes finding the object easier, since PICO does not have to look in every room again, bit can immeadily go to the room with the longest sequence.

Localization and Detection

Once a room has been mapped. PICO will be able to compare the detected lines to the map it has made. Based on this, PICO's location can be determined.

Still to come...

Still to come...

Motion Control and Trajectory Planning

This part plans the trajectory PICO should take during the hospital challenge.

Approaching a Door

From the worldmodel PICO obtains his current position, and the position of the two corners of the door. A function is made which basically calculates a point right in the middle of the two corners, and puts a setpoint at a specified distance in front of the midpoint of the door and behind the midpoint of the door seen in the Figure below. Using the P-controller and the repulsion, this setpoint can be reached by PICO. PICO first changes the angle and drives forwards to the 'DES 1' setpoint, when this setpoint is reached the angle changes again and starts driving to the second setpoint 'DES 2'.

Repulsion

A function is implemented which keeps PICO from touching the walls. This is done using virtual repulsion forces. The repulsion force is calculated using the reciprocal of the laser scan data. The laser data is scanned over the entire range of the sensor, except for the first 10 points and the last 10 points because the returned laser data at these points was not trustworthy. For each sensor reading a repulsion force is calculated in the x and y direction. By adding up all the repulsion forces over the entire scan range a single force vector is obtained. By putting this 'force' combined with a gain directly into PICO's speed input, it will have the tendency to drive away from walls. A tuning parameter has been made in the form of a maximum distance of 0.35m in which PICO can approach a wall. All laser data points which are larger than this value are ignored. Since the laser data only covers 240° of view PICO always changes its angle to drive with its head forward to the destination coordinates. This way there is always available laser data to avoid obstacles.

P-controller

In order to have PICO move from point A to point B a simple P-controller has been implemented. For testing purposes the P-controller currently relies on odometry data. The 'error' is calculated by subtracting the current position from the target position. The error is multiplied with a proportional action. During testing these parameters will be tuned to ensure optimal behavior of PICO.

P-controller with Repulsion

The speed outputted by the P-controller and the speed generated by the repulsion function are simultaneously used in the PICO speed command. This means that, for example, when PICO is closing in on a corner, the speed (depending on the direction in which PICO is oriented) is temporarily reduced in the direction of the walls, ensuring that PICO gets enough clearance before returning on his path. A problem arises however when a setpoint is located directly behind a wall. In this case PICO cannot reach his goal, as the P-controller 'pulls' PICO towards the setpoint, while the repulsion 'pushes' PICO away from the walls. Therefore it is necessary to give PICO setpoints which he can roughly 'see'. The generation of these setpoints is handled in one of the previous sections. To make sure the maximum translation velocity in the x and y direction is not exceeded the norm of both speeds are calculated, when the velocity exceeds the maximum translation speed it is lowered.

Task Manager

Still to come: charts!

The task manager states what task should take place at what moment. The task manager contains three tasks:

• Exploration
• Parking
• Search and rescue

These three tasks will be highlighted in more detail now.

Exploration

Starting the hospital challenge, exploration is the first task to complete. Starting in the corridor, the detection of this area will be done. This corridor is saved in a struct, which contains a vector of exits. These exits have a destination. In the beginning, each of these destinations have value -1. The exploration is programmed in such a way that always the exit with a destination of -1 will be taken, so always an unexplored room will be entered. After entering a new room, again detection will scan the area. The destination of the door through which PICO has entered the room, will change from -1 to a value belonging to the previous area (corridor in this case). Then, if a room is entered and more than one exit is detected, again the door with unknown destination will be chosen. In the end, all unknown exits are explored and PICO is back in the corridor. At this moment PICO should move to its starting position and parking phase will begin.

Parking

When the map building is complete, PICO has to park backwards to the wall behind the starting position. Parking is done upon touching the wall. After PICO parked, PICO should say: "I am parked!".

This is done by letting PICO drive forward to the starting position and stopping in front of the wall. Then PICO will turn around such that he can drive backwards to the wall. Since PICO cannnot scan what is behind him, a diffrent method is used to see/feel the wall he should touch. This is done using the control effort. If the control effort becomes large enough, PICO will stop since this increase in control effort is caused by touching the wall. All in all, a short video will summarize the result far better, so let's watch the parking skills of PICO:

Note that this is a GIF, so you can't hear PICO say "I am parked!". After parking, the last phase of the challenge can begin. This phase will be explained in Search and Rescue.

Search and Rescue

At this moment, the hospital is explored and mapped. In the world model, a graph is saved. As discussed before, the hint states that the object will be located in the room for which most openings (i.e., doors) have to be passed through to enter it. With the use of this hint and the graph, the position of the object should be known and PICO can drive towards it. Standing still in front of this object will be the end of the challenge. In case there are multiple rooms for which the most doors have to be passed through, PICO should just try one and in case the object is not found, the other possibility should be tried. For now, the code calculates the longest path and stores this value. Because of the graph, PICO always knows which exit to take. After taking all these exits, the room with the object is entered. As explained, there can be multiple options. However, this is not fixed yet and will be one of the recommendations. After entering the room with the object, the object search function will be called. In this function, the LRF data is used. For each laserpoint, the differences between its distance and the distance of laserpoints on the left and on the right are measured. If that difference is high enough, there has to be an object in the room. This function places a point in front of the object and PICO will drive towards that point with the normal motion functions and will speak 'I found the object'. This search and rescue is shown in the GIF below.

Hospital Challenge

Still to come...

The Real Challenge

As seen above, PICO thinks the 'challenge' is not an actual challenge for him... To see what PICO can do, a real challenge was created. Without wasting any more words: the map and the result in the simulator can be seen below.

Still to come: GIF

Documents

All the documents made, besides the Wiki page itself, are shared here.

Report of Initial Design

Due to a misunderstanding, the report was not uploaded as a PDF yet. However, the full report was uploaded on this Wiki page (see below) before May 11th (17.00h), and now it is also uploaded as a PDF file (<1Mb). Our apologies for any inconvenience. The PDF of the report of the initial design can be found here: File:InitialDesign Group3 Report.pdf

Presentation of Initial Design

On May 16th, all groups presented their initial design. So we presented our initial design too and included two GIFs of our process so far. A PDF of the slides can be found here: File:InitialDesign Group3 Presentation.pdf

Presentation of Final Design

On June 6th, the final presentation was presented to the other groups. This presentation of the final design mainly focussed on the architecture. A PDF of the slides can be found here: File:FinalDesign Group3 Presentation.pdf

Code

Still to come...