Silverwing IndustriesThe project has been scripted in MATLAB because of it's amazing matrix math functions and simply because I had it installed on my laptop.
What I eventually would like to do is develop a SLAM (Simultaneous Localization and Mapping) algorithm to generate maps with only the use of a LiDAR and no other navigation aid (incl. encoders).
This project however only addresses the issue of localization of a robot in a known map - like the arena our robot is going to be in for the Trinity Fire Fighting Robot competition.
The key to the algorithm is calculating best-match scores for every possible linear and angular offsets for the pixelized LiDAR map on a pixelized known map. The best score is then obtained and the x and y indexes of the best score is used as the map offset. The LiDAR map is first rotated to make all lines vertical or horizontal - this leaves only for angular possibilities for the map-matching algorithm which significantly reduces the load on the processor. The rotation of the best match is inverted and the H/V rotation is also inverted to get the robot's position and heading.
For those who wish to use the algorithm in your own LiDAR-based mapping robot - here's my code:
(Hackaday could definitely use MATLAB or Octave syntax highlighting btw)
clear all; close all; clc; % Hardware Specifications LiDARRange = [20 600]; % [cm] LiDAR Range LiDARdtheta = 1; % [deg] LiDAR Angular Resolution LiDARdr = 0.2; % [cm] LiDAR Linear Resolution map = []; % Simulation Parameters run = 1; % run simulation flag tth = 1; % [cm] Thickness tolerance threshold dt = 0.1; % [s] simulation time step dd = 10; % [cm] Discontinuity check threshold dr = 0.1; % Linearity check range threshold da = 10; % [deg] Angularity tolerance mR = 4; % Minimum repetition required to be valid bounds = [0 244 0 244]; % [cm] Maze bounds (MATLAB axis) RobotPos = [80 10]; % [cm] Position of Robot (x,y) RobotHdg = 80; % [deg] Heading of Robot, with 0 deg pointing E, CCW + LiDARMap = [transpose(0:359), zeros(360,1)+600]; res = 16; % [px] Map resolution % Define Arena Walls walls = [ 0 0 0 244; % W Boundary 0 0 244 0; % S Boundary 244 0 244 244; % E Boundary 0 244 244 244; % N Boundary 126 0 126 45; % Room 1 W Wall [M] 126 91 198 91; % Room 1 N Wall 0 103 72 103; % Room 2 N Wall 72 56 72 103; % Room 2 E Wall 46 157 72 157; % Room 3 S Wall 72 157 72 244; % Room 3 E Wall 126 147 126 198; % Room 4 W Wall 126 147 196 147; % Room 4 S Wall [M] 196 147 196 198; % Room 4 E Wall 126 198 150 198]; % Room 4 N Wall [M] % Localization STEP 1 - Pixelize Known map <40x40> [ONE TIME PROCESS] pixMap_s = zeros(res); % Stored Pixel Map ds = 244/res; for w = 1:size(walls,1) p1 = walls(w,1:2); p2 = walls(w,3:4); if p1(1) == p2(1) % V Line length = p2(2) - p1(2); for s = 0:round(length/ds); pixMap_s(1+round(p1(1)/ds),1+round(p1(2)/ds) + s) = 1; end else % H Line (There are only H and V Lines on this map) length = p2(1) - p1(1); for s = 0:round(length/ds); pixMap_s(1+round(p1(1)/ds) + s,1+round(p1(2)/ds)) = 1; end end end while run % clc; LiDARMap = [transpose(0:359), zeros(360,1)+600]; % LiDAR Generate Range Map for theta = 0:359 % Line12 - line from robot to LiDAR range along theta x1 = RobotPos(1); y1 = RobotPos(2); x2 = RobotPos(1) + LiDARRange(2)*cosd(RobotHdg + theta); y2 = RobotPos(2) + LiDARRange(2)*sind(RobotHdg + theta); for wall = 1:size(walls,1) % Line34 - wall x3 = walls(wall,1); y3 = walls(wall,2); x4 = walls(wall,3); y4 = walls(wall,4); % Find Intersection point den = (x1 - x2)*(y3 - y4) - (y1 - y2)*(x3 - x4); numTerm1 = x1*y2 - y1*x2; numTerm2 = x3*y4 - y3*x4; Px = (numTerm1*(x3 - x4) - numTerm2*(x1 - x2))/den; Py = (numTerm1*(y3 - y4) - numTerm2*(y1 - y2))/den; % Check if intersection point is between bounds if ((Px >= x1-tth && Px <= x2+tth) || (Px >= x2-tth && Px <= x1+tth)) && ... ((Py >= y1-tth && Py <= y2+tth) || (Py >= y2-tth && Py <= y1+tth)) && ... ((Px >= x3-tth && Px <= x4+tth) || (Px >= x4-tth && Px <= x3+tth)) && ... ((Py >= y3-tth && Py <= y4+tth) || (Py >= y4-tth && Py <= y3+tth)) ; theta_corr = theta; if theta > 359 theta_corr = theta - 360; end d = round(sqrt((Px - RobotPos(1))^2 + (Py - RobotPos(2))^2)*5)/5; if d < LiDARMap(theta_corr+1,2) % if d < LiDARRange(1) % d = LiDARRange(1); % end LiDARMap(theta_corr+1,2) = d; end end end end % for theta = 0:359 % d = LiDARMap(theta+1,2); % if(d>LiDARRange(1) && d<LiDARRange(2)) % map = [map; RobotPos + d.*[cosd(RobotHdg+theta), sind(RobotHdg+theta)]]; % end % end %%%%%%%%%%%%%%%%%%%%%%%%%% ONBOARD PROCESSING %%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Rotate LiDAR Map to H/V Lines % Start by identifying the general complimentary angles to find the % rotation angle m = []; % Vector of Slopes mapP = []; for theta = 1:358 % Make sure the linearity check is not at a discontinuity if (abs(LiDARMap(theta+2,2) - LiDARMap(theta+1,2)) < dd && abs(LiDARMap(theta+1,2) - LiDARMap(theta,2)) < d) p0 = LiDARMap(theta,2).*[cosd(theta); sind(theta)]; p1 = LiDARMap(theta+1, 2).*[cosd(theta+1); sind(theta+1)]; u = (p1-p0)./norm(p1-p0); A = [[cosd(theta+1); sind(theta+1)] -u]; x_vect = inv(A)*p1; r_exp = x_vect(1); r_tol = r_exp*dr; if abs(LiDARMap(theta+2,2) - r_exp) < r_tol % Grab angle and append to m vector m = [m; (180/pi)*atan2(u(2),u(1))]; mapP = [mapP; p0' p1']; end end end % There should only be 4 angles in the data set but ofcourse, % instrument and sensor errors can cause deviations (let's say +/- 10 % deg?) - so, I'm going to check if the next angle in within a certain % range, it's going to average the value. primAngles = [1, m(1)]; % [n AVG_ANGLE] for n = 2:size(m,1) foundMatch = 0; for p = 1:size(primAngles,1) if (abs(primAngles(p,2)-m(n)) < da) primAngles(p,2) = (primAngles(p,1)*primAngles(p,2) + m(n))/(primAngles(p,1)+1); primAngles(p,1) = primAngles(p,1)+1; % Increment n foundMatch = 1; break end end if ~foundMatch primAngles = [primAngles; 1 m(n)]; end end % The bottom code is to further refine the rotation angle, but a simple % alternative gives a pretty awesome approximation that has worked % every single time for me so far % % Keep only most seen angles - filter out the non-repetitive angles % primAngles_filtered = []; % for n = 1:size(primAngles,1) % if (primAngles(n,1) > mR) % primAngles_filtered = [primAngles_filtered; primAngles(n,:)]; % end % end % primAngles = sortrows(primAngles_filtered,-1); % % % Now, if an angle is complementary to the first one, rotate it to the % % original domain for further averaging (better angular determination) % rotAngle = primAngles(1,2); % rotN = primAngles(1,1); % % if rotAngle > 360 % rotAngle = rotAngle - 360; % elseif rotAngle < 0 % rotAngle = rotAngle + 360; % end % % if size(primAngles,1) > 1 % for n = 2:size(primAngles,1) % for phi = 90:90:270 % thisAngle = primAngles(n,2) + phi; % if thisAngle > 360 % thisAngle = thisAngle - 360; % elseif thisAngle < 0 % thisAngle = thisAngle + 360; % end % if (abs(thisAngle-rotAngle) < da) % rotAngle = ((rotN*rotAngle) + (primAngles(n,1)*thisAngle))/(rotN+primAngles(n,1)); % rotN = rotN + primAngles(n,1); % end % end % end % end % Simple most popular average selection approximation for rot. angle primAngles = sortrows(primAngles,-1); rotAngle = primAngles(1,2); % Now that the a complimentary (90 offset) rotation angle has been, % generate a cartesian map of the LiDAR data offset by the rotation % angle. cartMap = zeros(360,2); for theta = 1:360 cartMap(theta,:) = LiDARMap(theta,2).*[cosd(theta-rotAngle) sind(theta-rotAngle)]; end xRange = max(cartMap(:,2)) - min(cartMap(:,2)); yRange = max(cartMap(:,1)) - min(cartMap(:,1)); pixMap_l = zeros(floor(yRange/ds), floor(xRange/ds)); % LiDAR Data Pixel Map for n = 1:size(cartMap,1) pixMap_l(1+round((cartMap(n,1) - min(cartMap(:,1)))/ds), 1+round((cartMap(n,2) - min(cartMap(:,2)))/ds)) = 1; end % Now that both the known and LiDAR maps have been pixelized to the % same scale, they can be compared (at +0, +90, +180 and +270 deg) at % every possible x and y offset to search for a match. % For each of these angular and linear offsets, a similarity score can % be calculated based on how many pixels overlap and the offset with % the highest score can be selected to find the robot's position with % relation to the map rotMaps = {0, 0, 0, 0}; rotMaps{1} = pixMap_l; for n = 2:4 rotMaps{n} = rot90(rotMaps{n-1}); end xL = size(pixMap_l,2); % LiDAR pixmap map x range yL = size(pixMap_l,1); % LiDAR pixmap y range xO = (res - xL) + 2; % Possible number of x-offset values yO = (res - yL) + 2; % Possible number of y-offset values rotScores = {0, 0, 0, 0}; rotMaxScores = zeros(1,4); for r = 1:4 % For each angular/rotationl offset % Pre-allocate scores array for processing time optimization % Get rotated x and y offset values (flipped for certain rotations) if (r == 1 || r == 3) xR = xO; yR = yO; xRL = xL; yRL = yL; else xR = yO; yR = xO; xRL = yL; yRL = xL; end rotScores{r} = zeros(yR, xR); for x = 1:xR for y = 1:yR % Match Score calculation algorithm mapSeg = pixMap_s(y:(y+yRL-1),x:(x+xRL-1)); score = sum(sum(mapSeg & rotMaps{r})); % Add score to rest of the score for comparison rotScores{r}(x,y) = score; end end rotMaxScores(r) = max(max(rotScores{r})); end % Get the rotational and linear offsets used which yield the highest % match score. rMax = find(rotMaxScores==max(rotMaxScores),1); HDG_est = (rMax-1)*90 - rotAngle; if HDG_est > 360 HDG_est = HDG_est - 360; elseif HDG_est < 0 HDG_est = HDG_est + 360; end % Find the best score linear offset (x,y) index = find(rotScores{rMax}==max(max(rotScores{rMax})),1); xOffset = ceil(index/size(rotScores{rMax},1)); yOffset = index - (xOffset-1)*size(rotScores{rMax},1); switch rMax case 1 % 0 OFF xPos_est = - min(cartMap(:,1)) + (xOffset-1)*ds; yPos_est = - min(cartMap(:,2)) + (yOffset-1)*ds; case 2 % 90 OFF xPos_est = xRange + min(cartMap(:,2)) + (xOffset-1)*ds; yPos_est = - min(cartMap(:,1)) + (yOffset-1)*ds; case 3 % 180 OFF xPos_est = xRange + min(cartMap(:,2)) - (xOffset-1)*ds; yPos_est = yRange + min(cartMap(:,1)) - (yOffset-1)*ds; case 4 % 270 OFF xPos_est = - min(cartMap(:,2)) + (yOffset-1)*ds; yPos_est = yRange + min(cartMap(:,1)) - (xOffset-1)*ds; end % disp([num2str(rMax) ' POS: <' num2str(xPos_est) ', ' num2str(yPos_est) '> HDG: ' num2str(HDG_est)]); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % PLOTS figure(1); clf; subplot(2,2,1); % ARENA | SIMULATION VISUALIZATION hold on; % Draw Robot Position and Heading plot(RobotPos(1), RobotPos(2), 'bo'); % Plot Rover plot([RobotPos(1) RobotPos(1)+(10.*cosd(RobotHdg))], [RobotPos(2) RobotPos(2)+(10.*sind(RobotHdg))], 'b-'); % plot(xPos_est, yPos_est, 'ro'); % Plot Rover % plot([xPos_est xPos_est+(10.*cosd(HDG_est))], [yPos_est yPos_est+(10.*sind(HDG_est))], 'r-'); % % Draw Area Walls for wall = 1:size(walls,1) plot(walls(wall,[1 3]), walls(wall,[2 4]), 'Color', [0 0 0] ,'LineStyle', '-', 'LineWidth', 3); end hold off; grid on; axis(bounds); axis square; subplot(2,2,2); % LiDAR | VISIBLE RANGE MAP polar((LiDARMap(:,1) + 90).*(pi/180),LiDARMap(:,2),'r.'); grid on; axis equal; % % MAPPING PLOT - NOT REQ. % plot(map(:,1), map(:,2), 'k.'); % grid on; % axis(bounds); % axis square; subplot(2,2,3); % PIXELIZED MAP MATCHING VISUALIZATION hold on; for x = 0:res for y = 0:res if (pixMap_s(1+x,1+y) == 1) plot(x,y,'k*'); end end end for x = 0:(size(rotMaps{rMax},1)-1) for y = 0:(size(rotMaps{rMax},2)-1) if (rotMaps{rMax}(1+x,1+y) == 1) plot(x+xOffset-1,y+yOffset-1,'r*'); end end end hold off; axis([-1 res+1 -1 res+1]); axis square; grid on; subplot(2,2,4); % PIXELIZED MAP MATCHING VISUALIZATION % polar((LiDARMap(:,1) + 90 - rotAngle).*(pi/180),LiDARMap(:,2),'r.'); hold on; for x = 0:(size(pixMap_l,1)-1) for y = 0:(size(pixMap_l,2)-1) if (pixMap_l(1+x,1+y) == 1) plot(x,y,'k*'); end end end hold off; grid on; axis([-1 res+1 -1 res+1]); axis square; % AUTOMATIC CONTROL pause(dt); RobotPos = RobotPos + 2*[cosd(RobotHdg), sind(RobotHdg)]; % MANUAL CONTROL % ch = getkey; % switch ch % case 30 % UP % RobotPos = RobotPos + 5.*[cosd(RobotHdg), sind(RobotHdg)]; % case 31 % DOWN % RobotPos = RobotPos - 5.*[cosd(RobotHdg), sind(RobotHdg)]; % case 28 % LEFT % RobotHdg = RobotHdg + 30; % case 29 % RIGHT % RobotHdg = RobotHdg - 30; % otherwise % EXIT % run = 0; % end % run = 0; if ((RobotPos(1) < 0) || (RobotPos(1) > 244) || (RobotPos(2) < 0) || (RobotPos(2) > 244)) run = 0; end end