Repository: LenaShengzhen/AerialRobotics Branch: main Commit: b3fe62f2df62 Files: 32 Total size: 56.2 KB Directory structure: gitextract_60fphljc/ ├── .gitmodules ├── Motion_Planning/ │ ├── 0Maps/ │ │ ├── 3dCorner.txt │ │ ├── Z_3blocks.txt │ │ └── ellipsoid.txt │ ├── 1Map_Generation/ │ │ ├── GridMap.m │ │ └── PointCloudMap.m │ ├── 2Path_Planning/ │ │ ├── JPS_3D.m │ │ └── simplify_path.m │ ├── 3Safe_Flight_Corridors/ │ │ ├── Ellipsoid.m │ │ ├── Hyperplane.m │ │ ├── LineSegment.m │ │ ├── MultipleSegments.m │ │ ├── Polyhedron.m │ │ ├── SFC_3D.m │ │ └── rotationMatrix.m │ ├── 4Time_Allocation/ │ │ ├── averageSpeed_ta.m │ │ └── trapezoidalSpeed_ta.m │ ├── 5Trajectory_Planning/ │ │ ├── QPbyUseSFC.m │ │ ├── TrajectoryPlanning.m │ │ ├── calc_Q.m │ │ └── calc_dc.m │ ├── demo/ │ │ ├── demo_3dCorner.m │ │ ├── demo_Z_3blocks.m │ │ └── demo_ellipsoid.m │ ├── draw/ │ │ ├── draw_ObcPoints.m │ │ ├── makeGifAndJpg.m │ │ └── plot_path.m │ ├── init_data.m │ └── runsim.m ├── Motion_Planning.md ├── README.md └── Trajectory_Planning.md ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitmodules ================================================ [submodule "Motion_Planning/QuadrotorSimulation"] path = Motion_Planning/QuadrotorSimulation url = https://github.com/LenaShengzhen/QuadrotorSimulation.git ================================================ FILE: Motion_Planning/0Maps/3dCorner.txt ================================================ # map 3dCorner boundary 0.0 0.0 0.0 3.1 3.1 3.0 # downblock block 2.0 1.5 1.8 3.0 2.5 3.0 30.00 144.00 255.00 # midblock block 2.3 1.5 1.4 3.0 2.5 1.8 30.00 144.00 255.00 # upblock block 2.0 1.5 0.1 3.0 2.5 1.4 30.00 144.00 255.00 ================================================ FILE: Motion_Planning/0Maps/Z_3blocks.txt ================================================ # map Z_3blocks: Turn left and right boundary -2.00 0.0 0.00 14.00 3 3.50 block 2.25 0 0 2.65 1.9 3.4 30.00 144.00 255.00 block 0.05 1.1 0 0.45 3 3.4 0.00 191.00 255.00 block 4.5 1.1 0 4.9 3 3.4 0.00 191.00 255.00 ================================================ FILE: Motion_Planning/0Maps/ellipsoid.txt ================================================ # map ellipsoid boundary 0.0 0.0 0.0 3.1 3.1 3.0 block 1.0 0.5 0.1 2.0 0.7 3.0 0.000000 255.000000 0.000000 block 0 1.5 0 1.0 1.7 3.0 0.000000 255.000000 0.000000 block 2.0 1.5 0.1 3.0 1.7 3.0 0.000000 255.000000 0.000000 ================================================ FILE: Motion_Planning/1Map_Generation/GridMap.m ================================================ classdef GridMap < handle properties boundary; % size of Metric Map(leftdownPoint + rightupPoint) blocks; % coordinate of obstacles on the Metric Map res; % resolution margin; maxIndex; % max index of ijk of GridMap occ_map; % flag obstacles on the GridMap end methods function obj = GridMap() end function load_map(obj, filename, xy_res, z_res, margin) fid = fopen(filename); tline = fgets(fid); obj.blocks = []; while ischar(tline) % skip empty line and comments if numel(tline) > 1 & tline(1)~='#' % convert char array to string if strncmpi(tline, 'boundary', 8) boundarys = strread(tline(9:end)); end if strncmpi(tline, 'block', 5) block = strread(tline(6:end)); assert(size(block,2) == 9); obj.blocks = [obj.blocks; block]; end end tline = fgets(fid); end obj.boundary.ld = boundarys(1:3); obj.boundary.ru = boundarys(4:6); obj.res = [xy_res xy_res z_res]; obj.margin = margin; obj.maxIndex = ceil(abs( (obj.boundary.ru - obj.boundary.ld)./obj.res )); end % obstacles = block + margin function flag_obstacles(obj) obj.occ_map = false(obj.maxIndex); for i = 1 : size(obj.blocks,1) block = obj.blocks(i, :); leftdownPoint = [min(block(1), block(4)) min(block(2), block(5)) min(block(3), block(6))]; rightUpPoint = [max(block(1), block(4)) max(block(2), block(5)) max(block(3), block(6))]; obj.blocks(i, 1:3) = leftdownPoint; obj.blocks(i, 4:6) = rightUpPoint; xyzs = obj.points_to_idx(leftdownPoint - obj.margin); xyze = obj.points_to_idx(rightUpPoint + obj.margin); obj.occ_map(xyzs(1):xyze(1), xyzs(2):xyze(2), xyzs(3):xyze(3)) = 1; end end function ijk = points_to_idx(obj, xyz) assert(size(xyz,1) > 0); ijk = floor((xyz - obj.boundary.ld)./obj.res + 1); ijk = min(max(ijk, 1), obj.maxIndex); end function xyz = idx_to_points(obj, ijk) assert(size(ijk,1) > 0); xyz = obj.boundary.ld + (ijk - 1 + 0.5).* obj.res; end function ret = idOverflowCheck(obj, i, j, k) ret = (i < 1 || i > obj.maxIndex(1) || j < 1 || j > obj.maxIndex(2) || k < 1 || k > obj.maxIndex(3)); end % check points collide function C = collide(obj, points) ijk = obj.points_to_idx(points); C = false(size(ijk, 1), 1); for i = 1 : size(ijk, 1) id = ijk(i,:); C(i) = obj.occ_map(id(1), id(2), id(3)) ~= 0; end end %% generate intermedia points function pts = generateiIntermediaPoints(obj, start_pos, end_pos) %% generate Tiny direction vector dd = end_pos - start_pos; dir = obj.res(1)*(dd/norm(dd, 2))/(100); %% generate intermedia points lenxy = sqrt(sum((start_pos(1:2) - end_pos(1:2)).^2)); lenz = sqrt(sum((start_pos(3) - end_pos(3)).^2)); n_pts = int32(max(lenxy/obj.res(1), lenz/obj.res(3))) + 3; pts = [linspace(start_pos(1), end_pos(1), n_pts); linspace(start_pos(2), end_pos(2), n_pts); linspace(start_pos(3), end_pos(3), n_pts)]'; %% mid-point + Tiny direction vector for j = 2 : size(pts,1) - 1 m1 = mod(pts(j,1), obj.res(1)); m2 = mod(pts(j,2), obj.res(1)); m3 = mod(pts(j,3), obj.res(1)); if(m1*m2*m3 == 0) pts(j,:) = pts(j,:) + dir; end end end end end ================================================ FILE: Motion_Planning/1Map_Generation/PointCloudMap.m ================================================ % blocks of Metric Map change to point cloud function obps = PointCloudMap(blocks, margin) obps = []; density = 2 * margin; for id = 1 : size(blocks, 1) leftdownPoint = blocks(id, 1:3) - margin; rightUpPoint = blocks(id, 4:6) + margin; plane_Z = []; for i = leftdownPoint(1,1) : density : rightUpPoint(1,1) for j = leftdownPoint(1,2) : density : rightUpPoint(1,2) pz = leftdownPoint(1,3); plane_Z = [plane_Z; [i, j, pz]]; end end plane_Y = []; for i = leftdownPoint(1,1) : density : rightUpPoint(1,1) for k = leftdownPoint(1,3) : density : rightUpPoint(1,3) py = leftdownPoint(1,2); plane_Y = [plane_Y; [i, py, k]]; end end plane_X = []; for j = leftdownPoint(1,2) : density : rightUpPoint(1,2) for k = leftdownPoint(1,3) : density : rightUpPoint(1,3) px = leftdownPoint(1,1); plane_X = [plane_X; [px, j, k]]; end end long = rightUpPoint(1,1) - leftdownPoint(1,1); width = rightUpPoint(1,2) - leftdownPoint(1,2); high = rightUpPoint(1,3) - leftdownPoint(1,3); obps = [obps; plane_X; plane_X + [long 0 0]; plane_Y; plane_Y + [0 width 0]; plane_Z; plane_Z + [0 0 high]; ]; end end ================================================ FILE: Motion_Planning/2Path_Planning/JPS_3D.m ================================================ function [path] = JPS_3D(map, start, goal) map_start = map.points_to_idx(start); map_goal = map.points_to_idx(goal); map_visit = map.occ_map; fn = [0]; % fn = gn(EuclideanDistance) + hn(ManhattanDistance) all_direction = make_all_dir(); nodeStart = struct( ... 'pos', map_start, ... % index of the point on the grid-map 'fa', 1, ... % fa records the index of the parent node in the closelist 'id', 1, ... % id records the index of node in closelist 'dir', all_direction); openlist = [nodeStart]; closelist = [nodeStart]; map_visit(map_start(1), map_start(2), map_start(3)) = 1; findgoal = 0; while isempty(openlist) == 0 [~, temid] = min(fn); node = openlist(temid); fn(temid) = []; openlist(temid) = []; fa = node.id; [e1, ~] = size(node.dir); for j = 1 : e1 nodeNew = jump(map, map_goal, node.pos, node.dir(j,:)); if (length(nodeNew.pos) == 0 ) continue; end if (map_visit(nodeNew.pos(1), nodeNew.pos(2), nodeNew.pos(3)) == 0) nodeNew.fa = fa; nodeNew.id = length(closelist) + 1; ds = ManhattanDistance(map_goal, nodeNew.pos) + EuclideanDistance(node.pos, nodeNew.pos); fn = [fn ds]; openlist = [openlist; nodeNew]; closelist = [closelist; nodeNew]; map_visit(nodeNew.pos(1), nodeNew.pos(2), nodeNew.pos(3)) = 1; % find the goal if (nodeNew.pos == map_goal) findgoal = 1; break; end end end % find the goal if (findgoal == 1) break; end end path = []; k = length(closelist); while k > 1 renode = closelist(k); float_pos = map.idx_to_points(renode.pos); path = [float_pos; path]; k = renode.fa; end end function dis = EuclideanDistance(pos1, pos2) dis = norm(pos1 - pos2, 2); end function dis = ManhattanDistance(pos1, pos2) dis = norm(pos1 - pos2, 1); end function ret = obCheck(map, pos) x = pos(1); y = pos(2); z = pos(3); ret = map.occ_map(x, y, z); end function all_direction = make_all_dir() x = [1 1 1 -1 -1 -1 0 0]; b = unique(nchoosek(x,3),'rows'); A = []; for i = 1 : 9 A = [A; perms(b(i,:))]; end all_direction = unique(A,'rows'); end function newnode = jump(map, goal, startpos, d) newnode = struct( ... 'pos', [], ... 'fa', 1, ... 'id', 1, ... 'dir', []); dir_Dimension = sum(d.^2); if (dir_Dimension == 0) return; end newpos = step(startpos, d); x = newpos(1); y = newpos(2); z = newpos(3); % ourside the grid on the newpos if outmap(map, newpos) == 1 return; % meet the obstacle on the newpos elseif map.occ_map(x, y, z) == 1 return; elseif newpos == goal newnode.pos = newpos; return; end if dir_Dimension == 2 dside = dir2Dto1D(d); if (obCheck(map, startpos+dside(1,:)) == 1) && (obCheck(map, startpos+dside(2,:)) == 1) return; end end dirs = hasForceNeighbour(map, newpos, d); if ~isempty(dirs) newnode.pos = newpos; newnode.dir = dirs; return; end % go 3d diagonal % sum(d.^2) = d(1)^2 + d(2)^2 + d(3)^2 if(dir_Dimension == 3) % one 3d-diagonal change three 2d-diagonal + three straght line node1 = jump(map, goal, newpos, [0 d(2) d(3)]); node2 = jump(map, goal, newpos, [d(1) 0 d(3)]); node3 = jump(map, goal, newpos, [d(1) d(2) 0]); % three straght line node4 = jump(map, goal, newpos, [d(1) 0 0]); node5 = jump(map, goal, newpos, [0 d(2) 0]); node6 = jump(map, goal, newpos, [0 0 d(3)]); % find a ForceNegihbour if(length(node1.pos) + length(node2.pos) + length(node3.pos) + length(node4.pos) + length(node5.pos) + length(node6.pos) > 0) newnode.pos = newpos; newnode.dir = [d]; newnode.dir = addDir(node1.pos, [0 d(2) d(3)], newnode.dir); newnode.dir = addDir(node2.pos, [d(1) 0 d(3)], newnode.dir); newnode.dir = addDir(node3.pos, [d(1) d(2) 0], newnode.dir); newnode.dir = addDir(node4.pos, [d(1) 0 0], newnode.dir); newnode.dir = addDir(node5.pos, [0 d(2) 0], newnode.dir); newnode.dir = addDir(node6.pos, [0 0 d(3)], newnode.dir); return; end end % go 2d diagonal if(dir_Dimension == 2) node4 = jump(map, goal, newpos, [d(1) 0 0]); node5 = jump(map, goal, newpos, [0 d(2) 0]); node6 = jump(map, goal, newpos, [0 0 d(3)]); % find a ForceNegihbour if(length(node4.pos) + length(node5.pos) + length(node6.pos) > 0) newnode.pos = newpos; newnode.dir = [d]; % Add the direction of find ForceNegihbour newnode.dir = addDir(node4.pos, [d(1) 0 0], newnode.dir); newnode.dir = addDir(node5.pos, [0 d(2) 0], newnode.dir); newnode.dir = addDir(node6.pos, [0 0 d(3)], newnode.dir); return; end end % [go straght] or [go diagonal not find ForceNegihbour] newnode = jump(map, goal, newpos, d); return; end % if pos ~= Null, dirs = [dirs; d]; function dirs = addDir(pos, d, dirs) if(length(pos) > 0) dirs = [dirs; d]; end end function dside = dir2Dto1D(d) dside = [[0 0 0] ; [0 0 0]]; temi = 1; if(d(1) ~= 0) dside(temi,:) = [d(1) 0 0]; temi = temi + 1; end if(d(2) ~= 0) dside(temi,:) = [0 d(2) 0]; temi = temi + 1; end if(d(3) ~= 0) dside(temi,:) = [0 0 d(3)]; end end function ret = outmap(map, pos) ret = false; x = pos(1); y = pos(2); z = pos(3); if(x < 1 || x > map.maxIndex(1) || y < 1 || y > map.maxIndex(2) || z < 1 || z > map.maxIndex(3)) ret = true; end end function ret = isforceNeighbourExist(map, pos, d) ret = false; x = pos(1); y = pos(2); z = pos(3); posNeighbour = pos + d; if outmap(map, pos) == 1 || ... map.occ_map(x, y, z) == 0 || ... outmap(map, posNeighbour) == 1 || ... map.occ_map(posNeighbour(1), posNeighbour(2), posNeighbour(3)) == 1 return; end ret = true; end function dirs = hasForceNeighbour(map, pos, d) dirs = []; if outmap(map, pos + d) == 1 return; end % [go straght] ---------------------------------------------- if(sum(d.^2) == 1) if obCheck(map, pos + d) == 1 return; end d1 = [abs(d(3)) abs(d(1)) abs(d(2))]; d2 = [abs(d(2)) abs(d(3)) abs(d(1))]; dob = [d1; -d1; d2; -d2; d1+d2; -d1-d2; d1-d2; -d1+d2]; for i = 1 : 8 if isforceNeighbourExist(map, pos + dob(i,:), d) == 1 dirs = [dirs; dob(i,:) + d]; end end % go 2D-diagonal ---------------------------------------------- elseif (sum(d.^2) == 2) dside = dir2Dto1D(d); for i = 1 : 2 if isforceNeighbourExist(map, pos - dside(i,:), d - dside(i,:) ) == 1 dirs = [dirs; d - 2*dside(i,:)]; end end d1 = [abs(d(1))-1 abs(d(2))-1 abs(d(3))-1]; if isforceNeighbourExist(map, pos + d1, d) == 1 dirs = [dirs; d + d1]; end if isforceNeighbourExist(map, pos - d1, d) == 1 dirs = [dirs; d - d1]; end % go 3D-diagonal ---------------------------------------------- else % (sum(d.^2) == 3) dirs = hasForceNeighbour(map, pos, [0 d(2) d(3)]); dirs = [dirs; hasForceNeighbour(map, pos, [d(1) 0 d(3)])]; dirs = [dirs; hasForceNeighbour(map, pos, [d(1) d(2) 0])]; dirs = unique(dirs,'rows'); [e1, ~] = size(dirs); for i = 1 : e1 if(dirs(i,:) == d) dirs(i,:) = []; end end end end function newpos = step(pos, d) newpos = pos + d; end ================================================ FILE: Motion_Planning/2Path_Planning/simplify_path.m ================================================ function path = simplify_path(map, path) if size(path,1) == 0, return; end keep_pBooL = false(size(path,1),1); keep_pBooL(1) = 1; keep_pBooL(end) = 1; last_keep = path(1,:); for i = 2:size(path,1)-1 % ~check_line_collision() == 1 : No collision % check_line_collision() == 1 : collision if (~check_line_collision(last_keep, path(i,:)) & check_line_collision(last_keep, path(i+1,:))) last_keep = path(i,:); keep_pBooL(i) = 1; end end path = path(keep_pBooL,:); function c = check_line_collision(start_pos, end_pos) pts = map.generateiIntermediaPoints(start_pos, end_pos); %% Collision detection c = map.collide(pts); c = any(c); end end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/Ellipsoid.m ================================================ classdef Ellipsoid < handle properties C_; % Record the matrix of the ellipse, the semi-axis length of the ellipse without square d_; % Center point of ellipse axes_; % Length of each axis of ellipse invC_; % inv(obj.C_) end methods function obj = Ellipsoid (a,b) obj.d_ = b; obj.setC(a); end function setC(obj, a) obj.C_ = a; obj.invC_ = inv(a); if isnan(obj.invC_), assert(0 >= 42); end if (obj.invC_ == inf), assert(0 >= 42); end end % Calculate distance to the center function dis = dist(obj, pt) dis = sqrt(obj.square_of_dist(pt)); end % Calculate the square of the distance to the center function dis = square_of_dist(obj, pt) dd = obj.invC_ * (pt - obj.d_)'; dis = dot(dd, dd); % dot(A,B) end % Check if the point is inside function ret = inside(obj, pt) ret = false; if (obj.square_of_dist(pt) <= 1) ret = true; end end % Calculate points inside ellipsoid function pinside = points_inside(obj, points) pinside = []; [len, ~]=size(points); for i = 1 : len if(obj.inside(points(i,:))) pinside = [pinside; points(i,:)]; end end end % Find the closest point function p = closest_point(obj, points) p = points(1,:); min_dist = 1e8; [len, ~]=size(points); for i = 1 : len d = obj.dist(points(i,:)); if(d < min_dist) min_dist = d; p = points(i,:); end end end % Find the closest hyperplane from the closest point, function plane = closest_hyperplane(obj, points) closest_pt = obj.closest_point(points); n = ( obj.invC_ * (obj.invC_)' )*(closest_pt - obj.d_)'; plane = Hyperplane(closest_pt, n); end end end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/Hyperplane.m ================================================ classdef Hyperplane < handle properties p_; % point on the plane n_; % Normal of the plane, directional, local_bbox_; end methods function obj = Hyperplane(p, n) obj.p_ = p; obj.n_ = n; end % Calculate the signed distance from point function dis = signed_dist(obj, pt) dis = dot(obj.n_, pt - obj.p_); end % Calculate the distance from point function dis = dist(obj, pt) dis = abs(obj.signed_dist(pt)); end end end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/LineSegment.m ================================================ classdef LineSegment < handle properties p1_; p2_; R_; obs_; ellipsoid_; polyhedron_; local_bbox_; % Bounding Box of map epsilon_; end methods function obj = LineSegment(p1, p2) obj.p1_ = p1; obj.p2_ = p2; obj.epsilon_ = 1e-10; end function dilate(obj, radius) obj.find_ellipsoid(radius); obj.find_polyhedron(); obj.add_local_bbox(obj.polyhedron_); end function set_local_bbox(obj, local_bbox) obj.local_bbox_ = local_bbox; end function set_obs(obj, obs) Vs = Polyhedron(); obj.add_local_bbox(Vs); obj.obs_ = Vs.points_inside(obs); end function add_local_bbox(obj, Vs) r = norm(obj.p2_ - obj.p1_)/2; dir = (obj.p2_ - obj.p1_)/norm(obj.p2_ - obj.p1_); dir_h = [dir(2) -dir(1) 0]; if(norm(dir_h) == 0) dir_h = [-1 0 0]; end dir_h = dir_h/norm(dir_h); % along x pp1 = obj.p1_ + dir_h*(r); pp2 = obj.p1_ - dir_h*(r); Vs.add(Hyperplane(pp1, dir_h)); Vs.add(Hyperplane(pp2, -dir_h)); % along y pp3 = obj.p2_ + dir*(r); pp4 = obj.p1_ - dir*(r); Vs.add(Hyperplane(pp3, dir)); Vs.add(Hyperplane(pp4, -dir)); % along z, dir_v = [0 0 0]; dir_v(1) = dir(2) * dir_h(3) - dir(3) * dir_h(2); dir_v(2) = dir(3) * dir_h(1) - dir(1) * dir_h(3); dir_v(3) = dir(1) * dir_h(2) - dir(2) * dir_h(1); pp5 = obj.p1_ + dir_v*(r); pp6 = obj.p1_ - dir_v*(r); Vs.add(Hyperplane(pp5, dir_v)); Vs.add(Hyperplane(pp6, -dir_v)); end % 3D function find_ellipsoid(obj, offset_x) f = norm(obj.p1_ - obj.p2_)/2; C = [f 0 0; 0 f 0; 0 0 f]; % f is the radius of the ellipse, the initialized ellipse is a circle % C = [f 0 0] % [0 f 0] % [0 0 f] % h = (R^T)*x, (1 0 0)^T = R^T*(p2_ - p1_) => (p2_ - p1_) = R*(1 0 0)^T Ri = rotationMatrix([1 0 0]', (obj.p2_ - obj.p1_)'); C = Ri * C * (Ri'); axes = [f f f]; obj.R_ = Ri; E = Ellipsoid(C, (obj.p1_ + obj.p2_)/2 ); obs = E.points_inside(obj.obs_); obs_inside = obs; % decide short axes-1 while (~isempty(obs_inside)) cp = E.closest_point(obs_inside); rcp = (Ri')*(cp - E.d_)'; % Generate a new ellipse if(abs(rcp(1)) < axes(1)) newy = sqrt((rcp(2))^2 + (rcp(3))^2); axes(2) = newy / sqrt(1 - (rcp(1)/axes(1))^2); end new_C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(2)]; E.setC(Ri * new_C * (Ri')); % Delete all the points outside the new ellipse obs_new = []; [len, ~] = size(obs_inside); for i = 1 : len % constexpr decimal_t epsilon_ = 1e-10; if(1 - E.dist(obs_inside(i,:)) > obj.epsilon_ ) obs_new = [obs_new; obs_inside(i,:)]; end end obs_inside = obs_new; end % decide short axes-2 C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(3)]; E.setC(Ri * C * (Ri')); obs_inside = E.points_inside(obs); while (~isempty(obs_inside)) cp = E.closest_point(obs_inside); rcp = (Ri')*(cp - E.d_)'; dd = sqrt( 1 - (rcp(1)/axes(1))^2 - (rcp(2)/axes(2))^2 ); if(dd > obj.epsilon_) axes(3) = abs(rcp(3)) / dd; end new_C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(3)]; E.setC(Ri * new_C * (Ri')); % Delete all the points outside the new ellipse obs_new = []; [len, ~] = size(obs_inside); for i = 1 : len % constexpr decimal_t epsilon_ = 1e-10; if(1 - E.dist(obs_inside(i,:)) > obj.epsilon_ ) obs_new = [obs_new; obs_inside(i,:)]; end end obs_inside = obs_new; end E.axes_ = axes; obj.ellipsoid_ = E; end function find_polyhedron(obj) Vs = Polyhedron(); obs_remain = obj.obs_; while(length(obs_remain)) plane = obj.ellipsoid_.closest_hyperplane(obs_remain); Vs.add(plane); obs_tmp = []; [len, ~] = size(obs_remain); for i = 1 : len p = obs_remain(i,:); if(plane.signed_dist(p) < 0) obs_tmp = [obs_tmp; p]; end end obs_remain = obs_tmp; end obj.polyhedron_ = Vs; end end end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/MultipleSegments.m ================================================ classdef MultipleSegments < handle properties lines_; obs_; local_bbox_; end methods function obj = MultipleSegments(local_bbox) obj.local_bbox_ = local_bbox; end function set_obs(obj, obs) obj.obs_ = obs; end function dilate(obj, path, offset_x) [len, ~] = size(path); obj.lines_ = cell(1, len-1); for i = 1 : (len - 1) p1 = path(i,:); p2 = path(i+1,:); obj.lines_{i} = LineSegment(p1, p2); obj.lines_{i}.set_local_bbox(obj.local_bbox_); obj.lines_{i}.set_obs(obj.obs_); obj.lines_{i}.dilate(offset_x); end end function drawLines(obj) len = length(obj.lines_); for i = 1 : len obj.lines_{i}.p1_; end end function drawEllipsoids(obj) hold on; len = length(obj.lines_); for i = 1 : len center = obj.lines_{i}.ellipsoid_.d_; R = obj.lines_{i}.R_; axes = obj.lines_{i}.ellipsoid_.axes_; obj.draw_Ellipsoid(center, axes, R'); end hold off; end function draw_Ellipsoid(obj, center, axes, R) [X, Y, Z] = ellipsoid(0,0,0, axes(1), axes(2), axes(3)); [row, col] = size(X); A = [X(:), Y(:), Z(:)]; B = A*R + center; X = reshape(B(:,1),row,col); Y = reshape(B(:,2),row,col); Z = reshape(B(:,3),row,col); mesh(X, Y, Z, 'FaceAlpha','0.5'); end % draw the point of obs on the polyhedron function drawPlanePoint(obj) hold on; len = length(obj.lines_); planesCell = obj.lines_{len}.polyhedron_.polys_; [~, lenj] = size(planesCell); for i = 1 : lenj point = planesCell{i}.p_; plot3(point(1), point(2), point(3),'r*'); end hold off; end function drawpolyhedron(obj) hold on; len = length(obj.lines_); for i = len : len planesCell = obj.lines_{i}.polyhedron_.polys_; p1 = obj.lines_{i}.p1_; p2 = obj.lines_{i}.p2_; r = norm(p2 - p1)/2; pmid = (p2 + p1)/2; [~, lenj] = size(planesCell); for j = 1 : lenj obj.drawPlane(planesCell{j}.n_, planesCell{j}.p_, pmid, r); end end hold off; end function drawPlane(obj, n, p, pmid, r) r = r*2; xL = pmid(1) - r; xR = pmid(1) + r; yU = pmid(2) + r; yD = pmid(2) - r; [X,Y]=meshgrid(xL:0.5:xR, yD:0.5:yU); Z = p(3) - ((X - p(1))*n(1)+(Y - p(2))*n(2))/n(3); surf(X, Y, Z, 'FaceAlpha','0.5','EdgeColor','flat','FaceColor', '[0 1 1]'); end end end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m ================================================ classdef Polyhedron < handle properties polys_; epsilon_; end methods function obj = Polyhedron() obj.polys_ = cell(0); obj.epsilon_ = 1e-10; end % Append Hyperplane function add(obj, plane) obj.polys_{end+1} = plane; end % Check if the point is inside polyhedron, function isinside = inside(obj, pt) isinside = false; [len, ~]=size(obj.polys_); for i = 1 : len if(obj.polys_{i}.signed_dist(pt) > obj.epsilon_) return; end end isinside = true; end % Calculate points inside polyhedron function pinside = points_inside(obj, points) pinside = []; [len, ~]=size(points); for i = 1 : len if(obj.inside(points(i,:))) pinside = [pinside; points(i,:)]; end end end end end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/SFC_3D.m ================================================ function decomp = SFC_3D(path, obs, boundary) % Initialize MultipleSegments local_bbox = boundary; offset_x = 0; decomp = MultipleSegments(local_bbox); decomp.set_obs(obs); decomp.dilate(path, offset_x); end ================================================ FILE: Motion_Planning/3Safe_Flight_Corridors/rotationMatrix.m ================================================ function R = rotationMatrix(v1col, v2col) if (norm(v2col) == 0), assert(0 >= 42); end % There is no concept of collinear zero vector v1 = v1col'; v2 = v2col'; nv1 = v1/norm(v1); nv2 = v2/norm(v2); if norm(nv1+nv2)==0 q = [0 0 0 0]; else u = cross(nv1,nv2); % If nv1 and nv2 are collinear, u = [0 0 0] un = norm(u); % If u = [0 0 0], norm(u) = 0; if(un == 0) R = [1 0 0; 0 1 0; 0 0 1]; return; end u = u/un; theta = acos(sum(nv1.*nv2))/2; q = [cos(theta) sin(theta)*u]; end R=[2*q(1).^2-1+2*q(2)^2 2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)-q(1)*q(3)); 2*(q(2)*q(3)-q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)+q(1)*q(2)); 2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)-q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2]; R = R'; end ================================================ FILE: Motion_Planning/4Time_Allocation/averageSpeed_ta.m ================================================ % averageSpeed_TimeAllocation: % avg_v: the estimated average speed function [ts, total_time] = averageSpeed_ta(path, avg_v) path_seg_len = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2)); path_len = sum(path_seg_len); total_time = path_len/avg_v; ts = cumsum(path_seg_len); ts = ts/ts(end); ts = [0; ts]'; ts = ts*total_time; end ================================================ FILE: Motion_Planning/4Time_Allocation/trapezoidalSpeed_ta.m ================================================ % trapezoidalSpeed_TimeAllocation: % avg_v: the estimated average speed % acc: Acceleration trapezoidalSpeed function [ts, total_time] = trapezoidalSpeed_ta(path, avg_v, acc) t_acc = avg_v/acc; s_acc = 0.5*acc*(t_acc^2); path_seg_len = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2)); path_len = sum(path_seg_len); total_time = path_len/avg_v; ts = cumsum(path_seg_len); ts = ts/ts(end); ts = [0; ts]'; ts = ts*total_time; % trapezoid Area = 0.5(a + b) h % math ep % a1 = ts(2) - t_acc; % b1 = a1 - speed_h/acc; % ep1 = 0.5*(a1 + b1)*speed_h + t_add1*(avg_v + speed_h) == s_acc; % a2 = ts(end) - ts(end-1) - t_acc; % b2 = a2 - speed_h/acc; % ep2 = 0.5*(a2 + b2)*speed_h + t_add2*(avg_v + speed_h) == s_acc; % t_mid = (ts(end-1) - ts(2)); % ep3 = (t_mid-t_add1-t_add2)*speed_h - (t_add1 + t_add2)*avg_v == 0; %% solve syms t_add1 t_add2 speed_h; a1 = ts(2) - t_acc; ep1 = 0.5*(a1 + a1 - speed_h/acc)*speed_h + t_add1*(avg_v + speed_h) == s_acc; a2 = ts(end) - ts(end-1) - t_acc; ep2 = 0.5*(a2 + a2 - speed_h/acc)*speed_h + t_add2*(avg_v + speed_h) == s_acc; t_mid = (ts(end-1) - ts(2)); ep3 = (t_mid-t_add1-t_add2)*speed_h - (t_add1 + t_add2)*avg_v == 0; [x0,y0,z0] = solve(ep1,ep2,ep3, t_add1, t_add2, speed_h); ans = double([x0,y0,z0]); sort_ans = sortrows(ans,3); ts(2) = ts(2) + sort_ans(1,1); ts(end-1) = ts(end-1) - sort_ans(1,2); if(path_len < 2*s_acc), assert(1 >= 2); end if(path_seg_len(1) < s_acc), assert(1 >= 2); end if(path_seg_len(end) < s_acc), assert(1 >= 2); end end ================================================ FILE: Motion_Planning/5Trajectory_Planning/QPbyUseSFC.m ================================================ function X = QPbyUseSFC(waypts, ts, decomp) %% condition traj.n_order = 7; traj.n_poly = size(waypts, 1) - 1; traj.p0 = waypts(1,:); traj.pe = waypts(end,:); traj.v0 = [0,0,0]; traj.ve = [0,0,0]; traj.a0 = [0,0,0]; traj.ae = [0,0,0]; %% trajectory plan [minSnapValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp); disp(['minSnapValue is : ',num2str(minSnapValue)]); X = [px py pz]; end % v0 = [1*3] function [minValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp) % Dimension dim = 3; n_order = traj.n_order; p0 = traj.p0; pe = traj.pe; v0 = traj.v0; ve = traj.ve; a0 = traj.a0; ae = traj.ae; n_poly = traj.n_poly; n_coef = n_order+1; %% compute Q, Q is a Symmetric matrix % Q_1 = [0 0 0] % [0 f f] % [0 f f] % f = integral of ts(i) ~ ts(i+1) % Q_x = [Q_1 0 0 0] % [0 Q_2 0 0] % [0 0 Q_... 0] % [0 0 0 Q_n] % n = n_poly % Q_all = [Q_x 0 0] % [0 Q_y 0] % [0 0 Q_z] % Q_x == Q_y == Q_z Q_x = []; for i=1:n_poly Q_x = blkdiag(Q_x,calc_Q(n_order,4,ts(i),ts(i+1))); end zeroM = zeros(size(Q_x)); Q_all = [Q_x zeroM zeroM; zeroM Q_x zeroM; zeroM zeroM Q_x]; xlen = size(Q_all,1); f = zeros(xlen,1); %% min (1/2p^TQp + f^Tp) %% compute Aeq x = beq % beacuse Aeq_x == Aeq_y == Aeq_z % Aeq = [Aeq_x 0 0] % [0 Aeq_y 0] % [0 0 Aeq_z] neq = 8; %% (8 equations) eqNum_x = (7*(n_poly-1)+neq); Aeq_x = zeros(eqNum_x, n_coef*n_poly); beq = zeros(dim*eqNum_x, 1); % start/terminal pva constraints (8 equations) Aeq_x(1:4,1:n_coef) = [calc_dc(ts(1),n_order,0); calc_dc(ts(1),n_order,1); calc_dc(ts(1),n_order,2); calc_dc(ts(1),n_order,3)]; Aeq_x(5:8,n_coef*(n_poly-1)+1:n_coef*n_poly) = ... [calc_dc(ts(end),n_order,0); calc_dc(ts(end),n_order,1); calc_dc(ts(end),n_order,2); calc_dc(ts(end),n_order,3)]; beq(1:8,1) = [p0(1),v0(1),a0(1),0,pe(1),ve(1),ae(1),0]'; beq((eqNum_x + 1):(eqNum_x + 8),1) = [p0(2),v0(2),a0(2),0,pe(2),ve(2),ae(2),0]'; beq((eqNum_x*2 + 1):(eqNum_x*2 + 8),1) = [p0(3),v0(3),a0(3),0,pe(3),ve(3),ae(3),0]'; % continuous constraints ((n_poly-1)*7 equations) for i=1:n_poly-1 t_derc_p = calc_dc(ts(i+1),n_order,0); t_derc_v = calc_dc(ts(i+1),n_order,1); t_derc_a = calc_dc(ts(i+1),n_order,2); t_derc_j = calc_dc(ts(i+1),n_order,3); % jerk t_derc_4 = calc_dc(ts(i+1),n_order,4); t_derc_5 = calc_dc(ts(i+1),n_order,5); t_derc_6 = calc_dc(ts(i+1),n_order,6); Aeq_x(neq+1,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_p,-t_derc_p]; Aeq_x(neq+2,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_v,-t_derc_v]; Aeq_x(neq+3,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_a,-t_derc_a]; Aeq_x(neq+4,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_j,-t_derc_j]; Aeq_x(neq+5,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_4,-t_derc_4]; Aeq_x(neq+6,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_5,-t_derc_5]; Aeq_x(neq+7,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_6,-t_derc_6]; neq = neq + 7; end zeroM = zeros(size(Aeq_x)); Aeq = [Aeq_x zeroM zeroM; zeroM Aeq_x zeroM; zeroM zeroM Aeq_x]; %% compute Ax <= b A = []; b = []; ieq = 1; xll = xlen / dim; % 1/3 of xlen, = n_coef * (n_poly-1) for i = 1 : n_poly planesCell = decomp.lines_{i}.polyhedron_.polys_; [~, lenj] = size(planesCell); for j = 1 : lenj n = planesCell{j}.n_; p = planesCell{j}.p_; nx = zeros(1, xlen); % add (Denominator + 1) points on the poly's Ax 0 for k = 0 : Denominator tvec_mid = calc_dc(ts(i) + k*(ts(i+1) - ts(i))/Denominator, n_order, 0); % [1*8]vector nx(1, 8*(i-1) + 1 : 8*(i-1) + 8) = tvec_mid.*n(1); nx(1, 8*(i-1) + 1 + xll : 8*(i-1) + 8 + xll) = tvec_mid.*n(2); nx(1, 8*(i-1) + 1 + 2*xll : 8*(i-1) + 8 + 2*xll)= tvec_mid.*n(3); A(ieq,:) = nx; b(ieq,:) = dot(n, p); % dot(plane.n, p1 - plane.p) < 0 % => dot(plane.n, p1) - dot(plane.n, plane.p) < 0 % => dot(plane.n, p1) < dot(plane.n, plane.p) %% Ax < b ieq = ieq + 1; end end end % % Modify the number of iterations options = optimoptions('fmincon'); options.MaxIterations = 2000; % Defaults = 1000 lb = []; ub = []; x0 = []; p = quadprog(Q_all,f,A,b,Aeq,beq, lb,ub,x0, options); % p = quadprog(Q_all,f,A,b,Aeq,beq); minValue = p'*Q_all*p; px = p(1:xll,1); py = p(xll + 1:2*xll,1); pz = p(xll*2 + 1:3*xll,1); end ================================================ FILE: Motion_Planning/5Trajectory_Planning/TrajectoryPlanning.m ================================================ function [total_time, ts, X] = TrajectoryPlanning(path, decomp, time_allocation) speed = time_allocation.avg_speed; acc = time_allocation.acc; disp(['The planned speed is : ', num2str(speed)]); tic if strcmp(time_allocation.type, 'averageSpeed') [ts, total_time] = averageSpeed_ta(path0, speed); elseif strcmp(time_allocation.type, 'trapzoidSpeed') [ts, total_time] = trapezoidalSpeed_ta(path, speed, acc); end disp('TimeAllocation time is :'); toc disp(['time management: total_time is ', num2str(total_time), 'seconds']); disp(['Split time is : ', num2str(ts)]); tic % use Ax=b get Trajectory planning. =========================== % X = Ax_equal_b(path, total_time, ts); % use 'Quadratic Programming' and SFC(which make by Ax < b) get Trajectory planning. ======================== % use SFC make Inequality constraints X = QPbyUseSFC(path, ts, decomp); disp('generator trajectory time is :'); toc end ================================================ FILE: Motion_Planning/5Trajectory_Planning/calc_Q.m ================================================ % Calculation Q_1 of Q_x of Q of p^TQp % n:Polynomial equation maximum order % d:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap % t1:start timestamp for polynormial % t2:end timestap for polynormial function Q1 = calc_Q(n_order, d, t1, t2) T = zeros((n_order-d)*2+1,1); for n = 1:(n_order-d)*2+1 T(n) = t2^n-t1^n; end Q1 = zeros(n_order); for i = d+1 : n_order+1 for j = i : n_order+1 c = i + j - 2*d - 1; Q1(i,j) = prod(i-d:i-1) * prod(j-d:j-1) / c * T(c); Q1(j,i) = Q1(i,j); end end end ================================================ FILE: Motion_Planning/5Trajectory_Planning/calc_dc.m ================================================ % Calculation Derivative coefficient % n_order = Polynomial equation maximum order % d = derivertive order = 0:pos 1:vel 2:acc 3:jerk function t_derC = calc_dc(t, n_order, d) t_derC = zeros(1,n_order+1); for i = d+1 : n_order+1 t_derC(i) = prod(i-d:i-1) * t^(i-d-1); end end ================================================ FILE: Motion_Planning/demo/demo_3dCorner.m ================================================ close all; clear all; clc; demoActivate = true; choose_map = 2; path_id = 2; runsim ================================================ FILE: Motion_Planning/demo/demo_Z_3blocks.m ================================================ close all; clear all; clc; demoActivate = true; choose_map = 3; path_id = 2; runsim ================================================ FILE: Motion_Planning/demo/demo_ellipsoid.m ================================================ close all; clear all; clc; demoActivate = true; choose_map = 1; path_id = 2; runsim ================================================ FILE: Motion_Planning/draw/draw_ObcPoints.m ================================================ hold on; len = length(obps); for i = 1 : len plot3(obps(i,1), obps(i,2), obps(i,3), '*', 'Color', 'r'); end hold off; ================================================ FILE: Motion_Planning/draw/makeGifAndJpg.m ================================================ function makeGifAndJpg(fig_num) saveas(figure(4), 'postion','jpg'); pause(0.1); saveas(figure(5), 'velocity','jpg'); pause(0.1); filename = 'testAnimated6.gif'; h = figure(fig_num); for i = 1:90 view(4*i+15, 15); drawnow % Capture the plot as an image frame = getframe(h); im = frame2im(frame); [imind,cm] = rgb2ind(im,256); % Write to the GIF File if i == 1 imwrite(imind,cm,filename,'gif', 'Loopcount',inf); else imwrite(imind,cm,filename,'gif','WriteMode','append'); end end end ================================================ FILE: Motion_Planning/draw/plot_path.m ================================================ function plot_path(paths, mapClass, SFCs) % PLOT_PATH Visualize a path and sfc's ellipses % PLOT_PATH(path mapClass, SFCs) creates a figure showing a path through % the environment. % SFCs provide functions for drawing ellipses: drawEllipsoids(); persistent map decomps init; if isempty(init) map = mapClass; decomps = SFCs; init = 1; end figure('Name','Animation'); hold on; for i = 1:size(map.blocks,1) block = map.blocks(i, :); drawBlock(block); end path = paths{1}; % draw Inflated obstacle for i = 1:size(map.blocks,1) margin = map.margin; block = map.blocks(i, :); block(1,1:3) = block(1,1:3) - margin; block(1,4:6) = block(1,4:6) + margin; block(7) = max(150, block(7)); block(8) = max(150, block(8)); block(9) = max(150, block(9)); %% set color drawBlock(block); end if size(path,1) > 0 % pcshow(path, [0,1,0],'MarkerSize', 0.1); % The lower left corner position, width and height. 260 here is exactly 7cm % set(gcf,'position', [500 500 260 220]); % set(gcf,'position', [500 500 400 400]); % big pic % set(gcf,'position', [500 500 300 300]); % small pic set(gcf,'color','w'); set(gca,'color','w'); %% Display the unit of x y z % xlabel('x length(m)'); % ylabel('y length(m)'); % zlabel('z length(m)'); end % Set the axis range axis_add = 2*map.res(1); axis([map.boundary.ld(1)-axis_add, map.boundary.ru(1)+axis_add, ... map.boundary.ld(2)-axis_add, map.boundary.ru(2)+axis_add, ... map.boundary.ld(3)-axis_add, map.boundary.ru(3)+axis_add]) hold on for j = 1 : size(paths, 2) if j == 1 % JPS's result % plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3); elseif j == 2 % JPS() + simple path() result: % plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','#A2142F','LineWidth',3); plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3); end end hold off set(gca,'DataAspectRatio',[0.1 0.1 0.1]); % drawEllipsoid = true; if (exist('drawEllipsoid') && (drawEllipsoid == true)) decomps{1}.drawEllipsoids(); % decomps{2}.drawEllipsoids(); end grid on view(30, 10); hold off; end function drawBlock(block) x = [ones(4,1) * block(1); ones(4,1) * block(4)]; y = [ones(2,1) * block(5); ones(2,1) * block(2); ones(2,1) * block(5); ones(2,1) * block(2)]; z = [block(3);block(6);block(3);block(6);block(3);block(6);block(3);block(6)]; vert = [x, y, z]; fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8]; c = block(7:9)/255; patch('Vertices',vert,'Faces',fac,... 'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2); x = [ones(4,1) * block(1); ones(4,1) * block(4)]; y = [block(2);block(5);block(2);block(5);block(2);block(5);block(2);block(5)]; z = [ones(2,1) * block(3); ones(2,1) * block(6); ones(2,1) * block(3); ones(2,1) * block(6)]; vert = [x, y, z]; fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8]; c = block(7:9)/255; patch('Vertices',vert,'Faces',fac,... 'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2); end ================================================ FILE: Motion_Planning/init_data.m ================================================ start = {[]}; stop = {[]}; if (~exist('demoActivate')) || (demoActivate == false) choose_map = 2; end demoActivate = false; if (~exist('path_id')) path_id = 2; % Path planning: 2 = JPS, 4 = JPS-PF [This feature has not been uploaded yet] end SFC_id = path_id/2; % Path planning: 2 = path_id(4)'s SFC, 1 = path_id(2)'s SFC speed = 1; acc = 0; time_allocation.type = 'trapzoidSpeed'; %time_allocation.type = 'averageSpeed'; switch choose_map case 1 %% 3 blocks map.load_map('0Maps/ellipsoid.txt', 0.2, 0.2, 0.1); start = {[1.5 0.2 0.2]}; stop = {[1.5 2.0 1.9]}; speed = 1; acc = 2; case 2 map.load_map('0Maps/3dCorner.txt', 0.1, 0.1, 0.1); start = {[2.6 1.0 1.0]}; stop = {[2.5 3.0 2.6]}; acc = 2; % acceleration if path_id == 4 speed = 1.0767; % for path_id = 4, time = 4.3682, snap = 350.3038, fly use 4.1s else speed = 0.8; % for path_id = 2, time = 4.3681, snap = 895.7681, fly use 4.15s end case 3 %% Z_3blocks map.load_map('0Maps/Z_3blocks.txt', 0.2, 0.2, 0.1); start = {[-1 1.5 2]}; stop = {[11 1.5 1]}; acc = 2; if path_id == 4 speed = 1.4; %for path_id = 4, time = 12.9343, snap = 58.376, fly use 12.45s else speed = 1.10423; %for path_id = 2, time = 12.9343, snap = 300.3111, fly use 13.9s end otherwise end % Must be after acc assignment above time_allocation.avg_speed = speed; time_allocation.acc = acc; ================================================ FILE: Motion_Planning/runsim.m ================================================ % if demoActivate exist and equal true, not clean all variable. % runsim.m called by the demo, not clean all variable % runsim.m called by itself, clean all variable if (~exist('demoActivate')) || (demoActivate == false) close all; clear all; clc; end addpath(genpath('./')); %% map generation map = GridMap(); init_data map.flag_obstacles(); %% path planning nquad = length(start); for qn = 1:nquad disp('JPS time is :'); tic path{qn} = JPS_3D(map, start{qn}, stop{qn}); path{qn} = [start{qn}; path{qn}(1:end,:)]; path{qn}(end + 1,:) = stop{qn}; toc end %% delete the points of no-use path{2} = simplify_path(map, path{1}); %% Generating Convex Polytopes obps = PointCloudMap(map.blocks, map.margin); % blocks of Metric Map change to point cloud decomps{2} = [] disp('JPS -> SFC time is :'); tic decomps{1} = SFC_3D(path{2}, obps, map.boundary); % call SFC toc path{path_id} %% draw path and blocks if nquad == 1 plot_path(path, map, decomps); else % you could modify your plot_path to handle cell input for multiple robots end % draw_ObcPoints % makeGifAndJpg(1); %figure(1): Graph without trajectory %% Trajectory planning [t_time, ts_par, x_par] = TrajectoryPlanning(path{path_id}, decomps{SFC_id}, time_allocation); %% Trajectory tracking disp('Generating Trajectory ...'); trajectory_generator([], [], path{path_id}, t_time, ts_par, x_par); trajectory = test_trajectory(start, stop, path, true); % with visualization disp('Blue line is Trajectory planning.'); disp('Red line is Trajectory tracking.'); %% Gif % makeGifAndJpg(3); %figure(3): Graph with trajectory ================================================ FILE: Motion_Planning.md ================================================ # Motion Planning A new method to improve path planning polyhedron-1 - Path planning algorithms (Jump point search, JPS-PF ) - Generating Convex Polytopes(Safe Flight Corridors) - Time Allocation(Average time allocation, Trapezoidal time allocation) - Trajectory planning(QP) - Trajectory following control(PD controller) ## Required MATLAB(R2019b is tested) ## Demo Run the demo map script (Only three maps have been released so far) ： ``` demo_3dCorner.m demo_ellipsoid.m demo_Z_3blocks.m ``` ### Map: 3dCorner - Same starting point and ending point - same total planning time, use trapezoidal time allocation | Method | JPS | JPS-PF | | ---------- | ------------------------------------------------------------ | ------------------------------------------------------------ | | trajectory | 3dCorner-jps-ys | 3dCorner-new-ys | | SFC | 3dCorner-jps-sfc | 3dCorner-new-sfc | | Avg_speed | 0.8 | 1.0767 | | Time | 4.3681 | 4.3681 | | miniSnap | 895.7681 | 350.3038 | ### Map: Z_3blocks | Method | JPS | JPS-PF | | ---------- | ------------------------------------------------------------ | ------------------------------------------------------------ | | trajectory | Z_3blocks | Z_3blocks-new | | Top view | Z_3blocks | Z_3blocks-new | | Avg_speed | 1.10423 | 1.4 | | Time | 12.9343 | 12.9343 | | miniSnap | 300.3111 | 58.376 | ## Reference ##### Paper: [1] D. Harabor and A. Grastien. 2011. "**Online Graph Pruning for Pathfinding on Grid Maps**". In Proceedings of the 25th National Conference on Artificial Intelligence (AAAI), San Francisco, USA. [2] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor, et al., "**Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments**", IEEE Robotics and Automation Let- ters, vol. 2, no. 3, pp. 1688-1695, July 2017. [3] D.W.Mellinger,"**Trajectory generation and control for quadrotors**"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012. [4] D. Mellinger and V. Kumar, "**Minimum snap trajectory generation and control for quadrotors**", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011 [5] T. Lee, M. Leoky, and N. H. McClamroch, "**Geometric tracking control of a quadrotor UAV on SE (3)**" in *Proc. 49th IEEE Conf. Decis. Control*. IEEE, 2010, pp. 5420–5425. ================================================ FILE: README.md ================================================ # AerialRobotics Simulate the path planning and trajectory planning of quadrotors/UAVs. ## Motion Planning project #### The main purpose: - A new method to improve path planning - Compare JPS and new method #### Functions already implemented: - Path planning algorithms (Jump point search, JPS-PF ) - Generating Convex Polytopes(Safe Flight Corridors) - Time Allocation(Average time allocation, Trapezoidal time allocation) - Trajectory planning(QP) - Trajectory following control(PD controller) The more detailed data is in [Motion_Planning.md](Motion_Planning.md) #### Demo animation: polyhedron-1 3dCorner-new-ys ## Trajectory Planning project #### The main purpose: - Compare different pathfinding algorithms(Dijkstra, Astar, Jump point search) - Compare different trajectory planning methods(Ax = b, QP) #### Functions already implemented: - Path planning algorithms (Dijkstra, Astar, Jump point search) - Generating Convex Polytopes(Safe Flight Corridors) - Trajectory planning(Ax = b, QP) - Trajectory following control(PD controller) The more detailed data is in [Trajectory_Planning.md](Trajectory_Planning.md) #### Demo animation: polyhedron-1 **More related knowledge introduction on [Wiki](https://github.com/LenaShengzhen/AerialRobotics/wiki)** **New feature schedule on [projects](https://github.com/LenaShengzhen/AerialRobotics/projects/2)** **Bug and issue tracking on [projects](https://github.com/LenaShengzhen/AerialRobotics/projects/1)** ## Reference [1] D. Harabor and A. Grastien. 2011. "**Online Graph Pruning for Pathfinding on Grid Maps**". In Proceedings of the 25th National Conference on Artificial Intelligence (AAAI), San Francisco, USA. [2] R. Deits and R. Tedrake, "**Computing large convex regions of obstacle-free space through semidefinite programming**",in Algorithmic Foundations of Robotics XI. Berlin, Germany: Springer, 2015, pp. 109–124. [3] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor, et al., "**Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments**", IEEE Robotics and Automation Let- ters, vol. 2, no. 3, pp. 1688-1695, July 2017. [4] S. Savin, "**An algorithm for generating convex obstacle-free regions based on stereographic projection**,” in International Siberian Confer- ence on Control and Communications, 2017. [5] Xingguang Zhong, Yuwei Wu, Dong Wang, Qianhao Wang, Chao Xu, and Fei Gao, "**Generating Large Convex Polytopes Directly on Point Clouds**",2020 [6] D.W.Mellinger,"**Trajectory generation and control for quadrotors**"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012. [7] D. Mellinger and V. Kumar, "**Minimum snap trajectory generation and control for quadrotors**", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011 [8] C. Richter, A. Bry, and N. Roy, “**Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments**,” in *Proc. Int. Symp. Robot. Res.*, 2016. [9] T. Lee, M. Leoky, and N. H. McClamroch, "**Geometric tracking control of a quadrotor UAV on SE (3)**", in *Proc. 49th IEEE Conf. Decis. Control*. IEEE, 2010, pp. 5420–5425. [10] Daniel Mellinger, Alex Kushleyev and Vijay Kumar,"**mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams**", IEEE, 2012 ================================================ FILE: Trajectory_Planning.md ================================================ # Trajectory Planning polyhedron-1 - Path planning algorithms (Dijkstra, Astar, Jump point search) - Generating Convex Polytopes(Safe Flight Corridors) - Trajectory planning(Ax = b, QP) - Trajectory following control(PD controller) ## Required MATLAB(R2019b is tested) ## Pic ### 1. Path Planning | dijkstra | Astar | Jump Point Search | | -------------------------------------------------------- | -------------------------------------------------- | ---------------------------------------------- | | dijkstra | Astar | JPS | | 1.642271 seconds | 0.755504 seconds | 0.451664 seconds | ### 2. Generating Convex Polytopes ### Safe Flight Corridors - #### find_ellipsoid polyhedron-1polyhedron-2 - #### find_polyhedron polyhedron-1polyhedron-2 ### 3. Trajectory planning Compare different Trajectory planning： - #### Trajectory 1: use Ax = b get Trajectory. closeFormcloseFormcloseForm The Trajectory pass every Path point. - #### Trajectory 2: use 'Quadratic Programming' get Trajectory. use corridor constraints make Ax< b. x y z separately find minimum-snap. corridorConstraintscorridorConstraintscorridorConstraints The Trajectory don't need to pass every Path point. minSnapValue(X + Y + Z) is : 9376.0901 - #### Trajectory 3: use 'Quadratic Programming' get Trajectory. use SFC make Ax < b. SFCSFCSFC The Trajectory don't need to pass every Path point. minSnapValue(X + Y + Z) is : 2958.5877 ### 4. Trajectory following control Trajectory following by use PD Controller, you can learn by [coursera](https://www.coursera.org/learn/robotics-flight/home/welcome). ## Reference ##### Paper: [1] D. Harabor and A. Grastien. 2011. "**Online Graph Pruning for Pathfinding on Grid Maps**". In Proceedings of the 25th National Conference on Artificial Intelligence (AAAI), San Francisco, USA. [2] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor, et al., "**Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments**", IEEE Robotics and Automation Let- ters, vol. 2, no. 3, pp. 1688-1695, July 2017. [3] D.W.Mellinger,"**Trajectory generation and control for quadrotors**"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012. [4] D. Mellinger and V. Kumar, "**Minimum snap trajectory generation and control for quadrotors**", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011 [5] T. Lee, M. Leoky, and N. H. McClamroch, "**Geometric tracking control of a quadrotor UAV on SE (3)**" in *Proc. 49th IEEE Conf. Decis. Control*. IEEE, 2010, pp. 5420–5425. ##### Code: safe flight corridors: [C++ version](https://github.com/sikang/DecompUtil)