# Robust Localization of the Mobile Robot Driven by Lidar Measurement and Matching for Ongoing Scene
## 3 Localization of Mobile Robot in Ongoing Scene
### Localization of Artificial Landmarks and Generated Buildings Based on Adaptive Weights
#### 3.2.1 Frame of ICP Based on Generated Buildings  
Although the localization accuracy based on three-sided measurement is high and does not need the initial pose, the more numbers of artificial landmarks need to be set and moved by constructors. So, the ongoing scene can be regarded as a low-speed dynamic changing environment with a certain sequence. As the buildings are gradually generated, the feature-based ICP method is adopted to achieve the localization of mobile robot. According to [22], two sets of Lidar point cloud with overlapping areas are established, and they represent the source point cloud set \(S_{i}\) and the objective point cloud set \(P_{i}\), respectively. At this time, \(S_{i}\)\(=(x_{i},y_{i}),i=1,2,\cdots\cdots,n\), \(P_{i}\)\(=(x_{i},y_{i}),i=1,2,\cdots,n\), and the parameters n is the numbers of the two matching points. Then, an error matching function of two adjacent locations is established by using the nearest neighbor point principle. At last, the objective equation converges gradually to the minimum through iterations. The specific formula is as follows:  
\[E(R_{i},t_{i})=\underset{R\in\mathcal{O}(2),t\in\Re^{2}}{\operatorname{ argmin}}\frac{1}{n}\sum_{i=1}^{k}\left\|P_{i}-(R_{i}S_{i}+t_{i})\right\|^{2} \tag{13}\]  
where \(R_{i}\) and \(t_{i}\) are the rotation matrix and translation vector of the \(i\)-th locations, respectively. The value \(E(R_{i},t_{i})\) are the matching errors of point clouds at two locations, and the number k represents numbers of the nearest point pairs. In order to minimum the objective function, the Singular Value Decomposition (SVD) method is the common tool to solve it [23]. Assuming the centroids of the point clouds are represented as follow:  
\[\left\{ \begin{array}{l}P=\frac{1}{n}\sum\limits_{i=1}^{n}P_{i}\\ S=\frac{1}{n}\sum\limits_{i=1}^{n}S_{i}\end{array} \right. \tag{14}\]  
Then, the objective function (13) can be rewritten as:  
\[ \begin{array}{l}E(R,t)=\frac{1}{n}\sum\limits_{i=1}^{n}\left\|P_{i}-(RS_{i}+ t)\right\|^{2}\\ =\frac{1}{n}\sum\limits_{i=1}^{n}\left\|P_{i}-RS_{i}-t-P+RS+P-RS)\right\|^{2} \\ =\frac{1}{n}\sum\limits_{i=1}^{n}\left\|(P_{i}-P-R(S_{i}-S))+(P-RS-t)\right\|^{2 }\\ =\frac{1}{n}\sum\limits_{i=1}^{n}\left(\left\|P_{i}-P-R(S_{i}-S)\right\|^{2}+ \left\|P-RS-t\right\|^{2}\right.\\ \left.+2(\underbrace{P_{i}-P-R(S_{i}-S))^{T}(P-RS-t)}_{zero}\right)\end{array}  \tag{15}\]  
Figure 5: Robust localization model based on artificial reflectors and ongoing buildings. We can just minimize the former part, and set the third part to zero. So, the centroid can be removed by following equation:  
\[\left\{ \begin{array}{l}P^{\prime}=P_{i}-P\\ S^{\prime}=S_{i}-S\end{array} \right. \tag{16}\]  
and the minimum value of objective function (15) can be converted to following equation:  
\[E(R,t)=\text{argmin}\frac{1}{n}\sum\limits_{i=1}^{n}{{{\left\|{P^{\prime}-RS^{ \prime}}\right\|}^{2}}}. \tag{17}\]  
At this time, the Equation (17) can be deduced as:  
\[\frac{1}{n}{\sum\limits_{i=1}^{n}{{{\left\|{P^{\prime}-RS^{\prime}}\right\|}^ {2}}}}=\frac{1}{n}{\sum\limits_{i=1}^{n}{{{\left({P^{\prime}}^{T}{P^{\prime}}+{ S^{\prime}}^{T}{R^{T}}{R^{P^{\prime}}-2{P^{\prime}}^{T}{RS^{\prime}}}\right)}}}}. \tag{18}\]  
The third part of Equation (18) can be represented as:  
\[ \begin{array}{l}\frac{1}{n}{\sum\limits_{i=1}^{n}{{{\left({-2{P^{\prime}}^{T} {RS^{\prime}}}\right)}}}}=\frac{2}{n}{\sum\limits_{i=1}^{n}{{{\left-{\text{ tr}}({RS^{\prime}}{P^{\prime}}^{T})}\right.}}}\\ =-tr{\left({\frac{2}{n}R\sum\limits_{i=1}^{n}{{{S^{\prime}}{P^{\prime}}^{T}}} }\right)}\end{array}  \tag{19}\]  
Finally, the objective equation is solved by SVD, where \(W=UDV^{T}\), and the transformation matrices are  
\[\left\{ \begin{array}{l}R=UV^{T}\\ {\text{t}}=P-RS\end{array} \right.. \tag{20}\]  
When \(E(R_{i},t_{i})\) gets the minimum, the optimal matching matrix for the two positions is calculated.