A Novel Self-Positioning Based on Feature Map Creation and Laser Location Method for RBPF-SLAM

Algorithm 2

Algorithm of creating line feature map based on RBPF.

	Step 1. Algorithm initialization: , the system state samples are extracted from the prior distribution as particles, the initial weight of each particle is , and the size of particle set is ;
	Step 2. Building a local map: In this step, segment feature representation and extraction method are used to construct the local line segment map in the field of vision of unmanned vehicle at the current time according to the observation data of sensor. The sensor is 2D lidar, , which is the point cloud data observed by lidar;
	Step 3. Sampling: For all particles, the motion control input and observation data of unmanned vehicle are used to determine the sampling area;
	Step 4. Weight update: For all particles, the weight of particles is calculated according to the unmanned vehicle position , observation data and the constructed map ;
	Step 5. Resampling: When the weight of a few particles is large and the weight of other particles is small, resampling is conducted to delete the particles with large error;
	Step 6. Map update: For each particle, according to the estimated pose in step 2, the local feature subgraph created in step 1 is completed to the constructed global map to form a new global map .