PPP-RTK fast convergence method based on rescue equipment motion constraints
By building an adaptive motion model and filtering model of intelligent rescue equipment, and optimizing PPP-RTK technology, the problem of rapid recovery of PPP-RTK positioning in complex geological disaster environments is solved, and high-precision and continuous positioning results are achieved.
Patent Information
- Application Number
- CN202111248244.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-10-26
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2041-10-26
AI Technical Summary
In complex geological disaster environments, PPP-RTK technology is difficult to quickly restore high-precision positioning after signal loss or frequent round-route jumps, resulting in the inability to guarantee the positioning continuity, reliability and accuracy of intelligent rescue equipment, affecting vehicle driving and operation safety.
By building an adaptive filtering model based on the intelligent rescue equipment motion model, using inertial navigation data to predict positions, and adjusting the constraint weights in combination with acceleration and satellite visible quantity, the PPP-RTK high-precision measurement model is optimized to improve positioning accuracy and continuity.
In complex environments, the reconvergence of PPP-RTK ambiguity is significantly accelerated, the continuity, reliability and accuracy of positioning results are improved, and the reliable positioning technology means are provided with high precision.
Smart Images

Figure CN114047536B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a PPP-RTK fast convergence method based on rescue equipment motion constraints, belonging to the technical field of navigation and positioning applications. Background Art
[0002] At present, under good observation environment, PPP-RTK technology based on the global positioning system, as an emerging high-precision positioning technology, has been gradually applied to various industries and fields, providing high-precision value-added services for the public and equipment. However, in complex natural geological disaster environments such as mountainous areas and valleys where intelligent rescue equipment operates, such as satellite obstruction and signal loss, PPP-RTK high-precision positioning faces huge challenges. At this time, the rapid, high-precision and continuous positioning of intelligent rescue equipment cannot be effectively guaranteed, and the driving and operation safety of vehicles are seriously threatened. The reason is that in this environment, when the signal is lost or there are frequent cycle jumps, the ambiguity needs to be reinitialized, and its positioning continuity, reliability and accuracy cannot be effectively guaranteed. It is urgent to explore the motion characteristics of intelligent rescue equipment and improve the rapid convergence efficiency of PPP-RTK based on this.
[0003] In the rescue equipment operation scene, due to the limitations of the scene and the needs of rescue, its movement is mainly manifested in several basic forms such as forward, backward, and turning. The dynamics are not high, and there is a certain regularity between the previous and next epochs. By mastering its movement rules and adding external motion constraints, the PPP-RTK convergence time in the satellite occlusion environment can be improved, and finally a continuous high-precision positioning result can be obtained. However, such technology is still lacking in the existing technology. Summary of the invention
[0004] In view of this, the present invention proposes a PPP-RTK fast convergence method based on rescue equipment motion constraints. The method targets complex geological disaster environments, fully identifies and utilizes the kinematic characteristics of intelligent rescue equipment, and thereby improves the initialization time of PPP-RTK ambiguity, thereby solving the problem of fast and continuous high-precision positioning in complex geological disaster environments.
[0005] In order to achieve the above object, the technical solution adopted by the present invention is:
[0006] A PPP-RTK fast convergence method based on rescue equipment motion constraints includes the following steps:
[0007] (1) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the transfer probability of the rescue equipment motion model is determined in real time based on the historical data of the position of the intelligent rescue equipment and the speed, angular velocity and acceleration of inertial navigation;
[0008] (2) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration information measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted; according to the transfer probability of the motion model of the rescue equipment, the comprehensive weighted and evaluation model of the intelligent rescue equipment is adjusted in real time; based on the comprehensive weighted and evaluation model, the predicted position of the intelligent rescue equipment is obtained;
[0009] (3) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model with the acceleration and satellite number constraints of the intelligent rescue equipment is constructed, the PPP-RTK high-precision measurement model and state equation with the position constraints of the intelligent rescue equipment are optimized, and the high-precision position of the intelligent rescue equipment is estimated based on the PPP-RTK estimation method.
[0010] Furthermore, the specific method of step (1) is as follows:
[0011] (101) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the model is as follows:
[0012] The motion model set is X = {X CV ,X CA ,X CTRV ,X CTRA}, where X CV =[xy θ v] T , represents the constant speed model, X CA =[xy θ va] T , represents the constant acceleration model, X CTRV =[xy θ v ω] T , represents the constant rotation rate and speed model, X CTRA =[xy θ v ω a] T , represents the constant rotation rate and acceleration model;
[0013] Among them, x represents the horizontal coordinate, y represents the vertical coordinate, θ represents the angle with the x-axis, which is provided by the heading angle output by the inertial navigation device, v represents the speed, w represents the angular velocity, a represents the acceleration, and the rescue equipment switches among several models in the motion model set;
[0014] (102) Based on the historical data of the position of the intelligent rescue equipment and the heading angle and acceleration of the inertial navigation, the transfer probability of the rescue equipment motion model is determined in real time, and the implementation method is as follows:
[0015] According to the observed values of heading angle and acceleration, a motion model in the motion model set is matched for all historical data. The specific method is: if θ=0, a=0, it is recorded as CV model; if θ=0, a≠0, it is recorded as CA model; if θ≠0, a=0, it is recorded as CTRV model; if θ≠0, a≠0, it is recorded as CTRA model;
[0016] In the motion model set, each motion model may transfer to the other four models. The four basic motion models correspond to 16 state transitions. The number of times each state transition occurs in the historical data is counted. ij , i,j=1,2,3,4:
[0017] CV THAT CTRV CTRA CV <![CDATA[N 11 ]]> <![CDATA[N 12 ]]> <![CDATA[N 13 ]]> <![CDATA[N 14 ]]> THAT <![CDATA[N 21 ]]> <![CDATA[N 22 ]]> <![CDATA[N 23 ]]> <![CDATA[N 24 ]]> CTRV <![CDATA[N 31 ]]> <![CDATA[N 32 ]]> <![CDATA[N 33 ]]> <![CDATA[N 34 ]]> CTRA <![CDATA[N 41 ]]> <![CDATA[N 42 ]]> <![CDATA[N 43 ]]> <![CDATA[N 44 ]]>
[0018] Calculate the probability of transfer between different models to form a model transfer probability set. The specific calculation method is as follows:
[0019]
[0020] In the formula, p ij represents the transition probability between motion models i and j, N ij Indicates the number of transitions from motion model i to motion model j.
[0021] Furthermore, the specific method of step (2) is as follows:
[0022] (201) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration information measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted, and the implementation method is as follows:
[0023] A position calculator is set up independently for each basic motion model, with the state estimate at time k-1 The v, θ, w, and a information measured in real time by inertial navigation are used as input, and the predicted position at time k corresponding to each model in the motion model set is calculated according to the following state transfer equation
[0024]
[0025]
[0026] (202) According to the transfer probability of the rescue equipment motion model, the comprehensive weighted and evaluated model of the intelligent rescue equipment is adjusted in real time, and the predicted position of the intelligent rescue equipment is obtained based on the comprehensive weighted and evaluated model. The implementation method is as follows:
[0027] The corresponding predicted positions are calculated according to different motion models, and then the positions obtained under different motion models are fused according to probability; let the position prediction value corresponding to model j at time k be Each model is mixed according to probability. The specific calculation method is as follows:
[0028]
[0029] Furthermore, the specific method of step (3) is as follows:
[0030] (301) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model constrained by the acceleration of the intelligent rescue equipment and the number of satellites is constructed. The model is as follows:
[0031]
[0032] In the formula, n represents the number of available satellites, a represents the acceleration of the plane direction of the moving rescue equipment, a0 represents the acceleration threshold, n0 represents the satellite number threshold, b, c, d are constants given according to experience;
[0033] This model controls motion constraints by adjusting the weights of additional positions. In an environment with good observation environment and the number of visible satellites greater than n0, PPP-RTK is used to independently provide high-precision positions, and b is set to 10, thereby reducing motion feature constraints and relying more on observation values. When the number of satellites is insufficient or satellite cycle jumps are frequent, PPP-RTK cannot provide independent services, and motion feature constraints are increased. The specific method is: set the motion constraint weight through acceleration. If the acceleration is less than a0, it is considered that the motion prediction model fits the reality, and c is set to 2. If the acceleration is greater than a0, it is considered that there is an error in the motion prediction, and d is set to 1.
[0034] (302) The PPP-RTK high-precision measurement model and state equation of the position constraint of the intelligent rescue equipment are optimized, and the high-precision position of the intelligent rescue equipment is estimated based on the PPP-RTK estimation method. The implementation method is as follows:
[0035] The PPP-RTK function model is as follows:
[0036]
[0037]
[0038] Where subscript r and superscript s represent receiver and satellite respectively, ti represents epoch, P r s represents the pseudorange observation value, Φ r s represents the carrier phase observation value, represents the approximate geometric distance from the observation satellite to the receiver, c represents the speed of light, δt s ,δt r They represent the clock difference between the satellite and the receiver, λ represents the wavelength, N represents the satellite's integer ambiguity, δ s ion represents the ionospheric delay, δ s trop represents the tropospheric delay, δ s tide represents the tidal effect, δ s mul represents the multipath effect, δ s rel represents the relativistic effect, ε s p and represents the measurement noise of pseudorange and carrier phase, and the symbol “·” represents the rate of change of the corresponding variable; all terms in the formula are in meters; P s r,IF and Φ s r,IF The ionospheric-free combined observations representing pseudorange and carrier phase are obtained as follows:
[0039]
[0040]
[0041] Where f represents the wavelength, and the subscripts 1 and 2 represent the observation frequency.
[0042] Assuming that n satellites are observed in the current epoch, the PPP-RTK observation equation is further expressed as follows:
[0043]
[0044] Where: e 1×3 is the direction cosine vector from the approximate position of the station to the observation satellite, m wz represents the tropospheric wet delay projection function, λ IF The wavelength of the ionosphere-free combined observation, N IF represents the ambiguity of the ionosphere-free combined observation value, the superscript 1, 2...n represents the number of satellites, v represents the residual, the subscript l represents the carrier phase observation value, and p represents the pseudorange observation value;
[0045] The motion constraints are set up as:
[0046]
[0047] Solve the above two equations together and express them in the following matrix form:
[0048] L k=A k X k +V k ,V k ~N(0,R k )
[0049] Where: L k represents the observation value vector, V k represents the measurement noise vector, R k represents the noise matrix, ~N() indicates that it obeys the normal distribution; the other elements are briefly expressed as follows:
[0050]
[0051] Where: E is a 3×3 unit matrix, s, v, a represent the three-dimensional coordinates, velocity, and acceleration of the receiver, tro wet ,δt r represents the residual component of the zenith tropospheric wet component delay and the receiver clock error, and δ P 2 represents the noise variance of carrier phase and pseudorange;
[0052] The state equation is expressed as:
[0053] X k =F k X k-1 +W k ,W k ~N(0,Q w )
[0054] Where: W represents process noise, F k and Q w They represent the state transfer matrix and the noise matrix respectively, which are specifically expressed as follows:
[0055]
[0056] Where τ is the sampling interval, q a ,q z and q t They are the receiver acceleration, the residual error of the wet component of the tropospheric delay in the zenith direction, and the power density of the receiver clock error.
[0057] Compared with the prior art, the present invention has the following beneficial effects:
[0058] 1. The present invention provides external position constraints for GNSS in complex environments by establishing the best-fitting adaptive motion model, thereby accelerating the re-convergence of PPP-RTK ambiguity. Compared with the traditional method, the present invention makes full use of the kinematic characteristics of the rescue equipment, establishes an adaptive motion model with a higher degree of fit to the actual motion of the rescue equipment, and improves the continuity, reliability and accuracy of the positioning results.
[0059] 2. The present invention proposes to summarize the motion characteristics shown by historical data, provide the conversion probability between different models, and improve the prediction accuracy of a single motion model.
[0060] 3. The present invention adds motion constraints to the PPP-RTK algorithm, and accelerates the re-convergence of ambiguity in complex environments by setting adaptive filtering and reasonably setting motion constraint weights, thereby improving the continuity of actual positioning results.
[0061] 4. The method of the present invention is simple and easy to implement, and can provide a high-precision and reliable positioning technology means for the rescue platform in a geological disaster environment.
[0062] In summary, the present invention targets complex geological disaster environments, fully identifies and utilizes the kinematic characteristics of intelligent rescue equipment, collects inertial measurement velocity, acceleration and attitude angle information based on the intelligent rescue equipment, constructs and identifies the motion mode of the rescue equipment, and adopts the joint constraints of the motion mode of the intelligent rescue equipment and the inertial measurement information to improve the PPP-RTK rapid positioning in satellite occlusion environments, thereby solving the problem of rapid and continuous high-precision positioning in complex geological disaster environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0063] Figure 1 Schematic diagram of the PPP-RTK positioning principle with additional motion constraints in an embodiment of the present invention. DETAILED DESCRIPTION
[0064] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and specific implementation methods.
[0065] A PPP-RTK fast convergence method based on rescue equipment motion constraints, which is based on the intelligent rescue equipment collecting inertial measurement velocity, acceleration and attitude angle information, constructing and identifying the rescue equipment motion mode, and using the joint constraints of the motion mode of the intelligent rescue equipment and inertial measurement information to improve the PPP-RTK fast positioning in a satellite occlusion environment, and finally obtain continuous high-precision positioning results, specifically including the following steps:
[0066] (1) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the transfer probability of the rescue equipment motion model is determined in real time based on the historical data such as the position of the intelligent rescue equipment and the speed, angular velocity and acceleration of the inertial navigation;
[0067] (2) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration information measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted. Then, according to the transfer probability of the rescue equipment motion model, the comprehensive weighted and evaluation model of the intelligent rescue equipment is adjusted in real time. Finally, based on the comprehensive weighted and evaluation model, the predicted position of the intelligent rescue equipment is obtained;
[0068] (3) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model with the acceleration and satellite number constraints of the intelligent rescue equipment is constructed. Based on this, the PPP-RTK high-precision measurement model and state equation of the position constraints of the intelligent rescue equipment are optimized. Based on the PPP-RTK estimation method, the high-precision position of the intelligent rescue equipment is estimated.
[0069] Furthermore, the specific method of step (1) is as follows:
[0070] (101) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the model is as follows:
[0071] The motion model set is X = {X CV ,X CA ,X CTRV ,X CTRA}, where X CV =[xy θ v] T , represents the constant speed model, X CA =[xy θv a] T , represents the constant acceleration model, X CTRV =[xy θ v ω] T , represents the constant rotation rate and speed model, X CTRA =[xy θ v ω a] T , represents the constant rate of rotation and acceleration model, x represents the horizontal coordinate, y represents the vertical coordinate, θ represents the angle with the x-axis, which can be provided by the heading angle output by the inertial navigation device, v represents the speed, w represents the angular velocity, a represents the acceleration, and the rescue equipment switches among several models in the motion model set according to certain rules.
[0072] (103) Based on the historical data of the position of the intelligent rescue equipment and the heading angle and acceleration of the inertial navigation, the transfer probability of the rescue equipment motion model is determined in real time, and the implementation method is as follows:
[0073] According to the observed values of heading angle and acceleration, a motion model in the motion model set is matched for all historical data. The specific method is: if θ=0, a=0, it is recorded as CV model; if θ=0, a≠0, it is recorded as CA model; if θ≠0, a=0, it is recorded as CTRV model; if θ≠0, a≠0, it is recorded as CTRA model.
[0074]
[0075]
[0076] As shown in the table above, in the motion model set, each motion model may transfer to the other four models. The four basic motion models correspond to 16 state transitions. The number of times each state transition occurs in the historical data is counted, recorded as N. ij , calculate the probability of transfer between different models, thereby forming a model transfer probability set. The specific calculation method is as follows:
[0077]
[0078] p ij represents the transition probability between motion models i and j, N ij Indicates the number of transitions from motion model i to motion model j.
[0079] The specific method of step (2) is as follows:
[0080] (201) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration information measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted, and the implementation method is as follows:
[0081] A position calculator is set up independently for each basic motion model, with the state estimate at time k-1 The information of v, θ, w, a and other real-time measurements of inertial navigation are used as input, and the predicted position at time k corresponding to each model in the motion model set is calculated according to the following state transfer equation
[0082]
[0083]
[0084] (202) According to the transfer probability of the rescue equipment motion model, the comprehensive weighted and evaluated model of the intelligent rescue equipment is adjusted in real time, and the predicted position of the intelligent rescue equipment is obtained based on the model. The implementation method is as follows:
[0085] The corresponding predicted positions are calculated according to different motion models, and then the positions obtained under different motion models are fused according to probability. Suppose the position prediction value corresponding to model j at time k is Each model is mixed according to probability. The specific calculation method is as follows:
[0086]
[0087] The specific method of step (3) is as follows:
[0088] (301) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model constrained by the acceleration of the intelligent rescue equipment and the number of satellites is constructed. The model is implemented as follows:
[0089]
[0090] Where n represents the number of available satellites, a represents the acceleration of the plane direction of the moving rescue equipment, a0 represents the acceleration threshold, n0 represents the threshold of the number of satellites, b and c are constants given according to empirical values. This model controls the motion constraint by adjusting the weight of the additional position. In an environment with good observation environment and the number of visible satellites greater than n0, PPP-RTK can independently provide high-precision positions, set smaller motion feature constraints, and trust more observation values. In a complex environment where PPP-RTK cannot provide independent services, such as insufficient number of satellites and frequent satellite cycle jumps, the motion feature constraints are increased. In addition, the motion constraint weight is set by the acceleration. If the acceleration is less than a0, it is considered that the motion prediction model is more realistic and a larger weight is set. If the acceleration is greater than a0, it is considered that there is an error in the motion prediction and the weight becomes smaller accordingly.
[0091] (303) The PPP-RTK high-precision measurement model and state equation of the position constraint of the intelligent rescue equipment are optimized, and the high-precision position of the intelligent rescue equipment is estimated based on the PPP-RTK estimation method. The implementation method is as follows:
[0092] The PPP-RTK function model is as follows:
[0093]
[0094]
[0095] Where subscript r and superscript s represent receiver and satellite respectively, ti represents epoch, P r s represents the pseudorange observation value, Φ r s represents the carrier phase observation value, represents the approximate geometric distance from the observation satellite to the receiver, c represents the speed of light, δt s,δt r They represent the clock difference between the satellite and the receiver, λ represents the wavelength, N represents the satellite's integer ambiguity, δ s ion represents the ionospheric delay, δ s trop represents the tropospheric delay, δ s tide represents the tidal effect, δ s mul represents the multipath effect, δ s rel represents the relativistic effect, ε s p and represents the measurement noise of pseudorange and carrier phase, and the symbol “·” represents the rate of change of the corresponding variable. All terms in the formula are in units of m. s r,IF and Φ s r,IF The ionospheric-free combined observation value representing the pseudorange and carrier phase can be obtained by the following formula
[0096]
[0097]
[0098] Assuming that n satellites are observed in the current epoch, the PPP-RTK observation equation can be further expressed as follows:
[0099]
[0100] The motion constraints are set up as:
[0101]
[0102] Solve the above two equations together and express them in the following matrix form:
[0103] L k =A k X k +V k ,V k ~N(0,R k )
[0104] Where: L represents the observation value vector, V represents the measurement noise vector, and for the convenience of subsequent representation and reading, the other elements are briefly expressed as follows:
[0105]
[0106] Where: e is the direction cosine vector from the approximate position of the observation station to the observation satellite, E is the 3×3 unit matrix, m wzrepresents the tropospheric wet delay projection function, s, v, a represent the three-dimensional coordinates, velocity, and acceleration of the receiver, tro wet ,δt r , N represents the residual component of the zenith tropospheric wet component delay, the receiver clock error and the ambiguity vector of the observed satellite, and δ P 2 represents the noise variance of carrier phase and pseudorange.
[0107] The state equation is expressed as:
[0108] X k =F k X k-1 +W k ,W k ~N(0,Q w )
[0109] W represents process noise, F k and Q w They represent the state transfer matrix and the noise matrix respectively, which are specifically expressed as follows:
[0110]
[0111] Where τ is the sampling interval, q a ,q z and q t They are the receiver acceleration, the residual error of the wet component of the tropospheric delay in the zenith direction, and the power density of the receiver clock error.
[0112] Aiming at the complex geological disaster environment, this method fully identifies and utilizes the kinematic characteristics of intelligent rescue equipment, collects inertial measurement velocity, acceleration and attitude angle information based on the intelligent rescue equipment, constructs and identifies the motion mode of the rescue equipment, and adopts the joint constraints of the motion mode of the intelligent rescue equipment and inertial measurement information to improve the PPP-RTK rapid positioning in satellite occlusion environments, and finally obtains continuous high-precision positioning results.
[0113] The actual motion law of the motion carrier and mathematical modeling are often difficult to accurately fit, and there are certain errors. If the motion modeling is unreasonable and the wrong motion constraints are added, it is easy to cause divergence of the results. In this regard, this method proposes two core technologies to control:
[0114] (1) Establishment of adaptive motion model
[0115] The best-fitting adaptive motion model is established by calculating the probabilities of several basic motion models occurring in the current epoch and then taking the weighted average of their predicted positions.
[0116] (2) Establishment of adaptive filter
[0117] Different weights are set for position constraints according to different situations. When the GNSS observation reliability is high, the motion constraints are weakened. When the GNSS observation fails partially or completely, the motion constraints are strengthened.
[0118] Here is a more specific example:
[0119] like Figure 1 As shown in the figure, a PPP-RTK fast convergence method based on rescue equipment motion constraints is used. It obtains parameters such as attitude angle, acceleration, angular velocity, and speed through an inertial navigation receiver. According to the motion characteristics of the rescue equipment shown in historical data, the probability transfer probability set of the motion model is calculated, and an adaptive motion model is established by combining the motion model set. In a complex environment, the PPP-RTK re-convergence speed is accelerated through motion characteristic constraints. It includes the following steps:
[0120] (1) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the transfer probability of the rescue equipment motion model is determined in real time based on the historical data such as the position of the intelligent rescue equipment and the speed, angular velocity and acceleration of the inertial navigation;
[0121] (2) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration information measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted. Then, according to the transfer probability of the rescue equipment motion model, the comprehensive weighted and evaluation model of the intelligent rescue equipment is adjusted in real time. Finally, based on the comprehensive weighted and evaluation model, the predicted position of the intelligent rescue equipment is obtained;
[0122] (3) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model with the acceleration and satellite number constraints of the intelligent rescue equipment is constructed. Based on this, the PPP-RTK high-precision measurement model and state equation of the position constraints of the intelligent rescue equipment are optimized. Based on the PPP-RTK estimation method, the high-precision position of the intelligent rescue equipment is estimated.
[0123] According to the PPP-RTK fast convergence method based on motion feature constraints of intelligent rescue equipment according to claim 1, the specific manner of step (1) is as follows:
[0124] (101) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the model is as follows:
[0125] The motion model set is set to X = {X CV ,X CA ,X CTRV ,X CTRA}, where X CV =[xy θ v]T , represents the constant speed model, X CA =[xy θ va] T , represents the constant acceleration model, X CTRV =[xy θ v ω] T , represents the constant rotation rate and speed model, X CTRA =[xy θ v ω a] T , represents the constant rate of rotation and acceleration model, x represents the horizontal coordinate, y represents the vertical coordinate, θ represents the angle with the x-axis, which can be provided by the heading angle output by the inertial navigation device, v represents the speed, w represents the angular velocity, a represents the acceleration, and the rescue equipment switches among several models in the motion model set according to certain rules;
[0126] (102) Based on the historical data of the position of the intelligent rescue equipment and the heading angle and acceleration of the inertial navigation, the transfer probability of the rescue equipment motion model is determined in real time, and the implementation method is as follows:
[0127] According to the observed values of heading angle and acceleration, a motion model in the motion model set is matched for all historical data. The specific method is: if θ=0, a=0, it is recorded as CV model; if θ=0, a≠0, it is recorded as CA model; if θ≠0, a=0, it is recorded as CTRV model; if θ≠0, a≠0, it is recorded as CTRA model.
[0128] CV THAT CTRV CTRA CV <![CDATA[N 11 ]]> <![CDATA[N 12 ]]> <![CDATA[N 13 ]]> <![CDATA[N 14 ]]> THAT <![CDATA[N 21 ]]> <![CDATA[N 22 ]]> <![CDATA[N 23 ]]> <![CDATA[N 24 ]]> CTRV <![CDATA[N 31 ]]> <![CDATA[N 32 ]]> <![CDATA[N 33 ]]> <![CDATA[N 34 ]]> CTRA <![CDATA[N 41 ]]> <![CDATA[N 42 ]]> <![CDATA[N 43 ]]> <![CDATA[N 44 ]]>
[0129] As shown in the table above, in the motion model set, each motion model may transfer to the other four models. The four basic motion models correspond to 16 state transitions. The number of times each state transition occurs in the historical data is counted, recorded as N. ij , calculate the probability of transfer between different models, thereby forming a model transfer probability set. The specific calculation method is as follows:
[0130]
[0131] p ij represents the transition probability between motion models i and j, N ij Indicates the number of transitions from motion model i to motion model j.
[0132] According to the PPP-RTK fast convergence method based on motion feature constraints of intelligent rescue equipment according to claim 1, the specific manner of step (2) is as follows:
[0133] (201) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration information measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted, and the implementation method is as follows:
[0134] A position calculator is set up independently for each basic motion model, with the state estimate at time k-1 The information of v, θ, w, a and other real-time measurements of inertial navigation are used as input, and the predicted position at time k corresponding to each model in the motion model set is calculated according to the following state transfer equation
[0135]
[0136]
[0137] (202) According to the transfer probability of the rescue equipment motion model, the comprehensive weighted and evaluated model of the intelligent rescue equipment is adjusted in real time, and the predicted position of the intelligent rescue equipment is obtained based on the model. The implementation method is as follows:
[0138] The corresponding predicted positions are calculated according to different motion models, and then the positions obtained under different motion models are fused according to probability. Suppose the position prediction value corresponding to model j at time k is Each model is mixed according to probability. The specific calculation method is as follows:
[0139]
[0140] According to the PPP-RTK fast convergence method based on motion feature constraints of intelligent rescue equipment according to claim 1, the specific manner of step (3) is as follows:
[0141] (301) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model constrained by the acceleration of the intelligent rescue equipment and the number of satellites is constructed. The model is implemented as follows:
[0142]
[0143] Where n represents the number of available satellites, a represents the acceleration of the plane direction of the moving rescue equipment, a0 represents the acceleration threshold, n0 represents the threshold of the number of satellites, b and c are constants given according to empirical values. This model controls the motion constraint by adjusting the weight of the additional position. In an environment with good observation environment and the number of visible satellites greater than n0, PPP-RTK can independently provide high-precision positions, set smaller motion feature constraints, and trust more observation values. In a complex environment where PPP-RTK cannot provide independent services, such as insufficient number of satellites and frequent satellite cycle jumps, the motion feature constraints are increased. In addition, the motion constraint weight is set by the acceleration. If the acceleration is less than a0, it is considered that the motion prediction model is more realistic and a larger weight is set. If the acceleration is greater than a0, it is considered that there is an error in the motion prediction and the weight becomes smaller accordingly.
[0144] (302) The PPP-RTK high-precision measurement model and state equation of the position constraint of the intelligent rescue equipment are optimized, and the high-precision position of the intelligent rescue equipment is estimated based on the PPP-RTK estimation method. The implementation method is as follows:
[0145] The PPP-RTK function model is as follows:
[0146]
[0147]
[0148] Where subscript r and superscript s represent receiver and satellite respectively, ti represents epoch, P r s represents the pseudorange observation value, Φ r s represents the carrier phase observation value, represents the approximate geometric distance from the observation satellite to the receiver, c represents the speed of light, δt s ,δt r They represent the clock difference between the satellite and the receiver, λ represents the wavelength, N represents the satellite's integer ambiguity, δ s ion represents the ionospheric delay, δ s trop represents the tropospheric delay, δ s tide represents the tidal effect, δ s mul represents the multipath effect, δ s rel represents the relativistic effect, ε s p and represents the measurement noise of pseudorange and carrier phase, and the symbol “·” represents the rate of change of the corresponding variable. All terms in the formula are in units of m. s r,IF and Φ s r,IF The ionospheric-free combined observation value representing the pseudorange and carrier phase can be obtained by the following formula
[0149]
[0150]
[0151] Assuming that n satellites are observed in the current epoch, the PPP-RTK observation equation can be further expressed as follows:
[0152]
[0153] The motion constraints are set up as:
[0154]
[0155] Solve the above two equations together and express them in the following matrix form:
[0156] L k =A k X k +V k ,V k ~N(0,R k )
[0157] Where: L represents the observation value vector, V represents the measurement noise vector, and for the convenience of subsequent representation and reading, the other elements are briefly expressed as follows:
[0158]
[0159] Where: e is the direction cosine vector from the approximate position of the observation station to the observation satellite, E is the 3×3 unit matrix, m wz represents the tropospheric wet delay projection function, s, v, a represent the three-dimensional coordinates, velocity, and acceleration of the receiver, tro wet ,δt r , N represents the residual component of the zenith tropospheric wet component delay, the receiver clock error and the ambiguity vector of the observed satellite, and δ P 2 represents the noise variance of carrier phase and pseudorange.
[0160] The state equation is expressed as:
[0161] X k =F k X k-1 +W k ,W k ~N(0,Q w )
[0162] W represents process noise, F k and Q w They represent the state transfer matrix and the noise matrix respectively, which are specifically expressed as follows:
[0163]
[0164] Where τ is the sampling interval, q a ,q z and q t They are the receiver acceleration, the residual error of the wet component of the tropospheric delay in the zenith direction, and the power density of the receiver clock error.
[0165] In short, in a geological disaster environment, the accuracy of positioning of mobile rescue platforms such as engineering machinery and vehicles will directly affect the driving and working safety of the rescue platform. Satellite navigation in a geological disaster environment is prone to satellite navigation positioning efficiency degradation and failure due to signal obstruction and multipath, and the positioning reliability is low. In this regard, the present invention provides a PPP-RTK fast convergence method based on motion feature constraints of intelligent rescue equipment, which can provide a technical means for high-precision and reliable positioning for rescue platforms in a geological disaster environment.
[0166] The present invention utilizes inertial navigation equipment to obtain key parameters of the motion characteristics of rescue equipment, calculates model conversion probability in real time, establishes an adaptive motion model, provides accurate predicted position, provides high-precision relative positioning results for the rescue platform, accelerates the re-convergence of PPP-RTK ambiguity in complex environments, and ultimately provides high-precision, continuous and reliable positioning results.
Claims
1. A PPP-RTK fast convergence method based on rescue equipment motion constraints, characterized in that: The steps include: (1) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed. Based on the historical data of the position of the intelligent rescue equipment and the speed, angular velocity and acceleration of inertial navigation, the transfer probability of the rescue equipment motion model is determined in real time. The specific method is as follows: (101) According to the type and mode of intelligent rescue equipment, a motion model set suitable for intelligent rescue equipment is constructed, and the model is as follows: The motion model set is X = {X CV ,X CA ,X CTRV ,X CTRA }, where X CV =[xy θ v] T , represents the constant speed model, X CA =[x yθ va] T , represents the constant acceleration model, X CTRV =[xy θ v ω] T , represents the constant rotation rate and speed model, X CTRA =[xy θv ω a] T , represents the constant rotation rate and acceleration model; Among them, x represents the horizontal coordinate, y represents the vertical coordinate, θ represents the angle with the x-axis, which is provided by the heading angle output by the inertial navigation device, v represents the speed, w represents the angular velocity, a represents the acceleration, and the rescue equipment switches among several models in the motion model set; (102) Based on the historical data of the position of the intelligent rescue equipment and the heading angle and acceleration of the inertial navigation, the transfer probability of the rescue equipment motion model is determined in real time, and the implementation method is as follows: According to the observed values of heading angle and acceleration, a motion model in the motion model set is matched for all historical data. The specific method is: if θ=0, a=0, it is recorded as CV model; if θ=0, a≠0, it is recorded as CA model; if θ≠0, a=0, it is recorded as CTRV model; if θ≠0, a≠0, it is recorded as CTRA model; In the motion model set, each motion model may transfer to the other four models. The four basic motion models correspond to 16 state transitions. The number of times each state transition occurs in the historical data is counted. ij , i,j=1,2,3,4: Calculate the probability of transfer between different models to form a model transfer probability set. The specific calculation method is as follows: In the formula, p ij represents the transition probability between motion models i and j, N ij represents the number of transitions from motion model i to motion model j; (2) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration data measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted; according to the transfer probability of the motion model of the rescue equipment, the comprehensive weighted and evaluation model of the intelligent rescue equipment is adjusted in real time; based on the comprehensive weighted and evaluation model, the predicted position of the intelligent rescue equipment is obtained; the specific method is as follows: (201) Based on the current position of the intelligent rescue equipment and the speed, angular velocity and acceleration data measured in real time by inertial navigation, combined with the prediction model of the motion model set of the intelligent rescue equipment, the position of the intelligent rescue equipment under the motion model set is predicted, and the implementation method is as follows: A position calculator is set up independently for each basic motion model, with the state estimate at time k-1 The v, θ, w, and a data measured in real time by inertial navigation are used as input, and the predicted position at time k corresponding to each model in the motion model set is calculated according to the following state transfer equation (202) According to the transfer probability of the rescue equipment motion model, the comprehensive weighted and evaluated model of the intelligent rescue equipment is adjusted in real time, and the predicted position of the intelligent rescue equipment is obtained based on the comprehensive weighted and evaluated model. The implementation method is as follows: The corresponding predicted positions are calculated according to different motion models, and then the positions obtained under different motion models are fused according to probability; let the position prediction value corresponding to model j at time k be Each model is mixed according to probability. The specific calculation method is as follows: (3) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model with the acceleration and satellite number constraints of the intelligent rescue equipment is constructed, the PPP-RTK high-precision measurement model and state equation with the position constraints of the intelligent rescue equipment are optimized, and the high-precision position of the intelligent rescue equipment is estimated based on the PPP-RTK estimation method.
2. The PPP-RTK fast convergence method based on rescue equipment motion constraint according to claim 1, characterized in that: The specific method of step (3) is as follows: (301) Based on the acceleration of the intelligent rescue equipment and the number of visible satellites, an adaptive filtering model constrained by the acceleration of the intelligent rescue equipment and the number of satellites is constructed. The model is as follows: In the formula, n represents the number of available satellites, a represents the acceleration of the plane direction of the moving rescue equipment, a0 represents the acceleration threshold, n0 represents the satellite number threshold, b, c, d are constants given according to experience; This model controls motion constraints by adjusting the weights of additional positions. In an environment with good observation environment and the number of visible satellites greater than n0, PPP-RTK is used to independently provide high-precision positions, and b is set to 10, thereby reducing motion feature constraints and relying more on observation values. When the number of satellites is insufficient or satellite cycle jumps are frequent, PPP-RTK cannot provide independent services, and motion feature constraints are increased. The specific method is: set the motion constraint weight through acceleration. If the acceleration is less than a0, it is considered that the motion prediction model fits the reality, and c is set to 2. If the acceleration is greater than a0, it is considered that there is an error in the motion prediction, and d is set to 1. (302) The PPP-RTK high-precision measurement model and state equation of the position constraint of the intelligent rescue equipment are optimized, and the high-precision position of the intelligent rescue equipment is estimated based on the PPP-RTK estimation method. The implementation method is as follows: The PPP-RTK function model is as follows: Where subscript r and superscript s represent receiver and satellite respectively, ti represents epoch, P r s represents the pseudorange observation value, Φ r s represents the carrier phase observation value, represents the approximate geometric distance from the observation satellite to the receiver, c represents the speed of light, δt s ,δt r They represent the clock difference between the satellite and the receiver, λ represents the wavelength, N represents the satellite's integer ambiguity, δ s ion represents the ionospheric delay, δ s trop represents the tropospheric delay, δ s tide represents the tidal effect, δ s mul represents the multipath effect, δ s rel represents the relativistic effect, ε s p and represents the measurement noise of pseudorange and carrier phase, and the symbol "·" represents the rate of change of the corresponding variable; all terms in the formula are in meters; P s r,IF and Φ s r,IF The ionospheric-free combined observations representing pseudorange and carrier phase are obtained as follows: In the formula, f represents the wavelength, and the subscripts 1 and 2 represent the observation frequency; Assuming that n satellites are observed in the current epoch, the PPP-RTK observation equation is further expressed as follows: Where: e 1×3 is the direction cosine vector from the approximate position of the station to the observation satellite, m wz represents the tropospheric wet delay projection function, λ IF The wavelength of the ionosphere-free combined observation, N IF represents the ambiguity of the ionosphere-free combined observation value, the superscript 1, 2...n represents the number of satellites, v represents the residual, the subscript l represents the carrier phase observation value, and p represents the pseudorange observation value; The motion constraints are set up as: Solve the above two equations together and express them in the following matrix form: L k =A k X k +V k ,V k ~N(0,R k ) Where: L k represents the observation value vector, V k represents the measurement noise vector, R k represents the noise matrix, ~N() indicates that it obeys the normal distribution; the other elements are briefly expressed as follows: Where: E is a 3×3 unit matrix, s, v, a represent the three-dimensional coordinates, velocity, and acceleration of the receiver, tro wet ,δt r represents the residual component of the zenith tropospheric wet component delay and the receiver clock error, and δ P 2 represents the noise variance of carrier phase and pseudorange; The state equation is expressed as: X k =F k X k-1 +W k ,W k ~N(0,Q w ) Where: W represents process noise, F k and Q w They represent the state transfer matrix and the noise matrix respectively, which are specifically expressed as follows: Where τ is the sampling interval, q a ,q z and q t They are the receiver acceleration, the residual error of the wet component of the tropospheric delay in the zenith direction, and the power density of the receiver clock error.
