# A Robust Localization System Fusion Vision-CNN
## 3 System Overview and Methodology
### Map Preprocessing  
An occupation grid map describes the environment in the form of discrete grids. In order to make it more convenient to apply to the positioning algorithm, the information contained in the original grid is expanded to include distance information \(d\) and score information \(score\). The specific steps are as follows. First, set the distance information of the occupied grid to 0 and the score to 1, with the remaining grid distances set to the maximum and the score to 0. Then, start from the occupied grid and expand outwards, updating the distance information and the score information of the surrounding grids. Assuming that the coordinates of the grid are \((x_{n},y_{n})\) and the grid coordinates of the nearest obstacle are \((x_{obstacle},y_{obstacle})\), then the distance information of the grid is calculated as a Euclidean distance:  
\[d(x_{n},y_{n})=\min\{\sqrt{(x_{n}-x_{obstacle})^{2}+(y_{n}-y_{obstacle})^{2}},d _{\max}\} \tag{2}\]  
In Formula (2), \(d_{\max}\) is the preset value. At the same time, the score information can be calculated according to the distance information:  
\[score(x_{n},y_{n})=\exp(-d(x_{n},y_{n})*d(x_{n},y_{n})/(2*sigma*sigma)) \tag{3}\]  
In Formula (3), \(sigma\) is the design parameter. In this way, the resolution of the updated map is the same as that of the original occupancy grid map (usually 5 cm). When initializing the global pose or the relocalization, there may be a large error between the actual pose and the initial estimated pose. Precise pose estimation at this resolution requires sampling a large number of particles to cover the pose space, which will greatly reduce the real-time performance of the system. Here, the original map is processed with multi-resolution first and then the branch and bound algorithm is used to accelerate the search for the optimal pose. When using branch and bound to solve, first sample the particles on the low-resolution grid map to obtain the particle with the highest score. On the basis of this particle, the resolution of the map is improved and a higher-precision pose is obtained until  
Figure 2: Convolutional neural network structure.  
the optimal pose is searched under the highest resolution map. In order to ensure that the method can obtain the global optimal solution in the search window, the low-resolution grid map needs to meet the following constraint conditions. Suppose the low-resolution grid map to be calculated is \(M^{h}\), and the sampling step of translation is \(2^{h}\) grid cell size. Under a specific rotation, the number of particles contained at the highest resolution is \(2^{h}\times 2^{h}\), and the corresponding state is \((\xi_{j},j=0:2^{h}\times 2^{h})\). When calculating the score of the laser point, the following needs to be met:  
\[score(c)=\sum_{k=1}^{K}M^{h}(T_{\xi_{j}}z_{k})\geq\max_{\xi_{j}}\sum_{k=1}^{K }M^{0}(T_{\xi_{j}}z_{k}) \tag{4}\]  
where \(M^{0}\) is the original high-resolution grid map and \(c\) is the particle index. The robot pose transformation matrix is \(T_{\xi_{c}}\) represented by particle \(c\) sampled based on the low-resolution grid map \(M^{h}\). The possible pose transformation matrix is \(T_{\xi_{j}}\) represented by the corresponding \(2^{h}\times 2^{h}\) particles on the original high-resolution map \(M^{0}\). The laser measurement data is \(z_{k}\), \(K\) is the number of laser measurement points. In order to ensure that the inequality is true, the method for solving the low-resolution map \(M^{h}\) according to the high-resolution map \(M^{0}\) is:  
\[M^{h}(x,y)=\max_{\begin{array}{c}x^{\prime}\in[x,x+r(2^{h}-1)]\\ y^{\prime}\in[y,y+r(2^{h}-1)]\end{array}}M^{0}(x^{\prime},y^{\prime}) \tag{5}\]  
where \(M^{0}\) and \(M^{h}\) have the same number of pixels. The original grid cell size is \(r\) and the preprocessed grid map is shown in the Figure 3. The darker the color, the closer to the obstacle.  
Figure 3: Grid map preprocessing. (a) h = 0; (b) h = 2; (c) h = 4; (d) original map.