robot navigation
DESCRIPTION
IIMS Lab 7. Robot Navigation. Robot Soccer Game. Fuzzy Logic + A*. Napoleon Reyes, Ph.D. Computer Science, Massey University, Auckland, New Zealand. Fuzzy Logic Theory. Fuzzy Inference System. Topics for Discussion. Hand-simulation of the FIS. - PowerPoint PPT PresentationTRANSCRIPT
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
(Xworld,Yworld)(XDevice,YDevice)
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
(Xworld,Yworld)(XDevice,YDevice)
Top-left: (x1, y1)Bottom-right: (x2, y2)
World System of Coordinates Device System of Coordinates
(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
(Xworld,Yworld)(XDevice,YDevice)
World System of Coordinates Device System of Coordinates
(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
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
ZE – ZeroNS – Negatively SmallNM-Negatively MediumNL – Negatively LargePS – Positively SmallPM-Positively MediumPL – Positively Large
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
ZE – ZeroNS – Negatively SmallNM-Negatively MediumNL – Negatively LargePS – Positively SmallPM-Positively MediumPL – Positively Large
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
ZE – ZeroNS – Negatively SmallNM-Negatively MediumNL – Negatively LargePS – Positively SmallPM-Positively MediumPL – Positively Large
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
ZE – ZeroNS – Negatively SmallNM-Negatively MediumNL – Negatively LargePS – Positively SmallPM-Positively MediumPL – Positively Large
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
ZE – ZeroNS – Negatively SmallNM-Negatively MediumNL – Negatively LargePS – Positively SmallPM-Positively MediumPL – Positively Large
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;
If ( angle is small) AND (distance is VERY_FAR) Then output is VERY_FAST.
Defining the Fuzzy Rule
Index: in_angle Index: 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;
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
FAMM
We are only storing the indices in the rule.
If ( angle is small) AND (distance is VERY_FAR) Then output is VERY_FAST.
Membership Function
Index: in_angle Index: 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;
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
fz->inp_mem_fns[variable_index][fuzzy_set]
Membership Function data structure
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 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);
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;
The init_trapz() function is used to initialize inp_mem_fns.
inp_mem_fns is a structure containing all the parameters of a membership function.
Calculation of degree of Membership
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 is a structure containing all the parameters of a membership function.
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
Keyboard HandlingGetAsyncKeyState
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
Keyboard HandlingGetAsyncKeyState
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
Fuzzy Outputs for Target Pursuit
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
FUZZY OUTPUTS FOR STEERING
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
Fuzzy Outputs for Target Pursuit
FUZZY OUTPUTS FOR STEERING
STEERING
End of PresentationLet me answer some of your questions…
Thanks for listening!
*