Embedded Motion Control 2017 Group 9

Group Members

Name:	Student id:
Mian Wei	1035075
Zhihao Wu	1041226
Petrus Teguh Handoko	X
Bo Deng	X
Bo Cong	X
Jian Wen Kok	X
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

Components

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

detection ‐170◦ wide‐angle camer
‐Laser Range Finder (LRF)
‐Wheel encoders (odometry)
‐Asus Xtion Depth sensor

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

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

At 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.
When PICO detects that it is inside the junction, the potential field goes on and finishes the challenge.

result

The corridor challenge failed

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 into crashing into the walls.

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

evaluation

The first trial used our old program that has proven itself as seen in the video below. Unfortunately we did not push the correct version to PICO.

The second trial 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 run without debugging in the real setting. Later on, a bug was found in the junction detection.

Maze challenge

design

architecture

main flow

The code programmed for maze solving is a flag-based one, which decide whether to run the corresponding blocks or not according to the flags set for indicating different cases. These flag are set when the “Detection & Decision Block” has detected the corresponding cases；when the movement corresponding to a particular case is completed，they will be reset. These flags are the base of logic of the whole program. As demonstrated on Figure1, the structure of Main Loop consists of four blocks: “Detection & Decision Block”，”Door Movement Block”, “Junction Movement Block” and “Normal Movement Block”. Initialization function will run for one time at the moment when the robot is turned on. Then we come to looping part, which is the main part of the main function and executed at a frequency of 20HZ. The first block to be executed in loop structure is “Detection & Decision Block”. “Detection & Decision Block” is responsible for junction detection, door detection ,open space detection and making movement decision. In this block, robot detects the existence of the door first and return “possible_door_flag”. This flag 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. “junction_flag”, which indicate the existence of the junction around the robot, will be returned by “Junction Detection” sub-block and an array named as “junction_direction” with three elements will be return at the mean time. If “junction_flag” is nonzero, which indicate the existence of junction, “Decision” sub-block will be executed to make the decision of the next corridor the robot is going to move to. If not, “Decision” sub-block will be blocked and the robot will take default movement mold——“Normal Move Forward”. Except for junction-detection, “Junction Detection” sub-block is responsible for “Open Space Detection” in the meanwhile. We will illustrate it in detail when introduce the mathematic model for open-space-detection the corresponding movement. One more point added, “Detection & Decision Block” can only be executed when both “junction_falg” and “possible_door_flag” are zero. i.e. , the “Detection & Decision Block” will not be executed anymore without any other blocks to help it to clean the flag set in the last time junction or door was detected. This condition is added to avoid the disturbance of error movement order. When the robot is executing actions, 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. We block the “Detection and Decision Block” until the last issued command is implemented completely and “junction_falg” , “possible_door_flag” are reset. 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.

detection

---

door detection

---

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

• Front door detection method

Detecting the distance of three ranges, the range is defined by angle θ=48°, and detection radius φ_f.
We consider the front door as a dead end, hence the detection is to check whether there is a dead end or not. If the average distance in the three detect ranges all smaller than φ_f, that means there is a dead end and return possible_door_flag=2 (means there is a front door).

• Side door detection method

Using the left and right side detect ranges to detect the side door, considering the given restrictions the depth of door template is around 0.3m and the corridor width is from 0.5m to 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).
For example, considering the right door in the Figure, first check whether two edge scan data 303 and 2(±24°) are in the red shadow (in Figure), and the data of front 45° smaller than φ_C. If the first condition satisfied then check the second condition: all the other data between scan data 303 and 2, if 75% data satisfied the length condition (in the red shadow), that means there is a right side door, and return possible_door_flag=1, (for the left door possible_door_flag=3 ).

junction detection

In the following picture, it is clear that three laser bundles in forward direction, left direction and right direction are selected to detect the junction. In order to find junction in perfect way, 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 degree.

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

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

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.

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 has been setted to 1, the decision block will be triggered and decide a direction to move based on the direction flag.

movement

door movement

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

mapping

protection

Code snippets

Movement

code snippet: move_forward.cpp
code snippet: junction_movement.cpp