Random slam algorithm of slam concept learning

In this section, after familiarity with featue maps concepts, we will begin to learn the EKF-based feature map slam algorithm.

1. Robots, graphs and enhanced state vectors

The random slam algorithm generally stores the robot's posture and the landmarks in the graph in a single state vector, and then estimates the state parameters through a recursive prediction and measurement process. In the prediction stage, the motion of the robot is processed by the incremental trajectory estimation, and the estimation of the robot pose uncertainty is increased. When the features stored in the map are observed again, the measurement phase, or the update phase, is performed, which can improve the overall state estimation. When a robot observes a new feature during motion, it adds the newly observed feature to the state vector through a state-enhanced process.

The state of the robot, a self-pose parameter relative to the reference Cartesian coordinate system, is defined by means of mean and covariance:

(1)

(2)

The covariance matrix Pm of a graph contains cross-correlation information (that is, non-diagonal items) between features, which characterize the dependency of each feature on information related to other features in the map. Since the position of the feature is static, when the surrounding environment is also known as map rigidity, the cross-correlation between features will be enhanced with two observations of the feature. The features in the diagram can be expressed as follows

(3)

(4)

The figure in Slam is defined by an enhanced state vector in which the robot poses and feature graph states are concatenated, as shown in 1. This is necessary because the consistent slam depends on the robot posture and the correlation between the graphs Pvm

(5)

(6)

　　

Fig. 1 Enhanced state vector.

It is important to note that the initialization of the state is usually the same as. That is , no feature has been observed by the robot, and the initial moment of the robot is at the origin of the reference coordinate system.

2. Forecast phase

The process model of stochastic slam algorithm is based on the trajectory estimation to determine the motion of the robot relative to the previous time posture, and the graph feature remains stationary. The effects of this model on state estimation exist in the state vector part and the state covariance matrix, as well as the items. And these diagram feature related items remain unchanged.

It is estimated that the robot (in this case, for example) pose change and covariance are usually obtained by the encoder measurement of the wheel and the kinematic model corresponding to the trolley. In this paper, the position and posture change is obtained based on the trajectory estimation of laser ranging, the algorithm can be used to find the relative posture relation between sequential laser scanning by batch processing Data Association algorithm.

Fig. 2 robot pose change signal.

Therefore, the enhanced state of the prediction can be expressed as

(7)

(8)

Wherein, the Jacobian matrix and the definition of

(9)

(10)

The Jacobian matrix and is defined as

(11)

(12)

Because the Jacobian matrix affects only the covariance matrix of the robot and the corresponding cross-correlation, the equation (8) is effectively implemented

(13)

3. Update phase

If the estimated characteristics that have been stored in the figure are observed by a distance-azimuth sensor two times, the corresponding measure can be expressed as

(14)

(15)

Among them, the measurement of the corresponding distance value and relative to the observation of the robot deflection angle. To measure the covariance. The correlation between the measured value obtained by the sensor and the graph can be expressed by the following equation

(16)

The Kalman gain can be obtained

(+ )(18)

(19)

Among them, the Jacobian matrix is

(20)

For slam graphs with many features, the Jacobian matrix will contain many 0 items, which makes the calculations of equations (18) and (19) very efficient.

Then we can get a posteriori slam estimate from the update process.

(21/22)

Practice has shown that if several independent measurements are available at the same time, it is possible to obtain more accurate updates if they are known.

4. Status Enhancement

When the robot roams the environment, once the new features are observed, it must be stored in the diagram. Next, we will give a new observed feature initialization method, firstly, the state vector and the corresponding covariance matrix are enhanced by the new measured extremum to

(23)

(24)

The measurement z is converted by the function to the position of the global Cartesian coordinate system, and the corresponding transformation is represented as follows

(25)

The enhanced state can be initialized with a linearization function.

(26/27)

Where the Jacobian matrix is expressed as

(28)

The deletion of features is very simple compared to the storage of features, simply by removing the element corresponding to the feature directly from the state vector and deleting the items in the related rows and columns from the covariance matrix.

Random slam algorithm of slam concept learning