The 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...Read more »