Innovation-Superposed Simultaneous Localization and Mapping of Mobile Robots Based on Limited Augmentation

Abstract

In this paper, Aaiming at the problem of simultaneous localization mapping (SLAM) for mobile robots, a limited-augmentation innovation superposition (LAIS) is proposed to solve the problems occurring in SLAM. By extending the single-time innovation superposition to multi-time innovation, the error accumulation during the movement of mobile robots is reduced and the accuracy of the algorithm is improved. At the same time, when the number of feature points observed by the sensor exceeds the threshold, the sensor range is restricted. Therefore, only the qualified feature points are added to the system state vector, which reduces the calculation amount of the algorithm and improves the running speed. Simulation results show that compared with other algorithms, LAIS has higher accuracy and higher running speed in environmental maps with a different number of landmark points.

1. Introduction

With the continuous development of mobile robot technology, the research on mobile robot autonomy has become more and more important, and the research on the operation of mobile robots in an unknown environment has attracted more and more attention [1,2]. Localization and mapping are two important research directions related to mobile robots in an unknown environment, and there exists a very close relationship between them. Positioning means that the robot can know its position in the environment, which requires map construction and precise positioning of the robot to obtain accurate map information of the environment [3,4,5]. Therefore, the idea of simultaneous location mapping, also known as SLAM technology, has been of wide concern. At present, there are also many methods to solve the problem of robot SLAM [6].

At present, many algorithms are used to solve SLAM problems, such as the extended Kalman filter algorithm [7,8,9,10], particle filter algorithm [11], RBPF SLAM [12], etc. Extended Kalman filter (EKF) is a popular SLAM method in the field of robot navigation because of its high mathematical rigor and algorithm structure [13]. Particle filter, also known as the sequential Monte Carlo method, bootstrap filter, or clustering algorithm, is an implementation method of Markov positioning. This method does not need to directly solve the probability density function, and determines the reliability of attitude through a series of random sampling points [14]. Layered RBPF SLAM is a robust SLAM framework recently proposed for indoor environments with sparse and proximity sensors. In order to overcome the limitation of the sensors, the whole area is divided into several independent local maps [15].

The SLAM of mobile robots in an unknown environment is studied in this paper. The earliest study on mobile robot SLAM is estimated to be conducted by Smith et al. [16], using an extended Kalman filter. EKF SLAM can generate a more accurate estimate of unknown variables based on each measured value at different times, taking into account the joint distribution of each measured value, than algorithms based on a single measured value alone [17]. However, the linear error of the traditional EKF SLAM algorithm increases with the increase in iteration times, which will lead to divergence of the SLAM system, and then lead to incorrect mapping [18]. In order to improve this problem, Sage and Husa [19] proposed the AEKF SLAM algorithm. By introducing a forgetting factor into the traditional EKF SLAM algorithm, the process noise and observation noise of the SLAM system during the movement of the mobile robot can be corrected in real time to ensure the convergence of the algorithm. However, the introduction of a forgetting factor will reduce the process noise estimation and the accuracy of the algorithm [20].

In order to improve the accuracy of the algorithm, the AEKF SLAM algorithm will be introduced after multi-innovation superposition, and the adjacent time information can be effectively utilized to make the filtering algorithm more accurate [21,22,23]. During the movement of the mobile robot, with the increase in the number of observed landmark points, the dimensions of system state vector keep increasing, which makes the calculation amount of covariance matrix and Jacobi matrix keep increasing. In addition, the superposition of single innovation is extended to multi-time innovation, which further increases the calculation difficulty and reduces the operation speed of the algorithm [24]. In order to improve the speed of algorithm, this paper limits the augmented vector of the system state. When the number of feature points observed by the sensor at a certain moment is large, the observation range of the sensor will be constrained, and the information of the feature point outside the constraint range will no longer be added to the system state augmented vector, which reduces the increase in the dimension of the system state vector in each iteration to decrease the calculation amount and improves the running speed of the algorithm.

The organizational structure of this paper is as follows: in Section 1, the motion model and the observation model of mobile robots are analyzed; in Section 2, a synchronous positioning and mapping algorithm for innovative mobile robots based on restriction augmentation is designed. The single-time innovation of robot motion process is superimposed into multi-time innovation to improve the accuracy of the algorithm. The limitation of system state vector augmentation is increased to improve the running speed of the algorithm. In Section 3, the performance of each algorithm is compared and analyzed through simulation experiments. In Section 4, the conclusion will be given.

2. Problem Formulation

The system model of the mobile robot is discussed here. When the mobile robot is in a position environment, the robot cannot know the environment map and its position in the map. At this time, the robot needs to observe the surrounding environment when moving, estimate its position and posture in the environment, and construct the environment map through the observed environmental information, which is the principle of the SLAM algorithm. In order to realize the SLAM algorithm, it is necessary to know the control input $u_k$ and record the observation information $Z_k$ of sensors in the process of robot motion; the motion model and observation model of the system are obtained. The motion model is used to describe the motion process from $k-1$ time to $k-1$ time under the action of the mobile robot. The observation model is used to obtain the observation value of the mobile robot to the environmental road sign. The motion model is used to describe the motion process of the mobile robot from $k-1$ time to $k$ time under the action of the control input, and the observation model is used to obtain the observation value of the mobile robot to the environmental road sign.

2.1. System Motion Model

In this paper, the robot motion is controlled by giving a translation speed $v_k$ and a rotation speed ${\omega }_k$. Assuming that the control input of the robot is constant in unit time, that is, the given translation speed and rotation speed do not change with time, under the control of this constant input $u_k={(v_k,{\omega }_k)}^T$, the robot will complete a circular arc motion from $k-1$ time to $k$ time, as shown in Figure 1, where $A_{k-1}$ is the pose of $k-1$ time, represented by ${(x_{k-1},y_{k-1},{\theta }_{k-1})}^T$; $A_k$ is the robot pose at $k$ times, which is represented by ${(x_k,y_k,{\theta }_k)}^T$.

The kinematics equation of the robot is Equation (1), where $w_k$ is the noise of the mobile robot system.

$$\begin{array}{l}\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\theta }_{k-1}\end{array}\right]+\left[\begin{array}{c}-\frac{v_k}{{\omega }_k}\sin {\theta }_{k-1}+\frac{v_k}{{\omega }_k}\sin ({\theta }_{k-1}+{\omega }_t\Delta t)\\ \frac{v_k}{{\omega }_k}\cos {\theta }_{k-1}-\frac{v_k}{{\omega }_k}\cos ({\theta }_{k-1}+{\omega }_t\Delta t)\\ {\omega }_t\Delta t\end{array}\right]+w_k\end{array}$$

(1)

In the process of constructing a SLAM point map, we need to estimate not only the robot’s motion, but also the road signs in the map, so we need to add the road sign information in the map to the state equation of the system, so that we can get the state equation of the robot in the whole SLAM system (2). Among them, $X_{i,k}^m$ is the coordinate of the $ith$ signpost point observed by the robot at time $k$, which does not change with the motion of the mobile robot.

$$\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\\ X_{1,k}^m\\ \cdots \\ X_{i,k}^m\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}-\frac{v_k}{{\omega }_k}\sin {\theta }_{k-1}+\frac{v_k}{{\omega }_k}\sin ({\theta }_{k-1}+{\omega }_k\Delta k)\\ y_{k-1}+\frac{v_k}{{\omega }_k}\cos {\theta }_{k-1}-\frac{v_k}{{\omega }_k}\cos ({\theta }_{k-1}+{\omega }_k\Delta k)\\ {\omega }_k\Delta k\\ X_{1,k-1}^m\\ \begin{array}{c}\cdots \\ X_{i,k-1}^m\end{array}\end{array}\right]$$

(2)

2.2. Systematic Observation Model

As shown in Figure 2, during the movement of the mobile robot, the position of the second road sign observed by the vision sensor at time $i$ can be expressed as the star-$M_i$=${(x_{i,k}^m,y_{i,k}^m)}^T$, $i=1,2\cdots n$. Among them, $n$ is the total number of road signs.

A mobile robot uses external sensors to perceive the features of the real-world environment. In this work, the sonars are adopted as the external sensors of the robot to provide the measurements of the range and the bearing of the distance and orientation of observation feature $(x_{i,k}^m,y_{i,k}^m)$ [25]. In this paper, assuming that the position of all road signs does not change, the observation equation of the SLAM system can be obtained as follows:

$$z_{i,k}^m=\left[\begin{array}{c}\sqrt{{(x_{i,k}^m-x_k^{})}^2+{(y_{i,k}^m-y_k^{})}^2}\\ \arctan \frac{y_{i,k}^m-y_k^{}}{x_{i,k}^m-x_k^{}}-{\theta }_k^{}\end{array}\right]$$

(3)

3. LAIS SLAM

3.1. AEKF SLAM

Based on the standard EKF SLAM algorithm, the AEKF SLAM algorithm introduces a forgetting factor to correct the process noise and observation noise in real time, which ensures the convergence of the algorithm. The specific implementation process is as follows:

Suppose that the $k$ time state of system can be expressed by nonlinear state transition matrix $f\left(•\right)$ and the system observation can be expressed by nonlinear observation matrix $h\left(•\right)$, then the system state equation and observation equation can be expressed as:

$$X_k=f(X_{k-1},u_k)+w_{k-1}$$

(4)

$$Z_k=h(X_k)+v_k$$

(5)

where $X_k$ is the $k$-time SLAM system state vector, $u_k$ is the system input control variable, and $w_{k-1}$ is the process noise from $k-1$ to $k$, with a mean value of $q_{k-1}$ and a covariance of $Q_{k-1}$. $Z_k$ is the $k$-time systematic observation vector, $v_k$ is the $k$-time systematic observation noise, and its mean value is $r_k$ and covariance is $R_k$.

(1)

The algorithm prediction step

The prior estimation equation of system state is:

$${\widehat{X}}_k^{-}=f({\widehat{X}}_{k-1})+{\widehat{q}}_k$$

(6)

The state prior estimated covariance equation is:

$$P_k^{-}=F_k{P}_{k-1}{F}_k^T+W{\widehat{Q}}_k{W}^T$$

(7)

where $F_k$ is the Jacobi matrix obtained by taking the partial derivative of $f\left(•\right)$ with respect to $X_k$, and ${\widehat{q}}_k$ is the estimated value of the system process noise.

Where $W$ is the Jacobian matrix obtained by taking partial derivatives from $f\left(·\right)$ to $w_k$, ${\widehat{Q}}_k$ is the covariance matrix of the estimated value of system process noise at $k$ time.

(2)

The algorithm update step

The innovation vector of the system at time $k$ is:

$${\tilde{Z}}_k=Z_k-H_k{\widehat{X}}_{k-1}-{\widehat{r}}_k$$

(8)

where $H_k$ is the Jacobian matrix obtained by taking partial derivatives from $h\left(·\right)$ to $X_k$, and ${\widehat{r}}_k$ is the estimated value of system observation noise.

The Kalman gain matrix is:

$$K_k=\frac{P_k^{-}{H}_k^T}{H_k{P}_k^{-}{H}_k^T+{\widehat{R}}_k}$$

(9)

The posterior estimation equation of system state is:

$${\widehat{X}}_k={\widehat{X}}_k^{-}+K_k{\tilde{Z}}_k$$

(10)

The posterior estimation covariance equation is:

$$P_k=(I-K_k{H}_k)P_k^{-}$$

(11)

In AEKF SLAM algorithm, a forgetting factor is introduced into standard EKF SLAM algorithm to correct system errors in real time. The correction process is as follows:

$${\widehat{r}}_k=(1-d_k){\widehat{r}}_{k-1}+d_k(Z_k-H_k{X}_{k+1|k})$$

(12)

where ${\widehat{r}}_k$ is observation noise of system estimation at $k$-time.

$${\widehat{R}}_k=(1-d_k){\widehat{R}}_{k-1}+d_k({\tilde{Z}}_k{\tilde{Z}}_k{}^T-H_k{P}_k{H}_k^T)$$

(13)

where ${\widehat{R}}_k$ is covariance matrix of system observation noise estimation at $k$-time.

$${\widehat{q}}_k=(1-d_k){\widehat{q}}_{k-1}+d_k({\widehat{X}}_k-F_k{\widehat{X}}_k)$$

(14)

where ${\widehat{q}}_k$ is process noise of system estimation at $k$-time.

$${\widehat{Q}}_k=(1-d_k){\widehat{Q}}_{k-1}+d_k(K_k{\tilde{Z}}_k{\tilde{Z}}^T{}_k{K}_k{}^T+P_k-F_k{P}_{k-1}{F}_k^T)$$

(15)

where ${\widehat{Q}}_k$ is covariance matrix of system process noise estimation at $k$-time.

$$d_k=\frac{1-b}{1-b^{k+1}}\begin{array}{cc}& \end{array}(0<b<1)$$

(16)

where $b$ is the forgetting factor, and 0.996 is taken to correct the process noise and observation noise of the system in real time.

3.2. IS AEKF SLAM

After the forgetting factor is introduced into the standard extended Kalman filter algorithm, the adaptive extended Kalman filter is designed, which can effectively ensure the convergence of the algorithm. However, when the forgetting factor is added, the process noise estimation $q_k$ of the system is small, which makes the accuracy of the algorithm lower.

In order to improve the accuracy of the algorithm, this paper combines the multi-innovation theory with the adaptive extended Kalman filter and designs an adaptive extended Kalman filter algorithm based on innovation superposition. Multi-innovation theory is a more accurate identification method based on single-innovation identification theory. It describes the motion law of the system by establishing a mathematical model. The main idea is to modify the current model by using the parameter estimation of the previous time system output and combining the product of gain vector and innovation. In order to improve the prediction accuracy of the algorithm, the innovation matrices of multiple moments during the movement of the mobile robot are superimposed, and the effective information of each moment is fully utilized. The specific implementation process is as follows:

$${\tilde{Z}}_k^n=\left[\begin{array}{c}{\tilde{Z}}_k\\ {\tilde{Z}}_{k-1}\\ \cdots \\ {\tilde{Z}}_{k-n+1}\end{array}\right]=\left[\begin{array}{c}{Z}_k-H_k{\widehat{X}}_k-{\widehat{r}}_k\\ Z_{k-1}-H_{k-1}{\widehat{X}}_{k-1}-{\widehat{r}}_{k-1}\\ \cdots \\ Z_{k-n+1}-H_{k-n+1}{\widehat{X}}_{k-n+1}-{\widehat{r}}_{k-n+1}\end{array}\right]$$

(17)

According to this, the state update equation of the system can be obtained as follows:

$${\widehat{X}}_k={\widehat{X}}_k^{-}+K_k{\tilde{Z}}_k{}^n$$

(18)

The covariance matrix of the system state vector is:

$$P_k=(I-K_k{H}_k)P_k^{-}$$

(19)

The system observation noise is estimated as:

$${\widehat{r}}_k=(1-d_k){\widehat{r}}_{k-1}+d_k(Z_k-H_{i,k}{X}_{k+1|k})$$

(20)

The covariance updating equation of the estimated observation noise of the system is as follows:

$${\widehat{R}}_k=(1-d_k){\widehat{R}}_{k-1}+d_k({\tilde{Z}}^n{}_k{\tilde{Z}}^n{{}_k}^T-H_k{P}_k{H}_k^T)$$

(21)

3.3. LAIS SLAM

During the movement of the mobile robot, the system state vector is a $3+2n$-dimensional vector, which represents the position information of three pose quantities and $n$ road landmark points of the robot. With the increase in the number of landmarks observed by the robot, the dimension of the system state vector is also increased, which leads to the increase in the calculation amount of covariance matrix and the slow speed of the algorithm.

In order to solve the above problems, we can restrict the observation range of the sensor and set the maximum observation range of the sensor to ${\beta }_{\max }$. $M_k={(x_k^m,y_k^m)}^T$ is a feature point observed by the sensor at three times, and the distance between it and the robot is $\beta$. If $\beta <{\beta }_{\max }$, the $M_k$ coordinate information is added into the system state augmented matrix and updated according to IS-AEKF-SLAM algorithm; if $\beta >{\beta }_{\max }$, it is ignored. This method can reduce the dimension of the state vector of the system at a certain time, reduce the amount of computation, and make the algorithm faster. However, when there are more distant points among the observed feature points at a certain time, the system will only add the points whose distance from the robot is less than ${\beta }_{\max }$ to the state vector, and other points will be ignored. This will lead to the inefficiency of map building, and there may be unobserved road signs, which will lead to incorrect map building. In this paper, a variable $s$ is given to limit the augmentation of the system state vector, and its expression is:

$$s=n-\tilde{n}$$

(22)

In Equation (22), $n$ is the dimension of observation vector at $k$ times, and $n=3+2i$.$i$ are the number of feature points observed by sensors at $k$ time. $\tilde{n}$ is the dimension of the system state vector at time $k-1$.

At time $k$, the sensor observation information can be as follows:

(1)

$s\le \tilde{n}/2$. In this case, the dimension of the system state vector at time $k-1$ is not much different from that of the sensor observation vector at time $k$, and the dimension of the system state vector will not increase too much after expansion, so there is no constraint on the sensor observation range.

(2)

$s>\tilde{n}/2$. In this case, the dimension of system state vector at time $k-1$ is quite different from that of sensor observation vector at time $k$. If all the observed feature points are added to the system state vector, the dimension of system state vector will be greatly increased and the algorithm accuracy will be reduced. At this time, the sensor observation range can be constrained, and the feature points whose distance from the robot is greater than ${\beta }_{\max }$ are ignored and not included in the system state vector, thus improving the running speed of the algorithm.

Let the sensor observe $i$ characteristic points at $k$ time, and the observation vector can be expressed as:

$$Z_k=(x_k,y_k,{\theta }_k,x_{1,k}^m,y_{1,k}^m,\cdots x_{i,k}^m,y_{i,k}^m)$$

(23)

Assuming that $\tilde{i}$ feature points out of $i$ feature points observed by the sensor are outside the constrained observation range of the sensor, the feature points outside the observation range are deleted to obtain a new observation vector:

$$Z_{new,k}=(x_k,y_k,{\theta }_k,x_{1,k}^m,y_{1,k}^m,\cdots x_{i-\tilde{i},k}^m,y_{i-\tilde{i},k}^m)$$

(24)

(1)

The prediction part of the algorithm

According to the system motion model, the robot moves from $k-1$ to $k$ under the action of control input $u_k$, and the sensor constantly observes the location information of the surrounding landmark points during the movement process. Thus, the prior state estimation equation of the mobile robot system can be written as:

$${\widehat{X}}_k^{-}=\left[\begin{array}{c}{x}_{k-1}-\frac{v_k}{{\omega }_k}\sin {\theta }_{k-1}+\frac{v_k}{{\omega }_k}\sin ({\theta }_{k-1}+{\omega }_k\Delta k)\\ y_{k-1}+\frac{v_k}{{\omega }_k}\cos {\theta }_{k-1}-\frac{v_k}{{\omega }_k}\cos ({\theta }_{k-1}+{\omega }_k\Delta k)\\ {\omega }_k\Delta k\\ X_{1,k-1}^m\\ \begin{array}{c}\cdots \\ X_{i,k-1}^m\end{array}\end{array}\right]+{\widehat{q}}_k$$

(25)

The prior estimation covariance equation of system state at time $k$ is:

$$P_k^{-}=F_k{P}_{k-1}{F}_k^T+WQ{W}^T$$

(26)

Since the position of each landmark point does not change with time during the movement of the mobile robot, only the robot’s own state updates with time, so the Jacobi matrix is:

$$F_k=\left[\begin{array}{ccc}0& 0& -\frac{v_k}{{\omega }_k}\cos {\theta }_{k-1}+\frac{v_k}{{\omega }_k}\cos ({\theta }_{k-1}+{\omega }_t\Delta t)\\ 0& 0& -\frac{v_k}{{\omega }_k}\sin {\theta }_{k-1}+\frac{v_k}{{\omega }_k}\sin ({\theta }_{k-1}+{\omega }_t\Delta t)\\ 0& 0& 0\end{array}\right]F_j$$

(27)

$$F_j=\left[\begin{array}{cccccc}1& 0& 0& 0& \cdots & 0\\ 0& 1& 0& 0& \cdots & 0\\ 0& 0& 1& 0& \cdots & 0\end{array}\right]$$

(28)

(2)

The algorithm update part

After limiting and augmenting the system state vector, the feature point information outside the sensor constraint range is deleted from the observation vector, and a new observation vector $Z_{new,k}$ is obtained. From $Z_{new,k}$, the innovation vector after limiting and augmenting and the extended multi-innovation matrix can be written as:

$${\tilde{Z}}_{new,k}=Z_{new,k}-H_k{\widehat{X}}_{k-1}^{-}-{\widehat{r}}_{new,k}$$

(29)

$${\tilde{Z}}_{new,k}^n=\left[\begin{array}{c}{\tilde{Z}}_{new,k}\\ {\tilde{Z}}_{new,k-1}\\ \cdots \\ {\tilde{Z}}_{new,k-n+1}\end{array}\right]=\left[\begin{array}{c}{Z}_{new,k}-H_k{\widehat{X}}_k^{-}-{\widehat{r}}_k\\ Z_{new,k-1}-H_{k-1}{\widehat{X}}_{k-1}^{-}-{\widehat{r}}_{k-1}\\ \cdots \\ Z_{new,k-n+1}-H_{k-n+1}{\widehat{X}}_{k-n+1}^{-}-{\widehat{r}}_{k-n+1}\end{array}\right]$$

(30)

According to the systematic observation model, the Jacobi matrix of partial derivative of $h\left(•\right)$ with respect to $X_k$ can be written:

$$H_k=\left[\begin{array}{cccccc}\frac{x_k^a-x_{i,k}^m}{\sqrt{m}}& \frac{y_k^a-y_{i,k}^m}{\sqrt{m}}& 0& \frac{x_{i,k}^m-x_k^a}{\sqrt{m}}& \frac{y_{i,k}^m-y_k^a}{\sqrt{m}}& 0\\ \frac{y_{i,k}^m-y_k^a}{m}& \frac{x_k^a-x_{i,k}^m}{m}& -1& \frac{y_k^a-y_{i,k}^m}{m}& \frac{x_{i,k}^m-x_k^a}{m}& 0\end{array}\right]H_j$$

(31)

$$H_j=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& \cdots & 0& 0& 0& 0& \cdots & 0\\ 0& 1& 0& 0& \cdots & 0& 0& 0& 0& \cdots & 0\\ 0& 0& 1& 0& \cdots & 0& 0& 0& 0& \cdots & 0\\ 0& 0& 0& 0& \cdots & 0& 0& 0& 0& \cdots & 0\\ 0& 0& 0& 0& \cdots & 0& 1& 0& 0& \cdots & 0\\ 0& 0& 0& 0& \cdots & 0& 0& 1& 0& \cdots & 0\end{array}\right]$$

(32)

Kalman gain after limiting augmentation is:

$$K_{new,k}=\frac{P_k^{-}{H}_k^T}{H_k{P}_k^{-}{H}_k^T+{\widehat{R}}_{new,k}}$$

(33)

The posterior estimation equation of system state is:

$${\widehat{X}}_{new,k}={\widehat{X}}_k^{-}+K_{,new,k}{\tilde{Z}}^n{}_{new,k}$$

(34)

The posterior estimation covariance equation is:

$$P_{new,k}=(I-K_{new,k}{H}_k)P_k^{-}$$

(35)

where ${\widehat{r}}_{new,k}$, ${\widehat{q}}_{new,k}$, ${\widehat{R}}_{new,k}$, and ${\widehat{Q}}_{new,k}$ are the estimated values of process noise and observation noise of the restricted augmented system and their respective covariance matrices.

$${\widehat{r}}_{new,k}=(1-d_k){\widehat{r}}_{k-1}+d_k(Z^n{}_{new,k}-H_k{X}_{k+1|k})$$

(36)

$${\widehat{R}}_k=(1-d_k){\widehat{R}}_{k-1}+d_k({\tilde{Z}}^n{}_{new,k}{\tilde{Z}}^n{{}_{new,k}}^T-H_k{P}_{new,k}{H}_k^T)$$

(37)

$${\widehat{q}}_{new,k}=(1-d_k){\widehat{q}}_{k-1}+d_k({\widehat{X}}_{new,k}-F_k{\widehat{X}}_{new,k})$$

(38)

$${\widehat{Q}}_{new,k}=(1-d_k){\widehat{Q}}_{k-1}+d_k(K_k{\tilde{Z}}^n{}_{new,k}{\tilde{Z}}^n{{}_{new,k}}^T{K}_{new,k}{}^T+P_k-F_k{P}_{k-1}{F}_k^T)$$

(39)

4. LAIS SLAM Simulation Experiment and Result Analysis

4.1. Establishment and Simulation of Environmental Map

In this paper, MATLAB is used to build an environmental map. The map covers an area of 120 m × 100 m, 80 Eighty blue star landmarks were set, and eight key points are selected as the actual trajectory of the robot. Assuming that the position of the sensor is the center of mass of the robot, the robot can be regarded as a particle in the process of motion. The mobile robot starts from the first key point, linear velocity $\mathrm{v}=4 \mathrm{m}/\mathrm{s}$, and angular velocity $\omega =1.5 \mathrm{rad}/\mathrm{s}$. The observation range of the sensor is 35 m and 180 degrees. The sensor constrains the observation range ${\beta }_{\max }=20 \mathrm{m}$. In this environment, the mobile robot uses the IS-AEKF-SLAM algorithm and restricted augmented IS-AEKF-SLAM algorithm to locate and build maps, and the simulation results are shown in Figure 3.

Figure 3 shows the real trajectory of the mobile robot and moving trajectory of the mobile robot under various algorithms. It can be seen that in the initial stage of robot positioning mapping, the trajectory of the mobile robot under each algorithm is not much different from the real trajectory, but with the increase in time, under the action of mobile robot system noise, each algorithm begins to accumulate errors, which leads to the trajectory deviation. As can be seen from Figure 3, the difference between the running trajectory of the mobile robot and the actual running trajectory of the mobile robot is the smallest under the LAIS SLAM algorithm.

4.2. Analysis of Simulation Results of LAIS SLAM Algorithm

Figure 4 shows the errors between each algorithm and the real value in the x axial direction, the errors between each algorithm and the real value in the y axial direction, and the errors between each algorithm and the real value in the rotation angle when there are 80 road landmark points on the map. It can be seen from the result diagram that in the first 200 iterations of the algorithm, the errors of the four algorithms are not much different, but with the increase in the number of iteration times, each algorithm begins to present errors. Compared with the other three algorithms, the error curve of the LAIS SLAM algorithm is more stable and the error value is the smallest; that is, the accuracy is the highest.

Because the process noise and observation noise of the mobile robot are random and the simulation results are different every time, it is impossible to judge the performance of the algorithm only by the results of one simulation experiment. In order to avoid contingency of the experiment and ensure rigor, the number of road signs in the map was changed to 60 for the subsequent simulation experiment. The experimental results are shown in Figure 5. As can be seen from the following result, the LAIS SLAM algorithm still has the highest accuracy compared with the other three algorithms after changing the road signs of the environmental map.

In the above experiment, the system noise was all achromatic noise. In order to increase the reliability of the experiment, the noise was changed to colored noise, and the simulation experiment was carried out again. The results are shown in Figure 6. It can be seen from the experimental results that the LAIM SLAM algorithm still has higher accuracy after the noise is changed to colored noise.

In the case of achromatic noise and the number of road points on the map is 60. The root-mean-square error (RMSE), the mean absolute error (MRE) and the maximum absolute error (MAE) of each algorithm in three directions are shown in

Table 1. The RMSE of each algorithm in three directions.

	SLAM	EKF-SLAM	AEKF-SLAM	ISAEKF-SLAM	LAIS-SLAM
Direction	RMSE
X(m)		0.4413	0.4187	0.4059	0.3984
Y(m)		0.4282	0.4191	0.4299	0.3872
$\theta$(rad)		0.0451	0.0442	0.0403	0.0301

,

Table 2. The MRE of each algorithm in three directions.

	SLAM	EKF-SLAM	AEKF-SLAM	ISAEKF-SLAM	LAIS-SLAM
Direction	RMSE
X(m)		5.27	3.15	2.26	0.55
Y(m)		4.86	4.01	2.72	0.57
$\theta$(rad)		0.18	0.082	0.15	0.027

, and

Table 3. The MAE of each algorithm in three directions.

	SLAM	EKF-SLAM	AEKF-SLAM	ISAEKF-SLAM	LAIS-SLAM
Direction	RMSE
X(m)		9.002	6.146	4.792	1.056
Y(m)		10.93	7.457	4.584	1.168
$\theta$(rad)		0.24	0.19	0.22	0.08

, respectively. From Table 1, Table 2 and Table 3 it can be seen that compared with other algorithms, the LAIS SLAM algorithm has achieved higher accuracy.

In order to verify the running speed of the algorithm, this paper compares the complete positioning and mapping time of four algorithms when the number of road signs in the environmental map is 60 and 80, respectively. The results are shown in

Table 4. Time for complete map construction of each algorithm.

	SLAM	EKF-SLAM	AEKF-SLAM	ISAEKF-SLAM	LAIS-SLAM
Numberof Road Signs	Time(s)
60		90.8	89.6	90.4	80.7
80		122.4	120.5	122.7	118.6

. It can be seen from Table 4 that the times required by the first three algorithms are not much different, while the time required by LAIS SLAM is reduced by 11% compared with the first three algorithms. It shows that the running speed of the algorithm is obviously improved after adding the state vector of the restricted system.

5. Conclusions

In this paper, a restricted augmented SLAM algorithm for innovative stacked mobile robots is proposed. In this algorithm, the single-time innovation superposition is extended to multi-time innovation in the process of mobile robot motion, which reduces the error accumulation in the iterative process. Simulation results show that this algorithm has higher accuracy than EKF SLAM, AEKF SLAM and IS AEKF SLAM in the same map environment. The simulation results show that the LAIS SLAM algorithm takes 11% less time and has a higher running speed than the other three algorithms.

However, this method has not been applied to complex maps in practice, and whether it can run stably in a complex environment remains to be studied. In our future work, we will focus on practical experiments to ensure that the algorithm can run stably and accurately in the real environment.