├── README.md └── RunAPF.m /README.md: -------------------------------------------------------------------------------- 1 | # ArtificialPotentialMatlab 2 | 3 | This algorithm runs a random obstable voxel map generation and uses classical artificial potential field equations to drive a robot. 4 | 5 | Related Paper: https://ieeexplore.ieee.org/abstract/document/10639980 Visual Autonomous Quadrotor Navigation Using an Improved Artificial Potential Field 6 | 7 | https://youtu.be/p4NFpEkLnwE 8 | 9 | 10 | https://user-images.githubusercontent.com/58446071/197935978-93dff721-a33b-41c5-8bc6-21cd546c821c.mp4 11 | 12 | As classical issues, local minima, narrow passages, and Goals Non-Reachable with Obstacle near by: 13 | 14 | 15 | Goals Non-Reachable with Obstacle near by: 16 | 17 | https://user-images.githubusercontent.com/58446071/197936159-c4c73696-1708-434d-afbc-f5945b8a2ceb.mp4 18 | 19 | LOCAL MINIMA: 20 | 21 | https://user-images.githubusercontent.com/58446071/197936161-9e8d1742-f3a3-4c9e-8f34-ea12a2f089f8.mp4 22 | 23 | Narrow passages: 24 | 25 | https://user-images.githubusercontent.com/58446071/197936162-39298b4c-82bd-4f47-985c-8a63b6afcdfa.mp4 26 | 27 | -------------------------------------------------------------------------------- /RunAPF.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | % 6 | % rng(7,"twister"); 7 | omap3D = occupancyMap3D(1); %keep this as 1 to work 8 | mapWidth = 30; %meters 9 | mapLength = 30; 10 | numberOfObstacles = 45; 11 | obstacleNumber = 1; 12 | 13 | while obstacleNumber <= numberOfObstacles 14 | width = randi([1 5],1); 15 | lengthh = randi([1 5],1); % can be changed as necessary to create different occupancy maps. 16 | height = randi([1 5],1); 17 | xPosition = randi([0 mapWidth-width],1); 18 | yPosition = randi([0 mapLength-lengthh],1); 19 | 20 | [xObstacle,yObstacle,zObstacle] = meshgrid(xPosition:xPosition+width,yPosition:yPosition+lengthh,0:height); 21 | xyzObstacles = [xObstacle(:) yObstacle(:) zObstacle(:)]; 22 | 23 | checkIntersection = false; 24 | for i = 1:size(xyzObstacles,1) 25 | if checkOccupancy(omap3D,xyzObstacles(i,:)) == 1 26 | checkIntersection = true; 27 | break 28 | end 29 | end 30 | if checkIntersection 31 | continue 32 | end 33 | 34 | setOccupancy(omap3D,xyzObstacles,1) 35 | 36 | obstacleNumber = obstacleNumber + 1; 37 | end 38 | % ground as obstacle 39 | [xGround,yGround,zGround] = meshgrid(0:mapWidth,0:mapLength,0); 40 | xyzGround = [xGround(:) yGround(:) zGround(:)]; 41 | setOccupancy(omap3D,xyzGround,1) 42 | 43 | 44 | rho0 = 0.3; %distance of influence, adaptative given velocity and freespace? 45 | ka = 1; 46 | krep = 1; 47 | goal.pos=[20 20 6]; 48 | box = collisionBox(0.4,0.4,0.1); % drone as a box XYZ 49 | currentpos=[0.5 0.5 2]; 50 | box.Pose = trvec2tform(currentpos); 51 | 52 | movement=[0 0 0]; 53 | Xhistory=0; 54 | Yhistory=0; 55 | Zhistory=0; 56 | fh = figure(); 57 | fh.WindowState = 'maximized'; 58 | pause(1) 59 | while norm(goal.pos-currentpos)>0.1 60 | 61 | 62 | bpOpts = occupancyMap3DCollisionOptions(ReturnDistance=true); 63 | [collisionStatus,details] = checkMapCollision(omap3D,box,bpOpts); 64 | rho = details.DistanceInfo.Distance; 65 | h=0.01; 66 | trvec = tform2trvec(box.Pose); 67 | 68 | box.Pose = trvec2tform([trvec(1) trvec(2) trvec(3)]); 69 | newX = trvec2tform([trvec(1)+h trvec(2) trvec(3)]); 70 | boxCopyx = collisionBox(box.X,box.Y,box.Z); 71 | boxCopyx.Pose=box.Pose; 72 | boxCopyx.Pose=newX; 73 | [collisionStatus,detailsX] = checkMapCollision(omap3D,boxCopyx,bpOpts); 74 | rhoX = detailsX.DistanceInfo.Distance; 75 | newY = trvec2tform([trvec(1) trvec(2)+h trvec(3)]); 76 | boxCopyy = collisionBox(box.X,box.Y,box.Z); 77 | boxCopyy.Pose=box.Pose; 78 | boxCopyy.Pose=newY; 79 | [collisionStatus,detailsY] = checkMapCollision(omap3D,boxCopyy,bpOpts); 80 | rhoY = detailsY.DistanceInfo.Distance; 81 | newZ = trvec2tform([trvec(1) trvec(2) trvec(3)+h]); 82 | boxCopyz = collisionBox(box.X,box.Y,box.Z); 83 | boxCopyz.Pose=box.Pose; 84 | boxCopyz.Pose=newZ; 85 | [collisionStatus,detailsZ] = checkMapCollision(omap3D,boxCopyz,bpOpts); 86 | rhoZ = detailsZ.DistanceInfo.Distance; 87 | % forward difference for gradient 88 | gradrho = [(rhoX-rho)/h (rhoY-rho)/h (rhoZ-rho)/h]; 89 | 90 | 91 | 92 | bpWitnessptsSphere = details.DistanceInfo.WitnessPoints; 93 | 94 | 95 | show(omap3D) 96 | hold on 97 | show(box) 98 | plot3(bpWitnessptsSphere(1,:),bpWitnessptsSphere(2,:),bpWitnessptsSphere(3,:),LineWidth=2,Color='r') 99 | 100 | state.pos=[trvec(1) trvec(2) trvec(3)]; 101 | 102 | Fa = -ka*(state.pos-goal.pos); 103 | if rho>rho0 104 | Fr = [0 0 0]; 105 | else 106 | Fr = krep*(((1/rho)-(1/rho0))*(1/rho)^2)*gradrho; 107 | end 108 | % centerobstacle = bpWitnessptsSphere(:,1)'; 109 | % Rspin = cross((goal.pos-state.pos),(centerobstacle-state.pos)); 110 | 111 | %plot3([box.X box.X+10*Fr(1)],[box.Y box.Y+10*Fr(2)],[box.Z box.Z+10*Fr(3)],LineWidth=2,Color='k') 112 | [X,Y,Z] = sphere; 113 | r = 0.2; 114 | X = X * r; 115 | Y = Y * r; 116 | Z = Z * r; 117 | surf(X+goal.pos(1),Y+goal.pos(2),Z+goal.pos(3),'FaceColor','k','EdgeColor','none') 118 | 119 | quiver3(trvec(1),trvec(2),trvec(3),Fr(1)/norm(Fr),Fr(2)/norm(Fr),Fr(3)/norm(Fr),LineWidth=3,Color='k') 120 | quiver3(trvec(1),trvec(2),trvec(3),Fa(1)/norm(Fa),Fa(2)/norm(Fa),Fa(3)/norm(Fa),LineWidth=3,Color='b') 121 | quiver3(trvec(1),trvec(2),trvec(3),(Fr(1)+Fa(1))/norm(Fa+Fr),(Fr(2)+Fa(2))/norm(Fa+Fr),(Fr(3)+Fa(3))/norm(Fa+Fr),LineWidth=3,Color='y') 122 | % quiver3(trvec(1),trvec(2),trvec(3),Rspin(1)/norm(Rspin),Rspin(2)/norm(Rspin),Rspin(3)/norm(Rspin),LineWidth=3,Color='g') 123 | 124 | 125 | axis equal 126 | plot3(Xhistory,Yhistory,Zhistory,'r') 127 | view(-15.6658,30.0343) 128 | 129 | legend({'Obstacle','Obstacle','Nearest Obstacle','Goal','Repulsive Force','Attractive Force','Total Force','Path'}) 130 | 131 | hold off 132 | 133 | movement = (Fr+Fa); 134 | movement = 0.1*movement/norm(movement); 135 | trvec = trvec+movement; 136 | currentpos = [trvec(1) trvec(2) trvec(3)]; 137 | box.Pose=trvec2tform([trvec(1) trvec(2) trvec(3)]); 138 | Xhistory=[Xhistory trvec(1)]; 139 | Yhistory=[Yhistory trvec(2)]; 140 | Zhistory=[Zhistory trvec(3)]; 141 | pause(0.001) 142 | 143 | end --------------------------------------------------------------------------------