lidarSLAM

Perform localization and mapping using lidar scans

Description

The lidarSLAM class performs simultaneous localization and mapping (SLAM) for lidar scan sensor inputs. The SLAM algorithm takes in lidar scans and attaches them to a node in an underlying pose graph. The algorithm then correlates the scans using scan matching. It also searches for loop closures, where scans overlap previously mapped regions, and optimizes the node poses in the pose graph.

Creation

Syntax

slamObj = lidarSLAM

slamObj = lidarSLAM(mapResolution,maxLidarRange)

slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans)

Description

slamObj = lidarSLAM creates a lidar SLAM object. The default occupancy map size is 20 cells per meter. The maximum range for each lidar scan is 8 meters.

slamObj = lidarSLAM(mapResolution,maxLidarRange) creates a lidar SLAM object and sets the MapResolution and MaxLidarRange properties based on the inputs.

slamObj = lidarSLAM(mapResolution,maxLidarRange,maxNumScans) specifies the upper bound on the number of accepted scans allowed when generating code. maxNumScans is a positive integer. This scan limit is only required when generating code.

Properties

PoseGraph — Underlying pose graph that connects scans

poseGraph object

Underlying pose graph that connects scans, specified as a poseGraph object. Adding scans to lidarSLAM updates this pose graph. When loop closures are found, the pose graph is optimized using OptimizationFcn.

MapResolution — Resolution of occupancy grid map

20 cells per meter (default) | positive integer

Resolution of the occupancy grid map, specified as a positive integer in cells per meter. Specify the map resolution on construction.

MaxLidarRange — Maximum range of lidar sensor

8 meters (default) | positive scalar

Maximum range of the lidar sensor, specified as a positive scalar in meters. Specify the maximum range on construction.

OptimizationFcn — Pose graph optimization function

optimizePoseGraph (default) | function handle

Pose graph optimization function, specified as a function handle. By default, the algorithm calls the optimizePoseGraph function. To specify your own optimization method, the class requires the function signature to be:

[updatedPose,stat] = myOptimizationFcn(poseGraph)

poseGraph is a poseGraph object. updatedPose is an n-by-3 vector of [x y theta] poses listed in sequential node ID order. stat is a structure containing a ResidualError field as a positive scalar. Use the stat structure to include other information relevant to your optimization.

LoopClosureThreshold — Threshold for accepting loop closures

100 (default) | positive scalar

Threshold on the score from the scan matching algorithm for accepting loop closures, specified as a positive scalar. Higher thresholds correspond to a better match, but scores vary based on sensor data.

LoopClosureSearchRadius — Search radius for loop closure detection

8 meters (default) | positive scalar

Search radius for loop closure detection, specified as a positive scalar. Increasing this radius affects performance by increasing search time. Tune this distance based on your environment and the expected vehicle trajectory.

LoopClosureMaxAttempts — Number of attempts at finding loop closures

1 (default) | positive integer

Number of attempts at finding looping closures, specified as a positive integer. Increasing the number of attempts affects performance by increasing search time.

LoopClosureAutoRollback — Allow automatic rollback of added loop closures

true (default) | false

Allow automatic rollback of added loop closures, specified as true or false. The SLAM object tracks the residual error returned by the OptimizationFcn. If it detects a sudden change in the residual error and this property is true, it rejects (rolls back) the loop closure.

OptimizationInterval — Number of loop closures accepted to trigger optimization

1 (default) | positive integer

Number of loop closures accepted to trigger optimization, specified as a positive integer. By default, the PoseGraph is optimized every time lidarSLAM adds a loop closure.

MovementThreshold — Minimum change in pose required to process scans

[0 0] (default) | [translation rotation]

Minimum change in pose required to process scans, specified as a [translation rotation] vector. A relative pose change for a newly added scan is calculated as [x y theta]. If the translation in xy-position or rotation of theta exceeds these thresholds, the lidarSLAM object accepts the scan and adds a pose is added to the PoseGraph.

ScanRegistrationMethod — Scan registration method

'BranchAndBound' (default) | 'PhaseCorrelation'

Scan registration method, specified as a character vector.

Note

Image Processing Toolbox™ is required for using Phase Correlation method.

TranslationSearchRange — Incremental match translational search range

[maxLidarRange/2 maxLidarRange/2] (default) | two-element vector of the form [x y]

Incremental match translational search range, specified as a two-element vector of the form [x y] in meters. This property is only applicable when the ScanRegistrationMethod property is set to 'BranchAndBound'.

These values define the search window around the initial translation estimate specified in the relPoseEst argument of the addScan function. Set the value of this property to the maximum expected translation between consecutive accepted scans.

This property is similar to the 'TranslationSearchRange' name-value pair argument in matchScansGrid function.

RotationSearchRange — Incremental match rotational search range

pi/2 (default) | positive scalar

Incremental match rotational search range, specified as positive scalar in radians. This property is only applicable when the ScanRegistrationMethod property is set to 'BranchAndBound'.

This values define the search window around the initial rotation estimate specified in the relPoseEst argument of the addScan function. Set the value of this property to the maximum expected rotation between consecutive accepted scans.

This property is similar to the 'RotationSearchRange' name-value pair argument in matchScansGrid function.

Object Functions

addScan	Add scan to lidar SLAM map
copy	Copy lidar SLAM object
removeLoopClosures	Remove loop closures from pose graph
scansAndPoses	Extract scans and corresponding poses
show	Plot scans and robot poses

Examples

Perform SLAM Using Lidar Scans

Use a lidarSLAM object to iteratively add and compare lidar scans and build an optimized pose graph of the robot trajectory. To get an occupancy map from the associated poses and scans, use the buildMap function.

Load Data and Set Up SLAM Algorithm

Load a cell array of lidarScan objects. The lidar scans were collected in a parking garage on a Husky® robot from ClearPath Robotics®. Typically, lidar scans are taken at a high frequency and each scan is not needed for SLAM. Therefore, down sample the scans by selecting only every 40th scan.

load garage_fl1_southend.mat scans
scans = scans(1:40:end);

To set up the SLAM algorithm, specify the lidar range, map resolution, loop closure threshold, and search radius. Tune these parameters for your specific robot and environment. Create the lidarSLAM object with these parameters.

maxRange = 19.2; % meters
resolution = 10; % cells per meter

slamObj = lidarSLAM(resolution,maxRange);
slamObj.LoopClosureThreshold = 360;
slamObj.LoopClosureSearchRadius = 8;

Add Scans Iteratively

Using a for loop, add scans to the SLAM object. The object uses scan matching to compare each added scan to previously added ones. To improve the map, the object optimizes the pose graph whenever it detects a loop closure. Every 10 scans, display the stored poses and scans.

for i = 1:numel(scans)

    addScan(slamObj,scans{i});
    
    if rem(i,10) == 0
        show(slamObj);
    end
end
title("Lidar Scans and Poses")
xlabel("X [meters]")
ylabel("Y [meters]")

View Occupancy Map

After adding all the scans to the SLAM object, build an occupancyMap map by calling buildMap with the scans and poses. Use the same map resolution and max range you used with the SLAM object.

[scansSLAM,poses] = scansAndPoses(slamObj);
occMap = buildMap(scansSLAM,poses,resolution,maxRange);
figure
show(occMap)
title('Occupancy Map of Garage')

More About

SLAM

Simultaneous localization and mapping (SLAM) is a general concept for algorithms correlating different sensor readings to build a map of a vehicle environment and track pose estimates. Different algorithms use different types of sensors and methods for correlating data.

The lidarSLAM algorithm uses lidar scans and odometry information as sensor inputs. The lidar scans map the environment and are correlated between each other to build an underlying pose graph of the vehicle trajectory. Odometry information is an optional input that gives an initial pose estimate for the scans to aid in the correlation. Scan matching algorithms correlate scans to previously added scans to estimate the relative pose between them and add them to an underlying pose graph.

The pose graph contains nodes connected by edges that represent the relative poses of the vehicle. Edges specify constraints on the node as an information matrix. To correct for drifting pose estimates, the algorithm optimizes over the whole pose graph whenever it detects loop closures.

The algorithm assumes that data comes from a vehicle navigating an environment and incrementally getting laser scans along its path. Therefore, scans are first compared to the most recent scan to identify relative poses and are added to the pose graph incrementally. However, the algorithm also searches for loop closures, which identify when the vehicle scans an area that was previously visited.

When working with SLAM algorithms, the environment and vehicle sensors affect the performance and data correlation quality. Tune your parameters properly for your expected environment or dataset.

References

[1] Hess, Wolfgang, Damon Kohler, Holger Rapp, and Daniel Andor. "Real-Time Loop Closure in 2D LIDAR SLAM." 2016 IEEE International Conference on Robotics and Automation (ICRA). 2016.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Use this syntax when constructing lidarSLAM objects for code generation:

slamObj= lidarSLAM(mapResolution,maxLidarRange,maxNumScans) specifies the upper bound on the number of accepted scans allowed when generating code. maxNumScans is a positive integer. This scan limit is only required when generating code.

Version History

Introduced in R2019b