Summary: For solving the problem of low efficiency and low localization accuracy about EKF-SLAM algorithm in large scale and complicated environment, an improved method by adding an adaptive observation range to EKF-SLAM algorithm was presented. By setting the constraints, the observed environment scale and the number of observed landmarks for mobile robot were constrained. Deleting the landmarks which has observed but beyond the constrained scale by judge the relationship between the observation range and maximum distance, dynamically adjusting the observation range by comparing the number of observed landmarks and number of landmarks. Simulation results demonstrate that the proposed method could reduce the computational and improve efficiency and localization accuracy.
Key words: mobile robot, improved EKF-SLAM algorithm, landmarks.
УДК 004.021
KeKe Geng
Ph.D. student in Engineering,
at the Department of Information and Control Systems
of Bauman Moscow State Technical University
Moscow, Russian
AN IMPROVED EKF-SLAM ALGORITHM FOR MOBILE ROBOT
Summary: For solving the problem of low efficiency and low localization accuracy about EKF-SLAM algorithm in large scale and complicated environment, an improved method by adding an adaptive observation range to EKF-SLAM algorithm was presented. By setting the constraints, the observed environment scale and the number of observed landmarks for mobile robot were constrained. Deleting the landmarks which has observed but beyond the constrained scale by judge the relationship between the observation range and maximum distance, dynamically adjusting the observation range by comparing the number of observed landmarks and number of landmarks. Simulation results demonstrate that the proposed method could reduce the computational and improve efficiency and localization accuracy.
Key words: mobile robot, improved EKF-SLAM algorithm, landmarks.
Introduction. The original stochastic solution to the SLAM(Simultaneous localization and mapping) problem by Smith et al. [[1]] is now almost twenty years old, and the concept has reached a state of maturity sufficient to permit practical implementations in challenging environments. SLAM problem can be described as a mobile robot in a completely unknown environment, beginning movement from an unknown location, during the movement to build an environment map through environmental observations carried by the sensor itself acquired, while taking advantage of built environment map to update robot's own location in the environment. In recent years, SLAM problem has been more and more attention in the field of robotics, which is considered to be the key to achieving a completely autonomous movement[[2],[3],[4]]. A variety of improved SLAM algorithm has been proposed: RO-SLAM(range-only)[[5]], FAST-SLAM[[6]], UKF-SLAM(Unscented Kalman Filter based SLAM algorithm)[[7]], the most widely used EKF-SLAM(Extended Kalman Filter based SLAM algorithm)[[8]][[9]]. However, a significant disadvantage of the standard EKF-SLAM algorithm is computationally intensive when the time for large and complex environment, low operating efficiency, and the error will increase with increased observed landmarks, easy to reduce the positioning accuracy and lead to map's inconsistencies[[10]].
In this paper we present an improved method by adding constraints of a adaptive observation range to EKF-SLAM algorithm. The excess landmarks, which beyond the constrained scale by judge the relationship between the observation and maximum distance, will be deleted. In order to ensure the number of landmarks, not too little or too much, the observation range will be dynamically adjusted by comparing the number of observed landmarks and Minimum number of landmarks. Simulation results show this method can effectively improve the positioning accuracy and operational efficiency of the algorithm.
Motion model of mobile robot. In this article we take two wheel differential drive mobile robot as research subjects. And it’s motion model is shown in Figure 1, where - world coordinate system; - object coordinate system. In the state of mobile robot in world coordinate system is represented by the position and orientation vector , where - coordinates of mobile robot in k moment; - the angle between the forward direction of mobile robot and the positive direction of x-axis of the world coordinate system.
Figure 1. Model of mobile robot
The linear velocity and rotational angular velocity of mobile robot in moment:
where , -linear velocity of right and left wheel; - Axis spacing between two wheels. Assuming the speed of the mobile robot is uniformly changed from k-1 moment to k moment, then the kinematic model of the mobile robot:
where -the sampling period.
State observation model. In this paper we set there are totally N landmarks and the coordinates ( ) do not change. The observation model of mobile robot:
where -the distance between the mobile robot and landmark i; -the angle between the forward direction of mobile robot and connection line of mobile robot and landmark i.
Standard EKF-SLAM algorithm. The basic idea of EKF-based SLAM algorithm is to form a combined vector using the position and orientation vector of mobile robot and position vector of landmark. By continuous movement of the robot and the sensor observation, add the new observed position vector of landmark vector to the combined vector, and the estimated position of the robot is updated. The set of position vector of all observed landmarks in k momen, . So the combined vector in k moment will be . Movement and observation equations for the mobile robot:
- control signal; - gaussian white noise with 0 mean value and their covariance are and .
EKF-SLAM algorithm includes the following processes: Assuming value of the estimated system state vector at k-1 moment is , the covariance matrix:
where - the covariance of robot’s position and orientation vector; , - the cross-covariance matrix between mobile robot and landmarks; the covariance matrix of landmark's position vector. Then we can obtain the estimated system state vector and the covariance matrix at k moment:
where -Jacobian matrix of function to at point :
Observation process: Robot obtain observations of landmarks at k moment using the sensor, then measurement residual and covariance matrix are:
where -Jacobian matrix of function to at point :
Update process: Use predictive value and the observed value to update the system state vector and covariance matrix:
where- Optimal Kalman gain.
State vector expansion process: The landmarks observed by sensor at k moment include landmarks, which already exist in map, and also include new landmarks. Existed landmarks will be used to update the predicted value of state vector, the new landmarks are added to the system state vector after initialization processing. Assuming at moment k the i-th observed landmark is a new landmark, and it's measurement value, then it’s coordinates in the world coordinates system will be:
The new expanded state vector is obtained . Covariance matrix of the new state vector:
where ,- Jacobian matrix of function to at point :
EKF-SLAM algorithm with adaptive observation range. From the above analysis we can see that the dimension of the system state vector is , where n is the number of observed landmarks. In large and complex environment, with the increment of the number of observed landmarks , the dimension of the system state vector will also increase. Computation of covariance matrix and the Jacobian matrix will be dramatically increased, also linearity error of Jacobian matrix increases, which will reduce the operating efficiency and positioning accuracy. To avoid this problem, we propose a EKF-SLAM algorithm with adaptive observation range(ARANGE-EKF-SLAM). Principle of the algorithm is to use a circular local map to estimate the position and attitude of mobile robot in the world coordinate system, while updating the global map, as shown in the following figure.
Figure 2. Observation range
In this figure S- circular local map; S1- observation range; S2- constraint area; black points-landmarks.
First, we need to detect whether there are new landmarks in the area S1 by constraint conditions:,where -existed landmark in map; - landmark in observation range; - sufficiently small scalar.
If it is a new landmark, then added it to the system state vector matrix. At the same time statistic the number landmarks NUM in the area S, if , then reduce radius of circle S, otherwise, if increase the radius under the premise of .
Simulation and analysis of results. Established by MATLAB simulation environment map, in which include total of 75 landmarks, represented by asterisk indicates, as well as the planned route, represented by solid lines, as shown in the following figure. In the simulation deemed the mobile robot as a circle, and assumed the sensor located on the centroid position of this circle. The initial coordinates of the robot is, the initial observation distance is 25m, observational viewing angle, speed is , constraint conditions are , , . The simulation results are shown in Figure 3.
Fig. 3 Simulation results of robot trajectories and landmark positions
The black solid line is the trajectory of mobile robot estimated by improved EKF-SLAM algorithm with adaptive observation range proposed in this paper. In order to verify the superiority of the proposed method, the simulation results are compared of EKF-SLAM algorithm with observation range of and , as shown in Figure 4.
(a) the size of the state vector
(b) x-direction position covariance
Figure 4. Comparison of simulation results
As can be seen when observation range is 10 m, the operation speed is faster(simulation time 39.325s), but the number of observed landmarks is less, difficult to accurately estimate the position of mobile robot, there is a large gap between the estimated path and the reference path. When observation range is 50 m, the number of observed landmarks is too much, although able to estimate the more accurate path, but the resulting increase in computational complexity(simulation time 53.168s). Improved algorithm can also estimate an accurate path, while reducing the computing time(simulation time 34.652s). And different from the traditional algorithm, the dimension of the state vector of inproved algorithm does not increase with increasing the number of iterations.
Conclusions. For solving the problems of computational complexity, low positioning accuracy and operating efficiency of standard EKF-SLAM algorithm, we propose an improved EKF-SLAM algorithm with adaptive observation range. The observation range of mobile robot is controlled in a circular area with a changeable radius and remove the landmarks beyond constraint conditions in the process of robot's movement. We need only to compute landmarks satisfy the constraints, the dimension of the system state vector will not constantly increase with the increment of the number of observed landmark. In this case the calculation won't increase dramatically, while reducing the linearity errors of jacobian matrices, improves operational efficiency and positioning accuracy of the algorithm. Simulation results show the proposed method can effectively improve the positioning accuracy and operational efficiency of the algorithm.
Reference:
