Robot Navigation

Robot Navigation

Napoleon Reyes, Ph.D.

Computer Science, Massey University, Auckland, New Zealand

Topics for DiscussionFuzzy Logic Theory

Fuzzy Inference System

Hand-simulation of the FIS

Robot Navigation: problem definition, geometry & trigonometry of the problem

Simulation System

Assignment

Topics

• Fuzzy Logic Theory Review

• Fuzzy Logic Engine• How to compile• Structure of the program• Angle Calculation – Dot

Product• Graphics Engine• World to Device

transformation

• Alternative Fuzzy System design for robot navigation

Robot Soccer

Robot Soccer Set-up

Colour objects

Fluorescent lampsOverhead Camera

Exploratory environment is indoor – room totally obstructed from sunlight

Multiple monochromatic light sources – fluorescent / fluoride lamps

Colour Object Recognition (Recognition speed: < 33ms)*

Research Areas: NavigationTarget Pursuit and Obstacle Avoidance

Calculation of Optimal path

Team cooperation among agents

Robot Behaviours: ball dribbling, ball passing, shooting from a stationary position

Intelligent Coach: Role Allocation, Role switching

Motor Control: take into account coefficient of friction, noise, delay in the communication module

Research Areas: Navigation

How to steer the robots automatically and smoothly

How to adjust the speed of the robot depending on the distance of the target, angle from the target, speed and direction of the target

How to discretise the exploration space to allow for A* to work? – fixed grid size, Voronio graphs

How to calibrate the navigation rules and fuzzy sets automatically?

Research Areas: Machine Vision

Colour Classification: Compensate for the effects due to changing illumination intensities

Colour Object Recognition: Shape, combination of colours, size

Colour Segmentation (also called labelling) + edge detection

Multiple camera colour object recognition

Basic Navigation

*

How does the robot follow the ball?

Direction

(Xr, Yr)

(Xc, Yc)

(0,0) Origin

*

How does the robot follow the ball?

x

y

Direction

θθ'

(Xt, Yt)

(Xr, Yr)

(Xc, Yc)

(0,0) Origin

Calculate angle from the ballCalculate distance from the ballCalculate optimum speed to reach the ballCalculate the steering angle to reach the ballMove the robot

Loop(we need to do this every few milliseconds)

*

Angle of the ball relative to the heading angle of the robot.

Direction

θ (Xr, Yr)

(Xc, Yc)

(0,0) Origin We are interested in finding the minimum turn to follow the ball.

*

Angle of the ball relative to the heading angle of the robot.

Direction

θ (Xr, Yr)

(Xc, Yc)

(0,0) Origin

θ'

*

Catching the ball is different from just following the ball.

Direction

(Xr, Yr)

(Xc, Yc)

(0,0) Origin

Home goal

You will have to take into account

the position of your home goal.

World Coordinates System to

Device Coordinates System Transformation

19

Transformation EquationsWORLD-to-DEVICE COORDINATES

159.302

1920 x 1080 pixels

100,000,000 miles x 500,000 miles

World System of Coordinates Device System of Coordinates

+x

+y+x

+y0

0

(Xworld,Yworld)(XDevice,YDevice)

We are accustomed to representing objects in terms of their World coordinates.

• Our Physics and Math equations all work in the World System of Coordinates, but our computer screen is using something different (Device System of Coordinates).

• It is more intuitive to use the World System of Coordinates in the formulation of our Fuzzy Control Systems.

• Transformation equations allow for easy scaling (zoom-in/zoom-out) of objects in the scene.

1920 x 1080 pixels100,000,000 miles x 500,000 miles

+x

+y+x

+y0

0


WORLD SYSTEM OF COORDINATES DEVICE SYSTEM OF COORDINATES

21

Boundaries of the Coordinate SystemsSETTING THE BOUNDARIES

159.302

Use the upper-left and bottom-right coordinates to set the boundaries

+x

+y+x

+y0

0


Top-left: (x1, y1)Bottom-right: (x2, y2)


(X1,y1)

(X1,y1)

(X2,y2) (X2,y2)

22

Transformation 159.302

Task: Given a pair of x & y-world coordinates, find the equivalent x & y-device coordinates.

+x

+y+x

+y0

0



(X1,y1)

(X1,y1)

(X2,y2) (X2,y2)

23

World-to-Device Coordinates

TRANSFORMATION EQUATIONS

159.302

12

12

XX

XX

WorldBoundWorldBounddDeviceBoundDeviceBoun

Xslope

11*tx_intercep XX WorldBoundXslopedDeviceBoun

tx_intercepWorld* XX XslopeDeviceworld X-coordinate

For calculating Devicey, replace all x’s with y

Path-Finding With the A* Algorithm

*

S

*

S

Complementary Path Finding Systems

The Hybrid Fuzzy A* Navigation Algorithm

Fuzzy Logic Theory

Fuzzy Inference Process

Fuzzification Rule Evaluation Defuzzification

e.g. theta e.g. force

Fuzzification: Translate input into truth valuesRule Evaluation: Compute output truth valuesDefuzzification: Transfer truth values into output

Fuzzy Rule

If (Distance is NEAR) and (obstacle is at the right hand side) and (Angle is SMALL)

But how can we define NEAR or Small

or Turn Sharp Left numerically?

Some solutions to a problem could be described linguistically but only very vaguely.

obstacle

(obsx, obsy)

(x,y)

Then Turn Sharp Left.

OBSTACLE AVOIDANCE

Fuzzy Rule

If (Distance is NEAR) …

NEAR: [0, 10.6cm]FAR: (10.6, 20cm]VERY FAR: (20cm, 50cm]

obstacle

(obsx, obsy)

(x,y)

OBSTACLE AVOIDANCE

NEAR FAR VERY FAR

Typically we would subdivide the space discreetly.

Fuzzy Rule

If (Distance is NEAR) …

Fuzzy Logic allows (actually encourages) us to define overlapping sub ranges.

obstacle

(obsx, obsy)

(x,y)

OBSTACLE AVOIDANCE

NEAR FAR VERY FAR

NEAR: [-infinity, 14.5cm]FAR: [10.6, 24cm]VERY FAR: [20cm, +infinity]

It provides all the mathematical functions to quantify our terms and use them in our rules.

FUZZY SETS

Fuzzy Rule

If Distance is NEAR and Angle is SMALL Then Turn Sharply.

obstacle

(obsx, obsy)

(x,y)

OBSTACLE AVOIDANCE

We could actually take advantage of symmetry and consider only half of the angle space.

Fuzzy Systemdistance

angle

Robot heading direction

Steering angle

Relative position of the ball

(left or right)

θ

θ

Fuzzy Rule

FNEAR(Distance) = degree of membership of the given distance in the Fuzzy Set NEAR

If Distance is NEAR and Angle is SMALL Then Turn Sharp Left.

FSMALL(Angle) = degree of membership of the given angle in the Fuzzy Set SMALL

Could be a constant or another MF

More on this later!

Fuzzy Logic System Design for Robot

Navigation

Cascade of Fuzzy Systems

Adjusted Speed

Adjusted Angle

Angle and Distance

N

Y

Adjusted Speed

Adjusted Angle

Fuzzy System 1: Target Pursuit

Fuzzy System 2: Speed Control for Target Pursuit

Fuzzy System 3: Obstacle Avoidance

Fuzzy System 4: Speed Control for Obstacle Avoidance

ObstacleDistance < MaxDistanceTolerance

and not on the same side as the Target

Actuators

Position of the Target and Nearest Obstacle

Multiple Fuzzy Systems employ the various robot behaviours

Fuzzy System 1

Fuzzy System 2

Fuzzy System 3

Fuzzy System 4

CentralControl

Target Pursuit

ObstacleAvoidance

Target & Nearest Obstacle (x,y coordinates)

Target Pursuit and Obstacle Avoidance

obstacle

(obsx, obsy)

(x,y)

Can you describe how the robot should turn based on the position

and angle of the obstacle?

Robot Navigation

Demonstration

Obstacle Avoidance & Target Pursuit

Fuzzy SetsAngles

Sub ranges for Small, Medium and Large Angles overlap

SMALL

MEDIUM

LARGE

*

Small Medium Large

Degr

ee o

f mem

bers

hip

Angle in degrees

Membership Functions

Taking advantage of Angle SymmetryFuzzy Sets for Angles

The approach developed takes advantage of the Angle symmetry, and is equivalent to having 6 Fuzzy Sets for the Angles.

*

SMALL

MEDIUM

LARGE

SMALL

MEDIUM

LARGE

Left Right

A simplified approach that reduces the computational cost without sacrificing accuracy.

Fuzzy SetsDistances

Sub ranges for Near, Far and Very Far Fuzzy Sets for Distances overlap*

Degr

ee o

f mem

bers

hip

Distances in cm.

Membership Functions

Near Far Very Far

NEAR

FAR

VERY FAR

Fuzzy Systems for Target Pursuit

NEAR FAR VERY FAR

SMALL Mild Turn Mild Turn Zero Turn

MEDIUM Med Turn Mild Turn Mild Turn

LARGE Sharp Turn Med Turn Med Turn

Next Waypoint

The A* Algorithm

NEAR FAR VERY FAR

SMALL Med Speed Fast Speed Very Fast

MEDIUM Slow Speed Med Speed Fast Speed

LARGE Very Slow Slow Speed Slow Speed

e.g. If the Distance from the Target is NEAR and the Angle from the Target is SMALL Then the robot should make a Mild Turn.

Fuzzy System 1 (Steering)

Fuzzy System 2 (Speed Adjustment)

e.g. If the Distance from the Target is NEAR and the Angle from the Target is SMALL Then the robot should move at a Medium Speed.

Angle

Speed

Fuzzy Systems for Obstacle Avoidance

NEAR FAR VERY FAR

SMALL Very Sharp Sharp Turn Med Turn

MEDIUM Sharp Turn Med Turn Mild Turn

LARGE Med Turn Mild Turn Zero Turn

Nearest Obstacle (Distance and Angle)

NEAR FAR VERY FAR

SMALL Very Slow Slow Speed Fast Fast

MEDIUM Slow Speed Fast Speed Very Fast

LARGE Fast Speed Very Fast Top Speed

e.g. If the Distance from the Obstacle is NEAR and the Angle from the Obstacle is SMALL Then turn Very Sharp.

Fuzzy System 3 (Steering)

Fuzzy System 4 (Speed Adjustment)

e.g. If the Distance from the Obstacle is NEAR and the Angle from the Obstacle is SMALL Then move Very Slowly.

Vision System

Angle

Speed

ADVANTAGES

*

In the robot soccer game, there are 11 robots to control at the most.

SPEED

We are able to demonstrate that our algorithm can control more than 30 robots at twice the required speed of calculation.

Robot NavigationSimulation

ASSIGNMENT

Start-up Codes

Let’s have a look at the Start-up Codes and a Sample Solution.

Inspect the files provided, the classes defined, how to use them, etc.

Path-Finding With the A* Algorithm

*

S

*

S

Complementary Path Finding Systems

The Hybrid Fuzzy A* Navigation Algorithm

Cascade of Fuzzy Systems

Adjusted Speed

Adjusted Angle

Next Waypoint

N

Y

Adjusted Speed

Adjusted Angle

Fuzzy System 1: Target Pursuit

Fuzzy System 2: Speed Control for Target Pursuit

Fuzzy System 3: Obstacle Avoidance

Fuzzy System 4: Speed Control for Obstacle Avoidance

ObstacleDistance < MaxDistanceTolerance

and not on the same side as the Target

Actuators

Path planning Layer:

The A* Algorithm

Multiple Fuzzy Systems employ the various robot behaviours

Fuzzy System 1

Fuzzy System 2

Fuzzy System 3

Fuzzy System 4

Path Planning Layer

CentralControl

Target Pursuit

ObstacleAvoidance

Traditional approach

Assignment

Assignment

Use this traditional approach in developing the Fuzzy Systems for the

assignment.

7 Fuzzy Sets for the input Angle

ZE – ZeroNS – Negatively SmallNM-Negatively MediumNL – Negatively LargePS – Positively SmallPM-Positively MediumPL – Positively Large

(x,y)

ZE PSNSPMNM

PLNL

(x,y)

ZE PSNSPMNM

PLNL


Assignment7 Fuzzy Sets for the input Angle, 3 Fuzzy Sets for the input Distance

NL NM NS PS PM PLZE

+∞-∞ 0

ANGLE

FAR VERY FAR

//

+∞-∞ 0

DISTANCE

NEAR

NEAR FAR VERY_FAR

NL

NM

NS Mild Left

ZE

PS Mild Right

PM

PL

(x,y)

ZE PSNSPMNM

PLNL


TARGET PURSUITFuzzy Logic System #1 - for Calculating the STEERING ANGLE

Distance

Angl

e

NEAR FAR VERY_FAR

NL

NM

NS Very Slow

ZE Fast Very Fast

PS

PM

PL

(x,y)

ZE PSNSPMNM

PLNL


TARGET PURSUITFuzzy Logic System #2 - for Calculating the SPEED

Distance

Angl

e

NEAR FAR VERY_FAR

NL

NM

NS Sharp Right

ZE Sharp Right or Left

PS Sharp Left

PM

PL

(x,y)

ZE PSNSPMNM

PLNL


OBSTACLE AVOIDACEFuzzy Logic System #3 - for Calculating the STEERING ANGLE

Distance

Angl

e

NEAR FAR VERY_FAR

NL

NM

NS

ZE

PS

PM

PL

(x,y)

ZE PSNSPMNM

PLNL


OBSTACLE AVOIDACEFuzzy Logic System #4 - for Calculating the SPEED

Distance

Angl

e

C/C++ version

Fuzzy Logic Engine

// Indices //////////////////////////////////////////////////////////////////////////////////////////////

typedef enum {in_angle, in_distance}; //input indices

typedef enum {in_small, in_medium, in_large}; //indices of fuzzy sets for angle input

typedef enum {in_near, in_far, in_very_far}; //indices of fuzzy sets for distance input

//indices for output steering angletypedef enum {out_ze, out_very_mild_turn, out_mild_turn,

out_turn,out_sharp_turn, out_very_sharp_turn};

//indices for output speedtypedef enum {out_very_slow, out_slow, out_medium, out_fast, out_very_fast,

out_wicked_fast};

Enumerated data types – for easy indexing

Trapezoidal Membership Function

// Trapezoidal Fuzzy Set //////////////////////////////////////////////

struct trapezoid {

trapz_type tp; //type of trapezoid

float a, b, c, d, l_slope, r_slope; //parameters

};

//Trapezoidal membership function typestypedef enum { regular_trapezoid, left_trapezoid, right_trapezoid } trapz_type;

Fuzzy Rule

// Fuzzy Rule //////////////////////////////////////////////////////////

typedef struct {

short inp_index[MAX_NO_OF_INPUTS], //input index inp_fuzzy_set[MAX_NO_OF_INPUTS], //input fuzzy set index out_fuzzy_set; //output index

} rule;

If ( angle is small) AND (distance is VERY_FAR) Then output is VERY_FAST.

Index: in_angle Index: in_distance out_fuzzy_set

We are storing only the indices of the components of the rule.

Fuzzy System

// The complete Fuzzy System //////////////////////////////////////////////////

typedef struct { bool allocated; trapezoid inp_mem_fns [MAX_NO_OF_INPUTS] [MAX_NO_OF_INP_REGIONS];

rule* rules; //note that we need to allocate memory for the rules

int no_of_inputs, //number of inputs no_of_inp_regions, //number of fuzzy sets associated with each input no_of_rules, //number of rules

no_of_outputs; //number of fuzzy outputs

float output_values[MAX_NO_OF_OUTPUT_VALUES]; //the values of the outputs } fuzzy_system_rec;


Defining the Fuzzy Rule


fl->rules[0].inp_fuzzy_set[0] = in_small; fl->rules[0].inp_fuzzy_set[1] = in_very_far; fl->rules[0].out_fuzzy_set = out_very_fast;

fl->rules[0].inp_index[0] = in_angle;fl->rules[0].inp_index[1] = in_distance;

NEAR FAR VERY FAR




FAMM

We are only storing the indices in the rule.


Membership Function



fl->rules[i].inp_index[0] = in_angle;fl->rules[i].inp_index[1] = in_distance;

NEAR FAR VERY FAR




fz->inp_mem_fns[variable_index][fuzzy_set]

Membership Function data structure

fz->inp_mem_fns[variable_index][fuzzy_set]



inp_mem_fns is a structure containing all the parameters of a membership function.

trapezoid inp_mem_fns [MAX_NO_OF_INPUTS] [MAX_NO_OF_INP_REGIONS];

Membership Function Initialisation

fl->inp_mem_fns[in_angle][in_small] =

init_trapz(14.0f, 20.0f, 0.0f, 0.0f, left_trapezoid);



The init_trapz() function is used to initialize inp_mem_fns.


Calculation of degree of Membership

trapz(inputs[variable_index],fz.inp_mem_fns[variable_index][fuzzy_set])




The trapz function calculates the actual degree of membership of a given input value in a fuzzy set.

which input which membership function

float fuzzy_system ( float inputs[ ], fuzzy_system_rec fz) { int i, j; short variable_index, fuzzy_set; float sum1 = 0.0f, sum2 = 0.0f, weight; float m_values[MAX_NO_OF_INPUTS]; for (i = 0; i < fz.no_of_rules; i++) { for (j = 0; j < fz.no_of_inputs; j++) {

variable_index = fz.rules[i].inp_index[j]; fuzzy_set = fz.rules[i].inp_fuzzy_set[j]; m_values[j] = trapz(inputs[variable_index], fz.inp_mem_fns[variable_index][fuzzy_set]);

} /* end j */ weight = min_of (m_values, fz.no_of_inputs); sum1 += weight * fz.output_values[ fz.rules[i].out_fuzzy_set ]; sum2 += weight; } /* end i */ if (fabs(sum2) < TOO_SMALL) { // TOO_SMALL = 1e-6 cout << "\r\nFLPRCS Error: sum2 in fuzzy_system is 0." << endl; exit(1); return 0.0; } return (sum1/sum2);}

Fuzzy System – from fuzzification to defuzzification

which input

which fuzzy set

File: fuzzylogic.cpp

How to use the Fuzzy Logic Engine?

fuzzy_system_rec g_fuzzy_system_pursuit_speed;

Fuzzy System Development

File: MyProg.cpp

1. Declare a global variable for the fuzzy system.

#define MAX_NO_OF_INP_REGIONS 7

File: fuzzylogic.h

e.g. 7 Maximum Number of Fuzzy

Sets allowed

Define the maximum number of Fuzzy Sets to be used.

void initFuzzySystemTargetPursuitSpeed (fuzzy_system_rec* fl) {

fl->no_of_inputs = 2; //inputs are handled 2 at a time only fl->no_of_inp_regions = 3; //number of fuzzy sets per input fl->no_of_rules = 9; //number of rules fl->no_of_outputs = 6; //the pre-defined constant outputs //---- fl->output_values [out_very_slow] = 0.15f; fl->output_values [out_slow] = 0.39f; //… and so on… ... fl->rules = new rule [fl->no_of_rules]; //allocate memory for the rules initFuzzyRulesTargetPursuitSpeed(fl); //initialise the rules initMembershipFunctionsTargetPursuitSpeed(fl); //initialise the membership

//functions}

Fuzzy System Development2. Initialise the fuzzy system. File: fuzzylogic.cpp

void initFuzzyRulesTargetPursuitSpeed(fuzzy_system_rec* fl) { int i; for (i = 0;i < fl->no_of_rules;i++) { //(*fl).rules[i].inp_index[0] = in_angle; //alternatively fl->rules[i].inp_index[0] = in_angle; fl->rules[i].inp_index[1] = in_distance; } fl->rules[0].inp_fuzzy_set[0] = in_small; fl->rules[0].inp_fuzzy_set[1] = in_very_far; fl->rules[0].out_fuzzy_set = out_wicked_fast;

fl->rules[1].inp_fuzzy_set[0] = in_medium; //... //and so on...

}

Fuzzy System Development3. Initialise fuzzy rules. File: fuzzylogic.cpp

Rule #0

void initMembershipFunctionsTargetPursuitSpeed( fuzzy_system_rec* fl ) { //angle fl->inp_mem_fns[in_angle][in_small] = init_trapz(14.0f,20.0f,0.0f,0.0f, left_trapezoid); fl->inp_mem_fns[in_angle][in_medium] = init_trapz(14.0f,20.0f,34.0f,40.0f, regular_trapezoid); fl->inp_mem_fns[in_angle][in_large] = init_trapz (34.0f, 40.0f, 0.0f, 0.0f, right_trapezoid); //distance //... //... //... //and so on...

}

Fuzzy System Development4. Initialise membership functions. File: fuzzylogic.cpp

which fuzzy set

void runGame() { //... initFuzzySystemTargetPursuitSpeed(&g_fuzzy_system_pursuit_speed); // keep running the program until the ESC key is pressed while((GetAsyncKeyState(VK_ESCAPE)) == 0 ) { //... inputValuesPursuitSpeed[ in_angle ] = angleFromTarget; inputValuesPursuitSpeed[ in_distance ] = distanceFromTarget; fuzzyPursuitSpeed = fuzzy_system(inputValuesPursuitSpeed,

g_fuzzy_system_pursuit_speed); robot.setSpeed(fuzzyPursuitSpeed); } //... free_fuzzy_rules(&g_fuzzy_system_pursuit_speed); //and so on...}

Fuzzy System Development4. Modify runGame(). File: MyProg.cpp

void runGame() { //... // Target pursuit fuzzyAngle = fuzzy_system(inputValues, g_fuzzy_system); //Use only if you are using the unorthodox approach if(targetPosition == RIGHT_SIDE){ //target to pursue is at the right hand-side

fuzzyAngle = -fuzzyAngle; //negate the angle } //... newAngle = robot.getAngle() + fuzzyAngle; //adjust robot’s angle //and so on...

robot.setAngle(newAngle); //update robot’s angle

//and so on... }

Fuzzy System Development4. Modify runGame(). File: MyProg.cpp

Fuzzy System Development5. Calibrate the fuzzy membership functions, fuzzy rules and output fuzzy sets.

• If the robot runs in circles, then make the fuzzy outputs smaller.

• Check also if the robot is hitting the obstacles. Adjust the parameters until the robot avoids them at different speeds and angles.

File: fuzzylogic.cpp

Randomisation

Some Extras...

75

Element of Surprisesrand()

159.234

Seed for random-number generation

Seed the random-number generator with current time so that the numbers will be different every time we run. srand( (unsigned)time( NULL ) );

/* Display 10 numbers. */ for( i = 0; i < 10;i++ ) printf( " %6d\n", rand() );

77

Element of Surpriserand()

159.234

float randomVal(float min, float max){

float r;

r = (float)rand()/RAND_MAX;r = min + (r*(max-min));

return r;}

rand() returns a pseudo-random integral number in the range (0 to RAND_MAX)-1

78

Time elapsed, wait…clock()

159.234

void wait ( int seconds ){ clock_t endwait;

endwait = clock () + seconds * CLOCKS_PER_SEC ; while (clock() < endwait) {}}

clock_t startTime, elapsedTime;

startTime = clock();…...elapsedTime = (clock() - startTime) / CLOCKS_PER_SEC;

Keyboard Handling

Function keys, arrow keys, Control and Shift keys, combination of keys

80

Keyboard HandlingGetAsyncKeyState

159.234

The GetAsyncKeyState function determines whether a key is up or down at the time the function is called, and whether the key was pressed after a previous call to GetAsyncKeyState.

To find other pre-defined constants: Using google, type the following keywords: msdn vk_shift

Virtual-key code e.g.

vk_shiftvk_control

SHORT GetAsyncKeyState( int vKey ); // vKey - virtual-key code

http://msdn.microsoft.com/en-us/library/ms645540(VS.85).aspx

81


159.234

void moveSprite(){

if(GetAsyncKeyState(VK_UP) < 0) { SpriteY = SpriteY + 2; //up outtext("UP"); }

if(GetAsyncKeyState(VK_DOWN) < 0) { SpriteY = SpriteY - 2; //down outtext("DOWN"); ….

To find other pre-defined constants: Using google, type the following keywords: msdn virtual key codeshttp://msdn.microsoft.com/en-us/library/ms645540(VS.85).aspx

82


159.234

//general key press eventif(GetAsyncKeyState(VK_SPACE) < 0) {...}

//key down eventif((GetAsyncKeyState(VK_SPACE) & 0X0001) ==0X0001){...}

//key up eventif((GetAsyncKeyState(VK_SPACE) & 0X8000) ==0X8000){...}

83

Keyboard HandlingMonitoring the Control and Shift keys:

if(GetAsyncKeyState(VK_CONTROL)<0) { ControlFlag =! ControlFlag; }

bool ControlFlag, ShiftFlag;

if(GetAsyncKeyState(VK_SHIFT)<0) { ShiftFlag =! ShiftFlag; }

For the Tank to Jump to the Right: Control + Shift + Right Arrow key

For the Tank to Jump to the Left: Control + Shift + Left Arrow key

84

Keyboard HandlingPossible approach in monitoring key combinations :

if(GetAsyncKeyState(VK_RIGHT)<0) {

XDir=RIGHT; if(ShiftFlag) {

outtext("SHIFT + RIGHT"); ShiftFlag=!ShiftFlag; } if(ControlFlag)

{ outtext("CTRL + RIGHT"); if (TankX < getmaxx()-W) TankX += 2; Angle=Angle-5; RaiseWheelFlag=TRUE; ControlFlag=!ControlFlag; }

…

Visual Basic version

FL(1).Init "TARGET: Angle and Distance", IN_TARGET_ANGLE, IN_TARGET_DISTANCE, 2, 3, 7

FL(1).LoadOutputVariable OUT_REVERSETURN, 50 FL(1).LoadOutputVariable OUT_VERYSHARPTURN, 40 FL(1).LoadOutputVariable OUT_SHARPTURN, 30 FL(1).LoadOutputVariable OUT_TURN, 20 FL(1).LoadOutputVariable OUT_MILDTURN, 10 FL(1).LoadOutputVariable OUT_VERYMILDTURN, 3 FL(1).LoadOutputVariable OUT_ZE, 0

Fuzzy Outputs for Target Pursuit

FUZZY OUTPUTS FOR STEERING

STEERING

Fuzzy Sets for Target Pursuit

FL(1).LoadInputMemFcn IN_TARGET_ANGLE, IN_SMALL, 16, 22, 0, 0, LeftTrapezoid FL(1).LoadInputMemFcn IN_TARGET_ANGLE, IN_MEDIUM, 16, 24.58, 30.57, 38, RegularTrapezoid FL(1).LoadInputMemFcn IN_TARGET_ANGLE, IN_LARGE, 30, 38, 0, 0, RightTrapezoid

FL(1).LoadInputMemFcn IN_TARGET_DISTANCE, IN_NEAR, 9.66, 15.33, 0, 0, LeftTrapezoid FL(1).LoadInputMemFcn IN_TARGET_DISTANCE, IN_FAR, 9.66, 15.33, 16.86, 20.19, RegularTrapezoid FL(1).LoadInputMemFcn IN_TARGET_DISTANCE, IN_VERYFAR, 16.86, 20.19, 0, 0, RightTrapezoid

DISTANCE

ANGLE

STEERING

FL(2).LoadOutputVariable OUT_WICKEDFAST, 5.5 FL(2).LoadOutputVariable OUT_VERYFAST, 4.048 FL(2).LoadOutputVariable OUT_FAST, 2.37 FL(2).LoadOutputVariable OUT_MEDIUM, 0.69 FL(2).LoadOutputVariable OUT_SLOW, 0.39 FL(2).LoadOutputVariable OUT_VERYSLOW, 0.15


SPEED ADJUSTMENT

SPEED ADJUSTMENT

Fuzzy Sets for Target Pursuit

FL(2).LoadInputMemFcn IN_DISTANCE, IN_NEAR, 8.09, 11.57, 0, 0, LeftTrapezoid FL(2).LoadInputMemFcn IN_DISTANCE, IN_FAR, 9.25, 13.88, 17.35, 25.8, RegularTrapezoid FL(2).LoadInputMemFcn IN_DISTANCE, IN_VERYFAR, 17.35, 25.8, 34.71, 0, RightTrapezoid

FL(2).LoadInputMemFcn IN_ANGLE, IN_SMALL, 18.18, 36.8, 0, 0, LeftTrapezoid FL(2).LoadInputMemFcn IN_ANGLE, IN_MEDIUM, 20, 38.73, 40, 69, RegularTrapezoid FL(2).LoadInputMemFcn IN_ANGLE, IN_LARGE, 44, 64, 180, 0, RightTrapezoid

DISTANCE

ANGLE

SPEED ADJUSTMENT

FL(0).Init "OBSTACLE: Angle and Distance", IN_ANGLE, IN_DISTANCE, 2, 3, 7 FL(0).LoadOutputVariable OUT_REVERSETURN, 80 FL(0).LoadOutputVariable OUT_VERYSHARPTURN, 60 '45 FL(0).LoadOutputVariable OUT_SHARPTURN, 35 FL(0).LoadOutputVariable OUT_TURN, 25 FL(0).LoadOutputVariable OUT_MILDTURN, 10 FL(0).LoadOutputVariable OUT_VERYMILDTURN, 3 FL(0).LoadOutputVariable OUT_ZE, 0

Fuzzy Outputs for Obstacle Avoidance


STEERING

Fuzzy Sets for Obstacle Avoidance

FL(0).LoadInputMemFcn IN_ANGLE, IN_SMALL, 19.81, 32, 0, 0, LeftTrapezoid FL(0).LoadInputMemFcn IN_ANGLE, IN_MEDIUM, 19, 30, 34, 45, RegularTrapezoid FL(0).LoadInputMemFcn IN_ANGLE, IN_LARGE, 36, 48, 76, 112, RightTrapezoid

FL(0).LoadInputMemFcn IN_DISTANCE, IN_NEAR, 15.84, 17.39, 0, 0, LeftTrapezoid FL(0).LoadInputMemFcn IN_DISTANCE, IN_FAR, 14.67, 17.05, 20.81, 22.76, RegularTrapezoid FL(0).LoadInputMemFcn IN_DISTANCE, IN_VERYFAR, 20.51, 22.76, 0, 0, RightTrapezoid

DISTANCE

ANGLE

STEERING

FL(3).LoadOutputVariable OUT_WICKEDFAST, 7.5 FL(3).LoadOutputVariable OUT_VERYFAST, 5.52 FL(3).LoadOutputVariable OUT_FAST, 2.37 FL(3).LoadOutputVariable OUT_MEDIUM, 0.69 FL(3).LoadOutputVariable OUT_SLOW, 0.39 FL(3).LoadOutputVariable OUT_VERYSLOW, 0.15

Fuzzy Outputs for Obstacle Avoidance

SPEED ADJUSTMENT

SPEED ADJUSTMENT

Fuzzy Sets for Obstacle Avoidance

FL(3).LoadInputMemFcn IN_DISTANCE, IN_NEAR, 8.09, 11.57, 0, 0, LeftTrapezoid FL(3).LoadInputMemFcn IN_DISTANCE, IN_FAR, 9.25, 13.88, 17.35, 25.8, RegularTrapezoid FL(3).LoadInputMemFcn IN_DISTANCE, IN_VERYFAR, 17.35, 25.8, 34.71, 0, RightTrapezoid

FL(3).LoadInputMemFcn IN_ANGLE, IN_SMALL, 18.18, 36.8, 0, 0, LeftTrapezoid FL(3).LoadInputMemFcn IN_ANGLE, IN_MEDIUM, 20, 38.73, 40, 69, RegularTrapezoid FL(3).LoadInputMemFcn IN_ANGLE, IN_LARGE, 44, 64, 180, 0, RightTrapezoid

DISTANCE

ANGLE

SPEED ADJUSTMENT

‘INPUTS: FuzzyTargetInputs(IN_TARGET_ANGLE) = ThetaAdjustmentToHitTarget FuzzyTargetInputs(IN_TARGET_DISTANCE) = DistanceFromTarget

‘FUZZY OUTPUT: FuzzyTargetAngleTurn = FL(1).Fuzzy_System(FuzzyTargetInputs)

Taking advantage of SymmetryTARGET SEEKING

'Determine correct direction of turn to hit target

If (TargetPosition = RIGHT_SIDE) Then LblTargetPosition.BackColor = vbBlue LblTargetPosition.ForeColor = vbWhite LblTargetPosition.Caption = "FOLLOW TARGET @RIGHT SIDE” ElseIf (TargetPosition = LEFT_SIDE) Then LblTargetPosition.ForeColor = vbWhite LblTargetPosition.BackColor = vbRed LblTargetPosition.Caption = "FOLLOW TARGET @LEFT SIDE” FuzzyTargetAngleTurn = -1 * FuzzyTargetAngleTurn

End If

Taking advantage of Symmetry

Angle = Angle + FuzzyTargetAngleTurn

TARGET SEEKING

Sub MoveObject()

Dim A As Single Dim NewX As Single, NewY As Single A = Deg2Rad(Angle) NewX = Obj.x + (Speed * Cos(A)) NewY = Obj.y + (Speed * Sin(A))

Moving the Robot

FL(1).Init "TARGET: Angle and Distance", IN_TARGET_ANGLE, IN_TARGET_DISTANCE, 2, 3, 7

FL(1).LoadOutputVariable OUT_REVERSETURN, 50 FL(1).LoadOutputVariable OUT_VERYSHARPTURN, 40 FL(1).LoadOutputVariable OUT_SHARPTURN, 30 FL(1).LoadOutputVariable OUT_TURN, 20 FL(1).LoadOutputVariable OUT_MILDTURN, 10 FL(1).LoadOutputVariable OUT_VERYMILDTURN, 3 FL(1).LoadOutputVariable OUT_ZE, 0



STEERING

End of PresentationLet me answer some of your questions…

Thanks for listening!

*

Documents

Robot Navigation