VO and GPS integrated navigation algorithm based on plane constraint

. In view of the deficiencies of GPS in the urban environment, such as signal occlusion, multipath and Non-LOS, this paper proposes a solution that using a low-cost monocular camera instead of inertial navigation system to combine with GPS. Firstly, considering that the actual road conditions of the vehicle are mostly plane and in order to reduce the computational resources of visual odometer (VO), this paper establishes a VO solving model based on plane constraint; then the VO&GPS loose coupling model is proposed. The position error and yaw angle error of the two sensors are taken as input, and the errors are filtered by the adaptive Kalman filter. Experiments show that the positioning effect of VO&GPS loose coupling system is better than that of single GPS, and it can suppress the drift of VO, which can meet the positioning requirements when satellite signals are interfered in a short time.


Introduction
Navigation positioning of vehicles is a key technology in the field of unmanned driving. Due to its own limitations, a single navigation and positioning system cannot meet the positioning accuracy requirements of unmanned vehicles, so integrated navigation technology is needed. At present, the INS/GPS integrated navigation system is the most common, but to obtain accurate vehicle positioning information, it is necessary to use the expensive INS to integrate with GPS, which will increase the vehicle cost. Therefore, a more suitable positioning scheme is needed to solve the above problems.
The development of computer vision has made vision-based positioning technology widely used [1] and visual odometer (VO) is gradually being used to assist GPS positioning. According to the classic GPS/INS loose combination system error model, a GPS/VO combination system is proposed in [2], which combines the data of both monocular VO and GPS through Kalman filter. The fusion algorithm proposed in [3] combines GPS carrier phase measurement with feature-based monocular visual odometer. The fusion algorithm can effectively suppress the error drift of the visual odometer even in the case that GPS can only receive two or three satellites. In [4], a EKF-based stereo vision and GPS fusion vehicle positioning method is proposed to reduce the cumulative error of stereo vision. A navigation system of the dual GPS fusion stereo camera is constructed in [5], which can provide vehicle status information position, attitude and speed for intelligent navigation. In [6], the Kalman filter is used to merge the binocular visual odometer positioning result with the GPS positioning result to improve the positioning accuracy of the integrated system. In [7], a method to improve the accuracy of GNSS positioning by binocular vision using the geographical location information of landmarks is proposed.
An algorithm for precise position based on low-cost camera combined with GPS is proposed in this paper. Considering that the actual road conditions of the vehicle are mostly planes and reducing the computational resources of VO, this paper first establishes a VO solving model based on plane constraint. Then the VO&GPS loose coupling model is proposed. The position error and yaw angle error of the two sensors are taken as input, and the error is filtered by the adaptive Kalman filter in the VO&GPS model. Finally, the effectiveness of the algorithm is verified by experiments.

Epipolar geometry constraint
Epipolar geometry constraint is the key to the model of monocular vision odometer based on plane constraint. It describes the mapping relationship of a certain point in three-dimensional space between the perspective of two cameras. Where, the camera pose information reflected by the perspective of two cameras is included in the essential matrix E. Fig. 1 shows the basic principle of epipolar geometry constraint. The three-dimensional point Q and its two-dimensional projection point 1 q and 2 q are coplanar under the camera perspective. If the camera perspective 0 C is fixed, it can be determined that the corresponding point of 1 q on the perspective image 0 C is located on the straight line 2 l , which is called the polar line.In other words, the polar line 2 l is the epipolar line of the imaging point 1 q , and there is also a epipolar line 1 l corresponding to 2 q on the imaging plane of the perspective 0 C .

General 8-point pose solution
According to the epipolar geometric constraint between two views: where, F is the basic matrix, 1 q and 2 q are a pair of feature points in the image coordinate system. 1 q and 2 q are 1 * 3 dimensional vectors, superscript T represents the transposition of matrix. Order: Eq. 1 and Eq. 2 can be used to establish the linear homogeneous equation. The rank of coefficient matrix of the linear homogeneous equation is 8. Only 8 pairs of matching feature points are needed to calculate the normalized basic matrix F. After that, calibrating the camera to estimate the internal parameter matrix K of the camera. According to the basic matrix F and the internal parameter matrix K of the camera, the essential matrix E can be obtained. The normalized rotation matrix R and the shift matrix T can be obtained by singular value decomposition of the essential matrix E.
The process of solving essential matrix E, rotation matrix R and normalized translation matrix T is as follows: According to Eq. 3, the essential matrix E can be obtained. The two non-zero singular values of the essential matrix E are equal. Therefore, matrix U and V can be obtained by the following singular value decomposition method.
Through U and V, four different groups of rotation matrix R and translation matrix T can be obtained. The calculation formula is shown in Eq. 5.
where 3 U represents the third row of the U matrix. A pair of feature points are used to calculate the depth value of the point in two images. If both depth values are greater than 0, the unique R and T are determined [8,9].

Design of visual odometer algorithm based on plane constraint
According to the knowledge of spatial rotation, the matrix n n R 1 can be obtained by rotating the vehicle coordinate system around the y-axis firstly in the n-time, and the rotation angle is defined as the yaw angle w ,then rotating around the x-axis, and the rotation angle is defined According to Eq. 6, the spatial coordinate system can be projected to the plane, and the vehicle can be simplified as a particle. As shown in Figure 2 can be expressed as the angle between the velocity of point A and displacement AB in Fig.   1, and the calculation process is shown in Eq. 7.
According to Fig. 2, the equation of position change and yaw angle change from point A to point B can be obtained, as shown in Eq. 8.

Acquisition of experimental data
Since the monocular VO lacks the scale information, the 35m long straight road was equidistantly marked at intervals of 10cm before the experiment, and the separation distance was used as the scale of the monocular VO solution. The experiment was divided into three groups. The first group: the CCD camera was mounted on a tripod, as shown in Fig. 3(a). The pictures were taken continuously along the marked points of the 35m long straight road, and the captured scene pictures are shown in Fig. 4. The second and third groups: the receiver was mounted on a tripod, as shown in Fig. 3(b), and the GPS and RTK data of the straight marker points were respectively collected. Where, the RTK data was used as the ground truth. Since the position of the RTK base station may not be very accurate, there may be some systematic error, but it does not affect the verification of the algorithm. The parameters of each experimental equipment are shown in Table 1.

Monocular VO straight driving performance analysis
The images collected by the experiment are solved according to the visual odometer model described in Section 2.3, and the calculated centroid sideslip angle change amount  Fig. 4 are used to pose calculation by the Eq. 10. Where AB represents the scale of the monocular VO, and the specific value is the 10 cm equidistant interval of the straight road. After the solution, the trajectory of the monocular VO under the straight road is obtained, and the comparison of the monocular VO with the GPS and RTK trajectories is shown in Fig. 6. Before the 20m of the straight road, the deviation of the VO trajectory is generally smaller than that of the GPS, which indicates that the monocular VO is more reliable than the single GPS positioning in a short time. Due to the cumulative error of the monocular VO, after 20m, the deviation of the VO trajectory begins to exceed the GPS.

VO&GPS loose coupling system principle
The integrated navigation system is divided into two modes: open-loop correction and closed-loop correction. This paper refers to the GPS and INS loose coupling system to build the open-loop correction system shown in Fig. 7. In practical engineering, the model error Q and measurement error R of the Kalman filter algorithm are difficult to be accurately measured. The accuracy of the estimation of Q and R determines the accuracy of the Kalman filter. Therefore, the adaptive Kalman filter [11] is used to fuse VO and GPS information.
where ,   T  gps  vo  gps  vo  gps  vo   y  y  x  x  Y  ,  ,  ,   T   y  x  X  ,  , , . Both w and v are Gaussian white noise.

VO&GPS Loose Coupling System Straight Driving Performance Analysis
The 10Hz monocular VO data obtained by the solution and the interpolated GPS data are combined in accordance with the adaptive Kalman algorithm. The trajectory comparison between the VO&GPS loose coupling system and other positioning systems is shown in Fig.  8. As can be seen from Fig. 8, before the 20m of the straight road, the trajectory of the VO&GPS loose coupling system is significantly closer to the trajectory of the RTK than the GPS alone due to the assistance of VO. After the 20m of the straight road, since the assistance of GPS weakens the influence of the cumulative error of VO, the positioning error of the VO&GPS loose coupling system is still in an acceptable range. The positioning errors of the three positioning systems in the X and Y directions are shown in Fig. 9 and Table 2, and they can also explain this problem. The above results can all indicate that the positioning effect of the VO&GPS loose coupling system is better than that of the single GPS when running 35m in a straight line, and it can suppress the cumulative drift of VO, which can meet the positioning requirements when the satellite signal is interfered in a short time.

Summary
Firstly, this paper proposes a monocular VO solving model based on plane constraint. By taking offline processing of the pictures collected by the experiment, the solved monocular VO can still maintain high precision by running 35m along the straight line. Then, the VO&GPS loose coupling model is proposed. The position error and yaw angle error of the two sensors are taking as input, and the errors are filtered by an adaptive Kalman filter. When the VO&GPS loose coupling system runs 35m in a straight line, its average positioning error is better than the single GPS. The experimental results show that the VO&GPS integrated navigation algorithm proposed in this paper can improve the positioning accuracy of the navigation system and suppress the cumulative drift of VO to meet the positioning requirements when satellite signals are interfered in a short time.