An Improved EKF-SLAM for Mars Surface Exploration

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.


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.

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 O G − x G y G z G as the inertial coordinates and global coordinates; O R − x R y R z R as the body coordinates of the Mars rover, where x R points to the direction of the rover's motion; O C − x C y C z C as the binocular camera coordinates, where x C points to the vertical direction of the camera lens; O A i − x A i y A i z A i as the coordinates of the rover's ith wheel, which is used to establish the trajectory of the rover's centroid based on the terrain of Mars; and O W i − x W i y W i z W i as the coordinates of the contact point for the Mars rover's ith wheel and the planet surface, where x W i 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:    International Journal of Aerospace Engineering where θ L t is the left wheel's velocity, θ R t is the right wheel's velocity, r w is the wheel's radius, and B 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 C G R is the rotation matrix; S G ω is the transformation matrix of the angular velocity; x R , y R , z R , ϕ x , ϕ y , ϕ z T denotes pose vectors of the rover, where ϕ z , ϕ y , and ϕ x stand for the yaw angle, pitch angle, and roll angle; v 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 W i is the transformation matrix from the contact point coordinates to the Mars rover's coordinates and The binocular camera is fixed on an oblique stick with two degrees of freedom at a certain angle.u L = ω δ , ω ϕ 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.ϕ L 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 p R = x R , y R , z R T is the position of the rover in global coordinates, d = a, b, c is the position of binocular camera platform in the Mars rover's coordinates, m i C is the position of the feature points in the camera coordinates, m i G is the position of the feature points in the global coordinates, C G R is the transformation matrix from the body coordinates to the global coordinates, C R H is the transformation matrix from the platform coordinates to the body coordinates, and C H C is the transformation matrix from the camera coordinates to the platform coordinates.

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 G X S k is the pose vector of the local submap in the global map, G X i k is the position of the feature point in the global map, S X R k is the pose of the Mars rover in the local submap, S X i k is the position of the same feature point in the local submap vector G X S k , and k is the discrete time coefficient.The state variables defined above can be simplified as follows: where G X m k is the feature point in the global map and S X m k is the feature point in the local submap.The integration process from the submap to the global map is 3 International Journal of Aerospace Engineering shown in Figure 4, where U k is the control input, X k is the state variable, and Z k is the observed variable.

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 G P SS k is the covariance matrix of the local submap pose estimation in the global state and S P RR k is the covariance matrix of the Mars rover pose estimation in the local state.
The state predicted value X ls k | k − 1 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 P ls k | k − 1 is the predicted value of the covariance matrix of the state variables.
The state predicted value ẑ k | k − 1 is obtained by the observation model.
Xi k − 1 includes the measure distance, azimuth angle, and rotation angle.
The observation residual and residual covariance matrix of X 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.
The local submaps Prediction/observation/update Data processing Establish rules of constraints Keep the global map consistent with the constraints S X(k) The local submap achieves its initialization by initializing the location of the rover.We hypothesize the initial state matrix is given as follows: where G XR is the estimated value of Mars rover's pose in the global map and G Xm is the estimated value of the feature point position in the global map.
The state covariance matrix is shown as follows: where G P RR k is the covariance matrix of the Mars rover's pose estimation in the global state, G P mm k is the covariance matrix of the feature point position estimation in the global state, and G P Rm k is the covariance matrix of the Mars rover and the feature point in the global state.A local submap is at time k − 1, 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 Xll k − 1 is shown as follows: where G P SS k is the covariance matrix of the local submap pose estimation in the global state and G P mm k 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.

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 T k 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 S Xi k is the position of a feature point in the local submap.
Likewise, we hypothesize that G XS k is the coordinate of the local coordinates in the global coordinates.
The global position of the feature point can be obtained as follows: where T G S k can be described as follows: where where ∇T G S k 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 Sz is the pose transformation matrix.Therefore, the covariance matrix of the Mars rover in the global coordinates can be described as follows:

25
where ∇R G S k is 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:

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). 6 International Journal of Aerospace Engineering 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  1.
x-axis (m) y-axis (m)  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   2, from which we can see the difference of the two algorithms clearly.

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.

Figure 1 :
Figure 1: Coordinate system of the Mars rover's motion. b

Figure 2 :
Figure 2: The installing model of the binocular camera (vertical view).

Figure 3 :
Figure 3: The installing model of the binocular camera (lateral view).

Figure 4 :
Figure 4: The flow chart of the integration process from submap to global map.
Sz which is the transformation matrix from the local coordinates to the global 5 International Journal of Aerospace Engineering coordinates.The covariance matrix of the global state variables can be expressed as follows:

k 26 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.

31 whereC
k = −I I 0 −I , 32 b = 0, and the constraint filtering can be applied to the conversion process to the global map for the local feature points.

Figure 5 :
Figure 5: 3D terrain and Mars rover's trajectory 200 landmarks (asterisks) are distributed in the map.The simulation parameters are shown in Table1.

Figure 6 :Figure 7 :Figure 8 :
Figure 6: The true trajectory and estimated trajectory of the Mars rover by standard EKF-SLAM.

Figure 9 :
Figure 9: The true trajectory and estimated trajectory of the Mars rover by EKF-SLAM with submaps.

Table 1 : 3 °Sensor
The simulation parameters.The number of feature points in the map 200 The radius of the wheel 0.3 m The initial position of the rover 100, 20,−0 3174 The SD of RN of the accelerometer 2 0 × 10 −3 m/s The SD of RN of the gyroscope 0 02 °/s The SD of RN of the measuring length 1.0 m The SD of RN of the measuring azimuth 3 °The SD of RN of the control stick denotes standard deviation; RN denotes random noise.8 International Journal of Aerospace Engineering comparison results of two algorithms are listed in Table