Robot indoor navigation point cloud map generation algorithm based on visual sensing

3.1 Robot indoor navigation point cloud pose estimation based on visual SLAM algorithm

Very effective representation of a robot navigation map, which is mainly obtained by laser point cloud and image point cloud [16,17]. The 3D laser scanner used in the 3D point cloud is expensive, bulky, and complicated in data processing. There are two main methods of generating image point clouds, SfM and SLAM. SfM is difficult to obtain the real size of the 3D scene structure in the processing process, and cannot meet the high real-time requirements. With the development of sensor technology and computer vision technology, SLAM in the field of robotics is more suitable for application scenarios with high real-time requirements. The sensor commonly used in visual SLAM is the camera. Among them, the depth camera (RGB-D) can provide more information than traditional cameras, and it is not necessary to calculate the depth as time-consuming and laborious as monocular or binocular. In terms of SLAM, it is mainly used indoor. A color image I RGB → R 3 and a depth image can be obtained at the same time by the depth camera I D → R . Suppose the focal length of the standard pinhole camera is f , the offset of the camera projection center along the image x axis and y axis are, respectively, c x , c y the projection function that maps π a point in the three-dimensional space P = ( X , Y , Z ) T to the image pixel position p = ( u , v ) , as shown in equation (1):

Given the pixel coordinates of the image p = ( u , v ) T , and the corresponding depth value Z ( p ) , the back projection of the three-dimensional point in space π − 1 is reconstructed, as shown in equation (2):

The definition of camera rigid body transformation is shown in equation (3):

In equation (3), it R ∈ SO ( 3 ) represents an orthogonal rotation matrix, and t ∈ R 3 represents a translation matrix. The camera pose is defined as the cylinder transformation matrix, and then, the curvilinear coordinate system is used (twist coordinate), as shown in equation (4):

In equation (4), it [ v 1 , v 2 , v 3 ] ∈ R 3 represents the linear velocity of the camera, [ w 1 , w 2 , w 3 ] ∈ R 3 represents the angular velocity of the camera. According to the operation rules of Lie group and Lie algebra, in space, the camera’s rigid body transforms the pose as shown in equation (5):

Pose ξ = ( 0 , 0 , 0 , 0 , 0 , 0 ) T corresponding to the first frame image is defined as the origin of coordinates, the camera pose is estimated from the relative change between two adjacent frames of images, and the motion of the camera in three-dimensional space is recorded and described. The depth camera (RGB-D) can obtain a color image and a dark image, respectively, and the obtained depth image obtains the spatial three-dimensional point coordinates corresponding to each pixel in the camera coordinate system. Assume that the set t − 1 of feature points of the P t − 1 color image is I RGB t − 1 , and the corresponding set of three-dimensional points in space is. At the moment, t the set of feature points of the p t color image is, and the set of matching between I RGB t and t through this feature point t − 1 is p , then I RGB t − 1 the projection error between and is shown in equation (6):

In equation (6), the corresponding three-dimensional point coordinates in space are represented. By optimizing the projection error of the color image, the pose of the camera can be obtained as shown in equation (7):

In order to improve the robustness of camera tracking, when the camera pose cannot be obtained, the camera pose is t − 1 estimated by combining the depth image at ξ ∧ the t moment with the ICP algorithm, and the camera pose is used to find matching features between the feature point set p t and P t − 1 point. In the SLAM point cloud map, only key frames that meet certain conditions are selected to be saved instead of all image frames, and the relative distance between two frames is defined as the combination of translation and rotation weights of the camera pose, as shown in equation (8):

In equation (8), W represents the diagonal matrix, which ξ i j represents the weight of each parameter. If the K i − 1 distance between the previous key frame and the current frame exceeds the set threshold, a new keyframe will be created K i . The processing steps of the key frame are shown in Figure 1.

Figure 1

Processing steps of keyframes.

As shown in Figure 1, for the newly added keyframes K , the keyframes that share the same visual word are quickly retrieved (Visual Words) through vectors to Bow form a local keyframe set K L , and at the same time, with the K L corresponding 3D map point set, the joint optimization of projection error and back-projection error is performed. The estimation accuracy of the camera pose and 3D map coordinates corresponding to the keyframe is improved. Assuming t that the color image and dark image obtained by the depth camera at time are divided into two I t , D t , the one t − 1 obtained at the time is respectively I t − 1 , D t − 1 , and the camera at the two moments is transformed into ξ , then the back-projection error between D t and D t − 1 is shown in equation (9):

In equation (9), it v t k represents the three-dimensional point in space obtained by back-projecting the pixel coordinates D t of the first k map point, and the projection error of the color image is shown in equation (10):

After jointly optimizing the back-projection error of the dark image and the projection error of the color image, the result obtained is shown in equation (11):

In equation (11), w rgb represents the weight parameter of the color image projection error. Figure 2 shows the algorithm flow of the indoor navigation point cloud pose estimation algorithm for the depth camera (RGB-D) robot based on the visual SLAM algorithm.

Figure 2

Algorithm flow of robot indoor navigation point cloud pose estimation based on RGB-D.

As shown in Figure 2, the study uses the ORB (Oriented FAST and Rotated BRIEF, ORB) operator to quickly extract and match the visual features of the color sequence images captured by the depth camera and then combines the Iterative Closest Point (ICP) algorithm and Real-time estimation of camera pose. And the ICP error constraint in the camera pose optimization operation is added, and then the graph optimization algorithm is use to jointly optimize the color image projection error obtained by the depth camera and the back-projection error of the depth image, and finally relatively accurate camera pose and 3D sparseness point cloud are obtained. However, in visual SLAM, the tracking drift of the camera is unavoidable due to the continuous accumulation of attitude errors.

3.2 Point cloud map based on visual SLAM and Kalman filter visual inertial navigation attitude fusion algorithm

In fact, the real-time performance of camera pose estimation based only on the visual SLAM algorithm in an indoor environment is still not high, and the output frequency is low, which is prone to tracking failure under high-speed rotation. Inertial measurement unit (IMU) plays an important role in navigation and positioning. The inertial sensor has a high output frequency for attitude estimation and high output accuracy under high-speed rotation [18]. The establishment of IMU kinematics model relies on the Kalman filter to achieve an accurate output of attitude. The visual-inertial navigation attitude fusion algorithm based on the Kalman filter is shown in Figure 3.

Figure 3

Algorithm flow of visual-inertial navigation attitude fusion based on Kalman filter.

As shown in Figure 3, you need to build a map immediately and start positioning to obtain IMU data and image frames. Then, the ORB descriptor is extracted and synchronized with the image frame at the same time. The pose is initialized by combining the image, IMU position, and attitude information. Next, Kalman filtering is performed and stopped after obtaining the updated data of IMU. The research combines the advantages of the two pose estimation algorithms, using the attitude information output by the inertial navigation as the predicted value, and then takes the attitude information obtained by the visual slam as the observation value. Kalman filter is an optimal autoregressive data processing algorithm. Before using this algorithm, it is necessary to obtain the state equation and observation equation of the system. The state equation of the inertial system is shown in equation (12):

In equation (12), Δ b represents the amount of error and δ θ represents the small angle error; n g is Gaussian noise whose gyroscope obeys a normal distribution and whose mean value is 0, which means that the n b g random walk model leads to a normal distribution noise whose mean value is 0. Among them, ⌊ ω ˆ × ⌋ represents the antisymmetric matrix about the rotation axis, such as equation (13):

In order to obtain the state transition matrix ϕ k , the system matrix needs to be discretized. This study uses Taylor series expansion and converges to the second-order term, as shown in equation (14) [19,20]:

In equation (14), n is an arbitrary constant, and F k the state transition matrix representing the system; combined with equation (12), the state transition matrix ϕ k can be represented by equation (15):

In equation (15), Θ and Ψ both represent matrix blocks. In order to obtain the system noise covariance in the prediction process, it is necessary to Q k discretize it according to the noise matrix and the state transition matrix and then approximate it into a third-order form, as shown in equation (16) [21,22]:

And the noise covariance matrix are Q k obtained from equation (16) ϕ k , and the Kalman wave prediction process is shown in equation (17):

In equation (17), x k , x k − 1 the state values of the P k ∣ k − 1 , Q k current moment k and the previous moment are, respectively, represented, and k − 1 both represent the system variance matrix, where x ∧ k ∣ k − 1 represents k the state-predicted value at x ∧ k the moment, and the state-estimated value at this moment. The prediction process is the basis of the Kalman filter update, which can obtain the optimal estimate of the system state through the criterion of minimizing the variance. With the growth of time, due to the extremely high frequency of attitude estimation output by the IMU, this also leads to a large accumulation error of the final attitude. Therefore, the attitude output value of IMU can be corrected by using the low-frequency output result of visual SLAM and improved the robustness of the system. Using the observation matrix H , the equation of the observation equation can be obtained as shown in equation (18):

The update process of the Kalman filter can be obtained from equation (18), as shown in equation (19) [23]:

In equation (19), R k represents the observation noise variance matrix. In the filtering process, it is necessary to set initial values for some parameters, including the initial state quantity X k , the initial system covariance matrix P k , the system noise Q c , and R represents the observation noise covariance matrix. After time synchronization, both the IMU and the depth camera sensor start to accumulate from the initial value of zero, and the initial value of the gyro bias is shown in equation (20):

The initial value obtained by equation (20) and its parameters are shown in equation (21):

Since the initial value of the system noise covariance matrix determines the initial filter gain of the system, but it has little effect on the convergence speed, it will be P k set to 6*6 the unit diagonal matrix. The filtering performance is greatly affected by the initial value of the system noise covariance matrix Q c . When the elements increase, the uncertainty of the system noise and system parameters will also increase, and the gain matrix will also increase, and the robustness and robustness of the system will also increase. Sex can be improved. If a better convergence value needs to be obtained, Q c the value is shown in equation (22):

When R the element value in the matrix increases, it means that the influence of measurement noise increases, while the gain matrix decreases, and the system correction weights also weaken, so that the transient response and steady-state value of the system also follow. reduce. The choice of matrix Q c and R is contradictory, R and Q c determines the degree of confidence in measurement and observation, respectively. In this study, the visual SLAM measurement value is used as the observation value, and the attitude value output by the IMU is used as the prediction value. Therefore, the study will assign a R smaller noise value. After adjustment, the setting of this value is shown in equation (23):

In general, it is not necessary to re-adjust the initial parameter values before replacing the IMU device. The overall algorithm flow of robot indoor navigation point cloud map generation based on visual SLAM and Kalman filter visual-inertial navigation attitude fusion is shown in Figure 4.

Figure 4

Algorithm flow of robot indoor navigation point cloud map generation based on visual sensing.

As shown in Figure 4, the algorithm for generating the point cloud map of robot indoor navigation is mainly divided into three modules. The tracking line mainly obtains the camera pose and the sparse point cloud through the depth camera of the visual SLAM, and then determines the new keyframe and analyzes the keyframe. It makes reasonable additions or deletions and performs point cloud splicing and fusion. Finally, the closed-loop detection is used to optimize the essential map, and the attitude information output by visual SLAM and inertial navigation is used as the observed value and the predicted value to improve the robustness of the attitude acquisition process and finally obtain a more accurate robot indoor navigation point cloud map [24,25].