Navigation method based on mobile phone GNSS and visual VO

Abstract

The application discloses a navigation method based on mobile phone GNSS and visual VO, solves the time synchronization problem of visual VO and GNSS data through a visual VO and GNSS data initialization algorithm, uses a trained visual VO error according to a time sequence model to model the visual VO error accumulation after preprocessing according to a time sequence, finally adopts a variational Bayesian adaptive Kalman filtering to fuse the visual VO and mobile phone GNSS data, and positioning accuracy better than that of the mobile phone GNSS is obtained. The application has better accuracy and robustness, establishes a relationship formula of GNSS observation data and VO data combination navigation, introduces a probability density calculation problem of a prediction value, namely, uses adaptive variational Bayesian Kalman filtering for fusion processing. The application avoids the problem that the VO data time accumulation is easy to drift to a certain extent, and can effectively remove GNSS measurement noise to restore the real positioning effect.

Description

Technical Field

[0001] This invention relates to a navigation method based on mobile phone GNSS and visual VO, belonging to the technical field of navigation methods. Background Technology

[0002] The demand for navigation, positioning, and timing (PNT) services has permeated all sectors of the national economy, with its application becoming increasingly widespread in high-precision mobile phone navigation and positioning. Smartphones are composed of multiple sensors, among which the GNSS module enables positioning functionality. The BeiDou Navigation Satellite System (BDS), as the core infrastructure of my country's location service system, can provide all-weather, global, and continuous high-precision positioning services.

[0003] However, in environments such as urban canyons and forested areas, the availability, reliability, continuity, and accuracy of BDS (Browser-Based Navigation System) are severely affected. To ensure continuous and reliable high-precision positioning services, integrated navigation and positioning systems based on multi-sensor data fusion have become the mainstream approach. These systems offer the advantages of complementary strengths across multiple systems, providing interference-resistant, anti-spoofing, continuous, and reliable PNT (Point-of-Touch) service information. Currently, high-performance GNSS / INS integrated navigation systems are expensive, hindering the industrialization of navigation and positioning. Visual odometry (VO), on the other hand, offers advantages such as low cost, ease of operation, and easy integration with target tracking, map building, and obstacle avoidance functions. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention aims to provide a navigation method based on mobile phone GNSS and visual VO. This invention solves the technical problem that the positioning accuracy of mobile phone GNSS alone drops sharply in complex urban environments, and effectively improves the robustness and high accuracy of mobile phone GNSS / VO combined navigation positioning in complex environments.

[0005] The technical solution provided by this invention to solve the above-mentioned technical problems is: a navigation method based on mobile phone GNSS and visual VO, comprising the following steps:

[0006] Step 1: Acquire mobile phone GNSS data and synchronized visual VO data;

[0007] Step 2: Perform preliminary localization and mapping using a monocular visual SLAM system, acquire visual VO keyframe data, and obtain GNSS information by broadcast ephemeris calculation;

[0008] Step 3: Preprocess the visual VO keyframe data and use the linear prediction method to calculate the visual VO location information and quaternion information corresponding to each mobile phone GNSS timestamp;

[0009] Step 4: Rotate and align the visual VO data, and solve for the scaling factor and rotation matrix;

[0010] Step 5: Scale and rotate the visual VO data using a scaling factor and a rotation matrix to restore the true scale of the visual VO data;

[0011] Step 6: Subtract the synchronized visual VO data and RTK data in Step 1 to obtain the drift error of the visual VO data, and perform LSTM modeling on the drift error to obtain a time series model of the visual VO error.

[0012] Step 7: Correct the visual VO data obtained in Step 5 according to the time series model based on the visual VO error;

[0013] Step 8: The corrected visual VO data and the GNSS information obtained in Step 2 are filtered by variational Bayesian adaptive Kalman filtering to obtain the filtered and corrected mobile phone GNSS data.

[0014] A further technical solution is that, in step 3, the method of position linear interpolation and quaternion spherical linear interpolation based on time is used to calculate the visual VO location information and quaternion information corresponding to each mobile phone GNSS timestamp.

[0015] A further technical solution is that the specific process of the linear interpolation of the position is as follows: retain the coordinates of VO data with corresponding GNSS time, and select the coordinates of the two most recent time points before and after the VO data without corresponding time for linear interpolation.

[0016] A further technical solution is that the calculation formula for the quaternion spherical linear interpolation is:

[0017]

[0018] In the formula: θ is the angle between the corresponding quaternions at the two time points; t is the interpolation parameter.

[0019] A further technical solution is that the rotation matrix in step 4 is:

[0020] R = VU T

[0021] T = -RP1 + P2

[0022]

[0023] [USV] = SVD(H)

[0024]

[0025] In the formula: P1 and P2 are the coordinates of the i-th point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; P1 and P2 are the coordinates of the center point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; R is the rotation matrix; T is the translation vector; H is a square matrix; S, V, and D are three matrices of the same size.

[0026] A further technical solution is that the scaling factor in step 4 is:

[0027]

[0028] In the formula: P1 and P2 are the coordinates of the i-th point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; P1 and P2 are the coordinates of the center point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; R is the rotation matrix; and a is the scaling factor.

[0029] A further technical solution is that the following transformation relationship is used for scaling in step 5;

[0030]

[0031] In the formula: a is the scaling factor; R is the rotation matrix; S2 is the VO positioning trajectory coordinate system; S'2 is the WGS84 coordinate system.

[0032] A further technical solution is that, in step 8, a variational Bayesian adaptive Kalman filter algorithm is used to form a joint recursive estimate based on the posterior distribution of the time step state and noise parameters.

[0033] Applying the variational Bayes principle to the standard Kalman filter, we first consider the mean x of the initial state. kk The initial covariance matrix P kk The prediction is performed, iterated N times, and the state vector and state covariance matrix at time k are output.

[0034] A further technical solution is that the iterative equation in step 8 is:

[0035]

[0036]

[0037] Update Kalman gain:

[0038]

[0039]

[0040]

[0041]

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050] In the formula: F is the state transition matrix of the system model, H is the observation matrix of the observation model, z is the observation sequence, Q is the process noise covariance matrix, R is the observation noise covariance matrix, v1 is the initial parameter of the state covariance matrix, and v2 is the initial parameter of the observation noise covariance matrix.

[0051] The present invention has the following beneficial effects:

[0052] 1) When fusing visual VO data and GNSS data, this invention uses a long short-term memory (LSTM) neural network to model and calculate the visual VO data error to optimize the visual VO data, and then loosely combines it with the GNSS results to improve the overall accuracy of the system.

[0053] 2) This invention offers improved accuracy and robustness with VO / GNSS data. It establishes a relationship between GNSS observation data and VO data for combined navigation, introducing the probability density calculation of predicted values, i.e., employing adaptive variational Bayesian Kalman filtering for fusion processing. This invention, to a certain extent, avoids the problem of VO data drift due to time accumulation and can effectively remove GNSS measurement noise to restore the true positioning effect. Attached Figure Description

[0054] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0055] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] This invention discloses a navigation method based on mobile phone GNSS and visual VO (Vision Object Array). Addressing the issue of drastic accuracy degradation of GNSS in complex urban environments, this method introduces combined GNSS and visual VO navigation to improve the availability, reliability, continuity, and accuracy of mobile phone GNSS positioning. A visual VO and GNSS data initialization algorithm solves the time synchronization problem between visual VO and GNSS data, while also addressing the scale matching and coordinate transformation issues between visual VO location information and the actual geographic location. A trained visual VO error model based on time series is used to model the accumulated visual VO error after preprocessing, reducing the cumulative drift of visual VO data and improving its accuracy. Finally, a variational Bayesian adaptive Kalman filter is employed to fuse visual VO and mobile phone GNSS data, achieving positioning accuracy superior to that of mobile phone GNSS.

[0057] like Figure 1 As shown, the specific steps include:

[0058] Step 1: Acquire mobile phone GNSS data and synchronized visual VO data;

[0059] Step 2: Perform preliminary localization and mapping using a monocular visual SLAM system, acquire visual VO keyframe data, and obtain GNSS information by broadcast ephemeris calculation;

[0060] Step 3: Preprocess the visual VO keyframe data by scaling the overall time of the visual VO keyframe data to match the actual length of the corresponding video, and use the linear prediction method to calculate the visual VO location information and quaternion information corresponding to each mobile phone GNSS timestamp.

[0061] Considering the mismatch between the GPS positioning and VO positioning times obtained by the mobile phone, it is necessary to perform time synchronization processing on the two sets of data. The initial GPS data acquisition frequency is 1 second, while the initial VO data is stretched out as a whole, and the displayed time is the time of the key frame of the SLAM video.

[0062] The matching method employs positional linear interpolation based on time and quaternion spherical linear interpolation.

[0063] VO data coordinates with corresponding GPS times are retained; for VO data without corresponding times, the coordinates of the two most recent time points are selected for linear interpolation.

[0064] By using spherical linear interpolation to process quaternions at the corresponding time, we can obtain the quaternion at the corresponding time:

[0065]

[0066] t=(tdesired -t1) / (t2-t1)(0≤t≤1) (2)

[0067] In the formula: θ is the angle between the corresponding quaternions at two different time points; t is the interpolation parameter;

[0068] Step 4: Rotate and align the visual VO data, and solve for the scaling factor and rotation matrix;

[0069] To convert the VO data to the WGS84 coordinate system, the two trajectory segments were first matched in space and time, and then the coordinates were corrected in three directions.

[0070] Let the GNSS trajectory coordinate system be S1 and the VO positioning trajectory coordinate system be S2. The transformation relationship between the two coordinate systems can be expressed by the following formula:

[0071] S2=RS1+T (3)

[0072] Before performing operations on two coordinate systems, a center point needs to be established for each system. The arithmetic mean of the coordinates of all points along the entire trajectory is selected as the center point. For n points in the motion process, assume that the coordinates of their respective center points are P1 and P2, and the coordinates of the i-th point in both coordinate systems are... but

[0073]

[0074] The overdetermined equations are solved using the Singular Value Decomposition (SVD) method:

[0075]

[0076] [USV] = SVD(H) (6)

[0077] Here, H is a square matrix, and by decomposing H using this method, we can obtain three matrices of the same size: S, V, and D.

[0078] Based on the three matrices obtained from the decomposition, the rotation matrix R and translation vector T between the two coordinate systems can be calculated.

[0079] R = VU T (7)

[0080] T = -RP1 + P2 (8)

[0081] In the formula: P1 and P2 are the coordinates of the i-th point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; P1 and P2 are the coordinates of the center point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; R is the rotation matrix; T is the translation vector; H is a square matrix; S, V, and D are three matrices of the same size.

[0082] The above algorithm can be used to obtain two trajectories with the same center position and the same direction of motion. At this time, the two trajectories can be considered to be consistent and coincident without considering the scale.

[0083] Correct the VO trajectory coordinates to a coordinate system in meters:

[0084]

[0085] In the formula: P1 and P2 are the coordinates of the i-th point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; P1 and P2 are the coordinates of the center point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; R is the rotation matrix; a is the scaling factor.

[0086] Step 5: Scale and rotate the visual VO data using a scaling factor and a rotation matrix to restore the true scale of the visual VO data;

[0087] The formula for transforming VO coordinate data to a coordinate system in meters can be obtained based on the scaling factor 'a':

[0088]

[0089] In the formula: a is the scaling factor; R is the rotation matrix; S2 is the coordinate system of the VO positioning trajectory; S'2 is the WGS84 coordinate system;

[0090] At this point, we can convert the VO data to the corresponding GPS WGS84 coordinate system;

[0091] Step 6: Subtract the synchronized visual VO data and RTK data in Step 1 to obtain the drift error of the visual VO data, and perform LSTM modeling on the drift error to obtain a time series model of the visual VO error.

[0092] Since the VO data has an accumulated offset over time relative to the precise coordinate data measured by RTK, LSTM time series is used to accurately predict and restore the error offset. This method retains the advantage of Recurrent Neural Networks (RNNs) in accurately analyzing sequential data, while solving the problems of gradient vanishing and gradient exploding in long sequence data. LSTM uses the cell state as the core and provides gate structures to control the flow of information, realizing state updates and predictions. The specific operations of input gate i, forget gate f, cell state update c, and output gate o are as follows:

[0093] i t =σ(W xi x t +W hi h t-1 +W ci c t-1 +b i (11)

[0094] f t =σ(W xf x t +W hf h t-1 +W ci c t-1 +b i (12)

[0095] c t =f t ×c t-1 +i t ×tanh(W xc x t +W hc h t-1 +b c (13)

[0096] o t =σ(W xo x t +W ho h t-1 +W co c t +b O (14)

[0097] h t =o t ×tanh(c t (15)

[0098] Where t represents the current time step, x t h represents the input at the current time step. tThis represents the output at the current time step, where W and b represent the weight matrix and bias term, respectively, σ represents the sigmoid activation function, and tanh represents the hyperbolic tangent activation function.

[0099] Step 7: Correct the visual VO data obtained in Step 5 according to the time series model based on the visual VO error;

[0100] Step 8: The corrected visual VO data and the GNSS information obtained in Step 2 are filtered by variational Bayesian adaptive Kalman filtering to obtain the filtered and corrected mobile phone GNSS data.

[0101] Based on the characteristics of the experimental data, the variational Bayesian adaptive Kalman filter algorithm is used to form a joint recursive estimate based on the posterior distribution of the time step state and noise parameters.

[0102] Applying the variational Bayes principle to the standard Kalman filter, we first consider the mean x of the initial state. kk The initial covariance matrix P kk To make predictions;

[0103]

[0104]

[0105] Update Kalman gain:

[0106]

[0107]

[0108]

[0109]

[0110]

[0111]

[0112]

[0113]

[0114]

[0115]

[0116]

[0117]

[0118] In the formula: F is the state transition matrix of the system model, H is the observation matrix of the observation model, z is the observation sequence, Q is the process noise covariance matrix, R is the observation noise covariance matrix, v1 is the initial parameter of the state covariance matrix, and v2 is the initial parameter of the observation noise covariance matrix.

[0119] Iterate N times and output the state vector and state covariance matrix at time k.

[0120] The above description is not intended to limit the present invention in any way. Although the present invention has been disclosed through the above embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some changes or modifications to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall fall within the scope of the present invention.

Claims

1. A navigation method based on mobile phone GNSS and visual VO, characterized in that, Includes the following steps: Step 1: Acquire mobile phone GNSS data and synchronized visual VO data; Step 2: Perform preliminary localization and mapping using a monocular visual SLAM system, acquire visual VO keyframe data, and obtain GNSS information by broadcast ephemeris calculation; Step 3: Preprocess the visual VO keyframe data and use the linear prediction method to calculate the visual VO location information and quaternion information corresponding to each mobile phone GNSS timestamp; Step 4: Rotate and align the visual VO data, and solve for the scaling factor and rotation matrix; The scaling factor in step 4 is: In the formula: , These are the coordinates of the i-th point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively. , These are the center point coordinates of the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; R is the rotation matrix. 'a' is the scaling factor; Step 5: Scale and rotate the visual VO data using a scaling factor and a rotation matrix to restore the true scale of the visual VO data; Step 6: Subtract the synchronized visual VO data and RTK data in Step 1 to obtain the drift error of the visual VO data, and perform LSTM modeling on the drift error to obtain a time series model of the visual VO error. Step 7: Correct the visual VO data obtained in Step 5 according to the time series model based on the visual VO error; Step 8: The corrected visual VO data and the GNSS information obtained in Step 2 are filtered by variational Bayesian adaptive Kalman filtering to obtain the filtered and corrected mobile phone GNSS data. The iterative equation in step 8 is: Update Kalman gain: In the formula: F is the state transition matrix of the system model, H is the observation matrix of the observation model, z is the observation sequence, Q is the process noise covariance matrix, and R is the observation noise covariance matrix. Let be the initial parameters of the state covariance matrix. These are the initial parameters for the observation noise covariance matrix.

2. The navigation method based on mobile phone GNSS and visual VO according to claim 1, characterized in that, In step 3, the location information and quaternion information corresponding to each mobile phone GNSS timestamp are calculated by using position linear interpolation and quaternion spherical linear interpolation based on time.

3. The navigation method based on mobile phone GNSS and visual VO according to claim 2, characterized in that, The specific process of linear interpolation of the location is as follows: retain the coordinates of VO data with corresponding GNSS time, and select the coordinates of the two most recent time points before and after the VO data without corresponding time for linear interpolation.

4. A navigation method based on mobile phone GNSS and visual VO according to claim 2, characterized in that, The calculation formula for the quaternion spherical linear interpolation method is as follows: In the formula: θ The angle between the quaternions corresponding to the two time points; t These are the interpolation parameters.

5. A navigation method based on mobile phone GNSS and visual VO according to claim 1, characterized in that, The rotation matrix in step 4 is: ， In the formula: , These are the coordinates of the i-th point in the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively. , R represents the center point coordinates of the GNSS trajectory coordinate system and the VO positioning trajectory coordinate system, respectively; R is the rotation matrix; T is the translation vector; H is a square matrix; S, V, and D are three matrices of the same size.

6. A navigation method based on mobile phone GNSS and visual VO according to claim 1, characterized in that, In step 5, the following transformation relationship is used for scaling; In the formula: a is the scaling factor; R is the rotation matrix; Establish the coordinate system for the VO positioning trajectory; It is in the WGS84 coordinate system.

7. A navigation method based on mobile phone GNSS and visual VO according to claim 1, characterized in that, In step 8, the variational Bayesian adaptive Kalman filter algorithm is used to form a joint recursive estimate based on the posterior distribution of the time step state and noise parameters. Applying the variational Bayes principle to the standard Kalman filter, we first consider the mean of the initial state. The covariance matrix of the initial state To make predictions and iterate. N Next, output the state vector and state covariance matrix at time k.

Images