Embedded Motion Control 2016 Group 5

Group Members

0976517	Rohan Lakhotia	r dot lakhotia at student dot tue dot nl
0976553	Parth Sharma	p dot sharma at student dot tue dot nl
0977462	Xixiao Wang	y dot wang dot 7 at student dot tue dot nl
0977242	Satyaki Chaudhuri	s.chaudhuri@student.tue.nl

Basic Design Idea

Requirements

For this assignment, we are expected to develop and implement a software code so that Pico can autonomously solve a maze. The assignment consists of the following challenges:

1. Corridor Challenge
   1. Pico must drive through a corridor and take the first exit.
   2. It must not touch the wall.
   3. Must detect the exit on its own.
   4. We have total 5 minutes to complete the challenge.

1. Maze Challenge
   1. Pico must be able to solve the maze and find the exit point which is not known.
   2. It should detect the door and send the request command for the door to open.
   3. The door opens when the robot is stationary.
   4. We have total 7 minutes to complete the challenge.

Functionality

Based on the requirements mentioned above, certain functionalities of the robot at task were detected. Based on levels of complexity these can be divided into 3 main groups:

1. Highest Abstraction Level- Tasks

These tasks directly lead to the realization of the goal, they are considerably complex and use various combination of skills for their accomplishment.

1. 1. Maze Solving Algorithm

This is the highest level of logic, decision algorithms which enable the autonomous system to negotiate the maze in the shortest time possible. Critical decisions like recognizing dead-ends and intersections and the optimum turning direction fall in this category. This involved various skill selection at different stages of the maze.

1. 1. Track Position of Robot: The robot should be able to track the time variance of its

position with respect to a Global Coordinate System (planer). This will be beneficial in avoiding movement in closed loop trajectories.

Skills are localized decision making event which enable realization of Tasks, they are of a lower complexity level and often involve computation in a specific situation.

1. 1. Avoid Collision with walls: The robot is required to maintain a safe distance from the walls at all points of time. This skill will typically be incorporated at all points of the maze and corridor challenge.
   2. Recognizing Intersections: The robot should be able to detect intersection of walls. In case of a simple turn in the maze, the robot will be able to decide the correct turning direction. A three way intersection will have to be judged based on the position trajectory of the robot.
   3. Recognizing Dead-Ends: The robot identifies a dead-end and beeps for a door to be opened. It has to wait a certain amount of time and monitor the walls for them to open.
   4. Detect End of the Maze: The robot should be able to detect the end of the maze once it appears and complete it eventually.

1. Lowest Abstraction Level Functions:

These are the very basic functions of the robot which enable the realization of the skills and eventually the tasks. These are the fundamental abilities of the robot which do not involve development of any algorithms on our part.

1. 1. Mobility: The robot is equipped with holonomic wheels which enable it to translate and rotate on a planar field. Due to the nature of the challenge, these actuators are sufficient for the robot to complete the maze.
   2. Sensors: The robot is equipped with Laser Range Finder and Wide Angle Camera to perceive its environment and be able to relay this information to higher levels of functional abstractions.

Components and Specifications

1. Hardware components:
   1. Laser Range Finder: A laser range finder uses a laser beam to determine the distance to an object. Here, the laser range finder will be used to calculate the distance between Pico and the walls.
   2. Wheel encoders/Rotary encoder/Shaft encoder: It is an electro-mechanical device that converts the angular position or motion of a shaft or axle to an analog or digital code. Here, it can be used to get information of robot such as speed, distance and position.
   3. Wide Angle Camera: With good image processing algorithm, it can be used to localize the position of the robot with respect to the surrounding environment, also to differentiate between walls and door and to interpret different signs (if there is a visual difference between the two).
   4. Computer with intel i7 processor
   5. Actuators
      1. Wheels & Motors: Omni wheels. The effect is that the wheel can be driven with full force, but will also slide laterally with great ease.
      2. Pan/ Tilt Head: The head can be moved to look around for obstacles.
2. Software Components
   1. Ubuntu 14.04
   2. Qt Creator

Interface

The control of robots can be done by calling the functions in the emc library, which will communicate with ROS topics for data exchange. By running C++ programs, data can be obtained, processed and transmitted from and to certain topics.

1. The libraries are: < emc=rate:h > < emc=io:h >
2. The sensor data can be obtained by: io.readLaserData(), io.readOdometryData().
3. The structures of data are saved in: emc::LaserData, emc::OdometryData.
4. For driving the robot: io.sendBaseReference & io.sendBaseVelocity.

The figure below shows the interface:

Design Document Download

The design document can be downloaded here

First Design Presentation

Functionality

We realised that a detailed inspection and understanding of the robot functionalities was essential, this would enable us to design the software structure. On the most generic level, functionality was divided into Functions, Skills and Tasks as described in our Design Idea. Skills consisted of a tasks which were of wide complexity, we have further divided that into three different segments.

1. Basic Complexity: This consists Processing Laser Range-finder and Odometer data. This skill segment enables PICO to understand the environment and also provide data for high levels of skill-sets.
2. Medium Complexity: This skill-set performs the safety function. It consists of two actions. The first one is to align and center the robot at all points of its travel in the corridor. The second one is to maintain a certain amount of distance from walls at all points of time, it avoids collision.
3. Higher Complexity: This skill-set uses the environment data to detect intersections and their nature. In the corridor challenge, it will consist of software which will detect the exit and respond accordingly. While in the maze challenge, the software will have to map the robot position in the maze, in order to avoid travelling in loops.It will also enable PICO to find the exit to the maze.

Composition Pattern

The software aims to evaluate the robot's surrounding and accordingly provide motion instruction and monitor the execution. In order to understand and implement this software, we have used a structured system architecture. The system is divided in to structure, behaviour and action. Behaviour is the system reaction to the environment, action is the implementation of what the algorithm instructs the robot to perform. Structure is the realisation between behaviour and action. The task-skill-motion diagram helps us explain the behaviour of the system.

1. Task Context : The decision making process works in a loop. Starting with the "Action to be Performed", the system decides on a certain immediate goal. A skill-set is decided upon to realise the goal. "Task Monitoring" & "Task Feedback" process data from the Environment and Skills Context to evaluate the execution of the decision and also the current position of the robot.
2. Skills Context : Contains all skill sets as mentioned in the Functionality Diagram. The skills are performed in increasing order of complexity.
3. Robot Context : It consists the hardware capabilities of the robot. The Laser Range-Finder and the Odometer relay environment data to the Task Context while the motion instruction data is provide to the Holonomic wheels to actuate the robot.
4. Environment Context : This consists of two blocks. The Local Environment pertains to the immediate surrounding of the robot, this information is used to avoid collision with the walls, center and align the robot and also detect intersections and their nature. The Global Environment data is used to generate an understanding of the maze structure, avoid travelling in loops and eventually find the exit of the maze.

The design presentation can be downloaded here

Corridor Challenge

The corridor challenge requires the robot to travel through a corridor without any wall collision. It needs to detect an exit either on the right or the left and perform the exit operation. The team decided to break down the various tasks in to simple function blocks. The environment is being studied in the main while loop, based on the conditions the relevant function blocks would be activated. The LRF Data was used to inspect the surroundings in order to understand local position and orientation. Odometer data was used only to track small changes in the displacement of the robot. We understood that due to wheel-slip, odometer data would be unreliable for larger motion. In the corridor challenge, the map was divided into two parts. One was the corridor where the robot was expected to travel straight with out any collision. The other part included the junction of the corridor an the exit.

 header files 
 .
 .
 double Safety()          // Collision Avoidance - Centring and Alignment. 
 double Turn()            // Take a right or a left turn based on the Laser Scanner Readings. 
 double GoStraight()      // Continue moving straight. 
 .
 .
 int main(){
 .
 .
 while (condition 1);     // Call specific functions based on the environment conditions.
 .
 while (condition 2);
 .
 .
 .
 sendBaseReference(x,y,θ)
 .}




Some of the relevant function blocks and working principles have been discussed below.

1. Safety: The safety function included software to center the robot, align it along the wall and also avoid collision with the walls. It was active during the corridor part of the maze. Two laser data bundles were averaged to obtain the values (Average Value Right)AVR and (Average Value Left)AVL as illustrated in the diagram below. The difference between these values were considered to be the centering error, based on which the robot was provided with an appropriate lateral velocity. For the align task, we inspect the bundle of data from 0 degs (front of PICO) to the last laser data on the right hand side. The alignment correction angle is equal to the difference between the shortest distance data and the data to the right ( no. 107 ). based on the angle of the shortest distance, the decision to turn either left or right is used.
2. Turn: Average Value Right 1(AVR1) and Average Value Left 1(AVL1) are smaller laser data bundles as depicted in the image below. A sharp rise in any of these values indicate the presence of an exit. Once an exit is detected, the safety function is turned off using a flag. The robot is made to move forward by a certain distance which is measured using the odometer data, turn left or right by 90 degs followed by a measured movement in the straight direction. Once the robot has exited the junction of the exit, the safety function is turned on till the robot make the final exit form the corridor.

Average Value Laser Bundle

Alignment Task

Average value Laser Bundle 1

The images below depict the detection of the corridor exit and the exit methodology used in the corridor challenge.

Exit Detection

Exit Action at the junction

Final Design

Software Structure

The Final Design included software to negotiate the maze for the Amazing challenge. The team identified the critical functions and decided to accordingly segment our software. The "Detection" block is responsible for processing and judging the nature of the environment. It passes on local environment data to the Decision Making Block. The "Decision Making" block is responsible for instructing PICO about its movement at intersections. The Decision Making block passes on its judgement to the "Movement Block" which uses virtual potential field function to negotiate PICO through the maze while avoiding collision with walls.

DETECTION

Detection Function data flow

The Detection Function Block is responsible for processing the robot local environment data and relaying this information to the subsequent Decision Making Block and Movement Block. We use three primary laser data bundles to understand PICO's surroundings. Average Value Left 1 (AVL1), Average Value Right 1 (AVR1) and Average Value Straight 1 (AVS1). Based on the values of these laser data bundles, the environment is analysed. A separate flag is raised in each of the scenarios which is eventually passed on to the Decision Making block. There are five different scenarios which are detected namely :

• Left Turn : When the data bundle AVL1 goes beyond a certain value, a left turn is detected.

Detection of a left turn

• Right Turn: When the data bundle AVR1 goes beyond a certain value, a right turn is detected.

Detection of a right turn

• T-Junction: When both the bundles AVR1 & AVL1 go beyond a certain value but AVS1 is limited, a T-junction is detected.

Detection of a T-Junction

• Four-way Junction: In the case where all the three values AVL1, AVR1 and AVS1 go beyond a certain value, a four way intersection is detected.

Detection of a four-way junction

• Dead-end: In the case where all the three values AVL1, AVR1 and AVS1 are limited to a certain value, a dead-end is detected.

Detection of a dead-end

MOVEMENT

The team decided to use a simple but effective implementation of the Artificial Potential Field method for obstacle avoidance. The movement of the robot was realised through one of the 3 primary blocks, the Movement Block.The Movement Block satisfied two primary functions, to avoid collision with walls and obstacles and to execute turns based on data from the Decision Making Block. There are 2 fundamental forces which decide the movement of the robot. The Attractive Force is calculated based on a fixed set point in-front of PICO. The Repulsive Force is calculated based on the laser data PICO generates.Repulsive Force is calculated along the direction of each of the laser range data, dependant on the distance of the obstacle or the wall from the robot. Each wall point or obstacle was given a repulsive force which was inversely proportional to the distance to the bot and along the direction of the Laser Range Data. An attractive force set point was put at a constant small distance in-front of PICO. The resultant force was calculated by summing all forces along the longitudinal and lateral direction. The resultant forces was then further extracted into horizontal and vertical directions in order to extract velocity for each direction.

The Movement Function has the following inputs:

1. x and y coordinates of PICO. Which are by default (0,0) as we are considering it in the PICO Local Coordinate System.
2. x and y coordinates of the set point . It is a constant distance in front of PICO. (K,0)
3. Direction Flag which can be a number between 1 to 4. 1-Left Turn, 2-Right Turn, 3-Straight Motion, 4-None.

The Outputs are : Transnational Velocity in the Longitudinal and Lateral Direction and Rotational velocity.

Collision Avoidance using Artificial Potential Field

• Collision Avoidance - The effective forces in the longitudinal and lateral directions are computed based on the Attractive and Repulsive Forces.

1. Attractive Force is a constant Force pushing PICO to move forward.

// attractive force

 AttF_x = 8* (Xg - Xr);          //in x direction ATTRACTIVE FORCE
 AttF_y = 0* (Yg - Yr);          //in y direction ATTRACTIVE FORCE 

Here Xg and Yg refer to the set-point coordinates while Xr and Yr refer to PICO coordinates. We want to make it clear that these values were constant at all points of time, it was implemented to provide PICO will a forward motion while the Repulsive Forces controlled the Lateral Motion.

1. Repulsive Force depends on the Laser data. It is the summation of the Repulsive Forces due to each of the Laser Data. Force is the inverse of the cube of the distance to the obstacle d[i] and a Scaling Factor Sf. Using the cube of the distance ensures strong repulsive forces when PICO is very close to the walls, this way the collision avoidance algorithm is more robust.

Repulsive Force