Embedded Motion Control 2017 Group 9

From Control Systems Technology Group
Jump to navigation Jump to search

Group Members

Name: Student id:
Mian Wei 1035075
Zhihao Wu X
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

Functionsg9.jpeg

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

Doortempg9.jpeg

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.
Corridor test.gif

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

Architecture.jpeg

main flow

Main flow.jpeg

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 dead end. 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).

Frontdoor.PNG

  • 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 (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:

\frac{detection radius}{cos24}>0.7

   [math]\displaystyle{ \sum_{i=0}^\infty 2^{-i} }[/math] 

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

detection radius*sin24>0.4

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.

Junction detection.jpeg


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.

door open detection

movement

door movement

Door movement.jpeg

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

Mapping2.jpeg

protection

Potential field2.jpeg

open space

result

evaluation

Evaluation group9 EMC 2017

Code snippets

Movement

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

Files