Embedded Motion Control 2015 Group 7

About the group

Student information

	Name	Student id	E-mail
Student 1	Camiel Beckers	0766317	c.j.j.beckers@student.tue.nl
Student 2	Brandon Caasenbrood	0772479	b.j.caasenbrood@student.tue.nl
Student 3	Chaoyu Chen	0752396	c.chen.1@student.tue.nl
Student 4	Falco Creemers	0773413	f.m.g.creemers@student.tue.nl
Student 5	Diem van Dongen	0782044	d.v.dongen@student.tue.nl
Student 6	Mathijs van Dongen	0768644	m.c.p.v.dongen@student.tue.nl
Student 7	Bram van de Schoot	0739157	b.a.c.v.d.schoot@student.tue.nl
Student 8	Rik van der Struijk	0739222	r.j.m.v.d.struijk@student.tue.nl
Student 9	Ward Wiechert	0775592	w.j.a.c.wiechert@student.tue.nl

Updates Log

28-04: Updated the page with planning and initial design.

29-04: Updated page with initial design PDf (not yet "wikistyle")

07-05: Small changes in wiki page

09-05: Changes in design (incl. composition pattern, removed components&specifications)

11-05: Changes in design (incl. composition pattern), evaluating first experiment

13-05: Evaluating second experiment, corridor competition and discuss initial software

14-05: Evaluating corridor competition, update initial ideas wrt detection, movement etc.

Planning

Week 1: 20 April - 26 April

• Introduction
• Brainstorming initial design
• 27-04 12:00: Deadline initial design

Week 2: 27 April - 3 May

• Download and Install Ubuntu
• Start and Finish C/C++ tutorials
• Additional Brainstorming Design

Week 3: 4 May - 10 May

• Evaluating initial design
• Composition pattern
• Initial Presentation
• 6 May: First presentation design
• implement point-to-point movement
• 8 May: First experiment with Pico
• Implement edge detection, collision avoidance
• Initial Git structure

Week 4: 11 May - 17 May

• Implement edge detection
• Implement obstacle avoidance
• Finish initial software for Pico, with primary functions for corridor competition
• 12 May: Second experiment Pico, check software and robustness
• 13 May: Corridor Competition

Introduction

Robots Pico and Taco

For the course Embedded Motion Control the "A-MAZE-ING PICO" challenge is designed, providing the learning curve between hardware and software. The goal of the "A-MAZE-ING PICO" challenge is to design and implement a robotic software system, which should make Pico or Taco be capable of autonomously solving a maze in the robotics lab.

PICO will be provided with lowlevel software (ROS based), which provides stability and the primary functions such as communication and movement. Proper software must be designed to complete the said goal, the software design requires the following points to be taken in account: requirements, functions, components, specifications, and interfaces. A top-down structure can be applied to discuss the following initial points.

Design

This software design is discussed in the following subsections. These sections will be updated frequently.

Requirements

Requirements specify and restrict the possible ways to reach a certain goal. In order for the robot to autonomously navigate trough a maze, and being able to solve this maze the following requirements are determined with respect to the software of the robot. It is desired that the software meets the following requirements:

• Move from certain A to certain B
• No collisions
• Environment awareness: interpeting the surroundins
• Fully autonomous: not depending on human interaction
• Strategic approach

Functions

Functions and requirements

To satisfy the said requirements, one or multiple functions are designed and assigned to the requirements. describes which functions are considered necessary to meet a specific requirement.

The function ’Build world model’ describes the visualisation of the environment of the robot. ’Path memory’ corresponds with memorizing the travelled path of the robot. ’Sensor calibration’ can be applied to ensure similar settings of both robots, so that the software can be applied for both.

Interface

Interface

The interface describes the communication between the specified contexts and functions necessarry to perform the task. The contexts and functions are represented by blocks in the interface and correspond with the requirements, functions, components and specifications previously discussed.

The robot context describes the abilities the robots Pico and Taco have in common, which in this case is purely hardware. The environment context describes the means of the robot to visualize the surroundings, which is done via the required world model. The task context describes the control necessary for the robot to deal with certain situations (such as a corner, intersection or a dead end). The skill context contains the functions which are considered necessary to realise the task.

Note that the block ’decision making’ is considered as feedback control, it acts based on the situation recognized by the robot. However ’Strategic Algorithm’ corresponds with feedforward control, based on the algorithm certain future decisions for instance can be ruled out or given a higher priority.

To further elaborate on the interface, the following example can be considered in which the robot approaches a T-junction: The robot will navigate trough a corridor, in which information from both the lasers as the kinect can be applied to avoid collision with the walls. At the end of the corridor, the robot will approach a T-junction. With use of the data gathered from the lasers and the kinect, the robot will be able to recognize this T-junction. The strategic algorithm will determine the best decision in this situation based on the lowest cost. The decision will be made which controls the hardware of the robot to perform the correct action. The T-junction is considered a ’decision location’, which will be saved in the case the robot is not capable of solving the maze with the initial decision at this location. During the movement of the robot, the walls are mapped to visualise the surroundings.

Composition Pattern

Composition Pattern

The structural model, also known as the composition pattern, decouples the behavior model functionalities into the 5C’s. The total structure is divided into three subsystems which will be discussed indivually: movement, detection and strategy. Note that black arrows are data communication and dotted arrows are events.

The movement subsystem is used to control movement of the robot and consists of three coordinators. Collision avoidance is only triggered if the robot detects close surroundings. Point-to-point movement is only active for a desired path. And handling situations is only received from navigation. The composer determines which entities (x-movement, y-movement or rotation ) are needed for the required motion. The entities control the movement for the robot. The monitor checks if the desired position and orientation is achieved. Then the monitor reports back to the coordinator.

The detection subsystem is used to process the data of the laser sensors. It's main function is to observe the environment and send relevant information to other subsystems. Therefore if sensory data is received the coordinators handle. The composer triggers all actions. Entity “measurement processing” filters the noise and converts the data to coordinates. Then the data is send to Update world model, situation recognition and close surrounding detection. The monitor will check the variance of the noise. If the variances changes the noise filter will adjust to that.

The strategy subsystem is used for path planning. This subsystem receives information from the detection subsystem. An optimization algorithm is used to determine the optimal strategy for exploring the maze. A navigation algorithm is used to determine which coordinate the robot through junctions. And path memory saves all locations.

Initial software, testing and Corridor competition

The first steps of actually coding the software is based on the requirements necessary for the corridor competition. In order to pass the corridor assignment succesfully, PICO should be capable of:

• Move from point A to point B
• Avoid collisions with walls/obstacles
• Detect edge corners in corridor
• Taking corners (with relative coordinates of detected edge)
• Detect end of corridor challenge

The group is divided into a "detection" group, "movement" group and a "world map" group (usefull in a later stage of the project). The primary ideas and implementations will be discussed in a similar structure.

Movement

The "movement" part of the software uses data gathered by "detection" to send travel commands to PICO. In the near future, a new "strategy" part of the software will be designed, which basically becomes the "decision maker" which commands the "movement" of PICO."

Local coordinates

The main idea is to work with local coordinates. At time t=0, coordinate A is considered the position of PICO in x, y and theta. With use of the coordinates of edges received by "detection" point B can be placed, which decribes the position PICO should travel towards. This point B will then become the new initial coordinate, as the routine is reset. In case of a junction as seen in the figure multiple points can be placed, allowing PICO to choose one of these points to travel to.

The initial plan is to first allow PICO to rotate, followed by the translation to the desired coordinates. This idea was scrapped, because this sequence would be time consuming. Instead a non-linear controller was used, that asymptotically stabilizes the robot towards the desired coordinates.

Although the PICO has an omni-wheel drive platform, it is considered that the robot has an uni-cylce like platform whose control signals are the linear and angular velocity ([math]\displaystyle{ u }[/math] and [math]\displaystyle{ \omega }[/math]). The mathematical model in polar coordinates describing the navigation of PICO in free space is as follow:

[math]\displaystyle{ \dot{r}=-u \, \text{cos}(\theta)\! }[/math]

[math]\displaystyle{ \dot{\theta}=-\omega + u\, \frac{\text{sin}(\theta)}{r},\! }[/math]

where are [math]\displaystyle{ r }[/math] is the distance between the robot and the goal, and [math]\displaystyle{ \theta }[/math] is the orientation error. Thus, the objective of the control is to make the variables [math]\displaystyle{ r }[/math] and [math]\displaystyle{ \theta }[/math] to asymptotically go to zeros. For checking such condition, one can consider the following Lyapunov function candidate: [math]\displaystyle{ V(r,\theta) = 0.5 \, r^2 + 0.5 \, \theta^2 }[/math]. The time derivative of [math]\displaystyle{ V(r,\theta) }[/math] should be negative definite to guarantee asymptotic stability; therefore the state feedback for the variables [math]\displaystyle{ u }[/math] and [math]\displaystyle{ \omega }[/math] are defined as,

[math]\displaystyle{ u = u_{max}\, \text{tanh}(r)\, \text{cos}(\theta) \! }[/math]

[math]\displaystyle{ \omega = k_{\omega} \, \theta + u_{max} \frac{\text{tanh}(r)}{r} \, \text{sin}( \theta) \, \text{cos}( \theta),\! }[/math]

where [math]\displaystyle{ u_{max} }[/math] and [math]\displaystyle{ k_{\omega} }[/math] both are postive constants.

Potential field

If PICO would be located at position A and would directly travel towards point C, this will result in a collision with the walls. To avoid collision with the walls and other objects, a potential field around the robot will be created using the laser. This can be interpreted as a repulsive force [math]\displaystyle{ \vec{U} }[/math] given by:

[math]\displaystyle{ \vec{U}(d_l) = \begin{cases} -K_0 \left(\frac{1}{d_{pt}}- \frac{1}{d_l} \right) \frac{1}{{d_l}^2} & \text{if } d_l \lt d_{pt}, \\ ~~\, 0 & \text{otherwise}, \end{cases} }[/math]

where [math]\displaystyle{ K_0 }[/math] is the virtual stiffness, [math]\displaystyle{ d_{pt} }[/math] the range of the potential field, and [math]\displaystyle{ d_l }[/math] the distance picked up from the laser. Summing these repulsive force [math]\displaystyle{ \vec{U} }[/math] results into a netto force,

[math]\displaystyle{ \vec{F}_n = \sum_{i=1}^N \vec{U}(d_{l,i}), }[/math]

where [math]\displaystyle{ N }[/math] is the number of laser data. Assume that the PICO has an initial velocity [math]\displaystyle{ \vec{v_0} }[/math]. Once a force is introduced, this velocity vector is influenced. This influence is modeled as an impuls reaction; therefore the new velocity vector [math]\displaystyle{ \vec{v}_r }[/math] becomes,

[math]\displaystyle{ \vec{v}_r = \vec{v_0} + \frac{\vec{F}_n}{m_0}, }[/math]

where [math]\displaystyle{ m_0 }[/math] is a virtual mass of PICO. The closer PICO moves to an object, the greater the repulsive force becomes

Detection

The initial idea was to create a database containing all possible problems PICO could encounter whole travelling trough the maze (i.e. dead end, intersection etc.). However, this was quickly discared as it would result in a databse of infinite size.

A similar idea is applied, however not in terms of a database but in terms of conditions. The software will be implemented with certain conditions which are checked at every (time)step. At first these conditions will be based on edge detection, and additional conditions will be implemented to recognize other situations. For the corridor comptetion, only the edge detection was designed and implemented.

To be updated later on: gap detection, line detection (walls).

Edge detection

File:Sketch edgedetec.png

Sketch of edge detection

An edge can be considered as a discontinuity of the walls perpendicular to the travel direction of PICO. This can be either an door (open/closed), a corner or a slit in the wall. At this stage only the detection of a corner will be implemented.

.

The edge is then defined as the position of the corner (relative to PICO). At every (time)step the relative distances (wall to pico)will be checked, based on LRF data. If at some point the distance between one increment of the LRF and the neighbouring increment exceeds a certain treshold, it can be considered as an edge point. This is further illustrated by the image. This primary idea is however not robust, therefore the average of neigbouring increments is checked. Robustness will be further improved later on.

SLAM

To be continued

Evaluating first experiment 8 May

The goal of the first experiment on 8 May was to get used with PICO and handling the communication between user and PICO (with use of GIT). Data is gathered for future use and the point-to-point movement is tested. Due to small errors, other software subsystems were not tested. During this experiment the following items were discussed/observed:

• Significant slits present in the walls are observed by PICO.
• Gradient present in the velocity, note the stick-slip like behaviour.
• Largers tolerances necessary in the code.
• The movement as prescribed by the code differs from the actual movement of PICO, monitoring will be important.
• Data is gathered during 6 different experiments.
• Armadillo is capable of detecting deadends

These items provided extra insight, the code will be changed to better suit the robot in the real case.

Evaluating second experiment 12 May

The goal of the second experiment was to check if all implemented functions act such as expected (in comparison with the simulator). During the experiment, PICO was not yet capable of solving the maze autonomously in every situation. The following observations are discussed:

• The coordinates of first edge of the corner is not recognized w.r.t. other detected edges (PICO must make the corner).
• Collision avoidance is working as expected.
• Every software piece indivually acts as expected, however the complete implementation need extra attention.
• There are some doubts with the implementation of a nonlinear controller

Evaluating Corridor Competition

Between the second experiment on 12 May and the corridor competition several changes were made to the existing code. The "detection" part of the software was improved and made slightly more robust by evaluating more laser beams wrt older versions. The communication between "detecion" and "movement" was further improved by implementing dynamic arrays (also known as Vector in C++).

The corridor competition was a succes, PICO managed to find the corner and to take this corner. With a winning time of 16 seconds we can call it a succes. However during the competition some minor things were observed which needs further attention:

• The nonlinear controller works fine, but could some fine tuning wrt slow velocity near the corner
• Cornering is happening at quite a slow pace.
• The "path planning" (planning the path to take the corner) was not as desired, as the collision avoidance was triggered.
• The second mainfile, consiting a "normal" controller was not considered a "better" option out of the two.