Abstract
In the traditional EKF-SLAM algorithm, the computational complexity and uncertainty will grow up rapidly with the increase of the feature points and the enlargement of the map coverage. As we know, the computational complexity is proportional to the quadratic of the number of feature points contained in a single filtering process. The approach represented in the paper combines EKF-SLAM with local submaps, which can improve the computational efficiency and reduce the computational complexity. At first, an independent local submap is established for the observed feature points. When the number of feature points contained in the local submap reaches a certain threshold value, the local submap is integrated into the global map. At last, the submap is initialized again. The simulation results show that the approach can reduce the computational complexity effectively and increase the computation speed greatly in the case of maintaining the computational accuracy of the traditional EKF-SLAM algorithm.
1. Introduction
With the development of the manned deep space exploration, Mars as the most suitable planet for human habitation has attracted extensive attention. The traditional flying-around method can no longer satisfy the deeper exploration mission. Therefore, the cruising exploration method of the Mars rover has become an excellent choice [1]. The relative pose information of the Mars rover can be obtained by traditional odometer during the cruising exploration process, which is not accurate on the complex Mars terrain. The navigation accuracy will be improved through the visual aid navigation method in this circumstance, especially SLAM method [2, 3]. The SLAM problem can be described as a problem that you place a robot in an unknown environment with sensors, it can build the incremental maps and achieve its location autonomously during the movement [4].
The initial framework for the navigation of the mobile robot was presented by Dissanayake et al. in the 1980s. Meanwhile, he established the SLAM navigation algorithm for the mobile robot based on Kalman filter [5]. The analysis of the consistency of the EKF-SLAM was presented by Bailey and Durrant-Whyte [6]. The robocentric map joining method was presented by Castellanos et al. for improving the consistency of EKF-SLAM [7]. The bounding uncertainty of the robocentric local map sequencing algorithm was analyzed by Martinez-Cantin and Castellanos [8]. A lot of landmarks need to be observed and introduced into the state equation by EKF-SLAM, which increases the computational complexity and decreases the running speed of the system [9, 6]. Therefore, the traditional EKF-SLAM algorithm needs to be improved. The computational cost analysis of EKF-SLAM was done by time measurement on substep motion update and measurement update on EKF by considering the total number of landmarks and numerous setting on range observation distance by Samsuri et al. [10], which showed the number of landmarks and the range observation distances were the factors to cause computational cost. A computationally efficient SLAM (CESLAM) algorithm was presented by Hsu et al. [11], where odometer information was considered for updating the robot’s pose in particles. When a measurement had a maximum likelihood with the known landmark in the particle, the particle state was updated before updating the land mark estimates. The algorithm aimed to improve the computational efficiency and estimation accuracy of conventional SLAM algorithm.
An improved EKF-SLAM with local submaps is presented in the paper. Firstly, the feature points observed by binocular camera are grouped; meanwhile, a few submaps are established, which are independent of the global map. And then only the local state variables are updated by the new algorithm in each update process, because the local submap is integrated into the global map only when the area of the submap reaches a certain scale. The global state variables will not be updated until the local submap is integrated into the global map. The main difference between the standard EKF-SLAM algorithm and the algorithm represented in this paper is the introduction of the local submaps, through which the number of feature points used for each filtering update is reduced. Since the computational complexity is proportional to the quadratic of the number of feature points contained in a single filtering process of EKF and the total computational complexity of EKF-SLAM algorithm is proportional to the cube of the final number of feature points in the state variables, the computational complexity is reduced by the improvement. The paper is organized as follows: Section 2 builds the motion model equation of the Mars rover. The EKF-SLAM algorithm is improved based on the local submaps in Section 3. Simulation results and the comparison of the standard EKF-SLAM and the SLAM with submaps are given in Section 4. Finally, the conclusion is drawn in the last section.
2. Motion Model of the Mars Rover
The set of the coordinate system of the Mars rover’s motion is given in Figure 1.
As is shown in Figure 1, we denote as the inertial coordinates and global coordinates; as the body coordinates of the Mars rover, where points to the direction of the rover’s motion; as the binocular camera coordinates, where points to the vertical direction of the camera lens; as the coordinates of the rover’s wheel, which is used to establish the trajectory of the rover’s centroid based on the terrain of Mars; and as the coordinates of the contact point for the Mars rover’s wheel and the planet surface, where points to the tangential direction of the contact point’s surface, which is used to establish the trajectory of the rover’s centroid based on the terrain of Mars.
The installing model of the binocular camera is shown in Figure 2 (vertical view) and Figure 3 (lateral view).
The Mars rover’s model used in the paper is a four-wheel differential rover with six degrees of freedom. The linear velocity and the angular velocity are controlled input variables, which is decided by angular velocities of differential wheels [12]. The motion equation is as follows: where is the left wheel’s velocity, is the right wheel’s velocity, is the wheel’s radius, and is the distance between the left wheel and the right wheel.
The derivative process is omitted; therefore, the kinematic model equation of the Mars rover is given as follows [13]: where is the rotation matrix; is the transformation matrix of the angular velocity; denotes pose vectors of the rover, where , , and stand for the yaw angle, pitch angle, and roll angle; is the linear velocity of the Mars rover; and is the angular velocity of the Mars rover.
The coordinates of the contact point can be obtained from the coordinates of the Mars rover’s wheel [14]. where is the transformation matrix from the contact point coordinates to the Mars rover’s coordinates and is the contact point’s coordinate.
The binocular camera is fixed on an oblique stick with two degrees of freedom at a certain angle. are the control inputs of the binocular camera, through which a wide range of 3D terrain can be scanned. is the rotation angle of the control stick around the platform, and is the rotation angle of the control stick around the connecting bolt. is the angle between the control stick and the vertical direction of the platform. The derivative process is omitted; therefore, the pose of the feature point in global coordinates can be described as follows [15]: where is the position of the rover in global coordinates, is the position of binocular camera platform in the Mars rover’s coordinates, is the position of the feature points in the camera coordinates, is the position of the feature points in the global coordinates, is the transformation matrix from the body coordinates to the global coordinates, is the transformation matrix from the platform coordinates to the body coordinates, and is the transformation matrix from the camera coordinates to the platform coordinates.
3. An Improved EKF-SLAM with Local Submaps
3.1. System State Variables
The system state variables of the local submap algorithm include the Mars rover and the feature points in the local coordinates and the feature points in the global coordinates and the coordinate of the local coordinates in the global coordinates, which can be described as follows: where is the pose vector of the local submap in the global map, is the position of the feature point in the global map, is the pose of the Mars rover in the local submap, is the position of the same feature point in the local submap vector , and k is the discrete time coefficient. The state variables defined above can be simplified as follows: where is the feature point in the global map and is the feature point in the local submap. The integration process from the submap to the global map is shown in Figure 4, where is the control input, is the state variable, and is the observed variable.
3.2. State Estimation Process
The state covariance matrixes include the covariance matrixes of the global state variables and local state variables and the cross covariance between the global state variables and the local state variables. In this paper, the independence of the global state variables and local state variables is assumed for short intervals, and the short interval independence will be used. The cross covariance between the global state and the local state is zero because of the independence between the global state variable estimation and the local state variables, which is given as follows: where is the covariance matrix of the local submap pose estimation in the global state and is the covariance matrix of the Mars rover pose estimation in the local state.
The state predicted value is given as follows:
The global estimation is not changed in the local update due to the independence between the local state and the global state, which reduces the computational complexity of the prediction process. where is the predicted value of the covariance matrix of the state variables.
The state predicted value is obtained by the observation model. includes the measure distance, azimuth angle, and rotation angle.
The observation residual and residual covariance matrix of can be described as follows:
Finally, the state update process is given as follows:
The global state is not updated in the process, because the global state is independent of the local state. The feature points contained in the local state are much less than those contained in the global state. Therefore, the computational complexity is reduced greatly in the update process.
3.3. The Independence of Local State Estimation
The local submap established above is composed of the Mars rover’s pose state in the local submap and the local submap state. The local submap achieves its initialization by initializing the location of the rover. We hypothesize the initial state matrix is given as follows: where is the estimated value of Mars rover’s pose in the global map and is the estimated value of the feature point position in the global map.
The state covariance matrix is shown as follows: where is the covariance matrix of the Mars rover’s pose estimation in the global state, is the covariance matrix of the feature point position estimation in the global state, and is the covariance matrix of the Mars rover and the feature point in the global state.
A local submap is established at time , which takes the Mars rover’s position as the center of the submap. Therefore, the Mars rover is located at the origin in the submap whose uncertainty is zero. We hypothesis that the initial state matrix is shown as follows: where is the covariance matrix of the local submap pose estimation in the global state and is the covariance matrix of the feature point position estimation in the global state.
The noise variance is generated by the local Mars rover’s state variables under the modeling error and the observation error during the update process in the local submap. However, this process is independent of global state, which will not affect the global state and covariance matrix. The feature points observed in the new local submap are independent of the global map. Meanwhile, if an independent Mars rover’s state estimation is initialized in the local submap, the estimation will maintain its independence to the global map. Therefore, the local state estimation and the global state estimation remain independence.
3.4. The Integration between the Submap and the Global Map
The submap is needed for integration into the global map when it reaches a certain scale. Then, the submap is rebuilt and initialized again. We hypothesize that is the transformation matrix, which can convert the local state to a global state. where denotes the state summation operation in different coordinates including the angular rotation process between different coordinates. We hypothesize that is the position of a feature point in the local submap.
Likewise, we hypothesize that is the coordinate of the local coordinates in the global coordinates.
The global position of the feature point can be obtained as follows: where can be described as follows: where which is the transformation matrix from the local coordinates to the global coordinates. The covariance matrix of the global state variables can be expressed as follows: where is the Jacobian matrix of the transformation matrix, which is shown as follows:
We hypothesize that the local pose of the rover is and the global pose of the rover is given as follows: where is the pose transformation matrix. Therefore, the covariance matrix of the Mars rover in the global coordinates can be described as follows: where is
3.5. The Constraints for Local Feature Point Estimation
The local pose estimation of the Mars rover is independent of the global coordinates in the local submap algorithm. The estimation of the feature points in the global coordinates cannot be applied to the update process for the local submap without the connection between the coordinates. The feature points in the global coordinates and the local coordinates are independent, which results in repeated calculations when the feature points in the local coordinates are integrated into the global coordinates. The estimation of the feature points must be able to apply in the global estimation process.
In the general process, the consistent estimation of the feature points state can be obtained by the known constraints between the common state. The independent estimation of the feature points can be integrated into the global coordinate; meanwhile, a simple and consistent global estimation is generated by using the known connection between the state variables.
The projection is defined as follows:
The constraint can be approximated estimation by EKF. The result can be described as follows: where
The system state variables are shown above, then the constraints must satisfy the condition, which is the constrains for local feature points integrating into the global map:
The constraints can be expressed as follows: where , and the constraint filtering can be applied to the conversion process to the global map for the local feature points.
4. Simulation Results
The simulation results are analyzed in this section. We can obtain the Mars terrain as the method mentioned in the paper [16]. We hypothesize that the real trajectory of the Mars rover is a circle. The 3D terrain and Mars rover’s trajectory are shown in Figure 5. The following simulation results are mainly divided into two parts. The results shown from Figures 6–8 are simulated by standard EKF-SLAM algorithm; meanwhile, the results shown from Figures 9–12 are simulated by the new algorithm which is presented in this paper. The total number of the feature points considered in the state variables of EKF-SLAM and the new algorithm are compared in Figure 13. Since the computational complexity is proportional to the quadratic of the number of the feature points contained in a single filtering process, the main difference between the two algorithms can be shown in Figure 13. Obviously, the number of the feature points used in the new algorithm is much less than which are used in standard EKF-SLAM algorithm. The lower computational complexity is obtained by the SLAM algorithm with local submaps (Table 1).
The true trajectory and estimated trajectory of the Mars rover by standard EKF-SLAM are shown in Figure 6. The pink solid line represents the true trajectory and the blue dashed line represents the estimated trajectory of the Mars rover. Meanwhile, the green asterisks represent land marks. The red asterisks represent the observed feature points. The position error is less than 1 m, and the attitude error is less than 0.5°, which can be seen from Figures 7 and 8.
In the following section, an improved EKF-SLAM approach with submaps is analyzed. The true trajectory and estimated trajectory of the Mars rover by improved EKF-SLAM algorithm with submaps are shown in Figure 9. The position error and attitude error can be seen in Figures 10 and 11 (1 m, 0.5°), which keep the same accuracy and error as the previous standard EKF-SLAM algorithm. However, the total number of the feature points considered in the state variables of the improved algorithm with submaps is much smaller than that of the standard EKF-SLAM algorithm. As we know, the computational complexity is proportional to the quadratic of the number of the feature points contained in a single filtering process. Therefore, the computational complexity of the improved EKF-SLAM algorithm with submaps has decreased significantly compared with the standard EKF-SLAM algorithm. The standard deviation curve of the local position and azimuth deviation of the Mars rover is given in Figure 12. The total number of the feature points considered in the state variables of the standard EKF-SLAM and SLAM with submaps are shown in Figure 13. The computational complexity is proportional to the square of the number of the feature points contained in the state variables, and the total computational complexity of EKF-SLAM algorithm is proportional to the cube of the final number of feature points in the state variables. Therefore, the computational complexity is reduced sharply in SLAM with submaps. Some comparison results of two algorithms are listed in Table 2, from which we can see the difference of the two algorithms clearly.
5. Conclusion
In the paper, we proposed an improved EKF-SLAM algorithm with submaps for Mars surface exploration. The presented method could reduce the computational complexity to a great extent. The error of the position and attitude could be controlled in an acceptable ranges (1 m, 0.5°) for both two algorithms, which can be seen in the simulation results. The position error of the EKF-SLAM with submaps is smaller than that of the standard EKF-SLAM. Meanwhile, the computational complexity is lower by the improved algorithm, which is more suitable for the complex terrain of Mars.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that there is no conflict of interest regarding the publication of this paper.
Acknowledgments
The paper is completed with the help of Professor Franco Bernelli-Zazzera and Professor Mauro Massari of the Aerospace Engineering Department of Politecnico di Milano.