Embedded Motion Control 2017 Group 9

Group Members

Name:	Student id:
Mian Wei	1035075
Zhihao Wu	1041226
Petrus Teguh Handoko	1033085
Bo Deng	X
Bo Cong	0976759
Jian Wen Kok	0808353
Nico Huebel	Tutor

Initial Design

The initial design for the maze challenge is elaborated below. It includes the requirements, functions, components, schematic of program structure, specifications and interfaces to define the working of PICO. The file for the initial design is included here: File:Assignment-for-week1.pdf

Requirements

➢ PICO drives autonomously through maze
➢ Being able to take a turn without touching a wall
➢ Being able to detect a turn or branching corridors
➢ Avoiding collisions with obstacles (including the walls)
➢ Driving straight and rotating smoothly
➢ PICO should not stand still for 30 seconds
➢ Avoid getting trapped in a loop of the maze
➢ Being able to recognize the door

Functions

Below is the scheme for the functions of PICO. The basic skills enables the advanced skills and these skills are incorporated in the main function to finish the maze.

Components

drive control
‐Holonomic base (omni‐wheels)
‐Pan‐tilt unit for head

detection
‐170◦ wide‐angle camer (Unavailable in this project)
‐Asus Xtion Depth sensor (Unavailable in this project)
‐Laser Range Finder (LRF)
‐Wheel encoders (odometry)
world model
computer
‐Intel I7
‐Ubuntu 14.04

Specifications

- maximum translational speed of 0.5 m/s ‐ maximum rotational speed of 1.2 rad/s ‐ Door template: length of 0.5 ‐ 1.5m and with side walls of approximately 30cm, see figure below ‐ LRF accuracy and range unknown ‐ odometer accuracy unknown (Not that accurate)

Door Template

Interfaces

The odometer and LRF generates data for mapping the environment.

The algorithm sets nodes on the junction as a setpoint for navigation, plans the route and put the actuators to work accordingly.

The odometer and LRF keeps on keeping track of the environment and the software recognizes obstructions, dead ends that might be doors and junction.

Corridor Challenge

Design

For corridor challenge, PICO is designed to behave as follows:
-. First, PICO moves forward with a modified potential field.
-. When it detects the junction, the potential field goes off and stops when it is in the middle of the junction.
-. PICO then rotates 90 degree and moves forward and finishes the challenge.

modified potential field

Laser beams A1 and A2 are used to check the relative distance between left side and right side of the robot. Based on this relative distance, the robot will be directed to the middle of the corridor by giving both a sidewards and rotational velocity.

potential field

junction detection

Laser beam B1 and B2 are used to check whether a junction exists or not. When a junction is detected, the potential field goes off and waits for A1 and A2 to detect the junction to make sure that PICO will be able to make the turn when it is turned 90 degrees.
junction detection

Result

PICO was unable to complete the corridor challenge.

In the first trial PICO moved straight forward without potential field. When it detects the junction it stopped and rotated 90 degrees in the wrong direction. This inevitably resulted in crashing into the walls.

In the second trial PICO did not detect the junction and drove straight forward, this was the latest program.

Evaluation

In the first trial we used our old program that has proven to be successful as seen in the video. Unfortunately we did not push the correct version to PICO. In the second trial, we used our latest program. Potential field is added because there exists a chance that PICO would run into the walls without it. The new program works in the simulation, it has been tested in the real setting and we had problems with the odometer to correctly make the turning. The program had to be run without checking it first with the real environment setting. Later on, a bug was found in the junction detection. The potential field can be added afterwards to prevent PICO from colliding into the wall. Instead of the static turn that is implemented now, a smooth turn into the junction is suggested to improve the time.

Architecture of The Code

Architecture of The Code

Maze Challenge

Design

Architecture

Architecture of The Code

Main Flow

---

The code programmed for maze uses several flags, like possible_door_flag and junction flag, to decide which block should run. The specific flags will be set when the ‘Detection & Decision Block’ has detected the corresponding cases, and be reset when the movement corresponding to a particular case is completed. These flags are the basis of logic of the whole program. For example, the possible_door_flag will be set when ‘Detection & Decision Block’ finds a door, and this flag will be reset when ‘Door Movement Block’ has finished. From Figure1, it is clear that ‘Initialization block’ will be executed at the beginning when the robot is turned on. After that, the main loop will be executed with 20HZ frequency. The main loop consists of four blocks: ‘Detection & Decision Block’, ‘Door Movement Block’, ‘Junction Movement Block’ and ‘Normal Movement Block’. The first block to be executed in main loop is ‘Detection & Decision Block’. This block takes charge of junction detection, door detection, open space detection and making movement decision. First, the robot will detect the existence of the door and returns ‘possible_door_flag’, which indicates the existence and the direction of the door with respect to the robot. When ‘possible_door_flag’ is nonzero, robot will skip ‘junction detection’ sub-block and directly execute ‘Door Movement Block’ because the information of junction existence is no more important if the door of existence is found already. Otherwise, ‘Detection & Decision Block’ will execute sub-block for junction checking. Second, the robot will detect the existance of junction. The junction will be described by ‘junction_flag’ and ‘junction direction flag’. The ‘junction_flag’ indicates the existence of the junction around the robot, and it will be set by ‘Junction Detection’ sub-block. The ‘junction direction flag’ indicates the type of junction by three elements, which represents left, forward and right respectively. If ‘junction_flag’ is nonzero, which indicate the existence of junction, ‘Decision’ sub-block will be executed to decide which direction the robot should move. If not, ‘Decision’ sub-block will be blocked and the robot will take default movement mold——’Normal Move Forward’. The ‘Open Space Detection’ is inside the ‘Junction Detection’ sub-block. It will be introduced later. It is noted that when the robot is executing actions, like rotate, the position and orientation of the robot w.r.t. the maze is changing, data collected by laser scanner is not reliable for making decision. In order to avoid this kind of error, we want to block the ‘Detection and Decision Block’ until the last issued command is implemented completely and ‘junction_flag’ , ‘possible_door_flag’ are reset. As a result, it is necessary to stipulate that ‘Detection & Decision Block’ can only be executed when both ‘junction_flag’ and ‘possible_door_flag’ are zero. For example, the ‘Detection & Decision Block’ will not be executed anymore without any other blocks to help it to reset the flag in the last time that junction or door was detected. When the execution of first block complete, “Junction Movement Block”, “Door Movement Block” and “Normal Move Forward Block” will executed according to the movement command issued by “Detection & Decision Block”. “Normal Move Forward Block” controls default forward movement. When neither junction nor door is detected, the movement of the robot is controlled by this block to move with potential field protection. But it will be substituted by “Junction Movement Block” or “Door Movement Block” when special case detected.

Structure of Main Loop

Detection

---

Junction Detection

---

In the Figure "Laser Bundle For Junction Detection", it shows clearly that three laser bundles in forward direction, left direction and right direction are selected to detect the junction. To make performance of detection block more reliable and robust, the detection radius and detection angle need to be well calculated. The detection angle is set to be 24° since the maximum detection range is ±114°.

As a result, the boundaries of three laser bundles are:

• Left bundle: +114° and +66°
• Forward bundle: +24° and −24°
• Right bundle: −114° and −66°

The width of the corridor is between 0.5m to 1.5m. In order to detect the widthest corridor, the following equation need to be satisfied:

Also the width of a junction should be bigger than PICO's width, so:

These two equations will make sure the pico can find junction in the widthest corridor and also could go through the junction in open loop. Finally, the detection radius is set to be 0.85.

Laser Bundle For Junction Detection

To find the junction, both boundaries in one laser bundle needs to have longer distance than the detection radius. After that, to increase the robustness of the detection, all the distance of the laser line inside the laser bundle will be measured, if 80% laser line has longer distance than the detection radius, PICO will consider there is a junction.

Two flag are used to describe the exist and feature of junctions: the junction flag and the direction flag.

Junction flag:

• Junction flag is used to describe the exist of junctions
  • 0: no junction or only forward junction
  • 1: existence of left or right junction

Direction flag:

• Direction flag D[3] is an array, and each number represent a direction
  • D[0]=true: right direction available
  • D[1]=true: forward direction available
  • D[2]=true: left direction available

If junction flag becomes 1, the decision block will be triggered and decide a direction to move based on the direction flag.

Door Detection

---

There are two kinds of door, front door and side door.

• Front door detection method

Door detection is made by checking the distance of three ranges. The range is defined by angle θ=48°, and detection radius φ_f.
front door is considered as a subcase of dead end. Hence front door and dead end are detected in the same way. If the average distance in the three detect ranges are smaller than φ_f, which indicates the discovery of dead end, possible_door_flag=2 will be returned to show the direction of door with respect to the robot

• Side door detection method

To make detection simpler and reduce workload, “Side Door Detection” block is built as an extension of side corridor detection by using the left and right side detect ranges. As illustrated in Figure “’’’Door Template’’’” in ‘’’2.4 Specifications’’’, the depth of door template is around 0.3m and the corridor width is between 0.5m and 1.5m. we use an annulus range with limitation φ_A=0.7m and φ_B=1.3m to define the possible door condition (the red shadow in Figure). To avoid the robot trap by the side door detection loop, we consider front data about 45° should smaller than φ_C=1m (the yellow line in Figure “’’’Door Detection”(right)).
For example, considering the right door in the “Door Detetcion”, first check whether the values of two edge laser data.212 and data.2(±24°) are between φ_A and φ_B. And then check whether the value of - 45°(laser data.303) and +45°(laser data.696) smaller than a specified constant φ_C. If the both conditions are satisfied, the percentage of datas whose value is between between φ_A and φ_B will be calculated among the data set from -114° to 66°. if 75% data satisfied the length condition (in the red shadow area), which means there is a right side door, possible_door_flag=1 (for the left door possible_door_flag=3 ) will be returned.

Door Open Detection

---

We use the similar way to detect if door is opened after 5 second. The forwad direction laser bundle has been used. If both boundaries have longer distance and more than 75% laser line inside the laser bundle is longer than detection radius, PICO will consider door has opened.

Open Space Detection

---

"Open Space Detection" block is an extension of "Junction Detection" block. By including another two laser beams ranges around -45°and 45°respectively (red ranges in the figure)，the feature of open-space can be distinguished from that of normal junctions. the differences will be introduced below in three different cases in detail:

Openspace flag:

• Openspace flag is used to describe the exist of openspace
  • 0: no openspace
  • 1: existence of openspace

• Case1:
  

Open space appears on the both side of the robot as showed in Figure "openspace-Detection-case1". The feature of this case is likely to confuse the robot with cross junction in Figure "openspave-case1"(right). With laser beam at -45° and 45° added, we can distinguish this case with cross junction because in open space, laser beam at -45° and 45° cannot detect any obstacles within its detecting radius. While if it is cross junction, some obstacle will be detected.

openspace_Detection_case1

• Case2:
  

open space only happens on the right side of the robot as showed in Figure "openspace-Detection-case2". The feature of this case is similar to that of right T junction showed in "openspave-case2"(right). In same way, laser beam at -45° helps to distinguish them.

openspace_Detection_case2

• Case3:
  

Open space only happens on the left side of the robot as showed in Figure "openspace-Detection-case3". The feature of this case is similar to that of left T junction showed in "openspave-case2"(right). Laser beam at 45° helps to distinguish them.

openspace_Detection_case3

The code introducing the information of front left/right laser beam into environment detection is positioned at the end of the "junction_detection.cpp", after the detection of junction is completed. The value of junction_flag and int array junction_direction which indicate the direction of the available corridors at the junction has been assigned. Then, checks on the result of LRF at -45° and 45° follows. If there is no obstacle found in one of these two directions, junction_flag will be reset and the information of left junction detection will be cleaned. i.e. the value of junction_direction[2] will be assigned zero. Variable openspace_flag will be set to indicate the discovery of open space, no matter it is left or right one.

And then, it comes the decision block. Now, the value of junction_flag is zero, Decision block will not execute "Corridor Decision Block" because the condition for this block doesn't hold as showed in figure '"Flow Chart of Decision Block". But the condition for the first case in "Open Space Decision Block" hold in this case and decision in this case is specified as 2(indicator of turning left movement) in this round and another variable "openspace_decision_flag" is set at the end of the this sub-block. When "turning left" movement is completed, same detection procedure is repetitively performed. Because the discovery of right open space, the indicator of the discovery of junctions keep being cleaned and indicator for left junction stays at 0 at the end of "Junction_detection Block", openspace_flag is set at the mean time as stated above. the second time when "Decision Block" is executed, the condition of the first case in "Open Space Decision Block" does't hold anymore beacause the value of "openspace_decision_flag" has been set to 1 and no block help to reset it up to now. It is until "Openspce_case3" that "Decision Block" executes sub-block3 and reset the value of "openspace_decision_flag". That is how open-space algorithms works.

The performance of the algorithm is show in video "Open space performance in simulato"

Open space performance in simulator

Decision Block

---

Decision Block is consisted of two sub-blocks. One of the sub-blocks is “Corridor Decision Block” and the other block is “Open Space Decision Block”. These two block account for decision in corridor and open-space respectively. The switch between two decision sub-block is based on the value of “openspace_flag”, which indicate the existence of the open space.

As demonstrated in “Figure2 Flow-Chart of Decision block”, when “Decision Block” is executed, program will be navigated to different sub-blocks. Navigating variables are given by “junction_detection” block. According to the internal logic of the “junction_detection” block, “junction_flag” and “openspace_flag” cannot be true at the same time, which ensures that “Decision” block cannot execute “Corridor Decision Block” and “Open Space Decision Block” in same round.

The detailed decision logic will be interpreted below：

“Decision Block” is called after “Detection Block”(junction_detection & door_check) has been executed with the information which indicated the existence of door、junction and open space provided. The value of junction_flag ,openspace_falg and openspace_decision_flag are checked.

• Junction Mode:When junction_flag is true, which indicates that junction is found and the absence of open space due to the mutual exclusiveness between junction indicator and open space indicator, Decision Block will be navigated to “Corridor Decision Block”. Arbitrary choice between available corridor directions will be made in “Corridor Decision Block” .

• Open Space Mode:If it is open space detected, “Open Space Decision Block” will be executed, difference priorities are given to different choice. “turning left” is given the highest priority, so the availability of the corridor at left side of the robot will be check first, and then the check on the availability of corridor at forward direction follows and the last direction to check is right side. With this order, the robot can implement the goal of moving along the left wall in open space. The difference between the second and the third sub-blocks in “Open Space Decision Block” is that they adopt different move-forward mode. When “Openspace_flag==true &&Openspace_decision_flag==true” holds, the robot is moving within the open space or just enter the open space. Under these two circumstance, the moving forward movement controlled in a close loop way to avoid bumping the wall . when “openspace_flag==false && openspace_decision_flag=true?” hold, the robot is gonna to encounter with the exit of the open space. Because of lead detection on open space, the indicator of open space has been reset. To ensure that the robot can move to the middle line of the exit before making the next decision, valuable openspace_decision_flag is declared. With this valuable distinguishing the last forward movement before leaving the open space with other forward movements in the open space, last forward movement is an open loop controlled one navigating the robot to move forward by 0.5 meters. “Protection Block” is executed all the time, which protects the robot from bumping in open loop procedures .
  

As for the first sub-block in "Open Space Decision Block", it will give the movement comment "Turning left" and set variable "openspace_decision_flag" at the end of itself, which ensures that it can only be executed by one time in every open space and only executed at the moment when robot enter the open-space.

Flow Chart of Decision Block

There are three cases that robot may encounter in open space:
Case1: As figure “openspace_case1” show, the first case that robot encountered in open space. In this case, laser beam at right, left and front side has detected the available corridors at the corresponding directions. While another two laser beams at front right and front left direction for open space detection will reset junction_flag when their detection has proved the existence of the open space. Openspace flag and openspace_decision_flag will be set at this moment. Movement control will switch to open space mode. According to the priority of different direction choice, the robot will take turning left as first choice and turn to left side.	openspace_case1
Case2: Then the robot comes to the second case illustrated in “openspace_case2”. Because the front right laser beam keeps reporting the existence of the open space on right side, open space indicator stay at 1. The priority of direction choice make “Forward movement” be the movement choice. Decision block executes “Open Space Decision Block” and the condition “Openspace_flag==true &&Openspace_decision_flag==true” holds, forward movement is controlled in close loop way.	openspace_case2
Case3: The third case following is illustrated in figure “openspace_case3”, laser beam for open space detection are blocked by the wall in front of the robot and thus openspace_flag is reset. While valuable openspace_decision_flag stay at 1 and this variable can only be reset by the second sub-block of “Open Space Decision Block”. Now, condition that “openspace_flag==false && openspace_decision_flag=true” holds and second sub-block of “Open Space Decision Block” is executed. With the priority of the choices, forward movement is selected again. However, this time forward movement is controlled by open loop block and the robot will move forward by 0.5 meter if it is not stopped by protection block.	openspace_case3

Movement

Door Movement

---

After the door check function has been completed, the door movement function is then called. The possible door flag obtained from the door check function is then utilized to determine the position of the door. If the possible door flag is 1 the robot has to turn to the right, if the flag is 2 the robot has to move straight and if the flag is 3, the robot has to turn left. After making the turn and adjust its position with respect to the potential door, the “io.sendOpendoorRequest” command which represented as a bell rings is executed and the robot has to wait for door respond in 5 seconds. After 5 seconds has expired, the robot then scan whether the door has opened or not. If the door is opened, then all the flag in the function are cleared and then the program will jump out from this door movement function into turn straight in the junction movement function. If the door is not opened (which means false door is detected) then the robot needs to make a turn. The turn that the robot has to made is based on where the door was before. If the door was detected in front then 180 degrees rotation is executed, if door was in the left then 90 degrees rotation to the right is executed and if detect the door was in the right then 90 degrees rotation to the left is executed. After the rotation is done, then all of the flag related to the door check and movement are cleared and the program back to the main function. There are few additional flags being used in this door movement function. Calibration flag is used indicate that the robot has adjusted its position with respect to the potential door. The rotate finished flag is used to make sure the robot has finished the rotation. The signal sent flag indicates whether the open door request has been sent or not. The waiting finished flag is used to indicates that the waiting time for door to be opened has been expired.

Door_movement.png

code snippet: door_movement.cpp

Junction Movement

The Junction Movement function consist of five cases: turnStraight, TurnRight, TurnLeft, protection and movForward.

turnStraight:
PICO shortly moves straight for 0.5m or when counters time[3s] has passed and continues with the moveForward function. The counter is needed to prevent the program go into a loop when PICO is not able to move the distance. The forward movement is protected by 5 laser sectors. When it is too close to the wall or obstruction, the forward movement will move backwards with an speed of -0.2m/s. When the laser beams for the obstruction detection on the sides of PICO detects that it is too close to the wall, the program goes into the state of protection and moves PICO away from the obstruction.

turnRight and turnLeft:
Both cases turnRight and turnLeft rotates 90 degrees to the corresponding direction and measures the rotation with the odometer to determine when to stop. To make sure that the odometer is accurate enough, the rotating speed is set at 0.2 rad/s.

movForward:
After finishing the turnStraight, the code will go into the movForward state. PICO moves straight for 5m or stops when counters time[5s] has passed. This movement is exactly the same as the turnStraight movement except for the distance and countertime.

protection:
When an obstruction is detected in the turnStraight or movForward, PICO then stops and moves sideways to avoid the obstruction.
The protection in the junction_movement.cpp and move_forward.cpp are explained here: protection

The code snippet is included below:
code snippet: junction_movement.cpp

Move Forward

Similarly to the junction_movement.cpp, move_forward.cpp uses three front laser bundles to adjust the speed. The forward speed is either 0.5m/s when the detection allows or -0.2 m/s otherwise. What differs from the junction_movement.cpp is that the forward movement will not stop when it adjusts with the side movement to avoid obstruction. With 6 laser bundles, 3 at both sides, it adjusts the sidewards speed. When it detects a obstruction, it will move sidewards to avoid the obstruction.

code snippet: move_forward.cpp

Protection

The picture on the left is the front detection for adjusting the forward velocity and on the right picture is the side detection to adjust the sideways velocity. All laser beams takes the average of 15 points. The forward speed is either 0.5m/s when there is no obstruction in the front or -0.2 m/s otherwise. The sidewards speed is 0.2m/s when an obstruction is detected or 0 m/s otherwise. Below is the protection in action to avoid collision.

The code snippet for side protection here:side protection
The code snippet for forward protection here:forward protection

Result

Unfortunately, our algorithm failed again in the challenge because some configurations don't well fit the dimension of the maze. The performance of our robot is disappointing as the follow video "Maze Challenge Record" shows:
Maze Challenge Record
The robot got trapped at the very beginning. The reason of failure is that the distance which indicates "No way forward" was too large and it made robot conclude that there is no way forward before it moved out of the U shaped structure. The action following the discovery of obstacle forward is 'detecting the existence of corridor on the both sides of the robot to check whether it is an junction or dead end'. Because the robot hasn't moved out of U-shaped structure when it was informed of the discovery of obstacle in front of it by the overlarge front laser beam, its detection on left and right side returned that the corridor doesn't exist on both side, which made the robot conclude that it was located in a dead end and door checking algorithm at dead end was activated. After "door open signal" was sent, the robot waited for 5 seconds and check whether the door was open. Definitely, it wasn't because it was not a door. According to "Door Movement Algorithm For Dead end", If no door was detected open, the robot would rotated back by 180° after waiting for 5s. While, it was a real dead end if it rotated back. So same movement repeated again. And it would rotated back after a failed try to open the door at the second dead end. In this way, PICO got trapped in the loop and would never find way out. Phenomenon stated above is illustrated more clearly in Figure "First Dead End" and "Second dead end".
First Dead End	Second Dead End
Causal analysis on the failure of the Maze Challenge was proved by simulator as well. We make the configures same as what we applied in Experiment and executed the code on simulator, same phenomenon was observed as video "Dead End Loop in Simulator" shows.
Dead End Loop in Simulator
After the Maze Challenge, we lowered the value of detection radius for front obstacle detection and ran the code in simulator. Because it is arbitrary decision, the time PICO costed for maze solving is not a constant. But we found the maze could always be solved within 5 mins. Video "Maze Solving performance in simulator" shows that "Maze is solved successfully within 5 mins"
Maze Solving performance in simulator

Evaluation

The problem of complexity:
By adding states we increase the complexity of the system and this can result very easily into undesired behavior as it is observed. PICO can only perform 20 tasks each second and since we are moving at a speed of 0.5 m/s and this combined with the many states the program has, can result into more undesired behavior.
Right now, states are being used to prevent undesired behavior. This way of solving requires the making of repeating states with different conditions which in itself increases the complexity. It is proposed to cut down the number of states and only focus on the key tasks: moving block, detection block, door handling block, maze solving block. A supervisory controller is implemented manually and undesired behavior is filtered based on observations in simulations and testing in the real environment. By implementing techniques for the synthesis of a supervisory control would have reduced many undesired behavior beforehand.

The problem of door detection and protection:
The door detection in the maze challenge went into a loop. By reducing the detection range of a door, this problem was solved.
Another problem was the protection. The protection field was wider than the corridor. Because when an obstruction at the side was detected, protection causes the forward velocity to be zero and activates the sidewards velocity. This resulted in a deadlock. By reducing the protection field, this problem could be resolved.

The problem of junction detection: use node is more robust--> make lines ?

Evaluation group9 EMC 2017

- learned programming in c++
- learned to work with GIT
- reuse code instead of repeating the code
- have a clear understanding of the design among team members is critical
- clear and frequent communication and clarity in assigned tasks will improve progress of the group

Code snippets

corridor challenge

potential field
junction detection

maze challenge

Door Movement

code snippet: door_movement.cpp

Movement

code snippet: move_forward.cpp
code snippet: junction_movement.cpp[math]\displaystyle{ Insert formula here }[/math]

Files

File:Assignment-for-week1.pdf