2.1

Front end of SLAM

The task of the front end of the SLAM system is to find the optimal pose estimation δ = (δ_x,δ_y,δ_θ) of the current frame in the local map, which includes the translation component (δ_x,δ_y) and rotation component δ_θ, and insert the laser scanning data to the maintaining local map. Through the correlative scan matching (CSM), we can combine the predicted values obtained by odometer with the observed values obtained by lidar to get the pose estimation at the current moment.

Correlative scan matching. Correlative scan matching is a scan-to-map matching algorithm which solve the problem in a probabilistic way. The goal of CSM is to find the optimal pose of the current frame in the local map that maximizes the probability of observing the objects. A rectangular area centred on the location estimated by the odometer is used to represent the possible extent of the final location. When matching begins, this region was exhaustive searched in (x, y, θ) windows for obtaining the location δ with the highest response value. M(p) is the probability function of the grid point in the local map, and T_δ is the transformation matrix of pose δ. The optimal pose can be computed by:

CSM also presents a multi-resolution searching strategy and uses lookup table that project the laser data in advance for better real-time performance, and specific theoretical details can be referred to Olson’s paper.

2.2

Back end of SLAM

With the passage of time, the measurement error of odometer and the calculation error of scan matching will gradually accumulate, which greatly reduces the mapping accuracy of the system. We can eliminate these errors as much as possible through global pose graph optimization and loop closure detection. Pose graph optimization can be formulated as a nonlinear least squares problem:

where x is the estimated pose of robot, and is the optimized pose. Ω_k is the information matrix. The goal of graph optimization is to minimize the sum of e_i(x) which represents the error between observations z_i and predictions f_i(x), it can be computed by:

Sparse pose adjustment (SPA) is used to solve the nonlinear optimization problem, and we use loop closure detection to detect loops which will be regarded as the observation constraints in the graph optimization problem. Traditional graph-based SLAM methods use correlative scan matching method to detect and compute the optimal loop closure constraints, it leads to a high quality of matching results at the cost of computation time which will cause the decrease of loop detection efficiency that benefit the global mapping accuracy. In order to speed up the matching process, we propose a GA-based self-adaptive matching strategy for the optimal poses in the relevant local maps.

3.1

GA-based matching approach

By speeding up the loop closure matching process at the back-end, the frequency of loop detection will increase, which can improve the mapping accuracy with more loop constraints when the global map grows large. We achieve this by using the detection and matching method based on Genetic Algorithm.

Determining the searching window. For the scan matching problem, we can use W as the search window, we set the linear and angular search window offsets W_x = W_y =8 m and W_θ = 30°. The linear search resolution is r_t, so we can choose the angular search resolution r_θ as:

where d_max is the maximum range of scan points. We can get the integral number of steps covering given linear and angular search window sizes:

Chromosome coding. We first expand the linear search space in two dimensions to one dimension for representation, i.e., converting set 𝒩 = {–n_x, …, n_x} × {–n_y, …, n_y} into linear index of one dimension, the range of is 0~2w_x ‧ 2w_y. Assume the coordinate of search centre is ε₀ (x₀, y₀), we can get the linear index of point ε_ρ (x_p, y_p) as:

And we use to represent the max value of , which is 2n_x ‧2n_y. In terms of the angular search window, angular index is created to represent the rotation component and the max value .

In the coding step, it is necessary to ensure that the chromosome can represent all the x, у and θ in the feasible solution for global optimization. Let the length of the chromosome be l = l₁+ l₂, where the length of the left part is l₁and the length of the right is l₂. In order to ensure that the genetic algorithm can reach any position of the feasible solution, the l₁ and l₂ must satisfy:

Fitness function. Response value R_v for scan matching is taken as fitness function in the GA, goal_i is the probability value of scan point p_i, and goalmax is the max response value with all the scan points hit surrounding objects.

Genetic operator. Genetic operators include mutation operator, selection operator and crossover operator. The conventional roulette strategy is used for selection operation, and the two-point crossover is employed for crossover operation. The mutation operation adopts the basic position mutation method. Considering the discontinuity of the matching process, the crossover rate should be reduced and the mutation rate should be expanded, which can prevent the genetic algorithm from falling into local iteration.

Chromosome decoding. After chromosomes (i, j) = (i₁i₂ … i_l1,j₁j₂ … j_l2) was obtained by crossover, mutation, and selection, the corresponding linear and angular index needs to be calculated from equations below, this is the decoding process of chromosomes.

3.2

Self-adaptive matching strategy

In the process of loop detection, GA-based matching method can significantly improve the matching efficiency. However, when the geometric structure of the surrounding environment of the robot is relatively simple, it is difficult to find a sufficient number of loops, at this time the accuracy requirement of loop matching is more important than its speed requirement. Genetic algorithm, as a global heuristic approach, has a higher optimization speed than exhaustive search. But this method can’t guarantee the final result to be globally optimal as exhaustive search. In view of the above situation, we make a compromise between matching speed and matching accuracy, and propose an adaptive matching strategy based on the expected number of loops to meet the actual needs of robots in different scenes. The actual number of loops at the current time can only be known after all the loop detection and matching are completed, and impossibly be calculated in advance. For this reason, we define the loop closure factor Ψ to estimate the degree of difficulty in loops detection. The loop closure factor Ψ_κ at the current time can be calculated by the following equation:

where C_i is the number of loop constraints at each time before the current moment, and its average value represents the preliminary prediction of the loop number at the current time. The loop gain η indicates the degree of geometric loop of the surrounding environment, which is essentially the ratio of mileage scalar to distance vector. d_j is the measurement of odometer at each time, t₀ is the translation component of robot pose at the beginning moment, and is the translation component of the estimated pose obtained by the front-end at the current time. In order to achieve self-adaptive matching in different environments, we set thresholds Ψ₁ and Ψ₂, where Ψ₁ < Ψ₂. If Ψ ≤ Ψ₁, the value of loopback detection is beneath expectation, so we could complete the matching through CSM to ensure the matching accuracy. When Ψ₁ < Ψ < Ψ₂, we adopt a compromise of using exhaustive search in the rough matching stage of CSM and GA-based search in the fine matching stage. If Ψ ≥ Ψ₂, only GA based matching method is used to find the optimal solution in the search space.