Presentation is loading. Please wait.

Presentation is loading. Please wait.

Robot Navigation Napoleon Reyes, Ph.D. Computer Science, Massey University, Auckland, New Zealand.

Similar presentations


Presentation on theme: "Robot Navigation Napoleon Reyes, Ph.D. Computer Science, Massey University, Auckland, New Zealand."— Presentation transcript:

1 Robot Navigation Napoleon Reyes, Ph.D. Computer Science, Massey University, Auckland, New Zealand

2 Topics for Discussion Fuzzy Logic Theory Fuzzy Inference System Hand-simulation of the FIS Robot Navigation: problem definition, geometry & trigonometry of the problem Simulation System Assignment

3 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

4

5 Robot Soccer Set-up Colour objects Fluorescent lamps Overhead Camera Exploratory environment is indoor – room totally obstructed from sunlight Multiple monochromatic light sources – fluorescent / fluoride lamps Colour Object Recognition (Recognition speed: < 33ms) *

6

7

8

9 Research Areas: Navigation Target 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

10 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?

11 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

12

13 * How does the robot follow the ball? Direction (X r, Y r ) (X c, Y c ) (0,0) Origin

14 * How does the robot follow the ball? xyDirection θ θ' (X t, Y t ) (X r, Y r ) (X c, Y c ) (0,0) Origin Calculate angle from the ball Calculate distance from the ball Calculate optimum speed to reach the ball Calculate the steering angle to reach the ball Move the robot Loop (we need to do this every few milliseconds)

15 * Angle of the ball relative to the heading angle of the robot. Direction θ (X r, Y r ) (X c, Y c ) (0,0) Origin We are interested in finding the minimum turn to follow the ball.

16 * Angle of the ball relative to the heading angle of the robot. Direction θ (X r, Y r ) (X c, Y c ) (0,0) Origin θ'

17 * Catching the ball is different from just following the ball. Direction (X r, Y r ) (X c, Y c ) (0,0) Origin Home goal You will have to take into account the position of your home goal.

18

19 19 Transformation Equations WORLD-to-DEVICE COORDINATES x 1080 pixels 100,000,000 miles x 500,000 miles World System of Coordinates Device System of Coordinates +x +y +x +y 0 0 (X world,Y world ) (X Device,Y Device ) We are accustomed to representing objects in terms of their World coordinates.

20 World System of Coordinates (Device System of 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 x 1080 pixels 100,000,000 miles x 500,000 miles +x +y +x +y 0 0 (X world,Y world ) (X Device,Y Device ) WORLD SYSTEM OF COORDINATES DEVICE SYSTEM OF COORDINATES

21 21 Boundaries of the Coordinate Systems SETTING THE BOUNDARIES Use the upper-left and bottom-right coordinates to set the boundaries +x +y +x +y 0 0 (X world,Y world ) (X Device,Y Device ) Top-left: (x1, y1) Bottom-right: (x2, y2) World System of Coordinates Device System of Coordinates (X 1,y 1 ) (X 2,y 2 )

22 22 Transformation Task: Given a pair of x & y-world coordinates, find the equivalent x & y-device coordinates. +x +y +x +y 0 0 (X world,Y world ) (X Device,Y Device ) World System of Coordinates Device System of Coordinates (X 1,y 1 ) (X 2,y 2 )

23 23 World-to-Device Coordinates TRANSFORMATION EQUATIONS world X-coordinate Device y For calculating Device y, replace all xs with y

24 Path-Finding With the A* Algorithm * S

25 * S Complementary Path Finding Systems The Hybrid Fuzzy A* Navigation Algorithm

26

27 Fuzzy Inference Process Fuzzification Rule Evaluation Defuzzification e.g. theta e.g. force Fuzzification: Translate input into truth values Rule Evaluation: Compute output truth values Defuzzification: Transfer truth values into output

28 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

29 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 NEARFARVERY FAR Typically we would subdivide the space discreetly.

30 Fuzzy Rule If (Distance is NEAR) … Fuzzy Logic allows (actually encourages) us to define overlapping sub ranges. obstacle (obsx, obsy) (x,y) OBSTACLE AVOIDANCE NEARFARVERY 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

31 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 System distance angle Robot heading direction Steering angle Relative position of the ball (left or right) θ θ

32 Fuzzy Rule F NEAR (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. F SMALL (Angle) = degree of membership of the given angle in the Fuzzy Set SMALL Could be a constant or another MF More on this later!

33

34 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 Multiple Fuzzy Systems employ the various robot behaviours Fuzzy System 1 Fuzzy System 2 Fuzzy System 3 Fuzzy System 4 CentralControl Target Pursuit Obstacle Avoidance Target & Nearest Obstacle (x,y coordinates)

35 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

36 Fuzzy Sets Angles Sub ranges for Small, Medium and Large Angles overlap SMALL MEDIUM LARGE * Small MediumLarge Degree of membership Angle in degrees Membership Functions

37 Taking advantage of Angle Symmetry Fuzzy 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.

38 Fuzzy Sets Distances Sub ranges for Near, Far and Very Far Fuzzy Sets for Distances overlap * Degree of membership Distances in cm. Membership Functions Near Far Very Far NEAR FAR VERY FAR

39 All Fuzzy Sets Angle and Distance Sub ranges for angles & distances overlap * SMALL MEDIUM LARGE NEAR FAR VERY FAR

40 Fuzzy Systems for Target Pursuit NEARFARVERY FAR SMALLMild Turn Zero Turn MEDIUMMed TurnMild Turn LARGESharp TurnMed Turn Next Waypoint The A* Algorithm NEARFARVERY FAR SMALLMed SpeedFast SpeedVery Fast MEDIUMSlow SpeedMed SpeedFast Speed LARGEVery SlowSlow 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. 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 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. 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

41 Fuzzy Systems for Obstacle Avoidance NEARFARVERY FAR SMALLVery SharpSharp TurnMed Turn MEDIUMSharp TurnMed TurnMild Turn LARGEMed TurnMild TurnZero Turn Nearest Obstacle (Distance and Angle) NEARFARVERY FAR SMALLVery SlowSlow SpeedFast MEDIUMSlow SpeedFast SpeedVery Fast LARGEFast SpeedVery FastTop Speed e.g. If the Distance from the Obstacle is NEAR and the Angle from the Obstacle is SMALL Then turn Very Sharp. 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 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. 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

42 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.

43 ASSIGNMENT

44 Start-up Codes Lets have a look at the Start-up Codes and a Sample Solution. Inspect the files provided, the classes defined, how to use them, etc.

45 Path-Finding With the A* Algorithm * S

46 * S Complementary Path Finding Systems The Hybrid Fuzzy A* Navigation Algorithm

47 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 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 Obstacle Avoidance

48 Traditional approach

49 Assignment Use this traditional approach in developing the Fuzzy Systems for the assignment. 7 Fuzzy Sets for the input Angle ZE – Zero NS – Negatively Small NM-Negatively Medium NL – Negatively Large PS – Positively Small PM-Positively Medium PL – Positively Large (x,y) ZE PS NS PM NM PL NL

50 (x,y) ZE PS NS PM NM PL NL ZE – Zero NS – Negatively Small NM-Negatively Medium NL – Negatively Large PS – Positively Small PM-Positively Medium PL – Positively Large Assignment 7 Fuzzy Sets for the input Angle, 3 Fuzzy Sets for the input Distance NLNMNSPSPMPLZE ANGLE FARVERY FAR // DISTANCE NEAR

51 FARVERY_FAR NL NM NS Mild Left ZE PS Mild Right PM PL (x,y) ZE PS NS PM NM PL NL ZE – Zero NS – Negatively Small NM-Negatively Medium NL – Negatively Large PS – Positively Small PM-Positively Medium PL – Positively Large TARGET PURSUIT STEERING ANGLE Fuzzy Logic System #1 - for Calculating the STEERING ANGLE Distance Angle

52 NEARFARVERY_FAR NL NM NS Very Slow ZE FastVery Fast PS PM PL (x,y) ZE PS NS PM NM PL NL ZE – Zero NS – Negatively Small NM-Negatively Medium NL – Negatively Large PS – Positively Small PM-Positively Medium PL – Positively Large TARGET PURSUIT SPEED Fuzzy Logic System #2 - for Calculating the SPEED Distance Angle

53 NEARFARVERY_FAR NL NM NS Sharp Right ZE Sharp Right or Left PS Sharp Left PM PL (x,y) ZE PS NS PM NM PL NL ZE – Zero NS – Negatively Small NM-Negatively Medium NL – Negatively Large PS – Positively Small PM-Positively Medium PL – Positively Large OBSTACLE AVOIDACE STEERING ANGLE Fuzzy Logic System #3 - for Calculating the STEERING ANGLE Distance Angle

54 NEARFARVERY_FAR NL NM NS ZE PS PM PL (x,y) ZE PS NS PM NM PL NL ZE – Zero NS – Negatively Small NM-Negatively Medium NL – Negatively Large PS – Positively Small PM-Positively Medium PL – Positively Large OBSTACLE AVOIDACE SPEED Fuzzy Logic System #4 - for Calculating the SPEED Distance Angle

55 C/C++ version

56 // 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 angle typedef enum {out_ze, out_very_mild_turn, out_mild_turn, out_turn,out_sharp_turn, out_very_sharp_turn}; //indices for output speed typedef enum {out_very_slow, out_slow, out_medium, out_fast, out_very_fast, out_wicked_fast}; Enumerated data types – for easy indexing

57 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 types typedef enum { regular_trapezoid, left_trapezoid, right_trapezoid } trapz_type;

58 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 smallVERY_FARThen VERY_FAST If ( angle is small) AND (distance is VERY_FAR) Then output is VERY_FAST. Index: in_angleIndex: in_distance out_fuzzy_set We are storing only the indices of the components of the rule.

59 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 intno_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 } fuzzy_system_rec ;

60 If ANDThen If ( angle is small) AND (distance is VERY_FAR) Then output is VERY_FAST. Defining the Fuzzy Rule Index: in_angleIndex: in_distance out_fuzzy_set 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; NEARFARVERY FAR SMALLMed SpeedFast SpeedVery Fast MEDIUMSlow SpeedMed SpeedFast Speed LARGEVery SlowSlow Speed FAMM We are only storing the indices in the rule.

61 If ( angle is small) AND (distance is VERY_FAR) Then output is VERY_FAST. Membership Function Index: in_angleIndex: in_distance out_fuzzy_set 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[i].inp_index[0] = in_angle; fl->rules[i].inp_index[1] = in_distance; NEARFARVERY FAR SMALLMed SpeedFast SpeedVery Fast MEDIUMSlow SpeedMed SpeedFast Speed LARGEVery SlowSlow Speed inp_mem_fns fz->inp_mem_fns[variable_index][fuzzy_set]

62 Membership Function data structure inp_mem_fns fz->inp_mem_fns[variable_index][fuzzy_set] 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[i].inp_index[0] = in_angle; fl->rules[i].inp_index[1] = in_distance; inp_mem_fns 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];

63 Membership Function Initialisation inp_mem_fns= fl->inp_mem_fns[in_angle][in_small] = init_trapz init_trapz(14.0f, 20.0f, 0.0f, 0.0f, left_trapezoid); 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[i].inp_index[0] = in_angle; fl->rules[i].inp_index[1] = in_distance; init_trapz() inp_mem_fns The init_trapz() function is used to initialize inp_mem_fns. inp_mem_fns inp_mem_fns is a structure containing all the parameters of a membership function.

64 Calculation of degree of Membership trapz trapz( inputs [ variable_index ],fz. inp_mem_fns [ variable_index ][ fuzzy_set ]) 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[i].inp_index[0] = in_angle; fl->rules[i].inp_index[1] = in_distance; inp_mem_fns inp_mem_fns is a structure containing all the parameters of a membership function. trapzactual degree of membership The trapz function calculates the actual degree of membership of a given input value in a fuzzy set. which inputwhich membership function

65 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

66 How to use the Fuzzy Logic Engine?

67 fuzzy_system_rec 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 7 e.g. 7 Maximum Number of Fuzzy Sets allowed Define the maximum number of Fuzzy Sets to be used.

68 fl 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 Development 2. Initialise the fuzzy system. File: fuzzylogic.cpp

69 void initFuzzyRulesTargetPursuitSpeed(fuzzy_system_rec* fl) { int i; for (i = 0;i 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 Development 3. Initialise fuzzy rules. File: fuzzylogic.cpp Rule #0

70 void initMembershipFunctionsTargetPursuitSpeed( fuzzy_system_rec* fl ) { //angle in_angle fl->inp_mem_fns[in_angle][in_small] = init_trapz( 14.0f,20.0f,0.0f,0.0f, left_trapezoid); in_angle fl->inp_mem_fns[in_angle][in_medium] = init_trapz( 14.0f,20.0f,34.0f,40.0f, regular_trapezoid); in_angle 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 Development 4. Initialise membership functions. File: fuzzylogic.cpp which fuzzy set

71 void runGame() { //... g_fuzzy_system_pursuit_speed initFuzzySystemTargetPursuitSpeed(&g_fuzzy_system_pursuit_speed); // keep running the program until the ESC key is pressed while((GetAsyncKeyState(VK_ESCAPE)) == 0 ) { //... inputValuesPursuitSpeed inputValuesPursuitSpeed[ in_angle ] = angleFromTarget; inputValuesPursuitSpeed inputValuesPursuitSpeed[ in_distance ] = distanceFromTarget; fuzzyPursuitSpeedinputValuesPursuitSpeed g_fuzzy_system_pursuit_speed fuzzyPursuitSpeed = fuzzy_system(inputValuesPursuitSpeed, g_fuzzy_system_pursuit_speed); fuzzyPursuitSpeed robot.setSpeed(fuzzyPursuitSpeed); } //... g_fuzzy_system_pursuit_speed free_fuzzy_rules(&g_fuzzy_system_pursuit_speed); //and so on... } Fuzzy System Development 4. Modify runGame(). File: MyProg.cpp

72 void runGame() { //... // Target pursuit fuzzyAngle = fuzzy_system(inputValues, g_fuzzy_system); //Use only if you are using the unorthodox approach RIGHT_SIDE if(targetPosition == RIGHT_SIDE){ //target to pursue is at the right hand-side -fuzzyAngle fuzzyAngle = -fuzzyAngle; //negate the angle } //... newAngle = robot.getAngle() + fuzzyAngle; //adjust robots angle //and so on... robot.setAngle(newAngle); //update robots angle //and so on... } Fuzzy System Development 4. Modify runGame(). File: MyProg.cpp

73 Fuzzy System Development 5. 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

74 Randomisation Some Extras...

75 75 Element of Surprise srand() 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 ) ); srand( (unsigned)time( NULL ) ); /* Display 10 numbers. */ for( i = 0; i < 10;i++ ) printf( " %6d\n", rand() );

76 76 Element of Surprise rand() generates a pseudorandom number - returns int int randomVal(int min, int max) { rand() return (min + (rand() % ((max-min)+1) )); } rand() rand() returns a pseudo-random integral number in the range (0 to RAND_MAX-1)

77 77 Element of Surprise rand() float randomVal(float min, float max) { float r; rand() r = (float)rand()/RAND_MAX; r = min + (r*(max-min)); return r; } rand() rand() returns a pseudo-random integral number in the range (0 to RAND_MAX)-1

78 78 Time elapsed, wait…clock() void wait ( int seconds ) { clock_t clock_t endwait; endwait = clock () + seconds * CLOCKS_PER_SEC ; while (clock() < endwait) {} } clock_t clock_t startTime, elapsedTime; startTime = clock(); …... elapsedTime = (clock() - startTime) / CLOCKS_PER_SEC;

79 Keyboard Handling Function keys, arrow keys, Control and Shift keys, combination of keys

80 80 Keyboard Handling GetAsyncKeyState 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: google Using google, type the following keywords: msdn vk_shift Virtual-key code e.g. vk_shift vk_control SHORT GetAsyncKeyState( int vKey ); // vKey - virtual-key code

81 81 Keyboard Handling GetAsyncKeyState 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 codes

82 82 Keyboard Handling GetAsyncKeyState //general key press event if(GetAsyncKeyState(VK_SPACE) < 0) {... } //key down event if((GetAsyncKeyState(VK_SPACE) & 0X0001) ==0X0001){... } //key up event if((GetAsyncKeyState(VK_SPACE) & 0X8000) ==0X8000){... }

83 83 Keyboard Handling Control Shift keys Monitoring 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 84 Keyboard Handling Possible 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; } …

85 Visual Basic version

86 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

87 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

88 FL(2).LoadOutputVariable OUT_WICKEDFAST, 5.5 FL(2).LoadOutputVariable OUT_VERYFAST, 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 Fuzzy Outputs for Target Pursuit SPEED ADJUSTMENT

89 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

90 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 FUZZY OUTPUTS FOR STEERING STEERING

91 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

92 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

93 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

94 INPUTS: FuzzyTargetInputs(IN_TARGET_ANGLE) = ThetaAdjustmentToHitTarget FuzzyTargetInputs(IN_TARGET_DISTANCE) = DistanceFromTarget FUZZY OUTPUT: FuzzyTargetAngleTurn = FL(1).Fuzzy_System(FuzzyTargetInputs) Taking advantage of Symmetry TARGET SEEKING

95 'Determine correct direction of turn to hit target If (TargetPosition = RIGHT_SIDE) Then LblTargetPosition.BackColor = vbBlue LblTargetPosition.ForeColor = vbWhite LblTargetPosition.Caption = "FOLLOW SIDE ElseIf (TargetPosition = LEFT_SIDE) Then LblTargetPosition.ForeColor = vbWhite LblTargetPosition.BackColor = vbRed LblTargetPosition.Caption = "FOLLOW SIDE FuzzyTargetAngleTurn = -1 * FuzzyTargetAngleTurn FuzzyTargetAngleTurn = -1 * FuzzyTargetAngleTurn End If Taking advantage of Symmetry Angle = Angle + FuzzyTargetAngleTurn Angle = Angle + FuzzyTargetAngleTurn TARGET SEEKING

96 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

97 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

98 End of Presentation Let me answer some of your questions… Thanks for listening! *


Download ppt "Robot Navigation Napoleon Reyes, Ph.D. Computer Science, Massey University, Auckland, New Zealand."

Similar presentations


Ads by Google