Unmanned aerial vehicle dog RTK positioning method based on singular value robust Kalman filtering

Abstract

The invention discloses an unmanned aerial vehicle dog RTK positioning method based on singular value robust Kalman filtering. The method comprises the following steps that 1, a robot dog GNSS receiver obtains satellite and base station data; 2, constructing a double-difference observation equation; the method comprises the following steps of: 1, carrying out a floating point ambiguity vector nK, 2, carrying out singular value robust Kalman filtering, and 4, carrying out integer ambiguity fixing and fixed solution calculation, fixing a floating point ambiguity vector nK as an integer ambiguity vector, and solving an integer non-ambiguity state vector # imgabs0 #, namely, a final robot dog receiver r position fixed solution. According to the method, the ambiguity fixing probability is improved, and the RTK positioning robustness of the unmanned aerial vehicle dog is improved.

Description

Technical Field

[0001] The invention belongs to the field of positioning and navigation of unmanned aerial vehicle dogs, and specifically relates to an unmanned aerial vehicle dog RTK positioning method based on singular value anti-error Kalman filtering. Background Art

[0002] In automation, rescue and other scenarios, drones usually need to map the moving area and perceive and locate. SLAM technology, LiDAR, inertial navigation, GPS / RTK positioning, etc. are commonly used positioning and perception methods for drones. GNSS-RTK is a carrier phase differential real-time dynamic positioning technology based on the global positioning satellite system. It can provide dynamic real-time centimeter-level precision positioning in real time. RTK technology involves two GNSS receivers. The principle of its application in drone terminals can be summarized as follows: Both the base station and the robot mobile station are equipped with GNSS receivers. The base station is fixed in a place with good reception and communication environment, and its precise three-dimensional coordinate information is known. The drone mobile station is placed in an outdoor application environment. The robot can be moving or stationary. The base station receives and processes the signals of all visible GNSS satellites and calculates the pseudorange observation value and carrier phase observation value. The observation value and its own position information are sent to the robot mobile station through a wireless data transmission link. After receiving the data, the robot dog mobile station synchronously observes and collects the carrier phase data of the GNSS satellite, and then calculates the carrier phase integer ambiguity through differential processing within the system. According to the correlation between the robot dog mobile station and the base station, the robot dog's precise northeast coordinates (E, N) and sky coordinates (U) are obtained.

[0003] However, in most robot dog application scenarios, due to the small space and dense components of the robot dog equipment, the mobile station GNSS antenna will be subject to electromagnetic interference from various devices after installation. In addition, the outdoor application scenarios of robot dogs usually have poor communication conditions. Occlusion and multipath interference will cause the antenna signal to be temporarily lost or fluctuate, which will significantly reduce the regression performance of the RTK algorithm based on the Kalman filter. A large number of gross errors will seriously affect the ambiguity fixation settlement, and the return to a stable settlement state will consume a lot of time, which will greatly reduce the positioning accuracy and make it impossible to achieve high-precision positioning. The decline is serious and ultimately the regional mapping and perception positioning results of the robot dog are invalid. In summary, how to achieve stable RTK settlement in harsh propagation environments when the robot dog GNSS receiver data is subject to interference, loss or fluctuations, and the results will not produce abnormal jumps, reduce gross errors to make the floating solution return to the actual state, increase the probability of ambiguity fixation, and improve the robustness of RTK positioning of unmanned robot dogs is an important issue. Summary of the invention

[0004] In order to overcome the shortcomings of the existing technology, when a robot dog performs regional mapping or perception positioning, the robot dog GNSS receiver signal is affected by electromagnetic interference, signal shielding, multipath effect path, etc., resulting in abnormal jumps or losses of RTK pseudorange observation values ​​and carrier phase observation values, a large number of gross errors, and integer ambiguity jump distortion that is difficult to search or cannot be fixed, and ultimately the positioning solution fails or the accuracy is reduced. The present invention provides an unmanned robot dog RTK positioning method based on a singular value anti-differential Kalman filter, which linearizes the double difference carrier and pseudorange observation equations solved by the robot dog receiver data, brings them into an innovative singular value anti-differential Mann filtering algorithm, and then searches and fixes the integer ambiguity through an ambiguity fixing algorithm (MLAMBDA), and finally obtains a high-precision fixed solution, and outputs the result to the mapping positioning data, thereby achieving high-precision and more stable and interference-resistant robust unmanned robot dog RTK positioning.

[0005] The technical solution adopted by the present invention to solve its technical problem is:

[0006] A method for RTK positioning of an unmanned aerial vehicle based on singular value robust Kalman filtering, the method comprising the following steps:

[0007] Step 1: The robot dog GNSS receiver obtains satellite and base station data. The base station GNSS receiver sends the base station coordinate position, carrier phase and pseudo-range measurement value, and ephemeris auxiliary data information to the robot dog receiver through a wireless transmission link; at the same time, the robot dog mobile station synchronously measures the corresponding satellite data based on the satellite data of the base station to obtain the carrier and pseudo-range measurement values ​​of the robot dog GNSS receiver; the carrier and pseudo-range measurement values ​​received by the robot dog GNSS receiver and the base station GNSS are combined to form a receiver observation value vector. Because the satellite sends carrier signals and ranging code signals of multiple frequencies, different frequency information corresponds to different carrier signals and ranging code signals, and multiple receiver observation value vectors of different frequencies can be formed. For L1 frequency, the corresponding observation value vector is:

[0008]

[0009] in, Indicates the robot dog GNSS receiver number, Indicates the base station GNSS receiver number, Indicates the common view satellite number, represents the pseudo-range observation value of the base station GNSS receiver r to the common-view satellite j, represents the carrier phase observation value of the common-view satellite j by the GNSS receiver r at the base station, Similarly;

[0010] Step 2: Construct the double difference observation equation, adjust (1), and establish equations for each carrier phase and pseudorange measurement value. The carrier measurement value and pseudorange measurement value of j received by r have the following observation relationship equation:

[0011]

[0012] in, c≈3.0×10 8 m / s represents the speed of light; Indicates the receiver clock error; represents the satellite clock error; represents the ionospheric delay; Indicates delay to the process; Indicates the integer value of the integer ambiguity; represents the pseudorange observation noise; represents the carrier phase observation noise; Indicates the actual distance between the satellite and the robot dog receiver;

[0013] Similarly, the equations shown in (2) are also established for the carrier measurement value and pseudorange measurement value sent by u. In the case of dual-frequency L1 and L2 multi-satellite systems, a satellite i with an elevation angle greater than 15°, the highest elevation angle stability, and a good signal-to-noise ratio is selected from the common-view satellites as the reference satellite. The two receivers observe satellite i and form the same observation equation as (2). The two receivers form different single-difference observation equations for i and j respectively, where the single-difference equation for satellite i is expressed as follows:

[0014]

[0015] The intersatellite difference of the single difference equation formed by a series of j and the single difference equation of i is called double difference, and the double difference observation equation of the carrier phase and pseudorange of the two receivers is obtained, which is expressed as follows:

[0016]

[0017] in Double-difference carrier phase observations; Double-difference pseudorange observations, Double difference ambiguity value, Double-difference pseudorange observation noise; Double-difference carrier phase observation noise, Indicates the double difference satellite-to-ground actual distance between two satellites and two receivers;

[0018] Step 3: Singular value robust Kalman filtering,

[0019] Step 4: Integer ambiguity fixation and fixed solution calculation. KIt is fixed to the integer ambiguity vector, and the integer unambiguous state vector a is solved, that is, the final fixed solution of the position of the robot dog receiver r.

[0020] Further, the process of step three is as follows:

[0021] Step 3.1: Linearize the observation equation, the system state prediction equation and system measurement equation of the singular value robust Kalman filter are as follows:

[0022]

[0023] in, represents the epoch time t(k), The state vector of the parameter to be estimated at the t(k-1)th epoch, The state vector of the a priori estimated parameters at the t(k)th epoch, The state vector of the posterior estimated parameters at the t(k)th epoch, A k Measurement state transfer matrix, ω k represents the process noise at time t(k), and the mean is zero and the covariance matrix is ​​Q k The normal distribution of k Represents the measurement noise at time t(k), and satisfies the zero mean and covariance matrix R k Normal distribution of y k represents the double difference observation vector at time t(k), which is synthesized by arranging the double difference carrier phase observations and double difference pseudorange observations of all satellites in the dual frequency case as shown in (4), H k represents the measurement state transition matrix at time t(k);

[0024] y k The corresponding double difference observation equation is transformed into:

[0025]

[0026] According to y k The state vector x can be obtained K , x K From the required position vector e r =(x r ,y r ,z r ) T Velocity information vector v r =(v x ,v y ,v z ), the single difference ambiguity values ​​of all (3) under dual-frequency L1 and L2 conditions are composed of n K vector;

[0027] The state vector x Kand k Corresponding to A K and H K , the state vector x K With y k There is a nonlinear relationship between them, so H K Expressed in Jacobian matrix form:

[0028]

[0029] Between the interval Δt of epoch k and the previous epoch k-1, the movement of the robot dog GNSS receiver r must have a position transfer transformation, A K It is expressed as follows:

[0030]

[0031] in, represents the number of j, m = (6 + (n + 1) * 2) * (6 + (n + 1) * 2), when r is in a static state, Δt = 0, and when it is not static, Δt remains unchanged;

[0032] Q K is called the process noise covariance matrix. The robot dog GNSS receiver r is in dynamic mode, so the corresponding Q K It is expressed in the following form:

[0033]

[0034] in, E r The transfer matrix representing the geocentric fixed-center geographic coordinates ENU, They represent the variance of acceleration in the horizontal latitude E direction, longitude N direction, and height U direction, respectively. diag() represents a diagonal matrix and the diagonal elements correspond to the corresponding position elements inside the brackets. When the robot dog GNSS receiver r is in static mode, Q v Take the zero matrix;

[0035] Step 3.2: The measurement error covariance matrix singular value robustness adjustment, measurement noise covariance matrix R k The form is as follows:

[0036]

[0037] Among them, R Φ,1 , R Φ,2 , R P,1 , R P,1 It is calculated as follows:

[0038]

[0039] in, represents the standard deviation of the carrier phase measurement value of the non-reference satellite j=1, and the subscript 1 indicates that the carrier frequency is L1; represents the standard deviation of the pseudorange measurement value of the non-reference satellite j=2, and the subscript number 1 indicates that the carrier frequency is L1;

[0040] For R k The matrix is ​​adjusted by singular value decomposition as follows:

[0041]

[0042] where Σ k It is expressed as follows:

[0043]

[0044] Among them, Σ k is a diagonal matrix, U k and V k are orthogonal matrices (i.e. their column vectors are orthogonal unit vectors), in particular Σ k The elements on the diagonal are called singular values. This decomposition transforms the matrix R k It is expressed as the product of three simple matrices, which is easy to analyze and process;

[0045] For the singular value matrix Σ k Take the square root of each element on the diagonal;

[0046]

[0047] Simplified to:

[0048]

[0049] The calculation and reorganization results are as follows:

[0050]

[0051] The observed residual vector is expressed as:

[0052]

[0053] For the observed residual vector z k The Element z i , adjust it linearly to the appropriate range, first calculate z k The mean μ of each element in is:

[0054]

[0055] The observation residual vector z kEach element z in i The final linear adjustment is Get the linearly adjusted observation residual vector Each element It is expressed as follows:

[0056]

[0057] but The corresponding adaptive weight factor γ i The calculation method is as follows:

[0058]

[0059] Among them, K1 is an empirical constant, K2 is an empirical constant;

[0060] According to the calculated adaptive weight reduction factor γ i The vector composed of γ adjusts the reorganization Duplicate the vector γ and call it γ', where the corresponding elements are renamed γ j and With i representing the element number, for a certain epoch K Each element in its i-th row and j-th column Adaptive processing to obtain the final singular value robust reconstruction measurement error covariance matrix as follows:

[0061]

[0062] Step 3.3: Calculate the singular value robustness gain and update the Kalman prediction. and the prior error covariance matrix The results of calculating the singular value adaptive gain are as follows:

[0063]

[0064] Calculate the prior estimate state vector at epoch k Update the state vector of the last epoch's filtered posterior The following transformation results are as follows:

[0065]

[0066] Similarly, calculate the prior error covariance matrix The error covariance matrix of the filter posterior update at the previous moment is required The results are as follows:

[0067]

[0068] Posterior state vector The updated result is:

[0069]

[0070] Posterior error covariance matrix The updated result is:

[0071]

[0072] The posterior state vector at epoch K is It is called the posterior error covariance matrix store.

[0073] Furthermore, the process of step 4 is as follows:

[0074] First, the singular value robust Kalman filter A specific linearization adjustment is made to convert and The adjusted state vector Decomposed into an unambiguous floating point state vector and the floating point ambiguity solution vector Further we get:

[0075]

[0076] Among them, Cov represents the covariance matrix of the corresponding vector. represents the covariance matrix of the unambiguous floating-point state vector, Q b represents the floating point ambiguity covariance matrix, Represents the covariance matrix of the non-ambiguity parameters and ambiguity parameters in the state vector;

[0077] Secondly, find the integer solution of the floating-point ambiguity and b LD decomposition is performed to obtain L represents a lower triangular matrix whose diagonal elements are 1 and whose off-diagonal elements are non-integers, Λ represents a diagonal matrix, and the matrix L1 is obtained by taking integers from the off-diagonal elements of L. Transform to get for:

[0078]

[0079] Among them, -T represents the inverse transpose of the matrix or the inverse of the transpose;

[0080] get Repeat the above LD decomposition steps until the nth decomposed unit lower triangular matrix L n Round it down and it can only be taken as the unit matrix, and the final result is Q z for:

[0081]

[0082] Pick in It is called floating point double difference phase deviation. The newly obtained lower triangular matrix G and Λ are brought into the MLAMBDA search algorithm to obtain the integer double difference phase deviation

[0083] Finally, we get the integer ambiguity solution vector The integer unambiguous parameter state vector is obtained by adjusting the integer ambiguity solution vector The process is as follows:

[0084]

[0085] in, represents the integer unambiguity parameter vector The state covariance matrix of represents the integer non-ambiguous state solution, which is also the final solution for the robot dog's position fixation.

[0086] The beneficial effects of the present invention are mainly manifested in: improving the probability of ambiguity fixation and improving the robustness of RTK positioning of unmanned aerial vehicle dogs. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 It is a flow chart of the RTK positioning method of unmanned aerial vehicle dog based on singular value robust Kalman filtering.

[0088] Figure 2 This is a diagram showing the gross error jump effect of the RTK positioning method based on Kalman filtering.

[0089] Figure 3 This is a diagram showing the gross error jump effect of the robust RTK positioning method based on singular value robust Kalman filtering.

[0090] Figure 4 It is the integer ambiguity fixed ratio factor of the robot dog RTK positioning method based on Kalman filter.

[0091] Figure 5 It is the integer ambiguity fixed ratio factor of the UAV RTK positioning method based on singular value robust Kalman filtering. DETAILED DESCRIPTION

[0092] The present invention will be further described below in conjunction with the accompanying drawings.

[0093] Reference Figure 1 to Figure 5 , a RTK positioning method for unmanned aerial vehicle based on singular value robust Kalman filtering, comprising the following steps:

[0094] Step 1: The robot dog GNSS receiver obtains satellite and base station data. The base station GNSS receiver sends the base station coordinate position, carrier phase and pseudo-range measurement value, and ephemeris auxiliary data information to the robot dog receiver through a wireless transmission link; at the same time, the robot dog mobile station synchronously measures the corresponding satellite data based on the satellite data of the base station to obtain the carrier and pseudo-range measurement values ​​of the robot dog GNSS receiver; the carrier and pseudo-range measurement values ​​received by the robot dog GNSS receiver and the base station GNSS are combined to form a receiver observation value vector. Because satellites send carrier signals and ranging code signals of multiple frequencies (usually L1, L2, and L5), different frequency information corresponds to different carrier signals and ranging code signals, and multiple receiver observation value vectors of different frequencies can be formed. Taking the L1 frequency as an example, the corresponding observation value vector is:

[0095]

[0096] in, Indicates the robot dog GNSS receiver number, Indicates the base station GNSS receiver number, Indicates the common view satellite number, represents the pseudo-range observation value of the base station GNSS receiver r to the common-view satellite j, represents the carrier phase observation value of the common-view satellite j by the GNSS receiver r at the base station, Similarly;

[0097] Step 2: Construct the double difference observation equation, adjust (1), and establish equations for each carrier phase and pseudorange measurement value. The carrier measurement value and pseudorange measurement value of j received by r have the following observation relationship equation:

[0098]

[0099] in, c≈3.0×10 8 m / s represents the speed of light; Indicates the receiver clock error; represents the satellite clock error; represents the ionospheric delay; Indicates delay to the process; Indicates the integer value of the integer ambiguity (such as L1, L2, L5 at a specific frequency); represents the pseudorange observation noise; represents the carrier phase observation noise; Indicates the actual distance between the satellite and the robot dog receiver;

[0100] Similarly, the equations shown in (2) are also established for the carrier measurement value and pseudorange measurement value sent by u. In the case of dual-frequency L1 and L2 multi-satellite systems, a satellite i (the only satellite) with an elevation angle greater than 15°, the highest elevation angle stability, and a good signal-to-noise ratio (SNR) is selected from the common-view satellites as the reference satellite. The two receivers can also form the same observation equation as (2) when observing satellite i. The two receivers form different single-difference observation equations for i and j respectively, where the single-difference equation for satellite i is expressed as follows:

[0101]

[0102] The intersatellite difference of the single difference equation formed by a series of j and the single difference equation of i is called double difference, and the double difference observation equation of the carrier phase and pseudorange of the two receivers is obtained, which is expressed as follows:

[0103]

[0104] in Double-difference carrier phase observations; Double-difference pseudorange observations, Double difference ambiguity value, Double-difference pseudorange observation noise; Double-difference carrier phase observation noise. Indicates the double difference satellite-to-ground actual distance between two satellites and two receivers;

[0105] Step 3: Singular value robust Kalman filtering, the process is as follows:

[0106] Step 3.1: Linearize the observation equation, the system state prediction equation and system measurement equation of the singular value robust Kalman filter are as follows:

[0107]

[0108] in, represents the epoch time t(k), The state vector of the parameter to be estimated at the t(k-1)th epoch, The state vector of the a priori estimated parameters at the t(k)th epoch, The state vector of the posterior estimated parameters at the t(k)th epoch, A k Measurement state transfer matrix, ω k represents the process noise at time t(k), and the mean is zero and the covariance matrix is ​​Q k The normal distribution of k Represents the measurement noise at time t(k), and satisfies the zero mean and covariance matrix R k Normal distribution of y krepresents the double difference observation vector at time t(k), which is synthesized by arranging the double difference carrier phase observations and double difference pseudorange observations of all satellites in the dual frequency case as shown in (4), H k represents the measurement state transition matrix at time t(k);

[0109] y k The corresponding double difference observation equation is transformed into:

[0110]

[0111] According to y k The state vector x can be obtained K .x K From the required position vector e r =(x r ,y r ,z r ) T Velocity information vector v r =(v x ,v y ,v z ), the single difference ambiguity values ​​of all (3) under dual-frequency L1 and L2 conditions are composed of n K vector;

[0112] The state vector x K and k Corresponding to A K and H K , the state vector x K With y k There is a nonlinear relationship between them, so H K Expressed in Jacobian matrix form:

[0113]

[0114] Between the interval Δt of epoch k and the previous epoch k-1, the movement of the robot dog GNSS receiver r must have a position transfer transformation, A K It is expressed as follows:

[0115]

[0116] in, Represents the number of j, m = (6 + (n + 1) * 2) * (6 + (n + 1) * 2). When r is in a static state, Δt = 0, and when it is not static, Δt remains unchanged;

[0117] Q K is called the process noise covariance matrix. The robot dog GNSS receiver r is in dynamic mode, so the corresponding Q K It is expressed in the following form:

[0118]

[0119] in, E r represents the transfer matrix from the geocentric center fixed (ECEF) to the geographic coordinates ENU, They represent the variance of acceleration in the horizontal latitude E direction, longitude N direction, and height (vertical) U direction, respectively. diag() represents a diagonal matrix and the diagonal elements correspond to the corresponding position elements inside the brackets. When the robot dog GNSS receiver r is in static mode, Q v Take the zero matrix;

[0120] Step 3.2: The measurement error covariance matrix singular value robustness adjustment, measurement noise covariance matrix R k The form is as follows:

[0121]

[0122] Among them, R Φ,1 , R Φ,2 , R P,1 , R P,1 It is calculated as follows:

[0123]

[0124] in, represents the standard deviation of the carrier phase measurement value of the non-reference satellite j=1, and the subscript 1 indicates that the carrier frequency is L1; represents the standard deviation of the pseudorange measurement value of the non-reference satellite j=2, and the subscript number 1 indicates that the carrier frequency is L1; the rest are similar;

[0125] In the numerical calculation of the system measurement relationship equation, when the measurement error covariance matrix R k When the condition number is large, it is easy to introduce numerical errors, affecting the stability and accuracy of the calculation; by decomposing the singular value and recombining it after certain processing, the condition number of the matrix can be appropriately reduced and the numerical stability can be improved; the robot dog is in a harsh communication transmission environment, and the measurement noise covariance matrix R k It may contain large uncertainty; through singular value decomposition, this uncertainty can be decomposed into different components, and then each component can be processed separately to improve the robustness to noise;

[0126] For R k The matrix is ​​adjusted by singular value decomposition (SVD) as follows:

[0127]

[0128] where Σ k It is expressed as follows:

[0129]

[0130] Among them, Σ k is a diagonal matrix, U k and V k are orthogonal matrices (i.e. their column vectors are orthogonal unit vectors), in particular Σ k The elements on the diagonal are called singular values. This decomposition transforms the matrix R k It is expressed as the product of three simple matrices, which is easy to analyze and process;

[0131] Direct use of Σ k This will lead to numerical instability, especially when the condition number of the matrix is ​​greatly affected by noise (that is, the singular values ​​of the matrix vary greatly). In order to improve numerical stability, the singular value matrix Σ k Taking the square root of each element on the diagonal and the square root of the singular value helps to better balance the influence of each component when combining, avoiding excessive influence of a certain component on the overall result, making the influence of noise in different directions more uniform, and reducing the problems caused by uneven noise distribution;

[0132]

[0133] Simplified to:

[0134]

[0135] After processing (16), the new measurement error covariance matrix required needs to be reassembled Recombinant It reduces the impact of measurement mutation outliers on the filter and greatly reduces the components that are sensitive to outliers. The calculation and reorganization results are as follows:

[0136]

[0137] After the reorganization It is necessary to further dynamically adjust according to the degree of change of the observed residuals of the observed values. For smaller observed residuals, increase the weight; for larger residuals, reduce the weight to adjust The value of each position element of; This adjustment can effectively suppress the influence of abnormal observations and retain the effective information of normal observations; suppress the gross jump of observations affected by multipath, etc., and improve the accuracy and adaptive robustness of robot dog RTK positioning; The observation residual vector is expressed as:

[0138]

[0139] For the observed residual vector z k The Element z i , adjust it linearly to the appropriate range, first calculate z k The mean μ of each element in is:

[0140]

[0141] The observation residual vector z k Each element z in i The final linear adjustment is Get the linearly adjusted observation residual vector Each element It is expressed as follows:

[0142]

[0143] but The corresponding adaptive weight factor γ i The calculation method is as follows:

[0144]

[0145] Among them, K1 is an empirical constant, 1.5<K1<2.5; K2 is an empirical constant, 3.0<K1<5.0;

[0146] According to the calculated adaptive weight reduction factor γ i The vector composed of γ adjusts the reorganization Duplicate the vector γ and call it γ', where the corresponding elements are renamed γ j and With i representing the element number, for a certain epoch K Each element in its i-th row and j-th column Adaptive processing to obtain the final singular value robust reconstruction measurement error covariance matrix as follows:

[0147]

[0148] Step 3.3: Calculate the singular value robustness gain and update the Kalman prediction. and the prior error covariance matrix (The initial error covariance matrix needs to be set manually for the first filtering ) The results of calculating the singular value adaptive gain are as follows:

[0149]

[0150] Calculate the prior estimate state vector at epoch k Update the state vector of the last epoch's filtered posterior (If it is the first filtering, the initial state vector needs to be set ) and the following transformation results are as follows:

[0151]

[0152] Similarly, calculate the prior error covariance matrix The error covariance matrix of the filter posterior update at the previous moment is required (If it is the first filtering, then ), the result is as follows:

[0153]

[0154] Posterior state vector The updated result is:

[0155]

[0156] Posterior error covariance matrix The updated result is:

[0157]

[0158] The posterior state vector at epoch K is It is called the posterior error covariance matrix store;

[0159] Step 4: Integer ambiguity fixation and fixed solution calculation. K Fix the integer ambiguity vector and solve for the integer unambiguous state vector (The final robot dog receiver r position is fixed); the process is as follows:

[0160] First, the singular value robust Kalman filter A specific linearization adjustment is made to convert and The adjusted state vector Decomposed into an unambiguous floating point state vector and the floating point ambiguity solution vector Further we get:

[0161]

[0162] Among them, Cov represents the covariance matrix of the corresponding vector. represents the covariance matrix of the unambiguous floating-point state vector, Q b represents the floating point ambiguity covariance matrix, Represents the covariance matrix of the non-ambiguity parameters and ambiguity parameters in the state vector;

[0163] Secondly, we need to find the integer solution of the floating point ambiguity. b LD decomposition is performed to obtain L represents a lower triangular matrix whose diagonal elements are 1 and whose off-diagonal elements are non-integers. Λ represents a diagonal matrix. The off-diagonal elements of L are integers to obtain the matrix L1. Transform to get for:

[0164]

[0165] Among them, -T represents the inverse transpose of the matrix or the inverse of the transpose;

[0166] get Repeat the above LD decomposition steps until the nth decomposed unit lower triangular matrix L n Round it down and it can only be taken as the unit matrix, and the final result is Q z for:

[0167]

[0168] Pick in It is called floating point double difference phase deviation. The newly obtained lower triangular matrix G and Λ are brought into the MLAMBDA search algorithm to obtain the integer double difference phase deviation

[0169] Finally, the integer ambiguity solution vector can be obtained The integer unambiguous parameter state vector is obtained by adjusting the integer ambiguity solution vector The process is as follows:

[0170]

[0171] in, represents the integer unambiguity parameter vector The state covariance matrix of represents the integer non-ambiguous state solution, which is also the final solution for the robot dog's position fixation.

[0172] This method is implemented using the robot dog Jueying lite3 produced by Yunshenchu ​​Company, equipped with an RTK positioning settlement module and a GNSS receiving antenna, which can perform satellite dual-frequency carrier positioning. It is implemented based on python code, and the software used are RTKplot and RTKconv. In order to verify the performance of the algorithm, a total of 3833 epochs of receiver static data were collected. The reference receiver and the robot dog receiver collected data simultaneously, and the time was 08:54-09:58 on June 6, 2024.

[0173] Since the collected data has high positioning accuracy and high fixation rate, it is difficult to obtain interference signals in an actual high-noise environment. In order to verify the performance of the present invention, artificial random noise is added for simulation. In the collected mobile station OBS file, Gaussian white noise with a mean of 0, a standard deviation of 1 and a probability of 75% is added to the carrier phase observation value. Figure 2 and Figure 3 The horizontal axis of the gross error effect diagram represents time, and the vertical axis represents the gross error jump value in the corresponding direction. It can be seen that the robot dog RTK positioning method based on Kalman filtering under interference has a large number of gross error results such as Figure 2 , it is basically impossible to achieve high-precision positioning. Figure 3 The unmanned robot dog RTK positioning method based on singular value robust Kalman filtering significantly reduces the gross error jump in the overall positioning time, making the gross error more stable and close to zero, greatly improving the stability of the robot dog RTK positioning.

[0174] Ratio Factor atio It is used to verify the integer ambiguity. It measures the superiority of the optimal solution over the suboptimal solution by comparing the residuals (or objective function values) of the optimal integer solution and the suboptimal integer solution. A higher ratio factor means that the optimal solution is significantly better than the suboptimal solution, thereby increasing the credibility of the solution. in, is the residual sum of squares corresponding to the optimal integer solution; is the square of the residual corresponding to the suboptimal integer solution. The new algorithm significantly improves R in many aspects. atio The numerical value of the ratio. Figure 5 It can be seen that the proposed algorithm increases the effectiveness of the fixed solution over a wide range of time periods and reduces the R atio The value has increased from the original 0-150 to 0-1500, which shows that the algorithm has obvious advantages.

[0175] The ambiguity fixation capability of the robot dog RTK positioning method based on Kalman filtering in an interference environment is generally shown in Table 1, and the ambiguity integer solution can only be successfully fixed in 35% of the epochs. Its performance is limited by the influence of noise and model errors. The robot dog RTK positioning method based on singular value robust Kalman filtering fixes the epoch number to 3222, with a fixation rate of 84%, which significantly improves the fixation rate of ambiguity and increases the duration of positioning settlement, achieving continuous high-precision and stable positioning of the robot dog RTK.

[0176]

[0177] Table 1

[0178] The contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A method for RTK positioning of unmanned aerial vehicles based on singular value robust Kalman filtering, characterized in that: The method comprises the following steps: Step 1: The robot dog GNSS receiver obtains satellite and base station data. The base station GNSS receiver sends the base station coordinate position, carrier phase and pseudo-range measurement value, and ephemeris auxiliary data information to the robot dog receiver through a wireless transmission link; at the same time, the robot dog mobile station synchronously measures the corresponding satellite data based on the satellite data of the base station to obtain the carrier and pseudo-range measurement values ​​of the robot dog GNSS receiver; the carrier and pseudo-range measurement values ​​received by the robot dog GNSS receiver and the base station GNSS are combined to form a receiver observation value vector. Because the satellite sends carrier signals and ranging code signals of multiple frequencies, different frequency information corresponds to different carrier signals and ranging code signals, and multiple receiver observation value vectors of different frequencies can be formed. For L1 frequency, the corresponding observation value vector is: in, Indicates the robot dog GNSS receiver number, Indicates the base station GNSS receiver number, Indicates the common view satellite number, represents the pseudo-range observation value of the base station GNSS receiver r to the common-view satellite j, represents the carrier phase observation value of the common-view satellite j by the GNSS receiver r at the base station, Similarly; Step 2: Construct the double difference observation equation, adjust (1), and establish equations for each carrier phase and pseudorange measurement value. The carrier measurement value and pseudorange measurement value of j received by r have the following observation relationship equation: in, c≈3.0×10 8 m / s represents the speed of light; Indicates the receiver clock error; represents the satellite clock error; represents the ionospheric delay; Indicates delay in process; Indicates the integer value of the integer ambiguity; represents the pseudorange observation noise; represents the carrier phase observation noise; Indicates the actual distance between the satellite and the robot dog receiver; Similarly, the equations shown in (2) are also established for the carrier measurement value and pseudorange measurement value sent by u. In the case of dual-frequency L1 and L2 multi-satellite systems, a satellite i with an elevation angle greater than 15°, the highest elevation angle stability, and a good signal-to-noise ratio is selected from the common-view satellites as the reference satellite. The two receivers observe satellite i and form the same observation equation as (2). The two receivers form different single-difference observation equations for i and j respectively, where the single-difference equation for satellite i is expressed as follows: The intersatellite difference of the single difference equation formed by a series of j and the single difference equation of i is called double difference, and the double difference observation equation of the carrier phase and pseudorange of the two receivers is obtained, which is expressed as follows: in Double-difference carrier phase observations; Double-difference pseudorange observations, Double difference ambiguity value, Double-difference pseudorange observation noise; Double-difference carrier phase observation noise, Indicates the double difference satellite-to-ground actual distance between two satellites and two receivers; Step 3: Singular value robust Kalman filtering, Step 4: Integer ambiguity fixation and fixed solution calculation. K Fix the integer ambiguity vector and solve for the integer unambiguous state vector That is, the final position r of the robot dog receiver is fixed.

2. The unmanned aerial vehicle RTK positioning method based on singular value robust Kalman filtering as claimed in claim 1, characterized in that: The process of step three is as follows: Step 3.1: Linearize the observation equation, the system state prediction equation and system measurement equation of the singular value robust Kalman filter are as follows: in, represents the epoch time t(k), The state vector of the parameter to be estimated at the t(k-1)th epoch, The state vector of the a priori estimated parameters at the t(k)th epoch, The state vector of the posterior estimated parameters at the t(k)th epoch, A k Measurement state transfer matrix, ω k represents the process noise at time t(k), and the mean is zero and the covariance matrix is ​​Q k The normal distribution of k Represents the measurement noise at time t(k), and satisfies the zero mean and covariance matrix R k Normal distribution of y k represents the double difference observation vector at time t(k), which is synthesized by arranging the double difference carrier phase observations and double difference pseudorange observations of all satellites in the dual frequency case as shown in (4), H k represents the measurement state transition matrix at time t(k); y k The corresponding double difference observation equation is transformed into: According to y k The state vector x can be obtained K , x K From the required position vector e r =(x r ,y r ,z r ) T Velocity information vector v r =(v x ,v y ,v z ), the single difference ambiguity values ​​of all (3) under dual-frequency L1 and L2 conditions are composed of n K vector; The state vector x K and k Corresponding to A K and H K , the state vector x K With y k There is a nonlinear relationship between them, so H K Expressed in Jacobian matrix form: Between the interval Δt of epoch k and the previous epoch k-1, the movement of the robot dog GNSS receiver r must have a position transfer transformation, A K It is expressed as follows: in, represents the number of j, m = (6 + (n + 1) * 2) * (6 + (n + 1) * 2), when r is in a static state, Δt = 0, and when it is not static, Δt remains unchanged; Q K is called the process noise covariance matrix. The robot dog GNSS receiver r is in dynamic mode, so the corresponding Q K It is expressed in the following form: in, E r The transfer matrix representing the geocentric fixed-center geographic coordinates ENU, They represent the variance of acceleration in the horizontal latitude E direction, longitude N direction, and height U direction, respectively. diag() represents a diagonal matrix and the diagonal elements correspond to the corresponding position elements inside the brackets. When the robot dog GNSS receiver r is in static mode, Q v Take the zero matrix; Step 3.2: The measurement error covariance matrix singular value robustness adjustment, measurement noise covariance matrix R k The form is as follows: Among them, R Φ,1 , R Φ,2 , R P,1 , R P,1 It is calculated as follows: in, represents the standard deviation of the carrier phase measurement value of the non-reference satellite j=1, and the subscript 1 indicates that the carrier frequency is L1; represents the standard deviation of the pseudorange measurement value of the non-reference satellite j=2, and the subscript number 1 indicates that the carrier frequency is L1; For R k The matrix is ​​adjusted by singular value decomposition as follows: where Σ k It is expressed as follows: Among them, Σ k is a diagonal matrix, U k and V k are orthogonal matrices (i.e. their column vectors are orthogonal unit vectors), in particular Σ k The elements on the diagonal are called singular values. This decomposition transforms the matrix R k It is expressed as the product of three simple matrices, which is easy to analyze and process; For the singular value matrix Σ k Take the square root of each element on the diagonal; Simplified to: The calculation and reorganization results are as follows: The observed residual vector is expressed as: For the observed residual vector z k The Element z i , adjust it linearly to the appropriate range, first calculate z k The mean μ of each element in is: The observation residual vector z k Each element z in i The final linear adjustment is Get the linearly adjusted observation residual vector Each element It is expressed as follows: but The corresponding adaptive weight factor γ i The calculation method is as follows: Among them, K1 is an empirical constant, K2 is an empirical constant; According to the calculated adaptive weight reduction factor γ i The vector composed of γ adjusts the reorganization Duplicate the vector γ and call it γ', where the corresponding elements are renamed γ j and With i representing the element number, for a certain epoch K Each element in its i-th row and j-th column Adaptive processing to obtain the final singular value robust reconstruction measurement error covariance matrix as follows: Step 3.3: Calculate the singular value robustness gain and update the Kalman prediction. and the prior error covariance matrix The results of calculating the singular value adaptive gain are as follows: Calculate the prior estimate state vector at epoch k Update the state vector of the last epoch's filtered posterior The following transformation results are as follows: Similarly, calculate the prior error covariance matrix The error covariance matrix of the filter posterior update at the previous moment is required The results are as follows: Posterior state vector The updated result is: Posterior error covariance matrix The updated result is: The posterior state vector at epoch K is It is called the posterior error covariance matrix store.

3. The unmanned vehicle RTK positioning method based on singular value robust Kalman filtering as claimed in claim 1 or 2, characterized in that: The process of step 4 is as follows: First, the singular value robust Kalman filter A specific linearization adjustment is made to convert and The adjusted state vector Decomposed into an unambiguous floating point state vector and the floating point ambiguity solution vector Further we get: Among them, Cov represents the covariance matrix of the corresponding vector. represents the covariance matrix of the unambiguous floating-point state vector, Q b represents the floating point ambiguity covariance matrix, Represents the covariance matrix of the non-ambiguity parameters and ambiguity parameters in the state vector; Secondly, find the integer solution of the floating-point ambiguity and b LD decomposition is performed to obtain L represents a lower triangular matrix whose diagonal elements are 1 and whose off-diagonal elements are non-integers, Λ represents a diagonal matrix, and the matrix L1 is obtained by taking integers from the off-diagonal elements of L. Transform to get for: Among them, -T represents the inverse transpose of the matrix or the inverse of the transpose; get Repeat the above LD decomposition steps until the nth decomposed unit lower triangular matrix L n Round it down and it can only be taken as the unit matrix, and the final result is Q z for: Pick in It is called floating point double difference phase deviation. The newly obtained lower triangular matrix G and Λ are brought into the MLAMBDA search algorithm to obtain the integer double difference phase deviation Finally, we get the integer ambiguity solution vector The integer unambiguous parameter state vector is obtained by adjusting the integer ambiguity solution vector The process is as follows: in, represents the integer unambiguity parameter vector The state covariance matrix of represents the integer non-ambiguous state solution, which is also the final solution for the robot dog's position fixation.

Images