├── LICENSE
├── README.md
├── docs
└── images
│ ├── FlowChart.PNG
│ ├── RPO.png
│ └── RPO1.png
└── main.m
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2020 Yasim-Ahmad
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # MATLAB implementation of Artificial Potential Field
2 |
3 |
4 | 
5 |
6 |
7 |
8 | 
9 |
10 |
11 | ## Running instruction:
12 | - Run the main.m file from directory.
13 | - User can defined objects like Robot, Goal, Obstacles in this file.
14 |
15 | ## Requirements
16 | - MATLAB
17 |
18 | ## Features
19 | - Single robot and goal object
20 | - Two obstacle objects
21 |
22 | ## Objective:
23 | - The aim is to move mobile robot from start point to goal point while avoiding obstacle in path
24 |
25 | ## Flow Chart Artificial Potential Field:
26 |
27 | 
28 |
29 | ## Attractive – Repulsive Potentials
30 |
31 | 
32 |
33 | ### Figure 2
34 |
35 | Obstacle for Environment
36 |
37 |
38 | 
39 |
40 |
41 | ### Figure 3
42 |
43 | Obstacles and Target in Environment
44 |
45 |
46 | 
47 |
48 |
49 | ## Mobile Robot Phases:
50 |
51 | ### The Glide Phase
52 |
53 | Initially, Mobile Robot(MR) will move in a straight line from start-point to End-point, unless and until the sensor detects an obstacle
54 |
55 | ### The Maneuver Phase
56 |
57 | The Mobile Robot(MR) enters the maneuver phase to “go around” the obstacle, as shown in Figure 4 below with the
58 | robot (R), obstacle (O) and target(T) in a line. Artificial Particles (AP) are placed, at equal angle
59 | intervals, on a circle of radius C t .
60 |
61 | #### Figure 4
62 |
63 | 2D Grid
64 |
65 |
66 | 
67 |
68 |
69 | ## Selection Criteria for point
70 |
71 | 
72 |
73 | ## Cost Function
74 |
75 | 
76 |
77 | ## Algorithm Development (Steps and Input Data)
78 |
79 | 
80 |
81 | 
82 |
83 | 
84 |
85 | ## Limitations
86 |
87 | 
--------------------------------------------------------------------------------
/docs/images/FlowChart.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Yaaximus/artificial-potential-field-matlab/63c8d2ff458909c4c3484ff01b996fe748be291c/docs/images/FlowChart.PNG
--------------------------------------------------------------------------------
/docs/images/RPO.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Yaaximus/artificial-potential-field-matlab/63c8d2ff458909c4c3484ff01b996fe748be291c/docs/images/RPO.png
--------------------------------------------------------------------------------
/docs/images/RPO1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Yaaximus/artificial-potential-field-matlab/63c8d2ff458909c4c3484ff01b996fe748be291c/docs/images/RPO1.png
--------------------------------------------------------------------------------
/main.m:
--------------------------------------------------------------------------------
1 | clc
2 | clear
3 | close all
4 |
5 | %......................Artificial Potential Field.........................
6 |
7 | Robot_Coordinate = [3 3] ;% Rabot Coordinate = [x-axis y-axis]
8 | Obstacle1_Coordinate = [6 7] ;% Obstacle1 Coordinate = [x-axis y-axis]
9 | Obstacle2_Coordinate = [6 5] ;% Obstacle2 Coordinate = [x-axis y-axis]
10 | Goal_Coordinate = [9 9] ;% Target Coordinate = [x-axis y-axis]
11 | Sensor_Range = 2 ;% Sensor Range being used
12 | Step_Size = 0.4*Sensor_Range ;% Radius of Circle for Artificial Points
13 | Obstacle = [1 4] ;% Obstacle = [Aplha Mew][0.5 0.5]
14 | Goal = [1 4] ;% Target = [Alpha Mew][4 70]
15 | NPTS = 60 ;% Number of Artificial Points %60
16 | StepDegree = 360/NPTS ;% Step Degree in Degree for Artificial Points
17 | Confirm_Message = 'Solution Exists' ;% Message Displayed if a Good Artificial Point Exists
18 | Error_Message = 'No Solution Exists';% Message Displayed if no Good Artificial Point Exists
19 | Bacteria_x = Robot_Coordinate(1) ;% Artificial Best Point x
20 | Bacteria_y = Robot_Coordinate(2) ;% Artificial Best Point y
21 | %.................................Plot.....................................
22 |
23 | hold on
24 | axis([0 12 0 12])
25 |
26 | %............................Create a Target...............................
27 |
28 | backx = Goal_Coordinate(1) - 0.15;backy1 = Goal_Coordinate(2) + 0.15;
29 | frontx = Goal_Coordinate(1) + 0.15;fronty1 = Goal_Coordinate(2) + 0.15;
30 | middlex = Goal_Coordinate(1);middley1 = Goal_Coordinate(2)+ 0.15;
31 | middley2 = Goal_Coordinate(2)- 0.15;
32 | tri2 = [backx middlex frontx ;backy1 middley1 fronty1 ];
33 | plot(tri2(1,:), tri2(2,:));
34 | tri3 = [middlex middlex ;middley1 middley2];
35 | plot(tri3(1,:), tri3(2,:));
36 |
37 | %.............Create a purple transparent circular Obstacle................
38 |
39 | xc = Obstacle1_Coordinate(1);
40 | yc = Obstacle1_Coordinate(2);
41 | r = 0.2;
42 | x = r*sin(-pi:0.2*pi:pi) + xc;
43 | y = r*cos(-pi:0.2*pi:pi) + yc;
44 | c = [0.6 0 1];
45 | fill(x, y, c, 'FaceAlpha', 0.4)
46 |
47 | xc = Obstacle2_Coordinate(1);
48 | yc = Obstacle2_Coordinate(2);
49 | r = 0.2;
50 | x = r*sin(-pi:0.2*pi:pi) + xc;
51 | y = r*cos(-pi:0.2*pi:pi) + yc;
52 | c = [0.6 0 1];
53 | fill(x, y, c, 'FaceAlpha', 0.4)
54 |
55 | DTG = 1;
56 | while(DTG > 0.6)
57 |
58 | %.............................Create a Robot...........................
59 |
60 | backx = Robot_Coordinate(1) - 0.15;backy1 = Robot_Coordinate(2) + 0.15;
61 | backy2 = Robot_Coordinate(2) - 0.15;frontx = Robot_Coordinate(1) + 0.075;
62 | fronty1 = Robot_Coordinate(2) + 0.15;fronty2 = Robot_Coordinate(2);
63 | tri1 = [backx backx frontx frontx backx frontx;backy2 backy1 fronty1 fronty2 fronty2 backy2];
64 | plot(tri1(1,:), tri1(2,:));
65 |
66 | %.........................Potential Calculations.......................
67 |
68 | J_ObstT = Obstacle(1)*exp(-Obstacle(2)*((Robot_Coordinate(1)-Obstacle1_Coordinate(1))^2+(Robot_Coordinate(2)-Obstacle1_Coordinate(2))^2))
69 | J_ObstT = J_ObstT + Obstacle(1)*exp(-Obstacle(2)*((Robot_Coordinate(1)-Obstacle2_Coordinate(1))^2+(Robot_Coordinate(2)-Obstacle2_Coordinate(2))^2))
70 | J_GoalT = -Goal(1)*exp(-Goal(2)*((Robot_Coordinate(1)-Goal_Coordinate(1))^2+(Robot_Coordinate(2)-Goal_Coordinate(2))^2))
71 | JT = J_ObstT + J_GoalT;
72 |
73 | %...........................Distance to Goal...........................
74 |
75 | DTG = sqrt((Robot_Coordinate(1)-Goal_Coordinate(1))^2+(Robot_Coordinate(2)-Goal_Coordinate(2))^2)
76 |
77 | %...........................Artificial Points..........................
78 |
79 | Theta = zeros(1,NPTS);
80 | Theta(1) = StepDegree;
81 | for i=1:NPTS-1
82 | Theta(i+1) = Theta(i) + StepDegree;
83 | end
84 |
85 | % a = 1;
86 | % b = 360;
87 | % Theta = a + (b-a).*rand(NPTS,1);
88 |
89 | Bacteria_x = zeros(1,NPTS);
90 | Bacteria_y = zeros(1,NPTS);
91 | for i=1:NPTS
92 | Bacteria_x(i) = Robot_Coordinate(1) + (Step_Size*cos(pi*Theta(i)/180));
93 | Bacteria_y(i) = Robot_Coordinate(2) + (Step_Size*sin(pi*Theta(i)/180));
94 | end
95 | plot(Bacteria_x,Bacteria_y,'.')
96 | pause(0.1);
97 | %........Calculating Cost Function and Distance of Artificial Point....
98 |
99 | [m,n] = size(Bacteria_x);
100 | J_ObstT_Bacteria = zeros(1,n);
101 | J_GoalT_Bacteria = zeros(1,n);
102 | JT_Bacteria = zeros(1,n);
103 | DTG_Bacteria = zeros(1,n);
104 | for i=1:n
105 | J_ObstT_Bacteria(i) = Obstacle(1)*exp(-Obstacle(2)*((Bacteria_x(i)-Obstacle1_Coordinate(1))^2+(Bacteria_y(i)-Obstacle1_Coordinate(2))^2));
106 | J_ObstT_Bacteria(i) = J_ObstT_Bacteria(i) + Obstacle(1)*exp(-Obstacle(2)*((Bacteria_x(i)-Obstacle2_Coordinate(1))^2+(Bacteria_y(i)-Obstacle2_Coordinate(2))^2));
107 | J_GoalT_Bacteria(i) = -Goal(1)*exp(-Goal(2)*((Bacteria_x(i)-Goal_Coordinate(1))^2+(Bacteria_y(i)-Goal_Coordinate(2))^2));
108 | JT_Bacteria(i) = J_ObstT_Bacteria(i) + J_GoalT_Bacteria(i);
109 | DTG_Bacteria(i) = sqrt((Bacteria_x(i)-Goal_Coordinate(1))^2+(Bacteria_y(i)-Goal_Coordinate(2))^2);
110 | end
111 | J_GoalT_Bacteria;
112 |
113 | %....................Error in Cost Function & Distance.................
114 |
115 | err_J = zeros(1,n);
116 | err_DTG = zeros(1,n);
117 | Fitness = zeros(1,n);
118 | for i=1:n
119 | err_J(i) = JT_Bacteria(i) - JT;
120 | err_DTG(i) = DTG_Bacteria(i) - DTG;
121 | Fitness(i) = -err_DTG(i);
122 | end
123 | Fitness;
124 | %..........................Best Point Selection........................
125 |
126 | Check = 0;
127 | for t=1:n
128 | [~,k]= max(Fitness);
129 | if err_J(k) < 0
130 | if err_DTG(k)<0
131 | Check = Check + 1;
132 | Robot_Coordinate(1) = Bacteria_x(k);
133 | Robot_Coordinate(2) = Bacteria_y(k);
134 | DTG = sqrt((Robot_Coordinate(1)-Goal_Coordinate(1))^2+(Robot_Coordinate(2)-Goal_Coordinate(2))^2);
135 | else
136 | Fitness(k) = 0;
137 | end
138 | else
139 | Fitness(k) = 0;
140 | end
141 | end
142 | Check;
143 | if Check == 0
144 | disp(Error_Message)
145 | else
146 | disp(Confirm_Message)
147 | end
148 | All_Data = zeros(NPTS,3);
149 | All_Data(:,1) = Robot_Coordinate(1);
150 | All_Data(:,2) = Robot_Coordinate(2);
151 | All_Data(:,3) = Theta;
152 | All_Data(:,4) = J_ObstT_Bacteria;
153 | All_Data(:,5) = J_GoalT_Bacteria;
154 | All_Data(:,6) = JT_Bacteria;
155 | All_Data(:,7) = err_J;
156 | All_Data(:,8) = err_DTG;
157 | All_Data;
158 | %.................................Plot.................................
159 |
160 | %............................Create a Target...........................
161 | backx = Goal_Coordinate(1) - 0.15;backy1 = Goal_Coordinate(2) + 0.15;
162 | frontx = Goal_Coordinate(1) + 0.15;fronty1 = Goal_Coordinate(2) + 0.15;
163 | middlex = Goal_Coordinate(1);middley1 = Goal_Coordinate(2)+ 0.15;
164 | middley2 = Goal_Coordinate(2)- 0.15;
165 | tri2 = [backx middlex frontx ;backy1 middley1 fronty1 ];
166 | plot(tri2(1,:), tri2(2,:));
167 | hold on
168 | tri3 = [middlex middlex ;middley1 middley2];
169 | plot(tri3(1,:), tri3(2,:));
170 |
171 | %.............Create a purple transparent circular Obstacle............
172 |
173 | xc = Obstacle1_Coordinate(1);
174 | yc = Obstacle1_Coordinate(2);
175 | r = 0.2;
176 | x = r*sin(-pi:0.2*pi:pi) + xc;
177 | y = r*cos(-pi:0.2*pi:pi) + yc;
178 | c = [0.6 0 1];
179 | fill(x, y, c, 'FaceAlpha', 0.4)
180 |
181 | xc = Obstacle2_Coordinate(1);
182 | yc = Obstacle2_Coordinate(2);
183 | r = 0.2;
184 | x = r*sin(-pi:0.2*pi:pi) + xc;
185 | y = r*cos(-pi:0.2*pi:pi) + yc;
186 | c = [0.6 0 1];
187 | fill(x, y, c, 'FaceAlpha', 0.4)
188 |
189 | %................................Outputs...............................
190 | % J_ObstT
191 | % J_GoalT
192 | % JT
193 | % DTG
194 | % JT_Bacteria
195 | % DTG_Bacteria
196 | % err_J
197 | % err_DTG
198 | % Check
199 | % k
200 | % Fitness
201 | end
202 | Variables = {'Rx' 'Ry' 'Theta' 'J_Obst_B' 'J_GoalT' 'JT' 'err_J' 'err_DTG'};
203 | filename = 'E:\PathPlanning_Yasim\RPO First Iteration\RPO_Data.xlsx';
204 | xlswrite(filename,Variables)
205 | xlswrite(filename,All_Data,1,'A2');
206 |
--------------------------------------------------------------------------------